def simulation(self): sim.simxSynchronous(self.clientID, 1) #同步模式 sim.simxSynchronousTrigger(self.clientID) # Start simulation sim.simxStartSimulation(self.clientID, sim.simx_opmode_oneshot_wait) print("Simulation start")
def resetSim(self): # print('[INFO] reseting sim...') ret = sim.simxStopSimulation(self.clientID, sim.simx_opmode_blocking) if ret == sim.simx_return_ok: # print('[INFO] sim reset successfully.') time.sleep(1) else: raise IOError('[ERROR] problem in reseting sim...') sim.simxGetPingTime(self.clientID) # print('[INFO] starting sim in synchronous mode...') sim.simxSynchronous(self.clientID, True) sim.simxGetPingTime(self.clientID) ret = sim.simxStartSimulation(self.clientID, sim.simx_opmode_oneshot) # if ret == sim.simx_return_ok: # print('[INFO] sim started successfully.') sim.simxGetPingTime(self.clientID) sim.simxClearFloatSignal(self.clientID, 'actionX', sim.simx_opmode_blocking) sim.simxClearFloatSignal(self.clientID, 'actionY', sim.simx_opmode_blocking) sim.simxClearFloatSignal(self.clientID, 'actionZ', sim.simx_opmode_blocking) sim.simxGetPingTime(self.clientID) self.stepCount = 0 self.reward = 0
def __init__(self): # n_actions:3 (target pos), n_states:6 (3pos+3force) self.metadata = {'render.modes': ['human']} super().__init__() sim.simxFinish(-1) for _ in range(5): self.clientID = sim.simxStart('127.0.0.1', 19997, True, True, 5000, 5) if self.clientID != -1: # print('[INFO] Connected to CoppeliaSim.') break if self.clientID == -1: raise IOError('[ERROR] Could not connect to CoppeliaSim.') sim.simxSynchronous(self.clientID, True) # sim.simxStartSimulation(self.clientID, sim.simx_opmode_oneshot) sim.simxGetPingTime(self.clientID) self.stepCount = 0 self.reward = 0 self.n_substeps = 10 # self.sim_timestep = 0.5 # set in coppeliaSim (not implemented) self.n_actions = 3 self.n_states = 6 self.action_space = spaces.Box(-1., 1, shape=(self.n_actions, ), dtype='float32') self.observation_space = spaces.Box(-np.inf, np.inf, shape=(self.n_states, ), dtype='float32') self._getHandles() sim.simxGetPingTime(self.clientID)
def __init__(self): sim.simxFinish(-1) # just in case, close all opened connections self.clientID = sim.simxStart('127.0.0.1', 19997, True, True, 5000, 5) if self.clientID != -1: #check if client connection successful print('Connected to remote API server') # enable the synchronous mode on the client: sim.simxSynchronous(self.clientID, True) else: print('Connection not successful') sys.exit('Could not connect') self.Vmax = 0.05 self.Wmax = np.pi / 4 # Action Space self.action_space = spaces.Discrete(3) # Observation Space self.observation_space = spaces.Box( low=np.array([0, 0, 0., -np.pi / 4, 0, 0, 0, 0, 0, 0, 0, 0], dtype=np.float32), high=np.array( [3, 2 * np.pi, 0.08, np.pi / 4, 1, 1, 1, 1, 1, 1, 1, 1], dtype=np.float32), dtype=np.float32) # Objetcs in the Simulation Scene errorCode, self.right_motor = sim.simxGetObjectHandle( self.clientID, 'K4_Right_Motor', sim.simx_opmode_oneshot_wait) errorCode, self.left_motor = sim.simxGetObjectHandle( self.clientID, 'K4_Left_Motor', sim.simx_opmode_oneshot_wait) errorCode, self.khepera = sim.simxGetObjectHandle( self.clientID, 'Khepera_IV', sim.simx_opmode_oneshot_wait) errorCode, self.target = sim.simxGetObjectHandle( self.clientID, 'Target', sim.simx_opmode_oneshot_wait) errorCode, self.camera = sim.simxGetObjectHandle( self.clientID, 'Vision_sensor', sim.simx_opmode_oneshot_wait) self.sensor = {} for i in range(8): handle = 'K4_Infrared_{}'.format(i + 1) errorCode, self.sensor[i] = sim.simxGetObjectHandle( self.clientID, handle, sim.simx_opmode_oneshot_wait) self.viewer = None self.seed() self.steps = 0 self.radius = 0.8 self.MaxSteps = 800 self.problem = True self.Velocities = [0, 0] self.Movements = [[4.285, 0.515], [4.285, 4.285], [0.515, 4.285]] self.xp, self.yp = self.getPositionTarget() self.ResetSimulationScene() self.Randomize = True self.RobotOrientationRand = True
def __init__(self): print('Program started') sim.simxFinish(-1) # just in case, close all opened connections self.clientID = sim.simxStart('127.0.0.1', 19997, True, True, 5000, 5) # Connect to CoppeliaSim if self.clientID != -1: print('Connected to remote API server') sim.simxSynchronous(self.clientID, True) sim.simxStartSimulation(self.clientID, sim.simx_opmode_blocking)
def __init__(self, queue_dict, sync=True): """ this class loads coppeliasim and is responsible for communication between the sim and python... api functions can be found https://www.coppeliarobotics.com/helpFiles/en/remoteApiFunctionsPython.htm """ # open queues self.queues = queue_dict # close all connection that are remaining sim.simxFinish(-1) self.clientID = -1 attempt_num = 0 while self.clientID == -1: print("attempting to connect to VREP...") self.clientID = sim.simxStart('127.0.0.1', 19999, True, True, 5000, 5) attempt_num += 1 if attempt_num >= 3: print("could not connect to vrep") return print("successful connection!") self.sync = sync if self.sync: # set the simulation to synchronise with api sim.simxSynchronous(self.clientID, True) # get coppeliasim object handles self.motor = {'left': self.getHandle("Pioneer_p3dx_leftMotor"), 'right': self.getHandle("Pioneer_p3dx_rightMotor")} self.camHandle = self.getHandle("camera") #self.gripperHandle = {"left": self.getHandle("left_joint"), # "right": self.getHandle("right_joint")} # init camera data stream with rgb and depth sim.simxGetVisionSensorImage(self.clientID, self.camHandle, 0, sim.simx_opmode_streaming) sim.simxGetVisionSensorDepthBuffer(self.clientID, self.camHandle, sim.simx_opmode_streaming) # init position data stream with cartesian position and euler orientation sim.simxGetObjectOrientation(self.clientID, self.camHandle, -1, sim.simx_opmode_streaming) sim.simxGetObjectPosition(self.clientID, self.camHandle, -1, sim.simx_opmode_streaming) # setting motor speed self.speed = 1 self.lr = 0.3 self.images = None # remote start simulation if not already sim.simxStartSimulation(self.clientID, sim.simx_opmode_oneshot) # used for making keyboard switches self.keyboard_key = Clicker("c", activated=True) self.keyboard_controlled = self.keyboard_key.activated self.gripper_key = Clicker("g", activated=False) self.gripper_activated = self.gripper_key.activated
def reset(self): # stop the simulation: sim.simxStopSimulation(self.clientID, sim.simx_opmode_blocking) time.sleep(0.1) sim.simxSynchronous(self.clientID, True) # start the simulation: sim.simxStartSimulation(self.clientID, sim.simx_opmode_blocking) self.steps = 0 self.ResetSimulationScene() if self.Randomize: ran_angle = random.random() * 360 self.xp, self.yp = self.change_target_angle(ran_angle) else: self.xp, self.yp = self.getPositionTarget() #self.xp, self.yp = self.change_target_angle(self.angle) d, Oc = self.get_obs() observation = np.array([d, Oc]) return observation
def __init__(self): sim.simxFinish(-1) # just in case, close all opened connections self.clientID = sim.simxStart('127.0.0.1', 19997, True, True, 5000, 5) if self.clientID != -1: #check if client connection successful print('Connected to remote API server') # enable the synchronous mode on the client: sim.simxSynchronous(self.clientID, True) else: print('Connection not successful') sys.exit('Could not connect') # Action Space self.action_space = spaces.Discrete(3) # Observation Space self.observation_space = spaces.Box(low=np.array([0., -np.pi, -np.pi], dtype=np.float32), high=np.array([5, np.pi, np.pi], dtype=np.float32), dtype=np.float32) # Objetcs in the Simulation Scene errorCode, self.right_motor = sim.simxGetObjectHandle( self.clientID, 'K4_Right_Motor', sim.simx_opmode_oneshot_wait) errorCode, self.left_motor = sim.simxGetObjectHandle( self.clientID, 'K4_Left_Motor', sim.simx_opmode_oneshot_wait) errorCode, self.khepera = sim.simxGetObjectHandle( self.clientID, 'Khepera_IV', sim.simx_opmode_oneshot_wait) errorCode, self.target = sim.simxGetObjectHandle( self.clientID, 'Target', sim.simx_opmode_oneshot_wait) errorCode, self.camera = sim.simxGetObjectHandle( self.clientID, 'Vision_sensor', sim.simx_opmode_oneshot_wait) self.viewer = None self.seed() self.steps = 0 self.MaxSteps = 300 self.radius = 0.8 self.angle = 0 self.Randomize = False self.xp, self.yp = self.getPositionTarget() self.ResetSimulationScene()
def reset(self): # stop the simulation: self.problem = False sim.simxStopSimulation(self.clientID, sim.simx_opmode_blocking) sim.simxSynchronous(self.clientID, True) # start the simulation: sim.simxStartSimulation(self.clientID, sim.simx_opmode_blocking) self.ResetSimulationScene() if self.Randomize: self.xp, self.yp = self.change_target_position(radius=self.radius) else: self.xp, self.yp = self.getPositionTarget() if self.RobotOrientationRand: self.change_robot_orientation() self.steps = 0 [d, Oc, alpha, v, w], Sensors = self.get_obs() observation = np.append([d, Oc, v, w], Sensors) return observation
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)
returnCode = sim.simxSetJointTargetVelocity(clientID, rightWheel, 0, sim.simx_opmode_oneshot) # start simulation and initialize vision sensor status = sim.simxStartSimulation(clientID, sim.simx_opmode_oneshot) #^ 改为不断查询直至获得图像 returnCode = 1 while returnCode != sim.simx_return_ok: returnCode, resolution, image = sim.simxGetVisionSensorImage( clientID, visionSensorHandle, 1, sim.simx_opmode_streaming) timeStart = time.time() last_time = sim.simxGetLastCmdTime(clientID) idx = 0 #^ 改为同步模式 sim.simxSynchronous(clientID, 1) try: while True: returnCode, resolution, image = sim.simxGetVisionSensorImage( clientID, visionSensorHandle, 1, sim.simx_opmode_buffer) image = np.flipud( np.array(image, dtype=np.uint8).reshape( [resolution[1], resolution[0]])) # if idx<=100: # plt.imsave(f'full{idx}.jpg',np.stack([image]*3, axis=2)) # idx+=1 cropImage = crop(image, mainSizeX, mainSizeY) #^ 大scale就是原图像下半部分 landscape = crop(image, bigX, bigY) #^ 增加一个图像followLine用于控制直线速度 followLine = crop(image, followLineX, followLineY)
velocity = velUpperLimit if (velocity < -velUpperLimit): velocity = -velUpperLimit return velocity # Start streaming client.intSignalName integer signal, that signals when a step is finished: sim.simxGetIntegerSignal(client.id, client.intSignalName, sim.simx_opmode_streaming) res, client.jointHandle = sim.simxGetObjectHandle( client.id, 'joint', sim.simx_opmode_blocking) sim.simxSetJointTargetVelocity(client.id, client.jointHandle, 360 * math.pi / 180, sim.simx_opmode_oneshot) sim.simxGetJointPosition(client.id, client.jointHandle, sim.simx_opmode_streaming) # enable the synchronous mode on the client: sim.simxSynchronous(client.id, True) sim.simxStartSimulation(client.id, sim.simx_opmode_oneshot) moveToAngle(45 * math.pi / 180) moveToAngle(90 * math.pi / 180) moveToAngle(-89 * math.pi / 180) #no -90, to avoid passing below moveToAngle(0 * math.pi / 180) sim.simxStopSimulation(client.id, sim.simx_opmode_blocking)
def disable_synchronization(self): """Выключить режим синхронизации """ sim.simxSynchronous(self.client, False) self.synchronous = False
def enable_synchronization(self): """Включить режим синхронизации """ sim.simxSynchronous(self.client, True) self.synchronous = True
if clientID > -1: break else: time.sleep(0.2) print("Failed connecting to remote API server!") print("Connection success!") RAD2DEG = 180 / math.pi # 常数,弧度转度数 tstep = 0.01 # 定义仿真步长 # 设置仿真步长,为了保持API端与V-rep端相同步长 vrep.simxSetFloatingParameter(clientID, vrep.sim_floatparam_simulation_time_step, tstep, vrep.simx_opmode_oneshot) # 然后打开同步模式 vrep.simxSynchronous(clientID, True) vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot) baseName = 'Body' jointName = 'Tran' _, baseHandle = vrep.simxGetObjectHandle(clientID, baseName, vrep.simx_opmode_blocking) _, jointHandle = vrep.simxGetObjectHandle(clientID, jointName, vrep.simx_opmode_blocking) print('Handles available!') # _, base_pos = vrep.simxGetObjectPosition(clientID, baseHandle, -1, vrep.simx_opmode_streaming) # _, jointConfig = vrep.simxGetJointPosition(clientID, jointHandle, vrep.simx_opmode_streaming)
if client_ID > -1: # connected print('Connect to remote API server.') break else: print('Failed connecting to remote API server! Try it again ...') # Pause the simulation # res = vrep_sim.simxPauseSimulation(client_ID, vrep_sim.simx_opmode_blocking) delta_t = 0.005 # simulation time step # Set the simulation step size for VREP vrep_sim.simxSetFloatingParameter(client_ID, vrep_sim.sim_floatparam_simulation_time_step, delta_t, vrep_sim.simx_opmode_oneshot) # Open synchronous mode vrep_sim.simxSynchronous(client_ID, True) # Start simulation vrep_sim.simxStartSimulation(client_ID, vrep_sim.simx_opmode_oneshot) # ------------------------------- Initialize simulation model ------------------------------- UR5_sim_model = UR5SimModel() UR5_sim_model.initializeSimModel(client_ID) time.sleep(0.1) t = np.linspace(0, 2 * np.pi, 800) data_len = len(t) reference_q = np.zeros((3, data_len)) reference_q[0, :] = np.sin(t) reference_q[1, :] = np.sin(t) reference_q[2, :] = np.sin(t)
def main(argv): # https://www.coppeliarobotics.com/helpFiles/en/remoteApiClientSide.htm --> how to remote API # Refer to https://www.coppeliarobotics.com/helpFiles/en/remoteApiFunctionsPython.htm#simxStart sim.simxFinish(-1) # just in case, close all opened connections clientID = sim.simxStart('127.0.0.1', 19997, True, True, 5000, 5) if clientID == -1: print('Failed connecting to remote API server') else: print('Connected to remote API client') numberOfRuns = 10 numberOfMeasurements = 1 variationSize = 0.001 for i in range(numberOfRuns): count = 0 dt = 0.0167 sim.simxSynchronous(clientID, True) ################################################### init variables ################################################### ros_handle = simROS.SIMROS(argv) targetAngle = 0 #targetAngles = list(np.random.randint(-25, 26, numberOfMeasurements)) targetAngles = list(np.zeros(numberOfMeasurements)) print(targetAngles) randomVar = [] for i in range(18): randomVar.append( list( np.random.uniform(-variationSize, variationSize, numberOfMeasurements))) if newFile: with open(dataFilePath, 'w+') as myfile: wr = csv.writer(myfile, quoting=csv.QUOTE_ALL) wr.writerow([ "Inclination", "LFx", "LFy", "LFz", "LMx", "LMy", "LMz", "LHx", "LHy", "LHz", "RFx", "RFy", "RFz", "RMx", "RMy", "RMz", "RHx", "RHy", "RHz" ]) counter = 0 Done = False #Start the simulation sim.simxStartSimulation(clientID, sim.simx_opmode_blocking) ################################################### Control Loop ################################################### while not Done: #code here #print(sim.simxReadForceSensor(clientID, "3D_force0", sim.simx_opmode_blocking)) if counter == 150: saveData(force, targetAngle) print("Runs remaining: ", len(targetAngles)) if len(targetAngles) <= 0: Done = True else: targetAngle = targetAngles.pop(0) motor_positions = [ 0, randomVar[1].pop() + 2.059, randomVar[2].pop(), 0, randomVar[4].pop() + 2.059, randomVar[5].pop(), 0, randomVar[7].pop() + 2.059, randomVar[8].pop(), 0, randomVar[10].pop() + 2.059, randomVar[11].pop(), 0, randomVar[13].pop() + 2.059, randomVar[14].pop(), 0, randomVar[16].pop() + 2.059, randomVar[17].pop() ] counter = 0 ros_handle.setSlopeAngle(targetAngle) ros_handle.setLegMotorPosition(motor_positions) counter = counter + 1 #Step the simulation sim.simxSynchronousTrigger(clientID) sim.simxStopSimulation(clientID, sim.simx_opmode_blocking) sim.simxGetPingTime(clientID) sim.simxFinish(clientID)
import time import sim import simConst sim.simxFinish(-1) clientID = sim.simxStart('127.0.0.1', 19997, True, True, 5000, 5) print("Connection success") if clientID!=-1: print ('Connected to remote API server') res,left_motor = sim.simxGetObjectHandle(clientID, "left_motor", sim.simx_opmode_oneshot_wait) sim.simxSynchronous(clientID,1)# -啟用同步模式(客戶端)。服務器端(即CoppeliaSim)也需要啟用。 sim.simxStartSimulation(clientID,sim.simx_opmode_oneshot)# //開始仿真 sim.simxSetJointForce(clientID,left_motor,1.0f,sim.simx_opmode_oneshot);#//設置聯合力/扭矩 sim.simxSetJointTargetVelocity(clientID,left_motor,180.0f * 3.1415f / 180.0f,sim.simx_opmode_oneshot)# //設置聯合目標速度 sim.simxSynchronousTrigger(clientID);#//觸發下一個模擬步驟。上面的命令將被應用 sim.simxSetJointForce(clientID,left_motor,0.5f,sim.simx_opmode_oneshot)# //設置聯合力/扭矩 sim.simxSetJointTargetVelocity(clientID,left_motor,180.0f * 3.1415f / 180.0f,sim.simx_opmode_oneshot)# //設置聯合目標速度 sim.simxSynchronousTrigger(clientID)# //下一個模擬步驟執行。上面的命令將被應用
print('"sim.py" could not be imported. This means very probably that') print('either "sim.py" or the remoteApi library could not be found.') print('Make sure both are in the same folder as this file,') print('or appropriately adjust the file "sim.py"') print('--------------------------------------------------------------') print('') import time print('Program started') sim.simxFinish(-1) # just in case, close all opened connections clientID = sim.simxStart('127.0.0.1', 19997, True, True, 5000, 5) # Connect to CoppeliaSim if clientID != -1: print('Connected to remote API server') sim.simxSynchronous(clientID, False) ret = sim.simxStopSimulation(clientID, sim.simx_opmode_blocking) time.sleep(2) ret = sim.simxStartSimulation(clientID, sim.simx_opmode_blocking) print('started simulation') # Now try to retrieve data in a blocking fashion (i.e. a service call): ret, motor_hand = sim.simxGetObjectHandle(clientID, "motor", sim.simx_opmode_blocking) ret, cyl_hand = sim.simxGetObjectHandle(clientID, "Cylinder", sim.simx_opmode_blocking) ret, rob_hand = sim.simxGetObjectHandle(clientID, "Cuboid", sim.simx_opmode_blocking) # Now retrieve streaming data (i.e. in a non-blocking fashion): startTime = time.time() ret, [i, j, k] = sim.simxGetObjectOrientation(
sim.simxFinish(-1) # just in case, close all opened connections clientID = sim.simxStart(ip,port,True,True,5000,5) # Connect to CoppeliaSim scene_path = os.path.join('vrep_scene', 'hexapod_scene.ttt') print('-'*5, 'Scene path:', scene_path, '-'*5) sim.simxLoadScene(clientID, scene_path, 0xFF, sim.simx_opmode_blocking) return clientID if __name__ == "__main__": args = parse_joint_args() print('Program started') clientID=conect_and_load(args.port, args.ip) print('Starting the simulation') sim.simxSynchronous(clientID, 1) # enable synchoronous mode sim.simxStartSimulation(clientID, sim.simx_opmode_oneshot) sim.simxSynchronousTrigger(clientID) time.sleep(1) if clientID!=-1: print('Connected to remote API server') time.sleep(1) env = HexapodEnv(clientID, args.faulty_joints) print_info(f"Eps Decay Rate is {args.eps_decay}") train_dir = get_train_dir(args.faulty_joints) # save hyperparameters
def main(): sim.simxFinish(-1) clientID = sim.simxStart('127.0.0.1', 19999, True, True, 5000, 5) if clientID != -1: print('Connected to remote API server') else: print('Failed connecting to remote API server') sys.exit('Could not connect to remote API server') sim.simxSynchronous(clientID, 1) #synchronous operation necessary for sim.simxStartSimulation(clientID, sim.simx_opmode_oneshot) jointHandles = [-1, -1, -1, -1, -1, -1] move_helper.getJointHandles(clientID, jointHandles) #handle of UR3 base_handle = sim.simxGetObjectHandle(clientID, "UR3", sim.simx_opmode_blocking)[1] #handle for end-effector end_effector_handle = sim.simxGetObjectHandle(clientID, "UR3_link7", sim.simx_opmode_blocking)[1] #position of UR3 end effector wrt UR3 base frame end_effector_wrt_base = sim.simxGetObjectPosition( clientID, end_effector_handle, base_handle, sim.simx_opmode_blocking)[1] #handle for the ball ball_handle = sim.simxGetObjectHandle(clientID, 'Ball', sim.simx_opmode_blocking)[1] #handle for ball sensor/proximity sensor sensorHandle = sim.simxGetObjectHandle(clientID, 'Ball_Sensor', sim.simx_opmode_blocking)[1] #position and orientation of proximity sensore wrt UR3 base frame T_sensor_in_base = np.array( \ [[0, 0, -1, end_effector_wrt_base[0]], [0, -1, 0, end_effector_wrt_base[1]], \ [-1, 0, 0, end_effector_wrt_base[2]], [0, 0, 0, 1]]) #the actual end point of the ball based on path in the world frame path = generate_path() end_pt = path[path.shape[0] - 1] detectedPoints = np.zeros((2, 3)) #the position and orientation of the UR3 base frame wrt world frame (T_wb) T_base_in_world = np.array([[-1, 0, 0, -1.4001], [0, -1, 0, -0.000086], [0, 0, 1, 0.043], [0, 0, 0, 1]]) #T_bw T_world_in_base = np.linalg.inv(T_base_in_world) #end pt coords in homog form end_pt_homogenous_coords = np.array([[end_pt[0], end_pt[1], end_pt[2], 1]]).T #T_bw * p_w = p_b (4,4) * (4, 1) -> (4, 1), left out final element -> (3, 1) #the actual endpt of the ball in the base frame end_pt_ball_in_base = np.matmul(T_world_in_base, end_pt_homogenous_coords)[:3] detection_thread = threading.Thread(target=utils.calc_ball_position, args=(clientID, sensorHandle, path, detectedPoints)) motion_thread = threading.Thread(target=utils.move_ball, args=(clientID, ball_handle, path)) motion_thread.start() detection_thread.start() detection_thread.join() #point_in_sensorX - homogeneous coords of detectedPoints in the frame of the proximity sensor point_in_sensor1 = np.array([[detectedPoints[0][0]], [detectedPoints[0][1]], [detectedPoints[0][2]], [1]]) point_in_sensor2 = np.array([[detectedPoints[1][0]], [detectedPoints[1][1]], [detectedPoints[1][2]], [1]]) #T_bs * p_s = p_b point1_ball_in_base = np.matmul(T_sensor_in_base, point_in_sensor1)[:3] point2_ball_in_base = np.matmul(T_sensor_in_base, point_in_sensor2)[:3] vector = (1.0 * point2_ball_in_base - point1_ball_in_base) #convert to unit vector vector /= np.linalg.norm(vector) t = (end_effector_wrt_base[0] - point2_ball_in_base[0]) / vector[0] final_x = end_effector_wrt_base[0] final_y = point2_ball_in_base[1] + (t * vector[1]) final_z = point2_ball_in_base[2] + (t * vector[2]) predicted_point = np.array([[final_x], [final_y[0]], [final_z[0]]]) difference = predicted_point - end_pt_ball_in_base print("Difference: " + str(difference)) #the predicted direction the ball is moving in, based off of predicted points 1 and 2 #if the difference between detected points is too small if (vector[0] == 0): vector[0] += 0.000000001 #find the estimated position of the ball when it is in the yz plane of the UR3 #the predicted point of the ball print("Predicted Point: " + str(predicted_point)) print("End Point Ball in Base: " + str(end_pt_ball_in_base)) #compare prediction to actual end pt of ball #our initial guess of what the UR3 joint angles should be to get to finalT thetas = np.array([0.0, 0.0, 0.0]) #get a rough initial guess for theta of joint 2 predicted_y = predicted_point[1] predicted_z = predicted_point[2] position_joint2 = sim.simxGetObjectPosition(clientID, jointHandles[1], base_handle, sim.simx_opmode_blocking)[1] print(position_joint2) joint2_y = position_joint2[1] joint2_z = position_joint2[2] #distance from predicted point to joint2 in yz plane d1 = np.sqrt((joint2_y - predicted_y)**2 + (joint2_z - predicted_z)**2) z_length = predicted_z - joint2_z y_length = predicted_y - joint2_y #our thetas to be calculated theta2_guess = 0.0 theta3_guess = 0.0 theta4_guess = 0.0 #define relevant link lengths L1 = 0.244 L2 = 0.213 L3 = 0.083 #if it's in this range, it doesn't make sense to move others if d1 < (L1 - L2): theta2_guess = np.arctan2(y_length, z_length) theta3_guess = 0.0 theta4_guess = 0.0 elif d1 <= (L1 + L2 - L3): top2 = L2**2 - L1**2 - d1**2 bot2 = -2 * L1 * d1 theta2_guess = np.arctan2(y_length, z_length) - np.arccos(top2 / bot2) top3 = d1**2 - L1**2 - L2**2 bot3 = -2 * L1 * L2 theta3_guess = np.pi - np.arccos(top3 / bot3) #fold in on self theta4_guess = np.pi / 2 else: d2 = d1 - L1 theta2_guess = np.arctan2(y_length, z_length) top3_2 = L3**2 - L2**2 - d2**2 bot3_2 = -2 * d2 * L2 print("top3_2/bot3_2: " + str(top3_2 / bot3_2)) theta3_guess = np.arccos(top3_2 / bot3_2) top4 = d2**2 - L2**2 - L3**2 bot4 = -2 * L2 * L3 print("top4/bot4: " + str(top4 / bot4)) theta4_guess = np.arccos(top4 / bot4) - np.pi thetas[0] = theta2_guess thetas[1] = theta3_guess thetas[2] = theta4_guess print("thetas: " + str(thetas)) for i in range(1, 4): sim.simxSetJointTargetPosition(clientID, jointHandles[i], thetas[i - 1], sim.simx_opmode_oneshot_wait) motion_thread.join( ) #stop motion thread after robot has moved to defending position print("Actual End Effector Position " + str( sim.simxGetObjectPosition(clientID, end_effector_handle, base_handle, sim.simx_opmode_blocking)[1])) sim.simxStopSimulation(clientID, sim.simx_opmode_oneshot) sim.simxFinish(clientID) return 1
sys.exit("Error: no se puede conectar") #Terminar este script except: print('Check if CoppeliaSim is open') UR3_joints_IP=get_joint_handle(clientID) #Getting the reference for i in UR3_joints_IP: UR3_joints_IP[i].append(get_joint_orientation(clientID, UR3_joints_IP[i][0])) #print(get_joint_orientation(clientID, UR3_joints_IP[i])) #J1z,J2z,J3Z #Practica 30 grados j1, 15 grados j2, 30 grados j3, 20 grados j4, 15 grados j5, 10 grados j6 new_angles=[60,25,10,30,15,50] c=0 sim.simxSynchronous(clientID,True) sim.simxSynchronousTrigger(clientID) sim.simxStartSimulation(clientID, sim.simx_opmode_blocking) for i in UR3_joints_IP: set_joint_orientation(clientID, UR3_joints_IP[i][0],new_angles[c], mode=sim.simx_opmode_blocking) #Todas las juntas posicion inicial c=c+1 time.sleep(5) sim.simxStopSimulation(clientID, sim.simx_opmode_blocking) sim.simxFinish(clientID)
def main(): obst_count = 68 targetCount = 5 obstaclePrefix = 'column' targetPrefix = 'End' sim.simxFinish(-1) clientID = sim.simxStart('127.0.0.1', 19997, True, True, 5000, 5) if clientID != -1: print('Connected to remote API server') err, QuadricopterT = sim.simxGetObjectHandle(clientID, 'Quadricopter_target', sim.simx_opmode_blocking) if err == -1: print("No Quadricopter") err, Quadricopter = sim.simxGetObjectHandle(clientID, 'Quadricopter', sim.simx_opmode_blocking) if err == -1: print("No Quadricopter") #sim.simxStartSimulation(clientID, sim.simx_opmode_blocking) # enable the synchronous mode on the client: print("is connected!!!") #retrieves the boxes from coppeliasim. This is slow (which is why it is commented out, we read from file instead) #obstacle collection #obst_list = [] bbox_list = [] if GENERATE_FILES: for i in range(obst_count): err, Obst = sim.simxGetObjectHandle(clientID, obstaclePrefix + str(i), sim.simx_opmode_blocking) if err > 0: print("could not retrieve column ", i) obst_pose = flib.get_pos(clientID, Obst) print("col ", i, "POSE: ", obst_pose) obst_size = flib.get_size(clientID, Obst) print("col ", i, "SIZE: ", obst_size) obst_bbox = convert_bbox(obst_pose, obst_size) print("col ", i, "BBOX: ", obst_bbox) #obst = Obstacle(obst_pose, obst_size, obst_bbox) bbox_list.append(obst_bbox) #obst_list.append(obst) #we write the boxes to a file to retrieve faster later. write_boxes_file(bbox_list) bbox_list = read_boxes_file() #plot_boxes(bbox_list) #target collection deliveries = [] for i in range(targetCount): err, targ = sim.simxGetObjectHandle(clientID, targetPrefix + str(i + 1), sim.simx_opmode_blocking) tmp = flib.get_pos(clientID, targ) print("Target ", i, "Location: ", tmp) deliveries.append(np.array([tmp[0], tmp[1], tmp[2]])) if GENERATE_FILES: np.savetxt("targets.csv", deliveries, delimiter=",") pose = flib.get_pos(clientID, Quadricopter) if GENERATE_FILES: np.savetxt("pose.csv", pose, delimiter=",") print("Start position: ", pose) print("Total distance: ", calc_total_distance(pose, deliveries)) #controller object pathControl = quadsim_P2P(pose, bbox_list) np.savetxt("pose.csv", pose, delimiter=",") np.savetxt("targets.csv", np.array(deliveries), delimiter=",") #plan route before_rrt_t = datetime.datetime.now() while not pathControl.plan(deliveries, clientID): print("Retrying planning with: max iterations=", pathControl.rrt.max_iter) if pathControl.rrt.use_funnel: print("search cone angle[Rad]=", pathControl.rrt.searchTheta) print("the path is worthy! Calculation took: ", (datetime.datetime.now() - before_rrt_t).total_seconds(), " seconds.") lastIter = -1 lastGoal = -1 #start simulation pathControl.iterRun_start() sim.simxStartSimulation(clientID, sim.simx_opmode_oneshot) sim.simxSynchronous(clientID, 1) lastTime = datetime.datetime.now() startTime = now = datetime.datetime.now() while pathControl.iterRunGo: pos, ori = pathControl.iterRun_move() #pathControl.display() sim.simxSetObjectPosition(clientID, Quadricopter, -1, pos, sim.simx_opmode_streaming) sim.simxSetObjectOrientation(clientID, Quadricopter, -1, ori, sim.simx_opmode_streaming) if (pathControl.pathIter != lastIter): sim.simxSetObjectPosition( clientID, QuadricopterT, -1, pathControl.path[pathControl.goalIter][pathControl.pathIter], sim.simx_opmode_streaming) now = datetime.datetime.now() print("Time between goals: ", (now - lastTime).total_seconds(), "[s]") lastIter = pathControl.pathIter lastTime = now if pathControl.goalIter != lastGoal: now = datetime.datetime.now() print("Time of flight: ", (now - startTime).total_seconds(), "[s]") lastGoal = pathControl.goalIter startTime = now pathControl.iterRun_stop() sim.simxStopSimulation(clientID, sim.simx_opmode_blocking) sim.simxFinish(clientID)