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 __init__(self, gui: bool, desired_position: Vec3, desired_orientation: Vec3, position_control: bool = True):
self.desired_position = desired_position
self.desired_orientation = desired_orientation
self.relative_yaw = 0
self.relative_pitch = 0
self.relative_roll = 0
self.desired_pitch_yaw_vector = [0, 0, 0]
self.gui = gui
self.previous_velocity = Vec3([0, 0, 0])
self.position_control = position_control
self.desired_velocity = Vec3([0.0, 0.0, 0.0])
self.desired_rates = Vec3([0.0, 0.0, 0.0])
if gui:
self.forward_velocity_slider = PybulletAPI.addUserDebugParameter("forward", -3, 3, 0)
self.upward_velocity_slider = PybulletAPI.addUserDebugParameter("upward", -3, 3, 0)
self.sideward_velocity_slider = PybulletAPI.addUserDebugParameter("sideward", -3, 3, 0)
if self.position_control:
self.desired_state_visualization = DebugScout(desired_position)
self.gamepad = Gamepad()
self.gamepad.start()
def _apply_damping(self):
"""
Applies damping to the robot on its linear and angular velocity.
See https://docs.lobster-robotics.com/scout/robots/scout-alpha/scout-simulator-model
"""
# Don't apply damping if the robot is above water
# TODO. This creates a unrealistic hard boundary for enabling and disabling damping, so perhaps this should be
# improved in the future.
if self.get_position()[Z] < 0:
return
velocity = vec3_rotate_vector_to_local(self.get_orientation(),
self.get_velocity())
angular_velocity = vec3_rotate_vector_to_local(
self.get_orientation(), self.get_angular_velocity())
damping = -np.dot(
self.damping_matrix,
np.concatenate((velocity.numpy(), angular_velocity.numpy())))
# Multiply the damping for now to get slightly more accurate results
damping *= 10
linear_damping_force = Vec3(damping[:3])
angular_damping_torque = Vec3(damping[3:])
p.applyExternalForce(self._object_id,
forceObj=linear_damping_force,
posObj=Vec3([0, 0, 0]),
frame=Frame.LINK_FRAME)
p.applyExternalTorque(self._object_id,
torqueObj=angular_damping_torque,
frame=Frame.LINK_FRAME)
def shutdown(self):
"""
Shuts down the pybullet Simulator. (This is needed when running multiple tests with the simulator because then
it doesn't automatically closes in between tests)
:return:
"""
PybulletAPI.disconnect()
def set_time_step(self, time_step_microseconds: int) -> None:
"""
Sets the size of the time steps the simulator makes
:param time_step_microseconds: Time step in microseconds
"""
self._time_step = SimulationTime(time_step_microseconds)
PybulletAPI.setTimeStep(self._time_step)
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 update_camera_position(self):
smoothing = 0.95
self._camera_position = smoothing * self._camera_position + (
1 - smoothing) * self._robot.get_position()
if self.rotate_camera_with_robot:
PybulletAPI.moveCameraToPosition(self._camera_position,
self._robot.get_orientation())
else:
PybulletAPI.moveCameraToPosition(self._camera_position)
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_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 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 do_step(self):
"""Progresses the simulation by exactly one time step."""
self._time.add_time_step(self._time_step.microseconds)
self.update_camera_position()
self._robot.update(self._time_step, self._time)
PybulletAPI.stepSimulation()
self._cycle += 1
if self._cycle % 50 == 0:
self._previous_update_time = copy.copy(self._time)
self._previous_update_real_time = t.perf_counter()
def set_position_and_orientation(self,
position: Vec3 = None,
orientation: Quaternion = None) -> None:
"""
Sets the position and/or the orientation of the robot (should only be used for testing purposes).
:param position: Position
:param orientation: Orientation
"""
if position is None:
position = self.get_position()
if orientation is None:
orientation = self.get_orientation()
p.resetBasePositionAndOrientation(self._object_id, position,
orientation)
def _update_debug_line(self) -> int:
return PybulletAPI.addUserDebugLine(lineFromXYZ=self.from_location,
lineToXYZ=self.to_location,
lineWidth=self._width,
lineColorRGB=self._color,
parentObjectUniqueId=self.parentIndex,
replaceItemUniqueId=self._object_id)
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 apply_force(self,
force_pos: Vec3,
force: Vec3,
relative_direction: bool = True) -> None:
"""
Applies a force to the robot (should only be used for testing purposes).
:param force_pos: Position of the acting force, given in the local frame
:param force: Force vector
:param relative_direction: Determines whether or not the force is defined in the local or world frame.
"""
if not relative_direction:
# If the force direction is given in the world frame, it should be rotated to the local frame
force = vec3_rotate_vector_to_local(self.get_orientation(), force)
# Apply the force in the local frame
p.applyExternalForce(self._object_id, force, force_pos,
Frame.LINK_FRAME)
def _update(self, dt: SimulationTime):
"""
:param dt: Delta time
"""
if abs(self._desired_thrust - self._theoretical_thrust
) <= self._maximum_delta_thrust_per_second * dt.seconds:
self._theoretical_thrust = self._desired_thrust
elif self._desired_thrust > self._theoretical_thrust:
self._theoretical_thrust += self._maximum_delta_thrust_per_second * dt.seconds
elif self._desired_thrust < self._theoretical_thrust:
self._theoretical_thrust -= self._maximum_delta_thrust_per_second * dt.seconds
self._theoretical_thrust = clip(self._theoretical_thrust,
-self._maximum_backward_thrust,
self._maximum_forward_thrust)
world_position = translation.vec3_local_to_world_id(
self._robot.object_id, self._position)
if world_position[Z] > WaterSurface.water_height(
world_position[X], world_position[Y]):
PybulletAPI.applyExternalForce(
objectUniqueId=self._robot.object_id,
forceObj=self._direction * self.current_thrust,
posObj=self._position,
frame=Frame.LINK_FRAME)
debug_line_color = [
0, 0, 1
] # Debug line color is blue when the thruster is in the water
else:
# Debug line color is red when the thruster is not in the water and can thus give no thrust
debug_line_color = [1, 0, 0]
self._motor_debug_line.update(
self._position,
self._position + self._direction * self.current_thrust / 100,
self._robot.object_id,
color=debug_line_color)
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 set_velocity(self,
linear_velocity: Vec3 = None,
angular_velocity: Vec3 = None,
local_frame=False) -> None:
"""
Sets the linear and/or angular velocity of the robot (should only be used for testing purposes).
:param linear_velocity: The linear velocity that the robot should have.
:param angular_velocity: The angular velocity that the robot should have.
:param local_frame: Whether the velocities are given in the local or world frame.
"""
if linear_velocity is None:
linear_velocity = self.get_velocity()
if angular_velocity is None:
angular_velocity = self.get_angular_velocity()
if local_frame:
linear_velocity = vec3_rotate_vector_to_world(
self.get_orientation(), linear_velocity)
angular_velocity = vec3_rotate_vector_to_world(
self.get_orientation(), angular_velocity)
p.resetBaseVelocity(self._object_id, linear_velocity, angular_velocity)
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 update(self, position):
current_chunk = (int(position[X] // self.chunk_size), int(position[Y] // self.chunk_size))
if self.current_chunk[X] != current_chunk[X] or self.current_chunk[Y] != current_chunk[Y]:
new_chunks = dict()
render_dist_min = self.render_distance//2
render_dist_max = self.render_distance - render_dist_min
for i in range(current_chunk[0] - render_dist_min, current_chunk[0] + render_dist_max):
for j in range(current_chunk[1] - render_dist_min, current_chunk[1] + render_dist_max):
if (i, j) not in self.chunks:
new_chunks[(i, j)] = self.load_chunk(i, j)
else:
new_chunks[(i, j)] = self.chunks[(i, j)]
for key, value in self.chunks.items():
if key not in new_chunks.keys():
PybulletAPI.removeBody(value)
self.chunks = new_chunks
self.current_chunk = current_chunk
def get_altitude(self) -> Optional[float]:
"""
Gets the actual altitude (so not based on the simulated dvl) of the auv in its own reference frame by casting a
ray to see where it intersects with terrain. This beam has length 100 so even if the altitude is larger than 100
it will still return 100.
"""
beam_length = 100
raytest_endpoint = 2 * Vec3([0, 0, beam_length])
world_frame_endpoint = vec3_local_to_world(self.get_position(),
self.get_orientation(),
raytest_endpoint)
result = p.rayTest(self.get_position(), world_frame_endpoint)
altitude = result[0] * beam_length
if altitude >= 100:
return None
else:
return altitude
def update(self):
buoyancy_point = Vec3([0, 0, 0])
buoyancy_force = Vec3([0, 0, -self._buoyancy])
under_water_count = 0
position, orientation = self._robot.get_position_and_orientation()
# Only do the computationally intensive calculation of the buoyancy point and force when the robot is close to
# the surface
# And when there are any points to be used to calculate the buoyancy.
max_distance_from_pos = max(self._radius, self._length / 2)
if position[Z] - max_distance_from_pos < WaterSurface.water_height(
position[X], position[Y]):
for i in range(len(self.test_points)):
dot_position = translation.vec3_local_to_world(
position, orientation, self.test_points[i])
if self.visualize:
PybulletAPI.resetBasePositionAndOrientation(
self.dots[i], dot_position)
if dot_position[Z] > WaterSurface.water_height(
dot_position[X], dot_position[Y]):
under_water_count += 1
buoyancy_point += self.test_points[i]
if self.visualize and not self.dot_under_water[i]:
PybulletAPI.changeVisualShapeColor(
self.dots[i], [0, 0, 1, 0.5])
self.dot_under_water[i] = True
else:
if self.visualize and self.dot_under_water[i]:
PybulletAPI.changeVisualShapeColor(
self.dots[i], [1, 0, 0, 0.5])
self.dot_under_water[i] = False
if under_water_count > 0:
buoyancy_point /= under_water_count
buoyancy_force = buoyancy_force * under_water_count / len(
self.test_points)
# Apply the buoyancy force
self._robot.apply_force(buoyancy_point,
buoyancy_force,
relative_direction=False)
def set_position_and_orientation(self, position: Vec3, orientation: Quaternion):
PybulletAPI.resetBasePositionAndOrientation(self._object_id, posObj=position, ornObj=orientation)
def update_position(self, position: Vec3):
PybulletAPI.resetBasePositionAndOrientation(self._object_id, posObj=position)
def __init__(self, radius, rgba_color):
object_id = PybulletAPI.createVisualSphere(radius, rgba_color)
super().__init__(object_id)
def _update_desired(self, orientation):
keyboard_events = PybulletAPI.getKeyboardEvents()
if self.position_control:
desired_position = vec3_rotate_vector_to_local(orientation, self.desired_position)
if self.key_is_down('q', keyboard_events):
desired_position[Z] -= 0.008
if self.key_is_down('e', keyboard_events):
desired_position[Z] += 0.008
if self.key_is_down('w', keyboard_events):
desired_position[X] += 0.008
if self.key_is_down('s', keyboard_events):
desired_position[X] -= 0.008
if self.key_is_down('a', keyboard_events):
desired_position[Y] -= 0.008
if self.key_is_down('d', keyboard_events):
desired_position[Y] += 0.008
desired_position[X] += self.gamepad.y / 40
desired_position[Y] += self.gamepad.x / 40
desired_position[Z] += self.gamepad.z / 40 - self.gamepad.rz / 40
self.desired_position = vec3_rotate_vector_to_world(orientation, desired_position)
if self.key_is_down('j', keyboard_events):
self.desired_orientation[Z] -= 0.003
if self.key_is_down('l', keyboard_events):
self.desired_orientation[Z] += 0.003
if self.key_is_down('u', keyboard_events):
self.desired_orientation[X] -= 0.003
if self.key_is_down('o', keyboard_events):
self.desired_orientation[X] += 0.003
if self.key_is_down('i', keyboard_events):
self.desired_orientation[Y] -= 0.003
if self.key_is_down('k', keyboard_events):
self.desired_orientation[Y] += 0.003
self.desired_orientation[Y] -= self.gamepad.ry / 200
self.desired_orientation[Z] += self.gamepad.rx / 200
else:
self.desired_velocity = Vec3([0, 0, 0])
if self.key_is_down('q', keyboard_events):
self.desired_velocity[Z] -= 10
if self.key_is_down('e', keyboard_events):
self.desired_velocity[Z] += 10
if self.key_is_down('w', keyboard_events):
self.desired_velocity[X] += 10
if self.key_is_down('s', keyboard_events):
self.desired_velocity[X] -= 10
if self.key_is_down('a', keyboard_events):
self.desired_velocity[Y] -= 10
if self.key_is_down('d', keyboard_events):
self.desired_velocity[Y] += 10
self.desired_velocity[X] += self.gamepad.y * 10
self.desired_velocity[Y] += self.gamepad.x * 10
# self.desired_velocity[Z] = self.gamepad.z * 3 - self.gamepad.rz * 3
self.desired_rates = Vec3([0, 0, 0])
if self.key_is_down('j', keyboard_events):
self.desired_rates[YAW] += 2
if self.key_is_down('l', keyboard_events):
self.desired_rates[YAW] -= 2
if self.key_is_down('u', keyboard_events):
self.desired_rates[ROLL] += 2
if self.key_is_down('o', keyboard_events):
self.desired_rates[ROLL] -= 2
if self.key_is_down('i', keyboard_events):
self.desired_rates[PITCH] += 2
if self.key_is_down('k', keyboard_events):
self.desired_rates[PITCH] -= 2
self.desired_rates[YAW] -= self.gamepad.rx * 2.0
self.desired_rates[PITCH] += self.gamepad.ry * 2.0
self.desired_rates[ROLL] += self.gamepad.z * 2.0 - self.gamepad.rz * 2.0
def main():
parser = argparse.ArgumentParser("Lobster Simulator")
parser.add_argument('--gui', type=bool, help='Run with or without GUI')
args = parser.parse_args()
gui = args.gui
time_step = 4000
simulator = Simulator(time_step,
gui=gui,
config={"rotate_camera_with_robot": False})
simulator.create_robot(Models.SCOUT_ALPHA, buoyancy_resolution=0.07)
# Only try to add debug sliders and visualisation when the gui is showing
if gui:
desired_pos_sliders = [
PybulletAPI.addUserDebugParameter("desired x", -100, 100, 0),
PybulletAPI.addUserDebugParameter("desired y", -100, 100, 0),
PybulletAPI.addUserDebugParameter("desired z", 0, 100, 90)
]
roll_rate_slider = PybulletAPI.addUserDebugParameter(
"rate ROLL", -10, 10, 0)
simulator_time_step_slider = PybulletAPI.addUserDebugParameter(
"simulation timestep microseconds", 1000, 500000, 4000)
high_level_controller = HighLevelController(gui,
simulator.robot.get_position(),
Vec3([.0, .0, .0]),
position_control=True)
terrain_loader = Terrain.perlin_noise_terrain(30)
paused = False
cycles = 0
previous_time = time.time()
cycles_per_second = 0
while True:
keys = PybulletAPI.getKeyboardEvents()
if ord('p') in keys and keys[ord('p')] == 3:
paused = not paused
if ord('r') in keys and keys[ord('r')] == 3:
simulator.reset_robot()
if PybulletAPI.DELETE_KEY in keys and keys[
PybulletAPI.DELETE_KEY] == 3:
break
lobster_pos, lobster_orn = simulator.robot.get_position_and_orientation(
)
terrain_loader.update(lobster_pos)
if not paused:
# Reading all the debug parameters (only if the gui is showing)
if gui:
time_step = PybulletAPI.readUserDebugParameter(
simulator_time_step_slider)
high_level_controller.set_target_rate(
ROLL, PybulletAPI.readUserDebugParameter(roll_rate_slider))
simulator.set_time_step(time_step)
velocity = PybulletAPI.getBaseVelocity(simulator.robot.object_id)
high_level_controller.update(lobster_pos, lobster_orn, velocity[0],
velocity[1], time_step / 1000000)
thrust_motors = high_level_controller.motor_thrust_outputs
for i, thruster in enumerate(simulator.robot.thrusters.values()):
thruster.set_desired_thrust(thrust_motors[i])
simulator.do_step()
previous_weight = 0.99
cycles_per_second = previous_weight * cycles_per_second + (
1 - previous_weight) / (time.time() - previous_time)
# print(f'{cycles_per_second:.0f}', end='\r')
previous_time = time.time()
PybulletAPI.disconnect()
def _check_ray_hits_robot(self, start_point: Vec3, endpoint: Vec3) -> bool:
return PybulletAPI.rayTest(
start_point, endpoint, object_id=self._robot.object_id)[0] < 1
def __init__(self,
robot: 'auv',
radius: float,
length: float,
resolution: Optional[float] = None,
visualize: bool = False):
if radius <= 0:
raise ValueError("Radius should be bigger than zero")
if length <= 0:
raise ValueError("Length should be bigger than zero")
self._radius = radius
self._length = length
self._robot: auv = robot
self._buoyancy: float = 550
self.resolution = resolution
self.visualize = visualize
self.dots = []
self.dot_under_water = []
self.test_points = []
if resolution:
# Expects that the robot has a cylindrical shape.
x_range = np.arange(-length / 2, length / 2, self.resolution)
y_range = np.arange(-radius, radius + self.resolution,
self.resolution)
z_range = np.arange(-radius, radius + self.resolution,
self.resolution)
else:
x_range = range(1)
y_range = range(1)
z_range = range(1)
total_points = len(x_range) * len(y_range) * len(z_range)
count = 0
# The following code tests for all points in a cylinder around the robot defined by length radius and resolution
# For each point it checks in 4 directions (positive and negative y and z) if a ray of a meter long hits the
# robot. If this is the case for all 4 directions this must mean the point lies within the robot, otherwise it
# doesn't
for x in x_range:
for y in y_range:
for z in z_range:
pos = Vec3([x, y, z])
count += 1
if count % 1000 == 0:
print(f"{int(count / total_points * 100)}%")
if np.math.sqrt(y ** 2 + z ** 2) < radius \
and self._check_ray_hits_robot(pos + Vec3([0, 1, 0]), pos) \
and self._check_ray_hits_robot(pos + Vec3([0, 0, 1]), pos) \
and self._check_ray_hits_robot(pos + Vec3([0, -1, 0]), pos) \
and self._check_ray_hits_robot(pos + Vec3([0, 0, -1]), pos):
if self.visualize:
self.dot_under_water.append(True)
if self.resolution:
sphere_size = self.resolution / 4
else:
sphere_size = 0.05
self.dots.append(
PybulletAPI.createVisualSphere(
sphere_size, [0, 0, 1, 1]))
self.test_points.append(Vec3([x, y, z]))
if len(self.test_points) == 0:
raise NoTestPointsCreated(
f"No buoyancy test points where created with robot: {robot}, radius: {radius},"
f" length: {length}"
f" and resolution {resolution} ")
def remove(self):
for dot in self.dots:
PybulletAPI.removeBody(dot)
