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