mirror of
https://github.com/bulletphysics/bullet3
synced 2024-12-13 13:20:07 +00:00
Merge pull request #3994 from VincentYu68/master
Add a visual-locomotion configuration: Laikago with two cameras walking over stepstone
This commit is contained in:
erwincoumans
GitHub
commit
aef54fed64
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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)
|
||||
@ -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
|
||||
@ -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
|
||||
|
||||
Loading…
Reference in New Issue
Block a user