o
|
ÿ' i= ã @ sZ d dl Zd dlm mZ d dlZd dlZd dlm Z dd Z
|
dd Z dd Z d d
|
Z dS ) é N)ÚDifferentialDriveModelc
|
|
C sÞ t } | ¡ | jdddd}|j}tj}d}d}|||d}||k}|svt d|fd||f¡dt ¡ v s:t
|
|¡r?t |¡ndt |¡d t ¡ v sOt
|
t¡rTt t¡nd t |¡t |¡t |¡t |¡d
|
}d d |i } t t | ¡d } } } } }}|j}tj}d}d}|||d }||k}|sét d|fd||f¡dt ¡ v st
|
|¡r²t |¡ndt |¡d t ¡ v sÂt
|
t¡rÇt t¡nd t |¡t |¡t |¡t |¡d
|
}d d |i } t t | ¡d } } } } }}|j}tj}d}d}|||d }||k}|sat d|fd||f¡dt ¡ v s#t
|
|¡r(t |¡ndt |¡d t ¡ v s:t
|
t¡r?t t¡nd t |¡t |¡t |¡t |¡d
|
}d d |i } t t | ¡d } } } } }}d S )Nç ð?ç ©Z target_linearZtarget_angularÚdtç©?)Úrel©ú==)zv%(py2)s
|
{%(py2)s = %(py0)s.east
|
} == %(py12)s
|
{%(py12)s = %(py6)s
|
{%(py6)s = %(py4)s.approx
|
}(%(py8)s, rel=%(py10)s)
|
}ÚstateÚpytest)Úpy0Úpy2Úpy4Úpy6Úpy8Úpy10Zpy12zassert %(py14)sZpy14©Úabs)zw%(py2)s
|
{%(py2)s = %(py0)s.north
|
} == %(py12)s
|
{%(py12)s = %(py6)s
|
{%(py6)s = %(py4)s.approx
|
}(%(py8)s, abs=%(py10)s)
|
}gü©ñÒMbP?)zy%(py2)s
|
{%(py2)s = %(py0)s.heading
|
} == %(py12)s
|
{%(py12)s = %(py6)s
|
{%(py6)s = %(py4)s.approx
|
}(%(py8)s, abs=%(py10)s)
|
})r ÚresetÚstepZeastr ÚapproxÚ
|
@pytest_arÚ_call_reprcompareÚ @py_builtinsÚlocalsÚ_should_repr_global_nameÚ _safereprÚAssertionErrorÚ_format_explanationZnorthÚheading)
|
Úmodelr Ú @py_assert1Ú @py_assert5Ú @py_assert7Ú @py_assert9Z @py_assert11Ú @py_assert3Z @py_format13Z @py_format15© r' ú;E:\GIT\Lawnmower_STM32H7\python\tests_hitl\test_dynamics.pyÚ%test_straight_motion_updates_position s ææôr) c C s t } | ¡ | jdt d¡dd}tj}|j}||}tj}d}d}|||d}||k} | st
|
d| fd||f¡dt ¡ v sDt t¡rIt t¡ndt |¡d t ¡ v sYt |¡r^t |¡nd t |¡t |¡d
|
t ¡ v srt t¡rwt t¡nd
|
t |¡t |¡t |¡t |¡d
|
}
|
d d |
|
i } tt | ¡d } } } } } } }}|j} t| }d} || k }|st
|
d|fd|| f¡dt ¡ v sÎt t¡rÓt t¡ndd t ¡ v sßt |¡rät |¡nd t | ¡t |¡t | ¡d }dd|i }tt |¡d } } }} d S )Nr g V@r r r r )z·%(py7)s
|
{%(py7)s = %(py2)s
|
{%(py2)s = %(py0)s.degrees
|
}(%(py5)s
|
{%(py5)s = %(py3)s.heading
|
})
|
} == %(py17)s
|
{%(py17)s = %(py11)s
|
{%(py11)s = %(py9)s.approx
|
}(%(py13)s, abs=%(py15)s)
|
}Úmathr r )
|
r r Úpy3Úpy5Úpy7Zpy9Úpy11Zpy13Zpy15Zpy17zassert %(py19)sZpy19çíµ ÷Æ°>©ú<)zT%(py5)s
|
{%(py5)s = %(py0)s(%(py3)s
|
{%(py3)s = %(py1)s.linear_velocity
|
})
|
} < %(py8)sr )r Úpy1r+ r, r úassert %(py10)sr )r r r r* ÚradiansÚdegreesr r r r r r r r r r r Úlinear_velocityr )r! r r" Ú @py_assert4Ú @py_assert6Z @py_assert10Z @py_assert12Z @py_assert14Z @py_assert16Ú @py_assert8Z @py_format18Z @py_format20Ú @py_assert2r$ Ú @py_format9Ú @py_format11r' r' r( Ú&test_in_place_rotation_changes_heading s þ0 Ær= c C sò t ddd} | ¡ | jdddd}|j}| j}d}|| }||k}|smt d|fd ||f¡d
|
t ¡ v s9t |¡r>t
|
|¡nd
|
t
|
|¡d t ¡ v sNt | ¡rSt
|
| ¡nd t
|
|¡t
|
|¡d }d d|i }t t |¡d } } } }}d S )Ng à?)Úmax_linear_speedZmax_linear_accelg @r gÉ?r r/ )ú<=)zi%(py2)s
|
{%(py2)s = %(py0)s.linear_velocity
|
} <= (%(py6)s
|
{%(py6)s = %(py4)s.max_linear_speed
|
} + %(py8)s)r r! )r r r r r zassert %(py11)sr. ) r r r r6 r> r r r r r r r r ) r! r r" r# r$ r% r& Z @py_format10Z @py_format12r' r' r( Ú test_velocity_limits_respected s Îr@ c C s< t } | ¡ | jdddd}|j\}}}d}| }||k }d}| } || k }
|
|r+|
|
sat d||
|
fd||| f¡t |¡dt ¡ v sFt |¡rKt |¡ndt |¡d } d
|
d | i } t
|
t | ¡d } } } }
|
}} t |}d } || k }
|
|
|
s¾t d |
|
fd|| f¡dt ¡ v st t ¡rt t ¡nddt ¡ v st |¡r¤t |¡ndt |¡t | ¡d }dd|i } t
|
t | ¡d } }
|
} t |}d } || k }
|
|
|
st d |
|
fd|| f¡dt ¡ v sæt t ¡rët t ¡nddt ¡ v s÷t |¡rüt |¡ndt |¡t | ¡d }dd|i } t
|
t | ¡d } }
|
} d S )Nr g¹?r gÍÌÌÌÌÌð?gffffffî?)r1 r1 )z-%(py1)s < %(py5)sz%(py5)s < -%(py7)sÚaz)r2 r, r- r3 r r r0 )z/%(py3)s
|
{%(py3)s = %(py0)s(%(py1)s)
|
} < %(py6)sr Úax)r r2 r+ r zassert %(py8)sr Úay) r r r Z body_accel_gr r r r r r r r r )r! r rB rC rA Z @py_assert0r: r& r8 r9 r7 r; r< r# Ú @py_format7r' r' r( Ú'test_body_acceleration_contains_gravity s °®´rE )Úbuiltinsr Ú_pytest.assertion.rewriteÚ assertionÚrewriter r* r Z hitl.dynamicsr r) r= r@ rE r' r' r' r( Ú<module> s "
|
