def test_add_robot_with_temporary(generate_playground):
# Test the function with temporarily created Robot and Pilot objects.
# This is to test for any cpp lifetime management weirdness.
playground = generate_playground
playground.add_robot(
Robot(DiffdriveModel(1.0, 1.0)),
CustomPilot([0, 0]),
IntegratorType.EULER_INTEGRATOR,
1,
)
playground.add_robot(
Robot(DiffdriveModel(1.0, 1.0)),
CustomPilot([1, 2]),
IntegratorType.MID_POINT_INTEGRATOR,
2,
)
# Make sure that the robots have been added correctly.
assert playground.state.num_robots == 2
assert np.allclose(
playground.state.get_robot_state(1).data, [0.0, 0.0, 0.0])
assert np.allclose(
playground.state.get_robot_state(2).data, [0.0, 0.0, 0.0])
def generate_robot_list():
r1 = Robot(DiffdriveModel(1.0, 1.0), Footprint([[0, 0]]))
r2 = Robot(
DiffdriveModel(2.0, 3.0), Footprint([[0, 0]]), State([1.0, 2.0, 3.0], [])
)
return r1, r2
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_execute_with_temporary(generate_playground):
# Test the function after adding robots using temporarily created Robot and Pilot objects.
# This is to test for any cpp lifetime management weirdness.
playground = generate_playground
playground.add_robot(
Robot(DiffdriveModel(1.0, 1.0)),
CustomPilot([0, 0]),
IntegratorType.EULER_INTEGRATOR,
1,
)
playground.add_robot(
Robot(DiffdriveModel(1.0, 1.0),
initial_state=State([1.0, 2.0, 0.0], [])),
CustomPilot([1, 1]),
IntegratorType.MID_POINT_INTEGRATOR,
2,
)
# Make sure that the playground time is 0.
assert playground.state.time == 0.0
# Executing a playground cycle.
playground.execute()
# Test the states of the robots.
# The first pilot gives zero control inputs, hence the state must remain
# unchanged.
assert np.allclose(
playground.state.get_robot_state(1).data, [0.0, 0.0, 0.0])
# The second robot receives a constant input of [1., 1.]. As the wheel
# radius = 1 m, the robot moves forward with a velocity of 1 m/s. Hence
# it moves a total distance of dt m in the x direction (The initial angle
# is zero).
assert np.allclose(
playground.state.get_robot_state(2).data,
[1.0 + playground.spec.dt, 2.0, 0.0])
# Make sure that the playground time has updated.
assert playground.state.time == playground.spec.dt
def test_invalid_default_drawing_kernels(generate_visualizer_and_canvas):
# Trying to invoke the default kernel on a non default kinematic
# model should throw an error.
robot_visualizer, canvas = generate_visualizer_and_canvas
# Create a robot with a non default kinematic model.
robot = Robot(CustomKinematicModel(1.0, 1.0))
with pytest.raises(MorphacVisualizationError):
robot_visualizer.visualize(canvas, robot, uid=0)
def test_add_drawing_kernel_to_custom_robot(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))
# Create a robot with a non default kinematic model.
robot = Robot(CustomKinematicModel(1.0, 1.0))
robot_visualizer.add_drawing_kernel(0, _drawing_kernel)
robot_visualizer.visualize(canvas, robot, 0)
assert np.allclose(canvas, np.ones_like(canvas))
def test_add_robot_with_temporary(generate_playground_state_list):
# Test the function with a temporarily created Robot object.
# This is to test for any cpp lifetime management weirdness.
ps1, _ = generate_playground_state_list
# Adding the robots.
ps1.add_robot(Robot(DiffdriveModel(1.0, 1.0)), uid=0)
assert ps1.num_robots == 1
# Adding another robot and testing.
ps1.add_robot(Robot(DiffdriveModel(1.0, 1.0)), 1)
assert ps1.num_robots == 2
# Test addition of robots with invalid or duplicate uids.
with pytest.raises(ValueError):
ps1.add_robot(Robot(DiffdriveModel(1.0, 1.0)), -1)
with pytest.raises(ValueError):
ps1.add_robot(Robot(DiffdriveModel(1.0, 1.0)), 0)
with pytest.raises(ValueError):
ps1.add_robot(Robot(DiffdriveModel(1.0, 1.0)), 1)
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 test_invalid_add_drawing_to_custom_robot(generate_visualizer_and_canvas):
# Test the visualize function invoked on an invalid uid on a custom robot.
robot_visualizer, canvas = generate_visualizer_and_canvas
# Define the drawing kernel.
def _drawing_kernel(canvas, robot, resolution):
paint_canvas(canvas, (1, 1, 1))
# Create a robot with a non default kinematic model.
robot = Robot(CustomKinematicModel(1.0, 1.0))
robot_visualizer.add_drawing_kernel(0, _drawing_kernel)
# Invoke visualize with an invalid
with pytest.raises(MorphacVisualizationError):
robot_visualizer.visualize(canvas, robot, 1)
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], []),
),
}