def publish(self): """ send msg to vrep msg : the frequency of leg """ vrep.simxSetFloatSignal(self.clientID, self.LSignalName, self.LCycleFreq, vrep.simx_opmode_oneshot) vrep.simxSetFloatSignal(self.clientID, self.RSignalName, self.RCycleFreq, vrep.simx_opmode_oneshot)
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 setParams(self, params): for key, value in params.items(): vrep.simxSetFloatSignal(self.clientID, key, value, vrep.simx_opmode_oneshot) vrep.simxSetIntegerSignal(self.clientID, 'clear', 1, vrep.simx_opmode_oneshot) vrep.simxSetIntegerSignal(self.clientID, 'stop', 1, vrep.simx_opmode_oneshot)
def init_rotors(clientID): # Clear all signals for i in range(len(propellers)): vrep.simxClearFloatSignal(clientID, propellers[i], vrep.simx_opmode_oneshot) # Set all propellers to zero for i in range(len(propellers)): vrep.simxSetFloatSignal(clientID, propellers[i], 1e-8, vrep.simx_opmode_oneshot)
def _write_actuators(self): """ Write in the robot the actuators values. Don't use directly, instead use 'step()' """ # We make a copy of the actuators list actuators = self._actuators_to_write[:] for m in actuators: if m[0] == 'L': # Leds # Sent as packed string to ensure update of the wanted LED test = vrep.simxPackInts([m[1] + 1, m[2]]) vrep.simxSetStringSignal(self._clientID, 'EPUCK_LED', test, vrep.simx_opmode_oneshot_wait) elif m[0] == 'D': # Set motor speed # maxVel = 120.0 * math.pi / 180.0 # maxVel of ePuck firmware is 1000 # => 120 * pi / (180*1000) = pi/1500 velLeft = m[1] * math.pi / 1500.0 velRight = m[2] * math.pi / 1500.0 if velLeft > 120.0 * math.pi / 180.0: velLeft = 120.0 * math.pi / 180.0 self._debug("velLeft too high, thresholded") if velLeft < -120.0 * math.pi / 180.0: velLeft = -120.0 * math.pi / 180.0 self._debug("velLeft too high, thresholded") if velRight > 120.0 * math.pi / 180.0: velRight = 120.0 * math.pi / 180.0 self._debug("velRight too high, thresholded") if velRight < -120.0 * math.pi / 180.0: velRight = -120.0 * math.pi / 180.0 self._debug("velRRight too high, thresholded") vrep.simxSetFloatSignal(self._clientID, 'EPUCK_velLeft', velLeft, vrep.simx_opmode_oneshot) vrep.simxSetFloatSignal(self._clientID, 'EPUCK_velRight', velRight, vrep.simx_opmode_oneshot) elif m[0] == 'P': # Motor position self._debug('WARNING: Motor position not yet implemented!') else: self._debug('Unknown actuator to write') self._actuators_to_write.remove(m) return
def conduct_action(self, a): self.action = a / 1000.0 if vrep.simxGetConnectionId(self.client_id) == -1: self.connection() #print("action2", a) lasttime = time.time() while True: for i in range(3): vrep.simxSetFloatSignal(self.client_id, "myTestValue" + str(i), self.action[i], vrep.simx_opmode_oneshot) currtime = time.time() if currtime - lasttime > 0.1: break
def connection(self): #关闭潜在的连接 vrep.simxFinish(-1) #每隔0.2秒检测一次,直到连接上V-rep while True: self.client_id = vrep.simxStart(self.client_ip, 19999, True, True, 5000, 5) if self.client_id > -1: vrep.simxSetFloatSignal(self.client_id, "ConnectFlag", 19999, vrep.simx_opmode_oneshot) break else: time.sleep(0.2) print('Failed connecting to remote API server') print('Connection success!')
def controller(self): self.LCycleFreq = self.BaseFreq self.RCycleFreq = self.BaseFreq error = vrep.simxSetFloatSignal(self.clientID, self.LSignalName, self.LCycleFreq, vrep.simx_opmode_oneshot) error = error or vrep.simxSetFloatSignal( self.clientID, self.RSignalName, self.RCycleFreq, vrep.simx_opmode_oneshot) if error != 0: print("Function error: ", error) time = vrep.simxGetLastCmdTime(clientID) self.telemetryData['cycleFrequencyTime'] = time self.telemetryData['leftCycleFrequency'] = self.LCycleFreq self.telemetryData['rightCycleFrequency'] = self.RCycleFreq
def setJointVelocity(self, jointIndex, vel, isHandJoint=False, useSignalToJoint=0): if (isHandJoint): jointSignalPrefixes = GB_CROBOT_HAND_JOINT_SIGNAL_NAME_PREFIXES else: jointSignalPrefixes = GB_CROBOT_JOINT_SIGNAL_NAME_PREFIXES # http://www.coppeliarobotics.com/helpFiles/en/jointDescription.htm # Signal to the joint control callback script if (useSignalToJoint): jointSignalName = jointSignalPrefixes[jointIndex] + "TargetVel" res = vrep.simxSetFloatSignal( self._clientID, jointSignalName, vel, vrep.simx_opmode_oneshot_wait ) # set the signal value: !!simx_opmode_oneshot_wait else: res = vrep.simxSetJointTargetVelocity( self._clientID, self._jointHandles[jointIndex], vel, # target velocity vrep.simx_opmode_blocking) if (res != 0): print('Set Joint Velocity Error', res) return res
def takeAction(self,doubleAction): """ state = (basePos[0], basePos[1], basePos[2], linVel[3], linVel[4], linVel[5], angVel[6], angVel[7], angVel[8], baseYaw[9], baseRoll[10], basePitch[11]) """ # self.prevState = self.state #print("oldState: "+str(self.state)) self.num_sim_steps += 1 action = np.asarray(doubleAction,dtype=np.float32) #print("takeAction: "+str(doubleAction)) # Get altitude directly from position Z altiMeters = self.state[2] # Get motor thrusts from FMU model thrusts = self.fmu.getMotors((self.state[11],self.state[10],self.state[9]),altiMeters, action, self.stepSeconds) # print("thrusts: " + str(thrusts[0]) + " " + str(thrusts[1]) + " " + \ # str(thrusts[2]) + " " + str(thrusts[3])) #vrep.simxPauseCommunication(self.clientID,True) for t in range(4): errorFlag = vrep.simxSetFloatSignal(self.clientID,'thrusts'+str(t+1), thrusts[t], vrep.simx_opmode_oneshot) #vrep.simxPauseCommunication(self.clientID,False) self.proceed_simulation()
def main(): print ('Program started') emptybuf = bytearray() vrep.simxFinish(-1) # just in case, close all opened connections clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 2000, 5) if clientID!=-1: print ('Connected to remote API server') # Start the simulation: vrep.simxStartSimulation(clientID,vrep.simx_opmode_oneshot_wait) # start init wheels -------------------------------------------------------------------------------------------- wheelJoints = np.empty(4, dtype=np.int) wheelJoints.fill(-1) # front left, rear left, rear right, front right res, wheelJoints[0] = vrep.simxGetObjectHandle(clientID, 'rollingJoint_fl', vrep.simx_opmode_oneshot_wait) res, wheelJoints[1] = vrep.simxGetObjectHandle(clientID, 'rollingJoint_rl', vrep.simx_opmode_oneshot_wait) res, wheelJoints[2] = vrep.simxGetObjectHandle(clientID, 'rollingJoint_fr', vrep.simx_opmode_oneshot_wait) res, wheelJoints[3] = vrep.simxGetObjectHandle(clientID, 'rollingJoint_rr', vrep.simx_opmode_oneshot_wait) # end init wheels ---------------------------------------------------------------------------------------------- # start init camera -------------------------------------------------------------------------------------------- # change the angle of the camera view (default is pi/4) res = vrep.simxSetFloatSignal(clientID, 'rgbd_sensor_scan_angle', math.pi / 2, vrep.simx_opmode_oneshot_wait) # turn on camera res = vrep.simxSetIntegerSignal(clientID, 'handle_rgb_sensor', 2, vrep.simx_opmode_oneshot_wait); # get camera object-handle res, youBotCam = vrep.simxGetObjectHandle(clientID, 'rgbSensor', vrep.simx_opmode_oneshot_wait) # get first image err, resolution, image = vrep.simxGetVisionSensorImage(clientID, youBotCam, 0, vrep.simx_opmode_streaming) time.sleep(1) # end init camera ---------------------------------------------------------------------------------------------- # programmable space ------------------------------------------------------------------------------------------- colorDet.exercise4_action(clientID, youBotCam) # end of programmable space -------------------------------------------------------------------------------------------- # Stop simulation vrep.simxStopSimulation(clientID,vrep.simx_opmode_oneshot_wait) # Close connection to V-REP: vrep.simxFinish(clientID) else: print ('Failed connecting to remote API server') print ('Program ended')
def setJointForce(self, jointIndex, force, isHandJoint=False, useSignalToJoint=0): if (isHandJoint): jointSignalPrefixes = GB_CROBOT_HAND_JOINT_SIGNAL_NAME_PREFIXES else: jointSignalPrefixes = GB_CROBOT_JOINT_SIGNAL_NAME_PREFIXES # Signal to the joint control callback script jointSignalName = jointSignalPrefixes[jointIndex] + "Force" if (useSignalToJoint): res = vrep.simxSetFloatSignal( self._clientID, jointSignalName, force, vrep.simx_opmode_oneshot_wait ) # set the signal value: !!simx_opmode_oneshot_wait else: res = vrep.simxSetJointForce( self._clientID, self._jointHandles[jointIndex], force, # force to apply vrep.simx_opmode_blocking) if (res != 0): print('Set Joint Force Error', res) return res
def takeAction(self, doubleAction): """ state = (basePos[0], basePos[1], basePos[2], linVel[3], linVel[4], linVel[5], angVel[6], angVel[7], angVel[8], baseYaw[9], baseRoll[10], basePitch[11]) """ # self.prevState = self.state #print("oldState: "+str(self.state)) self.num_sim_steps += 1 action = np.asarray(doubleAction, dtype=np.float32) #print("takeAction: "+str(doubleAction)) # Get altitude directly from position Z altiMeters = self.state[2] # Get motor thrusts from FMU model thrusts = self.fmu.getMotors( (self.state[11], self.state[10], self.state[9]), altiMeters, action, self.stepSeconds) # print("thrusts: " + str(thrusts[0]) + " " + str(thrusts[1]) + " " + \ # str(thrusts[2]) + " " + str(thrusts[3])) #vrep.simxPauseCommunication(self.clientID,True) for t in range(4): errorFlag = vrep.simxSetFloatSignal(self.clientID, 'thrusts' + str(t + 1), thrusts[t], vrep.simx_opmode_oneshot) #vrep.simxPauseCommunication(self.clientID,False) self.proceed_simulation()
def set_parameter(self, sensor_name, parameter_name, parameter_value): """Set sensor parameters. Sensor name can be fastHokuyo_sensor1, kinect_depth, kinect_rgb. To get list of possible params call youbot.parameters_list""" if parameter_name == 'perspective_angle': parameter_value = parameter_value / (180 * 2) * math.pi if parameter_name in self.params_f: error = vrep.simxSetObjectFloatParameter( self.client_id, self.handles[sensor_name + self.postfix], self.params_f[parameter_name], parameter_value, ONE_SHOT_MODE ) vrep.simxSetFloatSignal( self.client_id, 'change_params', parameter_value, ONE_SHOT_MODE ) vrep.simxClearFloatSignal( self.client_id, 'change_params', ONE_SHOT_MODE ) return error elif parameter_name in self.params_i: error = vrep.simxSetObjectFloatParameter( self.client_id, self.handles[sensor_name + self.postfix], self.params_i[parameter_name], parameter_value, ONE_SHOT_MODE ) vrep.simxSetFloatSignal( self.client_id, 'change_params', parameter_value, ONE_SHOT_MODE ) vrep.simxClearFloatSignal( self.client_id, 'change_params', ONE_SHOT_MODE ) return error else: return 'Parameter not found'
def _write_actuators(self): """ Write in the robot the actuators values. Don't use directly, instead use 'step()' """ # We make a copy of the actuators list actuators = self._actuators_to_write[:] for m in actuators: if m[0] == 'L': # Leds # Sent as packed string to ensure update of the wanted LED test = vrep.simxPackInts([m[1]+1, m[2]]) vrep.simxSetStringSignal(self._clientID, 'EPUCK_LED', test, vrep.simx_opmode_oneshot_wait) elif m[0] == 'D': # Set motor speed # maxVel = 120.0 * math.pi / 180.0 # maxVel of ePuck firmware is 1000 # => 120 * pi / (180*1000) = pi/1500 velLeft = m[1] * math.pi / 1500.0 velRight = m[2] * math.pi / 1500.0 if velLeft > 120.0 * math.pi / 180.0: velLeft = 120.0 * math.pi / 180.0 self._debug("velLeft too high, thresholded") if velLeft < -120.0 * math.pi / 180.0: velLeft = -120.0 * math.pi / 180.0 self._debug("velLeft too high, thresholded") if velRight > 120.0 * math.pi / 180.0: velRight = 120.0 * math.pi / 180.0 self._debug("velRight too high, thresholded") if velRight < -120.0 * math.pi / 180.0: velRight = -120.0 * math.pi / 180.0 self._debug("velRRight too high, thresholded") vrep.simxSetFloatSignal(self._clientID, 'EPUCK_velLeft', velLeft, vrep.simx_opmode_oneshot) vrep.simxSetFloatSignal(self._clientID, 'EPUCK_velRight', velRight, vrep.simx_opmode_oneshot) elif m[0] == 'P': # Motor position self._debug('WARNING: Motor position not yet implemented!') else: self._debug('Unknown actuator to write') self._actuators_to_write.remove(m) return
def move_rotors(clientID, propeller_vels): vrep.simxSetFloatSignal(clientID, propellers[0], propeller_vels[0], vrep.simx_opmode_oneshot) vrep.simxSetFloatSignal(clientID, propellers[1], propeller_vels[1], vrep.simx_opmode_oneshot) vrep.simxSetFloatSignal(clientID, propellers[2], propeller_vels[2], vrep.simx_opmode_oneshot) vrep.simxSetFloatSignal(clientID, propellers[3], propeller_vels[3], vrep.simx_opmode_oneshot)
def vrep_change_properties(client_id, object_id, parameter_id, parameter_value): """Changing properties of sensors in vrep client_id (int): ID of current scene in vrep object_id (int): ID of sensor to change parameter_id (int): ID of parameter to change parameter_value (int/double): value of parameter to change """ params_f = { 'near_clipping_plane': 1000, 'far_clipping_plane': 1001, 'perspective_angle': 1004 } params_i = { 'vision_sensor_resolution_x': 1002, 'vision_sensor_resolution_y': 1003 } if parameter_id == 'perspective_angle': parameter_value = parameter_value / (180 * 2) * math.pi if parameter_id in params_f: error = vrep.simxSetObjectFloatParameter(client_id, object_id, params_f[parameter_id], parameter_value, vrep.simx_opmode_blocking) vrep.simxSetFloatSignal(client_id, 'change_params', parameter_value, vrep.simx_opmode_blocking) vrep.simxClearFloatSignal(client_id, 'change_params', vrep.simx_opmode_blocking) return error elif parameter_id in params_i: error = vrep.simxSetObjectIntParameter(client_id, object_id, params_i[parameter_id], parameter_value, vrep.simx_opmode_blocking) vrep.simxSetFloatSignal(client_id, 'change_params', parameter_value, vrep.simx_opmode_blocking) vrep.simxClearFloatSignal(client_id, 'change_params', vrep.simx_opmode_blocking) return error else: return 'parameter wasn\'t found'
def setRequiredParams(self): # required_params = { # 'pParam':6, # 'iParam':0.04, # 'dParam':0.08, # 'vParam':-2, # 'alphaE':0.3, # 'dAlphaE':1.1, # 1.1 # 'alphaCumul': 0, # 'betaE':0.6, # 'dBetaE':1.1, # 2.5 # 'betaCumul':0.001, # 'sp2':0.15, # 'dsp2':4, # 'sp2Cumul':0.005, # 'sp1':0.08, #0.26 # 'dsp1':1, # 4.2 # 'sp1Cumul':0, # } required_params = { 'pParam': 30, 'iParam': 0, 'dParam': 20, 'vParam': -4, 'alphaE': 0.3, 'dAlphaE': 2, # 1.1 'alphaCumul': 0.01, 'betaE': 0.6, 'dBetaE': 3.5, # 2.5 'betaCumul': 0.02, 'sp2': 0.26, 'dsp2': 4.5, 'sp2Cumul': 0, 'sp1': 0.26, #0.26 'dsp1': 4.5, # 4.2 'sp1Cumul': 0, } # key to edit: betaE, dBetaE, betaZCumul, sp1, dsp1 for key, value in required_params.items(): vrep.simxSetFloatSignal(self.clientID, key, value, vrep.simx_opmode_oneshot)
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 place_all_3_obj(low_thresh, up_thresh): global objHandles objHandles = np.zeros(3) for i in range(0, 3): # Assign new object position (simulation) OP_x = (random.random() - 0.5) * 1.2 OP_y = abs(random.random() - 0.5) * 1.2 while math.sqrt(OP_x**2 + OP_y**2) < low_thresh or math.sqrt( OP_x**2 + OP_y**2) > up_thresh: OP_x = (random.random() - 0.5) * 1.2 OP_y = abs(random.random() - 0.5) * 1.2 vrep.simxSetFloatSignal(clientID, 'ObjPos_x', OP_x, vrep.simx_opmode_oneshot) vrep.simxSetFloatSignal(clientID, 'ObjPos_y', OP_y, vrep.simx_opmode_oneshot) if i == 0: vrep.simxSetStringSignal(clientID, 'Obj_type', 'Cube', vrep.simx_opmode_oneshot) errorCode, objHandles[0] = vrep.simxGetObjectHandle( clientID, 'Cuboid', vrep.simx_opmode_blocking) elif i == 1: vrep.simxSetStringSignal(clientID, 'Obj_type', 'Cylinder', vrep.simx_opmode_oneshot) errorCode, objHandles[1] = vrep.simxGetObjectHandle( clientID, 'Cylinder', vrep.simx_opmode_blocking) elif i == 2: vrep.simxSetStringSignal(clientID, 'Obj_type', 'Sphere', vrep.simx_opmode_oneshot) errorCode, objHandles[2] = vrep.simxGetObjectHandle( clientID, 'Sphere', vrep.simx_opmode_blocking) vrep.simxSetStringSignal(clientID, 'Obj_type', 'All', vrep.simx_opmode_oneshot) # get_20() return objHandles
def place_object(OP_x, OP_y, obj_type): global objHandle vrep.simxSetFloatSignal(clientID, 'ObjPos_x', OP_x, vrep.simx_opmode_oneshot) vrep.simxSetFloatSignal(clientID, 'ObjPos_y', OP_y, vrep.simx_opmode_oneshot) init_param = [] if obj_type == 'Cube': vrep.simxSetStringSignal(clientID, 'Obj_type', 'Cube', vrep.simx_opmode_oneshot) errorCode, objHandle = vrep.simxGetObjectHandle( clientID, 'Cuboid', vrep.simx_opmode_blocking) paramreadfile = open('ParamFileCube.csv', 'rb+') if obj_type == 'Cylinder': vrep.simxSetStringSignal(clientID, 'Obj_type', 'Cylinder', vrep.simx_opmode_oneshot) errorCode, objHandle = vrep.simxGetObjectHandle( clientID, 'Cylinder', vrep.simx_opmode_blocking) paramreadfile = open('ParamFileCyl.csv', 'rb+') if obj_type == 'Sphere': vrep.simxSetStringSignal(clientID, 'Obj_type', 'Sphere', vrep.simx_opmode_oneshot) errorCode, objHandle = vrep.simxGetObjectHandle( clientID, 'Sphere', vrep.simx_opmode_blocking) paramreadfile = open('ParamFileSphere.csv', 'rb+') param_reader = csv.reader(paramreadfile, delimiter=',', quotechar='|') init_param_t = [list(map(float, rec)) for rec in param_reader] if len(init_param_t) > 0: init_param = init_param_t[0] # print(init_param) paramreadfile.close() return [objHandle, init_param]
def set_ang_acc(self,ang_acc): mass_norm_acc = float(ang_acc[0]) roll_ang_acc = float(ang_acc[1]) pitch_ang_acc = float(ang_acc[2]) yaw_ang_acc = float(ang_acc[3]) print(mass_norm_acc) # print(roll_ang_acc) # print(pitch_ang_acc) # print(yaw_ang_acc) vrep.simxSetFloatSignal(self.clientID,'mass_norm_acc', mass_norm_acc,vrep.simx_opmode_oneshot) vrep.simxSetFloatSignal(self.clientID,'roll_ang_acc', roll_ang_acc,vrep.simx_opmode_oneshot) vrep.simxSetFloatSignal(self.clientID,'pitch_ang_acc', pitch_ang_acc,vrep.simx_opmode_oneshot) vrep.simxSetFloatSignal(self.clientID,'yaw_ang_acc', yaw_ang_acc,vrep.simx_opmode_oneshot) return
def update_paramter(d_position): print(d_position) x = d_position[0] y = d_position[1] dirs = np.array([x, -x, y, -y]) d = np.argmax(dirs) print(dirs[d]) res, p1 = vrep.simxGetFloatSignal(clientID, "p1", vrep.simx_opmode_blocking) res, p2 = vrep.simxGetFloatSignal(clientID, "p2", vrep.simx_opmode_blocking) res, p3 = vrep.simxGetFloatSignal(clientID, "p3", vrep.simx_opmode_blocking) res, p4 = vrep.simxGetFloatSignal(clientID, "p4", vrep.simx_opmode_blocking) print(d) if d == 0: p1 += delta p2 -= delta p3 -= delta p4 += delta elif d == 1: p1 -= delta p2 += delta p3 += delta p4 -= delta elif d == 2: p1 += delta p2 += delta p3 -= delta p4 -= delta elif d == 3: p1 -= delta p2 -= delta p3 += delta p4 += delta p = np.array([p1, p2, p3, p4]) [p1, p2, p3, p4] = p print(p1, p2, p3, p4) vrep.simxSetFloatSignal(clientID, "p1", p1, vrep.simx_opmode_blocking) vrep.simxSetFloatSignal(clientID, "p2", p2, vrep.simx_opmode_blocking) vrep.simxSetFloatSignal(clientID, "p3", p3, vrep.simx_opmode_blocking) vrep.simxSetFloatSignal(clientID, "p4", p4, vrep.simx_opmode_blocking)
def envstep(action): # Get old quadrotor state err, pos_old = vrep.simxGetObjectPosition(clientID, quad_handle, -1, vrep.simx_opmode_buffer) err, euler_old = vrep.simxGetObjectOrientation(clientID, quad_handle, -1, vrep.simx_opmode_buffer) # Set propeller thrusts print("Setting propeller thrusts...") propeller_vels = action # Send propeller thrusts print("Sending propeller thrusts...") print(propeller_vels) [vrep.simxSetFloatSignal(clientID, prop, vels, vrep.simx_opmode_oneshot) for prop, vels in zip(propellers, propeller_vels)] # Trigger simulator step print("Stepping simulator...") vrep.simxSynchronousTrigger(clientID) # Get new quadrotor state err, pos_new = vrep.simxGetObjectPosition(clientID, quad_handle, -1, vrep.simx_opmode_buffer) err, euler_new = vrep.simxGetObjectOrientation(clientID, quad_handle, -1, vrep.simx_opmode_buffer) observation = np.array(euler_new) reward_alpha = np.float32(-np.fabs(euler_new[0])) if not check_quad_flipped(euler_new) else np.float32(-10.) reward_beta = np.float32(-np.fabs(euler_new[1])) if not check_quad_flipped(euler_new) else np.float32(-10.) reward_gamma = np.float32(-np.fabs(euler_new[2])) if not check_quad_flipped(euler_new) else np.float32(-5.) reward_alpha_acc = np.float32(-np.fabs(euler_new[0] - euler_old[0])) if not check_quad_flipped( euler_new) else np.float32(-10.) reward_beta_acc = np.float32(-np.fabs(euler_new[1] - euler_old[1])) if not check_quad_flipped( euler_new) else np.float32(-10.) reward_gamma_acc = np.float32(-np.fabs(euler_new[2] - euler_old[2])) if not check_quad_flipped( euler_new) else np.float32(-5.) reward = reward_alpha + reward_beta + reward_gamma + reward_alpha_acc * 10. + reward_beta_acc * 10. \ + reward_gamma_acc * 10. done = False info = np.array(euler_new) return observation, reward, done, info
def float_signal(cid, handle, val, signal_name): err = vrep.simxSetFloatSignal(cid, signal_name, val, vrep_mode)
def step(self, actions): # Set propeller thrusts print("Setting propeller thrusts...") # Only PD control bc can't find api function for getting simulation time self.propeller_vels = pid(self.pos_error, self.euler_new, self.euler_error[2], self.vel_error, self.angvel_error) self.propeller_vels += actions # Send propeller thrusts print("Sending propeller thrusts...") [ vrep.simxSetFloatSignal(self.clientID, prop, vels, vrep.simx_opmode_oneshot) for prop, vels in zip(self.propellers, self.propeller_vels) ] # Trigger simulator step print("Stepping simulator...") vrep.simxSynchronousTrigger(self.clientID) # Get quadrotor initial position and orientation err, self.pos_new = vrep.simxGetObjectPosition(self.clientID, self.quad_handle, -1, vrep.simx_opmode_buffer) err, self.euler_new = vrep.simxGetObjectOrientation( self.clientID, self.quad_handle, -1, vrep.simx_opmode_buffer) err, self.vel_new, self.angvel_new = vrep.simxGetObjectVelocity( self.clientID, self.quad_handle, vrep.simx_opmode_buffer) self.pos_new = np.asarray(self.pos_new) self.euler_new = np.asarray(self.euler_new) * 10 self.vel_new = np.asarray(self.vel_new) self.angvel_new = np.asarray(self.angvel_new) self.pos_old = self.pos_new self.euler_old = self.euler_new self.vel_old = self.vel_new self.angvel_old = self.angvel_new self.pos_error = (self.pos_start + self.setpoint_delta[0:3]) \ - self.pos_new self.euler_error = (self.euler_start + self.setpoint_delta[3:6]) \ - self.euler_new self.euler_error %= 2 * np.pi for i in range(len(self.euler_error)): if self.euler_error[i] > np.pi: self.euler_error[i] -= 2 * np.pi self.vel_error = (self.vel_start + self.setpoint_delta[6:9]) \ - self.vel_new self.angvel_error = (self.angvel_start + self.setpoint_delta[9:12]) \ - self.angvel_new valid = self.is_valid_state() rew = self.get_reward() self.timestep += 1 done = False if self.timestep > self.episode_len or not valid: done = True return self.get_state(), rew, done, {}
time.sleep(0.2) # Initialize simulation parameters RobotNames = ["LineTracer", "LineTracer#0", "LineTracer#1"] Params = [[[0.1, 0.2, 0, 0], [0.1, 0, 0, 0.2]], [[0.2, 0.2, 0, 0], [0.2, 0, 0, 0.2]], [[0.3, 0.3, 0, 0], [0.3, 0, 0, 0.4]]] for i in range(len(RobotNames)): rN = RobotNames[i] Par = Params[i] for j in range(len(Par)): # For each motor for k in range(len(Par[j])): # For each sensor (+ base value) SignalName = rN+"_"+str(j+1)+"_"+str(k+1) res = vrep.simxSetFloatSignal(clientID, SignalName, Par[j][k], vrep.simx_opmode_oneshot_wait) rbt_names = ["LineTracer", "LineTracer#0", "LineTracer#1"] init_pos = [] rbts = [] for rbt_name in rbt_names: res, rbt_tmp = vrep.simxGetObjectHandle(clientID, rbt_name, vrep.simx_opmode_oneshot_wait) rbts.append(rbt_tmp) res, pos = vrep.simxGetObjectPosition(clientID, rbts[-1], -1, vrep.simx_opmode_streaming) time.sleep(0.2) for rbt in rbts: res, pos = vrep.simxGetObjectPosition(clientID, rbt, -1, vrep.simx_opmode_buffer) init_pos.append(pos) # print pos
def main(): print('Program started') emptybuf = bytearray() vrep.simxFinish(-1) # just in case, close all opened connections clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 2000, 5) if clientID != -1: print('Connected to remote API server') # Start the simulation: vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot_wait) # start init wheels -------------------------------------------------------------------------------------------- wheelJoints = np.empty(4, dtype=np.int) wheelJoints.fill(-1) # front left, rear left, rear right, front right res, wheelJoints[0] = vrep.simxGetObjectHandle( clientID, 'rollingJoint_fl', vrep.simx_opmode_oneshot_wait) res, wheelJoints[1] = vrep.simxGetObjectHandle( clientID, 'rollingJoint_rl', vrep.simx_opmode_oneshot_wait) res, wheelJoints[2] = vrep.simxGetObjectHandle( clientID, 'rollingJoint_fr', vrep.simx_opmode_oneshot_wait) res, wheelJoints[3] = vrep.simxGetObjectHandle( clientID, 'rollingJoint_rr', vrep.simx_opmode_oneshot_wait) # end init wheels ---------------------------------------------------------------------------------------------- # start init camera -------------------------------------------------------------------------------------------- # change the angle of the camera view (default is pi/4) res = vrep.simxSetFloatSignal(clientID, 'rgbd_sensor_scan_angle', math.pi / 2, vrep.simx_opmode_oneshot_wait) # turn on camera res = vrep.simxSetIntegerSignal(clientID, 'handle_rgb_sensor', 2, vrep.simx_opmode_oneshot_wait) # get camera object-handle res, youBotCam = vrep.simxGetObjectHandle( clientID, 'rgbSensor', vrep.simx_opmode_oneshot_wait) # get first image err, resolution, image = vrep.simxGetVisionSensorImage( clientID, youBotCam, 0, vrep.simx_opmode_streaming) time.sleep(1) # end init camera ---------------------------------------------------------------------------------------------- # programmable space ------------------------------------------------------------------------------------------- print("Begin calculation of H-matrix, please wait ...") err, res, image = vrep.simxGetVisionSensorImage( clientID, youBotCam, 0, vrep.simx_opmode_buffer) image = colorDet.convertToCv2Format(image, res) image = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY) found, prime_corners = cv2.findChessboardCorners(image, (3, 4)) prime_corners = addOne(prime_corners) # gCX are the world coordinates for the points on the chessboards which are later used to construct the h-matrix gCX = [[0.075, 0.55, 1.0], [0.075, 0.5, 1.0], [0.075, 0.45, 1.0], [0.025, 0.55, 1.0], [0.025, 0.5, 1.0], [0.025, 0.45, 1.0], [-0.025, 0.55, 1.0], [-0.025, 0.5, 1.0], [-0.025, 0.45, 1.0], [-0.075, 0.55, 1.0], [-0.075, 0.5, 1.0], [-0.075, 0.45, 1.0]] # convert all global corners of the chessboard in egocentric world space ego_corners = [] for gc in gCX: newCorner = colorDet.globalToEgocentric(gc, clientID) ego_corners.append(newCorner) # add a 1 in every row (globalToEgocentric only returns x,y coordinates ego_corners = addOne(ego_corners) # convert ego_corners in numpy array ego_corners = np.asarray(ego_corners) # calculate H-matrix A = getA(prime_corners, ego_corners) H = getH(A) newH = cv2.findHomography(prime_corners, ego_corners) print("H-matrix cv2:", newH) print("H-matrix own:", H / H[2][2]) blobs = colorDet.findAllBlobs(clientID, youBotCam, H) obstacleList = [] for b in blobs: obstacleList.append(b[0]) print("Found blobs:") # sort blob list and print it blobs = sortBlobsByRad(blobs) for blob in blobs: print("Coordinate: ", blob[0], " Upper and lower Bound of the color: ", blob[1]) # get position of youBot and goal roboPos, ori = move.getPos(clientID) res, objHandle = vrep.simxGetObjectHandle( clientID, "goal", vrep.simx_opmode_oneshot_wait) targetPosition = vrep.simxGetObjectPosition( clientID, objHandle, -1, vrep.simx_opmode_oneshot_wait) targetPosition = targetPosition[1][:2] # drive to the goal with obstacles ahead driveThroughPath(obstacleList, roboPos[:2], targetPosition, clientID) # end of programmable space -------------------------------------------------------------------------------------------- # Stop simulation vrep.simxStopSimulation(clientID, vrep.simx_opmode_oneshot_wait) # Close connection to V-REP: vrep.simxFinish(clientID) else: print('Failed connecting to remote API server') print('Program ended')
def setSteering(self, steering): # Steering value [-1,1] self.__steering = steering #if self.getName()=="BubbleRob#1": # print self.getName(), steering vrep.simxSetFloatSignal(self.__clientID, self.__name+":Steering", self.__steering, vrep.simx_opmode_oneshot)
def setSteering(self, steering): # Steering value [-1,1] self.__steering = steering vrep.simxSetFloatSignal(self.__clientID, self.__name+":Steering", self.__steering, vrep.simx_opmode_oneshot)
# Get "handle" to the end-effector of robot result, end_handle = vrep.simxGetObjectHandle(clientID, 'UR10_link7_visible', vrep.simx_opmode_blocking) if result != vrep.simx_return_ok: raise Exception('could not get object handle for end effector') # ==================================================================================================== # # Start simulation vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot) # ******************************** Your robot control code goes here ******************************** # time.sleep(0.1) counter = vrep.simxGetFloatSignal(clientID, "counter", vrep.simx_opmode_streaming)[1] vrep.simxSetFloatSignal(clientID, "RG2_open", 1, vrep.simx_opmode_oneshot) previousCounter = counter # Goal_joint_angles = np.array([[0,0,-0.5*np.pi,0.5*np.pi,-0.5*np.pi,-0.5*np.pi], \ # [0.5*np.pi,0,-0.5*np.pi,0.5*np.pi,0.5*np.pi,-0.5*np.pi],\ # [-0.5*np.pi,-0.5*np.pi,-0.5*np.pi,0,-0.5*np.pi,-0.5*np.pi]]) Goal_joint_angles = np.array([\ [0,0,0,0,0,0], \ [0.5*np.pi,0,0,0,0,0], \ [0,0.25*np.pi,0,0,0,0], \ [0,0,0.5*np.pi,0,0,0], \ [0,0,0,0.5*np.pi,0,0], \ [0,0,0,0,0.5*np.pi,0], \ [0,0,0,0,0,0.5*np.pi], \ # [0,0,0.45*np.pi,0,-0.5*np.pi,0], \ [0,0.1*np.pi,0.35*np.pi,0.05*np.pi,-0.5*np.pi,0], \ ])
# errorCode, cam_handle = vrep.simxGetObjectHandle(clientID, # 'cam1', # vrep.simx_opmode_oneshot_wait) # # errorCode, resolution, image = \ # vrep.simxGetVisionSensorImage(clientID, cam_handle, # 0, vrep.simx_opmode_streaming_split+4000) now = time.time() t0 = now-dT while True: if now > t0+dT: # Set the float signal that tells the robot how fast to drive res = vrep.simxSetFloatSignal(clientID, "Pioneer_p3dx_v0", speeds1[cur_speed], vrep.simx_opmode_oneshot) res = vrep.simxSetFloatSignal(clientID, "Pioneer_p3dx#0_v0", speeds2[cur_speed], vrep.simx_opmode_oneshot) res = vrep.simxSetFloatSignal(clientID, "Pioneer_p3dx#1_v0", speeds3[cur_speed], vrep.simx_opmode_oneshot) print 'New speeds: '+str(speeds1[cur_speed])+' '+str(speeds2[cur_speed])+' '+str(speeds3[cur_speed]) t0 = now cur_speed += 1 if cur_speed >= len(speeds1): cur_speed = 0 # Print elapsed time between each control step
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 res, drone = vrep.simxGetObjectHandle(clientID,"drone_zed",vrep.simx_opmode_blocking) res, target = vrep.simxGetObjectHandle(clientID,"Quadricopter_target",vrep.simx_opmode_blocking) assert drone != 0 assert target != 0 try: ps = np.arange(0.004,0.02,0.002) ds = np.arange(0.5,4,0.5) with open("mesh.txt",'w') as f: for p in ps: for d in ds: f.write("p: "+str(round(p,2))+"\td: "+str(round(d,2))+'\t') print("p: "+str(round(p,2))+"\td: "+str(round(d,2))) vrep.simxSetFloatSignal(clientID,"p",p,vrep.simx_opmode_blocking) vrep.simxSetFloatSignal(clientID,"d",d,vrep.simx_opmode_blocking) vrep.simxStartSimulation(clientID, vrep.simx_opmode_blocking) start = time.time() max_dis = 0 while True: res, sp = vrep.simxGetObjectPosition(clientID,drone,target,vrep.simx_opmode_blocking) max_dis =abs(sp[0]) if abs(sp[0]) > max_dis else max_dis res, vel, _ = vrep.simxGetObjectVelocity(clientID,drone,vrep.simx_opmode_blocking) if abs(sp[0])<0.1 and abs(vel[1])<0.1: f.write("time: "+str(round(time.time() - start,2))+'\n') f.write("dis: "+str(round(max_dis,2))+"\n\n") print("time: "+str(round(time.time() - start,2))) print("dis: "+str(round(max_dis,2))) break if max_dis > 2:
def sendSignal(self): r = vrep.simxSetFloatSignal(self.clientID, self.LSignalName, self.CycleFreqL, vrep.simx_opmode_oneshot) r = vrep.simxSetFloatSignal(self.clientID, self.RSignalName, self.CycleFreqR, vrep.simx_opmode_oneshot)
#float signals Joint = [500, 500, 500, 500, 500, 500] #FK mode joint values #IK mode position X pos_X = 0 pos_Y = 0 pos_Z = 0 Alpha = 0 Beta = 0 Gamma = 0 vrep.simxSetIntegerSignal(clientID, "Apimode", Apimode, vrep.simx_opmode_oneshot) vrep.simxSetIntegerSignal(clientID, "movementMode", movementMode, vrep.simx_opmode_oneshot) if (movementMode): vrep.simxSetFloatSignal(clientID, "pos_X", pos_X, vrep.simx_opmode_oneshot) vrep.simxSetFloatSignal(clientID, "pos_Y", pos_Y, vrep.simx_opmode_oneshot) vrep.simxSetFloatSignal(clientID, "pos_Z", pos_Z, vrep.simx_opmode_oneshot) vrep.simxSetFloatSignal(clientID, "Alpha", Alpha, vrep.simx_opmode_oneshot) vrep.simxSetFloatSignal(clientID, "Beta", Beta, vrep.simx_opmode_oneshot) vrep.simxSetFloatSignal(clientID, "Gamma", Gamma, vrep.simx_opmode_oneshot) else: vrep.simxSetFloatSignal(clientID, "Joint1", Joint[0], vrep.simx_opmode_oneshot) vrep.simxSetFloatSignal(clientID, "Joint2", Joint[1], vrep.simx_opmode_oneshot) vrep.simxSetFloatSignal(clientID, "Joint3", Joint[2], vrep.simx_opmode_oneshot) vrep.simxSetFloatSignal(clientID, "Joint4", Joint[3], vrep.simx_opmode_oneshot) vrep.simxSetFloatSignal(clientID, "Joint5", Joint[4],
def setThrust(self, thrust): # Average wheel speed self.__thrust = thrust vrep.simxSetFloatSignal(self.__clientID, self.__name+":Acceleration", self.__thrust, vrep.simx_opmode_oneshot)
def setStepMode(stepVelocity, stepAmplitude, stepHeight, movementDirection, rotationMode, movementStrength): if (validConnection()): vrep.simxSetFloatSignal(clientID, 'stepVelocity', stepVelocity, vrep.simx_opmode_oneshot_wait) vrep.simxSetFloatSignal(clientID, 'stepVelocity', stepAmplitude, vrep.simx_opmode_oneshot_wait) vrep.simxSetFloatSignal(clientID, 'stepHeight', stepHeight, vrep.simx_opmode_oneshot_wait) vrep.simxSetFloatSignal(clientID, 'movementDirection', movementDirection, vrep.simx_opmode_oneshot_wait) vrep.simxSetFloatSignal(clientID, 'rotationMode', rotationMode, vrep.simx_opmode_oneshot_wait) vrep.simxSetFloatSignal(clientID, 'movementStrength', movementStrength, vrep.simx_opmode_oneshot_wait)
def run_trial(self, genomes): # Set the signals for each robot for genome, robot in zip(genomes, self.robot_names): par = [genome[:4], genome[4:]] for j in range(len(par)): # For each motor for k in range(len(par[j])): # For each sensor (+ base value) signal_name = robot+"_"+str(j+1)+"_"+str(k+1) res = vrep.simxSetFloatSignal(self.clientID, signal_name, par[j][k], vrep.simx_opmode_oneshot) if res > 1: print 'Error setting signal '+signal_name+': '+str(res) # Start running simulation # print 'Running trial' vrep.simxStartSimulation(self.clientID, vrep.simx_opmode_oneshot_wait) # Initialize output arrays sim_time = [[] for i in range(self.n_robots)] robot_x = [[] for i in range(self.n_robots)] robot_y = [[] for i in range(self.n_robots)] init_pos = [[] for i in range(self.n_robots)] fit_y = [0 for i in range(self.n_robots)] t_step = 0.01 # how often to check the simulation's signals t_flag = time.time()+t_step once = True new_data = [0 for i in range(self.n_robots)] go_loop = True while go_loop: now = time.time() if now > t_flag: t_flag = now+t_step # Get info from robot for i in range(self.n_robots): res1, in1 = vrep.simxGetFloatSignal(self.clientID, self.robot_names[i]+'_1', vrep.simx_opmode_buffer) res2, in2 = vrep.simxGetFloatSignal(self.clientID, self.robot_names[i]+'_2', vrep.simx_opmode_buffer) res3, in3 = vrep.simxGetFloatSignal(self.clientID, self.robot_names[i]+'_3', vrep.simx_opmode_buffer) if res1 == 0 and res2 == 0 and res3 == 0: sim_time[i].append(in1) robot_x[i].append(in2) robot_y[i].append(in3) new_data[i] = 1 # new data arrived if once: if sum(new_data) == self.n_robots: for i in range(self.n_robots): init_pos[i] = [robot_x[i][-1], robot_y[i][-1]] once = False else: for i in range(self.n_robots): if new_data[i] == 1: fit_y[i] += abs(robot_y[i][-1])*(sim_time[i][-1]-sim_time[i][-2]) new_data[i] = 0 if sim_time[i][-1] > 3: # time limit for the simulation go_loop = False break # # Pause the simulation # vrep.simxPauseSimulation(self.clientID, vrep.simx_opmode_oneshot) # time.sleep(0.15) # # trial_fitness = [] # # Get the trial results for each robot # for i in range(len(self.robot_handles)): # res, pos = vrep.simxGetObjectPosition(self.clientID, self.robot_handles[i], -1, vrep.simx_opmode_buffer) # # print pos_tmp # # delta_x = pos[0] - self.robot_pos0[i][0] # delta_y = pos[1] - self.robot_pos0[i][1] # # # trial_fitness.append(delta_x) # trial_fitness.append([delta_x, 1./(1.+20*abs(delta_y))]) trial_fitness = [] for i in range(self.n_robots): x_fit = robot_x[i][-1] - init_pos[i][0] y_fit = 1/(1+20*fit_y[i]) trial_fitness.append([x_fit, y_fit]) # Stop and reset the simulation vrep.simxStopSimulation(self.clientID, vrep.simx_opmode_oneshot) time.sleep(0.35) return trial_fitness