def _reset(self): if self.physicsClientId < 0: self.ownsPhysicsClient = True if self.isRender: self._p = bullet_client.BulletClient( connection_mode=pybullet.GUI) else: self._p = bullet_client.BulletClient() self.physicsClientId = self._p._client self._p.configureDebugVisualizer(pybullet.COV_ENABLE_GUI, 0) if self.scene is None: self.scene = self.create_single_player_scene(self._p) if not self.scene.multiplayer and self.ownsPhysicsClient: self.scene.episode_restart(self._p) self.robot.scene = self.scene self.frame = 0 self.done = 0 self.reward = 0 dump = 0 s = self.robot.reset(self._p) self.potential = self.robot.calc_potential() return s
def __init__( self, urdfRoot=pybullet_data.getDataPath(), actionRepeat=5, isEnableSelfCollision=True, isDiscrete=False, renders=False, ): # just change to True if you want to visualize print("init") self._timeStep = 0.01 self._urdfRoot = urdfRoot self._actionRepeat = actionRepeat self._isEnableSelfCollision = isEnableSelfCollision self._observation = [] self._ballUniqueId = -1 self._envStepCounter = 0 self._renders = renders # self._renders = True self._isDiscrete = isDiscrete if self._renders: self._p = bullet_client.BulletClient(connection_mode=pybullet.GUI) else: self._p = bullet_client.BulletClient() self.seed() # self.reset() observationDim = 7 # len(self.getExtendedObservation()) # print("observationDim") # print(observationDim) # observation_high = np.array([np.finfo(np.float32).max] * observationDim) observation_high = np.ones(observationDim) * 1000 # np.inf if isDiscrete: self.action_space = spaces.Discrete(9) else: action_dim = 2 self._action_bound = 1 action_high = np.array([self._action_bound] * action_dim) self.action_space = spaces.Box(-action_high, action_high, dtype=np.float32) self.observation_space = spaces.Box(-observation_high, observation_high, dtype=np.float32) self.viewer = None self.goal = [] self.useSphere = False self.dist_to_goal_threshold = 0.2 # threshold to consider we "got to goal" self._hard_reset = True self.reset()
def __init__(self, urdfRoot=pybullet_data.getDataPath(), actionRepeat=50, isEnableSelfCollision=True, isDiscrete=False, renders=False, length=1, deterministic=True, r_type='neg_dist', lod=0): self._timeStep = 0.01 self._urdfRoot = urdfRoot self._lod = lod if lod > 0: actionRepeat = 10 self._actionRepeat = actionRepeat self._isEnableSelfCollision = isEnableSelfCollision self._observation = [] self._ballUniqueId = -1 self._envStepCounter = 0 self._renders = renders self._isDiscrete = isDiscrete if self._renders: self._p = bullet_client.BulletClient( connection_mode=pybullet.GUI) else: self._p = bullet_client.BulletClient() self._seed() #self.reset() observationDim = 5 observation_high = np.ones(observationDim) * 1000 #np.inf if (isDiscrete): self.action_space = spaces.Discrete(3) else: action_dim = 2 self._action_bound = 1 action_high = np.array([self._action_bound] * action_dim) self.action_space = spaces.Box(-action_high, action_high) self.observation_space = spaces.Box(-observation_high, observation_high) self.viewer = None self.length = length self.deterministic = deterministic self.switch = None self.max_x, self.min_y, self.max_y = None, None, None self.r_type = r_type
def __init__(self, map_id=1): self.p = bullet_client.BulletClient(connection_mode=pybullet.DIRECT) self.map_id = map_id self.cars = [] self.position_cars = [] # variable for tracking self.n_collision = 0 self.step_count = 0 self.n_reset = 0 # reset self._reset()
def __init__(self,pybullet_sim_factory = boxstack_pybullet_sim, render=True, render_sleep=False, debug_visualization=True, hard_reset = False, render_width=240, render_height=240, action_repeat=1, time_step = 1./240., num_bullet_solver_iterations=50, urdf_root=pybullet_data.getDataPath()): """Initialize the gym environment. Args: urdf_root: The path to the urdf data folder. """ self._pybullet_sim_factory = pybullet_sim_factory self._time_step = time_step self._urdf_root = urdf_root self._observation = [] self._action_repeat=action_repeat self._num_bullet_solver_iterations = num_bullet_solver_iterations self._env_step_counter = 0 self._is_render = render self._debug_visualization = debug_visualization self._render_sleep = render_sleep self._render_width=render_width self._render_height=render_height self._cam_dist = .3 self._cam_yaw = 50 self._cam_pitch = -35 self._hard_reset = True self._last_frame_time = 0.0 optionstring='--width={} --height={}'.format(render_width,render_height) print("urdf_root=" + self._urdf_root) if self._is_render: self._pybullet_client = bullet_client.BulletClient( connection_mode=pybullet.GUI, options=optionstring) else: self._pybullet_client = bullet_client.BulletClient() if (debug_visualization==False): self._pybullet_client.configureDebugVisualizer(flag=self._pybullet_client.COV_ENABLE_GUI,enable=0) self._pybullet_client.configureDebugVisualizer(flag=self._pybullet_client.COV_ENABLE_RGB_BUFFER_PREVIEW,enable=0) self._pybullet_client.configureDebugVisualizer(flag=self._pybullet_client.COV_ENABLE_DEPTH_BUFFER_PREVIEW,enable=0) self._pybullet_client.configureDebugVisualizer(flag=self._pybullet_client.COV_ENABLE_SEGMENTATION_MARK_PREVIEW,enable=0) self._pybullet_client.setAdditionalSearchPath(urdf_root) self._seed() self.reset() observation_high = ( self._example_sim.GetObservationUpperBound()) observation_low = ( self._example_sim.GetObservationLowerBound()) action_dim = self._example_sim.GetActionDimension() self._action_bound = 1 action_high = np.array([self._action_bound] * action_dim) self.action_space = spaces.Box(-action_high, action_high) self.observation_space = spaces.Box(observation_low, observation_high) self.viewer = None self._hard_reset = hard_reset # This assignment need to be after reset()
def __init__( self, urdf_root=pybullet_data.getDataPath(), action_repeat=1, distance_weight=1.0, energy_weight=0.005, shake_weight=0.0, drift_weight=0.0, distance_limit=float("inf"), observation_noise_stdev=0.0, self_collision_enabled=True, motor_velocity_limit=np.inf, pd_control_enabled=False, #not needed to be true if accurate motor model is enabled (has its own better PD) leg_model_enabled=True, accurate_motor_model_enabled=True, motor_kp=1.0, motor_kd=0.02, torque_control_enabled=False, motor_overheat_protection=True, hard_reset=True, on_rack=False, render=False, kd_for_pd_controllers=0.3, env_randomizer=minitaur_env_randomizer.MinitaurEnvRandomizer()): """Initialize the minitaur gym environment. Args: urdf_root: The path to the urdf data folder. action_repeat: The number of simulation steps before actions are applied. distance_weight: The weight of the distance term in the reward. energy_weight: The weight of the energy term in the reward. shake_weight: The weight of the vertical shakiness term in the reward. drift_weight: The weight of the sideways drift term in the reward. distance_limit: The maximum distance to terminate the episode. observation_noise_stdev: The standard deviation of observation noise. self_collision_enabled: Whether to enable self collision in the sim. motor_velocity_limit: The velocity limit of each motor. pd_control_enabled: Whether to use PD controller for each motor. leg_model_enabled: Whether to use a leg motor to reparameterize the action space. accurate_motor_model_enabled: Whether to use the accurate DC motor model. motor_kp: proportional gain for the accurate motor model. motor_kd: derivative gain for the accurate motor model. torque_control_enabled: Whether to use the torque control, if set to False, pose control will be used. motor_overheat_protection: Whether to shutdown the motor that has exerted large torque (OVERHEAT_SHUTDOWN_TORQUE) for an extended amount of time (OVERHEAT_SHUTDOWN_TIME). See ApplyAction() in minitaur.py for more details. hard_reset: Whether to wipe the simulation and load everything when reset is called. If set to false, reset just place the minitaur back to start position and set its pose to initial configuration. on_rack: Whether to place the minitaur on rack. This is only used to debug the walking gait. In this mode, the minitaur's base is hanged midair so that its walking gait is clearer to visualize. render: Whether to render the simulation. kd_for_pd_controllers: kd value for the pd controllers of the motors env_randomizer: An EnvRandomizer to randomize the physical properties during reset(). """ self._time_step = 0.01 self._action_repeat = action_repeat self._num_bullet_solver_iterations = 300 self._urdf_root = urdf_root self._self_collision_enabled = self_collision_enabled self._motor_velocity_limit = motor_velocity_limit self._observation = [] self._env_step_counter = 0 self._is_render = render self._last_base_position = [0, 0, 0] self._distance_weight = distance_weight self._energy_weight = energy_weight self._drift_weight = drift_weight self._shake_weight = shake_weight self._distance_limit = distance_limit self._observation_noise_stdev = observation_noise_stdev self._action_bound = 1 self._pd_control_enabled = pd_control_enabled self._leg_model_enabled = leg_model_enabled self._accurate_motor_model_enabled = accurate_motor_model_enabled self._motor_kp = motor_kp self._motor_kd = motor_kd self._torque_control_enabled = torque_control_enabled self._motor_overheat_protection = motor_overheat_protection self._on_rack = on_rack self._cam_dist = 1.0 self._cam_yaw = 0 self._duckId = -1 self._cam_pitch = -30 self._hard_reset = True self._kd_for_pd_controllers = kd_for_pd_controllers self._last_frame_time = 0.0 print("urdf_root=" + self._urdf_root) self._env_randomizer = env_randomizer # PD control needs smaller time step for stability. if pd_control_enabled or accurate_motor_model_enabled: self._time_step /= NUM_SUBSTEPS self._num_bullet_solver_iterations /= NUM_SUBSTEPS self._action_repeat *= NUM_SUBSTEPS if self._is_render: self._pybullet_client = bullet_client.BulletClient( connection_mode=pybullet.GUI) else: self._pybullet_client = bullet_client.BulletClient() self._seed() self.reset() observation_high = (self.minitaur.GetObservationUpperBound() + OBSERVATION_EPS) observation_low = (self.minitaur.GetObservationLowerBound() - OBSERVATION_EPS) action_dim = 8 action_high = np.array([self._action_bound] * action_dim) self.action_space = spaces.Box(-action_high, action_high) self.observation_space = spaces.Box(observation_low, observation_high) self.viewer = None self._hard_reset = hard_reset # This assignment need to be after reset()
def __init__(self, urdf_path, action_repeat=1, distance_weight=1.0, energy_weight=0.005, drift_weight=0.002, render=False): """Environment for the T-rex model. :param urdf_path: path to the urdf data folder. :param action_repeat: number of simulation steps before actions are applied. :param distance_weight: weight of the distance term in the reward. :param energy_weight: weight of the energy term in the reward. :param render: whether to render the simulation. """ self._time_step = 0.01 self._urdf_path = urdf_path self._action_repeat = action_repeat self._num_bullet_solver_iterations = 300 self._observation = [] self._env_step_counter = 0 self._is_render = render self._last_base_position = [0.] * 3 self._weight_distance = distance_weight self._weight_energy = energy_weight self._weight_drift = drift_weight self._action_bound = 1 self._cam_dist = 10.0 self._cam_yaw = 90.0 self._cam_pitch = -30.0 self._last_frame_time = 0.0 # PD control needs smaller time step for stability. self._time_step /= NUM_SUBSTEPS self._num_bullet_solver_iterations /= NUM_SUBSTEPS self._action_repeat *= NUM_SUBSTEPS connection_mode = pybullet.DIRECT if self._is_render: connection_mode = pybullet.GUI self._pybullet_client = bullet_client.BulletClient( connection_mode=connection_mode) self._starting_configuration = { 'femur_L_joint': -0.6, 'tibia_L_joint': 0.4, 'tarsometatarsus_L_joint': -1.2, 'femur_R_joint': -0.6, 'tibia_R_joint': 0.4, 'tarsometatarsus_R_joint': -1.2 } self.model = None self.np_random = None self.seed() self.reset() action_low, action_high = self.model.get_action_limits() self.action_space = spaces.Box(low=action_low, high=action_high, dtype=np.float32) observation_low, observation_high = self.model.get_observation_limits() self.observation_space = spaces.Box(low=observation_low, high=observation_high, dtype=np.float32)