From fd960629724d7a10be6e4d56b6ddaf72ceb51f7a Mon Sep 17 00:00:00 2001 From: Wenhao Yu Date: Wed, 13 Oct 2021 15:48:47 -0700 Subject: [PATCH] Add config for visual locomotion env with two cameras and random stepstone. --- .../configs/laikago_mpc_stepstone.gin | 3 +- ...aikago_mpc_two_camera_random_stepstone.gin | 165 ++++++++++++ .../examples/laikago_mpc_wrapper_example.py | 2 +- .../envs_v2/scenes/random_stepstone_scene.py | 149 +++++++++++ .../envs_v2/scenes/terrain/stepstones.py | 234 ++++++++++++++++++ .../minitaur/vision/sim_camera.py | 6 +- 6 files changed, 553 insertions(+), 6 deletions(-) create mode 100644 examples/pybullet/gym/pybullet_data/configs/laikago_mpc_two_camera_random_stepstone.gin create mode 100644 examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/scenes/random_stepstone_scene.py create mode 100644 examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/scenes/terrain/stepstones.py diff --git a/examples/pybullet/gym/pybullet_data/configs/laikago_mpc_stepstone.gin b/examples/pybullet/gym/pybullet_data/configs/laikago_mpc_stepstone.gin index 2644d3b7b..fa9ea31e6 100644 --- a/examples/pybullet/gym/pybullet_data/configs/laikago_mpc_stepstone.gin +++ b/examples/pybullet/gym/pybullet_data/configs/laikago_mpc_stepstone.gin @@ -82,7 +82,8 @@ locomotion_gym_config.SimulationParameters.num_action_repeat = %NUM_ACTION_REPEA random_stepstone_scene.RandomStepstoneScene.random_seed = 13 random_stepstone_scene.RandomStepstoneScene.gap_length_lower_bound = 0.07 random_stepstone_scene.RandomStepstoneScene.gap_length_upper_bound = 0.15 -random_stepstone_scene.RandomStepstoneScene.stone_width = 0.6 +random_stepstone_scene.RandomStepstoneScene.stone_width_lower_bound = 0.6 +random_stepstone_scene.RandomStepstoneScene.stone_width_upper_bound = 0.6 ######################################## # Setup the task and terminal condition parameters diff --git a/examples/pybullet/gym/pybullet_data/configs/laikago_mpc_two_camera_random_stepstone.gin b/examples/pybullet/gym/pybullet_data/configs/laikago_mpc_two_camera_random_stepstone.gin new file mode 100644 index 000000000..9c916b833 --- /dev/null +++ b/examples/pybullet/gym/pybullet_data/configs/laikago_mpc_two_camera_random_stepstone.gin @@ -0,0 +1,165 @@ +#-*-Python-*- + +# NOTE: Should be run with >=10CPU for decent performance. + +import pybullet_envs.minitaur.agents.baseline_controller.torque_stance_leg_controller +import pybullet_envs.minitaur.envs_v2.env_loader +import pybullet_envs.minitaur.envs_v2.env_wrappers.mpc_locomotion_wrapper +import pybullet_envs.minitaur.envs_v2.env_wrappers.observation_dictionary_to_array_wrapper +import pybullet_envs.minitaur.envs_v2.locomotion_gym_config +import pybullet_envs.minitaur.envs_v2.locomotion_gym_env +import pybullet_envs.minitaur.envs_v2.scenes.random_stepstone_scene +import pybullet_envs.minitaur.envs_v2.sensors.camera_sensor +import pybullet_envs.minitaur.envs_v2.sensors.imu_sensor +import pybullet_envs.minitaur.envs_v2.sensors.last_action_sensor +import pybullet_envs.minitaur.envs_v2.sensors.toe_position_sensor +import pybullet_envs.minitaur.envs_v2.tasks.simple_locomotion_task +import pybullet_envs.minitaur.envs_v2.tasks.terminal_conditions +import pybullet_envs.minitaur.envs_v2.utilities.noise_generators +import pybullet_envs.minitaur.robots.hybrid_motor_model +import pybullet_envs.minitaur.robots.laikago_v2 +import pybullet_envs.minitaur.robots.robot_config + + +# Configure the dynamic robot + +SIM_TIME_STEP = 0.001 +NUM_ACTION_REPEAT = 4 # Control frequency will be 100 Hz + + +######################################## +# Configure the sensors +######################################## +imu_sensor.IMUSensor.channels = [ + %imu_sensor.IMUChannel.ROLL, + %imu_sensor.IMUChannel.PITCH, + %imu_sensor.IMUChannel.ROLL_RATE, + %imu_sensor.IMUChannel.PITCH_RATE +] + +imu_sensor.IMUSensor.lower_bound = [-6.28318548203, -6.28318548203, -6283.18554688, -6283.18554688] +imu_sensor.IMUSensor.upper_bound = [6.28318548203, 6.28318548203, 6283.18554688, 6283.18554688] + +# Add noise to the IMU sensor and toe position sensor +IMUNoise/noise_generators.NormalNoise.scale = (0.025, 0.025, 0.1, 0.1) +TOENoise/noise_generators.NormalNoise.scale = (0.0025, 0.0025, 0.005, 0.0025, 0.0025, 0.005, 0.0025, 0.0025, 0.005, 0.0025, 0.0025, 0.005) +imu_sensor.IMUSensor.noise_generator = @IMUNoise/noise_generators.NormalNoise() +toe_position_sensor.ToePositionSensor.noise_generator = @TOENoise/noise_generators.NormalNoise() + +frontCamera/camera_sensor.CameraSensor.camera_translation_from_base = (0.197, 0.0, -0.115) +frontCamera/camera_sensor.CameraSensor.camera_rotation_from_base = (-0.4996018, 0.4999998, 0.4999998, 0.5003982) +frontCamera/camera_sensor.CameraSensor.parent_link_id = -1 +frontCamera/camera_sensor.CameraSensor.resolution = (32, 24) +frontCamera/camera_sensor.CameraSensor.sensor_latency = 0.03 +frontCamera/camera_sensor.CameraSensor.name = "frontCam" +frontCamera/camera_sensor.CameraSensor.fov_degree = 75 +frontCamera/camera_sensor.CameraSensor.camera_mode = %sim_camera.CameraMode.DEPTH +frontCamera/camera_sensor.CameraSensor.camera_update_frequency_hz = 30.0 + +rearCamera/camera_sensor.CameraSensor.camera_translation_from_base = (-0.092, 0.0, -0.105) +rearCamera/camera_sensor.CameraSensor.camera_rotation_from_base = (-0.4996018, 0.4999998, 0.4999998, 0.5003982) +rearCamera/camera_sensor.CameraSensor.parent_link_id = -1 +rearCamera/camera_sensor.CameraSensor.resolution = (32, 24) +rearCamera/camera_sensor.CameraSensor.sensor_latency = 0.03 +rearCamera/camera_sensor.CameraSensor.name = "rearCam" +rearCamera/camera_sensor.CameraSensor.fov_degree = 75 +rearCamera/camera_sensor.CameraSensor.camera_mode = %sim_camera.CameraMode.DEPTH +rearCamera/camera_sensor.CameraSensor.camera_update_frequency_hz = 30.0 + +sensors = [@imu_sensor.IMUSensor(), @last_action_sensor.LastActionSensor(), @toe_position_sensor.ToePositionSensor(), @frontCamera/camera_sensor.CameraSensor(), @rearCamera/camera_sensor.CameraSensor()] +laikago_v2.Laikago.sensors = %sensors + + +######################################## +# Specify the motor model and its parameters +######################################## +LAIKAGO_MOTOR_ANGLE_UPPER_LIMITS = 6.28318548203 +LAIKAGO_MOTOR_ANGLE_LOWER_LIMITS = -6.28318548203 +laikago/robot_config.MotorLimits.angle_lower_limits = %LAIKAGO_MOTOR_ANGLE_LOWER_LIMITS +laikago/robot_config.MotorLimits.angle_upper_limits = %LAIKAGO_MOTOR_ANGLE_UPPER_LIMITS +laikago/robot_config.MotorLimits.torque_lower_limits = -30 +laikago/robot_config.MotorLimits.torque_upper_limits = 30 +laikago_v2.Laikago.motor_limits = @laikago/robot_config.MotorLimits() +laikago_v2.Laikago.motor_control_mode = %robot_config.MotorControlMode.HYBRID +laikago_v2.Laikago.motor_model_class = @hybrid_motor_model.HybridMotorModel +locomotion_gym_env.LocomotionGymEnv.robot_class = @laikago_v2.Laikago +hybrid_motor_model.HybridMotorModel.kp = 250 +hybrid_motor_model.HybridMotorModel.kd = (0.3, 2.0, 2.0, 0.3, 2.0, 2.0, 0.3, 2.0, 2.0, 0.3, 2.0, 2.0) + + +######################################## +# Setup the terrain randomization and simulation parameters +######################################## + +locomotion_gym_config.LocomotionGymConfig.simulation_parameters = @locomotion_gym_config.SimulationParameters() +locomotion_gym_env.LocomotionGymEnv.robot_class = @laikago_v2.Laikago +locomotion_gym_env.LocomotionGymEnv.task = @simple_locomotion_task.SimpleForwardTask() +locomotion_gym_env.LocomotionGymEnv.gym_config = @locomotion_gym_config.LocomotionGymConfig() +locomotion_gym_env.LocomotionGymEnv.scene = @random_stepstone_scene.RandomStepstoneScene() +locomotion_gym_config.SimulationParameters.sim_time_step_s = %SIM_TIME_STEP +locomotion_gym_config.SimulationParameters.num_action_repeat = %NUM_ACTION_REPEAT + +random_stepstone_scene.RandomStepstoneScene.random_seed = 13 +random_stepstone_scene.RandomStepstoneScene.gap_length_lower_bound = 0.07 +random_stepstone_scene.RandomStepstoneScene.gap_length_upper_bound = 0.15 +random_stepstone_scene.RandomStepstoneScene.stone_width_lower_bound = 0.6 +random_stepstone_scene.RandomStepstoneScene.stone_width_upper_bound = 0.6 + + +######################################## +# Setup the task and terminal condition parameters +######################################## +terminal_conditions.maxstep_terminal_condition.max_step = 2000 +terminal_conditions.default_terminal_condition_for_laikago_v2.max_roll = 0.25 +terminal_conditions.default_terminal_condition_for_laikago_v2.max_pitch = 1.0 +terminal_conditions.default_terminal_condition_for_laikago_v2.min_height = 0.15 +terminal_conditions.default_terminal_condition_for_laikago_v2.enforce_foot_contacts = True + +# Setup the terminal condition +terminal_conditions.logical_any_terminal_condition.conditions = [ + @terminal_conditions.default_terminal_condition_for_laikago_v2, + @terminal_conditions.maxstep_terminal_condition, + ] + +simple_locomotion_task.SimpleForwardTask.terminal_condition = @terminal_conditions.logical_any_terminal_condition + +simple_locomotion_task.SimpleForwardTask.clip_velocity = 0.0015 + +time_ordered_buffer.TimeOrderedBuffer.error_on_duplicate_timestamp = False +time_ordered_buffer.TimeOrderedBuffer.replace_value_on_duplicate_timestamp = True +time_ordered_buffer.TimeOrderedBuffer.error_on_timestamp_reversal = False + + +observation_dictionary_to_array_wrapper.ObservationDictionaryToArrayWrapper.observation_excluded = ('frontCam', 'rearCam') + +env_loader.load.wrapper_classes = [ + @mpc_locomotion_wrapper.MPCLocomotionWrapper, + @observation_dictionary_to_array_wrapper.ObservationDictionaryToArrayWrapper, +] + +######################################## +# Configure the MPC-related parameters +######################################## +torque_stance_leg_controller.TorqueStanceLegController.qp_weights = (5, 5, 0.2, 0, 0, 10, 0.5, 0.5, 0.2, 0.2, 0.2, 0.1, 0) +torque_stance_leg_controller.TorqueStanceLegController.body_inertia = (0.183375, 0, 0, 0, 0.6267, 0, 0, 0, 0.636175) +torque_stance_leg_controller.TorqueStanceLegController.friction_coeffs = (0.45, 0.45, 0.45, 0.45) + +######################################## +# Configure the foothold wrapper parameters and action space +######################################## +mpc_locomotion_wrapper.MPCLocomotionWrapper.foot_friction_coeff = 1.0 +mpc_locomotion_wrapper.MPCLocomotionWrapper.locomotion_gait = %mpc_locomotion_wrapper.Gait.TROT +mpc_locomotion_wrapper.MPCLocomotionWrapper.control_frequency=20 +mpc_locomotion_wrapper.MPCLocomotionWrapper.target_horizontal_com_velocity_heuristic = @mpc_locomotion_wrapper.InverseRaibertTargetHorizontalComVelocityHeuristic() +mpc_locomotion_wrapper.InverseRaibertTargetHorizontalComVelocityHeuristic.gains = (-0.25, -0.1) + +mpc_locomotion_wrapper.MPCLocomotionWrapper.swing_target_action_range = ((-0.05, -0.05, -0.03), (0.1, 0.05, 0.03)) +mpc_locomotion_wrapper.MPCLocomotionWrapper.pitch_action_range = (-0.2, 0.2) +mpc_locomotion_wrapper.MPCLocomotionWrapper.roll_action_range = (-0.05, 0.05) +mpc_locomotion_wrapper.MPCLocomotionWrapper.base_velocity_action_range = ((-0.05, -0.05), (0.05, 0.05)) +mpc_locomotion_wrapper.MPCLocomotionWrapper.base_twist_action_range = (-0.2, 0.2) +mpc_locomotion_wrapper.MPCLocomotionWrapper.base_height_action_range = (0.42, 0.48) +mpc_locomotion_wrapper.MPCLocomotionWrapper.swing_clearance_action_range = (0.05, 0.1) + +imu_based_com_velocity_estimator.IMUBasedCOMVelocityEstimator.use_sensor_interface = False + diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/examples/laikago_mpc_wrapper_example.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/examples/laikago_mpc_wrapper_example.py index ad41f5633..5c6c01194 100644 --- a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/examples/laikago_mpc_wrapper_example.py +++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/examples/laikago_mpc_wrapper_example.py @@ -26,7 +26,7 @@ from pybullet_envs.minitaur.envs_v2 import env_loader FLAGS = flags.FLAGS flags.DEFINE_string("video_file", None, "The filename for saving the videos.") -CONFIG_FILE_SIM = (pd.getDataPath()+"/configs/laikago_mpc_example_flat.gin") +CONFIG_FILE_SIM = (pd.getDataPath()+"/configs/laikago_mpc_two_camera_random_stepstone.gin") NUM_STEPS = 100 ENABLE_RENDERING = True # Will be disabled for tests diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/scenes/random_stepstone_scene.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/scenes/random_stepstone_scene.py new file mode 100644 index 000000000..a9895cfc7 --- /dev/null +++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/scenes/random_stepstone_scene.py @@ -0,0 +1,149 @@ +# Lint as: python3 +"""Scene with randomly spaced stepstones.""" + +from typing import Optional, Sequence + +import gin +import numpy as np + +from pybullet_envs.minitaur.envs_v2.scenes import scene_base +from pybullet_envs.minitaur.envs_v2.scenes.terrain import stepstones + + +@gin.configurable +class RandomStepstoneScene(scene_base.SceneBase): + """Scene with randomly spaced stepstones.""" + + def __init__( + self, + num_stones: int = 50, + stone_height: float = 0.1, + stone_width_lower_bound: float = 10.0, + stone_width_upper_bound: float = 10.0, + stone_length_lower_bound: float = 0.1, + stone_length_upper_bound: float = 0.3, + gap_length_lower_bound: float = 0.1, + gap_length_upper_bound: float = 0.3, + height_offset_lower_bound: float = 0.0, + height_offset_upper_bound: float = 1e-6, + floor_height_lower_bound: float = -0.5, + floor_height_upper_bound: float = -0.55, + platform_length_lower_bound: float = 0.75, + platform_length_upper_bound: float = 1.0, + random_seed: Optional[int] = None, + color_sequence: Sequence[Sequence[float]] = stepstones.MULTICOLOR, + rebuild_scene_during_reset: bool = True): + """Initializes RandomStepstoneScene. + + Args: + num_stones: The number of stepstones. + stone_height: The height in meters of each stepstone. + stone_width_lower_bound: The lower bound in meters of the randomly sampled + stepstone width. + stone_width_upper_bound: The upper bound in meters of the randomly sampled + stepstone width. + stone_length_lower_bound: The lower bound in meters of the randomly + sampled stepstone length. + stone_length_upper_bound: The upper bound in meters of the randomly + sampled stepstone length. + gap_length_lower_bound: The lower bound in meters of the random sampled + gap distance. + gap_length_upper_bound: The upper bound in meters of the random sampled + gap distance. + height_offset_lower_bound: The lower bound in meters of the randomly + sampled stepstone height. + height_offset_upper_bound: The upper bound in meters of the randomly + sampled stepstone height. + floor_height_lower_bound: The lower bound in meters of the randomly + sampled floor height. + floor_height_upper_bound: The upper bound in meters of the randomly + sampled floor height. + platform_length_lower_bound: The lower bound in meters of the first step + stone length. + platform_length_upper_bound: The upper bound in meters of the first step + stone length. + random_seed: The random seed to generate the random stepstones. + color_sequence: A list of (red, green, blue, alpha) colors where each + element is in [0, 1] and alpha is transparency. The stepstones will + cycle through these colors. + rebuild_scene_during_reset: Whether to rebuild the stepstones during + reset. + """ + for color in color_sequence: + if len(color) != 4: + raise ValueError( + "Each color must be length 4; got <{}>".format(color_sequence)) + + super(RandomStepstoneScene, self).__init__(data_root=None) + self._num_stones = num_stones + self._stone_height = stone_height + self._stone_width_lower_bound = stone_width_lower_bound + self._stone_width_upper_bound = stone_width_upper_bound + self._stone_length_lower_bound = stone_length_lower_bound + self._stone_length_upper_bound = stone_length_upper_bound + self._gap_length_lower_bound = gap_length_lower_bound + self._gap_length_upper_bound = gap_length_upper_bound + self._height_offset_lower_bound = height_offset_lower_bound + self._height_offset_upper_bound = height_offset_upper_bound + self._floor_height_lower_bound = floor_height_lower_bound + self._floor_height_upper_bound = floor_height_upper_bound + self._platform_length_lower_bound = platform_length_lower_bound + self._platform_length_upper_bound = platform_length_upper_bound + self._random_seed = random_seed + self._color_sequence = color_sequence + self._rebuild_scene_during_reset = rebuild_scene_during_reset + + def reset(self): + super().reset() + + if self._rebuild_scene_during_reset: + for ground_id in self.ground_ids: + self._pybullet_client.removeBody(ground_id) + self.build_scene(self._pybullet_client) + + def build_scene(self, pybullet_client): + super().build_scene(pybullet_client) + # The first stone is to let the robot stand at the initial position. + + stone_width = np.random.uniform(self._stone_width_lower_bound, + self._stone_width_upper_bound) + platform_length = np.random.uniform(self._platform_length_lower_bound, + self._platform_length_upper_bound) + + end_pos, first_stone_id = stepstones.build_one_stepstone( + pybullet_client=pybullet_client, + start_pos=(-platform_length / 2.0, 0, 0), + stone_length=platform_length, + stone_height=self._stone_height, + stone_width=stone_width, + gap_length=0.0, + height_offset=0.0, + rgba_color=stepstones.GRAY) + + _, stepstone_ids = stepstones.build_random_stepstones( + pybullet_client=self.pybullet_client, + start_pos=end_pos, + num_stones=self._num_stones, + stone_height=self._stone_height, + stone_width=stone_width, + stone_length_lower_bound=self._stone_length_lower_bound, + stone_length_upper_bound=self._stone_length_upper_bound, + gap_length_lower_bound=self._gap_length_lower_bound, + gap_length_upper_bound=self._gap_length_upper_bound, + height_offset_lower_bound=self._height_offset_lower_bound, + height_offset_upper_bound=self._height_offset_upper_bound, + random_seed=self._random_seed, + color_sequence=self._color_sequence) + for pybullet_id in [first_stone_id] + stepstone_ids: + self.add_object(pybullet_id, scene_base.ObjectType.GROUND) + + self._floor_height = np.random.uniform(self._floor_height_lower_bound, + self._floor_height_upper_bound) + # Add floor + floor_id = stepstones.load_box(pybullet_client, + half_extents=[100, 100, 1], + position=np.array([0.0, 0.0, self._floor_height - 1.0]), + orientation=(0.0, 0.0, 0.0, 1.0), + rgba_color=(1.0, 1.0, 1.0, 1.0), + mass=0) + self.add_object(floor_id, scene_base.ObjectType.GROUND) diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/scenes/terrain/stepstones.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/scenes/terrain/stepstones.py new file mode 100644 index 000000000..63648dd8c --- /dev/null +++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/scenes/terrain/stepstones.py @@ -0,0 +1,234 @@ +# Lint as: python3 +"""Generates stepstones in PyBullet simulation.""" + +import itertools +import random +from typing import List, Optional, Sequence, Tuple +import numpy as np +import pybullet +from pybullet_utils import bullet_client + +GRAY = (0.3, 0.3, 0.3, 1) +# RGB values from the official Google logo colors. +GREEN = [60/255, 186/255, 84/255, 1] +YELLOW = [244/255, 194/255, 13/255, 1] +RED = [219/255, 50/255, 54/255, 1] +BLUE = [72/255, 133/255, 237/255, 1] + +MULTICOLOR = (GREEN, YELLOW, RED, BLUE) + +def load_box(pybullet_client: bullet_client.BulletClient, + half_extents: Sequence[float] = (1, 1, 1), + position: Sequence[float] = (0, 0, 0), + orientation: Sequence[float] = (0, 0, 0, 1), + rgba_color: Sequence[float] = (0.3, 0.3, 0.3, 1), + mass: float = 0) -> int: + """Loads a visible and tangible box. + + Args: + half_extents: Half the dimension of the box in meters in each direction. + position: Global coordinates of the center of the box. + orientation: As a quaternion. + rgba_color: The color and transparency of the box, each in the range + [0,1]. Defaults to opaque gray. + mass: Mass in kg. Mass 0 fixes the box in place. + + Returns: + Unique integer referring to the loaded box. + """ + col_box_id = pybullet_client.createCollisionShape( + pybullet.GEOM_BOX, halfExtents=half_extents) + visual_box_id = pybullet_client.createVisualShape( + pybullet.GEOM_BOX, halfExtents=half_extents, rgbaColor=rgba_color) + return pybullet_client.createMultiBody( + baseMass=mass, + baseCollisionShapeIndex=col_box_id, + baseVisualShapeIndex=visual_box_id, + basePosition=position, + baseOrientation=orientation) + + +def build_one_stepstone( + pybullet_client: bullet_client.BulletClient, + start_pos: Sequence[float] = (0, 0, 0), + stone_length: float = 1, + stone_height: float = 0.15, + stone_width: float = 5.0, + gap_length: float = 0.3, + height_offset: float = 0, + rgba_color: Sequence[float] = GRAY) -> Tuple[np.ndarray, int]: + """Generates one stepstone. + + Args: + pybullet_client: The pybullet client instance. + start_pos: The starting position (the midpoint of top-left edge) of the + stepstone. + stone_length: The length of the stepstone in meters. + stone_height: The height of the stepstone in meters. + stone_width: The width of the stepstone in meters. + gap_length: The distance in meters between two adjacent stepstones. + height_offset: The height difference in meters between two adjacent + stepstones. + rgba_color: The color and transparency of the object, each in the range + [0,1]. Defaults to opaque gray. + + Returns: + The position of the mid point of the right-top edge of the stepstone. + The pybullet id of the stepstone. + """ + half_length = stone_length / 2.0 + half_width = stone_width / 2.0 + half_height = stone_height / 2.0 + start_pos = np.asarray(start_pos) + np.array([gap_length, 0, height_offset]) + step_stone_id = load_box(pybullet_client, + half_extents=[half_length, half_width, half_height], + position=start_pos + np.array([half_length, 0, -half_height]), + orientation=(0, 0, 0, 1), + rgba_color=rgba_color, + mass=0) + end_pos = start_pos + np.array([stone_length, 0, 0]) + return end_pos, step_stone_id + + +def build_stepstones( + pybullet_client: bullet_client.BulletClient, + start_pos: Sequence[float] = (0, 0, 0), + num_stones: int = 5, + stone_length: float = 1, + stone_height: float = 0.15, + stone_width: float = 5.0, + gap_length: float = 0.3, + color_sequence: Sequence[Sequence[float]] = MULTICOLOR +) -> Tuple[np.ndarray, List[int]]: + """Generates a series of stepstones. + + All the stepstones share the same parameters, such as length, height, width + and gap distance. + + Args: + pybullet_client: The pybullet client instance. + start_pos: The starting position (the mid point of top-left edge) of the + first stepstone. + num_stones: The number of stepstones in this creation. + stone_length: The length of the stepstones in meters. + stone_height: The height of the stepstones in meters. + stone_width: The width of the stepstones in meters. + gap_length: The distance in meters between two adjacent stepstones. + color_sequence: A list of (red, green, blue, alpha) colors where each + element is in [0, 1] and alpha is transparency. The stepstones will cycle + through these colors. + + Returns: + The position of the midpoint of the right-top edge of the last stepstone. + The pybullet ids of the stepstones. + """ + end_pos = start_pos + ids = [] + for _, color in zip(range(num_stones), itertools.cycle(color_sequence)): + end_pos, step_stone_id = build_one_stepstone( + pybullet_client=pybullet_client, + start_pos=end_pos, + stone_length=stone_length, + stone_height=stone_height, + stone_width=stone_width, + gap_length=gap_length, + height_offset=0, + rgba_color=color) + ids.append(step_stone_id) + return end_pos, ids + + +def build_random_stepstones( + pybullet_client: bullet_client.BulletClient, + start_pos: Sequence[float] = (0, 0, 0), + num_stones: int = 5, + stone_height: float = 0.15, + stone_width: float = 5.0, + stone_length_lower_bound: float = 0.5, + stone_length_upper_bound: float = 1.5, + gap_length_lower_bound: float = 0, + gap_length_upper_bound: float = 0.3, + height_offset_lower_bound: float = -0.1, + height_offset_upper_bound: float = 0.1, + random_seed: Optional[int] = None, + color_sequence: Sequence[Sequence[float]] = MULTICOLOR +) -> Tuple[np.ndarray, List[int]]: + """Generates a series of stepstones with randomly chosen parameters. + + Each stepstone in this series might have different randomly chosen + parameters. + + Args: + pybullet_client: The pybullet client instance. + start_pos: The starting position (the midpoint of top-left edge) of the + stepstone. + num_stones: The number of stepstones in this creation. + stone_height: The height of the stepstones in meters. + stone_width: The width of the stepstones in meters. + stone_length_lower_bound: The lower bound of the stone lengths in meters + when sampled randomly. + stone_length_upper_bound: The upper bound of the stone lengths in meters + when sampled randomly. + gap_length_lower_bound: The lower bound of the gap distances in meters when + sampled randomly. + gap_length_upper_bound: The upper bound of the gap distances in meters when + sampled randomly. + height_offset_lower_bound: The lower bound of the stone height offsets in + meters when sampled randomly. + height_offset_upper_bound: The upper bound of the stone height offsets in + meters when sampled randomly. + random_seed: The random seed of the random number generator. + color_sequence: A list of (red, green, blue, alpha) colors where each + element is in [0, 1] and alpha is transparency. The stepstones will cycle + through these colors. + + Returns: + The position of the mid point of the right-top edge of the stepstone. + The pybullet ids of the stepstones. + """ + end_pos = start_pos + ids = [] + random_generator = random.Random() + random_generator.seed(random_seed) + for _, color in zip(range(num_stones), itertools.cycle(color_sequence)): + stone_length = random_generator.uniform(stone_length_lower_bound, + stone_length_upper_bound) + gap_length = random_generator.uniform(gap_length_lower_bound, + gap_length_upper_bound) + height_offset = random_generator.uniform(height_offset_lower_bound, + height_offset_upper_bound) + end_pos, step_stone_id = build_one_stepstone( + pybullet_client=pybullet_client, + start_pos=end_pos, + stone_length=stone_length, + stone_height=stone_height, + stone_width=stone_width, + gap_length=gap_length, + height_offset=height_offset, + rgba_color=color) + ids.append(step_stone_id) + return end_pos, ids + + +def build_platform_at_origin( + pybullet_client: bullet_client.BulletClient +) -> Tuple[np.ndarray, int]: + """Builds a platform for the robot to start standing on. + + Args: + pybullet_client: The pybullet client instance. + + Returns: + The position of the mid point of the right-top edge of the platform. + The pybullet id of the platform. + """ + end_pos, platform_id = build_one_stepstone( + pybullet_client=pybullet_client, + start_pos=(-0.5, 0, 0), + stone_length=1.0, + stone_height=0.1, + stone_width=10.0, + gap_length=0.0, + height_offset=0.0, + rgba_color=GRAY) + return end_pos, platform_id diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/vision/sim_camera.py b/examples/pybullet/gym/pybullet_envs/minitaur/vision/sim_camera.py index 27a194723..2cd8056cc 100644 --- a/examples/pybullet/gym/pybullet_envs/minitaur/vision/sim_camera.py +++ b/examples/pybullet/gym/pybullet_envs/minitaur/vision/sim_camera.py @@ -130,7 +130,6 @@ def create_camera_image(pybullet_client, projectionMatrix=projection_mat, renderer=renderer) - class MountedCamera(object): """A camera that is fixed to a robot's body part. @@ -305,12 +304,11 @@ class MountedCamera(object): depth, self._prev_depth) depth = self._prev_depth self._prev_depth = depth - rgba = np.array(rgba, dtype=np.float32) + rgba = np.reshape(np.array(rgba, dtype=np.float32), (self._resolution[0], self._resolution[1], -1)) # Converts from OpenGL depth map to a distance map (unit: meter). distances = from_opengl_depth_to_distance( - np.array(depth), self._near, self._far) - + np.reshape(np.array(depth), (self._resolution[0], self._resolution[1], -1)), self._near, self._far) if self._normalize_rgb: # Does not normalize the depth image. rgba = 2 * rgba / 255.0 - 1