def test_normalize_state():
ackermann_model = AckermannModel(1, 1)
# As the cpp side tests the actual computation, we just check that the
# normalize_state interface works.
assert ackermann_model.normalize_state(robot_state=State(4, 0)) == State(
4, 0)
def test_invalid_construction():
# Width and length must both be positive.
with pytest.raises(ValueError):
_ = AckermannModel(0.0, 2.0)
with pytest.raises(ValueError):
_ = AckermannModel(2.0, 0.0)
with pytest.raises(ValueError):
_ = AckermannModel(-1.0, 2.0)
with pytest.raises(ValueError):
_ = AckermannModel(1.0, -2.0)
def test_default_drawing_kernels(generate_visualizer_and_canvas):
robot_visualizer, canvas = generate_visualizer_and_canvas
# Make sure that visualization works for the default robot models
# through the default drawing kernels.
robots = [
Robot(AckermannModel(1.0, 1.0)),
Robot(DiffdriveModel(1.0, 1.0)),
Robot(DubinModel(1.0)),
Robot(TricycleModel(1.0, 1.0)),
]
for uid, robot in enumerate(robots):
robot_visualizer.visualize(canvas, robot, uid)
def test_add_drawing_kernel(generate_visualizer_and_canvas):
robot_visualizer, canvas = generate_visualizer_and_canvas
# Define the drawing kernel.
def _drawing_kernel(canvas, robot, resolution):
paint_canvas(canvas, (1, 1, 1))
robot = Robot(AckermannModel(1.0, 1.0))
robot_visualizer.add_drawing_kernel(0, _drawing_kernel)
# Make sure that the right kernel gets used.
# As an explicit kernel has been added, it should not be using the
# default model kernel.
robot_visualizer.visualize(canvas, robot, 0)
# The canvas should be painted (0, 0, 0)
assert np.allclose(canvas, np.ones_like(canvas))
def generate_playground_visualizer():
env_map = Map(width=20.0, height=20.0, resolution=0.02)
# Create the playground.
playground_spec = PlaygroundSpec(name="playground_spec", dt=0.01)
playground = Playground(playground_spec, env_map)
robot = Robot(AckermannModel(1.0, 1.0))
# Construct the pilot.
pilot = ZeroPilot(ZeroController(2))
# Add the robot to the playground.
playground.add_robot(robot, pilot, IntegratorType.EULER_INTEGRATOR, uid=0)
# Create the playground visualizer.
spec = PlaygroundVisualizerSpec(display_ratio=1.0)
playground_visualizer = PlaygroundVisualizer(spec, playground)
return playground_visualizer
def test_add_robot_drawing_kernel(generate_playground_visualizer):
playground_visualizer = generate_playground_visualizer
# Kernel to add.
def _drawing_kernel(canvas, robot, resolution):
paint_canvas(canvas, (1, 1, 1))
# Make sure that the added kernel is used (instead of the default one).
canvas = create_empty_canvas((500, 500))
playground_visualizer.add_robot_drawing_kernel(0, _drawing_kernel)
playground_visualizer.robot_visualizer.visualize(
canvas, Robot(AckermannModel(1.0, 1.0)), 0)
assert np.allclose(canvas, np.ones_like(canvas))
# Test invalid kernel.
def _invalid_drawing_kernel(canvas, robot):
paint_canvas(canvas, (0, 0, 0))
with pytest.raises(MorphacVisualizationError):
playground_visualizer.add_robot_drawing_kernel(
1, _invalid_drawing_kernel)
)
HELP_PROMPT = "Type of robot to use. The available options are: \n1. ackermann\n2. diffdrive\n3. dubin\n4. tricycle"
class RobotType(Enum):
ACKERMANN = "ackermann"
DIFFDRIVE = "diffdrive"
DUBIN = "dubin"
TRICYCLE = "tricycle"
robot_bank = {
RobotType.ACKERMANN:
Robot(
kinematic_model=AckermannModel(width=1.0, length=3.0),
initial_state=State([10.0, 5.0, 0.0, np.pi / 6], []),
),
RobotType.DIFFDRIVE:
Robot(
kinematic_model=DiffdriveModel(radius=0.3, width=1.0),
initial_state=State([10.0, 5.0, 0.0], []),
),
RobotType.DUBIN:
Robot(kinematic_model=DubinModel(speed=1.0),
initial_state=State([10.0, 5.0, 0.0], [])),
RobotType.TRICYCLE:
Robot(
kinematic_model=TricycleModel(width=1.0, length=2.0),
initial_state=State([10.0, 5.0, 0.0, np.pi / 8], []),
),
def generate_ackermann_model_list():
a1 = AckermannModel(1, 2)
a2 = AckermannModel(width=1.5, length=6.0)
return a1, a2