diff --git a/.gitignore b/.gitignore index 2f936454dc4081..38b208e41cc3d8 100644 --- a/.gitignore +++ b/.gitignore @@ -4,6 +4,7 @@ .idea model2.png +*.DSYM *.d *.pyc *.pyo diff --git a/.travis.yml b/.travis.yml index 9c668d75d91b78..74411b5de82281 100644 --- a/.travis.yml +++ b/.travis.yml @@ -8,5 +8,5 @@ install: script: - docker run --rm - -v "$(pwd)"/selfdrive/test/plant/out:/tmp/openpilot/selfdrive/test/plant/out - tmppilot /bin/sh -c 'cd /tmp/openpilot/selfdrive/test/plant && ./runtest.sh' + -v "$(pwd)"/selfdrive/test/tests/plant/out:/tmp/openpilot/selfdrive/test/tests/plant/out + tmppilot /bin/sh -c 'cd /tmp/openpilot/selfdrive/test/tests/plant && OPTEST=1 ./test_longitudinal.py' diff --git a/Dockerfile.openpilot b/Dockerfile.openpilot index 3b3638f9cae5c6..4efafd40df1d53 100644 --- a/Dockerfile.openpilot +++ b/Dockerfile.openpilot @@ -1,8 +1,7 @@ FROM ubuntu:16.04 ENV PYTHONUNBUFFERED 1 -RUN apt-get update && apt-get install -y build-essential clang vim screen wget bzip2 git libglib2.0-0 python-pip capnproto libcapnp-dev libzmq5-dev libffi-dev - +RUN apt-get update && apt-get install -y build-essential clang vim screen wget bzip2 git libglib2.0-0 python-pip capnproto libcapnp-dev libzmq5-dev libffi-dev libusb-1.0-0 RUN pip install numpy==1.11.2 scipy==0.18.1 matplotlib COPY requirements_openpilot.txt /tmp/ @@ -10,4 +9,11 @@ RUN pip install -r /tmp/requirements_openpilot.txt ENV PYTHONPATH /tmp/openpilot:$PYTHONPATH -COPY . /tmp/openpilot +COPY ./common /tmp/openpilot/common +COPY ./cereal /tmp/openpilot/cereal +COPY ./opendbc /tmp/openpilot/opendbc +COPY ./selfdrive /tmp/openpilot/selfdrive +COPY ./phonelibs /tmp/openpilot/phonelibs +COPY ./pyextra /tmp/openpilot/pyextra + +RUN mkdir /tmp/openpilot/selfdrive/test/out \ No newline at end of file diff --git a/README.md b/README.md index 32513579c43765..c70574e84a735f 100644 --- a/README.md +++ b/README.md @@ -22,12 +22,20 @@ Supported Cars - Acura ILX 2016 with AcuraWatch Plus - Due to use of the cruise control for gas, it can only be enabled above 25 mph -- Honda Civic 2016 with Honda Sensing +- Honda Civic 2016-2017 with Honda Sensing - Due to limitations in steering firmware, steering is disabled below 12 mph + - Note that the hatchback model is not supported - Honda CR-V Touring 2015-2016 (very alpha!) - Can only be enabled above 25 mph +In Progress Cars +------ + +- Chevy Volt 2016-2018 Premier with Driver Confidence II + +- All 2017 Toyota Prius, Corolla, and RAV4 + Directory structure ------ diff --git a/RELEASES.md b/RELEASES.md index bb2a2b85a4bd29..bf4baad56c0113 100644 --- a/RELEASES.md +++ b/RELEASES.md @@ -1,6 +1,18 @@ -Version 0.3.6.1 (2017-08-15) -============================ - * Mitigate low speed steering oscillations on some vehicles +Version 0.3.7 (2017-09-30) +========================== + * Improved lateral control using model predictive control + * Improved lane centering + * Improved GPS + * Reduced tendency of path deviation near right side exits + * Enable engagement while the accelerator pedal is pressed + * Enable engagement while the brake pedal is pressed, when stationary and with lead vehicle within 5m + * Disable engagement when park brake or brake hold are active + * Fixed sporadic longitudinal pulsing in Civic + * Cleanups to vehicle interface + +Version 0.3.6.1 (2017-08-15) +============================ + * Mitigate low speed steering oscillations on some vehicles * Include board steering check for CR-V Version 0.3.6 (2017-08-08) diff --git a/cereal/Makefile b/cereal/Makefile index ead08bf5be4108..3a580592b12951 100644 --- a/cereal/Makefile +++ b/cereal/Makefile @@ -1,4 +1,45 @@ --include build_from_src.mk +SRCS := log.capnp car.capnp + +GENS := gen/cpp/car.capnp.c++ gen/cpp/log.capnp.c++ + + +UNAME_M ?= $(shell uname -m) + +# only generate C++ for docker tests +ifneq ($(OPTEST),1) + GENS += gen/c/car.capnp.c gen/c/log.capnp.c gen/c/c++.capnp.h gen/c/java.capnp.h + +# Dont build java on the phone... +ifeq ($(UNAME_M),x86_64) + GENS += gen/java/Car.java gen/java/Log.java +endif + +endif + +.PHONY: all +all: $(GENS) + +.PHONY: clean +clean: + rm -rf gen + +gen/c/%.capnp.c: %.capnp + @echo "[ CAPNPC C ] $@" + mkdir -p gen/c/ + capnpc '$<' -o c:gen/c/ + +gen/cpp/%.capnp.c++: %.capnp + @echo "[ CAPNPC C++ ] $@" + mkdir -p gen/cpp/ + capnpc '$<' -o c++:gen/cpp/ + +gen/java/Car.java gen/java/Log.java: $(SRCS) + @echo "[ CAPNPC java ] $@" + mkdir -p gen/java/ + capnpc $^ -o java:gen/java + +# c-capnproto needs some empty headers +gen/c/c++.capnp.h gen/c/java.capnp.h: + mkdir -p gen/c/ + touch '$@' -release: - @echo "cereal: this is a release" diff --git a/cereal/build_from_src.mk b/cereal/build_from_src.mk deleted file mode 100644 index 3a580592b12951..00000000000000 --- a/cereal/build_from_src.mk +++ /dev/null @@ -1,45 +0,0 @@ -SRCS := log.capnp car.capnp - -GENS := gen/cpp/car.capnp.c++ gen/cpp/log.capnp.c++ - - -UNAME_M ?= $(shell uname -m) - -# only generate C++ for docker tests -ifneq ($(OPTEST),1) - GENS += gen/c/car.capnp.c gen/c/log.capnp.c gen/c/c++.capnp.h gen/c/java.capnp.h - -# Dont build java on the phone... -ifeq ($(UNAME_M),x86_64) - GENS += gen/java/Car.java gen/java/Log.java -endif - -endif - -.PHONY: all -all: $(GENS) - -.PHONY: clean -clean: - rm -rf gen - -gen/c/%.capnp.c: %.capnp - @echo "[ CAPNPC C ] $@" - mkdir -p gen/c/ - capnpc '$<' -o c:gen/c/ - -gen/cpp/%.capnp.c++: %.capnp - @echo "[ CAPNPC C++ ] $@" - mkdir -p gen/cpp/ - capnpc '$<' -o c++:gen/cpp/ - -gen/java/Car.java gen/java/Log.java: $(SRCS) - @echo "[ CAPNPC java ] $@" - mkdir -p gen/java/ - capnpc $^ -o java:gen/java - -# c-capnproto needs some empty headers -gen/c/c++.capnp.h gen/c/java.capnp.h: - mkdir -p gen/c/ - touch '$@' - diff --git a/cereal/car.capnp b/cereal/car.capnp index c52a5d12af3e13..6b5a019c7b5b35 100644 --- a/cereal/car.capnp +++ b/cereal/car.capnp @@ -7,11 +7,59 @@ $Java.outerClassname("Car"); @0x8e2af1e708af8b8d; +# ******* events causing controls state machine transition ******* + +struct CarEvent @0x9b1657f34caf3ad3 { + name @0 :EventName; + enable @1 :Bool; + preEnable @7 :Bool; + noEntry @2 :Bool; + warning @3 :Bool; + userDisable @4 :Bool; + softDisable @5 :Bool; + immediateDisable @6 :Bool; + + enum EventName @0xbaa8c5d505f727de { + # TODO: copy from error list + commIssue @0; + steerUnavailable @1; + brakeUnavailable @2; + gasUnavailable @3; + wrongGear @4; + doorOpen @5; + seatbeltNotLatched @6; + espDisabled @7; + wrongCarMode @8; + steerTempUnavailable @9; + reverseGear @10; + buttonCancel @11; + buttonEnable @12; + pedalPressed @13; + cruiseDisabled @14; + radarCommIssue @15; + dataNeeded @16; + speedTooLow @17; + outOfSpace @18; + overheat @19; + calibrationInProgress @20; + calibrationInvalid @21; + controlsMismatch @22; + pcmEnable @23; + pcmDisable @24; + noTarget @25; + radarFault @26; + modelCommIssue @27; + brakeHold @28; + parkBrake @29; + } +} + # ******* main car state @ 100hz ******* # all speeds in m/s struct CarState { - errors @0: List(Error); + errorsDEPRECATED @0 :List(CarEvent.EventName); + events @13 :List(CarEvent); # car speed vEgo @1 :Float32; # best estimate of speed @@ -33,6 +81,9 @@ struct CarState { # cruise state cruiseState @10 :CruiseState; + # gear + gearShifter @14 :GearShifter; + # button presses buttonEvents @11 :List(ButtonEvent); @@ -48,31 +99,28 @@ struct CarState { } struct CruiseState { - enabled @0: Bool; - speed @1: Float32; - available @2: Bool; + enabled @0 :Bool; + speed @1 :Float32; + available @2 :Bool; + speedOffset @3 :Float32; } - enum Error { - # TODO: copy from error list - commIssue @0; - steerUnavailable @1; - brakeUnavailable @2; - gasUnavailable @3; - wrongGear @4; - doorOpen @5; - seatbeltNotLatched @6; - espDisabled @7; - wrongCarMode @8; - steerTempUnavailable @9; - reverseGear @10; - # ... + enum GearShifter { + unknown @0; + park @1; + drive @2; + neutral @3; + reverse @4; + sport @5; + low @6; + brake @7; } + # send on change struct ButtonEvent { - pressed @0: Bool; - type @1: Type; + pressed @0 :Bool; + type @1 :Type; enum Type { unknown @0; @@ -91,29 +139,30 @@ struct CarState { # ******* radar state @ 20hz ******* struct RadarState { - errors @0: List(Error); - points @1: List(RadarPoint); + errors @0 :List(Error); + points @1 :List(RadarPoint); # which packets this state came from - canMonoTimes @2: List(UInt64); + canMonoTimes @2 :List(UInt64); enum Error { - notValid @0; + commIssue @0; + fault @1; } # similar to LiveTracks # is one timestamp valid for all? I think so struct RadarPoint { - trackId @0: UInt64; # no trackId reuse + trackId @0 :UInt64; # no trackId reuse # these 3 are the minimum required - dRel @1: Float32; # m from the front bumper of the car - yRel @2: Float32; # m - vRel @3: Float32; # m/s + dRel @1 :Float32; # m from the front bumper of the car + yRel @2 :Float32; # m + vRel @3 :Float32; # m/s # these are optional and valid if they are not NaN - aRel @4: Float32; # m/s^2 - yvRel @5: Float32; # m/s + aRel @4 :Float32; # m/s^2 + yvRel @5 :Float32; # m/s } } @@ -121,17 +170,24 @@ struct RadarState { struct CarControl { # must be true for any actuator commands to work - enabled @0: Bool; + enabled @0 :Bool; - # range from 0.0 - 1.0 - gas @1: Float32; - brake @2: Float32; + gasDEPRECATED @1 :Float32; + brakeDEPRECATED @2 :Float32; + steeringTorqueDEPRECATED @3 :Float32; - # range from -1.0 - 1.0 - steeringTorque @3 :Float32; + actuators @6 :Actuators; - cruiseControl @4: CruiseControl; - hudControl @5: HUDControl; + cruiseControl @4 :CruiseControl; + hudControl @5 :HUDControl; + + struct Actuators { + # range from 0.0 - 1.0 + gas @0: Float32; + brake @1: Float32; + # range from -1.0 - 1.0 + steer @2: Float32; + } struct CruiseControl { cancel @0: Bool; @@ -178,31 +234,48 @@ struct CarControl { # ****** car param ****** struct CarParams { - carName @0: Text; - radarName @1: Text; - carFingerprint @2: Text; - - enableSteer @3: Bool; - enableGas @4: Bool; - enableBrake @5: Bool; - enableCruise @6: Bool; + carName @0 :Text; + radarName @1 :Text; + carFingerprint @2 :Text; + + enableSteer @3 :Bool; + enableGas @4 :Bool; + enableBrake @5 :Bool; + enableCruise @6 :Bool; + + minEnableSpeed @18 :Float32; + safetyModel @19 :Int16; + + steerMaxBP @20 :List(Float32); + steerMaxV @21 :List(Float32); + gasMaxBP @22 :List(Float32); + gasMaxV @23 :List(Float32); + brakeMaxBP @24 :List(Float32); + brakeMaxV @25 :List(Float32); + + enum SafetyModels { + # from board + default @0; + honda @1; + toyota @2; + } # things about the car in the manual - m @7: Float32; # [kg] running weight - l @8: Float32; # [m] wheelbase - sR @9: Float32; # [] steering ratio - aF @10: Float32; # [m] GC distance to front axle - aR @11: Float32; # [m] GC distance to rear axle - chi @12: Float32; # [] rear steering ratio wrt front steering (usually 0) + m @7 :Float32; # [kg] running weight + l @8 :Float32; # [m] wheelbase + sR @9 :Float32; # [] steering ratio + aF @10 :Float32; # [m] GC distance to front axle + aR @11 :Float32; # [m] GC distance to rear axle + chi @12 :Float32; # [] rear steering ratio wrt front steering (usually 0) # things we can derive - j @13: Float32; # [kg*m2] body rot inertia - cF @14: Float32; # [N/rad] front tire coeff of stiff - cR @15: Float32; # [N/rad] rear tire coeff of stiff + j @13 :Float32; # [kg*m2] body rotational inertia + cF @14 :Float32; # [N/rad] front tire coeff of stiff + cR @15 :Float32; # [N/rad] rear tire coeff of stiff # Kp and Ki for the lateral control - steerKp @16: Float32; - steerKi @17: Float32; + steerKp @16 :Float32; + steerKi @17 :Float32; # TODO: Kp and Ki for long control, perhaps not needed? } diff --git a/cereal/log.capnp b/cereal/log.capnp index 183ca904d8dee4..1496ff135692ce 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -92,10 +92,10 @@ struct InitData { } struct PandaInfo { - hasPanda @0: Bool; - dongleId @1: Text; - stVersion @2: Text; - espVersion @3: Text; + hasPanda @0 :Bool; + dongleId @1 :Text; + stVersion @2 :Text; + espVersion @3 :Text; } } @@ -110,9 +110,10 @@ struct FrameData { frameType @7 :FrameType; timestampSof @8 :UInt64; + transform @10 :List(Float32); androidCaptureResult @9 :AndroidCaptureResult; - + enum FrameType { unknown @0; neo @1; @@ -196,10 +197,10 @@ struct GpsLocationData { timestamp @7 :Int64; source @8 :SensorSource; - + # Represents NED velocity in m/s. vNED @9 :List(Float32); - + # Represents expected vertical accuracy in meters. (presumably 1 sigma?) verticalAccuracy @10 :Float32; @@ -239,7 +240,9 @@ struct ThermalData { # not thermal freeSpace @7 :Float32; batteryPercent @8 :Int16; - batteryStatus @9: Text; + batteryStatus @9 :Text; + + fanSpeed @10 :UInt16; } struct HealthData { @@ -254,8 +257,8 @@ struct HealthData { struct LiveUI { rearViewCam @0 :Bool; - alertText1 @1 :Text; - alertText2 @2 :Text; + alertText1 @1 :Text; + alertText2 @2 :Text; awarenessStatus @3 :Float32; } @@ -264,6 +267,7 @@ struct Live20Data { mdMonoTime @6 :UInt64; ftMonoTimeDEPRECATED @7 :UInt64; l100MonoTime @11 :UInt64; + radarErrors @12 :List(Car.RadarState.Error); # all deprecated warpMatrixDEPRECATED @0 :List(Float32); @@ -294,6 +298,7 @@ struct Live20Data { struct LiveCalibrationData { warpMatrix @0 :List(Float32); + warpMatrix2 @5 :List(Float32); calStatus @1 :Int8; calCycle @2 :Int32; calPerc @3 :Int8; @@ -340,14 +345,14 @@ struct Live100Data { hudLeadDEPRECATED @14 :Int32; cumLagMs @15 :Float32; - enabled @19: Bool; - steerOverride @20: Bool; + enabled @19 :Bool; + steerOverride @20 :Bool; - vCruise @22: Float32; + vCruise @22 :Float32; rearViewCam @23 :Bool; - alertText1 @24 :Text; - alertText2 @25 :Text; + alertText1 @24 :Text; + alertText2 @25 :Text; awarenessStatus @26 :Float32; angleOffset @27 :Float32; @@ -387,6 +392,7 @@ struct ModelData { bigBoxHeight @3 :UInt16; boxProjection @4 :List(Float32); yuvCorrection @5 :List(Float32); + inputTransform @6 :List(Float32); } } @@ -399,7 +405,7 @@ struct CalibrationFeatures { } struct EncodeIndex { - # picture from camera + # picture from camera frameId @0 :UInt32; type @1 :Type; # index of encoder from start of route @@ -438,69 +444,79 @@ struct LogRotate { struct Plan { mdMonoTime @9 :UInt64; l20MonoTime @10 :UInt64; + events @13 :List(Car.CarEvent); # lateral, 3rd order polynomial - lateralValid @0: Bool; + lateralValid @0 :Bool; dPoly @1 :List(Float32); + laneWidth @11 :Float32; # longitudinal - longitudinalValid @2: Bool; + longitudinalValid @2 :Bool; vTarget @3 :Float32; aTargetMin @4 :Float32; aTargetMax @5 :Float32; jerkFactor @6 :Float32; hasLead @7 :Bool; fcw @8 :Bool; + + # gps trajectory in car frame + gpsTrajectory @12 :GpsTrajectory; + + struct GpsTrajectory { + x @0 :List(Float32); + y @1 :List(Float32); + } } struct LiveLocationData { - status @0: UInt8; + status @0 :UInt8; - # 3D fix - lat @1: Float64; - lon @2: Float64; - alt @3: Float32; # m + # 3D fix + lat @1 :Float64; + lon @2 :Float64; + alt @3 :Float32; # m # speed - speed @4: Float32; # m/s + speed @4 :Float32; # m/s # NED velocity components - vNED @5: List(Float32); + vNED @5 :List(Float32); # roll, pitch, heading (x,y,z) - roll @6: Float32; # WRT to center of earth? - pitch @7: Float32; # WRT to center of earth? - heading @8: Float32; # WRT to north? + roll @6 :Float32; # WRT to center of earth? + pitch @7 :Float32; # WRT to center of earth? + heading @8 :Float32; # WRT to north? # what are these? - wanderAngle @9: Float32; - trackAngle @10: Float32; + wanderAngle @9 :Float32; + trackAngle @10 :Float32; # car frame -- https://upload.wikimedia.org/wikipedia/commons/f/f5/RPY_angles_of_cars.png # gyro, in car frame, deg/s - gyro @11: List(Float32); + gyro @11 :List(Float32); # accel, in car frame, m/s^2 - accel @12: List(Float32); + accel @12 :List(Float32); - accuracy @13: Accuracy; + accuracy @13 :Accuracy; struct Accuracy { - pNEDError @0: List(Float32); - vNEDError @1: List(Float32); - rollError @2: Float32; - pitchError @3: Float32; - headingError @4: Float32; - ellipsoidSemiMajorError @5: Float32; - ellipsoidSemiMinorError @6: Float32; - ellipsoidOrientationError @7: Float32; + pNEDError @0 :List(Float32); + vNEDError @1 :List(Float32); + rollError @2 :Float32; + pitchError @3 :Float32; + headingError @4 :Float32; + ellipsoidSemiMajorError @5 :Float32; + ellipsoidSemiMinorError @6 :Float32; + ellipsoidOrientationError @7 :Float32; } } struct EthernetPacket { pkt @0 :Data; - ts @1: Float32; + ts @1 :Float32; } struct NavUpdate { @@ -602,7 +618,7 @@ struct AndroidGnss { hasLeapSecond @4 :Bool; leapSecond @5 :Int32; - + hasFullBiasNanos @6 :Bool; fullBiasNanos @7 :Int64; @@ -611,10 +627,10 @@ struct AndroidGnss { hasBiasUncertaintyNanos @10 :Bool; biasUncertaintyNanos @11 :Float64; - + hasDriftNanosPerSecond @12 :Bool; driftNanosPerSecond @13 :Float64; - + hasDriftUncertaintyNanosPerSecond @14 :Bool; driftUncertaintyNanosPerSecond @15 :Float64; } @@ -633,7 +649,7 @@ struct AndroidGnss { accumulatedDeltaRangeState @9 :Int32; accumulatedDeltaRangeMeters @10 :Float64; accumulatedDeltaRangeUncertaintyMeters @11 :Float64; - + hasCarrierFrequencyHz @12 :Bool; carrierFrequencyHz @13 :Float32; hasCarrierCycles @14 :Bool; @@ -705,6 +721,8 @@ struct QcomGnss { measurementReport @1 :MeasurementReport; clockReport @2 :ClockReport; drMeasurementReport @3 :DrMeasurementReport; + drSvPoly @4 :DrSvPolyReport; + rawLog @5 :Data; } enum MeasurementSource @0xd71a12b6faada7ee { @@ -742,18 +760,18 @@ struct QcomGnss { measurementNotUsable @12 :Bool; sirCheckIsNeeded @13 :Bool; probationMode @14 :Bool; - + glonassMeanderBitEdgeValid @15 :Bool; glonassTimeMarkValid @16 :Bool; - + gpsRoundRobinRxDiversity @17 :Bool; gpsRxDiversity @18 :Bool; gpsLowBandwidthRxDiversityCombined @19 :Bool; gpsHighBandwidthNu4 @20 :Bool; gpsHighBandwidthNu8 @21 :Bool; gpsHighBandwidthUniform @22 :Bool; - gpsMultipathIndicator @23 :Bool; - + multipathIndicator @23 :Bool; + imdJammingIndicator @24 :Bool; lteB13TxJammingIndicator @25 :Bool; freshMeasurementIndicator @26 :Bool; @@ -953,6 +971,37 @@ struct QcomGnss { goodParity @32 :Bool; } } + + struct DrSvPolyReport { + svId @0 :UInt16; + frequencyIndex @1 :Int8; + + hasPosition @2 :Bool; + hasIono @3 :Bool; + hasTropo @4 :Bool; + hasElevation @5 :Bool; + polyFromXtra @6 :Bool; + hasSbasIono @7 :Bool; + + iode @8 :UInt16; + t0 @9 :Float64; + xyz0 @10 :List(Float64); + xyzN @11 :List(Float64); + other @12 :List(Float32); + + positionUncertainty @13 :Float32; + ionoDelay @14 :Float32; + ionoDot @15 :Float32; + sbasIonoDelay @16 :Float32; + sbasIonoDot @17 :Float32; + tropoDelay @18 :Float32; + elevation @19 :Float32; + elevationDot @20 :Float32; + elevationUncertainty @21 :Float32; + + velocityCoeff @22 :List(Float64); + + } } struct LidarPts { @@ -1037,12 +1086,12 @@ struct UbloxGnss { # num of measurements to follow numMeas @4 :UInt8; measurements @5 :List(Measurement); - + struct ReceiverStatus { - # leap seconds have been determined + # leap seconds have been determined leapSecValid @0 :Bool; # Clock reset applied - clkReset @1 : Bool; + clkReset @1 :Bool; } struct Measurement { @@ -1060,7 +1109,7 @@ struct UbloxGnss { # carrier phase locktime counter in ms locktime @7 :UInt16; # Carrier-to-noise density ratio (signal strength) in dBHz - cno @8 : UInt8; + cno @8 :UInt8; # pseudorange standard deviation in meters pseudorangeStdev @9 :Float32; # carrier phase standard deviation in cycles @@ -1103,31 +1152,47 @@ struct UbloxGnss { ecc @15 :Float64; cus @16 :Float64; a @17 :Float64; # note that this is not the root!! - + toe @18 :Float64; cic @19 :Float64; omega0 @20 :Float64; cis @21 :Float64; - + i0 @22 :Float64; crc @23 :Float64; omega @24 :Float64; omegaDot @25 :Float64; - + iDot @26 :Float64; codesL2 @27 :Float64; gpsWeek @28 :Float64; l2 @29 :Float64; - + svAcc @30 :Float64; svHealth @31 :Float64; tgd @32 :Float64; iodc @33 :Float64; - + transmissionTime @34 :Float64; fitInterval @35 :Float64; } } + +struct Clocks { + bootTimeNanos @0 :UInt64; + monotonicNanos @1 :UInt64; + monotonicRawNanos @2 :UInt64; + wallTimeNanos @3 :UInt64; + modemUptimeMillis @4 :UInt64; +} + +struct LiveMpcData { + x @0 :List(Float32); + y @1 :List(Float32); + psi @2 :List(Float32); + delta @3 :List(Float32); +} + struct Event { # in nanoseconds? logMonoTime @0 :UInt64; @@ -1144,7 +1209,7 @@ struct Event { model @9 :ModelData; features @10 :CalibrationFeatures; sensorEvents @11 :List(SensorEventData); - health @12 : HealthData; + health @12 :HealthData; live20 @13 :Live20Data; liveUIDEPRECATED @14 :LiveUI; encodeIdx @15 :EncodeIndex; @@ -1167,5 +1232,7 @@ struct Event { lidarPts @32 :LidarPts; procLog @33 :ProcLog; ubloxGnss @34 :UbloxGnss; + clocks @35 :Clocks; + liveMpc @36 :LiveMpcData; } } diff --git a/common/basedir.py b/common/basedir.py new file mode 100644 index 00000000000000..991f6aaed92ae4 --- /dev/null +++ b/common/basedir.py @@ -0,0 +1,4 @@ +import os +BASEDIR = os.path.join(os.path.dirname(os.path.realpath(__file__)), "../") + + diff --git a/common/fingerprints.py b/common/fingerprints.py index 298424940b5131..34c84b7449de75 100644 --- a/common/fingerprints.py +++ b/common/fingerprints.py @@ -18,6 +18,9 @@ 57L: 3, 145L: 8, 316L: 8, 340L: 8, 342L: 6, 344L: 8, 380L: 8, 398L: 3, 399L: 6, 401L: 8, 420L: 8, 422L: 8, 426L: 8, 432L: 7, 464L: 8, 474L: 5, 476L: 4, 487L: 4, 490L: 8, 493L: 3, 507L: 1, 542L: 7, 545L: 4, 597L: 8, 660L: 8, 661L: 4, 773L: 7, 777L: 8, 800L: 8, 804L: 8, 808L: 8, 882L: 2, 884L: 7, 888L: 8, 891L: 8, 892L: 8, 923L: 2, 929L: 8, 983L: 8, 985L: 3, 1024L: 5, 1027L: 5, 1029L: 8, 1033L: 5, 1036L: 8, 1039L: 8, 1057L: 5, 1064L: 7, 1108L: 8, 1125L: 8, 1296L: 8, 1365L: 5, 1424L: 5, 1600L: 5, 1601L: 8, # sent messages 0x194: 4, 0x1fa: 8, 0x30c: 8, 0x33d: 5, + }, + "TOYOTA PRIUS 2017": { + 896L: 8, 832L: 8, 898L: 8, 899L: 8, 577L: 8, 528L: 8, 529L: 8, 530L: 8, 531L: 8, 532L: 8, 533L: 8, 534L: 8, 535L: 8, 536L: 8, 537L: 8, 538L: 8, 539L: 8, 540L: 8, 541L: 8, 542L: 8, 543L: 8, 544L: 8, 545L: 8, 546L: 8, 547L: 8, 548L: 8, 549L: 8, 550L: 8, 551L: 8, 296L: 6, 553L: 6, 554L: 6, 555L: 6, 556L: 6, 557L: 6, 558L: 6, 559L: 6, 560L: 7, 561L: 8, 562L: 8, 883L: 8, 837L: 8, 833L: 8, 576L: 8, 321L: 4, 834L: 8, 835L: 8, 580L: 8, 581L: 8, 897L: 8, 584L: 8, 1136L: 4, 976L: 8, 977L: 8, 978L: 8, 291L: 7, 881L: 8, 352L: 8, 353L: 7, 867L: 8, 868L: 8, 869L: 8, 1126L: 3, 304L: 7, 880L: 8, 552L: 6, 882L: 8, 979L: 8, 884L: 8, 885L: 8, 836L: 8 } } diff --git a/common/params.py b/common/params.py index 8e8a11edc207c8..ca3ee99923377f 100755 --- a/common/params.py +++ b/common/params.py @@ -66,6 +66,13 @@ class UnknownKeyName(Exception): # read: radard "CarParams": TxType.CLEAR_ON_CAR_START} +def fsync_dir(path): + fd = os.open(path, os.O_RDONLY) + try: + os.fsync(fd) + finally: + os.close(fd) + class FileLock(object): def __init__(self, path, create): @@ -182,15 +189,32 @@ def __exit__(self, type, value, traceback): self._check_entered() try: + # data_path refers to the externally used path to the params. It is a symlink. + # old_data_path is the path currently pointed to by data_path. + # tempdir_path is a path where the new params will go, which the new data path will point to. + # new_data_path is a temporary symlink that will atomically overwrite data_path. + # + # The current situation is: + # data_path -> old_data_path + # We're going to write params data to tempdir_path + # tempdir_path -> params data + # Then point new_data_path to tempdir_path + # new_data_path -> tempdir_path + # Then atomically overwrite data_path with new_data_path + # data_path -> tempdir_path old_data_path = None new_data_path = None tempdir_path = tempfile.mkdtemp(prefix=".tmp", dir=self._path) + try: # Write back all keys. os.chmod(tempdir_path, 0o777) for k, v in self._vals.items(): with open(os.path.join(tempdir_path, k), "wb") as f: f.write(v) + f.flush() + os.fsync(f.fileno()) + fsync_dir(tempdir_path) data_path = self._data_path() try: @@ -203,16 +227,21 @@ def __exit__(self, type, value, traceback): new_data_path = "{}.link".format(tempdir_path) os.symlink(os.path.basename(tempdir_path), new_data_path) os.rename(new_data_path, data_path) - # TODO(mgraczyk): raise useful error when values are bad. - except: - shutil.rmtree(tempdir_path) - if new_data_path is not None: + fsync_dir(self._path) + finally: + # If the rename worked, we can delete the old data. Otherwise delete the new one. + success = new_data_path is not None and os.path.exists(data_path) and ( + os.readlink(data_path) == os.path.basename(tempdir_path)) + + if success: + if old_data_path is not None: + shutil.rmtree(old_data_path) + else: + shutil.rmtree(tempdir_path) + + # Regardless of what happened above, there should be no link at new_data_path. + if new_data_path is not None and os.path.islink(new_data_path): os.remove(new_data_path) - raise - - # Keep holding the lock while we clean up the old data. - if old_data_path is not None: - shutil.rmtree(old_data_path) finally: os.umask(self._prev_umask) self._prev_umask = None @@ -249,6 +278,10 @@ def manager_start(self): def car_start(self): self._clear_keys_with_type(TxType.CLEAR_ON_CAR_START) + def delete(self, key): + with self.env.begin(write=True) as txn: + txn.delete(key) + def get(self, key, block=False): if key not in keys: raise UnknownKeyName(key) diff --git a/common/profiler.py b/common/profiler.py index b35a52d16fd81f..26fbeb382b57d9 100644 --- a/common/profiler.py +++ b/common/profiler.py @@ -14,6 +14,12 @@ def checkpoint(self, name): self.cp.append((name, tt - self.last_time)) self.last_time = tt + def reset(self, enabled=False): + self.enabled = enabled + self.cp = [] + self.start_time = sec_since_boot() + self.last_time = self.start_time + def display(self): if not self.enabled: return diff --git a/opendbc b/opendbc index 008089045e371a..b8c0034c5d6ded 160000 --- a/opendbc +++ b/opendbc @@ -1 +1 @@ -Subproject commit 008089045e371a9eebbe79d978ff22ae3e70bdba +Subproject commit b8c0034c5d6ded3d85a4f0414b3509baae35a34b diff --git a/panda b/panda index 4901d52104e369..5ea4df111b735c 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit 4901d52104e369f2444b5d56846199fdfc3fd695 +Subproject commit 5ea4df111b735ce9efb27ab2e0f87837210f6fc3 diff --git a/phonelibs/qpoases/EXAMPLES/example1.cpp b/phonelibs/qpoases/EXAMPLES/example1.cpp new file mode 100644 index 00000000000000..92b47fc93798d2 --- /dev/null +++ b/phonelibs/qpoases/EXAMPLES/example1.cpp @@ -0,0 +1,74 @@ +/* + * This file is part of qpOASES. + * + * qpOASES -- An Implementation of the Online Active Set Strategy. + * Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved. + * + * qpOASES is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * qpOASES is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with qpOASES; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + + +/** + * \file EXAMPLES/example1.cpp + * \author Hans Joachim Ferreau + * \version 1.3embedded + * \date 2007-2008 + * + * Very simple example for testing qpOASES (using QProblem class). + */ + + +#include + + +/** Example for qpOASES main function using the QProblem class. */ +int main( ) +{ + /* Setup data of first QP. */ + real_t H[2*2] = { 1.0, 0.0, 0.0, 0.5 }; + real_t A[1*2] = { 1.0, 1.0 }; + real_t g[2] = { 1.5, 1.0 }; + real_t lb[2] = { 0.5, -2.0 }; + real_t ub[2] = { 5.0, 2.0 }; + real_t lbA[1] = { -1.0 }; + real_t ubA[1] = { 2.0 }; + + /* Setup data of second QP. */ + real_t g_new[2] = { 1.0, 1.5 }; + real_t lb_new[2] = { 0.0, -1.0 }; + real_t ub_new[2] = { 5.0, -0.5 }; + real_t lbA_new[1] = { -2.0 }; + real_t ubA_new[1] = { 1.0 }; + + + /* Setting up QProblem object. */ + QProblem example( 2,1 ); + + /* Solve first QP. */ + int nWSR = 10; + example.init( H,g,A,lb,ub,lbA,ubA, nWSR,0 ); + + /* Solve second QP. */ + nWSR = 10; + example.hotstart( g_new,lb_new,ub_new,lbA_new,ubA_new, nWSR,0 ); + + return 0; +} + + +/* + * end of file + */ diff --git a/phonelibs/qpoases/EXAMPLES/example1b.cpp b/phonelibs/qpoases/EXAMPLES/example1b.cpp new file mode 100644 index 00000000000000..331f19d8da0cbc --- /dev/null +++ b/phonelibs/qpoases/EXAMPLES/example1b.cpp @@ -0,0 +1,69 @@ +/* + * This file is part of qpOASES. + * + * qpOASES -- An Implementation of the Online Active Set Strategy. + * Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved. + * + * qpOASES is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * qpOASES is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with qpOASES; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + + +/** + * \file EXAMPLES/example1b.cpp + * \author Hans Joachim Ferreau + * \version 1.3 + * \date 2007-2008 + * + * Very simple example for testing qpOASES using the QProblemB class. + */ + + +#include + + +/** Example for qpOASES main function using the QProblemB class. */ +int main( ) +{ + /* Setup data of first QP. */ + real_t H[2*2] = { 1.0, 0.0, 0.0, 0.5 }; + real_t g[2] = { 1.5, 1.0 }; + real_t lb[2] = { 0.5, -2.0 }; + real_t ub[2] = { 5.0, 2.0 }; + + /* Setup data of second QP. */ + real_t g_new[2] = { 1.0, 1.5 }; + real_t lb_new[2] = { 0.0, -1.0 }; + real_t ub_new[2] = { 5.0, -0.5 }; + + + /* Setting up QProblemB object. */ + QProblemB example( 2 ); + + /* Solve first QP. */ + int nWSR = 10; + example.init( H,g,lb,ub, nWSR,0 ); + + /* Solve second QP. */ + nWSR = 10; + example.hotstart( g_new,lb_new,ub_new, nWSR,0 ); + + return 0; +} + + +/* + * end of file + */ diff --git a/phonelibs/qpoases/INCLUDE/Bounds.hpp b/phonelibs/qpoases/INCLUDE/Bounds.hpp new file mode 100644 index 00000000000000..1fbd28ab38d342 --- /dev/null +++ b/phonelibs/qpoases/INCLUDE/Bounds.hpp @@ -0,0 +1,189 @@ +/* + * This file is part of qpOASES. + * + * qpOASES -- An Implementation of the Online Active Set Strategy. + * Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved. + * + * qpOASES is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * qpOASES is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with qpOASES; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + + +/** + * \file INCLUDE/Bounds.hpp + * \author Hans Joachim Ferreau + * \version 1.3embedded + * \date 2007-2008 + * + * Declaration of the Bounds class designed to manage working sets of + * bounds within a QProblem. + */ + + +#ifndef QPOASES_BOUNDS_HPP +#define QPOASES_BOUNDS_HPP + + +#include + + + +/** This class manages working sets of bounds by storing + * index sets and other status information. + * + * \author Hans Joachim Ferreau + * \version 1.3embedded + * \date 2007-2008 + */ +class Bounds : public SubjectTo +{ + /* + * PUBLIC MEMBER FUNCTIONS + */ + public: + /** Default constructor. */ + Bounds( ); + + /** Copy constructor (deep copy). */ + Bounds( const Bounds& rhs /**< Rhs object. */ + ); + + /** Destructor. */ + ~Bounds( ); + + /** Assignment operator (deep copy). */ + Bounds& operator=( const Bounds& rhs /**< Rhs object. */ + ); + + + /** Pseudo-constructor takes the number of bounds. + * \return SUCCESSFUL_RETURN */ + returnValue init( int n /**< Number of bounds. */ + ); + + + /** Initially adds number of a new (i.e. not yet in the list) bound to + * given index set. + * \return SUCCESSFUL_RETURN \n + RET_SETUP_BOUND_FAILED \n + RET_INDEX_OUT_OF_BOUNDS \n + RET_INVALID_ARGUMENTS */ + returnValue setupBound( int _number, /**< Number of new bound. */ + SubjectToStatus _status /**< Status of new bound. */ + ); + + /** Initially adds all numbers of new (i.e. not yet in the list) bounds to + * to the index set of free bounds; the order depends on the SujectToType + * of each index. + * \return SUCCESSFUL_RETURN \n + RET_SETUP_BOUND_FAILED */ + returnValue setupAllFree( ); + + + /** Moves index of a bound from index list of fixed to that of free bounds. + * \return SUCCESSFUL_RETURN \n + RET_MOVING_BOUND_FAILED \n + RET_INDEX_OUT_OF_BOUNDS */ + returnValue moveFixedToFree( int _number /**< Number of bound to be freed. */ + ); + + /** Moves index of a bound from index list of free to that of fixed bounds. + * \return SUCCESSFUL_RETURN \n + RET_MOVING_BOUND_FAILED \n + RET_INDEX_OUT_OF_BOUNDS */ + returnValue moveFreeToFixed( int _number, /**< Number of bound to be fixed. */ + SubjectToStatus _status /**< Status of bound to be fixed. */ + ); + + /** Swaps the indices of two free bounds within the index set. + * \return SUCCESSFUL_RETURN \n + RET_SWAPINDEX_FAILED */ + returnValue swapFree( int number1, /**< Number of first constraint or bound. */ + int number2 /**< Number of second constraint or bound. */ + ); + + + /** Returns number of variables. + * \return Number of variables. */ + inline int getNV( ) const; + + /** Returns number of implicitly fixed variables. + * \return Number of implicitly fixed variables. */ + inline int getNFV( ) const; + + /** Returns number of bounded (but possibly free) variables. + * \return Number of bounded (but possibly free) variables. */ + inline int getNBV( ) const; + + /** Returns number of unbounded variables. + * \return Number of unbounded variables. */ + inline int getNUV( ) const; + + + /** Sets number of implicitly fixed variables. + * \return SUCCESSFUL_RETURN */ + inline returnValue setNFV( int n /**< Number of implicitly fixed variables. */ + ); + + /** Sets number of bounded (but possibly free) variables. + * \return SUCCESSFUL_RETURN */ + inline returnValue setNBV( int n /**< Number of bounded (but possibly free) variables. */ + ); + + /** Sets number of unbounded variables. + * \return SUCCESSFUL_RETURN */ + inline returnValue setNUV( int n /**< Number of unbounded variables */ + ); + + + /** Returns number of free variables. + * \return Number of free variables. */ + inline int getNFR( ); + + /** Returns number of fixed variables. + * \return Number of fixed variables. */ + inline int getNFX( ); + + + /** Returns a pointer to free variables index list. + * \return Pointer to free variables index list. */ + inline Indexlist* getFree( ); + + /** Returns a pointer to fixed variables index list. + * \return Pointer to fixed variables index list. */ + inline Indexlist* getFixed( ); + + + /* + * PROTECTED MEMBER VARIABLES + */ + protected: + int nV; /**< Number of variables (nV = nFV + nBV + nUV). */ + int nFV; /**< Number of implicitly fixed variables. */ + int nBV; /**< Number of bounded (but possibly free) variables. */ + int nUV; /**< Number of unbounded variables. */ + + Indexlist free; /**< Index list of free variables. */ + Indexlist fixed; /**< Index list of fixed variables. */ +}; + +#include + +#endif /* QPOASES_BOUNDS_HPP */ + + +/* + * end of file + */ diff --git a/phonelibs/qpoases/INCLUDE/Constants.hpp b/phonelibs/qpoases/INCLUDE/Constants.hpp new file mode 100644 index 00000000000000..8293fa514c6a9c --- /dev/null +++ b/phonelibs/qpoases/INCLUDE/Constants.hpp @@ -0,0 +1,108 @@ +/* + * This file is part of qpOASES. + * + * qpOASES -- An Implementation of the Online Active Set Strategy. + * Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved. + * + * qpOASES is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * qpOASES is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with qpOASES; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + + +/** + * \file INCLUDE/Constants.hpp + * \author Hans Joachim Ferreau + * \version 1.3embedded + * \date 2008 + * + * Definition of all global constants. + */ + + +#ifndef QPOASES_CONSTANTS_HPP +#define QPOASES_CONSTANTS_HPP + +#ifndef QPOASES_CUSTOM_INTERFACE +#include "acado_qpoases_interface.hpp" +#else + #define XSTR(x) #x + #define STR(x) XSTR(x) + #include STR(QPOASES_CUSTOM_INTERFACE) +#endif + +/** Maximum number of variables within a QP formulation. + Note: this value has to be positive! */ +const int NVMAX = QPOASES_NVMAX; + +/** Maximum number of constraints within a QP formulation. + Note: this value has to be positive! */ +const int NCMAX = QPOASES_NCMAX; + +/** Redefinition of NCMAX used for memory allocation, to avoid zero sized arrays + and compiler errors. */ +const int NCMAX_ALLOC = (NCMAX == 0) ? 1 : NCMAX; + +/**< Maximum number of working set recalculations. + Note: this value has to be positive! */ +const int NWSRMAX = QPOASES_NWSRMAX; + +/** Desired KKT tolerance of QP solution; a warning RET_INACCURATE_SOLUTION is + * issued if this tolerance is not met. + * Note: this value has to be positive! */ +const real_t DESIREDACCURACY = (real_t) 1.0e-3; + +/** Critical KKT tolerance of QP solution; an error is issued if this + * tolerance is not met. + * Note: this value has to be positive! */ +const real_t CRITICALACCURACY = (real_t) 1.0e-2; + + + +/** Numerical value of machine precision (min eps, s.t. 1+eps > 1). + Note: this value has to be positive! */ +const real_t EPS = (real_t) QPOASES_EPS; + +/** Numerical value of zero (for situations in which it would be + * unreasonable to compare with 0.0). + * Note: this value has to be positive! */ +const real_t ZERO = (real_t) 1.0e-50; + +/** Numerical value of infinity (e.g. for non-existing bounds). + * Note: this value has to be positive! */ +const real_t INFTY = (real_t) 1.0e12; + + +/** Lower/upper (constraints') bound tolerance (an inequality constraint + * whose lower and upper bound differ by less than BOUNDTOL is regarded + * to be an equality constraint). + * Note: this value has to be positive! */ +const real_t BOUNDTOL = (real_t) 1.0e-10; + +/** Offset for relaxing (constraints') bounds at beginning of an initial homotopy. + * Note: this value has to be positive! */ +const real_t BOUNDRELAXATION = (real_t) 1.0e3; + + +/** Factor that determines physical lengths of index lists. + * Note: this value has to be greater than 1! */ +const int INDEXLISTFACTOR = 5; + + +#endif /* QPOASES_CONSTANTS_HPP */ + + +/* + * end of file + */ diff --git a/phonelibs/qpoases/INCLUDE/Constraints.hpp b/phonelibs/qpoases/INCLUDE/Constraints.hpp new file mode 100644 index 00000000000000..0b78747e29afeb --- /dev/null +++ b/phonelibs/qpoases/INCLUDE/Constraints.hpp @@ -0,0 +1,181 @@ +/* + * This file is part of qpOASES. + * + * qpOASES -- An Implementation of the Online Active Set Strategy. + * Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved. + * + * qpOASES is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * qpOASES is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with qpOASES; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + + +/** + * \file INCLUDE/Constraints.hpp + * \author Hans Joachim Ferreau + * \version 1.3embedded + * \date 2007-2008 + * + * Declaration of the Constraints class designed to manage working sets of + * constraints within a QProblem. + */ + + +#ifndef QPOASES_CONSTRAINTS_HPP +#define QPOASES_CONSTRAINTS_HPP + + +#include + + + +/** This class manages working sets of constraints by storing + * index sets and other status information. + * + * \author Hans Joachim Ferreau + * \version 1.3embedded + * \date 2007-2008 + */ +class Constraints : public SubjectTo +{ + /* + * PUBLIC MEMBER FUNCTIONS + */ + public: + /** Default constructor. */ + Constraints( ); + + /** Copy constructor (deep copy). */ + Constraints( const Constraints& rhs /**< Rhs object. */ + ); + + /** Destructor. */ + ~Constraints( ); + + /** Assignment operator (deep copy). */ + Constraints& operator=( const Constraints& rhs /**< Rhs object. */ + ); + + + /** Pseudo-constructor takes the number of constraints. + * \return SUCCESSFUL_RETURN */ + returnValue init( int n /**< Number of constraints. */ + ); + + + /** Initially adds number of a new (i.e. not yet in the list) constraint to + * a given index set. + * \return SUCCESSFUL_RETURN \n + RET_SETUP_CONSTRAINT_FAILED \n + RET_INDEX_OUT_OF_BOUNDS \n + RET_INVALID_ARGUMENTS */ + returnValue setupConstraint( int _number, /**< Number of new constraint. */ + SubjectToStatus _status /**< Status of new constraint. */ + ); + + /** Initially adds all enabled numbers of new (i.e. not yet in the list) constraints to + * to the index set of inactive constraints; the order depends on the SujectToType + * of each index. Only disabled constraints are added to index set of disabled constraints! + * \return SUCCESSFUL_RETURN \n + RET_SETUP_CONSTRAINT_FAILED */ + returnValue setupAllInactive( ); + + + /** Moves index of a constraint from index list of active to that of inactive constraints. + * \return SUCCESSFUL_RETURN \n + RET_MOVING_CONSTRAINT_FAILED */ + returnValue moveActiveToInactive( int _number /**< Number of constraint to become inactive. */ + ); + + /** Moves index of a constraint from index list of inactive to that of active constraints. + * \return SUCCESSFUL_RETURN \n + RET_MOVING_CONSTRAINT_FAILED */ + returnValue moveInactiveToActive( int _number, /**< Number of constraint to become active. */ + SubjectToStatus _status /**< Status of constraint to become active. */ + ); + + + /** Returns the number of constraints. + * \return Number of constraints. */ + inline int getNC( ) const; + + /** Returns the number of implicit equality constraints. + * \return Number of implicit equality constraints. */ + inline int getNEC( ) const; + + /** Returns the number of "real" inequality constraints. + * \return Number of "real" inequality constraints. */ + inline int getNIC( ) const; + + /** Returns the number of unbounded constraints (i.e. without any bounds). + * \return Number of unbounded constraints (i.e. without any bounds). */ + inline int getNUC( ) const; + + + /** Sets number of implicit equality constraints. + * \return SUCCESSFUL_RETURN */ + inline returnValue setNEC( int n /**< Number of implicit equality constraints. */ + ); + + /** Sets number of "real" inequality constraints. + * \return SUCCESSFUL_RETURN */ + inline returnValue setNIC( int n /**< Number of "real" inequality constraints. */ + ); + + /** Sets number of unbounded constraints (i.e. without any bounds). + * \return SUCCESSFUL_RETURN */ + inline returnValue setNUC( int n /**< Number of unbounded constraints (i.e. without any bounds). */ + ); + + + /** Returns the number of active constraints. + * \return Number of constraints. */ + inline int getNAC( ); + + /** Returns the number of inactive constraints. + * \return Number of constraints. */ + inline int getNIAC( ); + + + /** Returns a pointer to active constraints index list. + * \return Pointer to active constraints index list. */ + inline Indexlist* getActive( ); + + /** Returns a pointer to inactive constraints index list. + * \return Pointer to inactive constraints index list. */ + inline Indexlist* getInactive( ); + + + /* + * PROTECTED MEMBER VARIABLES + */ + protected: + int nC; /**< Number of constraints (nC = nEC + nIC + nUC). */ + int nEC; /**< Number of implicit equality constraints. */ + int nIC; /**< Number of "real" inequality constraints. */ + int nUC; /**< Number of unbounded constraints (i.e. without any bounds). */ + + Indexlist active; /**< Index list of active constraints. */ + Indexlist inactive; /**< Index list of inactive constraints. */ +}; + + +#include + +#endif /* QPOASES_CONSTRAINTS_HPP */ + + +/* + * end of file + */ diff --git a/phonelibs/qpoases/INCLUDE/CyclingManager.hpp b/phonelibs/qpoases/INCLUDE/CyclingManager.hpp new file mode 100644 index 00000000000000..9b5c064a0bd7a4 --- /dev/null +++ b/phonelibs/qpoases/INCLUDE/CyclingManager.hpp @@ -0,0 +1,126 @@ +/* + * This file is part of qpOASES. + * + * qpOASES -- An Implementation of the Online Active Set Strategy. + * Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved. + * + * qpOASES is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * qpOASES is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with qpOASES; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + + +/** + * \file INCLUDE/CyclingManager.hpp + * \author Hans Joachim Ferreau + * \version 1.3embedded + * \date 2007-2008 + * + * Declaration of the CyclingManager class designed to detect + * and handle possible cycling during QP iterations. + */ + + +#ifndef QPOASES_CYCLINGMANAGER_HPP +#define QPOASES_CYCLINGMANAGER_HPP + + +#include + + + +/** This class is intended to detect and handle possible cycling during QP iterations. + * As cycling seems to occur quite rarely, this class is NOT FULLY IMPLEMENTED YET! + * + * \author Hans Joachim Ferreau + * \version 1.3embedded + * \date 2007-2008 + */ +class CyclingManager +{ + /* + * PUBLIC MEMBER FUNCTIONS + */ + public: + /** Default constructor. */ + CyclingManager( ); + + /** Copy constructor (deep copy). */ + CyclingManager( const CyclingManager& rhs /**< Rhs object. */ + ); + + /** Destructor. */ + ~CyclingManager( ); + + /** Copy asingment operator (deep copy). */ + CyclingManager& operator=( const CyclingManager& rhs /**< Rhs object. */ + ); + + + /** Pseudo-constructor which takes the number of bounds/constraints. + * \return SUCCESSFUL_RETURN */ + returnValue init( int _nV, /**< Number of bounds to be managed. */ + int _nC /**< Number of constraints to be managed. */ + ); + + + /** Stores index of a bound/constraint that might cause cycling. + * \return SUCCESSFUL_RETURN \n + RET_INDEX_OUT_OF_BOUNDS */ + returnValue setCyclingStatus( int number, /**< Number of bound/constraint. */ + BooleanType isBound, /**< Flag that indicates if given number corresponds to a + * bound (BT_TRUE) or a constraint (BT_FALSE). */ + CyclingStatus _status /**< Cycling status of bound/constraint. */ + ); + + /** Returns if bound/constraint might cause cycling. + * \return BT_TRUE: bound/constraint might cause cycling \n + BT_FALSE: otherwise */ + CyclingStatus getCyclingStatus( int number, /**< Number of bound/constraint. */ + BooleanType isBound /**< Flag that indicates if given number corresponds to + * a bound (BT_TRUE) or a constraint (BT_FALSE). */ + ) const; + + + /** Clears all previous cycling information. + * \return SUCCESSFUL_RETURN */ + returnValue clearCyclingData( ); + + + /** Returns if cycling was detected. + * \return BT_TRUE iff cycling was detected. */ + inline BooleanType isCyclingDetected( ) const; + + + /* + * PROTECTED MEMBER VARIABLES + */ + protected: + int nV; /**< Number of managed bounds. */ + int nC; /**< Number of managed constraints. */ + + CyclingStatus status[NVMAX+NCMAX]; /**< Array to store cycling status of all bounds/constraints. */ + + BooleanType cyclingDetected; /**< Flag if cycling was detected. */ +}; + + +#include + +#endif /* QPOASES_CYCLINGMANAGER_HPP */ + + +/* + * end of file + */ diff --git a/phonelibs/qpoases/INCLUDE/EXTRAS/SolutionAnalysis.hpp b/phonelibs/qpoases/INCLUDE/EXTRAS/SolutionAnalysis.hpp new file mode 100644 index 00000000000000..056bb26f02d09d --- /dev/null +++ b/phonelibs/qpoases/INCLUDE/EXTRAS/SolutionAnalysis.hpp @@ -0,0 +1,107 @@ +/* + * This file is part of qpOASES. + * + * qpOASES -- An Implementation of the Online Active Set Strategy. + * Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved. + * + * qpOASES is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * qpOASES is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with qpOASES; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + + +/** + * \file INCLUDE/EXTRAS/SolutionAnalysis.hpp + * \author Milan Vukov, Boris Houska, Hans Joachim Ferreau + * \version 1.3embedded + * \date 2012 + * + * Solution analysis class, based on a class in the standard version of the qpOASES + */ + + +// + +#ifndef QPOASES_SOLUTIONANALYSIS_HPP +#define QPOASES_SOLUTIONANALYSIS_HPP + +#include + +/** Enables the computation of variance as is in the standard version of qpOASES */ +#define QPOASES_USE_OLD_VERSION 0 + +#if QPOASES_USE_OLD_VERSION +#define KKT_DIM (2 * NVMAX + NCMAX) +#endif + +class SolutionAnalysis +{ +public: + + /** Default constructor. */ + SolutionAnalysis( ); + + /** Copy constructor (deep copy). */ + SolutionAnalysis( const SolutionAnalysis& rhs /**< Rhs object. */ + ); + + /** Destructor. */ + ~SolutionAnalysis( ); + + /** Copy asingment operator (deep copy). */ + SolutionAnalysis& operator=( const SolutionAnalysis& rhs /**< Rhs object. */ + ); + + /** A routine for computation of inverse of the Hessian matrix. */ + returnValue getHessianInverse( + QProblem* qp, /** QP */ + real_t* hessianInverse /** Inverse of the Hessian matrix*/ + ); + + /** A routine for computation of inverse of the Hessian matrix. */ + returnValue getHessianInverse( QProblemB* qp, /** QP */ + real_t* hessianInverse /** Inverse of the Hessian matrix*/ + ); + +#if QPOASES_USE_OLD_VERSION + returnValue getVarianceCovariance( + QProblem* qp, + real_t* g_b_bA_VAR, + real_t* Primal_Dual_VAR + ); +#endif + +private: + + real_t delta_g_cov[ NVMAX ]; /** A covariance-vector of g */ + real_t delta_lb_cov[ NVMAX ]; /** A covariance-vector of lb */ + real_t delta_ub_cov[ NVMAX ]; /** A covariance-vector of ub */ + real_t delta_lbA_cov[ NCMAX_ALLOC ]; /** A covariance-vector of lbA */ + real_t delta_ubA_cov[ NCMAX_ALLOC ]; /** A covariance-vector of ubA */ + +#if QPOASES_USE_OLD_VERSION + real_t K[KKT_DIM * KKT_DIM]; /** A matrix to store an intermediate result */ +#endif + + int FR_idx[ NVMAX ]; /** Index array for free variables */ + int FX_idx[ NVMAX ]; /** Index array for fixed variables */ + int AC_idx[ NCMAX_ALLOC ]; /** Index array for active constraints */ + + real_t delta_xFR[ NVMAX ]; /** QP reaction, primal, w.r.t. free */ + real_t delta_xFX[ NVMAX ]; /** QP reaction, primal, w.r.t. fixed */ + real_t delta_yAC[ NVMAX ]; /** QP reaction, dual, w.r.t. active */ + real_t delta_yFX[ NVMAX ]; /** QP reaction, dual, w.r.t. fixed*/ +}; + +#endif // QPOASES_SOLUTIONANALYSIS_HPP diff --git a/phonelibs/qpoases/INCLUDE/Indexlist.hpp b/phonelibs/qpoases/INCLUDE/Indexlist.hpp new file mode 100644 index 00000000000000..18e3494e3f656c --- /dev/null +++ b/phonelibs/qpoases/INCLUDE/Indexlist.hpp @@ -0,0 +1,154 @@ +/* + * This file is part of qpOASES. + * + * qpOASES -- An Implementation of the Online Active Set Strategy. + * Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved. + * + * qpOASES is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * qpOASES is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with qpOASES; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + + +/** + * \file INCLUDE/Indexlist.hpp + * \author Hans Joachim Ferreau + * \version 1.3embedded + * \date 2007-2008 + * + * Declaration of the Indexlist class designed to manage index lists of + * constraints and bounds within a SubjectTo object. + */ + + +#ifndef QPOASES_INDEXLIST_HPP +#define QPOASES_INDEXLIST_HPP + + +#include + + +/** This class manages index lists. + * + * \author Hans Joachim Ferreau + * \version 1.3embedded + * \date 2007-2008 + */ +class Indexlist +{ + /* + * PUBLIC MEMBER FUNCTIONS + */ + public: + /** Default constructor. */ + Indexlist( ); + + /** Copy constructor (deep copy). */ + Indexlist( const Indexlist& rhs /**< Rhs object. */ + ); + + /** Destructor. */ + ~Indexlist( ); + + /** Assingment operator (deep copy). */ + Indexlist& operator=( const Indexlist& rhs /**< Rhs object. */ + ); + + /** Pseudo-constructor. + * \return SUCCESSFUL_RETURN */ + returnValue init( ); + + + /** Creates an array of all numbers within the index set in correct order. + * \return SUCCESSFUL_RETURN \n + RET_INDEXLIST_CORRUPTED */ + returnValue getNumberArray( int* const numberarray /**< Output: Array of numbers (NULL on error). */ + ) const; + + + /** Determines the index within the index list at with a given number is stored. + * \return >= 0: Index of given number. \n + -1: Number not found. */ + int getIndex( int givennumber /**< Number whose index shall be determined. */ + ) const; + + /** Determines the physical index within the index list at with a given number is stored. + * \return >= 0: Index of given number. \n + -1: Number not found. */ + int getPhysicalIndex( int givennumber /**< Number whose physical index shall be determined. */ + ) const; + + /** Returns the number stored at a given physical index. + * \return >= 0: Number stored at given physical index. \n + -RET_INDEXLIST_OUTOFBOUNDS */ + int getNumber( int physicalindex /**< Physical index of the number to be returned. */ + ) const; + + + /** Returns the current length of the index list. + * \return Current length of the index list. */ + inline int getLength( ); + + /** Returns last number within the index list. + * \return Last number within the index list. */ + inline int getLastNumber( ) const; + + + /** Adds number to index list. + * \return SUCCESSFUL_RETURN \n + RET_INDEXLIST_MUST_BE_REORDERD \n + RET_INDEXLIST_EXCEEDS_MAX_LENGTH */ + returnValue addNumber( int addnumber /**< Number to be added. */ + ); + + /** Removes number from index list. + * \return SUCCESSFUL_RETURN */ + returnValue removeNumber( int removenumber /**< Number to be removed. */ + ); + + /** Swaps two numbers within index list. + * \return SUCCESSFUL_RETURN */ + returnValue swapNumbers( int number1,/**< First number for swapping. */ + int number2 /**< Second number for swapping. */ + ); + + /** Determines if a given number is contained in the index set. + * \return BT_TRUE iff number is contain in the index set */ + inline BooleanType isMember( int _number /**< Number to be tested for membership. */ + ) const; + + + /* + * PROTECTED MEMBER VARIABLES + */ + protected: + int number[INDEXLISTFACTOR*(NVMAX+NCMAX)]; /**< Array to store numbers of constraints or bounds. */ + int next[INDEXLISTFACTOR*(NVMAX+NCMAX)]; /**< Array to store physical index of successor. */ + int previous[INDEXLISTFACTOR*(NVMAX+NCMAX)]; /**< Array to store physical index of predecossor. */ + int length; /**< Length of index list. */ + int first; /**< Physical index of first element. */ + int last; /**< Physical index of last element. */ + int lastusedindex; /**< Physical index of last entry in index list. */ + int physicallength; /**< Physical length of index list. */ +}; + + +#include + +#endif /* QPOASES_INDEXLIST_HPP */ + + +/* + * end of file + */ diff --git a/phonelibs/qpoases/INCLUDE/MessageHandling.hpp b/phonelibs/qpoases/INCLUDE/MessageHandling.hpp new file mode 100644 index 00000000000000..f0b0c79a473b85 --- /dev/null +++ b/phonelibs/qpoases/INCLUDE/MessageHandling.hpp @@ -0,0 +1,415 @@ +/* + * This file is part of qpOASES. + * + * qpOASES -- An Implementation of the Online Active Set Strategy. + * Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved. + * + * qpOASES is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * qpOASES is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with qpOASES; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + + +/** + * \file INCLUDE/MessageHandling.hpp + * \author Hans Joachim Ferreau + * \version 1.3embedded + * \date 2007-2008 + * + * Declaration of the MessageHandling class including global return values. + */ + + +#ifndef QPOASES_MESSAGEHANDLING_HPP +#define QPOASES_MESSAGEHANDLING_HPP + +// #define PC_DEBUG + +#ifdef PC_DEBUG + #include + + /** Defines an alias for FILE from stdio.h. */ + #define myFILE FILE + /** Defines an alias for stderr from stdio.h. */ + #define myStderr stderr + /** Defines an alias for stdout from stdio.h. */ + #define myStdout stdout +#else + /** Defines an alias for FILE from stdio.h. */ + #define myFILE int + /** Defines an alias for stderr from stdio.h. */ + #define myStderr 0 + /** Defines an alias for stdout from stdio.h. */ + #define myStdout 0 +#endif + + +#include +#include + + +/** Defines symbols for global return values. \n + * Important: All return values are assumed to be nonnegative! */ +enum returnValue +{ +TERMINAL_LIST_ELEMENT = -1, /**< Terminal list element, internal usage only! */ +/* miscellaneous */ +SUCCESSFUL_RETURN = 0, /**< Successful return. */ +RET_DIV_BY_ZERO, /**< Division by zero. */ +RET_INDEX_OUT_OF_BOUNDS, /**< Index out of bounds. */ +RET_INVALID_ARGUMENTS, /**< At least one of the arguments is invalid. */ +RET_ERROR_UNDEFINED, /**< Error number undefined. */ +RET_WARNING_UNDEFINED, /**< Warning number undefined. */ +RET_INFO_UNDEFINED, /**< Info number undefined. */ +RET_EWI_UNDEFINED, /**< Error/warning/info number undefined. */ +RET_AVAILABLE_WITH_LINUX_ONLY, /**< This function is available under Linux only. */ +RET_UNKNOWN_BUG, /**< The error occured is not yet known. */ +RET_PRINTLEVEL_CHANGED, /**< 10 Print level changed. */ +RET_NOT_YET_IMPLEMENTED, /**< Requested function is not yet implemented in this version of qpOASES. */ +/* Indexlist */ +RET_INDEXLIST_MUST_BE_REORDERD, /**< Index list has to be reordered. */ +RET_INDEXLIST_EXCEEDS_MAX_LENGTH, /**< Index list exceeds its maximal physical length. */ +RET_INDEXLIST_CORRUPTED, /**< Index list corrupted. */ +RET_INDEXLIST_OUTOFBOUNDS, /**< Physical index is out of bounds. */ +RET_INDEXLIST_ADD_FAILED, /**< Adding indices from another index set failed. */ +RET_INDEXLIST_INTERSECT_FAILED, /**< Intersection with another index set failed. */ +/* SubjectTo / Bounds / Constraints */ +RET_INDEX_ALREADY_OF_DESIRED_STATUS, /**< Index is already of desired status. */ +RET_ADDINDEX_FAILED, /**< Cannot swap between different indexsets. */ +RET_SWAPINDEX_FAILED, /**< 20 Adding index to index set failed. */ +RET_NOTHING_TO_DO, /**< Nothing to do. */ +RET_SETUP_BOUND_FAILED, /**< Setting up bound index failed. */ +RET_SETUP_CONSTRAINT_FAILED, /**< Setting up constraint index failed. */ +RET_MOVING_BOUND_FAILED, /**< Moving bound between index sets failed. */ +RET_MOVING_CONSTRAINT_FAILED, /**< Moving constraint between index sets failed. */ +/* QProblem */ +RET_QP_ALREADY_INITIALISED, /**< QProblem has already been initialised. */ +RET_NO_INIT_WITH_STANDARD_SOLVER, /**< Initialisation via extern QP solver is not yet implemented. */ +RET_RESET_FAILED, /**< Reset failed. */ +RET_INIT_FAILED, /**< Initialisation failed. */ +RET_INIT_FAILED_TQ, /**< 30 Initialisation failed due to TQ factorisation. */ +RET_INIT_FAILED_CHOLESKY, /**< Initialisation failed due to Cholesky decomposition. */ +RET_INIT_FAILED_HOTSTART, /**< Initialisation failed! QP could not be solved! */ +RET_INIT_FAILED_INFEASIBILITY, /**< Initial QP could not be solved due to infeasibility! */ +RET_INIT_FAILED_UNBOUNDEDNESS, /**< Initial QP could not be solved due to unboundedness! */ +RET_INIT_SUCCESSFUL, /**< Initialisation done. */ +RET_OBTAINING_WORKINGSET_FAILED, /**< Failed to obtain working set for auxiliary QP. */ +RET_SETUP_WORKINGSET_FAILED, /**< Failed to setup working set for auxiliary QP. */ +RET_SETUP_AUXILIARYQP_FAILED, /**< Failed to setup auxiliary QP for initialised homotopy. */ +RET_NO_EXTERN_SOLVER, /**< No extern QP solver available. */ +RET_QP_UNBOUNDED, /**< 40 QP is unbounded. */ +RET_QP_INFEASIBLE, /**< QP is infeasible. */ +RET_QP_NOT_SOLVED, /**< Problems occured while solving QP with standard solver. */ +RET_QP_SOLVED, /**< QP successfully solved. */ +RET_UNABLE_TO_SOLVE_QP, /**< Problems occured while solving QP. */ +RET_INITIALISATION_STARTED, /**< Starting problem initialisation. */ +RET_HOTSTART_FAILED, /**< Unable to perform homotopy due to internal error. */ +RET_HOTSTART_FAILED_TO_INIT, /**< Unable to initialise problem. */ +RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED, /**< Unable to perform homotopy as previous QP is not solved. */ +RET_ITERATION_STARTED, /**< Iteration... */ +RET_SHIFT_DETERMINATION_FAILED, /**< 50 Determination of shift of the QP data failed. */ +RET_STEPDIRECTION_DETERMINATION_FAILED, /**< Determination of step direction failed. */ +RET_STEPLENGTH_DETERMINATION_FAILED, /**< Determination of step direction failed. */ +RET_OPTIMAL_SOLUTION_FOUND, /**< Optimal solution of neighbouring QP found. */ +RET_HOMOTOPY_STEP_FAILED, /**< Unable to perform homotopy step. */ +RET_HOTSTART_STOPPED_INFEASIBILITY, /**< Premature homotopy termination because QP is infeasible. */ +RET_HOTSTART_STOPPED_UNBOUNDEDNESS, /**< Premature homotopy termination because QP is unbounded. */ +RET_WORKINGSET_UPDATE_FAILED, /**< Unable to update working sets according to initial guesses. */ +RET_MAX_NWSR_REACHED, /**< Maximum number of working set recalculations performed. */ +RET_CONSTRAINTS_NOT_SPECIFIED, /**< Problem does comprise constraints! You also have to specify new constraints' bounds. */ +RET_INVALID_FACTORISATION_FLAG, /**< 60 Invalid factorisation flag. */ +RET_UNABLE_TO_SAVE_QPDATA, /**< Unable to save QP data. */ +RET_STEPDIRECTION_FAILED_TQ, /**< Abnormal termination due to TQ factorisation. */ +RET_STEPDIRECTION_FAILED_CHOLESKY, /**< Abnormal termination due to Cholesky factorisation. */ +RET_CYCLING_DETECTED, /**< Cycling detected. */ +RET_CYCLING_NOT_RESOLVED, /**< Cycling cannot be resolved, QP probably infeasible. */ +RET_CYCLING_RESOLVED, /**< Cycling probably resolved. */ +RET_STEPSIZE, /**< For displaying performed stepsize. */ +RET_STEPSIZE_NONPOSITIVE, /**< For displaying non-positive stepsize. */ +RET_SETUPSUBJECTTOTYPE_FAILED, /**< Setup of SubjectToTypes failed. */ +RET_ADDCONSTRAINT_FAILED, /**< 70 Addition of constraint to working set failed. */ +RET_ADDCONSTRAINT_FAILED_INFEASIBILITY, /**< Addition of constraint to working set failed (due to QP infeasibility). */ +RET_ADDBOUND_FAILED, /**< Addition of bound to working set failed. */ +RET_ADDBOUND_FAILED_INFEASIBILITY, /**< Addition of bound to working set failed (due to QP infeasibility). */ +RET_REMOVECONSTRAINT_FAILED, /**< Removal of constraint from working set failed. */ +RET_REMOVEBOUND_FAILED, /**< Removal of bound from working set failed. */ +RET_REMOVE_FROM_ACTIVESET, /**< Removing from active set... */ +RET_ADD_TO_ACTIVESET, /**< Adding to active set... */ +RET_REMOVE_FROM_ACTIVESET_FAILED, /**< Removing from active set failed. */ +RET_ADD_TO_ACTIVESET_FAILED, /**< Adding to active set failed. */ +RET_CONSTRAINT_ALREADY_ACTIVE, /**< 80 Constraint is already active. */ +RET_ALL_CONSTRAINTS_ACTIVE, /**< All constraints are active, no further constraint can be added. */ +RET_LINEARLY_DEPENDENT, /**< New bound/constraint is linearly dependent. */ +RET_LINEARLY_INDEPENDENT, /**< New bound/constraint is linearly independent. */ +RET_LI_RESOLVED, /**< Linear independence of active contraint matrix successfully resolved. */ +RET_ENSURELI_FAILED, /**< Failed to ensure linear indepence of active contraint matrix. */ +RET_ENSURELI_FAILED_TQ, /**< Abnormal termination due to TQ factorisation. */ +RET_ENSURELI_FAILED_NOINDEX, /**< No index found, QP probably infeasible. */ +RET_ENSURELI_FAILED_CYCLING, /**< Cycling detected, QP probably infeasible. */ +RET_BOUND_ALREADY_ACTIVE, /**< Bound is already active. */ +RET_ALL_BOUNDS_ACTIVE, /**< 90 All bounds are active, no further bound can be added. */ +RET_CONSTRAINT_NOT_ACTIVE, /**< Constraint is not active. */ +RET_BOUND_NOT_ACTIVE, /**< Bound is not active. */ +RET_HESSIAN_NOT_SPD, /**< Projected Hessian matrix not positive definite. */ +RET_MATRIX_SHIFT_FAILED, /**< Unable to update matrices or to transform vectors. */ +RET_MATRIX_FACTORISATION_FAILED, /**< Unable to calculate new matrix factorisations. */ +RET_PRINT_ITERATION_FAILED, /**< Unable to print information on current iteration. */ +RET_NO_GLOBAL_MESSAGE_OUTPUTFILE, /**< No global message output file initialised. */ +/* Utils */ +RET_UNABLE_TO_OPEN_FILE, /**< Unable to open file. */ +RET_UNABLE_TO_WRITE_FILE, /**< Unable to write into file. */ +RET_UNABLE_TO_READ_FILE, /**< 100 Unable to read from file. */ +RET_FILEDATA_INCONSISTENT, /**< File contains inconsistent data. */ +/* SolutionAnalysis */ +RET_NO_SOLUTION, /**< QP solution does not satisfy KKT optimality conditions. */ +RET_INACCURATE_SOLUTION /**< KKT optimality conditions not satisfied to sufficient accuracy. */ +}; + + + +/** This class handles all kinds of messages (errors, warnings, infos) initiated + * by qpOASES modules and stores the correspoding global preferences. + * + * \author Hans Joachim Ferreau (special thanks to Leonard Wirsching) + * \version 1.3embedded + * \date 2007-2008 + */ +class MessageHandling +{ + /* + * INTERNAL DATA STRUCTURES + */ + public: + /** Data structure for entries in global message list. */ + typedef struct { + returnValue key; /**< Global return value. */ + const char* data; /**< Corresponding message. */ + VisibilityStatus globalVisibilityStatus; /**< Determines if message can be printed. + * If this value is set to VS_HIDDEN, no message is printed! */ + } ReturnValueList; + + + /* + * PUBLIC MEMBER FUNCTIONS + */ + public: + /** Default constructor. */ + MessageHandling( ); + + /** Constructor which takes the desired output file. */ + MessageHandling( myFILE* _outputFile /**< Output file. */ + ); + + /** Constructor which takes the desired visibility states. */ + MessageHandling( VisibilityStatus _errorVisibility, /**< Visibility status for error messages. */ + VisibilityStatus _warningVisibility,/**< Visibility status for warning messages. */ + VisibilityStatus _infoVisibility /**< Visibility status for info messages. */ + ); + + /** Constructor which takes the desired output file and desired visibility states. */ + MessageHandling( myFILE* _outputFile, /**< Output file. */ + VisibilityStatus _errorVisibility, /**< Visibility status for error messages. */ + VisibilityStatus _warningVisibility,/**< Visibility status for warning messages. */ + VisibilityStatus _infoVisibility /**< Visibility status for info messages. */ + ); + + /** Copy constructor (deep copy). */ + MessageHandling( const MessageHandling& rhs /**< Rhs object. */ + ); + + /** Destructor. */ + ~MessageHandling( ); + + /** Assignment operator (deep copy). */ + MessageHandling& operator=( const MessageHandling& rhs /**< Rhs object. */ + ); + + + /** Prints an error message(a simplified macro THROWERROR is also provided). \n + * Errors are definied as abnormal events which cause an immediate termination of the current (sub) function. + * Errors of a sub function should be commented by the calling function by means of a warning message + * (if this error does not cause an error of the calling function, either)! + * \return Error number returned by sub function call + */ + returnValue throwError( + returnValue Enumber, /**< Error number returned by sub function call. */ + const char* additionaltext, /**< Additional error text (0, if none). */ + const char* functionname, /**< Name of function which caused the error. */ + const char* filename, /**< Name of file which caused the error. */ + const unsigned long linenumber, /**< Number of line which caused the error.incompatible binary file */ + VisibilityStatus localVisibilityStatus /**< Determines (locally) if error message can be printed to myStderr. + * If GLOBAL visibility status of the message is set to VS_HIDDEN, + * no message is printed, anyway! */ + ); + + /** Prints a warning message (a simplified macro THROWWARNING is also provided). + * Warnings are definied as abnormal events which does NOT cause an immediate termination of the current (sub) function. + * \return Warning number returned by sub function call + */ + returnValue throwWarning( + returnValue Wnumber, /**< Warning number returned by sub function call. */ + const char* additionaltext, /**< Additional warning text (0, if none). */ + const char* functionname, /**< Name of function which caused the warning. */ + const char* filename, /**< Name of file which caused the warning. */ + const unsigned long linenumber, /**< Number of line which caused the warning. */ + VisibilityStatus localVisibilityStatus /**< Determines (locally) if warning message can be printed to myStderr. + * If GLOBAL visibility status of the message is set to VS_HIDDEN, + * no message is printed, anyway! */ + ); + + /** Prints a info message (a simplified macro THROWINFO is also provided). + * \return Info number returned by sub function call + */ + returnValue throwInfo( + returnValue Inumber, /**< Info number returned by sub function call. */ + const char* additionaltext, /**< Additional warning text (0, if none). */ + const char* functionname, /**< Name of function which submitted the info. */ + const char* filename, /**< Name of file which submitted the info. */ + const unsigned long linenumber, /**< Number of line which submitted the info. */ + VisibilityStatus localVisibilityStatus /**< Determines (locally) if info message can be printed to myStderr. + * If GLOBAL visibility status of the message is set to VS_HIDDEN, + * no message is printed, anyway! */ + ); + + + /** Resets all preferences to default values. + * \return SUCCESSFUL_RETURN */ + returnValue reset( ); + + + /** Prints a complete list of all messages to output file. + * \return SUCCESSFUL_RETURN */ + returnValue listAllMessages( ); + + + /** Returns visibility status for error messages. + * \return Visibility status for error messages. */ + inline VisibilityStatus getErrorVisibilityStatus( ) const; + + /** Returns visibility status for warning messages. + * \return Visibility status for warning messages. */ + inline VisibilityStatus getWarningVisibilityStatus( ) const; + + /** Returns visibility status for info messages. + * \return Visibility status for info messages. */ + inline VisibilityStatus getInfoVisibilityStatus( ) const; + + /** Returns pointer to output file. + * \return Pointer to output file. */ + inline myFILE* getOutputFile( ) const; + + /** Returns error count value. + * \return Error count value. */ + inline int getErrorCount( ) const; + + + /** Changes visibility status for error messages. */ + inline void setErrorVisibilityStatus( VisibilityStatus _errorVisibility /**< New visibility status for error messages. */ + ); + + /** Changes visibility status for warning messages. */ + inline void setWarningVisibilityStatus( VisibilityStatus _warningVisibility /**< New visibility status for warning messages. */ + ); + + /** Changes visibility status for info messages. */ + inline void setInfoVisibilityStatus( VisibilityStatus _infoVisibility /**< New visibility status for info messages. */ + ); + + /** Changes output file for messages. */ + inline void setOutputFile( myFILE* _outputFile /**< New output file for messages. */ + ); + + /** Changes error count. + * \return SUCCESSFUL_RETURN \n + * RET_INVALID_ARGUMENT */ + inline returnValue setErrorCount( int _errorCount /**< New error count value. */ + ); + + /** Return the error code string. */ + static const char* getErrorString(int error); + + /* + * PROTECTED MEMBER FUNCTIONS + */ + protected: + /** Prints a info message to myStderr (auxiliary function). + * \return Error/warning/info number returned by sub function call + */ + returnValue throwMessage( + returnValue RETnumber, /**< Error/warning/info number returned by sub function call. */ + const char* additionaltext, /**< Additional warning text (0, if none). */ + const char* functionname, /**< Name of function which caused the error/warning/info. */ + const char* filename, /**< Name of file which caused the error/warning/info. */ + const unsigned long linenumber, /**< Number of line which caused the error/warning/info. */ + VisibilityStatus localVisibilityStatus, /**< Determines (locally) if info message can be printed to myStderr. + * If GLOBAL visibility status of the message is set to VS_HIDDEN, + * no message is printed, anyway! */ + const char* RETstring /**< Leading string of error/warning/info message. */ + ); + + + /* + * PROTECTED MEMBER VARIABLES + */ + protected: + VisibilityStatus errorVisibility; /**< Error messages visible? */ + VisibilityStatus warningVisibility; /**< Warning messages visible? */ + VisibilityStatus infoVisibility; /**< Info messages visible? */ + + myFILE* outputFile; /**< Output file for messages. */ + + int errorCount; /**< Counts number of errors (for nicer output only). */ +}; + + +#ifndef __FUNCTION__ + /** Ensures that __FUNCTION__ macro is defined. */ + #define __FUNCTION__ 0 +#endif + +#ifndef __FILE__ + /** Ensures that __FILE__ macro is defined. */ + #define __FILE__ 0 +#endif + +#ifndef __LINE__ + /** Ensures that __LINE__ macro is defined. */ + #define __LINE__ 0 +#endif + + +/** Short version of throwError with default values, only returnValue is needed */ +#define THROWERROR(retval) ( getGlobalMessageHandler( )->throwError((retval),0,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE) ) + +/** Short version of throwWarning with default values, only returnValue is needed */ +#define THROWWARNING(retval) ( getGlobalMessageHandler( )->throwWarning((retval),0,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE) ) + +/** Short version of throwInfo with default values, only returnValue is needed */ +#define THROWINFO(retval) ( getGlobalMessageHandler( )->throwInfo((retval),0,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE) ) + + +/** Returns a pointer to global message handler. + * \return Pointer to global message handler. + */ +MessageHandling* getGlobalMessageHandler( ); + + +#include + +#endif /* QPOASES_MESSAGEHANDLING_HPP */ + + +/* + * end of file + */ diff --git a/phonelibs/qpoases/INCLUDE/QProblem.hpp b/phonelibs/qpoases/INCLUDE/QProblem.hpp new file mode 100644 index 00000000000000..68fc1af9c53a79 --- /dev/null +++ b/phonelibs/qpoases/INCLUDE/QProblem.hpp @@ -0,0 +1,666 @@ +/* + * This file is part of qpOASES. + * + * qpOASES -- An Implementation of the Online Active Set Strategy. + * Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved. + * + * qpOASES is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * qpOASES is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with qpOASES; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + + +/** + * \file INCLUDE/QProblem.hpp + * \author Hans Joachim Ferreau + * \version 1.3embedded + * \date 2007-2008 + * + * Declaration of the QProblem class which is able to use the newly + * developed online active set strategy for parametric quadratic programming. + */ + + + +#ifndef QPOASES_QPROBLEM_HPP +#define QPOASES_QPROBLEM_HPP + + +#include +#include +#include + + +/** A class for setting up and solving quadratic programs. The main feature is + * the possibily to use the newly developed online active set strategy for + * parametric quadratic programming. + * + * \author Hans Joachim Ferreau + * \version 1.3embedded + * \date 2007-2008 + */ +class QProblem : public QProblemB +{ + /* allow SolutionAnalysis class to access private members */ + friend class SolutionAnalysis; + + /* + * PUBLIC MEMBER FUNCTIONS + */ + public: + /** Default constructor. */ + QProblem( ); + + /** Constructor which takes the QP dimensions only. */ + QProblem( int _nV, /**< Number of variables. */ + int _nC /**< Number of constraints. */ + ); + + /** Copy constructor (deep copy). */ + QProblem( const QProblem& rhs /**< Rhs object. */ + ); + + /** Destructor. */ + ~QProblem( ); + + /** Assignment operator (deep copy). */ + QProblem& operator=( const QProblem& rhs /**< Rhs object. */ + ); + + + /** Clears all data structures of QProblemB except for QP data. + * \return SUCCESSFUL_RETURN \n + RET_RESET_FAILED */ + returnValue reset( ); + + + /** Initialises a QProblem with given QP data and solves it + * using an initial homotopy with empty working set (at most nWSR iterations). + * \return SUCCESSFUL_RETURN \n + RET_INIT_FAILED \n + RET_INIT_FAILED_CHOLESKY \n + RET_INIT_FAILED_TQ \n + RET_INIT_FAILED_HOTSTART \n + RET_INIT_FAILED_INFEASIBILITY \n + RET_INIT_FAILED_UNBOUNDEDNESS \n + RET_MAX_NWSR_REACHED \n + RET_INVALID_ARGUMENTS \n + RET_INACCURATE_SOLUTION \n + RET_NO_SOLUTION */ + returnValue init( const real_t* const _H, /**< Hessian matrix. */ + const real_t* const _g, /**< Gradient vector. */ + const real_t* const _A, /**< Constraint matrix. */ + const real_t* const _lb, /**< Lower bound vector (on variables). \n + If no lower bounds exist, a NULL pointer can be passed. */ + const real_t* const _ub, /**< Upper bound vector (on variables). \n + If no upper bounds exist, a NULL pointer can be passed. */ + const real_t* const _lbA, /**< Lower constraints' bound vector. \n + If no lower constraints' bounds exist, a NULL pointer can be passed. */ + const real_t* const _ubA, /**< Upper constraints' bound vector. \n + If no lower constraints' bounds exist, a NULL pointer can be passed. */ + int& nWSR, /**< Input: Maximum number of working set recalculations when using initial homotopy. + Output: Number of performed working set recalculations. */ + const real_t* const yOpt = 0, /**< Initial guess for dual solution vector. */ + real_t* const cputime = 0 /**< Output: CPU time required to initialise QP. */ + ); + + + /** Initialises a QProblem with given QP data and solves it + * using an initial homotopy with empty working set (at most nWSR iterations). + * \return SUCCESSFUL_RETURN \n + RET_INIT_FAILED \n + RET_INIT_FAILED_CHOLESKY \n + RET_INIT_FAILED_TQ \n + RET_INIT_FAILED_HOTSTART \n + RET_INIT_FAILED_INFEASIBILITY \n + RET_INIT_FAILED_UNBOUNDEDNESS \n + RET_MAX_NWSR_REACHED \n + RET_INVALID_ARGUMENTS \n + RET_INACCURATE_SOLUTION \n + RET_NO_SOLUTION */ + returnValue init( const real_t* const _H, /**< Hessian matrix. */ + const real_t* const _R, /**< Cholesky factorization of the Hessian matrix. */ + const real_t* const _g, /**< Gradient vector. */ + const real_t* const _A, /**< Constraint matrix. */ + const real_t* const _lb, /**< Lower bound vector (on variables). \n + If no lower bounds exist, a NULL pointer can be passed. */ + const real_t* const _ub, /**< Upper bound vector (on variables). \n + If no upper bounds exist, a NULL pointer can be passed. */ + const real_t* const _lbA, /**< Lower constraints' bound vector. \n + If no lower constraints' bounds exist, a NULL pointer can be passed. */ + const real_t* const _ubA, /**< Upper constraints' bound vector. \n + If no lower constraints' bounds exist, a NULL pointer can be passed. */ + int& nWSR, /**< Input: Maximum number of working set recalculations when using initial homotopy. + Output: Number of performed working set recalculations. */ + const real_t* const yOpt = 0, /**< Initial guess for dual solution vector. */ + real_t* const cputime = 0 /**< Output: CPU time required to initialise QP. */ + ); + + + /** Solves QProblem using online active set strategy. + * \return SUCCESSFUL_RETURN \n + RET_MAX_NWSR_REACHED \n + RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n + RET_HOTSTART_FAILED \n + RET_SHIFT_DETERMINATION_FAILED \n + RET_STEPDIRECTION_DETERMINATION_FAILED \n + RET_STEPLENGTH_DETERMINATION_FAILED \n + RET_HOMOTOPY_STEP_FAILED \n + RET_HOTSTART_STOPPED_INFEASIBILITY \n + RET_HOTSTART_STOPPED_UNBOUNDEDNESS \n + RET_INACCURATE_SOLUTION \n + RET_NO_SOLUTION */ + returnValue hotstart( const real_t* const g_new, /**< Gradient of neighbouring QP to be solved. */ + const real_t* const lb_new, /**< Lower bounds of neighbouring QP to be solved. \n + If no lower bounds exist, a NULL pointer can be passed. */ + const real_t* const ub_new, /**< Upper bounds of neighbouring QP to be solved. \n + If no upper bounds exist, a NULL pointer can be passed. */ + const real_t* const lbA_new, /**< Lower constraints' bounds of neighbouring QP to be solved. \n + If no lower constraints' bounds exist, a NULL pointer can be passed. */ + const real_t* const ubA_new, /**< Upper constraints' bounds of neighbouring QP to be solved. \n + If no upper constraints' bounds exist, a NULL pointer can be passed. */ + int& nWSR, /**< Input: Maximum number of working set recalculations; \n + Output: Number of performed working set recalculations. */ + real_t* const cputime /**< Output: CPU time required to solve QP (or to perform nWSR iterations). */ + ); + + + /** Returns constraint matrix of the QP (deep copy). + * \return SUCCESSFUL_RETURN */ + inline returnValue getA( real_t* const _A /**< Array of appropriate dimension for copying constraint matrix.*/ + ) const; + + /** Returns a single row of constraint matrix of the QP (deep copy). + * \return SUCCESSFUL_RETURN \n + RET_INDEX_OUT_OF_BOUNDS */ + inline returnValue getA( int number, /**< Number of entry to be returned. */ + real_t* const row /**< Array of appropriate dimension for copying (number)th constraint. */ + ) const; + + /** Returns lower constraints' bound vector of the QP (deep copy). + * \return SUCCESSFUL_RETURN */ + inline returnValue getLBA( real_t* const _lbA /**< Array of appropriate dimension for copying lower constraints' bound vector.*/ + ) const; + + /** Returns single entry of lower constraints' bound vector of the QP. + * \return SUCCESSFUL_RETURN \n + RET_INDEX_OUT_OF_BOUNDS */ + inline returnValue getLBA( int number, /**< Number of entry to be returned. */ + real_t& value /**< Output: lbA[number].*/ + ) const; + + /** Returns upper constraints' bound vector of the QP (deep copy). + * \return SUCCESSFUL_RETURN */ + inline returnValue getUBA( real_t* const _ubA /**< Array of appropriate dimension for copying upper constraints' bound vector.*/ + ) const; + + /** Returns single entry of upper constraints' bound vector of the QP. + * \return SUCCESSFUL_RETURN \n + RET_INDEX_OUT_OF_BOUNDS */ + inline returnValue getUBA( int number, /**< Number of entry to be returned. */ + real_t& value /**< Output: ubA[number].*/ + ) const; + + + /** Returns current constraints object of the QP (deep copy). + * \return SUCCESSFUL_RETURN */ + inline returnValue getConstraints( Constraints* const _constraints /** Output: Constraints object. */ + ) const; + + + /** Returns the number of constraints. + * \return Number of constraints. */ + inline int getNC( ) const; + + /** Returns the number of (implicitly defined) equality constraints. + * \return Number of (implicitly defined) equality constraints. */ + inline int getNEC( ) const; + + /** Returns the number of active constraints. + * \return Number of active constraints. */ + inline int getNAC( ); + + /** Returns the number of inactive constraints. + * \return Number of inactive constraints. */ + inline int getNIAC( ); + + /** Returns the dimension of null space. + * \return Dimension of null space. */ + int getNZ( ); + + + /** Returns the dual solution vector (deep copy). + * \return SUCCESSFUL_RETURN \n + RET_QP_NOT_SOLVED */ + returnValue getDualSolution( real_t* const yOpt /**< Output: Dual solution vector (if QP has been solved). */ + ) const; + + + /* + * PROTECTED MEMBER FUNCTIONS + */ + protected: + /** Determines type of constraints and bounds (i.e. implicitly fixed, unbounded etc.). + * \return SUCCESSFUL_RETURN \n + RET_SETUPSUBJECTTOTYPE_FAILED */ + returnValue setupSubjectToType( ); + + /** Computes the Cholesky decomposition R of the projected Hessian (i.e. R^T*R = Z^T*H*Z). + * \return SUCCESSFUL_RETURN \n + * RET_INDEXLIST_CORRUPTED */ + returnValue setupCholeskyDecompositionProjected( ); + + /** Initialises TQ factorisation of A (i.e. A*Q = [0 T]) if NO constraint is active. + * \return SUCCESSFUL_RETURN \n + RET_INDEXLIST_CORRUPTED */ + returnValue setupTQfactorisation( ); + + + /** Solves a QProblem whose QP data is assumed to be stored in the member variables. + * A guess for its primal/dual optimal solution vectors and the corresponding + * working sets of bounds and constraints can be provided. + * \return SUCCESSFUL_RETURN \n + RET_INIT_FAILED \n + RET_INIT_FAILED_CHOLESKY \n + RET_INIT_FAILED_TQ \n + RET_INIT_FAILED_HOTSTART \n + RET_INIT_FAILED_INFEASIBILITY \n + RET_INIT_FAILED_UNBOUNDEDNESS \n + RET_MAX_NWSR_REACHED */ + returnValue solveInitialQP( const real_t* const xOpt, /**< Optimal primal solution vector. + * A NULL pointer can be passed. */ + const real_t* const yOpt, /**< Optimal dual solution vector. + * A NULL pointer can be passed. */ + const Bounds* const guessedBounds, /**< Guessed working set of bounds for solution (xOpt,yOpt). + * A NULL pointer can be passed. */ + const Constraints* const guessedConstraints, /**< Optimal working set of constraints for solution (xOpt,yOpt). + * A NULL pointer can be passed. */ + int& nWSR, /**< Input: Maximum number of working set recalculations; \n + * Output: Number of performed working set recalculations. */ + real_t* const cputime /**< Output: CPU time required to solve QP (or to perform nWSR iterations). */ + ); + + /** Obtains the desired working set for the auxiliary initial QP in + * accordance with the user specifications + * (assumes that member AX has already been initialised!) + * \return SUCCESSFUL_RETURN \n + RET_OBTAINING_WORKINGSET_FAILED \n + RET_INVALID_ARGUMENTS */ + returnValue obtainAuxiliaryWorkingSet( const real_t* const xOpt, /**< Optimal primal solution vector. + * If a NULL pointer is passed, all entries are assumed to be zero. */ + const real_t* const yOpt, /**< Optimal dual solution vector. + * If a NULL pointer is passed, all entries are assumed to be zero. */ + const Bounds* const guessedBounds, /**< Guessed working set of bounds for solution (xOpt,yOpt). */ + const Constraints* const guessedConstraints, /**< Guessed working set for solution (xOpt,yOpt). */ + Bounds* auxiliaryBounds, /**< Input: Allocated bound object. \n + * Ouput: Working set of constraints for auxiliary QP. */ + Constraints* auxiliaryConstraints /**< Input: Allocated bound object. \n + * Ouput: Working set for auxiliary QP. */ + ) const; + + /** Setups bound and constraints data structures according to auxiliaryBounds/Constraints. + * (If the working set shall be setup afresh, make sure that + * bounds and constraints data structure have been resetted + * and the TQ factorisation has been initialised!) + * \return SUCCESSFUL_RETURN \n + RET_SETUP_WORKINGSET_FAILED \n + RET_INVALID_ARGUMENTS \n + RET_UNKNOWN BUG */ + returnValue setupAuxiliaryWorkingSet( const Bounds* const auxiliaryBounds, /**< Working set of bounds for auxiliary QP. */ + const Constraints* const auxiliaryConstraints, /**< Working set of constraints for auxiliary QP. */ + BooleanType setupAfresh /**< Flag indicating if given working set shall be + * setup afresh or by updating the current one. */ + ); + + /** Setups the optimal primal/dual solution of the auxiliary initial QP. + * \return SUCCESSFUL_RETURN */ + returnValue setupAuxiliaryQPsolution( const real_t* const xOpt, /**< Optimal primal solution vector. + * If a NULL pointer is passed, all entries are set to zero. */ + const real_t* const yOpt /**< Optimal dual solution vector. + * If a NULL pointer is passed, all entries are set to zero. */ + ); + + /** Setups gradient of the auxiliary initial QP for given + * optimal primal/dual solution and given initial working set + * (assumes that members X, Y and BOUNDS, CONSTRAINTS have already been initialised!). + * \return SUCCESSFUL_RETURN */ + returnValue setupAuxiliaryQPgradient( ); + + /** Setups (constraints') bounds of the auxiliary initial QP for given + * optimal primal/dual solution and given initial working set + * (assumes that members X, Y and BOUNDS, CONSTRAINTS have already been initialised!). + * \return SUCCESSFUL_RETURN \n + RET_UNKNOWN BUG */ + returnValue setupAuxiliaryQPbounds( const Bounds* const auxiliaryBounds, /**< Working set of bounds for auxiliary QP. */ + const Constraints* const auxiliaryConstraints, /**< Working set of constraints for auxiliary QP. */ + BooleanType useRelaxation /**< Flag indicating if inactive (constraints') bounds shall be relaxed. */ + ); + + + /** Adds a constraint to active set. + * \return SUCCESSFUL_RETURN \n + RET_ADDCONSTRAINT_FAILED \n + RET_ADDCONSTRAINT_FAILED_INFEASIBILITY \n + RET_ENSURELI_FAILED */ + returnValue addConstraint( int number, /**< Number of constraint to be added to active set. */ + SubjectToStatus C_status, /**< Status of new active constraint. */ + BooleanType updateCholesky /**< Flag indicating if Cholesky decomposition shall be updated. */ + ); + + /** Checks if new active constraint to be added is linearly dependent from + * from row of the active constraints matrix. + * \return RET_LINEARLY_DEPENDENT \n + RET_LINEARLY_INDEPENDENT \n + RET_INDEXLIST_CORRUPTED */ + returnValue addConstraint_checkLI( int number /**< Number of constraint to be added to active set. */ + ); + + /** Ensures linear independence of constraint matrix when a new constraint is added. + * To this end a bound or constraint is removed simultaneously if necessary. + * \return SUCCESSFUL_RETURN \n + RET_LI_RESOLVED \n + RET_ENSURELI_FAILED \n + RET_ENSURELI_FAILED_TQ \n + RET_ENSURELI_FAILED_NOINDEX \n + RET_REMOVE_FROM_ACTIVESET */ + returnValue addConstraint_ensureLI( int number, /**< Number of constraint to be added to active set. */ + SubjectToStatus C_status /**< Status of new active bound. */ + ); + + /** Adds a bound to active set. + * \return SUCCESSFUL_RETURN \n + RET_ADDBOUND_FAILED \n + RET_ADDBOUND_FAILED_INFEASIBILITY \n + RET_ENSURELI_FAILED */ + returnValue addBound( int number, /**< Number of bound to be added to active set. */ + SubjectToStatus B_status, /**< Status of new active bound. */ + BooleanType updateCholesky /**< Flag indicating if Cholesky decomposition shall be updated. */ + ); + + /** Checks if new active bound to be added is linearly dependent from + * from row of the active constraints matrix. + * \return RET_LINEARLY_DEPENDENT \n + RET_LINEARLY_INDEPENDENT */ + returnValue addBound_checkLI( int number /**< Number of bound to be added to active set. */ + ); + + /** Ensures linear independence of constraint matrix when a new bound is added. + * To this end a bound or constraint is removed simultaneously if necessary. + * \return SUCCESSFUL_RETURN \n + RET_LI_RESOLVED \n + RET_ENSURELI_FAILED \n + RET_ENSURELI_FAILED_TQ \n + RET_ENSURELI_FAILED_NOINDEX \n + RET_REMOVE_FROM_ACTIVESET */ + returnValue addBound_ensureLI( int number, /**< Number of bound to be added to active set. */ + SubjectToStatus B_status /**< Status of new active bound. */ + ); + + /** Removes a constraint from active set. + * \return SUCCESSFUL_RETURN \n + RET_CONSTRAINT_NOT_ACTIVE \n + RET_REMOVECONSTRAINT_FAILED \n + RET_HESSIAN_NOT_SPD */ + returnValue removeConstraint( int number, /**< Number of constraint to be removed from active set. */ + BooleanType updateCholesky /**< Flag indicating if Cholesky decomposition shall be updated. */ + ); + + /** Removes a bounds from active set. + * \return SUCCESSFUL_RETURN \n + RET_BOUND_NOT_ACTIVE \n + RET_HESSIAN_NOT_SPD \n + RET_REMOVEBOUND_FAILED */ + returnValue removeBound( int number, /**< Number of bound to be removed from active set. */ + BooleanType updateCholesky /**< Flag indicating if Cholesky decomposition shall be updated. */ + ); + + + /** Solves the system Ra = b or R^Ta = b where R is an upper triangular matrix. + * \return SUCCESSFUL_RETURN \n + RET_DIV_BY_ZERO */ + returnValue backsolveR( const real_t* const b, /**< Right hand side vector. */ + BooleanType transposed, /**< Indicates if the transposed system shall be solved. */ + real_t* const a /**< Output: Solution vector */ + ); + + /** Solves the system Ra = b or R^Ta = b where R is an upper triangular matrix. \n + * Special variant for the case that this function is called from within "removeBound()". + * \return SUCCESSFUL_RETURN \n + RET_DIV_BY_ZERO */ + returnValue backsolveR( const real_t* const b, /**< Right hand side vector. */ + BooleanType transposed, /**< Indicates if the transposed system shall be solved. */ + BooleanType removingBound, /**< Indicates if function is called from "removeBound()". */ + real_t* const a /**< Output: Solution vector */ + ); + + + /** Solves the system Ta = b or T^Ta = b where T is a reverse upper triangular matrix. + * \return SUCCESSFUL_RETURN \n + RET_DIV_BY_ZERO */ + returnValue backsolveT( const real_t* const b, /**< Right hand side vector. */ + BooleanType transposed, /**< Indicates if the transposed system shall be solved. */ + real_t* const a /**< Output: Solution vector */ + ); + + + /** Determines step direction of the shift of the QP data. + * \return SUCCESSFUL_RETURN */ + returnValue hotstart_determineDataShift(const int* const FX_idx, /**< Index array of fixed variables. */ + const int* const AC_idx, /**< Index array of active constraints. */ + const real_t* const g_new, /**< New gradient vector. */ + const real_t* const lbA_new,/**< New lower constraints' bounds. */ + const real_t* const ubA_new,/**< New upper constraints' bounds. */ + const real_t* const lb_new, /**< New lower bounds. */ + const real_t* const ub_new, /**< New upper bounds. */ + real_t* const delta_g, /**< Output: Step direction of gradient vector. */ + real_t* const delta_lbA, /**< Output: Step direction of lower constraints' bounds. */ + real_t* const delta_ubA, /**< Output: Step direction of upper constraints' bounds. */ + real_t* const delta_lb, /**< Output: Step direction of lower bounds. */ + real_t* const delta_ub, /**< Output: Step direction of upper bounds. */ + BooleanType& Delta_bC_isZero,/**< Output: Indicates if active constraints' bounds are to be shifted. */ + BooleanType& Delta_bB_isZero/**< Output: Indicates if active bounds are to be shifted. */ + ); + + /** Determines step direction of the homotopy path. + * \return SUCCESSFUL_RETURN \n + RET_STEPDIRECTION_FAILED_TQ \n + RET_STEPDIRECTION_FAILED_CHOLESKY */ + returnValue hotstart_determineStepDirection(const int* const FR_idx, /**< Index array of free variables. */ + const int* const FX_idx, /**< Index array of fixed variables. */ + const int* const AC_idx, /**< Index array of active constraints. */ + const real_t* const delta_g, /**< Step direction of gradient vector. */ + const real_t* const delta_lbA, /**< Step direction of lower constraints' bounds. */ + const real_t* const delta_ubA, /**< Step direction of upper constraints' bounds. */ + const real_t* const delta_lb, /**< Step direction of lower bounds. */ + const real_t* const delta_ub, /**< Step direction of upper bounds. */ + BooleanType Delta_bC_isZero, /**< Indicates if active constraints' bounds are to be shifted. */ + BooleanType Delta_bB_isZero, /**< Indicates if active bounds are to be shifted. */ + real_t* const delta_xFX, /**< Output: Primal homotopy step direction of fixed variables. */ + real_t* const delta_xFR, /**< Output: Primal homotopy step direction of free variables. */ + real_t* const delta_yAC, /**< Output: Dual homotopy step direction of active constraints' multiplier. */ + real_t* const delta_yFX /**< Output: Dual homotopy step direction of fixed variables' multiplier. */ + ); + + /** Determines the maximum possible step length along the homotopy path. + * \return SUCCESSFUL_RETURN */ + returnValue hotstart_determineStepLength( const int* const FR_idx, /**< Index array of free variables. */ + const int* const FX_idx, /**< Index array of fixed variables. */ + const int* const AC_idx, /**< Index array of active constraints. */ + const int* const IAC_idx, /**< Index array of inactive constraints. */ + const real_t* const delta_lbA, /**< Step direction of lower constraints' bounds. */ + const real_t* const delta_ubA, /**< Step direction of upper constraints' bounds. */ + const real_t* const delta_lb, /**< Step direction of lower bounds. */ + const real_t* const delta_ub, /**< Step direction of upper bounds. */ + const real_t* const delta_xFX, /**< Primal homotopy step direction of fixed variables. */ + const real_t* const delta_xFR, /**< Primal homotopy step direction of free variables. */ + const real_t* const delta_yAC, /**< Dual homotopy step direction of active constraints' multiplier. */ + const real_t* const delta_yFX, /**< Dual homotopy step direction of fixed variables' multiplier. */ + real_t* const delta_Ax, /**< Output: Step in vector Ax. */ + int& BC_idx, /**< Output: Index of blocking constraint. */ + SubjectToStatus& BC_status, /**< Output: Status of blocking constraint. */ + BooleanType& BC_isBound /**< Output: Indicates if blocking constraint is a bound. */ + ); + + /** Performs a step along the homotopy path (and updates active set). + * \return SUCCESSFUL_RETURN \n + RET_OPTIMAL_SOLUTION_FOUND \n + RET_REMOVE_FROM_ACTIVESET_FAILED \n + RET_ADD_TO_ACTIVESET_FAILED \n + RET_QP_INFEASIBLE */ + returnValue hotstart_performStep( const int* const FR_idx, /**< Index array of free variables. */ + const int* const FX_idx, /**< Index array of fixed variables. */ + const int* const AC_idx, /**< Index array of active constraints. */ + const int* const IAC_idx, /**< Index array of inactive constraints. */ + const real_t* const delta_g, /**< Step direction of gradient vector. */ + const real_t* const delta_lbA, /**< Step direction of lower constraints' bounds. */ + const real_t* const delta_ubA, /**< Step direction of upper constraints' bounds. */ + const real_t* const delta_lb, /**< Step direction of lower bounds. */ + const real_t* const delta_ub, /**< Step direction of upper bounds. */ + const real_t* const delta_xFX, /**< Primal homotopy step direction of fixed variables. */ + const real_t* const delta_xFR, /**< Primal homotopy step direction of free variables. */ + const real_t* const delta_yAC, /**< Dual homotopy step direction of active constraints' multiplier. */ + const real_t* const delta_yFX, /**< Dual homotopy step direction of fixed variables' multiplier. */ + const real_t* const delta_Ax, /**< Step in vector Ax. */ + int BC_idx, /**< Index of blocking constraint. */ + SubjectToStatus BC_status, /**< Status of blocking constraint. */ + BooleanType BC_isBound /**< Indicates if blocking constraint is a bound. */ + ); + + + /** Checks if lower/upper (constraints') bounds remain consistent + * (i.e. if lb <= ub and lbA <= ubA ) during the current step. + * \return BT_TRUE iff (constraints") bounds remain consistent + */ + BooleanType areBoundsConsistent( const real_t* const delta_lb, /**< Step direction of lower bounds. */ + const real_t* const delta_ub, /**< Step direction of upper bounds. */ + const real_t* const delta_lbA, /**< Step direction of lower constraints' bounds. */ + const real_t* const delta_ubA /**< Step direction of upper constraints' bounds. */ + ) const; + + + /** Setups internal QP data. + * \return SUCCESSFUL_RETURN \n + RET_INVALID_ARGUMENTS */ + returnValue setupQPdata( const real_t* const _H, /**< Hessian matrix. */ + const real_t* const _R, /**< Cholesky factorization of the Hessian matrix. */ + const real_t* const _g, /**< Gradient vector. */ + const real_t* const _A, /**< Constraint matrix. */ + const real_t* const _lb, /**< Lower bound vector (on variables). \n + If no lower bounds exist, a NULL pointer can be passed. */ + const real_t* const _ub, /**< Upper bound vector (on variables). \n + If no upper bounds exist, a NULL pointer can be passed. */ + const real_t* const _lbA, /**< Lower constraints' bound vector. \n + If no lower constraints' bounds exist, a NULL pointer can be passed. */ + const real_t* const _ubA /**< Upper constraints' bound vector. \n + If no lower constraints' bounds exist, a NULL pointer can be passed. */ + ); + + + #ifdef PC_DEBUG /* Define print functions only for debugging! */ + + /** Prints concise information on the current iteration. + * \return SUCCESSFUL_RETURN \n */ + returnValue printIteration( int iteration, /**< Number of current iteration. */ + int BC_idx, /**< Index of blocking constraint. */ + SubjectToStatus BC_status, /**< Status of blocking constraint. */ + BooleanType BC_isBound /**< Indicates if blocking constraint is a bound. */ + ); + + /** Prints concise information on the current iteration. + * NOTE: ONLY DEFINED FOR SUPPRESSING A COMPILER WARNING!! + * \return SUCCESSFUL_RETURN \n */ + returnValue printIteration( int iteration, /**< Number of current iteration. */ + int BC_idx, /**< Index of blocking bound. */ + SubjectToStatus BC_status /**< Status of blocking bound. */ + ); + + #endif /* PC_DEBUG */ + + + /** Determines the maximum violation of the KKT optimality conditions + * of the current iterate within the QProblem object. + * \return SUCCESSFUL_RETURN \n + * RET_INACCURATE_SOLUTION \n + * RET_NO_SOLUTION */ + returnValue checkKKTconditions( ); + + + /** Sets constraint matrix of the QP. \n + (Remark: Also internal vector Ax is recomputed!) + * \return SUCCESSFUL_RETURN */ + inline returnValue setA( const real_t* const A_new /**< New constraint matrix (with correct dimension!). */ + ); + + /** Changes single row of constraint matrix of the QP. \n + (Remark: Also correponding component of internal vector Ax is recomputed!) + * \return SUCCESSFUL_RETURN \n + RET_INDEX_OUT_OF_BOUNDS */ + inline returnValue setA( int number, /**< Number of row to be changed. */ + const real_t* const value /**< New (number)th constraint (with correct dimension!). */ + ); + + + /** Sets constraints' lower bound vector of the QP. + * \return SUCCESSFUL_RETURN */ + inline returnValue setLBA( const real_t* const lbA_new /**< New constraints' lower bound vector (with correct dimension!). */ + ); + + /** Changes single entry of lower constraints' bound vector of the QP. + * \return SUCCESSFUL_RETURN \n + RET_INDEX_OUT_OF_BOUNDS */ + inline returnValue setLBA( int number, /**< Number of entry to be changed. */ + real_t value /**< New value for entry of lower constraints' bound vector (with correct dimension!). */ + ); + + /** Sets constraints' upper bound vector of the QP. + * \return SUCCESSFUL_RETURN */ + inline returnValue setUBA( const real_t* const ubA_new /**< New constraints' upper bound vector (with correct dimension!). */ + ); + + /** Changes single entry of upper constraints' bound vector of the QP. + * \return SUCCESSFUL_RETURN \n + RET_INDEX_OUT_OF_BOUNDS */ + inline returnValue setUBA( int number, /**< Number of entry to be changed. */ + real_t value /**< New value for entry of upper constraints' bound vector (with correct dimension!). */ + ); + + + /* + * PROTECTED MEMBER VARIABLES + */ + protected: + real_t A[NCMAX_ALLOC*NVMAX]; /**< Constraint matrix. */ + real_t lbA[NCMAX_ALLOC]; /**< Lower constraints' bound vector. */ + real_t ubA[NCMAX_ALLOC]; /**< Upper constraints' bound vector. */ + + Constraints constraints; /**< Data structure for problem's constraints. */ + + real_t T[NVMAX*NVMAX]; /**< Reverse triangular matrix, A = [0 T]*Q'. */ + real_t Q[NVMAX*NVMAX]; /**< Orthonormal quadratic matrix, A = [0 T]*Q'. */ + int sizeT; /**< Matrix T is stored in a (sizeT x sizeT) array. */ + + real_t Ax[NCMAX_ALLOC]; /**< Stores the current product A*x (for increased efficiency only). */ + + CyclingManager cyclingManager; /**< Data structure for storing (possible) cycling information (NOT YET IMPLEMENTED!). */ +}; + + +#include + +#endif /* QPOASES_QPROBLEM_HPP */ + + +/* + * end of file + */ diff --git a/phonelibs/qpoases/INCLUDE/QProblemB.hpp b/phonelibs/qpoases/INCLUDE/QProblemB.hpp new file mode 100644 index 00000000000000..8b3acb82bddab4 --- /dev/null +++ b/phonelibs/qpoases/INCLUDE/QProblemB.hpp @@ -0,0 +1,628 @@ +/* + * This file is part of qpOASES. + * + * qpOASES -- An Implementation of the Online Active Set Strategy. + * Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved. + * + * qpOASES is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * qpOASES is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with qpOASES; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + + +/** + * \file INCLUDE/QProblemB.hpp + * \author Hans Joachim Ferreau + * \version 1.3embedded + * \date 2007-2008 + * + * Declaration of the QProblemB class which is able to use the newly + * developed online active set strategy for parametric quadratic programming + * for problems with (simple) bounds only. + */ + + + +#ifndef QPOASES_QPROBLEMB_HPP +#define QPOASES_QPROBLEMB_HPP + + +#include + + + +class SolutionAnalysis; + +/** Class for setting up and solving quadratic programs with (simple) bounds only. + * The main feature is the possibily to use the newly developed online active set strategy + * for parametric quadratic programming. + * + * \author Hans Joachim Ferreau + * \version 1.3embedded + * \date 2007-2008 + */ +class QProblemB +{ + /* allow SolutionAnalysis class to access private members */ + friend class SolutionAnalysis; + + /* + * PUBLIC MEMBER FUNCTIONS + */ + public: + /** Default constructor. */ + QProblemB( ); + + /** Constructor which takes the QP dimension only. */ + QProblemB( int _nV /**< Number of variables. */ + ); + + /** Copy constructor (deep copy). */ + QProblemB( const QProblemB& rhs /**< Rhs object. */ + ); + + /** Destructor. */ + ~QProblemB( ); + + /** Assignment operator (deep copy). */ + QProblemB& operator=( const QProblemB& rhs /**< Rhs object. */ + ); + + + /** Clears all data structures of QProblemB except for QP data. + * \return SUCCESSFUL_RETURN \n + RET_RESET_FAILED */ + returnValue reset( ); + + + /** Initialises a QProblemB with given QP data and solves it + * using an initial homotopy with empty working set (at most nWSR iterations). + * \return SUCCESSFUL_RETURN \n + RET_INIT_FAILED \n + RET_INIT_FAILED_CHOLESKY \n + RET_INIT_FAILED_HOTSTART \n + RET_INIT_FAILED_INFEASIBILITY \n + RET_INIT_FAILED_UNBOUNDEDNESS \n + RET_MAX_NWSR_REACHED \n + RET_INVALID_ARGUMENTS \n + RET_INACCURATE_SOLUTION \n + RET_NO_SOLUTION */ + returnValue init( const real_t* const _H, /**< Hessian matrix. */ + const real_t* const _g, /**< Gradient vector. */ + const real_t* const _lb, /**< Lower bounds (on variables). \n + If no lower bounds exist, a NULL pointer can be passed. */ + const real_t* const _ub, /**< Upper bounds (on variables). \n + If no upper bounds exist, a NULL pointer can be passed. */ + int& nWSR, /**< Input: Maximum number of working set recalculations when using initial homotopy. \n + Output: Number of performed working set recalculations. */ + const real_t* const yOpt = 0, /**< Initial guess for dual solution vector. */ + real_t* const cputime = 0 /**< Output: CPU time required to initialise QP. */ + ); + + + /** Initialises a QProblemB with given QP data and solves it + * using an initial homotopy with empty working set (at most nWSR iterations). + * \return SUCCESSFUL_RETURN \n + RET_INIT_FAILED \n + RET_INIT_FAILED_CHOLESKY \n + RET_INIT_FAILED_HOTSTART \n + RET_INIT_FAILED_INFEASIBILITY \n + RET_INIT_FAILED_UNBOUNDEDNESS \n + RET_MAX_NWSR_REACHED \n + RET_INVALID_ARGUMENTS \n + RET_INACCURATE_SOLUTION \n + RET_NO_SOLUTION */ + returnValue init( const real_t* const _H, /**< Hessian matrix. */ + const real_t* const _R, /**< Cholesky factorization of the Hessian matrix. */ + const real_t* const _g, /**< Gradient vector. */ + const real_t* const _lb, /**< Lower bounds (on variables). \n + If no lower bounds exist, a NULL pointer can be passed. */ + const real_t* const _ub, /**< Upper bounds (on variables). \n + If no upper bounds exist, a NULL pointer can be passed. */ + int& nWSR, /**< Input: Maximum number of working set recalculations when using initial homotopy. \n + Output: Number of performed working set recalculations. */ + const real_t* const yOpt = 0, /**< Initial guess for dual solution vector. */ + real_t* const cputime = 0 /**< Output: CPU time required to initialise QP. */ + ); + + + /** Solves an initialised QProblemB using online active set strategy. + * \return SUCCESSFUL_RETURN \n + RET_MAX_NWSR_REACHED \n + RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n + RET_HOTSTART_FAILED \n + RET_SHIFT_DETERMINATION_FAILED \n + RET_STEPDIRECTION_DETERMINATION_FAILED \n + RET_STEPLENGTH_DETERMINATION_FAILED \n + RET_HOMOTOPY_STEP_FAILED \n + RET_HOTSTART_STOPPED_INFEASIBILITY \n + RET_HOTSTART_STOPPED_UNBOUNDEDNESS \n + RET_INACCURATE_SOLUTION \n + RET_NO_SOLUTION */ + returnValue hotstart( const real_t* const g_new, /**< Gradient of neighbouring QP to be solved. */ + const real_t* const lb_new, /**< Lower bounds of neighbouring QP to be solved. \n + If no lower bounds exist, a NULL pointer can be passed. */ + const real_t* const ub_new, /**< Upper bounds of neighbouring QP to be solved. \n + If no upper bounds exist, a NULL pointer can be passed. */ + int& nWSR, /**< Input: Maximum number of working set recalculations; \n + Output: Number of performed working set recalculations. */ + real_t* const cputime /**< Output: CPU time required to solve QP (or to perform nWSR iterations). */ + ); + + + /** Returns Hessian matrix of the QP (deep copy). + * \return SUCCESSFUL_RETURN */ + inline returnValue getH( real_t* const _H /**< Array of appropriate dimension for copying Hessian matrix.*/ + ) const; + + /** Returns gradient vector of the QP (deep copy). + * \return SUCCESSFUL_RETURN */ + inline returnValue getG( real_t* const _g /**< Array of appropriate dimension for copying gradient vector.*/ + ) const; + + /** Returns lower bound vector of the QP (deep copy). + * \return SUCCESSFUL_RETURN */ + inline returnValue getLB( real_t* const _lb /**< Array of appropriate dimension for copying lower bound vector.*/ + ) const; + + /** Returns single entry of lower bound vector of the QP. + * \return SUCCESSFUL_RETURN \n + RET_INDEX_OUT_OF_BOUNDS */ + inline returnValue getLB( int number, /**< Number of entry to be returned. */ + real_t& value /**< Output: lb[number].*/ + ) const; + + /** Returns upper bound vector of the QP (deep copy). + * \return SUCCESSFUL_RETURN */ + inline returnValue getUB( real_t* const _ub /**< Array of appropriate dimension for copying upper bound vector.*/ + ) const; + + /** Returns single entry of upper bound vector of the QP. + * \return SUCCESSFUL_RETURN \n + RET_INDEX_OUT_OF_BOUNDS */ + inline returnValue getUB( int number, /**< Number of entry to be returned. */ + real_t& value /**< Output: ub[number].*/ + ) const; + + + /** Returns current bounds object of the QP (deep copy). + * \return SUCCESSFUL_RETURN */ + inline returnValue getBounds( Bounds* const _bounds /** Output: Bounds object. */ + ) const; + + + /** Returns the number of variables. + * \return Number of variables. */ + inline int getNV( ) const; + + /** Returns the number of free variables. + * \return Number of free variables. */ + inline int getNFR( ); + + /** Returns the number of fixed variables. + * \return Number of fixed variables. */ + inline int getNFX( ); + + /** Returns the number of implicitly fixed variables. + * \return Number of implicitly fixed variables. */ + inline int getNFV( ) const; + + /** Returns the dimension of null space. + * \return Dimension of null space. */ + int getNZ( ); + + + /** Returns the optimal objective function value. + * \return finite value: Optimal objective function value (QP was solved) \n + +infinity: QP was not yet solved */ + real_t getObjVal( ) const; + + /** Returns the objective function value at an arbitrary point x. + * \return Objective function value at point x */ + real_t getObjVal( const real_t* const _x /**< Point at which the objective function shall be evaluated. */ + ) const; + + /** Returns the primal solution vector. + * \return SUCCESSFUL_RETURN \n + RET_QP_NOT_SOLVED */ + returnValue getPrimalSolution( real_t* const xOpt /**< Output: Primal solution vector (if QP has been solved). */ + ) const; + + /** Returns the dual solution vector. + * \return SUCCESSFUL_RETURN \n + RET_QP_NOT_SOLVED */ + returnValue getDualSolution( real_t* const yOpt /**< Output: Dual solution vector (if QP has been solved). */ + ) const; + + + /** Returns status of the solution process. + * \return Status of solution process. */ + inline QProblemStatus getStatus( ) const; + + + /** Returns if the QProblem object is initialised. + * \return BT_TRUE: QProblemB initialised \n + BT_FALSE: QProblemB not initialised */ + inline BooleanType isInitialised( ) const; + + /** Returns if the QP has been solved. + * \return BT_TRUE: QProblemB solved \n + BT_FALSE: QProblemB not solved */ + inline BooleanType isSolved( ) const; + + /** Returns if the QP is infeasible. + * \return BT_TRUE: QP infeasible \n + BT_FALSE: QP feasible (or not known to be infeasible!) */ + inline BooleanType isInfeasible( ) const; + + /** Returns if the QP is unbounded. + * \return BT_TRUE: QP unbounded \n + BT_FALSE: QP unbounded (or not known to be unbounded!) */ + inline BooleanType isUnbounded( ) const; + + + /** Returns the print level. + * \return Print level. */ + inline PrintLevel getPrintLevel( ) const; + + /** Changes the print level. + * \return SUCCESSFUL_RETURN */ + returnValue setPrintLevel( PrintLevel _printlevel /**< New print level. */ + ); + + + /** Returns Hessian type flag (type is not determined due to this call!). + * \return Hessian type. */ + inline HessianType getHessianType( ) const; + + /** Changes the print level. + * \return SUCCESSFUL_RETURN */ + inline returnValue setHessianType( HessianType _hessianType /**< New Hessian type. */ + ); + + + /* + * PROTECTED MEMBER FUNCTIONS + */ + protected: + /** Checks if Hessian happens to be the identity matrix, + * and sets corresponding status flag (otherwise the flag remains unaltered!). + * \return SUCCESSFUL_RETURN */ + returnValue checkForIdentityHessian( ); + + /** Determines type of constraints and bounds (i.e. implicitly fixed, unbounded etc.). + * \return SUCCESSFUL_RETURN \n + RET_SETUPSUBJECTTOTYPE_FAILED */ + returnValue setupSubjectToType( ); + + /** Computes the Cholesky decomposition R of the (simply projected) Hessian (i.e. R^T*R = Z^T*H*Z). + * It only works in the case where Z is a simple projection matrix! + * \return SUCCESSFUL_RETURN \n + * RET_INDEXLIST_CORRUPTED */ + returnValue setupCholeskyDecomposition( ); + + + /** Solves a QProblemB whose QP data is assumed to be stored in the member variables. + * A guess for its primal/dual optimal solution vectors and the corresponding + * optimal working set can be provided. + * \return SUCCESSFUL_RETURN \n + RET_INIT_FAILED \n + RET_INIT_FAILED_CHOLESKY \n + RET_INIT_FAILED_HOTSTART \n + RET_INIT_FAILED_INFEASIBILITY \n + RET_INIT_FAILED_UNBOUNDEDNESS \n + RET_MAX_NWSR_REACHED */ + returnValue solveInitialQP( const real_t* const xOpt, /**< Optimal primal solution vector. + * A NULL pointer can be passed. */ + const real_t* const yOpt, /**< Optimal dual solution vector. + * A NULL pointer can be passed. */ + const Bounds* const guessedBounds, /**< Guessed working set for solution (xOpt,yOpt). + * A NULL pointer can be passed. */ + int& nWSR, /**< Input: Maximum number of working set recalculations; \n + * Output: Number of performed working set recalculations. */ + real_t* const cputime /**< Output: CPU time required to solve QP (or to perform nWSR iterations). */ + ); + + + /** Obtains the desired working set for the auxiliary initial QP in + * accordance with the user specifications + * \return SUCCESSFUL_RETURN \n + RET_OBTAINING_WORKINGSET_FAILED \n + RET_INVALID_ARGUMENTS */ + returnValue obtainAuxiliaryWorkingSet( const real_t* const xOpt, /**< Optimal primal solution vector. + * If a NULL pointer is passed, all entries are assumed to be zero. */ + const real_t* const yOpt, /**< Optimal dual solution vector. + * If a NULL pointer is passed, all entries are assumed to be zero. */ + const Bounds* const guessedBounds, /**< Guessed working set for solution (xOpt,yOpt). */ + Bounds* auxiliaryBounds /**< Input: Allocated bound object. \n + * Ouput: Working set for auxiliary QP. */ + ) const; + + /** Setups bound data structure according to auxiliaryBounds. + * (If the working set shall be setup afresh, make sure that + * bounds data structure has been resetted!) + * \return SUCCESSFUL_RETURN \n + RET_SETUP_WORKINGSET_FAILED \n + RET_INVALID_ARGUMENTS \n + RET_UNKNOWN BUG */ + returnValue setupAuxiliaryWorkingSet( const Bounds* const auxiliaryBounds, /**< Working set for auxiliary QP. */ + BooleanType setupAfresh /**< Flag indicating if given working set shall be + * setup afresh or by updating the current one. */ + ); + + /** Setups the optimal primal/dual solution of the auxiliary initial QP. + * \return SUCCESSFUL_RETURN */ + returnValue setupAuxiliaryQPsolution( const real_t* const xOpt, /**< Optimal primal solution vector. + * If a NULL pointer is passed, all entries are set to zero. */ + const real_t* const yOpt /**< Optimal dual solution vector. + * If a NULL pointer is passed, all entries are set to zero. */ + ); + + /** Setups gradient of the auxiliary initial QP for given + * optimal primal/dual solution and given initial working set + * (assumes that members X, Y and BOUNDS have already been initialised!). + * \return SUCCESSFUL_RETURN */ + returnValue setupAuxiliaryQPgradient( ); + + /** Setups bounds of the auxiliary initial QP for given + * optimal primal/dual solution and given initial working set + * (assumes that members X, Y and BOUNDS have already been initialised!). + * \return SUCCESSFUL_RETURN \n + RET_UNKNOWN BUG */ + returnValue setupAuxiliaryQPbounds( BooleanType useRelaxation /**< Flag indicating if inactive bounds shall be relaxed. */ + ); + + + /** Adds a bound to active set (specialised version for the case where no constraints exist). + * \return SUCCESSFUL_RETURN \n + RET_ADDBOUND_FAILED */ + returnValue addBound( int number, /**< Number of bound to be added to active set. */ + SubjectToStatus B_status, /**< Status of new active bound. */ + BooleanType updateCholesky /**< Flag indicating if Cholesky decomposition shall be updated. */ + ); + + /** Removes a bounds from active set (specialised version for the case where no constraints exist). + * \return SUCCESSFUL_RETURN \n + RET_HESSIAN_NOT_SPD \n + RET_REMOVEBOUND_FAILED */ + returnValue removeBound( int number, /**< Number of bound to be removed from active set. */ + BooleanType updateCholesky /**< Flag indicating if Cholesky decomposition shall be updated. */ + ); + + + /** Solves the system Ra = b or R^Ta = b where R is an upper triangular matrix. + * \return SUCCESSFUL_RETURN \n + RET_DIV_BY_ZERO */ + returnValue backsolveR( const real_t* const b, /**< Right hand side vector. */ + BooleanType transposed, /**< Indicates if the transposed system shall be solved. */ + real_t* const a /**< Output: Solution vector */ + ); + + /** Solves the system Ra = b or R^Ta = b where R is an upper triangular matrix. \n + * Special variant for the case that this function is called from within "removeBound()". + * \return SUCCESSFUL_RETURN \n + RET_DIV_BY_ZERO */ + returnValue backsolveR( const real_t* const b, /**< Right hand side vector. */ + BooleanType transposed, /**< Indicates if the transposed system shall be solved. */ + BooleanType removingBound, /**< Indicates if function is called from "removeBound()". */ + real_t* const a /**< Output: Solution vector */ + ); + + + /** Determines step direction of the shift of the QP data. + * \return SUCCESSFUL_RETURN */ + returnValue hotstart_determineDataShift(const int* const FX_idx, /**< Index array of fixed variables. */ + const real_t* const g_new, /**< New gradient vector. */ + const real_t* const lb_new, /**< New lower bounds. */ + const real_t* const ub_new, /**< New upper bounds. */ + real_t* const delta_g, /**< Output: Step direction of gradient vector. */ + real_t* const delta_lb, /**< Output: Step direction of lower bounds. */ + real_t* const delta_ub, /**< Output: Step direction of upper bounds. */ + BooleanType& Delta_bB_isZero/**< Output: Indicates if active bounds are to be shifted. */ + ); + + + /** Checks if lower/upper bounds remain consistent + * (i.e. if lb <= ub) during the current step. + * \return BT_TRUE iff bounds remain consistent + */ + BooleanType areBoundsConsistent( const real_t* const delta_lb, /**< Step direction of lower bounds. */ + const real_t* const delta_ub /**< Step direction of upper bounds. */ + ) const; + + + /** Setups internal QP data. + * \return SUCCESSFUL_RETURN \n + RET_INVALID_ARGUMENTS */ + returnValue setupQPdata( const real_t* const _H, /**< Hessian matrix. */ + const real_t* const _R, /**< Cholesky factorization of the Hessian matrix. */ + const real_t* const _g, /**< Gradient vector. */ + const real_t* const _lb, /**< Lower bounds (on variables). \n + If no lower bounds exist, a NULL pointer can be passed. */ + const real_t* const _ub /**< Upper bounds (on variables). \n + If no upper bounds exist, a NULL pointer can be passed. */ + ); + + + /** Sets Hessian matrix of the QP. + * \return SUCCESSFUL_RETURN */ + inline returnValue setH( const real_t* const H_new /**< New Hessian matrix (with correct dimension!). */ + ); + + /** Changes gradient vector of the QP. + * \return SUCCESSFUL_RETURN */ + inline returnValue setG( const real_t* const g_new /**< New gradient vector (with correct dimension!). */ + ); + + /** Changes lower bound vector of the QP. + * \return SUCCESSFUL_RETURN */ + inline returnValue setLB( const real_t* const lb_new /**< New lower bound vector (with correct dimension!). */ + ); + + /** Changes single entry of lower bound vector of the QP. + * \return SUCCESSFUL_RETURN \n + RET_INDEX_OUT_OF_BOUNDS */ + inline returnValue setLB( int number, /**< Number of entry to be changed. */ + real_t value /**< New value for entry of lower bound vector. */ + ); + + /** Changes upper bound vector of the QP. + * \return SUCCESSFUL_RETURN */ + inline returnValue setUB( const real_t* const ub_new /**< New upper bound vector (with correct dimension!). */ + ); + + /** Changes single entry of upper bound vector of the QP. + * \return SUCCESSFUL_RETURN \n + RET_INDEX_OUT_OF_BOUNDS */ + inline returnValue setUB( int number, /**< Number of entry to be changed. */ + real_t value /**< New value for entry of upper bound vector. */ + ); + + + /** Computes parameters for the Givens matrix G for which [x,y]*G = [z,0] + * \return SUCCESSFUL_RETURN */ + inline void computeGivens( real_t xold, /**< Matrix entry to be normalised. */ + real_t yold, /**< Matrix entry to be annihilated. */ + real_t& xnew, /**< Output: Normalised matrix entry. */ + real_t& ynew, /**< Output: Annihilated matrix entry. */ + real_t& c, /**< Output: Cosine entry of Givens matrix. */ + real_t& s /**< Output: Sine entry of Givens matrix. */ + ) const; + + /** Applies Givens matrix determined by c and s (cf. computeGivens). + * \return SUCCESSFUL_RETURN */ + inline void applyGivens( real_t c, /**< Cosine entry of Givens matrix. */ + real_t s, /**< Sine entry of Givens matrix. */ + real_t xold, /**< Matrix entry to be transformed corresponding to + * the normalised entry of the original matrix. */ + real_t yold, /**< Matrix entry to be transformed corresponding to + * the annihilated entry of the original matrix. */ + real_t& xnew, /**< Output: Transformed matrix entry corresponding to + * the normalised entry of the original matrix. */ + real_t& ynew /**< Output: Transformed matrix entry corresponding to + * the annihilated entry of the original matrix. */ + ) const; + + + /* + * PRIVATE MEMBER FUNCTIONS + */ + private: + /** Determines step direction of the homotopy path. + * \return SUCCESSFUL_RETURN \n + RET_STEPDIRECTION_FAILED_CHOLESKY */ + returnValue hotstart_determineStepDirection(const int* const FR_idx, /**< Index array of free variables. */ + const int* const FX_idx, /**< Index array of fixed variables. */ + const real_t* const delta_g, /**< Step direction of gradient vector. */ + const real_t* const delta_lb, /**< Step direction of lower bounds. */ + const real_t* const delta_ub, /**< Step direction of upper bounds. */ + BooleanType Delta_bB_isZero, /**< Indicates if active bounds are to be shifted. */ + real_t* const delta_xFX, /**< Output: Primal homotopy step direction of fixed variables. */ + real_t* const delta_xFR, /**< Output: Primal homotopy step direction of free variables. */ + real_t* const delta_yFX /**< Output: Dual homotopy step direction of fixed variables' multiplier. */ + ); + + /** Determines the maximum possible step length along the homotopy path. + * \return SUCCESSFUL_RETURN */ + returnValue hotstart_determineStepLength( const int* const FR_idx, /**< Index array of free variables. */ + const int* const FX_idx, /**< Index array of fixed variables. */ + const real_t* const delta_lb, /**< Step direction of lower bounds. */ + const real_t* const delta_ub, /**< Step direction of upper bounds. */ + const real_t* const delta_xFR, /**< Primal homotopy step direction of free variables. */ + const real_t* const delta_yFX, /**< Dual homotopy step direction of fixed variables' multiplier. */ + int& BC_idx, /**< Output: Index of blocking constraint. */ + SubjectToStatus& BC_status /**< Output: Status of blocking constraint. */ + ); + + /** Performs a step along the homotopy path (and updates active set). + * \return SUCCESSFUL_RETURN \n + RET_OPTIMAL_SOLUTION_FOUND \n + RET_REMOVE_FROM_ACTIVESET_FAILED \n + RET_ADD_TO_ACTIVESET_FAILED \n + RET_QP_INFEASIBLE */ + returnValue hotstart_performStep( const int* const FR_idx, /**< Index array of free variables. */ + const int* const FX_idx, /**< Index array of fixed variables. */ + const real_t* const delta_g, /**< Step direction of gradient vector. */ + const real_t* const delta_lb, /**< Step direction of lower bounds. */ + const real_t* const delta_ub, /**< Step direction of upper bounds. */ + const real_t* const delta_xFX, /**< Primal homotopy step direction of fixed variables. */ + const real_t* const delta_xFR, /**< Primal homotopy step direction of free variables. */ + const real_t* const delta_yFX, /**< Dual homotopy step direction of fixed variables' multiplier. */ + int BC_idx, /**< Index of blocking constraint. */ + SubjectToStatus BC_status /**< Status of blocking constraint. */ + ); + + + #ifdef PC_DEBUG /* Define print functions only for debugging! */ + + /** Prints concise information on the current iteration. + * \return SUCCESSFUL_RETURN \n */ + returnValue printIteration( int iteration, /**< Number of current iteration. */ + int BC_idx, /**< Index of blocking bound. */ + SubjectToStatus BC_status /**< Status of blocking bound. */ + ); + + #endif /* PC_DEBUG */ + + + /** Determines the maximum violation of the KKT optimality conditions + * of the current iterate within the QProblemB object. + * \return SUCCESSFUL_RETURN \n + * RET_INACCURATE_SOLUTION \n + * RET_NO_SOLUTION */ + returnValue checkKKTconditions( ); + + + /* + * PROTECTED MEMBER VARIABLES + */ + protected: + real_t H[NVMAX*NVMAX]; /**< Hessian matrix. */ + BooleanType hasHessian; /**< Flag indicating whether H contains Hessian or corresponding Cholesky factor R; \sa init. */ + + real_t g[NVMAX]; /**< Gradient. */ + real_t lb[NVMAX]; /**< Lower bound vector (on variables). */ + real_t ub[NVMAX]; /**< Upper bound vector (on variables). */ + + Bounds bounds; /**< Data structure for problem's bounds. */ + + real_t R[NVMAX*NVMAX]; /**< Cholesky decomposition of H (i.e. H = R^T*R). */ + BooleanType hasCholesky; /**< Flag indicating whether Cholesky decomposition has already been setup. */ + + real_t x[NVMAX]; /**< Primal solution vector. */ + real_t y[NVMAX+NCMAX]; /**< Dual solution vector. */ + + real_t tau; /**< Last homotopy step length. */ + + QProblemStatus status; /**< Current status of the solution process. */ + + BooleanType infeasible; /**< QP infeasible? */ + BooleanType unbounded; /**< QP unbounded? */ + + HessianType hessianType; /**< Type of Hessian matrix. */ + + PrintLevel printlevel; /**< Print level. */ + + int count; /**< Counts the number of hotstart function calls (internal usage only!). */ +}; + + +#include + +#endif /* QPOASES_QPROBLEMB_HPP */ + + +/* + * end of file + */ diff --git a/phonelibs/qpoases/INCLUDE/SubjectTo.hpp b/phonelibs/qpoases/INCLUDE/SubjectTo.hpp new file mode 100644 index 00000000000000..a014843ceabb24 --- /dev/null +++ b/phonelibs/qpoases/INCLUDE/SubjectTo.hpp @@ -0,0 +1,178 @@ +/* + * This file is part of qpOASES. + * + * qpOASES -- An Implementation of the Online Active Set Strategy. + * Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved. + * + * qpOASES is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * qpOASES is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with qpOASES; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + + +/** + * \file INCLUDE/SubjectTo.hpp + * \author Hans Joachim Ferreau + * \version 1.3embedded + * \date 2007-2008 + * + * Declaration of the SubjectTo class designed to manage working sets of + * constraints and bounds within a QProblem. + */ + + +#ifndef QPOASES_SUBJECTTO_HPP +#define QPOASES_SUBJECTTO_HPP + + +#include + + + +/** This class manages working sets of constraints and bounds by storing + * index sets and other status information. + * + * \author Hans Joachim Ferreau + * \version 1.3embedded + * \date 2007-2008 + */ +class SubjectTo +{ + /* + * PUBLIC MEMBER FUNCTIONS + */ + public: + /** Default constructor. */ + SubjectTo( ); + + /** Copy constructor (deep copy). */ + SubjectTo( const SubjectTo& rhs /**< Rhs object. */ + ); + + /** Destructor. */ + ~SubjectTo( ); + + /** Assignment operator (deep copy). */ + SubjectTo& operator=( const SubjectTo& rhs /**< Rhs object. */ + ); + + + /** Pseudo-constructor takes the number of constraints or bounds. + * \return SUCCESSFUL_RETURN */ + returnValue init( int n /**< Number of constraints or bounds. */ + ); + + + /** Returns type of (constraints') bound. + * \return Type of (constraints') bound \n + RET_INDEX_OUT_OF_BOUNDS */ + inline SubjectToType getType( int i /**< Number of (constraints') bound. */ + ) const ; + + /** Returns status of (constraints') bound. + * \return Status of (constraints') bound \n + ST_UNDEFINED */ + inline SubjectToStatus getStatus( int i /**< Number of (constraints') bound. */ + ) const; + + + /** Sets type of (constraints') bound. + * \return SUCCESSFUL_RETURN \n + RET_INDEX_OUT_OF_BOUNDS */ + inline returnValue setType( int i, /**< Number of (constraints') bound. */ + SubjectToType value /**< Type of (constraints') bound. */ + ); + + /** Sets status of (constraints') bound. + * \return SUCCESSFUL_RETURN \n + RET_INDEX_OUT_OF_BOUNDS */ + inline returnValue setStatus( int i, /**< Number of (constraints') bound. */ + SubjectToStatus value /**< Status of (constraints') bound. */ + ); + + + /** Sets status of lower (constraints') bounds. */ + inline void setNoLower( BooleanType _status /**< Status of lower (constraints') bounds. */ + ); + + /** Sets status of upper (constraints') bounds. */ + inline void setNoUpper( BooleanType _status /**< Status of upper (constraints') bounds. */ + ); + + + /** Returns status of lower (constraints') bounds. + * \return BT_TRUE if there is no lower (constraints') bound on any variable. */ + inline BooleanType isNoLower( ) const; + + /** Returns status of upper bounds. + * \return BT_TRUE if there is no upper (constraints') bound on any variable. */ + inline BooleanType isNoUpper( ) const; + + + /* + * PROTECTED MEMBER FUNCTIONS + */ + protected: + /** Adds the index of a new constraint or bound to index set. + * \return SUCCESSFUL_RETURN \n + RET_ADDINDEX_FAILED */ + returnValue addIndex( Indexlist* const indexlist, /**< Index list to which the new index shall be added. */ + int newnumber, /**< Number of new constraint or bound. */ + SubjectToStatus newstatus /**< Status of new constraint or bound. */ + ); + + /** Removes the index of a constraint or bound from index set. + * \return SUCCESSFUL_RETURN \n + RET_UNKNOWN_BUG */ + returnValue removeIndex( Indexlist* const indexlist, /**< Index list from which the new index shall be removed. */ + int removenumber /**< Number of constraint or bound to be removed. */ + ); + + /** Swaps the indices of two constraints or bounds within the index set. + * \return SUCCESSFUL_RETURN \n + RET_SWAPINDEX_FAILED */ + returnValue swapIndex( Indexlist* const indexlist, /**< Index list in which the indices shold be swapped. */ + int number1, /**< Number of first constraint or bound. */ + int number2 /**< Number of second constraint or bound. */ + ); + + + /* + * PROTECTED MEMBER VARIABLES + */ + protected: + SubjectToType type[NVMAX+NCMAX]; /**< Type of constraints/bounds. */ + SubjectToStatus status[NVMAX+NCMAX]; /**< Status of constraints/bounds. */ + + BooleanType noLower; /**< This flag indicates if there is no lower bound on any variable. */ + BooleanType noUpper; /**< This flag indicates if there is no upper bound on any variable. */ + + + /* + * PRIVATE MEMBER VARIABLES + */ + private: + int size; +}; + + + +#include + +#endif /* QPOASES_SUBJECTTO_HPP */ + + +/* + * end of file + */ diff --git a/phonelibs/qpoases/INCLUDE/Types.hpp b/phonelibs/qpoases/INCLUDE/Types.hpp new file mode 100644 index 00000000000000..330f187c1e3d47 --- /dev/null +++ b/phonelibs/qpoases/INCLUDE/Types.hpp @@ -0,0 +1,131 @@ +/* + * This file is part of qpOASES. + * + * qpOASES -- An Implementation of the Online Active Set Strategy. + * Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved. + * + * qpOASES is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * qpOASES is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with qpOASES; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + + +/** + * \file INCLUDE/Types.hpp + * \author Hans Joachim Ferreau + * \version 1.3embedded + * \date 2008 + * + * Declaration of all non-built-in types (except for classes). + */ + + +#ifndef QPOASES_TYPES_HPP +#define QPOASES_TYPES_HPP + + + +/** Define real_t for facilitating switching between double and float. */ +// typedef double real_t; + + +/** Summarises all possible logical values. */ +enum BooleanType +{ + BT_FALSE, /**< Logical value for "false". */ + BT_TRUE /**< Logical value for "true". */ +}; + + +/** Summarises all possible print levels. Print levels are used to describe + * the desired amount of output during runtime of qpOASES. */ +enum PrintLevel +{ + PL_NONE, /**< No output. */ + PL_LOW, /**< Print error messages only. */ + PL_MEDIUM, /**< Print error and warning messages as well as concise info messages. */ + PL_HIGH /**< Print all messages with full details. */ +}; + + +/** Defines visibility status of a message. */ +enum VisibilityStatus +{ + VS_VISIBLE, /**< Message visible. */ + VS_HIDDEN /**< Message not visible. */ +}; + + +/** Summarises all possible states of the (S)QProblem(B) object during the +solution process of a QP sequence. */ +enum QProblemStatus +{ + QPS_NOTINITIALISED, /**< QProblem object is freshly instantiated or reset. */ + QPS_PREPARINGAUXILIARYQP, /**< An auxiliary problem is currently setup, either at the very beginning + * via an initial homotopy or after changing the QP matrices. */ + QPS_AUXILIARYQPSOLVED, /**< An auxilary problem was solved, either at the very beginning + * via an initial homotopy or after changing the QP matrices. */ + QPS_PERFORMINGHOMOTOPY, /**< A homotopy according to the main idea of the online active + * set strategy is performed. */ + QPS_HOMOTOPYQPSOLVED, /**< An intermediate QP along the homotopy path was solved. */ + QPS_SOLVED /**< The solution of the actual QP was found. */ +}; + + +/** Summarises all possible types of bounds and constraints. */ +enum SubjectToType +{ + ST_UNBOUNDED, /**< Bound/constraint is unbounded. */ + ST_BOUNDED, /**< Bound/constraint is bounded but not fixed. */ + ST_EQUALITY, /**< Bound/constraint is fixed (implicit equality bound/constraint). */ + ST_UNKNOWN /**< Type of bound/constraint unknown. */ +}; + + +/** Summarises all possible states of bounds and constraints. */ +enum SubjectToStatus +{ + ST_INACTIVE, /**< Bound/constraint is inactive. */ + ST_LOWER, /**< Bound/constraint is at its lower bound. */ + ST_UPPER, /**< Bound/constraint is at its upper bound. */ + ST_UNDEFINED /**< Status of bound/constraint undefined. */ +}; + + +/** Summarises all possible cycling states of bounds and constraints. */ +enum CyclingStatus +{ + CYC_NOT_INVOLVED, /**< Bound/constraint is not involved in current cycling. */ + CYC_PREV_ADDED, /**< Bound/constraint has previously been added during the current cycling. */ + CYC_PREV_REMOVED /**< Bound/constraint has previously been removed during the current cycling. */ +}; + + +/** Summarises all possible types of the QP's Hessian matrix. */ +enum HessianType +{ + HST_SEMIDEF, /**< Hessian is positive semi-definite. */ + HST_POSDEF_NULLSPACE, /**< Hessian is positive definite on null space of active bounds/constraints. */ + HST_POSDEF, /**< Hessian is (strictly) positive definite. */ + HST_IDENTITY /**< Hessian is identity matrix. */ +}; + + + +#endif /* QPOASES_TYPES_HPP */ + + +/* + * end of file + */ diff --git a/phonelibs/qpoases/INCLUDE/Utils.hpp b/phonelibs/qpoases/INCLUDE/Utils.hpp new file mode 100644 index 00000000000000..16546a991b1945 --- /dev/null +++ b/phonelibs/qpoases/INCLUDE/Utils.hpp @@ -0,0 +1,197 @@ +/* + * This file is part of qpOASES. + * + * qpOASES -- An Implementation of the Online Active Set Strategy. + * Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved. + * + * qpOASES is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * qpOASES is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with qpOASES; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + + +/** + * \file INCLUDE/Utils.hpp + * \author Hans Joachim Ferreau + * \version 1.3embedded + * \date 2007-2008 + * + * Declaration of global utility functions for working with qpOASES. + */ + + +#ifndef QPOASES_UTILS_HPP +#define QPOASES_UTILS_HPP + + +#include + + +#ifdef PC_DEBUG /* Define print functions only for debugging! */ + +/** Prints a vector. + * \return SUCCESSFUL_RETURN */ +returnValue print( const real_t* const v, /**< Vector to be printed. */ + int n /**< Length of vector. */ + ); + +/** Prints a permuted vector. + * \return SUCCESSFUL_RETURN */ +returnValue print( const real_t* const v, /**< Vector to be printed. */ + int n, /**< Length of vector. */ + const int* const V_idx /**< Pemutation vector. */ + ); + +/** Prints a named vector. + * \return SUCCESSFUL_RETURN */ +returnValue print( const real_t* const v, /**< Vector to be printed. */ + int n, /**< Length of vector. */ + const char* name /** Name of vector. */ + ); + +/** Prints a matrix. + * \return SUCCESSFUL_RETURN */ +returnValue print( const real_t* const M, /**< Matrix to be printed. */ + int nrow, /**< Row number of matrix. */ + int ncol /**< Column number of matrix. */ + ); + +/** Prints a permuted matrix. + * \return SUCCESSFUL_RETURN */ +returnValue print( const real_t* const M, /**< Matrix to be printed. */ + int nrow, /**< Row number of matrix. */ + int ncol , /**< Column number of matrix. */ + const int* const ROW_idx, /**< Row pemutation vector. */ + const int* const COL_idx /**< Column pemutation vector. */ + ); + +/** Prints a named matrix. + * \return SUCCESSFUL_RETURN */ +returnValue print( const real_t* const M, /**< Matrix to be printed. */ + int nrow, /**< Row number of matrix. */ + int ncol, /**< Column number of matrix. */ + const char* name /** Name of matrix. */ + ); + +/** Prints an index array. + * \return SUCCESSFUL_RETURN */ +returnValue print( const int* const index, /**< Index array to be printed. */ + int n /**< Length of index array. */ + ); + +/** Prints a named index array. + * \return SUCCESSFUL_RETURN */ +returnValue print( const int* const index, /**< Index array to be printed. */ + int n, /**< Length of index array. */ + const char* name /**< Name of index array. */ + ); + + +/** Prints a string to desired output target (useful also for MATLAB output!). + * \return SUCCESSFUL_RETURN */ +returnValue myPrintf( const char* s /**< String to be written. */ + ); + + +/** Prints qpOASES copyright notice. + * \return SUCCESSFUL_RETURN */ +returnValue printCopyrightNotice( ); + + +/** Reads a real_t matrix from file. + * \return SUCCESSFUL_RETURN \n + RET_UNABLE_TO_OPEN_FILE \n + RET_UNABLE_TO_READ_FILE */ +returnValue readFromFile( real_t* data, /**< Matrix to be read from file. */ + int nrow, /**< Row number of matrix. */ + int ncol, /**< Column number of matrix. */ + const char* datafilename /**< Data file name. */ + ); + +/** Reads a real_t vector from file. + * \return SUCCESSFUL_RETURN \n + RET_UNABLE_TO_OPEN_FILE \n + RET_UNABLE_TO_READ_FILE */ +returnValue readFromFile( real_t* data, /**< Vector to be read from file. */ + int n, /**< Length of vector. */ + const char* datafilename /**< Data file name. */ + ); + +/** Reads an integer (column) vector from file. + * \return SUCCESSFUL_RETURN \n + RET_UNABLE_TO_OPEN_FILE \n + RET_UNABLE_TO_READ_FILE */ +returnValue readFromFile( int* data, /**< Vector to be read from file. */ + int n, /**< Length of vector. */ + const char* datafilename /**< Data file name. */ + ); + + +/** Writes a real_t matrix into a file. + * \return SUCCESSFUL_RETURN \n + RET_UNABLE_TO_OPEN_FILE */ +returnValue writeIntoFile( const real_t* const data, /**< Matrix to be written into file. */ + int nrow, /**< Row number of matrix. */ + int ncol, /**< Column number of matrix. */ + const char* datafilename, /**< Data file name. */ + BooleanType append /**< Indicates if data shall be appended if the file already exists (otherwise it is overwritten). */ + ); + +/** Writes a real_t vector into a file. + * \return SUCCESSFUL_RETURN \n + RET_UNABLE_TO_OPEN_FILE */ +returnValue writeIntoFile( const real_t* const data, /**< Vector to be written into file. */ + int n, /**< Length of vector. */ + const char* datafilename, /**< Data file name. */ + BooleanType append /**< Indicates if data shall be appended if the file already exists (otherwise it is overwritten). */ + ); + +/** Writes an integer (column) vector into a file. + * \return SUCCESSFUL_RETURN \n + RET_UNABLE_TO_OPEN_FILE */ +returnValue writeIntoFile( const int* const integer, /**< Integer vector to be written into file. */ + int n, /**< Length of vector. */ + const char* datafilename, /**< Data file name. */ + BooleanType append /**< Indicates if integer shall be appended if the file already exists (otherwise it is overwritten). */ + ); + +#endif /* PC_DEBUG */ + + +/** Returns the current system time. + * \return current system time */ +real_t getCPUtime( ); + + +/** Returns the Euclidean norm of a vector. + * \return 0: successful */ +real_t getNorm( const real_t* const v, /**< Vector. */ + int n /**< Vector's dimension. */ + ); + +/** Returns the absolute value of a real_t. + * \return Absolute value of a real_t */ +inline real_t getAbs( real_t x /**< Input argument. */ + ); + + + +#include + +#endif /* QPOASES_UTILS_HPP */ + + +/* + * end of file + */ diff --git a/phonelibs/qpoases/LICENSE.txt b/phonelibs/qpoases/LICENSE.txt new file mode 100644 index 00000000000000..5faba9d48ce750 --- /dev/null +++ b/phonelibs/qpoases/LICENSE.txt @@ -0,0 +1,504 @@ + GNU LESSER GENERAL PUBLIC LICENSE + Version 2.1, February 1999 + + Copyright (C) 1991, 1999 Free Software Foundation, Inc. + 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + Everyone is permitted to copy and distribute verbatim copies + of this license document, but changing it is not allowed. + +[This is the first released version of the Lesser GPL. It also counts + as the successor of the GNU Library Public License, version 2, hence + the version number 2.1.] + + Preamble + + The licenses for most software are designed to take away your +freedom to share and change it. By contrast, the GNU General Public +Licenses are intended to guarantee your freedom to share and change +free software--to make sure the software is free for all its users. + + This license, the Lesser General Public License, applies to some +specially designated software packages--typically libraries--of the +Free Software Foundation and other authors who decide to use it. You +can use it too, but we suggest you first think carefully about whether +this license or the ordinary General Public License is the better +strategy to use in any particular case, based on the explanations below. + + When we speak of free software, we are referring to freedom of use, +not price. Our General Public Licenses are designed to make sure that +you have the freedom to distribute copies of free software (and charge +for this service if you wish); that you receive source code or can get +it if you want it; that you can change the software and use pieces of +it in new free programs; and that you are informed that you can do +these things. + + To protect your rights, we need to make restrictions that forbid +distributors to deny you these rights or to ask you to surrender these +rights. These restrictions translate to certain responsibilities for +you if you distribute copies of the library or if you modify it. + + For example, if you distribute copies of the library, whether gratis +or for a fee, you must give the recipients all the rights that we gave +you. You must make sure that they, too, receive or can get the source +code. If you link other code with the library, you must provide +complete object files to the recipients, so that they can relink them +with the library after making changes to the library and recompiling +it. And you must show them these terms so they know their rights. + + We protect your rights with a two-step method: (1) we copyright the +library, and (2) we offer you this license, which gives you legal +permission to copy, distribute and/or modify the library. + + To protect each distributor, we want to make it very clear that +there is no warranty for the free library. Also, if the library is +modified by someone else and passed on, the recipients should know +that what they have is not the original version, so that the original +author's reputation will not be affected by problems that might be +introduced by others. + + Finally, software patents pose a constant threat to the existence of +any free program. We wish to make sure that a company cannot +effectively restrict the users of a free program by obtaining a +restrictive license from a patent holder. Therefore, we insist that +any patent license obtained for a version of the library must be +consistent with the full freedom of use specified in this license. + + Most GNU software, including some libraries, is covered by the +ordinary GNU General Public License. This license, the GNU Lesser +General Public License, applies to certain designated libraries, and +is quite different from the ordinary General Public License. We use +this license for certain libraries in order to permit linking those +libraries into non-free programs. + + When a program is linked with a library, whether statically or using +a shared library, the combination of the two is legally speaking a +combined work, a derivative of the original library. The ordinary +General Public License therefore permits such linking only if the +entire combination fits its criteria of freedom. The Lesser General +Public License permits more lax criteria for linking other code with +the library. + + We call this license the "Lesser" General Public License because it +does Less to protect the user's freedom than the ordinary General +Public License. It also provides other free software developers Less +of an advantage over competing non-free programs. These disadvantages +are the reason we use the ordinary General Public License for many +libraries. However, the Lesser license provides advantages in certain +special circumstances. + + For example, on rare occasions, there may be a special need to +encourage the widest possible use of a certain library, so that it becomes +a de-facto standard. To achieve this, non-free programs must be +allowed to use the library. A more frequent case is that a free +library does the same job as widely used non-free libraries. In this +case, there is little to gain by limiting the free library to free +software only, so we use the Lesser General Public License. + + In other cases, permission to use a particular library in non-free +programs enables a greater number of people to use a large body of +free software. For example, permission to use the GNU C Library in +non-free programs enables many more people to use the whole GNU +operating system, as well as its variant, the GNU/Linux operating +system. + + Although the Lesser General Public License is Less protective of the +users' freedom, it does ensure that the user of a program that is +linked with the Library has the freedom and the wherewithal to run +that program using a modified version of the Library. + + The precise terms and conditions for copying, distribution and +modification follow. Pay close attention to the difference between a +"work based on the library" and a "work that uses the library". The +former contains code derived from the library, whereas the latter must +be combined with the library in order to run. + + GNU LESSER GENERAL PUBLIC LICENSE + TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION + + 0. This License Agreement applies to any software library or other +program which contains a notice placed by the copyright holder or +other authorized party saying it may be distributed under the terms of +this Lesser General Public License (also called "this License"). +Each licensee is addressed as "you". + + A "library" means a collection of software functions and/or data +prepared so as to be conveniently linked with application programs +(which use some of those functions and data) to form executables. + + The "Library", below, refers to any such software library or work +which has been distributed under these terms. A "work based on the +Library" means either the Library or any derivative work under +copyright law: that is to say, a work containing the Library or a +portion of it, either verbatim or with modifications and/or translated +straightforwardly into another language. (Hereinafter, translation is +included without limitation in the term "modification".) + + "Source code" for a work means the preferred form of the work for +making modifications to it. For a library, complete source code means +all the source code for all modules it contains, plus any associated +interface definition files, plus the scripts used to control compilation +and installation of the library. + + Activities other than copying, distribution and modification are not +covered by this License; they are outside its scope. The act of +running a program using the Library is not restricted, and output from +such a program is covered only if its contents constitute a work based +on the Library (independent of the use of the Library in a tool for +writing it). Whether that is true depends on what the Library does +and what the program that uses the Library does. + + 1. You may copy and distribute verbatim copies of the Library's +complete source code as you receive it, in any medium, provided that +you conspicuously and appropriately publish on each copy an +appropriate copyright notice and disclaimer of warranty; keep intact +all the notices that refer to this License and to the absence of any +warranty; and distribute a copy of this License along with the +Library. + + You may charge a fee for the physical act of transferring a copy, +and you may at your option offer warranty protection in exchange for a +fee. + + 2. You may modify your copy or copies of the Library or any portion +of it, thus forming a work based on the Library, and copy and +distribute such modifications or work under the terms of Section 1 +above, provided that you also meet all of these conditions: + + a) The modified work must itself be a software library. + + b) You must cause the files modified to carry prominent notices + stating that you changed the files and the date of any change. + + c) You must cause the whole of the work to be licensed at no + charge to all third parties under the terms of this License. + + d) If a facility in the modified Library refers to a function or a + table of data to be supplied by an application program that uses + the facility, other than as an argument passed when the facility + is invoked, then you must make a good faith effort to ensure that, + in the event an application does not supply such function or + table, the facility still operates, and performs whatever part of + its purpose remains meaningful. + + (For example, a function in a library to compute square roots has + a purpose that is entirely well-defined independent of the + application. Therefore, Subsection 2d requires that any + application-supplied function or table used by this function must + be optional: if the application does not supply it, the square + root function must still compute square roots.) + +These requirements apply to the modified work as a whole. If +identifiable sections of that work are not derived from the Library, +and can be reasonably considered independent and separate works in +themselves, then this License, and its terms, do not apply to those +sections when you distribute them as separate works. But when you +distribute the same sections as part of a whole which is a work based +on the Library, the distribution of the whole must be on the terms of +this License, whose permissions for other licensees extend to the +entire whole, and thus to each and every part regardless of who wrote +it. + +Thus, it is not the intent of this section to claim rights or contest +your rights to work written entirely by you; rather, the intent is to +exercise the right to control the distribution of derivative or +collective works based on the Library. + +In addition, mere aggregation of another work not based on the Library +with the Library (or with a work based on the Library) on a volume of +a storage or distribution medium does not bring the other work under +the scope of this License. + + 3. You may opt to apply the terms of the ordinary GNU General Public +License instead of this License to a given copy of the Library. To do +this, you must alter all the notices that refer to this License, so +that they refer to the ordinary GNU General Public License, version 2, +instead of to this License. (If a newer version than version 2 of the +ordinary GNU General Public License has appeared, then you can specify +that version instead if you wish.) Do not make any other change in +these notices. + + Once this change is made in a given copy, it is irreversible for +that copy, so the ordinary GNU General Public License applies to all +subsequent copies and derivative works made from that copy. + + This option is useful when you wish to copy part of the code of +the Library into a program that is not a library. + + 4. You may copy and distribute the Library (or a portion or +derivative of it, under Section 2) in object code or executable form +under the terms of Sections 1 and 2 above provided that you accompany +it with the complete corresponding machine-readable source code, which +must be distributed under the terms of Sections 1 and 2 above on a +medium customarily used for software interchange. + + If distribution of object code is made by offering access to copy +from a designated place, then offering equivalent access to copy the +source code from the same place satisfies the requirement to +distribute the source code, even though third parties are not +compelled to copy the source along with the object code. + + 5. A program that contains no derivative of any portion of the +Library, but is designed to work with the Library by being compiled or +linked with it, is called a "work that uses the Library". Such a +work, in isolation, is not a derivative work of the Library, and +therefore falls outside the scope of this License. + + However, linking a "work that uses the Library" with the Library +creates an executable that is a derivative of the Library (because it +contains portions of the Library), rather than a "work that uses the +library". The executable is therefore covered by this License. +Section 6 states terms for distribution of such executables. + + When a "work that uses the Library" uses material from a header file +that is part of the Library, the object code for the work may be a +derivative work of the Library even though the source code is not. +Whether this is true is especially significant if the work can be +linked without the Library, or if the work is itself a library. The +threshold for this to be true is not precisely defined by law. + + If such an object file uses only numerical parameters, data +structure layouts and accessors, and small macros and small inline +functions (ten lines or less in length), then the use of the object +file is unrestricted, regardless of whether it is legally a derivative +work. (Executables containing this object code plus portions of the +Library will still fall under Section 6.) + + Otherwise, if the work is a derivative of the Library, you may +distribute the object code for the work under the terms of Section 6. +Any executables containing that work also fall under Section 6, +whether or not they are linked directly with the Library itself. + + 6. As an exception to the Sections above, you may also combine or +link a "work that uses the Library" with the Library to produce a +work containing portions of the Library, and distribute that work +under terms of your choice, provided that the terms permit +modification of the work for the customer's own use and reverse +engineering for debugging such modifications. + + You must give prominent notice with each copy of the work that the +Library is used in it and that the Library and its use are covered by +this License. You must supply a copy of this License. If the work +during execution displays copyright notices, you must include the +copyright notice for the Library among them, as well as a reference +directing the user to the copy of this License. Also, you must do one +of these things: + + a) Accompany the work with the complete corresponding + machine-readable source code for the Library including whatever + changes were used in the work (which must be distributed under + Sections 1 and 2 above); and, if the work is an executable linked + with the Library, with the complete machine-readable "work that + uses the Library", as object code and/or source code, so that the + user can modify the Library and then relink to produce a modified + executable containing the modified Library. (It is understood + that the user who changes the contents of definitions files in the + Library will not necessarily be able to recompile the application + to use the modified definitions.) + + b) Use a suitable shared library mechanism for linking with the + Library. A suitable mechanism is one that (1) uses at run time a + copy of the library already present on the user's computer system, + rather than copying library functions into the executable, and (2) + will operate properly with a modified version of the library, if + the user installs one, as long as the modified version is + interface-compatible with the version that the work was made with. + + c) Accompany the work with a written offer, valid for at + least three years, to give the same user the materials + specified in Subsection 6a, above, for a charge no more + than the cost of performing this distribution. + + d) If distribution of the work is made by offering access to copy + from a designated place, offer equivalent access to copy the above + specified materials from the same place. + + e) Verify that the user has already received a copy of these + materials or that you have already sent this user a copy. + + For an executable, the required form of the "work that uses the +Library" must include any data and utility programs needed for +reproducing the executable from it. However, as a special exception, +the materials to be distributed need not include anything that is +normally distributed (in either source or binary form) with the major +components (compiler, kernel, and so on) of the operating system on +which the executable runs, unless that component itself accompanies +the executable. + + It may happen that this requirement contradicts the license +restrictions of other proprietary libraries that do not normally +accompany the operating system. Such a contradiction means you cannot +use both them and the Library together in an executable that you +distribute. + + 7. You may place library facilities that are a work based on the +Library side-by-side in a single library together with other library +facilities not covered by this License, and distribute such a combined +library, provided that the separate distribution of the work based on +the Library and of the other library facilities is otherwise +permitted, and provided that you do these two things: + + a) Accompany the combined library with a copy of the same work + based on the Library, uncombined with any other library + facilities. This must be distributed under the terms of the + Sections above. + + b) Give prominent notice with the combined library of the fact + that part of it is a work based on the Library, and explaining + where to find the accompanying uncombined form of the same work. + + 8. You may not copy, modify, sublicense, link with, or distribute +the Library except as expressly provided under this License. Any +attempt otherwise to copy, modify, sublicense, link with, or +distribute the Library is void, and will automatically terminate your +rights under this License. However, parties who have received copies, +or rights, from you under this License will not have their licenses +terminated so long as such parties remain in full compliance. + + 9. You are not required to accept this License, since you have not +signed it. However, nothing else grants you permission to modify or +distribute the Library or its derivative works. These actions are +prohibited by law if you do not accept this License. Therefore, by +modifying or distributing the Library (or any work based on the +Library), you indicate your acceptance of this License to do so, and +all its terms and conditions for copying, distributing or modifying +the Library or works based on it. + + 10. Each time you redistribute the Library (or any work based on the +Library), the recipient automatically receives a license from the +original licensor to copy, distribute, link with or modify the Library +subject to these terms and conditions. You may not impose any further +restrictions on the recipients' exercise of the rights granted herein. +You are not responsible for enforcing compliance by third parties with +this License. + + 11. If, as a consequence of a court judgment or allegation of patent +infringement or for any other reason (not limited to patent issues), +conditions are imposed on you (whether by court order, agreement or +otherwise) that contradict the conditions of this License, they do not +excuse you from the conditions of this License. If you cannot +distribute so as to satisfy simultaneously your obligations under this +License and any other pertinent obligations, then as a consequence you +may not distribute the Library at all. For example, if a patent +license would not permit royalty-free redistribution of the Library by +all those who receive copies directly or indirectly through you, then +the only way you could satisfy both it and this License would be to +refrain entirely from distribution of the Library. + +If any portion of this section is held invalid or unenforceable under any +particular circumstance, the balance of the section is intended to apply, +and the section as a whole is intended to apply in other circumstances. + +It is not the purpose of this section to induce you to infringe any +patents or other property right claims or to contest validity of any +such claims; this section has the sole purpose of protecting the +integrity of the free software distribution system which is +implemented by public license practices. Many people have made +generous contributions to the wide range of software distributed +through that system in reliance on consistent application of that +system; it is up to the author/donor to decide if he or she is willing +to distribute software through any other system and a licensee cannot +impose that choice. + +This section is intended to make thoroughly clear what is believed to +be a consequence of the rest of this License. + + 12. If the distribution and/or use of the Library is restricted in +certain countries either by patents or by copyrighted interfaces, the +original copyright holder who places the Library under this License may add +an explicit geographical distribution limitation excluding those countries, +so that distribution is permitted only in or among countries not thus +excluded. In such case, this License incorporates the limitation as if +written in the body of this License. + + 13. The Free Software Foundation may publish revised and/or new +versions of the Lesser General Public License from time to time. +Such new versions will be similar in spirit to the present version, +but may differ in detail to address new problems or concerns. + +Each version is given a distinguishing version number. If the Library +specifies a version number of this License which applies to it and +"any later version", you have the option of following the terms and +conditions either of that version or of any later version published by +the Free Software Foundation. If the Library does not specify a +license version number, you may choose any version ever published by +the Free Software Foundation. + + 14. If you wish to incorporate parts of the Library into other free +programs whose distribution conditions are incompatible with these, +write to the author to ask for permission. For software which is +copyrighted by the Free Software Foundation, write to the Free +Software Foundation; we sometimes make exceptions for this. Our +decision will be guided by the two goals of preserving the free status +of all derivatives of our free software and of promoting the sharing +and reuse of software generally. + + NO WARRANTY + + 15. BECAUSE THE LIBRARY IS LICENSED FREE OF CHARGE, THERE IS NO +WARRANTY FOR THE LIBRARY, TO THE EXTENT PERMITTED BY APPLICABLE LAW. +EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR +OTHER PARTIES PROVIDE THE LIBRARY "AS IS" WITHOUT WARRANTY OF ANY +KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE +LIBRARY IS WITH YOU. SHOULD THE LIBRARY PROVE DEFECTIVE, YOU ASSUME +THE COST OF ALL NECESSARY SERVICING, REPAIR OR CORRECTION. + + 16. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN +WRITING WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY +AND/OR REDISTRIBUTE THE LIBRARY AS PERMITTED ABOVE, BE LIABLE TO YOU +FOR DAMAGES, INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR +CONSEQUENTIAL DAMAGES ARISING OUT OF THE USE OR INABILITY TO USE THE +LIBRARY (INCLUDING BUT NOT LIMITED TO LOSS OF DATA OR DATA BEING +RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD PARTIES OR A +FAILURE OF THE LIBRARY TO OPERATE WITH ANY OTHER SOFTWARE), EVEN IF +SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH +DAMAGES. + + END OF TERMS AND CONDITIONS + + How to Apply These Terms to Your New Libraries + + If you develop a new library, and you want it to be of the greatest +possible use to the public, we recommend making it free software that +everyone can redistribute and change. You can do so by permitting +redistribution under these terms (or, alternatively, under the terms of the +ordinary General Public License). + + To apply these terms, attach the following notices to the library. It is +safest to attach them to the start of each source file to most effectively +convey the exclusion of warranty; and each file should have at least the +"copyright" line and a pointer to where the full notice is found. + + + Copyright (C) + + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + + This library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with this library; if not, write to the Free Software + Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + +Also add information on how to contact you by electronic and paper mail. + +You should also get your employer (if you work as a programmer) or your +school, if any, to sign a "copyright disclaimer" for the library, if +necessary. Here is a sample; alter the names: + + Yoyodyne, Inc., hereby disclaims all copyright interest in the + library `Frob' (a library for tweaking knobs) written by James Random Hacker. + + , 1 April 1990 + Ty Coon, President of Vice + +That's all there is to it! + + diff --git a/phonelibs/qpoases/README.txt b/phonelibs/qpoases/README.txt new file mode 100644 index 00000000000000..37175d9192bdbe --- /dev/null +++ b/phonelibs/qpoases/README.txt @@ -0,0 +1,92 @@ +## +## qpOASES -- An Implementation of the Online Active Set Strategy. +## Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved. +## +## qpOASES is free software; you can redistribute it and/or +## modify it under the terms of the GNU Lesser General Public +## License as published by the Free Software Foundation; either +## version 2.1 of the License, or (at your option) any later version. +## +## qpOASES is distributed in the hope that it will be useful, +## but WITHOUT ANY WARRANTY; without even the implied warranty of +## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +## Lesser General Public License for more details. +## +## You should have received a copy of the GNU Lesser General Public +## License along with qpOASES; if not, write to the Free Software +## Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA +## + + + +INTRODUCTION +============= + +qpOASES is an open-source C++ implementation of the recently proposed +online active set strategy (see [1], [2]), which was inspired by important +observations from the field of parametric quadratic programming. It has +several theoretical features that make it particularly suited for model +predictive control (MPC) applications. + +The software package qpOASES implements these ideas and has already been +successfully used for closed-loop control of a real-world Diesel engine [3]. + + +References: + +[1] H.J. Ferreau. An Online Active Set Strategy for Fast Solution of +Parametric Quadratic Programs with Applications to Predictive Engine Control. +Diplom thesis, University of Heidelberg, 2006. + +[2] H.J. Ferreau, H.G. Bock, M. Diehl. An online active set strategy to +overcome the limitations of explicit MPC. International Journal of Robust +and Nonlinear Control, 18 (8), pp. 816-830, 2008. + +[3] H.J. Ferreau, P. Ortner, P. Langthaler, L. del Re, M. Diehl. Predictive +Control of a Real-World Diesel Engine using an Extended Online Active Set +Strategy. Annual Reviews in Control, 31 (2), pp. 293-301, 2007. + + + +GETTING STARTED +================ + +1. For installation, usage and additional information on this software package + see the qpOASES User's Manual located at ./DOC/manual.pdf! + + +2. The file ./LICENSE.txt contains a copy of the GNU Lesser General Public + License. Please read it carefully before using qpOASES! + + +3. The whole software package can be downloaded from + + http://homes.esat.kuleuven.be/~optec/software/qpOASES/ + + On this webpage you will also find a list of frequently asked questions. + + + +CONTACT THE AUTHORS +==================== + +If you have got questions, remarks or comments on qpOASES +please contact the main author: + + Hans Joachim Ferreau + Katholieke Universiteit Leuven + Department of Electrical Engineering (ESAT) + Kasteelpark Arenberg 10, bus 2446 + B-3001 Leuven-Heverlee, Belgium + + Phone: +32 16 32 03 63 + E-mail: joachim.ferreau@esat.kuleuven.be + qpOASES@esat.kuleuven.be + +Also bug reports and source code extensions are most welcome! + + + +## +## end of file +## diff --git a/phonelibs/qpoases/SRC/Bounds.cpp b/phonelibs/qpoases/SRC/Bounds.cpp new file mode 100644 index 00000000000000..236ab2f62d284b --- /dev/null +++ b/phonelibs/qpoases/SRC/Bounds.cpp @@ -0,0 +1,252 @@ +/* + * This file is part of qpOASES. + * + * qpOASES -- An Implementation of the Online Active Set Strategy. + * Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved. + * + * qpOASES is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * qpOASES is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with qpOASES; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + + +/** + * \file SRC/Bounds.cpp + * \author Hans Joachim Ferreau + * \version 1.3embedded + * \date 2007-2008 + * + * Implementation of the Bounds class designed to manage working sets of + * bounds within a QProblem. + */ + + +#include + + + +/***************************************************************************** + * P U B L I C * + *****************************************************************************/ + + +/* + * B o u n d s + */ +Bounds::Bounds( ) : SubjectTo( ), + nV( 0 ), + nFV( 0 ), + nBV( 0 ), + nUV( 0 ) +{ +} + + +/* + * B o u n d s + */ +Bounds::Bounds( const Bounds& rhs ) : SubjectTo( rhs ), + nV( rhs.nV ), + nFV( rhs.nFV ), + nBV( rhs.nBV ), + nUV( rhs.nUV ) +{ + free = rhs.free; + fixed = rhs.fixed; +} + + +/* + * ~ B o u n d s + */ +Bounds::~Bounds( ) +{ +} + + +/* + * o p e r a t o r = + */ +Bounds& Bounds::operator=( const Bounds& rhs ) +{ + if ( this != &rhs ) + { + SubjectTo::operator=( rhs ); + + nV = rhs.nV; + nFV = rhs.nFV; + nBV = rhs.nBV; + nUV = rhs.nUV; + + free = rhs.free; + fixed = rhs.fixed; + } + + return *this; +} + + +/* + * i n i t + */ +returnValue Bounds::init( int n ) +{ + nV = n; + nFV = 0; + nBV = 0; + nUV = 0; + + free.init( ); + fixed.init( ); + + return SubjectTo::init( n ); +} + + +/* + * s e t u p B o u n d + */ +returnValue Bounds::setupBound( int _number, SubjectToStatus _status + ) +{ + /* consistency check */ + if ( ( _number < 0 ) || ( _number >= getNV( ) ) ) + return THROWERROR( RET_INDEX_OUT_OF_BOUNDS ); + + /* Add bound index to respective index list. */ + switch ( _status ) + { + case ST_INACTIVE: + if ( this->addIndex( this->getFree( ),_number,_status ) != SUCCESSFUL_RETURN ) + return THROWERROR( RET_SETUP_BOUND_FAILED ); + break; + + case ST_LOWER: + if ( this->addIndex( this->getFixed( ),_number,_status ) != SUCCESSFUL_RETURN ) + return THROWERROR( RET_SETUP_BOUND_FAILED ); + break; + + case ST_UPPER: + if ( this->addIndex( this->getFixed( ),_number,_status ) != SUCCESSFUL_RETURN ) + return THROWERROR( RET_SETUP_BOUND_FAILED ); + break; + + default: + return THROWERROR( RET_INVALID_ARGUMENTS ); + } + + return SUCCESSFUL_RETURN; +} + + +/* + * s e t u p A l l F r e e + */ +returnValue Bounds::setupAllFree( ) +{ + int i; + + /* 1) Place unbounded variables at the beginning of the index list of free variables. */ + for( i=0; i= getNV( ) ) ) + return THROWERROR( RET_INDEX_OUT_OF_BOUNDS ); + + /* Move index from indexlist of fixed variables to that of free ones. */ + if ( this->removeIndex( this->getFixed( ),_number ) != SUCCESSFUL_RETURN ) + return THROWERROR( RET_MOVING_BOUND_FAILED ); + + if ( this->addIndex( this->getFree( ),_number,ST_INACTIVE ) != SUCCESSFUL_RETURN ) + return THROWERROR( RET_MOVING_BOUND_FAILED ); + + return SUCCESSFUL_RETURN; +} + + +/* + * m o v e F r e e T o F i x e d + */ +returnValue Bounds::moveFreeToFixed( int _number, SubjectToStatus _status + ) +{ + /* consistency check */ + if ( ( _number < 0 ) || ( _number >= getNV( ) ) ) + return THROWERROR( RET_INDEX_OUT_OF_BOUNDS ); + + /* Move index from indexlist of free variables to that of fixed ones. */ + if ( this->removeIndex( this->getFree( ),_number ) != SUCCESSFUL_RETURN ) + return THROWERROR( RET_MOVING_BOUND_FAILED ); + + if ( this->addIndex( this->getFixed( ),_number,_status ) != SUCCESSFUL_RETURN ) + return THROWERROR( RET_MOVING_BOUND_FAILED ); + + return SUCCESSFUL_RETURN; +} + + +/* + * s w a p F r e e + */ +returnValue Bounds::swapFree( int number1, int number2 + ) +{ + /* consistency check */ + if ( ( number1 < 0 ) || ( number1 >= getNV( ) ) || ( number2 < 0 ) || ( number2 >= getNV( ) ) ) + return THROWERROR( RET_INDEX_OUT_OF_BOUNDS ); + + /* Swap index within indexlist of free variables. */ + return this->swapIndex( this->getFree( ),number1,number2 ); +} + + +/* + * end of file + */ diff --git a/phonelibs/qpoases/SRC/Bounds.ipp b/phonelibs/qpoases/SRC/Bounds.ipp new file mode 100644 index 00000000000000..d0e7dbbf0904f7 --- /dev/null +++ b/phonelibs/qpoases/SRC/Bounds.ipp @@ -0,0 +1,144 @@ +/* + * This file is part of qpOASES. + * + * qpOASES -- An Implementation of the Online Active Set Strategy. + * Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved. + * + * qpOASES is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * qpOASES is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with qpOASES; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + + +/** + * \file SRC/Bounds.ipp + * \author Hans Joachim Ferreau + * \version 1.3embedded + * \date 2007-2008 + * + * Implementation of inlined member functions of the Bounds class designed + * to manage working sets of bounds within a QProblem. + */ + + +/***************************************************************************** + * P U B L I C * + *****************************************************************************/ + +/* + * g e t N V + */ +inline int Bounds::getNV( ) const +{ + return nV; +} + + +/* + * g e t N F X + */ +inline int Bounds::getNFV( ) const +{ + return nFV; +} + + +/* + * g e t N B V + */ +inline int Bounds::getNBV( ) const +{ + return nBV; +} + + +/* + * g e t N U V + */ +inline int Bounds::getNUV( ) const +{ + return nUV; +} + + + +/* + * s e t N F X + */ +inline returnValue Bounds::setNFV( int n ) +{ + nFV = n; + return SUCCESSFUL_RETURN; +} + + +/* + * s e t N B V + */ +inline returnValue Bounds::setNBV( int n ) +{ + nBV = n; + return SUCCESSFUL_RETURN; +} + + +/* + * s e t N U V + */ +inline returnValue Bounds::setNUV( int n ) +{ + nUV = n; + return SUCCESSFUL_RETURN; +} + + +/* + * g e t N F R + */ +inline int Bounds::getNFR( ) +{ + return free.getLength( ); +} + + +/* + * g e t N F X + */ +inline int Bounds::getNFX( ) +{ + return fixed.getLength( ); +} + + +/* + * g e t F r e e + */ +inline Indexlist* Bounds::getFree( ) +{ + return &free; +} + + +/* + * g e t F i x e d + */ +inline Indexlist* Bounds::getFixed( ) +{ + return &fixed; +} + + +/* + * end of file + */ diff --git a/phonelibs/qpoases/SRC/Constraints.cpp b/phonelibs/qpoases/SRC/Constraints.cpp new file mode 100644 index 00000000000000..81fc6ce8679d23 --- /dev/null +++ b/phonelibs/qpoases/SRC/Constraints.cpp @@ -0,0 +1,248 @@ +/* + * This file is part of qpOASES. + * + * qpOASES -- An Implementation of the Online Active Set Strategy. + * Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved. + * + * qpOASES is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * qpOASES is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with qpOASES; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + + +/** + * \file SRC/Constraints.cpp + * \author Hans Joachim Ferreau + * \version 1.3embedded + * \date 2007-2008 + * + * Implementation of the Constraints class designed to manage working sets of + * constraints within a QProblem. + */ + + +#include + + +/***************************************************************************** + * P U B L I C * + *****************************************************************************/ + + +/* + * C o n s t r a i n t s + */ +Constraints::Constraints( ) : SubjectTo( ), + nC( 0 ), + nEC( 0 ), + nIC( 0 ), + nUC( 0 ) +{ +} + + +/* + * C o n s t r a i n t s + */ +Constraints::Constraints( const Constraints& rhs ) : SubjectTo( rhs ), + nC( rhs.nC ), + nEC( rhs.nEC ), + nIC( rhs.nIC ), + nUC( rhs.nUC ) +{ + active = rhs.active; + inactive = rhs.inactive; +} + + +/* + * ~ C o n s t r a i n t s + */ +Constraints::~Constraints( ) +{ +} + + +/* + * o p e r a t o r = + */ +Constraints& Constraints::operator=( const Constraints& rhs ) +{ + if ( this != &rhs ) + { + SubjectTo::operator=( rhs ); + + nC = rhs.nC; + nEC = rhs.nEC; + nIC = rhs.nIC; + nUC = rhs.nUC; + + active = rhs.active; + inactive = rhs.inactive; + } + + return *this; +} + + +/* + * i n i t + */ +returnValue Constraints::init( int n ) +{ + nC = n; + nEC = 0; + nIC = 0; + nUC = 0; + + active.init( ); + inactive.init( ); + + return SubjectTo::init( n ); +} + + +/* + * s e t u p C o n s t r a i n t + */ +returnValue Constraints::setupConstraint( int _number, SubjectToStatus _status + ) +{ + /* consistency check */ + if ( ( _number < 0 ) || ( _number >= getNC( ) ) ) + return THROWERROR( RET_INDEX_OUT_OF_BOUNDS ); + + /* Add constraint index to respective index list. */ + switch ( _status ) + { + case ST_INACTIVE: + if ( this->addIndex( this->getInactive( ),_number,_status ) != SUCCESSFUL_RETURN ) + return THROWERROR( RET_SETUP_CONSTRAINT_FAILED ); + break; + + case ST_LOWER: + if ( this->addIndex( this->getActive( ),_number,_status ) != SUCCESSFUL_RETURN ) + return THROWERROR( RET_SETUP_CONSTRAINT_FAILED ); + break; + + case ST_UPPER: + if ( this->addIndex( this->getActive( ),_number,_status ) != SUCCESSFUL_RETURN ) + return THROWERROR( RET_SETUP_CONSTRAINT_FAILED ); + break; + + default: + return THROWERROR( RET_INVALID_ARGUMENTS ); + } + + return SUCCESSFUL_RETURN; +} + + +/* + * s e t u p A l l I n a c t i v e + */ +returnValue Constraints::setupAllInactive( ) +{ + int i; + + + /* 1) Place unbounded constraints at the beginning of the index list of inactive constraints. */ + for( i=0; i= getNC( ) ) ) + return THROWERROR( RET_INDEX_OUT_OF_BOUNDS ); + + /* Move index from indexlist of active constraints to that of inactive ones. */ + if ( this->removeIndex( this->getActive( ),_number ) != SUCCESSFUL_RETURN ) + return THROWERROR( RET_MOVING_BOUND_FAILED ); + + if ( this->addIndex( this->getInactive( ),_number,ST_INACTIVE ) != SUCCESSFUL_RETURN ) + return THROWERROR( RET_MOVING_BOUND_FAILED ); + + return SUCCESSFUL_RETURN; +} + + +/* + * m o v e I n a c t i v e T o A c t i v e + */ +returnValue Constraints::moveInactiveToActive( int _number, SubjectToStatus _status + ) +{ + /* consistency check */ + if ( ( _number < 0 ) || ( _number >= getNC( ) ) ) + return THROWERROR( RET_INDEX_OUT_OF_BOUNDS ); + + /* Move index from indexlist of inactive constraints to that of active ones. */ + if ( this->removeIndex( this->getInactive( ),_number ) != SUCCESSFUL_RETURN ) + return THROWERROR( RET_MOVING_BOUND_FAILED ); + + if ( this->addIndex( this->getActive( ),_number,_status ) != SUCCESSFUL_RETURN ) + return THROWERROR( RET_MOVING_BOUND_FAILED ); + + return SUCCESSFUL_RETURN; +} + + + +/* + * end of file + */ diff --git a/phonelibs/qpoases/SRC/Constraints.ipp b/phonelibs/qpoases/SRC/Constraints.ipp new file mode 100644 index 00000000000000..67add20434a1e4 --- /dev/null +++ b/phonelibs/qpoases/SRC/Constraints.ipp @@ -0,0 +1,144 @@ +/* + * This file is part of qpOASES. + * + * qpOASES -- An Implementation of the Online Active Set Strategy. + * Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved. + * + * qpOASES is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * qpOASES is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with qpOASES; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + + +/** + * \file SRC/Constraints.ipp + * \author Hans Joachim Ferreau + * \version 1.3embedded + * \date 2007-2008 + * + * Declaration of inlined member functions of the Constraints class designed + * to manage working sets of constraints within a QProblem. + */ + + + +/***************************************************************************** + * P U B L I C * + *****************************************************************************/ + +/* + * g e t N C + */ +inline int Constraints::getNC( ) const +{ + return nC; +} + + +/* + * g e t N E C + */ +inline int Constraints::getNEC( ) const +{ + return nEC; +} + + +/* + * g e t N I C + */ +inline int Constraints::getNIC( ) const +{ + return nIC; +} + + +/* + * g e t N U C + */ +inline int Constraints::getNUC( ) const +{ + return nUC; +} + + +/* + * s e t N E C + */ +inline returnValue Constraints::setNEC( int n ) +{ + nEC = n; + return SUCCESSFUL_RETURN; +} + + +/* + * s e t N I C + */ +inline returnValue Constraints::setNIC( int n ) +{ + nIC = n; + return SUCCESSFUL_RETURN; +} + + +/* + * s e t N U C + */ +inline returnValue Constraints::setNUC( int n ) +{ + nUC = n; + return SUCCESSFUL_RETURN; +} + + +/* + * g e t N A C + */ +inline int Constraints::getNAC( ) +{ + return active.getLength( ); +} + + +/* + * g e t N I A C + */ +inline int Constraints::getNIAC( ) +{ + return inactive.getLength( ); +} + + +/* + * g e t A c t i v e + */ +inline Indexlist* Constraints::getActive( ) +{ + return &active; +} + + +/* + * g e t I n a c t i v e + */ +inline Indexlist* Constraints::getInactive( ) +{ + return &inactive; +} + + +/* + * end of file + */ diff --git a/phonelibs/qpoases/SRC/CyclingManager.cpp b/phonelibs/qpoases/SRC/CyclingManager.cpp new file mode 100644 index 00000000000000..7bbccdac228460 --- /dev/null +++ b/phonelibs/qpoases/SRC/CyclingManager.cpp @@ -0,0 +1,188 @@ +/* + * This file is part of qpOASES. + * + * qpOASES -- An Implementation of the Online Active Set Strategy. + * Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved. + * + * qpOASES is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * qpOASES is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with qpOASES; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + + +/** + * \file SRC/CyclingManager.cpp + * \author Hans Joachim Ferreau + * \version 1.3embedded + * \date 2007-2008 + * + * Implementation of the CyclingManager class designed to detect + * and handle possible cycling during QP iterations. + * + */ + + +#include + + +/***************************************************************************** + * P U B L I C * + *****************************************************************************/ + + +/* + * C y c l i n g M a n a g e r + */ +CyclingManager::CyclingManager( ) : nV( 0 ), + nC( 0 ) +{ + cyclingDetected = BT_FALSE; +} + + +/* + * C y c l i n g M a n a g e r + */ +CyclingManager::CyclingManager( const CyclingManager& rhs ) : nV( rhs.nV ), + nC( rhs.nC ), + cyclingDetected( rhs.cyclingDetected ) +{ + int i; + + for( i=0; i= 0 ) && ( number < nV ) ) + { + status[number] = _status; + return SUCCESSFUL_RETURN; + } + else + return THROWERROR( RET_INDEX_OUT_OF_BOUNDS ); + } + else + { + /* Set cycling status of a constraint. */ + if ( ( number >= 0 ) && ( number < nC ) ) + { + status[nV+number] = _status; + return SUCCESSFUL_RETURN; + } + else + return THROWERROR( RET_INDEX_OUT_OF_BOUNDS ); + } +} + + +/* + * g e t C y c l i n g S t a t u s + */ +CyclingStatus CyclingManager::getCyclingStatus( int number, BooleanType isBound ) const +{ + if ( isBound == BT_TRUE ) + { + /* Return cycling status of a bound. */ + if ( ( number >= 0 ) && ( number < nV ) ) + return status[number]; + } + else + { + /* Return cycling status of a constraint. */ + if ( ( number >= 0 ) && ( number < nC ) ) + return status[nV+number]; + } + + return CYC_NOT_INVOLVED; +} + + +/* + * c l e a r C y c l i n g D a t a + */ +returnValue CyclingManager::clearCyclingData( ) +{ + int i; + + /* Reset all status values ... */ + for( i=0; i + +/* + * S o l u t i o n A n a l y s i s + */ +SolutionAnalysis::SolutionAnalysis( ) +{ + +} + +/* + * S o l u t i o n A n a l y s i s + */ +SolutionAnalysis::SolutionAnalysis( const SolutionAnalysis& rhs ) +{ + +} + +/* + * ~ S o l u t i o n A n a l y s i s + */ +SolutionAnalysis::~SolutionAnalysis( ) +{ + +} + +/* + * o p e r a t o r = + */ +SolutionAnalysis& SolutionAnalysis::operator=( const SolutionAnalysis& rhs ) +{ + if ( this != &rhs ) + { + + } + + return *this; +} + +/* + * g e t H e s s i a n I n v e r s e + */ +returnValue SolutionAnalysis::getHessianInverse( QProblem* qp, real_t* hessianInverse ) +{ + returnValue returnvalue; /* the return value */ + BooleanType Delta_bC_isZero = BT_FALSE; /* (just use FALSE here) */ + BooleanType Delta_bB_isZero = BT_FALSE; /* (just use FALSE here) */ + + register int run1, run2, run3; + + register int nFR, nFX; + + /* Ask for the number of free and fixed variables, assumes that active set + * is constant for the covariance evaluation */ + nFR = qp->getNFR( ); + nFX = qp->getNFX( ); + + /* Ask for the corresponding index arrays: */ + if ( qp->bounds.getFree( )->getNumberArray( FR_idx ) != SUCCESSFUL_RETURN ) + return THROWERROR( RET_HOTSTART_FAILED ); + + if ( qp->bounds.getFixed( )->getNumberArray( FX_idx ) != SUCCESSFUL_RETURN ) + return THROWERROR( RET_HOTSTART_FAILED ); + + if ( qp->constraints.getActive( )->getNumberArray( AC_idx ) != SUCCESSFUL_RETURN ) + return THROWERROR( RET_HOTSTART_FAILED ); + + /* Initialization: */ + for( run1 = 0; run1 < NVMAX; run1++ ) + delta_g_cov[ run1 ] = 0.0; + + for( run1 = 0; run1 < NVMAX; run1++ ) + delta_lb_cov[ run1 ] = 0.0; + + for( run1 = 0; run1 < NVMAX; run1++ ) + delta_ub_cov[ run1 ] = 0.0; + + for( run1 = 0; run1 < NCMAX; run1++ ) + delta_lbA_cov[ run1 ] = 0.0; + + for( run1 = 0; run1 < NCMAX; run1++ ) + delta_ubA_cov[ run1 ] = 0.0; + + /* The following loop solves the following: + * + * KKT * x = + * [delta_g_cov', delta_lbA_cov', delta_ubA_cov', delta_lb_cov', delta_ub_cov]' + * + * for the first NVMAX (negative) elementary vectors in order to get + * transposed inverse of the Hessian. Assuming that the Hessian is + * symmetric, the function will return transposed inverse, instead of the + * true inverse. + * + * Note, that we use negative elementary vectors due because internal + * implementation of the function hotstart_determineStepDirection requires + * so. + * + * */ + + for( run3 = 0; run3 < NVMAX; run3++ ) + { + /* Line wise loading of the corresponding (negative) elementary vector: */ + delta_g_cov[ run3 ] = -1.0; + + /* Evaluation of the step: */ + returnvalue = qp->hotstart_determineStepDirection( + FR_idx, FX_idx, AC_idx, + delta_g_cov, delta_lbA_cov, delta_ubA_cov, delta_lb_cov, delta_ub_cov, + Delta_bC_isZero, Delta_bB_isZero, + delta_xFX, delta_xFR, delta_yAC, delta_yFX + ); + if ( returnvalue != SUCCESSFUL_RETURN ) + { + return returnvalue; + } + + /* Line wise storage of the QP reaction: */ + for( run1 = 0; run1 < nFR; run1++ ) + { + run2 = FR_idx[ run1 ]; + + hessianInverse[run3 * NVMAX + run2] = delta_xFR[ run1 ]; + } + + for( run1 = 0; run1 < nFX; run1++ ) + { + run2 = FX_idx[ run1 ]; + + hessianInverse[run3 * NVMAX + run2] = delta_xFX[ run1 ]; + } + + /* Prepare for the next iteration */ + delta_g_cov[ run3 ] = 0.0; + } + + // TODO: Perform the transpose of the inverse of the Hessian matrix + + return SUCCESSFUL_RETURN; +} + +/* + * g e t H e s s i a n I n v e r s e + */ +returnValue SolutionAnalysis::getHessianInverse( QProblemB* qp, real_t* hessianInverse ) +{ + returnValue returnvalue; /* the return value */ + BooleanType Delta_bB_isZero = BT_FALSE; /* (just use FALSE here) */ + + register int run1, run2, run3; + + register int nFR, nFX; + + /* Ask for the number of free and fixed variables, assumes that active set + * is constant for the covariance evaluation */ + nFR = qp->getNFR( ); + nFX = qp->getNFX( ); + + /* Ask for the corresponding index arrays: */ + if ( qp->bounds.getFree( )->getNumberArray( FR_idx ) != SUCCESSFUL_RETURN ) + return THROWERROR( RET_HOTSTART_FAILED ); + + if ( qp->bounds.getFixed( )->getNumberArray( FX_idx ) != SUCCESSFUL_RETURN ) + return THROWERROR( RET_HOTSTART_FAILED ); + + /* Initialization: */ + for( run1 = 0; run1 < NVMAX; run1++ ) + delta_g_cov[ run1 ] = 0.0; + + for( run1 = 0; run1 < NVMAX; run1++ ) + delta_lb_cov[ run1 ] = 0.0; + + for( run1 = 0; run1 < NVMAX; run1++ ) + delta_ub_cov[ run1 ] = 0.0; + + /* The following loop solves the following: + * + * KKT * x = + * [delta_g_cov', delta_lb_cov', delta_ub_cov']' + * + * for the first NVMAX (negative) elementary vectors in order to get + * transposed inverse of the Hessian. Assuming that the Hessian is + * symmetric, the function will return transposed inverse, instead of the + * true inverse. + * + * Note, that we use negative elementary vectors due because internal + * implementation of the function hotstart_determineStepDirection requires + * so. + * + * */ + + for( run3 = 0; run3 < NVMAX; run3++ ) + { + /* Line wise loading of the corresponding (negative) elementary vector: */ + delta_g_cov[ run3 ] = -1.0; + + /* Evaluation of the step: */ + returnvalue = qp->hotstart_determineStepDirection( + FR_idx, FX_idx, + delta_g_cov, delta_lb_cov, delta_ub_cov, + Delta_bB_isZero, + delta_xFX, delta_xFR, delta_yFX + ); + if ( returnvalue != SUCCESSFUL_RETURN ) + { + return returnvalue; + } + + /* Line wise storage of the QP reaction: */ + for( run1 = 0; run1 < nFR; run1++ ) + { + run2 = FR_idx[ run1 ]; + + hessianInverse[run3 * NVMAX + run2] = delta_xFR[ run1 ]; + } + + for( run1 = 0; run1 < nFX; run1++ ) + { + run2 = FX_idx[ run1 ]; + + hessianInverse[run3 * NVMAX + run2] = delta_xFX[ run1 ]; + } + + /* Prepare for the next iteration */ + delta_g_cov[ run3 ] = 0.0; + } + + // TODO: Perform the transpose of the inverse of the Hessian matrix + + return SUCCESSFUL_RETURN; +} + +/* + * g e t V a r i a n c e C o v a r i a n c e + */ + +#if QPOASES_USE_OLD_VERSION + +returnValue SolutionAnalysis::getVarianceCovariance( QProblem* qp, real_t* g_b_bA_VAR, real_t* Primal_Dual_VAR ) +{ + int run1, run2, run3; /* simple run variables (for loops). */ + + returnValue returnvalue; /* the return value */ + BooleanType Delta_bC_isZero = BT_FALSE; /* (just use FALSE here) */ + BooleanType Delta_bB_isZero = BT_FALSE; /* (just use FALSE here) */ + + /* ASK FOR THE NUMBER OF FREE AND FIXED VARIABLES: + * (ASSUMES THAT ACTIVE SET IS CONSTANT FOR THE + * VARIANCE-COVARIANCE EVALUATION) + * ----------------------------------------------- */ + int nFR, nFX, nAC; + + nFR = qp->getNFR( ); + nFX = qp->getNFX( ); + nAC = qp->getNAC( ); + + if ( qp->bounds.getFree( )->getNumberArray( FR_idx ) != SUCCESSFUL_RETURN ) + return THROWERROR( RET_HOTSTART_FAILED ); + + if ( qp->bounds.getFixed( )->getNumberArray( FX_idx ) != SUCCESSFUL_RETURN ) + return THROWERROR( RET_HOTSTART_FAILED ); + + if ( qp->constraints.getActive( )->getNumberArray( AC_idx ) != SUCCESSFUL_RETURN ) + return THROWERROR( RET_HOTSTART_FAILED ); + + /* SOME INITIALIZATIONS: + * --------------------- */ + for( run1 = 0; run1 < KKT_DIM * KKT_DIM; run1++ ) + { + K [run1] = 0.0; + Primal_Dual_VAR[run1] = 0.0; + } + + /* ================================================================= */ + + /* FIRST MATRIX MULTIPLICATION (OBTAINS THE INTERMEDIATE RESULT + * K := [ ("ACTIVE" KKT-MATRIX OF THE QP)^(-1) * g_b_bA_VAR ]^T ) + * THE EVALUATION OF THE INVERSE OF THE KKT-MATRIX OF THE QP + * WITH RESPECT TO THE CURRENT ACTIVE SET + * USES THE EXISTING CHOLESKY AND TQ-DECOMPOSITIONS. FOR DETAILS + * cf. THE (protected) FUNCTION determineStepDirection. */ + + for( run3 = 0; run3 < KKT_DIM; run3++ ) + { + + for( run1 = 0; run1 < NVMAX; run1++ ) + { + delta_g_cov [run1] = g_b_bA_VAR[run3*KKT_DIM+run1]; + delta_lb_cov [run1] = g_b_bA_VAR[run3*KKT_DIM+NVMAX+run1]; /* LINE-WISE LOADING OF THE INPUT */ + delta_ub_cov [run1] = g_b_bA_VAR[run3*KKT_DIM+NVMAX+run1]; /* VARIANCE-COVARIANCE */ + } + for( run1 = 0; run1 < NCMAX; run1++ ) + { + delta_lbA_cov [run1] = g_b_bA_VAR[run3*KKT_DIM+2*NVMAX+run1]; + delta_ubA_cov [run1] = g_b_bA_VAR[run3*KKT_DIM+2*NVMAX+run1]; + } + + /* EVALUATION OF THE STEP: + * ------------------------------------------------------------------------------ */ + + returnvalue = qp->hotstart_determineStepDirection( + FR_idx, FX_idx, AC_idx, + delta_g_cov, delta_lbA_cov, delta_ubA_cov, delta_lb_cov, delta_ub_cov, + Delta_bC_isZero, Delta_bB_isZero, delta_xFX,delta_xFR, + delta_yAC,delta_yFX ); + + /* ------------------------------------------------------------------------------ */ + + /* STOP THE ALGORITHM IN THE CASE OF NO SUCCESFUL RETURN: + * ------------------------------------------------------ */ + if ( returnvalue != SUCCESSFUL_RETURN ) + { + return returnvalue; + } + + /* LINE WISE */ + /* STORAGE OF THE QP-REACTION */ + /* (uses the index list) */ + + for( run1=0; run1hotstart_determineStepDirection( + FR_idx, FX_idx, AC_idx, + delta_g_cov, delta_lbA_cov, delta_ubA_cov, delta_lb_cov, delta_ub_cov, + Delta_bC_isZero, Delta_bB_isZero, delta_xFX,delta_xFR, + delta_yAC,delta_yFX ); + + /* ------------------------------------------------------------------------------ */ + + /* STOP THE ALGORITHM IN THE CASE OF NO SUCCESFUL RETURN: + * ------------------------------------------------------ */ + if ( returnvalue != SUCCESSFUL_RETURN ) + { + return returnvalue; + } + + /* ROW-WISE STORAGE */ + /* OF THE RESULT. */ + + for( run1=0; run1 + + +/***************************************************************************** + * P U B L I C * + *****************************************************************************/ + + +/* + * I n d e x l i s t + */ +Indexlist::Indexlist( ) : length( 0 ), + first( -1 ), + last( -1 ), + lastusedindex( -1 ), + physicallength( INDEXLISTFACTOR*(NVMAX+NCMAX) ) +{ + int i; + + for( i=0; i= 0 ) && ( number[n] >= 0 ) ) + numberarray[i] = number[n]; + else + return THROWERROR( RET_INDEXLIST_CORRUPTED ); + + n = next[n]; + } + + return SUCCESSFUL_RETURN; +} + + +/* + * g e t I n d e x + */ +int Indexlist::getIndex( int givennumber ) const +{ + int i; + int n = first; + int index = -1; /* return -1 by default */ + + /* Run trough indexlist until number is found, if so return it index. */ + for ( i=0; i length ) ) + return -RET_INDEXLIST_OUTOFBOUNDS; + + return number[physicalindex]; +} + + +/* + * g e t L e n g t h + */ +inline int Indexlist::getLength( ) +{ + return length; +} + + +/* + * g e t L a s t N u m b e r + */ +inline int Indexlist::getLastNumber( ) const +{ + return number[last]; +} + + +/* + * g e t L a s t N u m b e r + */ +inline BooleanType Indexlist::isMember( int _number ) const +{ + if ( getIndex( _number ) >= 0 ) + return BT_TRUE; + else + return BT_FALSE; +} + + +/* + * end of file + */ diff --git a/phonelibs/qpoases/SRC/MessageHandling.cpp b/phonelibs/qpoases/SRC/MessageHandling.cpp new file mode 100644 index 00000000000000..6e68cd8552449e --- /dev/null +++ b/phonelibs/qpoases/SRC/MessageHandling.cpp @@ -0,0 +1,529 @@ +/* + * This file is part of qpOASES. + * + * qpOASES -- An Implementation of the Online Active Set Strategy. + * Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved. + * + * qpOASES is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * qpOASES is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with qpOASES; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + + +/** + * \file SRC/MessageHandling.cpp + * \author Hans Joachim Ferreau + * \version 1.3embedded + * \date 2007-2008 + * + * Implementation of the MessageHandling class including global return values. + * + */ + + + +#include +#include + + + + +/** Defines pairs of global return values and messages. */ +MessageHandling::ReturnValueList returnValueList[] = +{ +/* miscellaneous */ +{ SUCCESSFUL_RETURN, "Successful return", VS_VISIBLE }, +{ RET_DIV_BY_ZERO, "Division by zero", VS_VISIBLE }, +{ RET_INDEX_OUT_OF_BOUNDS, "Index out of bounds", VS_VISIBLE }, +{ RET_INVALID_ARGUMENTS, "At least one of the arguments is invalid", VS_VISIBLE }, +{ RET_ERROR_UNDEFINED, "Error number undefined", VS_VISIBLE }, +{ RET_WARNING_UNDEFINED, "Warning number undefined", VS_VISIBLE }, +{ RET_INFO_UNDEFINED, "Info number undefined", VS_VISIBLE }, +{ RET_EWI_UNDEFINED, "Error/warning/info number undefined", VS_VISIBLE }, +{ RET_AVAILABLE_WITH_LINUX_ONLY, "This function is available under Linux only", VS_HIDDEN }, +{ RET_UNKNOWN_BUG, "The error occured is not yet known", VS_VISIBLE }, +{ RET_PRINTLEVEL_CHANGED, "Print level changed", VS_VISIBLE }, +{ RET_NOT_YET_IMPLEMENTED, "Requested function is not yet implemented.", VS_VISIBLE }, +/* Indexlist */ +{ RET_INDEXLIST_MUST_BE_REORDERD, "Index list has to be reordered", VS_VISIBLE }, +{ RET_INDEXLIST_EXCEEDS_MAX_LENGTH, "Index list exceeds its maximal physical length", VS_VISIBLE }, +{ RET_INDEXLIST_CORRUPTED, "Index list corrupted", VS_VISIBLE }, +{ RET_INDEXLIST_OUTOFBOUNDS, "Physical index is out of bounds", VS_VISIBLE }, +{ RET_INDEXLIST_ADD_FAILED, "Adding indices from another index set failed", VS_VISIBLE }, +{ RET_INDEXLIST_INTERSECT_FAILED, "Intersection with another index set failed", VS_VISIBLE }, +/* SubjectTo / Bounds / Constraints */ +{ RET_INDEX_ALREADY_OF_DESIRED_STATUS, "Index is already of desired status", VS_VISIBLE }, +{ RET_SWAPINDEX_FAILED, "Cannot swap between different indexsets", VS_VISIBLE }, +{ RET_ADDINDEX_FAILED, "Adding index to index set failed", VS_VISIBLE }, +{ RET_NOTHING_TO_DO, "Nothing to do", VS_VISIBLE }, +{ RET_SETUP_BOUND_FAILED, "Setting up bound index failed", VS_VISIBLE }, +{ RET_SETUP_CONSTRAINT_FAILED, "Setting up constraint index failed", VS_VISIBLE }, +{ RET_MOVING_BOUND_FAILED, "Moving bound between index sets failed", VS_VISIBLE }, +{ RET_MOVING_CONSTRAINT_FAILED, "Moving constraint between index sets failed", VS_VISIBLE }, +/* QProblem */ +{ RET_QP_ALREADY_INITIALISED, "QProblem has already been initialised", VS_VISIBLE }, +{ RET_NO_INIT_WITH_STANDARD_SOLVER, "Initialisation via extern QP solver is not yet implemented", VS_VISIBLE }, +{ RET_RESET_FAILED, "Reset failed", VS_VISIBLE }, +{ RET_INIT_FAILED, "Initialisation failed", VS_VISIBLE }, +{ RET_INIT_FAILED_TQ, "Initialisation failed due to TQ factorisation", VS_VISIBLE }, +{ RET_INIT_FAILED_CHOLESKY, "Initialisation failed due to Cholesky decomposition", VS_VISIBLE }, +{ RET_INIT_FAILED_HOTSTART, "Initialisation failed! QP could not be solved!", VS_VISIBLE }, +{ RET_INIT_FAILED_INFEASIBILITY, "Initial QP could not be solved due to infeasibility!", VS_VISIBLE }, +{ RET_INIT_FAILED_UNBOUNDEDNESS, "Initial QP could not be solved due to unboundedness!", VS_VISIBLE }, +{ RET_INIT_SUCCESSFUL, "Initialisation done", VS_VISIBLE }, +{ RET_OBTAINING_WORKINGSET_FAILED, "Failed to obtain working set for auxiliary QP", VS_VISIBLE }, +{ RET_SETUP_WORKINGSET_FAILED, "Failed to setup working set for auxiliary QP", VS_VISIBLE }, +{ RET_SETUP_AUXILIARYQP_FAILED, "Failed to setup auxiliary QP for initialised homotopy", VS_VISIBLE }, +{ RET_NO_EXTERN_SOLVER, "No extern QP solver available", VS_VISIBLE }, +{ RET_QP_UNBOUNDED, "QP is unbounded", VS_VISIBLE }, +{ RET_QP_INFEASIBLE, "QP is infeasible", VS_VISIBLE }, +{ RET_QP_NOT_SOLVED, "Problems occured while solving QP with standard solver", VS_VISIBLE }, +{ RET_QP_SOLVED, "QP successfully solved", VS_VISIBLE }, +{ RET_UNABLE_TO_SOLVE_QP, "Problems occured while solving QP", VS_VISIBLE }, +{ RET_INITIALISATION_STARTED, "Starting problem initialisation...", VS_VISIBLE }, +{ RET_HOTSTART_FAILED, "Unable to perform homotopy due to internal error", VS_VISIBLE }, +{ RET_HOTSTART_FAILED_TO_INIT, "Unable to initialise problem", VS_VISIBLE }, +{ RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED, "Unable to perform homotopy as previous QP is not solved", VS_VISIBLE }, +{ RET_ITERATION_STARTED, "Iteration", VS_VISIBLE }, +{ RET_SHIFT_DETERMINATION_FAILED, "Determination of shift of the QP data failed", VS_VISIBLE }, +{ RET_STEPDIRECTION_DETERMINATION_FAILED, "Determination of step direction failed", VS_VISIBLE }, +{ RET_STEPLENGTH_DETERMINATION_FAILED, "Determination of step direction failed", VS_VISIBLE }, +{ RET_OPTIMAL_SOLUTION_FOUND, "Optimal solution of neighbouring QP found", VS_VISIBLE }, +{ RET_HOMOTOPY_STEP_FAILED, "Unable to perform homotopy step", VS_VISIBLE }, +{ RET_HOTSTART_STOPPED_INFEASIBILITY, "Premature homotopy termination because QP is infeasible", VS_VISIBLE }, +{ RET_HOTSTART_STOPPED_UNBOUNDEDNESS, "Premature homotopy termination because QP is unbounded", VS_VISIBLE }, +{ RET_WORKINGSET_UPDATE_FAILED, "Unable to update working sets according to initial guesses", VS_VISIBLE }, +{ RET_MAX_NWSR_REACHED, "Maximum number of working set recalculations performed", VS_VISIBLE }, +{ RET_CONSTRAINTS_NOT_SPECIFIED, "Problem does comprise constraints! You have to specify new constraints' bounds", VS_VISIBLE }, +{ RET_INVALID_FACTORISATION_FLAG, "Invalid factorisation flag", VS_VISIBLE }, +{ RET_UNABLE_TO_SAVE_QPDATA, "Unable to save QP data", VS_VISIBLE }, +{ RET_STEPDIRECTION_FAILED_TQ, "Abnormal termination due to TQ factorisation", VS_VISIBLE }, +{ RET_STEPDIRECTION_FAILED_CHOLESKY, "Abnormal termination due to Cholesky factorisation", VS_VISIBLE }, +{ RET_CYCLING_DETECTED, "Cycling detected", VS_VISIBLE }, +{ RET_CYCLING_NOT_RESOLVED, "Cycling cannot be resolved, QP is probably infeasible", VS_VISIBLE }, +{ RET_CYCLING_RESOLVED, "Cycling probably resolved", VS_VISIBLE }, +{ RET_STEPSIZE, "", VS_VISIBLE }, +{ RET_STEPSIZE_NONPOSITIVE, "", VS_VISIBLE }, +{ RET_SETUPSUBJECTTOTYPE_FAILED, "Setup of SubjectToTypes failed", VS_VISIBLE }, +{ RET_ADDCONSTRAINT_FAILED, "Addition of constraint to working set failed", VS_VISIBLE }, +{ RET_ADDCONSTRAINT_FAILED_INFEASIBILITY, "Addition of constraint to working set failed", VS_VISIBLE }, +{ RET_ADDBOUND_FAILED, "Addition of bound to working set failed", VS_VISIBLE }, +{ RET_ADDBOUND_FAILED_INFEASIBILITY, "Addition of bound to working set failed", VS_VISIBLE }, +{ RET_REMOVECONSTRAINT_FAILED, "Removal of constraint from working set failed", VS_VISIBLE }, +{ RET_REMOVEBOUND_FAILED, "Removal of bound from working set failed", VS_VISIBLE }, +{ RET_REMOVE_FROM_ACTIVESET, "Removing from active set:", VS_VISIBLE }, +{ RET_ADD_TO_ACTIVESET, "Adding to active set:", VS_VISIBLE }, +{ RET_REMOVE_FROM_ACTIVESET_FAILED, "Removing from active set failed", VS_VISIBLE }, +{ RET_ADD_TO_ACTIVESET_FAILED, "Adding to active set failed", VS_VISIBLE }, +{ RET_CONSTRAINT_ALREADY_ACTIVE, "Constraint is already active", VS_VISIBLE }, +{ RET_ALL_CONSTRAINTS_ACTIVE, "All constraints are active, no further constraint can be added", VS_VISIBLE }, +{ RET_LINEARLY_DEPENDENT, "New bound/constraint is linearly dependent", VS_VISIBLE }, +{ RET_LINEARLY_INDEPENDENT, "New bound/constraint is linearly independent", VS_VISIBLE }, +{ RET_LI_RESOLVED, "Linear independence of active contraint matrix successfully resolved", VS_VISIBLE }, +{ RET_ENSURELI_FAILED, "Failed to ensure linear indepence of active contraint matrix", VS_VISIBLE }, +{ RET_ENSURELI_FAILED_TQ, "Abnormal termination due to TQ factorisation", VS_VISIBLE }, +{ RET_ENSURELI_FAILED_NOINDEX, "No index found, QP is probably infeasible", VS_VISIBLE }, +{ RET_ENSURELI_FAILED_CYCLING, "Cycling detected, QP is probably infeasible", VS_VISIBLE }, +{ RET_BOUND_ALREADY_ACTIVE, "Bound is already active", VS_VISIBLE }, +{ RET_ALL_BOUNDS_ACTIVE, "All bounds are active, no further bound can be added", VS_VISIBLE }, +{ RET_CONSTRAINT_NOT_ACTIVE, "Constraint is not active", VS_VISIBLE }, +{ RET_BOUND_NOT_ACTIVE, "Bound is not active", VS_VISIBLE }, +{ RET_HESSIAN_NOT_SPD, "Projected Hessian matrix not positive definite", VS_VISIBLE }, +{ RET_MATRIX_SHIFT_FAILED, "Unable to update matrices or to transform vectors", VS_VISIBLE }, +{ RET_MATRIX_FACTORISATION_FAILED, "Unable to calculate new matrix factorisations", VS_VISIBLE }, +{ RET_PRINT_ITERATION_FAILED, "Unable to print information on current iteration", VS_VISIBLE }, +{ RET_NO_GLOBAL_MESSAGE_OUTPUTFILE, "No global message output file initialised", VS_VISIBLE }, +/* Utils */ +{ RET_UNABLE_TO_OPEN_FILE, "Unable to open file", VS_VISIBLE }, +{ RET_UNABLE_TO_WRITE_FILE, "Unable to write into file", VS_VISIBLE }, +{ RET_UNABLE_TO_READ_FILE, "Unable to read from file", VS_VISIBLE }, +{ RET_FILEDATA_INCONSISTENT, "File contains inconsistent data", VS_VISIBLE }, +/* SolutionAnalysis */ +{ RET_NO_SOLUTION, "QP solution does not satisfy KKT optimality conditions", VS_VISIBLE }, +{ RET_INACCURATE_SOLUTION, "KKT optimality conditions not satisfied to sufficient accuracy", VS_VISIBLE }, +{ TERMINAL_LIST_ELEMENT, "", VS_HIDDEN } /* IMPORTANT: Terminal list element! */ +}; + + + +/***************************************************************************** + * P U B L I C * + *****************************************************************************/ + + +/* + * M e s s a g e H a n d l i n g + */ +MessageHandling::MessageHandling( ) : errorVisibility( VS_VISIBLE ), + warningVisibility( VS_VISIBLE ), + infoVisibility( VS_VISIBLE ), + outputFile( myStdout ), + errorCount( 0 ) +{ +} + +/* + * M e s s a g e H a n d l i n g + */ +MessageHandling::MessageHandling( myFILE* _outputFile ) : + errorVisibility( VS_VISIBLE ), + warningVisibility( VS_VISIBLE ), + infoVisibility( VS_VISIBLE ), + outputFile( _outputFile ), + errorCount( 0 ) +{ +} + +/* + * M e s s a g e H a n d l i n g + */ +MessageHandling::MessageHandling( VisibilityStatus _errorVisibility, + VisibilityStatus _warningVisibility, + VisibilityStatus _infoVisibility + ) : + errorVisibility( _errorVisibility ), + warningVisibility( _warningVisibility ), + infoVisibility( _infoVisibility ), + outputFile( myStderr ), + errorCount( 0 ) +{ +} + +/* + * M e s s a g e H a n d l i n g + */ +MessageHandling::MessageHandling( myFILE* _outputFile, + VisibilityStatus _errorVisibility, + VisibilityStatus _warningVisibility, + VisibilityStatus _infoVisibility + ) : + errorVisibility( _errorVisibility ), + warningVisibility( _warningVisibility ), + infoVisibility( _infoVisibility ), + outputFile( _outputFile ), + errorCount( 0 ) +{ +} + + + +/* + * M e s s a g e H a n d l i n g + */ +MessageHandling::MessageHandling( const MessageHandling& rhs ) : + errorVisibility( rhs.errorVisibility ), + warningVisibility( rhs.warningVisibility ), + infoVisibility( rhs.infoVisibility ), + outputFile( rhs.outputFile ), + errorCount( rhs.errorCount ) +{ +} + + +/* + * ~ M e s s a g e H a n d l i n g + */ +MessageHandling::~MessageHandling( ) +{ + #ifdef PC_DEBUG + if ( outputFile != 0 ) + fclose( outputFile ); + #endif +} + + +/* + * o p e r a t o r = + */ +MessageHandling& MessageHandling::operator=( const MessageHandling& rhs ) +{ + if ( this != &rhs ) + { + errorVisibility = rhs.errorVisibility; + warningVisibility = rhs.warningVisibility; + infoVisibility = rhs.infoVisibility; + outputFile = rhs.outputFile; + errorCount = rhs.errorCount; + } + + return *this; +} + + +/* + * t h r o w E r r o r + */ +returnValue MessageHandling::throwError( + returnValue Enumber, + const char* additionaltext, + const char* functionname, + const char* filename, + const unsigned long linenumber, + VisibilityStatus localVisibilityStatus + ) +{ + /* consistency check */ + if ( Enumber <= SUCCESSFUL_RETURN ) + return throwError( RET_ERROR_UNDEFINED,0,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE ); + + /* Call to common throwMessage function if error shall be displayed. */ + if ( errorVisibility == VS_VISIBLE ) + return throwMessage( Enumber,additionaltext,functionname,filename,linenumber,localVisibilityStatus,"ERROR" ); + else + return Enumber; +} + + +/* + * t h r o w W a r n i n g + */ +returnValue MessageHandling::throwWarning( + returnValue Wnumber, + const char* additionaltext, + const char* functionname, + const char* filename, + const unsigned long linenumber, + VisibilityStatus localVisibilityStatus + ) +{ + /* consistency check */ + if ( Wnumber <= SUCCESSFUL_RETURN ) + return throwError( RET_WARNING_UNDEFINED,0,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE ); + + /* Call to common throwMessage function if warning shall be displayed. */ + if ( warningVisibility == VS_VISIBLE ) + return throwMessage( Wnumber,additionaltext,functionname,filename,linenumber,localVisibilityStatus,"WARNING" ); + else + return Wnumber; +} + + +/* + * t h r o w I n f o + */ +returnValue MessageHandling::throwInfo( + returnValue Inumber, + const char* additionaltext, + const char* functionname, + const char* filename, + const unsigned long linenumber, + VisibilityStatus localVisibilityStatus + ) +{ + /* consistency check */ + if ( Inumber < SUCCESSFUL_RETURN ) + return throwError( RET_INFO_UNDEFINED,0,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE ); + + /* Call to common throwMessage function if info shall be displayed. */ + if ( infoVisibility == VS_VISIBLE ) + return throwMessage( Inumber,additionaltext,functionname,filename,linenumber,localVisibilityStatus,"INFO" ); + else + return Inumber; +} + + +/* + * r e s e t + */ +returnValue MessageHandling::reset( ) +{ + setErrorVisibilityStatus( VS_VISIBLE ); + setWarningVisibilityStatus( VS_VISIBLE ); + setInfoVisibilityStatus( VS_VISIBLE ); + + setOutputFile( myStderr ); + setErrorCount( 0 ); + + return SUCCESSFUL_RETURN; +} + + +/* + * l i s t A l l M e s s a g e s + */ +returnValue MessageHandling::listAllMessages( ) +{ + #ifdef PC_DEBUG + int keypos = 0; + char myPrintfString[160]; + + /* Run through whole returnValueList and print each item. */ + while ( returnValueList[keypos].key != TERMINAL_LIST_ELEMENT ) + { + sprintf( myPrintfString," %d - %s \n",keypos,returnValueList[keypos].data ); + myPrintf( myPrintfString ); + + ++keypos; + } + #endif + + return SUCCESSFUL_RETURN; +} + + + +/***************************************************************************** + * P R O T E C T E D * + *****************************************************************************/ + + +#ifdef PC_DEBUG /* Re-define throwMessage function for embedded code! */ + +/* + * t h r o w M e s s a g e + */ +returnValue MessageHandling::throwMessage( + returnValue RETnumber, + const char* additionaltext, + const char* functionname, + const char* filename, + const unsigned long linenumber, + VisibilityStatus localVisibilityStatus, + const char* RETstring + ) +{ + int i; + + int keypos = 0; + char myPrintfString[160]; + + /* 1) Determine number of whitespace for output. */ + char whitespaces[41]; + int numberOfWhitespaces = (errorCount-1)*2; + + if ( numberOfWhitespaces < 0 ) + numberOfWhitespaces = 0; + + if ( numberOfWhitespaces > 40 ) + numberOfWhitespaces = 40; + + for( i=0; i 0 ) + { + sprintf( myPrintfString,"%s->", whitespaces ); + myPrintf( myPrintfString ); + } + + if ( additionaltext == 0 ) + { + sprintf( myPrintfString,"%s (%s, %s:%d): \t%s\n", + RETstring,functionname,filename,(int)linenumber,returnValueList[keypos].data + ); + myPrintf( myPrintfString ); + } + else + { + sprintf( myPrintfString,"%s (%s, %s:%d): \t%s %s\n", + RETstring,functionname,filename,(int)linenumber,returnValueList[keypos].data,additionaltext + ); + myPrintf( myPrintfString ); + } + + /* take care of proper indention for subsequent error messages */ + if ( RETstring[0] == 'E' ) + { + ++errorCount; + } + else + { + if ( errorCount > 0 ) + myPrintf( "\n" ); + errorCount = 0; + } + } + + return RETnumber; +} + +#else /* = PC_DEBUG not defined */ + +/* + * t h r o w M e s s a g e + */ +returnValue MessageHandling::throwMessage( + returnValue RETnumber, + const char* additionaltext, + const char* functionname, + const char* filename, + const unsigned long linenumber, + VisibilityStatus localVisibilityStatus, + const char* RETstring + ) +{ + /* DUMMY CODE FOR PRETENDING USE OF ARGUMENTS + * FOR SUPPRESSING COMPILER WARNINGS! */ + int i = 0; + if ( additionaltext == 0 ) i++; + if ( functionname == 0 ) i++; + if ( filename == 0 ) i++; + if ( linenumber == 0 ) i++; + if ( localVisibilityStatus == VS_VISIBLE ) i++; + if ( RETstring == 0 ) i++; + /* END OF DUMMY CODE */ + + return RETnumber; +} + +#endif /* PC_DEBUG */ + + + +/***************************************************************************** + * G L O B A L M E S S A G E H A N D L E R * + *****************************************************************************/ + + +/** Global message handler for all qpOASES modules.*/ +MessageHandling globalMessageHandler( myStderr,VS_VISIBLE,VS_VISIBLE,VS_VISIBLE ); + + +/* + * g e t G l o b a l M e s s a g e H a n d l e r + */ +MessageHandling* getGlobalMessageHandler( ) +{ + return &globalMessageHandler; +} + +const char* MessageHandling::getErrorString(int error) +{ + return returnValueList[ error ].data; +} + + + +/* + * end of file + */ diff --git a/phonelibs/qpoases/SRC/MessageHandling.ipp b/phonelibs/qpoases/SRC/MessageHandling.ipp new file mode 100644 index 00000000000000..ad9624b3d69a62 --- /dev/null +++ b/phonelibs/qpoases/SRC/MessageHandling.ipp @@ -0,0 +1,137 @@ +/* + * This file is part of qpOASES. + * + * qpOASES -- An Implementation of the Online Active Set Strategy. + * Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved. + * + * qpOASES is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * qpOASES is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with qpOASES; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + + +/** + * \file SRC/MessageHandling.ipp + * \author Hans Joachim Ferreau + * \version 1.3embedded + * \date 2007-2008 + * + * Implementation of inlined member functions of the MessageHandling class. + */ + + + +/***************************************************************************** + * P U B L I C * + *****************************************************************************/ + +/* + * g e t E r r o r V i s i b i l i t y S t a t u s + */ +inline VisibilityStatus MessageHandling::getErrorVisibilityStatus( ) const +{ + return errorVisibility; +} + + +/* + * g e t W a r n i n g V i s i b i l i t y S t a t u s + */ +inline VisibilityStatus MessageHandling::getWarningVisibilityStatus( ) const +{ + return warningVisibility; +} + + +/* + * g e t I n f o V i s i b i l i t y S t a t u s + */ +inline VisibilityStatus MessageHandling::getInfoVisibilityStatus( ) const +{ + return infoVisibility; +} + + +/* + * g e t O u t p u t F i l e + */ +inline myFILE* MessageHandling::getOutputFile( ) const +{ + return outputFile; +} + + +/* + * g e t E r r o r C o u n t + */ +inline int MessageHandling::getErrorCount( ) const +{ + return errorCount; +} + + +/* + * s e t E r r o r V i s i b i l i t y S t a t u s + */ +inline void MessageHandling::setErrorVisibilityStatus( VisibilityStatus _errorVisibility ) +{ + errorVisibility = _errorVisibility; +} + + +/* + * s e t W a r n i n g V i s i b i l i t y S t a t u s + */ +inline void MessageHandling::setWarningVisibilityStatus( VisibilityStatus _warningVisibility ) +{ + warningVisibility = _warningVisibility; +} + + +/* + * s e t I n f o V i s i b i l i t y S t a t u s + */ +inline void MessageHandling::setInfoVisibilityStatus( VisibilityStatus _infoVisibility ) +{ + infoVisibility = _infoVisibility; +} + + +/* + * s e t O u t p u t F i l e + */ +inline void MessageHandling::setOutputFile( myFILE* _outputFile ) +{ + outputFile = _outputFile; +} + + +/* + * s e t E r r o r C o u n t + */ +inline returnValue MessageHandling::setErrorCount( int _errorCount ) +{ + if ( _errorCount >= 0 ) + { + errorCount = _errorCount; + return SUCCESSFUL_RETURN; + } + else + return RET_INVALID_ARGUMENTS; +} + + +/* + * end of file + */ diff --git a/phonelibs/qpoases/SRC/QProblem.cpp b/phonelibs/qpoases/SRC/QProblem.cpp new file mode 100644 index 00000000000000..68a6a79e534f65 --- /dev/null +++ b/phonelibs/qpoases/SRC/QProblem.cpp @@ -0,0 +1,3867 @@ +/* + * This file is part of qpOASES. + * + * qpOASES -- An Implementation of the Online Active Set Strategy. + * Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved. + * + * qpOASES is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * qpOASES is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with qpOASES; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + + +/** + * \file SRC/QProblem.cpp + * \author Hans Joachim Ferreau + * \version 1.3embedded + * \date 2007-2008 + * + * Implementation of the QProblem class which is able to use the newly + * developed online active set strategy for parametric quadratic programming. + */ + + +#include + +#include + +void printmatrix2(char *name, double *A, int m, int n) { + int i, j; + + printf("%s = [...\n", name); + for (i = 0; i < m; i++) { + for (j = 0; j < n; j++) + printf(" % 9.4f", A[i*n+j]); + printf(",\n"); + } + printf("];\n"); +} + +//#define __PERFORM_KKT_TEST__ + + +/***************************************************************************** + * P U B L I C * + *****************************************************************************/ + + +/* + * Q P r o b l e m + */ +QProblem::QProblem( ) : QProblemB( ) +{ + constraints.init( 0 ); + + sizeT = 0; + + cyclingManager.init( 0,0 ); +} + + +/* + * Q P r o b l e m + */ +QProblem::QProblem( int _nV, int _nC ) : QProblemB( _nV ) +{ + /* consistency checks */ + if ( _nV <= 0 ) + _nV = 1; + + if ( _nC < 0 ) + { + _nC = 0; + THROWERROR( RET_INVALID_ARGUMENTS ); + } + + constraints.init( _nC ); + + + sizeT = _nC; + if ( _nC > _nV ) + sizeT = _nV; + + cyclingManager.init( _nV,_nC ); +} + + +/* + * Q P r o b l e m + */ +QProblem::QProblem( const QProblem& rhs ) : QProblemB( rhs ) +{ + int i, j; + + int _nV = rhs.bounds.getNV( ); + int _nC = rhs.constraints.getNC( ); + + for( i=0; i<_nC; ++i ) + for( j=0; j<_nV; ++j ) + A[i*NVMAX + j] = rhs.A[i*NVMAX + j]; + + for( i=0; i<_nC; ++i ) + lbA[i] = rhs.lbA[i]; + + for( i=0; i<_nC; ++i ) + ubA[i] = rhs.ubA[i]; + + constraints = rhs.constraints; + + for( i=0; i<(_nV+_nC); ++i ) + y[i] = rhs.y[i]; + + + sizeT = rhs.sizeT; + + for( i=0; ithrowInfo( RET_ITERATION_STARTED,messageString,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE ); + #endif + } + + /* 1) Setup index arrays. */ + if ( bounds.getFree( )->getNumberArray( FR_idx ) != SUCCESSFUL_RETURN ) + return THROWERROR( RET_HOTSTART_FAILED ); + + if ( bounds.getFixed( )->getNumberArray( FX_idx ) != SUCCESSFUL_RETURN ) + return THROWERROR( RET_HOTSTART_FAILED ); + + if ( constraints.getActive( )->getNumberArray( AC_idx ) != SUCCESSFUL_RETURN ) + return THROWERROR( RET_HOTSTART_FAILED ); + + if ( constraints.getInactive( )->getNumberArray( IAC_idx ) != SUCCESSFUL_RETURN ) + return THROWERROR( RET_HOTSTART_FAILED ); + + /* 2) Detemination of shift direction of the gradient and the (constraints') bounds. */ + returnvalue = hotstart_determineDataShift( FX_idx, AC_idx, + g_new,lbA_new,ubA_new,lb_new,ub_new, + delta_g,delta_lbA,delta_ubA,delta_lb,delta_ub, + Delta_bC_isZero, Delta_bB_isZero ); + if ( returnvalue != SUCCESSFUL_RETURN ) + { + nWSR = l; + THROWERROR( RET_SHIFT_DETERMINATION_FAILED ); + return returnvalue; + } + + /* 3) Determination of step direction of X and Y. */ + returnvalue = hotstart_determineStepDirection( FR_idx,FX_idx,AC_idx, + delta_g,delta_lbA,delta_ubA,delta_lb,delta_ub, + Delta_bC_isZero, Delta_bB_isZero, + delta_xFX,delta_xFR,delta_yAC,delta_yFX + ); + if ( returnvalue != SUCCESSFUL_RETURN ) + { + nWSR = l; + THROWERROR( RET_STEPDIRECTION_DETERMINATION_FAILED ); + return returnvalue; + } + + /* 4) Determination of step length TAU. */ + returnvalue = hotstart_determineStepLength( FR_idx,FX_idx,AC_idx,IAC_idx, + delta_lbA,delta_ubA,delta_lb,delta_ub, + delta_xFX,delta_xFR,delta_yAC,delta_yFX,delta_Ax, + BC_idx,BC_status,BC_isBound + ); + if ( returnvalue != SUCCESSFUL_RETURN ) + { + nWSR = l; + THROWERROR( RET_STEPLENGTH_DETERMINATION_FAILED ); + return returnvalue; + } + + /* 5) Realisation of the homotopy step. */ + returnvalue = hotstart_performStep( FR_idx,FX_idx,AC_idx,IAC_idx, + delta_g,delta_lbA,delta_ubA,delta_lb,delta_ub, + delta_xFX,delta_xFR,delta_yAC,delta_yFX,delta_Ax, + BC_idx,BC_status,BC_isBound + ); + + if ( returnvalue != SUCCESSFUL_RETURN ) + { + nWSR = l; + + /* stop runtime measurement */ + if ( cputime != 0 ) + *cputime = getCPUtime( ) - starttime; + + /* optimal solution found? */ + if ( returnvalue == RET_OPTIMAL_SOLUTION_FOUND ) + { + status = QPS_SOLVED; + + if ( printlevel == PL_HIGH ) + THROWINFO( RET_OPTIMAL_SOLUTION_FOUND ); + + #ifdef PC_DEBUG + if ( printIteration( l,BC_idx,BC_status,BC_isBound ) != SUCCESSFUL_RETURN ) + THROWERROR( RET_PRINT_ITERATION_FAILED ); /* do not pass this as return value! */ + #endif + + /* check KKT optimality conditions */ + return checkKKTconditions( ); + } + else + { + /* checks for infeasibility... */ + if ( isInfeasible( ) == BT_TRUE ) + { + status = QPS_HOMOTOPYQPSOLVED; + return THROWERROR( RET_HOTSTART_STOPPED_INFEASIBILITY ); + } + + /* ...unboundedness... */ + if ( unbounded == BT_TRUE ) /* not necessary since objective function convex! */ + return THROWERROR( RET_HOTSTART_STOPPED_UNBOUNDEDNESS ); + + /* ... and throw unspecific error otherwise */ + THROWERROR( RET_HOMOTOPY_STEP_FAILED ); + return returnvalue; + } + } + + /* 6) Output information of successful QP iteration. */ + status = QPS_HOMOTOPYQPSOLVED; + + #ifdef PC_DEBUG + if ( printIteration( l,BC_idx,BC_status,BC_isBound ) != SUCCESSFUL_RETURN ) + THROWERROR( RET_PRINT_ITERATION_FAILED ); /* do not pass this as return value! */ + #endif + } + + + /* stop runtime measurement */ + if ( cputime != 0 ) + *cputime = getCPUtime( ) - starttime; + + + /* if programm gets to here, output information that QP could not be solved + * within the given maximum numbers of working set changes */ + if ( printlevel == PL_HIGH ) + { + #ifdef PC_DEBUG + sprintf( messageString,"(nWSR = %d)",nWSR ); + return getGlobalMessageHandler( )->throwWarning( RET_MAX_NWSR_REACHED,messageString,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE ); + #endif + } + + /* Finally check KKT optimality conditions. */ + returnValue returnvalueKKTcheck = checkKKTconditions( ); + + if ( returnvalueKKTcheck != SUCCESSFUL_RETURN ) + return returnvalueKKTcheck; + else + return RET_MAX_NWSR_REACHED; +} + + +/* + * g e t N Z + */ +int QProblem::getNZ( ) +{ + /* nZ = nFR - nAC */ + return bounds.getFree( )->getLength( ) - constraints.getActive( )->getLength( ); +} + + +/* + * g e t D u a l S o l u t i o n + */ +returnValue QProblem::getDualSolution( real_t* const yOpt ) const +{ + int i; + + /* return optimal dual solution vector + * only if current QP has been solved */ + if ( ( getStatus( ) == QPS_AUXILIARYQPSOLVED ) || + ( getStatus( ) == QPS_HOMOTOPYQPSOLVED ) || + ( getStatus( ) == QPS_SOLVED ) ) + { + for( i=0; i -INFTY ) + { + constraints.setNoLower( BT_FALSE ); + break; + } + } + + /* 2) Check if upper constraints' bounds are present. */ + constraints.setNoUpper( BT_TRUE ); + for( i=0; i INFTY - BOUNDTOL ) ) + { + constraints.setType( i,ST_UNBOUNDED ); + ++nUC; + } + else + { + if ( lbA[i] > ubA[i] - BOUNDTOL ) + { + constraints.setType( i,ST_EQUALITY ); + ++nEC; + } + else + { + constraints.setType( i,ST_BOUNDED ); + } + } + } + + /* 4) Set dimensions of constraints structure. */ + constraints.setNEC( nEC ); + constraints.setNUC( nUC ); + constraints.setNIC( nC - nEC - nUC ); + + return SUCCESSFUL_RETURN; +} + + +/* + * c h o l e s k y D e c o m p o s i t i o n P r o j e c t e d + */ +returnValue QProblem::setupCholeskyDecompositionProjected( ) +{ + int i, j, k, ii, kk; + int nV = getNV( ); + int nFR = getNFR( ); + int nZ = getNZ( ); + + /* 1) Initialises R with all zeros. */ + for( i=0; i 0 ) + { + int FR_idx[NVMAX]; + if ( bounds.getFree( )->getNumberArray( FR_idx ) != SUCCESSFUL_RETURN ) + return THROWERROR( RET_INDEXLIST_CORRUPTED ); + +#if 0 + real_t HZ[NVMAX*NVMAX]; + real_t ZHZ[NVMAX*NVMAX]; + + /* calculate H*Z */ + for ( i=0; i=0; --k ) + sum -= R[k*NVMAX + i] * R[k*NVMAX + i]; + + if ( sum > 0.0 ) + { + R[i*NVMAX + i] = sqrt( sum ); + inv = 1.0 / R[i * NVMAX + i]; + } + else + { + hessianType = HST_SEMIDEF; + return THROWERROR( RET_HESSIAN_NOT_SPD ); + } + + for( j=(i+1); j=0; --k ) + sum -= R[k*NVMAX + i] * R[k*NVMAX + j]; + + R[i*NVMAX + j] = sum * inv; + } + } +#else + real_t HZ[NVMAX]; + real_t ZHZ[NVMAX]; + + real_t sum, inv; + for (j = 0; j < nZ; ++j) + { + /* Cache one column of Z. */ + for (i = 0; i < NVMAX; ++i) + ZHZ[i] = Q[i * NVMAX + j]; + + /* Create one column of the product H * Z. */ + for (i = 0; i < nFR; ++i) + { + ii = FR_idx[i]; + + sum = 0.0; + for (k = 0; k < nFR; ++k) + { + kk = FR_idx[k]; + sum += H[ii * NVMAX + kk] * ZHZ[kk]; + } + HZ[ii] = sum; + } + + /* Create one column of the product Z^T * H * Z. */ + for (i = j; i < nZ; ++i) + ZHZ[ i ] = 0.0; + + for (k = 0; k < nFR; ++k) + { + kk = FR_idx[k]; + real_t q = HZ[kk]; + for (i = j; i < nZ; ++i) + { + ZHZ[i] += Q[kk * NVMAX + i] * q; + } + } + + /* Use the computed column to update the factorization. */ + /* j == i */ + sum = ZHZ[j]; + + for (k = (j - 1); k >= 0; --k) + sum -= R[k * NVMAX + j] * R[k * NVMAX + j]; + + if (sum > 0.0) + { + R[j * NVMAX + j] = sqrt(sum); + inv = 1.0 / R[j * NVMAX + j]; + } + else + { + hessianType = HST_SEMIDEF; + return THROWERROR( RET_HESSIAN_NOT_SPD ); + } + + for (i = (j + 1); i < nZ; ++i) + { + sum = ZHZ[i]; + + for (k = (j - 1); k >= 0; --k) + sum -= R[k * NVMAX + j] * R[k * NVMAX + i]; + + R[j * NVMAX + i] = sum * inv; + } + } +#endif + } + } + + return SUCCESSFUL_RETURN; +} + + +/* + * s e t u p T Q f a c t o r i s a t i o n + */ +returnValue QProblem::setupTQfactorisation( ) +{ + int i, j, ii; + int nV = getNV( ); + int nFR = getNFR( ); + + int FR_idx[NVMAX]; + if ( bounds.getFree( )->getNumberArray( FR_idx ) != SUCCESSFUL_RETURN ) + return THROWERROR( RET_INDEXLIST_CORRUPTED ); + + /* 1) Set Q to unity matrix. */ + for( i=0; igetStatus( i ); + + if ( constraints.getType( i ) == ST_EQUALITY ) + { + if ( auxiliaryConstraints->setupConstraint( i,ST_LOWER ) != SUCCESSFUL_RETURN ) + return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED ); + } + else + { + if ( auxiliaryConstraints->setupConstraint( i,guessedStatus ) != SUCCESSFUL_RETURN ) + return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED ); + } + } + } + else /* No initial working set specified. */ + { + /* Obtain initial working set by "clipping". */ + if ( ( xOpt != 0 ) && ( yOpt == 0 ) ) + { + for( i=0; isetupConstraint( i,ST_LOWER ) != SUCCESSFUL_RETURN ) + return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED ); + continue; + } + + if ( Ax[i] >= ubA[i] - BOUNDTOL ) + { + if ( auxiliaryConstraints->setupConstraint( i,ST_UPPER ) != SUCCESSFUL_RETURN ) + return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED ); + continue; + } + + /* Moreover, add all equality constraints if specified. */ + if ( constraints.getType( i ) == ST_EQUALITY ) + { + if ( auxiliaryConstraints->setupConstraint( i,ST_LOWER ) != SUCCESSFUL_RETURN ) + return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED ); + } + else + { + if ( auxiliaryConstraints->setupConstraint( i,ST_INACTIVE ) != SUCCESSFUL_RETURN ) + return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED ); + } + } + } + + /* Obtain initial working set in accordance to sign of dual solution vector. */ + if ( ( xOpt == 0 ) && ( yOpt != 0 ) ) + { + for( i=0; i ZERO ) + { + if ( auxiliaryConstraints->setupConstraint( i,ST_LOWER ) != SUCCESSFUL_RETURN ) + return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED ); + continue; + } + + if ( yOpt[nV+i] < -ZERO ) + { + if ( auxiliaryConstraints->setupConstraint( i,ST_UPPER ) != SUCCESSFUL_RETURN ) + return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED ); + continue; + } + + /* Moreover, add all equality constraints if specified. */ + if ( constraints.getType( i ) == ST_EQUALITY ) + { + if ( auxiliaryConstraints->setupConstraint( i,ST_LOWER ) != SUCCESSFUL_RETURN ) + return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED ); + } + else + { + if ( auxiliaryConstraints->setupConstraint( i,ST_INACTIVE ) != SUCCESSFUL_RETURN ) + return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED ); + } + } + } + + /* If xOpt and yOpt are null pointer and no initial working is specified, + * start with empty working set (or implicitly fixed bounds and equality constraints only) + * for auxiliary QP. */ + if ( ( xOpt == 0 ) && ( yOpt == 0 ) ) + { + for( i=0; isetupConstraint( i,ST_LOWER ) != SUCCESSFUL_RETURN ) + return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED ); + } + else + { + if ( auxiliaryConstraints->setupConstraint( i,ST_INACTIVE ) != SUCCESSFUL_RETURN ) + return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED ); + } + } + } + } + + return SUCCESSFUL_RETURN; +} + + + +/* + * s e t u p A u x i l i a r y W o r k i n g S e t + */ +returnValue QProblem::setupAuxiliaryWorkingSet( const Bounds* const auxiliaryBounds, + const Constraints* const auxiliaryConstraints, + BooleanType setupAfresh + ) +{ + int i; + int nV = getNV( ); + int nC = getNC( ); + + /* consistency checks */ + if ( auxiliaryBounds != 0 ) + { + for( i=0; igetStatus( i ) == ST_UNDEFINED ) ) + return THROWERROR( RET_UNKNOWN_BUG ); + } + else + { + return THROWERROR( RET_INVALID_ARGUMENTS ); + } + + if ( auxiliaryConstraints != 0 ) + { + for( i=0; igetStatus( i ) == ST_UNDEFINED ) ) + return THROWERROR( RET_UNKNOWN_BUG ); + } + else + { + return THROWERROR( RET_INVALID_ARGUMENTS ); + } + + + /* I) SETUP CHOLESKY FLAG: + * Cholesky decomposition shall only be updated if working set + * shall be updated (i.e. NOT setup afresh!) */ + BooleanType updateCholesky; + if ( setupAfresh == BT_TRUE ) + updateCholesky = BT_FALSE; + else + updateCholesky = BT_TRUE; + + + /* II) REMOVE FORMERLY ACTIVE (CONSTRAINTS') BOUNDS (IF NECESSARY): */ + if ( setupAfresh == BT_FALSE ) + { + /* 1) Remove all active constraints that shall be inactive AND + * all active constraints that are active at the wrong bound. */ + for( i=0; igetStatus( i ) != ST_LOWER ) ) + if ( removeConstraint( i,updateCholesky ) != SUCCESSFUL_RETURN ) + return THROWERROR( RET_SETUP_WORKINGSET_FAILED ); + + if ( ( constraints.getStatus( i ) == ST_UPPER ) && ( auxiliaryConstraints->getStatus( i ) != ST_UPPER ) ) + if ( removeConstraint( i,updateCholesky ) != SUCCESSFUL_RETURN ) + return THROWERROR( RET_SETUP_WORKINGSET_FAILED ); + } + + /* 2) Remove all active bounds that shall be inactive AND + * all active bounds that are active at the wrong bound. */ + for( i=0; igetStatus( i ) != ST_LOWER ) ) + if ( removeBound( i,updateCholesky ) != SUCCESSFUL_RETURN ) + return THROWERROR( RET_SETUP_WORKINGSET_FAILED ); + + if ( ( bounds.getStatus( i ) == ST_UPPER ) && ( auxiliaryBounds->getStatus( i ) != ST_UPPER ) ) + if ( removeBound( i,updateCholesky ) != SUCCESSFUL_RETURN ) + return THROWERROR( RET_SETUP_WORKINGSET_FAILED ); + } + } + + + /* III) ADD NEWLY ACTIVE (CONSTRAINTS') BOUNDS: */ + /* 1) Add all inactive bounds that shall be active AND + * all formerly active bounds that have been active at the wrong bound. */ + for( i=0; igetStatus( i ) != ST_INACTIVE ) ) + { + /* Add bound only if it is linearly independent from the current working set. */ + if ( addBound_checkLI( i ) == RET_LINEARLY_INDEPENDENT ) + { + if ( addBound( i,auxiliaryBounds->getStatus( i ),updateCholesky ) != SUCCESSFUL_RETURN ) + return THROWERROR( RET_SETUP_WORKINGSET_FAILED ); + } + } + } + + /* 2) Add all inactive constraints that shall be active AND + * all formerly active constraints that have been active at the wrong bound. */ + for( i=0; igetStatus( i ) == ST_LOWER ) || ( auxiliaryConstraints->getStatus( i ) == ST_UPPER ) ) + { + /* formerly inactive */ + if ( constraints.getStatus( i ) == ST_INACTIVE ) + { + /* Add constraint only if it is linearly independent from the current working set. */ + if ( addConstraint_checkLI( i ) == RET_LINEARLY_INDEPENDENT ) + { + if ( addConstraint( i,auxiliaryConstraints->getStatus( i ),updateCholesky ) != SUCCESSFUL_RETURN ) + return THROWERROR( RET_SETUP_WORKINGSET_FAILED ); + } + } + } + } + + return SUCCESSFUL_RETURN; +} + + +/* + * s e t u p A u x i l i a r y Q P s o l u t i o n + */ +returnValue QProblem::setupAuxiliaryQPsolution( const real_t* const xOpt, const real_t* const yOpt + ) +{ + int i, j; + int nV = getNV( ); + int nC = getNC( ); + + + /* Setup primal/dual solution vector for auxiliary initial QP: + * if a null pointer is passed, a zero vector is assigned; + * old solution vector is kept if pointer to internal solution vevtor is passed. */ + if ( xOpt != 0 ) + { + if ( xOpt != x ) + for( i=0; igetStatus( i ) == ST_LOWER ) + lb[i] = x[i]; + else + lb[i] = x[i] - BOUNDRELAXATION; + + if ( auxiliaryBounds->getStatus( i ) == ST_UPPER ) + ub[i] = x[i]; + else + ub[i] = x[i] + BOUNDRELAXATION; + } + } + break; + + case ST_LOWER: + lb[i] = x[i]; + if ( bounds.getType( i ) == ST_EQUALITY ) + { + ub[i] = x[i]; + } + else + { + if ( useRelaxation == BT_TRUE ) + ub[i] = x[i] + BOUNDRELAXATION; + } + break; + + case ST_UPPER: + ub[i] = x[i]; + if ( bounds.getType( i ) == ST_EQUALITY ) + { + lb[i] = x[i]; + } + else + { + if ( useRelaxation == BT_TRUE ) + lb[i] = x[i] - BOUNDRELAXATION; + } + break; + + default: + return THROWERROR( RET_UNKNOWN_BUG ); + } + } + + /* 2) Setup constraints vectors. */ + for ( i=0; igetStatus( i ) == ST_LOWER ) + lbA[i] = Ax[i]; + else + lbA[i] = Ax[i] - BOUNDRELAXATION; + + if ( auxiliaryConstraints->getStatus( i ) == ST_UPPER ) + ubA[i] = Ax[i]; + else + ubA[i] = Ax[i] + BOUNDRELAXATION; + } + } + break; + + case ST_LOWER: + lbA[i] = Ax[i]; + if ( constraints.getType( i ) == ST_EQUALITY ) + { + ubA[i] = Ax[i]; + } + else + { + if ( useRelaxation == BT_TRUE ) + ubA[i] = Ax[i] + BOUNDRELAXATION; + } + break; + + case ST_UPPER: + ubA[i] = Ax[i]; + if ( constraints.getType( i ) == ST_EQUALITY ) + { + lbA[i] = Ax[i]; + } + else + { + if ( useRelaxation == BT_TRUE ) + lbA[i] = Ax[i] - BOUNDRELAXATION; + } + break; + + default: + return THROWERROR( RET_UNKNOWN_BUG ); + } + } + + return SUCCESSFUL_RETURN; +} + + +/* + * a d d C o n s t r a i n t + */ +returnValue QProblem::addConstraint( int number, SubjectToStatus C_status, + BooleanType updateCholesky + ) +{ + int i, j, ii; + + /* consistency checks */ + if ( constraints.getStatus( number ) != ST_INACTIVE ) + return THROWERROR( RET_CONSTRAINT_ALREADY_ACTIVE ); + + if ( ( constraints.getNC( ) - getNAC( ) ) == constraints.getNUC( ) ) + return THROWERROR( RET_ALL_CONSTRAINTS_ACTIVE ); + + if ( ( getStatus( ) == QPS_NOTINITIALISED ) || + ( getStatus( ) == QPS_AUXILIARYQPSOLVED ) || + ( getStatus( ) == QPS_HOMOTOPYQPSOLVED ) || + ( getStatus( ) == QPS_SOLVED ) ) + { + return THROWERROR( RET_UNKNOWN_BUG ); + } + + + /* I) ENSURE LINEAR INDEPENDENCE OF THE WORKING SET, + * i.e. remove a constraint or bound if linear dependence occurs. */ + /* check for LI only if Cholesky decomposition shall be updated! */ + if ( updateCholesky == BT_TRUE ) + { + returnValue ensureLIreturnvalue = addConstraint_ensureLI( number,C_status ); + + switch ( ensureLIreturnvalue ) + { + case SUCCESSFUL_RETURN: + break; + + case RET_LI_RESOLVED: + break; + + case RET_ENSURELI_FAILED_NOINDEX: + return THROWERROR( RET_ADDCONSTRAINT_FAILED_INFEASIBILITY ); + + case RET_ENSURELI_FAILED_CYCLING: + return THROWERROR( RET_ADDCONSTRAINT_FAILED_INFEASIBILITY ); + + default: + return THROWERROR( RET_ENSURELI_FAILED ); + } + } + + /* some definitions */ + int nFR = getNFR( ); + int nAC = getNAC( ); + int nZ = getNZ( ); + + int tcol = sizeT - nAC; + + + int FR_idx[NVMAX]; + if ( bounds.getFree( )->getNumberArray( FR_idx ) != SUCCESSFUL_RETURN ) + return THROWERROR( RET_ADDCONSTRAINT_FAILED ); + + real_t aFR[NVMAX]; + real_t wZ[NVMAX]; + for( i=0; i 0 ) + { + for( j=0; j 0 ) + { + /* II) RESTORE TRIANGULAR FORM OF T: */ + /* Use column-wise Givens rotations to restore reverse triangular form + * of T, simultanenous change of Q (i.e. Z) and R. */ + for( j=0; jgetNumberArray( FR_idx ) != SUCCESSFUL_RETURN ) + return THROWERROR( RET_INDEXLIST_CORRUPTED ); + + /* Check if constraint is linearly independent from the + the active ones (<=> is element of null space of Afr). */ + real_t sum; + + for( i=0; i 10.0*EPS ) + return RET_LINEARLY_INDEPENDENT; + } + + return RET_LINEARLY_DEPENDENT; +} + + +/* + * a d d C o n s t r a i n t _ e n s u r e L I + */ +returnValue QProblem::addConstraint_ensureLI( int number, SubjectToStatus C_status ) +{ + int i, j, ii, jj; + int nV = getNV( ); + int nFR = getNFR( ); + int nFX = getNFX( ); + int nAC = getNAC( ); + int nZ = getNZ( ); + + + /* I) Check if new constraint is linearly independent from the active ones. */ + returnValue returnvalueCheckLI = addConstraint_checkLI( number ); + + if ( returnvalueCheckLI == RET_INDEXLIST_CORRUPTED ) + return THROWERROR( RET_ENSURELI_FAILED ); + + if ( returnvalueCheckLI == RET_LINEARLY_INDEPENDENT ) + return SUCCESSFUL_RETURN; + + + /* II) NEW CONSTRAINT IS LINEARLY DEPENDENT: */ + /* 1) Determine coefficients of linear combination, + * cf. M.J. Best. Applied Mathematics and Parallel Computing, chapter: + * An Algorithm for the Solution of the Parametric Quadratic Programming + * Problem, pages 57-76. Physica-Verlag, Heidelberg, 1996. */ + int FR_idx[NVMAX]; + if ( bounds.getFree( )->getNumberArray( FR_idx ) != SUCCESSFUL_RETURN ) + return THROWERROR( RET_ENSURELI_FAILED ); + + int FX_idx[NVMAX]; + if ( bounds.getFixed( )->getNumberArray( FX_idx ) != SUCCESSFUL_RETURN ) + return THROWERROR( RET_ENSURELI_FAILED ); + + real_t xiC[NCMAX_ALLOC]; + real_t xiC_TMP[NCMAX_ALLOC]; + real_t xiB[NVMAX]; + + /* 2) Calculate xiC */ + if ( nAC > 0 ) + { + if ( C_status == ST_LOWER ) + { + for( i=0; igetNumberArray( AC_idx ) != SUCCESSFUL_RETURN ) + return THROWERROR( RET_ENSURELI_FAILED ); + + if ( C_status == ST_LOWER ) + { + for( i=0; i ZERO ) && ( y[nV+ii] >= 0.0 ) ) + { + if ( y[nV+ii]/xiC[i] < y_min ) + { + y_min = y[nV+ii]/xiC[i]; + y_min_number = ii; + } + } + } + else + { + if ( ( xiC[i] < -ZERO ) && ( y[nV+ii] <= 0.0 ) ) + { + if ( y[nV+ii]/xiC[i] < y_min ) + { + y_min = y[nV+ii]/xiC[i]; + y_min_number = ii; + } + } + } + } + + /* 2) Bounds. */ + for( i=0; i ZERO ) && ( y[ii] >= 0.0 ) ) + { + if ( y[ii]/xiB[i] < y_min ) + { + y_min = y[ii]/xiB[i]; + y_min_number = ii; + y_min_isBound = BT_TRUE; + } + } + } + else + { + if ( ( xiB[i] < -ZERO ) && ( y[ii] <= 0.0 ) ) + { + if ( y[ii]/xiB[i] < y_min ) + { + y_min = y[ii]/xiB[i]; + y_min_number = ii; + y_min_isBound = BT_TRUE; + } + } + } + } + + /* setup output preferences */ + #ifdef PC_DEBUG + char messageString[80]; + VisibilityStatus visibilityStatus; + + if ( printlevel == PL_HIGH ) + visibilityStatus = VS_VISIBLE; + else + visibilityStatus = VS_HIDDEN; + #endif + + + /* IV) REMOVE CONSTRAINT/BOUND FOR RESOLVING LINEAR DEPENDENCE: */ + if ( y_min_number >= 0 ) + { + /* 1) Check for cycling due to infeasibility. */ + if ( ( cyclingManager.getCyclingStatus( number,BT_FALSE ) == CYC_PREV_REMOVED ) && + ( cyclingManager.getCyclingStatus( y_min_number,y_min_isBound ) == CYC_PREV_ADDED ) ) + { + infeasible = BT_TRUE; + + return THROWERROR( RET_ENSURELI_FAILED_CYCLING ); + } + else + { + /* set cycling data */ + cyclingManager.clearCyclingData( ); + cyclingManager.setCyclingStatus( number,BT_FALSE, CYC_PREV_ADDED ); + cyclingManager.setCyclingStatus( y_min_number,y_min_isBound, CYC_PREV_REMOVED ); + } + + /* 2) Update Lagrange multiplier... */ + for( i=0; ithrowInfo( RET_REMOVE_FROM_ACTIVESET,messageString,__FUNCTION__,__FILE__,__LINE__,visibilityStatus ); + #endif + + if ( removeBound( y_min_number,BT_TRUE ) != SUCCESSFUL_RETURN ) + return THROWERROR( RET_REMOVE_FROM_ACTIVESET_FAILED ); + + y[y_min_number] = 0.0; + } + else + { + #ifdef PC_DEBUG + sprintf( messageString,"constraint no. %d.",y_min_number ); + getGlobalMessageHandler( )->throwInfo( RET_REMOVE_FROM_ACTIVESET,messageString,__FUNCTION__,__FILE__,__LINE__,visibilityStatus ); + #endif + + if ( removeConstraint( y_min_number,BT_TRUE ) != SUCCESSFUL_RETURN ) + return THROWERROR( RET_REMOVE_FROM_ACTIVESET_FAILED ); + + y[nV+y_min_number] = 0.0; + } + } + else + { + /* no constraint/bound can be removed => QP is infeasible! */ + infeasible = BT_TRUE; + + return THROWERROR( RET_ENSURELI_FAILED_NOINDEX ); + } + + return getGlobalMessageHandler( )->throwInfo( RET_LI_RESOLVED,0,__FUNCTION__,__FILE__,__LINE__,VS_HIDDEN ); +} + + + +/* + * a d d B o u n d + */ +returnValue QProblem::addBound( int number, SubjectToStatus B_status, + BooleanType updateCholesky + ) +{ + int i, j, ii; + + /* consistency checks */ + if ( bounds.getStatus( number ) != ST_INACTIVE ) + return THROWERROR( RET_BOUND_ALREADY_ACTIVE ); + + if ( getNFR( ) == bounds.getNUV( ) ) + return THROWERROR( RET_ALL_BOUNDS_ACTIVE ); + + if ( ( getStatus( ) == QPS_NOTINITIALISED ) || + ( getStatus( ) == QPS_AUXILIARYQPSOLVED ) || + ( getStatus( ) == QPS_HOMOTOPYQPSOLVED ) || + ( getStatus( ) == QPS_SOLVED ) ) + { + return THROWERROR( RET_UNKNOWN_BUG ); + } + + + /* I) ENSURE LINEAR INDEPENDENCE OF THE WORKING SET, + * i.e. remove a constraint or bound if linear dependence occurs. */ + /* check for LI only if Cholesky decomposition shall be updated! */ + if ( updateCholesky == BT_TRUE ) + { + returnValue ensureLIreturnvalue = addBound_ensureLI( number,B_status ); + + switch ( ensureLIreturnvalue ) + { + case SUCCESSFUL_RETURN: + break; + + case RET_LI_RESOLVED: + break; + + case RET_ENSURELI_FAILED_NOINDEX: + return THROWERROR( RET_ADDBOUND_FAILED_INFEASIBILITY ); + + case RET_ENSURELI_FAILED_CYCLING: + return THROWERROR( RET_ADDBOUND_FAILED_INFEASIBILITY ); + + default: + return THROWERROR( RET_ENSURELI_FAILED ); + } + } + + + /* some definitions */ + int nFR = getNFR( ); + int nAC = getNAC( ); + int nZ = getNZ( ); + + int tcol = sizeT - nAC; + + + /* I) SWAP INDEXLIST OF FREE VARIABLES: + * move the variable to be fixed to the end of the list of free variables. */ + int lastfreenumber = bounds.getFree( )->getLastNumber( ); + if ( lastfreenumber != number ) + if ( bounds.swapFree( number,lastfreenumber ) != SUCCESSFUL_RETURN ) + THROWERROR( RET_ADDBOUND_FAILED ); + + + int FR_idx[NVMAX]; + if ( bounds.getFree( )->getNumberArray( FR_idx ) != SUCCESSFUL_RETURN ) + return THROWERROR( RET_ADDBOUND_FAILED ); + + real_t w[NVMAX]; + + + /* II) ADD NEW ACTIVE BOUND TO TOP OF MATRIX T: */ + /* 1) add row [wZ wY] = [Z Y](number) at the top of T: assign w */ + for( i=0; i 0 ) /* ( nAC == 0 ) <=> ( nZ == nFR ) <=> Y and T are empty => nothing to do */ + { + /* store new column a in a temporary vector instead of shifting T one column to the left */ + real_t tmp[NCMAX_ALLOC]; + for( i=0; i is linearly independent from the + the active ones (<=> is element of null space of Afr). */ + for( i=0; i EPS ) + return RET_LINEARLY_INDEPENDENT; + } + + return RET_LINEARLY_DEPENDENT; +} + + +/* + * a d d B o u n d _ e n s u r e L I + */ +returnValue QProblem::addBound_ensureLI( int number, SubjectToStatus B_status ) +{ + int i, j, ii, jj; + int nV = getNV( ); + int nFX = getNFX( ); + int nAC = getNAC( ); + int nZ = getNZ( ); + + + /* I) Check if new constraint is linearly independent from the active ones. */ + returnValue returnvalueCheckLI = addBound_checkLI( number ); + + if ( returnvalueCheckLI == RET_LINEARLY_INDEPENDENT ) + return SUCCESSFUL_RETURN; + + + /* II) NEW BOUND IS LINEARLY DEPENDENT: */ + /* 1) Determine coefficients of linear combination, + * cf. M.J. Best. Applied Mathematics and Parallel Computing, chapter: + * An Algorithm for the Solution of the Parametric Quadratic Programming + * Problem, pages 57-76. Physica-Verlag, Heidelberg, 1996. */ + int FR_idx[NVMAX]; + if ( bounds.getFree( )->getNumberArray( FR_idx ) != SUCCESSFUL_RETURN ) + return THROWERROR( RET_ENSURELI_FAILED ); + + int FX_idx[NVMAX]; + if ( bounds.getFixed( )->getNumberArray( FX_idx ) != SUCCESSFUL_RETURN ) + return THROWERROR( RET_ENSURELI_FAILED ); + + int AC_idx[NCMAX_ALLOC]; + if ( constraints.getActive( )->getNumberArray( AC_idx ) != SUCCESSFUL_RETURN ) + return THROWERROR( RET_ENSURELI_FAILED ); + + real_t xiC[NCMAX_ALLOC]; + real_t xiC_TMP[NCMAX_ALLOC]; + real_t xiB[NVMAX]; + + /* 2) Calculate xiC. */ + if ( nAC > 0 ) + { + if ( B_status == ST_LOWER ) + { + for( i=0; i ZERO ) && ( y[nV+ii] >= 0.0 ) ) + { + if ( y[nV+ii]/xiC[i] < y_min ) + { + y_min = y[nV+ii]/xiC[i]; + y_min_number = ii; + } + } + } + else + { + if ( ( xiC[i] < -ZERO ) && ( y[nV+ii] <= 0.0 ) ) + { + if ( y[nV+ii]/xiC[i] < y_min ) + { + y_min = y[nV+ii]/xiC[i]; + y_min_number = ii; + } + } + } + } + + /* 2) Bounds. */ + for( i=0; i ZERO ) && ( y[ii] >= 0.0 ) ) + { + if ( y[ii]/xiB[i] < y_min ) + { + y_min = y[ii]/xiB[i]; + y_min_number = ii; + y_min_isBound = BT_TRUE; + } + } + } + else + { + if ( ( xiB[i] < -ZERO ) && ( y[ii] <= 0.0 ) ) + { + if ( y[ii]/xiB[i] < y_min ) + { + y_min = y[ii]/xiB[i]; + y_min_number = ii; + y_min_isBound = BT_TRUE; + } + } + } + } + + /* setup output preferences */ + #ifdef PC_DEBUG + char messageString[80]; + VisibilityStatus visibilityStatus; + + if ( printlevel == PL_HIGH ) + visibilityStatus = VS_VISIBLE; + else + visibilityStatus = VS_HIDDEN; + #endif + + + /* IV) REMOVE CONSTRAINT/BOUND FOR RESOLVING LINEAR DEPENDENCE: */ + if ( y_min_number >= 0 ) + { + /* 1) Check for cycling due to infeasibility. */ + if ( ( cyclingManager.getCyclingStatus( number,BT_TRUE ) == CYC_PREV_REMOVED ) && + ( cyclingManager.getCyclingStatus( y_min_number,y_min_isBound ) == CYC_PREV_ADDED ) ) + { + infeasible = BT_TRUE; + + return THROWERROR( RET_ENSURELI_FAILED_CYCLING ); + } + else + { + /* set cycling data */ + cyclingManager.clearCyclingData( ); + cyclingManager.setCyclingStatus( number,BT_TRUE, CYC_PREV_ADDED ); + cyclingManager.setCyclingStatus( y_min_number,y_min_isBound, CYC_PREV_REMOVED ); + } + + + /* 2) Update Lagrange multiplier... */ + for( i=0; ithrowInfo( RET_REMOVE_FROM_ACTIVESET,messageString,__FUNCTION__,__FILE__,__LINE__,visibilityStatus ); + #endif + + if ( removeBound( y_min_number,BT_TRUE ) != SUCCESSFUL_RETURN ) + return THROWERROR( RET_REMOVE_FROM_ACTIVESET_FAILED ); + + y[y_min_number] = 0.0; + } + else + { + #ifdef PC_DEBUG + sprintf( messageString,"constraint no. %d.",y_min_number ); + getGlobalMessageHandler( )->throwInfo( RET_REMOVE_FROM_ACTIVESET,messageString,__FUNCTION__,__FILE__,__LINE__,visibilityStatus ); + #endif + + if ( removeConstraint( y_min_number,BT_TRUE ) != SUCCESSFUL_RETURN ) + return THROWERROR( RET_REMOVE_FROM_ACTIVESET_FAILED ); + + y[nV+y_min_number] = 0.0; + } + } + else + { + /* no constraint/bound can be removed => QP is infeasible! */ + infeasible = BT_TRUE; + + return THROWERROR( RET_ENSURELI_FAILED_NOINDEX ); + } + + return getGlobalMessageHandler( )->throwInfo( RET_LI_RESOLVED,0,__FUNCTION__,__FILE__,__LINE__,VS_HIDDEN ); +} + + + +/* + * r e m o v e C o n s t r a i n t + */ +returnValue QProblem::removeConstraint( int number, + BooleanType updateCholesky + ) +{ + int i, j, ii, jj; + + /* consistency check */ + if ( ( getStatus( ) == QPS_NOTINITIALISED ) || + ( getStatus( ) == QPS_AUXILIARYQPSOLVED ) || + ( getStatus( ) == QPS_HOMOTOPYQPSOLVED ) || + ( getStatus( ) == QPS_SOLVED ) ) + { + return THROWERROR( RET_UNKNOWN_BUG ); + } + + /* some definitions */ + int nFR = getNFR( ); + int nAC = getNAC( ); + int nZ = getNZ( ); + + int tcol = sizeT - nAC; + int number_idx = constraints.getActive( )->getIndex( number ); + + + /* consistency checks */ + if ( constraints.getStatus( number ) == ST_INACTIVE ) + return THROWERROR( RET_CONSTRAINT_NOT_ACTIVE ); + + if ( ( number_idx < 0 ) || ( number_idx >= nAC ) ) + return THROWERROR( RET_CONSTRAINT_NOT_ACTIVE ); + + + int FR_idx[NVMAX]; + if ( bounds.getFree( )->getNumberArray( FR_idx ) != SUCCESSFUL_RETURN ) + return THROWERROR( RET_REMOVECONSTRAINT_FAILED ); + + + /* I) REMOVE th ROW FROM T, + * i.e. shift rows number+1 through nAC upwards (instead of the actual + * constraint number its corresponding index within matrix A is used). */ + if ( number_idx < nAC-1 ) + { + for( i=(number_idx+1); i=0; --j ) + { + computeGivens( T[(nAC-2-j)*NVMAX + tcol+1+j],T[(nAC-2-j)*NVMAX + tcol+j], T[(nAC-2-j)*NVMAX + tcol+1+j],T[(nAC-2-j)*NVMAX + tcol+j],c,s ); + + for( i=(nAC-j-1); i<(nAC-1); ++i ) + applyGivens( c,s,T[i*NVMAX + tcol+1+j],T[i*NVMAX + tcol+j], T[i*NVMAX + tcol+1+j],T[i*NVMAX + tcol+j] ); + + for( i=0; i 0 ) + { + real_t ZHz[NVMAX]; + for ( i=0; i 0.0 ) + R[nZ*NVMAX + nZ] = sqrt( rho2 ); + else + { + hessianType = HST_SEMIDEF; + return THROWERROR( RET_HESSIAN_NOT_SPD ); + } + } + + /* IV) UPDATE INDICES */ + if ( constraints.moveActiveToInactive( number ) != SUCCESSFUL_RETURN ) + return THROWERROR( RET_REMOVECONSTRAINT_FAILED ); + + + return SUCCESSFUL_RETURN; +} + + +/* + * r e m o v e B o u n d + */ +returnValue QProblem::removeBound( int number, + BooleanType updateCholesky + ) +{ + int i, j, ii, jj; + + /* consistency checks */ + if ( bounds.getStatus( number ) == ST_INACTIVE ) + return THROWERROR( RET_BOUND_NOT_ACTIVE ); + + if ( ( getStatus( ) == QPS_NOTINITIALISED ) || + ( getStatus( ) == QPS_AUXILIARYQPSOLVED ) || + ( getStatus( ) == QPS_HOMOTOPYQPSOLVED ) || + ( getStatus( ) == QPS_SOLVED ) ) + { + return THROWERROR( RET_UNKNOWN_BUG ); + } + + /* some definitions */ + int nFR = getNFR( ); + int nAC = getNAC( ); + int nZ = getNZ( ); + + int tcol = sizeT - nAC; + + + /* I) UPDATE INDICES */ + if ( bounds.moveFixedToFree( number ) != SUCCESSFUL_RETURN ) + return THROWERROR( RET_REMOVEBOUND_FAILED ); + + + int FR_idx[NVMAX]; + if ( bounds.getFree( )->getNumberArray( FR_idx ) != SUCCESSFUL_RETURN ) + return THROWERROR( RET_REMOVEBOUND_FAILED ); + + /* I) APPEND th UNITY VECOTR TO Q. */ + int nnFRp1 = FR_idx[nFR]; + for( i=0; i 0 ) + { + /* store new column a in a temporary vector instead of shifting T one column to the left and appending a */ + int AC_idx[NCMAX_ALLOC]; + if ( constraints.getActive( )->getNumberArray( AC_idx ) != SUCCESSFUL_RETURN ) + return THROWERROR( RET_REMOVEBOUND_FAILED ); + + real_t tmp[NCMAX_ALLOC]; + for( i=0; i=0; --j ) + { + computeGivens( tmp[nAC-1-j],T[(nAC-1-j)*NVMAX + tcol+j],T[(nAC-1-j)*NVMAX + tcol+j],tmp[nAC-1-j],c,s ); + + for( i=(nAC-j); i 0 ) + { + real_t Hz[NVMAX]; + for( i=0; i 0 ) + { + real_t r[NVMAX]; + real_t rhs[NVMAX]; + for( i=0; i 0.0 ) + R[nZ*NVMAX + nZ] = sqrt( rho2 ); + else + { + hessianType = HST_SEMIDEF; + return THROWERROR( RET_HESSIAN_NOT_SPD ); + } + } + + return SUCCESSFUL_RETURN; +} + + +/* + * b a c k s o l v e R (CODE DUPLICATE OF QProblemB CLASS!!!) + */ +returnValue QProblem::backsolveR( const real_t* const b, BooleanType transposed, + real_t* const a + ) +{ + /* Call standard backsolve procedure (i.e. removingBound == BT_FALSE). */ + return backsolveR( b,transposed,BT_FALSE,a ); +} + + +/* + * b a c k s o l v e R (CODE DUPLICATE OF QProblemB CLASS!!!) + */ +returnValue QProblem::backsolveR( const real_t* const b, BooleanType transposed, + BooleanType removingBound, + real_t* const a + ) +{ + int i, j; + int nR = getNZ( ); + + real_t sum; + + /* if backsolve is called while removing a bound, reduce nZ by one. */ + if ( removingBound == BT_TRUE ) + --nR; + + /* nothing to do */ + if ( nR <= 0 ) + return SUCCESSFUL_RETURN; + + + /* Solve Ra = b, where R might be transposed. */ + if ( transposed == BT_FALSE ) + { + /* solve Ra = b */ + for( i=(nR-1); i>=0; --i ) + { + sum = b[i]; + for( j=(i+1); j ZERO ) + a[i] = sum / R[i*NVMAX + i]; + else + return THROWERROR( RET_DIV_BY_ZERO ); + } + } + else + { + /* solve R^T*a = b */ + for( i=0; i ZERO ) + a[i] = sum / R[i*NVMAX + i]; + else + return THROWERROR( RET_DIV_BY_ZERO ); + } + } + + return SUCCESSFUL_RETURN; +} + + + +/* + * b a c k s o l v e T + */ +returnValue QProblem::backsolveT( const real_t* const b, BooleanType transposed, real_t* const a ) +{ + int i, j; + int nT = getNAC( ); + int tcol = sizeT - nT; + + real_t sum; + + /* nothing to do */ + if ( nT <= 0 ) + return SUCCESSFUL_RETURN; + + + /* Solve Ta = b, where T might be transposed. */ + if ( transposed == BT_FALSE ) + { + /* solve Ta = b */ + for( i=0; i ZERO ) + a[nT-1-i] = sum / T[i*NVMAX + sizeT-1-i]; + else + return THROWERROR( RET_DIV_BY_ZERO ); + } + } + else + { + /* solve T^T*a = b */ + for( i=0; i ZERO ) + a[nT-1-i] = sum / T[(nT-1-i)*NVMAX + tcol+i]; + else + return THROWERROR( RET_DIV_BY_ZERO ); + } + } + + + return SUCCESSFUL_RETURN; +} + + +/* + * h o t s t a r t _ d e t e r m i n e D a t a S h i f t + */ +returnValue QProblem::hotstart_determineDataShift( const int* const FX_idx, const int* const AC_idx, + const real_t* const g_new, const real_t* const lbA_new, const real_t* const ubA_new, + const real_t* const lb_new, const real_t* const ub_new, + real_t* const delta_g, real_t* const delta_lbA, real_t* const delta_ubA, + real_t* const delta_lb, real_t* const delta_ub, + BooleanType& Delta_bC_isZero, BooleanType& Delta_bB_isZero + ) +{ + int i, ii; + int nC = getNC( ); + int nAC = getNAC( ); + + + /* I) DETERMINE DATA SHIFT FOR BOUNDS */ + QProblemB::hotstart_determineDataShift( FX_idx,g_new,lb_new,ub_new, delta_g,delta_lb,delta_ub, Delta_bB_isZero ); + + + /* II) DETERMINE DATA SHIFT FOR CONSTRAINTS */ + /* 1) Calculate shift directions. */ + for( i=0; i EPS ) || ( getAbs( delta_ubA[ii] ) > EPS ) ) + { + Delta_bC_isZero = BT_FALSE; + break; + } + } + + return SUCCESSFUL_RETURN; +} + + +/* + * h o t s t a r t _ d e t e r m i n e S t e p D i r e c t i o n + */ +returnValue QProblem::hotstart_determineStepDirection( const int* const FR_idx, const int* const FX_idx, const int* const AC_idx, + const real_t* const delta_g, const real_t* const delta_lbA, const real_t* const delta_ubA, + const real_t* const delta_lb, const real_t* const delta_ub, + BooleanType Delta_bC_isZero, BooleanType Delta_bB_isZero, + real_t* const delta_xFX, real_t* const delta_xFR, + real_t* const delta_yAC, real_t* const delta_yFX + ) +{ + int i, j, ii, jj; + int nFR = getNFR( ); + int nFX = getNFX( ); + int nAC = getNAC( ); + int nZ = getNZ( ); + + /* initialise auxiliary vectors */ + real_t HMX_delta_xFX[NVMAX]; + real_t YFR_delta_xFRy[NVMAX]; + real_t ZFR_delta_xFRz[NVMAX]; + real_t HFR_YFR_delta_xFRy[NVMAX]; + for( i=0; i 0 ) + { + for( i=0; i 0 ) + { + /* 1) Determine delta_xFRy. */ + if ( nAC > 0 ) + { + if ( ( Delta_bC_isZero == BT_TRUE ) && ( Delta_bB_isZero == BT_TRUE ) ) + { + for( i=0; i 0 ) + { + for( i=0; i 0 ) + { + if ( ( Delta_bC_isZero == BT_FALSE ) || ( Delta_bB_isZero == BT_FALSE ) ) + { + for( i=0; i 0 ) + { + /* auxiliary variable */ + real_t delta_xFRz_TMP[NVMAX]; + real_t delta_xFRz_RHS[NVMAX]; + + + if ( ( nAC > 0 ) && ( nFX > 0 ) && ( Delta_bB_isZero == BT_FALSE ) ) + { + for( j=0; j 0 ) /* => Delta_bB_isZero == BT_TRUE, as BT_FALSE would imply nFX>0 */ + { + for( j=0; j 0 ) /* => ( nFR = nZ + nAC > 0 ) */ + { + /* auxiliary variables */ + real_t delta_yAC_TMP[NCMAX_ALLOC]; + for( i=0; i 0 ) + { + for( i=0; i 0 ) + { + if ( Delta_bB_isZero == BT_FALSE ) + { + for( i=0; i 0 ) + { + for( i=0; i 0.0 ) + maxStepLength[nV+ii] = y[nV+ii] / ( -delta_yAC[i] ); + else + maxStepLength[nV+ii] = 0.0; + } + } + else + { + /* active upper constraints' bounds */ + if ( delta_yAC[i] > ZERO ) + { + if ( y[nV+ii] < 0.0 ) + maxStepLength[nV+ii] = y[nV+ii] / ( -delta_yAC[i] ); + else + maxStepLength[nV+ii] = 0.0; + } + } + } + } + + /* 2) Ensure that active dual bounds remain valid + * (ignoring implicitly fixed variables). */ + for( i=0; i 0.0 ) + maxStepLength[ii] = y[ii] / ( -delta_yFX[i] ); + else + maxStepLength[ii] = 0.0; + } + } + else + { + /* active upper bounds */ + if ( delta_yFX[i] > ZERO ) + { + if ( y[ii] < 0.0 ) + maxStepLength[ii] = y[ii] / ( -delta_yFX[i] ); + else + maxStepLength[ii] = 0.0; + } + } + } + } + + + /* II) DETERMINE MAXIMUM PRIMAL STEPLENGTH */ + /* 1) Ensure that inactive constraints' bounds remain valid + * (ignoring unbounded constraints). */ + real_t delta_x[NVMAX]; + for( j=0; j delta_Ax[ii] ) + { + if ( Ax[ii] > lbA[ii] ) + maxStepLength[nV+ii] = ( Ax[ii] - lbA[ii] ) / ( delta_lbA[ii] - delta_Ax[ii] ); + else + maxStepLength[nV+ii] = 0.0; + } + } + + /* inactive upper constraints' bounds */ + if ( constraints.isNoUpper( ) == BT_FALSE ) + { + if ( delta_ubA[ii] < delta_Ax[ii] ) + { + if ( Ax[ii] < ubA[ii] ) + maxStepLength[nV+nC+nV+ii] = ( Ax[ii] - ubA[ii] ) / ( delta_ubA[ii] - delta_Ax[ii] ); + else + maxStepLength[nV+nC+nV+ii] = 0.0; + } + } + } + } + + + /* 2) Ensure that inactive bounds remain valid + * (ignoring unbounded variables). */ + /* inactive lower bounds */ + if ( bounds.isNoLower( ) == BT_FALSE ) + { + for( i=0; i delta_xFR[i] ) + { + if ( x[ii] > lb[ii] ) + maxStepLength[ii] = ( x[ii] - lb[ii] ) / ( delta_lb[ii] - delta_xFR[i] ); + else + maxStepLength[ii] = 0.0; + } + } + } + + /* inactive upper bounds */ + if ( bounds.isNoUpper( ) == BT_FALSE ) + { + for( i=0; i EPS ) + cyclingManager.clearCyclingData( ); + + + /* V) SET MAXIMUM HOMOTOPY STEPLENGTH */ + tau = tau_new; + + #ifdef PC_DEBUG + if ( printlevel == PL_HIGH ) + { + + char messageString[80]; + + if ( BC_status == ST_UNDEFINED ) + sprintf( messageString,"Stepsize is %.6e!",tau ); + else + sprintf( messageString,"Stepsize is %.6e! (BC_idx = %d, BC_isBound = %d, BC_status = %d)",tau,BC_idx,BC_isBound,BC_status ); + + getGlobalMessageHandler( )->throwInfo( RET_STEPSIZE_NONPOSITIVE,messageString,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE ); + } + #endif + + return SUCCESSFUL_RETURN; +} + + +/* + * h o t s t a r t _ p e r f o r m S t e p + */ +returnValue QProblem::hotstart_performStep( const int* const FR_idx, const int* const FX_idx, const int* const AC_idx, const int* const IAC_idx, + const real_t* const delta_g, const real_t* const delta_lbA, const real_t* const delta_ubA, + const real_t* const delta_lb, const real_t* const delta_ub, + const real_t* const delta_xFX, const real_t* const delta_xFR, + const real_t* const delta_yAC, const real_t* const delta_yFX, + const real_t* const delta_Ax, int BC_idx, SubjectToStatus BC_status, BooleanType BC_isBound + ) +{ + int i, j, ii; + int nV = getNV( ); + int nC = getNC( ); + int nFR = getNFR( ); + int nFX = getNFX( ); + int nAC = getNAC( ); + int nIAC = getNIAC( ); + + + /* I) CHECK (CONSTRAINTS') BOUNDS' CONSISTENCY */ + if ( areBoundsConsistent( delta_lb,delta_ub,delta_lbA,delta_ubA ) == BT_FALSE ) + { + infeasible = BT_TRUE; + tau = 0.0; + + return THROWERROR( RET_QP_INFEASIBLE ); + } + + + /* II) GO TO ACTIVE SET CHANGE */ + if ( tau > ZERO ) + { + /* 1) Perform step in primal und dual space... */ + for( i=0; ithrowWarning( RET_STEPSIZE,messageString,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE ); + #endif + } + + + /* setup output preferences */ + #ifdef PC_DEBUG + char messageString[80]; + VisibilityStatus visibilityStatus; + + if ( printlevel == PL_HIGH ) + visibilityStatus = VS_VISIBLE; + else + visibilityStatus = VS_HIDDEN; + #endif + + + /* III) UPDATE ACTIVE SET */ + switch ( BC_status ) + { + /* Optimal solution found as no working set change detected. */ + case ST_UNDEFINED: + return RET_OPTIMAL_SOLUTION_FOUND; + + + /* Remove one variable from active set. */ + case ST_INACTIVE: + if ( BC_isBound == BT_TRUE ) + { + #ifdef PC_DEBUG + sprintf( messageString,"bound no. %d.", BC_idx ); + getGlobalMessageHandler( )->throwInfo( RET_REMOVE_FROM_ACTIVESET,messageString,__FUNCTION__,__FILE__,__LINE__,visibilityStatus ); + #endif + + if ( removeBound( BC_idx,BT_TRUE ) != SUCCESSFUL_RETURN ) + return THROWERROR( RET_REMOVE_FROM_ACTIVESET_FAILED ); + + y[BC_idx] = 0.0; + } + else + { + #ifdef PC_DEBUG + sprintf( messageString,"constraint no. %d.", BC_idx ); + getGlobalMessageHandler( )->throwInfo( RET_REMOVE_FROM_ACTIVESET,messageString,__FUNCTION__,__FILE__,__LINE__,visibilityStatus ); + #endif + + if ( removeConstraint( BC_idx,BT_TRUE ) != SUCCESSFUL_RETURN ) + return THROWERROR( RET_REMOVE_FROM_ACTIVESET_FAILED ); + + y[nV+BC_idx] = 0.0; + } + break; + + + /* Add one variable to active set. */ + default: + if ( BC_isBound == BT_TRUE ) + { + #ifdef PC_DEBUG + if ( BC_status == ST_LOWER ) + sprintf( messageString,"lower bound no. %d.", BC_idx ); + else + sprintf( messageString,"upper bound no. %d.", BC_idx ); + getGlobalMessageHandler( )->throwInfo( RET_ADD_TO_ACTIVESET,messageString,__FUNCTION__,__FILE__,__LINE__,visibilityStatus ); + #endif + + if ( addBound( BC_idx,BC_status,BT_TRUE ) != SUCCESSFUL_RETURN ) + return THROWERROR( RET_ADD_TO_ACTIVESET_FAILED ); + } + else + { + #ifdef PC_DEBUG + if ( BC_status == ST_LOWER ) + sprintf( messageString,"lower constraint's bound no. %d.", BC_idx ); + else + sprintf( messageString,"upper constraint's bound no. %d.", BC_idx ); + getGlobalMessageHandler( )->throwInfo( RET_ADD_TO_ACTIVESET,messageString,__FUNCTION__,__FILE__,__LINE__,visibilityStatus ); + #endif + + if ( addConstraint( BC_idx,BC_status,BT_TRUE ) != SUCCESSFUL_RETURN ) + return THROWERROR( RET_ADD_TO_ACTIVESET_FAILED ); + } + break; + } + + return SUCCESSFUL_RETURN; +} + + +/* + * a r e B o u n d s C o n s i s t e n t + */ +BooleanType QProblem::areBoundsConsistent( const real_t* const delta_lb, const real_t* const delta_ub, + const real_t* const delta_lbA, const real_t* const delta_ubA + ) const +{ + int i; + + /* 1) Check bounds' consistency. */ + if ( QProblemB::areBoundsConsistent( delta_lb,delta_ub ) == BT_FALSE ) + return BT_FALSE; + + /* 2) Check constraints' consistency, i.e. + * check if delta_lb[i] is greater than delta_ub[i] + * for a component i whose bounds are already (numerically) equal. */ + for( i=0; i ubA[i] - BOUNDTOL ) && ( delta_lbA[i] > delta_ubA[i] + EPS ) ) + return BT_FALSE; + + return BT_TRUE; +} + + +/* + * s e t u p Q P d a t a + */ +returnValue QProblem::setupQPdata( const real_t* const _H, const real_t* const _R, const real_t* const _g, const real_t* const _A, + const real_t* const _lb, const real_t* const _ub, + const real_t* const _lbA, const real_t* const _ubA + ) +{ + int i, j; + int nV = getNV( ); + int nC = getNC( ); + + + /* 1) Load Hessian matrix as well as lower and upper bounds vectors. */ + if (QProblemB::setupQPdata(_H, _R, _g, _lb, _ub) != SUCCESSFUL_RETURN) + return THROWERROR( RET_INVALID_ARGUMENTS ); + + /* 2) Load constraint matrix. */ + if ( ( nC > 0 ) && ( _A == 0 ) ) + return THROWERROR( RET_INVALID_ARGUMENTS ); + + if ( nC > 0 ) + { + for( i=0; igetNumberArray( AC_idx ); + + /* 1) check for Hx + g - [yFX yAC]*[Id A]' = 0. */ + for( i=0; i maxKKTviolation ) + maxKKTviolation = getAbs( tmp ); + } + + /* 2) Check for [lb lbA] <= [Id A]*x <= [ub ubA]. */ + /* lbA <= Ax <= ubA */ + for( i=0; i maxKKTviolation ) + maxKKTviolation = lbA[i] - Ax[i]; + + if ( Ax[i] - ubA[i] > maxKKTviolation ) + maxKKTviolation = Ax[i] - ubA[i]; + } + + /* lb <= x <= ub */ + for( i=0; i maxKKTviolation ) + maxKKTviolation = lb[i] - x[i]; + + if ( x[i] - ub[i] > maxKKTviolation ) + maxKKTviolation = x[i] - ub[i]; + } + + /* 3) Check for correct sign of y and for complementary slackness. */ + /* bounds */ + for( i=0; i maxKKTviolation ) + maxKKTviolation = -y[i]; + if ( getAbs( x[i] - lb[i] ) > maxKKTviolation ) + maxKKTviolation = getAbs( x[i] - lb[i] ); + break; + + case ST_UPPER: + if ( y[i] > maxKKTviolation ) + maxKKTviolation = y[i]; + if ( getAbs( ub[i] - x[i] ) > maxKKTviolation ) + maxKKTviolation = getAbs( ub[i] - x[i] ); + break; + + default: /* inactive */ + if ( getAbs( y[i] ) > maxKKTviolation ) + maxKKTviolation = getAbs( y[i] ); + break; + } + } + + /* constraints */ + for( i=0; i maxKKTviolation ) + maxKKTviolation = -y[nV+i]; + if ( getAbs( Ax[i] - lbA[i] ) > maxKKTviolation ) + maxKKTviolation = getAbs( Ax[i] - lbA[i] ); + break; + + case ST_UPPER: + if ( y[nV+i] > maxKKTviolation ) + maxKKTviolation = y[nV+i]; + if ( getAbs( ubA[i] - Ax[i] ) > maxKKTviolation ) + maxKKTviolation = getAbs( ubA[i] - Ax[i] ); + break; + + default: /* inactive */ + if ( getAbs( y[nV+i] ) > maxKKTviolation ) + maxKKTviolation = getAbs( y[nV+i] ); + break; + } + } + + if ( maxKKTviolation > CRITICALACCURACY ) + return RET_NO_SOLUTION; + + if ( maxKKTviolation > DESIREDACCURACY ) + return RET_INACCURATE_SOLUTION; + + #endif /* __PERFORM_KKT_TEST__ */ + + return SUCCESSFUL_RETURN; +} + + +/* + * end of file + */ diff --git a/phonelibs/qpoases/SRC/QProblem.ipp b/phonelibs/qpoases/SRC/QProblem.ipp new file mode 100644 index 00000000000000..cec58e291ddeab --- /dev/null +++ b/phonelibs/qpoases/SRC/QProblem.ipp @@ -0,0 +1,299 @@ +/* + * This file is part of qpOASES. + * + * qpOASES -- An Implementation of the Online Active Set Strategy. + * Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved. + * + * qpOASES is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * qpOASES is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with qpOASES; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + + +/** + * \file SRC/QProblem.ipp + * \author Hans Joachim Ferreau + * \version 1.3embedded + * \date 2007-2008 + * + * Implementation of inlined member functions of the QProblem class which + * is able to use the newly developed online active set strategy for + * parametric quadratic programming. + */ + + + +/***************************************************************************** + * P U B L I C * + *****************************************************************************/ + +/* + * g e t A + */ +inline returnValue QProblem::getA( real_t* const _A ) const +{ + int i; + + for ( i=0; i= 0 ) && ( number < getNC( ) ) ) + { + for ( int i=0; i= 0 ) && ( number < getNC( ) ) ) + { + value = lbA[number]; + return SUCCESSFUL_RETURN; + } + else + return THROWERROR( RET_INDEX_OUT_OF_BOUNDS ); +} + + +/* + * g e t U B A + */ +inline returnValue QProblem::getUBA( real_t* const _ubA ) const +{ + int i; + + for ( i=0; i= 0 ) && ( number < getNC( ) ) ) + { + value = ubA[number]; + return SUCCESSFUL_RETURN; + } + else + return THROWERROR( RET_INDEX_OUT_OF_BOUNDS ); +} + + +/* + * g e t C o n s t r a i n t s + */ +inline returnValue QProblem::getConstraints( Constraints* const _constraints ) const +{ + *_constraints = constraints; + + return SUCCESSFUL_RETURN; +} + + + +/* + * g e t N C + */ +inline int QProblem::getNC( ) const +{ + return constraints.getNC( ); +} + + +/* + * g e t N E C + */ +inline int QProblem::getNEC( ) const +{ + return constraints.getNEC( ); +} + + +/* + * g e t N A C + */ +inline int QProblem::getNAC( ) +{ + return constraints.getNAC( ); +} + + +/* + * g e t N I A C + */ +inline int QProblem::getNIAC( ) +{ + return constraints.getNIAC( ); +} + + + +/***************************************************************************** + * P R O T E C T E D * + *****************************************************************************/ + + +/* + * s e t A + */ +inline returnValue QProblem::setA( const real_t* const A_new ) +{ + int i, j; + int nV = getNV( ); + int nC = getNC( ); + + /* Set constraint matrix AND update member AX. */ + for( j=0; j= 0 ) && ( number < getNC( ) ) ) + { + Ax[number] = 0.0; + + for( i=0; i= 0 ) && ( number < getNC( ) ) ) + { + lbA[number] = value; + return SUCCESSFUL_RETURN; + } + else + return THROWERROR( RET_INDEX_OUT_OF_BOUNDS ); +} + + +/* + * s e t U B A + */ +inline returnValue QProblem::setUBA( const real_t* const ubA_new ) +{ + int i; + int nC = getNC(); + + for( i=0; i= 0 ) && ( number < getNC( ) ) ) + { + ubA[number] = value; + return SUCCESSFUL_RETURN; + } + else + return THROWERROR( RET_INDEX_OUT_OF_BOUNDS ); +} + + +/* + * end of file + */ diff --git a/phonelibs/qpoases/SRC/QProblemB.cpp b/phonelibs/qpoases/SRC/QProblemB.cpp new file mode 100644 index 00000000000000..f84daabc025d3f --- /dev/null +++ b/phonelibs/qpoases/SRC/QProblemB.cpp @@ -0,0 +1,2151 @@ +/* + * This file is part of qpOASES. + * + * qpOASES -- An Implementation of the Online Active Set Strategy. + * Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved. + * + * qpOASES is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * qpOASES is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with qpOASES; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + + +/** + * \file SRC/QProblemB.cpp + * \author Hans Joachim Ferreau + * \version 1.3embedded + * \date 2007-2008 + * + * Implementation of the QProblemB class which is able to use the newly + * developed online active set strategy for parametric quadratic programming. + */ + + +#include + +#include + +void printmatrix(char *name, double *A, int m, int n) { + int i, j; + + printf("%s = [...\n", name); + for (i = 0; i < m; i++) { + for (j = 0; j < n; j++) + printf(" % 9.4f", A[i*n+j]); + printf(",\n"); + } + printf("];\n"); +} + + + +/***************************************************************************** + * P U B L I C * + *****************************************************************************/ + + +/* + * Q P r o b l e m B + */ +QProblemB::QProblemB( ) +{ + /* reset global message handler */ + getGlobalMessageHandler( )->reset( ); + + hasHessian = BT_FALSE; + + bounds.init( 0 ); + + hasCholesky = BT_FALSE; + + tau = 0.0; + + hessianType = HST_POSDEF_NULLSPACE; /* Hessian is assumed to be positive definite by default */ + infeasible = BT_FALSE; + unbounded = BT_FALSE; + + status = QPS_NOTINITIALISED; + + #ifdef PC_DEBUG + printlevel = PL_MEDIUM; + setPrintLevel( PL_MEDIUM ); + #else + printlevel = QPOASES_PRINTLEVEL; + #endif + + count = 0; +} + + +/* + * Q P r o b l e m B + */ +QProblemB::QProblemB( int _nV ) +{ + /* consistency check */ + if ( _nV <= 0 ) + { + _nV = 1; + THROWERROR( RET_INVALID_ARGUMENTS ); + } + + hasHessian = BT_FALSE; + + /* reset global message handler */ + getGlobalMessageHandler( )->reset( ); + + bounds.init( _nV ); + + hasCholesky = BT_FALSE; + + tau = 0.0; + + hessianType = HST_POSDEF_NULLSPACE; /* Hessian is assumed to be positive definite by default */ + infeasible = BT_FALSE; + unbounded = BT_FALSE; + + status = QPS_NOTINITIALISED; + + #ifdef PC_DEBUG + printlevel = PL_MEDIUM; + setPrintLevel( PL_MEDIUM ); + #else + printlevel = QPOASES_PRINTLEVEL; + #endif + + count = 0; +} + + +/* + * Q P r o b l e m B + */ +QProblemB::QProblemB( const QProblemB& rhs ) +{ + int i, j; + + int _nV = rhs.bounds.getNV( ); + + for( i=0; i<_nV; ++i ) + for( j=0; j<_nV; ++j ) + H[i*NVMAX + j] = rhs.H[i*NVMAX + j]; + + hasHessian = rhs.hasHessian; + + for( i=0; i<_nV; ++i ) + g[i] = rhs.g[i]; + + for( i=0; i<_nV; ++i ) + lb[i] = rhs.lb[i]; + + for( i=0; i<_nV; ++i ) + ub[i] = rhs.ub[i]; + + + bounds = rhs.bounds; + + for( i=0; i<_nV; ++i ) + for( j=0; j<_nV; ++j ) + R[i*NVMAX + j] = rhs.R[i*NVMAX + j]; + hasCholesky = rhs.hasCholesky; + + for( i=0; i<_nV; ++i ) + x[i] = rhs.x[i]; + + for( i=0; i<_nV; ++i ) + y[i] = rhs.y[i]; + + tau = rhs.tau; + + hessianType = rhs.hessianType; + infeasible = rhs.infeasible; + unbounded = rhs.unbounded; + + status = rhs.status; + + printlevel = rhs.printlevel; + + count = rhs.count; +} + + +/* + * ~ Q P r o b l e m B + */ +QProblemB::~QProblemB( ) +{ +} + + +/* + * o p e r a t o r = + */ +QProblemB& QProblemB::operator=( const QProblemB& rhs ) +{ + int i, j; + + if ( this != &rhs ) + { + int _nV = rhs.bounds.getNV( ); + + for( i=0; i<_nV; ++i ) + for( j=0; j<_nV; ++j ) + H[i*NVMAX + j] = rhs.H[i*NVMAX + j]; + + hasHessian = rhs.hasHessian; + + for( i=0; i<_nV; ++i ) + g[i] = rhs.g[i]; + + for( i=0; i<_nV; ++i ) + lb[i] = rhs.lb[i]; + + for( i=0; i<_nV; ++i ) + ub[i] = rhs.ub[i]; + + bounds = rhs.bounds; + + for( i=0; i<_nV; ++i ) + for( j=0; j<_nV; ++j ) + R[i*NVMAX + j] = rhs.R[i*NVMAX + j]; + hasCholesky = rhs.hasCholesky; + + + for( i=0; i<_nV; ++i ) + x[i] = rhs.x[i]; + + for( i=0; i<_nV; ++i ) + y[i] = rhs.y[i]; + + tau = rhs.tau; + + hessianType = rhs.hessianType; + infeasible = rhs.infeasible; + unbounded = rhs.unbounded; + + status = rhs.status; + + printlevel = rhs.printlevel; + setPrintLevel( rhs.printlevel ); + + count = rhs.count; + } + + return *this; +} + + +/* + * r e s e t + */ +returnValue QProblemB::reset( ) +{ + int i, j; + int nV = getNV( ); + + /** 0) Reset has Hessian flag. */ + hasHessian = BT_FALSE; + + /* 1) Reset bounds. */ + bounds.init( nV ); + + /* 2) Reset Cholesky decomposition. */ + for( i=0; ithrowInfo( RET_ITERATION_STARTED,messageString,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE ); + #endif + } + + /* 1) Setup index arrays. */ + if ( bounds.getFree( )->getNumberArray( FR_idx ) != SUCCESSFUL_RETURN ) + return THROWERROR( RET_HOTSTART_FAILED ); + + if ( bounds.getFixed( )->getNumberArray( FX_idx ) != SUCCESSFUL_RETURN ) + return THROWERROR( RET_HOTSTART_FAILED ); + + /* 2) Initialize shift direction of the gradient and the bounds. */ + returnvalue = hotstart_determineDataShift( FX_idx, + g_new,lb_new,ub_new, + delta_g,delta_lb,delta_ub, + Delta_bB_isZero + ); + if ( returnvalue != SUCCESSFUL_RETURN ) + { + nWSR = l; + THROWERROR( RET_SHIFT_DETERMINATION_FAILED ); + return returnvalue; + } + + /* 3) Determination of step direction of X and Y. */ + returnvalue = hotstart_determineStepDirection( FR_idx,FX_idx, + delta_g,delta_lb,delta_ub, + Delta_bB_isZero, + delta_xFX,delta_xFR,delta_yFX + ); + if ( returnvalue != SUCCESSFUL_RETURN ) + { + nWSR = l; + THROWERROR( RET_STEPDIRECTION_DETERMINATION_FAILED ); + return returnvalue; + } + + + /* 4) Determination of step length TAU. */ + returnvalue = hotstart_determineStepLength( FR_idx,FX_idx, + delta_lb,delta_ub, + delta_xFR,delta_yFX, + BC_idx,BC_status ); + if ( returnvalue != SUCCESSFUL_RETURN ) + { + nWSR = l; + THROWERROR( RET_STEPLENGTH_DETERMINATION_FAILED ); + return returnvalue; + } + + /* 5) Realization of the homotopy step. */ + returnvalue = hotstart_performStep( FR_idx,FX_idx, + delta_g,delta_lb,delta_ub, + delta_xFX,delta_xFR,delta_yFX, + BC_idx,BC_status + ); + + + if ( returnvalue != SUCCESSFUL_RETURN ) + { + nWSR = l; + + /* stop runtime measurement */ + if ( cputime != 0 ) + *cputime = getCPUtime( ) - starttime; + + /* optimal solution found? */ + if ( returnvalue == RET_OPTIMAL_SOLUTION_FOUND ) + { + status = QPS_SOLVED; + + if ( printlevel == PL_HIGH ) + THROWINFO( RET_OPTIMAL_SOLUTION_FOUND ); + + #ifdef PC_DEBUG + if ( printIteration( l,BC_idx,BC_status ) != SUCCESSFUL_RETURN ) + THROWERROR( RET_PRINT_ITERATION_FAILED ); /* do not pass this as return value! */ + #endif + + /* check KKT optimality conditions */ + return checkKKTconditions( ); + } + else + { + /* checks for infeasibility... */ + if ( infeasible == BT_TRUE ) + { + status = QPS_HOMOTOPYQPSOLVED; + return THROWERROR( RET_HOTSTART_STOPPED_INFEASIBILITY ); + } + + /* ...unboundedness... */ + if ( unbounded == BT_TRUE ) /* not necessary since objective function convex! */ + return THROWERROR( RET_HOTSTART_STOPPED_UNBOUNDEDNESS ); + + /* ... and throw unspecific error otherwise */ + THROWERROR( RET_HOMOTOPY_STEP_FAILED ); + return returnvalue; + } + } + + /* 6) Output information of successful QP iteration. */ + status = QPS_HOMOTOPYQPSOLVED; + + #ifdef PC_DEBUG + if ( printIteration( l,BC_idx,BC_status ) != SUCCESSFUL_RETURN ) + THROWERROR( RET_PRINT_ITERATION_FAILED ); /* do not pass this as return value! */ + #endif + } + + + /* stop runtime measurement */ + if ( cputime != 0 ) + *cputime = getCPUtime( ) - starttime; + + + /* if programm gets to here, output information that QP could not be solved + * within the given maximum numbers of working set changes */ + if ( printlevel == PL_HIGH ) + { + #ifdef PC_DEBUG + sprintf( messageString,"(nWSR = %d)",nWSR ); + return getGlobalMessageHandler( )->throwWarning( RET_MAX_NWSR_REACHED,messageString,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE ); + #endif + } + + /* Finally check KKT optimality conditions. */ + returnValue returnvalueKKTcheck = checkKKTconditions( ); + + if ( returnvalueKKTcheck != SUCCESSFUL_RETURN ) + return returnvalueKKTcheck; + else + return RET_MAX_NWSR_REACHED; +} + + +/* + * g e t N Z + */ +int QProblemB::getNZ( ) +{ + /* if no constraints are present: nZ=nFR */ + return bounds.getFree( )->getLength( ); +} + + +/* + * g e t O b j V a l + */ +real_t QProblemB::getObjVal( ) const +{ + real_t objVal; + + /* calculated optimal objective function value + * only if current QP has been solved */ + if ( ( getStatus( ) == QPS_AUXILIARYQPSOLVED ) || + ( getStatus( ) == QPS_HOMOTOPYQPSOLVED ) || + ( getStatus( ) == QPS_SOLVED ) ) + { + objVal = getObjVal( x ); + } + else + { + objVal = INFTY; + } + + return objVal; +} + + +/* + * g e t O b j V a l + */ +real_t QProblemB::getObjVal( const real_t* const _x ) const +{ + int i, j; + int nV = getNV( ); + + real_t obj_tmp = 0.0; + + for( i=0; i= PL_MEDIUM ) && ( printlevel != _printlevel ) ) + THROWINFO( RET_PRINTLEVEL_CHANGED ); + #endif + + printlevel = _printlevel; + + /* update message handler preferences */ + switch ( printlevel ) + { + case PL_NONE: + getGlobalMessageHandler( )->setErrorVisibilityStatus( VS_HIDDEN ); + getGlobalMessageHandler( )->setWarningVisibilityStatus( VS_HIDDEN ); + getGlobalMessageHandler( )->setInfoVisibilityStatus( VS_HIDDEN ); + break; + + case PL_LOW: + getGlobalMessageHandler( )->setErrorVisibilityStatus( VS_VISIBLE ); + getGlobalMessageHandler( )->setWarningVisibilityStatus( VS_HIDDEN ); + getGlobalMessageHandler( )->setInfoVisibilityStatus( VS_HIDDEN ); + break; + + default: /* PL_MEDIUM, PL_HIGH */ + getGlobalMessageHandler( )->setErrorVisibilityStatus( VS_VISIBLE ); + getGlobalMessageHandler( )->setWarningVisibilityStatus( VS_VISIBLE ); + getGlobalMessageHandler( )->setInfoVisibilityStatus( VS_VISIBLE ); + break; + } + + return SUCCESSFUL_RETURN; +} + + + +/***************************************************************************** + * P R O T E C T E D * + *****************************************************************************/ + +/* + * c h e c k F o r I d e n t i t y H e s s i a n + */ +returnValue QProblemB::checkForIdentityHessian( ) +{ + int i, j; + int nV = getNV( ); + + /* nothing to do as status flag remains unaltered + * if Hessian differs from identity matrix */ + if ( hessianType == HST_IDENTITY ) + return SUCCESSFUL_RETURN; + + /* 1) If Hessian differs from identity matrix, + * return without changing the internal HessianType. */ + for ( i=0; i EPS ) + return SUCCESSFUL_RETURN; + + for ( i=0; i EPS ) || ( getAbs( H[j*NVMAX + i] ) > EPS ) ) + return SUCCESSFUL_RETURN; + } + + /* 2) If this point is reached, Hessian equals the idetity matrix. */ + hessianType = HST_IDENTITY; + + return SUCCESSFUL_RETURN; +} + + +/* + * s e t u p S u b j e c t T o T y p e + */ +returnValue QProblemB::setupSubjectToType( ) +{ + int i; + int nV = getNV( ); + + + /* 1) Check if lower bounds are present. */ + bounds.setNoLower( BT_TRUE ); + for( i=0; i -INFTY ) + { + bounds.setNoLower( BT_FALSE ); + break; + } + + /* 2) Check if upper bounds are present. */ + bounds.setNoUpper( BT_TRUE ); + for( i=0; i INFTY - BOUNDTOL ) ) + { + bounds.setType( i,ST_UNBOUNDED ); + ++nUV; + } + else + { + if ( lb[i] > ub[i] - BOUNDTOL ) + { + bounds.setType( i,ST_EQUALITY ); + ++nFV; + } + else + { + bounds.setType( i,ST_BOUNDED ); + } + } + + /* 4) Set dimensions of bounds structure. */ + bounds.setNFV( nFV ); + bounds.setNUV( nUV ); + bounds.setNBV( nV - nFV - nUV ); + + return SUCCESSFUL_RETURN; +} + + +/* + * c h o l e s k y D e c o m p o s i t i o n + */ +returnValue QProblemB::setupCholeskyDecomposition( ) +{ + int i, j, k, ii, jj; + int nV = getNV( ); + int nFR = getNFR( ); + + /* If Hessian flag is false, it means that H & R already contain Cholesky + * factorization -- provided from outside. */ + if (hasHessian == BT_FALSE) + return SUCCESSFUL_RETURN; + + /* 1) Initialises R with all zeros. */ + for( i=0; i 0 ) + { + int FR_idx[NVMAX]; + if ( bounds.getFree( )->getNumberArray( FR_idx ) != SUCCESSFUL_RETURN ) + return THROWERROR( RET_INDEXLIST_CORRUPTED ); + + /* R'*R = H */ + real_t sum; + real_t inv; + + for( i=0; i=0; --k ) + sum -= R[k*NVMAX + i] * R[k*NVMAX + i]; + + if ( sum > 0.0 ) + { + R[i*NVMAX + i] = sqrt( sum ); + inv = 1.0 / R[i*NVMAX + i]; + } + else + { + hessianType = HST_SEMIDEF; + return THROWERROR( RET_HESSIAN_NOT_SPD ); + } + + /* j > i */ + for( j=(i+1); j=0; --k ) + sum -= R[k*NVMAX + i] * R[k*NVMAX + j]; + + R[i*NVMAX + j] = sum * inv; + } + } + } + } + + return SUCCESSFUL_RETURN; +} + + +/* + * s o l v e I n i t i a l Q P + */ +returnValue QProblemB::solveInitialQP( const real_t* const xOpt, const real_t* const yOpt, + const Bounds* const guessedBounds, + int& nWSR, real_t* const cputime + ) +{ + int i, nFR; + int nV = getNV( ); + + + /* start runtime measurement */ + real_t starttime = 0.0; + if ( cputime != 0 ) + starttime = getCPUtime( ); + + + status = QPS_NOTINITIALISED; + + /* I) ANALYSE QP DATA: */ + /* 1) Check if Hessian happens to be the identity matrix. */ + if ( checkForIdentityHessian( ) != SUCCESSFUL_RETURN ) + return THROWERROR( RET_INIT_FAILED ); + + /* 2) Setup type of bounds (i.e. unbounded, implicitly fixed etc.). */ + if ( setupSubjectToType( ) != SUCCESSFUL_RETURN ) + return THROWERROR( RET_INIT_FAILED ); + + status = QPS_PREPARINGAUXILIARYQP; + + + /* II) SETUP AUXILIARY QP WITH GIVEN OPTIMAL SOLUTION: */ + /* 1) Setup bounds data structure. */ + if ( bounds.setupAllFree( ) != SUCCESSFUL_RETURN ) + return THROWERROR( RET_INIT_FAILED ); + + /* 2) Setup optimal primal/dual solution for auxiliary QP. */ + if ( setupAuxiliaryQPsolution( xOpt,yOpt ) != SUCCESSFUL_RETURN ) + return THROWERROR( RET_INIT_FAILED ); + + /* 3) Obtain linear independent working set for auxiliary QP. */ + + static Bounds auxiliaryBounds; + + auxiliaryBounds.init( nV ); + + if ( obtainAuxiliaryWorkingSet( xOpt,yOpt,guessedBounds, &auxiliaryBounds ) != SUCCESSFUL_RETURN ) + return THROWERROR( RET_INIT_FAILED ); + + /* 4) Setup working set of auxiliary QP and setup cholesky decomposition. */ + if ( setupAuxiliaryWorkingSet( &auxiliaryBounds,BT_TRUE ) != SUCCESSFUL_RETURN ) + return THROWERROR( RET_INIT_FAILED ); + + nFR = getNFR(); + /* At the moment we can only provide a Cholesky of the Hessian if + * the solver is cold-started. */ + if (hasCholesky == BT_FALSE || nFR != nV) + if (setupCholeskyDecomposition() != SUCCESSFUL_RETURN) + return THROWERROR( RET_INIT_FAILED_CHOLESKY ); + + /* 5) Store original QP formulation... */ + real_t g_original[NVMAX]; + real_t lb_original[NVMAX]; + real_t ub_original[NVMAX]; + + for( i=0; isetupBound( i,ST_LOWER ) != SUCCESSFUL_RETURN ) + return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED ); + } + else + { + if ( auxiliaryBounds->setupBound( i,guessedBounds->getStatus( i ) ) != SUCCESSFUL_RETURN ) + return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED ); + } + } + } + else /* No initial working set specified. */ + { + if ( ( xOpt != 0 ) && ( yOpt == 0 ) ) + { + /* Obtain initial working set by "clipping". */ + for( i=0; isetupBound( i,ST_LOWER ) != SUCCESSFUL_RETURN ) + return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED ); + continue; + } + + if ( xOpt[i] >= ub[i] - BOUNDTOL ) + { + if ( auxiliaryBounds->setupBound( i,ST_UPPER ) != SUCCESSFUL_RETURN ) + return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED ); + continue; + } + + /* Moreover, add all implictly fixed variables if specified. */ + if ( bounds.getType( i ) == ST_EQUALITY ) + { + if ( auxiliaryBounds->setupBound( i,ST_LOWER ) != SUCCESSFUL_RETURN ) + return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED ); + } + else + { + if ( auxiliaryBounds->setupBound( i,ST_INACTIVE ) != SUCCESSFUL_RETURN ) + return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED ); + } + } + } + + if ( ( xOpt == 0 ) && ( yOpt != 0 ) ) + { + /* Obtain initial working set in accordance to sign of dual solution vector. */ + for( i=0; i ZERO ) + { + if ( auxiliaryBounds->setupBound( i,ST_LOWER ) != SUCCESSFUL_RETURN ) + return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED ); + continue; + } + + if ( yOpt[i] < -ZERO ) + { + if ( auxiliaryBounds->setupBound( i,ST_UPPER ) != SUCCESSFUL_RETURN ) + return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED ); + continue; + } + + /* Moreover, add all implictly fixed variables if specified. */ + if ( bounds.getType( i ) == ST_EQUALITY ) + { + if ( auxiliaryBounds->setupBound( i,ST_LOWER ) != SUCCESSFUL_RETURN ) + return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED ); + } + else + { + if ( auxiliaryBounds->setupBound( i,ST_INACTIVE ) != SUCCESSFUL_RETURN ) + return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED ); + } + } + } + + /* If xOpt and yOpt are null pointer and no initial working is specified, + * start with empty working set (or implicitly fixed bounds only) + * for auxiliary QP. */ + if ( ( xOpt == 0 ) && ( yOpt == 0 ) ) + { + for( i=0; isetupBound( i,ST_LOWER ) != SUCCESSFUL_RETURN ) + return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED ); + } + else + { + if ( auxiliaryBounds->setupBound( i,ST_INACTIVE ) != SUCCESSFUL_RETURN ) + return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED ); + } + } + } + } + + return SUCCESSFUL_RETURN; +} + + +/* + * s e t u p A u x i l i a r y W o r k i n g S e t + */ +returnValue QProblemB::setupAuxiliaryWorkingSet( const Bounds* const auxiliaryBounds, + BooleanType setupAfresh + ) +{ + int i; + int nV = getNV( ); + + /* consistency checks */ + if ( auxiliaryBounds != 0 ) + { + for( i=0; igetStatus( i ) == ST_UNDEFINED ) ) + return THROWERROR( RET_UNKNOWN_BUG ); + } + else + { + return THROWERROR( RET_INVALID_ARGUMENTS ); + } + + + /* I) SETUP CHOLESKY FLAG: + * Cholesky decomposition shall only be updated if working set + * shall be updated (i.e. NOT setup afresh!) */ + BooleanType updateCholesky; + if ( setupAfresh == BT_TRUE ) + updateCholesky = BT_FALSE; + else + updateCholesky = BT_TRUE; + + + /* II) REMOVE FORMERLY ACTIVE BOUNDS (IF NECESSARY): */ + if ( setupAfresh == BT_FALSE ) + { + /* Remove all active bounds that shall be inactive AND + * all active bounds that are active at the wrong bound. */ + for( i=0; igetStatus( i ) != ST_LOWER ) ) + if ( removeBound( i,updateCholesky ) != SUCCESSFUL_RETURN ) + return THROWERROR( RET_SETUP_WORKINGSET_FAILED ); + + if ( ( bounds.getStatus( i ) == ST_UPPER ) && ( auxiliaryBounds->getStatus( i ) != ST_UPPER ) ) + if ( removeBound( i,updateCholesky ) != SUCCESSFUL_RETURN ) + return THROWERROR( RET_SETUP_WORKINGSET_FAILED ); + } + } + + + /* III) ADD NEWLY ACTIVE BOUNDS: */ + /* Add all inactive bounds that shall be active AND + * all formerly active bounds that have been active at the wrong bound. */ + for( i=0; igetStatus( i ) != ST_INACTIVE ) ) + { + if ( addBound( i,auxiliaryBounds->getStatus( i ),updateCholesky ) != SUCCESSFUL_RETURN ) + return THROWERROR( RET_SETUP_WORKINGSET_FAILED ); + } + } + + return SUCCESSFUL_RETURN; +} + + +/* + * s e t u p A u x i l i a r y Q P s o l u t i o n + */ +returnValue QProblemB::setupAuxiliaryQPsolution( const real_t* const xOpt, const real_t* const yOpt + ) +{ + int i; + int nV = getNV( ); + + + /* Setup primal/dual solution vectors for auxiliary initial QP: + * if a null pointer is passed, a zero vector is assigned; + * old solution vector is kept if pointer to internal solution vector is passed. */ + if ( xOpt != 0 ) + { + if ( xOpt != x ) + for( i=0; igetIndex( number ); + + real_t c, s; + + /* 2) Use row-wise Givens rotations to restore upper triangular form of R. */ + for( i=number_idx+1; ith column and ... */ + for( i=0; igetNumberArray( FR_idx ) != SUCCESSFUL_RETURN ) + return THROWERROR( RET_REMOVEBOUND_FAILED ); + + /* 1) Calculate new column of cholesky decomposition. */ + real_t rhs[NVMAX]; + real_t r[NVMAX]; + real_t r0 = H[number*NVMAX + number]; + + for( i=0; i 0.0 ) + R[nFR*NVMAX + nFR] = sqrt( r0 ); + else + { + hessianType = HST_SEMIDEF; + return THROWERROR( RET_HESSIAN_NOT_SPD ); + } + + + return SUCCESSFUL_RETURN; +} + + +/* + * b a c k s o l v e R (CODE DUPLICATED IN QProblem CLASS!!!) + */ +returnValue QProblemB::backsolveR( const real_t* const b, BooleanType transposed, + real_t* const a + ) +{ + /* Call standard backsolve procedure (i.e. removingBound == BT_FALSE). */ + return backsolveR( b,transposed,BT_FALSE,a ); +} + + +/* + * b a c k s o l v e R (CODE DUPLICATED IN QProblem CLASS!!!) + */ +returnValue QProblemB::backsolveR( const real_t* const b, BooleanType transposed, + BooleanType removingBound, + real_t* const a + ) +{ + int i, j; + int nR = getNZ( ); + + real_t sum; + + /* if backsolve is called while removing a bound, reduce nZ by one. */ + if ( removingBound == BT_TRUE ) + --nR; + + /* nothing to do */ + if ( nR <= 0 ) + return SUCCESSFUL_RETURN; + + + /* Solve Ra = b, where R might be transposed. */ + if ( transposed == BT_FALSE ) + { + /* solve Ra = b */ + for( i=(nR-1); i>=0; --i ) + { + sum = b[i]; + for( j=(i+1); j ZERO ) + a[i] = sum / R[i*NVMAX + i]; + else + return THROWERROR( RET_DIV_BY_ZERO ); + } + } + else + { + /* solve R^T*a = b */ + for( i=0; i ZERO ) + a[i] = sum / R[i*NVMAX + i]; + else + return THROWERROR( RET_DIV_BY_ZERO ); + } + } + + return SUCCESSFUL_RETURN; +} + + +/* + * h o t s t a r t _ d e t e r m i n e D a t a S h i f t + */ +returnValue QProblemB::hotstart_determineDataShift( const int* const FX_idx, + const real_t* const g_new, const real_t* const lb_new, const real_t* const ub_new, + real_t* const delta_g, real_t* const delta_lb, real_t* const delta_ub, + BooleanType& Delta_bB_isZero + ) +{ + int i, ii; + int nV = getNV( ); + int nFX = getNFX( ); + + + /* 1) Calculate shift directions. */ + for( i=0; i EPS ) || ( getAbs( delta_ub[ii] ) > EPS ) ) + { + Delta_bB_isZero = BT_FALSE; + break; + } + } + + return SUCCESSFUL_RETURN; +} + + +/* + * a r e B o u n d s C o n s i s t e n t + */ +BooleanType QProblemB::areBoundsConsistent( const real_t* const delta_lb, const real_t* const delta_ub + ) const +{ + int i; + + /* Check if delta_lb[i] is greater than delta_ub[i] + * for a component i whose bounds are already (numerically) equal. */ + for( i=0; i ub[i] - BOUNDTOL ) && ( delta_lb[i] > delta_ub[i] + EPS ) ) + return BT_FALSE; + + return BT_TRUE; +} + + +/* + * s e t u p Q P d a t a + */ +returnValue QProblemB::setupQPdata( const real_t* const _H, const real_t* const _R, const real_t* const _g, + const real_t* const _lb, const real_t* const _ub + ) +{ + int i, j; + int nV = getNV( ); + + /* 1) Setup Hessian matrix and it's Cholesky factorization. */ + if (_H != 0) + { + for( i=0; i 0 ) + { + for( i=0; i 0 ) + { + /* auxiliary variables */ + real_t delta_xFRz_TMP[NVMAX]; + real_t delta_xFRz_RHS[NVMAX]; + + /* Determine delta_xFRz. */ + if ( Delta_bB_isZero == BT_FALSE ) + { + for( i=0; i 0 ) + { + for( i=0; i= 0.0 ) ) + { + tau_tmp = y[ii] / ( -delta_yFX[i] ); + if ( tau_tmp < tau_new ) + { + if ( tau_tmp >= 0.0 ) + { + tau_new = tau_tmp; + BC_idx = ii; + BC_status = ST_INACTIVE; + } + } + } + } + else + { + /* 2) Active upper bounds. */ + if ( ( delta_yFX[i] > ZERO ) && ( y[ii] <= 0.0 ) ) + { + tau_tmp = y[ii] / ( -delta_yFX[i] ); + if ( tau_tmp < tau_new ) + { + if ( tau_tmp >= 0.0 ) + { + tau_new = tau_tmp; + BC_idx = ii; + BC_status = ST_INACTIVE; + } + } + } + } + } + } + + + /* II) DETERMINE MAXIMUM PRIMAL STEPLENGTH, i.e. ensure that + * inactive bounds remain valid (ignoring unbounded variables). */ + /* 1) Inactive lower bounds. */ + if ( bounds.isNoLower( ) == BT_FALSE ) + { + for( i=0; i delta_xFR[i] ) + { + if ( x[ii] > lb[ii] ) + tau_tmp = ( x[ii] - lb[ii] ) / ( delta_lb[ii] - delta_xFR[i] ); + else + tau_tmp = 0.0; + + if ( tau_tmp < tau_new ) + { + if ( tau_tmp >= 0.0 ) + { + tau_new = tau_tmp; + BC_idx = ii; + BC_status = ST_LOWER; + } + } + } + } + } + } + + /* 2) Inactive upper bounds. */ + if ( bounds.isNoUpper( ) == BT_FALSE ) + { + for( i=0; i= 0.0 ) + { + tau_new = tau_tmp; + BC_idx = ii; + BC_status = ST_UPPER; + } + } + } + } + } + } + + + /* III) SET MAXIMUM HOMOTOPY STEPLENGTH */ + tau = tau_new; + + if ( printlevel == PL_HIGH ) + { + #ifdef PC_DEBUG + char messageString[80]; + + if ( BC_status == ST_UNDEFINED ) + sprintf( messageString,"Stepsize is %.6e!",tau ); + else + sprintf( messageString,"Stepsize is %.6e! (BC_idx = %d, BC_status = %d)",tau,BC_idx,BC_status ); + + getGlobalMessageHandler( )->throwInfo( RET_STEPSIZE_NONPOSITIVE,messageString,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE ); + #endif + } + + return SUCCESSFUL_RETURN; +} + + +/* + * h o t s t a r t _ p e r f o r m S t e p + */ +returnValue QProblemB::hotstart_performStep( const int* const FR_idx, const int* const FX_idx, + const real_t* const delta_g, const real_t* const delta_lb, const real_t* const delta_ub, + const real_t* const delta_xFX, const real_t* const delta_xFR, + const real_t* const delta_yFX, + int BC_idx, SubjectToStatus BC_status + ) +{ + int i, ii; + int nV = getNV( ); + int nFR = getNFR( ); + int nFX = getNFX( ); + + + /* I) CHECK BOUNDS' CONSISTENCY */ + if ( areBoundsConsistent( delta_lb,delta_ub ) == BT_FALSE ) + { + infeasible = BT_TRUE; + tau = 0.0; + + return THROWERROR( RET_QP_INFEASIBLE ); + } + + + /* II) GO TO ACTIVE SET CHANGE */ + if ( tau > ZERO ) + { + /* 1) Perform step in primal und dual space. */ + for( i=0; ithrowWarning( RET_STEPSIZE,messageString,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE ); + #endif + } + + + /* setup output preferences */ + #ifdef PC_DEBUG + char messageString[80]; + VisibilityStatus visibilityStatus; + + if ( printlevel == PL_HIGH ) + visibilityStatus = VS_VISIBLE; + else + visibilityStatus = VS_HIDDEN; + #endif + + + /* III) UPDATE ACTIVE SET */ + switch ( BC_status ) + { + /* Optimal solution found as no working set change detected. */ + case ST_UNDEFINED: + return RET_OPTIMAL_SOLUTION_FOUND; + + + /* Remove one variable from active set. */ + case ST_INACTIVE: + #ifdef PC_DEBUG + sprintf( messageString,"bound no. %d.", BC_idx ); + getGlobalMessageHandler( )->throwInfo( RET_REMOVE_FROM_ACTIVESET,messageString,__FUNCTION__,__FILE__,__LINE__,visibilityStatus ); + #endif + + if ( removeBound( BC_idx,BT_TRUE ) != SUCCESSFUL_RETURN ) + return THROWERROR( RET_REMOVE_FROM_ACTIVESET_FAILED ); + + y[BC_idx] = 0.0; + break; + + + /* Add one variable to active set. */ + default: + #ifdef PC_DEBUG + if ( BC_status == ST_LOWER ) + sprintf( messageString,"lower bound no. %d.", BC_idx ); + else + sprintf( messageString,"upper bound no. %d.", BC_idx ); + getGlobalMessageHandler( )->throwInfo( RET_ADD_TO_ACTIVESET,messageString,__FUNCTION__,__FILE__,__LINE__,visibilityStatus ); + #endif + + if ( addBound( BC_idx,BC_status,BT_TRUE ) != SUCCESSFUL_RETURN ) + return THROWERROR( RET_ADD_TO_ACTIVESET_FAILED ); + break; + } + + return SUCCESSFUL_RETURN; +} + + +#ifdef PC_DEBUG /* Define print functions only for debugging! */ + +/* + * p r i n t I t e r a t i o n + */ +returnValue QProblemB::printIteration( int iteration, + int BC_idx, SubjectToStatus BC_status + ) +{ + char myPrintfString[160]; + + /* consistency check */ + if ( iteration < 0 ) + return THROWERROR( RET_INVALID_ARGUMENTS ); + + /* nothing to do */ + if ( printlevel != PL_MEDIUM ) + return SUCCESSFUL_RETURN; + + + /* 1) Print header at first iteration. */ + if ( iteration == 0 ) + { + sprintf( myPrintfString,"\n############## qpOASES -- QP NO.%4.1d ###############\n", count ); + myPrintf( myPrintfString ); + + sprintf( myPrintfString," Iter | StepLength | Info | nFX \n" ); + myPrintf( myPrintfString ); + } + + /* 2) Print iteration line. */ + if ( BC_status == ST_UNDEFINED ) + { + sprintf( myPrintfString," %4.1d | %1.5e | QP SOLVED | %4.1d \n", iteration,tau,getNFX( ) ); + myPrintf( myPrintfString ); + } + else + { + char info[8]; + + if ( BC_status == ST_INACTIVE ) + sprintf( info,"REM BND" ); + else + sprintf( info,"ADD BND" ); + + sprintf( myPrintfString," %4.1d | %1.5e | %s%4.1d | %4.1d \n", iteration,tau,info,BC_idx,getNFX( ) ); + myPrintf( myPrintfString ); + } + + return SUCCESSFUL_RETURN; +} + +#endif /* PC_DEBUG */ + + + +/* + * c h e c k K K T c o n d i t i o n s + */ +returnValue QProblemB::checkKKTconditions( ) +{ + #ifdef __PERFORM_KKT_TEST__ + + int i, j; + int nV = getNV( ); + + real_t tmp; + real_t maxKKTviolation = 0.0; + + + /* 1) Check for Hx + g - y*A' = 0 (here: A = Id). */ + for( i=0; i maxKKTviolation ) + maxKKTviolation = getAbs( tmp ); + } + + /* 2) Check for lb <= x <= ub. */ + for( i=0; i maxKKTviolation ) + maxKKTviolation = lb[i] - x[i]; + + if ( x[i] - ub[i] > maxKKTviolation ) + maxKKTviolation = x[i] - ub[i]; + } + + /* 3) Check for correct sign of y and for complementary slackness. */ + for( i=0; i maxKKTviolation ) + maxKKTviolation = -y[i]; + if ( getAbs( ( x[i] - lb[i] ) * y[i] ) > maxKKTviolation ) + maxKKTviolation = getAbs( ( x[i] - lb[i] ) * y[i] ); + break; + + case ST_UPPER: + if ( y[i] > maxKKTviolation ) + maxKKTviolation = y[i]; + if ( getAbs( ( ub[i] - x[i] ) * y[i] ) > maxKKTviolation ) + maxKKTviolation = getAbs( ( ub[i] - x[i] ) * y[i] ); + break; + + default: /* inactive */ + if ( getAbs( y[i] ) > maxKKTviolation ) + maxKKTviolation = getAbs( y[i] ); + break; + } + } + + if ( maxKKTviolation > CRITICALACCURACY ) + return RET_NO_SOLUTION; + + if ( maxKKTviolation > DESIREDACCURACY ) + return RET_INACCURATE_SOLUTION; + + #endif /* __PERFORM_KKT_TEST__ */ + + return SUCCESSFUL_RETURN; +} + + + +/* + * end of file + */ diff --git a/phonelibs/qpoases/SRC/QProblemB.ipp b/phonelibs/qpoases/SRC/QProblemB.ipp new file mode 100644 index 00000000000000..0c1c8b231147b5 --- /dev/null +++ b/phonelibs/qpoases/SRC/QProblemB.ipp @@ -0,0 +1,425 @@ +/* + * This file is part of qpOASES. + * + * qpOASES -- An Implementation of the Online Active Set Strategy. + * Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved. + * + * qpOASES is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * qpOASES is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with qpOASES; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + + +/** + * \file SRC/QProblemB.ipp + * \author Hans Joachim Ferreau + * \version 1.3embedded + * \date 2007-2008 + * + * Implementation of inlined member functions of the QProblemB class which + * is able to use the newly developed online active set strategy for + * parametric quadratic programming. + */ + + + +#include + + + +/***************************************************************************** + * P U B L I C * + *****************************************************************************/ + +/* + * g e t H + */ +inline returnValue QProblemB::getH( real_t* const _H ) const +{ + int i; + + for ( i=0; i= 0 ) && ( number < getNV( ) ) ) + { + value = lb[number]; + return SUCCESSFUL_RETURN; + } + else + { + return THROWERROR( RET_INDEX_OUT_OF_BOUNDS ); + } +} + + +/* + * g e t U B + */ +inline returnValue QProblemB::getUB( real_t* const _ub ) const +{ + int i; + + for ( i=0; i= 0 ) && ( number < getNV( ) ) ) + { + value = ub[number]; + return SUCCESSFUL_RETURN; + } + else + { + return THROWERROR( RET_INDEX_OUT_OF_BOUNDS ); + } +} + + +/* + * g e t B o u n d s + */ +inline returnValue QProblemB::getBounds( Bounds* const _bounds ) const +{ + *_bounds = bounds; + + return SUCCESSFUL_RETURN; +} + + +/* + * g e t N V + */ +inline int QProblemB::getNV( ) const +{ + return bounds.getNV( ); +} + + +/* + * g e t N F R + */ +inline int QProblemB::getNFR( ) +{ + return bounds.getNFR( ); +} + + +/* + * g e t N F X + */ +inline int QProblemB::getNFX( ) +{ + return bounds.getNFX( ); +} + + +/* + * g e t N F V + */ +inline int QProblemB::getNFV( ) const +{ + return bounds.getNFV( ); +} + + +/* + * g e t S t a t u s + */ +inline QProblemStatus QProblemB::getStatus( ) const +{ + return status; +} + + +/* + * i s I n i t i a l i s e d + */ +inline BooleanType QProblemB::isInitialised( ) const +{ + if ( status == QPS_NOTINITIALISED ) + return BT_FALSE; + else + return BT_TRUE; +} + + +/* + * i s S o l v e d + */ +inline BooleanType QProblemB::isSolved( ) const +{ + if ( status == QPS_SOLVED ) + return BT_TRUE; + else + return BT_FALSE; +} + + +/* + * i s I n f e a s i b l e + */ +inline BooleanType QProblemB::isInfeasible( ) const +{ + return infeasible; +} + + +/* + * i s U n b o u n d e d + */ +inline BooleanType QProblemB::isUnbounded( ) const +{ + return unbounded; +} + + +/* + * g e t P r i n t L e v e l + */ +inline PrintLevel QProblemB::getPrintLevel( ) const +{ + return printlevel; +} + + +/* + * g e t H e s s i a n T y p e + */ +inline HessianType QProblemB::getHessianType( ) const +{ + return hessianType; +} + + +/* + * s e t H e s s i a n T y p e + */ +inline returnValue QProblemB::setHessianType( HessianType _hessianType ) +{ + hessianType = _hessianType; + return SUCCESSFUL_RETURN; +} + + + +/***************************************************************************** + * P R O T E C T E D * + *****************************************************************************/ + +/* + * s e t H + */ +inline returnValue QProblemB::setH( const real_t* const H_new ) +{ + int i, j; + + int nV = getNV(); + + for( i=0; i= 0 ) && ( number < getNV( ) ) ) + { + lb[number] = value; + return SUCCESSFUL_RETURN; + } + else + { + return THROWERROR( RET_INDEX_OUT_OF_BOUNDS ); + } +} + + +/* + * s e t U B + */ +inline returnValue QProblemB::setUB( const real_t* const ub_new ) +{ + int i; + + int nV = getNV(); + + for( i=0; i= 0 ) && ( number < getNV( ) ) ) + { + ub[number] = value; + + return SUCCESSFUL_RETURN; + } + else + { + return THROWERROR( RET_INDEX_OUT_OF_BOUNDS ); + } +} + + +/* + * c o m p u t e G i v e n s + */ +inline void QProblemB::computeGivens( real_t xold, real_t yold, real_t& xnew, real_t& ynew, + real_t& c, real_t& s + ) const +{ + if ( getAbs( yold ) <= ZERO ) + { + c = 1.0; + s = 0.0; + + xnew = xold; + ynew = yold; + } + else + { + real_t t, mu; + + mu = getAbs( xold ); + if ( getAbs( yold ) > mu ) + mu = getAbs( yold ); + + t = mu * sqrt( (xold/mu)*(xold/mu) + (yold/mu)*(yold/mu) ); + + if ( xold < 0.0 ) + t = -t; + + c = xold/t; + s = yold/t; + xnew = t; + ynew = 0.0; + } + + return; +} + + +/* + * a p p l y G i v e n s + */ +inline void QProblemB::applyGivens( real_t c, real_t s, real_t xold, real_t yold, + real_t& xnew, real_t& ynew + ) const +{ + /* Usual Givens plane rotation requiring four multiplications. */ + xnew = c*xold + s*yold; + ynew = -s*xold + c*yold; +// double nu = s/(1.0+c); +// +// xnew = xold*c + yold*s; +// ynew = (xnew+xold)*nu - yold; + + return; +} + + +/* + * end of file + */ diff --git a/phonelibs/qpoases/SRC/SubjectTo.cpp b/phonelibs/qpoases/SRC/SubjectTo.cpp new file mode 100644 index 00000000000000..5dbf707c9d1f21 --- /dev/null +++ b/phonelibs/qpoases/SRC/SubjectTo.cpp @@ -0,0 +1,200 @@ +/* + * This file is part of qpOASES. + * + * qpOASES -- An Implementation of the Online Active Set Strategy. + * Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved. + * + * qpOASES is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * qpOASES is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with qpOASES; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + + +/** + * \file SRC/SubjectTo.cpp + * \author Hans Joachim Ferreau + * \version 1.3embedded + * \date 2007-2008 + * + * Implementation of the SubjectTo class designed to manage working sets of + * constraints and bounds within a QProblem. + */ + + +#include + + +/***************************************************************************** + * P U B L I C * + *****************************************************************************/ + + +/* + * S u b j e c t T o + */ +SubjectTo::SubjectTo( ) : noLower( BT_TRUE ), + noUpper( BT_TRUE ), + size( 0 ) +{ + int i; + + for( i=0; iaddNumber( newnumber ) == RET_INDEXLIST_EXCEEDS_MAX_LENGTH ) + return THROWERROR( RET_ADDINDEX_FAILED ); + + return SUCCESSFUL_RETURN; +} + + +/* + * r e m o v e I n d e x + */ +returnValue SubjectTo::removeIndex( Indexlist* const indexlist, + int removenumber + ) +{ + status[removenumber] = ST_UNDEFINED; + + if ( indexlist->removeNumber( removenumber ) != SUCCESSFUL_RETURN ) + return THROWERROR( RET_UNKNOWN_BUG ); + + return SUCCESSFUL_RETURN; +} + + +/* + * s w a p I n d e x + */ +returnValue SubjectTo::swapIndex( Indexlist* const indexlist, + int number1, int number2 + ) +{ + /* consistency checks */ + if ( status[number1] != status[number2] ) + return THROWERROR( RET_SWAPINDEX_FAILED ); + + if ( number1 == number2 ) + { + THROWWARNING( RET_NOTHING_TO_DO ); + return SUCCESSFUL_RETURN; + } + + if ( indexlist->swapNumbers( number1,number2 ) != SUCCESSFUL_RETURN ) + return THROWERROR( RET_SWAPINDEX_FAILED ); + + return SUCCESSFUL_RETURN; +} + + +/* + * end of file + */ diff --git a/phonelibs/qpoases/SRC/SubjectTo.ipp b/phonelibs/qpoases/SRC/SubjectTo.ipp new file mode 100644 index 00000000000000..7cd6dc69fe9bd7 --- /dev/null +++ b/phonelibs/qpoases/SRC/SubjectTo.ipp @@ -0,0 +1,132 @@ +/* + * This file is part of qpOASES. + * + * qpOASES -- An Implementation of the Online Active Set Strategy. + * Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved. + * + * qpOASES is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * qpOASES is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with qpOASES; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + + +/** + * \file SRC/SubjectTo.ipp + * \author Hans Joachim Ferreau + * \version 1.3embedded + * \date 2007-2008 + * + * Implementation of the inlined member functions of the SubjectTo class + * designed to manage working sets of constraints and bounds within a QProblem. + */ + + +/***************************************************************************** + * P U B L I C * + *****************************************************************************/ + + +/* + * g e t T y p e + */ +inline SubjectToType SubjectTo::getType( int i ) const +{ + if ( ( i >= 0 ) && ( i < size ) ) + return type[i]; + else + return ST_UNKNOWN; +} + + +/* + * g e t S t a t u s + */ +inline SubjectToStatus SubjectTo::getStatus( int i ) const +{ + if ( ( i >= 0 ) && ( i < size ) ) + return status[i]; + else + return ST_UNDEFINED; +} + + +/* + * s e t T y p e + */ +inline returnValue SubjectTo::setType( int i, SubjectToType value ) +{ + if ( ( i >= 0 ) && ( i < size ) ) + { + type[i] = value; + return SUCCESSFUL_RETURN; + } + else + return THROWERROR( RET_INDEX_OUT_OF_BOUNDS ); +} + + +/* + * s e t S t a t u s + */ +inline returnValue SubjectTo::setStatus( int i, SubjectToStatus value ) +{ + if ( ( i >= 0 ) && ( i < size ) ) + { + status[i] = value; + return SUCCESSFUL_RETURN; + } + else + return THROWERROR( RET_INDEX_OUT_OF_BOUNDS ); +} + + +/* + * s e t N o L o w e r + */ +inline void SubjectTo::setNoLower( BooleanType _status ) +{ + noLower = _status; +} + + +/* + * s e t N o U p p e r + */ +inline void SubjectTo::setNoUpper( BooleanType _status ) +{ + noUpper = _status; +} + + +/* + * i s N o L o w e r + */ +inline BooleanType SubjectTo::isNoLower( ) const +{ + return noLower; +} + + +/* + * i s N o L o w e r + */ +inline BooleanType SubjectTo::isNoUpper( ) const +{ + return noUpper; +} + + +/* + * end of file + */ diff --git a/phonelibs/qpoases/SRC/Utils.cpp b/phonelibs/qpoases/SRC/Utils.cpp new file mode 100644 index 00000000000000..7a3d0a5226203b --- /dev/null +++ b/phonelibs/qpoases/SRC/Utils.cpp @@ -0,0 +1,471 @@ +/* + * This file is part of qpOASES. + * + * qpOASES -- An Implementation of the Online Active Set Strategy. + * Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved. + * + * qpOASES is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * qpOASES is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with qpOASES; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + + +/** + * \file SRC/Utils.cpp + * \author Hans Joachim Ferreau, Eckhard Arnold + * \version 1.3embedded + * \date 2007-2008 + * + * Implementation of some inlined utilities for working with the different QProblem + * classes. + */ + + +#include + +#if defined(__WIN32__) || defined(WIN32) + #include +#elif defined(LINUX) + #include + #include +#endif + +#ifdef __MATLAB__ + #include +#endif + + +#include + + + +#ifdef PC_DEBUG /* Define print functions only for debugging! */ +/* + * p r i n t + */ +returnValue print( const real_t* const v, int n ) +{ + int i; + char myPrintfString[160]; + + /* Print a vector. */ + myPrintf( "[\t" ); + for( i=0; ithrowError( RET_UNABLE_TO_OPEN_FILE,errstr,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE ); + } + + /* 2) Read data from file. */ + for( i=0; ithrowError( RET_UNABLE_TO_READ_FILE,errstr,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE ); + } + data[i*ncol + j] = ( (real_t) float_data ); + } + } + + /* 3) Close file. */ + fclose( datafile ); + + return SUCCESSFUL_RETURN; +} + + +/* + * r e a d F r o m F i l e + */ +returnValue readFromFile( real_t* data, int n, + const char* datafilename + ) +{ + return readFromFile( data, n, 1, datafilename ); +} + + + +/* + * r e a d F r o m F i l e + */ +returnValue readFromFile( int* data, int n, + const char* datafilename + ) +{ + int i; + myFILE* datafile; + + /* 1) Open file. */ + if ( ( datafile = fopen( datafilename, "r" ) ) == 0 ) + { + char errstr[80]; + sprintf( errstr,"(%s)",datafilename ); + return getGlobalMessageHandler( )->throwError( RET_UNABLE_TO_OPEN_FILE,errstr,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE ); + } + + /* 2) Read data from file. */ + for( i=0; ithrowError( RET_UNABLE_TO_READ_FILE,errstr,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE ); + } + } + + /* 3) Close file. */ + fclose( datafile ); + + return SUCCESSFUL_RETURN; +} + + +/* + * w r i t e I n t o F i l e + */ +returnValue writeIntoFile( const real_t* const data, int nrow, int ncol, + const char* datafilename, BooleanType append + ) +{ + int i, j; + myFILE* datafile; + + /* 1) Open file. */ + if ( append == BT_TRUE ) + { + /* append data */ + if ( ( datafile = fopen( datafilename, "a" ) ) == 0 ) + { + char errstr[80]; + sprintf( errstr,"(%s)",datafilename ); + return getGlobalMessageHandler( )->throwError( RET_UNABLE_TO_OPEN_FILE,errstr,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE ); + } + } + else + { + /* do not append data */ + if ( ( datafile = fopen( datafilename, "w" ) ) == 0 ) + { + char errstr[80]; + sprintf( errstr,"(%s)",datafilename ); + return getGlobalMessageHandler( )->throwError( RET_UNABLE_TO_OPEN_FILE,errstr,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE ); + } + } + + /* 2) Write data into file. */ + for( i=0; ithrowError( RET_UNABLE_TO_OPEN_FILE,errstr,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE ); + } + } + else + { + /* do not append data */ + if ( ( datafile = fopen( datafilename, "w" ) ) == 0 ) + { + char errstr[80]; + sprintf( errstr,"(%s)",datafilename ); + return getGlobalMessageHandler( )->throwError( RET_UNABLE_TO_OPEN_FILE,errstr,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE ); + } + } + + /* 2) Write data into file. */ + for( i=0; i #include "cereal/gen/cpp/log.capnp.h" +#include "cereal/gen/cpp/car.capnp.h" +#include "common/params.h" #include "common/swaglog.h" #include "common/timing.h" @@ -38,20 +40,55 @@ bool loopback_can = false; #define TIMEOUT 0 #define SAFETY_NOOUTPUT 0x0000 -#define SAFETY_HONDA 0x0001 -#define SAFETY_ALLOUTPUT 0x1337 +pthread_t safety_setter_thread_handle = -1; + +void *safety_setter_thread(void *s) { + char *value; + size_t value_sz = 0; + + LOGW("waiting for params to set safety model"); + while (1) { + const int result = read_db_value(NULL, "CarParams", &value, &value_sz); + if (value_sz > 0) break; + usleep(100*1000); + } + LOGW("got %d bytes CarParams", value_sz); + + // format for board, make copy due to alignment issues, will be freed on out of scope + auto amsg = kj::heapArray((value_sz / sizeof(capnp::word)) + 1); + memcpy(amsg.begin(), value, value_sz); + + capnp::FlatArrayMessageReader cmsg(amsg); + cereal::CarParams::Reader car_params = cmsg.getRoot(); + + int safety_model = car_params.getSafetyModel(); + LOGW("setting safety model: %d", safety_model); + + pthread_mutex_lock(&usb_lock); + + // set in the mutex to avoid race + safety_setter_thread_handle = -1; + + libusb_control_transfer(dev_handle, 0x40, 0xdc, safety_model, 0, NULL, 0, TIMEOUT); + + pthread_mutex_unlock(&usb_lock); + + return NULL; +} + +// must be called before threads or with mutex bool usb_connect() { int err; dev_handle = libusb_open_device_with_vid_pid(ctx, 0xbbaa, 0xddcc); - if (dev_handle == NULL) { return false; } + if (dev_handle == NULL) { goto fail; } err = libusb_set_configuration(dev_handle, 1); - if (err != 0) { return false; } + if (err != 0) { goto fail; } err = libusb_claim_interface(dev_handle, 0); - if (err != 0) { return false; } + if (err != 0) { goto fail; } if (loopback_can) { libusb_control_transfer(dev_handle, 0xc0, 0xe5, 1, 0, NULL, 0, TIMEOUT); @@ -60,30 +97,23 @@ bool usb_connect() { // power off ESP libusb_control_transfer(dev_handle, 0xc0, 0xd9, 0, 0, NULL, 0, TIMEOUT); - // forward CAN1 to CAN3...soon - //libusb_control_transfer(dev_handle, 0xc0, 0xdd, 1, 2, NULL, 0, TIMEOUT); - - // set UART modes for Honda Accord - /*for (int uart = 2; uart <= 3; uart++) { - // 9600 baud - libusb_control_transfer(dev_handle, 0x40, 0xe1, uart, 9600, NULL, 0, TIMEOUT); - // even parity - libusb_control_transfer(dev_handle, 0x40, 0xe2, uart, 1, NULL, 0, TIMEOUT); - // callback 1 - libusb_control_transfer(dev_handle, 0x40, 0xe3, uart, 1, NULL, 0, TIMEOUT); - } + // power on charging (may trigger a reconnection, should be okay) + #ifndef __x86_64__ + libusb_control_transfer(dev_handle, 0xc0, 0xe6, 1, 0, NULL, 0, TIMEOUT); + #else + LOGW("not enabling charging on x86_64"); + #endif - // TODO: Boardd should be able to set the baud rate - int baud = 500000; - libusb_control_transfer(dev_handle, 0x40, 0xde, 0, 0, - (unsigned char *)&baud, sizeof(baud), TIMEOUT); // CAN1 - libusb_control_transfer(dev_handle, 0x40, 0xde, 1, 0, - (unsigned char *)&baud, sizeof(baud), TIMEOUT); // CAN2*/ + // no output is the default + libusb_control_transfer(dev_handle, 0x40, 0xdc, SAFETY_NOOUTPUT, 0, NULL, 0, TIMEOUT); - // TODO: Boardd should be able to be told which safety model to use - libusb_control_transfer(dev_handle, 0x40, 0xdc, SAFETY_HONDA, 0, NULL, 0, TIMEOUT); + if (safety_setter_thread_handle == -1) { + err = pthread_create(&safety_setter_thread_handle, NULL, safety_setter_thread, NULL); + } return true; +fail: + return false; } void usb_retry_connect() { @@ -93,7 +123,7 @@ void usb_retry_connect() { } void handle_usb_issue(int err, const char func[]) { - LOGE("usb error %d \"%s\" in %s", err, libusb_strerror((enum libusb_error)err), func); + LOGE_100("usb error %d \"%s\" in %s", err, libusb_strerror((enum libusb_error)err), func); if (err == -4) { LOGE("lost connection"); usb_retry_connect(); @@ -113,7 +143,7 @@ void can_recv(void *s) { do { err = libusb_bulk_transfer(dev_handle, 0x81, (uint8_t*)data, RECV_SIZE, &recv, TIMEOUT); if (err != 0) { handle_usb_issue(err, __func__); } - if (err == -8) { LOGE("overflow got 0x%x", recv); }; + if (err == -8) { LOGE_100("overflow got 0x%x", recv); }; // timeout is okay to exit, recv still happened if (err == -7) { break; } @@ -261,6 +291,46 @@ void can_send(void *s) { // **** threads **** +void *thermal_thread(void *crap) { + int err; + LOGD("start thermal thread"); + + // thermal = 8005 + void *context = zmq_ctx_new(); + void *subscriber = zmq_socket(context, ZMQ_SUB); + zmq_setsockopt(subscriber, ZMQ_SUBSCRIBE, "", 0); + zmq_connect(subscriber, "tcp://127.0.0.1:8005"); + + // run as fast as messages come in + while (!do_exit) { + // recv from thermal + zmq_msg_t msg; + zmq_msg_init(&msg); + err = zmq_msg_recv(&msg, subscriber, 0); + assert(err >= 0); + + // format for board, make copy due to alignment issues, will be freed on out of scope + // copied from send thread... + auto amsg = kj::heapArray((zmq_msg_size(&msg) / sizeof(capnp::word)) + 1); + memcpy(amsg.begin(), zmq_msg_data(&msg), zmq_msg_size(&msg)); + + capnp::FlatArrayMessageReader cmsg(amsg); + cereal::Event::Reader event = cmsg.getRoot(); + + uint16_t target_fan_speed = event.getThermal().getFanSpeed(); + //LOGW("setting fan speed %d", target_fan_speed); + + pthread_mutex_lock(&usb_lock); + libusb_control_transfer(dev_handle, 0xc0, 0xd3, target_fan_speed, 0, NULL, 0, TIMEOUT); + pthread_mutex_unlock(&usb_lock); + } + + // turn the fan off when we exit + libusb_control_transfer(dev_handle, 0xc0, 0xd3, 0, 0, NULL, 0, TIMEOUT); + + return NULL; +} + void *can_send_thread(void *crap) { LOGD("start send thread"); @@ -364,8 +434,16 @@ int main() { can_recv_thread, NULL); assert(err == 0); + pthread_t thermal_thread_handle; + err = pthread_create(&thermal_thread_handle, NULL, + thermal_thread, NULL); + assert(err == 0); + // join threads + err = pthread_join(thermal_thread_handle, NULL); + assert(err == 0); + err = pthread_join(can_recv_thread_handle, NULL); assert(err == 0); diff --git a/selfdrive/boardd/boardd.py b/selfdrive/boardd/boardd.py index 8bda67923f08f2..06abe8b3a586e2 100755 --- a/selfdrive/boardd/boardd.py +++ b/selfdrive/boardd/boardd.py @@ -20,6 +20,8 @@ SAFETY_NOOUTPUT = 0 SAFETY_HONDA = 1 +SAFETY_TOYOTA = 2 +SAFETY_TOYOTA_NOLIMITS = 0x1336 SAFETY_ALLOUTPUT = 0x1337 # *** serialization functions *** @@ -33,7 +35,7 @@ def can_list_to_can_capnp(can_msgs, msgtype='can'): cc = dat.can[i] cc.address = can_msg[0] cc.busTime = can_msg[1] - cc.dat = can_msg[2] + cc.dat = str(can_msg[2]) cc.src = can_msg[3] return dat @@ -111,6 +113,7 @@ def can_init(): def boardd_mock_loop(): context = zmq.Context() can_init() + handle.controlWrite(0x40, 0xdc, SAFETY_ALLOUTPUT, 0, b'') logcan = messaging.sub_sock(context, service_list['can'].port) diff --git a/selfdrive/can/Makefile b/selfdrive/can/Makefile index 9c2ebbd7321b35..5a91e70291750a 100644 --- a/selfdrive/can/Makefile +++ b/selfdrive/can/Makefile @@ -1,6 +1,7 @@ CC = clang CXX = clang++ +BASEDIR = ../.. PHONELIBS := ../../phonelibs UNAME_S := $(shell uname -s) @@ -53,7 +54,7 @@ libdbc.so:: ../../cereal/gen/cpp/log.capnp.h ../../cereal/gen/cpp/log.capnp.h: cd ../../cereal && make -libdbc.so:: parser.cc $(DBC_CCS) +libdbc.so:: dbc.cc parser.cc packer.cc $(DBC_CCS) $(CXX) -fPIC -shared -o '$@' $^ \ -I. \ -I../.. \ @@ -63,7 +64,7 @@ libdbc.so:: parser.cc $(DBC_CCS) $(CEREAL_CXXFLAGS) \ $(CEREAL_LIBS) -dbc_out/%.cc: $(OPENDBC_PATH)/%.dbc +dbc_out/%.cc: $(OPENDBC_PATH)/%.dbc process_dbc.py dbc_template.cc PYTHONPATH=$(PYTHONPATH):$(CWD)/../../pyextra ./process_dbc.py '$<' '$@' .PHONY: clean diff --git a/selfdrive/can/parser_common.h b/selfdrive/can/common.h similarity index 81% rename from selfdrive/can/parser_common.h rename to selfdrive/can/common.h index e3b944f83ec2a0..3790c1358a9bfe 100644 --- a/selfdrive/can/parser_common.h +++ b/selfdrive/can/common.h @@ -1,13 +1,20 @@ -#ifndef PARSER_COMMON_H -#define PARSER_COMMON_H +#ifndef SELFDRIVE_CAN_COMMON_H +#define SELFDRIVE_CAN_COMMON_H #include #include +#include #define ARRAYSIZE(x) (sizeof(x)/sizeof(x[0])) +struct SignalPackValue { + const char* name; + double value; +}; + + struct SignalParseOptions { uint32_t address; const char* name; @@ -54,6 +61,8 @@ struct DBC { const Msg *msgs; }; +const DBC* dbc_lookup(const std::string& dbc_name); + void dbc_register(const DBC* dbc); #define dbc_init(dbc) \ diff --git a/selfdrive/can/dbc.cc b/selfdrive/can/dbc.cc new file mode 100644 index 00000000000000..c8295d1e0c8157 --- /dev/null +++ b/selfdrive/can/dbc.cc @@ -0,0 +1,26 @@ +#include +#include + +#include "common.h" + +namespace { + +std::vector& get_dbcs() { + static std::vector vec; + return vec; +} + +} + +const DBC* dbc_lookup(const std::string& dbc_name) { + for (const auto& dbci : get_dbcs()) { + if (dbc_name == dbci->name) { + return dbci; + } + } + return NULL; +} + +void dbc_register(const DBC* dbc) { + get_dbcs().push_back(dbc); +} diff --git a/selfdrive/can/dbc_template.cc b/selfdrive/can/dbc_template.cc index 3676c823ecd1eb..1d85b1e2f3eb3b 100644 --- a/selfdrive/can/dbc_template.cc +++ b/selfdrive/can/dbc_template.cc @@ -1,6 +1,6 @@ #include -#include "parser_common.h" +#include "common.h" namespace { diff --git a/selfdrive/can/libdbc_py.py b/selfdrive/can/libdbc_py.py new file mode 100644 index 00000000000000..0afeee40ef334b --- /dev/null +++ b/selfdrive/can/libdbc_py.py @@ -0,0 +1,51 @@ +import os +import subprocess + +from cffi import FFI + +can_dir = os.path.dirname(os.path.abspath(__file__)) +libdbc_fn = os.path.join(can_dir, "libdbc.so") +subprocess.check_output(["make"], cwd=can_dir) + +ffi = FFI() +ffi.cdef(""" + +typedef struct SignalParseOptions { + uint32_t address; + const char* name; + double default_value; +} SignalParseOptions; + +typedef struct MessageParseOptions { + uint32_t address; + int check_frequency; +} MessageParseOptions; + +typedef struct SignalValue { + uint32_t address; + uint16_t ts; + const char* name; + double value; +} SignalValue; + +void* can_init(int bus, const char* dbc_name, + size_t num_message_options, const MessageParseOptions* message_options, + size_t num_signal_options, const SignalParseOptions* signal_options); + +void can_update(void* can, uint64_t sec, bool wait); + +size_t can_query(void* can, uint64_t sec, bool *out_can_valid, size_t out_values_size, SignalValue* out_values); + + +typedef struct SignalPackValue { + const char* name; + double value; +} SignalPackValue; + +void* canpack_init(const char* dbc_name); + +uint64_t canpack_pack(void* inst, uint32_t address, size_t num_vals, const SignalPackValue *vals); + +""") + +libdbc = ffi.dlopen(libdbc_fn) diff --git a/selfdrive/can/packer.cc b/selfdrive/can/packer.cc new file mode 100644 index 00000000000000..3eddc6aeef79a3 --- /dev/null +++ b/selfdrive/can/packer.cc @@ -0,0 +1,81 @@ +#include + +#include +#include +#include +#include +#include + +#include "common.h" + +#define WARN printf + +namespace { + +class CANPacker { +public: + CANPacker(const std::string& dbc_name) { + dbc = dbc_lookup(dbc_name); + assert(dbc); + + for (int i=0; inum_msgs; i++) { + const Msg* msg = &dbc->msgs[i]; + for (int j=0; jnum_sigs; j++) { + const Signal* sig = &msg->sigs[j]; + signal_lookup[std::make_pair(msg->address, std::string(sig->name))] = *sig; + } + } + } + + uint64_t pack(uint32_t address, const std::vector &signals) { + uint64_t ret = 0; + for (const auto& sigval : signals) { + std::string name = std::string(sigval.name); + double value = sigval.value; + + auto sig_it = signal_lookup.find(make_pair(address, name)); + if (sig_it == signal_lookup.end()) { + WARN("undefined signal %s", name.c_str()); + continue; + } + auto sig = sig_it->second; + + int64_t ival = (int64_t)((value - sig.offset) / sig.factor); + if (ival < 0) { + WARN("signed pack unsupported right now"); + continue; + } + + uint64_t mask = ((1ULL << sig.b2)-1) << sig.bo; + uint64_t dat = (ival & ((1ULL << sig.b2)-1)) << sig.bo; + ret &= ~mask; + ret |= dat; + } + + return ret; + } + + + +private: + const DBC *dbc = NULL; + std::map, Signal> signal_lookup; +}; + +} + +extern "C" { + +void* canpack_init(const char* dbc_name) { + CANPacker *ret = new CANPacker(std::string(dbc_name)); + return (void*)ret; +} + +uint64_t canpack_pack(void* inst, uint32_t address, size_t num_vals, const SignalPackValue *vals) { + CANPacker *cp = (CANPacker*)inst; + + return cp->pack(address, std::vector(vals, vals+num_vals)); +} + +} + diff --git a/selfdrive/can/packer.py b/selfdrive/can/packer.py new file mode 100644 index 00000000000000..ff52e165e2f5bd --- /dev/null +++ b/selfdrive/can/packer.py @@ -0,0 +1,37 @@ +import struct + +from selfdrive.can.libdbc_py import libdbc, ffi + +class CANPacker(object): + def __init__(self, dbc_name): + self.packer = libdbc.canpack_init(dbc_name) + self.sig_names = {} + + def pack(self, addr, values): + # values: [(signal_name, signal_value)] + + values_thing = [] + for name, value in values: + if name not in self.sig_names: + self.sig_names[name] = ffi.new("char[]", name) + + values_thing.append({ + 'name': self.sig_names[name], + 'value': value + }) + + values_c = ffi.new("SignalPackValue[]", values_thing) + + return libdbc.canpack_pack(self.packer, addr, len(values_thing), values_c) + + def pack_bytes(self, addr, values): + return struct.pack(">Q", self.pack(addr, values)) + + +if __name__ == "__main__": + cp = CANPacker("honda_civic_touring_2016_can") + s = cp.pack_bytes(0x30c, [ + ("PCM_SPEED", 123), + ("PCM_GAS", 10), + ]) + print s.encode("hex") diff --git a/selfdrive/can/parser.cc b/selfdrive/can/parser.cc index 7e7913bf433862..839adf5222939d 100644 --- a/selfdrive/can/parser.cc +++ b/selfdrive/can/parser.cc @@ -18,7 +18,7 @@ #include #include "cereal/gen/cpp/log.capnp.h" -#include "parser_common.h" +#include "common.h" #define DEBUG(...) // #define DEBUG printf @@ -40,8 +40,6 @@ uint64_t read_u64_be(const uint8_t* v) { | (uint64_t)v[7]); } -std::vector g_dbc; - bool honda_checksum(int address, uint64_t d, int l) { int target = (d >> l) & 0xF; @@ -77,9 +75,9 @@ struct MessageState { for (int i=0; i < parse_sigs.size(); i++) { auto& sig = parse_sigs[i]; - int64_t tmp = (dat >> sig.bo) & ((1 << sig.b2)-1); + int64_t tmp = (dat >> sig.bo) & ((1ULL << sig.b2)-1); if (sig.is_signed) { - tmp -= (tmp >> (sig.b2-1)) ? (1<> (sig.b2-1)) ? (1ULL << sig.b2) : 0; //signed } DEBUG("parse %X %s -> %ld\n", address, sig.name, tmp); @@ -123,6 +121,7 @@ struct MessageState { }; + class CANParser { public: CANParser(int abus, const std::string& dbc_name, @@ -135,15 +134,10 @@ class CANParser { zmq_setsockopt(subscriber, ZMQ_SUBSCRIBE, "", 0); zmq_connect(subscriber, "tcp://127.0.0.1:8006"); - for (auto dbci : g_dbc) { - if (dbci->name == dbc_name) { - dbc = dbci; - break; - } - } + dbc = dbc_lookup(dbc_name); assert(dbc); - for (auto &op : options) { + for (const auto& op : options) { MessageState state = { .address = op.address, // .check_frequency = op.check_frequency, @@ -178,7 +172,7 @@ class CANParser { } // track requested signals for this message - for (auto &sigop : sigoptions) { + for (const auto& sigop : sigoptions) { if (sigop.address != op.address) continue; for (int i=0; inum_sigs; i++) { @@ -230,8 +224,8 @@ class CANParser { void UpdateValid(uint64_t sec) { can_valid = true; - for (auto &kv : message_states) { - auto &state = kv.second; + for (const auto& kv : message_states) { + const auto& state = kv.second; if (state.check_threshold > 0 && (sec - state.seen) > state.check_threshold) { if (state.seen > 0) { INFO("%X TIMEOUT\n", state.address); @@ -279,8 +273,8 @@ class CANParser { std::vector query(uint64_t sec) { std::vector ret; - for (auto &kv : message_states) { - auto &state = kv.second; + for (const auto& kv : message_states) { + const auto& state = kv.second; if (sec != 0 && state.seen != sec) continue; for (int i=0; i 0. + if (brake < brake_hyst_on and not braking) or brake < brake_hyst_off: + brake = 0. + braking = brake > 0. # for small brake oscillations within brake_hyst_gap, don't change the brake command - if final_brake == 0.: + if brake == 0.: brake_steady = 0. - elif final_brake > brake_steady + brake_hyst_gap: - brake_steady = final_brake - brake_hyst_gap - elif final_brake < brake_steady - brake_hyst_gap: - brake_steady = final_brake + brake_hyst_gap - final_brake = brake_steady + elif brake > brake_steady + brake_hyst_gap: + brake_steady = brake - brake_hyst_gap + elif brake < brake_steady - brake_hyst_gap: + brake_steady = brake + brake_hyst_gap + brake = brake_steady if not civic: brake_on_offset_v = [.25, .15] # min brake command on brake activation. below this no decel is perceived @@ -36,10 +36,10 @@ def actuator_hystereses(final_brake, braking, brake_steady, v_ego, civic): # offset the brake command for threshold in the brake system. no brake torque perceived below it brake_on_offset = interp(v_ego, brake_on_offset_bp, brake_on_offset_v) brake_offset = brake_on_offset - brake_hyst_on - if final_brake > 0.0: - final_brake += brake_offset + if brake > 0.0: + brake += brake_offset - return final_brake, braking, brake_steady + return brake, braking, brake_steady class AH: #[alert_idx, value] @@ -78,12 +78,9 @@ class CarController(object): def __init__(self): self.braking = False self.brake_steady = 0. - self.final_brake_last = 0. - - # redundant safety check with the board - self.controls_allowed = False + self.brake_last = 0. - def update(self, sendcan, enabled, CS, frame, final_gas, final_brake, final_steer, \ + def update(self, sendcan, enabled, CS, frame, actuators, \ pcm_speed, pcm_override, pcm_cancel_cmd, pcm_accel, \ hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert, \ snd_beep, snd_chime): @@ -94,20 +91,16 @@ def update(self, sendcan, enabled, CS, frame, final_gas, final_brake, final_stee return # *** apply brake hysteresis *** - final_brake, self.braking, self.brake_steady = actuator_hystereses(final_brake, self.braking, self.brake_steady, CS.v_ego, CS.civic) + brake, self.braking, self.brake_steady = actuator_hystereses(actuators.brake, self.braking, self.brake_steady, CS.v_ego, CS.civic) # *** no output if not enabled *** - if not enabled: - final_gas = 0. - final_brake = 0. - final_steer = 0. + if not enabled and CS.pcm_acc_status: # send pcm acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated - if CS.pcm_acc_status: - pcm_cancel_cmd = True + pcm_cancel_cmd = True # *** rate limit after the enable check *** - final_brake = rate_limit(final_brake, self.final_brake_last, -2., 1./100) - self.final_brake_last = final_brake + brake = rate_limit(brake, self.brake_last, -2., 1./100) + self.brake_last = brake # vehicle hud display, wait for one update from 10Hz 0x304 msg #TODO: use enum!! @@ -143,7 +136,8 @@ def update(self, sendcan, enabled, CS, frame, final_gas, final_brake, final_stee GAS_MAX = 1004 BRAKE_MAX = 1024/4 if CS.civic: - STEER_MAX = 0x1000 + is_fw_modified = os.getenv("DONGLE_ID") in ['b0f5a01cf604185c'] + STEER_MAX = 0x1FFF if is_fw_modified else 0x1000 elif CS.crv: STEER_MAX = 0x300 # CR-V only uses 12-bits and requires a lower value else: @@ -151,50 +145,22 @@ def update(self, sendcan, enabled, CS, frame, final_gas, final_brake, final_stee GAS_OFFSET = 328 # steer torque is converted back to CAN reference (positive when steering right) - apply_gas = int(clip(final_gas*GAS_MAX, 0, GAS_MAX-1)) - apply_brake = int(clip(final_brake*BRAKE_MAX, 0, BRAKE_MAX-1)) - apply_steer = int(clip(-final_steer*STEER_MAX, -STEER_MAX, STEER_MAX)) + apply_gas = int(clip(actuators.gas * GAS_MAX, 0, GAS_MAX - 1)) + apply_brake = int(clip(brake * BRAKE_MAX, 0, BRAKE_MAX - 1)) + apply_steer = int(clip(-actuators.steer * STEER_MAX, -STEER_MAX, STEER_MAX)) # no gas if you are hitting the brake or the user is if apply_gas > 0 and (apply_brake != 0 or CS.brake_pressed): - print "CANCELLING GAS", apply_brake apply_gas = 0 # no computer brake if the gas is being pressed if CS.car_gas > 0 and apply_brake != 0: - print "CANCELLING BRAKE" apply_brake = 0 # any other cp.vl[0x18F]['STEER_STATUS'] is common and can happen during user override. sending 0 torque to avoid EPS sending error 5 if CS.steer_not_allowed: - print "STEER ALERT, TORQUE INHIBITED" apply_steer = 0 - # *** entry into controls state *** - if (CS.prev_cruise_buttons == CruiseButtons.DECEL_SET or CS.prev_cruise_buttons == CruiseButtons.RES_ACCEL) and \ - CS.cruise_buttons == 0 and not self.controls_allowed: - print "CONTROLS ARE LIVE" - self.controls_allowed = True - - # *** exit from controls state on cancel, gas, or brake *** - if (CS.cruise_buttons == CruiseButtons.CANCEL or CS.brake_pressed or - CS.user_gas_pressed or (CS.pedal_gas > 0 and CS.brake_only)) and self.controls_allowed: - print "CONTROLS ARE DEAD" - self.controls_allowed = False - - # *** controls fail on steer error, brake error, or invalid can *** - if CS.steer_error: - print "STEER ERROR" - self.controls_allowed = False - - if CS.brake_error: - print "BRAKE ERROR" - self.controls_allowed = False - - if not CS.can_valid and self.controls_allowed: # 200 ms - print "CAN INVALID" - self.controls_allowed = False - # Send CAN commands. can_sends = [] diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py index d2c0f974ae324a..4eb8c6c38697aa 100644 --- a/selfdrive/car/honda/carstate.py +++ b/selfdrive/car/honda/carstate.py @@ -1,18 +1,54 @@ import os import time - +from cereal import car import common.numpy_fast as np from common.realtime import sec_since_boot - import selfdrive.messaging as messaging +from selfdrive.can.parser import CANParser +from selfdrive.config import Conversions as CV -from selfdrive.car.honda.can_parser import CANParser -from selfdrive.can.parser import CANParser as CANParserC -NEW_CAN = os.getenv("OLD_CAN") is None +def parse_gear_shifter(can_gear_shifter, is_acura): -def get_can_parser(CP): - # this function generates lists for signal, messages and initial values + if can_gear_shifter == 0x1: + return "park" + elif can_gear_shifter == 0x2: + return "reverse" + + if is_acura: + if can_gear_shifter == 0x3: + return "neutral" + elif can_gear_shifter == 0x4: + return "drive" + elif can_gear_shifter == 0xa: + return "sport" + + else: + if can_gear_shifter == 0x4: + return "neutral" + elif can_gear_shifter == 0x8: + return "drive" + elif can_gear_shifter == 0x10: + return "sport" + elif can_gear_shifter == 0x20: + return "low" + + return "unknown" + +_K0 = -0.3 +_K1 = -0.01879 +_K2 = 0.01013 + +def calc_cruise_offset(offset, speed): + # euristic formula so that speed is controlled to ~ 0.3m/s below pid_speed + # constraints to solve for _K0, _K1, _K2 are: + # - speed = 0m/s, out = -0.3 + # - speed = 34m/s, offset = 20, out = -0.25 + # - speed = 34m/s, offset = -2.5, out = -1.8 + return min(_K0 + _K1 * speed + _K2 * speed * offset, 0.) + +def get_can_signals(CP): +# this function generates lists for signal, messages and initial values if CP.carFingerprint == "HONDA CIVIC 2016 TOURING": dbc_f = 'honda_civic_touring_2016_can.dbc' signals = [ @@ -50,8 +86,9 @@ def get_can_parser(CP): ("CRUISE_SETTING", 0x296, 0), ("LEFT_BLINKER", 0x326, 0), ("RIGHT_BLINKER", 0x326, 0), - ("COUNTER", 0x324, 0), - ("ENGINE_RPM", 0x17C, 0) + ("CRUISE_SPEED_OFFSET", 0x37c, 0), + ("EPB_STATE", 0x1c2, 0), + ("BRAKE_HOLD_ACTIVE", 0x1A4, 0), ] checks = [ # address, frequency @@ -65,6 +102,7 @@ def get_can_parser(CP): (0x1d0, 50), (0x305, 10), (0x324, 10), + (0x37c, 10), (0x405, 3), ] @@ -104,8 +142,7 @@ def get_can_parser(CP): ("CRUISE_SETTING", 0x1a6, 0), ("LEFT_BLINKER", 0x294, 0), ("RIGHT_BLINKER", 0x294, 0), - ("COUNTER", 0x324, 0), - ("ENGINE_RPM", 0x17C, 0) + ("CRUISE_SPEED_OFFSET", 0x37c, 0) ] checks = [ (0x156, 100), @@ -118,6 +155,7 @@ def get_can_parser(CP): (0x1d0, 50), (0x305, 10), (0x324, 10), + (0x37c, 10), (0x405, 3), ] elif CP.carFingerprint == "HONDA ACCORD 2016 TOURING": @@ -157,8 +195,6 @@ def get_can_parser(CP): ("CRUISE_SETTING", 0x1a6, 0), ("LEFT_BLINKER", 0x294, 0), ("RIGHT_BLINKER", 0x294, 0), - ("COUNTER", 0x324, 0), - ("ENGINE_RPM", 0x17C, 0) ] checks = [ (0x156, 100), @@ -209,8 +245,6 @@ def get_can_parser(CP): ("CRUISE_SETTING", 0x1a6, 0), ("LEFT_BLINKER", 0x294, 0), ("RIGHT_BLINKER", 0x294, 0), - ("COUNTER", 0x324, 0), - ("ENGINE_RPM", 0x17C, 0) ] checks = [ (0x156, 100), @@ -230,22 +264,23 @@ def get_can_parser(CP): signals.append(("INTERCEPTOR_GAS", 0x201, 0)) checks.append((0x201, 50)) - if NEW_CAN: - return CANParserC(os.path.splitext(dbc_f)[0], signals, checks, 0) - else: - return CANParser(dbc_f, signals, checks) + return dbc_f, signals, checks + +def get_can_parser(CP): + dbc_f, signals, checks = get_can_signals(CP) + return CANParser(os.path.splitext(dbc_f)[0], signals, checks, 0) class CarState(object): def __init__(self, CP, logcan): + self.acura = False self.civic = False self.accord = False self.crv = False if CP.carFingerprint == "HONDA CIVIC 2016 TOURING": self.civic = True elif CP.carFingerprint == "ACURA ILX 2016 ACURAWATCH PLUS": - self.civic = False + self.acura = True elif CP.carFingerprint == "HONDA ACCORD 2016 TOURING": - # fake civic self.accord = True elif CP.carFingerprint == "HONDA CR-V 2016 TOURING": self.crv = True @@ -274,10 +309,7 @@ def __init__(self, CP, logcan): def update(self, can_pub_main=None): cp = self.cp - if NEW_CAN: - cp.update(int(sec_since_boot() * 1e9), False) - else: - cp.update_can(can_pub_main) + cp.update(int(sec_since_boot() * 1e9), False) # copy can_valid self.can_valid = cp.can_valid @@ -294,8 +326,6 @@ def update(self, can_pub_main=None): self.prev_left_blinker_on = self.left_blinker_on self.prev_right_blinker_on = self.right_blinker_on - self.rpm = cp.vl[0x17C]['ENGINE_RPM'] - # ******************* parse out can ******************* self.door_all_closed = not any([cp.vl[0x405]['DOOR_OPEN_FL'], cp.vl[0x405]['DOOR_OPEN_FR'], cp.vl[0x405]['DOOR_OPEN_RL'], cp.vl[0x405]['DOOR_OPEN_RR']]) @@ -316,9 +346,11 @@ def update(self, can_pub_main=None): self.brake_error = cp.vl[0x1B0]['BRAKE_ERROR_1'] or cp.vl[0x1B0]['BRAKE_ERROR_2'] self.esp_disabled = cp.vl[0x1A4]['ESP_DISABLED'] # calc best v_ego estimate, by averaging two opposite corners - self.v_wheel = ( - cp.vl[0x1D0]['WHEEL_SPEED_FL'] + cp.vl[0x1D0]['WHEEL_SPEED_FR'] + - cp.vl[0x1D0]['WHEEL_SPEED_RL'] + cp.vl[0x1D0]['WHEEL_SPEED_RR']) / 4. + self.v_wheel_fl = cp.vl[0x1D0]['WHEEL_SPEED_FL'] + self.v_wheel_fr = cp.vl[0x1D0]['WHEEL_SPEED_FR'] + self.v_wheel_rl = cp.vl[0x1D0]['WHEEL_SPEED_RL'] + self.v_wheel_rr = cp.vl[0x1D0]['WHEEL_SPEED_RR'] + self.v_wheel = (self.v_wheel_fl + self.v_wheel_fr + self.v_wheel_rl + self.v_wheel_rr) / 4. # blend in transmission speed at low speed, since it has more low speed accuracy self.v_weight = np.interp(self.v_wheel, v_weight_bp, v_weight_v) self.v_ego = (1. - self.v_weight) * cp.vl[0x158]['XMISSION_SPEED'] + self.v_weight * self.v_wheel @@ -328,49 +360,59 @@ def update(self, can_pub_main=None): self.user_gas_pressed = self.user_gas > 0 # this works because interceptor read < 0 when pedal position is 0. Once calibrated, this will change #print self.user_gas, self.user_gas_pressed if self.civic: - self.gear_shifter = cp.vl[0x191]['GEAR_SHIFTER'] + can_gear_shifter = cp.vl[0x191]['GEAR_SHIFTER'] self.angle_steers = cp.vl[0x14A]['STEER_ANGLE'] self.gear = 0 # TODO: civic has CVT... needs rev engineering self.cruise_setting = cp.vl[0x296]['CRUISE_SETTING'] self.cruise_buttons = cp.vl[0x296]['CRUISE_BUTTONS'] self.main_on = cp.vl[0x326]['MAIN_ON'] - self.gear_shifter_valid = self.gear_shifter in [1,8] # TODO: 1/P allowed for debug self.blinker_on = cp.vl[0x326]['LEFT_BLINKER'] or cp.vl[0x326]['RIGHT_BLINKER'] self.left_blinker_on = cp.vl[0x326]['LEFT_BLINKER'] self.right_blinker_on = cp.vl[0x326]['RIGHT_BLINKER'] + self.cruise_speed_offset = calc_cruise_offset(cp.vl[0x37c]['CRUISE_SPEED_OFFSET'], self.v_ego) + self.park_brake = cp.vl[0x1c2]['EPB_STATE'] != 0 + self.brake_hold = cp.vl[0x1A4]['BRAKE_HOLD_ACTIVE'] elif self.accord: - self.gear_shifter = cp.vl[0x191]['GEAR_SHIFTER'] + can_gear_shifter = cp.vl[0x191]['GEAR_SHIFTER'] self.angle_steers = cp.vl[0x156]['STEER_ANGLE'] self.gear = 0 # TODO: accord has CVT... needs rev engineering self.cruise_setting = cp.vl[0x1A6]['CRUISE_SETTING'] self.cruise_buttons = cp.vl[0x1A6]['CRUISE_BUTTONS'] self.main_on = cp.vl[0x1A6]['MAIN_ON'] - self.gear_shifter_valid = self.gear_shifter in [1,8] # TODO: 1/P allowed for debug self.blinker_on = cp.vl[0x294]['LEFT_BLINKER'] or cp.vl[0x294]['RIGHT_BLINKER'] self.left_blinker_on = cp.vl[0x294]['LEFT_BLINKER'] self.right_blinker_on = cp.vl[0x294]['RIGHT_BLINKER'] + self.cruise_speed_offset = -0.3 + self.park_brake = 0 # TODO + self.brake_hold = 0 # TODO elif self.crv: - self.gear_shifter = cp.vl[0x191]['GEAR_SHIFTER'] + can_gear_shifter = cp.vl[0x191]['GEAR_SHIFTER'] self.angle_steers = cp.vl[0x156]['STEER_ANGLE'] self.gear = cp.vl[0x191]['GEAR'] self.cruise_setting = cp.vl[0x1A6]['CRUISE_SETTING'] self.cruise_buttons = cp.vl[0x1A6]['CRUISE_BUTTONS'] self.main_on = cp.vl[0x1A6]['MAIN_ON'] - self.gear_shifter_valid = self.gear_shifter in [1,8] # TODO: 1/P allowed for debug self.blinker_on = cp.vl[0x294]['LEFT_BLINKER'] or cp.vl[0x294]['RIGHT_BLINKER'] self.left_blinker_on = cp.vl[0x294]['LEFT_BLINKER'] self.right_blinker_on = cp.vl[0x294]['RIGHT_BLINKER'] - else: - self.gear_shifter = cp.vl[0x1A3]['GEAR_SHIFTER'] + self.cruise_speed_offset = -0.3 + self.park_brake = 0 # TODO + self.brake_hold = 0 # TODO + elif self.acura: + can_gear_shifter = cp.vl[0x1A3]['GEAR_SHIFTER'] self.angle_steers = cp.vl[0x156]['STEER_ANGLE'] self.gear = cp.vl[0x1A3]['GEAR'] self.cruise_setting = cp.vl[0x1A6]['CRUISE_SETTING'] self.cruise_buttons = cp.vl[0x1A6]['CRUISE_BUTTONS'] self.main_on = cp.vl[0x1A6]['MAIN_ON'] - self.gear_shifter_valid = self.gear_shifter in [1,4] # TODO: 1/P allowed for debug self.blinker_on = cp.vl[0x294]['LEFT_BLINKER'] or cp.vl[0x294]['RIGHT_BLINKER'] self.left_blinker_on = cp.vl[0x294]['LEFT_BLINKER'] self.right_blinker_on = cp.vl[0x294]['RIGHT_BLINKER'] + self.cruise_speed_offset = calc_cruise_offset(cp.vl[0x37c]['CRUISE_SPEED_OFFSET'], self.v_ego) + self.park_brake = 0 # TODO + self.brake_hold = 0 + + self.gear_shifter = parse_gear_shifter(can_gear_shifter, self.acura) if self.accord: # on the accord, this doesn't seem to include cruise control @@ -398,4 +440,25 @@ def update(self, can_pub_main=None): self.pcm_acc_status = cp.vl[0x17C]['ACC_STATUS'] self.pedal_gas = cp.vl[0x17C]['PEDAL_GAS'] self.hud_lead = cp.vl[0x30C]['HUD_LEAD'] - self.counter_pcm = cp.vl[0x324]['COUNTER'] + + +# carstate standalone tester +if __name__ == '__main__': + import zmq + import time + from selfdrive.services import service_list + context = zmq.Context() + logcan = messaging.sub_sock(context, service_list['can'].port) + + class CarParams(object): + def __init__(self): + self.carFingerprint = "HONDA CIVIC 2016 TOURING" + self.enableGas = 0 + self.enableCruise = 0 + CP = CarParams() + CS = CarState(CP, logcan) + + while 1: + CS.update() + time.sleep(0.01) + diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index c020cc5d4c77ff..730f5ea032ec47 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -1,19 +1,65 @@ #!/usr/bin/env python import os import time -import common.numpy_fast as np +import numpy as np +from common.numpy_fast import clip +from common.realtime import sec_since_boot from selfdrive.config import Conversions as CV +from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET, get_events from .carstate import CarState from .carcontroller import CarController, AH from selfdrive.boardd.boardd import can_capnp_to_can_list - from cereal import car from selfdrive.services import service_list import selfdrive.messaging as messaging -NEW_CAN = os.getenv("OLD_CAN") is None + +def get_compute_gb(): + # generate a function that takes in [desired_accel, current_speed] -> [-1.0, 1.0] + # where -1.0 is max brake and 1.0 is max gas + # see debug/dump_accel_from_fiber.py to see how those parameters were generated + w0 = np.array([[ 1.22056961, -0.39625418, 0.67952657], + [ 1.03691769, 0.78210306, -0.41343188]]) + b0 = np.array([ 0.01536703, -0.14335321, -0.26932889]) + w2 = np.array([[-0.59124422, 0.42899439, 0.38660881], + [ 0.79973811, 0.13178682, 0.08550351], + [-0.15651935, -0.44360259, 0.76910877]]) + b2 = np.array([ 0.15624429, 0.02294923, -0.0341086 ]) + w4 = np.array([[-0.31521443], + [-0.38626176], + [ 0.52667892]]) + b4 = np.array([-0.02922216]) + + def compute_output(dat, w0, b0, w2, b2, w4, b4): + m0 = np.dot(dat, w0) + b0 + m0 = leakyrelu(m0, 0.1) + m2 = np.dot(m0, w2) + b2 + m2 = leakyrelu(m2, 0.1) + m4 = np.dot(m2, w4) + b4 + return m4 + + def leakyrelu(x, alpha): + return np.maximum(x, alpha * x) + + def _compute_gb(accel, speed): + #linearly extrap below v1 using v1 and v2 data + v1 = 5. + v2 = 10. + dat = np.array([accel, speed]) + if speed > 5.: + m4 = compute_output(dat, w0, b0, w2, b2, w4, b4) + else: + dat[1] = v1 + m4v1 = compute_output(dat, w0, b0, w2, b2, w4, b4) + dat[1] = v2 + m4v2 = compute_output(dat, w0, b0, w2, b2, w4, b4) + m4 = (speed - v1) * (m4v2 - m4v1) / (v2 - v1) + m4v1 + return float(m4) + + return _compute_gb + # Car button codes class CruiseButtons: @@ -22,6 +68,7 @@ class CruiseButtons: CANCEL = 2 MAIN = 1 + #car chimes: enumeration from dbc file. Chimes are for alerts and warnings class CM: MUTE = 0 @@ -30,6 +77,7 @@ class CM: REPEATED = 1 CONTINUOUS = 2 + #car beepss: enumeration from dbc file. Beeps are for activ and deactiv class BP: MUTE = 0 @@ -37,12 +85,17 @@ class BP: TRIPLE = 2 REPEATED = 1 + class CarInterface(object): def __init__(self, CP, logcan, sendcan=None): self.logcan = logcan self.CP = CP self.frame = 0 + self.last_enable_pressed = 0 + self.last_enable_sent = 0 + self.gas_pressed_prev = False + self.brake_pressed_prev = False self.can_invalid_count = 0 # *** init the major players *** @@ -54,7 +107,8 @@ def __init__(self, CP, logcan, sendcan=None): self.CC = CarController() if self.CS.accord: - self.accord_msg = [] + # self.accord_msg = [] + raise NotImplementedError @staticmethod def get_params(candidate, fingerprint): @@ -68,6 +122,8 @@ def get_params(candidate, fingerprint): ret.radarName = "nidec" ret.carFingerprint = candidate + ret.safetyModel = car.CarParams.SafetyModels.honda + ret.enableSteer = True ret.enableBrake = True @@ -75,7 +131,7 @@ def get_params(candidate, fingerprint): ret.enableGas = 0x201 in fingerprint ret.enableCruise = not ret.enableGas - # FIXME: hardcoding honda civic 2016 touring wight so it can be used to + # FIXME: hardcoding honda civic 2016 touring params so they can be used to # scale unknown params for other cars m_civic = 2923./2.205 + std_cargo l_civic = 2.70 @@ -86,27 +142,32 @@ def get_params(candidate, fingerprint): cR_civic = 90000 if candidate == "HONDA CIVIC 2016 TOURING": + stop_and_go = True ret.m = m_civic ret.l = l_civic ret.aF = aF_civic ret.sR = 13.0 - ret.steerKp, ret.steerKi = 0.8, 0.24 + # Civic at comma has modified steering FW, so different tuning for the Neo in that car + is_fw_modified = os.getenv("DONGLE_ID") in ['b0f5a01cf604185c'] + ret.steerKp, ret.steerKi = [0.4, 0.12] if is_fw_modified else [0.8, 0.24] elif candidate == "ACURA ILX 2016 ACURAWATCH PLUS": + stop_and_go = False ret.m = 3095./2.205 + std_cargo ret.l = 2.67 ret.aF = ret.l * 0.37 ret.sR = 15.3 # Acura at comma has modified steering FW, so different tuning for the Neo in that car - # FIXME: using dongleId isn't great, better to identify the car than the Neo - is_fw_modified = os.getenv("DONGLE_ID") == 'cb38263377b873ee' + is_fw_modified = os.getenv("DONGLE_ID") in ['cb38263377b873ee'] ret.steerKp, ret.steerKi = [0.4, 0.12] if is_fw_modified else [0.8, 0.24] elif candidate == "HONDA ACCORD 2016 TOURING": + stop_and_go = False ret.m = 3580./2.205 + std_cargo ret.l = 2.74 ret.aF = ret.l * 0.38 ret.sR = 15.3 ret.steerKp, ret.steerKi = 0.8, 0.24 elif candidate == "HONDA CR-V 2016 TOURING": + stop_and_go = False ret.m = 3572./2.205 + std_cargo ret.l = 2.62 ret.aF = ret.l * 0.41 @@ -114,48 +175,54 @@ def get_params(candidate, fingerprint): ret.steerKp, ret.steerKi = 0.8, 0.24 else: raise ValueError("unsupported car %s" % candidate) - + + # min speed to enable ACC. if car can do stop and go, then set enabling speed + # to a negative value, so it won't matter. Otherwise, add 0.5 mph margin to not + # conflict with PCM acc + ret.minEnableSpeed = -1. if (stop_and_go or ret.enableGas) else 25.5 * CV.MPH_TO_MS + ret.aR = ret.l - ret.aF # TODO: get actual value, for now starting with reasonable value for # civic and scaling by mass and wheelbase ret.j = j_civic * ret.m * ret.l**2 / (m_civic * l_civic**2) # TODO: start from empirically derived lateral slip stiffness for the civic and scale by - # mass and CG position... all cars will have approximately similar dyn behaviors + # mass and CG position, so all cars will have approximately similar dyn behaviors ret.cF = cF_civic * ret.m / m_civic * (ret.aR / ret.l) / (aR_civic / l_civic) ret.cR = cR_civic * ret.m / m_civic * (ret.aF / ret.l) / (aF_civic / l_civic) # no rear steering, at least on the listed cars above ret.chi = 0. + # no max steer limit VS speed + ret.steerMaxBP = [0.] # m/s + ret.steerMaxV = [1.] # max steer allowed + ret.gasMaxBP = [0.] # m/s + ret.gasMaxV = [0.6] if ret.enableGas else [0.] # max gas allowed + ret.brakeMaxBP = [5., 20.] # m/s + ret.brakeMaxV = [1., 0.8] # max brake allowed + return ret + compute_gb = staticmethod(get_compute_gb()) + # returns a car.CarState - def update(self): + def update(self, c): # ******************* do can recv ******************* can_pub_main = [] canMonoTimes = [] - if NEW_CAN: - self.CS.update(can_pub_main) - else: - for a in messaging.drain_sock(self.logcan): - canMonoTimes.append(a.logMonoTime) - can_pub_main.extend(can_capnp_to_can_list(a.can, [0,0x80])) - if self.CS.accord: - self.accord_msg.extend(can_capnp_to_can_list(a.can, [9])) - self.accord_msg = self.accord_msg[-1:] - self.CS.update(can_pub_main) + self.CS.update(can_pub_main) # create message ret = car.CarState.new_message() # speeds ret.vEgo = self.CS.v_ego - ret.wheelSpeeds.fl = self.CS.cp.vl[0x1D0]['WHEEL_SPEED_FL'] - ret.wheelSpeeds.fr = self.CS.cp.vl[0x1D0]['WHEEL_SPEED_FR'] - ret.wheelSpeeds.rl = self.CS.cp.vl[0x1D0]['WHEEL_SPEED_RL'] - ret.wheelSpeeds.rr = self.CS.cp.vl[0x1D0]['WHEEL_SPEED_RR'] + ret.wheelSpeeds.fl = self.CS.v_wheel_fl + ret.wheelSpeeds.fr = self.CS.v_wheel_fr + ret.wheelSpeeds.rl = self.CS.v_wheel_rl + ret.wheelSpeeds.rr = self.CS.v_wheel_rr # gas pedal ret.gas = self.CS.car_gas / 256.0 @@ -172,28 +239,17 @@ def update(self): # TODO: units ret.steeringAngle = self.CS.angle_steers - if self.CS.accord: - # TODO: move this into the CAN parser - ret.steeringTorque = 0 - if len(self.accord_msg) > 0: - aa = map(lambda x: ord(x)&0x7f, self.accord_msg[0][2]) - if len(aa) != 5 or (-(aa[0]+aa[1]+aa[2]+aa[3]))&0x7f != aa[4]: - print "ACCORD MSG BAD LEN OR CHECKSUM!" - # TODO: throw an error here? - else: - st = ((aa[0]&0xF) << 5) + (aa[1]&0x1F) - if st >= 256: - st = -(512-st) - ret.steeringTorque = st - ret.steeringPressed = abs(ret.steeringTorque) > 20 - else: - ret.steeringTorque = self.CS.cp.vl[0x18F]['STEER_TORQUE_SENSOR'] - ret.steeringPressed = self.CS.steer_override + # gear shifter lever + ret.gearShifter = self.CS.gear_shifter + + ret.steeringTorque = self.CS.cp.vl[0x18F]['STEER_TORQUE_SENSOR'] + ret.steeringPressed = self.CS.steer_override # cruise state ret.cruiseState.enabled = self.CS.pcm_acc_status != 0 ret.cruiseState.speed = self.CS.v_cruise_pcm * CV.KPH_TO_MS ret.cruiseState.available = bool(self.CS.main_on) + ret.cruiseState.speedOffset = self.CS.cruise_speed_offset # TODO: button presses buttonEvents = [] @@ -244,38 +300,94 @@ def update(self): buttonEvents.append(be) ret.buttonEvents = buttonEvents - # errors + # events # TODO: I don't like the way capnp does enums # These strings aren't checked at compile time - errors = [] + events = [] if not self.CS.can_valid: self.can_invalid_count += 1 if self.can_invalid_count >= 5: - errors.append('commIssue') + events.append(create_event('commIssue', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) else: self.can_invalid_count = 0 if self.CS.steer_error: - errors.append('steerUnavailable') + events.append(create_event('steerUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) elif self.CS.steer_not_allowed: - errors.append('steerTempUnavailable') + events.append(create_event('steerTempUnavailable', [ET.NO_ENTRY, ET.WARNING])) if self.CS.brake_error: - errors.append('brakeUnavailable') - if not self.CS.gear_shifter_valid: - errors.append('wrongGear') + events.append(create_event('brakeUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) + if not ret.gearShifter == 'drive': + events.append(create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if not self.CS.door_all_closed: - errors.append('doorOpen') + events.append(create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if not self.CS.seatbelt: - errors.append('seatbeltNotLatched') + events.append(create_event('seatbeltNotLatched', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if self.CS.esp_disabled: - errors.append('espDisabled') + events.append(create_event('espDisabled', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if not self.CS.main_on: - errors.append('wrongCarMode') - if self.CS.gear_shifter == 2: - errors.append('reverseGear') - - ret.errors = errors + events.append(create_event('wrongCarMode', [ET.NO_ENTRY, ET.USER_DISABLE])) + if ret.gearShifter == 'reverse': + events.append(create_event('reverseGear', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) + if self.CS.brake_hold: + events.append(create_event('brakeHold', [ET.NO_ENTRY, ET.USER_DISABLE])) + if self.CS.park_brake: + events.append(create_event('parkBrake', [ET.NO_ENTRY, ET.USER_DISABLE])) + + if self.CP.enableCruise and ret.vEgo < self.CP.minEnableSpeed: + events.append(create_event('speedTooLow', [ET.NO_ENTRY])) + + # disable on pedals rising edge or when brake is pressed and speed isn't zero + if (ret.gasPressed and not self.gas_pressed_prev) or \ + (ret.brakePressed and (not self.brake_pressed_prev or ret.vEgo > 0.001)): + events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE])) + + #if (ret.brakePressed and ret.vEgo < 0.001) or ret.gasPressed: + if ret.gasPressed: + events.append(create_event('pedalPressed', [ET.PRE_ENABLE])) + + # it can happen that car cruise disables while comma system is enabled: need to + # keep braking if needed or if the speed is very low + # TODO: for the Acura, cancellation below 25mph is normal. Issue a non loud alert + if self.CP.enableCruise and not ret.cruiseState.enabled and \ + (c.actuators.brake <= 0. or ret.vEgo < 0.3): + events.append(create_event("cruiseDisabled", [ET.IMMEDIATE_DISABLE])) + + cur_time = sec_since_boot() + enable_pressed = False + # handle button presses + for b in ret.buttonEvents: + + # do enable on both accel and decel buttons + if b.type in ["accelCruise", "decelCruise"] and not b.pressed: + print "enabled pressed at", cur_time + self.last_enable_pressed = cur_time + enable_pressed = True + + # do disable on button down + if b.type == "cancel" and b.pressed: + events.append(create_event('buttonCancel', [ET.USER_DISABLE])) + + if self.CP.enableCruise: + # KEEP THIS EVENT LAST! send enable event if button is pressed and there are + # NO_ENTRY events, so controlsd will display alerts. Also not send enable events + # too close in time, so a no_entry will not be followed by another one. + # TODO: button press should be the only thing that triggers enble + if ((cur_time - self.last_enable_pressed) < 0.2 and + (cur_time - self.last_enable_sent) > 0.2 and + ret.cruiseState.enabled) or \ + (enable_pressed and get_events(events, [ET.NO_ENTRY])): + events.append(create_event('buttonEnable', [ET.ENABLE])) + self.last_enable_sent = cur_time + elif enable_pressed: + events.append(create_event('buttonEnable', [ET.ENABLE])) + + ret.events = events ret.canMonoTimes = canMonoTimes + # update previous brake/gas pressed + self.gas_pressed_prev = ret.gasPressed + self.brake_pressed_prev = ret.brakePressed + # cast to reader so it can't be modified #print ret return ret.as_reader() @@ -309,10 +421,10 @@ def apply(self, c): "chimeRepeated": (BP.MUTE, CM.REPEATED), "chimeContinuous": (BP.MUTE, CM.CONTINUOUS)}[str(c.hudControl.audibleAlert)] - pcm_accel = int(np.clip(c.cruiseControl.accelOverride,0,1)*0xc6) + pcm_accel = int(clip(c.cruiseControl.accelOverride,0,1)*0xc6) self.CC.update(self.sendcan, c.enabled, self.CS, self.frame, \ - c.gas, c.brake, c.steeringTorque, \ + c.actuators, \ c.cruiseControl.speedOverride, \ c.cruiseControl.override, \ c.cruiseControl.cancel, \ @@ -324,4 +436,3 @@ def apply(self, c): snd_chime = snd_chime) self.frame += 1 - return not (c.enabled and not self.CC.controls_allowed) diff --git a/selfdrive/car/honda/can_parser.py b/selfdrive/car/honda/old_can_parser.py similarity index 97% rename from selfdrive/car/honda/can_parser.py rename to selfdrive/car/honda/old_can_parser.py index d12a33fe1e4f09..6f7f3dce3d6131 100644 --- a/selfdrive/car/honda/can_parser.py +++ b/selfdrive/car/honda/old_can_parser.py @@ -36,15 +36,16 @@ def __init__(self, dbc_f, signals, checks=[]): for _, addr, _ in signals: self.vl[addr] = {} - self.ts[addr] = 0 + self.ts[addr] = {} self.ct[addr] = sec_since_boot() - self.ok[addr] = False + self.ok[addr] = True self.cn[addr] = 0 self.cn_vl[addr] = 0 self.ck[addr] = False for name, addr, ival in signals: self.vl[addr][name] = ival + self.ts[addr][name] = 0 self._msgs = [s[1] for s in signals] self._sgs = [s[0] for s in signals] @@ -94,7 +95,6 @@ def update_can(self, can_recv): self.ok[msg] = False # update msg time stamps and counter value - self.ts[msg] = ts self.ct[msg] = self.sec_since_boot_cached self.cn[msg] = cn self.cn_vl[msg] = min(max(self.cn_vl[msg], 0), cn_vl_max) @@ -107,12 +107,14 @@ def update_can(self, can_recv): for ii in idxs: sg = self._sgs[ii] self.vl[msg][sg] = out[sg] + self.ts[msg][sg] = ts # for each message, check if it's too long since last time we received it self._check_dead_msgs() # assess overall can validity: if there is one relevant message invalid, then set can validity flag to False self.can_valid = True + if False in self.ok.values(): #print "CAN INVALID!" self.can_valid = False diff --git a/selfdrive/common/cereal.mk b/selfdrive/common/cereal.mk index f4603dc32d5c42..637a1040610628 100644 --- a/selfdrive/common/cereal.mk +++ b/selfdrive/common/cereal.mk @@ -1,14 +1,27 @@ UNAME_M ?= $(shell uname -m) -ifeq ($(UNAME_M),x86_64) +UNAME_S ?= $(shell uname -s) -CEREAL_CFLAGS = -I$(ONE)/external/capnp/include + +ifeq ($(UNAME_S),Darwin) + +CEREAL_CFLAGS = -I$(PHONELIBS)/capnp-c/include +CEREAL_CXXFLAGS = -I$(PHONELIBS)/capnp-cpp/mac/include + +CEREAL_LIBS = $(PHONELIBS)/capnp-cpp/mac/lib/libcapnp.a \ + $(PHONELIBS)/capnp-cpp/mac/lib/libkj.a \ + $(PHONELIBS)/capnp-c/mac/lib/libcapnp_c.a + +else ifeq ($(UNAME_M),x86_64) + +CEREAL_CFLAGS = -I$(BASEDIR)/external/capnp/include CEREAL_CXXFLAGS = $(CEREAL_CFLAGS) ifeq ($(CEREAL_LIBS),) - CEREAL_LIBS = -L$(ONE)/external/capnp/lib \ + CEREAL_LIBS = -L$(BASEDIR)/external/capnp/lib \ -l:libcapnp.a -l:libkj.a -l:libcapnp_c.a endif else + CEREAL_CFLAGS = -I$(PHONELIBS)/capnp-c/include CEREAL_CXXFLAGS = -I$(PHONELIBS)/capnp-cpp/include ifeq ($(CEREAL_LIBS),) @@ -30,4 +43,3 @@ car.capnp.o: ../../cereal/gen/cpp/car.capnp.c++ @echo "[ CXX ] $@" $(CXX) $(CXXFLAGS) $(CEREAL_CXXFLAGS) \ -c -o '$@' '$<' - diff --git a/selfdrive/common/mat.h b/selfdrive/common/mat.h index f429b0c5d37a39..1c20eae17fd034 100644 --- a/selfdrive/common/mat.h +++ b/selfdrive/common/mat.h @@ -65,4 +65,24 @@ static inline vec4 matvecmul(const mat4 a, const vec4 b) { return ret; } +// scales the input and output space of a transformation matrix +// that assumes pixel-center origin. +static inline mat3 transform_scale_buffer(const mat3 in, float s) { + // in_pt = ( transform(out_pt/s + 0.5) - 0.5) * s + + mat3 transform_out = {{ + 1.0f/s, 0.0f, 0.5f, + 0.0f, 1.0f/s, 0.5f, + 0.0f, 0.0f, 1.0f, + }}; + + mat3 transform_in = {{ + s, 0.0f, -0.5f*s, + 0.0f, s, -0.5f*s, + 0.0f, 0.0f, 1.0f, + }}; + + return matmul3(transform_in, matmul3(in, transform_out)); +} + #endif diff --git a/selfdrive/common/params.c b/selfdrive/common/params.cc similarity index 83% rename from selfdrive/common/params.c rename to selfdrive/common/params.cc index 7cefc7b06b4490..5b0d216bdd873c 100644 --- a/selfdrive/common/params.c +++ b/selfdrive/common/params.cc @@ -11,6 +11,18 @@ #include #include +namespace { + +template +T* null_coalesce(T* a, T* b) { + return a != NULL ? a : b; +} + +static const char* default_params_path = null_coalesce( + const_cast(getenv("PARAMS_PATH")), "/data/params"); + +} // namespace + int write_db_value(const char* params_path, const char* key, const char* value, size_t value_size) { int lock_fd = -1; @@ -18,6 +30,11 @@ int write_db_value(const char* params_path, const char* key, const char* value, int result; char tmp_path[1024]; char path[1024]; + ssize_t bytes_written; + + if (params_path == NULL) { + params_path = default_params_path; + } // Write value to temp. result = @@ -27,7 +44,7 @@ int write_db_value(const char* params_path, const char* key, const char* value, } tmp_fd = mkstemp(tmp_path); - const ssize_t bytes_written = write(tmp_fd, value, value_size); + bytes_written = write(tmp_fd, value, value_size); if (bytes_written != value_size) { result = -20; goto cleanup; @@ -79,6 +96,10 @@ int read_db_value(const char* params_path, const char* key, char** value, int result; char path[1024]; + if (params_path == NULL) { + params_path = default_params_path; + } + result = snprintf(path, sizeof(path), "%s/.lock", params_path); if (result < 0) { goto cleanup; @@ -99,7 +120,7 @@ int read_db_value(const char* params_path, const char* key, char** value, // Read value. // TODO(mgraczyk): If there is a lot of contention, we can release the lock // after opening the file, before reading. - *value = read_file(path, value_sz); + *value = static_cast(read_file(path, value_sz)); if (*value == NULL) { result = -22; goto cleanup; diff --git a/selfdrive/common/params.h b/selfdrive/common/params.h index 05ddb1dd791687..bd4d86dcfee97c 100644 --- a/selfdrive/common/params.h +++ b/selfdrive/common/params.h @@ -7,14 +7,12 @@ extern "C" { #endif -#define PARAMS_PATH "/data/params" - int write_db_value(const char* params_path, const char* key, const char* value, size_t value_size); // Reads a value from the params database. // Inputs: -// params_path: The path of the database, eg /sdcard/params. +// params_path: The path of the database, or NULL to use the default. // key: The key to read. // value: A pointer where a newly allocated string containing the db value will // be written. diff --git a/selfdrive/common/swaglog.h b/selfdrive/common/swaglog.h index b972f98d18fe23..33e77f618aef59 100644 --- a/selfdrive/common/swaglog.h +++ b/selfdrive/common/swaglog.h @@ -1,6 +1,8 @@ #ifndef SWAGLOG_H #define SWAGLOG_H +#include "common/timing.h" + #define CLOUDLOG_DEBUG 10 #define CLOUDLOG_INFO 20 #define CLOUDLOG_WARNING 30 @@ -24,9 +26,43 @@ void cloudlog_bind(const char* k, const char* v); __func__, \ fmt, ## __VA_ARGS__) +#define cloudlog_rl(burst, millis, lvl, fmt, ...) \ +{ \ + static uint64_t __begin = 0; \ + static int __printed = 0; \ + static int __missed = 0; \ + \ + int __burst = (burst); \ + int __millis = (millis); \ + uint64_t __ts = nanos_since_boot(); \ + \ + if (!__begin) __begin = __ts; \ + \ + if (__begin + __millis*1000000ULL < __ts) { \ + if (__missed) { \ + cloudlog(CLOUDLOG_WARNING, "cloudlog: %d messages supressed", __missed); \ + } \ + __begin = 0; \ + __printed = 0; \ + __missed = 0; \ + } \ + \ + if (__printed < __burst) { \ + cloudlog(lvl, fmt, ## __VA_ARGS__); \ + __printed++; \ + } else { \ + __missed++; \ + } \ +} + #define LOGD(fmt, ...) cloudlog(CLOUDLOG_DEBUG, fmt, ## __VA_ARGS__) #define LOG(fmt, ...) cloudlog(CLOUDLOG_INFO, fmt, ## __VA_ARGS__) #define LOGW(fmt, ...) cloudlog(CLOUDLOG_WARNING, fmt, ## __VA_ARGS__) #define LOGE(fmt, ...) cloudlog(CLOUDLOG_ERROR, fmt, ## __VA_ARGS__) +#define LOGD_100(fmt, ...) cloudlog_rl(2, 100, CLOUDLOG_DEBUG, fmt, ## __VA_ARGS__) +#define LOG_100(fmt, ...) cloudlog_rl(2, 100, CLOUDLOG_INFO, fmt, ## __VA_ARGS__) +#define LOGW_100(fmt, ...) cloudlog_rl(2, 100, CLOUDLOG_WARNING, fmt, ## __VA_ARGS__) +#define LOGE_100(fmt, ...) cloudlog_rl(2, 100, CLOUDLOG_ERROR, fmt, ## __VA_ARGS__) + #endif diff --git a/selfdrive/common/timing.h b/selfdrive/common/timing.h index 02e934dcb5f253..1a30ad6e1e4d7b 100644 --- a/selfdrive/common/timing.h +++ b/selfdrive/common/timing.h @@ -5,7 +5,7 @@ #include #ifdef __APPLE__ -#define CLOCK_BOOTTIME CLOCK_REALTIME +#define CLOCK_BOOTTIME CLOCK_MONOTONIC #endif static inline uint64_t nanos_since_boot() { @@ -38,4 +38,17 @@ static inline double seconds_since_epoch() { return (double)t.tv_sec + t.tv_nsec * 1e-9; } +// you probably should use nanos_since_boot instead +static inline uint64_t nanos_monotonic() { + struct timespec t; + clock_gettime(CLOCK_MONOTONIC, &t); + return t.tv_sec * 1000000000ULL + t.tv_nsec; +} + +static inline uint64_t nanos_monotonic_raw() { + struct timespec t; + clock_gettime(CLOCK_MONOTONIC_RAW, &t); + return t.tv_sec * 1000000000ULL + t.tv_nsec; +} + #endif diff --git a/selfdrive/common/util.c b/selfdrive/common/util.c index e5ab4d0a6c86a1..172345869ec8a4 100644 --- a/selfdrive/common/util.c +++ b/selfdrive/common/util.c @@ -3,6 +3,10 @@ #include #include +#ifdef __linux__ +#include +#endif + void* read_file(const char* path, size_t* out_len) { FILE* f = fopen(path, "r"); if (!f) { @@ -28,3 +32,10 @@ void* read_file(const char* path, size_t* out_len) { return buf; } + +void set_thread_name(const char* name) { +#ifdef __linux__ + // pthread_setname_np is dumb (fails instead of truncates) + prctl(PR_SET_NAME, (unsigned long)name, 0, 0, 0); +#endif +} diff --git a/selfdrive/common/util.h b/selfdrive/common/util.h index 9b5da8e726863b..ce18068bbe7c7d 100644 --- a/selfdrive/common/util.h +++ b/selfdrive/common/util.h @@ -1,6 +1,9 @@ #ifndef COMMON_UTIL_H #define COMMON_UTIL_H + +#ifndef __cplusplus + #define min(a,b) \ ({ __typeof__ (a) _a = (a); \ __typeof__ (b) _b = (b); \ @@ -11,6 +14,8 @@ __typeof__ (b) _b = (b); \ _a > _b ? _a : _b; }) +#endif + #define clamp(a,b,c) \ ({ __typeof__ (a) _a = (a); \ __typeof__ (b) _b = (b); \ @@ -19,11 +24,20 @@ #define ARRAYSIZE(x) (sizeof(x)/sizeof(x[0])) +#ifdef __cplusplus +extern "C" { +#endif + // Reads a file into a newly allocated buffer. // // Returns NULL on failure, otherwise the NULL-terminated file contents. // The result must be freed by the caller. void* read_file(const char* path, size_t* out_len); +void set_thread_name(const char* name); + +#ifdef __cplusplus +} +#endif #endif diff --git a/selfdrive/common/utilpp.h b/selfdrive/common/utilpp.h index 25abac66825883..bc1b2fd267d966 100644 --- a/selfdrive/common/utilpp.h +++ b/selfdrive/common/utilpp.h @@ -9,6 +9,10 @@ #include #include +#ifdef __x86_64 +#include +#endif + namespace util { inline bool starts_with(std::string s, std::string prefix) { diff --git a/selfdrive/common/version.h b/selfdrive/common/version.h index 4c2a881ae99340..de335592cdb0e1 100644 --- a/selfdrive/common/version.h +++ b/selfdrive/common/version.h @@ -1 +1 @@ -#define OPENPILOT_VERSION "0.3.6.1" +#define OPENPILOT_VERSION "0.3.7" diff --git a/selfdrive/common/visionipc.c b/selfdrive/common/visionipc.c index c371754f88fcc6..4299f512f9a04e 100644 --- a/selfdrive/common/visionipc.c +++ b/selfdrive/common/visionipc.c @@ -1,6 +1,7 @@ #include #include #include +#include #include #include #include @@ -126,7 +127,7 @@ int vipc_send(int fd, const VisionPacket *p2) { return sendrecv_with_fds(true, fd, (void*)&p, sizeof(p), (int*)p2->fds, p2->num_fds, NULL); } -void visionbufs_load(VisionBuf *bufs, const VisionStreamBufs *stream_bufs, +void vipc_bufs_load(VIPCBuf *bufs, const VisionStreamBufs *stream_bufs, int num_fds, const int* fds) { for (int i=0; ibufs_info = rp.d.stream_bufs; s->num_bufs = rp.num_fds; - s->bufs = calloc(s->num_bufs, sizeof(VisionBuf)); + s->bufs = calloc(s->num_bufs, sizeof(VIPCBuf)); assert(s->bufs); - visionbufs_load(s->bufs, &rp.d.stream_bufs, s->num_bufs, rp.fds); + vipc_bufs_load(s->bufs, &rp.d.stream_bufs, s->num_bufs, rp.fds); if (out_bufs_info) { *out_bufs_info = s->bufs_info; @@ -192,7 +193,7 @@ int visionstream_init(VisionStream *s, VisionStreamType type, bool tbuffer, Visi return 0; } -VisionBuf* visionstream_get(VisionStream *s, VisionBufExtra *out_extra) { +VIPCBuf* visionstream_get(VisionStream *s, VIPCBufExtra *out_extra) { int err; VisionPacket rp; diff --git a/selfdrive/common/visionipc.h b/selfdrive/common/visionipc.h index dab392ec54c619..24b882340fea87 100644 --- a/selfdrive/common/visionipc.h +++ b/selfdrive/common/visionipc.h @@ -47,9 +47,9 @@ typedef struct VisionStreamBufs { } buf_info; } VisionStreamBufs; -typedef struct VisionBufExtra { +typedef struct VIPCBufExtra { uint32_t frame_id; // only for yuv -} VisionBufExtra; +} VIPCBufExtra; typedef union VisionPacketData { struct { @@ -60,7 +60,7 @@ typedef union VisionPacketData { struct { VisionStreamType type; int idx; - VisionBufExtra extra; + VIPCBufExtra extra; } stream_acq; struct { VisionStreamType type; @@ -79,12 +79,12 @@ int vipc_connect(void); int vipc_recv(int fd, VisionPacket *out_p); int vipc_send(int fd, const VisionPacket *p); -typedef struct VisionBuf { +typedef struct VIPCBuf { int fd; size_t len; void* addr; -} VisionBuf; -void visionbufs_load(VisionBuf *bufs, const VisionStreamBufs *stream_bufs, +} VIPCBuf; +void vipc_bufs_load(VIPCBuf *bufs, const VisionStreamBufs *stream_bufs, int num_fds, const int* fds); @@ -94,11 +94,11 @@ typedef struct VisionStream { int last_idx; int num_bufs; VisionStreamBufs bufs_info; - VisionBuf *bufs; + VIPCBuf *bufs; } VisionStream; int visionstream_init(VisionStream *s, VisionStreamType type, bool tbuffer, VisionStreamBufs *out_bufs_info); -VisionBuf* visionstream_get(VisionStream *s, VisionBufExtra *out_extra); +VIPCBuf* visionstream_get(VisionStream *s, VIPCBufExtra *out_extra); void visionstream_destroy(VisionStream *s); #ifdef __cplusplus diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index b747341de461da..80b25875d7c5c0 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -2,24 +2,24 @@ import os import json from copy import copy - import zmq - from cereal import car, log from common.numpy_fast import clip from common.fingerprints import fingerprint from common.realtime import sec_since_boot, set_realtime_priority, Ratekeeper from common.profiler import Profiler from common.params import Params - import selfdrive.messaging as messaging from selfdrive.swaglog import cloudlog from selfdrive.config import Conversions as CV from selfdrive.services import service_list from selfdrive.car import get_car from selfdrive.controls.lib.planner import Planner -from selfdrive.controls.lib.drive_helpers import learn_angle_offset -from selfdrive.controls.lib.longcontrol import LongControl +from selfdrive.controls.lib.drive_helpers import learn_angle_offset, \ + get_events, \ + create_event, \ + EventTypes as ET +from selfdrive.controls.lib.longcontrol import LongControl, STARTING_TARGET_SPEED from selfdrive.controls.lib.latcontrol import LatControl from selfdrive.controls.lib.alertmanager import AlertManager from selfdrive.controls.lib.vehicle_model import VehicleModel @@ -34,427 +34,475 @@ AWARENESS_PRE_TIME = 20. # a first alert is issued 20s before start decelerating the car AWARENESS_DECEL = -0.2 # car smoothly decel at .2m/s^2 when user is distracted -# class Cal + class Calibration: UNCALIBRATED = 0 CALIBRATED = 1 INVALID = 2 -# to be used -class State(): + +class State: DISABLED = 0 ENABLED = 1 - SOFT_DISABLE = 2 - -class Controls(object): - def __init__(self, gctx, rate=100): - self.rate = rate - - # *** log *** - context = zmq.Context() - - # pub - self.live100 = messaging.pub_sock(context, service_list['live100'].port) - self.carstate = messaging.pub_sock(context, service_list['carState'].port) - self.carcontrol = messaging.pub_sock(context, service_list['carControl'].port) - sendcan = messaging.pub_sock(context, service_list['sendcan'].port) - - # sub - self.thermal = messaging.sub_sock(context, service_list['thermal'].port) - self.health = messaging.sub_sock(context, service_list['health'].port) - logcan = messaging.sub_sock(context, service_list['can'].port) - self.cal = messaging.sub_sock(context, service_list['liveCalibration'].port) - - self.CC = car.CarControl.new_message() - self.CI, self.CP = get_car(logcan, sendcan) - self.PL = Planner(self.CP) - self.AM = AlertManager() - self.LoC = LongControl() - self.LaC = LatControl() - self.VM = VehicleModel(self.CP) - - # write CarParams - params = Params() - params.put("CarParams", self.CP.to_bytes()) - - # fake plan - self.plan_ts = 0 - self.plan = log.Plan.new_message() - self.plan.lateralValid = False - self.plan.longitudinalValid = False - - # controls enabled state - self.enabled = False - self.last_enable_request = 0 - - # learned angle offset - self.angle_offset = 0 - calibration_params = params.get("CalibrationParams") - if calibration_params: - try: - calibration_params = json.loads(calibration_params) - self.angle_offset = calibration_params["angle_offset"] - except (ValueError, KeyError): - pass - - # rear view camera state - self.rear_view_toggle = False - self.rear_view_allowed = bool(params.get("IsRearViewMirror")) - - self.v_cruise_kph = 255 - - # 0.0 - 1.0 - self.awareness_status = 1.0 - - self.soft_disable_timer = None - - self.overtemp = False - self.free_space = 1.0 - self.cal_status = Calibration.UNCALIBRATED - self.cal_perc = 0 - - self.rk = Ratekeeper(self.rate, print_delay_threshold=2./1000) - - def data_sample(self): - self.prof = Profiler() - self.cur_time = sec_since_boot() - # first read can and compute car states - self.CS = self.CI.update() - - self.prof.checkpoint("CarInterface") - - # *** thermal checking logic *** - # thermal data, checked every second - td = messaging.recv_sock(self.thermal) - if td is not None: - # Check temperature. - self.overtemp = any( - t > 950 - for t in (td.thermal.cpu0, td.thermal.cpu1, td.thermal.cpu2, - td.thermal.cpu3, td.thermal.mem, td.thermal.gpu)) - # under 15% of space free - self.free_space = td.thermal.freeSpace - - # read calibration status - cal = messaging.recv_sock(self.cal) - if cal is not None: - self.cal_status = cal.liveCalibration.calStatus - self.cal_perc = cal.liveCalibration.calPerc - - - def state_transition(self): - pass # for now - - def state_control(self): - - # did it request to enable? - enable_request, enable_condition = False, False - - # reset awareness status on steering - if self.CS.steeringPressed or not self.enabled: - self.awareness_status = 1.0 - elif self.enabled: - # gives the user 6 minutes - self.awareness_status -= 1.0/(self.rate * AWARENESS_TIME) - if self.awareness_status <= 0.: - self.AM.add("driverDistracted", self.enabled) - elif self.awareness_status <= AWARENESS_PRE_TIME / AWARENESS_TIME and \ - self.awareness_status >= (AWARENESS_PRE_TIME - 0.1) / AWARENESS_TIME: - self.AM.add("preDriverDistracted", self.enabled) - - # handle button presses - for b in self.CS.buttonEvents: - print b - - # button presses for rear view - if b.type == "leftBlinker" or b.type == "rightBlinker": - if b.pressed and self.rear_view_allowed: - self.rear_view_toggle = True - else: - self.rear_view_toggle = False - - if b.type == "altButton1" and b.pressed: - self.rear_view_toggle = not self.rear_view_toggle - - if not self.CP.enableCruise and self.enabled and not b.pressed: - if b.type == "accelCruise": - self.v_cruise_kph -= (self.v_cruise_kph % V_CRUISE_DELTA) - V_CRUISE_DELTA - elif b.type == "decelCruise": - self.v_cruise_kph -= (self.v_cruise_kph % V_CRUISE_DELTA) + V_CRUISE_DELTA - self.v_cruise_kph = clip(self.v_cruise_kph, V_CRUISE_MIN, V_CRUISE_MAX) - - if not self.enabled and b.type in ["accelCruise", "decelCruise"] and not b.pressed: - enable_request = True - - # do disable on button down - if b.type == "cancel" and b.pressed: - self.AM.add("disable", self.enabled) - - self.prof.checkpoint("Buttons") - - # *** health checking logic *** - hh = messaging.recv_sock(self.health) - if hh is not None: - # if the board isn't allowing controls but somehow we are enabled! - # TODO: this should be in state transition with a function follower logic - if not hh.health.controlsAllowed and self.enabled: - self.AM.add("controlsMismatch", self.enabled) - - # disable if the pedals are pressed while engaged, this is a user disable - if self.enabled: - if self.CS.gasPressed or self.CS.brakePressed or not self.CS.cruiseState.available: - self.AM.add("disable", self.enabled) - - # it can happen that car cruise disables while comma system is enabled: need to - # keep braking if needed or if the speed is very low - # TODO: for the Acura, cancellation below 25mph is normal. Issue a non loud alert - if self.CP.enableCruise and not self.CS.cruiseState.enabled and \ - (self.CC.brake <= 0. or self.CS.vEgo < 0.3): - self.AM.add("cruiseDisabled", self.enabled) - - if enable_request: - # check for pressed pedals - if self.CS.gasPressed or self.CS.brakePressed: - self.AM.add("pedalPressed", self.enabled) - enable_request = False - else: - print "enabled pressed at", self.cur_time - self.last_enable_request = self.cur_time + PRE_ENABLED = 2 + SOFT_DISABLING = 3 - # don't engage with less than 15% free - if self.free_space < 0.15: - self.AM.add("outOfSpace", self.enabled) - enable_request = False - if self.CP.enableCruise: - enable_condition = ((self.cur_time - self.last_enable_request) < 0.2) and self.CS.cruiseState.enabled - else: - enable_condition = enable_request +# True when actuators are controlled +def isActive(state): + return state in [State.ENABLED, State.SOFT_DISABLING] - if self.CP.enableCruise and self.CS.cruiseState.enabled: - self.v_cruise_kph = self.CS.cruiseState.speed * CV.MS_TO_KPH - self.prof.checkpoint("AdaptiveCruise") +# True if system is engaged +def isEnabled(state): + return (isActive(state) or state == State.PRE_ENABLED) - # *** what's the plan *** - plan_packet = self.PL.update(self.CS, self.LoC) - self.plan = plan_packet.plan - self.plan_ts = plan_packet.logMonoTime - # if user is not responsive to awareness alerts, then start a smooth deceleration - if self.awareness_status < -0.: - self.plan.aTargetMax = min(self.plan.aTargetMax, AWARENESS_DECEL) - self.plan.aTargetMin = min(self.plan.aTargetMin, self.plan.aTargetMax) +def data_sample(CI, CC, thermal, health, cal, cal_status, overtemp, free_space): - if enable_request or enable_condition or self.enabled: - # add all alerts from car - for alert in self.CS.errors: - self.AM.add(alert, self.enabled) + # *** read can and compute car states *** + CS = CI.update(CC) + events = list(CS.events) - if not self.plan.longitudinalValid: - self.AM.add("radarCommIssue", self.enabled) + # *** thermal checking logic *** + # thermal data, checked every second + td = messaging.recv_sock(thermal) + if td is not None: + # overtemp above 95 deg + overtemp = any( + t > 950 + for t in (td.thermal.cpu0, td.thermal.cpu1, td.thermal.cpu2, + td.thermal.cpu3, td.thermal.mem, td.thermal.gpu)) - if self.cal_status != Calibration.CALIBRATED: - if self.cal_status == Calibration.UNCALIBRATED: - self.AM.add("calibrationInProgress", self.enabled, str(self.cal_perc) + '%') + # under 15% of space free no enable allowed + free_space = td.thermal.freeSpace < 0.15 + + if overtemp: + events.append(create_event('overheat', [ET.NO_ENTRY, ET.SOFT_DISABLE])) + + if free_space: + events.append(create_event('outOfSpace', [ET.NO_ENTRY])) + + # *** read calibration status *** + cal = messaging.recv_sock(cal) + if cal is not None: + cal_status = cal.liveCalibration.calStatus + + if cal_status != Calibration.CALIBRATED: + if cal_status == Calibration.UNCALIBRATED: + events.append(create_event('calibrationInProgress', [ET.NO_ENTRY, ET.SOFT_DISABLE])) + else: + events.append(create_event('calibrationInvalid', [ET.NO_ENTRY, ET.SOFT_DISABLE])) + + # *** health checking logic *** + hh = messaging.recv_sock(health) + if hh is not None: + controls_allowed = hh.health.controlsAllowed + if not controls_allowed: + events.append(create_event('controlsMismatch', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) + + return CS, events, cal_status, overtemp, free_space + + +def calc_plan(CS, events, PL, LoC): + # plan runs always, independently of the state + plan_packet = PL.update(CS, LoC) + plan = plan_packet.plan + plan_ts = plan_packet.logMonoTime + + # add events from planner + events += list(plan.events) + + # disable if lead isn't close when system is active and brake is pressed to avoid + # unexpected vehicle accelerations + if CS.brakePressed and plan.vTarget >= STARTING_TARGET_SPEED: + events.append(create_event('noTarget', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) + + return plan, plan_ts + + +def state_transition(CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM): + # compute conditional state transitions and execute actions on state transitions + enabled = isEnabled(state) + + # handle button presses. TODO: this should be in state_control, but a decelCruise press + # would have the effect of both enabling and changing speed is checked after the state transition + for b in CS.buttonEvents: + if not CP.enableCruise and enabled and not b.pressed: + if b.type == "accelCruise": + v_cruise_kph -= (v_cruise_kph % V_CRUISE_DELTA) - V_CRUISE_DELTA + elif b.type == "decelCruise": + v_cruise_kph -= (v_cruise_kph % V_CRUISE_DELTA) + V_CRUISE_DELTA + v_cruise_kph = clip(v_cruise_kph, V_CRUISE_MIN, V_CRUISE_MAX) + + # decrease the soft disable timer at every step, as it's reset on + # entrance in SOFT_DISABLING state + soft_disable_timer = max(0, soft_disable_timer - 1) + + # ***** handle state transitions ***** + + # DISABLED + if state == State.DISABLED: + if get_events(events, [ET.ENABLE]): + if get_events(events, [ET.NO_ENTRY, ET.SOFT_DISABLE, ET.IMMEDIATE_DISABLE]): + for e in get_events(events, [ET.SOFT_DISABLE, ET.IMMEDIATE_DISABLE]): + AM.add(e, enabled) + for e in get_events(events, [ET.NO_ENTRY]): + AM.add(str(e) + "NoEntry", enabled) + else: + if get_events(events, [ET.PRE_ENABLE]): + state = State.PRE_ENABLED else: - self.AM.add("calibrationInvalid", self.enabled) - - if not self.plan.lateralValid: - # If the model is not broadcasting, assume that it is because - # the user has uploaded insufficient data for calibration. - # Other cases that would trigger this are rare and unactionable by the user. - self.AM.add("dataNeeded", self.enabled) - - if self.overtemp: - self.AM.add("overheat", self.enabled) - - - # *** angle offset learning *** - if self.rk.frame % 5 == 2 and self.plan.lateralValid: - # *** run this at 20hz again *** - self.angle_offset = learn_angle_offset(self.enabled, self.CS.vEgo, self.angle_offset, - self.PL.PP.c_poly, self.PL.PP.c_prob, self.LaC.y_des, - self.CS.steeringPressed) - - # *** gas/brake PID loop *** - final_gas, final_brake = self.LoC.update(self.enabled, self.CS.vEgo, self.v_cruise_kph, - self.plan.vTarget, - [self.plan.aTargetMin, self.plan.aTargetMax], - self.plan.jerkFactor, self.CP) - - # *** steering PID loop *** - final_steer, sat_flag = self.LaC.update(self.enabled, self.CS.vEgo, self.CS.steeringAngle, - self.CS.steeringPressed, self.plan.dPoly, self.angle_offset, self.VM) - - self.prof.checkpoint("PID") - - # ***** handle alerts **** - # send FCW alert if triggered by planner - if self.plan.fcw: - self.AM.add("fcw", self.enabled) - - # send a "steering required alert" if saturation count has reached the limit - if sat_flag: - self.AM.add("steerSaturated", self.enabled) - - if self.enabled and self.AM.alertShouldDisable(): - print "DISABLING IMMEDIATELY ON ALERT" - self.enabled = False - - if self.enabled and self.AM.alertShouldSoftDisable(): - if self.soft_disable_timer is None: - self.soft_disable_timer = 3 * self.rate - elif self.soft_disable_timer == 0: - print "SOFT DISABLING ON ALERT" - self.enabled = False + state = State.ENABLED + AM.add("enable", enabled) + # on activation, let's always set v_cruise from where we are, even if PCM ACC is active + v_cruise_kph = int(round(max(CS.vEgo * CV.MS_TO_KPH, V_CRUISE_ENABLE_MIN))) + + # ENABLED + elif state == State.ENABLED: + if get_events(events, [ET.USER_DISABLE]): + state = State.DISABLED + AM.add("disable", enabled) + + elif get_events(events, [ET.IMMEDIATE_DISABLE]): + state = State.DISABLED + for e in get_events(events, [ET.IMMEDIATE_DISABLE]): + AM.add(e, enabled) + + elif get_events(events, [ET.SOFT_DISABLE]): + state = State.SOFT_DISABLING + soft_disable_timer = 300 # 3s TODO: use rate + for e in get_events(events, [ET.SOFT_DISABLE]): + AM.add(e, enabled) + + # SOFT DISABLING + elif state == State.SOFT_DISABLING: + if get_events(events, [ET.USER_DISABLE]): + state = State.DISABLED + AM.add("disable", enabled) + + elif get_events(events, [ET.IMMEDIATE_DISABLE]): + state = State.DISABLED + for e in get_events(events, [ET.IMMEDIATE_DISABLE]): + AM.add(e, enabled) + + elif not get_events(events, [ET.SOFT_DISABLE]): + # no more soft disabling condition, so go back to ENABLED + state = State.ENABLED + + elif soft_disable_timer <= 0: + state = State.DISABLED + + # TODO: PRE ENABLING + elif state == State.PRE_ENABLED: + if get_events(events, [ET.USER_DISABLE]): + state = State.DISABLED + AM.add("disable", enabled) + + elif get_events(events, [ET.IMMEDIATE_DISABLE, ET.SOFT_DISABLE]): + state = State.DISABLED + for e in get_events(events, [ET.IMMEDIATE_DISABLE, ET.SOFT_DISABLE]): + AM.add(e, enabled) + + elif not get_events(events, [ET.PRE_ENABLE]): + state = State.ENABLED + + return state, soft_disable_timer, v_cruise_kph + + +def state_control(plan, CS, CP, state, events, v_cruise_kph, AM, rk, awareness_status, + PL, LaC, LoC, VM, angle_offset, rear_view_allowed, rear_view_toggle): + # Given the state, this function returns the actuators + + # reset actuators to zero + actuators = car.CarControl.Actuators.new_message() + + enabled = isEnabled(state) + active = isActive(state) + + for b in CS.buttonEvents: + # any button event resets awarenesss_status + awareness_status = 1. + + # button presses for rear view + if b.type == "leftBlinker" or b.type == "rightBlinker": + if b.pressed and rear_view_allowed: + rear_view_toggle = True else: - self.soft_disable_timer -= 1 - else: - self.soft_disable_timer = None - - if enable_condition and not self.enabled and not self.AM.alertPresent(): - print "*** enabling controls" - - # beep for enabling - self.AM.add("enable", self.enabled) - - # enable both lateral and longitudinal controls - self.enabled = True - - # on activation, let's always set v_cruise from where we are, even if PCM ACC is active - self.v_cruise_kph = int(round(max(self.CS.vEgo * CV.MS_TO_KPH, V_CRUISE_ENABLE_MIN))) - - # 6 minutes driver you're on - self.awareness_status = 1.0 - - # reset the PID loops - self.LaC.reset() - # start long control at actual speed - self.LoC.reset(v_pid = self.CS.vEgo) - - # *** push the alerts to current *** - # TODO: remove output, store them inside AM class instead - self.alert_text_1, self.alert_text_2, self.visual_alert, self.audible_alert = self.AM.process_alerts(self.cur_time) - - # ***** control the car ***** - self.CC.enabled = self.enabled - - self.CC.gas = float(final_gas) - self.CC.brake = float(final_brake) - self.CC.steeringTorque = float(final_steer) - - self.CC.cruiseControl.override = True - # always cancel if we have an interceptor - self.CC.cruiseControl.cancel = bool(not self.CP.enableCruise or - (not self.enabled and self.CS.cruiseState.enabled)) - - # brake discount removes a sharp nonlinearity - brake_discount = (1.0 - clip(final_brake*3., 0.0, 1.0)) - self.CC.cruiseControl.speedOverride = float(max(0.0, ((self.LoC.v_pid - .5) * brake_discount)) if self.CP.enableCruise else 0.0) - - #CC.cruiseControl.accelOverride = float(AC.a_pcm) - # TODO: parametrize 0.714 in interface? - # accelOverride is more or less the max throttle allowed to pcm: usually set to a constant - # unless aTargetMax is very high and then we scale with it; this helpw in quicker restart - self.CC.cruiseControl.accelOverride = float(max(0.714, self.plan.aTargetMax/A_ACC_MAX)) - - self.CC.hudControl.setSpeed = float(self.v_cruise_kph * CV.KPH_TO_MS) - self.CC.hudControl.speedVisible = self.enabled - self.CC.hudControl.lanesVisible = self.enabled - self.CC.hudControl.leadVisible = self.plan.hasLead - self.CC.hudControl.visualAlert = self.visual_alert - self.CC.hudControl.audibleAlert = self.audible_alert - - # TODO: remove it from here and put it in state_transition - # this alert will apply next controls cycle - if not self.CI.apply(self.CC): - self.AM.add("controlsFailed", self.enabled) - - def data_send(self): - - # broadcast carControl first - cc_send = messaging.new_message() - cc_send.init('carControl') - cc_send.carControl = copy(self.CC) - self.carcontrol.send(cc_send.to_bytes()) - - self.prof.checkpoint("CarControl") - - # broadcast carState - cs_send = messaging.new_message() - cs_send.init('carState') - cs_send.carState = copy(self.CS) - self.carstate.send(cs_send.to_bytes()) - - # ***** publish state to logger ***** - - # publish controls state at 100Hz - dat = messaging.new_message() - dat.init('live100') + rear_view_toggle = False - # show rear view camera on phone if in reverse gear or when button is pressed - dat.live100.rearViewCam = ('reverseGear' in self.CS.errors and self.rear_view_allowed) or self.rear_view_toggle - dat.live100.alertText1 = self.alert_text_1 - dat.live100.alertText2 = self.alert_text_2 - dat.live100.awarenessStatus = max(self.awareness_status, 0.0) if self.enabled else 0.0 + if b.type == "altButton1" and b.pressed: + rear_view_toggle = not rear_view_toggle - # what packets were used to process - dat.live100.canMonoTimes = list(self.CS.canMonoTimes) - dat.live100.planMonoTime = self.plan_ts - # if controls is enabled - dat.live100.enabled = self.enabled + # send FCW alert if triggered by planner + if plan.fcw: + AM.add("fcw", enabled) - # car state - dat.live100.vEgo = self.CS.vEgo - dat.live100.angleSteers = self.CS.steeringAngle - dat.live100.steerOverride = self.CS.steeringPressed + # ***** state specific actions ***** - # longitudinal control state - dat.live100.vPid = float(self.LoC.v_pid) - dat.live100.vCruise = float(self.v_cruise_kph) - dat.live100.upAccelCmd = float(self.LoC.Up_accel_cmd) - dat.live100.uiAccelCmd = float(self.LoC.Ui_accel_cmd) + # DISABLED + if state in [State.PRE_ENABLED, State.DISABLED]: + awareness_status = 1. + LaC.reset() + LoC.reset(v_pid=CS.vEgo) - # lateral control state - dat.live100.yDes = float(self.LaC.y_des) - dat.live100.angleSteersDes = float(self.LaC.angle_steers_des) - dat.live100.upSteer = float(self.LaC.Up_steer) - dat.live100.uiSteer = float(self.LaC.Ui_steer) + # ENABLED or SOFT_DISABLING + elif state in [State.ENABLED, State.SOFT_DISABLING]: - # processed radar state, should add a_pcm? - dat.live100.vTargetLead = float(self.plan.vTarget) - dat.live100.aTargetMin = float(self.plan.aTargetMin) - dat.live100.aTargetMax = float(self.plan.aTargetMax) - dat.live100.jerkFactor = float(self.plan.jerkFactor) + if CS.steeringPressed: + # reset awareness status on steering + awareness_status = 1.0 - # log learned angle offset - dat.live100.angleOffset = float(self.angle_offset) + # 6 minutes driver you're on + awareness_status -= 0.01/(AWARENESS_TIME) + if awareness_status <= 0.: + AM.add("driverDistracted", enabled) + elif awareness_status <= AWARENESS_PRE_TIME / AWARENESS_TIME and \ + awareness_status >= (AWARENESS_PRE_TIME - 0.1) / AWARENESS_TIME: + AM.add("preDriverDistracted", enabled) - # lag - dat.live100.cumLagMs = -self.rk.remaining*1000. + # parse warnings from car specific interface + for e in get_events(events, [ET.WARNING]): + AM.add(e, enabled) - self.live100.send(dat.to_bytes()) + # if user is not responsive to awareness alerts, then start a smooth deceleration + if awareness_status < -0.: + plan.aTargetMax = min(plan.aTargetMax, AWARENESS_DECEL) + plan.aTargetMin = min(plan.aTargetMin, plan.aTargetMax) - self.prof.checkpoint("Live100") + # *** angle offset learning *** - def wait(self): - # *** run loop at fixed rate *** - if self.rk.keep_time(): - self.prof.display() + if rk.frame % 5 == 2 and plan.lateralValid: + # *** run this at 20hz again *** + angle_offset = learn_angle_offset(active, CS.vEgo, angle_offset, + PL.PP.c_poly, PL.PP.c_prob, LaC.y_des, + CS.steeringPressed) + + # *** gas/brake PID loop *** + actuators.gas, actuators.brake = LoC.update(active, CS.vEgo, CS.brakePressed, + v_cruise_kph, plan.vTarget, + [plan.aTargetMin, plan.aTargetMax], + plan.jerkFactor, CP) + + # *** steering PID loop *** + actuators.steer = LaC.update(active, CS.vEgo, CS.steeringAngle, + CS.steeringPressed, plan.dPoly, angle_offset, VM, PL) + + # send a "steering required alert" if saturation count has reached the limit + if LaC.sat_flag: + AM.add("steerSaturated", enabled) + + if CP.enableCruise and CS.cruiseState.enabled: + v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH + + # *** process alerts *** + + AM.process_alerts(sec_since_boot()) + + return actuators, v_cruise_kph, awareness_status, angle_offset, rear_view_toggle + + +def data_send(plan, plan_ts, CS, CI, CP, state, events, actuators, v_cruise_kph, rk, carstate, + carcontrol, live100, livempc, AM, rear_view_allowed, rear_view_toggle, awareness_status, + LaC, LoC, angle_offset): + + # ***** control the car ***** + + CC = car.CarControl.new_message() + + CC.enabled = isEnabled(state) + + CC.actuators = actuators + + CC.cruiseControl.override = True + # always cancel if we have an interceptor + CC.cruiseControl.cancel = not CP.enableCruise or (not isEnabled(state) and CS.cruiseState.enabled) + + # brake discount removes a sharp nonlinearity + brake_discount = (1.0 - clip(actuators.brake*3., 0.0, 1.0)) + CC.cruiseControl.speedOverride = float(max(0.0, (LoC.v_pid + CS.cruiseState.speedOffset) * brake_discount) if CP.enableCruise else 0.0) + + # TODO: parametrize 0.714 in interface? + # accelOverride is more or less the max throttle allowed to pcm: usually set to a constant + # unless aTargetMax is very high and then we scale with it; this helpw in quicker restart + CC.cruiseControl.accelOverride = float(max(0.714, plan.aTargetMax/A_ACC_MAX)) + + CC.hudControl.setSpeed = float(v_cruise_kph * CV.KPH_TO_MS) + CC.hudControl.speedVisible = isEnabled(state) + CC.hudControl.lanesVisible = isEnabled(state) + CC.hudControl.leadVisible = plan.hasLead + CC.hudControl.visualAlert = AM.visual_alert + CC.hudControl.audibleAlert = AM.audible_alert + + # send car controls over can + CI.apply(CC) + + # ***** publish state to logger ***** + # publish controls state at 100Hz + dat = messaging.new_message() + dat.init('live100') + + # show rear view camera on phone if in reverse gear or when button is pressed + dat.live100.rearViewCam = ('reverseGear' in [e.name for e in events] and rear_view_allowed) or rear_view_toggle + dat.live100.alertText1 = AM.alert_text_1 + dat.live100.alertText2 = AM.alert_text_2 + dat.live100.awarenessStatus = max(awareness_status, 0.0) if isEnabled(state) else 0.0 + + # what packets were used to process + dat.live100.canMonoTimes = list(CS.canMonoTimes) + dat.live100.planMonoTime = plan_ts + + # if controls is enabled + dat.live100.enabled = isEnabled(state) + + # car state + dat.live100.vEgo = CS.vEgo + dat.live100.angleSteers = CS.steeringAngle + dat.live100.steerOverride = CS.steeringPressed + + # longitudinal control state + dat.live100.vPid = float(LoC.v_pid) + dat.live100.vCruise = float(v_cruise_kph) + dat.live100.upAccelCmd = float(LoC.pid.p) + dat.live100.uiAccelCmd = float(LoC.pid.i) + + # lateral control state + dat.live100.yDes = float(LaC.y_des) + dat.live100.angleSteersDes = float(LaC.angle_steers_des) + dat.live100.upSteer = float(LaC.pid.p) + dat.live100.uiSteer = float(LaC.pid.i) + + # processed radar state, should add a_pcm? + dat.live100.vTargetLead = float(plan.vTarget) + dat.live100.aTargetMin = float(plan.aTargetMin) + dat.live100.aTargetMax = float(plan.aTargetMax) + dat.live100.jerkFactor = float(plan.jerkFactor) + + # log learned angle offset + dat.live100.angleOffset = float(angle_offset) + + # lag + dat.live100.cumLagMs = -rk.remaining*1000. + + live100.send(dat.to_bytes()) + + # broadcast carState + cs_send = messaging.new_message() + cs_send.init('carState') + # TODO: override CS.events with all the cumulated events + cs_send.carState = copy(CS) + carstate.send(cs_send.to_bytes()) + + # broadcast carControl + cc_send = messaging.new_message() + cc_send.init('carControl') + cc_send.carControl = copy(CC) + carcontrol.send(cc_send.to_bytes()) + #print [i.name for i in events] + + # publish mpc state at 20Hz + if hasattr(LaC, 'mpc_updated') and LaC.mpc_updated: + dat = messaging.new_message() + dat.init('liveMpc') + dat.liveMpc.x = list(LaC.mpc_solution[0].x) + dat.liveMpc.y = list(LaC.mpc_solution[0].y) + dat.liveMpc.psi = list(LaC.mpc_solution[0].psi) + dat.liveMpc.delta = list(LaC.mpc_solution[0].delta) + livempc.send(dat.to_bytes()) + + return CC def controlsd_thread(gctx, rate=100): # start the loop set_realtime_priority(2) - CTRLS = Controls(gctx, rate) + + context = zmq.Context() + + # pub + live100 = messaging.pub_sock(context, service_list['live100'].port) + carstate = messaging.pub_sock(context, service_list['carState'].port) + carcontrol = messaging.pub_sock(context, service_list['carControl'].port) + sendcan = messaging.pub_sock(context, service_list['sendcan'].port) + livempc = messaging.pub_sock(context, service_list['liveMpc'].port) + + # sub + thermal = messaging.sub_sock(context, service_list['thermal'].port) + health = messaging.sub_sock(context, service_list['health'].port) + cal = messaging.sub_sock(context, service_list['liveCalibration'].port) + logcan = messaging.sub_sock(context, service_list['can'].port) + + CC = car.CarControl.new_message() + CI, CP = get_car(logcan, sendcan) + PL = Planner(CP) + LoC = LongControl(CI.compute_gb) + VM = VehicleModel(CP) + LaC = LatControl(VM) + AM = AlertManager() + + # write CarParams + params = Params() + params.put("CarParams", CP.to_bytes()) + + state = State.DISABLED + soft_disable_timer = 0 + v_cruise_kph = 255 + overtemp = False + free_space = False + cal_status = Calibration.UNCALIBRATED + rear_view_toggle = False + rear_view_allowed = params.get("IsRearViewMirror") == "1" + + # 0.0 - 1.0 + awareness_status = 0. + + rk = Ratekeeper(rate, print_delay_threshold=2./1000) + + # learned angle offset + angle_offset = 0. + calibration_params = params.get("CalibrationParams") + if calibration_params: + try: + calibration_params = json.loads(calibration_params) + angle_offset = calibration_params["angle_offset"] + except (ValueError, KeyError): + pass + + prof = Profiler() + while 1: - CTRLS.data_sample() - CTRLS.state_transition() - CTRLS.state_control() - CTRLS.data_send() - CTRLS.wait() + + prof.reset() # avoid memory leak + + # sample data and compute car events + CS, events, cal_status, overtemp, free_space = data_sample(CI, CC, thermal, health, cal, cal_status, + overtemp, free_space) + prof.checkpoint("Sample") + + # define plan + plan, plan_ts = calc_plan(CS, events, PL, LoC) + prof.checkpoint("Plan") + + # update control state + state, soft_disable_timer, v_cruise_kph = state_transition(CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM) + prof.checkpoint("State transition") + + # compute actuators + actuators, v_cruise_kph, awareness_status, angle_offset, rear_view_toggle = state_control(plan, CS, CP, state, events, v_cruise_kph, + AM, rk, awareness_status, PL, LaC, LoC, VM, angle_offset, + rear_view_allowed, rear_view_toggle) + prof.checkpoint("State Control") + + # publish data + CC = data_send(plan, plan_ts, CS, CI, CP, state, events, actuators, v_cruise_kph, + rk, carstate, carcontrol, live100, livempc, AM, rear_view_allowed, + rear_view_toggle, awareness_status, LaC, LoC, angle_offset) + prof.checkpoint("Sent") + + # *** run loop at fixed rate *** + if rk.keep_time(): + prof.display() def main(gctx=None): @@ -462,4 +510,3 @@ def main(gctx=None): if __name__ == "__main__": main() - diff --git a/selfdrive/controls/lib/adaptivecruise.py b/selfdrive/controls/lib/adaptivecruise.py index 1fcf6cad68d4b6..8d3ceec67420d9 100644 --- a/selfdrive/controls/lib/adaptivecruise.py +++ b/selfdrive/controls/lib/adaptivecruise.py @@ -109,7 +109,7 @@ def calc_critical_decel(d_lead, v_rel, d_offset, v_offset): # maximum acceleration adjustment _A_CORR_BY_SPEED_V = [0.4, 0.4, 0] # speeds -_A_CORR_BY_SPEED_BP = [0., 5., 20.] +_A_CORR_BY_SPEED_BP = [0., 2., 10.] # max acceleration allowed in acc, which happens in restart A_ACC_MAX = max(_A_CORR_BY_SPEED_V) + max(_A_CRUISE_MAX_V) @@ -283,20 +283,12 @@ def compute_speed_with_leads(v_ego, angle_steers, v_pid, l1, l2, CP): class AdaptiveCruise(object): def __init__(self): - self.last_cal = 0. self.l1, self.l2 = None, None - self.dead = True - def update(self, cur_time, v_ego, angle_steers, v_pid, CP, l20): + def update(self, v_ego, angle_steers, v_pid, CP, l20): if l20 is not None: self.l1 = l20.live20.leadOne self.l2 = l20.live20.leadTwo - # TODO: no longer has anything to do with calibration - self.last_cal = cur_time - self.dead = False - elif cur_time - self.last_cal > 0.5: - self.dead = True - self.v_target_lead, self.a_target, self.a_pcm, self.jerk_factor = \ compute_speed_with_leads(v_ego, angle_steers, v_pid, self.l1, self.l2, CP) self.has_lead = self.v_target_lead != MAX_SPEED_POSSIBLE diff --git a/selfdrive/controls/lib/alertmanager.py b/selfdrive/controls/lib/alertmanager.py index 162a1977ed2168..19512b579ccb43 100644 --- a/selfdrive/controls/lib/alertmanager.py +++ b/selfdrive/controls/lib/alertmanager.py @@ -2,22 +2,31 @@ from selfdrive.swaglog import cloudlog import copy -class ET: - ENABLE = 0 - NO_ENTRY = 1 - WARNING = 2 - USER_DISABLE = 3 - SOFT_DISABLE = 4 - IMMEDIATE_DISABLE = 5 - -class alert(object): - def __init__(self, alert_text_1, alert_text_2, alert_type, visual_alert, audible_alert, duration_sound, duration_hud_alert, duration_text): + +# Priority +class PT: + HIGH = 3 + MID = 2 + LOW = 1 + + +class Alert(object): + def __init__(self, + alert_text_1, + alert_text_2, + alert_priority, + visual_alert, + audible_alert, + duration_sound, + duration_hud_alert, + duration_text): + self.alert_text_1 = alert_text_1 self.alert_text_2 = alert_text_2 - self.alert_type = alert_type + self.alert_priority = alert_priority self.visual_alert = visual_alert if visual_alert is not None else "none" self.audible_alert = audible_alert if audible_alert is not None else "none" - + self.duration_sound = duration_sound self.duration_hud_alert = duration_hud_alert self.duration_text = duration_text @@ -28,45 +37,297 @@ def __init__(self, alert_text_1, alert_text_2, alert_type, visual_alert, audible tst.hudControl.audibleAlert = self.audible_alert def __str__(self): - return self.alert_text_1 + "/" + self.alert_text_2 + " " + str(self.alert_type) + " " + str(self.visual_alert) + " " + str(self.audible_alert) + return self.alert_text_1 + "/" + self.alert_text_2 + " " + str(self.alert_priority) + " " + str( + self.visual_alert) + " " + str(self.audible_alert) def __gt__(self, alert2): - return self.alert_type > alert2.alert_type + return self.alert_priority > alert2.alert_priority + class AlertManager(object): alerts = { - "enable": alert("", "", ET.ENABLE, None, "beepSingle", .2, 0., 0.), - "disable": alert("", "", ET.USER_DISABLE, None, "beepSingle", .2, 0., 0.), - "pedalPressed": alert("Comma Unavailable", "Pedal Pressed", ET.NO_ENTRY, "brakePressed", "chimeDouble", .4, 2., 3.), - "preDriverDistracted": alert("Take Control ", "User Distracted", ET.WARNING, "steerRequired", "chimeDouble", .4, 2., 3.), - "driverDistracted": alert("Take Control to Regain Speed", "User Distracted", ET.WARNING, "steerRequired", "chimeRepeated", .5, .5, .5), - "steerSaturated": alert("Take Control", "Turn Exceeds Limit", ET.WARNING, "steerRequired", "chimeSingle", 1., 2., 3.), - "overheat": alert("Take Control Immediately", "System Overheated", ET.SOFT_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.), - "controlsMismatch": alert("Take Control Immediately", "Controls Mismatch", ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.), - "radarCommIssue": alert("Take Control Immediately", "Radar Error: Restart the Car",ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.), - "calibrationInvalid": alert("Take Control Immediately", "Calibration Invalid: Reposition Neo and Recalibrate", ET.SOFT_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.), - "calibrationInProgress": alert("Take Control Immediately", "Calibration in Progress: ", ET.SOFT_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.), - "modelCommIssue": alert("Take Control Immediately", "Model Error: Restart the Car",ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.), - "controlsFailed": alert("Take Control Immediately", "Controls Failed", ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.), - "fcw": alert("", "", ET.WARNING, None, None, .1, .1, .1), - # car errors - "commIssue": alert("Take Control Immediately","CAN Error: Restart the Car", ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.), - "steerUnavailable": alert("Take Control Immediately","Steer Error: Restart the Car", ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.), - "steerTempUnavailable": alert("Take Control", "Steer Temporarily Unavailable", ET.WARNING, "steerRequired", "chimeDouble", .4, 2., 3.), - "brakeUnavailable": alert("Take Control Immediately","Brake Error: Restart the Car", ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.), - "gasUnavailable": alert("Take Control Immediately","Gas Error: Restart the Car", ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.), - "wrongGear": alert("Take Control Immediately","Gear not D", ET.SOFT_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.), - "reverseGear": alert("Take Control Immediately","Car in Reverse", ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.), - "doorOpen": alert("Take Control Immediately","Door Open", ET.SOFT_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.), - "seatbeltNotLatched": alert("Take Control Immediately","Seatbelt Unlatched", ET.SOFT_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.), - "espDisabled": alert("Take Control Immediately","ESP Off", ET.SOFT_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.), - "cruiseDisabled": alert("Take Control Immediately","Cruise Is Off", ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.), - "wrongCarMode": alert("Comma Unavailable","Main Switch Off", ET.NO_ENTRY, None, "chimeDouble", .4, 0., 3.), - "outOfSpace": alert("Comma Unavailable","Out of Space", ET.NO_ENTRY, None, "chimeDouble", .4, 0., 3.), - "dataNeeded": alert("Comma Unavailable","Data needed for calibration. Upload drive, try again", ET.NO_ENTRY, None, "chimeDouble", .4, 0., 3.), - "ethicalDilemma": alert("Take Control Immediately","Ethical Dilemma Detected", ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.), - "startup": alert("Always Keep Hands on Wheel","Be Ready to Take Over Any Time", ET.NO_ENTRY, None, None, 0., 0., 15.), + + # Miscellaneous alerts + "enable": Alert( + "", + "", + PT.MID, None, "beepSingle", .2, 0., 0.), + + "disable": Alert( + "", + "", + PT.MID, None, "beepSingle", .2, 0., 0.), + + "fcw": Alert( + "", + "", + PT.LOW, None, None, .1, .1, .1), + + "steerSaturated": Alert( + "Take Control", + "Turn Exceeds Limit", + PT.LOW, "steerRequired", "chimeSingle", 1., 2., 3.), + + "steerTempUnavailable": Alert( + "Take Control", + "Steer Temporarily Unavailable", + PT.LOW, "steerRequired", "chimeDouble", .4, 2., 3.), + + "preDriverDistracted": Alert( + "Take Control ", + "User Distracted", + PT.LOW, "steerRequired", "chimeDouble", .4, 2., 3.), + + "driverDistracted": Alert( + "Take Control to Regain Speed", + "User Distracted", + PT.LOW, "steerRequired", "chimeRepeated", .5, .5, .5), + + "startup": Alert( + "Always Keep Hands on Wheel", + "Be Ready to Take Over Any Time", + PT.LOW, None, None, 0., 0., 15.), + + "ethicalDilemma": Alert( + "Take Control Immediately", + "Ethical Dilemma Detected", + PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.), + + "steerTempUnavailableNoEntry": Alert( + "Comma Unavailable", + "Steer Temporary Unavailable", + PT.LOW, None, "chimeDouble", .4, 0., 3.), + + # Non-entry only alerts + "wrongCarModeNoEntry": Alert( + "Comma Unavailable", + "Main Switch Off", + PT.LOW, None, "chimeDouble", .4, 0., 3.), + + "dataNeededNoEntry": Alert( + "Comma Unavailable", + "Data needed for calibration. Upload drive, try again", + PT.LOW, None, "chimeDouble", .4, 0., 3.), + + "outOfSpaceNoEntry": Alert( + "Comma Unavailable", + "Out of Space", + PT.LOW, None, "chimeDouble", .4, 0., 3.), + + "pedalPressedNoEntry": Alert( + "Comma Unavailable", + "Pedal Pressed", + PT.LOW, "brakePressed", "chimeDouble", .4, 2., 3.), + + "speedTooLowNoEntry": Alert( + "Comma Unavailable", + "Speed Too Low", + PT.LOW, None, "chimeDouble", .4, 2., 3.), + + "brakeHoldNoEntry": Alert( + "Comma Unavailable", + "Brake Hold Active", + PT.LOW, None, "chimeDouble", .4, 2., 3.), + + "parkBrakeNoEntry": Alert( + "Comma Unavailable", + "Park Brake Engaged", + PT.LOW, None, "chimeDouble", .4, 2., 3.), + + # Cancellation alerts causing disabling + "overheat": Alert( + "Take Control Immediately", + "System Overheated", + PT.MID, "steerRequired", "chimeRepeated", 1., 3., 3.), + + "wrongGear": Alert( + "Take Control Immediately", + "Gear not D", + PT.MID, "steerRequired", "chimeRepeated", 1., 3., 3.), + + "calibrationInvalid": Alert( + "Take Control Immediately", + "Calibration Invalid: Reposition Neo and Recalibrate", + PT.MID, "steerRequired", "chimeRepeated", 1., 3., 3.), + + "calibrationInProgress": Alert( + "Take Control Immediately", + "Calibration in Progress", + PT.MID, "steerRequired", "chimeRepeated", 1., 3., 3.), + + "doorOpen": Alert( + "Take Control Immediately", + "Door Open", + PT.MID, "steerRequired", "chimeRepeated", 1., 3., 3.), + + "seatbeltNotLatched": Alert( + "Take Control Immediately", + "Seatbelt Unlatched", + PT.MID, "steerRequired", "chimeRepeated", 1., 3., 3.), + + "espDisabled": Alert( + "Take Control Immediately", + "ESP Off", + PT.MID, "steerRequired", "chimeRepeated", 1., 3., 3.), + + "radarCommIssue": Alert( + "Take Control Immediately", + "Radar Error: Restart the Car", + PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.), + + "radarFault": Alert( + "Take Control Immediately", + "Radar Error: Restart the Car", + PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.), + + "modelCommIssue": Alert( + "Take Control Immediately", + "Model Error: Restart the Car", + PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.), + + "controlsFailed": Alert( + "Take Control Immediately", + "Controls Failed", + PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.), + + "controlsMismatch": Alert( + "Take Control Immediately", + "Controls Mismatch", + PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.), + + "commIssue": Alert( + "Take Control Immediately", + "CAN Error: Restart the Car", + PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.), + + "steerUnavailable": Alert( + "Take Control Immediately", + "Steer Error: Restart the Car", + PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.), + + "brakeUnavailable": Alert( + "Take Control Immediately", + "Brake Error: Restart the Car", + PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.), + + "gasUnavailable": Alert( + "Take Control Immediately", + "Gas Error: Restart the Car", + PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.), + + "reverseGear": Alert( + "Take Control Immediately", + "Reverse Gear", + PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.), + + "cruiseDisabled": Alert( + "Take Control Immediately", + "Cruise Is Off", + PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.), + + # not loud cancellations (user is in control) + "noTarget": Alert( + "Comma Canceled", + "No Close Lead", + PT.HIGH, None, "chimeDouble", .4, 2., 3.), + + # Cancellation alerts causing non-entry + "overheatNoEntry": Alert( + "Comma Unavailable", + "System Overheated", + PT.LOW, None, "chimeDouble", .4, 2., 3.), + + "wrongGearNoEntry": Alert( + "Comma Unavailable", + "Gear not D", + PT.LOW, None, "chimeDouble", .4, 2., 3.), + + "calibrationInvalidNoEntry": Alert( + "Comma Unavailable", + "Calibration Invalid: Reposition Neo and Recalibrate", + PT.LOW, None, "chimeDouble", .4, 2., 3.), + + "calibrationInProgressNoEntry": Alert( + "Comma Unavailable", + "Calibration in Progress", + PT.LOW, None, "chimeDouble", .4, 2., 3.), + + "doorOpenNoEntry": Alert( + "Comma Unavailable", + "Door Open", + PT.LOW, None, "chimeDouble", .4, 2., 3.), + + "seatbeltNotLatchedNoEntry": Alert( + "Comma Unavailable", + "Seatbelt Unlatched", + PT.LOW, None, "chimeDouble", .4, 2., 3.), + + "espDisabledNoEntry": Alert( + "Comma Unavailable", + "ESP Off", + PT.LOW, None, "chimeDouble", .4, 2., 3.), + + "radarCommIssueNoEntry": Alert( + "Comma Unavailable", + "Radar Error: Restart the Car", + PT.LOW, None, "chimeDouble", .4, 2., 3.), + + "radarFaultNoEntry": Alert( + "Comma Unavailable", + "Radar Error: Restart the Car", + PT.LOW, None, "chimeDouble", .4, 2., 3.), + + "modelCommIssueNoEntry": Alert( + "Comma Unavailable", + "Model Error: Restart the Car", + PT.LOW, None, "chimeDouble", .4, 2., 3.), + + "controlsFailedNoEntry": Alert( + "Comma Unavailable", + "Controls Failed", + PT.LOW, None, "chimeDouble", .4, 2., 3.), + + "controlsMismatchNoEntry": Alert( + "Comma Unavailable", + "Controls Mismatch", + PT.LOW, None, "chimeDouble", .4, 2., 3.), + + "commIssueNoEntry": Alert( + "Comma Unavailable", + "CAN Error: Restart the Car", + PT.LOW, None, "chimeDouble", .4, 2., 3.), + + "steerUnavailableNoEntry": Alert( + "Comma Unavailable", + "Steer Error: Restart the Car", + PT.LOW, None, "chimeDouble", .4, 2., 3.), + + "brakeUnavailableNoEntry": Alert( + "Comma Unavailable", + "Brake Error: Restart the Car", + PT.LOW, None, "chimeDouble", .4, 2., 3.), + + "gasUnavailableNoEntry": Alert( + "Comma Unavailable", + "Gas Error: Restart the Car", + PT.LOW, None, "chimeDouble", .4, 2., 3.), + + "reverseGearNoEntry": Alert( + "Comma Unavailable", + "Reverse Gear", + PT.LOW, None, "chimeDouble", .4, 2., 3.), + + "cruiseDisabledNoEntry": Alert( + "Comma Unavailable", + "Cruise is Off", + PT.LOW, None, "chimeDouble", .4, 2., 3.), + + "noTargetNoEntry": Alert( + "Comma Unavailable", + "No Close Lead", + PT.LOW, None, "chimeDouble", .4, 2., 3.), } + def __init__(self): self.activealerts = [] self.current_alert = None @@ -75,31 +336,16 @@ def __init__(self): def alertPresent(self): return len(self.activealerts) > 0 - def alertShouldSoftDisable(self): - return len(self.activealerts) > 0 and any(a.alert_type == ET.SOFT_DISABLE for a in self.activealerts) - - def alertShouldDisable(self): - return len(self.activealerts) > 0 and any(a.alert_type in [ET.IMMEDIATE_DISABLE, ET.USER_DISABLE] for a in self.activealerts) - def add(self, alert_type, enabled=True, extra_text=''): alert_type = str(alert_type) this_alert = copy.copy(self.alerts[alert_type]) this_alert.alert_text_2 += extra_text - # downgrade the alert if we aren't enabled, except if it's FCW, which remains the same - # TODO: remove this 'if' by adding more alerts - if not enabled and this_alert.alert_type in [ET.WARNING, ET.SOFT_DISABLE, ET.IMMEDIATE_DISABLE] \ - and this_alert != self.alerts['fcw']: - this_alert = alert("Comma Unavailable" if this_alert.alert_text_1 != "" else "", this_alert.alert_text_2, ET.NO_ENTRY, None, "chimeDouble", .4, 0., 3.) - - # ignore no entries if we are enabled - if enabled and this_alert.alert_type in [ET.ENABLE, ET.NO_ENTRY]: - return # if new alert is higher priority, log it if self.current_alert is None or this_alert > self.current_alert: cloudlog.event('alert_add', - alert_type=alert_type, - enabled=enabled) + alert_type=alert_type, + enabled=enabled) self.activealerts.append(this_alert) self.activealerts.sort() @@ -109,29 +355,29 @@ def process_alerts(self, cur_time): self.alert_start_time = cur_time self.current_alert = self.activealerts[0] print self.current_alert - alert_text_1 = "" - alert_text_2 = "" - visual_alert = "none" - audible_alert = "none" + + # start with assuming no alerts + self.alert_text_1 = "" + self.alert_text_2 = "" + self.visual_alert = "none" + self.audible_alert = "none" if self.current_alert is not None: # ewwwww if self.alert_start_time + self.current_alert.duration_sound > cur_time: - audible_alert = self.current_alert.audible_alert + self.audible_alert = self.current_alert.audible_alert if self.alert_start_time + self.current_alert.duration_hud_alert > cur_time: - visual_alert = self.current_alert.visual_alert + self.visual_alert = self.current_alert.visual_alert if self.alert_start_time + self.current_alert.duration_text > cur_time: - alert_text_1 = self.current_alert.alert_text_1 - alert_text_2 = self.current_alert.alert_text_2 + self.alert_text_1 = self.current_alert.alert_text_1 + self.alert_text_2 = self.current_alert.alert_text_2 # disable current alert - if self.alert_start_time + max(self.current_alert.duration_sound, self.current_alert.duration_hud_alert, self.current_alert.duration_text) < cur_time: + if self.alert_start_time + max(self.current_alert.duration_sound, self.current_alert.duration_hud_alert, + self.current_alert.duration_text) < cur_time: self.current_alert = None # reset self.activealerts = [] - - return alert_text_1, alert_text_2, visual_alert, audible_alert - diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index 2d01b40ca0a55c..699893134e5ba0 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -1,8 +1,38 @@ from common.numpy_fast import clip +from cereal import car + + +class EventTypes: + ENABLE = 'enable' + PRE_ENABLE = 'preEnable' + NO_ENTRY = 'noEntry' + WARNING = 'warning' + USER_DISABLE = 'userDisable' + SOFT_DISABLE = 'softDisable' + IMMEDIATE_DISABLE = 'immediateDisable' + + +def create_event(name, types): + event = car.CarEvent.new_message() + event.name = name + for t in types: + setattr(event, t, True) + return event + + +def get_events(events, types): + out = [] + for e in events: + for t in types: + if getattr(e, t): + out.append(e.name) + return out + def rate_limit(new_value, last_value, dw_step, up_step): return clip(new_value, last_value + dw_step, last_value + up_step) + def learn_angle_offset(lateral_control, v_ego, angle_offset, c_poly, c_prob, y_des, steer_override): # simple integral controller that learns how much steering offset to put to have the car going straight # while being in the middle of the lane diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py index 720d7947f8e3de..ab81435fc7996b 100644 --- a/selfdrive/controls/lib/latcontrol.py +++ b/selfdrive/controls/lib/latcontrol.py @@ -1,133 +1,80 @@ import math -import numpy as np +from selfdrive.controls.lib.pid import PIController +from selfdrive.controls.lib.lateral_mpc import libmpc_py from common.numpy_fast import clip, interp -from selfdrive.config import Conversions as CV - -_K_CURV_V = [1., 0.6] -_K_CURV_BP = [0., 0.002] - -def calc_d_lookahead(v_ego, d_poly): - #*** this function computes how far too look for lateral control - # howfar we look ahead is function of speed and how much curvy is the path - offset_lookahead = 1. - k_lookahead = 7. - # integrate abs value of second derivative of poly to get a measure of path curvature - pts_len = 50. # m - if len(d_poly)>0: - pts = np.polyval([6*d_poly[0], 2*d_poly[1]], np.arange(0, pts_len)) - else: - pts = 0. - curv = np.sum(np.abs(pts))/pts_len - - k_curv = interp(curv, _K_CURV_BP, _K_CURV_V) - - # sqrt on speed is needed to keep, for a given curvature, the y_des - # proportional to speed. Indeed, y_des is prop to d_lookahead^2 - # 36m at 25m/s - d_lookahead = offset_lookahead + math.sqrt(max(v_ego, 0)) * k_lookahead * k_curv - return d_lookahead - -def calc_lookahead_offset(v_ego, angle_steers, d_lookahead, VM, angle_offset): - #*** this function returns the lateral offset given the steering angle, speed and the lookahead distance - sa = (angle_steers - angle_offset) * CV.DEG_TO_RAD - curvature = VM.calc_curvature(sa, v_ego) - # clip is to avoid arcsin NaNs due to too sharp turns - y_actual = d_lookahead * np.tan(np.arcsin(np.clip(d_lookahead * curvature, -0.999, 0.999))/2.) - return y_actual, curvature - -def calc_desired_steer_angle(v_ego, y_des, d_lookahead, VM, angle_offset): - # inverse of the above function - curvature = np.sin(np.arctan(y_des / d_lookahead) * 2.) / d_lookahead - steer_des = VM.get_steer_from_curvature(curvature, v_ego) * CV.RAD_TO_DEG + angle_offset - return steer_des, curvature - -def pid_lateral_control(v_ego, sa_actual, sa_des, Ui_steer, steer_max, - steer_override, sat_count, enabled, Kp, Ki, rate): - - sat_count_rate = 1./rate - sat_count_limit = 0.8 # after 0.8s of continuous saturation, an alert will be sent - - error_steer = sa_des - sa_actual - Ui_unwind_speed = 0.3/rate #.3 per second - - Up_steer = error_steer*Kp - Ui_steer_new = Ui_steer + error_steer*Ki * 1./rate - output_steer_new = Ui_steer_new + Up_steer - - # Anti-wind up for integrator: do not integrate if we are against the steer limits - if ( - (error_steer >= 0. and (output_steer_new < steer_max or Ui_steer < 0)) or - (error_steer <= 0. and - (output_steer_new > -steer_max or Ui_steer > 0))) and not steer_override: - #update integrator - Ui_steer = Ui_steer_new - # unwind integrator if driver is maneuvering the steering wheel - elif steer_override: - Ui_steer -= Ui_unwind_speed * np.sign(Ui_steer) - - # still, intergral term should not be bigger then limits - Ui_steer = clip(Ui_steer, -steer_max, steer_max) - - output_steer = Up_steer + Ui_steer - - # don't run steer control if at very low speed - if v_ego < 0.3 or not enabled: - output_steer = 0. - Ui_steer = 0. - - # useful to know if control is against the limit - lateral_control_sat = False - if abs(output_steer) > steer_max: - lateral_control_sat = True - - output_steer = clip(output_steer, -steer_max, steer_max) - - # if lateral control is saturated for a certain period of time, send an alert for taking control of the car - # wind - if lateral_control_sat and not steer_override and v_ego > 10 and abs(error_steer) > 0.1: - sat_count += sat_count_rate - # unwind - else: - sat_count -= sat_count_rate - - sat_flag = False - if sat_count >= sat_count_limit: - sat_flag = True - - sat_count = clip(sat_count, 0, 1) - - return output_steer, Up_steer, Ui_steer, lateral_control_sat, sat_count, sat_flag + + +# 100ms is a rule of thumb estimation of lag from image processing to actuator command +ACTUATORS_DELAY = 0.1 + + +def calc_states_after_delay(states, v_ego, steer_angle, curvature_factor, steer_ratio): + states[0].x = v_ego * ACTUATORS_DELAY + states[0].psi = v_ego * curvature_factor * math.radians(steer_angle) / steer_ratio * ACTUATORS_DELAY + return states + + +def get_steer_max(CP, v_ego): + return interp(v_ego, CP.steerMaxBP, CP.steerMaxV) + class LatControl(object): - def __init__(self): - self.Up_steer = 0. - self.sat_count = 0 - self.y_des = 0.0 - self.lateral_control_sat = False - self.Ui_steer = 0. - self.reset() + def __init__(self, VM): + self.pid = PIController(VM.CP.steerKp, VM.CP.steerKi, pos_limit=1.0) + self.setup_mpc() + + self.y_des = -1 # Legacy + + def setup_mpc(self): + self.libmpc = libmpc_py.libmpc + self.libmpc.init() + + self.mpc_solution = libmpc_py.ffi.new("log_t *") + self.cur_state = libmpc_py.ffi.new("state_t *") + self.mpc_updated = False + self.cur_state[0].x = 0.0 + self.cur_state[0].y = 0.0 + self.cur_state[0].psi = 0.0 + self.cur_state[0].delta = 0.0 + + self.last_mpc_ts = 0.0 + self.angle_steers_des = 0 def reset(self): - self.Ui_steer = 0. + self.pid.reset() + + def update(self, active, v_ego, angle_steers, steer_override, d_poly, angle_offset, VM, PL): + self.mpc_updated = False + if self.last_mpc_ts + 0.001 < PL.last_md_ts: + self.last_mpc_ts = PL.last_md_ts + + curvature_factor = VM.curvature_factor(v_ego) - def update(self, enabled, v_ego, angle_steers, steer_override, d_poly, angle_offset, VM): - rate = 100 + l_poly = libmpc_py.ffi.new("double[4]", list(PL.PP.l_poly)) + r_poly = libmpc_py.ffi.new("double[4]", list(PL.PP.r_poly)) + p_poly = libmpc_py.ffi.new("double[4]", list(PL.PP.p_poly)) - steer_max = 1.0 + # account for actuation delay + self.cur_state = calc_states_after_delay(self.cur_state, v_ego, angle_steers, curvature_factor, VM.CP.sR) - # how far we look ahead is function of speed and desired path - d_lookahead = calc_d_lookahead(v_ego, d_poly) + self.libmpc.run_mpc(self.cur_state, self.mpc_solution, + l_poly, r_poly, p_poly, + PL.PP.l_prob, PL.PP.r_prob, PL.PP.p_prob, curvature_factor, v_ego, PL.PP.lane_width) - # desired lookahead offset - self.y_des = np.polyval(d_poly, d_lookahead) + delta_desired = self.mpc_solution[0].delta[1] + self.cur_state[0].delta = delta_desired - # calculate actual offset at the lookahead point - self.angle_steers_des, _ = calc_desired_steer_angle(v_ego, self.y_des, - d_lookahead, VM, angle_offset) + self.angle_steers_des = math.degrees(delta_desired * VM.CP.sR) + angle_offset + self.mpc_updated = True - output_steer, self.Up_steer, self.Ui_steer, self.lateral_control_sat, self.sat_count, sat_flag = pid_lateral_control( - v_ego, angle_steers, self.angle_steers_des, self.Ui_steer, steer_max, - steer_override, self.sat_count, enabled, VM.CP.steerKp, VM.CP.steerKi, rate) + if v_ego < 0.3 or not active: + output_steer = 0.0 + self.pid.reset() + else: + steer_max = get_steer_max(VM.CP, v_ego) + self.pid.pos_limit = steer_max + self.pid.neg_limit = -steer_max + output_steer = self.pid.update(self.angle_steers_des, angle_steers, check_saturation=(v_ego > 10), override=steer_override) - final_steer = clip(output_steer, -steer_max, steer_max) - return final_steer, sat_flag + self.sat_flag = self.pid.saturated + return output_steer diff --git a/selfdrive/controls/lib/latcontrol_helpers.py b/selfdrive/controls/lib/latcontrol_helpers.py new file mode 100644 index 00000000000000..982152d65c733a --- /dev/null +++ b/selfdrive/controls/lib/latcontrol_helpers.py @@ -0,0 +1,89 @@ +import numpy as np +import math +from common.numpy_fast import interp + +_K_CURV_V = [1., 0.6] +_K_CURV_BP = [0., 0.002] + +# lane width http://safety.fhwa.dot.gov/geometric/pubs/mitigationstrategies/chapter3/3_lanewidth.cfm +_LANE_WIDTH_V = [3., 3.8] + +# break points of speed +_LANE_WIDTH_BP = [0., 31.] + + +def calc_d_lookahead(v_ego, d_poly): + # this function computes how far too look for lateral control + # howfar we look ahead is function of speed and how much curvy is the path + offset_lookahead = 1. + k_lookahead = 7. + # integrate abs value of second derivative of poly to get a measure of path curvature + pts_len = 50. # m + if len(d_poly) > 0: + pts = np.polyval([6 * d_poly[0], 2 * d_poly[1]], np.arange(0, pts_len)) + else: + pts = 0. + curv = np.sum(np.abs(pts)) / pts_len + + k_curv = interp(curv, _K_CURV_BP, _K_CURV_V) + + # sqrt on speed is needed to keep, for a given curvature, the y_des + # proportional to speed. Indeed, y_des is prop to d_lookahead^2 + # 36m at 25m/s + d_lookahead = offset_lookahead + math.sqrt(max(v_ego, 0)) * k_lookahead * k_curv + return d_lookahead + + +def calc_lookahead_offset(v_ego, angle_steers, d_lookahead, VM, angle_offset): + # this function returns the lateral offset given the steering angle, speed and the lookahead distance + sa = math.radians(angle_steers - angle_offset) + curvature = VM.calc_curvature(sa, v_ego) + # clip is to avoid arcsin NaNs due to too sharp turns + y_actual = d_lookahead * np.tan(np.arcsin(np.clip(d_lookahead * curvature, -0.999, 0.999)) / 2.) + return y_actual, curvature + + +def calc_desired_steer_angle(v_ego, y_des, d_lookahead, VM, angle_offset): + # inverse of the above function + curvature = np.sin(np.arctan(y_des / d_lookahead) * 2.) / d_lookahead + steer_des = math.degrees(VM.get_steer_from_curvature(curvature, v_ego)) + angle_offset + return steer_des, curvature + + +def compute_path_pinv(): + deg = 3 + x = np.arange(50.0) + X = np.vstack(tuple(x**n for n in range(deg, -1, -1))).T + pinv = np.linalg.pinv(X) + return pinv + + +def model_polyfit(points, path_pinv): + return np.dot(path_pinv, map(float, points)) + + +def calc_desired_path(l_poly, + r_poly, + p_poly, + l_prob, + r_prob, + p_prob, + speed, + lane_width=None): + # this function computes the poly for the center of the lane, averaging left and right polys + if lane_width is None: + lane_width = interp(speed, _LANE_WIDTH_BP, _LANE_WIDTH_V) + + # lanes in US are ~3.6m wide + half_lane_poly = np.array([0., 0., 0., lane_width / 2.]) + if l_prob + r_prob > 0.01: + c_poly = ((l_poly - half_lane_poly) * l_prob + + (r_poly + half_lane_poly) * r_prob) / (l_prob + r_prob) + c_prob = l_prob + r_prob - l_prob * r_prob + else: + c_poly = np.zeros(4) + c_prob = 0. + + p_weight = 1. # predicted path weight relatively to the center of the lane + d_poly = list((c_poly * c_prob + p_poly * p_prob * p_weight) / (c_prob + p_prob * p_weight)) + return d_poly, c_poly, c_prob diff --git a/selfdrive/controls/lib/lateral_mpc/Makefile b/selfdrive/controls/lib/lateral_mpc/Makefile new file mode 100644 index 00000000000000..b0802f9a440e58 --- /dev/null +++ b/selfdrive/controls/lib/lateral_mpc/Makefile @@ -0,0 +1,75 @@ +CC = clang +CXX = clang++ + +PHONELIBS = ../../../../phonelibs + +UNAME_M := $(shell uname -m) + +CFLAGS = -O3 -fPIC -I. +CXXFLAGS = -O3 -fPIC -I. + +QPOASES_FLAGS = -I$(PHONELIBS)/qpoases -I$(PHONELIBS)/qpoases/INCLUDE -I$(PHONELIBS)/qpoases/SRC + +ACADO_FLAGS = -I$(PHONELIBS)/acado/include -I$(PHONELIBS)/acado/include/acado + +ifeq ($(UNAME_M),aarch64) +ACADO_LIBS := -L $(PHONELIBS)/acado/aarch64/lib -l:libacado_toolkit.a -l:libacado_casadi.a -l:libacado_csparse.a +else +ACADO_LIBS := -L $(PHONELIBS)/acado/x64/lib -l:libacado_toolkit.a -l:libacado_casadi.a -l:libacado_csparse.a +endif + +OBJS = \ + $(PHONELIBS)/qpoases/SRC/Bounds.o \ + $(PHONELIBS)/qpoases/SRC/Constraints.o \ + $(PHONELIBS)/qpoases/SRC/CyclingManager.o \ + $(PHONELIBS)/qpoases/SRC/Indexlist.o \ + $(PHONELIBS)/qpoases/SRC/MessageHandling.o \ + $(PHONELIBS)/qpoases/SRC/QProblem.o \ + $(PHONELIBS)/qpoases/SRC/QProblemB.o \ + $(PHONELIBS)/qpoases/SRC/SubjectTo.o \ + $(PHONELIBS)/qpoases/SRC/Utils.o \ + $(PHONELIBS)/qpoases/SRC/EXTRAS/SolutionAnalysis.o \ + mpc_export/acado_qpoases_interface.o \ + mpc_export/acado_integrator.o \ + mpc_export/acado_solver.o \ + mpc_export/acado_auxiliary_functions.o \ + mpc.o + +DEPS := $(OBJS:.o=.d) + +.PHONY: all +all: libcommampc.so + +libcommampc.so: $(OBJS) + $(CXX) -shared -o '$@' $^ -lm + +%.o: %.cpp + @echo "[ CXX ] $@" + $(CXX) $(CXXFLAGS) -MMD \ + -I mpc_export/ \ + $(QPOASES_FLAGS) \ + -c -o '$@' '$<' + +%.o: %.c + @echo "[ CC ] $@" + $(CC) $(CFLAGS) -MMD \ + -I mpc_export/ \ + $(QPOASES_FLAGS) \ + -c -o '$@' '$<' + +generator: generator.cpp + $(CXX) -Wall -std=c++11 \ + generator.cpp \ + -o generator \ + $(ACADO_FLAGS) \ + $(ACADO_LIBS) + +.PHONY: generate +generate: generator + ./generator + +.PHONY: clean +clean: + rm -f libcommampc.so generator $(OBJS) $(DEPS) + +-include $(DEPS) diff --git a/selfdrive/controls/lib/lateral_mpc/__init__.py b/selfdrive/controls/lib/lateral_mpc/__init__.py new file mode 100644 index 00000000000000..e69de29bb2d1d6 diff --git a/selfdrive/controls/lib/lateral_mpc/generator.cpp b/selfdrive/controls/lib/lateral_mpc/generator.cpp new file mode 100644 index 00000000000000..2e5c3a2ff4732f --- /dev/null +++ b/selfdrive/controls/lib/lateral_mpc/generator.cpp @@ -0,0 +1,137 @@ +#include + +#define PI 3.1415926536 +#define deg2rad(d) (d/180.0*PI) + +const int controlHorizon = 50; +const double samplingTime = 0.05; // 20 Hz + +using namespace std; + +int main( ) +{ + USING_NAMESPACE_ACADO + + + DifferentialEquation f; + + DifferentialState xx; // x position + DifferentialState yy; // y position + DifferentialState psi; // vehicle heading + DifferentialState delta; + + OnlineData curvature_factor; + OnlineData v_ref; // m/s + OnlineData l_poly_r0, l_poly_r1, l_poly_r2, l_poly_r3; + OnlineData r_poly_r0, r_poly_r1, r_poly_r2, r_poly_r3; + OnlineData p_poly_r0, p_poly_r1, p_poly_r2, p_poly_r3; + OnlineData l_prob, r_prob, p_prob; + OnlineData lane_width; + + Control t; + + // Equations of motion + f << dot(xx) == v_ref * cos(psi); + f << dot(yy) == v_ref * sin(psi); + f << dot(psi) == v_ref * delta * curvature_factor; + f << dot(delta) == t; + + auto lr_prob = l_prob + r_prob - l_prob * r_prob; + + auto poly_l = l_poly_r0*(xx*xx*xx) + l_poly_r1*(xx*xx) + l_poly_r2*xx + l_poly_r3; + auto poly_r = r_poly_r0*(xx*xx*xx) + r_poly_r1*(xx*xx) + r_poly_r2*xx + r_poly_r3; + auto poly_p = p_poly_r0*(xx*xx*xx) + p_poly_r1*(xx*xx) + p_poly_r2*xx + p_poly_r3; + + auto angle_l = atan(3*l_poly_r0*xx*xx + 2*l_poly_r1*xx + l_poly_r2); + auto angle_r = atan(3*r_poly_r0*xx*xx + 2*r_poly_r1*xx + r_poly_r2); + auto angle_p = atan(3*p_poly_r0*xx*xx + 2*p_poly_r1*xx + p_poly_r2); + + auto c_left_lane = exp(-(poly_l - yy)); + auto c_right_lane = exp(poly_r - yy); + + auto r_phantom = poly_l - lane_width/2.0; + auto l_phantom = poly_r + lane_width/2.0; + + auto path = lr_prob * (l_prob * r_phantom + r_prob * l_phantom) / (l_prob + r_prob + 0.0001) + + (1-lr_prob) * poly_p; + + auto angle = lr_prob * (l_prob * angle_l + r_prob * angle_r) / (l_prob + r_prob + 0.0001) + + (1-lr_prob) * angle_p; + + // Running cost + Function h; + + // Distance errors + h << path - yy; + h << l_prob * c_left_lane; + h << r_prob * c_right_lane; + + // Heading error + h << (v_ref + 1.0 ) * (angle - psi); + + // Angular rate error + h << (v_ref + 1.0 ) * t; + + DMatrix Q(5,5); + Q(0,0) = 1.0; + Q(1,1) = 1.0; + Q(2,2) = 1.0; + + Q(3,3) = 1.0; + + Q(4,4) = 1.0; + + // Terminal cost + Function hN; + + // Distance errors + hN << path - yy; + hN << l_prob * c_left_lane; + hN << r_prob * c_right_lane; + + // Heading errors + hN << (2.0 * v_ref + 1.0 ) * (angle - psi); + + DMatrix QN(4,4); + QN(0,0) = 1.0; + QN(1,1) = 1.0; + QN(2,2) = 1.0; + + QN(3,3) = 1.0; + + // Setup Optimal Control Problem + const double tStart = 0.0; + const double tEnd = samplingTime * controlHorizon; + + OCP ocp( tStart, tEnd, controlHorizon ); + ocp.subjectTo(f); + + ocp.minimizeLSQ(Q, h); + ocp.minimizeLSQEndTerm(QN, hN); + + ocp.subjectTo( deg2rad(-90) <= psi <= deg2rad(90)); + ocp.subjectTo( deg2rad(-25) <= delta <= deg2rad(25)); + ocp.subjectTo( -0.1 <= t <= 0.1); + ocp.setNOD(18); + + OCPexport mpc(ocp); + mpc.set( HESSIAN_APPROXIMATION, GAUSS_NEWTON ); + mpc.set( DISCRETIZATION_TYPE, MULTIPLE_SHOOTING ); + mpc.set( INTEGRATOR_TYPE, INT_RK4 ); + mpc.set( NUM_INTEGRATOR_STEPS, 250 ); + + mpc.set( SPARSE_QP_SOLUTION, CONDENSING ); + mpc.set( QP_SOLVER, QP_QPOASES ); + mpc.set( HOTSTART_QP, YES ); + mpc.set( GENERATE_TEST_FILE, NO); + mpc.set( GENERATE_MAKE_FILE, NO ); + mpc.set( GENERATE_MATLAB_INTERFACE, NO ); + mpc.set( GENERATE_SIMULINK_INTERFACE, NO ); + + if (mpc.exportCode( "mpc_export" ) != SUCCESSFUL_RETURN) + exit( EXIT_FAILURE ); + + mpc.printDimensionsQP( ); + + return EXIT_SUCCESS; +} diff --git a/selfdrive/controls/lib/lateral_mpc/libmpc_py.py b/selfdrive/controls/lib/lateral_mpc/libmpc_py.py new file mode 100644 index 00000000000000..032567be59a9b9 --- /dev/null +++ b/selfdrive/controls/lib/lateral_mpc/libmpc_py.py @@ -0,0 +1,30 @@ +import os +import subprocess + +from cffi import FFI + +mpc_dir = os.path.dirname(os.path.abspath(__file__)) + +libmpc_fn = os.path.join(mpc_dir, "libcommampc.so") +subprocess.check_output(["make", "-j4"], cwd=mpc_dir) + +ffi = FFI() +ffi.cdef(""" +typedef struct { + double x, y, psi, delta, t; +} state_t; + +typedef struct { + double x[50]; + double y[50]; + double psi[50]; + double delta[50]; +} log_t; + +void init(); +void run_mpc(state_t * x0, log_t * solution, + double l_poly[4], double r_poly[4], double p_poly[4], + double l_prob, double r_prob, double p_prob, double curvature_factor, double v_ref, double lane_width); +""") + +libmpc = ffi.dlopen(libmpc_fn) diff --git a/selfdrive/controls/lib/lateral_mpc/mpc.c b/selfdrive/controls/lib/lateral_mpc/mpc.c new file mode 100644 index 00000000000000..6823c554c2f644 --- /dev/null +++ b/selfdrive/controls/lib/lateral_mpc/mpc.c @@ -0,0 +1,100 @@ +#include "acado_common.h" +#include "acado_auxiliary_functions.h" + +#include + +#define NX ACADO_NX /* Number of differential state variables. */ +#define NXA ACADO_NXA /* Number of algebraic variables. */ +#define NU ACADO_NU /* Number of control inputs. */ +#define NOD ACADO_NOD /* Number of online data values. */ + +#define NY ACADO_NY /* Number of measurements/references on nodes 0..N - 1. */ +#define NYN ACADO_NYN /* Number of measurements/references on node N. */ + +#define N ACADO_N /* Number of intervals in the horizon. */ + +ACADOvariables acadoVariables; +ACADOworkspace acadoWorkspace; + +typedef struct { + double x, y, psi, delta, t; +} state_t; + + +typedef struct { + double x[N]; + double y[N]; + double psi[N]; + double delta[N]; +} log_t; + +void init(){ + acado_initializeSolver(); + int i; + + /* Initialize the states and controls. */ + for (i = 0; i < NX * (N + 1); ++i) acadoVariables.x[ i ] = 0.0; + for (i = 0; i < NU * N; ++i) acadoVariables.u[ i ] = 0.1; + + /* Initialize the measurements/reference. */ + for (i = 0; i < NY * N; ++i) acadoVariables.y[ i ] = 0.0; + for (i = 0; i < NYN; ++i) acadoVariables.yN[ i ] = 0.0; + + /* MPC: initialize the current state feedback. */ + for (i = 0; i < NX; ++i) acadoVariables.x0[ i ] = 0.0; +} + +void run_mpc(state_t * x0, log_t * solution, + double l_poly[4], double r_poly[4], double p_poly[4], + double l_prob, double r_prob, double p_prob, double curvature_factor, double v_ref, double lane_width){ + + int i; + + for (i = 0; i <= NOD * N; i+= NOD){ + acadoVariables.od[i] = curvature_factor; + acadoVariables.od[i+1] = v_ref; + + acadoVariables.od[i+2] = l_poly[0]; + acadoVariables.od[i+3] = l_poly[1]; + acadoVariables.od[i+4] = l_poly[2]; + acadoVariables.od[i+5] = l_poly[3]; + + acadoVariables.od[i+6] = r_poly[0]; + acadoVariables.od[i+7] = r_poly[1]; + acadoVariables.od[i+8] = r_poly[2]; + acadoVariables.od[i+9] = r_poly[3]; + + acadoVariables.od[i+10] = p_poly[0]; + acadoVariables.od[i+11] = p_poly[1]; + acadoVariables.od[i+12] = p_poly[2]; + acadoVariables.od[i+13] = p_poly[3]; + + + acadoVariables.od[i+14] = l_prob; + acadoVariables.od[i+15] = r_prob; + acadoVariables.od[i+16] = p_prob; + acadoVariables.od[i+17] = lane_width; + + } + + acadoVariables.x0[0] = x0->x; + acadoVariables.x0[1] = x0->y; + acadoVariables.x0[2] = x0->psi; + acadoVariables.x0[3] = x0->delta; + + + acado_preparationStep(); + acado_feedbackStep( ); + + acado_shiftStates(2, 0, 0); + acado_shiftControls( 0 ); + + for (i = 0; i <= N; i++){ + solution->x[i] = acadoVariables.x[i*NX]; + solution->y[i] = acadoVariables.x[i*NX+1]; + solution->psi[i] = acadoVariables.x[i*NX+2]; + solution->delta[i] = acadoVariables.x[i*NX+3]; + } + + return; +} diff --git a/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_auxiliary_functions.c b/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_auxiliary_functions.c new file mode 100644 index 00000000000000..6f0bb705c8a897 --- /dev/null +++ b/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_auxiliary_functions.c @@ -0,0 +1,212 @@ +/* + * This file was auto-generated using the ACADO Toolkit. + * + * While ACADO Toolkit is free software released under the terms of + * the GNU Lesser General Public License (LGPL), the generated code + * as such remains the property of the user who used ACADO Toolkit + * to generate this code. In particular, user dependent data of the code + * do not inherit the GNU LGPL license. On the other hand, parts of the + * generated code that are a direct copy of source code from the + * ACADO Toolkit or the software tools it is based on, remain, as derived + * work, automatically covered by the LGPL license. + * + * ACADO Toolkit is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * + */ + + +#include "acado_auxiliary_functions.h" + +#include + +real_t* acado_getVariablesX( ) +{ + return acadoVariables.x; +} + +real_t* acado_getVariablesU( ) +{ + return acadoVariables.u; +} + +#if ACADO_NY > 0 +real_t* acado_getVariablesY( ) +{ + return acadoVariables.y; +} +#endif + +#if ACADO_NYN > 0 +real_t* acado_getVariablesYN( ) +{ + return acadoVariables.yN; +} +#endif + +real_t* acado_getVariablesX0( ) +{ +#if ACADO_INITIAL_VALUE_FIXED + return acadoVariables.x0; +#else + return 0; +#endif +} + +/** Print differential variables. */ +void acado_printDifferentialVariables( ) +{ + int i, j; + printf("\nDifferential variables:\n[\n"); + for (i = 0; i < ACADO_N + 1; ++i) + { + for (j = 0; j < ACADO_NX; ++j) + printf("\t%e", acadoVariables.x[i * ACADO_NX + j]); + printf("\n"); + } + printf("]\n\n"); +} + +/** Print control variables. */ +void acado_printControlVariables( ) +{ + int i, j; + printf("\nControl variables:\n[\n"); + for (i = 0; i < ACADO_N; ++i) + { + for (j = 0; j < ACADO_NU; ++j) + printf("\t%e", acadoVariables.u[i * ACADO_NU + j]); + printf("\n"); + } + printf("]\n\n"); +} + +/** Print ACADO code generation notice. */ +void acado_printHeader( ) +{ + printf( + "\nACADO Toolkit -- A Toolkit for Automatic Control and Dynamic Optimization.\n" + "Copyright (C) 2008-2015 by Boris Houska, Hans Joachim Ferreau,\n" + "Milan Vukov and Rien Quirynen, KU Leuven.\n" + ); + + printf( + "Developed within the Optimization in Engineering Center (OPTEC) under\n" + "supervision of Moritz Diehl. All rights reserved.\n\n" + "ACADO Toolkit is distributed under the terms of the GNU Lesser\n" + "General Public License 3 in the hope that it will be useful,\n" + "but WITHOUT ANY WARRANTY; without even the implied warranty of\n" + "MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\n" + "GNU Lesser General Public License for more details.\n\n" + ); +} + +#if !(defined _DSPACE) +#if (defined _WIN32 || defined _WIN64) && !(defined __MINGW32__ || defined __MINGW64__) + +void acado_tic( acado_timer* t ) +{ + QueryPerformanceFrequency(&t->freq); + QueryPerformanceCounter(&t->tic); +} + +real_t acado_toc( acado_timer* t ) +{ + QueryPerformanceCounter(&t->toc); + return ((t->toc.QuadPart - t->tic.QuadPart) / (real_t)t->freq.QuadPart); +} + + +#elif (defined __APPLE__) + +void acado_tic( acado_timer* t ) +{ + /* read current clock cycles */ + t->tic = mach_absolute_time(); +} + +real_t acado_toc( acado_timer* t ) +{ + + uint64_t duration; /* elapsed time in clock cycles*/ + + t->toc = mach_absolute_time(); + duration = t->toc - t->tic; + + /*conversion from clock cycles to nanoseconds*/ + mach_timebase_info(&(t->tinfo)); + duration *= t->tinfo.numer; + duration /= t->tinfo.denom; + + return (real_t)duration / 1e9; +} + +#else + +#if __STDC_VERSION__ >= 199901L +/* C99 mode */ + +/* read current time */ +void acado_tic( acado_timer* t ) +{ + gettimeofday(&t->tic, 0); +} + +/* return time passed since last call to tic on this timer */ +real_t acado_toc( acado_timer* t ) +{ + struct timeval temp; + + gettimeofday(&t->toc, 0); + + if ((t->toc.tv_usec - t->tic.tv_usec) < 0) + { + temp.tv_sec = t->toc.tv_sec - t->tic.tv_sec - 1; + temp.tv_usec = 1000000 + t->toc.tv_usec - t->tic.tv_usec; + } + else + { + temp.tv_sec = t->toc.tv_sec - t->tic.tv_sec; + temp.tv_usec = t->toc.tv_usec - t->tic.tv_usec; + } + + return (real_t)temp.tv_sec + (real_t)temp.tv_usec / 1e6; +} + +#else +/* ANSI */ + +/* read current time */ +void acado_tic( acado_timer* t ) +{ + clock_gettime(CLOCK_MONOTONIC, &t->tic); +} + + +/* return time passed since last call to tic on this timer */ +real_t acado_toc( acado_timer* t ) +{ + struct timespec temp; + + clock_gettime(CLOCK_MONOTONIC, &t->toc); + + if ((t->toc.tv_nsec - t->tic.tv_nsec) < 0) + { + temp.tv_sec = t->toc.tv_sec - t->tic.tv_sec - 1; + temp.tv_nsec = 1000000000+t->toc.tv_nsec - t->tic.tv_nsec; + } + else + { + temp.tv_sec = t->toc.tv_sec - t->tic.tv_sec; + temp.tv_nsec = t->toc.tv_nsec - t->tic.tv_nsec; + } + + return (real_t)temp.tv_sec + (real_t)temp.tv_nsec / 1e9; +} + +#endif /* __STDC_VERSION__ >= 199901L */ + +#endif /* (defined _WIN32 || _WIN64) */ + +#endif diff --git a/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_auxiliary_functions.h b/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_auxiliary_functions.h new file mode 100644 index 00000000000000..b1be0a661c0125 --- /dev/null +++ b/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_auxiliary_functions.h @@ -0,0 +1,138 @@ +/* + * This file was auto-generated using the ACADO Toolkit. + * + * While ACADO Toolkit is free software released under the terms of + * the GNU Lesser General Public License (LGPL), the generated code + * as such remains the property of the user who used ACADO Toolkit + * to generate this code. In particular, user dependent data of the code + * do not inherit the GNU LGPL license. On the other hand, parts of the + * generated code that are a direct copy of source code from the + * ACADO Toolkit or the software tools it is based on, remain, as derived + * work, automatically covered by the LGPL license. + * + * ACADO Toolkit is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * + */ + + +#ifndef ACADO_AUXILIARY_FUNCTIONS_H +#define ACADO_AUXILIARY_FUNCTIONS_H + +#include "acado_common.h" + +#ifndef __MATLAB__ +#ifdef __cplusplus +extern "C" +{ +#endif /* __cplusplus */ +#endif /* __MATLAB__ */ + +/** Get pointer to the matrix with differential variables. */ +real_t* acado_getVariablesX( ); + +/** Get pointer to the matrix with control variables. */ +real_t* acado_getVariablesU( ); + +#if ACADO_NY > 0 +/** Get pointer to the matrix with references/measurements. */ +real_t* acado_getVariablesY( ); +#endif + +#if ACADO_NYN > 0 +/** Get pointer to the vector with references/measurement on the last node. */ +real_t* acado_getVariablesYN( ); +#endif + +/** Get pointer to the current state feedback vector. Only applicable for NMPC. */ +real_t* acado_getVariablesX0( ); + +/** Print differential variables. */ +void acado_printDifferentialVariables( ); + +/** Print control variables. */ +void acado_printControlVariables( ); + +/** Print ACADO code generation notice. */ +void acado_printHeader( ); + +/* + * A huge thanks goes to Alexander Domahidi from ETHZ, Switzerland, for + * providing us with the following timing routines. + */ + +#if !(defined _DSPACE) +#if (defined _WIN32 || defined _WIN64) && !(defined __MINGW32__ || defined __MINGW64__) + +/* Use Windows QueryPerformanceCounter for timing. */ +#include + +/** A structure for keeping internal timer data. */ +typedef struct acado_timer_ +{ + LARGE_INTEGER tic; + LARGE_INTEGER toc; + LARGE_INTEGER freq; +} acado_timer; + + +#elif (defined __APPLE__) + +#include "unistd.h" +#include + +/** A structure for keeping internal timer data. */ +typedef struct acado_timer_ +{ + uint64_t tic; + uint64_t toc; + mach_timebase_info_data_t tinfo; +} acado_timer; + +#else + +/* Use POSIX clock_gettime() for timing on non-Windows machines. */ +#include + +#if __STDC_VERSION__ >= 199901L +/* C99 mode of operation. */ + +#include +#include + +typedef struct acado_timer_ +{ + struct timeval tic; + struct timeval toc; +} acado_timer; + +#else +/* ANSI C */ + +/** A structure for keeping internal timer data. */ +typedef struct acado_timer_ +{ + struct timespec tic; + struct timespec toc; +} acado_timer; + +#endif /* __STDC_VERSION__ >= 199901L */ + +#endif /* (defined _WIN32 || defined _WIN64) */ + +/** A function for measurement of the current time. */ +void acado_tic( acado_timer* t ); + +/** A function which returns the elapsed time. */ +real_t acado_toc( acado_timer* t ); + +#endif + +#ifndef __MATLAB__ +#ifdef __cplusplus +} /* extern "C" */ +#endif /* __cplusplus */ +#endif /* __MATLAB__ */ + +#endif /* ACADO_AUXILIARY_FUNCTIONS_H */ diff --git a/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_common.h b/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_common.h new file mode 100644 index 00000000000000..a183304f0181b4 --- /dev/null +++ b/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_common.h @@ -0,0 +1,349 @@ +/* + * This file was auto-generated using the ACADO Toolkit. + * + * While ACADO Toolkit is free software released under the terms of + * the GNU Lesser General Public License (LGPL), the generated code + * as such remains the property of the user who used ACADO Toolkit + * to generate this code. In particular, user dependent data of the code + * do not inherit the GNU LGPL license. On the other hand, parts of the + * generated code that are a direct copy of source code from the + * ACADO Toolkit or the software tools it is based on, remain, as derived + * work, automatically covered by the LGPL license. + * + * ACADO Toolkit is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * + */ + + +#ifndef ACADO_COMMON_H +#define ACADO_COMMON_H + +#include +#include + +#ifndef __MATLAB__ +#ifdef __cplusplus +extern "C" +{ +#endif /* __cplusplus */ +#endif /* __MATLAB__ */ + +/** \defgroup ACADO ACADO CGT generated module. */ +/** @{ */ + +/** qpOASES QP solver indicator. */ +#define ACADO_QPOASES 0 +#define ACADO_QPOASES3 1 +/** FORCES QP solver indicator.*/ +#define ACADO_FORCES 2 +/** qpDUNES QP solver indicator.*/ +#define ACADO_QPDUNES 3 +/** HPMPC QP solver indicator. */ +#define ACADO_HPMPC 4 +#define ACADO_GENERIC 5 + +/** Indicator for determining the QP solver used by the ACADO solver code. */ +#define ACADO_QP_SOLVER ACADO_QPOASES + +#include "acado_qpoases_interface.hpp" + + +/* + * Common definitions + */ +/** User defined block based condensing. */ +#define ACADO_BLOCK_CONDENSING 0 +/** Compute covariance matrix of the last state estimate. */ +#define ACADO_COMPUTE_COVARIANCE_MATRIX 0 +/** Flag indicating whether constraint values are hard-coded or not. */ +#define ACADO_HARDCODED_CONSTRAINT_VALUES 1 +/** Indicator for fixed initial state. */ +#define ACADO_INITIAL_STATE_FIXED 1 +/** Number of control/estimation intervals. */ +#define ACADO_N 50 +/** Number of online data values. */ +#define ACADO_NOD 18 +/** Number of path constraints. */ +#define ACADO_NPAC 0 +/** Number of control variables. */ +#define ACADO_NU 1 +/** Number of differential variables. */ +#define ACADO_NX 4 +/** Number of algebraic variables. */ +#define ACADO_NXA 0 +/** Number of differential derivative variables. */ +#define ACADO_NXD 0 +/** Number of references/measurements per node on the first N nodes. */ +#define ACADO_NY 5 +/** Number of references/measurements on the last (N + 1)st node. */ +#define ACADO_NYN 4 +/** Total number of QP optimization variables. */ +#define ACADO_QP_NV 54 +/** Number of integration steps per shooting interval. */ +#define ACADO_RK_NIS 5 +/** Number of Runge-Kutta stages per integration step. */ +#define ACADO_RK_NSTAGES 4 +/** Providing interface for arrival cost. */ +#define ACADO_USE_ARRIVAL_COST 0 +/** Indicator for usage of non-hard-coded linear terms in the objective. */ +#define ACADO_USE_LINEAR_TERMS 0 +/** Indicator for type of fixed weighting matrices. */ +#define ACADO_WEIGHTING_MATRICES_TYPE 0 + + +/* + * Globally used structure definitions + */ + +/** The structure containing the user data. + * + * Via this structure the user "communicates" with the solver code. + */ +typedef struct ACADOvariables_ +{ +int dummy; +/** Matrix of size: 51 x 4 (row major format) + * + * Matrix containing 51 differential variable vectors. + */ +real_t x[ 204 ]; + +/** Column vector of size: 50 + * + * Matrix containing 50 control variable vectors. + */ +real_t u[ 50 ]; + +/** Matrix of size: 51 x 18 (row major format) + * + * Matrix containing 51 online data vectors. + */ +real_t od[ 918 ]; + +/** Column vector of size: 250 + * + * Matrix containing 50 reference/measurement vectors of size 5 for first 50 nodes. + */ +real_t y[ 250 ]; + +/** Column vector of size: 4 + * + * Reference/measurement vector for the 51. node. + */ +real_t yN[ 4 ]; + +/** Column vector of size: 4 + * + * Current state feedback vector. + */ +real_t x0[ 4 ]; + + +} ACADOvariables; + +/** Private workspace used by the auto-generated code. + * + * Data members of this structure are private to the solver. + * In other words, the user code should not modify values of this + * structure. + */ +typedef struct ACADOworkspace_ +{ +/** Column vector of size: 14 */ +real_t rhs_aux[ 14 ]; + +real_t rk_ttt; + +/** Row vector of size: 43 */ +real_t rk_xxx[ 43 ]; + +/** Matrix of size: 4 x 24 (row major format) */ +real_t rk_kkk[ 96 ]; + +/** Row vector of size: 43 */ +real_t state[ 43 ]; + +/** Column vector of size: 200 */ +real_t d[ 200 ]; + +/** Column vector of size: 250 */ +real_t Dy[ 250 ]; + +/** Column vector of size: 4 */ +real_t DyN[ 4 ]; + +/** Matrix of size: 200 x 4 (row major format) */ +real_t evGx[ 800 ]; + +/** Column vector of size: 200 */ +real_t evGu[ 200 ]; + +/** Column vector of size: 19 */ +real_t objAuxVar[ 19 ]; + +/** Row vector of size: 23 */ +real_t objValueIn[ 23 ]; + +/** Row vector of size: 30 */ +real_t objValueOut[ 30 ]; + +/** Matrix of size: 200 x 4 (row major format) */ +real_t Q1[ 800 ]; + +/** Matrix of size: 200 x 5 (row major format) */ +real_t Q2[ 1000 ]; + +/** Column vector of size: 50 */ +real_t R1[ 50 ]; + +/** Matrix of size: 50 x 5 (row major format) */ +real_t R2[ 250 ]; + +/** Matrix of size: 4 x 4 (row major format) */ +real_t QN1[ 16 ]; + +/** Matrix of size: 4 x 4 (row major format) */ +real_t QN2[ 16 ]; + +/** Column vector of size: 4 */ +real_t Dx0[ 4 ]; + +/** Matrix of size: 4 x 4 (row major format) */ +real_t T[ 16 ]; + +/** Column vector of size: 5100 */ +real_t E[ 5100 ]; + +/** Column vector of size: 5100 */ +real_t QE[ 5100 ]; + +/** Matrix of size: 200 x 4 (row major format) */ +real_t QGx[ 800 ]; + +/** Column vector of size: 200 */ +real_t Qd[ 200 ]; + +/** Column vector of size: 204 */ +real_t QDy[ 204 ]; + +/** Matrix of size: 50 x 4 (row major format) */ +real_t H10[ 200 ]; + +/** Matrix of size: 54 x 54 (row major format) */ +real_t H[ 2916 ]; + +/** Matrix of size: 100 x 54 (row major format) */ +real_t A[ 5400 ]; + +/** Column vector of size: 54 */ +real_t g[ 54 ]; + +/** Column vector of size: 54 */ +real_t lb[ 54 ]; + +/** Column vector of size: 54 */ +real_t ub[ 54 ]; + +/** Column vector of size: 100 */ +real_t lbA[ 100 ]; + +/** Column vector of size: 100 */ +real_t ubA[ 100 ]; + +/** Column vector of size: 54 */ +real_t x[ 54 ]; + +/** Column vector of size: 154 */ +real_t y[ 154 ]; + + +} ACADOworkspace; + +/* + * Forward function declarations. + */ + + +/** Performs the integration and sensitivity propagation for one shooting interval. + * + * \param rk_eta Working array to pass the input values and return the results. + * \param resetIntegrator The internal memory of the integrator can be reset. + * + * \return Status code of the integrator. + */ +int acado_integrate( real_t* const rk_eta, int resetIntegrator ); + +/** Export of an ACADO symbolic function. + * + * \param in Input to the exported function. + * \param out Output of the exported function. + */ +void acado_rhs_forw(const real_t* in, real_t* out); + +/** Preparation step of the RTI scheme. + * + * \return Status of the integration module. =0: OK, otherwise the error code. + */ +int acado_preparationStep( ); + +/** Feedback/estimation step of the RTI scheme. + * + * \return Status code of the qpOASES QP solver. + */ +int acado_feedbackStep( ); + +/** Solver initialization. Must be called once before any other function call. + * + * \return =0: OK, otherwise an error code of a QP solver. + */ +int acado_initializeSolver( ); + +/** Initialize shooting nodes by a forward simulation starting from the first node. + */ +void acado_initializeNodesByForwardSimulation( ); + +/** Shift differential variables vector by one interval. + * + * \param strategy Shifting strategy: 1. Initialize node 51 with xEnd. 2. Initialize node 51 by forward simulation. + * \param xEnd Value for the x vector on the last node. If =0 the old value is used. + * \param uEnd Value for the u vector on the second to last node. If =0 the old value is used. + */ +void acado_shiftStates( int strategy, real_t* const xEnd, real_t* const uEnd ); + +/** Shift controls vector by one interval. + * + * \param uEnd Value for the u vector on the second to last node. If =0 the old value is used. + */ +void acado_shiftControls( real_t* const uEnd ); + +/** Get the KKT tolerance of the current iterate. + * + * \return The KKT tolerance value. + */ +real_t acado_getKKT( ); + +/** Calculate the objective value. + * + * \return Value of the objective function. + */ +real_t acado_getObjective( ); + + +/* + * Extern declarations. + */ + +extern ACADOworkspace acadoWorkspace; +extern ACADOvariables acadoVariables; + +/** @} */ + +#ifndef __MATLAB__ +#ifdef __cplusplus +} /* extern "C" */ +#endif /* __cplusplus */ +#endif /* __MATLAB__ */ + +#endif /* ACADO_COMMON_H */ diff --git a/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_integrator.c b/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_integrator.c new file mode 100644 index 00000000000000..f243d0ffa28169 --- /dev/null +++ b/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_integrator.c @@ -0,0 +1,252 @@ +/* + * This file was auto-generated using the ACADO Toolkit. + * + * While ACADO Toolkit is free software released under the terms of + * the GNU Lesser General Public License (LGPL), the generated code + * as such remains the property of the user who used ACADO Toolkit + * to generate this code. In particular, user dependent data of the code + * do not inherit the GNU LGPL license. On the other hand, parts of the + * generated code that are a direct copy of source code from the + * ACADO Toolkit or the software tools it is based on, remain, as derived + * work, automatically covered by the LGPL license. + * + * ACADO Toolkit is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * + */ + + +#include "acado_common.h" + + +void acado_rhs_forw(const real_t* in, real_t* out) +{ +const real_t* xd = in; +const real_t* u = in + 24; +const real_t* od = in + 25; +/* Vector of auxiliary variables; number of elements: 14. */ +real_t* a = acadoWorkspace.rhs_aux; + +/* Compute intermediate quantities: */ +a[0] = (cos(xd[2])); +a[1] = (sin(xd[2])); +a[2] = ((real_t)(-1.0000000000000000e+00)*(sin(xd[2]))); +a[3] = (xd[12]*a[2]); +a[4] = (xd[13]*a[2]); +a[5] = (xd[14]*a[2]); +a[6] = (xd[15]*a[2]); +a[7] = (cos(xd[2])); +a[8] = (xd[12]*a[7]); +a[9] = (xd[13]*a[7]); +a[10] = (xd[14]*a[7]); +a[11] = (xd[15]*a[7]); +a[12] = (xd[22]*a[2]); +a[13] = (xd[22]*a[7]); + +/* Compute outputs: */ +out[0] = (od[1]*a[0]); +out[1] = (od[1]*a[1]); +out[2] = ((od[1]*xd[3])*od[0]); +out[3] = u[0]; +out[4] = (od[1]*a[3]); +out[5] = (od[1]*a[4]); +out[6] = (od[1]*a[5]); +out[7] = (od[1]*a[6]); +out[8] = (od[1]*a[8]); +out[9] = (od[1]*a[9]); +out[10] = (od[1]*a[10]); +out[11] = (od[1]*a[11]); +out[12] = ((od[1]*xd[16])*od[0]); +out[13] = ((od[1]*xd[17])*od[0]); +out[14] = ((od[1]*xd[18])*od[0]); +out[15] = ((od[1]*xd[19])*od[0]); +out[16] = (real_t)(0.0000000000000000e+00); +out[17] = (real_t)(0.0000000000000000e+00); +out[18] = (real_t)(0.0000000000000000e+00); +out[19] = (real_t)(0.0000000000000000e+00); +out[20] = (od[1]*a[12]); +out[21] = (od[1]*a[13]); +out[22] = ((od[1]*xd[23])*od[0]); +out[23] = (real_t)(1.0000000000000000e+00); +} + +/* Fixed step size:0.01 */ +int acado_integrate( real_t* const rk_eta, int resetIntegrator ) +{ +int error; + +int run1; +acadoWorkspace.rk_ttt = 0.0000000000000000e+00; +rk_eta[4] = 1.0000000000000000e+00; +rk_eta[5] = 0.0000000000000000e+00; +rk_eta[6] = 0.0000000000000000e+00; +rk_eta[7] = 0.0000000000000000e+00; +rk_eta[8] = 0.0000000000000000e+00; +rk_eta[9] = 1.0000000000000000e+00; +rk_eta[10] = 0.0000000000000000e+00; +rk_eta[11] = 0.0000000000000000e+00; +rk_eta[12] = 0.0000000000000000e+00; +rk_eta[13] = 0.0000000000000000e+00; +rk_eta[14] = 1.0000000000000000e+00; +rk_eta[15] = 0.0000000000000000e+00; +rk_eta[16] = 0.0000000000000000e+00; +rk_eta[17] = 0.0000000000000000e+00; +rk_eta[18] = 0.0000000000000000e+00; +rk_eta[19] = 1.0000000000000000e+00; +rk_eta[20] = 0.0000000000000000e+00; +rk_eta[21] = 0.0000000000000000e+00; +rk_eta[22] = 0.0000000000000000e+00; +rk_eta[23] = 0.0000000000000000e+00; +acadoWorkspace.rk_xxx[24] = rk_eta[24]; +acadoWorkspace.rk_xxx[25] = rk_eta[25]; +acadoWorkspace.rk_xxx[26] = rk_eta[26]; +acadoWorkspace.rk_xxx[27] = rk_eta[27]; +acadoWorkspace.rk_xxx[28] = rk_eta[28]; +acadoWorkspace.rk_xxx[29] = rk_eta[29]; +acadoWorkspace.rk_xxx[30] = rk_eta[30]; +acadoWorkspace.rk_xxx[31] = rk_eta[31]; +acadoWorkspace.rk_xxx[32] = rk_eta[32]; +acadoWorkspace.rk_xxx[33] = rk_eta[33]; +acadoWorkspace.rk_xxx[34] = rk_eta[34]; +acadoWorkspace.rk_xxx[35] = rk_eta[35]; +acadoWorkspace.rk_xxx[36] = rk_eta[36]; +acadoWorkspace.rk_xxx[37] = rk_eta[37]; +acadoWorkspace.rk_xxx[38] = rk_eta[38]; +acadoWorkspace.rk_xxx[39] = rk_eta[39]; +acadoWorkspace.rk_xxx[40] = rk_eta[40]; +acadoWorkspace.rk_xxx[41] = rk_eta[41]; +acadoWorkspace.rk_xxx[42] = rk_eta[42]; + +for (run1 = 0; run1 < 5; ++run1) +{ +acadoWorkspace.rk_xxx[0] = + rk_eta[0]; +acadoWorkspace.rk_xxx[1] = + rk_eta[1]; +acadoWorkspace.rk_xxx[2] = + rk_eta[2]; +acadoWorkspace.rk_xxx[3] = + rk_eta[3]; +acadoWorkspace.rk_xxx[4] = + rk_eta[4]; +acadoWorkspace.rk_xxx[5] = + rk_eta[5]; +acadoWorkspace.rk_xxx[6] = + rk_eta[6]; +acadoWorkspace.rk_xxx[7] = + rk_eta[7]; +acadoWorkspace.rk_xxx[8] = + rk_eta[8]; +acadoWorkspace.rk_xxx[9] = + rk_eta[9]; +acadoWorkspace.rk_xxx[10] = + rk_eta[10]; +acadoWorkspace.rk_xxx[11] = + rk_eta[11]; +acadoWorkspace.rk_xxx[12] = + rk_eta[12]; +acadoWorkspace.rk_xxx[13] = + rk_eta[13]; +acadoWorkspace.rk_xxx[14] = + rk_eta[14]; +acadoWorkspace.rk_xxx[15] = + rk_eta[15]; +acadoWorkspace.rk_xxx[16] = + rk_eta[16]; +acadoWorkspace.rk_xxx[17] = + rk_eta[17]; +acadoWorkspace.rk_xxx[18] = + rk_eta[18]; +acadoWorkspace.rk_xxx[19] = + rk_eta[19]; +acadoWorkspace.rk_xxx[20] = + rk_eta[20]; +acadoWorkspace.rk_xxx[21] = + rk_eta[21]; +acadoWorkspace.rk_xxx[22] = + rk_eta[22]; +acadoWorkspace.rk_xxx[23] = + rk_eta[23]; +acado_rhs_forw( acadoWorkspace.rk_xxx, acadoWorkspace.rk_kkk ); +acadoWorkspace.rk_xxx[0] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[0] + rk_eta[0]; +acadoWorkspace.rk_xxx[1] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[1] + rk_eta[1]; +acadoWorkspace.rk_xxx[2] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[2] + rk_eta[2]; +acadoWorkspace.rk_xxx[3] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[3] + rk_eta[3]; +acadoWorkspace.rk_xxx[4] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[4] + rk_eta[4]; +acadoWorkspace.rk_xxx[5] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[5] + rk_eta[5]; +acadoWorkspace.rk_xxx[6] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[6] + rk_eta[6]; +acadoWorkspace.rk_xxx[7] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[7] + rk_eta[7]; +acadoWorkspace.rk_xxx[8] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[8] + rk_eta[8]; +acadoWorkspace.rk_xxx[9] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[9] + rk_eta[9]; +acadoWorkspace.rk_xxx[10] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[10] + rk_eta[10]; +acadoWorkspace.rk_xxx[11] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[11] + rk_eta[11]; +acadoWorkspace.rk_xxx[12] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[12] + rk_eta[12]; +acadoWorkspace.rk_xxx[13] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[13] + rk_eta[13]; +acadoWorkspace.rk_xxx[14] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[14] + rk_eta[14]; +acadoWorkspace.rk_xxx[15] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[15] + rk_eta[15]; +acadoWorkspace.rk_xxx[16] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[16] + rk_eta[16]; +acadoWorkspace.rk_xxx[17] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[17] + rk_eta[17]; +acadoWorkspace.rk_xxx[18] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[18] + rk_eta[18]; +acadoWorkspace.rk_xxx[19] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[19] + rk_eta[19]; +acadoWorkspace.rk_xxx[20] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[20] + rk_eta[20]; +acadoWorkspace.rk_xxx[21] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[21] + rk_eta[21]; +acadoWorkspace.rk_xxx[22] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[22] + rk_eta[22]; +acadoWorkspace.rk_xxx[23] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[23] + rk_eta[23]; +acado_rhs_forw( acadoWorkspace.rk_xxx, &(acadoWorkspace.rk_kkk[ 24 ]) ); +acadoWorkspace.rk_xxx[0] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[24] + rk_eta[0]; +acadoWorkspace.rk_xxx[1] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[25] + rk_eta[1]; +acadoWorkspace.rk_xxx[2] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[26] + rk_eta[2]; +acadoWorkspace.rk_xxx[3] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[27] + rk_eta[3]; +acadoWorkspace.rk_xxx[4] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[28] + rk_eta[4]; +acadoWorkspace.rk_xxx[5] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[29] + rk_eta[5]; +acadoWorkspace.rk_xxx[6] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[30] + rk_eta[6]; +acadoWorkspace.rk_xxx[7] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[31] + rk_eta[7]; +acadoWorkspace.rk_xxx[8] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[32] + rk_eta[8]; +acadoWorkspace.rk_xxx[9] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[33] + rk_eta[9]; +acadoWorkspace.rk_xxx[10] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[34] + rk_eta[10]; +acadoWorkspace.rk_xxx[11] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[35] + rk_eta[11]; +acadoWorkspace.rk_xxx[12] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[36] + rk_eta[12]; +acadoWorkspace.rk_xxx[13] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[37] + rk_eta[13]; +acadoWorkspace.rk_xxx[14] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[38] + rk_eta[14]; +acadoWorkspace.rk_xxx[15] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[39] + rk_eta[15]; +acadoWorkspace.rk_xxx[16] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[40] + rk_eta[16]; +acadoWorkspace.rk_xxx[17] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[41] + rk_eta[17]; +acadoWorkspace.rk_xxx[18] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[42] + rk_eta[18]; +acadoWorkspace.rk_xxx[19] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[43] + rk_eta[19]; +acadoWorkspace.rk_xxx[20] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[44] + rk_eta[20]; +acadoWorkspace.rk_xxx[21] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[45] + rk_eta[21]; +acadoWorkspace.rk_xxx[22] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[46] + rk_eta[22]; +acadoWorkspace.rk_xxx[23] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[47] + rk_eta[23]; +acado_rhs_forw( acadoWorkspace.rk_xxx, &(acadoWorkspace.rk_kkk[ 48 ]) ); +acadoWorkspace.rk_xxx[0] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[48] + rk_eta[0]; +acadoWorkspace.rk_xxx[1] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[49] + rk_eta[1]; +acadoWorkspace.rk_xxx[2] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[50] + rk_eta[2]; +acadoWorkspace.rk_xxx[3] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[51] + rk_eta[3]; +acadoWorkspace.rk_xxx[4] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[52] + rk_eta[4]; +acadoWorkspace.rk_xxx[5] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[53] + rk_eta[5]; +acadoWorkspace.rk_xxx[6] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[54] + rk_eta[6]; +acadoWorkspace.rk_xxx[7] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[55] + rk_eta[7]; +acadoWorkspace.rk_xxx[8] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[56] + rk_eta[8]; +acadoWorkspace.rk_xxx[9] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[57] + rk_eta[9]; +acadoWorkspace.rk_xxx[10] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[58] + rk_eta[10]; +acadoWorkspace.rk_xxx[11] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[59] + rk_eta[11]; +acadoWorkspace.rk_xxx[12] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[60] + rk_eta[12]; +acadoWorkspace.rk_xxx[13] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[61] + rk_eta[13]; +acadoWorkspace.rk_xxx[14] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[62] + rk_eta[14]; +acadoWorkspace.rk_xxx[15] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[63] + rk_eta[15]; +acadoWorkspace.rk_xxx[16] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[64] + rk_eta[16]; +acadoWorkspace.rk_xxx[17] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[65] + rk_eta[17]; +acadoWorkspace.rk_xxx[18] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[66] + rk_eta[18]; +acadoWorkspace.rk_xxx[19] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[67] + rk_eta[19]; +acadoWorkspace.rk_xxx[20] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[68] + rk_eta[20]; +acadoWorkspace.rk_xxx[21] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[69] + rk_eta[21]; +acadoWorkspace.rk_xxx[22] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[70] + rk_eta[22]; +acadoWorkspace.rk_xxx[23] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[71] + rk_eta[23]; +acado_rhs_forw( acadoWorkspace.rk_xxx, &(acadoWorkspace.rk_kkk[ 72 ]) ); +rk_eta[0] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[0] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[24] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[48] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[72]; +rk_eta[1] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[1] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[25] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[49] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[73]; +rk_eta[2] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[2] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[26] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[50] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[74]; +rk_eta[3] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[3] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[27] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[51] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[75]; +rk_eta[4] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[4] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[28] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[52] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[76]; +rk_eta[5] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[5] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[29] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[53] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[77]; +rk_eta[6] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[6] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[30] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[54] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[78]; +rk_eta[7] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[7] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[31] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[55] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[79]; +rk_eta[8] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[8] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[32] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[56] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[80]; +rk_eta[9] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[9] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[33] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[57] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[81]; +rk_eta[10] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[10] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[34] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[58] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[82]; +rk_eta[11] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[11] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[35] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[59] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[83]; +rk_eta[12] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[12] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[36] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[60] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[84]; +rk_eta[13] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[13] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[37] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[61] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[85]; +rk_eta[14] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[14] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[38] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[62] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[86]; +rk_eta[15] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[15] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[39] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[63] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[87]; +rk_eta[16] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[16] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[40] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[64] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[88]; +rk_eta[17] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[17] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[41] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[65] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[89]; +rk_eta[18] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[18] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[42] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[66] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[90]; +rk_eta[19] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[19] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[43] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[67] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[91]; +rk_eta[20] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[20] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[44] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[68] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[92]; +rk_eta[21] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[21] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[45] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[69] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[93]; +rk_eta[22] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[22] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[46] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[70] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[94]; +rk_eta[23] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[23] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[47] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[71] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[95]; +acadoWorkspace.rk_ttt += 2.0000000000000001e-01; +} +error = 0; +return error; +} + diff --git a/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_qpoases_interface.cpp b/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_qpoases_interface.cpp new file mode 100644 index 00000000000000..09c7da96cad333 --- /dev/null +++ b/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_qpoases_interface.cpp @@ -0,0 +1,70 @@ +/* + * This file was auto-generated using the ACADO Toolkit. + * + * While ACADO Toolkit is free software released under the terms of + * the GNU Lesser General Public License (LGPL), the generated code + * as such remains the property of the user who used ACADO Toolkit + * to generate this code. In particular, user dependent data of the code + * do not inherit the GNU LGPL license. On the other hand, parts of the + * generated code that are a direct copy of source code from the + * ACADO Toolkit or the software tools it is based on, remain, as derived + * work, automatically covered by the LGPL license. + * + * ACADO Toolkit is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * + */ + + +extern "C" +{ +#include "acado_common.h" +} + +#include "INCLUDE/QProblem.hpp" + +#if ACADO_COMPUTE_COVARIANCE_MATRIX == 1 +#include "INCLUDE/EXTRAS/SolutionAnalysis.hpp" +#endif /* ACADO_COMPUTE_COVARIANCE_MATRIX */ + +static int acado_nWSR; + + + +#if ACADO_COMPUTE_COVARIANCE_MATRIX == 1 +static SolutionAnalysis acado_sa; +#endif /* ACADO_COMPUTE_COVARIANCE_MATRIX */ + +int acado_solve( void ) +{ + acado_nWSR = QPOASES_NWSRMAX; + + QProblem qp(54, 100); + + returnValue retVal = qp.init(acadoWorkspace.H, acadoWorkspace.g, acadoWorkspace.A, acadoWorkspace.lb, acadoWorkspace.ub, acadoWorkspace.lbA, acadoWorkspace.ubA, acado_nWSR, acadoWorkspace.y); + + qp.getPrimalSolution( acadoWorkspace.x ); + qp.getDualSolution( acadoWorkspace.y ); + +#if ACADO_COMPUTE_COVARIANCE_MATRIX == 1 + + if (retVal != SUCCESSFUL_RETURN) + return (int)retVal; + + retVal = acado_sa.getHessianInverse( &qp,var ); + +#endif /* ACADO_COMPUTE_COVARIANCE_MATRIX */ + + return (int)retVal; +} + +int acado_getNWSR( void ) +{ + return acado_nWSR; +} + +const char* acado_getErrorString( int error ) +{ + return MessageHandling::getErrorString( error ); +} diff --git a/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_qpoases_interface.hpp b/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_qpoases_interface.hpp new file mode 100644 index 00000000000000..d14523d05a1a0d --- /dev/null +++ b/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_qpoases_interface.hpp @@ -0,0 +1,65 @@ +/* + * This file was auto-generated using the ACADO Toolkit. + * + * While ACADO Toolkit is free software released under the terms of + * the GNU Lesser General Public License (LGPL), the generated code + * as such remains the property of the user who used ACADO Toolkit + * to generate this code. In particular, user dependent data of the code + * do not inherit the GNU LGPL license. On the other hand, parts of the + * generated code that are a direct copy of source code from the + * ACADO Toolkit or the software tools it is based on, remain, as derived + * work, automatically covered by the LGPL license. + * + * ACADO Toolkit is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * + */ + + +#ifndef QPOASES_HEADER +#define QPOASES_HEADER + +#ifdef PC_DEBUG +#include +#endif /* PC_DEBUG */ + +#include + +#ifdef __cplusplus +#define EXTERNC extern "C" +#else +#define EXTERNC +#endif + +/* + * A set of options for qpOASES + */ + +/** Maximum number of optimization variables. */ +#define QPOASES_NVMAX 54 +/** Maximum number of constraints. */ +#define QPOASES_NCMAX 100 +/** Maximum number of working set recalculations. */ +#define QPOASES_NWSRMAX 462 +/** Print level for qpOASES. */ +#define QPOASES_PRINTLEVEL PL_NONE +/** The value of EPS */ +#define QPOASES_EPS 2.221e-16 +/** Internally used floating point type */ +typedef double real_t; + +/* + * Forward function declarations + */ + +/** A function that calls the QP solver */ +EXTERNC int acado_solve( void ); + +/** Get the number of active set changes */ +EXTERNC int acado_getNWSR( void ); + +/** Get the error string. */ +const char* acado_getErrorString( int error ); + +#endif /* QPOASES_HEADER */ diff --git a/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_solver.c b/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_solver.c new file mode 100644 index 00000000000000..963a36430a9bb7 --- /dev/null +++ b/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_solver.c @@ -0,0 +1,1981 @@ +/* + * This file was auto-generated using the ACADO Toolkit. + * + * While ACADO Toolkit is free software released under the terms of + * the GNU Lesser General Public License (LGPL), the generated code + * as such remains the property of the user who used ACADO Toolkit + * to generate this code. In particular, user dependent data of the code + * do not inherit the GNU LGPL license. On the other hand, parts of the + * generated code that are a direct copy of source code from the + * ACADO Toolkit or the software tools it is based on, remain, as derived + * work, automatically covered by the LGPL license. + * + * ACADO Toolkit is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * + */ + + +#include "acado_common.h" + + + + +/******************************************************************************/ +/* */ +/* ACADO code generation */ +/* */ +/******************************************************************************/ + + +int acado_modelSimulation( ) +{ +int ret; + +int lRun1; +ret = 0; +for (lRun1 = 0; lRun1 < 50; ++lRun1) +{ +acadoWorkspace.state[0] = acadoVariables.x[lRun1 * 4]; +acadoWorkspace.state[1] = acadoVariables.x[lRun1 * 4 + 1]; +acadoWorkspace.state[2] = acadoVariables.x[lRun1 * 4 + 2]; +acadoWorkspace.state[3] = acadoVariables.x[lRun1 * 4 + 3]; + +acadoWorkspace.state[24] = acadoVariables.u[lRun1]; +acadoWorkspace.state[25] = acadoVariables.od[lRun1 * 18]; +acadoWorkspace.state[26] = acadoVariables.od[lRun1 * 18 + 1]; +acadoWorkspace.state[27] = acadoVariables.od[lRun1 * 18 + 2]; +acadoWorkspace.state[28] = acadoVariables.od[lRun1 * 18 + 3]; +acadoWorkspace.state[29] = acadoVariables.od[lRun1 * 18 + 4]; +acadoWorkspace.state[30] = acadoVariables.od[lRun1 * 18 + 5]; +acadoWorkspace.state[31] = acadoVariables.od[lRun1 * 18 + 6]; +acadoWorkspace.state[32] = acadoVariables.od[lRun1 * 18 + 7]; +acadoWorkspace.state[33] = acadoVariables.od[lRun1 * 18 + 8]; +acadoWorkspace.state[34] = acadoVariables.od[lRun1 * 18 + 9]; +acadoWorkspace.state[35] = acadoVariables.od[lRun1 * 18 + 10]; +acadoWorkspace.state[36] = acadoVariables.od[lRun1 * 18 + 11]; +acadoWorkspace.state[37] = acadoVariables.od[lRun1 * 18 + 12]; +acadoWorkspace.state[38] = acadoVariables.od[lRun1 * 18 + 13]; +acadoWorkspace.state[39] = acadoVariables.od[lRun1 * 18 + 14]; +acadoWorkspace.state[40] = acadoVariables.od[lRun1 * 18 + 15]; +acadoWorkspace.state[41] = acadoVariables.od[lRun1 * 18 + 16]; +acadoWorkspace.state[42] = acadoVariables.od[lRun1 * 18 + 17]; + +ret = acado_integrate(acadoWorkspace.state, 1); + +acadoWorkspace.d[lRun1 * 4] = acadoWorkspace.state[0] - acadoVariables.x[lRun1 * 4 + 4]; +acadoWorkspace.d[lRun1 * 4 + 1] = acadoWorkspace.state[1] - acadoVariables.x[lRun1 * 4 + 5]; +acadoWorkspace.d[lRun1 * 4 + 2] = acadoWorkspace.state[2] - acadoVariables.x[lRun1 * 4 + 6]; +acadoWorkspace.d[lRun1 * 4 + 3] = acadoWorkspace.state[3] - acadoVariables.x[lRun1 * 4 + 7]; + +acadoWorkspace.evGx[lRun1 * 16] = acadoWorkspace.state[4]; +acadoWorkspace.evGx[lRun1 * 16 + 1] = acadoWorkspace.state[5]; +acadoWorkspace.evGx[lRun1 * 16 + 2] = acadoWorkspace.state[6]; +acadoWorkspace.evGx[lRun1 * 16 + 3] = acadoWorkspace.state[7]; +acadoWorkspace.evGx[lRun1 * 16 + 4] = acadoWorkspace.state[8]; +acadoWorkspace.evGx[lRun1 * 16 + 5] = acadoWorkspace.state[9]; +acadoWorkspace.evGx[lRun1 * 16 + 6] = acadoWorkspace.state[10]; +acadoWorkspace.evGx[lRun1 * 16 + 7] = acadoWorkspace.state[11]; +acadoWorkspace.evGx[lRun1 * 16 + 8] = acadoWorkspace.state[12]; +acadoWorkspace.evGx[lRun1 * 16 + 9] = acadoWorkspace.state[13]; +acadoWorkspace.evGx[lRun1 * 16 + 10] = acadoWorkspace.state[14]; +acadoWorkspace.evGx[lRun1 * 16 + 11] = acadoWorkspace.state[15]; +acadoWorkspace.evGx[lRun1 * 16 + 12] = acadoWorkspace.state[16]; +acadoWorkspace.evGx[lRun1 * 16 + 13] = acadoWorkspace.state[17]; +acadoWorkspace.evGx[lRun1 * 16 + 14] = acadoWorkspace.state[18]; +acadoWorkspace.evGx[lRun1 * 16 + 15] = acadoWorkspace.state[19]; + +acadoWorkspace.evGu[lRun1 * 4] = acadoWorkspace.state[20]; +acadoWorkspace.evGu[lRun1 * 4 + 1] = acadoWorkspace.state[21]; +acadoWorkspace.evGu[lRun1 * 4 + 2] = acadoWorkspace.state[22]; +acadoWorkspace.evGu[lRun1 * 4 + 3] = acadoWorkspace.state[23]; +} +return ret; +} + +void acado_evaluateLSQ(const real_t* in, real_t* out) +{ +const real_t* xd = in; +const real_t* u = in + 4; +const real_t* od = in + 5; +/* Vector of auxiliary variables; number of elements: 19. */ +real_t* a = acadoWorkspace.objAuxVar; + +/* Compute intermediate quantities: */ +a[0] = (exp(((real_t)(0.0000000000000000e+00)-(((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5])-xd[1])))); +a[1] = (exp((((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9])-xd[1]))); +a[2] = (atan(((((((real_t)(3.0000000000000000e+00)*od[2])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[3])*xd[0]))+od[4]))); +a[3] = (atan(((((((real_t)(3.0000000000000000e+00)*od[6])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[7])*xd[0]))+od[8]))); +a[4] = (atan(((((((real_t)(3.0000000000000000e+00)*od[10])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[11])*xd[0]))+od[12]))); +a[5] = ((real_t)(1.0000000000000000e+00)/((od[14]+od[15])+(real_t)(1.0000000000000000e-04))); +a[6] = (exp(((real_t)(0.0000000000000000e+00)-(((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5])-xd[1])))); +a[7] = (((real_t)(0.0000000000000000e+00)-(((od[2]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[3]*(xd[0]+xd[0])))+od[4]))*a[6]); +a[8] = (((real_t)(0.0000000000000000e+00)-((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00)))*a[6]); +a[9] = (exp((((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9])-xd[1]))); +a[10] = ((((od[6]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[7]*(xd[0]+xd[0])))+od[8])*a[9]); +a[11] = (((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00))*a[9]); +a[12] = ((real_t)(1.0000000000000000e+00)/((real_t)(1.0000000000000000e+00)+(pow(((((((real_t)(3.0000000000000000e+00)*od[2])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[3])*xd[0]))+od[4]),2)))); +a[13] = ((((((real_t)(3.0000000000000000e+00)*od[2])*xd[0])+(((real_t)(3.0000000000000000e+00)*od[2])*xd[0]))+((real_t)(2.0000000000000000e+00)*od[3]))*a[12]); +a[14] = ((real_t)(1.0000000000000000e+00)/((real_t)(1.0000000000000000e+00)+(pow(((((((real_t)(3.0000000000000000e+00)*od[6])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[7])*xd[0]))+od[8]),2)))); +a[15] = ((((((real_t)(3.0000000000000000e+00)*od[6])*xd[0])+(((real_t)(3.0000000000000000e+00)*od[6])*xd[0]))+((real_t)(2.0000000000000000e+00)*od[7]))*a[14]); +a[16] = ((real_t)(1.0000000000000000e+00)/((od[14]+od[15])+(real_t)(1.0000000000000000e-04))); +a[17] = ((real_t)(1.0000000000000000e+00)/((real_t)(1.0000000000000000e+00)+(pow(((((((real_t)(3.0000000000000000e+00)*od[10])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[11])*xd[0]))+od[12]),2)))); +a[18] = ((((((real_t)(3.0000000000000000e+00)*od[10])*xd[0])+(((real_t)(3.0000000000000000e+00)*od[10])*xd[0]))+((real_t)(2.0000000000000000e+00)*od[11]))*a[17]); + +/* Compute outputs: */ +out[0] = ((((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5])-(od[17]/(real_t)(2.0000000000000000e+00))))+(od[15]*(((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9])+(od[17]/(real_t)(2.0000000000000000e+00))))))/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)))+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])))-xd[1]); +out[1] = (od[14]*a[0]); +out[2] = (od[15]*a[1]); +out[3] = ((od[1]+(real_t)(1.0000000000000000e+00))*((((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*a[2])+(od[15]*a[3])))/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)))+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*a[4]))-xd[2])); +out[4] = ((od[1]+(real_t)(1.0000000000000000e+00))*u[0]); +out[5] = (((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((od[2]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[3]*(xd[0]+xd[0])))+od[4]))+(od[15]*(((od[6]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[7]*(xd[0]+xd[0])))+od[8]))))*a[5])+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*(((od[10]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[11]*(xd[0]+xd[0])))+od[12]))); +out[6] = ((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00)); +out[7] = (real_t)(0.0000000000000000e+00); +out[8] = (real_t)(0.0000000000000000e+00); +out[9] = (od[14]*a[7]); +out[10] = (od[14]*a[8]); +out[11] = (real_t)(0.0000000000000000e+00); +out[12] = (real_t)(0.0000000000000000e+00); +out[13] = (od[15]*a[10]); +out[14] = (od[15]*a[11]); +out[15] = (real_t)(0.0000000000000000e+00); +out[16] = (real_t)(0.0000000000000000e+00); +out[17] = ((od[1]+(real_t)(1.0000000000000000e+00))*(((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*a[13])+(od[15]*a[15])))*a[16])+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*a[18]))); +out[18] = (real_t)(0.0000000000000000e+00); +out[19] = ((od[1]+(real_t)(1.0000000000000000e+00))*((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00))); +out[20] = (real_t)(0.0000000000000000e+00); +out[21] = (real_t)(0.0000000000000000e+00); +out[22] = (real_t)(0.0000000000000000e+00); +out[23] = (real_t)(0.0000000000000000e+00); +out[24] = (real_t)(0.0000000000000000e+00); +out[25] = (real_t)(0.0000000000000000e+00); +out[26] = (real_t)(0.0000000000000000e+00); +out[27] = (real_t)(0.0000000000000000e+00); +out[28] = (real_t)(0.0000000000000000e+00); +out[29] = (od[1]+(real_t)(1.0000000000000000e+00)); +} + +void acado_evaluateLSQEndTerm(const real_t* in, real_t* out) +{ +const real_t* xd = in; +const real_t* od = in + 4; +/* Vector of auxiliary variables; number of elements: 19. */ +real_t* a = acadoWorkspace.objAuxVar; + +/* Compute intermediate quantities: */ +a[0] = (exp(((real_t)(0.0000000000000000e+00)-(((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5])-xd[1])))); +a[1] = (exp((((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9])-xd[1]))); +a[2] = (atan(((((((real_t)(3.0000000000000000e+00)*od[2])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[3])*xd[0]))+od[4]))); +a[3] = (atan(((((((real_t)(3.0000000000000000e+00)*od[6])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[7])*xd[0]))+od[8]))); +a[4] = (atan(((((((real_t)(3.0000000000000000e+00)*od[10])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[11])*xd[0]))+od[12]))); +a[5] = ((real_t)(1.0000000000000000e+00)/((od[14]+od[15])+(real_t)(1.0000000000000000e-04))); +a[6] = (exp(((real_t)(0.0000000000000000e+00)-(((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5])-xd[1])))); +a[7] = (((real_t)(0.0000000000000000e+00)-(((od[2]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[3]*(xd[0]+xd[0])))+od[4]))*a[6]); +a[8] = (((real_t)(0.0000000000000000e+00)-((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00)))*a[6]); +a[9] = (exp((((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9])-xd[1]))); +a[10] = ((((od[6]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[7]*(xd[0]+xd[0])))+od[8])*a[9]); +a[11] = (((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00))*a[9]); +a[12] = ((real_t)(1.0000000000000000e+00)/((real_t)(1.0000000000000000e+00)+(pow(((((((real_t)(3.0000000000000000e+00)*od[2])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[3])*xd[0]))+od[4]),2)))); +a[13] = ((((((real_t)(3.0000000000000000e+00)*od[2])*xd[0])+(((real_t)(3.0000000000000000e+00)*od[2])*xd[0]))+((real_t)(2.0000000000000000e+00)*od[3]))*a[12]); +a[14] = ((real_t)(1.0000000000000000e+00)/((real_t)(1.0000000000000000e+00)+(pow(((((((real_t)(3.0000000000000000e+00)*od[6])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[7])*xd[0]))+od[8]),2)))); +a[15] = ((((((real_t)(3.0000000000000000e+00)*od[6])*xd[0])+(((real_t)(3.0000000000000000e+00)*od[6])*xd[0]))+((real_t)(2.0000000000000000e+00)*od[7]))*a[14]); +a[16] = ((real_t)(1.0000000000000000e+00)/((od[14]+od[15])+(real_t)(1.0000000000000000e-04))); +a[17] = ((real_t)(1.0000000000000000e+00)/((real_t)(1.0000000000000000e+00)+(pow(((((((real_t)(3.0000000000000000e+00)*od[10])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[11])*xd[0]))+od[12]),2)))); +a[18] = ((((((real_t)(3.0000000000000000e+00)*od[10])*xd[0])+(((real_t)(3.0000000000000000e+00)*od[10])*xd[0]))+((real_t)(2.0000000000000000e+00)*od[11]))*a[17]); + +/* Compute outputs: */ +out[0] = ((((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5])-(od[17]/(real_t)(2.0000000000000000e+00))))+(od[15]*(((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9])+(od[17]/(real_t)(2.0000000000000000e+00))))))/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)))+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])))-xd[1]); +out[1] = (od[14]*a[0]); +out[2] = (od[15]*a[1]); +out[3] = ((((real_t)(2.0000000000000000e+00)*od[1])+(real_t)(1.0000000000000000e+00))*((((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*a[2])+(od[15]*a[3])))/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)))+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*a[4]))-xd[2])); +out[4] = (((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((od[2]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[3]*(xd[0]+xd[0])))+od[4]))+(od[15]*(((od[6]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[7]*(xd[0]+xd[0])))+od[8]))))*a[5])+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*(((od[10]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[11]*(xd[0]+xd[0])))+od[12]))); +out[5] = ((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00)); +out[6] = (real_t)(0.0000000000000000e+00); +out[7] = (real_t)(0.0000000000000000e+00); +out[8] = (od[14]*a[7]); +out[9] = (od[14]*a[8]); +out[10] = (real_t)(0.0000000000000000e+00); +out[11] = (real_t)(0.0000000000000000e+00); +out[12] = (od[15]*a[10]); +out[13] = (od[15]*a[11]); +out[14] = (real_t)(0.0000000000000000e+00); +out[15] = (real_t)(0.0000000000000000e+00); +out[16] = ((((real_t)(2.0000000000000000e+00)*od[1])+(real_t)(1.0000000000000000e+00))*(((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*a[13])+(od[15]*a[15])))*a[16])+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*a[18]))); +out[17] = (real_t)(0.0000000000000000e+00); +out[18] = ((((real_t)(2.0000000000000000e+00)*od[1])+(real_t)(1.0000000000000000e+00))*((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00))); +out[19] = (real_t)(0.0000000000000000e+00); +} + +void acado_setObjQ1Q2( real_t* const tmpFx, real_t* const tmpQ1, real_t* const tmpQ2 ) +{ +tmpQ2[0] = + tmpFx[0]; +tmpQ2[1] = + tmpFx[4]; +tmpQ2[2] = + tmpFx[8]; +tmpQ2[3] = + tmpFx[12]; +tmpQ2[4] = + tmpFx[16]; +tmpQ2[5] = + tmpFx[1]; +tmpQ2[6] = + tmpFx[5]; +tmpQ2[7] = + tmpFx[9]; +tmpQ2[8] = + tmpFx[13]; +tmpQ2[9] = + tmpFx[17]; +tmpQ2[10] = + tmpFx[2]; +tmpQ2[11] = + tmpFx[6]; +tmpQ2[12] = + tmpFx[10]; +tmpQ2[13] = + tmpFx[14]; +tmpQ2[14] = + tmpFx[18]; +tmpQ2[15] = + tmpFx[3]; +tmpQ2[16] = + tmpFx[7]; +tmpQ2[17] = + tmpFx[11]; +tmpQ2[18] = + tmpFx[15]; +tmpQ2[19] = + tmpFx[19]; +tmpQ1[0] = + tmpQ2[0]*tmpFx[0] + tmpQ2[1]*tmpFx[4] + tmpQ2[2]*tmpFx[8] + tmpQ2[3]*tmpFx[12] + tmpQ2[4]*tmpFx[16]; +tmpQ1[1] = + tmpQ2[0]*tmpFx[1] + tmpQ2[1]*tmpFx[5] + tmpQ2[2]*tmpFx[9] + tmpQ2[3]*tmpFx[13] + tmpQ2[4]*tmpFx[17]; +tmpQ1[2] = + tmpQ2[0]*tmpFx[2] + tmpQ2[1]*tmpFx[6] + tmpQ2[2]*tmpFx[10] + tmpQ2[3]*tmpFx[14] + tmpQ2[4]*tmpFx[18]; +tmpQ1[3] = + tmpQ2[0]*tmpFx[3] + tmpQ2[1]*tmpFx[7] + tmpQ2[2]*tmpFx[11] + tmpQ2[3]*tmpFx[15] + tmpQ2[4]*tmpFx[19]; +tmpQ1[4] = + tmpQ2[5]*tmpFx[0] + tmpQ2[6]*tmpFx[4] + tmpQ2[7]*tmpFx[8] + tmpQ2[8]*tmpFx[12] + tmpQ2[9]*tmpFx[16]; +tmpQ1[5] = + tmpQ2[5]*tmpFx[1] + tmpQ2[6]*tmpFx[5] + tmpQ2[7]*tmpFx[9] + tmpQ2[8]*tmpFx[13] + tmpQ2[9]*tmpFx[17]; +tmpQ1[6] = + tmpQ2[5]*tmpFx[2] + tmpQ2[6]*tmpFx[6] + tmpQ2[7]*tmpFx[10] + tmpQ2[8]*tmpFx[14] + tmpQ2[9]*tmpFx[18]; +tmpQ1[7] = + tmpQ2[5]*tmpFx[3] + tmpQ2[6]*tmpFx[7] + tmpQ2[7]*tmpFx[11] + tmpQ2[8]*tmpFx[15] + tmpQ2[9]*tmpFx[19]; +tmpQ1[8] = + tmpQ2[10]*tmpFx[0] + tmpQ2[11]*tmpFx[4] + tmpQ2[12]*tmpFx[8] + tmpQ2[13]*tmpFx[12] + tmpQ2[14]*tmpFx[16]; +tmpQ1[9] = + tmpQ2[10]*tmpFx[1] + tmpQ2[11]*tmpFx[5] + tmpQ2[12]*tmpFx[9] + tmpQ2[13]*tmpFx[13] + tmpQ2[14]*tmpFx[17]; +tmpQ1[10] = + tmpQ2[10]*tmpFx[2] + tmpQ2[11]*tmpFx[6] + tmpQ2[12]*tmpFx[10] + tmpQ2[13]*tmpFx[14] + tmpQ2[14]*tmpFx[18]; +tmpQ1[11] = + tmpQ2[10]*tmpFx[3] + tmpQ2[11]*tmpFx[7] + tmpQ2[12]*tmpFx[11] + tmpQ2[13]*tmpFx[15] + tmpQ2[14]*tmpFx[19]; +tmpQ1[12] = + tmpQ2[15]*tmpFx[0] + tmpQ2[16]*tmpFx[4] + tmpQ2[17]*tmpFx[8] + tmpQ2[18]*tmpFx[12] + tmpQ2[19]*tmpFx[16]; +tmpQ1[13] = + tmpQ2[15]*tmpFx[1] + tmpQ2[16]*tmpFx[5] + tmpQ2[17]*tmpFx[9] + tmpQ2[18]*tmpFx[13] + tmpQ2[19]*tmpFx[17]; +tmpQ1[14] = + tmpQ2[15]*tmpFx[2] + tmpQ2[16]*tmpFx[6] + tmpQ2[17]*tmpFx[10] + tmpQ2[18]*tmpFx[14] + tmpQ2[19]*tmpFx[18]; +tmpQ1[15] = + tmpQ2[15]*tmpFx[3] + tmpQ2[16]*tmpFx[7] + tmpQ2[17]*tmpFx[11] + tmpQ2[18]*tmpFx[15] + tmpQ2[19]*tmpFx[19]; +} + +void acado_setObjR1R2( real_t* const tmpFu, real_t* const tmpR1, real_t* const tmpR2 ) +{ +tmpR2[0] = + tmpFu[0]; +tmpR2[1] = + tmpFu[1]; +tmpR2[2] = + tmpFu[2]; +tmpR2[3] = + tmpFu[3]; +tmpR2[4] = + tmpFu[4]; +tmpR1[0] = + tmpR2[0]*tmpFu[0] + tmpR2[1]*tmpFu[1] + tmpR2[2]*tmpFu[2] + tmpR2[3]*tmpFu[3] + tmpR2[4]*tmpFu[4]; +} + +void acado_setObjQN1QN2( real_t* const tmpFx, real_t* const tmpQN1, real_t* const tmpQN2 ) +{ +tmpQN2[0] = + tmpFx[0]; +tmpQN2[1] = + tmpFx[4]; +tmpQN2[2] = + tmpFx[8]; +tmpQN2[3] = + tmpFx[12]; +tmpQN2[4] = + tmpFx[1]; +tmpQN2[5] = + tmpFx[5]; +tmpQN2[6] = + tmpFx[9]; +tmpQN2[7] = + tmpFx[13]; +tmpQN2[8] = + tmpFx[2]; +tmpQN2[9] = + tmpFx[6]; +tmpQN2[10] = + tmpFx[10]; +tmpQN2[11] = + tmpFx[14]; +tmpQN2[12] = + tmpFx[3]; +tmpQN2[13] = + tmpFx[7]; +tmpQN2[14] = + tmpFx[11]; +tmpQN2[15] = + tmpFx[15]; +tmpQN1[0] = + tmpQN2[0]*tmpFx[0] + tmpQN2[1]*tmpFx[4] + tmpQN2[2]*tmpFx[8] + tmpQN2[3]*tmpFx[12]; +tmpQN1[1] = + tmpQN2[0]*tmpFx[1] + tmpQN2[1]*tmpFx[5] + tmpQN2[2]*tmpFx[9] + tmpQN2[3]*tmpFx[13]; +tmpQN1[2] = + tmpQN2[0]*tmpFx[2] + tmpQN2[1]*tmpFx[6] + tmpQN2[2]*tmpFx[10] + tmpQN2[3]*tmpFx[14]; +tmpQN1[3] = + tmpQN2[0]*tmpFx[3] + tmpQN2[1]*tmpFx[7] + tmpQN2[2]*tmpFx[11] + tmpQN2[3]*tmpFx[15]; +tmpQN1[4] = + tmpQN2[4]*tmpFx[0] + tmpQN2[5]*tmpFx[4] + tmpQN2[6]*tmpFx[8] + tmpQN2[7]*tmpFx[12]; +tmpQN1[5] = + tmpQN2[4]*tmpFx[1] + tmpQN2[5]*tmpFx[5] + tmpQN2[6]*tmpFx[9] + tmpQN2[7]*tmpFx[13]; +tmpQN1[6] = + tmpQN2[4]*tmpFx[2] + tmpQN2[5]*tmpFx[6] + tmpQN2[6]*tmpFx[10] + tmpQN2[7]*tmpFx[14]; +tmpQN1[7] = + tmpQN2[4]*tmpFx[3] + tmpQN2[5]*tmpFx[7] + tmpQN2[6]*tmpFx[11] + tmpQN2[7]*tmpFx[15]; +tmpQN1[8] = + tmpQN2[8]*tmpFx[0] + tmpQN2[9]*tmpFx[4] + tmpQN2[10]*tmpFx[8] + tmpQN2[11]*tmpFx[12]; +tmpQN1[9] = + tmpQN2[8]*tmpFx[1] + tmpQN2[9]*tmpFx[5] + tmpQN2[10]*tmpFx[9] + tmpQN2[11]*tmpFx[13]; +tmpQN1[10] = + tmpQN2[8]*tmpFx[2] + tmpQN2[9]*tmpFx[6] + tmpQN2[10]*tmpFx[10] + tmpQN2[11]*tmpFx[14]; +tmpQN1[11] = + tmpQN2[8]*tmpFx[3] + tmpQN2[9]*tmpFx[7] + tmpQN2[10]*tmpFx[11] + tmpQN2[11]*tmpFx[15]; +tmpQN1[12] = + tmpQN2[12]*tmpFx[0] + tmpQN2[13]*tmpFx[4] + tmpQN2[14]*tmpFx[8] + tmpQN2[15]*tmpFx[12]; +tmpQN1[13] = + tmpQN2[12]*tmpFx[1] + tmpQN2[13]*tmpFx[5] + tmpQN2[14]*tmpFx[9] + tmpQN2[15]*tmpFx[13]; +tmpQN1[14] = + tmpQN2[12]*tmpFx[2] + tmpQN2[13]*tmpFx[6] + tmpQN2[14]*tmpFx[10] + tmpQN2[15]*tmpFx[14]; +tmpQN1[15] = + tmpQN2[12]*tmpFx[3] + tmpQN2[13]*tmpFx[7] + tmpQN2[14]*tmpFx[11] + tmpQN2[15]*tmpFx[15]; +} + +void acado_evaluateObjective( ) +{ +int runObj; +for (runObj = 0; runObj < 50; ++runObj) +{ +acadoWorkspace.objValueIn[0] = acadoVariables.x[runObj * 4]; +acadoWorkspace.objValueIn[1] = acadoVariables.x[runObj * 4 + 1]; +acadoWorkspace.objValueIn[2] = acadoVariables.x[runObj * 4 + 2]; +acadoWorkspace.objValueIn[3] = acadoVariables.x[runObj * 4 + 3]; +acadoWorkspace.objValueIn[4] = acadoVariables.u[runObj]; +acadoWorkspace.objValueIn[5] = acadoVariables.od[runObj * 18]; +acadoWorkspace.objValueIn[6] = acadoVariables.od[runObj * 18 + 1]; +acadoWorkspace.objValueIn[7] = acadoVariables.od[runObj * 18 + 2]; +acadoWorkspace.objValueIn[8] = acadoVariables.od[runObj * 18 + 3]; +acadoWorkspace.objValueIn[9] = acadoVariables.od[runObj * 18 + 4]; +acadoWorkspace.objValueIn[10] = acadoVariables.od[runObj * 18 + 5]; +acadoWorkspace.objValueIn[11] = acadoVariables.od[runObj * 18 + 6]; +acadoWorkspace.objValueIn[12] = acadoVariables.od[runObj * 18 + 7]; +acadoWorkspace.objValueIn[13] = acadoVariables.od[runObj * 18 + 8]; +acadoWorkspace.objValueIn[14] = acadoVariables.od[runObj * 18 + 9]; +acadoWorkspace.objValueIn[15] = acadoVariables.od[runObj * 18 + 10]; +acadoWorkspace.objValueIn[16] = acadoVariables.od[runObj * 18 + 11]; +acadoWorkspace.objValueIn[17] = acadoVariables.od[runObj * 18 + 12]; +acadoWorkspace.objValueIn[18] = acadoVariables.od[runObj * 18 + 13]; +acadoWorkspace.objValueIn[19] = acadoVariables.od[runObj * 18 + 14]; +acadoWorkspace.objValueIn[20] = acadoVariables.od[runObj * 18 + 15]; +acadoWorkspace.objValueIn[21] = acadoVariables.od[runObj * 18 + 16]; +acadoWorkspace.objValueIn[22] = acadoVariables.od[runObj * 18 + 17]; + +acado_evaluateLSQ( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut ); +acadoWorkspace.Dy[runObj * 5] = acadoWorkspace.objValueOut[0]; +acadoWorkspace.Dy[runObj * 5 + 1] = acadoWorkspace.objValueOut[1]; +acadoWorkspace.Dy[runObj * 5 + 2] = acadoWorkspace.objValueOut[2]; +acadoWorkspace.Dy[runObj * 5 + 3] = acadoWorkspace.objValueOut[3]; +acadoWorkspace.Dy[runObj * 5 + 4] = acadoWorkspace.objValueOut[4]; + +acado_setObjQ1Q2( &(acadoWorkspace.objValueOut[ 5 ]), &(acadoWorkspace.Q1[ runObj * 16 ]), &(acadoWorkspace.Q2[ runObj * 20 ]) ); + +acado_setObjR1R2( &(acadoWorkspace.objValueOut[ 25 ]), &(acadoWorkspace.R1[ runObj ]), &(acadoWorkspace.R2[ runObj * 5 ]) ); + +} +acadoWorkspace.objValueIn[0] = acadoVariables.x[200]; +acadoWorkspace.objValueIn[1] = acadoVariables.x[201]; +acadoWorkspace.objValueIn[2] = acadoVariables.x[202]; +acadoWorkspace.objValueIn[3] = acadoVariables.x[203]; +acadoWorkspace.objValueIn[4] = acadoVariables.od[900]; +acadoWorkspace.objValueIn[5] = acadoVariables.od[901]; +acadoWorkspace.objValueIn[6] = acadoVariables.od[902]; +acadoWorkspace.objValueIn[7] = acadoVariables.od[903]; +acadoWorkspace.objValueIn[8] = acadoVariables.od[904]; +acadoWorkspace.objValueIn[9] = acadoVariables.od[905]; +acadoWorkspace.objValueIn[10] = acadoVariables.od[906]; +acadoWorkspace.objValueIn[11] = acadoVariables.od[907]; +acadoWorkspace.objValueIn[12] = acadoVariables.od[908]; +acadoWorkspace.objValueIn[13] = acadoVariables.od[909]; +acadoWorkspace.objValueIn[14] = acadoVariables.od[910]; +acadoWorkspace.objValueIn[15] = acadoVariables.od[911]; +acadoWorkspace.objValueIn[16] = acadoVariables.od[912]; +acadoWorkspace.objValueIn[17] = acadoVariables.od[913]; +acadoWorkspace.objValueIn[18] = acadoVariables.od[914]; +acadoWorkspace.objValueIn[19] = acadoVariables.od[915]; +acadoWorkspace.objValueIn[20] = acadoVariables.od[916]; +acadoWorkspace.objValueIn[21] = acadoVariables.od[917]; +acado_evaluateLSQEndTerm( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut ); + +acadoWorkspace.DyN[0] = acadoWorkspace.objValueOut[0]; +acadoWorkspace.DyN[1] = acadoWorkspace.objValueOut[1]; +acadoWorkspace.DyN[2] = acadoWorkspace.objValueOut[2]; +acadoWorkspace.DyN[3] = acadoWorkspace.objValueOut[3]; + +acado_setObjQN1QN2( &(acadoWorkspace.objValueOut[ 4 ]), acadoWorkspace.QN1, acadoWorkspace.QN2 ); + +} + +void acado_multGxd( real_t* const dOld, real_t* const Gx1, real_t* const dNew ) +{ +dNew[0] += + Gx1[0]*dOld[0] + Gx1[1]*dOld[1] + Gx1[2]*dOld[2] + Gx1[3]*dOld[3]; +dNew[1] += + Gx1[4]*dOld[0] + Gx1[5]*dOld[1] + Gx1[6]*dOld[2] + Gx1[7]*dOld[3]; +dNew[2] += + Gx1[8]*dOld[0] + Gx1[9]*dOld[1] + Gx1[10]*dOld[2] + Gx1[11]*dOld[3]; +dNew[3] += + Gx1[12]*dOld[0] + Gx1[13]*dOld[1] + Gx1[14]*dOld[2] + Gx1[15]*dOld[3]; +} + +void acado_moveGxT( real_t* const Gx1, real_t* const Gx2 ) +{ +Gx2[0] = Gx1[0]; +Gx2[1] = Gx1[1]; +Gx2[2] = Gx1[2]; +Gx2[3] = Gx1[3]; +Gx2[4] = Gx1[4]; +Gx2[5] = Gx1[5]; +Gx2[6] = Gx1[6]; +Gx2[7] = Gx1[7]; +Gx2[8] = Gx1[8]; +Gx2[9] = Gx1[9]; +Gx2[10] = Gx1[10]; +Gx2[11] = Gx1[11]; +Gx2[12] = Gx1[12]; +Gx2[13] = Gx1[13]; +Gx2[14] = Gx1[14]; +Gx2[15] = Gx1[15]; +} + +void acado_multGxGx( real_t* const Gx1, real_t* const Gx2, real_t* const Gx3 ) +{ +Gx3[0] = + Gx1[0]*Gx2[0] + Gx1[1]*Gx2[4] + Gx1[2]*Gx2[8] + Gx1[3]*Gx2[12]; +Gx3[1] = + Gx1[0]*Gx2[1] + Gx1[1]*Gx2[5] + Gx1[2]*Gx2[9] + Gx1[3]*Gx2[13]; +Gx3[2] = + Gx1[0]*Gx2[2] + Gx1[1]*Gx2[6] + Gx1[2]*Gx2[10] + Gx1[3]*Gx2[14]; +Gx3[3] = + Gx1[0]*Gx2[3] + Gx1[1]*Gx2[7] + Gx1[2]*Gx2[11] + Gx1[3]*Gx2[15]; +Gx3[4] = + Gx1[4]*Gx2[0] + Gx1[5]*Gx2[4] + Gx1[6]*Gx2[8] + Gx1[7]*Gx2[12]; +Gx3[5] = + Gx1[4]*Gx2[1] + Gx1[5]*Gx2[5] + Gx1[6]*Gx2[9] + Gx1[7]*Gx2[13]; +Gx3[6] = + Gx1[4]*Gx2[2] + Gx1[5]*Gx2[6] + Gx1[6]*Gx2[10] + Gx1[7]*Gx2[14]; +Gx3[7] = + Gx1[4]*Gx2[3] + Gx1[5]*Gx2[7] + Gx1[6]*Gx2[11] + Gx1[7]*Gx2[15]; +Gx3[8] = + Gx1[8]*Gx2[0] + Gx1[9]*Gx2[4] + Gx1[10]*Gx2[8] + Gx1[11]*Gx2[12]; +Gx3[9] = + Gx1[8]*Gx2[1] + Gx1[9]*Gx2[5] + Gx1[10]*Gx2[9] + Gx1[11]*Gx2[13]; +Gx3[10] = + Gx1[8]*Gx2[2] + Gx1[9]*Gx2[6] + Gx1[10]*Gx2[10] + Gx1[11]*Gx2[14]; +Gx3[11] = + Gx1[8]*Gx2[3] + Gx1[9]*Gx2[7] + Gx1[10]*Gx2[11] + Gx1[11]*Gx2[15]; +Gx3[12] = + Gx1[12]*Gx2[0] + Gx1[13]*Gx2[4] + Gx1[14]*Gx2[8] + Gx1[15]*Gx2[12]; +Gx3[13] = + Gx1[12]*Gx2[1] + Gx1[13]*Gx2[5] + Gx1[14]*Gx2[9] + Gx1[15]*Gx2[13]; +Gx3[14] = + Gx1[12]*Gx2[2] + Gx1[13]*Gx2[6] + Gx1[14]*Gx2[10] + Gx1[15]*Gx2[14]; +Gx3[15] = + Gx1[12]*Gx2[3] + Gx1[13]*Gx2[7] + Gx1[14]*Gx2[11] + Gx1[15]*Gx2[15]; +} + +void acado_multGxGu( real_t* const Gx1, real_t* const Gu1, real_t* const Gu2 ) +{ +Gu2[0] = + Gx1[0]*Gu1[0] + Gx1[1]*Gu1[1] + Gx1[2]*Gu1[2] + Gx1[3]*Gu1[3]; +Gu2[1] = + Gx1[4]*Gu1[0] + Gx1[5]*Gu1[1] + Gx1[6]*Gu1[2] + Gx1[7]*Gu1[3]; +Gu2[2] = + Gx1[8]*Gu1[0] + Gx1[9]*Gu1[1] + Gx1[10]*Gu1[2] + Gx1[11]*Gu1[3]; +Gu2[3] = + Gx1[12]*Gu1[0] + Gx1[13]*Gu1[1] + Gx1[14]*Gu1[2] + Gx1[15]*Gu1[3]; +} + +void acado_moveGuE( real_t* const Gu1, real_t* const Gu2 ) +{ +Gu2[0] = Gu1[0]; +Gu2[1] = Gu1[1]; +Gu2[2] = Gu1[2]; +Gu2[3] = Gu1[3]; +} + +void acado_setBlockH11( int iRow, int iCol, real_t* const Gu1, real_t* const Gu2 ) +{ +acadoWorkspace.H[(iRow * 54 + 216) + (iCol + 4)] += + Gu1[0]*Gu2[0] + Gu1[1]*Gu2[1] + Gu1[2]*Gu2[2] + Gu1[3]*Gu2[3]; +} + +void acado_setBlockH11_R1( int iRow, int iCol, real_t* const R11 ) +{ +acadoWorkspace.H[(iRow * 54 + 216) + (iCol + 4)] = R11[0]; +} + +void acado_zeroBlockH11( int iRow, int iCol ) +{ +acadoWorkspace.H[(iRow * 54 + 216) + (iCol + 4)] = 0.0000000000000000e+00; +} + +void acado_copyHTH( int iRow, int iCol ) +{ +acadoWorkspace.H[(iRow * 54 + 216) + (iCol + 4)] = acadoWorkspace.H[(iCol * 54 + 216) + (iRow + 4)]; +} + +void acado_multQ1d( real_t* const Gx1, real_t* const dOld, real_t* const dNew ) +{ +dNew[0] = + Gx1[0]*dOld[0] + Gx1[1]*dOld[1] + Gx1[2]*dOld[2] + Gx1[3]*dOld[3]; +dNew[1] = + Gx1[4]*dOld[0] + Gx1[5]*dOld[1] + Gx1[6]*dOld[2] + Gx1[7]*dOld[3]; +dNew[2] = + Gx1[8]*dOld[0] + Gx1[9]*dOld[1] + Gx1[10]*dOld[2] + Gx1[11]*dOld[3]; +dNew[3] = + Gx1[12]*dOld[0] + Gx1[13]*dOld[1] + Gx1[14]*dOld[2] + Gx1[15]*dOld[3]; +} + +void acado_multQN1d( real_t* const QN1, real_t* const dOld, real_t* const dNew ) +{ +dNew[0] = + acadoWorkspace.QN1[0]*dOld[0] + acadoWorkspace.QN1[1]*dOld[1] + acadoWorkspace.QN1[2]*dOld[2] + acadoWorkspace.QN1[3]*dOld[3]; +dNew[1] = + acadoWorkspace.QN1[4]*dOld[0] + acadoWorkspace.QN1[5]*dOld[1] + acadoWorkspace.QN1[6]*dOld[2] + acadoWorkspace.QN1[7]*dOld[3]; +dNew[2] = + acadoWorkspace.QN1[8]*dOld[0] + acadoWorkspace.QN1[9]*dOld[1] + acadoWorkspace.QN1[10]*dOld[2] + acadoWorkspace.QN1[11]*dOld[3]; +dNew[3] = + acadoWorkspace.QN1[12]*dOld[0] + acadoWorkspace.QN1[13]*dOld[1] + acadoWorkspace.QN1[14]*dOld[2] + acadoWorkspace.QN1[15]*dOld[3]; +} + +void acado_multRDy( real_t* const R2, real_t* const Dy1, real_t* const RDy1 ) +{ +RDy1[0] = + R2[0]*Dy1[0] + R2[1]*Dy1[1] + R2[2]*Dy1[2] + R2[3]*Dy1[3] + R2[4]*Dy1[4]; +} + +void acado_multQDy( real_t* const Q2, real_t* const Dy1, real_t* const QDy1 ) +{ +QDy1[0] = + Q2[0]*Dy1[0] + Q2[1]*Dy1[1] + Q2[2]*Dy1[2] + Q2[3]*Dy1[3] + Q2[4]*Dy1[4]; +QDy1[1] = + Q2[5]*Dy1[0] + Q2[6]*Dy1[1] + Q2[7]*Dy1[2] + Q2[8]*Dy1[3] + Q2[9]*Dy1[4]; +QDy1[2] = + Q2[10]*Dy1[0] + Q2[11]*Dy1[1] + Q2[12]*Dy1[2] + Q2[13]*Dy1[3] + Q2[14]*Dy1[4]; +QDy1[3] = + Q2[15]*Dy1[0] + Q2[16]*Dy1[1] + Q2[17]*Dy1[2] + Q2[18]*Dy1[3] + Q2[19]*Dy1[4]; +} + +void acado_multEQDy( real_t* const E1, real_t* const QDy1, real_t* const U1 ) +{ +U1[0] += + E1[0]*QDy1[0] + E1[1]*QDy1[1] + E1[2]*QDy1[2] + E1[3]*QDy1[3]; +} + +void acado_multQETGx( real_t* const E1, real_t* const Gx1, real_t* const H101 ) +{ +H101[0] += + E1[0]*Gx1[0] + E1[1]*Gx1[4] + E1[2]*Gx1[8] + E1[3]*Gx1[12]; +H101[1] += + E1[0]*Gx1[1] + E1[1]*Gx1[5] + E1[2]*Gx1[9] + E1[3]*Gx1[13]; +H101[2] += + E1[0]*Gx1[2] + E1[1]*Gx1[6] + E1[2]*Gx1[10] + E1[3]*Gx1[14]; +H101[3] += + E1[0]*Gx1[3] + E1[1]*Gx1[7] + E1[2]*Gx1[11] + E1[3]*Gx1[15]; +} + +void acado_zeroBlockH10( real_t* const H101 ) +{ +{ int lCopy; for (lCopy = 0; lCopy < 4; lCopy++) H101[ lCopy ] = 0; } +} + +void acado_multEDu( real_t* const E1, real_t* const U1, real_t* const dNew ) +{ +dNew[0] += + E1[0]*U1[0]; +dNew[1] += + E1[1]*U1[0]; +dNew[2] += + E1[2]*U1[0]; +dNew[3] += + E1[3]*U1[0]; +} + +void acado_zeroBlockH00( ) +{ +acadoWorkspace.H[0] = 0.0000000000000000e+00; +acadoWorkspace.H[1] = 0.0000000000000000e+00; +acadoWorkspace.H[2] = 0.0000000000000000e+00; +acadoWorkspace.H[3] = 0.0000000000000000e+00; +acadoWorkspace.H[54] = 0.0000000000000000e+00; +acadoWorkspace.H[55] = 0.0000000000000000e+00; +acadoWorkspace.H[56] = 0.0000000000000000e+00; +acadoWorkspace.H[57] = 0.0000000000000000e+00; +acadoWorkspace.H[108] = 0.0000000000000000e+00; +acadoWorkspace.H[109] = 0.0000000000000000e+00; +acadoWorkspace.H[110] = 0.0000000000000000e+00; +acadoWorkspace.H[111] = 0.0000000000000000e+00; +acadoWorkspace.H[162] = 0.0000000000000000e+00; +acadoWorkspace.H[163] = 0.0000000000000000e+00; +acadoWorkspace.H[164] = 0.0000000000000000e+00; +acadoWorkspace.H[165] = 0.0000000000000000e+00; +} + +void acado_multCTQC( real_t* const Gx1, real_t* const Gx2 ) +{ +acadoWorkspace.H[0] += + Gx1[0]*Gx2[0] + Gx1[4]*Gx2[4] + Gx1[8]*Gx2[8] + Gx1[12]*Gx2[12]; +acadoWorkspace.H[1] += + Gx1[0]*Gx2[1] + Gx1[4]*Gx2[5] + Gx1[8]*Gx2[9] + Gx1[12]*Gx2[13]; +acadoWorkspace.H[2] += + Gx1[0]*Gx2[2] + Gx1[4]*Gx2[6] + Gx1[8]*Gx2[10] + Gx1[12]*Gx2[14]; +acadoWorkspace.H[3] += + Gx1[0]*Gx2[3] + Gx1[4]*Gx2[7] + Gx1[8]*Gx2[11] + Gx1[12]*Gx2[15]; +acadoWorkspace.H[54] += + Gx1[1]*Gx2[0] + Gx1[5]*Gx2[4] + Gx1[9]*Gx2[8] + Gx1[13]*Gx2[12]; +acadoWorkspace.H[55] += + Gx1[1]*Gx2[1] + Gx1[5]*Gx2[5] + Gx1[9]*Gx2[9] + Gx1[13]*Gx2[13]; +acadoWorkspace.H[56] += + Gx1[1]*Gx2[2] + Gx1[5]*Gx2[6] + Gx1[9]*Gx2[10] + Gx1[13]*Gx2[14]; +acadoWorkspace.H[57] += + Gx1[1]*Gx2[3] + Gx1[5]*Gx2[7] + Gx1[9]*Gx2[11] + Gx1[13]*Gx2[15]; +acadoWorkspace.H[108] += + Gx1[2]*Gx2[0] + Gx1[6]*Gx2[4] + Gx1[10]*Gx2[8] + Gx1[14]*Gx2[12]; +acadoWorkspace.H[109] += + Gx1[2]*Gx2[1] + Gx1[6]*Gx2[5] + Gx1[10]*Gx2[9] + Gx1[14]*Gx2[13]; +acadoWorkspace.H[110] += + Gx1[2]*Gx2[2] + Gx1[6]*Gx2[6] + Gx1[10]*Gx2[10] + Gx1[14]*Gx2[14]; +acadoWorkspace.H[111] += + Gx1[2]*Gx2[3] + Gx1[6]*Gx2[7] + Gx1[10]*Gx2[11] + Gx1[14]*Gx2[15]; +acadoWorkspace.H[162] += + Gx1[3]*Gx2[0] + Gx1[7]*Gx2[4] + Gx1[11]*Gx2[8] + Gx1[15]*Gx2[12]; +acadoWorkspace.H[163] += + Gx1[3]*Gx2[1] + Gx1[7]*Gx2[5] + Gx1[11]*Gx2[9] + Gx1[15]*Gx2[13]; +acadoWorkspace.H[164] += + Gx1[3]*Gx2[2] + Gx1[7]*Gx2[6] + Gx1[11]*Gx2[10] + Gx1[15]*Gx2[14]; +acadoWorkspace.H[165] += + Gx1[3]*Gx2[3] + Gx1[7]*Gx2[7] + Gx1[11]*Gx2[11] + Gx1[15]*Gx2[15]; +} + +void acado_macCTSlx( real_t* const C0, real_t* const g0 ) +{ +g0[0] += 0.0; +; +g0[1] += 0.0; +; +g0[2] += 0.0; +; +g0[3] += 0.0; +; +} + +void acado_macETSlu( real_t* const E0, real_t* const g1 ) +{ +g1[0] += 0.0; +; +} + +void acado_condensePrep( ) +{ +int lRun1; +int lRun2; +int lRun3; +int lRun4; +int lRun5; +/** Row vector of size: 100 */ +static const int xBoundIndices[ 100 ] = +{ 6, 7, 10, 11, 14, 15, 18, 19, 22, 23, 26, 27, 30, 31, 34, 35, 38, 39, 42, 43, 46, 47, 50, 51, 54, 55, 58, 59, 62, 63, 66, 67, 70, 71, 74, 75, 78, 79, 82, 83, 86, 87, 90, 91, 94, 95, 98, 99, 102, 103, 106, 107, 110, 111, 114, 115, 118, 119, 122, 123, 126, 127, 130, 131, 134, 135, 138, 139, 142, 143, 146, 147, 150, 151, 154, 155, 158, 159, 162, 163, 166, 167, 170, 171, 174, 175, 178, 179, 182, 183, 186, 187, 190, 191, 194, 195, 198, 199, 202, 203 }; +acado_moveGuE( acadoWorkspace.evGu, acadoWorkspace.E ); +for (lRun1 = 1; lRun1 < 50; ++lRun1) +{ +acado_moveGxT( &(acadoWorkspace.evGx[ lRun1 * 16 ]), acadoWorkspace.T ); +acado_multGxd( &(acadoWorkspace.d[ lRun1 * 4-4 ]), &(acadoWorkspace.evGx[ lRun1 * 16 ]), &(acadoWorkspace.d[ lRun1 * 4 ]) ); +acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ lRun1 * 16-16 ]), &(acadoWorkspace.evGx[ lRun1 * 16 ]) ); +for (lRun2 = 0; lRun2 < lRun1; ++lRun2) +{ +lRun4 = (((lRun1) * (lRun1-1)) / (2)) + (lRun2); +lRun3 = (((lRun1 + 1) * (lRun1)) / (2)) + (lRun2); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ lRun4 * 4 ]), &(acadoWorkspace.E[ lRun3 * 4 ]) ); +} +lRun3 = (((lRun1 + 1) * (lRun1)) / (2)) + (lRun2); +acado_moveGuE( &(acadoWorkspace.evGu[ lRun1 * 4 ]), &(acadoWorkspace.E[ lRun3 * 4 ]) ); +} + +acado_multGxGx( &(acadoWorkspace.Q1[ 16 ]), acadoWorkspace.evGx, acadoWorkspace.QGx ); +acado_multGxGx( &(acadoWorkspace.Q1[ 32 ]), &(acadoWorkspace.evGx[ 16 ]), &(acadoWorkspace.QGx[ 16 ]) ); +acado_multGxGx( &(acadoWorkspace.Q1[ 48 ]), &(acadoWorkspace.evGx[ 32 ]), &(acadoWorkspace.QGx[ 32 ]) ); +acado_multGxGx( &(acadoWorkspace.Q1[ 64 ]), &(acadoWorkspace.evGx[ 48 ]), &(acadoWorkspace.QGx[ 48 ]) ); +acado_multGxGx( &(acadoWorkspace.Q1[ 80 ]), &(acadoWorkspace.evGx[ 64 ]), &(acadoWorkspace.QGx[ 64 ]) ); +acado_multGxGx( &(acadoWorkspace.Q1[ 96 ]), &(acadoWorkspace.evGx[ 80 ]), &(acadoWorkspace.QGx[ 80 ]) ); +acado_multGxGx( &(acadoWorkspace.Q1[ 112 ]), &(acadoWorkspace.evGx[ 96 ]), &(acadoWorkspace.QGx[ 96 ]) ); +acado_multGxGx( &(acadoWorkspace.Q1[ 128 ]), &(acadoWorkspace.evGx[ 112 ]), &(acadoWorkspace.QGx[ 112 ]) ); +acado_multGxGx( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.evGx[ 128 ]), &(acadoWorkspace.QGx[ 128 ]) ); +acado_multGxGx( &(acadoWorkspace.Q1[ 160 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.QGx[ 144 ]) ); +acado_multGxGx( &(acadoWorkspace.Q1[ 176 ]), &(acadoWorkspace.evGx[ 160 ]), &(acadoWorkspace.QGx[ 160 ]) ); +acado_multGxGx( &(acadoWorkspace.Q1[ 192 ]), &(acadoWorkspace.evGx[ 176 ]), &(acadoWorkspace.QGx[ 176 ]) ); +acado_multGxGx( &(acadoWorkspace.Q1[ 208 ]), &(acadoWorkspace.evGx[ 192 ]), &(acadoWorkspace.QGx[ 192 ]) ); +acado_multGxGx( &(acadoWorkspace.Q1[ 224 ]), &(acadoWorkspace.evGx[ 208 ]), &(acadoWorkspace.QGx[ 208 ]) ); +acado_multGxGx( &(acadoWorkspace.Q1[ 240 ]), &(acadoWorkspace.evGx[ 224 ]), &(acadoWorkspace.QGx[ 224 ]) ); +acado_multGxGx( &(acadoWorkspace.Q1[ 256 ]), &(acadoWorkspace.evGx[ 240 ]), &(acadoWorkspace.QGx[ 240 ]) ); +acado_multGxGx( &(acadoWorkspace.Q1[ 272 ]), &(acadoWorkspace.evGx[ 256 ]), &(acadoWorkspace.QGx[ 256 ]) ); +acado_multGxGx( &(acadoWorkspace.Q1[ 288 ]), &(acadoWorkspace.evGx[ 272 ]), &(acadoWorkspace.QGx[ 272 ]) ); +acado_multGxGx( &(acadoWorkspace.Q1[ 304 ]), &(acadoWorkspace.evGx[ 288 ]), &(acadoWorkspace.QGx[ 288 ]) ); +acado_multGxGx( &(acadoWorkspace.Q1[ 320 ]), &(acadoWorkspace.evGx[ 304 ]), &(acadoWorkspace.QGx[ 304 ]) ); +acado_multGxGx( &(acadoWorkspace.Q1[ 336 ]), &(acadoWorkspace.evGx[ 320 ]), &(acadoWorkspace.QGx[ 320 ]) ); +acado_multGxGx( &(acadoWorkspace.Q1[ 352 ]), &(acadoWorkspace.evGx[ 336 ]), &(acadoWorkspace.QGx[ 336 ]) ); +acado_multGxGx( &(acadoWorkspace.Q1[ 368 ]), &(acadoWorkspace.evGx[ 352 ]), &(acadoWorkspace.QGx[ 352 ]) ); +acado_multGxGx( &(acadoWorkspace.Q1[ 384 ]), &(acadoWorkspace.evGx[ 368 ]), &(acadoWorkspace.QGx[ 368 ]) ); +acado_multGxGx( &(acadoWorkspace.Q1[ 400 ]), &(acadoWorkspace.evGx[ 384 ]), &(acadoWorkspace.QGx[ 384 ]) ); +acado_multGxGx( &(acadoWorkspace.Q1[ 416 ]), &(acadoWorkspace.evGx[ 400 ]), &(acadoWorkspace.QGx[ 400 ]) ); +acado_multGxGx( &(acadoWorkspace.Q1[ 432 ]), &(acadoWorkspace.evGx[ 416 ]), &(acadoWorkspace.QGx[ 416 ]) ); +acado_multGxGx( &(acadoWorkspace.Q1[ 448 ]), &(acadoWorkspace.evGx[ 432 ]), &(acadoWorkspace.QGx[ 432 ]) ); +acado_multGxGx( &(acadoWorkspace.Q1[ 464 ]), &(acadoWorkspace.evGx[ 448 ]), &(acadoWorkspace.QGx[ 448 ]) ); +acado_multGxGx( &(acadoWorkspace.Q1[ 480 ]), &(acadoWorkspace.evGx[ 464 ]), &(acadoWorkspace.QGx[ 464 ]) ); +acado_multGxGx( &(acadoWorkspace.Q1[ 496 ]), &(acadoWorkspace.evGx[ 480 ]), &(acadoWorkspace.QGx[ 480 ]) ); +acado_multGxGx( &(acadoWorkspace.Q1[ 512 ]), &(acadoWorkspace.evGx[ 496 ]), &(acadoWorkspace.QGx[ 496 ]) ); +acado_multGxGx( &(acadoWorkspace.Q1[ 528 ]), &(acadoWorkspace.evGx[ 512 ]), &(acadoWorkspace.QGx[ 512 ]) ); +acado_multGxGx( &(acadoWorkspace.Q1[ 544 ]), &(acadoWorkspace.evGx[ 528 ]), &(acadoWorkspace.QGx[ 528 ]) ); +acado_multGxGx( &(acadoWorkspace.Q1[ 560 ]), &(acadoWorkspace.evGx[ 544 ]), &(acadoWorkspace.QGx[ 544 ]) ); +acado_multGxGx( &(acadoWorkspace.Q1[ 576 ]), &(acadoWorkspace.evGx[ 560 ]), &(acadoWorkspace.QGx[ 560 ]) ); +acado_multGxGx( &(acadoWorkspace.Q1[ 592 ]), &(acadoWorkspace.evGx[ 576 ]), &(acadoWorkspace.QGx[ 576 ]) ); +acado_multGxGx( &(acadoWorkspace.Q1[ 608 ]), &(acadoWorkspace.evGx[ 592 ]), &(acadoWorkspace.QGx[ 592 ]) ); +acado_multGxGx( &(acadoWorkspace.Q1[ 624 ]), &(acadoWorkspace.evGx[ 608 ]), &(acadoWorkspace.QGx[ 608 ]) ); +acado_multGxGx( &(acadoWorkspace.Q1[ 640 ]), &(acadoWorkspace.evGx[ 624 ]), &(acadoWorkspace.QGx[ 624 ]) ); +acado_multGxGx( &(acadoWorkspace.Q1[ 656 ]), &(acadoWorkspace.evGx[ 640 ]), &(acadoWorkspace.QGx[ 640 ]) ); +acado_multGxGx( &(acadoWorkspace.Q1[ 672 ]), &(acadoWorkspace.evGx[ 656 ]), &(acadoWorkspace.QGx[ 656 ]) ); +acado_multGxGx( &(acadoWorkspace.Q1[ 688 ]), &(acadoWorkspace.evGx[ 672 ]), &(acadoWorkspace.QGx[ 672 ]) ); +acado_multGxGx( &(acadoWorkspace.Q1[ 704 ]), &(acadoWorkspace.evGx[ 688 ]), &(acadoWorkspace.QGx[ 688 ]) ); +acado_multGxGx( &(acadoWorkspace.Q1[ 720 ]), &(acadoWorkspace.evGx[ 704 ]), &(acadoWorkspace.QGx[ 704 ]) ); +acado_multGxGx( &(acadoWorkspace.Q1[ 736 ]), &(acadoWorkspace.evGx[ 720 ]), &(acadoWorkspace.QGx[ 720 ]) ); +acado_multGxGx( &(acadoWorkspace.Q1[ 752 ]), &(acadoWorkspace.evGx[ 736 ]), &(acadoWorkspace.QGx[ 736 ]) ); +acado_multGxGx( &(acadoWorkspace.Q1[ 768 ]), &(acadoWorkspace.evGx[ 752 ]), &(acadoWorkspace.QGx[ 752 ]) ); +acado_multGxGx( &(acadoWorkspace.Q1[ 784 ]), &(acadoWorkspace.evGx[ 768 ]), &(acadoWorkspace.QGx[ 768 ]) ); +acado_multGxGx( acadoWorkspace.QN1, &(acadoWorkspace.evGx[ 784 ]), &(acadoWorkspace.QGx[ 784 ]) ); + +for (lRun1 = 0; lRun1 < 49; ++lRun1) +{ +for (lRun2 = 0; lRun2 < lRun1 + 1; ++lRun2) +{ +lRun3 = (((lRun1 + 1) * (lRun1)) / (2)) + (lRun2); +acado_multGxGu( &(acadoWorkspace.Q1[ lRun1 * 16 + 16 ]), &(acadoWorkspace.E[ lRun3 * 4 ]), &(acadoWorkspace.QE[ lRun3 * 4 ]) ); +} +} + +for (lRun2 = 0; lRun2 < lRun1 + 1; ++lRun2) +{ +lRun3 = (((lRun1 + 1) * (lRun1)) / (2)) + (lRun2); +acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ lRun3 * 4 ]), &(acadoWorkspace.QE[ lRun3 * 4 ]) ); +} + +acado_zeroBlockH00( ); +acado_multCTQC( acadoWorkspace.evGx, acadoWorkspace.QGx ); +acado_multCTQC( &(acadoWorkspace.evGx[ 16 ]), &(acadoWorkspace.QGx[ 16 ]) ); +acado_multCTQC( &(acadoWorkspace.evGx[ 32 ]), &(acadoWorkspace.QGx[ 32 ]) ); +acado_multCTQC( &(acadoWorkspace.evGx[ 48 ]), &(acadoWorkspace.QGx[ 48 ]) ); +acado_multCTQC( &(acadoWorkspace.evGx[ 64 ]), &(acadoWorkspace.QGx[ 64 ]) ); +acado_multCTQC( &(acadoWorkspace.evGx[ 80 ]), &(acadoWorkspace.QGx[ 80 ]) ); +acado_multCTQC( &(acadoWorkspace.evGx[ 96 ]), &(acadoWorkspace.QGx[ 96 ]) ); +acado_multCTQC( &(acadoWorkspace.evGx[ 112 ]), &(acadoWorkspace.QGx[ 112 ]) ); +acado_multCTQC( &(acadoWorkspace.evGx[ 128 ]), &(acadoWorkspace.QGx[ 128 ]) ); +acado_multCTQC( &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.QGx[ 144 ]) ); +acado_multCTQC( &(acadoWorkspace.evGx[ 160 ]), &(acadoWorkspace.QGx[ 160 ]) ); +acado_multCTQC( &(acadoWorkspace.evGx[ 176 ]), &(acadoWorkspace.QGx[ 176 ]) ); +acado_multCTQC( &(acadoWorkspace.evGx[ 192 ]), &(acadoWorkspace.QGx[ 192 ]) ); +acado_multCTQC( &(acadoWorkspace.evGx[ 208 ]), &(acadoWorkspace.QGx[ 208 ]) ); +acado_multCTQC( &(acadoWorkspace.evGx[ 224 ]), &(acadoWorkspace.QGx[ 224 ]) ); +acado_multCTQC( &(acadoWorkspace.evGx[ 240 ]), &(acadoWorkspace.QGx[ 240 ]) ); +acado_multCTQC( &(acadoWorkspace.evGx[ 256 ]), &(acadoWorkspace.QGx[ 256 ]) ); +acado_multCTQC( &(acadoWorkspace.evGx[ 272 ]), &(acadoWorkspace.QGx[ 272 ]) ); +acado_multCTQC( &(acadoWorkspace.evGx[ 288 ]), &(acadoWorkspace.QGx[ 288 ]) ); +acado_multCTQC( &(acadoWorkspace.evGx[ 304 ]), &(acadoWorkspace.QGx[ 304 ]) ); +acado_multCTQC( &(acadoWorkspace.evGx[ 320 ]), &(acadoWorkspace.QGx[ 320 ]) ); +acado_multCTQC( &(acadoWorkspace.evGx[ 336 ]), &(acadoWorkspace.QGx[ 336 ]) ); +acado_multCTQC( &(acadoWorkspace.evGx[ 352 ]), &(acadoWorkspace.QGx[ 352 ]) ); +acado_multCTQC( &(acadoWorkspace.evGx[ 368 ]), &(acadoWorkspace.QGx[ 368 ]) ); +acado_multCTQC( &(acadoWorkspace.evGx[ 384 ]), &(acadoWorkspace.QGx[ 384 ]) ); +acado_multCTQC( &(acadoWorkspace.evGx[ 400 ]), &(acadoWorkspace.QGx[ 400 ]) ); +acado_multCTQC( &(acadoWorkspace.evGx[ 416 ]), &(acadoWorkspace.QGx[ 416 ]) ); +acado_multCTQC( &(acadoWorkspace.evGx[ 432 ]), &(acadoWorkspace.QGx[ 432 ]) ); +acado_multCTQC( &(acadoWorkspace.evGx[ 448 ]), &(acadoWorkspace.QGx[ 448 ]) ); +acado_multCTQC( &(acadoWorkspace.evGx[ 464 ]), &(acadoWorkspace.QGx[ 464 ]) ); +acado_multCTQC( &(acadoWorkspace.evGx[ 480 ]), &(acadoWorkspace.QGx[ 480 ]) ); +acado_multCTQC( &(acadoWorkspace.evGx[ 496 ]), &(acadoWorkspace.QGx[ 496 ]) ); +acado_multCTQC( &(acadoWorkspace.evGx[ 512 ]), &(acadoWorkspace.QGx[ 512 ]) ); +acado_multCTQC( &(acadoWorkspace.evGx[ 528 ]), &(acadoWorkspace.QGx[ 528 ]) ); +acado_multCTQC( &(acadoWorkspace.evGx[ 544 ]), &(acadoWorkspace.QGx[ 544 ]) ); +acado_multCTQC( &(acadoWorkspace.evGx[ 560 ]), &(acadoWorkspace.QGx[ 560 ]) ); +acado_multCTQC( &(acadoWorkspace.evGx[ 576 ]), &(acadoWorkspace.QGx[ 576 ]) ); +acado_multCTQC( &(acadoWorkspace.evGx[ 592 ]), &(acadoWorkspace.QGx[ 592 ]) ); +acado_multCTQC( &(acadoWorkspace.evGx[ 608 ]), &(acadoWorkspace.QGx[ 608 ]) ); +acado_multCTQC( &(acadoWorkspace.evGx[ 624 ]), &(acadoWorkspace.QGx[ 624 ]) ); +acado_multCTQC( &(acadoWorkspace.evGx[ 640 ]), &(acadoWorkspace.QGx[ 640 ]) ); +acado_multCTQC( &(acadoWorkspace.evGx[ 656 ]), &(acadoWorkspace.QGx[ 656 ]) ); +acado_multCTQC( &(acadoWorkspace.evGx[ 672 ]), &(acadoWorkspace.QGx[ 672 ]) ); +acado_multCTQC( &(acadoWorkspace.evGx[ 688 ]), &(acadoWorkspace.QGx[ 688 ]) ); +acado_multCTQC( &(acadoWorkspace.evGx[ 704 ]), &(acadoWorkspace.QGx[ 704 ]) ); +acado_multCTQC( &(acadoWorkspace.evGx[ 720 ]), &(acadoWorkspace.QGx[ 720 ]) ); +acado_multCTQC( &(acadoWorkspace.evGx[ 736 ]), &(acadoWorkspace.QGx[ 736 ]) ); +acado_multCTQC( &(acadoWorkspace.evGx[ 752 ]), &(acadoWorkspace.QGx[ 752 ]) ); +acado_multCTQC( &(acadoWorkspace.evGx[ 768 ]), &(acadoWorkspace.QGx[ 768 ]) ); +acado_multCTQC( &(acadoWorkspace.evGx[ 784 ]), &(acadoWorkspace.QGx[ 784 ]) ); + +for (lRun1 = 0; lRun1 < 50; ++lRun1) +{ +acado_zeroBlockH10( &(acadoWorkspace.H10[ lRun1 * 4 ]) ); +for (lRun2 = lRun1; lRun2 < 50; ++lRun2) +{ +lRun3 = (((lRun2 + 1) * (lRun2)) / (2)) + (lRun1); +acado_multQETGx( &(acadoWorkspace.QE[ lRun3 * 4 ]), &(acadoWorkspace.evGx[ lRun2 * 16 ]), &(acadoWorkspace.H10[ lRun1 * 4 ]) ); +} +} + +for (lRun1 = 0;lRun1 < 4; ++lRun1) +for (lRun2 = 0;lRun2 < 50; ++lRun2) +acadoWorkspace.H[(lRun1 * 54) + (lRun2 + 4)] = acadoWorkspace.H10[(lRun2 * 4) + (lRun1)]; + +for (lRun1 = 0; lRun1 < 50; ++lRun1) +{ +acado_setBlockH11_R1( lRun1, lRun1, &(acadoWorkspace.R1[ lRun1 ]) ); +lRun2 = lRun1; +for (lRun3 = lRun1; lRun3 < 50; ++lRun3) +{ +lRun4 = (((lRun3 + 1) * (lRun3)) / (2)) + (lRun1); +lRun5 = (((lRun3 + 1) * (lRun3)) / (2)) + (lRun2); +acado_setBlockH11( lRun1, lRun2, &(acadoWorkspace.E[ lRun4 * 4 ]), &(acadoWorkspace.QE[ lRun5 * 4 ]) ); +} +for (lRun2 = lRun1 + 1; lRun2 < 50; ++lRun2) +{ +acado_zeroBlockH11( lRun1, lRun2 ); +for (lRun3 = lRun2; lRun3 < 50; ++lRun3) +{ +lRun4 = (((lRun3 + 1) * (lRun3)) / (2)) + (lRun1); +lRun5 = (((lRun3 + 1) * (lRun3)) / (2)) + (lRun2); +acado_setBlockH11( lRun1, lRun2, &(acadoWorkspace.E[ lRun4 * 4 ]), &(acadoWorkspace.QE[ lRun5 * 4 ]) ); +} +} +} + +for (lRun1 = 0; lRun1 < 50; ++lRun1) +{ +for (lRun2 = 0; lRun2 < lRun1; ++lRun2) +{ +acado_copyHTH( lRun1, lRun2 ); +} +} + +for (lRun1 = 0;lRun1 < 50; ++lRun1) +for (lRun2 = 0;lRun2 < 4; ++lRun2) +acadoWorkspace.H[(lRun1 * 54 + 216) + (lRun2)] = acadoWorkspace.H10[(lRun1 * 4) + (lRun2)]; + +acado_multQ1d( &(acadoWorkspace.Q1[ 16 ]), acadoWorkspace.d, acadoWorkspace.Qd ); +acado_multQ1d( &(acadoWorkspace.Q1[ 32 ]), &(acadoWorkspace.d[ 4 ]), &(acadoWorkspace.Qd[ 4 ]) ); +acado_multQ1d( &(acadoWorkspace.Q1[ 48 ]), &(acadoWorkspace.d[ 8 ]), &(acadoWorkspace.Qd[ 8 ]) ); +acado_multQ1d( &(acadoWorkspace.Q1[ 64 ]), &(acadoWorkspace.d[ 12 ]), &(acadoWorkspace.Qd[ 12 ]) ); +acado_multQ1d( &(acadoWorkspace.Q1[ 80 ]), &(acadoWorkspace.d[ 16 ]), &(acadoWorkspace.Qd[ 16 ]) ); +acado_multQ1d( &(acadoWorkspace.Q1[ 96 ]), &(acadoWorkspace.d[ 20 ]), &(acadoWorkspace.Qd[ 20 ]) ); +acado_multQ1d( &(acadoWorkspace.Q1[ 112 ]), &(acadoWorkspace.d[ 24 ]), &(acadoWorkspace.Qd[ 24 ]) ); +acado_multQ1d( &(acadoWorkspace.Q1[ 128 ]), &(acadoWorkspace.d[ 28 ]), &(acadoWorkspace.Qd[ 28 ]) ); +acado_multQ1d( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.d[ 32 ]), &(acadoWorkspace.Qd[ 32 ]) ); +acado_multQ1d( &(acadoWorkspace.Q1[ 160 ]), &(acadoWorkspace.d[ 36 ]), &(acadoWorkspace.Qd[ 36 ]) ); +acado_multQ1d( &(acadoWorkspace.Q1[ 176 ]), &(acadoWorkspace.d[ 40 ]), &(acadoWorkspace.Qd[ 40 ]) ); +acado_multQ1d( &(acadoWorkspace.Q1[ 192 ]), &(acadoWorkspace.d[ 44 ]), &(acadoWorkspace.Qd[ 44 ]) ); +acado_multQ1d( &(acadoWorkspace.Q1[ 208 ]), &(acadoWorkspace.d[ 48 ]), &(acadoWorkspace.Qd[ 48 ]) ); +acado_multQ1d( &(acadoWorkspace.Q1[ 224 ]), &(acadoWorkspace.d[ 52 ]), &(acadoWorkspace.Qd[ 52 ]) ); +acado_multQ1d( &(acadoWorkspace.Q1[ 240 ]), &(acadoWorkspace.d[ 56 ]), &(acadoWorkspace.Qd[ 56 ]) ); +acado_multQ1d( &(acadoWorkspace.Q1[ 256 ]), &(acadoWorkspace.d[ 60 ]), &(acadoWorkspace.Qd[ 60 ]) ); +acado_multQ1d( &(acadoWorkspace.Q1[ 272 ]), &(acadoWorkspace.d[ 64 ]), &(acadoWorkspace.Qd[ 64 ]) ); +acado_multQ1d( &(acadoWorkspace.Q1[ 288 ]), &(acadoWorkspace.d[ 68 ]), &(acadoWorkspace.Qd[ 68 ]) ); +acado_multQ1d( &(acadoWorkspace.Q1[ 304 ]), &(acadoWorkspace.d[ 72 ]), &(acadoWorkspace.Qd[ 72 ]) ); +acado_multQ1d( &(acadoWorkspace.Q1[ 320 ]), &(acadoWorkspace.d[ 76 ]), &(acadoWorkspace.Qd[ 76 ]) ); +acado_multQ1d( &(acadoWorkspace.Q1[ 336 ]), &(acadoWorkspace.d[ 80 ]), &(acadoWorkspace.Qd[ 80 ]) ); +acado_multQ1d( &(acadoWorkspace.Q1[ 352 ]), &(acadoWorkspace.d[ 84 ]), &(acadoWorkspace.Qd[ 84 ]) ); +acado_multQ1d( &(acadoWorkspace.Q1[ 368 ]), &(acadoWorkspace.d[ 88 ]), &(acadoWorkspace.Qd[ 88 ]) ); +acado_multQ1d( &(acadoWorkspace.Q1[ 384 ]), &(acadoWorkspace.d[ 92 ]), &(acadoWorkspace.Qd[ 92 ]) ); +acado_multQ1d( &(acadoWorkspace.Q1[ 400 ]), &(acadoWorkspace.d[ 96 ]), &(acadoWorkspace.Qd[ 96 ]) ); +acado_multQ1d( &(acadoWorkspace.Q1[ 416 ]), &(acadoWorkspace.d[ 100 ]), &(acadoWorkspace.Qd[ 100 ]) ); +acado_multQ1d( &(acadoWorkspace.Q1[ 432 ]), &(acadoWorkspace.d[ 104 ]), &(acadoWorkspace.Qd[ 104 ]) ); +acado_multQ1d( &(acadoWorkspace.Q1[ 448 ]), &(acadoWorkspace.d[ 108 ]), &(acadoWorkspace.Qd[ 108 ]) ); +acado_multQ1d( &(acadoWorkspace.Q1[ 464 ]), &(acadoWorkspace.d[ 112 ]), &(acadoWorkspace.Qd[ 112 ]) ); +acado_multQ1d( &(acadoWorkspace.Q1[ 480 ]), &(acadoWorkspace.d[ 116 ]), &(acadoWorkspace.Qd[ 116 ]) ); +acado_multQ1d( &(acadoWorkspace.Q1[ 496 ]), &(acadoWorkspace.d[ 120 ]), &(acadoWorkspace.Qd[ 120 ]) ); +acado_multQ1d( &(acadoWorkspace.Q1[ 512 ]), &(acadoWorkspace.d[ 124 ]), &(acadoWorkspace.Qd[ 124 ]) ); +acado_multQ1d( &(acadoWorkspace.Q1[ 528 ]), &(acadoWorkspace.d[ 128 ]), &(acadoWorkspace.Qd[ 128 ]) ); +acado_multQ1d( &(acadoWorkspace.Q1[ 544 ]), &(acadoWorkspace.d[ 132 ]), &(acadoWorkspace.Qd[ 132 ]) ); +acado_multQ1d( &(acadoWorkspace.Q1[ 560 ]), &(acadoWorkspace.d[ 136 ]), &(acadoWorkspace.Qd[ 136 ]) ); +acado_multQ1d( &(acadoWorkspace.Q1[ 576 ]), &(acadoWorkspace.d[ 140 ]), &(acadoWorkspace.Qd[ 140 ]) ); +acado_multQ1d( &(acadoWorkspace.Q1[ 592 ]), &(acadoWorkspace.d[ 144 ]), &(acadoWorkspace.Qd[ 144 ]) ); +acado_multQ1d( &(acadoWorkspace.Q1[ 608 ]), &(acadoWorkspace.d[ 148 ]), &(acadoWorkspace.Qd[ 148 ]) ); +acado_multQ1d( &(acadoWorkspace.Q1[ 624 ]), &(acadoWorkspace.d[ 152 ]), &(acadoWorkspace.Qd[ 152 ]) ); +acado_multQ1d( &(acadoWorkspace.Q1[ 640 ]), &(acadoWorkspace.d[ 156 ]), &(acadoWorkspace.Qd[ 156 ]) ); +acado_multQ1d( &(acadoWorkspace.Q1[ 656 ]), &(acadoWorkspace.d[ 160 ]), &(acadoWorkspace.Qd[ 160 ]) ); +acado_multQ1d( &(acadoWorkspace.Q1[ 672 ]), &(acadoWorkspace.d[ 164 ]), &(acadoWorkspace.Qd[ 164 ]) ); +acado_multQ1d( &(acadoWorkspace.Q1[ 688 ]), &(acadoWorkspace.d[ 168 ]), &(acadoWorkspace.Qd[ 168 ]) ); +acado_multQ1d( &(acadoWorkspace.Q1[ 704 ]), &(acadoWorkspace.d[ 172 ]), &(acadoWorkspace.Qd[ 172 ]) ); +acado_multQ1d( &(acadoWorkspace.Q1[ 720 ]), &(acadoWorkspace.d[ 176 ]), &(acadoWorkspace.Qd[ 176 ]) ); +acado_multQ1d( &(acadoWorkspace.Q1[ 736 ]), &(acadoWorkspace.d[ 180 ]), &(acadoWorkspace.Qd[ 180 ]) ); +acado_multQ1d( &(acadoWorkspace.Q1[ 752 ]), &(acadoWorkspace.d[ 184 ]), &(acadoWorkspace.Qd[ 184 ]) ); +acado_multQ1d( &(acadoWorkspace.Q1[ 768 ]), &(acadoWorkspace.d[ 188 ]), &(acadoWorkspace.Qd[ 188 ]) ); +acado_multQ1d( &(acadoWorkspace.Q1[ 784 ]), &(acadoWorkspace.d[ 192 ]), &(acadoWorkspace.Qd[ 192 ]) ); +acado_multQN1d( acadoWorkspace.QN1, &(acadoWorkspace.d[ 196 ]), &(acadoWorkspace.Qd[ 196 ]) ); + +acado_macCTSlx( acadoWorkspace.evGx, acadoWorkspace.g ); +acado_macCTSlx( &(acadoWorkspace.evGx[ 16 ]), acadoWorkspace.g ); +acado_macCTSlx( &(acadoWorkspace.evGx[ 32 ]), acadoWorkspace.g ); +acado_macCTSlx( &(acadoWorkspace.evGx[ 48 ]), acadoWorkspace.g ); +acado_macCTSlx( &(acadoWorkspace.evGx[ 64 ]), acadoWorkspace.g ); +acado_macCTSlx( &(acadoWorkspace.evGx[ 80 ]), acadoWorkspace.g ); +acado_macCTSlx( &(acadoWorkspace.evGx[ 96 ]), acadoWorkspace.g ); +acado_macCTSlx( &(acadoWorkspace.evGx[ 112 ]), acadoWorkspace.g ); +acado_macCTSlx( &(acadoWorkspace.evGx[ 128 ]), acadoWorkspace.g ); +acado_macCTSlx( &(acadoWorkspace.evGx[ 144 ]), acadoWorkspace.g ); +acado_macCTSlx( &(acadoWorkspace.evGx[ 160 ]), acadoWorkspace.g ); +acado_macCTSlx( &(acadoWorkspace.evGx[ 176 ]), acadoWorkspace.g ); +acado_macCTSlx( &(acadoWorkspace.evGx[ 192 ]), acadoWorkspace.g ); +acado_macCTSlx( &(acadoWorkspace.evGx[ 208 ]), acadoWorkspace.g ); +acado_macCTSlx( &(acadoWorkspace.evGx[ 224 ]), acadoWorkspace.g ); +acado_macCTSlx( &(acadoWorkspace.evGx[ 240 ]), acadoWorkspace.g ); +acado_macCTSlx( &(acadoWorkspace.evGx[ 256 ]), acadoWorkspace.g ); +acado_macCTSlx( &(acadoWorkspace.evGx[ 272 ]), acadoWorkspace.g ); +acado_macCTSlx( &(acadoWorkspace.evGx[ 288 ]), acadoWorkspace.g ); +acado_macCTSlx( &(acadoWorkspace.evGx[ 304 ]), acadoWorkspace.g ); +acado_macCTSlx( &(acadoWorkspace.evGx[ 320 ]), acadoWorkspace.g ); +acado_macCTSlx( &(acadoWorkspace.evGx[ 336 ]), acadoWorkspace.g ); +acado_macCTSlx( &(acadoWorkspace.evGx[ 352 ]), acadoWorkspace.g ); +acado_macCTSlx( &(acadoWorkspace.evGx[ 368 ]), acadoWorkspace.g ); +acado_macCTSlx( &(acadoWorkspace.evGx[ 384 ]), acadoWorkspace.g ); +acado_macCTSlx( &(acadoWorkspace.evGx[ 400 ]), acadoWorkspace.g ); +acado_macCTSlx( &(acadoWorkspace.evGx[ 416 ]), acadoWorkspace.g ); +acado_macCTSlx( &(acadoWorkspace.evGx[ 432 ]), acadoWorkspace.g ); +acado_macCTSlx( &(acadoWorkspace.evGx[ 448 ]), acadoWorkspace.g ); +acado_macCTSlx( &(acadoWorkspace.evGx[ 464 ]), acadoWorkspace.g ); +acado_macCTSlx( &(acadoWorkspace.evGx[ 480 ]), acadoWorkspace.g ); +acado_macCTSlx( &(acadoWorkspace.evGx[ 496 ]), acadoWorkspace.g ); +acado_macCTSlx( &(acadoWorkspace.evGx[ 512 ]), acadoWorkspace.g ); +acado_macCTSlx( &(acadoWorkspace.evGx[ 528 ]), acadoWorkspace.g ); +acado_macCTSlx( &(acadoWorkspace.evGx[ 544 ]), acadoWorkspace.g ); +acado_macCTSlx( &(acadoWorkspace.evGx[ 560 ]), acadoWorkspace.g ); +acado_macCTSlx( &(acadoWorkspace.evGx[ 576 ]), acadoWorkspace.g ); +acado_macCTSlx( &(acadoWorkspace.evGx[ 592 ]), acadoWorkspace.g ); +acado_macCTSlx( &(acadoWorkspace.evGx[ 608 ]), acadoWorkspace.g ); +acado_macCTSlx( &(acadoWorkspace.evGx[ 624 ]), acadoWorkspace.g ); +acado_macCTSlx( &(acadoWorkspace.evGx[ 640 ]), acadoWorkspace.g ); +acado_macCTSlx( &(acadoWorkspace.evGx[ 656 ]), acadoWorkspace.g ); +acado_macCTSlx( &(acadoWorkspace.evGx[ 672 ]), acadoWorkspace.g ); +acado_macCTSlx( &(acadoWorkspace.evGx[ 688 ]), acadoWorkspace.g ); +acado_macCTSlx( &(acadoWorkspace.evGx[ 704 ]), acadoWorkspace.g ); +acado_macCTSlx( &(acadoWorkspace.evGx[ 720 ]), acadoWorkspace.g ); +acado_macCTSlx( &(acadoWorkspace.evGx[ 736 ]), acadoWorkspace.g ); +acado_macCTSlx( &(acadoWorkspace.evGx[ 752 ]), acadoWorkspace.g ); +acado_macCTSlx( &(acadoWorkspace.evGx[ 768 ]), acadoWorkspace.g ); +acado_macCTSlx( &(acadoWorkspace.evGx[ 784 ]), acadoWorkspace.g ); +for (lRun1 = 0; lRun1 < 50; ++lRun1) +{ +for (lRun2 = lRun1; lRun2 < 50; ++lRun2) +{ +lRun3 = (((lRun2 + 1) * (lRun2)) / (2)) + (lRun1); +acado_macETSlu( &(acadoWorkspace.QE[ lRun3 * 4 ]), &(acadoWorkspace.g[ lRun1 + 4 ]) ); +} +} +acadoWorkspace.lb[4] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[0]; +acadoWorkspace.lb[5] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[1]; +acadoWorkspace.lb[6] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[2]; +acadoWorkspace.lb[7] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[3]; +acadoWorkspace.lb[8] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[4]; +acadoWorkspace.lb[9] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[5]; +acadoWorkspace.lb[10] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[6]; +acadoWorkspace.lb[11] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[7]; +acadoWorkspace.lb[12] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[8]; +acadoWorkspace.lb[13] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[9]; +acadoWorkspace.lb[14] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[10]; +acadoWorkspace.lb[15] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[11]; +acadoWorkspace.lb[16] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[12]; +acadoWorkspace.lb[17] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[13]; +acadoWorkspace.lb[18] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[14]; +acadoWorkspace.lb[19] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[15]; +acadoWorkspace.lb[20] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[16]; +acadoWorkspace.lb[21] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[17]; +acadoWorkspace.lb[22] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[18]; +acadoWorkspace.lb[23] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[19]; +acadoWorkspace.lb[24] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[20]; +acadoWorkspace.lb[25] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[21]; +acadoWorkspace.lb[26] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[22]; +acadoWorkspace.lb[27] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[23]; +acadoWorkspace.lb[28] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[24]; +acadoWorkspace.lb[29] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[25]; +acadoWorkspace.lb[30] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[26]; +acadoWorkspace.lb[31] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[27]; +acadoWorkspace.lb[32] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[28]; +acadoWorkspace.lb[33] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[29]; +acadoWorkspace.lb[34] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[30]; +acadoWorkspace.lb[35] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[31]; +acadoWorkspace.lb[36] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[32]; +acadoWorkspace.lb[37] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[33]; +acadoWorkspace.lb[38] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[34]; +acadoWorkspace.lb[39] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[35]; +acadoWorkspace.lb[40] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[36]; +acadoWorkspace.lb[41] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[37]; +acadoWorkspace.lb[42] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[38]; +acadoWorkspace.lb[43] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[39]; +acadoWorkspace.lb[44] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[40]; +acadoWorkspace.lb[45] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[41]; +acadoWorkspace.lb[46] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[42]; +acadoWorkspace.lb[47] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[43]; +acadoWorkspace.lb[48] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[44]; +acadoWorkspace.lb[49] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[45]; +acadoWorkspace.lb[50] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[46]; +acadoWorkspace.lb[51] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[47]; +acadoWorkspace.lb[52] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[48]; +acadoWorkspace.lb[53] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[49]; +acadoWorkspace.ub[4] = (real_t)1.0000000000000001e-01 - acadoVariables.u[0]; +acadoWorkspace.ub[5] = (real_t)1.0000000000000001e-01 - acadoVariables.u[1]; +acadoWorkspace.ub[6] = (real_t)1.0000000000000001e-01 - acadoVariables.u[2]; +acadoWorkspace.ub[7] = (real_t)1.0000000000000001e-01 - acadoVariables.u[3]; +acadoWorkspace.ub[8] = (real_t)1.0000000000000001e-01 - acadoVariables.u[4]; +acadoWorkspace.ub[9] = (real_t)1.0000000000000001e-01 - acadoVariables.u[5]; +acadoWorkspace.ub[10] = (real_t)1.0000000000000001e-01 - acadoVariables.u[6]; +acadoWorkspace.ub[11] = (real_t)1.0000000000000001e-01 - acadoVariables.u[7]; +acadoWorkspace.ub[12] = (real_t)1.0000000000000001e-01 - acadoVariables.u[8]; +acadoWorkspace.ub[13] = (real_t)1.0000000000000001e-01 - acadoVariables.u[9]; +acadoWorkspace.ub[14] = (real_t)1.0000000000000001e-01 - acadoVariables.u[10]; +acadoWorkspace.ub[15] = (real_t)1.0000000000000001e-01 - acadoVariables.u[11]; +acadoWorkspace.ub[16] = (real_t)1.0000000000000001e-01 - acadoVariables.u[12]; +acadoWorkspace.ub[17] = (real_t)1.0000000000000001e-01 - acadoVariables.u[13]; +acadoWorkspace.ub[18] = (real_t)1.0000000000000001e-01 - acadoVariables.u[14]; +acadoWorkspace.ub[19] = (real_t)1.0000000000000001e-01 - acadoVariables.u[15]; +acadoWorkspace.ub[20] = (real_t)1.0000000000000001e-01 - acadoVariables.u[16]; +acadoWorkspace.ub[21] = (real_t)1.0000000000000001e-01 - acadoVariables.u[17]; +acadoWorkspace.ub[22] = (real_t)1.0000000000000001e-01 - acadoVariables.u[18]; +acadoWorkspace.ub[23] = (real_t)1.0000000000000001e-01 - acadoVariables.u[19]; +acadoWorkspace.ub[24] = (real_t)1.0000000000000001e-01 - acadoVariables.u[20]; +acadoWorkspace.ub[25] = (real_t)1.0000000000000001e-01 - acadoVariables.u[21]; +acadoWorkspace.ub[26] = (real_t)1.0000000000000001e-01 - acadoVariables.u[22]; +acadoWorkspace.ub[27] = (real_t)1.0000000000000001e-01 - acadoVariables.u[23]; +acadoWorkspace.ub[28] = (real_t)1.0000000000000001e-01 - acadoVariables.u[24]; +acadoWorkspace.ub[29] = (real_t)1.0000000000000001e-01 - acadoVariables.u[25]; +acadoWorkspace.ub[30] = (real_t)1.0000000000000001e-01 - acadoVariables.u[26]; +acadoWorkspace.ub[31] = (real_t)1.0000000000000001e-01 - acadoVariables.u[27]; +acadoWorkspace.ub[32] = (real_t)1.0000000000000001e-01 - acadoVariables.u[28]; +acadoWorkspace.ub[33] = (real_t)1.0000000000000001e-01 - acadoVariables.u[29]; +acadoWorkspace.ub[34] = (real_t)1.0000000000000001e-01 - acadoVariables.u[30]; +acadoWorkspace.ub[35] = (real_t)1.0000000000000001e-01 - acadoVariables.u[31]; +acadoWorkspace.ub[36] = (real_t)1.0000000000000001e-01 - acadoVariables.u[32]; +acadoWorkspace.ub[37] = (real_t)1.0000000000000001e-01 - acadoVariables.u[33]; +acadoWorkspace.ub[38] = (real_t)1.0000000000000001e-01 - acadoVariables.u[34]; +acadoWorkspace.ub[39] = (real_t)1.0000000000000001e-01 - acadoVariables.u[35]; +acadoWorkspace.ub[40] = (real_t)1.0000000000000001e-01 - acadoVariables.u[36]; +acadoWorkspace.ub[41] = (real_t)1.0000000000000001e-01 - acadoVariables.u[37]; +acadoWorkspace.ub[42] = (real_t)1.0000000000000001e-01 - acadoVariables.u[38]; +acadoWorkspace.ub[43] = (real_t)1.0000000000000001e-01 - acadoVariables.u[39]; +acadoWorkspace.ub[44] = (real_t)1.0000000000000001e-01 - acadoVariables.u[40]; +acadoWorkspace.ub[45] = (real_t)1.0000000000000001e-01 - acadoVariables.u[41]; +acadoWorkspace.ub[46] = (real_t)1.0000000000000001e-01 - acadoVariables.u[42]; +acadoWorkspace.ub[47] = (real_t)1.0000000000000001e-01 - acadoVariables.u[43]; +acadoWorkspace.ub[48] = (real_t)1.0000000000000001e-01 - acadoVariables.u[44]; +acadoWorkspace.ub[49] = (real_t)1.0000000000000001e-01 - acadoVariables.u[45]; +acadoWorkspace.ub[50] = (real_t)1.0000000000000001e-01 - acadoVariables.u[46]; +acadoWorkspace.ub[51] = (real_t)1.0000000000000001e-01 - acadoVariables.u[47]; +acadoWorkspace.ub[52] = (real_t)1.0000000000000001e-01 - acadoVariables.u[48]; +acadoWorkspace.ub[53] = (real_t)1.0000000000000001e-01 - acadoVariables.u[49]; + +for (lRun1 = 0; lRun1 < 100; ++lRun1) +{ +lRun3 = xBoundIndices[ lRun1 ] - 4; +lRun4 = ((lRun3) / (4)) + (1); +acadoWorkspace.A[lRun1 * 54] = acadoWorkspace.evGx[lRun3 * 4]; +acadoWorkspace.A[lRun1 * 54 + 1] = acadoWorkspace.evGx[lRun3 * 4 + 1]; +acadoWorkspace.A[lRun1 * 54 + 2] = acadoWorkspace.evGx[lRun3 * 4 + 2]; +acadoWorkspace.A[lRun1 * 54 + 3] = acadoWorkspace.evGx[lRun3 * 4 + 3]; +for (lRun2 = 0; lRun2 < lRun4; ++lRun2) +{ +lRun5 = (((((lRun4) * (lRun4-1)) / (2)) + (lRun2)) * (4)) + ((lRun3) % (4)); +acadoWorkspace.A[(lRun1 * 54) + (lRun2 + 4)] = acadoWorkspace.E[lRun5]; +} +} + +} + +void acado_condenseFdb( ) +{ +int lRun1; +int lRun2; +int lRun3; +real_t tmp; + +acadoWorkspace.Dx0[0] = acadoVariables.x0[0] - acadoVariables.x[0]; +acadoWorkspace.Dx0[1] = acadoVariables.x0[1] - acadoVariables.x[1]; +acadoWorkspace.Dx0[2] = acadoVariables.x0[2] - acadoVariables.x[2]; +acadoWorkspace.Dx0[3] = acadoVariables.x0[3] - acadoVariables.x[3]; + +for (lRun2 = 0; lRun2 < 250; ++lRun2) +acadoWorkspace.Dy[lRun2] -= acadoVariables.y[lRun2]; + +acadoWorkspace.DyN[0] -= acadoVariables.yN[0]; +acadoWorkspace.DyN[1] -= acadoVariables.yN[1]; +acadoWorkspace.DyN[2] -= acadoVariables.yN[2]; +acadoWorkspace.DyN[3] -= acadoVariables.yN[3]; + +acado_multRDy( acadoWorkspace.R2, acadoWorkspace.Dy, &(acadoWorkspace.g[ 4 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 5 ]), &(acadoWorkspace.Dy[ 5 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 10 ]), &(acadoWorkspace.Dy[ 10 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 15 ]), &(acadoWorkspace.Dy[ 15 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 20 ]), &(acadoWorkspace.Dy[ 20 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 25 ]), &(acadoWorkspace.Dy[ 25 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 30 ]), &(acadoWorkspace.Dy[ 30 ]), &(acadoWorkspace.g[ 10 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 35 ]), &(acadoWorkspace.Dy[ 35 ]), &(acadoWorkspace.g[ 11 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 40 ]), &(acadoWorkspace.Dy[ 40 ]), &(acadoWorkspace.g[ 12 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 45 ]), &(acadoWorkspace.Dy[ 45 ]), &(acadoWorkspace.g[ 13 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 50 ]), &(acadoWorkspace.Dy[ 50 ]), &(acadoWorkspace.g[ 14 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 55 ]), &(acadoWorkspace.Dy[ 55 ]), &(acadoWorkspace.g[ 15 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 60 ]), &(acadoWorkspace.Dy[ 60 ]), &(acadoWorkspace.g[ 16 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 65 ]), &(acadoWorkspace.Dy[ 65 ]), &(acadoWorkspace.g[ 17 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 70 ]), &(acadoWorkspace.Dy[ 70 ]), &(acadoWorkspace.g[ 18 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 75 ]), &(acadoWorkspace.Dy[ 75 ]), &(acadoWorkspace.g[ 19 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 80 ]), &(acadoWorkspace.Dy[ 80 ]), &(acadoWorkspace.g[ 20 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 85 ]), &(acadoWorkspace.Dy[ 85 ]), &(acadoWorkspace.g[ 21 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 90 ]), &(acadoWorkspace.Dy[ 90 ]), &(acadoWorkspace.g[ 22 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 95 ]), &(acadoWorkspace.Dy[ 95 ]), &(acadoWorkspace.g[ 23 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 100 ]), &(acadoWorkspace.Dy[ 100 ]), &(acadoWorkspace.g[ 24 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 105 ]), &(acadoWorkspace.Dy[ 105 ]), &(acadoWorkspace.g[ 25 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 110 ]), &(acadoWorkspace.Dy[ 110 ]), &(acadoWorkspace.g[ 26 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 115 ]), &(acadoWorkspace.Dy[ 115 ]), &(acadoWorkspace.g[ 27 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 120 ]), &(acadoWorkspace.Dy[ 120 ]), &(acadoWorkspace.g[ 28 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 125 ]), &(acadoWorkspace.Dy[ 125 ]), &(acadoWorkspace.g[ 29 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 130 ]), &(acadoWorkspace.Dy[ 130 ]), &(acadoWorkspace.g[ 30 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 135 ]), &(acadoWorkspace.Dy[ 135 ]), &(acadoWorkspace.g[ 31 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 140 ]), &(acadoWorkspace.Dy[ 140 ]), &(acadoWorkspace.g[ 32 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 145 ]), &(acadoWorkspace.Dy[ 145 ]), &(acadoWorkspace.g[ 33 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 150 ]), &(acadoWorkspace.Dy[ 150 ]), &(acadoWorkspace.g[ 34 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 155 ]), &(acadoWorkspace.Dy[ 155 ]), &(acadoWorkspace.g[ 35 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 160 ]), &(acadoWorkspace.Dy[ 160 ]), &(acadoWorkspace.g[ 36 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 165 ]), &(acadoWorkspace.Dy[ 165 ]), &(acadoWorkspace.g[ 37 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 170 ]), &(acadoWorkspace.Dy[ 170 ]), &(acadoWorkspace.g[ 38 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 175 ]), &(acadoWorkspace.Dy[ 175 ]), &(acadoWorkspace.g[ 39 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 180 ]), &(acadoWorkspace.Dy[ 180 ]), &(acadoWorkspace.g[ 40 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 185 ]), &(acadoWorkspace.Dy[ 185 ]), &(acadoWorkspace.g[ 41 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 190 ]), &(acadoWorkspace.Dy[ 190 ]), &(acadoWorkspace.g[ 42 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 195 ]), &(acadoWorkspace.Dy[ 195 ]), &(acadoWorkspace.g[ 43 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 200 ]), &(acadoWorkspace.Dy[ 200 ]), &(acadoWorkspace.g[ 44 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 205 ]), &(acadoWorkspace.Dy[ 205 ]), &(acadoWorkspace.g[ 45 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 210 ]), &(acadoWorkspace.Dy[ 210 ]), &(acadoWorkspace.g[ 46 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 215 ]), &(acadoWorkspace.Dy[ 215 ]), &(acadoWorkspace.g[ 47 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 220 ]), &(acadoWorkspace.Dy[ 220 ]), &(acadoWorkspace.g[ 48 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 225 ]), &(acadoWorkspace.Dy[ 225 ]), &(acadoWorkspace.g[ 49 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 230 ]), &(acadoWorkspace.Dy[ 230 ]), &(acadoWorkspace.g[ 50 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 235 ]), &(acadoWorkspace.Dy[ 235 ]), &(acadoWorkspace.g[ 51 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 240 ]), &(acadoWorkspace.Dy[ 240 ]), &(acadoWorkspace.g[ 52 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 245 ]), &(acadoWorkspace.Dy[ 245 ]), &(acadoWorkspace.g[ 53 ]) ); + +acado_multQDy( acadoWorkspace.Q2, acadoWorkspace.Dy, acadoWorkspace.QDy ); +acado_multQDy( &(acadoWorkspace.Q2[ 20 ]), &(acadoWorkspace.Dy[ 5 ]), &(acadoWorkspace.QDy[ 4 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 40 ]), &(acadoWorkspace.Dy[ 10 ]), &(acadoWorkspace.QDy[ 8 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 60 ]), &(acadoWorkspace.Dy[ 15 ]), &(acadoWorkspace.QDy[ 12 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 80 ]), &(acadoWorkspace.Dy[ 20 ]), &(acadoWorkspace.QDy[ 16 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 100 ]), &(acadoWorkspace.Dy[ 25 ]), &(acadoWorkspace.QDy[ 20 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 120 ]), &(acadoWorkspace.Dy[ 30 ]), &(acadoWorkspace.QDy[ 24 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 140 ]), &(acadoWorkspace.Dy[ 35 ]), &(acadoWorkspace.QDy[ 28 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 160 ]), &(acadoWorkspace.Dy[ 40 ]), &(acadoWorkspace.QDy[ 32 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 180 ]), &(acadoWorkspace.Dy[ 45 ]), &(acadoWorkspace.QDy[ 36 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 200 ]), &(acadoWorkspace.Dy[ 50 ]), &(acadoWorkspace.QDy[ 40 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 220 ]), &(acadoWorkspace.Dy[ 55 ]), &(acadoWorkspace.QDy[ 44 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 240 ]), &(acadoWorkspace.Dy[ 60 ]), &(acadoWorkspace.QDy[ 48 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 260 ]), &(acadoWorkspace.Dy[ 65 ]), &(acadoWorkspace.QDy[ 52 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 280 ]), &(acadoWorkspace.Dy[ 70 ]), &(acadoWorkspace.QDy[ 56 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 300 ]), &(acadoWorkspace.Dy[ 75 ]), &(acadoWorkspace.QDy[ 60 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 320 ]), &(acadoWorkspace.Dy[ 80 ]), &(acadoWorkspace.QDy[ 64 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 340 ]), &(acadoWorkspace.Dy[ 85 ]), &(acadoWorkspace.QDy[ 68 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 360 ]), &(acadoWorkspace.Dy[ 90 ]), &(acadoWorkspace.QDy[ 72 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 380 ]), &(acadoWorkspace.Dy[ 95 ]), &(acadoWorkspace.QDy[ 76 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 400 ]), &(acadoWorkspace.Dy[ 100 ]), &(acadoWorkspace.QDy[ 80 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 420 ]), &(acadoWorkspace.Dy[ 105 ]), &(acadoWorkspace.QDy[ 84 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 440 ]), &(acadoWorkspace.Dy[ 110 ]), &(acadoWorkspace.QDy[ 88 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 460 ]), &(acadoWorkspace.Dy[ 115 ]), &(acadoWorkspace.QDy[ 92 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 480 ]), &(acadoWorkspace.Dy[ 120 ]), &(acadoWorkspace.QDy[ 96 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 500 ]), &(acadoWorkspace.Dy[ 125 ]), &(acadoWorkspace.QDy[ 100 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 520 ]), &(acadoWorkspace.Dy[ 130 ]), &(acadoWorkspace.QDy[ 104 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 540 ]), &(acadoWorkspace.Dy[ 135 ]), &(acadoWorkspace.QDy[ 108 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 560 ]), &(acadoWorkspace.Dy[ 140 ]), &(acadoWorkspace.QDy[ 112 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 580 ]), &(acadoWorkspace.Dy[ 145 ]), &(acadoWorkspace.QDy[ 116 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 600 ]), &(acadoWorkspace.Dy[ 150 ]), &(acadoWorkspace.QDy[ 120 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 620 ]), &(acadoWorkspace.Dy[ 155 ]), &(acadoWorkspace.QDy[ 124 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 640 ]), &(acadoWorkspace.Dy[ 160 ]), &(acadoWorkspace.QDy[ 128 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 660 ]), &(acadoWorkspace.Dy[ 165 ]), &(acadoWorkspace.QDy[ 132 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 680 ]), &(acadoWorkspace.Dy[ 170 ]), &(acadoWorkspace.QDy[ 136 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 700 ]), &(acadoWorkspace.Dy[ 175 ]), &(acadoWorkspace.QDy[ 140 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 720 ]), &(acadoWorkspace.Dy[ 180 ]), &(acadoWorkspace.QDy[ 144 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 740 ]), &(acadoWorkspace.Dy[ 185 ]), &(acadoWorkspace.QDy[ 148 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 760 ]), &(acadoWorkspace.Dy[ 190 ]), &(acadoWorkspace.QDy[ 152 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 780 ]), &(acadoWorkspace.Dy[ 195 ]), &(acadoWorkspace.QDy[ 156 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 800 ]), &(acadoWorkspace.Dy[ 200 ]), &(acadoWorkspace.QDy[ 160 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 820 ]), &(acadoWorkspace.Dy[ 205 ]), &(acadoWorkspace.QDy[ 164 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 840 ]), &(acadoWorkspace.Dy[ 210 ]), &(acadoWorkspace.QDy[ 168 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 860 ]), &(acadoWorkspace.Dy[ 215 ]), &(acadoWorkspace.QDy[ 172 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 880 ]), &(acadoWorkspace.Dy[ 220 ]), &(acadoWorkspace.QDy[ 176 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 900 ]), &(acadoWorkspace.Dy[ 225 ]), &(acadoWorkspace.QDy[ 180 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 920 ]), &(acadoWorkspace.Dy[ 230 ]), &(acadoWorkspace.QDy[ 184 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 940 ]), &(acadoWorkspace.Dy[ 235 ]), &(acadoWorkspace.QDy[ 188 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 960 ]), &(acadoWorkspace.Dy[ 240 ]), &(acadoWorkspace.QDy[ 192 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 980 ]), &(acadoWorkspace.Dy[ 245 ]), &(acadoWorkspace.QDy[ 196 ]) ); + +acadoWorkspace.QDy[200] = + acadoWorkspace.QN2[0]*acadoWorkspace.DyN[0] + acadoWorkspace.QN2[1]*acadoWorkspace.DyN[1] + acadoWorkspace.QN2[2]*acadoWorkspace.DyN[2] + acadoWorkspace.QN2[3]*acadoWorkspace.DyN[3]; +acadoWorkspace.QDy[201] = + acadoWorkspace.QN2[4]*acadoWorkspace.DyN[0] + acadoWorkspace.QN2[5]*acadoWorkspace.DyN[1] + acadoWorkspace.QN2[6]*acadoWorkspace.DyN[2] + acadoWorkspace.QN2[7]*acadoWorkspace.DyN[3]; +acadoWorkspace.QDy[202] = + acadoWorkspace.QN2[8]*acadoWorkspace.DyN[0] + acadoWorkspace.QN2[9]*acadoWorkspace.DyN[1] + acadoWorkspace.QN2[10]*acadoWorkspace.DyN[2] + acadoWorkspace.QN2[11]*acadoWorkspace.DyN[3]; +acadoWorkspace.QDy[203] = + acadoWorkspace.QN2[12]*acadoWorkspace.DyN[0] + acadoWorkspace.QN2[13]*acadoWorkspace.DyN[1] + acadoWorkspace.QN2[14]*acadoWorkspace.DyN[2] + acadoWorkspace.QN2[15]*acadoWorkspace.DyN[3]; + +for (lRun2 = 0; lRun2 < 200; ++lRun2) +acadoWorkspace.QDy[lRun2 + 4] += acadoWorkspace.Qd[lRun2]; + + +acadoWorkspace.g[0] = + acadoWorkspace.evGx[0]*acadoWorkspace.QDy[4] + acadoWorkspace.evGx[4]*acadoWorkspace.QDy[5] + acadoWorkspace.evGx[8]*acadoWorkspace.QDy[6] + acadoWorkspace.evGx[12]*acadoWorkspace.QDy[7] + acadoWorkspace.evGx[16]*acadoWorkspace.QDy[8] + acadoWorkspace.evGx[20]*acadoWorkspace.QDy[9] + acadoWorkspace.evGx[24]*acadoWorkspace.QDy[10] + acadoWorkspace.evGx[28]*acadoWorkspace.QDy[11] + acadoWorkspace.evGx[32]*acadoWorkspace.QDy[12] + acadoWorkspace.evGx[36]*acadoWorkspace.QDy[13] + acadoWorkspace.evGx[40]*acadoWorkspace.QDy[14] + acadoWorkspace.evGx[44]*acadoWorkspace.QDy[15] + acadoWorkspace.evGx[48]*acadoWorkspace.QDy[16] + acadoWorkspace.evGx[52]*acadoWorkspace.QDy[17] + acadoWorkspace.evGx[56]*acadoWorkspace.QDy[18] + acadoWorkspace.evGx[60]*acadoWorkspace.QDy[19] + acadoWorkspace.evGx[64]*acadoWorkspace.QDy[20] + acadoWorkspace.evGx[68]*acadoWorkspace.QDy[21] + acadoWorkspace.evGx[72]*acadoWorkspace.QDy[22] + acadoWorkspace.evGx[76]*acadoWorkspace.QDy[23] + acadoWorkspace.evGx[80]*acadoWorkspace.QDy[24] + acadoWorkspace.evGx[84]*acadoWorkspace.QDy[25] + acadoWorkspace.evGx[88]*acadoWorkspace.QDy[26] + acadoWorkspace.evGx[92]*acadoWorkspace.QDy[27] + acadoWorkspace.evGx[96]*acadoWorkspace.QDy[28] + acadoWorkspace.evGx[100]*acadoWorkspace.QDy[29] + acadoWorkspace.evGx[104]*acadoWorkspace.QDy[30] + acadoWorkspace.evGx[108]*acadoWorkspace.QDy[31] + acadoWorkspace.evGx[112]*acadoWorkspace.QDy[32] + acadoWorkspace.evGx[116]*acadoWorkspace.QDy[33] + acadoWorkspace.evGx[120]*acadoWorkspace.QDy[34] + acadoWorkspace.evGx[124]*acadoWorkspace.QDy[35] + acadoWorkspace.evGx[128]*acadoWorkspace.QDy[36] + acadoWorkspace.evGx[132]*acadoWorkspace.QDy[37] + acadoWorkspace.evGx[136]*acadoWorkspace.QDy[38] + acadoWorkspace.evGx[140]*acadoWorkspace.QDy[39] + acadoWorkspace.evGx[144]*acadoWorkspace.QDy[40] + acadoWorkspace.evGx[148]*acadoWorkspace.QDy[41] + acadoWorkspace.evGx[152]*acadoWorkspace.QDy[42] + acadoWorkspace.evGx[156]*acadoWorkspace.QDy[43] + acadoWorkspace.evGx[160]*acadoWorkspace.QDy[44] + acadoWorkspace.evGx[164]*acadoWorkspace.QDy[45] + acadoWorkspace.evGx[168]*acadoWorkspace.QDy[46] + acadoWorkspace.evGx[172]*acadoWorkspace.QDy[47] + acadoWorkspace.evGx[176]*acadoWorkspace.QDy[48] + acadoWorkspace.evGx[180]*acadoWorkspace.QDy[49] + acadoWorkspace.evGx[184]*acadoWorkspace.QDy[50] + acadoWorkspace.evGx[188]*acadoWorkspace.QDy[51] + acadoWorkspace.evGx[192]*acadoWorkspace.QDy[52] + acadoWorkspace.evGx[196]*acadoWorkspace.QDy[53] + acadoWorkspace.evGx[200]*acadoWorkspace.QDy[54] + acadoWorkspace.evGx[204]*acadoWorkspace.QDy[55] + acadoWorkspace.evGx[208]*acadoWorkspace.QDy[56] + acadoWorkspace.evGx[212]*acadoWorkspace.QDy[57] + acadoWorkspace.evGx[216]*acadoWorkspace.QDy[58] + acadoWorkspace.evGx[220]*acadoWorkspace.QDy[59] + acadoWorkspace.evGx[224]*acadoWorkspace.QDy[60] + acadoWorkspace.evGx[228]*acadoWorkspace.QDy[61] + acadoWorkspace.evGx[232]*acadoWorkspace.QDy[62] + acadoWorkspace.evGx[236]*acadoWorkspace.QDy[63] + acadoWorkspace.evGx[240]*acadoWorkspace.QDy[64] + acadoWorkspace.evGx[244]*acadoWorkspace.QDy[65] + acadoWorkspace.evGx[248]*acadoWorkspace.QDy[66] + acadoWorkspace.evGx[252]*acadoWorkspace.QDy[67] + acadoWorkspace.evGx[256]*acadoWorkspace.QDy[68] + acadoWorkspace.evGx[260]*acadoWorkspace.QDy[69] + acadoWorkspace.evGx[264]*acadoWorkspace.QDy[70] + acadoWorkspace.evGx[268]*acadoWorkspace.QDy[71] + acadoWorkspace.evGx[272]*acadoWorkspace.QDy[72] + acadoWorkspace.evGx[276]*acadoWorkspace.QDy[73] + acadoWorkspace.evGx[280]*acadoWorkspace.QDy[74] + acadoWorkspace.evGx[284]*acadoWorkspace.QDy[75] + acadoWorkspace.evGx[288]*acadoWorkspace.QDy[76] + acadoWorkspace.evGx[292]*acadoWorkspace.QDy[77] + acadoWorkspace.evGx[296]*acadoWorkspace.QDy[78] + acadoWorkspace.evGx[300]*acadoWorkspace.QDy[79] + acadoWorkspace.evGx[304]*acadoWorkspace.QDy[80] + acadoWorkspace.evGx[308]*acadoWorkspace.QDy[81] + acadoWorkspace.evGx[312]*acadoWorkspace.QDy[82] + acadoWorkspace.evGx[316]*acadoWorkspace.QDy[83] + acadoWorkspace.evGx[320]*acadoWorkspace.QDy[84] + acadoWorkspace.evGx[324]*acadoWorkspace.QDy[85] + acadoWorkspace.evGx[328]*acadoWorkspace.QDy[86] + acadoWorkspace.evGx[332]*acadoWorkspace.QDy[87] + acadoWorkspace.evGx[336]*acadoWorkspace.QDy[88] + acadoWorkspace.evGx[340]*acadoWorkspace.QDy[89] + acadoWorkspace.evGx[344]*acadoWorkspace.QDy[90] + acadoWorkspace.evGx[348]*acadoWorkspace.QDy[91] + acadoWorkspace.evGx[352]*acadoWorkspace.QDy[92] + acadoWorkspace.evGx[356]*acadoWorkspace.QDy[93] + acadoWorkspace.evGx[360]*acadoWorkspace.QDy[94] + acadoWorkspace.evGx[364]*acadoWorkspace.QDy[95] + acadoWorkspace.evGx[368]*acadoWorkspace.QDy[96] + acadoWorkspace.evGx[372]*acadoWorkspace.QDy[97] + acadoWorkspace.evGx[376]*acadoWorkspace.QDy[98] + acadoWorkspace.evGx[380]*acadoWorkspace.QDy[99] + acadoWorkspace.evGx[384]*acadoWorkspace.QDy[100] + acadoWorkspace.evGx[388]*acadoWorkspace.QDy[101] + acadoWorkspace.evGx[392]*acadoWorkspace.QDy[102] + acadoWorkspace.evGx[396]*acadoWorkspace.QDy[103] + acadoWorkspace.evGx[400]*acadoWorkspace.QDy[104] + acadoWorkspace.evGx[404]*acadoWorkspace.QDy[105] + acadoWorkspace.evGx[408]*acadoWorkspace.QDy[106] + acadoWorkspace.evGx[412]*acadoWorkspace.QDy[107] + acadoWorkspace.evGx[416]*acadoWorkspace.QDy[108] + acadoWorkspace.evGx[420]*acadoWorkspace.QDy[109] + acadoWorkspace.evGx[424]*acadoWorkspace.QDy[110] + acadoWorkspace.evGx[428]*acadoWorkspace.QDy[111] + acadoWorkspace.evGx[432]*acadoWorkspace.QDy[112] + acadoWorkspace.evGx[436]*acadoWorkspace.QDy[113] + acadoWorkspace.evGx[440]*acadoWorkspace.QDy[114] + acadoWorkspace.evGx[444]*acadoWorkspace.QDy[115] + acadoWorkspace.evGx[448]*acadoWorkspace.QDy[116] + acadoWorkspace.evGx[452]*acadoWorkspace.QDy[117] + acadoWorkspace.evGx[456]*acadoWorkspace.QDy[118] + acadoWorkspace.evGx[460]*acadoWorkspace.QDy[119] + acadoWorkspace.evGx[464]*acadoWorkspace.QDy[120] + acadoWorkspace.evGx[468]*acadoWorkspace.QDy[121] + acadoWorkspace.evGx[472]*acadoWorkspace.QDy[122] + acadoWorkspace.evGx[476]*acadoWorkspace.QDy[123] + acadoWorkspace.evGx[480]*acadoWorkspace.QDy[124] + acadoWorkspace.evGx[484]*acadoWorkspace.QDy[125] + acadoWorkspace.evGx[488]*acadoWorkspace.QDy[126] + acadoWorkspace.evGx[492]*acadoWorkspace.QDy[127] + acadoWorkspace.evGx[496]*acadoWorkspace.QDy[128] + acadoWorkspace.evGx[500]*acadoWorkspace.QDy[129] + acadoWorkspace.evGx[504]*acadoWorkspace.QDy[130] + acadoWorkspace.evGx[508]*acadoWorkspace.QDy[131] + acadoWorkspace.evGx[512]*acadoWorkspace.QDy[132] + acadoWorkspace.evGx[516]*acadoWorkspace.QDy[133] + acadoWorkspace.evGx[520]*acadoWorkspace.QDy[134] + acadoWorkspace.evGx[524]*acadoWorkspace.QDy[135] + acadoWorkspace.evGx[528]*acadoWorkspace.QDy[136] + acadoWorkspace.evGx[532]*acadoWorkspace.QDy[137] + acadoWorkspace.evGx[536]*acadoWorkspace.QDy[138] + acadoWorkspace.evGx[540]*acadoWorkspace.QDy[139] + acadoWorkspace.evGx[544]*acadoWorkspace.QDy[140] + acadoWorkspace.evGx[548]*acadoWorkspace.QDy[141] + acadoWorkspace.evGx[552]*acadoWorkspace.QDy[142] + acadoWorkspace.evGx[556]*acadoWorkspace.QDy[143] + acadoWorkspace.evGx[560]*acadoWorkspace.QDy[144] + acadoWorkspace.evGx[564]*acadoWorkspace.QDy[145] + acadoWorkspace.evGx[568]*acadoWorkspace.QDy[146] + acadoWorkspace.evGx[572]*acadoWorkspace.QDy[147] + acadoWorkspace.evGx[576]*acadoWorkspace.QDy[148] + acadoWorkspace.evGx[580]*acadoWorkspace.QDy[149] + acadoWorkspace.evGx[584]*acadoWorkspace.QDy[150] + acadoWorkspace.evGx[588]*acadoWorkspace.QDy[151] + acadoWorkspace.evGx[592]*acadoWorkspace.QDy[152] + acadoWorkspace.evGx[596]*acadoWorkspace.QDy[153] + acadoWorkspace.evGx[600]*acadoWorkspace.QDy[154] + acadoWorkspace.evGx[604]*acadoWorkspace.QDy[155] + acadoWorkspace.evGx[608]*acadoWorkspace.QDy[156] + acadoWorkspace.evGx[612]*acadoWorkspace.QDy[157] + acadoWorkspace.evGx[616]*acadoWorkspace.QDy[158] + acadoWorkspace.evGx[620]*acadoWorkspace.QDy[159] + acadoWorkspace.evGx[624]*acadoWorkspace.QDy[160] + acadoWorkspace.evGx[628]*acadoWorkspace.QDy[161] + acadoWorkspace.evGx[632]*acadoWorkspace.QDy[162] + acadoWorkspace.evGx[636]*acadoWorkspace.QDy[163] + acadoWorkspace.evGx[640]*acadoWorkspace.QDy[164] + acadoWorkspace.evGx[644]*acadoWorkspace.QDy[165] + acadoWorkspace.evGx[648]*acadoWorkspace.QDy[166] + acadoWorkspace.evGx[652]*acadoWorkspace.QDy[167] + acadoWorkspace.evGx[656]*acadoWorkspace.QDy[168] + acadoWorkspace.evGx[660]*acadoWorkspace.QDy[169] + acadoWorkspace.evGx[664]*acadoWorkspace.QDy[170] + acadoWorkspace.evGx[668]*acadoWorkspace.QDy[171] + acadoWorkspace.evGx[672]*acadoWorkspace.QDy[172] + acadoWorkspace.evGx[676]*acadoWorkspace.QDy[173] + acadoWorkspace.evGx[680]*acadoWorkspace.QDy[174] + acadoWorkspace.evGx[684]*acadoWorkspace.QDy[175] + acadoWorkspace.evGx[688]*acadoWorkspace.QDy[176] + acadoWorkspace.evGx[692]*acadoWorkspace.QDy[177] + acadoWorkspace.evGx[696]*acadoWorkspace.QDy[178] + acadoWorkspace.evGx[700]*acadoWorkspace.QDy[179] + acadoWorkspace.evGx[704]*acadoWorkspace.QDy[180] + acadoWorkspace.evGx[708]*acadoWorkspace.QDy[181] + acadoWorkspace.evGx[712]*acadoWorkspace.QDy[182] + acadoWorkspace.evGx[716]*acadoWorkspace.QDy[183] + acadoWorkspace.evGx[720]*acadoWorkspace.QDy[184] + acadoWorkspace.evGx[724]*acadoWorkspace.QDy[185] + acadoWorkspace.evGx[728]*acadoWorkspace.QDy[186] + acadoWorkspace.evGx[732]*acadoWorkspace.QDy[187] + acadoWorkspace.evGx[736]*acadoWorkspace.QDy[188] + acadoWorkspace.evGx[740]*acadoWorkspace.QDy[189] + acadoWorkspace.evGx[744]*acadoWorkspace.QDy[190] + acadoWorkspace.evGx[748]*acadoWorkspace.QDy[191] + acadoWorkspace.evGx[752]*acadoWorkspace.QDy[192] + acadoWorkspace.evGx[756]*acadoWorkspace.QDy[193] + acadoWorkspace.evGx[760]*acadoWorkspace.QDy[194] + acadoWorkspace.evGx[764]*acadoWorkspace.QDy[195] + acadoWorkspace.evGx[768]*acadoWorkspace.QDy[196] + acadoWorkspace.evGx[772]*acadoWorkspace.QDy[197] + acadoWorkspace.evGx[776]*acadoWorkspace.QDy[198] + acadoWorkspace.evGx[780]*acadoWorkspace.QDy[199] + acadoWorkspace.evGx[784]*acadoWorkspace.QDy[200] + acadoWorkspace.evGx[788]*acadoWorkspace.QDy[201] + acadoWorkspace.evGx[792]*acadoWorkspace.QDy[202] + acadoWorkspace.evGx[796]*acadoWorkspace.QDy[203]; +acadoWorkspace.g[1] = + acadoWorkspace.evGx[1]*acadoWorkspace.QDy[4] + acadoWorkspace.evGx[5]*acadoWorkspace.QDy[5] + acadoWorkspace.evGx[9]*acadoWorkspace.QDy[6] + acadoWorkspace.evGx[13]*acadoWorkspace.QDy[7] + acadoWorkspace.evGx[17]*acadoWorkspace.QDy[8] + acadoWorkspace.evGx[21]*acadoWorkspace.QDy[9] + acadoWorkspace.evGx[25]*acadoWorkspace.QDy[10] + acadoWorkspace.evGx[29]*acadoWorkspace.QDy[11] + acadoWorkspace.evGx[33]*acadoWorkspace.QDy[12] + acadoWorkspace.evGx[37]*acadoWorkspace.QDy[13] + acadoWorkspace.evGx[41]*acadoWorkspace.QDy[14] + acadoWorkspace.evGx[45]*acadoWorkspace.QDy[15] + acadoWorkspace.evGx[49]*acadoWorkspace.QDy[16] + acadoWorkspace.evGx[53]*acadoWorkspace.QDy[17] + acadoWorkspace.evGx[57]*acadoWorkspace.QDy[18] + acadoWorkspace.evGx[61]*acadoWorkspace.QDy[19] + acadoWorkspace.evGx[65]*acadoWorkspace.QDy[20] + acadoWorkspace.evGx[69]*acadoWorkspace.QDy[21] + acadoWorkspace.evGx[73]*acadoWorkspace.QDy[22] + acadoWorkspace.evGx[77]*acadoWorkspace.QDy[23] + acadoWorkspace.evGx[81]*acadoWorkspace.QDy[24] + acadoWorkspace.evGx[85]*acadoWorkspace.QDy[25] + acadoWorkspace.evGx[89]*acadoWorkspace.QDy[26] + acadoWorkspace.evGx[93]*acadoWorkspace.QDy[27] + acadoWorkspace.evGx[97]*acadoWorkspace.QDy[28] + acadoWorkspace.evGx[101]*acadoWorkspace.QDy[29] + acadoWorkspace.evGx[105]*acadoWorkspace.QDy[30] + acadoWorkspace.evGx[109]*acadoWorkspace.QDy[31] + acadoWorkspace.evGx[113]*acadoWorkspace.QDy[32] + acadoWorkspace.evGx[117]*acadoWorkspace.QDy[33] + acadoWorkspace.evGx[121]*acadoWorkspace.QDy[34] + acadoWorkspace.evGx[125]*acadoWorkspace.QDy[35] + acadoWorkspace.evGx[129]*acadoWorkspace.QDy[36] + acadoWorkspace.evGx[133]*acadoWorkspace.QDy[37] + acadoWorkspace.evGx[137]*acadoWorkspace.QDy[38] + acadoWorkspace.evGx[141]*acadoWorkspace.QDy[39] + acadoWorkspace.evGx[145]*acadoWorkspace.QDy[40] + acadoWorkspace.evGx[149]*acadoWorkspace.QDy[41] + acadoWorkspace.evGx[153]*acadoWorkspace.QDy[42] + acadoWorkspace.evGx[157]*acadoWorkspace.QDy[43] + acadoWorkspace.evGx[161]*acadoWorkspace.QDy[44] + acadoWorkspace.evGx[165]*acadoWorkspace.QDy[45] + acadoWorkspace.evGx[169]*acadoWorkspace.QDy[46] + acadoWorkspace.evGx[173]*acadoWorkspace.QDy[47] + acadoWorkspace.evGx[177]*acadoWorkspace.QDy[48] + acadoWorkspace.evGx[181]*acadoWorkspace.QDy[49] + acadoWorkspace.evGx[185]*acadoWorkspace.QDy[50] + acadoWorkspace.evGx[189]*acadoWorkspace.QDy[51] + acadoWorkspace.evGx[193]*acadoWorkspace.QDy[52] + acadoWorkspace.evGx[197]*acadoWorkspace.QDy[53] + acadoWorkspace.evGx[201]*acadoWorkspace.QDy[54] + acadoWorkspace.evGx[205]*acadoWorkspace.QDy[55] + acadoWorkspace.evGx[209]*acadoWorkspace.QDy[56] + acadoWorkspace.evGx[213]*acadoWorkspace.QDy[57] + acadoWorkspace.evGx[217]*acadoWorkspace.QDy[58] + acadoWorkspace.evGx[221]*acadoWorkspace.QDy[59] + acadoWorkspace.evGx[225]*acadoWorkspace.QDy[60] + acadoWorkspace.evGx[229]*acadoWorkspace.QDy[61] + acadoWorkspace.evGx[233]*acadoWorkspace.QDy[62] + acadoWorkspace.evGx[237]*acadoWorkspace.QDy[63] + acadoWorkspace.evGx[241]*acadoWorkspace.QDy[64] + acadoWorkspace.evGx[245]*acadoWorkspace.QDy[65] + acadoWorkspace.evGx[249]*acadoWorkspace.QDy[66] + acadoWorkspace.evGx[253]*acadoWorkspace.QDy[67] + acadoWorkspace.evGx[257]*acadoWorkspace.QDy[68] + acadoWorkspace.evGx[261]*acadoWorkspace.QDy[69] + acadoWorkspace.evGx[265]*acadoWorkspace.QDy[70] + acadoWorkspace.evGx[269]*acadoWorkspace.QDy[71] + acadoWorkspace.evGx[273]*acadoWorkspace.QDy[72] + acadoWorkspace.evGx[277]*acadoWorkspace.QDy[73] + acadoWorkspace.evGx[281]*acadoWorkspace.QDy[74] + acadoWorkspace.evGx[285]*acadoWorkspace.QDy[75] + acadoWorkspace.evGx[289]*acadoWorkspace.QDy[76] + acadoWorkspace.evGx[293]*acadoWorkspace.QDy[77] + acadoWorkspace.evGx[297]*acadoWorkspace.QDy[78] + acadoWorkspace.evGx[301]*acadoWorkspace.QDy[79] + acadoWorkspace.evGx[305]*acadoWorkspace.QDy[80] + acadoWorkspace.evGx[309]*acadoWorkspace.QDy[81] + acadoWorkspace.evGx[313]*acadoWorkspace.QDy[82] + acadoWorkspace.evGx[317]*acadoWorkspace.QDy[83] + acadoWorkspace.evGx[321]*acadoWorkspace.QDy[84] + acadoWorkspace.evGx[325]*acadoWorkspace.QDy[85] + acadoWorkspace.evGx[329]*acadoWorkspace.QDy[86] + acadoWorkspace.evGx[333]*acadoWorkspace.QDy[87] + acadoWorkspace.evGx[337]*acadoWorkspace.QDy[88] + acadoWorkspace.evGx[341]*acadoWorkspace.QDy[89] + acadoWorkspace.evGx[345]*acadoWorkspace.QDy[90] + acadoWorkspace.evGx[349]*acadoWorkspace.QDy[91] + acadoWorkspace.evGx[353]*acadoWorkspace.QDy[92] + acadoWorkspace.evGx[357]*acadoWorkspace.QDy[93] + acadoWorkspace.evGx[361]*acadoWorkspace.QDy[94] + acadoWorkspace.evGx[365]*acadoWorkspace.QDy[95] + acadoWorkspace.evGx[369]*acadoWorkspace.QDy[96] + acadoWorkspace.evGx[373]*acadoWorkspace.QDy[97] + acadoWorkspace.evGx[377]*acadoWorkspace.QDy[98] + acadoWorkspace.evGx[381]*acadoWorkspace.QDy[99] + acadoWorkspace.evGx[385]*acadoWorkspace.QDy[100] + acadoWorkspace.evGx[389]*acadoWorkspace.QDy[101] + acadoWorkspace.evGx[393]*acadoWorkspace.QDy[102] + acadoWorkspace.evGx[397]*acadoWorkspace.QDy[103] + acadoWorkspace.evGx[401]*acadoWorkspace.QDy[104] + acadoWorkspace.evGx[405]*acadoWorkspace.QDy[105] + acadoWorkspace.evGx[409]*acadoWorkspace.QDy[106] + acadoWorkspace.evGx[413]*acadoWorkspace.QDy[107] + acadoWorkspace.evGx[417]*acadoWorkspace.QDy[108] + acadoWorkspace.evGx[421]*acadoWorkspace.QDy[109] + acadoWorkspace.evGx[425]*acadoWorkspace.QDy[110] + acadoWorkspace.evGx[429]*acadoWorkspace.QDy[111] + acadoWorkspace.evGx[433]*acadoWorkspace.QDy[112] + acadoWorkspace.evGx[437]*acadoWorkspace.QDy[113] + acadoWorkspace.evGx[441]*acadoWorkspace.QDy[114] + acadoWorkspace.evGx[445]*acadoWorkspace.QDy[115] + acadoWorkspace.evGx[449]*acadoWorkspace.QDy[116] + acadoWorkspace.evGx[453]*acadoWorkspace.QDy[117] + acadoWorkspace.evGx[457]*acadoWorkspace.QDy[118] + acadoWorkspace.evGx[461]*acadoWorkspace.QDy[119] + acadoWorkspace.evGx[465]*acadoWorkspace.QDy[120] + acadoWorkspace.evGx[469]*acadoWorkspace.QDy[121] + acadoWorkspace.evGx[473]*acadoWorkspace.QDy[122] + acadoWorkspace.evGx[477]*acadoWorkspace.QDy[123] + acadoWorkspace.evGx[481]*acadoWorkspace.QDy[124] + acadoWorkspace.evGx[485]*acadoWorkspace.QDy[125] + acadoWorkspace.evGx[489]*acadoWorkspace.QDy[126] + acadoWorkspace.evGx[493]*acadoWorkspace.QDy[127] + acadoWorkspace.evGx[497]*acadoWorkspace.QDy[128] + acadoWorkspace.evGx[501]*acadoWorkspace.QDy[129] + acadoWorkspace.evGx[505]*acadoWorkspace.QDy[130] + acadoWorkspace.evGx[509]*acadoWorkspace.QDy[131] + acadoWorkspace.evGx[513]*acadoWorkspace.QDy[132] + acadoWorkspace.evGx[517]*acadoWorkspace.QDy[133] + acadoWorkspace.evGx[521]*acadoWorkspace.QDy[134] + acadoWorkspace.evGx[525]*acadoWorkspace.QDy[135] + acadoWorkspace.evGx[529]*acadoWorkspace.QDy[136] + acadoWorkspace.evGx[533]*acadoWorkspace.QDy[137] + acadoWorkspace.evGx[537]*acadoWorkspace.QDy[138] + acadoWorkspace.evGx[541]*acadoWorkspace.QDy[139] + acadoWorkspace.evGx[545]*acadoWorkspace.QDy[140] + acadoWorkspace.evGx[549]*acadoWorkspace.QDy[141] + acadoWorkspace.evGx[553]*acadoWorkspace.QDy[142] + acadoWorkspace.evGx[557]*acadoWorkspace.QDy[143] + acadoWorkspace.evGx[561]*acadoWorkspace.QDy[144] + acadoWorkspace.evGx[565]*acadoWorkspace.QDy[145] + acadoWorkspace.evGx[569]*acadoWorkspace.QDy[146] + acadoWorkspace.evGx[573]*acadoWorkspace.QDy[147] + acadoWorkspace.evGx[577]*acadoWorkspace.QDy[148] + acadoWorkspace.evGx[581]*acadoWorkspace.QDy[149] + acadoWorkspace.evGx[585]*acadoWorkspace.QDy[150] + acadoWorkspace.evGx[589]*acadoWorkspace.QDy[151] + acadoWorkspace.evGx[593]*acadoWorkspace.QDy[152] + acadoWorkspace.evGx[597]*acadoWorkspace.QDy[153] + acadoWorkspace.evGx[601]*acadoWorkspace.QDy[154] + acadoWorkspace.evGx[605]*acadoWorkspace.QDy[155] + acadoWorkspace.evGx[609]*acadoWorkspace.QDy[156] + acadoWorkspace.evGx[613]*acadoWorkspace.QDy[157] + acadoWorkspace.evGx[617]*acadoWorkspace.QDy[158] + acadoWorkspace.evGx[621]*acadoWorkspace.QDy[159] + acadoWorkspace.evGx[625]*acadoWorkspace.QDy[160] + acadoWorkspace.evGx[629]*acadoWorkspace.QDy[161] + acadoWorkspace.evGx[633]*acadoWorkspace.QDy[162] + acadoWorkspace.evGx[637]*acadoWorkspace.QDy[163] + acadoWorkspace.evGx[641]*acadoWorkspace.QDy[164] + acadoWorkspace.evGx[645]*acadoWorkspace.QDy[165] + acadoWorkspace.evGx[649]*acadoWorkspace.QDy[166] + acadoWorkspace.evGx[653]*acadoWorkspace.QDy[167] + acadoWorkspace.evGx[657]*acadoWorkspace.QDy[168] + acadoWorkspace.evGx[661]*acadoWorkspace.QDy[169] + acadoWorkspace.evGx[665]*acadoWorkspace.QDy[170] + acadoWorkspace.evGx[669]*acadoWorkspace.QDy[171] + acadoWorkspace.evGx[673]*acadoWorkspace.QDy[172] + acadoWorkspace.evGx[677]*acadoWorkspace.QDy[173] + acadoWorkspace.evGx[681]*acadoWorkspace.QDy[174] + acadoWorkspace.evGx[685]*acadoWorkspace.QDy[175] + acadoWorkspace.evGx[689]*acadoWorkspace.QDy[176] + acadoWorkspace.evGx[693]*acadoWorkspace.QDy[177] + acadoWorkspace.evGx[697]*acadoWorkspace.QDy[178] + acadoWorkspace.evGx[701]*acadoWorkspace.QDy[179] + acadoWorkspace.evGx[705]*acadoWorkspace.QDy[180] + acadoWorkspace.evGx[709]*acadoWorkspace.QDy[181] + acadoWorkspace.evGx[713]*acadoWorkspace.QDy[182] + acadoWorkspace.evGx[717]*acadoWorkspace.QDy[183] + acadoWorkspace.evGx[721]*acadoWorkspace.QDy[184] + acadoWorkspace.evGx[725]*acadoWorkspace.QDy[185] + acadoWorkspace.evGx[729]*acadoWorkspace.QDy[186] + acadoWorkspace.evGx[733]*acadoWorkspace.QDy[187] + acadoWorkspace.evGx[737]*acadoWorkspace.QDy[188] + acadoWorkspace.evGx[741]*acadoWorkspace.QDy[189] + acadoWorkspace.evGx[745]*acadoWorkspace.QDy[190] + acadoWorkspace.evGx[749]*acadoWorkspace.QDy[191] + acadoWorkspace.evGx[753]*acadoWorkspace.QDy[192] + acadoWorkspace.evGx[757]*acadoWorkspace.QDy[193] + acadoWorkspace.evGx[761]*acadoWorkspace.QDy[194] + acadoWorkspace.evGx[765]*acadoWorkspace.QDy[195] + acadoWorkspace.evGx[769]*acadoWorkspace.QDy[196] + acadoWorkspace.evGx[773]*acadoWorkspace.QDy[197] + acadoWorkspace.evGx[777]*acadoWorkspace.QDy[198] + acadoWorkspace.evGx[781]*acadoWorkspace.QDy[199] + acadoWorkspace.evGx[785]*acadoWorkspace.QDy[200] + acadoWorkspace.evGx[789]*acadoWorkspace.QDy[201] + acadoWorkspace.evGx[793]*acadoWorkspace.QDy[202] + acadoWorkspace.evGx[797]*acadoWorkspace.QDy[203]; +acadoWorkspace.g[2] = + acadoWorkspace.evGx[2]*acadoWorkspace.QDy[4] + acadoWorkspace.evGx[6]*acadoWorkspace.QDy[5] + acadoWorkspace.evGx[10]*acadoWorkspace.QDy[6] + acadoWorkspace.evGx[14]*acadoWorkspace.QDy[7] + acadoWorkspace.evGx[18]*acadoWorkspace.QDy[8] + acadoWorkspace.evGx[22]*acadoWorkspace.QDy[9] + acadoWorkspace.evGx[26]*acadoWorkspace.QDy[10] + acadoWorkspace.evGx[30]*acadoWorkspace.QDy[11] + acadoWorkspace.evGx[34]*acadoWorkspace.QDy[12] + acadoWorkspace.evGx[38]*acadoWorkspace.QDy[13] + acadoWorkspace.evGx[42]*acadoWorkspace.QDy[14] + acadoWorkspace.evGx[46]*acadoWorkspace.QDy[15] + acadoWorkspace.evGx[50]*acadoWorkspace.QDy[16] + acadoWorkspace.evGx[54]*acadoWorkspace.QDy[17] + acadoWorkspace.evGx[58]*acadoWorkspace.QDy[18] + acadoWorkspace.evGx[62]*acadoWorkspace.QDy[19] + acadoWorkspace.evGx[66]*acadoWorkspace.QDy[20] + acadoWorkspace.evGx[70]*acadoWorkspace.QDy[21] + acadoWorkspace.evGx[74]*acadoWorkspace.QDy[22] + acadoWorkspace.evGx[78]*acadoWorkspace.QDy[23] + acadoWorkspace.evGx[82]*acadoWorkspace.QDy[24] + acadoWorkspace.evGx[86]*acadoWorkspace.QDy[25] + acadoWorkspace.evGx[90]*acadoWorkspace.QDy[26] + acadoWorkspace.evGx[94]*acadoWorkspace.QDy[27] + acadoWorkspace.evGx[98]*acadoWorkspace.QDy[28] + acadoWorkspace.evGx[102]*acadoWorkspace.QDy[29] + acadoWorkspace.evGx[106]*acadoWorkspace.QDy[30] + acadoWorkspace.evGx[110]*acadoWorkspace.QDy[31] + acadoWorkspace.evGx[114]*acadoWorkspace.QDy[32] + acadoWorkspace.evGx[118]*acadoWorkspace.QDy[33] + acadoWorkspace.evGx[122]*acadoWorkspace.QDy[34] + acadoWorkspace.evGx[126]*acadoWorkspace.QDy[35] + acadoWorkspace.evGx[130]*acadoWorkspace.QDy[36] + acadoWorkspace.evGx[134]*acadoWorkspace.QDy[37] + acadoWorkspace.evGx[138]*acadoWorkspace.QDy[38] + acadoWorkspace.evGx[142]*acadoWorkspace.QDy[39] + acadoWorkspace.evGx[146]*acadoWorkspace.QDy[40] + acadoWorkspace.evGx[150]*acadoWorkspace.QDy[41] + acadoWorkspace.evGx[154]*acadoWorkspace.QDy[42] + acadoWorkspace.evGx[158]*acadoWorkspace.QDy[43] + acadoWorkspace.evGx[162]*acadoWorkspace.QDy[44] + acadoWorkspace.evGx[166]*acadoWorkspace.QDy[45] + acadoWorkspace.evGx[170]*acadoWorkspace.QDy[46] + acadoWorkspace.evGx[174]*acadoWorkspace.QDy[47] + acadoWorkspace.evGx[178]*acadoWorkspace.QDy[48] + acadoWorkspace.evGx[182]*acadoWorkspace.QDy[49] + acadoWorkspace.evGx[186]*acadoWorkspace.QDy[50] + acadoWorkspace.evGx[190]*acadoWorkspace.QDy[51] + acadoWorkspace.evGx[194]*acadoWorkspace.QDy[52] + acadoWorkspace.evGx[198]*acadoWorkspace.QDy[53] + acadoWorkspace.evGx[202]*acadoWorkspace.QDy[54] + acadoWorkspace.evGx[206]*acadoWorkspace.QDy[55] + acadoWorkspace.evGx[210]*acadoWorkspace.QDy[56] + acadoWorkspace.evGx[214]*acadoWorkspace.QDy[57] + acadoWorkspace.evGx[218]*acadoWorkspace.QDy[58] + acadoWorkspace.evGx[222]*acadoWorkspace.QDy[59] + acadoWorkspace.evGx[226]*acadoWorkspace.QDy[60] + acadoWorkspace.evGx[230]*acadoWorkspace.QDy[61] + acadoWorkspace.evGx[234]*acadoWorkspace.QDy[62] + acadoWorkspace.evGx[238]*acadoWorkspace.QDy[63] + acadoWorkspace.evGx[242]*acadoWorkspace.QDy[64] + acadoWorkspace.evGx[246]*acadoWorkspace.QDy[65] + acadoWorkspace.evGx[250]*acadoWorkspace.QDy[66] + acadoWorkspace.evGx[254]*acadoWorkspace.QDy[67] + acadoWorkspace.evGx[258]*acadoWorkspace.QDy[68] + acadoWorkspace.evGx[262]*acadoWorkspace.QDy[69] + acadoWorkspace.evGx[266]*acadoWorkspace.QDy[70] + acadoWorkspace.evGx[270]*acadoWorkspace.QDy[71] + acadoWorkspace.evGx[274]*acadoWorkspace.QDy[72] + acadoWorkspace.evGx[278]*acadoWorkspace.QDy[73] + acadoWorkspace.evGx[282]*acadoWorkspace.QDy[74] + acadoWorkspace.evGx[286]*acadoWorkspace.QDy[75] + acadoWorkspace.evGx[290]*acadoWorkspace.QDy[76] + acadoWorkspace.evGx[294]*acadoWorkspace.QDy[77] + acadoWorkspace.evGx[298]*acadoWorkspace.QDy[78] + acadoWorkspace.evGx[302]*acadoWorkspace.QDy[79] + acadoWorkspace.evGx[306]*acadoWorkspace.QDy[80] + acadoWorkspace.evGx[310]*acadoWorkspace.QDy[81] + acadoWorkspace.evGx[314]*acadoWorkspace.QDy[82] + acadoWorkspace.evGx[318]*acadoWorkspace.QDy[83] + acadoWorkspace.evGx[322]*acadoWorkspace.QDy[84] + acadoWorkspace.evGx[326]*acadoWorkspace.QDy[85] + acadoWorkspace.evGx[330]*acadoWorkspace.QDy[86] + acadoWorkspace.evGx[334]*acadoWorkspace.QDy[87] + acadoWorkspace.evGx[338]*acadoWorkspace.QDy[88] + acadoWorkspace.evGx[342]*acadoWorkspace.QDy[89] + acadoWorkspace.evGx[346]*acadoWorkspace.QDy[90] + acadoWorkspace.evGx[350]*acadoWorkspace.QDy[91] + acadoWorkspace.evGx[354]*acadoWorkspace.QDy[92] + acadoWorkspace.evGx[358]*acadoWorkspace.QDy[93] + acadoWorkspace.evGx[362]*acadoWorkspace.QDy[94] + acadoWorkspace.evGx[366]*acadoWorkspace.QDy[95] + acadoWorkspace.evGx[370]*acadoWorkspace.QDy[96] + acadoWorkspace.evGx[374]*acadoWorkspace.QDy[97] + acadoWorkspace.evGx[378]*acadoWorkspace.QDy[98] + acadoWorkspace.evGx[382]*acadoWorkspace.QDy[99] + acadoWorkspace.evGx[386]*acadoWorkspace.QDy[100] + acadoWorkspace.evGx[390]*acadoWorkspace.QDy[101] + acadoWorkspace.evGx[394]*acadoWorkspace.QDy[102] + acadoWorkspace.evGx[398]*acadoWorkspace.QDy[103] + acadoWorkspace.evGx[402]*acadoWorkspace.QDy[104] + acadoWorkspace.evGx[406]*acadoWorkspace.QDy[105] + acadoWorkspace.evGx[410]*acadoWorkspace.QDy[106] + acadoWorkspace.evGx[414]*acadoWorkspace.QDy[107] + acadoWorkspace.evGx[418]*acadoWorkspace.QDy[108] + acadoWorkspace.evGx[422]*acadoWorkspace.QDy[109] + acadoWorkspace.evGx[426]*acadoWorkspace.QDy[110] + acadoWorkspace.evGx[430]*acadoWorkspace.QDy[111] + acadoWorkspace.evGx[434]*acadoWorkspace.QDy[112] + acadoWorkspace.evGx[438]*acadoWorkspace.QDy[113] + acadoWorkspace.evGx[442]*acadoWorkspace.QDy[114] + acadoWorkspace.evGx[446]*acadoWorkspace.QDy[115] + acadoWorkspace.evGx[450]*acadoWorkspace.QDy[116] + acadoWorkspace.evGx[454]*acadoWorkspace.QDy[117] + acadoWorkspace.evGx[458]*acadoWorkspace.QDy[118] + acadoWorkspace.evGx[462]*acadoWorkspace.QDy[119] + acadoWorkspace.evGx[466]*acadoWorkspace.QDy[120] + acadoWorkspace.evGx[470]*acadoWorkspace.QDy[121] + acadoWorkspace.evGx[474]*acadoWorkspace.QDy[122] + acadoWorkspace.evGx[478]*acadoWorkspace.QDy[123] + acadoWorkspace.evGx[482]*acadoWorkspace.QDy[124] + acadoWorkspace.evGx[486]*acadoWorkspace.QDy[125] + acadoWorkspace.evGx[490]*acadoWorkspace.QDy[126] + acadoWorkspace.evGx[494]*acadoWorkspace.QDy[127] + acadoWorkspace.evGx[498]*acadoWorkspace.QDy[128] + acadoWorkspace.evGx[502]*acadoWorkspace.QDy[129] + acadoWorkspace.evGx[506]*acadoWorkspace.QDy[130] + acadoWorkspace.evGx[510]*acadoWorkspace.QDy[131] + acadoWorkspace.evGx[514]*acadoWorkspace.QDy[132] + acadoWorkspace.evGx[518]*acadoWorkspace.QDy[133] + acadoWorkspace.evGx[522]*acadoWorkspace.QDy[134] + acadoWorkspace.evGx[526]*acadoWorkspace.QDy[135] + acadoWorkspace.evGx[530]*acadoWorkspace.QDy[136] + acadoWorkspace.evGx[534]*acadoWorkspace.QDy[137] + acadoWorkspace.evGx[538]*acadoWorkspace.QDy[138] + acadoWorkspace.evGx[542]*acadoWorkspace.QDy[139] + acadoWorkspace.evGx[546]*acadoWorkspace.QDy[140] + acadoWorkspace.evGx[550]*acadoWorkspace.QDy[141] + acadoWorkspace.evGx[554]*acadoWorkspace.QDy[142] + acadoWorkspace.evGx[558]*acadoWorkspace.QDy[143] + acadoWorkspace.evGx[562]*acadoWorkspace.QDy[144] + acadoWorkspace.evGx[566]*acadoWorkspace.QDy[145] + acadoWorkspace.evGx[570]*acadoWorkspace.QDy[146] + acadoWorkspace.evGx[574]*acadoWorkspace.QDy[147] + acadoWorkspace.evGx[578]*acadoWorkspace.QDy[148] + acadoWorkspace.evGx[582]*acadoWorkspace.QDy[149] + acadoWorkspace.evGx[586]*acadoWorkspace.QDy[150] + acadoWorkspace.evGx[590]*acadoWorkspace.QDy[151] + acadoWorkspace.evGx[594]*acadoWorkspace.QDy[152] + acadoWorkspace.evGx[598]*acadoWorkspace.QDy[153] + acadoWorkspace.evGx[602]*acadoWorkspace.QDy[154] + acadoWorkspace.evGx[606]*acadoWorkspace.QDy[155] + acadoWorkspace.evGx[610]*acadoWorkspace.QDy[156] + acadoWorkspace.evGx[614]*acadoWorkspace.QDy[157] + acadoWorkspace.evGx[618]*acadoWorkspace.QDy[158] + acadoWorkspace.evGx[622]*acadoWorkspace.QDy[159] + acadoWorkspace.evGx[626]*acadoWorkspace.QDy[160] + acadoWorkspace.evGx[630]*acadoWorkspace.QDy[161] + acadoWorkspace.evGx[634]*acadoWorkspace.QDy[162] + acadoWorkspace.evGx[638]*acadoWorkspace.QDy[163] + acadoWorkspace.evGx[642]*acadoWorkspace.QDy[164] + acadoWorkspace.evGx[646]*acadoWorkspace.QDy[165] + acadoWorkspace.evGx[650]*acadoWorkspace.QDy[166] + acadoWorkspace.evGx[654]*acadoWorkspace.QDy[167] + acadoWorkspace.evGx[658]*acadoWorkspace.QDy[168] + acadoWorkspace.evGx[662]*acadoWorkspace.QDy[169] + acadoWorkspace.evGx[666]*acadoWorkspace.QDy[170] + acadoWorkspace.evGx[670]*acadoWorkspace.QDy[171] + acadoWorkspace.evGx[674]*acadoWorkspace.QDy[172] + acadoWorkspace.evGx[678]*acadoWorkspace.QDy[173] + acadoWorkspace.evGx[682]*acadoWorkspace.QDy[174] + acadoWorkspace.evGx[686]*acadoWorkspace.QDy[175] + acadoWorkspace.evGx[690]*acadoWorkspace.QDy[176] + acadoWorkspace.evGx[694]*acadoWorkspace.QDy[177] + acadoWorkspace.evGx[698]*acadoWorkspace.QDy[178] + acadoWorkspace.evGx[702]*acadoWorkspace.QDy[179] + acadoWorkspace.evGx[706]*acadoWorkspace.QDy[180] + acadoWorkspace.evGx[710]*acadoWorkspace.QDy[181] + acadoWorkspace.evGx[714]*acadoWorkspace.QDy[182] + acadoWorkspace.evGx[718]*acadoWorkspace.QDy[183] + acadoWorkspace.evGx[722]*acadoWorkspace.QDy[184] + acadoWorkspace.evGx[726]*acadoWorkspace.QDy[185] + acadoWorkspace.evGx[730]*acadoWorkspace.QDy[186] + acadoWorkspace.evGx[734]*acadoWorkspace.QDy[187] + acadoWorkspace.evGx[738]*acadoWorkspace.QDy[188] + acadoWorkspace.evGx[742]*acadoWorkspace.QDy[189] + acadoWorkspace.evGx[746]*acadoWorkspace.QDy[190] + acadoWorkspace.evGx[750]*acadoWorkspace.QDy[191] + acadoWorkspace.evGx[754]*acadoWorkspace.QDy[192] + acadoWorkspace.evGx[758]*acadoWorkspace.QDy[193] + acadoWorkspace.evGx[762]*acadoWorkspace.QDy[194] + acadoWorkspace.evGx[766]*acadoWorkspace.QDy[195] + acadoWorkspace.evGx[770]*acadoWorkspace.QDy[196] + acadoWorkspace.evGx[774]*acadoWorkspace.QDy[197] + acadoWorkspace.evGx[778]*acadoWorkspace.QDy[198] + acadoWorkspace.evGx[782]*acadoWorkspace.QDy[199] + acadoWorkspace.evGx[786]*acadoWorkspace.QDy[200] + acadoWorkspace.evGx[790]*acadoWorkspace.QDy[201] + acadoWorkspace.evGx[794]*acadoWorkspace.QDy[202] + acadoWorkspace.evGx[798]*acadoWorkspace.QDy[203]; +acadoWorkspace.g[3] = + acadoWorkspace.evGx[3]*acadoWorkspace.QDy[4] + acadoWorkspace.evGx[7]*acadoWorkspace.QDy[5] + acadoWorkspace.evGx[11]*acadoWorkspace.QDy[6] + acadoWorkspace.evGx[15]*acadoWorkspace.QDy[7] + acadoWorkspace.evGx[19]*acadoWorkspace.QDy[8] + acadoWorkspace.evGx[23]*acadoWorkspace.QDy[9] + acadoWorkspace.evGx[27]*acadoWorkspace.QDy[10] + acadoWorkspace.evGx[31]*acadoWorkspace.QDy[11] + acadoWorkspace.evGx[35]*acadoWorkspace.QDy[12] + acadoWorkspace.evGx[39]*acadoWorkspace.QDy[13] + acadoWorkspace.evGx[43]*acadoWorkspace.QDy[14] + acadoWorkspace.evGx[47]*acadoWorkspace.QDy[15] + acadoWorkspace.evGx[51]*acadoWorkspace.QDy[16] + acadoWorkspace.evGx[55]*acadoWorkspace.QDy[17] + acadoWorkspace.evGx[59]*acadoWorkspace.QDy[18] + acadoWorkspace.evGx[63]*acadoWorkspace.QDy[19] + acadoWorkspace.evGx[67]*acadoWorkspace.QDy[20] + acadoWorkspace.evGx[71]*acadoWorkspace.QDy[21] + acadoWorkspace.evGx[75]*acadoWorkspace.QDy[22] + acadoWorkspace.evGx[79]*acadoWorkspace.QDy[23] + acadoWorkspace.evGx[83]*acadoWorkspace.QDy[24] + acadoWorkspace.evGx[87]*acadoWorkspace.QDy[25] + acadoWorkspace.evGx[91]*acadoWorkspace.QDy[26] + acadoWorkspace.evGx[95]*acadoWorkspace.QDy[27] + acadoWorkspace.evGx[99]*acadoWorkspace.QDy[28] + acadoWorkspace.evGx[103]*acadoWorkspace.QDy[29] + acadoWorkspace.evGx[107]*acadoWorkspace.QDy[30] + acadoWorkspace.evGx[111]*acadoWorkspace.QDy[31] + acadoWorkspace.evGx[115]*acadoWorkspace.QDy[32] + acadoWorkspace.evGx[119]*acadoWorkspace.QDy[33] + acadoWorkspace.evGx[123]*acadoWorkspace.QDy[34] + acadoWorkspace.evGx[127]*acadoWorkspace.QDy[35] + acadoWorkspace.evGx[131]*acadoWorkspace.QDy[36] + acadoWorkspace.evGx[135]*acadoWorkspace.QDy[37] + acadoWorkspace.evGx[139]*acadoWorkspace.QDy[38] + acadoWorkspace.evGx[143]*acadoWorkspace.QDy[39] + acadoWorkspace.evGx[147]*acadoWorkspace.QDy[40] + acadoWorkspace.evGx[151]*acadoWorkspace.QDy[41] + acadoWorkspace.evGx[155]*acadoWorkspace.QDy[42] + acadoWorkspace.evGx[159]*acadoWorkspace.QDy[43] + acadoWorkspace.evGx[163]*acadoWorkspace.QDy[44] + acadoWorkspace.evGx[167]*acadoWorkspace.QDy[45] + acadoWorkspace.evGx[171]*acadoWorkspace.QDy[46] + acadoWorkspace.evGx[175]*acadoWorkspace.QDy[47] + acadoWorkspace.evGx[179]*acadoWorkspace.QDy[48] + acadoWorkspace.evGx[183]*acadoWorkspace.QDy[49] + acadoWorkspace.evGx[187]*acadoWorkspace.QDy[50] + acadoWorkspace.evGx[191]*acadoWorkspace.QDy[51] + acadoWorkspace.evGx[195]*acadoWorkspace.QDy[52] + acadoWorkspace.evGx[199]*acadoWorkspace.QDy[53] + acadoWorkspace.evGx[203]*acadoWorkspace.QDy[54] + acadoWorkspace.evGx[207]*acadoWorkspace.QDy[55] + acadoWorkspace.evGx[211]*acadoWorkspace.QDy[56] + acadoWorkspace.evGx[215]*acadoWorkspace.QDy[57] + acadoWorkspace.evGx[219]*acadoWorkspace.QDy[58] + acadoWorkspace.evGx[223]*acadoWorkspace.QDy[59] + acadoWorkspace.evGx[227]*acadoWorkspace.QDy[60] + acadoWorkspace.evGx[231]*acadoWorkspace.QDy[61] + acadoWorkspace.evGx[235]*acadoWorkspace.QDy[62] + acadoWorkspace.evGx[239]*acadoWorkspace.QDy[63] + acadoWorkspace.evGx[243]*acadoWorkspace.QDy[64] + acadoWorkspace.evGx[247]*acadoWorkspace.QDy[65] + acadoWorkspace.evGx[251]*acadoWorkspace.QDy[66] + acadoWorkspace.evGx[255]*acadoWorkspace.QDy[67] + acadoWorkspace.evGx[259]*acadoWorkspace.QDy[68] + acadoWorkspace.evGx[263]*acadoWorkspace.QDy[69] + acadoWorkspace.evGx[267]*acadoWorkspace.QDy[70] + acadoWorkspace.evGx[271]*acadoWorkspace.QDy[71] + acadoWorkspace.evGx[275]*acadoWorkspace.QDy[72] + acadoWorkspace.evGx[279]*acadoWorkspace.QDy[73] + acadoWorkspace.evGx[283]*acadoWorkspace.QDy[74] + acadoWorkspace.evGx[287]*acadoWorkspace.QDy[75] + acadoWorkspace.evGx[291]*acadoWorkspace.QDy[76] + acadoWorkspace.evGx[295]*acadoWorkspace.QDy[77] + acadoWorkspace.evGx[299]*acadoWorkspace.QDy[78] + acadoWorkspace.evGx[303]*acadoWorkspace.QDy[79] + acadoWorkspace.evGx[307]*acadoWorkspace.QDy[80] + acadoWorkspace.evGx[311]*acadoWorkspace.QDy[81] + acadoWorkspace.evGx[315]*acadoWorkspace.QDy[82] + acadoWorkspace.evGx[319]*acadoWorkspace.QDy[83] + acadoWorkspace.evGx[323]*acadoWorkspace.QDy[84] + acadoWorkspace.evGx[327]*acadoWorkspace.QDy[85] + acadoWorkspace.evGx[331]*acadoWorkspace.QDy[86] + acadoWorkspace.evGx[335]*acadoWorkspace.QDy[87] + acadoWorkspace.evGx[339]*acadoWorkspace.QDy[88] + acadoWorkspace.evGx[343]*acadoWorkspace.QDy[89] + acadoWorkspace.evGx[347]*acadoWorkspace.QDy[90] + acadoWorkspace.evGx[351]*acadoWorkspace.QDy[91] + acadoWorkspace.evGx[355]*acadoWorkspace.QDy[92] + acadoWorkspace.evGx[359]*acadoWorkspace.QDy[93] + acadoWorkspace.evGx[363]*acadoWorkspace.QDy[94] + acadoWorkspace.evGx[367]*acadoWorkspace.QDy[95] + acadoWorkspace.evGx[371]*acadoWorkspace.QDy[96] + acadoWorkspace.evGx[375]*acadoWorkspace.QDy[97] + acadoWorkspace.evGx[379]*acadoWorkspace.QDy[98] + acadoWorkspace.evGx[383]*acadoWorkspace.QDy[99] + acadoWorkspace.evGx[387]*acadoWorkspace.QDy[100] + acadoWorkspace.evGx[391]*acadoWorkspace.QDy[101] + acadoWorkspace.evGx[395]*acadoWorkspace.QDy[102] + acadoWorkspace.evGx[399]*acadoWorkspace.QDy[103] + acadoWorkspace.evGx[403]*acadoWorkspace.QDy[104] + acadoWorkspace.evGx[407]*acadoWorkspace.QDy[105] + acadoWorkspace.evGx[411]*acadoWorkspace.QDy[106] + acadoWorkspace.evGx[415]*acadoWorkspace.QDy[107] + acadoWorkspace.evGx[419]*acadoWorkspace.QDy[108] + acadoWorkspace.evGx[423]*acadoWorkspace.QDy[109] + acadoWorkspace.evGx[427]*acadoWorkspace.QDy[110] + acadoWorkspace.evGx[431]*acadoWorkspace.QDy[111] + acadoWorkspace.evGx[435]*acadoWorkspace.QDy[112] + acadoWorkspace.evGx[439]*acadoWorkspace.QDy[113] + acadoWorkspace.evGx[443]*acadoWorkspace.QDy[114] + acadoWorkspace.evGx[447]*acadoWorkspace.QDy[115] + acadoWorkspace.evGx[451]*acadoWorkspace.QDy[116] + acadoWorkspace.evGx[455]*acadoWorkspace.QDy[117] + acadoWorkspace.evGx[459]*acadoWorkspace.QDy[118] + acadoWorkspace.evGx[463]*acadoWorkspace.QDy[119] + acadoWorkspace.evGx[467]*acadoWorkspace.QDy[120] + acadoWorkspace.evGx[471]*acadoWorkspace.QDy[121] + acadoWorkspace.evGx[475]*acadoWorkspace.QDy[122] + acadoWorkspace.evGx[479]*acadoWorkspace.QDy[123] + acadoWorkspace.evGx[483]*acadoWorkspace.QDy[124] + acadoWorkspace.evGx[487]*acadoWorkspace.QDy[125] + acadoWorkspace.evGx[491]*acadoWorkspace.QDy[126] + acadoWorkspace.evGx[495]*acadoWorkspace.QDy[127] + acadoWorkspace.evGx[499]*acadoWorkspace.QDy[128] + acadoWorkspace.evGx[503]*acadoWorkspace.QDy[129] + acadoWorkspace.evGx[507]*acadoWorkspace.QDy[130] + acadoWorkspace.evGx[511]*acadoWorkspace.QDy[131] + acadoWorkspace.evGx[515]*acadoWorkspace.QDy[132] + acadoWorkspace.evGx[519]*acadoWorkspace.QDy[133] + acadoWorkspace.evGx[523]*acadoWorkspace.QDy[134] + acadoWorkspace.evGx[527]*acadoWorkspace.QDy[135] + acadoWorkspace.evGx[531]*acadoWorkspace.QDy[136] + acadoWorkspace.evGx[535]*acadoWorkspace.QDy[137] + acadoWorkspace.evGx[539]*acadoWorkspace.QDy[138] + acadoWorkspace.evGx[543]*acadoWorkspace.QDy[139] + acadoWorkspace.evGx[547]*acadoWorkspace.QDy[140] + acadoWorkspace.evGx[551]*acadoWorkspace.QDy[141] + acadoWorkspace.evGx[555]*acadoWorkspace.QDy[142] + acadoWorkspace.evGx[559]*acadoWorkspace.QDy[143] + acadoWorkspace.evGx[563]*acadoWorkspace.QDy[144] + acadoWorkspace.evGx[567]*acadoWorkspace.QDy[145] + acadoWorkspace.evGx[571]*acadoWorkspace.QDy[146] + acadoWorkspace.evGx[575]*acadoWorkspace.QDy[147] + acadoWorkspace.evGx[579]*acadoWorkspace.QDy[148] + acadoWorkspace.evGx[583]*acadoWorkspace.QDy[149] + acadoWorkspace.evGx[587]*acadoWorkspace.QDy[150] + acadoWorkspace.evGx[591]*acadoWorkspace.QDy[151] + acadoWorkspace.evGx[595]*acadoWorkspace.QDy[152] + acadoWorkspace.evGx[599]*acadoWorkspace.QDy[153] + acadoWorkspace.evGx[603]*acadoWorkspace.QDy[154] + acadoWorkspace.evGx[607]*acadoWorkspace.QDy[155] + acadoWorkspace.evGx[611]*acadoWorkspace.QDy[156] + acadoWorkspace.evGx[615]*acadoWorkspace.QDy[157] + acadoWorkspace.evGx[619]*acadoWorkspace.QDy[158] + acadoWorkspace.evGx[623]*acadoWorkspace.QDy[159] + acadoWorkspace.evGx[627]*acadoWorkspace.QDy[160] + acadoWorkspace.evGx[631]*acadoWorkspace.QDy[161] + acadoWorkspace.evGx[635]*acadoWorkspace.QDy[162] + acadoWorkspace.evGx[639]*acadoWorkspace.QDy[163] + acadoWorkspace.evGx[643]*acadoWorkspace.QDy[164] + acadoWorkspace.evGx[647]*acadoWorkspace.QDy[165] + acadoWorkspace.evGx[651]*acadoWorkspace.QDy[166] + acadoWorkspace.evGx[655]*acadoWorkspace.QDy[167] + acadoWorkspace.evGx[659]*acadoWorkspace.QDy[168] + acadoWorkspace.evGx[663]*acadoWorkspace.QDy[169] + acadoWorkspace.evGx[667]*acadoWorkspace.QDy[170] + acadoWorkspace.evGx[671]*acadoWorkspace.QDy[171] + acadoWorkspace.evGx[675]*acadoWorkspace.QDy[172] + acadoWorkspace.evGx[679]*acadoWorkspace.QDy[173] + acadoWorkspace.evGx[683]*acadoWorkspace.QDy[174] + acadoWorkspace.evGx[687]*acadoWorkspace.QDy[175] + acadoWorkspace.evGx[691]*acadoWorkspace.QDy[176] + acadoWorkspace.evGx[695]*acadoWorkspace.QDy[177] + acadoWorkspace.evGx[699]*acadoWorkspace.QDy[178] + acadoWorkspace.evGx[703]*acadoWorkspace.QDy[179] + acadoWorkspace.evGx[707]*acadoWorkspace.QDy[180] + acadoWorkspace.evGx[711]*acadoWorkspace.QDy[181] + acadoWorkspace.evGx[715]*acadoWorkspace.QDy[182] + acadoWorkspace.evGx[719]*acadoWorkspace.QDy[183] + acadoWorkspace.evGx[723]*acadoWorkspace.QDy[184] + acadoWorkspace.evGx[727]*acadoWorkspace.QDy[185] + acadoWorkspace.evGx[731]*acadoWorkspace.QDy[186] + acadoWorkspace.evGx[735]*acadoWorkspace.QDy[187] + acadoWorkspace.evGx[739]*acadoWorkspace.QDy[188] + acadoWorkspace.evGx[743]*acadoWorkspace.QDy[189] + acadoWorkspace.evGx[747]*acadoWorkspace.QDy[190] + acadoWorkspace.evGx[751]*acadoWorkspace.QDy[191] + acadoWorkspace.evGx[755]*acadoWorkspace.QDy[192] + acadoWorkspace.evGx[759]*acadoWorkspace.QDy[193] + acadoWorkspace.evGx[763]*acadoWorkspace.QDy[194] + acadoWorkspace.evGx[767]*acadoWorkspace.QDy[195] + acadoWorkspace.evGx[771]*acadoWorkspace.QDy[196] + acadoWorkspace.evGx[775]*acadoWorkspace.QDy[197] + acadoWorkspace.evGx[779]*acadoWorkspace.QDy[198] + acadoWorkspace.evGx[783]*acadoWorkspace.QDy[199] + acadoWorkspace.evGx[787]*acadoWorkspace.QDy[200] + acadoWorkspace.evGx[791]*acadoWorkspace.QDy[201] + acadoWorkspace.evGx[795]*acadoWorkspace.QDy[202] + acadoWorkspace.evGx[799]*acadoWorkspace.QDy[203]; + + +for (lRun1 = 0; lRun1 < 50; ++lRun1) +{ +for (lRun2 = lRun1; lRun2 < 50; ++lRun2) +{ +lRun3 = (((lRun2 + 1) * (lRun2)) / (2)) + (lRun1); +acado_multEQDy( &(acadoWorkspace.E[ lRun3 * 4 ]), &(acadoWorkspace.QDy[ lRun2 * 4 + 4 ]), &(acadoWorkspace.g[ lRun1 + 4 ]) ); +} +} + +acadoWorkspace.lb[0] = acadoWorkspace.Dx0[0]; +acadoWorkspace.lb[1] = acadoWorkspace.Dx0[1]; +acadoWorkspace.lb[2] = acadoWorkspace.Dx0[2]; +acadoWorkspace.lb[3] = acadoWorkspace.Dx0[3]; +acadoWorkspace.ub[0] = acadoWorkspace.Dx0[0]; +acadoWorkspace.ub[1] = acadoWorkspace.Dx0[1]; +acadoWorkspace.ub[2] = acadoWorkspace.Dx0[2]; +acadoWorkspace.ub[3] = acadoWorkspace.Dx0[3]; +tmp = acadoVariables.x[6] + acadoWorkspace.d[2]; +acadoWorkspace.lbA[0] = (real_t)-1.5707963268000000e+00 - tmp; +acadoWorkspace.ubA[0] = (real_t)1.5707963268000000e+00 - tmp; +tmp = acadoVariables.x[7] + acadoWorkspace.d[3]; +acadoWorkspace.lbA[1] = (real_t)-4.3633231300000003e-01 - tmp; +acadoWorkspace.ubA[1] = (real_t)4.3633231300000003e-01 - tmp; +tmp = acadoVariables.x[10] + acadoWorkspace.d[6]; +acadoWorkspace.lbA[2] = (real_t)-1.5707963268000000e+00 - tmp; +acadoWorkspace.ubA[2] = (real_t)1.5707963268000000e+00 - tmp; +tmp = acadoVariables.x[11] + acadoWorkspace.d[7]; +acadoWorkspace.lbA[3] = (real_t)-4.3633231300000003e-01 - tmp; +acadoWorkspace.ubA[3] = (real_t)4.3633231300000003e-01 - tmp; +tmp = acadoVariables.x[14] + acadoWorkspace.d[10]; +acadoWorkspace.lbA[4] = (real_t)-1.5707963268000000e+00 - tmp; +acadoWorkspace.ubA[4] = (real_t)1.5707963268000000e+00 - tmp; +tmp = acadoVariables.x[15] + acadoWorkspace.d[11]; +acadoWorkspace.lbA[5] = (real_t)-4.3633231300000003e-01 - tmp; +acadoWorkspace.ubA[5] = (real_t)4.3633231300000003e-01 - tmp; +tmp = acadoVariables.x[18] + acadoWorkspace.d[14]; +acadoWorkspace.lbA[6] = (real_t)-1.5707963268000000e+00 - tmp; +acadoWorkspace.ubA[6] = (real_t)1.5707963268000000e+00 - tmp; +tmp = acadoVariables.x[19] + acadoWorkspace.d[15]; +acadoWorkspace.lbA[7] = (real_t)-4.3633231300000003e-01 - tmp; +acadoWorkspace.ubA[7] = (real_t)4.3633231300000003e-01 - tmp; +tmp = acadoVariables.x[22] + acadoWorkspace.d[18]; +acadoWorkspace.lbA[8] = (real_t)-1.5707963268000000e+00 - tmp; +acadoWorkspace.ubA[8] = (real_t)1.5707963268000000e+00 - tmp; +tmp = acadoVariables.x[23] + acadoWorkspace.d[19]; +acadoWorkspace.lbA[9] = (real_t)-4.3633231300000003e-01 - tmp; +acadoWorkspace.ubA[9] = (real_t)4.3633231300000003e-01 - tmp; +tmp = acadoVariables.x[26] + acadoWorkspace.d[22]; +acadoWorkspace.lbA[10] = (real_t)-1.5707963268000000e+00 - tmp; +acadoWorkspace.ubA[10] = (real_t)1.5707963268000000e+00 - tmp; +tmp = acadoVariables.x[27] + acadoWorkspace.d[23]; +acadoWorkspace.lbA[11] = (real_t)-4.3633231300000003e-01 - tmp; +acadoWorkspace.ubA[11] = (real_t)4.3633231300000003e-01 - tmp; +tmp = acadoVariables.x[30] + acadoWorkspace.d[26]; +acadoWorkspace.lbA[12] = (real_t)-1.5707963268000000e+00 - tmp; +acadoWorkspace.ubA[12] = (real_t)1.5707963268000000e+00 - tmp; +tmp = acadoVariables.x[31] + acadoWorkspace.d[27]; +acadoWorkspace.lbA[13] = (real_t)-4.3633231300000003e-01 - tmp; +acadoWorkspace.ubA[13] = (real_t)4.3633231300000003e-01 - tmp; +tmp = acadoVariables.x[34] + acadoWorkspace.d[30]; +acadoWorkspace.lbA[14] = (real_t)-1.5707963268000000e+00 - tmp; +acadoWorkspace.ubA[14] = (real_t)1.5707963268000000e+00 - tmp; +tmp = acadoVariables.x[35] + acadoWorkspace.d[31]; +acadoWorkspace.lbA[15] = (real_t)-4.3633231300000003e-01 - tmp; +acadoWorkspace.ubA[15] = (real_t)4.3633231300000003e-01 - tmp; +tmp = acadoVariables.x[38] + acadoWorkspace.d[34]; +acadoWorkspace.lbA[16] = (real_t)-1.5707963268000000e+00 - tmp; +acadoWorkspace.ubA[16] = (real_t)1.5707963268000000e+00 - tmp; +tmp = acadoVariables.x[39] + acadoWorkspace.d[35]; +acadoWorkspace.lbA[17] = (real_t)-4.3633231300000003e-01 - tmp; +acadoWorkspace.ubA[17] = (real_t)4.3633231300000003e-01 - tmp; +tmp = acadoVariables.x[42] + acadoWorkspace.d[38]; +acadoWorkspace.lbA[18] = (real_t)-1.5707963268000000e+00 - tmp; +acadoWorkspace.ubA[18] = (real_t)1.5707963268000000e+00 - tmp; +tmp = acadoVariables.x[43] + acadoWorkspace.d[39]; +acadoWorkspace.lbA[19] = (real_t)-4.3633231300000003e-01 - tmp; +acadoWorkspace.ubA[19] = (real_t)4.3633231300000003e-01 - tmp; +tmp = acadoVariables.x[46] + acadoWorkspace.d[42]; +acadoWorkspace.lbA[20] = (real_t)-1.5707963268000000e+00 - tmp; +acadoWorkspace.ubA[20] = (real_t)1.5707963268000000e+00 - tmp; +tmp = acadoVariables.x[47] + acadoWorkspace.d[43]; +acadoWorkspace.lbA[21] = (real_t)-4.3633231300000003e-01 - tmp; +acadoWorkspace.ubA[21] = (real_t)4.3633231300000003e-01 - tmp; +tmp = acadoVariables.x[50] + acadoWorkspace.d[46]; +acadoWorkspace.lbA[22] = (real_t)-1.5707963268000000e+00 - tmp; +acadoWorkspace.ubA[22] = (real_t)1.5707963268000000e+00 - tmp; +tmp = acadoVariables.x[51] + acadoWorkspace.d[47]; +acadoWorkspace.lbA[23] = (real_t)-4.3633231300000003e-01 - tmp; +acadoWorkspace.ubA[23] = (real_t)4.3633231300000003e-01 - tmp; +tmp = acadoVariables.x[54] + acadoWorkspace.d[50]; +acadoWorkspace.lbA[24] = (real_t)-1.5707963268000000e+00 - tmp; +acadoWorkspace.ubA[24] = (real_t)1.5707963268000000e+00 - tmp; +tmp = acadoVariables.x[55] + acadoWorkspace.d[51]; +acadoWorkspace.lbA[25] = (real_t)-4.3633231300000003e-01 - tmp; +acadoWorkspace.ubA[25] = (real_t)4.3633231300000003e-01 - tmp; +tmp = acadoVariables.x[58] + acadoWorkspace.d[54]; +acadoWorkspace.lbA[26] = (real_t)-1.5707963268000000e+00 - tmp; +acadoWorkspace.ubA[26] = (real_t)1.5707963268000000e+00 - tmp; +tmp = acadoVariables.x[59] + acadoWorkspace.d[55]; +acadoWorkspace.lbA[27] = (real_t)-4.3633231300000003e-01 - tmp; +acadoWorkspace.ubA[27] = (real_t)4.3633231300000003e-01 - tmp; +tmp = acadoVariables.x[62] + acadoWorkspace.d[58]; +acadoWorkspace.lbA[28] = (real_t)-1.5707963268000000e+00 - tmp; +acadoWorkspace.ubA[28] = (real_t)1.5707963268000000e+00 - tmp; +tmp = acadoVariables.x[63] + acadoWorkspace.d[59]; +acadoWorkspace.lbA[29] = (real_t)-4.3633231300000003e-01 - tmp; +acadoWorkspace.ubA[29] = (real_t)4.3633231300000003e-01 - tmp; +tmp = acadoVariables.x[66] + acadoWorkspace.d[62]; +acadoWorkspace.lbA[30] = (real_t)-1.5707963268000000e+00 - tmp; +acadoWorkspace.ubA[30] = (real_t)1.5707963268000000e+00 - tmp; +tmp = acadoVariables.x[67] + acadoWorkspace.d[63]; +acadoWorkspace.lbA[31] = (real_t)-4.3633231300000003e-01 - tmp; +acadoWorkspace.ubA[31] = (real_t)4.3633231300000003e-01 - tmp; +tmp = acadoVariables.x[70] + acadoWorkspace.d[66]; +acadoWorkspace.lbA[32] = (real_t)-1.5707963268000000e+00 - tmp; +acadoWorkspace.ubA[32] = (real_t)1.5707963268000000e+00 - tmp; +tmp = acadoVariables.x[71] + acadoWorkspace.d[67]; +acadoWorkspace.lbA[33] = (real_t)-4.3633231300000003e-01 - tmp; +acadoWorkspace.ubA[33] = (real_t)4.3633231300000003e-01 - tmp; +tmp = acadoVariables.x[74] + acadoWorkspace.d[70]; +acadoWorkspace.lbA[34] = (real_t)-1.5707963268000000e+00 - tmp; +acadoWorkspace.ubA[34] = (real_t)1.5707963268000000e+00 - tmp; +tmp = acadoVariables.x[75] + acadoWorkspace.d[71]; +acadoWorkspace.lbA[35] = (real_t)-4.3633231300000003e-01 - tmp; +acadoWorkspace.ubA[35] = (real_t)4.3633231300000003e-01 - tmp; +tmp = acadoVariables.x[78] + acadoWorkspace.d[74]; +acadoWorkspace.lbA[36] = (real_t)-1.5707963268000000e+00 - tmp; +acadoWorkspace.ubA[36] = (real_t)1.5707963268000000e+00 - tmp; +tmp = acadoVariables.x[79] + acadoWorkspace.d[75]; +acadoWorkspace.lbA[37] = (real_t)-4.3633231300000003e-01 - tmp; +acadoWorkspace.ubA[37] = (real_t)4.3633231300000003e-01 - tmp; +tmp = acadoVariables.x[82] + acadoWorkspace.d[78]; +acadoWorkspace.lbA[38] = (real_t)-1.5707963268000000e+00 - tmp; +acadoWorkspace.ubA[38] = (real_t)1.5707963268000000e+00 - tmp; +tmp = acadoVariables.x[83] + acadoWorkspace.d[79]; +acadoWorkspace.lbA[39] = (real_t)-4.3633231300000003e-01 - tmp; +acadoWorkspace.ubA[39] = (real_t)4.3633231300000003e-01 - tmp; +tmp = acadoVariables.x[86] + acadoWorkspace.d[82]; +acadoWorkspace.lbA[40] = (real_t)-1.5707963268000000e+00 - tmp; +acadoWorkspace.ubA[40] = (real_t)1.5707963268000000e+00 - tmp; +tmp = acadoVariables.x[87] + acadoWorkspace.d[83]; +acadoWorkspace.lbA[41] = (real_t)-4.3633231300000003e-01 - tmp; +acadoWorkspace.ubA[41] = (real_t)4.3633231300000003e-01 - tmp; +tmp = acadoVariables.x[90] + acadoWorkspace.d[86]; +acadoWorkspace.lbA[42] = (real_t)-1.5707963268000000e+00 - tmp; +acadoWorkspace.ubA[42] = (real_t)1.5707963268000000e+00 - tmp; +tmp = acadoVariables.x[91] + acadoWorkspace.d[87]; +acadoWorkspace.lbA[43] = (real_t)-4.3633231300000003e-01 - tmp; +acadoWorkspace.ubA[43] = (real_t)4.3633231300000003e-01 - tmp; +tmp = acadoVariables.x[94] + acadoWorkspace.d[90]; +acadoWorkspace.lbA[44] = (real_t)-1.5707963268000000e+00 - tmp; +acadoWorkspace.ubA[44] = (real_t)1.5707963268000000e+00 - tmp; +tmp = acadoVariables.x[95] + acadoWorkspace.d[91]; +acadoWorkspace.lbA[45] = (real_t)-4.3633231300000003e-01 - tmp; +acadoWorkspace.ubA[45] = (real_t)4.3633231300000003e-01 - tmp; +tmp = acadoVariables.x[98] + acadoWorkspace.d[94]; +acadoWorkspace.lbA[46] = (real_t)-1.5707963268000000e+00 - tmp; +acadoWorkspace.ubA[46] = (real_t)1.5707963268000000e+00 - tmp; +tmp = acadoVariables.x[99] + acadoWorkspace.d[95]; +acadoWorkspace.lbA[47] = (real_t)-4.3633231300000003e-01 - tmp; +acadoWorkspace.ubA[47] = (real_t)4.3633231300000003e-01 - tmp; +tmp = acadoVariables.x[102] + acadoWorkspace.d[98]; +acadoWorkspace.lbA[48] = (real_t)-1.5707963268000000e+00 - tmp; +acadoWorkspace.ubA[48] = (real_t)1.5707963268000000e+00 - tmp; +tmp = acadoVariables.x[103] + acadoWorkspace.d[99]; +acadoWorkspace.lbA[49] = (real_t)-4.3633231300000003e-01 - tmp; +acadoWorkspace.ubA[49] = (real_t)4.3633231300000003e-01 - tmp; +tmp = acadoVariables.x[106] + acadoWorkspace.d[102]; +acadoWorkspace.lbA[50] = (real_t)-1.5707963268000000e+00 - tmp; +acadoWorkspace.ubA[50] = (real_t)1.5707963268000000e+00 - tmp; +tmp = acadoVariables.x[107] + acadoWorkspace.d[103]; +acadoWorkspace.lbA[51] = (real_t)-4.3633231300000003e-01 - tmp; +acadoWorkspace.ubA[51] = (real_t)4.3633231300000003e-01 - tmp; +tmp = acadoVariables.x[110] + acadoWorkspace.d[106]; +acadoWorkspace.lbA[52] = (real_t)-1.5707963268000000e+00 - tmp; +acadoWorkspace.ubA[52] = (real_t)1.5707963268000000e+00 - tmp; +tmp = acadoVariables.x[111] + acadoWorkspace.d[107]; +acadoWorkspace.lbA[53] = (real_t)-4.3633231300000003e-01 - tmp; +acadoWorkspace.ubA[53] = (real_t)4.3633231300000003e-01 - tmp; +tmp = acadoVariables.x[114] + acadoWorkspace.d[110]; +acadoWorkspace.lbA[54] = (real_t)-1.5707963268000000e+00 - tmp; +acadoWorkspace.ubA[54] = (real_t)1.5707963268000000e+00 - tmp; +tmp = acadoVariables.x[115] + acadoWorkspace.d[111]; +acadoWorkspace.lbA[55] = (real_t)-4.3633231300000003e-01 - tmp; +acadoWorkspace.ubA[55] = (real_t)4.3633231300000003e-01 - tmp; +tmp = acadoVariables.x[118] + acadoWorkspace.d[114]; +acadoWorkspace.lbA[56] = (real_t)-1.5707963268000000e+00 - tmp; +acadoWorkspace.ubA[56] = (real_t)1.5707963268000000e+00 - tmp; +tmp = acadoVariables.x[119] + acadoWorkspace.d[115]; +acadoWorkspace.lbA[57] = (real_t)-4.3633231300000003e-01 - tmp; +acadoWorkspace.ubA[57] = (real_t)4.3633231300000003e-01 - tmp; +tmp = acadoVariables.x[122] + acadoWorkspace.d[118]; +acadoWorkspace.lbA[58] = (real_t)-1.5707963268000000e+00 - tmp; +acadoWorkspace.ubA[58] = (real_t)1.5707963268000000e+00 - tmp; +tmp = acadoVariables.x[123] + acadoWorkspace.d[119]; +acadoWorkspace.lbA[59] = (real_t)-4.3633231300000003e-01 - tmp; +acadoWorkspace.ubA[59] = (real_t)4.3633231300000003e-01 - tmp; +tmp = acadoVariables.x[126] + acadoWorkspace.d[122]; +acadoWorkspace.lbA[60] = (real_t)-1.5707963268000000e+00 - tmp; +acadoWorkspace.ubA[60] = (real_t)1.5707963268000000e+00 - tmp; +tmp = acadoVariables.x[127] + acadoWorkspace.d[123]; +acadoWorkspace.lbA[61] = (real_t)-4.3633231300000003e-01 - tmp; +acadoWorkspace.ubA[61] = (real_t)4.3633231300000003e-01 - tmp; +tmp = acadoVariables.x[130] + acadoWorkspace.d[126]; +acadoWorkspace.lbA[62] = (real_t)-1.5707963268000000e+00 - tmp; +acadoWorkspace.ubA[62] = (real_t)1.5707963268000000e+00 - tmp; +tmp = acadoVariables.x[131] + acadoWorkspace.d[127]; +acadoWorkspace.lbA[63] = (real_t)-4.3633231300000003e-01 - tmp; +acadoWorkspace.ubA[63] = (real_t)4.3633231300000003e-01 - tmp; +tmp = acadoVariables.x[134] + acadoWorkspace.d[130]; +acadoWorkspace.lbA[64] = (real_t)-1.5707963268000000e+00 - tmp; +acadoWorkspace.ubA[64] = (real_t)1.5707963268000000e+00 - tmp; +tmp = acadoVariables.x[135] + acadoWorkspace.d[131]; +acadoWorkspace.lbA[65] = (real_t)-4.3633231300000003e-01 - tmp; +acadoWorkspace.ubA[65] = (real_t)4.3633231300000003e-01 - tmp; +tmp = acadoVariables.x[138] + acadoWorkspace.d[134]; +acadoWorkspace.lbA[66] = (real_t)-1.5707963268000000e+00 - tmp; +acadoWorkspace.ubA[66] = (real_t)1.5707963268000000e+00 - tmp; +tmp = acadoVariables.x[139] + acadoWorkspace.d[135]; +acadoWorkspace.lbA[67] = (real_t)-4.3633231300000003e-01 - tmp; +acadoWorkspace.ubA[67] = (real_t)4.3633231300000003e-01 - tmp; +tmp = acadoVariables.x[142] + acadoWorkspace.d[138]; +acadoWorkspace.lbA[68] = (real_t)-1.5707963268000000e+00 - tmp; +acadoWorkspace.ubA[68] = (real_t)1.5707963268000000e+00 - tmp; +tmp = acadoVariables.x[143] + acadoWorkspace.d[139]; +acadoWorkspace.lbA[69] = (real_t)-4.3633231300000003e-01 - tmp; +acadoWorkspace.ubA[69] = (real_t)4.3633231300000003e-01 - tmp; +tmp = acadoVariables.x[146] + acadoWorkspace.d[142]; +acadoWorkspace.lbA[70] = (real_t)-1.5707963268000000e+00 - tmp; +acadoWorkspace.ubA[70] = (real_t)1.5707963268000000e+00 - tmp; +tmp = acadoVariables.x[147] + acadoWorkspace.d[143]; +acadoWorkspace.lbA[71] = (real_t)-4.3633231300000003e-01 - tmp; +acadoWorkspace.ubA[71] = (real_t)4.3633231300000003e-01 - tmp; +tmp = acadoVariables.x[150] + acadoWorkspace.d[146]; +acadoWorkspace.lbA[72] = (real_t)-1.5707963268000000e+00 - tmp; +acadoWorkspace.ubA[72] = (real_t)1.5707963268000000e+00 - tmp; +tmp = acadoVariables.x[151] + acadoWorkspace.d[147]; +acadoWorkspace.lbA[73] = (real_t)-4.3633231300000003e-01 - tmp; +acadoWorkspace.ubA[73] = (real_t)4.3633231300000003e-01 - tmp; +tmp = acadoVariables.x[154] + acadoWorkspace.d[150]; +acadoWorkspace.lbA[74] = (real_t)-1.5707963268000000e+00 - tmp; +acadoWorkspace.ubA[74] = (real_t)1.5707963268000000e+00 - tmp; +tmp = acadoVariables.x[155] + acadoWorkspace.d[151]; +acadoWorkspace.lbA[75] = (real_t)-4.3633231300000003e-01 - tmp; +acadoWorkspace.ubA[75] = (real_t)4.3633231300000003e-01 - tmp; +tmp = acadoVariables.x[158] + acadoWorkspace.d[154]; +acadoWorkspace.lbA[76] = (real_t)-1.5707963268000000e+00 - tmp; +acadoWorkspace.ubA[76] = (real_t)1.5707963268000000e+00 - tmp; +tmp = acadoVariables.x[159] + acadoWorkspace.d[155]; +acadoWorkspace.lbA[77] = (real_t)-4.3633231300000003e-01 - tmp; +acadoWorkspace.ubA[77] = (real_t)4.3633231300000003e-01 - tmp; +tmp = acadoVariables.x[162] + acadoWorkspace.d[158]; +acadoWorkspace.lbA[78] = (real_t)-1.5707963268000000e+00 - tmp; +acadoWorkspace.ubA[78] = (real_t)1.5707963268000000e+00 - tmp; +tmp = acadoVariables.x[163] + acadoWorkspace.d[159]; +acadoWorkspace.lbA[79] = (real_t)-4.3633231300000003e-01 - tmp; +acadoWorkspace.ubA[79] = (real_t)4.3633231300000003e-01 - tmp; +tmp = acadoVariables.x[166] + acadoWorkspace.d[162]; +acadoWorkspace.lbA[80] = (real_t)-1.5707963268000000e+00 - tmp; +acadoWorkspace.ubA[80] = (real_t)1.5707963268000000e+00 - tmp; +tmp = acadoVariables.x[167] + acadoWorkspace.d[163]; +acadoWorkspace.lbA[81] = (real_t)-4.3633231300000003e-01 - tmp; +acadoWorkspace.ubA[81] = (real_t)4.3633231300000003e-01 - tmp; +tmp = acadoVariables.x[170] + acadoWorkspace.d[166]; +acadoWorkspace.lbA[82] = (real_t)-1.5707963268000000e+00 - tmp; +acadoWorkspace.ubA[82] = (real_t)1.5707963268000000e+00 - tmp; +tmp = acadoVariables.x[171] + acadoWorkspace.d[167]; +acadoWorkspace.lbA[83] = (real_t)-4.3633231300000003e-01 - tmp; +acadoWorkspace.ubA[83] = (real_t)4.3633231300000003e-01 - tmp; +tmp = acadoVariables.x[174] + acadoWorkspace.d[170]; +acadoWorkspace.lbA[84] = (real_t)-1.5707963268000000e+00 - tmp; +acadoWorkspace.ubA[84] = (real_t)1.5707963268000000e+00 - tmp; +tmp = acadoVariables.x[175] + acadoWorkspace.d[171]; +acadoWorkspace.lbA[85] = (real_t)-4.3633231300000003e-01 - tmp; +acadoWorkspace.ubA[85] = (real_t)4.3633231300000003e-01 - tmp; +tmp = acadoVariables.x[178] + acadoWorkspace.d[174]; +acadoWorkspace.lbA[86] = (real_t)-1.5707963268000000e+00 - tmp; +acadoWorkspace.ubA[86] = (real_t)1.5707963268000000e+00 - tmp; +tmp = acadoVariables.x[179] + acadoWorkspace.d[175]; +acadoWorkspace.lbA[87] = (real_t)-4.3633231300000003e-01 - tmp; +acadoWorkspace.ubA[87] = (real_t)4.3633231300000003e-01 - tmp; +tmp = acadoVariables.x[182] + acadoWorkspace.d[178]; +acadoWorkspace.lbA[88] = (real_t)-1.5707963268000000e+00 - tmp; +acadoWorkspace.ubA[88] = (real_t)1.5707963268000000e+00 - tmp; +tmp = acadoVariables.x[183] + acadoWorkspace.d[179]; +acadoWorkspace.lbA[89] = (real_t)-4.3633231300000003e-01 - tmp; +acadoWorkspace.ubA[89] = (real_t)4.3633231300000003e-01 - tmp; +tmp = acadoVariables.x[186] + acadoWorkspace.d[182]; +acadoWorkspace.lbA[90] = (real_t)-1.5707963268000000e+00 - tmp; +acadoWorkspace.ubA[90] = (real_t)1.5707963268000000e+00 - tmp; +tmp = acadoVariables.x[187] + acadoWorkspace.d[183]; +acadoWorkspace.lbA[91] = (real_t)-4.3633231300000003e-01 - tmp; +acadoWorkspace.ubA[91] = (real_t)4.3633231300000003e-01 - tmp; +tmp = acadoVariables.x[190] + acadoWorkspace.d[186]; +acadoWorkspace.lbA[92] = (real_t)-1.5707963268000000e+00 - tmp; +acadoWorkspace.ubA[92] = (real_t)1.5707963268000000e+00 - tmp; +tmp = acadoVariables.x[191] + acadoWorkspace.d[187]; +acadoWorkspace.lbA[93] = (real_t)-4.3633231300000003e-01 - tmp; +acadoWorkspace.ubA[93] = (real_t)4.3633231300000003e-01 - tmp; +tmp = acadoVariables.x[194] + acadoWorkspace.d[190]; +acadoWorkspace.lbA[94] = (real_t)-1.5707963268000000e+00 - tmp; +acadoWorkspace.ubA[94] = (real_t)1.5707963268000000e+00 - tmp; +tmp = acadoVariables.x[195] + acadoWorkspace.d[191]; +acadoWorkspace.lbA[95] = (real_t)-4.3633231300000003e-01 - tmp; +acadoWorkspace.ubA[95] = (real_t)4.3633231300000003e-01 - tmp; +tmp = acadoVariables.x[198] + acadoWorkspace.d[194]; +acadoWorkspace.lbA[96] = (real_t)-1.5707963268000000e+00 - tmp; +acadoWorkspace.ubA[96] = (real_t)1.5707963268000000e+00 - tmp; +tmp = acadoVariables.x[199] + acadoWorkspace.d[195]; +acadoWorkspace.lbA[97] = (real_t)-4.3633231300000003e-01 - tmp; +acadoWorkspace.ubA[97] = (real_t)4.3633231300000003e-01 - tmp; +tmp = acadoVariables.x[202] + acadoWorkspace.d[198]; +acadoWorkspace.lbA[98] = (real_t)-1.5707963268000000e+00 - tmp; +acadoWorkspace.ubA[98] = (real_t)1.5707963268000000e+00 - tmp; +tmp = acadoVariables.x[203] + acadoWorkspace.d[199]; +acadoWorkspace.lbA[99] = (real_t)-4.3633231300000003e-01 - tmp; +acadoWorkspace.ubA[99] = (real_t)4.3633231300000003e-01 - tmp; + +} + +void acado_expand( ) +{ +int lRun1; +int lRun2; +int lRun3; +acadoVariables.x[0] += acadoWorkspace.x[0]; +acadoVariables.x[1] += acadoWorkspace.x[1]; +acadoVariables.x[2] += acadoWorkspace.x[2]; +acadoVariables.x[3] += acadoWorkspace.x[3]; + +acadoVariables.u[0] += acadoWorkspace.x[4]; +acadoVariables.u[1] += acadoWorkspace.x[5]; +acadoVariables.u[2] += acadoWorkspace.x[6]; +acadoVariables.u[3] += acadoWorkspace.x[7]; +acadoVariables.u[4] += acadoWorkspace.x[8]; +acadoVariables.u[5] += acadoWorkspace.x[9]; +acadoVariables.u[6] += acadoWorkspace.x[10]; +acadoVariables.u[7] += acadoWorkspace.x[11]; +acadoVariables.u[8] += acadoWorkspace.x[12]; +acadoVariables.u[9] += acadoWorkspace.x[13]; +acadoVariables.u[10] += acadoWorkspace.x[14]; +acadoVariables.u[11] += acadoWorkspace.x[15]; +acadoVariables.u[12] += acadoWorkspace.x[16]; +acadoVariables.u[13] += acadoWorkspace.x[17]; +acadoVariables.u[14] += acadoWorkspace.x[18]; +acadoVariables.u[15] += acadoWorkspace.x[19]; +acadoVariables.u[16] += acadoWorkspace.x[20]; +acadoVariables.u[17] += acadoWorkspace.x[21]; +acadoVariables.u[18] += acadoWorkspace.x[22]; +acadoVariables.u[19] += acadoWorkspace.x[23]; +acadoVariables.u[20] += acadoWorkspace.x[24]; +acadoVariables.u[21] += acadoWorkspace.x[25]; +acadoVariables.u[22] += acadoWorkspace.x[26]; +acadoVariables.u[23] += acadoWorkspace.x[27]; +acadoVariables.u[24] += acadoWorkspace.x[28]; +acadoVariables.u[25] += acadoWorkspace.x[29]; +acadoVariables.u[26] += acadoWorkspace.x[30]; +acadoVariables.u[27] += acadoWorkspace.x[31]; +acadoVariables.u[28] += acadoWorkspace.x[32]; +acadoVariables.u[29] += acadoWorkspace.x[33]; +acadoVariables.u[30] += acadoWorkspace.x[34]; +acadoVariables.u[31] += acadoWorkspace.x[35]; +acadoVariables.u[32] += acadoWorkspace.x[36]; +acadoVariables.u[33] += acadoWorkspace.x[37]; +acadoVariables.u[34] += acadoWorkspace.x[38]; +acadoVariables.u[35] += acadoWorkspace.x[39]; +acadoVariables.u[36] += acadoWorkspace.x[40]; +acadoVariables.u[37] += acadoWorkspace.x[41]; +acadoVariables.u[38] += acadoWorkspace.x[42]; +acadoVariables.u[39] += acadoWorkspace.x[43]; +acadoVariables.u[40] += acadoWorkspace.x[44]; +acadoVariables.u[41] += acadoWorkspace.x[45]; +acadoVariables.u[42] += acadoWorkspace.x[46]; +acadoVariables.u[43] += acadoWorkspace.x[47]; +acadoVariables.u[44] += acadoWorkspace.x[48]; +acadoVariables.u[45] += acadoWorkspace.x[49]; +acadoVariables.u[46] += acadoWorkspace.x[50]; +acadoVariables.u[47] += acadoWorkspace.x[51]; +acadoVariables.u[48] += acadoWorkspace.x[52]; +acadoVariables.u[49] += acadoWorkspace.x[53]; + +acadoVariables.x[4] += + acadoWorkspace.evGx[0]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1]*acadoWorkspace.x[1] + acadoWorkspace.evGx[2]*acadoWorkspace.x[2] + acadoWorkspace.evGx[3]*acadoWorkspace.x[3] + acadoWorkspace.d[0]; +acadoVariables.x[5] += + acadoWorkspace.evGx[4]*acadoWorkspace.x[0] + acadoWorkspace.evGx[5]*acadoWorkspace.x[1] + acadoWorkspace.evGx[6]*acadoWorkspace.x[2] + acadoWorkspace.evGx[7]*acadoWorkspace.x[3] + acadoWorkspace.d[1]; +acadoVariables.x[6] += + acadoWorkspace.evGx[8]*acadoWorkspace.x[0] + acadoWorkspace.evGx[9]*acadoWorkspace.x[1] + acadoWorkspace.evGx[10]*acadoWorkspace.x[2] + acadoWorkspace.evGx[11]*acadoWorkspace.x[3] + acadoWorkspace.d[2]; +acadoVariables.x[7] += + acadoWorkspace.evGx[12]*acadoWorkspace.x[0] + acadoWorkspace.evGx[13]*acadoWorkspace.x[1] + acadoWorkspace.evGx[14]*acadoWorkspace.x[2] + acadoWorkspace.evGx[15]*acadoWorkspace.x[3] + acadoWorkspace.d[3]; +acadoVariables.x[8] += + acadoWorkspace.evGx[16]*acadoWorkspace.x[0] + acadoWorkspace.evGx[17]*acadoWorkspace.x[1] + acadoWorkspace.evGx[18]*acadoWorkspace.x[2] + acadoWorkspace.evGx[19]*acadoWorkspace.x[3] + acadoWorkspace.d[4]; +acadoVariables.x[9] += + acadoWorkspace.evGx[20]*acadoWorkspace.x[0] + acadoWorkspace.evGx[21]*acadoWorkspace.x[1] + acadoWorkspace.evGx[22]*acadoWorkspace.x[2] + acadoWorkspace.evGx[23]*acadoWorkspace.x[3] + acadoWorkspace.d[5]; +acadoVariables.x[10] += + acadoWorkspace.evGx[24]*acadoWorkspace.x[0] + acadoWorkspace.evGx[25]*acadoWorkspace.x[1] + acadoWorkspace.evGx[26]*acadoWorkspace.x[2] + acadoWorkspace.evGx[27]*acadoWorkspace.x[3] + acadoWorkspace.d[6]; +acadoVariables.x[11] += + acadoWorkspace.evGx[28]*acadoWorkspace.x[0] + acadoWorkspace.evGx[29]*acadoWorkspace.x[1] + acadoWorkspace.evGx[30]*acadoWorkspace.x[2] + acadoWorkspace.evGx[31]*acadoWorkspace.x[3] + acadoWorkspace.d[7]; +acadoVariables.x[12] += + acadoWorkspace.evGx[32]*acadoWorkspace.x[0] + acadoWorkspace.evGx[33]*acadoWorkspace.x[1] + acadoWorkspace.evGx[34]*acadoWorkspace.x[2] + acadoWorkspace.evGx[35]*acadoWorkspace.x[3] + acadoWorkspace.d[8]; +acadoVariables.x[13] += + acadoWorkspace.evGx[36]*acadoWorkspace.x[0] + acadoWorkspace.evGx[37]*acadoWorkspace.x[1] + acadoWorkspace.evGx[38]*acadoWorkspace.x[2] + acadoWorkspace.evGx[39]*acadoWorkspace.x[3] + acadoWorkspace.d[9]; +acadoVariables.x[14] += + acadoWorkspace.evGx[40]*acadoWorkspace.x[0] + acadoWorkspace.evGx[41]*acadoWorkspace.x[1] + acadoWorkspace.evGx[42]*acadoWorkspace.x[2] + acadoWorkspace.evGx[43]*acadoWorkspace.x[3] + acadoWorkspace.d[10]; +acadoVariables.x[15] += + acadoWorkspace.evGx[44]*acadoWorkspace.x[0] + acadoWorkspace.evGx[45]*acadoWorkspace.x[1] + acadoWorkspace.evGx[46]*acadoWorkspace.x[2] + acadoWorkspace.evGx[47]*acadoWorkspace.x[3] + acadoWorkspace.d[11]; +acadoVariables.x[16] += + acadoWorkspace.evGx[48]*acadoWorkspace.x[0] + acadoWorkspace.evGx[49]*acadoWorkspace.x[1] + acadoWorkspace.evGx[50]*acadoWorkspace.x[2] + acadoWorkspace.evGx[51]*acadoWorkspace.x[3] + acadoWorkspace.d[12]; +acadoVariables.x[17] += + acadoWorkspace.evGx[52]*acadoWorkspace.x[0] + acadoWorkspace.evGx[53]*acadoWorkspace.x[1] + acadoWorkspace.evGx[54]*acadoWorkspace.x[2] + acadoWorkspace.evGx[55]*acadoWorkspace.x[3] + acadoWorkspace.d[13]; +acadoVariables.x[18] += + acadoWorkspace.evGx[56]*acadoWorkspace.x[0] + acadoWorkspace.evGx[57]*acadoWorkspace.x[1] + acadoWorkspace.evGx[58]*acadoWorkspace.x[2] + acadoWorkspace.evGx[59]*acadoWorkspace.x[3] + acadoWorkspace.d[14]; +acadoVariables.x[19] += + acadoWorkspace.evGx[60]*acadoWorkspace.x[0] + acadoWorkspace.evGx[61]*acadoWorkspace.x[1] + acadoWorkspace.evGx[62]*acadoWorkspace.x[2] + acadoWorkspace.evGx[63]*acadoWorkspace.x[3] + acadoWorkspace.d[15]; +acadoVariables.x[20] += + acadoWorkspace.evGx[64]*acadoWorkspace.x[0] + acadoWorkspace.evGx[65]*acadoWorkspace.x[1] + acadoWorkspace.evGx[66]*acadoWorkspace.x[2] + acadoWorkspace.evGx[67]*acadoWorkspace.x[3] + acadoWorkspace.d[16]; +acadoVariables.x[21] += + acadoWorkspace.evGx[68]*acadoWorkspace.x[0] + acadoWorkspace.evGx[69]*acadoWorkspace.x[1] + acadoWorkspace.evGx[70]*acadoWorkspace.x[2] + acadoWorkspace.evGx[71]*acadoWorkspace.x[3] + acadoWorkspace.d[17]; +acadoVariables.x[22] += + acadoWorkspace.evGx[72]*acadoWorkspace.x[0] + acadoWorkspace.evGx[73]*acadoWorkspace.x[1] + acadoWorkspace.evGx[74]*acadoWorkspace.x[2] + acadoWorkspace.evGx[75]*acadoWorkspace.x[3] + acadoWorkspace.d[18]; +acadoVariables.x[23] += + acadoWorkspace.evGx[76]*acadoWorkspace.x[0] + acadoWorkspace.evGx[77]*acadoWorkspace.x[1] + acadoWorkspace.evGx[78]*acadoWorkspace.x[2] + acadoWorkspace.evGx[79]*acadoWorkspace.x[3] + acadoWorkspace.d[19]; +acadoVariables.x[24] += + acadoWorkspace.evGx[80]*acadoWorkspace.x[0] + acadoWorkspace.evGx[81]*acadoWorkspace.x[1] + acadoWorkspace.evGx[82]*acadoWorkspace.x[2] + acadoWorkspace.evGx[83]*acadoWorkspace.x[3] + acadoWorkspace.d[20]; +acadoVariables.x[25] += + acadoWorkspace.evGx[84]*acadoWorkspace.x[0] + acadoWorkspace.evGx[85]*acadoWorkspace.x[1] + acadoWorkspace.evGx[86]*acadoWorkspace.x[2] + acadoWorkspace.evGx[87]*acadoWorkspace.x[3] + acadoWorkspace.d[21]; +acadoVariables.x[26] += + acadoWorkspace.evGx[88]*acadoWorkspace.x[0] + acadoWorkspace.evGx[89]*acadoWorkspace.x[1] + acadoWorkspace.evGx[90]*acadoWorkspace.x[2] + acadoWorkspace.evGx[91]*acadoWorkspace.x[3] + acadoWorkspace.d[22]; +acadoVariables.x[27] += + acadoWorkspace.evGx[92]*acadoWorkspace.x[0] + acadoWorkspace.evGx[93]*acadoWorkspace.x[1] + acadoWorkspace.evGx[94]*acadoWorkspace.x[2] + acadoWorkspace.evGx[95]*acadoWorkspace.x[3] + acadoWorkspace.d[23]; +acadoVariables.x[28] += + acadoWorkspace.evGx[96]*acadoWorkspace.x[0] + acadoWorkspace.evGx[97]*acadoWorkspace.x[1] + acadoWorkspace.evGx[98]*acadoWorkspace.x[2] + acadoWorkspace.evGx[99]*acadoWorkspace.x[3] + acadoWorkspace.d[24]; +acadoVariables.x[29] += + acadoWorkspace.evGx[100]*acadoWorkspace.x[0] + acadoWorkspace.evGx[101]*acadoWorkspace.x[1] + acadoWorkspace.evGx[102]*acadoWorkspace.x[2] + acadoWorkspace.evGx[103]*acadoWorkspace.x[3] + acadoWorkspace.d[25]; +acadoVariables.x[30] += + acadoWorkspace.evGx[104]*acadoWorkspace.x[0] + acadoWorkspace.evGx[105]*acadoWorkspace.x[1] + acadoWorkspace.evGx[106]*acadoWorkspace.x[2] + acadoWorkspace.evGx[107]*acadoWorkspace.x[3] + acadoWorkspace.d[26]; +acadoVariables.x[31] += + acadoWorkspace.evGx[108]*acadoWorkspace.x[0] + acadoWorkspace.evGx[109]*acadoWorkspace.x[1] + acadoWorkspace.evGx[110]*acadoWorkspace.x[2] + acadoWorkspace.evGx[111]*acadoWorkspace.x[3] + acadoWorkspace.d[27]; +acadoVariables.x[32] += + acadoWorkspace.evGx[112]*acadoWorkspace.x[0] + acadoWorkspace.evGx[113]*acadoWorkspace.x[1] + acadoWorkspace.evGx[114]*acadoWorkspace.x[2] + acadoWorkspace.evGx[115]*acadoWorkspace.x[3] + acadoWorkspace.d[28]; +acadoVariables.x[33] += + acadoWorkspace.evGx[116]*acadoWorkspace.x[0] + acadoWorkspace.evGx[117]*acadoWorkspace.x[1] + acadoWorkspace.evGx[118]*acadoWorkspace.x[2] + acadoWorkspace.evGx[119]*acadoWorkspace.x[3] + acadoWorkspace.d[29]; +acadoVariables.x[34] += + acadoWorkspace.evGx[120]*acadoWorkspace.x[0] + acadoWorkspace.evGx[121]*acadoWorkspace.x[1] + acadoWorkspace.evGx[122]*acadoWorkspace.x[2] + acadoWorkspace.evGx[123]*acadoWorkspace.x[3] + acadoWorkspace.d[30]; +acadoVariables.x[35] += + acadoWorkspace.evGx[124]*acadoWorkspace.x[0] + acadoWorkspace.evGx[125]*acadoWorkspace.x[1] + acadoWorkspace.evGx[126]*acadoWorkspace.x[2] + acadoWorkspace.evGx[127]*acadoWorkspace.x[3] + acadoWorkspace.d[31]; +acadoVariables.x[36] += + acadoWorkspace.evGx[128]*acadoWorkspace.x[0] + acadoWorkspace.evGx[129]*acadoWorkspace.x[1] + acadoWorkspace.evGx[130]*acadoWorkspace.x[2] + acadoWorkspace.evGx[131]*acadoWorkspace.x[3] + acadoWorkspace.d[32]; +acadoVariables.x[37] += + acadoWorkspace.evGx[132]*acadoWorkspace.x[0] + acadoWorkspace.evGx[133]*acadoWorkspace.x[1] + acadoWorkspace.evGx[134]*acadoWorkspace.x[2] + acadoWorkspace.evGx[135]*acadoWorkspace.x[3] + acadoWorkspace.d[33]; +acadoVariables.x[38] += + acadoWorkspace.evGx[136]*acadoWorkspace.x[0] + acadoWorkspace.evGx[137]*acadoWorkspace.x[1] + acadoWorkspace.evGx[138]*acadoWorkspace.x[2] + acadoWorkspace.evGx[139]*acadoWorkspace.x[3] + acadoWorkspace.d[34]; +acadoVariables.x[39] += + acadoWorkspace.evGx[140]*acadoWorkspace.x[0] + acadoWorkspace.evGx[141]*acadoWorkspace.x[1] + acadoWorkspace.evGx[142]*acadoWorkspace.x[2] + acadoWorkspace.evGx[143]*acadoWorkspace.x[3] + acadoWorkspace.d[35]; +acadoVariables.x[40] += + acadoWorkspace.evGx[144]*acadoWorkspace.x[0] + acadoWorkspace.evGx[145]*acadoWorkspace.x[1] + acadoWorkspace.evGx[146]*acadoWorkspace.x[2] + acadoWorkspace.evGx[147]*acadoWorkspace.x[3] + acadoWorkspace.d[36]; +acadoVariables.x[41] += + acadoWorkspace.evGx[148]*acadoWorkspace.x[0] + acadoWorkspace.evGx[149]*acadoWorkspace.x[1] + acadoWorkspace.evGx[150]*acadoWorkspace.x[2] + acadoWorkspace.evGx[151]*acadoWorkspace.x[3] + acadoWorkspace.d[37]; +acadoVariables.x[42] += + acadoWorkspace.evGx[152]*acadoWorkspace.x[0] + acadoWorkspace.evGx[153]*acadoWorkspace.x[1] + acadoWorkspace.evGx[154]*acadoWorkspace.x[2] + acadoWorkspace.evGx[155]*acadoWorkspace.x[3] + acadoWorkspace.d[38]; +acadoVariables.x[43] += + acadoWorkspace.evGx[156]*acadoWorkspace.x[0] + acadoWorkspace.evGx[157]*acadoWorkspace.x[1] + acadoWorkspace.evGx[158]*acadoWorkspace.x[2] + acadoWorkspace.evGx[159]*acadoWorkspace.x[3] + acadoWorkspace.d[39]; +acadoVariables.x[44] += + acadoWorkspace.evGx[160]*acadoWorkspace.x[0] + acadoWorkspace.evGx[161]*acadoWorkspace.x[1] + acadoWorkspace.evGx[162]*acadoWorkspace.x[2] + acadoWorkspace.evGx[163]*acadoWorkspace.x[3] + acadoWorkspace.d[40]; +acadoVariables.x[45] += + acadoWorkspace.evGx[164]*acadoWorkspace.x[0] + acadoWorkspace.evGx[165]*acadoWorkspace.x[1] + acadoWorkspace.evGx[166]*acadoWorkspace.x[2] + acadoWorkspace.evGx[167]*acadoWorkspace.x[3] + acadoWorkspace.d[41]; +acadoVariables.x[46] += + acadoWorkspace.evGx[168]*acadoWorkspace.x[0] + acadoWorkspace.evGx[169]*acadoWorkspace.x[1] + acadoWorkspace.evGx[170]*acadoWorkspace.x[2] + acadoWorkspace.evGx[171]*acadoWorkspace.x[3] + acadoWorkspace.d[42]; +acadoVariables.x[47] += + acadoWorkspace.evGx[172]*acadoWorkspace.x[0] + acadoWorkspace.evGx[173]*acadoWorkspace.x[1] + acadoWorkspace.evGx[174]*acadoWorkspace.x[2] + acadoWorkspace.evGx[175]*acadoWorkspace.x[3] + acadoWorkspace.d[43]; +acadoVariables.x[48] += + acadoWorkspace.evGx[176]*acadoWorkspace.x[0] + acadoWorkspace.evGx[177]*acadoWorkspace.x[1] + acadoWorkspace.evGx[178]*acadoWorkspace.x[2] + acadoWorkspace.evGx[179]*acadoWorkspace.x[3] + acadoWorkspace.d[44]; +acadoVariables.x[49] += + acadoWorkspace.evGx[180]*acadoWorkspace.x[0] + acadoWorkspace.evGx[181]*acadoWorkspace.x[1] + acadoWorkspace.evGx[182]*acadoWorkspace.x[2] + acadoWorkspace.evGx[183]*acadoWorkspace.x[3] + acadoWorkspace.d[45]; +acadoVariables.x[50] += + acadoWorkspace.evGx[184]*acadoWorkspace.x[0] + acadoWorkspace.evGx[185]*acadoWorkspace.x[1] + acadoWorkspace.evGx[186]*acadoWorkspace.x[2] + acadoWorkspace.evGx[187]*acadoWorkspace.x[3] + acadoWorkspace.d[46]; +acadoVariables.x[51] += + acadoWorkspace.evGx[188]*acadoWorkspace.x[0] + acadoWorkspace.evGx[189]*acadoWorkspace.x[1] + acadoWorkspace.evGx[190]*acadoWorkspace.x[2] + acadoWorkspace.evGx[191]*acadoWorkspace.x[3] + acadoWorkspace.d[47]; +acadoVariables.x[52] += + acadoWorkspace.evGx[192]*acadoWorkspace.x[0] + acadoWorkspace.evGx[193]*acadoWorkspace.x[1] + acadoWorkspace.evGx[194]*acadoWorkspace.x[2] + acadoWorkspace.evGx[195]*acadoWorkspace.x[3] + acadoWorkspace.d[48]; +acadoVariables.x[53] += + acadoWorkspace.evGx[196]*acadoWorkspace.x[0] + acadoWorkspace.evGx[197]*acadoWorkspace.x[1] + acadoWorkspace.evGx[198]*acadoWorkspace.x[2] + acadoWorkspace.evGx[199]*acadoWorkspace.x[3] + acadoWorkspace.d[49]; +acadoVariables.x[54] += + acadoWorkspace.evGx[200]*acadoWorkspace.x[0] + acadoWorkspace.evGx[201]*acadoWorkspace.x[1] + acadoWorkspace.evGx[202]*acadoWorkspace.x[2] + acadoWorkspace.evGx[203]*acadoWorkspace.x[3] + acadoWorkspace.d[50]; +acadoVariables.x[55] += + acadoWorkspace.evGx[204]*acadoWorkspace.x[0] + acadoWorkspace.evGx[205]*acadoWorkspace.x[1] + acadoWorkspace.evGx[206]*acadoWorkspace.x[2] + acadoWorkspace.evGx[207]*acadoWorkspace.x[3] + acadoWorkspace.d[51]; +acadoVariables.x[56] += + acadoWorkspace.evGx[208]*acadoWorkspace.x[0] + acadoWorkspace.evGx[209]*acadoWorkspace.x[1] + acadoWorkspace.evGx[210]*acadoWorkspace.x[2] + acadoWorkspace.evGx[211]*acadoWorkspace.x[3] + acadoWorkspace.d[52]; +acadoVariables.x[57] += + acadoWorkspace.evGx[212]*acadoWorkspace.x[0] + acadoWorkspace.evGx[213]*acadoWorkspace.x[1] + acadoWorkspace.evGx[214]*acadoWorkspace.x[2] + acadoWorkspace.evGx[215]*acadoWorkspace.x[3] + acadoWorkspace.d[53]; +acadoVariables.x[58] += + acadoWorkspace.evGx[216]*acadoWorkspace.x[0] + acadoWorkspace.evGx[217]*acadoWorkspace.x[1] + acadoWorkspace.evGx[218]*acadoWorkspace.x[2] + acadoWorkspace.evGx[219]*acadoWorkspace.x[3] + acadoWorkspace.d[54]; +acadoVariables.x[59] += + acadoWorkspace.evGx[220]*acadoWorkspace.x[0] + acadoWorkspace.evGx[221]*acadoWorkspace.x[1] + acadoWorkspace.evGx[222]*acadoWorkspace.x[2] + acadoWorkspace.evGx[223]*acadoWorkspace.x[3] + acadoWorkspace.d[55]; +acadoVariables.x[60] += + acadoWorkspace.evGx[224]*acadoWorkspace.x[0] + acadoWorkspace.evGx[225]*acadoWorkspace.x[1] + acadoWorkspace.evGx[226]*acadoWorkspace.x[2] + acadoWorkspace.evGx[227]*acadoWorkspace.x[3] + acadoWorkspace.d[56]; +acadoVariables.x[61] += + acadoWorkspace.evGx[228]*acadoWorkspace.x[0] + acadoWorkspace.evGx[229]*acadoWorkspace.x[1] + acadoWorkspace.evGx[230]*acadoWorkspace.x[2] + acadoWorkspace.evGx[231]*acadoWorkspace.x[3] + acadoWorkspace.d[57]; +acadoVariables.x[62] += + acadoWorkspace.evGx[232]*acadoWorkspace.x[0] + acadoWorkspace.evGx[233]*acadoWorkspace.x[1] + acadoWorkspace.evGx[234]*acadoWorkspace.x[2] + acadoWorkspace.evGx[235]*acadoWorkspace.x[3] + acadoWorkspace.d[58]; +acadoVariables.x[63] += + acadoWorkspace.evGx[236]*acadoWorkspace.x[0] + acadoWorkspace.evGx[237]*acadoWorkspace.x[1] + acadoWorkspace.evGx[238]*acadoWorkspace.x[2] + acadoWorkspace.evGx[239]*acadoWorkspace.x[3] + acadoWorkspace.d[59]; +acadoVariables.x[64] += + acadoWorkspace.evGx[240]*acadoWorkspace.x[0] + acadoWorkspace.evGx[241]*acadoWorkspace.x[1] + acadoWorkspace.evGx[242]*acadoWorkspace.x[2] + acadoWorkspace.evGx[243]*acadoWorkspace.x[3] + acadoWorkspace.d[60]; +acadoVariables.x[65] += + acadoWorkspace.evGx[244]*acadoWorkspace.x[0] + acadoWorkspace.evGx[245]*acadoWorkspace.x[1] + acadoWorkspace.evGx[246]*acadoWorkspace.x[2] + acadoWorkspace.evGx[247]*acadoWorkspace.x[3] + acadoWorkspace.d[61]; +acadoVariables.x[66] += + acadoWorkspace.evGx[248]*acadoWorkspace.x[0] + acadoWorkspace.evGx[249]*acadoWorkspace.x[1] + acadoWorkspace.evGx[250]*acadoWorkspace.x[2] + acadoWorkspace.evGx[251]*acadoWorkspace.x[3] + acadoWorkspace.d[62]; +acadoVariables.x[67] += + acadoWorkspace.evGx[252]*acadoWorkspace.x[0] + acadoWorkspace.evGx[253]*acadoWorkspace.x[1] + acadoWorkspace.evGx[254]*acadoWorkspace.x[2] + acadoWorkspace.evGx[255]*acadoWorkspace.x[3] + acadoWorkspace.d[63]; +acadoVariables.x[68] += + acadoWorkspace.evGx[256]*acadoWorkspace.x[0] + acadoWorkspace.evGx[257]*acadoWorkspace.x[1] + acadoWorkspace.evGx[258]*acadoWorkspace.x[2] + acadoWorkspace.evGx[259]*acadoWorkspace.x[3] + acadoWorkspace.d[64]; +acadoVariables.x[69] += + acadoWorkspace.evGx[260]*acadoWorkspace.x[0] + acadoWorkspace.evGx[261]*acadoWorkspace.x[1] + acadoWorkspace.evGx[262]*acadoWorkspace.x[2] + acadoWorkspace.evGx[263]*acadoWorkspace.x[3] + acadoWorkspace.d[65]; +acadoVariables.x[70] += + acadoWorkspace.evGx[264]*acadoWorkspace.x[0] + acadoWorkspace.evGx[265]*acadoWorkspace.x[1] + acadoWorkspace.evGx[266]*acadoWorkspace.x[2] + acadoWorkspace.evGx[267]*acadoWorkspace.x[3] + acadoWorkspace.d[66]; +acadoVariables.x[71] += + acadoWorkspace.evGx[268]*acadoWorkspace.x[0] + acadoWorkspace.evGx[269]*acadoWorkspace.x[1] + acadoWorkspace.evGx[270]*acadoWorkspace.x[2] + acadoWorkspace.evGx[271]*acadoWorkspace.x[3] + acadoWorkspace.d[67]; +acadoVariables.x[72] += + acadoWorkspace.evGx[272]*acadoWorkspace.x[0] + acadoWorkspace.evGx[273]*acadoWorkspace.x[1] + acadoWorkspace.evGx[274]*acadoWorkspace.x[2] + acadoWorkspace.evGx[275]*acadoWorkspace.x[3] + acadoWorkspace.d[68]; +acadoVariables.x[73] += + acadoWorkspace.evGx[276]*acadoWorkspace.x[0] + acadoWorkspace.evGx[277]*acadoWorkspace.x[1] + acadoWorkspace.evGx[278]*acadoWorkspace.x[2] + acadoWorkspace.evGx[279]*acadoWorkspace.x[3] + acadoWorkspace.d[69]; +acadoVariables.x[74] += + acadoWorkspace.evGx[280]*acadoWorkspace.x[0] + acadoWorkspace.evGx[281]*acadoWorkspace.x[1] + acadoWorkspace.evGx[282]*acadoWorkspace.x[2] + acadoWorkspace.evGx[283]*acadoWorkspace.x[3] + acadoWorkspace.d[70]; +acadoVariables.x[75] += + acadoWorkspace.evGx[284]*acadoWorkspace.x[0] + acadoWorkspace.evGx[285]*acadoWorkspace.x[1] + acadoWorkspace.evGx[286]*acadoWorkspace.x[2] + acadoWorkspace.evGx[287]*acadoWorkspace.x[3] + acadoWorkspace.d[71]; +acadoVariables.x[76] += + acadoWorkspace.evGx[288]*acadoWorkspace.x[0] + acadoWorkspace.evGx[289]*acadoWorkspace.x[1] + acadoWorkspace.evGx[290]*acadoWorkspace.x[2] + acadoWorkspace.evGx[291]*acadoWorkspace.x[3] + acadoWorkspace.d[72]; +acadoVariables.x[77] += + acadoWorkspace.evGx[292]*acadoWorkspace.x[0] + acadoWorkspace.evGx[293]*acadoWorkspace.x[1] + acadoWorkspace.evGx[294]*acadoWorkspace.x[2] + acadoWorkspace.evGx[295]*acadoWorkspace.x[3] + acadoWorkspace.d[73]; +acadoVariables.x[78] += + acadoWorkspace.evGx[296]*acadoWorkspace.x[0] + acadoWorkspace.evGx[297]*acadoWorkspace.x[1] + acadoWorkspace.evGx[298]*acadoWorkspace.x[2] + acadoWorkspace.evGx[299]*acadoWorkspace.x[3] + acadoWorkspace.d[74]; +acadoVariables.x[79] += + acadoWorkspace.evGx[300]*acadoWorkspace.x[0] + acadoWorkspace.evGx[301]*acadoWorkspace.x[1] + acadoWorkspace.evGx[302]*acadoWorkspace.x[2] + acadoWorkspace.evGx[303]*acadoWorkspace.x[3] + acadoWorkspace.d[75]; +acadoVariables.x[80] += + acadoWorkspace.evGx[304]*acadoWorkspace.x[0] + acadoWorkspace.evGx[305]*acadoWorkspace.x[1] + acadoWorkspace.evGx[306]*acadoWorkspace.x[2] + acadoWorkspace.evGx[307]*acadoWorkspace.x[3] + acadoWorkspace.d[76]; +acadoVariables.x[81] += + acadoWorkspace.evGx[308]*acadoWorkspace.x[0] + acadoWorkspace.evGx[309]*acadoWorkspace.x[1] + acadoWorkspace.evGx[310]*acadoWorkspace.x[2] + acadoWorkspace.evGx[311]*acadoWorkspace.x[3] + acadoWorkspace.d[77]; +acadoVariables.x[82] += + acadoWorkspace.evGx[312]*acadoWorkspace.x[0] + acadoWorkspace.evGx[313]*acadoWorkspace.x[1] + acadoWorkspace.evGx[314]*acadoWorkspace.x[2] + acadoWorkspace.evGx[315]*acadoWorkspace.x[3] + acadoWorkspace.d[78]; +acadoVariables.x[83] += + acadoWorkspace.evGx[316]*acadoWorkspace.x[0] + acadoWorkspace.evGx[317]*acadoWorkspace.x[1] + acadoWorkspace.evGx[318]*acadoWorkspace.x[2] + acadoWorkspace.evGx[319]*acadoWorkspace.x[3] + acadoWorkspace.d[79]; +acadoVariables.x[84] += + acadoWorkspace.evGx[320]*acadoWorkspace.x[0] + acadoWorkspace.evGx[321]*acadoWorkspace.x[1] + acadoWorkspace.evGx[322]*acadoWorkspace.x[2] + acadoWorkspace.evGx[323]*acadoWorkspace.x[3] + acadoWorkspace.d[80]; +acadoVariables.x[85] += + acadoWorkspace.evGx[324]*acadoWorkspace.x[0] + acadoWorkspace.evGx[325]*acadoWorkspace.x[1] + acadoWorkspace.evGx[326]*acadoWorkspace.x[2] + acadoWorkspace.evGx[327]*acadoWorkspace.x[3] + acadoWorkspace.d[81]; +acadoVariables.x[86] += + acadoWorkspace.evGx[328]*acadoWorkspace.x[0] + acadoWorkspace.evGx[329]*acadoWorkspace.x[1] + acadoWorkspace.evGx[330]*acadoWorkspace.x[2] + acadoWorkspace.evGx[331]*acadoWorkspace.x[3] + acadoWorkspace.d[82]; +acadoVariables.x[87] += + acadoWorkspace.evGx[332]*acadoWorkspace.x[0] + acadoWorkspace.evGx[333]*acadoWorkspace.x[1] + acadoWorkspace.evGx[334]*acadoWorkspace.x[2] + acadoWorkspace.evGx[335]*acadoWorkspace.x[3] + acadoWorkspace.d[83]; +acadoVariables.x[88] += + acadoWorkspace.evGx[336]*acadoWorkspace.x[0] + acadoWorkspace.evGx[337]*acadoWorkspace.x[1] + acadoWorkspace.evGx[338]*acadoWorkspace.x[2] + acadoWorkspace.evGx[339]*acadoWorkspace.x[3] + acadoWorkspace.d[84]; +acadoVariables.x[89] += + acadoWorkspace.evGx[340]*acadoWorkspace.x[0] + acadoWorkspace.evGx[341]*acadoWorkspace.x[1] + acadoWorkspace.evGx[342]*acadoWorkspace.x[2] + acadoWorkspace.evGx[343]*acadoWorkspace.x[3] + acadoWorkspace.d[85]; +acadoVariables.x[90] += + acadoWorkspace.evGx[344]*acadoWorkspace.x[0] + acadoWorkspace.evGx[345]*acadoWorkspace.x[1] + acadoWorkspace.evGx[346]*acadoWorkspace.x[2] + acadoWorkspace.evGx[347]*acadoWorkspace.x[3] + acadoWorkspace.d[86]; +acadoVariables.x[91] += + acadoWorkspace.evGx[348]*acadoWorkspace.x[0] + acadoWorkspace.evGx[349]*acadoWorkspace.x[1] + acadoWorkspace.evGx[350]*acadoWorkspace.x[2] + acadoWorkspace.evGx[351]*acadoWorkspace.x[3] + acadoWorkspace.d[87]; +acadoVariables.x[92] += + acadoWorkspace.evGx[352]*acadoWorkspace.x[0] + acadoWorkspace.evGx[353]*acadoWorkspace.x[1] + acadoWorkspace.evGx[354]*acadoWorkspace.x[2] + acadoWorkspace.evGx[355]*acadoWorkspace.x[3] + acadoWorkspace.d[88]; +acadoVariables.x[93] += + acadoWorkspace.evGx[356]*acadoWorkspace.x[0] + acadoWorkspace.evGx[357]*acadoWorkspace.x[1] + acadoWorkspace.evGx[358]*acadoWorkspace.x[2] + acadoWorkspace.evGx[359]*acadoWorkspace.x[3] + acadoWorkspace.d[89]; +acadoVariables.x[94] += + acadoWorkspace.evGx[360]*acadoWorkspace.x[0] + acadoWorkspace.evGx[361]*acadoWorkspace.x[1] + acadoWorkspace.evGx[362]*acadoWorkspace.x[2] + acadoWorkspace.evGx[363]*acadoWorkspace.x[3] + acadoWorkspace.d[90]; +acadoVariables.x[95] += + acadoWorkspace.evGx[364]*acadoWorkspace.x[0] + acadoWorkspace.evGx[365]*acadoWorkspace.x[1] + acadoWorkspace.evGx[366]*acadoWorkspace.x[2] + acadoWorkspace.evGx[367]*acadoWorkspace.x[3] + acadoWorkspace.d[91]; +acadoVariables.x[96] += + acadoWorkspace.evGx[368]*acadoWorkspace.x[0] + acadoWorkspace.evGx[369]*acadoWorkspace.x[1] + acadoWorkspace.evGx[370]*acadoWorkspace.x[2] + acadoWorkspace.evGx[371]*acadoWorkspace.x[3] + acadoWorkspace.d[92]; +acadoVariables.x[97] += + acadoWorkspace.evGx[372]*acadoWorkspace.x[0] + acadoWorkspace.evGx[373]*acadoWorkspace.x[1] + acadoWorkspace.evGx[374]*acadoWorkspace.x[2] + acadoWorkspace.evGx[375]*acadoWorkspace.x[3] + acadoWorkspace.d[93]; +acadoVariables.x[98] += + acadoWorkspace.evGx[376]*acadoWorkspace.x[0] + acadoWorkspace.evGx[377]*acadoWorkspace.x[1] + acadoWorkspace.evGx[378]*acadoWorkspace.x[2] + acadoWorkspace.evGx[379]*acadoWorkspace.x[3] + acadoWorkspace.d[94]; +acadoVariables.x[99] += + acadoWorkspace.evGx[380]*acadoWorkspace.x[0] + acadoWorkspace.evGx[381]*acadoWorkspace.x[1] + acadoWorkspace.evGx[382]*acadoWorkspace.x[2] + acadoWorkspace.evGx[383]*acadoWorkspace.x[3] + acadoWorkspace.d[95]; +acadoVariables.x[100] += + acadoWorkspace.evGx[384]*acadoWorkspace.x[0] + acadoWorkspace.evGx[385]*acadoWorkspace.x[1] + acadoWorkspace.evGx[386]*acadoWorkspace.x[2] + acadoWorkspace.evGx[387]*acadoWorkspace.x[3] + acadoWorkspace.d[96]; +acadoVariables.x[101] += + acadoWorkspace.evGx[388]*acadoWorkspace.x[0] + acadoWorkspace.evGx[389]*acadoWorkspace.x[1] + acadoWorkspace.evGx[390]*acadoWorkspace.x[2] + acadoWorkspace.evGx[391]*acadoWorkspace.x[3] + acadoWorkspace.d[97]; +acadoVariables.x[102] += + acadoWorkspace.evGx[392]*acadoWorkspace.x[0] + acadoWorkspace.evGx[393]*acadoWorkspace.x[1] + acadoWorkspace.evGx[394]*acadoWorkspace.x[2] + acadoWorkspace.evGx[395]*acadoWorkspace.x[3] + acadoWorkspace.d[98]; +acadoVariables.x[103] += + acadoWorkspace.evGx[396]*acadoWorkspace.x[0] + acadoWorkspace.evGx[397]*acadoWorkspace.x[1] + acadoWorkspace.evGx[398]*acadoWorkspace.x[2] + acadoWorkspace.evGx[399]*acadoWorkspace.x[3] + acadoWorkspace.d[99]; +acadoVariables.x[104] += + acadoWorkspace.evGx[400]*acadoWorkspace.x[0] + acadoWorkspace.evGx[401]*acadoWorkspace.x[1] + acadoWorkspace.evGx[402]*acadoWorkspace.x[2] + acadoWorkspace.evGx[403]*acadoWorkspace.x[3] + acadoWorkspace.d[100]; +acadoVariables.x[105] += + acadoWorkspace.evGx[404]*acadoWorkspace.x[0] + acadoWorkspace.evGx[405]*acadoWorkspace.x[1] + acadoWorkspace.evGx[406]*acadoWorkspace.x[2] + acadoWorkspace.evGx[407]*acadoWorkspace.x[3] + acadoWorkspace.d[101]; +acadoVariables.x[106] += + acadoWorkspace.evGx[408]*acadoWorkspace.x[0] + acadoWorkspace.evGx[409]*acadoWorkspace.x[1] + acadoWorkspace.evGx[410]*acadoWorkspace.x[2] + acadoWorkspace.evGx[411]*acadoWorkspace.x[3] + acadoWorkspace.d[102]; +acadoVariables.x[107] += + acadoWorkspace.evGx[412]*acadoWorkspace.x[0] + acadoWorkspace.evGx[413]*acadoWorkspace.x[1] + acadoWorkspace.evGx[414]*acadoWorkspace.x[2] + acadoWorkspace.evGx[415]*acadoWorkspace.x[3] + acadoWorkspace.d[103]; +acadoVariables.x[108] += + acadoWorkspace.evGx[416]*acadoWorkspace.x[0] + acadoWorkspace.evGx[417]*acadoWorkspace.x[1] + acadoWorkspace.evGx[418]*acadoWorkspace.x[2] + acadoWorkspace.evGx[419]*acadoWorkspace.x[3] + acadoWorkspace.d[104]; +acadoVariables.x[109] += + acadoWorkspace.evGx[420]*acadoWorkspace.x[0] + acadoWorkspace.evGx[421]*acadoWorkspace.x[1] + acadoWorkspace.evGx[422]*acadoWorkspace.x[2] + acadoWorkspace.evGx[423]*acadoWorkspace.x[3] + acadoWorkspace.d[105]; +acadoVariables.x[110] += + acadoWorkspace.evGx[424]*acadoWorkspace.x[0] + acadoWorkspace.evGx[425]*acadoWorkspace.x[1] + acadoWorkspace.evGx[426]*acadoWorkspace.x[2] + acadoWorkspace.evGx[427]*acadoWorkspace.x[3] + acadoWorkspace.d[106]; +acadoVariables.x[111] += + acadoWorkspace.evGx[428]*acadoWorkspace.x[0] + acadoWorkspace.evGx[429]*acadoWorkspace.x[1] + acadoWorkspace.evGx[430]*acadoWorkspace.x[2] + acadoWorkspace.evGx[431]*acadoWorkspace.x[3] + acadoWorkspace.d[107]; +acadoVariables.x[112] += + acadoWorkspace.evGx[432]*acadoWorkspace.x[0] + acadoWorkspace.evGx[433]*acadoWorkspace.x[1] + acadoWorkspace.evGx[434]*acadoWorkspace.x[2] + acadoWorkspace.evGx[435]*acadoWorkspace.x[3] + acadoWorkspace.d[108]; +acadoVariables.x[113] += + acadoWorkspace.evGx[436]*acadoWorkspace.x[0] + acadoWorkspace.evGx[437]*acadoWorkspace.x[1] + acadoWorkspace.evGx[438]*acadoWorkspace.x[2] + acadoWorkspace.evGx[439]*acadoWorkspace.x[3] + acadoWorkspace.d[109]; +acadoVariables.x[114] += + acadoWorkspace.evGx[440]*acadoWorkspace.x[0] + acadoWorkspace.evGx[441]*acadoWorkspace.x[1] + acadoWorkspace.evGx[442]*acadoWorkspace.x[2] + acadoWorkspace.evGx[443]*acadoWorkspace.x[3] + acadoWorkspace.d[110]; +acadoVariables.x[115] += + acadoWorkspace.evGx[444]*acadoWorkspace.x[0] + acadoWorkspace.evGx[445]*acadoWorkspace.x[1] + acadoWorkspace.evGx[446]*acadoWorkspace.x[2] + acadoWorkspace.evGx[447]*acadoWorkspace.x[3] + acadoWorkspace.d[111]; +acadoVariables.x[116] += + acadoWorkspace.evGx[448]*acadoWorkspace.x[0] + acadoWorkspace.evGx[449]*acadoWorkspace.x[1] + acadoWorkspace.evGx[450]*acadoWorkspace.x[2] + acadoWorkspace.evGx[451]*acadoWorkspace.x[3] + acadoWorkspace.d[112]; +acadoVariables.x[117] += + acadoWorkspace.evGx[452]*acadoWorkspace.x[0] + acadoWorkspace.evGx[453]*acadoWorkspace.x[1] + acadoWorkspace.evGx[454]*acadoWorkspace.x[2] + acadoWorkspace.evGx[455]*acadoWorkspace.x[3] + acadoWorkspace.d[113]; +acadoVariables.x[118] += + acadoWorkspace.evGx[456]*acadoWorkspace.x[0] + acadoWorkspace.evGx[457]*acadoWorkspace.x[1] + acadoWorkspace.evGx[458]*acadoWorkspace.x[2] + acadoWorkspace.evGx[459]*acadoWorkspace.x[3] + acadoWorkspace.d[114]; +acadoVariables.x[119] += + acadoWorkspace.evGx[460]*acadoWorkspace.x[0] + acadoWorkspace.evGx[461]*acadoWorkspace.x[1] + acadoWorkspace.evGx[462]*acadoWorkspace.x[2] + acadoWorkspace.evGx[463]*acadoWorkspace.x[3] + acadoWorkspace.d[115]; +acadoVariables.x[120] += + acadoWorkspace.evGx[464]*acadoWorkspace.x[0] + acadoWorkspace.evGx[465]*acadoWorkspace.x[1] + acadoWorkspace.evGx[466]*acadoWorkspace.x[2] + acadoWorkspace.evGx[467]*acadoWorkspace.x[3] + acadoWorkspace.d[116]; +acadoVariables.x[121] += + acadoWorkspace.evGx[468]*acadoWorkspace.x[0] + acadoWorkspace.evGx[469]*acadoWorkspace.x[1] + acadoWorkspace.evGx[470]*acadoWorkspace.x[2] + acadoWorkspace.evGx[471]*acadoWorkspace.x[3] + acadoWorkspace.d[117]; +acadoVariables.x[122] += + acadoWorkspace.evGx[472]*acadoWorkspace.x[0] + acadoWorkspace.evGx[473]*acadoWorkspace.x[1] + acadoWorkspace.evGx[474]*acadoWorkspace.x[2] + acadoWorkspace.evGx[475]*acadoWorkspace.x[3] + acadoWorkspace.d[118]; +acadoVariables.x[123] += + acadoWorkspace.evGx[476]*acadoWorkspace.x[0] + acadoWorkspace.evGx[477]*acadoWorkspace.x[1] + acadoWorkspace.evGx[478]*acadoWorkspace.x[2] + acadoWorkspace.evGx[479]*acadoWorkspace.x[3] + acadoWorkspace.d[119]; +acadoVariables.x[124] += + acadoWorkspace.evGx[480]*acadoWorkspace.x[0] + acadoWorkspace.evGx[481]*acadoWorkspace.x[1] + acadoWorkspace.evGx[482]*acadoWorkspace.x[2] + acadoWorkspace.evGx[483]*acadoWorkspace.x[3] + acadoWorkspace.d[120]; +acadoVariables.x[125] += + acadoWorkspace.evGx[484]*acadoWorkspace.x[0] + acadoWorkspace.evGx[485]*acadoWorkspace.x[1] + acadoWorkspace.evGx[486]*acadoWorkspace.x[2] + acadoWorkspace.evGx[487]*acadoWorkspace.x[3] + acadoWorkspace.d[121]; +acadoVariables.x[126] += + acadoWorkspace.evGx[488]*acadoWorkspace.x[0] + acadoWorkspace.evGx[489]*acadoWorkspace.x[1] + acadoWorkspace.evGx[490]*acadoWorkspace.x[2] + acadoWorkspace.evGx[491]*acadoWorkspace.x[3] + acadoWorkspace.d[122]; +acadoVariables.x[127] += + acadoWorkspace.evGx[492]*acadoWorkspace.x[0] + acadoWorkspace.evGx[493]*acadoWorkspace.x[1] + acadoWorkspace.evGx[494]*acadoWorkspace.x[2] + acadoWorkspace.evGx[495]*acadoWorkspace.x[3] + acadoWorkspace.d[123]; +acadoVariables.x[128] += + acadoWorkspace.evGx[496]*acadoWorkspace.x[0] + acadoWorkspace.evGx[497]*acadoWorkspace.x[1] + acadoWorkspace.evGx[498]*acadoWorkspace.x[2] + acadoWorkspace.evGx[499]*acadoWorkspace.x[3] + acadoWorkspace.d[124]; +acadoVariables.x[129] += + acadoWorkspace.evGx[500]*acadoWorkspace.x[0] + acadoWorkspace.evGx[501]*acadoWorkspace.x[1] + acadoWorkspace.evGx[502]*acadoWorkspace.x[2] + acadoWorkspace.evGx[503]*acadoWorkspace.x[3] + acadoWorkspace.d[125]; +acadoVariables.x[130] += + acadoWorkspace.evGx[504]*acadoWorkspace.x[0] + acadoWorkspace.evGx[505]*acadoWorkspace.x[1] + acadoWorkspace.evGx[506]*acadoWorkspace.x[2] + acadoWorkspace.evGx[507]*acadoWorkspace.x[3] + acadoWorkspace.d[126]; +acadoVariables.x[131] += + acadoWorkspace.evGx[508]*acadoWorkspace.x[0] + acadoWorkspace.evGx[509]*acadoWorkspace.x[1] + acadoWorkspace.evGx[510]*acadoWorkspace.x[2] + acadoWorkspace.evGx[511]*acadoWorkspace.x[3] + acadoWorkspace.d[127]; +acadoVariables.x[132] += + acadoWorkspace.evGx[512]*acadoWorkspace.x[0] + acadoWorkspace.evGx[513]*acadoWorkspace.x[1] + acadoWorkspace.evGx[514]*acadoWorkspace.x[2] + acadoWorkspace.evGx[515]*acadoWorkspace.x[3] + acadoWorkspace.d[128]; +acadoVariables.x[133] += + acadoWorkspace.evGx[516]*acadoWorkspace.x[0] + acadoWorkspace.evGx[517]*acadoWorkspace.x[1] + acadoWorkspace.evGx[518]*acadoWorkspace.x[2] + acadoWorkspace.evGx[519]*acadoWorkspace.x[3] + acadoWorkspace.d[129]; +acadoVariables.x[134] += + acadoWorkspace.evGx[520]*acadoWorkspace.x[0] + acadoWorkspace.evGx[521]*acadoWorkspace.x[1] + acadoWorkspace.evGx[522]*acadoWorkspace.x[2] + acadoWorkspace.evGx[523]*acadoWorkspace.x[3] + acadoWorkspace.d[130]; +acadoVariables.x[135] += + acadoWorkspace.evGx[524]*acadoWorkspace.x[0] + acadoWorkspace.evGx[525]*acadoWorkspace.x[1] + acadoWorkspace.evGx[526]*acadoWorkspace.x[2] + acadoWorkspace.evGx[527]*acadoWorkspace.x[3] + acadoWorkspace.d[131]; +acadoVariables.x[136] += + acadoWorkspace.evGx[528]*acadoWorkspace.x[0] + acadoWorkspace.evGx[529]*acadoWorkspace.x[1] + acadoWorkspace.evGx[530]*acadoWorkspace.x[2] + acadoWorkspace.evGx[531]*acadoWorkspace.x[3] + acadoWorkspace.d[132]; +acadoVariables.x[137] += + acadoWorkspace.evGx[532]*acadoWorkspace.x[0] + acadoWorkspace.evGx[533]*acadoWorkspace.x[1] + acadoWorkspace.evGx[534]*acadoWorkspace.x[2] + acadoWorkspace.evGx[535]*acadoWorkspace.x[3] + acadoWorkspace.d[133]; +acadoVariables.x[138] += + acadoWorkspace.evGx[536]*acadoWorkspace.x[0] + acadoWorkspace.evGx[537]*acadoWorkspace.x[1] + acadoWorkspace.evGx[538]*acadoWorkspace.x[2] + acadoWorkspace.evGx[539]*acadoWorkspace.x[3] + acadoWorkspace.d[134]; +acadoVariables.x[139] += + acadoWorkspace.evGx[540]*acadoWorkspace.x[0] + acadoWorkspace.evGx[541]*acadoWorkspace.x[1] + acadoWorkspace.evGx[542]*acadoWorkspace.x[2] + acadoWorkspace.evGx[543]*acadoWorkspace.x[3] + acadoWorkspace.d[135]; +acadoVariables.x[140] += + acadoWorkspace.evGx[544]*acadoWorkspace.x[0] + acadoWorkspace.evGx[545]*acadoWorkspace.x[1] + acadoWorkspace.evGx[546]*acadoWorkspace.x[2] + acadoWorkspace.evGx[547]*acadoWorkspace.x[3] + acadoWorkspace.d[136]; +acadoVariables.x[141] += + acadoWorkspace.evGx[548]*acadoWorkspace.x[0] + acadoWorkspace.evGx[549]*acadoWorkspace.x[1] + acadoWorkspace.evGx[550]*acadoWorkspace.x[2] + acadoWorkspace.evGx[551]*acadoWorkspace.x[3] + acadoWorkspace.d[137]; +acadoVariables.x[142] += + acadoWorkspace.evGx[552]*acadoWorkspace.x[0] + acadoWorkspace.evGx[553]*acadoWorkspace.x[1] + acadoWorkspace.evGx[554]*acadoWorkspace.x[2] + acadoWorkspace.evGx[555]*acadoWorkspace.x[3] + acadoWorkspace.d[138]; +acadoVariables.x[143] += + acadoWorkspace.evGx[556]*acadoWorkspace.x[0] + acadoWorkspace.evGx[557]*acadoWorkspace.x[1] + acadoWorkspace.evGx[558]*acadoWorkspace.x[2] + acadoWorkspace.evGx[559]*acadoWorkspace.x[3] + acadoWorkspace.d[139]; +acadoVariables.x[144] += + acadoWorkspace.evGx[560]*acadoWorkspace.x[0] + acadoWorkspace.evGx[561]*acadoWorkspace.x[1] + acadoWorkspace.evGx[562]*acadoWorkspace.x[2] + acadoWorkspace.evGx[563]*acadoWorkspace.x[3] + acadoWorkspace.d[140]; +acadoVariables.x[145] += + acadoWorkspace.evGx[564]*acadoWorkspace.x[0] + acadoWorkspace.evGx[565]*acadoWorkspace.x[1] + acadoWorkspace.evGx[566]*acadoWorkspace.x[2] + acadoWorkspace.evGx[567]*acadoWorkspace.x[3] + acadoWorkspace.d[141]; +acadoVariables.x[146] += + acadoWorkspace.evGx[568]*acadoWorkspace.x[0] + acadoWorkspace.evGx[569]*acadoWorkspace.x[1] + acadoWorkspace.evGx[570]*acadoWorkspace.x[2] + acadoWorkspace.evGx[571]*acadoWorkspace.x[3] + acadoWorkspace.d[142]; +acadoVariables.x[147] += + acadoWorkspace.evGx[572]*acadoWorkspace.x[0] + acadoWorkspace.evGx[573]*acadoWorkspace.x[1] + acadoWorkspace.evGx[574]*acadoWorkspace.x[2] + acadoWorkspace.evGx[575]*acadoWorkspace.x[3] + acadoWorkspace.d[143]; +acadoVariables.x[148] += + acadoWorkspace.evGx[576]*acadoWorkspace.x[0] + acadoWorkspace.evGx[577]*acadoWorkspace.x[1] + acadoWorkspace.evGx[578]*acadoWorkspace.x[2] + acadoWorkspace.evGx[579]*acadoWorkspace.x[3] + acadoWorkspace.d[144]; +acadoVariables.x[149] += + acadoWorkspace.evGx[580]*acadoWorkspace.x[0] + acadoWorkspace.evGx[581]*acadoWorkspace.x[1] + acadoWorkspace.evGx[582]*acadoWorkspace.x[2] + acadoWorkspace.evGx[583]*acadoWorkspace.x[3] + acadoWorkspace.d[145]; +acadoVariables.x[150] += + acadoWorkspace.evGx[584]*acadoWorkspace.x[0] + acadoWorkspace.evGx[585]*acadoWorkspace.x[1] + acadoWorkspace.evGx[586]*acadoWorkspace.x[2] + acadoWorkspace.evGx[587]*acadoWorkspace.x[3] + acadoWorkspace.d[146]; +acadoVariables.x[151] += + acadoWorkspace.evGx[588]*acadoWorkspace.x[0] + acadoWorkspace.evGx[589]*acadoWorkspace.x[1] + acadoWorkspace.evGx[590]*acadoWorkspace.x[2] + acadoWorkspace.evGx[591]*acadoWorkspace.x[3] + acadoWorkspace.d[147]; +acadoVariables.x[152] += + acadoWorkspace.evGx[592]*acadoWorkspace.x[0] + acadoWorkspace.evGx[593]*acadoWorkspace.x[1] + acadoWorkspace.evGx[594]*acadoWorkspace.x[2] + acadoWorkspace.evGx[595]*acadoWorkspace.x[3] + acadoWorkspace.d[148]; +acadoVariables.x[153] += + acadoWorkspace.evGx[596]*acadoWorkspace.x[0] + acadoWorkspace.evGx[597]*acadoWorkspace.x[1] + acadoWorkspace.evGx[598]*acadoWorkspace.x[2] + acadoWorkspace.evGx[599]*acadoWorkspace.x[3] + acadoWorkspace.d[149]; +acadoVariables.x[154] += + acadoWorkspace.evGx[600]*acadoWorkspace.x[0] + acadoWorkspace.evGx[601]*acadoWorkspace.x[1] + acadoWorkspace.evGx[602]*acadoWorkspace.x[2] + acadoWorkspace.evGx[603]*acadoWorkspace.x[3] + acadoWorkspace.d[150]; +acadoVariables.x[155] += + acadoWorkspace.evGx[604]*acadoWorkspace.x[0] + acadoWorkspace.evGx[605]*acadoWorkspace.x[1] + acadoWorkspace.evGx[606]*acadoWorkspace.x[2] + acadoWorkspace.evGx[607]*acadoWorkspace.x[3] + acadoWorkspace.d[151]; +acadoVariables.x[156] += + acadoWorkspace.evGx[608]*acadoWorkspace.x[0] + acadoWorkspace.evGx[609]*acadoWorkspace.x[1] + acadoWorkspace.evGx[610]*acadoWorkspace.x[2] + acadoWorkspace.evGx[611]*acadoWorkspace.x[3] + acadoWorkspace.d[152]; +acadoVariables.x[157] += + acadoWorkspace.evGx[612]*acadoWorkspace.x[0] + acadoWorkspace.evGx[613]*acadoWorkspace.x[1] + acadoWorkspace.evGx[614]*acadoWorkspace.x[2] + acadoWorkspace.evGx[615]*acadoWorkspace.x[3] + acadoWorkspace.d[153]; +acadoVariables.x[158] += + acadoWorkspace.evGx[616]*acadoWorkspace.x[0] + acadoWorkspace.evGx[617]*acadoWorkspace.x[1] + acadoWorkspace.evGx[618]*acadoWorkspace.x[2] + acadoWorkspace.evGx[619]*acadoWorkspace.x[3] + acadoWorkspace.d[154]; +acadoVariables.x[159] += + acadoWorkspace.evGx[620]*acadoWorkspace.x[0] + acadoWorkspace.evGx[621]*acadoWorkspace.x[1] + acadoWorkspace.evGx[622]*acadoWorkspace.x[2] + acadoWorkspace.evGx[623]*acadoWorkspace.x[3] + acadoWorkspace.d[155]; +acadoVariables.x[160] += + acadoWorkspace.evGx[624]*acadoWorkspace.x[0] + acadoWorkspace.evGx[625]*acadoWorkspace.x[1] + acadoWorkspace.evGx[626]*acadoWorkspace.x[2] + acadoWorkspace.evGx[627]*acadoWorkspace.x[3] + acadoWorkspace.d[156]; +acadoVariables.x[161] += + acadoWorkspace.evGx[628]*acadoWorkspace.x[0] + acadoWorkspace.evGx[629]*acadoWorkspace.x[1] + acadoWorkspace.evGx[630]*acadoWorkspace.x[2] + acadoWorkspace.evGx[631]*acadoWorkspace.x[3] + acadoWorkspace.d[157]; +acadoVariables.x[162] += + acadoWorkspace.evGx[632]*acadoWorkspace.x[0] + acadoWorkspace.evGx[633]*acadoWorkspace.x[1] + acadoWorkspace.evGx[634]*acadoWorkspace.x[2] + acadoWorkspace.evGx[635]*acadoWorkspace.x[3] + acadoWorkspace.d[158]; +acadoVariables.x[163] += + acadoWorkspace.evGx[636]*acadoWorkspace.x[0] + acadoWorkspace.evGx[637]*acadoWorkspace.x[1] + acadoWorkspace.evGx[638]*acadoWorkspace.x[2] + acadoWorkspace.evGx[639]*acadoWorkspace.x[3] + acadoWorkspace.d[159]; +acadoVariables.x[164] += + acadoWorkspace.evGx[640]*acadoWorkspace.x[0] + acadoWorkspace.evGx[641]*acadoWorkspace.x[1] + acadoWorkspace.evGx[642]*acadoWorkspace.x[2] + acadoWorkspace.evGx[643]*acadoWorkspace.x[3] + acadoWorkspace.d[160]; +acadoVariables.x[165] += + acadoWorkspace.evGx[644]*acadoWorkspace.x[0] + acadoWorkspace.evGx[645]*acadoWorkspace.x[1] + acadoWorkspace.evGx[646]*acadoWorkspace.x[2] + acadoWorkspace.evGx[647]*acadoWorkspace.x[3] + acadoWorkspace.d[161]; +acadoVariables.x[166] += + acadoWorkspace.evGx[648]*acadoWorkspace.x[0] + acadoWorkspace.evGx[649]*acadoWorkspace.x[1] + acadoWorkspace.evGx[650]*acadoWorkspace.x[2] + acadoWorkspace.evGx[651]*acadoWorkspace.x[3] + acadoWorkspace.d[162]; +acadoVariables.x[167] += + acadoWorkspace.evGx[652]*acadoWorkspace.x[0] + acadoWorkspace.evGx[653]*acadoWorkspace.x[1] + acadoWorkspace.evGx[654]*acadoWorkspace.x[2] + acadoWorkspace.evGx[655]*acadoWorkspace.x[3] + acadoWorkspace.d[163]; +acadoVariables.x[168] += + acadoWorkspace.evGx[656]*acadoWorkspace.x[0] + acadoWorkspace.evGx[657]*acadoWorkspace.x[1] + acadoWorkspace.evGx[658]*acadoWorkspace.x[2] + acadoWorkspace.evGx[659]*acadoWorkspace.x[3] + acadoWorkspace.d[164]; +acadoVariables.x[169] += + acadoWorkspace.evGx[660]*acadoWorkspace.x[0] + acadoWorkspace.evGx[661]*acadoWorkspace.x[1] + acadoWorkspace.evGx[662]*acadoWorkspace.x[2] + acadoWorkspace.evGx[663]*acadoWorkspace.x[3] + acadoWorkspace.d[165]; +acadoVariables.x[170] += + acadoWorkspace.evGx[664]*acadoWorkspace.x[0] + acadoWorkspace.evGx[665]*acadoWorkspace.x[1] + acadoWorkspace.evGx[666]*acadoWorkspace.x[2] + acadoWorkspace.evGx[667]*acadoWorkspace.x[3] + acadoWorkspace.d[166]; +acadoVariables.x[171] += + acadoWorkspace.evGx[668]*acadoWorkspace.x[0] + acadoWorkspace.evGx[669]*acadoWorkspace.x[1] + acadoWorkspace.evGx[670]*acadoWorkspace.x[2] + acadoWorkspace.evGx[671]*acadoWorkspace.x[3] + acadoWorkspace.d[167]; +acadoVariables.x[172] += + acadoWorkspace.evGx[672]*acadoWorkspace.x[0] + acadoWorkspace.evGx[673]*acadoWorkspace.x[1] + acadoWorkspace.evGx[674]*acadoWorkspace.x[2] + acadoWorkspace.evGx[675]*acadoWorkspace.x[3] + acadoWorkspace.d[168]; +acadoVariables.x[173] += + acadoWorkspace.evGx[676]*acadoWorkspace.x[0] + acadoWorkspace.evGx[677]*acadoWorkspace.x[1] + acadoWorkspace.evGx[678]*acadoWorkspace.x[2] + acadoWorkspace.evGx[679]*acadoWorkspace.x[3] + acadoWorkspace.d[169]; +acadoVariables.x[174] += + acadoWorkspace.evGx[680]*acadoWorkspace.x[0] + acadoWorkspace.evGx[681]*acadoWorkspace.x[1] + acadoWorkspace.evGx[682]*acadoWorkspace.x[2] + acadoWorkspace.evGx[683]*acadoWorkspace.x[3] + acadoWorkspace.d[170]; +acadoVariables.x[175] += + acadoWorkspace.evGx[684]*acadoWorkspace.x[0] + acadoWorkspace.evGx[685]*acadoWorkspace.x[1] + acadoWorkspace.evGx[686]*acadoWorkspace.x[2] + acadoWorkspace.evGx[687]*acadoWorkspace.x[3] + acadoWorkspace.d[171]; +acadoVariables.x[176] += + acadoWorkspace.evGx[688]*acadoWorkspace.x[0] + acadoWorkspace.evGx[689]*acadoWorkspace.x[1] + acadoWorkspace.evGx[690]*acadoWorkspace.x[2] + acadoWorkspace.evGx[691]*acadoWorkspace.x[3] + acadoWorkspace.d[172]; +acadoVariables.x[177] += + acadoWorkspace.evGx[692]*acadoWorkspace.x[0] + acadoWorkspace.evGx[693]*acadoWorkspace.x[1] + acadoWorkspace.evGx[694]*acadoWorkspace.x[2] + acadoWorkspace.evGx[695]*acadoWorkspace.x[3] + acadoWorkspace.d[173]; +acadoVariables.x[178] += + acadoWorkspace.evGx[696]*acadoWorkspace.x[0] + acadoWorkspace.evGx[697]*acadoWorkspace.x[1] + acadoWorkspace.evGx[698]*acadoWorkspace.x[2] + acadoWorkspace.evGx[699]*acadoWorkspace.x[3] + acadoWorkspace.d[174]; +acadoVariables.x[179] += + acadoWorkspace.evGx[700]*acadoWorkspace.x[0] + acadoWorkspace.evGx[701]*acadoWorkspace.x[1] + acadoWorkspace.evGx[702]*acadoWorkspace.x[2] + acadoWorkspace.evGx[703]*acadoWorkspace.x[3] + acadoWorkspace.d[175]; +acadoVariables.x[180] += + acadoWorkspace.evGx[704]*acadoWorkspace.x[0] + acadoWorkspace.evGx[705]*acadoWorkspace.x[1] + acadoWorkspace.evGx[706]*acadoWorkspace.x[2] + acadoWorkspace.evGx[707]*acadoWorkspace.x[3] + acadoWorkspace.d[176]; +acadoVariables.x[181] += + acadoWorkspace.evGx[708]*acadoWorkspace.x[0] + acadoWorkspace.evGx[709]*acadoWorkspace.x[1] + acadoWorkspace.evGx[710]*acadoWorkspace.x[2] + acadoWorkspace.evGx[711]*acadoWorkspace.x[3] + acadoWorkspace.d[177]; +acadoVariables.x[182] += + acadoWorkspace.evGx[712]*acadoWorkspace.x[0] + acadoWorkspace.evGx[713]*acadoWorkspace.x[1] + acadoWorkspace.evGx[714]*acadoWorkspace.x[2] + acadoWorkspace.evGx[715]*acadoWorkspace.x[3] + acadoWorkspace.d[178]; +acadoVariables.x[183] += + acadoWorkspace.evGx[716]*acadoWorkspace.x[0] + acadoWorkspace.evGx[717]*acadoWorkspace.x[1] + acadoWorkspace.evGx[718]*acadoWorkspace.x[2] + acadoWorkspace.evGx[719]*acadoWorkspace.x[3] + acadoWorkspace.d[179]; +acadoVariables.x[184] += + acadoWorkspace.evGx[720]*acadoWorkspace.x[0] + acadoWorkspace.evGx[721]*acadoWorkspace.x[1] + acadoWorkspace.evGx[722]*acadoWorkspace.x[2] + acadoWorkspace.evGx[723]*acadoWorkspace.x[3] + acadoWorkspace.d[180]; +acadoVariables.x[185] += + acadoWorkspace.evGx[724]*acadoWorkspace.x[0] + acadoWorkspace.evGx[725]*acadoWorkspace.x[1] + acadoWorkspace.evGx[726]*acadoWorkspace.x[2] + acadoWorkspace.evGx[727]*acadoWorkspace.x[3] + acadoWorkspace.d[181]; +acadoVariables.x[186] += + acadoWorkspace.evGx[728]*acadoWorkspace.x[0] + acadoWorkspace.evGx[729]*acadoWorkspace.x[1] + acadoWorkspace.evGx[730]*acadoWorkspace.x[2] + acadoWorkspace.evGx[731]*acadoWorkspace.x[3] + acadoWorkspace.d[182]; +acadoVariables.x[187] += + acadoWorkspace.evGx[732]*acadoWorkspace.x[0] + acadoWorkspace.evGx[733]*acadoWorkspace.x[1] + acadoWorkspace.evGx[734]*acadoWorkspace.x[2] + acadoWorkspace.evGx[735]*acadoWorkspace.x[3] + acadoWorkspace.d[183]; +acadoVariables.x[188] += + acadoWorkspace.evGx[736]*acadoWorkspace.x[0] + acadoWorkspace.evGx[737]*acadoWorkspace.x[1] + acadoWorkspace.evGx[738]*acadoWorkspace.x[2] + acadoWorkspace.evGx[739]*acadoWorkspace.x[3] + acadoWorkspace.d[184]; +acadoVariables.x[189] += + acadoWorkspace.evGx[740]*acadoWorkspace.x[0] + acadoWorkspace.evGx[741]*acadoWorkspace.x[1] + acadoWorkspace.evGx[742]*acadoWorkspace.x[2] + acadoWorkspace.evGx[743]*acadoWorkspace.x[3] + acadoWorkspace.d[185]; +acadoVariables.x[190] += + acadoWorkspace.evGx[744]*acadoWorkspace.x[0] + acadoWorkspace.evGx[745]*acadoWorkspace.x[1] + acadoWorkspace.evGx[746]*acadoWorkspace.x[2] + acadoWorkspace.evGx[747]*acadoWorkspace.x[3] + acadoWorkspace.d[186]; +acadoVariables.x[191] += + acadoWorkspace.evGx[748]*acadoWorkspace.x[0] + acadoWorkspace.evGx[749]*acadoWorkspace.x[1] + acadoWorkspace.evGx[750]*acadoWorkspace.x[2] + acadoWorkspace.evGx[751]*acadoWorkspace.x[3] + acadoWorkspace.d[187]; +acadoVariables.x[192] += + acadoWorkspace.evGx[752]*acadoWorkspace.x[0] + acadoWorkspace.evGx[753]*acadoWorkspace.x[1] + acadoWorkspace.evGx[754]*acadoWorkspace.x[2] + acadoWorkspace.evGx[755]*acadoWorkspace.x[3] + acadoWorkspace.d[188]; +acadoVariables.x[193] += + acadoWorkspace.evGx[756]*acadoWorkspace.x[0] + acadoWorkspace.evGx[757]*acadoWorkspace.x[1] + acadoWorkspace.evGx[758]*acadoWorkspace.x[2] + acadoWorkspace.evGx[759]*acadoWorkspace.x[3] + acadoWorkspace.d[189]; +acadoVariables.x[194] += + acadoWorkspace.evGx[760]*acadoWorkspace.x[0] + acadoWorkspace.evGx[761]*acadoWorkspace.x[1] + acadoWorkspace.evGx[762]*acadoWorkspace.x[2] + acadoWorkspace.evGx[763]*acadoWorkspace.x[3] + acadoWorkspace.d[190]; +acadoVariables.x[195] += + acadoWorkspace.evGx[764]*acadoWorkspace.x[0] + acadoWorkspace.evGx[765]*acadoWorkspace.x[1] + acadoWorkspace.evGx[766]*acadoWorkspace.x[2] + acadoWorkspace.evGx[767]*acadoWorkspace.x[3] + acadoWorkspace.d[191]; +acadoVariables.x[196] += + acadoWorkspace.evGx[768]*acadoWorkspace.x[0] + acadoWorkspace.evGx[769]*acadoWorkspace.x[1] + acadoWorkspace.evGx[770]*acadoWorkspace.x[2] + acadoWorkspace.evGx[771]*acadoWorkspace.x[3] + acadoWorkspace.d[192]; +acadoVariables.x[197] += + acadoWorkspace.evGx[772]*acadoWorkspace.x[0] + acadoWorkspace.evGx[773]*acadoWorkspace.x[1] + acadoWorkspace.evGx[774]*acadoWorkspace.x[2] + acadoWorkspace.evGx[775]*acadoWorkspace.x[3] + acadoWorkspace.d[193]; +acadoVariables.x[198] += + acadoWorkspace.evGx[776]*acadoWorkspace.x[0] + acadoWorkspace.evGx[777]*acadoWorkspace.x[1] + acadoWorkspace.evGx[778]*acadoWorkspace.x[2] + acadoWorkspace.evGx[779]*acadoWorkspace.x[3] + acadoWorkspace.d[194]; +acadoVariables.x[199] += + acadoWorkspace.evGx[780]*acadoWorkspace.x[0] + acadoWorkspace.evGx[781]*acadoWorkspace.x[1] + acadoWorkspace.evGx[782]*acadoWorkspace.x[2] + acadoWorkspace.evGx[783]*acadoWorkspace.x[3] + acadoWorkspace.d[195]; +acadoVariables.x[200] += + acadoWorkspace.evGx[784]*acadoWorkspace.x[0] + acadoWorkspace.evGx[785]*acadoWorkspace.x[1] + acadoWorkspace.evGx[786]*acadoWorkspace.x[2] + acadoWorkspace.evGx[787]*acadoWorkspace.x[3] + acadoWorkspace.d[196]; +acadoVariables.x[201] += + acadoWorkspace.evGx[788]*acadoWorkspace.x[0] + acadoWorkspace.evGx[789]*acadoWorkspace.x[1] + acadoWorkspace.evGx[790]*acadoWorkspace.x[2] + acadoWorkspace.evGx[791]*acadoWorkspace.x[3] + acadoWorkspace.d[197]; +acadoVariables.x[202] += + acadoWorkspace.evGx[792]*acadoWorkspace.x[0] + acadoWorkspace.evGx[793]*acadoWorkspace.x[1] + acadoWorkspace.evGx[794]*acadoWorkspace.x[2] + acadoWorkspace.evGx[795]*acadoWorkspace.x[3] + acadoWorkspace.d[198]; +acadoVariables.x[203] += + acadoWorkspace.evGx[796]*acadoWorkspace.x[0] + acadoWorkspace.evGx[797]*acadoWorkspace.x[1] + acadoWorkspace.evGx[798]*acadoWorkspace.x[2] + acadoWorkspace.evGx[799]*acadoWorkspace.x[3] + acadoWorkspace.d[199]; + +for (lRun1 = 0; lRun1 < 50; ++lRun1) +{ +for (lRun2 = 0; lRun2 < lRun1 + 1; ++lRun2) +{ +lRun3 = (((lRun1 + 1) * (lRun1)) / (2)) + (lRun2); +acado_multEDu( &(acadoWorkspace.E[ lRun3 * 4 ]), &(acadoWorkspace.x[ lRun2 + 4 ]), &(acadoVariables.x[ lRun1 * 4 + 4 ]) ); +} +} +} + +int acado_preparationStep( ) +{ +int ret; + +ret = acado_modelSimulation(); +acado_evaluateObjective( ); +acado_condensePrep( ); +return ret; +} + +int acado_feedbackStep( ) +{ +int tmp; + +acado_condenseFdb( ); + +tmp = acado_solve( ); + +acado_expand( ); +return tmp; +} + +int acado_initializeSolver( ) +{ +int ret; + +/* This is a function which must be called once before any other function call! */ + + +ret = 0; + +memset(&acadoWorkspace, 0, sizeof( acadoWorkspace )); +return ret; +} + +void acado_initializeNodesByForwardSimulation( ) +{ +int index; +for (index = 0; index < 50; ++index) +{ +acadoWorkspace.state[0] = acadoVariables.x[index * 4]; +acadoWorkspace.state[1] = acadoVariables.x[index * 4 + 1]; +acadoWorkspace.state[2] = acadoVariables.x[index * 4 + 2]; +acadoWorkspace.state[3] = acadoVariables.x[index * 4 + 3]; +acadoWorkspace.state[24] = acadoVariables.u[index]; +acadoWorkspace.state[25] = acadoVariables.od[index * 18]; +acadoWorkspace.state[26] = acadoVariables.od[index * 18 + 1]; +acadoWorkspace.state[27] = acadoVariables.od[index * 18 + 2]; +acadoWorkspace.state[28] = acadoVariables.od[index * 18 + 3]; +acadoWorkspace.state[29] = acadoVariables.od[index * 18 + 4]; +acadoWorkspace.state[30] = acadoVariables.od[index * 18 + 5]; +acadoWorkspace.state[31] = acadoVariables.od[index * 18 + 6]; +acadoWorkspace.state[32] = acadoVariables.od[index * 18 + 7]; +acadoWorkspace.state[33] = acadoVariables.od[index * 18 + 8]; +acadoWorkspace.state[34] = acadoVariables.od[index * 18 + 9]; +acadoWorkspace.state[35] = acadoVariables.od[index * 18 + 10]; +acadoWorkspace.state[36] = acadoVariables.od[index * 18 + 11]; +acadoWorkspace.state[37] = acadoVariables.od[index * 18 + 12]; +acadoWorkspace.state[38] = acadoVariables.od[index * 18 + 13]; +acadoWorkspace.state[39] = acadoVariables.od[index * 18 + 14]; +acadoWorkspace.state[40] = acadoVariables.od[index * 18 + 15]; +acadoWorkspace.state[41] = acadoVariables.od[index * 18 + 16]; +acadoWorkspace.state[42] = acadoVariables.od[index * 18 + 17]; + +acado_integrate(acadoWorkspace.state, index == 0); + +acadoVariables.x[index * 4 + 4] = acadoWorkspace.state[0]; +acadoVariables.x[index * 4 + 5] = acadoWorkspace.state[1]; +acadoVariables.x[index * 4 + 6] = acadoWorkspace.state[2]; +acadoVariables.x[index * 4 + 7] = acadoWorkspace.state[3]; +} +} + +void acado_shiftStates( int strategy, real_t* const xEnd, real_t* const uEnd ) +{ +int index; +for (index = 0; index < 50; ++index) +{ +acadoVariables.x[index * 4] = acadoVariables.x[index * 4 + 4]; +acadoVariables.x[index * 4 + 1] = acadoVariables.x[index * 4 + 5]; +acadoVariables.x[index * 4 + 2] = acadoVariables.x[index * 4 + 6]; +acadoVariables.x[index * 4 + 3] = acadoVariables.x[index * 4 + 7]; +} + +if (strategy == 1 && xEnd != 0) +{ +acadoVariables.x[200] = xEnd[0]; +acadoVariables.x[201] = xEnd[1]; +acadoVariables.x[202] = xEnd[2]; +acadoVariables.x[203] = xEnd[3]; +} +else if (strategy == 2) +{ +acadoWorkspace.state[0] = acadoVariables.x[200]; +acadoWorkspace.state[1] = acadoVariables.x[201]; +acadoWorkspace.state[2] = acadoVariables.x[202]; +acadoWorkspace.state[3] = acadoVariables.x[203]; +if (uEnd != 0) +{ +acadoWorkspace.state[24] = uEnd[0]; +} +else +{ +acadoWorkspace.state[24] = acadoVariables.u[49]; +} +acadoWorkspace.state[25] = acadoVariables.od[900]; +acadoWorkspace.state[26] = acadoVariables.od[901]; +acadoWorkspace.state[27] = acadoVariables.od[902]; +acadoWorkspace.state[28] = acadoVariables.od[903]; +acadoWorkspace.state[29] = acadoVariables.od[904]; +acadoWorkspace.state[30] = acadoVariables.od[905]; +acadoWorkspace.state[31] = acadoVariables.od[906]; +acadoWorkspace.state[32] = acadoVariables.od[907]; +acadoWorkspace.state[33] = acadoVariables.od[908]; +acadoWorkspace.state[34] = acadoVariables.od[909]; +acadoWorkspace.state[35] = acadoVariables.od[910]; +acadoWorkspace.state[36] = acadoVariables.od[911]; +acadoWorkspace.state[37] = acadoVariables.od[912]; +acadoWorkspace.state[38] = acadoVariables.od[913]; +acadoWorkspace.state[39] = acadoVariables.od[914]; +acadoWorkspace.state[40] = acadoVariables.od[915]; +acadoWorkspace.state[41] = acadoVariables.od[916]; +acadoWorkspace.state[42] = acadoVariables.od[917]; + +acado_integrate(acadoWorkspace.state, 1); + +acadoVariables.x[200] = acadoWorkspace.state[0]; +acadoVariables.x[201] = acadoWorkspace.state[1]; +acadoVariables.x[202] = acadoWorkspace.state[2]; +acadoVariables.x[203] = acadoWorkspace.state[3]; +} +} + +void acado_shiftControls( real_t* const uEnd ) +{ +int index; +for (index = 0; index < 49; ++index) +{ +acadoVariables.u[index] = acadoVariables.u[index + 1]; +} + +if (uEnd != 0) +{ +acadoVariables.u[49] = uEnd[0]; +} +} + +real_t acado_getKKT( ) +{ +real_t kkt; + +int index; +real_t prd; + +kkt = + acadoWorkspace.g[0]*acadoWorkspace.x[0] + acadoWorkspace.g[1]*acadoWorkspace.x[1] + acadoWorkspace.g[2]*acadoWorkspace.x[2] + acadoWorkspace.g[3]*acadoWorkspace.x[3] + acadoWorkspace.g[4]*acadoWorkspace.x[4] + acadoWorkspace.g[5]*acadoWorkspace.x[5] + acadoWorkspace.g[6]*acadoWorkspace.x[6] + acadoWorkspace.g[7]*acadoWorkspace.x[7] + acadoWorkspace.g[8]*acadoWorkspace.x[8] + acadoWorkspace.g[9]*acadoWorkspace.x[9] + acadoWorkspace.g[10]*acadoWorkspace.x[10] + acadoWorkspace.g[11]*acadoWorkspace.x[11] + acadoWorkspace.g[12]*acadoWorkspace.x[12] + acadoWorkspace.g[13]*acadoWorkspace.x[13] + acadoWorkspace.g[14]*acadoWorkspace.x[14] + acadoWorkspace.g[15]*acadoWorkspace.x[15] + acadoWorkspace.g[16]*acadoWorkspace.x[16] + acadoWorkspace.g[17]*acadoWorkspace.x[17] + acadoWorkspace.g[18]*acadoWorkspace.x[18] + acadoWorkspace.g[19]*acadoWorkspace.x[19] + acadoWorkspace.g[20]*acadoWorkspace.x[20] + acadoWorkspace.g[21]*acadoWorkspace.x[21] + acadoWorkspace.g[22]*acadoWorkspace.x[22] + acadoWorkspace.g[23]*acadoWorkspace.x[23] + acadoWorkspace.g[24]*acadoWorkspace.x[24] + acadoWorkspace.g[25]*acadoWorkspace.x[25] + acadoWorkspace.g[26]*acadoWorkspace.x[26] + acadoWorkspace.g[27]*acadoWorkspace.x[27] + acadoWorkspace.g[28]*acadoWorkspace.x[28] + acadoWorkspace.g[29]*acadoWorkspace.x[29] + acadoWorkspace.g[30]*acadoWorkspace.x[30] + acadoWorkspace.g[31]*acadoWorkspace.x[31] + acadoWorkspace.g[32]*acadoWorkspace.x[32] + acadoWorkspace.g[33]*acadoWorkspace.x[33] + acadoWorkspace.g[34]*acadoWorkspace.x[34] + acadoWorkspace.g[35]*acadoWorkspace.x[35] + acadoWorkspace.g[36]*acadoWorkspace.x[36] + acadoWorkspace.g[37]*acadoWorkspace.x[37] + acadoWorkspace.g[38]*acadoWorkspace.x[38] + acadoWorkspace.g[39]*acadoWorkspace.x[39] + acadoWorkspace.g[40]*acadoWorkspace.x[40] + acadoWorkspace.g[41]*acadoWorkspace.x[41] + acadoWorkspace.g[42]*acadoWorkspace.x[42] + acadoWorkspace.g[43]*acadoWorkspace.x[43] + acadoWorkspace.g[44]*acadoWorkspace.x[44] + acadoWorkspace.g[45]*acadoWorkspace.x[45] + acadoWorkspace.g[46]*acadoWorkspace.x[46] + acadoWorkspace.g[47]*acadoWorkspace.x[47] + acadoWorkspace.g[48]*acadoWorkspace.x[48] + acadoWorkspace.g[49]*acadoWorkspace.x[49] + acadoWorkspace.g[50]*acadoWorkspace.x[50] + acadoWorkspace.g[51]*acadoWorkspace.x[51] + acadoWorkspace.g[52]*acadoWorkspace.x[52] + acadoWorkspace.g[53]*acadoWorkspace.x[53]; +kkt = fabs( kkt ); +for (index = 0; index < 54; ++index) +{ +prd = acadoWorkspace.y[index]; +if (prd > 1e-12) +kkt += fabs(acadoWorkspace.lb[index] * prd); +else if (prd < -1e-12) +kkt += fabs(acadoWorkspace.ub[index] * prd); +} +for (index = 0; index < 100; ++index) +{ +prd = acadoWorkspace.y[index + 54]; +if (prd > 1e-12) +kkt += fabs(acadoWorkspace.lbA[index] * prd); +else if (prd < -1e-12) +kkt += fabs(acadoWorkspace.ubA[index] * prd); +} +return kkt; +} + +real_t acado_getObjective( ) +{ +real_t objVal; + +int lRun1; +/** Row vector of size: 5 */ +real_t tmpDy[ 5 ]; + +/** Row vector of size: 4 */ +real_t tmpDyN[ 4 ]; + +for (lRun1 = 0; lRun1 < 50; ++lRun1) +{ +acadoWorkspace.objValueIn[0] = acadoVariables.x[lRun1 * 4]; +acadoWorkspace.objValueIn[1] = acadoVariables.x[lRun1 * 4 + 1]; +acadoWorkspace.objValueIn[2] = acadoVariables.x[lRun1 * 4 + 2]; +acadoWorkspace.objValueIn[3] = acadoVariables.x[lRun1 * 4 + 3]; +acadoWorkspace.objValueIn[4] = acadoVariables.u[lRun1]; +acadoWorkspace.objValueIn[5] = acadoVariables.od[lRun1 * 18]; +acadoWorkspace.objValueIn[6] = acadoVariables.od[lRun1 * 18 + 1]; +acadoWorkspace.objValueIn[7] = acadoVariables.od[lRun1 * 18 + 2]; +acadoWorkspace.objValueIn[8] = acadoVariables.od[lRun1 * 18 + 3]; +acadoWorkspace.objValueIn[9] = acadoVariables.od[lRun1 * 18 + 4]; +acadoWorkspace.objValueIn[10] = acadoVariables.od[lRun1 * 18 + 5]; +acadoWorkspace.objValueIn[11] = acadoVariables.od[lRun1 * 18 + 6]; +acadoWorkspace.objValueIn[12] = acadoVariables.od[lRun1 * 18 + 7]; +acadoWorkspace.objValueIn[13] = acadoVariables.od[lRun1 * 18 + 8]; +acadoWorkspace.objValueIn[14] = acadoVariables.od[lRun1 * 18 + 9]; +acadoWorkspace.objValueIn[15] = acadoVariables.od[lRun1 * 18 + 10]; +acadoWorkspace.objValueIn[16] = acadoVariables.od[lRun1 * 18 + 11]; +acadoWorkspace.objValueIn[17] = acadoVariables.od[lRun1 * 18 + 12]; +acadoWorkspace.objValueIn[18] = acadoVariables.od[lRun1 * 18 + 13]; +acadoWorkspace.objValueIn[19] = acadoVariables.od[lRun1 * 18 + 14]; +acadoWorkspace.objValueIn[20] = acadoVariables.od[lRun1 * 18 + 15]; +acadoWorkspace.objValueIn[21] = acadoVariables.od[lRun1 * 18 + 16]; +acadoWorkspace.objValueIn[22] = acadoVariables.od[lRun1 * 18 + 17]; + +acado_evaluateLSQ( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut ); +acadoWorkspace.Dy[lRun1 * 5] = acadoWorkspace.objValueOut[0] - acadoVariables.y[lRun1 * 5]; +acadoWorkspace.Dy[lRun1 * 5 + 1] = acadoWorkspace.objValueOut[1] - acadoVariables.y[lRun1 * 5 + 1]; +acadoWorkspace.Dy[lRun1 * 5 + 2] = acadoWorkspace.objValueOut[2] - acadoVariables.y[lRun1 * 5 + 2]; +acadoWorkspace.Dy[lRun1 * 5 + 3] = acadoWorkspace.objValueOut[3] - acadoVariables.y[lRun1 * 5 + 3]; +acadoWorkspace.Dy[lRun1 * 5 + 4] = acadoWorkspace.objValueOut[4] - acadoVariables.y[lRun1 * 5 + 4]; +} +acadoWorkspace.objValueIn[0] = acadoVariables.x[200]; +acadoWorkspace.objValueIn[1] = acadoVariables.x[201]; +acadoWorkspace.objValueIn[2] = acadoVariables.x[202]; +acadoWorkspace.objValueIn[3] = acadoVariables.x[203]; +acadoWorkspace.objValueIn[4] = acadoVariables.od[900]; +acadoWorkspace.objValueIn[5] = acadoVariables.od[901]; +acadoWorkspace.objValueIn[6] = acadoVariables.od[902]; +acadoWorkspace.objValueIn[7] = acadoVariables.od[903]; +acadoWorkspace.objValueIn[8] = acadoVariables.od[904]; +acadoWorkspace.objValueIn[9] = acadoVariables.od[905]; +acadoWorkspace.objValueIn[10] = acadoVariables.od[906]; +acadoWorkspace.objValueIn[11] = acadoVariables.od[907]; +acadoWorkspace.objValueIn[12] = acadoVariables.od[908]; +acadoWorkspace.objValueIn[13] = acadoVariables.od[909]; +acadoWorkspace.objValueIn[14] = acadoVariables.od[910]; +acadoWorkspace.objValueIn[15] = acadoVariables.od[911]; +acadoWorkspace.objValueIn[16] = acadoVariables.od[912]; +acadoWorkspace.objValueIn[17] = acadoVariables.od[913]; +acadoWorkspace.objValueIn[18] = acadoVariables.od[914]; +acadoWorkspace.objValueIn[19] = acadoVariables.od[915]; +acadoWorkspace.objValueIn[20] = acadoVariables.od[916]; +acadoWorkspace.objValueIn[21] = acadoVariables.od[917]; +acado_evaluateLSQEndTerm( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut ); +acadoWorkspace.DyN[0] = acadoWorkspace.objValueOut[0] - acadoVariables.yN[0]; +acadoWorkspace.DyN[1] = acadoWorkspace.objValueOut[1] - acadoVariables.yN[1]; +acadoWorkspace.DyN[2] = acadoWorkspace.objValueOut[2] - acadoVariables.yN[2]; +acadoWorkspace.DyN[3] = acadoWorkspace.objValueOut[3] - acadoVariables.yN[3]; +objVal = 0.0000000000000000e+00; +for (lRun1 = 0; lRun1 < 50; ++lRun1) +{ +tmpDy[0] = + acadoWorkspace.Dy[lRun1 * 5]; +tmpDy[1] = + acadoWorkspace.Dy[lRun1 * 5 + 1]; +tmpDy[2] = + acadoWorkspace.Dy[lRun1 * 5 + 2]; +tmpDy[3] = + acadoWorkspace.Dy[lRun1 * 5 + 3]; +tmpDy[4] = + acadoWorkspace.Dy[lRun1 * 5 + 4]; +objVal += + acadoWorkspace.Dy[lRun1 * 5]*tmpDy[0] + acadoWorkspace.Dy[lRun1 * 5 + 1]*tmpDy[1] + acadoWorkspace.Dy[lRun1 * 5 + 2]*tmpDy[2] + acadoWorkspace.Dy[lRun1 * 5 + 3]*tmpDy[3] + acadoWorkspace.Dy[lRun1 * 5 + 4]*tmpDy[4]; +} + +tmpDyN[0] = + acadoWorkspace.DyN[0]; +tmpDyN[1] = + acadoWorkspace.DyN[1]; +tmpDyN[2] = + acadoWorkspace.DyN[2]; +tmpDyN[3] = + acadoWorkspace.DyN[3]; +objVal += + acadoWorkspace.DyN[0]*tmpDyN[0] + acadoWorkspace.DyN[1]*tmpDyN[1] + acadoWorkspace.DyN[2]*tmpDyN[2] + acadoWorkspace.DyN[3]*tmpDyN[3]; + +objVal *= 0.5; +return objVal; +} + diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index ec9893293b2b88..5b65fef7deeb09 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -1,205 +1,117 @@ import numpy as np from common.numpy_fast import clip, interp from selfdrive.config import Conversions as CV +from selfdrive.controls.lib.pid import PIController + +STOPPING_EGO_SPEED = 0.5 +STOPPING_TARGET_SPEED = 0.3 +STARTING_TARGET_SPEED = 0.5 +BRAKE_THRESHOLD_TO_PID = 0.2 + class LongCtrlState: #*** this function handles the long control state transitions # long_control_state labels: - off = 0 # Off - pid = 1 # moving and tracking targets, with PID control running + off = 0 # Off + pid = 1 # moving and tracking targets, with PID control running stopping = 2 # stopping and changing controls to almost open loop as PID does not fit well at such a low speed starting = 3 # starting and releasing brakes in open loop before giving back to PID -def long_control_state_trans(enabled, long_control_state, v_ego, v_target, v_pid, output_gb): - stopping_speed = 0.5 - stopping_target_speed = 0.3 - starting_target_speed = 0.5 - brake_threshold_to_pid = 0.2 +def long_control_state_trans(active, long_control_state, v_ego, v_target, v_pid, + output_gb, brake_pressed): - stopping_condition = ((v_ego < stopping_speed) and (v_pid < stopping_target_speed) and (v_target < stopping_target_speed)) + stopping_condition = (v_ego < STOPPING_EGO_SPEED) and \ + (((v_pid < STOPPING_TARGET_SPEED) and (v_target < STOPPING_TARGET_SPEED)) or + (brake_pressed)) - if not enabled: + if not active: long_control_state = LongCtrlState.off + else: if long_control_state == LongCtrlState.off: - if enabled: + if active: long_control_state = LongCtrlState.pid + elif long_control_state == LongCtrlState.pid: if stopping_condition: long_control_state = LongCtrlState.stopping + elif long_control_state == LongCtrlState.stopping: - if (v_target > starting_target_speed): + if (v_target > STARTING_TARGET_SPEED): long_control_state = LongCtrlState.starting + elif long_control_state == LongCtrlState.starting: if stopping_condition: long_control_state = LongCtrlState.stopping - elif output_gb >= -brake_threshold_to_pid: + elif output_gb >= -BRAKE_THRESHOLD_TO_PID: long_control_state = LongCtrlState.pid return long_control_state -def get_compute_gb(): - # see debug/dump_accel_from_fiber.py - w0 = np.array([[ 1.22056961, -0.39625418, 0.67952657], - [ 1.03691769, 0.78210306, -0.41343188]]) - b0 = np.array([ 0.01536703, -0.14335321, -0.26932889]) - w2 = np.array([[-0.59124422, 0.42899439, 0.38660881], - [ 0.79973811, 0.13178682, 0.08550351], - [-0.15651935, -0.44360259, 0.76910877]]) - b2 = np.array([ 0.15624429, 0.02294923, -0.0341086 ]) - w4 = np.array([[-0.31521443], - [-0.38626176], - [ 0.52667892]]) - b4 = np.array([-0.02922216]) - - def compute_output(dat, w0, b0, w2, b2, w4, b4): - m0 = np.dot(dat, w0) + b0 - m0 = leakyrelu(m0, 0.1) - m2 = np.dot(m0, w2) + b2 - m2 = leakyrelu(m2, 0.1) - m4 = np.dot(m2, w4) + b4 - return m4 - - def leakyrelu(x, alpha): - return np.maximum(x, alpha * x) - - def _compute_gb(dat): - #linearly extrap below v1 using v1 and v2 data - v1 = 5. - v2 = 10. - vx = dat[1] - if vx > 5.: - m4 = compute_output(dat, w0, b0, w2, b2, w4, b4) - else: - dat[1] = v1 - m4v1 = compute_output(dat, w0, b0, w2, b2, w4, b4) - dat[1] = v2 - m4v2 = compute_output(dat, w0, b0, w2, b2, w4, b4) - m4 = (vx - v1) * (m4v2 - m4v1) / (v2 - v1) + m4v1 - return m4 - return _compute_gb - -# takes in [desired_accel, current_speed] -> [-1.0, 1.0] where -1.0 is max brake and 1.0 is max gas -compute_gb = get_compute_gb() - _KP_BP = [0., 5., 35.] -_KP_V = [1.2, 0.8, 0.5] - -_kI_BP = [0., 35.] -_kI_V = [0.18, 0.12] - -def pid_long_control(v_ego, v_pid, Ui_accel_cmd, gas_max, brake_max, jerk_factor, gear, rate): - #*** This function compute the gb pedal positions in order to track the desired speed - # proportional and integral terms. More precision at low speed - Kp = interp(v_ego, _KP_BP, _KP_V) - Ki = interp(v_ego, _kI_BP, _kI_V) - - # scale Kp and Ki by jerk factor from drive_thread - Kp = (1. + jerk_factor)*Kp - Ki = (1. + jerk_factor)*Ki - - # this is ugly but can speed reports 0 when speed<0.3m/s and we can't have that jump - v_ego_min = 0.3 - v_ego = max(v_ego, v_ego_min) - - v_error = v_pid - v_ego - - Up_accel_cmd = v_error*Kp - Ui_accel_cmd_new = Ui_accel_cmd + v_error*Ki*1.0/rate - accel_cmd_new = Ui_accel_cmd_new + Up_accel_cmd - output_gb_new = compute_gb([accel_cmd_new, v_ego]) - - # Anti-wind up for integrator: only update integrator if we not against the throttle and brake limits - # do not wind up if we are changing gear and we are on the gas pedal - if (((v_error >= 0. and (output_gb_new < gas_max or Ui_accel_cmd < 0)) or - (v_error <= 0. and (output_gb_new > - brake_max or Ui_accel_cmd > 0))) and - not (v_error >= 0. and gear == 11 and output_gb_new > 0)): - #update integrator - Ui_accel_cmd = Ui_accel_cmd_new +_KP_V = [1.2, 0.8, 0.5] - accel_cmd = Ui_accel_cmd + Up_accel_cmd +_KI_BP = [0., 35.] +_KI_V = [0.18, 0.12] - # go from accel to pedals - output_gb = compute_gb([accel_cmd, v_ego]) - output_gb = output_gb[0] - - # useful to know if control is against the limit - long_control_sat = False - if output_gb > gas_max or output_gb < -brake_max: - long_control_sat = True - - output_gb = clip(output_gb, -brake_max, gas_max) - - return output_gb, Up_accel_cmd, Ui_accel_cmd, long_control_sat - - -stopping_brake_rate = 0.2 # brake_travel/s while trying to stop -starting_brake_rate = 0.6 # brake_travel/s while releasing on restart -starting_Ui = 0.5 # Since we don't have much info about acceleration at this point, be conservative +stopping_brake_rate = 0.2 # brake_travel/s while trying to stop +starting_brake_rate = 0.6 # brake_travel/s while releasing on restart +starting_Ui = 0.5 # Since we don't have much info about acceleration at this point, be conservative brake_stopping_target = 0.5 # apply at least this amount of brake to maintain the vehicle stationary _MAX_SPEED_ERROR_BP = [0., 30.] # speed breakpoints -_MAX_SPEED_ERROR_V = [1.5, .8] # max positive v_pid error VS actual speed; this avoids controls windup due to slow pedal resp +_MAX_SPEED_ERROR_V = [1.5, .8] # max positive v_pid error VS actual speed; this avoids controls windup due to slow pedal resp + class LongControl(object): - def __init__(self): - self.long_control_state = LongCtrlState.off # initialized to off - self.long_control_sat = False - self.Up_accel_cmd = 0. - self.last_output_gb = 0. - self.reset(0.) + def __init__(self, compute_gb): + self.long_control_state = LongCtrlState.off # initialized to off + self.pid = PIController((_KP_BP, _KP_V), + (_KI_BP, _KI_V), + rate=100.0, + sat_limit=0.8, + convert=compute_gb) + self.v_pid = 0.0 + self.last_output_gb = 0.0 def reset(self, v_pid): - self.Ui_accel_cmd = 0. + self.pid.reset() self.v_pid = v_pid - def update(self, enabled, v_ego, v_cruise, v_target_lead, a_target, jerk_factor, CP): - brake_max_bp = [0., 5., 20., 100.] # speeds - brake_max_v = [1.0, 1.0, 0.8, 0.8] # values - - # brake and gas limits - brake_max = interp(v_ego, brake_max_bp, brake_max_v) + def update(self, active, v_ego, brake_pressed, v_cruise, v_target_lead, a_target, + jerk_factor, CP): - # TODO: not every time - if CP.enableGas: - gas_max_bp = [0., 100.] # speeds - gas_max_v = [0.6, 0.6] # values - gas_max = interp(v_ego, gas_max_bp, gas_max_v) - else: - gas_max = 0 + # actuation limits + gas_max = interp(v_ego, CP.gasMaxBP, CP.gasMaxV) + brake_max = interp(v_ego, CP.brakeMaxBP, CP.brakeMaxV) - overshoot_allowance = 2.0 # overshoot allowed when changing accel sign + overshoot_allowance = 2.0 # overshoot allowed when changing accel sign output_gb = self.last_output_gb - rate = 100 - # limit max target speed based on cruise setting: - v_cruise_mph = round(v_cruise * CV.KPH_TO_MPH) # what's displayed in mph on the IC - v_target = min(v_target_lead, v_cruise_mph * CV.MPH_TO_MS) + # limit max target speed based on cruise setting + v_target = min(v_target_lead, v_cruise * CV.KPH_TO_MS) + rate = 100.0 + max_speed_delta_up = a_target[1] * 1.0 / rate + max_speed_delta_down = a_target[0] * 1.0 / rate - max_speed_delta_up = a_target[1]*1.0/rate - max_speed_delta_down = a_target[0]*1.0/rate - - # *** long control substate transitions - self.long_control_state = long_control_state_trans(enabled, self.long_control_state, v_ego, v_target, self.v_pid, output_gb) + self.long_control_state = long_control_state_trans(active, self.long_control_state, v_ego,\ + v_target, self.v_pid, output_gb, brake_pressed) # *** long control behavior based on state - # TODO: move this to drive_helpers - # disabled if self.long_control_state == LongCtrlState.off: - self.v_pid = v_ego # do nothing + self.v_pid = v_ego # do nothing output_gb = 0. - self.Ui_accel_cmd = 0. + self.pid.reset() + # tracking objects and driving elif self.long_control_state == LongCtrlState.pid: #reset v_pid close to v_ego if it was too far and new v_target is closer to v_ego - if ((self.v_pid > v_ego + overshoot_allowance) and - (v_target < self.v_pid)): + if ((self.v_pid > v_ego + overshoot_allowance) and (v_target < self.v_pid)): self.v_pid = max(v_target, v_ego + overshoot_allowance) - elif ((self.v_pid < v_ego - overshoot_allowance) and - (v_target > self.v_pid)): + elif ((self.v_pid < v_ego - overshoot_allowance) and (v_target > self.v_pid)): self.v_pid = min(v_target, v_ego - overshoot_allowance) # move v_pid no faster than allowed accel limits @@ -215,25 +127,31 @@ def update(self, enabled, v_ego, v_cruise, v_target_lead, a_target, jerk_factor, max_speed_error = interp(v_ego, _MAX_SPEED_ERROR_BP, _MAX_SPEED_ERROR_V) self.v_pid = min(self.v_pid, v_ego + max_speed_error) - # TODO: removed anti windup on gear change, does it matter? - output_gb, self.Up_accel_cmd, self.Ui_accel_cmd, self.long_control_sat = pid_long_control(v_ego, self.v_pid, \ - self.Ui_accel_cmd, gas_max, brake_max, jerk_factor, 0, rate) + self.pid.pos_limit = gas_max + self.pid.neg_limit = - brake_max + v_ego_pid = max(v_ego, 0.3) # Without this we get jumps, CAN bus reports 0 when speed < 0.3 + output_gb = self.pid.update(self.v_pid, v_ego_pid, speed=v_ego_pid, jerk_factor=jerk_factor) + # intention is to stop, switch to a different brake control until we stop elif self.long_control_state == LongCtrlState.stopping: + # TODO: use the standstill bit from CAN to detect movements, usually more accurate than looking at v_ego if v_ego > 0. or output_gb > -brake_stopping_target: - output_gb -= stopping_brake_rate/rate + output_gb -= stopping_brake_rate / rate output_gb = clip(output_gb, -brake_max, gas_max) + self.v_pid = v_ego - self.Ui_accel_cmd = 0. + self.pid.reset() + # intention is to move again, release brake fast before handling control to PID elif self.long_control_state == LongCtrlState.starting: if output_gb < -0.2: - output_gb += starting_brake_rate/rate + output_gb += starting_brake_rate / rate self.v_pid = v_ego - self.Ui_accel_cmd = starting_Ui + self.pid.reset() + self.pid.i = starting_Ui self.last_output_gb = output_gb final_gas = clip(output_gb, 0., gas_max) final_brake = -clip(output_gb, -brake_max, 0.) - return final_gas, final_brake + return final_gas, final_brake diff --git a/selfdrive/controls/lib/pathplanner.py b/selfdrive/controls/lib/pathplanner.py index d65367b995e533..68b4f22cfb3963 100644 --- a/selfdrive/controls/lib/pathplanner.py +++ b/selfdrive/controls/lib/pathplanner.py @@ -1,66 +1,9 @@ -import math -import numpy as np - from common.numpy_fast import interp -import selfdrive.messaging as messaging - - -def compute_path_pinv(): - deg = 3 - x = np.arange(50.0) - X = np.vstack(tuple(x**n for n in range(deg, -1, -1))).T - pinv = np.linalg.pinv(X) - return pinv - -def model_polyfit(points, path_pinv): - return np.dot(path_pinv, map(float, points)) - -# lane width http://safety.fhwa.dot.gov/geometric/pubs/mitigationstrategies/chapter3/3_lanewidth.cfm -_LANE_WIDTH_V = [3., 3.8] - -# break points of speed -_LANE_WIDTH_BP = [0., 31.] +from selfdrive.controls.lib.latcontrol_helpers import model_polyfit, calc_desired_path, compute_path_pinv -def calc_desired_path(l_poly, r_poly, p_poly, l_prob, r_prob, p_prob, speed): - #*** this function computes the poly for the center of the lane, averaging left and right polys - lane_width = interp(speed, _LANE_WIDTH_BP, _LANE_WIDTH_V) - - # lanes in US are ~3.6m wide - half_lane_poly = np.array([0., 0., 0., lane_width / 2.]) - if l_prob + r_prob > 0.01: - c_poly = ((l_poly - half_lane_poly) * l_prob + - (r_poly + half_lane_poly) * r_prob) / (l_prob + r_prob) - c_prob = math.sqrt((l_prob**2 + r_prob**2) / 2.) - else: - c_poly = np.zeros(4) - c_prob = 0. - - p_weight = 1. # predicted path weight relatively to the center of the lane - d_poly = list((c_poly*c_prob + p_poly*p_prob*p_weight ) / (c_prob + p_prob*p_weight)) - return d_poly, c_poly, c_prob - -class OptPathPlanner(object): - def __init__(self, model): - self.model = model - self.dead = True - self.d_poly = [0., 0., 0., 0.] - self.last_model = 0. - self._path_pinv = compute_path_pinv() - - def update(self, cur_time, v_ego, md): - if md is not None: - # simple compute of the center of the lane - pts = [(x+y)/2 for x,y in zip(md.model.leftLane.points, md.model.rightLane.points)] - self.d_poly = model_polyfit(pts, self._path_pinv) - - self.last_model = cur_time - self.dead = False - elif cur_time - self.last_model > 0.5: - self.dead = True class PathPlanner(object): def __init__(self): - self.dead = True self.d_poly = [0., 0., 0., 0.] self.c_poly = [0., 0., 0., 0.] self.c_prob = 0. @@ -68,24 +11,47 @@ def __init__(self): self.lead_dist, self.lead_prob, self.lead_var = 0, 0, 1 self._path_pinv = compute_path_pinv() - def update(self, cur_time, v_ego, md): + self.lane_width_estimate = 3.7 + self.lane_width_certainty = 1.0 + self.lane_width = 3.7 + + def update(self, v_ego, md): if md is not None: - p_poly = model_polyfit(md.model.path.points, self._path_pinv) # predicted path - l_poly = model_polyfit(md.model.leftLane.points, self._path_pinv) # left line + p_poly = model_polyfit(md.model.path.points, self._path_pinv) # predicted path + l_poly = model_polyfit(md.model.leftLane.points, self._path_pinv) # left line r_poly = model_polyfit(md.model.rightLane.points, self._path_pinv) # right line - p_prob = 1. # model does not tell this probability yet, so set to 1 for now - l_prob = md.model.leftLane.prob # left line prob + p_prob = 1. # model does not tell this probability yet, so set to 1 for now + l_prob = md.model.leftLane.prob # left line prob r_prob = md.model.rightLane.prob # right line prob + # Find current lanewidth + lr_prob = l_prob * r_prob + self.lane_width_certainty += 0.05 * (lr_prob - self.lane_width_certainty) + current_lane_width = abs(l_poly[3] - r_poly[3]) + self.lane_width_estimate += 0.005 * (current_lane_width - self.lane_width_estimate) + speed_lane_width = interp(v_ego, [0., 31.], [3., 3.8]) + self.lane_width = self.lane_width_certainty * self.lane_width_estimate + \ + (1 - self.lane_width_certainty) * speed_lane_width + + lane_width_diff = abs(self.lane_width - current_lane_width) + lane_r_prob = interp(lane_width_diff, [0.3, 1.0], [1.0, 0.0]) + + r_prob *= lane_r_prob + self.lead_dist = md.model.lead.dist self.lead_prob = md.model.lead.prob self.lead_var = md.model.lead.std**2 # compute target path - self.d_poly, self.c_poly, self.c_prob = calc_desired_path(l_poly, r_poly, p_poly, l_prob, r_prob, p_prob, v_ego) + self.d_poly, self.c_poly, self.c_prob = calc_desired_path( + l_poly, r_poly, p_poly, l_prob, r_prob, p_prob, v_ego, self.lane_width) + + self.r_poly = r_poly + self.r_prob = r_prob + + self.l_poly = l_poly + self.l_prob = l_prob - self.last_model = cur_time - self.dead = False - elif cur_time - self.last_model > 0.5: - self.dead = True + self.p_poly = p_poly + self.p_prob = p_prob diff --git a/selfdrive/controls/lib/pid.py b/selfdrive/controls/lib/pid.py new file mode 100644 index 00000000000000..faf5355db5c2e9 --- /dev/null +++ b/selfdrive/controls/lib/pid.py @@ -0,0 +1,91 @@ +import numpy as np +from common.numpy_fast import clip, interp +import numbers + +class PIController(object): + def __init__(self, k_p, k_i, pos_limit=None, neg_limit=None, rate=100, sat_limit=0.8, convert=None): + self._k_p = k_p + self._k_i = k_i + + self.pos_limit = pos_limit + self.neg_limit = neg_limit + + self.sat_count_rate = 1.0 / rate + self.i_unwind_rate = 0.3 / rate + self.i_rate = 1.0 / rate + self.sat_limit = sat_limit + self.jerk_factor = 0.0 + self.convert = convert + + self.reset() + + @property + def k_p(self): + if isinstance(self._k_p, numbers.Number): + kp = self._k_p + else: + kp = interp(self.speed, self._k_p[0], self._k_p[1]) + + return (1.0 + self.jerk_factor) * kp + + @property + def k_i(self): + if isinstance(self._k_i, numbers.Number): + ki = self._k_i + else: + ki = interp(self.speed, self._k_i[0], self._k_i[1]) + + return (1.0 + self.jerk_factor) * ki + + def _check_saturation(self, control, override, error): + saturated = (control < self.neg_limit) or (control > self.pos_limit) + + if saturated and not override and abs(error) > 0.1: + self.sat_count += self.sat_count_rate + else: + self.sat_count -= self.sat_count_rate + + self.sat_count = clip(self.sat_count, 0.0, 1.0) + + return self.sat_count > self.sat_limit + + def reset(self): + self.p = 0.0 + self.i = 0.0 + self.sat_count = 0.0 + self.saturated = False + self.control = 0 + + def update(self, setpoint, measurement, speed=0.0, check_saturation=True, jerk_factor=0.0, override=False): + self.speed = speed + self.jerk_factor = jerk_factor + + error = float(setpoint - measurement) + self.p = error * self.k_p + + if override: + self.i -= self.i_unwind_rate * float(np.sign(self.i)) + else: + i = self.i + error * self.k_i * self.i_rate + control = self.p + i + + if self.convert is not None: + control = self.convert(control, speed=self.speed) + + # Update when changing i will move the control away from the limits + # or when i will move towards the sign of the error + if (error >= 0 and (control <= self.pos_limit or i < 0.0)) or \ + (error <= 0 and (control >= self.neg_limit or i > 0.0)): + self.i = i + + control = self.p + self.i + if self.convert is not None: + control = self.convert(control, speed=self.speed) + + if check_saturation: + self.saturated = self._check_saturation(control, override, error) + else: + self.saturated = False + + self.control = clip(control, self.neg_limit, self.pos_limit) + return self.control diff --git a/selfdrive/controls/lib/planner.py b/selfdrive/controls/lib/planner.py index e9e7adb6718e8d..9cc657c138499b 100755 --- a/selfdrive/controls/lib/planner.py +++ b/selfdrive/controls/lib/planner.py @@ -1,80 +1,93 @@ #!/usr/bin/env python -import os import zmq -import numpy as np +from common.realtime import sec_since_boot import selfdrive.messaging as messaging - from selfdrive.services import service_list -from common.realtime import sec_since_boot -from common.params import Params - -from selfdrive.swaglog import cloudlog -from cereal import car - -from selfdrive.controls.lib.pathplanner import OptPathPlanner +from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET from selfdrive.controls.lib.pathplanner import PathPlanner from selfdrive.controls.lib.adaptivecruise import AdaptiveCruise from selfdrive.controls.lib.fcw import ForwardCollisionWarning _DT = 0.01 # 100Hz + class Planner(object): def __init__(self, CP): context = zmq.Context() self.CP = CP self.live20 = messaging.sub_sock(context, service_list['live20'].port) self.model = messaging.sub_sock(context, service_list['model'].port) - + self.plan = messaging.pub_sock(context, service_list['plan'].port) - + self.last_md_ts = 0 self.last_l20_ts = 0 + self.last_model = 0. + self.last_l20 = 0. + self.model_dead = True + self.radar_dead = True + self.radar_errors = [] - if os.getenv("OPT") is not None: - self.PP = OptPathPlanner(self.model) - else: - self.PP = PathPlanner() + self.PP = PathPlanner() self.AC = AdaptiveCruise() self.FCW = ForwardCollisionWarning(_DT) # this runs whenever we get a packet that can change the plan - def update(self, CS, LoC): cur_time = sec_since_boot() md = messaging.recv_sock(self.model) if md is not None: self.last_md_ts = md.logMonoTime + self.last_model = cur_time + self.model_dead = False + if cur_time - self.last_model > 0.5: + self.model_dead = True + l20 = messaging.recv_sock(self.live20) if l20 is not None: self.last_l20_ts = l20.logMonoTime + self.last_l20 = cur_time + self.radar_dead = False + self.radar_errors = list(l20.live20.radarErrors) + if cur_time - self.last_l20 > 0.5: + self.radar_dead = True - self.PP.update(cur_time, CS.vEgo, md) + self.PP.update(CS.vEgo, md) # LoC.v_pid -> CS.vEgo # TODO: is this change okay? - self.AC.update(cur_time, CS.vEgo, CS.steeringAngle, LoC.v_pid, self.CP, l20) + self.AC.update(CS.vEgo, CS.steeringAngle, LoC.v_pid, self.CP, l20) # **** send the plan **** plan_send = messaging.new_message() plan_send.init('plan') + events = [] + if self.model_dead: + events.append(create_event('modelCommIssue', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) + if self.radar_dead or 'commIssue' in self.radar_errors: + events.append(create_event('radarCommIssue', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) + if 'fault' in self.radar_errors: + events.append(create_event('radarFault', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) + + plan_send.plan.events = events + plan_send.plan.mdMonoTime = self.last_md_ts plan_send.plan.l20MonoTime = self.last_l20_ts # lateral plan - plan_send.plan.lateralValid = not self.PP.dead - if plan_send.plan.lateralValid: - plan_send.plan.dPoly = map(float, self.PP.d_poly) + plan_send.plan.lateralValid = not self.model_dead + plan_send.plan.dPoly = map(float, self.PP.d_poly) + plan_send.plan.laneWidth = float(self.PP.lane_width) # longitudal plan - plan_send.plan.longitudinalValid = not self.AC.dead - if plan_send.plan.longitudinalValid: - plan_send.plan.vTarget = float(self.AC.v_target_lead) - plan_send.plan.aTargetMin = float(self.AC.a_target[0]) - plan_send.plan.aTargetMax = float(self.AC.a_target[1]) - plan_send.plan.jerkFactor = float(self.AC.jerk_factor) - plan_send.plan.hasLead = self.AC.has_lead + plan_send.plan.longitudinalValid = not self.radar_dead + plan_send.plan.vTarget = float(self.AC.v_target_lead) + plan_send.plan.aTargetMin = float(self.AC.a_target[0]) + plan_send.plan.aTargetMax = float(self.AC.a_target[1]) + plan_send.plan.jerkFactor = float(self.AC.jerk_factor) + plan_send.plan.hasLead = self.AC.has_lead # compute risk of collision events: fcw self.FCW.process(CS, self.AC) diff --git a/selfdrive/controls/lib/vehicle_model.py b/selfdrive/controls/lib/vehicle_model.py index 3c676fbeab98ed..91e26e4ff4bde1 100755 --- a/selfdrive/controls/lib/vehicle_model.py +++ b/selfdrive/controls/lib/vehicle_model.py @@ -1,33 +1,32 @@ #!/usr/bin/env python import numpy as np from numpy.linalg import inv -from selfdrive.car.honda.interface import CarInterface - -## dynamic bycicle model from "The Science of Vehicle Dynamics (2014), M. Guiggiani"## +# dynamic bycicle model from "The Science of Vehicle Dynamics (2014), M. Guiggiani"## # Xdot = A*X + B*U # where X = [v, r], with v and r lateral speed and rotational speed, respectively # and U is the steering angle (controller input) -# +# # A depends on longitudinal speed, u, and vehicle parameters CP + def create_dyn_state_matrices(u, CP): - A = np.zeros((2,2)) - B = np.zeros((2,1)) - A[0,0] = - (CP.cF + CP.cR)/(CP.m*u) - A[0,1] = - (CP.cF*CP.aF - CP.cR*CP.aR) / (CP.m*u) - u - A[1,0] = - (CP.cF*CP.aF - CP.cR*CP.aR) / (CP.j*u) - A[1,1] = - (CP.cF*CP.aF**2 + CP.cR*CP.aR**2) / (CP.j*u) - B[0,0] = (CP.cF + CP.chi*CP.cR) / CP.m / CP.sR - B[1,0] = (CP.cF*CP.aF - CP.chi*CP.cR*CP.aR) / CP.j / CP.sR + A = np.zeros((2, 2)) + B = np.zeros((2, 1)) + A[0, 0] = - (CP.cF + CP.cR) / (CP.m * u) + A[0, 1] = - (CP.cF * CP.aF - CP.cR * CP.aR) / (CP.m * u) - u + A[1, 0] = - (CP.cF * CP.aF - CP.cR * CP.aR) / (CP.j * u) + A[1, 1] = - (CP.cF * CP.aF**2 + CP.cR * CP.aR**2) / (CP.j * u) + B[0, 0] = (CP.cF + CP.chi * CP.cR) / CP.m / CP.sR + B[1, 0] = (CP.cF * CP.aF - CP.chi * CP.cR * CP.aR) / CP.j / CP.sR return A, B def kin_ss_sol(sa, u, CP): # kinematic solution, useful when speed ~ 0 - K = np.zeros((2,1)) - K[0,0] = CP.aR / CP.sR / CP.l * u - K[1,0] = 1. / CP.sR / CP.l * u + K = np.zeros((2, 1)) + K[0, 0] = CP.aR / CP.sR / CP.l * u + K[1, 0] = 1. / CP.sR / CP.l * u return K * sa @@ -36,13 +35,15 @@ def dyn_ss_sol(sa, u, CP): A, B = create_dyn_state_matrices(u, CP) return - np.matmul(inv(A), B) * sa + def calc_slip_factor(CP): # the slip factor is a measure of how the curvature changes with speed # it's positive for Oversteering vehicle, negative (usual case) otherwise return CP.m * (CP.cF * CP.aF - CP.cR * CP.aR) / (CP.l**2 * CP.cF * CP.cR) + class VehicleModel(object): - def __init__(self, CP, init_state=np.asarray([[0.],[0.]])): + def __init__(self, CP, init_state=np.asarray([[0.], [0.]])): self.dt = 0.1 lookahead = 2. # s self.steps = int(lookahead / self.dt) @@ -54,7 +55,7 @@ def update_state(self, state): self.state = state def steady_state_sol(self, sa, u): - # if the speed is too small we can't use the dynamic model + # if the speed is too small we can't use the dynamic model # (tire slip is undefined), we then use the kinematic model if u > 0.1: return dyn_ss_sol(sa, u, self.CP) @@ -63,13 +64,14 @@ def steady_state_sol(self, sa, u): def calc_curvature(self, sa, u): # this formula can be derived from state equations in steady state conditions + return self.curvature_factor(u) * sa / self.CP.sR + + def curvature_factor(self, u): sf = calc_slip_factor(self.CP) - return (1. - self.CP.chi)/(1. - sf * u**2) * sa / self.CP.sR / self.CP.l + return (1. - self.CP.chi)/(1. - sf * u**2) / self.CP.l def get_steer_from_curvature(self, curv, u): - # this function is the exact inverse of calc_curvature, returning steer angle given curvature - sf = calc_slip_factor(self.CP) - return self.CP.l * self.CP.sR * (1. - sf * u**2) / (1. - self.CP.chi) * curv + return curv * self.CP.sR * 1.0 / self.curvature_factor(u) def state_prediction(self, sa, u): # U is the matrix of the controls @@ -79,9 +81,10 @@ def state_prediction(self, sa, u): if __name__ == '__main__': + from selfdrive.car.toyota.interface import CarInterface # load car params - CP = CarInterface.get_params("HONDA CIVIC 2016 TOURING", {}) + CP = CarInterface.get_params("TOYOTA PRIUS 2017", {}) print CP VM = VehicleModel(CP) print VM.steady_state_sol(.1, 0.15) - + print calc_slip_factor(CP) diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index 00ee9182df1710..353b7a73a58a5a 100755 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -7,7 +7,7 @@ from fastcluster import linkage_vector import selfdrive.messaging as messaging from selfdrive.services import service_list -from selfdrive.controls.lib.latcontrol import calc_lookahead_offset +from selfdrive.controls.lib.latcontrol_helpers import calc_lookahead_offset from selfdrive.controls.lib.pathplanner import PathPlanner from selfdrive.controls.lib.radar_helpers import Track, Cluster, fcluster, RDR_TO_LDR from selfdrive.controls.lib.vehicle_model import VehicleModel @@ -84,7 +84,7 @@ def radard_thread(gctx=None): tracks = defaultdict(dict) - # Kalman filter stuff: + # Kalman filter stuff: ekfv = EKFV1D() speedSensorV = SimpleSensor(XV, 1, 2) @@ -121,7 +121,7 @@ def radard_thread(gctx=None): last_md_ts = md.logMonoTime # *** get path prediction from the model *** - PP.update(sec_since_boot(), v_ego, md) + PP.update(v_ego, md) # run kalman filter only if prob is high enough if PP.lead_prob > 0.7: @@ -156,7 +156,7 @@ def radard_thread(gctx=None): continue rpt = ar_pts[ids] - # align v_ego by a fixed time to align it with the radar measurement + # align v_ego by a fixed time to align it with the radar measurement cur_time = float(rk.frame)/rate v_ego_t_aligned = np.interp(cur_time - rdr_delay, v_ego_array[1], v_ego_array[0]) d_path = np.sqrt(np.amin((path_x - rpt[0]) ** 2 + (path_y - rpt[1]) ** 2)) @@ -175,7 +175,10 @@ def radard_thread(gctx=None): # publish tracks (debugging) dat = messaging.new_message() dat.init('liveTracks', len(tracks)) + #print "NEW TRACKS" for cnt, ids in enumerate(tracks.keys()): + #print "%5s %5s %5s %5s" % \ + # (ids, round(tracks[ids].dRel, 2), round(tracks[ids].vRel, 2), round(tracks[ids].yRel, 2)) dat.liveTracks[cnt].trackId = ids dat.liveTracks[cnt].dRel = float(tracks[ids].dRel) dat.liveTracks[cnt].yRel = float(tracks[ids].yRel) @@ -224,6 +227,7 @@ def radard_thread(gctx=None): dat.init('live20') dat.live20.mdMonoTime = last_md_ts dat.live20.canMonoTimes = list(rr.canMonoTimes) + dat.live20.radarErrors = list(rr.errors) dat.live20.l100MonoTime = last_l100_ts if lead_len > 0: lead_clusters[0].toLive20(dat.live20.leadOne) diff --git a/common/crash.py b/selfdrive/crash.py similarity index 75% rename from common/crash.py rename to selfdrive/crash.py index 06fe9a91325994..18c27ec1f505cc 100644 --- a/common/crash.py +++ b/selfdrive/crash.py @@ -2,7 +2,9 @@ import os import sys -if os.getenv("NOLOG"): +from selfdrive.swaglog import cloudlog + +if os.getenv("NOLOG") or os.getenv("NOCRASH"): def capture_exception(*exc_info): pass def bind_user(**kwargs): @@ -18,7 +20,9 @@ def install(): client = Client('https://1994756b5e6f41cf939a4c65de45f4f2:cefebaf3a8aa40d182609785f7189bd7@app.getsentry.com/77924', install_sys_hook=False, transport=HTTPTransport) - capture_exception = client.captureException + def capture_exception(*args, **kwargs): + client.captureException(*args, **kwargs) + cloudlog.error("crash", exc_info=kwargs.get('exc_info', 1)) def bind_user(**kwargs): client.user_context(kwargs) @@ -31,6 +35,6 @@ def install(): __excepthook__ = sys.excepthook def handle_exception(*exc_info): if exc_info[0] not in (KeyboardInterrupt, SystemExit): - client.captureException(exc_info=exc_info) + capture_exception(exc_info=exc_info) __excepthook__(*exc_info) sys.excepthook = handle_exception diff --git a/selfdrive/debug/can_printer.py b/selfdrive/debug/can_printer.py index aeab94ed8eeab4..115b54ea60f4ea 100755 --- a/selfdrive/debug/can_printer.py +++ b/selfdrive/debug/can_printer.py @@ -1,5 +1,6 @@ #!/usr/bin/env python import os +import sys import struct from collections import defaultdict from common.realtime import sec_since_boot @@ -8,14 +9,14 @@ from selfdrive.services import service_list -def can_printer(): +def can_printer(bus=0, max_msg=0x10000, addr="127.0.0.1"): context = zmq.Context() - logcan = messaging.sub_sock(context, service_list['can'].port) + logcan = messaging.sub_sock(context, service_list['can'].port, addr=addr) start = sec_since_boot() lp = sec_since_boot() msgs = defaultdict(list) - canbus = int(os.getenv("CAN", 0)) + canbus = int(os.getenv("CAN", bus)) while 1: can_recv = messaging.drain_sock(logcan, wait_for_one=True) for x in can_recv: @@ -27,10 +28,18 @@ def can_printer(): dd = chr(27) + "[2J" dd += "%5.2f\n" % (sec_since_boot() - start) for k,v in sorted(zip(msgs.keys(), map(lambda x: x[-1].encode("hex"), msgs.values()))): - dd += "%s(%6d) %s\n" % ("%04X(%4d)" % (k,k),len(msgs[k]), v) + if k < max_msg: + dd += "%s(%6d) %s\n" % ("%04X(%4d)" % (k,k),len(msgs[k]), v) print dd lp = sec_since_boot() if __name__ == "__main__": - can_printer() - + if len(sys.argv) > 3: + can_printer(int(sys.argv[1]), int(sys.argv[2]), sys.argv[3]) + elif len(sys.argv) > 2: + can_printer(int(sys.argv[1]), int(sys.argv[2])) + elif len(sys.argv) > 1: + can_printer(int(sys.argv[1])) + else: + can_printer() + diff --git a/selfdrive/debug/dump.py b/selfdrive/debug/dump.py index 2387d299fd623a..2d312408355410 100755 --- a/selfdrive/debug/dump.py +++ b/selfdrive/debug/dump.py @@ -27,5 +27,5 @@ if args.raw: hexdump(sock.recv()) else: - print messaging.recv_sock(sock) + print messaging.recv_one(sock) diff --git a/selfdrive/debug/getframes/getframes.py b/selfdrive/debug/getframes/getframes.py index 5c63b8986d01b5..498716faf1dbee 100755 --- a/selfdrive/debug/getframes/getframes.py +++ b/selfdrive/debug/getframes/getframes.py @@ -42,26 +42,26 @@ } buf_info; } VisionStreamBufs; -typedef struct VisionBuf { +typedef struct VIPCBuf { int fd; size_t len; void* addr; -} VisionBuf; +} VIPCBuf; -typedef struct VisionBufExtra { +typedef struct VIPCBufExtra { uint32_t frame_id; // only for yuv -} VisionBufExtra; +} VIPCBufExtra; typedef struct VisionStream { int ipc_fd; int last_idx; int num_bufs; VisionStreamBufs bufs_info; - VisionBuf *bufs; + VIPCBuf *bufs; } VisionStream; int visionstream_init(VisionStream *s, VisionStreamType type, bool tbuffer, VisionStreamBufs *out_bufs_info); -VisionBuf* visionstream_get(VisionStream *s, VisionBufExtra *out_extra); +VIPCBuf* visionstream_get(VisionStream *s, VIPCBufExtra *out_extra); void visionstream_destroy(VisionStream *s); """ diff --git a/selfdrive/debug/test_carcontroller.py b/selfdrive/debug/test_carcontroller.py index 4d9dcbda86abbc..d672cfb3d3c371 100755 --- a/selfdrive/debug/test_carcontroller.py +++ b/selfdrive/debug/test_carcontroller.py @@ -28,6 +28,7 @@ sendcan = messaging.pub_sock(context, service_list['sendcan'].port) CI, CP = get_car(logcan, sendcan) + CC = car.CarControl.new_message() rk = Ratekeeper(100) @@ -56,11 +57,11 @@ print axis_values, button_values # **** handle car **** - CS = CI.update() + CS = CI.update(CC) #print CS - CC = car.CarControl.new_message() + CC.enabled = True CC.gas = float(np.clip(-axis_values[1], 0, 1.0)) diff --git a/selfdrive/debug/test_carstate.py b/selfdrive/debug/test_carstate.py index f88c521d1f6670..98d2f7fc154427 100755 --- a/selfdrive/debug/test_carstate.py +++ b/selfdrive/debug/test_carstate.py @@ -1,6 +1,7 @@ #!/usr/bin/env python import os import zmq +from cereal import car import selfdrive.messaging as messaging from selfdrive.services import service_list @@ -36,8 +37,9 @@ def test_loop(): "False"] while 1: + CC = car.CarControl.new_message() # read CAN - CS = CI.update() + CS = CI.update(CC) while eval(states[state]) == True: state += 1 diff --git a/selfdrive/logcatd/Makefile b/selfdrive/logcatd/Makefile index 0d3423647aa0b9..83684abe5b9837 100644 --- a/selfdrive/logcatd/Makefile +++ b/selfdrive/logcatd/Makefile @@ -2,6 +2,7 @@ CC = clang CXX = clang++ PHONELIBS = ../../phonelibs +BASEDIR = ../.. WARN_FLAGS = -Werror=implicit-function-declaration \ -Werror=incompatible-pointer-types \ diff --git a/selfdrive/loggerd/config.py b/selfdrive/loggerd/config.py index 0759ae2663a55a..9d1c3120fbaec5 100644 --- a/selfdrive/loggerd/config.py +++ b/selfdrive/loggerd/config.py @@ -1,4 +1,9 @@ import os -ROOT = '/data/media/0/realdata/' +if os.environ.get('LOGGERD_ROOT', False): + ROOT = os.environ['LOGGERD_ROOT'] + print("Custom loggerd root: ", ROOT) +else: + ROOT = '/data/media/0/realdata/' + SEGMENT_LENGTH = 60 diff --git a/selfdrive/loggerd/loggerd b/selfdrive/loggerd/loggerd index c3bf23ddb90a95..579b4062846224 100755 Binary files a/selfdrive/loggerd/loggerd and b/selfdrive/loggerd/loggerd differ diff --git a/selfdrive/manager.py b/selfdrive/manager.py index 136cd7345707a2..77b9395c38c205 100755 --- a/selfdrive/manager.py +++ b/selfdrive/manager.py @@ -1,57 +1,61 @@ #!/usr/bin/env python import os +import time -# check if NEOS update is required -while 1: - if ((not os.path.isfile("/VERSION") - or int(open("/VERSION").read()) < 3) - and not os.path.isfile("/data/media/0/noupdate")): - os.system("curl -o /tmp/updater https://openpilot.comma.ai/updater && chmod +x /tmp/updater && /tmp/updater") - else: - break +if os.path.isfile("/init.qcom.rc"): + # check if NEOS update is required + while 1: + if ((not os.path.isfile("/VERSION") + or int(open("/VERSION").read()) < 3) + and not os.path.isfile("/data/media/0/noupdate")): + os.system("curl -o /tmp/updater https://openpilot.comma.ai/updater && chmod +x /tmp/updater && /tmp/updater") + else: + break + time.sleep(10) import sys -import time import importlib import subprocess import signal import traceback -import usb1 from multiprocessing import Process -from selfdrive.services import service_list +from common.basedir import BASEDIR + +sys.path.append(os.path.join(BASEDIR, "pyextra")) +os.environ['BASEDIR'] = BASEDIR +import usb1 import hashlib import zmq - from setproctitle import setproctitle +from selfdrive.services import service_list + from selfdrive.swaglog import cloudlog import selfdrive.messaging as messaging from selfdrive.thermal import read_thermal from selfdrive.registration import register from selfdrive.version import version +import selfdrive.crash as crash -import common.crash as crash from common.params import Params from selfdrive.loggerd.config import ROOT -BASEDIR = os.path.join(os.path.dirname(os.path.realpath(__file__)), "../") - # comment out anything you don't want to run managed_processes = { "uploader": "selfdrive.loggerd.uploader", "controlsd": "selfdrive.controls.controlsd", "radard": "selfdrive.controls.radard", - "loggerd": ("loggerd", ["./loggerd"]), + "loggerd": ("selfdrive/loggerd", ["./loggerd"]), "logmessaged": "selfdrive.logmessaged", "tombstoned": "selfdrive.tombstoned", - "logcatd": ("logcatd", ["./logcatd"]), - "proclogd": ("proclogd", ["./proclogd"]), - "boardd": ("boardd", ["./boardd"]), # switch to c++ boardd - "ui": ("ui", ["./ui"]), - "visiond": ("visiond", ["./visiond"]), - "sensord": ("sensord", ["./sensord"]), } + "logcatd": ("selfdrive/logcatd", ["./logcatd"]), + "proclogd": ("selfdrive/proclogd", ["./proclogd"]), + "boardd": ("selfdrive/boardd", ["./boardd"]), # switch to c++ boardd + "ui": ("selfdrive/ui", ["./ui"]), + "visiond": ("selfdrive/visiond", ["./visiond"]), + "sensord": ("selfdrive/sensord", ["./sensord"]), } running = {} def get_running(): @@ -116,13 +120,28 @@ def start_managed_process(name): running[name] = Process(name=name, target=launcher, args=(proc, gctx)) else: pdir, pargs = proc - cwd = os.path.join(BASEDIR, "selfdrive") - if pdir is not None: - cwd = os.path.join(cwd, pdir) + cwd = os.path.join(BASEDIR, pdir) cloudlog.info("starting process %s" % name) running[name] = Process(name=name, target=nativelauncher, args=(pargs, cwd)) running[name].start() +def prepare_managed_process(p): + proc = managed_processes[p] + if isinstance(proc, basestring): + # import this python + cloudlog.info("preimporting %s" % proc) + importlib.import_module(proc) + else: + # build this process + cloudlog.info("building %s" % (proc,)) + try: + subprocess.check_call(["make", "-j4"], cwd=os.path.join(BASEDIR, proc[0])) + except subprocess.CalledProcessError: + # make clean if the build failed + cloudlog.info("building %s failed, make clean" % (proc, )) + subprocess.check_call(["make", "clean"], cwd=os.path.join(BASEDIR, proc[0])) + subprocess.check_call(["make", "-j4"], cwd=os.path.join(BASEDIR, proc[0])) + def kill_managed_process(name): if name not in running or name not in managed_processes: return @@ -212,9 +231,67 @@ def system(cmd): except subprocess.CalledProcessError, e: cloudlog.event("running failed", cmd=e.cmd, - output=e.output, + output=e.output[-1024:], returncode=e.returncode) +# TODO: this is not proper gating for EON +try: + from smbus2 import SMBus + EON = True +except ImportError: + EON = False + +def setup_eon_fan(): + if not EON: + return + + os.system("echo 2 > /sys/module/dwc3_msm/parameters/otg_switch") + + bus = SMBus(7, force=True) + bus.write_byte_data(0x21, 0x10, 0xf) # mask all interrupts + bus.write_byte_data(0x21, 0x03, 0x1) # set drive current and global interrupt disable + bus.write_byte_data(0x21, 0x02, 0x2) # needed? + bus.write_byte_data(0x21, 0x04, 0x4) # manual override source + bus.close() + +last_eon_fan_val = None +def set_eon_fan(val): + global last_eon_fan_val + + if not EON: + return + + if last_eon_fan_val is None or last_eon_fan_val != val: + bus = SMBus(7, force=True) + bus.write_byte_data(0x21, 0x04, 0x2) + bus.write_byte_data(0x21, 0x03, (val*2)+1) + bus.write_byte_data(0x21, 0x04, 0x4) + bus.close() + last_eon_fan_val = val + + +# temp thresholds to control fan speed - high hysteresis +_TEMP_THRS_H = [50., 65., 80., 10000] +# temp thresholds to control fan speed - low hysteresis +_TEMP_THRS_L = [42.5, 57.5, 72.5, 10000] +# fan speed options +_FAN_SPEEDS = [0, 16384, 32768, 65535] + +def handle_fan(max_temp, fan_speed): + new_speed_h = next(speed for speed, temp_h in zip(_FAN_SPEEDS, _TEMP_THRS_H) if temp_h > max_temp) + new_speed_l = next(speed for speed, temp_l in zip(_FAN_SPEEDS, _TEMP_THRS_L) if temp_l > max_temp) + + if new_speed_h > fan_speed: + # update speed if using the high thresholds results in fan speed increment + fan_speed = new_speed_h + elif new_speed_l < fan_speed: + # update speed if using the low thresholds results in fan speed decrement + fan_speed = new_speed_l + + set_eon_fan(fan_speed/16384) + + return fan_speed + def manager_thread(): global baseui_running @@ -233,24 +310,18 @@ def manager_thread(): start_managed_process("ui") manage_baseui(True) - panda = False - if os.getenv("NOBOARD") is None: - # *** wait for the board *** - panda = wait_for_device() == 0x2300 - - # flash the device - if os.getenv("NOPROG") is None: - # flash the board - boarddir = os.path.join(BASEDIR, "panda/board/") - mkfile = "Makefile" if panda else "Makefile.legacy" - print "using", mkfile - system("cd %s && make -f %s" % (boarddir, mkfile)) + # do this before panda flashing + setup_eon_fan() - start_managed_process("boardd") + if os.getenv("NOBOARD") is None: + from panda import ensure_st_up_to_date + ensure_st_up_to_date() + start_managed_process("boardd") started = False logger_dead = False count = 0 + fan_speed = 0 # set 5 second timeout on health socket # 5x slower than expected @@ -274,12 +345,15 @@ def manager_thread(): msg.thermal.batteryPercent = int(f.read()) with open("/sys/class/power_supply/battery/status") as f: msg.thermal.batteryStatus = f.read().strip() - thermal_sock.send(msg.to_bytes()) - print msg # TODO: add car battery voltage check max_temp = max(msg.thermal.cpu0, msg.thermal.cpu1, msg.thermal.cpu2, msg.thermal.cpu3) / 10.0 + fan_speed = handle_fan(max_temp, fan_speed) + msg.thermal.fanSpeed = fan_speed + + thermal_sock.send(msg.to_bytes()) + print msg # uploader is gated based on the phone temperature if max_temp > 85.0: @@ -352,21 +426,7 @@ def manager_prepare(): # build all processes os.chdir(os.path.dirname(os.path.abspath(__file__))) for p in managed_processes: - proc = managed_processes[p] - if isinstance(proc, basestring): - # import this python - cloudlog.info("preimporting %s" % proc) - importlib.import_module(proc) - else: - # build this process - cloudlog.info("building %s" % (proc,)) - try: - subprocess.check_call(["make", "-j4"], cwd=proc[0]) - except subprocess.CalledProcessError: - # make clean if the build failed - cloudlog.info("building %s failed, make clean" % (proc, )) - subprocess.check_call(["make", "clean"], cwd=proc[0]) - subprocess.check_call(["make", "-j4"], cwd=proc[0]) + prepare_managed_process(p) # install apks installed = get_installed_apks() diff --git a/selfdrive/proclogd/Makefile b/selfdrive/proclogd/Makefile index cd970c97e7dc91..e7a6a57f1dc93e 100644 --- a/selfdrive/proclogd/Makefile +++ b/selfdrive/proclogd/Makefile @@ -2,6 +2,7 @@ CC = clang CXX = clang++ PHONELIBS = ../../phonelibs +BASEDIR = ../.. WARN_FLAGS = -Werror=implicit-function-declaration \ -Werror=incompatible-pointer-types \ diff --git a/selfdrive/radar/nidec/interface.py b/selfdrive/radar/nidec/interface.py index 554ce944c629f4..dc8b5d073eb5a8 100755 --- a/selfdrive/radar/nidec/interface.py +++ b/selfdrive/radar/nidec/interface.py @@ -2,8 +2,7 @@ import os import numpy as np -from selfdrive.car.honda.can_parser import CANParser -from selfdrive.can.parser import CANParser as CANParserC +from selfdrive.can.parser import CANParser from selfdrive.boardd.boardd import can_capnp_to_can_list @@ -14,29 +13,27 @@ from selfdrive.services import service_list import selfdrive.messaging as messaging -NEW_CAN = os.getenv("OLD_CAN") is None - -def _create_radard_can_parser(): +def _create_nidec_can_parser(): dbc_f = 'acura_ilx_2016_nidec.dbc' - radar_messages = range(0x430, 0x43A) + range(0x440, 0x446) - signals = zip(['LONG_DIST'] * 16 + ['NEW_TRACK'] * 16 + ['LAT_DIST'] * 16 + - ['REL_SPEED'] * 16, radar_messages * 4, - [255] * 16 + [1] * 16 + [0] * 16 + [0] * 16) - checks = zip(radar_messages, [20]*16) + radar_messages = [0x400] + range(0x430, 0x43A) + range(0x440, 0x446) + signals = zip(['RADAR_STATE'] + + ['LONG_DIST'] * 16 + ['NEW_TRACK'] * 16 + ['LAT_DIST'] * 16 + + ['REL_SPEED'] * 16, + [0x400] + radar_messages[1:] * 4, + [0] + [255] * 16 + [1] * 16 + [0] * 16 + [0] * 16) + checks = zip([0x445], [20]) - if NEW_CAN: - return CANParserC(os.path.splitext(dbc_f)[0], signals, checks, 1) - else: - return CANParser(dbc_f, signals, checks) + return CANParser(os.path.splitext(dbc_f)[0], signals, checks, 1) class RadarInterface(object): def __init__(self): # radar self.pts = {} self.track_id = 0 + self.radar_fault = False # Nidec - self.rcp = _create_radard_can_parser() + self.rcp = _create_nidec_can_parser() context = zmq.Context() self.logcan = messaging.sub_sock(context, service_list['can'].port) @@ -44,39 +41,19 @@ def __init__(self): def update(self): canMonoTimes = [] - if NEW_CAN: - updated_messages = set() - while 1: - tm = int(sec_since_boot() * 1e9) - updated_messages.update(self.rcp.update(tm, True)) - if 0x445 in updated_messages: - break - else: - can_pub_radar = [] - - # TODO: can hang if no packets show up - while 1: - for a in messaging.drain_sock(self.logcan, wait_for_one=True): - canMonoTimes.append(a.logMonoTime) - can_pub_radar.extend(can_capnp_to_can_list(a.can, [1, 3])) - - # only run on the 0x445 packets, used for timing - if any(x[0] == 0x445 for x in can_pub_radar): - break - - updated_messages = self.rcp.update_can(can_pub_radar) - - ret = car.RadarState.new_message() - errors = [] - if not self.rcp.can_valid: - errors.append("notValid") - ret.errors = errors - ret.canMonoTimes = canMonoTimes + updated_messages = set() + while 1: + tm = int(sec_since_boot() * 1e9) + updated_messages.update(self.rcp.update(tm, True)) + if 0x445 in updated_messages: + break for ii in updated_messages: cpt = self.rcp.vl[ii] - #print cpt - if cpt['LONG_DIST'] < 255: + if ii == 0x400: + # check for radar faults + self.radar_fault = cpt['RADAR_STATE'] != 0x79 + elif cpt['LONG_DIST'] < 255: if ii not in self.pts or cpt['NEW_TRACK']: self.pts[ii] = car.RadarState.RadarPoint.new_message() self.pts[ii].trackId = self.track_id @@ -90,6 +67,15 @@ def update(self): if ii in self.pts: del self.pts[ii] + ret = car.RadarState.new_message() + errors = [] + if not self.rcp.can_valid: + errors.append("commIssue") + if self.radar_fault: + errors.append("fault") + ret.errors = errors + ret.canMonoTimes = canMonoTimes + ret.points = self.pts.values() return ret diff --git a/selfdrive/sensord/sensord b/selfdrive/sensord/sensord index 1e28cf22c92aac..fc78d8ff800c89 100755 Binary files a/selfdrive/sensord/sensord and b/selfdrive/sensord/sensord differ diff --git a/selfdrive/service_list.yaml b/selfdrive/service_list.yaml index e6a1ce2144233a..b2d3cc07681aaa 100644 --- a/selfdrive/service_list.yaml +++ b/selfdrive/service_list.yaml @@ -43,6 +43,9 @@ lidarPts: [8030, true] procLog: [8031, true] gpsLocationExternal: [8032, true] ubloxGnss: [8033, true] +clocks: [8034, true] +liveMpc: [8035, true] + testModel: [8040, false] # manager -- base process to manage starting and stopping of all others diff --git a/selfdrive/test/plant/maneuver.py b/selfdrive/test/plant/maneuver.py index 80b2fcd2b9dc3e..8117b8b193d824 100644 --- a/selfdrive/test/plant/maneuver.py +++ b/selfdrive/test/plant/maneuver.py @@ -66,6 +66,8 @@ def evaluate(self): jerk_factor=last_live100.jerkFactor, a_target_min=last_live100.aTargetMin, a_target_max=last_live100.aTargetMax) + print "maneuver end" + return (None, plot) diff --git a/selfdrive/test/plant/plant.py b/selfdrive/test/plant/plant.py index ec9e5b590f6c20..26dcc5b2295190 100755 --- a/selfdrive/test/plant/plant.py +++ b/selfdrive/test/plant/plant.py @@ -13,10 +13,10 @@ from selfdrive.services import service_list from selfdrive.config import CruiseButtons from selfdrive.car.honda.hondacan import fix -from selfdrive.car.honda.carstate import get_can_parser +from selfdrive.car.honda.carstate import get_can_signals from selfdrive.boardd.boardd import can_capnp_to_can_list, can_list_to_can_capnp -from selfdrive.car.honda.can_parser import CANParser +from selfdrive.car.honda.old_can_parser import CANParser from selfdrive.car.honda.interface import CarInterface from cereal import car @@ -81,6 +81,11 @@ def get_car_can_parser(): ] return CANParser(dbc_f, signals, checks) +def to_3_byte(x): + return struct.pack("!H", int(x)).encode("hex")[1:] + +def to_3s_byte(x): + return struct.pack("!h", int(x)).encode("hex")[1:] class Plant(object): messaging_initialized = False @@ -142,11 +147,12 @@ def current_time(self): return float(self.rk.frame) / self.rate def step(self, v_lead=0.0, cruise_buttons=None, grade=0.0, publish_model = True): - # dbc_f, sgs, ivs, msgs, cks_msgs, frqs = initialize_can_struct(self.civic, self.brake_only) - cp2 = get_can_parser(CP) - sgs = cp2._sgs - msgs = cp2._msgs - cks_msgs = cp2.msgs_ck + gen_dbc, gen_signals, gen_checks = get_can_signals(CP) + sgs = [s[0] for s in gen_signals] + msgs = [s[1] for s in gen_signals] + cks_msgs = set(check[0] for check in gen_checks) + cks_msgs.add(0x18F) + cks_msgs.add(0x30C) # ******** get messages sent to the car ******** can_msgs = [] @@ -212,10 +218,8 @@ def step(self, v_lead=0.0, cruise_buttons=None, grade=0.0, publish_model = True) self.user_brake, self.steer_error, self.brake_error, self.brake_error, self.gear_shifter, self.main_on, self.acc_status, self.pedal_gas, self.cruise_setting, - # left_blinker, right_blinker, counter - 0,0,0, - # interceptor_gas - 0,0] + # append one more zero for gas interceptor + 0,0,0,0] # TODO: publish each message at proper frequency can_msgs = [] @@ -236,15 +240,12 @@ def step(self, v_lead=0.0, cruise_buttons=None, grade=0.0, publish_model = True) # add the radar message # TODO: use the DBC - def to_3_byte(x): - return struct.pack("!H", int(x)).encode("hex")[1:] - - def to_3s_byte(x): - return struct.pack("!h", int(x)).encode("hex")[1:] + radar_state_msg = '\x79\x00\x00\x00\x00\x00\x00\x00' radar_msg = to_3_byte(d_rel*16.0) + \ to_3_byte(int(lateral_pos_rel*16.0)&0x3ff) + \ to_3s_byte(int(v_rel*32.0)) + \ "0f00000" + can_msgs.append([0x400, 0, radar_state_msg, 1]) can_msgs.append([0x445, 0, radar_msg.decode("hex"), 1]) Plant.logcan.send(can_list_to_can_capnp(can_msgs).to_bytes()) diff --git a/selfdrive/test/plant/plant_ui.py b/selfdrive/test/plant/plant_ui.py new file mode 100755 index 00000000000000..fed1234ca70ae6 --- /dev/null +++ b/selfdrive/test/plant/plant_ui.py @@ -0,0 +1,122 @@ +#!/usr/bin/env python +import pygame +from plant import Plant +from selfdrive.config import CruiseButtons +import numpy as np +import selfdrive.messaging as messaging +import math + +CAR_WIDTH = 2.0 +CAR_LENGTH = 4.5 + +METER = 8 + +def rot_center(image, angle): + """rotate an image while keeping its center and size""" + orig_rect = image.get_rect() + rot_image = pygame.transform.rotate(image, angle) + rot_rect = orig_rect.copy() + rot_rect.center = rot_image.get_rect().center + rot_image = rot_image.subsurface(rot_rect).copy() + return rot_image + +def car_w_color(c): + car = pygame.Surface((METER*CAR_LENGTH, METER*CAR_LENGTH)) + car.set_alpha(0) + car.fill((10,10,10)) + car.set_alpha(128) + pygame.draw.rect(car, c, (METER*1.25, 0, METER*CAR_WIDTH, METER*CAR_LENGTH), 1) + return car + +if __name__ == "__main__": + pygame.init() + display = pygame.display.set_mode((1000, 1000)) + pygame.display.set_caption('Plant UI') + + car = car_w_color((255,0,255)) + leadcar = car_w_color((255,0,0)) + + carx, cary, heading = 10.0, 50.0, 0.0 + + plant = Plant(100, distance_lead = 40.0) + + control_offset = 2.0 + control_pts = zip(np.arange(0, 100.0, 10.0), [50.0 + control_offset]*10) + + def pt_to_car(pt): + x,y = pt + x -= carx + y -= cary + rx = x * math.cos(-heading) + y * -math.sin(-heading) + ry = x * math.sin(-heading) + y * math.cos(-heading) + return rx, ry + + def pt_from_car(pt): + x,y = pt + rx = x * math.cos(heading) + y * -math.sin(heading) + ry = x * math.sin(heading) + y * math.cos(heading) + rx += carx + ry += cary + return rx, ry + + while 1: + if plant.rk.frame%100 >= 20 and plant.rk.frame%100 <= 25: + cruise_buttons = CruiseButtons.RES_ACCEL + else: + cruise_buttons = 0 + + md = messaging.new_message() + md.init('model') + md.model.frameId = 0 + for x in [md.model.path, md.model.leftLane, md.model.rightLane]: + x.points = [0.0]*50 + x.prob = 0.0 + x.std = 1.0 + + car_pts = map(pt_to_car, control_pts) + + print car_pts + + car_poly = np.polyfit([x[0] for x in car_pts], [x[1] for x in car_pts], 3) + md.model.path.points = np.polyval(car_poly, np.arange(0, 50)).tolist() + md.model.path.prob = 1.0 + Plant.model.send(md.to_bytes()) + + plant.step(cruise_buttons = cruise_buttons, v_lead = 2.0, publish_model = False) + + display.fill((10,10,10)) + + carx += plant.speed * plant.ts * math.cos(heading) + cary += plant.speed * plant.ts * math.sin(heading) + + # positive steering angle = steering right + print plant.angle_steer + heading += plant.angle_steer * plant.ts + print heading + + # draw my car + display.blit(pygame.transform.rotate(car, 90-math.degrees(heading)), (carx*METER, cary*METER)) + + # draw control pts + for x,y in control_pts: + pygame.draw.circle(display, (255,255,0), (int(x * METER),int(y * METER)), 2) + + # draw path + path_pts = zip(np.arange(0, 50), md.model.path.points) + + for x,y in path_pts: + x,y = pt_from_car((x,y)) + pygame.draw.circle(display, (0,255,0), (int(x * METER),int(y * METER)), 1) + + """ + # draw lead car + dl = (plant.distance_lead - plant.distance) + 4.5 + lx = carx + dl * math.cos(heading) + ly = cary + dl * math.sin(heading) + + display.blit(pygame.transform.rotate(leadcar, 90-math.degrees(heading)), (lx*METER, ly*METER)) + """ + + pygame.display.flip() + + diff --git a/selfdrive/test/plant/runtest.sh b/selfdrive/test/plant/runtest.sh deleted file mode 100755 index a955d9f9369ace..00000000000000 --- a/selfdrive/test/plant/runtest.sh +++ /dev/null @@ -1,14 +0,0 @@ -#!/bin/bash - -export OPTEST=1 -export OLD_CAN=1 - -pushd ../../controls -./controlsd.py & -pid1=$! -./radard.py & -pid2=$! -trap "trap - SIGTERM && kill $pid1 && kill $pid2" SIGINT SIGTERM EXIT -popd -mkdir -p out -MPLBACKEND=svg ./runtracks.py out diff --git a/selfdrive/test/plant/runtracks.py b/selfdrive/test/plant/runtracks.py deleted file mode 100755 index 7647e2f58a2ce9..00000000000000 --- a/selfdrive/test/plant/runtracks.py +++ /dev/null @@ -1,207 +0,0 @@ -#!/usr/bin/env python -import sys -import time, json - -from selfdrive.test.plant import plant -from selfdrive.config import Conversions as CV, CruiseButtons as CB -from maneuver import * - -maneuvers = [ - Maneuver( - 'while cruising at 40 mph, change cruise speed to 50mph', - duration=30., - initial_speed = 40. * CV.MPH_TO_MS, - cruise_button_presses = [(CB.DECEL_SET, 2.), (0, 2.3), - (CB.RES_ACCEL, 10.), (0, 10.1), - (CB.RES_ACCEL, 10.2), (0, 10.3)] - ), - Maneuver( - 'while cruising at 60 mph, change cruise speed to 50mph', - duration=30., - initial_speed=60. * CV.MPH_TO_MS, - cruise_button_presses = [(CB.DECEL_SET, 2.), (0, 2.3), - (CB.DECEL_SET, 10.), (0, 10.1), - (CB.DECEL_SET, 10.2), (0, 10.3)] - ), - Maneuver( - 'while cruising at 20mph, grade change +10%', - duration=25., - initial_speed=20. * CV.MPH_TO_MS, - cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)], - grade_values = [0., 0., 1.0], - grade_breakpoints = [0., 10., 11.] - ), - Maneuver( - 'while cruising at 20mph, grade change -10%', - duration=25., - initial_speed=20. * CV.MPH_TO_MS, - cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)], - grade_values = [0., 0., -1.0], - grade_breakpoints = [0., 10., 11.] - ), - Maneuver( - 'approaching a 40mph car while cruising at 60mph from 100m away', - duration=30., - initial_speed = 60. * CV.MPH_TO_MS, - lead_relevancy=True, - initial_distance_lead=100., - speed_lead_values = [40.*CV.MPH_TO_MS, 40.*CV.MPH_TO_MS], - speed_lead_breakpoints = [0., 100.], - cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)] - ), - Maneuver( - 'approaching a 0mph car while cruising at 40mph from 150m away', - duration=30., - initial_speed = 40. * CV.MPH_TO_MS, - lead_relevancy=True, - initial_distance_lead=150., - speed_lead_values = [0.*CV.MPH_TO_MS, 0.*CV.MPH_TO_MS], - speed_lead_breakpoints = [0., 100.], - cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)] - ), - Maneuver( - 'steady state following a car at 20m/s, then lead decel to 0mph at 1m/s^2', - duration=50., - initial_speed = 20., - lead_relevancy=True, - initial_distance_lead=35., - speed_lead_values = [20.*CV.MPH_TO_MS, 20.*CV.MPH_TO_MS, 0.*CV.MPH_TO_MS], - speed_lead_breakpoints = [0., 15., 35.0], - cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)] - ), - Maneuver( - 'steady state following a car at 20m/s, then lead decel to 0mph at 2m/s^2', - duration=50., - initial_speed = 20., - lead_relevancy=True, - initial_distance_lead=35., - speed_lead_values = [20.*CV.MPH_TO_MS, 20.*CV.MPH_TO_MS, 0.*CV.MPH_TO_MS], - speed_lead_breakpoints = [0., 15., 25.0], - cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)] - ), - Maneuver( - 'starting at 0mph, approaching a stopped car 100m away', - duration=30., - initial_speed = 0., - lead_relevancy=True, - initial_distance_lead=100., - cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3), - (CB.RES_ACCEL, 1.4), (0.0, 1.5), - (CB.RES_ACCEL, 1.6), (0.0, 1.7), - (CB.RES_ACCEL, 1.8), (0.0, 1.9)] - ), - Maneuver( - "following a car at 60mph, lead accel and decel at 0.5m/s^2 every 2s", - duration=25., - initial_speed=30., - lead_relevancy=True, - initial_distance_lead=49., - speed_lead_values=[30.,30.,29.,31.,29.,31.,29.], - speed_lead_breakpoints=[0., 6., 8., 12.,16.,20.,24.], - cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3), - (CB.RES_ACCEL, 1.4), (0.0, 1.5), - (CB.RES_ACCEL, 1.6), (0.0, 1.7)] - ), - Maneuver( - "following a car at 10mph, stop and go at 1m/s2 lead dece1 and accel", - duration=70., - initial_speed=10., - lead_relevancy=True, - initial_distance_lead=20., - speed_lead_values=[10., 0., 0., 10., 0.,10.], - speed_lead_breakpoints=[10., 20., 30., 40., 50., 60.], - cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3), - (CB.RES_ACCEL, 1.4), (0.0, 1.5), - (CB.RES_ACCEL, 1.6), (0.0, 1.7)] - ), - Maneuver( - "green light: stopped behind lead car, lead car accelerates at 1.5 m/s", - duration=30., - initial_speed=0., - lead_relevancy=True, - initial_distance_lead=4., - speed_lead_values=[0, 0 , 45], - speed_lead_breakpoints=[0, 10., 40.], - cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3), - (CB.RES_ACCEL, 1.4), (0.0, 1.5), - (CB.RES_ACCEL, 1.6), (0.0, 1.7), - (CB.RES_ACCEL, 1.8), (0.0, 1.9), - (CB.RES_ACCEL, 2.0), (0.0, 2.1), - (CB.RES_ACCEL, 2.2), (0.0, 2.3)] - ), - Maneuver( - "stop and go with 1m/s2 lead decel and accel, with full stops", - duration=70., - initial_speed=0., - lead_relevancy=True, - initial_distance_lead=20., - speed_lead_values=[10., 0., 0., 10., 0., 0.] , - speed_lead_breakpoints=[10., 20., 30., 40., 50., 60.], - cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3), - (CB.RES_ACCEL, 1.4), (0.0, 1.5), - (CB.RES_ACCEL, 1.6), (0.0, 1.7)] - ), - Maneuver( - "accelerate from 20 while lead vehicle decelerates from 40 to 20 at 1m/s2", - duration=30., - initial_speed=10., - lead_relevancy=True, - initial_distance_lead=10., - speed_lead_values=[20., 10.], - speed_lead_breakpoints=[1., 11.], - cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3), - (CB.RES_ACCEL, 1.4), (0.0, 1.5), - (CB.RES_ACCEL, 1.6), (0.0, 1.7), - (CB.RES_ACCEL, 1.8), (0.0, 1.9), - (CB.RES_ACCEL, 2.0), (0.0, 2.1), - (CB.RES_ACCEL, 2.2), (0.0, 2.3)] - ), - Maneuver( - "accelerate from 20 while lead vehicle decelerates from 40 to 0 at 2m/s2", - duration=30., - initial_speed=10., - lead_relevancy=True, - initial_distance_lead=10., - speed_lead_values=[20., 0.], - speed_lead_breakpoints=[1., 11.], - cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3), - (CB.RES_ACCEL, 1.4), (0.0, 1.5), - (CB.RES_ACCEL, 1.6), (0.0, 1.7), - (CB.RES_ACCEL, 1.8), (0.0, 1.9), - (CB.RES_ACCEL, 2.0), (0.0, 2.1), - (CB.RES_ACCEL, 2.2), (0.0, 2.3)] - ) -] - -css_style = """ -.maneuver_title { - font-size: 24px; - text-align: center; -} -.maneuver_graph { - width: 100%; -} -""" - -def main(output_dir): - view_html = "" % (css_style,) - for i, man in enumerate(maneuvers): - view_html += "" % (man.title,) - for c in ['distance.svg', 'speeds.svg', 'acceleration.svg', 'pedals.svg', 'pid.svg']: - view_html += "" % (os.path.join("maneuver" + str(i+1).zfill(2), c), ) - view_html += "" - - with open(os.path.join(output_dir, "index.html"), "w") as f: - f.write(view_html) - - for i, man in enumerate(maneuvers): - score, plot = man.evaluate() - plot.write_plot(output_dir, "maneuver" + str(i+1).zfill(2)) - -if __name__ == "__main__": - if len(sys.argv) <= 1: - print "Usage:", sys.argv[0], "" - exit(1) - - main(sys.argv[1]) - diff --git a/selfdrive/test/test_openpilot.py b/selfdrive/test/test_openpilot.py index 732df0cfa9ace2..2fcce2acbc0915 100644 --- a/selfdrive/test/test_openpilot.py +++ b/selfdrive/test/test_openpilot.py @@ -42,25 +42,25 @@ def wrap(): return wrap return wrapper -@phone_only -@with_processes(['controlsd', 'radard']) -def test_controls(): - from selfdrive.test.plant.plant import Plant - - # start the fake car for 2 seconds - plant = Plant(100) - for i in range(200): - if plant.rk.frame >= 20 and plant.rk.frame <= 25: - cruise_buttons = CruiseButtons.RES_ACCEL - # rolling forward - assert plant.speed > 0 - else: - cruise_buttons = 0 - plant.step(cruise_buttons = cruise_buttons) - plant.close() - - # assert that we stopped - assert plant.speed == 0.0 +#@phone_only +#@with_processes(['controlsd', 'radard']) +#def test_controls(): +# from selfdrive.test.plant.plant import Plant +# +# # start the fake car for 2 seconds +# plant = Plant(100) +# for i in range(200): +# if plant.rk.frame >= 20 and plant.rk.frame <= 25: +# cruise_buttons = CruiseButtons.RES_ACCEL +# # rolling forward +# assert plant.speed > 0 +# else: +# cruise_buttons = 0 +# plant.step(cruise_buttons = cruise_buttons) +# plant.close() +# +# # assert that we stopped +# assert plant.speed == 0.0 @phone_only @with_processes(['loggerd', 'logmessaged', 'tombstoned', 'proclogd', 'logcatd']) diff --git a/selfdrive/test/tests/plant/__init__.py b/selfdrive/test/tests/plant/__init__.py new file mode 100644 index 00000000000000..e69de29bb2d1d6 diff --git a/selfdrive/test/tests/plant/test_longitudinal.py b/selfdrive/test/tests/plant/test_longitudinal.py new file mode 100755 index 00000000000000..3c8286b775e3b7 --- /dev/null +++ b/selfdrive/test/tests/plant/test_longitudinal.py @@ -0,0 +1,255 @@ +#!/usr/bin/env python +import os +os.environ['OLD_CAN'] = '1' +os.environ['NOCRASH'] = '1' + +import time +import unittest +import shutil + +import matplotlib +matplotlib.use('svg') + +from selfdrive.config import Conversions as CV, CruiseButtons as CB +from selfdrive.test.plant.maneuver import Maneuver +import selfdrive.manager as manager + +def create_dir(path): + try: + os.makedirs(path) + except OSError: + pass + +maneuvers = [ + Maneuver( + 'while cruising at 40 mph, change cruise speed to 50mph', + duration=30., + initial_speed = 40. * CV.MPH_TO_MS, + cruise_button_presses = [(CB.DECEL_SET, 2.), (0, 2.3), + (CB.RES_ACCEL, 10.), (0, 10.1), + (CB.RES_ACCEL, 10.2), (0, 10.3)] + ), + Maneuver( + 'while cruising at 60 mph, change cruise speed to 50mph', + duration=30., + initial_speed=60. * CV.MPH_TO_MS, + cruise_button_presses = [(CB.DECEL_SET, 2.), (0, 2.3), + (CB.DECEL_SET, 10.), (0, 10.1), + (CB.DECEL_SET, 10.2), (0, 10.3)] + ), + Maneuver( + 'while cruising at 20mph, grade change +10%', + duration=25., + initial_speed=20. * CV.MPH_TO_MS, + cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)], + grade_values = [0., 0., 1.0], + grade_breakpoints = [0., 10., 11.] + ), + Maneuver( + 'while cruising at 20mph, grade change -10%', + duration=25., + initial_speed=20. * CV.MPH_TO_MS, + cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)], + grade_values = [0., 0., -1.0], + grade_breakpoints = [0., 10., 11.] + ), + Maneuver( + 'approaching a 40mph car while cruising at 60mph from 100m away', + duration=30., + initial_speed = 60. * CV.MPH_TO_MS, + lead_relevancy=True, + initial_distance_lead=100., + speed_lead_values = [40.*CV.MPH_TO_MS, 40.*CV.MPH_TO_MS], + speed_lead_breakpoints = [0., 100.], + cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)] + ), + Maneuver( + 'approaching a 0mph car while cruising at 40mph from 150m away', + duration=30., + initial_speed = 40. * CV.MPH_TO_MS, + lead_relevancy=True, + initial_distance_lead=150., + speed_lead_values = [0.*CV.MPH_TO_MS, 0.*CV.MPH_TO_MS], + speed_lead_breakpoints = [0., 100.], + cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)] + ), + Maneuver( + 'steady state following a car at 20m/s, then lead decel to 0mph at 1m/s^2', + duration=50., + initial_speed = 20., + lead_relevancy=True, + initial_distance_lead=35., + speed_lead_values = [20.*CV.MPH_TO_MS, 20.*CV.MPH_TO_MS, 0.*CV.MPH_TO_MS], + speed_lead_breakpoints = [0., 15., 35.0], + cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)] + ), + Maneuver( + 'steady state following a car at 20m/s, then lead decel to 0mph at 2m/s^2', + duration=50., + initial_speed = 20., + lead_relevancy=True, + initial_distance_lead=35., + speed_lead_values = [20.*CV.MPH_TO_MS, 20.*CV.MPH_TO_MS, 0.*CV.MPH_TO_MS], + speed_lead_breakpoints = [0., 15., 25.0], + cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3)] + ), + Maneuver( + 'starting at 0mph, approaching a stopped car 100m away', + duration=30., + initial_speed = 0., + lead_relevancy=True, + initial_distance_lead=100., + cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3), + (CB.RES_ACCEL, 1.4), (0.0, 1.5), + (CB.RES_ACCEL, 1.6), (0.0, 1.7), + (CB.RES_ACCEL, 1.8), (0.0, 1.9)] + ), + Maneuver( + "following a car at 60mph, lead accel and decel at 0.5m/s^2 every 2s", + duration=25., + initial_speed=30., + lead_relevancy=True, + initial_distance_lead=49., + speed_lead_values=[30.,30.,29.,31.,29.,31.,29.], + speed_lead_breakpoints=[0., 6., 8., 12.,16.,20.,24.], + cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3), + (CB.RES_ACCEL, 1.4), (0.0, 1.5), + (CB.RES_ACCEL, 1.6), (0.0, 1.7)] + ), + Maneuver( + "following a car at 10mph, stop and go at 1m/s2 lead dece1 and accel", + duration=70., + initial_speed=10., + lead_relevancy=True, + initial_distance_lead=20., + speed_lead_values=[10., 0., 0., 10., 0.,10.], + speed_lead_breakpoints=[10., 20., 30., 40., 50., 60.], + cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3), + (CB.RES_ACCEL, 1.4), (0.0, 1.5), + (CB.RES_ACCEL, 1.6), (0.0, 1.7)] + ), + Maneuver( + "green light: stopped behind lead car, lead car accelerates at 1.5 m/s", + duration=30., + initial_speed=0., + lead_relevancy=True, + initial_distance_lead=4., + speed_lead_values=[0, 0 , 45], + speed_lead_breakpoints=[0, 10., 40.], + cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3), + (CB.RES_ACCEL, 1.4), (0.0, 1.5), + (CB.RES_ACCEL, 1.6), (0.0, 1.7), + (CB.RES_ACCEL, 1.8), (0.0, 1.9), + (CB.RES_ACCEL, 2.0), (0.0, 2.1), + (CB.RES_ACCEL, 2.2), (0.0, 2.3)] + ), + Maneuver( + "stop and go with 1m/s2 lead decel and accel, with full stops", + duration=70., + initial_speed=0., + lead_relevancy=True, + initial_distance_lead=20., + speed_lead_values=[10., 0., 0., 10., 0., 0.] , + speed_lead_breakpoints=[10., 20., 30., 40., 50., 60.], + cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3), + (CB.RES_ACCEL, 1.4), (0.0, 1.5), + (CB.RES_ACCEL, 1.6), (0.0, 1.7)] + ), + Maneuver( + "accelerate from 20 while lead vehicle decelerates from 40 to 20 at 1m/s2", + duration=30., + initial_speed=10., + lead_relevancy=True, + initial_distance_lead=10., + speed_lead_values=[20., 10.], + speed_lead_breakpoints=[1., 11.], + cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3), + (CB.RES_ACCEL, 1.4), (0.0, 1.5), + (CB.RES_ACCEL, 1.6), (0.0, 1.7), + (CB.RES_ACCEL, 1.8), (0.0, 1.9), + (CB.RES_ACCEL, 2.0), (0.0, 2.1), + (CB.RES_ACCEL, 2.2), (0.0, 2.3)] + ), + Maneuver( + "accelerate from 20 while lead vehicle decelerates from 40 to 0 at 2m/s2", + duration=30., + initial_speed=10., + lead_relevancy=True, + initial_distance_lead=10., + speed_lead_values=[20., 0.], + speed_lead_breakpoints=[1., 11.], + cruise_button_presses = [(CB.DECEL_SET, 1.2), (0, 1.3), + (CB.RES_ACCEL, 1.4), (0.0, 1.5), + (CB.RES_ACCEL, 1.6), (0.0, 1.7), + (CB.RES_ACCEL, 1.8), (0.0, 1.9), + (CB.RES_ACCEL, 2.0), (0.0, 2.1), + (CB.RES_ACCEL, 2.2), (0.0, 2.3)] + ) +] + +def setup_output(): + output_dir = os.path.join(os.getcwd(), 'out/longitudinal') + if not os.path.exists(os.path.join(output_dir, "index.html")): + # write test output header + + css_style = """ + .maneuver_title { + font-size: 24px; + text-align: center; + } + .maneuver_graph { + width: 100%; + } + """ + + view_html = "
%s
" % (css_style,) + for i, man in enumerate(maneuvers): + view_html += "" % (man.title,) + for c in ['distance.svg', 'speeds.svg', 'acceleration.svg', 'pedals.svg', 'pid.svg']: + view_html += "" % (os.path.join("maneuver" + str(i+1).zfill(2), c), ) + view_html += "" + + create_dir(output_dir) + with open(os.path.join(output_dir, "index.html"), "w") as f: + f.write(view_html) + +class LongitudinalControl(unittest.TestCase): + @classmethod + def setUpClass(cls): + + setup_output() + + shutil.rmtree('/data/params', ignore_errors=True) + + manager.gctx = {} + manager.prepare_managed_process('radard') + manager.prepare_managed_process('controlsd') + + manager.start_managed_process('radard') + manager.start_managed_process('controlsd') + + @classmethod + def tearDownClass(cls): + manager.kill_managed_process('radard') + manager.kill_managed_process('controlsd') + time.sleep(5) + + # hack + def test_longitudinal_setup(self): + pass + +WORKERS = 8 +def run_maneuver_worker(k): + output_dir = os.path.join(os.getcwd(), 'out/longitudinal') + for i, man in enumerate(maneuvers[k::WORKERS]): + score, plot = man.evaluate() + plot.write_plot(output_dir, "maneuver" + str(WORKERS * i + k+1).zfill(2)) + +for k in xrange(WORKERS): + setattr(LongitudinalControl, + "test_longitudinal_maneuvers_%d" % (k+1), + lambda self, k=k: run_maneuver_worker(k)) + +if __name__ == "__main__": + unittest.main() + diff --git a/selfdrive/tombstoned.py b/selfdrive/tombstoned.py index df7e60069b5593..65d925e0d562d4 100644 --- a/selfdrive/tombstoned.py +++ b/selfdrive/tombstoned.py @@ -61,6 +61,7 @@ def report_tombstone(fn, client): user={'id': os.environ.get('DONGLE_ID')}, message=message, ) + cloudlog.error({"tombstone": message}) def main(gctx): diff --git a/selfdrive/ui/Makefile b/selfdrive/ui/Makefile index d2289228e344d4..d0a3778e21e5b8 100644 --- a/selfdrive/ui/Makefile +++ b/selfdrive/ui/Makefile @@ -55,9 +55,10 @@ ui: $(OBJS) $(OPENGL_LIBS) \ -lcutils -lm -llog -../common/framebuffer.o: ../common/framebuffer.cc +%.o: %.cc @echo "[ CXX ] $@" $(CXX) $(CXXFLAGS) -MMD \ + -Iinclude -I.. -I../.. \ -I$(PHONELIBS)/android_frameworks_native/include \ -I$(PHONELIBS)/android_system_core/include \ -I$(PHONELIBS)/android_hardware_libhardware/include \ diff --git a/selfdrive/ui/ui.c b/selfdrive/ui/ui.c index fc5c16a9421b45..3e1d5ea12fd5db 100644 --- a/selfdrive/ui/ui.c +++ b/selfdrive/ui/ui.c @@ -32,22 +32,28 @@ #include "cereal/gen/c/log.capnp.h" +// Calibration status values from controlsd.py +#define CALIBRATION_UNCALIBRATED 0 +#define CALIBRATION_CALIBRATED 1 +#define CALIBRATION_INVALID 2 + #define UI_BUF_COUNT 4 typedef struct UIScene { int frontview; uint8_t *bgr_ptr; - int big_box_x, big_box_y, big_box_width, big_box_height; int transformed_width, transformed_height; uint64_t model_ts; ModelData model; + float mpc_x[50]; + float mpc_y[50]; + bool world_objects_visible; - // TODO(mgraczyk): Remove and use full frame for everything. - mat3 warp_matrix; // transformed box -> big_box. + mat3 warp_matrix; // transformed box -> frame. mat4 extrinsic_matrix; // Last row is 0 so we can use mat4. float v_cruise; @@ -66,6 +72,10 @@ typedef struct UIScene { char alert_text2[1024]; float awareness_status; + + // Used to display calibration progress + int cal_status; + int cal_perc; } UIScene; typedef struct UIState { @@ -88,14 +98,16 @@ typedef struct UIState { void *livecalibration_sock_raw; zsock_t *live20_sock; void *live20_sock_raw; + zsock_t *livempc_sock; + void *livempc_sock_raw; // vision state bool vision_connected; bool vision_connect_firstrun; int ipc_fd; - VisionBuf bufs[UI_BUF_COUNT]; - VisionBuf front_bufs[UI_BUF_COUNT]; + VIPCBuf bufs[UI_BUF_COUNT]; + VIPCBuf front_bufs[UI_BUF_COUNT]; int cur_vision_idx; int cur_vision_front_idx; @@ -228,6 +240,11 @@ static void ui_init(UIState *s) { assert(s->live20_sock); s->live20_sock_raw = zsock_resolve(s->live20_sock); + s->livempc_sock = zsock_new_sub(">tcp://127.0.0.1:8035", ""); + assert(s->livempc_sock); + s->livempc_sock_raw = zsock_resolve(s->livempc_sock); + + s->ipc_fd = -1; // init display @@ -271,8 +288,7 @@ static void ui_init(UIState *s) { // intrinsic_matrix and returns true. Otherwise returns false. static bool try_load_intrinsics(mat3 *intrinsic_matrix) { char *value; - const int result = - read_db_value("/data/params", "CloudCalibration", &value, NULL); + const int result = read_db_value(NULL, "CloudCalibration", &value, NULL); if (result == 0) { JsonNode* calibration_json = json_decode(value); @@ -309,18 +325,15 @@ static void ui_init_vision(UIState *s, const VisionStreamBufs back_bufs, assert(num_back_fds == UI_BUF_COUNT); assert(num_front_fds == UI_BUF_COUNT); - visionbufs_load(s->bufs, &back_bufs, num_back_fds, back_fds); - visionbufs_load(s->front_bufs, &front_bufs, num_front_fds, front_fds); + vipc_bufs_load(s->bufs, &back_bufs, num_back_fds, back_fds); + vipc_bufs_load(s->front_bufs, &front_bufs, num_front_fds, front_fds); s->cur_vision_idx = -1; s->cur_vision_front_idx = -1; s->scene = (UIScene){ .frontview = 0, - .big_box_x = ui_info.big_box_x, - .big_box_y = ui_info.big_box_y, - .big_box_width = ui_info.big_box_width, - .big_box_height = ui_info.big_box_height, + .cal_status = CALIBRATION_CALIBRATED, .transformed_width = ui_info.transformed_width, .transformed_height = ui_info.transformed_height, .front_box_x = ui_info.front_box_x, @@ -344,7 +357,7 @@ static void ui_init_vision(UIState *s, const VisionStreamBufs back_bufs, }}; char *value; - const int result = read_db_value("/data/params", "IsMetric", &value, NULL); + const int result = read_db_value(NULL, "IsMetric", &value, NULL); if (result == 0) { s->is_metric = value[0] == '1'; free(value); @@ -392,9 +405,8 @@ static void ui_draw_transformed_box(UIState *s, uint32_t color) { }; for (int i=0; ibig_box_x + verts[i].pos.v[0] / verts[i].pos.v[2]; - verts[i].pos.v[1] = s->rgb_height - (scene->big_box_y + - verts[i].pos.v[1] / verts[i].pos.v[2]); + verts[i].pos.v[0] = verts[i].pos.v[0] / verts[i].pos.v[2]; + verts[i].pos.v[1] = s->rgb_height - verts[i].pos.v[1] / verts[i].pos.v[2]; } glUseProgram(s->line_program); @@ -469,6 +481,51 @@ static void draw_cross(UIState *s, float x_in, float y_in, float sz, NVGcolor co nvgRestore(s->vg); } +static void draw_x_y(UIState *s, const float *x_coords, const float *y_coords, size_t num_points, + NVGcolor color) { + const UIScene *scene = &s->scene; + + nvgSave(s->vg); + + // path coords are worked out in rgb-box space + nvgTranslate(s->vg, 240.0f, 0.0); + + // zooom in 2x + nvgTranslate(s->vg, -1440.0f / 2, -1080.0f / 2); + nvgScale(s->vg, 2.0, 2.0); + + nvgScale(s->vg, 1440.0f / s->rgb_width, 1080.0f / s->rgb_height); + + nvgBeginPath(s->vg); + nvgStrokeColor(s->vg, color); + nvgStrokeWidth(s->vg, 2); + bool started = false; + + for (int i=0; ivg, x, y); + started = true; + } else { + nvgLineTo(s->vg, x, y); + } + } + + nvgStroke(s->vg); + + nvgRestore(s->vg); +} + static void draw_path(UIState *s, const float *points, float off, NVGcolor color) { const UIScene *scene = &s->scene; @@ -669,6 +726,8 @@ static void ui_draw_world(UIState *s) { draw_model_path( s, scene->model.right_lane, nvgRGBA(0, (int)(255 * scene->model.right_lane.prob), 0, 128)); + + draw_x_y(s, scene->mpc_x, scene->mpc_y, 50, nvgRGBA(255, 0, 0, 255)); } if (scene->lead_status) { @@ -794,6 +853,25 @@ static void ui_draw_vision(UIState *s) { 255 * scene->awareness_status, 0, 128)); nvgFill(s->vg); } + + // Draw calibration progress (if needed) + if (scene->cal_status == CALIBRATION_UNCALIBRATED) { + int rec_width = 1020; + int x_pos = 470; + nvgBeginPath(s->vg); + nvgStrokeWidth(s->vg, 14); + nvgRoundedRect(s->vg, (1920-rec_width)/2, 970, rec_width, 100, 20); + nvgStroke(s->vg); + nvgFillColor(s->vg, nvgRGBA(10,100,220,180)); + nvgFill(s->vg); + + nvgFontSize(s->vg, labelfontsize); + nvgTextAlign(s->vg, NVG_ALIGN_LEFT | NVG_ALIGN_BASELINE); + nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 220)); + char calib_status_str[32]; + snprintf(calib_status_str, sizeof(calib_status_str), "Calibration In Progress: %d%%", scene->cal_perc); + nvgText(s->vg, x_pos, 1040, calib_status_str, NULL); + } } nvgEndFrame(s->vg); @@ -939,7 +1017,7 @@ static void ui_update(UIState *s) { // poll for events while (true) { - zmq_pollitem_t polls[5] = {{0}}; + zmq_pollitem_t polls[6] = {{0}}; polls[0].socket = s->live100_sock_raw; polls[0].events = ZMQ_POLLIN; polls[1].socket = s->livecalibration_sock_raw; @@ -948,15 +1026,19 @@ static void ui_update(UIState *s) { polls[2].events = ZMQ_POLLIN; polls[3].socket = s->live20_sock_raw; polls[3].events = ZMQ_POLLIN; - - int num_polls = 4; + polls[4].socket = s->livempc_sock_raw; + polls[4].events = ZMQ_POLLIN; + + int num_polls = 5; if (s->vision_connected) { assert(s->ipc_fd >= 0); - polls[4].fd = s->ipc_fd; - polls[4].events = ZMQ_POLLIN; + polls[5].fd = s->ipc_fd; + polls[5].events = ZMQ_POLLIN; num_polls++; } + + int ret = zmq_poll(polls, num_polls, 0); if (ret < 0) { LOGW("poll failed (%d)", ret); @@ -969,7 +1051,7 @@ static void ui_update(UIState *s) { // awake on any activity set_awake(s, true); - if (s->vision_connected && polls[4].revents) { + if (s->vision_connected && polls[5].revents) { // vision ipc event VisionPacket rp; err = vipc_recv(s->ipc_fd, &rp); @@ -1021,7 +1103,7 @@ static void ui_update(UIState *s) { } else { // zmq messages void* which = NULL; - for (int i=0; i<4; i++) { + for (int i=0; i<5; i++) { if (polls[i].revents) { which = polls[i].socket; break; @@ -1084,8 +1166,11 @@ static void ui_update(UIState *s) { struct cereal_LiveCalibrationData datad; cereal_read_LiveCalibrationData(&datad, eventd.liveCalibration); + s->scene.cal_status = datad.calStatus; + s->scene.cal_perc = datad.calPerc; + // should we still even have this? - capn_list32 warpl = datad.warpMatrix; + capn_list32 warpl = datad.warpMatrix2; capn_resolve(&warpl.p); // is this a bug? for (int i = 0; i < 3 * 3; i++) { s->scene.warp_matrix.v[i] = capn_to_f32(capn_get32(warpl, i)); @@ -1100,6 +1185,23 @@ static void ui_update(UIState *s) { } else if (eventd.which == cereal_Event_model) { s->scene.model_ts = eventd.logMonoTime; s->scene.model = read_model(eventd.model); + } else if (eventd.which == cereal_Event_liveMpc) { + struct cereal_LiveMpcData datad; + cereal_read_LiveMpcData(&datad, eventd.liveMpc); + + capn_list32 x_list = datad.x; + capn_resolve(&x_list.p); + + for (int i = 0; i < 50; i++){ + s->scene.mpc_x[i] = capn_to_f32(capn_get32(x_list, i)); + } + + capn_list32 y_list = datad.y; + capn_resolve(&y_list.p); + + for (int i = 0; i < 50; i++){ + s->scene.mpc_y[i] = capn_to_f32(capn_get32(y_list, i)); + } } capn_free(&ctx); diff --git a/selfdrive/visiond/visiond b/selfdrive/visiond/visiond index 23e6505bf2c3e4..10ed8069330fad 100755 Binary files a/selfdrive/visiond/visiond and b/selfdrive/visiond/visiond differ
%s