Scarfski/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3.git synced 2026-06-08 08:13:55 +00:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'bulletphysics:master' into master

Browse Source
This commit is contained in:
jingyuc
2021-10-28 01:39:07 -04:00
committed by GitHub
parent 88863f92eb e4a450a53d
commit 3540400a9d
21 changed files with 629 additions and 52 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
VERSION
View File

@@ -1 +1 @@
3.19
3.20

8
examples/pybullet/examples/mimicJointConstraint.py
View File

@@ -7,6 +7,8 @@ import pybullet_data
p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.resetDebugVisualizerCamera(cameraDistance=1.1, cameraYaw = 3.6,cameraPitch=-27, cameraTargetPosition=[0.19,1.21,-0.44])
p.loadURDF("plane.urdf", 0, 0, -2)
wheelA = p.loadURDF("differential/diff_ring.urdf", [0, 0, 0])
for i in range(p.getNumJoints(wheelA)):
@@ -21,7 +23,7 @@ c = p.createConstraint(wheelA,
jointAxis=[0, 1, 0],
parentFramePosition=[0, 0, 0],
childFramePosition=[0, 0, 0])
p.changeConstraint(c, gearRatio=1, maxForce=10000)
p.changeConstraint(c, gearRatio=1, maxForce=10000,erp=0.2)
c = p.createConstraint(wheelA,
2,
@@ -31,7 +33,7 @@ c = p.createConstraint(wheelA,
jointAxis=[0, 1, 0],
parentFramePosition=[0, 0, 0],
childFramePosition=[0, 0, 0])
p.changeConstraint(c, gearRatio=-1, maxForce=10000)
p.changeConstraint(c, gearRatio=-1, maxForce=10000,erp=0.2)
c = p.createConstraint(wheelA,
1,
@@ -41,7 +43,7 @@ c = p.createConstraint(wheelA,
jointAxis=[0, 1, 0],
parentFramePosition=[0, 0, 0],
childFramePosition=[0, 0, 0])
p.changeConstraint(c, gearRatio=-1, maxForce=10000)
p.changeConstraint(c, gearRatio=-1, maxForce=10000,erp=0.2)
p.setRealTimeSimulation(1)
while (1):

3
examples/pybullet/gym/pybullet_data/configs/laikago_mpc_stepstone.gin
View File

@@ -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

165
examples/pybullet/gym/pybullet_data/configs/laikago_mpc_two_camera_random_stepstone.gin Normal file
View File

@@ -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

20
examples/pybullet/gym/pybullet_envs/examples/testEnv.py
View File

@@ -13,10 +13,28 @@ def test(args):
count = 0
env = gym.make(args.env)
env.env.configure(args)
print("args.render=", args.render)
if (args.render == 1):
env.render(mode="human")
env.reset()
w, h, vmat,projmat,camup,camfwd,hor,ver,yaw,pitch,dist,target= p.getDebugVisualizerCamera()
dist = 0.4
yaw = 0
p.resetDebugVisualizerCamera(dist,yaw, pitch,target)
for obindex in range (p.getNumBodies()):
obuid = p.getBodyUniqueId(obindex)
p.changeDynamics(obuid, -1, linearDamping=0, angularDamping=0)
for l in range (p.getNumJoints(obuid)):
p.changeDynamics(obuid, l, linearDamping=0, angularDamping=0, jointDamping=0)
#if (l==0):
# p.setJointMotorControl2(obuid,l,p.POSITION_CONTROL,targetPosition=0)
if (l==2):
jp,jv,_,_ = p.getJointState(obuid,l)
p.resetJointState(obuid,l, jp,0.01 )
if (args.resetbenchmark):
while (1):
env.reset()
@@ -26,6 +44,8 @@ def test(args):
print("action space:")
sample = env.action_space.sample()
action = sample * 0.0
action = [0,0]#sample * 0.0
print("action=")
print(action)
for i in range(args.steps):

1
examples/pybullet/gym/pybullet_envs/minitaur/envs/README.md
View File

@@ -9,7 +9,6 @@ The following two environments are used in the RSS paper "[Sim-to-Real: Learning
The rest are experimental environments.
## Prerequisites
Install [TensorFlow](https://www.tensorflow.org/install/)
Install OpenAI gym
```

13
examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_logging.py
View File

@@ -18,8 +18,8 @@ os.sys.path.insert(0, parentdir)
import datetime
import os
import time
import logging
import tensorflow.compat.v1 as tf
from pybullet_envs.minitaur.envs import minitaur_logging_pb2
NUM_MOTORS = 8
@@ -74,7 +74,7 @@ def update_episode_proto(episode_proto, minitaur, action, step):
"""
max_num_steps = len(episode_proto.state_action)
if step >= max_num_steps:
tf.logging.warning("{}th step is not recorded in the logging since only {} steps were "
logging.warning("{}th step is not recorded in the logging since only {} steps were "
"pre-allocated.".format(step, max_num_steps))
return
step_log = episode_proto.state_action[step]
@@ -119,12 +119,13 @@ class MinitaurLogging(object):
"""
if not self._log_path or not episode_proto.state_action:
return self._log_path
if not tf.gfile.Exists(self._log_path):
tf.gfile.MakeDirs(self._log_path)
if not os.path.exists(self._log_path):
os.mkdir(self._log_path)
ts = time.time()
time_stamp = datetime.datetime.fromtimestamp(ts).strftime("%Y-%m-%d-%H%M%S")
log_path = os.path.join(self._log_path, "minitaur_log_{}".format(time_stamp))
with tf.gfile.Open(log_path, "w") as f:
with open(log_path, 'wb') as f:
f.write(episode_proto.SerializeToString())
return log_path
@@ -136,7 +137,7 @@ class MinitaurLogging(object):
Returns:
The minitaur episode proto.
"""
with tf.gfile.Open(log_path, 'rb') as f:
with open(log_path, 'rb') as f:
content = f.read()
episode_proto = minitaur_logging_pb2.MinitaurEpisode()
episode_proto.ParseFromString(content)

2
examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/examples/laikago_mpc_wrapper_example.py
View File

@@ -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

6
examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/examples/laikago_pmtg_example.py
View File

@@ -20,7 +20,8 @@ from __future__ import print_function
import os
import gin
import tensorflow.compat.v1 as tf
import sys
from pybullet_envs.minitaur.envs_v2 import env_loader
import pybullet_data as pd
@@ -81,4 +82,5 @@ def main(argv):
if __name__ == "__main__":
tf.app.run(main)
main(sys.argv)

6
examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/locomotion_gym_config_test.py
View File

@@ -6,12 +6,12 @@ from __future__ import print_function
import gin
from pybullet_envs.minitaur.envs_v2 import locomotion_gym_config
import tensorflow.compat.v1 as tf
import unittest
from absl.testing import parameterized
class LocomotionGymConfigTest(tf.test.TestCase, parameterized.TestCase):
class LocomotionGymConfigTest(unittest.TestCase):
def testSimulationParametersFromGinString(self):
config_text = (
@@ -72,4 +72,4 @@ class LocomotionGymConfigTest(tf.test.TestCase, parameterized.TestCase):
if __name__ == '__main__':
tf.test.main()
unittest.main()

31
examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/locomotion_gym_env_test.py
View File

@@ -10,7 +10,7 @@ import random
import gin
import mock
import numpy as np
import tensorflow.compat.v1 as tf
import unittest
from absl.testing import parameterized
from pybullet_envs.minitaur.envs_v2 import locomotion_gym_env
@@ -56,7 +56,7 @@ class TestTask(task_interface.Task):
return False
class LocomotionGymEnvTest(tf.test.TestCase, parameterized.TestCase):
class LocomotionGymEnvTest(unittest.TestCase):
def setUp(self):
super().setUp()
@@ -68,7 +68,7 @@ class LocomotionGymEnvTest(tf.test.TestCase, parameterized.TestCase):
env = locomotion_gym_env.LocomotionGymEnv()
self.assertIsInstance(env.robot, minitaur_v2.Minitaur)
# The robot will stand on the ground.
self.assertNear(env.robot.base_position[2], 0.25, 5e-2)
self.assertAlmostEqual(env.robot.base_position[2], 0.25, 1)
def test_reset_gym(self):
gin.parse_config_file(_CONFIG_FILE)
@@ -80,8 +80,8 @@ class LocomotionGymEnvTest(tf.test.TestCase, parameterized.TestCase):
action_dim)
observations = env_utils.flatten_observations(observations)
self.assertEqual(observations.size, 12)
self.assertNear(observations[0], 0, 1e-2)
self.assertNear(observations[4], desired_init_motor_angle, 2e-1)
self.assertAlmostEqual(observations[0], 0, 1)
self.assertAlmostEqual(observations[4], desired_init_motor_angle, 0)
def test_step_gym(self):
gin.parse_config_file(_CONFIG_FILE)
@@ -98,8 +98,8 @@ class LocomotionGymEnvTest(tf.test.TestCase, parameterized.TestCase):
self.assertFalse(done)
self.assertEqual(reward, steps)
self.assertEqual(observations.size, 12)
self.assertNear(observations[0], 0, 1e-2)
self.assertNear(observations[4], desired_motor_angle, 2e-1)
self.assertAlmostEqual(observations[0], 0, 1)
self.assertAlmostEqual(observations[4], desired_motor_angle, 1)
np.testing.assert_allclose(env._last_action,
[desired_motor_angle] * action_dim, 2e-1)
@@ -126,7 +126,7 @@ class LocomotionGymEnvTest(tf.test.TestCase, parameterized.TestCase):
env = locomotion_gym_env.LocomotionGymEnv(task=None, scene=None)
# The robot will free fall.
self.assertNear(env.robot.base_position[2], 0.15, 5e-2)
self.assertAlmostEqual(env.robot.base_position[2], 0.15, 1)
def test_seed_draw_with_np(self):
gin.parse_config_file(_CONFIG_FILE)
@@ -150,12 +150,11 @@ class LocomotionGymEnvTest(tf.test.TestCase, parameterized.TestCase):
action_dim = len(env.action_space.high)
observations = env.reset(initial_motor_angles=[desired_init_motor_angle] *
action_dim)
self.assertLen(observations, 2)
self.assertLen(observations['IMU'], 4)
self.assertNear(observations['IMU'][0], 0, 1e-2)
self.assertLen(observations['MotorAngle'], 8)
self.assertNear(observations['MotorAngle'][0], desired_init_motor_angle,
2e-1)
self.assertEqual(len(observations), 2)
self.assertEqual(len(observations['IMU']), 4)
self.assertAlmostEqual(observations['IMU'][0], 0, 2)
self.assertEqual(len(observations['MotorAngle']), 8)
self.assertAlmostEqual(observations['MotorAngle'][0], desired_init_motor_angle,0)
@@ -171,7 +170,7 @@ class LocomotionGymEnvTest(tf.test.TestCase, parameterized.TestCase):
])
env = locomotion_gym_env.LocomotionGymEnv()
self.assertLen(env.scene.dynamic_objects, 2)
self.assertEqual(len(env.scene.dynamic_objects), 2)
for obj in env.scene.dynamic_objects:
self.assertIsInstance(obj, autonomous_object.AutonomousObject)
@@ -238,4 +237,4 @@ class LocomotionGymEnvTest(tf.test.TestCase, parameterized.TestCase):
if __name__ == '__main__':
tf.test.main()
unittest.main()

149
examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/scenes/random_stepstone_scene.py Normal file
View File

@@ -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)

234
examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/scenes/terrain/stepstones.py Normal file
View File

@@ -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

7
examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/utilities/env_utils.py
View File

@@ -8,8 +8,13 @@ import collections
import gin
from gym import spaces
import numpy as np
import traceback
import logging
import tensorflow.compat.v1 as tf
try:
import tensorflow.compat.v1 as tf
except Exception as e:
pass # logging.warning(traceback.format_exc())

6
examples/pybullet/gym/pybullet_envs/minitaur/vision/sim_camera.py
View File

@@ -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

8
examples/pybullet/gym/pybullet_envs/robot_bases.py
View File

@@ -22,10 +22,10 @@ class XmlBasedRobot:
self.ordered_joints = None
self.robot_body = None
high = np.ones([action_dim])
self.action_space = gym.spaces.Box(-high, high)
high = np.inf * np.ones([obs_dim])
self.observation_space = gym.spaces.Box(-high, high)
high = np.ones([action_dim], dtype=np.float32)
self.action_space = gym.spaces.Box(-high, high, dtype=np.float32)
high = np.inf * np.ones([obs_dim], dtype=np.float32)
self.observation_space = gym.spaces.Box(-high, high, dtype=np.float32)
#self.model_xml = model_xml
self.robot_name = robot_name

2
setup.py
View File

@@ -501,7 +501,7 @@ if 'BT_USE_EGL' in EGL_CXX_FLAGS:
setup(
name='pybullet',
version='3.1.9',
version='3.2.0',
description=
'Official Python Interface for the Bullet Physics SDK specialized for Robotics Simulation and Reinforcement Learning',
long_description=

4
src/BulletSoftBody/btSoftBody.h
View File

@@ -1327,8 +1327,8 @@ public:
}
for (int k = 0; k < m_faceNodeContacts.size(); ++k)
{
int i = indices[k];
btSoftBody::DeformableFaceNodeContact& c = m_faceNodeContacts[i];
int idx = indices[k];
btSoftBody::DeformableFaceNodeContact& c = m_faceNodeContacts[idx];
btSoftBody::Node* node = c.m_node;
btSoftBody::Face* face = c.m_face;
const btVector3& w = c.m_bary;

5
src/LinearMath/TaskScheduler/btThreadSupportWin32.cpp
View File

@@ -82,6 +82,10 @@ typedef BOOL(WINAPI* Pfn_GetLogicalProcessorInformation)(PSYSTEM_LOGICAL_PROCESS
void getProcessorInformation(btProcessorInfo* procInfo)
{
memset(procInfo, 0, sizeof(*procInfo));
#if WINAPI_FAMILY_PARTITION(WINAPI_PARTITION_APP) && !WINAPI_FAMILY_PARTITION(WINAPI_PARTITION_DESKTOP)
// Can't dlopen libraries on UWP.
return;
#else
Pfn_GetLogicalProcessorInformation getLogicalProcInfo =
(Pfn_GetLogicalProcessorInformation)GetProcAddress(GetModuleHandle(TEXT("kernel32")), "GetLogicalProcessorInformation");
if (getLogicalProcInfo == NULL)
@@ -160,6 +164,7 @@ void getProcessorInformation(btProcessorInfo* procInfo)
}
}
free(buf);
#endif
}
///btThreadSupportWin32 helps to initialize/shutdown libspe2, start/stop SPU tasks and communication

2
src/LinearMath/btScalar.h
View File

@@ -25,7 +25,7 @@ subject to the following restrictions:
#include <float.h>
/* SVN $Revision$ on $Date$ from http://bullet.googlecode.com*/
#define BT_BULLET_VERSION 319
#define BT_BULLET_VERSION 320
inline int btGetVersion()
{

7
src/LinearMath/btSerializer.h
View File

@@ -480,8 +480,8 @@ public:
}
buffer[9] = '3';
buffer[10] = '1';
buffer[11] = '9';
buffer[10] = '2';
buffer[11] = '0';
}
virtual void startSerialization()
@@ -499,7 +499,6 @@ public:
writeDNA();
//if we didn't pre-allocate a buffer, we need to create a contiguous buffer now
int mysize = 0;
if (!m_totalSize)
{
if (m_buffer)
@@ -511,14 +510,12 @@ public:
unsigned char* currentPtr = m_buffer;
writeHeader(m_buffer);
currentPtr += BT_HEADER_LENGTH;
mysize += BT_HEADER_LENGTH;
for (int i = 0; i < m_chunkPtrs.size(); i++)
{
int curLength = sizeof(btChunk) + m_chunkPtrs[i]->m_length;
memcpy(currentPtr, m_chunkPtrs[i], curLength);
btAlignedFree(m_chunkPtrs[i]);
currentPtr += curLength;
mysize += curLength;
}
}