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)