def go_to_z(z1):
global x, y, z
err, Qpos = vrep.simxGetObjectPosition(clientID, QuadricopterT, -1,
vrep.simx_opmode_oneshot_wait)
x = Qpos[0]
y = Qpos[1]
z = Qpos[2]
if z < z1:
sign = -1
elif z > z1:
sign = 1
else:
return 0
while abs(z - z1) > 0.01:
err = vrep.simxSetObjectPosition(clientID, QuadricopterT, -1,
(x, y, z),
vrep.simx_opmode_oneshot_wait)
time.sleep(0.2)
z -= sign * 0.01
print(z)
print('\n')
err = vrep.simxSetObjectPosition(clientID, QuadricopterT, -1, (x, y, z1),
vrep.simx_opmode_oneshot_wait)
time.sleep(0.2)
err, Qpos = vrep.simxGetObjectPosition(clientID, QuadricopterT, -1,
vrep.simx_opmode_oneshot_wait)
print(Qpos)
z = z1
def go_to_y(y1):
global x, y, z
err, Qpos = vrep.simxGetObjectPosition(clientID, QuadricopterT, -1,
vrep.simx_opmode_oneshot_wait)
x = Qpos[0]
y = Qpos[1]
z = Qpos[2]
if y < y1:
sign = -1
elif y > y1:
sign = 1
else:
return 0
while abs(y - y1) > 0.01:
err = vrep.simxSetObjectPosition(clientID, QuadricopterT, -1,
(x, y, z),
vrep.simx_opmode_oneshot_wait)
time.sleep(0.2)
y -= sign * 0.01
print(y)
print('\n')
err = vrep.simxSetObjectPosition(clientID, QuadricopterT, -1, (x, y1, z),
vrep.simx_opmode_oneshot_wait)
time.sleep(0.2)
err, Qpos = vrep.simxGetObjectPosition(clientID, QuadricopterT, -1,
vrep.simx_opmode_oneshot_wait)
print(Qpos)
y = y1
def __init__(self):
try:
sim.simxFinish(-1) #close all opened connections
clientID = sim.simxStart('127.0.0.1', 19999, True, True, 5000,
5) # Connect to CoppeliaSim
if clientID != -1:
print('connect successfully')
else:
sys.exit("connect error")
except:
print('Check if CoppeliaSim is open')
_, Quadcopter_target = sim.simxGetObjectHandle(
clientID, 'Quadricopter_target', sim.simx_opmode_blocking)
_, targetPosition = sim.simxGetObjectPosition(clientID,
Quadcopter_target, -1,
sim.simx_opmode_buffer)
print(targetPosition)
ArrayPosition = [-0.18570, 0.99366, 0.615]
sim.simxSetObjectPosition(clientID, Quadcopter_target, -1,
ArrayPosition, sim.simx_opmode_oneshot)
self.clientID = clientID
self.Quadcopter_target = Quadcopter_target
self.targetPosition = targetPosition
def movePackage(self, pkg_name, pos, handle=None):
_, pkg = sim.simxGetObjectHandle(self.client, pkg_name,
sim.simx_opmode_blocking)
sim.simxSetObjectPosition(self.client, pkg, -1, pos,
sim.simx_opmode_blocking)
if handle:
sim.simxSetObjectParent(self.client, pkg, handle, False,
sim.simx_opmode_blocking)
def set_object_position(self):
if self.cate is Category.CONST:
return
x, y = self.pos[0:2]
z = self.z_displacement + self.z_dim / 2
x, y = x - self.x_displacement, y - self.y_displacement
sim.simxSetObjectPosition(self.clientID, self.handle, self.parent,
[x, y, z], sim.simx_opmode_blocking)
self.actual_pos = self.pos.copy()
def move_zz(self, length):
clientID = self.clientID
Quadcopter_target = self.Quadcopter_target
targetPosition = self.targetPosition
sim.simxSetObjectPosition(clientID, Quadcopter_target, -1,
targetPosition, sim.simx_opmode_oneshot)
targetPosition[2] = targetPosition[2] - length
def init_sensors(self, clientID): self.clientID = clientID # initialize proximity sensors err, detectionState, detectedPoint, detectedObjectHandle, detectedSurfaceNormalVector = sim.simxReadProximitySensor(self.clientID, self.quadHandle, sim.simx_opmode_streaming) # initialize rotors sim_rotors.init_rotors(self.clientID) # reset quadrotor position sim.simxSetObjectPosition(self.clientID, self.quadHandle, -1, self.initial_position, sim.simx_opmode_oneshot)
def grasp_object(self, object_handle, parent_handle):
res, rot = sim.simxGetObjectOrientation(self.clientID, object_handle,
parent_handle,
sim.simx_opmode_blocking)
sim.simxSetObjectParent(self.clientID, object_handle, self.endEffector,
False, sim.simx_opmode_blocking)
sim.simxSetObjectPosition(
self.clientID, object_handle, sim.sim_handle_parent, np.zeros(3),
sim.simx_opmode_blocking) # assume it has been grasped
sim.simxSetObjectOrientation(
self.clientID, object_handle, self.endEffector, rot,
sim.simx_opmode_blocking) # not changeing the object orientation
return rot
def step(self, action):
if action <=5:
x,y,z = self.moves[action]
self.S = [self.S[0] + x, self.S[1] + y, self.S[2] + z]
if self.S[0]<0 or self.S[1]<0 or self.S[2]<0:
self.S = [max(0, self.S[0]), max(0, self.S[1]), max(0, self.S[2])]
return self.S,-10,False,{}
if self.S[0]>=5 or self.S[1]>=5 or self.S[2]>=10:
self.S = [min(self.S[0], 5 - 1), min(self.S[1], 5 - 1), min(self.S[2], 10 - 1)]
return self.S,-10,False,{}
else:
sim.simxSetIntegerSignal(self.clientID,'actuateGripperSignal',0,sim.simx_opmode_oneshot)
sim.simxSetObjectPosition(self.clientID, self.target,-1, [0+0.1*self.S[0],0+0.1*self.S[1],0+0.1*self.S[2]],sim.simx_opmode_oneshot) #starting point is (0.3,0.3,0.3)
while True:
distance = sim.simxGetObjectPosition(self.clientID,self.copter,self.target,sim.simx_opmode_oneshot_wait)
if np.abs(np.linalg.norm(np.array(distance[1])))<0.02:
time.sleep(2)
break
# if (sim.simxGetCollisionHandle(self.clientID,))
return self.S,-1,False,{}
elif action==6:
# pdb.set_trace()
gripperAngle = 150
angle_displacement = (gripperAngle/2-45)*math.pi/180
err, joint_6 = sim.simxGetObjectHandle(self.clientID, "Fourbar_joint6", sim.simx_opmode_oneshot)
# sim.simxClearIntegerSignal(self.clientID,'actuateGripperSignal',sim.simx_opmode_oneshot)
# sim.simxClearIntegerSignal(self.clientID,'gripperAngle',sim.simx_opmode_oneshot)
sim.simxSetIntegerSignal(self.clientID,'actuateGripperSignal',1,sim.simx_opmode_oneshot)
sim.simxSetIntegerSignal(self.clientID,'gripperAngle',gripperAngle,sim.simx_opmode_oneshot)
while True:
current_joint6_angle = sim.simxGetJointPosition(self.clientID, joint_6, sim.simx_opmode_oneshot)
if abs(abs(current_joint6_angle[1])-abs(angle_displacement))<0.05*math.pi/180:
time.sleep(5)
break
oldPos = sim.simxGetObjectPosition(self.clientID, self.sphere, -1, sim.simx_opmode_oneshot)
sim.simxSetObjectPosition(self.clientID, self.target, self.copter, [0,0,0.5], sim.simx_opmode_oneshot)
while True:
distance = sim.simxGetObjectPosition(self.clientID,self.copter,self.target,sim.simx_opmode_oneshot_wait)
if np.abs(np.linalg.norm(np.array(distance[1])))<0.02:
time.sleep(2)
break
newPos = sim.simxGetObjectPosition(self.clientID, self.sphere, -1, sim.simx_opmode_oneshot)
if np.linalg.norm(np.array(newPos[1])-np.array(oldPos[1]))>0.4 and np.linalg.norm(np.array(newPos)-np.array(oldPos))<0.5:
return self.S,100,True,{}
else:
return self.S,-100,True,{}
def step(self, action, reset):
if sys.version_info[0] == 3:
_ = sim.simxSetObjectPosition(self.clientID, self.target, -1,
self.target_position,
sim.simx_opmode_oneshot)
sensor_data = np.zeros((self.total_sensors), dtype=np.float32)
vel_reading = np.zeros((2), dtype=np.float32)
angular_reading = 0
collision = np.zeros((2), dtype=np.float32)
target_location = np.zeros((3), dtype=np.float32)
target_location = np.round_(
np.subtract(np.array(self.position[:2]),
np.array(self.target_position[:2])), 3)
if (reset == False):
if (self.flag):
_, target_location = sim.simxGetObjectPosition(
self.clientID, self.target, -1, sim.simx_opmode_buffer)
_, bot_location = sim.simxGetObjectPosition(
self.clientID, self.bot, -1, sim.simx_opmode_buffer)
target_location = np.round_([
bot_location[0] - target_location[0],
bot_location[1] - target_location[1]
], 3)
self.flag = True
speed = (self.velocity * action[0]) / 100
turn = (self.angular_velocity * action[1]) / 100
l_wheel_vel = round(
(speed - self.wheel_basis * turn) / self.wheel_radius, 4)
r_wheel_vel = round(
(speed + self.wheel_basis * turn) / self.wheel_radius, 4)
_ = sim.simxSetJointTargetVelocity(self.clientID,
self.left_wheel,
l_wheel_vel,
sim.simx_opmode_streaming)
_ = sim.simxSetJointTargetVelocity(self.clientID,
self.right_wheel,
r_wheel_vel,
sim.simx_opmode_streaming)
#Collision
_, collision[0] = sim.simxGetIntegerSignal(
self.clientID, "collision_wall", sim.simx_opmode_buffer)
_, collision[1] = sim.simxGetIntegerSignal(
self.clientID, "collision_target", sim.simx_opmode_buffer)
sensor_data = self._readsensor_continue()
vel_reading, angular_reading = self._get_velocity_reading_continue(
)
else:
self._create(self.position)
#_ = sim.simxSetObjectPosition(self.clientID, self.target, self.bot, self.target_position, sim.simx_opmode_oneshot)
#target_location = self.position - self.target_position
sim.simxSynchronousTrigger(self.clientID)
return sensor_data, vel_reading, angular_reading, target_location, collision
else:
raw_input('Press <enter> key to step the !')
def set_position(self, x=-6.5, y=-12, z=0.3):
error = sim.simxSetModelProperty(self.clientId, self.pioneerHandle,
sim.sim_modelproperty_not_dynamic,
sim.simx_opmode_oneshot)
if error == sim.simx_return_ok:
print(str(error) + '! ERROR: simxSetModelProperty pioneer')
error = sim.simxSetModelProperty(self.clientId, self.leftMotorHandle,
sim.sim_modelproperty_not_dynamic,
sim.simx_opmode_oneshot)
if error == sim.simx_return_ok:
print(
str(error) +
'! ERROR: simxSetModelProperty self.leftMotorHandle')
error = sim.simxSetModelProperty(self.clientId, self.rightMotorHandle,
sim.sim_modelproperty_not_dynamic,
sim.simx_opmode_oneshot)
if error == sim.simx_return_ok:
print(
str(error) +
'! ERROR: simxSetModelProperty self.rightMotorHandle')
error = sim.simxSetModelProperty(self.clientId, self.casterFreeHandle,
sim.sim_modelproperty_not_dynamic,
sim.simx_opmode_oneshot)
if error == sim.simx_return_ok:
print(
str(error) +
'! ERROR: simxSetModelProperty self.casterFreeHandle')
error = sim.simxSetObjectPosition(self.clientId, self.pioneerHandle,
-1, (x, y, z),
sim.simx_opmode_oneshot_wait)
if error != sim.simx_return_ok:
print(str(error) + '! ERROR: simxSetObjectPosition pioneer')
error = sim.simxSetModelProperty(self.clientId, self.pioneerHandle,
not sim.sim_modelproperty_not_dynamic,
sim.simx_opmode_oneshot)
if error != sim.simx_return_ok:
print(str(error) + '! ERROR: simxSetModelProperty pioneer')
error = sim.simxSetModelProperty(self.clientId, self.leftMotorHandle,
not sim.sim_modelproperty_not_dynamic,
sim.simx_opmode_oneshot)
if error != sim.simx_return_ok:
print(
str(error) +
'! ERROR: simxSetModelProperty self.leftMotorHandle')
error = sim.simxSetModelProperty(self.clientId, self.rightMotorHandle,
not sim.sim_modelproperty_not_dynamic,
sim.simx_opmode_oneshot)
if error != sim.simx_return_ok:
print(
str(error) +
'! ERROR: simxSetModelProperty self.rightMotorHandle')
error = sim.simxSetModelProperty(self.clientId, self.casterFreeHandle,
not sim.sim_modelproperty_not_dynamic,
sim.simx_opmode_oneshot)
if error != sim.simx_return_ok:
print(
str(error) +
'! ERROR: simxSetModelProperty self.casterFreeHandle')
def spiral_move(radius, height):
err, Qpos = vrep.simxGetObjectPosition(clientID, QuadricopterT, -1,
vrep.simx_opmode_oneshot_wait)
x = Qpos[0]
y = Qpos[1]
z = Qpos[2]
go_to_z(height)
z = height
theta = 0
go_to_x(x + radius * np.cos(theta))
time.sleep(2)
theta = 0
while True:
err = vrep.simxSetObjectPosition(
clientID, QuadricopterT, -1,
(x + radius * np.cos(theta), y + radius * np.sin(theta), z),
vrep.simx_opmode_oneshot_wait)
time.sleep(0.01)
theta += np.pi / (90 * radius + 1)
radius /= 1.001
err, Qpos = vrep.simxGetObjectPosition(clientID, QuadricopterT, -1,
vrep.simx_opmode_oneshot_wait)
print(Qpos[0]**2 + Qpos[1]**2)
def _create(self, position):
_ = sim.simxRemoveModel(self.clientID, self.bot,
sim.simx_opmode_blocking)
error = sim.simxLoadModel(self.clientID, self.roomba_path, 0,
sim.simx_opmode_blocking)
self._intial()
error = sim.simxSetObjectPosition(self.clientID, self.bot, -1,
position, sim.simx_opmode_oneshot)
def set_position(self, position, relative_object=-1):
# By default, get the position wrt the reference frame
if relative_object != -1:
relative_object = self._get_handler(relative_object)
err_code = sim.simxSetObjectPosition(self.client_id, self.frame,
relative_object, position, sim.simx_opmode_oneshot)
if err_code != 0:
print("ERROR: CANNOT SET POSITION W.R.T. {} TO {}".format(relative_object, position))
def reset(self):
# stop the simulation and restart the simulation
self.S = [0,0,9]
# sim.simxStartSimulation(self.clientID,sim.simx_opmode_oneshot)
sim.simxSetObjectPosition(self.clientID,self.target,-1,[0,0,1],sim.simx_opmode_oneshot)
sim.simxSetObjectPosition(self.clientID,self.sphere,-1,[0,0,0.175],sim.simx_opmode_oneshot)
while True:
distance = sim.simxGetObjectPosition(self.clientID,self.copter,self.target,sim.simx_opmode_oneshot_wait)
sim.simxSetIntegerSignal(self.clientID,'gripperAngle',90,sim.simx_opmode_oneshot)
sim.simxSetIntegerSignal(self.clientID,'actuateGripperSignal',0,sim.simx_opmode_oneshot)
angle = sim.simxGetJointPosition(self.clientID,self.joint_6,sim.simx_opmode_oneshot)
if np.abs(np.linalg.norm(np.array(distance[1])))<0.02 and abs(angle[1])<0.5*math.pi/180:
sim.simxSetIntegerSignal(self.clientID,'actuateGripperSignal',0,sim.simx_opmode_oneshot)
time.sleep(5)
break
return self.S
def setAbsolutePose(handle, pos, rot):
res1 = sim.simxSetObjectPosition(clientID, handle, -1, pos,
sim.simx_opmode_oneshot)
# print(res1)
res2 = sim.simxSetObjectOrientation(clientID, handle, -1, rot,
sim.simx_opmode_oneshot)
# print(res2)
return res1, res2
def change_target_angle(self, angle):
X = math.cos(math.radians(angle)) * self.radius
Y = math.sin(math.radians(angle)) * self.radius
Z = -0.005
new_position = [X, Y, Z]
errorCode = sim.simxSetObjectPosition(self.clientID, self.target, -1,
new_position,
sim.simx_opmode_oneshot)
return X, Y
def reset_target_object(self, x, y, z):
error_code = vrep.simxSetObjectPosition(self.clientID,
self.main_target, -1,
[x, y, z],
vrep.simx_opmode_oneshot)
time.sleep(.1) # needs a brief pause or it is skipped.
error_code = vrep.simxSetObjectOrientation(self.clientID,
self.main_target, -1,
self.main_target_angles,
vrep.simx_opmode_oneshot)
def setPoseAtHandle(self, targetHandle, refHandle, pos, quat, ignoreError = False):
res1 = sim.simxSetObjectPosition( self.clientID, targetHandle, refHandle, pos, sim.simx_opmode_oneshot)
res2 = sim.simxSetObjectQuaternion(self.clientID, targetHandle, refHandle, quat, sim.simx_opmode_oneshot)
if res1!=sim.simx_return_ok and not ignoreError:
print('Failed to set position')
print(res1)
if res2!=sim.simx_return_ok and not ignoreError:
print('Failed to set orientation')
print(res2)
def go_to_x(x1, action=None):
global x, y, z, start_stream
err, Qpos = vrep.simxGetObjectPosition(clientID, QuadricopterT, -1,
vrep.simx_opmode_oneshot_wait)
x = Qpos[0]
y = Qpos[1]
z = Qpos[2]
if x < x1:
sign = -1
elif x > x1:
sign = 1
else:
return 0
while abs(x - x1) > 0.02 and len(result) < 3:
err = vrep.simxSetObjectPosition(clientID, QuadricopterT, -1,
(x, y, z),
vrep.simx_opmode_oneshot_wait)
detect_bag(action)
time.sleep(0.2)
x -= sign * 0.02
print(x)
print('\n')
result.append(max(colour_rad['green']))
result.append(max(colour_rad['yellow']))
result.append(max(colour_rad['purple']))
print(result)
err = vrep.simxSetObjectPosition(clientID, QuadricopterT, -1, (x1, y, z),
vrep.simx_opmode_oneshot_wait)
time.sleep(0.2)
err, Qpos = vrep.simxGetObjectPosition(clientID, QuadricopterT, -1,
vrep.simx_opmode_oneshot_wait)
print(Qpos)
x = x1
start_stream = False
def reset_target_object(self, x, y, z):
error_code = vrep.simxSetObjectPosition(self.clientID,
self.main_target, -1,
[x, y, z],
vrep.simx_opmode_oneshot)
time.sleep(
.2
) # needs a brief pause or it is skipped, don't make this less... it won't put it back right!
error_code = vrep.simxSetObjectOrientation(self.clientID,
self.main_target, -1,
self.main_target_angles,
vrep.simx_opmode_oneshot)
def reset(self):
for handle in self.joint_handles:
sim.simxSetJointTargetPosition(self.clientID, handle, 0, blocking)
# Change Box positions
rand_angle = 0 # radians(random.randint(0, 359))
box_x, box_y = (BOX_DISTANCE * cos(rand_angle),
BOX_DISTANCE * sin(rand_angle))
sim.simxSetObjectPosition(self.clientID, self.green_box,
self.joint_handles[0], (box_x, box_y, 0),
oneshot)
sim.simxSetObjectPosition(self.clientID, self.red_box,
self.joint_handles[0], (-box_x, -box_y, 0),
oneshot)
box_distance = sim.simxGetObjectPosition(self.clientID, self.lamp_end,
self.green_box, buffer)[1]
state = [d for d in box_distance]
for handle in self.joint_handles:
state.append(
sim.simxGetJointPosition(self.clientID, handle, buffer)[1])
return np.array(state)
def change_target_position(self, radius):
minimum = -radius
maximum = radius
X = minimum + (maximum - minimum) * random.random()
Y = math.sqrt(self.radius**2 - X**2) * (-1)**(random.sample([0, 1],
1)[0])
Z = -0.005
new_position = [X, Y, Z]
errorCode = sim.simxSetObjectPosition(self.clientID, self.target, -1,
new_position,
sim.simx_opmode_oneshot)
return X, Y
def droneInitialMovement(clientID, drone_base_hanlde, drone_target_hanlde,
floor):
drone_base_position = sim.simxGetObjectPosition(clientID,
drone_base_hanlde, floor,
sim.simx_opmode_blocking)
drone_target_position = sim.simxGetObjectPosition(clientID,
drone_target_hanlde,
floor,
sim.simx_opmode_blocking)
print(drone_base_position)
# Drone move in z axis
if (drone_base_position[1][2] <= 8 and repeatseed == 0):
repeatseed = 1
for i in range(int(drone_base_position[1][2] + 1), 9):
drone_base_position = sim.simxGetObjectPosition(
clientID, drone_target_hanlde, floor, sim.simx_opmode_blocking)
sim.simxSetObjectPosition(
clientID, drone_target_hanlde, floor,
[drone_base_position[1][0], drone_base_position[1][1], i],
sim.simx_opmode_blocking)
print(drone_base_position)
time.sleep(2)
# Drone move in x axis
if (drone_base_position[1][0] != 0 and repeatseed == 1):
repeatseed = 2
drone_x_sign = drone_base_position[1][0] / \
abs(drone_base_position[1][0])
for i in range(1, ((int(abs(drone_base_position[1][0]))) * 10) + 1):
drone_base_position = sim.simxGetObjectPosition(
clientID, drone_target_hanlde, floor, sim.simx_opmode_blocking)
sim.simxSetObjectPosition(clientID, drone_target_hanlde, floor, [
drone_base_position[1][0] - drone_x_sign * 0.1,
drone_base_position[1][1], drone_base_position[1][2]
], sim.simx_opmode_blocking)
print(drone_base_position)
time.sleep(0.1)
time.sleep(4)
drone_base_position = sim.simxGetObjectPosition(
clientID, drone_target_hanlde, floor, sim.simx_opmode_blocking)
print(drone_base_position)
if (drone_base_position[1][0] != 0 and repeatseed == 2):
repeatseed = 3
drone_y_sign = drone_base_position[1][1] / \
abs(drone_base_position[1][1])
for i in range(1, ((int(abs(drone_base_position[1][1]))) * 10) + 1):
drone_base_position = sim.simxGetObjectPosition(
clientID, drone_target_hanlde, floor, sim.simx_opmode_blocking)
sim.simxSetObjectPosition(clientID, drone_target_hanlde, floor, [
drone_base_position[1][0], drone_base_position[1][1] -
drone_y_sign * 0.1, drone_base_position[1][2]
], sim.simx_opmode_blocking)
print(drone_base_position)
time.sleep(0.1)
time.sleep(4)
drone_base_position = sim.simxGetObjectPosition(
clientID, drone_target_hanlde, floor, sim.simx_opmode_blocking)
print(drone_base_position)
def step(self, action, object_presence):
# getting the current state of the quadcopter
state = sim.simxGetObjectPosition(self.clientID, self.quadHandle, -1, sim.simx_opmode_oneshot)
state = np.array(state)
state = state[1]
self.do_action()
# checking the waypoint
err, waypt = sim.simxGetObjectHandle(self.clientID, "Quadricopter_target", sim.simx_opmode_oneshot_wait)
waypt_position = sim.simxGetObjectPosition(self.clientID, waypt, -1, sim.simx_opmode_oneshot)
# calculating the waypoint
while waypt_position != self.goal_position:
if state[0] != self.goal_position[0] and state[1] != self.goal_position[1] and state[2] != self.goal_position[2]:
state[0] = state[0] + (1-action)*(self.goal_position[0]-state[0])*self.goal_velocity
state[1] = state[1] + (1-action)*(self.goal_position[1]-state[1])*self.goal_velocity
state[2] = state[2] + (1-action)*(self.goal_position[2]-state[2])*self.goal_velocity
state[0] = np.clip(state[0], self.min_position, self.max_position)
state[1] = np.clip(state[1], self.min_position, self.max_position)
state[2] = np.clip(state[2], self.min_position, self.max_position)
# assigning the next waypoint
sim.simxSetObjectPosition(self.clientID, waypt, -1, state, sim.simx_opmode_oneshot)
sim.simxStartSimulation(self.clientID, sim.simx_opmode_oneshot_wait)
# print(state)
done = bool(abs(state[0] - self.goal_position[0]) <= 0.001 and abs(state[1] - self.goal_position[1]) <= 0.001 and abs(state[2] - self.goal_position[2]) <= 0.001)
if object_presence == True and done == True:
action = self.max_action
print('Object present', action)
if object_presence == False and done == True:
action = self.min_action
print('object absent', action)
return np.array(state), done, action
def droneInitialMovement(clientID, drone_base_handle, drone_target_handle,
floor, drone_viewposition, repeatseed):
print("Moving to the centre of the scene...")
drone_base_position = sim.simxGetObjectPosition(clientID,
drone_base_handle, floor,
sim.simx_opmode_blocking)
# drone_target_position = sim.simxGetObjectPosition(
# clientID, drone_target_handle, floor, sim.simx_opmode_blocking)
# Drone move in z axis
if (drone_base_position[1][2] <= 8 and repeatseed == 0):
repeatseed = 1
for i in range(int(drone_base_position[1][2] + 1), 9):
drone_base_position = sim.simxGetObjectPosition(
clientID, drone_target_handle, floor, sim.simx_opmode_blocking)
sim.simxSetObjectPosition(
clientID, drone_target_handle, floor,
[drone_base_position[1][0], drone_base_position[1][1], i],
sim.simx_opmode_blocking)
time.sleep(3)
# Drone move vertically
if (drone_base_position[1][0] != 0 and repeatseed == 1):
repeatseed = 2
drone_x_sign = drone_base_position[1][0] / \
abs(drone_base_position[1][0])
for i in range(1, ((int(abs(drone_base_position[1][0]))) * 10) + 1):
drone_base_position = sim.simxGetObjectPosition(
clientID, drone_target_handle, floor, sim.simx_opmode_blocking)
sim.simxSetObjectPosition(clientID, drone_target_handle, floor, [
drone_base_position[1][0] - drone_x_sign * 0.1,
drone_base_position[1][1], drone_base_position[1][2]
], sim.simx_opmode_blocking)
time.sleep(0.3)
time.sleep(4)
drone_base_position = sim.simxGetObjectPosition(
clientID, drone_target_handle, floor, sim.simx_opmode_blocking)
# Drone move horizontally
if (drone_base_position[1][0] != 0 and repeatseed == 2):
repeatseed = 3
drone_y_sign = drone_base_position[1][1] / \
abs(drone_base_position[1][1])
for i in range(1, ((int(abs(drone_base_position[1][1]))) * 10) + 1):
drone_base_position = sim.simxGetObjectPosition(
clientID, drone_target_handle, floor, sim.simx_opmode_blocking)
sim.simxSetObjectPosition(clientID, drone_target_handle, floor, [
drone_base_position[1][0], drone_base_position[1][1] -
drone_y_sign * 0.1, drone_base_position[1][2]
], sim.simx_opmode_blocking)
time.sleep(0.4)
time.sleep(4)
drone_base_position = sim.simxGetObjectPosition(
clientID, drone_target_handle, floor, sim.simx_opmode_blocking)
return drone_viewposition, repeatseed, True
def __init__(self, clientID): self.clientID = clientID self.goal_velocity = 0.2 self.min_action = 0 self.max_action = 1 self.min_position = 0 self.max_position = 100 self.initial_position = [0,0,0] self.rotor_data = [0,0,0,0] self.goal_position = [2,1,1] err, self.quadHandle = sim.simxGetObjectHandle(self.clientID, "Quadricopter_base", sim.simx_opmode_blocking) err, target = sim.simxGetObjectHandle(self.clientID, "Sphere", sim.simx_opmode_oneshot_wait) target_pos = sim.simxSetObjectPosition(self.clientID, target, -1, self.goal_position, sim.simx_opmode_oneshot) self.low_state = np.array([self.min_position, self.min_position, self.min_position], dtype = np.float32) self.max_state = np.array([self.max_position, self.max_position, self.max_position], dtype = np.float32)
def step(self): # Now step a few times:
if sys.version_info[0] == 3:
self._get_pos()
_ = sim.simxSetObjectPosition(self.clientID, self.target, -1, self.target_position, sim.simx_opmode_oneshot)
# error, value = sim.simxGetIntegerSignal(self.clientID, "data", sim.simx_opmode_streaming)
# print(value)
# self.set_pos() +8.0299e-02
else:
raw_input('Press <enter> key to step the !')
sim.simxSynchronousTrigger(self.clientID)
def __init__(self,
velocity=20,
angular_velocity=100,
wheel_basis=0.212,
wheel_radius=0.03):
sim.simxFinish(-1)
self.clientID = sim.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
self.reset = True
self.velocity = velocity
self.angular_velocity = angular_velocity
self.wheel_basis = wheel_basis
self.total_sensors = 13
self.wheel_radius = wheel_radius
self.position = [+6.5000e-01, -4.4900e+00, +8.0299e-02]
self.target_position = [+1.0900e+00, -3.5380e+00, +3.8000e-02
] #[+3.9470e+00,+3.8440e+00,+3.8000e-02]
self.sensor = np.zeros((self.total_sensors), dtype=np.int32)
self.flag = False
self.roomba_path = 'C:/Program Files/CoppeliaRobotics/CoppeliaSimEdu/models/selfmade/Roomba.ttm'
if self.clientID != -1:
print("Connected to API")
sim.simxSynchronous(self.clientID, True)
sim.simxStartSimulation(self.clientID, sim.simx_opmode_blocking)
else:
print("Unable to connect")
#Loading Model
_ = sim.simxLoadModel(self.clientID, self.roomba_path, 0,
sim.simx_opmode_blocking)
self._intial()
_ = sim.simxSetObjectPosition(self.clientID, self.bot, -1,
self.position, sim.simx_opmode_oneshot)
_ = sim.simxSetObjectPosition(self.clientID, self.target, -1,
self.target_position,
sim.simx_opmode_oneshot)
def set_drone_position(point):
err, Qpos0 = vrep.simxGetObjectPosition(clientID, Quadricopter, -1,
vrep.simx_opmode_oneshot_wait)
dist = sqrt((point[0] - Qpos[0])**2 + (point[1] - Qpos[1])**2 +
(point[2] - Qpos[2])**2)
tp = dist / max_vel
t0 = time.time()
while (time.time() - t0 < tp):
t = time.time() - t0
for j in range(3):
Qpos[j] = (
(point[j] - Qpos0[j]) / 2) * (1 - cos(pi * t / tp)) + Qpos0[j]
err = vrep.simxSetObjectPosition(clientID, QuadricopterT, -1,
(Qpos[0], Qpos[1], Qpos[2]),
vrep.simx_opmode_oneshot)
time.sleep(0.02)
time.sleep(1)
