def __init__(self, radius, rgba_color):
object_id = PybulletAPI.createVisualSphere(radius, rgba_color)
super().__init__(object_id)
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} ")
