def show_path(path, clientID): #make a signal for each path-component datax = path[0] datay = path[1] dataz = path[2] packedDatax = vrep.simxPackFloats(datax) vrep.simxClearStringSignal(clientID, 'Path_Signalx', vrep.simx_opmode_oneshot) vrep.simxSetStringSignal(clientID, 'Path_Signalx', packedDatax, vrep.simx_opmode_oneshot) packedDatay = vrep.simxPackFloats(datay) vrep.simxClearStringSignal(clientID, 'Path_Signaly', vrep.simx_opmode_oneshot) vrep.simxSetStringSignal(clientID, 'Path_Signaly', packedDatay, vrep.simx_opmode_oneshot) packedDataz = vrep.simxPackFloats(dataz) vrep.simxClearStringSignal(clientID, 'Path_Signalz', vrep.simx_opmode_oneshot) vrep.simxSetStringSignal(clientID, 'Path_Signalz', packedDataz, vrep.simx_opmode_oneshot) #signals that the data of the path is ready to be read data2 = [1] packedData2 = vrep.simxPackFloats(data2) vrep.simxClearStringSignal(clientID, 'Command_Path_Ready', vrep.simx_opmode_oneshot) vrep.simxSetStringSignal(clientID, 'Command_Path_Ready', packedData2, vrep.simx_opmode_oneshot)
def __call__(self, t, values): if self.size is None: packedData = vrep.simxPackFloats(values.flatten()) else: packedData = vrep.simxPackFloats(values[:self.size].flatten()) raw_bytes = (ctypes.c_ubyte * len(packedData)).from_buffer_copy(packedData) err = vrep.simxSetStringSignal(self.cid, self.signal_name, raw_bytes, self.mode)
def __call__( self, t, values ): if self.size is None: packedData=vrep.simxPackFloats(values.flatten()) else: packedData=vrep.simxPackFloats(values[:self.size].flatten()) raw_bytes = (ctypes.c_ubyte * len(packedData)).from_buffer_copy(packedData) err = vrep.simxSetStringSignal(self.cid, self.signal_name, raw_bytes, self.mode)
def sendAction(self, s): msg = String() changeAction = self.checkConditions(s, self.prev['A']) angles = self.getAngles(s) states = self.splitState(s) ret = [] for i, name in enumerate(self.pubs.keys()): loc = "robot" + str(i) angle = angles[loc] state = states[loc] if changeAction[i]: self.counter[i] = self.period action = self.getNextAction(state, angle, i) else: action = self.prev['A'][i] ret.append(action) action = self.getPrimitive(angle, self.actionMap[action], i) msg.data = vrep.simxPackFloats(action) self.pubs[name].publish(msg) self.counter[i] -= 1 self.time += 1 if self.time % np.inf == 0: self.turn = -1 # turn left for now # np.random.randint(2) + 1 if self.turn == 0 else 0 self.phase = (self.phase + 1) % 4 print("PHASE ", self.phase) print(ret, self.time) return ret
def _setMotorSpeeds(self, leftSpeed, rightSpeed): """ set the desired speeds of the left and right wheel of the robot :param leftSpeed: float speed in rad/sec of left motor :param rightSpeed: float speed in rad/sec of right motor """ """ res, ret_ints, ret_floats, ret_strings, ret_buffer = vrep.simxCallScriptFunction(self.__clientID, self._name, vrep.sim_scripttype_childscript, 'setVelocitiesForRemote', [], [leftSpeed, rightSpeed], [], self.__empty_buff, vrep.simx_opmode_blocking) """ velocities = vrep.simxPackFloats([leftSpeed, rightSpeed]) res = vrep.simxSetStringSignal(self.__clientID, self.__signalName+'_velocities', velocities, vrep.simx_opmode_oneshot) # res = vrep.simx_return_ok # res = vrep.simxSetFloatSignal(self.__clientID, self.__signalName+'_velLeft', 0.0, vrep.simx_opmode_oneshot) # res = vrep.simxSetFloatSignal(self.__clientID, self.__signalName+'_velRight', 0.0, vrep.simx_opmode_oneshot) if res == vrep.simx_return_ok: logging.debug(' motor values', leftSpeed, rightSpeed) else: logging.error('Remote function call simxSetStringSignal for signal EPUCK_velLeft/-Right failed')
def run_threaded_drop(self, frame_work2obj): """Gets an initial object pose by 'dropping' it WRT frame_work2obj. This function launches a threaded script that sets an object position, enables the object to be dynamically simulated, and allows it to fall to the ground and come to a resting pose. This is usually the first step when collecting grasps. Note that we also set the gripper to be non-collidable / respondable, so it doesn't interfere with the drop process """ self._clear_signals() self.set_gripper_properties(visible=True, dynamic=True, collidable=True) frame = self._format_matrix(frame_work2obj) # Launch the threaded script vrep.simxSetStringSignal(self.clientID, 'run_drop_object', vrep.simxPackFloats(frame), vrep.simx_opmode_oneshot) r = -1 while r != vrep.simx_return_ok: r, success = vrep.simxGetIntegerSignal( self.clientID, 'object_resting', vrep.simx_opmode_oneshot_wait) if success == 0: raise Exception('Error dropping object! Return code: ', r)
def __init__( self, sim_dt=0.01, max_target_distance=3, noise=False, noise_std=[0,0,0,0,0,0], ): # Call the superclass of Quadcopter, which is Robot super(Quadcopter, self).__init__(sim_dt) err, self.copter = vrep.simxGetObjectHandle(self.cid, "Quadricopter_base", vrep.simx_opmode_oneshot_wait ) # Reset the motor commands to zero packedData=vrep.simxPackFloats([0,0,0,0]) raw_bytes = (ctypes.c_ubyte * len(packedData)).from_buffer_copy(packedData) err = vrep.simxSetStringSignal(self.cid, "rotorTargetVelocities", raw_bytes, vrep_mode) self.pos = [0,0,0] self.pos_err = [0,0,0] self.t_pos = [0,0,0] self.lin = [0,0,0] self.ori = [0,0,0] self.ori_err = [0,0,0] self.t_ori = [0,0,0] self.ang = [0,0,0] # Maximum target distance error that can be returned self.max_target_distance = max_target_distance # If noise is being modelled self.noise = noise # Standard Deviation of the noise for the 4 state variables self.noise_std = noise_std
def __init__( self, sim_dt=0.01, max_target_distance=3, noise=False, noise_std=[0,0,0,0,0,0], target_func=None, ): super(Quadcopter, self).__init__(sim_dt) 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 ) # Reset the motor commands to zero packedData=vrep.simxPackFloats([0,0,0,0]) raw_bytes = (ctypes.c_ubyte * len(packedData)).from_buffer_copy(packedData) err = vrep.simxSetStringSignal(self.cid, "rotorTargetVelocities", raw_bytes, vrep_mode) self.pos = [0,0,0] self.pos_err = [0,0,0] self.t_pos = [0,0,0] self.lin = [0,0,0] self.ori = [0,0,0] self.ori_err = [0,0,0] self.t_ori = [0,0,0] self.ang = [0,0,0] self.vert_prox_dist = 0 self.left_prox_dist = 0 self.right_prox_dist = 0 # Distance reading recorded when nothing is in range self.max_vert_dist = 1.5 self.max_left_dist = 1.0 self.max_right_dist = 1.0 # Maximum target distance error that can be returned self.max_target_distance = max_target_distance # If noise is being modelled self.noise = noise # Standard Deviation of the noise for the 4 state variables self.noise_std = noise_std # Overwrite the get_target method if the target is to be controlled by a # function instead of by V-REP if target_func is not None: self.step = 0 self.target_func = target_func def get_target(): self.t_pos, self.t_ori = self.target_func( self.step ) self.step += 1 self.get_target = get_target
def calcTheForce(self): # расчет сил dx = self.XYZ_d[0] - self.XYZ[0] dy = self.XYZ_d[1] - self.XYZ[1] self.ABG_d[2] = np.arctan2(dy, dx) Fx_d = 0 if -0.05 <= self.ABG_d[2] - self.ABG[2] <= 0.05: if dx < 0 and dy < 0: if dx > dy: Fx_d = self.fwd_ctrl.pidUpdate(self.XYZ_d[0], self.XYZ[0]) * -1 else: Fx_d = self.fwd_ctrl.pidUpdate(self.XYZ_d[1], self.XYZ[1]) * -1 if dx > 0 and dy > 0: if dx > dy: Fx_d = self.fwd_ctrl.pidUpdate(self.XYZ_d[0], self.XYZ[0]) else: Fx_d = self.fwd_ctrl.pidUpdate(self.XYZ_d[1], self.XYZ[1]) if dx < 0 and dy > 0: if dx > dy: Fx_d = self.fwd_ctrl.pidUpdate(self.XYZ_d[0], self.XYZ[0]) * -1 else: Fx_d = self.fwd_ctrl.pidUpdate(self.XYZ_d[1], self.XYZ[1]) if dx > 0 and dy < 0: if dx > dy: Fx_d = self.fwd_ctrl.pidUpdate(self.XYZ_d[0], self.XYZ[0]) else: Fx_d = self.fwd_ctrl.pidUpdate(self.XYZ_d[1], self.XYZ[1]) * -1 # перемещения вдоль игрек if -0.5 <= dx <= 0.5 and (self.XYZ[1] <= self.XYZ_d[1]): Fx_d = self.fwd_ctrl.pidUpdate(self.XYZ_d[1], self.XYZ[1]) if (-0.5 <= dx <= 0.5) and (self.XYZ[1] > self.XYZ_d[1]): Fx_d = self.fwd_ctrl.pidUpdate(self.XYZ_d[1], self.XYZ[1]) * -1 # перемещения вдоль икс if -0.5 <= dy <= 0.5 and (self.XYZ[0] <= self.XYZ_d[0]): Fx_d = self.fwd_ctrl.pidUpdate(self.XYZ_d[0], self.XYZ[0]) if (-0.5 <= dy <= 0.5) and (self.XYZ[0] > self.XYZ_d[0]): Fx_d = self.fwd_ctrl.pidUpdate(self.XYZ_d[0], self.XYZ[0]) * -1 else: Fx_d = 0 Fy_d = 0 Fz_d = PIDc.pidUpdate(self.stab_ctrl, self.XYZ_d[2], self.XYZ[2]) # повороты: рыскание, тангаж и крен Mx_d = PIDc.pidUpdate(self.rotation_ctrl, self.ABG_d[2], self.ABG[2]) # вокруг Z, рыскание My_d = PIDc.pidUpdate(self.pitchroll_ctrl, self.ABG_d[1], self.ABG[1]) Mz_d = PIDc.pidUpdate(self.pitchroll_ctrl, self.ABG_d[0], self.ABG[0]) forceD = [Fx_d, Fy_d, Fz_d, Mx_d, My_d, Mz_d] s_force = vrep.simxPackFloats(forceD) vrep.simxSetStringSignal(self.clientID, "ForceD", s_force, vrep.simx_opmode_oneshot)
def show_path2(path,clientID): errorCode,Graph=vrep.simxGetObjectHandle(clientID,'Graph',vrep.simx_opmode_oneshot_wait) print ("sending path to VREP") datax=path[0] datay=path[1] dataz=path[2] packedDatax=vrep.simxPackFloats(datax) vrep.simxClearStringSignal(clientID,'Path_Signalx',vrep.simx_opmode_oneshot) vrep.simxSetStringSignal(clientID,'Path_Signalx',packedDatax,vrep.simx_opmode_oneshot) packedDatay=vrep.simxPackFloats(datay) vrep.simxClearStringSignal(clientID,'Path_Signaly',vrep.simx_opmode_oneshot) vrep.simxSetStringSignal(clientID,'Path_Signaly',packedDatay,vrep.simx_opmode_oneshot) packedDataz=vrep.simxPackFloats(dataz) vrep.simxClearStringSignal(clientID,'Path_Signalz',vrep.simx_opmode_oneshot) vrep.simxSetStringSignal(clientID,'Path_Signalz',packedDataz,vrep.simx_opmode_oneshot)
def quadcopter_rotors(cid, handle, val): # This function does not use handle, it just exists in the signature # to make it consistent motor_values = np.zeros(4) for i in range(4): motor_values[i] = val[i] packedData = vrep.simxPackFloats(motor_values.flatten()) raw_bytes = (ctypes.c_ubyte * len(packedData)).from_buffer_copy(packedData) err = vrep.simxSetStringSignal(cid, "rotorTargetVelocities", raw_bytes, vrep_mode)
def show_path(path, clientID): # make a signal for each path-component datax = path[0] datay = path[1] dataz = path[2] packedDatax = vrep.simxPackFloats(datax) vrep.simxClearStringSignal(clientID, "Path_Signalx", vrep.simx_opmode_oneshot) vrep.simxSetStringSignal(clientID, "Path_Signalx", packedDatax, vrep.simx_opmode_oneshot) packedDatay = vrep.simxPackFloats(datay) vrep.simxClearStringSignal(clientID, "Path_Signaly", vrep.simx_opmode_oneshot) vrep.simxSetStringSignal(clientID, "Path_Signaly", packedDatay, vrep.simx_opmode_oneshot) packedDataz = vrep.simxPackFloats(dataz) vrep.simxClearStringSignal(clientID, "Path_Signalz", vrep.simx_opmode_oneshot) vrep.simxSetStringSignal(clientID, "Path_Signalz", packedDataz, vrep.simx_opmode_oneshot) # signals that the data of the path is ready to be read data2 = [1] packedData2 = vrep.simxPackFloats(data2) vrep.simxClearStringSignal(clientID, "Command_Path_Ready", vrep.simx_opmode_oneshot) vrep.simxSetStringSignal(clientID, "Command_Path_Ready", packedData2, vrep.simx_opmode_oneshot)
def send_motor_commands( self, values ): motor_values = np.zeros(4) for i in range(4): motor_values[i] = values[i] packedData=vrep.simxPackFloats(motor_values.flatten()) raw_bytes = (ctypes.c_ubyte * len(packedData)).from_buffer_copy(packedData) err = vrep.simxSetStringSignal(self.cid, "rotorTargetVelocities", raw_bytes, vrep_mode)
def forward(self, action=None): controller.target_move(self.cid,self.target,self.function,self.args) if not(action is None): packedData = vrep.simxPackFloats([x for x in action]) raw_bytes = (ctypes.c_ubyte * len(packedData)).from_buffer_copy(packedData) err = vrep.simxSetStringSignal(self.cid, "rotorTargetVelocities", packedData, vrep.simx_opmode_oneshot) firstPass = False else: controller.controller_motor(self.cid,self.copter,self.target)
def quadcopter_rotors(cid, handle, val): # This function does not use handle, it just exists in the signature # to make it consistent motor_values = np.zeros(4) for i in range(4): motor_values[i] = val[i] packedData=vrep.simxPackFloats(motor_values.flatten()) raw_bytes = (ctypes.c_ubyte * len(packedData)).from_buffer_copy(packedData) err = vrep.simxSetStringSignal(cid, "rotorTargetVelocities", raw_bytes, vrep_mode)
def send_path_4_drawing(path, sleep_time=0.07): #the bigger the sleep time the more accurate the points are placed but you have to be very patient :D for i in path: point2send = transform_points_from_image2real(i) packedData = vrep.simxPackFloats(point2send.flatten()) raw_bytes = (ctypes.c_ubyte * len(packedData)).from_buffer_copy(packedData) returnCode = vrep.simxWriteStringStream(clientID, "path_coord", raw_bytes, vrep.simx_opmode_oneshot) time.sleep(sleep_time)
def forward(self, action=None): controller.target_move(self.cid, self.target, self.function, self.args) if not (action is None): packedData = vrep.simxPackFloats([x for x in action]) raw_bytes = (ctypes.c_ubyte * len(packedData)).from_buffer_copy(packedData) err = vrep.simxSetStringSignal(self.cid, "rotorTargetVelocities", packedData, vrep.simx_opmode_oneshot) firstPass = False else: controller.controller_motor(self.cid, self.copter, self.target)
def send_motor_commands(self, values): # Limit motors by max and min values motor_values = np.zeros(4) for i in range(4): # if values[i] > 30: # motor_values[i] = 30 # elif values[i] < 0: # motor_values[i] = 0 # else: # motor_values[i] = values[i] motor_values[i] = values[i] # print('motor_value', motor_values) packedData = vrep.simxPackFloats(motor_values.flatten()) raw_bytes = (ctypes.c_ubyte * len(packedData)).from_buffer_copy(packedData) err = vrep.simxSetStringSignal(self.cid, "rotorTargetVelocities", raw_bytes, vrep_mode)
def send_motor_commands( self, values ): # Limit motors by max and min values motor_values = np.zeros(4) for i in range(4): """ if values[i] > 30: motor_values[i] = 30 elif values[i] < 0: motor_values[i] = 0 else: motor_values[i] = values[i] """ motor_values[i] = values[i] packedData=vrep.simxPackFloats(motor_values.flatten()) raw_bytes = (ctypes.c_ubyte * len(packedData)).from_buffer_copy(packedData) err = vrep.simxSetStringSignal(self.cid, "rotorTargetVelocities", raw_bytes, vrep_mode)
def _connect(self): self.__clientID = vrep.simxStart(self.__host_url, self.__port, True, True, 5000, 5) # Connect to V-REP if self.__clientID != -1: print('Connected to remote API server of Vrep with clientID: ', self.__clientID) self._connectionStatus = True vrep.simxGetStringSignal( self.__clientID, self.__signalName + '_allSens', vrep.simx_opmode_streaming) #first time call vrep.simxGetStringSignal( self.__clientID, self.__signalName + '_camera', vrep.simx_opmode_streaming) #first time call vrep.simxSetStringSignal( self.__clientID, self.__signalName + '_velocities', vrep.simxPackFloats([0.0, 0.0]), vrep.simx_opmode_streaming) #first time call # if self.__synchronous: # self.startsim() else: logging.error('Failed connecting to remote API server of Vrep')
initialCall=True print ('VREP Simulation Program') vrep.simxFinish(-1) # just in case, close all opened connections clientID=vrep.simxStart('127.0.0.1',19999,True,True,5000,5) # Connect to V-REP if clientID!=-1: print ('Connected to remote API server') joystick = quadCTRL() # Object handle # _,Quadbase=vrep.simxGetObjectHandle(clientID,'Quadricopter_base',vrep.simx_opmode_oneshot_wait) else: print('Connection Failure') sys.exit('Abort Connection') while True: # Code for testing... if initialCall: mode = vrep.simx_opmode_streaming initialCall = False else: mode = vrep.simx_opmode_buffer # Poll controller demands = joystick.DetectAction() print("R: " + str(demands[0]) + " " +str(demands[1]) + " " + "L: " + str(demands[2]) + " " +str(demands[3])) demandString=vrep.simxPackFloats(demands) vrep.simxSetStringSignal(clientID,'demandstring',demandString,vrep.simx_opmode_oneshot) print('EOS')
def __init__( self, max_target_distance=4, noise=False, noise_std=None, dodging=True, target_func=None, cid=None ): # If a cid is specified, assume the connection has already been # established and should remain open if cid is None: vrep.simxFinish(-1) # just in case, close all opened connections self.cid = vrep.simxStart('127.0.0.1',19997,True,True,5000,5) else: self.cid = cid if self.cid != -1: print ('Connected to V-REP remote API server, client id: %s' % self.cid) vrep.simxStartSimulation( self.cid, vrep.simx_opmode_oneshot ) if SYNC: vrep.simxSynchronous( self.cid, True ) else: print ('Failed connecting to V-REP remote API server') self.exit() 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 ) # Reset the motor commands to zero packedData=vrep.simxPackFloats([0,0,0,0]) raw_bytes = (ctypes.c_ubyte * len(packedData)).from_buffer_copy(packedData) err = vrep.simxSetStringSignal(self.cid, "rotorTargetVelocities", raw_bytes, vrep_mode) self.pos = [0,0,0] self.pos_err = [0,0,0] self.t_pos = [0,0,0] self.lin = [0,0,0] self.ori = [0,0,0] self.ori_err = [0,0,0] self.t_ori = [0,0,0] self.ang = [0,0,0] self.count = 0 # Maximum target distance error that can be returned self.max_target_distance = max_target_distance # If noise is being modelled if noise_std is not None: self.noise = True else: self.noise = False # Standard Deviation of the noise for the 4 state variables self.noise_std = noise_std # Overwrite the get_target method if the target is to be controlled by a # function instead of by V-REP if target_func is not None: self.step = 0 self.target_func = target_func def get_target(): self.t_pos, self.t_ori = self.target_func( self.step ) self.step += 1 self.get_target = get_target
def __set(self, action): self.action = np.clip(action, self.action_space.low, self.action_space.high) vrep.simxSetStringSignal(self.__ID, "actions", vrep.simxPackFloats(self.action), vrep.simx_opmode_oneshot)
def useTheForce(self, Fx, Fy, Fz, Rx, Ry, Rz): forceD = [Fx, Fy, Fz, Rx, Ry, Rz] s_force = vrep.simxPackFloats(forceD) res = vrep.simxSetStringSignal(self.clientID, "ForceD", s_force, vrep.simx_opmode_oneshot)
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
def __init__(self, max_target_distance=4, noise=False, noise_std=None, dodging=True, target_func=None, cid=None): # If a cid is specified, assume the connection has already been # established and should remain open if cid is None: vrep.simxFinish(-1) # just in case, close all opened connections self.cid = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5) else: self.cid = cid if self.cid != -1: print('Connected to V-REP remote API server, client id: %s' % self.cid) vrep.simxStartSimulation(self.cid, vrep.simx_opmode_oneshot) if SYNC: vrep.simxSynchronous(self.cid, True) else: print('Failed connecting to V-REP remote API server') self.exit() 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.laser_signal = vrep.simxReadStringStream( self.cid, 'laser_data', vrep.simx_opmode_streaming) err, self.dis_signal = vrep.simxReadStringStream( self.cid, 'dis_data', vrep.simx_opmode_streaming) # Reset the motor commands to zero packedData = vrep.simxPackFloats([0, 0, 0, 0]) raw_bytes = (ctypes.c_ubyte * len(packedData)).from_buffer_copy(packedData) err = vrep.simxSetStringSignal(self.cid, "rotorTargetVelocities", raw_bytes, vrep_mode) self.pos = [0, 0, 0] self.pos_err = [0, 0, 0] self.t_pos = [0, 0, 0] self.lin = [0, 0, 0] self.ori = [0, 0, 0] self.ori_err = [0, 0, 0] self.t_ori = [0, 0, 0] self.ang = [0, 0, 0] self.count = 0 self.first = True self.figure = plt.figure() self.x_min = 999 self.y_min = 999 #min distance to wall # Maximum target distance error that can be returned self.max_target_distance = max_target_distance # If noise is being modelled if noise_std is not None: self.noise = True else: self.noise = False # Standard Deviation of the noise for the 4 state variables self.noise_std = noise_std
def __init__(self, max_target_distance=4, noise=False, noise_std=None, dodging=True, target_func=None, cid=None, ori_mode=False): self.ori_mode = ori_mode # If a cid is specified, assume the connection has already been # established and should remain open if cid is None: vrep.simxFinish(-1) # just in case, close all opened connections self.cid = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5) else: self.cid = cid if self.cid != -1: print('Connected to V-REP remote API server, client id: %s' % self.cid) vrep.simxStartSimulation(self.cid, vrep.simx_opmode_oneshot) if SYNC: vrep.simxSynchronous(self.cid, True) else: print('Failed connecting to V-REP remote API server') self.exit() 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) # Reset the motor commands to zero packedData = vrep.simxPackFloats([0, 0, 0, 0]) raw_bytes = (ctypes.c_ubyte * len(packedData)).from_buffer_copy(packedData) err = vrep.simxSetStringSignal(self.cid, "rotorTargetVelocities", raw_bytes, vrep_mode) self.pos = [0, 0, 0] self.pos_err = [0, 0, 0] self.t_pos = [0, 0, 0] self.lin = [0, 0, 0] self.ori = [0, 0, 0] self.ori_err = [0, 0, 0] self.t_ori = [0, 0, 0] self.ang = [0, 0, 0] self.count = 0 # Maximum target distance error that can be returned self.max_target_distance = max_target_distance # If noise is being modelled if noise_std is not None: self.noise = True else: self.noise = False # Standard Deviation of the noise for the 4 state variables self.noise_std = noise_std # Overwrite the get_target method if the target is to be controlled by a # function instead of by V-REP if target_func is not None: self.step = 0 self.target_func = target_func def get_target(): self.t_pos, self.t_ori = self.target_func(self.step) self.step += 1 self.get_target = get_target
def followPath(clientID, path, goal): # arrange the data given to this function pathx = path[0] pathy = path[1] pathz = path[2] # get the start infos for the following errorCode, UAV = vrep.simxGetObjectHandle(clientID, "UAV", vrep.simx_opmode_oneshot_wait) errorCode, pos = vrep.simxGetObjectPosition(clientID, UAV, -1, vrep.simx_opmode_streaming) errorCode, orientation = vrep.simxGetObjectOrientation(clientID, UAV, -1, vrep.simx_opmode_streaming) time.sleep(0.1) errorCode, pos = vrep.simxGetObjectPosition(clientID, UAV, -1, vrep.simx_opmode_buffer) errorCode, orientation = vrep.simxGetObjectOrientation(clientID, UAV, -1, vrep.simx_opmode_buffer) # some initialization xPosition = pos[0] yPosition = pos[1] zPosition = pos[2] xp = [] yp = [] zp = [] xpnear = [] ypnear = [] zpnear = [] xerror = [] yerror = [] zerror = [] # define the radius around the goal, in which the UAV is slowed to lower the error between UAV and path slowvelo_dis = 4 absolut_dis = 1 # path-following loop # here is the goal defined, in this case reach the goal <5cm, <2cm also works in nearly all case, but needs some more time at the end while absolut_dis > 0.05: # define the max possible velocities xvelomax = 1.5 yvelomax = 1.5 # define the max possible turnrate turnmax = 8 # refresh the UAV information errorCode, pos = vrep.simxGetObjectPosition(clientID, UAV, -1, vrep.simx_opmode_buffer) errorCode, orientation = vrep.simxGetObjectOrientation(clientID, UAV, -1, vrep.simx_opmode_buffer) # arrange the information xPosition = pos[0] yPosition = pos[1] zPosition = pos[2] # calculate the distance to the goal absolut_dis = math.sqrt( (xPosition - pathx[(len(pathx) - 1)]) ** 2 + (yPosition - pathy[(len(pathx) - 1)]) ** 2 + (zPosition - pathz[(len(pathx) - 1)]) ** 2 ) # lower the max velocities in the defined radius, closer to the goal the UAV is more slowed if absolut_dis < slowvelo_dis: xvelomax = (xvelomax - 0.15) * absolut_dis / slowvelo_dis * absolut_dis / slowvelo_dis + 0.15 yvelomax = (yvelomax - 0.15) * absolut_dis / slowvelo_dis * absolut_dis / slowvelo_dis + 0.15 # save some information for the documentation after the following xp.append(xPosition) yp.append(yPosition) zp.append(zPosition) # get the direction the UAV should fly to follow the path vec, vec_path, pnear, dis = pathfollowing.findnearst(pos, path) # some more info for documentation xpnear.append(pnear[0]) ypnear.append(pnear[1]) zpnear.append(pnear[2]) xerror.append(abs(xPosition - pnear[0])) yerror.append(abs(yPosition - pnear[1])) zerror.append(abs(zPosition - pnear[2])) # calculate the velocities from the direction vector absolut = math.sqrt(vec[0, 0] ** 2 + vec[0, 1] ** 2) xvelo = 0 yvelo = 0 height = zPosition xvelo_w = xvelomax * vec[0, 0] / absolut # ref_velx yvelo_w = yvelomax * vec[0, 1] / absolut # ref_vely height = zPosition + vec[0, 2] # e # ref_angz, angle between (1/0/0) and (xvelo/yvelo/0) a1 = [1, 0, 0] b1 = [vec_path[0], vec_path[1], 0] ref_angz = angle_calculation(a1, b1) # arrange some info angle = orientation[2] dangle = ref_angz - angle # angle correction to prevent the UAV from turning not into the shorter direction if dangle > np.pi: dangle = dangle - 2 * np.pi if dangle < -np.pi: dangle = 2 * np.pi + dangle # calculate the turnrate, its lower, if the orientation error is lower, to prevent the UAV from doing a oszillation around the right orientation veloz = turnmax * (dangle) / np.pi # transfom the velocities into the UAV-coordinate-system from the world-coordinates xvelo = xvelo_w * np.cos(angle) + yvelo_w * np.sin(angle) yvelo = yvelo_w * np.cos(angle) - xvelo_w * np.sin(angle) # modify the velocities to improve the path following, by using the current distances and errors to the path and his orientation if abs(vec[0, 2]) > 0.2: xvelo = 0 yvelo = 0 else: if dis < 0.5: xvelo = ( xvelo * ((np.pi - abs(dangle)) / np.pi) ** 130 * (((0.2 - abs(vec[0, 2])) / 0.2) ** 6) * ((0.5 - dis) / 0.5) ) yvelo = ( yvelo * ((np.pi - abs(dangle)) / np.pi) ** 15 * (((0.2 - abs(vec[0, 2])) / 0.2) ** 6) * (dis + 0.2) ** 2 ) else: xvelo = ( xvelo * ((np.pi - abs(dangle)) / np.pi) ** 130 * (((0.2 - abs(vec[0, 2])) / 0.2) ** 6) * ((0.5 - dis) / 0.5) ) yvelo = yvelo * ((np.pi - abs(dangle)) / np.pi) ** 15 * (((0.2 - abs(vec[0, 2])) / 0.2) ** 6) if abs(vec_path[2]) > 0.003: print vec_path[2] xvelo = xvelo * (0.003 / abs(vec_path[2])) ** 3 yvelo = yvelo * (0.003 / abs(vec_path[2])) height = height + vec[0, 2] * 4 # create the data-signal, which is read to the LUA-script data = [xvelo, yvelo, height, 0, 0, veloz] packedData = vrep.simxPackFloats(data) vrep.simxClearStringSignal(clientID, "Command_Twist_Quad", vrep.simx_opmode_oneshot) vrep.simxSetStringSignal(clientID, "Command_Twist_Quad", packedData, vrep.simx_opmode_oneshot) # trigger the next simulation step vrep.simxSynchronousTrigger(clientID) # clean up some signals data = [0, 0, pathz[(len(pathx) - 1)], 0, 0, 0] packedData = vrep.simxPackFloats(data) vrep.simxClearStringSignal(clientID, "Command_Twist_Quad", vrep.simx_opmode_oneshot) vrep.simxSetStringSignal(clientID, "Command_Twist_Quad", packedData, vrep.simx_opmode_oneshot) data2 = [0] packedData2 = vrep.simxPackFloats(data2) vrep.simxClearStringSignal(clientID, "Command_Path_Ready", vrep.simx_opmode_oneshot) vrep.simxSetStringSignal(clientID, "Command_Path_Ready", packedData2, vrep.simx_opmode_oneshot) # prepare some data for the plot of the path xyzparray = np.ndarray(shape=(3, len(xp)), dtype=float) for next in range(len(xp)): xyzparray[0, next] = xp[next] xyzparray[1, next] = yp[next] xyzparray[2, next] = zp[next] xyzneararray = np.ndarray(shape=(3, len(xpnear)), dtype=float) for next in range(len(xpnear)): xyzneararray[0, next] = xpnear[next] xyzneararray[1, next] = ypnear[next] xyzneararray[2, next] = zpnear[next] # return plot data return xyzparray, xyzneararray
# # Replies are coded in a same way. An executed command should reply with the same command counter. # # Above simple protocol is just an example: you could use your own protocol! (but it has to be the same on the V-REP side) # 1. First send a command to display a specific message in a dialog box: cmdID=1 # this command id means: 'display a text in a message box' cmdCnt=0 cmdData=b'Hello world!' dataToSend=getCmdString(cmdID,cmdCnt,cmdData) vrep.simxWriteStringStream(clientID,'commandsFromRemoteApiClient',dataToSend,vrep.simx_opmode_oneshot) replyData=waitForCmdReply(cmdCnt) if sys.version_info[0] == 3: replyData=str(replyData,'utf-8') print ('Return string: ',replyData) # display the reply from V-REP (in this case, just a string) # 2. Now create a dummy object at coordinate 0.1,0.2,0.3: cmdID=2 # this command id means: 'create a dummy at a specific coordinate with a specific name' cmdCnt=cmdCnt+1 cmdData=b'MyDummyName'+vrep.simxPackFloats([0.1,0.2,0.3]) dataToSend=getCmdString(cmdID,cmdCnt,cmdData) vrep.simxWriteStringStream(clientID,'commandsFromRemoteApiClient',dataToSend,vrep.simx_opmode_oneshot) replyData=waitForCmdReply(cmdCnt) print ('Dummy handle: ',vrep.simxUnpackInts(replyData)[0]) # display the reply from V-REP (in this case, the handle of the created dummy) # Now close the connection to V-REP: vrep.simxFinish(clientID) else: print ('Failed connecting to remote API server') print ('Program ended')
def run_simulation(self, trajectory): """ Trajectory is a list 6 pairs of vectors, each of the same length. For each pair: 1. The first vector is the position of the motor in rad. 2. The second vector is the max velocity of the motor in rad/s. """ self._com(remote_api.simxStopSimulation, get=False) if self.verbose: print("Setting parameters...") traj = self._prepare_traj(trajectory) packed_data = remote_api.simxPackFloats(traj) raw_bytes = (ctypes.c_ubyte * len(packed_data)).from_buffer_copy(packed_data) assert remote_api.simxSetStringSignal(self.api_id, 'trajectory', raw_bytes, remote_api.simx_opmode_oneshot_wait) == 0 self._com(remote_api.simxSetIntegerSignal, 'state', 1, get=False) self._com(remote_api.simxGetIntegerSignal, 'state', get=True, mode=remote_api.simx_opmode_streaming, ret_ok=(0, 1)) time.sleep(0.1) self._com(remote_api.simxStartSimulation, get=False) if self.verbose: print("Simulation started.") time.sleep(0.01) start_time = time.time() while not self.simulation_paused(): if time.time() - start_time > 60.0: print("Simulation seems hung. Exiting...") sys.exit(1) time.sleep(0.005) time.sleep(1.0) if self.verbose: print("Getting resulting parameters.") object_sensors = self._get_signal('object_sensors') collide_data = self._get_signal('collide_data') contact_data = self._get_signal('contact_data') contact_type = self._get_signal('contact_type', int_type=True) contacts = [] for i in range(0, len(contact_type), 3): step = contact_type[i] obj_names = [self.handles[contact_type[i+1]],self.handles[contact_type[i+2]]] data = contact_data[2*i:2*i+6] contacts.append(Contact(step, obj_names, data)) first_c, last_c = None, None max_f, max_c = 0.0, None for c in contacts: if c.obj_names[0] in self.tracked_objects or c.obj_names[1] in self.tracked_objects: if first_c is None: first_c = c last_c = c if max_f < c.force_norm: max_f, max_c = c.force_norm, c salient_contacts = {'first': first_c, 'last': last_c, 'max': max_c} if first_c is not None: print('first contact: {} {:.2f} N'.format('|'.join(first_c.obj_names), first_c.force_norm)) if max_c is not None: print(' max contact: {} {:.2f} N'.format('|'.join(max_c.obj_names), max_c.force_norm)) marker_sensors = None if self.cfg.sprims.tip: marker_sensors = np.array(remote_api.simxUnpackFloats( remote_api.simxGetStringSignal(self.api_id, 'marker_sensors', remote_api.simx_opmode_oneshot_wait))) # assert len(positions) == len(quaternions) == len(velocities) if self.verbose: print("End of simulation.") joint_sensors = None return {'object_sensors' : object_sensors, 'joint_sensors' : joint_sensors, 'marker_sensors' : marker_sensors, 'collide_data' : collide_data, 'contacts' : contacts, 'salient_contacts': salient_contacts}
h, Xside, Yside, vel = cui.setStuff() vrep.simxFinish(-1) clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 15000, 5) if clientID != -1: print('| Connected to AUV |') res, auv = vrep.simxGetObjectHandle(clientID, "Sphere", vrep.simx_opmode_oneshot_wait) # создание объектов классов path = PP(clientID, auv, vel) camera = cam(clientID, "Vision_sensor") # отправка нулевых сил forceD = [0, 0, 0, 0] s_force = vrep.simxPackFloats(forceD) vrep.simxSetStringSignal(clientID, "ForceD", s_force, vrep.simx_opmode_oneshot) # планировщик маршрута path.setPolygon(h, Xside, Yside, vel) path.makePath() # ========== начало работы ============================================================================================= time.sleep(1) vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot) print("| start |") print("| |") while True: path.stateOfMove() camera.stateOfSearch(path.returnCoord(), auv)
""" #UAV_main.py import vrep import UAV_mapgen import UAV_pathfinding import UAV_VREP import numpy as np import time from scipy import ndimage #start Connection to V-REP vrep.simxFinish(-1) # just in case, close all opened connections clientID=vrep.simxStart('127.0.0.1',19999,True,True,5000,5) # Connect to V-REP data=[0,0,1,0,0,0] packedData=vrep.simxPackFloats(data) vrep.simxClearStringSignal(clientID,'Command_Twist_Quad',vrep.simx_opmode_oneshot) vrep.simxSetStringSignal(clientID,'Command_Twist_Quad',packedData,vrep.simx_opmode_oneshot) #generate mapdata, load data if mapdata for scene exist mapdata=UAV_mapgen.mapgen_fast("columns_and_blocks",16,16,10,clientID) # Extending obstacles boundaries using dilation mapdata = ndimage.binary_dilation(mapdata).astype(np.int64) #mapdata[:,:,1] = np.ones_like(mapdata[:,:,1]) #mapdata[:,:,2] = np.ones_like(mapdata[:,:,2]) # Get start and goal data from v-REP start_position=UAV_VREP.getPosition(clientID,'UAV_target') goal_position=UAV_VREP.getPosition(clientID,'goal_new') print ("Start position:", start_position)
def run_simulation(self, trajectory): """ Trajectory is a list 6 pairs of vectors, each of the same length. For each pair: 1. The first vector is the position of the motor in rad. 2. The second vector is the max velocity of the motor in rad/s. """ self._com(remote_api.simxStopSimulation, get=False) if self.verbose: print("Setting parameters...") traj = self._prepare_traj(trajectory) packed_data = remote_api.simxPackFloats(traj) raw_bytes = (ctypes.c_ubyte * len(packed_data)).from_buffer_copy(packed_data) assert remote_api.simxSetStringSignal( self.api_id, 'trajectory', raw_bytes, remote_api.simx_opmode_oneshot_wait) == 0 self._com(remote_api.simxSetIntegerSignal, 'state', 1, get=False) self._com(remote_api.simxGetIntegerSignal, 'state', get=True, mode=remote_api.simx_opmode_streaming, ret_ok=(0, 1)) time.sleep(0.1) self._com(remote_api.simxStartSimulation, get=False) if self.verbose: print("Simulation started.") time.sleep(0.01) start_time = time.time() while not self.simulation_paused(): if time.time() - start_time > 60.0: print("Simulation seems hung. Exiting...") sys.exit(1) time.sleep(0.005) time.sleep(1.0) if self.verbose: print("Getting resulting parameters.") object_sensors = self._get_signal('object_sensors') collide_data = self._get_signal('collide_data') contact_data = self._get_signal('contact_data') contact_type = self._get_signal('contact_type', int_type=True) contacts = [] for i in range(0, len(contact_type), 3): step = contact_type[i] obj_names = [ self.handles[contact_type[i + 1]], self.handles[contact_type[i + 2]] ] data = contact_data[2 * i:2 * i + 6] contacts.append(Contact(step, obj_names, data)) first_c, last_c = None, None max_f, max_c = 0.0, None for c in contacts: if c.obj_names[0] in self.tracked_objects or c.obj_names[ 1] in self.tracked_objects: if first_c is None: first_c = c last_c = c if max_f < c.force_norm: max_f, max_c = c.force_norm, c salient_contacts = {'first': first_c, 'last': last_c, 'max': max_c} if first_c is not None: print('first contact: {} {:.2f} N'.format( '|'.join(first_c.obj_names), first_c.force_norm)) if max_c is not None: print(' max contact: {} {:.2f} N'.format( '|'.join(max_c.obj_names), max_c.force_norm)) marker_sensors = None if self.cfg.sprims.tip: marker_sensors = np.array( remote_api.simxUnpackFloats( remote_api.simxGetStringSignal( self.api_id, 'marker_sensors', remote_api.simx_opmode_oneshot_wait))) # assert len(positions) == len(quaternions) == len(velocities) if self.verbose: print("End of simulation.") joint_sensors = None return { 'object_sensors': object_sensors, 'joint_sensors': joint_sensors, 'marker_sensors': marker_sensors, 'collide_data': collide_data, 'contacts': contacts, 'salient_contacts': salient_contacts }
def followPath2(clientID,path,goal): pathx=path[0] pathy=path[1] pathz=path[2] errorCode,UAV=vrep.simxGetObjectHandle(clientID,'UAV',vrep.simx_opmode_oneshot_wait) errorCode,pos=vrep.simxGetObjectPosition(clientID,UAV,-1,vrep.simx_opmode_streaming) errorCode,orientation=vrep.simxGetObjectOrientation(clientID,UAV,-1,vrep.simx_opmode_streaming) time.sleep(0.1) errorCode,pos=vrep.simxGetObjectPosition(clientID,UAV,-1,vrep.simx_opmode_buffer) errorCode,orientation=vrep.simxGetObjectOrientation(clientID,UAV,-1,vrep.simx_opmode_buffer) xPosition=pos[0] yPosition=pos[1] zPosition=pos[2] vecp=[0,0,0] pdangle=0 pveloz=0 pangle=0 pref_angz=0 while (xPosition > pathx[199]+0.1) or (xPosition < pathx[199]-0.1) or (yPosition > pathy[199]+0.1) or (yPosition < pathy[199]-0.1) or (zPosition > pathz[199]+0.1) or (zPosition < pathz[199]-0.1): xvelomax=0.6 yvelomax=0.6 zmax=1 absolut_dis=math.sqrt((xPosition-pathx[199])**2+(yPosition-pathy[199])**2+(zPosition-pathz[199])**2) #print absolut_dis slowvelo_dis=4 if absolut_dis < slowvelo_dis: xvelomax=xvelomax*absolut_dis/slowvelo_dis yvelomax=yvelomax*absolut_dis/slowvelo_dis #zmax=zmax*absolut_dis/slowvelo_dis start_time = time.time() errorCode,pos=vrep.simxGetObjectPosition(clientID,UAV,-1,vrep.simx_opmode_buffer) errorCode,orientation=vrep.simxGetObjectOrientation(clientID,UAV,-1,vrep.simx_opmode_buffer) xPosition=pos[0] yPosition=pos[1] zPosition=pos[2] # Find nearest point pnear = pathfollowing.find_nearp(pos, path) vec=pathfollowing.findnearst(pos,path) absolut=math.sqrt(vec[0]**2+vec[1]**2+vec[2]**2) xvelo=0 yvelo=0 height=zPosition xvelo_w=xvelomax*vec[0]/absolut#ref_velx yvelo_w=yvelomax*vec[1]/absolut#ref_vely height = pnear[2] #ref_angz, angle between (1/0/0) and (xvelo/yvelo/0) a1=[1,0,0] b1=[xvelo_w,yvelo_w,0] ref_angz=angle_calculationx(a1,b1) if orientation[2]<0: angle=2*np.pi+orientation[2] else: angle=orientation[2] if ref_angz<0: ref_angz=2*np.pi+ref_angz dangle=angle-ref_angz if dangle>np.pi: veloz=6*(2*np.pi-dangle)/np.pi else: if dangle<-np.pi: veloz=-6*(2*np.pi+dangle)/np.pi else: veloz=-6*(dangle)/np.pi if dangle-pdangle>1: print (pdangle,dangle) print (pangle,angle) print (pref_angz,ref_angz) print (pveloz,veloz) pdangle=dangle pveloz=veloz pangle=angle pref_angz=ref_angz xvelo=xvelo_w*np.cos(-orientation[2])-yvelo_w*np.sin(-orientation[2]) yvelo=xvelo_w*np.sin(-orientation[2])+yvelo_w*np.cos(-orientation[2]) data=[xvelo,yvelo,height,0,0,veloz] packedData=vrep.simxPackFloats(data) vrep.simxClearStringSignal(clientID,'Command_Twist_Quad',vrep.simx_opmode_oneshot) vrep.simxSetStringSignal(clientID,'Command_Twist_Quad',packedData,vrep.simx_opmode_oneshot) data=[0,0,height,0,0,0] packedData=vrep.simxPackFloats(data) vrep.simxClearStringSignal(clientID,'Command_Twist_Quad',vrep.simx_opmode_oneshot) vrep.simxSetStringSignal(clientID,'Command_Twist_Quad',packedData,vrep.simx_opmode_oneshot)
def followPath(clientID, path, goal): #arrange the data given to this function pathx = path[0] pathy = path[1] pathz = path[2] #get the start infos for the following errorCode, UAV = vrep.simxGetObjectHandle(clientID, 'UAV', vrep.simx_opmode_oneshot_wait) errorCode, pos = vrep.simxGetObjectPosition(clientID, UAV, -1, vrep.simx_opmode_streaming) errorCode, orientation = vrep.simxGetObjectOrientation( clientID, UAV, -1, vrep.simx_opmode_streaming) time.sleep(0.1) errorCode, pos = vrep.simxGetObjectPosition(clientID, UAV, -1, vrep.simx_opmode_buffer) errorCode, orientation = vrep.simxGetObjectOrientation( clientID, UAV, -1, vrep.simx_opmode_buffer) #some initialization xPosition = pos[0] yPosition = pos[1] zPosition = pos[2] xp = [] yp = [] zp = [] xpnear = [] ypnear = [] zpnear = [] xerror = [] yerror = [] zerror = [] #define the radius around the goal, in which the UAV is slowed to lower the error between UAV and path slowvelo_dis = 4 absolut_dis = 1 #path-following loop #here is the goal defined, in this case reach the goal <5cm, <2cm also works in nearly all case, but needs some more time at the end while (absolut_dis > 0.05): #define the max possible velocities xvelomax = 1.5 yvelomax = 1.5 #define the max possible turnrate turnmax = 8 #refresh the UAV information errorCode, pos = vrep.simxGetObjectPosition(clientID, UAV, -1, vrep.simx_opmode_buffer) errorCode, orientation = vrep.simxGetObjectOrientation( clientID, UAV, -1, vrep.simx_opmode_buffer) #arrange the information xPosition = pos[0] yPosition = pos[1] zPosition = pos[2] #calculate the distance to the goal absolut_dis = math.sqrt((xPosition - pathx[(len(pathx) - 1)])**2 + (yPosition - pathy[(len(pathx) - 1)])**2 + (zPosition - pathz[(len(pathx) - 1)])**2) #lower the max velocities in the defined radius, closer to the goal the UAV is more slowed if absolut_dis < slowvelo_dis: xvelomax = ( xvelomax - 0.15 ) * absolut_dis / slowvelo_dis * absolut_dis / slowvelo_dis + 0.15 yvelomax = ( yvelomax - 0.15 ) * absolut_dis / slowvelo_dis * absolut_dis / slowvelo_dis + 0.15 #save some information for the documentation after the following xp.append(xPosition) yp.append(yPosition) zp.append(zPosition) #get the direction the UAV should fly to follow the path vec, vec_path, pnear, dis = pathfollowing.findnearst(pos, path) #some more info for documentation xpnear.append(pnear[0]) ypnear.append(pnear[1]) zpnear.append(pnear[2]) xerror.append(abs(xPosition - pnear[0])) yerror.append(abs(yPosition - pnear[1])) zerror.append(abs(zPosition - pnear[2])) #calculate the velocities from the direction vector absolut = math.sqrt(vec[0, 0]**2 + vec[0, 1]**2) xvelo = 0 yvelo = 0 height = zPosition xvelo_w = xvelomax * vec[0, 0] / absolut #ref_velx yvelo_w = yvelomax * vec[0, 1] / absolut #ref_vely height = zPosition + vec[0, 2] #e #ref_angz, angle between (1/0/0) and (xvelo/yvelo/0) a1 = [1, 0, 0] b1 = [vec_path[0], vec_path[1], 0] ref_angz = angle_calculation(a1, b1) #arrange some info angle = orientation[2] dangle = ref_angz - angle #angle correction to prevent the UAV from turning not into the shorter direction if dangle > np.pi: dangle = dangle - 2 * np.pi if dangle < -np.pi: dangle = 2 * np.pi + dangle #calculate the turnrate, its lower, if the orientation error is lower, to prevent the UAV from doing a oszillation around the right orientation veloz = turnmax * (dangle) / np.pi #transfom the velocities into the UAV-coordinate-system from the world-coordinates xvelo = xvelo_w * np.cos(angle) + yvelo_w * np.sin(angle) yvelo = yvelo_w * np.cos(angle) - xvelo_w * np.sin(angle) #modify the velocities to improve the path following, by using the current distances and errors to the path and his orientation if abs(vec[0, 2]) > 0.2: xvelo = 0 yvelo = 0 else: if dis < 0.5: xvelo = xvelo * ((np.pi - abs(dangle)) / np.pi)**130 * (( (0.2 - abs(vec[0, 2])) / 0.2)**6) * ((0.5 - dis) / 0.5) yvelo = yvelo * ((np.pi - abs(dangle)) / np.pi)**15 * (( (0.2 - abs(vec[0, 2])) / 0.2)**6) * (dis + 0.2)**2 else: xvelo = xvelo * ((np.pi - abs(dangle)) / np.pi)**130 * (( (0.2 - abs(vec[0, 2])) / 0.2)**6) * ((0.5 - dis) / 0.5) yvelo = yvelo * ((np.pi - abs(dangle)) / np.pi)**15 * (( (0.2 - abs(vec[0, 2])) / 0.2)**6) if abs(vec_path[2]) > 0.003: print vec_path[2] xvelo = xvelo * (0.003 / abs(vec_path[2]))**3 yvelo = yvelo * (0.003 / abs(vec_path[2])) height = height + vec[0, 2] * 4 #create the data-signal, which is read to the LUA-script data = [xvelo, yvelo, height, 0, 0, veloz] packedData = vrep.simxPackFloats(data) vrep.simxClearStringSignal(clientID, 'Command_Twist_Quad', vrep.simx_opmode_oneshot) vrep.simxSetStringSignal(clientID, 'Command_Twist_Quad', packedData, vrep.simx_opmode_oneshot) #trigger the next simulation step vrep.simxSynchronousTrigger(clientID) #clean up some signals data = [0, 0, pathz[(len(pathx) - 1)], 0, 0, 0] packedData = vrep.simxPackFloats(data) vrep.simxClearStringSignal(clientID, 'Command_Twist_Quad', vrep.simx_opmode_oneshot) vrep.simxSetStringSignal(clientID, 'Command_Twist_Quad', packedData, vrep.simx_opmode_oneshot) data2 = [0] packedData2 = vrep.simxPackFloats(data2) vrep.simxClearStringSignal(clientID, 'Command_Path_Ready', vrep.simx_opmode_oneshot) vrep.simxSetStringSignal(clientID, 'Command_Path_Ready', packedData2, vrep.simx_opmode_oneshot) #prepare some data for the plot of the path xyzparray = np.ndarray(shape=(3, len(xp)), dtype=float) for next in range(len(xp)): xyzparray[0, next] = xp[next] xyzparray[1, next] = yp[next] xyzparray[2, next] = zp[next] xyzneararray = np.ndarray(shape=(3, len(xpnear)), dtype=float) for next in range(len(xpnear)): xyzneararray[0, next] = xpnear[next] xyzneararray[1, next] = ypnear[next] xyzneararray[2, next] = zpnear[next] #return plot data return xyzparray, xyzneararray
cmdID = 1 # this command id means: 'display a text in a message box' cmdCnt = 0 cmdData = b'Hello world!' dataToSend = getCmdString(cmdID, cmdCnt, cmdData) vrep.simxWriteStringStream(clientID, 'commandsFromRemoteApiClient', dataToSend, vrep.simx_opmode_oneshot) replyData = waitForCmdReply(cmdCnt) if sys.version_info[0] == 3: replyData = str(replyData, 'utf-8') print('Return string: ', replyData ) # display the reply from V-REP (in this case, just a string) # 2. Now create a dummy object at coordinate 0.1,0.2,0.3: cmdID = 2 # this command id means: 'create a dummy at a specific coordinate with a specific name' cmdCnt = cmdCnt + 1 cmdData = b'MyDummyName' + vrep.simxPackFloats([0.1, 0.2, 0.3]) dataToSend = getCmdString(cmdID, cmdCnt, cmdData) vrep.simxWriteStringStream(clientID, 'commandsFromRemoteApiClient', dataToSend, vrep.simx_opmode_oneshot) replyData = waitForCmdReply(cmdCnt) print( 'Dummy handle: ', vrep.simxUnpackInts(replyData)[0] ) # display the reply from V-REP (in this case, the handle of the created dummy) # Now close the connection to V-REP: vrep.simxFinish(clientID) else: print('Failed connecting to remote API server') print('Program ended')
def _set_Action(self, action, prefix=""): vrep.simxSetStringSignal(self.id_, prefix + "actions", vrep.simxPackFloats(action), vrep.simx_opmode_oneshot)