def connect(self, sim_port: int):
# Code required to connect to the Coppelia Remote API
print('Simulation started')
self.sim_port = sim_port
sim.simxFinish(-1) # close all opened connections
self.client_id = sim.simxStart('127.0.0.1', sim_port, True, False,
5000, 3) # Connect to CoppeliaSim
if self.client_id != -1:
self.display_info('Hello! PAWS Connected.')
print('Connected to remote API server')
is_connected = True
else:
print('Not connected to remote API server')
is_connected = False
if is_connected:
# Get necessary object handles
self._get_object_handles()
return is_connected
def _get_sensor_readings(self):
sensor_list = [
self.paws_north_sensor, self.paws_south_sensor,
self.paws_east_sensor, self.paws_west_sensor
]
detection_list = []
for sensor in sensor_list:
detection_list.append(
sim.simxReadProximitySensor(self.client_id, sensor,
sim.simx_opmode_blocking)[1])
return detection_list
def _get_object_rot(self, object_id):
start_quaternion = sim.simxGetObjectQuaternion(
self.client_id, object_id, -1, sim.simx_opmode_blocking)[1]
if all(q == 0 for q in start_quaternion):
rot_deg = 0.0
is_increasing = 0
else:
r = Rotation.from_quat(start_quaternion)
rot_vec = r.as_euler('zyx', degrees=True)
rot_deg = rot_vec[1]
is_increasing = rot_vec[0] >= 0
return rot_deg, is_increasing
def _get_object_handles(self):
self.paws_bot = sim.simxGetObjectHandle(self.client_id, "paws_bot",
sim.simx_opmode_blocking)[1]
self.paws_north_sensor = sim.simxGetObjectHandle(
self.client_id, "paws_northProxSensor",
sim.simx_opmode_blocking)[1]
self.paws_south_sensor = sim.simxGetObjectHandle(
self.client_id, "paws_southProxSensor",
sim.simx_opmode_blocking)[1]
self.paws_east_sensor = sim.simxGetObjectHandle(
self.client_id, "paws_eastProxSensor", sim.simx_opmode_blocking)[1]
self.paws_west_sensor = sim.simxGetObjectHandle(
self.client_id, "paws_westProxSensor", sim.simx_opmode_blocking)[1]
self.paws_left_motor = sim.simxGetObjectHandle(
self.client_id, "paws_leftMotor", sim.simx_opmode_blocking)[1]
self.paws_right_motor = sim.simxGetObjectHandle(
self.client_id, "paws_rightMotor", sim.simx_opmode_blocking)[1]
self.human = sim.simxGetObjectHandle(self.client_id, "human",
sim.simx_opmode_blocking)[1]
def _detect_collision(self):
collision = False
det_state = []
det_dist = []
sensor_list = [
self.paws_north_sensor, self.paws_east_sensor,
self.paws_west_sensor, self.paws_south_sensor
]
for sensor in sensor_list:
det_reading = sim.simxReadProximitySensor(self.client_id, sensor,
sim.simx_opmode_blocking)
det_state.append(det_reading[1])
det_dist.append(det_reading[2])
index = list(locate(det_state, lambda det: det == True))
for i in index:
dist = self.get_length(det_dist[i])
if dist <= COLLISION_DIST:
collision = True
break
return collision
def initialize(self):
# Remove robot from scene
absolute_ref = -1
model_path = os.path.join('..', self.this_folder,
f"{PAWS_BOT_MODEL_PATH}")
sim.simxRemoveModel(self.client_id, self.paws_bot,
sim.simx_opmode_blocking)
# Set random waypoint location for human on edge of map
self.human_coords = self._get_random_location(self.waypoint_array)
sim.simxSetObjectPosition(
self.client_id, self.human, absolute_ref,
(self.human_coords[0], self.human_coords[1], self.human_coords[2]),
sim.simx_opmode_blocking)
# Load robot into inital location
sim.simxLoadModel(self.client_id, model_path, 0,
sim.simx_opmode_blocking)
self._get_object_handles()
self.set_optimal_distance(self.initial_pos, self.human_coords)
def display_info(self, info):
sim.simxAddStatusbarMessage(self.client_id, info,
sim.simx_opmode_oneshot)
def close_connection(self):
sim.simxFinish(-1)
def _set_target_velocity(self, motor, speed):
sim.simxSetJointTargetVelocity(self.client_id, motor, speed,
sim.simx_opmode_streaming)
def get_postion(self, object_handle: int, relative_to: int = -1):
# relative_to = -1 is the absolute position
pos = sim.simxGetObjectPosition(self.client_id, object_handle,
relative_to,
sim.simx_opmode_blocking)[1]
return np.array(pos)