95 const double psi =
x.getPSI();
98 const double vx =
x.getvx();
99 const double omega =
x.getOmega();
101 const double psis = xref.
getPSI();
104 const double vxs = xref.
getvx();
105 const double omegas = xref.
getOmega();
106 const double axs = xref.
getAx();
107 const double deltas = xref.
getDelta();
109 const double eomega = omega - omegas;
110 const double ev = vx-vxs;
112 const double cpsi = (
std::cos)(psi);
113 const double spsi = (
std::sin)(psi);
114 const double cpsis = (
std::cos)(psis);
115 const double spsis = (
std::sin)(psis);
117 const double tcpsi = cpsis * cpsi + spsis * spsi;
118 const double tspsi = -spsis * cpsi + cpsis * spsi;
119 const double epsi = (
std::atan2)(tspsi,tcpsi);
146 double a_out = axs-kIx*
m_Ix-k0x*
m_ex-k1x*ev;
147 double delta_out = deltas -kIy*
m_Iy-key*
m_ey -kepsi*epsi -keomega*eomega;
Definition: linear_tracking_controller.h:39
LinearTrackingController(adore::params::APVehicle *pVehicle, adore::params::APTrajectoryTracking *pControl)
Definition: linear_tracking_controller.h:55
double m_Ix
lateral tracking error
Definition: linear_tracking_controller.h:45
double m_ex
Definition: linear_tracking_controller.h:43
double get_ex()
returns the longitudinal tracking error
Definition: linear_tracking_controller.h:64
adore::params::APTrajectoryTracking * m_pCtrlParameters
Definition: linear_tracking_controller.h:42
double m_ey
longitudinal tracking error
Definition: linear_tracking_controller.h:44
double get_ey()
returns the lateral tracking error
Definition: linear_tracking_controller.h:66
void compute_control_input(const VehicleMotionState9d &x, const PlanarVehicleState10d &xref, MotionCommand &u)
Definition: linear_tracking_controller.h:78
bool m_use_integrator
integrator in lateral direction
Definition: linear_tracking_controller.h:47
bool m_reset_integrator
Definition: linear_tracking_controller.h:48
adore::params::APVehicle * m_pVehParameters
Definition: linear_tracking_controller.h:41
void setUseIntegrator(bool value)
Definition: linear_tracking_controller.h:68
double m_Iy
integrator in longitudinal direction
Definition: linear_tracking_controller.h:46
void resetIntegrator(bool value)
Definition: linear_tracking_controller.h:70
Definition: motioncommand.h:26
void setAcceleration(double acceleration)
Definition: motioncommand.h:51
void setSteeringAngle(double steeringAngle)
Definition: motioncommand.h:41
abstract class containing parameters to configure the behaviour of the trajactory tracking controller
Definition: ap_trajectory_tracking.h:26
virtual double getDeltaMin() const =0
the minimum controllable steering angle
virtual double getEyStatic() const =0
static trajectory tracking offset in lateral direction, which should be compensated by tracking contr...
virtual double getKepsi() const =0
lateral control gain for yaw angle error epsi
virtual double getAxMax() const =0
hard coded maximum longitudinal acceleration
virtual double getK0x() const =0
returns P control gain for longitudinal direction
virtual double getDeltaMax() const =0
the maximum controllable steering angle
virtual double getK1x() const =0
returns D control gain for longitudinal direction
virtual double getKey() const =0
lateral control gain for lateral error ey
virtual double getAxMin() const =0
hard coded minimum longitudinal acceleration
virtual double getKIx() const =0
returns I control gain for longitudinal direction
virtual double getKeomega() const =0
lateral control gain for yaw rate error eomega
virtual double getExStatic() const =0
static trajectory tracking offset in longitudinal direction, which should be compensated by tracking ...
virtual double getKIy() const =0
returns I control gain for lateral direction
abstract class for vehicle configuration related paremeters
Definition: ap_vehicle.h:29
virtual double get_b() const =0
rear axle to cog
virtual double get_observationPointForPosition() const =0
interval< T > atan2(interval< T > y, interval< T > x)
Definition: intervalarithmetic.h:234
interval< T > cos(interval< T > x)
Definition: intervalarithmetic.h:225
T bound(T lb, T value, T ub)
Definition: adoremath.h:569
interval< T > sin(interval< T > x)
Definition: intervalarithmetic.h:204
x
Definition: adore_set_goal.py:30
Definition: areaofeffectconverter.h:20
This struct holds the motion state of the vehicle in 9d.
Definition: vehiclemotionstate9d.h:39
Definition: planarvehiclestate10d.h:41
double getvx() const
Definition: planarvehiclestate10d.h:65
double getX() const
Definition: planarvehiclestate10d.h:62
double getDelta() const
Definition: planarvehiclestate10d.h:69
double getPSI() const
Definition: planarvehiclestate10d.h:64
double getOmega() const
Definition: planarvehiclestate10d.h:67
double getY() const
Definition: planarvehiclestate10d.h:63
double getAx() const
Definition: planarvehiclestate10d.h:68