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 JacoHandGrap(): global grap_state # input input_invalid = 1 while (input_invalid): input_msg = input("grap or release? (please enter grap or release)") if ((input_msg == "grap") | (input_msg == "release")): input_invalid = 0 else: print("invalid input!\n") # grap if (input_msg == "grap"): if (grap_state == 0): vrep.simxSetStringSignal(clientID, 'jacoHand', 'true', vrep.simx_opmode_oneshot) time.sleep(1) grap_state = 1 else: print("Already grapped. Ignore your command.") return # release if (input_msg == "release"): if (grap_state == 1): vrep.simxSetStringSignal(clientID, 'jacoHand', 'false', vrep.simx_opmode_oneshot) grap_state = 0 else: print("Already release. Ignore your command.") return
def search_object(): print('Searching...') vrep.simxSetStringSignal(clientID, 'IKCommands', 'LookPos', vrep.simx_opmode_oneshot) # "Looking" position center_res = 10 robot_joints.velocity[0] = 0.5 robot_joint_pub.publish(robot_joints) # vrep.simxSetJointTargetVelocity(clientID, joint_handles[0], 0.5, vrep.simx_opmode_streaming) tic_so = time.clock() objectFound = False while objectFound is False: toc_so = time.clock() robot_joints.velocity[0] = 0.5 robot_joint_pub.publish(robot_joints) rgb_image.resize([resolution[0], resolution[1], 3]) [grasp_type, im_x, im_y, obj_pix_prop] = image_process(rgb_image, resolution) # if grasp_type > 0: print('Object Found!') print('Grasp Type: ', grasp_type) full_center(im_x, im_y, center_res, resolution) # lock_all_joints() # center_object(0.0025, im_x, im_y, resolution) return 1 elif toc_so - tic_so > 20: return 6
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 search_object_2(): print('Searching...') vrep.simxSetStringSignal(clientID, 'IKCommands', 'LookPos', vrep.simx_opmode_oneshot) # "Looking" position center_res = 10 tic_so = time.clock() objectFound = False while objectFound is False: toc_so = time.clock() errorCode2, resolution, image = vrep.simxGetVisionSensorImage( clientID, cam_Handle, 0, vrep.simx_opmode_buffer) im = np.array(image, dtype=np.uint8) if len(resolution) > 0: im.resize([resolution[0], resolution[1], 3]) obj_list = image_process_2(im, resolution) if len(obj_list) > 0: print(obj_list[0]) if len(obj_list[0]) > 1: im_x = obj_list[0][1] im_y = obj_list[0][2] print('Object Found!') full_center(im_x, im_y, center_res, resolution) return 1 errorCode, joint_ang_0 = vrep.simxGetJointPosition( clientID, joint_handles[0], vrep.simx_opmode_buffer) errorCode, joint_ang_4 = vrep.simxGetJointPosition( clientID, joint_handles[4], vrep.simx_opmode_buffer) if joint_ang_0 > 70 * math.pi / 180: vrep.simxSetJointTargetVelocity(clientID, joint_handles[0], 0, vrep.simx_opmode_streaming) vrep.simxSetJointTargetVelocity(clientID, joint_handles[4], -0.2, vrep.simx_opmode_streaming) if joint_ang_4 < 100 * math.pi / 180: vrep.simxSetJointTargetVelocity(clientID, joint_handles[0], -0.2, vrep.simx_opmode_streaming) vrep.simxSetJointTargetVelocity(clientID, joint_handles[4], 0, vrep.simx_opmode_streaming) if joint_ang_0 < -70 * math.pi / 180: vrep.simxSetJointTargetVelocity(clientID, joint_handles[0], 0, vrep.simx_opmode_streaming) vrep.simxSetJointTargetVelocity(clientID, joint_handles[4], 0.2, vrep.simx_opmode_streaming) if joint_ang_4 > 118 * math.pi / 180: vrep.simxSetJointTargetVelocity(clientID, joint_handles[0], 0.2, vrep.simx_opmode_streaming) vrep.simxSetJointTargetVelocity(clientID, joint_handles[4], 0, vrep.simx_opmode_streaming) if toc_so - tic_so > 20: return 6
def final_grasp(): # print('Final Grasp!') gripper_type = 2 if gripper_type == 2: vrep.simxSetJointTargetVelocity(clientID, joint_handles[4], 0.04, vrep.simx_opmode_oneshot) time.sleep(2) time.sleep(1) vrep.simxSetStringSignal(clientID, 'Hand', 'true', vrep.simx_opmode_oneshot) time.sleep(5) return 6
def set_led_light(self, led_number, intensity=0): if led_number > 7 or led_number < 0: if self.debug: err_print(prefix="SET LED LIGHT", message=["LED NUMBER SHOULD BE BETWEEN 0 AND 7"]) elif intensity not in (0, 1): pass else: self.led_lights[led_number] = intensity pack = vrep.simxPackInts([led_number, intensity]) vrep.simxSetStringSignal(self.client_id, 'EPUCK_LED' + self.suffix, pack, vrep.simx_opmode_oneshot_wait)
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')
def _write_actuators(self): """ Write in the robot the actuators values. Don't use directly, instead use 'step()' """ # We make a copy of the actuators list actuators = self._actuators_to_write[:] for m in actuators: if m[0] == 'L': # Leds # Sent as packed string to ensure update of the wanted LED test = vrep.simxPackInts([m[1] + 1, m[2]]) vrep.simxSetStringSignal(self._clientID, 'EPUCK_LED', test, vrep.simx_opmode_oneshot_wait) elif m[0] == 'D': # Set motor speed # maxVel = 120.0 * math.pi / 180.0 # maxVel of ePuck firmware is 1000 # => 120 * pi / (180*1000) = pi/1500 velLeft = m[1] * math.pi / 1500.0 velRight = m[2] * math.pi / 1500.0 if velLeft > 120.0 * math.pi / 180.0: velLeft = 120.0 * math.pi / 180.0 self._debug("velLeft too high, thresholded") if velLeft < -120.0 * math.pi / 180.0: velLeft = -120.0 * math.pi / 180.0 self._debug("velLeft too high, thresholded") if velRight > 120.0 * math.pi / 180.0: velRight = 120.0 * math.pi / 180.0 self._debug("velRight too high, thresholded") if velRight < -120.0 * math.pi / 180.0: velRight = -120.0 * math.pi / 180.0 self._debug("velRRight too high, thresholded") vrep.simxSetFloatSignal(self._clientID, 'EPUCK_velLeft', velLeft, vrep.simx_opmode_oneshot) vrep.simxSetFloatSignal(self._clientID, 'EPUCK_velRight', velRight, vrep.simx_opmode_oneshot) elif m[0] == 'P': # Motor position self._debug('WARNING: Motor position not yet implemented!') else: self._debug('Unknown actuator to write') self._actuators_to_write.remove(m) return
def 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 _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 dvs(cid, handle=None, signal_name="dataFromThisTimestep", dim_x=32, dim_y=32, offset=0, magnitude=1): empty_str = "" raw_bytes = (ctypes.c_ubyte * len(empty_str)).from_buffer_copy(empty_str) err, data = vrep.simxGetStringSignal(cid, "dataFromThisTimeStep", vrep.simx_opmode_oneshot) err = vrep.simxSetStringSignal(cid, "dataFromThisTimeStep", raw_bytes, vrep.simx_opmode_oneshot_wait) image = np.zeros((dim_x, dim_y), dtype='uint8') + offset l = len(data) for i in range(int(l / 4)): b = list(bytearray(data[i * 4:i * 4 + 4])) x_coord = b[0] y_coord = b[1] if x_coord >= 128: x_coord -= 128 polarity = 1 else: #polarity = 0 polarity = -1 timestamp = (b[3] * 256) + b[2] # Distinguish between different polarities #image[127-y_coord][127-x_coord] = polarity * magnitude image[dim_y - 1 - y_coord][dim_x - 1 - x_coord] = polarity * magnitude # No Distinction between different polarities #image[127-y_coord][127-x_coord] = magnitude return image.ravel()
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 dvs(cid, handle=None, signal_name="dataFromThisTimestep", dim_x=32, dim_y=32, offset=0, magnitude=1): empty_str="" raw_bytes = (ctypes.c_ubyte * len(empty_str)).from_buffer_copy(empty_str) err, data = vrep.simxGetStringSignal(cid, "dataFromThisTimeStep", vrep.simx_opmode_oneshot) err = vrep.simxSetStringSignal(cid, "dataFromThisTimeStep", raw_bytes, vrep.simx_opmode_oneshot_wait) image = np.zeros( (dim_x, dim_y), dtype='uint8' ) + offset l = len(data) for i in range( int(l/4) ): b = list(bytearray(data[i*4:i*4+4])) x_coord = b[0] y_coord = b[1] if x_coord >= 128: x_coord -= 128 polarity = 1 else: #polarity = 0 polarity = -1 timestamp = (b[3] * 256) + b[2] # Distinguish between different polarities #image[127-y_coord][127-x_coord] = polarity * magnitude image[dim_y-1-y_coord][dim_x-1-x_coord] = polarity * magnitude # No Distinction between different polarities #image[127-y_coord][127-x_coord] = magnitude return image.ravel()
def _write_actuators(self): """ Write in the robot the actuators values. Don't use directly, instead use 'step()' """ # We make a copy of the actuators list actuators = self._actuators_to_write[:] for m in actuators: if m[0] == 'L': # Leds # Sent as packed string to ensure update of the wanted LED test = vrep.simxPackInts([m[1]+1, m[2]]) vrep.simxSetStringSignal(self._clientID, 'EPUCK_LED', test, vrep.simx_opmode_oneshot_wait) elif m[0] == 'D': # Set motor speed # maxVel = 120.0 * math.pi / 180.0 # maxVel of ePuck firmware is 1000 # => 120 * pi / (180*1000) = pi/1500 velLeft = m[1] * math.pi / 1500.0 velRight = m[2] * math.pi / 1500.0 if velLeft > 120.0 * math.pi / 180.0: velLeft = 120.0 * math.pi / 180.0 self._debug("velLeft too high, thresholded") if velLeft < -120.0 * math.pi / 180.0: velLeft = -120.0 * math.pi / 180.0 self._debug("velLeft too high, thresholded") if velRight > 120.0 * math.pi / 180.0: velRight = 120.0 * math.pi / 180.0 self._debug("velRight too high, thresholded") if velRight < -120.0 * math.pi / 180.0: velRight = -120.0 * math.pi / 180.0 self._debug("velRRight too high, thresholded") vrep.simxSetFloatSignal(self._clientID, 'EPUCK_velLeft', velLeft, vrep.simx_opmode_oneshot) vrep.simxSetFloatSignal(self._clientID, 'EPUCK_velRight', velRight, vrep.simx_opmode_oneshot) elif m[0] == 'P': # Motor position self._debug('WARNING: Motor position not yet implemented!') else: self._debug('Unknown actuator to write') self._actuators_to_write.remove(m) return
def __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 set_swarm_data(self, val): signal_name = "signal" res = vrep.simxSetStringSignal(self.client_id, signal_name, val, vrep.simx_opmode_blocking) if res != 0: err_print("SET WARM DATA", parse_error(res)) raise Exception("ERROR IN SET WARM DATA")
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 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_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 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 sendSignalVREP(self,signalName, signalValue): if type(signalValue) == str: eCode = vrep.simxSetStringSignal(self._VREP_clientId, signalName, asByteArray(signalValue), vrep.simx_opmode_oneshot_wait) elif type(signalValue) == int: eCode = vrep.simxSetIntegerSignal(self._VREP_clientId, signalName, signalValue, vrep.simx_opmode_oneshot_wait) elif type(signalValue) == float: eCode = vrep.simxSetFloatSignal(self._VREP_clientId, signalName, signalValue, vrep.simx_opmode_oneshot_wait) else: raise Exception("Trying to send a signal of unknown data type. Only strings, floats and and ints are accepted") if eCode != 0: raise Exception("Could not send string signal", signalValue) vrep.simxSynchronousTrigger(self._VREP_clientId)
def place_all_3_obj_setpos(): global objHandles objHandles = np.zeros(3) theta = 45 * math.pi / 180 distance = 0.5 # print('Place All 3 Object Set Positions!!!!!') OP_x_cube, OP_y_cube = -0.1035, distance OP_x_cyl, OP_y_cyl = distance * math.sin( theta) - 0.1035, distance * math.cos(theta) OP_x_sphere, OP_y_sphere = -distance * math.sin( theta) - 0.1035, distance * math.cos(theta) # OP_x_sphere, OP_y_sphere = -0.1035, distance # OP_x_cyl, OP_y_cyl = distance * math.sin(theta) - 0.1035, distance * math.cos(theta) # OP_x_cube, OP_y_cube = -distance * math.sin(theta) - 0.1035, distance * math.cos(theta) vrep.simxSetFloatSignal(clientID, 'ObjPos_x', OP_x_cube, vrep.simx_opmode_oneshot) vrep.simxSetFloatSignal(clientID, 'ObjPos_y', OP_y_cube, vrep.simx_opmode_oneshot) vrep.simxSetStringSignal(clientID, 'Obj_type', 'Cube', vrep.simx_opmode_oneshot) errorCode, objHandles[0] = vrep.simxGetObjectHandle( clientID, 'Cuboid', vrep.simx_opmode_blocking) vrep.simxSetFloatSignal(clientID, 'ObjPos_x', OP_x_cyl, vrep.simx_opmode_oneshot) vrep.simxSetFloatSignal(clientID, 'ObjPos_y', OP_y_cyl, vrep.simx_opmode_oneshot) vrep.simxSetStringSignal(clientID, 'Obj_type', 'Cylinder', vrep.simx_opmode_oneshot) errorCode, objHandles[1] = vrep.simxGetObjectHandle( clientID, 'Cylinder', vrep.simx_opmode_blocking) vrep.simxSetFloatSignal(clientID, 'ObjPos_x', OP_x_sphere, vrep.simx_opmode_oneshot) vrep.simxSetFloatSignal(clientID, 'ObjPos_y', OP_y_sphere, vrep.simx_opmode_oneshot) vrep.simxSetStringSignal(clientID, 'Obj_type', 'Sphere', vrep.simx_opmode_oneshot) errorCode, objHandles[2] = vrep.simxGetObjectHandle( clientID, 'Sphere', vrep.simx_opmode_blocking) vrep.simxSetStringSignal(clientID, 'Obj_type', 'All', vrep.simx_opmode_oneshot) return objHandles
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): # 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 place_all_3_obj(low_thresh, up_thresh): global objHandles objHandles = np.zeros(3) for i in range(0, 3): # Assign new object position (simulation) OP_x = (random.random() - 0.5) * 1.2 OP_y = abs(random.random() - 0.5) * 1.2 while math.sqrt(OP_x**2 + OP_y**2) < low_thresh or math.sqrt( OP_x**2 + OP_y**2) > up_thresh: OP_x = (random.random() - 0.5) * 1.2 OP_y = abs(random.random() - 0.5) * 1.2 vrep.simxSetFloatSignal(clientID, 'ObjPos_x', OP_x, vrep.simx_opmode_oneshot) vrep.simxSetFloatSignal(clientID, 'ObjPos_y', OP_y, vrep.simx_opmode_oneshot) if i == 0: vrep.simxSetStringSignal(clientID, 'Obj_type', 'Cube', vrep.simx_opmode_oneshot) errorCode, objHandles[0] = vrep.simxGetObjectHandle( clientID, 'Cuboid', vrep.simx_opmode_blocking) elif i == 1: vrep.simxSetStringSignal(clientID, 'Obj_type', 'Cylinder', vrep.simx_opmode_oneshot) errorCode, objHandles[1] = vrep.simxGetObjectHandle( clientID, 'Cylinder', vrep.simx_opmode_blocking) elif i == 2: vrep.simxSetStringSignal(clientID, 'Obj_type', 'Sphere', vrep.simx_opmode_oneshot) errorCode, objHandles[2] = vrep.simxGetObjectHandle( clientID, 'Sphere', vrep.simx_opmode_blocking) vrep.simxSetStringSignal(clientID, 'Obj_type', 'All', vrep.simx_opmode_oneshot) # get_20() return objHandles
def traversePath(clientID, path, robotHandle, signalname, signalval): #ret = vrep.simxSetStringSignal(clientID,signalname,'change',vrepConst.simx_opmode_oneshot) #ret = True startTime = time.time() while time.time() - startTime < 4: x = True ret, robotHandle = vrep.simxGetObjectHandle(clientID, robotHandle, vrep.simx_opmode_oneshot_wait) if (ret == 0): for i in range(0, len(path), 3): # print("dinchak") pos = (path[i], path[i + 1], path[i + 2]) ret = setRobotPosition(clientID, robotHandle, pos) startTime = time.time() while time.time() - startTime < 6: x = True print(signalname) print(signalval) ret = vrep.simxSetStringSignal(clientID, signalname, signalval, vrepConst.simx_opmode_oneshot_wait) print(ret) return ret
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 place_object(OP_x, OP_y, obj_type): global objHandle vrep.simxSetFloatSignal(clientID, 'ObjPos_x', OP_x, vrep.simx_opmode_oneshot) vrep.simxSetFloatSignal(clientID, 'ObjPos_y', OP_y, vrep.simx_opmode_oneshot) init_param = [] if obj_type == 'Cube': vrep.simxSetStringSignal(clientID, 'Obj_type', 'Cube', vrep.simx_opmode_oneshot) errorCode, objHandle = vrep.simxGetObjectHandle( clientID, 'Cuboid', vrep.simx_opmode_blocking) paramreadfile = open('ParamFileCube.csv', 'rb+') if obj_type == 'Cylinder': vrep.simxSetStringSignal(clientID, 'Obj_type', 'Cylinder', vrep.simx_opmode_oneshot) errorCode, objHandle = vrep.simxGetObjectHandle( clientID, 'Cylinder', vrep.simx_opmode_blocking) paramreadfile = open('ParamFileCyl.csv', 'rb+') if obj_type == 'Sphere': vrep.simxSetStringSignal(clientID, 'Obj_type', 'Sphere', vrep.simx_opmode_oneshot) errorCode, objHandle = vrep.simxGetObjectHandle( clientID, 'Sphere', vrep.simx_opmode_blocking) paramreadfile = open('ParamFileSphere.csv', 'rb+') param_reader = csv.reader(paramreadfile, delimiter=',', quotechar='|') init_param_t = [list(map(float, rec)) for rec in param_reader] if len(init_param_t) > 0: init_param = init_param_t[0] # print(init_param) paramreadfile.close() return [objHandle, init_param]
def set_signal(self, signal, val): res = vrep.simxSetStringSignal(self.client_id, signal, val, vrep.simx_opmode_blocking) if res != 0: err_print("SET SIGNAL", parse_error(res)) raise Exception("ERROR IN SET SIGNAL")
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 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
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 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 setUtterance(self, utterance): raw_ubytes = (ctypes.c_ubyte * len(utterance)).from_buffer_copy(utterance) vrep.simxSetStringSignal(self.__clientID, self.__name + ":Utterance", raw_ubytes, vrep.simx_opmode_oneshot)
# ******************************** Your robot control code goes here ******************************** # time.sleep(1) pi = np.pi Goal_joint_angles = np.array([-pi/2,0.,0.,0.,0.,pi/2]) SetJointPosition(Goal_joint_angles) SetJointPosition(np.array([0.,0.,0.,0.,0.,pi/2])) # Wait two seconds time.sleep(3) # *************************************************************************************************** # command = np.array([0,0,0,0,0,0]) #close the Jaco hand at first# vrep.simxSetStringSignal(clientID,'jacoHand','false',vrep.simx_opmode_oneshot) j=0 # Decision making hasItem = 0 while(hasItem == 0): if (JacoHandHasItem()): hasItem = 1 time.sleep(3) JacoHandGrasp() time.sleep(1) SetJointPosition(np.array([-pi/2,0.,0.,0.,0.,pi/2])) # JacoHandRelease() break print("No item, wait!") time.sleep(1)
def JacoHandGrasp(): vrep.simxSetStringSignal(clientID,'jacoHand','true',vrep.simx_opmode_oneshot) time.sleep(1) print("Try to grasp.") return
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 JacoHandRelease(): vrep.simxSetStringSignal(clientID,'jacoHand','false',vrep.simx_opmode_oneshot) time.sleep(1) print("Release item.") return
#!/usr/bin/env python import vrep import time vrep.simxFinish(-1) #clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5) clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5) gripper_handler = 'JacoHand' vrep.simxSetStringSignal(clientID, gripper_handler, 'true', vrep.simx_opmode_oneshot) time.sleep(0.6) vrep.simxSetStringSignal(clientID, gripper_handler, 'false', vrep.simx_opmode_oneshot) time.sleep(0.6) if clientID != -1: print('Connected to remote API server') res, v0 = vrep.simxGetObjectHandle(clientID, 'robot_view', vrep.simx_opmode_oneshot_wait) res, v1 = vrep.simxGetObjectHandle(clientID, 'passive_sensor', vrep.simx_opmode_oneshot_wait) err, resolution, image = vrep.simxGetVisionSensorImage( clientID, v0, 0, vrep.simx_opmode_streaming_split) time.sleep(1) while vrep.simxGetConnectionId(clientID) != -1: err, resolution, image = vrep.simxGetVisionSensorImage(
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) time.sleep(0.05)
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)
detection_display.set_data(img) detection_display.axes.figure.canvas.draw() cid = vrep.simxStart('127.0.0.1',20002,True,True,5000,5) if cid != -1: print ('Connected to V-REP remote API server, client id: %s' % cid) vrep.simxStartSimulation( cid, vrep.simx_opmode_oneshot ) else: print ('Failed connecting to V-REP remote API server') exit(1) while True: err, data = vrep.simxGetStringSignal(cid, "dataFromThisTimeStep", vrep.simx_opmode_oneshot) err = vrep.simxSetStringSignal(cid, "dataFromThisTimeStep", raw_bytes, vrep.simx_opmode_oneshot_wait) l = len(data) for i in range( int(l/4) ): b = list(bytearray(data[i*4:i*4+4])) x_coord = b[0] y_coord = b[1] if x_coord > 128: x_coord -= 128 polarity = 1 else: polarity = 0 timestamp = (b[3] * 256) + b[2] # Distinguish between different polarities image[127-y_coord][127-x_coord] = polarity*127
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 braiten2a(self): "Seek light source" "PARAMETERS" intens = 100 ambientIntensRatio = 0 attVect = [0,0,1] HOMEODIR = '/home/stefano/Documents/Projects/Homeostat/Simulator/Python-port/Homeo/' dataDir = 'SimsData-'+strftime("%Y-%m-%d-%H-%M-%S", localtime(time())) simsDataDir = os.path.join(HOMEODIR,"SimulationsData",dataDir) os.mkdir(simsDataDir) print "Saving to: ", simsDataDir e = vrep.simxSetStringSignal(self.simulID,"HOMEO_SIGNAL_SIM_DATA_DIR" ,asByteArray(simsDataDir), vrep.simx_opmode_oneshot_wait) vrep.simxSynchronousTrigger(self.simulID) print "Message sent, error code: ", e "END PARAMETERS" for run in xrange(self.noRuns): eCode = vrep.simxStartSimulation(self.simulID, vrep.simx_opmode_oneshot_wait) vrep.simxSynchronousTrigger(self.simulID) e = vrep.simxSetStringSignal(self.simulID,"HOMEO_SIGNAL_SIM_DATA_DIR" ,asByteArray(simsDataDir), vrep.simx_opmode_oneshot_wait) vrep.simxSynchronousTrigger(self.simulID) print "Simulation started: run number %d, error code: %d"% (run+1, eCode) "Wait until simulation is ready, otherwise we will miss a few movement commands" # sleep(2) np.random.seed(64) # resetRobotInitPose(initPose, self.simulID, ePuckHandle) eCode = vrep.simxSetStringSignal(self.simulID, self.trajStateSignalName, asByteArray("NEWFILE"), vrep.simx_opmode_oneshot_wait) vrep.simxSynchronousTrigger(self.simulID) if eCode == 0: print "Starting a new trajectory file" else: print "ERROR: Could not start a new trajectory file" timeStart = time() for step in xrange(self.noSteps): rightLight = vrep.simxGetFloatSignal(self.simulID, "HOMEO_SIGNAL_rightEye_LIGHT_READING", vrep.simx_opmode_oneshot_wait) vrep.simxSynchronousTrigger(self.simulID) leftLight = vrep.simxGetFloatSignal(self.simulID, "HOMEO_SIGNAL_leftEye_LIGHT_READING", vrep.simx_opmode_oneshot_wait) vrep.simxSynchronousTrigger(self.simulID) # print "rightLight %.3f\t left light: %.3f" %(rightLight[1],leftLight[1]) eCode = vrep.simxSetJointTargetVelocity(self.simulID, self.rightMotor, clip(leftLight[1],0,self.maxSpeed), vrep.simx_opmode_oneshot_wait) eCode = vrep.simxSetJointTargetVelocity(self.simulID, self.leftMotor, clip(rightLight[1],0, self.maxSpeed), vrep.simx_opmode_oneshot_wait) vrep.simxSynchronousTrigger(self.simulID) sleep(0) timeElapsed = time() - timeStart "Stop the robot" self.stopRobot(self.simulID, [self.rightMotor, self.leftMotor]) eCode = vrep.simxSetStringSignal(self.simulID, self.trajStateSignalName, asByteArray("SAVE"), vrep.simx_opmode_oneshot_wait) vrep.simxSynchronousTrigger(self.simulID) if eCode == 0: print "Saving trajectory file" else: print "ERROR: Could not save a new trajectory file" sleep(.5) robotPose = vrep.simxGetObjectPosition(self.simulID, self.robotHandle, -1, vrep.simx_opmode_oneshot_wait)[1][:2] vrep.simxSynchronousTrigger(self.simulID) print "%d: Robot is at: %.3f, %.3f Distance from target is: %.4f. Run took exactly %.3f seconds" % (run, robotPose[0], robotPose[1], self.computeDistance(self.targetPose, robotPose), timeElapsed) # eCode = vrep.simxStopSimulation(self.simulID, vrep.simx_opmode_oneshot_wait) vrep.simxSynchronousTrigger(self.simulID) sleep(1) # eCode = vrep.simxStartSimulation(self.simulID, vrep.simx_opmode_oneshot_wait) # vrep.simxSynchronousTrigger(self.simulID) eCode = vrep.simxSetStringSignal(self.simulID, self.trajStateSignalName, asByteArray("CLOSEFILE"), vrep.simx_opmode_oneshot_wait) vrep.simxSynchronousTrigger(self.simulID) if eCode == 0: print "Starting a new trajectory file" else: print "ERROR: Could not close a new trajectory file" print "Done"
#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) print ("Goal position:", goal_position)
def moveRandomly(self): "Set trajectory data directory and communicate to V-REP" HOMEODIR = '/home/stefano/Documents/Projects/Homeostat/Simulator/Python-port/Homeo/' dataDir = 'SimsData-'+strftime("%Y-%m-%d-%H-%M-%S", localtime(time())) simsDataDir = os.path.join(HOMEODIR,"SimulationsData",dataDir) os.mkdir(simsDataDir) print "Saving to: ", simsDataDir e = vrep.simxSetStringSignal(self.simulID,"HOMEO_SIGNAL_SIM_DATA_DIR" ,asByteArray(simsDataDir), vrep.simx_opmode_oneshot_wait) vrep.simxSynchronousTrigger(self.simulID) print "Message sent, error code: ", e for run in xrange(self.noRuns): eCode = vrep.simxStartSimulation(self.simulID, vrep.simx_opmode_oneshot_wait) vrep.simxSynchronousTrigger(self.simulID) e = vrep.simxSetStringSignal(self.simulID,"HOMEO_SIGNAL_SIM_DATA_DIR" ,asByteArray(simsDataDir), vrep.simx_opmode_oneshot_wait) vrep.simxSynchronousTrigger(self.simulID) print "Simulation started: run number %d, error code: %d"% (run+1, eCode) "Wait until simulation is ready, otherwise we will miss a few movement commands" # sleep(2) np.random.seed(64) # resetRobotInitPose(initPose, self.simulID, ePuckHandle) eCode = vrep.simxSetStringSignal(self.simulID, self.trajStateSignalName, asByteArray("NEWFILE"), vrep.simx_opmode_oneshot_wait) vrep.simxSynchronousTrigger(self.simulID) if eCode == 0: print "Starting a new trajectory file" else: print "ERROR: Could not start a new trajectory file" for step in xrange(self.noSteps): timeStart = time() rightSpeed = np.random.uniform(self.maxSpeed * 2) # - self.maxSpeed leftSpeed = np.random.uniform(self.maxSpeed * 2) # -maxSpeed eCode = vrep.simxSetJointTargetVelocity(self.simulID, self.rightMotor, rightSpeed, vrep.simx_opmode_oneshot_wait) vrep.simxSynchronousTrigger(self.simulID) eCode = vrep.simxSetJointTargetVelocity(self.simulID, self.leftMotor, leftSpeed, vrep.simx_opmode_oneshot_wait) vrep.simxSynchronousTrigger(self.simulID) for i in xrange(self.betwCmdDelays): vrep.simxSynchronousTrigger(self.simulID) timeElapsed = time() - timeStart "Stop the robot" self.stopRobot(self.simulID, [self.rightMotor, self.leftMotor]) eCode = vrep.simxSetStringSignal(self.simulID, self.trajStateSignalName, asByteArray("SAVE"), vrep.simx_opmode_oneshot_wait) vrep.simxSynchronousTrigger(self.simulID) if eCode == 0: print "Saving trajectory file" else: print "ERROR: Could not save a new trajectory file" sleep(.5) robotPose = vrep.simxGetObjectPosition(self.simulID, self.robotHandle, -1, vrep.simx_opmode_oneshot_wait)[1][:2] vrep.simxSynchronousTrigger(self.simulID) print "%d: Robot is at: %.3f, %.3f Distance from target is: %.4f. Run took exactly %.3f seconds" % (run, robotPose[0], robotPose[1], self.computeDistance(self.targetPose, robotPose), timeElapsed) # eCode = vrep.simxStopSimulation(self.simulID, vrep.simx_opmode_oneshot_wait) vrep.simxSynchronousTrigger(self.simulID) sleep(1) # eCode = vrep.simxStartSimulation(self.simulID, vrep.simx_opmode_oneshot_wait) # vrep.simxSynchronousTrigger(self.simulID) eCode = vrep.simxSetStringSignal(self.simulID, self.trajStateSignalName, asByteArray("CLOSEFILE"), vrep.simx_opmode_oneshot_wait) vrep.simxSynchronousTrigger(self.simulID) if eCode == 0: print "Starting a new trajectory file" else: print "ERROR: Could not close a new trajectory file" print "Done"
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 _set_Action(self, action, prefix=""): vrep.simxSetStringSignal(self.id_, prefix + "actions", vrep.simxPackFloats(action), vrep.simx_opmode_oneshot)