def getDif(cid,copter,target): copter_pos = vrep.simxGetObjectPosition(cid, copter, -1, mode())[1] copter_vel = vrep.simxGetObjectVelocity(cid, copter, mode())[1] copter_orientation = vrep.simxGetObjectOrientation(cid,copter,-1,mode())[1] target_pos = vrep.simxGetObjectPosition(cid, target, -1, mode())[1] target_vel = vrep.simxGetObjectVelocity(cid, target, mode())[1] return np.asarray([(-np.asarray(copter_pos) + np.asarray(target_pos)),(-np.asarray(copter_vel) + np.asarray(target_vel)),np.asarray(copter_orientation)]).flatten()
def get_object_velocity(self, name): """Get an (x,y) velocity for object with name <name>""" handle = self.get_handle(name) if handle not in self._v: vrep.simxGetObjectVelocity( self.id, handle, vrep.simx_opmode_streaming) self._v.append(handle) time.sleep(0.5) r, lin, ang = vrep.simxGetObjectVelocity( self.id, handle, vrep.simx_opmode_buffer) if r != vrep.simx_return_ok: raise NameError('Velocity unknown') return lin[:2]
def loop(self): operationMode = vrep.simx_opmode_streaming if self.__initLoop: self.__initLoop = False else: operationMode = vrep.simx_opmode_buffer returnCode, position = vrep.simxGetObjectPosition(self.__clientID, self.__bodyHandle, -1, operationMode) if returnCode == vrep.simx_return_ok: self.__position = [0.0, 0.0] self.__position[0] = position[0] # X self.__position[1] = position[1] # Y else: self.__position = None # print >> sys.stderr, "Error in VRepBubbleRob.getPosition()" returnCode, linearVelocity, angularVelocity = vrep.simxGetObjectVelocity( self.__clientID, self.__bodyHandle, operationMode ) if returnCode == vrep.simx_return_ok: try: self.__velocity = linearVelocity[0] * math.cos(self.__orientation) + linearVelocity[1] * math.sin( self.__orientation ) self.__mind.setInput("velocity", self.__velocity) except TypeError: pass # if self.__name=="BubbleRob#1": # print self.__velocity, linearVelocity[0], math.cos(self.__orientation), linearVelocity[1], math.sin(self.__orientation) else: self.__velocity = None # print >> sys.stderr, "Error in VRepBubbleRob.getPosition()" self.__cnt = self.__cnt + 1
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 getState(self, initial=False): if initial: mode = vrep.simx_opmode_streaming else: mode = vrep.simx_opmode_buffer # Retrieve IMU data errorSignal, self.stepSeconds = vrep.simxGetFloatSignal(self.clientID, 'stepSeconds', mode) errorOrien, baseEuler = vrep.simxGetObjectOrientation(self.clientID, self.Quadbase, -1, mode) errorPos, basePos = vrep.simxGetObjectPosition(self.clientID, self.Quadbase,-1, mode) errorVel, linVel, angVel = vrep.simxGetObjectVelocity(self.clientID, self.Quadbase, mode) if initial: if (errorSignal or errorOrien or errorPos or errorVel != vrep.simx_return_ok): time.sleep(0.05) pass else: # Convert Euler angles to pitch, roll, yaw rollRad, pitchRad = rotate((baseEuler[0], baseEuler[1]), baseEuler[2]) pitchRad = -pitchRad yawRad = -baseEuler[2] baseRad = np.array([yawRad,rollRad,pitchRad])+0.0 self.state = np.asarray(np.concatenate((basePos,linVel,angVel,baseRad)),dtype=np.float32) #print("data_core: " + str(self.state)) return self.state
def loop(self): operationMode=vrep.simx_opmode_streaming if self.__initLoop: self.__initLoop=False else: operationMode=vrep.simx_opmode_buffer returnCode, orientation = vrep.simxGetObjectOrientation(self.__clientID, self.__bodyHandle, -1, operationMode) if returnCode==vrep.simx_return_ok: self.__orientation=orientation[2] else: self.__orientation = None # print >> sys.stderr, "Error in VRepBubbleRob.getOrientation()" self.__mind.setInput("orientation", self.__orientation) returnCode, position = vrep.simxGetObjectPosition(self.__clientID, self.__bodyHandle, -1, operationMode) if returnCode==vrep.simx_return_ok: self.__position=[0.0,0.0] self.__position[0]=position[0] #X self.__position[1]=position[1] #Y else: self.__position=None # print >> sys.stderr, "Error in VRepBubbleRob.getPosition()" returnCode, linearVelocity, angularVelocity = vrep.simxGetObjectVelocity(self.__clientID, self.__bodyHandle, operationMode) if returnCode==vrep.simx_return_ok: try: # self.__velocity=linearVelocity[0]*math.cos(self.__orientation)+linearVelocity[1]*math.sin(self.__orientation) self.__velocity=math.sqrt(linearVelocity[0]**2+linearVelocity[1]**2) self.__mind.setInput("velocity", self.__velocity) except TypeError: pass # if self.__name=="BubbleRob#1": # print self.__velocity, linearVelocity[0], math.cos(self.__orientation), linearVelocity[1], math.sin(self.__orientation) else: self.__velocity=None # print >> sys.stderr, "Error in VRepBubbleRob.getPosition()" returnCode, sensorTrigger, dp, doh, dsnv = vrep.simxReadProximitySensor(self.__clientID, self.__sensorHandle, operationMode) if returnCode==vrep.simx_return_ok: # We succeeded at reading the proximity sensor self.__mind.setInput("lastProximitySensorTime", vrep.simxGetLastCmdTime(self.__clientID)) self.__mind.setInput("sensorTrigger", sensorTrigger) self.__mind.setInput("mostSalientItem", self.__mind.selectMostSalient("I")) # print self.__name, self.__mind.getAttendedItem(self.__mind.getOutput("attendedItemName")) self.__mind.setInput("attendedItem", self.__mind.getAttendedItem(self.__mind.getOutput("attendedItemName"))) self.__mind.applyRules() self.__mind.setStates() if self.__mind.getOutput("steering")!=None: self.setSteering(self.__mind.getOutput("steering")) if self.__mind.getOutput("thrust")!=None: self.setThrust(self.__mind.getOutput("thrust")) if self.__mind.getOutput("reward")!=None: if self.__mind.getOutput("reward")>0.5: self.setEmotionalExpression(1) elif self.__mind.getOutput("reward")<-0.5: self.setEmotionalExpression(-1) else: self.setEmotionalExpression(0) getSignalReturnCode, dMessage = vrep.simxGetStringSignal(self.__clientID, "Debug", vrep.simx_opmode_streaming) if dMessage!="": print("Debug:"+dMessage) self.__cnt=self.__cnt+1
def calculate_error( self ): # Return the state variables err, self.ori = vrep.simxGetObjectOrientation(self.cid, self.copter, -1, vrep_mode ) err, self.pos = vrep.simxGetObjectPosition(self.cid, self.copter, -1, vrep_mode ) err, self.lin, self.ang = vrep.simxGetObjectVelocity(self.cid, self.copter, vrep_mode ) self.ori = convert_angles(self.ori) # Apply noise to each measurement if required #FIXME this is a dumb way to do this, clean it up later if self.noise: n_pos = np.random.normal(0,self.noise_std[0],3) n_lin = np.random.normal(0,self.noise_std[1],3) n_ori = np.random.normal(0,self.noise_std[2],3) n_ang = np.random.normal(0,self.noise_std[3],3) for i in range(3): self.pos[i] += n_pos[i] self.lin[i] += n_lin[i] self.ori[i] += n_ori[i] self.ang[i] += n_ang[i] #TODO: might have to wrap angles here # Find the error self.ori_err = [self.t_ori[0] - self.ori[0], self.t_ori[1] - self.ori[1], self.t_ori[2] - self.ori[2]] cz = math.cos(self.ori[2]) sz = math.sin(self.ori[2]) x_err = self.t_pos[0] - self.pos[0] y_err = self.t_pos[1] - self.pos[1] self.pos_err = [ x_err * cz + y_err * sz, -x_err * sz + y_err * cz, self.t_pos[2] - self.pos[2]] self.lin = [self.lin[0]*cz+self.lin[1]*sz, -self.lin[0]*sz+self.lin[1]*cz, self.lin[2]] self.ang = [self.ang[0]*cz+self.ang[1]*sz, -self.ang[0]*sz+self.ang[1]*cz, self.ang[2]] for i in range(3): if self.ori_err[i] > math.pi: self.ori_err[i] -= 2 * math.pi elif self.ori_err[i] < -math.pi: self.ori_err[i] += 2 * math.pi
def observe(self): operationMode=vrep.simx_opmode_streaming if self.__initLoop: self.__initLoop=False else: operationMode=vrep.simx_opmode_buffer returnCode, orientation = vrep.simxGetObjectOrientation(self.__clientID, self.__bodyHandle, -1, operationMode) if returnCode==vrep.simx_return_ok: self.__orientation=orientation[2] else: self.__orientation = None # print >> sys.stderr, "Error in VRepBubbleRob.getOrientation()" self.__mind.setInput("orientation", self.__orientation) returnCode, position = vrep.simxGetObjectPosition(self.__clientID, self.__bodyHandle, -1, operationMode) if returnCode==vrep.simx_return_ok: self.__position=[0.0,0.0] self.__position[0]=position[0] #X self.__position[1]=position[1] #Y else: self.__position=None print >> sys.stderr, "Error in VRepBubbleRob.getPosition()", self.__clientID, self.__bodyHandle returnCode, linearVelocity, angularVelocity = vrep.simxGetObjectVelocity(self.__clientID, self.__bodyHandle, operationMode) if returnCode==vrep.simx_return_ok: try: # self.__velocity=linearVelocity[0]*math.cos(self.__orientation)+linearVelocity[1]*math.sin(self.__orientation) self.__velocity=math.sqrt(linearVelocity[0]**2+linearVelocity[1]**2) self.__mind.setInput("velocity", self.__velocity) self.__linearVelocity=linearVelocity except TypeError: pass # if self.__name=="BubbleRob#1": # print self.__velocity, linearVelocity[0], math.cos(self.__orientation), linearVelocity[1], math.sin(self.__orientation) else: self.__velocity=None # print >> sys.stderr, "Error in VRepBubbleRob.getVelocity()" returnCode, sensorTrigger, dp, doh, dsnv = vrep.simxReadProximitySensor(self.__clientID, self.__sensorHandle, operationMode) if returnCode==vrep.simx_return_ok: # We succeeded at reading the proximity sensor self.__mind.setInput("lastProximitySensorTime", vrep.simxGetLastCmdTime(self.__clientID)) self.__mind.setInput("sensorTrigger", sensorTrigger) self.blocked() # judge if blocked
def step(self, action): done =False VREP_com.vrep_moves_robot(clientID, Handle, action) vrep.simxSynchronousTrigger(clientID) vrep.simxGetPingTime(clientID) ## observe the state after taking movement Observation_array = [] Observation_array = np.array(Observation_array) ### get joint position, angular_vel, head_vel, torque of each joints joints_pos = VREP_com.vrep_get_joint_pos(clientID, Handle) joints_angular_vel = VREP_com.vrep_get_angular_vel(clientID, Handle) joints_torque = VREP_com.vrep_get_torque(clientID, Handle) Observation_array = np.concatenate((joints_pos,joints_angular_vel, joints_torque)) err_code ,linearVelocity_cam_each_step, angularVelocity_cam = vrep.simxGetObjectVelocity(clientID,Handle[0, 9],vrep.simx_opmode_blocking) if err_code !=0 : raise Exception() robot_vel = linearVelocity_cam_each_step[1] Observation_array = np.append(Observation_array, round(robot_vel, 4)) Observation_array = np.append(Observation_array, self.Target_vel) ## get reward power = np.sum(np.abs(joints_torque*joints_angular_vel)) #nor_power = (np.sum((np.abs(joints_torque*joints_angular_vel)))/(self.torque_max*self.angular_vel_max))/8 #print('power: ', power) #reward = ((1 - (np.abs(self.Target_vel - head_vel))/self.a1)**(1/self.a2))*((np.abs(1-nor_power))**(self.b1**-2)) #reward = np.round(reward, 5) #print('head_velocity',head_vel ) err_code, MyRobotPos_Current = vrep.simxGetObjectPosition(clientID, Handle[0, 9], -1, vrep.simx_opmode_blocking) MyRobotPos_Current = np.array(MyRobotPos_Current) COT_inv = (5.000e-01 * 9.8 * MyRobotPos_Current[1])/power self.state = np.array(Observation_array) if robot_vel >= self.Target_vel: done = True return self.state, COT_inv, done, {}
def angular_velocity(cid, handle): err, lin, ang = vrep.simxGetObjectVelocity(cid, handle, vrep_mode) return ang
def obterVelocidade(self, corpo): angVel = vrep.simxGetObjectVelocity(self.cliente, corpo, vrep.simx_opmode_streaming) print(angVel) return angVel[1]
def controller_motor(cid, copter, targHandle): global firstPass global pParam global iParam global dParam global vParam global cumul global lastE global pAlphaE global pBetaE global psp2 global psp1 global prevEuler global firstPass global particlesTargetVelocities global propellerScripts if (firstPass): propellerScripts=[-1,-1,-1,-1] for i in range(4): propellerScripts[i]= vrep.simxGetObjectHandle(cid,'Quadricopter_propeller_respondable'+str(i)+str(1),mode()) particlesTargetVelocities = [0]*4 pParam=.8 iParam=0 dParam=0 vParam= .6 cumul=0 lastE=0 pAlphaE=0 pBetaE=0 psp2=0 psp1=0 prevEuler=0 print("OMG FIRSTPASS") print("PREV EULER") print(prevEuler) #Vertical control: #d = vrep.simxGetObjectHandle(cid,'Quadricopter_base',mode())[1] #targetPos=simGetObjectPosition(targetObj,-1) targetPos = vrep.simxGetObjectPosition(cid,targHandle,-1,mode())[1] print("Target_Pos IS") print(targetPos) #pos = simGetObjectPosition(d,-1) pos = vrep.simxGetObjectPosition(cid, copter, -1, mode())[1] l=vrep.simxGetObjectVelocity(cid, copter, mode())[1] print("l is:{}".format(l)) e=(targetPos[2]-pos[2]) cumul=cumul+e pv=pParam*e #thrust=5.335+pv+iParam*cumul+dParam*(e-lastE)+l[2]*vParam lastE=e thrust =5.335+ pParam * e +vParam *(0- l[2]) print("THIS IS THRUST" + str(thrust)) #Horizontal control: #sp=simGetObjectPosition(targetObj,d) sp = vrep.simxGetObjectPosition(cid,targHandle,copter,mode())[1] m=simGetObjectMatrix(cid,copter,-1) #m = vrep.simxGetJointMatrix(cid,d,mode()) print(m) vx=np.array([1,0,0,1], dtype = np.float64) vx = np.reshape(vx,(4,1)) #vx=simMultiplyVector(m,vx) print(m.shape) print(m) print(vx.shape) print(vx) vx = np.dot(m,vx) print("CHECK KEK") print(m.shape) print(m) print(vx.shape) print(vx) vy = np.array([0,1,0,1],dtype = np.float64) vy = np.reshape(vy,(4,1)) vy=np.dot(m,vy) m = m.flatten() alphaE=(vy[2]-m[11]) alphaCorr=0.25*alphaE+2.1*(alphaE-pAlphaE) betaE=(vx[2]-m[11]) betaCorr=-0.25*betaE-2.1*(betaE-pBetaE) pAlphaE=alphaE pBetaE=betaE alphaCorr=alphaCorr+sp[1]*0.005+1*(sp[1]-psp2) betaCorr=betaCorr-sp[0]*0.005-1*(sp[0]-psp1) psp2=sp[1] psp1=sp[0] #-- Rotational control: #euler=simGetObjectOrientation(d,targetObj) euler = vrep.simxGetObjectOrientation(cid,copter,targHandle,mode())[1] rotCorr=euler[2]*0.1+2*(euler[2]-prevEuler) prevEuler=euler[2] #-- Decide of the motor velocities: particlesTargetVelocities[0]=thrust*(1-alphaCorr+betaCorr+rotCorr) particlesTargetVelocities[1]=thrust*(1-alphaCorr-betaCorr-rotCorr) particlesTargetVelocities[2]=thrust*(1+alphaCorr-betaCorr+rotCorr) particlesTargetVelocities[3]=thrust*(1+alphaCorr+betaCorr-rotCorr) #-- Send the desired motor velocities to the 4 rotors: packedData = vrep.simxPackFloats([x for x in particlesTargetVelocities]) #packedData = vrep.simxPackFloats([100,100,100,100]) raw_bytes = (ctypes.c_ubyte * len(packedData)).from_buffer_copy(packedData) err = vrep.simxSetStringSignal(cid, "rotorTargetVelocities", packedData, vrep.simx_opmode_oneshot) firstPass = False return particlesTargetVelocities
import sys vrep.simxFinish(-1) clientID = vrep.simxStart('192.168.0.19',19999, True, True, 5000, 5) if clientID != -1: print('Conexion Establecida!') else: sys.exit('Error: No se puede conectar!') _, left_motor = vrep.simxGetObjectHandle(clientID, 'Pioneer_p3dx_leftMotor', vrep.simx_opmode_oneshot_wait) _, right_motor = vrep.simxGetObjectHandle(clientID, 'Pioneer_p3dx_rightMotor', vrep.simx_opmode_oneshot_wait) _, pioneer_handler = vrep.simxGetObjectHandle(clientID, 'Pioneer_p3dx', vrep.simx_opmode_oneshot_wait) while True: position = vrep.simxGetObjectPosition(clientID, pioneer_handler, -1, vrep.simx_opmode_streaming) orientation = vrep.simxGetObjectOrientation(clientID, pioneer_handler, -1, vrep.simx_opmode_streaming) velocity = vrep.simxGetObjectVelocity(clientID, pioneer_handler, vrep.simx_opmode_streaming) quaternion = vrep.simxGetObjectQuaternion(clientID, pioneer_handler, -1, vrep.simx_opmode_streaming) #print('p>:\t', position) #print('o>:\t', orientation) #print('q>:\t', quaternion) #print(velocity[1]) RotMat = GetFlatRotationMatrix(orientation[1]) rowdata = np.append(RotMat, position[1]) rowdata = np.append(rowdata, velocity[1]) rowdata = np.append(rowdata, velocity[2]) #print(np.shape(rowdata)) print(rowdata) vrep.simxFinish(-1)
#Sensor handlers sensorList = ('Proximity_sensor1', 'Proximity_sensor2', 'Proximity_sensor3') sensor_errorCode, sensor_handles = tv.ObjectHandle(clientID, sensorList) #Read sensor raw data (first time initial) readval_errorCode, sensor_val, sensor_state = tv.INI_ReadProximitySensor(clientID, sensor_handles) #for x in range(1,3+1): #errorCode,sensor_handle=vrep.simxGetObjectHandle(clientID,'Proximity_sensor'+str(x),vrep.simx_opmode_oneshot_wait) #sensor_h.append(sensor_handle) #keep list of handles #errorCode,detectionState,detectedPoint,detectedObjectHandle,detectedSurfaceNormalVector=vrep.simxReadProximitySensor(clientID,sensor_handles[x-1],vrep.simx_opmode_streaming) #sensor_val=np.append(sensor_val,np.linalg.norm(detectedPoint)) #get list of values #sensor_state=np.append(sensor_state,detectionState) #get list of values errorCode,linearVelocity,angularVelocity=vrep.simxGetObjectVelocity(clientID,car_handle,vrep.simx_opmode_streaming) sensor_val=np.append(sensor_val,5*np.linalg.norm(linearVelocity)) train = np.floor(20*sensor_val).astype(int) #Get New Rewards #errorCode,linearVelocity,angularVelocity=vrep.simxGetObjectVelocity(clientID,cam_handle,vrep.simx_opmode_buffer) #print(linearVelocity) weightReadings = [1, 1, 1, 0] reward = np.dot(train,weightReadings) reward = reward.astype(int) state = np.array([train]) state = state.astype(int) proximitysensor = state[0][0], state[0][1], state[0][2] loss_log = [] replay = []
def collectImageData(clientID): list_of_images = [] collector = [] col_lst = [] if clientID != -1: err, tracer_handle = vrep.simxGetObjectHandle( clientID, 'LineTracer', vrep.simx_opmode_oneshot_wait) res, v0 = vrep.simxGetObjectHandle(clientID, 'Vision_sensor', vrep.simx_opmode_oneshot_wait) res, v1 = vrep.simxGetObjectHandle(clientID, 'PassiveVision_sensor', vrep.simx_opmode_oneshot_wait) ret_code, left_handle = vrep.simxGetObjectHandle( clientID, 'DynamicLeftJoint', vrep.simx_opmode_oneshot_wait) ret_code, right_handle = vrep.simxGetObjectHandle( clientID, 'DynamicRightJoint', vrep.simx_opmode_oneshot_wait) ret_code, base_handle = vrep.simxGetObjectHandle( clientID, 'LineTracerBase', vrep.simx_opmode_oneshot_wait) res, resolution, image = vrep.simxGetVisionSensorImage( clientID, v0, 0, vrep.simx_opmode_streaming) ret_code, euler_angles = vrep.simxGetObjectOrientation( clientID, tracer_handle, -1, vrep.simx_opmode_streaming) count = 0 action = 2 inference_counter = 0 steps = 30 delay = 0 while (vrep.simxGetConnectionId(clientID) != -1 and count < steps): tim = time.time() '''The amount of time for inference for my model is .05 - .1 seconds''' _x = np.random.randint(0, 10) _x += 10 for i in range(_x): vrep.simxSynchronousTrigger(clientID) col_lst.append(detectCollisionSignal(clientID)) res, resolution, image = vrep.simxGetVisionSensorImage( clientID, v0, 0, vrep.simx_opmode_buffer) if res == vrep.simx_return_ok: img = np.array(image, dtype=np.uint8) img.resize([resolution[1], resolution[0], 3]) rotate_img = img.copy() rotate_img = np.flipud(img) list_of_images.append(rotate_img) ret_code, pos = vrep.simxGetObjectPosition( clientID, tracer_handle, -1, vrep.simx_opmode_oneshot) ret_code, velo, angle_velo = vrep.simxGetObjectVelocity( clientID, tracer_handle, vrep.simx_opmode_oneshot) ret_code, euler_angles = vrep.simxGetObjectOrientation( clientID, tracer_handle, -1, vrep.simx_opmode_buffer) collector.append([ pos[0], pos[1], pos[2], velo[0], velo[1], velo[2], angle_velo[0], angle_velo[1], angle_velo[2], euler_angles[0], euler_angles[1], euler_angles[2], action ]) if delay <= 0: action = np.random.randint(0, 5) delay = np.random.randint(2, 10) velo = (action - 2) * 15 return_val = vrep.simxSetJointTargetVelocity( clientID, left_handle, velo, vrep.simx_opmode_oneshot) return_val2 = vrep.simxSetJointTargetVelocity( clientID, right_handle, velo, vrep.simx_opmode_oneshot_wait) delay -= 1 count += 1 return list_of_images, collector, col_lst else: sys.exit()
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, {}
vrep.simx_opmode_streaming) # wait for simulation time.sleep(2) res, resolution, image = vrep.simxGetVisionSensorImage( clientID, vision_sensor, 0, vrep.simx_opmode_streaming) ErrorCode, posL = vrep.simxGetJointPosition(clientID, motorLeft, vrep.simx_opmode_streaming) ErrorCode, posR = vrep.simxGetJointPosition(clientID, motorRight, vrep.simx_opmode_streaming) ErrorCode, pos = vrep.simxGetObjectPosition(clientID, car, -1, vrep.simx_opmode_streaming) ErrorCode, euler = vrep.simxGetObjectOrientation(clientID, car, -1, vrep.simx_opmode_streaming) speedx, speedy, speedz = vrep.simxGetObjectVelocity(clientID, car, vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID, motorLeft, 0, vrep.simx_opmode_buffer) vrep.simxSetJointTargetPosition(clientID, motorRight, 0, vrep.simx_opmode_buffer) time.sleep(1) euler[1] = -euler[1] euler[2] = -euler[2] if euler[2] <= 0: euler[1] = math.pi - euler[1] c = euler[1] + math.pi / 2 aux_gpsthetaant = c desiredSteeringAngle = 0 j = 0
errorCode, left_motor_handle = vrep.simxGetObjectHandle( clientID, 'Pioneer_p3dx_leftMotor', vrep.simx_opmode_oneshot_wait) errorCode, right_motor_handle = vrep.simxGetObjectHandle( clientID, 'Pioneer_p3dx_rightMotor', vrep.simx_opmode_oneshot_wait) # Get visionSensorHandle visionSensorHandleReturnCode, visionSensorHandle = vrep.simxGetObjectHandle( clientID, 'cam1', vrep.simx_opmode_oneshot_wait) pioneerHandleReturnCode, pioneerHandle = vrep.simxGetObjectHandle( clientID, 'Pioneer_p3dx', vrep.simx_opmode_oneshot_wait) # Setup visionSensorValues visionSensorReturnCode, visionSensorDetectionState, visionSensorAuxPackets = vrep.simxReadVisionSensor( clientID, visionSensorHandle, vrep.simx_opmode_streaming) pioneerVelocityReturnCode, pioneerVelocityLinearVelocity, pioneerVelocityAngularVelocity = vrep.simxGetObjectVelocity( clientID, pioneerHandle, vrep.simx_opmode_streaming) sensor_h = [] #empty list for handles sensor_val = np.array([]) #empty array for sensor measurements #orientation of all the sensors: sensor_loc = np.array([ -PI / 2, -50 / 180.0 * PI, -30 / 180.0 * PI, -10 / 180.0 * PI, 10 / 180.0 * PI, 30 / 180.0 * PI, 50 / 180.0 * PI, PI / 2, PI / 2, 130 / 180.0 * PI, 150 / 180.0 * PI, 170 / 180.0 * PI, -170 / 180.0 * PI, -150 / 180.0 * PI, -130 / 180.0 * PI, -PI / 2 ]) #for loop to retrieve sensor arrays and initiate sensors for x in range(1, 16 + 1): errorCode, sensor_handle = vrep.simxGetObjectHandle(
def collectImageData(clientID): list_of_images = [] collector = [] if clientID != -1: res, v0 = vrep.simxGetObjectHandle(clientID, 'Vision_sensor', vrep.simx_opmode_oneshot_wait) res, v1 = vrep.simxGetObjectHandle(clientID, 'PassiveVision_sensor', vrep.simx_opmode_oneshot_wait) ret_code, left_handle = vrep.simxGetObjectHandle( clientID, 'DynamicLeftJoint', vrep.simx_opmode_oneshot_wait) ret_code, right_handle = vrep.simxGetObjectHandle( clientID, 'DynamicRightJoint', vrep.simx_opmode_oneshot_wait) ret_code, base_handle = vrep.simxGetObjectHandle( clientID, 'LineTracerBase', vrep.simx_opmode_oneshot_wait) res, resolution, image = vrep.simxGetVisionSensorImage( clientID, v0, 0, vrep.simx_opmode_streaming) ret_code, euler_angles = vrep.simxGetObjectOrientation( clientID, base_handle, -1, vrep.simx_opmode_streaming) t_end = time.time() + 2.8 collision_bool = False count = 0 updated_counter = 0 while (vrep.simxGetConnectionId(clientID) != -1 and time.time() < t_end): res, resolution, image = vrep.simxGetVisionSensorImage( clientID, v0, 0, vrep.simx_opmode_buffer) if count % 25 == 0: act = np.random.randint(5) action = (act - 2) * 15 updated_counter += 1 return_val = vrep.simxSetJointTargetVelocity( clientID, left_handle, action, vrep.simx_opmode_oneshot) return_val2 = vrep.simxSetJointTargetVelocity( clientID, right_handle, action, vrep.simx_opmode_oneshot_wait) #convert the image add to numpy array we collect about 35 images per simulation if res == vrep.simx_return_ok: ret_code, pos = vrep.simxGetObjectPosition( clientID, base_handle, -1, vrep.simx_opmode_oneshot) ret_code, velo, angle_velo = vrep.simxGetObjectVelocity( clientID, base_handle, vrep.simx_opmode_oneshot) ret_code, euler_angles = vrep.simxGetObjectOrientation( clientID, base_handle, -1, vrep.simx_opmode_buffer) collector.append([ pos[0], pos[1], pos[2], velo[0], velo[1], velo[2], euler_angles[0], euler_angles[1], euler_angles[2], action ]) img = np.array(image, dtype=np.uint8) img.resize([resolution[1], resolution[0], 3]) rotate_img = img.copy() rotate_img = np.flipud(img) list_of_images.append(rotate_img) count += 1 return list_of_images, collector else: sys.exit()
def getState(): #height res, handle = vrep.simxGetObjectHandle(clientID, 'LHipYawPitch3', vrep.simx_opmode_blocking) print(handle) res, pos = vrep.simxGetObjectPosition(clientID, handle, -1, vrep.simx_opmode_blocking) height = pos[2] #torso res, handle = vrep.simxGetObjectHandle(clientID, "imported_part_20_sub0", vrep.simx_opmode_blocking) print(handle) res, Angle = vrep.simxGetObjectOrientation(clientID, handle, -1, vrep.simx_opmode_blocking) res, Torsolv, Torsoav = vrep.simxGetObjectVelocity( clientID, handle, vrep.simx_opmode_blocking) TorsoAngle = Angle #Rthigh res, handle = vrep.simxGetObjectHandle(clientID, "imported_part_13_sub", vrep.simx_opmode_blocking) res, Angle = vrep.simxGetObjectOrientation(clientID, handle, -1, vrep.simx_opmode_blocking) res, Rthighlv, Rthighav = vrep.simxGetObjectVelocity( clientID, handle, vrep.simx_opmode_blocking) RthighAngle = Angle #Lthigh res, handle = vrep.simxGetObjectHandle(clientID, "imported_part_10_sub", vrep.simx_opmode_blocking) res, Angle = vrep.simxGetObjectOrientation(clientID, handle, -1, vrep.simx_opmode_blocking) res, Lthighlv, Lthighav = vrep.simxGetObjectVelocity( clientID, handle, vrep.simx_opmode_blocking) LthighAngle = Angle #Rcalf res, handle = vrep.simxGetObjectHandle(clientID, "r_knee_pitch_link_pure2", vrep.simx_opmode_blocking) res, Angle = vrep.simxGetObjectOrientation(clientID, handle, -1, vrep.simx_opmode_blocking) res, Rcalflv, Rcalfav = vrep.simxGetObjectVelocity( clientID, handle, vrep.simx_opmode_blocking) RcalfAngle = Angle #Lcalf res, handle = vrep.simxGetObjectHandle(clientID, "l_knee_pitch_link_pure2", vrep.simx_opmode_blocking) res, Angle = vrep.simxGetObjectOrientation(clientID, handle, -1, vrep.simx_opmode_blocking) res, Lcalflv, Lcalfav = vrep.simxGetObjectVelocity( clientID, handle, vrep.simx_opmode_blocking) LcalfAngle = Angle state = [height] state.extend(TorsoAngle) state.extend(Torsolv) state.extend(Torsoav) state.extend(RthighAngle) state.extend(Rthighlv) state.extend(Rthighav) state.extend(LthighAngle) state.extend(Lthighlv) state.extend(Lthighav) state.extend(RcalfAngle) state.extend(Rcalflv) state.extend(Rcalfav) state.extend(LcalfAngle) state.extend(Lcalflv) state.extend(Lcalfav) state = np.array(state) return state
def readSensorData(self): if self.scene.vrepConnected == False: return if "readSensorData_firstCall" not in self.__dict__: self.readSensorData_firstCall = True else: self.readSensorData_firstCall = False # Read robot states res, pos = vrep.simxGetObjectPosition(self.scene.clientID, self.robotHandle, -1, vrep.simx_opmode_blocking) if res != 0: raise VrepError("Cannot get object position with error code " + str(res)) res, ori = vrep.simxGetObjectOrientation(self.scene.clientID, self.robotHandle, -1, vrep.simx_opmode_blocking) if res != 0: raise VrepError("Cannot get object orientation with error code " + str(res)) res, vel, omega = vrep.simxGetObjectVelocity(self.scene.clientID, self.robotHandle, vrep.simx_opmode_blocking) if res != 0: raise VrepError("Cannot get object velocity with error code " + str(res)) #print("Linear speed: %.3f" % (vel[0]**2 + vel[1]**2)**0.5, # "m/s. Angular speed: %.3f" % omega[2]) #print("pos: %.2f" % pos[0], ", %.2f" % pos[1]) #print("Robot #", self.index, " ori: %.3f" % ori[0], ", %.3f" % ori[1], ", %.3f" % ori[2]) self.xi.x = pos[0] self.xi.y = pos[1] self.xi.alpha = ori[0] self.xi.beta = ori[1] self.xi.theta = ori[2] sgn = np.sign( np.dot( np.asarray(vel[0:2]), np.asarray([math.cos(self.xi.theta), math.sin(self.xi.theta)]))) self.vActual = sgn * (vel[0]**2 + vel[1]**2)**0.5 self.omegaActual = omega[2] # Read laser/vision sensor data if self.scene.SENSOR_TYPE == "2d_": # self.laserFrontHandle # self.laserRearHandle if self.readSensorData_firstCall: opmode = vrep.simx_opmode_streaming else: opmode = vrep.simx_opmode_buffer laserFront_points = vrep.simxGetStringSignal( self.scene.clientID, self.laserFrontName + '_signal', opmode) print(self.laserFrontName + '_signal: ', len(laserFront_points[1])) laserRear_points = vrep.simxGetStringSignal( self.scene.clientID, self.laserRearName + '_signal', opmode) print(self.laserRearName + '_signal: ', len(laserRear_points[1])) elif self.scene.SENSOR_TYPE == "2d": # deprecated raise Exception('2d sensor is not supported!!!!') elif self.scene.SENSOR_TYPE == "VPL16": # self.pointCloudHandle velodyne_points = vrep.simxCallScriptFunction( self.scene.clientID, self.pointCloudName, 1, 'getVelodyneData_function', [], [], [], 'abc', vrep.simx_opmode_blocking) #print(len(velodyne_points[2])) #print(velodyne_points[2]) res = velodyne_points[0] # Parse data if 'VPL16_counter' not in self.__dict__: self.VPL16_counter = 0 # reset the counter every fourth time if self.VPL16_counter == 4: self.VPL16_counter = 0 if self.VPL16_counter == 0: # Reset point cloud self.pointCloud.clearData() #print('VPL16_counter = ', self.VPL16_counter) self.pointCloud.addRawData(velodyne_points[2]) # will rotate here if self.VPL16_counter == 3: #print("Length of point cloud is " + str(len(self.pointCloud.data))) if res != 0: raise VrepError("Cannot get point cloud with error code " + str(res)) #start = time.clock() self.pointCloud.crop() #end = time.clock() #self.pointCloud.updateScanVector() # option 2 self.pointCloud.updateOccupancyMap() # option 1 #print('Time elapsed: ', end - start) self.VPL16_counter += 1 elif self.scene.SENSOR_TYPE == "kinect": pass else: return
setMotorPosition(clientID, steer_handle, 0) vrep.simxSynchronousTrigger(clientID) e = np.zeros((4, 1)) while not Reach: print("-------------------------------------------") vehLin = np.zeros(3) _, vehiclePos = vrep.simxGetObjectPosition(clientID, vehicle_handle, -1, vrep.simx_opmode_blocking) _, vehicleOri = vrep.simxGetObjectOrientation( clientID, vehicle_handle, -1, vrep.simx_opmode_blocking) _, vehicleLin, vehicleAng = vrep.simxGetObjectVelocity( clientID, vehicle_handle, vrep.simx_opmode_streaming) ## global coordinate vehLin[0] = (math.cos(vehicleOri[2]) * vehicleLin[0] + math.sin(vehicleOri[2]) * vehicleLin[1] ) ## Rotate Vx from world frame to vehicle frame vehLin[1] = (-math.sin(vehicleOri[2]) * vehicleLin[0] + math.cos(vehicleOri[2]) * vehicleLin[1] ) ## Rotate Vy form wold frame to vehicle frame vehLin[2] = vehicleLin[2] goalDist = np.linalg.norm(np.subtract(vehiclePos, goalPos)) if goalDist < 0.5: Reach = True
def show_speed(self): _, spe, _ = vrep.simxGetObjectVelocity(self.client_id, self.body, vrep.simx_opmode_oneshot) # speed print(sqrt(spe[0]**2 + spe[1]**2 + spe[2]**2), end='\r')
def __init__(self, episode_len=None): self.clientID = None # Start V-REP connection try: vrep.simxFinish(-1) print("Connecting to simulator...") self.clientID = vrep.simxStart('127.0.0.1', 19999, True, True, 5000, 5) if self.clientID == -1: print("Failed to connect to remote API Server") self.vrep_exit() except KeyboardInterrupt: self.vrep_exit() return self.episode_len = episode_len self.timestep = 0 self.dt = .001 self.propellers = [ 'rotor1thrust', 'rotor2thrust', 'rotor3thrust', 'rotor4thrust' ] self.quad_name = 'Quadricopter' self.scene_name = 'quad_innerloop.ttt' self.setpoint_delta = np.array( [0., 0., 1., 0., 0., 0., 0., 0., 0., 0., 0., 0.]) self.quad_handle = None self.pos_start = np.zeros(3) self.euler_start = np.zeros(3) self.vel_start = np.zeros(3) self.angvel_start = np.zeros(3) self.pos_old = self.pos_start self.euler_old = self.euler_start self.vel_old = self.vel_start self.angvel_old = self.angvel_start self.pos_new = self.pos_old self.euler_new = self.euler_old self.vel_new = self.vel_old self.angvel_new = self.angvel_old ob_high = np.array([ 10., 10., 10., 20., 20., 20., 21., 21., 21., 21., 21., 21., 10., 10., 10., 20., 20., 20., 21., 21., 21., 21., 21., 21. ]) ob_low = -ob_high self.observation_space = Box(low=ob_low, high=ob_high, dtype=np.float32) ac_high = np.array([20., 20., 20., 20.]) ac_low = np.zeros(4) self.action_space = Box(low=ac_low, high=ac_high, dtype=np.float32) print("Setting simulator to async mode...") vrep.simxSynchronous(self.clientID, True) vrep.simxSetFloatingParameter( self.clientID, vrep.sim_floatparam_simulation_time_step, self.dt, # specify a simulation time step vrep.simx_opmode_oneshot) # Load V-REP scene print("Loading scene...") vrep.simxLoadScene(self.clientID, self.scene_name, 0xFF, vrep.simx_opmode_blocking) # Get quadrotor handle err, self.quad_handle = vrep.simxGetObjectHandle( self.clientID, self.quad_name, vrep.simx_opmode_blocking) # Initialize quadrotor position and orientation vrep.simxGetObjectPosition(self.clientID, self.quad_handle, -1, vrep.simx_opmode_streaming) vrep.simxGetObjectOrientation(self.clientID, self.quad_handle, -1, vrep.simx_opmode_streaming) vrep.simxGetObjectVelocity(self.clientID, self.quad_handle, vrep.simx_opmode_streaming)
def get_linear_velocity(self): res, linear_vel, angular_vel = vrep.simxGetObjectVelocity( self.client_id, self.pioneer, vrep.simx_opmode_oneshot_wait) return linear_vel
def reset(self): vrep.simxStopSimulation(self.clientID, vrep.simx_opmode_oneshot_wait) time.sleep(0.1) # Setup V-REP simulation print("Setting simulator to async mode...") vrep.simxSynchronous(self.clientID, True) vrep.simxSetFloatingParameter( self.clientID, vrep.sim_floatparam_simulation_time_step, self.dt, # specify a simulation time step vrep.simx_opmode_oneshot) # Load V-REP scene print("Loading scene...") vrep.simxLoadScene(self.clientID, self.scene_name, 0xFF, vrep.simx_opmode_blocking) # Get quadrotor handle err, self.quad_handle = vrep.simxGetObjectHandle( self.clientID, self.quad_name, vrep.simx_opmode_blocking) # Initialize quadrotor position and orientation vrep.simxGetObjectPosition(self.clientID, self.quad_handle, -1, vrep.simx_opmode_streaming) vrep.simxGetObjectOrientation(self.clientID, self.quad_handle, -1, vrep.simx_opmode_streaming) vrep.simxGetObjectVelocity(self.clientID, self.quad_handle, vrep.simx_opmode_streaming) # Start simulation vrep.simxStartSimulation(self.clientID, vrep.simx_opmode_blocking) # Initialize rotors print("Initializing propellers...") for i in range(len(self.propellers)): vrep.simxClearFloatSignal(self.clientID, self.propellers[i], vrep.simx_opmode_oneshot) # Get quadrotor initial position and orientation err, self.pos_start = vrep.simxGetObjectPosition( self.clientID, self.quad_handle, -1, vrep.simx_opmode_buffer) err, self.euler_start = vrep.simxGetObjectOrientation( self.clientID, self.quad_handle, -1, vrep.simx_opmode_buffer) err, self.vel_start, self.angvel_start = vrep.simxGetObjectVelocity( self.clientID, self.quad_handle, vrep.simx_opmode_buffer) self.pos_start = np.asarray(self.pos_start) self.euler_start = np.asarray(self.euler_start) * 10. self.vel_start = np.asarray(self.vel_start) self.angvel_start = np.asarray(self.angvel_start) self.pos_old = self.pos_start self.euler_old = self.euler_start self.vel_old = self.vel_start self.angvel_old = self.angvel_start self.pos_new = self.pos_old self.euler_new = self.euler_old self.vel_new = self.vel_old self.angvel_new = self.angvel_old 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.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 self.pos_error_all = self.pos_error self.euler_error_all = self.euler_error self.init_f = 5.8 self.propeller_vels = [ self.init_f, self.init_f, self.init_f, self.init_f ] self.timestep = 1 return self.get_state()
gpsZ = vrep.simxGetFloatSignal(clientID, 'gpsZ', vrep.simx_opmode_oneshot)[1] #print("GPS : {:.5f} {:.5f} {:.5f}".format(gpsX, gpsY, gpsZ)) # Absolute Position Get errorCode, pos = vrep.simxGetObjectPosition(clientID, car_handle, -1, vrep.simx_opmode_streaming) #print("Position : {:.5f} {:.5f} {:.5f}\n".format(pos[0], pos[1], pos[2])) # Orientation Get errorCode, ori = vrep.simxGetObjectOrientation( clientID, car_handle, -1, vrep.simx_opmode_streaming) # Velocity errorCode, LinearV, AngularV = vrep.simxGetObjectVelocity( clientID, car_handle, vrep.simx_opmode_streaming if i is 1 else vrep.simx_opmode_buffer) # Distance errorCode, d = vrep.simxReadDistance(clientID, dist_handle, vrep.simx_opmode_streaming) # Simulation Time if t != vrep.simxGetLastCmdTime(clientID): t = vrep.simxGetLastCmdTime(clientID) if not i % 100: print(i) for col, data in enumerate([ pos[0], pos[1], pos[2], gyroX, gyroY, gyroZ, accelX,
def QC_controller(Quadricopter_target, Quadricopter_base, Quadricopter): try: import vrep except: print('--------------------------------------------------------------') print('"vrep.py" could not be imported. This means very probably that') print('either "vrep.py" or the remoteApi library could not be found.') print('Make sure both are in the same folder as this file,') print('or appropriately adjust the file "vrep.py"') print('--------------------------------------------------------------') print('') import time import numpy as np from captains_log_v1 import log_data import os import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D # output limits: #min_output=0 #max_output=8.335 # program parameters: global i i = 0 xe = [] ye = [] ze = [] xs = [] ys = [] zs = [] x_qc = [] y_qc = [] z_qc = [] u = [] v1 = [] v2 = [] v3 = [] v4 = [] #global variables: cumul = 0 last_e = 0 pAlphaE = 0 pBetaE = 0 psp2 = 0 psp1 = 0 prevEuler = 0 cumulAlpha = 0 cumulBeta = 0 cumulAlphaPos = 0 cumulBetaPos = 0 particlesTargetVelocities = [0, 0, 0, 0] # weight how much the parameters are changed each iteration: a = 0.1 # Compute derivative with respec to first parameter: delta = 0.01 # control signals parameters: last_thrust = 0 #speed weight: vParam = -2 #parameters for vertical control Kpv = 2 Kiv = 0 Kdv = 2 #parameters for horizontal control: Kph = 0.4 Kih = 0.1 Kdh = 1.5 Kph_pos1 = 0.4 Kih_pos1 = 0.001 Kdh_pos1 = 0.05 Kph_pos0 = 0.4 Kih_pos0 = 0.001 Kdh_pos0 = 0.05 #parameters for rotational control: Kpr = 0.05 Kir = 0 Kdr = 0.9 theta_v = [Kpv, Kiv, Kdv] theta_h = [ Kph, Kih, Kdh, Kph_pos1, Kih_pos1, Kdh_pos1, Kph_pos0, Kih_pos0, Kdh_pos0 ] theta_r = [Kpr, Kir, Kdr] def sum_lists(l1, l2): """ Arguments: list1, list2 """ ln = [] for i in xrange(len(l1)): ln.append(l1[i] + l2[i]) return ln def diff_lists(l1, l2): """ Arguments: list1, list2 """ ln = [] for i in xrange(len(l1)): ln.append(l1[i] - l2[i]) return ln def dot_multiply(l1, k): """ Arguments: list, scalar """ ln = [] for j in l1: pom = j * k ln.append(pom) return ln def cost(theta, sigma_err, delta_u): """ theta = [Kp, Ki, Kd] """ # step (step of sum TBD?): dt = 0.01 """ sigma_err = sum of errors -> integral/sum function -> These sums are gained in the code. """ # Compute the integral: t0 = 0 tf = 1 J = (1 / (tf - t0)) * (sigma_err**2 + delta_u**2) * dt return J def autotune(theta, var): # weight how much the parameters are changed each iteration: a = 0.1 # Compute derivative with respec to first parameter: delta = 0.01 theta_v = [Kpv, Kiv, Kdv] theta_h = [ Kph, Kih, Kdh, Kph_pos1, Kih_pos1, Kdh_pos1, Kph_pos0, Kih_pos0, Kdh_pos0 ] theta_r = [Kpr, Kir, Kdr] var_vi = [1, 0, 0] var_vj = [0, 1, 0] var_vk = [0, 0, 1] theta_vKp1 = sum_lists(theta_v, dot_multiply( var_vi, delta)) #theta_v + dot_multiply(var_vi,delta) theta_vKp2 = diff_lists(theta_v, dot_multiply( var_vi, delta)) #theta_v - dot_multiply(var_vi,delta) theta_vKi1 = sum_lists(theta_v, dot_multiply( var_vj, delta)) #theta_v + dot_multiply(var_vj,delta) theta_vKi2 = diff_lists(theta_v, dot_multiply( var_vj, delta)) #theta_v - dot_multiply(var_vj,delta) theta_vKd1 = sum_lists(theta_v, dot_multiply( var_vk, delta)) #theta_v + dot_multiply(var_vk,delta) theta_vKd2 = diff_lists(theta_v, dot_multiply( var_vk, delta)) #theta_v - dot_multiply(var_vk,delta) derivative_vKp = (cost(theta_vKp1, cumul, delta_thrust) - cost(theta_vKp2, cumul, delta_thrust)) / (2 * delta) derivative_vKi = (cost(theta_vKi1, cumul, delta_thrust) - cost(theta_vKi2, cumul, delta_thrust)) / (2 * delta) derivative_vKd = (cost(theta_vKd1, cumul, delta_thrust) - cost(theta_vKd2, cumul, delta_thrust)) / (2 * delta) theta_v1 = sum_lists(theta_v, dot_multiply(derivative_vKp, a)) # OVA IDE NA KRAJ OD LOOP theta_v2 = sum_lists(theta_v, dot_multiply(derivative_vKi, a)) theta_v3 = sum_lists(theta_v, dot_multiply(derivative_vKd, a)) theta_v = min(theta_v1, theta_v2, theta_v3) derivative_h = (cost() - cost()) / (2 * delta) derivative_r = (cost() - cost()) / (2 * delta) """ SIMULATION: theta = [Kp Ki Kd] => sim(theta) """ theta_new = theta - dot_multiply(derivative, a) #a*derivative print theta_new return theta_new 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_blocking) #functional/handle code: emptyBuff = bytearray() [returnCode, targetObj] = vrep.simxGetObjectHandle(clientID, Quadricopter_target, vrep.simx_opmode_blocking) [returnCode, qc_base_handle] = vrep.simxGetObjectHandle(clientID, Quadricopter_base, vrep.simx_opmode_blocking) [returnCode, qc_handle] = vrep.simxGetObjectHandle(clientID, Quadricopter, vrep.simx_opmode_blocking) # main loop: while True: #theta_v = [Kpv, Kiv, Kdv] '''==========''' ''' vertical control: ''' '''==========''' [returnCode, targetPos] = vrep.simxGetObjectPosition(clientID, targetObj, -1, vrep.simx_opmode_blocking) [returnCode, pos] = vrep.simxGetObjectPosition(clientID, qc_base_handle, -1, vrep.simx_opmode_blocking) [returnCode, l, w] = vrep.simxGetObjectVelocity(clientID, qc_base_handle, vrep.simx_opmode_blocking) e = targetPos[2] - pos[2] cumul = cumul + e diff_e = e - last_e Pvert = theta_v[0] * e Ivert = theta_v[1] * cumul Dvert = theta_v[2] * diff_e thrust = 5.335 + Pvert + Ivert + Dvert + l[2] * vParam # get thrust delta_thrust = thrust - last_thrust last_thrust = thrust last_e = e # autotune: # """ Code for self-tuning """ # var_vi = [1,0,0] # var_vj = [0,1,0] # var_vk = [0,0,1] # # theta_vKp1 = sum_lists(theta_v,dot_multiply(var_vi,delta))#theta_v + dot_multiply(var_vi,delta) # theta_vKp2 = diff_lists(theta_v,dot_multiply(var_vi,delta)) #theta_v - dot_multiply(var_vi,delta) # theta_vKi1 = sum_lists(theta_v,dot_multiply(var_vj,delta)) #theta_v + dot_multiply(var_vj,delta) # theta_vKi2 = diff_lists(theta_v,dot_multiply(var_vj,delta)) #theta_v - dot_multiply(var_vj,delta) # theta_vKd1 = sum_lists(theta_v,dot_multiply(var_vk,delta)) #theta_v + dot_multiply(var_vk,delta) # theta_vKd2 = diff_lists(theta_v,dot_multiply(var_vk,delta)) #theta_v - dot_multiply(var_vk,delta) # # derivative_vKp = (cost(theta_vKp1,cumul,delta_thrust) - cost(theta_vKp2,cumul,delta_thrust))/(2*delta) # derivative_vKi = (cost(theta_vKi1,cumul,delta_thrust) - cost(theta_vKi2,cumul,delta_thrust))/(2*delta) # derivative_vKd = (cost(theta_vKd1,cumul,delta_thrust) - cost(theta_vKd2,cumul,delta_thrust))/(2*delta) # # print derivative_vKp # theta_v1 = sum_lists(theta_v,dot_multiply(derivative_vKp,a))# OVA IDE NA KRAJ OD LOOP # theta_v2 = sum_lists(theta_v,dot_multiply(derivative_vKi,a)) # theta_v3 = sum_lists(theta_v,dot_multiply(derivative_vKd,a)) # theta_v = min(theta_v1, theta_v2, theta_v3) # print " THETA_V =", theta_v # theta = [Kpv, Kiv, Kdv] # sigma_err = cumul # autotune(theta, sigma_err) '''==========''' ''' horizontal control: ''' '''==========''' [returnCode, sp] = vrep.simxGetObjectPosition(clientID, targetObj, qc_base_handle, vrep.simx_opmode_blocking) [rc, rc, vx, rc, rc] = vrep.simxCallScriptFunction( clientID, Quadricopter, vrep.sim_scripttype_childscript, 'qc_Get_vx', [], [], [], emptyBuff, vrep.simx_opmode_blocking) [rc, rc, vy, rc, rc] = vrep.simxCallScriptFunction( clientID, Quadricopter, vrep.sim_scripttype_childscript, 'qc_Get_vy', [], [], [], emptyBuff, vrep.simx_opmode_blocking) [rc, rc, rc, rc, rc] = vrep.simxCallScriptFunction(clientID, Quadricopter, vrep.sim_scripttype_childscript, 'qc_Get_Object_Matrix', [], [], [], emptyBuff, vrep.simx_opmode_blocking) [errorCode, M] = vrep.simxGetStringSignal(clientID, 'mtable', vrep.simx_opmode_oneshot_wait) if (errorCode == vrep.simx_return_ok): m = vrep.simxUnpackFloats(M) alphaE = vy[2] - m[11] cumulAlpha = cumulAlpha + alphaE diff_alphaE = alphaE - pAlphaE alphaCorr = Kph * alphaE + Kih * cumulAlpha + Kdh * diff_alphaE #alpha correction betaE = vx[2] - m[11] cumulBeta = cumulBeta + betaE diff_betaE = betaE - pBetaE betaCorr = -Kph * betaE - Kih * cumulBeta - Kdh * diff_betaE #beta correction pAlphaE = alphaE pBetaE = betaE cumulAlphaPos = cumulAlphaPos + sp[1] cumulBetaPos = cumulBetaPos + sp[0] alphaPos = Kph_pos1 * ( sp[1]) + Kih_pos1 * cumulAlphaPos + Kdh_pos1 * ( sp[1] - psp2) #alpha position correction betaPos = Kph_pos0 * ( sp[0]) + Kih_pos0 * cumulBetaPos + Kdh_pos0 * ( sp[0] - psp1) #beta position correction alphaCorr = alphaCorr + alphaPos betaCorr = betaCorr - betaPos psp2 = sp[1] psp1 = sp[0] '''==========''' ''' rotational control: ''' '''==========''' [returnCode, euler] = vrep.simxGetObjectOrientation(clientID, targetObj, qc_base_handle, vrep.simx_opmode_blocking) [returnCode, orientation ] = vrep.simxGetObjectOrientation(clientID, qc_base_handle, -1, vrep.simx_opmode_blocking) Prot = Kpr * euler[2] Drot = Kdr * (euler[2] - prevEuler) rotCorr = Prot + Drot prevEuler = euler[2] # set propeller velocities: propeller1_PTV = thrust * (1 - alphaCorr + betaCorr - rotCorr) propeller2_PTV = thrust * (1 - alphaCorr - betaCorr + rotCorr) propeller3_PTV = thrust * (1 + alphaCorr - betaCorr - rotCorr) propeller4_PTV = thrust * (1 + alphaCorr + betaCorr + rotCorr) particlesTargetVelocities = [ propeller1_PTV, propeller2_PTV, propeller3_PTV, propeller4_PTV ] '''=============================================================''' C_parameters_vert = [0, 0, 0] C_parameters_horr = [0, 0, 0, 0, 0, 0, 0] C_parameters_rot = [0, 0, 0] vert_comp = [0, 0, 0, 0] horr_comp = [0, 0, 0, 0, [0, 0, 0], 0, 0, 0, 0, 0, 0] rot_comp = [[0, 0, 0], 0] C_parameters_vert[0] = Kpv C_parameters_vert[1] = Kiv C_parameters_vert[2] = Kdv C_parameters_horr[0] = Kph C_parameters_horr[1] = Kih C_parameters_horr[2] = Kdh C_parameters_horr[3] = Kph_pos0 C_parameters_horr[4] = Kdh_pos0 C_parameters_horr[5] = Kph_pos1 C_parameters_horr[6] = Kdh_pos1 C_parameters_rot[0] = Kpr C_parameters_rot[1] = Kir C_parameters_rot[2] = Kdr vert_comp[0] = targetPos vert_comp[1] = pos vert_comp[2] = e vert_comp[3] = thrust horr_comp[0] = alphaE horr_comp[1] = betaE horr_comp[2] = cumulAlpha horr_comp[3] = cumulBeta horr_comp[4] = sp horr_comp[5] = cumulAlphaPos horr_comp[6] = cumulBetaPos horr_comp[7] = alphaCorr horr_comp[8] = betaCorr horr_comp[9] = vx horr_comp[10] = vy rot_comp[0] = euler rot_comp[1] = rotCorr '''==========''' ''' PLOTTING ''' '''==========''' ze.append(e) # otstapuvanje od z-oska xe.append(sp[0]) # otstapuvanje od x-oska ye.append(sp[1]) # otstapuvawe od y-oska xs.append(targetPos[0]) ys.append(targetPos[1]) zs.append(targetPos[2]) x_qc.append(pos[0]) y_qc.append(pos[1]) z_qc.append(pos[2]) fig = plt.figure() ax = fig.add_subplot(111, projection='3d') # ax.xaxis.label.set_color('blue') # ax.yaxis.label.set_color('blue') # ax.zaxis.label.set_color('blue') # ax.tick_params(axis='xs',colors='blue') # ax.tick_params(axis='ys',colors='blue') # ax.tick_params(axis='zs',colors='blue') ax.plot_wireframe(xs, ys, zs, color='blue') # plt.hold(True) # ax.xaxis.label.set_color('red') # ax.yaxis.label.set_color('red') # ax.zaxis.label.set_color('red') # ax.tick_params(axis='x_qc',colors='red') # ax.tick_params(axis='y_qc',colors='red') # ax.tick_params(axis='z_qc',colors='red') ax.plot_wireframe(x_qc, y_qc, z_qc, color='red') plt.show() # u.append(thrust) # v1.append(propeller1_PTV) # v2.append(propeller2_PTV) # v3.append(propeller3_PTV) # v4.append(propeller4_PTV) # # plt.plot(v1,color='blue') # plt.hold(True) # plt.plot(v2,color='red') # plt.hold(True) # plt.plot(v3,color='green') # plt.hold(True) # plt.plot(v4,color='pink') # plt.hold(True) # plt.axis([0,25,0,15]) # plt.show() # # plt.plot(xe,color='blue') # plt.axis([0,100,-4,4]) # plt.show() # plt.plot(ye,color='red') # plt.axis([0,100,-4,4]) # plt.show() # plt.plot(ze,color='green') # plt.axis([0,100,-4,4]) # plt.show() '''=============================================================''' # print '////////////' # print '------------' # print '-Controller parameters-' # print 'Vertical control parameters=',[Kpv,Kiv,Kdv] # print 'Horizontal control parameters=',[Kph,Kih,Kdh,Kph_pos0,Kdh_pos0,Kph_pos1,Kdh_pos1] # print 'Rotational control parameters=',[Kpr,Kir,Kdr] # print '------------' # print '-Vertical component-' # print 'targetPos=',targetPos # print 'pos=',pos # print 'e=',e # print 'thrust=',thrust # print '------------' # print '-Horizontal component-' # print 'cumulAlpha=',cumulAlpha # print 'cumulBeta=',cumulBeta # print 'cumulAlphaPos=',cumulAlphaPos # print 'cumulBetaPos=',cumulBetaPos # print 'alphaE=',alphaE # print 'betaE=',betaE # print 'alphaCorr=',alphaCorr # print 'betaCorr=',betaCorr # print 'orientation=',orientation # print 'sp_X=',sp[0] # print 'sp_Y=',sp[1] # print '------------' # print '-Rotational component-' # print 'vx=',vx # print 'vy=',vy # print 'gamma=',euler[2] # print 'rotCorr=',rotCorr # print '------------' # print 'Velocity_propeller_1 = ',particlesTargetVelocities[0] # print 'Velocity_propeller_2 = ',particlesTargetVelocities[1] # print 'Velocity_propeller_3 = ',particlesTargetVelocities[2] # print 'Velocity_propeller_4 = ',particlesTargetVelocities[3] # print '------------' # print '////////////' '''=============================================================''' ## WRITE TO TEXT: log_data(C_parameters_vert, C_parameters_horr, C_parameters_rot, vert_comp, horr_comp, rot_comp, particlesTargetVelocities, i) i += 1 # send propeller velocities to output: [res, retInts, retFloats, retStrings, retBuffer] = vrep.simxCallScriptFunction( clientID, Quadricopter, vrep.sim_scripttype_childscript, 'qc_propeller_v', [], particlesTargetVelocities, [], emptyBuff, vrep.simx_opmode_blocking) vrep.simxSynchronousTrigger(clientID) # stop the simulation: vrep.simxStopSimulation(clientID, vrep.simx_opmode_blocking) # Now close the connection to V-REP: vrep.simxFinish(clientID) else: print('Failed connecting to remote API server')
# start the simulation: vrep.simxStartSimulation(clientID,vrep.simx_opmode_blocking) #functional/handle code: emptyBuff=bytearray() [returnCode,targetObj]=vrep.simxGetObjectHandle(clientID,'Quadricopter_target',vrep.simx_opmode_blocking) [returnCode,qc_base_handle]=vrep.simxGetObjectHandle(clientID,'Quadricopter_base',vrep.simx_opmode_blocking) [returnCode,qc_handle]=vrep.simxGetObjectHandle(clientID,'Quadricopter',vrep.simx_opmode_blocking) # main loop: while True: # vertical control: [returnCode,targetPos]=vrep.simxGetObjectPosition(clientID,targetObj,-1,vrep.simx_opmode_blocking) [returnCode,pos]=vrep.simxGetObjectPosition(clientID,qc_base_handle,-1,vrep.simx_opmode_blocking) [returnCode, l, w] = vrep.simxGetObjectVelocity(clientID, qc_base_handle, vrep.simx_opmode_blocking) e=targetPos[2]-pos[2] cumul=cumul+e diff_e=e-last_e Pvert=Kpv*e Ivert=Kiv*cumul Dvert=Kdv*diff_e thrust=5.335+Pvert+Ivert+Dvert+l[2]*vParam# get thrust last_e=e # horizontal control: [returnCode,sp]=vrep.simxGetObjectPosition(clientID,targetObj,qc_base_handle,vrep.simx_opmode_blocking) [rc,rc,vx,rc,rc]=vrep.simxCallScriptFunction(clientID,'Quadricopter',vrep.sim_scripttype_childscript,'qc_Get_vx',[],[],[],emptyBuff,vrep.simx_opmode_blocking) [rc,rc,vy,rc,rc]=vrep.simxCallScriptFunction(clientID,'Quadricopter',vrep.sim_scripttype_childscript,'qc_Get_vy',[],[],[],emptyBuff,vrep.simx_opmode_blocking) [rc,rc,rc,rc,rc]=vrep.simxCallScriptFunction(clientID,'Quadricopter',vrep.sim_scripttype_childscript,'qc_Get_Object_Matrix',[],[],[],emptyBuff,vrep.simx_opmode_blocking) [errorCode,M]=vrep.simxGetStringSignal(clientID,'mtable',vrep.simx_opmode_oneshot_wait);
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: f.write("dis: "+str(round(max_dis,2))+"\n\n") print("dis: "+str(round(max_dis,2))+"\n") break if time.time() - start > 7: f.write("time: inf\n") f.write("dis: "+str(round(max_dis,2))+"\n\n") print("dis: "+str(round(max_dis,2))) print("time: inf\n")
def velocity(cid, handle): err, lin, ang = vrep.simxGetObjectVelocity(cid, handle, vrep_mode) return [lin[0], lin[1], lin[2], ang[0], ang[1], ang[2]]
def collectImageData(ca_model, pn_model, clientID, states, input_type, use_ca): if use_ca: ca_model.eval() pn_model.eval() list_of_images = [] if (input_type == 0 or input_type == 2 or input_type == 3) and use_ca: vid_states = states[0] st_states = states[1] elif input_type == 1: if use_ca: vid_states = states[2] st_states = states[3] pn_vidstates = states[0] pn_ststates = states[1] collector = [] if clientID != -1: err, tracer_handle = vrep.simxGetObjectHandle( clientID, 'LineTracer', vrep.simx_opmode_oneshot_wait) res, v0 = vrep.simxGetObjectHandle(clientID, 'Vision_sensor', vrep.simx_opmode_oneshot_wait) res, v1 = vrep.simxGetObjectHandle(clientID, 'PassiveVision_sensor', vrep.simx_opmode_oneshot_wait) ret_code, left_handle = vrep.simxGetObjectHandle( clientID, 'DynamicLeftJoint', vrep.simx_opmode_oneshot_wait) ret_code, right_handle = vrep.simxGetObjectHandle( clientID, 'DynamicRightJoint', vrep.simx_opmode_oneshot_wait) ret_code, base_handle = vrep.simxGetObjectHandle( clientID, 'LineTracerBase', vrep.simx_opmode_oneshot_wait) res, resolution, image = vrep.simxGetVisionSensorImage( clientID, v0, 0, vrep.simx_opmode_streaming) ret_code, euler_angles = vrep.simxGetObjectOrientation( clientID, tracer_handle, -1, vrep.simx_opmode_streaming) t_end = time.time() + 5.2 count = 0 action = 0 inference_counter = 0 tim = 0 while (vrep.simxGetConnectionId(clientID) != -1 and time.time() < t_end): res, resolution, image = vrep.simxGetVisionSensorImage( clientID, v0, 0, vrep.simx_opmode_buffer) if res == vrep.simx_return_ok: img = np.array(image, dtype=np.uint8) img.resize([resolution[1], resolution[0], 3]) rotate_img = img.copy() rotate_img = np.flipud(img) list_of_images.append(rotate_img) ret_code, pos = vrep.simxGetObjectPosition( clientID, tracer_handle, -1, vrep.simx_opmode_oneshot) ret_code, velo, angle_velo = vrep.simxGetObjectVelocity( clientID, tracer_handle, vrep.simx_opmode_oneshot) ret_code, euler_angles = vrep.simxGetObjectOrientation( clientID, tracer_handle, -1, vrep.simx_opmode_buffer) collector.append([ pos[0], pos[1], pos[2], velo[0], velo[1], velo[2], angle_velo[0], angle_velo[1], angle_velo[2], euler_angles[0], euler_angles[1], euler_angles[2], action ]) if use_ca: torch_vid = torch.from_numpy( np.transpose( np.expand_dims( (list_of_images[-1]).astype('float'), axis=0), (0, 3, 1, 2))) torch_st = torch.from_numpy( np.asarray(collector[-1]).astype('float')) vid_to_ca = Variable(torch_vid.float().cuda(), volatile=True) st_to_ca = Variable(torch_st.float().cuda(), volatile=True) output, vid_states, st_states = ca_model( vid_to_ca, st_to_ca, vid_states, st_states) output.detach_() inference_counter += 1 if input_type == 0: if count == 0: a = torch.from_numpy( list_of_images[-1].flatten()).float().cuda() b = torch.from_numpy( list_of_images[-1].flatten()).float().cuda() c = torch.from_numpy( np.asarray( collector[-1]).astype('float')).float().cuda() if use_ca: d = torch.squeeze(output.data).float().cuda() input_to_model = torch.cat([a, b, c, d]) else: input_to_model = torch.cat([a, b, c]) else: a = torch.from_numpy( list_of_images[-1].flatten()).float().cuda() b = torch.from_numpy( list_of_images[-2].flatten()).float().cuda() c = torch.from_numpy( np.asarray( collector[-1]).astype('float')).float().cuda() if use_ca: d = torch.squeeze(output.data).float().cuda() input_to_model = torch.cat([a, b, c, d]) else: input_to_model = torch.cat([a, b, c]) input_to_model = Variable(input_to_model, volatile=True) out = pn_model(input_to_model) elif input_type == 1: pn_vid = torch.from_numpy( np.transpose( np.expand_dims( (list_of_images[-1]).astype('float'), axis=0), (0, 3, 1, 2))) vid_input = Variable(pn_vid.float().cuda(), volatile=True) c = torch.from_numpy( np.asarray( collector[-1]).astype('float')).float().cuda() if use_ca: d = torch.squeeze(output.data).float().cuda() st_input = Variable(torch.cat([c, d]).unsqueeze(0), volatile=True) else: st_input = Variable(c.unsqueeze(0), volatile=True) out, pn_vidstates, pn_ststates = pn_model( vid_input, st_input, pn_vidstates, pn_ststates) elif input_type == 2 or input_type == 3: #stack two images together and I need to verify the images being stacked are reasonable if count == 0: stacked_img = np.concatenate( (np.transpose( np.expand_dims( (list_of_images[-1]).astype('float'), axis=0), (0, 3, 1, 2)), np.transpose( np.expand_dims( (list_of_images[-1]).astype('float'), axis=0), (0, 3, 1, 2))), axis=1) vid_input = Variable( torch.from_numpy(stacked_img).float().cuda(), volatile=True) c = torch.from_numpy( np.asarray( collector[-1]).astype('float')).float().cuda() if use_ca: d = torch.squeeze(output.data).float().cuda() st_input = Variable(torch.cat([c, d]).unsqueeze(0), volatile=True) else: st_input = Variable(c.unsqueeze(0), volatile=True) else: stacked_img = np.concatenate( (np.transpose( np.expand_dims( (list_of_images[-1]).astype('float'), axis=0), (0, 3, 1, 2)), np.transpose( np.expand_dims( (list_of_images[-2]).astype('float'), axis=0), (0, 3, 1, 2))), axis=1) vid_input = Variable( torch.from_numpy(stacked_img).float().cuda(), volatile=True) c = torch.from_numpy( np.asarray( collector[-1]).astype('float')).float().cuda() if use_ca: d = torch.squeeze(output.data).float().cuda() st_input = Variable(torch.cat([c, d]).unsqueeze(0), volatile=True) else: st_input = Variable(c.unsqueeze(0), volatile=True) out = pn_model(st_input, vid_input) else: print("Error 12") sys.exit() action = out.max(1)[1] velo = (action - 2) * 15 return_val = vrep.simxSetJointTargetVelocity( clientID, left_handle, velo, vrep.simx_opmode_oneshot) return_val2 = vrep.simxSetJointTargetVelocity( clientID, right_handle, velo, vrep.simx_opmode_oneshot_wait) count += 1 return list_of_images, collector else: sys.exit()
simDiscreteTime += dt * simStep for i in range(0, jointNum - 1): # 将控制器参数转化为关节角 vJointSet[i] = controlFunction.sin_controller(i, A[i], omega[i], B[i], phi[i], simDiscreteTime) vrep.c_SetJointTargetPosition(clientID, vJointHandle[i], vJointSet[i], vrep.simx_opmode_oneshot) # 读取各个模块的速度及位置 for i in range(0, moduleNum - 1): _, jpos = vrep.simxGetObjectPosition(clientID, moduleHandle[i], -1, vrep.simx_opmode_oneshot) modulePositionSensor[i, :] = jpos # print("modulePosition[",i,"] = ", modulePositionSensor[i]) _, vel, ang = vrep.simxGetObjectVelocity(clientID, moduleHandle[0], vrep.simx_opmode_oneshot) # print("velocity = ", vel) data.append((vel[0], vel[1], vel[2], vJointSet[0], vJointSet[1], vJointSet[2], vJointSet[3], vJointSet[4], vJointSet[5], vJointSet[6], vJointSet[7], velAverageValue.x[0])) # 当前速度 vc = vel[0] * vel[0] + vel[1] * vel[1] vc = math.sqrt(vc) # 求N次采样的平均速度 # velSample.x = velSample.x*(steps-1)/steps+(1/steps)*vc # 求近10次采样平均 if steps < 10: velSample.x[0, steps] = vc else: for i in range(1, 9):
time.sleep(1) cid = connect() time.sleep(1) vrep.simxLoadScene(cid,'/home/elias/[email protected]/_Winter2015/CSC494/Scenes/Base_Quad.ttt',0,mode()) vrep.simxStartSimulation(cid,mode()) runtime = 0 err, copter = vrep.simxGetObjectHandle(cid, "Quadricopter_base", vrep.simx_opmode_oneshot_wait) err, target = vrep.simxGetObjectHandle(cid, "Quadricopter_target", vrep.simx_opmode_oneshot_wait) err, front_camera = vrep.simxGetObjectHandle(cid, 'Quadricopter_frontCamera', vrep.simx_opmode_oneshot) err, lin, ang = vrep.simxGetObjectVelocity(cid, copter, vrep.simx_opmode_streaming) err, orgpos = getTargetStart(cid,target) real_args(args,orgpos ) args["addxToZ"] = [orgpos, 2.5] args["fuckIt"] = [orgpos, 2.5] #This prepared the data print(err) #while(1): while(runtime <= 10):
(err, lwmotor) = vrep.simxGetObjectHandle(clientID,"K3_leftWheelMotor#",vrep.simx_opmode_oneshot_wait) oldJoystick = data.find_one({"item": "joystick"}) while angleModel[1]==0: (err, orientation) = vrep.simxGetObjectOrientation(clientID,robot,-1,vrep.simx_opmode_oneshot) angleModel[1]= orientation[1] angleModel[2] = abs(angleModel[1]) while clientID!=-1: joystick = data.find_one({"_id": 0}) x = float(joystick['x']) y = float(joystick['y']) lockMode = str(joystick['mode']) (err, position) = vrep.simxGetObjectPosition(clientID,robot,-1,vrep.simx_opmode_oneshot) (err, orientation) = vrep.simxGetObjectOrientation(clientID,robot,-1,vrep.simx_opmode_oneshot) (err, speed, angularSpeed) = vrep.simxGetObjectVelocity(clientID,robot,vrep.simx_opmode_oneshot) angleModel[0] = orientation[1] (velizq, velder, targetReached) = setSpeeds(x, y, angleModel, lockMode) if (joystick != oldJoystick) or targetReached: oldJoystick = joystick #print('x: '+str(x)+' y: '+str(y)+' vel: izq '+str(velizq)+' der '+str(velder)+' Lock Mode: '+str(lockMode)) vrep.simxSetJointTargetVelocity(clientID,lwmotor,velizq,vrep.simx_opmode_oneshot) vrep.simxSetJointTargetVelocity(clientID,rwmotor,velder,vrep.simx_opmode_oneshot) if (position != oldPosition): data.update({"item": "robotData"}, {"$set":{"position": {"x": round(position[0],4), "y": round(position[1],4), "z": round(position[2],4)}}}) oldPosition = position if (orientation!= oldOrientation): data.update({"item": "robotData"}, {"$set":{"orientation": {"alpha": round(math.degrees(orientation[0]),2), "beta": round(math.degrees(orientation[1]),2), "gamma": round(math.degrees(orientation[2]),2)}}}) oldOrientation = orientation if (speed!= oldSpeed):
res, servo_front = vrep.simxGetObjectHandle(clientID, 'Front_rev', vrep.simx_opmode_blocking) res, servo_back = vrep.simxGetObjectHandle(clientID, 'Back_rev', vrep.simx_opmode_blocking) res, servo_middle = vrep.simxGetObjectHandle(clientID, 'Middle_rev', vrep.simx_opmode_blocking) res, body_back = vrep.simxGetObjectHandle(clientID, 'Back', vrep.simx_opmode_blocking) print('dude') t = time.time() z = 0 v = 0 while (time.time() - t) <= 100: res, linearVelocity, angularVelocity = vrep.simxGetObjectVelocity( clientID, body_back, vrep.simx_opmode_streaming) print('vy', linearVelocity[1]) z = z + 1 v = v + (linearVelocity[1]) #plt.plot(time.time(),linearVelocity[1],'go--') #plt.xlabel('time(s)') #plt.ylabel('velocity(m/s)') #plt.grid(True) #plt.hold(True) tf, tr, tm = phase(time.time()) vrep.simxSetJointTargetPosition(clientID, servo_front, tf,
def QC_controller(Quadricopter_target, Quadricopter_base, Quadricopter): try: import vrep except: print ('--------------------------------------------------------------') print ('"vrep.py" could not be imported. This means very probably that') print ('either "vrep.py" or the remoteApi library could not be found.') print ('Make sure both are in the same folder as this file,') print ('or appropriately adjust the file "vrep.py"') print ('--------------------------------------------------------------') print ('') """ =========================================================== """ """ ============ Imported Libraries: ============ """ import time import numpy as np from captains_log_v1 import log_data import os import cost import csv import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D """ =================================================================== """ # output limits: #min_output=0 #max_output=8.335 # program parameters: global i i = 0 xe = [] ye = [] ze = [] xs = [] ys = [] zs = [] x_qc = [] y_qc = [] z_qc = [] u = [] v1 = [] v2 = [] v3 = [] v4 = [] #global variables: cumul=0 last_e=0 pAlphaE=0 pBetaE=0 psp2=0 psp1=0 prevEuler=0 cumulAlpha = 0 cumulBeta = 0 cumulAlphaPos = 0 cumulBetaPos = 0 s_r = 0 particlesTargetVelocities=[0,0,0,0] #speed weight: vParam=-2 #parameters for vertical control Kpv=2 Kiv=0 Kdv=2 #parameters for horizontal control: Kph=0.4 # TBD Kih=0.1 # TBD Kdh=1.5 Kph_pos1=0.4 Kih_pos1=0.001 Kdh_pos1=0.05 Kph_pos0=0.4 Kih_pos0=0.001 Kdh_pos0=0.05 #parameters for rotational control: Kpr=0.05 Kir=0 Kdr=0.9 """ =========================================================== """ # parameters needed for gradient descent: t0 = 0 tf = 1 dt = 0.01 sum_h_alpha = [] sum_h_beta = [] sum_h_pos0 = [] sum_h_pos1 = [] last_alpha_angle = 0 last_alpha_pos = 0 last_beta_angle = 0 last_beta_pos = 0 delta_alpha_angle = [] delta_alpha_pos = [] delta_beta_angle = [] delta_beta_pos = [] J_h_alpha = [] J_h_beta = [] J_h_pos0 = [] J_h_pos1 = [] paramX = [] paramY = [] theta_h_angles = [Kph, Kih, Kdh] theta_h_pos = [Kph_pos0, Kih_pos0, Kdh_pos0] # Kph_pos0 == Kph_pos1 ... gd = 0 """ =========================================================== """ 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_blocking) #functional/handle code: emptyBuff=bytearray() [returnCode,targetObj]=vrep.simxGetObjectHandle(clientID,Quadricopter_target,vrep.simx_opmode_blocking) [returnCode,qc_base_handle]=vrep.simxGetObjectHandle(clientID,Quadricopter_base,vrep.simx_opmode_blocking) [returnCode,qc_handle]=vrep.simxGetObjectHandle(clientID,Quadricopter,vrep.simx_opmode_blocking) # main loop: while True: """ ========== vertical control: ========== """ [returnCode,targetPos]=vrep.simxGetObjectPosition(clientID,targetObj,-1,vrep.simx_opmode_blocking) [returnCode,pos]=vrep.simxGetObjectPosition(clientID,qc_base_handle,-1,vrep.simx_opmode_blocking) [returnCode, l, w] = vrep.simxGetObjectVelocity(clientID, qc_base_handle, vrep.simx_opmode_blocking) e=targetPos[2]-pos[2] cumul=cumul+e diff_e=e-last_e Pvert=Kpv*e Ivert=Kiv*cumul Dvert=Kdv*diff_e thrust=5.335+Pvert+Ivert+Dvert+l[2]*vParam# get thrust last_e=e """ =========================================================== """ """ ========== horizontal control: ========== """ [returnCode,sp]=vrep.simxGetObjectPosition(clientID,targetObj,qc_base_handle,vrep.simx_opmode_blocking) [rc,rc,vx,rc,rc]=vrep.simxCallScriptFunction(clientID,Quadricopter,vrep.sim_scripttype_childscript,'qc_Get_vx',[],[],[],emptyBuff,vrep.simx_opmode_blocking) [rc,rc,vy,rc,rc]=vrep.simxCallScriptFunction(clientID,Quadricopter,vrep.sim_scripttype_childscript,'qc_Get_vy',[],[],[],emptyBuff,vrep.simx_opmode_blocking) [rc,rc,rc,rc,rc]=vrep.simxCallScriptFunction(clientID,Quadricopter,vrep.sim_scripttype_childscript,'qc_Get_Object_Matrix',[],[],[],emptyBuff,vrep.simx_opmode_blocking) [errorCode,M]=vrep.simxGetStringSignal(clientID,'mtable',vrep.simx_opmode_oneshot_wait); if (errorCode==vrep.simx_return_ok): m=vrep.simxUnpackFloats(M) alphaE=vy[2]-m[11] cumulAlpha = cumulAlpha + alphaE diff_alphaE=alphaE-pAlphaE alphaCorr=Kph*alphaE + Kih*cumulAlpha + Kdh*diff_alphaE #alpha correction betaE=vx[2]-m[11] cumulBeta = cumulBeta + betaE diff_betaE=betaE-pBetaE betaCorr=-Kph*betaE - Kih*cumulBeta - Kdh*diff_betaE #beta correction pAlphaE=alphaE pBetaE=betaE cumulAlphaPos = cumulAlphaPos + sp[1] cumulBetaPos = cumulBetaPos + sp[0] alphaPos=Kph_pos1*(sp[1])+ Kih_pos1*cumulAlphaPos +Kdh_pos1*(sp[1]-psp2) #alpha position correction betaPos=Kph_pos0*(sp[0])+ Kih_pos0*cumulBetaPos + Kdh_pos0*(sp[0]-psp1) #beta position correction alphaCorr=alphaCorr+alphaPos betaCorr=betaCorr-betaPos psp2=sp[1] psp1=sp[0] delta_alpha_angle.append(alphaCorr - last_alpha_angle) delta_beta_angle.append(betaCorr - last_beta_angle) delta_alpha_pos.append(alphaPos - last_alpha_pos) delta_beta_pos.append(betaPos - last_beta_pos) last_alpha_angle = alphaCorr last_beta_angle = betaCorr last_alpha_pos = alphaPos last_beta_pos = betaPos """ =========================================================== """ """ ========== rotational control: ========== """ [returnCode,euler]=vrep.simxGetObjectOrientation(clientID,targetObj,qc_base_handle,vrep.simx_opmode_blocking) [returnCode,orientation]=vrep.simxGetObjectOrientation(clientID,qc_base_handle,-1,vrep.simx_opmode_blocking) Prot=Kpr*euler[2] Drot=Kdr*(euler[2]-prevEuler) s_r = s_r + euler[2] rotCorr=Prot+Drot prevEuler=euler[2] """ =========================================================== """ """ ========== set propeller velocities: ========== """ propeller1_PTV = thrust*(1-alphaCorr+betaCorr-rotCorr) propeller2_PTV = thrust*(1-alphaCorr-betaCorr+rotCorr) propeller3_PTV = thrust*(1+alphaCorr-betaCorr-rotCorr) propeller4_PTV = thrust*(1+alphaCorr+betaCorr+rotCorr) particlesTargetVelocities=[propeller1_PTV, propeller2_PTV, propeller3_PTV, propeller4_PTV] """ =========================================================== """ """ ========== set parameters for logging data: ========== """ C_parameters_vert = [0,0,0] C_parameters_horr = [0,0,0,0,0,0,0] C_parameters_rot = [0,0,0] vert_comp = [0,0,0,0] horr_comp = [0,0,0,0,[0,0,0],0,0,0,0,0,0] rot_comp = [[0,0,0],0] C_parameters_vert[0] = Kpv C_parameters_vert[1] = Kiv C_parameters_vert[2] = Kdv C_parameters_horr[0] = Kph C_parameters_horr[1] = Kih C_parameters_horr[2] = Kdh C_parameters_horr[3] = Kph_pos0 C_parameters_horr[4] = Kdh_pos0 C_parameters_horr[5] = Kph_pos1 C_parameters_horr[6] = Kdh_pos1 C_parameters_rot[0] = Kpr C_parameters_rot[1] = Kir C_parameters_rot[2] = Kdr vert_comp[0] = targetPos vert_comp[1] = pos vert_comp[2] = e vert_comp[3] = thrust horr_comp[0] = alphaE horr_comp[1] = betaE horr_comp[2] = cumulAlpha horr_comp[3] = cumulBeta horr_comp[4] = sp horr_comp[5] = cumulAlphaPos horr_comp[6] = cumulBetaPos horr_comp[7] = alphaCorr horr_comp[8] = betaCorr horr_comp[9] = vx horr_comp[10] = vy rot_comp[0] = euler rot_comp[1] = rotCorr """ =========================================================== """ """ ========== PLOTTING: ========== """ ## PLOTTING: ze.append(e) # otstapuvanje od z-oska xe.append(sp[0]) # otstapuvanje od x-oska ye.append(sp[1]) # otstapuvawe od y-oska # xs.append(targetPos[0]) # ys.append(targetPos[1]) # zs.append(targetPos[2]) # x_qc.append(pos[0]) # y_qc.append(pos[1]) # z_qc.append(pos[2]) # fig = plt.figure() # ax = fig.add_subplot(111, projection='3d') # ax.plot_wireframe(xs,ys,zs,color='blue') # ax.plot_wireframe(x_qc,y_qc,z_qc,color='red') # ax.set_xlim3d(-2.1,2.1) # ax.set_ylim3d(-2.1,2.1) # ax.set_zlim3d(0,1.9) # plt.show() # u.append(thrust) # v1.append(propeller1_PTV) # v2.append(propeller2_PTV) # v3.append(propeller3_PTV) # v4.append(propeller4_PTV) # plt.plot(xe,color='blue') plt.hold(True) plt.plot(ye,color='red') plt.hold(True) plt.plot(ze,color='green') plt.hold(True) # plt.plot(v4,color='pink') # plt.hold(True) # plt.axis([0,25,0,15]) # plt.show() # # plt.plot(xe,color='blue') # plt.axis([0,100,-4,4]) # plt.show() # plt.plot(ye,color='red') # plt.axis([0,100,-4,4]) # plt.show() # plt.plot(ze,color='green') plt.axis([0,25,-2,2]) plt.show() """ =========================================================== """ """ ========== Gradient descent: ========== """ sum_h_alpha.append(cumulAlpha) sum_h_beta.append(cumulBeta) sum_h_pos1.append(cumulAlphaPos) sum_h_pos0.append(cumulBetaPos) J_h_alpha.append((1/(tf - t0))*(5*cumulAlpha**2 + delta_alpha_angle[gd]**2)*dt) J_h_beta.append((1/(tf - t0))*(5*cumulBeta**2 + delta_beta_angle[gd]**2)*dt) J_h_pos1.append((1/(tf - t0))*(5*cumulAlphaPos**2 + delta_alpha_pos[gd]**2)*dt) J_h_pos0.append((1/(tf - t0))*(5*cumulBetaPos**2 + delta_beta_pos[gd]**2)*dt) paramX.append(targetPos[0]) paramY.append(targetPos[1]) print "=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=" print "theta_h_angles : ", theta_h_angles print "theta_h_pos : ", theta_h_pos print "sum_h_alpha : ", sum_h_alpha[gd] print "sum_h_beta : ", sum_h_beta[gd] print "sum_h_pos1 : ", sum_h_pos1[gd] print "sum_h_pos0 : ", sum_h_pos0[gd] print "J_h_alpha : ", J_h_alpha[gd] print "J_h_beta : ", J_h_beta[gd] print "J_h_pos1 : ", J_h_pos1[gd] print "J_h_pos0 : ", J_h_pos0[gd] print "paramX : ", paramX[gd] print "paramY : ", paramY[gd] print "=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=" gd = gd + 1 print "gd (iterations) : ", gd print "=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=" """ =========================================================== """ if gd == 400: with open('GD_43.csv','wb') as f1: writer=csv.writer(f1) for i in range(0, gd): writer.writerow([paramX[i],paramY[i],theta_h_angles[0],theta_h_angles[1],theta_h_angles[2],theta_h_pos[0],theta_h_pos[1],theta_h_pos[2],J_h_alpha[i],J_h_beta[i],J_h_pos1[i],J_h_pos0[i]]) break """ =========================================================== """ """ ========== WRITE TO TEXT: ========== """ ## WRITE TO TEXT: log_data(C_parameters_vert, C_parameters_horr, C_parameters_rot, vert_comp, horr_comp, rot_comp, particlesTargetVelocities, i) i +=1 """ =========================================================== """ # send propeller velocities to output: [res,retInts,retFloats,retStrings,retBuffer]=vrep.simxCallScriptFunction(clientID,Quadricopter,vrep.sim_scripttype_childscript,'qc_propeller_v',[],particlesTargetVelocities,[],emptyBuff,vrep.simx_opmode_blocking) vrep.simxSynchronousTrigger(clientID) """ =========================================================== """ # stop the simulation: vrep.simxStopSimulation(clientID,vrep.simx_opmode_blocking) # Now close the connection to V-REP: vrep.simxFinish(clientID) else: print ('Failed connecting to remote API server')
'/home/elias/[email protected]/_Winter2015/CSC494/Scenes/Base_Quad.ttt', 0, mode()) vrep.simxStartSimulation(cid, mode()) runtime = 0 err, copter = vrep.simxGetObjectHandle(cid, "Quadricopter_base", vrep.simx_opmode_oneshot_wait) err, target = vrep.simxGetObjectHandle(cid, "Quadricopter_target", vrep.simx_opmode_oneshot_wait) err, front_camera = vrep.simxGetObjectHandle(cid, 'Quadricopter_frontCamera', vrep.simx_opmode_oneshot) err, lin, ang = vrep.simxGetObjectVelocity(cid, copter, vrep.simx_opmode_streaming) err, orgpos = getTargetStart(cid, target) real_args(args, orgpos) args["addxToZ"] = [orgpos, 2.5] args["fuckIt"] = [orgpos, 2.5] #This prepared the data print(err) #while(1): while (runtime <= 10): error, pos = vrep.simxGetObjectPosition(cid, copter, -1, mode()) args["AsinToZ"] = [pos, getTime]
def QC_controller(Quadricopter_target, Quadricopter_base, Quadricopter, Quadricopter_target2, Quadricopter_base2, Quadricopter2): try: import vrep except: print('--------------------------------------------------------------') print('"vrep.py" could not be imported. This means very probably that') print('either "vrep.py" or the remoteApi library could not be found.') print('Make sure both are in the same folder as this file,') print('or appropriately adjust the file "vrep.py"') print('--------------------------------------------------------------') print('') """ =========================================================== """ """ ============ Imported Libraries: ============ """ import time import numpy as np from captains_log_v1 import log_data import os import cost import csv import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D import main_PID_controller """ =================================================================== """ # output limits: #min_output=0 #max_output=8.335 # program parameters: global i i = 0 xf = [] xl = [] yl = [] yf = [] zl = [] zf = [] vxl = [] vxf = [] vyl = [] vyf = [] euler_l = [] euler_f = [] # xe = [] # ye = [] ze = [] # xs = [] # ys = [] # zs = [] x_qc = [] y_qc = [] z_qc = [] x_qc2 = [] y_qc2 = [] z_qc2 = [] # u = [] v1 = [] v2 = [] v3 = [] v4 = [] v12 = [] v22 = [] v32 = [] v42 = [] #global variables: cumul = 0 last_e = 0 pAlphaE = 0 pBetaE = 0 psp2 = 0 psp1 = 0 prevEuler = 0 cumulAlpha = 0 cumulBeta = 0 cumulAlphaPos = 0 cumulBetaPos = 0 cumul2 = 0 last_e2 = 0 pAlphaE2 = 0 pBetaE2 = 0 psp22 = 0 psp12 = 0 prevEuler2 = 0 cumulAlpha2 = 0 cumulBeta2 = 0 cumulAlphaPos2 = 0 cumulBetaPos2 = 0 cumulPOS = 0 cumulVY = 0 cumulVX = 0 cumulSP0 = 0 cumulSP1 = 0 cumulEULER = 0 cumulPOS2 = 0 cumulVY2 = 0 cumulVX2 = 0 cumulSP02 = 0 cumulSP12 = 0 cumulEULER2 = 0 particlesTargetVelocities = [0, 0, 0, 0] particlesTargetVelocities2 = [0, 0, 0, 0] #speed weight: vParam = -2 #parameters for vertical control Kpv = 2 Kiv = 0 Kdv = 2 #parameters for horizontal control: Kph = 0.4 # TBD Kih = 0.1 # TBD Kdh = 1.5 Kph_pos1 = 0.4 Kih_pos1 = 0.002 Kdh_pos1 = 0.05 Kph_pos0 = 0.4 Kih_pos0 = 0.002 Kdh_pos0 = 0.05 #parameters for rotational control: Kpr = 0.05 # Kir=0 Kdr = 0.9 """ =========================================================== """ gd = 0 """ =========================================================== """ 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_blocking) #functional/handle code: emptyBuff = bytearray() [returnCode, targetObj] = vrep.simxGetObjectHandle(clientID, Quadricopter_target, vrep.simx_opmode_blocking) [returnCode, qc_base_handle] = vrep.simxGetObjectHandle(clientID, Quadricopter_base, vrep.simx_opmode_blocking) [returnCode, qc_handle] = vrep.simxGetObjectHandle(clientID, Quadricopter, vrep.simx_opmode_blocking) [returnCode, targetObj2] = vrep.simxGetObjectHandle(clientID, Quadricopter_target2, vrep.simx_opmode_blocking) [returnCode, qc_base_handle2] = vrep.simxGetObjectHandle(clientID, Quadricopter_base2, vrep.simx_opmode_blocking) [returnCode, qc_handle2] = vrep.simxGetObjectHandle(clientID, Quadricopter2, vrep.simx_opmode_blocking) # main loop: while True: """ ========== vertical control: ========== """ [returnCode, targetPos] = vrep.simxGetObjectPosition(clientID, targetObj, -1, vrep.simx_opmode_blocking) [returnCode, pos] = vrep.simxGetObjectPosition(clientID, qc_base_handle, -1, vrep.simx_opmode_blocking) [returnCode, l, w] = vrep.simxGetObjectVelocity(clientID, qc_base_handle, vrep.simx_opmode_blocking) [returnCode, targetPos2 ] = vrep.simxGetObjectPosition(clientID, targetObj2, -1, vrep.simx_opmode_blocking) [returnCode, pos2] = vrep.simxGetObjectPosition(clientID, qc_base_handle2, -1, vrep.simx_opmode_blocking) [returnCode, l2, w2] = vrep.simxGetObjectVelocity(clientID, qc_base_handle2, vrep.simx_opmode_blocking) """ =========================================================== """ """ ========== horizontal control: ========== """ [returnCode, sp] = vrep.simxGetObjectPosition(clientID, targetObj, qc_base_handle, vrep.simx_opmode_blocking) [rc, rc, vx, rc, rc] = vrep.simxCallScriptFunction( clientID, Quadricopter, vrep.sim_scripttype_childscript, 'qc_Get_vx', [], [], [], emptyBuff, vrep.simx_opmode_blocking) [rc, rc, vy, rc, rc] = vrep.simxCallScriptFunction( clientID, Quadricopter, vrep.sim_scripttype_childscript, 'qc_Get_vy', [], [], [], emptyBuff, vrep.simx_opmode_blocking) [rc, rc, rc, rc, rc] = vrep.simxCallScriptFunction(clientID, Quadricopter, vrep.sim_scripttype_childscript, 'qc_Get_Object_Matrix', [], [], [], emptyBuff, vrep.simx_opmode_blocking) [errorCode, M] = vrep.simxGetStringSignal(clientID, 'mtable', vrep.simx_opmode_oneshot_wait) if (errorCode == vrep.simx_return_ok): m = vrep.simxUnpackFloats(M) m1 = m[0] m2 = m[1] vx1 = [vx[0], vx[1], vx[2]] vx2 = [vx[3], vx[4], vx[5]] vy1 = [vy[0], vy[1], vy[2]] vy2 = [vy[3], vy[4], vy[5]] [returnCode, sp2] = vrep.simxGetObjectPosition(clientID, targetObj2, qc_base_handle2, vrep.simx_opmode_blocking) """ =========================================================== """ """ ========== rotational control: ========== """ [returnCode, euler] = vrep.simxGetObjectOrientation(clientID, targetObj, qc_base_handle, vrep.simx_opmode_blocking) [returnCode, orientation ] = vrep.simxGetObjectOrientation(clientID, qc_base_handle, -1, vrep.simx_opmode_blocking) [returnCode, euler2] = vrep.simxGetObjectOrientation(clientID, targetObj2, qc_base_handle2, vrep.simx_opmode_blocking) [returnCode, orientation2 ] = vrep.simxGetObjectOrientation(clientID, qc_base_handle2, -1, vrep.simx_opmode_blocking) """ =========================================================== """ """ ========== set propeller velocities: ========== """ particlesTargetVelocities, cumul, last_e, cumulAlpha, pAlphaE, cumulBeta, pBetaE, cumulAlphaPos, cumulBetaPos, psp2, psp1, prevEuler, cumulPOS, cumulVY, cumulVX, cumulSP0, cumulSP1, cumulEULER = main_PID_controller.PID( targetPos, pos, cumul, last_e, Kpv, Kiv, Kdv, vParam, l, vy1, m1, cumulAlpha, pAlphaE, Kph, Kih, Kdh, vx1, cumulBeta, pBetaE, cumulAlphaPos, cumulBetaPos, sp, Kph_pos1, Kih_pos1, Kdh_pos1, Kph_pos0, Kih_pos0, Kdh_pos0, psp2, psp1, Kpr, Kdr, euler, prevEuler, cumulPOS, cumulVY, cumulVX, cumulSP0, cumulSP1, cumulEULER, vx, vy, pos, sp, euler) particlesTargetVelocities2, cumul2, last_e2, cumulAlpha2, pAlphaE2, cumulBeta2, pBetaE2, cumulAlphaPos2, cumulBetaPos2, psp22, psp12, prevEuler2, cumulPOS2, cumulVY2, cumulVX2, cumulSP02, cumulSP12, cumulEULER2 = main_PID_controller.PID( targetPos2, pos2, cumul2, last_e2, Kpv, Kiv, Kdv, vParam, l2, vy2, m2, cumulAlpha2, pAlphaE2, Kph, Kih, Kdh, vx2, cumulBeta2, pBetaE2, cumulAlphaPos2, cumulBetaPos2, sp2, Kph_pos1, Kih_pos1, Kdh_pos1, Kph_pos0, Kih_pos0, Kdh_pos0, psp22, psp12, Kpr, Kdr, euler2, prevEuler2, cumulPOS2, cumulVY2, cumulVX2, cumulSP02, cumulSP12, cumulEULER2, vx, vy, pos, sp, euler) particlesTargetVelocities_s = [ particlesTargetVelocities[0], particlesTargetVelocities[1], particlesTargetVelocities[2], particlesTargetVelocities[3], particlesTargetVelocities2[0], particlesTargetVelocities2[1], particlesTargetVelocities2[2], particlesTargetVelocities2[3] ] """ =========================================================== """ """ =========================================================== """ """ ========== PLOTTING: ========== """ ## PLOTTING: # ze.append(targetPos[2]-pos[2]) # otstapuvanje od z-oska # xe.append(sp[0]) # otstapuvanje od x-oska # ye.append(sp[1]) # otstapuvawe od y-oska # xs.append(targetPos[0]) # ys.append(targetPos[1]) # zs.append(targetPos[2]) # x_qc.append(pos[0]) # y_qc.append(pos[1]) # z_qc.append(pos[2]) # x_qc2.append(pos2[0]) # y_qc2.append(pos2[1]) # z_qc2.append(pos2[2]) # fig = plt.figure() # ax = fig.add_subplot(111, projection='3d') # ax.plot_wireframe(x_qc,y_qc,z_qc,color='blue') # ax.plot_wireframe(x_qc2,y_qc2,z_qc2,color='red') # ax.set_xlim3d(-2.1,2.1) # ax.set_ylim3d(-2.1,2.1) # ax.set_zlim3d(0,1.9) # plt.show() # u.append(thrust) ''' !!! treba da se nacrta i pozicijata i da se sporedi so i bez consensus !!! ''' xl.append(pos[0]) xf.append(pos2[0]) yl.append(pos[1]) yf.append(pos2[1]) zl.append(pos[2]) zf.append(pos2[2]) vxl.append(vx1[2]) vxf.append(vx2[2]) vyl.append(vy1[2]) vyf.append(vy2[2]) euler_l.append(euler[2]) euler_f.append(euler2[2]) v1.append(particlesTargetVelocities[0]) v2.append(particlesTargetVelocities[1]) v3.append(particlesTargetVelocities[2]) v4.append(particlesTargetVelocities[3]) v12.append(particlesTargetVelocities2[0]) v22.append(particlesTargetVelocities2[1]) v32.append(particlesTargetVelocities2[2]) v42.append(particlesTargetVelocities2[3]) plt.plot(yl, color='blue') plt.hold(True) plt.plot(yf, color='red') # plt.plot(euler_l,color='blue') # plt.plot(euler_f,color='red') # plt.plot(yl,color='green') # plt.hold(True) # plt.plot(yf,color='pink') # plt.hold(True) # plt.plot(v3,color='green') # plt.hold(True) # plt.plot(v4,color='pink') plt.hold(True) plt.axis([0, 149, -3, 3]) plt.show() # print " Zaxis error between drones: ", pos[2]-pos2[2] ze.append(pos[2] - pos2[2]) # ze.append(abs(euler[2])-abs(euler2[2])) plt.plot(ze, color='green') plt.axis([0, 149, -3, 3]) plt.show() """ =========================================================== """ """ ========== Gradient descent: ========== """ # sum_h_alpha.append(cumulAlpha) # sum_h_beta.append(cumulBeta) # sum_h_pos1.append(cumulAlphaPos) # sum_h_pos0.append(cumulBetaPos) # J_h_alpha.append((1/(tf - t0))*(5*cumulAlpha**2 + delta_alpha_angle[gd]**2)*dt) # J_h_beta.append((1/(tf - t0))*(5*cumulBeta**2 + delta_beta_angle[gd]**2)*dt) # J_h_pos1.append((1/(tf - t0))*(5*cumulAlphaPos**2 + delta_alpha_pos[gd]**2)*dt) # J_h_pos0.append((1/(tf - t0))*(5*cumulBetaPos**2 + delta_beta_pos[gd]**2)*dt) # paramX.append(targetPos[0]) # paramY.append(targetPos[1]) # print "=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=" # print "theta_h_angles : ", theta_h_angles # print "theta_h_pos : ", theta_h_pos # # print "sum_h_alpha : ", sum_h_alpha[gd] # print "sum_h_beta : ", sum_h_beta[gd] # print "sum_h_pos1 : ", sum_h_pos1[gd] # print "sum_h_pos0 : ", sum_h_pos0[gd] # print "J_h_alpha : ", J_h_alpha[gd] # print "J_h_beta : ", J_h_beta[gd] # print "J_h_pos1 : ", J_h_pos1[gd] # print "J_h_pos0 : ", J_h_pos0[gd] # print "paramX : ", paramX[gd] # print "paramY : ", paramY[gd] # print "cumulPOS : ", cumulPOS2 # print "cumulVY : ", cumulVY2 # print "cumulVX : ", cumulVX2 # print "cumulSP0 : ", cumulSP02 # print "cumulSP1 : ", cumulSP12 # print "cumulEULER : ", cumulEULER2 # print "cumul : ", cumul print "=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=" gd = gd + 1 print "gd (iterations) : ", gd print "=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=" """ =========================================================== """ if gd == 150: # with open('GD_43.csv','wb') as f1: # writer=csv.writer(f1) # for i in range(0, gd): # writer.writerow([paramX[i],paramY[i],theta_h_angles[0],theta_h_angles[1],theta_h_angles[2],theta_h_pos[0],theta_h_pos[1],theta_h_pos[2],J_h_alpha[i],J_h_beta[i],J_h_pos1[i],J_h_pos0[i]]) break # """ =========================================================== """ """ ========== WRITE TO TEXT: ========== """ ## WRITE TO TEXT: # log_data(C_parameters_vert, C_parameters_horr, C_parameters_rot, vert_comp, horr_comp, rot_comp, particlesTargetVelocities, i) # i +=1 """ =========================================================== """ # send propeller velocities to output: [res, retInts, retFloats, retStrings, retBuffer] = vrep.simxCallScriptFunction( clientID, Quadricopter, vrep.sim_scripttype_childscript, 'qc_propeller_v', [], particlesTargetVelocities_s, [], emptyBuff, vrep.simx_opmode_blocking) # [res,retInts,retFloats,retStrings,retBuffer]=vrep.simxCallScriptFunction(clientID,Quadricopter2,vrep.sim_scripttype_childscript,'qc_propeller_v',[],particlesTargetVelocities2,[],emptyBuff,vrep.simx_opmode_blocking) vrep.simxSynchronousTrigger(clientID) """ =========================================================== """ print " Zaxis error between drones: ", ze # stop the simulation: vrep.simxStopSimulation(clientID, vrep.simx_opmode_blocking) # Now close the connection to V-REP: vrep.simxFinish(clientID) else: print('Failed connecting to remote API server')
def start(clientID, quads, targets, speed, proxs, path, endpos, leadfoll=False): """ Boids model main program :param clientID: ID of the VRep connection :param quads: quadrotor handles :param targets: quadrotor target handles :param speed: speed of quadrotors :param proxs: proximity sensor handles :param path: quadrotor path coordinates :param endpos: ending position of quadrotors :param leadfoll: True - leader/followers mode, False - all boids following path (default) """ # definition of constants quadsNum = len(quads) # number of quadrotors viewRange = 3 # view range of quadrotors smp = 0.2 # sampling period kS = [0.30, 2.0] # separation constants [multiplication const, power const] kC = [0.30, 0.0] # cohesion constants [multiplication const, power const] kA = [0.00, 0.0] # alignment constants [multiplication const, power const] kD = [speed, 1.0] # path following constants [multiplication const, power const] kO = [0.20, 2.0 ] # obstacle avoidance constants [multiplication const, power const] # data stream init for i in range(quadsNum): vrep.simxGetObjectPosition(clientID, quads[i], -1, vrep.simx_opmode_streaming) vrep.simxGetObjectVelocity(clientID, quads[i], vrep.simx_opmode_streaming) vrep.simxReadProximitySensor(clientID, proxs[i], vrep.simx_opmode_streaming) # variables init position = [[0 for _ in range(3)] for _ in range(quadsNum)] # position of quadrotors velocity = [[0 for _ in range(3)] for _ in range(quadsNum)] # velocity of quadrotors closest = [[0 for _ in range(3)] for _ in range(quadsNum)] # coords of closest obstacle to quads visibleQuads = [[0 for _ in range(quadsNum)] for _ in range(quadsNum)] # visible quadrotors individualTarget = [ 0 ] * quadsNum # current waypoint index for each quadrotor # get closest boid to starting point leader = 0 _, tmp = vrep.simxGetObjectPosition(clientID, quads[0], -1, vrep.simx_opmode_buffer) dist = ut.norm(ut.sub(path[1], tmp)) for i in range(1, quadsNum): _, tmp = vrep.simxGetObjectPosition(clientID, quads[i], -1, vrep.simx_opmode_buffer) nrm = ut.norm(ut.sub(path[1], tmp)) if nrm < dist: dist = nrm leader = i # main boids program t1 = 0 t2 = 0.0 finished = [False] * quadsNum count = 0 counting = False file = open('data.txt', 'wt', encoding='utf-8') while vrep.simxGetConnectionId(clientID) != -1: time.sleep(smp) separation = [[0 for _ in range(3)] for _ in range(quadsNum)] # separation force cohesion = [[0 for _ in range(3)] for _ in range(quadsNum)] # cohesion force alignment = [[0 for _ in range(3)] for _ in range(quadsNum)] # alignment force destination = [[0 for _ in range(3)] for _ in range(quadsNum)] # path following force avoidance = [[0 for _ in range(3)] for _ in range(quadsNum)] # obstacle avoidance force output = [[0 for _ in range(3)] for _ in range(quadsNum)] # output force # check if all quads finished if counting: if count >= 0: file.close() return (t2 - t1) / 1000 count += 1 else: for i in range(quadsNum): nrm = ut.norm(ut.sub(position[i][0:2], path[-1][0:2])) if nrm < 2 and not finished[i]: finished[i] = True print('Quad #' + str(i) + ' finished in ' + str((vrep.simxGetLastCmdTime(clientID) - t1) / 1000) + 's') if (leadfoll and finished[leader]) or ( not leadfoll and all(_ for _ in finished)): counting = True t2 = vrep.simxGetLastCmdTime(clientID) if endpos is None: file.close() return (t2 - t1) / 1000 # read data from VRep for i in range(quadsNum): _, position[i] = vrep.simxGetObjectPosition( clientID, quads[i], -1, vrep.simx_opmode_buffer) _, velocity[i], _ = vrep.simxGetObjectVelocity( clientID, quads[i], vrep.simx_opmode_buffer) _, res, closest[i], _, _ = vrep.simxReadProximitySensor( clientID, proxs[i], vrep.simx_opmode_buffer) if not res: closest[i] = [0, 0, 0] closest[i][2] = 0 # write into data file ct = vrep.simxGetLastCmdTime(clientID) file.write(str(ct)) for i in range(quadsNum): file.write(' ' + str(position[i][0]) + ' ' + str(position[i][1]) + ' ' + str(position[i][2])) file.write(' ' + str(velocity[i][0]) + ' ' + str(velocity[i][1]) + ' ' + str(velocity[i][2])) file.write(' ' + str(closest[i][0]) + ' ' + str(closest[i][1])) file.write('\n') # compute visible quadrotors for i in range(quadsNum): for j in range(quadsNum): if i != j: temp = ut.sub(position[i], position[j]) if ut.norm(temp) < viewRange: visibleQuads[i][j] = 1 else: visibleQuads[i][j] = 0 for i in range(quadsNum): # compute separation force for j in range(quadsNum): if i != j and visibleQuads[i][j] == 1: temp = ut.sub(position[i], position[j]) nrm = ut.norm(temp) if nrm != 0: temp = ut.mul(temp, kS[0] / (nrm**kS[1])) separation[i] = ut.add(separation[i], temp) # compute cohesion and alignment forces center = [0, 0, 0] # center of the swarm if sum(visibleQuads[i]) != 0: for j in range(quadsNum): if i != j and visibleQuads[i][j] == 1: temp = ut.mul(position[j], 1 / sum(visibleQuads[i])) center = ut.add(center, temp) temp = ut.mul(velocity[j], 1 / sum(visibleQuads[i])) alignment[i] = ut.add(alignment[i], temp) cohesion[i] = ut.sub(center, position[i]) nrm = ut.norm(cohesion[i]) if nrm != 0: cohesion[i] = ut.mul(cohesion[i], kC[0] / (nrm**kC[1])) nrm = ut.norm(alignment[i]) if nrm != 0: alignment[i] = ut.mul(alignment[i], kA[0] / (nrm**kA[1])) # compute path following force if not leadfoll or i == leader or counting: if not counting: nrm = ut.norm( ut.sub(position[i][0:2], path[individualTarget[i]][0:2])) if individualTarget[i] != 0: vec1 = ut.sub(path[individualTarget[i] - 1], path[individualTarget[i]]) vec2 = ut.sub(position[i], path[individualTarget[i]]) if individualTarget[i] < len(path) - 1 and ( nrm <= 1 or ut.angle(vec1, vec2) >= math.pi / 2): individualTarget[i] += 1 # if individualTarget[i] == 2 and min(individualTarget) == 2: # print((vrep.simxGetLastCmdTime(clientID)-tt)/1000) else: vec1 = ut.sub(path[individualTarget[i] + 1], path[individualTarget[i]]) vec2 = ut.sub(position[i], path[individualTarget[i]]) if nrm <= 1 or ut.angle(vec1, vec2) <= math.pi / 2: individualTarget[i] += 1 if t1 == 0 and min(individualTarget) == 1: t1 = vrep.simxGetLastCmdTime(clientID) # tt = vrep.simxGetLastCmdTime(clientID) destination[i] = ut.sub(path[individualTarget[i]], position[i]) else: destination[i] = ut.sub(endpos[i], position[i]) nrm = ut.norm(destination[i]) if nrm != 0: if finished[i]: destination[i] = ut.mul(destination[i], 0.1) else: destination[i] = ut.mul(destination[i], kD[0] / (nrm**kD[1])) # compute output force without obstacle avoidance output[i] = separation[i] output[i] = ut.add(output[i], cohesion[i]) output[i] = ut.add(output[i], alignment[i]) output[i] = ut.add(output[i], destination[i]) # compute obstacle avoidance force # angle = ut.angle(closest[i], output[i]) # if angle > math.pi/2+0.3: # avoidance[i] = [0, 0, 0] # else: avoidance[i] = ut.sub([0, 0, 0], closest[i]) nrm = ut.norm(avoidance[i]) if nrm != 0: avoidance[i] = ut.mul(avoidance[i], kO[0] / (nrm**kO[1])) # compute output force output[i] = ut.add(output[i], avoidance[i]) if position[i][2] < 0.5: output[i][2] = 0.05 # export output to VRep for i in range(quadsNum): vrep.simxSetObjectPosition(clientID, targets[i], quads[i], output[i], vrep.simx_opmode_streaming)