50 adoreMatrix<double,0,1>
dx_;
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
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);
158 const double rho =
rho_;
160 const double cpsi =
std::cos(
x.getPSI());
161 const double spsi =
std::sin(
x.getPSI());
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();
169 std::cout<<
"toRoadCoordinates: ay_q="<<ay<<std::endl;
170 std::cout<<
"toRoadCoordinates: ay_cg="<<ay+
dx_(5,0)*(
b_-rho)<<std::endl;
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;
181 const double ctheta =
std::cos(theta);
182 const double stheta =
std::sin(theta);
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);
207 const double ctheta =
std::cos(theta);
208 const double stheta =
std::sin(theta);
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;
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_);
227 x.setAx( cpsi * px2 + spsi * py2);
228 const double ayq = -spsi * px2 + cpsi * py2;
234 const double fb = k1 *
x.getvy()/
x.getvx();
238 std::cout<<
"toVehicleState: ay_q="<<ayq<<std::endl;
239 std::cout<<
"toVehicleState: ay_cg="<<fa+fb<<std::endl;
241 const double delta = (
x.getvy()+
L_*omega)/
x.getvx()-fa/k0;
265 const adoreMatrix<double,1,0>& integration_time,
266 double s0,
double psi0,
double omega0)
269 adoreMatrix<double,0,0> Yode;
270 Yode.set_size(10,integration_time.nc());
271 adoreMatrix<double,2,1>
x0;
279 for(
int j=1;j<Yode.nc();j++)
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;
Definition: roadcoordinates.h:68
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
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