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 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 inputxmlfile(self): sim.simxStopSimulation(self.clientID, sim.simx_opmode_oneshot) #number returnCode = simxCloseScene(number clientID,number operationMode) sim.simxCloseScene(self.clientID,sim.simx_opmode_blocking) #number returnCode=simxLoadScene(number clientID,string scenePathAndName,number options,number operationMode) sim.simxLoadScene(self.clientID,"D:\fall2020_v2\data\home\Desktop\40723103\add_stl.simscene.xml",0x00,sim.simx_opmode_blocking) ''' # relative to remote API client location, relative path: sim.simxLoadScene(clientID,'test/testScene.ttt',0xFF,vrep.simx_opmode_blocking) # relative to V-REP executable location, relative path: sim.simxLoadScene(clientID,'scenes/collisionDetectionDemo.ttt',0x00,vrep.simx_opmode_blocking) # relative to remote API client location, absolute path: sim.simxLoadScene(clientID,'c:/python27/test/testScene.ttt',0xFF,vrep.simx_opmode_blocking) # relative to V-REP executable location, absolute path: sim.simxLoadScene(clientID,'d:/v_rep/qrelease/release/scenes/collisionDetectionDemo.ttt',0x00,vrep.simx_opmode_blocking) ''' print("successful input file")
def doTracking(self): self.resetSim() self.prepareSim() pathIsDone = sim.simxGetFloatSignal(self.clientID, 'movePathDone', sim.simx_opmode_streaming)[1] posX = [] posY = [] posZ = [] tactile1 = [] reward = [] while pathIsDone == 0: X, Y, Z = self.getKinectXYZ(False) # forces = self.getForce(False) # diffForce = (forces[0][0] + ((forces[1][0] + forces[2][0]) / 2))/2 # print('[DATA] diff force: {:.4f}'.format(diffForce)) actionX = X # + 0.005*diffForce actionY = Y actionZ = Z # print('[DATA] X:{:.4f}, Y:{:.4f}, Z:{:.4f}'.format(actionX,actionY,actionZ)) sim.simxSetFloatSignal(self.clientID, 'actionX', actionX, sim.simx_opmode_oneshot) sim.simxSetFloatSignal(self.clientID, 'actionY', actionY, sim.simx_opmode_oneshot) sim.simxSetFloatSignal(self.clientID, 'actionZ', actionZ, sim.simx_opmode_oneshot) sim.simxGetPingTime(self.clientID) sim.simxSynchronousTrigger(self.clientID) forces = self.getForceMagnitude(False) posX.append(X) posY.append(Y) posZ.append(Z) tactile1.append(forces[0]) reward.append(self._getReward()) sim.simxGetPingTime(self.clientID) pathIsDone = sim.simxGetFloatSignal(self.clientID, 'movePathDone', sim.simx_opmode_buffer)[1] # time.sleep(0.5) # print('[INFO] tracking is done.') # print('[DATA] accumulated reward: {}'.format(np.sum(np.array(reward)))) sim.simxStopSimulation(self.clientID, sim.simx_opmode_blocking)
def reset(self): """ stop the simulation (will reset the scene in vrep) clear signals and erase the client's local genome files """ ret = sim.simxStopSimulation(self.clientID, mode1) for filename in self.genome_filenames: sim.simxEraseFile(self.clientID, filename, mode1) sim.simxClearIntegerSignal(self.clientID, '', mode1) sim.simxClearFloatSignal(self.clientID, '', mode1) sim.simxClearStringSignal(self.clientID, '', mode1)
def get_label(unit_data, converter, clientID, Body): #1 for stable and 0 for fall down labels = [] continue_flag = 0 duration = 0.03 for i in tqdm(range(unit_data.shape[0])): sequence_sample = unit_data[i] #for duration in [0.025, 0.035]: for idx_f in range(sequence_sample.shape[0]): angle_recon = converter.frameRecon(sequence_sample[idx_f]) # angles: LSP, LSR, LEY, LER, RSP, RSR, REY, RER, LHP, LHR, LHYP, LKP, RHP, RHR, RHYP, RKP, LAP, LAR, RAP, RAR angles = converter.generateWholeJoints(angle_recon) assert (len(angles) == 20) transmit(clientID, Body, angles) time.sleep(duration) returnCode, position = sim.simxGetObjectPosition( clientID, Body['HeadYaw'], -1, sim.simx_opmode_buffer) if position[2] < 0.4 and position[2] > 0: #fall down print('fall') sim.simxStopSimulation(clientID, sim.simx_opmode_oneshot) print('stop') time.sleep(.3) sim.simxStartSimulation(clientID, sim.simx_opmode_oneshot) print('start') time.sleep(.5) labels.append(0) continue_flag = 1 break if continue_flag: continue_flag = 0 continue labels.append(1) time.sleep(.3) labels = np.array(labels) print(labels.shape, np.sum(labels == 0)) np.save('unit_stability_labels.npy', np.array(labels))
def stop_simulation(): """ Purpose: --- This function should stop the running simulation in CoppeliaSim server. NOTE: In this Task, do not call the exit_remote_api_server function in case of failed connection to the server. The test_task_2a executable script will handle that condition. Input Arguments: --- None Returns: --- `return_code` : [ integer ] the return code generated from the stop running simulation remote API Example call: --- return_code = stop_simulation() NOTE: This function will be automatically called by test_task_2a executable at the end of simulation. """ global client_id return_code = 0 ############## ADD YOUR CODE HERE ############## return_code = sim.simxStopSimulation(client_id, sim.simx_opmode_oneshot) ################################################## return return_code
def stop_sim(): sim.simxStopSimulation(clientID, sim.simx_opmode_blocking) print("Simulation stopped.")
def end(self): sim.simxStopSimulation(self.clientID, sim.simx_opmode_blocking) sim.simxFinish(self.clientID)
def get_image(self): err, resolution, image = sim.simxGetVisionSensorImage( self.clientID, self.v0, 0, sim.simx_opmode_streaming) if err == sim.simx_return_ok: img = numpy.array(image, dtype=numpy.uint8) img.resize([resolution[1], resolution[0], 3]) img = imutils.rotate_bound(img, 180) img = cv2.flip(img, 1) cv2.putText(img, 'Image Recognition', (50, 500), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 0), 1, cv2.LINE_AA) global ret_blue global ret_red global ret_green ret_green = color.track_green_object(img) ret_red = color.track_red_object(img) ret_blue = color.track_blue_object(img) img = cv2.cvtColor(img, cv2.COLOR_BGR2RGB) if ret_green and ret_red and ret_blue: #Use Rectangle and Text Mark Green Object Rec_range = 6 cv2.rectangle( img, (ret_green[0] - Rec_range, ret_green[1] - Rec_range), (ret_green[0] + Rec_range, ret_green[1] + Rec_range), (0, 255, 0), 1) cv2.putText(img, 'Pla', (ret_green[0] - 10, ret_green[1] + 20), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 1, cv2.LINE_AA) #Use Rectangle and Text Mark Red Object cv2.rectangle(img, (ret_red[0] - Rec_range, ret_red[1] - Rec_range), (ret_red[0] + Rec_range, ret_red[1] + Rec_range), (0, 0, 200), 1) cv2.putText(img, 'Com', (ret_red[0] - 15, ret_red[1] - 10), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 255), 1, cv2.LINE_AA) #Use Rectangle and Text Mark Blue Object cv2.rectangle( img, (ret_blue[0] - Rec_range, ret_blue[1] - Rec_range), (ret_blue[0] + Rec_range, ret_blue[1] + Rec_range), (255, 0, 0), 1) cv2.putText(img, 'Ball', (ret_blue[0] - 20, ret_blue[1] - 10), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 0, 0), 1, cv2.LINE_AA) print(threading.enumerate()) if threading.active_count() < 3: thread1 = threading.Thread( target=self.Computer1_thread_job, name='Computer_1') thread1.start() else: if not ret_green: print('not ret_green') if not ret_red: print('not ret_red') if not ret_blue: print('not ret_blue') sim.simxStopSimulation(clientID2, sim.simx_opmode_oneshot_wait) time.sleep(1) sim.simxStartSimulation(clientID2, sim.simx_opmode_oneshot_wait) time.sleep(0.5) self.lastFrame = img #self.lastFrame = numpy.hstack((image_ori,img)) return 1, self.lastFrame elif err == sim.simx_return_novalue_flag: return 0, None else: return err, None
def stopvrep(): sim.simxStopSimulation(clientID, sim.simx_opmode_oneshot_wait) sim.simxFinish(-1) # just in case, close all opened connections sim.simxStopSimulation(clientID, sim.simx_opmode_oneshot_wait) return render_template('stopvrep.html')
y = Qpos[1] + obj_pos_pixel[1] * pixel_size set_drone_position([x, y, 1]) img = get_image() objects = get_objects(img) objects = sorted(objects.items(), key=itemgetter(0), reverse=True) obj_pos_pixel = objects[0][1] pixel_size = (2 * (1 - 0.25) / sqrt(3)) / 256 err, cameraPos = vrep.simxGetObjectPosition(clientID, cameraID, -1, vrep.simx_opmode_oneshot_wait) x = cameraPos[0] + obj_pos_pixel[0] * pixel_size y = cameraPos[1] + obj_pos_pixel[1] * pixel_size objects = set_drone_position([x, y, 0.4]) time.sleep(2) err, Qpos = vrep.simxGetObjectPosition(clientID, Quadricopter, -1, vrep.simx_opmode_oneshot_wait) # Throw ball emptyBuff = bytearray() res, retInts, retFloats, retStrings, retBuffer = vrep.simxCallScriptFunction( clientID, 'Quadricopter', vrep.sim_scripttype_childscript, 'ThrowBallFunction', [], [], [""], emptyBuff, vrep.simx_opmode_blocking) time.sleep(1) img = get_image() cv2.imwrite("ball.png", img) # finish work vrep.simxStopSimulation(clientID, vrep.simx_opmode_blocking) time.sleep(1) vrep.simxFinish(clientID) print("stop simulation")
def listener_callback(self, msg): if msg.transforms[0].child_frame_id == 'robot1': self.x1 = msg.transforms[0].transform.translation.x self.y1 = msg.transforms[0].transform.translation.y self.xr1 = msg.transforms[0].transform.rotation.x self.yr1 = msg.transforms[0].transform.rotation.y self.zr1 = msg.transforms[0].transform.rotation.z self.wr1 = msg.transforms[0].transform.rotation.w self.Theta1 = euler_from_quaternion(self.xr1, self.yr1, self.zr1, self.wr1) if msg.transforms[0].child_frame_id == 'robot2': self.x2 = msg.transforms[0].transform.translation.x self.y2 = msg.transforms[0].transform.translation.y self.xr2 = msg.transforms[0].transform.rotation.x self.yr2 = msg.transforms[0].transform.rotation.y self.zr2 = msg.transforms[0].transform.rotation.z self.wr2 = msg.transforms[0].transform.rotation.w self.Theta2 = euler_from_quaternion(self.xr2, self.yr2, self.zr2, self.wr2) if msg.transforms[0].child_frame_id == 'robot3': self.x3 = msg.transforms[0].transform.translation.x self.y3 = msg.transforms[0].transform.translation.y self.xr3 = msg.transforms[0].transform.rotation.x self.yr3 = msg.transforms[0].transform.rotation.y self.zr3 = msg.transforms[0].transform.rotation.z self.wr3 = msg.transforms[0].transform.rotation.w self.Theta3 = euler_from_quaternion(self.xr3, self.yr3, self.zr3, self.wr3) if msg.transforms[0].child_frame_id == 'robot4': self.x4 = msg.transforms[0].transform.translation.x self.y4 = msg.transforms[0].transform.translation.y self.xr4 = msg.transforms[0].transform.rotation.x self.yr4 = msg.transforms[0].transform.rotation.y self.zr4 = msg.transforms[0].transform.rotation.z self.wr4 = msg.transforms[0].transform.rotation.w self.Theta4 = euler_from_quaternion(self.xr4, self.yr4, self.zr4, self.wr4) if msg.transforms[0].child_frame_id == 'robot5': self.x5 = msg.transforms[0].transform.translation.x self.y5 = msg.transforms[0].transform.translation.y self.xr5 = msg.transforms[0].transform.rotation.x self.yr5 = msg.transforms[0].transform.rotation.y self.zr5 = msg.transforms[0].transform.rotation.z self.wr5 = msg.transforms[0].transform.rotation.w self.Theta5 = euler_from_quaternion(self.xr5, self.yr5, self.zr5, self.wr5) if msg.transforms[0].child_frame_id == 'robot6': self.x6 = msg.transforms[0].transform.translation.x self.y6 = msg.transforms[0].transform.translation.y self.xr6 = msg.transforms[0].transform.rotation.x self.yr6 = msg.transforms[0].transform.rotation.y self.zr6 = msg.transforms[0].transform.rotation.z self.wr6 = msg.transforms[0].transform.rotation.w self.Theta6 = euler_from_quaternion(self.xr6, self.yr6, self.zr6, self.wr6) self.distance = abs(self.x1 - self.x2) + abs(self.y1 - self.y2) + abs( self.x1 - self.x3) + abs(self.y1 - self.y3) + abs(self.x1 - self.x4) + abs( self.y1 - self.y4) + abs(self.x1 - self.x5) + abs( self.y1 - self.y5) + abs(self.x1 - self.x6) + abs(self.y1 - self.y6) print(self.distance) if self.distance > 2.2: " Calculate Control inputs u1, u2, u3, u4, u5, u6 " A = np.ones(6) - np.identity(6) # Adjancency Matrix self.X = np.array([[self.x1], [self.x2], [self.x3], [self.x4], [self.x5], [self.x6]]) #6x1 self.Y = np.array([[self.y1], [self.y2], [self.y3], [self.y4], [self.y5], [self.y6]]) #6x1 ux = np.zeros((6, 1)) # 6x1 uy = np.zeros((6, 1)) # 6x1 for i in range(1, 7): for j in range(1, 7): ux[i - 1] += -(A[i - 1][j - 1]) * ( self.X[i - 1] - self.X[j - 1]) # 1x1 each uy[i - 1] += -(A[i - 1][j - 1]) * ( self.Y[i - 1] - self.Y[j - 1]) # 1x1 each u1 = np.array([[float(ux[0])], [float(uy[0])]]) # 2x1 u2 = np.array([[float(ux[1])], [float(uy[1])]]) # 2x1 u3 = np.array([[float(ux[2])], [float(uy[2])]]) # 2x1 u4 = np.array([[float(ux[3])], [float(uy[3])]]) # 2x1 u5 = np.array([[float(ux[4])], [float(uy[4])]]) # 2x1 u6 = np.array([[float(ux[5])], [float(uy[5])]]) # 2x1 " Data Transformation into M_x, M_y, Phi_x, Phi_y " Phix = (u1[0][0] + u3[0][0]) / 2 # 1x1 Phiy = (u1[1][0] + u3[1][0]) / 2 # 1x1 Mx = ((self.x1 - self.x2) + (self.x3 - self.x2)) / 2 # 1x1 My = ((self.y1 - self.y2) + (self.y3 - self.y2)) / 2 # 1x1 " Calculate V1/W1, V2/W2, V3/W3, V4/W4, V5/W5, V6/W6 " S1 = np.array([[self.v1], [self.w1]]) #2x1 G1 = np.array([[1, 0], [0, 1 / L]]) #2x2 R1 = np.array([[math.cos(self.Theta1), math.sin(self.Theta1)], [-math.sin(self.Theta1), math.cos(self.Theta1)]]) #2x2 S1 = np.dot(np.dot(G1, R1), u1) #2x1 S2 = np.array([[self.v2], [self.w2]]) #2x1 G2 = np.array([[1, 0], [0, 1 / L]]) #2x2 R2 = np.array([[math.cos(self.Theta2), math.sin(self.Theta2)], [-math.sin(self.Theta2), math.cos(self.Theta2)]]) #2x2 S2 = np.dot(np.dot(G2, R2), u2) # 2x1 S3 = np.array([[self.v3], [self.w3]]) #2x1 G3 = np.array([[1, 0], [0, 1 / L]]) #2x2 R3 = np.array([[math.cos(self.Theta3), math.sin(self.Theta3)], [-math.sin(self.Theta3), math.cos(self.Theta3)]]) #2x2 S3 = np.dot(np.dot(G3, R3), u3) #2x1 S4 = np.array([[self.v4], [self.w4]]) #2x1 G4 = np.array([[1, 0], [0, 1 / L]]) #2x2 R4 = np.array([[math.cos(self.Theta4), math.sin(self.Theta4)], [-math.sin(self.Theta4), math.cos(self.Theta4)]]) #2x2 S4 = np.dot(np.dot(G4, R4), u4) #2x1 S5 = np.array([[self.v5], [self.w5]]) #2x1 G5 = np.array([[1, 0], [0, 1 / L]]) #2x2 R5 = np.array([[math.cos(self.Theta5), math.sin(self.Theta5)], [-math.sin(self.Theta5), math.cos(self.Theta5)]]) #2x2 S5 = np.dot(np.dot(G5, R5), u5) #2x1 S6 = np.array([[self.v6], [self.w6]]) #2x1 G6 = np.array([[1, 0], [0, 1 / L]]) #2x2 R6 = np.array([[math.cos(self.Theta6), math.sin(self.Theta6)], [-math.sin(self.Theta6), math.cos(self.Theta6)]]) #2x2 S6 = np.dot(np.dot(G6, R6), u6) #2x1 " Calculate VL1/VR1, VL2/VR2, VL3/VR3, VL4/VR4, VL5/VR5, VL6/VR6 " D = np.array([[1 / 2, 1 / 2], [-1 / (2 * d), 1 / (2 * d)]]) #2x2 Di = np.linalg.inv(D) #2x2 Speed_L1 = np.array([[self.vL1], [self.vR1] ]) # Vector 2x1 for Speed of Robot 1 Speed_L2 = np.array([[self.vL2], [self.vR2] ]) # Vector 2x1 for Speed of Robot 2 Speed_L3 = np.array([[self.vL3], [self.vR3] ]) # Vector 2x1 for Speed of Robot 3 Speed_L4 = np.array([[self.vL4], [self.vR4] ]) # Vector 2x1 for Speed of Robot 4 Speed_L5 = np.array([[self.vL5], [self.vR5] ]) # Vector 2x1 for Speed of Robot 5 Speed_L6 = np.array([[self.vL6], [self.vR6] ]) # Vector 2x1 for Speed of Robot 6 M1 = np.array([[S1[0]], [S1[1]]]).reshape(2, 1) #2x1 M2 = np.array([[S2[0]], [S2[1]]]).reshape(2, 1) #2x1 M3 = np.array([[S3[0]], [S3[1]]]).reshape(2, 1) #2x1 M4 = np.array([[S4[0]], [S4[1]]]).reshape(2, 1) #2x1 M5 = np.array([[S5[0]], [S5[1]]]).reshape(2, 1) #2x1 M6 = np.array([[S6[0]], [S6[1]]]).reshape(2, 1) #2x1 Speed_L1 = np.dot(Di, M1) # 2x1 (VL1, VR1) Speed_L2 = np.dot(Di, M2) # 2x1 (VL2, VR2) Speed_L3 = np.dot(Di, M3) # 2x1 (VL3, VR3) Speed_L4 = np.dot(Di, M4) # 2x1 (VL4, VR4) Speed_L5 = np.dot(Di, M5) # 2x1 (VL5, VR5) Speed_L6 = np.dot(Di, M6) # 2x1 (VL6, VR6) VL1 = float(Speed_L1[0]) VR1 = float(Speed_L1[1]) VL2 = float(Speed_L2[0]) VR2 = float(Speed_L2[1]) VL3 = float(Speed_L3[0]) VR3 = float(Speed_L3[1]) VL4 = float(Speed_L4[0]) VR4 = float(Speed_L4[1]) VL5 = float(Speed_L5[0]) VR5 = float(Speed_L5[1]) VL6 = float(Speed_L6[0]) VR6 = float(Speed_L6[1]) " Publish Speed Commands to Robot 1 " msgl1 = Float32() msgr1 = Float32() msgl1.data = VL1 msgr1.data = VR1 self.publisher_l1.publish(msgl1) self.publisher_r1.publish(msgr1) #self.get_logger().info('Publishing R1: "%s"' % msgr1.data) " Publish Speed Commands to Robot 2 " msgl2 = Float32() msgr2 = Float32() msgl2.data = VL2 msgr2.data = VR2 self.publisher_l2.publish(msgl2) self.publisher_r2.publish(msgr2) " Publish Speed Commands to Robot 3 " msgl3 = Float32() msgr3 = Float32() msgl3.data = VL3 msgr3.data = VR3 self.publisher_l3.publish(msgl3) self.publisher_r3.publish(msgr3) " Publish Speed Commands to Robot 4 " msgl4 = Float32() msgr4 = Float32() msgl4.data = VL4 msgr4.data = VR4 self.publisher_l4.publish(msgl4) self.publisher_r4.publish(msgr4) " Publish Speed Commands to Robot 5 " msgl5 = Float32() msgr5 = Float32() msgl5.data = VL5 msgr5.data = VR5 self.publisher_l5.publish(msgl5) self.publisher_r5.publish(msgr5) " Publish Speed Commands to Robot 6 " msgl6 = Float32() msgr6 = Float32() msgl6.data = VL6 msgr6.data = VR6 self.publisher_l6.publish(msgl6) self.publisher_r6.publish(msgr6) " Write Values to CSV1 and CSV2 " if self.count % 2 == 0: with open('transformed_dataset.csv', 'a', newline='') as f: fieldnames = ['M_x', 'M_y', 'Phi_x', 'Phi_y', 'U_x', 'U_y'] thewriter = csv.DictWriter(f, fieldnames=fieldnames) if self.i2 == 0: # write header value once thewriter.writeheader() self.i2 = 1 if self.j2 != 0: thewriter.writerow({ 'M_x': Mx, 'M_y': My, 'Phi_x': Phix, 'Phi_y': Phiy, 'U_x': u2[0][0], 'U_y': u2[1][0] }) if self.j2 == 0: # skip first value because it's noisy self.j2 = 1 self.count += 1 # Counter to skip values while saving to csv file else: print(" Simulation ", self.scene) # Stop Simulation sim.simxStopSimulation(clientID, sim.simx_opmode_oneshot_wait) # Retrieve some handles: ErrLocM1, LocM1 = sim.simxGetObjectHandle( clientID, 'robot1', sim.simx_opmode_oneshot_wait) if (not ErrLocM1 == sim.simx_return_ok): pass ErrLocM2, LocM2 = sim.simxGetObjectHandle( clientID, 'robot2#0', sim.simx_opmode_oneshot_wait) if (not ErrLocM2 == sim.simx_return_ok): pass ErrLoc1, Loc1 = sim.simxGetObjectPosition( clientID, LocM1, -1, sim.simx_opmode_oneshot_wait) if (not ErrLoc1 == sim.simx_return_ok): pass ErrLoc2, Loc2 = sim.simxGetObjectPosition( clientID, LocM2, -1, sim.simx_opmode_oneshot_wait) if (not ErrLoc2 == sim.simx_return_ok): pass ErrLocO1, OriRobo1 = sim.simxGetObjectOrientation( clientID, LocM1, -1, sim.simx_opmode_oneshot_wait) if (not ErrLocO1 == sim.simx_return_ok): pass ErrLocO2, OriRobo2 = sim.simxGetObjectOrientation( clientID, LocM2, -1, sim.simx_opmode_oneshot_wait) if (not ErrLocO2 == sim.simx_return_ok): pass OriRobo1[2] = scenes[self.scene][2] OriRobo2[2] = scenes[self.scene][5] # Set Robot Orientation sim.simxSetObjectOrientation(clientID, LocM1, -1, OriRobo1, sim.simx_opmode_oneshot_wait) sim.simxSetObjectOrientation(clientID, LocM2, -1, OriRobo2, sim.simx_opmode_oneshot_wait) Loc1[0] = scenes[self.scene][0] Loc2[0] = scenes[self.scene][3] Loc1[1] = scenes[self.scene][1] Loc2[1] = scenes[self.scene][4] # Set Robot Position sim.simxSetObjectPosition(clientID, LocM1, -1, Loc1, sim.simx_opmode_oneshot) sim.simxSetObjectPosition(clientID, LocM2, -1, Loc2, sim.simx_opmode_oneshot) # Print Positions and Orientation #print("Robot1 Position:", Loc1) #print("Robot2 Position:", Loc2) #print("Robot1 Orientation:", OriRobo1) #print("Robot2 Orientation:", OriRobo2) # Nb of Scene Counter self.scene += 1 # Start Simulation sim.simxStartSimulation(clientID, sim.simx_opmode_oneshot_wait) time.sleep(5) if self.scene == scenes.shape[0] - 1: # Stop Simulation sim.simxStopSimulation(clientID, sim.simx_opmode_oneshot_wait) # End Connection to V-Rep sim.simxFinish(clientID)
# Set Robot Position sim.simxSetObjectPosition(clientID, LocM1, -1, Loc1, sim.simx_opmode_oneshot) sim.simxSetObjectPosition(clientID, LocM2, -1, Loc2, sim.simx_opmode_oneshot) # Start the Simulation print("Robot1 Position:", Loc1) print("Robot2 Position:", Loc2) print("Robot1 Orientation:", OriRobo1) print("Robot2 Orientation:", OriRobo2) print("Simulation Running ...") #sim.simxStartSimulation(clientID, sim.simx_opmode_oneshot_wait) time.sleep(1) sim.simxStopSimulation(clientID, sim.simx_opmode_oneshot_wait) iter += 1 # Before closing the connection to CoppeliaSim, make sure that the last command sent out had time to arrive. You can guarantee this with (for example): sim.simxGetPingTime(clientID) # End Connection to V-Rep sim.simxFinish(clientID) else: print("Failed connecting to remote API server") print("Program Ended")
def stopSimulation(self): vrep.simxStopSimulation(self.clientId, vrep.simx_opmode_blocking) vrep.simxFinish(self.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)
vrep.simxSetJointTargetVelocity(clientID, slide1_handle, 0, opmode) vrep.simxSetJointTargetVelocity(clientID, slide2_handle, 0, opmode) vrep.simxSetJointTargetVelocity(clientID, Worm_2_handle, 0, opmode) vrep.simxSetJointTargetVelocity(clientID, Board_handle, 0, opmode) #keyboard "S" if keyboard.is_pressed("S"): vrep.simxSetJointTargetVelocity(clientID, slide1_handle, -1, opmode) vrep.simxSetJointTargetVelocity(clientID, slide2_handle, -1, opmode) vrep.simxSetJointTargetVelocity(clientID, Worm_2_handle, -1, opmode) vrep.simxSetJointTargetVelocity(clientID, Board_handle, -1, opmode) #keyboard "C" if keyboard.is_pressed("C"): vrep.simxSetJointTargetVelocity(clientID, DOOR_handle, 3, opmode) #keyboard "V" if keyboard.is_pressed("V"): vrep.simxSetJointTargetVelocity(clientID, DOOR_handle, 0, opmode) #keyboard "esc" if keyboard.is_pressed("esc"): vrep.simxStopSimulation(clientID, opmode) break else: print('Failed connecting to remote API server') print('End')
generated_img_file_path, transformed_image) if return_code == True: print( '\nTransformed maze image from CoppeliaSim: ' + str(generated_img_file_path) + ' was saved in \'generated_images\' folder successfully!' ) else: print( '\n[ERROR] Failed to save Transformed maze image from CoppeliaSim in \'generated_images\' folder.' ) # Stop the simulation return_code = sim.simxStopSimulation( client_id, sim.simx_opmode_oneshot) # Making sure that last command sent out had time to arrive sim.simxGetPingTime(client_id) if ((return_code == sim.simx_return_novalue_flag) or (return_code == sim.simx_return_ok)): print('\nSimulation stopped correctly.') time.sleep(2) # Stop the Remote API connection with CoppeliaSim server try: exit_remote_api_server()
def listener_callback(self, msg): if msg.transforms[0].child_frame_id == 'robot1' : self.x1 = msg.transforms[0].transform.translation.x self.y1 = msg.transforms[0].transform.translation.y self.xr1 = msg.transforms[0].transform.rotation.x self.yr1 = msg.transforms[0].transform.rotation.y self.zr1 = msg.transforms[0].transform.rotation.z self.wr1 = msg.transforms[0].transform.rotation.w self.Theta1 = euler_from_quaternion(self.xr1,self.yr1,self.zr1,self.wr1) if msg.transforms[0].child_frame_id == 'robot2' : self.x2 = msg.transforms[0].transform.translation.x self.y2 = msg.transforms[0].transform.translation.y self.xr2 = msg.transforms[0].transform.rotation.x self.yr2 = msg.transforms[0].transform.rotation.y self.zr2 = msg.transforms[0].transform.rotation.z self.wr2 = msg.transforms[0].transform.rotation.w self.Theta2 = euler_from_quaternion(self.xr2,self.yr2,self.zr2,self.wr2) distance = abs(self.x2 - self.x1) + abs(self.y2 - self.y1) print(distance) # Run Consensus Algorithm as long as they don't meet if distance > 1: " Calculate Control inputs u1 and u2 " u1 = np.array([[ self.k*(self.x2-self.x1)],[self.k*(self.y2-self.y1)]]) # 2x1 u2 = np.array([[ self.k*(self.x1-self.x2)],[self.k*(self.y1-self.y2)]]) # 2x1 " Calculate V1/W1 and V2/W2 " S1 = np.array([[self.v1], [self.w1]]) #2x1 G1 = np.array([[1,0], [0,1/L]]) #2x2 R1 = np.array([[math.cos(self.Theta1),math.sin(self.Theta1)],[-math.sin(self.Theta1),math.cos(self.Theta1)]]) #2x2 S1 = np.dot(np.dot(G1, R1), u1) #2x1 print(S1) S2 = np.array([[self.v2], [self.w2]]) #2x1 G2 = np.array([[1,0], [0,1/L]]) #2x2 R2 = np.array([[math.cos(self.Theta2),math.sin(self.Theta2)],[-math.sin(self.Theta2),math.cos(self.Theta2)]]) #2x2 S2 = np.dot(np.dot(G2, R2), u2) # 2x1 " Calculate VL1/VR1 and VL2/VR2 " D = np.array([[1/2,1/2],[-1/(2*d),1/(2*d)]]) #2x2 Di = np.linalg.inv(D) #2x2 Speed_L1 = np.array([[self.vL1], [self.vR1]]) # Vector 2x1 for Speed of Robot 1 Speed_L2 = np.array([[self.vL2], [self.vR2]]) # Vector 2x1 for Speed of Robot 2 M1 = np.array([[S1[0]],[S1[1]]]).reshape(2,1) #2x1 M2 = np.array([[S2[0]], [S2[1]]]).reshape(2,1) #2x1 Speed_L1 = np.dot(Di, M1) # 2x1 (VL1, VR1) Speed_L2 = np.dot(Di, M2) # 2x1 (VL2, VR2) VL1 = float(Speed_L1[0]) VR1 = float(Speed_L1[1]) VL2 = float(Speed_L2[0]) VR2 = float(Speed_L2[1]) " Calculate the Pose of Robot 2 w.r.t Robot 1 and Control input U1 " self.X1 = self.x2 - self.x1 # Relative Pose of Robot 2 wrt Robot 1 in Global frame for X coordinate of dimension 1x1 self.Y1 = self.y2 -self.y1 # Relative Pose of Robot 2 wrt Robot 1 in Global frame for Y coordinate of dimension 1x1 self.U1 = u1 # Control input U1 in Global frame for robot 1 of dimension 2x1 " Calculate the Pose of Robot 1 w.r.t Robot 2 and Control input U2 " self.X2 = self.x1 - self.x2 # Relative Pose of Robot 1 wrt Robot 2 in Global frame for X coordinate of dimension 1x1 self.Y2 = self.y1 -self.y2 # Relative Pose of Robot 1 wrt Robot 2 in Global frame for Y coordinate of dimension 1x1 self.U2 = u2 # Control input U2 in Global frame for robot 2 of dimension 2x1 " Transform Control Input U1 from Global to Local Reference Frame " U1L = np.dot(R1, self.U1) # Control input of Robot 1 in Local Frame of dimension 2x1 U2L = np.dot(R2, self.U2) # Control input of Robot 2 in Local Frame of dimension 2x1 " Transform Relative Pose from Global to Local Reference Frame " PoseG1 = np.array([[self.X1],[self.Y1]]) # Relative Pose of Robot 2 wrt Robot 1 in Global Frame of dimension 2x1 PoseL1 = np.dot(R1, PoseG1) # Relative Pose of Robot 2 wrt Robot 2 in Local Frame of dimension 2x1 PoseG2 = np.array([[self.X2],[self.Y2]]) # Relative Pose of Robot 1 wrt Robot 1 in Global Frame of dimension 2x1 PoseL2 = np.dot(R2, PoseG2) # Relative Pose of Robot 1 wrt Robot 2 in Local Frame of dimension 2x1 " Publish Speed Commands to Robot 1" msgl1 = Float32() msgr1 = Float32() msgl1.data = VL1 msgr1.data = VR1 self.publisher_l1.publish(msgl1) self.publisher_r1.publish(msgr1) #self.get_logger().info('Publishing R1: "%s"' % msgr1.data) " Publish Speed Commands to Robot 2" msgl2 = Float32() msgr2 = Float32() msgl2.data = VL2 msgr2.data = VR2 self.publisher_l2.publish(msgl2) self.publisher_r2.publish(msgr2) " Write Values to CSV1 and CSV2 " if self.count % 2 == 0: with open('robot1.csv', 'a', newline='') as f: fieldnames = ['Data_X', 'Data_Y', 'Angle', 'Label_X', 'Label_Y'] thewriter = csv.DictWriter(f, fieldnames=fieldnames) if self.i1 == 0: # write header value once thewriter.writeheader() self.i1 = 1 if self.j1 != 0: thewriter.writerow({'Data_X' : PoseL1[0][0], 'Data_Y' : PoseL1[1][0], 'Angle' : self.Theta1, 'Label_X' : U1L[0][0], 'Label_Y' : U1L[1][0]}) if self.j1 == 0: # skip first value because it's noisy self.j1 = 1 with open('robot2.csv', 'a', newline='') as f: fieldnames = ['Data_X', 'Data_Y', 'Angle', 'Label_X', 'Label_Y'] thewriter = csv.DictWriter(f, fieldnames=fieldnames) if self.i2 == 0: # write header value once thewriter.writeheader() self.i2 = 1 if self.j2 != 0: thewriter.writerow({'Data_X' : PoseL2[0][0], 'Data_Y' : PoseL2[1][0], 'Angle' : self.Theta2, 'Label_X' : U2L[0][0], 'Label_Y' : U2L[1][0]}) if self.j2 == 0: # skip first value because it's noisy self.j2 = 1 self.count += 1 # Counter to skip values while saving to csv file else: print(" Simulation ", self.scene) # Stop Simulation sim.simxStopSimulation(clientID, sim.simx_opmode_oneshot_wait) # Retrieve some handles: ErrLocM1,LocM1 =sim.simxGetObjectHandle(clientID, 'robot1', sim.simx_opmode_oneshot_wait) if (not ErrLocM1==sim.simx_return_ok): pass ErrLocM2,LocM2 =sim.simxGetObjectHandle(clientID, 'robot2#0', sim.simx_opmode_oneshot_wait) if (not ErrLocM2==sim.simx_return_ok): pass ErrLoc1,Loc1 =sim.simxGetObjectPosition(clientID, LocM1, -1, sim.simx_opmode_oneshot_wait) if (not ErrLoc1==sim.simx_return_ok): pass ErrLoc2,Loc2 =sim.simxGetObjectPosition(clientID, LocM2, -1, sim.simx_opmode_oneshot_wait) if (not ErrLoc2==sim.simx_return_ok): pass ErrLocO1,OriRobo1 =sim.simxGetObjectOrientation(clientID,LocM1, -1, sim.simx_opmode_oneshot_wait) if (not ErrLocO1==sim.simx_return_ok): pass ErrLocO2,OriRobo2 =sim.simxGetObjectOrientation(clientID,LocM2, -1, sim.simx_opmode_oneshot_wait) if (not ErrLocO2==sim.simx_return_ok): pass OriRobo1[2] = scenes[self.scene][2] OriRobo2[2] = scenes[self.scene][5] # Set Robot Orientation sim.simxSetObjectOrientation(clientID, LocM1, -1, OriRobo1, sim.simx_opmode_oneshot_wait) sim.simxSetObjectOrientation(clientID, LocM2, -1, OriRobo2, sim.simx_opmode_oneshot_wait) Loc1[0] = scenes[self.scene][0] Loc2[0] = scenes[self.scene][3] Loc1[1] = scenes[self.scene][1] Loc2[1] = scenes[self.scene][4] # Set Robot Position sim.simxSetObjectPosition(clientID, LocM1, -1, Loc1, sim.simx_opmode_oneshot) sim.simxSetObjectPosition(clientID, LocM2, -1, Loc2, sim.simx_opmode_oneshot) # Print Positions and Orientation #print("Robot1 Position:", Loc1) #print("Robot2 Position:", Loc2) #print("Robot1 Orientation:", OriRobo1) #print("Robot2 Orientation:", OriRobo2) # Nb of Scene Counter self.scene += 1 # Start Simulation sim.simxStartSimulation(clientID, sim.simx_opmode_oneshot_wait) time.sleep(3) if self.scene == scenes.shape[0]-1: # Stop Simulation sim.simxStopSimulation(clientID, sim.simx_opmode_oneshot_wait) # End Connection to V-Rep sim.simxFinish(clientID)
mean_x, mean_y = calMeanPos(cropImage) #^ 计算大scale的平均黑色像素点位置,用于小车找不到方向时决定方向使用 dir_x, dir_y = calMeanPos(landscape) #^ 增加一个函数calMeanPosOfBlackLine用于计算目标直线的平均黑点坐标 acc_x, acc_y = calMeanPosOfBlackLine(followLine) # print(acc_x) #^ 加速速率设为对数增长 accRate = max( math.log((followLineX - acc_x) / (followLineX / 5) + 1), 1) if acc_x else 1 #^ 计算大尺度下的error landscape_error = dir_y - bigY / 2 if dir_y else None # print(landscape_error) leftVelocity, rightVelocity = motor(mean_x, mean_y, landscape_error, accRate) #^ 同步触发 sim.simxSynchronousTrigger(clientID) #^ 暂且使用try-except来处理控制无限循环,具体逻辑懒得讲了,自己试试Ctrl+C是什么效果吧 except KeyboardInterrupt: print('=============================================') print('Simulation stopped due to keyboard interrupt.') print('=============================================') except Exception as e: traceback.print_exc() # stop simulation and cleanup status = sim.simxStopSimulation(clientID, sim.simx_opmode_oneshot) sim.simxGetPingTime(clientID) sim.simxFinish(clientID) #%%
def reset(self): if self.sim_running: self.stop_simulation() # Stop Simulation sim.simxStopSimulation(clientID, sim.simx_opmode_oneshot_wait) # Retrieve some handles: ErrLocM1, LocM1 = sim.simxGetObjectHandle( clientID, 'robot1', sim.simx_opmode_oneshot_wait) if (not ErrLocM1 == sim.simx_return_ok): pass ErrLocM2, LocM2 = sim.simxGetObjectHandle( clientID, 'robot2#0', sim.simx_opmode_oneshot_wait) if (not ErrLocM2 == sim.simx_return_ok): pass ErrLoc1, Loc1 = sim.simxGetObjectPosition( clientID, LocM1, -1, sim.simx_opmode_oneshot_wait) if (not ErrLoc1 == sim.simx_return_ok): pass ErrLoc2, Loc2 = sim.simxGetObjectPosition( clientID, LocM2, -1, sim.simx_opmode_oneshot_wait) if (not ErrLoc2 == sim.simx_return_ok): pass ErrLocO1, OriRobo1 = sim.simxGetObjectOrientation( clientID, LocM1, -1, sim.simx_opmode_oneshot_wait) if (not ErrLocO1 == sim.simx_return_ok): pass ErrLocO2, OriRobo2 = sim.simxGetObjectOrientation( clientID, LocM2, -1, sim.simx_opmode_oneshot_wait) if (not ErrLocO2 == sim.simx_return_ok): pass OriRobo1[2] = scenes[self.scene][2] OriRobo2[2] = scenes[self.scene][5] # Set Robot Orientation sim.simxSetObjectOrientation(clientID, LocM1, -1, OriRobo1, sim.simx_opmode_oneshot_wait) sim.simxSetObjectOrientation(clientID, LocM2, -1, OriRobo2, sim.simx_opmode_oneshot_wait) Loc1[0] = scenes[self.scene][0] Loc2[0] = scenes[self.scene][3] Loc1[1] = scenes[self.scene][1] Loc2[1] = scenes[self.scene][4] # Set Robot Position sim.simxSetObjectPosition(clientID, LocM1, -1, Loc1, sim.simx_opmode_oneshot) sim.simxSetObjectPosition(clientID, LocM2, -1, Loc2, sim.simx_opmode_oneshot) # Nb of Scene Counter self.scene += 1 " Use Adjacency Matrix to find Mxy and Phi's " A = np.ones(6) - np.identity(6) # Adjancency Matrix self.X = np.array([[self.x1], [self.x2], [self.x3], [self.x4], [self.x5], [self.x6]]) #6x1 self.Y = np.array([[self.y1], [self.y2], [self.y3], [self.y4], [self.y5], [self.y6]]) #6x1 Mx = np.zeros((6, 1)) # 6x1 My = np.zeros((6, 1)) # 6x1 for i in range(1, 7): for j in range(1, 7): Mx[i - 1] += (A[i - 1][j - 1]) * ( self.X[j - 1] - self.X[i - 1]) # 1x1 each My[i - 1] += (A[i - 1][j - 1]) * ( self.Y[j - 1] - self.Y[i - 1]) # 1x1 each Mx1 = float(Mx[0]) / 5 # 1x1 My1 = float(My[0]) / 5 # 1x1 Mx2 = float(Mx[1]) / 5 # 1x1 My2 = float(My[1]) / 5 # 1x1 Mx3 = float(Mx[2]) / 5 # 1x1 My3 = float(My[2]) / 5 # 1x1 Mx4 = float(Mx[3]) / 5 # 1x1 My4 = float(My[3]) / 5 # 1x1 Mx5 = float(Mx[4]) / 5 # 1x1 My5 = float(My[4]) / 5 # 1x1 Mx6 = float(Mx[5]) / 5 # 1x1 My6 = float(My[5]) / 5 # 1x1 self.Phix1 = (Mx2 + Mx3 + Mx4 + Mx5 + Mx6) / 5 # 1x1 self.Phiy1 = (My2 + My3 + My4 + My5 + My6) / 5 # 1x1 self.Phix2 = (Mx1 + Mx3 + Mx4 + Mx5 + Mx6) / 5 # 1x1 self.Phiy2 = (My1 + My3 + My4 + My5 + My6) / 5 # 1x1 self.Phix3 = (Mx1 + Mx2 + Mx4 + Mx5 + Mx6) / 5 # 1x1 self.Phiy3 = (My1 + My2 + My4 + My5 + My6) / 5 # 1x1 self.Phix4 = (Mx1 + Mx2 + Mx3 + Mx5 + Mx6) / 5 # 1x1 self.Phiy4 = (My1 + My2 + My3 + My5 + My6) / 5 # 1x1 self.Phix5 = (Mx1 + Mx2 + Mx3 + Mx4 + Mx6) / 5 # 1x1 self.Phiy5 = (My1 + My2 + My3 + My4 + My6) / 5 # 1x1 self.Phix6 = (Mx1 + Mx2 + Mx3 + Mx4 + Mx5) / 5 # 1x1 self.Phiy6 = (My1 + My2 + My3 + My4 + My5) / 5 # 1x1 observation_DQN = np.array([Mx1, My1, self.Phix1, self.Phiy1]) # Start Simulation sim.simxStartSimulation(clientID, sim.simx_opmode_oneshot_wait) time.sleep(5) return observation_DQN
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
def close(self): sim.simxStopSimulation(self.clientID, sim.simx_opmode_oneshot) sim.simxGetPingTime(self.clientID) sim.simxFinish(self.clientID)
def stop(self): s.simxStopSimulation(self._id, self._def_op_mode)
def endSimulation(clientID): sim.simxStopSimulation(clientID, sim.simx_opmode_oneshot) sim.simxFinish(clientID)
def close_connection_to_server(self): sim.simxGetPingTime(self.clientId) error = sim.simxStopSimulation(self.clientId, sim.simx_opmode_oneshot) if error == sim.simx_return_ok: print(str(error) + '! ERROR: simxSetModelProperty pioneer') sim.simxFinish(self.clientId)
avoid, ulb, urb = pc.braitenberg(clientID, usensor) errp, ulc, urc, pos, rot = pc.continuosControl( clientID, robot, (pointsx[step], pointsy[step])) ul = ulb if avoid else ulc ur = urb if avoid else urc # Check achieved goals achieved = achieved + 1 if pc.distance2p( pos, goals[achieved]) <= 0.3 else achieved # If an obstacle was avoided, replan the path. Only works when there are more than 2 goals left if prev and not avoid: path = pc.splinePath(p[:, 0][achieved:], p[:, 1][achieved:]) # New path of remaining points pointsx = np.linspace(min(p[:, 0][achieved:]), max(p[:, 0][achieved:]), num=(60 - step), endpoint=True) pointsy = path(pointsx) step = 0 # Start at the beginning of new path errf = vrep.simxSetJointTargetVelocity(clientID, motorL, ul, vrep.simx_opmode_streaming) errf = vrep.simxSetJointTargetVelocity(clientID, motorR, ur, vrep.simx_opmode_streaming) # The End vrep.simxStopSimulation(clientID, vrep.simx_opmode_oneshot)
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)
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)
UR5_sim_model.setJointAngle('UR5_joint4', desired_q[i, 1]) UR5_sim_model.setJointAngle('UR5_joint5', desired_q[i, 2]) desired_q1_record.append(desired_q[i, 0]) desired_q2_record.append(desired_q[i, 1]) desired_q3_record.append(desired_q[i, 2]) q = UR5_sim_model.getAllJointAngles() current_q1_record.append(q[2]) current_q2_record.append(q[3]) current_q3_record.append(q[4]) vrep_sim.simxSynchronousTrigger( client_ID) # trigger one simulation step vrep_sim.simxStopSimulation( client_ID, vrep_sim.simx_opmode_blocking) # stop the simulation vrep_sim.simxFinish(-1) # Close the connection print('Program terminated') plt.figure(figsize=(10, 5)) plt.plot(desired_q1_record, 'g', label='desired q 1') plt.plot(current_q1_record, 'r--', label='current q 1') plt.plot(desired_q2_record, 'b', label='desired q 2') plt.plot(current_q2_record, 'm--', label='current q 2') plt.plot(desired_q3_record, 'k', label='desired q 3') plt.plot(current_q3_record, 'y--', label='current q 3') plt.legend() plt.grid() plt.xlabel('time') plt.ylabel('joint angles') plt.show()