yincheng.zhong
2025-11-24 275b03224aa6170d4dc8c661c1cd949dd88c1fcb
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
o
Ç$iŠã@srdZddlmZddlZddlZddlZddlmZdZdd d „Z ddd„Z
ej Gdd„dƒƒZ Gdd„dƒZ dS)u*
差速履带运动学/动力学模型。
é)Ú annotationsN)ÚTupleg£’:#@ÚvalueÚfloatÚ  min_valueÚ  max_valueÚreturncCst|t||ƒƒS©N)ÚmaxÚmin)rrr©r ú0E:\GIT\Lawnmower_STM32H7\PYTHON\hitl\dynamics.pyÚ_clampsrÚradcCsFtj}|tjkr||8}|tjks|tj kr!||7}|tj ks|S)zWrap angle to [-pi, pi).)ÚmathÚtauÚpi)rZpi2r r r Ú _wrap_angles
 
ÿ  ÿrc@sÀeZdZUdZded<dZded<dZded<dZded<dZded<dZ   ded<dZ
ded   <dZ ded
<dZ ded <dZ ded <dZded <dZded<dZded<dZded<ddd„ZdS)ÚDifferentialDriveStateçrÚeastÚnorthÚupÚheadingÚlinear_velocityÚangular_velocityÚ  pitch_degÚroll_degÚ east_velocityÚnorth_velocityÚ up_velocity)rrçð¿zTuple[float, float, float]Ú body_accel_g)rrrÚ
gyro_deg_sg>@Ú temperature_crú'DifferentialDriveState'cCs
t |¡Sr  )Ú dataclassesÚreplace)Úselfr r r Úcopy.s
zDifferentialDriveState.copyN)rr%)Ú__name__Ú
__module__Ú __qualname__rÚ__annotations__rrrrrrrrrr r"r#r$r)r r r r rs 
             rc@sHeZdZdZddde d¡e d¡fdd d„Zdd dd„Zd!dd„ZdS)"ÚDifferentialDriveModelu6准静态差速履带模型,可模拟原地转向。gš™™™™™é?g@gø?g^@g€f@Ú track_widthrÚmax_linear_speedÚmax_linear_accelÚmax_angular_speedÚmax_angular_accelcCs6||_||_||_||_||_tƒ|_t d¡|_ dS)Né*)
r/r0r1r2r3rÚstateÚrandomZRandomÚ_rng)r(r/r0r1r2r3r r r Ú__init__5szDifferentialDriveModel.__init__rrrrÚ heading_degcCst|||t |¡d|_dS)N)rrrr)rrÚradiansr5)r(rrrr9r r r ÚresetEszDifferentialDriveModel.resetÚ target_linearÚtarget_angularÚdtrrcCsB|dkr|jSt||j |jƒ}t||j |jƒ}||jj}|j|}t|| |ƒ}||}|jj|7_| }||jj}|j|}    t||    | ƒ}||}
|jj|7_t|jj |jj|ƒ|j_ t
  |jj ¡} t
  |jj ¡} |jj |jj| |7_ |jj|jj| |7_|jjd7_|jj| |j_|jj| |j_d|j_|jj|jj} |t}| t}d}||j dd¡||j dd¡||j dd¡f|j_t
 t
 |d¡¡|j_t
 t
 | d¡¡|j_|j dd¡|j dd¡t
 |jj¡ |j dd¡f|j_|j dd¡}|jj|7_|jS)Nrr!gü©ñÒMb`?gü©ñÒMb@?gð?g{®Gáz„?gš™™™™™¹?)r5rr0r2rr1rr3rrrÚcosÚsinrrrrrr ÚGRAVITYr7Zgaussr"ÚdegreesÚatan2rrr#r$)r(r<r=r>ZdvZmax_dvZ
linear_accZtarget_angular_mathZdwZmax_dwZ angular_accZcos_hZsin_hZ lateral_accZax_gZay_gZaz_gZ
temp_driftr r r ÚstepHsR 
 
ý  ýzDifferentialDriveModel.stepN)
r/rr0rr1rr2rr3r)rrrr)rrrrrrr9r)r<rr=rr>rrr)   r*r+r,Ú__doc__rr:r8r;rDr r r r r.2s ú r.)rrrrrrrr)rrrr)rEÚ
__future__rr&rr6ÚtypingrrArrÚ dataclassrr.r r r r Ú<module>s