def init_fitness_streaming(self): """ subscribe to the fitness streams associated with this client """ for genome in self.genomes: genome_ID = genome.GetID() sig_name = 'FitnessScore_' + str(genome_ID) sim.simxGetFloatSignal(self.clientID, sig_name, mode4)
def stop_fitness_streaming(self): """ unsubscribe from streaming fitness signals at the end of every generation: otherwise streaming will continue, subscriptions pile up and block python """ for genome in self.genomes: genome_ID = genome.GetID() sig_name = 'FitnessScore_' + str(genome_ID) sim.simxGetFloatSignal(self.clientID, sig_name, mode5)
def backward(self, speed, distance): res, previousPosition = sim.simxGetFloatSignal( self.simulation, "signal", sim.simx_opmode_streaming) drivenDistance = 0 distance = previousPosition - distance while drivenDistance > distance: res, drivenDistance = sim.simxGetFloatSignal( self.simulation, "signal", sim.simx_opmode_streaming) self.setMotors(speed, speed) self.setMotors(0, 0)
def forward(self, speed, distance): res, previousPosition = sim.simxGetFloatSignal( self.simulation, "signal", sim.simx_opmode_streaming) drivenDistance = 0 distance = distance + previousPosition while drivenDistance < distance: res, drivenDistance = sim.simxGetFloatSignal( self.simulation, "signal", sim.simx_opmode_streaming) self.setMotors(-speed, -speed) self.setMotors(0, 0)
def get_simulation_time(self): # We get the error code and floating signal back from the server if self.first_time_list[0]: _, simulation_time = sim.simxGetFloatSignal( self.client_id, 'simulation_time', sim.simx_opmode_streaming) self.first_time_list[0] = False else: _, simulation_time = sim.simxGetFloatSignal( self.client_id, 'simulation_time', sim.simx_opmode_buffer) # We return the simulation time return simulation_time
def get_conveyor_4_speed(self): # We get the error code and floating signal back from the server if self.first_time_list[7]: _, conveyor_4_speed = sim.simxGetFloatSignal( self.client_id, 'active_conveyorBeltVelocity_4', sim.simx_opmode_streaming) self.first_time_list[7] = False else: _, conveyor_4_speed = sim.simxGetFloatSignal( self.client_id, 'active_conveyorBeltVelocity_4', sim.simx_opmode_buffer) # We return the simulation time return conveyor_4_speed
def gyro_read(): error = 1 error2 = 1 error3 = 1 while (error != 0): error, x = sim.simxGetFloatSignal(0, 'gyroX', sim.simx_opmode_streaming) while (error2 != 0): error2, y = sim.simxGetFloatSignal(0, 'gyroY', sim.simx_opmode_streaming) while (error3 != 0): error3, z = sim.simxGetFloatSignal(0, 'gyroZ', sim.simx_opmode_streaming) return [x, y, z]
def imu_read(): error = 1 error2 = 1 error3 = 1 while (error != 0): error, x = sim.simxGetFloatSignal(0, 'accelerometerX', sim.simx_opmode_streaming) while (error2 != 0): error2, y = sim.simxGetFloatSignal(0, 'accelerometerY', sim.simx_opmode_streaming) while (error3 != 0): error3, z = sim.simxGetFloatSignal(0, 'accelerometerZ', sim.simx_opmode_streaming) return [x, y, z]
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 gyro_read(): error = 1 error2 = 1 error3 = 1 while (error != 0): error, x = sim.simxGetFloatSignal(0, 'gyroX', sim.simx_opmode_streaming) print("LINEAR Y IS ") print(y) while (error2 != 0): error2, y = sim.simxGetFloatSignal(0, 'gyroY', sim.simx_opmode_streaming) print("LINEAR Y IS ") print(y) while (error3 != 0): error3, z = sim.simxGetFloatSignal(0, 'gyroZ', sim.simx_opmode_streaming) print("LINEAR Y IS ") print(y) return x, y, z
def prepareSim(self): self.initializeFunctions() isTracking = sim.simxGetFloatSignal(self.clientID, 'isTracking', sim.simx_opmode_streaming)[1] while isTracking != 1: sim.simxSynchronousTrigger(self.clientID) isTracking = sim.simxGetFloatSignal(self.clientID, 'isTracking', sim.simx_opmode_buffer)[1] sim.simxGetPingTime(self.clientID) X, Y, Z = self.getKinectXYZ(True) actionX = X actionY = Y actionZ = Z # print(actionX,actionY,actionZ) # 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)
def laiser_pointer(listVar): #sim.simxGetFloatSignal(client.id, "laserDistance", sim.simx_opmode_streaming) while 1: time.sleep(1.5) try: err_code, distance = sim.simxGetFloatSignal( client.id, "laserPointerData", sim.simx_opmode_streaming) print(f'Distancia {distance}') listVar['distance'] = distance if err_code != 0: raise Exception("teste") except Exception: print("Code error is {}".format(err_code))
def _sense_encoders(self) -> Tuple[float, float]: """Solve forward differential kinematics from encoder readings. Returns: z_v: Linear velocity of the robot center [m/s]. z_w: Angular velocity of the robot center [rad/s]. """ _, th_l = sim.simxGetFloatSignal(self._client_id, 'leftEncoder', sim.simx_opmode_buffer) _, th_r = sim.simxGetFloatSignal(self._client_id, 'rightEncoder', sim.simx_opmode_buffer) wr = th_r / self._dt wl = th_l / self._dt z_v = ((wr + wl) * self.WHEEL_RADIUS) / 2.0 z_w = ((wr - wl) * self.WHEEL_RADIUS) / self.TRACK return z_v, z_w
def update_fitness_scores(self, print_report): """ fetch fitness values from vrep and save to python genomes """ for genome in self.genomes: genome_ID = genome.GetID() sig_name = 'FitnessScore_' + str(genome_ID) ret, fitness = sim.simxGetFloatSignal(self.clientID, sig_name, mode4) if print_report: print("client", self.clientID, ": genome", genome_ID, "has", sig_name, ": ", fitness, "success: ", ret) if ret == 0: genome.SetEvaluated() genome.SetFitness(fitness)
def get_data_from_simulation(id): """Connects to the simulation and gets a float signal value Parameters ---------- id : str The signal id in CoppeliaSim. Possible values are 'accelX', 'accelY' and 'accelZ'. Returns ------- data : float The float value retrieved from the simulation. None if retrieval fails. """ if clientID != -1: res, data = sim.simxGetFloatSignal(clientID, id, sim.simx_opmode_blocking) if res == sim.simx_return_ok: return data return None
def imu_read_firsttime(): error, x = sim.simxGetFloatSignal(0, 'accelerometerX', sim.simx_opmode_buffer) while (error != sim.simx_return_ok): error, x = sim.simxGetFloatSignal(0, 'accelerometerX', sim.simx_opmode_streaming) error2, y = sim.simxGetFloatSignal(0, 'accelerometerY', sim.simx_opmode_buffer) while (error2 != sim.simx_return_ok): error2, y = sim.simxGetFloatSignal(0, 'accelerometerY', sim.simx_opmode_streaming) error3, z = sim.simxGetFloatSignal(0, 'accelerometerZ', sim.simx_opmode_buffer) while (error3 != sim.simx_return_ok): error3, z = sim.simxGetFloatSignal(0, 'accelerometerZ', sim.simx_opmode_streaming)
def getKinectXYZ(self, initialize=True): if initialize: X = sim.simxGetFloatSignal(self.clientID, 'actX', sim.simx_opmode_streaming)[1] Y = sim.simxGetFloatSignal(self.clientID, 'actY', sim.simx_opmode_streaming)[1] Z = sim.simxGetFloatSignal(self.clientID, 'actZ', sim.simx_opmode_streaming)[1] else: X = sim.simxGetFloatSignal(self.clientID, 'actX', sim.simx_opmode_buffer)[1] Y = sim.simxGetFloatSignal(self.clientID, 'actY', sim.simx_opmode_buffer)[1] Z = sim.simxGetFloatSignal(self.clientID, 'actZ', sim.simx_opmode_buffer)[1] sim.simxGetPingTime(self.clientID) return X, Y, Z
def gyro_read_firsttime(): error, x = sim.simxGetFloatSignal(0, 'gyroX', sim.simx_opmode_buffer) while (error != sim.simx_return_ok): error, x = sim.simxGetFloatSignal(0, 'gyroX', sim.simx_opmode_streaming) print("Angular X IS ") print(x) error2, y = sim.simxGetFloatSignal(0, 'gyroY', sim.simx_opmode_buffer) while (error2 != sim.simx_return_ok): error2, y = sim.simxGetFloatSignal(0, 'gyroY', sim.simx_opmode_streaming) print("Angular Y IS ") print(y) error3, z = sim.simxGetFloatSignal(0, 'gyroZ', sim.simx_opmode_buffer) while (error3 != sim.simx_return_ok): error3, z = sim.simxGetFloatSignal(0, 'gyroZ', sim.simx_opmode_streaming) print("Angular Z IS ") print(z)
print('Connected to remote API server') # Now try to retrieve data in a blocking fashion (i.e. a service call): # res,objs=sim.simxGetObjects(clientID,sim.sim_handle_all,sim.simx_opmode_blocking) # if res==sim.simx_return_ok: # print ('Number of objects in the scene: ',len(objs)) # else: # print ('Remote API function call returned with error code: ',res) time = 0 # retrieve motor handles errorCode, left_motor_handle = sim.simxGetObjectHandle( clientID, 'wheel_left_joint', sim.simx_opmode_blocking) errorCode, right_motor_handle = sim.simxGetObjectHandle( clientID, 'wheel_right_joint', sim.simx_opmode_blocking) r, signalValue = sim.simxGetFloatSignal(clientID, 'Turtlebot2_simulation_time', sim.simx_opmode_streaming) video = [[5, 5], [10, 10], [10, 5], [5, 10], [10, 5], [10, 10], [10, 5], [5, 10], [10, 0], [5, 10], [10, 5], [0, 10], [5, 10], [10, 5], [5, 10], [10, 5], [5, 10], [5, 0], [0, 5], [5, 0], [0, 10], [10, 5], [0, 10], [10, 0], [5, 0], [0, 5], [10, 5], [5, 10], [10, 10], [10, 10], [10, 10], [10, 10], [10, 10], [10, 5], [0, 10]] rev1 = video[::-1] for k in rev1: time = 0 err_code1 = 1 err_code2 = 2 # print(type(k[0])) while (err_code1 != 0 and err_code2 != 0):
def color_sort(): color_list = [] result, cs_bookline = sim.simxGetFloatSignal(clientID, "cs_bookline", sim.simx_opmode_streaming) result, book = sim.simxGetFloatSignal(clientID, "book", sim.simx_opmode_streaming) #move to intial position moveJoints(cs_robot, position0) #set the robot as busy sim.simxSetFloatSignal(clientID, 'cs_robotBusy', 1, sim.simx_opmode_oneshot) while (cs_bookline == 0): #wait for book to be detected while (book == 0): result, book = sim.simxGetFloatSignal(clientID, "book", sim.simx_opmode_buffer) #move book to cs table moveJoints(cs_robot, cs_left_conveyor) sim.simxSetFloatSignal(clientID, 'cs_succ', 1, sim.simx_opmode_oneshot) # pick up the book time.sleep(0.5) moveJoints(cs_robot, cs_table) sim.simxSetFloatSignal(clientID, 'cs_succ', 0, sim.simx_opmode_oneshot) # drop the book off #check book color result, res, image = sim.simxGetVisionSensorImage( clientID, cs_vis, 0, sim.simx_opmode_blocking) book_color = [image[0], image[1], image[2]] #RGB color value book_order = len(color_list) #assumes color is new initially rng = 10 #range to determine wheter to colors are the same or not #loop that check RGB values of every color list with book_color to determine is the same as a color in color_list for color in enumerate(color_list): if (abs(color[1][0] - book_color[0]) < rng) and (abs(color[1][1] - book_color[1]) < rng) and ( abs(color[1][2] - book_color[2]) < rng): book_order = color[0] #if color is new append it to the color_list if (book_order == len(color_list)): color_list.append(book_color) #set the book position depending on its order book_position = invk(cs_robot, -1.2952, -0.68 + 0.154 * (book_order), 0.25) #move and place the book in the proper position sim.simxSetFloatSignal(clientID, 'cs_succ', 1, sim.simx_opmode_oneshot) # pick up the book\ time.sleep(0.5) moveJoints(cs_robot, book_position) time.sleep(1.1) sim.simxSetFloatSignal(clientID, 'cs_succ', 0, sim.simx_opmode_oneshot) # drop the book off result, cs_bookline = sim.simxGetFloatSignal(clientID, "cs_bookline", sim.simx_opmode_buffer) time.sleep(1) #set the robot as free sim.simxSetFloatSignal(clientID, 'cs_robotBusy', 0, sim.simx_opmode_oneshot) moveJoints(cs_robot, position0) return color_list
def _is_done(self): pathIsDone = sim.simxGetFloatSignal(self.clientID, 'movePathDone', sim.simx_opmode_blocking)[1] sim.simxGetPingTime(self.clientID) return True if pathIsDone == 1 else False
def size_sort(): result, stack = sim.simxGetFloatSignal(clientID, "stack", sim.simx_opmode_streaming) result, height = sim.simxGetFloatSignal(clientID, "height", sim.simx_opmode_streaming) result, size = sim.simxGetFloatSignal(clientID, "size", sim.simx_opmode_streaming) size_list = [] #wait for stack to be detected while (stack == 0): result, stack = sim.simxGetFloatSignal(clientID, "stack", sim.simx_opmode_buffer) #set the robot as busy sim.simxSetFloatSignal(clientID, 'ss_robotBusy', 1, sim.simx_opmode_oneshot) while (stack > 0): #get top book height result, height = sim.simxGetFloatSignal(clientID, "height", sim.simx_opmode_buffer) #set book position depending on stack length book_position = invk(ss_robot, -1.2952, 0.6111, height) #move book to ss table moveJoints(ss_robot, book_position) sim.simxSetFloatSignal(clientID, 'ss_succ', 1, sim.simx_opmode_oneshot) # pick up the book time.sleep(1) moveJoints(ss_robot, ss_table) sim.simxSetFloatSignal(clientID, 'ss_succ', 0, sim.simx_opmode_oneshot) # drop the book off time.sleep(2) #get book size result, size = sim.simxGetFloatSignal(clientID, "size", sim.simx_opmode_buffer) size_list.append([len(size_list), size, height]) #separate stack sim.simxSetFloatSignal(clientID, 'ss_succ', 1, sim.simx_opmode_oneshot) # pick up the book book_position = invk(ss_robot, -0.425, 0.6025 - (len(size_list) - 1) * 0.16, 0.17) moveJoints(ss_robot, book_position) #move to appropriate position sim.simxSetFloatSignal(clientID, 'ss_succ', 0, sim.simx_opmode_oneshot) # drop the book off time.sleep(1.1) result, stack = sim.simxGetFloatSignal(clientID, "stack", sim.simx_opmode_buffer) #loop to get book thickness for i in range(len(size_list) - 1): size_list[i][2] = size_list[i][2] - size_list[i + 1][2] size_list[len(size_list) - 1][2] = size_list[len(size_list) - 1][2] - 0.15 #sort the size list size_list = sorted(size_list, key=lambda size: size[1], reverse=True) #largest books largest_book = size_list[0] #remove largest book from size list size_list.pop(0) #intial stack height height = largest_book[2] + 0.15 #stack the books depending on size for i in range(len(size_list)): intial_position = invk(ss_robot, -0.425, 0.6025 - (size_list[i][0]) * 0.15, 0.14) moveJoints(ss_robot, intial_position) sim.simxSetFloatSignal(clientID, 'ss_succ', 1, sim.simx_opmode_oneshot) # pick up the book time.sleep(1) #define a mid position to stop the robot from knocking over other robots mid_position = getJointAngles(ss_robot) mid_position[1] = 0 moveJoints(ss_robot, mid_position) #finding height so the robot knows where to put the robot height = height + size_list[i][2] #get the largest book's position target_position = invk(ss_robot, -0.425, 0.6025 - (largest_book[0]) * 0.15, height) #move to the smaller book on top of the bigger book moveJoints(ss_robot, target_position) time.sleep(1.5) sim.simxSetFloatSignal(clientID, 'ss_succ', 0, sim.simx_opmode_oneshot) # drop the book off time.sleep(1) time.sleep(1) #set the robot as free sim.simxSetFloatSignal(clientID, 'ss_robotBusy', 0, sim.simx_opmode_oneshot) time.sleep(3) #wait for book stack to reach the end return size_list
def tiempoSimulador(self): _, tiempo = sim.simxGetFloatSignal(self.clientID, "tiemposimulacion", sim.simx_opmode_streaming) return tiempo
def _init_encoders(self): """Initialize encoder streaming.""" sim.simxGetFloatSignal(self._client_id, 'leftEncoder', sim.simx_opmode_streaming) sim.simxGetFloatSignal(self._client_id, 'rightEncoder', sim.simx_opmode_streaming)
rf_z = -0.482 lf_z = -0.482 rb_z = -0.482 if gait_state == 1: #trot步态 lb_x = -0.1 rb_x = 0.1 lf_x = 0.1 rf_x = -0.1 lb_z = -0.482 rf_z = -0.482 lf_z = -0.482 rb_z = -0.482 [rec, vrep_time ] = sim.simxGetFloatSignal(clientID, 'time', sim.simx_opmode_oneshot) #获取仿真时间 else: [rec, vrep_realtime ] = sim.simxGetFloatSignal(clientID, 'time', sim.simx_opmode_oneshot) #获取仿真时间 if gait_state == 2: #慢步步态 T = 1 time1 = vrep_realtime - vrep_time time2 = vrep_realtime - vrep_time + 0.25 time3 = vrep_realtime - vrep_time + 0.5 time4 = vrep_realtime - vrep_time + 0.75 T1 = time1 % T T2 = time2 % T #mod 函数为取模运算 T3 = time3 % T T4 = time4 % T [lb_x, lb_z] = gait_plan2(T1, T)