#NEWLINE# def step( self ) : #NEWLINE# #INDENT# ( theta , thetad , omega , omegad , _ , xf , yf , xb , yb , psi ) = self . sensors #NEWLINE# ( T , d ) = self . actions #NEWLINE# if self . _save_wheel_contact_trajectories : #NEWLINE# #INDENT# self . xfhist . append ( xf ) #NEWLINE# self . yfhist . append ( yf ) #NEWLINE# self . xbhist . append ( xb ) #NEWLINE# self . ybhist . append ( yb ) #NEWLINE# #UNINDENT# if ( theta == 0 ) : #NEWLINE# #INDENT# rf = 100000000.0 #NEWLINE# rb = 100000000.0 #NEWLINE# rCM = 100000000.0 #NEWLINE# #UNINDENT# else : #NEWLINE# #INDENT# rf = ( self . L / np . abs ( sin ( theta ) ) ) #NEWLINE# rb = ( self . L / np . abs ( tan ( theta ) ) ) #NEWLINE# rCM = sqrt ( ( ( ( self . L - self . c ) ** 2 ) + ( ( self . L ** 2 ) / ( tan ( theta ) ** 2 ) ) ) ) #NEWLINE# #UNINDENT# phi = ( omega + np . arctan ( ( d / self . h ) ) ) #NEWLINE# omegadd = ( ( 1 / self . Itot ) * ( ( ( ( self . M * self . h ) * self . g ) * sin ( phi ) ) - ( cos ( phi ) * ( ( ( self . Idc * self . sigmad ) * thetad ) + ( ( sign ( theta ) * ( self . v ** 2 ) ) * ( ( ( self . Md * self . r ) * ( ( 1.0 / rf ) + ( 1.0 / rb ) ) ) + ( ( self . M * self . h ) / rCM ) ) ) ) ) ) ) #NEWLINE# thetadd = ( ( omegad - ( ( self . Idv * self . sigmad ) * omegad ) ) / self . Idl ) #NEWLINE# omegad += ( omegadd * self . time_step ) #NEWLINE# omega += ( omegad * self . time_step ) #NEWLINE# thetad += ( thetadd * self . time_step ) #NEWLINE# theta += ( thetad * self . time_step ) #NEWLINE# theta = np . clip ( theta , ( - 1.3963 ) , 1.3963 ) #NEWLINE# front_temp = ( ( self . v * self . time_step ) / ( 2 * rf ) ) #NEWLINE# if ( front_temp > 1 ) : #NEWLINE# #INDENT# front_temp = ( ( sign ( ( psi + theta ) ) * 0.5 ) * np . pi ) #NEWLINE# #UNINDENT# else : #NEWLINE# #INDENT# front_temp = ( sign ( ( psi + theta ) ) * arcsin ( front_temp ) ) #NEWLINE# #UNINDENT# xf += ( ( self . v * self . time_step ) * ( - sin ( ( ( psi + theta ) + front_temp ) ) ) ) #NEWLINE# yf += ( ( self . v * self . time_step ) * cos ( ( ( psi + theta ) + front_temp ) ) ) #NEWLINE# back_temp = ( ( self . v * self . time_step ) / ( 2 * rb ) ) #NEWLINE# if ( back_temp > 1 ) : #NEWLINE# #INDENT# back_temp = ( ( np . sign ( psi ) * 0.5 ) * np . pi ) #NEWLINE# #UNINDENT# else : #NEWLINE# #INDENT# back_temp = ( np . sign ( psi ) * np . arcsin ( back_temp ) ) #NEWLINE# #UNINDENT# xb += ( ( self . v * self . time_step ) * ( - sin ( ( psi + back_temp ) ) ) ) #NEWLINE# yb += ( ( self . v * self . time_step ) * cos ( ( psi + back_temp ) ) ) #NEWLINE# current_wheelbase = sqrt ( ( ( ( xf - xb ) ** 2 ) + ( ( yf - yb ) ** 2 ) ) ) #NEWLINE# if ( np . abs ( ( current_wheelbase - self . L ) ) > 0.01 ) : #NEWLINE# #INDENT# relative_error = ( ( self . L / current_wheelbase ) - 1.0 ) #NEWLINE# xb += ( ( xb - xf ) * relative_error ) #NEWLINE# yb += ( ( yb - yf ) * relative_error ) #NEWLINE# #UNINDENT# delta_y = ( yf - yb ) #NEWLINE# if ( ( xf == xb ) and ( delta_y < 0.0 ) ) : #NEWLINE# #INDENT# psi = np . pi #NEWLINE# #UNINDENT# else : #NEWLINE# #INDENT# if ( delta_y > 0.0 ) : #NEWLINE# #INDENT# psi = arctan ( ( ( xb - xf ) / delta_y ) ) #NEWLINE# #UNINDENT# else : #NEWLINE# #INDENT# psi = ( ( ( sign ( ( xb - xf ) ) * 0.5 ) * np . pi ) - arctan ( ( delta_y / ( xb - xf ) ) ) ) #NEWLINE# #UNINDENT# #UNINDENT# self . sensors = np . array ( [ theta , thetad , omega , omegad , omegadd , xf , yf , xb , yb , psi ] )