def control_propeller_thrust(self, num_p) -> None: """ set thrust for the particular propeller, num_p is the number of the propeller(1 to 4) """ particleVelocity = self.velocities[num_p - 1] s = sim.simGetObjectSizeFactor( self.force_sensors[num_p - 1]) # current size factor if (s != self.pre_v): #When the size of the propeller changes self.particleSize = self.particleSize * 0.005 * s self.pre_v = s self.ts = sim.simGetSimulationTimeStep() m = sim.simGetObjectMatrix(self.force_sensors[num_p - 1], -1) requiredParticleCnt = self.particleCountPerSecond * self.ts + self.notFullParticles self.notFullParticles = requiredParticleCnt % 1 requiredParticleCnt = np.floor(requiredParticleCnt) totalExertedForce = requiredParticleCnt * self.particleDensity * particleVelocity * np.pi * self.particleSize * self.particleSize * self.particleSize / ( 6 * self.ts) force = [0, 0, totalExertedForce] m[3] = 0 m[7] = 0 m[11] = 0 force = sim.simMultiplyVector(m, force) torque = [0, 0, pow(-1, num_p) * 0.002 * particleVelocity] torque = sim.simMultiplyVector(m, torque) sim.simAddForceAndTorque(self.respondables[num_p - 1], force, torque)
def _get_rml_handle(self) -> int: dt = sim.simGetSimulationTimeStep() limits = np.array(self._arm.get_joint_upper_velocity_limits()) vel_correction = 1.0 max_vel = self._arm.max_velocity max_accel = self._arm.max_acceleration max_jerk = self._arm.max_jerk lengths = self._get_path_point_lengths() target_pos_vel = [lengths[-1], 0] previous_q = self._path_points[0:len(self._arm.joints)] while True: pos_vel_accel = [0, 0, 0] rMax = 0 rml_handle = sim.simRMLPos( 1, 0.0001, -1, pos_vel_accel, [max_vel * vel_correction, max_accel, max_jerk], [1], target_pos_vel) state = 0 while state == 0: state, pos_vel_accel = sim.simRMLStep(rml_handle, dt, 1) if state >= 0: pos = pos_vel_accel[0] for i in range(len(lengths) - 1): if lengths[i] <= pos <= lengths[i + 1]: t = (pos - lengths[i]) / (lengths[i + 1] - lengths[i]) # For each joint offset = len(self._arm.joints) * i p1 = self._path_points[offset:offset + self._num_joints] offset = len(self._arm.joints) * (i + 1) p2 = self._path_points[offset:offset + self._num_joints] dx = p2 - p1 qs = p1 + dx * t dq = qs - previous_q previous_q = qs r = np.abs(dq / dt) / limits m = np.max(r) if m > rMax: rMax = m break sim.simRMLRemove(rml_handle) if rMax > 1.001: vel_correction = vel_correction / rMax else: break pos_vel_accel = [0, 0, 0] rml_handle = sim.simRMLPos( 1, 0.0001, -1, pos_vel_accel, [max_vel * vel_correction, max_accel, max_jerk], [1], target_pos_vel) return rml_handle
def start_video_recording( self, filename="{sensor_name}_video_{timestamp}.avi", fourcc="DIVX", timestamp_format="%Y-%m-%d_%H%M%S%f", ): """ Starts video recording that records each simulation step. :param filename: filename with optional 'sensor_name' and 'timestamp' placeholders. :type filename: str :param fourcc: The fourcc of the video codec. :type fourcc: str :param timestamp_format: formatstring for the timestamp :type timestamp_format: str """ if self._video_callback_ids: logger.warn("Cannot start multiple video recordings.") else: # calculate fps from timestep fps = round(1.0 / sim.simGetSimulationTimeStep()) # codec fourcc = cv2.VideoWriter_fourcc(*fourcc) # setup video writer for each sensor for device in self._devices: writer = cv2.VideoWriter( filename.format( sensor_name=device.sensor_name, timestamp=datetime.now().strftime(timestamp_format), ), fourcc, fps, device.resolution, ) # add callback callback = self.create_video_callback(writer) callback_id = device.add_callback(callback) self._video_callback_ids.append(callback_id)
def _step_motion(self) -> int: dt = sim.simGetSimulationTimeStep() lengths = self._get_path_point_lengths() state, posVelAccel = sim.simRMLStep(self._rml_handle, dt, 1) if state >= 0: pos = posVelAccel[0] for i in range(len(lengths) - 1): if lengths[i] <= pos <= lengths[i + 1]: t = (pos - lengths[i]) / (lengths[i + 1] - lengths[i]) # For each joint offset = len(self._arm.joints) * i p1 = self._path_points[ offset:offset + len(self._arm.joints)] offset = self._arm._num_joints * (i + 1) p2 = self._path_points[ offset:offset + len(self._arm.joints)] dx = p2 - p1 qs = p1 + dx * t self._arm.set_joint_target_positions(qs) break if state == 1: sim.simRMLRemove(self._rml_handle) return state
def get_simulation_timestep(self) -> float: """Gets the simulation time step. :return: The time step value in seconds. """ return sim.simGetSimulationTimeStep()
def __init__(self, random, headless=True, use_vision_sensor=False, seed=42, state="Normal", SCENE_FILE=None, reward_function_name='Normal', buffer_size=None, neta=0.9, restart=False): if reward_function_name not in list(rew_functions2.import_dict.keys()): print( "Wrong parameter passed on 'reward_function_name. Must be one of these: ", list(rew_functions.import_dict.keys())) if SCENE_FILE == None: if (use_vision_sensor): SCENE_FILE = join( dirname(abspath(__file__)) ) + '/../../scenes/ardrone_modeled_headless_vision_sensor.ttt' ## FIX else: SCENE_FILE = join( dirname(abspath(__file__)) ) + '/../../scenes/ardrone_modeled_headless.ttt' ## FIX else: SCENE_FILE = join(dirname(abspath(__file__))) + SCENE_FILE assert state in ['Normal', 'New_Double', 'New_action'] assert random in [False, 'Gaussian', 'Uniform','Discretized_Uniform'], \ "random should be one of these values [False, 'Gaussian', 'Uniform', 'Discretized_Uniform]" # assert random in [False, 'Gaussian', 'Uniform','Weighted','Discretized_Uniform'], \ # "random should be one of these values [False, 'Gaussian', 'Uniform', 'Weighted','Discretized_Uniform]" ## Setting Pyrep self.pr = PyRep() self.pr.launch(SCENE_FILE, headless=headless) self.pr.start() self.agent = Drone(count=0, name="Quadricopter", num_joints=4, use_vision_sensor=use_vision_sensor) self.agent.set_control_loop_enabled( False) ## Disables the built-in PID control on V-REP motors #self.agent.set_motor_locked_at_zero_velocity(True) ## When the force is set to zero, it locks the motor to prevent drifting self.agent.set_motor_locked_at_zero_velocity( False ) ## When the force is set to zero, it locks the motor to prevent drifting self.target = Shape('Quadricopter_target') ##Attributes self.action_space = self.agent.action_space if state == 'New_action': self.observation_space = 22 elif state == 'New_Double': self.observation_space = 36 else: self.observation_space = self.agent.observation_space self.restart = restart self.random = random self.dt = np.round(sim.simGetSimulationTimeStep(), 4) ## timestep ## Integrative buffer self.buffer_size = buffer_size # if self.buffer_size: # assert (isinstance(buffer_size,int)) and (buffer_size < 100) # self.integrative_buffer_size = self.buffer_size # self.integrative_buffer = np.empty((self.buffer_size, 3)) # 3 because its [x,y,z] or [roll,pitch,yaw] # self.neta = neta self.state = state ## initial observation self._initial_state() self.first_obs = True ## hack so it doesn't calculate it at the first time self._make_observation() self.last_state = self.observation[:18] self.weighted = False # Creating lists if self.random == 'Discretized_Uniform': self._create_discretized_uniform_list() self.ptr = 0 # self._creating_initialization_list() ## Setting seed self.seed = seed self._set_seed(self.seed, self.seed) ## Reward Functions self.reward_function = partial( rew_functions2.reward, rew_functions2.import_dict[reward_function_name]['weight_list']) keys = ["r_alive","radius","pitch","yaw","roll","pitch_vel","yaw_vel","roll_vel","lin_x_vel","lin_y_vel","lin_z_vel","norm_a", \ "std_a","death","integrative_error_x","integrative_error_y","integrative_error_z"] self.weight_dict = dict( zip( keys, rew_functions2.import_dict[reward_function_name] ['weight_list']))