diff --git a/VERSION b/VERSION index d22ba0602..b95829267 100644 --- a/VERSION +++ b/VERSION @@ -1 +1 @@ -3.19 +3.20 diff --git a/examples/pybullet/examples/mimicJointConstraint.py b/examples/pybullet/examples/mimicJointConstraint.py index 7a54f5c10..45a10d918 100644 --- a/examples/pybullet/examples/mimicJointConstraint.py +++ b/examples/pybullet/examples/mimicJointConstraint.py @@ -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): 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/examples/testEnv.py b/examples/pybullet/gym/pybullet_envs/examples/testEnv.py index 5d30d27af..6c550fe4f 100644 --- a/examples/pybullet/gym/pybullet_envs/examples/testEnv.py +++ b/examples/pybullet/gym/pybullet_envs/examples/testEnv.py @@ -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): diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs/README.md b/examples/pybullet/gym/pybullet_envs/minitaur/envs/README.md index bcdb8d0f6..6124264c9 100644 --- a/examples/pybullet/gym/pybullet_envs/minitaur/envs/README.md +++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs/README.md @@ -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 ``` diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_logging.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_logging.py index b6f1c5a3d..2cff24e94 100644 --- a/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_logging.py +++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs/minitaur_logging.py @@ -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) 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/examples/laikago_pmtg_example.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/examples/laikago_pmtg_example.py index c86c10db4..d791dd18a 100644 --- a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/examples/laikago_pmtg_example.py +++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/examples/laikago_pmtg_example.py @@ -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) diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/locomotion_gym_config_test.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/locomotion_gym_config_test.py index fc53f02e7..343b58db0 100644 --- a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/locomotion_gym_config_test.py +++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/locomotion_gym_config_test.py @@ -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() diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/locomotion_gym_env_test.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/locomotion_gym_env_test.py index 51f3c9ae6..1c9ea0e9b 100644 --- a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/locomotion_gym_env_test.py +++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/locomotion_gym_env_test.py @@ -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() 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/envs_v2/utilities/env_utils.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/utilities/env_utils.py index f82283bf3..5ae427a43 100644 --- a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/utilities/env_utils.py +++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/utilities/env_utils.py @@ -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()) 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 diff --git a/examples/pybullet/gym/pybullet_envs/robot_bases.py b/examples/pybullet/gym/pybullet_envs/robot_bases.py index 1e22f1bbd..951470ac3 100644 --- a/examples/pybullet/gym/pybullet_envs/robot_bases.py +++ b/examples/pybullet/gym/pybullet_envs/robot_bases.py @@ -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 diff --git a/setup.py b/setup.py index 0af1e4985..fd6f486a0 100644 --- a/setup.py +++ b/setup.py @@ -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= diff --git a/src/BulletSoftBody/btSoftBody.h b/src/BulletSoftBody/btSoftBody.h index b4ace7b7a..ea8fcd76f 100644 --- a/src/BulletSoftBody/btSoftBody.h +++ b/src/BulletSoftBody/btSoftBody.h @@ -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; diff --git a/src/LinearMath/TaskScheduler/btThreadSupportWin32.cpp b/src/LinearMath/TaskScheduler/btThreadSupportWin32.cpp index 922e449cc..5010cca84 100644 --- a/src/LinearMath/TaskScheduler/btThreadSupportWin32.cpp +++ b/src/LinearMath/TaskScheduler/btThreadSupportWin32.cpp @@ -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 diff --git a/src/LinearMath/btScalar.h b/src/LinearMath/btScalar.h index 4c9c1f385..28c03b286 100644 --- a/src/LinearMath/btScalar.h +++ b/src/LinearMath/btScalar.h @@ -25,7 +25,7 @@ subject to the following restrictions: #include /* SVN $Revision$ on $Date$ from http://bullet.googlecode.com*/ -#define BT_BULLET_VERSION 319 +#define BT_BULLET_VERSION 320 inline int btGetVersion() { diff --git a/src/LinearMath/btSerializer.h b/src/LinearMath/btSerializer.h index 6b624122a..2dc981454 100644 --- a/src/LinearMath/btSerializer.h +++ b/src/LinearMath/btSerializer.h @@ -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; } }