ADORe
ADORe is a modular open source software library and toolkit for decision making, planning, control and simulation of automated vehicles
roadcoordinates.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 
17 #include <adore/view/alane.h>
24 #include <adore/mad/oderk4.h>
25 
26 namespace adore
27 {
28  namespace fun
29  {
35  {
36  public:
37  double s0,s1,s2,s3;
38  double n0,n1,n2,n3;
39  bool valid;
40  };
41 
46  {
47  private:
50  adoreMatrix<double,0,1> dx_;
51  double a_;
52  double b_;
53  double L_;
54  double h_;
55  double g_;
56  double mu_;
57  double cf_;
58  double cr_;
59  double Iz_m_;
60  double rho_;
61 
62  private:
68  {
69  private:
73  adoreMatrix<double,0,1> dx_full_;
74  double s0_;
75  public:
81  adore::mad::AScalarToN<double,4>* lon_trajectory,
82  adore::mad::AScalarToN<double,4>* lat_trajectory,
83  double s0)
84  :parent_(parent),lon_trajectory_(lon_trajectory),lat_trajectory_(lat_trajectory),s0_(s0)
85  {
86  dx_full_.set_size(10,1);
87  }
88 
93  virtual void fh(double t, const adoreMatrix<double, 0, 1>& x_in, adoreMatrix<double, 0, 1>& dx_out,adoreMatrix<double,0,1>& y_out) override
94  {
95  auto s = lon_trajectory_->f(t);
96  auto n = lat_trajectory_->f(t);
97 
99  r.s0=s(0) + s0_;r.s1=s(1);r.s2=s(2);r.s3=s(3);
100  r.n0=n(0);r.n1=n(1);r.n2=n(2);r.n3=n(3);
101  //retrieve full state
102  PlanarVehicleState10d x_full;
103  parent_->toVehicleState(r,x_in(0),x_in(1),x_full);
104  // RoadCoordinates r_test = parent_->toRoadCoordinates(x_full);
105 
106  //full state derivative
107  parent_->model_.f(t,x_full.data,dx_full_);
108  //zero dynamics substate
109  dx_out(0) = dx_full_(2);
110  dx_out(1) = dx_full_(5);
111  //save full state in output
112  y_out = x_full.data;
113  }
114  };
115 
116  public:
124  :lfg_(lfg),model_(p)
125  {
126  updateParameters(p,tp);
127  }
128  bool isValid()
129  {
130  return lfg_->isValid();
131  }
136  {
138  dx_.set_size(10,1);
139  a_ = p->get_a();
140  b_ = p->get_b();
141  L_ = a_+b_;
142  h_ = p->get_h();
143  g_ = p->get_g();
144  mu_ = p->get_mu();
145  cf_ = p->get_cf();
146  cr_ = p->get_cr();
147  Iz_m_ = p->get_Iz_m();
148  rho_ = tp->get_rho();
149  }
150 
157  {
158  const double rho = rho_;
160  const double cpsi = std::cos(x.getPSI());
161  const double spsi = std::sin(x.getPSI());
162  model_.f(0.0,x.data,dx_);
163  const double vxp = x.getvx();
164  const double vyp = x.getvy() + x.getOmega() * rho;
165  const double ax = x.getAx();
166  const double ay = dx_(4,0) + dx_(5,0) * rho + x.getvx() * x.getOmega();
167  if(log)
168  {
169  std::cout<<"toRoadCoordinates: ay_q="<<ay<<std::endl;
170  std::cout<<"toRoadCoordinates: ay_cg="<<ay+dx_(5,0)*(b_-rho)<<std::endl;
171  }
172  const double px0 = x.getX() + cpsi * rho;
173  const double py0 = x.getY() + spsi * rho;
174  const double px1 = cpsi * vxp - spsi * vyp;
175  const double py1 = spsi * vxp + cpsi * vyp;
176  const double px2 = cpsi * ax - spsi * ay;
177  const double py2 = spsi * ax + cpsi * ay;
178  lfg_->toRelativeCoordinates(px0, py0, r.s0, r.n0);
179  r.valid = lfg_->getSMin()<r.s0 && r.s0<lfg_->getSMax();
180  const double theta = lfg_->getHeading(r.s0);
181  const double ctheta = std::cos(theta);
182  const double stheta = std::sin(theta);
183  const double kappa0 = lfg_->getCurvature(r.s0,0);
184  const double kappa1 = lfg_->getCurvature(r.s0,1);
185  r.s1 = ( ctheta * px1 + stheta * py1) / (1.0 - r.n0 * kappa0);
186  r.n1 = (-stheta * px1 + ctheta * py1);
187  r.s2 = ( ( ctheta * px2 + stheta * py2) + 2.0 * r.s1 * r.n1 * kappa0 + r.s1 * r.s1 * r.n0 * kappa1 ) / (1.0 - r.n0 * kappa0);
188  r.n2 = (-stheta * px2 + ctheta * py2) - r.s1 * r.s1 * kappa0 * (1.0 - r.n0 * kappa0);
189  r.s3 = 0.0;
190  r.n3 = 0.0;
191  return r;
192  }
202  void toVehicleState(const RoadCoordinates& r,double psi, double omega,PlanarVehicleState10d& x, bool log=false)
203  {
204  double px0,py0,pz0;
205  lfg_->toEucledianCoordinates(r.s0,r.n0,px0,py0,pz0);
206  const double theta = lfg_->getHeading(r.s0);
207  const double ctheta = std::cos(theta);
208  const double stheta = std::sin(theta);
209  const double kappa0 = lfg_->getCurvature(r.s0,0);
210  const double kappa1 = lfg_->getCurvature(r.s0,1);
211  const double pxr1 = r.s1 * (1.0-r.n0 * kappa0);
212  const double pyr1 = r.n1;
213  const double pxr2 = r.s2 * (1.0-r.n0 * kappa0) - 2.0 * r.s1 * r.n1 * kappa0 - r.s1 * r.s1 * r.n0 * kappa1;
214  const double pyr2 = r.n2 + r.s1 * r.s1 * kappa0 * (1.0 - r.n0 * kappa0);
215  const double px1 = ctheta * pxr1 - stheta * pyr1;
216  const double py1 = stheta * pxr1 + ctheta * pyr1;
217  const double px2 = ctheta * pxr2 - stheta * pyr2;
218  const double py2 = stheta * pxr2 + ctheta * pyr2;
219  const double cpsi = std::cos(psi);
220  const double spsi = std::sin(psi);
221  x.setX(px0 - cpsi * rho_);
222  x.setY(py0 - spsi * rho_);
223  x.setvx( cpsi * px1 + spsi * py1);
224  x.setvy(- spsi * px1 + cpsi * py1 - omega * rho_);
225  x.setPSI(psi);
226  x.setOmega(omega);
227  x.setAx( cpsi * px2 + spsi * py2);
228  const double ayq = -spsi * px2 + cpsi * py2;
229  if(x.getvx()>0.01)
230  {
231  //linear tire force model
232  const double k0 = -mu_*g_*cf_*b_/L_;
233  const double k1 = -mu_*g_*cr_*a_/L_;
234  const double fb = k1 * x.getvy()/x.getvx();
235  const double fa = (ayq-(1.0-(rho_-b_)*b_/Iz_m_)*fb) / (1.0+(rho_-b_)*a_/Iz_m_);
236  if(log)
237  {
238  std::cout<<"toVehicleState: ay_q="<<ayq<<std::endl;
239  std::cout<<"toVehicleState: ay_cg="<<fa+fb<<std::endl;
240  }
241  const double delta = (x.getvy()+L_*omega)/x.getvx()-fa/k0;
242  x.setDelta(delta);
243  }
244  else
245  {
246  //no low velocity model for formulation with lateral acceleration
247  x.setDelta(0.0);
248  }
249  x.setDAx(0.0);//not implemented
250  x.setDDelta(0.0);//not implemented
251  }
252 
263  adoreMatrix<double,0,0> toVehicleStateTrajectory(adore::mad::AScalarToN<double,4>* lon_trajectory_rc,
264  adore::mad::AScalarToN<double,4>* lat_trajectory_rc,
265  const adoreMatrix<double,1,0>& integration_time,
266  double s0,double psi0,double omega0)
267  {
268  ZeroDynamicsModel zd(this,lon_trajectory_rc,lat_trajectory_rc,s0);
269  adoreMatrix<double,0,0> Yode;//output
270  Yode.set_size(10,integration_time.nc());
271  adoreMatrix<double,2,1> x0;//the initial state
272  x0(0) = psi0;
273  x0(1) = omega0;
274 
275  //solve ode
277  ode4.solve_with_output(&zd,integration_time,x0,Yode);
278  //compute d/dt ax and d/dt delta by finite differences
279  for(int j=1;j<Yode.nc();j++)
280  {
281  int i=j-1;
282  double dt = integration_time(j)-integration_time(i);
283  Yode(8,i) = (Yode(6,j)-Yode(6,i))/dt;
284  Yode(9,i) = (Yode(7,j)-Yode(7,i))/dt;
285  }
286  return Yode;
287  }
288 
289  };
290  }
291 }
adoreMatrix< double, 0, 1 > dx_full_
Definition: roadcoordinates.h:73
adore::mad::AScalarToN< double, 4 > * lon_trajectory_
Definition: roadcoordinates.h:71
double s0_
Definition: roadcoordinates.h:74
RoadCoordinateConverter * parent_
Definition: roadcoordinates.h:70
adore::mad::AScalarToN< double, 4 > * lat_trajectory_
Definition: roadcoordinates.h:72
ZeroDynamicsModel(RoadCoordinateConverter *parent, adore::mad::AScalarToN< double, 4 > *lon_trajectory, adore::mad::AScalarToN< double, 4 > *lat_trajectory, double s0)
Definition: roadcoordinates.h:80
virtual void fh(double t, const adoreMatrix< double, 0, 1 > &x_in, adoreMatrix< double, 0, 1 > &dx_out, adoreMatrix< double, 0, 1 > &y_out) override
Definition: roadcoordinates.h:93
Definition: roadcoordinates.h:46
double b_
Definition: roadcoordinates.h:52
adoreMatrix< double, 0, 0 > toVehicleStateTrajectory(adore::mad::AScalarToN< double, 4 > *lon_trajectory_rc, adore::mad::AScalarToN< double, 4 > *lat_trajectory_rc, const adoreMatrix< double, 1, 0 > &integration_time, double s0, double psi0, double omega0)
Definition: roadcoordinates.h:263
double Iz_m_
Definition: roadcoordinates.h:59
void updateParameters(adore::params::APVehicle *p, adore::params::APTrajectoryGeneration *tp)
Definition: roadcoordinates.h:135
RoadCoordinateConverter(adore::view::ALane *lfg, adore::params::APVehicle *p, adore::params::APTrajectoryGeneration *tp)
Definition: roadcoordinates.h:123
VLB_OpenLoop model_
Definition: roadcoordinates.h:49
void toVehicleState(const RoadCoordinates &r, double psi, double omega, PlanarVehicleState10d &x, bool log=false)
Definition: roadcoordinates.h:202
double cf_
Definition: roadcoordinates.h:57
double mu_
Definition: roadcoordinates.h:56
double h_
Definition: roadcoordinates.h:54
double L_
Definition: roadcoordinates.h:53
double g_
Definition: roadcoordinates.h:55
adoreMatrix< double, 0, 1 > dx_
Definition: roadcoordinates.h:50
adore::view::ALane * lfg_
Definition: roadcoordinates.h:48
double cr_
Definition: roadcoordinates.h:58
RoadCoordinates toRoadCoordinates(const PlanarVehicleState10d &x, bool log=false)
Definition: roadcoordinates.h:156
bool isValid()
Definition: roadcoordinates.h:128
double a_
Definition: roadcoordinates.h:51
double rho_
Definition: roadcoordinates.h:60
Definition: vlb_openloop.h:32
virtual void f(double t, const adoreMatrix< double, 0, 1 > &x_in, adoreMatrix< double, 0, 1 > &dx_out)
Definition: vlb_openloop.h:106
void updateParameters(adore::params::APVehicle *p)
Definition: vlb_openloop.h:64
virtual CT f(DT x) const =0
Definition: aodemodel.h:53
virtual adoreMatrix< T > solve_with_output(AOdeModelWithOutput< T > *model, const adoreMatrix< double, 1, 0 > &time, const adoreMatrix< double, 0, 1 > &x0, adoreMatrix< double > &Y_out) override
Definition: oderk4.h:59
abstract class containing parameters to configure the behaviour of trajectory generation
Definition: ap_trajectory_generation.h:26
virtual double get_rho() const =0
cor to planning point: movement of planning point shall planned by the trajectory planner
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_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_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
Definition: alane.h:28
virtual void toRelativeCoordinates(double xe, double ye, double &s, double &n)=0
virtual double getSMin() const =0
virtual double getSMax() const =0
virtual void toEucledianCoordinates(double s, double n, double &xe, double &ye, double &ze)=0
virtual bool isValid() const =0
virtual double getCurvature(double s, int derivative)=0
virtual double getHeading(double s)=0
interval< T > cos(interval< T > x)
Definition: intervalarithmetic.h:225
interval< T > sin(interval< T > x)
Definition: intervalarithmetic.h:204
x0
Definition: adore_set_goal.py:25
x
Definition: adore_set_goal.py:30
r
Definition: adore_suppress_lanechanges.py:209
Definition: areaofeffectconverter.h:20
Definition: planarvehiclestate10d.h:41
adoreMatrix< double, 10, 1 > data
Definition: planarvehiclestate10d.h:61
Definition: roadcoordinates.h:35
double s1
Definition: roadcoordinates.h:37
double n0
longitudinal coordinate derivative 0 to 3
Definition: roadcoordinates.h:38
double n1
Definition: roadcoordinates.h:38
double s2
Definition: roadcoordinates.h:37
double n3
Definition: roadcoordinates.h:38
double s0
Definition: roadcoordinates.h:37
double n2
Definition: roadcoordinates.h:38
double s3
Definition: roadcoordinates.h:37
bool valid
lateral coordiante derivative 0 to 3
Definition: roadcoordinates.h:39