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 test_creating_test_points_scout_alpha(self): PybulletAPI.initialize(SimulationTime(1000), False) model_config = 'scout-alpha.json' with resource_stream('lobster_simulator', f'data/{model_config}') as f: self.robot_config = json.load(f) # Currently resolution is not existing? resolution = self.robot_config.get('buoyancy_resolution') robot = auv.AUV(SimulationTime(5000), self.robot_config) buoyancy_obj = buoyancy.Buoyancy(robot, 0.10, 2, resolution=resolution) self.assertGreater(len(buoyancy_obj.test_points), 0)
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
def test_finding_test_points_not_raising_error(self): with mock.patch( "lobster_simulator.robot.buoyancy.Buoyancy._check_ray_hits_robot", return_value=True): buoyancy_obj = buoyancy.Buoyancy(Mock(), 1, 1) self.assertGreater(len(buoyancy_obj.test_points), 0)
def test_incorrect_length_raises(self): with self.assertRaises(ValueError): buoyancy.Buoyancy(Mock(), 1, 0)
def test_incorrect_radius_raises(self): with self.assertRaises(ValueError): buoyancy.Buoyancy(Mock(), 0, 1)
def test_not_finding_test_points(self): with mock.patch( "lobster_simulator.robot.buoyancy.Buoyancy._check_ray_hits_robot", return_value=False): with self.assertRaises(NoTestPointsCreated): buoyancy.Buoyancy(Mock(), 1, 1)