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)