def test_simple_car(self):
simple_car = SimpleCar()
simulator = Simulator(simple_car)
context = simulator.get_mutable_context()
output = simple_car.AllocateOutput()
# Fix the input.
command = DrivingCommand()
command.set_steering_angle(0.5)
command.set_acceleration(1.)
context.FixInputPort(0, command)
# Verify the inputs.
command_eval = simple_car.EvalVectorInput(context, 0)
self.assertTrue(np.allclose(
command.get_value(), command_eval.get_value()))
# Initialize all the states to zero and take a simulation step.
state = context.get_mutable_continuous_state_vector()
state.SetFromVector([0.] * state.size())
simulator.StepTo(1.0)
# Verify the outputs.
simple_car.CalcOutput(context, output)
state_index = simple_car.state_output().get_index()
state_value = output.get_vector_data(state_index)
self.assertIsInstance(state_value, SimpleCarState)
self.assertTrue(
np.allclose(state.CopyToVector(), state_value.get_value()))
pose_index = simple_car.pose_output().get_index()
pose_value = output.get_vector_data(pose_index)
self.assertIsInstance(pose_value, PoseVector)
self.assertTrue(pose_value.get_translation()[0] > 0.)
velocity_index = simple_car.velocity_output().get_index()
velocity_value = output.get_vector_data(velocity_index)
self.assertIsInstance(velocity_value, FrameVelocity)
self.assertTrue(velocity_value.get_velocity().translational()[0] > 0.)
def test_simple_car(self):
simple_car = SimpleCar()
simulator = Simulator(simple_car)
context = simulator.get_mutable_context()
output = simple_car.AllocateOutput()
# Fix the input.
command = DrivingCommand()
command.set_steering_angle(0.5)
command.set_acceleration(1.)
context.FixInputPort(0, command)
# Verify the inputs.
command_eval = simple_car.EvalVectorInput(context, 0)
self.assertTrue(np.allclose(
command.get_value(), command_eval.get_value()))
# Initialize all the states to zero and take a simulation step.
state = context.get_mutable_continuous_state_vector()
state.SetFromVector([0.] * state.size())
simulator.StepTo(1.0)
# Verify the outputs.
simple_car.CalcOutput(context, output)
state_index = simple_car.state_output().get_index()
state_value = output.get_vector_data(state_index)
self.assertIsInstance(state_value, SimpleCarState)
self.assertTrue(
np.allclose(state.CopyToVector(), state_value.get_value()))
pose_index = simple_car.pose_output().get_index()
pose_value = output.get_vector_data(pose_index)
self.assertIsInstance(pose_value, PoseVector)
self.assertTrue(pose_value.get_translation()[0] > 0.)
velocity_index = simple_car.velocity_output().get_index()
velocity_value = output.get_vector_data(velocity_index)
self.assertIsInstance(velocity_value, FrameVelocity)
self.assertTrue(velocity_value.get_velocity().translational()[0] > 0.)