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