def setUp(self): PybulletAPI.initialize(SimulationTime(1000), False) PybulletAPI.gui = MagicMock(return_value=True) # Use side_effect to spy on a method: Can assert calls but functionality doesn't change. PybulletAPI.addUserDebugLine = MagicMock( side_effect=PybulletAPI.addUserDebugLine) self._debug_line = DebugLine(Vec3([0, 0, 0]), Vec3([0, 0, 0]))
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 _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 shutdown(self): """ Shuts down the pybullet Simulator. (This is needed when running multiple tests with the simulator because then it doesn't automatically closes in between tests) :return: """ PybulletAPI.disconnect()
def set_time_step(self, time_step_microseconds: int) -> None: """ Sets the size of the time steps the simulator makes :param time_step_microseconds: Time step in microseconds """ self._time_step = SimulationTime(time_step_microseconds) PybulletAPI.setTimeStep(self._time_step)
def add_ocean_floor(self, depth=100): id = PybulletAPI.loadURDF(resource_filename("lobster_simulator", "data/plane1000.urdf"), base_position=Vec3((0, 0, depth))) texture = PybulletAPI.loadTexture( resource_filename("lobster_simulator", "data/checker_blue.png")) PybulletAPI.changeVisualShape(id, texture, rgbaColor=[1, 1, 1, 1])
def update_camera_position(self): smoothing = 0.95 self._camera_position = smoothing * self._camera_position + ( 1 - smoothing) * self._robot.get_position() if self.rotate_camera_with_robot: PybulletAPI.moveCameraToPosition(self._camera_position, self._robot.get_orientation()) else: PybulletAPI.moveCameraToPosition(self._camera_position)
def __init__(self, time: SimulationTime): water_id = PybulletAPI.loadURDF( resource_filename("lobster_simulator", "data/water_surface.urdf"), Vec3([0, 0, 0])) self.water_texture = PybulletAPI.loadTexture( resource_filename("lobster_simulator", "data/water_texture.png")) PybulletAPI.changeVisualShape(water_id, textureUniqueId=self.water_texture, rgbaColor=[0, 0.3, 1, 0.5])
def test_creating_test_points_scout_alpha(self): PybulletAPI.initialize(SimulationTime(1000), False) model_config = 'scout-alpha.json' with resource_stream('lobster_simulator', f'data/{model_config}') as f: self.robot_config = json.load(f) # Currently resolution is not existing? resolution = self.robot_config.get('buoyancy_resolution') robot = auv.AUV(SimulationTime(5000), self.robot_config) buoyancy_obj = buoyancy.Buoyancy(robot, 0.10, 2, resolution=resolution) self.assertGreater(len(buoyancy_obj.test_points), 0)
def test_velocity(self): simulator = Simulator(4000, gui=False) PybulletAPI.loadURDF("plane.urdf", Vec3([0, 0, 30])) simulator.create_robot() simulator.robot.set_velocity(linear_velocity=Vec3([1, 1, 1])) simulator.do_step() previous_velocity = simulator.robot.get_velocity() amount_sensor_updates = 0 for _ in range(500): simulator.robot.set_velocity(linear_velocity=Vec3([1, 1, 1])) actual_velocity = simulator.robot.get_velocity() simulator.do_step() sensor_data = simulator.robot.dvl.get_latest_value() # Since the dvl runs if sensor_data: # Velocity_valid should always be true otherwise the dvl is too far away from the surface and this test wouldn't work. self.assertTrue(sensor_data[1]['velocity_valid']) amount_sensor_updates += 1 vx, vy, vz = sensor_data[1]['vx'], sensor_data[1]['vy'], sensor_data[1]['vz'] min_vx = min((actual_velocity[X], previous_velocity[X])) max_vx = max((actual_velocity[X], previous_velocity[X])) min_vy = min((actual_velocity[Y], previous_velocity[Y])) max_vy = max((actual_velocity[Y], previous_velocity[Y])) min_vz = min((actual_velocity[Z], previous_velocity[Z])) max_vz = max((actual_velocity[Z], previous_velocity[Z])) # Make sure the sensor velocities are between the actual velocities, since it is interpolated between # the two self.assertLessEqual(min_vx, vx) self.assertLessEqual(min_vy, vy) self.assertLessEqual(min_vz, vz) self.assertLessEqual(vx, max_vx) self.assertLessEqual(vy, max_vy) self.assertLessEqual(vz, max_vz) previous_velocity = Vec3(actual_velocity) # If there weren't at least 10 sensor updates the there went something wrong with the test. self.assertGreater(amount_sensor_updates, 10)
def do_step(self): """Progresses the simulation by exactly one time step.""" self._time.add_time_step(self._time_step.microseconds) self.update_camera_position() self._robot.update(self._time_step, self._time) PybulletAPI.stepSimulation() self._cycle += 1 if self._cycle % 50 == 0: self._previous_update_time = copy.copy(self._time) self._previous_update_real_time = t.perf_counter()
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_debug_line(self) -> int: return PybulletAPI.addUserDebugLine(lineFromXYZ=self.from_location, lineToXYZ=self.to_location, lineWidth=self._width, lineColorRGB=self._color, parentObjectUniqueId=self.parentIndex, replaceItemUniqueId=self._object_id)
def __init__(self, pos: Vec3 = None): if not pos: pos = Vec3([0, 0, 0]) object_id = PybulletAPI.loadURDF(resource_filename("lobster_simulator", "data/scout-alpha-visual.urdf"), pos) super().__init__(object_id)
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)
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 set_velocity(self, linear_velocity: Vec3 = None, angular_velocity: Vec3 = None, local_frame=False) -> None: """ Sets the linear and/or angular velocity of the robot (should only be used for testing purposes). :param linear_velocity: The linear velocity that the robot should have. :param angular_velocity: The angular velocity that the robot should have. :param local_frame: Whether the velocities are given in the local or world frame. """ if linear_velocity is None: linear_velocity = self.get_velocity() if angular_velocity is None: angular_velocity = self.get_angular_velocity() if local_frame: linear_velocity = vec3_rotate_vector_to_world( self.get_orientation(), linear_velocity) angular_velocity = vec3_rotate_vector_to_world( self.get_orientation(), angular_velocity) p.resetBaseVelocity(self._object_id, linear_velocity, angular_velocity)
def test_altitude(self): dt = SimulationTime(4000) simulator = Simulator(4000, gui=False) # Load a plane so that the DVL works PybulletAPI.loadURDF("plane.urdf", Vec3([0, 0, 30])) simulator.create_robot() simulator.do_step() previous_altitude = simulator.robot._dvl._get_real_values(dt)[0] downwards_velocity = Vec3([0, 0, 1]) amount_sensor_updates = 0 for _ in range(500): simulator.do_step() simulator.robot.set_velocity(linear_velocity=downwards_velocity) actual_altitude = simulator.robot._dvl._get_real_values(dt)[0] sensor_data = simulator.robot._dvl.get_latest_value() # Since the dvl runs at a slower rate than the simulator, it's possible that there is no new data point if sensor_data is not None: # Velocity_valid should always be true otherwise the dvl is too far away from the surface and this test wouldn't work. self.assertTrue(sensor_data[1]['velocity_valid']) amount_sensor_updates += 1 sensor_altitude = sensor_data[1]['altitude'] min_altitude = min((actual_altitude, previous_altitude)) max_altitude = max((actual_altitude, previous_altitude)) # Make sure the sensor altitude is between the actual altitudes, since it is interpolated between the # two self.assertLessEqual(min_altitude, sensor_altitude) self.assertLessEqual(sensor_altitude, max_altitude) previous_altitude = actual_altitude # If there weren't at least 10 sensor updates the there went something wrong with the test. self.assertGreater(amount_sensor_updates, 10)
def update(self, position): current_chunk = (int(position[X] // self.chunk_size), int(position[Y] // self.chunk_size)) if self.current_chunk[X] != current_chunk[X] or self.current_chunk[Y] != current_chunk[Y]: new_chunks = dict() render_dist_min = self.render_distance//2 render_dist_max = self.render_distance - render_dist_min for i in range(current_chunk[0] - render_dist_min, current_chunk[0] + render_dist_max): for j in range(current_chunk[1] - render_dist_min, current_chunk[1] + render_dist_max): if (i, j) not in self.chunks: new_chunks[(i, j)] = self.load_chunk(i, j) else: new_chunks[(i, j)] = self.chunks[(i, j)] for key, value in self.chunks.items(): if key not in new_chunks.keys(): PybulletAPI.removeBody(value) self.chunks = new_chunks self.current_chunk = current_chunk
def get_altitude(self) -> Optional[float]: """ Gets the actual altitude (so not based on the simulated dvl) of the auv in its own reference frame by casting a ray to see where it intersects with terrain. This beam has length 100 so even if the altitude is larger than 100 it will still return 100. """ beam_length = 100 raytest_endpoint = 2 * Vec3([0, 0, beam_length]) world_frame_endpoint = vec3_local_to_world(self.get_position(), self.get_orientation(), raytest_endpoint) result = p.rayTest(self.get_position(), world_frame_endpoint) altitude = result[0] * beam_length if altitude >= 100: return None else: return altitude
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)
def __init__(self, radius, rgba_color): object_id = PybulletAPI.createVisualSphere(radius, rgba_color) super().__init__(object_id)
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
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 _check_ray_hits_robot(self, start_point: Vec3, endpoint: Vec3) -> bool: return PybulletAPI.rayTest( start_point, endpoint, object_id=self._robot.object_id)[0] < 1
def __init__(self, robot: 'auv', radius: float, length: float, resolution: Optional[float] = None, visualize: bool = False): if radius <= 0: raise ValueError("Radius should be bigger than zero") if length <= 0: raise ValueError("Length should be bigger than zero") self._radius = radius self._length = length self._robot: auv = robot self._buoyancy: float = 550 self.resolution = resolution self.visualize = visualize self.dots = [] self.dot_under_water = [] self.test_points = [] if resolution: # Expects that the robot has a cylindrical shape. x_range = np.arange(-length / 2, length / 2, self.resolution) y_range = np.arange(-radius, radius + self.resolution, self.resolution) z_range = np.arange(-radius, radius + self.resolution, self.resolution) else: x_range = range(1) y_range = range(1) z_range = range(1) total_points = len(x_range) * len(y_range) * len(z_range) count = 0 # The following code tests for all points in a cylinder around the robot defined by length radius and resolution # For each point it checks in 4 directions (positive and negative y and z) if a ray of a meter long hits the # robot. If this is the case for all 4 directions this must mean the point lies within the robot, otherwise it # doesn't for x in x_range: for y in y_range: for z in z_range: pos = Vec3([x, y, z]) count += 1 if count % 1000 == 0: print(f"{int(count / total_points * 100)}%") if np.math.sqrt(y ** 2 + z ** 2) < radius \ and self._check_ray_hits_robot(pos + Vec3([0, 1, 0]), pos) \ and self._check_ray_hits_robot(pos + Vec3([0, 0, 1]), pos) \ and self._check_ray_hits_robot(pos + Vec3([0, -1, 0]), pos) \ and self._check_ray_hits_robot(pos + Vec3([0, 0, -1]), pos): if self.visualize: self.dot_under_water.append(True) if self.resolution: sphere_size = self.resolution / 4 else: sphere_size = 0.05 self.dots.append( PybulletAPI.createVisualSphere( sphere_size, [0, 0, 1, 1])) self.test_points.append(Vec3([x, y, z])) if len(self.test_points) == 0: raise NoTestPointsCreated( f"No buoyancy test points where created with robot: {robot}, radius: {radius}," f" length: {length}" f" and resolution {resolution} ")
def remove(self): for dot in self.dots: PybulletAPI.removeBody(dot)