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(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)
