def read_proximity_sensors(self, proximity_sensor_handle_list):
# We obtain the length of the joint name list
sensor_list_length = len(proximity_sensor_handle_list)
# We create an empty list to hold the joint error code that will be returned from the server
sensor_error_list = [0] * sensor_list_length
# We create an empty list to hold the detection state of the proximity sensor
detection_state_list = [False] * sensor_list_length
# We create an empty list to hold the point detected by the proximity sensor with respect to the sensor frame
detected_point_list = [[0, 0, 0]] * sensor_list_length
# We iterate through the proximity sensor handle list
for i in range(sensor_list_length):
# We check if it is the first time running the function
if self.first_time_list[6]:
# We obtain the result from coppeliasim in streaming mode
sensor_error_list[i], detection_state_list[i], detected_point_list[i], _, _ = \
sim.simxReadProximitySensor(self.client_id, proximity_sensor_handle_list[i],
sim.simx_opmode_streaming)
# We update the list and specify that we have run the function before
self.first_time_list[6] = False
# We have run this function before
else:
# We obtain the result from coppeliasim in buffer mode
sensor_error_list[i], detection_state_list[i], detected_point_list[i], _, _ = \
sim.simxReadProximitySensor(self.client_id, proximity_sensor_handle_list[i],
sim.simx_opmode_buffer)
# We only care about the absolute distance
detected_point_list[i] = (detected_point_list[i][0]**2 +
detected_point_list[i][1]**2 +
detected_point_list[i][2]**2)**0.5
# We return the error returned for each joint of the handle list and the joint property list
return sensor_error_list, detection_state_list, detected_point_list
def check_suction_prox(self):
_, self.right_state, _, self.right_object, _ = vrep.simxReadProximitySensor(
self.clientID, self.right_prox_sensor, vrep.simx_opmode_buffer)
_, self.left_state, _, self.left_object, _ = vrep.simxReadProximitySensor(
self.clientID, self.left_prox_sensor, vrep.simx_opmode_buffer)
if self.left_state and self.right_state:
return True
else:
return False
def drive(self, vL, vR):
#Method with receives the proposed wheel speeds but adjusts for obstacles
listReturnCodes = []
listDetectionStates = []
listDistances = []
detect = []
braitenbergL = [-0.2, -0.4, -0.6, -0.8, -1, -1.2, -1.4, -1.6]
braitenbergR = [-1.6, -1.4, -1.2, -1, -0.8, -0.6, -0.4, -0.2]
for i in range(0, 8):
[returnCode, detectionState, distance, d1,
d2] = sim.simxReadProximitySensor(clientID, self.sonarHandles[i],
sim.simx_opmode_buffer)
listReturnCodes.append(returnCode)
listDetectionStates.append(detectionState)
listDistances.append(distance)
if detectionState == 1 and np.linalg.norm(
distance) < self.antiCollisionDistance:
if np.linalg.norm(distance) < self.minFrontDistance:
distance = self.minFrontDistance
detect.append(1 - (
(np.linalg.norm(distance) - self.minFrontDistance) /
(self.antiCollisionDistance - self.minFrontDistance)))
else:
detect.append(0)
vLeft = vL
vRight = vR
for i in range(0, 8):
vLeft = vLeft + braitenbergL[i] * detect[i]
vRight = vRight + braitenbergR[i] * detect[i]
return vLeft, vRight
def read_sonar(ID, handlers, sonar_ready):
points = [None]*8
states = [False]*8
for i in range(8):
res, states[i], points[i], _, normal_vec = sim.simxReadProximitySensor(ID, handlers[i], sim.simx_opmode_buffer)
dists = [i[2] for i in points]
if(sonar_ready):
for i in range(len(dists)):
if(states[i] and dists[i] < 0.5):
if(dists[i] < 0.2):
dists[i] = 0.2
# map how close an obstacle is to the robot to [0, 1]
dists[i] = 1.0 - (dists[i] - 0.2) / (0.5 - 0.2)
else:
dists[i] = 0.0
return dists, sonar_ready
else:
flag = True
for i in range(len(dists)):
if(dists[i] == 0.0):
flag = False
break
if(flag):
sonar_ready = True
return None, sonar_ready
def actuation_sample(self, duration=60 * 5):
"""
This is a very simple EXAMPLE navigation program, which avoids obstacles using the Braitenberg algorithm
"""
self.v0 = 2.0
noDetectionDist = 0.5
maxDetectionDist = 0.2
detect = np.zeros(self.N_ULT_SENSOR)
braitenbergL = [
-0.2, -0.4, -0.6, -0.8, -1.0, -1.2, -1.4, -1.6, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0
]
braitenbergR = [
-1.6, -1.4, -1.2, -1.0, -0.8, -0.6, -0.4, -0.2, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0
]
# Start actuation loop
startTime = time.time()
while time.time() - startTime < duration:
for i in range(self.N_ULT_SENSOR):
# Sensing information from the proximity sensor
(
returnCode,
detectionState,
detectedPoint,
detectedObjectHandle,
detectedSurfaceNormalVector,
) = sim.simxReadProximitySensor(
self.clientID, self.u_sensors[i], sim.simx_opmode_blocking
# self.clientID, self.u_sensors[i], sim.simx_opmode_streaming
)
dist = np.linalg.norm(detectedPoint)
if detectionState and dist < noDetectionDist:
if dist < maxDetectionDist:
dist = maxDetectionDist
detect[i] = 1 - ((dist - maxDetectionDist) /
(noDetectionDist - maxDetectionDist))
else:
detect[i] = 0
vLeft = self.v0
vRight = self.v0
for i in range(self.N_ULT_SENSOR):
vLeft = vLeft + braitenbergL[i] * detect[i]
vRight = vRight + braitenbergR[i] * detect[i]
_ = sim.simxSetJointTargetVelocity(
# self.clientID, self.motorLeft, vLeft, sim.simx_opmode_blocking
self.clientID,
self.motorLeft,
vLeft,
sim.simx_opmode_oneshot)
_ = sim.simxSetJointTargetVelocity(
# self.clientID, self.motorRight, vRight, sim.simx_opmode_blocking
self.clientID,
self.motorRight,
vRight,
sim.simx_opmode_oneshot)
def __init__(self, clientID):
self.clientID = clientID
self.pioneer3DX_array = [None] * 19
self.position_coordX = [None] * 3
self.position_coordXc = [None] * 3
self.orientation = None
self.velocity = [None]* 2
self.ultrasonic = np.zeros((16, 3))
self.name = "Pioneer_p3dx"
self.left_motor = 'Pioneer_p3dx_leftMotor'
self.right_motor = 'Pioneer_p3dx_rightMotor'
self.ultrasonic_sensors = "Pioneer_p3dx_ultrasonicSensor"
### Load Mobile Robot Pioneer parameters
# self.pioneer3DX_array[0] represents the entire mobile robot block
error,self.pioneer3DX_array[0] = sim.simxGetObjectHandle(self.clientID,self.name,sim.simx_opmode_blocking)
# self.pioneer3DX_array[1] represents only the left motor
error,self.pioneer3DX_array[1] = sim.simxGetObjectHandle(self.clientID,self.left_motor,sim.simx_opmode_blocking)
# self.pioneer3DX_array[2] represents only the right motor
error,self.pioneer3DX_array[2] = sim.simxGetObjectHandle(self.clientID,self.right_motor,sim.simx_opmode_blocking)
# self.pioneer3DX_array[3:18] represents the 16 ultrasonic sensors
num = 1
while num < 17:
error,self.pioneer3DX_array[2+num] = sim.simxGetObjectHandle(self.clientID,self.ultrasonic_sensors+str(num),sim.simx_opmode_blocking)
error, DetectionState, Points ,detectedObjectHandle, Vector = sim.simxReadProximitySensor(self.clientID,self.pioneer3DX_array[2+num],sim.simx_opmode_streaming)
num+=1
error, linearVelocity, angularVelocity = sim.simxGetObjectVelocity(self.clientID,self.pioneer3DX_array[0], sim.simx_opmode_streaming)
error, position = sim.simxGetObjectPosition(self.clientID,self.pioneer3DX_array[0],-1, sim.simx_opmode_streaming)
error, angle = sim.simxGetObjectOrientation(self.clientID,self.pioneer3DX_array[0],-1,sim.simx_opmode_streaming)
print("Pioneer 3DX loaded")
def read_proximity_sensor(sensor_handle):
return_tuple = sim.simxReadProximitySensor(clientID, sensor_handle, sim.simx_opmode_blocking)
return_code = return_tuple[0]
if not return_code:
detection_state = return_tuple[1]
detected_point = return_tuple[2]
return detection_state, detected_point[2]
def getStatusDist(self):
if self.craneStatus and self.connectionStatus:
# Get proximity sensor status
status = sim.simxReadProximitySensor(self.clientID,
self.proximity_sensor,
sim.simx_opmode_buffer)[1]
return status
return -1
def get_distance(self, sensor, max_dist):
_, sta, point, objh, vec = sim.simxReadProximitySensor(
self.clientID, sensor, sim.simx_opmode_streaming)
if (sta == False): #no se detecto nada
distance = max_dist
else:
distance = math.sqrt(point[0]**2 + point[1]**2 + point[2]**2)
return distance
def read(self) -> (bool, Vec3):
"""
Reads the state of a proximity sensor.
@return detection state and detected point
@rtype (bool, Vec3)
"""
code, state, point, handle, snv = s.simxReadProximitySensor(
self._id, self._handle, self._def_op_mode)
return state, Vec3(point[0], point[1], point[2])
def _init_sensors(self) -> List[Any]:
"""Acquire ultrasonic sensor handles and initialize US and encoder streaming.
Returns: List with ultrasonic sensor handles.
"""
self._init_encoders()
sensors = [None] * len(self.SENSORS)
for i in range(len(sensors)):
sensor_name = "Pioneer_p3dx_ultrasonicSensor" + str(i + 1)
rc, handle = sim.simxGetObjectHandle(self._client_id, sensor_name,
sim.simx_opmode_blocking)
sensors[i] = handle
sim.simxReadProximitySensor(self._client_id, sensors[i],
sim.simx_opmode_streaming)
return sensors
def getSonar(clientID, hRobot):
r = [1.0] * 16
for i in range(16):
handle = hRobot[1][i]
e, s, p, _, _ = sim.simxReadProximitySensor(clientID, handle,
sim.simx_opmode_buffer)
if e == sim.simx_return_ok and s:
r[i] = math.sqrt(p[0] * p[0] + p[1] * p[1] + p[2] * p[2])
return r
def getDists(clientID, objectHandles):
""" retorna a distancia entre um objeto e o robô """
dists = []
# extraindo as distancias lidas pelos sensores ultrassônicos (b), se houve objeto detectado (a == True).
for i in range(7):
x, a, b, c, d = sim.simxReadProximitySensor(clientID, objectHandles[i],
sim.simx_opmode_buffer)
dists.append((a, b[2]))
return dists
def read_proximity_sensor(sensor_handle):
return_tuple = sim.simxReadProximitySensor(clientID, sensor_handle,
sim.simx_opmode_blocking)
return_code = return_tuple[0]
if not return_code:
detection_state = return_tuple[1]
detected_distance = return_tuple[2][2]
if not detection_state or detected_distance < 0:
detected_distance = None
return detected_distance
def getDistanceIR(sensor):
max_distance_IR = 1
erro = 1
while (erro != 0):
erro, detectable, distancePoint, detectedObjectHandle, detectedSurface = sim.simxReadProximitySensor(clientID, sensor, sim.simx_opmode_streaming)
#print(erro, detectable, distancePoint, detectedObjectHandle, detectedSurface)
distance = distancePoint[2]
if(detectable == False):
distance = max_distance_IR
#print(distance)
return distance
def getSensorsData(self):
sensor_val = []
for i in range(8):
handle = self.sensor[i]
errorCode, det_State, det_Point, det_Object, NormalVector = sim.simxReadProximitySensor(
self.clientID, handle, sim.simx_opmode_buffer)
if det_State:
sensor_val.append((0.2 - np.linalg.norm(det_Point)) / 0.2)
else:
sensor_val.append(0)
return sensor_val
def update_distances(self):
self._sensor_vals = np.zeros(16)
for i, handle in enumerate(self._handles):
_, det_state, det_point, det_obj_handle, det_surface_norm = \
sim.simxReadProximitySensor(
self.client_id, handle, self.op_mode
)
# print(f"i: {i}\n"
# f"det_point: {det_point}\n"
# f"Norm {np.linalg.norm(det_point)}")
self._sensor_vals[i] = np.linalg.norm(det_point) if np.linalg.norm(det_point) > 1e-10 else np.Inf
def get_UltrasonicData(self):
# Get ultrasonic data from Pioneer
num = 1
while num < 17:
error, DetectionState, self.ultrasonic[
num -
1, :], detectedObjectHandle, Vector = sim.simxReadProximitySensor(
self.clientID, self.pioneer3DX_array[2 + num],
sim.simx_opmode_buffer)
self.ultrasonic[num - 1, 2] = np.linalg.norm(
self.ultrasonic[num - 1, 0:2]) * DetectionState
num += 1
print(self.ultrasonic[:, 2])
def getRobotHandles(clientID):
# Motor handles
_, lmh = sim.simxGetObjectHandle(clientID, 'Pioneer_p3dx_leftMotor',
sim.simx_opmode_blocking)
_, rmh = sim.simxGetObjectHandle(clientID, 'Pioneer_p3dx_rightMotor',
sim.simx_opmode_blocking)
# Sonar handles
str = 'Pioneer_p3dx_ultrasonicSensor%d'
sonar = [0] * 16
for i in range(16):
_, h = sim.simxGetObjectHandle(clientID, str % (i + 1),
sim.simx_opmode_blocking)
sonar[i] = h
sim.simxReadProximitySensor(clientID, h, sim.simx_opmode_streaming)
# Camera handles
_, cam = sim.simxGetObjectHandle(clientID, 'Pioneer_p3dx_camera',
sim.simx_opmode_oneshot_wait)
sim.simxGetVisionSensorImage(clientID, cam, 0, sim.simx_opmode_streaming)
sim.simxReadVisionSensor(clientID, cam, sim.simx_opmode_streaming)
return [lmh, rmh], sonar, cam
def _readsensor_continue(self):
sensor_bool = [False] * self.total_sensors
sensor_point = np.zeros((self.total_sensors, 3))
sensor_data = np.ones((self.total_sensors, 1))
for i in range(self.total_sensors):
_, sensor_bool[i], sensor_point[
i], _, _ = sim.simxReadProximitySensor(self.clientID,
self.sensor[i],
sim.simx_opmode_buffer)
sensor_point[i] = np.round_(sensor_point[i], 3)
if (sensor_bool[i]):
sensor_data[i] = np.round_(
math.sqrt(sensor_point[i][0]**2 + sensor_point[i][1]**2 +
sensor_point[i][2]**2), 3)
return sensor_data
def calc_ball_position(clientID, sensor_handle, path, detectedPoints):
found1 = False
found2 = False
count = 0
for i in range(path.shape[0]):
detectionData = simxReadProximitySensor(clientID, sensor_handle,
simx_opmode_streaming)
print(i) #too little time between reads results in garbage data
if (
detectionData[1] and not found1
): #detectionData[1]: ball detection is true and first point hasn't been found
detectedPoints[0] = detectionData[2]
found1 = True
#wait a sufficient amount of time to get a second position, so that a "velocity"/slope of movement can be found
if (detectionData[1] and found1 and not found2):
if (count > 50):
detectedPoints[1] = detectionData[2]
return
count += 1
def braitenberg(clientID, usensor):
"""
Braitenberg algorithm for the front sensors of the pioneer 3dx
"""
for i in range(len(usensor)):
err, state, point, detectedObj, detectedSurfNormVec = vrep.simxReadProximitySensor(
clientID, usensor[i], vrep.simx_opmode_oneshot)
distance = np.linalg.norm(point)
# if a detection occurs
if state and (distance <
noDetectDist): # don't care about distant objects
distance = max(distance, maxDist)
detectW[i] = 1 - ((distance - maxDist) /
(noDetectDist - maxDist)) # Normalize the weight
else:
detectW[i] = 0
dL = np.sum(braitenbergL * detectW)
dR = np.sum(braitenbergR * detectW)
vLeft = v0 + dL
vRight = v0 + dR
avoid = True if (abs(dL) + abs(dR)) else False
return avoid, vLeft, vRight
def _intial(self):
#handles
#bot_handle
_, self.bot = sim.simxGetObjectHandle(self.clientID, "Bot_collider",
sim.simx_opmode_blocking)
_, _ = sim.simxGetObjectPosition(self.clientID, self.bot, -1,
sim.simx_opmode_streaming)
#wheels
_, self.left_wheel = sim.simxGetObjectHandle(self.clientID,
"Left_wheel_joint",
sim.simx_opmode_blocking)
_, self.right_wheel = sim.simxGetObjectHandle(self.clientID,
"Right_wheel_joint",
sim.simx_opmode_blocking)
#Wall_collection
_, self.wall = sim.simxGetCollectionHandle(self.clientID, "wall",
sim.simx_opmode_blocking)
#Target
_, self.target = sim.simxGetObjectHandle(self.clientID, "target",
sim.simx_opmode_blocking)
_, _ = sim.simxGetObjectPosition(self.clientID, self.target, -1,
sim.simx_opmode_streaming)
#Sensors
for i in range(self.total_sensors):
proxy = "proxy_" + str(i) + "_"
_, self.sensor[i] = sim.simxGetObjectHandle(
self.clientID, proxy, sim.simx_opmode_blocking)
_, _, _, _, _ = sim.simxReadProximitySensor(
self.clientID, self.sensor[i], sim.simx_opmode_streaming)
#Sensor velocity
_, _, _ = sim.simxGetObjectVelocity(self.clientID, self.bot,
sim.simx_opmode_streaming)
#Collision Data Stream
_, _ = sim.simxGetIntegerSignal(self.clientID, "collision_wall",
sim.simx_opmode_streaming)
_, _ = sim.simxGetIntegerSignal(self.clientID, "collision_target",
sim.simx_opmode_streaming)
self.reset = False
def init_connection(self, ip='127.0.0.1', port=19997):
#print ('Program started')
sim.simxFinish(-1) # just in case, close all opened connections
clientID = sim.simxStart(ip, port, True, True, 5000,
5) # Connect to CoppeliaSim
if clientID == -1:
return -1, False
self.clientID = clientID
self.connectionStatus = True
# Get Coppelia Objects ID
self.boom = sim.simxGetObjectHandle(clientID, 'Atuador_braco',
sim.simx_opmode_blocking)[-1]
self.claw = sim.simxGetObjectHandle(clientID, 'Atuador_garra',
sim.simx_opmode_blocking)[-1]
self.crane = sim.simxGetObjectHandle(clientID, 'Atuador_guindaste',
sim.simx_opmode_blocking)[-1]
self.magnet = sim.simxGetObjectHandle(clientID, 'suctionPad',
sim.simx_opmode_blocking)[-1]
self.cam = sim.simxGetObjectHandle(clientID, 'Vision_sensor',
sim.simx_opmode_blocking)[-1]
self.cam2 = sim.simxGetObjectHandle(clientID, 'cam2',
sim.simx_opmode_blocking)[-1]
self.proximity_sensor = sim.simxGetObjectHandle(
clientID, 'Proximity_sensor', sim.simx_opmode_blocking)[-1]
self.boomStructure = sim.simxGetObjectHandle(
clientID, 'Braco', sim.simx_opmode_blocking)[-1]
self.err_code, _, _ = sim.simxGetVisionSensorImage(
clientID, self.cam, 0, sim.simx_opmode_streaming)
self.err_code, _, _ = sim.simxGetVisionSensorImage(
clientID, self.cam2, 0, sim.simx_opmode_streaming)
self.status = sim.simxReadProximitySensor(clientID,
self.proximity_sensor,
sim.simx_opmode_streaming)[1]
return self.clientID, self.connectionStatus
def SEARCHING_BEAR(tshirt_x, unique_values, clientID, left_motor, right_motor,
prox_sensor):
# Function returns isRescueDone, isReadyToSearch
# Calculated offset
delta = fun.controllerMove(tshirt_x)
if (len(unique_values) == 1) and (unique_values[0] == 0):
fun.groundMovement('TURN_RIGHT', clientID, left_motor, right_motor,
TURNING_SPEED)
else:
sim.simxSetJointTargetVelocity(clientID, left_motor,
APPROACHING_BEAR_SPEED + delta,
sim.simx_opmode_oneshot)
sim.simxSetJointTargetVelocity(clientID, right_motor,
APPROACHING_BEAR_SPEED - delta,
sim.simx_opmode_oneshot)
isDetecting = sim.simxReadProximitySensor(clientID, prox_sensor,
sim.simx_opmode_oneshot)[1]
if isDetecting:
fun.groundMovement('STOP', clientID, left_motor, right_motor, 0.0)
return True, False
return False, True
def sense(self) -> Tuple[List[float], float, float]:
"""Read ultrasonic sensors and encoders.
Returns:
z_us: Distance from every ultrasonic sensor to the closest obstacle [m].
z_v: Linear velocity of the robot center [m/s].
z_w: Angular velocity of the robot center [rad/s].
"""
# Read ultrasonic sensors
z_us = [float('inf')] * len(self.SENSORS)
for i in range(len(self._sensors)):
_, is_valid, detected_point, _, _ = sim.simxReadProximitySensor(
self._client_id, self._sensors[i], sim.simx_opmode_buffer)
if is_valid is True:
distance = np.linalg.norm(detected_point)
z_us[i] = distance
# Read encoders
z_v, z_w = self._sense_encoders()
return z_us, z_v, z_w
def threaded():
while True:
err_code, detectionState, detectedPoint, detectedObjectHandle, detectedSurfaceNormalVector = sim.simxReadProximitySensor(
clientID, sensor, sim.simx_opmode_streaming)
sleep(0.5)
start_sim = True # simxGetPingTime(clientID) _, drone = sim.simxGetObjectHandle(clientID, 'my_drone', sim.simx_opmode_blocking) _, target = sim.simxGetObjectHandle(clientID, 'Quadcopter_target', sim.simx_opmode_blocking) _, camera = sim.simxGetObjectHandle(clientID, 'Main_camera', sim.simx_opmode_oneshot_wait) _, h_sensor = sim.simxGetObjectHandle(clientID, 'H_sensor', sim.simx_opmode_oneshot_wait) _, gps = sim.simxGetStringSignal(clientID, 'gps', sim.simx_opmode_streaming) _, gyro = sim.simxGetStringSignal(clientID, 'gyro', sim.simx_opmode_streaming) _, accel = sim.simxGetStringSignal(clientID, 'nydavai', sim.simx_opmode_streaming) _, velocity, _ = sim.simxGetObjectVelocity(clientID, drone, sim.simx_opmode_streaming) _, euler = sim.simxGetObjectOrientation(clientID, drone, target, sim.simx_opmode_streaming) _, h_detected, h_point, h_det_obj, h_norm = sim.simxReadProximitySensor(clientID, h_sensor, sim.simx_opmode_streaming) _, orientation_target = sim.simxGetObjectOrientation(clientID, target, -1, sim.simx_opmode_streaming) _, orientation_drone = sim.simxGetObjectOrientation(clientID, drone, -1, sim.simx_opmode_streaming) _, location_drone = sim.simxGetObjectPosition(clientID, drone, -1, sim.simx_opmode_streaming) _, location_target = sim.simxGetObjectPosition(clientID, target, -1, sim.simx_opmode_streaming) _, resolution, image = sim.simxGetVisionSensorImage(clientID, camera, 0, sim.simx_opmode_streaming) pid1 = PID_controll(2, 0, 0) pid1.k_v = -2 pid2 = PID_controll(0.005, 0, 1) pid3 = PID_controll(-0.005, 0, -1) pid4 = PID_controll(0.1, 0, 2) pid5 = PID_controll(0.25, 0, 2.1) pid6 = PID_controll(-0.25, 0, -2.1) controller = control_positioon() pev_time = 0
err, motorR = vrep.simxGetObjectHandle(clientID, 'Pioneer_p3dx_rightMotor',
vrep.simx_opmode_blocking)
err, robot = vrep.simxGetObjectHandle(clientID, 'Pioneer_p3dx',
vrep.simx_opmode_blocking)
# Assigning handles to the ultrasonic sensors
usensor = []
for i in range(1, 17):
err, s = vrep.simxGetObjectHandle(clientID,
'Pioneer_p3dx_ultrasonicSensor' + str(i),
vrep.simx_opmode_blocking)
usensor.append(s)
# Sensor initialization
for i in range(16):
err, state, point, detectedObj, detectedSurfNormVec = vrep.simxReadProximitySensor(
clientID, usensor[i], vrep.simx_opmode_streaming)
#<-----------------------------------Control----------------------------------------->
goals = [(-6.0, 0.5), (-5.5, -3.5), (-4.5, 1.5), (-3.0, 3.5), (-1.5, -5.0),
(-0.5, -6.5), (1.5, -4.5), (2.5, -0.5), (3.5, -1.5), (6.5, -4.5)]
p = np.array(goals)
path = pc.splinePath(p[:, 0], p[:, 1])
pointsx = np.linspace(min(p[:, 0]), max(p[:, 0]), num=60, endpoint=True)
pointsy = path(pointsx)
step = 0
errp = 10
achieved = 0
avoid = False
while step < len(pointsx) and achieved < len(goals):
def get_ball_info(clientID):
"""
stores the linear velocity and the position variable of the last time that the ball hit the wall
stores it in the global coordinates ball_coord and ball_linear
@param clientID: ID used to connect to CoppeliaSim's environment
@return None
"""
global ball_coord
global ball_linear
global hit_wall
global ball_hit
global detect_handle
global prox_handle
global stop_prog
print("Getting ball info: ")
# while 1:
# if cnt == 0:
# ret, ball_handle = sim.simxGetObjectHandle(clientID, 'Sphere', sim.simx_opmode_blocking)
# ret, ball_coord = sim.simxGetObjectPosition(clientID, ball_handle, -1, sim.simx_opmode_streaming)
# ret, ball_linear, ball_angular = sim.simxGetObjectVelocity(clientID, ball_handle, sim.simx_opmode_streaming)
# cnt += 1
# else:
# ret, ball_coord = sim.simxGetObjectPosition(clientID, ball_handle, -1, sim.simx_opmode_buffer)
# ret, ball_linear_new, ball_angular = sim.simxGetObjectVelocity(clientID, ball_handle, sim.simx_opmode_buffer)
# if(ball_linear_new[1] < 0 and ball_linear[1] > 0):
# #if we come here, then the velocity direction has changed, meaning we hit the wall
# hit_wall = True
# detect_handle = sim.simxGetObjectHandle(clientID, 'Sphere', sim.simx_opmode_blocking)
# print("Sphere Handle: ", detect_handle[1])
# prox_handle = sim.simxGetObjectHandle(clientID, 'Proximity_sensor', sim.simx_opmode_blocking)
# print("Proximity Sensor Handle: ", prox_handle[1])
y = 0
points = []
velocities = []
leave = 0
while True:
if stop_prog:
break
if ball_hit:
#print("We hit the ball, we are now processing")
y = 0 #Represents if the ball is moving away from the wall or not
else:
#print("waiting for ball to be hit")
continue
while (y == 0): #While ball still coming towards wall
# read prox sensor and get detected points
ret, dS, dP, dOH, dSNV = sim.simxReadProximitySensor(
clientID, prox_handle[1], sim.simx_opmode_buffer)
ret, linear, angular = sim.simxGetObjectVelocity(
clientID, detect_handle[1], sim.simx_opmode_buffer)
# detecting
if (dS == 1):
# store all the detected points
points.append(dP)
velocities.append(linear)
leave = 1
# not detecting and is heading away from wall
elif (dS == 0 and leave == 1):
y = 1
hit_wall = True #To show we have just hit the wall
ball_hit = False #To show that the ball has not been hit yet. Ideally, we don't start this loop
#until the ball has been hit
leave = 0
print("Storing left velocity and position")
if len(velocities) > 0:
ball_linear = velocities[-1]
if len(points) > 0:
ball_coord = points[-1]
velocities = []
points = []
