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 __init__(self, pos: Vec3 = None):
if not pos:
pos = Vec3([0, 0, 0])
object_id = PybulletAPI.loadURDF(resource_filename("lobster_simulator",
"data/scout-alpha-visual.urdf"), pos)
super().__init__(object_id)
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 __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 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 __init__(self,
time: SimulationTime,
config,
start_position: Optional[Vec3] = None,
start_orientation: Optional[Quaternion] = None):
if config is None:
raise ArgumentNoneError("config parameter should not be None")
if start_position is None:
start_position = Vec3([0, 0, 0])
if start_orientation is None:
start_orientation = Quaternion([0, 0, 0, 1])
self._center_of_volume = Vec3(config['center_of_volume'])
self.damping_matrix: np.ndarray = np.diag(
config['damping_matrix_diag'])
object_id = p.loadURDF(
resource_filename("lobster_simulator", "data/scout-alpha.urdf"),
start_position, start_orientation)
super().__init__(object_id)
if config.get('visualize_buoyancy', False):
# Make sure the robot is slightly see through so that the buoyancy dots are visible
PybulletAPI.changeVisualShape(object_id,
rgbaColor=[1, 1, 1, 0.7],
textureUniqueId=-1)
self._buoyancy = buoyancy.Buoyancy(
self,
0.10,
2,
resolution=config.get('buoyancy_resolution'),
visualize=config.get('visualize_buoyancy', False))
config_thrusters = config['thrusters']
self.thrusters: Dict[str, Thruster] = dict()
for i in range(len(config_thrusters)):
self.thrusters[config_thrusters[i]['name']] = Thruster.new_T200(
self, config_thrusters[i]['name'],
Vec3(config_thrusters[i]['position']),
Vec3(config_thrusters[i]['direction']))
# Set damping to zero, because default is not zero
p.changeDynamics(self._object_id,
linearDamping=0.0,
angularDamping=0.0)
self._motor_debug_lines = list()
self._motor_count = len(config_thrusters)
self._rpm_motors = list()
self._desired_rpm_motors: List[float] = list()
self._pressure_sensor = PressureSensor(self,
Vec3([1, 0, 0]),
SimulationTime(4000),
time=time)
self._accelerometer = Accelerometer(self,
Vec3([1, 0, 0]),
SimulationTime(4000),
time=time)
self._gyroscope = Gyroscope(self,
Vec3([1, 0, 0]),
SimulationTime(4000),
time=time)
self._magnetometer = Magnetometer(self,
Vec3([1, 0, 0]),
SimulationTime(4000),
time=time)
self._dvl = DVL(self,
Vec3([-.5, 0, 0.10]),
time_step=SimulationTime(4000),
time=time)
self._max_thrust = 100
