def reinitialization_process(conf): vrep.simxStopSimulation(conf['clientID'], vrep.simx_opmode_oneshot_wait) time.sleep(1.) vrep.simxSynchronous(conf['clientID'],True) return_code = vrep.simxStartSimulation(conf['clientID'], vrep.simx_opmode_oneshot)# However, v-rep will do strange initialization. while return_code != 0: return_code = vrep.simxStartSimulation(conf['clientID'], vrep.simx_opmode_oneshot) # vrep.simxSetStringSignal(clientID,'jacoHand','true',vrep.simx_opmode_oneshot) # starting streaming mode for i in range(conf['num_particles']): starting_streaming_buffer_mode(conf['clientID'], conf['particle_handles'][i], 'simxGetObjectPosition') starting_streaming_buffer_mode(conf['clientID'], conf['cup_handle'], 'simxGetObjectPosition') starting_streaming_buffer_mode(conf['clientID'], conf['cup_handle'], 'simxGetObjectOrientation') starting_streaming_buffer_mode(conf['clientID'], conf['end_effector_handle'], 'simxGetObjectOrientation') starting_streaming_buffer_mode(conf['clientID'], conf['end_effector_handle'], 'simxGetObjectPosition') # print('STREAMING_MODE FINISHED') for i in range(len(conf['joint_handles'])): returnCode = vrep.simxSetJointTargetPosition(conf['clientID'], conf['joint_handles'][i], conf['initial_joint_angles'][i], vrep.simx_opmode_streaming) if returnCode != vrep.simx_return_ok: print('Initialization of the joint angles fails.' + str(returnCode)) for i in range(round(0.03/conf['dt'])): return_code = vrep.simxSynchronousTrigger(conf['clientID']) # get cup position returnCode, cup_position = vrep.simxGetObjectPosition(conf['clientID'], conf['cup_handle'], -1, vrep.simx_opmode_buffer) if returnCode != 0: print('Can not find Cup positions!') # Distribute different positions to particles according to the position of liquid for i in range(len(conf['particle_handles'])): returnCode = vrep.simxSetObjectPosition(conf['clientID'], conf['particle_handles'][i], -1, np.array([cup_position[0],cup_position[1],cup_position[2] + 0.022* i]), vrep.simx_opmode_oneshot) if returnCode != 0: print('Can not set particle position! Return code: %i' %returnCode) for i in range(round(0.7/conf['dt'])): return_code = vrep.simxSynchronousTrigger(conf['clientID'])
def step_forward_move(self): result = {} if self.synchronous: if self.moving_queue.empty(): result['flag'] = False vrep.simxSynchronousTrigger(self.clientID) # take photos if needed if self.need_take_photo: image_0 = self.getImage(0) image_1 = self.getImage(1) if image_0 is not None and image_1 is not None: result['photos'] = [image_0, image_1] self.need_take_photo = False else: speed = self.moving_queue.get() self.move(self.target_orientation, speed) self.left_time = self.moving_queue.qsize() # reset cumul if needed if self.has_reset_cumul: vrep.simxSetIntegerSignal(self.clientID, 'clear', 1, vrep.simx_opmode_oneshot) self.has_reset_cumul = False # take photos if needed if self.need_take_photo: image_0 = self.getImage(0) image_1 = self.getImage(1) if image_0 is not None and image_1 is not None: result['photos'] = [image_0, image_1] self.need_take_photo = False result['flag'] = True return result else: assert 0
def step(self, action): # set actions self.__MODE.set(action) vrep.simxSynchronousTrigger(self.__ID) vrep.simxGetPingTime(self.__ID) # get new states return self.__MODE.get()
def __init__(self): self.portNumb = 19997 self.actions = [] self.score = 0 self.hasfallen = False self.LUMSpeed = 0 self.LLMSpeed = 0 self.RUMSpeed = 0 self.RLMSpeed = 0 self.clientID = vrep.simxStart('127.0.0.1', self.portNumb, True, True, 5000, 5) if self.clientID != -1: print("Se pudo establecer la conexión con la api del simulador") LUM, LLM, RUM, RLM, head = recoverRobotParts(self.clientID) self.LUM = LUM self.LLM = LLM self.RUM = RUM self.RLM = RLM self.head = head #Set simulation to be Synchonous instead of Asynchronous vrep.simxSynchronous(self.clientID, True) #Start simulation if it didn't start vrep.simxStartSimulation(self.clientID, vrep.simx_opmode_blocking) setVelocity(self.clientID, self.LUM, self.LUMSpeed) setVelocity(self.clientID, self.LLM, self.LLMSpeed) setVelocity(self.clientID, self.RUM, self.RUMSpeed) setVelocity(self.clientID, self.RLM, self.RLMSpeed) #Esto es necesario porque la primera observación siempre es incorrecta self.observation_space() vrep.simxSynchronousTrigger(self.clientID)
def step(self, action): # execute one step in env self._set_joint_effort(action) # torque = [vrep.simxGetJointForce(self.clientID, self.handles['joint'][i], vrep.simx_opmode_blocking)[1] # for i in range(self.action_dims)] # retrieve the current torque from 7 joints # print('now=', torque, '\nnext=', action) # send a synchronization trigger signal to inform V-REP to execute the next simulation step vrep.simxSynchronousTrigger(self.clientID) vrep.simxGetPingTime(self.clientID) Reward, ter_flag, env_info = self._reward( action) # ter_flag: True if finish the task # TODO: define the scaled reward if self.t < self._max_episode_length * 0.8: scaled_reward = (self.t / self._max_episode_length) * Reward else: scaled_reward = (self.final_phase_scaler * self.t / self._max_episode_length) * Reward self.t += 1 next_observation = self._get_observation() self.cur_obs = next_observation return next_observation, scaled_reward, ter_flag, env_info
def __init__(self, max_actions=600): self.max_actions = max_actions self.portNumb = recoverPorts()[0] self.actions = [] self.score = 0 self.hasfallen = False # self.LUMSpeed = 0 # self.LLMSpeed = 0 # self.RUMSpeed = 0 # self.RLMSpeed = 0 self.clientID = vrep.simxStart('127.0.0.1', self.portNumb, True, True, 5000, 5) if self.clientID != -1 : print ("Connection to the simulator has been established") self.robotController = robotController(self.clientID) # LUM,LLM,RUM,RLM,head = recoverRobotParts(self.clientID) # self.LUM = LUM # self.LLM = LLM # self.RUM = RUM # self.RLM = RLM # self.head = head #Set simulation to be Synchonous instead of Asynchronous vrep.simxSynchronous(self.clientID, True) #Start simulation if it didn't start vrep.simxStartSimulation(self.clientID,vrep.simx_opmode_blocking) self.robotController.resetRobot() # setVelocity(self.clientID,self.LUM,self.LUMSpeed) # setVelocity(self.clientID,self.LLM,self.LLMSpeed) # setVelocity(self.clientID,self.RUM,self.RUMSpeed) # setVelocity(self.clientID,self.RLM,self.RLMSpeed) #Esto es necesario porque la primera observación siempre es incorrecta self.observation_space() vrep.simxSynchronousTrigger(self.clientID)
def getRange(self): "get sensor range" eCode, value = vrep.simxGetFloatSignal(self.robot, "HOMEO_SIGNAL_"+self._eye+"Eye"+"_MAX_LIGHT",getattr(vrep,self._opMode)) vrep.simxSynchronousTrigger(self.robot) if eCode != 0: raise TransducerException("Cannot get maxLight of VREP sensor: " + self._eye+"Eye") return value
def setAngle(self, angleToRotate): if angleToRotate == 0.0: return speedRotation = 0.5 sign = angleToRotate / math.fabs(angleToRotate) self.setLeftMotorVelocity(speedRotation * sign) self.setRightMotorVelocity(-speedRotation * sign) vrep.simxSynchronousTrigger(self.clientID) previousAngle = self.getAngle() rotation = 0 while math.fabs(rotation) <= math.fabs(angleToRotate): angle = self.getAngle() da = angle - previousAngle if da > 0: da = math.fmod(da + math.pi, 2 * math.pi) - math.pi else: da = math.fmod(da - math.pi, 2 * math.pi) + math.pi rotation += da previousAngle = angle #self.printMessage(str(rotation) + ' ' + str(angleToRotate)) self.setLeftMotorVelocity(0) self.setRightMotorVelocity(0) vrep.simxSynchronousTrigger(self.clientID)
def control_routine(_setpoint, _time): # Loop de Controle pid.setpoint = _setpoint start = millis() while (millis() - start < _time): # Realiza coleta do tempo atual (ms) cur_time = millis() # Realiza leitura do ângulo da junta e converte para graus, com precisão decimal de 2 casas ret, ang = vrep.simxGetJointPosition(clientID, handler_junta, vrep.simx_opmode_oneshot_wait) ang = round(math.degrees(ang), 2) # Atualiza PID com ângulo atual como entrada output = pid(ang) output = round(output, 2) print("Output", output) # Adiciona força à planta add_force_and_torque(handler_helice, [0, 0, output]) # Adiciona valores para plot do gráfico list_time.append(cur_time) list_angle.append(ang) list_setpoint.append(pid.setpoint) # Aguarda pelo trigger de modo síncrono da simulação vrep.simxSynchronousTrigger(clientID)
def simulate(self): # vrep related ''' cmd = input('Press <enter> key to step the simulation!') if cmd == 'q': # quit return False ''' self.t += self.dt self.ts.append(self.t) self.propagateXid() countReachedGoal = 0 for robot in self.robots: robot.precompute() for robot in self.robots: robot.readSensorData() robot.propagateDesired() robot.propagate() if robot.reachedGoal: countReachedGoal += 1 self.calcCOG() if self.vrepConnected: vrep.simxSynchronousTrigger(self.clientID) if countReachedGoal == len(self.robots): return False else: return True
def run_simulation(self): self.set_num_steps() vrep.simxSynchronous(self._clientID,True); vrep.simxStartSimulation(self._clientID,vrep.simx_opmode_oneshot); for simStep in range (self._num_steps): self._pid = set_target_thetas(self._num_steps, self._pid,self._experiment,self._simulator,simStep) for joint in range(6): errorCode, self._theta[joint] = vrep.simxGetJointPosition(self._clientID,self._arm_joints[joint],vrep.simx_opmode_blocking) self._linearVelocity[joint] = self._pid[joint].get_velocity(math.degrees(self._theta[joint]))/self._convertdeg2rad vrep.simxSetJointTargetVelocity(self._clientID,self._arm_joints[joint],self._linearVelocity[joint],vrep.simx_opmode_oneshot) vrep.simxSynchronousTrigger(self._clientID) vrep.simxGetPingTime(self._clientID) self._positions.append(vrep.simxGetObjectPosition(self._clientID,self._arm_joints[5],-1,vrep.simx_opmode_blocking)[1] + vrep.simxGetObjectOrientation(self._clientID,self._arm_joints[5],-1,vrep.simx_opmode_blocking)[1] + vrep.simxGetObjectPosition(self._clientID,self._cube,-1,vrep.simx_opmode_blocking)[1] + vrep.simxGetObjectQuaternion(self._clientID,self._cube,-1,vrep.simx_opmode_blocking)[1]) vrep.simxStopSimulation(self._clientID, vrep.simx_opmode_blocking) vrep.simxFinish(self._clientID) saveStats(self._experiment,self._current_iteration, self._physics_engine,self._simulator, self._positions)
def restart(self,earlyStop = False): if (self.cid != None): vrep.simxStopSimulation(self.cid, self.mode()) vrep.simxSynchronousTrigger(self.cid) vrep.simxFinish(-1) # just in case, close all opened connections time.sleep(1) self.connect() time.sleep(1) vrep.simxLoadScene(self.cid, '/home/elias/[email protected]/_Winter2015/CSC494/Scenes/Base_Quad.ttt', 0, self.mode()) if earlyStop: vrep.simxStopSimulation(self.cid, self.mode()) vrep.simxSynchronousTrigger(self.cid) vrep.simxFinish(-1) # just in case, close all opened connections return vrep.simxStartSimulation(self.cid, self.mode()) self.runtime = 0 err, self.copter = vrep.simxGetObjectHandle(self.cid, "Quadricopter_base", vrep.simx_opmode_oneshot_wait) err, self.target = vrep.simxGetObjectHandle(self.cid, "Quadricopter_target", vrep.simx_opmode_oneshot_wait) err, self.front_camera = vrep.simxGetObjectHandle(self.cid, 'Quadricopter_frontCamera', vrep.simx_opmode_oneshot) err, lin, ang = vrep.simxGetObjectVelocity(self.cid, self.copter, vrep.simx_opmode_streaming) self.getTargetStart() for i in range(4): self.propellerScripts[i] = vrep.simxGetObjectHandle(self.cid, 'Quadricopter_propeller_respondable' + str(i) + str(1), self.mode())
def step_simulation(self, torque_commands): vrep.simxSetJointTargetVelocity( self.clientID, self.jointHandles[0], 90000 if torque_commands[0] > 0 else -9000, vrep.simx_opmode_oneshot) vrep.simxSetJointForce( self.clientID, self.jointHandles[0], torque_commands[0] if torque_commands[0] > 0 else -torque_commands[0], vrep.simx_opmode_oneshot) vrep.simxSetJointTargetVelocity( self.clientID, self.jointHandles[1], 90000 if torque_commands[1] > 0 else -9000, vrep.simx_opmode_oneshot) vrep.simxSetJointForce( self.clientID, self.jointHandles[1], torque_commands[1] if torque_commands[1] > 0 else -torque_commands[1], vrep.simx_opmode_oneshot) vrep.simxSynchronousTrigger( self.clientID ) # Trigger next simulation step (Blocking function call) vrep.simxGetPingTime( self.clientID ) # Ensure simulation step has completed (Blocking function call)
def _take_action(self, action): if (abs(action[0]) > 1): l = 1 if action[0] > 0 else -1 else: l = action[0] l = l/10 omega = action[1] HALF_AXIS = 0.0325 WHEEL_R = 0.032/2 motorSpeed = [0,0] #angularFreqL = l - HALF_AXIS * omega; #angularFreqR = l + HALF_AXIS * omega; #motorSpeedL = (angularFreqL / WHEEL_R); #motorSpeedR = (angularFreqR / WHEEL_R); angularFreqL = l - HALF_AXIS * omega; angularFreqR = l + HALF_AXIS * omega; motorSpeed[0] = (angularFreqL / WHEEL_R); motorSpeed[1] = (angularFreqR / WHEEL_R); handles = [self.act_handles[key] for key in sorted(self.act_handles.keys(), reverse=True)] for idx,motor_handle in enumerate(handles): vrep.simxSetJointTargetVelocity(self.clientID, motor_handle, motorSpeed[idx], # target velocity vrep.simx_opmode_blocking) vrep.simxSynchronousTrigger(self.clientID)
def control_vrep(clientID, count): # 读取Base 和 joint 的handle jointHandle = np.zeros((jointNum, ), dtype=np.int) for i in range(jointNum): _, returnHandle = vrep.simxGetObjectHandle(clientID, jointName + str(i + 1), vrep.simx_opmode_blocking) jointHandle[i] = returnHandle _, baseHandle = vrep.simxGetObjectHandle(clientID, baseName, vrep.simx_opmode_blocking) print('Handles available') # 首次读取关节的初始值,以streaming的形式 jointConfigure = np.zeros((jointNum, )) for i in range(jointNum): _, jpos = vrep.simxGetJointPosition(clientID, jointHandle[i], vrep.simx_opmode_streaming) jointConfigure[i] = jpos vrep.simxSynchronousTrigger(clientID) # 让仿真走一步 ### 控制指定关节 #### vrep.simxPauseCommunication(clientID, True) vrep.simxSetJointTargetPosition(clientID, jointHandle[count], jointConfigure[count] + 1 / RAD2EDG, vrep.simx_opmode_oneshot) vrep.simxPauseCommunication(clientID, False) vrep.simxSynchronousTrigger(clientID) vrep.simxGetPingTime(clientID) # 使得该仿真步
def cleanUp(self): print "About to stop simulation connected to self.simulID: ", self.simulID vrep.simxStopSimulation(self.simulID, vrep.simx_opmode_oneshot) vrep.simxSynchronousTrigger(self.simulID) vrep.simxFinish(self.simulID) vrep.simxFinish(-1) print "Disconnected from V-REP"
def shake_arm(self): self.move_to_positions = [] for i in range(4): self.move_to_positions.append([np.random.uniform(low=1.018, high=1.223, size=1)[0], \ np.random.uniform(low=0.951, high=1.294, size=1)[0], np.random.uniform(low=0.1, \ high=0.18, size=1)[0]]) for i in range(len(self.move_to_positions)): _, sawyer_target_position = vrep.simxGetObjectPosition(self.client_id, \ self.sawyer_target_handle, -1, vrep.simx_opmode_blocking) move_position = self.move_to_positions[i] move_direction = np.asarray([move_position[0] - sawyer_target_position[0], \ move_position[1] - sawyer_target_position[1], move_position[2] - \ sawyer_target_position[2]]) move_magnitude = np.linalg.norm(move_direction) move_step = 0.01 * move_direction / move_magnitude num_move_steps = int(np.floor(move_magnitude / 0.01)) for step_iter in range(num_move_steps): vrep.simxSetObjectPosition(self.client_id, self.sawyer_target_handle, -1, \ (sawyer_target_position[0]+move_step[0], sawyer_target_position[1] + \ move_step[1], sawyer_target_position[2] + move_step[2]), \ vrep.simx_opmode_blocking) _, sawyer_target_position = vrep.simxGetObjectPosition(self.client_id, \ self.sawyer_target_handle, -1, vrep.simx_opmode_blocking) vrep.simxSynchronousTrigger(self.client_id) vrep.simxGetPingTime(self.client_id) _ = vrep.simxSetObjectPosition(self.client_id, self.sawyer_target_handle, -1, \ move_position, vrep.simx_opmode_blocking) vrep.simxSynchronousTrigger(self.client_id) vrep.simxGetPingTime(self.client_id)
def step(self, action): # set actions self.__set(action) vrep.simxSynchronousTrigger(self.__ID) # get new states self.__get() return (self.state, self.reward, self.done, {})
def set_gripper_position(self): _, sawyer_target_position = vrep.simxGetObjectPosition(self.client_id, \ self.robot.sawyer_target_handle, -1, vrep.simx_opmode_blocking) self.image = self.robot.get_image() move_direction = np.asarray([self.pickup_position[0] - sawyer_target_position[0], \ self.pickup_position[1] - sawyer_target_position[1], self.pickup_position[2] - \ sawyer_target_position[2]]) move_magnitude = np.linalg.norm(move_direction) move_step = 0.03 * move_direction / move_magnitude num_move_steps = int(np.floor(move_magnitude / 0.03)) remaining_magnitude = -num_move_steps * 0.03 + move_magnitude remaining_distance = remaining_magnitude * move_direction / move_magnitude for step_iter in range( num_move_steps): #selects action and executes action vrep.simxSetObjectPosition(self.client_id, self.robot.sawyer_target_handle, -1, \ (sawyer_target_position[0] + move_step[0], sawyer_target_position[1] + \ move_step[1], sawyer_target_position[2] + move_step[2]), vrep.simx_opmode_blocking) _, sawyer_target_position = vrep.simxGetObjectPosition(self.client_id, \ self.robot.sawyer_target_handle, -1, vrep.simx_opmode_blocking) vrep.simxSynchronousTrigger(self.client_id) vrep.simxGetPingTime(self.client_id) vrep.simxSetObjectPosition(self.robot.client_id, self.robot.sawyer_target_handle, -1, \ (sawyer_target_position[0] + remaining_distance[0], sawyer_target_position[1] + \ remaining_distance[1], sawyer_target_position[2]+ remaining_distance[2]), \ vrep.simx_opmode_blocking) vrep.simxSynchronousTrigger(self.client_id) vrep.simxGetPingTime(self.client_id)
def move(self, target_orientation, speed): self.v_now = speed speed = target_orientation * speed for i in range(3): self.controller_position[i] += speed[i] self.setPosition('controller', self.controller_position) vrep.simxSynchronousTrigger(self.clientID)
def send_gear_commands(clientID, target_degree1, target_degree2, gearHandle1, gearHandle2): RAD = 180 / math.pi lastCmdTime = vrep.simxGetLastCmdTime(clientID) vrep.simxSynchronousTrigger(clientID) count = 0 while vrep.simxGetConnectionId(clientID) != -1: count += 1 if count > 20: break currCmdTime = vrep.simxGetLastCmdTime(clientID) dt = currCmdTime - lastCmdTime ret, gear_pos1 = vrep.simxGetJointPosition(clientID, gearHandle1, vrep.simx_opmode_buffer) ret, gear_pos2 = vrep.simxGetJointPosition(clientID, gearHandle2, vrep.simx_opmode_buffer) degree1 = round(gear_pos1 * RAD, 2) degree2 = round(gear_pos2 * RAD, 2) print(degree1, degree2) if abs(degree1 - target_degree1) < 0.5 and abs(degree2 - target_degree2) < 0.5: break vrep.simxPauseCommunication(clientID, True) vrep.simxSetJointTargetPosition(clientID, gearHandle1, target_degree1 / RAD, vrep.simx_opmode_oneshot) vrep.simxSetJointTargetPosition(clientID, gearHandle2, target_degree2 / RAD, vrep.simx_opmode_oneshot) vrep.simxPauseCommunication(clientID, False) lastCmdTime = currCmdTime vrep.simxSynchronousTrigger(clientID)
def iterate_inference_aggregation(self, step): end = 0 # Current agents' positions agent_positions = self.get_agent_positions(step) total_discrepancy = 0 # Calculate the discrepancy of every agent and sum them all for agent_ID in range(0, self.__num_agents): total_discrepancy += utils.find_discrepancy_aggregation(agent_ID, agent_positions, self.__desired_distance) discrepancy = total_discrepancy/self.__num_agents # Continue the simulation for the number of steps defined in skip_steps for i in range(0, self.__skip_steps): vrep.simxSynchronousTrigger(self.__client_ID) # Decide whether or not the training should end at the next step if step >= self.__steps_limit: end = 1 reset = 1 elif ((discrepancy > self.__fail_threshold) or (discrepancy < self.__success_threshold)) and discrepancy > 0: end = 1 reset = 1 else: end = 0 reset = 0 return discrepancy, end, reset
def ur5moveto(self, x, y, z): """ Move ur5 to location (x,y,z) """ vrep.simxSynchronousTrigger(self.clientID) # 让仿真走一步 self.targetQuaternion[0] = 0.707 self.targetQuaternion[1] = 0 self.targetQuaternion[2] = 0.707 self.targetQuaternion[3] = 0 #四元数 self.targetPosition[0] = x self.targetPosition[1] = y self.targetPosition[2] = z vrep.simxPauseCommunication(self.clientID, True) #开启仿真 vrep.simxSetIntegerSignal(self.clientID, 'ICECUBE_0', 21, vrep.simx_opmode_oneshot) for i in range(3): vrep.simxSetFloatSignal(self.clientID, 'ICECUBE_' + str(i + 1), self.targetPosition[i], vrep.simx_opmode_oneshot) for i in range(4): vrep.simxSetFloatSignal(self.clientID, 'ICECUBE_' + str(i + 4), self.targetQuaternion[i], vrep.simx_opmode_oneshot) vrep.simxPauseCommunication(self.clientID, False) vrep.simxSynchronousTrigger(self.clientID) # 进行下一步 vrep.simxGetPingTime(self.clientID) # 使得该仿真步走完
def getRange(self): "get motor range" eCode, value = vrep.simxGetFloatSignal(self.robot, "HOMEO_SIGNAL_"+self._wheel+"Wheel"+"_MAX_SPEED",getattr(vrep,self._opMode)) vrep.simxSynchronousTrigger(self.robot) if eCode != 0: raise TransducerException("Cannot get maxSpeed of VREP motor: " + self._wheel+"Wheel") return value
def move_down(self): #3 time-steps _, object_position = vrep.simxGetObjectPosition(self.client_id, \ self.robot.object_handle[0], -1, vrep.simx_opmode_blocking) _, sawyer_target_position = vrep.simxGetObjectPosition(self.client_id, \ self.robot.sawyer_target_handle, -1, vrep.simx_opmode_blocking) move_direction = np.asarray([self.pickup_position[0] - sawyer_target_position[0], \ self.pickup_position[1] - sawyer_target_position[1], object_position[2] + 0.01 \ - sawyer_target_position[2]]) move_magnitude = np.linalg.norm(move_direction) move_step = 0.03 * move_direction / move_magnitude num_move_steps = int(np.floor(move_magnitude / 0.03)) remaining_magnitude = -num_move_steps * 0.03 + move_magnitude remaining_distance = remaining_magnitude * move_direction / move_magnitude for step_iter in range(num_move_steps): vrep.simxSetObjectPosition(self.client_id, self.robot.sawyer_target_handle,\ -1, (sawyer_target_position[0] + move_step[0], sawyer_target_position[1] \ + move_step[1], sawyer_target_position[2] + move_step[2]), \ vrep.simx_opmode_blocking) _, sawyer_target_position = vrep.simxGetObjectPosition(self.client_id,\ self.robot.sawyer_target_handle, -1, vrep.simx_opmode_blocking) vrep.simxSynchronousTrigger(self.client_id) vrep.simxGetPingTime(self.client_id) vrep.simxSetObjectPosition(self.robot.client_id, self.robot.sawyer_target_handle, \ -1, (sawyer_target_position[0] + remaining_distance[0], sawyer_target_position[1] \ + remaining_distance[1], sawyer_target_position[2]+ remaining_distance[2]), \ vrep.simx_opmode_blocking) vrep.simxSynchronousTrigger(self.client_id) vrep.simxGetPingTime(self.client_id)
def control_step( self ): self.count += 1 if self.count == 10: self.get_target() self.calculate_error() self.state = np.matrix([[self.pos_err[0]], [self.pos_err[1]], [self.pos_err[2]], [self.lin[0]], [self.lin[1]], [self.lin[2]], [self.ori_err[0]], [self.ori_err[1]], [self.ori_err[2]], [self.ang[0]], [self.ang[1]], [self.ang[2]], ]) self.update_integral() self.count = 0 self.send_motor_commands( self.compute_output() ) vrep.simxSynchronousTrigger( self.cid ) else: self.update_integral()
def three_end_step(Lx, Ly, Lz, n=N): for i in range(1, n + 1): vrep.simxSynchronousTrigger(clientID) for j in range(1, 6, 2): vrep.simxSetObjectPosition(clientID, Tip_target[j], BCS, [Lx[j][i], Ly[j][i], Lz[j][i]], vrep.simx_opmode_oneshot_wait)
def lift_arm(self): _, sawyer_target_position = vrep.simxGetObjectPosition(self.client_id, \ self.sawyer_target_handle, -1, vrep.simx_opmode_blocking) lift_position_x = 1.137 lift_position_y = 1.2151 lift_position_z = 0.18 self.lift_position = np.array( [lift_position_x, lift_position_y, lift_position_z]) move_direction = np.asarray([self.lift_position[0] - sawyer_target_position[0], \ self.lift_position[1] - sawyer_target_position[1], self.lift_position[2] - \ sawyer_target_position[2]]) move_magnitude = np.linalg.norm(move_direction) move_step = 0.01 * move_direction / move_magnitude num_move_steps = int(np.floor(move_magnitude / 0.01)) for step_iter in range(num_move_steps): vrep.simxSetObjectPosition(self.client_id, self.sawyer_target_handle, -1, \ (sawyer_target_position[0] + move_step[0], sawyer_target_position[1] + \ move_step[1], sawyer_target_position[2] + move_step[2]), vrep.simx_opmode_blocking) _, sawyer_target_position = vrep.simxGetObjectPosition(self.client_id, \ self.sawyer_target_handle, -1, vrep.simx_opmode_blocking) vrep.simxSynchronousTrigger(self.client_id) vrep.simxGetPingTime(self.client_id) _ = vrep.simxSetObjectPosition(self.client_id, self.sawyer_target_handle, -1, \ self.lift_position, vrep.simx_opmode_blocking) vrep.simxSynchronousTrigger(self.client_id) vrep.simxGetPingTime(self.client_id)
def set_back(self): vrep.simxSetObjectPosition(self.client_id, self.object_handle[0], -1, \ self.object_position, vrep.simx_opmode_blocking) vrep.simxSetObjectOrientation(self.client_id, self.object_handle[0], \ self.robot_handle, self.object_orientation, vrep.simx_opmode_blocking) vrep.simxSynchronousTrigger(self.client_id) vrep.simxGetPingTime(self.client_id)
def close_hand(self): vrep.simxSetJointForce(self.client_id, self.motor_handle, 100, \ vrep.simx_opmode_blocking) vrep.simxSetJointTargetVelocity(self.client_id, self.motor_handle, \ 0.5, vrep.simx_opmode_blocking) vrep.simxSynchronousTrigger(self.client_id) vrep.simxGetPingTime(self.client_id)
def test_fk(): print("Testing FK") q = [np.pi / 4., np.pi / 2., -np.pi / 4., -np.pi / 2., -np.pi / 6., np.pi] src = transform(np.zeros((6, )), from_frame=EE_FRAME, to_frame=WORLD_FRAME) dst = transform(q, from_frame=EE_FRAME, to_frame=WORLD_FRAME) src_euler = np.asarray(tf.euler_from_quaternion(src[1], axes='szyx'), dtype=np.float32) dst_euler = np.asarray(tf.euler_from_quaternion(dst[1], axes="szyx"), dtype=np.float32) print("Source position: " + str(src[0]) + "; source orientation: " + str(src_euler * 180. / np.pi)) print("Destination position: " + str(dst[0]) + "; destination orientation: " + str(dst_euler * 180. / np.pi)) clientID, joints = vrep_connect_and_get_handles() for joint, _q in zip(joints, q): vrep.simxSetJointPosition(clientID, joint, _q, vrep.simx_opmode_blocking) vrep.simxSynchronousTrigger(clientID)
def applyAction(self, action, requestRemoteServer=True): # if (requestRemoteServer): if (self._id == RC.CYOUBOT): remoteObjectName = 'youBot' else: remoteObjectName = RC.CSERVER_REMOTE_API_OBJECT_NAME scale = 1 if (self._id == RC.CKUKA_ARM_BARRETT_HAND): scale = 1 elif (self._id == RC.CJACO_ARM_HAND): hand_eigengrasp_amps = action dofs = gb_hand_eigengrasp_interface.toDOF(hand_eigengrasp_amps) action = self.accumulateMove(dofs) # dofs -> jointPoses action = [action[i] * scale for i in range(len(action))] # ----------------------------------------------------------------------- # inputInts = [] inputFloats = action inputStrings = [] inputBuffer = bytearray() print('ApplyAction:', action) res, retInts, retFloats, retStrings, retBuffer = vrep.simxCallScriptFunction(self._clientID, remoteObjectName, \ vrep.sim_scripttype_childscript, \ CSERVER_REMOTE_FUNC_ROBOT_ACT, \ inputInts, inputFloats, inputStrings, inputBuffer, \ vrep.simx_opmode_oneshot_wait) # =================================================================================================================================== # # move simulation ahead one time step vrep.simxSynchronousTrigger(self._clientID) return 1
def two_legs_recovery(n=N): print("Start recovering legs!") init_position = np.zeros((2, 3)) Lx = np.zeros((2, n + 1)) Ly = np.zeros((2, n + 1)) Lz = np.zeros((2, n + 1)) res, init_position[0] = vrep.simxGetObjectPosition( clientID, S1[1], BCS, vrep.simx_opmode_oneshot_wait) res, init_position[1] = vrep.simxGetObjectPosition( clientID, S1[4], BCS, vrep.simx_opmode_oneshot_wait) for i in range(n + 1): Lx[0][i] = init_position[0][0] + i * (INIT_POSITION[1][0] - init_position[0][0]) / n Ly[0][i] = init_position[0][1] + i * (INIT_POSITION[1][1] - 0.2 - init_position[0][1]) / n Lz[0][i] = init_position[0][2] + i * (INIT_POSITION[1][2] - init_position[0][2]) / n for i in range(n + 1): Lx[1][i] = init_position[1][0] + i * (INIT_POSITION[4][0] - init_position[1][0]) / n Ly[1][i] = init_position[1][1] + i * (INIT_POSITION[4][1] + 0.2 - init_position[1][1]) / n Lz[1][i] = init_position[1][2] + i * (INIT_POSITION[4][2] - init_position[1][2]) / n for i in range(n): vrep.simxSynchronousTrigger(clientID) vrep.simxSetObjectPosition(clientID, Tip_target[1], BCS, [Lx[0][i], Ly[0][i], Lz[0][i]], vrep.simx_opmode_oneshot_wait) vrep.simxSetObjectPosition(clientID, Tip_target[4], BCS, [Lx[1][i], Ly[1][i], Lz[1][i]], vrep.simx_opmode_oneshot_wait)
def update_sim(self): for handle, velocity in zip(self.joint_handles, self.target_velocities): return_code = vrep.simxSetJointTargetVelocity(self.cid, int(handle), velocity, vrep.simx_opmode_oneshot) check_for_errors(return_code) vrep.simxSynchronousTrigger(self.cid) vrep.simxGetPingTime(self.cid)
def testMaxSpeed(self, maxSpeed, mode): """test max speed of khepera-like robot in V-Rep revving the motors up to maxSpeed in the self.noSteps and then backward. mode--> 1, both motors, 2: right only, 3: left only""" if mode == 1: rightOn = leftOn = 1 elif mode == 2: rightOn = 1 leftOn = 0 elif mode == 3: rightOn = 0 leftOn = 1 unitSpeed = maxSpeed /self.noSteps for i in xrange(self.noSteps): eCode = vrep.simxSetJointTargetVelocity(self.simulID, self.rightMotor, unitSpeed *(i+1)*rightOn, vrep.simx_opmode_oneshot_wait) eCode = vrep.simxSetJointTargetVelocity(self.simulID, self.leftMotor, unitSpeed *(i+1)*leftOn, vrep.simx_opmode_oneshot_wait) vrep.simxSynchronousTrigger(self.simulID) print "Step: %s\t Speed now: %.2f" %(str(i),(unitSpeed *(i+1))) for i in xrange(self.noSteps): eCode = vrep.simxSetJointTargetVelocity(self.simulID, self.rightMotor, -(maxSpeed/(i+1))*rightOn, vrep.simx_opmode_oneshot_wait) eCode = vrep.simxSetJointTargetVelocity(self.simulID, self.leftMotor, -(maxSpeed/(i+1))*leftOn, vrep.simx_opmode_oneshot_wait) vrep.simxSynchronousTrigger(self.simulID) print "Step: %s\t Speed now: %.2f" % (str(i), (maxSpeed/(i+1))*rightOn)
def __call__(self, t, values): self.count += 1 if self.count == int(round(self.sim_dt/self.nengo_dt)): self.count = 0 self.handle_input( values ) if self.sync: vrep.simxSynchronousTrigger( self.cid ) return self.handle_output()
def act(self): '''activates the wheel motor by calling the actuator function (transdFunction) with VREP client (stored in self.robot) and the needed parameters (stored in funcParameters)''' # parametersString = str(self.robot) + ", " + str(self._transducID) + ", " + str(self.funcParameters) + ", " + 'vrep.'+self._opMode eCode = self.transdFunction(self.robot, self._transducID, self.funcParameters, getattr(vrep, self._opMode)) vrep.simxSynchronousTrigger(self.robot) if eCode != 0: stderr.write("Motor command to VREP motor:%sWheel failed " % self._wheel)
def __init__(self, simulatorModel=None): self.robotOrientationFirstTime = True self.getDistanceToGoalFirstTime = True self.getUpDistanceFirstTime = True self.getObjectPositionFirstTime = True self.sysConf = LoadSystemConfiguration() #this data structure is like a cache for the joint handles self.jointHandleMapping = {} robotConf = LoadRobotConfiguration() self.model = simulatorModel self.LucyJoints = robotConf.getJointsName() for joint in self.LucyJoints: self.jointHandleMapping[joint]=0 self.clientId = self.connectVREP() RETRY_ATTEMPTS = 50 if simulatorModel: for i in range(RETRY_ATTEMPTS): #TODO try to reutilize the same scene for the sake of performance error = self.loadscn(self.clientId, simulatorModel) if not error: break print "retrying connection to vrep" if error: raise VrepException("error loading Vrep robot model", -1) if int(self.sysConf.getProperty("synchronous mode?"))==1: self.synchronous = True vrep.simxSynchronousTrigger(self.clientId) else: self.synchronous = False self.speedmodifier = int(self.sysConf.getProperty("speedmodifier")) #setting the simulation time step self.simulationTimeStepDT = float(self.sysConf.getProperty("simulation time step")) '''#testing printing in vrep error, consoleHandler = vrep.simxAuxiliaryConsoleOpen(self.clientId, "lucy debugging", 8, 22, None, None, None, None, vrep.simx_opmode_oneshot_wait) print "console handler: ", consoleHandler vrep.simxAuxiliaryConsoleShow(self.clientId, consoleHandler, 1, vrep.simx_opmode_oneshot_wait) error = vrep.simxAuxiliaryConsolePrint(self.clientId, consoleHandler, "Hello World", vrep.simx_opmode_oneshot_wait)''' error, self.upDistanceHandle = vrep.simxGetDistanceHandle(self.clientId, "upDistance#", vrep.simx_opmode_blocking) error, self.distLFootToGoalHandle = vrep.simxGetDistanceHandle(self.clientId, "distanceLFootGoal#", vrep.simx_opmode_blocking) error, self.distRFootToGoalHandle = vrep.simxGetDistanceHandle(self.clientId, "distanceRFootGoal#", vrep.simx_opmode_blocking) error, self.bioloidHandle = vrep.simxGetObjectHandle(self.clientId,"Bioloid", vrep.simx_opmode_oneshot_wait) error, self.cuboidHandle = vrep.simxGetObjectHandle(self.clientId,"Cuboid", vrep.simx_opmode_oneshot_wait) error, self.armHandler = vrep.simxGetObjectHandle(self.clientId, "Pivot", vrep.simx_opmode_oneshot_wait) self.populateJointHandleCache(self.clientId) self.isRobotUpFirstCall = True self.getDistanceToSceneGoal() #to fix the first invocation self.getUpDistance() self.isRobotUp(self.clientId) self.armPositionXAxis = -9.0000e-02
def reset(self): """In VREP we reset a simulation by stopping and restarting it""" eCode = vrep.simxStopSimulation(self._VREP_clientId, vrep.simx_opmode_oneshot_wait) if eCode != 0: raise Exception("Could not stop VREP simulation") eCode = vrep.simxStartSimulation(self._VREP_clientId, vrep.simx_opmode_oneshot_wait) if eCode != 0: raise Exception("Could not start VREP simulation") vrep.simxSynchronousTrigger(self._VREP_clientId)
def read(self): # sensingParameters = self.robot + ", " + self._VREPSignalName + ", " + 'vrep.'+self._opMode eCode, value = self._transdFunction(self.robot, self._VREPSignalName , getattr(vrep, self._opMode)) vrep.simxSynchronousTrigger(self.robot) if eCode != 0: # raise Exception("Cannot read value for sensor " + self._eye+"Eye") stderr.write("Cannot read value for sensor " + self._eye+"Eye") return 0 return value
def stepsim(self, steps): """ Do n-steps of simulation. :param steps: Number of steps you want to simulate :type steps: int """ for i in xrange(steps): vrep.simxSynchronousTrigger(self._clientID)
def sendSignalVREP(self,signalName, signalValue): if type(signalValue) == str: eCode = vrep.simxSetStringSignal(self._VREP_clientId, signalName, asByteArray(signalValue), vrep.simx_opmode_oneshot_wait) elif type(signalValue) == int: eCode = vrep.simxSetIntegerSignal(self._VREP_clientId, signalName, signalValue, vrep.simx_opmode_oneshot_wait) elif type(signalValue) == float: eCode = vrep.simxSetFloatSignal(self._VREP_clientId, signalName, signalValue, vrep.simx_opmode_oneshot_wait) else: raise Exception("Trying to send a signal of unknown data type. Only strings, floats and and ints are accepted") if eCode != 0: raise Exception("Could not send string signal", signalValue) vrep.simxSynchronousTrigger(self._VREP_clientId)
def setJointPosition(self, clientID, joint, angle): error = False handle = self.jointHandleMapping[joint] if handle == 0: errorGetObjetHandle, handle=vrep.simxGetObjectHandle(clientID,joint,vrep.simx_opmode_oneshot_wait) error = errorGetObjetHandle or error self.jointHandleMapping[joint]=handle #error = error or vrep.simxSetJointPosition(clientID,handle,angle,vrep.simx_opmode_oneshot_wait) error = error or vrep.simxSetJointPosition(clientID,handle,angle,vrep.simx_opmode_oneshot) #using oneshot instead of oneshot_wait make it a nonblocking call but is the only way to avoid 5 seconds wait in the execution of the individual if self.synchronous: vrep.simxSynchronousTrigger(clientID) return error
def __call__( self, t, values ): """ This class will be callable within a nengo node. It will accept as input the control signals for each rotor, and will output the relevant state variables (position, velocity, orientation, angular velocity). """ self.count += 1 if self.count == int(round(sim_dt/dt)): self.count = 0 self.handle_input( values ) if SYNC: vrep.simxSynchronousTrigger( self.cid ) return self.handle_output()
def moveKJuniorReadProxSensors(self): "slowly move forward and print normal vector readings" rightInput = vrep.simxReadProximitySensor(self.simulID, self.KJrightEye, vrep.simx_opmode_streaming) leftInput = vrep.simxReadProximitySensor(self.simulID, self.KJleftEye, vrep.simx_opmode_streaming) vrep.simxSynchronousTrigger(self.simulID) print "Proximity sensor readings error codes: ", rightInput[0],leftInput[0] for step in xrange(self.noSteps): rightSpeed = 10 leftSpeed = rightSpeed eCode = vrep.simxSetJointTargetVelocity(self.simulID, self.KJrightMotor, rightSpeed, vrep.simx_opmode_oneshot_wait) eCode = vrep.simxSetJointTargetVelocity(self.simulID, self.KJleftMotor, leftSpeed, vrep.simx_opmode_oneshot_wait) vrep.simxSynchronousTrigger(self.simulID) rightInput = vrep.simxReadProximitySensor(self.simulID, self.KJrightEye, vrep.simx_opmode_buffer) vrep.simxSynchronousTrigger(self.simulID) leftInput = vrep.simxReadProximitySensor(self.simulID, self.KJleftEye, vrep.simx_opmode_buffer) vrep.simxSynchronousTrigger(self.simulID) print "Left-->err:%s - Detct'd: %s\t%s\t\t\tRight--> err:%s - Detct'd: %s\t%s" % (leftInput[0], leftInput[3], leftInput[4], rightInput[0], rightInput[3], rightInput[4]) sleep(.2)
def VREPConnect(self): vrep.simxFinish(-1) "Connect to Simulation" self.simulID = vrep.simxStart(self.robot_host,self.simulation_port,True,True, 5000,5) eCode = vrep.simxSynchronous(self.simulID, True) if eCode != 0: print "Could not get V-REP to synchronize operation with me" if not self.simulID == -1: eCode = vrep.simxStartSimulation(self.simulID, vrep.simx_opmode_oneshot) vrep.simxSynchronousTrigger(self.simulID) print "my SimulID is ", self.simulID else: sys.exit("Failed to connect to VREP simulation. Bailing out")
def moveAndReadProxSensors(self): "rotate in place and print sensor distance and normal vector readings" for step in xrange(self.noSteps): if step>self.noSteps / 2: rightSpeed = -1 leftSpeed = -rightSpeed else: rightSpeed = 1 leftSpeed = -rightSpeed eCode = vrep.simxSetJointTargetVelocity(self.simulID, self.rightMotor, rightSpeed, vrep.simx_opmode_oneshot_wait) eCode = vrep.simxSetJointTargetVelocity(self.simulID, self.leftMotor, leftSpeed, vrep.simx_opmode_oneshot_wait) vrep.simxSynchronousTrigger(self.simulID) rightInput = vrep.simxReadProximitySensor(self.simulID, self.rightEye, vrep.simx_opmode_oneshot_wait) vrep.simxSynchronousTrigger(self.simulID) leftInput = vrep.simxReadProximitySensor(self.simulID, self.leftEye, vrep.simx_opmode_oneshot_wait) vrep.simxSynchronousTrigger(self.simulID) print "Left-->err:%s - Detct'd: %s\t%s\t\tRight--> err:%s - Detct'd: %s\t\t\t%s" % (leftInput[0], leftInput[3], leftInput[2], rightInput[0], rightInput[3], rightInput[2]) sleep(.1) self.stopRobot(self.simulID,[self.rightMotor,self.leftMotor]) vrep.simxSynchronousTrigger(self.simulID)
def braiten1b(self): "slowly move forward and print normal vector readings" intens = 100 ambientIntensRatio = 0.2 attVect = [0,0,pi *4] for step in xrange(self.noSteps): rightInput = vrep.simxReadProximitySensor(self.simulID, self.rightEye, vrep.simx_opmode_oneshot_wait) vrep.simxSynchronousTrigger(self.simulID) leftInput = vrep.simxReadProximitySensor(self.simulID, self.leftEye, vrep.simx_opmode_oneshot_wait) vrep.simxSynchronousTrigger(self.simulID) centerInput = vrep.simxReadProximitySensor(self.simulID, self.KJcenterEye, vrep.simx_opmode_oneshot_wait) vrep.simxSynchronousTrigger(self.simulID) angle = degrees(self.angleBetVecs([0,0,1], centerInput[2])) lightReading = self.irradAtSensor(intens, ambientIntensRatio, centerInput[2], attVect) print "Center-->err:%s - Detct'd: %s\tAngle:%.3f\tIrrad:%.3f\tNorm: %.3f\tVector:%s\t" % (centerInput[0], centerInput[3], angle, lightReading, norm(centerInput[2]), centerInput[2]) eCode = vrep.simxSetJointTargetVelocity(self.simulID, self.rightMotor, 1/lightReading, vrep.simx_opmode_oneshot_wait) eCode = vrep.simxSetJointTargetVelocity(self.simulID, self.leftMotor, 1/lightReading, vrep.simx_opmode_oneshot_wait) vrep.simxSynchronousTrigger(self.simulID) sleep(0)
def connect(self): #os.chdir(VREP_HOME) #subprocess.call([os.path.join(VREP_HOME,'vrep.sh'), VREP_scene_file], shell = True, cwd = VREP_HOME) "Close existing connections" vrep.simxFinish(-1) "Connect to Simulation" self.simulID = vrep.simxStart(self.robot_host,self.simulation_port,True,True, 5000,5) eCode = vrep.simxSynchronous(self.simulID, True) if eCode != 0: print "Could not get V-REP to synchronize operation with me" if not self.simulID == -1: eCode = vrep.simxStartSimulation(self.simulID, vrep.simx_opmode_oneshot) vrep.simxSynchronousTrigger(self.simulID) print "my SimulID is ", self.simulID else: sys.exit("Failed to connect to VREP simulation. Bailing out")
def __init__(self, wheel, clientID): "Wheel could either 'right or 'left' and nothing else" if wheel not in ["right","left"]: raise TransducerException("Wheel must either be right or left") self._wheel = wheel self.robot = clientID self._transdFunction = vrep.simxSetJointTargetVelocity self._opMode = 'simx_opmode_oneshot_wait' try: eCode, self._transducID = vrep.simxGetObjectHandle(self.robot, (self._wheel+"Wheel"), getattr(vrep,self._opMode)) vrep.simxSynchronousTrigger(self.robot) except Exception as e: print "Cannot connect to VREP!" print "Error: ",e if eCode != 0: raise TransducerException("Cannot connect to VREP motor: " + self._wheel+"Wheel") self._range = self.getRange()
def moveKJuniorReadProxSensors(self): "slowly move forward and print normal vector readings" intens = 100 ambientIntens = 0 attVect = [0,0,pi *4] rightInput = vrep.simxReadProximitySensor(self.simulID, self.KJrightEye, vrep.simx_opmode_streaming) leftInput = vrep.simxReadProximitySensor(self.simulID, self.KJleftEye, vrep.simx_opmode_streaming) centerInput = vrep.simxReadProximitySensor(self.simulID, self.KJcenterEye, vrep.simx_opmode_streaming) vrep.simxSynchronousTrigger(self.simulID) print "Proximity sensor readings error codes: ", rightInput[0],leftInput[0] for step in xrange(self.noSteps): rightSpeed = 10 leftSpeed = rightSpeed eCode = vrep.simxSetJointTargetVelocity(self.simulID, self.KJrightMotor, rightSpeed, vrep.simx_opmode_oneshot_wait) eCode = vrep.simxSetJointTargetVelocity(self.simulID, self.KJleftMotor, leftSpeed, vrep.simx_opmode_oneshot_wait) vrep.simxSynchronousTrigger(self.simulID) rightInput = vrep.simxReadProximitySensor(self.simulID, self.KJrightEye, vrep.simx_opmode_buffer) vrep.simxSynchronousTrigger(self.simulID) leftInput = vrep.simxReadProximitySensor(self.simulID, self.KJleftEye, vrep.simx_opmode_buffer) vrep.simxSynchronousTrigger(self.simulID) centerInput = vrep.simxReadProximitySensor(self.simulID, self.KJcenterEye, vrep.simx_opmode_buffer) vrep.simxSynchronousTrigger(self.simulID) #=================================================================== # print "Left-->err:%s - Detct'd: %s\t%s\t\t\tRight--> err:%s - Detct'd: %s\t%s" % (leftInput[0], # leftInput[3], # leftInput[4], # rightInput[0], # rightInput[3], # rightInput[4]) #=================================================================== angle = degrees(self.angleBetVecs([0,0,1], centerInput[2])) lightReading = self.irradAtSensor(intens, ambientIntens, centerInput[2], attVect) print "Center-->err:%s - Detct'd: %s\tAngle:%.3f\tIrrad:%.3f\tNorm: %.3f\tVector:%s\t" % (centerInput[0], centerInput[3], angle, lightReading, norm(centerInput[2]), centerInput[2]) sleep(0)
def start(self): returnCode = vrep.simxStartSimulation(self.clientID,vrep.simx_opmode_oneshot) if (returnCode>1): print "returnCode: ", returnCode raise Exception('Could not start') returnCode=vrep.simxSynchronous(self.clientID,True) for k in range(10): #Run to steps to step through initial "drop" returncode = vrep.simxSynchronousTrigger(self.clientID) if (returnCode!=0): raise Exception('Could not set synchronous mode')
def braiten2a(self): "Seek light source" intens = 50 ambientIntensRatio = .2 attVect = [0,0,1] rightInput = vrep.simxReadProximitySensor(self.simulID, self.KJrightEye, vrep.simx_opmode_streaming) leftInput = vrep.simxReadProximitySensor(self.simulID, self.KJleftEye, vrep.simx_opmode_streaming) centerInput = vrep.simxReadProximitySensor(self.simulID, self.KJcenterEye, vrep.simx_opmode_streaming) vrep.simxSynchronousTrigger(self.simulID) print "Proximity sensor readings error codes: ", rightInput[0],leftInput[0] for step in xrange(self.noSteps): rightInput = vrep.simxReadProximitySensor(self.simulID, self.KJrightEye, vrep.simx_opmode_buffer) vrep.simxSynchronousTrigger(self.simulID) leftInput = vrep.simxReadProximitySensor(self.simulID, self.KJleftEye, vrep.simx_opmode_buffer) vrep.simxSynchronousTrigger(self.simulID) centerInput = vrep.simxReadProximitySensor(self.simulID, self.KJcenterEye, vrep.simx_opmode_buffer) vrep.simxSynchronousTrigger(self.simulID) #=================================================================== # print "Left-->err:%s - Detct'd: %s\t%s\t\t\tRight--> err:%s - Detct'd: %s\t%s" % (leftInput[0], # leftInput[3], # leftInput[4], # rightInput[0], # rightInput[3], # rightInput[4]) #=================================================================== angle = degrees(self.angleBetVecs([0,0,1], centerInput[2])) lightReading = self.irradAtSensor(intens, ambientIntens, centerInput[2], attVect) print "Center-->err:%s - Detct'd: %s\tAngle:%.3f\tIrrad:%.3f\tNorm: %.3f\tVector:%s\t" % (centerInput[0], centerInput[3], angle, lightReading, norm(centerInput[2]), centerInput[2]) eCode = vrep.simxSetJointTargetVelocity(self.simulID, self.KJrightMotor, lightReading, vrep.simx_opmode_oneshot_wait) eCode = vrep.simxSetJointTargetVelocity(self.simulID, self.KJleftMotor, lightReading, vrep.simx_opmode_oneshot_wait) vrep.simxSynchronousTrigger(self.simulID) sleep(0)
def applyAction(self, action): if self.state[0]<self.boundaries[0][0] or self.state[0]>self.boundaries[0][1] or self.state[1]<self.boundaries[1][0] or self.state[1]>self.boundaries[1][1] or (abs(self.target[0]-self.state[0])+abs(self.target[1]-self.state[1]))<self.wininngRadius: reward = self.calculateRewardSingle() if self.verbose: print "Terminal state: ", self.state, " - Reward: ", reward self.state = self.initState self.stop() time.sleep(0.1) #100ms delay between stopping and starting to avoid problems self.start() return None,reward if (self.actionStrategy == 'Absolute'): self.robot.applyActionAbsolute(action) elif (self.actionStrategy == 'Differential'): self.robot.applyActionIncremental(action) else: print "Not valid action strategy" return returncode = vrep.simxSynchronousTrigger(self.clientID) newState = self.getState() if (self.rewardStrategy == 'Differential'): reward = self.calculateRewardPair(newState) elif (self.rewardStrategy == 'Absolute'): reward = self.calculateRewardSingle() else: print "Not valid reward strategy" return if self.verbose: print "From state: ", self.state, " applied action: ", action, " got reward: ", reward self.state = newState return tuple(newState),reward
def moveAndReadLights(self): "rotate in place and print light readings" eCode, res, rightEyeRead = vrep.simxGetVisionSensorImage(self.simulID, self.rightEye, 0, vrep.simx_opmode_streaming) ecode, res, leftEyeRead = vrep.simxGetVisionSensorImage(self.simulID, self.leftEye, 0, vrep.simx_opmode_streaming) vrep.simxSynchronousTrigger(self.simulID) for step in xrange(self.noSteps): rightSpeed = 25 leftSpeed = rightSpeed eCode = vrep.simxSetJointTargetVelocity(self.simulID, self.rightMotor, rightSpeed, vrep.simx_opmode_oneshot_wait) eCode = vrep.simxSetJointTargetVelocity(self.simulID, self.leftMotor, leftSpeed, vrep.simx_opmode_oneshot_wait) vrep.simxSynchronousTrigger(self.simulID) eCodeR, res, rightEyeRead = vrep.simxGetVisionSensorImage(self.simulID, self.rightEye, 0, vrep.simx_opmode_buffer) eCodeL, res, leftEyeRead = vrep.simxGetVisionSensorImage(self.simulID, self.leftEye, 0, vrep.simx_opmode_buffer) vrep.simxSynchronousTrigger(self.simulID) # print "Right eCode:\t", eCodeR, # print "Left eCode:\t", eCodeL # leftImg = np.array(leftEyeRead, np.uint8) # rightImg.resize(res[0],res[1],3) print "Right:\t%d, %d\tLeft:\t%d, %d"% (len(rightEyeRead),sum(rightEyeRead), len(leftEyeRead),sum(leftEyeRead))
import vrep from time import sleep vrep.simxFinish(-1) simulID = vrep.simxStart('127.0.0.1',19997,True,True, 5000,5) vrep.simxSynchronous(simulID, True) for i in xrange(5): print "starting no : ", i print "now starting" vrep.simxStartSimulation(simulID, vrep.simx_opmode_oneshot_wait) vrep.simxSynchronousTrigger(simulID) sleep(4) print "now stopping" vrep.simxStopSimulation(simulID, vrep.simx_opmode_oneshot_wait) vrep.simxSynchronousTrigger(simulID) sleep(2) print "done with run no: ", i
import sys print ('Program started') vrep.simxFinish(-1) # just in case, close all opened connections clientID=vrep.simxStart('127.0.0.1',19997,True,True,5000,5) # Connect to V-REP if clientID!=-1: print ('Connected to remote API server') # enable the synchronous mode on the client: vrep.simxSynchronous(clientID,True) # start the simulation: vrep.simxStartSimulation(clientID,vrep.simx_opmode_oneshot_wait) # Now step a few times: for i in range(1,10): if sys.version_info[0] == 3: input('Press <enter> key to step the simulation!') else: raw_input('Press <enter> key to step the simulation!') vrep.simxSynchronousTrigger(clientID); # stop the simulation: vrep.simxStopSimulation(clientID,vrep.simx_opmode_oneshot_wait) # Now close the connection to V-REP: vrep.simxFinish(clientID) else: print ('Failed connecting to remote API server') print ('Program ended')
def _compute(self): if self.synchronous: self.returnSynchro=vrep.simxSynchronousTrigger(self.clientID) self._data=self
def finish_iteration(self): vrep.simxSynchronousTrigger(self.clientID)
vrep.simxSynchronous(client_id, True) position_history = [] e, body_pos = vrep.simxGetObjectPosition(client_id, body, -1, vrep.simx_opmode_buffer) position_history.append(body_pos) joint_pos_history = [] e, joint_0_pos = vrep.simxGetJointPosition(client_id, joint_0, vrep.simx_opmode_streaming) e, joint_1_pos = vrep.simxGetJointPosition(client_id, joint_1, vrep.simx_opmode_streaming) joint_pos_history.append([joint_0_pos, joint_1_pos]) with contexttimer.Timer() as timer: for i in range(4): e = vrep.simxSetJointTargetPosition(client_id, joint_0, 0.5, vrep.simx_opmode_streaming) for t in range(3): vrep.simxSynchronousTrigger(client_id) e, body_pos = vrep.simxGetObjectPosition(client_id, body, -1, vrep.simx_opmode_buffer) position_history.append(body_pos) e, joint_0_pos = vrep.simxGetJointPosition(client_id, joint_0, vrep.simx_opmode_buffer) e, joint_1_pos = vrep.simxGetJointPosition(client_id, joint_1, vrep.simx_opmode_buffer) joint_pos_history.append([joint_0_pos, joint_1_pos]) e = vrep.simxSetJointTargetPosition(client_id, joint_1, 2.5, vrep.simx_opmode_streaming) for t in range(10): vrep.simxSynchronousTrigger(client_id) e, body_pos = vrep.simxGetObjectPosition(client_id, body, -1, vrep.simx_opmode_buffer) position_history.append(body_pos) e, joint_0_pos = vrep.simxGetJointPosition(client_id, joint_0, vrep.simx_opmode_buffer) e, joint_1_pos = vrep.simxGetJointPosition(client_id, joint_1, vrep.simx_opmode_buffer) joint_pos_history.append([joint_0_pos, joint_1_pos])