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