ADORe
ADORe is a modular open source software library and toolkit for decision making, planning, control and simulation of automated vehicles
vlb_openloop.h
Go to the documentation of this file.
1 /********************************************************************************
2  * Copyright (C) 2017-2020 German Aerospace Center (DLR).
3  * Eclipse ADORe, Automated Driving Open Research https://eclipse.org/adore
4  *
5  * This program and the accompanying materials are made available under the
6  * terms of the Eclipse Public License 2.0 which is available at
7  * http://www.eclipse.org/legal/epl-2.0.
8  *
9  * SPDX-License-Identifier: EPL-2.0
10  *
11  * Contributors:
12  * Daniel Heß - initial API and implementation
13  ********************************************************************************/
14 
15 #pragma once
16 #include <adore/mad/aodemodel.h>
19 
20 namespace adore
21 {
22  namespace fun
23  {
31  class VLB_OpenLoop:public adore::mad::AOdeModel<double>
32  {
33  private:
34  //parameters
35  double lambda_;
36  double a_;
37  double b_;
38  double L_;
39  double h_;
40  double g_;
41  double mu_;
42  double cf_;
43  double cr_;
44  double Iz_m_;
45  double delta_max_;
46  double delta_min_;
48 
49  public:
55  {
56  //parameters
57  lambda_ = 0.0;
59  }
65  {
66  a_ = p->get_a();
67  b_ = p->get_b();
68  L_ = a_+b_;
69  h_ = p->get_h();
70  g_ = p->get_g();
71  mu_ = p->get_mu();
72  cf_ = p->get_cf();
73  cr_ = p->get_cr();
74  Iz_m_ = p->get_Iz_m();
78  }
83  void setReference(double value)
84  {
85  lambda_=value;
86  }
92  {
93  gearState_=value;
94  }
95 
96 
97 
106  virtual void f(double t, const adoreMatrix<double, 0, 1>& x_in, adoreMatrix<double, 0, 1>& dx_out)
107  {
108  //states
109  const double PSI = x_in(2);
110  const double vx = x_in(3);
111  const double vyr = x_in(4)-lambda_*x_in(5);
112  // TODO investigate if the following is a bug or if the variable can be fully removed
113  // const double vyf = vyr+L_*x_in(5); // fix -Wunused-variable
114  const double vy = x_in(4);
115  const double omega = x_in(5);
116  double ax = x_in(6);
117  const double delta = adore::mad::bound(delta_min_,x_in(7),delta_max_);
118  const double dax = x_in(8);
119  const double ddelta = x_in(9);
120 
121  switch(gearState_)
122  {
124  if(vx<0.0 && ax<0.0)
125  {
126  ax = -ax;
127  }
128  break;
130  if(vx>0.0 && ax>0.0)
131  {
132  ax = -ax;
133  ax = -ax;
134  }
135  break;
137  if ((vx>0.0 && ax>0.0)
138  || ( vx<0.0 && ax<0.0))
139  {
140  ax = -0.05 * vx;
141  }
142  break;
144  if(vx!=0.0)
145  {
146  ax = -5.0 * vx;
147  }
148  break;
149 
150  }
151 
152 
153  if (vx > 1.0)
154  {
155  //slipangles
156  // TODO investigate the following variables are no longer used, usage commented out few lines below, can they be fully removed?
157  // double alpha_f = atan(vyf / vx) - delta; // -Wunused-variable
158  // double alpha_r = atan(vyr / vx); // -Wunused-variable
159  //forces
160  double Fyf_Fzf, Fyr_Fzr;
161  //Fyf_Fzf = -cos(delta)*mu_*(b_/L_*g_ - h_/L_*0.0)*cf_*alpha_f;
162  // Fyr_Fzr = -mu_*(a_ / L_*g_ + h_ / L_*0.0)*cr_*alpha_r;
163  Fyf_Fzf = -mu_*(b_/L_)*g_*cf_*((vyr+L_*omega)/vx-delta);
164  Fyr_Fzr = -mu_*(a_/L_)*g_*cr_*vyr/vx;
165  //accelerations
166  double ay = Fyf_Fzf + Fyr_Fzr;
167  double domega = (1.0 / Iz_m_) * (a_*Fyf_Fzf - b_*Fyr_Fzr);
168  //derivative - dynamic
169  dx_out(0) = cos(PSI)*vx - sin(PSI)*vy;
170  dx_out(1) = sin(PSI)*vx + cos(PSI)*vy;
171  dx_out(2) = omega;
172  dx_out(3) = ax;
173  dx_out(4) = ay - vx*omega + domega*(lambda_-b_);
174  dx_out(5) = domega;
175  dx_out(6) = dax;
176  dx_out(7) = ddelta;
177  dx_out(8) = 0;
178  dx_out(9) = 0;
179  }
180  else
181  {
182  //derivative - kinematic
183  double kappa = tan(delta) / L_;
184  dx_out(0) = cos(PSI)*vx - sin(PSI)*vy;
185  dx_out(1) = sin(PSI)*vx + cos(PSI)*vy;
186  dx_out(2) = vx*kappa;
187  dx_out(3) = ax;
188  dx_out(4) = -5.0 * (vy - vx*kappa*lambda_);
189  dx_out(5) = -5.0 * (omega - vx*kappa);
190  dx_out(6) = dax;
191  dx_out(7) = ddelta;
192  dx_out(8) = 0;
193  dx_out(9) = 0;
194  }
195  }
196  };
197  }
198 }
Definition: vlb_openloop.h:32
double cr_
Definition: vlb_openloop.h:43
double mu_
Definition: vlb_openloop.h:41
double a_
Definition: vlb_openloop.h:36
double delta_min_
Definition: vlb_openloop.h:46
double delta_max_
Definition: vlb_openloop.h:45
virtual void f(double t, const adoreMatrix< double, 0, 1 > &x_in, adoreMatrix< double, 0, 1 > &dx_out)
Definition: vlb_openloop.h:106
double g_
Definition: vlb_openloop.h:40
void setReference(double value)
Definition: vlb_openloop.h:83
double lambda_
Definition: vlb_openloop.h:35
void setGearState(VehicleExtendedState::GearState value)
Definition: vlb_openloop.h:91
VehicleExtendedState::GearState gearState_
Definition: vlb_openloop.h:47
VLB_OpenLoop(adore::params::APVehicle *p)
Definition: vlb_openloop.h:54
void updateParameters(adore::params::APVehicle *p)
Definition: vlb_openloop.h:64
double Iz_m_
Definition: vlb_openloop.h:44
double L_
Definition: vlb_openloop.h:38
double h_
Definition: vlb_openloop.h:39
double cf_
Definition: vlb_openloop.h:42
double b_
Definition: vlb_openloop.h:37
GearState
Definition: vehicleextendedstate.h:29
@ Drive
Definition: vehicleextendedstate.h:30
@ Park
Definition: vehicleextendedstate.h:30
@ Neutral
Definition: vehicleextendedstate.h:30
@ Reverse
Definition: vehicleextendedstate.h:30
Definition: aodemodel.h:30
abstract class for vehicle configuration related paremeters
Definition: ap_vehicle.h:29
virtual double get_h() const =0
cog height above ground
virtual double get_steeringAngleMin() const =0
virtual double get_mu() const =0
friction coefficient
virtual double get_cr() const =0
rear normalized tire stiffness for bicycle model
virtual double get_Iz_m() const =0
rotational inertia around up axis devided by mass
virtual double get_b() const =0
rear axle to cog
virtual double get_steeringAngleMax() const =0
virtual double get_a() const =0
cog to front axle
virtual double get_cf() const =0
front normalized tire stiffness for bicycle model
virtual double get_g() const =0
gravitational constant
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
Definition: areaofeffectconverter.h:20