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()
def _update_desired(self, orientation):
keyboard_events = PybulletAPI.getKeyboardEvents()
if self.position_control:
desired_position = vec3_rotate_vector_to_local(orientation, self.desired_position)
if self.key_is_down('q', keyboard_events):
desired_position[Z] -= 0.008
if self.key_is_down('e', keyboard_events):
desired_position[Z] += 0.008
if self.key_is_down('w', keyboard_events):
desired_position[X] += 0.008
if self.key_is_down('s', keyboard_events):
desired_position[X] -= 0.008
if self.key_is_down('a', keyboard_events):
desired_position[Y] -= 0.008
if self.key_is_down('d', keyboard_events):
desired_position[Y] += 0.008
desired_position[X] += self.gamepad.y / 40
desired_position[Y] += self.gamepad.x / 40
desired_position[Z] += self.gamepad.z / 40 - self.gamepad.rz / 40
self.desired_position = vec3_rotate_vector_to_world(orientation, desired_position)
if self.key_is_down('j', keyboard_events):
self.desired_orientation[Z] -= 0.003
if self.key_is_down('l', keyboard_events):
self.desired_orientation[Z] += 0.003
if self.key_is_down('u', keyboard_events):
self.desired_orientation[X] -= 0.003
if self.key_is_down('o', keyboard_events):
self.desired_orientation[X] += 0.003
if self.key_is_down('i', keyboard_events):
self.desired_orientation[Y] -= 0.003
if self.key_is_down('k', keyboard_events):
self.desired_orientation[Y] += 0.003
self.desired_orientation[Y] -= self.gamepad.ry / 200
self.desired_orientation[Z] += self.gamepad.rx / 200
else:
self.desired_velocity = Vec3([0, 0, 0])
if self.key_is_down('q', keyboard_events):
self.desired_velocity[Z] -= 10
if self.key_is_down('e', keyboard_events):
self.desired_velocity[Z] += 10
if self.key_is_down('w', keyboard_events):
self.desired_velocity[X] += 10
if self.key_is_down('s', keyboard_events):
self.desired_velocity[X] -= 10
if self.key_is_down('a', keyboard_events):
self.desired_velocity[Y] -= 10
if self.key_is_down('d', keyboard_events):
self.desired_velocity[Y] += 10
self.desired_velocity[X] += self.gamepad.y * 10
self.desired_velocity[Y] += self.gamepad.x * 10
# self.desired_velocity[Z] = self.gamepad.z * 3 - self.gamepad.rz * 3
self.desired_rates = Vec3([0, 0, 0])
if self.key_is_down('j', keyboard_events):
self.desired_rates[YAW] += 2
if self.key_is_down('l', keyboard_events):
self.desired_rates[YAW] -= 2
if self.key_is_down('u', keyboard_events):
self.desired_rates[ROLL] += 2
if self.key_is_down('o', keyboard_events):
self.desired_rates[ROLL] -= 2
if self.key_is_down('i', keyboard_events):
self.desired_rates[PITCH] += 2
if self.key_is_down('k', keyboard_events):
self.desired_rates[PITCH] -= 2
self.desired_rates[YAW] -= self.gamepad.rx * 2.0
self.desired_rates[PITCH] += self.gamepad.ry * 2.0
self.desired_rates[ROLL] += self.gamepad.z * 2.0 - self.gamepad.rz * 2.0