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