def dd_test():
radius = 0.1
radius_left = radius
radius_right = radius
wheel_distance = 0.5
dddp = DifferentialDriveDynamicsParameters(radius_left=radius_left, radius_right=radius_right,
wheel_distance=wheel_distance)
q0 = geo.SE2_from_translation_angle([0, 0], 0)
v0 = geo.se2.zero()
c0 = q0, v0
s0 = dddp.initialize(c0)
omega = 0.3
commands = WheelVelocityCommands(omega, omega)
dt = 4.0
s1 = s0.integrate(dt, commands)
q1, v1 = s1.TSE2_from_state()
print(geo.se2.friendly(v1))
print(geo.SE2.friendly(q1))
p1, theta = geo.translation_angle_from_SE2(q1)
assert_almost_equal(p1, [dt * radius * omega, 0])
omega_left = 0.3
omega_right = 0
commands = WheelVelocityCommands(omega_left, omega_right)
dt = 4.0
s1 = s0.integrate(dt, commands)
q1, v1 = s1.TSE2_from_state()
p1, theta = geo.translation_angle_from_SE2(q1)
print(geo.se2.friendly(v1))
print(geo.SE2.friendly(q1))
def reasonable_duckiebot():
radius = 0.1
radius_left = radius
radius_right = radius
wheel_distance = 0.5
dddp = DifferentialDriveDynamicsParameters(radius_left=radius_left,
radius_right=radius_right,
wheel_distance=wheel_distance)
return dddp