def setUp(self):
PybulletAPI.initialize(SimulationTime(1000), False)
PybulletAPI.gui = MagicMock(return_value=True)
# Use side_effect to spy on a method: Can assert calls but functionality doesn't change.
PybulletAPI.addUserDebugLine = MagicMock(
side_effect=PybulletAPI.addUserDebugLine)
self._debug_line = DebugLine(Vec3([0, 0, 0]), Vec3([0, 0, 0]))
def test_updating_when_time_has_passed(self):
# Mocking time to return number bigger than update frequency
with patch(
'time.time',
mock.MagicMock(return_value=DebugLine._MIN_UPDATE_INTERVAL +
1E-6)):
self._debug_line.update(Vec3([0, 0, 0]), Vec3([0, 0, 0]))
# Assert debug line is not called twice because cannot create line too frequent
self.assertEqual(2, PybulletAPI.addUserDebugLine.call_count)
def addUserDebugLine(lineFromXYZ: Vec3, lineToXYZ: Vec3, lineWidth: float, lineColorRGB: List[float],
parentObjectUniqueId: int = -1, replaceItemUniqueId: int = -1) -> int:
if PybulletAPI.gui():
return p.addUserDebugLine(lineFromXYZ=lineFromXYZ.as_nwu(),
lineToXYZ=lineToXYZ.as_nwu(),
lineWidth=lineWidth,
lineColorRGB=lineColorRGB,
parentObjectUniqueId=parentObjectUniqueId,
replaceItemUniqueId=replaceItemUniqueId)
def test_velocity(self):
simulator = Simulator(4000, gui=False)
PybulletAPI.loadURDF("plane.urdf", Vec3([0, 0, 30]))
simulator.create_robot()
simulator.robot.set_velocity(linear_velocity=Vec3([1, 1, 1]))
simulator.do_step()
previous_velocity = simulator.robot.get_velocity()
amount_sensor_updates = 0
for _ in range(500):
simulator.robot.set_velocity(linear_velocity=Vec3([1, 1, 1]))
actual_velocity = simulator.robot.get_velocity()
simulator.do_step()
sensor_data = simulator.robot.dvl.get_latest_value()
# Since the dvl runs
if sensor_data:
# Velocity_valid should always be true otherwise the dvl is too far away from the surface and this test wouldn't work.
self.assertTrue(sensor_data[1]['velocity_valid'])
amount_sensor_updates += 1
vx, vy, vz = sensor_data[1]['vx'], sensor_data[1]['vy'], sensor_data[1]['vz']
min_vx = min((actual_velocity[X], previous_velocity[X]))
max_vx = max((actual_velocity[X], previous_velocity[X]))
min_vy = min((actual_velocity[Y], previous_velocity[Y]))
max_vy = max((actual_velocity[Y], previous_velocity[Y]))
min_vz = min((actual_velocity[Z], previous_velocity[Z]))
max_vz = max((actual_velocity[Z], previous_velocity[Z]))
# Make sure the sensor velocities are between the actual velocities, since it is interpolated between
# the two
self.assertLessEqual(min_vx, vx)
self.assertLessEqual(min_vy, vy)
self.assertLessEqual(min_vz, vz)
self.assertLessEqual(vx, max_vx)
self.assertLessEqual(vy, max_vy)
self.assertLessEqual(vz, max_vz)
previous_velocity = Vec3(actual_velocity)
# If there weren't at least 10 sensor updates the there went something wrong with the test.
self.assertGreater(amount_sensor_updates, 10)
def test_advanced_buoyancy_is_calculated_when_robot_is_close_to_surface(
self):
z_pos = 0.9
# Length should be twice as much as distance from robot position to sea surface such that buoyancy will be calculated.
robot_length = (z_pos + 0.1) * 2
robot_mock = MagicMock()
with mock.patch(
"lobster_simulator.robot.buoyancy.Buoyancy._check_ray_hits_robot",
return_value=True):
buoyancy_obj = buoyancy.Buoyancy(robot_mock, 0.5, robot_length)
robot_mock.apply_force = Mock()
robot_mock.get_position_and_orientation = Mock(
return_value=(Vec3((0, 0, z_pos)), Quaternion((0, 0, 0, 1))))
buoyancy_obj.update()
robot_mock.get_position_and_orientation.assert_called_once()
# First and third argument could change in the future.
robot_mock.apply_force.assert_called_once()
self.assertEqual(
buoyancy_obj._buoyancy,
-robot_mock.apply_force.call_args[0][1][Z],
msg=
"buoyancy force is not as expected to be when robot is fully underwater."
)
def add_ocean_floor(self, depth=100):
id = PybulletAPI.loadURDF(resource_filename("lobster_simulator",
"data/plane1000.urdf"),
base_position=Vec3((0, 0, depth)))
texture = PybulletAPI.loadTexture(
resource_filename("lobster_simulator", "data/checker_blue.png"))
PybulletAPI.changeVisualShape(id, texture, rgbaColor=[1, 1, 1, 1])
def load_chunk(self, chunk_x, chunk_y):
height_field_data = self.get_height_field(chunk_x, chunk_y)
middle = (max(height_field_data) + min(height_field_data)) / 2
terrainShape = p.createCollisionShape(shapeType=p.GEOM_HEIGHTFIELD,
meshScale=[self.point_spacing, self.point_spacing, 1],
heightfieldTextureScaling=(self.points_per_chunk - 1) / 2,
heightfieldData=height_field_data,
numHeightfieldRows=self.points_per_chunk,
numHeightfieldColumns=self.points_per_chunk)
terrain = p.createMultiBody(0, terrainShape,
basePosition=Vec3([(self.chunk_size * chunk_x + self.chunk_size / 2),
(self.chunk_size * chunk_y + self.chunk_size / 2),
self.depth - middle]).as_nwu(),
baseOrientation=Quaternion.from_euler(Vec3([0, 0, math.pi])).as_nwu())
return terrain
def test_pressure_at_surface(self):
simulator = Simulator(4000, gui=False)
robot = simulator.create_robot()
robot.set_position_and_orientation(position=Vec3((0, 0, 0)))
simulator.do_step()
depth = robot.get_position()[Z]
print("depth", depth)
pressure = robot.pressure_sensor.get_pressure()
# Only checking accuracy up to 1 decimal place, since the robot moves a bit during the step
self.assertAlmostEqual(101.3, pressure, places=1)
def __init__(self, time: SimulationTime):
water_id = PybulletAPI.loadURDF(
resource_filename("lobster_simulator", "data/water_surface.urdf"),
Vec3([0, 0, 0]))
self.water_texture = PybulletAPI.loadTexture(
resource_filename("lobster_simulator", "data/water_texture.png"))
PybulletAPI.changeVisualShape(water_id,
textureUniqueId=self.water_texture,
rgbaColor=[0, 0.3, 1, 0.5])
def vec3_local_to_world(local_frame_position: Vec3,
local_frame_orientation: Quaternion,
local_vec: Vec3) -> Vec3:
"""
Converts a vector in a local reference frame to the global reference frame
:param local_frame_position: Position of the local frame
:param local_frame_orientation: Orientation of the local frame
:param local_vec: Vector in the local reference frame
:return: Vector in the global reference frame
"""
word_vec = local_vec.rotate(local_frame_orientation) + local_frame_position
return word_vec
def test_altitude(self):
dt = SimulationTime(4000)
simulator = Simulator(4000, gui=False)
# Load a plane so that the DVL works
PybulletAPI.loadURDF("plane.urdf", Vec3([0, 0, 30]))
simulator.create_robot()
simulator.do_step()
previous_altitude = simulator.robot._dvl._get_real_values(dt)[0]
downwards_velocity = Vec3([0, 0, 1])
amount_sensor_updates = 0
for _ in range(500):
simulator.do_step()
simulator.robot.set_velocity(linear_velocity=downwards_velocity)
actual_altitude = simulator.robot._dvl._get_real_values(dt)[0]
sensor_data = simulator.robot._dvl.get_latest_value()
# Since the dvl runs at a slower rate than the simulator, it's possible that there is no new data point
if sensor_data is not None:
# Velocity_valid should always be true otherwise the dvl is too far away from the surface and this test wouldn't work.
self.assertTrue(sensor_data[1]['velocity_valid'])
amount_sensor_updates += 1
sensor_altitude = sensor_data[1]['altitude']
min_altitude = min((actual_altitude, previous_altitude))
max_altitude = max((actual_altitude, previous_altitude))
# Make sure the sensor altitude is between the actual altitudes, since it is interpolated between the
# two
self.assertLessEqual(min_altitude, sensor_altitude)
self.assertLessEqual(sensor_altitude, max_altitude)
previous_altitude = actual_altitude
# If there weren't at least 10 sensor updates the there went something wrong with the test.
self.assertGreater(amount_sensor_updates, 10)
def test_pressure_increases_when_going_down(self):
"""Move the robot down and assert that the pressure will only increase."""
simulator = Simulator(4000, gui=False)
simulator.create_robot()
downwards_velocity = Vec3([0, 0, 1])
simulator.robot.set_velocity(linear_velocity=downwards_velocity)
simulator.do_step()
previous_pressure = simulator.robot.pressure_sensor.get_pressure()
for _ in range(100):
simulator.robot.set_velocity(linear_velocity=downwards_velocity)
simulator.do_step()
current_pressure = simulator.robot.pressure_sensor.get_pressure()
self.assertLess(previous_pressure, current_pressure)
previous_pressure = current_pressure
def __init__(self, time_step: int, config=None, gui=True):
"""
Simulator
:param time_step: duration of a step in microseconds
:param config: config of the robot.
:param gui: start the PyBullet gui when true
"""
with resource_stream('lobster_simulator', 'data/config.json') as f:
base_config = json.load(f)
if config is not None:
base_config.update(config)
config = base_config
self.rotate_camera_with_robot = bool(
config['rotate_camera_with_robot'])
self._time: SimulationTime = SimulationTime(0)
self._previous_update_time: SimulationTime = SimulationTime(0)
self._previous_update_real_time: float = t.perf_counter() # in seconds
self._time_step: SimulationTime = SimulationTime(
initial_microseconds=time_step)
self._cycle = 0
PybulletAPI.initialize(self._time_step, gui)
self.water_surface = WaterSurface(self._time)
self._simulator_frequency_slider = PybulletAPI.addUserDebugParameter(
"simulation frequency", 10, 1000, 1 / self._time_step.microseconds)
self._buoyancy_force_slider = PybulletAPI.addUserDebugParameter(
"buoyancyForce", 0, 1000, 550)
self._model = None
self.robot_config = None
self._camera_position = Vec3([0, 0, 0])
self._robot: Optional[AUV] = None
def __init__(self, robot: AUV, position: Vec3, time_step: SimulationTime, time: SimulationTime, orientation: Quaternion,
noise_stds: Optional[Union[List[float], float]]):
"""
Parameters
----------
robot : AUV
The robot the sensor is attached to.
position : array[3]
The local position of the sensor on the robot.
orientation : array[4]
The orientation of the sensor on the robot as a quaternion.
time_step : int
The time step between two polls on the sensor in microseconds.
orientation : Quaternion
Orientation of the sensor w.r.t. the robot.
noise_stds :Union[List[float], float]
Number or list of numbers with the standard deviation for each of the outputs of the sensor.
"""
if orientation is None:
orientation = Quaternion.from_euler(Vec3([0, 0, 0]))
self._robot: AUV = robot
self._sensor_position: Vec3 = position
self._sensor_orientation: Quaternion = orientation
self._time_step = time_step
self._queue = list()
self._next_sample_time: SimulationTime = SimulationTime(initial_microseconds=time.microseconds)
self._previous_update_time: SimulationTime = SimulationTime(initial_microseconds=time.microseconds)
self._previous_real_value = self._get_real_values(SimulationTime(1))
self.noise_stds = None
if noise_stds:
self.set_noise(noise_stds)
