sunnypilot v2026.002.000
version: sunnypilot v2026.002.000 (staging) date: 2026-07-21T07:44:02 master commit: 2c334ede443d7391d27575af8c854a095ba702a8
This commit is contained in:
github-actions[bot] committed
commit
de77c89cf6
3995 files changed
+1260688
No files matched your search
Whitespace-only changes.
Executable
+264
@@ -0,0 +1,264 @@
|
||||
#!/usr/bin/env python3
|
||||
import math
|
||||
from numbers import Number
|
||||
|
||||
from openpilot.cereal import log
|
||||
from opendbc.car.structs import car
|
||||
import openpilot.cereal.messaging as messaging
|
||||
from openpilot.common.constants import CV
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.realtime import config_realtime_process, DT_CTRL, Priority, Ratekeeper
|
||||
from openpilot.common.swaglog import cloudlog
|
||||
|
||||
from opendbc.car.car_helpers import interfaces
|
||||
from opendbc.car.vehicle_model import VehicleModel
|
||||
from openpilot.selfdrive.controls.lib.drive_helpers import clip_curvature
|
||||
from openpilot.selfdrive.controls.lib.latcontrol import LatControl
|
||||
from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID
|
||||
from openpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle, STEER_ANGLE_SATURATION_THRESHOLD
|
||||
from openpilot.selfdrive.controls.lib.latcontrol_curvature import LatControlCurvature
|
||||
from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque
|
||||
from openpilot.selfdrive.controls.lib.longcontrol import LongControl
|
||||
from openpilot.selfdrive.modeld.modeld import LAT_SMOOTH_SECONDS
|
||||
from openpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose
|
||||
|
||||
from openpilot.sunnypilot.selfdrive.controls.controlsd_ext import ControlsExt
|
||||
|
||||
State = log.SelfdriveState.OpenpilotState
|
||||
LaneChangeState = log.LaneChangeState
|
||||
LaneChangeDirection = log.LaneChangeDirection
|
||||
|
||||
ACTUATOR_FIELDS = tuple(car.CarControl.Actuators.schema.fields.keys())
|
||||
|
||||
|
||||
class Controls(ControlsExt):
|
||||
def __init__(self) -> None:
|
||||
self.params = Params()
|
||||
cloudlog.info("controlsd is waiting for CarParams")
|
||||
self.CP = messaging.log_from_bytes(self.params.get("CarParams", block=True), car.CarParams)
|
||||
cloudlog.info("controlsd got CarParams")
|
||||
|
||||
# Initialize sunnypilot controlsd extension and base model state
|
||||
ControlsExt.__init__(self, self.CP, self.params)
|
||||
|
||||
self.CI = interfaces[self.CP.carFingerprint](self.CP, self.CP_SP)
|
||||
|
||||
self.sm = messaging.SubMaster(['liveDelay', 'liveParameters', 'liveTorqueParameters', 'modelV2', 'selfdriveState',
|
||||
'liveCalibration', 'livePose', 'longitudinalPlan', 'lateralManeuverPlan', 'carState', 'carOutput',
|
||||
'driverMonitoringState', 'onroadEvents', 'driverAssistance', 'liveDelay'] + self.sm_services_ext,
|
||||
poll='selfdriveState')
|
||||
self.pm = messaging.PubMaster(['carControl', 'controlsState'] + self.pm_services_ext)
|
||||
|
||||
self.steer_limited_by_safety = False
|
||||
self.curvature = 0.0
|
||||
self.desired_curvature = 0.0
|
||||
|
||||
self.pose_calibrator = PoseCalibrator()
|
||||
self.calibrated_pose: Pose | None = None
|
||||
|
||||
self.LoC = LongControl(self.CP, self.CP_SP)
|
||||
self.VM = VehicleModel(self.CP)
|
||||
self.LaC: LatControl
|
||||
if self.CP.steerControlType == car.CarParams.SteerControlType.angle:
|
||||
self.LaC = LatControlAngle(self.CP, self.CP_SP, self.CI, DT_CTRL)
|
||||
elif self.CP.steerControlType == car.CarParams.SteerControlType.curvature:
|
||||
self.LaC = LatControlCurvature(self.CP, self.CP_SP, self.CI, DT_CTRL)
|
||||
elif self.CP.lateralTuning.which() == 'pid':
|
||||
self.LaC = LatControlPID(self.CP, self.CP_SP, self.CI, DT_CTRL)
|
||||
elif self.CP.lateralTuning.which() == 'torque':
|
||||
self.LaC = LatControlTorque(self.CP, self.CP_SP, self.CI, DT_CTRL)
|
||||
|
||||
self.LaC = ControlsExt.initialize_lateral_control(self, self.LaC, self.CI, DT_CTRL)
|
||||
|
||||
def update(self):
|
||||
self.sm.update(15)
|
||||
if self.sm.updated["liveCalibration"]:
|
||||
self.pose_calibrator.feed_live_calib(self.sm['liveCalibration'])
|
||||
if self.sm.updated["livePose"]:
|
||||
device_pose = Pose.from_live_pose(self.sm['livePose'])
|
||||
self.calibrated_pose = self.pose_calibrator.build_calibrated_pose(device_pose)
|
||||
|
||||
def state_control(self):
|
||||
CS = self.sm['carState']
|
||||
|
||||
# Update VehicleModel
|
||||
lp = self.sm['liveParameters']
|
||||
x = max(lp.stiffnessFactor, 0.1)
|
||||
sr = max(lp.steerRatio, 0.1)
|
||||
self.VM.update_params(x, sr)
|
||||
|
||||
steer_angle_without_offset = math.radians(CS.steeringAngleDeg - lp.angleOffsetDeg)
|
||||
self.curvature = -self.VM.calc_curvature(steer_angle_without_offset, CS.vEgo, lp.roll)
|
||||
|
||||
# Update Torque Params
|
||||
if self.CP.lateralTuning.which() == 'torque':
|
||||
torque_params = self.sm['liveTorqueParameters']
|
||||
if self.sm.all_checks(['liveTorqueParameters']) and torque_params.useParams:
|
||||
self.LaC.update_live_torque_params(torque_params.latAccelFactorFiltered, torque_params.latAccelOffsetFiltered,
|
||||
torque_params.frictionCoefficientFiltered)
|
||||
|
||||
self.LaC.extension.update_limits()
|
||||
|
||||
self.LaC.extension.update_model_v2(self.sm['modelV2'])
|
||||
|
||||
self.LaC.extension.update_lateral_lag(self.lat_delay)
|
||||
|
||||
long_plan = self.sm['longitudinalPlan']
|
||||
model_v2 = self.sm['modelV2']
|
||||
|
||||
CC = car.CarControl.new_message()
|
||||
CC.enabled = self.sm['selfdriveState'].enabled
|
||||
|
||||
# Check which actuators can be enabled
|
||||
standstill = abs(CS.vEgo) <= max(self.CP.minSteerSpeed, 0.3) or CS.standstill
|
||||
|
||||
# Get which state to use for active lateral control
|
||||
_lat_active = self.get_lat_active(self.sm)
|
||||
|
||||
CC.latActive = _lat_active and not CS.steerFaultTemporary and not CS.steerFaultPermanent and \
|
||||
(not standstill or self.CP.steerAtStandstill)
|
||||
CC.longActive = CC.enabled and not any(e.overrideLongitudinal for e in self.sm['onroadEvents']) and \
|
||||
(self.CP.openpilotLongitudinalControl or not self.CP_SP.pcmCruiseSpeed)
|
||||
|
||||
actuators = CC.actuators
|
||||
actuators.longControlState = self.LoC.long_control_state
|
||||
|
||||
# Enable blinkers while lane changing
|
||||
if model_v2.meta.laneChangeState != LaneChangeState.off:
|
||||
CC.leftBlinker = model_v2.meta.laneChangeDirection == LaneChangeDirection.left
|
||||
CC.rightBlinker = model_v2.meta.laneChangeDirection == LaneChangeDirection.right
|
||||
|
||||
if not CC.latActive:
|
||||
self.LaC.reset()
|
||||
if not CC.longActive:
|
||||
self.LoC.reset()
|
||||
|
||||
# accel PID loop
|
||||
pid_accel_limits = self.CI.get_pid_accel_limits(self.CP, self.CP_SP, CS.vEgo, CS.vCruise * CV.KPH_TO_MS)
|
||||
actuators.accel = float(self.LoC.update(CC.longActive, CS, long_plan.aTarget, long_plan.shouldStop, pid_accel_limits))
|
||||
|
||||
# Steering PID loop and lateral MPC
|
||||
# Reset desired curvature to current to avoid violating the limits on engage
|
||||
if self.sm.valid['lateralManeuverPlan']:
|
||||
new_desired_curvature = self.sm['lateralManeuverPlan'].desiredCurvature if CC.latActive else self.curvature
|
||||
else:
|
||||
new_desired_curvature = model_v2.action.desiredCurvature if CC.latActive else self.curvature
|
||||
self.desired_curvature, curvature_limited = clip_curvature(CS.vEgo, self.desired_curvature, new_desired_curvature, lp.roll)
|
||||
lat_delay = self.sm["liveDelay"].lateralDelay + LAT_SMOOTH_SECONDS
|
||||
|
||||
actuators.curvature = self.desired_curvature
|
||||
steer, lateral_output, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp,
|
||||
self.steer_limited_by_safety, self.desired_curvature,
|
||||
self.calibrated_pose, curvature_limited, lat_delay)
|
||||
actuators.torque = float(steer)
|
||||
if self.CP.steerControlType == car.CarParams.SteerControlType.curvature:
|
||||
actuators.curvature = float(lateral_output)
|
||||
else:
|
||||
actuators.steeringAngleDeg = float(lateral_output)
|
||||
# Ensure no NaNs/Infs
|
||||
for p in ACTUATOR_FIELDS:
|
||||
attr = getattr(actuators, p)
|
||||
if not isinstance(attr, Number):
|
||||
continue
|
||||
|
||||
if not math.isfinite(attr):
|
||||
cloudlog.error(f"actuators.{p} not finite {actuators.to_dict()}")
|
||||
setattr(actuators, p, 0.0)
|
||||
|
||||
return CC, lac_log
|
||||
|
||||
def publish(self, CC, lac_log):
|
||||
CS = self.sm['carState']
|
||||
|
||||
# Orientation and angle rates can be useful for carcontroller
|
||||
# Only calibrated (car) frame is relevant for the carcontroller
|
||||
CC.currentCurvature = self.curvature
|
||||
if self.calibrated_pose is not None:
|
||||
CC.orientationNED = self.calibrated_pose.orientation.xyz.tolist()
|
||||
CC.angularVelocity = self.calibrated_pose.angular_velocity.xyz.tolist()
|
||||
|
||||
CC.cruiseControl.override = CC.enabled and not CC.longActive and (self.CP.openpilotLongitudinalControl or not self.CP_SP.pcmCruiseSpeed)
|
||||
CC.cruiseControl.cancel = CS.cruiseState.enabled and (not CC.enabled or not self.CP.pcmCruise)
|
||||
CC.cruiseControl.resume = CC.enabled and CS.cruiseState.standstill and not self.sm['longitudinalPlan'].shouldStop
|
||||
|
||||
hudControl = CC.hudControl
|
||||
hudControl.setSpeed = float(CS.vCruiseCluster * CV.KPH_TO_MS)
|
||||
hudControl.speedVisible = CC.enabled
|
||||
hudControl.lanesVisible = CC.enabled
|
||||
hudControl.leadVisible = self.sm['longitudinalPlan'].hasLead
|
||||
hudControl.leadDistanceBars = self.sm['selfdriveState'].personality.raw + 1
|
||||
hudControl.visualAlert = self.sm['selfdriveState'].alertHudVisual
|
||||
|
||||
hudControl.rightLaneVisible = True
|
||||
hudControl.leftLaneVisible = True
|
||||
if self.sm.valid['driverAssistance']:
|
||||
hudControl.leftLaneDepart = self.sm['driverAssistance'].leftLaneDeparture
|
||||
hudControl.rightLaneDepart = self.sm['driverAssistance'].rightLaneDeparture
|
||||
|
||||
if self.get_lat_active(self.sm):
|
||||
CO = self.sm['carOutput']
|
||||
if self.CP.steerControlType == car.CarParams.SteerControlType.angle:
|
||||
self.steer_limited_by_safety = abs(CC.actuators.steeringAngleDeg - CO.actuatorsOutput.steeringAngleDeg) > \
|
||||
STEER_ANGLE_SATURATION_THRESHOLD
|
||||
else:
|
||||
self.steer_limited_by_safety = abs(CC.actuators.torque - CO.actuatorsOutput.torque) > 1e-2
|
||||
|
||||
# TODO: both controlsState and carControl valids should be set by
|
||||
# sm.all_checks(), but this creates a circular dependency
|
||||
|
||||
# controlsState
|
||||
dat = messaging.new_message('controlsState')
|
||||
dat.valid = CS.canValid
|
||||
cs = dat.controlsState
|
||||
|
||||
cs.curvature = self.curvature
|
||||
cs.longitudinalPlanMonoTime = self.sm.logMonoTime['longitudinalPlan']
|
||||
cs.lateralPlanMonoTime = self.sm.logMonoTime['modelV2']
|
||||
cs.desiredCurvature = self.desired_curvature
|
||||
cs.longControlState = self.LoC.long_control_state
|
||||
cs.upAccelCmd = float(self.LoC.pid.p)
|
||||
cs.uiAccelCmd = float(self.LoC.pid.i)
|
||||
cs.ufAccelCmd = float(self.LoC.pid.f)
|
||||
cs.forceDecel = bool(self.sm['driverMonitoringState'].noResponseForceDecel or
|
||||
(self.sm['selfdriveState'].state == State.softDisabling))
|
||||
|
||||
# trigger the car's stock driver monitoring escalation
|
||||
CC.driverMonitoringEscalation = cs.forceDecel
|
||||
|
||||
lat_tuning = self.CP.lateralTuning.which()
|
||||
if self.CP.steerControlType == car.CarParams.SteerControlType.angle:
|
||||
cs.lateralControlState.angleState = lac_log
|
||||
elif self.CP.steerControlType == car.CarParams.SteerControlType.curvature:
|
||||
cs.lateralControlState.curvatureState = lac_log
|
||||
elif lat_tuning == 'pid':
|
||||
cs.lateralControlState.pidState = lac_log
|
||||
elif lat_tuning == 'torque':
|
||||
cs.lateralControlState.torqueState = lac_log
|
||||
|
||||
self.pm.send('controlsState', dat)
|
||||
|
||||
# carControl
|
||||
cc_send = messaging.new_message('carControl')
|
||||
cc_send.valid = CS.canValid
|
||||
cc_send.carControl = CC
|
||||
self.pm.send('carControl', cc_send)
|
||||
|
||||
def run(self):
|
||||
rk = Ratekeeper(100, print_delay_threshold=None)
|
||||
while True:
|
||||
self.update()
|
||||
CC, lac_log = self.state_control()
|
||||
self.publish(CC, lac_log)
|
||||
self.get_params_sp(self.sm)
|
||||
self.run_ext(self.sm, self.pm)
|
||||
rk.monitor_time()
|
||||
|
||||
|
||||
def main():
|
||||
config_realtime_process(4, Priority.CTRL_HIGH)
|
||||
controls = Controls()
|
||||
controls.run()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
Whitespace-only changes.
@@ -0,0 +1,104 @@
|
||||
from openpilot.cereal import log, custom
|
||||
from openpilot.common.constants import CV
|
||||
from openpilot.common.realtime import DT_MDL
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.auto_lane_change import AutoLaneChangeController, AutoLaneChangeMode
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.lane_turn_desire import LaneTurnController
|
||||
|
||||
LaneChangeState = log.LaneChangeState
|
||||
LaneChangeDirection = log.LaneChangeDirection
|
||||
TurnDirection = custom.ModelDataV2SP.TurnDirection
|
||||
|
||||
LANE_CHANGE_SPEED_MIN = 20 * CV.MPH_TO_MS
|
||||
LANE_CHANGE_TIME_MAX = 10.
|
||||
LANE_CHANGE_START_TIME = 0.5
|
||||
|
||||
TURN_DESIRES = {
|
||||
TurnDirection.none: log.Desire.none,
|
||||
TurnDirection.turnLeft: log.Desire.turnLeft,
|
||||
TurnDirection.turnRight: log.Desire.turnRight,
|
||||
}
|
||||
|
||||
class DesireHelper:
|
||||
def __init__(self):
|
||||
self.lane_change_state = LaneChangeState.off
|
||||
self.lane_change_direction = LaneChangeDirection.none
|
||||
self.lane_change_timer = 0.0
|
||||
self.prev_one_blinker = False
|
||||
self.desire = log.Desire.none
|
||||
self.alc = AutoLaneChangeController(self)
|
||||
self.lane_turn_controller = LaneTurnController(self)
|
||||
self.lane_turn_direction = TurnDirection.none
|
||||
|
||||
@staticmethod
|
||||
def get_lane_change_direction(CS):
|
||||
return LaneChangeDirection.left if CS.leftBlinker else LaneChangeDirection.right
|
||||
|
||||
def update(self, carstate, lateral_active, lane_change_prob):
|
||||
self.alc.update_params()
|
||||
self.lane_turn_controller.update_params()
|
||||
v_ego = carstate.vEgo
|
||||
one_blinker = carstate.leftBlinker != carstate.rightBlinker
|
||||
below_lane_change_speed = v_ego < LANE_CHANGE_SPEED_MIN
|
||||
|
||||
# Lane turn controller update
|
||||
self.lane_turn_controller.update_lane_turn(blindspot_left=carstate.leftBlindspot, blindspot_right=carstate.rightBlindspot,
|
||||
left_blinker=carstate.leftBlinker, right_blinker=carstate.rightBlinker, v_ego=v_ego)
|
||||
self.lane_turn_direction = self.lane_turn_controller.get_turn_direction()
|
||||
|
||||
if not lateral_active or self.lane_change_timer > LANE_CHANGE_TIME_MAX or self.alc.lane_change_set_timer == AutoLaneChangeMode.OFF:
|
||||
self.lane_change_state = LaneChangeState.off
|
||||
self.lane_change_direction = LaneChangeDirection.none
|
||||
self.lane_change_timer = 0.0
|
||||
else:
|
||||
if self.lane_change_state == LaneChangeState.off and one_blinker and not self.prev_one_blinker and not below_lane_change_speed:
|
||||
self.lane_change_state = LaneChangeState.preLaneChange
|
||||
self.lane_change_timer = 0.0
|
||||
# Initialize lane change direction to prevent UI alert flicker
|
||||
self.lane_change_direction = self.get_lane_change_direction(carstate)
|
||||
|
||||
elif self.lane_change_state == LaneChangeState.preLaneChange:
|
||||
# Update lane change direction
|
||||
self.lane_change_direction = self.get_lane_change_direction(carstate)
|
||||
|
||||
torque_applied = carstate.steeringPressed and \
|
||||
((carstate.steeringTorque > 0 and self.lane_change_direction == LaneChangeDirection.left) or
|
||||
(carstate.steeringTorque < 0 and self.lane_change_direction == LaneChangeDirection.right))
|
||||
|
||||
blindspot_detected = ((carstate.leftBlindspot and self.lane_change_direction == LaneChangeDirection.left) or
|
||||
(carstate.rightBlindspot and self.lane_change_direction == LaneChangeDirection.right))
|
||||
|
||||
self.alc.update_lane_change(blindspot_detected, carstate.brakePressed)
|
||||
|
||||
if not one_blinker or below_lane_change_speed:
|
||||
self.lane_change_state = LaneChangeState.off
|
||||
self.lane_change_direction = LaneChangeDirection.none
|
||||
self.lane_change_timer = 0.0
|
||||
elif (torque_applied or self.alc.auto_lane_change_allowed) and not blindspot_detected:
|
||||
self.lane_change_state = LaneChangeState.laneChangeStarting
|
||||
self.lane_change_timer = 0.0
|
||||
|
||||
elif self.lane_change_state == LaneChangeState.laneChangeStarting:
|
||||
self.lane_change_timer += DT_MDL
|
||||
|
||||
if lane_change_prob < 0.02 and self.lane_change_timer >= LANE_CHANGE_START_TIME:
|
||||
self.lane_change_timer = 0.0
|
||||
if one_blinker:
|
||||
self.lane_change_state = LaneChangeState.preLaneChange
|
||||
self.lane_change_direction = self.get_lane_change_direction(carstate)
|
||||
else:
|
||||
self.lane_change_state = LaneChangeState.off
|
||||
self.lane_change_direction = LaneChangeDirection.none
|
||||
|
||||
self.prev_one_blinker = one_blinker and lateral_active
|
||||
|
||||
if self.lane_turn_direction != TurnDirection.none:
|
||||
self.desire = TURN_DESIRES[self.lane_turn_direction]
|
||||
else:
|
||||
self.desire = log.Desire.none
|
||||
if self.lane_change_state == LaneChangeState.laneChangeStarting:
|
||||
if self.lane_change_direction == LaneChangeDirection.left:
|
||||
self.desire = log.Desire.laneChangeLeft
|
||||
elif self.lane_change_direction == LaneChangeDirection.right:
|
||||
self.desire = log.Desire.laneChangeRight
|
||||
|
||||
self.alc.update_state()
|
||||
@@ -0,0 +1,70 @@
|
||||
import numpy as np
|
||||
from openpilot.common.constants import ACCELERATION_DUE_TO_GRAVITY
|
||||
from openpilot.common.realtime import DT_CTRL, DT_MDL
|
||||
|
||||
MIN_SPEED = 1.0
|
||||
CONTROL_N = 17
|
||||
CAR_ROTATION_RADIUS = 0.0
|
||||
# This is a turn radius smaller than most cars can achieve
|
||||
MAX_CURVATURE = 0.2
|
||||
MAX_VEL_ERR = 5.0 # m/s
|
||||
MIN_STABLE_DELAY = 0.3
|
||||
|
||||
# EU guidelines
|
||||
MAX_LATERAL_JERK = 5.0 # m/s^3
|
||||
MAX_LATERAL_ACCEL_NO_ROLL = 3.0 # m/s^2
|
||||
|
||||
|
||||
def clamp(val, min_val, max_val):
|
||||
clamped_val = float(np.clip(val, min_val, max_val))
|
||||
return clamped_val, clamped_val != val
|
||||
|
||||
def smooth_value(val, prev_val, tau, dt=DT_MDL):
|
||||
alpha = 1 - np.exp(-dt/tau) if tau > 0 else 1
|
||||
return alpha * val + (1 - alpha) * prev_val
|
||||
|
||||
def clip_curvature(v_ego, prev_curvature, new_curvature, roll) -> tuple[float, bool]:
|
||||
# This function respects ISO lateral jerk and acceleration limits + a max curvature
|
||||
v_ego = max(v_ego, MIN_SPEED)
|
||||
max_curvature_rate = MAX_LATERAL_JERK / (v_ego ** 2) # inexact calculation, check https://github.com/commaai/openpilot/pull/24755
|
||||
new_curvature = np.clip(new_curvature,
|
||||
prev_curvature - max_curvature_rate * DT_CTRL,
|
||||
prev_curvature + max_curvature_rate * DT_CTRL)
|
||||
|
||||
roll_compensation = roll * ACCELERATION_DUE_TO_GRAVITY
|
||||
max_lat_accel = MAX_LATERAL_ACCEL_NO_ROLL + roll_compensation
|
||||
min_lat_accel = -MAX_LATERAL_ACCEL_NO_ROLL + roll_compensation
|
||||
new_curvature, limited_accel = clamp(new_curvature, min_lat_accel / v_ego ** 2, max_lat_accel / v_ego ** 2)
|
||||
|
||||
new_curvature, limited_max_curv = clamp(new_curvature, -MAX_CURVATURE, MAX_CURVATURE)
|
||||
return float(new_curvature), limited_accel or limited_max_curv
|
||||
|
||||
|
||||
def get_accel_from_plan(speeds, accels, t_idxs, action_t=DT_MDL, vEgoStopping=0.3):
|
||||
if len(speeds) == len(t_idxs):
|
||||
v_now = speeds[0]
|
||||
a_now = accels[0]
|
||||
if action_t < MIN_STABLE_DELAY:
|
||||
v_target = v_now + (action_t / MIN_STABLE_DELAY) * (np.interp(MIN_STABLE_DELAY, t_idxs, speeds) - v_now)
|
||||
else:
|
||||
v_target = np.interp(action_t, t_idxs, speeds)
|
||||
a_target = 2 * (v_target - v_now) / (action_t) - a_now
|
||||
else:
|
||||
v_now = 0.0
|
||||
v_target = 0.0
|
||||
a_target = 0.0
|
||||
should_stop = (v_now < vEgoStopping and a_target < 0.1)
|
||||
return a_target, should_stop
|
||||
|
||||
def curv_from_psis(psi_target, psi_rate, vego, action_t):
|
||||
vego = np.clip(vego, MIN_SPEED, np.inf)
|
||||
curv_from_psi = psi_target / (vego * action_t)
|
||||
return 2*curv_from_psi - psi_rate / vego
|
||||
|
||||
def get_curvature_from_plan(yaws, yaw_rates, t_idxs, vego, action_t):
|
||||
if action_t < MIN_STABLE_DELAY:
|
||||
psi_target = (action_t / MIN_STABLE_DELAY) * np.interp(MIN_STABLE_DELAY, t_idxs, yaws)
|
||||
else:
|
||||
psi_target = np.interp(action_t, t_idxs, yaws)
|
||||
psi_rate = yaw_rates[0]
|
||||
return curv_from_psis(psi_target, psi_rate, vego, action_t)
|
||||
@@ -0,0 +1,31 @@
|
||||
import numpy as np
|
||||
from abc import abstractmethod, ABC
|
||||
from openpilot.selfdrive.locationd.helpers import Pose
|
||||
|
||||
|
||||
class LatControl(ABC):
|
||||
def __init__(self, CP, CP_SP, CI, dt):
|
||||
self.dt = dt
|
||||
self.sat_limit = CP.steerLimitTimer
|
||||
self.sat_time = 0.
|
||||
self.sat_check_min_speed = 10.
|
||||
|
||||
# we define the steer torque scale as [-1.0...1.0]
|
||||
self.steer_max = 1.0
|
||||
|
||||
@abstractmethod
|
||||
def update(self, active: bool, CS, VM, params, steer_limited_by_safety: bool, desired_curvature: float, calibrated_pose: Pose,
|
||||
curvature_limited: bool, lat_delay: float):
|
||||
pass
|
||||
|
||||
def reset(self):
|
||||
self.sat_time = 0.
|
||||
|
||||
def _check_saturation(self, saturated, CS, steer_limited_by_safety, curvature_limited):
|
||||
# Saturated only if control output is not being limited by car torque/angle rate limits
|
||||
if (saturated or curvature_limited) and CS.vEgo > self.sat_check_min_speed and not steer_limited_by_safety and not CS.steeringPressed:
|
||||
self.sat_time += self.dt
|
||||
else:
|
||||
self.sat_time -= self.dt
|
||||
self.sat_time = np.clip(self.sat_time, 0.0, self.sat_limit)
|
||||
return self.sat_time > (self.sat_limit - 1e-3)
|
||||
@@ -0,0 +1,37 @@
|
||||
import math
|
||||
|
||||
from openpilot.cereal import log
|
||||
from openpilot.selfdrive.controls.lib.latcontrol import LatControl
|
||||
|
||||
# TODO This is speed dependent
|
||||
STEER_ANGLE_SATURATION_THRESHOLD = 2.5 # Degrees
|
||||
|
||||
|
||||
class LatControlAngle(LatControl):
|
||||
def __init__(self, CP, CP_SP, CI, dt):
|
||||
super().__init__(CP, CP_SP, CI, dt)
|
||||
self.sat_check_min_speed = 5.
|
||||
self.use_steer_limited_by_safety = CP.brand in ("tesla", "hyundai")
|
||||
|
||||
def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, calibrated_pose, curvature_limited, lat_delay):
|
||||
angle_log = log.ControlsState.LateralAngleState.new_message()
|
||||
|
||||
if not active:
|
||||
angle_log.active = False
|
||||
angle_steers_des = float(CS.steeringAngleDeg)
|
||||
else:
|
||||
angle_log.active = True
|
||||
angle_steers_des = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo, params.roll))
|
||||
angle_steers_des += params.angleOffsetDeg
|
||||
|
||||
if self.use_steer_limited_by_safety:
|
||||
# these cars' carcontrollers calculate max lateral accel and jerk, so we can rely on carOutput for saturation
|
||||
angle_control_saturated = steer_limited_by_safety
|
||||
else:
|
||||
# for cars which use a method of limiting torque such as a torque signal (Nissan and Toyota)
|
||||
# or relying on EPS (Ford Q3), carOutput does not capture maxing out torque # TODO: this can be improved
|
||||
angle_control_saturated = abs(angle_steers_des - CS.steeringAngleDeg) > STEER_ANGLE_SATURATION_THRESHOLD
|
||||
angle_log.saturated = bool(self._check_saturation(angle_control_saturated, CS, False, curvature_limited))
|
||||
angle_log.steeringAngleDeg = float(CS.steeringAngleDeg)
|
||||
angle_log.steeringAngleDesiredDeg = angle_steers_des
|
||||
return 0, float(angle_steers_des), angle_log
|
||||
@@ -0,0 +1,58 @@
|
||||
import math
|
||||
|
||||
from openpilot.cereal import log
|
||||
from openpilot.common.pid import PIDController
|
||||
from openpilot.selfdrive.controls.lib.latcontrol import LatControl
|
||||
from openpilot.selfdrive.controls.lib.drive_helpers import MAX_CURVATURE
|
||||
|
||||
CURVATURE_SATURATION_THRESHOLD = 1e-3 # 1/m
|
||||
|
||||
|
||||
class LatControlCurvature(LatControl):
|
||||
def __init__(self, CP, CP_SP, CI, dt):
|
||||
super().__init__(CP, CP_SP, CI, dt)
|
||||
self.sat_check_min_speed = 5.
|
||||
if CP.lateralTuning.which() == 'pid':
|
||||
ct = CP.lateralTuning.pid
|
||||
self.pid = PIDController((ct.kpBP, ct.kpV), (ct.kiBP, ct.kiV),
|
||||
pos_limit=MAX_CURVATURE, neg_limit=-MAX_CURVATURE, rate=1 / dt)
|
||||
self.kf = ct.kf
|
||||
else:
|
||||
self.pid = None
|
||||
self.kf = 1.
|
||||
|
||||
def reset(self):
|
||||
super().reset()
|
||||
if self.pid is not None:
|
||||
self.pid.reset()
|
||||
|
||||
def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, calibrated_pose, curvature_limited, lat_delay):
|
||||
curvature_log = log.ControlsState.LateralCurvatureState.new_message()
|
||||
actual_curvature = -VM.calc_curvature(math.radians(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, params.roll)
|
||||
error = desired_curvature - actual_curvature
|
||||
|
||||
if not active:
|
||||
output_curvature = 0.0
|
||||
curvature_log.active = False
|
||||
if self.pid is not None:
|
||||
self.pid.reset()
|
||||
elif self.pid is None or CS.steeringPressed:
|
||||
# no PID or override: feedforward only
|
||||
if self.pid is not None:
|
||||
self.pid.reset()
|
||||
output_curvature = self.kf * desired_curvature
|
||||
curvature_log.active = True
|
||||
else:
|
||||
output_curvature = self.pid.update(error, speed=CS.vEgo, feedforward=self.kf * desired_curvature)
|
||||
curvature_log.p = float(self.pid.p)
|
||||
curvature_log.i = float(self.pid.i)
|
||||
curvature_log.f = float(self.pid.f)
|
||||
curvature_log.active = True
|
||||
|
||||
curvature_log.error = float(error)
|
||||
curvature_log.actualCurvature = float(actual_curvature)
|
||||
curvature_log.desiredCurvature = float(desired_curvature)
|
||||
curvature_log.output = float(output_curvature)
|
||||
curvature_log.saturated = bool(self._check_saturation(abs(error) > CURVATURE_SATURATION_THRESHOLD, CS,
|
||||
False, curvature_limited))
|
||||
return 0.0, float(output_curvature), curvature_log
|
||||
@@ -0,0 +1,49 @@
|
||||
import math
|
||||
|
||||
from openpilot.cereal import log
|
||||
from openpilot.selfdrive.controls.lib.latcontrol import LatControl
|
||||
from openpilot.common.pid import PIDController
|
||||
|
||||
|
||||
class LatControlPID(LatControl):
|
||||
def __init__(self, CP, CP_SP, CI, dt):
|
||||
super().__init__(CP, CP_SP, CI, dt)
|
||||
self.pid = PIDController((CP.lateralTuning.pid.kpBP, CP.lateralTuning.pid.kpV),
|
||||
(CP.lateralTuning.pid.kiBP, CP.lateralTuning.pid.kiV),
|
||||
pos_limit=self.steer_max, neg_limit=-self.steer_max)
|
||||
self.ff_factor = CP.lateralTuning.pid.kf
|
||||
self.get_steer_feedforward = CI.get_steer_feedforward_function()
|
||||
|
||||
def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, calibrated_pose, curvature_limited, lat_delay):
|
||||
pid_log = log.ControlsState.LateralPIDState.new_message()
|
||||
pid_log.steeringAngleDeg = float(CS.steeringAngleDeg)
|
||||
pid_log.steeringRateDeg = float(CS.steeringRateDeg)
|
||||
|
||||
angle_steers_des_no_offset = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo, params.roll))
|
||||
angle_steers_des = angle_steers_des_no_offset + params.angleOffsetDeg
|
||||
error = angle_steers_des - CS.steeringAngleDeg
|
||||
|
||||
pid_log.steeringAngleDesiredDeg = angle_steers_des
|
||||
pid_log.angleError = error
|
||||
if not active:
|
||||
output_torque = 0.0
|
||||
pid_log.active = False
|
||||
|
||||
else:
|
||||
# offset does not contribute to resistive torque
|
||||
ff = self.ff_factor * self.get_steer_feedforward(angle_steers_des_no_offset, CS.vEgo)
|
||||
freeze_integrator = steer_limited_by_safety or CS.steeringPressed or CS.vEgo < 5
|
||||
|
||||
output_torque = self.pid.update(error,
|
||||
feedforward=ff,
|
||||
speed=CS.vEgo,
|
||||
freeze_integrator=freeze_integrator)
|
||||
|
||||
pid_log.active = True
|
||||
pid_log.p = float(self.pid.p)
|
||||
pid_log.i = float(self.pid.i)
|
||||
pid_log.f = float(self.pid.f)
|
||||
pid_log.output = float(output_torque)
|
||||
pid_log.saturated = bool(self._check_saturation(self.steer_max - abs(output_torque) < 1e-3, CS, steer_limited_by_safety, curvature_limited))
|
||||
|
||||
return output_torque, angle_steers_des, pid_log
|
||||
@@ -0,0 +1,122 @@
|
||||
import math
|
||||
import numpy as np
|
||||
from collections import deque
|
||||
|
||||
from openpilot.cereal import log
|
||||
from opendbc.car.lateral import FRICTION_THRESHOLD, get_friction
|
||||
from openpilot.common.constants import ACCELERATION_DUE_TO_GRAVITY
|
||||
from openpilot.common.filter_simple import FirstOrderFilter
|
||||
from openpilot.selfdrive.controls.lib.latcontrol import LatControl
|
||||
from openpilot.common.pid import PIDController
|
||||
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.latcontrol_torque_ext import LatControlTorqueExt
|
||||
|
||||
# At higher speeds (25+mph) we can assume:
|
||||
# Lateral acceleration achieved by a specific car correlates to
|
||||
# torque applied to the steering rack. It does not correlate to
|
||||
# wheel slip, or to speed.
|
||||
|
||||
# This controller applies torque to achieve desired lateral
|
||||
# accelerations. To compensate for the low speed effects the
|
||||
# proportional gain is increased at low speeds by the PID controller.
|
||||
# Additionally, there is friction in the steering wheel that needs
|
||||
# to be overcome to move it at all, this is compensated for too.
|
||||
|
||||
KP = 0.8
|
||||
KI = 0.15
|
||||
|
||||
INTERP_SPEEDS = [1, 1.5, 2.0, 3.0, 5, 7.5, 10, 15, 30]
|
||||
KP_INTERP = [250, 120, 65, 30, 11.5, 5.5, 3.5, 2.0, KP]
|
||||
|
||||
LP_FILTER_CUTOFF_HZ = 1.2
|
||||
JERK_LOOKAHEAD_SECONDS = 0.19
|
||||
JERK_GAIN = 0.3
|
||||
LAT_ACCEL_REQUEST_BUFFER_SECONDS = 1.0
|
||||
VERSION = 1
|
||||
|
||||
class LatControlTorque(LatControl):
|
||||
def __init__(self, CP, CP_SP, CI, dt):
|
||||
super().__init__(CP, CP_SP, CI, dt)
|
||||
self.torque_params = CP.lateralTuning.torque.as_builder()
|
||||
self.torque_from_lateral_accel = CI.torque_from_lateral_accel()
|
||||
self.lateral_accel_from_torque = CI.lateral_accel_from_torque()
|
||||
self.pid = PIDController([INTERP_SPEEDS, KP_INTERP], KI, rate=1/self.dt)
|
||||
self.update_limits()
|
||||
self.steering_angle_deadzone_deg = self.torque_params.steeringAngleDeadzoneDeg
|
||||
self.lat_accel_request_buffer_len = int(LAT_ACCEL_REQUEST_BUFFER_SECONDS / self.dt)
|
||||
self.lat_accel_request_buffer = deque([0.] * self.lat_accel_request_buffer_len , maxlen=self.lat_accel_request_buffer_len)
|
||||
self.lookahead_frames = int(JERK_LOOKAHEAD_SECONDS / self.dt)
|
||||
self.jerk_filter = FirstOrderFilter(0.0, 1 / (2 * np.pi * LP_FILTER_CUTOFF_HZ), self.dt)
|
||||
|
||||
self.extension = LatControlTorqueExt(self, CP, CP_SP, CI)
|
||||
|
||||
def update_live_torque_params(self, latAccelFactor, latAccelOffset, friction):
|
||||
self.torque_params.latAccelFactor = latAccelFactor
|
||||
self.torque_params.latAccelOffset = latAccelOffset
|
||||
self.torque_params.friction = friction
|
||||
self.update_limits()
|
||||
|
||||
def update_limits(self):
|
||||
self.pid.set_limits(self.lateral_accel_from_torque(self.steer_max, self.torque_params),
|
||||
self.lateral_accel_from_torque(-self.steer_max, self.torque_params))
|
||||
|
||||
def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, calibrated_pose, curvature_limited, lat_delay):
|
||||
# Override torque params from extension
|
||||
if self.extension.update_override_torque_params(self.torque_params):
|
||||
self.update_limits()
|
||||
|
||||
pid_log = log.ControlsState.LateralTorqueState.new_message()
|
||||
pid_log.version = VERSION
|
||||
measured_curvature = -VM.calc_curvature(math.radians(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, params.roll)
|
||||
measurement = measured_curvature * CS.vEgo ** 2
|
||||
future_desired_lateral_accel = desired_curvature * CS.vEgo ** 2
|
||||
self.lat_accel_request_buffer.append(future_desired_lateral_accel)
|
||||
|
||||
roll_compensation = params.roll * ACCELERATION_DUE_TO_GRAVITY
|
||||
curvature_deadzone = abs(VM.calc_curvature(math.radians(self.steering_angle_deadzone_deg), CS.vEgo, 0.0))
|
||||
lateral_accel_deadzone = curvature_deadzone * CS.vEgo ** 2
|
||||
|
||||
delay_frames = int(np.clip(lat_delay / self.dt + 1, 1, self.lat_accel_request_buffer_len))
|
||||
expected_lateral_accel = self.lat_accel_request_buffer[-delay_frames]
|
||||
setpoint = expected_lateral_accel
|
||||
error = setpoint - measurement
|
||||
|
||||
lookahead_idx = int(np.clip(-delay_frames + self.lookahead_frames, -self.lat_accel_request_buffer_len+1, -2))
|
||||
raw_lateral_jerk = (self.lat_accel_request_buffer[lookahead_idx+1] - self.lat_accel_request_buffer[lookahead_idx-1]) / (2 * self.dt)
|
||||
desired_lateral_jerk = self.jerk_filter.update(raw_lateral_jerk)
|
||||
gravity_adjusted_future_lateral_accel = future_desired_lateral_accel - roll_compensation
|
||||
ff = gravity_adjusted_future_lateral_accel
|
||||
# latAccelOffset corrects roll compensation bias from device roll misalignment relative to car roll
|
||||
ff -= self.torque_params.latAccelOffset
|
||||
ff += get_friction(error + JERK_GAIN * desired_lateral_jerk, lateral_accel_deadzone, FRICTION_THRESHOLD, self.torque_params)
|
||||
|
||||
if not active:
|
||||
output_torque = 0.0
|
||||
pid_log.active = False
|
||||
else:
|
||||
# do error correction in lateral acceleration space, convert at end to handle non-linear torque responses correctly
|
||||
pid_log.error = float(error)
|
||||
|
||||
freeze_integrator = steer_limited_by_safety or CS.steeringPressed or CS.vEgo < 5
|
||||
output_lataccel = self.pid.update(pid_log.error, speed=CS.vEgo, feedforward=ff, freeze_integrator=freeze_integrator)
|
||||
output_torque = self.torque_from_lateral_accel(output_lataccel, self.torque_params)
|
||||
|
||||
# Lateral acceleration torque controller extension updates
|
||||
# Overrides pid_log.error and output_torque
|
||||
pid_log, output_torque = self.extension.update(CS, VM, self.pid, params, ff, pid_log, setpoint, measurement, calibrated_pose, roll_compensation,
|
||||
future_desired_lateral_accel, measurement, lateral_accel_deadzone, gravity_adjusted_future_lateral_accel,
|
||||
desired_curvature, measured_curvature, steer_limited_by_safety, output_torque)
|
||||
|
||||
pid_log.active = True
|
||||
pid_log.p = float(self.pid.p)
|
||||
pid_log.i = float(self.pid.i)
|
||||
pid_log.d = float(self.pid.d)
|
||||
pid_log.f = float(self.pid.f)
|
||||
pid_log.output = float(-output_torque) # TODO: log lat accel?
|
||||
pid_log.actualLateralAccel = float(measurement)
|
||||
pid_log.desiredLateralAccel = float(setpoint)
|
||||
pid_log.desiredLateralJerk = float(desired_lateral_jerk)
|
||||
pid_log.saturated = bool(self._check_saturation(self.steer_max - abs(output_torque) < 1e-3, CS, steer_limited_by_safety, curvature_limited))
|
||||
|
||||
# TODO left is positive in this convention
|
||||
return -output_torque, 0.0, pid_log
|
||||
@@ -0,0 +1,41 @@
|
||||
from openpilot.cereal import log
|
||||
from openpilot.common.realtime import DT_CTRL
|
||||
from openpilot.common.constants import CV
|
||||
|
||||
|
||||
CAMERA_OFFSET = 0.04
|
||||
LDW_MIN_SPEED = 31 * CV.MPH_TO_MS
|
||||
LANE_DEPARTURE_THRESHOLD = 0.1
|
||||
|
||||
class LaneDepartureWarning:
|
||||
def __init__(self):
|
||||
self.left = False
|
||||
self.right = False
|
||||
self.last_blinker_frame = 0
|
||||
|
||||
def update(self, frame, modelV2, CS, CC):
|
||||
if CS.leftBlinker or CS.rightBlinker:
|
||||
self.last_blinker_frame = frame
|
||||
|
||||
recent_blinker = (frame - self.last_blinker_frame) * DT_CTRL < 5.0 # 5s blinker cooldown
|
||||
ldw_allowed = CS.vEgo > LDW_MIN_SPEED and not recent_blinker and not CC.latActive
|
||||
|
||||
desire_prediction = modelV2.meta.desirePrediction
|
||||
if len(desire_prediction) and ldw_allowed:
|
||||
right_lane_visible = modelV2.laneLineProbs[2] > 0.5
|
||||
left_lane_visible = modelV2.laneLineProbs[1] > 0.5
|
||||
l_lane_change_prob = desire_prediction[log.Desire.laneChangeLeft]
|
||||
r_lane_change_prob = desire_prediction[log.Desire.laneChangeRight]
|
||||
|
||||
lane_lines = modelV2.laneLines
|
||||
l_lane_close = left_lane_visible and (lane_lines[1].y[0] > -(1.08 + CAMERA_OFFSET))
|
||||
r_lane_close = right_lane_visible and (lane_lines[2].y[0] < (1.08 - CAMERA_OFFSET))
|
||||
|
||||
self.left = bool(l_lane_change_prob > LANE_DEPARTURE_THRESHOLD and l_lane_close)
|
||||
self.right = bool(r_lane_change_prob > LANE_DEPARTURE_THRESHOLD and r_lane_close)
|
||||
else:
|
||||
self.left, self.right = False, False
|
||||
|
||||
@property
|
||||
def warning(self) -> bool:
|
||||
return bool(self.left or self.right)
|
||||
@@ -0,0 +1,89 @@
|
||||
import numpy as np
|
||||
from opendbc.car.structs import car
|
||||
from openpilot.common.realtime import DT_CTRL
|
||||
from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N
|
||||
from openpilot.common.pid import PIDController
|
||||
from openpilot.selfdrive.modeld.constants import ModelConstants
|
||||
|
||||
CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N]
|
||||
|
||||
LongCtrlState = car.CarControl.Actuators.LongControlState
|
||||
|
||||
|
||||
def long_control_state_trans(CP, CP_SP, active, long_control_state, v_ego,
|
||||
should_stop, brake_pressed, cruise_standstill):
|
||||
# Gas Interceptor
|
||||
cruise_standstill = cruise_standstill and not CP_SP.enableGasInterceptor
|
||||
|
||||
starting_condition = (not should_stop and
|
||||
not cruise_standstill and
|
||||
not brake_pressed)
|
||||
|
||||
if not active:
|
||||
long_control_state = LongCtrlState.off
|
||||
|
||||
else:
|
||||
if long_control_state == LongCtrlState.off:
|
||||
if not starting_condition:
|
||||
long_control_state = LongCtrlState.stopping
|
||||
elif CP.startingState:
|
||||
long_control_state = LongCtrlState.starting
|
||||
else:
|
||||
long_control_state = LongCtrlState.pid
|
||||
|
||||
elif long_control_state == LongCtrlState.stopping:
|
||||
if starting_condition and CP.startingState:
|
||||
long_control_state = LongCtrlState.starting
|
||||
elif starting_condition:
|
||||
long_control_state = LongCtrlState.pid
|
||||
|
||||
elif long_control_state in [LongCtrlState.starting, LongCtrlState.pid]:
|
||||
if should_stop:
|
||||
long_control_state = LongCtrlState.stopping
|
||||
elif v_ego > CP.vEgoStarting:
|
||||
long_control_state = LongCtrlState.pid
|
||||
return long_control_state
|
||||
|
||||
class LongControl:
|
||||
def __init__(self, CP, CP_SP):
|
||||
self.CP = CP
|
||||
self.CP_SP = CP_SP
|
||||
self.long_control_state = LongCtrlState.off
|
||||
self.pid = PIDController((CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV),
|
||||
(CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV),
|
||||
rate=1 / DT_CTRL)
|
||||
self.last_output_accel = 0.0
|
||||
|
||||
def reset(self):
|
||||
self.pid.reset()
|
||||
|
||||
def update(self, active, CS, a_target, should_stop, accel_limits):
|
||||
"""Update longitudinal control. This updates the state machine and runs a PID loop"""
|
||||
self.pid.neg_limit = accel_limits[0]
|
||||
self.pid.pos_limit = accel_limits[1]
|
||||
|
||||
self.long_control_state = long_control_state_trans(self.CP, self.CP_SP, active, self.long_control_state, CS.vEgo,
|
||||
should_stop, CS.brakePressed,
|
||||
CS.cruiseState.standstill)
|
||||
if self.long_control_state == LongCtrlState.off:
|
||||
self.reset()
|
||||
output_accel = 0.
|
||||
|
||||
elif self.long_control_state == LongCtrlState.stopping:
|
||||
output_accel = self.last_output_accel
|
||||
if output_accel > self.CP.stopAccel:
|
||||
output_accel = min(output_accel, 0.0)
|
||||
output_accel -= self.CP.stoppingDecelRate * DT_CTRL
|
||||
self.reset()
|
||||
|
||||
elif self.long_control_state == LongCtrlState.starting:
|
||||
output_accel = self.CP.startAccel
|
||||
self.reset()
|
||||
|
||||
else: # LongCtrlState.pid
|
||||
error = a_target - CS.aEgo
|
||||
output_accel = self.pid.update(error, speed=CS.vEgo,
|
||||
feedforward=a_target)
|
||||
|
||||
self.last_output_accel = np.clip(output_accel, accel_limits[0], accel_limits[1])
|
||||
return self.last_output_accel
|
||||
@@ -0,0 +1,2 @@
|
||||
acados_ocp_long.json
|
||||
c_generated_code/
|
||||
Whitespace-only changes.
@@ -0,0 +1,524 @@
|
||||
{
|
||||
"acados_include_path": "/usr/local/venv/lib/python3.12/site-packages/acados/install/include",
|
||||
"acados_lib_path": "/usr/local/venv/lib/python3.12/site-packages/acados/install/lib",
|
||||
"code_export_directory": "/data/openpilot/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code",
|
||||
"constraints": {
|
||||
"C": [],
|
||||
"C_e": [],
|
||||
"D": [],
|
||||
"constr_type": "BGH",
|
||||
"constr_type_e": "BGH",
|
||||
"idxbu": [],
|
||||
"idxbx": [],
|
||||
"idxbx_0": [
|
||||
0,
|
||||
1,
|
||||
2
|
||||
],
|
||||
"idxbx_e": [],
|
||||
"idxbxe_0": [
|
||||
0,
|
||||
1,
|
||||
2
|
||||
],
|
||||
"idxsbu": [],
|
||||
"idxsbx": [],
|
||||
"idxsbx_e": [],
|
||||
"idxsg": [],
|
||||
"idxsg_e": [],
|
||||
"idxsh": [
|
||||
0,
|
||||
1,
|
||||
2,
|
||||
3
|
||||
],
|
||||
"idxsh_e": [],
|
||||
"idxsphi": [],
|
||||
"idxsphi_e": [],
|
||||
"lbu": [],
|
||||
"lbx": [],
|
||||
"lbx_0": [
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
"lbx_e": [],
|
||||
"lg": [],
|
||||
"lg_e": [],
|
||||
"lh": [
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
"lh_e": [],
|
||||
"lphi": [],
|
||||
"lphi_e": [],
|
||||
"lsbu": [],
|
||||
"lsbx": [],
|
||||
"lsbx_e": [],
|
||||
"lsg": [],
|
||||
"lsg_e": [],
|
||||
"lsh": [
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
"lsh_e": [],
|
||||
"lsphi": [],
|
||||
"lsphi_e": [],
|
||||
"ubu": [],
|
||||
"ubx": [],
|
||||
"ubx_0": [
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
"ubx_e": [],
|
||||
"ug": [],
|
||||
"ug_e": [],
|
||||
"uh": [
|
||||
10000.0,
|
||||
10000.0,
|
||||
10000.0,
|
||||
10000.0
|
||||
],
|
||||
"uh_e": [],
|
||||
"uphi": [],
|
||||
"uphi_e": [],
|
||||
"usbu": [],
|
||||
"usbx": [],
|
||||
"usbx_e": [],
|
||||
"usg": [],
|
||||
"usg_e": [],
|
||||
"ush": [
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
"ush_e": [],
|
||||
"usphi": [],
|
||||
"usphi_e": []
|
||||
},
|
||||
"cost": {
|
||||
"Vu": [],
|
||||
"Vu_0": [],
|
||||
"Vx": [],
|
||||
"Vx_0": [],
|
||||
"Vx_e": [],
|
||||
"Vz": [],
|
||||
"Vz_0": [],
|
||||
"W": [
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
]
|
||||
],
|
||||
"W_0": [
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
]
|
||||
],
|
||||
"W_e": [
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
]
|
||||
],
|
||||
"Zl": [
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
"Zl_e": [],
|
||||
"Zu": [
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
"Zu_e": [],
|
||||
"cost_ext_fun_type": "casadi",
|
||||
"cost_ext_fun_type_0": "casadi",
|
||||
"cost_ext_fun_type_e": "casadi",
|
||||
"cost_type": "NONLINEAR_LS",
|
||||
"cost_type_0": "NONLINEAR_LS",
|
||||
"cost_type_e": "NONLINEAR_LS",
|
||||
"yref": [
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
"yref_0": [
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
"yref_e": [
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
"zl": [
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
"zl_e": [],
|
||||
"zu": [
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
"zu_e": []
|
||||
},
|
||||
"cython_include_dirs": [
|
||||
"/usr/local/venv/lib/python3.12/site-packages/numpy/_core/include",
|
||||
"/usr/include/python3.12"
|
||||
],
|
||||
"dims": {
|
||||
"N": 12,
|
||||
"nbu": 0,
|
||||
"nbx": 0,
|
||||
"nbx_0": 3,
|
||||
"nbx_e": 0,
|
||||
"nbxe_0": 3,
|
||||
"ng": 0,
|
||||
"ng_e": 0,
|
||||
"nh": 4,
|
||||
"nh_e": 0,
|
||||
"np": 6,
|
||||
"nphi": 0,
|
||||
"nphi_e": 0,
|
||||
"nr": 0,
|
||||
"nr_e": 0,
|
||||
"ns": 4,
|
||||
"ns_e": 0,
|
||||
"nsbu": 0,
|
||||
"nsbx": 0,
|
||||
"nsbx_e": 0,
|
||||
"nsg": 0,
|
||||
"nsg_e": 0,
|
||||
"nsh": 4,
|
||||
"nsh_e": 0,
|
||||
"nsphi": 0,
|
||||
"nsphi_e": 0,
|
||||
"nu": 1,
|
||||
"nx": 3,
|
||||
"ny": 6,
|
||||
"ny_0": 6,
|
||||
"ny_e": 5,
|
||||
"nz": 0
|
||||
},
|
||||
"json_file": "/data/openpilot/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/acados_ocp_long.json",
|
||||
"model": {
|
||||
"con_h_expr": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegiaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaaghpffghgpgegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaabgpffghgpgegpcaaaaaaaaaaaaaafaaaaaaabgpfngjgogegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaabgpfngbgihchcaaaaaaaaaaaaaaaegeaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaakaaaaaaaihpfpgcgdhehbgdgmgfgegpcaaaaaaaaaaaaaafaaaaaaaihpffghgpgegdaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaacbaaaaaamgfgbgegpfegbgoghgfgchpfggbgdgehpgchegbaaaaaaaaaaaaaaaegbaaaaaaaaaaaaaaaegeaaaaaaaaaaaaaaaeglaaaaaaaaaaaaaaachbaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaajgfaaaaaaaegdaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaanaaaaaaamgfgbgegpfehpfggpgmgmgpghhchbaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaajggaaaaaaaegbaaaaaaaaaaaaaaachbaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaajgkaaaaaaa",
|
||||
"con_h_expr_e": null,
|
||||
"con_phi_expr": null,
|
||||
"con_phi_expr_e": null,
|
||||
"con_r_expr": null,
|
||||
"con_r_expr_e": null,
|
||||
"con_r_in_phi": null,
|
||||
"con_r_in_phi_e": null,
|
||||
"cost_conl_custom_outer_hess": null,
|
||||
"cost_conl_custom_outer_hess_0": null,
|
||||
"cost_conl_custom_outer_hess_e": null,
|
||||
"cost_expr_ext_cost": null,
|
||||
"cost_expr_ext_cost_0": null,
|
||||
"cost_expr_ext_cost_custom_hess": null,
|
||||
"cost_expr_ext_cost_custom_hess_0": null,
|
||||
"cost_expr_ext_cost_custom_hess_e": null,
|
||||
"cost_expr_ext_cost_e": null,
|
||||
"cost_psi_expr": null,
|
||||
"cost_psi_expr_0": null,
|
||||
"cost_psi_expr_e": null,
|
||||
"cost_r_in_psi_expr": null,
|
||||
"cost_r_in_psi_expr_0": null,
|
||||
"cost_r_in_psi_expr_e": null,
|
||||
"cost_y_expr": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegkaaaaaaaaaaaaaaagaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaagaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaafaaaaaaaaaaaaaaagaaaaaaaaaaaaaaaegeaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaakaaaaaaaihpfpgcgdhehbgdgmgfgegpcaaaaaaaaaaaaaafaaaaaaaihpffghgpgegbaaaaaaaaaaaaaaaegbaaaaaaaaaaaaaaaegeaaaaaaaaaaaaaaaeglaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaaghpffghgpgegmcaaaaaaaaaaaaaajgfaaaaaaaegdaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaanaaaaaaamgfgbgegpfehpfggpgmgmgpghhcheaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaajggaaaaaaaegbaaaaaaaaaaaaaaacheaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaajgkaaaaaaachcaaaaaaaaaaaaaaacheaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaabgpffghgpgegcaaaaaaaaaaaaaaachbbaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaagaaaaaaabgpfahchfgghegpcaaaaaaaaaaaaaafaaaaaaakgpffghgpg",
|
||||
"cost_y_expr_0": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegkaaaaaaaaaaaaaaagaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaagaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaafaaaaaaaaaaaaaaagaaaaaaaaaaaaaaaegeaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaakaaaaaaaihpfpgcgdhehbgdgmgfgegpcaaaaaaaaaaaaaafaaaaaaaihpffghgpgegbaaaaaaaaaaaaaaaegbaaaaaaaaaaaaaaaegeaaaaaaaaaaaaaaaeglaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaaghpffghgpgegmcaaaaaaaaaaaaaajgfaaaaaaaegdaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaanaaaaaaamgfgbgegpfehpfggpgmgmgpghhcheaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaajggaaaaaaaegbaaaaaaaaaaaaaaacheaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaajgkaaaaaaachcaaaaaaaaaaaaaaacheaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaabgpffghgpgegcaaaaaaaaaaaaaaachbbaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaagaaaaaaabgpfahchfgghegpcaaaaaaaaaaaaaafaaaaaaakgpffghgpg",
|
||||
"cost_y_expr_e": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegjaaaaaaaaaaaaaaafaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaafaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaafaaaaaaaaaaaaaaaegeaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaakaaaaaaaihpfpgcgdhehbgdgmgfgegpcaaaaaaaaaaaaaafaaaaaaaihpffghgpgegbaaaaaaaaaaaaaaaegbaaaaaaaaaaaaaaaegeaaaaaaaaaaaaaaaeglaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaaghpffghgpgegmcaaaaaaaaaaaaaajgfaaaaaaaegdaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaanaaaaaaamgfgbgegpfehpfggpgmgmgpghhcheaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaajggaaaaaaaegbaaaaaaaaaaaaaaacheaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaajgkaaaaaaachcaaaaaaaaaaaaaaacheaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaabgpffghgpgegcaaaaaaaaaaaaaaachbbaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaagaaaaaaabgpfahchfggh",
|
||||
"disc_dyn_expr": null,
|
||||
"dyn_disc_fun": null,
|
||||
"dyn_disc_fun_jac": null,
|
||||
"dyn_disc_fun_jac_hess": null,
|
||||
"dyn_ext_fun_type": "casadi",
|
||||
"dyn_generic_source": null,
|
||||
"f_expl_expr": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaeghaaaaaaaaaaaaaaadaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaaghpffghgpgegpcaaaaaaaaaaaaaafaaaaaaabgpffghgpgegpcaaaaaaaaaaaaaafaaaaaaakgpffghgpg",
|
||||
"f_impl_expr": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaeghaaaaaaaaaaaaaaadaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaajaaaaaaaihpffghgpgpfegpgehegpcaaaaaaaaaaaaaafaaaaaaaghpffghgpgegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaajaaaaaaaghpffghgpgpfegpgehegpcaaaaaaaaaaaaaafaaaaaaabgpffghgpgegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaajaaaaaaabgpffghgpgpfegpgehegpcaaaaaaaaaaaaaafaaaaaaakgpffghgpg",
|
||||
"gnsf": {
|
||||
"nontrivial_f_LO": 1,
|
||||
"purely_linear": 0
|
||||
},
|
||||
"name": "long",
|
||||
"p": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegkaaaaaaaaaaaaaaagaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaagaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaafaaaaaaaaaaaaaaagaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaabgpfngjgogegpcaaaaaaaaaaaaaafaaaaaaabgpfngbgihegpcaaaaaaaaaaaaaakaaaaaaaihpfpgcgdhehbgdgmgfgegpcaaaaaaaaaaaaaagaaaaaaabgpfahchfgghegpcaaaaaaaaaaaaaanaaaaaaamgfgbgegpfehpfggpgmgmgpghhegpcaaaaaaaaaaaaaacbaaaaaamgfgbgegpfegbgoghgfgchpfggbgdgehpgch",
|
||||
"u": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegfaaaaaaaaaaaaaaabaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaakgpffghgpg",
|
||||
"x": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaeghaaaaaaaaaaaaaaadaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaaihpffghgpgegpcaaaaaaaaaaaaaafaaaaaaaghpffghgpgegpcaaaaaaaaaaaaaafaaaaaaabgpffghgpg",
|
||||
"xdot": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaeghaaaaaaaaaaaaaaadaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaajaaaaaaaihpffghgpgpfegpgehegpcaaaaaaaaaaaaaajaaaaaaaghpffghgpgpfegpgehegpcaaaaaaaaaaaaaajaaaaaaabgpffghgpgpfegpgeh",
|
||||
"z": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegdaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaa"
|
||||
},
|
||||
"parameter_values": [
|
||||
-1.2,
|
||||
1.2,
|
||||
0.0,
|
||||
0.0,
|
||||
1.45,
|
||||
0.75
|
||||
],
|
||||
"problem_class": "OCP",
|
||||
"shared_lib_ext": ".so",
|
||||
"solver_options": {
|
||||
"Tsim": 0.06944444444444445,
|
||||
"alpha_min": 0.05,
|
||||
"alpha_reduction": 0.7,
|
||||
"collocation_type": "GAUSS_LEGENDRE",
|
||||
"custom_templates": [],
|
||||
"custom_update_copy": true,
|
||||
"custom_update_filename": "",
|
||||
"custom_update_header_filename": "",
|
||||
"eps_sufficient_descent": 0.0001,
|
||||
"exact_hess_constr": 1,
|
||||
"exact_hess_cost": 1,
|
||||
"exact_hess_dyn": 1,
|
||||
"ext_cost_num_hess": 0,
|
||||
"ext_fun_compile_flags": "-O2",
|
||||
"full_step_dual": 0,
|
||||
"globalization": "FIXED_STEP",
|
||||
"globalization_use_SOC": 0,
|
||||
"hessian_approx": "GAUSS_NEWTON",
|
||||
"hpipm_mode": "BALANCE",
|
||||
"initialize_t_slacks": 0,
|
||||
"integrator_type": "ERK",
|
||||
"levenberg_marquardt": 0.0,
|
||||
"line_search_use_sufficient_descent": 0,
|
||||
"model_external_shared_lib_dir": null,
|
||||
"model_external_shared_lib_name": null,
|
||||
"nlp_solver_ext_qp_res": 0,
|
||||
"nlp_solver_max_iter": 100,
|
||||
"nlp_solver_step_length": 1.0,
|
||||
"nlp_solver_tol_comp": 1e-06,
|
||||
"nlp_solver_tol_eq": 1e-06,
|
||||
"nlp_solver_tol_ineq": 1e-06,
|
||||
"nlp_solver_tol_stat": 1e-06,
|
||||
"nlp_solver_type": "SQP_RTI",
|
||||
"print_level": 0,
|
||||
"qp_solver": "PARTIAL_CONDENSING_HPIPM",
|
||||
"qp_solver_cond_N": 1,
|
||||
"qp_solver_cond_ric_alg": 1,
|
||||
"qp_solver_iter_max": 10,
|
||||
"qp_solver_ric_alg": 1,
|
||||
"qp_solver_tol_comp": 0.001,
|
||||
"qp_solver_tol_eq": 0.001,
|
||||
"qp_solver_tol_ineq": 0.001,
|
||||
"qp_solver_tol_stat": 0.001,
|
||||
"qp_solver_warm_start": 0,
|
||||
"regularize_method": null,
|
||||
"shooting_nodes": [
|
||||
0.0,
|
||||
0.06944444444444445,
|
||||
0.2777777777777778,
|
||||
0.625,
|
||||
1.1111111111111112,
|
||||
1.7361111111111114,
|
||||
2.5,
|
||||
3.4027777777777786,
|
||||
4.444444444444445,
|
||||
5.625,
|
||||
6.9444444444444455,
|
||||
8.402777777777777,
|
||||
10.0
|
||||
],
|
||||
"sim_method_jac_reuse": [
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
0
|
||||
],
|
||||
"sim_method_newton_iter": 3,
|
||||
"sim_method_newton_tol": 0.0,
|
||||
"sim_method_num_stages": [
|
||||
4,
|
||||
4,
|
||||
4,
|
||||
4,
|
||||
4,
|
||||
4,
|
||||
4,
|
||||
4,
|
||||
4,
|
||||
4,
|
||||
4,
|
||||
4
|
||||
],
|
||||
"sim_method_num_steps": [
|
||||
1,
|
||||
1,
|
||||
1,
|
||||
1,
|
||||
1,
|
||||
1,
|
||||
1,
|
||||
1,
|
||||
1,
|
||||
1,
|
||||
1,
|
||||
1
|
||||
],
|
||||
"tf": 10.0,
|
||||
"time_steps": [
|
||||
0.06944444444444445,
|
||||
0.20833333333333334,
|
||||
0.3472222222222222,
|
||||
0.48611111111111116,
|
||||
0.6250000000000002,
|
||||
0.7638888888888886,
|
||||
0.9027777777777786,
|
||||
1.041666666666666,
|
||||
1.1805555555555554,
|
||||
1.3194444444444455,
|
||||
1.4583333333333313,
|
||||
1.5972222222222232
|
||||
]
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,213 @@
|
||||
#
|
||||
# Copyright (c) The acados authors.
|
||||
#
|
||||
# This file is part of acados.
|
||||
#
|
||||
# The 2-Clause BSD License
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions are met:
|
||||
#
|
||||
# 1. Redistributions of source code must retain the above copyright notice,
|
||||
# this list of conditions and the following disclaimer.
|
||||
#
|
||||
# 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
# this list of conditions and the following disclaimer in the documentation
|
||||
# and/or other materials provided with the distribution.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.;
|
||||
#
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
# define sources and use make's implicit rules to generate object files (*.o)
|
||||
|
||||
# model
|
||||
MODEL_SRC=
|
||||
MODEL_SRC+= long_model/long_expl_ode_fun.c
|
||||
MODEL_SRC+= long_model/long_expl_vde_forw.c
|
||||
MODEL_SRC+= long_model/long_expl_vde_adj.c
|
||||
MODEL_OBJ := $(MODEL_SRC:.c=.o)
|
||||
|
||||
# optimal control problem - mostly CasADi exports
|
||||
OCP_SRC=
|
||||
OCP_SRC+= long_constraints/long_constr_h_fun_jac_uxt_zt.c
|
||||
OCP_SRC+= long_constraints/long_constr_h_fun.c
|
||||
OCP_SRC+= long_cost/long_cost_y_0_fun.c
|
||||
OCP_SRC+= long_cost/long_cost_y_0_fun_jac_ut_xt.c
|
||||
OCP_SRC+= long_cost/long_cost_y_0_hess.c
|
||||
OCP_SRC+= long_cost/long_cost_y_fun.c
|
||||
OCP_SRC+= long_cost/long_cost_y_fun_jac_ut_xt.c
|
||||
OCP_SRC+= long_cost/long_cost_y_hess.c
|
||||
OCP_SRC+= long_cost/long_cost_y_e_fun.c
|
||||
OCP_SRC+= long_cost/long_cost_y_e_fun_jac_ut_xt.c
|
||||
OCP_SRC+= long_cost/long_cost_y_e_hess.c
|
||||
|
||||
OCP_SRC+= acados_solver_long.c
|
||||
OCP_OBJ := $(OCP_SRC:.c=.o)
|
||||
|
||||
# for sim solver
|
||||
SIM_SRC= acados_sim_solver_long.c
|
||||
SIM_OBJ := $(SIM_SRC:.c=.o)
|
||||
|
||||
# for target example
|
||||
EX_SRC= main_long.c
|
||||
EX_OBJ := $(EX_SRC:.c=.o)
|
||||
EX_EXE := $(EX_SRC:.c=)
|
||||
|
||||
# for target example_sim
|
||||
EX_SIM_SRC= main_sim_long.c
|
||||
EX_SIM_OBJ := $(EX_SIM_SRC:.c=.o)
|
||||
EX_SIM_EXE := $(EX_SIM_SRC:.c=)
|
||||
|
||||
# combine model, sim and ocp object files
|
||||
OBJ=
|
||||
OBJ+= $(MODEL_OBJ)
|
||||
OBJ+= $(SIM_OBJ)
|
||||
OBJ+= $(OCP_OBJ)
|
||||
|
||||
EXTERNAL_DIR=
|
||||
EXTERNAL_LIB=
|
||||
|
||||
INCLUDE_PATH = /usr/local/venv/lib/python3.12/site-packages/acados/install/include
|
||||
LIB_PATH = /usr/local/venv/lib/python3.12/site-packages/acados/install/lib
|
||||
|
||||
# preprocessor flags for make's implicit rules
|
||||
CPPFLAGS+= -I$(INCLUDE_PATH)
|
||||
CPPFLAGS+= -I$(INCLUDE_PATH)/acados
|
||||
CPPFLAGS+= -I$(INCLUDE_PATH)/blasfeo/include
|
||||
CPPFLAGS+= -I$(INCLUDE_PATH)/hpipm/include
|
||||
|
||||
|
||||
# define the c-compiler flags for make's implicit rules
|
||||
CFLAGS = -fPIC -std=c99 -O2#-fno-diagnostics-show-line-numbers -g
|
||||
# # Debugging
|
||||
# CFLAGS += -g3
|
||||
|
||||
# linker flags
|
||||
LDFLAGS+= -L$(LIB_PATH)
|
||||
|
||||
# link to libraries
|
||||
LDLIBS+= -lacados
|
||||
LDLIBS+= -lhpipm
|
||||
LDLIBS+= -lblasfeo
|
||||
LDLIBS+= -lm
|
||||
LDLIBS+=
|
||||
|
||||
# libraries
|
||||
LIBACADOS_SOLVER=libacados_solver_long.so
|
||||
LIBACADOS_OCP_SOLVER=libacados_ocp_solver_long.so
|
||||
LIBACADOS_SIM_SOLVER=lib$(SIM_SRC:.c=.so)
|
||||
|
||||
# virtual targets
|
||||
.PHONY : all clean
|
||||
|
||||
#all: clean example_sim example shared_lib
|
||||
|
||||
all: clean example_sim example
|
||||
shared_lib: bundled_shared_lib ocp_shared_lib sim_shared_lib
|
||||
|
||||
# some linker targets
|
||||
example: $(EX_OBJ) $(OBJ)
|
||||
$(CC) $^ -o $(EX_EXE) $(LDFLAGS) $(LDLIBS)
|
||||
|
||||
example_sim: $(EX_SIM_OBJ) $(MODEL_OBJ) $(SIM_OBJ)
|
||||
$(CC) $^ -o $(EX_SIM_EXE) $(LDFLAGS) $(LDLIBS)
|
||||
|
||||
bundled_shared_lib: $(OBJ)
|
||||
$(CC) -shared $^ -o $(LIBACADOS_SOLVER) $(LDFLAGS) $(LDLIBS)
|
||||
|
||||
ocp_shared_lib: $(OCP_OBJ) $(MODEL_OBJ)
|
||||
$(CC) -shared $^ -o $(LIBACADOS_OCP_SOLVER) $(LDFLAGS) $(LDLIBS) \
|
||||
-L$(EXTERNAL_DIR) -l$(EXTERNAL_LIB)
|
||||
|
||||
sim_shared_lib: $(SIM_OBJ) $(MODEL_OBJ)
|
||||
$(CC) -shared $^ -o $(LIBACADOS_SIM_SOLVER) $(LDFLAGS) $(LDLIBS)
|
||||
|
||||
|
||||
# Cython targets
|
||||
ocp_cython_c: ocp_shared_lib
|
||||
cython \
|
||||
-o acados_ocp_solver_pyx.c \
|
||||
-I $(INCLUDE_PATH)/../interfaces/acados_template/acados_template \
|
||||
$(INCLUDE_PATH)/../interfaces/acados_template/acados_template/acados_ocp_solver_pyx.pyx \
|
||||
-I /data/openpilot/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code \
|
||||
|
||||
ocp_cython_o: ocp_cython_c
|
||||
$(CC) $(ACADOS_FLAGS) -c -O2 \
|
||||
-fPIC \
|
||||
-o acados_ocp_solver_pyx.o \
|
||||
-I $(INCLUDE_PATH)/blasfeo/include/ \
|
||||
-I $(INCLUDE_PATH)/hpipm/include/ \
|
||||
-I $(INCLUDE_PATH) \
|
||||
-I /usr/local/venv/lib/python3.12/site-packages/numpy/_core/include \
|
||||
-I /usr/include/python3.12 \
|
||||
acados_ocp_solver_pyx.c \
|
||||
|
||||
ocp_cython: ocp_cython_o
|
||||
$(CC) $(ACADOS_FLAGS) -shared \
|
||||
-o acados_ocp_solver_pyx.so \
|
||||
-Wl,-rpath=$(LIB_PATH) \
|
||||
acados_ocp_solver_pyx.o \
|
||||
$(abspath .)/libacados_ocp_solver_long.so \
|
||||
$(LDFLAGS) $(LDLIBS)
|
||||
|
||||
# Sim Cython targets
|
||||
sim_cython_c: sim_shared_lib
|
||||
cython \
|
||||
-o acados_sim_solver_pyx.c \
|
||||
-I $(INCLUDE_PATH)/../interfaces/acados_template/acados_template \
|
||||
$(INCLUDE_PATH)/../interfaces/acados_template/acados_template/acados_sim_solver_pyx.pyx \
|
||||
-I /data/openpilot/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code \
|
||||
|
||||
sim_cython_o: sim_cython_c
|
||||
$(CC) $(ACADOS_FLAGS) -c -O2 \
|
||||
-fPIC \
|
||||
-o acados_sim_solver_pyx.o \
|
||||
-I $(INCLUDE_PATH)/blasfeo/include/ \
|
||||
-I $(INCLUDE_PATH)/hpipm/include/ \
|
||||
-I $(INCLUDE_PATH) \
|
||||
-I /usr/local/venv/lib/python3.12/site-packages/numpy/_core/include \
|
||||
-I /usr/include/python3.12 \
|
||||
acados_sim_solver_pyx.c \
|
||||
|
||||
sim_cython: sim_cython_o
|
||||
$(CC) $(ACADOS_FLAGS) -shared \
|
||||
-o acados_sim_solver_pyx.so \
|
||||
-Wl,-rpath=$(LIB_PATH) \
|
||||
acados_sim_solver_pyx.o \
|
||||
$(abspath .)/libacados_sim_solver_long.so \
|
||||
$(LDFLAGS) $(LDLIBS)
|
||||
|
||||
clean:
|
||||
$(RM) $(OBJ) $(EX_OBJ) $(EX_SIM_OBJ)
|
||||
$(RM) $(LIBACADOS_SOLVER) $(LIBACADOS_OCP_SOLVER) $(LIBACADOS_SIM_SOLVER)
|
||||
$(RM) $(EX_EXE) $(EX_SIM_EXE)
|
||||
|
||||
clean_ocp_shared_lib:
|
||||
$(RM) $(LIBACADOS_OCP_SOLVER)
|
||||
$(RM) $(OCP_OBJ)
|
||||
|
||||
clean_ocp_cython:
|
||||
$(RM) libacados_ocp_solver_long.so
|
||||
$(RM) acados_solver_long.o
|
||||
$(RM) acados_ocp_solver_pyx.so
|
||||
$(RM) acados_ocp_solver_pyx.o
|
||||
|
||||
clean_sim_cython:
|
||||
$(RM) libacados_sim_solver_long.so
|
||||
$(RM) acados_sim_solver_long.o
|
||||
$(RM) acados_sim_solver_pyx.so
|
||||
$(RM) acados_sim_solver_pyx.o
|
||||
+41883
File diff suppressed because it is too large.
Load diff
Executable
BIN
Binary file not shown.
+294
@@ -0,0 +1,294 @@
|
||||
/*
|
||||
* Copyright (c) The acados authors.
|
||||
*
|
||||
* This file is part of acados.
|
||||
*
|
||||
* The 2-Clause BSD License
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice,
|
||||
* this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.;
|
||||
*/
|
||||
// standard
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
|
||||
// acados
|
||||
#include "acados_c/external_function_interface.h"
|
||||
#include "acados_c/sim_interface.h"
|
||||
#include "acados_c/external_function_interface.h"
|
||||
|
||||
#include "acados/sim/sim_common.h"
|
||||
#include "acados/utils/external_function_generic.h"
|
||||
#include "acados/utils/print.h"
|
||||
|
||||
|
||||
// example specific
|
||||
#include "long_model/long_model.h"
|
||||
#include "acados_sim_solver_long.h"
|
||||
|
||||
|
||||
// ** solver data **
|
||||
|
||||
sim_solver_capsule * long_acados_sim_solver_create_capsule()
|
||||
{
|
||||
void* capsule_mem = malloc(sizeof(sim_solver_capsule));
|
||||
sim_solver_capsule *capsule = (sim_solver_capsule *) capsule_mem;
|
||||
|
||||
return capsule;
|
||||
}
|
||||
|
||||
|
||||
int long_acados_sim_solver_free_capsule(sim_solver_capsule * capsule)
|
||||
{
|
||||
free(capsule);
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
int long_acados_sim_create(sim_solver_capsule * capsule)
|
||||
{
|
||||
// initialize
|
||||
const int nx = LONG_NX;
|
||||
const int nu = LONG_NU;
|
||||
const int nz = LONG_NZ;
|
||||
const int np = LONG_NP;
|
||||
bool tmp_bool;
|
||||
|
||||
|
||||
double Tsim = 0.06944444444444445;
|
||||
|
||||
|
||||
// explicit ode
|
||||
capsule->sim_forw_vde_casadi = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
|
||||
capsule->sim_vde_adj_casadi = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
|
||||
capsule->sim_expl_ode_fun_casadi = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
|
||||
|
||||
capsule->sim_forw_vde_casadi->casadi_fun = &long_expl_vde_forw;
|
||||
capsule->sim_forw_vde_casadi->casadi_n_in = &long_expl_vde_forw_n_in;
|
||||
capsule->sim_forw_vde_casadi->casadi_n_out = &long_expl_vde_forw_n_out;
|
||||
capsule->sim_forw_vde_casadi->casadi_sparsity_in = &long_expl_vde_forw_sparsity_in;
|
||||
capsule->sim_forw_vde_casadi->casadi_sparsity_out = &long_expl_vde_forw_sparsity_out;
|
||||
capsule->sim_forw_vde_casadi->casadi_work = &long_expl_vde_forw_work;
|
||||
external_function_param_casadi_create(capsule->sim_forw_vde_casadi, np);
|
||||
|
||||
capsule->sim_vde_adj_casadi->casadi_fun = &long_expl_vde_adj;
|
||||
capsule->sim_vde_adj_casadi->casadi_n_in = &long_expl_vde_adj_n_in;
|
||||
capsule->sim_vde_adj_casadi->casadi_n_out = &long_expl_vde_adj_n_out;
|
||||
capsule->sim_vde_adj_casadi->casadi_sparsity_in = &long_expl_vde_adj_sparsity_in;
|
||||
capsule->sim_vde_adj_casadi->casadi_sparsity_out = &long_expl_vde_adj_sparsity_out;
|
||||
capsule->sim_vde_adj_casadi->casadi_work = &long_expl_vde_adj_work;
|
||||
external_function_param_casadi_create(capsule->sim_vde_adj_casadi, np);
|
||||
|
||||
capsule->sim_expl_ode_fun_casadi->casadi_fun = &long_expl_ode_fun;
|
||||
capsule->sim_expl_ode_fun_casadi->casadi_n_in = &long_expl_ode_fun_n_in;
|
||||
capsule->sim_expl_ode_fun_casadi->casadi_n_out = &long_expl_ode_fun_n_out;
|
||||
capsule->sim_expl_ode_fun_casadi->casadi_sparsity_in = &long_expl_ode_fun_sparsity_in;
|
||||
capsule->sim_expl_ode_fun_casadi->casadi_sparsity_out = &long_expl_ode_fun_sparsity_out;
|
||||
capsule->sim_expl_ode_fun_casadi->casadi_work = &long_expl_ode_fun_work;
|
||||
external_function_param_casadi_create(capsule->sim_expl_ode_fun_casadi, np);
|
||||
|
||||
|
||||
|
||||
// sim plan & config
|
||||
sim_solver_plan_t plan;
|
||||
plan.sim_solver = ERK;
|
||||
|
||||
// create correct config based on plan
|
||||
sim_config * long_sim_config = sim_config_create(plan);
|
||||
capsule->acados_sim_config = long_sim_config;
|
||||
|
||||
// sim dims
|
||||
void *long_sim_dims = sim_dims_create(long_sim_config);
|
||||
capsule->acados_sim_dims = long_sim_dims;
|
||||
sim_dims_set(long_sim_config, long_sim_dims, "nx", &nx);
|
||||
sim_dims_set(long_sim_config, long_sim_dims, "nu", &nu);
|
||||
sim_dims_set(long_sim_config, long_sim_dims, "nz", &nz);
|
||||
|
||||
|
||||
// sim opts
|
||||
sim_opts *long_sim_opts = sim_opts_create(long_sim_config, long_sim_dims);
|
||||
capsule->acados_sim_opts = long_sim_opts;
|
||||
int tmp_int = 3;
|
||||
sim_opts_set(long_sim_config, long_sim_opts, "newton_iter", &tmp_int);
|
||||
double tmp_double = 0.0;
|
||||
sim_opts_set(long_sim_config, long_sim_opts, "newton_tol", &tmp_double);
|
||||
sim_collocation_type collocation_type = GAUSS_LEGENDRE;
|
||||
sim_opts_set(long_sim_config, long_sim_opts, "collocation_type", &collocation_type);
|
||||
|
||||
|
||||
tmp_int = 4;
|
||||
sim_opts_set(long_sim_config, long_sim_opts, "num_stages", &tmp_int);
|
||||
tmp_int = 1;
|
||||
sim_opts_set(long_sim_config, long_sim_opts, "num_steps", &tmp_int);
|
||||
tmp_bool = 0;
|
||||
sim_opts_set(long_sim_config, long_sim_opts, "jac_reuse", &tmp_bool);
|
||||
|
||||
|
||||
// sim in / out
|
||||
sim_in *long_sim_in = sim_in_create(long_sim_config, long_sim_dims);
|
||||
capsule->acados_sim_in = long_sim_in;
|
||||
sim_out *long_sim_out = sim_out_create(long_sim_config, long_sim_dims);
|
||||
capsule->acados_sim_out = long_sim_out;
|
||||
|
||||
sim_in_set(long_sim_config, long_sim_dims,
|
||||
long_sim_in, "T", &Tsim);
|
||||
|
||||
// model functions
|
||||
long_sim_config->model_set(long_sim_in->model,
|
||||
"expl_vde_forw", capsule->sim_forw_vde_casadi);
|
||||
long_sim_config->model_set(long_sim_in->model,
|
||||
"expl_vde_adj", capsule->sim_vde_adj_casadi);
|
||||
long_sim_config->model_set(long_sim_in->model,
|
||||
"expl_ode_fun", capsule->sim_expl_ode_fun_casadi);
|
||||
|
||||
// sim solver
|
||||
sim_solver *long_sim_solver = sim_solver_create(long_sim_config,
|
||||
long_sim_dims, long_sim_opts);
|
||||
capsule->acados_sim_solver = long_sim_solver;
|
||||
|
||||
|
||||
/* initialize parameter values */
|
||||
double* p = calloc(np, sizeof(double));
|
||||
|
||||
p[0] = -1.2;
|
||||
p[1] = 1.2;
|
||||
p[4] = 1.45;
|
||||
p[5] = 0.75;
|
||||
|
||||
long_acados_sim_update_params(capsule, p, np);
|
||||
free(p);
|
||||
|
||||
|
||||
/* initialize input */
|
||||
// x
|
||||
double x0[3];
|
||||
for (int ii = 0; ii < 3; ii++)
|
||||
x0[ii] = 0.0;
|
||||
|
||||
sim_in_set(long_sim_config, long_sim_dims,
|
||||
long_sim_in, "x", x0);
|
||||
|
||||
|
||||
// u
|
||||
double u0[1];
|
||||
for (int ii = 0; ii < 1; ii++)
|
||||
u0[ii] = 0.0;
|
||||
|
||||
sim_in_set(long_sim_config, long_sim_dims,
|
||||
long_sim_in, "u", u0);
|
||||
|
||||
// S_forw
|
||||
double S_forw[12];
|
||||
for (int ii = 0; ii < 12; ii++)
|
||||
S_forw[ii] = 0.0;
|
||||
for (int ii = 0; ii < 3; ii++)
|
||||
S_forw[ii + ii * 3 ] = 1.0;
|
||||
|
||||
|
||||
sim_in_set(long_sim_config, long_sim_dims,
|
||||
long_sim_in, "S_forw", S_forw);
|
||||
|
||||
int status = sim_precompute(long_sim_solver, long_sim_in, long_sim_out);
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
|
||||
int long_acados_sim_solve(sim_solver_capsule *capsule)
|
||||
{
|
||||
// integrate dynamics using acados sim_solver
|
||||
int status = sim_solve(capsule->acados_sim_solver,
|
||||
capsule->acados_sim_in, capsule->acados_sim_out);
|
||||
if (status != 0)
|
||||
printf("error in long_acados_sim_solve()! Exiting.\n");
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
|
||||
int long_acados_sim_free(sim_solver_capsule *capsule)
|
||||
{
|
||||
// free memory
|
||||
sim_solver_destroy(capsule->acados_sim_solver);
|
||||
sim_in_destroy(capsule->acados_sim_in);
|
||||
sim_out_destroy(capsule->acados_sim_out);
|
||||
sim_opts_destroy(capsule->acados_sim_opts);
|
||||
sim_dims_destroy(capsule->acados_sim_dims);
|
||||
sim_config_destroy(capsule->acados_sim_config);
|
||||
|
||||
// free external function
|
||||
external_function_param_casadi_free(capsule->sim_forw_vde_casadi);
|
||||
external_function_param_casadi_free(capsule->sim_vde_adj_casadi);
|
||||
external_function_param_casadi_free(capsule->sim_expl_ode_fun_casadi);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
int long_acados_sim_update_params(sim_solver_capsule *capsule, double *p, int np)
|
||||
{
|
||||
int status = 0;
|
||||
int casadi_np = LONG_NP;
|
||||
|
||||
if (casadi_np != np) {
|
||||
printf("long_acados_sim_update_params: trying to set %i parameters for external functions."
|
||||
" External function has %i parameters. Exiting.\n", np, casadi_np);
|
||||
exit(1);
|
||||
}
|
||||
capsule->sim_forw_vde_casadi[0].set_param(capsule->sim_forw_vde_casadi, p);
|
||||
capsule->sim_vde_adj_casadi[0].set_param(capsule->sim_vde_adj_casadi, p);
|
||||
capsule->sim_expl_ode_fun_casadi[0].set_param(capsule->sim_expl_ode_fun_casadi, p);
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
/* getters pointers to C objects*/
|
||||
sim_config * long_acados_get_sim_config(sim_solver_capsule *capsule)
|
||||
{
|
||||
return capsule->acados_sim_config;
|
||||
};
|
||||
|
||||
sim_in * long_acados_get_sim_in(sim_solver_capsule *capsule)
|
||||
{
|
||||
return capsule->acados_sim_in;
|
||||
};
|
||||
|
||||
sim_out * long_acados_get_sim_out(sim_solver_capsule *capsule)
|
||||
{
|
||||
return capsule->acados_sim_out;
|
||||
};
|
||||
|
||||
void * long_acados_get_sim_dims(sim_solver_capsule *capsule)
|
||||
{
|
||||
return capsule->acados_sim_dims;
|
||||
};
|
||||
|
||||
sim_opts * long_acados_get_sim_opts(sim_solver_capsule *capsule)
|
||||
{
|
||||
return capsule->acados_sim_opts;
|
||||
};
|
||||
|
||||
sim_solver * long_acados_get_sim_solver(sim_solver_capsule *capsule)
|
||||
{
|
||||
return capsule->acados_sim_solver;
|
||||
};
|
||||
|
||||
+101
@@ -0,0 +1,101 @@
|
||||
/*
|
||||
* Copyright (c) The acados authors.
|
||||
*
|
||||
* This file is part of acados.
|
||||
*
|
||||
* The 2-Clause BSD License
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice,
|
||||
* this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.;
|
||||
*/
|
||||
|
||||
#ifndef ACADOS_SIM_long_H_
|
||||
#define ACADOS_SIM_long_H_
|
||||
|
||||
#include "acados_c/sim_interface.h"
|
||||
#include "acados_c/external_function_interface.h"
|
||||
|
||||
#define LONG_NX 3
|
||||
#define LONG_NZ 0
|
||||
#define LONG_NU 1
|
||||
#define LONG_NP 6
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
|
||||
// ** capsule for solver data **
|
||||
typedef struct sim_solver_capsule
|
||||
{
|
||||
// acados objects
|
||||
sim_in *acados_sim_in;
|
||||
sim_out *acados_sim_out;
|
||||
sim_solver *acados_sim_solver;
|
||||
sim_opts *acados_sim_opts;
|
||||
sim_config *acados_sim_config;
|
||||
void *acados_sim_dims;
|
||||
|
||||
/* external functions */
|
||||
// ERK
|
||||
external_function_param_casadi * sim_forw_vde_casadi;
|
||||
external_function_param_casadi * sim_vde_adj_casadi;
|
||||
external_function_param_casadi * sim_expl_ode_fun_casadi;
|
||||
external_function_param_casadi * sim_expl_ode_hess;
|
||||
|
||||
// IRK
|
||||
external_function_param_casadi * sim_impl_dae_fun;
|
||||
external_function_param_casadi * sim_impl_dae_fun_jac_x_xdot_z;
|
||||
external_function_param_casadi * sim_impl_dae_jac_x_xdot_u_z;
|
||||
external_function_param_casadi * sim_impl_dae_hess;
|
||||
|
||||
// GNSF
|
||||
external_function_param_casadi * sim_gnsf_phi_fun;
|
||||
external_function_param_casadi * sim_gnsf_phi_fun_jac_y;
|
||||
external_function_param_casadi * sim_gnsf_phi_jac_y_uhat;
|
||||
external_function_param_casadi * sim_gnsf_f_lo_jac_x1_x1dot_u_z;
|
||||
external_function_param_casadi * sim_gnsf_get_matrices_fun;
|
||||
|
||||
} sim_solver_capsule;
|
||||
|
||||
|
||||
ACADOS_SYMBOL_EXPORT int long_acados_sim_create(sim_solver_capsule *capsule);
|
||||
ACADOS_SYMBOL_EXPORT int long_acados_sim_solve(sim_solver_capsule *capsule);
|
||||
ACADOS_SYMBOL_EXPORT int long_acados_sim_free(sim_solver_capsule *capsule);
|
||||
ACADOS_SYMBOL_EXPORT int long_acados_sim_update_params(sim_solver_capsule *capsule, double *value, int np);
|
||||
|
||||
ACADOS_SYMBOL_EXPORT sim_config * long_acados_get_sim_config(sim_solver_capsule *capsule);
|
||||
ACADOS_SYMBOL_EXPORT sim_in * long_acados_get_sim_in(sim_solver_capsule *capsule);
|
||||
ACADOS_SYMBOL_EXPORT sim_out * long_acados_get_sim_out(sim_solver_capsule *capsule);
|
||||
ACADOS_SYMBOL_EXPORT void * long_acados_get_sim_dims(sim_solver_capsule *capsule);
|
||||
ACADOS_SYMBOL_EXPORT sim_opts * long_acados_get_sim_opts(sim_solver_capsule *capsule);
|
||||
ACADOS_SYMBOL_EXPORT sim_solver * long_acados_get_sim_solver(sim_solver_capsule *capsule);
|
||||
|
||||
|
||||
ACADOS_SYMBOL_EXPORT sim_solver_capsule * long_acados_sim_solver_create_capsule(void);
|
||||
ACADOS_SYMBOL_EXPORT int long_acados_sim_solver_free_capsule(sim_solver_capsule *capsule);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif // ACADOS_SIM_long_H_
|
||||
+62
@@ -0,0 +1,62 @@
|
||||
#
|
||||
# Copyright (c) The acados authors.
|
||||
#
|
||||
# This file is part of acados.
|
||||
#
|
||||
# The 2-Clause BSD License
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions are met:
|
||||
#
|
||||
# 1. Redistributions of source code must retain the above copyright notice,
|
||||
# this list of conditions and the following disclaimer.
|
||||
#
|
||||
# 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
# this list of conditions and the following disclaimer in the documentation
|
||||
# and/or other materials provided with the distribution.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.;
|
||||
#
|
||||
|
||||
cimport acados_solver_common
|
||||
|
||||
cdef extern from "acados_solver_long.h":
|
||||
ctypedef struct nlp_solver_capsule "long_solver_capsule":
|
||||
pass
|
||||
|
||||
nlp_solver_capsule * acados_create_capsule "long_acados_create_capsule"()
|
||||
int acados_free_capsule "long_acados_free_capsule"(nlp_solver_capsule *capsule)
|
||||
|
||||
int acados_create "long_acados_create"(nlp_solver_capsule * capsule)
|
||||
|
||||
int acados_create_with_discretization "long_acados_create_with_discretization"(nlp_solver_capsule * capsule, int n_time_steps, double* new_time_steps)
|
||||
int acados_update_time_steps "long_acados_update_time_steps"(nlp_solver_capsule * capsule, int N, double* new_time_steps)
|
||||
int acados_update_qp_solver_cond_N "long_acados_update_qp_solver_cond_N"(nlp_solver_capsule * capsule, int qp_solver_cond_N)
|
||||
|
||||
int acados_update_params "long_acados_update_params"(nlp_solver_capsule * capsule, int stage, double *value, int np_)
|
||||
int acados_update_params_sparse "long_acados_update_params_sparse"(nlp_solver_capsule * capsule, int stage, int *idx, double *p, int n_update)
|
||||
int acados_solve "long_acados_solve"(nlp_solver_capsule * capsule)
|
||||
int acados_reset "long_acados_reset"(nlp_solver_capsule * capsule, int reset_qp_solver_mem)
|
||||
int acados_free "long_acados_free"(nlp_solver_capsule * capsule)
|
||||
void acados_print_stats "long_acados_print_stats"(nlp_solver_capsule * capsule)
|
||||
|
||||
int acados_custom_update "long_acados_custom_update"(nlp_solver_capsule* capsule, double * data, int data_len)
|
||||
|
||||
acados_solver_common.ocp_nlp_in *acados_get_nlp_in "long_acados_get_nlp_in"(nlp_solver_capsule * capsule)
|
||||
acados_solver_common.ocp_nlp_out *acados_get_nlp_out "long_acados_get_nlp_out"(nlp_solver_capsule * capsule)
|
||||
acados_solver_common.ocp_nlp_out *acados_get_sens_out "long_acados_get_sens_out"(nlp_solver_capsule * capsule)
|
||||
acados_solver_common.ocp_nlp_solver *acados_get_nlp_solver "long_acados_get_nlp_solver"(nlp_solver_capsule * capsule)
|
||||
acados_solver_common.ocp_nlp_config *acados_get_nlp_config "long_acados_get_nlp_config"(nlp_solver_capsule * capsule)
|
||||
void *acados_get_nlp_opts "long_acados_get_nlp_opts"(nlp_solver_capsule * capsule)
|
||||
acados_solver_common.ocp_nlp_dims *acados_get_nlp_dims "long_acados_get_nlp_dims"(nlp_solver_capsule * capsule)
|
||||
acados_solver_common.ocp_nlp_plan *acados_get_nlp_plan "long_acados_get_nlp_plan"(nlp_solver_capsule * capsule)
|
||||
+1087
File diff suppressed because it is too large.
Load diff
+172
@@ -0,0 +1,172 @@
|
||||
/*
|
||||
* Copyright (c) The acados authors.
|
||||
*
|
||||
* This file is part of acados.
|
||||
*
|
||||
* The 2-Clause BSD License
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice,
|
||||
* this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.;
|
||||
*/
|
||||
|
||||
#ifndef ACADOS_SOLVER_long_H_
|
||||
#define ACADOS_SOLVER_long_H_
|
||||
|
||||
#include "acados/utils/types.h"
|
||||
|
||||
#include "acados_c/ocp_nlp_interface.h"
|
||||
#include "acados_c/external_function_interface.h"
|
||||
|
||||
#define LONG_NX 3
|
||||
#define LONG_NZ 0
|
||||
#define LONG_NU 1
|
||||
#define LONG_NP 6
|
||||
#define LONG_NBX 0
|
||||
#define LONG_NBX0 3
|
||||
#define LONG_NBU 0
|
||||
#define LONG_NSBX 0
|
||||
#define LONG_NSBU 0
|
||||
#define LONG_NSH 4
|
||||
#define LONG_NSG 0
|
||||
#define LONG_NSPHI 0
|
||||
#define LONG_NSHN 0
|
||||
#define LONG_NSGN 0
|
||||
#define LONG_NSPHIN 0
|
||||
#define LONG_NSBXN 0
|
||||
#define LONG_NS 4
|
||||
#define LONG_NSN 0
|
||||
#define LONG_NG 0
|
||||
#define LONG_NBXN 0
|
||||
#define LONG_NGN 0
|
||||
#define LONG_NY0 6
|
||||
#define LONG_NY 6
|
||||
#define LONG_NYN 5
|
||||
#define LONG_N 12
|
||||
#define LONG_NH 4
|
||||
#define LONG_NPHI 0
|
||||
#define LONG_NHN 0
|
||||
#define LONG_NPHIN 0
|
||||
#define LONG_NR 0
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
|
||||
// ** capsule for solver data **
|
||||
typedef struct long_solver_capsule
|
||||
{
|
||||
// acados objects
|
||||
ocp_nlp_in *nlp_in;
|
||||
ocp_nlp_out *nlp_out;
|
||||
ocp_nlp_out *sens_out;
|
||||
ocp_nlp_solver *nlp_solver;
|
||||
void *nlp_opts;
|
||||
ocp_nlp_plan_t *nlp_solver_plan;
|
||||
ocp_nlp_config *nlp_config;
|
||||
ocp_nlp_dims *nlp_dims;
|
||||
|
||||
// number of expected runtime parameters
|
||||
unsigned int nlp_np;
|
||||
|
||||
/* external functions */
|
||||
// dynamics
|
||||
|
||||
external_function_param_casadi *forw_vde_casadi;
|
||||
external_function_param_casadi *expl_ode_fun;
|
||||
|
||||
|
||||
|
||||
|
||||
// cost
|
||||
|
||||
external_function_param_casadi *cost_y_fun;
|
||||
external_function_param_casadi *cost_y_fun_jac_ut_xt;
|
||||
external_function_param_casadi *cost_y_hess;
|
||||
|
||||
|
||||
|
||||
external_function_param_casadi cost_y_0_fun;
|
||||
external_function_param_casadi cost_y_0_fun_jac_ut_xt;
|
||||
external_function_param_casadi cost_y_0_hess;
|
||||
|
||||
|
||||
|
||||
external_function_param_casadi cost_y_e_fun;
|
||||
external_function_param_casadi cost_y_e_fun_jac_ut_xt;
|
||||
external_function_param_casadi cost_y_e_hess;
|
||||
|
||||
|
||||
// constraints
|
||||
external_function_param_casadi *nl_constr_h_fun_jac;
|
||||
external_function_param_casadi *nl_constr_h_fun;
|
||||
|
||||
|
||||
|
||||
|
||||
} long_solver_capsule;
|
||||
|
||||
ACADOS_SYMBOL_EXPORT long_solver_capsule * long_acados_create_capsule(void);
|
||||
ACADOS_SYMBOL_EXPORT int long_acados_free_capsule(long_solver_capsule *capsule);
|
||||
|
||||
ACADOS_SYMBOL_EXPORT int long_acados_create(long_solver_capsule * capsule);
|
||||
|
||||
ACADOS_SYMBOL_EXPORT int long_acados_reset(long_solver_capsule* capsule, int reset_qp_solver_mem);
|
||||
|
||||
/**
|
||||
* Generic version of long_acados_create which allows to use a different number of shooting intervals than
|
||||
* the number used for code generation. If new_time_steps=NULL and n_time_steps matches the number used for code
|
||||
* generation, the time-steps from code generation is used.
|
||||
*/
|
||||
ACADOS_SYMBOL_EXPORT int long_acados_create_with_discretization(long_solver_capsule * capsule, int n_time_steps, double* new_time_steps);
|
||||
/**
|
||||
* Update the time step vector. Number N must be identical to the currently set number of shooting nodes in the
|
||||
* nlp_solver_plan. Returns 0 if no error occurred and a otherwise a value other than 0.
|
||||
*/
|
||||
ACADOS_SYMBOL_EXPORT int long_acados_update_time_steps(long_solver_capsule * capsule, int N, double* new_time_steps);
|
||||
/**
|
||||
* This function is used for updating an already initialized solver with a different number of qp_cond_N.
|
||||
*/
|
||||
ACADOS_SYMBOL_EXPORT int long_acados_update_qp_solver_cond_N(long_solver_capsule * capsule, int qp_solver_cond_N);
|
||||
ACADOS_SYMBOL_EXPORT int long_acados_update_params(long_solver_capsule * capsule, int stage, double *value, int np);
|
||||
ACADOS_SYMBOL_EXPORT int long_acados_update_params_sparse(long_solver_capsule * capsule, int stage, int *idx, double *p, int n_update);
|
||||
|
||||
ACADOS_SYMBOL_EXPORT int long_acados_solve(long_solver_capsule * capsule);
|
||||
ACADOS_SYMBOL_EXPORT int long_acados_free(long_solver_capsule * capsule);
|
||||
ACADOS_SYMBOL_EXPORT void long_acados_print_stats(long_solver_capsule * capsule);
|
||||
ACADOS_SYMBOL_EXPORT int long_acados_custom_update(long_solver_capsule* capsule, double* data, int data_len);
|
||||
|
||||
|
||||
ACADOS_SYMBOL_EXPORT ocp_nlp_in *long_acados_get_nlp_in(long_solver_capsule * capsule);
|
||||
ACADOS_SYMBOL_EXPORT ocp_nlp_out *long_acados_get_nlp_out(long_solver_capsule * capsule);
|
||||
ACADOS_SYMBOL_EXPORT ocp_nlp_out *long_acados_get_sens_out(long_solver_capsule * capsule);
|
||||
ACADOS_SYMBOL_EXPORT ocp_nlp_solver *long_acados_get_nlp_solver(long_solver_capsule * capsule);
|
||||
ACADOS_SYMBOL_EXPORT ocp_nlp_config *long_acados_get_nlp_config(long_solver_capsule * capsule);
|
||||
ACADOS_SYMBOL_EXPORT void *long_acados_get_nlp_opts(long_solver_capsule * capsule);
|
||||
ACADOS_SYMBOL_EXPORT ocp_nlp_dims *long_acados_get_nlp_dims(long_solver_capsule * capsule);
|
||||
ACADOS_SYMBOL_EXPORT ocp_nlp_plan_t *long_acados_get_nlp_plan(long_solver_capsule * capsule);
|
||||
|
||||
#ifdef __cplusplus
|
||||
} /* extern "C" */
|
||||
#endif
|
||||
|
||||
#endif // ACADOS_SOLVER_long_H_
|
||||
BIN
Binary file not shown.
Executable
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
Executable
BIN
Binary file not shown.
+187
@@ -0,0 +1,187 @@
|
||||
/* This file was automatically generated by CasADi 3.6.7.
|
||||
* It consists of:
|
||||
* 1) content generated by CasADi runtime: not copyrighted
|
||||
* 2) template code copied from CasADi source: permissively licensed (MIT-0)
|
||||
* 3) user code: owned by the user
|
||||
*
|
||||
*/
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/* How to prefix internal symbols */
|
||||
#ifdef CASADI_CODEGEN_PREFIX
|
||||
#define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID)
|
||||
#define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID
|
||||
#define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID)
|
||||
#else
|
||||
#define CASADI_PREFIX(ID) long_constr_h_fun_ ## ID
|
||||
#endif
|
||||
|
||||
#include <math.h>
|
||||
|
||||
#ifndef casadi_real
|
||||
#define casadi_real double
|
||||
#endif
|
||||
|
||||
#ifndef casadi_int
|
||||
#define casadi_int int
|
||||
#endif
|
||||
|
||||
/* Add prefix to internal symbols */
|
||||
#define casadi_f0 CASADI_PREFIX(f0)
|
||||
#define casadi_s0 CASADI_PREFIX(s0)
|
||||
#define casadi_s1 CASADI_PREFIX(s1)
|
||||
#define casadi_s2 CASADI_PREFIX(s2)
|
||||
#define casadi_s3 CASADI_PREFIX(s3)
|
||||
#define casadi_s4 CASADI_PREFIX(s4)
|
||||
#define casadi_sq CASADI_PREFIX(sq)
|
||||
|
||||
/* Symbol visibility in DLLs */
|
||||
#ifndef CASADI_SYMBOL_EXPORT
|
||||
#if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__)
|
||||
#if defined(STATIC_LINKED)
|
||||
#define CASADI_SYMBOL_EXPORT
|
||||
#else
|
||||
#define CASADI_SYMBOL_EXPORT __declspec(dllexport)
|
||||
#endif
|
||||
#elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY)
|
||||
#define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default")))
|
||||
#else
|
||||
#define CASADI_SYMBOL_EXPORT
|
||||
#endif
|
||||
#endif
|
||||
|
||||
casadi_real casadi_sq(casadi_real x) { return x*x;}
|
||||
|
||||
static const casadi_int casadi_s0[7] = {3, 1, 0, 3, 0, 1, 2};
|
||||
static const casadi_int casadi_s1[5] = {1, 1, 0, 1, 0};
|
||||
static const casadi_int casadi_s2[3] = {0, 0, 0};
|
||||
static const casadi_int casadi_s3[10] = {6, 1, 0, 6, 0, 1, 2, 3, 4, 5};
|
||||
static const casadi_int casadi_s4[8] = {4, 1, 0, 4, 0, 1, 2, 3};
|
||||
|
||||
/* long_constr_h_fun:(i0[3],i1,i2[],i3[6])->(o0[4]) */
|
||||
static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
|
||||
casadi_real a0, a1, a2, a3, a4;
|
||||
a0=arg[0]? arg[0][1] : 0;
|
||||
if (res[0]!=0) res[0][0]=a0;
|
||||
a1=arg[0]? arg[0][2] : 0;
|
||||
a2=arg[3]? arg[3][0] : 0;
|
||||
a2=(a1-a2);
|
||||
if (res[0]!=0) res[0][1]=a2;
|
||||
a2=arg[3]? arg[3][1] : 0;
|
||||
a2=(a2-a1);
|
||||
if (res[0]!=0) res[0][2]=a2;
|
||||
a2=arg[3]? arg[3][2] : 0;
|
||||
a1=arg[0]? arg[0][0] : 0;
|
||||
a2=(a2-a1);
|
||||
a1=arg[3]? arg[3][5] : 0;
|
||||
a3=casadi_sq(a0);
|
||||
a4=5.;
|
||||
a3=(a3/a4);
|
||||
a4=arg[3]? arg[3][4] : 0;
|
||||
a4=(a4*a0);
|
||||
a3=(a3+a4);
|
||||
a4=6.;
|
||||
a3=(a3+a4);
|
||||
a1=(a1*a3);
|
||||
a2=(a2-a1);
|
||||
a1=10.;
|
||||
a0=(a0+a1);
|
||||
a2=(a2/a0);
|
||||
if (res[0]!=0) res[0][3]=a2;
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int long_constr_h_fun(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){
|
||||
return casadi_f0(arg, res, iw, w, mem);
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int long_constr_h_fun_alloc_mem(void) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int long_constr_h_fun_init_mem(int mem) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void long_constr_h_fun_free_mem(int mem) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int long_constr_h_fun_checkout(void) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void long_constr_h_fun_release(int mem) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void long_constr_h_fun_incref(void) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void long_constr_h_fun_decref(void) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT casadi_int long_constr_h_fun_n_in(void) { return 4;}
|
||||
|
||||
CASADI_SYMBOL_EXPORT casadi_int long_constr_h_fun_n_out(void) { return 1;}
|
||||
|
||||
CASADI_SYMBOL_EXPORT casadi_real long_constr_h_fun_default_in(casadi_int i) {
|
||||
switch (i) {
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const char* long_constr_h_fun_name_in(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return "i0";
|
||||
case 1: return "i1";
|
||||
case 2: return "i2";
|
||||
case 3: return "i3";
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const char* long_constr_h_fun_name_out(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return "o0";
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const casadi_int* long_constr_h_fun_sparsity_in(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return casadi_s0;
|
||||
case 1: return casadi_s1;
|
||||
case 2: return casadi_s2;
|
||||
case 3: return casadi_s3;
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const casadi_int* long_constr_h_fun_sparsity_out(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return casadi_s4;
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int long_constr_h_fun_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
|
||||
if (sz_arg) *sz_arg = 4;
|
||||
if (sz_res) *sz_res = 1;
|
||||
if (sz_iw) *sz_iw = 0;
|
||||
if (sz_w) *sz_w = 0;
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int long_constr_h_fun_work_bytes(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
|
||||
if (sz_arg) *sz_arg = 4*sizeof(const casadi_real*);
|
||||
if (sz_res) *sz_res = 1*sizeof(casadi_real*);
|
||||
if (sz_iw) *sz_iw = 0*sizeof(casadi_int);
|
||||
if (sz_w) *sz_w = 0*sizeof(casadi_real);
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
} /* extern "C" */
|
||||
#endif
|
||||
+213
@@ -0,0 +1,213 @@
|
||||
/* This file was automatically generated by CasADi 3.6.7.
|
||||
* It consists of:
|
||||
* 1) content generated by CasADi runtime: not copyrighted
|
||||
* 2) template code copied from CasADi source: permissively licensed (MIT-0)
|
||||
* 3) user code: owned by the user
|
||||
*
|
||||
*/
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/* How to prefix internal symbols */
|
||||
#ifdef CASADI_CODEGEN_PREFIX
|
||||
#define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID)
|
||||
#define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID
|
||||
#define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID)
|
||||
#else
|
||||
#define CASADI_PREFIX(ID) long_constr_h_fun_jac_uxt_zt_ ## ID
|
||||
#endif
|
||||
|
||||
#include <math.h>
|
||||
|
||||
#ifndef casadi_real
|
||||
#define casadi_real double
|
||||
#endif
|
||||
|
||||
#ifndef casadi_int
|
||||
#define casadi_int int
|
||||
#endif
|
||||
|
||||
/* Add prefix to internal symbols */
|
||||
#define casadi_f0 CASADI_PREFIX(f0)
|
||||
#define casadi_s0 CASADI_PREFIX(s0)
|
||||
#define casadi_s1 CASADI_PREFIX(s1)
|
||||
#define casadi_s2 CASADI_PREFIX(s2)
|
||||
#define casadi_s3 CASADI_PREFIX(s3)
|
||||
#define casadi_s4 CASADI_PREFIX(s4)
|
||||
#define casadi_s5 CASADI_PREFIX(s5)
|
||||
#define casadi_s6 CASADI_PREFIX(s6)
|
||||
#define casadi_sq CASADI_PREFIX(sq)
|
||||
|
||||
/* Symbol visibility in DLLs */
|
||||
#ifndef CASADI_SYMBOL_EXPORT
|
||||
#if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__)
|
||||
#if defined(STATIC_LINKED)
|
||||
#define CASADI_SYMBOL_EXPORT
|
||||
#else
|
||||
#define CASADI_SYMBOL_EXPORT __declspec(dllexport)
|
||||
#endif
|
||||
#elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY)
|
||||
#define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default")))
|
||||
#else
|
||||
#define CASADI_SYMBOL_EXPORT
|
||||
#endif
|
||||
#endif
|
||||
|
||||
casadi_real casadi_sq(casadi_real x) { return x*x;}
|
||||
|
||||
static const casadi_int casadi_s0[7] = {3, 1, 0, 3, 0, 1, 2};
|
||||
static const casadi_int casadi_s1[5] = {1, 1, 0, 1, 0};
|
||||
static const casadi_int casadi_s2[3] = {0, 0, 0};
|
||||
static const casadi_int casadi_s3[10] = {6, 1, 0, 6, 0, 1, 2, 3, 4, 5};
|
||||
static const casadi_int casadi_s4[8] = {4, 1, 0, 4, 0, 1, 2, 3};
|
||||
static const casadi_int casadi_s5[12] = {4, 4, 0, 1, 2, 3, 5, 2, 3, 3, 1, 2};
|
||||
static const casadi_int casadi_s6[3] = {4, 0, 0};
|
||||
|
||||
/* long_constr_h_fun_jac_uxt_zt:(i0[3],i1,i2[],i3[6])->(o0[4],o1[4x4,5nz],o2[4x0]) */
|
||||
static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
|
||||
casadi_real a0, a1, a2, a3, a4, a5;
|
||||
a0=arg[0]? arg[0][1] : 0;
|
||||
if (res[0]!=0) res[0][0]=a0;
|
||||
a1=arg[0]? arg[0][2] : 0;
|
||||
a2=arg[3]? arg[3][0] : 0;
|
||||
a2=(a1-a2);
|
||||
if (res[0]!=0) res[0][1]=a2;
|
||||
a2=arg[3]? arg[3][1] : 0;
|
||||
a2=(a2-a1);
|
||||
if (res[0]!=0) res[0][2]=a2;
|
||||
a2=arg[3]? arg[3][2] : 0;
|
||||
a1=arg[0]? arg[0][0] : 0;
|
||||
a2=(a2-a1);
|
||||
a1=arg[3]? arg[3][5] : 0;
|
||||
a3=casadi_sq(a0);
|
||||
a4=5.;
|
||||
a3=(a3/a4);
|
||||
a4=arg[3]? arg[3][4] : 0;
|
||||
a5=(a4*a0);
|
||||
a3=(a3+a5);
|
||||
a5=6.;
|
||||
a3=(a3+a5);
|
||||
a3=(a1*a3);
|
||||
a2=(a2-a3);
|
||||
a3=10.;
|
||||
a3=(a0+a3);
|
||||
a2=(a2/a3);
|
||||
if (res[0]!=0) res[0][3]=a2;
|
||||
a5=1.;
|
||||
if (res[1]!=0) res[1][0]=a5;
|
||||
if (res[1]!=0) res[1][1]=a5;
|
||||
a5=-1.;
|
||||
if (res[1]!=0) res[1][2]=a5;
|
||||
a5=(1./a3);
|
||||
a5=(-a5);
|
||||
if (res[1]!=0) res[1][3]=a5;
|
||||
a5=2.0000000000000001e-01;
|
||||
a0=(a0+a0);
|
||||
a5=(a5*a0);
|
||||
a5=(a5+a4);
|
||||
a1=(a1*a5);
|
||||
a1=(a1/a3);
|
||||
a2=(a2/a3);
|
||||
a1=(a1+a2);
|
||||
a1=(-a1);
|
||||
if (res[1]!=0) res[1][4]=a1;
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int long_constr_h_fun_jac_uxt_zt(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){
|
||||
return casadi_f0(arg, res, iw, w, mem);
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int long_constr_h_fun_jac_uxt_zt_alloc_mem(void) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int long_constr_h_fun_jac_uxt_zt_init_mem(int mem) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void long_constr_h_fun_jac_uxt_zt_free_mem(int mem) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int long_constr_h_fun_jac_uxt_zt_checkout(void) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void long_constr_h_fun_jac_uxt_zt_release(int mem) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void long_constr_h_fun_jac_uxt_zt_incref(void) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void long_constr_h_fun_jac_uxt_zt_decref(void) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT casadi_int long_constr_h_fun_jac_uxt_zt_n_in(void) { return 4;}
|
||||
|
||||
CASADI_SYMBOL_EXPORT casadi_int long_constr_h_fun_jac_uxt_zt_n_out(void) { return 3;}
|
||||
|
||||
CASADI_SYMBOL_EXPORT casadi_real long_constr_h_fun_jac_uxt_zt_default_in(casadi_int i) {
|
||||
switch (i) {
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const char* long_constr_h_fun_jac_uxt_zt_name_in(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return "i0";
|
||||
case 1: return "i1";
|
||||
case 2: return "i2";
|
||||
case 3: return "i3";
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const char* long_constr_h_fun_jac_uxt_zt_name_out(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return "o0";
|
||||
case 1: return "o1";
|
||||
case 2: return "o2";
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const casadi_int* long_constr_h_fun_jac_uxt_zt_sparsity_in(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return casadi_s0;
|
||||
case 1: return casadi_s1;
|
||||
case 2: return casadi_s2;
|
||||
case 3: return casadi_s3;
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const casadi_int* long_constr_h_fun_jac_uxt_zt_sparsity_out(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return casadi_s4;
|
||||
case 1: return casadi_s5;
|
||||
case 2: return casadi_s6;
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int long_constr_h_fun_jac_uxt_zt_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
|
||||
if (sz_arg) *sz_arg = 4;
|
||||
if (sz_res) *sz_res = 3;
|
||||
if (sz_iw) *sz_iw = 0;
|
||||
if (sz_w) *sz_w = 0;
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int long_constr_h_fun_jac_uxt_zt_work_bytes(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
|
||||
if (sz_arg) *sz_arg = 4*sizeof(const casadi_real*);
|
||||
if (sz_res) *sz_res = 3*sizeof(casadi_real*);
|
||||
if (sz_iw) *sz_iw = 0*sizeof(casadi_int);
|
||||
if (sz_w) *sz_w = 0*sizeof(casadi_real);
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
} /* extern "C" */
|
||||
#endif
|
||||
+66
@@ -0,0 +1,66 @@
|
||||
/*
|
||||
* Copyright (c) The acados authors.
|
||||
*
|
||||
* This file is part of acados.
|
||||
*
|
||||
* The 2-Clause BSD License
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice,
|
||||
* this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.;
|
||||
*/
|
||||
|
||||
#ifndef long_CONSTRAINTS
|
||||
#define long_CONSTRAINTS
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
int long_constr_h_fun_jac_uxt_zt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
|
||||
int long_constr_h_fun_jac_uxt_zt_work(int *, int *, int *, int *);
|
||||
const int *long_constr_h_fun_jac_uxt_zt_sparsity_in(int);
|
||||
const int *long_constr_h_fun_jac_uxt_zt_sparsity_out(int);
|
||||
int long_constr_h_fun_jac_uxt_zt_n_in(void);
|
||||
int long_constr_h_fun_jac_uxt_zt_n_out(void);
|
||||
|
||||
int long_constr_h_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
|
||||
int long_constr_h_fun_work(int *, int *, int *, int *);
|
||||
const int *long_constr_h_fun_sparsity_in(int);
|
||||
const int *long_constr_h_fun_sparsity_out(int);
|
||||
int long_constr_h_fun_n_in(void);
|
||||
int long_constr_h_fun_n_out(void);
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
} /* extern "C" */
|
||||
#endif
|
||||
|
||||
#endif // long_CONSTRAINTS
|
||||
+119
@@ -0,0 +1,119 @@
|
||||
/*
|
||||
* Copyright (c) The acados authors.
|
||||
*
|
||||
* This file is part of acados.
|
||||
*
|
||||
* The 2-Clause BSD License
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice,
|
||||
* this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.;
|
||||
*/
|
||||
|
||||
|
||||
#ifndef long_COST
|
||||
#define long_COST
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
|
||||
// Cost at initial shooting node
|
||||
|
||||
int long_cost_y_0_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
|
||||
int long_cost_y_0_fun_work(int *, int *, int *, int *);
|
||||
const int *long_cost_y_0_fun_sparsity_in(int);
|
||||
const int *long_cost_y_0_fun_sparsity_out(int);
|
||||
int long_cost_y_0_fun_n_in(void);
|
||||
int long_cost_y_0_fun_n_out(void);
|
||||
|
||||
int long_cost_y_0_fun_jac_ut_xt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
|
||||
int long_cost_y_0_fun_jac_ut_xt_work(int *, int *, int *, int *);
|
||||
const int *long_cost_y_0_fun_jac_ut_xt_sparsity_in(int);
|
||||
const int *long_cost_y_0_fun_jac_ut_xt_sparsity_out(int);
|
||||
int long_cost_y_0_fun_jac_ut_xt_n_in(void);
|
||||
int long_cost_y_0_fun_jac_ut_xt_n_out(void);
|
||||
|
||||
int long_cost_y_0_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
|
||||
int long_cost_y_0_hess_work(int *, int *, int *, int *);
|
||||
const int *long_cost_y_0_hess_sparsity_in(int);
|
||||
const int *long_cost_y_0_hess_sparsity_out(int);
|
||||
int long_cost_y_0_hess_n_in(void);
|
||||
int long_cost_y_0_hess_n_out(void);
|
||||
|
||||
|
||||
|
||||
// Cost at path shooting node
|
||||
|
||||
int long_cost_y_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
|
||||
int long_cost_y_fun_work(int *, int *, int *, int *);
|
||||
const int *long_cost_y_fun_sparsity_in(int);
|
||||
const int *long_cost_y_fun_sparsity_out(int);
|
||||
int long_cost_y_fun_n_in(void);
|
||||
int long_cost_y_fun_n_out(void);
|
||||
|
||||
int long_cost_y_fun_jac_ut_xt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
|
||||
int long_cost_y_fun_jac_ut_xt_work(int *, int *, int *, int *);
|
||||
const int *long_cost_y_fun_jac_ut_xt_sparsity_in(int);
|
||||
const int *long_cost_y_fun_jac_ut_xt_sparsity_out(int);
|
||||
int long_cost_y_fun_jac_ut_xt_n_in(void);
|
||||
int long_cost_y_fun_jac_ut_xt_n_out(void);
|
||||
|
||||
int long_cost_y_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
|
||||
int long_cost_y_hess_work(int *, int *, int *, int *);
|
||||
const int *long_cost_y_hess_sparsity_in(int);
|
||||
const int *long_cost_y_hess_sparsity_out(int);
|
||||
int long_cost_y_hess_n_in(void);
|
||||
int long_cost_y_hess_n_out(void);
|
||||
|
||||
|
||||
|
||||
// Cost at terminal shooting node
|
||||
|
||||
int long_cost_y_e_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
|
||||
int long_cost_y_e_fun_work(int *, int *, int *, int *);
|
||||
const int *long_cost_y_e_fun_sparsity_in(int);
|
||||
const int *long_cost_y_e_fun_sparsity_out(int);
|
||||
int long_cost_y_e_fun_n_in(void);
|
||||
int long_cost_y_e_fun_n_out(void);
|
||||
|
||||
int long_cost_y_e_fun_jac_ut_xt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
|
||||
int long_cost_y_e_fun_jac_ut_xt_work(int *, int *, int *, int *);
|
||||
const int *long_cost_y_e_fun_jac_ut_xt_sparsity_in(int);
|
||||
const int *long_cost_y_e_fun_jac_ut_xt_sparsity_out(int);
|
||||
int long_cost_y_e_fun_jac_ut_xt_n_in(void);
|
||||
int long_cost_y_e_fun_jac_ut_xt_n_out(void);
|
||||
|
||||
int long_cost_y_e_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
|
||||
int long_cost_y_e_hess_work(int *, int *, int *, int *);
|
||||
const int *long_cost_y_e_hess_sparsity_in(int);
|
||||
const int *long_cost_y_e_hess_sparsity_out(int);
|
||||
int long_cost_y_e_hess_n_in(void);
|
||||
int long_cost_y_e_hess_n_out(void);
|
||||
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
} /* extern "C" */
|
||||
#endif
|
||||
|
||||
#endif // long_COST
|
||||
+184
@@ -0,0 +1,184 @@
|
||||
/* This file was automatically generated by CasADi 3.6.7.
|
||||
* It consists of:
|
||||
* 1) content generated by CasADi runtime: not copyrighted
|
||||
* 2) template code copied from CasADi source: permissively licensed (MIT-0)
|
||||
* 3) user code: owned by the user
|
||||
*
|
||||
*/
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/* How to prefix internal symbols */
|
||||
#ifdef CASADI_CODEGEN_PREFIX
|
||||
#define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID)
|
||||
#define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID
|
||||
#define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID)
|
||||
#else
|
||||
#define CASADI_PREFIX(ID) long_cost_y_0_fun_ ## ID
|
||||
#endif
|
||||
|
||||
#include <math.h>
|
||||
|
||||
#ifndef casadi_real
|
||||
#define casadi_real double
|
||||
#endif
|
||||
|
||||
#ifndef casadi_int
|
||||
#define casadi_int int
|
||||
#endif
|
||||
|
||||
/* Add prefix to internal symbols */
|
||||
#define casadi_f0 CASADI_PREFIX(f0)
|
||||
#define casadi_s0 CASADI_PREFIX(s0)
|
||||
#define casadi_s1 CASADI_PREFIX(s1)
|
||||
#define casadi_s2 CASADI_PREFIX(s2)
|
||||
#define casadi_s3 CASADI_PREFIX(s3)
|
||||
#define casadi_sq CASADI_PREFIX(sq)
|
||||
|
||||
/* Symbol visibility in DLLs */
|
||||
#ifndef CASADI_SYMBOL_EXPORT
|
||||
#if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__)
|
||||
#if defined(STATIC_LINKED)
|
||||
#define CASADI_SYMBOL_EXPORT
|
||||
#else
|
||||
#define CASADI_SYMBOL_EXPORT __declspec(dllexport)
|
||||
#endif
|
||||
#elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY)
|
||||
#define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default")))
|
||||
#else
|
||||
#define CASADI_SYMBOL_EXPORT
|
||||
#endif
|
||||
#endif
|
||||
|
||||
casadi_real casadi_sq(casadi_real x) { return x*x;}
|
||||
|
||||
static const casadi_int casadi_s0[7] = {3, 1, 0, 3, 0, 1, 2};
|
||||
static const casadi_int casadi_s1[5] = {1, 1, 0, 1, 0};
|
||||
static const casadi_int casadi_s2[3] = {0, 0, 0};
|
||||
static const casadi_int casadi_s3[10] = {6, 1, 0, 6, 0, 1, 2, 3, 4, 5};
|
||||
|
||||
/* long_cost_y_0_fun:(i0[3],i1,i2[],i3[6])->(o0[6]) */
|
||||
static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
|
||||
casadi_real a0, a1, a2, a3, a4;
|
||||
a0=arg[3]? arg[3][2] : 0;
|
||||
a1=arg[0]? arg[0][0] : 0;
|
||||
a0=(a0-a1);
|
||||
a2=arg[0]? arg[0][1] : 0;
|
||||
a3=casadi_sq(a2);
|
||||
a4=5.;
|
||||
a3=(a3/a4);
|
||||
a4=arg[3]? arg[3][4] : 0;
|
||||
a4=(a4*a2);
|
||||
a3=(a3+a4);
|
||||
a4=6.;
|
||||
a3=(a3+a4);
|
||||
a0=(a0-a3);
|
||||
a3=10.;
|
||||
a3=(a2+a3);
|
||||
a0=(a0/a3);
|
||||
if (res[0]!=0) res[0][0]=a0;
|
||||
if (res[0]!=0) res[0][1]=a1;
|
||||
if (res[0]!=0) res[0][2]=a2;
|
||||
a2=arg[0]? arg[0][2] : 0;
|
||||
if (res[0]!=0) res[0][3]=a2;
|
||||
a1=arg[3]? arg[3][3] : 0;
|
||||
a2=(a2-a1);
|
||||
if (res[0]!=0) res[0][4]=a2;
|
||||
a2=arg[1]? arg[1][0] : 0;
|
||||
if (res[0]!=0) res[0][5]=a2;
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int long_cost_y_0_fun(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){
|
||||
return casadi_f0(arg, res, iw, w, mem);
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int long_cost_y_0_fun_alloc_mem(void) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int long_cost_y_0_fun_init_mem(int mem) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void long_cost_y_0_fun_free_mem(int mem) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int long_cost_y_0_fun_checkout(void) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void long_cost_y_0_fun_release(int mem) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void long_cost_y_0_fun_incref(void) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void long_cost_y_0_fun_decref(void) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT casadi_int long_cost_y_0_fun_n_in(void) { return 4;}
|
||||
|
||||
CASADI_SYMBOL_EXPORT casadi_int long_cost_y_0_fun_n_out(void) { return 1;}
|
||||
|
||||
CASADI_SYMBOL_EXPORT casadi_real long_cost_y_0_fun_default_in(casadi_int i) {
|
||||
switch (i) {
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const char* long_cost_y_0_fun_name_in(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return "i0";
|
||||
case 1: return "i1";
|
||||
case 2: return "i2";
|
||||
case 3: return "i3";
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const char* long_cost_y_0_fun_name_out(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return "o0";
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const casadi_int* long_cost_y_0_fun_sparsity_in(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return casadi_s0;
|
||||
case 1: return casadi_s1;
|
||||
case 2: return casadi_s2;
|
||||
case 3: return casadi_s3;
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const casadi_int* long_cost_y_0_fun_sparsity_out(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return casadi_s3;
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int long_cost_y_0_fun_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
|
||||
if (sz_arg) *sz_arg = 4;
|
||||
if (sz_res) *sz_res = 1;
|
||||
if (sz_iw) *sz_iw = 0;
|
||||
if (sz_w) *sz_w = 0;
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int long_cost_y_0_fun_work_bytes(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
|
||||
if (sz_arg) *sz_arg = 4*sizeof(const casadi_real*);
|
||||
if (sz_res) *sz_res = 1*sizeof(casadi_real*);
|
||||
if (sz_iw) *sz_iw = 0*sizeof(casadi_int);
|
||||
if (sz_w) *sz_w = 0*sizeof(casadi_real);
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
} /* extern "C" */
|
||||
#endif
|
||||
+210
@@ -0,0 +1,210 @@
|
||||
/* This file was automatically generated by CasADi 3.6.7.
|
||||
* It consists of:
|
||||
* 1) content generated by CasADi runtime: not copyrighted
|
||||
* 2) template code copied from CasADi source: permissively licensed (MIT-0)
|
||||
* 3) user code: owned by the user
|
||||
*
|
||||
*/
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/* How to prefix internal symbols */
|
||||
#ifdef CASADI_CODEGEN_PREFIX
|
||||
#define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID)
|
||||
#define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID
|
||||
#define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID)
|
||||
#else
|
||||
#define CASADI_PREFIX(ID) long_cost_y_0_fun_jac_ut_xt_ ## ID
|
||||
#endif
|
||||
|
||||
#include <math.h>
|
||||
|
||||
#ifndef casadi_real
|
||||
#define casadi_real double
|
||||
#endif
|
||||
|
||||
#ifndef casadi_int
|
||||
#define casadi_int int
|
||||
#endif
|
||||
|
||||
/* Add prefix to internal symbols */
|
||||
#define casadi_f0 CASADI_PREFIX(f0)
|
||||
#define casadi_s0 CASADI_PREFIX(s0)
|
||||
#define casadi_s1 CASADI_PREFIX(s1)
|
||||
#define casadi_s2 CASADI_PREFIX(s2)
|
||||
#define casadi_s3 CASADI_PREFIX(s3)
|
||||
#define casadi_s4 CASADI_PREFIX(s4)
|
||||
#define casadi_s5 CASADI_PREFIX(s5)
|
||||
#define casadi_sq CASADI_PREFIX(sq)
|
||||
|
||||
/* Symbol visibility in DLLs */
|
||||
#ifndef CASADI_SYMBOL_EXPORT
|
||||
#if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__)
|
||||
#if defined(STATIC_LINKED)
|
||||
#define CASADI_SYMBOL_EXPORT
|
||||
#else
|
||||
#define CASADI_SYMBOL_EXPORT __declspec(dllexport)
|
||||
#endif
|
||||
#elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY)
|
||||
#define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default")))
|
||||
#else
|
||||
#define CASADI_SYMBOL_EXPORT
|
||||
#endif
|
||||
#endif
|
||||
|
||||
casadi_real casadi_sq(casadi_real x) { return x*x;}
|
||||
|
||||
static const casadi_int casadi_s0[7] = {3, 1, 0, 3, 0, 1, 2};
|
||||
static const casadi_int casadi_s1[5] = {1, 1, 0, 1, 0};
|
||||
static const casadi_int casadi_s2[3] = {0, 0, 0};
|
||||
static const casadi_int casadi_s3[10] = {6, 1, 0, 6, 0, 1, 2, 3, 4, 5};
|
||||
static const casadi_int casadi_s4[16] = {4, 6, 0, 2, 3, 4, 5, 6, 7, 1, 2, 1, 2, 3, 3, 0};
|
||||
static const casadi_int casadi_s5[3] = {6, 0, 0};
|
||||
|
||||
/* long_cost_y_0_fun_jac_ut_xt:(i0[3],i1,i2[],i3[6])->(o0[6],o1[4x6,7nz],o2[6x0]) */
|
||||
static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
|
||||
casadi_real a0, a1, a2, a3, a4, a5;
|
||||
a0=arg[3]? arg[3][2] : 0;
|
||||
a1=arg[0]? arg[0][0] : 0;
|
||||
a0=(a0-a1);
|
||||
a2=arg[0]? arg[0][1] : 0;
|
||||
a3=casadi_sq(a2);
|
||||
a4=5.;
|
||||
a3=(a3/a4);
|
||||
a4=arg[3]? arg[3][4] : 0;
|
||||
a5=(a4*a2);
|
||||
a3=(a3+a5);
|
||||
a5=6.;
|
||||
a3=(a3+a5);
|
||||
a0=(a0-a3);
|
||||
a3=10.;
|
||||
a3=(a2+a3);
|
||||
a0=(a0/a3);
|
||||
if (res[0]!=0) res[0][0]=a0;
|
||||
if (res[0]!=0) res[0][1]=a1;
|
||||
if (res[0]!=0) res[0][2]=a2;
|
||||
a1=arg[0]? arg[0][2] : 0;
|
||||
if (res[0]!=0) res[0][3]=a1;
|
||||
a5=arg[3]? arg[3][3] : 0;
|
||||
a1=(a1-a5);
|
||||
if (res[0]!=0) res[0][4]=a1;
|
||||
a1=arg[1]? arg[1][0] : 0;
|
||||
if (res[0]!=0) res[0][5]=a1;
|
||||
a1=(1./a3);
|
||||
a1=(-a1);
|
||||
if (res[1]!=0) res[1][0]=a1;
|
||||
a1=2.0000000000000001e-01;
|
||||
a2=(a2+a2);
|
||||
a1=(a1*a2);
|
||||
a1=(a1+a4);
|
||||
a1=(a1/a3);
|
||||
a0=(a0/a3);
|
||||
a1=(a1+a0);
|
||||
a1=(-a1);
|
||||
if (res[1]!=0) res[1][1]=a1;
|
||||
a1=1.;
|
||||
if (res[1]!=0) res[1][2]=a1;
|
||||
if (res[1]!=0) res[1][3]=a1;
|
||||
if (res[1]!=0) res[1][4]=a1;
|
||||
if (res[1]!=0) res[1][5]=a1;
|
||||
if (res[1]!=0) res[1][6]=a1;
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int long_cost_y_0_fun_jac_ut_xt(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){
|
||||
return casadi_f0(arg, res, iw, w, mem);
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int long_cost_y_0_fun_jac_ut_xt_alloc_mem(void) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int long_cost_y_0_fun_jac_ut_xt_init_mem(int mem) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void long_cost_y_0_fun_jac_ut_xt_free_mem(int mem) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int long_cost_y_0_fun_jac_ut_xt_checkout(void) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void long_cost_y_0_fun_jac_ut_xt_release(int mem) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void long_cost_y_0_fun_jac_ut_xt_incref(void) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void long_cost_y_0_fun_jac_ut_xt_decref(void) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT casadi_int long_cost_y_0_fun_jac_ut_xt_n_in(void) { return 4;}
|
||||
|
||||
CASADI_SYMBOL_EXPORT casadi_int long_cost_y_0_fun_jac_ut_xt_n_out(void) { return 3;}
|
||||
|
||||
CASADI_SYMBOL_EXPORT casadi_real long_cost_y_0_fun_jac_ut_xt_default_in(casadi_int i) {
|
||||
switch (i) {
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const char* long_cost_y_0_fun_jac_ut_xt_name_in(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return "i0";
|
||||
case 1: return "i1";
|
||||
case 2: return "i2";
|
||||
case 3: return "i3";
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const char* long_cost_y_0_fun_jac_ut_xt_name_out(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return "o0";
|
||||
case 1: return "o1";
|
||||
case 2: return "o2";
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const casadi_int* long_cost_y_0_fun_jac_ut_xt_sparsity_in(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return casadi_s0;
|
||||
case 1: return casadi_s1;
|
||||
case 2: return casadi_s2;
|
||||
case 3: return casadi_s3;
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const casadi_int* long_cost_y_0_fun_jac_ut_xt_sparsity_out(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return casadi_s3;
|
||||
case 1: return casadi_s4;
|
||||
case 2: return casadi_s5;
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int long_cost_y_0_fun_jac_ut_xt_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
|
||||
if (sz_arg) *sz_arg = 4;
|
||||
if (sz_res) *sz_res = 3;
|
||||
if (sz_iw) *sz_iw = 0;
|
||||
if (sz_w) *sz_w = 0;
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int long_cost_y_0_fun_jac_ut_xt_work_bytes(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
|
||||
if (sz_arg) *sz_arg = 4*sizeof(const casadi_real*);
|
||||
if (sz_res) *sz_res = 3*sizeof(casadi_real*);
|
||||
if (sz_iw) *sz_iw = 0*sizeof(casadi_int);
|
||||
if (sz_w) *sz_w = 0*sizeof(casadi_real);
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
} /* extern "C" */
|
||||
#endif
|
||||
+209
@@ -0,0 +1,209 @@
|
||||
/* This file was automatically generated by CasADi 3.6.7.
|
||||
* It consists of:
|
||||
* 1) content generated by CasADi runtime: not copyrighted
|
||||
* 2) template code copied from CasADi source: permissively licensed (MIT-0)
|
||||
* 3) user code: owned by the user
|
||||
*
|
||||
*/
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/* How to prefix internal symbols */
|
||||
#ifdef CASADI_CODEGEN_PREFIX
|
||||
#define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID)
|
||||
#define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID
|
||||
#define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID)
|
||||
#else
|
||||
#define CASADI_PREFIX(ID) long_cost_y_0_hess_ ## ID
|
||||
#endif
|
||||
|
||||
#include <math.h>
|
||||
|
||||
#ifndef casadi_real
|
||||
#define casadi_real double
|
||||
#endif
|
||||
|
||||
#ifndef casadi_int
|
||||
#define casadi_int int
|
||||
#endif
|
||||
|
||||
/* Add prefix to internal symbols */
|
||||
#define casadi_f0 CASADI_PREFIX(f0)
|
||||
#define casadi_s0 CASADI_PREFIX(s0)
|
||||
#define casadi_s1 CASADI_PREFIX(s1)
|
||||
#define casadi_s2 CASADI_PREFIX(s2)
|
||||
#define casadi_s3 CASADI_PREFIX(s3)
|
||||
#define casadi_s4 CASADI_PREFIX(s4)
|
||||
#define casadi_sq CASADI_PREFIX(sq)
|
||||
|
||||
/* Symbol visibility in DLLs */
|
||||
#ifndef CASADI_SYMBOL_EXPORT
|
||||
#if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__)
|
||||
#if defined(STATIC_LINKED)
|
||||
#define CASADI_SYMBOL_EXPORT
|
||||
#else
|
||||
#define CASADI_SYMBOL_EXPORT __declspec(dllexport)
|
||||
#endif
|
||||
#elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY)
|
||||
#define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default")))
|
||||
#else
|
||||
#define CASADI_SYMBOL_EXPORT
|
||||
#endif
|
||||
#endif
|
||||
|
||||
casadi_real casadi_sq(casadi_real x) { return x*x;}
|
||||
|
||||
static const casadi_int casadi_s0[7] = {3, 1, 0, 3, 0, 1, 2};
|
||||
static const casadi_int casadi_s1[5] = {1, 1, 0, 1, 0};
|
||||
static const casadi_int casadi_s2[3] = {0, 0, 0};
|
||||
static const casadi_int casadi_s3[10] = {6, 1, 0, 6, 0, 1, 2, 3, 4, 5};
|
||||
static const casadi_int casadi_s4[10] = {4, 4, 0, 0, 1, 3, 3, 2, 1, 2};
|
||||
|
||||
/* long_cost_y_0_hess:(i0[3],i1,i2[],i3[6],i4[6])->(o0[4x4,3nz]) */
|
||||
static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
|
||||
casadi_real a0, a1, a10, a2, a3, a4, a5, a6, a7, a8, a9;
|
||||
a0=arg[3]? arg[3][0] : 0;
|
||||
a1=arg[0]? arg[0][1] : 0;
|
||||
a2=10.;
|
||||
a2=(a1+a2);
|
||||
a3=(1./a2);
|
||||
a3=(a3/a2);
|
||||
a3=(a0*a3);
|
||||
if (res[0]!=0) res[0][0]=a3;
|
||||
a3=(a0/a2);
|
||||
a4=(a3/a2);
|
||||
if (res[0]!=0) res[0][1]=a4;
|
||||
a5=2.0000000000000001e-01;
|
||||
a6=(a1+a1);
|
||||
a6=(a5*a6);
|
||||
a7=arg[4]? arg[4][4] : 0;
|
||||
a6=(a6+a7);
|
||||
a6=(a6/a2);
|
||||
a8=arg[4]? arg[4][2] : 0;
|
||||
a9=arg[0]? arg[0][0] : 0;
|
||||
a8=(a8-a9);
|
||||
a9=casadi_sq(a1);
|
||||
a10=5.;
|
||||
a9=(a9/a10);
|
||||
a10=(a7*a1);
|
||||
a9=(a9+a10);
|
||||
a10=6.;
|
||||
a9=(a9+a10);
|
||||
a8=(a8-a9);
|
||||
a8=(a8/a2);
|
||||
a9=(a8/a2);
|
||||
a6=(a6+a9);
|
||||
a6=(a6/a2);
|
||||
a8=(a8/a2);
|
||||
a8=(a8/a2);
|
||||
a6=(a6+a8);
|
||||
a0=(a0*a6);
|
||||
a7=(a7*a4);
|
||||
a0=(a0+a7);
|
||||
a7=2.;
|
||||
a3=(a5*a3);
|
||||
a7=(a7*a3);
|
||||
a1=(a1+a1);
|
||||
a5=(a5*a4);
|
||||
a1=(a1*a5);
|
||||
a7=(a7-a1);
|
||||
a0=(a0-a7);
|
||||
if (res[0]!=0) res[0][2]=a0;
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int long_cost_y_0_hess(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){
|
||||
return casadi_f0(arg, res, iw, w, mem);
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int long_cost_y_0_hess_alloc_mem(void) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int long_cost_y_0_hess_init_mem(int mem) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void long_cost_y_0_hess_free_mem(int mem) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int long_cost_y_0_hess_checkout(void) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void long_cost_y_0_hess_release(int mem) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void long_cost_y_0_hess_incref(void) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void long_cost_y_0_hess_decref(void) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT casadi_int long_cost_y_0_hess_n_in(void) { return 5;}
|
||||
|
||||
CASADI_SYMBOL_EXPORT casadi_int long_cost_y_0_hess_n_out(void) { return 1;}
|
||||
|
||||
CASADI_SYMBOL_EXPORT casadi_real long_cost_y_0_hess_default_in(casadi_int i) {
|
||||
switch (i) {
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const char* long_cost_y_0_hess_name_in(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return "i0";
|
||||
case 1: return "i1";
|
||||
case 2: return "i2";
|
||||
case 3: return "i3";
|
||||
case 4: return "i4";
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const char* long_cost_y_0_hess_name_out(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return "o0";
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const casadi_int* long_cost_y_0_hess_sparsity_in(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return casadi_s0;
|
||||
case 1: return casadi_s1;
|
||||
case 2: return casadi_s2;
|
||||
case 3: return casadi_s3;
|
||||
case 4: return casadi_s3;
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const casadi_int* long_cost_y_0_hess_sparsity_out(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return casadi_s4;
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int long_cost_y_0_hess_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
|
||||
if (sz_arg) *sz_arg = 5;
|
||||
if (sz_res) *sz_res = 1;
|
||||
if (sz_iw) *sz_iw = 0;
|
||||
if (sz_w) *sz_w = 0;
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int long_cost_y_0_hess_work_bytes(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
|
||||
if (sz_arg) *sz_arg = 5*sizeof(const casadi_real*);
|
||||
if (sz_res) *sz_res = 1*sizeof(casadi_real*);
|
||||
if (sz_iw) *sz_iw = 0*sizeof(casadi_int);
|
||||
if (sz_w) *sz_w = 0*sizeof(casadi_real);
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
} /* extern "C" */
|
||||
#endif
|
||||
+182
@@ -0,0 +1,182 @@
|
||||
/* This file was automatically generated by CasADi 3.6.7.
|
||||
* It consists of:
|
||||
* 1) content generated by CasADi runtime: not copyrighted
|
||||
* 2) template code copied from CasADi source: permissively licensed (MIT-0)
|
||||
* 3) user code: owned by the user
|
||||
*
|
||||
*/
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/* How to prefix internal symbols */
|
||||
#ifdef CASADI_CODEGEN_PREFIX
|
||||
#define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID)
|
||||
#define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID
|
||||
#define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID)
|
||||
#else
|
||||
#define CASADI_PREFIX(ID) long_cost_y_e_fun_ ## ID
|
||||
#endif
|
||||
|
||||
#include <math.h>
|
||||
|
||||
#ifndef casadi_real
|
||||
#define casadi_real double
|
||||
#endif
|
||||
|
||||
#ifndef casadi_int
|
||||
#define casadi_int int
|
||||
#endif
|
||||
|
||||
/* Add prefix to internal symbols */
|
||||
#define casadi_f0 CASADI_PREFIX(f0)
|
||||
#define casadi_s0 CASADI_PREFIX(s0)
|
||||
#define casadi_s1 CASADI_PREFIX(s1)
|
||||
#define casadi_s2 CASADI_PREFIX(s2)
|
||||
#define casadi_s3 CASADI_PREFIX(s3)
|
||||
#define casadi_sq CASADI_PREFIX(sq)
|
||||
|
||||
/* Symbol visibility in DLLs */
|
||||
#ifndef CASADI_SYMBOL_EXPORT
|
||||
#if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__)
|
||||
#if defined(STATIC_LINKED)
|
||||
#define CASADI_SYMBOL_EXPORT
|
||||
#else
|
||||
#define CASADI_SYMBOL_EXPORT __declspec(dllexport)
|
||||
#endif
|
||||
#elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY)
|
||||
#define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default")))
|
||||
#else
|
||||
#define CASADI_SYMBOL_EXPORT
|
||||
#endif
|
||||
#endif
|
||||
|
||||
casadi_real casadi_sq(casadi_real x) { return x*x;}
|
||||
|
||||
static const casadi_int casadi_s0[7] = {3, 1, 0, 3, 0, 1, 2};
|
||||
static const casadi_int casadi_s1[3] = {0, 0, 0};
|
||||
static const casadi_int casadi_s2[10] = {6, 1, 0, 6, 0, 1, 2, 3, 4, 5};
|
||||
static const casadi_int casadi_s3[9] = {5, 1, 0, 5, 0, 1, 2, 3, 4};
|
||||
|
||||
/* long_cost_y_e_fun:(i0[3],i1[],i2[],i3[6])->(o0[5]) */
|
||||
static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
|
||||
casadi_real a0, a1, a2, a3, a4;
|
||||
a0=arg[3]? arg[3][2] : 0;
|
||||
a1=arg[0]? arg[0][0] : 0;
|
||||
a0=(a0-a1);
|
||||
a2=arg[0]? arg[0][1] : 0;
|
||||
a3=casadi_sq(a2);
|
||||
a4=5.;
|
||||
a3=(a3/a4);
|
||||
a4=arg[3]? arg[3][4] : 0;
|
||||
a4=(a4*a2);
|
||||
a3=(a3+a4);
|
||||
a4=6.;
|
||||
a3=(a3+a4);
|
||||
a0=(a0-a3);
|
||||
a3=10.;
|
||||
a3=(a2+a3);
|
||||
a0=(a0/a3);
|
||||
if (res[0]!=0) res[0][0]=a0;
|
||||
if (res[0]!=0) res[0][1]=a1;
|
||||
if (res[0]!=0) res[0][2]=a2;
|
||||
a2=arg[0]? arg[0][2] : 0;
|
||||
if (res[0]!=0) res[0][3]=a2;
|
||||
a1=arg[3]? arg[3][3] : 0;
|
||||
a2=(a2-a1);
|
||||
if (res[0]!=0) res[0][4]=a2;
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int long_cost_y_e_fun(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){
|
||||
return casadi_f0(arg, res, iw, w, mem);
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int long_cost_y_e_fun_alloc_mem(void) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int long_cost_y_e_fun_init_mem(int mem) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void long_cost_y_e_fun_free_mem(int mem) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int long_cost_y_e_fun_checkout(void) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void long_cost_y_e_fun_release(int mem) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void long_cost_y_e_fun_incref(void) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void long_cost_y_e_fun_decref(void) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT casadi_int long_cost_y_e_fun_n_in(void) { return 4;}
|
||||
|
||||
CASADI_SYMBOL_EXPORT casadi_int long_cost_y_e_fun_n_out(void) { return 1;}
|
||||
|
||||
CASADI_SYMBOL_EXPORT casadi_real long_cost_y_e_fun_default_in(casadi_int i) {
|
||||
switch (i) {
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const char* long_cost_y_e_fun_name_in(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return "i0";
|
||||
case 1: return "i1";
|
||||
case 2: return "i2";
|
||||
case 3: return "i3";
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const char* long_cost_y_e_fun_name_out(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return "o0";
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const casadi_int* long_cost_y_e_fun_sparsity_in(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return casadi_s0;
|
||||
case 1: return casadi_s1;
|
||||
case 2: return casadi_s1;
|
||||
case 3: return casadi_s2;
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const casadi_int* long_cost_y_e_fun_sparsity_out(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return casadi_s3;
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int long_cost_y_e_fun_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
|
||||
if (sz_arg) *sz_arg = 4;
|
||||
if (sz_res) *sz_res = 1;
|
||||
if (sz_iw) *sz_iw = 0;
|
||||
if (sz_w) *sz_w = 0;
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int long_cost_y_e_fun_work_bytes(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
|
||||
if (sz_arg) *sz_arg = 4*sizeof(const casadi_real*);
|
||||
if (sz_res) *sz_res = 1*sizeof(casadi_real*);
|
||||
if (sz_iw) *sz_iw = 0*sizeof(casadi_int);
|
||||
if (sz_w) *sz_w = 0*sizeof(casadi_real);
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
} /* extern "C" */
|
||||
#endif
|
||||
+207
@@ -0,0 +1,207 @@
|
||||
/* This file was automatically generated by CasADi 3.6.7.
|
||||
* It consists of:
|
||||
* 1) content generated by CasADi runtime: not copyrighted
|
||||
* 2) template code copied from CasADi source: permissively licensed (MIT-0)
|
||||
* 3) user code: owned by the user
|
||||
*
|
||||
*/
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/* How to prefix internal symbols */
|
||||
#ifdef CASADI_CODEGEN_PREFIX
|
||||
#define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID)
|
||||
#define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID
|
||||
#define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID)
|
||||
#else
|
||||
#define CASADI_PREFIX(ID) long_cost_y_e_fun_jac_ut_xt_ ## ID
|
||||
#endif
|
||||
|
||||
#include <math.h>
|
||||
|
||||
#ifndef casadi_real
|
||||
#define casadi_real double
|
||||
#endif
|
||||
|
||||
#ifndef casadi_int
|
||||
#define casadi_int int
|
||||
#endif
|
||||
|
||||
/* Add prefix to internal symbols */
|
||||
#define casadi_f0 CASADI_PREFIX(f0)
|
||||
#define casadi_s0 CASADI_PREFIX(s0)
|
||||
#define casadi_s1 CASADI_PREFIX(s1)
|
||||
#define casadi_s2 CASADI_PREFIX(s2)
|
||||
#define casadi_s3 CASADI_PREFIX(s3)
|
||||
#define casadi_s4 CASADI_PREFIX(s4)
|
||||
#define casadi_s5 CASADI_PREFIX(s5)
|
||||
#define casadi_sq CASADI_PREFIX(sq)
|
||||
|
||||
/* Symbol visibility in DLLs */
|
||||
#ifndef CASADI_SYMBOL_EXPORT
|
||||
#if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__)
|
||||
#if defined(STATIC_LINKED)
|
||||
#define CASADI_SYMBOL_EXPORT
|
||||
#else
|
||||
#define CASADI_SYMBOL_EXPORT __declspec(dllexport)
|
||||
#endif
|
||||
#elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY)
|
||||
#define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default")))
|
||||
#else
|
||||
#define CASADI_SYMBOL_EXPORT
|
||||
#endif
|
||||
#endif
|
||||
|
||||
casadi_real casadi_sq(casadi_real x) { return x*x;}
|
||||
|
||||
static const casadi_int casadi_s0[7] = {3, 1, 0, 3, 0, 1, 2};
|
||||
static const casadi_int casadi_s1[3] = {0, 0, 0};
|
||||
static const casadi_int casadi_s2[10] = {6, 1, 0, 6, 0, 1, 2, 3, 4, 5};
|
||||
static const casadi_int casadi_s3[9] = {5, 1, 0, 5, 0, 1, 2, 3, 4};
|
||||
static const casadi_int casadi_s4[14] = {3, 5, 0, 2, 3, 4, 5, 6, 0, 1, 0, 1, 2, 2};
|
||||
static const casadi_int casadi_s5[3] = {5, 0, 0};
|
||||
|
||||
/* long_cost_y_e_fun_jac_ut_xt:(i0[3],i1[],i2[],i3[6])->(o0[5],o1[3x5,6nz],o2[5x0]) */
|
||||
static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
|
||||
casadi_real a0, a1, a2, a3, a4, a5;
|
||||
a0=arg[3]? arg[3][2] : 0;
|
||||
a1=arg[0]? arg[0][0] : 0;
|
||||
a0=(a0-a1);
|
||||
a2=arg[0]? arg[0][1] : 0;
|
||||
a3=casadi_sq(a2);
|
||||
a4=5.;
|
||||
a3=(a3/a4);
|
||||
a4=arg[3]? arg[3][4] : 0;
|
||||
a5=(a4*a2);
|
||||
a3=(a3+a5);
|
||||
a5=6.;
|
||||
a3=(a3+a5);
|
||||
a0=(a0-a3);
|
||||
a3=10.;
|
||||
a3=(a2+a3);
|
||||
a0=(a0/a3);
|
||||
if (res[0]!=0) res[0][0]=a0;
|
||||
if (res[0]!=0) res[0][1]=a1;
|
||||
if (res[0]!=0) res[0][2]=a2;
|
||||
a1=arg[0]? arg[0][2] : 0;
|
||||
if (res[0]!=0) res[0][3]=a1;
|
||||
a5=arg[3]? arg[3][3] : 0;
|
||||
a1=(a1-a5);
|
||||
if (res[0]!=0) res[0][4]=a1;
|
||||
a1=(1./a3);
|
||||
a1=(-a1);
|
||||
if (res[1]!=0) res[1][0]=a1;
|
||||
a1=2.0000000000000001e-01;
|
||||
a2=(a2+a2);
|
||||
a1=(a1*a2);
|
||||
a1=(a1+a4);
|
||||
a1=(a1/a3);
|
||||
a0=(a0/a3);
|
||||
a1=(a1+a0);
|
||||
a1=(-a1);
|
||||
if (res[1]!=0) res[1][1]=a1;
|
||||
a1=1.;
|
||||
if (res[1]!=0) res[1][2]=a1;
|
||||
if (res[1]!=0) res[1][3]=a1;
|
||||
if (res[1]!=0) res[1][4]=a1;
|
||||
if (res[1]!=0) res[1][5]=a1;
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int long_cost_y_e_fun_jac_ut_xt(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){
|
||||
return casadi_f0(arg, res, iw, w, mem);
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int long_cost_y_e_fun_jac_ut_xt_alloc_mem(void) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int long_cost_y_e_fun_jac_ut_xt_init_mem(int mem) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void long_cost_y_e_fun_jac_ut_xt_free_mem(int mem) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int long_cost_y_e_fun_jac_ut_xt_checkout(void) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void long_cost_y_e_fun_jac_ut_xt_release(int mem) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void long_cost_y_e_fun_jac_ut_xt_incref(void) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void long_cost_y_e_fun_jac_ut_xt_decref(void) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT casadi_int long_cost_y_e_fun_jac_ut_xt_n_in(void) { return 4;}
|
||||
|
||||
CASADI_SYMBOL_EXPORT casadi_int long_cost_y_e_fun_jac_ut_xt_n_out(void) { return 3;}
|
||||
|
||||
CASADI_SYMBOL_EXPORT casadi_real long_cost_y_e_fun_jac_ut_xt_default_in(casadi_int i) {
|
||||
switch (i) {
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const char* long_cost_y_e_fun_jac_ut_xt_name_in(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return "i0";
|
||||
case 1: return "i1";
|
||||
case 2: return "i2";
|
||||
case 3: return "i3";
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const char* long_cost_y_e_fun_jac_ut_xt_name_out(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return "o0";
|
||||
case 1: return "o1";
|
||||
case 2: return "o2";
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const casadi_int* long_cost_y_e_fun_jac_ut_xt_sparsity_in(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return casadi_s0;
|
||||
case 1: return casadi_s1;
|
||||
case 2: return casadi_s1;
|
||||
case 3: return casadi_s2;
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const casadi_int* long_cost_y_e_fun_jac_ut_xt_sparsity_out(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return casadi_s3;
|
||||
case 1: return casadi_s4;
|
||||
case 2: return casadi_s5;
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int long_cost_y_e_fun_jac_ut_xt_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
|
||||
if (sz_arg) *sz_arg = 4;
|
||||
if (sz_res) *sz_res = 3;
|
||||
if (sz_iw) *sz_iw = 0;
|
||||
if (sz_w) *sz_w = 0;
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int long_cost_y_e_fun_jac_ut_xt_work_bytes(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
|
||||
if (sz_arg) *sz_arg = 4*sizeof(const casadi_real*);
|
||||
if (sz_res) *sz_res = 3*sizeof(casadi_real*);
|
||||
if (sz_iw) *sz_iw = 0*sizeof(casadi_int);
|
||||
if (sz_w) *sz_w = 0*sizeof(casadi_real);
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
} /* extern "C" */
|
||||
#endif
|
||||
+209
@@ -0,0 +1,209 @@
|
||||
/* This file was automatically generated by CasADi 3.6.7.
|
||||
* It consists of:
|
||||
* 1) content generated by CasADi runtime: not copyrighted
|
||||
* 2) template code copied from CasADi source: permissively licensed (MIT-0)
|
||||
* 3) user code: owned by the user
|
||||
*
|
||||
*/
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/* How to prefix internal symbols */
|
||||
#ifdef CASADI_CODEGEN_PREFIX
|
||||
#define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID)
|
||||
#define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID
|
||||
#define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID)
|
||||
#else
|
||||
#define CASADI_PREFIX(ID) long_cost_y_e_hess_ ## ID
|
||||
#endif
|
||||
|
||||
#include <math.h>
|
||||
|
||||
#ifndef casadi_real
|
||||
#define casadi_real double
|
||||
#endif
|
||||
|
||||
#ifndef casadi_int
|
||||
#define casadi_int int
|
||||
#endif
|
||||
|
||||
/* Add prefix to internal symbols */
|
||||
#define casadi_f0 CASADI_PREFIX(f0)
|
||||
#define casadi_s0 CASADI_PREFIX(s0)
|
||||
#define casadi_s1 CASADI_PREFIX(s1)
|
||||
#define casadi_s2 CASADI_PREFIX(s2)
|
||||
#define casadi_s3 CASADI_PREFIX(s3)
|
||||
#define casadi_s4 CASADI_PREFIX(s4)
|
||||
#define casadi_sq CASADI_PREFIX(sq)
|
||||
|
||||
/* Symbol visibility in DLLs */
|
||||
#ifndef CASADI_SYMBOL_EXPORT
|
||||
#if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__)
|
||||
#if defined(STATIC_LINKED)
|
||||
#define CASADI_SYMBOL_EXPORT
|
||||
#else
|
||||
#define CASADI_SYMBOL_EXPORT __declspec(dllexport)
|
||||
#endif
|
||||
#elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY)
|
||||
#define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default")))
|
||||
#else
|
||||
#define CASADI_SYMBOL_EXPORT
|
||||
#endif
|
||||
#endif
|
||||
|
||||
casadi_real casadi_sq(casadi_real x) { return x*x;}
|
||||
|
||||
static const casadi_int casadi_s0[7] = {3, 1, 0, 3, 0, 1, 2};
|
||||
static const casadi_int casadi_s1[3] = {0, 0, 0};
|
||||
static const casadi_int casadi_s2[9] = {5, 1, 0, 5, 0, 1, 2, 3, 4};
|
||||
static const casadi_int casadi_s3[10] = {6, 1, 0, 6, 0, 1, 2, 3, 4, 5};
|
||||
static const casadi_int casadi_s4[9] = {3, 3, 0, 1, 3, 3, 1, 0, 1};
|
||||
|
||||
/* long_cost_y_e_hess:(i0[3],i1[],i2[],i3[5],i4[6])->(o0[3x3,3nz]) */
|
||||
static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
|
||||
casadi_real a0, a1, a10, a2, a3, a4, a5, a6, a7, a8, a9;
|
||||
a0=arg[3]? arg[3][0] : 0;
|
||||
a1=arg[0]? arg[0][1] : 0;
|
||||
a2=10.;
|
||||
a2=(a1+a2);
|
||||
a3=(1./a2);
|
||||
a3=(a3/a2);
|
||||
a3=(a0*a3);
|
||||
if (res[0]!=0) res[0][0]=a3;
|
||||
a3=(a0/a2);
|
||||
a4=(a3/a2);
|
||||
if (res[0]!=0) res[0][1]=a4;
|
||||
a5=2.0000000000000001e-01;
|
||||
a6=(a1+a1);
|
||||
a6=(a5*a6);
|
||||
a7=arg[4]? arg[4][4] : 0;
|
||||
a6=(a6+a7);
|
||||
a6=(a6/a2);
|
||||
a8=arg[4]? arg[4][2] : 0;
|
||||
a9=arg[0]? arg[0][0] : 0;
|
||||
a8=(a8-a9);
|
||||
a9=casadi_sq(a1);
|
||||
a10=5.;
|
||||
a9=(a9/a10);
|
||||
a10=(a7*a1);
|
||||
a9=(a9+a10);
|
||||
a10=6.;
|
||||
a9=(a9+a10);
|
||||
a8=(a8-a9);
|
||||
a8=(a8/a2);
|
||||
a9=(a8/a2);
|
||||
a6=(a6+a9);
|
||||
a6=(a6/a2);
|
||||
a8=(a8/a2);
|
||||
a8=(a8/a2);
|
||||
a6=(a6+a8);
|
||||
a0=(a0*a6);
|
||||
a7=(a7*a4);
|
||||
a0=(a0+a7);
|
||||
a7=2.;
|
||||
a3=(a5*a3);
|
||||
a7=(a7*a3);
|
||||
a1=(a1+a1);
|
||||
a5=(a5*a4);
|
||||
a1=(a1*a5);
|
||||
a7=(a7-a1);
|
||||
a0=(a0-a7);
|
||||
if (res[0]!=0) res[0][2]=a0;
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int long_cost_y_e_hess(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){
|
||||
return casadi_f0(arg, res, iw, w, mem);
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int long_cost_y_e_hess_alloc_mem(void) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int long_cost_y_e_hess_init_mem(int mem) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void long_cost_y_e_hess_free_mem(int mem) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int long_cost_y_e_hess_checkout(void) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void long_cost_y_e_hess_release(int mem) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void long_cost_y_e_hess_incref(void) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void long_cost_y_e_hess_decref(void) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT casadi_int long_cost_y_e_hess_n_in(void) { return 5;}
|
||||
|
||||
CASADI_SYMBOL_EXPORT casadi_int long_cost_y_e_hess_n_out(void) { return 1;}
|
||||
|
||||
CASADI_SYMBOL_EXPORT casadi_real long_cost_y_e_hess_default_in(casadi_int i) {
|
||||
switch (i) {
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const char* long_cost_y_e_hess_name_in(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return "i0";
|
||||
case 1: return "i1";
|
||||
case 2: return "i2";
|
||||
case 3: return "i3";
|
||||
case 4: return "i4";
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const char* long_cost_y_e_hess_name_out(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return "o0";
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const casadi_int* long_cost_y_e_hess_sparsity_in(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return casadi_s0;
|
||||
case 1: return casadi_s1;
|
||||
case 2: return casadi_s1;
|
||||
case 3: return casadi_s2;
|
||||
case 4: return casadi_s3;
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const casadi_int* long_cost_y_e_hess_sparsity_out(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return casadi_s4;
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int long_cost_y_e_hess_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
|
||||
if (sz_arg) *sz_arg = 5;
|
||||
if (sz_res) *sz_res = 1;
|
||||
if (sz_iw) *sz_iw = 0;
|
||||
if (sz_w) *sz_w = 0;
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int long_cost_y_e_hess_work_bytes(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
|
||||
if (sz_arg) *sz_arg = 5*sizeof(const casadi_real*);
|
||||
if (sz_res) *sz_res = 1*sizeof(casadi_real*);
|
||||
if (sz_iw) *sz_iw = 0*sizeof(casadi_int);
|
||||
if (sz_w) *sz_w = 0*sizeof(casadi_real);
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
} /* extern "C" */
|
||||
#endif
|
||||
+184
@@ -0,0 +1,184 @@
|
||||
/* This file was automatically generated by CasADi 3.6.7.
|
||||
* It consists of:
|
||||
* 1) content generated by CasADi runtime: not copyrighted
|
||||
* 2) template code copied from CasADi source: permissively licensed (MIT-0)
|
||||
* 3) user code: owned by the user
|
||||
*
|
||||
*/
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/* How to prefix internal symbols */
|
||||
#ifdef CASADI_CODEGEN_PREFIX
|
||||
#define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID)
|
||||
#define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID
|
||||
#define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID)
|
||||
#else
|
||||
#define CASADI_PREFIX(ID) long_cost_y_fun_ ## ID
|
||||
#endif
|
||||
|
||||
#include <math.h>
|
||||
|
||||
#ifndef casadi_real
|
||||
#define casadi_real double
|
||||
#endif
|
||||
|
||||
#ifndef casadi_int
|
||||
#define casadi_int int
|
||||
#endif
|
||||
|
||||
/* Add prefix to internal symbols */
|
||||
#define casadi_f0 CASADI_PREFIX(f0)
|
||||
#define casadi_s0 CASADI_PREFIX(s0)
|
||||
#define casadi_s1 CASADI_PREFIX(s1)
|
||||
#define casadi_s2 CASADI_PREFIX(s2)
|
||||
#define casadi_s3 CASADI_PREFIX(s3)
|
||||
#define casadi_sq CASADI_PREFIX(sq)
|
||||
|
||||
/* Symbol visibility in DLLs */
|
||||
#ifndef CASADI_SYMBOL_EXPORT
|
||||
#if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__)
|
||||
#if defined(STATIC_LINKED)
|
||||
#define CASADI_SYMBOL_EXPORT
|
||||
#else
|
||||
#define CASADI_SYMBOL_EXPORT __declspec(dllexport)
|
||||
#endif
|
||||
#elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY)
|
||||
#define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default")))
|
||||
#else
|
||||
#define CASADI_SYMBOL_EXPORT
|
||||
#endif
|
||||
#endif
|
||||
|
||||
casadi_real casadi_sq(casadi_real x) { return x*x;}
|
||||
|
||||
static const casadi_int casadi_s0[7] = {3, 1, 0, 3, 0, 1, 2};
|
||||
static const casadi_int casadi_s1[5] = {1, 1, 0, 1, 0};
|
||||
static const casadi_int casadi_s2[3] = {0, 0, 0};
|
||||
static const casadi_int casadi_s3[10] = {6, 1, 0, 6, 0, 1, 2, 3, 4, 5};
|
||||
|
||||
/* long_cost_y_fun:(i0[3],i1,i2[],i3[6])->(o0[6]) */
|
||||
static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
|
||||
casadi_real a0, a1, a2, a3, a4;
|
||||
a0=arg[3]? arg[3][2] : 0;
|
||||
a1=arg[0]? arg[0][0] : 0;
|
||||
a0=(a0-a1);
|
||||
a2=arg[0]? arg[0][1] : 0;
|
||||
a3=casadi_sq(a2);
|
||||
a4=5.;
|
||||
a3=(a3/a4);
|
||||
a4=arg[3]? arg[3][4] : 0;
|
||||
a4=(a4*a2);
|
||||
a3=(a3+a4);
|
||||
a4=6.;
|
||||
a3=(a3+a4);
|
||||
a0=(a0-a3);
|
||||
a3=10.;
|
||||
a3=(a2+a3);
|
||||
a0=(a0/a3);
|
||||
if (res[0]!=0) res[0][0]=a0;
|
||||
if (res[0]!=0) res[0][1]=a1;
|
||||
if (res[0]!=0) res[0][2]=a2;
|
||||
a2=arg[0]? arg[0][2] : 0;
|
||||
if (res[0]!=0) res[0][3]=a2;
|
||||
a1=arg[3]? arg[3][3] : 0;
|
||||
a2=(a2-a1);
|
||||
if (res[0]!=0) res[0][4]=a2;
|
||||
a2=arg[1]? arg[1][0] : 0;
|
||||
if (res[0]!=0) res[0][5]=a2;
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int long_cost_y_fun(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){
|
||||
return casadi_f0(arg, res, iw, w, mem);
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int long_cost_y_fun_alloc_mem(void) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int long_cost_y_fun_init_mem(int mem) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void long_cost_y_fun_free_mem(int mem) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int long_cost_y_fun_checkout(void) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void long_cost_y_fun_release(int mem) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void long_cost_y_fun_incref(void) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void long_cost_y_fun_decref(void) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT casadi_int long_cost_y_fun_n_in(void) { return 4;}
|
||||
|
||||
CASADI_SYMBOL_EXPORT casadi_int long_cost_y_fun_n_out(void) { return 1;}
|
||||
|
||||
CASADI_SYMBOL_EXPORT casadi_real long_cost_y_fun_default_in(casadi_int i) {
|
||||
switch (i) {
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const char* long_cost_y_fun_name_in(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return "i0";
|
||||
case 1: return "i1";
|
||||
case 2: return "i2";
|
||||
case 3: return "i3";
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const char* long_cost_y_fun_name_out(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return "o0";
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const casadi_int* long_cost_y_fun_sparsity_in(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return casadi_s0;
|
||||
case 1: return casadi_s1;
|
||||
case 2: return casadi_s2;
|
||||
case 3: return casadi_s3;
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const casadi_int* long_cost_y_fun_sparsity_out(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return casadi_s3;
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int long_cost_y_fun_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
|
||||
if (sz_arg) *sz_arg = 4;
|
||||
if (sz_res) *sz_res = 1;
|
||||
if (sz_iw) *sz_iw = 0;
|
||||
if (sz_w) *sz_w = 0;
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int long_cost_y_fun_work_bytes(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
|
||||
if (sz_arg) *sz_arg = 4*sizeof(const casadi_real*);
|
||||
if (sz_res) *sz_res = 1*sizeof(casadi_real*);
|
||||
if (sz_iw) *sz_iw = 0*sizeof(casadi_int);
|
||||
if (sz_w) *sz_w = 0*sizeof(casadi_real);
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
} /* extern "C" */
|
||||
#endif
|
||||
+210
@@ -0,0 +1,210 @@
|
||||
/* This file was automatically generated by CasADi 3.6.7.
|
||||
* It consists of:
|
||||
* 1) content generated by CasADi runtime: not copyrighted
|
||||
* 2) template code copied from CasADi source: permissively licensed (MIT-0)
|
||||
* 3) user code: owned by the user
|
||||
*
|
||||
*/
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/* How to prefix internal symbols */
|
||||
#ifdef CASADI_CODEGEN_PREFIX
|
||||
#define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID)
|
||||
#define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID
|
||||
#define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID)
|
||||
#else
|
||||
#define CASADI_PREFIX(ID) long_cost_y_fun_jac_ut_xt_ ## ID
|
||||
#endif
|
||||
|
||||
#include <math.h>
|
||||
|
||||
#ifndef casadi_real
|
||||
#define casadi_real double
|
||||
#endif
|
||||
|
||||
#ifndef casadi_int
|
||||
#define casadi_int int
|
||||
#endif
|
||||
|
||||
/* Add prefix to internal symbols */
|
||||
#define casadi_f0 CASADI_PREFIX(f0)
|
||||
#define casadi_s0 CASADI_PREFIX(s0)
|
||||
#define casadi_s1 CASADI_PREFIX(s1)
|
||||
#define casadi_s2 CASADI_PREFIX(s2)
|
||||
#define casadi_s3 CASADI_PREFIX(s3)
|
||||
#define casadi_s4 CASADI_PREFIX(s4)
|
||||
#define casadi_s5 CASADI_PREFIX(s5)
|
||||
#define casadi_sq CASADI_PREFIX(sq)
|
||||
|
||||
/* Symbol visibility in DLLs */
|
||||
#ifndef CASADI_SYMBOL_EXPORT
|
||||
#if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__)
|
||||
#if defined(STATIC_LINKED)
|
||||
#define CASADI_SYMBOL_EXPORT
|
||||
#else
|
||||
#define CASADI_SYMBOL_EXPORT __declspec(dllexport)
|
||||
#endif
|
||||
#elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY)
|
||||
#define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default")))
|
||||
#else
|
||||
#define CASADI_SYMBOL_EXPORT
|
||||
#endif
|
||||
#endif
|
||||
|
||||
casadi_real casadi_sq(casadi_real x) { return x*x;}
|
||||
|
||||
static const casadi_int casadi_s0[7] = {3, 1, 0, 3, 0, 1, 2};
|
||||
static const casadi_int casadi_s1[5] = {1, 1, 0, 1, 0};
|
||||
static const casadi_int casadi_s2[3] = {0, 0, 0};
|
||||
static const casadi_int casadi_s3[10] = {6, 1, 0, 6, 0, 1, 2, 3, 4, 5};
|
||||
static const casadi_int casadi_s4[16] = {4, 6, 0, 2, 3, 4, 5, 6, 7, 1, 2, 1, 2, 3, 3, 0};
|
||||
static const casadi_int casadi_s5[3] = {6, 0, 0};
|
||||
|
||||
/* long_cost_y_fun_jac_ut_xt:(i0[3],i1,i2[],i3[6])->(o0[6],o1[4x6,7nz],o2[6x0]) */
|
||||
static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
|
||||
casadi_real a0, a1, a2, a3, a4, a5;
|
||||
a0=arg[3]? arg[3][2] : 0;
|
||||
a1=arg[0]? arg[0][0] : 0;
|
||||
a0=(a0-a1);
|
||||
a2=arg[0]? arg[0][1] : 0;
|
||||
a3=casadi_sq(a2);
|
||||
a4=5.;
|
||||
a3=(a3/a4);
|
||||
a4=arg[3]? arg[3][4] : 0;
|
||||
a5=(a4*a2);
|
||||
a3=(a3+a5);
|
||||
a5=6.;
|
||||
a3=(a3+a5);
|
||||
a0=(a0-a3);
|
||||
a3=10.;
|
||||
a3=(a2+a3);
|
||||
a0=(a0/a3);
|
||||
if (res[0]!=0) res[0][0]=a0;
|
||||
if (res[0]!=0) res[0][1]=a1;
|
||||
if (res[0]!=0) res[0][2]=a2;
|
||||
a1=arg[0]? arg[0][2] : 0;
|
||||
if (res[0]!=0) res[0][3]=a1;
|
||||
a5=arg[3]? arg[3][3] : 0;
|
||||
a1=(a1-a5);
|
||||
if (res[0]!=0) res[0][4]=a1;
|
||||
a1=arg[1]? arg[1][0] : 0;
|
||||
if (res[0]!=0) res[0][5]=a1;
|
||||
a1=(1./a3);
|
||||
a1=(-a1);
|
||||
if (res[1]!=0) res[1][0]=a1;
|
||||
a1=2.0000000000000001e-01;
|
||||
a2=(a2+a2);
|
||||
a1=(a1*a2);
|
||||
a1=(a1+a4);
|
||||
a1=(a1/a3);
|
||||
a0=(a0/a3);
|
||||
a1=(a1+a0);
|
||||
a1=(-a1);
|
||||
if (res[1]!=0) res[1][1]=a1;
|
||||
a1=1.;
|
||||
if (res[1]!=0) res[1][2]=a1;
|
||||
if (res[1]!=0) res[1][3]=a1;
|
||||
if (res[1]!=0) res[1][4]=a1;
|
||||
if (res[1]!=0) res[1][5]=a1;
|
||||
if (res[1]!=0) res[1][6]=a1;
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int long_cost_y_fun_jac_ut_xt(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){
|
||||
return casadi_f0(arg, res, iw, w, mem);
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int long_cost_y_fun_jac_ut_xt_alloc_mem(void) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int long_cost_y_fun_jac_ut_xt_init_mem(int mem) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void long_cost_y_fun_jac_ut_xt_free_mem(int mem) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int long_cost_y_fun_jac_ut_xt_checkout(void) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void long_cost_y_fun_jac_ut_xt_release(int mem) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void long_cost_y_fun_jac_ut_xt_incref(void) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void long_cost_y_fun_jac_ut_xt_decref(void) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT casadi_int long_cost_y_fun_jac_ut_xt_n_in(void) { return 4;}
|
||||
|
||||
CASADI_SYMBOL_EXPORT casadi_int long_cost_y_fun_jac_ut_xt_n_out(void) { return 3;}
|
||||
|
||||
CASADI_SYMBOL_EXPORT casadi_real long_cost_y_fun_jac_ut_xt_default_in(casadi_int i) {
|
||||
switch (i) {
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const char* long_cost_y_fun_jac_ut_xt_name_in(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return "i0";
|
||||
case 1: return "i1";
|
||||
case 2: return "i2";
|
||||
case 3: return "i3";
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const char* long_cost_y_fun_jac_ut_xt_name_out(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return "o0";
|
||||
case 1: return "o1";
|
||||
case 2: return "o2";
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const casadi_int* long_cost_y_fun_jac_ut_xt_sparsity_in(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return casadi_s0;
|
||||
case 1: return casadi_s1;
|
||||
case 2: return casadi_s2;
|
||||
case 3: return casadi_s3;
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const casadi_int* long_cost_y_fun_jac_ut_xt_sparsity_out(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return casadi_s3;
|
||||
case 1: return casadi_s4;
|
||||
case 2: return casadi_s5;
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int long_cost_y_fun_jac_ut_xt_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
|
||||
if (sz_arg) *sz_arg = 4;
|
||||
if (sz_res) *sz_res = 3;
|
||||
if (sz_iw) *sz_iw = 0;
|
||||
if (sz_w) *sz_w = 0;
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int long_cost_y_fun_jac_ut_xt_work_bytes(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
|
||||
if (sz_arg) *sz_arg = 4*sizeof(const casadi_real*);
|
||||
if (sz_res) *sz_res = 3*sizeof(casadi_real*);
|
||||
if (sz_iw) *sz_iw = 0*sizeof(casadi_int);
|
||||
if (sz_w) *sz_w = 0*sizeof(casadi_real);
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
} /* extern "C" */
|
||||
#endif
|
||||
+209
@@ -0,0 +1,209 @@
|
||||
/* This file was automatically generated by CasADi 3.6.7.
|
||||
* It consists of:
|
||||
* 1) content generated by CasADi runtime: not copyrighted
|
||||
* 2) template code copied from CasADi source: permissively licensed (MIT-0)
|
||||
* 3) user code: owned by the user
|
||||
*
|
||||
*/
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/* How to prefix internal symbols */
|
||||
#ifdef CASADI_CODEGEN_PREFIX
|
||||
#define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID)
|
||||
#define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID
|
||||
#define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID)
|
||||
#else
|
||||
#define CASADI_PREFIX(ID) long_cost_y_hess_ ## ID
|
||||
#endif
|
||||
|
||||
#include <math.h>
|
||||
|
||||
#ifndef casadi_real
|
||||
#define casadi_real double
|
||||
#endif
|
||||
|
||||
#ifndef casadi_int
|
||||
#define casadi_int int
|
||||
#endif
|
||||
|
||||
/* Add prefix to internal symbols */
|
||||
#define casadi_f0 CASADI_PREFIX(f0)
|
||||
#define casadi_s0 CASADI_PREFIX(s0)
|
||||
#define casadi_s1 CASADI_PREFIX(s1)
|
||||
#define casadi_s2 CASADI_PREFIX(s2)
|
||||
#define casadi_s3 CASADI_PREFIX(s3)
|
||||
#define casadi_s4 CASADI_PREFIX(s4)
|
||||
#define casadi_sq CASADI_PREFIX(sq)
|
||||
|
||||
/* Symbol visibility in DLLs */
|
||||
#ifndef CASADI_SYMBOL_EXPORT
|
||||
#if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__)
|
||||
#if defined(STATIC_LINKED)
|
||||
#define CASADI_SYMBOL_EXPORT
|
||||
#else
|
||||
#define CASADI_SYMBOL_EXPORT __declspec(dllexport)
|
||||
#endif
|
||||
#elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY)
|
||||
#define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default")))
|
||||
#else
|
||||
#define CASADI_SYMBOL_EXPORT
|
||||
#endif
|
||||
#endif
|
||||
|
||||
casadi_real casadi_sq(casadi_real x) { return x*x;}
|
||||
|
||||
static const casadi_int casadi_s0[7] = {3, 1, 0, 3, 0, 1, 2};
|
||||
static const casadi_int casadi_s1[5] = {1, 1, 0, 1, 0};
|
||||
static const casadi_int casadi_s2[3] = {0, 0, 0};
|
||||
static const casadi_int casadi_s3[10] = {6, 1, 0, 6, 0, 1, 2, 3, 4, 5};
|
||||
static const casadi_int casadi_s4[10] = {4, 4, 0, 0, 1, 3, 3, 2, 1, 2};
|
||||
|
||||
/* long_cost_y_hess:(i0[3],i1,i2[],i3[6],i4[6])->(o0[4x4,3nz]) */
|
||||
static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
|
||||
casadi_real a0, a1, a10, a2, a3, a4, a5, a6, a7, a8, a9;
|
||||
a0=arg[3]? arg[3][0] : 0;
|
||||
a1=arg[0]? arg[0][1] : 0;
|
||||
a2=10.;
|
||||
a2=(a1+a2);
|
||||
a3=(1./a2);
|
||||
a3=(a3/a2);
|
||||
a3=(a0*a3);
|
||||
if (res[0]!=0) res[0][0]=a3;
|
||||
a3=(a0/a2);
|
||||
a4=(a3/a2);
|
||||
if (res[0]!=0) res[0][1]=a4;
|
||||
a5=2.0000000000000001e-01;
|
||||
a6=(a1+a1);
|
||||
a6=(a5*a6);
|
||||
a7=arg[4]? arg[4][4] : 0;
|
||||
a6=(a6+a7);
|
||||
a6=(a6/a2);
|
||||
a8=arg[4]? arg[4][2] : 0;
|
||||
a9=arg[0]? arg[0][0] : 0;
|
||||
a8=(a8-a9);
|
||||
a9=casadi_sq(a1);
|
||||
a10=5.;
|
||||
a9=(a9/a10);
|
||||
a10=(a7*a1);
|
||||
a9=(a9+a10);
|
||||
a10=6.;
|
||||
a9=(a9+a10);
|
||||
a8=(a8-a9);
|
||||
a8=(a8/a2);
|
||||
a9=(a8/a2);
|
||||
a6=(a6+a9);
|
||||
a6=(a6/a2);
|
||||
a8=(a8/a2);
|
||||
a8=(a8/a2);
|
||||
a6=(a6+a8);
|
||||
a0=(a0*a6);
|
||||
a7=(a7*a4);
|
||||
a0=(a0+a7);
|
||||
a7=2.;
|
||||
a3=(a5*a3);
|
||||
a7=(a7*a3);
|
||||
a1=(a1+a1);
|
||||
a5=(a5*a4);
|
||||
a1=(a1*a5);
|
||||
a7=(a7-a1);
|
||||
a0=(a0-a7);
|
||||
if (res[0]!=0) res[0][2]=a0;
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int long_cost_y_hess(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){
|
||||
return casadi_f0(arg, res, iw, w, mem);
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int long_cost_y_hess_alloc_mem(void) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int long_cost_y_hess_init_mem(int mem) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void long_cost_y_hess_free_mem(int mem) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int long_cost_y_hess_checkout(void) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void long_cost_y_hess_release(int mem) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void long_cost_y_hess_incref(void) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void long_cost_y_hess_decref(void) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT casadi_int long_cost_y_hess_n_in(void) { return 5;}
|
||||
|
||||
CASADI_SYMBOL_EXPORT casadi_int long_cost_y_hess_n_out(void) { return 1;}
|
||||
|
||||
CASADI_SYMBOL_EXPORT casadi_real long_cost_y_hess_default_in(casadi_int i) {
|
||||
switch (i) {
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const char* long_cost_y_hess_name_in(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return "i0";
|
||||
case 1: return "i1";
|
||||
case 2: return "i2";
|
||||
case 3: return "i3";
|
||||
case 4: return "i4";
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const char* long_cost_y_hess_name_out(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return "o0";
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const casadi_int* long_cost_y_hess_sparsity_in(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return casadi_s0;
|
||||
case 1: return casadi_s1;
|
||||
case 2: return casadi_s2;
|
||||
case 3: return casadi_s3;
|
||||
case 4: return casadi_s3;
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const casadi_int* long_cost_y_hess_sparsity_out(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return casadi_s4;
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int long_cost_y_hess_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
|
||||
if (sz_arg) *sz_arg = 5;
|
||||
if (sz_res) *sz_res = 1;
|
||||
if (sz_iw) *sz_iw = 0;
|
||||
if (sz_w) *sz_w = 0;
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int long_cost_y_hess_work_bytes(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
|
||||
if (sz_arg) *sz_arg = 5*sizeof(const casadi_real*);
|
||||
if (sz_res) *sz_res = 1*sizeof(casadi_real*);
|
||||
if (sz_iw) *sz_iw = 0*sizeof(casadi_int);
|
||||
if (sz_w) *sz_w = 0*sizeof(casadi_real);
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
} /* extern "C" */
|
||||
#endif
|
||||
+157
@@ -0,0 +1,157 @@
|
||||
/* This file was automatically generated by CasADi 3.6.7.
|
||||
* It consists of:
|
||||
* 1) content generated by CasADi runtime: not copyrighted
|
||||
* 2) template code copied from CasADi source: permissively licensed (MIT-0)
|
||||
* 3) user code: owned by the user
|
||||
*
|
||||
*/
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/* How to prefix internal symbols */
|
||||
#ifdef CASADI_CODEGEN_PREFIX
|
||||
#define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID)
|
||||
#define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID
|
||||
#define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID)
|
||||
#else
|
||||
#define CASADI_PREFIX(ID) long_expl_ode_fun_ ## ID
|
||||
#endif
|
||||
|
||||
#include <math.h>
|
||||
|
||||
#ifndef casadi_real
|
||||
#define casadi_real double
|
||||
#endif
|
||||
|
||||
#ifndef casadi_int
|
||||
#define casadi_int int
|
||||
#endif
|
||||
|
||||
/* Add prefix to internal symbols */
|
||||
#define casadi_f0 CASADI_PREFIX(f0)
|
||||
#define casadi_s0 CASADI_PREFIX(s0)
|
||||
#define casadi_s1 CASADI_PREFIX(s1)
|
||||
#define casadi_s2 CASADI_PREFIX(s2)
|
||||
|
||||
/* Symbol visibility in DLLs */
|
||||
#ifndef CASADI_SYMBOL_EXPORT
|
||||
#if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__)
|
||||
#if defined(STATIC_LINKED)
|
||||
#define CASADI_SYMBOL_EXPORT
|
||||
#else
|
||||
#define CASADI_SYMBOL_EXPORT __declspec(dllexport)
|
||||
#endif
|
||||
#elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY)
|
||||
#define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default")))
|
||||
#else
|
||||
#define CASADI_SYMBOL_EXPORT
|
||||
#endif
|
||||
#endif
|
||||
|
||||
static const casadi_int casadi_s0[7] = {3, 1, 0, 3, 0, 1, 2};
|
||||
static const casadi_int casadi_s1[5] = {1, 1, 0, 1, 0};
|
||||
static const casadi_int casadi_s2[10] = {6, 1, 0, 6, 0, 1, 2, 3, 4, 5};
|
||||
|
||||
/* long_expl_ode_fun:(i0[3],i1,i2[6])->(o0[3]) */
|
||||
static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
|
||||
casadi_real a0;
|
||||
a0=arg[0]? arg[0][1] : 0;
|
||||
if (res[0]!=0) res[0][0]=a0;
|
||||
a0=arg[0]? arg[0][2] : 0;
|
||||
if (res[0]!=0) res[0][1]=a0;
|
||||
a0=arg[1]? arg[1][0] : 0;
|
||||
if (res[0]!=0) res[0][2]=a0;
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int long_expl_ode_fun(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){
|
||||
return casadi_f0(arg, res, iw, w, mem);
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int long_expl_ode_fun_alloc_mem(void) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int long_expl_ode_fun_init_mem(int mem) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void long_expl_ode_fun_free_mem(int mem) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int long_expl_ode_fun_checkout(void) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void long_expl_ode_fun_release(int mem) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void long_expl_ode_fun_incref(void) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void long_expl_ode_fun_decref(void) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT casadi_int long_expl_ode_fun_n_in(void) { return 3;}
|
||||
|
||||
CASADI_SYMBOL_EXPORT casadi_int long_expl_ode_fun_n_out(void) { return 1;}
|
||||
|
||||
CASADI_SYMBOL_EXPORT casadi_real long_expl_ode_fun_default_in(casadi_int i) {
|
||||
switch (i) {
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const char* long_expl_ode_fun_name_in(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return "i0";
|
||||
case 1: return "i1";
|
||||
case 2: return "i2";
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const char* long_expl_ode_fun_name_out(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return "o0";
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const casadi_int* long_expl_ode_fun_sparsity_in(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return casadi_s0;
|
||||
case 1: return casadi_s1;
|
||||
case 2: return casadi_s2;
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const casadi_int* long_expl_ode_fun_sparsity_out(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return casadi_s0;
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int long_expl_ode_fun_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
|
||||
if (sz_arg) *sz_arg = 3;
|
||||
if (sz_res) *sz_res = 1;
|
||||
if (sz_iw) *sz_iw = 0;
|
||||
if (sz_w) *sz_w = 0;
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int long_expl_ode_fun_work_bytes(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
|
||||
if (sz_arg) *sz_arg = 3*sizeof(const casadi_real*);
|
||||
if (sz_res) *sz_res = 1*sizeof(casadi_real*);
|
||||
if (sz_iw) *sz_iw = 0*sizeof(casadi_int);
|
||||
if (sz_w) *sz_w = 0*sizeof(casadi_real);
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
} /* extern "C" */
|
||||
#endif
|
||||
+163
@@ -0,0 +1,163 @@
|
||||
/* This file was automatically generated by CasADi 3.6.7.
|
||||
* It consists of:
|
||||
* 1) content generated by CasADi runtime: not copyrighted
|
||||
* 2) template code copied from CasADi source: permissively licensed (MIT-0)
|
||||
* 3) user code: owned by the user
|
||||
*
|
||||
*/
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/* How to prefix internal symbols */
|
||||
#ifdef CASADI_CODEGEN_PREFIX
|
||||
#define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID)
|
||||
#define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID
|
||||
#define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID)
|
||||
#else
|
||||
#define CASADI_PREFIX(ID) long_expl_vde_adj_ ## ID
|
||||
#endif
|
||||
|
||||
#include <math.h>
|
||||
|
||||
#ifndef casadi_real
|
||||
#define casadi_real double
|
||||
#endif
|
||||
|
||||
#ifndef casadi_int
|
||||
#define casadi_int int
|
||||
#endif
|
||||
|
||||
/* Add prefix to internal symbols */
|
||||
#define casadi_f0 CASADI_PREFIX(f0)
|
||||
#define casadi_s0 CASADI_PREFIX(s0)
|
||||
#define casadi_s1 CASADI_PREFIX(s1)
|
||||
#define casadi_s2 CASADI_PREFIX(s2)
|
||||
#define casadi_s3 CASADI_PREFIX(s3)
|
||||
|
||||
/* Symbol visibility in DLLs */
|
||||
#ifndef CASADI_SYMBOL_EXPORT
|
||||
#if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__)
|
||||
#if defined(STATIC_LINKED)
|
||||
#define CASADI_SYMBOL_EXPORT
|
||||
#else
|
||||
#define CASADI_SYMBOL_EXPORT __declspec(dllexport)
|
||||
#endif
|
||||
#elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY)
|
||||
#define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default")))
|
||||
#else
|
||||
#define CASADI_SYMBOL_EXPORT
|
||||
#endif
|
||||
#endif
|
||||
|
||||
static const casadi_int casadi_s0[7] = {3, 1, 0, 3, 0, 1, 2};
|
||||
static const casadi_int casadi_s1[5] = {1, 1, 0, 1, 0};
|
||||
static const casadi_int casadi_s2[10] = {6, 1, 0, 6, 0, 1, 2, 3, 4, 5};
|
||||
static const casadi_int casadi_s3[8] = {4, 1, 0, 4, 0, 1, 2, 3};
|
||||
|
||||
/* long_expl_vde_adj:(i0[3],i1[3],i2,i3[6])->(o0[4]) */
|
||||
static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
|
||||
casadi_real a0;
|
||||
a0=0.;
|
||||
if (res[0]!=0) res[0][0]=a0;
|
||||
a0=arg[1]? arg[1][0] : 0;
|
||||
if (res[0]!=0) res[0][1]=a0;
|
||||
a0=arg[1]? arg[1][1] : 0;
|
||||
if (res[0]!=0) res[0][2]=a0;
|
||||
a0=arg[1]? arg[1][2] : 0;
|
||||
if (res[0]!=0) res[0][3]=a0;
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int long_expl_vde_adj(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){
|
||||
return casadi_f0(arg, res, iw, w, mem);
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int long_expl_vde_adj_alloc_mem(void) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int long_expl_vde_adj_init_mem(int mem) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void long_expl_vde_adj_free_mem(int mem) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int long_expl_vde_adj_checkout(void) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void long_expl_vde_adj_release(int mem) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void long_expl_vde_adj_incref(void) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void long_expl_vde_adj_decref(void) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT casadi_int long_expl_vde_adj_n_in(void) { return 4;}
|
||||
|
||||
CASADI_SYMBOL_EXPORT casadi_int long_expl_vde_adj_n_out(void) { return 1;}
|
||||
|
||||
CASADI_SYMBOL_EXPORT casadi_real long_expl_vde_adj_default_in(casadi_int i) {
|
||||
switch (i) {
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const char* long_expl_vde_adj_name_in(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return "i0";
|
||||
case 1: return "i1";
|
||||
case 2: return "i2";
|
||||
case 3: return "i3";
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const char* long_expl_vde_adj_name_out(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return "o0";
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const casadi_int* long_expl_vde_adj_sparsity_in(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return casadi_s0;
|
||||
case 1: return casadi_s0;
|
||||
case 2: return casadi_s1;
|
||||
case 3: return casadi_s2;
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const casadi_int* long_expl_vde_adj_sparsity_out(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return casadi_s3;
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int long_expl_vde_adj_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
|
||||
if (sz_arg) *sz_arg = 4;
|
||||
if (sz_res) *sz_res = 1;
|
||||
if (sz_iw) *sz_iw = 0;
|
||||
if (sz_w) *sz_w = 0;
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int long_expl_vde_adj_work_bytes(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
|
||||
if (sz_arg) *sz_arg = 4*sizeof(const casadi_real*);
|
||||
if (sz_res) *sz_res = 1*sizeof(casadi_real*);
|
||||
if (sz_iw) *sz_iw = 0*sizeof(casadi_int);
|
||||
if (sz_w) *sz_w = 0*sizeof(casadi_real);
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
} /* extern "C" */
|
||||
#endif
|
||||
+189
@@ -0,0 +1,189 @@
|
||||
/* This file was automatically generated by CasADi 3.6.7.
|
||||
* It consists of:
|
||||
* 1) content generated by CasADi runtime: not copyrighted
|
||||
* 2) template code copied from CasADi source: permissively licensed (MIT-0)
|
||||
* 3) user code: owned by the user
|
||||
*
|
||||
*/
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/* How to prefix internal symbols */
|
||||
#ifdef CASADI_CODEGEN_PREFIX
|
||||
#define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID)
|
||||
#define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID
|
||||
#define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID)
|
||||
#else
|
||||
#define CASADI_PREFIX(ID) long_expl_vde_forw_ ## ID
|
||||
#endif
|
||||
|
||||
#include <math.h>
|
||||
|
||||
#ifndef casadi_real
|
||||
#define casadi_real double
|
||||
#endif
|
||||
|
||||
#ifndef casadi_int
|
||||
#define casadi_int int
|
||||
#endif
|
||||
|
||||
/* Add prefix to internal symbols */
|
||||
#define casadi_f0 CASADI_PREFIX(f0)
|
||||
#define casadi_s0 CASADI_PREFIX(s0)
|
||||
#define casadi_s1 CASADI_PREFIX(s1)
|
||||
#define casadi_s2 CASADI_PREFIX(s2)
|
||||
#define casadi_s3 CASADI_PREFIX(s3)
|
||||
|
||||
/* Symbol visibility in DLLs */
|
||||
#ifndef CASADI_SYMBOL_EXPORT
|
||||
#if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__)
|
||||
#if defined(STATIC_LINKED)
|
||||
#define CASADI_SYMBOL_EXPORT
|
||||
#else
|
||||
#define CASADI_SYMBOL_EXPORT __declspec(dllexport)
|
||||
#endif
|
||||
#elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY)
|
||||
#define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default")))
|
||||
#else
|
||||
#define CASADI_SYMBOL_EXPORT
|
||||
#endif
|
||||
#endif
|
||||
|
||||
static const casadi_int casadi_s0[7] = {3, 1, 0, 3, 0, 1, 2};
|
||||
static const casadi_int casadi_s1[15] = {3, 3, 0, 3, 6, 9, 0, 1, 2, 0, 1, 2, 0, 1, 2};
|
||||
static const casadi_int casadi_s2[5] = {1, 1, 0, 1, 0};
|
||||
static const casadi_int casadi_s3[10] = {6, 1, 0, 6, 0, 1, 2, 3, 4, 5};
|
||||
|
||||
/* long_expl_vde_forw:(i0[3],i1[3x3],i2[3],i3,i4[6])->(o0[3],o1[3x3],o2[3]) */
|
||||
static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
|
||||
casadi_real a0, a1;
|
||||
a0=arg[0]? arg[0][1] : 0;
|
||||
if (res[0]!=0) res[0][0]=a0;
|
||||
a0=arg[0]? arg[0][2] : 0;
|
||||
if (res[0]!=0) res[0][1]=a0;
|
||||
a0=arg[3]? arg[3][0] : 0;
|
||||
if (res[0]!=0) res[0][2]=a0;
|
||||
a0=arg[1]? arg[1][1] : 0;
|
||||
if (res[1]!=0) res[1][0]=a0;
|
||||
a0=arg[1]? arg[1][2] : 0;
|
||||
if (res[1]!=0) res[1][1]=a0;
|
||||
a0=0.;
|
||||
if (res[1]!=0) res[1][2]=a0;
|
||||
a1=arg[1]? arg[1][4] : 0;
|
||||
if (res[1]!=0) res[1][3]=a1;
|
||||
a1=arg[1]? arg[1][5] : 0;
|
||||
if (res[1]!=0) res[1][4]=a1;
|
||||
if (res[1]!=0) res[1][5]=a0;
|
||||
a1=arg[1]? arg[1][7] : 0;
|
||||
if (res[1]!=0) res[1][6]=a1;
|
||||
a1=arg[1]? arg[1][8] : 0;
|
||||
if (res[1]!=0) res[1][7]=a1;
|
||||
if (res[1]!=0) res[1][8]=a0;
|
||||
a0=arg[2]? arg[2][1] : 0;
|
||||
if (res[2]!=0) res[2][0]=a0;
|
||||
a0=arg[2]? arg[2][2] : 0;
|
||||
if (res[2]!=0) res[2][1]=a0;
|
||||
a0=1.;
|
||||
if (res[2]!=0) res[2][2]=a0;
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int long_expl_vde_forw(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){
|
||||
return casadi_f0(arg, res, iw, w, mem);
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int long_expl_vde_forw_alloc_mem(void) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int long_expl_vde_forw_init_mem(int mem) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void long_expl_vde_forw_free_mem(int mem) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int long_expl_vde_forw_checkout(void) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void long_expl_vde_forw_release(int mem) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void long_expl_vde_forw_incref(void) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void long_expl_vde_forw_decref(void) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT casadi_int long_expl_vde_forw_n_in(void) { return 5;}
|
||||
|
||||
CASADI_SYMBOL_EXPORT casadi_int long_expl_vde_forw_n_out(void) { return 3;}
|
||||
|
||||
CASADI_SYMBOL_EXPORT casadi_real long_expl_vde_forw_default_in(casadi_int i) {
|
||||
switch (i) {
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const char* long_expl_vde_forw_name_in(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return "i0";
|
||||
case 1: return "i1";
|
||||
case 2: return "i2";
|
||||
case 3: return "i3";
|
||||
case 4: return "i4";
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const char* long_expl_vde_forw_name_out(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return "o0";
|
||||
case 1: return "o1";
|
||||
case 2: return "o2";
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const casadi_int* long_expl_vde_forw_sparsity_in(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return casadi_s0;
|
||||
case 1: return casadi_s1;
|
||||
case 2: return casadi_s0;
|
||||
case 3: return casadi_s2;
|
||||
case 4: return casadi_s3;
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const casadi_int* long_expl_vde_forw_sparsity_out(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return casadi_s0;
|
||||
case 1: return casadi_s1;
|
||||
case 2: return casadi_s0;
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int long_expl_vde_forw_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
|
||||
if (sz_arg) *sz_arg = 5;
|
||||
if (sz_res) *sz_res = 3;
|
||||
if (sz_iw) *sz_iw = 0;
|
||||
if (sz_w) *sz_w = 0;
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int long_expl_vde_forw_work_bytes(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
|
||||
if (sz_arg) *sz_arg = 5*sizeof(const casadi_real*);
|
||||
if (sz_res) *sz_res = 3*sizeof(casadi_real*);
|
||||
if (sz_iw) *sz_iw = 0*sizeof(casadi_int);
|
||||
if (sz_w) *sz_w = 0*sizeof(casadi_real);
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
} /* extern "C" */
|
||||
#endif
|
||||
+71
@@ -0,0 +1,71 @@
|
||||
/*
|
||||
* Copyright (c) The acados authors.
|
||||
*
|
||||
* This file is part of acados.
|
||||
*
|
||||
* The 2-Clause BSD License
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice,
|
||||
* this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.;
|
||||
*/
|
||||
|
||||
#ifndef long_MODEL
|
||||
#define long_MODEL
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
|
||||
/* explicit ODE */
|
||||
|
||||
// explicit ODE
|
||||
int long_expl_ode_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
|
||||
int long_expl_ode_fun_work(int *, int *, int *, int *);
|
||||
const int *long_expl_ode_fun_sparsity_in(int);
|
||||
const int *long_expl_ode_fun_sparsity_out(int);
|
||||
int long_expl_ode_fun_n_in(void);
|
||||
int long_expl_ode_fun_n_out(void);
|
||||
|
||||
// explicit forward VDE
|
||||
int long_expl_vde_forw(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
|
||||
int long_expl_vde_forw_work(int *, int *, int *, int *);
|
||||
const int *long_expl_vde_forw_sparsity_in(int);
|
||||
const int *long_expl_vde_forw_sparsity_out(int);
|
||||
int long_expl_vde_forw_n_in(void);
|
||||
int long_expl_vde_forw_n_out(void);
|
||||
|
||||
// explicit adjoint VDE
|
||||
int long_expl_vde_adj(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
|
||||
int long_expl_vde_adj_work(int *, int *, int *, int *);
|
||||
const int *long_expl_vde_adj_sparsity_in(int);
|
||||
const int *long_expl_vde_adj_sparsity_out(int);
|
||||
int long_expl_vde_adj_n_in(void);
|
||||
int long_expl_vde_adj_n_out(void);
|
||||
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
} /* extern "C" */
|
||||
#endif
|
||||
|
||||
#endif // long_MODEL
|
||||
@@ -0,0 +1,216 @@
|
||||
/*
|
||||
* Copyright (c) The acados authors.
|
||||
*
|
||||
* This file is part of acados.
|
||||
*
|
||||
* The 2-Clause BSD License
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice,
|
||||
* this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.;
|
||||
*/
|
||||
|
||||
|
||||
// standard
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
// acados
|
||||
#include "acados/utils/print.h"
|
||||
#include "acados/utils/math.h"
|
||||
#include "acados_c/ocp_nlp_interface.h"
|
||||
#include "acados_c/external_function_interface.h"
|
||||
#include "acados_solver_long.h"
|
||||
|
||||
// blasfeo
|
||||
#include "blasfeo/include/blasfeo_d_aux_ext_dep.h"
|
||||
|
||||
#define NX LONG_NX
|
||||
#define NZ LONG_NZ
|
||||
#define NU LONG_NU
|
||||
#define NP LONG_NP
|
||||
#define NBX LONG_NBX
|
||||
#define NBX0 LONG_NBX0
|
||||
#define NBU LONG_NBU
|
||||
#define NSBX LONG_NSBX
|
||||
#define NSBU LONG_NSBU
|
||||
#define NSH LONG_NSH
|
||||
#define NSG LONG_NSG
|
||||
#define NSPHI LONG_NSPHI
|
||||
#define NSHN LONG_NSHN
|
||||
#define NSGN LONG_NSGN
|
||||
#define NSPHIN LONG_NSPHIN
|
||||
#define NSBXN LONG_NSBXN
|
||||
#define NS LONG_NS
|
||||
#define NSN LONG_NSN
|
||||
#define NG LONG_NG
|
||||
#define NBXN LONG_NBXN
|
||||
#define NGN LONG_NGN
|
||||
#define NY0 LONG_NY0
|
||||
#define NY LONG_NY
|
||||
#define NYN LONG_NYN
|
||||
#define NH LONG_NH
|
||||
#define NPHI LONG_NPHI
|
||||
#define NHN LONG_NHN
|
||||
#define NPHIN LONG_NPHIN
|
||||
#define NR LONG_NR
|
||||
|
||||
|
||||
int main()
|
||||
{
|
||||
|
||||
long_solver_capsule *acados_ocp_capsule = long_acados_create_capsule();
|
||||
// there is an opportunity to change the number of shooting intervals in C without new code generation
|
||||
int N = LONG_N;
|
||||
// allocate the array and fill it accordingly
|
||||
double* new_time_steps = NULL;
|
||||
int status = long_acados_create_with_discretization(acados_ocp_capsule, N, new_time_steps);
|
||||
|
||||
if (status)
|
||||
{
|
||||
printf("long_acados_create() returned status %d. Exiting.\n", status);
|
||||
exit(1);
|
||||
}
|
||||
|
||||
ocp_nlp_config *nlp_config = long_acados_get_nlp_config(acados_ocp_capsule);
|
||||
ocp_nlp_dims *nlp_dims = long_acados_get_nlp_dims(acados_ocp_capsule);
|
||||
ocp_nlp_in *nlp_in = long_acados_get_nlp_in(acados_ocp_capsule);
|
||||
ocp_nlp_out *nlp_out = long_acados_get_nlp_out(acados_ocp_capsule);
|
||||
ocp_nlp_solver *nlp_solver = long_acados_get_nlp_solver(acados_ocp_capsule);
|
||||
void *nlp_opts = long_acados_get_nlp_opts(acados_ocp_capsule);
|
||||
|
||||
// initial condition
|
||||
int idxbx0[NBX0];
|
||||
idxbx0[0] = 0;
|
||||
idxbx0[1] = 1;
|
||||
idxbx0[2] = 2;
|
||||
|
||||
double lbx0[NBX0];
|
||||
double ubx0[NBX0];
|
||||
lbx0[0] = 0.0;
|
||||
ubx0[0] = 0.0;
|
||||
lbx0[1] = 0.0;
|
||||
ubx0[1] = 0.0;
|
||||
lbx0[2] = 0.0;
|
||||
ubx0[2] = 0.0;
|
||||
|
||||
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "idxbx", idxbx0);
|
||||
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "lbx", lbx0);
|
||||
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "ubx", ubx0);
|
||||
|
||||
// initialization for state values
|
||||
double x_init[NX];
|
||||
x_init[0] = 0.0;
|
||||
x_init[1] = 0.0;
|
||||
x_init[2] = 0.0;
|
||||
|
||||
// initial value for control input
|
||||
double u0[NU];
|
||||
u0[0] = 0.0;
|
||||
// set parameters
|
||||
double p[NP];
|
||||
p[0] = -1.2;
|
||||
p[1] = 1.2;
|
||||
p[2] = 0.0;
|
||||
p[3] = 0.0;
|
||||
p[4] = 1.45;
|
||||
p[5] = 0.75;
|
||||
|
||||
for (int ii = 0; ii <= N; ii++)
|
||||
{
|
||||
long_acados_update_params(acados_ocp_capsule, ii, p, NP);
|
||||
}
|
||||
|
||||
|
||||
// prepare evaluation
|
||||
int NTIMINGS = 1;
|
||||
double min_time = 1e12;
|
||||
double kkt_norm_inf;
|
||||
double elapsed_time;
|
||||
int sqp_iter;
|
||||
|
||||
double xtraj[NX * (N+1)];
|
||||
double utraj[NU * N];
|
||||
|
||||
|
||||
// solve ocp in loop
|
||||
int rti_phase = 0;
|
||||
|
||||
for (int ii = 0; ii < NTIMINGS; ii++)
|
||||
{
|
||||
// initialize solution
|
||||
for (int i = 0; i < N; i++)
|
||||
{
|
||||
ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, i, "x", x_init);
|
||||
ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, i, "u", u0);
|
||||
}
|
||||
ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, N, "x", x_init);
|
||||
ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "rti_phase", &rti_phase);
|
||||
status = long_acados_solve(acados_ocp_capsule);
|
||||
ocp_nlp_get(nlp_config, nlp_solver, "time_tot", &elapsed_time);
|
||||
min_time = MIN(elapsed_time, min_time);
|
||||
}
|
||||
|
||||
/* print solution and statistics */
|
||||
for (int ii = 0; ii <= nlp_dims->N; ii++)
|
||||
ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, ii, "x", &xtraj[ii*NX]);
|
||||
for (int ii = 0; ii < nlp_dims->N; ii++)
|
||||
ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, ii, "u", &utraj[ii*NU]);
|
||||
|
||||
printf("\n--- xtraj ---\n");
|
||||
d_print_exp_tran_mat( NX, N+1, xtraj, NX);
|
||||
printf("\n--- utraj ---\n");
|
||||
d_print_exp_tran_mat( NU, N, utraj, NU );
|
||||
// ocp_nlp_out_print(nlp_solver->dims, nlp_out);
|
||||
|
||||
printf("\nsolved ocp %d times, solution printed above\n\n", NTIMINGS);
|
||||
|
||||
if (status == ACADOS_SUCCESS)
|
||||
{
|
||||
printf("long_acados_solve(): SUCCESS!\n");
|
||||
}
|
||||
else
|
||||
{
|
||||
printf("long_acados_solve() failed with status %d.\n", status);
|
||||
}
|
||||
|
||||
// get solution
|
||||
ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, 0, "kkt_norm_inf", &kkt_norm_inf);
|
||||
ocp_nlp_get(nlp_config, nlp_solver, "sqp_iter", &sqp_iter);
|
||||
|
||||
long_acados_print_stats(acados_ocp_capsule);
|
||||
|
||||
printf("\nSolver info:\n");
|
||||
printf(" SQP iterations %2d\n minimum time for %d solve %f [ms]\n KKT %e\n",
|
||||
sqp_iter, NTIMINGS, min_time*1000, kkt_norm_inf);
|
||||
|
||||
// free solver
|
||||
status = long_acados_free(acados_ocp_capsule);
|
||||
if (status) {
|
||||
printf("long_acados_free() returned status %d. \n", status);
|
||||
}
|
||||
// free solver capsule
|
||||
status = long_acados_free_capsule(acados_ocp_capsule);
|
||||
if (status) {
|
||||
printf("long_acados_free_capsule() returned status %d. \n", status);
|
||||
}
|
||||
|
||||
return status;
|
||||
}
|
||||
+127
@@ -0,0 +1,127 @@
|
||||
/*
|
||||
* Copyright (c) The acados authors.
|
||||
*
|
||||
* This file is part of acados.
|
||||
*
|
||||
* The 2-Clause BSD License
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice,
|
||||
* this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.;
|
||||
*/
|
||||
|
||||
|
||||
// standard
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
// acados
|
||||
#include "acados/utils/print.h"
|
||||
#include "acados/utils/math.h"
|
||||
#include "acados_c/sim_interface.h"
|
||||
#include "acados_sim_solver_long.h"
|
||||
|
||||
#define NX LONG_NX
|
||||
#define NZ LONG_NZ
|
||||
#define NU LONG_NU
|
||||
#define NP LONG_NP
|
||||
|
||||
|
||||
int main()
|
||||
{
|
||||
int status = 0;
|
||||
sim_solver_capsule *capsule = long_acados_sim_solver_create_capsule();
|
||||
status = long_acados_sim_create(capsule);
|
||||
|
||||
if (status)
|
||||
{
|
||||
printf("acados_create() returned status %d. Exiting.\n", status);
|
||||
exit(1);
|
||||
}
|
||||
|
||||
sim_config *acados_sim_config = long_acados_get_sim_config(capsule);
|
||||
sim_in *acados_sim_in = long_acados_get_sim_in(capsule);
|
||||
sim_out *acados_sim_out = long_acados_get_sim_out(capsule);
|
||||
void *acados_sim_dims = long_acados_get_sim_dims(capsule);
|
||||
|
||||
// initial condition
|
||||
double x_current[NX];
|
||||
x_current[0] = 0.0;
|
||||
x_current[1] = 0.0;
|
||||
x_current[2] = 0.0;
|
||||
|
||||
|
||||
x_current[0] = 0.0;
|
||||
x_current[1] = 0.0;
|
||||
x_current[2] = 0.0;
|
||||
|
||||
|
||||
|
||||
|
||||
// initial value for control input
|
||||
double u0[NU];
|
||||
u0[0] = 0.0;
|
||||
// set parameters
|
||||
double p[NP];
|
||||
p[0] = -1.2;
|
||||
p[1] = 1.2;
|
||||
p[2] = 0.0;
|
||||
p[3] = 0.0;
|
||||
p[4] = 1.45;
|
||||
p[5] = 0.75;
|
||||
|
||||
long_acados_sim_update_params(capsule, p, NP);
|
||||
|
||||
|
||||
int n_sim_steps = 3;
|
||||
// solve ocp in loop
|
||||
for (int ii = 0; ii < n_sim_steps; ii++)
|
||||
{
|
||||
sim_in_set(acados_sim_config, acados_sim_dims,
|
||||
acados_sim_in, "x", x_current);
|
||||
status = long_acados_sim_solve(capsule);
|
||||
|
||||
if (status != ACADOS_SUCCESS)
|
||||
{
|
||||
printf("acados_solve() failed with status %d.\n", status);
|
||||
}
|
||||
|
||||
sim_out_get(acados_sim_config, acados_sim_dims,
|
||||
acados_sim_out, "x", x_current);
|
||||
|
||||
printf("\nx_current, %d\n", ii);
|
||||
for (int jj = 0; jj < NX; jj++)
|
||||
{
|
||||
printf("%e\n", x_current[jj]);
|
||||
}
|
||||
}
|
||||
|
||||
printf("\nPerformed %d simulation steps with acados integrator successfully.\n\n", n_sim_steps);
|
||||
|
||||
// free solver
|
||||
status = long_acados_sim_free(capsule);
|
||||
if (status) {
|
||||
printf("long_acados_sim_free() returned status %d. \n", status);
|
||||
}
|
||||
|
||||
long_acados_sim_solver_free_capsule(capsule);
|
||||
|
||||
return status;
|
||||
}
|
||||
@@ -0,0 +1,385 @@
|
||||
#!/usr/bin/env python3
|
||||
import os
|
||||
import time
|
||||
import numpy as np
|
||||
from openpilot.cereal import log
|
||||
from opendbc.car.interfaces import ACCEL_MIN, ACCEL_MAX
|
||||
from openpilot.common.realtime import DT_MDL
|
||||
from openpilot.common.swaglog import cloudlog
|
||||
# WARNING: imports outside of constants will not trigger a rebuild
|
||||
from openpilot.selfdrive.modeld.constants import index_function
|
||||
from openpilot.selfdrive.controls.radard import _LEAD_ACCEL_TAU
|
||||
|
||||
if __name__ == '__main__': # generating code
|
||||
from acados.acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver
|
||||
else:
|
||||
from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.c_generated_code.acados_ocp_solver_pyx import AcadosOcpSolverCython
|
||||
|
||||
from casadi import SX, vertcat
|
||||
|
||||
MODEL_NAME = 'long'
|
||||
LONG_MPC_DIR = os.path.dirname(os.path.abspath(__file__))
|
||||
EXPORT_DIR = os.path.join(LONG_MPC_DIR, "c_generated_code")
|
||||
JSON_FILE = os.path.join(LONG_MPC_DIR, "acados_ocp_long.json")
|
||||
|
||||
LongitudinalPlanSource = log.LongitudinalPlan.LongitudinalPlanSource
|
||||
MPC_SOURCES = (LongitudinalPlanSource.lead0, LongitudinalPlanSource.lead1, LongitudinalPlanSource.cruise)
|
||||
|
||||
X_DIM = 3
|
||||
U_DIM = 1
|
||||
PARAM_DIM = 6
|
||||
COST_E_DIM = 5
|
||||
COST_DIM = COST_E_DIM + 1
|
||||
CONSTR_DIM = 4
|
||||
|
||||
X_EGO_OBSTACLE_COST = 3.
|
||||
X_EGO_COST = 0.
|
||||
V_EGO_COST = 0.
|
||||
A_EGO_COST = 0.
|
||||
J_EGO_COST = 5.
|
||||
A_CHANGE_COST = 200.
|
||||
DANGER_ZONE_COST = 100.
|
||||
CRASH_DISTANCE = .25
|
||||
LEAD_DANGER_FACTOR = 0.75
|
||||
LIMIT_COST = 1e6
|
||||
ACADOS_SOLVER_TYPE = 'SQP_RTI'
|
||||
|
||||
# Fewer timestamps don't hurt performance and lead to
|
||||
# much better convergence of the MPC with low iterations
|
||||
N = 12
|
||||
MAX_T = 10.0
|
||||
T_IDXS_LST = [index_function(idx, max_val=MAX_T, max_idx=N) for idx in range(N+1)]
|
||||
|
||||
T_IDXS = np.array(T_IDXS_LST)
|
||||
FCW_IDXS = T_IDXS < 5.0
|
||||
T_DIFFS = np.diff(T_IDXS, prepend=[0.])
|
||||
COMFORT_BRAKE = 2.5
|
||||
STOP_DISTANCE = 6.0
|
||||
CRUISE_MIN_ACCEL = -1.2
|
||||
CRUISE_MAX_ACCEL = 1.6
|
||||
MIN_X_LEAD_FACTOR = 0.5
|
||||
|
||||
def get_jerk_factor(personality=log.LongitudinalPersonality.standard):
|
||||
if personality==log.LongitudinalPersonality.relaxed:
|
||||
return 1.0
|
||||
elif personality==log.LongitudinalPersonality.standard:
|
||||
return 1.0
|
||||
elif personality==log.LongitudinalPersonality.aggressive:
|
||||
return 0.5
|
||||
else:
|
||||
raise NotImplementedError("Longitudinal personality not supported")
|
||||
|
||||
|
||||
def get_T_FOLLOW(personality=log.LongitudinalPersonality.standard):
|
||||
if personality==log.LongitudinalPersonality.relaxed:
|
||||
return 1.75
|
||||
elif personality==log.LongitudinalPersonality.standard:
|
||||
return 1.45
|
||||
elif personality==log.LongitudinalPersonality.aggressive:
|
||||
return 1.25
|
||||
else:
|
||||
raise NotImplementedError("Longitudinal personality not supported")
|
||||
|
||||
def get_stopped_equivalence_factor(v_lead):
|
||||
return (v_lead**2) / (2 * COMFORT_BRAKE)
|
||||
|
||||
def get_safe_obstacle_distance(v_ego, t_follow):
|
||||
return (v_ego**2) / (2 * COMFORT_BRAKE) + t_follow * v_ego + STOP_DISTANCE
|
||||
|
||||
def gen_long_model():
|
||||
model = AcadosModel()
|
||||
model.name = MODEL_NAME
|
||||
|
||||
# states
|
||||
x_ego, v_ego, a_ego = SX.sym('x_ego'), SX.sym('v_ego'), SX.sym('a_ego')
|
||||
model.x = vertcat(x_ego, v_ego, a_ego)
|
||||
|
||||
# controls
|
||||
j_ego = SX.sym('j_ego')
|
||||
model.u = vertcat(j_ego)
|
||||
|
||||
# xdot
|
||||
x_ego_dot = SX.sym('x_ego_dot')
|
||||
v_ego_dot = SX.sym('v_ego_dot')
|
||||
a_ego_dot = SX.sym('a_ego_dot')
|
||||
model.xdot = vertcat(x_ego_dot, v_ego_dot, a_ego_dot)
|
||||
|
||||
# live parameters
|
||||
a_min = SX.sym('a_min')
|
||||
a_max = SX.sym('a_max')
|
||||
x_obstacle = SX.sym('x_obstacle')
|
||||
a_prev = SX.sym('a_prev')
|
||||
lead_t_follow = SX.sym('lead_t_follow')
|
||||
lead_danger_factor = SX.sym('lead_danger_factor')
|
||||
model.p = vertcat(a_min, a_max, x_obstacle, a_prev, lead_t_follow, lead_danger_factor)
|
||||
|
||||
# dynamics model
|
||||
f_expl = vertcat(v_ego, a_ego, j_ego)
|
||||
model.f_impl_expr = model.xdot - f_expl
|
||||
model.f_expl_expr = f_expl
|
||||
return model
|
||||
|
||||
def gen_long_ocp():
|
||||
ocp = AcadosOcp()
|
||||
ocp.model = gen_long_model()
|
||||
|
||||
Tf = T_IDXS[-1]
|
||||
|
||||
# set dimensions
|
||||
ocp.dims.N = N
|
||||
|
||||
# set cost module
|
||||
ocp.cost.cost_type = 'NONLINEAR_LS'
|
||||
ocp.cost.cost_type_e = 'NONLINEAR_LS'
|
||||
|
||||
QR = np.zeros((COST_DIM, COST_DIM))
|
||||
Q = np.zeros((COST_E_DIM, COST_E_DIM))
|
||||
|
||||
ocp.cost.W = QR
|
||||
ocp.cost.W_e = Q
|
||||
|
||||
x_ego, v_ego, a_ego = ocp.model.x[0], ocp.model.x[1], ocp.model.x[2]
|
||||
j_ego = ocp.model.u[0]
|
||||
|
||||
a_min, a_max = ocp.model.p[0], ocp.model.p[1]
|
||||
x_obstacle = ocp.model.p[2]
|
||||
a_prev = ocp.model.p[3]
|
||||
lead_t_follow = ocp.model.p[4]
|
||||
lead_danger_factor = ocp.model.p[5]
|
||||
|
||||
ocp.cost.yref = np.zeros((COST_DIM, ))
|
||||
ocp.cost.yref_e = np.zeros((COST_E_DIM, ))
|
||||
|
||||
desired_dist_comfort = get_safe_obstacle_distance(v_ego, lead_t_follow)
|
||||
|
||||
# The main cost in normal operation is how close you are to the "desired" distance
|
||||
# from an obstacle at every timestep. This obstacle can be a lead car
|
||||
# or other object. In e2e mode we can use x_position targets as a cost
|
||||
# instead.
|
||||
costs = [((x_obstacle - x_ego) - (desired_dist_comfort)) / (v_ego + 10.),
|
||||
x_ego,
|
||||
v_ego,
|
||||
a_ego,
|
||||
a_ego - a_prev,
|
||||
j_ego]
|
||||
ocp.model.cost_y_expr = vertcat(*costs)
|
||||
ocp.model.cost_y_expr_e = vertcat(*costs[:-1])
|
||||
|
||||
# Constraints on speed, acceleration and desired distance to
|
||||
# the obstacle, which is treated as a slack constraint so it
|
||||
# behaves like an asymmetrical cost.
|
||||
constraints = vertcat(v_ego,
|
||||
(a_ego - a_min),
|
||||
(a_max - a_ego),
|
||||
((x_obstacle - x_ego) - lead_danger_factor * (desired_dist_comfort)) / (v_ego + 10.))
|
||||
ocp.model.con_h_expr = constraints
|
||||
|
||||
x0 = np.zeros(X_DIM)
|
||||
ocp.constraints.x0 = x0
|
||||
ocp.parameter_values = np.array([-1.2, 1.2, 0.0, 0.0, get_T_FOLLOW(), LEAD_DANGER_FACTOR])
|
||||
|
||||
|
||||
# We put all constraint cost weights to 0 and only set them at runtime
|
||||
cost_weights = np.zeros(CONSTR_DIM)
|
||||
ocp.cost.zl = cost_weights
|
||||
ocp.cost.Zl = cost_weights
|
||||
ocp.cost.Zu = cost_weights
|
||||
ocp.cost.zu = cost_weights
|
||||
|
||||
ocp.constraints.lh = np.zeros(CONSTR_DIM)
|
||||
ocp.constraints.uh = 1e4*np.ones(CONSTR_DIM)
|
||||
ocp.constraints.idxsh = np.arange(CONSTR_DIM)
|
||||
|
||||
# The HPIPM solver can give decent solutions even when it is stopped early
|
||||
# Which is critical for our purpose where compute time is strictly bounded
|
||||
# We use HPIPM in the SPEED_ABS mode, which ensures fastest runtime. This
|
||||
# does not cause issues since the problem is well bounded.
|
||||
ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM'
|
||||
ocp.solver_options.hessian_approx = 'GAUSS_NEWTON'
|
||||
ocp.solver_options.integrator_type = 'ERK'
|
||||
ocp.solver_options.nlp_solver_type = ACADOS_SOLVER_TYPE
|
||||
ocp.solver_options.qp_solver_cond_N = 1
|
||||
|
||||
# More iterations take too much time and less lead to inaccurate convergence in
|
||||
# some situations. Ideally we would run just 1 iteration to ensure fixed runtime.
|
||||
ocp.solver_options.qp_solver_iter_max = 10
|
||||
ocp.solver_options.qp_tol = 1e-3
|
||||
|
||||
# set prediction horizon
|
||||
ocp.solver_options.tf = Tf
|
||||
ocp.solver_options.shooting_nodes = T_IDXS
|
||||
|
||||
ocp.code_export_directory = EXPORT_DIR
|
||||
return ocp
|
||||
|
||||
|
||||
class LongitudinalMpc:
|
||||
def __init__(self, dt=DT_MDL):
|
||||
self.dt = dt
|
||||
self.solver = AcadosOcpSolverCython(MODEL_NAME, ACADOS_SOLVER_TYPE, N)
|
||||
self.reset()
|
||||
self.source = LongitudinalPlanSource.cruise
|
||||
|
||||
def reset(self):
|
||||
self.solver.reset()
|
||||
|
||||
self.x_sol = np.zeros((N+1, X_DIM))
|
||||
self.u_sol = np.zeros((N, 1))
|
||||
self.v_solution = np.zeros(N+1)
|
||||
self.a_solution = np.zeros(N+1)
|
||||
self.j_solution = np.zeros(N)
|
||||
self.a_prev = np.array(self.a_solution)
|
||||
self.yref = np.zeros((N+1, COST_DIM))
|
||||
|
||||
for i in range(N):
|
||||
self.solver.cost_set(i, "yref", self.yref[i])
|
||||
self.solver.cost_set(N, "yref", self.yref[N][:COST_E_DIM])
|
||||
|
||||
self.params = np.zeros((N+1, PARAM_DIM))
|
||||
for i in range(N+1):
|
||||
self.solver.set(i, 'x', np.zeros(X_DIM))
|
||||
|
||||
self.last_cloudlog_t = 0
|
||||
self.crash_cnt = 0.0
|
||||
self.solution_status = 0
|
||||
# timers
|
||||
self.solve_time = 0.0
|
||||
self.x0 = np.zeros(X_DIM)
|
||||
self.set_weights()
|
||||
|
||||
def set_cost_weights(self, cost_weights, constraint_cost_weights):
|
||||
W = np.asfortranarray(np.diag(cost_weights))
|
||||
for i in range(N):
|
||||
# TODO don't hardcode A_CHANGE_COST idx
|
||||
# reduce the cost on (a-a_prev) later in the horizon.
|
||||
W[4,4] = cost_weights[4] * np.interp(T_IDXS[i], [0.0, 1.0, 2.0], [1.0, 1.0, 0.0])
|
||||
self.solver.cost_set(i, 'W', W)
|
||||
# Setting the slice without the copy make the array not contiguous,
|
||||
# causing issues with the C interface.
|
||||
self.solver.cost_set(N, 'W', np.copy(W[:COST_E_DIM, :COST_E_DIM]))
|
||||
|
||||
# Set L2 slack cost on lower bound constraints
|
||||
Zl = np.array(constraint_cost_weights)
|
||||
for i in range(N):
|
||||
self.solver.cost_set(i, 'Zl', Zl)
|
||||
|
||||
def set_weights(self, prev_accel_constraint=True, personality=log.LongitudinalPersonality.standard):
|
||||
jerk_factor = get_jerk_factor(personality)
|
||||
a_change_cost = A_CHANGE_COST if prev_accel_constraint else 0
|
||||
cost_weights = [X_EGO_OBSTACLE_COST, X_EGO_COST, V_EGO_COST, A_EGO_COST, jerk_factor * a_change_cost, jerk_factor * J_EGO_COST]
|
||||
constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, DANGER_ZONE_COST]
|
||||
self.set_cost_weights(cost_weights, constraint_cost_weights)
|
||||
|
||||
def set_cur_state(self, v, a):
|
||||
v_prev = self.x0[1]
|
||||
self.x0[1] = v
|
||||
self.x0[2] = a
|
||||
if abs(v_prev - v) > 2.: # probably only helps if v < v_prev
|
||||
for i in range(N+1):
|
||||
self.solver.set(i, 'x', self.x0)
|
||||
|
||||
@staticmethod
|
||||
def extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau):
|
||||
a_lead_traj = a_lead * np.exp(-a_lead_tau * (T_IDXS**2)/2.)
|
||||
v_lead_traj = np.clip(v_lead + np.cumsum(T_DIFFS * a_lead_traj), 0.0, 1e8)
|
||||
x_lead_traj = x_lead + np.cumsum(T_DIFFS * v_lead_traj)
|
||||
lead_xv = np.column_stack((x_lead_traj, v_lead_traj))
|
||||
return lead_xv
|
||||
|
||||
def process_lead(self, lead):
|
||||
v_ego = self.x0[1]
|
||||
if lead is not None and lead.present:
|
||||
x_lead = lead.dRel
|
||||
v_lead = lead.vLead
|
||||
a_lead = lead.aLeadK
|
||||
a_lead_tau = lead.aLeadTau
|
||||
else:
|
||||
# Fake a fast lead car, so mpc can keep running in the same mode
|
||||
x_lead = 50.0
|
||||
v_lead = v_ego + 10.0
|
||||
a_lead = 0.0
|
||||
a_lead_tau = _LEAD_ACCEL_TAU
|
||||
|
||||
# MPC will not converge if immediate crash is expected
|
||||
# Clip lead distance to what is still possible to brake for
|
||||
min_x_lead = MIN_X_LEAD_FACTOR * (v_ego + v_lead) * (v_ego - v_lead) / (-ACCEL_MIN * 2)
|
||||
x_lead = np.clip(x_lead, min_x_lead, 1e8)
|
||||
v_lead = np.clip(v_lead, 0.0, 1e8)
|
||||
a_lead = np.clip(a_lead, -10., 5.)
|
||||
lead_xv = self.extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau)
|
||||
return lead_xv
|
||||
|
||||
def update(self, radarstate, v_cruise, personality=log.LongitudinalPersonality.standard):
|
||||
t_follow = get_T_FOLLOW(personality)
|
||||
v_ego = self.x0[1]
|
||||
|
||||
lead_xv_0 = self.process_lead(radarstate.leadOne)
|
||||
lead_xv_1 = self.process_lead(radarstate.leadTwo)
|
||||
|
||||
# To estimate a safe distance from a moving lead, we calculate how much stopping
|
||||
# distance that lead needs as a minimum. We can add that to the current distance
|
||||
# and then treat that as a stopped car/obstacle at this new distance.
|
||||
lead_0_obstacle = lead_xv_0[:,0] + get_stopped_equivalence_factor(lead_xv_0[:,1])
|
||||
lead_1_obstacle = lead_xv_1[:,0] + get_stopped_equivalence_factor(lead_xv_1[:,1])
|
||||
|
||||
# Fake an obstacle for cruise, this ensures smooth acceleration to set speed
|
||||
# when the leads are no factor.
|
||||
v_lower = v_ego + (T_IDXS * CRUISE_MIN_ACCEL * 1.05)
|
||||
# TODO does this make sense when max_a is negative?
|
||||
v_upper = v_ego + (T_IDXS * CRUISE_MAX_ACCEL * 1.05)
|
||||
v_cruise_clipped = np.clip(v_cruise * np.ones(N+1), v_lower, v_upper)
|
||||
cruise_obstacle = np.cumsum(T_DIFFS * v_cruise_clipped) + get_safe_obstacle_distance(v_cruise_clipped, t_follow)
|
||||
|
||||
x_obstacles = np.column_stack([lead_0_obstacle, lead_1_obstacle, cruise_obstacle])
|
||||
self.source = MPC_SOURCES[np.argmin(x_obstacles[0])]
|
||||
|
||||
self.yref[:,:] = 0.0
|
||||
for i in range(N):
|
||||
self.solver.set(i, "yref", self.yref[i])
|
||||
self.solver.set(N, "yref", self.yref[N][:COST_E_DIM])
|
||||
|
||||
self.params[:,0] = ACCEL_MIN
|
||||
self.params[:,1] = ACCEL_MAX
|
||||
self.params[:,2] = np.min(x_obstacles, axis=1)
|
||||
self.params[:,3] = np.copy(self.a_prev)
|
||||
self.params[:,4] = t_follow
|
||||
self.params[:,5] = LEAD_DANGER_FACTOR
|
||||
|
||||
self.run()
|
||||
if (np.any(lead_xv_0[FCW_IDXS,0] - self.x_sol[FCW_IDXS,0] < CRASH_DISTANCE) and
|
||||
radarstate.leadOne.modelProb > 0.9):
|
||||
self.crash_cnt += 1
|
||||
else:
|
||||
self.crash_cnt = 0
|
||||
|
||||
def run(self):
|
||||
for i in range(N+1):
|
||||
self.solver.set(i, 'p', self.params[i])
|
||||
self.solver.constraints_set(0, "lbx", self.x0)
|
||||
self.solver.constraints_set(0, "ubx", self.x0)
|
||||
|
||||
self.solution_status = self.solver.solve()
|
||||
self.solve_time = float(self.solver.get_stats('time_tot')[0])
|
||||
|
||||
for i in range(N+1):
|
||||
self.x_sol[i] = self.solver.get(i, 'x')
|
||||
for i in range(N):
|
||||
self.u_sol[i] = self.solver.get(i, 'u')
|
||||
|
||||
self.v_solution = self.x_sol[:,1]
|
||||
self.a_solution = self.x_sol[:,2]
|
||||
self.j_solution = self.u_sol[:,0]
|
||||
|
||||
self.a_prev = np.interp(T_IDXS + self.dt, T_IDXS, self.a_solution)
|
||||
|
||||
t = time.monotonic()
|
||||
if self.solution_status != 0:
|
||||
if t > self.last_cloudlog_t + 5.0:
|
||||
self.last_cloudlog_t = t
|
||||
cloudlog.warning(f"Long mpc reset, solution_status: {self.solution_status}")
|
||||
self.reset()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
ocp = gen_long_ocp()
|
||||
AcadosOcpSolver.generate(ocp, json_file=JSON_FILE)
|
||||
+183
@@ -0,0 +1,183 @@
|
||||
#!/usr/bin/env python3
|
||||
import math
|
||||
import numpy as np
|
||||
|
||||
import openpilot.cereal.messaging as messaging
|
||||
from opendbc.car.interfaces import ACCEL_MIN, ACCEL_MAX
|
||||
from openpilot.common.constants import CV
|
||||
from openpilot.common.filter_simple import FirstOrderFilter
|
||||
from openpilot.common.realtime import DT_MDL
|
||||
from openpilot.selfdrive.modeld.constants import ModelConstants
|
||||
from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState
|
||||
from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpc, LongitudinalPlanSource
|
||||
from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import T_IDXS as T_IDXS_MPC
|
||||
from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N, get_accel_from_plan
|
||||
from openpilot.selfdrive.car.cruise import V_CRUISE_MAX, V_CRUISE_UNSET
|
||||
from openpilot.common.swaglog import cloudlog
|
||||
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlannerSP
|
||||
|
||||
A_CRUISE_MAX_VALS = [1.6, 1.2, 0.8, 0.6]
|
||||
A_CRUISE_MAX_BP = [0., 10.0, 25., 40.]
|
||||
CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N]
|
||||
ALLOW_THROTTLE_THRESHOLD = 0.4
|
||||
MIN_ALLOW_THROTTLE_SPEED = 2.5
|
||||
|
||||
# Lookup table for turns
|
||||
_A_TOTAL_MAX_V = [1.7, 3.2]
|
||||
_A_TOTAL_MAX_BP = [20., 40.]
|
||||
|
||||
def get_max_accel(v_ego):
|
||||
return np.interp(v_ego, A_CRUISE_MAX_BP, A_CRUISE_MAX_VALS)
|
||||
|
||||
def get_coast_accel(pitch):
|
||||
return np.sin(pitch) * -5.65 - 0.3 # fitted from data using xx/projects/allow_throttle/compute_coast_accel.py
|
||||
|
||||
def limit_accel_in_turns(v_ego, angle_steers, a_target, CP):
|
||||
"""
|
||||
This function returns a limited long acceleration allowed, depending on the existing lateral acceleration
|
||||
this should avoid accelerating when losing the target in turns
|
||||
"""
|
||||
# FIXME: This function to calculate lateral accel is incorrect and should use the VehicleModel
|
||||
# The lookup table for turns should also be updated if we do this
|
||||
a_total_max = np.interp(v_ego, _A_TOTAL_MAX_BP, _A_TOTAL_MAX_V)
|
||||
a_y = v_ego ** 2 * angle_steers * CV.DEG_TO_RAD / (CP.steerRatio * CP.wheelbase)
|
||||
a_x_allowed = math.sqrt(max(a_total_max ** 2 - a_y ** 2, 0.))
|
||||
|
||||
return [a_target[0], min(a_target[1], a_x_allowed)]
|
||||
|
||||
|
||||
class LongitudinalPlanner(LongitudinalPlannerSP):
|
||||
def __init__(self, CP, CP_SP, init_v=0.0, init_a=0.0, dt=DT_MDL):
|
||||
self.CP = CP
|
||||
self.mpc = LongitudinalMpc(dt=dt)
|
||||
LongitudinalPlannerSP.__init__(self, self.CP, CP_SP, self.mpc)
|
||||
self.fcw = False
|
||||
self.dt = dt
|
||||
self.allow_throttle = True
|
||||
|
||||
self.a_desired = init_a
|
||||
self.v_desired_filter = FirstOrderFilter(init_v, 2.0, self.dt)
|
||||
self.prev_accel_clip = [ACCEL_MIN, ACCEL_MAX]
|
||||
self.output_a_target = 0.0
|
||||
self.output_should_stop = False
|
||||
|
||||
self.v_desired_trajectory = np.zeros(CONTROL_N)
|
||||
self.a_desired_trajectory = np.zeros(CONTROL_N)
|
||||
self.j_desired_trajectory = np.zeros(CONTROL_N)
|
||||
|
||||
def update(self, sm):
|
||||
LongitudinalPlannerSP.update(self, sm)
|
||||
|
||||
if len(sm['carControl'].orientationNED) == 3:
|
||||
accel_coast = get_coast_accel(sm['carControl'].orientationNED[1])
|
||||
else:
|
||||
accel_coast = ACCEL_MAX
|
||||
|
||||
v_ego = sm['carState'].vEgo
|
||||
v_cruise_kph = min(sm['carState'].vCruise, V_CRUISE_MAX)
|
||||
v_cruise = v_cruise_kph * CV.KPH_TO_MS
|
||||
v_cruise_initialized = sm['carState'].vCruise != V_CRUISE_UNSET
|
||||
|
||||
long_control_off = sm['controlsState'].longControlState == LongCtrlState.off
|
||||
force_slow_decel = sm['controlsState'].forceDecel
|
||||
|
||||
# Reset current state when not engaged, or user is controlling the speed
|
||||
reset_state = long_control_off if self.CP.openpilotLongitudinalControl else not sm['selfdriveState'].enabled
|
||||
# PCM cruise speed may be updated a few cycles later, check if initialized
|
||||
reset_state = reset_state or not v_cruise_initialized
|
||||
|
||||
# No change cost when user is controlling the speed, or when standstill
|
||||
prev_accel_constraint = not (reset_state or sm['carState'].standstill)
|
||||
|
||||
accel_clip = [ACCEL_MIN, get_max_accel(v_ego)]
|
||||
steer_angle_without_offset = sm['carState'].steeringAngleDeg - sm['liveParameters'].angleOffsetDeg
|
||||
accel_clip = limit_accel_in_turns(v_ego, steer_angle_without_offset, accel_clip, self.CP)
|
||||
|
||||
if reset_state:
|
||||
self.v_desired_filter.x = v_ego
|
||||
# Clip aEgo to cruise limits to prevent large accelerations when becoming active
|
||||
self.a_desired = np.clip(sm['carState'].aEgo, accel_clip[0], accel_clip[1])
|
||||
|
||||
# Prevent divergence, smooth in current v_ego
|
||||
self.v_desired_filter.x = max(0.0, self.v_desired_filter.update(v_ego))
|
||||
throttle_probs = sm['modelV2'].meta.disengagePredictions.gasPressProbs
|
||||
throttle_prob = throttle_probs[1] if len(throttle_probs) > 1 else 1.0
|
||||
# Don't clip at low speeds since throttle_prob doesn't account for creep
|
||||
self.allow_throttle = throttle_prob > ALLOW_THROTTLE_THRESHOLD or v_ego <= MIN_ALLOW_THROTTLE_SPEED
|
||||
|
||||
if not self.allow_throttle:
|
||||
clipped_accel_coast = max(accel_coast, accel_clip[0])
|
||||
clipped_accel_coast_interp = np.interp(v_ego, [MIN_ALLOW_THROTTLE_SPEED, MIN_ALLOW_THROTTLE_SPEED*2], [accel_clip[1], clipped_accel_coast])
|
||||
accel_clip[1] = min(accel_clip[1], clipped_accel_coast_interp)
|
||||
|
||||
# Get new v_cruise and a_desired from Smart Cruise Control and Speed Limit Assist
|
||||
v_cruise, self.a_desired = LongitudinalPlannerSP.update_targets(self, sm, self.v_desired_filter.x, self.a_desired, v_cruise)
|
||||
|
||||
if force_slow_decel:
|
||||
v_cruise = 0.0
|
||||
|
||||
self.mpc.set_weights(prev_accel_constraint, personality=sm['selfdriveState'].personality)
|
||||
self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired)
|
||||
self.mpc.update(sm['radarState'], v_cruise, personality=sm['selfdriveState'].personality)
|
||||
|
||||
self.v_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.v_solution)
|
||||
self.a_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.a_solution)
|
||||
self.j_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC[:-1], self.mpc.j_solution)
|
||||
|
||||
# TODO counter is only needed because radar is glitchy, remove once radar is gone
|
||||
self.fcw = self.mpc.crash_cnt > 2 and not sm['carState'].standstill
|
||||
if self.fcw:
|
||||
cloudlog.info("FCW triggered")
|
||||
|
||||
# Interpolate 0.05 seconds and save as starting point for next iteration
|
||||
a_prev = self.a_desired
|
||||
self.a_desired = float(np.interp(self.dt, CONTROL_N_T_IDX, self.a_desired_trajectory))
|
||||
self.v_desired_filter.x = self.v_desired_filter.x + self.dt * (self.a_desired + a_prev) / 2.0
|
||||
|
||||
action_t = self.CP.longitudinalActuatorDelay + DT_MDL
|
||||
output_a_target_mpc, output_should_stop_mpc = get_accel_from_plan(self.v_desired_trajectory, self.a_desired_trajectory, CONTROL_N_T_IDX,
|
||||
action_t=action_t, vEgoStopping=self.CP.vEgoStopping)
|
||||
output_a_target_e2e = sm['modelV2'].action.desiredAcceleration
|
||||
output_should_stop_e2e = sm['modelV2'].action.shouldStop
|
||||
|
||||
if self.is_e2e(sm):
|
||||
output_a_target = min(output_a_target_e2e, output_a_target_mpc)
|
||||
self.output_should_stop = output_should_stop_e2e or output_should_stop_mpc
|
||||
if output_a_target < output_a_target_mpc:
|
||||
self.mpc.source = LongitudinalPlanSource.e2e
|
||||
else:
|
||||
output_a_target = output_a_target_mpc
|
||||
self.output_should_stop = output_should_stop_mpc
|
||||
|
||||
for idx in range(2):
|
||||
accel_clip[idx] = np.clip(accel_clip[idx], self.prev_accel_clip[idx] - 0.05, self.prev_accel_clip[idx] + 0.05)
|
||||
self.output_a_target = np.clip(output_a_target, accel_clip[0], accel_clip[1])
|
||||
self.prev_accel_clip = accel_clip
|
||||
|
||||
def publish(self, sm, pm):
|
||||
plan_send = messaging.new_message('longitudinalPlan')
|
||||
|
||||
plan_send.valid = sm.all_checks()
|
||||
|
||||
longitudinalPlan = plan_send.longitudinalPlan
|
||||
longitudinalPlan.modelMonoTime = sm.logMonoTime['modelV2']
|
||||
longitudinalPlan.processingDelay = (plan_send.logMonoTime / 1e9) - sm.logMonoTime['modelV2']
|
||||
longitudinalPlan.solverExecutionTime = self.mpc.solve_time
|
||||
|
||||
longitudinalPlan.speeds = self.v_desired_trajectory.tolist()
|
||||
longitudinalPlan.accels = self.a_desired_trajectory.tolist()
|
||||
longitudinalPlan.jerks = self.j_desired_trajectory.tolist()
|
||||
|
||||
longitudinalPlan.hasLead = sm['radarState'].leadOne.present
|
||||
longitudinalPlan.longitudinalPlanSource = self.mpc.source
|
||||
longitudinalPlan.fcw = self.fcw
|
||||
|
||||
longitudinalPlan.aTarget = float(self.output_a_target)
|
||||
longitudinalPlan.shouldStop = bool(self.output_should_stop)
|
||||
longitudinalPlan.allowBrake = True
|
||||
longitudinalPlan.allowThrottle = bool(self.allow_throttle)
|
||||
|
||||
pm.send('longitudinalPlan', plan_send)
|
||||
|
||||
self.publish_longitudinal_plan_sp(sm, pm)
|
||||
Executable
+51
@@ -0,0 +1,51 @@
|
||||
#!/usr/bin/env python3
|
||||
from openpilot.cereal import custom
|
||||
from opendbc.car.structs import car
|
||||
from openpilot.common.gps import get_gps_location_service
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.realtime import Priority, config_realtime_process
|
||||
from openpilot.common.swaglog import cloudlog
|
||||
from openpilot.selfdrive.controls.lib.ldw import LaneDepartureWarning
|
||||
from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner
|
||||
import openpilot.cereal.messaging as messaging
|
||||
|
||||
|
||||
def main():
|
||||
config_realtime_process(5, Priority.CTRL_LOW)
|
||||
|
||||
cloudlog.info("plannerd is waiting for CarParams")
|
||||
params = Params()
|
||||
CP = messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams)
|
||||
cloudlog.info("plannerd got CarParams: %s", CP.brand)
|
||||
|
||||
cloudlog.info("plannerd is waiting for CarParamsSP")
|
||||
CP_SP = messaging.log_from_bytes(params.get("CarParamsSP", block=True), custom.CarParamsSP)
|
||||
cloudlog.info("plannerd got CarParamsSP")
|
||||
|
||||
gps_location_service = get_gps_location_service(params)
|
||||
ignore_services = ["liveMapDataSP", gps_location_service]
|
||||
|
||||
ldw = LaneDepartureWarning()
|
||||
longitudinal_planner = LongitudinalPlanner(CP, CP_SP)
|
||||
pm = messaging.PubMaster(['longitudinalPlan', 'driverAssistance', 'longitudinalPlanSP'])
|
||||
sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'liveParameters', 'radarState', 'modelV2', 'selfdriveState',
|
||||
'liveMapDataSP', 'carStateSP', gps_location_service],
|
||||
poll='carState', ignore_alive=ignore_services, ignore_avg_freq=ignore_services, ignore_valid=ignore_services)
|
||||
|
||||
while True:
|
||||
sm.update()
|
||||
longitudinal_planner.sla.update_car_state(sm['carState'])
|
||||
if sm.updated['modelV2']:
|
||||
longitudinal_planner.update(sm)
|
||||
longitudinal_planner.publish(sm, pm)
|
||||
|
||||
ldw.update(sm.frame, sm['modelV2'], sm['carState'], sm['carControl'])
|
||||
msg = messaging.new_message('driverAssistance')
|
||||
msg.valid = sm.all_checks()
|
||||
msg.driverAssistance.leftLaneDeparture = ldw.left
|
||||
msg.driverAssistance.rightLaneDeparture = ldw.right
|
||||
pm.send('driverAssistance', msg)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
Executable
+303
@@ -0,0 +1,303 @@
|
||||
#!/usr/bin/env python3
|
||||
import math
|
||||
import numpy as np
|
||||
from collections import deque
|
||||
from typing import Any
|
||||
|
||||
import capnp
|
||||
from openpilot.cereal import messaging, log, custom
|
||||
from opendbc.car.structs import car
|
||||
from openpilot.common.filter_simple import FirstOrderFilter
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.realtime import DT_MDL, Priority, config_realtime_process
|
||||
from openpilot.common.swaglog import cloudlog
|
||||
from openpilot.common.simple_kalman import KF1D
|
||||
|
||||
from opendbc.car import structs
|
||||
from opendbc.car.hyundai.values import HyundaiFlags
|
||||
from opendbc.sunnypilot.car.hyundai.values import HyundaiFlagsSP
|
||||
|
||||
|
||||
# Default lead acceleration decay set to 50% at 1s
|
||||
_LEAD_ACCEL_TAU = 1.5
|
||||
|
||||
# radar tracks
|
||||
SPEED, ACCEL = 0, 1 # Kalman filter states enum
|
||||
|
||||
# stationary qualification parameters
|
||||
V_EGO_STATIONARY = 4. # no stationary object flag below this speed
|
||||
|
||||
RADAR_TO_CAMERA = 1.52 # RADAR is ~ 1.5m ahead from center of mesh frame
|
||||
|
||||
|
||||
class KalmanParams:
|
||||
def __init__(self, dt: float):
|
||||
# Lead Kalman Filter params, calculating K from A, C, Q, R requires the control library.
|
||||
# hardcoding a lookup table to compute K for values of radar_ts between 0.01s and 0.2s
|
||||
assert dt > .01 and dt < .2, "Radar time step must be between .01s and 0.2s"
|
||||
self.A = [[1.0, dt], [0.0, 1.0]]
|
||||
self.C = [1.0, 0.0]
|
||||
#Q = np.matrix([[10., 0.0], [0.0, 100.]])
|
||||
#R = 1e3
|
||||
#K = np.matrix([[ 0.05705578], [ 0.03073241]])
|
||||
dts = [i * 0.01 for i in range(1, 21)]
|
||||
K0 = [0.12287673, 0.14556536, 0.16522756, 0.18281627, 0.1988689, 0.21372394,
|
||||
0.22761098, 0.24069424, 0.253096, 0.26491023, 0.27621103, 0.28705801,
|
||||
0.29750003, 0.30757767, 0.31732515, 0.32677158, 0.33594201, 0.34485814,
|
||||
0.35353899, 0.36200124]
|
||||
K1 = [0.29666309, 0.29330885, 0.29042818, 0.28787125, 0.28555364, 0.28342219,
|
||||
0.28144091, 0.27958406, 0.27783249, 0.27617149, 0.27458948, 0.27307714,
|
||||
0.27162685, 0.27023228, 0.26888809, 0.26758976, 0.26633338, 0.26511557,
|
||||
0.26393339, 0.26278425]
|
||||
self.K = [[np.interp(dt, dts, K0)], [np.interp(dt, dts, K1)]]
|
||||
|
||||
|
||||
class Track:
|
||||
def __init__(self, identifier: int, v_lead: float, kalman_params: KalmanParams):
|
||||
self.identifier = identifier
|
||||
self.cnt = 0
|
||||
self.aLeadTau = FirstOrderFilter(_LEAD_ACCEL_TAU, 0.45, DT_MDL)
|
||||
self.K_A = kalman_params.A
|
||||
self.K_C = kalman_params.C
|
||||
self.K_K = kalman_params.K
|
||||
self.kf = KF1D([[v_lead], [0.0]], self.K_A, self.K_C, self.K_K)
|
||||
|
||||
def update(self, d_rel: float, y_rel: float, v_rel: float, v_lead: float):
|
||||
# relative values, copy
|
||||
self.dRel = d_rel # LONG_DIST
|
||||
self.yRel = y_rel # -LAT_DIST
|
||||
self.vRel = v_rel # REL_SPEED
|
||||
self.vLead = v_lead
|
||||
|
||||
# computed velocity and accelerations
|
||||
if self.cnt > 0:
|
||||
self.kf.update(self.vLead)
|
||||
|
||||
self.vLeadK = float(self.kf.x[SPEED][0])
|
||||
self.aLeadK = float(self.kf.x[ACCEL][0])
|
||||
|
||||
# Learn if constant acceleration
|
||||
if abs(self.aLeadK) < 0.5:
|
||||
self.aLeadTau.x = _LEAD_ACCEL_TAU
|
||||
else:
|
||||
self.aLeadTau.update(0.0)
|
||||
|
||||
self.cnt += 1
|
||||
|
||||
def get_RadarState(self, model_prob: float = 0.0):
|
||||
return {
|
||||
"dRel": float(self.dRel),
|
||||
"yRel": float(self.yRel),
|
||||
"vRel": float(self.vRel),
|
||||
"vLead": float(self.vLead),
|
||||
"vLeadK": float(self.vLeadK),
|
||||
"aLeadK": float(self.aLeadK),
|
||||
"aLeadTau": float(self.aLeadTau.x),
|
||||
"present": True,
|
||||
"modelProb": model_prob,
|
||||
"radar": True,
|
||||
"radarTrackId": self.identifier,
|
||||
}
|
||||
|
||||
def potential_low_speed_lead(self, v_ego: float):
|
||||
# stop for stuff in front of you and low speed, even without model confirmation
|
||||
# Radar points closer than 0.75, are almost always glitches on toyota radars
|
||||
return abs(self.yRel) < 1.0 and (v_ego < V_EGO_STATIONARY) and (0.75 < self.dRel < 25)
|
||||
|
||||
def __str__(self):
|
||||
ret = f"x: {self.dRel:4.1f} y: {self.yRel:4.1f} v: {self.vRel:4.1f} a: {self.aLeadK:4.1f}"
|
||||
return ret
|
||||
|
||||
|
||||
def laplacian_pdf(x: float, mu: float, b: float):
|
||||
b = max(b, 1e-4)
|
||||
return math.exp(-abs(x-mu)/b)
|
||||
|
||||
|
||||
def match_vision_to_track(v_ego: float, lead: capnp._DynamicStructReader, tracks: dict[int, Track]):
|
||||
offset_vision_dist = lead.x[0] - RADAR_TO_CAMERA
|
||||
|
||||
def prob(c):
|
||||
prob_d = laplacian_pdf(c.dRel, offset_vision_dist, lead.xStd[0])
|
||||
prob_y = laplacian_pdf(c.yRel, -lead.y[0], lead.yStd[0])
|
||||
prob_v = laplacian_pdf(c.vRel + v_ego, lead.v[0], lead.vStd[0])
|
||||
|
||||
# This isn't exactly right, but it's a good heuristic
|
||||
return prob_d * prob_y * prob_v
|
||||
|
||||
track = max(tracks.values(), key=prob)
|
||||
|
||||
# if no 'sane' match is found return -1
|
||||
# stationary radar points can be false positives
|
||||
dist_sane = abs(track.dRel - offset_vision_dist) < max([(offset_vision_dist)*.25, 5.0])
|
||||
vel_sane = (abs(track.vRel + v_ego - lead.v[0]) < 10) or (v_ego + track.vRel > 3)
|
||||
if dist_sane and vel_sane:
|
||||
return track
|
||||
else:
|
||||
return None
|
||||
|
||||
|
||||
def get_RadarState_from_vision(lead_msg: capnp._DynamicStructReader, v_ego: float, model_v_ego: float, lead_prob: float):
|
||||
lead_v_rel_pred = lead_msg.v[0] - model_v_ego
|
||||
return {
|
||||
"dRel": float(lead_msg.x[0] - RADAR_TO_CAMERA),
|
||||
"yRel": float(-lead_msg.y[0]),
|
||||
"vRel": float(lead_v_rel_pred),
|
||||
"vLead": float(v_ego + lead_v_rel_pred),
|
||||
"vLeadK": float(v_ego + lead_v_rel_pred),
|
||||
"aLeadK": float(lead_msg.a[0]),
|
||||
"aLeadTau": 0.3,
|
||||
"modelProb": float(lead_prob),
|
||||
"present": True,
|
||||
"radar": False,
|
||||
"radarTrackId": -1,
|
||||
}
|
||||
|
||||
|
||||
def get_lead(v_ego: float, ready: bool, tracks: dict[int, Track], lead_msg: capnp._DynamicStructReader,
|
||||
model_v_ego: float, lead_prob: float, CP: structs.CarParams, CP_SP: structs.CarParamsSP,
|
||||
low_speed_override: bool = True) -> dict[str, Any]:
|
||||
# Determine leads, this is where the essential logic happens
|
||||
if len(tracks) > 0 and ready and lead_prob > .5:
|
||||
track = match_vision_to_track(v_ego, lead_msg, tracks)
|
||||
else:
|
||||
track = None
|
||||
|
||||
lead_dict = {'present': False}
|
||||
if track is not None:
|
||||
lead_dict = track.get_RadarState(lead_prob)
|
||||
lead_dict = get_custom_yrel(CP, CP_SP, lead_dict, lead_msg)
|
||||
elif (track is None) and ready and (lead_prob > .5):
|
||||
lead_dict = get_RadarState_from_vision(lead_msg, v_ego, model_v_ego, lead_prob)
|
||||
|
||||
if low_speed_override:
|
||||
low_speed_tracks = [c for c in tracks.values() if c.potential_low_speed_lead(v_ego)]
|
||||
if len(low_speed_tracks) > 0:
|
||||
closest_track = min(low_speed_tracks, key=lambda c: c.dRel)
|
||||
|
||||
# Only choose new track if it is actually closer than the previous one
|
||||
if (not lead_dict['present']) or (closest_track.dRel < lead_dict['dRel']):
|
||||
lead_dict = closest_track.get_RadarState()
|
||||
|
||||
return lead_dict
|
||||
|
||||
|
||||
def get_custom_yrel(CP: structs.CarParams, CP_SP: structs.CarParamsSP, lead_dict: dict[str, Any],
|
||||
lead_msg: capnp._DynamicStructReader) -> dict[str, Any]:
|
||||
if CP.brand == "hyundai" and (CP_SP.flags & HyundaiFlagsSP.ENHANCED_SCC or
|
||||
CP.flags & (HyundaiFlags.CANFD_CAMERA_SCC | HyundaiFlags.CAMERA_SCC)):
|
||||
lead_dict['yRel'] = float(-lead_msg.y[0])
|
||||
|
||||
return lead_dict
|
||||
|
||||
|
||||
class RadarD:
|
||||
def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParams, delay: float = 0.0):
|
||||
self.CP = CP
|
||||
self.CP_SP = CP_SP
|
||||
|
||||
self.current_time = 0.0
|
||||
|
||||
self.tracks: dict[int, Track] = {}
|
||||
self.kalman_params = KalmanParams(DT_MDL)
|
||||
self.lead_prob_filters = [FirstOrderFilter(0.0, 0.2, DT_MDL) for _ in range(2)]
|
||||
|
||||
self.v_ego = 0.0
|
||||
self.v_ego_hist = deque([0.0], maxlen=int(round(delay / DT_MDL))+1)
|
||||
self.last_v_ego_frame = -1
|
||||
|
||||
self.radar_state: capnp._DynamicStructBuilder | None = None
|
||||
self.radar_state_valid = False
|
||||
|
||||
self.ready = False
|
||||
|
||||
def update(self, sm: messaging.SubMaster, rr: car.RadarData):
|
||||
self.ready = sm.seen['modelV2']
|
||||
self.current_time = 1e-9*max(sm.logMonoTime.values())
|
||||
|
||||
if sm.recv_frame['carState'] != self.last_v_ego_frame:
|
||||
self.v_ego = sm['carState'].vEgo
|
||||
self.v_ego_hist.append(self.v_ego)
|
||||
self.last_v_ego_frame = sm.recv_frame['carState']
|
||||
|
||||
ar_pts = {pt.trackId: [pt.dRel, pt.yRel, pt.vRel] for pt in rr.points}
|
||||
|
||||
# *** remove missing points from meta data ***
|
||||
for ids in list(self.tracks.keys()):
|
||||
if ids not in ar_pts:
|
||||
self.tracks.pop(ids, None)
|
||||
|
||||
# *** compute the tracks ***
|
||||
for ids in ar_pts:
|
||||
rpt = ar_pts[ids]
|
||||
|
||||
# align v_ego by a fixed time to align it with the radar measurement
|
||||
v_lead = rpt[2] + self.v_ego_hist[0]
|
||||
|
||||
# create the track if it doesn't exist or it's a new track
|
||||
if ids not in self.tracks:
|
||||
self.tracks[ids] = Track(ids, v_lead, self.kalman_params)
|
||||
self.tracks[ids].update(rpt[0], rpt[1], rpt[2], v_lead)
|
||||
|
||||
# *** publish radarState ***
|
||||
self.radar_state_valid = sm.all_checks()
|
||||
self.radar_state = log.RadarState.new_message()
|
||||
self.radar_state.mdMonoTime = sm.logMonoTime['modelV2']
|
||||
self.radar_state.radarErrors = rr.errors
|
||||
|
||||
if len(sm['modelV2'].velocity.x):
|
||||
model_v_ego = sm['modelV2'].velocity.x[0]
|
||||
else:
|
||||
model_v_ego = self.v_ego
|
||||
leads_v3 = sm['modelV2'].leadsV3
|
||||
if len(leads_v3) > 1:
|
||||
for i in range(2):
|
||||
# Asymmetric filter on lead prob to keep lead when uncertain
|
||||
lead_prob = leads_v3[i].prob
|
||||
if lead_prob > self.lead_prob_filters[i].x:
|
||||
self.lead_prob_filters[i].x = lead_prob
|
||||
else:
|
||||
self.lead_prob_filters[i].update(lead_prob)
|
||||
|
||||
self.radar_state.leadOne = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[0], model_v_ego, self.lead_prob_filters[0].x,
|
||||
self.CP, self.CP_SP, low_speed_override=True)
|
||||
self.radar_state.leadTwo = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[1], model_v_ego, self.lead_prob_filters[1].x,
|
||||
self.CP, self.CP_SP, low_speed_override=False)
|
||||
|
||||
def publish(self, pm: messaging.PubMaster):
|
||||
assert self.radar_state is not None
|
||||
|
||||
radar_msg = messaging.new_message("radarState")
|
||||
radar_msg.valid = self.radar_state_valid
|
||||
radar_msg.radarState = self.radar_state
|
||||
pm.send("radarState", radar_msg)
|
||||
|
||||
|
||||
# fuses camera and radar data for best lead detection
|
||||
def main() -> None:
|
||||
config_realtime_process(5, Priority.CTRL_LOW)
|
||||
|
||||
# wait for stats about the car to come in from controls
|
||||
cloudlog.info("radard is waiting for CarParams")
|
||||
CP = messaging.log_from_bytes(Params().get("CarParams", block=True), car.CarParams)
|
||||
cloudlog.info("radard got CarParams")
|
||||
|
||||
cloudlog.info("radard is waiting for CarParamsSP")
|
||||
CP_SP = messaging.log_from_bytes(Params().get("CarParamsSP", block=True), custom.CarParamsSP)
|
||||
cloudlog.info("radard got CarParamsSP")
|
||||
|
||||
# *** setup messaging
|
||||
sm = messaging.SubMaster(['modelV2', 'carState', 'liveTracks'], poll='modelV2')
|
||||
pm = messaging.PubMaster(['radarState'])
|
||||
|
||||
RD = RadarD(CP, CP_SP, CP.radarDelay)
|
||||
|
||||
while 1:
|
||||
sm.update()
|
||||
|
||||
RD.update(sm, sm['liveTracks'])
|
||||
RD.publish(pm)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
Whitespace-only changes.
@@ -0,0 +1,46 @@
|
||||
import pytest
|
||||
import itertools
|
||||
from openpilot.common.parameterized import parameterized_class
|
||||
|
||||
from openpilot.cereal import log
|
||||
|
||||
from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import get_safe_obstacle_distance, get_stopped_equivalence_factor, get_T_FOLLOW
|
||||
from openpilot.selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver
|
||||
|
||||
|
||||
def desired_follow_distance(v_ego, v_lead, t_follow=None):
|
||||
if t_follow is None:
|
||||
t_follow = get_T_FOLLOW()
|
||||
return get_safe_obstacle_distance(v_ego, t_follow) - get_stopped_equivalence_factor(v_lead)
|
||||
|
||||
def run_following_distance_simulation(v_lead, t_end=100.0, e2e=False, personality=0):
|
||||
man = Maneuver(
|
||||
'',
|
||||
duration=t_end,
|
||||
initial_speed=float(v_lead),
|
||||
lead_relevancy=True,
|
||||
initial_distance_lead=100,
|
||||
speed_lead_values=[v_lead],
|
||||
breakpoints=[0.],
|
||||
e2e=e2e,
|
||||
personality=personality,
|
||||
)
|
||||
valid, output = man.evaluate()
|
||||
assert valid
|
||||
return output[-1,2] - output[-1,1]
|
||||
|
||||
|
||||
@parameterized_class(("e2e", "personality", "speed"), itertools.product(
|
||||
[True, False], # e2e
|
||||
[log.LongitudinalPersonality.relaxed, # personality
|
||||
log.LongitudinalPersonality.standard,
|
||||
log.LongitudinalPersonality.aggressive],
|
||||
[0,10,35])) # speed
|
||||
class TestFollowingDistance:
|
||||
def test_following_distance(self):
|
||||
v_lead = float(self.speed)
|
||||
simulation_steady_state = run_following_distance_simulation(v_lead, e2e=self.e2e, personality=self.personality)
|
||||
correct_steady_state = desired_follow_distance(v_lead, v_lead, get_T_FOLLOW(self.personality))
|
||||
err_ratio = 0.2 if self.e2e else 0.1
|
||||
abs_err_margin = 0.5 if v_lead > 0.0 else 1.15
|
||||
assert simulation_steady_state == pytest.approx(correct_steady_state, abs=err_ratio * correct_steady_state + abs_err_margin)
|
||||
@@ -0,0 +1,56 @@
|
||||
from openpilot.common.parameterized import parameterized
|
||||
|
||||
from openpilot.cereal import log
|
||||
from opendbc.car.structs import car
|
||||
from opendbc.car.car_helpers import interfaces
|
||||
from opendbc.car.honda.values import CAR as HONDA
|
||||
from opendbc.car.toyota.values import CAR as TOYOTA
|
||||
from opendbc.car.nissan.values import CAR as NISSAN
|
||||
from opendbc.car.gm.values import CAR as GM
|
||||
from opendbc.car.vehicle_model import VehicleModel
|
||||
from openpilot.common.realtime import DT_CTRL
|
||||
from openpilot.selfdrive.car.helpers import convert_to_capnp
|
||||
from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID
|
||||
from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque
|
||||
from openpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle
|
||||
from openpilot.selfdrive.locationd.helpers import Pose
|
||||
from openpilot.common.mock.generators import generate_livePose
|
||||
from openpilot.sunnypilot.selfdrive.car import interfaces as sunnypilot_interfaces
|
||||
|
||||
|
||||
class TestLatControl:
|
||||
|
||||
@parameterized.expand([(HONDA.HONDA_CIVIC, LatControlPID), (TOYOTA.TOYOTA_RAV4, LatControlTorque),
|
||||
(NISSAN.NISSAN_LEAF, LatControlAngle), (GM.CHEVROLET_BOLT_EUV, LatControlTorque)])
|
||||
def test_saturation(self, car_name, controller):
|
||||
CarInterface = interfaces[car_name]
|
||||
CP = CarInterface.get_non_essential_params(car_name)
|
||||
CP_SP = CarInterface.get_non_essential_params_sp(CP, car_name)
|
||||
CI = CarInterface(CP, CP_SP)
|
||||
sunnypilot_interfaces.setup_interfaces(CI)
|
||||
CP_SP = convert_to_capnp(CP_SP)
|
||||
VM = VehicleModel(CP)
|
||||
|
||||
controller = controller(CP.as_reader(), CP_SP.as_reader(), CI, DT_CTRL)
|
||||
|
||||
CS = car.CarState.new_message()
|
||||
CS.vEgo = 30
|
||||
CS.steeringPressed = False
|
||||
|
||||
params = log.LiveParametersData.new_message()
|
||||
|
||||
lp = generate_livePose()
|
||||
pose = Pose.from_live_pose(lp.livePose)
|
||||
|
||||
# Saturate for curvature limited and controller limited
|
||||
for _ in range(1000):
|
||||
_, _, lac_log = controller.update(True, CS, VM, params, False, 0, pose, True, 0.2)
|
||||
assert lac_log.saturated
|
||||
|
||||
for _ in range(1000):
|
||||
_, _, lac_log = controller.update(True, CS, VM, params, False, 0, pose, False, 0.2)
|
||||
assert not lac_log.saturated
|
||||
|
||||
for _ in range(1000):
|
||||
_, _, lac_log = controller.update(True, CS, VM, params, False, 1, pose, False, 0.2)
|
||||
assert lac_log.saturated
|
||||
@@ -0,0 +1,48 @@
|
||||
from openpilot.common.parameterized import parameterized
|
||||
|
||||
from openpilot.cereal import log
|
||||
from opendbc.car.structs import car
|
||||
from opendbc.car.car_helpers import interfaces
|
||||
from opendbc.car.toyota.values import CAR as TOYOTA
|
||||
from opendbc.car.vehicle_model import VehicleModel
|
||||
from openpilot.common.realtime import DT_CTRL
|
||||
from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque, LAT_ACCEL_REQUEST_BUFFER_SECONDS
|
||||
|
||||
from openpilot.selfdrive.car.helpers import convert_to_capnp
|
||||
from openpilot.selfdrive.locationd.helpers import Pose
|
||||
from openpilot.common.mock.generators import generate_livePose
|
||||
from openpilot.sunnypilot.selfdrive.car import interfaces as sunnypilot_interfaces
|
||||
|
||||
def get_controller(car_name):
|
||||
CarInterface = interfaces[car_name]
|
||||
CP = CarInterface.get_non_essential_params(car_name)
|
||||
CP_SP = CarInterface.get_non_essential_params_sp(CP, car_name)
|
||||
CI = CarInterface(CP, CP_SP)
|
||||
sunnypilot_interfaces.setup_interfaces(CI)
|
||||
CP_SP = convert_to_capnp(CP_SP)
|
||||
VM = VehicleModel(CP)
|
||||
controller = LatControlTorque(CP.as_reader(), CP_SP.as_reader(), CI, DT_CTRL)
|
||||
return controller, VM
|
||||
|
||||
class TestLatControlTorqueBuffer:
|
||||
|
||||
@parameterized.expand([(TOYOTA.TOYOTA_COROLLA_TSS2,)])
|
||||
def test_request_buffer_consistency(self, car_name):
|
||||
buffer_steps = int(LAT_ACCEL_REQUEST_BUFFER_SECONDS / DT_CTRL)
|
||||
controller, VM = get_controller(car_name)
|
||||
|
||||
CS = car.CarState.new_message()
|
||||
CS.vEgo = 30
|
||||
CS.steeringPressed = False
|
||||
params = log.LiveParametersData.new_message()
|
||||
|
||||
lp = generate_livePose()
|
||||
pose = Pose.from_live_pose(lp.livePose)
|
||||
|
||||
for _ in range(buffer_steps):
|
||||
controller.update(True, CS, VM, params, False, 0.001, pose, False, 0.2)
|
||||
assert all(val != 0 for val in controller.lat_accel_request_buffer)
|
||||
|
||||
for _ in range(buffer_steps):
|
||||
controller.update(False, CS, VM, params, False, 0.0, pose, False, 0.2)
|
||||
assert all(val == 0 for val in controller.lat_accel_request_buffer)
|
||||
@@ -0,0 +1,31 @@
|
||||
import openpilot.cereal.messaging as messaging
|
||||
|
||||
from opendbc.car.toyota.values import CAR as TOYOTA
|
||||
from openpilot.selfdrive.test.process_replay import replay_process_with_name
|
||||
|
||||
|
||||
class TestLeads:
|
||||
def test_radar_fault(self):
|
||||
# if there's no radar-related can traffic, radard should either not respond or respond with an error
|
||||
# this is tightly coupled with underlying car radar_interface implementation, but it's a good sanity check
|
||||
def single_iter_pkg():
|
||||
# single iter package, with meaningless cans and empty carState/modelV2
|
||||
msgs = []
|
||||
for _ in range(500):
|
||||
can = messaging.new_message("can", 1)
|
||||
cs = messaging.new_message("carState")
|
||||
cp = messaging.new_message("carParams")
|
||||
msgs.append(can.as_reader())
|
||||
msgs.append(cs.as_reader())
|
||||
msgs.append(cp.as_reader())
|
||||
model = messaging.new_message("modelV2")
|
||||
msgs.append(model.as_reader())
|
||||
|
||||
return msgs
|
||||
|
||||
msgs = [m for _ in range(3) for m in single_iter_pkg()]
|
||||
out = replay_process_with_name("card", msgs, fingerprint=TOYOTA.TOYOTA_COROLLA_TSS2)
|
||||
states = [m for m in out if m.which() == "liveTracks"]
|
||||
failures = [not state.valid for state in states]
|
||||
|
||||
assert len(states) == 0 or all(failures)
|
||||
@@ -0,0 +1,60 @@
|
||||
from openpilot.cereal import custom
|
||||
from opendbc.car.structs import car
|
||||
from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState, long_control_state_trans
|
||||
|
||||
|
||||
|
||||
|
||||
class TestLongControlStateTransition:
|
||||
|
||||
def test_stay_stopped(self):
|
||||
CP = car.CarParams.new_message()
|
||||
CP_SP = custom.CarParamsSP.new_message()
|
||||
active = True
|
||||
current_state = LongCtrlState.stopping
|
||||
next_state = long_control_state_trans(CP, CP_SP, active, current_state, v_ego=0.1,
|
||||
should_stop=True, brake_pressed=False, cruise_standstill=False)
|
||||
assert next_state == LongCtrlState.stopping
|
||||
next_state = long_control_state_trans(CP, CP_SP, active, current_state, v_ego=0.1,
|
||||
should_stop=False, brake_pressed=True, cruise_standstill=False)
|
||||
assert next_state == LongCtrlState.stopping
|
||||
next_state = long_control_state_trans(CP, CP_SP, active, current_state, v_ego=0.1,
|
||||
should_stop=False, brake_pressed=False, cruise_standstill=True)
|
||||
assert next_state == LongCtrlState.stopping
|
||||
next_state = long_control_state_trans(CP, CP_SP, active, current_state, v_ego=1.0,
|
||||
should_stop=False, brake_pressed=False, cruise_standstill=False)
|
||||
assert next_state == LongCtrlState.pid
|
||||
active = False
|
||||
next_state = long_control_state_trans(CP, CP_SP, active, current_state, v_ego=1.0,
|
||||
should_stop=False, brake_pressed=False, cruise_standstill=False)
|
||||
assert next_state == LongCtrlState.off
|
||||
|
||||
def test_engage():
|
||||
CP = car.CarParams.new_message()
|
||||
CP_SP = custom.CarParamsSP.new_message()
|
||||
active = True
|
||||
current_state = LongCtrlState.off
|
||||
next_state = long_control_state_trans(CP, CP_SP, active, current_state, v_ego=0.1,
|
||||
should_stop=True, brake_pressed=False, cruise_standstill=False)
|
||||
assert next_state == LongCtrlState.stopping
|
||||
next_state = long_control_state_trans(CP, CP_SP, active, current_state, v_ego=0.1,
|
||||
should_stop=False, brake_pressed=True, cruise_standstill=False)
|
||||
assert next_state == LongCtrlState.stopping
|
||||
next_state = long_control_state_trans(CP, CP_SP, active, current_state, v_ego=0.1,
|
||||
should_stop=False, brake_pressed=False, cruise_standstill=True)
|
||||
assert next_state == LongCtrlState.stopping
|
||||
next_state = long_control_state_trans(CP, CP_SP, active, current_state, v_ego=0.1,
|
||||
should_stop=False, brake_pressed=False, cruise_standstill=False)
|
||||
assert next_state == LongCtrlState.pid
|
||||
|
||||
def test_starting():
|
||||
CP = car.CarParams.new_message(startingState=True, vEgoStarting=0.5)
|
||||
CP_SP = custom.CarParamsSP.new_message()
|
||||
active = True
|
||||
current_state = LongCtrlState.starting
|
||||
next_state = long_control_state_trans(CP, CP_SP, active, current_state, v_ego=0.1,
|
||||
should_stop=False, brake_pressed=False, cruise_standstill=False)
|
||||
assert next_state == LongCtrlState.starting
|
||||
next_state = long_control_state_trans(CP, CP_SP, active, current_state, v_ego=1.0,
|
||||
should_stop=False, brake_pressed=False, cruise_standstill=False)
|
||||
assert next_state == LongCtrlState.pid
|
||||
@@ -0,0 +1,73 @@
|
||||
import numpy as np
|
||||
from openpilot.cereal import messaging
|
||||
from opendbc.car.structs import car
|
||||
from opendbc.car import ACCELERATION_DUE_TO_GRAVITY
|
||||
from opendbc.car import structs
|
||||
from opendbc.car.lateral import get_friction, FRICTION_THRESHOLD
|
||||
from openpilot.common.realtime import DT_MDL
|
||||
from openpilot.selfdrive.locationd.torqued import TorqueEstimator, MIN_BUCKET_POINTS, POINTS_PER_BUCKET, STEER_BUCKET_BOUNDS
|
||||
|
||||
np.random.seed(0)
|
||||
|
||||
LA_ERR_STD = 1.0
|
||||
INPUT_NOISE_STD = 0.08
|
||||
V_EGO = 30.0
|
||||
|
||||
WARMUP_BUCKET_POINTS = (1.5*MIN_BUCKET_POINTS).astype(int)
|
||||
STRAIGHT_ROAD_LA_BOUNDS = (0.02, 0.03)
|
||||
|
||||
ROLL_BIAS_DEG = 2.0
|
||||
ROLL_COMPENSATION_BIAS = ACCELERATION_DUE_TO_GRAVITY*float(np.sin(np.deg2rad(ROLL_BIAS_DEG)))
|
||||
TORQUE_TUNE = structs.CarParams.LateralTorqueTuning(latAccelFactor=2.0, latAccelOffset=0.0, friction=0.2)
|
||||
TORQUE_TUNE_BIASED = structs.CarParams.LateralTorqueTuning(latAccelFactor=2.0, latAccelOffset=-ROLL_COMPENSATION_BIAS, friction=0.2)
|
||||
|
||||
def generate_inputs(torque_tune, la_err_std, input_noise_std=None):
|
||||
rng = np.random.default_rng(0)
|
||||
steer_torques = np.concat([rng.uniform(bnd[0], bnd[1], pts) for bnd, pts in zip(STEER_BUCKET_BOUNDS, WARMUP_BUCKET_POINTS, strict=True)])
|
||||
la_errs = rng.normal(scale=la_err_std, size=steer_torques.size)
|
||||
frictions = np.array([get_friction(la_err, 0.0, FRICTION_THRESHOLD, torque_tune) for la_err in la_errs])
|
||||
lat_accels = torque_tune.latAccelFactor*steer_torques + torque_tune.latAccelOffset + frictions
|
||||
if input_noise_std is not None:
|
||||
steer_torques += rng.normal(scale=input_noise_std, size=steer_torques.size)
|
||||
lat_accels += rng.normal(scale=input_noise_std, size=steer_torques.size)
|
||||
return steer_torques, lat_accels
|
||||
|
||||
def get_warmed_up_estimator(steer_torques, lat_accels):
|
||||
est = TorqueEstimator(car.CarParams())
|
||||
for steer_torque, lat_accel in zip(steer_torques, lat_accels, strict=True):
|
||||
est.filtered_points.add_point(steer_torque, lat_accel)
|
||||
return est
|
||||
|
||||
def simulate_straight_road_msgs(est):
|
||||
carControl = messaging.new_message('carControl').carControl
|
||||
carOutput = messaging.new_message('carOutput').carOutput
|
||||
carState = messaging.new_message('carState').carState
|
||||
livePose = messaging.new_message('livePose').livePose
|
||||
carControl.latActive = True
|
||||
carState.vEgo = V_EGO
|
||||
carState.steeringPressed = False
|
||||
ts = DT_MDL*np.arange(2*POINTS_PER_BUCKET)
|
||||
steer_torques = np.concat((np.linspace(-0.03, -0.02, POINTS_PER_BUCKET), np.linspace(0.02, 0.03, POINTS_PER_BUCKET)))
|
||||
lat_accels = TORQUE_TUNE.latAccelFactor * steer_torques
|
||||
for t, steer_torque, lat_accel in zip(ts, steer_torques, lat_accels, strict=True):
|
||||
carOutput.actuatorsOutput.torque = float(-steer_torque)
|
||||
livePose.orientationNED = {'x': float(np.deg2rad(ROLL_BIAS_DEG)), 'valid': True}
|
||||
livePose.angularVelocityDevice = {'z': float(lat_accel / V_EGO), 'valid': True}
|
||||
livePose.inputsOK, livePose.sensorsOK, livePose.posenetOK = True, True, True
|
||||
livePose.timestamp = int(t * 1e9)
|
||||
for which, msg in (('carControl', carControl), ('carOutput', carOutput), ('carState', carState), ('livePose', livePose)):
|
||||
est.handle_log(t, which, msg)
|
||||
|
||||
def test_estimated_offset():
|
||||
steer_torques, lat_accels = generate_inputs(TORQUE_TUNE_BIASED, la_err_std=LA_ERR_STD, input_noise_std=INPUT_NOISE_STD)
|
||||
est = get_warmed_up_estimator(steer_torques, lat_accels)
|
||||
msg = est.get_msg()
|
||||
# TODO add lataccelfactor and friction check when we have more accurate estimates
|
||||
assert abs(msg.liveTorqueParameters.latAccelOffsetRaw - TORQUE_TUNE_BIASED.latAccelOffset) < 0.1
|
||||
|
||||
def test_straight_road_roll_bias():
|
||||
steer_torques, lat_accels = generate_inputs(TORQUE_TUNE, la_err_std=LA_ERR_STD, input_noise_std=INPUT_NOISE_STD)
|
||||
est = get_warmed_up_estimator(steer_torques, lat_accels)
|
||||
simulate_straight_road_msgs(est)
|
||||
msg = est.get_msg()
|
||||
assert (msg.liveTorqueParameters.latAccelOffsetRaw < -0.05) and np.isfinite(msg.liveTorqueParameters.latAccelOffsetRaw)
|
||||
Reference in new issue
Block a user