diff --git a/examples/pybullet/gym/pybullet_data/configs/laikago_gym_config.gin b/examples/pybullet/gym/pybullet_data/configs/laikago_gym_config.gin
new file mode 100644
index 000000000..3a7095680
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/configs/laikago_gym_config.gin
@@ -0,0 +1,80 @@
+import pybullet_envs.minitaur.envs_v2.locomotion_gym_config
+import pybullet_envs.minitaur.envs_v2.sensors.robot_sensors
+
+UPPER_BOUND = 6.28318548203
+LOWER_BOUND = -6.28318548203
+SIM_TIME_STEP = 0.001
+NUM_ACTION_REPEAT = 2
+NUM_MOTORS = 12
+NOISY_READING = True
+
+locomotion_gym_config.SimulationParameters.sim_time_step_s = %SIM_TIME_STEP
+locomotion_gym_config.SimulationParameters.num_action_repeat = %NUM_ACTION_REPEAT
+locomotion_gym_config.SimulationParameters.enable_rendering = False
+locomotion_gym_config.LocomotionGymConfig.simulation_parameters = @locomotion_gym_config.SimulationParameters()
+
+robot_sensors.IMUSensor.channels = ["R", "P", "dR", "dP"]
+robot_sensors.IMUSensor.noisy_reading = %NOISY_READING
+robot_sensors.IMUSensor.lower_bound = [-6.28318548203, -6.28318548203,
+ -6283.18554688, -6283.18554688]
+robot_sensors.IMUSensor.upper_bound = [6.28318548203, 6.28318548203,
+ 6283.18554688, 6283.18554688]
+
+robot_sensors.MotorAngleSensor.num_motors = %NUM_MOTORS
+robot_sensors.MotorAngleSensor.noisy_reading = %NOISY_READING
+robot_sensors.MotorAngleSensor.lower_bound = -6.28318548203
+robot_sensors.MotorAngleSensor.upper_bound = 6.28318548203
+
+sensors = [@robot_sensors.IMUSensor(), @robot_sensors.MotorAngleSensor()]
+
+Act0/locomotion_gym_config.ScalarField.name = "motor_angle_0"
+Act0/locomotion_gym_config.ScalarField.upper_bound = %UPPER_BOUND
+Act0/locomotion_gym_config.ScalarField.lower_bound = %LOWER_BOUND
+Act1/locomotion_gym_config.ScalarField.name = "motor_angle_1"
+Act1/locomotion_gym_config.ScalarField.upper_bound = %UPPER_BOUND
+Act1/locomotion_gym_config.ScalarField.lower_bound = %LOWER_BOUND
+Act2/locomotion_gym_config.ScalarField.name = "motor_angle_2"
+Act2/locomotion_gym_config.ScalarField.upper_bound = %UPPER_BOUND
+Act2/locomotion_gym_config.ScalarField.lower_bound = %LOWER_BOUND
+Act3/locomotion_gym_config.ScalarField.name = "motor_angle_3"
+Act3/locomotion_gym_config.ScalarField.upper_bound = %UPPER_BOUND
+Act3/locomotion_gym_config.ScalarField.lower_bound = %LOWER_BOUND
+Act4/locomotion_gym_config.ScalarField.name = "motor_angle_4"
+Act4/locomotion_gym_config.ScalarField.upper_bound = %UPPER_BOUND
+Act4/locomotion_gym_config.ScalarField.lower_bound = %LOWER_BOUND
+Act5/locomotion_gym_config.ScalarField.name = "motor_angle_5"
+Act5/locomotion_gym_config.ScalarField.upper_bound = %UPPER_BOUND
+Act5/locomotion_gym_config.ScalarField.lower_bound = %LOWER_BOUND
+Act6/locomotion_gym_config.ScalarField.name = "motor_angle_6"
+Act6/locomotion_gym_config.ScalarField.upper_bound = %UPPER_BOUND
+Act6/locomotion_gym_config.ScalarField.lower_bound = %LOWER_BOUND
+Act7/locomotion_gym_config.ScalarField.name = "motor_angle_7"
+Act7/locomotion_gym_config.ScalarField.upper_bound = %UPPER_BOUND
+Act7/locomotion_gym_config.ScalarField.lower_bound = %LOWER_BOUND
+Act8/locomotion_gym_config.ScalarField.name = "motor_angle_8"
+Act8/locomotion_gym_config.ScalarField.upper_bound = %UPPER_BOUND
+Act8/locomotion_gym_config.ScalarField.lower_bound = %LOWER_BOUND
+Act9/locomotion_gym_config.ScalarField.name = "motor_angle_9"
+Act9/locomotion_gym_config.ScalarField.upper_bound = %UPPER_BOUND
+Act9/locomotion_gym_config.ScalarField.lower_bound = %LOWER_BOUND
+Act10/locomotion_gym_config.ScalarField.name = "motor_angle_10"
+Act10/locomotion_gym_config.ScalarField.upper_bound = %UPPER_BOUND
+Act10/locomotion_gym_config.ScalarField.lower_bound = %LOWER_BOUND
+Act11/locomotion_gym_config.ScalarField.name = "motor_angle_11"
+Act11/locomotion_gym_config.ScalarField.upper_bound = %UPPER_BOUND
+Act11/locomotion_gym_config.ScalarField.lower_bound = %LOWER_BOUND
+
+
+locomotion_gym_config.LocomotionGymConfig.actions = [
+ @Act0/locomotion_gym_config.ScalarField(),
+ @Act1/locomotion_gym_config.ScalarField(),
+ @Act2/locomotion_gym_config.ScalarField(),
+ @Act3/locomotion_gym_config.ScalarField(),
+ @Act4/locomotion_gym_config.ScalarField(),
+ @Act5/locomotion_gym_config.ScalarField(),
+ @Act6/locomotion_gym_config.ScalarField(),
+ @Act7/locomotion_gym_config.ScalarField(),
+ @Act8/locomotion_gym_config.ScalarField(),
+ @Act9/locomotion_gym_config.ScalarField(),
+ @Act10/locomotion_gym_config.ScalarField(),
+ @Act11/locomotion_gym_config.ScalarField()]
diff --git a/examples/pybullet/gym/pybullet_data/configs/laikago_gym_env.gin b/examples/pybullet/gym/pybullet_data/configs/laikago_gym_env.gin
new file mode 100644
index 000000000..0b3b1ce95
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/configs/laikago_gym_env.gin
@@ -0,0 +1,116 @@
+import pybullet_envs.minitaur.envs_v2.locomotion_gym_env
+import pybullet_envs.minitaur.envs_v2.scenes.simple_scene
+import pybullet_envs.minitaur.envs_v2.tasks.simple_locomotion_task
+import pybullet_envs.minitaur.robots.laikago
+
+URDF_ROOT = "urdf/"
+ABDUCTION_P_GAIN = 220.0
+ABDUCTION_D_GAIN = 0.3
+HIP_P_GAIN = 220.0
+HIP_D_GAIN = 2.0
+KNEE_P_GAIN = 220.0
+KNEE_D_GAIN = 2.0
+
+import pybullet_envs.minitaur.envs_v2.locomotion_gym_config
+import pybullet_envs.minitaur.envs_v2.sensors.robot_sensors
+
+UPPER_BOUND = 6.28318548203
+LOWER_BOUND = -6.28318548203
+SIM_TIME_STEP = 0.001
+NUM_ACTION_REPEAT = 2
+NUM_MOTORS = 12
+NOISY_READING = True
+
+locomotion_gym_config.SimulationParameters.sim_time_step_s = %SIM_TIME_STEP
+locomotion_gym_config.SimulationParameters.num_action_repeat = %NUM_ACTION_REPEAT
+locomotion_gym_config.SimulationParameters.enable_rendering = False
+locomotion_gym_config.LocomotionGymConfig.simulation_parameters = @locomotion_gym_config.SimulationParameters()
+
+robot_sensors.IMUSensor.channels = ["R", "P", "dR", "dP"]
+robot_sensors.IMUSensor.noisy_reading = %NOISY_READING
+robot_sensors.IMUSensor.lower_bound = [-6.28318548203, -6.28318548203,
+ -6283.18554688, -6283.18554688]
+robot_sensors.IMUSensor.upper_bound = [6.28318548203, 6.28318548203,
+ 6283.18554688, 6283.18554688]
+
+robot_sensors.MotorAngleSensor.num_motors = %NUM_MOTORS
+robot_sensors.MotorAngleSensor.noisy_reading = %NOISY_READING
+robot_sensors.MotorAngleSensor.lower_bound = -6.28318548203
+robot_sensors.MotorAngleSensor.upper_bound = 6.28318548203
+
+sensors = [@robot_sensors.IMUSensor(), @robot_sensors.MotorAngleSensor()]
+
+Act0/locomotion_gym_config.ScalarField.name = "motor_angle_0"
+Act0/locomotion_gym_config.ScalarField.upper_bound = %UPPER_BOUND
+Act0/locomotion_gym_config.ScalarField.lower_bound = %LOWER_BOUND
+Act1/locomotion_gym_config.ScalarField.name = "motor_angle_1"
+Act1/locomotion_gym_config.ScalarField.upper_bound = %UPPER_BOUND
+Act1/locomotion_gym_config.ScalarField.lower_bound = %LOWER_BOUND
+Act2/locomotion_gym_config.ScalarField.name = "motor_angle_2"
+Act2/locomotion_gym_config.ScalarField.upper_bound = %UPPER_BOUND
+Act2/locomotion_gym_config.ScalarField.lower_bound = %LOWER_BOUND
+Act3/locomotion_gym_config.ScalarField.name = "motor_angle_3"
+Act3/locomotion_gym_config.ScalarField.upper_bound = %UPPER_BOUND
+Act3/locomotion_gym_config.ScalarField.lower_bound = %LOWER_BOUND
+Act4/locomotion_gym_config.ScalarField.name = "motor_angle_4"
+Act4/locomotion_gym_config.ScalarField.upper_bound = %UPPER_BOUND
+Act4/locomotion_gym_config.ScalarField.lower_bound = %LOWER_BOUND
+Act5/locomotion_gym_config.ScalarField.name = "motor_angle_5"
+Act5/locomotion_gym_config.ScalarField.upper_bound = %UPPER_BOUND
+Act5/locomotion_gym_config.ScalarField.lower_bound = %LOWER_BOUND
+Act6/locomotion_gym_config.ScalarField.name = "motor_angle_6"
+Act6/locomotion_gym_config.ScalarField.upper_bound = %UPPER_BOUND
+Act6/locomotion_gym_config.ScalarField.lower_bound = %LOWER_BOUND
+Act7/locomotion_gym_config.ScalarField.name = "motor_angle_7"
+Act7/locomotion_gym_config.ScalarField.upper_bound = %UPPER_BOUND
+Act7/locomotion_gym_config.ScalarField.lower_bound = %LOWER_BOUND
+Act8/locomotion_gym_config.ScalarField.name = "motor_angle_8"
+Act8/locomotion_gym_config.ScalarField.upper_bound = %UPPER_BOUND
+Act8/locomotion_gym_config.ScalarField.lower_bound = %LOWER_BOUND
+Act9/locomotion_gym_config.ScalarField.name = "motor_angle_9"
+Act9/locomotion_gym_config.ScalarField.upper_bound = %UPPER_BOUND
+Act9/locomotion_gym_config.ScalarField.lower_bound = %LOWER_BOUND
+Act10/locomotion_gym_config.ScalarField.name = "motor_angle_10"
+Act10/locomotion_gym_config.ScalarField.upper_bound = %UPPER_BOUND
+Act10/locomotion_gym_config.ScalarField.lower_bound = %LOWER_BOUND
+Act11/locomotion_gym_config.ScalarField.name = "motor_angle_11"
+Act11/locomotion_gym_config.ScalarField.upper_bound = %UPPER_BOUND
+Act11/locomotion_gym_config.ScalarField.lower_bound = %LOWER_BOUND
+
+
+locomotion_gym_config.LocomotionGymConfig.actions = [
+ @Act0/locomotion_gym_config.ScalarField(),
+ @Act1/locomotion_gym_config.ScalarField(),
+ @Act2/locomotion_gym_config.ScalarField(),
+ @Act3/locomotion_gym_config.ScalarField(),
+ @Act4/locomotion_gym_config.ScalarField(),
+ @Act5/locomotion_gym_config.ScalarField(),
+ @Act6/locomotion_gym_config.ScalarField(),
+ @Act7/locomotion_gym_config.ScalarField(),
+ @Act8/locomotion_gym_config.ScalarField(),
+ @Act9/locomotion_gym_config.ScalarField(),
+ @Act10/locomotion_gym_config.ScalarField(),
+ @Act11/locomotion_gym_config.ScalarField()]
+
+
+
+laikago.Laikago.urdf_root = %URDF_ROOT
+laikago.Laikago.time_step = %SIM_TIME_STEP
+laikago.Laikago.action_repeat = %NUM_ACTION_REPEAT
+laikago.Laikago.self_collision_enabled = False
+laikago.Laikago.control_latency = 0.002
+laikago.Laikago.pd_latency = 0.0
+laikago.Laikago.motor_kp = [%ABDUCTION_P_GAIN, %HIP_P_GAIN, %KNEE_P_GAIN,
+ %ABDUCTION_P_GAIN, %HIP_P_GAIN, %KNEE_P_GAIN,
+ %ABDUCTION_P_GAIN, %HIP_P_GAIN, %KNEE_P_GAIN,
+ %ABDUCTION_P_GAIN, %HIP_P_GAIN, %KNEE_P_GAIN]
+laikago.Laikago.motor_kd = [%ABDUCTION_D_GAIN, %HIP_D_GAIN, %KNEE_D_GAIN,
+ %ABDUCTION_D_GAIN, %HIP_D_GAIN, %KNEE_D_GAIN,
+ %ABDUCTION_D_GAIN, %HIP_D_GAIN, %KNEE_D_GAIN,
+ %ABDUCTION_D_GAIN, %HIP_D_GAIN, %KNEE_D_GAIN]
+laikago.Laikago.sensors = %sensors
+
+locomotion_gym_env.LocomotionGymEnv.gym_config = @locomotion_gym_config.LocomotionGymConfig()
+locomotion_gym_env.LocomotionGymEnv.robot_class = @laikago.Laikago
+locomotion_gym_env.LocomotionGymEnv.scene = @simple_scene.SimpleScene()
+locomotion_gym_env.LocomotionGymEnv.task = @simple_locomotion_task.SimpleForwardTask()
diff --git a/examples/pybullet/gym/pybullet_data/configs/laikago_mpc_example_flat.gin b/examples/pybullet/gym/pybullet_data/configs/laikago_mpc_example_flat.gin
new file mode 100644
index 000000000..df735f43f
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/configs/laikago_mpc_example_flat.gin
@@ -0,0 +1,149 @@
+#-*-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.locomotion_gym_config
+import pybullet_envs.minitaur.envs_v2.locomotion_gym_env
+import pybullet_envs.minitaur.envs_v2.scenes.simple_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 = @simple_scene.SimpleScene()
+locomotion_gym_config.SimulationParameters.sim_time_step_s = %SIM_TIME_STEP
+locomotion_gym_config.SimulationParameters.num_action_repeat = %NUM_ACTION_REPEAT
+
+
+########################################
+# 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
+
+env_loader.load.wrapper_classes = [
+ @mpc_locomotion_wrapper.MPCLocomotionWrapper,
+]
+
+########################################
+# 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_data/configs/laikago_mpc_stepstone.gin b/examples/pybullet/gym/pybullet_data/configs/laikago_mpc_stepstone.gin
new file mode 100644
index 000000000..2644d3b7b
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/configs/laikago_mpc_stepstone.gin
@@ -0,0 +1,142 @@
+#-*-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.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()
+
+
+sensors = [@imu_sensor.IMUSensor(), @last_action_sensor.LastActionSensor(), @toe_position_sensor.ToePositionSensor()]
+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 = 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_data/configs/laikago_reactive.gin b/examples/pybullet/gym/pybullet_data/configs/laikago_reactive.gin
new file mode 100644
index 000000000..b4d6f8ba0
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/configs/laikago_reactive.gin
@@ -0,0 +1,133 @@
+#-*-Python-*-
+
+# NOTE: Should be run with >=10CPU for decent performance.
+
+import pybullet_envs.minitaur.envs_v2.env_loader
+import pybullet_envs.minitaur.envs_v2.env_wrappers.observation_dictionary_to_array_wrapper
+import pybullet_envs.minitaur.envs_v2.env_wrappers.simple_openloop
+import pybullet_envs.minitaur.envs_v2.env_wrappers.trajectory_generator_wrapper_env
+
+
+import pybullet_envs.minitaur.envs_v2.locomotion_gym_env
+import pybullet_envs.minitaur.envs_v2.scenes.simple_scene
+import pybullet_envs.minitaur.envs_v2.tasks.simple_locomotion_task
+import pybullet_envs.minitaur.robots.laikago
+
+URDF_ROOT = "urdf/"
+ABDUCTION_P_GAIN = 220.0
+ABDUCTION_D_GAIN = 0.3
+HIP_P_GAIN = 220.0
+HIP_D_GAIN = 2.0
+KNEE_P_GAIN = 220.0
+KNEE_D_GAIN = 2.0
+
+import pybullet_envs.minitaur.envs_v2.locomotion_gym_config
+import pybullet_envs.minitaur.envs_v2.sensors.robot_sensors
+
+UPPER_BOUND = 6.28318548203
+LOWER_BOUND = -6.28318548203
+SIM_TIME_STEP = 0.001
+NUM_ACTION_REPEAT = 2
+NUM_MOTORS = 12
+NOISY_READING = True
+
+locomotion_gym_config.SimulationParameters.sim_time_step_s = %SIM_TIME_STEP
+locomotion_gym_config.SimulationParameters.num_action_repeat = %NUM_ACTION_REPEAT
+locomotion_gym_config.SimulationParameters.enable_rendering = False
+locomotion_gym_config.LocomotionGymConfig.simulation_parameters = @locomotion_gym_config.SimulationParameters()
+
+robot_sensors.IMUSensor.channels = ["R", "P", "dR", "dP"]
+robot_sensors.IMUSensor.noisy_reading = %NOISY_READING
+robot_sensors.IMUSensor.lower_bound = [-6.28318548203, -6.28318548203,
+ -6283.18554688, -6283.18554688]
+robot_sensors.IMUSensor.upper_bound = [6.28318548203, 6.28318548203,
+ 6283.18554688, 6283.18554688]
+
+robot_sensors.MotorAngleSensor.num_motors = %NUM_MOTORS
+robot_sensors.MotorAngleSensor.noisy_reading = %NOISY_READING
+robot_sensors.MotorAngleSensor.lower_bound = -6.28318548203
+robot_sensors.MotorAngleSensor.upper_bound = 6.28318548203
+
+sensors = [@robot_sensors.IMUSensor(), @robot_sensors.MotorAngleSensor()]
+
+Act0/locomotion_gym_config.ScalarField.name = "motor_angle_0"
+Act0/locomotion_gym_config.ScalarField.upper_bound = %UPPER_BOUND
+Act0/locomotion_gym_config.ScalarField.lower_bound = %LOWER_BOUND
+Act1/locomotion_gym_config.ScalarField.name = "motor_angle_1"
+Act1/locomotion_gym_config.ScalarField.upper_bound = %UPPER_BOUND
+Act1/locomotion_gym_config.ScalarField.lower_bound = %LOWER_BOUND
+Act2/locomotion_gym_config.ScalarField.name = "motor_angle_2"
+Act2/locomotion_gym_config.ScalarField.upper_bound = %UPPER_BOUND
+Act2/locomotion_gym_config.ScalarField.lower_bound = %LOWER_BOUND
+Act3/locomotion_gym_config.ScalarField.name = "motor_angle_3"
+Act3/locomotion_gym_config.ScalarField.upper_bound = %UPPER_BOUND
+Act3/locomotion_gym_config.ScalarField.lower_bound = %LOWER_BOUND
+Act4/locomotion_gym_config.ScalarField.name = "motor_angle_4"
+Act4/locomotion_gym_config.ScalarField.upper_bound = %UPPER_BOUND
+Act4/locomotion_gym_config.ScalarField.lower_bound = %LOWER_BOUND
+Act5/locomotion_gym_config.ScalarField.name = "motor_angle_5"
+Act5/locomotion_gym_config.ScalarField.upper_bound = %UPPER_BOUND
+Act5/locomotion_gym_config.ScalarField.lower_bound = %LOWER_BOUND
+Act6/locomotion_gym_config.ScalarField.name = "motor_angle_6"
+Act6/locomotion_gym_config.ScalarField.upper_bound = %UPPER_BOUND
+Act6/locomotion_gym_config.ScalarField.lower_bound = %LOWER_BOUND
+Act7/locomotion_gym_config.ScalarField.name = "motor_angle_7"
+Act7/locomotion_gym_config.ScalarField.upper_bound = %UPPER_BOUND
+Act7/locomotion_gym_config.ScalarField.lower_bound = %LOWER_BOUND
+Act8/locomotion_gym_config.ScalarField.name = "motor_angle_8"
+Act8/locomotion_gym_config.ScalarField.upper_bound = %UPPER_BOUND
+Act8/locomotion_gym_config.ScalarField.lower_bound = %LOWER_BOUND
+Act9/locomotion_gym_config.ScalarField.name = "motor_angle_9"
+Act9/locomotion_gym_config.ScalarField.upper_bound = %UPPER_BOUND
+Act9/locomotion_gym_config.ScalarField.lower_bound = %LOWER_BOUND
+Act10/locomotion_gym_config.ScalarField.name = "motor_angle_10"
+Act10/locomotion_gym_config.ScalarField.upper_bound = %UPPER_BOUND
+Act10/locomotion_gym_config.ScalarField.lower_bound = %LOWER_BOUND
+Act11/locomotion_gym_config.ScalarField.name = "motor_angle_11"
+Act11/locomotion_gym_config.ScalarField.upper_bound = %UPPER_BOUND
+Act11/locomotion_gym_config.ScalarField.lower_bound = %LOWER_BOUND
+
+
+locomotion_gym_config.LocomotionGymConfig.actions = [
+ @Act0/locomotion_gym_config.ScalarField(),
+ @Act1/locomotion_gym_config.ScalarField(),
+ @Act2/locomotion_gym_config.ScalarField(),
+ @Act3/locomotion_gym_config.ScalarField(),
+ @Act4/locomotion_gym_config.ScalarField(),
+ @Act5/locomotion_gym_config.ScalarField(),
+ @Act6/locomotion_gym_config.ScalarField(),
+ @Act7/locomotion_gym_config.ScalarField(),
+ @Act8/locomotion_gym_config.ScalarField(),
+ @Act9/locomotion_gym_config.ScalarField(),
+ @Act10/locomotion_gym_config.ScalarField(),
+ @Act11/locomotion_gym_config.ScalarField()]
+
+
+
+laikago.Laikago.urdf_root = %URDF_ROOT
+laikago.Laikago.time_step = %SIM_TIME_STEP
+laikago.Laikago.action_repeat = %NUM_ACTION_REPEAT
+laikago.Laikago.self_collision_enabled = False
+laikago.Laikago.control_latency = 0.002
+laikago.Laikago.pd_latency = 0.0
+laikago.Laikago.motor_kp = [%ABDUCTION_P_GAIN, %HIP_P_GAIN, %KNEE_P_GAIN,
+ %ABDUCTION_P_GAIN, %HIP_P_GAIN, %KNEE_P_GAIN,
+ %ABDUCTION_P_GAIN, %HIP_P_GAIN, %KNEE_P_GAIN,
+ %ABDUCTION_P_GAIN, %HIP_P_GAIN, %KNEE_P_GAIN]
+laikago.Laikago.motor_kd = [%ABDUCTION_D_GAIN, %HIP_D_GAIN, %KNEE_D_GAIN,
+ %ABDUCTION_D_GAIN, %HIP_D_GAIN, %KNEE_D_GAIN,
+ %ABDUCTION_D_GAIN, %HIP_D_GAIN, %KNEE_D_GAIN,
+ %ABDUCTION_D_GAIN, %HIP_D_GAIN, %KNEE_D_GAIN]
+laikago.Laikago.sensors = %sensors
+
+locomotion_gym_env.LocomotionGymEnv.gym_config = @locomotion_gym_config.LocomotionGymConfig()
+locomotion_gym_env.LocomotionGymEnv.robot_class = @laikago.Laikago
+locomotion_gym_env.LocomotionGymEnv.scene = @simple_scene.SimpleScene()
+locomotion_gym_env.LocomotionGymEnv.task = @simple_locomotion_task.SimpleForwardTask()
+
+
+
+trajectory_generator_wrapper_env.TrajectoryGeneratorWrapperEnv.trajectory_generator = @simple_openloop.LaikagoPoseOffsetGenerator()
+env_loader.load.wrapper_classes = [
+ @observation_dictionary_to_array_wrapper.ObservationDictionaryToArrayWrapper,
+ @trajectory_generator_wrapper_env.TrajectoryGeneratorWrapperEnv]
diff --git a/examples/pybullet/gym/pybullet_data/configs/laikago_walk_static_gait.gin b/examples/pybullet/gym/pybullet_data/configs/laikago_walk_static_gait.gin
new file mode 100644
index 000000000..2b6cd680d
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/configs/laikago_walk_static_gait.gin
@@ -0,0 +1,65 @@
+import pybullet_envs.minitaur.envs_v2.env_loader
+import pybullet_envs.minitaur.envs_v2.env_wrappers.simple_openloop
+import pybullet_envs.minitaur.envs_v2.env_wrappers.trajectory_generator_wrapper_env
+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.simple_scene
+import pybullet_envs.minitaur.envs_v2.sensors.imu_sensor
+import pybullet_envs.minitaur.envs_v2.sensors.motor_angle_sensor
+import pybullet_envs.minitaur.envs_v2.tasks.simple_locomotion_task
+import pybullet_envs.minitaur.robots.hybrid_motor_model
+import pybullet_envs.minitaur.robots.laikago_v2
+import pybullet_envs.minitaur.robots.robot_config
+
+
+# Specify the gym env parameters
+SIM_TIME_STEP = 0.001
+NUM_ACTION_REPEAT = 4
+locomotion_gym_config.SimulationParameters.sim_time_step_s = %SIM_TIME_STEP
+locomotion_gym_config.SimulationParameters.num_action_repeat = %NUM_ACTION_REPEAT
+locomotion_gym_config.SimulationParameters.enable_rendering = False
+locomotion_gym_config.LocomotionGymConfig.simulation_parameters = @locomotion_gym_config.SimulationParameters()
+locomotion_gym_env.LocomotionGymEnv.gym_config = @locomotion_gym_config.LocomotionGymConfig()
+
+# Specify the world.
+URDF_ROOT = ""
+scene_base.SceneBase.data_root = %URDF_ROOT
+locomotion_gym_env.LocomotionGymEnv.scene = @simple_scene.SimpleScene()
+
+# Specify the task.
+locomotion_gym_env.LocomotionGymEnv.task = @simple_locomotion_task.SimpleForwardTask()
+
+# Specify the sensors. Sensors determine the observation space.
+# Sensors can either be mounted on robots (see below), or passed to envs
+# i.e. like ambient sensors, or provided by tasks (task specific measures).
+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]
+
+# We use the default confirugration for MotorAngleSensor, which reads limits from the robot.
+SENSORS = [@imu_sensor.IMUSensor(), @motor_angle_sensor.MotorAngleSensor()]
+
+# Specify the motor limits, and motor control mode.
+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_v2.Laikago.motor_limits = @laikago/robot_config.MotorLimits()
+laikago_v2.Laikago.motor_control_mode = %robot_config.MotorControlMode.POSITION
+laikago_v2.Laikago.motor_model_class = @hybrid_motor_model.HybridMotorModel
+locomotion_gym_env.LocomotionGymEnv.robot_class = @laikago_v2.Laikago
+
+# Specify the motor model parameters. Notice that we don't need to specify the control mode or motor limits here.
+hybrid_motor_model.HybridMotorModel.kp = 2400
+hybrid_motor_model.HybridMotorModel.kd = 5
+
+# Finally, mount sensors specified above to the Laikago.
+laikago_v2.Laikago.sensors = %SENSORS
diff --git a/examples/pybullet/gym/pybullet_data/configs/minitaur_gym_config.gin b/examples/pybullet/gym/pybullet_data/configs/minitaur_gym_config.gin
new file mode 100644
index 000000000..4b268980d
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/configs/minitaur_gym_config.gin
@@ -0,0 +1,48 @@
+#-*-Python-*-
+
+import pybullet_envs.minitaur.envs_v2.locomotion_gym_config
+import pybullet_envs.minitaur.envs_v2.sensors.imu_sensor
+import pybullet_envs.minitaur.envs_v2.sensors.motor_angle_sensor
+import pybullet_envs.minitaur.robots.minitaur_motor_model_v2
+import pybullet_envs.minitaur.robots.minitaur_v2
+import pybullet_envs.minitaur.robots.robot_config
+
+
+UPPER_BOUND = 1.0
+LOWER_BOUND = -1.0
+SIM_TIME_STEP = 0.001
+NUM_ACTION_REPEAT = 20
+NUM_MOTORS = 8
+NOISY_READING = True
+SENSOR_LATENCY = 0.02
+
+locomotion_gym_config.SimulationParameters.sim_time_step_s = %SIM_TIME_STEP
+locomotion_gym_config.SimulationParameters.num_action_repeat = %NUM_ACTION_REPEAT
+locomotion_gym_config.SimulationParameters.enable_rendering = False
+locomotion_gym_config.LocomotionGymConfig.simulation_parameters = @locomotion_gym_config.SimulationParameters()
+
+robot_config.MotorLimits.angle_lower_limits = %LOWER_BOUND
+robot_config.MotorLimits.angle_upper_limits = %UPPER_BOUND
+
+minitaur_v2.Minitaur.motor_control_mode = %robot_config.MotorControlMode.POSITION
+minitaur_v2.Minitaur.motor_limits = @robot_config.MotorLimits()
+minitaur_v2.Minitaur.motor_model_class = @minitaur_motor_model_v2.MinitaurMotorModel
+minitaur_motor_model_v2.MinitaurMotorModel.pd_latency = 0.003
+minitaur_motor_model_v2.MinitaurMotorModel.kp = 1.0
+minitaur_motor_model_v2.MinitaurMotorModel.kd = 0.015
+
+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]
+
+motor_angle_sensor.MotorAngleSensor.sensor_latency = %SENSOR_LATENCY
+motor_angle_sensor.MotorAngleSensor.lower_bound = -6.28318548203
+motor_angle_sensor.MotorAngleSensor.upper_bound = 6.28318548203
+minitaur_v2.Minitaur.sensors = [@imu_sensor.IMUSensor(), @motor_angle_sensor.MotorAngleSensor()]
diff --git a/examples/pybullet/gym/pybullet_data/configs/minitaur_gym_env.gin b/examples/pybullet/gym/pybullet_data/configs/minitaur_gym_env.gin
new file mode 100644
index 000000000..4b5140f0f
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/configs/minitaur_gym_env.gin
@@ -0,0 +1,63 @@
+import pybullet_envs.minitaur.envs_v2.locomotion_gym_env
+import pybullet_envs.minitaur.envs_v2.scenes.simple_scene
+import pybullet_envs.minitaur.envs_v2.tasks.simple_locomotion_task
+import pybullet_envs.minitaur.robots.minitaur_v2
+import pybullet_envs.minitaur.robots.robot_config
+import pybullet_envs.minitaur.robots.robot_urdf_loader
+
+URDF_ROOT = "urdf/"
+#-*-Python-*-
+
+import pybullet_envs.minitaur.envs_v2.locomotion_gym_config
+import pybullet_envs.minitaur.envs_v2.sensors.imu_sensor
+import pybullet_envs.minitaur.envs_v2.sensors.motor_angle_sensor
+import pybullet_envs.minitaur.robots.minitaur_motor_model_v2
+import pybullet_envs.minitaur.robots.minitaur_v2
+import pybullet_envs.minitaur.robots.robot_config
+
+
+UPPER_BOUND = 1.0
+LOWER_BOUND = -1.0
+SIM_TIME_STEP = 0.001
+NUM_ACTION_REPEAT = 20
+NUM_MOTORS = 8
+NOISY_READING = True
+SENSOR_LATENCY = 0.02
+
+locomotion_gym_config.SimulationParameters.sim_time_step_s = %SIM_TIME_STEP
+locomotion_gym_config.SimulationParameters.num_action_repeat = %NUM_ACTION_REPEAT
+locomotion_gym_config.SimulationParameters.enable_rendering = False
+locomotion_gym_config.LocomotionGymConfig.simulation_parameters = @locomotion_gym_config.SimulationParameters()
+
+robot_config.MotorLimits.angle_lower_limits = %LOWER_BOUND
+robot_config.MotorLimits.angle_upper_limits = %UPPER_BOUND
+
+minitaur_v2.Minitaur.motor_control_mode = %robot_config.MotorControlMode.POSITION
+minitaur_v2.Minitaur.motor_limits = @robot_config.MotorLimits()
+minitaur_v2.Minitaur.motor_model_class = @minitaur_motor_model_v2.MinitaurMotorModel
+minitaur_motor_model_v2.MinitaurMotorModel.pd_latency = 0.003
+minitaur_motor_model_v2.MinitaurMotorModel.kp = 1.0
+minitaur_motor_model_v2.MinitaurMotorModel.kd = 0.015
+
+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]
+
+motor_angle_sensor.MotorAngleSensor.sensor_latency = %SENSOR_LATENCY
+motor_angle_sensor.MotorAngleSensor.lower_bound = -6.28318548203
+motor_angle_sensor.MotorAngleSensor.upper_bound = 6.28318548203
+minitaur_v2.Minitaur.sensors = [@imu_sensor.IMUSensor(), @motor_angle_sensor.MotorAngleSensor()]
+
+
+locomotion_gym_env.LocomotionGymEnv.gym_config = @locomotion_gym_config.LocomotionGymConfig()
+locomotion_gym_env.LocomotionGymEnv.robot_class = @minitaur_v2.Minitaur
+locomotion_gym_env.LocomotionGymEnv.scene = @simple_scene.SimpleScene()
+locomotion_gym_env.LocomotionGymEnv.task = @simple_locomotion_task.SimpleForwardTask()
+
diff --git a/examples/pybullet/gym/pybullet_data/configs_v2/base/laikago_reactive.gin b/examples/pybullet/gym/pybullet_data/configs_v2/base/laikago_reactive.gin
new file mode 100644
index 000000000..030bf4d8f
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/configs_v2/base/laikago_reactive.gin
@@ -0,0 +1,80 @@
+import pybullet_envs.minitaur.envs_v2.env_loader
+import pybullet_envs.minitaur.envs_v2.env_wrappers.observation_dictionary_to_array_wrapper
+import pybullet_envs.minitaur.envs_v2.env_wrappers.simple_openloop
+import pybullet_envs.minitaur.envs_v2.env_wrappers.trajectory_generator_wrapper_env
+import pybullet_envs.minitaur.envs_v2.locomotion_gym_config
+import pybullet_envs.minitaur.envs_v2.locomotion_gym_env
+import pybullet_envs.minitaur.envs_v2.locomotion_gym_env
+import pybullet_envs.minitaur.envs_v2.scenes.simple_scene
+import pybullet_envs.minitaur.envs_v2.sensors.accelerometer_sensor
+import pybullet_envs.minitaur.envs_v2.sensors.imu_sensor
+import pybullet_envs.minitaur.envs_v2.sensors.motor_angle_sensor
+import pybullet_envs.minitaur.envs_v2.tasks.simple_locomotion_task
+import pybullet_envs.minitaur.envs_v2.tasks.terminal_conditions
+import pybullet_envs.minitaur.robots.hybrid_motor_model
+import pybullet_envs.minitaur.robots.laikago_v2
+import pybullet_envs.minitaur.robots.time_ordered_buffer
+
+
+# Specify the gym env parameters
+SIM_TIME_STEP = 0.001
+NUM_ACTION_REPEAT = 2
+
+locomotion_gym_config.SimulationParameters.sim_time_step_s = %SIM_TIME_STEP
+locomotion_gym_config.SimulationParameters.num_action_repeat = %NUM_ACTION_REPEAT
+locomotion_gym_config.SimulationParameters.enable_rendering = False
+locomotion_gym_config.LocomotionGymConfig.simulation_parameters = @locomotion_gym_config.SimulationParameters()
+locomotion_gym_env.LocomotionGymEnv.gym_config = @locomotion_gym_config.LocomotionGymConfig()
+
+
+# Specify the robot
+locomotion_gym_env.LocomotionGymEnv.robot_class = @laikago_v2.Laikago
+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_v2.Laikago.motor_limits = @laikago/robot_config.MotorLimits()
+laikago_v2.Laikago.motor_control_mode = %robot_config.MotorControlMode.POSITION
+
+# Specify the motor model parameters. Notice that we don't need to specify the control mode
+# and motor limits here, as they will be passed from the robot interface.
+hybrid_motor_model.HybridMotorModel.kp = 220
+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)
+
+# This will make sure the hybrid motor model does not throw error during reset, when the timestamp is alwasy zero.
+time_ordered_buffer.TimeOrderedBuffer.error_on_duplicate_timestamp = False
+time_ordered_buffer.TimeOrderedBuffer.replace_value_on_duplicate_timestamp = True
+
+# Add the sensors
+laikago_v2.Laikago.sensors = [@imu_sensor.IMUSensor(), @motor_angle_sensor.MotorAngleSensor(), @accelerometer_sensor.AccelerometerSensor()]
+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]
+
+# Specify the scene
+locomotion_gym_env.LocomotionGymEnv.scene = @simple_scene.SimpleScene()
+simple_scene.SimpleScene.data_root = "third_party/bullet/data"
+
+# Specify the task.
+locomotion_gym_env.LocomotionGymEnv.task = @simple_locomotion_task.SimpleForwardTask()
+simple_locomotion_task.SimpleForwardTask.terminal_condition = @terminal_conditions.default_terminal_condition_for_laikago
+
+accelerometer_sensor.AccelerometerSensor.lower_bound = [-50.0, -50.0, -50.0]
+accelerometer_sensor.AccelerometerSensor.upper_bound = [50.0, 50.0, 50.0]
+accelerometer_sensor.AccelerometerSensor.sensor_latency = 0.01
+imu_sensor.IMUSensor.sensor_latency = 0.1
+
+
+# Define the wrappers needed
+env_loader.load.wrapper_classes = [
+ @trajectory_generator_wrapper_env.TrajectoryGeneratorWrapperEnv,
+]
+trajectory_generator_wrapper_env.TrajectoryGeneratorWrapperEnv.trajectory_generator = @simple_openloop.LaikagoPoseOffsetGenerator()
diff --git a/examples/pybullet/gym/pybullet_data/configs_v2/base/laikago_with_imu.gin b/examples/pybullet/gym/pybullet_data/configs_v2/base/laikago_with_imu.gin
new file mode 100644
index 000000000..03d7c3308
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/configs_v2/base/laikago_with_imu.gin
@@ -0,0 +1,66 @@
+#import pybullet_data
+#MYPATH = pybullet_data.getDataPath()+'/configs_v2/robots/laikago.gin'
+#MYPATH = 'D:/dev/bullet3/examples/pybullet\gym/pybullet_data/configs_v2/robots/laikago.gin'
+#include '$MYPATH/ambiguous.gin'
+
+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.scene_base
+import pybullet_envs.minitaur.envs_v2.scenes.simple_scene
+import pybullet_envs.minitaur.envs_v2.sensors.imu_sensor
+import pybullet_envs.minitaur.envs_v2.sensors.motor_angle_sensor
+import pybullet_envs.minitaur.robots.hybrid_motor_model
+import pybullet_envs.minitaur.robots.laikago_v2
+import pybullet_envs.minitaur.robots.robot_config
+
+URDF_ROOT = ""
+UPPER_BOUND = 6.28318548203
+LOWER_BOUND = -6.28318548203
+SIM_TIME_STEP = 0.001
+NUM_ACTION_REPEAT = 2
+
+locomotion_gym_config.SimulationParameters.sim_time_step_s = %SIM_TIME_STEP
+locomotion_gym_config.SimulationParameters.num_action_repeat = %NUM_ACTION_REPEAT
+locomotion_gym_config.SimulationParameters.enable_rendering = False
+locomotion_gym_config.LocomotionGymConfig.simulation_parameters = @locomotion_gym_config.SimulationParameters()
+
+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]
+
+# We use the default confirugration for MotorAngleSensor, which reads limits from the robot.
+SENSORS = [@imu_sensor.IMUSensor(), @motor_angle_sensor.MotorAngleSensor()]
+locomotion_gym_config.SimulationParameters.sim_time_step_s = %SIM_TIME_STEP
+locomotion_gym_config.SimulationParameters.num_action_repeat = %NUM_ACTION_REPEAT
+locomotion_gym_config.SimulationParameters.enable_rendering = False
+locomotion_gym_config.LocomotionGymConfig.simulation_parameters = @locomotion_gym_config.SimulationParameters()
+locomotion_gym_env.LocomotionGymEnv.gym_config = @locomotion_gym_config.LocomotionGymConfig()
+
+# Specify the scene.
+scene_base.SceneBase.data_root = %URDF_ROOT
+locomotion_gym_env.LocomotionGymEnv.scene = @simple_scene.SimpleScene()
+
+# Specify the motor limits, and motor control mode.
+laikago/robot_config.MotorLimits.angle_lower_limits = %LOWER_BOUND
+laikago/robot_config.MotorLimits.angle_upper_limits = %UPPER_BOUND
+laikago_v2.Laikago.motor_limits = @laikago/robot_config.MotorLimits()
+laikago_v2.Laikago.motor_control_mode = %robot_config.MotorControlMode.POSITION
+laikago_v2.Laikago.motor_model_class = @hybrid_motor_model.HybridMotorModel
+locomotion_gym_env.LocomotionGymEnv.robot_class = @laikago_v2.Laikago
+
+# Specify the motor model parameters. Notice that we don't need to specify the control mode or motor limits here.
+hybrid_motor_model.HybridMotorModel.kp = 220
+hybrid_motor_model.HybridMotorModel.kd = 2
+
+laikago_v2.Laikago.sensors = %SENSORS
+
+
+laikago_v2.Laikago.sensors = [@imu_sensor.IMUSensor()]
diff --git a/examples/pybullet/gym/pybullet_data/configs_v2/base/mini_cheetah_with_imu.gin b/examples/pybullet/gym/pybullet_data/configs_v2/base/mini_cheetah_with_imu.gin
new file mode 100644
index 000000000..d7ebe63f7
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/configs_v2/base/mini_cheetah_with_imu.gin
@@ -0,0 +1,9 @@
+import pybullet_data as pd
+
+include pd.getDataPath()+'/configs_v2/robots/mini_cheetah.gin'
+include pd.getDataPath()+'/configs_v2/sensors/imu.gin'
+
+#include 'robotics/reinforcement_learning/minitaur/envs_v2/configs_v2/robots/mini_cheetah.gin'
+#include 'robotics/reinforcement_learning/minitaur/envs_v2/configs_v2/sensors/imu.gin'
+
+mini_cheetah.MiniCheetah.sensors = [@robot_sensors.IMUSensor()]
diff --git a/examples/pybullet/gym/pybullet_data/configs_v2/robots/laikago.gin b/examples/pybullet/gym/pybullet_data/configs_v2/robots/laikago.gin
new file mode 100644
index 000000000..a639c83c9
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/configs_v2/robots/laikago.gin
@@ -0,0 +1,58 @@
+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.scene_base
+import pybullet_envs.minitaur.envs_v2.scenes.simple_scene
+import pybullet_envs.minitaur.envs_v2.sensors.imu_sensor
+import pybullet_envs.minitaur.envs_v2.sensors.motor_angle_sensor
+import pybullet_envs.minitaur.robots.hybrid_motor_model
+import pybullet_envs.minitaur.robots.laikago_v2
+import pybullet_envs.minitaur.robots.robot_config
+
+URDF_ROOT = ""
+UPPER_BOUND = 6.28318548203
+LOWER_BOUND = -6.28318548203
+SIM_TIME_STEP = 0.001
+NUM_ACTION_REPEAT = 2
+
+locomotion_gym_config.SimulationParameters.sim_time_step_s = %SIM_TIME_STEP
+locomotion_gym_config.SimulationParameters.num_action_repeat = %NUM_ACTION_REPEAT
+locomotion_gym_config.SimulationParameters.enable_rendering = False
+locomotion_gym_config.LocomotionGymConfig.simulation_parameters = @locomotion_gym_config.SimulationParameters()
+
+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]
+
+# We use the default confirugration for MotorAngleSensor, which reads limits from the robot.
+SENSORS = [@imu_sensor.IMUSensor(), @motor_angle_sensor.MotorAngleSensor()]
+locomotion_gym_config.SimulationParameters.sim_time_step_s = %SIM_TIME_STEP
+locomotion_gym_config.SimulationParameters.num_action_repeat = %NUM_ACTION_REPEAT
+locomotion_gym_config.SimulationParameters.enable_rendering = False
+locomotion_gym_config.LocomotionGymConfig.simulation_parameters = @locomotion_gym_config.SimulationParameters()
+locomotion_gym_env.LocomotionGymEnv.gym_config = @locomotion_gym_config.LocomotionGymConfig()
+
+# Specify the scene.
+scene_base.SceneBase.data_root = %URDF_ROOT
+locomotion_gym_env.LocomotionGymEnv.scene = @simple_scene.SimpleScene()
+
+# Specify the motor limits, and motor control mode.
+laikago/robot_config.MotorLimits.angle_lower_limits = %LOWER_BOUND
+laikago/robot_config.MotorLimits.angle_upper_limits = %UPPER_BOUND
+laikago_v2.Laikago.motor_limits = @laikago/robot_config.MotorLimits()
+laikago_v2.Laikago.motor_control_mode = %robot_config.MotorControlMode.POSITION
+laikago_v2.Laikago.motor_model_class = @hybrid_motor_model.HybridMotorModel
+locomotion_gym_env.LocomotionGymEnv.robot_class = @laikago_v2.Laikago
+
+# Specify the motor model parameters. Notice that we don't need to specify the control mode or motor limits here.
+hybrid_motor_model.HybridMotorModel.kp = 220
+hybrid_motor_model.HybridMotorModel.kd = 2
+
+laikago_v2.Laikago.sensors = %SENSORS
diff --git a/examples/pybullet/gym/pybullet_data/configs_v2/robots/mini_cheetah.gin b/examples/pybullet/gym/pybullet_data/configs_v2/robots/mini_cheetah.gin
new file mode 100644
index 000000000..7433da2f8
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/configs_v2/robots/mini_cheetah.gin
@@ -0,0 +1,98 @@
+import pybullet_envs.minitaur.envs_v2.locomotion_gym_config
+import pybullet_envs.minitaur.envs_v2.locomotion_gym_env
+import pybullet_envs.minitaur.robots.mini_cheetah
+import pybullet_data as pd
+
+URDF_ROOT = pd.getDataPath()+"/urdf/"
+
+ABDUCTION_P_GAIN = 100.0
+ABDUCTION_D_GAIN = 1.0
+HIP_P_GAIN = 30
+HIP_D_GAIN = 2.0
+KNEE_P_GAIN = 50
+KNEE_D_GAIN = 2.0
+
+
+UPPER_BOUND = 6.28318548203
+LOWER_BOUND = -6.28318548203
+SIM_TIME_STEP = 0.001
+NUM_ACTION_REPEAT = 2
+NUM_MOTORS = 12
+NOISY_READING = True
+
+locomotion_gym_config.SimulationParameters.sim_time_step_s = %SIM_TIME_STEP
+locomotion_gym_config.SimulationParameters.num_action_repeat = %NUM_ACTION_REPEAT
+locomotion_gym_config.SimulationParameters.enable_rendering = False
+locomotion_gym_config.LocomotionGymConfig.simulation_parameters = @locomotion_gym_config.SimulationParameters()
+
+Act0/locomotion_gym_config.ScalarField.name = "motor_angle_0"
+Act0/locomotion_gym_config.ScalarField.upper_bound = %UPPER_BOUND
+Act0/locomotion_gym_config.ScalarField.lower_bound = %LOWER_BOUND
+Act1/locomotion_gym_config.ScalarField.name = "motor_angle_1"
+Act1/locomotion_gym_config.ScalarField.upper_bound = %UPPER_BOUND
+Act1/locomotion_gym_config.ScalarField.lower_bound = %LOWER_BOUND
+Act2/locomotion_gym_config.ScalarField.name = "motor_angle_2"
+Act2/locomotion_gym_config.ScalarField.upper_bound = %UPPER_BOUND
+Act2/locomotion_gym_config.ScalarField.lower_bound = %LOWER_BOUND
+Act3/locomotion_gym_config.ScalarField.name = "motor_angle_3"
+Act3/locomotion_gym_config.ScalarField.upper_bound = %UPPER_BOUND
+Act3/locomotion_gym_config.ScalarField.lower_bound = %LOWER_BOUND
+Act4/locomotion_gym_config.ScalarField.name = "motor_angle_4"
+Act4/locomotion_gym_config.ScalarField.upper_bound = %UPPER_BOUND
+Act4/locomotion_gym_config.ScalarField.lower_bound = %LOWER_BOUND
+Act5/locomotion_gym_config.ScalarField.name = "motor_angle_5"
+Act5/locomotion_gym_config.ScalarField.upper_bound = %UPPER_BOUND
+Act5/locomotion_gym_config.ScalarField.lower_bound = %LOWER_BOUND
+Act6/locomotion_gym_config.ScalarField.name = "motor_angle_6"
+Act6/locomotion_gym_config.ScalarField.upper_bound = %UPPER_BOUND
+Act6/locomotion_gym_config.ScalarField.lower_bound = %LOWER_BOUND
+Act7/locomotion_gym_config.ScalarField.name = "motor_angle_7"
+Act7/locomotion_gym_config.ScalarField.upper_bound = %UPPER_BOUND
+Act7/locomotion_gym_config.ScalarField.lower_bound = %LOWER_BOUND
+Act8/locomotion_gym_config.ScalarField.name = "motor_angle_8"
+Act8/locomotion_gym_config.ScalarField.upper_bound = %UPPER_BOUND
+Act8/locomotion_gym_config.ScalarField.lower_bound = %LOWER_BOUND
+Act9/locomotion_gym_config.ScalarField.name = "motor_angle_9"
+Act9/locomotion_gym_config.ScalarField.upper_bound = %UPPER_BOUND
+Act9/locomotion_gym_config.ScalarField.lower_bound = %LOWER_BOUND
+Act10/locomotion_gym_config.ScalarField.name = "motor_angle_10"
+Act10/locomotion_gym_config.ScalarField.upper_bound = %UPPER_BOUND
+Act10/locomotion_gym_config.ScalarField.lower_bound = %LOWER_BOUND
+Act11/locomotion_gym_config.ScalarField.name = "motor_angle_11"
+Act11/locomotion_gym_config.ScalarField.upper_bound = %UPPER_BOUND
+Act11/locomotion_gym_config.ScalarField.lower_bound = %LOWER_BOUND
+
+
+locomotion_gym_config.LocomotionGymConfig.actions = [
+ @Act0/locomotion_gym_config.ScalarField(),
+ @Act1/locomotion_gym_config.ScalarField(),
+ @Act2/locomotion_gym_config.ScalarField(),
+ @Act3/locomotion_gym_config.ScalarField(),
+ @Act4/locomotion_gym_config.ScalarField(),
+ @Act5/locomotion_gym_config.ScalarField(),
+ @Act6/locomotion_gym_config.ScalarField(),
+ @Act7/locomotion_gym_config.ScalarField(),
+ @Act8/locomotion_gym_config.ScalarField(),
+ @Act9/locomotion_gym_config.ScalarField(),
+ @Act10/locomotion_gym_config.ScalarField(),
+ @Act11/locomotion_gym_config.ScalarField()]
+
+locomotion_gym_env.LocomotionGymEnv.gym_config = @locomotion_gym_config.LocomotionGymConfig()
+
+mini_cheetah.MiniCheetah.urdf_root = %URDF_ROOT
+mini_cheetah.MiniCheetah.time_step = %SIM_TIME_STEP
+mini_cheetah.MiniCheetah.action_repeat = %NUM_ACTION_REPEAT
+mini_cheetah.MiniCheetah.self_collision_enabled = False
+mini_cheetah.MiniCheetah.control_latency = 0.002
+mini_cheetah.MiniCheetah.pd_latency = 0.0
+mini_cheetah.MiniCheetah.motor_kp = [%ABDUCTION_P_GAIN, %HIP_P_GAIN, %KNEE_P_GAIN,
+ %ABDUCTION_P_GAIN, %HIP_P_GAIN, %KNEE_P_GAIN,
+ %ABDUCTION_P_GAIN, %HIP_P_GAIN, %KNEE_P_GAIN,
+ %ABDUCTION_P_GAIN, %HIP_P_GAIN, %KNEE_P_GAIN]
+mini_cheetah.MiniCheetah.motor_kd = [%ABDUCTION_D_GAIN, %HIP_D_GAIN, %KNEE_D_GAIN,
+ %ABDUCTION_D_GAIN, %HIP_D_GAIN, %KNEE_D_GAIN,
+ %ABDUCTION_D_GAIN, %HIP_D_GAIN, %KNEE_D_GAIN,
+ %ABDUCTION_D_GAIN, %HIP_D_GAIN, %KNEE_D_GAIN]
+
+locomotion_gym_env.LocomotionGymEnv.robot_class = @mini_cheetah.MiniCheetah
+
diff --git a/examples/pybullet/gym/pybullet_data/configs_v2/robots/minitaur.gin b/examples/pybullet/gym/pybullet_data/configs_v2/robots/minitaur.gin
new file mode 100644
index 000000000..091483380
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/configs_v2/robots/minitaur.gin
@@ -0,0 +1,31 @@
+import pybullet_envs.minitaur.envs_v2.locomotion_gym_config
+import pybullet_envs.minitaur.envs_v2.locomotion_gym_env
+import pybullet_envs.minitaur.robots.minitaur_motor_model_v2
+import pybullet_envs.minitaur.robots.minitaur_v2
+
+URDF_ROOT = "robotics/reinforcement_learning/minitaur/robots/data/urdf/"
+
+UPPER_BOUND = 6.28318548203
+LOWER_BOUND = -6.28318548203
+SIM_TIME_STEP = 0.001
+NUM_ACTION_REPEAT = 6
+NUM_MOTORS = 8
+NOISY_READING = True
+
+locomotion_gym_config.SimulationParameters.sim_time_step_s = %SIM_TIME_STEP
+locomotion_gym_config.SimulationParameters.num_action_repeat = %NUM_ACTION_REPEAT
+locomotion_gym_config.SimulationParameters.enable_rendering = False
+locomotion_gym_config.LocomotionGymConfig.simulation_parameters = @locomotion_gym_config.SimulationParameters()
+locomotion_gym_env.LocomotionGymEnv.gym_config = @locomotion_gym_config.LocomotionGymConfig()
+
+minitaur_v2.Minitaur.motor_control_mode = %robot_config.MotorControlMode.POSITION
+minitaur_v2.Minitaur.motor_limits = @robot_config.MotorLimits()
+minitaur_v2.Minitaur.motor_model_class = @minitaur_motor_model_v2.MinitaurMotorModel
+minitaur_motor_model_v2.MinitaurMotorModel.pd_latency = 0.003
+minitaur_motor_model_v2.MinitaurMotorModel.kp = 1.0
+minitaur_motor_model_v2.MinitaurMotorModel.kd = 0.015
+
+locomotion_gym_env.LocomotionGymEnv.robot_class = @minitaur_v2.Minitaur
+
+robot_config.MotorLimits.angle_lower_limits = %LOWER_BOUND
+robot_config.MotorLimits.angle_upper_limits = %UPPER_BOUND
diff --git a/examples/pybullet/gym/pybullet_data/configs_v2/scenes/simple_scene.gin b/examples/pybullet/gym/pybullet_data/configs_v2/scenes/simple_scene.gin
new file mode 100644
index 000000000..6cb12467a
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/configs_v2/scenes/simple_scene.gin
@@ -0,0 +1,4 @@
+import pybullet_envs.minitaur.envs_v2.locomotion_gym_env
+import pybullet_envs.minitaur.envs_v2.scenes.simple_scene
+
+locomotion_gym_env.LocomotionGymEnv.scene = @simple_scene.SimpleScene()
diff --git a/examples/pybullet/gym/pybullet_data/configs_v2/scenes/stair_scene.gin b/examples/pybullet/gym/pybullet_data/configs_v2/scenes/stair_scene.gin
new file mode 100644
index 000000000..134f5774f
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/configs_v2/scenes/stair_scene.gin
@@ -0,0 +1,3 @@
+import pybullet_envs.minitaur.envs_v2.scenes.stair_scene
+# Specify the scene (overwrite the setting from laikago_reactive.gin)
+locomotion_gym_env.LocomotionGymEnv.scene = @stair_scene.StairScene()
diff --git a/examples/pybullet/gym/pybullet_data/configs_v2/tasks/fwd_task.gin b/examples/pybullet/gym/pybullet_data/configs_v2/tasks/fwd_task.gin
new file mode 100644
index 000000000..316431710
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/configs_v2/tasks/fwd_task.gin
@@ -0,0 +1,4 @@
+import pybullet_envs.minitaur.envs_v2.locomotion_gym_env
+import pybullet_envs.minitaur.envs_v2.tasks.simple_locomotion_task
+
+locomotion_gym_env.LocomotionGymEnv.task = @simple_locomotion_task.SimpleForwardTask()
diff --git a/examples/pybullet/gym/pybullet_data/configs_v2/tasks/fwd_task_laikago.gin b/examples/pybullet/gym/pybullet_data/configs_v2/tasks/fwd_task_laikago.gin
new file mode 100644
index 000000000..59a50e4e7
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/configs_v2/tasks/fwd_task_laikago.gin
@@ -0,0 +1,8 @@
+import pybullet_envs.minitaur.envs_v2.locomotion_gym_env
+import pybullet_envs.minitaur.envs_v2.tasks.simple_locomotion_task
+
+locomotion_gym_env.LocomotionGymEnv.task = @simple_locomotion_task.SimpleForwardTask()
+
+simple_locomotion_task.SimpleForwardTask.energy_penalty_coef = 0.002
+simple_locomotion_task.SimpleForwardTask.min_com_height = 0.3
+simple_locomotion_task.SimpleForwardTask.clip_velocity = 0.002
diff --git a/examples/pybullet/gym/pybullet_data/configs_v2/tasks/fwd_task_no_termination.gin b/examples/pybullet/gym/pybullet_data/configs_v2/tasks/fwd_task_no_termination.gin
new file mode 100644
index 000000000..5fc8189ca
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/configs_v2/tasks/fwd_task_no_termination.gin
@@ -0,0 +1,11 @@
+import pybullet_envs.minitaur.envs_v2.locomotion_gym_env
+import pybullet_envs.minitaur.envs_v2.tasks.simple_locomotion_task
+import pybullet_envs.minitaur.envs_v2.tasks.terminal_conditions
+
+terminal_conditions.maxstep_terminal_condition.max_step = 1500
+# Setup the terminal condition to not to terminate early when the robot falls.
+terminal_conditions.logical_any_terminal_condition.conditions = [
+ @terminal_conditions.maxstep_terminal_condition,
+ ]
+simple_locomotion_task.SimpleForwardTask.terminal_condition = @terminal_conditions.logical_any_terminal_condition
+locomotion_gym_env.LocomotionGymEnv.task = @simple_locomotion_task.SimpleForwardTask()
diff --git a/examples/pybullet/gym/pybullet_data/configs_v2/tasks/fwd_task_no_termination_simplified.gin b/examples/pybullet/gym/pybullet_data/configs_v2/tasks/fwd_task_no_termination_simplified.gin
new file mode 100644
index 000000000..c7d9630d5
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/configs_v2/tasks/fwd_task_no_termination_simplified.gin
@@ -0,0 +1,12 @@
+import pybullet_envs.minitaur.envs_v2.locomotion_gym_env
+import pybullet_envs.minitaur.envs_v2.tasks.simple_locomotion_task
+import pybullet_envs.minitaur.envs_v2.tasks.terminal_conditions
+
+terminal_conditions.maxstep_terminal_condition.max_step = 1500
+simple_locomotion_task.SimpleForwardTask.terminal_condition = @terminal_conditions.default_terminal_condition_for_laikago_v2
+
+simple_locomotion_task.SimpleForwardTask.divide_with_dt = True
+simple_locomotion_task.SimpleForwardTask.clip_velocity = 0.4
+simple_locomotion_task.SimpleForwardTask.energy_penalty_coef = 0
+
+locomotion_gym_env.LocomotionGymEnv.task = @simple_locomotion_task.SimpleForwardTask()
diff --git a/examples/pybullet/gym/pybullet_data/configs_v2/wrappers/pmtg_wrapper.gin b/examples/pybullet/gym/pybullet_data/configs_v2/wrappers/pmtg_wrapper.gin
new file mode 100644
index 000000000..f9466a316
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/configs_v2/wrappers/pmtg_wrapper.gin
@@ -0,0 +1,18 @@
+import pybullet_envs.minitaur.envs_v2.env_loader
+import pybullet_envs.minitaur.envs_v2.env_wrappers.observation_dictionary_to_array_wrapper
+import pybullet_envs.minitaur.envs_v2.env_wrappers.pmtg_wrapper_env
+
+pmtg_wrapper_env.PmtgWrapperEnv.action_filter_enable = True
+pmtg_wrapper_env.PmtgWrapperEnv.action_filter_enable = 1
+pmtg_wrapper_env.PmtgWrapperEnv.intensity_upper_bound = 1.0
+pmtg_wrapper_env.PmtgWrapperEnv.max_delta_time = 4.0
+pmtg_wrapper_env.PmtgWrapperEnv.min_delta_time = 2.0
+pmtg_wrapper_env.PmtgWrapperEnv.residual_range = 0.35
+pmtg_wrapper_env.PmtgWrapperEnv.integrator_coupling_mode = "all coupled"
+pmtg_wrapper_env.PmtgWrapperEnv.walk_height_coupling_mode = "all coupled"
+pmtg_wrapper_env.PmtgWrapperEnv.variable_swing_stance_ratio = 1
+pmtg_wrapper_env.PmtgWrapperEnv.init_gait = "walk"
+
+env_loader.load.wrapper_classes = [
+ @pmtg_wrapper_env.PmtgWrapperEnv,
+ @observation_dictionary_to_array_wrapper.ObservationDictionaryToArrayWrapper]
diff --git a/examples/pybullet/gym/pybullet_data/configs_v2/wrappers/pmtg_wrapper_dict.gin b/examples/pybullet/gym/pybullet_data/configs_v2/wrappers/pmtg_wrapper_dict.gin
new file mode 100644
index 000000000..74251e31c
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/configs_v2/wrappers/pmtg_wrapper_dict.gin
@@ -0,0 +1,6 @@
+import pybullet_envs.minitaur.envs_v2.env_loader
+import pybullet_envs.minitaur.envs_v2.env_wrappers.pmtg_wrapper_env
+
+pmtg_wrapper_env.PmtgWrapperEnv.action_filter_enable = True
+
+env_loader.load.wrapper_classes = [@pmtg_wrapper_env.PmtgWrapperEnv]
diff --git a/examples/pybullet/gym/pybullet_data/configs_v2/wrappers/pmtg_wrapper_laikago.gin b/examples/pybullet/gym/pybullet_data/configs_v2/wrappers/pmtg_wrapper_laikago.gin
new file mode 100644
index 000000000..8f46b1591
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/configs_v2/wrappers/pmtg_wrapper_laikago.gin
@@ -0,0 +1,10 @@
+import pybullet_envs.minitaur.envs_v2.env_loader
+import pybullet_envs.minitaur.envs_v2.env_wrappers.observation_dictionary_to_array_wrapper
+import pybullet_envs.minitaur.envs_v2.env_wrappers.pmtg_wrapper_env
+
+pmtg_wrapper_env.PmtgWrapperEnv.action_filter_enable = True
+pmtg_wrapper_env.PmtgWrapperEnv.action_filter_high_cut = 0.5
+
+env_loader.load.wrapper_classes = [
+ @pmtg_wrapper_env.PmtgWrapperEnv,
+ @observation_dictionary_to_array_wrapper.ObservationDictionaryToArrayWrapper]
diff --git a/examples/pybullet/gym/pybullet_data/configs_v2/wrappers/pmtg_wrapper_simplified_env.gin b/examples/pybullet/gym/pybullet_data/configs_v2/wrappers/pmtg_wrapper_simplified_env.gin
new file mode 100644
index 000000000..f6736cb09
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/configs_v2/wrappers/pmtg_wrapper_simplified_env.gin
@@ -0,0 +1,18 @@
+import pybullet_envs.minitaur.envs_v2.env_loader
+import pybullet_envs.minitaur.envs_v2.env_wrappers.observation_dictionary_to_array_wrapper
+import pybullet_envs.minitaur.envs_v2.env_wrappers.pmtg_wrapper_env
+
+
+pmtg_wrapper_env.PmtgWrapperEnv.init_leg_phase_offsets = [0, 0.5, 0.5, 0]
+pmtg_wrapper_env.PmtgWrapperEnv.integrator_coupling_mode = 'all coupled'
+pmtg_wrapper_env.PmtgWrapperEnv.intensity_upper_bound = 0.5
+pmtg_wrapper_env.PmtgWrapperEnv.max_delta_time = 4
+pmtg_wrapper_env.PmtgWrapperEnv.min_delta_time = 1
+pmtg_wrapper_env.PmtgWrapperEnv.residual_range = 0.2
+pmtg_wrapper_env.PmtgWrapperEnv.variable_swing_stance_ratio = False
+pmtg_wrapper_env.PmtgWrapperEnv.walk_height_coupling_mode = 'null'
+pmtg_wrapper_env.PmtgWrapperEnv.action_filter_high_cut = 0.5
+
+env_loader.load.wrapper_classes = [
+ @pmtg_wrapper_env.PmtgWrapperEnv,
+ @observation_dictionary_to_array_wrapper.ObservationDictionaryToArrayWrapper]
diff --git a/examples/pybullet/gym/pybullet_data/testdata/__init__.py b/examples/pybullet/gym/pybullet_data/testdata/__init__.py
new file mode 100644
index 000000000..e69de29bb
diff --git a/examples/pybullet/gym/pybullet_data/testdata/test_imu_state_estimator/__init__.py b/examples/pybullet/gym/pybullet_data/testdata/test_imu_state_estimator/__init__.py
new file mode 100644
index 000000000..e69de29bb
diff --git a/examples/pybullet/gym/pybullet_data/testdata/test_imu_state_estimator/accelerometer.npy b/examples/pybullet/gym/pybullet_data/testdata/test_imu_state_estimator/accelerometer.npy
new file mode 100644
index 000000000..5201d49f1
Binary files /dev/null and b/examples/pybullet/gym/pybullet_data/testdata/test_imu_state_estimator/accelerometer.npy differ
diff --git a/examples/pybullet/gym/pybullet_data/testdata/test_imu_state_estimator/estimated_velocities.npy b/examples/pybullet/gym/pybullet_data/testdata/test_imu_state_estimator/estimated_velocities.npy
new file mode 100644
index 000000000..cb988fe3d
Binary files /dev/null and b/examples/pybullet/gym/pybullet_data/testdata/test_imu_state_estimator/estimated_velocities.npy differ
diff --git a/examples/pybullet/gym/pybullet_data/testdata/test_imu_state_estimator/feet_contact_forces.npy b/examples/pybullet/gym/pybullet_data/testdata/test_imu_state_estimator/feet_contact_forces.npy
new file mode 100644
index 000000000..b235bdc36
Binary files /dev/null and b/examples/pybullet/gym/pybullet_data/testdata/test_imu_state_estimator/feet_contact_forces.npy differ
diff --git a/examples/pybullet/gym/pybullet_data/testdata/test_imu_state_estimator/gyroscope.npy b/examples/pybullet/gym/pybullet_data/testdata/test_imu_state_estimator/gyroscope.npy
new file mode 100644
index 000000000..a446a2ba7
Binary files /dev/null and b/examples/pybullet/gym/pybullet_data/testdata/test_imu_state_estimator/gyroscope.npy differ
diff --git a/examples/pybullet/gym/pybullet_data/testdata/test_imu_state_estimator/jacobians.npy b/examples/pybullet/gym/pybullet_data/testdata/test_imu_state_estimator/jacobians.npy
new file mode 100644
index 000000000..5f7bd7a14
Binary files /dev/null and b/examples/pybullet/gym/pybullet_data/testdata/test_imu_state_estimator/jacobians.npy differ
diff --git a/examples/pybullet/gym/pybullet_data/testdata/test_imu_state_estimator/motor_velocities.npy b/examples/pybullet/gym/pybullet_data/testdata/test_imu_state_estimator/motor_velocities.npy
new file mode 100644
index 000000000..1cb98d24a
Binary files /dev/null and b/examples/pybullet/gym/pybullet_data/testdata/test_imu_state_estimator/motor_velocities.npy differ
diff --git a/examples/pybullet/gym/pybullet_data/testdata/test_imu_state_estimator/timestamp.npy b/examples/pybullet/gym/pybullet_data/testdata/test_imu_state_estimator/timestamp.npy
new file mode 100644
index 000000000..79384090a
Binary files /dev/null and b/examples/pybullet/gym/pybullet_data/testdata/test_imu_state_estimator/timestamp.npy differ
diff --git a/examples/pybullet/gym/pybullet_data/urdf/mug.obj b/examples/pybullet/gym/pybullet_data/urdf/mug.obj
new file mode 100644
index 000000000..4fb8d9186
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/urdf/mug.obj
@@ -0,0 +1,1352 @@
+# Blender v2.79 (sub 0) OBJ File: 'mug.blend'
+# www.blender.org
+o mug_Cylinder.002
+v 0.000000 0.000000 0.000000
+v 0.000000 0.032649 0.008628
+v 0.000000 -0.000000 0.008628
+v 0.008450 0.031536 0.008628
+v 0.016324 0.028275 0.008628
+v 0.023086 0.023086 0.008628
+v 0.028275 0.016324 0.008628
+v 0.031536 0.008450 0.008628
+v 0.032649 0.000000 0.008628
+v 0.031536 -0.008450 0.008628
+v 0.028275 -0.016324 0.008628
+v 0.023086 -0.023086 0.008628
+v 0.016324 -0.028275 0.008628
+v 0.008450 -0.031536 0.008628
+v 0.000000 -0.032649 0.008628
+v -0.008450 -0.031536 0.008628
+v -0.016324 -0.028275 0.008628
+v -0.023086 -0.023086 0.008628
+v -0.028275 -0.016324 0.008628
+v -0.031536 -0.008450 0.008628
+v -0.032649 -0.000000 0.008628
+v -0.031536 0.008450 0.008628
+v -0.028275 0.016324 0.008628
+v -0.023086 0.023086 0.008628
+v -0.016324 0.028275 0.008628
+v -0.008450 0.031536 0.008628
+v 0.000000 0.041000 0.097171
+v 0.000000 0.038146 0.100000
+v 0.000000 0.040164 0.099171
+v 0.009873 0.036846 0.100000
+v 0.010612 0.039603 0.097171
+v 0.010395 0.038796 0.099171
+v 0.019073 0.033036 0.100000
+v 0.020500 0.035507 0.097171
+v 0.020082 0.034783 0.099171
+v 0.026973 0.026973 0.100000
+v 0.028991 0.028991 0.097171
+v 0.028400 0.028400 0.099171
+v 0.033036 0.019073 0.100000
+v 0.035507 0.020500 0.097171
+v 0.034783 0.020082 0.099171
+v 0.036846 0.009873 0.100000
+v 0.039603 0.010612 0.097171
+v 0.038796 0.010395 0.099171
+v 0.038146 0.000000 0.100000
+v 0.041000 0.000000 0.097171
+v 0.040164 0.000000 0.099171
+v 0.036846 -0.009873 0.100000
+v 0.039603 -0.010612 0.097171
+v 0.038796 -0.010395 0.099171
+v 0.033036 -0.019073 0.100000
+v 0.035507 -0.020500 0.097171
+v 0.034783 -0.020082 0.099171
+v 0.026974 -0.026973 0.100000
+v 0.028991 -0.028991 0.097171
+v 0.028400 -0.028400 0.099171
+v 0.019073 -0.033036 0.100000
+v 0.020500 -0.035507 0.097171
+v 0.020082 -0.034783 0.099171
+v 0.009873 -0.036846 0.100000
+v 0.010612 -0.039603 0.097171
+v 0.010395 -0.038796 0.099171
+v 0.000000 -0.038146 0.100000
+v 0.000000 -0.041000 0.097171
+v 0.000000 -0.040164 0.099171
+v -0.009873 -0.036846 0.100000
+v -0.010612 -0.039603 0.097171
+v -0.010395 -0.038796 0.099171
+v -0.019073 -0.033036 0.100000
+v -0.020500 -0.035507 0.097171
+v -0.020082 -0.034783 0.099171
+v -0.026973 -0.026974 0.100000
+v -0.028991 -0.028991 0.097171
+v -0.028400 -0.028400 0.099171
+v -0.033036 -0.019073 0.100000
+v -0.035507 -0.020500 0.097171
+v -0.034783 -0.020082 0.099171
+v -0.036846 -0.009873 0.100000
+v -0.039603 -0.010612 0.097171
+v -0.038796 -0.010395 0.099171
+v -0.038146 -0.000000 0.100000
+v -0.041000 -0.000000 0.097171
+v -0.040164 -0.000000 0.099171
+v -0.036846 0.009873 0.100000
+v -0.039603 0.010612 0.097171
+v -0.038796 0.010395 0.099171
+v -0.033036 0.019073 0.100000
+v -0.035507 0.020500 0.097171
+v -0.034783 0.020082 0.099171
+v -0.026974 0.026973 0.100000
+v -0.028991 0.028991 0.097171
+v -0.028400 0.028400 0.099171
+v -0.019073 0.033036 0.100000
+v -0.020500 0.035507 0.097171
+v -0.020082 0.034783 0.099171
+v -0.009873 0.036846 0.100000
+v -0.010612 0.039603 0.097171
+v -0.010395 0.038796 0.099171
+v 0.000000 0.035502 0.100000
+v 0.000000 0.032649 0.097171
+v 0.000000 0.033484 0.099171
+v 0.008450 0.031536 0.097171
+v 0.009189 0.034293 0.100000
+v 0.008666 0.032344 0.099171
+v 0.016324 0.028275 0.097171
+v 0.017751 0.030746 0.100000
+v 0.016742 0.028998 0.099171
+v 0.023086 0.023086 0.097171
+v 0.025104 0.025104 0.100000
+v 0.023677 0.023677 0.099171
+v 0.028275 0.016324 0.097171
+v 0.030746 0.017751 0.100000
+v 0.028998 0.016742 0.099171
+v 0.031536 0.008450 0.097171
+v 0.034293 0.009189 0.100000
+v 0.032344 0.008666 0.099171
+v 0.032649 0.000000 0.097171
+v 0.035502 0.000000 0.100000
+v 0.033484 0.000000 0.099171
+v 0.031536 -0.008450 0.097171
+v 0.034293 -0.009189 0.100000
+v 0.032344 -0.008666 0.099171
+v 0.028275 -0.016324 0.097171
+v 0.030746 -0.017751 0.100000
+v 0.028998 -0.016742 0.099171
+v 0.023086 -0.023086 0.097171
+v 0.025104 -0.025104 0.100000
+v 0.023677 -0.023677 0.099171
+v 0.016324 -0.028275 0.097171
+v 0.017751 -0.030746 0.100000
+v 0.016742 -0.028998 0.099171
+v 0.008450 -0.031536 0.097171
+v 0.009189 -0.034293 0.100000
+v 0.008666 -0.032343 0.099171
+v 0.000000 -0.032649 0.097171
+v 0.000000 -0.035502 0.100000
+v 0.000000 -0.033484 0.099171
+v -0.008450 -0.031536 0.097171
+v -0.009189 -0.034293 0.100000
+v -0.008666 -0.032344 0.099171
+v -0.016324 -0.028275 0.097171
+v -0.017751 -0.030746 0.100000
+v -0.016742 -0.028998 0.099171
+v -0.023086 -0.023086 0.097171
+v -0.025104 -0.025104 0.100000
+v -0.023677 -0.023677 0.099171
+v -0.028275 -0.016324 0.097171
+v -0.030746 -0.017751 0.100000
+v -0.028998 -0.016742 0.099171
+v -0.031536 -0.008450 0.097171
+v -0.034293 -0.009189 0.100000
+v -0.032343 -0.008666 0.099171
+v -0.032649 -0.000000 0.097171
+v -0.035502 -0.000000 0.100000
+v -0.033484 -0.000000 0.099171
+v -0.031536 0.008450 0.097171
+v -0.034293 0.009189 0.100000
+v -0.032344 0.008666 0.099171
+v -0.028275 0.016324 0.097171
+v -0.030746 0.017751 0.100000
+v -0.028998 0.016742 0.099171
+v -0.023086 0.023086 0.097171
+v -0.025104 0.025104 0.100000
+v -0.023677 0.023677 0.099171
+v -0.016324 0.028275 0.097171
+v -0.017751 0.030746 0.100000
+v -0.016742 0.028998 0.099171
+v -0.008450 0.031536 0.097171
+v -0.009189 0.034293 0.100000
+v -0.008666 0.032344 0.099171
+v 0.000000 0.039460 0.000000
+v 0.000000 0.041000 0.001527
+v 0.000000 0.040549 0.000447
+v 0.010612 0.039603 0.001527
+v 0.010213 0.038116 0.000000
+v 0.010495 0.039167 0.000447
+v 0.020500 0.035507 0.001527
+v 0.019730 0.034174 0.000000
+v 0.020275 0.035116 0.000447
+v 0.028991 0.028991 0.001527
+v 0.027903 0.027903 0.000000
+v 0.028672 0.028672 0.000447
+v 0.035507 0.020500 0.001527
+v 0.034174 0.019730 0.000000
+v 0.035116 0.020275 0.000447
+v 0.039603 0.010612 0.001527
+v 0.038116 0.010213 0.000000
+v 0.039167 0.010495 0.000447
+v 0.041000 0.000000 0.001527
+v 0.039460 0.000000 0.000000
+v 0.040549 0.000000 0.000447
+v 0.039603 -0.010612 0.001527
+v 0.038116 -0.010213 0.000000
+v 0.039167 -0.010495 0.000447
+v 0.035507 -0.020500 0.001527
+v 0.034174 -0.019730 0.000000
+v 0.035116 -0.020275 0.000447
+v 0.028991 -0.028991 0.001527
+v 0.027903 -0.027903 0.000000
+v 0.028672 -0.028672 0.000447
+v 0.020500 -0.035507 0.001527
+v 0.019730 -0.034174 0.000000
+v 0.020275 -0.035116 0.000447
+v 0.010612 -0.039603 0.001527
+v 0.010213 -0.038116 0.000000
+v 0.010495 -0.039167 0.000447
+v 0.000000 -0.041000 0.001527
+v 0.000000 -0.039460 0.000000
+v 0.000000 -0.040549 0.000447
+v -0.010612 -0.039603 0.001527
+v -0.010213 -0.038116 0.000000
+v -0.010495 -0.039167 0.000447
+v -0.020500 -0.035507 0.001527
+v -0.019730 -0.034174 0.000000
+v -0.020274 -0.035116 0.000447
+v -0.028991 -0.028991 0.001527
+v -0.027903 -0.027903 0.000000
+v -0.028672 -0.028672 0.000447
+v -0.035507 -0.020500 0.001527
+v -0.034174 -0.019730 0.000000
+v -0.035116 -0.020275 0.000447
+v -0.039603 -0.010612 0.001527
+v -0.038116 -0.010213 0.000000
+v -0.039167 -0.010495 0.000447
+v -0.041000 -0.000000 0.001527
+v -0.039460 -0.000000 0.000000
+v -0.040549 -0.000000 0.000447
+v -0.039603 0.010612 0.001527
+v -0.038116 0.010213 0.000000
+v -0.039167 0.010495 0.000447
+v -0.035507 0.020500 0.001527
+v -0.034174 0.019730 0.000000
+v -0.035116 0.020274 0.000447
+v -0.028991 0.028991 0.001527
+v -0.027903 0.027903 0.000000
+v -0.028673 0.028672 0.000447
+v -0.020500 0.035507 0.001527
+v -0.019730 0.034174 0.000000
+v -0.020275 0.035116 0.000447
+v -0.010612 0.039603 0.001527
+v -0.010213 0.038116 0.000000
+v -0.010495 0.039167 0.000447
+v -0.003627 0.038527 0.081890
+v -0.005507 0.038527 0.080294
+v -0.005059 0.038527 0.081441
+v 0.005507 0.038527 0.080294
+v 0.003627 0.038527 0.081890
+v 0.005059 0.038527 0.081441
+v -0.003600 0.080633 0.059432
+v -0.005474 0.079067 0.059309
+v -0.005036 0.080199 0.059398
+v -0.005474 0.056411 0.081965
+v -0.003600 0.056534 0.083531
+v -0.005036 0.056500 0.083097
+v -0.005477 0.077925 0.066460
+v -0.003600 0.079455 0.066958
+v -0.005046 0.079026 0.066818
+v -0.005477 0.074748 0.072697
+v -0.003600 0.076049 0.073642
+v -0.005046 0.075684 0.073377
+v -0.005477 0.069799 0.077646
+v -0.003600 0.070744 0.078947
+v -0.005046 0.070479 0.078582
+v -0.005477 0.063562 0.080823
+v -0.003600 0.064059 0.082353
+v -0.005046 0.063920 0.081924
+v 0.003600 0.056534 0.083531
+v 0.005474 0.056411 0.081965
+v 0.005036 0.056500 0.083097
+v 0.005474 0.079067 0.059309
+v 0.003600 0.080633 0.059432
+v 0.005036 0.080199 0.059398
+v 0.003600 0.064059 0.082353
+v 0.005477 0.063562 0.080823
+v 0.005046 0.063920 0.081924
+v 0.003600 0.070744 0.078947
+v 0.005477 0.069799 0.077646
+v 0.005046 0.070479 0.078582
+v 0.003600 0.076049 0.073642
+v 0.005477 0.074748 0.072697
+v 0.005046 0.075684 0.073377
+v 0.003600 0.079455 0.066958
+v 0.005477 0.077925 0.066460
+v 0.005046 0.079026 0.066818
+v -0.003631 0.038527 0.018578
+v -0.005510 0.038527 0.020185
+v -0.005068 0.038527 0.019031
+v 0.003631 0.038527 0.018578
+v 0.005510 0.038527 0.020185
+v 0.005068 0.038527 0.019031
+v -0.003599 0.080629 0.050000
+v -0.005471 0.079081 0.050000
+v -0.005023 0.080207 0.050000
+v 0.005471 0.079081 0.050000
+v 0.003599 0.080629 0.050000
+v 0.005023 0.080207 0.050000
+v -0.005474 0.079067 0.040691
+v -0.003600 0.080633 0.040568
+v -0.005036 0.080199 0.040602
+v -0.003600 0.056534 0.016469
+v -0.005474 0.056411 0.018035
+v -0.005036 0.056500 0.016903
+v -0.003600 0.079455 0.033042
+v -0.005477 0.077925 0.033540
+v -0.005046 0.079026 0.033182
+v -0.003600 0.076049 0.026358
+v -0.005477 0.074748 0.027303
+v -0.005046 0.075684 0.026623
+v -0.003600 0.070744 0.021053
+v -0.005477 0.069799 0.022354
+v -0.005046 0.070479 0.021418
+v -0.003600 0.064059 0.017647
+v -0.005477 0.063562 0.019177
+v -0.005046 0.063920 0.018076
+v 0.005474 0.056411 0.018035
+v 0.003600 0.056534 0.016469
+v 0.005036 0.056500 0.016903
+v 0.003600 0.080633 0.040568
+v 0.005474 0.079067 0.040691
+v 0.005036 0.080199 0.040602
+v 0.005477 0.063562 0.019177
+v 0.003600 0.064059 0.017647
+v 0.005046 0.063920 0.018076
+v 0.005477 0.069799 0.022354
+v 0.003600 0.070744 0.021053
+v 0.005046 0.070479 0.021418
+v 0.005477 0.074748 0.027303
+v 0.003600 0.076049 0.026358
+v 0.005046 0.075684 0.026623
+v 0.005477 0.077925 0.033540
+v 0.003600 0.079455 0.033042
+v 0.005046 0.079026 0.033182
+v 0.003628 0.038527 0.070184
+v 0.005471 0.038527 0.072262
+v 0.004983 0.038527 0.070850
+v -0.003627 0.038527 0.070184
+v -0.005471 0.038527 0.072267
+v -0.004983 0.038527 0.070852
+v -0.005472 0.075182 0.050000
+v -0.003602 0.073666 0.050000
+v -0.005029 0.074082 0.050000
+v 0.003602 0.073666 0.050000
+v 0.005472 0.075177 0.050000
+v 0.005030 0.074081 0.050000
+v 0.005464 0.074276 0.065273
+v 0.003594 0.072835 0.064794
+v 0.004992 0.073209 0.064919
+v 0.003600 0.073692 0.058871
+v 0.005468 0.075191 0.059001
+v 0.005015 0.074095 0.058907
+v -0.005468 0.075196 0.059001
+v -0.003599 0.073692 0.058871
+v -0.005014 0.074096 0.058907
+v -0.003593 0.072835 0.064794
+v -0.005464 0.074281 0.065274
+v -0.004991 0.073210 0.064919
+v 0.005464 0.071645 0.070442
+v 0.003594 0.070415 0.069549
+v 0.004994 0.070735 0.069781
+v -0.003593 0.070415 0.069549
+v -0.005464 0.071649 0.070445
+v -0.004993 0.070735 0.069782
+v 0.005464 0.067544 0.074543
+v 0.003594 0.066651 0.073313
+v 0.004994 0.066883 0.073633
+v -0.003593 0.066650 0.073313
+v -0.005464 0.067547 0.074547
+v -0.004993 0.066884 0.073634
+v 0.005464 0.062374 0.077175
+v 0.003594 0.061896 0.075733
+v 0.004992 0.062021 0.076107
+v -0.003593 0.061896 0.075733
+v -0.005464 0.062376 0.077179
+v -0.004991 0.062021 0.076108
+v 0.005468 0.056103 0.078090
+v 0.003600 0.055974 0.076590
+v 0.005015 0.056009 0.076993
+v -0.003599 0.055974 0.076590
+v -0.005468 0.056103 0.078095
+v -0.005014 0.056010 0.076994
+v 0.005472 0.038527 0.027531
+v 0.003627 0.038527 0.029560
+v 0.004985 0.038527 0.028916
+v -0.003626 0.038527 0.029560
+v -0.005472 0.038527 0.027526
+v -0.004984 0.038527 0.028915
+v 0.003594 0.072835 0.035206
+v 0.005464 0.074276 0.034727
+v 0.004992 0.073209 0.035081
+v 0.005468 0.075191 0.040999
+v 0.003600 0.073692 0.041129
+v 0.005015 0.074095 0.041093
+v -0.003599 0.073692 0.041129
+v -0.005468 0.075196 0.040999
+v -0.005014 0.074096 0.041093
+v -0.005464 0.074281 0.034726
+v -0.003593 0.072835 0.035206
+v -0.004991 0.073210 0.035081
+v 0.003594 0.070415 0.030451
+v 0.005464 0.071645 0.029558
+v 0.004994 0.070735 0.030219
+v -0.005464 0.071649 0.029555
+v -0.003593 0.070415 0.030451
+v -0.004993 0.070735 0.030218
+v 0.003594 0.066651 0.026687
+v 0.005464 0.067544 0.025457
+v 0.004994 0.066883 0.026367
+v -0.005464 0.067547 0.025453
+v -0.003593 0.066650 0.026687
+v -0.004993 0.066884 0.026366
+v 0.003594 0.061896 0.024267
+v 0.005464 0.062374 0.022825
+v 0.004992 0.062021 0.023893
+v -0.005464 0.062376 0.022821
+v -0.003593 0.061896 0.024267
+v -0.004991 0.062021 0.023892
+v 0.003600 0.055974 0.023410
+v 0.005468 0.056103 0.021910
+v 0.005015 0.056009 0.023007
+v -0.005468 0.056103 0.021905
+v -0.003599 0.055974 0.023410
+v -0.005014 0.056010 0.023006
+v -0.005477 0.045795 0.081957
+v -0.003603 0.045916 0.083535
+v -0.005045 0.045879 0.083093
+v 0.005477 0.045795 0.081957
+v 0.003603 0.045916 0.083535
+v 0.005045 0.045879 0.083093
+v -0.003608 0.045257 0.076495
+v -0.005460 0.045676 0.078027
+v -0.004995 0.045382 0.076898
+v 0.003609 0.045257 0.076495
+v 0.005460 0.045676 0.078022
+v 0.004996 0.045381 0.076897
+v -0.003604 0.045959 0.016460
+v -0.005478 0.045808 0.018049
+v -0.005052 0.045914 0.016909
+v 0.003604 0.045959 0.016460
+v 0.005478 0.045808 0.018049
+v 0.005052 0.045914 0.016909
+v -0.005460 0.045677 0.021971
+v -0.003608 0.045270 0.023501
+v -0.004995 0.045392 0.023098
+v 0.005460 0.045677 0.021977
+v 0.003608 0.045270 0.023501
+v 0.004996 0.045392 0.023099
+vn -0.2539 0.9477 -0.1935
+vn -0.2539 0.9477 0.1935
+vn 0.0000 0.9811 0.1935
+vn 0.0000 0.9811 -0.1935
+vn 0.0000 0.0000 -1.0000
+vn -0.1478 0.1478 -0.9779
+vn -0.1045 0.1810 -0.9779
+vn 0.2539 0.9477 0.1935
+vn 0.2539 0.9477 -0.1935
+vn 0.4905 0.8496 0.1935
+vn 0.4905 0.8496 -0.1935
+vn -0.2090 0.0000 -0.9779
+vn -0.2019 0.0541 -0.9779
+vn 0.1045 0.1810 0.9779
+vn 0.0541 0.2019 0.9779
+vn -0.0461 -0.1719 0.9840
+vn -0.0890 -0.1541 0.9840
+vn 0.6937 0.6937 0.1935
+vn 0.6937 0.6937 -0.1935
+vn -0.1478 -0.1478 -0.9779
+vn -0.1810 -0.1045 -0.9779
+vn 0.1478 0.1478 0.9779
+vn -0.1258 -0.1258 0.9840
+vn 0.8496 0.4905 0.1935
+vn 0.8496 0.4905 -0.1935
+vn -0.0541 -0.2019 0.9779
+vn 0.0000 -0.2090 0.9779
+vn 0.0000 0.1780 0.9840
+vn 0.0461 0.1719 0.9840
+vn 0.1810 0.1045 0.9779
+vn -0.1541 -0.0890 0.9840
+vn 0.9477 0.2539 0.1935
+vn 0.9477 0.2539 -0.1935
+vn 0.1045 -0.1810 0.9779
+vn 0.1478 -0.1478 0.9779
+vn -0.1258 0.1258 0.9840
+vn -0.0890 0.1541 0.9840
+vn 0.2019 0.0541 0.9779
+vn -0.1719 -0.0461 0.9840
+vn 0.9811 0.0000 0.1935
+vn 0.9811 0.0000 -0.1935
+vn 0.2019 -0.0541 0.9779
+vn 0.2090 0.0000 0.9779
+vn -0.1780 0.0000 0.9840
+vn -0.1719 0.0461 0.9840
+vn 0.9477 -0.2539 0.1935
+vn 0.9477 -0.2539 -0.1935
+vn 0.1810 0.1045 -0.9779
+vn 0.2019 0.0541 -0.9779
+vn 0.1810 -0.1045 -0.9779
+vn 0.1478 -0.1478 -0.9779
+vn 0.8496 -0.4905 0.1935
+vn 0.8496 -0.4905 -0.1935
+vn 0.0541 0.2019 -0.9779
+vn 0.1045 0.1810 -0.9779
+vn 0.1810 -0.1045 0.9779
+vn -0.1541 0.0890 0.9840
+vn 0.6937 -0.6937 0.1935
+vn 0.6937 -0.6937 -0.1935
+vn -0.0541 0.2019 -0.9779
+vn 0.4905 -0.8496 0.1935
+vn 0.4905 -0.8496 -0.1935
+vn -0.1810 0.1045 -0.9779
+vn 0.0541 -0.2019 -0.9779
+vn 0.0000 -0.2090 -0.9779
+vn 0.2539 -0.9477 0.1935
+vn 0.2539 -0.9477 -0.1935
+vn -0.2019 -0.0541 -0.9779
+vn 0.0541 -0.2019 0.9779
+vn -0.0461 0.1719 0.9840
+vn 0.0000 -0.9811 0.1935
+vn 0.0000 -0.9811 -0.1935
+vn -0.0541 -0.2019 -0.9779
+vn -0.1045 -0.1810 -0.9779
+vn -0.2539 -0.9477 0.1935
+vn -0.2539 -0.9477 -0.1935
+vn 0.1045 -0.1810 -0.9779
+vn -0.4905 -0.8496 0.1935
+vn -0.4905 -0.8496 -0.1935
+vn 0.2019 -0.0541 -0.9779
+vn -0.1045 -0.1810 0.9779
+vn 0.0890 0.1541 0.9840
+vn -0.6937 -0.6937 0.1935
+vn -0.6937 -0.6937 -0.1935
+vn 0.2090 0.0000 -0.9779
+vn -0.1810 -0.1045 0.9779
+vn -0.1478 -0.1478 0.9779
+vn 0.1258 0.1258 0.9840
+vn 0.1541 0.0890 0.9840
+vn -0.8496 -0.4905 0.1935
+vn -0.8496 -0.4905 -0.1935
+vn 0.0000 0.2090 0.9779
+vn -0.0541 0.2019 0.9779
+vn 0.0461 -0.1719 0.9840
+vn 0.0000 -0.1780 0.9840
+vn -0.2019 -0.0541 0.9779
+vn 0.1719 0.0461 0.9840
+vn -0.9477 -0.2539 0.1935
+vn -0.9477 -0.2539 -0.1935
+vn -0.2090 0.0000 0.9779
+vn 0.1780 0.0000 0.9840
+vn -0.9811 0.0000 0.1935
+vn -0.9811 0.0000 -0.1935
+vn -0.2019 0.0541 0.9779
+vn 0.1719 -0.0461 0.9840
+vn -0.9477 0.2539 0.1935
+vn -0.9477 0.2539 -0.1935
+vn -0.1810 0.1045 0.9779
+vn 0.1541 -0.0890 0.9840
+vn -0.8496 0.4905 0.1935
+vn -0.8496 0.4905 -0.1935
+vn -0.1478 0.1478 0.9779
+vn 0.1258 -0.1258 0.9840
+vn -0.6937 0.6937 0.1935
+vn -0.6937 0.6937 -0.1935
+vn -0.1045 0.1810 0.9779
+vn 0.0890 -0.1541 0.9840
+vn -0.4905 0.8496 0.1935
+vn -0.4905 0.8496 -0.1935
+vn -0.2536 -0.9464 0.1998
+vn 0.0000 -0.9798 0.1998
+vn 0.0000 -0.7342 0.6789
+vn -0.1900 -0.7092 0.6789
+vn 0.0000 0.2090 -0.9779
+vn 0.1478 0.1478 -0.9779
+vn 0.2536 0.9464 0.1998
+vn 0.0000 0.9798 0.1998
+vn 0.0000 0.7342 0.6789
+vn 0.1900 0.7092 0.6789
+vn -0.6928 0.6928 0.1998
+vn -0.8485 0.4899 0.1998
+vn -0.6359 0.3671 0.6789
+vn -0.5192 0.5192 0.6789
+vn 0.8485 -0.4899 0.1998
+vn 0.9464 -0.2536 0.1998
+vn 0.7092 -0.1900 0.6789
+vn 0.6359 -0.3671 0.6789
+vn -0.9464 -0.2536 0.1998
+vn -0.8485 -0.4899 0.1998
+vn -0.6359 -0.3671 0.6789
+vn -0.7092 -0.1900 0.6789
+vn 0.8485 0.4899 0.1998
+vn 0.6928 0.6928 0.1998
+vn 0.5192 0.5192 0.6789
+vn 0.6359 0.3671 0.6789
+vn -0.2536 0.9464 0.1998
+vn -0.1900 0.7092 0.6789
+vn 0.2536 -0.9464 0.1998
+vn 0.4899 -0.8485 0.1998
+vn 0.3671 -0.6359 0.6789
+vn 0.1900 -0.7092 0.6789
+vn -0.9464 0.2536 0.1998
+vn -0.7092 0.1900 0.6789
+vn 0.9798 0.0000 0.1998
+vn 0.7342 0.0000 0.6789
+vn -0.6928 -0.6928 0.1998
+vn -0.5192 -0.5192 0.6789
+vn 0.4899 0.8485 0.1998
+vn 0.3671 0.6359 0.6789
+vn -0.4899 0.8485 0.1998
+vn -0.3671 0.6359 0.6789
+vn 0.6928 -0.6928 0.1998
+vn 0.5192 -0.5192 0.6789
+vn -0.9798 0.0000 0.1998
+vn -0.7342 0.0000 0.6789
+vn 0.9464 0.2536 0.1998
+vn 0.7092 0.1900 0.6789
+vn -0.4899 -0.8485 0.1998
+vn -0.3671 -0.6359 0.6789
+vn 0.0000 0.0000 1.0000
+vn 0.0000 0.7203 0.6937
+vn 0.1864 0.6957 0.6937
+vn 0.3601 0.6238 0.6937
+vn 0.5093 0.5093 0.6937
+vn 0.6238 0.3601 0.6937
+vn 0.6957 0.1864 0.6937
+vn 0.7203 0.0000 0.6937
+vn 0.6957 -0.1864 0.6937
+vn 0.6238 -0.3601 0.6937
+vn 0.5093 -0.5093 0.6937
+vn 0.3601 -0.6238 0.6937
+vn 0.1864 -0.6957 0.6937
+vn 0.0000 -0.7203 0.6937
+vn -0.1864 -0.6957 0.6937
+vn -0.3601 -0.6238 0.6937
+vn -0.5093 -0.5093 0.6937
+vn -0.6238 -0.3601 0.6937
+vn -0.6957 -0.1864 0.6937
+vn -0.7203 0.0000 0.6937
+vn -0.6957 0.1864 0.6937
+vn -0.6238 0.3601 0.6937
+vn -0.5093 0.5093 0.6937
+vn -0.3601 0.6238 0.6937
+vn -0.1864 0.6957 0.6937
+vn 0.0000 -0.6882 0.7255
+vn -0.1781 -0.6648 0.7255
+vn -0.3441 -0.5960 0.7255
+vn -0.4866 -0.4866 0.7255
+vn -0.5960 -0.3441 0.7255
+vn -0.6648 -0.1781 0.7255
+vn -0.6882 0.0000 0.7255
+vn -0.6648 0.1781 0.7255
+vn -0.5960 0.3441 0.7255
+vn -0.4866 0.4866 0.7255
+vn -0.3441 0.5960 0.7255
+vn -0.1781 0.6648 0.7255
+vn 0.0000 0.6882 0.7255
+vn 0.1781 0.6648 0.7255
+vn 0.3441 0.5960 0.7255
+vn 0.4866 0.4866 0.7255
+vn 0.5960 0.3441 0.7255
+vn 0.6648 0.1781 0.7255
+vn 0.6882 0.0000 0.7255
+vn 0.6648 -0.1781 0.7255
+vn 0.5960 -0.3441 0.7255
+vn 0.4866 -0.4866 0.7255
+vn 0.3441 -0.5960 0.7255
+vn 0.1781 -0.6648 0.7255
+vn 0.1864 0.6957 -0.6937
+vn 0.0000 0.7203 -0.6937
+vn 0.3601 0.6238 -0.6937
+vn 0.5093 0.5093 -0.6937
+vn 0.6238 0.3601 -0.6937
+vn 0.6957 0.1864 -0.6937
+vn 0.7203 0.0000 -0.6937
+vn 0.6957 -0.1864 -0.6937
+vn 0.6238 -0.3601 -0.6937
+vn 0.5093 -0.5093 -0.6937
+vn 0.3601 -0.6238 -0.6937
+vn 0.1864 -0.6957 -0.6937
+vn 0.0000 -0.7203 -0.6937
+vn -0.1864 -0.6957 -0.6937
+vn -0.3601 -0.6238 -0.6937
+vn -0.5093 -0.5093 -0.6937
+vn -0.6238 -0.3601 -0.6937
+vn -0.6957 -0.1864 -0.6937
+vn -0.7203 0.0000 -0.6937
+vn -0.6957 0.1864 -0.6937
+vn -0.6238 0.3601 -0.6937
+vn -0.5093 0.5093 -0.6937
+vn -0.3601 0.6238 -0.6937
+vn -0.1864 0.6957 -0.6937
+vn 0.9873 -0.0316 0.1558
+vn 0.9365 0.2263 -0.2680
+vn 0.9869 0.0385 -0.1568
+vn 0.9807 -0.0226 0.1941
+vn -0.9809 0.0597 0.1849
+vn -0.9841 -0.0538 -0.1691
+vn -0.9822 -0.0130 -0.1871
+vn -0.9820 0.0147 0.1883
+vn -0.9826 0.1857 0.0000
+vn -0.9812 -0.1929 0.0000
+vn -0.9825 -0.1859 -0.0129
+vn -0.9815 0.1907 0.0146
+vn -0.9809 0.1849 0.0597
+vn -0.9841 -0.1691 -0.0538
+vn -0.9838 -0.1451 -0.1054
+vn -0.9812 0.1562 0.1134
+vn -0.9838 -0.1054 -0.1451
+vn -0.9812 0.1134 0.1562
+vn 0.1468 0.0770 0.9861
+vn -0.1468 0.0770 0.9861
+vn -0.1455 -0.1062 0.9836
+vn 0.1455 -0.1062 0.9836
+vn 0.9802 -0.0302 -0.1954
+vn 0.9866 0.0386 0.1584
+vn 0.9385 0.2178 0.2678
+vn 0.9884 -0.0388 -0.1467
+vn -0.9815 0.1907 -0.0146
+vn -0.9825 -0.1859 0.0129
+vn -0.9809 0.1849 -0.0597
+vn -0.9841 -0.1691 0.0538
+vn -0.9812 0.1562 -0.1134
+vn -0.9838 -0.1451 0.1054
+vn -0.9812 0.1134 -0.1562
+vn -0.9838 -0.1054 0.1451
+vn -0.9809 0.0597 -0.1849
+vn -0.9841 -0.0538 0.1691
+vn 0.1472 0.0772 -0.9861
+vn 0.1452 -0.1337 -0.9803
+vn -0.1452 -0.1337 -0.9803
+vn -0.1472 0.0772 -0.9861
+vn -0.1399 -0.9901 0.0000
+vn -0.1397 -0.9877 0.0698
+vn 0.1394 -0.9878 0.0698
+vn 0.1395 -0.9902 0.0000
+vn 0.1394 -0.9878 -0.0698
+vn 0.1398 -0.9438 -0.2995
+vn -0.1401 -0.9438 -0.2994
+vn -0.1397 -0.9877 -0.0698
+vn 0.1391 -0.8012 -0.5819
+vn -0.1395 -0.8012 -0.5819
+vn 0.1391 -0.5819 -0.8012
+vn -0.1395 -0.5819 -0.8012
+vn 0.1398 -0.2995 -0.9438
+vn -0.1402 -0.2994 -0.9437
+vn 0.1393 -0.0667 -0.9880
+vn -0.1396 -0.0667 -0.9879
+vn 0.1446 0.3836 -0.9121
+vn -0.1450 0.3836 -0.9120
+vn -0.1401 -0.9438 0.2994
+vn 0.1398 -0.9438 0.2995
+vn -0.1395 -0.8012 0.5819
+vn 0.1391 -0.8012 0.5819
+vn -0.1395 -0.5819 0.8012
+vn 0.1391 -0.5819 0.8012
+vn -0.1401 -0.2994 0.9437
+vn 0.1398 -0.2995 0.9438
+vn -0.1396 -0.0670 0.9879
+vn 0.1392 -0.0670 0.9880
+vn -0.1444 0.3729 0.9165
+vn 0.1440 0.3729 0.9166
+vn -0.9884 -0.0388 -0.1467
+vn -0.9387 0.2175 0.2674
+vn -0.9867 0.0385 0.1581
+vn -0.9802 -0.0302 -0.1954
+vn 0.9812 0.1562 0.1135
+vn 0.9837 -0.1454 -0.1056
+vn 0.9840 -0.1694 -0.0539
+vn 0.9809 0.1849 0.0597
+vn 0.9821 0.0148 -0.1877
+vn 0.9821 -0.0130 0.1876
+vn 0.9812 0.1135 -0.1562
+vn 0.9837 -0.1056 0.1454
+vn 0.9840 -0.0539 0.1694
+vn 0.9809 0.0597 -0.1849
+vn 0.1485 0.5813 0.8000
+vn -0.1485 0.5813 0.8000
+vn -0.1478 0.3049 0.9408
+vn 0.1478 0.3049 0.9408
+vn 0.1478 0.3049 -0.9408
+vn -0.1478 0.3049 -0.9408
+vn -0.9821 0.0148 -0.1877
+vn -0.9822 -0.0130 0.1873
+vn 0.9812 0.1135 0.1562
+vn 0.9837 -0.1056 -0.1454
+vn 0.9826 0.1857 0.0000
+vn 0.9811 -0.1932 0.0000
+vn 0.9824 -0.1862 0.0130
+vn 0.9815 0.1907 -0.0146
+vn 0.9820 0.0147 0.1884
+vn 0.9822 -0.0130 -0.1874
+vn 0.9840 -0.0539 -0.1694
+vn 0.9809 0.0597 0.1849
+vn 0.1478 0.9408 -0.3049
+vn 0.1485 0.8000 -0.5813
+vn -0.1485 0.8000 -0.5813
+vn -0.1478 0.9408 -0.3049
+vn -0.9807 -0.0226 0.1941
+vn -0.9869 0.0384 -0.1565
+vn -0.9366 0.2260 -0.2676
+vn -0.9873 -0.0316 0.1558
+vn 0.1123 0.6749 -0.7293
+vn -0.1126 0.6749 -0.7292
+vn -0.1448 0.9894 0.0000
+vn 0.1448 0.9894 0.0000
+vn 0.1456 0.9864 -0.0767
+vn -0.1456 0.9864 -0.0767
+vn 0.1485 0.5813 -0.8000
+vn -0.1485 0.5813 -0.8000
+vn 0.9815 0.1907 0.0146
+vn 0.9824 -0.1862 -0.0130
+vn 0.9809 0.1849 -0.0597
+vn 0.9840 -0.1694 0.0539
+vn 0.9837 -0.1454 0.1056
+vn 0.9812 0.1562 -0.1135
+vn -0.1456 0.9864 0.0767
+vn 0.1456 0.9864 0.0767
+vn -0.1484 -0.2151 0.9652
+vn 0.1484 -0.2151 0.9652
+vn 0.1485 0.8000 0.5813
+vn -0.1485 0.8000 0.5813
+vn -0.1478 0.9408 0.3049
+vn 0.1478 0.9408 0.3049
+vn -0.1134 0.6594 0.7432
+vn 0.1131 0.6594 0.7432
+vn 0.1475 -0.2713 -0.9511
+vn -0.1475 -0.2713 -0.9511
+vn 0.6697 0.7404 0.0575
+vn 0.6653 0.7103 0.2300
+vn -0.6653 0.7103 0.2300
+vn -0.6697 0.7404 0.0575
+vn 0.6664 0.6031 0.4382
+vn -0.6664 0.6031 0.4382
+vn 0.6664 0.4382 0.6031
+vn -0.6664 0.4382 0.6031
+vn 0.6653 0.2300 0.7103
+vn -0.6653 0.2300 0.7103
+vn 0.6718 0.0577 0.7385
+vn -0.6718 0.0577 0.7385
+vn -0.6767 0.7362 0.0000
+vn -0.6652 -0.0813 0.7422
+vn 0.7048 -0.1535 0.6926
+vn 0.6652 -0.0813 0.7422
+vn 0.6767 0.7362 0.0000
+vn 0.6697 0.7404 -0.0575
+vn 0.6653 0.7103 -0.2300
+vn -0.6653 0.7103 -0.2300
+vn -0.6697 0.7404 -0.0575
+vn 0.6664 0.6031 -0.4382
+vn -0.6664 0.6031 -0.4382
+vn 0.6664 0.4382 -0.6031
+vn -0.6664 0.4382 -0.6031
+vn 0.6653 0.2300 -0.7103
+vn -0.6653 0.2300 -0.7103
+vn 0.6724 0.0579 -0.7378
+vn -0.6724 0.0579 -0.7378
+vn -0.6623 -0.1034 -0.7421
+vn 0.7106 -0.1924 -0.6768
+vn 0.6623 -0.1034 -0.7421
+vn 0.6762 -0.7349 -0.0517
+vn 0.6870 -0.6926 -0.2197
+vn -0.6876 -0.6921 -0.2195
+vn -0.6767 -0.7344 -0.0517
+vn 0.6854 -0.5892 -0.4279
+vn -0.6859 -0.5888 -0.4276
+vn 0.6854 -0.4279 -0.5892
+vn -0.6859 -0.4276 -0.5888
+vn 0.6870 -0.2197 -0.6926
+vn -0.6876 -0.2195 -0.6921
+vn 0.6761 -0.0500 -0.7350
+vn -0.6767 -0.0500 -0.7345
+vn -0.6685 -0.7437 0.0000
+vn -0.6990 0.2732 -0.6608
+vn 0.4884 0.5826 -0.6496
+vn 0.6985 0.2735 -0.6613
+vn 0.6680 -0.7442 0.0000
+vn 0.6762 -0.7349 0.0517
+vn 0.6870 -0.6926 0.2197
+vn -0.6876 -0.6921 0.2195
+vn -0.6767 -0.7344 0.0517
+vn 0.6854 -0.5892 0.4279
+vn -0.6859 -0.5888 0.4276
+vn 0.6854 -0.4279 0.5892
+vn -0.6859 -0.4276 0.5888
+vn 0.6870 -0.2197 0.6926
+vn -0.6876 -0.2195 0.6921
+vn 0.6760 -0.0501 0.7352
+vn -0.6765 -0.0501 0.7347
+vn -0.6982 0.2655 0.6648
+vn 0.4946 0.5670 0.6587
+vn 0.6977 0.2658 0.6653
+vn -0.7048 -0.1535 0.6926
+vn -0.4891 0.5823 -0.6494
+vn -0.7106 -0.1924 -0.6768
+vn -0.4952 0.5667 0.6584
+s 1
+f 240//1 97//2 27//3 172//4
+f 1//5 235//6 238//7
+f 172//4 27//3 31//8 174//9
+f 174//9 31//8 34//10 177//11
+f 1//5 226//12 229//13
+f 33//14 30//15 103//16 106//17
+f 177//11 34//10 37//18 180//19
+f 1//5 217//20 220//21
+f 36//22 33//14 106//17 109//23
+f 180//19 37//18 40//24 183//25
+f 66//26 63//27 136//28 139//29
+f 39//30 36//22 109//23 112//31
+f 183//25 40//24 43//32 186//33
+f 57//34 54//35 127//36 130//37
+f 42//38 39//30 112//31 115//39
+f 186//33 43//32 46//40 189//41
+f 48//42 45//43 118//44 121//45
+f 45//43 42//38 115//39 118//44
+f 189//41 46//40 49//46 192//47
+f 1//5 184//48 187//49
+f 1//5 196//50 199//51
+f 192//47 49//46 52//52 195//53
+f 1//5 175//54 178//55
+f 51//56 48//42 121//45 124//57
+f 195//53 52//52 55//58 198//59
+f 1//5 238//7 241//60
+f 54//35 51//56 124//57 127//36
+f 198//59 55//58 58//61 201//62
+f 1//5 229//13 232//63
+f 1//5 205//64 208//65
+f 201//62 58//61 61//66 204//67
+f 1//5 220//21 223//68
+f 60//69 57//34 130//37 133//70
+f 204//67 61//66 64//71 207//72
+f 1//5 211//73 214//74
+f 63//27 60//69 133//70 136//28
+f 207//72 64//71 67//75 210//76
+f 1//5 202//77 205//64
+f 1//5 214//74 217//20
+f 210//76 67//75 70//78 213//79
+f 1//5 193//80 196//50
+f 69//81 66//26 139//29 142//82
+f 213//79 70//78 73//83 216//84
+f 1//5 187//49 190//85
+f 75//86 72//87 145//88 148//89
+f 216//84 73//83 76//90 219//91
+f 28//92 96//93 169//94 99//95
+f 78//96 75//86 148//89 151//97
+f 219//91 76//90 79//98 222//99
+f 81//100 78//96 151//97 154//101
+f 222//99 79//98 82//102 225//103
+f 1//5 232//63 235//6
+f 84//104 81//100 154//101 157//105
+f 225//103 82//102 85//106 228//107
+f 1//5 223//68 226//12
+f 87//108 84//104 157//105 160//109
+f 228//107 85//106 88//110 231//111
+f 72//87 69//81 142//82 145//88
+f 90//112 87//108 160//109 163//113
+f 231//111 88//110 91//114 234//115
+f 1//5 208//65 211//73
+f 93//116 90//112 163//113 166//117
+f 234//115 91//114 94//118 237//119
+f 1//5 199//51 202//77
+f 96//93 93//116 166//117 169//94
+f 237//119 94//118 97//2 240//1
+f 1//5 190//85 193//80
+f 102//120 100//121 2//122 4//123
+f 1//5 171//124 175//54
+f 1//5 181//125 184//48
+f 1//5 178//55 181//125
+f 138//126 135//127 15//128 16//129
+f 126//130 123//131 11//132 12//133
+f 159//134 156//135 22//136 23//137
+f 114//138 111//139 7//140 8//141
+f 147//142 144//143 18//144 19//145
+f 135//127 132//146 14//147 15//128
+f 168//148 165//149 25//150 26//151
+f 123//131 120//152 10//153 11//132
+f 156//135 153//154 21//155 22//136
+f 111//139 108//156 6//157 7//140
+f 144//143 141//158 17//159 18//144
+f 132//146 129//160 13//161 14//147
+f 165//149 162//162 24//163 25//150
+f 120//152 117//164 9//165 10//153
+f 153//154 150//166 20//167 21//155
+f 108//156 105//168 5//169 6//157
+f 141//158 138//126 16//129 17//159
+f 129//160 126//130 12//133 13//161
+f 162//162 159//134 23//137 24//163
+f 117//164 114//138 8//141 9//165
+f 105//168 102//120 4//123 5//169
+f 150//166 147//142 19//145 20//167
+f 100//121 168//148 26//151 2//122
+f 3//170 4//123 2//122
+f 3//170 5//169 4//123
+f 3//170 6//157 5//169
+f 3//170 7//140 6//157
+f 3//170 8//141 7//140
+f 3//170 9//165 8//141
+f 3//170 10//153 9//165
+f 3//170 11//132 10//153
+f 3//170 12//133 11//132
+f 3//170 13//161 12//133
+f 3//170 14//147 13//161
+f 3//170 15//128 14//147
+f 3//170 16//129 15//128
+f 3//170 17//159 16//129
+f 3//170 18//144 17//159
+f 3//170 19//145 18//144
+f 3//170 20//167 19//145
+f 3//170 21//155 20//167
+f 3//170 22//136 21//155
+f 3//170 23//137 22//136
+f 3//170 24//163 23//137
+f 3//170 25//150 24//163
+f 3//170 26//151 25//150
+f 3//170 2//122 26//151
+f 31//8 27//3 29//171 32//172
+f 32//172 29//171 28//92 30//15
+f 34//10 31//8 32//172 35//173
+f 35//173 32//172 30//15 33//14
+f 37//18 34//10 35//173 38//174
+f 38//174 35//173 33//14 36//22
+f 40//24 37//18 38//174 41//175
+f 41//175 38//174 36//22 39//30
+f 43//32 40//24 41//175 44//176
+f 44//176 41//175 39//30 42//38
+f 46//40 43//32 44//176 47//177
+f 47//177 44//176 42//38 45//43
+f 49//46 46//40 47//177 50//178
+f 50//178 47//177 45//43 48//42
+f 52//52 49//46 50//178 53//179
+f 53//179 50//178 48//42 51//56
+f 55//58 52//52 53//179 56//180
+f 56//180 53//179 51//56 54//35
+f 58//61 55//58 56//180 59//181
+f 59//181 56//180 54//35 57//34
+f 61//66 58//61 59//181 62//182
+f 62//182 59//181 57//34 60//69
+f 64//71 61//66 62//182 65//183
+f 65//183 62//182 60//69 63//27
+f 67//75 64//71 65//183 68//184
+f 68//184 65//183 63//27 66//26
+f 70//78 67//75 68//184 71//185
+f 71//185 68//184 66//26 69//81
+f 73//83 70//78 71//185 74//186
+f 74//186 71//185 69//81 72//87
+f 76//90 73//83 74//186 77//187
+f 77//187 74//186 72//87 75//86
+f 79//98 76//90 77//187 80//188
+f 80//188 77//187 75//86 78//96
+f 82//102 79//98 80//188 83//189
+f 83//189 80//188 78//96 81//100
+f 85//106 82//102 83//189 86//190
+f 86//190 83//189 81//100 84//104
+f 88//110 85//106 86//190 89//191
+f 89//191 86//190 84//104 87//108
+f 91//114 88//110 89//191 92//192
+f 92//192 89//191 87//108 90//112
+f 94//118 91//114 92//192 95//193
+f 95//193 92//192 90//112 93//116
+f 97//2 94//118 95//193 98//194
+f 98//194 95//193 93//116 96//93
+f 27//3 97//2 98//194 29//171
+f 29//171 98//194 96//93 28//92
+f 103//16 99//95 101//195 104//196
+f 104//196 101//195 100//121 102//120
+f 106//17 103//16 104//196 107//197
+f 107//197 104//196 102//120 105//168
+f 109//23 106//17 107//197 110//198
+f 110//198 107//197 105//168 108//156
+f 112//31 109//23 110//198 113//199
+f 113//199 110//198 108//156 111//139
+f 115//39 112//31 113//199 116//200
+f 116//200 113//199 111//139 114//138
+f 118//44 115//39 116//200 119//201
+f 119//201 116//200 114//138 117//164
+f 121//45 118//44 119//201 122//202
+f 122//202 119//201 117//164 120//152
+f 124//57 121//45 122//202 125//203
+f 125//203 122//202 120//152 123//131
+f 127//36 124//57 125//203 128//204
+f 128//204 125//203 123//131 126//130
+f 130//37 127//36 128//204 131//205
+f 131//205 128//204 126//130 129//160
+f 133//70 130//37 131//205 134//206
+f 134//206 131//205 129//160 132//146
+f 136//28 133//70 134//206 137//207
+f 137//207 134//206 132//146 135//127
+f 139//29 136//28 137//207 140//208
+f 140//208 137//207 135//127 138//126
+f 142//82 139//29 140//208 143//209
+f 143//209 140//208 138//126 141//158
+f 145//88 142//82 143//209 146//210
+f 146//210 143//209 141//158 144//143
+f 148//89 145//88 146//210 149//211
+f 149//211 146//210 144//143 147//142
+f 151//97 148//89 149//211 152//212
+f 152//212 149//211 147//142 150//166
+f 154//101 151//97 152//212 155//213
+f 155//213 152//212 150//166 153//154
+f 157//105 154//101 155//213 158//214
+f 158//214 155//213 153//154 156//135
+f 160//109 157//105 158//214 161//215
+f 161//215 158//214 156//135 159//134
+f 163//113 160//109 161//215 164//216
+f 164//216 161//215 159//134 162//162
+f 166//117 163//113 164//216 167//217
+f 167//217 164//216 162//162 165//149
+f 169//94 166//117 167//217 170//218
+f 170//218 167//217 165//149 168//148
+f 99//95 169//94 170//218 101//195
+f 101//195 170//218 168//148 100//121
+f 30//15 28//92 99//95 103//16
+f 172//4 174//9 176//219 173//220
+f 173//220 176//219 175//54 171//124
+f 174//9 177//11 179//221 176//219
+f 176//219 179//221 178//55 175//54
+f 177//11 180//19 182//222 179//221
+f 179//221 182//222 181//125 178//55
+f 180//19 183//25 185//223 182//222
+f 182//222 185//223 184//48 181//125
+f 183//25 186//33 188//224 185//223
+f 185//223 188//224 187//49 184//48
+f 186//33 189//41 191//225 188//224
+f 188//224 191//225 190//85 187//49
+f 189//41 192//47 194//226 191//225
+f 191//225 194//226 193//80 190//85
+f 192//47 195//53 197//227 194//226
+f 194//226 197//227 196//50 193//80
+f 195//53 198//59 200//228 197//227
+f 197//227 200//228 199//51 196//50
+f 198//59 201//62 203//229 200//228
+f 200//228 203//229 202//77 199//51
+f 201//62 204//67 206//230 203//229
+f 203//229 206//230 205//64 202//77
+f 204//67 207//72 209//231 206//230
+f 206//230 209//231 208//65 205//64
+f 207//72 210//76 212//232 209//231
+f 209//231 212//232 211//73 208//65
+f 210//76 213//79 215//233 212//232
+f 212//232 215//233 214//74 211//73
+f 213//79 216//84 218//234 215//233
+f 215//233 218//234 217//20 214//74
+f 216//84 219//91 221//235 218//234
+f 218//234 221//235 220//21 217//20
+f 219//91 222//99 224//236 221//235
+f 221//235 224//236 223//68 220//21
+f 222//99 225//103 227//237 224//236
+f 224//236 227//237 226//12 223//68
+f 225//103 228//107 230//238 227//237
+f 227//237 230//238 229//13 226//12
+f 228//107 231//111 233//239 230//238
+f 230//238 233//239 232//63 229//13
+f 231//111 234//115 236//240 233//239
+f 233//239 236//240 235//6 232//63
+f 234//115 237//119 239//241 236//240
+f 236//240 239//241 238//7 235//6
+f 237//119 240//1 242//242 239//241
+f 239//241 242//242 241//60 238//7
+f 240//1 172//4 173//220 242//242
+f 242//242 173//220 171//124 241//60
+f 1//5 241//60 171//124
+f 246//243 334//244 433//245 426//246
+f 264//247 373//248 379//249 252//250
+f 292//251 339//252 351//253 250//254
+f 255//255 355//256 361//257 258//258
+f 258//258 361//257 367//259 261//260
+f 261//260 367//259 373//248 264//247
+f 267//261 253//262 424//263 427//264
+f 439//265 444//266 381//267 289//268
+f 297//269 394//270 339//252 292//251
+f 304//271 396//272 394//270 297//269
+f 307//273 402//274 396//272 304//271
+f 310//275 408//276 402//274 307//273
+f 313//277 414//278 408//276 310//275
+f 316//279 438//280 435//281 300//282
+f 340//283 393//284 391//285 342//286
+f 348//287 346//288 354//289 352//290
+f 346//288 358//291 360//292 354//289
+f 358//291 364//293 366//294 360//292
+f 364//293 370//295 372//296 366//294
+f 370//295 376//297 378//298 372//296
+f 376//297 432//299 429//300 378//298
+f 391//285 393//284 397//301 387//302
+f 340//283 342//286 348//287 352//290
+f 387//302 397//301 403//303 399//304
+f 399//304 403//303 409//305 405//306
+f 405//306 409//305 415//307 411//308
+f 411//308 415//307 421//309 417//310
+f 417//310 421//309 442//311 445//312
+f 286//313 385//314 441//315 436//316
+f 280//317 357//318 345//319 283//320
+f 315//321 418//322 444//266 439//265
+f 324//323 406//324 412//325 321//326
+f 276//327 262//328 265//329 273//330
+f 322//331 316//279 300//282 312//332
+f 301//333 420//334 414//278 313//277
+f 277//335 363//336 357//318 280//317
+f 294//337 343//338 390//339 319//340
+f 268//341 375//342 369//343 274//344
+f 331//345 328//346 306//347 303//348
+f 423//349 430//350 337//351 244//352
+f 432//299 333//353 336//354 429//300
+f 291//355 295//356 318//357 298//358
+f 328//346 325//359 309//360 306//347
+f 426//246 433//245 375//342 268//341
+f 318//357 331//345 303//348 298//358
+f 270//361 349//362 343//338 294//337
+f 273//330 265//329 253//262 267//261
+f 330//363 388//364 400//365 327//366
+f 291//355 249//367 271//368 295//356
+f 325//359 322//331 312//332 309//360
+f 250//254 351//253 355//256 255//255
+f 427//264 424//263 243//369 247//370
+f 327//366 400//365 406//324 324//323
+f 283//320 345//319 349//362 270//361
+f 279//371 259//372 262//328 276//327
+f 321//326 412//325 418//322 315//321
+f 274//344 369//343 363//336 277//335
+f 271//368 249//367 256//373 282//374
+f 282//374 256//373 259//372 279//371
+f 445//312 442//311 384//375 382//376
+f 252//250 379//249 430//350 423//349
+f 319//340 390//339 388//364 330//363
+f 438//280 288//377 285//378 435//281
+f 436//316 441//315 420//334 301//333
+f 283//320 270//361 272//379 284//380
+f 284//380 272//379 271//368 282//374
+f 250//254 255//255 257//381 251//382
+f 251//382 257//381 256//373 249//367
+f 280//317 283//320 284//380 281//383
+f 281//383 284//380 282//374 279//371
+f 255//255 258//258 260//384 257//381
+f 257//381 260//384 259//372 256//373
+f 277//335 280//317 281//383 278//385
+f 278//385 281//383 279//371 276//327
+f 258//258 261//260 263//386 260//384
+f 260//384 263//386 262//328 259//372
+f 274//344 277//335 278//385 275//387
+f 275//387 278//385 276//327 273//330
+f 261//260 264//247 266//388 263//386
+f 263//386 266//388 265//329 262//328
+f 268//341 274//344 275//387 269//389
+f 269//389 275//387 273//330 267//261
+f 264//247 252//250 254//390 266//388
+f 266//388 254//390 253//262 265//329
+f 249//367 291//355 293//391 251//382
+f 251//382 293//391 292//251 250//254
+f 424//263 253//262 254//390 425//392
+f 425//392 254//390 252//250 423//349
+f 427//264 247//370 248//393 428//394
+f 428//394 248//393 246//243 426//246
+f 295//356 271//368 272//379 296//395
+f 296//395 272//379 270//361 294//337
+f 331//345 318//357 320//396 332//397
+f 332//397 320//396 319//340 330//363
+f 298//358 303//348 305//398 299//399
+f 299//399 305//398 304//271 297//269
+f 328//346 331//345 332//397 329//400
+f 329//400 332//397 330//363 327//366
+f 303//348 306//347 308//401 305//398
+f 305//398 308//401 307//273 304//271
+f 325//359 328//346 329//400 326//402
+f 326//402 329//400 327//366 324//323
+f 306//347 309//360 311//403 308//401
+f 308//401 311//403 310//275 307//273
+f 322//331 325//359 326//402 323//404
+f 323//404 326//402 324//323 321//326
+f 309//360 312//332 314//405 311//403
+f 311//403 314//405 313//277 310//275
+f 316//279 322//331 323//404 317//406
+f 317//406 323//404 321//326 315//321
+f 312//332 300//282 302//407 314//405
+f 314//405 302//407 301//333 313//277
+f 297//269 292//251 293//391 299//399
+f 299//399 293//391 291//355 298//358
+f 436//316 301//333 302//407 437//408
+f 437//408 302//407 300//282 435//281
+f 439//265 289//268 290//409 440//410
+f 440//410 290//409 288//377 438//280
+f 294//337 319//340 320//396 296//395
+f 296//395 320//396 318//357 295//356
+f 346//288 348//287 350//411 347//412
+f 347//412 350//411 349//362 345//319
+f 352//290 354//289 356//413 353//414
+f 353//414 356//413 355//256 351//253
+f 358//291 346//288 347//412 359//415
+f 359//415 347//412 345//319 357//318
+f 354//289 360//292 362//416 356//413
+f 356//413 362//416 361//257 355//256
+f 364//293 358//291 359//415 365//417
+f 365//417 359//415 357//318 363//336
+f 360//292 366//294 368//418 362//416
+f 362//416 368//418 367//259 361//257
+f 370//295 364//293 365//417 371//419
+f 371//419 365//417 363//336 369//343
+f 366//294 372//296 374//420 368//418
+f 368//418 374//420 373//248 367//259
+f 376//297 370//295 371//419 377//421
+f 377//421 371//419 369//343 375//342
+f 372//296 378//298 380//422 374//420
+f 374//420 380//422 379//249 373//248
+f 351//253 339//252 341//423 353//414
+f 353//414 341//423 340//283 352//290
+f 430//350 379//249 380//422 431//424
+f 431//424 380//422 378//298 429//300
+f 433//245 334//244 335//425 434//426
+f 434//426 335//425 333//353 432//299
+f 343//338 349//362 350//411 344//427
+f 344//427 350//411 348//287 342//286
+f 388//364 390//339 392//428 389//429
+f 389//429 392//428 391//285 387//302
+f 394//270 396//272 398//430 395//431
+f 395//431 398//430 397//301 393//284
+f 400//365 388//364 389//429 401//432
+f 401//432 389//429 387//302 399//304
+f 396//272 402//274 404//433 398//430
+f 398//430 404//433 403//303 397//301
+f 406//324 400//365 401//432 407//434
+f 407//434 401//432 399//304 405//306
+f 402//274 408//276 410//435 404//433
+f 404//433 410//435 409//305 403//303
+f 412//325 406//324 407//434 413//436
+f 413//436 407//434 405//306 411//308
+f 408//276 414//278 416//437 410//435
+f 410//435 416//437 415//307 409//305
+f 418//322 412//325 413//436 419//438
+f 419//438 413//436 411//308 417//310
+f 414//278 420//334 422//439 416//437
+f 416//437 422//439 421//309 415//307
+f 393//284 340//283 341//423 395//431
+f 395//431 341//423 339//252 394//270
+f 442//311 421//309 422//439 443//440
+f 443//440 422//439 420//334 441//315
+f 445//312 382//376 383//441 446//442
+f 446//442 383//441 381//267 444//266
+f 342//286 391//285 392//428 344//427
+f 344//427 392//428 390//339 343//338
+f 243//369 424//263 425//392 245//443
+f 245//443 425//392 423//349 244//352
+f 267//261 427//264 428//394 269//389
+f 269//389 428//394 426//246 268//341
+f 337//351 430//350 431//424 338//444
+f 338//444 431//424 429//300 336//354
+f 375//342 433//245 434//426 377//421
+f 377//421 434//426 432//299 376//297
+f 286//313 436//316 437//408 287//445
+f 287//445 437//408 435//281 285//378
+f 315//321 439//265 440//410 317//406
+f 317//406 440//410 438//280 316//279
+f 384//375 442//311 443//440 386//446
+f 386//446 443//440 441//315 385//314
+f 417//310 445//312 446//442 419//438
+f 419//438 446//442 444//266 418//322
diff --git a/examples/pybullet/gym/pybullet_data/urdf/mug.urdf b/examples/pybullet/gym/pybullet_data/urdf/mug.urdf
new file mode 100644
index 000000000..ab105ea54
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/urdf/mug.urdf
@@ -0,0 +1,28 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/examples/pybullet/gym/pybullet_data/urdf/mug_col.obj b/examples/pybullet/gym/pybullet_data/urdf/mug_col.obj
new file mode 100644
index 000000000..4d56449df
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/urdf/mug_col.obj
@@ -0,0 +1,1310 @@
+# Blender v2.79 (sub 0) OBJ File: 'mug.blend'
+# www.blender.org
+o mug_col_Cylinder.003
+v 0.000000 0.000000 0.000000
+v 0.000000 0.041000 0.097171
+v 0.000000 0.038146 0.100000
+v 0.000000 0.040164 0.099171
+v 0.009873 0.036846 0.100000
+v 0.010612 0.039603 0.097171
+v 0.010395 0.038796 0.099171
+v 0.019073 0.033036 0.100000
+v 0.020500 0.035507 0.097171
+v 0.020082 0.034783 0.099171
+v 0.026973 0.026973 0.100000
+v 0.028991 0.028991 0.097171
+v 0.028400 0.028400 0.099171
+v 0.033036 0.019073 0.100000
+v 0.035507 0.020500 0.097171
+v 0.034783 0.020082 0.099171
+v 0.036846 0.009873 0.100000
+v 0.039603 0.010612 0.097171
+v 0.038796 0.010395 0.099171
+v 0.038146 0.000000 0.100000
+v 0.041000 0.000000 0.097171
+v 0.040164 0.000000 0.099171
+v 0.036846 -0.009873 0.100000
+v 0.039603 -0.010612 0.097171
+v 0.038796 -0.010395 0.099171
+v 0.033036 -0.019073 0.100000
+v 0.035507 -0.020500 0.097171
+v 0.034783 -0.020082 0.099171
+v 0.026974 -0.026973 0.100000
+v 0.028991 -0.028991 0.097171
+v 0.028400 -0.028400 0.099171
+v 0.019073 -0.033036 0.100000
+v 0.020500 -0.035507 0.097171
+v 0.020082 -0.034783 0.099171
+v 0.009873 -0.036846 0.100000
+v 0.010612 -0.039603 0.097171
+v 0.010395 -0.038796 0.099171
+v 0.000000 -0.038146 0.100000
+v 0.000000 -0.041000 0.097171
+v 0.000000 -0.040164 0.099171
+v -0.009873 -0.036846 0.100000
+v -0.010612 -0.039603 0.097171
+v -0.010395 -0.038796 0.099171
+v -0.019073 -0.033036 0.100000
+v -0.020500 -0.035507 0.097171
+v -0.020082 -0.034783 0.099171
+v -0.026973 -0.026974 0.100000
+v -0.028991 -0.028991 0.097171
+v -0.028400 -0.028400 0.099171
+v -0.033036 -0.019073 0.100000
+v -0.035507 -0.020500 0.097171
+v -0.034783 -0.020082 0.099171
+v -0.036846 -0.009873 0.100000
+v -0.039603 -0.010612 0.097171
+v -0.038796 -0.010395 0.099171
+v -0.038146 -0.000000 0.100000
+v -0.041000 -0.000000 0.097171
+v -0.040164 -0.000000 0.099171
+v -0.036846 0.009873 0.100000
+v -0.039603 0.010612 0.097171
+v -0.038796 0.010395 0.099171
+v -0.033036 0.019073 0.100000
+v -0.035507 0.020500 0.097171
+v -0.034783 0.020082 0.099171
+v -0.026974 0.026973 0.100000
+v -0.028991 0.028991 0.097171
+v -0.028400 0.028400 0.099171
+v -0.019073 0.033036 0.100000
+v -0.020500 0.035507 0.097171
+v -0.020082 0.034783 0.099171
+v -0.009873 0.036846 0.100000
+v -0.010612 0.039603 0.097171
+v -0.010395 0.038796 0.099171
+v 0.000000 -0.000000 0.100000
+v 0.000000 0.039460 0.000000
+v 0.000000 0.041000 0.001527
+v 0.000000 0.040549 0.000447
+v 0.010612 0.039603 0.001527
+v 0.010213 0.038116 0.000000
+v 0.010495 0.039167 0.000447
+v 0.020500 0.035507 0.001527
+v 0.019730 0.034174 0.000000
+v 0.020275 0.035116 0.000447
+v 0.028991 0.028991 0.001527
+v 0.027903 0.027903 0.000000
+v 0.028672 0.028672 0.000447
+v 0.035507 0.020500 0.001527
+v 0.034174 0.019730 0.000000
+v 0.035116 0.020275 0.000447
+v 0.039603 0.010612 0.001527
+v 0.038116 0.010213 0.000000
+v 0.039167 0.010495 0.000447
+v 0.041000 0.000000 0.001527
+v 0.039460 0.000000 0.000000
+v 0.040549 0.000000 0.000447
+v 0.039603 -0.010612 0.001527
+v 0.038116 -0.010213 0.000000
+v 0.039167 -0.010495 0.000447
+v 0.035507 -0.020500 0.001527
+v 0.034174 -0.019730 0.000000
+v 0.035116 -0.020275 0.000447
+v 0.028991 -0.028991 0.001527
+v 0.027903 -0.027903 0.000000
+v 0.028672 -0.028672 0.000447
+v 0.020500 -0.035507 0.001527
+v 0.019730 -0.034174 0.000000
+v 0.020275 -0.035116 0.000447
+v 0.010612 -0.039603 0.001527
+v 0.010213 -0.038116 0.000000
+v 0.010495 -0.039167 0.000447
+v 0.000000 -0.041000 0.001527
+v 0.000000 -0.039460 0.000000
+v 0.000000 -0.040549 0.000447
+v -0.010612 -0.039603 0.001527
+v -0.010213 -0.038116 0.000000
+v -0.010495 -0.039167 0.000447
+v -0.020500 -0.035507 0.001527
+v -0.019730 -0.034174 0.000000
+v -0.020274 -0.035116 0.000447
+v -0.028991 -0.028991 0.001527
+v -0.027903 -0.027903 0.000000
+v -0.028672 -0.028672 0.000447
+v -0.035507 -0.020500 0.001527
+v -0.034174 -0.019730 0.000000
+v -0.035116 -0.020275 0.000447
+v -0.039603 -0.010612 0.001527
+v -0.038116 -0.010213 0.000000
+v -0.039167 -0.010495 0.000447
+v -0.041000 -0.000000 0.001527
+v -0.039460 -0.000000 0.000000
+v -0.040549 -0.000000 0.000447
+v -0.039603 0.010612 0.001527
+v -0.038116 0.010213 0.000000
+v -0.039167 0.010495 0.000447
+v -0.035507 0.020500 0.001527
+v -0.034174 0.019730 0.000000
+v -0.035116 0.020274 0.000447
+v -0.028991 0.028991 0.001527
+v -0.027903 0.027903 0.000000
+v -0.028673 0.028672 0.000447
+v -0.020500 0.035507 0.001527
+v -0.019730 0.034174 0.000000
+v -0.020275 0.035116 0.000447
+v -0.010612 0.039603 0.001527
+v -0.010213 0.038116 0.000000
+v -0.010495 0.039167 0.000447
+vn -0.2539 0.9477 -0.1935
+vn -0.2539 0.9477 0.1935
+vn 0.0000 0.9811 0.1935
+vn 0.0000 0.9811 -0.1935
+vn 0.0000 0.0000 -1.0000
+vn -0.1478 0.1478 -0.9779
+vn -0.1045 0.1810 -0.9779
+vn 0.2539 0.9477 0.1935
+vn 0.2539 0.9477 -0.1935
+vn 0.4905 0.8496 0.1935
+vn 0.4905 0.8496 -0.1935
+vn -0.2090 0.0000 -0.9779
+vn -0.2019 0.0541 -0.9779
+vn 0.6937 0.6937 0.1935
+vn 0.6937 0.6937 -0.1935
+vn -0.1478 -0.1478 -0.9779
+vn -0.1810 -0.1045 -0.9779
+vn 0.8496 0.4905 0.1935
+vn 0.8496 0.4905 -0.1935
+vn 0.9477 0.2539 0.1935
+vn 0.9477 0.2539 -0.1935
+vn 0.9811 0.0000 0.1935
+vn 0.9811 0.0000 -0.1935
+vn 0.9477 -0.2539 0.1935
+vn 0.9477 -0.2539 -0.1935
+vn 0.1810 0.1045 -0.9779
+vn 0.2019 0.0541 -0.9779
+vn 0.1810 -0.1045 -0.9779
+vn 0.1478 -0.1478 -0.9779
+vn 0.8496 -0.4905 0.1935
+vn 0.8496 -0.4905 -0.1935
+vn 0.0541 0.2019 -0.9779
+vn 0.1045 0.1810 -0.9779
+vn 0.6937 -0.6937 0.1935
+vn 0.6937 -0.6937 -0.1935
+vn -0.0541 0.2019 -0.9779
+vn 0.4905 -0.8496 0.1935
+vn 0.4905 -0.8496 -0.1935
+vn -0.1810 0.1045 -0.9779
+vn 0.0541 -0.2019 -0.9779
+vn 0.0000 -0.2090 -0.9779
+vn 0.2539 -0.9477 0.1935
+vn 0.2539 -0.9477 -0.1935
+vn -0.2019 -0.0541 -0.9779
+vn 0.0000 -0.9811 0.1935
+vn 0.0000 -0.9811 -0.1935
+vn -0.0541 -0.2019 -0.9779
+vn -0.1045 -0.1810 -0.9779
+vn -0.2539 -0.9477 0.1935
+vn -0.2539 -0.9477 -0.1935
+vn 0.1045 -0.1810 -0.9779
+vn -0.4905 -0.8496 0.1935
+vn -0.4905 -0.8496 -0.1935
+vn 0.2019 -0.0541 -0.9779
+vn -0.6937 -0.6937 0.1935
+vn -0.6937 -0.6937 -0.1935
+vn 0.2090 0.0000 -0.9779
+vn -0.8496 -0.4905 0.1935
+vn -0.8496 -0.4905 -0.1935
+vn -0.9477 -0.2539 0.1935
+vn -0.9477 -0.2539 -0.1935
+vn -0.9811 0.0000 0.1935
+vn -0.9811 0.0000 -0.1935
+vn -0.9477 0.2539 0.1935
+vn -0.9477 0.2539 -0.1935
+vn -0.8496 0.4905 0.1935
+vn -0.8496 0.4905 -0.1935
+vn -0.6937 0.6937 0.1935
+vn -0.6937 0.6937 -0.1935
+vn -0.4905 0.8496 0.1935
+vn -0.4905 0.8496 -0.1935
+vn 0.0000 0.2090 -0.9779
+vn 0.1478 0.1478 -0.9779
+vn 0.0000 0.7203 0.6937
+vn 0.1864 0.6957 0.6937
+vn 0.0000 0.2090 0.9779
+vn 0.0541 0.2019 0.9779
+vn 0.3601 0.6238 0.6937
+vn 0.1045 0.1810 0.9779
+vn 0.5093 0.5093 0.6937
+vn 0.1478 0.1478 0.9779
+vn 0.6238 0.3601 0.6937
+vn 0.1810 0.1045 0.9779
+vn 0.6957 0.1864 0.6937
+vn 0.2019 0.0541 0.9779
+vn 0.7203 0.0000 0.6937
+vn 0.2090 0.0000 0.9779
+vn 0.6957 -0.1864 0.6937
+vn 0.2019 -0.0541 0.9779
+vn 0.6238 -0.3601 0.6937
+vn 0.1810 -0.1045 0.9779
+vn 0.5093 -0.5093 0.6937
+vn 0.1478 -0.1478 0.9779
+vn 0.3601 -0.6238 0.6937
+vn 0.1045 -0.1810 0.9779
+vn 0.1864 -0.6957 0.6937
+vn 0.0541 -0.2019 0.9779
+vn 0.0000 -0.7203 0.6937
+vn 0.0000 -0.2090 0.9779
+vn -0.1864 -0.6957 0.6937
+vn -0.0541 -0.2019 0.9779
+vn -0.3601 -0.6238 0.6937
+vn -0.1045 -0.1810 0.9779
+vn -0.5093 -0.5093 0.6937
+vn -0.1478 -0.1478 0.9779
+vn -0.6238 -0.3601 0.6937
+vn -0.1810 -0.1045 0.9779
+vn -0.6957 -0.1864 0.6937
+vn -0.2019 -0.0541 0.9779
+vn -0.7203 0.0000 0.6937
+vn -0.2090 0.0000 0.9779
+vn -0.6957 0.1864 0.6937
+vn -0.2019 0.0541 0.9779
+vn -0.6238 0.3601 0.6937
+vn -0.1810 0.1045 0.9779
+vn -0.5093 0.5093 0.6937
+vn -0.1478 0.1478 0.9779
+vn -0.3601 0.6238 0.6937
+vn -0.1045 0.1810 0.9779
+vn -0.1864 0.6957 0.6937
+vn -0.0541 0.2019 0.9779
+vn 0.0000 0.0000 1.0000
+vn 0.1864 0.6957 -0.6937
+vn 0.0000 0.7203 -0.6937
+vn 0.3601 0.6238 -0.6937
+vn 0.5093 0.5093 -0.6937
+vn 0.6238 0.3601 -0.6937
+vn 0.6957 0.1864 -0.6937
+vn 0.7203 0.0000 -0.6937
+vn 0.6957 -0.1864 -0.6937
+vn 0.6238 -0.3601 -0.6937
+vn 0.5093 -0.5093 -0.6937
+vn 0.3601 -0.6238 -0.6937
+vn 0.1864 -0.6957 -0.6937
+vn 0.0000 -0.7203 -0.6937
+vn -0.1864 -0.6957 -0.6937
+vn -0.3601 -0.6238 -0.6937
+vn -0.5093 -0.5093 -0.6937
+vn -0.6238 -0.3601 -0.6937
+vn -0.6957 -0.1864 -0.6937
+vn -0.7203 0.0000 -0.6937
+vn -0.6957 0.1864 -0.6937
+vn -0.6238 0.3601 -0.6937
+vn -0.5093 0.5093 -0.6937
+vn -0.3601 0.6238 -0.6937
+vn -0.1864 0.6957 -0.6937
+s 1
+f 144//1 72//2 2//3 76//4
+f 1//5 139//6 142//7
+f 76//4 2//3 6//8 78//9
+f 78//9 6//8 9//10 81//11
+f 1//5 130//12 133//13
+f 81//11 9//10 12//14 84//15
+f 1//5 121//16 124//17
+f 84//15 12//14 15//18 87//19
+f 87//19 15//18 18//20 90//21
+f 90//21 18//20 21//22 93//23
+f 93//23 21//22 24//24 96//25
+f 1//5 88//26 91//27
+f 1//5 100//28 103//29
+f 96//25 24//24 27//30 99//31
+f 1//5 79//32 82//33
+f 99//31 27//30 30//34 102//35
+f 1//5 142//7 145//36
+f 102//35 30//34 33//37 105//38
+f 1//5 133//13 136//39
+f 1//5 109//40 112//41
+f 105//38 33//37 36//42 108//43
+f 1//5 124//17 127//44
+f 108//43 36//42 39//45 111//46
+f 1//5 115//47 118//48
+f 111//46 39//45 42//49 114//50
+f 1//5 106//51 109//40
+f 1//5 118//48 121//16
+f 114//50 42//49 45//52 117//53
+f 1//5 97//54 100//28
+f 117//53 45//52 48//55 120//56
+f 1//5 91//27 94//57
+f 120//56 48//55 51//58 123//59
+f 123//59 51//58 54//60 126//61
+f 126//61 54//60 57//62 129//63
+f 1//5 136//39 139//6
+f 129//63 57//62 60//64 132//65
+f 1//5 127//44 130//12
+f 132//65 60//64 63//66 135//67
+f 135//67 63//66 66//68 138//69
+f 1//5 112//41 115//47
+f 138//69 66//68 69//70 141//71
+f 1//5 103//29 106//51
+f 141//71 69//70 72//2 144//1
+f 1//5 94//57 97//54
+f 1//5 75//72 79//32
+f 1//5 85//73 88//26
+f 1//5 82//33 85//73
+f 6//8 2//3 4//74 7//75
+f 7//75 4//74 3//76 5//77
+f 9//10 6//8 7//75 10//78
+f 10//78 7//75 5//77 8//79
+f 12//14 9//10 10//78 13//80
+f 13//80 10//78 8//79 11//81
+f 15//18 12//14 13//80 16//82
+f 16//82 13//80 11//81 14//83
+f 18//20 15//18 16//82 19//84
+f 19//84 16//82 14//83 17//85
+f 21//22 18//20 19//84 22//86
+f 22//86 19//84 17//85 20//87
+f 24//24 21//22 22//86 25//88
+f 25//88 22//86 20//87 23//89
+f 27//30 24//24 25//88 28//90
+f 28//90 25//88 23//89 26//91
+f 30//34 27//30 28//90 31//92
+f 31//92 28//90 26//91 29//93
+f 33//37 30//34 31//92 34//94
+f 34//94 31//92 29//93 32//95
+f 36//42 33//37 34//94 37//96
+f 37//96 34//94 32//95 35//97
+f 39//45 36//42 37//96 40//98
+f 40//98 37//96 35//97 38//99
+f 42//49 39//45 40//98 43//100
+f 43//100 40//98 38//99 41//101
+f 45//52 42//49 43//100 46//102
+f 46//102 43//100 41//101 44//103
+f 48//55 45//52 46//102 49//104
+f 49//104 46//102 44//103 47//105
+f 51//58 48//55 49//104 52//106
+f 52//106 49//104 47//105 50//107
+f 54//60 51//58 52//106 55//108
+f 55//108 52//106 50//107 53//109
+f 57//62 54//60 55//108 58//110
+f 58//110 55//108 53//109 56//111
+f 60//64 57//62 58//110 61//112
+f 61//112 58//110 56//111 59//113
+f 63//66 60//64 61//112 64//114
+f 64//114 61//112 59//113 62//115
+f 66//68 63//66 64//114 67//116
+f 67//116 64//114 62//115 65//117
+f 69//70 66//68 67//116 70//118
+f 70//118 67//116 65//117 68//119
+f 72//2 69//70 70//118 73//120
+f 73//120 70//118 68//119 71//121
+f 2//3 72//2 73//120 4//74
+f 4//74 73//120 71//121 3//76
+f 20//87 17//85 74//122
+f 11//81 8//79 74//122
+f 3//76 71//121 74//122
+f 68//119 65//117 74//122
+f 59//113 56//111 74//122
+f 50//107 47//105 74//122
+f 41//101 38//99 74//122
+f 32//95 29//93 74//122
+f 23//89 20//87 74//122
+f 14//83 11//81 74//122
+f 5//77 3//76 74//122
+f 71//121 68//119 74//122
+f 62//115 59//113 74//122
+f 53//109 50//107 74//122
+f 44//103 41//101 74//122
+f 35//97 32//95 74//122
+f 26//91 23//89 74//122
+f 17//85 14//83 74//122
+f 8//79 5//77 74//122
+f 65//117 62//115 74//122
+f 56//111 53//109 74//122
+f 47//105 44//103 74//122
+f 38//99 35//97 74//122
+f 29//93 26//91 74//122
+f 76//4 78//9 80//123 77//124
+f 77//124 80//123 79//32 75//72
+f 78//9 81//11 83//125 80//123
+f 80//123 83//125 82//33 79//32
+f 81//11 84//15 86//126 83//125
+f 83//125 86//126 85//73 82//33
+f 84//15 87//19 89//127 86//126
+f 86//126 89//127 88//26 85//73
+f 87//19 90//21 92//128 89//127
+f 89//127 92//128 91//27 88//26
+f 90//21 93//23 95//129 92//128
+f 92//128 95//129 94//57 91//27
+f 93//23 96//25 98//130 95//129
+f 95//129 98//130 97//54 94//57
+f 96//25 99//31 101//131 98//130
+f 98//130 101//131 100//28 97//54
+f 99//31 102//35 104//132 101//131
+f 101//131 104//132 103//29 100//28
+f 102//35 105//38 107//133 104//132
+f 104//132 107//133 106//51 103//29
+f 105//38 108//43 110//134 107//133
+f 107//133 110//134 109//40 106//51
+f 108//43 111//46 113//135 110//134
+f 110//134 113//135 112//41 109//40
+f 111//46 114//50 116//136 113//135
+f 113//135 116//136 115//47 112//41
+f 114//50 117//53 119//137 116//136
+f 116//136 119//137 118//48 115//47
+f 117//53 120//56 122//138 119//137
+f 119//137 122//138 121//16 118//48
+f 120//56 123//59 125//139 122//138
+f 122//138 125//139 124//17 121//16
+f 123//59 126//61 128//140 125//139
+f 125//139 128//140 127//44 124//17
+f 126//61 129//63 131//141 128//140
+f 128//140 131//141 130//12 127//44
+f 129//63 132//65 134//142 131//141
+f 131//141 134//142 133//13 130//12
+f 132//65 135//67 137//143 134//142
+f 134//142 137//143 136//39 133//13
+f 135//67 138//69 140//144 137//143
+f 137//143 140//144 139//6 136//39
+f 138//69 141//71 143//145 140//144
+f 140//144 143//145 142//7 139//6
+f 141//71 144//1 146//146 143//145
+f 143//145 146//146 145//36 142//7
+f 144//1 76//4 77//124 146//146
+f 146//146 77//124 75//72 145//36
+f 1//5 145//36 75//72
+o handle_col.001_Cube.009
+v -0.003600 0.080633 0.059432
+v -0.005474 0.079067 0.059309
+v -0.005036 0.080199 0.059398
+v -0.005477 0.077925 0.066460
+v -0.003600 0.079455 0.066958
+v -0.005046 0.079026 0.066818
+v 0.005474 0.079067 0.059309
+v 0.003600 0.080633 0.059432
+v 0.005036 0.080199 0.059398
+v 0.003600 0.079455 0.066958
+v 0.005477 0.077925 0.066460
+v 0.005046 0.079026 0.066818
+v -0.003599 0.080629 0.050000
+v -0.005471 0.079081 0.050000
+v -0.005023 0.080207 0.050000
+v 0.005471 0.079081 0.050000
+v 0.003599 0.080629 0.050000
+v 0.005023 0.080207 0.050000
+v -0.005474 0.079067 0.040691
+v -0.003600 0.080633 0.040568
+v -0.005036 0.080199 0.040602
+v -0.003600 0.079455 0.033042
+v -0.005477 0.077925 0.033540
+v -0.005046 0.079026 0.033182
+v 0.003600 0.080633 0.040568
+v 0.005474 0.079067 0.040691
+v 0.005036 0.080199 0.040602
+v 0.005477 0.077925 0.033540
+v 0.003600 0.079455 0.033042
+v 0.005046 0.079026 0.033182
+v -0.005472 0.075182 0.050000
+v -0.003602 0.073666 0.050000
+v -0.005029 0.074082 0.050000
+v 0.003602 0.073666 0.050000
+v 0.005472 0.075177 0.050000
+v 0.005030 0.074081 0.050000
+v 0.005464 0.074276 0.065273
+v 0.003594 0.072835 0.064794
+v 0.004992 0.073209 0.064919
+v 0.003600 0.073692 0.058871
+v 0.005468 0.075191 0.059001
+v 0.005015 0.074095 0.058907
+v -0.005468 0.075196 0.059001
+v -0.003599 0.073692 0.058871
+v -0.005014 0.074096 0.058907
+v -0.003593 0.072835 0.064794
+v -0.005464 0.074281 0.065274
+v -0.004991 0.073210 0.064919
+v 0.003594 0.072835 0.035206
+v 0.005464 0.074276 0.034727
+v 0.004992 0.073209 0.035081
+v 0.005468 0.075191 0.040999
+v 0.003600 0.073692 0.041129
+v 0.005015 0.074095 0.041093
+v -0.003599 0.073692 0.041129
+v -0.005468 0.075196 0.040999
+v -0.005014 0.074096 0.041093
+v -0.005464 0.074281 0.034726
+v -0.003593 0.072835 0.035206
+v -0.004991 0.073210 0.035081
+v 0.000000 0.076121 0.065870
+v 0.000000 0.076121 0.034130
+vn -0.9826 0.1857 0.0000
+vn -0.9812 -0.1929 0.0000
+vn -0.9825 -0.1859 -0.0129
+vn -0.9815 0.1907 0.0146
+vn -0.9815 0.1907 -0.0146
+vn -0.9825 -0.1859 0.0129
+vn -0.7388 -0.0633 -0.6709
+vn -0.7237 -0.3338 -0.6040
+vn -0.1399 -0.9901 0.0000
+vn -0.1397 -0.9877 0.0698
+vn 0.1394 -0.9878 0.0698
+vn 0.1395 -0.9902 0.0000
+vn 0.1394 -0.9878 -0.0698
+vn 0.0944 -0.8650 0.4928
+vn -0.0946 -0.8650 0.4927
+vn -0.1397 -0.9877 -0.0698
+vn -0.0946 -0.8650 -0.4927
+vn 0.0944 -0.8650 -0.4928
+vn 0.9826 0.1857 0.0000
+vn 0.9811 -0.1932 0.0000
+vn 0.9824 -0.1862 0.0130
+vn 0.9815 0.1907 -0.0146
+vn -0.1448 0.9894 0.0000
+vn 0.1448 0.9894 0.0000
+vn 0.1456 0.9864 -0.0767
+vn -0.1456 0.9864 -0.0767
+vn 0.1159 0.5509 -0.8265
+vn -0.1159 0.5509 -0.8265
+vn 0.9815 0.1907 0.0146
+vn 0.9824 -0.1862 -0.0130
+vn -0.1456 0.9864 0.0767
+vn 0.1456 0.9864 0.0767
+vn -0.7237 -0.3338 0.6040
+vn -0.7388 -0.0633 0.6709
+vn 0.7388 -0.0632 0.6709
+vn 0.7237 -0.3340 0.6039
+vn -0.1159 0.5509 0.8265
+vn 0.1159 0.5509 0.8265
+vn 0.7237 -0.3340 -0.6039
+vn 0.7388 -0.0633 -0.6709
+vn 0.6697 0.7404 0.0575
+vn 0.5433 0.4040 0.7359
+vn -0.5433 0.4040 0.7359
+vn -0.6697 0.7404 0.0575
+vn -0.6767 0.7362 0.0000
+vn 0.6767 0.7362 0.0000
+vn 0.6697 0.7404 -0.0575
+vn 0.5433 0.4040 -0.7359
+vn -0.5433 0.4040 -0.7359
+vn -0.6697 0.7404 -0.0575
+vn 0.6762 -0.7349 -0.0517
+vn 0.5124 -0.7210 0.4665
+vn -0.5128 -0.7206 0.4666
+vn -0.6767 -0.7344 -0.0517
+vn -0.6685 -0.7437 0.0000
+vn 0.6680 -0.7442 0.0000
+vn 0.6762 -0.7349 0.0517
+vn 0.5124 -0.7210 -0.4665
+vn -0.5128 -0.7206 -0.4666
+vn -0.6767 -0.7344 0.0517
+vn 0.0000 -0.3106 0.9505
+vn 0.0000 -0.3106 -0.9505
+s 1
+f 160//147 177//148 189//149 148//150
+f 165//151 202//152 177//148 160//147
+f 169//153 204//154 202//152 165//151
+f 178//155 201//156 199//157 180//158
+f 186//159 184//160 192//161 190//162
+f 199//157 201//156 205//163 195//164
+f 178//155 180//158 186//159 190//162
+f 162//165 181//166 198//167 172//168
+f 159//169 163//170 171//171 166//172
+f 171//171 175//173 168//174 166//172
+f 153//175 187//176 181//166 162//165
+f 159//169 147//177 154//178 163//170
+f 148//150 189//149 193//179 150//180
+f 157//181 183//182 187//176 153//175
+f 154//178 147//177 151//183 156//184
+f 172//168 198//167 196//185 174//186
+f 157//181 153//175 155//187 158//188
+f 158//188 155//187 154//178 156//184
+f 148//150 150//180 152//189 149//190
+f 149//190 152//189 151//183 147//177
+f 147//177 159//169 161//191 149//190
+f 149//190 161//191 160//147 148//150
+f 163//170 154//178 155//187 164//192
+f 164//192 155//187 153//175 162//165
+f 175//173 171//171 173//193 176//194
+f 176//194 173//193 172//168 174//186
+f 166//172 168//174 170//195 167//196
+f 167//196 170//195 169//153 165//151
+f 165//151 160//147 161//191 167//196
+f 167//196 161//191 159//169 166//172
+f 162//165 172//168 173//193 164//192
+f 164//192 173//193 171//171 163//170
+f 184//160 186//159 188//197 185//198
+f 185//198 188//197 187//176 183//182
+f 190//162 192//161 194//199 191//200
+f 191//200 194//199 193//179 189//149
+f 189//149 177//148 179//201 191//200
+f 191//200 179//201 178//155 190//162
+f 181//166 187//176 188//197 182//202
+f 182//202 188//197 186//159 180//158
+f 196//185 198//167 200//203 197//204
+f 197//204 200//203 199//157 195//164
+f 202//152 204//154 206//205 203//206
+f 203//206 206//205 205//163 201//156
+f 201//156 178//155 179//201 203//206
+f 203//206 179//201 177//148 202//152
+f 180//158 199//157 200//203 182//202
+f 182//202 200//203 198//167 181//166
+f 184//160 185//198 207//207
+f 157//181 158//188 207//207
+f 150//180 193//179 207//207
+f 185//198 183//182 207//207
+f 192//161 184//160 207//207
+f 156//184 151//183 207//207
+f 158//188 156//184 207//207
+f 193//179 194//199 207//207
+f 151//183 152//189 207//207
+f 194//199 192//161 207//207
+f 183//182 157//181 207//207
+f 152//189 150//180 207//207
+f 174//186 196//185 208//208
+f 196//185 197//204 208//208
+f 175//173 176//194 208//208
+f 197//204 195//164 208//208
+f 176//194 174//186 208//208
+f 168//174 175//173 208//208
+f 195//164 205//163 208//208
+f 205//163 206//205 208//208
+f 169//153 170//195 208//208
+f 206//205 204//154 208//208
+f 204//154 169//153 208//208
+f 170//195 168//174 208//208
+o handle_col.002_Cube.008
+v -0.003600 0.079455 0.033042
+v -0.005477 0.077925 0.033540
+v -0.005046 0.079026 0.033182
+v -0.003600 0.076049 0.026358
+v -0.005477 0.074748 0.027303
+v -0.005046 0.075684 0.026623
+v -0.003600 0.070744 0.021053
+v -0.005477 0.069799 0.022354
+v -0.005046 0.070479 0.021418
+v -0.003600 0.064059 0.017647
+v -0.005477 0.063562 0.019177
+v -0.005046 0.063920 0.018076
+v 0.005477 0.063562 0.019177
+v 0.003600 0.064059 0.017647
+v 0.005046 0.063920 0.018076
+v 0.005477 0.069799 0.022354
+v 0.003600 0.070744 0.021053
+v 0.005046 0.070479 0.021418
+v 0.005477 0.074748 0.027303
+v 0.003600 0.076049 0.026358
+v 0.005046 0.075684 0.026623
+v 0.005477 0.077925 0.033540
+v 0.003600 0.079455 0.033042
+v 0.005046 0.079026 0.033182
+v 0.003594 0.072835 0.035206
+v 0.005464 0.074276 0.034727
+v 0.004992 0.073209 0.035081
+v -0.005464 0.074281 0.034726
+v -0.003593 0.072835 0.035206
+v -0.004991 0.073210 0.035081
+v 0.003594 0.070415 0.030451
+v 0.005464 0.071645 0.029558
+v 0.004994 0.070735 0.030219
+v -0.005464 0.071649 0.029555
+v -0.003593 0.070415 0.030451
+v -0.004993 0.070735 0.030218
+v 0.003594 0.066651 0.026687
+v 0.005464 0.067544 0.025457
+v 0.004994 0.066883 0.026367
+v -0.005464 0.067547 0.025453
+v -0.003593 0.066650 0.026687
+v -0.004993 0.066884 0.026366
+v 0.003594 0.061896 0.024267
+v 0.005464 0.062374 0.022825
+v 0.004992 0.062021 0.023893
+v -0.005464 0.062376 0.022821
+v -0.003593 0.061896 0.024267
+v -0.004991 0.062021 0.023892
+v 0.000000 0.076121 0.034130
+v 0.000000 0.062972 0.020980
+vn -0.9812 0.1562 -0.1134
+vn -0.9838 -0.1451 0.1054
+vn -0.7259 0.0844 0.6826
+vn -0.7394 0.3416 0.5801
+vn -0.9812 0.1134 -0.1562
+vn -0.9838 -0.1054 0.1451
+vn -0.7394 -0.5801 -0.3416
+vn -0.7259 -0.6826 -0.0844
+vn 0.0953 -0.4129 0.9057
+vn -0.0955 -0.4130 0.9057
+vn -0.1395 -0.8012 0.5819
+vn 0.1391 -0.8012 0.5819
+vn -0.1395 -0.5819 0.8012
+vn 0.1391 -0.5819 0.8012
+vn -0.0955 -0.9057 0.4130
+vn 0.0953 -0.9057 0.4129
+vn 0.9812 0.1135 -0.1562
+vn 0.9837 -0.1056 0.1454
+vn 0.7259 -0.6826 -0.0842
+vn 0.7394 -0.5801 -0.3416
+vn 0.1171 0.9313 0.3449
+vn 0.1485 0.8000 -0.5813
+vn -0.1485 0.8000 -0.5813
+vn -0.1171 0.9313 0.3449
+vn 0.1485 0.5813 -0.8000
+vn -0.1485 0.5813 -0.8000
+vn 0.7394 0.3416 0.5801
+vn 0.7259 0.0842 0.6826
+vn 0.9837 -0.1454 0.1056
+vn 0.9812 0.1562 -0.1135
+vn 0.1171 -0.3449 -0.9313
+vn -0.1171 -0.3449 -0.9313
+vn 0.5451 0.7577 0.3587
+vn 0.6664 0.6031 -0.4382
+vn -0.6664 0.6031 -0.4382
+vn -0.5451 0.7577 0.3587
+vn 0.6664 0.4382 -0.6031
+vn -0.6664 0.4382 -0.6031
+vn 0.5451 -0.3587 -0.7577
+vn -0.5451 -0.3587 -0.7577
+vn 0.5130 -0.3110 0.8000
+vn 0.6854 -0.5892 0.4279
+vn -0.6859 -0.5888 0.4276
+vn -0.5135 -0.3107 0.7998
+vn 0.6854 -0.4279 0.5892
+vn -0.6859 -0.4276 0.5888
+vn 0.5130 -0.8000 0.3110
+vn -0.5135 -0.7998 0.3107
+vn 0.0000 0.3106 0.9505
+vn 0.0000 -0.9505 -0.3106
+s 1
+f 213//209 242//210 236//211 210//212
+f 216//213 248//214 242//210 213//209
+f 219//215 254//216 248//214 216//213
+f 233//217 237//218 243//219 239//220
+f 239//220 243//219 249//221 245//222
+f 245//222 249//221 255//223 251//224
+f 224//225 246//226 252//227 221//228
+f 231//229 228//230 212//231 209//232
+f 228//230 225//233 215//234 212//231
+f 230//235 234//236 240//237 227//238
+f 225//233 222//239 218//240 215//234
+f 227//238 240//237 246//226 224//225
+f 228//230 231//229 232//241 229//242
+f 229//242 232//241 230//235 227//238
+f 209//232 212//231 214//243 211//244
+f 211//244 214//243 213//209 210//212
+f 225//233 228//230 229//242 226//245
+f 226//245 229//242 227//238 224//225
+f 212//231 215//234 217//246 214//243
+f 214//243 217//246 216//213 213//209
+f 222//239 225//233 226//245 223//247
+f 223//247 226//245 224//225 221//228
+f 215//234 218//240 220//248 217//246
+f 217//246 220//248 219//215 216//213
+f 240//237 234//236 235//249 241//250
+f 241//250 235//249 233//217 239//220
+f 236//211 242//210 244//251 238//252
+f 238//252 244//251 243//219 237//218
+f 246//226 240//237 241//250 247//253
+f 247//253 241//250 239//220 245//222
+f 242//210 248//214 250//254 244//251
+f 244//251 250//254 249//221 243//219
+f 252//227 246//226 247//253 253//255
+f 253//255 247//253 245//222 251//224
+f 248//214 254//216 256//256 250//254
+f 250//254 256//256 255//223 249//221
+f 211//244 210//212 257//257
+f 238//252 237//218 257//257
+f 235//249 234//236 257//257
+f 232//241 231//229 257//257
+f 231//229 209//232 257//257
+f 210//212 236//211 257//257
+f 230//235 232//241 257//257
+f 233//217 235//249 257//257
+f 237//218 233//217 257//257
+f 236//211 238//252 257//257
+f 209//232 211//244 257//257
+f 234//236 230//235 257//257
+f 218//240 222//239 258//258
+f 223//247 221//228 258//258
+f 253//255 251//224 258//258
+f 252//227 253//255 258//258
+f 222//239 223//247 258//258
+f 221//228 252//227 258//258
+f 251//224 255//223 258//258
+f 255//223 256//256 258//258
+f 219//215 220//248 258//258
+f 254//216 219//215 258//258
+f 256//256 254//216 258//258
+f 220//248 218//240 258//258
+o handle_col_Cube.007
+v -0.005477 0.077925 0.066460
+v -0.003600 0.079455 0.066958
+v -0.005046 0.079026 0.066818
+v -0.005477 0.074748 0.072697
+v -0.003600 0.076049 0.073642
+v -0.005046 0.075684 0.073377
+v -0.005477 0.069799 0.077646
+v -0.003600 0.070744 0.078947
+v -0.005046 0.070479 0.078582
+v -0.005477 0.063562 0.080823
+v -0.003600 0.064059 0.082353
+v -0.005046 0.063920 0.081924
+v 0.003600 0.064059 0.082353
+v 0.005477 0.063562 0.080823
+v 0.005046 0.063920 0.081924
+v 0.003600 0.070744 0.078947
+v 0.005477 0.069799 0.077646
+v 0.005046 0.070479 0.078582
+v 0.003600 0.076049 0.073642
+v 0.005477 0.074748 0.072697
+v 0.005046 0.075684 0.073377
+v 0.003600 0.079455 0.066958
+v 0.005477 0.077925 0.066460
+v 0.005046 0.079026 0.066818
+v 0.005464 0.074276 0.065273
+v 0.003594 0.072835 0.064794
+v 0.004992 0.073209 0.064919
+v -0.003593 0.072835 0.064794
+v -0.005464 0.074281 0.065274
+v -0.004991 0.073210 0.064919
+v 0.005464 0.071645 0.070442
+v 0.003594 0.070415 0.069549
+v 0.004994 0.070735 0.069781
+v -0.003593 0.070415 0.069549
+v -0.005464 0.071649 0.070445
+v -0.004993 0.070735 0.069782
+v 0.005464 0.067544 0.074543
+v 0.003594 0.066651 0.073313
+v 0.004994 0.066883 0.073633
+v -0.003593 0.066650 0.073313
+v -0.005464 0.067547 0.074547
+v -0.004993 0.066884 0.073634
+v 0.005464 0.062374 0.077175
+v 0.003594 0.061896 0.075733
+v 0.004992 0.062021 0.076107
+v -0.003593 0.061896 0.075733
+v -0.005464 0.062376 0.077179
+v -0.004991 0.062021 0.076108
+v 0.000000 0.062972 0.079020
+v 0.000000 0.076121 0.065870
+vn -0.7394 0.3416 -0.5801
+vn -0.7259 0.0844 -0.6826
+vn -0.9838 -0.1451 -0.1054
+vn -0.9812 0.1562 0.1134
+vn -0.9838 -0.1054 -0.1451
+vn -0.9812 0.1134 0.1562
+vn -0.7259 -0.6826 0.0844
+vn -0.7394 -0.5801 0.3416
+vn 0.0953 -0.4129 -0.9057
+vn 0.1391 -0.8012 -0.5819
+vn -0.1395 -0.8012 -0.5819
+vn -0.0955 -0.4130 -0.9057
+vn 0.1391 -0.5819 -0.8012
+vn -0.1395 -0.5819 -0.8012
+vn 0.0953 -0.9057 -0.4129
+vn -0.0955 -0.9057 -0.4130
+vn 0.9812 0.1562 0.1135
+vn 0.9837 -0.1454 -0.1056
+vn 0.7259 0.0842 -0.6826
+vn 0.7394 0.3416 -0.5801
+vn 0.1485 0.5813 0.8000
+vn -0.1485 0.5813 0.8000
+vn -0.1171 -0.3449 0.9313
+vn 0.1171 -0.3449 0.9313
+vn 0.9812 0.1135 0.1562
+vn 0.9837 -0.1056 -0.1454
+vn 0.1485 0.8000 0.5813
+vn -0.1485 0.8000 0.5813
+vn 0.7394 -0.5801 0.3416
+vn 0.7259 -0.6826 0.0842
+vn 0.1171 0.9313 -0.3449
+vn -0.1171 0.9313 -0.3449
+vn 0.5451 0.7577 -0.3587
+vn 0.6664 0.6031 0.4382
+vn -0.6664 0.6031 0.4382
+vn -0.5451 0.7577 -0.3587
+vn 0.6664 0.4382 0.6031
+vn -0.6664 0.4382 0.6031
+vn 0.5451 -0.3587 0.7577
+vn -0.5451 -0.3587 0.7577
+vn 0.5130 -0.3110 -0.8000
+vn 0.6854 -0.5892 -0.4279
+vn -0.6859 -0.5888 -0.4276
+vn -0.5135 -0.3107 -0.7998
+vn 0.6854 -0.4279 -0.5892
+vn -0.6859 -0.4276 -0.5888
+vn 0.5130 -0.8000 -0.3110
+vn -0.5135 -0.7998 -0.3107
+vn 0.0000 -0.9505 0.3106
+vn 0.0000 0.3106 -0.9505
+s 1
+f 259//259 287//260 293//261 262//262
+f 262//262 293//261 299//263 265//264
+f 265//264 299//263 305//265 268//266
+f 284//267 290//268 292//269 286//270
+f 290//268 296//271 298//272 292//269
+f 296//271 302//273 304//274 298//272
+f 278//275 289//276 283//277 281//278
+f 274//279 266//280 269//281 271//282
+f 275//283 295//284 289//276 278//275
+f 277//285 263//286 266//280 274//279
+f 272//287 301//288 295//284 275//283
+f 280//289 260//290 263//286 277//285
+f 278//275 281//278 282//291 279//292
+f 279//292 282//291 280//289 277//285
+f 259//259 262//262 264//293 261//294
+f 261//294 264//293 263//286 260//290
+f 275//283 278//275 279//292 276//295
+f 276//295 279//292 277//285 274//279
+f 262//262 265//264 267//296 264//293
+f 264//293 267//296 266//280 263//286
+f 272//287 275//283 276//295 273//297
+f 273//297 276//295 274//279 271//282
+f 265//264 268//266 270//298 267//296
+f 267//296 270//298 269//281 266//280
+f 290//268 284//267 285//299 291//300
+f 291//300 285//299 283//277 289//276
+f 286//270 292//269 294//301 288//302
+f 288//302 294//301 293//261 287//260
+f 296//271 290//268 291//300 297//303
+f 297//303 291//300 289//276 295//284
+f 292//269 298//272 300//304 294//301
+f 294//301 300//304 299//263 293//261
+f 302//273 296//271 297//303 303//305
+f 303//305 297//303 295//284 301//288
+f 298//272 304//274 306//306 300//304
+f 300//304 306//306 305//265 299//263
+f 301//288 272//287 307//307
+f 304//274 302//273 307//307
+f 305//265 306//306 307//307
+f 269//281 270//298 307//307
+f 268//266 305//265 307//307
+f 270//298 268//266 307//307
+f 306//306 304//274 307//307
+f 271//282 269//281 307//307
+f 303//305 301//288 307//307
+f 273//297 271//282 307//307
+f 272//287 273//297 307//307
+f 302//273 303//305 307//307
+f 284//267 286//270 308//308
+f 259//259 261//294 308//308
+f 286//270 288//302 308//308
+f 281//278 283//277 308//308
+f 288//302 287//260 308//308
+f 261//294 260//290 308//308
+f 282//291 281//278 308//308
+f 260//290 280//289 308//308
+f 285//299 284//267 308//308
+f 287//260 259//259 308//308
+f 283//277 285//299 308//308
+f 280//289 282//291 308//308
+o handle_col.004_Cube.006
+v -0.003627 0.038527 0.081890
+v -0.005507 0.038527 0.080294
+v -0.005059 0.038527 0.081441
+v 0.005507 0.038527 0.080294
+v 0.003627 0.038527 0.081890
+v 0.005059 0.038527 0.081441
+v -0.005474 0.056411 0.081965
+v -0.003600 0.056534 0.083531
+v -0.005036 0.056500 0.083097
+v -0.005477 0.063562 0.080823
+v -0.003600 0.064059 0.082353
+v -0.005046 0.063920 0.081924
+v 0.003600 0.056534 0.083531
+v 0.005474 0.056411 0.081965
+v 0.005036 0.056500 0.083097
+v 0.003600 0.064059 0.082353
+v 0.005477 0.063562 0.080823
+v 0.005046 0.063920 0.081924
+v 0.003628 0.038527 0.070184
+v 0.005471 0.038527 0.072262
+v 0.004983 0.038527 0.070850
+v -0.003627 0.038527 0.070184
+v -0.005471 0.038527 0.072267
+v -0.004983 0.038527 0.070852
+v 0.005464 0.062374 0.077175
+v 0.003594 0.061896 0.075733
+v 0.004992 0.062021 0.076107
+v -0.003593 0.061896 0.075733
+v -0.005464 0.062376 0.077179
+v -0.004991 0.062021 0.076108
+v 0.005468 0.056103 0.078090
+v 0.003600 0.055974 0.076590
+v 0.005015 0.056009 0.076993
+v -0.003599 0.055974 0.076590
+v -0.005468 0.056103 0.078095
+v -0.005014 0.056010 0.076994
+v -0.005477 0.045795 0.081957
+v -0.003603 0.045916 0.083535
+v -0.005045 0.045879 0.083093
+v 0.005477 0.045795 0.081957
+v 0.003603 0.045916 0.083535
+v 0.005045 0.045879 0.083093
+v -0.003608 0.045257 0.076495
+v -0.005460 0.045676 0.078027
+v -0.004995 0.045382 0.076898
+v 0.003609 0.045257 0.076495
+v 0.005460 0.045676 0.078022
+v 0.004996 0.045381 0.076897
+v 0.000000 0.062972 0.079020
+v 0.000000 0.038527 0.076154
+vn 0.7254 -0.6787 0.1145
+vn 0.7789 -0.5862 -0.2228
+vn 0.9869 0.0385 -0.1568
+vn 0.9807 -0.0226 0.1941
+vn -0.7388 0.6709 -0.0633
+vn -0.7237 0.6040 -0.3338
+vn -0.9822 -0.0130 -0.1871
+vn -0.9820 0.0147 0.1883
+vn 0.1468 0.0770 0.9861
+vn -0.1468 0.0770 0.9861
+vn -0.1455 -0.1062 0.9836
+vn 0.1455 -0.1062 0.9836
+vn 0.0944 0.4928 -0.8650
+vn 0.1393 -0.0667 -0.9880
+vn -0.1396 -0.0667 -0.9879
+vn -0.0946 0.4927 -0.8650
+vn 0.1446 0.3836 -0.9121
+vn -0.1450 0.3836 -0.9120
+vn 0.9820 0.0147 0.1884
+vn 0.9822 -0.0130 -0.1874
+vn 0.7237 0.6039 -0.3340
+vn 0.7388 0.6709 -0.0633
+vn -0.9807 -0.0226 0.1941
+vn -0.9869 0.0384 -0.1565
+vn -0.7789 -0.5864 -0.2225
+vn -0.7254 -0.6787 0.1145
+vn 0.1423 -0.3544 -0.9242
+vn -0.1427 -0.3542 -0.9242
+vn 0.1159 0.8265 0.5509
+vn -0.1159 0.8265 0.5509
+vn -0.1004 -0.7506 0.6530
+vn 0.1004 -0.7506 0.6530
+vn 0.5433 0.7359 0.4040
+vn 0.6718 0.0577 0.7385
+vn -0.6718 0.0577 0.7385
+vn -0.5433 0.7359 0.4040
+vn -0.6652 -0.0813 0.7422
+vn 0.5236 -0.6789 0.5146
+vn 0.6652 -0.0813 0.7422
+vn 0.5124 0.4665 -0.7209
+vn 0.6761 -0.0500 -0.7350
+vn -0.6767 -0.0500 -0.7345
+vn -0.5128 0.4666 -0.7206
+vn -0.6990 0.2732 -0.6608
+vn 0.5548 -0.3842 -0.7379
+vn 0.6985 0.2735 -0.6613
+vn -0.5236 -0.6789 0.5146
+vn -0.5553 -0.3845 -0.7374
+vn 0.0000 0.9505 -0.3106
+vn 0.0000 -1.0000 0.0000
+s 1
+f 312//309 328//310 355//311 348//312
+f 318//313 337//314 343//315 315//316
+f 321//317 316//318 346//319 349//320
+f 334//321 340//322 342//323 336//324
+f 340//322 354//325 351//326 342//323
+f 322//327 339//328 333//329 325//330
+f 345//331 352//332 331//333 310//334
+f 354//325 327//335 330//336 351//326
+f 348//312 355//311 339//328 322//327
+f 324//337 319//338 316//318 321//317
+f 349//320 346//319 309//339 313//340
+f 315//316 343//315 352//332 345//331
+f 322//327 325//330 326//341 323//342
+f 323//342 326//341 324//337 321//317
+f 318//313 315//316 317//343 320//344
+f 320//344 317//343 316//318 319//338
+f 346//319 316//318 317//343 347//345
+f 347//345 317//343 315//316 345//331
+f 349//320 313//340 314//346 350//347
+f 350//347 314//346 312//309 348//312
+f 340//322 334//321 335//348 341//349
+f 341//349 335//348 333//329 339//328
+f 336//324 342//323 344//350 338//351
+f 338//351 344//350 343//315 337//314
+f 352//332 343//315 344//350 353//352
+f 353//352 344//350 342//323 351//326
+f 355//311 328//310 329//353 356//354
+f 356//354 329//353 327//335 354//325
+f 309//339 346//319 347//345 311//355
+f 311//355 347//345 345//331 310//334
+f 321//317 349//320 350//347 323//342
+f 323//342 350//347 348//312 322//327
+f 331//333 352//332 353//352 332//356
+f 332//356 353//352 351//326 330//336
+f 339//328 355//311 356//354 341//349
+f 341//349 356//354 354//325 340//322
+f 325//330 333//329 357//357
+f 320//344 319//338 357//357
+f 336//324 338//351 357//357
+f 319//338 324//337 357//357
+f 326//341 325//330 357//357
+f 338//351 337//314 357//357
+f 324//337 326//341 357//357
+f 335//348 334//321 357//357
+f 337//314 318//313 357//357
+f 334//321 336//324 357//357
+f 318//313 320//344 357//357
+f 333//329 335//348 357//357
+f 314//346 313//340 358//358
+f 309//339 311//355 358//358
+f 328//310 312//309 358//358
+f 331//333 332//356 358//358
+f 327//335 329//353 358//358
+f 311//355 310//334 358//358
+f 310//334 331//333 358//358
+f 313//340 309//339 358//358
+f 329//353 328//310 358//358
+f 312//309 314//346 358//358
+f 330//336 327//335 358//358
+f 332//356 330//336 358//358
+o handle_col.003_Cube.005
+v -0.003631 0.038527 0.018578
+v -0.005510 0.038527 0.020185
+v -0.005068 0.038527 0.019031
+v 0.003631 0.038527 0.018578
+v 0.005510 0.038527 0.020185
+v 0.005068 0.038527 0.019031
+v -0.003600 0.056534 0.016469
+v -0.005474 0.056411 0.018035
+v -0.005036 0.056500 0.016903
+v -0.003600 0.064059 0.017647
+v -0.005477 0.063562 0.019177
+v -0.005046 0.063920 0.018076
+v 0.005474 0.056411 0.018035
+v 0.003600 0.056534 0.016469
+v 0.005036 0.056500 0.016903
+v 0.005477 0.063562 0.019177
+v 0.003600 0.064059 0.017647
+v 0.005046 0.063920 0.018076
+v 0.005472 0.038527 0.027531
+v 0.003627 0.038527 0.029560
+v 0.004985 0.038527 0.028916
+v -0.003626 0.038527 0.029560
+v -0.005472 0.038527 0.027526
+v -0.004984 0.038527 0.028915
+v 0.003594 0.061896 0.024267
+v 0.005464 0.062374 0.022825
+v 0.004992 0.062021 0.023893
+v -0.005464 0.062376 0.022821
+v -0.003593 0.061896 0.024267
+v -0.004991 0.062021 0.023892
+v 0.003600 0.055974 0.023410
+v 0.005468 0.056103 0.021910
+v 0.005015 0.056009 0.023007
+v -0.005468 0.056103 0.021905
+v -0.003599 0.055974 0.023410
+v -0.005014 0.056010 0.023006
+v -0.003604 0.045959 0.016460
+v -0.005478 0.045808 0.018049
+v -0.005052 0.045914 0.016909
+v 0.003604 0.045959 0.016460
+v 0.005478 0.045808 0.018049
+v 0.005052 0.045914 0.016909
+v -0.005460 0.045677 0.021971
+v -0.003608 0.045270 0.023501
+v -0.004995 0.045392 0.023098
+v 0.005460 0.045677 0.021977
+v 0.003608 0.045270 0.023501
+v 0.004996 0.045392 0.023099
+v 0.000000 0.062972 0.020980
+v 0.000000 0.038527 0.023966
+vn 0.9802 -0.0302 -0.1954
+vn 0.9866 0.0386 0.1584
+vn 0.7770 -0.5891 0.2217
+vn 0.7235 -0.6819 -0.1074
+vn 0.1472 0.0772 -0.9861
+vn 0.1452 -0.1337 -0.9803
+vn -0.1452 -0.1337 -0.9803
+vn -0.1472 0.0772 -0.9861
+vn 0.0944 0.4928 0.8650
+vn -0.0946 0.4927 0.8650
+vn -0.1396 -0.0670 0.9879
+vn 0.1392 -0.0670 0.9880
+vn -0.1444 0.3729 0.9165
+vn 0.1440 0.3729 0.9166
+vn -0.7235 -0.6819 -0.1074
+vn -0.7770 -0.5892 0.2214
+vn -0.9867 0.0385 0.1581
+vn -0.9802 -0.0302 -0.1954
+vn 0.9821 0.0148 -0.1877
+vn 0.9821 -0.0130 0.1876
+vn 0.1159 0.8265 -0.5509
+vn -0.1159 0.8265 -0.5509
+vn -0.9821 0.0148 -0.1877
+vn -0.9822 -0.0130 0.1873
+vn -0.7237 0.6040 0.3338
+vn -0.7388 0.6709 0.0633
+vn 0.7388 0.6709 0.0632
+vn 0.7237 0.6039 0.3340
+vn -0.1404 -0.3648 0.9204
+vn 0.1400 -0.3650 0.9204
+vn 0.0978 -0.7696 -0.6309
+vn -0.0978 -0.7696 -0.6309
+vn 0.5433 0.7359 -0.4040
+vn 0.6724 0.0579 -0.7378
+vn -0.6724 0.0579 -0.7378
+vn -0.5433 0.7359 -0.4040
+vn -0.6623 -0.1034 -0.7421
+vn 0.5222 -0.6927 -0.4974
+vn 0.6623 -0.1034 -0.7421
+vn 0.5124 0.4665 0.7209
+vn 0.6760 -0.0501 0.7352
+vn -0.6765 -0.0501 0.7347
+vn -0.5128 0.4666 0.7206
+vn -0.6982 0.2655 0.6648
+vn 0.5524 -0.3917 0.7357
+vn 0.6977 0.2658 0.6653
+vn -0.5222 -0.6927 -0.4974
+vn -0.5529 -0.3920 0.7352
+vn 0.0000 0.9505 0.3106
+vn 0.0000 -1.0000 0.0000
+s 1
+f 399//359 404//360 377//361 363//362
+f 372//363 398//364 395//365 365//366
+f 383//367 387//368 393//369 389//370
+f 389//370 393//369 402//371 405//372
+f 360//373 381//374 401//375 396//376
+f 371//377 390//378 404//360 399//359
+f 375//379 372//363 365//366 368//380
+f 366//381 392//382 386//383 369//384
+f 374//385 384//386 390//378 371//377
+f 405//372 402//371 380//387 378//388
+f 398//364 362//389 359//390 395//365
+f 396//376 401//375 392//382 366//381
+f 372//363 375//379 376//391 373//392
+f 373//392 376//391 374//385 371//377
+f 368//380 365//366 367//393 370//394
+f 370//394 367//393 366//381 369//384
+f 396//376 366//381 367//393 397//395
+f 397//395 367//393 365//366 395//365
+f 399//359 363//362 364//396 400//397
+f 400//397 364//396 362//389 398//364
+f 390//378 384//386 385//398 391//399
+f 391//399 385//398 383//367 389//370
+f 386//383 392//382 394//400 388//401
+f 388//401 394//400 393//369 387//368
+f 402//371 393//369 394//400 403//402
+f 403//402 394//400 392//382 401//375
+f 405//372 378//388 379//403 406//404
+f 406//404 379//403 377//361 404//360
+f 360//373 396//376 397//395 361//405
+f 361//405 397//395 395//365 359//390
+f 371//377 399//359 400//397 373//392
+f 373//392 400//397 398//364 372//363
+f 380//387 402//371 403//402 382//406
+f 382//406 403//402 401//375 381//374
+f 389//370 405//372 406//404 391//399
+f 391//399 406//404 404//360 390//378
+f 388//401 387//368 407//407
+f 374//385 376//391 407//407
+f 385//398 384//386 407//407
+f 387//368 383//367 407//407
+f 384//386 374//385 407//407
+f 368//380 370//394 407//407
+f 383//367 385//398 407//407
+f 369//384 386//383 407//407
+f 370//394 369//384 407//407
+f 386//383 388//401 407//407
+f 375//379 368//380 407//407
+f 376//391 375//379 407//407
+f 364//396 363//362 408//408
+f 361//405 359//390 408//408
+f 381//374 360//373 408//408
+f 382//406 381//374 408//408
+f 377//361 379//403 408//408
+f 360//373 361//405 408//408
+f 363//362 377//361 408//408
+f 359//390 362//389 408//408
+f 379//403 378//388 408//408
+f 362//389 364//396 408//408
+f 378//388 380//387 408//408
+f 380//387 382//406 408//408
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/__init__.py b/examples/pybullet/gym/pybullet_envs/minitaur/__init__.py
index 8b1378917..e69de29bb 100644
--- a/examples/pybullet/gym/pybullet_envs/minitaur/__init__.py
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/__init__.py
@@ -1 +0,0 @@
-
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/agents/__init__.py b/examples/pybullet/gym/pybullet_envs/minitaur/agents/__init__.py
index 8b1378917..e69de29bb 100644
--- a/examples/pybullet/gym/pybullet_envs/minitaur/agents/__init__.py
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/agents/__init__.py
@@ -1 +0,0 @@
-
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/__init__.py b/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/__init__.py
new file mode 100644
index 000000000..e69de29bb
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/com_height_estimator.py b/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/com_height_estimator.py
new file mode 100644
index 000000000..3838c74aa
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/com_height_estimator.py
@@ -0,0 +1,76 @@
+"""State estimator for robot height."""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+import copy
+from typing import Any, Sequence
+
+import gin
+
+from pybullet_envs.minitaur.agents.baseline_controller import state_estimator
+
+
+@gin.configurable
+class COMHeightEstimator(state_estimator.StateEstimatorBase):
+ """Estimate the CoM height using base orientation and local toe positions."""
+
+ def __init__(
+ self,
+ robot: Any,
+ com_estimate_leg_indices: Sequence[int] = (0, 1, 2, 3),
+ initial_com_height: float = 0.45,
+ ):
+ """Initializes the class.
+
+ Args:
+ robot: A quadruped robot.
+ com_estimate_leg_indices: Leg indices used in estimating the CoM height.
+ initial_com_height: CoM height used during reset.
+ """
+ self._robot = robot
+ self._com_estimate_leg_indices = com_estimate_leg_indices
+ self._initial_com_estimate_leg_indices = copy.copy(com_estimate_leg_indices)
+ self._initial_com_height = initial_com_height
+ self.reset(0)
+
+ @property
+ def estimated_com_height(self):
+ return self._com_height
+
+ def reset(self, current_time):
+ del current_time
+ self._com_height = self._initial_com_height
+ self._com_estimate_leg_indices = copy.copy(
+ self._initial_com_estimate_leg_indices)
+
+ def update(self, current_time):
+ del current_time
+ local_toe_poses = self._robot.foot_positions(
+ position_in_world_frame=False)
+ # We rotate the local toe positions into the world orientation centered
+ # at the robot base to estimate the height of the robot.
+ world_toe_poses = []
+ for toe_p in local_toe_poses:
+ world_toe_poses.append(
+ self._robot.pybullet_client.multiplyTransforms(
+ positionA=(0, 0, 0),
+ orientationA=self._robot.base_orientation_quaternion,
+ positionB=toe_p,
+ orientationB=(0, 0, 0, 1))[0])
+ mean_height = 0.0
+ num_toe_in_contact = 0
+ for leg_id in self._com_estimate_leg_indices:
+ mean_height += world_toe_poses[leg_id][2]
+ num_toe_in_contact += 1
+ mean_height /= num_toe_in_contact
+ self._com_height = abs(mean_height)
+
+ @property
+ def com_estimate_leg_indices(self):
+ return self._com_estimate_leg_indices
+
+ @com_estimate_leg_indices.setter
+ def com_estimate_leg_indices(self, leg_indices):
+ self._com_estimate_leg_indices = leg_indices
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/com_velocity_estimator.py b/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/com_velocity_estimator.py
new file mode 100644
index 000000000..609f83271
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/com_velocity_estimator.py
@@ -0,0 +1,83 @@
+"""State estimator."""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+from typing import Any, Sequence
+
+import gin
+import numpy as np
+
+from pybullet_envs.minitaur.agents.baseline_controller import state_estimator
+from pybullet_envs.minitaur.robots.safety.python import moving_window_filter
+
+_DEFAULT_WINDOW_SIZE = 20
+
+
+@gin.configurable
+class COMVelocityEstimator(state_estimator.StateEstimatorBase):
+ """Estimate the CoM velocity using on board sensors.
+
+
+ Requires knowledge about the base velocity in world frame, which for example
+ can be obtained from a MoCap system. This estimator will filter out the high
+ frequency noises in the velocity so the results can be used with controllers
+ reliably.
+
+ """
+
+ def __init__(
+ self,
+ robot: Any,
+ window_size: int = _DEFAULT_WINDOW_SIZE,
+ ):
+ self._robot = robot
+ self._window_size = window_size
+ self.reset(0)
+
+ @property
+ def com_velocity_body_yaw_aligned_frame(self) -> Sequence[float]:
+ """The base velocity projected in the body aligned inertial frame.
+
+ The body aligned frame is a intertia frame that coincides with the body
+ frame, but has a zero relative velocity/angular velocity to the world frame.
+
+ Returns:
+ The com velocity in body aligned frame.
+ """
+ return self._com_velocity_body_yaw_aligned_frame
+
+ @property
+ def com_velocity_world_frame(self) -> Sequence[float]:
+ return self._com_velocity_world_frame
+
+ def reset(self, current_time):
+ del current_time
+ # We use a moving window filter to reduce the noise in velocity estimation.
+ self._velocity_filter_x = moving_window_filter.MovingWindowFilter(
+ window_size=self._window_size)
+ self._velocity_filter_y = moving_window_filter.MovingWindowFilter(
+ window_size=self._window_size)
+ self._velocity_filter_z = moving_window_filter.MovingWindowFilter(
+ window_size=self._window_size)
+ self._com_velocity_world_frame = np.array((0, 0, 0))
+ self._com_velocity_body_yaw_aligned_frame = np.array((0, 0, 0))
+
+ def update(self, current_time):
+ del current_time
+ velocity = self._robot.base_velocity
+
+ vx = self._velocity_filter_x.CalculateAverage(velocity[0])
+ vy = self._velocity_filter_y.CalculateAverage(velocity[1])
+ vz = self._velocity_filter_z.CalculateAverage(velocity[2])
+ self._com_velocity_world_frame = np.array((vx, vy, vz))
+
+ base_orientation = self._robot.base_orientation_quaternion
+ _, inverse_rotation = self._robot.pybullet_client.invertTransform(
+ (0, 0, 0), base_orientation)
+
+ self._com_velocity_body_yaw_aligned_frame, _ = (
+ self._robot.pybullet_client.multiplyTransforms(
+ (0, 0, 0), inverse_rotation, self._com_velocity_world_frame,
+ (0, 0, 0, 1)))
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/dummy_gait_generator.py b/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/dummy_gait_generator.py
new file mode 100644
index 000000000..51747904f
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/dummy_gait_generator.py
@@ -0,0 +1,87 @@
+"""A dummy gait generator module for storing gait patterns from higher-level controller."""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import google_type_annotations
+from __future__ import print_function
+
+import copy
+from typing import Any, Sequence
+
+import gin
+
+from pybullet_envs.minitaur.agents.baseline_controller import gait_generator
+
+LAIKAGO_STANDING = (
+ gait_generator.LegState.STANCE,
+ gait_generator.LegState.STANCE,
+ gait_generator.LegState.STANCE,
+ gait_generator.LegState.STANCE,
+)
+
+
+@gin.configurable
+class DummyGaitGenerator(gait_generator.GaitGenerator):
+ """A module for storing quadruped gait patterns from high-level controller.
+
+ This module stores the state for each leg of a quadruped robot. The data is
+ used by the stance leg controller to determine the appropriate contact forces.
+ A high-level controller, such as a neural network policy, can be used to
+ control the gait pattern and set corresponding leg states.
+ """
+
+ def __init__(
+ self,
+ robot: Any,
+ initial_leg_state: Sequence[gait_generator.LegState] = LAIKAGO_STANDING,
+ ):
+ """Initializes the class.
+
+ Args:
+ robot: A quadruped robot.
+ initial_leg_state: The desired initial swing/stance state of legs indexed
+ by their id.
+ """
+ self._robot = robot
+ if len(initial_leg_state) != len(
+ list(self._robot.urdf_loader.get_end_effector_id_dict().values())):
+ raise ValueError(
+ "The number of leg states should be the same of number of legs.")
+ self._initial_leg_state = initial_leg_state
+ self._leg_state = list(initial_leg_state)
+ self._desired_leg_state = list(initial_leg_state)
+
+ self.reset(0)
+
+ def reset(self, current_time):
+ del current_time
+ self._leg_state = list(self._initial_leg_state)
+ self._desired_leg_state = list(self._initial_leg_state)
+
+ @property
+ def desired_leg_state(self) -> Sequence[gait_generator.LegState]:
+ """The desired leg SWING/STANCE states.
+
+ Returns:
+ The SWING/STANCE states for all legs.
+
+ """
+ return self._desired_leg_state
+
+ @desired_leg_state.setter
+ def desired_leg_state(self, state):
+ self._desired_leg_state = copy.deepcopy(state)
+
+ @property
+ def leg_state(self) -> Sequence[gait_generator.LegState]:
+ """The leg state after considering contact with ground.
+
+ Returns:
+ The actual state of each leg after accounting for contacts.
+ """
+ return self._leg_state
+
+ @leg_state.setter
+ def leg_state(self, state):
+ self._leg_state = copy.deepcopy(state)
+
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/foot_stepper.py b/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/foot_stepper.py
new file mode 100644
index 000000000..8a79a236f
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/foot_stepper.py
@@ -0,0 +1,199 @@
+# Lint as: python3
+"""A state machine that steps each foot for a static gait. Experimental code."""
+
+import copy
+import math
+
+import numpy as np
+
+
+class StepInput(object):
+
+ def __init__(self):
+ self.base_com_pos = np.array([0, 0, 0])
+ self.base_com_orn = np.array([0, 0, 0, 1])
+ self.toe_pos_world = np.array([0, 0, 0] * 4)
+ self.new_pos_world = np.array([0, 0, 0])
+
+
+class StepOutput(object):
+
+ def __init__(self, new_toe_pos_world):
+ self.new_toe_pos_world = new_toe_pos_world
+
+
+class FootStepper(object):
+ """This class computes desired foot placement for a quadruped robot."""
+
+ def __init__(self, bullet_client, toe_ids, toe_pos_local_ref):
+ self.bullet_client = bullet_client
+ self.state_time = 0.
+ self.toe_ids = toe_ids
+ self.toe_pos_local_ref = toe_pos_local_ref
+ self.sphere_uid = self.bullet_client.loadURDF(
+ "sphere_small.urdf", [0, 0, 0], useFixedBase=True)
+ self.is_far = True
+ self.max_shift = 0.0008
+ self.far_bound = 0.005
+ self.close_bound = 0.03
+
+ self.move_swing_foot = False
+ self.amp = 0.2
+ alpha = 1
+
+ # Loads/draws spheres for debugging purpose. The spheres visualize the
+ # target COM, the current COM and the target foothold location.
+ self.sphere_uid_centroid = self.bullet_client.loadURDF(
+ "sphere_small.urdf", [0, 0, 0], useFixedBase=True)
+ self.bullet_client.changeVisualShape(
+ self.sphere_uid_centroid, -1, rgbaColor=[1, 1, 0, alpha])
+
+ # Disable collision since visualization spheres should not collide with the
+ # robot.
+ self.bullet_client.setCollisionFilterGroupMask(self.sphere_uid_centroid, -1,
+ 0, 0)
+
+ self.sphere_uid_com = self.bullet_client.loadURDF(
+ "sphere_small.urdf", [0, 0, 0], useFixedBase=True)
+ self.bullet_client.changeVisualShape(
+ self.sphere_uid_com, -1, rgbaColor=[1, 0, 1, alpha])
+ self.bullet_client.setCollisionFilterGroupMask(self.sphere_uid_com, -1, 0,
+ 0)
+
+ self.bullet_client.setCollisionFilterGroupMask(self.sphere_uid, -1, 0, 0)
+ self.feetindices = [1, 3, 0, 2]
+ self.swing_foot_index1 = 0
+ self.swing_foot_index = self.feetindices[self.swing_foot_index1]
+ self.colors = [[1, 0, 0, 1], [0, 1, 0, 1], [0, 0, 1, 1], [1, 1, 1, 1]]
+ self.support_vertices = [[1, 2, 3], [0, 2, 3], [0, 1, 3], [0, 1, 2]]
+ self.local_diff_y_threshold = 0.05
+ self.local_diff_y = 100
+ self.is_far = True
+ self.get_reference_pos_swing_foot()
+
+ def next_foot(self):
+ self.swing_foot_index1 = (self.swing_foot_index1 + 1) % 4
+ self.swing_foot_index = self.feetindices[self.swing_foot_index1]
+
+ def swing_foot(self):
+ self.move_swing_foot = True
+
+ def get_reference_pos_swing_foot(self):
+ self.new_pos_local = np.array(
+ self.toe_pos_local_ref[self.swing_foot_index])
+ return self.new_pos_local
+
+ def set_reference_pos_swing_foot(self, new_pos_local):
+ self.new_pos_local = new_pos_local
+
+ def is_com_stable(self):
+ ld2 = self.local_diff_y * self.local_diff_y
+ yaw_ok = ld2 < (self.local_diff_y_threshold * self.local_diff_y_threshold)
+ com_ok = not self.is_far
+ return com_ok and yaw_ok
+
+ def update(self, step_input):
+ """Updates the state machine and toe movements per state."""
+ base_com_pos = step_input.base_com_pos
+ base_com_orn = step_input.base_com_orn
+ base_com_pos_inv, base_com_orn_inv = self.bullet_client.invertTransform(
+ base_com_pos, base_com_orn)
+
+ dt = step_input.dt
+ self.bullet_client.resetBasePositionAndOrientation(self.sphere_uid,
+ step_input.new_pos_world,
+ [0, 0, 0, 1])
+ self.bullet_client.changeVisualShape(
+ self.sphere_uid, -1, rgbaColor=self.colors[self.swing_foot_index])
+
+ all_toes_pos_locals = []
+ for toe_pos_world in step_input.toe_pos_world:
+ toe_pos_local, _ = self.bullet_client.multiplyTransforms(
+ base_com_pos_inv, base_com_orn_inv, toe_pos_world, [0, 0, 0, 1])
+ all_toes_pos_locals.append(toe_pos_local)
+ all_toes_pos_locals = np.array(all_toes_pos_locals)
+ centroid_world = np.zeros(3)
+ for v in self.support_vertices[self.swing_foot_index]:
+ vtx_pos_world = step_input.toe_pos_world[v]
+ centroid_world += vtx_pos_world
+ centroid_world /= 3.
+
+ sphere_z_offset = 0.05
+ self.diff_world = base_com_pos - centroid_world
+ self.diff_world[2] = 0.
+ self.bullet_client.resetBasePositionAndOrientation(self.sphere_uid_centroid,
+ centroid_world,
+ [0, 0, 0, 1])
+ self.bullet_client.resetBasePositionAndOrientation(
+ self.sphere_uid_com,
+ [base_com_pos[0], base_com_pos[1], sphere_z_offset], [0, 0, 0, 1])
+
+ l = np.linalg.norm(self.diff_world)
+ if self.is_far:
+ bound = self.far_bound
+ else:
+ bound = self.close_bound
+
+ if l > bound:
+ self.diff_world *= self.max_shift * 0.5 / l
+ if not self.is_far:
+ self.is_far = True
+ else:
+ if self.is_far:
+ self.is_far = False
+
+ if not self.is_far:
+ self.diff_world = np.zeros(3)
+ for i in range(len(self.toe_pos_local_ref)):
+ toe = self.toe_pos_local_ref[i]
+ toe = [
+ toe[0] + self.diff_world[0], toe[1] + self.diff_world[1],
+ toe[2] + self.diff_world[2]
+ ]
+ self.toe_pos_local_ref[i] = toe
+
+ self.local_diff_y = self.toe_pos_local_ref[0][
+ 1] + self.toe_pos_local_ref[1][1] - self.toe_pos_local_ref[
+ 2][1] - self.toe_pos_local_ref[3][1]
+
+ self.yaw = 0
+ if self.local_diff_y < -self.local_diff_y_threshold:
+ self.yaw = 0.001
+ if self.local_diff_y > self.local_diff_y_threshold:
+ self.yaw = -0.001
+
+ yaw_trans = self.bullet_client.getQuaternionFromEuler([0, 0, self.yaw])
+
+ if not self.is_far:
+ for i in range(len(self.toe_pos_local_ref)):
+ toe = self.toe_pos_local_ref[i]
+ toe, _ = self.bullet_client.multiplyTransforms([0, 0, 0], yaw_trans,
+ toe, [0, 0, 0, 1])
+ self.toe_pos_local_ref[i] = toe
+
+ new_toe_pos_world = []
+
+ # Moves the swing foot to the target location.
+ if self.move_swing_foot:
+ if self.state_time <= 1:
+ self.state_time += 4 * dt
+ if self.state_time >= 1:
+ self.move_swing_foot = False
+ self.state_time = 0
+ self.toe_pos_local_ref[self.swing_foot_index] = self.new_pos_local
+ toe_pos_local_ref_copy = copy.deepcopy(self.toe_pos_local_ref)
+ old_pos = self.toe_pos_local_ref[self.swing_foot_index]
+ new_pos = [
+ old_pos[0] * (1 - self.state_time) + self.new_pos_local[0] *
+ (self.state_time), old_pos[1] * (1 - self.state_time) +
+ self.new_pos_local[1] * (self.state_time),
+ old_pos[2] * (1 - self.state_time) + self.new_pos_local[2] *
+ (self.state_time) + self.amp * math.sin(self.state_time * math.pi)
+ ]
+ toe_pos_local_ref_copy[self.swing_foot_index] = new_pos
+ for toe_pos_local in toe_pos_local_ref_copy:
+ new_toe_pos_world.append(self.bullet_client.multiplyTransforms(
+ base_com_pos, base_com_orn, toe_pos_local, [0, 0, 0, 1])[0])
+
+ step_output = StepOutput(new_toe_pos_world)
+ return step_output
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/gait_generator.py b/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/gait_generator.py
new file mode 100644
index 000000000..61bd849ac
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/gait_generator.py
@@ -0,0 +1,32 @@
+"""Gait pattern planning module."""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+import abc
+import enum
+
+
+class LegState(enum.Enum):
+ """The state of a leg during locomotion."""
+ SWING = 0
+ STANCE = 1
+ # A swing leg that collides with the ground.
+ EARLY_CONTACT = 2
+ # A stance leg that loses contact.
+ LOSE_CONTACT = 3
+
+
+class GaitGenerator(object): # pytype: disable=ignored-metaclass
+ """Generates the leg swing/stance pattern for the robot."""
+
+ __metaclass__ = abc.ABCMeta
+
+ @abc.abstractmethod
+ def reset(self, current_time):
+ pass
+
+ @abc.abstractmethod
+ def update(self, current_time):
+ pass
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/imu_based_com_velocity_estimator.py b/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/imu_based_com_velocity_estimator.py
new file mode 100644
index 000000000..4caea0906
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/imu_based_com_velocity_estimator.py
@@ -0,0 +1,216 @@
+"""State estimator."""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+from typing import Any, Sequence
+
+from filterpy import kalman
+import gin
+import numpy as np
+
+from pybullet_envs.minitaur.agents.baseline_controller import state_estimator
+from pybullet_envs.minitaur.agents.baseline_controller import time_based_moving_window_filter
+from pybullet_envs.minitaur.envs_v2.sensors import accelerometer_sensor
+from pybullet_envs.minitaur.envs_v2.sensors import imu_sensor
+
+_DEFAULT_VELOCITY_FILTER_WINDOW = 0.2
+_DEFAULT_GYRO_FILTER_WINDOW = 0.1
+_DEFAULT_VELOCITY_CLIPPING = 0.8
+_STATE_DIMENSION = 3
+_GRAVITY = (0.0, 0.0, -9.8)
+
+
+@gin.configurable
+class IMUBasedCOMVelocityEstimator(state_estimator.StateEstimatorBase):
+ """Estimate the CoM velocity using IMU sensors and velocities of stance feet.
+
+
+ Estimates the com velocity of the robot using IMU data and stance feet
+ velocities fused by a Kalman Filter. Kalman Filter assumes the true state x
+ follows a linear dynamics: x'=Fx+Bu+w, where x' and x denots the
+ current and previous state of the robot, F is the state transition matrix,
+ B is the control-input matrix, and w is the noise in the dynamics, assuming to
+ follow a zero-mean multivariate normal distribution with covariance Q. It also
+ assumes that we can obtain a noisy observation of the state with the model
+ z=Hx+v, where z is the observed state, H is the observation matrix, and v is
+ a noise model, assuming to follow a zero-mean multivariate normal distribution
+ with covariance R. In our case, x is the CoM velocity of the robot,
+ F=B=H=eye(3), u=dt * CoM acceleration, which is obtained from accelerometer,
+ and the noisy observation z is obtained from the negated average velocities at
+ the end-effectors in contact with the ground.
+
+ """
+
+ def __init__(
+ self,
+ robot: Any,
+ use_sensor_interface: bool = True,
+ accelerometer_variance=0.1,
+ observation_variance=0.1,
+ initial_variance=0.1,
+ velocity_filter_window: float = _DEFAULT_VELOCITY_FILTER_WINDOW,
+ gyroscope_filter_window: float = _DEFAULT_GYRO_FILTER_WINDOW,
+ contact_detection_threshold: float = 0.0,
+ velocity_clipping: float = _DEFAULT_VELOCITY_CLIPPING,
+ ):
+ """Initializes the class.
+
+ Args:
+ robot: A quadruped robot.
+ use_sensor_interface: Whether to use the sensor interface to obtain the
+ IMU readings or directly get them from the robot class. Former should
+ be used in simulation to enable added latency and noise while latter
+ should be used on real robot for better performance.
+ accelerometer_variance: The estimated variance in the accelerometer
+ readings, used in the Kalman Filter.
+ observation_variance: The estimated variance in the observed CoM velocity
+ from the stance feet velocities, used in the Kalman Filter.
+ initial_variance: The variance of the initial distribution for the
+ estimated CoM variance.
+ velocity_filter_window: The filtering window (in time) used to smooth the
+ estimated CoM velocity.
+ gyroscope_filter_window: The filtering window (in time) used to smooth the
+ input gyroscope readings.
+ contact_detection_threshold: Threshold on the contact sensor readings to
+ determine whether the foot is in contact with the ground.
+ velocity_clipping: Clipping value for the estimated velocity to prevent
+ unrealistically large velocity estimations.
+ """
+ self._robot = robot
+ self._contact_detection_threshold = contact_detection_threshold
+ self._velocity_clipping = velocity_clipping
+ self._use_sensor_interface = use_sensor_interface
+
+ # Use the accelerometer and gyroscope sensor from the robot
+ if self._use_sensor_interface:
+ for sensor in self._robot.sensors:
+ if isinstance(sensor, accelerometer_sensor.AccelerometerSensor):
+ self._accelerometer = sensor
+ if isinstance(sensor, imu_sensor.IMUSensor):
+ self._gyroscope = sensor
+ assert hasattr(self, "_accelerometer") and self._accelerometer is not None
+ assert hasattr(self, "_gyroscope") and self._gyroscope is not None
+
+ # x is the underlying CoM velocity we want to estimate, z is the observed
+ # CoM velocity from the stance feet velocities, and u is the accelerometer
+ # readings.
+ self._filter = kalman.KalmanFilter(
+ dim_x=_STATE_DIMENSION,
+ dim_z=_STATE_DIMENSION,
+ dim_u=_STATE_DIMENSION)
+ # Initialize the state distribution to be a zero-mean multi-variate normal
+ # distribution with initial variance.
+ self._filter.x = np.zeros(_STATE_DIMENSION)
+ self._initial_variance = initial_variance
+ self._filter.P = np.eye(_STATE_DIMENSION) * self._initial_variance
+ # Covariance matrix for the control variable.
+ self._filter.Q = np.eye(_STATE_DIMENSION) * accelerometer_variance
+ # Covariance matrix for the observed states.
+ self._filter.R = np.eye(_STATE_DIMENSION) * observation_variance
+
+ # observation function (z=H*x+N(0,R))
+ self._filter.H = np.eye(_STATE_DIMENSION)
+ # state transition matrix (x'=F*x+B*u+N(0,Q))
+ self._filter.F = np.eye(_STATE_DIMENSION)
+ # Control transition matrix
+ self._filter.B = np.eye(_STATE_DIMENSION)
+
+ self._velocity_filter = time_based_moving_window_filter.TimeBasedMovingWindowFilter(
+ velocity_filter_window)
+ self._gyroscope_filter = time_based_moving_window_filter.TimeBasedMovingWindowFilter(
+ gyroscope_filter_window)
+
+ self.reset(0)
+
+ @property
+ def com_velocity_body_yaw_aligned_frame(self) -> Sequence[float]:
+ """The base velocity projected in the body yaw aligned inertial frame.
+
+ The body yaw aligned frame is a intertia frame where the z axis coincides
+ with the yaw of the robot base and the x and y axis coincides with the world
+ frame. It has a zero relative velocity/angular velocity
+ to the world frame.
+
+ Returns:
+ The com velocity in body yaw aligned frame.
+ """
+ clipped_velocity = np.clip(self._com_velocity_body_yaw_aligned_frame,
+ -self._velocity_clipping,
+ self._velocity_clipping)
+
+ return clipped_velocity
+
+ def reset(self, current_time):
+ del current_time
+ self._filter.x = np.zeros(_STATE_DIMENSION)
+ self._filter.P = np.eye(_STATE_DIMENSION) * self._initial_variance
+
+ self._com_velocity_body_yaw_aligned_frame = np.zeros(_STATE_DIMENSION)
+
+ self._velocity_filter.reset()
+ self._gyroscope_filter.reset()
+
+ # Use None instead of 0 in case of a big gap between reset and first step.
+ self._last_timestamp = None
+
+ def update(self, current_time):
+ del current_time
+ current_timestamp = self._robot.timestamp
+
+ # First time step
+ if self._last_timestamp is None:
+ delta_time_s = 0.0
+ else:
+ delta_time_s = current_timestamp - self._last_timestamp
+ self._last_timestamp = current_timestamp
+
+ if self._use_sensor_interface:
+ sensor_acc = np.array(self._accelerometer.get_observation())
+ gyroscope_reading = self._gyroscope.get_observation()
+ else:
+ sensor_acc = np.array(self._robot.base_acceleration_accelerometer)
+ gyroscope_reading = self._robot.base_roll_pitch_yaw
+
+ filtered_gyroscope_reading = self._gyroscope_filter.calculate_average(
+ gyroscope_reading, current_timestamp)
+ # The yaw angle is not used here because reliably estimating the yaw angle
+ # of the robot is in general difficult. This leads to a body yaw aligned
+ # inertia frame for the estimated velocity.
+ yaw_aligned_base_orientation = self._robot.pybullet_client.getQuaternionFromEuler(
+ (filtered_gyroscope_reading[0], filtered_gyroscope_reading[1], 0.0))
+
+ rot_mat = self._robot.pybullet_client.getMatrixFromQuaternion(
+ yaw_aligned_base_orientation)
+ rot_mat = np.array(rot_mat).reshape((_STATE_DIMENSION, _STATE_DIMENSION))
+ calibrated_acc = rot_mat.dot(sensor_acc) + np.array(_GRAVITY)
+ self._filter.predict(u=calibrated_acc * delta_time_s)
+ observed_velocities = []
+
+ foot_contact = [
+ np.linalg.norm(contact_force) > self._contact_detection_threshold
+ for contact_force in self._robot.feet_contact_forces()
+ ]
+ for leg_id in range(4):
+ if foot_contact[leg_id]:
+ jacobian = self._robot.compute_jacobian_for_one_leg(leg_id)
+ # Only pick the jacobian related to joint motors
+ # TODO(magicmelon): standardize the process of picking out relevant dofs
+ com_dof = self._robot.urdf_loader.com_dof
+ jacobian = jacobian[:,
+ com_dof + leg_id * 3:com_dof + (leg_id + 1) * 3]
+ joint_velocities = self._robot.motor_velocities[leg_id *
+ 3:(leg_id + 1) * 3]
+ leg_velocity_in_base_frame = jacobian.dot(joint_velocities)
+ base_velocity_in_base_frame = -leg_velocity_in_base_frame
+ observed_velocities.append(rot_mat.dot(base_velocity_in_base_frame))
+
+ if observed_velocities:
+ observed_velocities = np.mean(observed_velocities, axis=0)
+ self._filter.update(observed_velocities)
+
+ velocity = self._filter.x.copy()
+
+ self._com_velocity_body_yaw_aligned_frame = self._velocity_filter.calculate_average(
+ velocity, current_timestamp)
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/leg_controller.py b/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/leg_controller.py
new file mode 100644
index 000000000..731f7ae9f
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/leg_controller.py
@@ -0,0 +1,29 @@
+"""The leg controller class interface."""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+import abc
+from typing import Any
+
+
+class LegController(object): # pytype: disable=ignored-metaclass
+ """Generates the leg control signal."""
+
+ __metaclass__ = abc.ABCMeta
+
+ @abc.abstractmethod
+ def reset(self, current_time: float):
+ """Resets the controller's internal state."""
+ pass
+
+ @abc.abstractmethod
+ def update(self, current_time: float):
+ """Updates the controller's internal state."""
+ pass
+
+ @abc.abstractmethod
+ def get_action(self) -> Any:
+ """Gets the control signal e.g. torques/positions for the leg."""
+ pass
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/locomotion_controller.py b/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/locomotion_controller.py
new file mode 100644
index 000000000..d9bca69f5
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/locomotion_controller.py
@@ -0,0 +1,96 @@
+"""A model based controller framework."""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+import time
+from typing import Any, Callable
+
+from pybullet_envs.minitaur.agents.baseline_controller import gait_generator as gait_generator_lib
+from pybullet_envs.minitaur.agents.baseline_controller import leg_controller as leg_controller_lib
+from pybullet_envs.minitaur.agents.baseline_controller import state_estimator as state_estimator_lib
+
+
+class LocomotionController(object):
+ """Generates the quadruped locomotion.
+
+ The actual effect of this controller depends on the composition of each
+ individual subcomponent.
+
+ """
+
+ def __init__(
+ self,
+ robot: Any,
+ gait_generator: gait_generator_lib.GaitGenerator,
+ state_estimator: state_estimator_lib.StateEstimatorBase,
+ swing_leg_controller: leg_controller_lib.LegController,
+ stance_leg_controller: leg_controller_lib.LegController,
+ clock: Callable[[], float] = None,
+ ):
+ """Initializes the class.
+
+ Args:
+ robot: A robot instance.
+ gait_generator: Generates the leg swing/stance pattern.
+ state_estimator: Estimates the state of the robot (e.g. center of mass
+ position or velocity that may not be observable from sensors).
+ swing_leg_controller: Generates motor actions for swing legs.
+ stance_leg_controller: Generates motor actions for stance legs.
+ clock: A real or fake clock source.
+ """
+ self._robot = robot
+ self._clock = clock
+ if clock is None:
+ self._clock = time.time
+ self._reset_time = self._clock()
+ self._time_since_reset = 0
+ self._gait_generator = gait_generator
+ self._state_estimator = state_estimator
+ self._swing_leg_controller = swing_leg_controller
+ self._stance_leg_controller = stance_leg_controller
+
+ @property
+ def swing_leg_controller(self):
+ return self._swing_leg_controller
+
+ @property
+ def stance_leg_controller(self):
+ return self._stance_leg_controller
+
+ @property
+ def gait_generator(self):
+ return self._gait_generator
+
+ @property
+ def state_estimator(self):
+ return self._state_estimator
+
+ def reset(self):
+ self._reset_time = self._clock()
+ self._time_since_reset = 0
+ self._gait_generator.reset(self._time_since_reset)
+ self._state_estimator.reset(self._time_since_reset)
+ self._swing_leg_controller.reset(self._time_since_reset)
+ self._stance_leg_controller.reset(self._time_since_reset)
+
+ def update(self):
+ self._time_since_reset = self._clock() - self._reset_time
+ self._gait_generator.update(self._time_since_reset)
+ self._state_estimator.update(self._time_since_reset)
+ self._swing_leg_controller.update(self._time_since_reset)
+ self._stance_leg_controller.update(self._time_since_reset)
+
+ def get_action(self):
+ """Returns the control ouputs (e.g. positions/torques) for all motors."""
+ swing_action = self._swing_leg_controller.get_action()
+ stance_action = self._stance_leg_controller.get_action()
+ action = []
+ for joint_id in range(self._robot.num_motors):
+ if joint_id in swing_action:
+ action.extend(swing_action[joint_id])
+ else:
+ assert joint_id in stance_action
+ action.extend(stance_action[joint_id])
+ return action
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/locomotion_controller_example.py b/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/locomotion_controller_example.py
new file mode 100644
index 000000000..fd867950d
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/locomotion_controller_example.py
@@ -0,0 +1,190 @@
+r"""Laikago walking example using the locomotion controller framework.
+
+"""
+
+import gc
+import pickle
+
+from absl import app
+from absl import flags
+import numpy as np
+import scipy.interpolate
+
+
+from pybullet_envs.minitaur.agents.baseline_controller import locomotion_controller_setup
+from pybullet_envs.minitaur.envs_v2 import env_loader
+from pybullet_envs.minitaur.robots import robot_config
+
+FLAGS = flags.FLAGS
+flags.DEFINE_boolean("run_on_robot", False,
+ "whether to run in sim or on real hardware")
+flags.DEFINE_boolean(
+ "use_ground_truth_velocity", False,
+ "whether to use a ground truth velocity estimator (available in sim)")
+flags.DEFINE_enum("gait", "fast_trot",
+ ["fast_trot", "slow_trot", "walk", "stand"],
+ "The gait pattern to use")
+flags.DEFINE_boolean(
+ "use_keyboard_control", False,
+ "whether to use a keyboard to control or demo speed profile.")
+flags.DEFINE_string("log_path", None, "Path to save robot logs")
+flags.DEFINE_boolean("add_random_push", False,
+ "whether to add random push to the robot in simulation")
+
+_MAX_TIME_SECONDS = 100
+
+
+def _load_config(render=True, run_on_robot=False):
+ """Builds the environment for the quadruped robot.
+
+ Args:
+ render: Enable/disable rendering.
+ run_on_robot: Whether deploy to robot or run in sim.
+ """
+ if run_on_robot:
+ locomotion_controller_setup.load_real_config()
+ else:
+ locomotion_controller_setup.load_sim_config(render)
+ if FLAGS.add_random_push:
+ locomotion_controller_setup.add_random_push_config()
+
+
+def _generate_example_linear_angular_speed(t):
+ """Creates an example speed profile based on time for demo purpose."""
+ vx = 0.1
+ vy = 0.1
+ wz = 0.3
+ time_points = (0, 4, 7, 11, 13, 15, 17, 19, 100)
+ speed_points = ((0, 0, 0, 0), (vx, 0, 0, 0), (-vx, 0, 0, 0), (0, -vy, 0, 0),
+ (0, vy, 0, 0), (0, 0, 0, wz), (0, 0, 0, -wz), (0, 0, 0, 0),
+ (0, 0, 0, 0))
+
+ speed = scipy.interpolate.interp1d(
+ time_points,
+ speed_points,
+ kind="previous",
+ fill_value="extrapolate",
+ axis=0)(
+ t)
+
+ return speed[0:3], speed[3]
+
+
+def _update_speed_from_kb(kb, lin_speed, ang_speed):
+ """Updates the controller behavior parameters."""
+ if kb.is_keyboard_hit():
+ c = kb.get_input_character()
+ if c == "w":
+ lin_speed += np.array((0.05, 0, 0))
+ if c == "s":
+ lin_speed += np.array((-0.05, 0, 0))
+ if c == "q":
+ ang_speed += 0.1
+ if c == "e":
+ ang_speed += -0.1
+ if c == "a":
+ lin_speed += np.array((0, 0.05, 0))
+ if c == "d":
+ lin_speed += np.array((0, -0.05, 0))
+ if c == "r":
+ lin_speed = np.array([0.0, 0.0, 0.0])
+ ang_speed = 0.0
+
+ lin_speed[0] = np.clip(lin_speed[0], -0.2, 0.4)
+ lin_speed[1] = np.clip(lin_speed[1], -0.2, 0.2)
+ ang_speed = np.clip(ang_speed, -0.3, 0.3)
+ print("desired speed: ", lin_speed, ang_speed)
+
+ return lin_speed, ang_speed
+
+
+def _update_controller_params(controller, lin_speed, ang_speed):
+ controller.swing_leg_controller.desired_speed = lin_speed
+ controller.swing_leg_controller.desired_twisting_speed = ang_speed
+ controller.stance_leg_controller.desired_speed = lin_speed
+ controller.stance_leg_controller.desired_twisting_speed = ang_speed
+
+
+def _run_example(max_time=_MAX_TIME_SECONDS,
+ run_on_robot=False,
+ use_keyboard=False):
+ """Runs the locomotion controller example."""
+ if use_keyboard:
+ kb = keyboard_utils.KeyboardInput()
+
+ env = env_loader.load()
+ env.reset()
+
+ # To mitigate jittering from the python
+ gc.collect()
+
+ # Wait for the robot to be placed properly.
+ if run_on_robot:
+ input("Press Enter to continue when robot is ready.")
+
+ lin_speed = np.array([0.0, 0.0, 0.0])
+ ang_speed = 0.0
+
+ controller = locomotion_controller_setup.setup_controller(
+ env.robot, FLAGS.gait, run_on_robot, FLAGS.use_ground_truth_velocity)
+ controller.reset()
+
+ loop_start_time = env.get_time_since_reset()
+ loop_elapsed_time = 0
+ robot_log = {
+ "timestamps": [],
+ "motor_angles": [],
+ "motor_velocities": [],
+ "base_velocities": [],
+ "foot_positions": [],
+ "base_rollpitchyaw": [],
+ "base_angular_velocities": [],
+ "actions": []
+ }
+ try:
+ while loop_elapsed_time < max_time:
+ #if use_keyboard:
+ # lin_speed, ang_speed = _update_speed_from_kb(kb, lin_speed, ang_speed)
+ #else:
+ lin_speed, ang_speed = _generate_example_linear_angular_speed(
+ loop_elapsed_time)
+
+ # Needed before every call to get_action().
+ _update_controller_params(controller, lin_speed, ang_speed)
+ controller.update()
+ hybrid_action = controller.get_action()
+
+ # Log the robot data.
+ robot_log["timestamps"].append(env.robot.GetTimeSinceReset())
+ robot_log["motor_angles"].append(env.robot.motor_angles)
+ robot_log["motor_velocities"].append(env.robot.motor_velocities)
+ robot_log["base_velocities"].append(
+ controller.state_estimator.com_velocity_body_yaw_aligned_frame)
+ robot_log["foot_positions"].append(env.robot.foot_positions())
+ robot_log["base_rollpitchyaw"].append(env.robot.base_roll_pitch_yaw)
+ robot_log["base_angular_velocities"].append(
+ env.robot.base_roll_pitch_yaw_rate)
+ robot_log["actions"].append(hybrid_action)
+
+ env.step(hybrid_action)
+ loop_elapsed_time = env.get_time_since_reset() - loop_start_time
+
+ finally:
+ if FLAGS.run_on_robot:
+ # Apply zero torques to the robot.
+ env.robot.apply_action(
+ [0] * env.robot.num_motors,
+ motor_control_mode=robot_config.MotorControlMode.TORQUE)
+ if FLAGS.log_path:
+ pickle.dump(robot_log, gfile.Open(FLAGS.log_path + "/robot.log", "wb"))
+
+
+def main(argv):
+ del argv
+ _load_config(render=True, run_on_robot=FLAGS.run_on_robot)
+ _run_example(
+ run_on_robot=FLAGS.run_on_robot, use_keyboard=FLAGS.use_keyboard_control)
+
+
+if __name__ == "__main__":
+ app.run(main)
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/locomotion_controller_in_scenario_set_example.py b/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/locomotion_controller_in_scenario_set_example.py
new file mode 100644
index 000000000..71fefa7b5
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/locomotion_controller_in_scenario_set_example.py
@@ -0,0 +1,180 @@
+r"""ScenarioSet example for Laikago MPC controller.
+
+blaze run -c opt \
+//robotics/reinforcement_learning/minitaur/agents/baseline_controller\
+:locomotion_controller_in_scenario_set_example -- --gait=slow_trot \
+--add_random_push=True
+"""
+
+from absl import app
+from absl import flags
+import gin
+import numpy as np
+import scipy.interpolate
+
+from pybullet_envs.minitaur.agents.baseline_controller import locomotion_controller_setup
+from pybullet_envs.minitaur.envs_v2 import env_loader
+
+FLAGS = flags.FLAGS
+
+SCENARIO_SET_CONFIG = """
+import pybullet_envs.minitaur.envs_v2.scenarios.locomotion_simple_scenario_set
+
+include "google3/robotics/reinforcement_learning/minitaur/envs_v2/scenarios/default_scenario_set.gin"
+
+default_scenario_set/singleton.constructor = @locomotion_simple_scenario_set.LocomotionSimpleScenarioSet
+
+
+locomotion_simple_scenario_set.LocomotionSimpleScenarioSet.selector = "flat_ground"
+locomotion_gym_env.LocomotionGymEnv.task = @scenario_set.task()
+locomotion_gym_env.LocomotionGymEnv.scene = @scenario_set.scene()
+locomotion_gym_env.LocomotionGymEnv.env_randomizers = [
+ @scenario_set.env_randomizer()
+]
+"""
+
+_MAX_TIME_SECONDS = 30
+
+flags.DEFINE_enum("gait", "fast_trot",
+ ["fast_trot", "slow_trot", "walk", "stand"],
+ "The gait pattern to use")
+
+flags.DEFINE_boolean("add_random_push", False,
+ "whether to add random push to the robot in simulation")
+
+
+def _start_stop_profile(max_speed=0.5, axis=0, duration=3):
+ speed_profile = np.zeros((3, 4))
+
+ speed_profile[1, axis] = max_speed
+
+ return (0, 0.5, duration + 0.5), speed_profile.tolist()
+
+
+def _random_speed_profile(max_speed=1, axis=0, time_interval=1.0):
+ num_pts = 11
+ time_points = np.arange(num_pts) * time_interval
+
+ speed_profile = np.zeros((num_pts, 4))
+ speed_profile[:, axis] = np.random.uniform(0, max_speed, num_pts)
+ speed_profile[-1, :] = 0
+ return time_points.tolist(), speed_profile.tolist()
+
+
+def _body_height_profile(z_range=(0.3, 0.55)):
+ del z_range
+ # TODO(tingnan): Implement this.
+
+
+def _generate_linear_angular_speed(t, time_points, speed_points):
+ """Creates an example speed profile based on time for demo purpose."""
+
+ speed = scipy.interpolate.interp1d(
+ time_points,
+ speed_points,
+ kind="previous",
+ fill_value="extrapolate",
+ axis=0)(
+ t)
+
+ return speed[0:3], speed[3]
+
+
+def _update_controller_params(controller, lin_speed, ang_speed):
+ controller.swing_leg_controller.desired_speed = lin_speed
+ controller.swing_leg_controller.desired_twisting_speed = ang_speed
+ controller.stance_leg_controller.desired_speed = lin_speed
+ controller.stance_leg_controller.desired_twisting_speed = ang_speed
+
+
+def _gen_stability_test_start_stop():
+ """Generates the speed profile for start/stop tests."""
+ axis_to_name = {
+ 0: "velocity x",
+ 1: "velocity y",
+ 3: "angular velocity z",
+ }
+
+ axis_to_max_speed = {
+ 0: 1.0,
+ 1: 0.5,
+ 3: 2.5,
+ }
+
+ gait_multiplier = {
+ "slow_trot": 0.7,
+ "walk": 0.3,
+ "fast_trot": 1.0,
+ }
+
+ for axis in (0, 1, 3):
+ yield axis_to_name[axis], _start_stop_profile(
+ axis_to_max_speed[axis] * gait_multiplier[FLAGS.gait], axis)
+
+
+def _gen_stability_test_random():
+ """Generates the speed profile for random walking tests."""
+ axis_to_name = {
+ 0: "velocity x",
+ 1: "velocity y",
+ 3: "angular velocity z",
+ }
+
+ axis_to_max_speed = {
+ 0: 1.0,
+ 1: 0.5,
+ 3: 2.5,
+ }
+
+ gait_multiplier = {
+ "slow_trot": 0.7,
+ "walk": 0.3,
+ "fast_trot": 1.0,
+ }
+
+ for axis in (0, 1, 3):
+ yield axis_to_name[axis], _random_speed_profile(
+ axis_to_max_speed[axis] * gait_multiplier[FLAGS.gait], axis)
+
+
+def _test_stability(max_time=5, render=False, test_generator=None):
+ """Tests the stability of the controller using speed profiles."""
+ locomotion_controller_setup.load_sim_config(render=render)
+ gin.parse_config(SCENARIO_SET_CONFIG)
+ if FLAGS.add_random_push:
+ locomotion_controller_setup.add_random_push_config()
+
+ env = env_loader.load()
+ controller = locomotion_controller_setup.setup_controller(
+ env.robot, gait=FLAGS.gait)
+
+ for name, speed_profile in test_generator():
+ env.reset()
+ controller.reset()
+ current_time = 0
+ while current_time < max_time:
+ current_time = env.get_time_since_reset()
+ lin_speed, ang_speed = _generate_linear_angular_speed(
+ current_time, speed_profile[0], speed_profile[1])
+ _update_controller_params(controller, lin_speed, ang_speed)
+
+ # Needed before every call to get_action().
+ controller.update()
+ hybrid_action = controller.get_action()
+
+ _, _, done, _ = env.step(hybrid_action)
+ if done:
+ break
+ print(f"Scene name: flat ground. Random push: {FLAGS.add_random_push}. "
+ f"Survival time for {name} = {speed_profile[1]} is {current_time}")
+
+
+def main(argv):
+ del argv
+ _test_stability(render=True, test_generator=_gen_stability_test_start_stop)
+ _test_stability(
+ max_time=15, render=True, test_generator=_gen_stability_test_random)
+
+
+if __name__ == "__main__":
+ app.run(main)
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/locomotion_controller_setup.py b/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/locomotion_controller_setup.py
new file mode 100644
index 000000000..afaed6cb7
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/locomotion_controller_setup.py
@@ -0,0 +1,175 @@
+"""The common setups for MPC based locoomtion controller environments."""
+import time
+import gin
+import numpy as np
+
+from pybullet_envs.minitaur.agents.baseline_controller import com_velocity_estimator
+from pybullet_envs.minitaur.agents.baseline_controller import imu_based_com_velocity_estimator
+from pybullet_envs.minitaur.agents.baseline_controller import locomotion_controller
+from pybullet_envs.minitaur.agents.baseline_controller import openloop_gait_generator
+from pybullet_envs.minitaur.agents.baseline_controller import raibert_swing_leg_controller
+from pybullet_envs.minitaur.agents.baseline_controller import torque_stance_leg_controller
+#from pybullet_envs.minitaur.envs.env_randomizers import minitaur_push_randomizer
+from pybullet_envs.minitaur.envs.env_randomizers import minitaur_push_randomizer
+from pybullet_envs.minitaur.robots import robot_config
+import pybullet_data as pd
+
+CONFIG_FILE = (pd.getDataPath()+"/configs_v2/base/laikago_reactive.gin")
+
+_MOTOR_KD = [1.0, 2.0, 2.0] * 4
+_BODY_HEIGHT = 0.45
+_MAX_TIME_SECONDS = 1000000000
+_MOTOR_KD = [1.0, 2.0, 2.0] * 4
+# TODO(tingnan): This is for tunining the moments of inertia of the model.
+# Once we identified the correct value we can remove this.
+_SCALE = 4
+_INERTIA = (0.07335 * _SCALE, 0, 0, 0, 0.25068 * _SCALE, 0, 0, 0,
+ 0.25447 * _SCALE)
+
+
+def load_sim_config(render=True):
+ """Builds the environment for the quadruped robot.
+
+ Args:
+ render: Enable/disable rendering.
+ """
+ gin.clear_config(clear_constants=False)
+ config_file = CONFIG_FILE
+ gin.parse_config_file(config_file)
+
+ # Sim bindings
+ # Overwrite a few parameters.
+
+ action_repeat = 4
+ gin.bind_parameter("SimulationParameters.num_action_repeat", action_repeat)
+ gin.bind_parameter("laikago_v2.Laikago.action_repeat", action_repeat)
+
+ # Control latency is NOT modeled properly for inverse kinematics and
+ # jacobians, as we are directly calling the pybullet API. We will try to fix
+ # this by loading a separate pybullet instance, set the pose and joint
+ # angles which has latency in them, and then run the jacobian/IK.
+ gin.bind_parameter("laikago_v2.Laikago.motor_control_mode",
+ robot_config.MotorControlMode.HYBRID)
+ # Bump up a bit the adduction/abduction motor d gain for a better tracking.
+ gin.bind_parameter("hybrid_motor_model.HybridMotorModel.kd", _MOTOR_KD)
+ gin.bind_parameter("SimulationParameters.enable_rendering", render)
+ gin.bind_parameter("env_loader.load.wrapper_classes", [])
+
+
+
+def add_random_push_config():
+ """Adds a random push randomizers to the config."""
+ try:
+ current_env_randomizers = gin.query_parameter(
+ "locomotion_gym_env.LocomotionGymEnv.env_randomizers")
+
+ current_env_randomizers.append(
+ minitaur_push_randomizer.MinitaurPushRandomizer(
+ horizontal_force_bound=(500, 900),
+ vertical_force_bound=(50, 100),
+ visualize_perturbation_force=True))
+ gin.bind_parameter("locomotion_gym_env.LocomotionGymEnv.env_randomizers",
+ current_env_randomizers)
+ except ValueError:
+ # No randoimzers bind so far
+ gin.bind_parameter("locomotion_gym_env.LocomotionGymEnv.env_randomizers", [
+ minitaur_push_randomizer.MinitaurPushRandomizer(
+ horizontal_force_bound=(500, 900),
+ vertical_force_bound=(50, 100),
+ visualize_perturbation_force=True)
+ ])
+
+
+def select_gait(gait_type="fast_trot"):
+ """Selects a gait pattern.
+
+ Args:
+ gait_type: which gait to use.
+
+ Returns:
+ A tuple of (stance_duration, duty_factor, initial_phase)
+ """
+ # Each gait is composed of stance_duration, duty_factor, and
+ # init_phase_full_cycle.
+ if gait_type == "fast_trot":
+ return [0.25] * 4, [0.6] * 4, [0, 0.5, 0.5, 0]
+ elif gait_type == "slow_trot":
+ return [0.4] * 4, [0.65] * 4, [0, 0.5, 0.5, 0]
+ elif gait_type == "walk":
+ return [0.75] * 4, [0.8] * 4, [0.25, 0.75, 0.5, 0]
+ else:
+ # Means four leg stand for as long as possible.
+ return [_MAX_TIME_SECONDS] * 4, [0.99] * 4, [0, 0, 0, 0]
+
+
+def setup_controller(robot,
+ gait="fast_trot",
+ run_on_robot=False,
+ use_ground_truth_velocity=False):
+ """Demonstrates how to create a locomotion controller.
+
+ Args:
+ robot: A robot instance.
+ gait: The type of gait to use.
+ run_on_robot: Whether this controller is running on the real robot or not.
+ use_ground_truth_velocity: Whether to use ground truth velocity or velocity
+ estimator.
+
+ Returns:
+ A locomotion controller.
+ """
+ desired_speed = (0, 0)
+ desired_twisting_speed = 0
+
+ feet_positions = np.array(robot.foot_positions())
+ feet_positions[:, 2] = 0
+
+ # Sim and real robots have different mass and contact detection parameters.
+ body_weight, contact_force_threshold = (200, 20) if run_on_robot else (215, 0)
+
+ stance_duration, duty_factor, init_phase = select_gait(gait)
+ gait_generator = openloop_gait_generator.OpenloopGaitGenerator(
+ robot,
+ stance_duration=stance_duration,
+ duty_factor=duty_factor,
+ initial_leg_phase=init_phase,
+ contact_detection_force_threshold=contact_force_threshold,
+ )
+ state_estimator = (
+ imu_based_com_velocity_estimator.IMUBasedCOMVelocityEstimator(
+ robot,
+ contact_detection_threshold=contact_force_threshold,
+ ))
+
+ # Use this in sim to test ground truth velocity estimation.
+ if use_ground_truth_velocity:
+ state_estimator = com_velocity_estimator.COMVelocityEstimator(robot)
+
+ sw_controller = raibert_swing_leg_controller.RaibertSwingLegController(
+ robot,
+ gait_generator,
+ state_estimator,
+ desired_speed=desired_speed,
+ desired_twisting_speed=desired_twisting_speed,
+ desired_height=_BODY_HEIGHT,
+ local_hip_positions=feet_positions,
+ )
+ st_controller = torque_stance_leg_controller.TorqueStanceLegController(
+ robot,
+ gait_generator,
+ state_estimator,
+ desired_speed=desired_speed,
+ desired_twisting_speed=desired_twisting_speed,
+ desired_body_height=_BODY_HEIGHT,
+ body_mass=body_weight / 9.8,
+ body_inertia=_INERTIA,
+ )
+
+ controller = locomotion_controller.LocomotionController(
+ robot=robot,
+ gait_generator=gait_generator,
+ state_estimator=state_estimator,
+ swing_leg_controller=sw_controller,
+ stance_leg_controller=st_controller,
+ clock=robot.GetTimeSinceReset)
+ return controller
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/minitaur_raibert_controller.py b/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/minitaur_raibert_controller.py
new file mode 100644
index 000000000..82ddebcab
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/minitaur_raibert_controller.py
@@ -0,0 +1,448 @@
+"""A Raibert style controller for Minitaur."""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+import math
+import attr
+import gin
+import numpy as np
+
+from pybullet_envs.minitaur.agents.baseline_controller import minitaur_raibert_controller_utils
+from pybullet_envs.minitaur.envs.utilities import minitaur_pose_utils
+
+LEFT_FRONT_LEG_ID = 0
+LEFT_HIND_LEG_ID = 1
+RIGHT_FRONT_LEG_ID = 2
+RIGHT_HIND_LEG_ID = 3
+
+DIAGONAL_LEG_PAIR_1 = (LEFT_FRONT_LEG_ID, RIGHT_HIND_LEG_ID)
+DIAGONAL_LEG_PAIR_2 = (LEFT_HIND_LEG_ID, RIGHT_FRONT_LEG_ID)
+
+LEFT_LEG_IDS = (LEFT_FRONT_LEG_ID, LEFT_HIND_LEG_ID)
+RIGHT_LEG_IDS = (RIGHT_FRONT_LEG_ID, RIGHT_HIND_LEG_ID)
+
+# The max horizontal foot offset (in meters) for turning.
+_FOOT_HORIZONTAL_OFFSET_FOR_TURNING = 0.1
+
+_STANCE_TG_PHASE_COMPRESSION = 1.5
+_STANCE_TG_DELTA_EXTENSION = 0.2
+_STANCE_HORIZONTAL_SCALING_FACTOR = 1.2
+
+_DEFAULT_SWING_SPEED_GAIN = 0.015
+
+_DEFAULT_SWING_FOOT_CLEARANCE = 0.005
+
+_PITCH_SWING_FEEDBACK_SCALING_FACTOR = 1.2
+
+
+# A POD container to describe the controller's high level behavior.
+@attr.s
+class BehaviorParameters(object):
+ """Highlevel parameters for Raibert style controller."""
+ stance_duration = attr.ib(type=float, default=0.25)
+ desired_forward_speed = attr.ib(type=float, default=0.)
+ desired_turning_speed = attr.ib(type=float, default=0.)
+ standing_height = attr.ib(type=float, default=0.2)
+ desired_incline_angle = attr.ib(type=float, default=0.)
+
+
+def generate_default_swing_trajectory(phase, init_pose, end_pose):
+ """A swing trajectory generator.
+
+ Args:
+ phase: Float. Between [0, 1].
+ init_pose: A tuple. The leg pose (swing, extension) at phase == 0 the
+ beginning of swing.
+ end_pose: A tuple. The leg pose at phase == 1 the end of swing.
+
+ Returns:
+ The desired leg pose for the current phase.
+ """
+ # Try phase compression
+ normalized_phase = math.sqrt(min(phase * 1.5, 1))
+
+ # For swing, we use a linear interpolation:
+ swing = (end_pose[0] - init_pose[0]) * normalized_phase + init_pose[0]
+
+ # For extension, we can fit a second order polynomial:
+ min_ext = (init_pose[1] + end_pose[1]) / 2 - 0.8
+ min_ext = max(min_ext, 0.5)
+
+ # The phase value at which the extension reaches the minimal value min_ext.
+ # phi is small, the swing leg will try to lift higher in the first half of
+ # swing.
+ phi = 0.1
+
+ # We convert the extension back into the cartesion space. In this way we can
+ # guarantee a lift-up trajectory. The ankle to hip distance is easier to
+ # compute than a full forward-kinematics.
+ min_ankle_dist = minitaur_raibert_controller_utils.extension_to_ankle_dist(
+ min_ext)
+ init_ankle_dist = minitaur_raibert_controller_utils.extension_to_ankle_dist(
+ init_pose[1])
+ end_ankle_dist = minitaur_raibert_controller_utils.extension_to_ankle_dist(
+ end_pose[1])
+
+ # The polynomial is: a * phi^2 + b * phi + c
+ delta_1 = min_ankle_dist - init_ankle_dist
+ delta_2 = end_ankle_dist - init_ankle_dist
+ delta_p = phi * phi - phi
+
+ a = (delta_1 - phi * delta_2) / delta_p
+
+ b = (phi * phi * delta_2 - delta_1) / delta_p
+
+ ankle_dist = (
+ a * normalized_phase * normalized_phase + b * normalized_phase +
+ init_ankle_dist)
+
+ l1 = minitaur_raibert_controller_utils.UPPER_LEG_LEN
+ l2 = minitaur_raibert_controller_utils.LOWER_SHORT_LEG_LEN
+
+ ankle_dist = min(max(ankle_dist, l2 - l1 + 0.01), l2 + l1 - 0.01)
+
+ extension = minitaur_raibert_controller_utils.ankle_dist_to_extension(
+ ankle_dist)
+
+ return (swing, extension)
+
+
+@gin.configurable
+def generate_default_stance_trajectory(phase,
+ init_pose,
+ end_pose,
+ use_constant_extension=False):
+ """A stance strajectory generator.
+
+ Args:
+ phase: Float. Between [0, 1].
+ init_pose: A tuple. The leg pose (swing, extension) at phase == 0, i.e. the
+ beginning of stance.
+ end_pose: A tuple. The leg pose at phase == 1, i.e. the end of stance.
+ use_constant_extension: Boolean. Whether or not to fix the extension during
+ stance.
+
+ Returns:
+ The desired leg pose for the current phase.
+ """
+ normalized_phase = min(_STANCE_TG_PHASE_COMPRESSION * math.sqrt(phase), 1)
+ swing = (end_pose[0] - init_pose[0]) * normalized_phase + init_pose[0]
+
+ # The extension evolves nonlinearly according to the parabola equation.
+ if use_constant_extension:
+ extension = end_pose[1]
+ else:
+ extension = end_pose[1] - 4 * _STANCE_TG_DELTA_EXTENSION * (
+ normalized_phase**2 - normalized_phase)
+ return (swing, extension)
+
+
+def get_stance_foot_offset_for_turning(leg_id, steering_signal):
+ """Modify the stance foot position to achieve turning.
+
+ The strategy works for trotting gaits.
+
+ Args:
+ leg_id: Integer. The leg index.
+ steering_signal: Float. The desired turning signal in [-1, 1]. Because we
+ don't have an accurate mapping from angular velocity to the foot offset,
+ the steering signal should be treated as a reference and only its relative
+ magnitude matters.
+
+ Returns:
+ The stance foot's horizontal offset.
+
+ """
+ clipped_steering_signal = np.clip(steering_signal, -1, 1)
+
+ if leg_id in LEFT_LEG_IDS:
+ return _FOOT_HORIZONTAL_OFFSET_FOR_TURNING * clipped_steering_signal
+ else:
+ return -(_FOOT_HORIZONTAL_OFFSET_FOR_TURNING * clipped_steering_signal)
+
+
+def get_leg_swing_offset_for_pitching(body_pitch, desired_incline_angle):
+ """Get the leg swing zero point when the body is tilted.
+
+ For example, when climbing up or down stairs/slopes, the robot body will tilt
+ up or down. By compensating the body pitch, the leg's trajectory will be
+ centered around the vertical direction (not perpendicular to the surface).
+ This helps the robot to generate thrust when going upwards, and braking when
+ going downwards.
+
+ Args:
+ body_pitch: Float. Current body pitch angle.
+ desired_incline_angle: Float. The desired body pitch angle.
+
+ Returns:
+ The stance and swing leg swing offset.
+
+ """
+ kp = 0.2
+ return -((1 - kp) * body_pitch + kp * desired_incline_angle)
+
+
+@gin.configurable
+class RaibertSwingLegController(object):
+ """The swing leg controller."""
+
+ def __init__(self,
+ speed_gain=_DEFAULT_SWING_SPEED_GAIN,
+ foot_clearance=_DEFAULT_SWING_FOOT_CLEARANCE,
+ leg_trajectory_generator=generate_default_swing_trajectory):
+ """Initializes the controller.
+
+ Args:
+ speed_gain: Float. The speed feedback gain to modulate the target foot
+ position.
+ foot_clearance: Float. The foot clearance (at the end of the swing phase)
+ in meters.
+ leg_trajectory_generator: A trajectory generator function.
+ """
+ self._speed_gain = speed_gain
+ self._foot_clearance = foot_clearance
+ self._leg_trajectory_generator = leg_trajectory_generator
+
+ def get_action(self, raibert_controller):
+ """Get the swing legs' desired pose."""
+ current_speed = raibert_controller.estimate_base_velocity()
+ phase = raibert_controller.get_phase()
+ rpy = raibert_controller.robot.base_roll_pitch_yaw
+
+ leg_pose_set = {}
+ for i in raibert_controller.swing_set:
+ # Target foot horizontal position is calculated according to Raibert's
+ # original formula in "Legged robots that balance".
+ target_foot_horizontal_position = (
+ raibert_controller.behavior_parameters.stance_duration / 2 *
+ current_speed + self._speed_gain *
+ (current_speed -
+ raibert_controller.behavior_parameters.desired_forward_speed))
+
+ # 1) Convert the target foot position to leg pose space.
+ # Lift the foot a little bit.
+ target_foot_vertical_position = -(
+ raibert_controller.behavior_parameters.standing_height -
+ self._foot_clearance)
+ target_foot_position = (target_foot_horizontal_position,
+ target_foot_vertical_position)
+ target_leg_pose = minitaur_raibert_controller_utils.foot_position_to_leg_pose(
+ target_foot_position)
+
+ # 2) Generates the curve from the swing start leg pose to the target leg
+ # pose and find the next leg pose on the curve based on current swing
+ # phase.
+
+ desired_leg_pose = self._leg_trajectory_generator(
+ phase, raibert_controller.swing_start_leg_pose, target_leg_pose)
+
+ swing_offset = get_leg_swing_offset_for_pitching(
+ rpy[1], raibert_controller.behavior_parameters.desired_incline_angle)
+
+ leg_pose_set[i] = (desired_leg_pose[0] + swing_offset,
+ desired_leg_pose[1])
+
+ return leg_pose_set
+
+
+@gin.configurable
+class RaibertStanceLegController(object):
+ """The controller that modulates the behavior of the stance legs."""
+
+ def __init__(self,
+ speed_gain=0.1,
+ leg_trajectory_generator=generate_default_stance_trajectory):
+ """Initializes the controller.
+
+ Args:
+ speed_gain: Float. The speed feedback gain to modulate the target stance
+ foot position.
+ leg_trajectory_generator: A trajectory generator function. Generates the
+ desired leg pose during the stance phase.
+ """
+ self._speed_gain = speed_gain
+ self._leg_trajectory_generator = leg_trajectory_generator
+
+ def get_action(self, raibert_controller):
+ """Get the desired leg pose for the stance legs."""
+
+ phase = raibert_controller.get_phase()
+ current_speed = raibert_controller.estimate_base_velocity()
+ rpy = raibert_controller.robot.base_roll_pitch_yaw
+
+ leg_pose_set = {}
+ for i in raibert_controller.stance_set:
+ desired_forward_speed = (
+ raibert_controller.behavior_parameters.desired_forward_speed)
+
+ target_foot_horizontal_position = -_STANCE_HORIZONTAL_SCALING_FACTOR * (
+ raibert_controller.behavior_parameters.stance_duration / 2 *
+ current_speed - self._speed_gain *
+ (current_speed - desired_forward_speed))
+
+ target_foot_horizontal_position += get_stance_foot_offset_for_turning(
+ i, raibert_controller.behavior_parameters.desired_turning_speed)
+
+ target_foot_position = (
+ target_foot_horizontal_position,
+ -raibert_controller.behavior_parameters.standing_height)
+ target_leg_pose = minitaur_raibert_controller_utils.foot_position_to_leg_pose(
+ target_foot_position)
+
+ desired_leg_pose = (
+ self._leg_trajectory_generator(
+ phase, raibert_controller.stance_start_leg_pose, target_leg_pose))
+
+ swing_offset = _PITCH_SWING_FEEDBACK_SCALING_FACTOR * get_leg_swing_offset_for_pitching(
+ rpy[1], raibert_controller.behavior_parameters.desired_incline_angle)
+
+ leg_pose_set[i] = (desired_leg_pose[0] + swing_offset,
+ desired_leg_pose[1])
+
+ return leg_pose_set
+
+
+@gin.configurable
+class MinitaurRaibertController(object):
+ """A Raibert style controller for trotting gait."""
+
+ def __init__(self,
+ robot,
+ behavior_parameters=BehaviorParameters(),
+ swing_leg_controller=RaibertSwingLegController(),
+ stance_leg_controller=RaibertStanceLegController(),
+ pose_feedback_controller=None):
+ self._time = 0
+ self._robot = robot
+ self.behavior_parameters = behavior_parameters
+
+ self._last_recorded_speed = 0
+
+ self._swing_leg_controller = swing_leg_controller
+ self._stance_leg_controller = stance_leg_controller
+ self._pose_feeback_controller = pose_feedback_controller
+
+ # The leg order is FL, RL, FR, RR -> [0, 1, 2, 3]
+ self._swing_set = DIAGONAL_LEG_PAIR_1
+ self._stance_set = DIAGONAL_LEG_PAIR_2
+
+ # Compute the initial leg pose.
+ self._swing_start_leg_pose = self.get_swing_leg_pose()
+ self._stance_start_leg_pose = self.get_stance_leg_pose()
+
+ @property
+ def robot(self):
+ return self._robot
+
+ @property
+ def swing_set(self):
+ return self._swing_set
+
+ @property
+ def stance_set(self):
+ return self._stance_set
+
+ @property
+ def swing_start_leg_pose(self):
+ return self._swing_start_leg_pose
+
+ @property
+ def stance_start_leg_pose(self):
+ return self._stance_start_leg_pose
+
+ def _get_average_leg_pose(self, leg_indices):
+ """Get the average leg pose."""
+ current_leg_pose = minitaur_pose_utils.motor_angles_to_leg_pose(
+ self._robot.motor_angles)
+
+ # extract the swing leg pose from the current_leg_pose
+ leg_pose = []
+ for index in leg_indices:
+ leg_pose.append([
+ current_leg_pose[index],
+ current_leg_pose[index + minitaur_pose_utils.NUM_LEGS]
+ ])
+
+ leg_pose = np.array(leg_pose)
+ return np.mean(leg_pose, axis=0)
+
+ def get_swing_leg_pose(self):
+ """Get the current swing legs' average pose."""
+ return self._get_average_leg_pose(self._swing_set)
+
+ def get_stance_leg_pose(self):
+ """Get the current stance legs' average pose."""
+ return self._get_average_leg_pose(self._stance_set)
+
+ def get_phase(self):
+ """Compute the current stance/swing phase."""
+ return math.fmod(self._time, self.behavior_parameters.stance_duration
+ ) / self.behavior_parameters.stance_duration
+
+ def _get_new_swing_stance_set(self):
+ """Switch the set of swing/stance legs based on timing."""
+ swing_stance_phase = math.fmod(self._time,
+ 2 * self.behavior_parameters.stance_duration)
+ if swing_stance_phase < self.behavior_parameters.stance_duration:
+ return (DIAGONAL_LEG_PAIR_1, DIAGONAL_LEG_PAIR_2)
+ return (DIAGONAL_LEG_PAIR_2, DIAGONAL_LEG_PAIR_1)
+
+ def update(self, t):
+ """Update the internal status of the controller.
+
+ Args:
+ t: Float. The current time after reset in seconds.
+ """
+ self._time = t
+ new_swing_set, new_stance_set = self._get_new_swing_stance_set()
+
+ # If there is a stance/swing switch.
+ if new_swing_set[0] is not self._swing_set[0]:
+ self._swing_set = new_swing_set
+ self._stance_set = new_stance_set
+
+ # Also records the starting pose.
+ self._swing_start_leg_pose = self.get_swing_leg_pose()
+ self._stance_start_leg_pose = self.get_stance_leg_pose()
+
+ def estimate_base_velocity(self):
+ """Estimate the current CoM speed."""
+ # It is best to use a sensor fusion approach.
+ stance_leg_pose = self.get_stance_leg_pose()
+
+ delta_sw = stance_leg_pose[0] - self._stance_start_leg_pose[0]
+
+ x, y = minitaur_raibert_controller_utils.leg_pose_to_foot_position(
+ stance_leg_pose)
+ toe_hip_len = math.sqrt(x**2 + y**2)
+ horizontal_dist = toe_hip_len * delta_sw
+ phase = self.get_phase()
+ speed = self._last_recorded_speed
+ if phase > 0.1:
+ speed = horizontal_dist / (
+ self.behavior_parameters.stance_duration * phase)
+ self._last_recorded_speed = speed
+ return speed
+
+ def get_swing_leg_action(self):
+ return self._swing_leg_controller.get_action(self)
+
+ def get_stance_leg_action(self):
+ return self._stance_leg_controller.get_action(self)
+
+ def get_action(self):
+ """Gets the desired motor angles."""
+ leg_pose = [0] * minitaur_pose_utils.NUM_MOTORS
+ swing_leg_pose = self.get_swing_leg_action()
+ for i in self._swing_set:
+ leg_pose[i] = swing_leg_pose[i][0]
+ leg_pose[i + minitaur_pose_utils.NUM_LEGS] = swing_leg_pose[i][1]
+
+ stance_leg_pose = self.get_stance_leg_action()
+ for i in self._stance_set:
+ leg_pose[i] = stance_leg_pose[i][0]
+ leg_pose[i + minitaur_pose_utils.NUM_LEGS] = stance_leg_pose[i][1]
+
+ return minitaur_pose_utils.leg_pose_to_motor_angles(leg_pose)
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/minitaur_raibert_controller_utils.py b/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/minitaur_raibert_controller_utils.py
new file mode 100644
index 000000000..cc992a197
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/minitaur_raibert_controller_utils.py
@@ -0,0 +1,82 @@
+"""Utility functions for the Minitaur Raibert controller."""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+import math
+
+UPPER_LEG_LEN = 0.112
+LOWER_SHORT_LEG_LEN = 0.199
+LOWER_LONG_LEG_LEN = 0.2315
+
+
+def leg_pose_to_foot_position(leg_pose):
+ """The forward kinematics."""
+ l1 = UPPER_LEG_LEN
+ l2 = LOWER_SHORT_LEG_LEN
+ l3 = LOWER_LONG_LEG_LEN
+
+ ext = leg_pose[1]
+ alpha = math.asin(l1 * math.sin(ext) / l2)
+
+ sw = -leg_pose[0]
+ x = l3 * math.sin(alpha + sw) - l1 * math.sin(ext + sw)
+ y = l3 * math.cos(alpha + sw) - l1 * math.cos(ext + sw)
+
+ return (x, -y)
+
+
+def foot_position_to_leg_pose(foot_position):
+ """The inverse kinematics."""
+ l1 = UPPER_LEG_LEN
+ l2 = LOWER_SHORT_LEG_LEN
+ l3 = LOWER_LONG_LEG_LEN
+
+ x = foot_position[0]
+ y = foot_position[1]
+
+ assert y < 0
+ hip_toe_sqr = x**2 + y**2
+ cos_beta = (l1 * l1 + l3 * l3 - hip_toe_sqr) / (2 * l1 * l3)
+ assert -1 <= cos_beta <= 1
+ hip_ankle_sqr = l1 * l1 + l2 * l2 - 2 * l1 * l2 * cos_beta
+ hip_ankle = math.sqrt(hip_ankle_sqr)
+ cos_ext = -(l1 * l1 + hip_ankle_sqr - l2 * l2) / (2 * l1 * hip_ankle)
+ ext = math.acos(cos_ext)
+
+ hip_toe = math.sqrt(hip_toe_sqr)
+ cos_theta = (hip_toe_sqr + hip_ankle_sqr -
+ (l3 - l2)**2) / (2 * hip_ankle * hip_toe)
+
+ assert cos_theta > 0
+ theta = math.acos(cos_theta)
+ sw = math.asin(x / hip_toe) - theta
+ return (-sw, ext)
+
+
+def extension_to_ankle_dist(extension):
+ """Converts leg extension to ankle-hip distance in meters.
+
+ The ankle is defined as the joint of the two lower links, which is different
+ from the toe which is the tip of the longer lower limb.
+
+ Args:
+ extension: Float. the leg extension.
+
+ Returns:
+ Float. The hip to ankle distance in meters.
+
+ """
+ l1 = UPPER_LEG_LEN
+ l2 = LOWER_SHORT_LEG_LEN
+ alpha = math.asin(l1 / l2 * math.sin(extension))
+ return l2 * math.cos(alpha) - l1 * math.cos(extension)
+
+
+def ankle_dist_to_extension(dist):
+ """Converts ankle-hip distance (meters) to extension."""
+ l1 = UPPER_LEG_LEN
+ l2 = LOWER_SHORT_LEG_LEN
+ cos_extension = -(l1**2 + dist**2 - l2**2) / (2 * l1 * dist)
+ return math.acos(cos_extension)
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/model_predictive_control.py b/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/model_predictive_control.py
new file mode 100644
index 000000000..739424360
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/model_predictive_control.py
@@ -0,0 +1,149 @@
+# Lint as: python3
+"""Classic model predictive control methods."""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+import math
+from typing import Sequence
+import numpy as np
+
+_MAX_ABS_RPY = 0.3
+_MAX_ABS_ANGULAR_VELOCITY = math.pi
+
+# The center of mass torque is computed using a simple PD control: tau = -KP *
+# delta_rotation - KD * delta_angular_velocity
+_TORQUE_KP = 2000
+_TORQUE_KD = 150
+
+# For center of mass force, we only need to track position in the z direction
+# (i.e. maintain the body height), and speed in x-y plane.
+_FORCE_KP = 500
+_FORCE_KD = 200
+
+
+def compute_contact_force_projection_matrix(
+ foot_positions_in_com_frame: Sequence[Sequence[float]],
+ stance_foot_ids: Sequence[int],
+) -> np.ndarray:
+ r"""Computes the 6 x 3n matrix to map contact force to com dynamics.
+
+ This is essentially the vectorized rhs of com dynamics equation:
+ ma = \sum f
+ I\omega_dot = \sum r \cross f
+
+ where the summation if over all feet in contact with ground.
+
+ Caveats: Current we have not taken the com rotation into account as we assume
+ the com rotation would be small. Ideally we should rotate the foot_positions
+ to a world frame centered at com. Also, since absolute yaw angles are not
+ accurate dute to drifting, we should use (roll, pitch, 0) to do the foot
+ position projection. This feature will be quite useful for MPC.
+ TODO(b/143378213): Fix this.
+
+ Args:
+ foot_positions_in_com_frame: the local position of each foot.
+ stance_foot_ids: The stance foot to be used to assemble the matrix.
+
+ Returns:
+ The contact force projection matrix.
+
+ """
+ jacobians = []
+ for foot_id in stance_foot_ids:
+ jv = np.identity(3)
+ foot_position = foot_positions_in_com_frame[foot_id]
+ x, y, z = foot_position[:3]
+ jw = np.array(((0, -z, y), (z, 0, -x), (-y, x, 0)))
+ jacobians.append(np.vstack((jv, jw)))
+
+ return np.hstack(jacobians)
+
+
+def plan_foot_contact_force(
+ mass: float,
+ inertia: np.ndarray,
+ com_position: np.ndarray,
+ com_velocity: np.ndarray,
+ com_roll_pitch_yaw: np.ndarray,
+ com_angular_velocity: np.ndarray,
+ foot_positions_in_com_frame: Sequence[Sequence[float]],
+ foot_contact_state: Sequence[bool],
+ desired_com_position: np.ndarray,
+ desired_com_velocity: np.ndarray,
+ desired_com_roll_pitch_yaw: np.ndarray,
+ desired_com_angular_velocity: np.ndarray,
+):
+ """Plan the foot contact forces using robot states.
+
+ TODO(b/143382305): Wrap this interface in a MPC class so we can use other
+ planning algorithms.
+
+ Args:
+ mass: The total mass of the robot.
+ inertia: The diagnal elements [Ixx, Iyy, Izz] of the robot.
+ com_position: Center of mass position in world frame. Usually we cannot
+ accurrately obtain this without motion capture.
+ com_velocity: Center of mass velocity in world frame.
+ com_roll_pitch_yaw: Center of mass rotation wrt world frame in euler angles.
+ com_angular_velocity: The angular velocity (roll_dot, pitch_dot, yaw_dot).
+ foot_positions_in_com_frame: The position of all feet/toe joints in the body
+ frame.
+ foot_contact_state: Indicates if a foot is in contact with the ground.
+ desired_com_position: We usually just care about the body height.
+ desired_com_velocity: In world frame.
+ desired_com_roll_pitch_yaw: We usually care about roll and pitch, since yaw
+ measurement can be unreliable.
+ desired_com_angular_velocity: Roll and pitch change rate are usually zero.
+ Yaw rate is the turning speed of the robot.
+
+ Returns:
+ The desired stance foot contact forces.
+ """
+ del inertia
+ del com_position
+ body_height = []
+ stance_foot_ids = []
+ for foot_id, foot_position in enumerate(foot_positions_in_com_frame):
+ if not foot_contact_state[foot_id]:
+ continue
+ stance_foot_ids.append(foot_id)
+ body_height.append(foot_position[2])
+
+ avg_bogy_height = abs(sum(body_height) / len(body_height))
+
+ rpy = com_roll_pitch_yaw
+ rpy[:2] = np.clip(rpy[:2], -_MAX_ABS_RPY, _MAX_ABS_RPY)
+ rpy_dot = com_angular_velocity
+ rpy_dot = np.clip(rpy_dot, -_MAX_ABS_ANGULAR_VELOCITY,
+ _MAX_ABS_ANGULAR_VELOCITY)
+
+ com_torque = -avg_bogy_height * (
+ _TORQUE_KP * (rpy - desired_com_roll_pitch_yaw) + _TORQUE_KD * rpy_dot)
+
+ # We don't care about the absolute yaw angle in the low level controller.
+ # Instead, we stabialize the angular velocity in the z direction.
+ com_torque[2] = -avg_bogy_height * _TORQUE_KD * (
+ rpy_dot[2] - desired_com_angular_velocity[2])
+
+ # Track a desired com velocity.
+ com_force = -_FORCE_KD * (com_velocity - desired_com_velocity)
+
+ # In the z-direction we also want to track the body height.
+ com_force[2] += mass * 9.8 - _FORCE_KP * (
+ avg_bogy_height - desired_com_position[2])
+
+ com_force_torque = np.concatenate((com_force, com_torque)).transpose()
+
+ # Map the com force torque to foot contact forces.
+ foot_force_to_com = compute_contact_force_projection_matrix(
+ foot_positions_in_com_frame, stance_foot_ids)
+ all_contact_force = -np.matmul(
+ np.linalg.pinv(foot_force_to_com), com_force_torque).transpose()
+ contact_force = {}
+
+ for i, foot_id in enumerate(stance_foot_ids):
+ contact_force[foot_id] = all_contact_force[3 * i:3 * i + 3]
+
+ return contact_force
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/multi_state_estimator.py b/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/multi_state_estimator.py
new file mode 100644
index 000000000..60f8cd86b
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/multi_state_estimator.py
@@ -0,0 +1,48 @@
+"""A class for combining multiple state estimators."""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+from typing import Any, Sequence
+
+import gin
+
+from pybullet_envs.minitaur.agents.baseline_controller import state_estimator
+
+
+@gin.configurable
+class MultiStateEstimator(state_estimator.StateEstimatorBase):
+ """Combine multiple state estimators.
+
+
+ This class can be used to combine multiple state estimators into one. For
+ example, one can use the COMVelocityEstimator to estimate the com velocity
+ and COMHeightEstimator to estimate the com height.
+
+ """
+
+ def __init__(
+ self,
+ robot: Any,
+ state_estimators: Sequence[state_estimator.StateEstimatorBase],
+ ):
+ self._robot = robot
+ self._state_estimators = state_estimators
+ self.reset(0)
+
+ def reset(self, current_time):
+ for single_state_estimator in self._state_estimators:
+ single_state_estimator.reset(current_time)
+
+ def update(self, current_time):
+ for single_state_estimator in self._state_estimators:
+ single_state_estimator.update(current_time)
+
+ def __getattr__(self, attr):
+ for single_state_estimator in self._state_estimators:
+ if hasattr(single_state_estimator, attr):
+ return getattr(single_state_estimator, attr)
+ raise ValueError(
+ "{} is not found in any of the state estimators".format(attr))
+
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/openloop_gait_generator.py b/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/openloop_gait_generator.py
new file mode 100644
index 000000000..c8696dc27
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/openloop_gait_generator.py
@@ -0,0 +1,194 @@
+"""Gait pattern planning module."""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+import logging
+import math
+from typing import Any, Sequence
+
+import gin
+import numpy as np
+
+from pybullet_envs.minitaur.agents.baseline_controller import gait_generator
+
+_DEFAULT_INITIAL_LEG_STATE = (
+ gait_generator.LegState.STANCE,
+ gait_generator.LegState.STANCE,
+ gait_generator.LegState.STANCE,
+ gait_generator.LegState.STANCE,
+)
+
+_NOMINAL_STANCE_DURATION = (0.25, 0.25, 0.25, 0.25)
+_NOMINAL_DUTY_FACTOR = (0.6, 0.6, 0.6, 0.6)
+_TROTTING_LEG_PHASE = (0, 0.5, 0.5, 0)
+_NOMINAL_CONTACT_DETECTION_PHASE = 0.4
+
+
+@gin.configurable
+class OpenloopGaitGenerator(gait_generator.GaitGenerator):
+ """Generates openloop gaits for quadruped robots.
+
+ A flexible open-loop gait generator. Each leg has its own cycle and duty
+ factor. And the state of each leg alternates between stance and swing. One can
+ easily formuate a set of common quadruped gaits like trotting, pacing,
+ pronking, bounding, etc by tweaking the input parameters.
+ """
+
+ def __init__(
+ self,
+ robot: Any,
+ stance_duration: Sequence[float] = _NOMINAL_STANCE_DURATION,
+ duty_factor: Sequence[float] = _NOMINAL_DUTY_FACTOR,
+ initial_leg_phase: Sequence[float] = _TROTTING_LEG_PHASE,
+ contact_detection_force_threshold: float = 0,
+ contact_detection_phase_threshold:
+ float = _NOMINAL_CONTACT_DETECTION_PHASE,
+ ):
+ """Initializes the class.
+
+ Args:
+ robot: A quadruped robot that at least implements the GetFootContacts API
+ and num_legs property.
+ stance_duration: The desired stance duration.
+ duty_factor: The ratio stance_duration / total_gait_cycle.
+ initial_leg_phase: The desired initial phase [0, 1] of the legs within the
+ full swing + stance cycle.
+ contact_detection_force_threshold: The minimal contact force required to
+ detect if a foot is in contact with the ground. For real robots this
+ needs to be larger (i.e. 25 for Laikago).
+ contact_detection_phase_threshold: Updates the state of each leg based on
+ contact info, when the current normalized phase is greater than this
+ threshold. This is essential to remove false positives in contact
+ detection when phase switches. For example, a swing foot at at the
+ beginning of the gait cycle might be still on the ground.
+ """
+ self._robot = robot
+ self._stance_duration = stance_duration
+ self._duty_factor = duty_factor
+ self._swing_duration = np.array(stance_duration) / np.array(
+ duty_factor) - np.array(stance_duration)
+ if len(initial_leg_phase) != len(
+ list(self._robot.urdf_loader.get_end_effector_id_dict().values())):
+ raise ValueError(
+ "The number of leg phases should be the same as number of legs.")
+ self._initial_leg_phase = initial_leg_phase
+
+ self._initial_leg_state = _DEFAULT_INITIAL_LEG_STATE
+ self._next_leg_state = []
+ # The ratio in cycle is duty factor if initial state of the leg is STANCE,
+ # and 1 - duty_factory if the initial state of the leg is SWING.
+ self._initial_state_ratio_in_cycle = []
+ for state, duty in zip(self._initial_leg_state, duty_factor):
+ assert state == gait_generator.LegState.STANCE
+ self._initial_state_ratio_in_cycle.append(duty)
+ self._next_leg_state.append(gait_generator.LegState.SWING)
+
+ self._contact_detection_force_threshold = contact_detection_force_threshold
+ self._contact_detection_phase_threshold = contact_detection_phase_threshold
+
+ # The normalized phase within swing or stance duration.
+ self._normalized_phase = None
+
+ # The current leg state, when contact is considered.
+ self._leg_state = None
+
+ # The desired leg state (i.e. SWING or STANCE).
+ self._desired_leg_state = None
+
+ self.reset(0)
+
+ def reset(self, current_time):
+ # The normalized phase within swing or stance duration.
+ self._normalized_phase = np.zeros(
+ len(list(self._robot.urdf_loader.get_end_effector_id_dict().values())))
+ self._leg_state = list(self._initial_leg_state)
+ self._desired_leg_state = list(self._initial_leg_state)
+
+ @property
+ def desired_leg_state(self) -> Sequence[gait_generator.LegState]:
+ """The desired leg SWING/STANCE states.
+
+ Returns:
+ The SWING/STANCE states for all legs.
+
+ """
+ return self._desired_leg_state
+
+ @property
+ def leg_state(self) -> Sequence[gait_generator.LegState]:
+ """The leg state after considering contact with ground.
+
+ Returns:
+ The actual state of each leg after accounting for contacts.
+ """
+ return self._leg_state
+
+ @property
+ def swing_duration(self) -> Sequence[float]:
+ return self._swing_duration
+
+ @property
+ def stance_duration(self) -> Sequence[float]:
+ return self._stance_duration
+
+ @property
+ def normalized_phase(self) -> Sequence[float]:
+ """The phase within the current swing or stance cycle.
+
+ Reflects the leg's phase within the curren swing or stance stage. For
+ example, at the end of the current swing duration, the phase will
+ be set to 1 for all swing legs. Same for stance legs.
+
+ Returns:
+ Normalized leg phase for all legs.
+
+ """
+ return self._normalized_phase
+
+ def update(self, current_time):
+ contact_state = [
+ np.linalg.norm(contact_force) > self._contact_detection_force_threshold
+ for contact_force in self._robot.feet_contact_forces()
+ ]
+
+ for leg_id in range(
+ len(list(self._robot.urdf_loader.get_end_effector_id_dict().values()))):
+ # Here is the explanation behind this logic: We use the phase within the
+ # full swing/stance cycle to determine if a swing/stance switch occurs
+ # for a leg. The threshold value is the "initial_state_ratio_in_cycle" as
+ # explained before. If the current phase is less than the initial state
+ # ratio, the leg is either in the initial state or has switched back after
+ # one or more full cycles.
+ full_cycle_period = (
+ self._stance_duration[leg_id] / self._duty_factor[leg_id])
+ # To account for the non-zero initial phase, we offset the time duration
+ # with the effect time contribution from the initial leg phase.
+ augmented_time = current_time + self._initial_leg_phase[
+ leg_id] * full_cycle_period
+ phase_in_full_cycle = math.fmod(augmented_time,
+ full_cycle_period) / full_cycle_period
+ ratio = self._initial_state_ratio_in_cycle[leg_id]
+ if phase_in_full_cycle < ratio:
+ self._desired_leg_state[leg_id] = self._initial_leg_state[leg_id]
+ self._normalized_phase[leg_id] = phase_in_full_cycle / ratio
+ else:
+ # A phase switch happens for this leg.
+ self._desired_leg_state[leg_id] = self._next_leg_state[leg_id]
+ self._normalized_phase[leg_id] = (phase_in_full_cycle - ratio) / (1 -
+ ratio)
+
+ self._leg_state[leg_id] = self._desired_leg_state[leg_id]
+
+ # No contact detection at the beginning of each SWING/STANCE phase.
+ if (self._normalized_phase[leg_id] <
+ self._contact_detection_phase_threshold):
+ continue
+ if (self._leg_state[leg_id] == gait_generator.LegState.SWING and
+ contact_state[leg_id]):
+ logging.info("early touch down detected")
+ self._leg_state[leg_id] = gait_generator.LegState.EARLY_CONTACT
+ if (self._leg_state[leg_id] == gait_generator.LegState.STANCE and
+ not contact_state[leg_id]):
+ self._leg_state[leg_id] = gait_generator.LegState.LOSE_CONTACT
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/raibert_swing_leg_controller.py b/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/raibert_swing_leg_controller.py
new file mode 100644
index 000000000..aad4b9b67
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/raibert_swing_leg_controller.py
@@ -0,0 +1,242 @@
+"""The swing leg controller class."""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+import copy
+import math
+from typing import Any, Mapping, Sequence, Tuple
+
+import gin
+import numpy as np
+
+from pybullet_envs.minitaur.agents.baseline_controller import gait_generator as gait_generator_lib
+from pybullet_envs.minitaur.agents.baseline_controller import leg_controller
+
+# The position correction coefficients in Raibert's formula.
+_KP = 0.025
+
+# At the end of swing, we leave a small clearance to prevent unexpected foot
+# collision.
+_FOOT_CLEARANCE_M = 0.0
+_DEFAULT_LOCAL_HIP_POSITIONS = ((0.21, -0.1157, 0), (0.21, 0.1157, 0),
+ (-0.21, -0.1157, 0), (-0.21, 0.1157, 0))
+_DEFAULT_SWING_EASE_UP_PHASE = 1.0
+_DEFAULT_SWING_EASE_UP_PERCENT = 1.0
+_FEED_FORWARD_TORQUES = (-0.7, 0, 0, 0.7, 0, 0, -0.7, 0, 0, 0.7, 0, 0)
+
+
+def _gen_parabola(phase: float, start: float, mid: float, end: float) -> float:
+ """Gets a point on a parabola y = a x^2 + b x + c.
+
+ The Parabola is determined by three points (0, start), (0.5, mid), (1, end) in
+ the plane.
+
+ Args:
+ phase: Normalized to [0, 1]. A point on the x-axis of the parabola.
+ start: The y value at x == 0.
+ mid: The y value at x == 0.5.
+ end: The y value at x == 1.
+
+ Returns:
+ The y value at x == phase.
+ """
+ mid_phase = 0.5
+ delta_1 = mid - start
+ delta_2 = end - start
+ delta_3 = mid_phase**2 - mid_phase
+ coef_a = (delta_1 - delta_2 * mid_phase) / delta_3
+ coef_b = (delta_2 * mid_phase**2 - delta_1) / delta_3
+ coef_c = start
+
+ return coef_a * phase**2 + coef_b * phase + coef_c
+
+
+def _gen_swing_foot_trajectory(input_phase: float, start_pos: Sequence[float],
+ end_pos: Sequence[float], ease_up_phase: float,
+ ease_up_percent: float) -> Tuple[float]:
+ """Generates the swing trajectory using a parabola.
+
+ Args:
+ input_phase: the swing/stance phase value between [0, 1].
+ start_pos: The foot's position at the beginning of swing cycle.
+ end_pos: The foot's desired position at the end of swing cycle.
+ ease_up_phase: Time length for the initial ease up phase dueing swing cycle.
+ ease_up_percent: Percentage of the swing cycle completed after
+ ease_up_phase.
+
+ Returns:
+ The desired foot position at the current phase.
+ """
+ # We augment the swing speed using the below formula. For the first portion of
+ # the swing cycle (ease_up_phase), the swing leg moves faster and finishes
+ # ease_up_percent% of the full swing trajectory. The rest of trajectory takes
+ # the rest of the swing cycle. Intuitely, we want to move the swing foot
+ # quickly to the target landing location and stay above the ground, in this
+ # way the control is more robust to perturbations to the body that may cause
+ # the swing foot to drop onto the ground earlier than expected. This is a
+ # common practice similar to the MIT cheetah and Marc Raibert's original
+ # controllers.
+ assert 0 <= ease_up_percent <= 1
+ assert 0 <= ease_up_phase <= 1
+ phase = input_phase
+ if input_phase <= ease_up_phase:
+ phase = ease_up_percent * math.sin(input_phase /
+ (2 * ease_up_phase) * math.pi)
+ else:
+ phase = ease_up_percent + (input_phase - ease_up_phase) * (
+ 1 - ease_up_percent) / (1 - ease_up_phase)
+
+ x = (1 - phase) * start_pos[0] + phase * end_pos[0]
+ y = (1 - phase) * start_pos[1] + phase * end_pos[1]
+ max_clearance = 0.1
+ mid = max(end_pos[2], start_pos[2]) + max_clearance
+ z = _gen_parabola(phase, start_pos[2], mid, end_pos[2])
+
+ # PyType detects the wrong return type here.
+ return (x, y, z) # pytype: disable=bad-return-type
+
+
+@gin.configurable
+class RaibertSwingLegController(leg_controller.LegController):
+ """Controls the swing leg position using Raibert's formula.
+
+ For details, please refer to chapter 2 in "Legged robbots that balance" by
+ Marc Raibert. The key idea is to stablize the swing foot's location based on
+ the CoM moving speed.
+
+ """
+
+ def __init__(
+ self,
+ robot: Any,
+ gait_generator: Any,
+ state_estimator: Any,
+ desired_speed: Tuple[float] = (0, 0),
+ desired_twisting_speed: float = 0,
+ desired_height: float = 0.45,
+ foot_clearance: float = _FOOT_CLEARANCE_M,
+ local_hip_positions: Tuple[Tuple[float]] = _DEFAULT_LOCAL_HIP_POSITIONS,
+ ease_up_phase: float = _DEFAULT_SWING_EASE_UP_PHASE,
+ ease_up_percent: float = _DEFAULT_SWING_EASE_UP_PERCENT,
+ feed_forward_torques: Sequence[float] = _FEED_FORWARD_TORQUES
+ ):
+ """Initializes the class.
+
+ Args:
+ robot: A robot instance.
+ gait_generator: Generates the stance/swing pattern.
+ state_estimator: Estiamtes the CoM speeds.
+ desired_speed: Behavior parameters. X-Y speed.
+ desired_twisting_speed: Behavior control parameters.
+ desired_height: Desired standing height.
+ foot_clearance: The foot clearance on the ground at the end of the swing
+ cycle.
+ local_hip_positions: Positions of the robot's hips in local frames.
+ ease_up_phase: Time length for the initial ease up phase dueing swing
+ cycle.
+ ease_up_percent: Percentage of the swing cycle completed after
+ ease_up_phase.
+ feed_forward_torques: A feed-forward torque applied to the actuators on
+ the swing legs (e.g. for gravity compensation).
+ """
+ self._robot = robot
+ self._state_estimator = state_estimator
+ self._gait_generator = gait_generator
+ self._last_leg_state = gait_generator.desired_leg_state
+ self.desired_speed = np.array((desired_speed[0], desired_speed[1], 0))
+ self.desired_twisting_speed = desired_twisting_speed
+ self._desired_height = np.array((0, 0, desired_height - foot_clearance))
+ self._local_hip_positions = local_hip_positions
+ self._ease_up_phase = ease_up_phase
+ self._ease_up_percent = ease_up_percent
+ self._feed_forward_torques = feed_forward_torques
+
+ self._joint_angles = None
+ self._phase_switch_foot_local_position = None
+ self.reset(0)
+
+ def reset(self, current_time: float) -> None:
+ """Called during the start of a swing cycle.
+
+ Args:
+ current_time: The wall time in seconds.
+ """
+ del current_time
+ self._last_leg_state = self._gait_generator.desired_leg_state
+ self._phase_switch_foot_local_position = (self._robot.foot_positions())
+ self._joint_angles = {}
+
+ def update(self, current_time: float) -> None:
+ """Called at each control step.
+
+ Args:
+ current_time: The wall time in seconds.
+ """
+ del current_time
+ new_leg_state = self._gait_generator.desired_leg_state
+
+ # Detects phase switch for each leg so we can remember the feet position at
+ # the beginning of the swing phase.
+ for leg_id, state in enumerate(new_leg_state):
+ if (state == gait_generator_lib.LegState.SWING and
+ state != self._last_leg_state[leg_id]):
+ self._phase_switch_foot_local_position[leg_id] = (
+ self._robot.foot_positions()[leg_id])
+
+ self._last_leg_state = copy.deepcopy(new_leg_state)
+
+ def get_action(self) -> Mapping[Any, Any]:
+ com_velocity = self._state_estimator.com_velocity_body_yaw_aligned_frame
+ com_velocity = np.array((com_velocity[0], com_velocity[1], 0))
+
+ _, _, yaw_dot = self._robot.base_roll_pitch_yaw_rate
+
+ local_toe_positions = np.array(self._robot.foot_positions())
+
+ for leg_id, leg_state in enumerate(self._gait_generator.leg_state):
+ if (leg_state == gait_generator_lib.LegState.STANCE or
+ leg_state == gait_generator_lib.LegState.EARLY_CONTACT):
+ continue
+
+ # For now we did not consider the body pitch/roll and all calculation is
+ # in the body frame. TODO(b/143378213): Calculate the foot_target_position
+ # in world farme and then project back to calculate the joint angles.
+ hip_offset = self._local_hip_positions[leg_id]
+ twisting_vector = np.array((-hip_offset[1], hip_offset[0], 0))
+ hip_horizontal_velocity = com_velocity + yaw_dot * twisting_vector
+ target_hip_horizontal_velocity = (
+ self.desired_speed + self.desired_twisting_speed * twisting_vector)
+
+ foot_target_position = (
+ hip_horizontal_velocity *
+ self._gait_generator.swing_duration[leg_id] / 2 - _KP *
+ (target_hip_horizontal_velocity - hip_horizontal_velocity)
+ ) - self._desired_height + np.array((hip_offset[0], hip_offset[1], 0))
+
+ foot_position = _gen_swing_foot_trajectory(
+ self._gait_generator.normalized_phase[leg_id],
+ self._phase_switch_foot_local_position[leg_id], foot_target_position,
+ self._ease_up_phase, self._ease_up_percent)
+
+ local_toe_positions[leg_id] = foot_position
+ joint_ids, joint_angles = self._robot.motor_angles_from_foot_positions(
+ local_toe_positions, position_in_world_frame=False)
+ # Update the stored joint angles as needed.
+ motors_per_leg = len(joint_ids) // len(local_toe_positions)
+ for joint_id, joint_angle in zip(joint_ids, joint_angles):
+ self._joint_angles[joint_id] = (joint_angle, joint_id // motors_per_leg)
+
+ action = {}
+ kps, kds = self._robot.motor_model.get_motor_gains()
+
+ for joint_id, joint_angle_leg_id in self._joint_angles.items():
+ leg_id = joint_angle_leg_id[1]
+ if self._gait_generator.leg_state[
+ leg_id] == gait_generator_lib.LegState.SWING:
+ # This is a hybrid action for PD control.
+ action[joint_id] = (joint_angle_leg_id[0], kps[joint_id], 0,
+ kds[joint_id], self._feed_forward_torques[joint_id])
+
+ return action
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/state_estimator.py b/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/state_estimator.py
new file mode 100644
index 000000000..6b12a05ad
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/state_estimator.py
@@ -0,0 +1,21 @@
+"""State estimator."""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+import abc
+
+
+class StateEstimatorBase(object): # pytype: disable=ignored-metaclass
+ """Estimates the unmeasurable state of the robot."""
+
+ __metaclass__ = abc.ABCMeta
+
+ @abc.abstractmethod
+ def reset(self, current_time):
+ pass
+
+ @abc.abstractmethod
+ def update(self, current_time):
+ pass
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/static_gait_controller.py b/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/static_gait_controller.py
new file mode 100644
index 000000000..eb4ccdd1c
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/static_gait_controller.py
@@ -0,0 +1,63 @@
+# Lint as: python3
+"""A static gait controller for a quadruped robot. Experimental code."""
+
+import gin
+import numpy as np
+from pybullet_envs.minitaur.agents.baseline_controller import foot_stepper
+
+toe_pos_local_ref = np.array([[0.1478, -0.11459, -0.45576],
+ [0.1478, 0.11688, -0.45576],
+ [-0.2895, -0.11459, -0.45576],
+ [-0.2895, 0.11688, -0.45576]])
+
+
+@gin.configurable
+class StaticGaitController(object):
+ """A static gait controller for a quadruped robot."""
+
+ def __init__(self, robot):
+ self._robot = robot
+ self._toe_ids = tuple(robot.urdf_loader.get_end_effector_id_dict().values())
+ self._wait_count = 0
+ self._stepper = foot_stepper.FootStepper(self._robot.pybullet_client,
+ self._toe_ids, toe_pos_local_ref)
+
+ def act(self, observation):
+ """Computes actions based on observations."""
+ del observation
+ p = self._robot.pybullet_client
+ quadruped = self._robot.robot_id
+ step_input = foot_stepper.StepInput()
+ ls = p.getLinkStates(
+ quadruped, self._toe_ids, computeForwardKinematics=True)
+ toe_pos_world = np.array([ls[0][0], ls[1][0], ls[2][0], ls[3][0]])
+ base_com_pos, base_com_orn = p.getBasePositionAndOrientation(quadruped)
+ new_pos_world = np.array([0, 0, 0])
+
+ if self._stepper.is_com_stable() and not self._stepper.move_swing_foot:
+ self._wait_count += 1
+ if self._wait_count == 20:
+ self._stepper.next_foot()
+ if self._wait_count > 50:
+ self._wait_count = 0
+ step_dist = 0.15
+ print("time {}, make a step of {}".format(
+ self._robot.GetTimeSinceReset(), step_dist))
+ new_pos_local = self._stepper.get_reference_pos_swing_foot()
+ new_pos_local[0] += step_dist
+ new_pos_world, _ = p.multiplyTransforms(base_com_pos, base_com_orn,
+ new_pos_local, [0, 0, 0, 1])
+ self._stepper.swing_foot()
+
+ step_input.new_pos_world = new_pos_world
+ step_input.base_com_pos = base_com_pos
+ step_input.base_com_orn = base_com_orn
+ step_input.toe_pos_world = toe_pos_world
+ step_input.dt = 1.0 / 250
+ step_output = self._stepper.update(step_input)
+
+ # Finds joint poses to achieve toePosWorld
+ desired_joint_angles = self._robot.motor_angles_from_foot_positions(
+ foot_positions=step_output.new_toe_pos_world,
+ position_in_world_frame=True)[1]
+ return desired_joint_angles
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/time_based_moving_window_filter.py b/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/time_based_moving_window_filter.py
new file mode 100644
index 000000000..42868795e
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/time_based_moving_window_filter.py
@@ -0,0 +1,45 @@
+"""A moving-window filter for smoothing the signals within certain time interval."""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+
+import gin
+import numpy as np
+
+
+@gin.configurable
+class TimeBasedMovingWindowFilter:
+ """A moving-window filter for smoothing the signals within certain time interval."""
+
+ def __init__(
+ self,
+ filter_window: float = 0.1,
+ ):
+ """Initializes the class.
+
+ Args:
+ filter_window: The filtering window (in time) used to smooth the input
+ signal.
+ """
+ self._filter_window = filter_window
+ self.reset()
+
+ def reset(self):
+ self._timestamp_buffer = []
+ self._value_buffer = []
+
+ def calculate_average(self, new_value, timestamp):
+ """Compute the filtered signals based on the time-based moving window."""
+ self._timestamp_buffer.append(timestamp)
+ self._value_buffer.append(new_value)
+
+ while len(self._value_buffer) > 1:
+ if self._timestamp_buffer[
+ 0] < timestamp - self._filter_window:
+ self._timestamp_buffer.pop(0)
+ self._value_buffer.pop(0)
+ else:
+ break
+ return np.mean(self._value_buffer, axis=0)
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/torque_stance_leg_controller.py b/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/torque_stance_leg_controller.py
new file mode 100644
index 000000000..87ebed458
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/agents/baseline_controller/torque_stance_leg_controller.py
@@ -0,0 +1,229 @@
+# Lint as: python3
+"""A torque based stance controller framework."""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+import functools
+import logging
+from typing import Any, Sequence, Tuple
+
+import gin
+import numpy as np
+
+from pybullet_envs.minitaur.agents.baseline_controller import gait_generator as gait_generator_lib
+from pybullet_envs.minitaur.agents.baseline_controller import leg_controller
+#from pybullet_envs.minitaur.agents.baseline_controller.convex_mpc.python import convex_mpc
+#from google3.util.task.python import error
+
+try:
+ import mpc_osqp as convex_mpc # pytype: disable=import-error
+except: #pylint: disable=W0702
+ print("You need to install motion_imitation")
+ print("or use pip3 install motion_imitation --user")
+ print("see also https://github.com/google-research/motion_imitation")
+ import sys
+ sys.exit()
+
+
+_FORCE_DIMENSION = 3
+# The QP weights in the convex MPC formulation. See the MIT paper for details:
+# https://ieeexplore.ieee.org/document/8594448/
+# Intuitively, this is the weights of each state dimension when tracking a
+# desired CoM trajectory. The full CoM state is represented by
+# (roll_pitch_yaw, position, angular_velocity, velocity, gravity_place_holder).
+_MPC_WEIGHTS = (5, 5, 0.2, 0, 0, 10, 0.5, 0.5, 0.2, 0.2, 0.2, 0.1, 0)
+_PLANNING_HORIZON_STEPS = 10
+_PLANNING_TIMESTEP = 0.025
+#_MPC_CONSTRUCTOR = functools.partial(
+# convex_mpc.ConvexMpc, qp_solver_name=convex_mpc.QPSolverName.QPOASES)
+
+
+@gin.configurable
+class TorqueStanceLegController(leg_controller.LegController):
+ """A torque based stance leg controller framework.
+
+ Takes in high level parameters like walking speed and turning speed, and
+ generates necessary the torques for stance legs.
+ """
+
+ def __init__(
+ self,
+ robot: Any,
+ gait_generator: Any,
+ state_estimator: Any,
+ desired_speed: Tuple[float] = (0, 0),
+ desired_twisting_speed: float = 0,
+ desired_roll_pitch: Tuple[float] = (0, 0),
+ desired_body_height: float = 0.45,
+ body_mass: float = 220 / 9.8,
+ body_inertia: Tuple[float] = (0.183375, 0, 0, 0, 0.6267, 0, 0, 0,
+ 0.636175),
+ num_legs: int = 4,
+ friction_coeffs: Sequence[float] = (0.5, 0.5, 0.5, 0.5),
+ qp_weights: Sequence[float] = _MPC_WEIGHTS,
+ planning_horizon: int = _PLANNING_HORIZON_STEPS,
+ planning_timestep: int = _PLANNING_TIMESTEP,
+ ):
+ """Initializes the class.
+
+ Tracks the desired position/velocity of the robot by computing proper joint
+ torques using MPC module.
+
+ Args:
+ robot: A robot instance.
+ gait_generator: Used to query the locomotion phase and leg states.
+ state_estimator: Estimate the robot states (e.g. CoM velocity).
+ desired_speed: desired CoM speed in x-y plane.
+ desired_twisting_speed: desired CoM rotating speed in z direction.
+ desired_roll_pitch: desired CoM roll and pitch.
+ desired_body_height: The standing height of the robot.
+ body_mass: The total mass of the robot.
+ body_inertia: The inertia matrix in the body principle frame. We assume
+ the body principle coordinate frame has x-forward and z-up.
+ num_legs: The number of legs used for force planning.
+ friction_coeffs: The friction coeffs on the contact surfaces.
+ qp_weights: The weights used in solving the QP problem.
+ planning_horizon: Number of steps to roll-out in the QP formulation.
+ planning_timestep: Timestep between each step in the QP formulation.
+ """
+
+ self._robot = robot
+ self._gait_generator = gait_generator
+ self._state_estimator = state_estimator
+ self._desired_speed = desired_speed
+ self._desired_twisting_speed = desired_twisting_speed
+ self._desired_roll_pitch = desired_roll_pitch
+ self._desired_body_height = desired_body_height
+ self._body_mass = body_mass
+ self._num_legs = num_legs
+ self._friction_coeffs = np.array(friction_coeffs)
+ self._qp_solver_fail = False
+ self._com_estimate_leg_indices = None
+ qp_solver = convex_mpc.QPOASES #convex_mpc.OSQP #
+
+ body_inertia_list = list(body_inertia)
+ weights_list = list(qp_weights)
+
+ self._mpc = convex_mpc.ConvexMpc(
+ body_mass,
+ body_inertia_list,
+ self._num_legs,
+ planning_horizon,
+ planning_timestep,
+ weights_list,
+ 1e-6,
+ qp_solver
+ )
+
+
+ def reset(self, current_time):
+ del current_time
+ self._qp_solver_fail = False
+ self._com_estimate_leg_indices = None
+
+ def update(self, current_time):
+ del current_time
+
+ def get_action(self):
+ """Computes the torque for stance legs."""
+ desired_com_position = np.array((0., 0., self._desired_body_height),
+ dtype=np.float64)
+ desired_com_velocity = np.array(
+ (self.desired_speed[0], self.desired_speed[1], 0.), dtype=np.float64)
+ desired_com_roll_pitch_yaw = np.array(
+ (self.desired_roll_pitch[0], self.desired_roll_pitch[1], 0.),
+ dtype=np.float64)
+ desired_com_angular_velocity = np.array(
+ (0., 0., self.desired_twisting_speed), dtype=np.float64)
+ foot_contact_state = np.array(
+ [(leg_state == gait_generator_lib.LegState.STANCE or
+ leg_state == gait_generator_lib.LegState.EARLY_CONTACT)
+ for leg_state in self._gait_generator.desired_leg_state],
+ dtype=np.int32)
+
+ # We use the body yaw aligned world frame for MPC computation.
+ com_roll_pitch_yaw = np.array(
+ self._robot.base_roll_pitch_yaw, dtype=np.float64)
+ com_roll_pitch_yaw[2] = 0
+ #try:
+ estimated_com_position = np.array(())
+ if hasattr(self._state_estimator, "estimated_com_height"):
+ estimated_com_position = np.array(
+ (0, 0, self._state_estimator.estimated_com_height))
+ try:
+ predicted_contact_forces = self._mpc.compute_contact_forces(
+ estimated_com_position, #com_position
+ np.asarray(self._state_estimator.com_velocity_body_yaw_aligned_frame,
+ dtype=np.float64), #com_velocity
+ np.array(com_roll_pitch_yaw, dtype=np.float64), #com_roll_pitch_yaw
+ # Angular velocity in the yaw aligned world frame is actually different
+ # from rpy rate. We use it here as a simple approximation.
+ np.asarray(self._robot.base_roll_pitch_yaw_rate,
+ dtype=np.float64), #com_angular_velocity
+ foot_contact_state, #foot_contact_states
+ np.array(self._robot.foot_positions(
+ position_in_world_frame=False).flatten(),
+ dtype=np.float64), #foot_positions_base_frame
+ self._friction_coeffs, #foot_friction_coeffs
+ desired_com_position, #desired_com_position
+ desired_com_velocity, #desired_com_velocity
+ desired_com_roll_pitch_yaw, #desired_com_roll_pitch_yaw
+ desired_com_angular_velocity #desired_com_angular_velocity
+ )
+ except:# error.StatusNotOk as e:
+ logging.error("Error in Torque Stance Leg")#e.message)
+ self._qp_solver_fail = True
+ predicted_contact_forces = np.zeros(self._num_legs * _FORCE_DIMENSION)
+
+ contact_forces = {}
+ for i in range(self._num_legs):
+ contact_forces[i] = np.array(
+ predicted_contact_forces[i * _FORCE_DIMENSION:(i + 1) *
+ _FORCE_DIMENSION])
+
+ _, kds = self._robot.motor_model.get_motor_gains()
+ action = {}
+ for leg_id, force in contact_forces.items():
+ motor_torques = self._robot.map_contact_force_to_joint_torques(
+ leg_id, force)
+ for joint_id, torque in motor_torques.items():
+ action[joint_id] = (0, 0, 0, kds[joint_id], torque)
+ return action
+
+ @property
+ def qp_solver_fail(self):
+ return self._qp_solver_fail
+
+ @property
+ def desired_speed(self):
+ return self._desired_speed
+
+ @desired_speed.setter
+ def desired_speed(self, speed):
+ self._desired_speed = speed
+
+ @property
+ def desired_twisting_speed(self):
+ return self._desired_twisting_speed
+
+ @desired_twisting_speed.setter
+ def desired_twisting_speed(self, twisting_speed):
+ self._desired_twisting_speed = twisting_speed
+
+ @property
+ def desired_roll_pitch(self):
+ return self._desired_roll_pitch
+
+ @desired_roll_pitch.setter
+ def desired_roll_pitch(self, roll_pitch):
+ self._desired_roll_pitch = roll_pitch
+
+ @property
+ def desired_body_height(self):
+ return self._desired_body_height
+
+ @desired_body_height.setter
+ def desired_body_height(self, body_height):
+ self._desired_body_height = body_height
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/agents/trajectory_generator/controller_simple.py b/examples/pybullet/gym/pybullet_envs/minitaur/agents/trajectory_generator/controller_simple.py
new file mode 100644
index 000000000..746e4cfd5
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/agents/trajectory_generator/controller_simple.py
@@ -0,0 +1,87 @@
+"""Asymmetric sine controller for quadruped locomotion.
+
+Asymmetric sine uses cosine and sine waves to generate swinging and extension
+for leg motion. It's asymmetric because sine waves are split into two phases
+(swing forward and stance) and these phases have different frequencies according
+to what proportion of a period will be spend on swinging forward forward
+swinging backwards. In addition, the sine wave for extension has different
+amplitudes during these two phases.
+"""
+import math
+
+TWO_PI = 2 * math.pi
+_DEFAULT_LEG_AMPLITUDE_EXTENSION = -0.02
+_DEFAULT_LEG_AMPLITUDE_SWING = 0.5
+_DEFAULT_LEG_AMPLITUDE_LIFT = 0.9
+_DEFAULT_WALKING_HEIGHT = 0.0
+_DEFAULT_LEG_CENTER_SWING = 0.0
+_DELTA_CENTER_EXTENSION_CAP = 1
+_DELTA_INTENSITY_CAP = 0.1
+
+
+class SimpleLegController(object):
+ """Controller that gives swing and extension based on phase and parameters.
+
+
+ The controller returns the swing-extend pair based on a parameterized
+ ellipsoid trajectory that depends on center of motion, amplitude and phase.
+ The parameters are
+ amplitude_extension: Amplitude for extension during stance (phase < pi).
+ amplitude_lift: Amplitude for extension during swing (phase > pi).
+ amplitude_swing: Amplitude for swing.
+ center_extension: The value extension signal oscillates around.
+ center_swing: The value swing signal oscillates around.
+ intensity: A coefficient that scales the motion of the legs.
+ The formula to calculate motion and more detailed information about these
+ parameters can be found at go/pmtg-refactored.
+ """
+
+ def __init__(self, init_phase=0):
+ self.amplitude_extension = _DEFAULT_LEG_AMPLITUDE_EXTENSION
+ self.amplitude_swing = _DEFAULT_LEG_AMPLITUDE_SWING
+ self.amplitude_lift = _DEFAULT_LEG_AMPLITUDE_LIFT
+ self.center_extension = _DEFAULT_WALKING_HEIGHT
+ self.center_swing = _DEFAULT_LEG_CENTER_SWING
+ self.intensity = 1.0
+ self._init_phase = init_phase
+ self.phase = init_phase
+ self.phase_offset = 0
+
+ def reset(self):
+ self.phase = self._init_phase
+
+ def get_swing_extend(self):
+ """Returns the swing and extend parameters for the leg.
+
+ Returns:
+ swing: Desired swing of the leg.
+ extend: Desired extension amount of the leg.
+ """
+
+ # Increase default extension by the extra extension scaled by intensity.
+ # Extend reduces to default center extension when intensity goes to 0,
+ # because we prefer the legs to stay at walking height when intensity is
+ # 0.
+ amplitude_extension = self.amplitude_extension
+ # The leg is in swing phase when phase > pi.
+ if self.phase > math.pi:
+ amplitude_extension = self.amplitude_lift
+ extend = self.center_extension + (
+ amplitude_extension * math.sin(self.phase)) * self.intensity
+ # Calculate the swing based on the signal and scale it with intensity.
+ # Swing reduces to 0 when intensity goes to 0, because we would prefer the
+ # legs to stay neutral (standing position instead of center swing) when
+ # intensity is 0.
+ swing = self.center_swing + self.amplitude_swing * math.cos(self.phase)
+ swing *= self.intensity
+
+ return swing, extend
+
+ def adjust_center_extension(self, target_center_extension):
+ delta = min(_DELTA_CENTER_EXTENSION_CAP,
+ target_center_extension - self.center_extension)
+ self.center_extension += delta
+
+ def adjust_intensity(self, target_intensity):
+ delta = min(_DELTA_INTENSITY_CAP, target_intensity - self.intensity)
+ self.intensity += delta
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/agents/trajectory_generator/tg_inplace.py b/examples/pybullet/gym/pybullet_envs/minitaur/agents/trajectory_generator/tg_inplace.py
new file mode 100644
index 000000000..b07048e75
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/agents/trajectory_generator/tg_inplace.py
@@ -0,0 +1,57 @@
+"""Trajectory Generator for in-place stepping motion for quadruped robot."""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+import math
+import numpy as np
+
+TWO_PI = 2 * math.pi
+
+
+def _get_actions_asymmetric_sine(phase, tg_params):
+ """Returns the leg extension given current phase of TG and parameters.
+
+ Args:
+ phase: a number in [0, 2pi) representing current leg phase
+ tg_params: a dictionary of tg parameters:
+ stance_lift_cutoff -- switches the TG between stance (phase < cutoff) and
+ lift (phase > cutoff) phase
+ amplitude_swing -- amplitude in swing phase
+ amplitude_lift -- amplitude in lift phase
+ center_extension -- center of leg extension
+ """
+ stance_lift_cutoff = tg_params['stance_lift_cutoff']
+ a_prime = np.where(phase < stance_lift_cutoff, tg_params['amplitude_stance'],
+ tg_params['amplitude_lift'])
+ scaled_phase = np.where(
+ phase > stance_lift_cutoff, np.pi + (phase - stance_lift_cutoff) /
+ (TWO_PI - stance_lift_cutoff) * np.pi, phase / stance_lift_cutoff * np.pi)
+ return tg_params['center_extension'] + a_prime * np.sin(scaled_phase)
+
+
+def step(current_phases, leg_frequencies, dt, tg_params):
+ """Steps forward the in-place trajectory generator.
+
+ Args:
+ current_phases: phases of each leg.
+ leg_frequencies: the frequency to proceed the phase of each leg.
+ dt: amount of time (sec) between consecutive time steps.
+ tg_params: a set of parameters for trajectory generator, see the docstring
+ of "_get_actions_asymmetric_sine" for details.
+
+ Returns:
+ actions: leg swing/extensions as output by the trajectory generator.
+ new_state: new swing/extension.
+ """
+ new_phases = np.fmod(current_phases + TWO_PI * leg_frequencies * dt, TWO_PI)
+ extensions = []
+ for leg_id in range(4):
+ extensions.append(
+ _get_actions_asymmetric_sine(new_phases[..., leg_id], tg_params))
+ return new_phases, extensions
+
+
+def reset():
+ return np.array([0, np.pi * 0.5, np.pi, np.pi * 1.5])
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/agents/trajectory_generator/tg_simple.py b/examples/pybullet/gym/pybullet_envs/minitaur/agents/trajectory_generator/tg_simple.py
new file mode 100644
index 000000000..c63884d81
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/agents/trajectory_generator/tg_simple.py
@@ -0,0 +1,400 @@
+"""Trajectory Generator generates walking leg motions for a quadruped robot.
+
+Trajectory Generator (TG) has an internal state (phase) and generates
+walking-like motion for 8 motors of minitaur quadruped robot based on
+parameters
+such as:
+ - delta time to progress the TG's internal state.
+ - intensity to control amount of movement (stride length and lift of the legs).
+ - waking height to control the average extension of the legs.
+
+Each time step() is called, the internal state is progressed and 8 motor
+positions are generated. This TG uses the open-loop SineController class to
+provide leg positions. It is mainly a wrapper for ability to modulating the
+time
+and other parameters of the SineController.
+"""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+import math
+import numpy as np
+import gin
+from pybullet_envs.minitaur.agents.trajectory_generator import controller_simple
+
+PHASE_LOWER_BOUND = 0.0
+PHASE_UPPER_BOUND = 1.0
+WALK_HEIGHT_LOWER_BOUND = -0.5
+WALK_HEIGHT_UPPER_BOUND = 1.0
+INTENSITY_LOWER_BOUND = 0.0
+INTENSITY_UPPER_BOUND = 1.5
+_SWING_STANCE_LOWER_BOUND = 0.2
+_SWING_STANCE_UPPER_BOUND = 5.0
+_DELTA_SWING_STANCE_CAP = 0.4
+_TWO_PI = math.pi * 2.0
+_LEG_COUPLING_DICT = {
+ "null": [],
+ # All the legs are coupled.
+ "all coupled": [0, 0, 0, 0],
+ # Front legs and back legs are coupled separately.
+ "front back": [0, 1, 0, 1],
+ # Left legs and right legs are coupled separately.
+ "left right": [0, 0, 1, 1],
+ # Diagonal legs are coupled (i.e. trottting).
+ "diagonal": [0, 1, 1, 0],
+ # Each leg is indepenent.
+ "decoupled": [0, 1, 2, 3]
+}
+
+
+@gin.configurable
+class TgSimple(object):
+ """TgSimple class is a simplified trajectory generator for quadruped walking.
+
+ It returns 8 actions for quadruped slow walking behavior
+ based on the parameters provided such as intensity, walking height and delta
+ time. It returns its internal phase as information.
+ """
+
+ def __init__(self,
+ walk_height_lower_bound=WALK_HEIGHT_LOWER_BOUND,
+ walk_height_upper_bound=WALK_HEIGHT_UPPER_BOUND,
+ intensity_lower_bound=INTENSITY_LOWER_BOUND,
+ intensity_upper_bound=INTENSITY_UPPER_BOUND,
+ swing_stance_lower_bound=_SWING_STANCE_LOWER_BOUND,
+ swing_stance_upper_bound=_SWING_STANCE_UPPER_BOUND,
+ integrator_coupling_mode="all coupled",
+ walk_height_coupling_mode="all coupled",
+ variable_swing_stance_ratio=False,
+ swing_stance_ratio=1.0,
+ init_leg_phase_offsets=None):
+ """Initialize the trajectory generator with a controller.
+
+ For trajectory generator, we create an asymmetric sine controller with
+ parameters that was previously optimized as an open-loop controller.
+
+ Args:
+ walk_height_lower_bound: Lower bound for walking height which sets the
+ default leg extension of the gait. Unit is rad, -0.5 by default.
+ walk_height_upper_bound: Lower bound for walking height which sets the
+ default leg extension of the gait. Unit is rad, 1.0 by default.
+ intensity_lower_bound: The upper bound for intensity of the trajectory
+ generator. It can be used to limit the leg movement.
+ intensity_upper_bound: The upper bound for intensity of the trajectory
+ generator. It can be used to limit the leg movement.
+ swing_stance_lower_bound: Lower bound for the swing vs stance ratio
+ parameter. Default value is 0.2.
+ swing_stance_upper_bound: Upper bound for the swing vs stance ratio
+ parameter. Default value is 0.2.
+ integrator_coupling_mode: How the legs should be coupled for integrators.
+ walk_height_coupling_mode: The same coupling mode used for walking
+ heights for the legs.
+ variable_swing_stance_ratio: A boolean to indicate if the swing stance
+ ratio can change per time step or not.
+ swing_stance_ratio: Time taken by swing phase vs stance phase. This is
+ only relevant if variable_swing_stance_ratio is False.
+ init_leg_phase_offsets: The initial phases of the legs. A list of 4
+ variables within [0,1). The order is front-left, rear-left, front-right
+ and rear-right.
+
+ Raises:
+ ValueError: If parameters are not valid values.
+ """
+ self._walk_height_lower_bound = walk_height_lower_bound
+ self._walk_height_upper_bound = walk_height_upper_bound
+ self._intensity_lower_bound = intensity_lower_bound
+ self._intensity_upper_bound = intensity_upper_bound
+ self._swing_stance_lower_bound = swing_stance_lower_bound
+ self._swing_stance_upper_bound = swing_stance_upper_bound
+ if not init_leg_phase_offsets:
+ init_leg_phase_offsets = [0, 0.25, 0.5, 0.75]
+ if len(init_leg_phase_offsets) != 4:
+ raise ValueError("The number of leg phase offsets is not equal to 4.")
+ if min(init_leg_phase_offsets) < 0 or max(init_leg_phase_offsets) >= 1:
+ raise ValueError("Leg phase offsets are not within [0,1)")
+ self._legs = []
+ for period in init_leg_phase_offsets:
+ init_phase = period * 2 * math.pi
+ self._legs.append(controller_simple.SimpleLegController(init_phase))
+
+ if integrator_coupling_mode not in _LEG_COUPLING_DICT:
+ raise ValueError("Invalid integrator_coupling_mode.")
+ if walk_height_coupling_mode not in _LEG_COUPLING_DICT:
+ raise ValueError("Invalid walk_height_coupling_mode.")
+
+ # Set the phase couplings and build a list of legs per phase coupling.
+ self._integrator_id_per_leg = _LEG_COUPLING_DICT[integrator_coupling_mode]
+ self._num_integrators = max(
+ self._integrator_id_per_leg) + 1 if self._integrator_id_per_leg else 0
+ self._legs_per_integrator_id = [[], [], [], []]
+ for idx, phase_id in enumerate(self._integrator_id_per_leg):
+ self._legs_per_integrator_id[phase_id].append(self._legs[idx])
+
+ # For each integrator coupling, create a integrator unit.
+ # For each leg controlled by that phase generator, mark the phase offset.
+ self._integrator_units = []
+ for legs_per_integrator in self._legs_per_integrator_id:
+ if legs_per_integrator:
+ circular_integrator = CircularAsymmetricalIntegratorUnit(
+ legs_per_integrator[0].phase)
+ self._integrator_units.append(circular_integrator)
+ for leg in legs_per_integrator:
+ leg.phase_offset = leg.phase - circular_integrator.phase
+
+ # Set the walking heights couplings.
+ self._walk_height_id_per_leg = _LEG_COUPLING_DICT[walk_height_coupling_mode]
+ self._num_walk_heights = max(
+ self._walk_height_id_per_leg) + 1 if self._walk_height_id_per_leg else 0
+ self._variable_swing_stance_ratio = variable_swing_stance_ratio
+ self._swing_stance_ratio = swing_stance_ratio
+
+ def reset(self):
+ """Resets leg phase offsets to their initial values."""
+ for leg in self._legs:
+ leg.reset()
+ for circular_integrator in self._integrator_units:
+ circular_integrator.reset()
+
+ def get_parameter_bounds(self):
+ """Lower and upper bounds for the parameters generator's parameters.
+
+ Returns:
+ 2-tuple of:
+ - Lower bounds for the parameters such as intensity, walking height and
+ lift fraction.
+ - Upper bounds for the same parameters.
+ """
+ lower_bounds = [self._intensity_lower_bound]
+ upper_bounds = [self._intensity_upper_bound]
+ lower_bounds += [self._walk_height_lower_bound] * self._num_walk_heights
+ upper_bounds += [self._walk_height_upper_bound] * self._num_walk_heights
+ lower_bounds += [self._swing_stance_lower_bound
+ ] * self._variable_swing_stance_ratio
+ upper_bounds += [self._swing_stance_upper_bound
+ ] * self._variable_swing_stance_ratio
+
+ return lower_bounds, upper_bounds
+
+ def get_actions(self, delta_real_time, tg_params):
+ """Get actions for 8 motors after increasing the phase delta_time.
+
+ Args:
+ delta_real_time: Time in seconds that have actually passed since the last
+ step of the trajectory generator.
+ tg_params: An ndarray of the parameters for generating the trajectory. The
+ parameters must be in the correct order (time_scale, intensity,
+ walking_height, and swing vs stance)
+
+ Raises:
+ ValueError: In case the input dimension does not match expected.
+ Returns:
+ The rotations for all the 8 motors for this time step
+ returned in an array [front_left_motor_1, front_left_motor_2, etc].
+ """
+ speeds, intensity, heights, swing_stance_ratio = self._process_tg_params(
+ tg_params)
+ # Adjust the swing stance ratio of the controller (used for all four legs).
+ if swing_stance_ratio:
+ self.adjust_swing_stance_ratio(swing_stance_ratio)
+ # Adjust the walking height, intensity and swing vs stance of the legs.
+ for idx, leg in enumerate(self._legs):
+ leg.adjust_intensity(intensity)
+ if heights:
+ leg.adjust_center_extension(heights[self._walk_height_id_per_leg[idx]])
+
+ # Progress all the phase generators based on delta time.
+ for idx, integrator_unit in enumerate(self._integrator_units):
+ integrator_unit.progress_phase(speeds[idx] * delta_real_time,
+ self._swing_stance_ratio)
+
+ # Set the phases for the legs based on their offsets with phase generators.
+ for phase_id, leg_list in enumerate(self._legs_per_integrator_id):
+ for leg in leg_list:
+ delta_period = leg.phase_offset / (2.0 * math.pi)
+ leg.phase = self._integrator_units[phase_id].calculate_progressed_phase(
+ delta_period, self._swing_stance_ratio)
+
+ # Calculate swingextend and convert it to the motor rotations.
+ actions = []
+ for idx, leg in enumerate(self._legs):
+ swing, extend = leg.get_swing_extend()
+ actions.extend([swing, extend])
+ return actions
+
+ def _process_tg_params(self, tg_params):
+ """Process the trajectory generator parameters and split them.
+
+ Args:
+ tg_params: A list consisting of time_scales, intensity, walking_heights,
+ swing_stance_ratio. The size depends on the configuration and inital
+ flags.
+
+ Returns:
+ time_scales: A list of multipliers of delta time (one per integrator).
+ intensity: Intensity of the trajectory generator (one variable).
+ walking_heights: Walking heights used for the legs. The length depends on
+ the coupling between the legs selected at the initialization.
+ swing_stance_ratio: The ratio of the speed of the leg during swing stance
+ vs stance phase.
+ """
+
+ # Check if the given input's dimension matches the expectation considering
+ # the number of parameters the trajectory generator uses.
+ if isinstance(tg_params, np.ndarray):
+ tg_params = tg_params.tolist()
+ expected_action_dim = 1 + self._num_integrators + self._num_walk_heights
+ if self._variable_swing_stance_ratio:
+ expected_action_dim += 1
+ if len(tg_params) != expected_action_dim:
+ raise ValueError(
+ "Action dimension does not match the expectation {} vs {}".format(
+ len(tg_params), expected_action_dim))
+ # Split input into different parts based on type. The order must match the
+ # order given by the order in get_parameter_bounds
+ time_scales = tg_params[0:self._num_integrators]
+ intensity = tg_params[self._num_integrators]
+ walking_heights = tg_params[(self._num_integrators + 1):(
+ 1 + self._num_integrators + self._num_walk_heights)]
+ swing_stance_ratio = None
+ if self._variable_swing_stance_ratio:
+ swing_stance_ratio = tg_params[1 + self._num_integrators +
+ self._num_walk_heights]
+
+ return time_scales, intensity, walking_heights, swing_stance_ratio
+
+ def get_state(self):
+ """Returns a list of floats representing the phase of the controller.
+
+ The phase of the controller is composed of the phases of the integrators.
+ For each integrator, the phase is composed of 2 floats that represents the
+ sine and cosine of the phase of that integrator.
+
+ Returns:
+ List containing sine and cosine of the phases of all the integrators.
+ """
+ return [x for y in self._integrator_units for x in y.get_state()]
+
+ def get_state_lower_bounds(self):
+ """Lower bounds for the internal state.
+
+ Returns:
+ The list containing the lower bounds.
+ """
+ return [PHASE_LOWER_BOUND] * 2 * self._num_integrators
+
+ def get_state_upper_bounds(self):
+ """Upper bounds for the internal state.
+
+ Returns:
+ The list containing the upper bounds.
+ """
+ return [PHASE_UPPER_BOUND] * 2 * self._num_integrators
+
+ def adjust_swing_stance_ratio(self, target_swing_stance_ratio):
+ """Adjust the parameter swing_stance_ratio towards a given target value.
+
+ Args:
+ target_swing_stance_ratio: The target value for the ratio between swing
+ and stance phases.
+ """
+ delta = min(_DELTA_SWING_STANCE_CAP,
+ target_swing_stance_ratio - self._swing_stance_ratio)
+ self._swing_stance_ratio += delta
+
+ @property
+ def num_integrators(self):
+ """Gets the number of integrators used based on coupling mode."""
+ return self._num_integrators
+
+
+class CircularAsymmetricalIntegratorUnit(object):
+ """A circular integrator with asymmetry between first and second half.
+
+ An integrator is a memory unit that accumulates the given parameter at every
+ time step.
+ A circular integrator is when the integrator cycles within [0,2pi].
+ The phase of a circular integrator indicates the accumulated number and it is
+ stored as fmod of 2Pi.
+ Asymmetrical circular integrator has a further characteristic where it
+ distinguishes between the first half of the period vs the second half. It
+ allows the integrator to move at different speeds during these two periods.
+ From a locomotion perspective these two halves of the period can be considered
+ as swing and stance phases. The speed difference is calculated using the
+ variable swing_stance_ratio provided at every time step.
+ CircularAsymmetricalIntegratorUnit can be used to control one or multiple legs
+ depending on the preference. If more than one leg is assigned to a single unit
+ the other legs are calculated based on their initial phase difference.
+ """
+
+ def __init__(self, init_phase=0):
+ self._init_phase = init_phase
+ self.reset()
+
+ def reset(self):
+ self.phase = self._init_phase
+
+ def calculate_progressed_phase(self, delta_period, swing_stance_speed_ratio):
+ """Calculate a hypotethical phase based on the current phase and args.
+
+ This is used to both calculate the new phase, as well as the current phase
+ of the other legs with a given offset of delta_period.
+
+ Args:
+ delta_period: The fraction of the period to add to the current phase of
+ the integrator. If set to 1, the integrator will accomplish one full
+ period and return the same phase. The calculated phase will depend on
+ the current phase (if it is in first half vs second half) and swing vs
+ stance speed ratio.
+ swing_stance_speed_ratio: The ratio of the speed of the phase when it is
+ in swing (second half) vs stance (first half). Set to 1.0 by default,
+ making it symettric, same as a classical integrator.
+
+ Returns:
+ The new phase between 0 and 2 * pi.
+ """
+ stance_speed_coef = (
+ swing_stance_speed_ratio + 1) * 0.5 / swing_stance_speed_ratio
+ swing_speed_coef = (swing_stance_speed_ratio + 1) * 0.5
+ delta_left = delta_period
+ new_phase = self.phase
+ while delta_left > 0:
+ if 0 <= new_phase < math.pi:
+ delta_phase_multiplier = stance_speed_coef * _TWO_PI
+ new_phase += delta_left * delta_phase_multiplier
+ delta_left = 0
+ if new_phase < math.pi:
+ delta_left = 0
+ else:
+ delta_left = (new_phase - math.pi) / delta_phase_multiplier
+ new_phase = math.pi
+ else:
+ delta_phase_multiplier = swing_speed_coef * _TWO_PI
+ new_phase += delta_left * delta_phase_multiplier
+ if math.pi <= new_phase < _TWO_PI:
+ delta_left = 0
+ else:
+ delta_left = (new_phase - _TWO_PI) / delta_phase_multiplier
+ new_phase = 0
+ return math.fmod(new_phase, _TWO_PI)
+
+ def progress_phase(self, delta_period, swing_stance_ratio):
+ """Updates the phase based on the current phase, delta period and ratio.
+
+ Args:
+ delta_period: The fraction of the period to add to the current phase of
+ the integrator. If set to 1, the integrator will accomplish one full
+ period and return the same phase. The calculated phase will depend on
+ the current phase (if it is in first half vs second half) and swing vs
+ stance speed ratio.
+ swing_stance_ratio: The ratio of the speed of the phase when it is in
+ swing (second half) vs stance (first half). Set to 1.0 by default,
+ making it symettric, same as a classical integrator.
+ """
+ self.phase = self.calculate_progressed_phase(delta_period,
+ swing_stance_ratio)
+
+ def get_state(self):
+ """Returns the sin and cos of the phase as state."""
+ return [(math.cos(self.phase) + 1) / 2.0, (math.sin(self.phase) + 1) / 2.0]
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/__init__.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/__init__.py
new file mode 100644
index 000000000..e69de29bb
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/base_client.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/base_client.py
new file mode 100644
index 000000000..523780124
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/base_client.py
@@ -0,0 +1,63 @@
+"""Base class for simulation client."""
+
+import enum
+from typing import Text
+
+GRAY = (0.3, 0.3, 0.3, 1)
+
+
+class ClientType(enum.Enum):
+ """Type of client."""
+ BULLET = "pybullet"
+
+
+
+class ClientMode(enum.Enum):
+ """Client running mode."""
+
+ # Client is being initialized, object is being loaded, teleported, etc.
+ CONFIGURATION = 1
+
+ # Client is in a mode that simulate motion according to physics and control.
+ SIMULATION = 2
+
+
+class WrongClientModeError(Exception):
+ """Client mode does not meet expectation (e.g. load object in sim mode)."""
+
+
+class BaseClient(object):
+ """Base class for simulation client."""
+
+ def __init__(self, client_type: Text = ""):
+ self._client_type = client_type
+
+ # Default to configuration mode.
+ self._client_mode = ClientMode.CONFIGURATION
+
+ @property
+ def client_type(self) -> ClientType:
+ return self._client_type
+
+ def switch_mode(self, mode: ClientMode) -> bool:
+ """Switches running mode of simulation client and return if mode changed."""
+ if mode not in (ClientMode.CONFIGURATION, ClientMode.SIMULATION):
+ raise ValueError(f"Invalid client mode {mode}.")
+ if mode == self._client_mode:
+ return False
+ self._client_mode = mode
+ return True
+
+ @property
+ def client_mode(self) -> ClientMode:
+ """Returns current client mode."""
+ return self._client_mode
+
+ def _assert_in_configuration_mode(self, operation: Text = "this operation"):
+ """Raises exception if client is not in configuration mode."""
+ if self._client_mode != ClientMode.CONFIGURATION:
+ raise WrongClientModeError(
+ f"Sim client is expected to be in configuration mode for "
+ f"{operation}.")
+
+
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/curriculum_reset_helpers.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/curriculum_reset_helpers.py
new file mode 100644
index 000000000..86fa8cfc3
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/curriculum_reset_helpers.py
@@ -0,0 +1,62 @@
+"""Implements dynamic locomotion gym env that changes based on iteration."""
+import gin
+
+import pybullet
+
+
+# TODO(b/142642890): Make this reset to the initial world state first.
+@gin.configurable
+def gap_task_curriculum_update(env,
+ num_iter,
+ distance_to_gap_or_hurdle=1.5,
+ initial_gap_length=0.1,
+ max_iterations=500,
+ gap_delta=0.0008):
+ """Linearly increase the gap width wrt the iteration number.
+
+ This is specific to BuildSingleGapWorld.
+
+ Args:
+ env: An instance of a LocomotionGymEnv.
+ num_iter: The training iteration we are on.
+ distance_to_gap_or_hurdle: The distance to the gap.
+ initial_gap_length: The starting gap length.
+ max_iterations: The number of iterations up to which we will modify the
+ environment.
+ gap_delta: The amount to increase the gap width by for each increase of 1 in
+ the iteration.
+ """
+
+ gap_length = initial_gap_length + gap_delta * min(max_iterations, num_iter)
+ env.task.reset(
+ env,
+ distance_to_gap_or_hurdle=distance_to_gap_or_hurdle,
+ gap_or_hurdle_width=gap_length)
+
+
+@gin.configurable
+def gap_world_curriculum_update(env,
+ num_iter,
+ initial_second_block_x=8.15,
+ max_iterations=500,
+ gap_delta=0.0008):
+ """Update the world, linearly increasing gap width wrt iteration number.
+
+ This is specific to SingleGapScene.
+
+ Args:
+ env: An instance of a LocomotionGymEnv.
+ num_iter: The training iteration we are on.
+ initial_second_block_x: The initial x position of the second block.
+ max_iterations: The number of iterations up to which we will modify the
+ environment.
+ gap_delta: The amount to increase the gap width by for each increase of 1 in
+ the iteration.
+ """
+
+ ground = env.scene.ground_ids
+ pos = pybullet.getBasePositionAndOrientation(ground[-1])[0]
+ # Linearly increase the gap width to 0.5m by the last iteration.
+ next_x = initial_second_block_x + gap_delta * min(max_iterations, num_iter)
+ pybullet.resetBasePositionAndOrientation(ground[-1], (next_x, pos[1], pos[2]),
+ [0, 0, 0, 1])
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_loader.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_loader.py
new file mode 100644
index 000000000..aa4afe495
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_loader.py
@@ -0,0 +1,75 @@
+"""Load the locomotion gym env using the gin config files."""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+import gin
+from pybullet_envs.minitaur.envs_v2 import locomotion_gym_env
+from pybullet_envs.minitaur.envs_v2 import multiagent_mobility_gym_env
+
+ROBOT_FIELD_IN_CONFIG = 'robot_params'
+TASK_FIELD_IN_CONFIG = 'task_params'
+PMTG_FIELD_IN_CONFIG = 'pmtg_params'
+_PMTG_GIN_QUERY = 'pmtg_wrapper_env.PmtgWrapperEnv.'
+
+
+@gin.configurable
+def load(wrapper_classes=None, multiagent=False, **kwargs):
+ """load a pre-defined locomotion gym environment.
+
+ The env specific settings should have been set in the gin files.
+
+ Args:
+ wrapper_classes: A list of wrapper_classes.
+ multiagent: Whether to use multiagent environment.
+ **kwargs: Keyword arguments to be passed to the environment constructor.
+
+ Returns:
+ env: The instance of the minitaur gym environment.
+ """
+ # Gin config are not always specified this way (e.g. namescoped config).
+ # Only guery parameters when it is necessary.
+ if any(
+ k in (PMTG_FIELD_IN_CONFIG, TASK_FIELD_IN_CONFIG, ROBOT_FIELD_IN_CONFIG)
+ for k in kwargs):
+ with gin.unlock_config():
+ if multiagent:
+ # Currently assume robots and tasks are identical
+ robot_class = gin.query_parameter(
+ 'multiagent_mobility_gym_env.MultiagentMobilityGymEnv.robot_classes'
+ )[0].selector
+ task = gin.query_parameter(
+ 'multiagent_mobility_gym_env.MultiagentMobilityGymEnv.tasks'
+ )[0].selector
+ else:
+ robot_class = gin.query_parameter(
+ 'locomotion_gym_env.LocomotionGymEnv.robot_class').selector
+ task = gin.query_parameter(
+ 'locomotion_gym_env.LocomotionGymEnv.task').selector
+ gin_prefix_dict = {
+ PMTG_FIELD_IN_CONFIG: _PMTG_GIN_QUERY,
+ TASK_FIELD_IN_CONFIG: task + '.',
+ ROBOT_FIELD_IN_CONFIG: robot_class + '.',
+ }
+ for field_name, field_values in kwargs.items():
+ if field_name in gin_prefix_dict:
+ for var_name, value in field_values.items():
+ gin.bind_parameter(gin_prefix_dict[field_name] + var_name, value)
+ else:
+ raise ValueError(
+ 'Environment argument type is not found in gin_prefix_dict.')
+ if multiagent:
+ env = multiagent_mobility_gym_env.MultiagentMobilityGymEnv()
+ else:
+ env = locomotion_gym_env.LocomotionGymEnv()
+ if wrapper_classes is not None:
+ # A little macro for the automatic list expansion
+ if not isinstance(wrapper_classes, list):
+ wrapper_classes = [wrapper_classes]
+
+ # Wrap environments with user-provided wrappers
+ # (e.g. TrajectoryGeneratorWrapperEnv)
+ for wrapper_cls in wrapper_classes:
+ env = wrapper_cls(env)
+ return env
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_wrappers/__init__.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_wrappers/__init__.py
new file mode 100644
index 000000000..e69de29bb
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_wrappers/action_denormalize_wrapper.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_wrappers/action_denormalize_wrapper.py
new file mode 100644
index 000000000..edfead1ab
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_wrappers/action_denormalize_wrapper.py
@@ -0,0 +1,41 @@
+"""Denormalize the action from [-1, 1] to the env.action_space."""
+
+import gin
+import gym
+import numpy as np
+
+
+def _denomralize(env, action):
+ action = np.array(action)
+ low = np.array(env.action_space.low)
+ high = np.array(env.action_space.high)
+ return (high - low) / 2.0 * action + (high + low) / 2.0
+
+
+@gin.configurable
+class ActionDenormalizeWrapper(object):
+ """An env wrapper that denormalize the action from [-1, 1] to the bounds."""
+
+ def __init__(self, gym_env):
+ """Initializes the wrapper."""
+ self._gym_env = gym_env
+ self.action_space = gym.spaces.Box(
+ low=-1.0,
+ high=1.0,
+ shape=self._gym_env.action_space.low.shape,
+ dtype=np.float32)
+
+ def __getattr__(self, attr):
+ return getattr(self._gym_env, attr)
+
+ def step(self, action):
+ """Steps the wrapped environment.
+
+ Args:
+ action: Numpy array between [-1.0, 1.0].
+
+ Returns:
+ The tuple containing the observation, the reward, and the epsiode
+ end indicator.
+ """
+ return self._gym_env.step(_denomralize(self._gym_env, action))
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_wrappers/alternating_legs_openloop.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_wrappers/alternating_legs_openloop.py
new file mode 100644
index 000000000..85a8d99c2
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_wrappers/alternating_legs_openloop.py
@@ -0,0 +1,125 @@
+"""A trajectory generator that return signals for alternating legs."""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import google_type_annotations
+from __future__ import print_function
+
+import math
+import attr
+import gin
+from gym import spaces
+import numpy as np
+from pybullet_envs.minitaur.envs.utilities import laikago_pose_utils
+
+TROT_GAIT = "trot"
+PACE_GAIT = "pace"
+NUM_MOTORS_LAIKAGO = 12
+STD_FOR_GAUSSIAN_TRAJECTORY = 0.15
+MOTION_FREQUENCY = 1.0
+MOTION_AMPLITUDE = 0.25
+ACTION_BOUND = 0.25
+
+
+# TODO(b/131193449): Add a test to this class.
+@gin.configurable
+class LaikagoAlternatingLegsTrajectoryGenerator(object):
+ """A trajectory generator that return signals for alternating legs."""
+
+ def __init__(
+ self,
+ init_abduction=laikago_pose_utils.LAIKAGO_DEFAULT_ABDUCTION_ANGLE,
+ init_hip=laikago_pose_utils.LAIKAGO_DEFAULT_HIP_ANGLE,
+ init_knee=laikago_pose_utils.LAIKAGO_DEFAULT_KNEE_ANGLE,
+ amplitude=MOTION_AMPLITUDE,
+ frequency=MOTION_FREQUENCY,
+ gait=PACE_GAIT, # can be TROT_GAIT or PACE_GAIT
+ ):
+ """Initializes the controller."""
+ self._pose = np.array(
+ attr.astuple(
+ laikago_pose_utils.LaikagoPose(
+ abduction_angle_0=init_abduction,
+ hip_angle_0=init_hip,
+ knee_angle_0=init_knee,
+ abduction_angle_1=init_abduction,
+ hip_angle_1=init_hip,
+ knee_angle_1=init_knee,
+ abduction_angle_2=init_abduction,
+ hip_angle_2=init_hip,
+ knee_angle_2=init_knee,
+ abduction_angle_3=init_abduction,
+ hip_angle_3=init_hip,
+ knee_angle_3=init_knee)))
+ action_high = np.array([ACTION_BOUND] * NUM_MOTORS_LAIKAGO)
+ self.action_space = spaces.Box(-action_high, action_high, dtype=np.float32)
+ self.amplitude = amplitude
+ self.period = 1.0 / frequency
+ self.gait = gait
+
+ def _alternating_legs_trajectory(self, t):
+ """The reference trajectory of each joint when alternating legs.
+
+ Args:
+ t: The time since the latest robot reset.
+
+ Returns:
+ An array of 12 desired motor angles.
+ """
+ phase_in_period = (t % self.period) / self.period
+ is_first_half_gait = phase_in_period < 0.5
+ if self.gait == TROT_GAIT and is_first_half_gait:
+ phases = [0, 1, 1, 0] # 0 means stance and 1 means retraction.
+ elif self.gait == TROT_GAIT and not is_first_half_gait:
+ phases = [1, 0, 0, 1]
+ elif self.gait == PACE_GAIT and is_first_half_gait:
+ phases = [0, 1, 0, 1]
+ elif self.gait == PACE_GAIT and not is_first_half_gait:
+ phases = [1, 0, 1, 0]
+ else:
+ raise ValueError("{} gait is not supported in alternating legs.".format(
+ self.gait))
+
+ phase_step_center = 0.25 if is_first_half_gait else 0.75
+ std = STD_FOR_GAUSSIAN_TRAJECTORY
+ # Uses Gaussian instead of sine for gentle foot touch down.
+ # The following joint angles are added to self._pose.
+ retract_hip_angle = self.amplitude * math.exp(
+ -(phase_in_period - phase_step_center) *
+ (phase_in_period - phase_step_center) / (std * std))
+ retract_knee_angle = -2.0 * retract_hip_angle
+ retract_abduction_angle = 0.0
+ stance_hip_angle = 0.0
+ stance_knee_angle = 0.0
+ stance_abduction_angle = 0.0
+ angles = []
+ for is_retract in phases:
+ if is_retract:
+ angles.extend([retract_abduction_angle, retract_hip_angle,
+ retract_knee_angle])
+ else:
+ angles.extend([stance_abduction_angle, stance_hip_angle,
+ stance_knee_angle])
+ return np.array(angles)
+
+ def reset(self):
+ pass
+
+ def get_action(self, current_time, input_action):
+ """Computes the trajectory according to input time and action.
+
+ Args:
+ current_time: The time in gym env since reset.
+ input_action: A numpy array. The input leg pose from a NN controller.
+
+ Returns:
+ A numpy array. The desired motor angles.
+ """
+
+ return self._pose + self._alternating_legs_trajectory(
+ current_time) + input_action
+
+ def get_observation(self, input_observation):
+ """Get the trajectory generator's observation."""
+
+ return input_observation
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_wrappers/curriculum_wrapper_env.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_wrappers/curriculum_wrapper_env.py
new file mode 100644
index 000000000..1e2f33586
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_wrappers/curriculum_wrapper_env.py
@@ -0,0 +1,50 @@
+"""A wrapped LocomotionGymEnv with functions that change the world and task."""
+
+import gin
+
+
+@gin.configurable
+class CurriculumWrapperEnv(object):
+ """A wrapped LocomotionGymEnv with an evolving environment."""
+
+ def __init__(self,
+ gym_env,
+ num_iter=0,
+ curriculum_world_update=None,
+ curriculum_task_update=None):
+ """Initializes the wrapped env.
+
+ Args:
+ gym_env: An instance of a (potentially previously wrapped)
+ LocomotionGymEnv.
+ num_iter: The training iteration we are on.
+ curriculum_world_update: A function that updates the environment based on
+ the iteration. Takes in the environment as an argument.
+ curriculum_task_update: A function that updates the task (eg.
+ observations) based on the iteration. Takes in the environment as an
+ argument.
+ """
+ self._gym_env = gym_env
+ self._num_iter = num_iter
+ self._curriculum_world_update = curriculum_world_update
+ self._curriculum_task_update = curriculum_task_update
+
+ def modify(self, step=0):
+ self._num_iter = step
+
+ def __getattr__(self, attr):
+ return getattr(self._gym_env, attr)
+
+ def reset(self, *args, **kwargs):
+ """Reset and adjust the environment."""
+ self._gym_env.reset(*args, **kwargs)
+ if self._curriculum_world_update is not None:
+ self._curriculum_world_update(self._gym_env, self._num_iter)
+ if self._curriculum_task_update is not None:
+ self._curriculum_task_update(self._gym_env, self._num_iter)
+ return self._get_observation()
+
+ # Used for testing.
+ @property
+ def num_iter(self):
+ return self._num_iter
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_wrappers/depth_uv_to_footplacement_wrapper.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_wrappers/depth_uv_to_footplacement_wrapper.py
new file mode 100644
index 000000000..4b86b6df6
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_wrappers/depth_uv_to_footplacement_wrapper.py
@@ -0,0 +1,357 @@
+"""Change the action from uv of the depth map to xyz in world frame."""
+
+import copy
+import math
+from typing import Sequence
+import gin
+import gym
+import numpy as np
+
+from pybullet_envs.minitaur.robots import laikago_kinematic_constants
+from pybullet_envs.minitaur.robots import quadruped_base
+from pybullet_envs.minitaur.robots import wheeled_robot_base_sim
+
+UNIT_QUATERNION = (0, 0, 0, 1)
+# A small gap between the target foot position and the floor.
+FLOOR_HEIGHT_EPSILON = 0.02
+FOOT_HEIGHT_CLEARNCE = 0.075
+BASE_MOVEMENT_SPEED = 0.3
+INIT_ABDUCTION_ANGLE = laikago_kinematic_constants.INIT_ABDUCTION_ANGLE
+INIT_HIP_ANGLE = laikago_kinematic_constants.INIT_HIP_ANGLE
+INIT_KNEE_ANGLE = laikago_kinematic_constants.INIT_KNEE_ANGLE
+NUM_LEGS = laikago_kinematic_constants.NUM_LEGS
+COM_VIZ_BOX_SIZE = [0.025, 0.025, 0.005]
+_DEFAULT_JOINT_POSE = (INIT_ABDUCTION_ANGLE, INIT_HIP_ANGLE,
+ INIT_KNEE_ANGLE) * NUM_LEGS
+# Weights for computing the target COM from the supporting feet locations.
+# The target COM for the front feet are biased forward for better robot
+# stability and seeing farther in distance, which enables larger steps.
+_SUPPORT_WEIGHT_MAP = [[0.0, 0.4, 0.3, 0.3], [0.4, 0.0, 0.3, 0.3],
+ [1.0 / 3, 1.0 / 3, 0.0, 1.0 / 3],
+ [1.0 / 3, 1.0 / 3, 1.0 / 3, 0.0]]
+_TASK_SENSOR_NAME = "sensors"
+
+
+@gin.configurable
+class DepthUVToFootPlacementWrapper(object):
+ """Changes the action from a point in depth map to a point in the world frame.
+
+ Attributes:
+ observation: The current observation of the environment.
+ last_action: The last that was used to step the environment.
+ env_step_counter: The number of control steps that have been elapesed since
+ the environment is reset.
+ action_space: The action space of the environment.
+ """
+
+ def __init__(self,
+ gym_env,
+ visualization=True,
+ foot_movement=False,
+ foot_clearance_height=FOOT_HEIGHT_CLEARNCE,
+ base_movement_speed=BASE_MOVEMENT_SPEED,
+ foothold_update_frequency=4):
+ """Initializes the wrapper.
+
+ Args:
+ gym_env: the wrapped gym environment. The robot is controlled
+ kinematically when gym_env.robot inherits from WheeledRobotBase and is
+ controlled dynamically using a static gait controller when gym_env.robot
+ inherits from QudrupedBase.
+ visualization: whether to draw a sphere that represents the foothold
+ position.
+ foot_movement: whether move the toe to the desired foothold position using
+ IK for visualization/debugging purpose.
+ foot_clearance_height: the maximum height of the swing foot.
+ base_movement_speed: the speed of the robot base.
+ foothold_update_frequency: the frequency of updating the foothold, which
+ is the same as the frequency of the steps. The default value 4 means
+ four steps (1 complete cycle of static gait) per second.
+ """
+ self._gym_env = gym_env
+ self._num_control_steps_per_foothold_update = max(
+ 1, int(1.0 / foothold_update_frequency / self._gym_env.env_time_step))
+ self.last_action = None
+ self._visualization = visualization
+ self._foot_movement = foot_movement
+ self._foot_clearance_height = foot_clearance_height
+ self._base_movement_speed = base_movement_speed
+ self._step_counter = 0
+ self.observation_space.spaces["toe_position"] = gym.spaces.Box(
+ np.array([-1.0] * 3), np.array([1.0] * 3))
+ self.action_space = gym.spaces.Box(
+ np.array([-1.0] * 2), np.array([1.0] * 2))
+ self.task.reset(self)
+ if hasattr(self.task, _TASK_SENSOR_NAME):
+ self.observation_space.spaces[
+ self.task.get_name()] = self.task.observation_space
+
+ def __getattr__(self, attr):
+ return getattr(self._gym_env, attr)
+
+ def reset(self, **kwargs):
+ """Reset the environment."""
+ obs = self._gym_env.reset(**kwargs)
+ self.task.reset(self)
+ self._step_counter = 0
+ current_end_effector_pos = np.array(
+ self._gym_env.robot.foot_positions()[self.task.swing_foot_id])
+ self.last_action = [0, 0]
+ if self._visualization:
+ self._create_foot_placement_visualization()
+ self._create_com_visualization()
+ self._initial_local_toe_positions = np.array(
+ self._gym_env.robot.foot_positions(position_in_world_frame=False))
+
+ # Move COM to prepare for the first swing step.
+ if isinstance(self._gym_env.robot, quadruped_base.QuadrupedBase):
+ obs, _ = self._move_com_dynamic(
+ self.task.swing_foot_id,
+ self._num_control_steps_per_foothold_update // 3 * 2)
+
+ # TODO(b/157614175): Adds a toe_position_sensor.
+ obs["toe_position"] = current_end_effector_pos
+ obs["vision"] = self.task.get_depth_image_for_foot()
+ obs["LastAction"] = self.last_action
+
+ self._observation = obs
+ return self._observation
+
+ def _move_kinematic(self):
+ """Move robot kinematically.
+
+ Returns:
+ The tuple containing the observation and env info.
+ """
+ if self._foot_movement:
+ toe_positions = np.array(
+ self._gym_env.robot.foot_positions(position_in_world_frame=True))
+ toe_positions[:, 2] = FLOOR_HEIGHT_EPSILON
+ destination_foothold_xyz_global = self.task.get_foothold_location(
+ self.last_action, use_world_frame=True)
+ destination_foothold_xyz_global[2] = FLOOR_HEIGHT_EPSILON
+ joint_pose = _DEFAULT_JOINT_POSE
+ for i in range(self._num_control_steps_per_foothold_update):
+ if self._foot_movement:
+ alpha = i / (self._num_control_steps_per_foothold_update - 1)
+ toe_positions_over_time = copy.deepcopy(toe_positions)
+ toe_positions_over_time[self.task.swing_foot_id] = (
+ self._construct_foot_trajectories(
+ alpha, toe_positions[self.task.swing_foot_id],
+ destination_foothold_xyz_global, self._foot_clearance_height))
+ joint_pose = np.array(
+ self.robot.motor_angles_from_foot_positions(
+ toe_positions_over_time, position_in_world_frame=True)[1])
+ action = {
+ wheeled_robot_base_sim.BASE_ACTION_NAME:
+ (self._base_movement_speed, 0),
+ wheeled_robot_base_sim.BODY_ACTION_NAME:
+ joint_pose,
+ }
+ obs, _, _, info = self._gym_env.step(action)
+ return obs, info
+
+ def _move_com_dynamic(self, swing_foot_id, num_control_steps):
+ """Move robot COM to the weightd center of the support polygon through dynamics.
+
+ Args:
+ swing_foot_id: Index of the swing foot.
+ num_control_steps: Total number of control steps for moving the COM.
+
+ Returns:
+ The tuple containing the observation and env info.
+ """
+
+ toe_positions = np.array(
+ self._gym_env.robot.foot_positions(position_in_world_frame=False))
+ support_polygon_center = np.array([0.0, 0.0, 0.0])
+ for i in range(NUM_LEGS):
+ if i != swing_foot_id:
+ support_polygon_center += toe_positions[i] * _SUPPORT_WEIGHT_MAP[
+ swing_foot_id][i]
+ support_polygon_center[2] = 0.0
+
+ for i in range(num_control_steps):
+ alpha = i / (num_control_steps - 1)
+ # Create a flat phase towards the end to stabilize the com movement.
+ alpha = np.clip(alpha * 1.1, 0, 1)
+ toe_positions_over_time = copy.deepcopy(
+ toe_positions) - alpha * support_polygon_center
+ # Use initial toe height to maintain the overal base height.
+ for j in range(len(toe_positions_over_time)):
+ toe_positions_over_time[j][2] = self._initial_local_toe_positions[j][2]
+ joint_pose = np.array(
+ self.robot.motor_angles_from_foot_positions(
+ toe_positions_over_time, position_in_world_frame=False)[1])
+ obs, _, _, info = self._gym_env.step(joint_pose)
+ self._update_com_visualization()
+ return obs, info
+
+ def _swing_leg_dynamic(self, swing_foot_id, destination_foothold_xyz_local,
+ num_control_steps):
+ """Move swing leg to the target foothold through IK and dynamics.
+
+ Args:
+ swing_foot_id: Index of the swing foot.
+ destination_foothold_xyz_local: Target foothold position.
+ num_control_steps: Total number of control steps for swinging the leg.
+
+ Returns:
+ The tuple containing the observation and env info.
+ """
+
+ local_toe_positions = np.array(
+ self._gym_env.robot.foot_positions(position_in_world_frame=False))
+
+ # Move swing leg
+ for i in range(num_control_steps):
+ alpha = i / (num_control_steps - 1)
+ toe_positions_over_time = copy.deepcopy(local_toe_positions)
+ toe_positions_over_time[swing_foot_id] = (
+ self._construct_foot_trajectories(alpha,
+ local_toe_positions[swing_foot_id],
+ destination_foothold_xyz_local,
+ self._foot_clearance_height))
+ joint_pose = np.array(
+ self.robot.motor_angles_from_foot_positions(
+ toe_positions_over_time, position_in_world_frame=False)[1])
+
+ obs, _, _, info = self._gym_env.step(joint_pose)
+ return obs, info
+
+ def _move_dynamic(self):
+ """Move robot dynamically.
+
+ Returns:
+ The tuple containing the observation and env info.
+ """
+ destination_foothold_xyz_local = self.task.get_foothold_location(
+ self.last_action, use_world_frame=False)
+ # Lift the target foothold slightly to account for thickness of the feet.
+ destination_foothold_xyz_local[2] += FLOOR_HEIGHT_EPSILON
+
+ # Swing leg in the first 1/3 of the duration.
+ self._swing_leg_dynamic(self.task.swing_foot_id,
+ destination_foothold_xyz_local,
+ self._num_control_steps_per_foothold_update // 3)
+
+ # Move COM in the rest 2/3 of the duration.
+ obs, info = self._move_com_dynamic(
+ self.task.next_swing_foot_id,
+ self._num_control_steps_per_foothold_update // 3 * 2)
+
+ return obs, info
+
+ def step(self, action: Sequence[float]):
+ """Steps the wrapped environment.
+
+ Args:
+ action: 2 dimensional numpy array between [-1.0, 1.0]. They represents the
+ depth image pixel index. We assume that only one foot is swinging in
+ this wrapper, and this is the foothold location for that swinging leg.
+ The order of the swinging leg and the index of the current swinging leg
+ is defined in stepstone_visiontask.py.
+
+ Returns:
+ The tuple containing the observation, the reward, and the episode
+ end indicator.
+ """
+ self.last_action = action
+ reward = self.task(self)
+ done = self.task.done(self)
+ if self._visualization:
+ self._update_foothold_visualization(action)
+
+ if isinstance(self._gym_env.robot, quadruped_base.QuadrupedBase):
+ obs, info = self._move_dynamic()
+ else:
+ obs, info = self._move_kinematic()
+
+ self._step_counter += 1
+ current_end_effector_pos = np.array(
+ self._gym_env.robot.foot_positions()[self.task.swing_foot_id])
+ obs["toe_position"] = current_end_effector_pos
+ obs["vision"] = self.task.get_depth_image_for_foot()
+ self._observation = obs
+ return obs, reward, done, info
+
+ def _construct_foot_trajectories(self, alpha, swing_foot_start_position,
+ swing_foot_destination,
+ foot_clearance_height):
+ """Construct the target foot position for the swing foot.
+
+ Args:
+ alpha: a float in [0.0, 1.0] indicating the phase of the swing foot.
+ swing_foot_start_position: foot position at the beginning of the swing
+ motion.
+ swing_foot_destination: target foot position at the end of the swing
+ motion.
+ foot_clearance_height: the maximum height of the swing foot.
+
+ Returns:
+ The interpolated swing foot position.
+ """
+ new_swing_foot_position = swing_foot_start_position + alpha * (
+ swing_foot_destination - swing_foot_start_position)
+ new_swing_foot_position[2] += (
+ foot_clearance_height * math.sin((alpha) * math.pi))
+ return new_swing_foot_position
+
+ def _create_foot_placement_visualization(self):
+ """Creates a visualization sphere that represents the foothold position."""
+ visual_id = self._gym_env.pybullet_client.createVisualShape(
+ self._gym_env.pybullet_client.GEOM_SPHERE,
+ radius=0.02,
+ rgbaColor=[0.7, 0.7, 0.7, 1])
+ self._foothold_visual_body = self._gym_env.pybullet_client.createMultiBody(
+ baseMass=0, baseVisualShapeIndex=visual_id, basePosition=[0, 0, 0])
+
+ def _update_foothold_visualization(self, action):
+ """Moves the visualization sphere that represents the next foothold."""
+ foothold_xyz = self.task.get_foothold_location(
+ action, use_world_frame=True)
+ self._gym_env.pybullet_client.resetBasePositionAndOrientation(
+ self._foothold_visual_body, foothold_xyz, UNIT_QUATERNION)
+
+ def _create_com_visualization(self):
+ """Creates visualization boxes for COM and support polygon center."""
+ visual_id = self._gym_env.pybullet_client.createVisualShape(
+ self._gym_env.pybullet_client.GEOM_BOX,
+ halfExtents=COM_VIZ_BOX_SIZE,
+ rgbaColor=[0.7, 0.5, 0.5, 1])
+ self._support_polygon_center_visual_body = self._gym_env.pybullet_client.createMultiBody(
+ baseMass=0, baseVisualShapeIndex=visual_id, basePosition=[0, 0, 0])
+
+ visual_id = self._gym_env.pybullet_client.createVisualShape(
+ self._gym_env.pybullet_client.GEOM_BOX,
+ halfExtents=COM_VIZ_BOX_SIZE,
+ rgbaColor=[0.5, 0.75, 0.5, 1])
+ self._projected_com_visual_body = self._gym_env.pybullet_client.createMultiBody(
+ baseMass=0, baseVisualShapeIndex=visual_id, basePosition=[0, 0, 0])
+
+ def _update_com_visualization(self):
+ """Moves the visualization for the COM and support polygon center."""
+ toe_positions = np.array(
+ self._gym_env.robot.foot_positions(position_in_world_frame=True))
+ support_polygon_center_global = np.array([0.0, 0.0, 0.0])
+ for i in range(NUM_LEGS):
+ if i != self.task.next_swing_foot_id:
+ support_polygon_center_global += toe_positions[i] * _SUPPORT_WEIGHT_MAP[
+ self.task.next_swing_foot_id][i]
+ support_polygon_center_global[2] = 0.0
+ self._gym_env.pybullet_client.resetBasePositionAndOrientation(
+ self._support_polygon_center_visual_body, support_polygon_center_global,
+ UNIT_QUATERNION)
+
+ com = np.copy(self._gym_env.robot.base_position)
+ com[2] = 0.0
+ self._gym_env.pybullet_client.resetBasePositionAndOrientation(
+ self._projected_com_visual_body, com, UNIT_QUATERNION)
+
+ @property
+ def observation(self):
+ return self._observation
+
+ @property
+ def env_step_counter(self):
+ return self._step_counter
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_wrappers/fixed_steptime_wrapper_env.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_wrappers/fixed_steptime_wrapper_env.py
new file mode 100644
index 000000000..c06ffacab
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_wrappers/fixed_steptime_wrapper_env.py
@@ -0,0 +1,92 @@
+"""A wrapper that controls the timing between steps.
+"""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import google_type_annotations
+from __future__ import print_function
+
+import time
+import gin
+
+
+@gin.configurable
+class FixedSteptimeWrapperEnv(object):
+ """A wrapped LocomotionGymEnv with timing control between steps."""
+
+ def __init__(self,
+ gym_env,
+ desired_time_between_steps=None):
+ """Initializes the wrapper env.
+
+ Args:
+ gym_env: An instance of LocomotionGymEnv.
+ desired_time_between_steps: The desired time between steps in seconds.
+ If this is None, it is set to the env_time_step of the gym_env.
+ """
+ self._gym_env = gym_env
+ if desired_time_between_steps is None:
+ self._desired_time_between_steps = gym_env.env_time_step
+ else:
+ self._desired_time_between_steps = desired_time_between_steps
+
+ self._last_reset_time = time.time()
+ self._last_step_time = time.time()
+ self._step_counter = 0
+
+ def __getattr__(self, attr):
+ return getattr(self._gym_env, attr)
+
+ def reset(self, initial_motor_angles=None, reset_duration=1.0):
+ """Reset the environment.
+
+ This function records the timing of the reset.
+
+ Args:
+ initial_motor_angles: Not used.
+ reset_duration: Not used.
+
+ Returns:
+ The observation of the environment after reset.
+ """
+ obs = self._gym_env.reset(initial_motor_angles=initial_motor_angles,
+ reset_duration=reset_duration)
+ self._last_reset_time = time.time()
+ self._last_step_time = time.time()
+ self._step_counter = 0
+ return obs
+
+ def step(self, action):
+ """Steps the wrapped environment.
+
+ Args:
+ action: Numpy array. The input action from an NN agent.
+
+ Returns:
+ The tuple containing the observation, the reward, the epsiode end
+ indicator.
+
+ Raises:
+ ValueError if input action is None.
+ """
+ time_between_steps = time.time() - self._last_step_time
+ if time_between_steps < self._desired_time_between_steps:
+ time.sleep(self._desired_time_between_steps - time_between_steps)
+ self._last_step_time = time.time()
+ self._step_counter += 1
+ return self._gym_env.step(action)
+
+ @property
+ def elapsed_time(self):
+ """Returns the elapsed time in seconds."""
+ return time.time() - self._last_reset_time
+
+ @property
+ def steps_per_second(self):
+ """Returns the average number of time steps for 1 second."""
+ return self._step_counter / self.elapsed_time
+
+ @property
+ def seconds_per_step(self):
+ """Returns the average time between steps."""
+ return self.elapsed_time / self._step_counter
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_wrappers/ik_based_wrapper_env.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_wrappers/ik_based_wrapper_env.py
new file mode 100644
index 000000000..da80bf159
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_wrappers/ik_based_wrapper_env.py
@@ -0,0 +1,109 @@
+"""A wrapped Quadruped with Inverse Kinematics based controller."""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+import gin
+import gym
+import numpy as np
+from pybullet_envs.minitaur.robots import vision60
+from pybullet_envs.minitaur.robots.utilities import kinematics
+
+ACTION_DIM_PER_LEG = 3
+ACTION_DIM_BASE = 7
+ACTION_DIM_TOTAL = vision60.NUM_LEGS * ACTION_DIM_PER_LEG + ACTION_DIM_BASE
+
+
+@gin.configurable
+class IKBasedWrapperEnv(object):
+ """An env using IK to convert toe positions to joint angles."""
+
+ def __init__(self,
+ gym_env,
+ toe_indices=(3, 7, 11, 15),
+ abduction_motor_ids=(0, 3, 6, 9)):
+ """Initialzes the wrapped env.
+
+ Args:
+ gym_env: An instance of LocomotionGymEnv.
+ toe_indices: A list of four pybullet joint indices for the four toes. [3,
+ 7, 11, 15] for the vision60.
+ abduction_motor_ids: A list of four pybullet joint indices for the four
+ abuduction motors. [0, 3, 6, 9] for the vision60.
+ """
+ lower_bound = np.array([-1.0] * ACTION_DIM_TOTAL)
+ upper_bound = np.array([1.0] * ACTION_DIM_TOTAL)
+ self._gym_env = gym_env
+ self.action_space = gym.spaces.Box(lower_bound, upper_bound)
+ self._toe_ids = toe_indices
+ self._abduction_motor_ids = abduction_motor_ids
+
+ def __getattr__(self, attr):
+ return getattr(self._gym_env, attr)
+
+ def get_toe_indices(self):
+ return self._toe_ids
+
+ def _joint_angles_from_toe_positions_and_base_pose(self, ik_actions):
+ """Uses Inverse Kinematics to calculate jont angles.
+
+ Args:
+ ik_actions: The action should be local (x, y, z) for each toe. action for
+ each leg [x, y, z] in a local frame. This local frame is transformed
+ relative to the COM frame using a given translation, and rotation. The
+ total action space would be 3 + 4 + 3 * ACTION_DIM_PER_LEG = 16.
+
+ Returns:
+ A list of joint angles.
+ """
+ assert len(ik_actions) == ACTION_DIM_TOTAL
+
+ base_translation_index = vision60.NUM_LEGS * ACTION_DIM_PER_LEG
+ base_rotation_index = vision60.NUM_LEGS * ACTION_DIM_PER_LEG + 3
+
+ base_translation = ik_actions[
+ base_translation_index:base_translation_index + 3]
+ base_rotation = ik_actions[base_rotation_index:base_rotation_index + 4]
+ desired_joint_angles = []
+ for i in range(vision60.NUM_LEGS):
+ local_toe_pos = ik_actions[i * ACTION_DIM_PER_LEG:i * ACTION_DIM_PER_LEG +
+ ACTION_DIM_PER_LEG]
+ leg_joint_ids = [
+ self._abduction_motor_ids[i], self._abduction_motor_ids[i] + 1,
+ self._abduction_motor_ids[i] + 2
+ ]
+
+ desired_joint_angles.extend(
+ kinematics.joint_angles_from_link_position(
+ robot=self._gym_env.robot,
+ link_position=local_toe_pos,
+ link_id=self._toe_ids[i],
+ joint_ids=leg_joint_ids,
+ base_translation=base_translation,
+ base_rotation=base_rotation))
+
+ return desired_joint_angles
+
+ def step(self, action):
+ """Steps the wrapped environment.
+
+ Args:
+ action: Numpy array. The input action from an NN agent.
+
+ Returns:
+ The tuple containing the modified observation, the reward, the epsiode end
+ indicator.
+
+ Raises:
+ ValueError if input action is None.
+
+ """
+ if action is None:
+ raise ValueError('Action cannot be None')
+
+ desired_joint_angles = self._joint_angles_from_toe_positions_and_base_pose(
+ ik_actions=action)
+ observation, reward, done, _ = self._gym_env.step(desired_joint_angles)
+
+ return observation, reward, done, _
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_wrappers/imitation_wrapper_env.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_wrappers/imitation_wrapper_env.py
new file mode 100644
index 000000000..bbe14ea83
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_wrappers/imitation_wrapper_env.py
@@ -0,0 +1,101 @@
+"""A wrapper for motion imitation environment."""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+import gin
+import gym
+import numpy as np
+
+
+@gin.configurable
+class ImitationWrapperEnv(object):
+ """An env using for training policy with motion imitation."""
+
+ def __init__(self, gym_env):
+ """Initialzes the wrapped env.
+
+ Args:
+ gym_env: An instance of LocomotionGymEnv.
+ """
+ self._gym_env = gym_env
+ self.observation_space = self._build_observation_space()
+
+ self.seed()
+ return
+
+ def __getattr__(self, attr):
+ return getattr(self._gym_env, attr)
+
+ def step(self, action):
+ """Steps the wrapped environment.
+
+ Args:
+ action: Numpy array. The input action from an NN agent.
+
+ Returns:
+ The tuple containing the modified observation, the reward, the epsiode end
+ indicator.
+
+ Raises:
+ ValueError if input action is None.
+
+ """
+ original_observation, reward, done, _ = self._gym_env.step(action)
+ observation = self._modify_observation(original_observation)
+
+ return observation, reward, done, _
+
+ @gin.configurable('imitation_wrapper_env.ImitationWrapperEnv.reset')
+ def reset(self, initial_motor_angles=None, reset_duration=1.0):
+ """Resets the robot's position in the world or rebuild the sim world.
+
+ The simulation world will be rebuilt if self._hard_reset is True.
+
+ Args:
+ initial_motor_angles: A list of Floats. The desired joint angles after
+ reset. If None, the robot will use its built-in value.
+ reset_duration: Float. The time (in seconds) needed to rotate all motors
+ to the desired initial values.
+
+ Returns:
+ A numpy array contains the initial observation after reset.
+ """
+ original_observation = self._gym_env.reset(initial_motor_angles, reset_duration)
+ observation = self._modify_observation(original_observation)
+ return observation
+
+ def _modify_observation(self, original_observation):
+ """Appends target observations from the reference motion to the observations.
+
+ Args:
+ original_observation: A numpy array containing the original observations.
+
+ Returns:
+ A numpy array contains the initial original concatenated with target
+ observations from the reference motion.
+ """
+ target_observation = self._task.build_target_obs()
+ observation = np.concatenate([original_observation, target_observation], axis=-1)
+ return observation
+
+ def _build_observation_space(self):
+ """Constructs the observation space, including target observations from
+ the reference motion.
+
+ Returns:
+ Observation space representing the concatenations of the original
+ observations and target observations.
+ """
+ obs_space0 = self._gym_env.observation_space
+ low0 = obs_space0.low
+ high0 = obs_space0.high
+
+ task_low, task_high = self._task.get_target_obs_bounds()
+ low = np.concatenate([low0, task_low], axis=-1)
+ high = np.concatenate([high0, task_high], axis=-1)
+
+ obs_space = gym.spaces.Box(low, high)
+
+ return obs_space
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_wrappers/mpc_locomotion_wrapper.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_wrappers/mpc_locomotion_wrapper.py
new file mode 100644
index 000000000..faebcdfea
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_wrappers/mpc_locomotion_wrapper.py
@@ -0,0 +1,794 @@
+"""An env that uses MPC-based motion controller to realize higher level footstep planning."""
+
+import copy
+import enum
+from typing import Sequence
+
+import dataclasses
+import gin
+import gym
+import numpy as np
+
+from pybullet_envs.minitaur.agents.baseline_controller import com_height_estimator
+from pybullet_envs.minitaur.agents.baseline_controller import gait_generator as gait_generator_lib
+from pybullet_envs.minitaur.agents.baseline_controller import imu_based_com_velocity_estimator
+from pybullet_envs.minitaur.agents.baseline_controller import multi_state_estimator
+from pybullet_envs.minitaur.agents.baseline_controller import openloop_gait_generator
+from pybullet_envs.minitaur.agents.baseline_controller import torque_stance_leg_controller
+from pybullet_envs.minitaur.robots import laikago_kinematic_constants
+
+_UNIT_QUATERNION = (0, 0, 0, 1)
+_NUM_LEGS = laikago_kinematic_constants.NUM_LEGS
+_MOTORS_PER_LEG = 3
+_DEFAULT_BODY_HEIGHT = 0.45
+_DEFAULT_BASE_SPEED = (0.0, 0.0)
+_DEFAULT_BASE_TWIST_SPEED = 0.0
+_DEFAULT_ROLL_PITCH = (0.0, 0.0)
+_DEFAULT_SWING_TARGET = (0.0, 0.0, 0.0)
+_DEFAULT_SWING_CLERANCE = 0.05
+_MOTOR_KP = [220.0] * 12
+_MOTOR_KD = [0.3, 2.0, 2.0] * 4
+_BASE_VELOCITY_ACTION_RANGE = ((-1.0, -0.1), (1.0, 0.1))
+_BASE_TWIST_SPEED_ACTION_RANGE = (-0.5, 0.5)
+_BASE_HEIGHT_ACTION_RANGE = (0.3, 0.5)
+# Action bound for the swing target in local x, y, z direction.
+_SWING_TARGET_ACTION_RANGE = ((-0.3, -0.1, -0.25), (0.3, 0.1, 0.25))
+_PITCH_ROLL_ACTION_RANGE = (-0.35, 0.35)
+_SWING_CLEARANCE_ACTION_RANGE = (0.05, 0.3)
+_SWING_TARGET_DELTA_ACTION_RANGE = ((-0.02, -0.01, -0.02), (0.02, 0.01, 0.02))
+_SWING_CLEARANCE_DELTA_RANGE = (-0.02, 0.02)
+
+
+@gin.configurable
+@dataclasses.dataclass
+class BaseTargetHorizontalComVelocityHeuristic(object):
+ """A class for mapping swing foot targets to a heuristic target com velocity."""
+ horizontal_com_velocity_heuristic: np.ndarray = np.zeros(2)
+
+ def update_horizontal_com_velocity_heuristic(self, hip_relative_swing_targets,
+ com_velocity, swing_durations):
+ del hip_relative_swing_targets, com_velocity, swing_durations
+ pass
+
+ def reset(self):
+ self.horizontal_com_velocity_heuristic = np.zeros(2)
+
+
+# TODO(magicmelon): Add a one-pager to explain the inverse raibert heuristics.
+@gin.configurable
+class InverseRaibertTargetHorizontalComVelocityHeuristic(
+ BaseTargetHorizontalComVelocityHeuristic):
+ """A class for mapping swing foot targets to a target com velocity with Raibert Heuristics."""
+
+ def __init__(self, gains=(-0.25, -0.1)):
+ self._gains = np.array(gains)
+
+ def update_horizontal_com_velocity_heuristic(self, hip_relative_swing_targets,
+ com_velocity, swing_durations):
+ assert len(hip_relative_swing_targets) == len(swing_durations)
+ target_com_horizontal_velocities = []
+ for i in range(len(hip_relative_swing_targets)):
+ target_com_horizontal_velocity = (
+ com_velocity / 2.0 * swing_durations[i] -
+ hip_relative_swing_targets[i]) / self._gains + com_velocity
+ target_com_horizontal_velocities.append(target_com_horizontal_velocity)
+ if target_com_horizontal_velocities:
+ self.horizontal_com_velocity_heuristic = np.mean(
+ target_com_horizontal_velocities, axis=0)
+
+
+@gin.constants_from_enum
+class Gait(enum.Enum):
+ """The possible gaits."""
+ WALK = 0
+ TROT = 1
+
+
+def _select_gait(gait_type=Gait.WALK):
+ """Selects a gait pattern.
+
+ Args:
+ gait_type: which gait to use.
+
+ Returns:
+ A tuple of (stance_duration, duty_factor, initial_phase)
+ """
+ # Each gait is composed of stance_duration, duty_factor, and
+ # init_phase_full_cycle.
+ if gait_type == Gait.TROT:
+ return [0.3] * 4, [0.6] * 4, [0, 0.5, 0.5, 0]
+ elif gait_type == Gait.WALK:
+ return [0.75] * 4, [0.8] * 4, [0.25, 0.75, 0.5, 0]
+ else:
+ raise NotImplementedError
+
+
+@gin.configurable
+class MPCLocomotionWrapper(object):
+ """An env that uses MPC-based motion controller to realize footstep planning.
+
+ The env takes as input the target position of the swing feet and the target
+ base movements, and internally uses an MPC-based controller to achieve these
+ targets. It assumes that the robot follows a given gait pattern, specified
+ during initialization.
+ Before each foot starts to swing, the env will request from the policy a
+ target swing location and height defined in the local frame w.r.t the
+ default toe position. During the swing of the foot, the policy can adjust
+ the base velocity, height, roll, pitch, and twist. Optionally, the policy
+ can also output a delta to the last target swing location to adjust the
+ swing trajectory during the swing
+ Observations (introduced in this env):
+ gait_phases (4D): Normalized phase within the gait cycle for each foot.
+ feet_states (4D): State of each foot. -1: stance, 1: swing, -2: lose
+ contact, 2: early contact.
+ need_new_swing_target (4D): Whether the foot needs a new swing target at
+ the current step. Set to 1 when the foot switches to swing from a
+ different state. When equals to 0, the corresponding foot target will
+ not have effect.
+ estimated_base_speed (3D): Estimated base velocity.
+ estimated_body_height (1D): Estimated base height.
+ heuristics_com_velocity (3D): Target base velocity calculated from the
+ input step-length using inverse Raibert heuristics. Used when
+ compute_heuristic_com_speed is True.
+ current_toe_target: Immediate tracking targets for the four feet in the
+ local frame.
+ Action components:
+ swing_targets (12D): Used in HL_LL and HL_only mode. Specifies
+ the swing target for each foot w.r.t the default local toe position.
+ swing_clearance (4D): Used in HL_LL and HL_only mode. Specifies
+ the height of the highest point in the swing trajectory.
+ swing_targets_delta (12D): Used if policy_output_swing_action_delta.
+ Specifies the change in the swing target for each foot.
+ swing_clearance_delta (4D): Used if policy_output_swing_action_delta.
+ Specifies the change in the swing clearance for each foot.
+ target_base_velocity (2D): Target base velocity in the horizontal plane.
+ Used when compute_heuristic_com_speed is False.
+ base_twist (1D): Target base twist.
+ base_height (1D): Target base height.
+ base_roll_pitch (2D): Target base roll and pitch.
+
+ Attributes:
+ observation: The current observation of the environment.
+ last_action: The last that was used to step the environment.
+ env_step_counter: The number of control steps that have been elapesed since
+ the environment is reset.
+ action_space: The action space of the environment.
+ """
+
+ def __init__(
+ self,
+ gym_env,
+ swing_target_action_range=_SWING_TARGET_ACTION_RANGE,
+ swing_clearance_action_range=_SWING_CLEARANCE_ACTION_RANGE,
+ pitch_action_range=_PITCH_ROLL_ACTION_RANGE,
+ roll_action_range=_PITCH_ROLL_ACTION_RANGE,
+ base_velocity_action_range=_BASE_VELOCITY_ACTION_RANGE,
+ base_twist_action_range=_BASE_TWIST_SPEED_ACTION_RANGE,
+ base_height_action_range=_BASE_HEIGHT_ACTION_RANGE,
+ policy_output_swing_action_delta=False,
+ swing_target_delta_range=_SWING_TARGET_DELTA_ACTION_RANGE,
+ swing_clearance_delta_range=_SWING_CLEARANCE_DELTA_RANGE,
+ foot_friction_coeff=0.5,
+ contact_detection_force_threshold=0.0,
+ locomotion_gait=Gait.WALK,
+ target_horizontal_com_velocity_heuristic=BaseTargetHorizontalComVelocityHeuristic(
+ ),
+ robot_mass_in_mpc=235.0 / 9.8,
+ control_frequency=20,
+ com_velocity_estimator_class=imu_based_com_velocity_estimator
+ .IMUBasedCOMVelocityEstimator):
+ """Initializes the wrapper.
+
+ Args:
+ gym_env: the wrapped gym environment.
+ swing_target_action_range: range for the swing targets specified before
+ each swing.
+ swing_clearance_action_range: range for the swing clearance.
+ pitch_action_range: range for the target body pitch.
+ roll_action_range: range for the target body roll.
+ base_velocity_action_range: range for the base velocity.
+ base_twist_action_range: range for the base twist.
+ base_height_action_range: range for the base height.
+ policy_output_swing_action_delta: whether to allow the policy to output an
+ adjustment to the last swing target during the swing motion.
+ swing_target_delta_range: range for the adjustment of the swing target.
+ swing_clearance_delta_range: range for the swing clearance adjustments.
+ foot_friction_coeff: friction on the feet.
+ contact_detection_force_threshold: Threshold of the contact sensor for
+ determining whether a foot is in contact. Use 20 for real robot and 0
+ for simulation.
+ locomotion_gait: Gait to be used.
+ target_horizontal_com_velocity_heuristic: .
+ robot_mass_in_mpc: mass of the robot used in MPC.
+ control_frequency: frequency of querying the policy. The internal MPC
+ controller can have higher frequency. Note that the policy outputs a
+ swing target and clearance at each query, however, it is only used by
+ the environment at the beginning of each swing phase (when
+ need_new_swing_target is 1).
+ com_velocity_estimator_class: class of the com velocity estimator. Use
+ IMUBasedCOMVelocityEstimator for estimating velocity from IMU sensor and
+ contact states. Use COMVelocityEstimator for using the ground-truth com
+ velocity (e.g. when mocap is available).
+ """
+ self._gym_env = gym_env
+ self._time_per_control_step = 1.0 / control_frequency
+ self._foot_friction_coeff = foot_friction_coeff
+ self._contact_detection_force_threshold = contact_detection_force_threshold
+ self._locomotion_gait = locomotion_gait
+ self._policy_output_swing_action_delta = policy_output_swing_action_delta
+ self._target_horizontal_com_velocity_heuristic = target_horizontal_com_velocity_heuristic
+
+ self.last_action = None
+
+ self._configure_action_space(
+ swing_target_action_range, swing_clearance_action_range,
+ pitch_action_range, roll_action_range, base_velocity_action_range,
+ base_twist_action_range, base_height_action_range,
+ policy_output_swing_action_delta, swing_target_delta_range,
+ swing_clearance_delta_range)
+
+ self._configure_observation_space()
+
+ # Set up the MPC controller
+ stance_duration, duty_factor, initial_phase = _select_gait(locomotion_gait)
+ self._gait_generator = openloop_gait_generator.OpenloopGaitGenerator(
+ self._gym_env.robot, stance_duration, duty_factor, initial_phase,
+ contact_detection_force_threshold)
+ self._com_velocity_estimator = com_velocity_estimator_class(
+ self._gym_env.robot)
+ self._com_height_estimator = com_height_estimator.COMHeightEstimator(
+ self._gym_env.robot)
+ self._state_estimator = multi_state_estimator.MultiStateEstimator(
+ self._gym_env.robot,
+ state_estimators=[
+ self._com_velocity_estimator, self._com_height_estimator
+ ])
+ self._stance_controller = torque_stance_leg_controller.TorqueStanceLegController(
+ self._gym_env.robot,
+ self._gait_generator,
+ self._state_estimator,
+ desired_speed=np.array(_DEFAULT_BASE_SPEED),
+ desired_twisting_speed=_DEFAULT_BASE_TWIST_SPEED,
+ desired_body_height=_DEFAULT_BODY_HEIGHT,
+ desired_roll_pitch=np.array(_DEFAULT_ROLL_PITCH),
+ body_mass=robot_mass_in_mpc)
+
+ def _configure_observation_space(self):
+ """Configure the observation space."""
+ self.observation_space.spaces["gait_phases"] = gym.spaces.Box(
+ np.array([-1.0] * 4), np.array([1.0] * 4))
+ self.observation_space.spaces["feet_states"] = gym.spaces.Box(
+ np.array([-2.0] * 4), np.array([2.0] * 4))
+ self.observation_space.spaces["need_new_swing_target"] = gym.spaces.Box(
+ np.array([0.0] * 4), np.array([1.0] * 4))
+ self.observation_space.spaces["estimated_base_speed"] = gym.spaces.Box(
+ np.array([-1.0, -1.0, -1.0]), np.array([1.0, 1.0, 1.0]))
+ self.observation_space.spaces["estimated_body_height"] = gym.spaces.Box(
+ np.array([0.35]), np.array([0.5]))
+ self.observation_space.spaces["heuristics_com_velocity"] = gym.spaces.Box(
+ np.array([-1.0] * 2), np.array([1.0] * 2))
+ self.observation_space.spaces["current_toe_target"] = gym.spaces.Box(
+ np.array([-1.0, -1.0, -1.0] * _NUM_LEGS),
+ np.array([1.0, 1.0, 1.0] * _NUM_LEGS))
+
+ # Needed so that LastActionSensor uses the correct action space.
+ for s in self.all_sensors():
+ s.on_reset(self)
+ for sensor in self.all_sensors():
+ if sensor.get_name() not in self._gym_config.ignored_sensor_list:
+ if hasattr(sensor, "observation_space"):
+ self.observation_space.spaces[
+ sensor.get_name()] = sensor.observation_space
+
+ self.task.reset(self)
+ if hasattr(self.task, "observation_space"):
+ self.observation_space.spaces[
+ self.task.get_name()] = self.task.observation_space
+
+ def _configure_action_space(self, swing_target_action_range,
+ swing_clearance_action_range, pitch_action_range,
+ roll_action_range, base_velocity_action_range,
+ base_twist_action_range, base_height_action_range,
+ policy_output_swing_action_delta,
+ swing_target_delta_range,
+ swing_clearance_delta_range):
+ """Configure the action space."""
+ ac_lb = np.array([])
+ ac_ub = np.array([])
+
+ # Index of different part of the actions within the action array.
+ self._action_start_indices = {}
+ self._action_dimensions = {}
+ self._action_names = []
+
+ # Swing targets and swing clearance.
+ for leg_id in range(_NUM_LEGS):
+ self._action_start_indices["swing_targets_" + str(leg_id)] = len(ac_lb)
+ self._action_dimensions["swing_targets_" + str(leg_id)] = len(
+ swing_target_action_range[0])
+ self._action_names.append("swing_targets_" + str(leg_id))
+ ac_lb = np.concatenate([ac_lb, swing_target_action_range[0]])
+ ac_ub = np.concatenate([ac_ub, swing_target_action_range[1]])
+ for leg_id in range(_NUM_LEGS):
+ self._action_start_indices["swing_clearance_" + str(leg_id)] = len(ac_lb)
+ self._action_dimensions["swing_clearance_" + str(leg_id)] = 1
+ self._action_names.append("swing_clearance_" + str(leg_id))
+ ac_lb = np.concatenate([ac_lb, [swing_clearance_action_range[0]]])
+ ac_ub = np.concatenate([ac_ub, [swing_clearance_action_range[1]]])
+
+ # Delta to the swing targets and clearance.
+ if policy_output_swing_action_delta:
+ for leg_id in range(_NUM_LEGS):
+ self._action_start_indices["swing_targets_delta_" +
+ str(leg_id)] = len(ac_lb)
+ self._action_dimensions["swing_targets_delta_" + str(leg_id)] = len(
+ swing_target_delta_range[0])
+ self._action_names.append("swing_targets_delta_" + str(leg_id))
+ ac_lb = np.concatenate([ac_lb, swing_target_delta_range[0]])
+ ac_ub = np.concatenate([ac_ub, swing_target_delta_range[1]])
+ for leg_id in range(_NUM_LEGS):
+ self._action_start_indices["swing_clearance_delta_" +
+ str(leg_id)] = len(ac_lb)
+ self._action_dimensions["swing_clearance_delta_" + str(leg_id)] = 1
+ self._action_names.append("swing_clearance_delta_" + str(leg_id))
+ ac_lb = np.concatenate([ac_lb, [swing_clearance_delta_range[0]]])
+ ac_ub = np.concatenate([ac_ub, [swing_clearance_delta_range[1]]])
+
+ # Desired CoM velocity actions
+ # Do not include the action if bounds are all equal to zero
+ if not np.all(np.array(base_velocity_action_range) == 0):
+ self._action_start_indices["target_base_velocity"] = len(ac_lb)
+ self._action_dimensions["target_base_velocity"] = 2
+ self._action_names.append("target_base_velocity")
+ ac_lb = np.concatenate([
+ ac_lb,
+ [base_velocity_action_range[0][0], base_velocity_action_range[0][1]]
+ ])
+ ac_ub = np.concatenate([
+ ac_ub,
+ [base_velocity_action_range[1][0], base_velocity_action_range[1][1]]
+ ])
+
+ # Base twist speed action
+ self._action_start_indices["base_twist"] = len(ac_lb)
+ self._action_dimensions["base_twist"] = 1
+ self._action_names.append("base_twist")
+ ac_lb = np.concatenate([ac_lb, [base_twist_action_range[0]]])
+ ac_ub = np.concatenate([ac_ub, [base_twist_action_range[1]]])
+
+ # Base height action
+ self._action_start_indices["base_height"] = len(ac_lb)
+ self._action_dimensions["base_height"] = 1
+ self._action_names.append("base_height")
+ ac_lb = np.concatenate([ac_lb, [base_height_action_range[0]]])
+ ac_ub = np.concatenate([ac_ub, [base_height_action_range[1]]])
+
+ # Roll-pitch action
+ self._action_start_indices["base_roll_pitch"] = len(ac_lb)
+ self._action_dimensions["base_roll_pitch"] = 2
+ self._action_names.append("base_roll_pitch")
+ ac_lb = np.concatenate(
+ [ac_lb, [roll_action_range[0], pitch_action_range[0]]])
+ ac_ub = np.concatenate(
+ [ac_ub, [roll_action_range[1], pitch_action_range[1]]])
+
+ self.action_space = gym.spaces.Box(ac_lb, ac_ub)
+
+ def __getattr__(self, attr):
+ return getattr(self._gym_env, attr)
+
+ def _fill_observations(self, obs):
+ """Fill the additional observations from this wrapper."""
+ phase_offset = np.array([
+ 0 if leg_state == gait_generator_lib.LegState.STANCE else 1
+ for leg_state in self._gait_generator.desired_leg_state
+ ])
+ obs["gait_phases"] = self._gait_generator.normalized_phase - phase_offset
+ obs["feet_states"] = []
+ for leg_state in self._gait_generator.desired_leg_state:
+ if leg_state == gait_generator_lib.LegState.STANCE:
+ obs["feet_states"].append(-1)
+ if leg_state == gait_generator_lib.LegState.SWING:
+ obs["feet_states"].append(1)
+ if leg_state == gait_generator_lib.LegState.EARLY_CONTACT:
+ obs["feet_states"].append(2)
+ if leg_state == gait_generator_lib.LegState.LOSE_CONTACT:
+ obs["feet_states"].append(-2)
+ obs["need_new_swing_target"] = np.copy(self._need_new_swing_target)
+ obs["estimated_base_speed"] = self._state_estimator.com_velocity_body_yaw_aligned_frame
+ obs["estimated_body_height"] = [self._state_estimator.estimated_com_height]
+ obs["heuristics_com_velocity"] = np.copy(
+ self._target_horizontal_com_velocity_heuristic
+ .horizontal_com_velocity_heuristic)
+ obs["current_toe_target"] = np.copy(self._current_toe_target)
+
+ def _reset_mpc_controller(self):
+ """Reset the state of the MPC controller."""
+ self._gait_generator.reset(0.0)
+ self._state_estimator.reset(0.0)
+ self._stance_controller.reset(0.0)
+ self._stance_controller.desired_speed = np.array(_DEFAULT_BASE_SPEED)
+ self._stance_controller.desired_twisting_speed = _DEFAULT_BASE_TWIST_SPEED
+ self._stance_controller.desired_body_height = _DEFAULT_BODY_HEIGHT
+ self._stance_controller.desired_roll_pitch = np.array(_DEFAULT_ROLL_PITCH)
+ self._mpc_reset_time = self.robot.GetTimeSinceReset()
+
+ def reset(self, **kwargs):
+ """Reset the environment."""
+ self._gym_env.reset(**kwargs)
+
+ self._reset_mpc_controller()
+
+ self._last_leg_state = copy.copy(self._gait_generator.leg_state)
+
+ self._initial_local_toe_positions = np.array(
+ self._gym_env.robot.foot_positions(position_in_world_frame=False))
+
+ self._current_toe_target = np.reshape(
+ copy.deepcopy(self._initial_local_toe_positions), 3 * _NUM_LEGS)
+
+ # Record last lift off position for each foot for computing swing
+ # trajectory.
+ self._lift_off_positions = copy.deepcopy(self._initial_local_toe_positions)
+
+ # Swing command by policy at the beginning of each swing phase.
+ self._nominal_swing_leg_commands = []
+ for _ in range(_NUM_LEGS):
+ self._nominal_swing_leg_commands.append({
+ "swing_target": np.array(_DEFAULT_SWING_TARGET),
+ "swing_clearance": _DEFAULT_SWING_CLERANCE
+ })
+ # Actual swing leg commands that incorporated potential delta adjustments
+ # from the policy.
+ self._actual_swing_leg_commands = []
+ for _ in range(_NUM_LEGS):
+ self._actual_swing_leg_commands.append({
+ "swing_target": np.array(_DEFAULT_SWING_TARGET),
+ "swing_clearance": _DEFAULT_SWING_CLERANCE
+ })
+
+ self._need_new_swing_target = np.array([0.0] * _NUM_LEGS)
+
+ self._target_horizontal_com_velocity_heuristic.reset()
+
+ self.last_action = []
+ for leg_id in range(_NUM_LEGS):
+ self.last_action.extend(
+ [0.0] * self._action_dimensions["swing_targets_" + str(leg_id)])
+ for leg_id in range(_NUM_LEGS):
+ self.last_action.extend(
+ [0.0] * self._action_dimensions["swing_clearance_" + str(leg_id)])
+ if self._policy_output_swing_action_delta:
+ for leg_id in range(_NUM_LEGS):
+ self.last_action.extend(
+ [0.0] *
+ self._action_dimensions["swing_targets_delta_" + str(leg_id)])
+ for leg_id in range(_NUM_LEGS):
+ self.last_action.extend(
+ [0.0] *
+ self._action_dimensions["swing_clearance_delta_" + str(leg_id)])
+ print("=========", len(self.last_action))
+ if "target_base_velocity" in self._action_names:
+ self.last_action.extend([0.0] *
+ self._action_dimensions["target_base_velocity"])
+ self.last_action.extend([0.0] * self._action_dimensions["base_twist"])
+ self.last_action.extend([0.0] * self._action_dimensions["base_height"])
+ self.last_action.extend([0.0] * self._action_dimensions["base_roll_pitch"])
+
+ # Needed for LastActionSensor to use the correct last_action.
+ for s in self.all_sensors():
+ s.on_reset(self)
+ obs = self._get_observation()
+ self._fill_observations(obs)
+
+ self.task.reset(self)
+ self._step_counter = 0
+
+ # Change feet friction.
+ for link_id in list(
+ self.robot.urdf_loader.get_end_effector_id_dict().values()):
+ self.pybullet_client.changeDynamics(
+ self.robot.robot_id,
+ link_id,
+ lateralFriction=self._foot_friction_coeff)
+ self._observation = obs
+
+ return self._observation
+
+ def _clean_up_action(self, action):
+ """Return a cleaned up action to use previous value or zero for components not used.
+
+ Args:
+ action: Input action from the policy.
+
+ Returns:
+ A cleaned up action where components not used in this step is replaced
+ with zero or value from previous steps.
+ """
+ cleaned_action = np.copy(action)
+ for leg_id in range(_NUM_LEGS):
+ if not self._need_new_swing_target[leg_id]:
+ swing_target_name = "swing_targets_" + str(leg_id)
+ swing_targets_start_index = self._action_start_indices[
+ swing_target_name]
+ swing_targets_end_index = swing_targets_start_index + self._action_dimensions[
+ swing_target_name]
+ cleaned_action[swing_targets_start_index:
+ swing_targets_end_index] = self.last_action[
+ swing_targets_start_index:swing_targets_end_index]
+
+ swing_clearance_name = "swing_clearance_" + str(leg_id)
+ swing_clearance_start_index = self._action_start_indices[
+ swing_clearance_name]
+ cleaned_action[swing_clearance_start_index] = self.last_action[
+ swing_clearance_start_index]
+ if self._policy_output_swing_action_delta:
+ for leg_id in range(_NUM_LEGS):
+ if self._observation["feet_states"][leg_id] != 1:
+ swing_target_delta_name = "swing_targets_delta_" + str(leg_id)
+ swing_targets_delta_start_index = self._action_start_indices[
+ swing_target_delta_name]
+ swing_targets_delta_end_index = swing_targets_delta_start_index + self._action_dimensions[
+ swing_target_delta_name]
+ cleaned_action[
+ swing_targets_delta_start_index:
+ swing_targets_delta_end_index] = self.last_action[
+ swing_targets_delta_start_index:swing_targets_delta_end_index]
+
+ swing_clearance_delta_name = "swing_clearance_delta_" + str(leg_id)
+ swing_clearance_delta_start_index = self._action_start_indices[
+ swing_clearance_delta_name]
+ cleaned_action[swing_clearance_delta_start_index] = self.last_action[
+ swing_clearance_delta_start_index]
+
+ return cleaned_action
+
+ def _get_toe_tracking_target(self, lift_off_position, phase, swing_clearance,
+ landing_position):
+ """Get the tracking target for the toes during the swing phase.
+
+ The swing toe will move 70% of the distance in the first half of the swing.
+ Intuitely, we want to move the swing foot quickly to the target landing
+ location and stay above the ground, in this way the control is more robust
+ to perturbations to the body that may cause the swing foot to drop onto
+ the ground earlier than expected. This is a common practice similar
+ to the MIT cheetah and Marc Raibert's original controllers. After the
+ designated swing motion finishes, we also command the foot to go down
+ for a short ditance (0.03m). This is to mitigate issues when the swing
+ finishes before it touches the ground.
+
+ Args:
+ lift_off_position: Local position when the foot leaves ground.
+ phase: Normalized phase of the foot in the current swing cycle.
+ swing_clearance: Height of the highest point in the swing trajectory.
+ landing_position: Target landing position in the local space.
+
+ Returns:
+ The interpolated foot target for the current step.
+ """
+ # Up vector in the world coordinate (without considering yaw).
+ rotated_up_vec = np.array(
+ self.pybullet_client.multiplyTransforms(
+ (0, 0, 0),
+ self.pybullet_client.getQuaternionFromEuler(
+ (self.robot.base_roll_pitch_yaw[0],
+ self.robot.base_roll_pitch_yaw[1], 0)), (0, 0, 1),
+ _UNIT_QUATERNION)[0])
+
+ # Linearly interpolate the trajectory to get the foot target.
+ keyframe_timings = [0.0, 0.45, 0.9, 0.9001, 1.0]
+ peak_toe_position = 0.3 * lift_off_position + 0.7 * landing_position + rotated_up_vec * swing_clearance
+ # TODO(magicmelon): Update the gait generator to warp the gait and keep
+ # the swing leg going down until it touches the ground.
+ landing_position_pressing_down = landing_position - rotated_up_vec * 0.03
+ keyframe_positions = np.array([
+ lift_off_position, peak_toe_position, landing_position,
+ landing_position_pressing_down, landing_position_pressing_down
+ ])
+
+ target_toe_positions = np.array([
+ np.interp(phase, keyframe_timings, keyframe_positions[:, 0]),
+ np.interp(phase, keyframe_timings, keyframe_positions[:, 1]),
+ np.interp(phase, keyframe_timings, keyframe_positions[:, 2])
+ ])
+ return target_toe_positions
+
+ def step(self, action: Sequence[float]):
+ """Steps the wrapped environment.
+
+ Args:
+ action:
+
+ Returns:
+ The tuple containing the observation, the reward, and the episode
+ end indicator.
+ """
+ self.last_action = self._clean_up_action(action)
+
+ obs, reward, done, info = self._step_motion_controller(self.last_action)
+ self._step_counter += 1
+ self._fill_observations(obs)
+ self._observation = obs
+
+ return obs, reward, done, info
+
+ def _extract_action(self, action, name):
+ return action[self.
+ _action_start_indices[name]:self._action_start_indices[name] +
+ self._action_dimensions[name]]
+
+ def _get_swing_foot_ids(self):
+ swing_foot_ids = []
+ for leg_id in range(_NUM_LEGS):
+ if self._gait_generator.leg_state[
+ leg_id] == gait_generator_lib.LegState.SWING:
+ swing_foot_ids.append(leg_id)
+ return swing_foot_ids
+
+ def _update_gait_states_and_flags(self):
+ """Update gait-related variables and flags."""
+ current_leg_state = self._gait_generator.leg_state
+ for leg_id in range(_NUM_LEGS):
+ if current_leg_state[
+ leg_id] == gait_generator_lib.LegState.SWING and self._last_leg_state[
+ leg_id] != gait_generator_lib.LegState.SWING:
+ self._lift_off_positions[leg_id] = self._gym_env.robot.foot_positions(
+ )[leg_id]
+ self._need_new_swing_target[leg_id] = 1
+ self._last_leg_state = copy.copy(current_leg_state)
+
+ com_estimate_leg_indices = []
+ for leg_id in range(_NUM_LEGS):
+ # Use the ones not swinging to estimate the com height
+ if leg_id not in self._get_swing_foot_ids():
+ com_estimate_leg_indices.append(leg_id)
+ self._com_height_estimator.com_estimate_leg_indices = com_estimate_leg_indices
+ self._state_estimator.update(self.robot.GetTimeSinceReset() -
+ self._mpc_reset_time)
+ self._gait_generator.update(self.robot.GetTimeSinceReset() -
+ self._mpc_reset_time)
+
+ def _process_action(self, action):
+ """Process the action and set relevant variables."""
+ # Extract the swing targets from the input action.
+ for leg_id in range(_NUM_LEGS):
+ if self._need_new_swing_target[leg_id]:
+ self._nominal_swing_leg_commands[leg_id]["swing_target"] = (
+ self._extract_action(action, "swing_targets_" + str(leg_id)))
+ self._nominal_swing_leg_commands[leg_id]["swing_clearance"] = (
+ self._extract_action(action, "swing_clearance_" + str(leg_id)))
+
+ self._actual_swing_leg_commands[leg_id]["swing_target"] = np.copy(
+ self._nominal_swing_leg_commands[leg_id]["swing_target"])
+ self._actual_swing_leg_commands[leg_id][
+ "swing_clearance"] = self._nominal_swing_leg_commands[leg_id][
+ "swing_clearance"]
+
+ # Reset the flags so the next high level commands are not used until the
+ # next swing happens.
+ self._need_new_swing_target = np.zeros(_NUM_LEGS)
+
+ # Extract the delta swing targets from the input action.
+ if self._policy_output_swing_action_delta:
+ for leg_id in self._get_swing_foot_ids():
+ self._actual_swing_leg_commands[leg_id]["swing_target"] = (
+ self._nominal_swing_leg_commands[leg_id]["swing_target"] +
+ self._extract_action(action, "swing_targets_delta_" + str(leg_id)))
+ self._actual_swing_leg_commands[leg_id]["swing_clearance"] = (
+ self._nominal_swing_leg_commands[leg_id]["swing_clearance"] +
+ self._extract_action(action,
+ "swing_clearance_delta_" + str(leg_id)))
+ # Extract the target base movement commands from the input action.
+ if "target_base_velocity" in self._action_names:
+ self._target_base_velocity_from_policy = self._extract_action(
+ action, "target_base_velocity")
+ else:
+ self._target_base_velocity_from_policy = np.zeros(3)
+ desired_twist_speed = self._extract_action(action, "base_twist")
+ desired_body_height = self._extract_action(action, "base_height")
+ desired_roll_pitch = self._extract_action(action, "base_roll_pitch")
+
+ self._stance_controller.desired_twisting_speed = desired_twist_speed
+ self._stance_controller.desired_body_height = desired_body_height
+ self._stance_controller.desired_roll_pitch = desired_roll_pitch
+
+ def _compute_swing_action(self):
+ """Compute actions for the swing legs."""
+ local_toe_positions = np.array(
+ self._gym_env.robot.foot_positions(position_in_world_frame=False))
+ toe_positions_over_time = copy.deepcopy(local_toe_positions)
+
+ for leg_id in self._get_swing_foot_ids():
+ target_toe_positions_local = self._get_toe_tracking_target(
+ self._lift_off_positions[leg_id],
+ self._gait_generator.normalized_phase[leg_id],
+ self._actual_swing_leg_commands[leg_id]["swing_clearance"],
+ self._actual_swing_leg_commands[leg_id]["swing_target"] +
+ self._initial_local_toe_positions[leg_id])
+ toe_positions_over_time[leg_id] = target_toe_positions_local
+ self._current_toe_target = np.reshape(
+ copy.deepcopy(toe_positions_over_time), 3 * _NUM_LEGS)
+ return np.array(
+ self.robot.motor_angles_from_foot_positions(
+ toe_positions_over_time, position_in_world_frame=False)[1])
+
+ def _compute_stance_action(self):
+ """Compute actions for the stance legs."""
+
+ # update target com velocity by combining policy output and heuristics
+ hip_relative_swing_targets = []
+ swing_durations = []
+ com_horizontal_velocity = np.array(
+ self._state_estimator.com_velocity_body_yaw_aligned_frame)[0:2]
+ for swing_id in self._get_swing_foot_ids():
+ hip_relative_swing_targets.append(
+ np.array([
+ self._actual_swing_leg_commands[swing_id]["swing_target"][0],
+ self._actual_swing_leg_commands[swing_id]["swing_target"][1]
+ ]))
+ swing_durations.append(self._gait_generator.swing_duration[swing_id])
+
+ self._target_horizontal_com_velocity_heuristic.update_horizontal_com_velocity_heuristic(
+ hip_relative_swing_targets, com_horizontal_velocity, swing_durations)
+ self._stance_controller.desired_speed = np.array(
+ self._target_base_velocity_from_policy)
+ self._stance_controller.desired_speed[
+ 0:
+ 2] += self._target_horizontal_com_velocity_heuristic.horizontal_com_velocity_heuristic
+
+ # compute actions for the stance leg
+ return self._stance_controller.get_action()
+
+ def _combine_swing_stance_action(self, swing_action, stance_action):
+ """Combine stance and swing leg actions."""
+ feet_contact_states = copy.copy(self._gait_generator.leg_state)
+ actions = []
+ for leg_id in range(_NUM_LEGS):
+ if leg_id in self._get_swing_foot_ids(
+ ) and feet_contact_states[leg_id] == gait_generator_lib.LegState.SWING:
+ for motor_id_in_leg in range(_MOTORS_PER_LEG):
+ actions.extend(
+ (swing_action[leg_id * _MOTORS_PER_LEG + motor_id_in_leg],
+ _MOTOR_KP[leg_id * _MOTORS_PER_LEG + motor_id_in_leg], 0.0,
+ _MOTOR_KD[leg_id * _MOTORS_PER_LEG + motor_id_in_leg], 0.0))
+ else:
+ for motor_id_in_leg in range(_MOTORS_PER_LEG):
+ actions.extend(stance_action[leg_id * _MOTORS_PER_LEG +
+ motor_id_in_leg])
+ return actions
+
+ def _step_motion_controller(self, action):
+ """Run the MPC controller to advance the robot's state."""
+
+ self._process_action(action)
+
+ # run MPC-based control for a certain amount of time
+ total_reward = 0.0
+ start_time = self._gym_env.robot.GetTimeSinceReset()
+ while self._gym_env.robot.GetTimeSinceReset(
+ ) - start_time < self._time_per_control_step:
+ self._update_gait_states_and_flags()
+
+ swing_action = self._compute_swing_action()
+
+ stance_action = self._compute_stance_action()
+
+ actions = self._combine_swing_stance_action(swing_action, stance_action)
+
+ obs, rew, done, info = self._gym_env.step(actions)
+ if self._stance_controller.qp_solver_fail:
+ done = True
+ total_reward += rew
+
+ if done:
+ return obs, 0.0, done, info
+
+ return obs, total_reward, done, info
+
+ @property
+ def observation(self):
+ return self._observation
+
+ @property
+ def env_step_counter(self):
+ return self._step_counter
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_wrappers/observation_dictionary_to_array_wrapper.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_wrappers/observation_dictionary_to_array_wrapper.py
new file mode 100644
index 000000000..34e6049cc
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_wrappers/observation_dictionary_to_array_wrapper.py
@@ -0,0 +1,65 @@
+"""An env wrapper that flattens the observation dictionary to an array."""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+import gym
+import gin
+from pybullet_envs.minitaur.envs_v2.utilities import env_utils
+
+
+@gin.configurable
+class ObservationDictionaryToArrayWrapper(gym.Env):
+ """An env wrapper that flattens the observation dictionary to an array."""
+
+ def __init__(self, gym_env, observation_excluded=()):
+ """Initializes the wrapper."""
+ self.observation_excluded = observation_excluded
+ self._gym_env = gym_env
+ self.observation_space = self._flatten_observation_spaces(
+ self._gym_env.observation_space)
+ self.action_space = self._gym_env.action_space
+
+ def __getattr__(self, attr):
+ return getattr(self._gym_env, attr)
+
+ def _flatten_observation_spaces(self, observation_spaces):
+ flat_observation_space = env_utils.flatten_observation_spaces(
+ observation_spaces=observation_spaces,
+ observation_excluded=self.observation_excluded)
+ return flat_observation_space
+
+ def _flatten_observation(self, input_observation):
+ """Flatten the dictionary to an array."""
+ return env_utils.flatten_observations(
+ observation_dict=input_observation,
+ observation_excluded=self.observation_excluded)
+
+ def seed(self, seed=None):
+ return self._gym_env.seed(seed)
+
+ def reset(self, initial_motor_angles=None, reset_duration=1.0):
+ observation = self._gym_env.reset(
+ initial_motor_angles=initial_motor_angles,
+ reset_duration=reset_duration)
+ return self._flatten_observation(observation)
+
+ def step(self, action):
+ """Steps the wrapped environment.
+
+ Args:
+ action: Numpy array. The input action from an NN agent.
+
+ Returns:
+ The tuple containing the flattened observation, the reward, the epsiode
+ end indicator.
+ """
+ observation_dict, reward, done, _ = self._gym_env.step(action)
+ return self._flatten_observation(observation_dict), reward, done, _
+
+ def render(self, mode='human'):
+ return self._gym_env.render(mode)
+
+ def close(self):
+ return self._gym_env.close()
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_wrappers/pmtg_inplace_wrapper_env.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_wrappers/pmtg_inplace_wrapper_env.py
new file mode 100644
index 000000000..0a274d399
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_wrappers/pmtg_inplace_wrapper_env.py
@@ -0,0 +1,176 @@
+"""A wrapped MinitaurGymEnv with a built-in controller."""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+import attr
+from gym import spaces
+import numpy as np
+import gin
+from pybullet_envs.minitaur.agents.trajectory_generator import tg_inplace
+from pybullet_envs.minitaur.envs.utilities import laikago_pose_utils
+from pybullet_envs.minitaur.envs.utilities import minitaur_pose_utils
+
+_NUM_LEGS = 4
+_LAIKAGO_NUM_ACTIONS = 12
+_FREQ_LOWER_BOUND = 0.0
+_FREQ_UPPER_BOUND = 3
+_DEFAULT_AMPLITUDE_STANCE = -0.02
+_DEFAULT_AMPLITUDE_LIFT = 0.9
+_DEFAULT_CENTER_EXTENSION = 0
+_DEFAULT_STANCE_LIFT_CUTOFF = 2 * np.pi * 0.67
+_DEFAULT_RESIDUAL_RANGE = 0.4
+_LAIKAGO_KNEE_ACTION_INDEXES = [2, 5, 8, 11]
+MINITAUR_INIT_EXTENSION_POS = 2.0
+MINITAUR_INIT_SWING_POS = 0.0
+
+
+@gin.configurable
+class PmtgInplaceWrapperEnv(object):
+ """A wrapped LocomotionGymEnv with a built-in trajectory generator."""
+
+ def __init__(self,
+ gym_env,
+ freq_lower_bound=_FREQ_LOWER_BOUND,
+ freq_upper_bound=_FREQ_UPPER_BOUND,
+ residual_range=_DEFAULT_RESIDUAL_RANGE,
+ amplitude_stance=_DEFAULT_AMPLITUDE_STANCE,
+ amplitude_lift=_DEFAULT_AMPLITUDE_LIFT,
+ center_extension=_DEFAULT_CENTER_EXTENSION,
+ stance_lift_cutoff=_DEFAULT_STANCE_LIFT_CUTOFF):
+ """Initializes the TG inplace wrapper class.
+
+ Args:
+ gym_env: the gym environment to wrap on.
+ freq_lower_bound: minimum frequency that the TGs can be propagated at.
+ freq_upper_bound: maximum frequency that the TGs can be propagated at.
+ residual_range: range of residuals that can be added to tg outputs.
+ amplitude_stance: stance amplitude of TG (see tg_inplace.py for details).
+ amplitude_lift: swing amplitude of TG (see tg_inplace.py for details).
+ center_extension: center extension of TG (see tg_inplace.py for details).
+ stance_lift_cutoff: phase cutoff between stance and lift phase (see
+ tg_inplace.py for details).
+ """
+ self._gym_env = gym_env
+ self._num_actions = gym_env.robot.num_motors
+ self._tg_phases = tg_inplace.reset()
+ self._tg_params = dict(
+ amplitude_stance=amplitude_stance,
+ amplitude_lift=amplitude_lift,
+ center_extension=center_extension,
+ stance_lift_cutoff=stance_lift_cutoff)
+
+ # Add the action boundaries for delta time, one per integrator.
+ action_low = np.hstack(
+ ([-residual_range] * self._num_actions, [freq_lower_bound] * _NUM_LEGS))
+ action_high = np.hstack(
+ ([residual_range] * self._num_actions, [freq_upper_bound] * _NUM_LEGS))
+ self.action_space = spaces.Box(action_low, action_high)
+
+ # Set the observation space and boundaries.
+ if hasattr(self._gym_env.observation_space, "spaces"):
+ self.observation_space = self._gym_env.observation_space
+ self.observation_space.spaces["pmtg_inplace"] = spaces.Box(
+ -1 * np.ones(2 * _NUM_LEGS), np.ones(2 * _NUM_LEGS))
+ else:
+ lower_bound = self._gym_env.observation_space.low
+ upper_bound = self._gym_env.observation_space.high
+ lower_bound = np.hstack((lower_bound, [-1.] * 2 * _NUM_LEGS))
+ upper_bound = np.hstack((upper_bound, [1.] * 2 * _NUM_LEGS))
+ self.observation_space = spaces.Box(lower_bound, upper_bound)
+
+ def __getattr__(self, attrb):
+ return getattr(self._gym_env, attrb)
+
+ def _modify_observation(self, observation):
+ if isinstance(observation, dict):
+ observation["tg_inplace"] = np.hstack(
+ (np.cos(self._tg_phases), np.sin(self._tg_phases)))
+ return observation
+ else:
+ return np.hstack(
+ (observation, np.cos(self._tg_phases), np.sin(self._tg_phases)))
+
+ def reset(self, initial_motor_angles=None, reset_duration=1.0):
+ """Resets the environment as well as trajectory generators."""
+ self._last_real_time = 0
+ self._num_step = 0
+ self._tg_phases = tg_inplace.reset()
+ if self._num_actions == _LAIKAGO_NUM_ACTIONS:
+ # Use laikago's init pose as zero action.
+ init_pose = np.array(
+ attr.astuple(
+ laikago_pose_utils.LaikagoPose(
+ abduction_angle_0=laikago_pose_utils
+ .LAIKAGO_DEFAULT_ABDUCTION_ANGLE,
+ hip_angle_0=laikago_pose_utils.LAIKAGO_DEFAULT_HIP_ANGLE,
+ knee_angle_0=laikago_pose_utils.LAIKAGO_DEFAULT_KNEE_ANGLE,
+ abduction_angle_1=laikago_pose_utils
+ .LAIKAGO_DEFAULT_ABDUCTION_ANGLE,
+ hip_angle_1=laikago_pose_utils.LAIKAGO_DEFAULT_HIP_ANGLE,
+ knee_angle_1=laikago_pose_utils.LAIKAGO_DEFAULT_KNEE_ANGLE,
+ abduction_angle_2=laikago_pose_utils
+ .LAIKAGO_DEFAULT_ABDUCTION_ANGLE,
+ hip_angle_2=laikago_pose_utils.LAIKAGO_DEFAULT_HIP_ANGLE,
+ knee_angle_2=laikago_pose_utils.LAIKAGO_DEFAULT_KNEE_ANGLE,
+ abduction_angle_3=laikago_pose_utils
+ .LAIKAGO_DEFAULT_ABDUCTION_ANGLE,
+ hip_angle_3=laikago_pose_utils.LAIKAGO_DEFAULT_HIP_ANGLE,
+ knee_angle_3=laikago_pose_utils.LAIKAGO_DEFAULT_KNEE_ANGLE)))
+ self._init_pose = init_pose
+ observation = self._gym_env.reset(init_pose, reset_duration)
+ else:
+ # Use minitaur's init pose as zero action.
+ init_pose = np.array(
+ attr.astuple(
+ minitaur_pose_utils.MinitaurPose(
+ swing_angle_0=MINITAUR_INIT_SWING_POS,
+ swing_angle_1=MINITAUR_INIT_SWING_POS,
+ swing_angle_2=MINITAUR_INIT_SWING_POS,
+ swing_angle_3=MINITAUR_INIT_SWING_POS,
+ extension_angle_0=MINITAUR_INIT_EXTENSION_POS,
+ extension_angle_1=MINITAUR_INIT_EXTENSION_POS,
+ extension_angle_2=MINITAUR_INIT_EXTENSION_POS,
+ extension_angle_3=MINITAUR_INIT_EXTENSION_POS)))
+ initial_motor_angles = minitaur_pose_utils.leg_pose_to_motor_angles(
+ init_pose)
+ observation = self._gym_env.reset(initial_motor_angles, reset_duration)
+ return self._modify_observation(observation)
+
+ def step(self, action):
+ """Steps the wrapped PMTG inplace environment."""
+ time = self._gym_env.get_time_since_reset()
+
+ # Convert the policy's residual actions to motor space.
+ if self._num_actions == _LAIKAGO_NUM_ACTIONS:
+ action_residual = np.array(
+ attr.astuple(
+ laikago_pose_utils.LaikagoPose(*(action[0:self._num_actions]))))
+ else:
+ action_residual = minitaur_pose_utils.leg_pose_to_motor_angles(
+ action[0:self._num_actions])
+
+ self._last_real_time = time
+ self._tg_phases, tg_extensions = tg_inplace.step(
+ self._tg_phases, action[-_NUM_LEGS:], self._gym_env.env_time_step,
+ self._tg_params)
+ # Convert TG's actions to motor space depending on the robot type.
+ if self._num_actions == _LAIKAGO_NUM_ACTIONS:
+ # If the legs have 3 DOF, apply extension directly to knee joints
+ action_tg_motor_space = np.zeros(self._num_actions)
+ for tg_idx, knee_idx in zip(
+ range(_NUM_LEGS), _LAIKAGO_KNEE_ACTION_INDEXES):
+ action_tg_motor_space[knee_idx] = tg_extensions[tg_idx]
+ else:
+ # Conversion to motor space for minitaur robot.
+ action_tg_motor_space = []
+ for idx in range(_NUM_LEGS):
+ extend = tg_extensions[idx]
+ action_tg_motor_space.extend(
+ minitaur_pose_utils.swing_extend_to_motor_angles(idx, 0, extend))
+ new_action = action_tg_motor_space + action_residual
+ if self._num_actions == _LAIKAGO_NUM_ACTIONS:
+ new_action += self._init_pose
+ original_observation, reward, done, _ = self._gym_env.step(new_action)
+
+ return self._modify_observation(original_observation), reward, done, _
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_wrappers/pmtg_wrapper_env.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_wrappers/pmtg_wrapper_env.py
new file mode 100644
index 000000000..1df14a296
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_wrappers/pmtg_wrapper_env.py
@@ -0,0 +1,290 @@
+"""A wrapped MinitaurGymEnv with a built-in controller."""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+from gym import spaces
+import numpy as np
+import gin
+from pybullet_envs.minitaur.agents.trajectory_generator import tg_simple
+from pybullet_envs.minitaur.envs_v2.utilities import robot_pose_utils
+from pybullet_envs.minitaur.robots.utilities import action_filter
+
+_DELTA_TIME_LOWER_BOUND = 0.0
+_DELTA_TIME_UPPER_BOUND = 3
+
+_GAIT_PHASE_MAP = {
+ "walk": [0, 0.25, 0.5, 0.75],
+ "trot": [0, 0.5, 0.5, 0],
+ "bound": [0, 0.5, 0, 0.5],
+ "pace": [0, 0, 0.5, 0.5],
+ "pronk": [0, 0, 0, 0]
+}
+
+
+@gin.configurable
+class PmtgWrapperEnv(object):
+ """A wrapped LocomotionGymEnv with a built-in trajectory generator."""
+
+ def __init__(self,
+ gym_env,
+ intensity_upper_bound=1.5,
+ min_delta_time=_DELTA_TIME_LOWER_BOUND,
+ max_delta_time=_DELTA_TIME_UPPER_BOUND,
+ integrator_coupling_mode="all coupled",
+ walk_height_coupling_mode="all coupled",
+ variable_swing_stance_ratio=True,
+ swing_stance_ratio=1.0,
+ residual_range=0.15,
+ init_leg_phase_offsets=None,
+ init_gait=None,
+ default_walk_height=0,
+ action_filter_enable=True,
+ action_filter_order=1,
+ action_filter_low_cut=0,
+ action_filter_high_cut=3.0,
+ action_filter_initialize=False,
+ leg_pose_class=None):
+ """Initialzes the wrapped env.
+
+ Args:
+ gym_env: An instance of LocomotionGymEnv.
+ intensity_upper_bound: The upper bound for intensity of the trajectory
+ generator. It can be used to limit the leg movement.
+ min_delta_time: Lower limit for the time in seconds that the trajectory
+ generator can be moved forward at each simulation step. The effective
+ frequency of the gait is based on the delta time multiplied by the
+ internal frequency of the trajectory generator.
+ max_delta_time: Upper limit for the time in seconds that the trajectory
+ generator can be moved forward at each simulation step.
+ integrator_coupling_mode: How the legs should be coupled for integrators.
+ walk_height_coupling_mode: The same coupling mode used for walking walking
+ heights for the legs.
+ variable_swing_stance_ratio: A boolean to indicate if the swing stance
+ ratio can change per time step or not.
+ swing_stance_ratio: Time taken by swing phase vs stance phase. This is
+ only relevant if variable_swing_stance_ratio is False.
+ residual_range: The upper limit for the residual actions that adds to the
+ leg motion. It is 0.15 by default, can be increased for more flexibility
+ or decreased to only to use the trajectory generator's motion.
+ init_leg_phase_offsets: The initial phases of the legs. A list of 4
+ variables within [0,1). The order is front-left, rear-left, front-right
+ and rear-right.
+ init_gait: The initial gait that sets the starting phase difference
+ between the legs. Overrides the arg init_phase_offsets. Has to be
+ "walk", "trot", "bound" or "pronk". Used in vizier search.
+ default_walk_height: Offset for the extension of the legs for the robot.
+ Applied to the legs at every time step. Implicitly affects the walking
+ and standing height of the policy. Zero by default. Units is in
+ extension space (can be considered in radiant since it is a linear
+ transformation to motor angles based on the robot's geometry).
+ action_filter_enable: Use a butterworth filter for the output of the PMTG
+ actions (before conversion to leg swing-extend model). It forces
+ smoother behaviors depending on the parameters used.
+ action_filter_order: The order for the action_filter (1 by default).
+ action_filter_low_cut: The cut for the lower frequencies (0 by default).
+ action_filter_high_cut: The cut for the higher frequencies (3 by default).
+ action_filter_initialize: If the action filter should be initialized when
+ the first action is taken. If enabled, the filter does not affect action
+ value the first time it is called and fills the history with that value.
+ leg_pose_class: A class providing a convert_leg_pose_to_motor_angle
+ instance method or None. If None, robot_pose_utils is used.
+
+ Raises:
+ ValueError if the controller does not implement get_action and
+ get_observation.
+
+ """
+ self._gym_env = gym_env
+ self._num_actions = gym_env.robot.num_motors
+ self._residual_range = residual_range
+ self._min_delta_time = min_delta_time
+ self._max_delta_time = max_delta_time
+ self._leg_pose_util = leg_pose_class() if leg_pose_class else None
+ # If not specified, default leg phase offsets to walking.
+ if init_gait:
+ if init_gait in _GAIT_PHASE_MAP:
+ init_leg_phase_offsets = _GAIT_PHASE_MAP[init_gait]
+ else:
+ raise ValueError("init_gait is not one of the defined gaits.")
+ else:
+ init_leg_phase_offsets = init_leg_phase_offsets or [0, 0.25, 0.5, 0.75]
+ # Create the Trajectory Generator based on the parameters.
+ self._trajectory_generator = tg_simple.TgSimple(
+ intensity_upper_bound=intensity_upper_bound,
+ integrator_coupling_mode=integrator_coupling_mode,
+ walk_height_coupling_mode=walk_height_coupling_mode,
+ variable_swing_stance_ratio=variable_swing_stance_ratio,
+ swing_stance_ratio=swing_stance_ratio,
+ init_leg_phase_offsets=init_leg_phase_offsets)
+
+ action_dim = self._extend_action_space()
+ self._extend_obs_space()
+
+ self._default_walk_height = default_walk_height
+ self._action_filter_enable = action_filter_enable
+ if self._action_filter_enable:
+ self._action_filter_initialize = action_filter_initialize
+ self._action_filter_order = action_filter_order
+ self._action_filter_low_cut = action_filter_low_cut
+ self._action_filter_high_cut = action_filter_high_cut
+ self._action_filter = self._build_action_filter(action_dim)
+
+ def _extend_obs_space(self):
+ """Extend observation space to include pmtg phase variables."""
+ # Set the observation space and boundaries.
+ lower_bound, upper_bound = self._get_observation_bounds()
+ if hasattr(self._gym_env.observation_space, "spaces"):
+ new_obs_space = spaces.Box(np.array(lower_bound), np.array(upper_bound))
+ self.observation_space.spaces.update({"pmtg_phase": new_obs_space})
+ else:
+ lower_bound = np.append(self._gym_env.observation_space.low, lower_bound)
+ upper_bound = np.append(self._gym_env.observation_space.high, upper_bound)
+ self.observation_space = spaces.Box(
+ np.array(lower_bound), np.array(upper_bound), dtype=np.float32)
+
+ def _extend_action_space(self):
+ """Extend the action space to include pmtg parameters."""
+ # Add the action boundaries for delta time, one per integrator.
+ action_low = [-self._residual_range] * self._num_actions
+ action_high = [self._residual_range] * self._num_actions
+ action_low = np.append(action_low, [self._min_delta_time] *
+ self._trajectory_generator.num_integrators)
+ action_high = np.append(action_high, [self._max_delta_time] *
+ self._trajectory_generator.num_integrators)
+
+ # Add the action boundaries for parameters of the trajectory generator.
+ l_bound, u_bound = self._trajectory_generator.get_parameter_bounds()
+ action_low = np.append(action_low, l_bound)
+ action_high = np.append(action_high, u_bound)
+ self.action_space = spaces.Box(
+ np.array(action_low), np.array(action_high), dtype=np.float32)
+ return len(action_high)
+
+ def __getattr__(self, attrb):
+ return getattr(self._gym_env, attrb)
+
+ def _modify_observation(self, observation):
+ if isinstance(observation, dict):
+ observation["pmtg_phase"] = self._trajectory_generator.get_state()
+ return observation
+ else:
+ return np.append(observation, self._trajectory_generator.get_state())
+
+ def reset(self, initial_motor_angles=None, reset_duration=1.0):
+ """Resets the environment as well as the trajectory generator(s).
+
+ Args:
+ initial_motor_angles: Unused for PMTG. Instead, it sets the legs to a pose
+ with the neutral action for the trajectory generator.
+ reset_duration: Float. The time (in seconds) needed to rotate all motors
+ to the desired initial values.
+
+ Returns:
+ A numpy array contains the initial observation after reset.
+ """
+ del initial_motor_angles
+ if self._action_filter_enable:
+ self._reset_action_filter()
+ self._last_real_time = 0
+ self._num_step = 0
+ self._target_speed_coef = 0.0
+ if self._trajectory_generator:
+ self._trajectory_generator.reset()
+ if self._leg_pose_util:
+ initial_motor_angles = self._leg_pose_util.convert_leg_pose_to_motor_angles(
+ [0, 0, 0] * 4)
+ else:
+ initial_motor_angles = robot_pose_utils.convert_leg_pose_to_motor_angles(
+ self._gym_env.robot_class, [0, 0, 0] * 4)
+ observation = self._gym_env.reset(initial_motor_angles, reset_duration)
+ return self._modify_observation(observation)
+
+ def step(self, action):
+ """Steps the wrapped environment.
+
+ Args:
+ action: Numpy array. The input action from an NN agent.
+
+ Returns:
+ The tuple containing the modified observation, the reward, the epsiode end
+ indicator.
+
+ Raises:
+ ValueError if input action is None.
+
+ """
+
+ if action is None:
+ raise ValueError("Action cannot be None")
+
+ if self._action_filter_enable:
+ action = self._filter_action(action)
+
+ time = self._gym_env.get_time_since_reset()
+
+ action_residual = action[0:self._num_actions]
+ # Add the default walking height offset to extension.
+ dimensions = len(action_residual) // 4
+ action_residual[dimensions - 1::dimensions] += self._default_walk_height
+ # Calculate trajectory generator's output based on the rest of the actions.
+ delta_real_time = time - self._last_real_time
+ self._last_real_time = time
+ action_tg = self._trajectory_generator.get_actions(
+ delta_real_time, action[self._num_actions:])
+ # If the residuals have a larger dimension, extend trajectory generator's
+ # actions to include abduction motors.
+ if len(action_tg) == 8 and len(action_residual) == 12:
+ for i in [0, 3, 6, 9]:
+ action_tg.insert(i, 0)
+ # Add TG actions with residual actions (in swing - extend space).
+ action_total = [a + b for a, b in zip(action_tg, action_residual)]
+ # Convert them to motor space based on the robot-specific conversions.
+ if self._leg_pose_util:
+ action_motor_space = self._leg_pose_util.convert_leg_pose_to_motor_angles(
+ action_total)
+ else:
+ action_motor_space = robot_pose_utils.convert_leg_pose_to_motor_angles(
+ self._gym_env.robot_class, action_total)
+ original_observation, reward, done, _ = self._gym_env.step(
+ action_motor_space)
+
+ return self._modify_observation(original_observation), np.float32(
+ reward), done, _
+
+ def _get_observation_bounds(self):
+ """Get the bounds of the observation added from the trajectory generator.
+
+ Returns:
+ lower_bounds: Lower bounds for observations.
+ upper_bounds: Upper bounds for observations.
+ """
+ lower_bounds = self._trajectory_generator.get_state_lower_bounds()
+ upper_bounds = self._trajectory_generator.get_state_upper_bounds()
+ return lower_bounds, upper_bounds
+
+ def _build_action_filter(self, num_joints):
+ order = self._action_filter_order
+ low_cut = self._action_filter_low_cut
+ high_cut = self._action_filter_high_cut
+ sampling_rate = 1 / (0.01)
+ a_filter = action_filter.ActionFilterButter([low_cut], [high_cut],
+ sampling_rate, order,
+ num_joints)
+ return a_filter
+
+ def _reset_action_filter(self):
+ self._action_filter.reset()
+ self._action_filter_empty = True
+ return
+
+ def _filter_action(self, action):
+ if self._action_filter_empty and self._action_filter_initialize:
+ # If initialize is selected and it is the first time filter is called,
+ # fill the buffer with that action so that it starts from that value
+ # instead of zero(s).
+ init_action = np.array(action).reshape(len(action), 1)
+ self._action_filter.reset(init_action)
+ self._action_filter_empty = False
+ filtered_action = self._action_filter.filter(np.array(action))
+ return filtered_action
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_wrappers/simple_openloop.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_wrappers/simple_openloop.py
new file mode 100644
index 000000000..65cb6446f
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_wrappers/simple_openloop.py
@@ -0,0 +1,127 @@
+"""Simple openloop trajectory generators."""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+import attr
+from gym import spaces
+import numpy as np
+
+import gin
+from pybullet_envs.minitaur.envs_v2.utilities import laikago_pose_utils
+from pybullet_envs.minitaur.envs_v2.utilities import minitaur_pose_utils
+
+
+@gin.configurable
+class MinitaurPoseOffsetGenerator(object):
+ """A trajectory generator that return a constant leg pose."""
+
+ def __init__(self,
+ init_swing=0,
+ init_extension=2.0,
+ init_pose=None,
+ action_scale=1.0,
+ action_limit=0.5):
+ """Initializes the controller.
+
+ Args:
+ init_swing: the swing of the default pose offset
+ init_extension: the extension of the default pose offset
+ init_pose: the default pose offset, which is None by default. If not None,
+ it will define the default pose offset while ignoring init_swing and
+ init_extension.
+ action_scale: changes the magnitudes of actions
+ action_limit: clips actions
+ """
+ if init_pose is None:
+ self._pose = np.array(
+ attr.astuple(
+ minitaur_pose_utils.MinitaurPose(
+ swing_angle_0=init_swing,
+ swing_angle_1=init_swing,
+ swing_angle_2=init_swing,
+ swing_angle_3=init_swing,
+ extension_angle_0=init_extension,
+ extension_angle_1=init_extension,
+ extension_angle_2=init_extension,
+ extension_angle_3=init_extension)))
+ else: # Ignore init_swing and init_extension
+ self._pose = np.array(init_pose)
+ action_high = np.array([action_limit] * minitaur_pose_utils.NUM_MOTORS)
+ self.action_space = spaces.Box(-action_high, action_high, dtype=np.float32)
+ self._action_scale = action_scale
+
+ def reset(self):
+ pass
+
+ def get_action(self, current_time=None, input_action=None):
+ """Computes the trajectory according to input time and action.
+
+ Args:
+ current_time: The time in gym env since reset.
+ input_action: A numpy array. The input leg pose from a NN controller.
+
+ Returns:
+ A numpy array. The desired motor angles.
+ """
+ del current_time
+ return minitaur_pose_utils.leg_pose_to_motor_angles(
+ self._pose + self._action_scale * np.array(input_action))
+
+ def get_observation(self, input_observation):
+ """Get the trajectory generator's observation."""
+
+ return input_observation
+
+
+@gin.configurable
+class LaikagoPoseOffsetGenerator(object):
+ """A trajectory generator that return constant motor angles."""
+
+ def __init__(
+ self,
+ init_abduction=laikago_pose_utils.LAIKAGO_DEFAULT_ABDUCTION_ANGLE,
+ init_hip=laikago_pose_utils.LAIKAGO_DEFAULT_HIP_ANGLE,
+ init_knee=laikago_pose_utils.LAIKAGO_DEFAULT_KNEE_ANGLE,
+ action_limit=0.5,
+ ):
+ """Initializes the controller."""
+ self._pose = np.array(
+ attr.astuple(
+ laikago_pose_utils.LaikagoPose(
+ abduction_angle_0=init_abduction,
+ hip_angle_0=init_hip,
+ knee_angle_0=init_knee,
+ abduction_angle_1=init_abduction,
+ hip_angle_1=init_hip,
+ knee_angle_1=init_knee,
+ abduction_angle_2=init_abduction,
+ hip_angle_2=init_hip,
+ knee_angle_2=init_knee,
+ abduction_angle_3=init_abduction,
+ hip_angle_3=init_hip,
+ knee_angle_3=init_knee)))
+ action_high = np.array([action_limit] * 12)
+ self.action_space = spaces.Box(-action_high, action_high, dtype=np.float32)
+
+ def reset(self):
+ pass
+
+ def get_action(self, current_time=None, input_action=None):
+ """Computes the trajectory according to input time and action.
+
+ Args:
+ current_time: The time in gym env since reset.
+ input_action: A numpy array. The input leg pose from a NN controller.
+
+ Returns:
+ A numpy array. The desired motor angles.
+ """
+ del current_time
+ return self._pose + input_action
+
+ def get_observation(self, input_observation):
+ """Get the trajectory generator's observation."""
+
+ return input_observation
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_wrappers/state_machine_based_wrapper_env.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_wrappers/state_machine_based_wrapper_env.py
new file mode 100644
index 000000000..56a92dcd0
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_wrappers/state_machine_based_wrapper_env.py
@@ -0,0 +1,321 @@
+"""A wrapped Quadruped with State-machine based controller."""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+import enum
+import gin
+import gym
+import numpy as np
+
+NUM_LEGS = 4
+ACTION_DIM_COM = 2
+ACTION_DIM_TOE = 1
+ACTION_DIM_TOTAL = ACTION_DIM_COM + ACTION_DIM_TOE
+OBSERVATION_DIM_LEG_ID = NUM_LEGS
+OBSERVATION_DIM_TOE_POS = 2 * NUM_LEGS
+OBSERVATION_DIM_TOTAL = OBSERVATION_DIM_TOE_POS + OBSERVATION_DIM_LEG_ID
+
+
+# States of the state machine.
+class GaitStateMachine(enum.IntEnum):
+ """The state machine for quadruped gait."""
+ STEP_LEFT_FRONT_TOE = 0
+ STEP_RIGHT_HIND_TOE = 1
+ STEP_RIGHT_FRONT_TOE = 2
+ STEP_LEFT_HIND_TOE = 3
+ TOTAL_GAIT_STATE_NUM = 4
+
+
+@gin.configurable
+class StateMachineBasedWrapperEnv(object):
+ """An env using IK to convert toe positions to joint angles.
+
+ The state machine consists of 4 states. During each state, the center of
+ mass of the base link is moved first, then one of the legs will take a
+ step by following a planned elliptical trajectory. The legs will move in
+ the order of front left -> hind right -> front right -> hind left.
+ The state transition is determined by elapsed time since last transition.
+ Observation (16 dimensions):
+ [one hot vector of the state id, local toe positions in x and y direction]
+ Action (3 dimensions):
+ [target moving distance of the current moving leg in x direction,
+ target moving distance of base COM in x direction,
+ target moving distance of base COM in y direction]
+ """
+
+ def __init__(self,
+ gym_env,
+ default_local_toe_positions,
+ toe_link_indices=(3, 7, 11, 15),
+ foot_lift_height=0.15,
+ state_duration=2.0,
+ action_lower_bound=(-0.0, -0.25, -0.25),
+ action_upper_bound=(0.3, 0.25, 0.25),
+ state_to_foot_id=(0, 3, 2, 1)):
+ """Initialzes the wrapped env.
+
+ Args:
+ gym_env: An instance of LocomotionGymEnv.
+ default_local_toe_positions: A list of vectors that contains the default
+ local position of each toe.
+ toe_link_indices: A list of indices to the toe link. Used for calculating
+ local toe positions.
+ foot_lift_height: Specifies how high the foot lifts during swing stage.
+ state_duration: Specifies the duration of each state.
+ action_lower_bound: Lower bound for the actions.
+ action_upper_bound: Upper bound for the actions.
+ state_to_foot_id: Mapping from state machine state to foot id.
+ """
+ self._gym_env = gym_env
+
+ assert len(action_lower_bound) == ACTION_DIM_TOTAL
+ assert len(action_upper_bound) == ACTION_DIM_TOTAL
+ self.action_space = gym.spaces.Box(
+ np.array(action_lower_bound), np.array(action_upper_bound))
+ observation_lower_bound = np.array([-1.0] * OBSERVATION_DIM_TOTAL)
+ observation_upper_bound = np.array([1.0] * OBSERVATION_DIM_TOTAL)
+ self.observation_space = gym.spaces.Box(observation_lower_bound,
+ observation_upper_bound)
+ self._default_local_toe_positions = default_local_toe_positions
+ self._toe_link_indices = toe_link_indices
+ self._foot_lift_height = foot_lift_height
+ self._state_to_foot_id = state_to_foot_id
+
+ # Use the largest value for bounding the toe movement
+ self._toe_move_bound = np.max(
+ np.abs(np.concatenate([action_lower_bound, action_upper_bound])))
+
+ # Duration of each state
+ self.state_machine_state_duration = [state_duration] * int(
+ GaitStateMachine.TOTAL_GAIT_STATE_NUM)
+
+ def __getattr__(self, attr):
+ return getattr(self._gym_env, attr)
+
+ def _state_machine_observation(self):
+ """Get the current observation: [state_id, local_toe_positions]."""
+ observation = []
+ # One-hot vector for the current state machine state.
+ one_hot_foot_id = np.zeros(GaitStateMachine.TOTAL_GAIT_STATE_NUM)
+ one_hot_foot_id[self.current_state_machine_state] = 1
+ observation.extend(one_hot_foot_id)
+
+ # Toe positions in X and Y direction in the local space.
+ for toe_index in range(NUM_LEGS):
+ toe_pos_local = self.current_local_toe_positions[toe_index]
+ toe_pos_local_xy = [toe_pos_local[0], toe_pos_local[1]]
+ observation.extend(toe_pos_local_xy)
+
+ return observation
+
+ def _get_constant_accel_interpolation(self, interp_ratio):
+ """Modify an interpolation between 0 and 1 to have constant acceleration."""
+ assert interp_ratio <= 1.0 and interp_ratio >= 0.0
+ if interp_ratio < 0.5:
+ return 0.5 * (2 * interp_ratio)**2
+ else:
+ return -0.5 * (2 * interp_ratio - 2)**2 + 1
+
+ def _move_com(self, target_com_movement, time_since_transition,
+ state_duration):
+ """Get the ik action for moving the COM.
+
+ Args:
+ target_com_movement: Target COM movement relative to the previous COM
+ position in the x-y plane.
+ time_since_transition: Time elapsed since last state machien transition.
+ state_duration: Duration of the state machine. The first half will be used
+ for moving COM and the second half for moving the swing leg.
+
+ Returns:
+ ik_action: ik targets for moving the COM.
+ """
+ com_movement_ratio = np.clip(
+ time_since_transition / ((state_duration - 0.0) / 2.0), 0, 1)
+ com_movement_ratio = self._get_constant_accel_interpolation(
+ com_movement_ratio)
+ current_com_movement = np.array(target_com_movement) * com_movement_ratio
+
+ ik_action = []
+ for toe_index in range(NUM_LEGS):
+ toe_pos_local = np.copy(self.current_local_toe_positions[toe_index])
+ toe_pos_local[2] = self._default_local_toe_positions[toe_index][2]
+ toe_pos_local[0] -= current_com_movement[0]
+ toe_pos_local[1] -= current_com_movement[1]
+ toe_pos_local[0] = np.clip(
+ toe_pos_local[0], self._default_local_toe_positions[toe_index][0] -
+ self._toe_move_bound,
+ self._default_local_toe_positions[toe_index][0] +
+ self._toe_move_bound)
+ toe_pos_local[1] = np.clip(
+ toe_pos_local[1], self._default_local_toe_positions[toe_index][1] -
+ self._toe_move_bound,
+ self._default_local_toe_positions[toe_index][1] +
+ self._toe_move_bound)
+ ik_action.extend(toe_pos_local)
+
+ zero_translation = [0, 0, 0]
+ identity_rotation = [0, 0, 0, 1]
+ ik_action.extend(zero_translation)
+ ik_action.extend(identity_rotation)
+
+ return ik_action
+
+ def _move_leg(self, target_toe_movement, time_since_transition,
+ state_duration):
+ """Get the ik action for moving the swing leg.
+
+ Args:
+ target_toe_movement: Target toe movement relative to the default toe
+ position in the positive x direction.
+ time_since_transition: Time elapsed since last state machien transition.
+ state_duration: Duration of the state machine. The first half will be used
+ for moving COM and the second half for moving the swing leg.
+
+ Returns:
+ ik_action: ik targets for moving the swing leg.
+ """
+
+ # The target toe position at the end of the movement.
+ target_toe_local_position = np.array(self._default_local_toe_positions[
+ self._state_to_foot_id[self.current_state_machine_state]])
+ target_toe_local_position[0] += target_toe_movement
+
+ # Toe position at the beginning of the movement.
+ initial_toe_local_position = np.array(self.current_local_toe_positions[
+ self._state_to_foot_id[self.current_state_machine_state]])
+
+ # Auxiliary variables for computing the interpolation between the current
+ # toe position and the target toe position.
+ toe_circle_radius = 0.5 * np.linalg.norm(
+ np.array(target_toe_local_position) - initial_toe_local_position)
+ toe_moving_direction = np.array(
+ target_toe_local_position) - initial_toe_local_position
+ toe_moving_direction /= np.max([np.linalg.norm(toe_moving_direction), 1e-5])
+ toe_traj_scale_ratio = self._foot_lift_height / np.max(
+ [toe_circle_radius, 1e-5])
+
+ # Current percentage of time into the state machine.
+ toe_movement_ratio = np.clip(
+ (time_since_transition - state_duration / 2.0) /
+ ((state_duration - 0.0) / 2.0), 0, 1)
+ toe_movement_ratio = self._get_constant_accel_interpolation(
+ toe_movement_ratio)
+ toe_circle_moved_angle = np.pi * toe_movement_ratio
+
+ # Target horizontal movement from the previous local toe position.
+ target_toe_horizontal_movement = toe_circle_radius - np.cos(
+ toe_circle_moved_angle) * toe_circle_radius
+ current_toe_target = np.array([
+ target_toe_horizontal_movement * toe_moving_direction[0],
+ target_toe_horizontal_movement * toe_moving_direction[1],
+ np.sin(toe_circle_moved_angle) * toe_circle_radius *
+ toe_traj_scale_ratio
+ ]) + initial_toe_local_position
+
+ ik_action = []
+ for toe_index in range(NUM_LEGS):
+ toe_pos_local = np.copy(self.current_local_toe_positions[toe_index])
+ toe_pos_local[2] = self._default_local_toe_positions[toe_index][2]
+ if toe_index == self._state_to_foot_id[self.current_state_machine_state]:
+ toe_pos_local[0] = current_toe_target[0]
+ toe_pos_local[1] = current_toe_target[1]
+ toe_pos_local[2] = current_toe_target[2]
+
+ ik_action.extend(toe_pos_local)
+
+ zero_translation = [0, 0, 0]
+ identity_rotation = [0, 0, 0, 1]
+ ik_action.extend(zero_translation)
+ ik_action.extend(identity_rotation)
+ return ik_action
+
+ def _update_state_machine_transition(self):
+ """Update the state machine state if the duration has been reached."""
+ self.current_state_machine_state = (self.current_state_machine_state + 1
+ ) % GaitStateMachine.TOTAL_GAIT_STATE_NUM
+ self.last_state_transition_time = self.robot.GetTimeSinceReset()
+
+ def _update_local_toe_positions(self):
+ """Update the local position of the toes."""
+ identity_orientation = [0, 0, 0, 1]
+ base_position = self.robot.GetBasePosition()
+ base_orientation = self.robot.GetBaseOrientation()
+ inv_base_position, inv_base_orientation = self.pybullet_client.invertTransform(
+ base_position, base_orientation)
+ for toe_index in range(NUM_LEGS):
+ toe_pose_world = self.pybullet_client.getLinkState(
+ self.robot.quadruped, self._toe_link_indices[toe_index])[0]
+ toe_pos_local, _ = self.pybullet_client.multiplyTransforms(
+ inv_base_position, inv_base_orientation, toe_pose_world,
+ identity_orientation)
+ self.current_local_toe_positions[toe_index] = np.array(toe_pos_local)
+
+ def step(self, action):
+ """Steps the wrapped environment.
+
+ Args:
+ action: Numpy array. The input action from an NN agent.
+
+ Returns:
+ The tuple containing the modified observation, the reward, the epsiode end
+ indicator.
+
+ Raises:
+ ValueError if input action is None.
+
+ """
+ if action is None:
+ raise ValueError("Action cannot be None")
+ action = np.clip(action, self.action_space.low, self.action_space.high)
+ sum_reward = 0
+ step_num = 0
+ state_duration = self.state_machine_state_duration[
+ self.current_state_machine_state]
+ time_since_transition = self.robot.GetTimeSinceReset(
+ ) - self.last_state_transition_time
+
+ # Move COM
+ while time_since_transition < state_duration / 2.0:
+ ik_actions = self._move_com(action[1:3], time_since_transition,
+ state_duration)
+ _, reward, done, _ = self._gym_env.step(ik_actions)
+ sum_reward += reward
+ step_num += 1
+ time_since_transition = self.robot.GetTimeSinceReset(
+ ) - self.last_state_transition_time
+ if done:
+ break
+ self._update_local_toe_positions()
+ # Move Leg
+ while time_since_transition < state_duration:
+ ik_actions = self._move_leg(action[0], time_since_transition,
+ state_duration)
+ _, reward, done, _ = self._gym_env.step(ik_actions)
+ sum_reward += reward
+ step_num += 1
+ time_since_transition = self.robot.GetTimeSinceReset(
+ ) - self.last_state_transition_time
+ if done:
+ break
+ self._update_local_toe_positions()
+ self._update_state_machine_transition()
+
+ state_machine_observation = self._state_machine_observation()
+
+ return state_machine_observation, sum_reward, done, _
+
+ def reset(self):
+ """Reset the simulation and state machine states."""
+ self.current_state_machine_state = GaitStateMachine.STEP_LEFT_FRONT_TOE
+ self.last_state_transition_time = 0
+ self.current_local_toe_positions = np.copy(
+ self._default_local_toe_positions)
+
+ self._gym_env.reset()
+
+ state_machine_observation = self._state_machine_observation()
+
+ return state_machine_observation
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_wrappers/step_based_curriculum_wrapper_env.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_wrappers/step_based_curriculum_wrapper_env.py
new file mode 100644
index 000000000..e6296cd80
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_wrappers/step_based_curriculum_wrapper_env.py
@@ -0,0 +1,231 @@
+"""A wrapped LocomotionGymEnv with functions that change the world and task."""
+
+import numbers
+from typing import Optional, Sequence, Text, Tuple, Union
+
+import numpy as np
+
+import gin
+
+
+@gin.configurable
+class CurriculumParameter(object):
+ """Definition of a env parameter tuned by a curriculum."""
+
+ def __init__(self,
+ name: Text,
+ init_val: numbers.Real,
+ bounds: Tuple[numbers.Real, numbers.Real]):
+ """A parameter to tune throughout the curriculum.
+
+ Args:
+ name: The name of the curriculum parameter. This must be the name of
+ attribute of the scene class.
+ init_val: The value to use at the start of the curriculum.
+ bounds: A tuple of [lower_bound, upper_bound] defining the minimum and
+ maximum values of the parameter.
+ """
+ self.name = name
+
+ if not isinstance(bounds[0], type(bounds[1])):
+ raise ValueError("All elements in [bounds] must be of the same type.")
+
+ if (init_val < min(bounds)) or (init_val > max(bounds)):
+ raise ValueError("Initial parameter value must lie in range defined"
+ " by [bounds].")
+
+ if not isinstance(init_val, type(bounds[0])):
+ raise ValueError("[init_val] type must match the type of the elements"
+ " in [bounds].")
+
+ self.init_val = init_val
+ self.bounds = bounds
+ self.dtype = type(init_val)
+
+
+@gin.configurable
+class RandomSamplingCurriculumParameter(CurriculumParameter):
+ """Env parameter whose value is sampled randomly without curriculum."""
+
+ def __init__(self, name: Text, bounds: Tuple[numbers.Real, numbers.Real]):
+ """A parameter whose value to sample randomly.
+
+ Args:
+ name: The name of the parameter.
+ bounds: A tuple of [lower_bound, upper_bound] defining the minimum and
+ maximum values of the parameter.
+ """
+ super(RandomSamplingCurriculumParameter, self).__init__(
+ name=name, init_val=bounds[0], bounds=bounds)
+
+ def sample(
+ self, step: numbers.Real,
+ curriculum_steps: Optional[numbers.Real] = None) -> Union[int, float]:
+ del step, curriculum_steps
+ sampled_val = np.random.uniform(*self.bounds)
+
+ if not isinstance(self.bounds[0], float):
+ sampled_val = int(round(sampled_val))
+
+ return sampled_val
+
+ def __call__(
+ self, step: numbers.Real,
+ curriculum_steps: Optional[numbers.Real] = None) -> Union[int, float]:
+ return self.sample(step, curriculum_steps)
+
+
+@gin.configurable
+class LinearStepBasedCurriculumParameter(CurriculumParameter):
+ """Definition of a env parameter tuned by a linear time-based curriculum."""
+
+ def __init__(self,
+ name: Text,
+ init_val: numbers.Real,
+ bounds: Tuple[numbers.Real, numbers.Real],
+ curriculum_steps: Optional[numbers.Real] = None):
+ """A parameter to tune throughout the curriculum.
+
+ Args:
+ name: The name of the curriculum parameter. This must be the name of
+ attribute of the scene class.
+ init_val: The value to use at the start of the curriculum.
+ bounds: A tuple of [lower_bound, upper_bound] defining the minimum and
+ maximum values of the parameter.
+ curriculum_steps: Integer defining the number of steps to take when
+ varying the curriculum parameter value from the init_val to either
+ bound. If None is specified, then the curriculum must provide the
+ curriculum_steps when sampling.
+ """
+ super(LinearStepBasedCurriculumParameter, self).__init__(
+ name=name, init_val=init_val, bounds=bounds)
+ self.curriculum_steps = curriculum_steps
+
+ def get_bounds_at_step(
+ self, step: numbers.Real,
+ curriculum_steps: Optional[numbers.Real] = None
+ ) -> Tuple[Union[int, float], Union[int, float]]:
+ """Compute the bounds of the parameter at the current step.
+
+ Args:
+ step: An integer defining the current timestep.
+ curriculum_steps: Optional curriculum steps. Must be passed if not passed
+ during initialization.
+ Returns:
+ A tuple containing the lower bound and upper bound at the current step.
+ """
+
+ if not self.curriculum_steps and not curriculum_steps:
+ raise ValueError("curriculum_steps not defined. Must be passed upon"
+ " initialization or must specified by curriculum"
+ " wrapper env on method call.")
+
+ if self.curriculum_steps:
+ curriculum_steps = self.curriculum_steps
+
+ prog = min(float(step) / curriculum_steps, 1.0)
+ curr_lower_bound = self.init_val - prog * (self.init_val - self.bounds[0])
+ curr_upper_bound = self.init_val + prog * (self.bounds[1] - self.init_val)
+
+ if not isinstance(self.bounds[0], float):
+ curr_lower_bound = int(round(curr_lower_bound))
+ curr_upper_bound = int(round(curr_upper_bound))
+
+ return (curr_lower_bound, curr_upper_bound)
+
+ def sample(
+ self, step: numbers.Real,
+ curriculum_steps: Optional[numbers.Real] = None) -> Union[int, float]:
+ sampled_val = np.random.uniform(*self.get_bounds_at_step(
+ step, curriculum_steps))
+
+ if not isinstance(self.bounds[0], float):
+ sampled_val = int(round(sampled_val))
+
+ return sampled_val
+
+ def __call__(
+ self, step: numbers.Real,
+ curriculum_steps: Optional[numbers.Real] = None) -> Union[int, float]:
+ return self.sample(step, curriculum_steps)
+
+
+@gin.configurable
+class Task(object):
+ """Defines a single task in the environment and its corresponding params."""
+
+ def __init__(self, name: Text,
+ curriculum_parameters: Sequence[CurriculumParameter]):
+ """Initialize the task.
+
+ Args:
+ name: The name of the task.
+ curriculum_parameters: A list of CurriculumParameter instances which
+ define the parameters that this task makes use of.
+ """
+ self.name = name
+ self.curriculum_parameters = curriculum_parameters
+
+
+@gin.configurable
+class StepBasedCurriculumWrapperEnv(object):
+ """A wrapper to tune the scene parameters linearly with the steps taken."""
+
+ def __init__(self, env, tasks: Sequence[Task],
+ default_curriculum_steps: Optional[numbers.Real] = None,
+ reset_total_step_count_val: numbers.Real = -1,
+ steps_before_curriculum_start: numbers.Real = 0):
+ """Initializes the linear curriculum wrapper env.
+
+ Args:
+ env: An instance of a (potentially previously wrapped) LocomotionGymEnv.
+ tasks: Various tasks to shuffle through throughout the curriculum.
+ default_curriculum_steps: Optional default value for curriculum steps.
+ reset_total_step_count_val: Step at which to reset the total step count.
+ The internal total_step_count is reset to 0 once this value is reached.
+ steps_before_curriculum_start: Steps to take in environment before the
+ curriculum begins.
+ """
+ self._gym_env = env
+ self._tasks = tasks
+ self._default_curriculum_steps = default_curriculum_steps
+ self._reset_total_step_count_val = reset_total_step_count_val
+ self._steps_before_curriculum_start = steps_before_curriculum_start
+
+ # Total number of environment steps.
+ self._total_step_count = 0
+
+ def __getattr__(self, attr):
+ return getattr(self._gym_env, attr)
+
+ def set_scene_params(self):
+ # Choose a task at random.
+ curr_task = np.random.choice(self._tasks)
+
+ # Cycle through the task's parameters and set them in the scene.
+ self._gym_env.scene.reset_scene_params()
+ self._gym_env.scene.scene_type = curr_task.name
+ for curriculum_parameter in curr_task.curriculum_parameters:
+ setattr(
+ self._gym_env.scene, curriculum_parameter.name,
+ curriculum_parameter(
+ self._total_step_count - self._steps_before_curriculum_start,
+ self._default_curriculum_steps))
+
+ def reset(self, *args, **kwargs):
+ """Reset and adjust the environment."""
+
+ # Update the total step count.
+ self._total_step_count += self._gym_env.env_step_counter
+ if self._reset_total_step_count_val >= 0:
+ if self._total_step_count >= self._reset_total_step_count_val:
+ self._total_step_count = 0
+
+ if self._total_step_count < self._steps_before_curriculum_start:
+ return self._get_observation()
+
+ self.set_scene_params()
+ self._gym_env.reset(*args, **kwargs)
+
+ return self._get_observation()
+
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_wrappers/trajectory_generator_wrapper_env.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_wrappers/trajectory_generator_wrapper_env.py
new file mode 100644
index 000000000..a2c6f0bf6
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_wrappers/trajectory_generator_wrapper_env.py
@@ -0,0 +1,81 @@
+"""A wrapped MinitaurGymEnv with a built-in controller."""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+
+import gin
+
+
+@gin.configurable
+class TrajectoryGeneratorWrapperEnv(object):
+ """A wrapped LocomotionGymEnv with a built-in trajectory generator."""
+
+ def __init__(self, gym_env, trajectory_generator):
+ """Initialzes the wrapped env.
+
+ Args:
+ gym_env: An instance of LocomotionGymEnv.
+ trajectory_generator: A trajectory_generator that can potentially modify
+ the action and observation. Typticall generators includes the PMTG and
+ openloop signals. Expected to have get_action and get_observation
+ interfaces.
+
+ Raises:
+ ValueError if the controller does not implement get_action and
+ get_observation.
+
+ """
+ self._gym_env = gym_env
+ if not hasattr(trajectory_generator, 'get_action') or not hasattr(
+ trajectory_generator, 'get_observation'):
+ raise ValueError(
+ 'The controller does not have the necessary interface(s) implemented.'
+ )
+
+ self._trajectory_generator = trajectory_generator
+
+ # The trajectory generator can subsume the action/observation space.
+ if hasattr(trajectory_generator, 'observation_space'):
+ self.observation_space = self._trajectory_generator.observation_space
+
+ if hasattr(trajectory_generator, 'action_space'):
+ self.action_space = self._trajectory_generator.action_space
+
+ def __getattr__(self, attr):
+ return getattr(self._gym_env, attr)
+
+ def _modify_observation(self, observation):
+ return self._trajectory_generator.get_observation(observation)
+
+ def reset(self, initial_motor_angles=None, reset_duration=1.0):
+ if getattr(self._trajectory_generator, 'reset'):
+ self._trajectory_generator.reset()
+ observation = self._gym_env.reset(initial_motor_angles, reset_duration)
+ return self._modify_observation(observation)
+
+ def step(self, action):
+ """Steps the wrapped environment.
+
+ Args:
+ action: Numpy array. The input action from an NN agent.
+
+ Returns:
+ The tuple containing the modified observation, the reward, the epsiode end
+ indicator.
+
+ Raises:
+ ValueError if input action is None.
+
+ """
+
+ if action is None:
+ raise ValueError('Action cannot be None')
+
+ new_action = self._trajectory_generator.get_action(
+ self._gym_env.robot.GetTimeSinceReset(), action)
+
+ original_observation, reward, done, _ = self._gym_env.step(new_action)
+
+ return self._modify_observation(original_observation), reward, done, _
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_wrappers/walking_wrapper.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_wrappers/walking_wrapper.py
new file mode 100644
index 000000000..6235a8536
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/env_wrappers/walking_wrapper.py
@@ -0,0 +1,134 @@
+"""Wraps a quadruped walking controller for navigation control."""
+
+from typing import Any
+import gin
+from gym import spaces
+import numpy as np
+
+from pybullet_envs.minitaur.agents.baseline_controller import com_velocity_estimator
+from pybullet_envs.minitaur.agents.baseline_controller import locomotion_controller
+from pybullet_envs.minitaur.agents.baseline_controller import openloop_gait_generator
+from pybullet_envs.minitaur.agents.baseline_controller import raibert_swing_leg_controller
+from pybullet_envs.minitaur.agents.baseline_controller import torque_stance_leg_controller
+
+_N_LEGS = 4
+_STANCE_DURATION_SECONDS = [
+ 0.25
+] * _N_LEGS # The stance phase duration for each leg.
+_DUTY_FACTOR = [
+ 0.6
+] * _N_LEGS # Percentage of the leg in the stance phase within the cycle.
+_BODY_HEIGHT = 0.45
+_INIT_PHASE_FULL_CYCLE = [0, 0.5, 0.5, 0]
+
+
+def _setup_controller(robot: Any) -> locomotion_controller.LocomotionController:
+ """Creates the controller."""
+ desired_speed = (0, 0)
+ desired_twisting_speed = 0
+
+ gait_generator = openloop_gait_generator.OpenloopGaitGenerator(
+ robot,
+ stance_duration=_STANCE_DURATION_SECONDS,
+ duty_factor=_DUTY_FACTOR,
+ initial_leg_phase=_INIT_PHASE_FULL_CYCLE)
+ state_estimator = com_velocity_estimator.COMVelocityEstimator(robot)
+ sw_controller = raibert_swing_leg_controller.RaibertSwingLegController(
+ robot,
+ gait_generator,
+ state_estimator,
+ desired_speed=desired_speed,
+ desired_twisting_speed=desired_twisting_speed,
+ desired_height=_BODY_HEIGHT,
+ )
+ st_controller = torque_stance_leg_controller.TorqueStanceLegController(
+ robot,
+ gait_generator,
+ state_estimator,
+ desired_speed=desired_speed,
+ desired_twisting_speed=desired_twisting_speed,
+ desired_body_height=_BODY_HEIGHT,
+ body_mass=215 / 9.8,
+ body_inertia=(0.07335, 0, 0, 0, 0.25068, 0, 0, 0, 0.25447),
+ )
+
+ controller = locomotion_controller.LocomotionController(
+ robot=robot,
+ gait_generator=gait_generator,
+ state_estimator=state_estimator,
+ swing_leg_controller=sw_controller,
+ stance_leg_controller=st_controller,
+ clock=robot.GetTimeSinceReset)
+ return controller
+
+
+def _update_controller_params(
+ controller: locomotion_controller.LocomotionController,
+ lin_speed: np.ndarray, ang_speed: float):
+ """Apply the desired speed and twisting speed."""
+ controller.swing_leg_controller.desired_speed = lin_speed
+ controller.swing_leg_controller.desired_twisting_speed = ang_speed
+ controller.stance_leg_controller.desired_speed = lin_speed
+ controller.stance_leg_controller.desired_twisting_speed = ang_speed
+
+
+@gin.configurable
+class WalkingWrapper(object):
+ """Wraps a baseline walking controller for Laikago/Vision60."""
+
+ def __init__(self,
+ gym_env: Any,
+ action_repeat=20,
+ speed_bound=(-0.3, 0.3),
+ angular_speed_bound=(-0.3, 0.3)):
+ """Initialzes the wrapped env.
+
+ Args:
+ gym_env: An instance of LocomotionGymEnv.
+ action_repeat: Number of control steps to run low level controller with
+ the high level inputs per step().
+ speed_bound: The min/max of the input speed.
+ angular_speed_bound: The min/max of the twisting speed.
+ """
+ self._gym_env = gym_env
+ self._controller = _setup_controller(self._gym_env.robot)
+ self._action_repeat = action_repeat
+
+ action_low = np.array(speed_bound)
+ action_high = np.array(angular_speed_bound)
+
+ # Overwrite the action space.
+ self.action_space = spaces.Box(action_low, action_high)
+
+ def reset(self, *args, **kwargs) -> Any:
+ obs = self._gym_env.reset(*args, **kwargs)
+ # The robot instance might have been replaced if hard_reset is called. We
+ # just recreate the controller.
+ self._controller = _setup_controller(self._gym_env.robot)
+ self._controller.reset()
+ return obs
+
+ def __getattr__(self, attr):
+ return getattr(self._gym_env, attr)
+
+ def step(self, action) -> Any:
+ """Steps the wrapped env with high level commands.
+
+ Args:
+ action: A high level command containing the desired linear and angular
+ speed of the robot base. The speed can be adjusted at any time.
+
+ Returns:
+ The gym env observation, reward, termination, and additional info.
+
+ """
+ lin_speed = np.array((action[0], 0, 0))
+ ang_speed = action[1]
+ _update_controller_params(self._controller, lin_speed, ang_speed)
+ for _ in range(self._action_repeat):
+ self._controller.update()
+ hybrid_action = self._controller.get_action()
+ obs, reward, done, info = self._gym_env.step(hybrid_action)
+ if done:
+ break
+ return obs, reward, done, info
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/evaluation/__init__.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/evaluation/__init__.py
new file mode 100644
index 000000000..e69de29bb
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/evaluation/metric.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/evaluation/metric.py
new file mode 100644
index 000000000..00dea1800
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/evaluation/metric.py
@@ -0,0 +1,125 @@
+"""The metric reporting system used in our env.
+
+In the gym environment, there are many variables or quantities that can help
+researchers to debug, evaluate policy performance. Such quantities may
+include: Motor torques for quadrupeds, even when they are controlled in
+POSITION mode; Distance to walls while a wheeled robot is navigating the
+indoor environment. Often, these metrics are private variables (or can only be
+computed from private variables). To expose the user interested metrics
+outside the environment's observations, we designed this Metric system that
+can be invoked in any modules (robot, tasks, sensors) inside the gym env.
+"""
+
+import enum
+import logging
+from typing import Any, Callable, Dict, Sequence, Text
+import gin
+
+
+@gin.constants_from_enum
+class MetricScope(enum.Enum):
+ """The supported scope of metrics."""
+ # The performance metrics.
+ PERFORMANCE = 1,
+
+ # The debug metrics for diagnostic purposes.
+ DEBUG = 2,
+
+ # The safety metrics.
+ SAFETY = 3,
+
+ # The statistics of episodes in metric format.
+ STATISTIC = 4,
+
+
+class MetricCore(object):
+ """Aggregates values of interest to compute statistics."""
+
+ def __init__(
+ self,
+ name: Text,
+ scope: MetricScope,
+ single_ep_aggregator: Callable[[Sequence[Any]], Any],
+ multi_ep_aggregator: Callable[[Sequence[Any]], Dict[Text, Any]],
+ ):
+ """Initializes the class.
+
+ Args:
+ name: The name of the metric, for example "motor_torques",
+ "distance_to_wall", etc. The full name of the metric will have scope
+ name in the prefix, i.e. "scope/name".
+ scope: The scope of this metric. Most metric should be for DEBUG purpose.
+ The scope name will be added to the final name of metric in this way:
+ "scope/name", which is standarded format for Tensorboard to group
+ named variables.
+ single_ep_aggregator: The function to process all aggregated metric
+ values. The derived MetricReporter (see below) will implements
+ reset_episode() which clears the episode data, and will be called during
+ env.reset().
+ multi_ep_aggregator: The functions to process multi-episode metric values.
+ We assume the inputs to the functions is a list of per episode metric
+ values, i.e. each element of the list is the output from the
+ single_ep_aggregator.
+ """
+ self._name = scope.name + "/" + name
+ self._single_ep_aggregator = single_ep_aggregator
+ self._multi_ep_aggregator = multi_ep_aggregator
+ self._episode_data = []
+
+ def report(self, metric_value: Any):
+ """Stores the reported metric in the internal buffer.
+
+ Args:
+ metric_value: The metric we are interested to report.
+ """
+ self._episode_data.append(metric_value)
+
+
+class MetricReporter(MetricCore):
+ """Reports the metric using the provided aggregator functions."""
+
+ def get_episode_metric(self) -> Dict[Text, Sequence[Any]]:
+ """Processes and returns episode metric values.
+
+ Returns:
+ Aggregated metrics for the current episode.
+ """
+ if self._episode_data:
+ return {self._name: self._single_ep_aggregator(self._episode_data)}
+ else:
+ return {}
+
+ def get_multi_ep_metric(
+ self, episodic_metric: Dict[Text, Sequence[Any]]) -> Dict[Text, Any]:
+ """Processes the aggregated metrics over many episodes.
+
+ Will not be affected by reset_episode, since we take multi-episode data as
+ inputs.
+
+ Args:
+ episodic_metric: The per episode metrics. We expect the inputs to contain
+ the same key as self._name, and that the value is a list of metric
+ values computed using self.get_episode_metrc().
+
+ Returns:
+ The processed multi-episode metrics.
+ """
+ if self._name not in episodic_metric:
+ logging.warning(
+ "The inputs does not contain the key for the current metric: %s",
+ self._name)
+ return {}
+ outputs = {}
+ for key, val in self._multi_ep_aggregator(
+ episodic_metric[self._name]).items():
+ outputs[self._name + "_" + key] = val
+ return outputs
+
+ def reset_episode(self):
+ """Clears the episode data stored.
+
+ Will be invoked during env.reset(). This will effect how get_episode_metric
+ gets computed.
+
+ """
+ self._episode_data = []
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/evaluation/metric_logger.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/evaluation/metric_logger.py
new file mode 100644
index 000000000..05d24b4e1
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/evaluation/metric_logger.py
@@ -0,0 +1,155 @@
+# Lint as: python3
+"""The system to log and manage all metrics."""
+
+import logging
+from typing import Any, Callable, Dict, Sequence, Text, Union
+import numpy as np
+
+from pybullet_envs.minitaur.envs_v2.evaluation import metric as metric_lib
+
+
+def _merge_dict_throw_on_duplicates(
+ dict_1: Dict[Text, Any],
+ dict_2: Dict[Text, Any],
+):
+ """Merge the contents of dict_2 to dict_1.
+
+ Args:
+ dict_1: The dictionary to merge into.
+ dict_2: The dictionary to merge from.
+
+ Raises:
+ KeyError: if duplicated keys are found.
+ """
+ for key, val in dict_2.items():
+ if key in dict_1:
+ raise KeyError(f"Duplicate key: {key} found in both "
+ f"dictionaries: {dict_1} {dict_2}.")
+ dict_1[key] = val
+
+
+def common_stats(x: Sequence[Union[float, Sequence[float]]],
+ flatten_array=False):
+ out = x
+ if flatten_array:
+ # Deals with array of arrays.
+ out = np.concatenate(x).flatten()
+ return {
+ "mean": np.mean(out),
+ "max": np.max(out),
+ "min": np.min(out),
+ "std": np.std(out),
+ }
+
+
+class MetricLogger(object):
+ """The central system to manage all metrics."""
+
+ def __init__(self, ignore_duplicate_metrics: bool = True):
+ """Initializes the system.
+
+ Args:
+ ignore_duplicate_metrics: Don't throw error when users want to recreate
+ the same metric.
+ """
+ self._metric_reporters = {}
+ self._ignore_duplicate_metrics = ignore_duplicate_metrics
+
+ def reset_episode(self):
+ """Resets all metric reporters's internal buffer.
+
+ Will be called by the gym env during reset.
+ """
+ for metric in self._metric_reporters.values():
+ metric.reset_episode()
+
+ def create_metric(
+ self,
+ name: Text,
+ scope: metric_lib.MetricScope,
+ single_ep_aggregator: Callable[[Sequence[Any]], Any],
+ multi_ep_aggregator: Callable[[Sequence[Any]], Dict[Text, Any]],
+ ) -> metric_lib.MetricCore:
+ """Creates a new metric.
+
+ Args:
+ name: The name of the metric, for example "motor_torques",
+ "distance_to_wall", etc. The full name of the metric will have scope
+ name in the prefix, i.e. "scope/name".
+ scope: The scope of this metric. Most metric should be for DEBUG purpose.
+ The scope name will be added to the final name of metric in this way:
+ "scope/name", which is standarded format for Tensorboard to group
+ named variables.
+ single_ep_aggregator: The function to process all aggregated metric
+ values. The derived MetricReporter (see below) will implements
+ reset_episode() which clears the episode data, and will be called during
+ env.reset().
+ multi_ep_aggregator: The function to process multi-episode metric values.
+ We assume the inputs to the function is a list of per episode metric
+ values, i.e. each element of the list is the output from the
+ single_ep_aggregator.
+
+ Returns:
+ A MetricCore which can be used to report values of interest.
+ """
+ if name in self._metric_reporters:
+ if self._ignore_duplicate_metrics:
+ logging.warning("Trying to create an existing metric: %s", name)
+ name = self._get_valid_duplicate_name(name)
+ logging.warning("Changed duplicate metric to new name: %s", name)
+ else:
+ raise ValueError(f"Duplicated metrics found: {name}")
+
+ self._metric_reporters[name] = metric_lib.MetricReporter(
+ name, scope, single_ep_aggregator, multi_ep_aggregator)
+ return self._metric_reporters[name]
+
+ def create_scalar_metric(
+ self,
+ name: Text,
+ scope: metric_lib.MetricScope,
+ single_ep_aggregator: Callable[[Sequence[Any]], Any],
+ ) -> metric_lib.MetricCore:
+ """Shortcut to create a metric for scalar variables."""
+ return self.create_metric(
+ name,
+ scope,
+ single_ep_aggregator,
+ multi_ep_aggregator=common_stats,
+ )
+
+ def _get_valid_duplicate_name(self, original_name: Text) -> Text:
+ counter = 1
+ test_name = "{}_duplicate_{}".format(original_name, str(counter))
+ while test_name in self._metric_reporters:
+ counter += 1
+ test_name = "{}_duplicate_{}".format(original_name, str(counter))
+ return test_name
+
+ def get_episode_metrics(self) -> Dict[Text, Any]:
+ """Return all metrics registered in the logger for the current episode."""
+ ep_stats = {}
+ for metric in self._metric_reporters.values():
+ _merge_dict_throw_on_duplicates(ep_stats, metric.get_episode_metric())
+ return ep_stats
+
+ def get_multi_episode_metrics(
+ self, episodic_metrics: Dict[Text, Sequence[Any]]) -> Dict[Text, Any]:
+ """Processes the aggregated metrics over many episodes.
+
+ Will not be affected by reset_episode, since we take multi-episode data as
+ inputs.
+
+ Args:
+ episodic_metrics: The per episode metrics. For each key in the inputs, we
+ expect at least one metric reporter can process the corresponding value,
+ which contains a list of episodic metrics.
+
+ Returns:
+ The processed multi-episode metrics.
+ """
+ stats = {}
+ for metric in self._metric_reporters.values():
+ _merge_dict_throw_on_duplicates(
+ stats, metric.get_multi_ep_metric(episodic_metrics))
+ return stats
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/evaluation/metric_utils.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/evaluation/metric_utils.py
new file mode 100644
index 000000000..1defba977
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/evaluation/metric_utils.py
@@ -0,0 +1,29 @@
+"""Helper class and functions to make computing meteric statistics easier."""
+from typing import Any, Sequence
+
+import numpy as np
+
+
+class MetricStats(object):
+ """Helper class to make computing meteric statistics easier to manage."""
+
+ def __init__(self, data: Sequence[Any]):
+ if None or not list(data):
+ raise ValueError("Input data for ComputeMetricStats cannot be empty.")
+ self._data = np.asarray(data).flatten()
+
+ @property
+ def avg(self):
+ return np.mean(self._data)
+
+ @property
+ def min(self):
+ return np.min(self._data)
+
+ @property
+ def max(self):
+ return np.max(self._data)
+
+ @property
+ def sum(self):
+ return np.sum(self._data)
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
new file mode 100644
index 000000000..ad41f5633
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/examples/laikago_mpc_wrapper_example.py
@@ -0,0 +1,96 @@
+# Lint as: python3
+r"""An example where Laikago walks forward using mpc controller.
+
+This script illustrates an example of MPCLocomotionWrapper class for
+controlling a simulated Laikago robot to walk on a flat ground. The wrapped
+environment takes as input the target local foothold locations and the desired
+base pose. An MPC-based controller is executed internally to compute the
+required forces to achieve the desired foothold position and base movement.
+
+
+blaze run -c opt \
+//robotics/reinforcement_learning/minitaur/envs_v2/examples\
+:laikago_mpc_wrapper_example
+"""
+import os
+import tempfile
+
+from absl import app
+from absl import flags
+import gin
+import numpy as np
+import pybullet_data as pd
+
+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")
+
+NUM_STEPS = 100
+ENABLE_RENDERING = True # Will be disabled for tests
+ENV_RANDOM_SEED = 100
+DEFAULT_TARGET_FOOTHOLD = (0.05, 0.0, -0.01)
+DEFAULT_BASE_VELOCITY = (0.0, 0.0)
+DEFAULT_TWIST_SPEED = 0.0
+DEFAULT_BODY_HEIGHT = 0.45
+DEFAULT_ROLL_PITCH = (0.0, 0.0)
+DEFAULT_SWING_HEIGHT = 0.07
+
+
+def _build_env():
+ """Builds the environment for the Laikago robot.
+
+ Returns:
+ The OpenAI gym environment.
+ """
+ gin.parse_config_file(CONFIG_FILE_SIM)
+ gin.bind_parameter("SimulationParameters.enable_rendering", ENABLE_RENDERING)
+ env = env_loader.load()
+ env.seed(ENV_RANDOM_SEED)
+
+ return env
+
+
+def _run_example():
+ """An example that Laikago moves with a constant speed and predicts foothold.
+
+ Returns:
+ env: the environment after the simulation
+ """
+
+ env = _build_env()
+
+ env.reset()
+ if FLAGS.video_file is not None:
+ pybullet = env.pybullet_client
+ pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_GUI, 0)
+ log_id = pybullet.startStateLogging(pybullet.STATE_LOGGING_VIDEO_MP4,
+ FLAGS.video_file)
+
+ try:
+ max_step = NUM_STEPS
+ for _ in range(max_step):
+ target_foothold = np.array(DEFAULT_TARGET_FOOTHOLD)
+ action = np.concatenate([
+ target_foothold, target_foothold, target_foothold, target_foothold,
+ [
+ DEFAULT_SWING_HEIGHT, DEFAULT_SWING_HEIGHT, DEFAULT_SWING_HEIGHT,
+ DEFAULT_SWING_HEIGHT
+ ], DEFAULT_BASE_VELOCITY, [DEFAULT_TWIST_SPEED],
+ [DEFAULT_BODY_HEIGHT], DEFAULT_ROLL_PITCH
+ ])
+ _ = env.step(action)
+ finally:
+ if FLAGS.video_dir is not None:
+ pybullet.stopStateLogging(log_id)
+
+
+def main(argv):
+ del argv
+ _run_example()
+
+
+if __name__ == "__main__":
+ app.run(main)
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
new file mode 100644
index 000000000..c86c10db4
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/examples/laikago_pmtg_example.py
@@ -0,0 +1,84 @@
+r"""An example to run an OpenAI gym environment with laikago and PMTG wrapper.
+
+This is an open-loop controller where we use 0 residuals and only execute the
+trajectory generator. The parameters that are (normally modulated by the policy)
+are fixed (except the intensity) and not optimized. They are hand picked as
+follows:
+ - The gait cycle frequency is 3 Hz.
+ - Walking height is neutral (0).
+ - Swing vs stance ratio is 2 (swing takes half the time vs stance).
+ - Intensity starts from zero and is gradually increased over time.
+
+blaze run -c opt \
+//robotics/reinforcement_learning/minitaur/envs_v2/examples\
+:laikago_pmtg_example
+"""
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+import os
+
+import gin
+import tensorflow.compat.v1 as tf
+from pybullet_envs.minitaur.envs_v2 import env_loader
+import pybullet_data as pd
+
+CONFIG_DIR = pd.getDataPath()+"/configs_v2/"
+CONFIG_FILES = [
+ os.path.join(CONFIG_DIR, "base/laikago_with_imu.gin"),
+ os.path.join(CONFIG_DIR, "tasks/fwd_task_no_termination.gin"),
+ os.path.join(CONFIG_DIR, "wrappers/pmtg_wrapper.gin"),
+ os.path.join(CONFIG_DIR, "scenes/simple_scene.gin")
+]
+# Constants used to show example PMTG behavior with zero residual.
+# Since we use the default PMTG configuration, there is only one trajectory
+# generator. So the parameters that can be changed are:
+# - Multiplier for delta time (similar to frequency but per time step).
+# - Intensity of the trajectory generator.
+# - Walking heights used for the legs.
+# - The ratio of the speed of the leg during swing vs stance phase.
+# For more details, check out TgSimple._process_tg_params method.
+_PMTG_DELTA_TIME_MULTIPLIER = 2.0
+_PMTG_INTENSITY_RANGE = (0.0, 1.5)
+_PMTG_INTENSITY_STEP_SIZE = 0.0001
+_PMTG_WALKING_HEIGHT = -0.3
+_PMTG_SWING_VS_STANCE = 2
+_NUM_MOTORS = 12
+
+
+def main(argv):
+ del argv # Unused.
+
+ # Parse the gym config and create the environment.
+ for gin_file in CONFIG_FILES:
+ gin.parse_config_file(gin_file)
+ gin.bind_parameter("SimulationParameters.enable_rendering", True)
+ gin.bind_parameter("terminal_conditions.maxstep_terminal_condition.max_step",
+ 10000)
+ env = env_loader.load()
+ tg_intensity = _PMTG_INTENSITY_RANGE[0]
+ sum_reward = 0
+ env.reset()
+ done = False
+ # Use zero residual, only use the output of the trajectory generator.
+ residual = [0] * _NUM_MOTORS
+ # Since we fix residuals and all the parameters of the TG, this example
+ # is practically an open loop controller. A learned policy would provide
+ # different values for these parameters at every timestep.
+ while not done:
+ # Increase the intensity of the trajectory generator gradually
+ # to illustrate increasingly larger steps.
+ if tg_intensity < _PMTG_INTENSITY_RANGE[1]:
+ tg_intensity += _PMTG_INTENSITY_STEP_SIZE
+ tg_params = [
+ _PMTG_DELTA_TIME_MULTIPLIER, tg_intensity, _PMTG_WALKING_HEIGHT,
+ _PMTG_SWING_VS_STANCE
+ ]
+ action = residual + tg_params
+ _, reward, done, _ = env.step(action)
+ sum_reward += reward
+
+
+if __name__ == "__main__":
+ tf.app.run(main)
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/examples/laikago_static_gait_example.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/examples/laikago_static_gait_example.py
new file mode 100644
index 000000000..13e2387cc
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/examples/laikago_static_gait_example.py
@@ -0,0 +1,52 @@
+# Lint as: python3
+r"""An example that the Laikago walks forward using a static gait.
+
+blaze run -c opt //robotics/reinforcement_learning/minitaur/envs_v2/examples:\
+laikago_static_gait_example
+"""
+from absl import app
+from absl import flags
+import gin
+from pybullet_envs.minitaur.agents.baseline_controller import static_gait_controller
+from pybullet_envs.minitaur.envs_v2 import env_loader
+import pybullet_data as pd
+
+flags.DEFINE_bool("render", True, "Whether to render the example.")
+
+FLAGS = flags.FLAGS
+_CONFIG_FILE = pd.getDataPath()+"/configs/laikago_walk_static_gait.gin"
+_NUM_STEPS = 10000
+_ENV_RANDOM_SEED = 13
+
+
+def _load_config(render=False):
+ gin.parse_config_file(_CONFIG_FILE)
+ gin.bind_parameter("SimulationParameters.enable_rendering", render)
+
+
+def run_example(num_max_steps=_NUM_STEPS):
+ """Runs the example.
+
+ Args:
+ num_max_steps: Maximum number of steps this example should run for.
+ """
+ env = env_loader.load()
+
+ env.seed(_ENV_RANDOM_SEED)
+ observation = env.reset()
+ policy = static_gait_controller.StaticGaitController(env.robot)
+
+ for _ in range(num_max_steps):
+ action = policy.act(observation)
+ _, _, done, _ = env.step(action)
+ if done:
+ break
+
+
+def main(_):
+ _load_config(FLAGS.render)
+ run_example()
+
+
+if __name__ == "__main__":
+ app.run(main)
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/locomotion_gym_config.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/locomotion_gym_config.py
new file mode 100644
index 000000000..7b9ad955a
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/locomotion_gym_config.py
@@ -0,0 +1,58 @@
+"""A gin-config class for locomotion_gym_env.
+
+This should be identical to locomotion_gym_config.proto.
+"""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+from typing import Sequence, Text
+
+import attr
+import gin
+
+
+@gin.configurable
+@attr.s
+class SimulationParameters(object):
+ """Parameters specific for the pyBullet simulation."""
+ sim_time_step_s = attr.ib(type=float, default=0.002)
+ num_action_repeat = attr.ib(type=int, default=5)
+ enable_hard_reset = attr.ib(type=bool, default=False)
+ enable_rendering = attr.ib(type=bool, default=False)
+ enable_rendering_gui = attr.ib(type=bool, default=True)
+ robot_on_rack = attr.ib(type=bool, default=False)
+ camera_target = attr.ib(type=Sequence[float], default=None)
+ camera_distance = attr.ib(type=float, default=1.0)
+ camera_yaw = attr.ib(type=float, default=0)
+ camera_pitch = attr.ib(type=float, default=-30)
+ render_width = attr.ib(type=int, default=480)
+ render_height = attr.ib(type=int, default=360)
+ egl_rendering = attr.ib(type=bool, default=False)
+
+
+@gin.configurable
+@attr.s
+class ScalarField(object):
+ """A named scalar space with bounds."""
+ # TODO(sehoonha) extension to vector fields.
+ name = attr.ib(type=str)
+ upper_bound = attr.ib(type=float)
+ lower_bound = attr.ib(type=float)
+
+
+@gin.configurable
+@attr.s
+class LocomotionGymConfig(object):
+ """Grouped Config Parameters for LocomotionGym."""
+ simulation_parameters = attr.ib(type=SimulationParameters)
+ # TODO(sehoonha) implement attr validators for the list
+ actions = attr.ib(type=list, default=None) # pylint: disable=g-bare-generic
+ log_path = attr.ib(type=Text, default=None)
+ data_dir = attr.ib(
+ type=Text,
+ default='robotics/reinforcement_learning/minitaur/data/')
+ profiling_path = attr.ib(type=Text, default=None)
+ seed = attr.ib(type=int, default=None)
+ ignored_sensor_list = attr.ib(type=Sequence[Text], default=())
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
new file mode 100644
index 000000000..fc53f02e7
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/locomotion_gym_config_test.py
@@ -0,0 +1,75 @@
+"""Tests for pybullet_envs.minitaur.envs.locomotion_gym_config."""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+import gin
+from pybullet_envs.minitaur.envs_v2 import locomotion_gym_config
+import tensorflow.compat.v1 as tf
+from absl.testing import parameterized
+
+
+
+class LocomotionGymConfigTest(tf.test.TestCase, parameterized.TestCase):
+
+ def testSimulationParametersFromGinString(self):
+ config_text = (
+ 'import pybullet_envs.minitaur'
+ '.envs_v2.locomotion_gym_config\n'
+ 'locomotion_gym_config.SimulationParameters.sim_time_step_s = 0.005\n'
+ 'locomotion_gym_config.SimulationParameters.camera_distance = 5.0\n'
+ 'locomotion_gym_config.SimulationParameters.camera_yaw = 10\n'
+ 'locomotion_gym_config.SimulationParameters.camera_pitch = -50\n'
+ )
+ gin.parse_config(config_text)
+
+ cfg = locomotion_gym_config.SimulationParameters()
+ self.assertEqual(cfg.sim_time_step_s, 0.005)
+ self.assertFalse(cfg.enable_hard_reset)
+ self.assertEqual(cfg.camera_distance, 5.0)
+ self.assertEqual(cfg.camera_yaw, 10)
+ self.assertEqual(cfg.camera_pitch, -50)
+
+ def testScalarFieldFromGinString(self):
+ config_text = (
+ 'import pybullet_envs.minitaur'
+ '.envs_v2.locomotion_gym_config\n'
+ 'locomotion_gym_config.ScalarField.name = "MotorUpperLimit"\n'
+ 'locomotion_gym_config.ScalarField.upper_bound = 1.0\n'
+ 'locomotion_gym_config.ScalarField.lower_bound = -1.0\n'
+ )
+ gin.parse_config(config_text)
+
+ cfg = locomotion_gym_config.ScalarField()
+ self.assertEqual(cfg.name, 'MotorUpperLimit')
+ self.assertEqual(cfg.upper_bound, 1.0)
+ self.assertEqual(cfg.lower_bound, -1.0)
+
+ def testLocomotionGymConfigFromGinString(self):
+ config_text = (
+ 'import pybullet_envs.minitaur'
+ '.envs_v2.locomotion_gym_config\n'
+ # SimulationParameters
+ 'locomotion_gym_config.SimulationParameters.sim_time_step_s = 0.005\n'
+ # Actions
+ 'Motor/locomotion_gym_config.ScalarField.name = "MotorUpperLimit"\n'
+ 'Motor/locomotion_gym_config.ScalarField.upper_bound = 1.0\n'
+ 'Motor/locomotion_gym_config.ScalarField.lower_bound = -1.0\n'
+ # LocomotionGymConfigs
+ 'locomotion_gym_config.LocomotionGymConfig.simulation_parameters = '
+ '@locomotion_gym_config.SimulationParameters()\n'
+ 'locomotion_gym_config.LocomotionGymConfig.actions = ['
+ '@Motor/locomotion_gym_config.ScalarField()]\n'
+ 'locomotion_gym_config.LocomotionGymConfig.ignored_sensor_list = ['
+ '"Collisions"]\n')
+ gin.parse_config(config_text)
+
+ cfg = locomotion_gym_config.LocomotionGymConfig()
+ self.assertEqual(cfg.simulation_parameters.sim_time_step_s, 0.005)
+ self.assertEqual(cfg.actions[0].upper_bound, 1.0)
+ self.assertEqual(cfg.ignored_sensor_list, ['Collisions'])
+
+
+if __name__ == '__main__':
+ tf.test.main()
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/locomotion_gym_env.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/locomotion_gym_env.py
new file mode 100644
index 000000000..0c30e5b8e
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/locomotion_gym_env.py
@@ -0,0 +1,731 @@
+# Lint as: python3
+"""This file implements the locomotion gym env."""
+
+import atexit
+import collections
+import time
+from typing import Any, Callable, Sequence, Text, Union
+import gin
+import gym
+import numpy as np
+
+from pybullet_envs.minitaur.envs_v2 import base_client
+from pybullet_utils import bullet_client
+import pybullet_data
+import pybullet
+from pybullet_envs.minitaur.envs import minitaur_logging
+from pybullet_envs.minitaur.envs import minitaur_logging_pb2
+#from pybullet_envs.minitaur.envs import minitaur_logging
+#from pybullet_envs.minitaur.envs import minitaur_logging_pb2
+from pybullet_envs.minitaur.envs_v2.evaluation import metric_logger
+from pybullet_envs.minitaur.envs_v2.scenes import scene_base
+from pybullet_envs.minitaur.envs_v2.sensors import sensor
+from pybullet_envs.minitaur.envs_v2.sensors import space_utils
+from pybullet_envs.minitaur.envs_v2.utilities import rendering_utils
+from pybullet_envs.minitaur.robots import autonomous_object
+from pybullet_envs.minitaur.robots import robot_base
+
+_ACTION_EPS = 0.01
+_NUM_SIMULATION_ITERATION_STEPS = 300
+_LOG_BUFFER_LENGTH = 5000
+
+SIM_CLOCK = 'SIM_CLOCK'
+
+# Exports this symbol so we can use it in the config file.
+gin.constant('locomotion_gym_env.SIM_CLOCK', SIM_CLOCK)
+
+# This allows us to bind @time.time in the gin configuration.
+gin.external_configurable(time.time, module='time')
+
+
+# TODO(b/122048194): Enable position/torque/hybrid control mode.
+@gin.configurable
+class LocomotionGymEnv(gym.Env):
+ """The gym environment for the locomotion tasks."""
+ metadata = {
+ 'render.modes': ['human', 'rgb_array', 'topdown'],
+ 'video.frames_per_second': 100
+ }
+
+ def __init__(self,
+ gym_config,
+ clock: Union[Callable[..., float], Text] = 'SIM_CLOCK',
+ robot_class: Any = None,
+ scene: scene_base.SceneBase = None,
+ sensors: Sequence[sensor.Sensor] = None,
+ task: Any = None,
+ env_randomizers: Any = None):
+ """Initializes the locomotion gym environment.
+
+ Args:
+ gym_config: An instance of LocomotionGymConfig.
+ clock: The clock source to be used for the gym env. The clock should
+ return a timestamp in seconds. Setting clock == "SIM_CLOCK" will enable
+ the built-in simulation clock. For real robot experiments, we can use
+ time.time or other clock wall clock sources.
+ robot_class: A class of a robot. We provide a class rather than an
+ instance due to hard_reset functionality. Parameters are expected to be
+ configured with gin.
+ scene: An object for managing the robot's surroundings.
+ sensors: A list of environmental sensors for observation.
+ task: A callable function/class to calculate the reward and termination
+ condition. Takes the gym env as the argument when calling.
+ env_randomizers: A list of EnvRandomizer(s). An EnvRandomizer may
+ randomize the physical property of minitaur, change the terrrain during
+ reset(), or add perturbation forces during step().
+ client_factory: A function to create a simulation client, it can be a
+ pybullet client.
+
+ Raises:
+ ValueError: If the num_action_repeat is less than 1.
+
+ """
+ self._pybullet_client = None
+ self._metric_logger = metric_logger.MetricLogger()
+ # TODO(sehoonha) split observation and full-state sensors (b/129858214)
+
+ # Makes sure that close() is always called to flush out the logs to the
+ # disk.
+ atexit.register(self.close)
+ self.seed()
+ self._gym_config = gym_config
+ if robot_class is None:
+ raise ValueError('robot_class cannot be None.')
+ self._robot_class = robot_class
+ if issubclass(self._robot_class, robot_base.RobotBase):
+ self._use_new_robot_class = True
+ else:
+ self._use_new_robot_class = False
+ self._robot = None
+
+ self._scene = scene or scene_base.SceneBase()
+
+ # TODO(sehoonha) change the data structure to dictionary
+ self._env_sensors = list(sensors) if sensors is not None else list()
+
+ # TODO(b/152161457): Make logging a standalone module.
+ self._log_path = gym_config.log_path
+ self._logging = minitaur_logging.MinitaurLogging(self._log_path)
+ self._episode_proto = minitaur_logging_pb2.MinitaurEpisode()
+ self._data_dir = gym_config.data_dir
+
+ self._task = task
+
+ self._env_randomizers = env_randomizers if env_randomizers else []
+
+ # Simulation related parameters.
+ self._num_action_repeat = gym_config.simulation_parameters.num_action_repeat
+ self._on_rack = gym_config.simulation_parameters.robot_on_rack
+ if self._num_action_repeat < 1:
+ raise ValueError('number of action repeats should be at least 1.')
+ # TODO(b/73829334): Fix the value of self._num_bullet_solver_iterations.
+ self._num_bullet_solver_iterations = int(_NUM_SIMULATION_ITERATION_STEPS /
+ self._num_action_repeat)
+
+ self._sim_time_step = gym_config.simulation_parameters.sim_time_step_s
+ # The sim step counter is an internal varialbe to count the number of
+ # pybullet stepSimulation() has been called since last reset.
+ self._sim_step_counter = 0
+
+ self._env_time_step = self._num_action_repeat * self._sim_time_step
+ # The env step counter accounts for how many times env.step has been
+ # called since reset.
+ self._env_step_counter = 0
+
+ if clock == SIM_CLOCK:
+ self._clock = self._get_sim_time
+ else:
+ self._clock = clock
+
+ # Creates the bullet client.
+ self._is_render = gym_config.simulation_parameters.enable_rendering
+ # The wall-clock time at which the last frame is rendered.
+ self._last_frame_time = 0.0
+
+ if gym_config.simulation_parameters.enable_rendering:
+ self._pybullet_client = bullet_client.BulletClient(connection_mode=pybullet.GUI)
+ self._pybullet_client.configureDebugVisualizer(
+ pybullet.COV_ENABLE_GUI,
+ gym_config.simulation_parameters.enable_rendering_gui)
+ else:
+ self._pybullet_client = bullet_client.BulletClient()
+ if gym_config.simulation_parameters.egl_rendering:
+ self._pybullet_client.loadPlugin('eglRendererPlugin')
+
+ self._pybullet_client.setAdditionalSearchPath(
+ pybullet_data.getDataPath())
+
+ # If enabled, save the performance profile to profiling_path
+ # Use Google Chrome about://tracing to open the file
+ if gym_config.profiling_path is not None:
+ self._profiling_slot = self._pybullet_client.startStateLogging(
+ self._pybullet_client.STATE_LOGGING_PROFILE_TIMINGS,
+ gym_config.profiling_path)
+ self._profiling_counter = 10
+ else:
+ self._profiling_slot = -1
+
+ # Set the default render options. TODO(b/152161124): Make rendering a
+ # standalone module.
+ self._camera_target = gym_config.simulation_parameters.camera_target
+ self._camera_dist = gym_config.simulation_parameters.camera_distance
+ self._camera_yaw = gym_config.simulation_parameters.camera_yaw
+ self._camera_pitch = gym_config.simulation_parameters.camera_pitch
+ self._render_width = gym_config.simulation_parameters.render_width
+ self._render_height = gym_config.simulation_parameters.render_height
+
+ # Loads the environment and robot. Actions space will be created as well.
+ self._hard_reset = True
+ self._observation_dict = {}
+ self.reset()
+ self._hard_reset = gym_config.simulation_parameters.enable_hard_reset
+
+ # Construct the observation space from the list of sensors.
+ self.observation_space = (
+ space_utils.convert_sensors_to_gym_space_dictionary([
+ sensor for sensor in self.all_sensors()
+ if sensor.get_name() not in self._gym_config.ignored_sensor_list
+ ]))
+
+ def __del__(self):
+ self.close()
+
+ def _load_old_robot_class(self):
+ self._robot = self._robot_class(
+ pybullet_client=self._pybullet_client, on_rack=self._on_rack)
+ self._action_list = []
+ action_upper_bound = []
+ action_lower_bound = []
+ for action in self._gym_config.actions:
+ self._action_list.append(action.name)
+ action_upper_bound.append(action.upper_bound)
+ action_lower_bound.append(action.lower_bound)
+ self.action_space = gym.spaces.Box(
+ np.array(action_lower_bound),
+ np.array(action_upper_bound),
+ dtype=np.float32)
+
+ def _load_new_robot_class(self):
+ self._robot = self._robot_class(
+ pybullet_client=self._pybullet_client, clock=self._clock)
+ self.action_space = self._robot.action_space
+
+ def _load(self):
+ self._pybullet_client.resetSimulation()
+ self._pybullet_client.setPhysicsEngineParameter(
+ numSolverIterations=self._num_bullet_solver_iterations)
+ self._pybullet_client.setTimeStep(self._sim_time_step)
+ self._pybullet_client.setGravity(0, 0, -10)
+ self._pybullet_client.setPhysicsEngineParameter(enableConeFriction=0)
+
+ # Disable rendering during scene loading will speed up simulation.
+ if self._is_render:
+ self._pybullet_client.configureDebugVisualizer(
+ self._pybullet_client.COV_ENABLE_RENDERING, 0)
+
+ # Rebuild the scene.
+ self._scene.build_scene(self._pybullet_client)
+
+ # TODO(b/151975607): Deprecate old robot support.
+ if self._use_new_robot_class:
+ self._load_new_robot_class()
+ else:
+ self._load_old_robot_class()
+
+ # Check action space.
+ if (isinstance(self.action_space, gym.spaces.Box) and
+ not np.all(self.action_space.low < self.action_space.high)):
+ raise ValueError(f'Action space contains invalid dimensions, '
+ f'action space low = {self.action_space.low}, '
+ f'action space high = {self.action_space.high}')
+
+ for an_object in self._dynamic_objects():
+ an_object.set_clock(self._clock)
+
+ # Enable rendering after loading finishes.
+ if self._is_render:
+ self._pybullet_client.configureDebugVisualizer(
+ self._pybullet_client.COV_ENABLE_RENDERING, 1)
+
+ def close(self):
+ atexit.unregister(self.close)
+ #if self._pybullet_client:
+
+ if self._log_path is not None:
+ self._logging.save_episode(self._episode_proto)
+ for sensor_ in self.all_sensors():
+ sensor_.on_terminate(self)
+ if self._robot:
+ if self._use_new_robot_class:
+ self._robot.terminate()
+ else:
+ self._robot.Terminate()
+ if self._pybullet_client:
+ self._pybullet_client.disconnect()
+ self._pybullet_client = None
+
+ def seed(self, seed=None):
+ self.np_random, self.np_random_seed = gym.utils.seeding.np_random(seed)
+ return [self.np_random_seed]
+
+ def _dynamic_objects(self):
+ """Returns the python objects controlling moving obstacles."""
+ if self._scene:
+ return self._scene.dynamic_objects
+ else:
+ return []
+
+ def all_sensors(self):
+ """Returns all robot, environmental and dynamic objects sensors."""
+ if self._use_new_robot_class:
+ all_sensors = list(self._env_sensors)
+ if self._robot:
+ all_sensors.extend(list(self._robot.sensors))
+ for obj in self._dynamic_objects():
+ all_sensors.extend(obj.sensors)
+
+ # The new way of adding task specific sensors to the sensor lists.
+ if hasattr(self._task, 'sensors'):
+ all_sensors.extend(self._task.sensors)
+ return all_sensors
+ else:
+ # This is a workaround due to the issue in b/130128505#comment5
+ task_sensor = ([self._task]
+ if isinstance(self._task, sensor.Sensor) else [])
+ robot_sensors = []
+ if self._robot:
+ robot_sensors = self._robot.GetAllSensors()
+ return robot_sensors + self._env_sensors + task_sensor
+
+ def sensor_by_name(self, name):
+ """Returns the sensor with the given name, or None if not exist."""
+ # TODO(b/154162104): Store sensors as dictionary.
+ for sensor_ in self.all_sensors():
+ if sensor_.get_name() == name:
+ return sensor_
+ return None
+
+ @gin.configurable('locomotion_gym_env.LocomotionGymEnv.reset')
+ def reset(
+ self,
+ initial_motor_angles=None,
+ reset_duration=1.0,
+ reset_visualization_camera=True,
+ ):
+ """Resets the robot's position in the world or rebuild the sim world.
+
+ The simulation world will be rebuilt if self._hard_reset is True.
+
+ Args:
+ initial_motor_angles: A list of Floats. The desired joint angles after
+ reset. If None, the robot will use its built-in value.
+ reset_duration: Float. The time (in seconds) needed to rotate all motors
+ to the desired initial values.
+ reset_visualization_camera: Whether to reset debug visualization camera on
+ reset.
+
+ Returns:
+ A numpy array contains the initial observation after reset.
+ """
+
+
+ self._env_step_counter = 0
+ self._sim_step_counter = 0
+ self._last_reset_time = self._clock()
+ self._metric_logger.reset_episode()
+
+ # Clear the simulation world and rebuild the robot interface.
+ if self._hard_reset:
+ self._load()
+
+ # Resets the scene
+ self._scene.reset()
+
+ # Resets the robot with the provided init parameters.
+ if self._use_new_robot_class:
+ self._robot.reset()
+ else:
+ self._robot.Reset(
+ reload_urdf=False,
+ default_motor_angles=initial_motor_angles,
+ reset_time=reset_duration)
+
+ # Flush the logs to disc and reinitialize the logging system.
+ if self._log_path is not None:
+ self._logging.save_episode(self._episode_proto)
+ self._episode_proto = minitaur_logging_pb2.MinitaurEpisode()
+ minitaur_logging.preallocate_episode_proto(self._episode_proto,
+ _LOG_BUFFER_LENGTH,
+ self._robot)
+
+ # TODO(b/152161124): Move this part to the renderer module.
+ if reset_visualization_camera:
+ self._pybullet_client.resetDebugVisualizerCamera(self._camera_dist,
+ self._camera_yaw,
+ self._camera_pitch,
+ [0, 0, 0])
+
+ # Create an example last action based on the type of action space.
+ self._last_action = space_utils.create_constant_action(self.action_space)
+
+ for s in self.all_sensors():
+ s.on_reset(self)
+
+ if self._task and hasattr(self._task, 'reset'):
+ self._task.reset(self)
+
+ # Loop over all env randomizers.
+ for env_randomizer in self._env_randomizers:
+ env_randomizer.randomize_env(self)
+
+ for obj in self._dynamic_objects():
+ obj.reset()
+
+ # Initialize the robot base position.
+ if self._use_new_robot_class:
+ self._last_base_position = self._robot.base_position
+ else:
+ self._last_base_position = self._robot.GetBasePosition()
+
+ # Resets the observations again, since randomizers might change the env.
+ for s in self.all_sensors():
+ s.on_reset(self)
+
+
+ self._last_reset_time = self._clock()
+ return self._get_observation()
+
+ def _wait_for_rendering(self):
+ # Sleep, otherwise the computation takes less time than real time,
+ # which will make the visualization like a fast-forward video.
+ time_spent = time.time() - self._last_frame_time
+ self._last_frame_time = time.time()
+ time_to_sleep = self._env_time_step - time_spent
+ if time_to_sleep > 0:
+ time.sleep(time_to_sleep)
+
+ # Also keep the previous orientation of the camera set by the user.
+ [yaw, pitch, dist] = self._pybullet_client.getDebugVisualizerCamera()[8:11]
+ self._pybullet_client.resetDebugVisualizerCamera(dist, yaw, pitch,
+ self._last_base_position)
+
+ def _step_old_robot_class(self, action):
+ self._last_base_position = self._robot.GetBasePosition()
+ self._last_action = action
+
+ if self._is_render:
+ self._wait_for_rendering()
+
+ for env_randomizer in self._env_randomizers:
+ env_randomizer.randomize_step(self)
+
+ self._robot.Step(action)
+
+ if self._profiling_slot >= 0:
+ self._profiling_counter -= 1
+ if self._profiling_counter == 0:
+ self._pybullet_client.stopStateLogging(self._profiling_slot)
+
+ if self._log_path is not None:
+ minitaur_logging.update_episode_proto(self._episode_proto, self._robot,
+ action, self._env_step_counter)
+ reward = self._reward()
+
+ for s in self.all_sensors():
+ s.on_step(self)
+
+ if self._task and hasattr(self._task, 'update'):
+ self._task.update(self) # TODO(b/154635313): resolve API mismatch
+
+ done = self._termination()
+ self._env_step_counter += 1
+ # TODO(b/161941829): terminate removed for now, change terminate to other
+ # names.
+ return self._get_observation(), reward, done, {}
+
+ def _step_new_robot_class(self, action):
+ self._last_base_position = self._robot.base_position
+ self._last_action = action
+
+ if self._is_render:
+ self._wait_for_rendering()
+
+ for env_randomizer in self._env_randomizers:
+ env_randomizer.randomize_step(self)
+
+ action = self._robot.pre_control_step(action, self._env_time_step)
+ for obj in self._dynamic_objects():
+ obj.pre_control_step(autonomous_object.AUTONOMOUS_ACTION)
+ for _ in range(self._num_action_repeat):
+ self._robot.apply_action(action)
+ for obj in self._dynamic_objects():
+ obj.update(self.get_time_since_reset(), self._observation_dict)
+ obj.apply_action(autonomous_object.AUTONOMOUS_ACTION)
+
+ self._pybullet_client.stepSimulation()
+ self._sim_step_counter += 1
+
+ self._robot.receive_observation()
+ for obj in self._dynamic_objects():
+ obj.receive_observation()
+
+ for s in self.all_sensors():
+ s.on_new_observation()
+
+ self._robot.post_control_step()
+ for obj in self._dynamic_objects():
+ obj.post_control_step()
+
+ if self._profiling_slot >= 0:
+ self._profiling_counter -= 1
+ if self._profiling_counter == 0:
+ self._pybullet_client.stopStateLogging(self._profiling_slot)
+
+ if self._log_path is not None:
+ minitaur_logging.update_episode_proto(self._episode_proto, self._robot,
+ action, self._env_step_counter)
+ reward = self._reward()
+
+ for s in self.all_sensors():
+ s.on_step(self)
+
+ if self._task and hasattr(self._task, 'update'):
+ self._task.update(self) # TODO(b/154635313): resolve API mismatch
+
+ done = self._termination()
+ self._env_step_counter += 1
+ # TODO(b/161941829): terminate removed for now, change terminate to other
+ # names.
+ return self._get_observation(), reward, done, {}
+
+ def step(self, action):
+ """Step forward the simulation, given the action.
+
+ Args:
+ action: Can be a list of desired motor angles for all motors when the
+ robot is in position control mode; A list of desired motor torques. Or a
+ list of tuples (q, qdot, kp, kd, tau) for hybrid control mode. The
+ action must be compatible with the robot's motor control mode. Also, we
+ are not going to use the leg space (swing/extension) definition at the
+ gym level, since they are specific to Minitaur.
+
+ Returns:
+ observations: The observation dictionary. The keys are the sensor names
+ and the values are the sensor readings.
+ reward: The reward for the current state-action pair.
+ done: Whether the episode has ended.
+ info: A dictionary that stores diagnostic information.
+
+ Raises:
+ ValueError: The action dimension is not the same as the number of motors.
+ ValueError: The magnitude of actions is out of bounds.
+ """
+ # TODO(b/151975607): Finish the migration and remove old robot class
+ # support.
+ if self._use_new_robot_class:
+ return self._step_new_robot_class(action)
+ else:
+ return self._step_old_robot_class(action)
+
+ @gin.configurable('locomotion_gym_env.LocomotionGymEnv.render')
+ def render(self, mode='rgb_array'):
+
+ if mode == 'topdown':
+ # Provide ground height if we know it. Otherwise leave it as gin
+ # configurable.
+ if hasattr(self.scene, 'ground_height'):
+ return rendering_utils.render_topdown(
+ self._pybullet_client, ground_height=self.scene.ground_height)
+ else:
+ return rendering_utils.render_topdown(self._pybullet_client)
+
+ if mode != 'rgb_array':
+ raise ValueError('Unsupported render mode:{}'.format(mode))
+
+ if self._camera_target is not None:
+ target_position = self._camera_target
+ else:
+ target_position = self._last_base_position
+ view_matrix = self._pybullet_client.computeViewMatrixFromYawPitchRoll(
+ cameraTargetPosition=target_position,
+ distance=self._camera_dist,
+ yaw=self._camera_yaw,
+ pitch=self._camera_pitch,
+ roll=0,
+ upAxisIndex=2)
+ proj_matrix = self._pybullet_client.computeProjectionMatrixFOV(
+ fov=60,
+ aspect=float(self._render_width) / self._render_height,
+ nearVal=0.1,
+ farVal=100.0)
+ return rendering_utils.render_image(self._pybullet_client,
+ self._render_width, self._render_height,
+ view_matrix, proj_matrix)
+
+ @property
+ def scene(self):
+ return self._scene
+
+ @property
+ def rendering_enabled(self):
+ return self._is_render
+
+ @property
+ def env_randomizers(self):
+ return self._env_randomizers
+
+ @property
+ def last_base_position(self):
+ return self._last_base_position
+
+ @property
+ def gym_config(self):
+ return self._gym_config
+
+ def _termination(self):
+ if not self._robot.is_safe:
+ return True
+
+ if self._task and hasattr(self._task, 'done'):
+ return self._task.done(self) # TODO(b/154635313): resolve API mismatch
+
+ return False
+
+ def _reward(self):
+ if self._task:
+ return self._task.reward(self) # TODO(b/154635313): resolve API mismatch
+ return 0
+
+ def _get_observation(self):
+ """Get observation of this environment from a list of sensors.
+
+ Returns:
+ observations: dictionary of sensory observation with sensor name as key
+ and corresponding observation in numpy array as value.
+ """
+ sensors_dict = {}
+ for s in self.all_sensors():
+ if s.get_name() in self._gym_config.ignored_sensor_list:
+ continue
+
+ obs = s.get_observation()
+ if isinstance(obs, dict):
+ sensors_dict.update(obs)
+ else:
+ sensors_dict[s.get_name()] = obs
+
+ self._observation_dict = collections.OrderedDict(
+ sorted(sensors_dict.items()))
+ return self._observation_dict
+
+ def set_time_step(self, num_action_repeat, sim_step=0.001):
+ """Sets the time step of the environment.
+
+ Args:
+ num_action_repeat: The number of simulation steps/action repeats to be
+ executed when calling env.step().
+ sim_step: The simulation time step in PyBullet. By default, the simulation
+ step is 0.001s, which is a good trade-off between simulation speed and
+ accuracy.
+
+ Raises:
+ ValueError: If the num_action_repeat is less than 1.
+ """
+ if num_action_repeat < 1:
+ raise ValueError('number of action repeats should be at least 1.')
+ self._sim_time_step = sim_step
+ self._num_action_repeat = num_action_repeat
+ self._env_time_step = sim_step * num_action_repeat
+ self._num_bullet_solver_iterations = int(
+ _NUM_SIMULATION_ITERATION_STEPS / self._num_action_repeat)
+ self._pybullet_client.setPhysicsEngineParameter(
+ numSolverIterations=self._num_bullet_solver_iterations)
+ self._pybullet_client.setTimeStep(self._sim_time_step)
+ if not self._use_new_robot_class:
+ self._robot.SetTimeSteps(self._num_action_repeat, self._sim_time_step)
+
+ def _get_sim_time(self):
+ """Returns the simulation time since the sim resets."""
+ return self._sim_step_counter * self._sim_time_step
+
+ def get_time_since_reset(self):
+ """Get the time passed (in seconds) since the last reset.
+
+ Returns:
+ Time in seconds since the last reset.
+ """
+ if self._use_new_robot_class:
+ return self._clock() - self._last_reset_time
+ else:
+ return self._robot.GetTimeSinceReset()
+
+ def get_time(self):
+ """Gets the time reading from the clock source."""
+ return self._clock()
+
+ @property
+ def observation(self):
+ return self._observation_dict
+
+ @property
+ def pybullet_client(self):
+ return self._pybullet_client
+
+ @property
+ def robot(self):
+ return self._robot
+
+ @property
+ def num_action_repeat(self):
+ return self._num_action_repeat
+
+ @property
+ def sim_time_step(self):
+ return self._sim_time_step
+
+ @property
+ def env_step_counter(self):
+ return self._env_step_counter
+
+ @property
+ def hard_reset(self):
+ return self._hard_reset
+
+ @property
+ def last_action(self):
+ return self._last_action
+
+ @property
+ def env_time_step(self):
+ return self._env_time_step
+
+ @property
+ def data_dir(self):
+ return self._data_dir
+
+ @property
+ def task(self):
+ return self._task
+
+ @property
+ def robot_class(self):
+ return self._robot_class
+
+ @property
+ def action_names(self):
+ """Name of each action in the action space.
+
+ By default this returns the actions the robot executes (e.g.
+ "VELOCITY_elbow_joint"), but env wrappers may override this if they change
+ the action space (e.g. if they convert twist to motor commands).
+
+ Returns:
+ Tuple of strings, the action names.
+ """
+ if self._use_new_robot_class:
+ return self._robot.action_names
+ return self._action_list
+
+ @property
+ def metric_logger(self):
+ return self._metric_logger
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
new file mode 100644
index 000000000..51f3c9ae6
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/locomotion_gym_env_test.py
@@ -0,0 +1,241 @@
+# Lint as: python3
+r"""Tests for locomotion_gym_env.
+
+
+"""
+
+import math
+import random
+
+import gin
+import mock
+import numpy as np
+import tensorflow.compat.v1 as tf
+from absl.testing import parameterized
+
+from pybullet_envs.minitaur.envs_v2 import locomotion_gym_env
+from pybullet_envs.minitaur.envs_v2.evaluation import metric as metric_lib
+from pybullet_envs.minitaur.envs_v2.scenes import scene_base
+from pybullet_envs.minitaur.envs_v2.scenes import simple_scene
+from pybullet_envs.minitaur.envs_v2.tasks import task_interface
+from pybullet_envs.minitaur.envs_v2.utilities import env_utils
+from pybullet_envs.minitaur.robots import autonomous_object
+from pybullet_envs.minitaur.robots import minitaur_v2
+import pybullet_data as pd
+
+import unittest
+
+
+_POSITION_GAIN = 1.0
+_VELOCITY_GAIN = 0.015
+_CONTROL_LATENCY = 0.015
+_CONFIG_FILE = (pd.getDataPath()+'/configs/minitaur_gym_env.gin')
+_CONFIG_FILE_NEW_ROBOT = (pd.getDataPath()+'/configs_v2/base/laikago_reactive.gin')
+
+
+class TestTask(task_interface.Task):
+ """A step counter task for test purpose."""
+
+ def __init__(self):
+ self._counter = 0
+
+ def reset(self, env):
+ del env
+ self._counter = 0
+
+ def reward(self, env):
+ del env # TODO(b/154635313): resolve the API mismatch
+ self._counter += 1
+ return self._counter
+
+ def update(self, env):
+ del env # TODO(b/154635313): resolve the API mismatch
+
+ def done(self, env):
+ del env # TODO(b/154635313): resolve the API mismatch
+ return False
+
+
+class LocomotionGymEnvTest(tf.test.TestCase, parameterized.TestCase):
+
+ def setUp(self):
+ super().setUp()
+ gin.clear_config()
+
+ def test_env_from_gin(self):
+ # TODO(sehoonha) rename locomotion_gym_*test.gin to locomotion_gym_*.gin
+ gin.parse_config_file(_CONFIG_FILE)
+ 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)
+
+ def test_reset_gym(self):
+ gin.parse_config_file(_CONFIG_FILE)
+ env = locomotion_gym_env.LocomotionGymEnv(task=None)
+
+ desired_init_motor_angle = math.pi / 2
+ action_dim = len(env.action_space.high)
+ observations = env.reset(initial_motor_angles=[desired_init_motor_angle] *
+ 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)
+
+ def test_step_gym(self):
+ gin.parse_config_file(_CONFIG_FILE)
+ env = locomotion_gym_env.LocomotionGymEnv(task=TestTask())
+
+ desired_motor_angle = math.pi / 3
+ steps = 1000
+ action_dim = len(env.action_space.high)
+ for _ in range(steps):
+ observations, reward, done, _ = env.step([desired_motor_angle] *
+ action_dim)
+ observations = env_utils.flatten_observations(observations)
+
+ 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)
+ np.testing.assert_allclose(env._last_action,
+ [desired_motor_angle] * action_dim, 2e-1)
+
+ def test_scene(self):
+ gin.parse_config_file(_CONFIG_FILE)
+ data_root = 'urdf/'
+ env = locomotion_gym_env.LocomotionGymEnv(
+ task=TestTask(), scene=simple_scene.SimpleScene(data_root=data_root))
+ desired_motor_angle = math.pi / 3
+ steps = 2
+ action_dim = len(env.action_space.high)
+ for _ in range(steps):
+ _, reward, _, _ = env.step([desired_motor_angle] * action_dim)
+ self.assertEqual(reward, steps)
+
+ def test_except_on_invalid_config(self):
+ gin.parse_config_file(_CONFIG_FILE)
+ gin.bind_parameter('SimulationParameters.num_action_repeat', 0)
+ with self.assertRaises(ValueError):
+ locomotion_gym_env.LocomotionGymEnv(task=None)
+
+ def test_no_scene(self):
+ gin.parse_config_file(_CONFIG_FILE)
+ 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)
+
+ def test_seed_draw_with_np(self):
+ gin.parse_config_file(_CONFIG_FILE)
+ env = locomotion_gym_env.LocomotionGymEnv(task=None)
+ # first draw
+ env.seed(42)
+ nums1 = []
+ for _ in range(3):
+ nums1.append(env.np_random.randint(2**31 - 1))
+ # second draw
+ env.seed(42)
+ nums2 = []
+ for _ in range(3):
+ nums2.append(env.np_random.randint(2**31 - 1))
+ self.assertListEqual(nums1, nums2)
+
+ def test_get_observations(self):
+ gin.parse_config_file(_CONFIG_FILE)
+ env = locomotion_gym_env.LocomotionGymEnv(task=None)
+ desired_init_motor_angle = math.pi / 2
+ 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)
+
+
+
+
+
+ def test_step_with_dynamic_objects(self):
+ gin.parse_config_file(_CONFIG_FILE_NEW_ROBOT)
+
+ gin.parse_config([
+ 'AutonomousObject.urdf_file = "urdf/mug.urdf"',
+ 'SceneBase.dynamic_objects = [@AutonomousObject(), @AutonomousObject()]',
+ 'LocomotionGymEnv.scene = @SceneBase()',
+ ])
+ env = locomotion_gym_env.LocomotionGymEnv()
+
+ self.assertLen(env.scene.dynamic_objects, 2)
+ for obj in env.scene.dynamic_objects:
+ self.assertIsInstance(obj, autonomous_object.AutonomousObject)
+
+ # Replace dynamic objects with mocks for step tests.
+ mock_objects = [
+ mock.create_autospec(autonomous_object.AutonomousObject),
+ mock.create_autospec(autonomous_object.AutonomousObject)
+ ]
+ env.scene._type_to_objects_dict[
+ scene_base.ObjectType.DYNAMIC_OBJECT] = mock_objects
+ env.step(env.robot.action_space.sample())
+
+ expected_update_calls = [
+ mock.call(i * env.sim_time_step, mock.ANY)
+ for i in range(env.num_action_repeat)
+ ]
+ expected_apply_action_calls = [
+ mock.call(autonomous_object.AUTONOMOUS_ACTION)
+ for i in range(env.num_action_repeat)
+ ]
+ expected_receive_observation_calls = [
+ mock.call() for i in range(env.num_action_repeat)
+ ]
+
+ for mock_obj in mock_objects:
+ mock_obj.pre_control_step.assert_called_once_with(
+ autonomous_object.AUTONOMOUS_ACTION)
+ self.assertEqual(mock_obj.update.call_args_list, expected_update_calls)
+ self.assertEqual(mock_obj.apply_action.call_args_list,
+ expected_apply_action_calls)
+ self.assertEqual(mock_obj.receive_observation.call_args_list,
+ expected_receive_observation_calls)
+ mock_obj.post_control_step.assert_called_once_with()
+
+
+ def test_env_metrics(self):
+ gin.parse_config_file(_CONFIG_FILE_NEW_ROBOT)
+ env = locomotion_gym_env.LocomotionGymEnv()
+ metric_logger = env.metric_logger
+ test_metric_1 = metric_logger.create_scalar_metric(
+ 'test_metric_1',
+ scope=metric_lib.MetricScope.DEBUG,
+ single_ep_aggregator=np.mean)
+ test_metric_1.report(22)
+
+ test_metric_2 = metric_logger.create_scalar_metric(
+ 'test_metric_2',
+ scope=metric_lib.MetricScope.PERFORMANCE,
+ single_ep_aggregator=np.max)
+ test_metric_2.report(15)
+ test_metric_2.report(16)
+
+ all_metrics = metric_logger.get_episode_metrics()
+
+ self.assertDictEqual(all_metrics, {
+ 'DEBUG/test_metric_1': 22,
+ 'PERFORMANCE/test_metric_2': 16
+ })
+
+ env.reset()
+
+ all_metrics = metric_logger.get_episode_metrics()
+ self.assertDictEqual(all_metrics, {})
+
+
+if __name__ == '__main__':
+ tf.test.main()
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/multiagent_mobility_gym_env.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/multiagent_mobility_gym_env.py
new file mode 100644
index 000000000..c716d367b
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/multiagent_mobility_gym_env.py
@@ -0,0 +1,485 @@
+"""This file implements the locomotion gym env."""
+# pylint: disable=dangerous-default-value
+
+import atexit
+import collections
+import time
+
+import gin
+from gym import spaces
+import numpy as np
+
+from pybullet_utils import bullet_client
+from pybullet_envs.minitaur.envs import minitaur_logging
+from pybullet_envs.minitaur.envs import minitaur_logging_pb2
+from pybullet_envs.minitaur.envs_v2 import locomotion_gym_env
+from pybullet_envs.minitaur.envs_v2.scenes import scene_base
+from pybullet_envs.minitaur.envs_v2.sensors import sensor
+from pybullet_envs.minitaur.envs_v2.sensors import space_utils
+import pybullet
+
+_ACTION_EPS = 0.01
+_NUM_SIMULATION_ITERATION_STEPS = 300
+_LOG_BUFFER_LENGTH = 5000
+
+
+@gin.configurable
+class MultiagentMobilityGymEnv(locomotion_gym_env.LocomotionGymEnv):
+ """The gym environment for the locomotion tasks."""
+ metadata = {
+ 'render.modes': ['human', 'rgb_array'],
+ 'video.frames_per_second': 100
+ }
+
+ def __init__(self,
+ gym_config,
+ robot_classes,
+ scene: scene_base.SceneBase = scene_base.SceneBase(),
+ sensors=None,
+ tasks=[],
+ global_task=None,
+ single_reward=False,
+ env_randomizers=None):
+ """Initializes the locomotion gym environment.
+
+ Args:
+ gym_config: An instance of LocomotionGymConfig.
+ robot_classes: A list of robot classes. We provide a class rather than an
+ instance due to hard_reset functionality. Parameters are expected to be
+ configured with gin.
+ scene: An object for managing the robot's surroundings.
+ sensors: A list of environmental sensors for observation. This does not
+ include on-robot sensors.
+ tasks: A list of callable function/class to calculate the reward and
+ termination condition. Takes the gym env as the argument when calling.
+ global_task: A callable function/class to calculate the reward and
+ termination condition for all robots. Takes the gym env as the argument
+ when calling.
+ single_reward: Whether the environment returns a single reward for all
+ agents or a dictionary.
+ env_randomizers: A list of EnvRandomizer(s). An EnvRandomizer may
+ randomize the physical property of minitaur, change the terrrain during
+ reset(), or add perturbation forces during step().
+
+ Raises:
+ ValueError: If the num_action_repeat is less than 1, or if number of
+ unique robot names do not match the number of robot classes.
+
+ """
+ # TODO(sehoonha) split observation and full-state sensors (b/129858214)
+
+ # Makes sure that close() is always called to flush out the logs to the
+ # disk.
+ atexit.register(self.close)
+ self.seed()
+ self._gym_config = gym_config
+ self._robot_classes = robot_classes
+ # Checking uniqueness of names and number of names
+ self._scene = scene
+ # TODO(sehoonha) change the data structure to dictionary
+ # TODO(b/144521291) make sure sensors have their own robot names
+ self._sensors = sensors if sensors is not None else list()
+ self._log_path = gym_config.log_path
+ self._logging = minitaur_logging.MinitaurLogging(self._log_path)
+ self._episode_proto = minitaur_logging_pb2.MinitaurEpisode()
+ self._data_dir = gym_config.data_dir
+
+ # A dictionary containing the objects in the world other than the robot.
+ self._tasks = tasks
+ self._global_task = global_task
+ self._single_reward = single_reward
+
+ self._env_randomizers = env_randomizers if env_randomizers else []
+
+ # This is a workaround due to the issue in b/130128505#comment5
+ for task in self._tasks:
+ if isinstance(task, sensor.Sensor):
+ self._sensors.append(task)
+ if global_task and isinstance(global_task, sensor.Sensor):
+ self._sensors.append(global_task)
+
+ # Simulation related parameters.
+ self._num_action_repeat = gym_config.simulation_parameters.num_action_repeat
+ self._on_rack = gym_config.simulation_parameters.robot_on_rack
+ if self._num_action_repeat < 1:
+ raise ValueError('number of action repeats should be at least 1.')
+ self._sim_time_step = gym_config.simulation_parameters.sim_time_step_s
+ self._env_time_step = self._num_action_repeat * self._sim_time_step
+ self._env_step_counter = 0
+
+ # TODO(b/73829334): Fix the value of self._num_bullet_solver_iterations.
+ self._num_bullet_solver_iterations = int(_NUM_SIMULATION_ITERATION_STEPS /
+ self._num_action_repeat)
+ self._is_render = gym_config.simulation_parameters.enable_rendering
+
+ # The wall-clock time at which the last frame is rendered.
+ self._last_frame_time = 0.0
+ if self._is_render:
+ self._pybullet_client = bullet_client.BulletClient(
+ connection_mode=pybullet.GUI)
+ else:
+ self._pybullet_client = bullet_client.BulletClient()
+
+ if gym_config.simulation_parameters.egl_rendering:
+ self._pybullet_client.loadPlugin('eglRendererPlugin')
+ self._pybullet_client.enable_cns()
+
+ # If enabled, save the performance profile to profiling_path
+ # Use Google Chrome about://tracing to open the file
+ if gym_config.profiling_path is not None:
+ self._profiling_slot = self._pybullet_client.startStateLogging(
+ self._pybullet_client.STATE_LOGGING_PROFILE_TIMINGS,
+ gym_config.profiling_path)
+ self._profiling_counter = 10
+ else:
+ self._profiling_slot = -1
+ # Build the action space. The action space must be compatible with the
+ # robot configuration.
+
+ # The action list contains the name of all actions.
+ # TODO(b/144479707): Allow robots to set the action space automatically.
+
+ action_space = collections.OrderedDict()
+ for robot_name, action in gym_config.actions.items():
+ action_lower_bound = []
+ action_upper_bound = []
+ for action_scalar in action:
+ action_upper_bound.append(action_scalar.upper_bound)
+ action_lower_bound.append(action_scalar.lower_bound)
+ action_space[robot_name] = spaces.Box(
+ np.asarray(action_lower_bound),
+ np.asarray(action_upper_bound),
+ dtype=np.float32)
+ self.action_space = spaces.Dict(action_space)
+
+ # Set the default render options.
+ self._camera_dist = gym_config.simulation_parameters.camera_distance
+ self._camera_yaw = gym_config.simulation_parameters.camera_yaw
+ self._camera_pitch = gym_config.simulation_parameters.camera_pitch
+ self._render_width = gym_config.simulation_parameters.render_width
+ self._render_height = gym_config.simulation_parameters.render_height
+
+ self._hard_reset = True
+ self.reset()
+
+ self._hard_reset = gym_config.simulation_parameters.enable_hard_reset
+
+ # Construct the observation space from the list of sensors. Note that we
+ # will reconstruct the observation_space after the robot is created.
+ self.observation_space = (
+ space_utils.convert_sensors_to_gym_space_dictionary(self.all_sensors()))
+
+ def close(self):
+ if self._log_path is not None:
+ self._logging.save_episode(self._episode_proto)
+
+ for robot in self._robots:
+ robot.Terminate()
+
+ def all_sensors(self):
+ """Returns all robot and environmental sensors."""
+ robot_sensors = []
+ for robot in self._robots:
+ robot_sensors += robot.GetAllSensors()
+ return robot_sensors + self._sensors
+
+ @gin.configurable('multiagent_mobility_gym_env.MultiagentMobilityGymEnv.reset'
+ )
+ def reset(self,
+ initial_motor_angles=None,
+ reset_duration=1.0,
+ reset_visualization_camera=True):
+ """Resets the robot's position in the world or rebuild the sim world.
+
+ The simulation world will be rebuilt if self._hard_reset is True.
+
+ Args:
+ initial_motor_angles: A list of Floats. The desired joint angles after
+ reset. If None, the robot will use its built-in value.
+ reset_duration: Float. The time (in seconds) needed to rotate all motors
+ to the desired initial values.
+ reset_visualization_camera: Whether to reset debug visualization camera on
+ reset.
+
+ Returns:
+ A numpy array contains the initial observation after reset.
+ """
+ if self._is_render:
+ self._pybullet_client.configureDebugVisualizer(
+ self._pybullet_client.COV_ENABLE_RENDERING, 0)
+
+ # Clear the simulation world and rebuild the robot interface.
+ if self._hard_reset:
+ self._pybullet_client.resetSimulation()
+ self._pybullet_client.setPhysicsEngineParameter(
+ numSolverIterations=self._num_bullet_solver_iterations)
+ self._pybullet_client.setTimeStep(self._sim_time_step)
+ self._pybullet_client.setGravity(0, 0, -10)
+
+ # Rebuild the world.
+ self._scene.build_scene(self._pybullet_client)
+
+ # Rebuild the robots
+ # TODO(b/144545080): Make this scale to more than two agents
+ # Have multiple robot classes as a list.
+ self._robots = []
+ for robot_class in self._robot_classes:
+
+ self._robots.append(
+ robot_class(
+ pybullet_client=self._pybullet_client,
+ # TODO(rosewang): Remove on rack in multiagent acase
+ on_rack=self._on_rack))
+
+ # Reset the pose of the robot.
+ for robot in self._robots:
+ robot.Reset(
+ reload_urdf=False,
+ default_motor_angles=initial_motor_angles,
+ reset_time=reset_duration)
+
+ self._env_step_counter = 0
+ self._pybullet_client.resetDebugVisualizerCamera(self._camera_dist,
+ self._camera_yaw,
+ self._camera_pitch,
+ [0, 0, 0])
+
+ # Flush the logs to disc and reinitialize the logging system.
+ if self._log_path is not None:
+ self._logging.save_episode(self._episode_proto)
+ self._episode_proto = minitaur_logging_pb2.MinitaurEpisode()
+ minitaur_logging.preallocate_episode_proto(self._episode_proto,
+ _LOG_BUFFER_LENGTH,
+ self._robots[0])
+ self._pybullet_client.setPhysicsEngineParameter(enableConeFriction=0)
+ self._env_step_counter = 0
+ if reset_visualization_camera:
+ self._pybullet_client.resetDebugVisualizerCamera(self._camera_dist,
+ self._camera_yaw,
+ self._camera_pitch,
+ [0, 0, 0])
+
+ self._last_action = {
+ robot_name: np.zeros(space.shape)
+ for robot_name, space in self.action_space.spaces.items()
+ }
+
+ if self._is_render:
+ self._pybullet_client.configureDebugVisualizer(
+ self._pybullet_client.COV_ENABLE_RENDERING, 1)
+
+ for s in self.all_sensors():
+ # set name
+ if any([r.name in s.get_name() for r in self.robots]):
+ robot = [r for r in self.robots if r.name in s.get_name()][0]
+ s.set_robot(robot)
+
+ for task in self._tasks:
+ if hasattr(task, 'reset'):
+ task.reset(self)
+ if self._global_task and hasattr(self._global_task, 'reset'):
+ self._global_task.reset(self)
+
+ # Loop over all env randomizers.
+ for env_randomizer in self._env_randomizers:
+ env_randomizer.randomize_env(self)
+
+ for s in self.all_sensors():
+ s.on_reset(self)
+
+ return self._get_observation()
+
+ def get_robot(self, name):
+ for robot in self.robots:
+ if robot.name == name:
+ return robot
+
+ def _reward(self):
+ """Returns a list of rewards.
+
+ Returns:
+ A list of rewards corresponding to each robot and their task.
+ """
+ global_reward = 0
+ if self._global_task:
+ global_reward = self._global_task(self)
+ if self._single_reward: # Needed for tfagents compatibility.
+ if self._tasks:
+ return min([task(self) + global_reward for task in self._tasks])
+ return 0
+ else:
+ if self._tasks:
+ return [task(self) + global_reward for task in self._tasks]
+ return [0 for _ in self.robots]
+
+ def step(self, actions):
+ """Step forward the simulation, given the actions.
+
+ Args:
+ actions: A dictionary of actions for all robots. The action for each robot
+ can be joint angles for legged platforms like Laikago, and base
+ velocity/steering for kinematic robots such like Fetch.
+
+ Returns:
+ observations: The observation dictionary. The keys are the sensor names
+ and the values are the sensor readings.
+ reward: The reward for the current state-action pair.
+ done: Whether the episode has ended.
+ info: A dictionary that stores diagnostic information.
+
+ Raises:
+ ValueError: The action dimension is not the same as the number of motors.
+ ValueError: The magnitude of actions is out of bounds.
+ """
+ self._last_base_position = [
+ robot.GetBasePosition() for robot in self._robots
+ ]
+ self._last_action = actions
+
+ if self._is_render:
+ # Sleep, otherwise the computation takes less time than real time,
+ # which will make the visualization like a fast-forward video.
+ time_spent = time.time() - self._last_frame_time
+ self._last_frame_time = time.time()
+ time_to_sleep = self._env_time_step - time_spent
+ if time_to_sleep > 0:
+ time.sleep(time_to_sleep)
+ camera_target = np.mean(
+ [robot.GetBasePosition() for robot in self._robots], axis=0)
+
+ # Also keep the previous orientation of the camera set by the user.
+ [yaw, pitch,
+ dist] = self._pybullet_client.getDebugVisualizerCamera()[8:11]
+ self._pybullet_client.resetDebugVisualizerCamera(dist, yaw, pitch,
+ camera_target)
+
+ for env_randomizer in self._env_randomizers:
+ env_randomizer.randomize_step(self)
+
+ # Stepping broken down into their parts
+ for robot in self._robots:
+ robot.PreStepPerStep(actions)
+
+ for _ in range(self._num_action_repeat):
+ for robot in self._robots:
+ robot.PreStepPerActionRepeat(actions)
+
+ self._pybullet_client.stepSimulation()
+
+ for robot in self._robots:
+ robot.PostStepPerActionRepeat()
+
+ for robot in self._robots:
+ robot.PostStepPerStep()
+
+ if self._profiling_slot >= 0:
+ self._profiling_counter -= 1
+ if self._profiling_counter == 0:
+ self._pybullet_client.stopStateLogging(self._profiling_slot)
+ if self._log_path is not None:
+ minitaur_logging.update_episode_proto(self._episode_proto,
+ self._robots[0], actions,
+ self._env_step_counter)
+ reward = self._reward()
+
+ for s in self.all_sensors():
+ s.on_step(self)
+
+ for task in self._tasks:
+ if hasattr(task, 'update'):
+ task.update(self)
+ if self._global_task and hasattr(self._global_task, 'update'):
+ self._global_task.update(self)
+
+ done = self._termination()
+ self._env_step_counter += 1
+ if done:
+ for robot in self._robots:
+ robot.Terminate()
+ return self._get_observation(), reward, done, {}
+
+ def render(self, mode='rgb_array'):
+ if mode != 'rgb_array':
+ raise ValueError('Unsupported render mode:{}'.format(mode))
+ base_pos = np.mean([robot.GetBasePosition() for robot in self._robots],
+ axis=0)
+ view_matrix = self._pybullet_client.computeViewMatrixFromYawPitchRoll(
+ cameraTargetPosition=base_pos,
+ distance=self._camera_dist,
+ yaw=self._camera_yaw,
+ pitch=self._camera_pitch,
+ roll=0,
+ upAxisIndex=2)
+ proj_matrix = self._pybullet_client.computeProjectionMatrixFOV(
+ fov=60,
+ aspect=float(self._render_width) / self._render_height,
+ nearVal=0.1,
+ farVal=100.0)
+ (_, _, px, _, _) = self._pybullet_client.getCameraImage(
+ width=self._render_width,
+ height=self._render_height,
+ renderer=self._pybullet_client.ER_BULLET_HARDWARE_OPENGL,
+ viewMatrix=view_matrix,
+ projectionMatrix=proj_matrix)
+ rgb_array = np.array(px)
+ rgb_array = rgb_array[:, :, :3]
+ return rgb_array
+
+ def _termination(self):
+ if not all([robot.is_safe for robot in self._robots]):
+ return True
+
+ if self._tasks:
+ return (self._global_task and self._global_task.done(self)) or any(
+ [task.done(self) for task in self._tasks])
+
+ for s in self.all_sensors():
+ s.on_terminate(self)
+
+ return False
+
+ def set_time_step(self, num_action_repeat, sim_step=0.001):
+ """Sets the time step of the environment.
+
+ Args:
+ num_action_repeat: The number of simulation steps/action repeats to be
+ executed when calling env.step().
+ sim_step: The simulation time step in PyBullet. By default, the simulation
+ step is 0.001s, which is a good trade-off between simulation speed and
+ accuracy.
+
+ Raises:
+ ValueError: If the num_action_repeat is less than 1.
+ """
+ if num_action_repeat < 1:
+ raise ValueError('number of action repeats should be at least 1.')
+ self._sim_time_step = sim_step
+ self._num_action_repeat = num_action_repeat
+ self._env_time_step = sim_step * num_action_repeat
+ self._num_bullet_solver_iterations = (
+ _NUM_SIMULATION_ITERATION_STEPS / self._num_action_repeat)
+ self._pybullet_client.setPhysicsEngineParameter(
+ numSolverIterations=self._num_bullet_solver_iterations)
+ self._pybullet_client.setTimeStep(self._sim_time_step)
+ for robot in self._robots:
+ robot.SetTimeSteps(self._num_action_repeat, self._sim_time_step)
+
+ def get_time_since_reset(self):
+ """Get the time passed (in seconds) since the last reset.
+
+ Returns:
+ List of time in seconds since the last reset for each robot.
+ """
+ return self._robots[0].GetTimeSinceReset()
+
+ @property
+ def tasks(self):
+ return self._tasks
+
+ @property
+ def robots(self):
+ return self._robots
+
+ @property
+ def num_robots(self):
+ return len(self._robots)
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/scenes/scene_base.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/scenes/scene_base.py
new file mode 100644
index 000000000..c52ce10f6
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/scenes/scene_base.py
@@ -0,0 +1,256 @@
+# Lint as: python3
+"""Class for loading and managing a scene in pybullet."""
+
+import enum
+from typing import Any, Dict, List, Optional, Sequence, Text
+import gin
+import numpy as np
+
+from pybullet_envs.minitaur.envs_v2.scenes import world_asset_pb2
+from pybullet_envs.minitaur.envs_v2 import base_client
+from pybullet_utils import bullet_client
+from pybullet_envs.minitaur.robots import autonomous_object
+
+
+
+# The 2D coordinates of the corners of a polygon. The corners are specified in
+# counterclock-wise direction.
+Polygon = Sequence[Sequence[float]]
+
+
+class ObjectType(enum.Enum):
+ """Categories of objects that may be found in a scene."""
+ OTHER = 0
+ GROUND = 1
+ OBSTACLE = 2
+ GOAL = 3
+ DYNAMIC_OBJECT = 4
+
+
+@gin.configurable
+class SceneBase(object):
+ """Class for loading and managing a scene."""
+
+ def __init__(
+ self,
+ data_root: Text = None,
+ dynamic_objects: Sequence[autonomous_object.AutonomousObject] = ()):
+ """Initializes SceneBase.
+
+ Args:
+ data_root: Root directory for finding object models.
+ dynamic_objects: Dynamic objects to be added into the scene.
+ crowd_builders: Builders of crowds formed by autonomous objects.
+ """
+ self._pybullet_client = None
+ self._data_root = data_root
+ temp_dynamic_objects = list(dynamic_objects)
+
+ self._dynamic_objects = tuple(temp_dynamic_objects)
+ # Dictionaries and world_asset are declared outside init to make sure they
+ # are all reset in _reset_scene_tracking().
+ self._reset_scene_tracking()
+
+ def _reset_scene_tracking(self):
+ """Clears all scene dicts. Used when the simulation is reset."""
+ self._type_to_ids_dict = {object_type: [] for object_type in ObjectType}
+ self._type_to_objects_dict = {object_type: [] for object_type in ObjectType}
+ self._id_to_type_dict = {}
+ self._id_to_object_dict = {}
+ self._world_asset = None
+
+ def destroy_scene(self):
+ """Destroys contents of scene to get ready for another build_scene call."""
+ id_to_remove = list(self._id_to_object_dict.keys())
+ for object_id in id_to_remove:
+ self.remove_object(object_id)
+ self._reset_scene_tracking()
+
+ def build_scene(self, pybullet_client: bullet_client.BulletClient):
+ """Loads and positions all scene objects in pybullet.
+
+ Override this function in subclass to implement customized scene. The
+ overriding function must call base function first.
+
+ Args:
+ pybullet_client: A pybullet client in which the scene will be built.
+ """
+ self._reset_scene_tracking()
+ self._pybullet_client = pybullet_client
+ self._init_dynamic_objects()
+
+ def reset(self):
+ """The soft reset of scene.
+
+ Unlike "build_scene", this is called at each env.reset() before robot
+ resetting. Typically we use this API to do some soft resetting like putting
+ objects back to its place. Howevever, for special cases such as P2P multimap
+ training, we can reload a different mesh scene once a while.
+ """
+ pass
+
+ def _init_dynamic_objects(self):
+ """Adds dynamic objects to scene."""
+ for an_object in self._dynamic_objects:
+ an_object.set_sim_client(self._pybullet_client)
+ self.add_object(an_object.sim_object_id, ObjectType.DYNAMIC_OBJECT,
+ an_object)
+
+ @property
+ def pybullet_client(self) -> bullet_client.BulletClient:
+ if self._pybullet_client is None:
+ raise ValueError("pybullet_client is None; did you call build_scene()?")
+ return self._pybullet_client
+
+ @property
+ def ground_height(self) -> float:
+ """Returns ground height of the scene."""
+ return 0.0
+
+ @property
+ def ground_ids(self) -> List[int]:
+ """Returns the pybullet ids of the ground."""
+ return self._type_to_ids_dict[ObjectType.GROUND]
+
+ @property
+ def obstacle_ids(self) -> List[int]:
+ """Returns the pybullet ids of all obstacles in the scene."""
+ return self._type_to_ids_dict[ObjectType.OBSTACLE]
+
+ @property
+ def goal_ids(self) -> List[int]:
+ """Returns the pybullet ids of any goals in the scene."""
+ return self._type_to_ids_dict[ObjectType.GOAL]
+
+ @property
+ def dynamic_object_ids(self) -> List[int]:
+ """Returns the pybullet ids of dynamic objects."""
+ return self._type_to_ids_dict[ObjectType.DYNAMIC_OBJECT]
+
+ @property
+ def dynamic_objects(self) -> List[autonomous_object.AutonomousObject]:
+ """Returns the dynamic objects python object (AutonomousObject)."""
+ return self._type_to_objects_dict[ObjectType.DYNAMIC_OBJECT]
+
+ @property
+ def world_asset(self) -> world_asset_pb2.WorldAsset:
+ """Returns a proto describing the semantics of the scene.
+
+ If the scene keeps a WorldAsset, then mutating this proto will mutate it for
+ everyone. If the scene generates a WorldAsset from _type_to_ids_dict, then
+ this is not an issue.
+ """
+ if self._world_asset:
+ return self._world_asset
+ return self._dict_to_world_asset(self._type_to_ids_dict)
+
+ def add_object(self,
+ object_id: int,
+ class_label: ObjectType,
+ python_object: Optional[Any] = None):
+ """Adds an object to be tracked.
+
+ Does not load anything into pybullet.
+
+ Args:
+ object_id: objectUniqueId from pybullet.
+ class_label: What type to consider the new object.
+ python_object: Associated python object for the pybullet object of
+ objectUniqueId == object_id. Environment uses the python object to
+ control object in pybullet in these cases. One example is python objects
+ of class label DYNAMIC_OBJECT: they are associated with python objects
+ of type AutonomousObject.
+ """
+ if python_object is not None:
+ if (isinstance(python_object, autonomous_object.AutonomousObject) and
+ python_object.sim_object_id != object_id):
+ raise ValueError(
+ f"Mismatch object ids, object_id = {object_id}, sim_object_id = "
+ f"{python_object.sim_object_id}")
+ self._type_to_objects_dict[class_label].append(python_object)
+ self._type_to_ids_dict[class_label].append(object_id)
+ self._id_to_type_dict[object_id] = class_label
+ self._id_to_object_dict[object_id] = python_object
+
+ def remove_object(self, object_id: int):
+ """Removes an object from tracking and from pybullet.
+
+ Args:
+ object_id: objectUniqueID from pybullet.
+
+ Raises:
+ KeyError: if object_id does not exist in the record.
+ """
+ if object_id not in self._id_to_type_dict.keys():
+ raise KeyError(
+ f"Object with object_id = {object_id} does not exist in the record.")
+
+ self.pybullet_client.removeBody(object_id)
+ object_type = self._id_to_type_dict[object_id]
+ self._type_to_ids_dict[object_type].remove(object_id)
+
+ object_to_remove = self.id_to_object(object_id)
+ if object_to_remove is not None:
+ # Removes item by identity comparison and avoid slow down due to objects
+ # with complex equality comparison function. list.remove() compares
+ # equality instead of identity.
+ for i, an_object in enumerate(self._type_to_objects_dict[object_type]):
+ if an_object is object_to_remove:
+ del self._type_to_objects_dict[object_type][i]
+ break
+ del self._id_to_type_dict[object_id]
+ del self._id_to_object_dict[object_id]
+
+ def id_to_object(self, object_id: int) -> Any:
+ """Returns underlying python object from sim object id.
+
+ Args:
+ object_id: objectUniqueID from pybullet.
+
+ Returns:
+ None is returned if the sim object does not have a corresponding python
+ object.
+ """
+ return self._id_to_object_dict[object_id]
+
+ def _dict_to_world_asset(
+ self, type_to_ids_dict: Dict[ObjectType,
+ List[int]]) -> world_asset_pb2.WorldAsset:
+ """Converts a dictionary to a WorldAsset.
+
+ Args:
+ type_to_ids_dict: Dictionary that describes the scene. Keys are
+ ObjectTypes and values are lists of integers, where each integer is a
+ pybullet id for an object of a given type.
+
+ Returns:
+ A WorldAsset proto with the types, locations and bounding boxes of all
+ objects in the scene.
+ """
+ world_asset = world_asset_pb2.WorldAsset()
+ for object_type in type_to_ids_dict.keys():
+ for obj_id in type_to_ids_dict[object_type]:
+ bbox = np.array(self.pybullet_client.getAABB(obj_id))
+ bbox_center = np.mean(bbox, axis=0)
+ bbox_dimensions = bbox[1] - bbox[0]
+
+ obj = world_asset_pb2.Object()
+ obj.id = str(obj_id)
+ obj.label = str(object_type)
+ obj.bounding_box.center.x = bbox_center[0]
+ obj.bounding_box.center.y = bbox_center[1]
+ obj.bounding_box.center.z = bbox_center[2]
+ obj.bounding_box.dimensions.x = bbox_dimensions[0]
+ obj.bounding_box.dimensions.y = bbox_dimensions[1]
+ obj.bounding_box.dimensions.z = bbox_dimensions[2]
+ world_asset.objects.append(obj)
+ return world_asset
+
+ def close(self):
+ """Closes the scene at the end of life cycle of the environment."""
+ pass
+
+ @property
+ def vectorized_map(self) -> Sequence[Polygon]:
+ """Returns vectorized map containing a list of polygon obstacles."""
+ raise NotImplementedError("vectorized_map is not implemented by default.")
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/scenes/simple_scene.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/scenes/simple_scene.py
new file mode 100644
index 000000000..0099e9e96
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/scenes/simple_scene.py
@@ -0,0 +1,35 @@
+# Lint as: python3
+"""A scene containing only a planar floor."""
+
+from typing import Sequence
+
+import gin
+from pybullet_envs.minitaur.envs_v2 import base_client
+from pybullet_envs.minitaur.envs_v2.scenes import scene_base
+
+_PLANE_URDF = (
+ "plane.urdf")
+
+
+@gin.configurable
+class SimpleScene(scene_base.SceneBase):
+ """A scene containing only a planar floor."""
+
+ def build_scene(self, pybullet_client):
+ super().build_scene(pybullet_client)
+
+ visual_shape_id = self._pybullet_client.createVisualShape(
+ shapeType=self._pybullet_client.GEOM_PLANE)
+ collision_shape_id = self._pybullet_client.createCollisionShape(
+ shapeType=self._pybullet_client.GEOM_PLANE)
+ ground_id = self._pybullet_client.createMultiBody(
+ baseMass=0,
+ baseCollisionShapeIndex=collision_shape_id,
+ baseVisualShapeIndex=visual_shape_id)
+ self._pybullet_client.changeDynamics(ground_id, -1, lateralFriction=1.0)
+ self.add_object(ground_id, scene_base.ObjectType.GROUND)
+
+ @property
+ def vectorized_map(self) -> Sequence[scene_base.Polygon]:
+ """Returns vectorized map containing a list of polygon obstacles."""
+ return []
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/scenes/world_asset.proto b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/scenes/world_asset.proto
new file mode 100644
index 000000000..0e0b97cc4
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/scenes/world_asset.proto
@@ -0,0 +1,86 @@
+// A set of protocol buffer definitions for representing 'Worlds' for
+// used by simulation engines.
+syntax = "proto3";
+
+package robotics.messages;
+option cc_enable_arenas = true;
+
+
+// A single precision quaternion.
+message QQuaternionf {
+ // The x-component.
+ float x = 1;
+ // The y-component.
+ float y = 2;
+ // The z-component.
+ float z = 3;
+ // The w-component.
+ float w = 4;
+}
+
+
+
+
+// A three-dimensional single precision vector.
+message VVector3f {
+ float x = 1;
+ float y = 2;
+ float z = 3;
+}
+
+
+
+message Labels {
+ map label = 1;
+}
+
+// A general bounding box is specified by its center, dimensions and
+// orientation. If the orientation field is not specified, then the bounding box
+// is aligned with the axes of the world coordinate system.
+message GeneralBox3f {
+ VVector3f center = 1;
+ VVector3f dimensions = 2;
+ QQuaternionf orientation = 3;
+}
+
+// An object is specified by its label, bounding box, and a path to an obj
+// file containing its mesh. Not all to be specified.
+message Object {
+ string id = 1;
+ string label = 2;
+ GeneralBox3f bounding_box = 3;
+ string mesh_path = 4;
+}
+
+message Polygon3f {
+ repeated VVector3f vertex = 1;
+}
+
+// A region is specified by its label, spatial extent, and a path to an obj
+// file containing its mesh. Not all to be specified.
+message Region {
+ string id = 1;
+ string label = 2;
+ oneof spatial_extent {
+ GeneralBox3f bounding_box = 3;
+ Polygon3f polygon = 5;
+ }
+ string mesh_path = 6;
+}
+
+// A world consist of a mesh, and potentially a set of object and regions.
+message WorldAsset {
+ // A set of labels for objects. This the space of all object labels.
+ Labels object_labels = 1;
+ repeated Object objects = 2;
+
+ // A set of labels for regions. This the space of all region labels.
+ Labels region_labels = 3;
+ repeated Region regions = 4;
+
+ // A path to a mesh (as obj file) containing the geometry of the world.
+ string mesh_path = 5;
+
+ // A path to a mesh (as obj file) containing the semantic labels of the world.
+ string segmentation_mesh_path = 6;
+}
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/scenes/world_asset_pb2.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/scenes/world_asset_pb2.py
new file mode 100644
index 000000000..a7d99baea
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/scenes/world_asset_pb2.py
@@ -0,0 +1,555 @@
+# -*- coding: utf-8 -*-
+# Generated by the protocol buffer compiler. DO NOT EDIT!
+# source: world_asset.proto
+"""Generated protocol buffer code."""
+from google.protobuf import descriptor as _descriptor
+from google.protobuf import message as _message
+from google.protobuf import reflection as _reflection
+from google.protobuf import symbol_database as _symbol_database
+# @@protoc_insertion_point(imports)
+
+_sym_db = _symbol_database.Default()
+
+
+
+
+DESCRIPTOR = _descriptor.FileDescriptor(
+ name='world_asset.proto',
+ package='robotics.messages',
+ syntax='proto3',
+ serialized_options=b'\370\001\001',
+ create_key=_descriptor._internal_create_key,
+ serialized_pb=b'\n\x11world_asset.proto\x12\x11robotics.messages\":\n\x0cQQuaternionf\x12\t\n\x01x\x18\x01 \x01(\x02\x12\t\n\x01y\x18\x02 \x01(\x02\x12\t\n\x01z\x18\x03 \x01(\x02\x12\t\n\x01w\x18\x04 \x01(\x02\",\n\tVVector3f\x12\t\n\x01x\x18\x01 \x01(\x02\x12\t\n\x01y\x18\x02 \x01(\x02\x12\t\n\x01z\x18\x03 \x01(\x02\"k\n\x06Labels\x12\x33\n\x05label\x18\x01 \x03(\x0b\x32$.robotics.messages.Labels.LabelEntry\x1a,\n\nLabelEntry\x12\x0b\n\x03key\x18\x01 \x01(\t\x12\r\n\x05value\x18\x02 \x01(\t:\x02\x38\x01\"\xa4\x01\n\x0cGeneralBox3f\x12,\n\x06\x63\x65nter\x18\x01 \x01(\x0b\x32\x1c.robotics.messages.VVector3f\x12\x30\n\ndimensions\x18\x02 \x01(\x0b\x32\x1c.robotics.messages.VVector3f\x12\x34\n\x0borientation\x18\x03 \x01(\x0b\x32\x1f.robotics.messages.QQuaternionf\"m\n\x06Object\x12\n\n\x02id\x18\x01 \x01(\t\x12\r\n\x05label\x18\x02 \x01(\t\x12\x35\n\x0c\x62ounding_box\x18\x03 \x01(\x0b\x32\x1f.robotics.messages.GeneralBox3f\x12\x11\n\tmesh_path\x18\x04 \x01(\t\"9\n\tPolygon3f\x12,\n\x06vertex\x18\x01 \x03(\x0b\x32\x1c.robotics.messages.VVector3f\"\xb2\x01\n\x06Region\x12\n\n\x02id\x18\x01 \x01(\t\x12\r\n\x05label\x18\x02 \x01(\t\x12\x37\n\x0c\x62ounding_box\x18\x03 \x01(\x0b\x32\x1f.robotics.messages.GeneralBox3fH\x00\x12/\n\x07polygon\x18\x05 \x01(\x0b\x32\x1c.robotics.messages.Polygon3fH\x00\x12\x11\n\tmesh_path\x18\x06 \x01(\tB\x10\n\x0espatial_extent\"\xfb\x01\n\nWorldAsset\x12\x30\n\robject_labels\x18\x01 \x01(\x0b\x32\x19.robotics.messages.Labels\x12*\n\x07objects\x18\x02 \x03(\x0b\x32\x19.robotics.messages.Object\x12\x30\n\rregion_labels\x18\x03 \x01(\x0b\x32\x19.robotics.messages.Labels\x12*\n\x07regions\x18\x04 \x03(\x0b\x32\x19.robotics.messages.Region\x12\x11\n\tmesh_path\x18\x05 \x01(\t\x12\x1e\n\x16segmentation_mesh_path\x18\x06 \x01(\tB\x03\xf8\x01\x01\x62\x06proto3'
+)
+
+
+
+
+_QQUATERNIONF = _descriptor.Descriptor(
+ name='QQuaternionf',
+ full_name='robotics.messages.QQuaternionf',
+ filename=None,
+ file=DESCRIPTOR,
+ containing_type=None,
+ create_key=_descriptor._internal_create_key,
+ fields=[
+ _descriptor.FieldDescriptor(
+ name='x', full_name='robotics.messages.QQuaternionf.x', index=0,
+ number=1, type=2, cpp_type=6, label=1,
+ has_default_value=False, default_value=float(0),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='y', full_name='robotics.messages.QQuaternionf.y', index=1,
+ number=2, type=2, cpp_type=6, label=1,
+ has_default_value=False, default_value=float(0),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='z', full_name='robotics.messages.QQuaternionf.z', index=2,
+ number=3, type=2, cpp_type=6, label=1,
+ has_default_value=False, default_value=float(0),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='w', full_name='robotics.messages.QQuaternionf.w', index=3,
+ number=4, type=2, cpp_type=6, label=1,
+ has_default_value=False, default_value=float(0),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ ],
+ extensions=[
+ ],
+ nested_types=[],
+ enum_types=[
+ ],
+ serialized_options=None,
+ is_extendable=False,
+ syntax='proto3',
+ extension_ranges=[],
+ oneofs=[
+ ],
+ serialized_start=40,
+ serialized_end=98,
+)
+
+
+_VVECTOR3F = _descriptor.Descriptor(
+ name='VVector3f',
+ full_name='robotics.messages.VVector3f',
+ filename=None,
+ file=DESCRIPTOR,
+ containing_type=None,
+ create_key=_descriptor._internal_create_key,
+ fields=[
+ _descriptor.FieldDescriptor(
+ name='x', full_name='robotics.messages.VVector3f.x', index=0,
+ number=1, type=2, cpp_type=6, label=1,
+ has_default_value=False, default_value=float(0),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='y', full_name='robotics.messages.VVector3f.y', index=1,
+ number=2, type=2, cpp_type=6, label=1,
+ has_default_value=False, default_value=float(0),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='z', full_name='robotics.messages.VVector3f.z', index=2,
+ number=3, type=2, cpp_type=6, label=1,
+ has_default_value=False, default_value=float(0),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ ],
+ extensions=[
+ ],
+ nested_types=[],
+ enum_types=[
+ ],
+ serialized_options=None,
+ is_extendable=False,
+ syntax='proto3',
+ extension_ranges=[],
+ oneofs=[
+ ],
+ serialized_start=100,
+ serialized_end=144,
+)
+
+
+_LABELS_LABELENTRY = _descriptor.Descriptor(
+ name='LabelEntry',
+ full_name='robotics.messages.Labels.LabelEntry',
+ filename=None,
+ file=DESCRIPTOR,
+ containing_type=None,
+ create_key=_descriptor._internal_create_key,
+ fields=[
+ _descriptor.FieldDescriptor(
+ name='key', full_name='robotics.messages.Labels.LabelEntry.key', index=0,
+ number=1, type=9, cpp_type=9, label=1,
+ has_default_value=False, default_value=b"".decode('utf-8'),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='value', full_name='robotics.messages.Labels.LabelEntry.value', index=1,
+ number=2, type=9, cpp_type=9, label=1,
+ has_default_value=False, default_value=b"".decode('utf-8'),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ ],
+ extensions=[
+ ],
+ nested_types=[],
+ enum_types=[
+ ],
+ serialized_options=b'8\001',
+ is_extendable=False,
+ syntax='proto3',
+ extension_ranges=[],
+ oneofs=[
+ ],
+ serialized_start=209,
+ serialized_end=253,
+)
+
+_LABELS = _descriptor.Descriptor(
+ name='Labels',
+ full_name='robotics.messages.Labels',
+ filename=None,
+ file=DESCRIPTOR,
+ containing_type=None,
+ create_key=_descriptor._internal_create_key,
+ fields=[
+ _descriptor.FieldDescriptor(
+ name='label', full_name='robotics.messages.Labels.label', index=0,
+ number=1, type=11, cpp_type=10, label=3,
+ has_default_value=False, default_value=[],
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ ],
+ extensions=[
+ ],
+ nested_types=[_LABELS_LABELENTRY, ],
+ enum_types=[
+ ],
+ serialized_options=None,
+ is_extendable=False,
+ syntax='proto3',
+ extension_ranges=[],
+ oneofs=[
+ ],
+ serialized_start=146,
+ serialized_end=253,
+)
+
+
+_GENERALBOX3F = _descriptor.Descriptor(
+ name='GeneralBox3f',
+ full_name='robotics.messages.GeneralBox3f',
+ filename=None,
+ file=DESCRIPTOR,
+ containing_type=None,
+ create_key=_descriptor._internal_create_key,
+ fields=[
+ _descriptor.FieldDescriptor(
+ name='center', full_name='robotics.messages.GeneralBox3f.center', index=0,
+ number=1, type=11, cpp_type=10, label=1,
+ has_default_value=False, default_value=None,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='dimensions', full_name='robotics.messages.GeneralBox3f.dimensions', index=1,
+ number=2, type=11, cpp_type=10, label=1,
+ has_default_value=False, default_value=None,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='orientation', full_name='robotics.messages.GeneralBox3f.orientation', index=2,
+ number=3, type=11, cpp_type=10, label=1,
+ has_default_value=False, default_value=None,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ ],
+ extensions=[
+ ],
+ nested_types=[],
+ enum_types=[
+ ],
+ serialized_options=None,
+ is_extendable=False,
+ syntax='proto3',
+ extension_ranges=[],
+ oneofs=[
+ ],
+ serialized_start=256,
+ serialized_end=420,
+)
+
+
+_OBJECT = _descriptor.Descriptor(
+ name='Object',
+ full_name='robotics.messages.Object',
+ filename=None,
+ file=DESCRIPTOR,
+ containing_type=None,
+ create_key=_descriptor._internal_create_key,
+ fields=[
+ _descriptor.FieldDescriptor(
+ name='id', full_name='robotics.messages.Object.id', index=0,
+ number=1, type=9, cpp_type=9, label=1,
+ has_default_value=False, default_value=b"".decode('utf-8'),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='label', full_name='robotics.messages.Object.label', index=1,
+ number=2, type=9, cpp_type=9, label=1,
+ has_default_value=False, default_value=b"".decode('utf-8'),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='bounding_box', full_name='robotics.messages.Object.bounding_box', index=2,
+ number=3, type=11, cpp_type=10, label=1,
+ has_default_value=False, default_value=None,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='mesh_path', full_name='robotics.messages.Object.mesh_path', index=3,
+ number=4, type=9, cpp_type=9, label=1,
+ has_default_value=False, default_value=b"".decode('utf-8'),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ ],
+ extensions=[
+ ],
+ nested_types=[],
+ enum_types=[
+ ],
+ serialized_options=None,
+ is_extendable=False,
+ syntax='proto3',
+ extension_ranges=[],
+ oneofs=[
+ ],
+ serialized_start=422,
+ serialized_end=531,
+)
+
+
+_POLYGON3F = _descriptor.Descriptor(
+ name='Polygon3f',
+ full_name='robotics.messages.Polygon3f',
+ filename=None,
+ file=DESCRIPTOR,
+ containing_type=None,
+ create_key=_descriptor._internal_create_key,
+ fields=[
+ _descriptor.FieldDescriptor(
+ name='vertex', full_name='robotics.messages.Polygon3f.vertex', index=0,
+ number=1, type=11, cpp_type=10, label=3,
+ has_default_value=False, default_value=[],
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ ],
+ extensions=[
+ ],
+ nested_types=[],
+ enum_types=[
+ ],
+ serialized_options=None,
+ is_extendable=False,
+ syntax='proto3',
+ extension_ranges=[],
+ oneofs=[
+ ],
+ serialized_start=533,
+ serialized_end=590,
+)
+
+
+_REGION = _descriptor.Descriptor(
+ name='Region',
+ full_name='robotics.messages.Region',
+ filename=None,
+ file=DESCRIPTOR,
+ containing_type=None,
+ create_key=_descriptor._internal_create_key,
+ fields=[
+ _descriptor.FieldDescriptor(
+ name='id', full_name='robotics.messages.Region.id', index=0,
+ number=1, type=9, cpp_type=9, label=1,
+ has_default_value=False, default_value=b"".decode('utf-8'),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='label', full_name='robotics.messages.Region.label', index=1,
+ number=2, type=9, cpp_type=9, label=1,
+ has_default_value=False, default_value=b"".decode('utf-8'),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='bounding_box', full_name='robotics.messages.Region.bounding_box', index=2,
+ number=3, type=11, cpp_type=10, label=1,
+ has_default_value=False, default_value=None,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='polygon', full_name='robotics.messages.Region.polygon', index=3,
+ number=5, type=11, cpp_type=10, label=1,
+ has_default_value=False, default_value=None,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='mesh_path', full_name='robotics.messages.Region.mesh_path', index=4,
+ number=6, type=9, cpp_type=9, label=1,
+ has_default_value=False, default_value=b"".decode('utf-8'),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ ],
+ extensions=[
+ ],
+ nested_types=[],
+ enum_types=[
+ ],
+ serialized_options=None,
+ is_extendable=False,
+ syntax='proto3',
+ extension_ranges=[],
+ oneofs=[
+ _descriptor.OneofDescriptor(
+ name='spatial_extent', full_name='robotics.messages.Region.spatial_extent',
+ index=0, containing_type=None,
+ create_key=_descriptor._internal_create_key,
+ fields=[]),
+ ],
+ serialized_start=593,
+ serialized_end=771,
+)
+
+
+_WORLDASSET = _descriptor.Descriptor(
+ name='WorldAsset',
+ full_name='robotics.messages.WorldAsset',
+ filename=None,
+ file=DESCRIPTOR,
+ containing_type=None,
+ create_key=_descriptor._internal_create_key,
+ fields=[
+ _descriptor.FieldDescriptor(
+ name='object_labels', full_name='robotics.messages.WorldAsset.object_labels', index=0,
+ number=1, type=11, cpp_type=10, label=1,
+ has_default_value=False, default_value=None,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='objects', full_name='robotics.messages.WorldAsset.objects', index=1,
+ number=2, type=11, cpp_type=10, label=3,
+ has_default_value=False, default_value=[],
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='region_labels', full_name='robotics.messages.WorldAsset.region_labels', index=2,
+ number=3, type=11, cpp_type=10, label=1,
+ has_default_value=False, default_value=None,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='regions', full_name='robotics.messages.WorldAsset.regions', index=3,
+ number=4, type=11, cpp_type=10, label=3,
+ has_default_value=False, default_value=[],
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='mesh_path', full_name='robotics.messages.WorldAsset.mesh_path', index=4,
+ number=5, type=9, cpp_type=9, label=1,
+ has_default_value=False, default_value=b"".decode('utf-8'),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='segmentation_mesh_path', full_name='robotics.messages.WorldAsset.segmentation_mesh_path', index=5,
+ number=6, type=9, cpp_type=9, label=1,
+ has_default_value=False, default_value=b"".decode('utf-8'),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ ],
+ extensions=[
+ ],
+ nested_types=[],
+ enum_types=[
+ ],
+ serialized_options=None,
+ is_extendable=False,
+ syntax='proto3',
+ extension_ranges=[],
+ oneofs=[
+ ],
+ serialized_start=774,
+ serialized_end=1025,
+)
+
+_LABELS_LABELENTRY.containing_type = _LABELS
+_LABELS.fields_by_name['label'].message_type = _LABELS_LABELENTRY
+_GENERALBOX3F.fields_by_name['center'].message_type = _VVECTOR3F
+_GENERALBOX3F.fields_by_name['dimensions'].message_type = _VVECTOR3F
+_GENERALBOX3F.fields_by_name['orientation'].message_type = _QQUATERNIONF
+_OBJECT.fields_by_name['bounding_box'].message_type = _GENERALBOX3F
+_POLYGON3F.fields_by_name['vertex'].message_type = _VVECTOR3F
+_REGION.fields_by_name['bounding_box'].message_type = _GENERALBOX3F
+_REGION.fields_by_name['polygon'].message_type = _POLYGON3F
+_REGION.oneofs_by_name['spatial_extent'].fields.append(
+ _REGION.fields_by_name['bounding_box'])
+_REGION.fields_by_name['bounding_box'].containing_oneof = _REGION.oneofs_by_name['spatial_extent']
+_REGION.oneofs_by_name['spatial_extent'].fields.append(
+ _REGION.fields_by_name['polygon'])
+_REGION.fields_by_name['polygon'].containing_oneof = _REGION.oneofs_by_name['spatial_extent']
+_WORLDASSET.fields_by_name['object_labels'].message_type = _LABELS
+_WORLDASSET.fields_by_name['objects'].message_type = _OBJECT
+_WORLDASSET.fields_by_name['region_labels'].message_type = _LABELS
+_WORLDASSET.fields_by_name['regions'].message_type = _REGION
+DESCRIPTOR.message_types_by_name['QQuaternionf'] = _QQUATERNIONF
+DESCRIPTOR.message_types_by_name['VVector3f'] = _VVECTOR3F
+DESCRIPTOR.message_types_by_name['Labels'] = _LABELS
+DESCRIPTOR.message_types_by_name['GeneralBox3f'] = _GENERALBOX3F
+DESCRIPTOR.message_types_by_name['Object'] = _OBJECT
+DESCRIPTOR.message_types_by_name['Polygon3f'] = _POLYGON3F
+DESCRIPTOR.message_types_by_name['Region'] = _REGION
+DESCRIPTOR.message_types_by_name['WorldAsset'] = _WORLDASSET
+_sym_db.RegisterFileDescriptor(DESCRIPTOR)
+
+QQuaternionf = _reflection.GeneratedProtocolMessageType('QQuaternionf', (_message.Message,), {
+ 'DESCRIPTOR' : _QQUATERNIONF,
+ '__module__' : 'world_asset_pb2'
+ # @@protoc_insertion_point(class_scope:robotics.messages.QQuaternionf)
+ })
+_sym_db.RegisterMessage(QQuaternionf)
+
+VVector3f = _reflection.GeneratedProtocolMessageType('VVector3f', (_message.Message,), {
+ 'DESCRIPTOR' : _VVECTOR3F,
+ '__module__' : 'world_asset_pb2'
+ # @@protoc_insertion_point(class_scope:robotics.messages.VVector3f)
+ })
+_sym_db.RegisterMessage(VVector3f)
+
+Labels = _reflection.GeneratedProtocolMessageType('Labels', (_message.Message,), {
+
+ 'LabelEntry' : _reflection.GeneratedProtocolMessageType('LabelEntry', (_message.Message,), {
+ 'DESCRIPTOR' : _LABELS_LABELENTRY,
+ '__module__' : 'world_asset_pb2'
+ # @@protoc_insertion_point(class_scope:robotics.messages.Labels.LabelEntry)
+ })
+ ,
+ 'DESCRIPTOR' : _LABELS,
+ '__module__' : 'world_asset_pb2'
+ # @@protoc_insertion_point(class_scope:robotics.messages.Labels)
+ })
+_sym_db.RegisterMessage(Labels)
+_sym_db.RegisterMessage(Labels.LabelEntry)
+
+GeneralBox3f = _reflection.GeneratedProtocolMessageType('GeneralBox3f', (_message.Message,), {
+ 'DESCRIPTOR' : _GENERALBOX3F,
+ '__module__' : 'world_asset_pb2'
+ # @@protoc_insertion_point(class_scope:robotics.messages.GeneralBox3f)
+ })
+_sym_db.RegisterMessage(GeneralBox3f)
+
+Object = _reflection.GeneratedProtocolMessageType('Object', (_message.Message,), {
+ 'DESCRIPTOR' : _OBJECT,
+ '__module__' : 'world_asset_pb2'
+ # @@protoc_insertion_point(class_scope:robotics.messages.Object)
+ })
+_sym_db.RegisterMessage(Object)
+
+Polygon3f = _reflection.GeneratedProtocolMessageType('Polygon3f', (_message.Message,), {
+ 'DESCRIPTOR' : _POLYGON3F,
+ '__module__' : 'world_asset_pb2'
+ # @@protoc_insertion_point(class_scope:robotics.messages.Polygon3f)
+ })
+_sym_db.RegisterMessage(Polygon3f)
+
+Region = _reflection.GeneratedProtocolMessageType('Region', (_message.Message,), {
+ 'DESCRIPTOR' : _REGION,
+ '__module__' : 'world_asset_pb2'
+ # @@protoc_insertion_point(class_scope:robotics.messages.Region)
+ })
+_sym_db.RegisterMessage(Region)
+
+WorldAsset = _reflection.GeneratedProtocolMessageType('WorldAsset', (_message.Message,), {
+ 'DESCRIPTOR' : _WORLDASSET,
+ '__module__' : 'world_asset_pb2'
+ # @@protoc_insertion_point(class_scope:robotics.messages.WorldAsset)
+ })
+_sym_db.RegisterMessage(WorldAsset)
+
+
+DESCRIPTOR._options = None
+_LABELS_LABELENTRY._options = None
+# @@protoc_insertion_point(module_scope)
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/sensors/accelerometer_sensor.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/sensors/accelerometer_sensor.py
new file mode 100644
index 000000000..954e8ecb3
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/sensors/accelerometer_sensor.py
@@ -0,0 +1,86 @@
+# Lint as: python3
+"""A sensor that measures the acceleration of the robot base."""
+
+from typing import Any, Callable, Sequence, Type, Text, Union
+
+import gin
+import gym
+import numpy as np
+
+from pybullet_envs.minitaur.envs_v2.sensors import sensor
+from pybullet_envs.minitaur.envs_v2.utilities import noise_generators
+
+_ACCELEROMETER_DIM = 3
+_DEFAULT_ACCELEROMETER_LOWER_BOUND = (-1, -1, -1)
+_DEFAULT_ACCELEROMETER_UPPER_BOUND = (1, 1, 1)
+
+
+@gin.configurable
+class AccelerometerSensor(sensor.Sensor):
+ """An Accelerometer sensor."""
+
+ def __init__(
+ self,
+ name: Text = "Accelerometer",
+ dtype: Type[Any] = np.float64,
+ lower_bound: Sequence[float] = _DEFAULT_ACCELEROMETER_LOWER_BOUND,
+ upper_bound: Sequence[float] = _DEFAULT_ACCELEROMETER_UPPER_BOUND,
+ noise_generator: Union[Callable[..., Any],
+ noise_generators.NoiseGenerator] = None,
+ sensor_latency: Union[float, Sequence[float]] = 0.0,
+ ):
+ """Constructs AccelerometerSensor.
+
+ Generates separate IMU value channels as per configuration.
+
+ Args:
+ name: the name of the sensor.
+ dtype: data type of sensor value.
+ lower_bound: The lower bounds of the sensor reading.
+ upper_bound: The upper bounds of the sensor reading.
+ noise_generator: Used to add noise to the readings.
+ sensor_latency: There are two ways to use this expected sensor latency.
+ For both methods, the latency should be in the same unit as the sensor
+ data timestamp. 1. As a single float number, the observation will be a
+ 1D array. For real robots, this should be set to 0.0. 2. As a array of
+ floats, the observation will be a 2D array based on how long the history
+ need to be. Thus, [0.0, 0.1, 0.2] is a history length of 3.
+
+ """
+ super().__init__(
+ name=name,
+ sensor_latency=sensor_latency,
+ interpolator_fn=sensor.linear_obs_blender)
+
+ self._noise_generator = noise_generator
+ self._dtype = dtype
+
+ if lower_bound is None or upper_bound is None:
+ raise ValueError("Must provides bounds for the Accelerometer readings.")
+
+ if len(lower_bound) != _ACCELEROMETER_DIM or len(
+ upper_bound) != _ACCELEROMETER_DIM:
+ raise ValueError(
+ "Bounds must be {} dimensions.".format(_ACCELEROMETER_DIM))
+
+ lower_bound = np.array(lower_bound, dtype=dtype)
+ upper_bound = np.array(upper_bound, dtype=dtype)
+
+ self._observation_space = self._stack_space(
+ gym.spaces.Box(
+ low=np.array(lower_bound, dtype=self._dtype),
+ high=np.array(upper_bound, dtype=self._dtype),
+ dtype=self._dtype))
+
+ def _get_original_observation(self):
+ return self._robot.timestamp, np.array(
+ self._robot.base_acceleration_accelerometer, dtype=self._dtype)
+
+ def get_observation(self) -> np.ndarray:
+ delayed_observation = super().get_observation()
+ if self._noise_generator:
+ if callable(self._noise_generator):
+ return self._noise_generator(delayed_observation)
+ else:
+ return self._noise_generator.add_noise(delayed_observation)
+ return delayed_observation
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/sensors/camera_sensor.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/sensors/camera_sensor.py
new file mode 100644
index 000000000..d1c6bde8c
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/sensors/camera_sensor.py
@@ -0,0 +1,184 @@
+# Lint as: python3
+"""A sensor for robot-mounted 1D lidar (laser scan)."""
+
+from typing import Any, Iterable, Sequence, Type, Union
+
+import gin
+import gym
+import numpy as np
+
+from pybullet_envs.minitaur.envs_v2.sensors import sensor
+from pybullet_envs.minitaur.vision import point_cloud_utils
+from pybullet_envs.minitaur.vision import sim_camera
+
+_MODE_TO_NUM_CHANNELS_DICT = {
+ sim_camera.CameraMode.DEPTH: 1,
+ sim_camera.CameraMode.RGB: 3,
+ sim_camera.CameraMode.RGBD: 4,
+ sim_camera.CameraMode.POINTCLOUD_WORLD_FRAME: 3,
+ sim_camera.CameraMode.POINTCLOUD_ROBOT_FRAME: 3,
+}
+
+
+@gin.configurable
+class CameraSensor(sensor.Sensor):
+ """A robot-mounted sensor that returns RGBD images.
+
+ Attributes:
+ resolution: A 2-tuple (width, height) that represents the resolution of the
+ camera.
+ fov_degree: A floating point value that represents the field of view of the
+ camera in the vertical direction. The unit is degree.
+ """
+
+ def __init__(self,
+ camera_translation_from_base,
+ camera_rotation_from_base,
+ parent_link_id=-1,
+ camera_mode=sim_camera.CameraMode.DEPTH,
+ camera_update_frequency_hz=10,
+ camera_stabilized=False,
+ fov_degree=60,
+ resolution=(32, 32),
+ lower_bound: Union[float, Iterable[float]] = 0.0,
+ upper_bound: Union[float, Iterable[float]] = 255.0,
+ sensor_latency: Union[float, Sequence[float]] = 0.0,
+ dtype: Type[Any] = np.float64,
+ name="vision"):
+ """Initializes the CameraSensor.
+
+ Args:
+ camera_translation_from_base: A 3-vector translation from the center of
+ the specified link.
+ camera_rotation_from_base: A 4-vector quaternion represents the rotation
+ of the camera relative to the specified link.
+ parent_link_id: The pybullet link id, where the camera is mounted on.
+ camera_mode: An enum that specifies the mode that the camera operates. See
+ sim_camera.CameraMode for more details.
+ camera_update_frequency_hz: The frequency at which the camera will capture
+ a frame.
+ camera_stabilized: Whether the camera is stabilized. See
+ sim_camera.MountedCamera for more details.
+ fov_degree: The vertical field of view of the camera (in degree).
+ resolution: A 2-tuple that represents the width and the height of the
+ camera image.
+ lower_bound: The lower bound of values of the camera output. It could be a
+ single float or an array of floats with shape (height, width, channels).
+ upper_bound: The upper bound of values of the camera output. It could be a
+ single float or an array of floats with shape (height, width,
+ channels)..
+ sensor_latency: See base class.
+ dtype: See base class.
+ name: The name of the sensor.
+ """
+ super().__init__(
+ name=name,
+ sensor_latency=sensor_latency,
+ interpolator_fn=sensor.closest_obs_blender)
+ self._parent_link_id = parent_link_id
+ self._camera_mode = camera_mode
+ self._camera_translation_from_base = camera_translation_from_base
+ self._camera_rotation_from_base = camera_rotation_from_base
+ self.camera_update_frequency_hz = camera_update_frequency_hz
+ self._time_interval_every_camera_update = (1.0 /
+ self.camera_update_frequency_hz)
+ self._camera_stabilized = camera_stabilized
+ self._fov_degree = fov_degree
+ self._resolution = resolution
+ num_channels = _MODE_TO_NUM_CHANNELS_DICT[self._camera_mode]
+ self._camera = None
+ self._camera_image = None
+ self._dtype = dtype
+ if isinstance(lower_bound, list):
+ lower_bound = np.array(lower_bound, dtype=self._dtype)
+ else:
+ lower_bound = lower_bound * np.ones(
+ shape=(resolution[1], resolution[0], num_channels), dtype=self._dtype)
+ if isinstance(upper_bound, list):
+ upper_bound = np.array(upper_bound, dtype=self._dtype)
+ else:
+ upper_bound = upper_bound * np.ones(
+ shape=(resolution[1], resolution[0], num_channels), dtype=self._dtype)
+ self._observation_space = gym.spaces.Box(
+ low=lower_bound, high=upper_bound, dtype=self._dtype)
+ self._last_camera_image_timestamp = None
+
+ def change_mounting_point(
+ self,
+ camera_translation_from_link: Sequence[float] = (0, 0, 0),
+ camera_rotation_from_link: Sequence[float] = (0, 0, 0, 1),
+ parent_link_id: int = -1):
+ """Changes mounting point. Must be called before calls to set_robot().
+
+ Args:
+ camera_translation_from_link: A 3-vector translation from the center of
+ the specified link.
+ camera_rotation_from_link: A 4-vector quaternion represents the rotation
+ of the camera relative to the specified link.
+ parent_link_id: The pybullet link id, where the camera is mounted on.
+ """
+ self._parent_link_id = parent_link_id
+ self._camera_translation_from_base = camera_translation_from_link
+ self._camera_rotation_from_base = camera_rotation_from_link
+
+ def set_robot(self, robot):
+ super().set_robot(robot)
+
+ self._camera = sim_camera.MountedCamera(
+ pybullet_client=robot.pybullet_client,
+ body_id=robot.robot_id,
+ parent_link_id=self._parent_link_id,
+ relative_translation=self._camera_translation_from_base,
+ relative_rotation=self._camera_rotation_from_base,
+ stabilized=self._camera_stabilized,
+ camera_mode=self._camera_mode,
+ fov_degree=self._fov_degree,
+ resolution=self._resolution)
+
+ def on_reset(self, env):
+ self._env = env
+ self._last_camera_image_timestamp = None
+ super().on_reset(env)
+
+ def _get_original_observation(self):
+ if self._last_camera_image_timestamp is None or (
+ self._robot.timestamp >= self._last_camera_image_timestamp +
+ self._time_interval_every_camera_update):
+ self._camera_image = self._camera.render_image().astype(self._dtype)
+ self._last_camera_image_timestamp = self._robot.timestamp
+ return self._robot.timestamp, self._camera_image
+
+ def project_depth_map_to_point_cloud(self, depth_map, use_world_frame=True):
+ """Convert the depth map into a 3D point cloud.
+
+ Args:
+ depth_map: A 2D numpy array with shape (height, width) which represents
+ the depth map.
+ use_world_frame: Whether converts the depth map into a point cloud in the
+ world frame. If False, the point cloud is in the robot's local frame. If
+ True, the point cloud is in the world frame if the robot's base
+ position/orientation can be measured (e.g. in sim, using SLAM or mocap).
+
+ Returns:
+ A point cloud represented by a numpy array of shape (height, width, 3).
+ """
+ point_cloud = point_cloud_utils.distance_map_to_point_cloud(
+ np.squeeze(depth_map), self.fov_degree / 180.0 * np.pi,
+ depth_map.shape[1], depth_map.shape[0])
+ if use_world_frame:
+ point_cloud = (
+ self._camera.transform_point_cloud_from_camera_to_world_frame(
+ point_cloud))
+ else:
+ point_cloud = (
+ self._camera.transform_point_cloud_from_camera_to_robot_frame(
+ point_cloud))
+ return point_cloud
+
+ @property
+ def resolution(self):
+ return self._camera.resolution
+
+ @property
+ def fov_degree(self):
+ return self._camera.fov_degree
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/sensors/imu_sensor.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/sensors/imu_sensor.py
new file mode 100644
index 000000000..a26cf491d
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/sensors/imu_sensor.py
@@ -0,0 +1,133 @@
+# Lint as: python3
+"""The on robot sensor classes."""
+
+import enum
+from typing import Any, Callable, Iterable, Sequence, Type, Text, Union
+
+import gin
+import gym
+import numpy as np
+
+from pybullet_envs.minitaur.envs_v2.sensors import sensor
+from pybullet_envs.minitaur.envs_v2.utilities import noise_generators
+
+
+@gin.constants_from_enum
+class IMUChannel(enum.Enum):
+ ROLL = 1,
+ PITCH = 2,
+ YAW = 3,
+ ROLL_RATE = 4,
+ PITCH_RATE = 5,
+ YAW_RATE = 6,
+
+
+@gin.configurable
+class IMUSensor(sensor.Sensor):
+ """An IMU sensor."""
+
+ def __init__(
+ self,
+ name: Text = "IMU",
+ dtype: Type[Any] = np.float64,
+ channels: Sequence[IMUChannel] = None,
+ lower_bound: Union[float, Iterable[float]] = None,
+ upper_bound: Union[float, Iterable[float]] = None,
+ noise_generator: Union[Callable[..., Any],
+ noise_generators.NoiseGenerator] = None,
+ sensor_latency: Union[float, Sequence[float]] = 0.0,
+ ):
+ """Constructs IMUSensor.
+
+ Generates separate IMU value channels as per configuration.
+
+ Args:
+ name: the name of the sensor.
+ dtype: data type of sensor value.
+ channels: value channels wants to subscribe. Must be members of the
+ IMUChannel class.
+ lower_bound: The lower bounds of the sensor reading.
+ upper_bound: The upper bounds of the sensor reading.
+ noise_generator: Used to add noise to the readings.
+ sensor_latency: There are two ways to use this expected sensor latency.
+ For both methods, the latency should be in the same unit as the sensor
+ data timestamp. 1. As a single float number, the observation will be a
+ 1D array. For real robots, this should be set to 0.0. 2. As a array of
+ floats, the observation will be a 2D array based on how long the history
+ need to be. Thus, [0.0, 0.1, 0.2] is a history length of 3.
+
+ Raises:
+ ValueError: If no IMU channel is provided and no bounds for the channels.
+ """
+ super().__init__(
+ name=name,
+ sensor_latency=sensor_latency,
+ interpolator_fn=sensor.linear_obs_blender)
+ if channels is None:
+ raise ValueError("IMU channels are not provided.")
+ self._channels = channels
+ self._num_channels = len(self._channels)
+ self._noise_generator = noise_generator
+ self._dtype = dtype
+
+ if lower_bound is None or upper_bound is None:
+ raise ValueError("Must provides bounds for the IMU readings.")
+
+ if isinstance(lower_bound, (float, int)):
+ lower_bound = np.full(self._num_channels, lower_bound, dtype=dtype)
+ else:
+ lower_bound = np.array(lower_bound, dtype=dtype)
+
+ if len(lower_bound) != self._num_channels:
+ raise ValueError("length of sensor lower bound {lower_bound} does not"
+ " match the number of channels.")
+
+ if isinstance(upper_bound, (float, int)):
+ upper_bound = np.full(self._num_channels, upper_bound, dtype=dtype)
+ else:
+ upper_bound = np.array(upper_bound, dtype=dtype)
+
+ if len(upper_bound) != self._num_channels:
+ raise ValueError("length of sensor upper bound {upper_bound} does not"
+ " match the number of channels.")
+
+ self._observation_space = self._stack_space(
+ gym.spaces.Box(
+ low=np.array(lower_bound, dtype=self._dtype),
+ high=np.array(upper_bound, dtype=self._dtype),
+ dtype=self._dtype))
+
+ def get_channels(self) -> Sequence[IMUChannel]:
+ return self._channels
+
+ def get_num_channels(self) -> int:
+ return self._num_channels
+
+ def _get_original_observation(self):
+ rpy = self._robot.base_roll_pitch_yaw
+ observations = np.zeros(self._num_channels)
+ for i, channel in enumerate(self._channels):
+ if channel == IMUChannel.ROLL:
+ observations[i] = rpy[0]
+ elif channel == IMUChannel.PITCH:
+ observations[i] = rpy[1]
+ elif channel == IMUChannel.YAW:
+ observations[i] = rpy[2]
+ elif channel == IMUChannel.ROLL_RATE:
+ observations[i] = self._robot.base_roll_pitch_yaw_rate[0]
+ elif channel == IMUChannel.PITCH_RATE:
+ observations[i] = self._robot.base_roll_pitch_yaw_rate[1]
+ elif channel == IMUChannel.YAW_RATE:
+ observations[i] = self._robot.base_roll_pitch_yaw_rate[2]
+
+ return self._robot.timestamp, np.array(observations, dtype=self._dtype)
+
+ def get_observation(self) -> np.ndarray:
+ delayed_observation = super().get_observation()
+ if self._noise_generator:
+ if callable(self._noise_generator):
+ return self._noise_generator(delayed_observation)
+ else:
+ return self._noise_generator.add_noise(delayed_observation)
+
+ return delayed_observation
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/sensors/last_action_sensor.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/sensors/last_action_sensor.py
new file mode 100644
index 000000000..e61299f02
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/sensors/last_action_sensor.py
@@ -0,0 +1,60 @@
+# Lint as: python3
+"""A sensor that returns the last action(s) sent to the environment."""
+
+from typing import Any, Dict, Sequence, Text, Type, Tuple, Union
+import gin
+import gym
+import numpy as np
+
+from pybullet_envs.minitaur.envs_v2.sensors import sensor
+from pybullet_envs.minitaur.envs_v2.sensors import space_utils
+
+
+@gin.configurable
+class LastActionSensor(sensor.Sensor):
+ """A sensor that reports the last action taken."""
+
+ def __init__(self,
+ name: Text = "LastAction",
+ dtype: Type[Any] = np.float64,
+ sensor_latency: Union[float, Sequence[float]] = 0):
+ """Constructs LastActionSensor.
+
+ We do not provide a robot instance during __init__, as robot instances may
+ be reloaded/recreated during the simulation.
+
+ Args:
+ name: the name of the sensor
+ dtype: data type of sensor value.
+ sensor_latency: There are two ways to use this expected sensor latency.
+ For both methods, the latency should be in the same unit as the sensor
+ data timestamp. 1. As a single float number, the observation will be a
+ 1D array. For real robots, this should be set to 0.0. 2. As a array of
+ floats, the observation will be a 2D array based on how long the history
+ need to be. Thus, [0.0, 0.1, 0.2] is a history length of 3.
+ """
+ super().__init__(name=name,
+ sensor_latency=sensor_latency,
+ # We generally don't interpolate actions.
+ interpolator_fn=sensor.older_obs_blender)
+
+ self._dtype = dtype
+ self._env = None
+
+ def on_reset(self, env: gym.Env):
+ """From the callback, the sensor remembers the environment.
+
+ Args:
+ env: the environment who invokes this callback function.
+ """
+ # Constructs the observation space using the env's action space.
+ self._observation_space = self._stack_space(
+ env.action_space, dtype=self._dtype)
+
+ # Call the super class methods to initialize the buffers
+ super().on_reset(env)
+
+ def _get_original_observation(
+ self) -> Tuple[float, Union[Dict[Text, np.ndarray], np.ndarray]]:
+ return self._env.get_time(), space_utils.action_astype(
+ self._env.last_action, self._dtype)
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/sensors/motor_angle_sensor.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/sensors/motor_angle_sensor.py
new file mode 100644
index 000000000..ce1754547
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/sensors/motor_angle_sensor.py
@@ -0,0 +1,122 @@
+# Lint as: python3
+"""The on robot sensor classes."""
+
+from typing import Any, Callable, Iterable, Optional, Sequence, Type, Text, Tuple, Union
+
+import gin
+import gym
+import numpy as np
+
+from pybullet_envs.minitaur.envs_v2.sensors import sensor
+
+from pybullet_envs.minitaur.envs_v2.utilities import noise_generators
+
+
+def _convert_to_np_array(inputs: Union[float, Tuple[float], np.ndarray], dim):
+ """Converts the inputs to a numpy array.
+
+ Args:
+ inputs: The input scalar or array.
+ dim: The dimension of the converted numpy array.
+
+ Returns:
+ The converted numpy array.
+
+ Raises:
+ ValueError: If the inputs is an array whose dimension does not match the
+ provided dimension.
+ """
+ outputs = None
+ if isinstance(inputs, (tuple, np.ndarray)):
+ outputs = np.array(inputs)
+ else:
+ outputs = np.full(dim, inputs)
+
+ if len(outputs) != dim:
+ raise ValueError("The inputs array has a different dimension {}"
+ " than provided, which is {}.".format(len(outputs), dim))
+
+ return outputs
+
+
+@gin.configurable
+class MotorAngleSensor(sensor.Sensor):
+ """A sensor that reads motor angles from the robot."""
+
+ def __init__(self,
+ name: Text = "MotorAngle",
+ dtype: Type[Any] = np.float64,
+ lower_bound: Optional[Union[float, Iterable[float]]] = None,
+ upper_bound: Optional[Union[float, Iterable[float]]] = None,
+ noise_generator: Union[Callable[..., Any],
+ noise_generators.NoiseGenerator] = None,
+ sensor_latency: Union[float, Sequence[float]] = 0.0,
+ observe_sine_cosine: bool = False):
+ """Initializes the class.
+
+ Args:
+ name: The name of the sensor.
+ dtype: The datatype of this sensor.
+ lower_bound: The optional lower bounds of the sensor reading. If not
+ provided, will extract from the motor limits of the robot class.
+ upper_bound: The optional upper bounds of the sensor reading. If not
+ provided, will extract from the motor limits of the robot class.
+ noise_generator: Adds noise to the sensor readings.
+ sensor_latency: There are two ways to use this expected sensor latency.
+ For both methods, the latency should be in the same unit as the sensor
+ data timestamp. 1. As a single float number, the observation will be a
+ 1D array. For real robots, this should be set to 0.0. 2. As a array of
+ floats, the observation will be a 2D array based on how long the history
+ need to be. Thus, [0.0, 0.1, 0.2] is a history length of 3.
+ observe_sine_cosine: whether to observe motor angles as sine and cosine
+ values.
+ """
+ super().__init__(
+ name=name,
+ sensor_latency=sensor_latency,
+ interpolator_fn=sensor.linear_obs_blender)
+ self._noise_generator = noise_generator
+ self._dtype = dtype
+ self._lower_bound = lower_bound
+ self._upper_bound = upper_bound
+ self._observe_sine_cosine = observe_sine_cosine
+
+ def set_robot(self, robot):
+ self._robot = robot
+ # Creates the observation space based on the robot motor limitations.
+ if self._observe_sine_cosine:
+ lower_bound = _convert_to_np_array(-1, 2 * self._robot.num_motors)
+ elif self._lower_bound:
+ lower_bound = _convert_to_np_array(self._lower_bound,
+ self._robot.num_motors)
+ else:
+ lower_bound = _convert_to_np_array(
+ self._robot.motor_limits.angle_lower_limits, self._robot.num_motors)
+ if self._observe_sine_cosine:
+ upper_bound = _convert_to_np_array(1, 2 * self._robot.num_motors)
+ elif self._upper_bound:
+ upper_bound = _convert_to_np_array(self._upper_bound,
+ self._robot.num_motors)
+ else:
+ upper_bound = _convert_to_np_array(
+ self._robot.motor_limits.angle_upper_limits, self._robot.num_motors)
+
+ self._observation_space = self._stack_space(
+ gym.spaces.Box(low=lower_bound, high=upper_bound, dtype=self._dtype))
+
+ def _get_original_observation(self):
+ if self._observe_sine_cosine:
+ return self._robot.timestamp, np.hstack(
+ (np.cos(self._robot.motor_angles), np.sin(self._robot.motor_angles)))
+ else:
+ return self._robot.timestamp, self._robot.motor_angles
+
+ def get_observation(self) -> np.ndarray:
+ delayed_observation = super().get_observation()
+ if self._noise_generator:
+ if callable(self._noise_generator):
+ return self._noise_generator(delayed_observation)
+ else:
+ return self._noise_generator.add_noise(delayed_observation)
+
+ return delayed_observation
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/sensors/sensor.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/sensors/sensor.py
new file mode 100644
index 000000000..e3208cdc3
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/sensors/sensor.py
@@ -0,0 +1,451 @@
+# Lint as: python3
+"""A sensor prototype class.
+
+The concept is explained in: go/minitaur-gym-redesign-1.1
+"""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+from typing import Any, Iterable, Optional, Sequence, Text, Tuple, Union
+
+import gin
+import gym
+import numpy as np
+
+from pybullet_envs.minitaur.robots import robot_base
+from pybullet_envs.minitaur.robots import time_ordered_buffer
+
+_ARRAY = Sequence[float]
+_FloatOrArray = Union[float, _ARRAY]
+_DataTypeList = Iterable[Any]
+
+# For sensor with multiput outputs, key of the main observation in output dict.
+MAIN_OBS_KEY = ""
+
+# This allows referencing np.float32 in gin config files. For example:
+# lidar_sensor.LidarSensor.dtype = @np.float32
+gin.external_configurable(np.float32, module="np")
+gin.external_configurable(np.float64, module="np")
+gin.external_configurable(np.uint8, module="np")
+
+
+# Observation blenders take a pair of low/high values. The low/high is measured
+# by the latency of the observation. So the low value is actually newer in time
+# and high value older. The coeff [0, 1] can be thinked as the distance between
+# the low and high value value, with 0 being 100% low value and 1 as 100% high
+# value.
+def linear_obs_blender(low_value: Any, high_value: Any, coeff: float):
+ """Linear interpolation of low/high values based on coefficient value."""
+ return low_value * (1 - coeff) + high_value * coeff
+
+
+def closest_obs_blender(low_value: Any, high_value: Any, coeff: float):
+ """Choosing the high or low value based on coefficient value."""
+ return low_value if coeff < 0.5 else high_value
+
+
+def newer_obs_blender(low_value: Any, unused_high_value: Any,
+ unused_coeff: float):
+ """Always choosing low value, which is the newer value between low/high."""
+ return low_value
+
+
+def older_obs_blender(unused_low_value: Any, high_value: Any,
+ unused_coeff: float):
+ """Always choosing the high value, which is the older value between low/high."""
+ return high_value
+
+
+@gin.configurable
+class Sensor(object):
+ """A prototype class of sensors."""
+
+ def __init__(
+ self,
+ name: Text,
+ sensor_latency: _FloatOrArray,
+ interpolator_fn: Any,
+ enable_debug_visualization: bool = False,
+ ):
+ """A basic constructor of the sensor.
+
+ We do not provide a robot instance during __init__, as robot instances may
+ be reloaded/recreated during the simulation.
+
+ Args:
+ name: the name of the sensor
+ sensor_latency: There are two ways to use this expected sensor latency.
+ For both methods, the latency should be in the same unit as the sensor
+ data timestamp. 1. As a single float number, the observation will be a
+ 1D array. For real robots, this should be set to 0.0. 2. As an array of
+ floats, the observation will be a 2D array based on how long the history
+ need to be. Thus, [0.0, 0.1, 0.2] is a history length of 3. Observations
+ are stacked on a new axis appended after existing axes.
+ interpolator_fn: Function that controls how to interpolate the two values
+ that is returned from the time ordered buffer.
+ enable_debug_visualization: Whether to draw debugging visualization.
+ """
+ self._robot = None
+ self._name = name
+ # Observation space will be implemented by derived classes.
+ self._observation_space = None
+ self._sensor_latency = sensor_latency
+ self._single_latency = True if isinstance(sensor_latency,
+ (float, int)) else False
+ self._enable_debug_visualization = enable_debug_visualization
+ if not self._is_valid_latency():
+ raise ValueError("sensor_latency is expected to be a non-negative number "
+ "or a non-empty list of non-negative numbers.")
+ self._interpolator_fn = interpolator_fn or newer_obs_blender
+ self._axis = -1
+ timespan = sensor_latency if self._single_latency else max(sensor_latency)
+ self._observation_buffer = time_ordered_buffer.TimeOrderedBuffer(
+ max_buffer_timespan=timespan)
+
+ def _is_valid_latency(self):
+ if self._single_latency:
+ return self._sensor_latency >= 0
+ if self._sensor_latency:
+ return all(value >= 0 for value in self._sensor_latency)
+ return False
+
+ def get_name(self) -> Text:
+ return self._name
+
+ @property
+ def is_single_latency(self) -> bool:
+ return self._single_latency
+
+ @property
+ def observation_space(self) -> gym.spaces.Space:
+ return self._observation_space
+
+ @property
+ def enable_debug_visualization(self):
+ return self._enable_debug_visualization
+
+ @enable_debug_visualization.setter
+ def enable_debug_visualization(self, enable):
+ self._enable_debug_visualization = enable
+
+ def get_observation_datatype(self):
+ """Returns the data type for the numpy structured array.
+
+ It is recommended to define a list of tuples: (name, datatype, shape)
+ Reference: https://docs.scipy.org/doc/numpy-1.15.0/user/basics.rec.html
+ Ex:
+ return [('motor_angles', np.float64, (8, ))] # motor angle sensor
+ return [('IMU_x', np.float64), ('IMU_z', np.float64), ] # IMU
+ Will be deprecated (b/150818246) in favor of observation_space.
+
+ Returns:
+ datatype: a list of data types.
+ """
+ raise NotImplementedError("Deprecated. Are you using the old robot class?")
+
+ def get_lower_bound(self):
+ """Returns the lower bound of the observation.
+
+ Will be deprecated (b/150818246) in favor of observation_space.
+
+ Returns:
+ lower_bound: the lower bound of sensor values in np.array format
+ """
+ raise NotImplementedError("Deprecated. Are you using the old robot class?")
+
+ def get_upper_bound(self):
+ """Returns the upper bound of the observation.
+
+ Will be deprecated (b/150818246) in favor of observation_space.
+
+ Returns:
+ upper_bound: the upper bound of sensor values in np.array format
+ """
+ raise NotImplementedError("Deprecated. Are you using the old robot class?")
+
+ def _get_original_observation(self) -> Tuple[float, Any]:
+ """Gets the non-modified observation.
+
+ Different from the get_observation, which can pollute and sensor data with
+ noise and latency, this method shall return the best effort measurements of
+ the sensor. For simulated robots, this will return the clean data. For reals
+ robots, just return the measurements as is. All inherited class shall
+ implement this method.
+
+ Returns:
+ The timestamp and the original sensor measurements.
+
+ Raises:
+ NotImplementedError for the base class.
+
+ """
+ raise NotImplementedError("Not implemented for base class." "")
+
+ def get_observation(self):
+ """Returns the observation data.
+
+ Returns:
+ observation: the observed sensor values in np.array format
+ """
+ obs = self._observation_buffer.get_delayed_value(self._sensor_latency)
+
+ if self._single_latency:
+ if isinstance(self._observation_space, gym.spaces.Dict):
+ return self._interpolator_fn(obs.value_0, obs.value_1, obs.coeff)
+ else:
+ return np.asarray(
+ self._interpolator_fn(obs.value_0, obs.value_1, obs.coeff))
+ else:
+ if isinstance(self._observation_space, gym.spaces.Dict):
+ # interpolate individual sub observation
+ interpolated = [
+ self._interpolator_fn(data.value_0, data.value_1, data.coeff)
+ for data in obs
+ ]
+
+ stacked_per_sub_obs = {}
+ for k in interpolated[0]:
+ stacked_per_sub_obs[k] = np.stack(
+ np.asarray([d[k] for d in interpolated]), axis=self._axis)
+ return stacked_per_sub_obs
+ else:
+ obs = np.asarray([
+ self._interpolator_fn(data.value_0, data.value_1, data.coeff)
+ for data in obs
+ ])
+ return np.stack(obs, axis=self._axis)
+
+ def set_robot(self, robot: robot_base.RobotBase):
+ """Set a robot instance."""
+ self._robot = robot
+
+ def get_robot(self):
+ """Returns the robot instance."""
+ return self._robot
+
+ def on_reset(self, env):
+ """A callback function for the reset event.
+
+ Args:
+ env: the environment who invokes this callback function.
+ """
+ self._env = env
+ self._observation_buffer.reset()
+ self.on_new_observation()
+
+ def on_step(self, env):
+ """A callback function for the control step event.
+
+ Args:
+ env: the environment who invokes this callback function.
+ """
+ pass
+
+ def visualize(self):
+ """Visualizes the sensor information."""
+ pass
+
+ def on_new_observation(self):
+ """A callback for each observation received.
+
+ To be differentiated from on_step, which will be called only once per
+ control step (i.e. env.step), this API will be called everytime in the
+ substep/action repeat loop, when new observations are expected. Each derived
+ sensor class should implement this API by implementing:
+
+ my_obs = call env/robot api to get the observation
+ self._observation_buffer.add(my_obs)
+ """
+ timestamp, obs = self._get_original_observation()
+ if self._enable_debug_visualization:
+ self.visualize()
+ self._observation_buffer.add(timestamp, obs)
+
+ def on_terminate(self, env):
+ """A callback function for the terminate event.
+
+ Args:
+ env: the environment who invokes this callback function.
+ """
+ pass
+
+ def _stack_space(self,
+ space: Union[gym.spaces.Box, gym.spaces.Dict],
+ dtype: np.dtype = None) -> Any:
+ """Returns stacked version of observation space.
+
+ This stacks a gym.spaces.Box or gym.spaces.Dict action space based on the
+ length of the sensor latency and the axis for stacking specified in the
+ sensor. A gym.spaces.Box is just stacked, but a gym.spaces.Dict is
+ recursively stacked, preserving its dictionary structure while stacking
+ any gym.spaces.Box contained within. For example, the input action space:
+
+ gym.spaces.Dict({
+ 'space_1': gym.spaces.Box(low=0, high=10, shape=(1,)),
+ 'space_2': gym.spaces.Dict({
+ 'space_3': gym.spaces.Box(low=0, high=10, shape=(2,)),
+ }),
+ }))
+
+ would be converted to the following if sensor latency was [0, 1]:
+
+ gym.spaces.Dict({
+ 'space_1': gym.spaces.Box(low=0, high=10, shape=(1, 2)),
+ 'space_2': gym.spaces.Dict({
+ 'space_3': gym.spaces.Box(low=0, high=10, shape=(2, 2)),
+ }),
+ }))
+
+ Args:
+ space: A gym.spaces.Dict or gym.spaces.Box to be stacked.
+ dtype: Datatype for the stacking.
+
+ Returns:
+ stacked_space: A stacked version of the action space.
+ """
+ if self._single_latency:
+ return space
+
+ # Allow sensors such as last_action_sensor to override the dtype.
+ dtype = dtype or space.dtype
+
+ if isinstance(space, gym.spaces.Box):
+ return self._stack_space_box(space, dtype)
+ elif isinstance(space, gym.spaces.Dict):
+ return self._stack_space_dict(space, dtype)
+ else:
+ raise ValueError(f"Space {space} is an unsupported type.")
+
+ def _stack_space_box(self, space: gym.spaces.Box,
+ dtype: np.dtype) -> gym.spaces.Box:
+ """Returns stacked version of a box observation space.
+
+ This stacks a gym.spaces.Box action space based on the length of the sensor
+ latency and the axis for stacking specified in the sensor.
+
+ Args:
+ space: A gym.spaces.Box to be stacked.
+ dtype: Datatype for the stacking
+
+ Returns:
+ stacked_space: A stacked version of the gym.spaces.Box action space.
+ """
+ length = len(self._sensor_latency)
+ stacked_space = gym.spaces.Box(
+ low=np.repeat(
+ np.expand_dims(space.low, axis=self._axis), length,
+ axis=self._axis),
+ high=np.repeat(
+ np.expand_dims(space.high, axis=self._axis),
+ length,
+ axis=self._axis),
+ dtype=dtype)
+
+ return stacked_space
+
+ def _stack_space_dict(self, space: gym.spaces.Dict,
+ dtype: np.dtype) -> gym.spaces.Dict:
+ """Returns stacked version of a dict observation space.
+
+ This stacks a gym.spaces.Dict action space based on the length of the sensor
+ latency and the recursive structure of the gym.spaces.Dict itself.
+
+ Args:
+ space: A gym.spaces.Dict to be stacked.
+ dtype: Datatype for the stacking.
+
+ Returns:
+ stacked_space: A stacked version of the dictionary action space.
+ """
+ return gym.spaces.Dict([
+ (k, self._stack_space(v, dtype)) for k, v in space.spaces.items()
+ ])
+
+ def _encode_obs_dict_keys(self, obs_dict):
+ """Encodes sub obs keys of observation dict or observsation space dict."""
+ return {encode_sub_obs_key(self, k): v for k, v in obs_dict.items()}
+
+
+class BoxSpaceSensor(Sensor):
+ """A prototype class of sensors with Box shapes."""
+
+ def __init__(self,
+ name: Text,
+ shape: Tuple[int, ...],
+ lower_bound: _FloatOrArray = -np.pi,
+ upper_bound: _FloatOrArray = np.pi,
+ dtype=np.float64) -> None:
+ """Constructs a box type sensor.
+
+ Will be deprecated (b/150818246) once we switch to gym spaces.
+ Args:
+ name: the name of the sensor
+ shape: the shape of the sensor values
+ lower_bound: the lower_bound of sensor value, in float or np.array.
+ upper_bound: the upper_bound of sensor value, in float or np.array.
+ dtype: data type of sensor value
+ """
+ super(BoxSpaceSensor, self).__init__(
+ name=name, sensor_latency=0.0, interpolator_fn=newer_obs_blender)
+ self._shape = shape
+ self._dtype = dtype
+
+ if isinstance(lower_bound, float):
+ self._lower_bound = np.full(shape, lower_bound, dtype=dtype)
+ else:
+ self._lower_bound = np.array(lower_bound)
+
+ if isinstance(upper_bound, float):
+ self._upper_bound = np.full(shape, upper_bound, dtype=dtype)
+ else:
+ self._upper_bound = np.array(upper_bound)
+
+ def set_robot(self, robot):
+ # Since all old robot class do not inherit from RobotBase, we can enforce
+ # the checking here.
+ if isinstance(robot, robot_base.RobotBase):
+ raise ValueError(
+ "Cannot use new robot interface RobotBase with old sensor calss.")
+ self._robot = robot
+
+ def get_shape(self) -> Tuple[int, ...]:
+ return self._shape
+
+ def get_dimension(self) -> int:
+ return len(self._shape)
+
+ def get_dtype(self):
+ return self._dtype
+
+ def get_observation_datatype(self) -> _DataTypeList:
+ """Returns box-shape data type."""
+ return [(self._name, self._dtype, self._shape)]
+
+ def get_lower_bound(self) -> _ARRAY:
+ """Returns the computed lower bound."""
+ return self._lower_bound
+
+ def get_upper_bound(self) -> _ARRAY:
+ """Returns the computed upper bound."""
+ return self._upper_bound
+
+ def get_observation(self) -> np.ndarray:
+ return np.asarray(self._get_observation(), dtype=self._dtype)
+
+ def _get_original_observation(self) -> Tuple[float, Any]:
+ # Maintains compatibility with the new sensor classes."""
+ raise NotImplementedError("Not implemented for this class.")
+
+ def on_new_observation(self):
+ # Maintains compatibility with the new sensor classes."""
+ pass
+
+
+def encode_sub_obs_key(s: Sensor, sub_obs_name: Optional[Text]):
+ """Returns a sub observation key for use in observation dictionary."""
+ if sub_obs_name == MAIN_OBS_KEY:
+ return s.get_name()
+ else:
+ return f"{s.get_name()}/{sub_obs_name}"
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/sensors/space_utils.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/sensors/space_utils.py
new file mode 100644
index 000000000..270bee2f1
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/sensors/space_utils.py
@@ -0,0 +1,137 @@
+# Lint as: python3
+"""Converts a list of sensors to gym space."""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+from typing import List
+import gin
+import gym
+from gym import spaces
+import numpy as np
+
+from pybullet_envs.minitaur.envs_v2.sensors import sensor
+
+
+class UnsupportedConversionError(NotImplementedError):
+ """An exception when the function cannot convert sensors to the gym space."""
+
+
+class AmbiguousDataTypeError(TypeError):
+ """An exception when the function cannot determine the data type."""
+
+
+@gin.configurable
+def convert_sensors_to_gym_space(sensors: List[sensor.Sensor]) -> gym.Space:
+ """Convert a list of sensors to the corresponding gym space.
+
+ Args:
+ sensors: a list of the current sensors
+
+ Returns:
+ space: the converted gym space
+
+ Raises:
+ UnsupportedConversionError: raises when the function cannot convert the
+ given list of sensors.
+ """
+
+ if all([
+ isinstance(s, sensor.BoxSpaceSensor) and s.get_dimension() == 1
+ for s in sensors
+ ]):
+ return convert_1d_box_sensors_to_gym_space(sensors)
+ raise UnsupportedConversionError('sensors = ' + str(sensors))
+
+
+@gin.configurable
+def convert_1d_box_sensors_to_gym_space(
+ sensors: List[sensor.Sensor]) -> gym.Space:
+ """Convert a list of 1D BoxSpaceSensors to the corresponding gym space.
+
+ Args:
+ sensors: a list of the current sensors
+
+ Returns:
+ space: the converted gym space
+
+ Raises:
+ UnsupportedConversionError: raises when the function cannot convert the
+ given list of sensors.
+ AmbiguousDataTypeError: raises when the function cannot determine the
+ data types because they are not uniform.
+ """
+ # Check if all sensors are 1D BoxSpaceSensors
+ if not all([
+ isinstance(s, sensor.BoxSpaceSensor) and s.get_dimension() == 1
+ for s in sensors
+ ]):
+ raise UnsupportedConversionError('sensors = ' + str(sensors))
+
+ # Check if all sensors have the same data type
+ sensor_dtypes = [s.get_dtype() for s in sensors]
+ if sensor_dtypes.count(sensor_dtypes[0]) != len(sensor_dtypes):
+ raise AmbiguousDataTypeError('sensor datatypes are inhomogeneous')
+
+ lower_bound = np.concatenate([s.get_lower_bound() for s in sensors])
+ upper_bound = np.concatenate([s.get_upper_bound() for s in sensors])
+ observation_space = spaces.Box(
+ np.array(lower_bound), np.array(upper_bound), dtype=np.float32)
+ return observation_space
+
+
+@gin.configurable
+def convert_sensors_to_gym_space_dictionary(
+ sensors: List[sensor.Sensor]) -> gym.Space:
+ """Convert a list of sensors to the corresponding gym space dictionary.
+
+ Args:
+ sensors: a list of the current sensors
+
+ Returns:
+ space: the converted gym space dictionary
+
+ Raises:
+ UnsupportedConversionError: raises when the function cannot convert the
+ given list of sensors.
+ """
+ gym_space_dict = {}
+ for s in sensors:
+ if isinstance(s, sensor.BoxSpaceSensor):
+ gym_space_dict[s.get_name()] = spaces.Box(
+ np.array(s.get_lower_bound()),
+ np.array(s.get_upper_bound()),
+ dtype=np.float32)
+ elif isinstance(s, sensor.Sensor):
+ if isinstance(s.observation_space, spaces.Box):
+ gym_space_dict[s.get_name()] = s.observation_space
+ elif isinstance(s.observation_space, spaces.Dict):
+ gym_space_dict.update(s.observation_space.spaces)
+ else:
+ raise UnsupportedConversionError(
+ f'Unsupported space type {type(s.observation_space)}, '
+ f'must be Box or Dict. sensor = {s}')
+ else:
+ raise UnsupportedConversionError('sensors = ' + str(sensors))
+ return spaces.Dict(gym_space_dict)
+
+
+def create_constant_action(action_space, action_value=0):
+ """Create an uniform value action based on the type of action space."""
+ if isinstance(action_space, gym.spaces.Dict):
+ # gym.spaces.Dict has a shape of None, so construct action over subspaces.
+ return {
+ sub_name: create_constant_action(sub_space, action_value)
+ for sub_name, sub_space in action_space.spaces.items()
+ }
+ else: # Presumably gym.spaces.Box, but in case it is not ...
+ return np.full(action_space.shape, action_value)
+
+
+def action_astype(action, dtype):
+ """Transform an action to a different datatype."""
+ if isinstance(action, dict):
+ return {key: action_astype(value, dtype) for key, value in action.items()}
+ else:
+ return np.array(action, dtype=dtype)
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/sensors/toe_position_sensor.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/sensors/toe_position_sensor.py
new file mode 100644
index 000000000..5e875045a
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/sensors/toe_position_sensor.py
@@ -0,0 +1,100 @@
+# Lint as: python3
+"""Quadruped toe position sensor."""
+
+from typing import Any, Callable, Sequence, Text, Tuple, Type, Union
+
+import gin
+import gym
+import numpy as np
+
+from pybullet_envs.minitaur.envs_v2.sensors import sensor
+from pybullet_envs.minitaur.envs_v2.utilities import noise_generators
+
+
+def _convert_to_np_array(inputs: Union[float, Tuple[float], np.ndarray], dim):
+ """Converts the inputs to a numpy array.
+
+ Args:
+ inputs: The input scalar or array.
+ dim: The dimension of the converted numpy array.
+
+ Returns:
+ The converted numpy array.
+
+ Raises:
+ ValueError: If the inputs is an array whose dimension does not match the
+ provided dimension.
+ """
+ outputs = None
+ if isinstance(inputs, (tuple, np.ndarray)):
+ outputs = np.array(inputs)
+ else:
+ outputs = np.full(dim, inputs)
+
+ if len(outputs) != dim:
+ raise ValueError("The inputs array has a different dimension {}"
+ " than provided, which is {}.".format(len(outputs), dim))
+
+ return outputs
+
+
+@gin.configurable
+class ToePositionSensor(sensor.Sensor):
+ """A sensor that outputs the toe positions of attached robots or objects."""
+
+ def __init__(
+ self,
+ name: Text = "toe_position",
+ dtype: Type[Any] = np.float64,
+ lower_bound: Union[float, Sequence[float]] = -1.0,
+ upper_bound: Union[float, Sequence[float]] = 1.0,
+ noise_generator: Union[Callable[..., Any],
+ noise_generators.NoiseGenerator] = None,
+ sensor_latency: Union[float, Sequence[float]] = 0.0,
+ ):
+ """Constructor.
+
+ Args:
+ name: Name of the sensor.
+ dtype: Data type of sensor value.
+ lower_bound: The optional lower bounds of the sensor reading.
+ upper_bound: The optional upper bounds of the sensor reading.
+ noise_generator: Used to add noise to the readings.
+ sensor_latency: Single or multiple latency in seconds. See sensor.Sensor
+ docstring for details.
+ """
+ super().__init__(
+ name=name,
+ sensor_latency=sensor_latency,
+ interpolator_fn=sensor.linear_obs_blender)
+ self._dtype = dtype
+ self._lower_bound = lower_bound
+ self._upper_bound = upper_bound
+ self._noise_generator = noise_generator
+
+ def set_robot(self, robot):
+ self._robot = robot
+ num_legs = len(robot.urdf_loader.get_end_effector_id_dict().values())
+ lower_bound = _convert_to_np_array(self._lower_bound, num_legs * 3)
+
+ upper_bound = _convert_to_np_array(self._upper_bound, num_legs * 3)
+
+ self._observation_space = self._stack_space(
+ gym.spaces.Box(low=lower_bound, high=upper_bound, dtype=self._dtype))
+
+ def _get_original_observation(self) -> Tuple[float, np.ndarray]:
+ """Returns raw observation with timestamp."""
+ toe_position = np.array(
+ self._robot.foot_positions(), dtype=self._dtype).flatten()
+
+ return self._robot.timestamp, toe_position
+
+ def get_observation(self) -> np.ndarray:
+ delayed_observation = super().get_observation()
+ if self._noise_generator:
+ if callable(self._noise_generator):
+ return self._noise_generator(delayed_observation)
+ else:
+ return self._noise_generator.add_noise(delayed_observation)
+
+ return delayed_observation
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/tasks/__init__.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/tasks/__init__.py
new file mode 100644
index 000000000..e69de29bb
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/tasks/simple_locomotion_task.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/tasks/simple_locomotion_task.py
new file mode 100644
index 000000000..9046f9d8d
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/tasks/simple_locomotion_task.py
@@ -0,0 +1,125 @@
+"""A simple locomotion taskand termination condition."""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+import numpy as np
+
+import gin
+from pybullet_envs.minitaur.envs_v2.tasks import task_interface
+from pybullet_envs.minitaur.envs_v2.tasks import task_utils
+from pybullet_envs.minitaur.envs_v2.tasks import terminal_conditions
+from pybullet_envs.minitaur.envs_v2.utilities import env_utils_v2 as env_utils
+
+
+@gin.configurable
+class SimpleForwardTask(task_interface.Task):
+ """A basic "move forward" task."""
+
+ def __init__(self,
+ weight=1.0,
+ terminal_condition=terminal_conditions
+ .default_terminal_condition_for_minitaur,
+ divide_with_dt=False,
+ clip_velocity=None,
+ energy_penalty_coef=0.0,
+ min_com_height=None,
+ weight_action_accel=None):
+ """Initializes the task.
+
+ Args:
+ weight: Float. The scaling factor for the reward.
+ terminal_condition: Callable object or function. Determines if the task is
+ done.
+ divide_with_dt: if True, we divide the velocity reward with dt.
+ clip_velocity: if not None, we will clip the velocity with this value.
+ energy_penalty_coef: Coefficient for the energy penalty that will be added
+ to the reward. 0 by default.
+ min_com_height: Minimum height for the center of mass of the robot that
+ will be used to terminate the task. This is used to obtain task specific
+ gaits and set by the config or gin files based on the task and robot.
+ weight_action_accel: if not None, penalize the action acceleration.
+
+ Raises:
+ ValueError: The energey coefficient is smaller than zero.
+ """
+ self._weight = weight
+ self._terminal_condition = terminal_condition
+ self._last_base_position = None
+ self._divide_with_dt = divide_with_dt
+ self._clip_velocity = clip_velocity
+ self._weight_action_accel = weight_action_accel
+ self._action_history_sensor = None
+ self._min_com_height = min_com_height
+ self._energy_penalty_coef = energy_penalty_coef
+ self._env = None
+ self._step_count = 0
+ if energy_penalty_coef < 0:
+ raise ValueError("Energy Penalty Coefficient should be >= 0")
+
+ def __call__(self, env):
+ return self.reward(env)
+
+ def reset(self, env):
+ self._env = env
+ self._last_base_position = env_utils.get_robot_base_position(
+ self._env.robot)
+
+ if self._weight_action_accel is not None:
+ sensor_name = "LastAction"
+ self._action_history_sensor = env.sensor_by_name(sensor_name)
+
+ @property
+ def step_count(self):
+ return self._step_count
+
+ def update(self, env):
+ """Updates the internal state of the task."""
+ del env
+ self._last_base_position = env_utils.get_robot_base_position(
+ self._env.robot)
+
+ def reward(self, env):
+ """Get the reward without side effects."""
+ del env
+
+ self._step_count += 1
+ env = self._env
+ current_base_position = env_utils.get_robot_base_position(self._env.robot)
+ velocity = current_base_position[0] - self._last_base_position[0]
+ if self._divide_with_dt:
+ velocity /= env.env_time_step
+ if self._clip_velocity is not None:
+ limit = float(self._clip_velocity)
+ velocity = np.clip(velocity, -limit, limit)
+
+ if self._weight_action_accel is None:
+ action_acceleration_penalty = 0.0
+ else:
+ past_actions = self._action_history_sensor.get_observation().T
+ action = past_actions[0]
+ prev_action = past_actions[1]
+ prev_prev_action = past_actions[2]
+ acc = action - 2 * prev_action + prev_prev_action
+ action_acceleration_penalty = (
+ float(self._weight_action_accel) * np.mean(np.abs(acc)))
+
+ reward = velocity
+ reward -= action_acceleration_penalty
+
+ # Energy
+ if self._energy_penalty_coef > 0:
+ energy_reward = -task_utils.calculate_estimated_energy_consumption(
+ self._env.robot.motor_torques, self._env.robot.motor_velocities,
+ self._env.sim_time_step, self._env.num_action_repeat)
+ reward += energy_reward * self._energy_penalty_coef
+
+ return reward * self._weight
+
+ def done(self, env):
+ del env
+ position = env_utils.get_robot_base_position(self._env.robot)
+ if self._min_com_height and position[2] < self._min_com_height:
+ return True
+ return self._terminal_condition(self._env)
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/tasks/task_interface.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/tasks/task_interface.py
new file mode 100644
index 000000000..720d45010
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/tasks/task_interface.py
@@ -0,0 +1,37 @@
+# Lint as: python3
+"""Interface that specifies tasks."""
+# TODO(tingnan): Def. proper task interface - see TODO(b/154635313) in comments.
+
+import abc
+from typing import Sequence
+
+import gym
+
+from pybullet_envs.minitaur.envs_v2.sensors import sensor
+
+
+class Task(metaclass=abc.ABCMeta):
+ """Base class for tasks."""
+
+ # TODO(b/154635313): Deprecate this method. Consolidate it into update().
+ @abc.abstractmethod
+ def reward(self, env: gym.Env) -> float:
+ """Returns the reward for the current state of the environment."""
+
+ @abc.abstractmethod
+ def reset(self, env: gym.Env) -> None:
+ """Resets the task."""
+
+ @abc.abstractmethod
+ def update(self, env: gym.Env) -> None:
+ """Updates the internal state of the task."""
+
+ # TODO(b/154635313): Deprecate this method. Consolidate it into update().
+ @abc.abstractmethod
+ def done(self, env: gym.Env) -> bool:
+ """Determines whether the task is done."""
+
+ @property
+ def sensors(self) -> Sequence[sensor.Sensor]:
+ """Returns sensors used by task."""
+ return []
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/tasks/task_utils.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/tasks/task_utils.py
new file mode 100644
index 000000000..90ac2814a
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/tasks/task_utils.py
@@ -0,0 +1,108 @@
+"""Common tools and functionalities used in different tasks."""
+import numpy as np
+
+
+def calculate_target_speed_at_timestep(speed_stages):
+ """Interpolates the speed based on a speed profile and simulation steps.
+
+ Args:
+ speed_stages: The list of timesteps (in increasing order) and speed (float
+ or a list of floats for x and y components) at that specific timestep.
+ Example formats can be found on task_utils_test.py and at the header of
+ the speed_reward_task.py.
+
+ Returns:
+ Target speed for the specific step (meters per simulation step).
+ Raises:
+ ValueError if the input is not in the expected format.
+ """
+ if len(speed_stages) != 2 or len(speed_stages[0]) != len(speed_stages[1]):
+ raise ValueError('Speed stages for the task is not in correct format!')
+ steps = np.array(speed_stages[0])
+ speeds = np.array(speed_stages[1])
+ num_steps = steps[-1]
+ if len(speeds.shape) == 1:
+ return np.interp(range(num_steps), steps, speeds)
+ speed_at_timestep = np.interp(range(num_steps), steps, speeds[:, 0]).reshape(
+ (num_steps, 1))
+ speed_at_timestep = speed_at_timestep.reshape((num_steps, 1))
+ if speeds.shape[1] == 2:
+ speed_y = np.interp(range(num_steps), steps, speeds[:, 1]).reshape(
+ (num_steps, 1))
+ speed_at_timestep = np.concatenate((speed_at_timestep, speed_y), axis=1)
+ else:
+ speed_at_timestep = np.concatenate(
+ (speed_at_timestep, np.zeros((num_steps, 1))), axis=1)
+ return speed_at_timestep
+
+
+def calculate_distance(vector_1, vector_2):
+ """Calculates the distance between 2 vectors.
+
+ This is used to calculate distance between 2 points in 3D space as well as
+ distances between two orientation vectors.
+
+ Args:
+ vector_1: First vector that will be used for comparison with the other.
+ vector_2: Second vector used for comparison.
+
+ Returns:
+ Distance between the two vectors represented by a float.
+ """
+ return np.linalg.norm(np.array(vector_1) - np.array(vector_2))
+
+
+def turn_angle(new_vector, reference_vector):
+ """Calculates the change in orientation of the two vectors.
+
+ This is used to calculate the relative angle perception for the
+ robot.
+
+ Args:
+ new_vector: The front vector of the robot at current timestep.
+ reference_vector: The front vector of the robot at previous timestep.
+
+ Returns:
+ Angle representing the change in orientation of the robot projected to
+ x-y plane.
+ """
+ # Project the vectors to x-y plane
+ v1 = np.resize(new_vector, 3)
+ v2 = np.resize(reference_vector, 3)
+ v1[2] = 0
+ v2[2] = 0
+ # Calculate the right hand rotation between two vectors using z vector
+ # (0,0,-1) as reference cross product vector.
+ # Compatible with the yaw rotation of pyBullet.
+ return np.arctan2(np.dot(np.cross(v2, v1), (0, 0, -1)), np.dot(v1, v2))
+
+
+def front_vector(pybullet_client, orientation):
+ """Calculates the front vector of the robot on x-y plane.
+
+ Args:
+ pybullet_client: Pybullet client instantiation.
+ orientation: Orientation of the robot in quaternion form.
+
+ Returns:
+ 3D vector where z component is set to 0.
+ """
+ rot_matrix = pybullet_client.getMatrixFromQuaternion(orientation)
+ return [rot_matrix[0], -rot_matrix[1], 0]
+
+
+def calculate_estimated_energy_consumption(motor_torques, motor_velocities,
+ sim_time_step, num_action_repeat):
+ """Calculates energy consumption based on the args listed.
+
+ Args:
+ motor_torques: Torques of all the motors
+ motor_velocities: Velocities of all the motors.
+ sim_time_step: Simulation time step length (seconds).
+ num_action_repeat: How many steps the simulation repeats the same action.
+
+ Returns:
+ Total energy consumption of all the motors (watts).
+ """
+ return np.abs(np.dot(motor_torques,
+ motor_velocities)) * sim_time_step * num_action_repeat
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/tasks/terminal_conditions.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/tasks/terminal_conditions.py
new file mode 100644
index 000000000..9fe512057
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/tasks/terminal_conditions.py
@@ -0,0 +1,301 @@
+"""Contains the terminal conditions for locomotion tasks."""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+import gin
+import numpy as np
+
+from pybullet_envs.minitaur.envs_v2.utilities import minitaur_pose_utils
+from pybullet_envs.minitaur.envs_v2.utilities import env_utils_v2 as env_utils
+from pybullet_envs.minitaur.envs_v2.utilities import termination_reason as tr
+
+
+@gin.configurable
+def default_terminal_condition_for_minitaur(env):
+ """A default terminal condition for Minitaur.
+
+ Minitaur is considered as fallen if the base position is too low or the base
+ tilts/rolls too much.
+
+ Args:
+ env: An instance of MinitaurGymEnv
+
+ Returns:
+ A boolean indicating if Minitaur is fallen.
+ """
+ orientation = env_utils.get_robot_base_orientation(env.robot)
+ rot_mat = env.pybullet_client.getMatrixFromQuaternion(orientation)
+ local_up = rot_mat[6:]
+ pos = env_utils.get_robot_base_position(env.robot)
+ return (np.dot(np.asarray([0, 0, 1]), np.asarray(local_up)) < 0.85 or
+ pos[2] < 0.13)
+
+
+@gin.configurable
+def terminal_condition_for_minitaur_extended_env(env):
+ """Returns a bool indicating that the extended env is terminated.
+
+ This predicate checks whether 1) the legs are bent inward too much or
+ 2) the body is tilted too much.
+
+ Args:
+ env: An instance of MinitaurGymEnv
+ """
+ motor_angles = env.robot.motor_angles
+ leg_pose = minitaur_pose_utils.motor_angles_to_leg_pose(motor_angles)
+
+ swing_threshold = np.radians(35.0)
+ if (leg_pose[0] > swing_threshold or leg_pose[2] > swing_threshold or # Front
+ leg_pose[1] < -swing_threshold or leg_pose[3] < -swing_threshold): # Rear
+ return True
+ roll, _, _ = env.robot.base_roll_pitch_yaw
+ if abs(roll) > np.radians(30.0):
+ return True
+
+ return False
+
+
+@gin.configurable
+def default_terminal_condition_for_laikago(env):
+ """A default terminal condition for Laikago.
+
+ Minitaur is considered as fallen if the base position is too low or the base
+ tilts/rolls too much.
+
+ Args:
+ env: An instance of MinitaurGymEnv
+
+ Returns:
+ A boolean indicating if Minitaur is fallen.
+ """
+ roll, pitch, _ = env.robot.base_roll_pitch_yaw
+ pos = env_utils.get_robot_base_position(env.robot)
+ return abs(roll) > 0.2 or abs(pitch) > 0.2 or pos[2] < 0.35
+
+
+@gin.configurable
+def default_terminal_condition_for_laikago_v2(
+ env,
+ max_roll: float = 1.2,
+ max_pitch: float = 1.2,
+ min_height: float = 0.15,
+ enforce_foot_contacts: bool = False):
+ """A default terminal condition for Laikago_v2.
+
+ Laikago is considered as fallen if the base position is too low or the base
+ tilts/rolls too much.
+
+ Args:
+ env: An instance of MinitaurGymEnv
+ max_roll: Max roll before the episode terminates.
+ max_pitch: Max pitch before the episode terminates.
+ min_height: Min height before the episode terminates.
+ enforce_foot_contacts: Ensure that contacts are established with the feet.
+
+ Returns:
+ A boolean indicating if Minitaur is fallen.
+ """
+ # Make sure that contacts are only made with the robot's feet.
+ unwanted_collision = False
+ if enforce_foot_contacts:
+ # Get list of foot and knee link ids. Sometimes, the simulation will
+ # register a contact as having been made with the knee link, even though it
+ # was actually the foot that made the contact. Checking both the foot and
+ # knee links for contact accounts for that.
+ foot_link_ids = list(
+ env.robot.urdf_loader.get_end_effector_id_dict().values())
+ knee_link_ids = [foot_link_id - 1 for foot_link_id in foot_link_ids]
+ contacts = env.pybullet_client.getContactPoints(bodyA=env.robot.robot_id)
+ for contact in contacts:
+ # Two different bodies made contact (i.e. not a self-collision).
+ if contact[1] != contact[2]:
+ foot_contact = (contact[3] in foot_link_ids) or (
+ contact[3] in knee_link_ids)
+ unwanted_collision = unwanted_collision or not foot_contact
+
+ roll, pitch, _ = env.robot.base_roll_pitch_yaw
+ pos = env.robot.base_position
+ return (abs(roll) > max_roll or abs(pitch) > max_pitch or
+ pos[2] < min_height or unwanted_collision)
+
+
+@gin.configurable
+def default_terminal_condition_for_agility(env,
+ max_roll: float = 1.8,
+ max_pitch: float = 1.8,
+ min_height: float = 0.0,
+ enforce_foot_contacts: bool = False):
+ """A default terminal condition for more agile tasks (i.e. jumping).
+
+ The robot is considered as fallen if the base position is too low, the base
+ tilts/rolls too much or parts of the body other than the feet touch the
+ ground.
+
+ Args:
+ env: An instance of the gym env.
+ max_roll: Max roll before the episode terminates.
+ max_pitch: Max pitch before the episode terminates.
+ min_height: Min height before the episode terminates.
+ enforce_foot_contacts: Ensure that contacts are established with the feet.
+
+ Returns:
+ A boolean indicating if the episode should be terminated.
+ """
+
+ # Make sure that contacts are only made with the robot's feet.
+ unwanted_collision = False
+ if enforce_foot_contacts:
+ knee_link_ids = [2, 5, 8, 11]
+ contacts = env.pybullet_client.getContactPoints(bodyA=env.robot.robot_id)
+ for contact in contacts:
+ if contact[1] != contact[2]:
+ foot_contact = contact[3] in knee_link_ids
+ unwanted_collision = unwanted_collision or not foot_contact
+
+ roll, pitch, _ = env.robot.base_roll_pitch_yaw
+ pos = env.robot.base_position
+ return (abs(roll) > max_roll or abs(pitch) > max_pitch or
+ pos[2] < min_height or unwanted_collision)
+
+
+@gin.configurable
+def get_terminal_reason(collisions, task):
+ termination_reason = None
+ # Checking collision termination
+ if collisions:
+ termination_reason = tr.TerminationReason.AGENT_COLLISION
+ if task.is_task_success():
+ termination_reason = tr.TerminationReason.GOAL_REACHED
+ return termination_reason
+
+
+@gin.configurable
+def maxstep_terminal_condition(env, max_step=500):
+ """A terminal condition based on the time step.
+
+ Args:
+ env: An instance of MinitaurGymEnv
+ max_step: The maximum time step allowed for the environment
+
+ Returns:
+ A boolean indicating if the env.step exceeds the given limit
+ """
+ return env.env_step_counter > max_step
+
+
+@gin.configurable
+class MaxTimeTerminalCondition(object):
+ """Terminal condition based on time, independent of step length."""
+
+ def __init__(self, max_time_s: float):
+ """Initializes the MaxTimeTerminalCondition.
+
+ Args:
+ max_time_s: Time limit in seconds. In sim, this is the simulation time,
+ not wall time.
+ """
+ if max_time_s <= 0:
+ raise ValueError("Max time for MaxTimeTerminalCondition cannot be zero "
+ "or less. Input value: %s" % max_time_s)
+ self._max_time_s = max_time_s
+
+ def __call__(self, env):
+ if self._max_time_s is None:
+ return False, None
+ is_done = env.get_time_since_reset() >= self._max_time_s
+ term_reason = tr.TerminationReason.RUN_TIME_LIMIT if is_done else None
+ return is_done, term_reason
+
+
+@gin.configurable
+class MovementDetectorTerminalCondition(object):
+ """Terminal condition for not moving past a distance in specified time."""
+
+ def __init__(self,
+ max_time_s: float = None,
+ min_travel_distance_m: float = 1.0):
+ """Initializes the MovementDetectorTerminalCondition.
+
+ Args:
+ max_time_s: Time limit in seconds. In sim, this is the simulation time,
+ not wall time.
+ min_travel_distance_m: Minimum distance in meters to travel in time limit.
+ """
+ if max_time_s is not None and max_time_s <= 0:
+ raise ValueError("Max time for MovementDetectorTerminalCondition cannot "
+ "be zero or less. Input value: %s" % max_time_s)
+ if min_travel_distance_m is not None and min_travel_distance_m < 0:
+ raise ValueError(
+ "Minimum travel distance for MovementDetectorTerminalCondition "
+ "cannot be less than zero. Input value: %s" % min_travel_distance_m)
+ self._max_time_s = max_time_s
+ self._min_travel_distance_m = min_travel_distance_m
+ self._not_advancing_limit = None
+ self._reference_position = None
+
+ def _update_limit_time(self, env):
+ self._not_advancing_limit = env.get_time_since_reset() + self._max_time_s
+
+ def _current_position(self, env):
+ return np.asarray(env.robot.base_position[:2])
+
+ def _exceed_time_limit(self, env):
+ return self._not_advancing_limit < env.get_time_since_reset()
+
+ def __call__(self, env):
+ is_done = False
+ term_reason = None
+
+ if self._max_time_s is None:
+ return is_done, term_reason
+
+ if self._not_advancing_limit is None or self._reference_position is None:
+ self._update_limit_time(env)
+ self._reference_position = self._current_position(env)
+
+ distance_shifted = np.linalg.norm(
+ self._current_position(env) - self._reference_position)
+ if distance_shifted >= self._min_travel_distance_m:
+ self._update_limit_time(env)
+ self._reference_position = self._current_position(env)
+
+ if self._exceed_time_limit(env):
+ is_done = True
+ term_reason = tr.TerminationReason.NOT_ADVANCING_LIMIT
+
+ return is_done, term_reason
+
+
+
+@gin.configurable
+def bad_front_leg_terminal_condition(env, max_angle=0.8):
+ """A terminal condition for checking whether front legs are bent backward.
+
+ Args:
+ env: An instance of MinitaurGymEnv
+ max_angle: The maximum angle allowed for front legs
+
+ Returns:
+ A boolean indicating if front legs are bent backward or not
+ """
+ motor_angles = env.robot.motor_angles
+ leg_pose = minitaur_pose_utils.motor_angles_to_leg_pose(motor_angles)
+ swing0 = leg_pose[0]
+ swing1 = leg_pose[2]
+ return swing0 > max_angle or swing1 > max_angle
+
+
+@gin.configurable
+def logical_any_terminal_condition(env, conditions):
+ """A logical "Any" operator for terminal conditions.
+
+ Args:
+ env: An instance of MinitaurGymEnv
+ conditions: a list of terminal conditions
+
+ Returns:
+ A boolean indicating if any of terminal conditions is satisfied
+ """
+ return any([cond(env) for cond in conditions])
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
new file mode 100644
index 000000000..f82283bf3
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/utilities/env_utils.py
@@ -0,0 +1,175 @@
+"""Utility functions to manipulate environment."""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+import collections
+import gin
+from gym import spaces
+import numpy as np
+
+import tensorflow.compat.v1 as tf
+
+
+
+
+def flatten_observations(observation_dict, observation_excluded=()):
+ """Flattens the observation dictionary to an array.
+
+ If observation_excluded is passed in, it will still return a dictionary,
+ which includes all the (key, observation_dict[key]) in observation_excluded,
+ and ('other': the flattened array).
+
+ Args:
+ observation_dict: A dictionary of all the observations.
+ observation_excluded: A list/tuple of all the keys of the observations to be
+ ignored during flattening.
+
+ Returns:
+ An array or a dictionary of observations based on whether
+ observation_excluded is empty.
+ """
+ if not isinstance(observation_excluded, (list, tuple)):
+ observation_excluded = [observation_excluded]
+ observations = []
+ for key, value in observation_dict.items():
+ if key not in observation_excluded:
+ observations.append(np.asarray(value).flatten())
+ flat_observations = np.float32(np.concatenate(observations))
+ if not observation_excluded:
+ return flat_observations
+ else:
+ observation_dict_after_flatten = {"other": flat_observations}
+ for key in observation_excluded:
+ if key in observation_dict:
+ observation_dict_after_flatten[key] = observation_dict[key]
+ return collections.OrderedDict(
+ sorted(list(observation_dict_after_flatten.items())))
+
+
+def flatten_observation_dim(observation_dim, observation_excluded=()):
+ """Flattens the observation dimensions to an array.
+
+ If observation_excluded is passed in, it will still return a dictionary,
+ which includes all the (key, observation_dict[key]) in observation_excluded,
+ and ('other': the flattened array).
+
+ Args:
+ observation_dim: A dictionary of all the observation dimensions.
+ observation_excluded: A list/tuple of all the keys of the observations to be
+ ignored during flattening.
+
+ Returns:
+ An array or a dictionary of observation dimensions based on whether
+ observation_excluded is empty.
+ """
+ if not isinstance(observation_excluded, (list, tuple)):
+ observation_excluded = [observation_excluded]
+ observation_dims = 0
+ for key, value in observation_dim.items():
+ if key not in observation_excluded:
+ observation_dims += value
+ if not observation_excluded:
+ return observation_dims
+ else:
+ dim_dict_after_flatten = {"other": observation_dims}
+ for key in observation_excluded:
+ if key in observation_dim:
+ dim_dict_after_flatten[key] = observation_dim[key]
+ return collections.OrderedDict(sorted(list(dim_dict_after_flatten.items())))
+
+
+def flatten_observation_spaces(observation_spaces, observation_excluded=()):
+ """Flattens the dictionary observation spaces to gym.spaces.Box.
+
+ If observation_excluded is passed in, it will still return a dictionary,
+ which includes all the (key, observation_spaces[key]) in observation_excluded,
+ and ('other': the flattened Box space).
+
+ Args:
+ observation_spaces: A dictionary of all the observation spaces.
+ observation_excluded: A list/tuple of all the keys of the observations to be
+ ignored during flattening.
+
+ Returns:
+ A box space or a dictionary of observation spaces based on whether
+ observation_excluded is empty.
+ """
+ if not isinstance(observation_excluded, (list, tuple)):
+ observation_excluded = [observation_excluded]
+ lower_bound = []
+ upper_bound = []
+ for key, value in observation_spaces.spaces.items():
+ if key not in observation_excluded:
+ lower_bound.append(np.asarray(value.low).flatten())
+ upper_bound.append(np.asarray(value.high).flatten())
+ lower_bound = np.concatenate(lower_bound)
+ upper_bound = np.concatenate(upper_bound)
+ observation_space = spaces.Box(
+ np.array(lower_bound), np.array(upper_bound), dtype=np.float32)
+ if not observation_excluded:
+ return observation_space
+ else:
+ observation_spaces_after_flatten = {"other": observation_space}
+ for key in observation_excluded:
+ if key in observation_spaces.spaces:
+ observation_spaces_after_flatten[key] = observation_spaces[key]
+ return spaces.Dict(observation_spaces_after_flatten)
+
+
+
+@gin.configurable
+def get_action_spec(action_spec):
+ """Get action spec for one agent from the environment specs."""
+ return list(action_spec.values())[0]
+
+
+
+@gin.configurable
+def get_get_actions_fn(agent_name_to_index):
+ """Get function which returns other agents' actions."""
+
+ def get_actions(action):
+ """Returns a list of actions.
+
+ Args:
+ action: A dictionary of action tensors with keys matching agent names
+
+ Returns:
+ critic_actions: A list of action tensors for (N-1) other agents.
+ Shape: (B x N, N-1, D)
+ """
+ critic_actions = []
+ for agent_name in sorted(agent_name_to_index.keys()):
+ other_agent_actions = []
+ for other_agent_name in sorted(agent_name_to_index.keys()):
+ if other_agent_name != agent_name:
+ other_agent_actions.append(action[other_agent_name])
+ critic_actions.append(other_agent_actions)
+ print([tf.shape(critic_action) for critic_action in critic_actions])
+ # Shape goes from (N, N-1, B, D) to (B x N, N-1, D)
+ critic_actions = tf.transpose(tf.concat(critic_actions, axis=1), (1, 0, 2))
+ return critic_actions
+
+ return get_actions
+
+
+
+
+def get_robot_base_position(robot):
+ """Gets the base position of robot."""
+ # TODO(b/151975607): Clean this after robot interface migration.
+ if hasattr(robot, "GetBasePosition"):
+ return robot.GetBasePosition()
+ else:
+ return robot.base_position
+
+
+def get_robot_base_orientation(robot):
+ """Gets the base orientation of robot."""
+ # TODO(b/151975607): Clean this after robot interface migration.
+ if hasattr(robot, "GetBaseOrientation"):
+ return robot.GetBaseOrientation()
+ else:
+ return robot.base_orientation_quaternion
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/utilities/env_utils_v2.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/utilities/env_utils_v2.py
new file mode 100644
index 000000000..02d2eeaef
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/utilities/env_utils_v2.py
@@ -0,0 +1,22 @@
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+
+def get_robot_base_position(robot):
+ """Gets the base position of robot."""
+ # TODO(b/151975607): Clean this after robot interface migration.
+ if hasattr(robot, "GetBasePosition"):
+ return robot.GetBasePosition()
+ else:
+ return robot.base_position
+
+
+def get_robot_base_orientation(robot):
+ """Gets the base orientation of robot."""
+ # TODO(b/151975607): Clean this after robot interface migration.
+ if hasattr(robot, "GetBaseOrientation"):
+ return robot.GetBaseOrientation()
+ else:
+ return robot.base_orientation_quaternion
\ No newline at end of file
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/utilities/laikago_pose_utils.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/utilities/laikago_pose_utils.py
new file mode 100644
index 000000000..f328c3418
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/utilities/laikago_pose_utils.py
@@ -0,0 +1,50 @@
+# coding=utf-8
+# Copyright 2020 The Google Research Authors.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+"""Utility functions to calculate Laikago's pose and motor angles."""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+import attr
+
+LAIKAGO_DEFAULT_ABDUCTION_ANGLE = 0
+LAIKAGO_DEFAULT_HIP_ANGLE = 0.67
+LAIKAGO_DEFAULT_KNEE_ANGLE = -1.25
+
+
+@attr.s
+class LaikagoPose(object):
+ """Default pose of the Laikago.
+
+ Leg order:
+ 0 -> Front Right.
+ 1 -> Front Left.
+ 2 -> Rear Right.
+ 3 -> Rear Left.
+ """
+ abduction_angle_0 = attr.ib(type=float, default=0)
+ hip_angle_0 = attr.ib(type=float, default=0)
+ knee_angle_0 = attr.ib(type=float, default=0)
+ abduction_angle_1 = attr.ib(type=float, default=0)
+ hip_angle_1 = attr.ib(type=float, default=0)
+ knee_angle_1 = attr.ib(type=float, default=0)
+ abduction_angle_2 = attr.ib(type=float, default=0)
+ hip_angle_2 = attr.ib(type=float, default=0)
+ knee_angle_2 = attr.ib(type=float, default=0)
+ abduction_angle_3 = attr.ib(type=float, default=0)
+ hip_angle_3 = attr.ib(type=float, default=0)
+ knee_angle_3 = attr.ib(type=float, default=0)
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/utilities/mini_cheetah_pose_utils.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/utilities/mini_cheetah_pose_utils.py
new file mode 100644
index 000000000..37d12fe95
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/utilities/mini_cheetah_pose_utils.py
@@ -0,0 +1,35 @@
+"""Utility functions to calculate Mini Cheetah's pose and motor angles."""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+import attr
+
+MINI_CHEETAH_DEFAULT_ABDUCTION_ANGLE = 0
+MINI_CHEETAH_DEFAULT_HIP_ANGLE = -1.2
+MINI_CHEETAH_DEFAULT_KNEE_ANGLE = 2.1
+
+
+@attr.s
+class MiniCheetahPose(object):
+ """Default pose of the Laikago.
+
+ Leg order:
+ 0 -> Front Right.
+ 1 -> Front Left.
+ 2 -> Rear Right.
+ 3 -> Rear Left.
+ """
+ abduction_angle_0 = attr.ib(type=float, default=0)
+ hip_angle_0 = attr.ib(type=float, default=0)
+ knee_angle_0 = attr.ib(type=float, default=0)
+ abduction_angle_1 = attr.ib(type=float, default=0)
+ hip_angle_1 = attr.ib(type=float, default=0)
+ knee_angle_1 = attr.ib(type=float, default=0)
+ abduction_angle_2 = attr.ib(type=float, default=0)
+ hip_angle_2 = attr.ib(type=float, default=0)
+ knee_angle_2 = attr.ib(type=float, default=0)
+ abduction_angle_3 = attr.ib(type=float, default=0)
+ hip_angle_3 = attr.ib(type=float, default=0)
+ knee_angle_3 = attr.ib(type=float, default=0)
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/utilities/minitaur_pose_utils.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/utilities/minitaur_pose_utils.py
new file mode 100644
index 000000000..34ad9da01
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/utilities/minitaur_pose_utils.py
@@ -0,0 +1,187 @@
+# coding=utf-8
+# Copyright 2020 The Google Research Authors.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+"""Utility functions to calculate Minitaur's pose and motor angles."""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+import math
+import attr
+import numpy as np
+
+NUM_MOTORS = 8
+NUM_LEGS = 4
+MOTOR_SIGNS = (1, 1, -1, -1)
+# Constants for the function swing_extend_to_motor_angles
+EPS = 0.1
+# Range of motion for the legs (does not allow pointing towards the body).
+LEG_SWING_LIMIT_LOW = -math.pi / 2 + EPS
+LEG_SWING_LIMIT_HIGH = 3 * math.pi / 2 - EPS
+# Range of gap between motors for feasibility.
+MOTORS_GAP_LIMIT_HIGH = 2 * math.pi - EPS
+MOTORS_GAP_LIMIT_LOW = EPS
+
+
+@attr.s
+class MinitaurPose(object):
+ """Default pose of the Minitaur."""
+ swing_angle_0 = attr.ib(type=float, default=0)
+ swing_angle_1 = attr.ib(type=float, default=0)
+ swing_angle_2 = attr.ib(type=float, default=0)
+ swing_angle_3 = attr.ib(type=float, default=0)
+ extension_angle_0 = attr.ib(type=float, default=0)
+ extension_angle_1 = attr.ib(type=float, default=0)
+ extension_angle_2 = attr.ib(type=float, default=0)
+ extension_angle_3 = attr.ib(type=float, default=0)
+
+
+def motor_angles_to_leg_pose(motor_angles):
+ """Convert motor angles to the leg pose.
+
+ A single leg pose is a tuple (swing, extension). The definition can be find
+ in:
+ Sim-to-Real: Learning Agile Locomotion For Quadruped Robot
+
+ Args:
+ motor_angles: A numpy array. Contains all eight motor angles for Minitaur.
+
+ Returns:
+ A numpy array. Contains the leg pose for all four legs: [swing_0, swing_1,
+ swing_2, swing_3, extension_0, extension_1, extension_2, extension_3]
+
+ """
+ motor_angles = np.array(motor_angles)
+
+ swings = 0.5 * np.multiply(
+ np.array(MOTOR_SIGNS), (motor_angles[1::2] - motor_angles[::2]))
+ extensions = 0.5 * (motor_angles[::2] + motor_angles[1::2])
+
+ return np.concatenate((swings, extensions), axis=None)
+
+
+def leg_pose_to_motor_angles(leg_pose):
+ """Converts the leg pose to the motor angles.
+
+ Args:
+ leg_pose: A numpy array. Contains the leg pose for all four legs: [swing_0,
+ swing_1, swing_2, swing_3, extension_0, extension_1, extension_2,
+ extension_3]
+
+ Returns:
+ A numpy array. All eight motor angles.
+ """
+ leg_pose = np.array(leg_pose)
+
+ # swings multiplied with the sign array.
+ signed_swings = np.multiply(np.array(MOTOR_SIGNS), leg_pose[0:NUM_LEGS])
+ extensions = leg_pose[NUM_LEGS:]
+
+ motor_angles = np.zeros(NUM_MOTORS)
+ motor_angles[1::2] = signed_swings + extensions
+ motor_angles[::2] = extensions - signed_swings
+ return motor_angles
+
+
+# This method also does the same conversion, but 0 swing and 0 extension maps
+# to a neutral standing still motor positions with motors at + or - pi. It also
+# contains a safety layer so that the legs don't swing or extend too much to hit
+# the body of the robot.
+def leg_pose_to_motor_angles_with_half_pi_offset_and_safety(leg_pose):
+ """Converts the swing extension poses to the motor angles with safety limits.
+
+ Args:
+ leg_pose: A numpy array. Contains the leg pose for all four legs: [swing_0,
+ extension_0, swing_1, extension_1, swing_2, extension_2, swing_3,
+ extension_3]
+
+ Returns:
+ A numpy array. All eight motor angles.
+ """
+
+ motor_angles = []
+ for idx in range(4):
+ swing = leg_pose[idx * 2]
+ extend = leg_pose[idx * 2 + 1]
+ motor_angles.extend(swing_extend_to_motor_angles(idx, swing, extend))
+ return motor_angles
+
+
+def swing_extend_to_motor_angles(leg_id, swing, extension, noise_stdev=0):
+ """Swing - extension based leg model for minitaur.
+
+ Swing extension leg model calculates motor positions using 2 separate motions:
+ swing and extension. Swing rotates the whole leg by rotating both motors
+ equally towards same direction. Extension increases or decreases the length
+ of the leg by turning both motors equally in opposite direction.
+
+ This method also does the same conversion as leg_pose_to_motor_angles, but 0
+ swing and 0 extension maps to a neutral standing still motor positions with
+ motors at + or - pi.
+ Args:
+ leg_id: The id of the leg that the conversion is made for (0, 1, 2, 3).
+ swing: Swing degree for the leg (in radians). 0 means perpendicular to the
+ body).
+ extension: Extension level (length) of the leg, limited to [-1, 1].
+ noise_stdev: Standard deviation of the introduced noise at the motor
+ position level. Noise is turned off by default.
+
+ Returns:
+ motor0: Position for the first motor for that leg.
+ motor1: Position for the second motor for that leg.
+ Raises:
+ ValueError: In case calculated positions are outside the allowed boundaries.
+ """
+ # Check if the leg_id is in valid range
+ if not 0 <= leg_id <= 3:
+ raise ValueError('leg {} does not exist for a quadruped.'.format(leg_id))
+
+ # Front legs can not swing too much towards the body.
+ if leg_id % 2 == 0:
+ swing = np.clip(swing, LEG_SWING_LIMIT_LOW, LEG_SWING_LIMIT_HIGH)
+ # Back legs can not swing too much towards the body (opposite direction).
+ else:
+ swing = np.clip(swing, -LEG_SWING_LIMIT_HIGH, -LEG_SWING_LIMIT_LOW)
+
+ # Check if the motors are too close or too far away to make it impossible
+ # for the physical robot.
+ gap = math.pi - 2 * extension
+ if gap < MOTORS_GAP_LIMIT_LOW or gap > MOTORS_GAP_LIMIT_HIGH:
+ top_extension = (math.pi - MOTORS_GAP_LIMIT_LOW) / 2.0
+ least_extension = (math.pi - MOTORS_GAP_LIMIT_HIGH) / 2.0
+ extension = np.clip(extension, least_extension, top_extension)
+
+ # Initialization to neutral standing position where both motors point to
+ # opposite directions
+ motor0 = math.pi / 2
+ motor1 = math.pi / 2
+ # Rotational move
+ if leg_id in (0, 1):
+ motor0 += swing
+ motor1 -= swing
+ elif leg_id in (2, 3):
+ motor0 -= swing
+ motor1 += swing
+ # Extension
+ motor0 += extension
+ motor1 += extension
+
+ # Add noise if requested.
+ if noise_stdev > 0:
+ motor0 += np.random.normal(0, noise_stdev)
+ motor1 += np.random.normal(0, noise_stdev)
+
+ return motor0, motor1
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/utilities/noise_generators.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/utilities/noise_generators.py
new file mode 100644
index 000000000..012a93421
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/utilities/noise_generators.py
@@ -0,0 +1,255 @@
+"""Noise generators to simulate noise in sensor / actuator classes."""
+
+import abc
+import gin
+import numpy as np
+
+
+class NoiseGenerator(metaclass=abc.ABCMeta):
+ """Base class for noise generators."""
+
+ @abc.abstractmethod
+ def _get_noise(self, shape, dtype=None):
+ """Gets noise as a numpy array in the specified shape and dtype.
+
+ Tensorflow requires the shape and dtype of noise to be correctly specified,
+ so the generator needs to know this to produce data of the correct type.
+
+ Args:
+ shape: Shape of the returned array.
+ dtype: Datatype of returned array (None for default).
+ """
+
+ @abc.abstractmethod
+ def add_noise(self, data):
+ """Adds noise generated by _get_noise to the given data with clipping.
+
+ Args:
+ data: Numpy array of data to be modified with noise.
+ """
+
+
+@gin.configurable
+class BiasNoise(NoiseGenerator):
+ """Adds bias to the data, possibly with clipping."""
+
+ def __init__(self,
+ bias=0.0,
+ clipping_lower_bound=-np.inf,
+ clipping_upper_bound=np.inf):
+ """Create a bias noise generator.
+
+ Args:
+ bias: Absolute magnitude of bias applied to input.
+ clipping_lower_bound: lower bound of add_noise (use -np.inf to ignore).
+ clipping_upper_bound: Upper bound of add_noise (use np.inf to ignore).
+ """
+ self._bias = bias
+ self._clipping_lower_bound = clipping_lower_bound
+ self._clipping_upper_bound = clipping_upper_bound
+
+ def _get_noise(self, shape, dtype=None):
+ """Create bias noise of the given direction and datatype."""
+ return np.full(shape, self._bias, dtype)
+
+ def add_noise(self, data):
+ """Add bias noise to the given data, clipping to the given range."""
+ noise = self._get_noise(data.shape, data.dtype)
+ return np.clip(data + noise, self._clipping_lower_bound,
+ self._clipping_upper_bound)
+
+
+@gin.configurable
+class NormalNoise(BiasNoise):
+ """Adds Gaussian noise to the data, possibly with clipping."""
+
+ def __init__(self, scale, **kwargs):
+ """Create a normal noise generator.
+
+ Args:
+ scale: Absolute magnitude of standard deviation of Gaussian noise. Note
+ numpy will throw an error if scale < 0.
+ **kwargs: Arguments passed to BiasNoise (e.g. bias and clipping).
+ """
+ super(NormalNoise, self).__init__(**kwargs)
+ self._scale = scale
+
+ def _get_noise(self, shape, dtype=None):
+ """Create normal noise of the given direction and datatype."""
+ return np.random.normal(self._bias, self._scale, shape).astype(dtype)
+
+
+@gin.configurable
+class UniformNoise(NoiseGenerator):
+ """Generates uniform noise in the given range."""
+
+ def __init__(self,
+ low,
+ high,
+ clipping_lower_bound=-np.inf,
+ clipping_upper_bound=np.inf):
+ """Creates a uniform noise generator.
+
+ Args:
+ low: the lower bound of the noise.
+ high: the higher bound of the noise.
+ clipping_lower_bound: lower bound of add_noise (use -np.inf to ignore).
+ clipping_upper_bound: Upper bound of add_noise (use np.inf to ignore).
+ """
+ super().__init__()
+ self._low = low
+ self._high = high
+ self._clipping_lower_bound = clipping_lower_bound
+ self._clipping_upper_bound = clipping_upper_bound
+
+ def _get_noise(self, shape, dtype=None):
+ """Generates a noise using the given shape and data type."""
+ return np.random.uniform(self._low, self._high, shape).astype(dtype)
+
+ def add_noise(self, data):
+ """Adds noise to the given data, clipping to the given bound."""
+ noise = self._get_noise(data.shape, data.dtype)
+ return np.clip(data + noise, self._clipping_lower_bound,
+ self._clipping_upper_bound)
+
+
+@gin.configurable
+class RangeNoise(NormalNoise):
+ """Add normally distributed noise in m, applied to hit fractions in (0, 1).
+
+ This enables us to specify range noise in terms of meters of Gaussian noise
+ between a maximum and minimum range, but the add_noise is applied as above
+ to values expected to be in a hit fraction range of (0, 1) as needed for the
+ SimLidarSensor API. Separate methods return noise or noisify data in meters.
+ """
+
+ def __init__(self, range_noise_m, max_range_m, min_range_m=0.0, **kwargs):
+ """Create a normal noise generator suitable for use in a range scanner.
+
+ Args:
+ range_noise_m: Absolute magnitude of standard deviation of Gaussian noise,
+ applied to range observation readngs, measured in meters.
+ max_range_m: Maximum range in meters of the data, used for clipping.
+ min_range_m: Minimum range in meters of the data, used for clipping.
+ **kwargs: Other arguments passed to NormalNoise (principally bias).
+ """
+ # Validate range values.
+ if range_noise_m < 0.0:
+ raise ValueError("Range noise should not be negative: %r" % range_noise_m)
+ if min_range_m >= max_range_m:
+ raise ValueError("min_range_m %s must be less than max_range_m %s" %
+ (min_range_m, max_range_m))
+
+ self._range_noise_m = range_noise_m
+ self._max_range_m = max_range_m
+ self._min_range_m = min_range_m
+ self._total_range = max_range_m - min_range_m
+ super(RangeNoise, self).__init__(
+ scale=range_noise_m / self._total_range,
+ clipping_lower_bound=0.0,
+ clipping_upper_bound=1.0,
+ **kwargs)
+
+ def _get_noise_m(self, shape, dtype=None):
+ """Create normal noise of the given direction and datatype, in meters."""
+ return self.range_to_m(self._get_noise(shape=shape, dtype=dtype))
+
+ def add_noise_m(self, data):
+ """Add normal noise to the given data, scaled in meters."""
+ return self.range_to_m(self.add_noise(self.m_to_range(data)))
+
+ def m_to_range(self, data):
+ """Scale data in meters to a range of (0, 1)."""
+ return (data - self._min_range_m) / self._total_range
+
+ def range_to_m(self, data):
+ """Scale data in range of (0, 1) to meters."""
+ return data * self._total_range + self._min_range_m
+
+
+@gin.configurable
+class TwistNoise(object):
+ """Add normally distributed noise to twist actions.
+
+ Note this is a simplified noise model in action space designed for parity
+ with DriveWorld's r/s/e/drive_models/twist_drive.py;rcl=307540784;l=161.
+ This assumes that the TwistNoise will be applied to a twist action which is
+ then clipped, as currently done in wheeled_robot_base.py:
+
+ robotics/reinforcement_learning/minitaur/robots/wheeled_robot_base.py;l=533
+ # We assume that the velocity clipping would be absorbed in this API.
+ if self._action_filter:
+ action = self._action_filter.filter(action)
+
+ where action is a linear_velocity, angular_velocity pair, which is clipped
+ to limits subsequently by the _compute_kinematic_base_velocity method.
+ """
+
+ def __init__(self,
+ linear_velocity_noise_stdev_mps: float,
+ linear_velocity_noise_max_stdevs: float,
+ angular_velocity_noise_stdev_rps: float,
+ angular_velocity_noise_max_stdevs: float,
+ noise_scaling_cutoff_mps: float = 0.0):
+ """Create a normal noise generator suitable for use in a range scanner.
+
+ Supports the API specified in the DriveWorld TwistDrive class:
+ robotics/simulation/environments/drive_models/twist_drive.py;l=54
+
+ Args:
+ linear_velocity_noise_stdev_mps: One standard deviation of normal noise
+ for linear velocity in meters per second.
+ linear_velocity_noise_max_stdevs: Max stdevs for linear velocity noise.
+ This ensures that the noise values do not spike too crazy.
+ angular_velocity_noise_stdev_rps: One standard deviation of normal noise
+ for angular velocity in radians per second.
+ angular_velocity_noise_max_stdevs: Max stdevs for angular velocity noise.
+ noise_scaling_cutoff_mps: If linear velocity is less than this cutoff,
+ linear and angular noise are scaled so that zero velocity produces zero
+ noise. This enables a robot at rest to remain at rest, while still
+ applying reasonable noise values to finite velocities. Angular velocity
+ does not contribute to this computation to keep the model simple.
+ """
+ # Validate range values.
+ if linear_velocity_noise_stdev_mps < 0.0:
+ raise ValueError("Linear action noise should not be negative: %r" %
+ linear_velocity_noise_stdev_mps)
+ if linear_velocity_noise_max_stdevs < 0.0:
+ raise ValueError("Maximum linear noise should not be negative: %r" %
+ linear_velocity_noise_max_stdevs)
+ if angular_velocity_noise_stdev_rps < 0.0:
+ raise ValueError("Angular action noise should not be negative: %r" %
+ angular_velocity_noise_stdev_rps)
+ if angular_velocity_noise_max_stdevs < 0.0:
+ raise ValueError("Maximum action noise should not be negative: %r" %
+ angular_velocity_noise_max_stdevs)
+ if noise_scaling_cutoff_mps < 0.0:
+ raise ValueError("Noise scaling cutoff should not be negative: %r" %
+ noise_scaling_cutoff_mps)
+
+ # Save the values to create our noise later.
+ self._noise_shape = [
+ linear_velocity_noise_stdev_mps, angular_velocity_noise_stdev_rps
+ ]
+ # The noise clipping is performed using one standard deviation as the unit.
+ self._noise_lower_bound = np.array([
+ -linear_velocity_noise_max_stdevs * linear_velocity_noise_stdev_mps,
+ -angular_velocity_noise_max_stdevs * angular_velocity_noise_stdev_rps
+ ])
+ self._noise_upper_bound = -self._noise_lower_bound
+ self._noise_scaling_cutoff_mps = noise_scaling_cutoff_mps
+
+ def filter(self, action):
+ """Filter the linear and angular velocity by adding noise."""
+ linear_velocity, angular_velocity = action
+ linear_noise, angular_noise = np.clip(
+ np.random.normal(0, self._noise_shape, 2), self._noise_lower_bound,
+ self._noise_upper_bound)
+ if self._noise_scaling_cutoff_mps:
+ clipped_velocity = min(abs(linear_velocity),
+ self._noise_scaling_cutoff_mps)
+ scaling_factor = clipped_velocity / self._noise_scaling_cutoff_mps
+ linear_noise *= scaling_factor
+ angular_noise *= scaling_factor
+
+ return (linear_velocity + linear_noise, angular_velocity + angular_noise)
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/utilities/rendering_utils.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/utilities/rendering_utils.py
new file mode 100644
index 000000000..99568108b
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/utilities/rendering_utils.py
@@ -0,0 +1,272 @@
+"""Rendering utilities."""
+
+import enum
+import math
+import os
+from typing import Any, Callable, Dict, Iterable, Optional, Text
+from absl import logging
+import gin
+import numpy as np
+
+
+# These matrices will change by any call to render() and should be saved
+# immediately to the local object.
+last_used_view_matrix = None
+last_used_proj_matrix = None
+last_rendered_image_size = None
+
+# Bounds of plane loaded with GEOM_PLANE.
+_INFINITY = 1.0e30
+# In case of infinite plane, use this scene bounding box.
+_DEFAULT_BOUNDING_BOX = ((-15, -15, -10), (15, 15, 0))
+
+
+def render_image(pybullet_client, width, height, view_matrix, proj_matrix):
+ """Renders image as numpy array given view and projection matrices."""
+ global last_used_view_matrix, last_used_proj_matrix, last_rendered_image_size
+ last_used_view_matrix = view_matrix
+ last_used_proj_matrix = proj_matrix
+
+ (_, _, px, _, _) = pybullet_client.getCameraImage(
+ width=width,
+ height=height,
+ renderer=pybullet_client.ER_BULLET_HARDWARE_OPENGL,
+ viewMatrix=view_matrix,
+ projectionMatrix=proj_matrix)
+ rgb_array = np.array(px)
+ image = rgb_array[:, :, :3]
+ last_rendered_image_size = (image.shape[1], image.shape[0])
+ return image
+
+
+def project_world_to_image(points,
+ view_matrix=None,
+ proj_matrix=None,
+ image_size=None,
+ include_z_coord=False):
+ """Projects 3D world-space points to 2D or 3D image-space points.
+
+ If no projection matrices or image_size are given, the last ones are used.
+ This means that you can render an image with `render_image` function (or other
+ functions that use this one) and then project points to that image immediately
+ after that call. Otherwise, you can save `last_used_view_matrix` and
+ `last_used_proj_matrix` variables and pass them in later.
+
+ Args:
+ points: Sequence of 3D points.
+ view_matrix: Optional view matrix (column-major).
+ proj_matrix: Optional projection matrix (column-major).
+ image_size: Optional image size for resulting coords.
+ include_z_coord: Whether to include Z coordinate in the resulting
+ projection. Z coordinate goes from 0 (far plane) to 1 (near plane).
+
+ Returns:
+ A Numpy array of shape (points_count, 2) or (points_count, 3) when
+ `include_z_coord` is True, with dtype of np.float32.
+ """
+ # Note that these matrices are column-major.
+ view_matrix = np.array(
+ view_matrix or last_used_view_matrix, dtype=np.float32).reshape((4, 4))
+ proj_matrix = np.array(
+ proj_matrix or last_used_proj_matrix, dtype=np.float32).reshape((4, 4))
+ mvp_matrix = np.matmul(view_matrix, proj_matrix)
+
+ # Add w component equal to 1 (for perspective projection).
+ points = np.asarray(points, dtype=np.float32)
+ points_4d = np.concatenate(
+ [points, np.ones((points.shape[0], 1), dtype=np.float32)], axis=-1)
+ points_proj = np.matmul(points_4d, mvp_matrix)
+
+ if include_z_coord:
+ # Perspective divide (only keep X, Y, and Z, discard W).
+ points_proj_3d = points_proj[:, 0:3] / np.expand_dims(points_proj[:, 3], -1)
+ # Shift origin to bottom left and rescale range to [0,1]. This assumes
+ # OpenGL projection space.
+ points_proj_3d = (points_proj_3d + 1) * 0.5
+ # Invert y-axis to have a coordinate with (0,0) on the top left.
+ points_proj_3d[:, 1] = 1 - points_proj_3d[:, 1]
+ # Scale projection to image size, ignore Z.
+ image_size = np.asarray(
+ image_size or last_rendered_image_size, dtype=np.float32)
+ image_size = np.append(image_size, 1)
+ return points_proj_3d * image_size
+ else:
+ # Perspective divide (only keep X and Y, discard Z and W).
+ points_proj_2d = points_proj[:, 0:2] / np.expand_dims(points_proj[:, 3], -1)
+ # Shift origin to bottom left and rescale range to [0,1]. This assumes
+ # OpenGL projection space.
+ points_proj_2d = (points_proj_2d + 1) * 0.5
+ # Invert y-axis to have a coordinate with (0,0) on the top left.
+ points_proj_2d[:, 1] = 1 - points_proj_2d[:, 1]
+ # Scale projection to image size.
+ image_size = np.asarray(
+ image_size or last_rendered_image_size, dtype=np.float32)
+ return points_proj_2d * image_size
+
+
+def get_scene_bounding_box(pybullet_client, scene=None):
+ """Computes scene axis-aligned bounding box.
+
+ Args:
+ pybullet_client: PyBullet client.
+ scene: Scene instance for filtering the bounding box of camera.
+
+ Returns:
+ A tuple of min and max 3D coordinates. Returns (None, None) if the scene
+ is empty.
+ """
+ aabb_min = None
+ aabb_max = None
+
+ for i in range(pybullet_client.getNumBodies()):
+ body_id = pybullet_client.getBodyUniqueId(i)
+
+ # If a scene has been provided, only count bodes which are in
+ # either the ground or obstacle id lists.
+ if scene is not None:
+ if body_id not in scene.ground_ids and body_id not in scene.obstacle_ids:
+ continue
+
+ aabb = pybullet_client.getAABB(body_id)
+ if np.any(np.abs(aabb) >= _INFINITY):
+ aabb = _DEFAULT_BOUNDING_BOX
+ if aabb_min is None:
+ aabb_min = aabb[0]
+ aabb_max = aabb[1]
+ else:
+ aabb_min = np.minimum(aabb_min, aabb[0])
+ aabb_max = np.maximum(aabb_max, aabb[1])
+
+ return aabb_min, aabb_max
+
+
+@gin.configurable
+def render_topdown(
+ pybullet_client,
+ result_size=(1280, 720),
+ scale_px_per_meter=None,
+ camera_height=50,
+ ground_height=None,
+ low_render_height_from_ground=None,
+ high_render_height_from_ground=None,
+ scene=None,
+ use_y_as_up_axis=False,
+ rendered_origin_and_size=None,
+):
+ """Renders top-down image of the environment.
+
+ Args:
+ pybullet_client: PyBullet client.
+ result_size: Resulting image size.
+ scale_px_per_meter: Resulting image scale in pixels per meter. This
+ overrides `result_size`.
+ camera_height: Height of the camera above the environment. This is not very
+ significant, the lower the height, the larger perspective distortion.
+ ground_height: Ground height for following two parameters.
+ low_render_height_from_ground: If set, rendering is cut below this distance
+ from ground.
+ high_render_height_from_ground: If set, rendering is cut above this
+ distance from ground.
+ scene: Scene instance for filtering the bounding box of camera.
+ use_y_as_up_axis: Whether to consider Y axis as world's "up" instead of Z.
+ rendered_origin_and_size: If set, sets rendered origin (bounding box min)
+ and size (bounding box size) instead of computing that from the scene.
+ Expected format: two tuples of [x, y z] coords containing origin and size
+ of rendered bounding box.
+
+ Returns:
+ RGB image as 3D numpy array.
+ """
+ # Get bounds of the current environment.
+ if rendered_origin_and_size is not None:
+ aabb_min, aabb_size = rendered_origin_and_size
+ if len(aabb_min) != 3:
+ raise ValueError(
+ "Invalid render origin, expected [x, y, z]: {}".format(aabb_min))
+ if len(aabb_size) != 3:
+ raise ValueError(
+ "Invalid render size, expected [x, y, z]: {}".format(aabb_size))
+ aabb_max = tuple(m + s for m, s in zip(aabb_min, aabb_size))
+ else:
+ aabb_min, aabb_max = get_scene_bounding_box(pybullet_client, scene)
+
+ if use_y_as_up_axis:
+ width = aabb_max[0] - aabb_min[0]
+ height = aabb_max[2] - aabb_min[2]
+ z_size = aabb_max[1] - aabb_min[1]
+ z_max = aabb_max[1]
+ else:
+ width = aabb_max[0] - aabb_min[0]
+ height = aabb_max[1] - aabb_min[1]
+ z_size = aabb_max[2] - aabb_min[2]
+ z_max = aabb_max[2]
+
+ if scale_px_per_meter is not None:
+ result_size = (int(width * scale_px_per_meter),
+ int(height * scale_px_per_meter))
+ else:
+ # Adjust scene size to fit inside of the given result size.
+ if len(result_size) != 2 or result_size[0] <= 0 or result_size[1] <= 0:
+ raise ValueError("Invalid result size: {}".format(result_size))
+ img_width, img_height = result_size
+ if img_width / width < img_height / height:
+ # Width is limiting. Adjust height to match.
+ adjusted_height = img_height * width / img_width
+ assert adjusted_height >= height, (adjusted_height, height)
+ height = adjusted_height
+ else:
+ # Height is limiting. Adjust width to match.
+ adjusted_width = img_width * height / img_height
+ assert adjusted_width >= width, (adjusted_width, width)
+ width = adjusted_width
+
+ # Compute view and projection matrices.
+ if use_y_as_up_axis:
+ center_x = (aabb_min[0] + aabb_max[0]) / 2.0
+ center_z = (aabb_min[2] + aabb_max[2]) / 2.0
+ view_matrix = pybullet_client.computeViewMatrix(
+ cameraEyePosition=(center_x, aabb_max[1] + camera_height, center_z),
+ cameraTargetPosition=(center_x, aabb_max[1], center_z),
+ cameraUpVector=(0, 0, 1))
+ else:
+ center_x = (aabb_min[0] + aabb_max[0]) / 2.0
+ center_y = (aabb_min[1] + aabb_max[1]) / 2.0
+ view_matrix = pybullet_client.computeViewMatrix(
+ cameraEyePosition=(center_x, center_y, aabb_max[2] + camera_height),
+ cameraTargetPosition=(center_x, center_y, aabb_max[2]),
+ cameraUpVector=(0, 1, 0))
+
+ near_plane = camera_height
+ far_plane = camera_height + z_size
+ if ground_height is not None:
+ if low_render_height_from_ground is not None:
+ far_plane = (
+ camera_height + z_max - ground_height - low_render_height_from_ground)
+ if high_render_height_from_ground is not None:
+ near_plane = (
+ camera_height + z_max - ground_height -
+ high_render_height_from_ground)
+ else:
+ if (low_render_height_from_ground is not None or
+ high_render_height_from_ground is not None):
+ raise ValueError(
+ "The `low_render_height_from_ground` or "
+ "`high_render_height_from_ground` were specified but no reference "
+ "ground height was given.")
+ vertical_fov = 2 * math.atan2(height / 2, camera_height)
+ proj_matrix = pybullet_client.computeProjectionMatrixFOV(
+ fov=math.degrees(vertical_fov),
+ aspect=width / height,
+ nearVal=near_plane,
+ farVal=far_plane)
+
+ # Render and return image.
+ return render_image(
+ pybullet_client,
+ result_size[0],
+ result_size[1],
+ view_matrix,
+ proj_matrix,
+ )
+
+
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/utilities/robot_pose_utils.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/utilities/robot_pose_utils.py
new file mode 100644
index 000000000..6e0e92243
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/utilities/robot_pose_utils.py
@@ -0,0 +1,189 @@
+"""This file implements the robot specific pose tools."""
+import math
+
+import attr
+import numpy as np
+
+from pybullet_envs.minitaur.envs_v2.utilities import laikago_pose_utils
+from pybullet_envs.minitaur.envs_v2.utilities import mini_cheetah_pose_utils
+from pybullet_envs.minitaur.envs_v2.utilities import minitaur_pose_utils
+from pybullet_envs.minitaur.robots import laikago
+from pybullet_envs.minitaur.robots import laikago_v2
+from pybullet_envs.minitaur.robots import mini_cheetah
+from pybullet_envs.minitaur.robots import minitaur_v2
+
+_ABDUCTION_ACTION_INDEXES = [0, 3, 6, 9]
+
+# The default values used to give a neutral pose for minitaur.
+_MINITAUR_DEFAULT_EXTENSION_POS = math.pi / 2
+_MINITAUR_DEFAULT_SWING_POS = 0
+
+_LAIKAGO_NEUTRAL_POSE_HIP_ANGLE = math.pi / 4
+_LAIKAGO_NEUTRAL_POSE_KNEE_ANGLE = -math.pi / 2
+_LAIKAGO_EXTENSION_CONVERSION_MULTIPLIER = 1.0
+_LAIKAGO_SWING_CONVERSION_MULTIPLIER = -1.0
+
+_MINI_CHEETAH_NEUTRAL_POSE_HIP_ANGLE = -math.pi / 4
+_MINI_CHEETAH_NEUTRAL_POSE_KNEE_ANGLE = math.pi / 2
+_MINI_CHEETAH_EXTENSION_CONVERSION_MULTIPLIER = -1.0
+_MINI_CHEETAH_SWING_CONVERSION_MULTIPLIER = 1.0
+
+
+def get_neutral_motor_angles(robot_class):
+ """Return a neutral (standing) pose for a given robot type.
+
+ Args:
+ robot_class: This returns the class (not the instance) for the robot.
+ Currently it supports minitaur, laikago and mini-cheetah.
+
+ Returns:
+ Pose object for the given robot. It's either MinitaurPose, LaikagoPose or
+ MiniCheetahPose.
+
+ Raises:
+ ValueError: If the given robot_class is different than the supported robots.
+ """
+ if str(robot_class) in [
+ str(minitaur_v2.Minitaur)
+ ]:
+ init_pose = minitaur_pose_utils.leg_pose_to_motor_angles(
+ np.array(
+ attr.astuple(
+ minitaur_pose_utils.MinitaurPose(
+ swing_angle_0=_MINITAUR_DEFAULT_SWING_POS,
+ swing_angle_1=_MINITAUR_DEFAULT_SWING_POS,
+ swing_angle_2=_MINITAUR_DEFAULT_SWING_POS,
+ swing_angle_3=_MINITAUR_DEFAULT_SWING_POS,
+ extension_angle_0=_MINITAUR_DEFAULT_EXTENSION_POS,
+ extension_angle_1=_MINITAUR_DEFAULT_EXTENSION_POS,
+ extension_angle_2=_MINITAUR_DEFAULT_EXTENSION_POS,
+ extension_angle_3=_MINITAUR_DEFAULT_EXTENSION_POS))))
+ elif str(robot_class) in [
+ str(laikago.Laikago),
+ str(laikago_v2.Laikago),
+ ]:
+ init_pose = np.array(
+ attr.astuple(
+ laikago_pose_utils.LaikagoPose(
+ abduction_angle_0=0,
+ hip_angle_0=_LAIKAGO_NEUTRAL_POSE_HIP_ANGLE,
+ knee_angle_0=_LAIKAGO_NEUTRAL_POSE_KNEE_ANGLE,
+ abduction_angle_1=0,
+ hip_angle_1=_LAIKAGO_NEUTRAL_POSE_HIP_ANGLE,
+ knee_angle_1=_LAIKAGO_NEUTRAL_POSE_KNEE_ANGLE,
+ abduction_angle_2=0,
+ hip_angle_2=_LAIKAGO_NEUTRAL_POSE_HIP_ANGLE,
+ knee_angle_2=_LAIKAGO_NEUTRAL_POSE_KNEE_ANGLE,
+ abduction_angle_3=0,
+ hip_angle_3=_LAIKAGO_NEUTRAL_POSE_HIP_ANGLE,
+ knee_angle_3=_LAIKAGO_NEUTRAL_POSE_KNEE_ANGLE)))
+ elif str(robot_class) == str(mini_cheetah.MiniCheetah):
+ init_pose = np.array(
+ attr.astuple(
+ mini_cheetah_pose_utils.MiniCheetahPose(
+ abduction_angle_0=0,
+ hip_angle_0=_MINI_CHEETAH_NEUTRAL_POSE_HIP_ANGLE,
+ knee_angle_0=_MINI_CHEETAH_NEUTRAL_POSE_KNEE_ANGLE,
+ abduction_angle_1=0,
+ hip_angle_1=_MINI_CHEETAH_NEUTRAL_POSE_HIP_ANGLE,
+ knee_angle_1=_MINI_CHEETAH_NEUTRAL_POSE_KNEE_ANGLE,
+ abduction_angle_2=0,
+ hip_angle_2=_MINI_CHEETAH_NEUTRAL_POSE_HIP_ANGLE,
+ knee_angle_2=_MINI_CHEETAH_NEUTRAL_POSE_KNEE_ANGLE,
+ abduction_angle_3=0,
+ hip_angle_3=_MINI_CHEETAH_NEUTRAL_POSE_HIP_ANGLE,
+ knee_angle_3=_MINI_CHEETAH_NEUTRAL_POSE_KNEE_ANGLE)))
+ else:
+ init_pose = robot_class.get_neutral_motor_angles()
+ return init_pose
+
+
+def convert_leg_pose_to_motor_angles(robot_class, leg_poses):
+ """Convert swing-extend coordinate space to motor angles for a robot type.
+
+ Args:
+ robot_class: This returns the class (not the instance) for the robot.
+ Currently it supports minitaur, laikago and mini-cheetah.
+ leg_poses: A list of leg poses in [swing,extend] or [abduction, swing,
+ extend] space for all 4 legs. The order is [abd_0, swing_0, extend_0,
+ abd_1, swing_1, extend_1, ...] or [swing_0, extend_0, swing_1, extend_1,
+ ...]. Zero swing and zero extend gives a neutral standing pose for all the
+ robots. For minitaur, the conversion is fully accurate, for laikago and
+ mini-cheetah the conversion is approximate where swing is reflected to hip
+ and extend is reflected to both knee and the hip.
+
+ Returns:
+ List of motor positions for the selected robot. The list include 8 or 12
+ motor angles depending on the given robot type as an argument. Currently
+ laikago and mini-cheetah has motors for abduction which does not exist for
+ minitaur robot.
+
+ Raises:
+ ValueError: Conversion fails due to wrong inputs.
+ """
+ default_leg_order = ["front_left", "back_left", "front_right", "back_right"]
+ leg_order = default_leg_order
+ if len(leg_poses) not in [8, 12]:
+ raise ValueError("Dimension of the leg pose provided is not 8 or 12.")
+ neutral_motor_angles = get_neutral_motor_angles(robot_class)
+ motor_angles = leg_poses
+ # If it is a robot with 12 motors but the provided leg pose does not contain
+ # abduction, extend the pose to include abduction.
+ if len(neutral_motor_angles) == 12 and len(leg_poses) == 8:
+ for i in _ABDUCTION_ACTION_INDEXES:
+ motor_angles.insert(i, 0)
+ # If the robot does not have abduction (minitaur) but the input contains them,
+ # ignore the abduction angles for the conversion.
+ elif len(neutral_motor_angles) == 8 and len(leg_poses) == 12:
+ del leg_poses[::3]
+ # Minitaur specific conversion calculations using minitaur-specific safety
+ # limits.
+ if str(robot_class) in [
+
+ str(minitaur_v2.Minitaur)
+ ]:
+ motor_angles = minitaur_pose_utils.leg_pose_to_motor_angles_with_half_pi_offset_and_safety(
+ leg_poses)
+ # Laikago and mini-cheetah specific conversion calculations.
+ elif str(robot_class) in [
+ str(mini_cheetah.MiniCheetah),
+ str(laikago.Laikago),
+ str(laikago_v2.Laikago),
+
+ ]:
+ swing_scale = 1.0
+ extension_scale = 1.0
+ # Laikago specific conversion multipliers.
+ if str(robot_class) in [
+ str(laikago.Laikago),
+ str(laikago_v2.Laikago),
+
+ ]:
+ swing_scale = _LAIKAGO_SWING_CONVERSION_MULTIPLIER
+ extension_scale = _LAIKAGO_EXTENSION_CONVERSION_MULTIPLIER
+ leg_order = ["front_right", "front_left", "back_right", "back_left"]
+ # Mini-cheetah specific multipliers.
+ elif str(robot_class) in [str(mini_cheetah.MiniCheetah)]:
+ swing_scale = _MINI_CHEETAH_SWING_CONVERSION_MULTIPLIER
+ extension_scale = _MINI_CHEETAH_EXTENSION_CONVERSION_MULTIPLIER
+ # In this approximate conversion for mini-cheetah and laikago we set hip
+ # angle swing + half of the extend and knee angle to extend as rotation.
+ # We also scale swing and extend based on some hand-tuned constants.
+ multipliers = np.array([1.0, swing_scale, extension_scale] * 4)
+ swing_extend_scaled = leg_poses * multipliers
+ # Swing is (swing - half of the extension) due to the geometry of the leg.
+ extra_swing = swing_extend_scaled * ([0, 0, -0.5] * 4)
+ swing_extend_scaled += np.roll(extra_swing, -1)
+ motor_angles = list(swing_extend_scaled)
+ motor_angles = neutral_motor_angles + motor_angles
+ # Change the order of the legs if it is different for the specific robot.
+ if leg_order != default_leg_order:
+ leg_order = [default_leg_order.index(leg) for leg in leg_order]
+ ordered_motor_angles = []
+ for i in leg_order:
+ ordered_motor_angles.extend(motor_angles[3 * i:3 * i + 3])
+ motor_angles = ordered_motor_angles
+ else:
+ motor_angles = robot_class.convert_leg_pose_to_motor_angles(leg_poses)
+
+ return motor_angles
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/utilities/termination_reason.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/utilities/termination_reason.py
new file mode 100644
index 000000000..5a943d9e4
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/utilities/termination_reason.py
@@ -0,0 +1,40 @@
+"""Enum for classifying the termination reason of an episode."""
+
+import enum
+import gin
+
+
+@gin.constants_from_enum
+class TerminationReason(enum.IntEnum):
+ """Enum that identifies termination reasons of an episode.
+
+ For any new termination reason added here, please update the corresponding
+ termination reward files to make sure it is used properly.
+ """
+ UNKNOWN = 0
+ STEP_LIMIT = 1
+ WALL_COLLISION = 2
+ BAD_LOCATION = 3
+ AGENT_COLLISION = 4
+ GOAL_REACHED = 5
+ INVALID_STEP_REVERT_AND_CONTINUE = 6
+ INVALID_EPISODE = 7
+ RUN_TIME_LIMIT = 8
+ NOT_ADVANCING_LIMIT = 9
+ NOT_LOCALIZED = 10
+
+
+COLORMAP = {
+ TerminationReason.UNKNOWN: (64, 64, 64), # Dark gray.
+ TerminationReason.STEP_LIMIT: (128, 64, 192), # Purple.
+ TerminationReason.WALL_COLLISION: (255, 64, 128), # Bright red.
+ TerminationReason.BAD_LOCATION: (255, 0, 192), # Magenta.
+ TerminationReason.AGENT_COLLISION: (255, 128, 0), # Orange.
+ TerminationReason.GOAL_REACHED: (96, 255, 96), # Bright green.
+ TerminationReason.INVALID_STEP_REVERT_AND_CONTINUE: (255, 255,
+ 255), # White.
+ TerminationReason.INVALID_EPISODE: (0, 0, 0), # Black.
+ TerminationReason.RUN_TIME_LIMIT: (128, 64, 192), # Purple.
+ TerminationReason.NOT_ADVANCING_LIMIT: (128, 64, 192), # Purple.
+ TerminationReason.NOT_LOCALIZED: (255, 0, 192), # Magenta.
+}
\ No newline at end of file
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/robots/__init__.py b/examples/pybullet/gym/pybullet_envs/minitaur/robots/__init__.py
new file mode 100644
index 000000000..e69de29bb
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/robots/autonomous_object.py b/examples/pybullet/gym/pybullet_envs/minitaur/robots/autonomous_object.py
new file mode 100644
index 000000000..44c9d65ec
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/robots/autonomous_object.py
@@ -0,0 +1,286 @@
+# Lint as: python3
+"""A module that defines autonomous object class and related functions."""
+from typing import Any, Callable, Dict, Optional, Sequence, Text, Union
+
+from absl import logging
+import gin
+import numpy as np
+
+from pybullet_utils import bullet_client
+from pybullet_envs.minitaur.envs_v2 import base_client
+from pybullet_envs.minitaur.envs_v2.sensors import sensor
+from pybullet_envs.minitaur.robots import object_controller
+from pybullet_envs.minitaur.robots import robot_base
+
+# The action value to pass into AutonomousObject pre_control_step() and
+# apply_action().
+AUTONOMOUS_ACTION = None
+
+# Maximum force used in constraint based actuation.
+_MAX_FORCE = 1000
+
+
+# TODO(b/155124699): find a better way actuate object than using constraint or
+# modifying URDF.
+@gin.configurable
+class AutonomousObject(robot_base.RobotBase):
+ """Autonomous object that moves/acts in simulation guided by a controller."""
+
+ def __init__(self,
+ urdf_file: Text,
+ sensors: Sequence[sensor.Sensor] = (),
+ controller: object_controller.ControllerBase = None,
+ actuate_by_reset: bool = False):
+ """Constructor.
+
+ Args:
+ urdf_file: The path to urdf file of the object.
+ sensors: A list of sensor objects to attach to autonomous object.
+ controller: A controller object that governs autonomous object's motion.
+ If not specified, StationaryController is used.
+ actuate_by_reset: Use pybullet resetBasePositionAndOrientation to actuate
+ the object. Default is False, which means actuate by constraint. In the
+ actuate by constrained mode, be extra cautious when the position or
+ orientation control is based on position or orientation sensor reading
+ of the same object. This loop-back condition is known to be problematic
+ and causes slower than expected motion.
+ """
+ self._urdf_file = urdf_file
+ self._controller = controller or object_controller.StationaryController()
+ self._actuate_by_reset = actuate_by_reset
+ self._actuate_function = (
+ self._actuate_base_pose
+ if not actuate_by_reset else self._reset_base_pose)
+
+ self._sensors = list(sensors)
+ self._object_id = -1
+ self._constraint_id = -1
+
+ self._pybullet_client = None # will be initialized in set_sim_client()
+ self._clock = None # will be initialized in set_clock()
+ self._init_internal_states()
+
+ def _init_internal_states(self) -> None:
+ self._observations_time_since_reset = 0
+ self._observations = {}
+ self._position = np.zeros(3)
+ self._orientation = np.array([0, 0, 0, 1])
+
+ def set_sim_client(self, pybullet_client: bullet_client.BulletClient) -> None:
+ """Sets new simulation client and reload assets."""
+ self._pybullet_client = pybullet_client
+ self._init_internal_states()
+ self.load()
+
+ def set_clock(self, clock: Callable[[], float]) -> None:
+ """Sets monotonic clock when adding into simulation environment."""
+ self._clock = clock
+
+ @property
+ def sim_object_id(self):
+ return self._object_id
+
+ def update(self, time_since_reset_sec: float,
+ observations: Dict[Text, Any]) -> None:
+ """Updates simulation time and observations.
+
+ This function should be called before apply_action in each simulation step.
+
+ Args:
+ time_since_reset_sec: Time from start of simulation reset in seconds.
+ observations: A dict of observations.
+ """
+ if time_since_reset_sec < self._observations_time_since_reset:
+ raise ValueError(
+ "Time cannot go backwards. Current t = %f, new t = %f." %
+ (self._observations_time_since_reset, time_since_reset_sec))
+ self._observations_time_since_reset = time_since_reset_sec
+ self._observations = observations
+
+ def _load_urdf(self):
+ """Loads object URDF file."""
+ try:
+ print("loading: ", self._urdf_file)
+ self._object_id = self._pybullet_client.loadURDF(self._urdf_file)
+ except:
+ print("Error: cannot load ", self._urdf_file)
+ import sys
+ sys.exit(0)
+
+ def load(self) -> None:
+ """Reconstructs the robot and resets its states."""
+ self._load_urdf()
+ if not self._actuate_by_reset:
+ self._constraint_id = self._pybullet_client.createConstraint(
+ parentBodyUniqueId=self._object_id,
+ parentLinkIndex=-1,
+ childBodyUniqueId=-1,
+ childLinkIndex=-1,
+ jointType=self._pybullet_client.JOINT_FIXED,
+ jointAxis=(0, 0, 0),
+ parentFramePosition=(0, 0, 0),
+ childFramePosition=(0, 0, 0),
+ childFrameOrientation=(0, 0, 0, 1))
+
+ for s in self._sensors:
+ s.set_robot(self)
+
+ # Resets the pose and updates the initial observations.
+ self.reset()
+
+ def reset(
+ self,
+ base_position: Optional[Sequence[float]] = None,
+ base_orientation_quaternion: Optional[Sequence[float]] = None,
+ controller: Optional[object_controller.ControllerBase] = None) -> None:
+ """Resets the states (e.g.
+
+ pose and sensor readings) of the robot.
+
+ This is called at the start of each episode by the environment.
+
+ Args:
+ base_position: Robot base position after reset. Must be None.
+ base_orientation_quaternion: Robot base orientation after reset. Must be
+ None.
+ controller: A new controller to replace original controller.
+ """
+ if base_position is not None or base_orientation_quaternion is not None:
+ raise ValueError("Reset position and orientation of AutonomousObject is "
+ "specified in controller.")
+
+ if controller is not None:
+ self._controller = controller
+
+ self._init_internal_states()
+ self._position, self._orientation, _ = self._controller.get_action(
+ object_controller.INIT_TIME, self._observations)
+
+ self._reset_base_pose(self._position, self._orientation)
+ self.receive_observation()
+
+ def terminate(self) -> None:
+ """Shuts down the robot, no-op in simulation."""
+
+ def pre_control_step(self, action: Any) -> Any:
+ """Processes the input action before the action repeat loop.
+
+ Args:
+ action: expect it to be `AUTONOMOUS_ACTION` at present.
+
+ Returns:
+ the action as is.
+ """
+ # Environment should not pass action other than AUTONOMOUS_ACTION.
+ if action is not AUTONOMOUS_ACTION:
+ raise ValueError("AutonomousObject only accept AUTONOMOUS_ACTION as "
+ "action value input.")
+ return action
+
+ def apply_action(self, action: Any) -> None:
+ """Applies the action to the robot."""
+ # Environment should not pass action other than AUTONOMOUS_ACTION.
+ if action is not AUTONOMOUS_ACTION:
+ raise ValueError("AutonomousObject only accept AUTONOMOUS_ACTION as "
+ "action value input.")
+ position, orientation, _ = self._controller.get_action(
+ self._observations_time_since_reset, self._observations)
+
+ self._actuate_function(position, orientation)
+
+ def receive_observation(self) -> None:
+ """Updates the robot sensor readings."""
+ position, orientation = (
+ self._pybullet_client.getBasePositionAndOrientation(self._object_id))
+ self._position = np.array(position)
+ self._orientation = np.array(orientation)
+
+ def post_control_step(self) -> None:
+ """Updates internal variables. Not yet used in AutonomousObject."""
+ pass
+
+ def _reset_base_pose(self,
+ position: Union[Sequence[float], np.ndarray] = None,
+ orientation_quat: Union[Sequence[float],
+ np.ndarray] = None):
+ """Resets the base to the desired position and orientation.
+
+ Args:
+ position: The desired base position. If omitted, current location is used.
+ orientation_quat: The desired base orientation in quaternion. If omitted,
+ current orientation is used.
+ """
+ if position is None:
+ position = self._position
+
+ if orientation_quat is None:
+ orientation_quat = self._orientation
+
+ self._pybullet_client.resetBaseVelocity(self._object_id, (0, 0, 0),
+ (0, 0, 0))
+ self._pybullet_client.resetBasePositionAndOrientation(
+ self._object_id, position, orientation_quat)
+
+ def _actuate_base_pose(self, position: Union[Sequence[float], np.ndarray],
+ orientation_quat: Union[Sequence[float], np.ndarray]):
+ """Actuates the base to the desired position and orientation.
+
+ Difference of this function from _reset_base_pose() is that this function
+ considers dynamics along the path and collisions along the motion path.
+
+ Args:
+ position: The desired base position.
+ orientation_quat: The desired base orientation in quaternion.
+ """
+ self._pybullet_client.changeConstraint(
+ self._constraint_id,
+ position,
+ jointChildFrameOrientation=orientation_quat,
+ maxForce=_MAX_FORCE)
+
+ def _reset_joint_angles(self, joint_angles=None):
+ """Resets the joint angles. Not yet used in AutonomousObject."""
+ del joint_angles
+
+ @property
+ def action_names(self) -> Sequence[Text]:
+ """Returns a sequence of action names. Always () for AutonomousObject."""
+ return ()
+
+ @property
+ def sensors(self) -> Sequence[sensor.Sensor]:
+ """Returns the sensors on this robot."""
+ return self._sensors
+
+ @property
+ def base_orientation_quaternion(self) -> np.ndarray:
+ """Returns the base pose as a quaternion in format (x, y, z, w)."""
+ return self._orientation.copy()
+
+ @property
+ def base_roll_pitch_yaw(self) -> np.ndarray:
+ """Returns the base roll, pitch, and yaw angles in radians."""
+ return np.array(
+ self._pybullet_client.getEulerFromQuaternion(self._orientation))
+
+ @property
+ def base_position(self) -> np.ndarray:
+ """Returns the base cartesian coordinates in meters."""
+ return self._position.copy()
+
+ @property
+ def timestamp(self):
+ """Simulation monotonic time."""
+ if self._clock is None:
+ raise RuntimeError("Must call set_clock() before accessing timestamp.")
+ return self._clock()
+
+ # This is need for CameraSensor.set_robot() to work.
+ @property
+ def pybullet_client(self):
+ return self._pybullet_client
+
+ # This is need for CameraSensor.set_robot() to work.
+ @property
+ def robot_id(self) -> int:
+ return self._object_id
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/robots/crowd_controller.py b/examples/pybullet/gym/pybullet_envs/minitaur/robots/crowd_controller.py
new file mode 100644
index 000000000..409c029cf
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/robots/crowd_controller.py
@@ -0,0 +1,706 @@
+# Lint as: python3
+"""Crowd objects/human controllers module."""
+
+import abc
+import collections
+from typing import Any, Callable, Dict, Iterable, List, Optional, Union, Sequence, Text
+
+from absl import logging
+import dataclasses
+import gin
+import numpy as np
+#import rvo2
+
+from pybullet_envs.minitaur.envs_v2.sensors import base_position_sensor
+from pybullet_envs.minitaur.envs_v2.sensors import sensor as generic_sensor
+from pybullet_envs.minitaur.robots import autonomous_object
+from pybullet_envs.minitaur.robots import object_controller
+
+
+POSITION_SENSOR_POSTFIX = "_pos"
+
+
+@dataclasses.dataclass
+class MovingObjectRecord:
+ position_key: Text
+ agent_id: int
+ radius: float
+ last_position: Optional[np.ndarray] = None
+
+
+@gin.configurable
+def sample_start_target_position(scene,
+ start=None,
+ start_circles=None,
+ target_circles=None,
+ num_sampling_retries=1,
+ min_wall_distance=0.0,
+ min_goal_euclidean_distance=0.0,
+ max_goal_euclidean_distance=np.Inf,
+ min_path_clearance=None):
+ """Sample valid start and target position reachable from start.
+
+ Args:
+ scene: a SceneBase instance implementing get_random_valid_position function.
+ start: a 2-tuple (x, y) of start position. If specified, no start is
+ sampled.
+ start_circles: a list of circle specification. Each circle is specified as
+ a tuple ((x, y), r) of a center (x, y) and radius r. If specified, start
+ position is sampled from within one of the start_circles.
+ target_circles: same as start_circle. If specified, target positions is
+ sampled from within one of the start_circles.
+ num_sampling_retries: a positive int, number of attempts to sample a
+ start, target pair.
+ min_wall_distance: a float, the minimum distance to a wall.
+ min_goal_euclidean_distance: a positive float, the minimum distance between
+ start and target.
+ max_goal_euclidean_distance: a positive float, the maximum distance between
+ start and target.
+ min_path_clearance: float, clearance of shortest path to walls.
+
+ Returns:
+ A 4 tuple (start, target, shortest_path, is_valid). start and target are
+ start and target positions, shortest_path is a list of 2-tuples specifying
+ the shortest path from start to target, is_valid is bool specifying whether
+ the start, target pair is valid. If min_path_clearance is not specified,
+ then shortest_path is None.
+ """
+ if not hasattr(scene, "get_random_valid_position"):
+ raise ValueError(
+ "Incompatible scene {}. Expected to have `get_random_valid_position` "
+ "method.".format(scene))
+
+ def _print_counters(counters):
+ for name, value in counters.items():
+ logging.info(" %s: %d", name, value)
+
+ sampling_counters = collections.defaultdict(lambda: 0)
+ for _ in range(num_sampling_retries):
+ if start is None:
+ start_pos = scene.get_random_valid_position(
+ min_wall_distance, inclusion_circles=start_circles)
+ else:
+ if start_circles is not None:
+ raise ValueError("At most one of the arguments start and start_circles "
+ "can be not None.")
+ start_pos = start
+ target_pos = scene.get_random_valid_position(
+ min_wall_distance, inclusion_circles=target_circles)
+ sampling_counters["attempts"] += 1
+
+ euclidean_distance = np.linalg.norm(target_pos - start_pos)
+ if euclidean_distance < min_goal_euclidean_distance:
+ sampling_counters["min_euclidean"] += 1
+ continue
+ if euclidean_distance > max_goal_euclidean_distance:
+ sampling_counters["max_euclidean"] += 1
+ continue
+
+ # Skip the path computation is no path clearance is provided.
+ if min_path_clearance is None:
+ logging.info("Valid goal with no minimum path clearance checking.")
+ _print_counters(sampling_counters)
+ return start_pos, target_pos, None, True
+
+ # Check the goal clearance along the shortest path
+ if not hasattr(scene, "find_shortest_path"):
+ raise ValueError(
+ f"scene %s missing find_shortest_path method {scene}")
+
+ # This is a slow process.
+ shortest_path = scene.find_shortest_path(
+ start_pos[:2], target_pos[:2], min_path_clearance)
+ # No path exists between current robot position and goal satisfying the
+ # clearance.
+ if shortest_path is None:
+ sampling_counters["path_clearance"] += 1
+ continue
+
+ logging.info("Valid start/target with path clearance checking.")
+ _print_counters(sampling_counters)
+ return start_pos, target_pos, shortest_path, True
+
+ logging.info("No valid start/target found.")
+ _print_counters(sampling_counters)
+ return start_pos, target_pos, None, False
+
+
+class CrowdController(metaclass=abc.ABCMeta):
+ """Crowd controller interface."""
+
+ def __init__(self, names: Iterable[Text],
+ position_key_formatter="%s" + POSITION_SENSOR_POSTFIX):
+ """Constructor.
+
+ Args:
+ names: Name of instance (dynamic object or human).
+ position_key_formatter: Formatter to convert name to position sensor name.
+ """
+ self._names = list(names)
+ self._position_key_formatter = position_key_formatter
+ self._num_instance = len(self._names)
+
+ self._current_time = 0
+
+ def _validate_instance_id(self, instance_id):
+ if not 0 <= instance_id < self._num_instance:
+ raise ValueError(
+ f"instance_id must be an integer in [0, {self.num_instance}), "
+ f"got {instance_id}.")
+
+ @property
+ def num_instance(self):
+ """Returns the number of crowd instances."""
+ return self._num_instance
+
+ def instance_name(self, instance_id: int) -> Text:
+ """Returns the name of instance."""
+ self._validate_instance_id(instance_id)
+ return self._names[instance_id]
+
+ def instance_controller(
+ self, instance_id: int) -> object_controller.ControllerBase:
+ """Returns the individual controller of certain instance."""
+ self._validate_instance_id(instance_id)
+ return _IndividualController(self, instance_id)
+
+ def instance_get_action(
+ self, instance_id: int, time_sec: float,
+ observations: Dict[Text, Any]) -> object_controller.ControllerOutput:
+ """Returns action of specific instance given observation.
+
+ This method is for _IndividualController.
+
+ Args:
+ instance_id: Identifier of an object in the crowd.
+ time_sec: Time since simulation reset in seconds. If time < 0, returns
+ initial values and ignores observations.
+ observations: A dict of all observations.
+
+ Returns:
+ Position, orientation and an extra info dict for robot joints, human
+ skeletal pose, etc.
+ """
+ if time_sec < 0:
+ self._recalculate_actions(object_controller.INIT_TIME, {})
+ self._current_time = object_controller.INIT_TIME
+ elif time_sec > self._current_time:
+ self._current_time = time_sec
+ self._recalculate_actions(self._current_time, observations)
+
+ self._validate_instance_id(instance_id)
+
+ return self._get_action_of_instance(instance_id)
+
+ @abc.abstractmethod
+ def _recalculate_actions(
+ self, time_sec: float, observations: Dict[Text, Any]) -> None:
+ """Calculates crowd command for all instances in crowd."""
+ raise NotImplementedError(
+ "_recalculate_actions() should be implemented by subclass.")
+
+ @abc.abstractmethod
+ def _get_action_of_instance(
+ self, instance_id: int) -> object_controller.ControllerOutput:
+ """Returns calculated actions of specific instance."""
+ raise NotImplementedError(
+ "_get_action_of_instance() should be implemented by subclass.")
+
+ def set_scene(self, scene) -> None:
+ """Sets the scene for crowd controller to obtain scene information."""
+ del scene
+
+
+class _IndividualController(object_controller.ControllerBase):
+ """A utility class that wraps crowd controller in ControllerBase interface."""
+
+ def __init__(self, crowd_controller: CrowdController, instance_id: int):
+ """Constructor.
+
+ Args:
+ crowd_controller: The controller of crowd to which this instance belong.
+ instance_id: Identifier of a crowd instance.
+ """
+ self._instance_id = instance_id
+ self._crowd_controller = crowd_controller
+
+ def get_action(
+ self, time_sec: float,
+ observations: Dict[Text, Any]) -> object_controller.ControllerOutput:
+ """Returns position, orientation and pose based on time and observations.
+
+ Args:
+ time_sec: Time since simulation reset in seconds. If time < 0, returns
+ initial values and ignores observations.
+ observations: A dict of all observations.
+
+ Returns:
+ Position, orientation and an extra info dict for robot joints, human
+ skeletal pose, etc.
+ """
+ return self._crowd_controller.instance_get_action(
+ self._instance_id, time_sec, observations)
+
+
+@gin.configurable
+class StationaryController(CrowdController):
+ """A crowd controller that places crowd objects at fixed positions."""
+
+ def __init__(
+ self, positions: Sequence[Sequence[float]],
+ orientations: Optional[Sequence[Sequence[float]]] = None, **kwargs):
+ """Constructor.
+
+ Args:
+ positions: Fixed positions (3D points) of crowd instances.
+ orientations: Fixed orientations in quaternion of crowd instances.
+ **kwargs: Keyword arguments to pass on to base class.
+ """
+ super().__init__(**kwargs)
+
+ if orientations is None:
+ orientations = np.array(((0, 0, 0, 1),) * self.num_instance)
+
+ if not len(positions) == len(orientations) == self.num_instance:
+ raise ValueError(
+ f"positions and orientations should all have the same length "
+ f"{self.num_instance}. Got len(positions) = {len(positions)}, "
+ f"len(orientations) = {len(orientations)}.")
+
+ self._positions = positions
+ self._orientations = orientations
+
+ def _recalculate_actions(
+ self, time_sec: float, observations: Dict[Text, Any]) -> None:
+ """Calculates crowd command for all instances in crowd."""
+ del time_sec
+ del observations
+
+ def _get_action_of_instance(
+ self, instance_id: int) -> object_controller.ControllerOutput:
+ """Returns calculated actions of specific instance."""
+ self._validate_instance_id(instance_id)
+ return self._positions[instance_id], self._orientations[instance_id], {}
+
+
+@gin.configurable
+class OrcaController(CrowdController):
+ """A crowd controller that controls crowd instances using ORCA algorithm.
+
+ Crowd instance will be initialized at a specified start position and move
+ towards specified target position in a linear path while avoid collision with
+ each other.
+ """
+
+ _DEFAULT_NEIGHBOR_DISTANCE_M = 5
+ _DEFAULT_MAX_NEIGHBORS = 10
+ _DEFAULT_RADIUS_M = 0.5
+ _DEFAULT_MAX_SPEED_MPS = 2
+ _DEFAULT_TIME_HORIZON_SEC = 1.0
+ _DEFAULT_OBSTACLE_TIME_HORIZON_SEC = 0.3
+
+ def __init__(
+ self,
+ timestep: float,
+ start_positions: Optional[Sequence[Sequence[float]]] = None,
+ target_positions: Optional[Sequence[Sequence[float]]] = None,
+ use_position_generator: Optional[bool] = False,
+ group_sizes: Sequence[int] = None,
+ radius: float = _DEFAULT_RADIUS_M,
+ max_speed_mps: float = _DEFAULT_MAX_SPEED_MPS,
+ time_horizon_sec: float = _DEFAULT_TIME_HORIZON_SEC,
+ obstacle_time_horizon_sec: float = _DEFAULT_OBSTACLE_TIME_HORIZON_SEC,
+ neighbor_distance_m: float = _DEFAULT_NEIGHBOR_DISTANCE_M,
+ max_neighbors: int = _DEFAULT_MAX_NEIGHBORS,
+ workaround_erp_issue: bool = True,
+ moving_objects_pos_key: Sequence[Text] = (),
+ moving_objects_radius: Union[float, Sequence[float]] = _DEFAULT_RADIUS_M,
+ endless_trajectory: bool = True,
+ **kwargs):
+ """Constructor.
+
+ Args:
+ timestep: Timestep of simulation.
+ start_positions: A list of position (x, y, z) for crowd instances as
+ their starting position.
+ target_positions: A list of position (x, y, z) for crowd instances as
+ their target position.
+ use_position_generator: a boolean, if True than the start and end
+ positions are sampled. start_positions and target_positions must be None
+ group_sizes: If set, then crowd is split in groups randomly, whose sizes
+ are picked in random from this group_size list. In this way, the
+ crowd simulator sumulaters clusters of objects moving around.
+ radius: Radius of crowd instances.
+ max_speed_mps: Maximum crowd instance speed.
+ time_horizon_sec: Time horizon in second.
+ obstacle_time_horizon_sec: Time horizon for static obstacle in second.
+ neighbor_distance_m: Neighbor distance in meters. Instances closer than
+ this distance are considered neighbors.
+ max_neighbors: Max number of neighbors.
+ workaround_erp_issue: There is an issue with pybullet constraint that the
+ constraint is solved only 20% per timestep. Need to amplify position
+ delta by 5x to workaround this issue.
+ moving_objects_pos_key: Position observation key of moving objects not
+ controlled by the ORCA controller.
+ moving_objects_radius: Radius of moving objects. Should be a float, which
+ applies to all moving objects, or a sequence of float, which should be
+ of the same length as moving_objects_pos_key.
+ endless_trajectory: Only valid if use_position_generator is True. Agent
+ returns to starting point after reaching goal to achieve endless motion.
+ **kwargs: Keyword arguments to pass on to base class.
+ """
+ super().__init__(**kwargs)
+
+ assert ((start_positions is not None and target_positions is not None) or
+ use_position_generator)
+ if not use_position_generator:
+ if not len(start_positions) == len(target_positions) == self.num_instance:
+ raise ValueError(
+ f"start_positions and target_positions should both have length "
+ f"equals {self.num_instance}: "
+ f"len(start_positions) = {len(start_positions)}, "
+ f"len(target_positions) = {len(target_positions)}.")
+
+ self._timestep = timestep
+ self._radius = radius
+ self._max_speed_mps = max_speed_mps
+ self._time_horizon_sec = time_horizon_sec
+ self._obstacle_time_horizon_sec = obstacle_time_horizon_sec
+ self._neighbor_distance_m = neighbor_distance_m
+ self._max_neighbors = max_neighbors
+ self._use_position_generator = use_position_generator
+ self._endless_trajectory = endless_trajectory
+ self._scene = None
+ if isinstance(moving_objects_radius, float):
+ moving_objects_radius = [
+ moving_objects_radius] * len(moving_objects_pos_key)
+ if len(moving_objects_radius) != len(moving_objects_pos_key):
+ raise ValueError(
+ "moving_objects_radius should be either a float or a sequence of "
+ "float with the same length as moving_objects_pos_key.")
+ self._moving_objects = [
+ MovingObjectRecord(position_key=key, agent_id=-1, radius=radius)
+ for key, radius in zip(moving_objects_pos_key, moving_objects_radius)]
+
+ self._paths = None
+ self._path_indices = None
+ if self._use_position_generator:
+ self._start_positions = None
+ self._target_positions = None
+ else:
+ self._start_positions = np.array(start_positions, dtype=np.float64)
+ self._target_positions = np.array(target_positions, dtype=np.float64)
+ # A guard against multiple initializations. See recalculate_actions below.
+ self._already_initialized = False
+ self._group_sizes = [1] if group_sizes is None else group_sizes
+
+ # The following variables are initialized in _recalculate_actions()
+ self._current_positions = None
+ self._command_positions = None
+ self._command_orientations = None
+
+ #self._orca = rvo2.PyRVOSimulator(
+ # self._timestep, # timestep
+ # self._neighbor_distance_m, # neighborDist
+ # self._max_neighbors, # maxNeighbors
+ # self._time_horizon_sec, # timeHorizon
+ # self._obstacle_time_horizon_sec, # timeHorizonObst
+ # self._radius, # radius
+ # self._max_speed_mps # maxSpeed
+ #)
+ for i in range(self.num_instance):
+ if self._use_position_generator:
+ start_position = (0, 0)
+ else:
+ start_position = self._start_positions[i, :2]
+ agent_id = self._orca.addAgent(
+ tuple(start_position),
+ self._neighbor_distance_m, # neighborDist
+ self._max_neighbors, # maxNeighbors
+ self._time_horizon_sec, # timeHorizon
+ self._obstacle_time_horizon_sec, # timeHorizonObst
+ self._radius, # radius
+ self._max_speed_mps, # maxSpeed
+ (0.0, 0.0)) # velocity
+ assert agent_id == i
+
+ for obj in self._moving_objects:
+ obj.agent_id = self._orca.addAgent(
+ (0.0, 0.0), # position (will adjust after simulation starts)
+ self._neighbor_distance_m, # neighborDist
+ self._max_neighbors, # maxNeighbors
+ self._timestep, # timeHorizon
+ self._timestep, # timeHorizonObst
+ obj.radius, # radius
+ self._max_speed_mps, # maxSpeed
+ (0.0, 0.0)) # velocity
+
+ self._workaround_erp_issue = workaround_erp_issue
+
+ def _subsample_path(self, path, subsample_step=1.0):
+ subsampled_path = [path[0]]
+ traveled_dist = 0.0
+ for i, (s, t) in enumerate(zip(path[:-1], path[1:])):
+ traveled_dist += np.sqrt(
+ np.square(s[0] - t[0]) + np.square(s[1] - t[1]))
+ if traveled_dist > subsample_step or i >= len(path) - 2:
+ subsampled_path.append(t)
+ traveled_dist = 0.0
+ return subsampled_path
+
+ def _generate_start_target_positions(self):
+ """Generates start and target positions using goal generartors."""
+ assert self._scene is not None
+ self._start_positions = np.zeros((self.num_instance, 3), dtype=np.float64)
+ self._target_positions = np.zeros((self.num_instance, 3), dtype=np.float64)
+
+ self._paths = []
+ self._path_indices = []
+ start_circles, target_circles = None, None
+ group_radius = 1.0
+ current_group_size = np.random.choice(self._group_sizes)
+ index_in_current_group = 0
+ for i in range(self._num_instance):
+ start_pos, target_pos, path, is_valid = sample_start_target_position(
+ self._scene,
+ start_circles=start_circles,
+ target_circles=target_circles)
+ if index_in_current_group == current_group_size - 1:
+ start_circles, target_circles = None, None
+ index_in_current_group = 0
+ current_group_size = np.random.choice(self._group_sizes)
+ else:
+ if start_circles is None:
+ start_circles = [(start_pos[:2], group_radius)]
+ target_circles = [(target_pos[:2], group_radius)]
+ else:
+ start_circles += [(start_pos[:2], group_radius)]
+ target_circles += [(target_pos[:2], group_radius)]
+ index_in_current_group += 1
+ if not is_valid:
+ raise ValueError("No valid start/target positions.")
+ self._start_positions[i, :] = start_pos
+ self._target_positions[i, :] = target_pos
+
+ subsampled_path = self._subsample_path(path)
+ self._paths.append(np.array(subsampled_path, dtype=np.float32))
+ self._path_indices.append(0)
+
+ def _recalculate_actions(
+ self, time_sec: float, observations: Dict[Text, Any]) -> None:
+ """Calculates crowd command for all crowd instances."""
+ if self._use_position_generator:
+ if (time_sec == object_controller.INIT_TIME and
+ self._start_positions is None and
+ not self._already_initialized):
+ self._generate_start_target_positions()
+ # Initialize only once per initial time even if recalculate actions
+ # is called multiple times.
+ self._already_initialized = True
+ if time_sec == object_controller.INIT_TIME:
+ # Resets orca simulator.
+ for i in range(len(self._names)):
+ self._orca.setAgentPosition(i, tuple(self._start_positions[i, :2]))
+
+ self._command_positions = self._start_positions.copy()
+ self._current_positions = self._start_positions.copy()
+ self._command_orientations = np.repeat(
+ ((0.0, 0.0, 0.0, 1.0),), len(self._names), axis=0)
+ self._last_target_recalculation_sec = time_sec
+ return
+ else:
+ # The moment we step beyond initial time, we can initialize again.
+ self._already_initialized = False
+
+ if self._use_position_generator:
+ for i in range(self._num_instance):
+ dist = np.linalg.norm(
+ self._current_positions[i, :] - self._target_positions[i, :])
+ if dist < 2.0:
+ _, target_pos, path, is_valid = sample_start_target_position(
+ self._scene, self._current_positions[i, :])
+ if is_valid:
+ self._target_positions[i, :] = target_pos
+ subsampled_path = self._subsample_path(path)
+ self._paths.append(np.array(subsampled_path, dtype=np.float32))
+ self._path_indices.append(0)
+
+ # Sets agent position and preferred velocity based on target.
+ for i, agent_name in enumerate(self._names):
+ position = observations[self._position_key_formatter % agent_name]
+ self._orca.setAgentPosition(
+ i, tuple(position[:2])) # ORCA uses 2D position.
+ self._current_positions[i, :2] = position[:2]
+
+ if self._paths is not None:
+ # Find closest point on the path from start to target, which (1) hasn't
+ # been covered already; (2) is at least max_coverage_distance away from
+ # current position.
+ distances = np.sqrt(np.sum(np.square(
+ self._paths[i] - position[:2]), axis=1))
+ max_coverage_distance = 1.0
+ index = self._path_indices[i]
+ while True:
+ if index >= len(self._paths[i]) - 1:
+ if self._endless_trajectory:
+ self._paths[i] = self._paths[i][::-1]
+ distances = distances[::-1]
+ index = 0
+ break
+ elif distances[index] > max_coverage_distance:
+ break
+ else:
+ index += 1
+ self._path_indices[i] = index
+ target_position = self._paths[i][index, :]
+ else:
+ target_position = self._target_positions[i][:2]
+
+ goal_vector = target_position - position[:2]
+ goal_vector_norm = np.linalg.norm(goal_vector) + np.finfo(np.float32).eps
+ goal_unit_vector = goal_vector / goal_vector_norm
+
+ kv = 1
+ velocity = min(kv * goal_vector_norm,
+ self._DEFAULT_MAX_SPEED_MPS) * goal_unit_vector
+ self._orca.setAgentPrefVelocity(i, tuple(velocity))
+
+ for obj in self._moving_objects:
+ position = observations[obj.position_key]
+ self._orca.setAgentPosition(obj.agent_id, tuple(position[:2]))
+ if obj.last_position is None:
+ self._orca.setAgentPrefVelocity(obj.agent_id, (0.0, 0.0))
+ else:
+ velocity = (position - obj.last_position) / self._timestep
+ self._orca.setAgentPrefVelocity(obj.agent_id, tuple(velocity[:2]))
+ obj.last_position = position.copy()
+
+ # Advances orca simulator.
+ self._orca.doStep()
+
+ # Retrieve agent position and save in buffer.
+ for i in range(len(self._names)):
+ x, y = self._orca.getAgentPosition(i)
+ self._command_positions[i, :2] = (x, y)
+
+ yaw = np.arctan2(y - self._current_positions[i, 1],
+ x - self._current_positions[i, 0])
+ self._command_orientations[i] = (0, 0, np.sin(yaw / 2), np.cos(yaw / 2))
+
+ def _get_action_of_instance(
+ self, instance_id) -> object_controller.ControllerOutput:
+ """Returns calculated actions of specific instance."""
+
+ if self._command_positions is None:
+ raise RuntimeError(
+ "Attempted to get action of instance before _recalculate_actions().")
+
+ self._validate_instance_id(instance_id)
+
+ if self._workaround_erp_issue:
+ k_erp = 1 / 0.2
+ delta_position = (
+ self._command_positions[instance_id] -
+ self._current_positions[instance_id])
+ command_position = (
+ self._current_positions[instance_id] + k_erp * delta_position)
+ else:
+ command_position = self._command_positions[instance_id].copy()
+ return command_position, self._command_orientations[instance_id], {}
+
+ def set_scene(self, scene) -> None:
+ """Sets the scene for crowd controller to obtain scene information."""
+ try:
+ polygons = scene.vectorized_map
+ for polygon in polygons:
+ self._orca.addObstacle([tuple(point) for point in polygon])
+ self._orca.processObstacles()
+ self._scene = scene
+ except NotImplementedError:
+ logging.exception("Scene does not implement vectorized_map property. "
+ "Crowd agent cannot avoid static obstacles.")
+
+
+@gin.configurable
+def uniform_object_factory(
+ instance_id: int,
+ object_factory: Callable[..., autonomous_object.AutonomousObject],
+ *args, **kwargs) -> autonomous_object.AutonomousObject:
+ """A wrapper that removes instance_id in default crowd object factory."""
+ del instance_id
+ return object_factory(*args, **kwargs)
+
+
+@gin.configurable
+def random_object_factory(
+ instance_id: int,
+ object_factories: Iterable[
+ Callable[..., autonomous_object.AutonomousObject]],
+ *args, **kwargs) -> autonomous_object.AutonomousObject:
+ """A wrapper that removes instance_id in default crowd object factory."""
+ del instance_id
+ object_factory = np.random.choice(object_factories)
+ return object_factory(*args, **kwargs)
+
+
+@gin.configurable
+def sensor_factory(instance_id: int, sensor: Callable[...,
+ generic_sensor.Sensor],
+ *args, **kwargs) -> generic_sensor.Sensor:
+ del instance_id
+ return sensor(*args, **kwargs)
+
+
+@gin.configurable
+class CrowdBuilder(object):
+ """A helper class to construct a crowd."""
+
+ def __init__(
+ self,
+ num_instance: int,
+ crowd_controller_factory: Callable[..., CrowdController],
+ object_factory: Callable[..., autonomous_object.AutonomousObject],
+ sensor_factories: Iterable[Callable[..., generic_sensor.Sensor]] = None):
+ """Constructor.
+
+ Args:
+ num_instance: Number of autonomous objects in the crowd.
+ crowd_controller_factory: A callable that returns a crowd controller
+ object.
+ object_factory: Callable that returns an autonomous object.
+ sensor_factories: list of sensor callables.
+ """
+ self._objects = []
+ crowd_id_prefix = "crowd"
+ names = [crowd_id_prefix + "_%d" % i for i in range(num_instance)]
+
+ self._controller = crowd_controller_factory(names=names)
+
+ for i in range(num_instance):
+ position_sensor = base_position_sensor.BasePositionSensor(
+ name=names[i] + POSITION_SENSOR_POSTFIX)
+
+ # Add additional per agent sensors (e.g. camera, occupancy, etc.).
+ add_sensors = []
+ if sensor_factories:
+ for s in sensor_factories:
+ add_sensors.append(
+ sensor_factory(
+ instance_id=i, sensor=s, name=names[i] + "_" + s.__name__))
+
+ an_object = object_factory(
+ instance_id=i,
+ sensors=(position_sensor,) + tuple(add_sensors),
+ controller=self._controller.instance_controller(i))
+
+ self._objects.append(an_object)
+
+ @property
+ def crowd_objects(self) -> List[autonomous_object.AutonomousObject]:
+ """Returns list of AutonomousObjects in the crowd."""
+ return self._objects
+
+ @property
+ def crowd_controller(self) -> CrowdController:
+ """Returns the crowd controller."""
+ return self._controller
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/robots/hybrid_motor_model.py b/examples/pybullet/gym/pybullet_envs/minitaur/robots/hybrid_motor_model.py
new file mode 100644
index 000000000..4a15422b9
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/robots/hybrid_motor_model.py
@@ -0,0 +1,278 @@
+# Lint as: python3
+"""A generic PD motor model."""
+
+from typing import Tuple, Union
+import gin
+import numpy as np
+
+from pybullet_envs.minitaur.robots import robot_config
+from pybullet_envs.minitaur.robots import time_ordered_buffer
+
+_DEFAULT_BUFFER_SIZE = 200
+
+_HYBRID_ACTION_LEN = len(robot_config.HybridActionIndex)
+_HYBRID_POS_INDEX = robot_config.HybridActionIndex.POSITION.value
+_HYBRID_KP_INDEX = robot_config.HybridActionIndex.POSITION_GAIN.value
+_HYBRID_VEL_INDEX = robot_config.HybridActionIndex.VELOCITY.value
+_HYBRID_KD_INDEX = robot_config.HybridActionIndex.VELOCITY_GAIN.value
+_HYBRID_TORQUE_INDEX = robot_config.HybridActionIndex.TORQUE.value
+
+
+def _convert_to_np_array(inputs: Union[float, Tuple[float], np.ndarray], dim):
+ """Converts the inputs to a numpy array.
+
+ Args:
+ inputs: The input scalar or array.
+ dim: The dimension of the converted numpy array.
+
+ Returns:
+ The converted numpy array.
+
+ Raises:
+ ValueError: If the inputs is an array whose dimension does not match the
+ provied dimension.
+ """
+ outputs = None
+ if isinstance(inputs, (tuple, np.ndarray)):
+ outputs = np.array(inputs)
+ else:
+ outputs = np.full(dim, inputs)
+
+ if len(outputs) != dim:
+ raise ValueError("The inputs array has a different dimension {}"
+ " than provided, which is {}.".format(len(outputs), dim))
+
+ return outputs
+
+
+@gin.configurable
+class HybridMotorModel(object):
+ """A simple motor model that supports proportional and derivative control.
+
+ When in POSITION mode, the torque is calculated according to the difference
+ between current and desired joint angle, as well as the joint velocity
+ differences. For more information about PD control, please refer to:
+ https://en.wikipedia.org/wiki/PID_controller.
+
+ The model supports a HYBRID mode in which each motor command can be a tuple
+ (desired_motor_angle, position_gain, desired_motor_velocity, velocity_gain,
+ torque).
+ """
+
+ def __init__(
+ self,
+ num_motors: int,
+ pd_latency: float = 0,
+ motor_control_mode=robot_config.MotorControlMode.POSITION,
+ kp: Union[float, Tuple[float], np.ndarray] = 60,
+ kd: Union[float, Tuple[float], np.ndarray] = 1,
+ strength_ratios: Union[float, Tuple[float], np.ndarray] = 1,
+ torque_lower_limits: Union[float, Tuple[float], np.ndarray] = None,
+ torque_upper_limits: Union[float, Tuple[float], np.ndarray] = None,
+ ):
+ """Initializes the class.
+
+ Args:
+ num_motors: The number of motors for parallel computation.
+ pd_latency: Simulates the motor controller's latency in reading motor
+ angles and velocities.
+ motor_control_mode: Can be POSITION, TORQUE, or HYBRID. In POSITION
+ control mode, the PD formula is used to track a desired position and a
+ zero desired velocity. In TORQUE control mode, we assume a pass through
+ of the provided torques. In HYBRID control mode, the users need to
+ provie (desired_position, position_gain, desired_velocity,
+ velocity_gain, feedfoward_torque) for each motor.
+ kp: The default position gains for motors.
+ kd: The default velocity gains for motors.
+ strength_ratios: The scaling ratio for motor torque outputs. This can be
+ useful for quick debugging when sim-to-real gap is observed in the
+ actuator behavior.
+ torque_lower_limits: The lower bounds for torque outputs.
+ torque_upper_limits: The upper bounds for torque outputs. The output
+ torques will be clipped by the lower and upper bounds.
+
+ Raises:
+ ValueError: If the number of motors provided is negative or zero.
+ """
+ if num_motors <= 0:
+ raise ValueError(
+ "Number of motors must be positive, not {}".format(num_motors))
+ self._num_motors = num_motors
+ self._zero_array = np.full(num_motors, 0)
+ self._pd_latency = pd_latency
+ self._hybrid_command_dim = _HYBRID_ACTION_LEN * self._num_motors
+ self.set_motor_gains(kp, kd)
+ self.set_strength_ratios(strength_ratios)
+ self._torque_lower_limits = None
+ if torque_lower_limits:
+ self._torque_lower_limits = _convert_to_np_array(torque_lower_limits,
+ self._num_motors)
+
+ self._torque_upper_limits = None
+ if torque_upper_limits:
+ self._torque_upper_limits = _convert_to_np_array(torque_upper_limits,
+ self._num_motors)
+ self._motor_control_mode = motor_control_mode
+
+ # The history buffer is used to simulate the pd latency effect.
+ # TODO(b/157786642): remove hacks on duplicate timestep once the sim clock
+ # is fixed.
+ self._observation_buffer = time_ordered_buffer.TimeOrderedBuffer(
+ max_buffer_timespan=pd_latency,
+ error_on_duplicate_timestamp=False,
+ replace_value_on_duplicate_timestamp=True)
+
+ def set_strength_ratios(
+ self,
+ strength_ratios: Union[float, Tuple[float], np.ndarray],
+ ):
+ """Sets the strength of each motor relative to the default value.
+
+ Args:
+ strength_ratios: The relative strength of motor output, ranging from [0,
+ 1] inclusive.
+ """
+ self._strength_ratios = np.clip(
+ _convert_to_np_array(strength_ratios, self._num_motors), 0, 1)
+
+ def set_motor_gains(
+ self,
+ kp: Union[float, Tuple[float], np.ndarray],
+ kd: Union[float, Tuple[float], np.ndarray],
+ ):
+ """Sets the gains of all motors.
+
+ These gains are PD gains for motor positional control. kp is the
+ proportional gain and kd is the derivative gain.
+
+ Args:
+ kp: Proportional gain of the motors.
+ kd: Derivative gain of the motors.
+ """
+ self._kp = _convert_to_np_array(kp, self._num_motors)
+ self._kd = _convert_to_np_array(kd, self._num_motors)
+
+ def get_motor_gains(self):
+ """Get the PD gains of all motors.
+
+ Returns:
+ Proportional and derivative gain of the motors.
+ """
+ return self._kp, self._kd
+
+ def reset(self):
+ self._observation_buffer.reset()
+
+ def update(self, timestamp, true_motor_positions: np.ndarray,
+ true_motor_velocities: np.ndarray):
+ # Push these to the buffer
+ self._observation_buffer.add(timestamp,
+ (true_motor_positions, true_motor_velocities))
+
+ def get_motor_torques(
+ self,
+ motor_commands: np.ndarray,
+ motor_control_mode=None) -> Tuple[np.ndarray, np.ndarray]:
+ """Computes the motor torques.
+
+ Args:
+ motor_commands: The desired motor angle if the motor is in position
+ control mode. The pwm signal if the motor is in torque control mode.
+ motor_control_mode: A MotorControlMode enum.
+
+ Returns:
+ observed_torque: The torque observed. This emulates the limitations in
+ torque measurement, which is generally obtained from current estimations.
+ actual_torque: The torque that needs to be applied to the motor.
+
+ Raises:
+ NotImplementedError if the motor_control_mode is not supported.
+
+ """
+ if not motor_control_mode:
+ motor_control_mode = self._motor_control_mode
+
+ motor_torques = None
+
+ if motor_control_mode is robot_config.MotorControlMode.TORQUE:
+ motor_torques = motor_commands
+
+ if motor_control_mode is robot_config.MotorControlMode.POSITION:
+ motor_torques = self._compute_pd_torques(
+ desired_motor_angles=motor_commands,
+ kp=self._kp,
+ desired_motor_velocities=self._zero_array,
+ kd=self._kd)
+
+ if motor_control_mode is robot_config.MotorControlMode.HYBRID:
+ motor_torques = self._compute_hybrid_action_torques(motor_commands)
+
+ if motor_torques is None:
+ raise ValueError(
+ "{} is not a supported motor control mode".format(motor_control_mode))
+
+ # Rescale and clip the motor torques as needed.
+ motor_torques = self._strength_ratios * motor_torques
+ if (self._torque_lower_limits is not None or
+ self._torque_upper_limits is not None):
+ motor_torques = np.clip(motor_torques, self._torque_lower_limits,
+ self._torque_upper_limits)
+
+ return motor_torques, motor_torques
+
+ def get_motor_states(self, latency=None):
+ """Computes observation of motor angle and velocity under latency."""
+ if latency is None:
+ latency = self._pd_latency
+ buffer = self._observation_buffer.get_delayed_value(latency)
+ angle_vel_t0 = buffer.value_0
+ angle_vel_t1 = buffer.value_1
+ coeff = buffer.coeff
+
+ pos_idx = 0
+ motor_angles = angle_vel_t0[pos_idx] * (
+ 1 - coeff) + coeff * angle_vel_t1[pos_idx]
+ vel_idx = 1
+ motor_velocities = angle_vel_t0[vel_idx] * (
+ 1 - coeff) + coeff * angle_vel_t1[vel_idx]
+ return motor_angles, motor_velocities
+
+ def _compute_pd_torques(
+ self,
+ desired_motor_angles: np.ndarray,
+ kp: np.ndarray,
+ desired_motor_velocities,
+ kd: np.ndarray,
+ ) -> Tuple[np.ndarray, np.ndarray]:
+ """Computes the pd torques.
+
+ Args:
+ desired_motor_angles: The motor angles to track.
+ kp: The position gains.
+ desired_motor_velocities: The motor velocities to track.
+ kd: The velocity gains.
+
+ Returns:
+ The computed motor torques.
+ """
+ motor_angles, motor_velocities = self.get_motor_states()
+ motor_torques = -kp * (motor_angles - desired_motor_angles) - kd * (
+ motor_velocities - desired_motor_velocities)
+
+ return motor_torques
+
+ def _compute_hybrid_action_torques(
+ self, motor_commands: np.ndarray) -> Tuple[np.ndarray, np.ndarray]:
+ """Computes the pd torques in the HYBRID mode."""
+ assert len(motor_commands) == self._hybrid_command_dim
+ kp = motor_commands[_HYBRID_KP_INDEX::_HYBRID_ACTION_LEN]
+ kd = motor_commands[_HYBRID_KD_INDEX::_HYBRID_ACTION_LEN]
+ desired_motor_angles = motor_commands[_HYBRID_POS_INDEX::_HYBRID_ACTION_LEN]
+ desired_motor_velocities = motor_commands[
+ _HYBRID_VEL_INDEX::_HYBRID_ACTION_LEN]
+ additional_torques = motor_commands[
+ _HYBRID_TORQUE_INDEX::_HYBRID_ACTION_LEN]
+
+ return self._compute_pd_torques(desired_motor_angles, kp,
+ desired_motor_velocities,
+ kd) + additional_torques
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/robots/laikago.py b/examples/pybullet/gym/pybullet_envs/minitaur/robots/laikago.py
new file mode 100644
index 000000000..bcfcbdc3e
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/robots/laikago.py
@@ -0,0 +1,319 @@
+"""Pybullet simulation of a Laikago robot."""
+import math
+import os
+import re
+import gin
+import numpy as np
+from pybullet_utils import transformations
+from pybullet_envs.minitaur.envs_v2.utilities import laikago_pose_utils
+from pybullet_envs.minitaur.robots import laikago_constants
+from pybullet_envs.minitaur.robots import laikago_motor
+from pybullet_envs.minitaur.robots import minitaur
+from pybullet_envs.minitaur.robots import robot_config
+
+NUM_MOTORS = 12
+NUM_LEGS = 4
+MOTOR_NAMES = [
+ "FR_hip_motor_2_chassis_joint",
+ "FR_upper_leg_2_hip_motor_joint",
+ "FR_lower_leg_2_upper_leg_joint",
+ "FL_hip_motor_2_chassis_joint",
+ "FL_upper_leg_2_hip_motor_joint",
+ "FL_lower_leg_2_upper_leg_joint",
+ "RR_hip_motor_2_chassis_joint",
+ "RR_upper_leg_2_hip_motor_joint",
+ "RR_lower_leg_2_upper_leg_joint",
+ "RL_hip_motor_2_chassis_joint",
+ "RL_upper_leg_2_hip_motor_joint",
+ "RL_lower_leg_2_upper_leg_joint",
+]
+INIT_RACK_POSITION = [0, 0, 1]
+INIT_POSITION = [0, 0, 0.48]
+JOINT_DIRECTIONS = np.array([-1, 1, 1, 1, 1, 1, -1, 1, 1, 1, 1, 1])
+HIP_JOINT_OFFSET = 0.0
+UPPER_LEG_JOINT_OFFSET = -0.6
+KNEE_JOINT_OFFSET = 0.66
+DOFS_PER_LEG = 3
+JOINT_OFFSETS = np.array(
+ [HIP_JOINT_OFFSET, UPPER_LEG_JOINT_OFFSET, KNEE_JOINT_OFFSET] * 4)
+PI = math.pi
+
+MAX_MOTOR_ANGLE_CHANGE_PER_STEP = 0.12
+_DEFAULT_HIP_POSITIONS = (
+ (0.21, -0.1157, 0),
+ (0.21, 0.1157, 0),
+ (-0.21, -0.1157, 0),
+ (-0.21, 0.1157, 0),
+)
+
+# Bases on the readings from Laikago's default pose.
+INIT_MOTOR_ANGLES = np.array([
+ laikago_pose_utils.LAIKAGO_DEFAULT_ABDUCTION_ANGLE,
+ laikago_pose_utils.LAIKAGO_DEFAULT_HIP_ANGLE,
+ laikago_pose_utils.LAIKAGO_DEFAULT_KNEE_ANGLE
+] * NUM_LEGS)
+
+CHASSIS_NAME_PATTERN = re.compile(r"\w+_chassis_\w+")
+MOTOR_NAME_PATTERN = re.compile(r"\w+_hip_motor_\w+")
+KNEE_NAME_PATTERN = re.compile(r"\w+_lower_leg_\w+")
+TOE_NAME_PATTERN = re.compile(r"jtoe\d*")
+
+URDF_NO_TOES = "laikago.urdf"
+URDF_WITH_TOES = "laikago_toes_zup.urdf"
+
+_BODY_B_FIELD_NUMBER = 2
+_LINK_A_FIELD_NUMBER = 3
+
+
+@gin.configurable
+class Laikago(minitaur.Minitaur):
+ """A simulation for the Laikago robot."""
+
+ def __init__(self, urdf_filename=URDF_WITH_TOES, **kwargs):
+ self._urdf_filename = urdf_filename
+ if "motor_kp" not in kwargs:
+ kwargs["motor_kp"] = 100.0
+ if "motor_kd" not in kwargs:
+ kwargs["motor_kd"] = 2.0
+ if "motor_torque_limits" not in kwargs:
+ kwargs["motor_torque_limits"] = None
+
+ # enable_clip_motor_commands: Boolean indicating if clipping should be
+ # applied to motor commands, which limits the amount of change in joint
+ # pose between timesteps.
+ if "enable_clip_motor_commands" in kwargs:
+ self._enable_clip_motor_commands = kwargs["enable_clip_motor_commands"]
+ del kwargs["enable_clip_motor_commands"]
+ else:
+ self._enable_clip_motor_commands = False
+
+ # The follwing parameters are fixed for the Laikago robot.
+ kwargs["num_motors"] = NUM_MOTORS
+ kwargs["dofs_per_leg"] = DOFS_PER_LEG
+ kwargs["motor_direction"] = JOINT_DIRECTIONS
+ kwargs["motor_offset"] = JOINT_OFFSETS
+ kwargs["motor_overheat_protection"] = False
+ kwargs["motor_model_class"] = laikago_motor.LaikagoMotorModel
+ kwargs["safety_config"] = None
+
+ super(Laikago, self).__init__(**kwargs)
+
+ def _LoadRobotURDF(self):
+ laikago_urdf_path = self.GetURDFFile()
+ if self._self_collision_enabled:
+ self.quadruped = self._pybullet_client.loadURDF(
+ laikago_urdf_path,
+ self._GetDefaultInitPosition(),
+ self._GetDefaultInitOrientation(),
+ flags=self._pybullet_client.URDF_USE_SELF_COLLISION)
+ else:
+ self.quadruped = self._pybullet_client.loadURDF(
+ laikago_urdf_path, self._GetDefaultInitPosition(),
+ self._GetDefaultInitOrientation())
+
+ def _SettleDownForReset(self, default_motor_angles, reset_time):
+ self.ReceiveObservation()
+
+ if reset_time <= 0:
+ return
+
+ for _ in range(500):
+ self._StepInternal(
+ INIT_MOTOR_ANGLES,
+ motor_control_mode=robot_config.MotorControlMode.POSITION)
+ if default_motor_angles is not None:
+ num_steps_to_reset = int(reset_time / self.time_step)
+ for _ in range(num_steps_to_reset):
+ self._StepInternal(
+ default_motor_angles,
+ motor_control_mode=robot_config.MotorControlMode.POSITION)
+
+ def GetHipPositionsInBaseFrame(self):
+ return _DEFAULT_HIP_POSITIONS
+
+ def GetFootContacts(self):
+ all_contacts = self._pybullet_client.getContactPoints(bodyA=self.quadruped)
+
+ contacts = [False, False, False, False]
+ for contact in all_contacts:
+ # Ignore self contacts
+ if contact[_BODY_B_FIELD_NUMBER] == self.quadruped:
+ continue
+ try:
+ toe_link_index = self._foot_link_ids.index(
+ contact[_LINK_A_FIELD_NUMBER])
+ contacts[toe_link_index] = True
+ except ValueError:
+ continue
+ return contacts
+
+ def ComputeJacobian(self, leg_id):
+ """Compute the Jacobian for a given leg."""
+ # Because of the default rotation in the Laikago URDF, we need to reorder
+ # the rows in the Jacobian matrix.
+ if self._urdf_filename == URDF_WITH_TOES:
+ return super(Laikago, self).ComputeJacobian(leg_id)
+ else:
+ return super(Laikago, self).ComputeJacobian(leg_id)[(2, 0, 1), :]
+
+ def ResetPose(self, add_constraint):
+ del add_constraint
+ for name in self._joint_name_to_id:
+ joint_id = self._joint_name_to_id[name]
+ self._pybullet_client.setJointMotorControl2(
+ bodyIndex=self.quadruped,
+ jointIndex=(joint_id),
+ controlMode=self._pybullet_client.VELOCITY_CONTROL,
+ targetVelocity=0,
+ force=0)
+ for name, i in zip(MOTOR_NAMES, range(len(MOTOR_NAMES))):
+ if "hip_motor_2_chassis_joint" in name:
+ angle = INIT_MOTOR_ANGLES[i] + HIP_JOINT_OFFSET
+ elif "upper_leg_2_hip_motor_joint" in name:
+ angle = INIT_MOTOR_ANGLES[i] + UPPER_LEG_JOINT_OFFSET
+ elif "lower_leg_2_upper_leg_joint" in name:
+ angle = INIT_MOTOR_ANGLES[i] + KNEE_JOINT_OFFSET
+ else:
+ raise ValueError("The name %s is not recognized as a motor joint." %
+ name)
+ self._pybullet_client.resetJointState(
+ self.quadruped, self._joint_name_to_id[name], angle, targetVelocity=0)
+
+ def GetURDFFile(self):
+ return os.path.join(self._urdf_root, "laikago/" + self._urdf_filename)
+
+ def _BuildUrdfIds(self):
+ """Build the link Ids from its name in the URDF file.
+
+ Raises:
+ ValueError: Unknown category of the joint name.
+ """
+ num_joints = self._pybullet_client.getNumJoints(self.quadruped)
+ self._chassis_link_ids = [-1]
+ self._leg_link_ids = []
+ self._motor_link_ids = []
+ self._knee_link_ids = []
+ self._foot_link_ids = []
+
+ for i in range(num_joints):
+ joint_info = self._pybullet_client.getJointInfo(self.quadruped, i)
+ joint_name = joint_info[1].decode("UTF-8")
+ joint_id = self._joint_name_to_id[joint_name]
+ if CHASSIS_NAME_PATTERN.match(joint_name):
+ self._chassis_link_ids.append(joint_id)
+ elif MOTOR_NAME_PATTERN.match(joint_name):
+ self._motor_link_ids.append(joint_id)
+ # We either treat the lower leg or the toe as the foot link, depending on
+ # the urdf version used.
+ elif KNEE_NAME_PATTERN.match(joint_name):
+ self._knee_link_ids.append(joint_id)
+ elif TOE_NAME_PATTERN.match(joint_name):
+ assert self._urdf_filename == URDF_WITH_TOES
+ self._foot_link_ids.append(joint_id)
+ else:
+ raise ValueError("Unknown category of joint %s" % joint_name)
+
+ self._leg_link_ids.extend(self._knee_link_ids)
+ self._leg_link_ids.extend(self._foot_link_ids)
+
+ if self._urdf_filename == URDF_NO_TOES:
+ self._foot_link_ids.extend(self._knee_link_ids)
+
+ assert len(self._foot_link_ids) == NUM_LEGS
+ self._chassis_link_ids.sort()
+ self._motor_link_ids.sort()
+ self._knee_link_ids.sort()
+ self._foot_link_ids.sort()
+ self._leg_link_ids.sort()
+
+ return
+
+ def _GetMotorNames(self):
+ return MOTOR_NAMES
+
+ def _GetDefaultInitPosition(self):
+ if self._on_rack:
+ return INIT_RACK_POSITION
+ else:
+ return INIT_POSITION
+
+ def _GetDefaultInitOrientation(self):
+ # The Laikago URDF assumes the initial pose of heading towards z axis,
+ # and belly towards y axis. The following transformation is to transform
+ # the Laikago initial orientation to our commonly used orientation: heading
+ # towards -x direction, and z axis is the up direction.
+ if self._urdf_filename == URDF_WITH_TOES:
+ return [0, 0, 0, 1]
+ else:
+ return transformations.quaternion_from_euler(
+ ai=math.pi / 2.0, aj=0, ak=math.pi / 2.0, axes="sxyz")
+
+ def GetDefaultInitPosition(self):
+ """Get default initial base position."""
+ return self._GetDefaultInitPosition()
+
+ def GetDefaultInitOrientation(self):
+ """Get default initial base orientation."""
+ return self._GetDefaultInitOrientation()
+
+ def GetDefaultInitJointPose(self):
+ """Get default initial joint pose."""
+ joint_pose = (INIT_MOTOR_ANGLES + JOINT_OFFSETS) * JOINT_DIRECTIONS
+ return joint_pose
+
+ def ApplyAction(self, motor_commands, motor_control_mode=None):
+ """Clips and then apply the motor commands using the motor model.
+
+ Args:
+ motor_commands: np.array. Can be motor angles, torques, hybrid commands,
+ or motor pwms (for Minitaur only).N
+ motor_control_mode: A MotorControlMode enum.
+ """
+ if self._enable_clip_motor_commands:
+ motor_commands = self._ClipMotorCommands(motor_commands)
+
+ super(Laikago, self).ApplyAction(motor_commands, motor_control_mode)
+ return
+
+ def _ClipMotorCommands(self, motor_commands):
+ """Clips motor commands.
+
+ Args:
+ motor_commands: np.array. Can be motor angles, torques, hybrid commands,
+ or motor pwms (for Minitaur only).
+
+ Returns:
+ Clipped motor commands.
+ """
+
+ # clamp the motor command by the joint limit, in case weired things happens
+ max_angle_change = MAX_MOTOR_ANGLE_CHANGE_PER_STEP
+ current_motor_angles = self.GetMotorAngles()
+ motor_commands = np.clip(motor_commands,
+ current_motor_angles - max_angle_change,
+ current_motor_angles + max_angle_change)
+ return motor_commands
+
+ @classmethod
+ def GetConstants(cls):
+ del cls
+ return laikago_constants
+
+ # The following functions are added for the migration purpose. Will be removed
+ # after the migration is complete.
+
+ @property
+ def robot_id(self):
+ return self.quadruped
+
+ @property
+ def base_position(self):
+ return self.GetBasePosition()
+
+ @property
+ def base_roll_pitch_yaw(self):
+ return self.GetTrueBaseRollPitchYaw()
+
+ @property
+ def timestamp(self):
+ return self.GetTimeSinceReset()
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/robots/laikago_constants.py b/examples/pybullet/gym/pybullet_envs/minitaur/robots/laikago_constants.py
new file mode 100644
index 000000000..d07e73071
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/robots/laikago_constants.py
@@ -0,0 +1,120 @@
+# Lint as: python3
+"""Defines the laikago robot related constants and URDF specs."""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+import collections
+import gin
+
+URDF_PATH = "laikago/laikago_toes_zup.urdf"
+
+NUM_MOTORS = 12
+NUM_LEGS = 4
+MOTORS_PER_LEG = 3
+
+INIT_RACK_POSITION = [0, 0, 1]
+INIT_POSITION = [0, 0, 0.48]
+
+# Will be default to (0, 0, 0, 1) once the new laikago_toes_zup.urdf checked in.
+INIT_ORIENTATION = [0, 0, 0, 1]
+
+# Can be different from the motors, although for laikago they are the same list.
+JOINT_NAMES = (
+ # front right leg
+ "FR_hip_motor_2_chassis_joint",
+ "FR_upper_leg_2_hip_motor_joint",
+ "FR_lower_leg_2_upper_leg_joint",
+ # front left leg
+ "FL_hip_motor_2_chassis_joint",
+ "FL_upper_leg_2_hip_motor_joint",
+ "FL_lower_leg_2_upper_leg_joint",
+ # rear right leg
+ "RR_hip_motor_2_chassis_joint",
+ "RR_upper_leg_2_hip_motor_joint",
+ "RR_lower_leg_2_upper_leg_joint",
+ # rear left leg
+ "RL_hip_motor_2_chassis_joint",
+ "RL_upper_leg_2_hip_motor_joint",
+ "RL_lower_leg_2_upper_leg_joint",
+)
+
+INIT_ABDUCTION_ANGLE = 0
+INIT_HIP_ANGLE = 0.67
+INIT_KNEE_ANGLE = -1.25
+
+# Note this matches the Laikago SDK/control convention, but is different from
+# URDF's internal joint angles which needs to be computed using the joint
+# offsets and directions. The conversion formula is (sdk_joint_angle + offset) *
+# joint direction.
+INIT_JOINT_ANGLES = collections.OrderedDict(
+ zip(JOINT_NAMES,
+ (INIT_ABDUCTION_ANGLE, INIT_HIP_ANGLE, INIT_KNEE_ANGLE) * NUM_LEGS))
+
+# Used to convert the robot SDK joint angles to URDF joint angles.
+JOINT_DIRECTIONS = collections.OrderedDict(
+ zip(JOINT_NAMES, (-1, 1, 1, 1, 1, 1, -1, 1, 1, 1, 1, 1)))
+
+HIP_JOINT_OFFSET = 0.0
+UPPER_LEG_JOINT_OFFSET = -0.6
+KNEE_JOINT_OFFSET = 0.66
+
+# Used to convert the robot SDK joint angles to URDF joint angles.
+JOINT_OFFSETS = collections.OrderedDict(
+ zip(JOINT_NAMES,
+ [HIP_JOINT_OFFSET, UPPER_LEG_JOINT_OFFSET, KNEE_JOINT_OFFSET] *
+ NUM_LEGS))
+
+LEG_NAMES = (
+ "front_right",
+ "front_left",
+ "rear_right",
+ "rear_left",
+)
+
+LEG_ORDER = (
+ "front_right",
+ "front_left",
+ "back_right",
+ "back_left",
+)
+
+END_EFFECTOR_NAMES = (
+ "jtoeFR",
+ "jtoeFL",
+ "jtoeRR",
+ "jtoeRL",
+)
+
+MOTOR_NAMES = JOINT_NAMES
+MOTOR_GROUP = collections.OrderedDict((
+ (LEG_NAMES[0], JOINT_NAMES[0:3]),
+ (LEG_NAMES[1], JOINT_NAMES[3:6]),
+ (LEG_NAMES[2], JOINT_NAMES[6:9]),
+ (LEG_NAMES[3], JOINT_NAMES[9:12]),
+))
+
+# Regulates the joint angle change when in position control mode.
+MAX_MOTOR_ANGLE_CHANGE_PER_STEP = 0.12
+
+# The hip joint location in the CoM frame.
+HIP_POSITIONS = collections.OrderedDict((
+ (LEG_NAMES[0], (0.21, -0.1157, 0)),
+ (LEG_NAMES[1], (0.21, 0.1157, 0)),
+ (LEG_NAMES[2], (-0.21, -0.1157, 0)),
+ (LEG_NAMES[3], (-0.21, 0.1157, 0)),
+))
+
+# Add the gin constants to be used for gin binding in config. Append "LAIKAGO_"
+# for unique binding names.
+gin.constant("laikago_constants.LAIKAGO_NUM_MOTORS", NUM_MOTORS)
+gin.constant("laikago_constants.LAIKAGO_URDF_PATH", URDF_PATH)
+gin.constant("laikago_constants.LAIKAGO_INIT_POSITION", INIT_POSITION)
+gin.constant("laikago_constants.LAIKAGO_INIT_ORIENTATION", INIT_ORIENTATION)
+gin.constant("laikago_constants.LAIKAGO_INIT_JOINT_ANGLES", INIT_JOINT_ANGLES)
+gin.constant("laikago_constants.LAIKAGO_JOINT_DIRECTIONS", JOINT_DIRECTIONS)
+gin.constant("laikago_constants.LAIKAGO_JOINT_OFFSETS", JOINT_OFFSETS)
+gin.constant("laikago_constants.LAIKAGO_MOTOR_NAMES", MOTOR_NAMES)
+gin.constant("laikago_constants.LAIKAGO_END_EFFECTOR_NAMES", END_EFFECTOR_NAMES)
+gin.constant("laikago_constants.LAIKAGO_MOTOR_GROUP", MOTOR_GROUP)
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/robots/laikago_interface.proto b/examples/pybullet/gym/pybullet_envs/minitaur/robots/laikago_interface.proto
new file mode 100644
index 000000000..3e2a31a97
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/robots/laikago_interface.proto
@@ -0,0 +1,181 @@
+syntax = "proto3";
+
+package minitaur_fluxworks.control;
+
+import "timestamp.proto";
+import "vector.proto";
+
+// A general motor command.
+message MotorCommand {
+ // The unique motor id.
+ uint32 motor_id = 1;
+
+ // The motor angle.
+ float position = 2;
+
+ float position_gain = 3;
+
+ // The motor velocity.
+ float velocity = 4;
+ float velocity_gain = 5;
+
+ // The feed forward torque.
+ float torque = 6;
+}
+
+// LED command for the foot.
+message Led {
+ uint32 leg_id = 1;
+ uint32 r = 2;
+ uint32 g = 3;
+ uint32 b = 4;
+}
+
+// The message type for Laikago's motor command.
+message LaikagoCommand {
+ google.protobuf.Timestamp timestamp = 1;
+ enum ControlMode {
+ CONTROL_MODE_UNSPECIFIED = 0;
+ CONTROL_MODE_POSITION = 1;
+ CONTROL_MODE_TORQUE = 2;
+ CONTROL_MODE_HYBRID = 3;
+ }
+ ControlMode control_mode = 2;
+ repeated MotorCommand motor_command = 3;
+ repeated Led led = 4;
+}
+
+// Empty message just to request a state from the control server.
+message LaikagoStateRequest {}
+
+message Imu {
+ robotics.messages.Vector4f quaternion = 1;
+
+ // The unit is rad/s
+ robotics.messages.Vector3f gyroscope = 2;
+
+ // The unit is m/s^2
+ robotics.messages.Vector3f acceleration = 3;
+
+ // The unit is rad
+ robotics.messages.Vector3f rpy = 4;
+
+ // The IMU temperature.
+ float temperature = 5;
+}
+
+message MotorState {
+ uint32 motor_id = 1;
+ uint32 mode = 2;
+
+ float position = 3;
+ // Position/Velocity gains cannot be read from the motor. We just save the
+ // last used value.
+ float position_gain = 4;
+ float velocity = 5;
+ float velocity_gain = 6;
+ float torque = 7;
+ float temperature = 8;
+}
+
+message ContactState {
+ uint32 leg_id = 1;
+
+ // Contact force is measured in one dimension for Laikago.
+ float force = 2;
+
+ // The contact force measurement direction.
+ robotics.messages.Vector3f axis = 3;
+}
+
+// The message type for Laikago's low level state.
+message LaikagoState {
+ google.protobuf.Timestamp timestamp = 1;
+ uint32 control_level = 2;
+ Imu imu = 3;
+ repeated MotorState motor_state = 4;
+ repeated ContactState contact_state = 5;
+ // The microcontroller_time is millis.
+ uint32 microcontroller_time_millis = 6;
+ bytes wireless_remote = 7;
+ uint32 crc = 8;
+}
+
+message LaikagoCommandState {
+ LaikagoCommand command = 1;
+ LaikagoState state = 2;
+}
+
+// The optional gRPC interface for Laikago control.
+service LaikagoControlGrpcInterface {
+ // Sends the low level control command and receives a state.
+ rpc SendCommand(LaikagoCommand) returns (LaikagoState) {}
+
+ // Receives a robot state without sending motor commands.
+ rpc GetState(LaikagoStateRequest) returns (LaikagoState) {}
+}
+
+// Reserved for Laikago's high level command.
+message LaikagoHighLevelCommand {
+ google.protobuf.Timestamp timestamp = 1;
+ uint32 control_level = 2;
+
+ // 1 for standing and 2 for walking.
+ uint32 control_mode = 3;
+
+ // The normalized speed tuple (x, y, \omega_z)
+ robotics.messages.Vector3f walk_speed = 4;
+
+ float body_height = 5;
+ float foot_clearance_height = 6;
+
+ // The target roll, pitch, yaw of the body in the stand mode.
+ robotics.messages.Vector3f rpy = 7;
+}
+
+// Reserved for Laikago's high level status.
+message LaikagoHighLevelState {
+ google.protobuf.Timestamp timestamp = 1;
+ uint32 control_level = 2;
+
+ // 1 for standing and 2 for walking.
+ uint32 control_mode = 3;
+ Imu imu = 4;
+
+ // The normalized speed tuple (x, y, \omega_z)
+ robotics.messages.Vector3f walk_speed = 5;
+
+ // In stand mode.
+ float body_height = 8;
+ float up_down_speed = 9;
+
+ // The com position estimation. Will drift in x-y plane.
+ robotics.messages.Vector3f com_position = 10;
+ repeated robotics.messages.Vector3f foot_position_to_com = 11;
+ repeated robotics.messages.Vector3f foot_velocity_to_com = 12;
+ repeated ContactState contact_state = 13;
+ // The microcontroller_time is millis.
+ uint32 microcontroller_time_millis = 14;
+ // Bytes 4-7: slider_lx (side step speed); Bytes 8-11: slider_rx (twisting
+ // speed); Bytes 12-15: -slider_ry. Bytes 16-19: (slider_r + 1) / 2; Bytes
+ // 20-23: -slider_ly (forward/backward speed). Each float number (4 bytes) are
+ // packed using big endian convention.
+ bytes wireless_remote = 15;
+ uint32 crc = 16;
+}
+
+message LaikagoHighLevelStateRequest {}
+
+message LaikagoHighLevelCommandState {
+ LaikagoHighLevelCommand command = 1;
+ LaikagoHighLevelState state = 2;
+}
+
+// The optional gRPC interface for Laikago control.
+service LaikagoHighLevelControlGrpcInterface {
+ // Sends the high level control command and receives a state.
+ rpc SendCommand(LaikagoHighLevelCommand) returns (LaikagoHighLevelState) {}
+
+ // Requests a state without sending commands.
+ rpc GetState(LaikagoHighLevelStateRequest) returns (LaikagoHighLevelState) {}
+}
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/robots/laikago_interface_pb2.py b/examples/pybullet/gym/pybullet_envs/minitaur/robots/laikago_interface_pb2.py
new file mode 100644
index 000000000..7d67f55cc
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/robots/laikago_interface_pb2.py
@@ -0,0 +1,1040 @@
+# -*- coding: utf-8 -*-
+# Generated by the protocol buffer compiler. DO NOT EDIT!
+# source: laikago_interface.proto
+"""Generated protocol buffer code."""
+from google.protobuf import descriptor as _descriptor
+from google.protobuf import message as _message
+from google.protobuf import reflection as _reflection
+from google.protobuf import symbol_database as _symbol_database
+# @@protoc_insertion_point(imports)
+
+_sym_db = _symbol_database.Default()
+
+
+from pybullet_envs.minitaur.robots import timestamp_pb2 as timestamp__pb2
+from pybullet_envs.minitaur.robots import vector_pb2 as vector__pb2
+
+
+
+DESCRIPTOR = _descriptor.FileDescriptor(
+ name='laikago_interface.proto',
+ package='minitaur_fluxworks.control',
+ syntax='proto3',
+ serialized_options=None,
+ create_key=_descriptor._internal_create_key,
+ serialized_pb=b'\n\x17laikago_interface.proto\x12\x1aminitaur_fluxworks.control\x1a\x0ftimestamp.proto\x1a\x0cvector.proto\"\x82\x01\n\x0cMotorCommand\x12\x10\n\x08motor_id\x18\x01 \x01(\r\x12\x10\n\x08position\x18\x02 \x01(\x02\x12\x15\n\rposition_gain\x18\x03 \x01(\x02\x12\x10\n\x08velocity\x18\x04 \x01(\x02\x12\x15\n\rvelocity_gain\x18\x05 \x01(\x02\x12\x0e\n\x06torque\x18\x06 \x01(\x02\"6\n\x03Led\x12\x0e\n\x06leg_id\x18\x01 \x01(\r\x12\t\n\x01r\x18\x02 \x01(\r\x12\t\n\x01g\x18\x03 \x01(\r\x12\t\n\x01\x62\x18\x04 \x01(\r\"\xf6\x02\n\x0eLaikagoCommand\x12-\n\ttimestamp\x18\x01 \x01(\x0b\x32\x1a.google.protobuf.Timestamp\x12L\n\x0c\x63ontrol_mode\x18\x02 \x01(\x0e\x32\x36.minitaur_fluxworks.control.LaikagoCommand.ControlMode\x12?\n\rmotor_command\x18\x03 \x03(\x0b\x32(.minitaur_fluxworks.control.MotorCommand\x12,\n\x03led\x18\x04 \x03(\x0b\x32\x1f.minitaur_fluxworks.control.Led\"x\n\x0b\x43ontrolMode\x12\x1c\n\x18\x43ONTROL_MODE_UNSPECIFIED\x10\x00\x12\x19\n\x15\x43ONTROL_MODE_POSITION\x10\x01\x12\x17\n\x13\x43ONTROL_MODE_TORQUE\x10\x02\x12\x17\n\x13\x43ONTROL_MODE_HYBRID\x10\x03\"\x15\n\x13LaikagoStateRequest\"\xd8\x01\n\x03Imu\x12/\n\nquaternion\x18\x01 \x01(\x0b\x32\x1b.robotics.messages.Vector4f\x12.\n\tgyroscope\x18\x02 \x01(\x0b\x32\x1b.robotics.messages.Vector3f\x12\x31\n\x0c\x61\x63\x63\x65leration\x18\x03 \x01(\x0b\x32\x1b.robotics.messages.Vector3f\x12(\n\x03rpy\x18\x04 \x01(\x0b\x32\x1b.robotics.messages.Vector3f\x12\x13\n\x0btemperature\x18\x05 \x01(\x02\"\xa3\x01\n\nMotorState\x12\x10\n\x08motor_id\x18\x01 \x01(\r\x12\x0c\n\x04mode\x18\x02 \x01(\r\x12\x10\n\x08position\x18\x03 \x01(\x02\x12\x15\n\rposition_gain\x18\x04 \x01(\x02\x12\x10\n\x08velocity\x18\x05 \x01(\x02\x12\x15\n\rvelocity_gain\x18\x06 \x01(\x02\x12\x0e\n\x06torque\x18\x07 \x01(\x02\x12\x13\n\x0btemperature\x18\x08 \x01(\x02\"X\n\x0c\x43ontactState\x12\x0e\n\x06leg_id\x18\x01 \x01(\r\x12\r\n\x05\x66orce\x18\x02 \x01(\x02\x12)\n\x04\x61xis\x18\x03 \x01(\x0b\x32\x1b.robotics.messages.Vector3f\"\xcb\x02\n\x0cLaikagoState\x12-\n\ttimestamp\x18\x01 \x01(\x0b\x32\x1a.google.protobuf.Timestamp\x12\x15\n\rcontrol_level\x18\x02 \x01(\r\x12,\n\x03imu\x18\x03 \x01(\x0b\x32\x1f.minitaur_fluxworks.control.Imu\x12;\n\x0bmotor_state\x18\x04 \x03(\x0b\x32&.minitaur_fluxworks.control.MotorState\x12?\n\rcontact_state\x18\x05 \x03(\x0b\x32(.minitaur_fluxworks.control.ContactState\x12#\n\x1bmicrocontroller_time_millis\x18\x06 \x01(\r\x12\x17\n\x0fwireless_remote\x18\x07 \x01(\x0c\x12\x0b\n\x03\x63rc\x18\x08 \x01(\r\"\x8b\x01\n\x13LaikagoCommandState\x12;\n\x07\x63ommand\x18\x01 \x01(\x0b\x32*.minitaur_fluxworks.control.LaikagoCommand\x12\x37\n\x05state\x18\x02 \x01(\x0b\x32(.minitaur_fluxworks.control.LaikagoState\"\x84\x02\n\x17LaikagoHighLevelCommand\x12-\n\ttimestamp\x18\x01 \x01(\x0b\x32\x1a.google.protobuf.Timestamp\x12\x15\n\rcontrol_level\x18\x02 \x01(\r\x12\x14\n\x0c\x63ontrol_mode\x18\x03 \x01(\r\x12/\n\nwalk_speed\x18\x04 \x01(\x0b\x32\x1b.robotics.messages.Vector3f\x12\x13\n\x0b\x62ody_height\x18\x05 \x01(\x02\x12\x1d\n\x15\x66oot_clearance_height\x18\x06 \x01(\x02\x12(\n\x03rpy\x18\x07 \x01(\x0b\x32\x1b.robotics.messages.Vector3f\"\xb3\x04\n\x15LaikagoHighLevelState\x12-\n\ttimestamp\x18\x01 \x01(\x0b\x32\x1a.google.protobuf.Timestamp\x12\x15\n\rcontrol_level\x18\x02 \x01(\r\x12\x14\n\x0c\x63ontrol_mode\x18\x03 \x01(\r\x12,\n\x03imu\x18\x04 \x01(\x0b\x32\x1f.minitaur_fluxworks.control.Imu\x12/\n\nwalk_speed\x18\x05 \x01(\x0b\x32\x1b.robotics.messages.Vector3f\x12\x13\n\x0b\x62ody_height\x18\x08 \x01(\x02\x12\x15\n\rup_down_speed\x18\t \x01(\x02\x12\x31\n\x0c\x63om_position\x18\n \x01(\x0b\x32\x1b.robotics.messages.Vector3f\x12\x39\n\x14\x66oot_position_to_com\x18\x0b \x03(\x0b\x32\x1b.robotics.messages.Vector3f\x12\x39\n\x14\x66oot_velocity_to_com\x18\x0c \x03(\x0b\x32\x1b.robotics.messages.Vector3f\x12?\n\rcontact_state\x18\r \x03(\x0b\x32(.minitaur_fluxworks.control.ContactState\x12#\n\x1bmicrocontroller_time_millis\x18\x0e \x01(\r\x12\x17\n\x0fwireless_remote\x18\x0f \x01(\x0c\x12\x0b\n\x03\x63rc\x18\x10 \x01(\r\"\x1e\n\x1cLaikagoHighLevelStateRequest\"\xa6\x01\n\x1cLaikagoHighLevelCommandState\x12\x44\n\x07\x63ommand\x18\x01 \x01(\x0b\x32\x33.minitaur_fluxworks.control.LaikagoHighLevelCommand\x12@\n\x05state\x18\x02 \x01(\x0b\x32\x31.minitaur_fluxworks.control.LaikagoHighLevelState2\xed\x01\n\x1bLaikagoControlGrpcInterface\x12\x65\n\x0bSendCommand\x12*.minitaur_fluxworks.control.LaikagoCommand\x1a(.minitaur_fluxworks.control.LaikagoState\"\x00\x12g\n\x08GetState\x12/.minitaur_fluxworks.control.LaikagoStateRequest\x1a(.minitaur_fluxworks.control.LaikagoState\"\x00\x32\x9a\x02\n$LaikagoHighLevelControlGrpcInterface\x12w\n\x0bSendCommand\x12\x33.minitaur_fluxworks.control.LaikagoHighLevelCommand\x1a\x31.minitaur_fluxworks.control.LaikagoHighLevelState\"\x00\x12y\n\x08GetState\x12\x38.minitaur_fluxworks.control.LaikagoHighLevelStateRequest\x1a\x31.minitaur_fluxworks.control.LaikagoHighLevelState\"\x00\x62\x06proto3'
+ ,
+ dependencies=[timestamp__pb2.DESCRIPTOR,vector__pb2.DESCRIPTOR,])
+
+
+
+_LAIKAGOCOMMAND_CONTROLMODE = _descriptor.EnumDescriptor(
+ name='ControlMode',
+ full_name='minitaur_fluxworks.control.LaikagoCommand.ControlMode',
+ filename=None,
+ file=DESCRIPTOR,
+ create_key=_descriptor._internal_create_key,
+ values=[
+ _descriptor.EnumValueDescriptor(
+ name='CONTROL_MODE_UNSPECIFIED', index=0, number=0,
+ serialized_options=None,
+ type=None,
+ create_key=_descriptor._internal_create_key),
+ _descriptor.EnumValueDescriptor(
+ name='CONTROL_MODE_POSITION', index=1, number=1,
+ serialized_options=None,
+ type=None,
+ create_key=_descriptor._internal_create_key),
+ _descriptor.EnumValueDescriptor(
+ name='CONTROL_MODE_TORQUE', index=2, number=2,
+ serialized_options=None,
+ type=None,
+ create_key=_descriptor._internal_create_key),
+ _descriptor.EnumValueDescriptor(
+ name='CONTROL_MODE_HYBRID', index=3, number=3,
+ serialized_options=None,
+ type=None,
+ create_key=_descriptor._internal_create_key),
+ ],
+ containing_type=None,
+ serialized_options=None,
+ serialized_start=530,
+ serialized_end=650,
+)
+_sym_db.RegisterEnumDescriptor(_LAIKAGOCOMMAND_CONTROLMODE)
+
+
+_MOTORCOMMAND = _descriptor.Descriptor(
+ name='MotorCommand',
+ full_name='minitaur_fluxworks.control.MotorCommand',
+ filename=None,
+ file=DESCRIPTOR,
+ containing_type=None,
+ create_key=_descriptor._internal_create_key,
+ fields=[
+ _descriptor.FieldDescriptor(
+ name='motor_id', full_name='minitaur_fluxworks.control.MotorCommand.motor_id', index=0,
+ number=1, type=13, cpp_type=3, label=1,
+ has_default_value=False, default_value=0,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='position', full_name='minitaur_fluxworks.control.MotorCommand.position', index=1,
+ number=2, type=2, cpp_type=6, label=1,
+ has_default_value=False, default_value=float(0),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='position_gain', full_name='minitaur_fluxworks.control.MotorCommand.position_gain', index=2,
+ number=3, type=2, cpp_type=6, label=1,
+ has_default_value=False, default_value=float(0),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='velocity', full_name='minitaur_fluxworks.control.MotorCommand.velocity', index=3,
+ number=4, type=2, cpp_type=6, label=1,
+ has_default_value=False, default_value=float(0),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='velocity_gain', full_name='minitaur_fluxworks.control.MotorCommand.velocity_gain', index=4,
+ number=5, type=2, cpp_type=6, label=1,
+ has_default_value=False, default_value=float(0),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='torque', full_name='minitaur_fluxworks.control.MotorCommand.torque', index=5,
+ number=6, type=2, cpp_type=6, label=1,
+ has_default_value=False, default_value=float(0),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ ],
+ extensions=[
+ ],
+ nested_types=[],
+ enum_types=[
+ ],
+ serialized_options=None,
+ is_extendable=False,
+ syntax='proto3',
+ extension_ranges=[],
+ oneofs=[
+ ],
+ serialized_start=87,
+ serialized_end=217,
+)
+
+
+_LED = _descriptor.Descriptor(
+ name='Led',
+ full_name='minitaur_fluxworks.control.Led',
+ filename=None,
+ file=DESCRIPTOR,
+ containing_type=None,
+ create_key=_descriptor._internal_create_key,
+ fields=[
+ _descriptor.FieldDescriptor(
+ name='leg_id', full_name='minitaur_fluxworks.control.Led.leg_id', index=0,
+ number=1, type=13, cpp_type=3, label=1,
+ has_default_value=False, default_value=0,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='r', full_name='minitaur_fluxworks.control.Led.r', index=1,
+ number=2, type=13, cpp_type=3, label=1,
+ has_default_value=False, default_value=0,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='g', full_name='minitaur_fluxworks.control.Led.g', index=2,
+ number=3, type=13, cpp_type=3, label=1,
+ has_default_value=False, default_value=0,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='b', full_name='minitaur_fluxworks.control.Led.b', index=3,
+ number=4, type=13, cpp_type=3, label=1,
+ has_default_value=False, default_value=0,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ ],
+ extensions=[
+ ],
+ nested_types=[],
+ enum_types=[
+ ],
+ serialized_options=None,
+ is_extendable=False,
+ syntax='proto3',
+ extension_ranges=[],
+ oneofs=[
+ ],
+ serialized_start=219,
+ serialized_end=273,
+)
+
+
+_LAIKAGOCOMMAND = _descriptor.Descriptor(
+ name='LaikagoCommand',
+ full_name='minitaur_fluxworks.control.LaikagoCommand',
+ filename=None,
+ file=DESCRIPTOR,
+ containing_type=None,
+ create_key=_descriptor._internal_create_key,
+ fields=[
+ _descriptor.FieldDescriptor(
+ name='timestamp', full_name='minitaur_fluxworks.control.LaikagoCommand.timestamp', index=0,
+ number=1, type=11, cpp_type=10, label=1,
+ has_default_value=False, default_value=None,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='control_mode', full_name='minitaur_fluxworks.control.LaikagoCommand.control_mode', index=1,
+ number=2, type=14, cpp_type=8, label=1,
+ has_default_value=False, default_value=0,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='motor_command', full_name='minitaur_fluxworks.control.LaikagoCommand.motor_command', index=2,
+ number=3, type=11, cpp_type=10, label=3,
+ has_default_value=False, default_value=[],
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='led', full_name='minitaur_fluxworks.control.LaikagoCommand.led', index=3,
+ number=4, type=11, cpp_type=10, label=3,
+ has_default_value=False, default_value=[],
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ ],
+ extensions=[
+ ],
+ nested_types=[],
+ enum_types=[
+ _LAIKAGOCOMMAND_CONTROLMODE,
+ ],
+ serialized_options=None,
+ is_extendable=False,
+ syntax='proto3',
+ extension_ranges=[],
+ oneofs=[
+ ],
+ serialized_start=276,
+ serialized_end=650,
+)
+
+
+_LAIKAGOSTATEREQUEST = _descriptor.Descriptor(
+ name='LaikagoStateRequest',
+ full_name='minitaur_fluxworks.control.LaikagoStateRequest',
+ filename=None,
+ file=DESCRIPTOR,
+ containing_type=None,
+ create_key=_descriptor._internal_create_key,
+ fields=[
+ ],
+ extensions=[
+ ],
+ nested_types=[],
+ enum_types=[
+ ],
+ serialized_options=None,
+ is_extendable=False,
+ syntax='proto3',
+ extension_ranges=[],
+ oneofs=[
+ ],
+ serialized_start=652,
+ serialized_end=673,
+)
+
+
+_IMU = _descriptor.Descriptor(
+ name='Imu',
+ full_name='minitaur_fluxworks.control.Imu',
+ filename=None,
+ file=DESCRIPTOR,
+ containing_type=None,
+ create_key=_descriptor._internal_create_key,
+ fields=[
+ _descriptor.FieldDescriptor(
+ name='quaternion', full_name='minitaur_fluxworks.control.Imu.quaternion', index=0,
+ number=1, type=11, cpp_type=10, label=1,
+ has_default_value=False, default_value=None,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='gyroscope', full_name='minitaur_fluxworks.control.Imu.gyroscope', index=1,
+ number=2, type=11, cpp_type=10, label=1,
+ has_default_value=False, default_value=None,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='acceleration', full_name='minitaur_fluxworks.control.Imu.acceleration', index=2,
+ number=3, type=11, cpp_type=10, label=1,
+ has_default_value=False, default_value=None,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='rpy', full_name='minitaur_fluxworks.control.Imu.rpy', index=3,
+ number=4, type=11, cpp_type=10, label=1,
+ has_default_value=False, default_value=None,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='temperature', full_name='minitaur_fluxworks.control.Imu.temperature', index=4,
+ number=5, type=2, cpp_type=6, label=1,
+ has_default_value=False, default_value=float(0),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ ],
+ extensions=[
+ ],
+ nested_types=[],
+ enum_types=[
+ ],
+ serialized_options=None,
+ is_extendable=False,
+ syntax='proto3',
+ extension_ranges=[],
+ oneofs=[
+ ],
+ serialized_start=676,
+ serialized_end=892,
+)
+
+
+_MOTORSTATE = _descriptor.Descriptor(
+ name='MotorState',
+ full_name='minitaur_fluxworks.control.MotorState',
+ filename=None,
+ file=DESCRIPTOR,
+ containing_type=None,
+ create_key=_descriptor._internal_create_key,
+ fields=[
+ _descriptor.FieldDescriptor(
+ name='motor_id', full_name='minitaur_fluxworks.control.MotorState.motor_id', index=0,
+ number=1, type=13, cpp_type=3, label=1,
+ has_default_value=False, default_value=0,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='mode', full_name='minitaur_fluxworks.control.MotorState.mode', index=1,
+ number=2, type=13, cpp_type=3, label=1,
+ has_default_value=False, default_value=0,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='position', full_name='minitaur_fluxworks.control.MotorState.position', index=2,
+ number=3, type=2, cpp_type=6, label=1,
+ has_default_value=False, default_value=float(0),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='position_gain', full_name='minitaur_fluxworks.control.MotorState.position_gain', index=3,
+ number=4, type=2, cpp_type=6, label=1,
+ has_default_value=False, default_value=float(0),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='velocity', full_name='minitaur_fluxworks.control.MotorState.velocity', index=4,
+ number=5, type=2, cpp_type=6, label=1,
+ has_default_value=False, default_value=float(0),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='velocity_gain', full_name='minitaur_fluxworks.control.MotorState.velocity_gain', index=5,
+ number=6, type=2, cpp_type=6, label=1,
+ has_default_value=False, default_value=float(0),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='torque', full_name='minitaur_fluxworks.control.MotorState.torque', index=6,
+ number=7, type=2, cpp_type=6, label=1,
+ has_default_value=False, default_value=float(0),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='temperature', full_name='minitaur_fluxworks.control.MotorState.temperature', index=7,
+ number=8, type=2, cpp_type=6, label=1,
+ has_default_value=False, default_value=float(0),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ ],
+ extensions=[
+ ],
+ nested_types=[],
+ enum_types=[
+ ],
+ serialized_options=None,
+ is_extendable=False,
+ syntax='proto3',
+ extension_ranges=[],
+ oneofs=[
+ ],
+ serialized_start=895,
+ serialized_end=1058,
+)
+
+
+_CONTACTSTATE = _descriptor.Descriptor(
+ name='ContactState',
+ full_name='minitaur_fluxworks.control.ContactState',
+ filename=None,
+ file=DESCRIPTOR,
+ containing_type=None,
+ create_key=_descriptor._internal_create_key,
+ fields=[
+ _descriptor.FieldDescriptor(
+ name='leg_id', full_name='minitaur_fluxworks.control.ContactState.leg_id', index=0,
+ number=1, type=13, cpp_type=3, label=1,
+ has_default_value=False, default_value=0,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='force', full_name='minitaur_fluxworks.control.ContactState.force', index=1,
+ number=2, type=2, cpp_type=6, label=1,
+ has_default_value=False, default_value=float(0),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='axis', full_name='minitaur_fluxworks.control.ContactState.axis', index=2,
+ number=3, type=11, cpp_type=10, label=1,
+ has_default_value=False, default_value=None,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ ],
+ extensions=[
+ ],
+ nested_types=[],
+ enum_types=[
+ ],
+ serialized_options=None,
+ is_extendable=False,
+ syntax='proto3',
+ extension_ranges=[],
+ oneofs=[
+ ],
+ serialized_start=1060,
+ serialized_end=1148,
+)
+
+
+_LAIKAGOSTATE = _descriptor.Descriptor(
+ name='LaikagoState',
+ full_name='minitaur_fluxworks.control.LaikagoState',
+ filename=None,
+ file=DESCRIPTOR,
+ containing_type=None,
+ create_key=_descriptor._internal_create_key,
+ fields=[
+ _descriptor.FieldDescriptor(
+ name='timestamp', full_name='minitaur_fluxworks.control.LaikagoState.timestamp', index=0,
+ number=1, type=11, cpp_type=10, label=1,
+ has_default_value=False, default_value=None,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='control_level', full_name='minitaur_fluxworks.control.LaikagoState.control_level', index=1,
+ number=2, type=13, cpp_type=3, label=1,
+ has_default_value=False, default_value=0,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='imu', full_name='minitaur_fluxworks.control.LaikagoState.imu', index=2,
+ number=3, type=11, cpp_type=10, label=1,
+ has_default_value=False, default_value=None,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='motor_state', full_name='minitaur_fluxworks.control.LaikagoState.motor_state', index=3,
+ number=4, type=11, cpp_type=10, label=3,
+ has_default_value=False, default_value=[],
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='contact_state', full_name='minitaur_fluxworks.control.LaikagoState.contact_state', index=4,
+ number=5, type=11, cpp_type=10, label=3,
+ has_default_value=False, default_value=[],
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='microcontroller_time_millis', full_name='minitaur_fluxworks.control.LaikagoState.microcontroller_time_millis', index=5,
+ number=6, type=13, cpp_type=3, label=1,
+ has_default_value=False, default_value=0,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='wireless_remote', full_name='minitaur_fluxworks.control.LaikagoState.wireless_remote', index=6,
+ number=7, type=12, cpp_type=9, label=1,
+ has_default_value=False, default_value=b"",
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='crc', full_name='minitaur_fluxworks.control.LaikagoState.crc', index=7,
+ number=8, type=13, cpp_type=3, label=1,
+ has_default_value=False, default_value=0,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ ],
+ extensions=[
+ ],
+ nested_types=[],
+ enum_types=[
+ ],
+ serialized_options=None,
+ is_extendable=False,
+ syntax='proto3',
+ extension_ranges=[],
+ oneofs=[
+ ],
+ serialized_start=1151,
+ serialized_end=1482,
+)
+
+
+_LAIKAGOCOMMANDSTATE = _descriptor.Descriptor(
+ name='LaikagoCommandState',
+ full_name='minitaur_fluxworks.control.LaikagoCommandState',
+ filename=None,
+ file=DESCRIPTOR,
+ containing_type=None,
+ create_key=_descriptor._internal_create_key,
+ fields=[
+ _descriptor.FieldDescriptor(
+ name='command', full_name='minitaur_fluxworks.control.LaikagoCommandState.command', index=0,
+ number=1, type=11, cpp_type=10, label=1,
+ has_default_value=False, default_value=None,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='state', full_name='minitaur_fluxworks.control.LaikagoCommandState.state', index=1,
+ number=2, type=11, cpp_type=10, label=1,
+ has_default_value=False, default_value=None,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ ],
+ extensions=[
+ ],
+ nested_types=[],
+ enum_types=[
+ ],
+ serialized_options=None,
+ is_extendable=False,
+ syntax='proto3',
+ extension_ranges=[],
+ oneofs=[
+ ],
+ serialized_start=1485,
+ serialized_end=1624,
+)
+
+
+_LAIKAGOHIGHLEVELCOMMAND = _descriptor.Descriptor(
+ name='LaikagoHighLevelCommand',
+ full_name='minitaur_fluxworks.control.LaikagoHighLevelCommand',
+ filename=None,
+ file=DESCRIPTOR,
+ containing_type=None,
+ create_key=_descriptor._internal_create_key,
+ fields=[
+ _descriptor.FieldDescriptor(
+ name='timestamp', full_name='minitaur_fluxworks.control.LaikagoHighLevelCommand.timestamp', index=0,
+ number=1, type=11, cpp_type=10, label=1,
+ has_default_value=False, default_value=None,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='control_level', full_name='minitaur_fluxworks.control.LaikagoHighLevelCommand.control_level', index=1,
+ number=2, type=13, cpp_type=3, label=1,
+ has_default_value=False, default_value=0,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='control_mode', full_name='minitaur_fluxworks.control.LaikagoHighLevelCommand.control_mode', index=2,
+ number=3, type=13, cpp_type=3, label=1,
+ has_default_value=False, default_value=0,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='walk_speed', full_name='minitaur_fluxworks.control.LaikagoHighLevelCommand.walk_speed', index=3,
+ number=4, type=11, cpp_type=10, label=1,
+ has_default_value=False, default_value=None,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='body_height', full_name='minitaur_fluxworks.control.LaikagoHighLevelCommand.body_height', index=4,
+ number=5, type=2, cpp_type=6, label=1,
+ has_default_value=False, default_value=float(0),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='foot_clearance_height', full_name='minitaur_fluxworks.control.LaikagoHighLevelCommand.foot_clearance_height', index=5,
+ number=6, type=2, cpp_type=6, label=1,
+ has_default_value=False, default_value=float(0),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='rpy', full_name='minitaur_fluxworks.control.LaikagoHighLevelCommand.rpy', index=6,
+ number=7, type=11, cpp_type=10, label=1,
+ has_default_value=False, default_value=None,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ ],
+ extensions=[
+ ],
+ nested_types=[],
+ enum_types=[
+ ],
+ serialized_options=None,
+ is_extendable=False,
+ syntax='proto3',
+ extension_ranges=[],
+ oneofs=[
+ ],
+ serialized_start=1627,
+ serialized_end=1887,
+)
+
+
+_LAIKAGOHIGHLEVELSTATE = _descriptor.Descriptor(
+ name='LaikagoHighLevelState',
+ full_name='minitaur_fluxworks.control.LaikagoHighLevelState',
+ filename=None,
+ file=DESCRIPTOR,
+ containing_type=None,
+ create_key=_descriptor._internal_create_key,
+ fields=[
+ _descriptor.FieldDescriptor(
+ name='timestamp', full_name='minitaur_fluxworks.control.LaikagoHighLevelState.timestamp', index=0,
+ number=1, type=11, cpp_type=10, label=1,
+ has_default_value=False, default_value=None,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='control_level', full_name='minitaur_fluxworks.control.LaikagoHighLevelState.control_level', index=1,
+ number=2, type=13, cpp_type=3, label=1,
+ has_default_value=False, default_value=0,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='control_mode', full_name='minitaur_fluxworks.control.LaikagoHighLevelState.control_mode', index=2,
+ number=3, type=13, cpp_type=3, label=1,
+ has_default_value=False, default_value=0,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='imu', full_name='minitaur_fluxworks.control.LaikagoHighLevelState.imu', index=3,
+ number=4, type=11, cpp_type=10, label=1,
+ has_default_value=False, default_value=None,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='walk_speed', full_name='minitaur_fluxworks.control.LaikagoHighLevelState.walk_speed', index=4,
+ number=5, type=11, cpp_type=10, label=1,
+ has_default_value=False, default_value=None,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='body_height', full_name='minitaur_fluxworks.control.LaikagoHighLevelState.body_height', index=5,
+ number=8, type=2, cpp_type=6, label=1,
+ has_default_value=False, default_value=float(0),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='up_down_speed', full_name='minitaur_fluxworks.control.LaikagoHighLevelState.up_down_speed', index=6,
+ number=9, type=2, cpp_type=6, label=1,
+ has_default_value=False, default_value=float(0),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='com_position', full_name='minitaur_fluxworks.control.LaikagoHighLevelState.com_position', index=7,
+ number=10, type=11, cpp_type=10, label=1,
+ has_default_value=False, default_value=None,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='foot_position_to_com', full_name='minitaur_fluxworks.control.LaikagoHighLevelState.foot_position_to_com', index=8,
+ number=11, type=11, cpp_type=10, label=3,
+ has_default_value=False, default_value=[],
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='foot_velocity_to_com', full_name='minitaur_fluxworks.control.LaikagoHighLevelState.foot_velocity_to_com', index=9,
+ number=12, type=11, cpp_type=10, label=3,
+ has_default_value=False, default_value=[],
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='contact_state', full_name='minitaur_fluxworks.control.LaikagoHighLevelState.contact_state', index=10,
+ number=13, type=11, cpp_type=10, label=3,
+ has_default_value=False, default_value=[],
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='microcontroller_time_millis', full_name='minitaur_fluxworks.control.LaikagoHighLevelState.microcontroller_time_millis', index=11,
+ number=14, type=13, cpp_type=3, label=1,
+ has_default_value=False, default_value=0,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='wireless_remote', full_name='minitaur_fluxworks.control.LaikagoHighLevelState.wireless_remote', index=12,
+ number=15, type=12, cpp_type=9, label=1,
+ has_default_value=False, default_value=b"",
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='crc', full_name='minitaur_fluxworks.control.LaikagoHighLevelState.crc', index=13,
+ number=16, type=13, cpp_type=3, label=1,
+ has_default_value=False, default_value=0,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ ],
+ extensions=[
+ ],
+ nested_types=[],
+ enum_types=[
+ ],
+ serialized_options=None,
+ is_extendable=False,
+ syntax='proto3',
+ extension_ranges=[],
+ oneofs=[
+ ],
+ serialized_start=1890,
+ serialized_end=2453,
+)
+
+
+_LAIKAGOHIGHLEVELSTATEREQUEST = _descriptor.Descriptor(
+ name='LaikagoHighLevelStateRequest',
+ full_name='minitaur_fluxworks.control.LaikagoHighLevelStateRequest',
+ filename=None,
+ file=DESCRIPTOR,
+ containing_type=None,
+ create_key=_descriptor._internal_create_key,
+ fields=[
+ ],
+ extensions=[
+ ],
+ nested_types=[],
+ enum_types=[
+ ],
+ serialized_options=None,
+ is_extendable=False,
+ syntax='proto3',
+ extension_ranges=[],
+ oneofs=[
+ ],
+ serialized_start=2455,
+ serialized_end=2485,
+)
+
+
+_LAIKAGOHIGHLEVELCOMMANDSTATE = _descriptor.Descriptor(
+ name='LaikagoHighLevelCommandState',
+ full_name='minitaur_fluxworks.control.LaikagoHighLevelCommandState',
+ filename=None,
+ file=DESCRIPTOR,
+ containing_type=None,
+ create_key=_descriptor._internal_create_key,
+ fields=[
+ _descriptor.FieldDescriptor(
+ name='command', full_name='minitaur_fluxworks.control.LaikagoHighLevelCommandState.command', index=0,
+ number=1, type=11, cpp_type=10, label=1,
+ has_default_value=False, default_value=None,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='state', full_name='minitaur_fluxworks.control.LaikagoHighLevelCommandState.state', index=1,
+ number=2, type=11, cpp_type=10, label=1,
+ has_default_value=False, default_value=None,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ ],
+ extensions=[
+ ],
+ nested_types=[],
+ enum_types=[
+ ],
+ serialized_options=None,
+ is_extendable=False,
+ syntax='proto3',
+ extension_ranges=[],
+ oneofs=[
+ ],
+ serialized_start=2488,
+ serialized_end=2654,
+)
+
+_LAIKAGOCOMMAND.fields_by_name['timestamp'].message_type = timestamp__pb2._TIMESTAMP
+_LAIKAGOCOMMAND.fields_by_name['control_mode'].enum_type = _LAIKAGOCOMMAND_CONTROLMODE
+_LAIKAGOCOMMAND.fields_by_name['motor_command'].message_type = _MOTORCOMMAND
+_LAIKAGOCOMMAND.fields_by_name['led'].message_type = _LED
+_LAIKAGOCOMMAND_CONTROLMODE.containing_type = _LAIKAGOCOMMAND
+_IMU.fields_by_name['quaternion'].message_type = vector__pb2._VECTOR4F
+_IMU.fields_by_name['gyroscope'].message_type = vector__pb2._VECTOR3F
+_IMU.fields_by_name['acceleration'].message_type = vector__pb2._VECTOR3F
+_IMU.fields_by_name['rpy'].message_type = vector__pb2._VECTOR3F
+_CONTACTSTATE.fields_by_name['axis'].message_type = vector__pb2._VECTOR3F
+_LAIKAGOSTATE.fields_by_name['timestamp'].message_type = timestamp__pb2._TIMESTAMP
+_LAIKAGOSTATE.fields_by_name['imu'].message_type = _IMU
+_LAIKAGOSTATE.fields_by_name['motor_state'].message_type = _MOTORSTATE
+_LAIKAGOSTATE.fields_by_name['contact_state'].message_type = _CONTACTSTATE
+_LAIKAGOCOMMANDSTATE.fields_by_name['command'].message_type = _LAIKAGOCOMMAND
+_LAIKAGOCOMMANDSTATE.fields_by_name['state'].message_type = _LAIKAGOSTATE
+_LAIKAGOHIGHLEVELCOMMAND.fields_by_name['timestamp'].message_type = timestamp__pb2._TIMESTAMP
+_LAIKAGOHIGHLEVELCOMMAND.fields_by_name['walk_speed'].message_type = vector__pb2._VECTOR3F
+_LAIKAGOHIGHLEVELCOMMAND.fields_by_name['rpy'].message_type = vector__pb2._VECTOR3F
+_LAIKAGOHIGHLEVELSTATE.fields_by_name['timestamp'].message_type = timestamp__pb2._TIMESTAMP
+_LAIKAGOHIGHLEVELSTATE.fields_by_name['imu'].message_type = _IMU
+_LAIKAGOHIGHLEVELSTATE.fields_by_name['walk_speed'].message_type = vector__pb2._VECTOR3F
+_LAIKAGOHIGHLEVELSTATE.fields_by_name['com_position'].message_type = vector__pb2._VECTOR3F
+_LAIKAGOHIGHLEVELSTATE.fields_by_name['foot_position_to_com'].message_type = vector__pb2._VECTOR3F
+_LAIKAGOHIGHLEVELSTATE.fields_by_name['foot_velocity_to_com'].message_type = vector__pb2._VECTOR3F
+_LAIKAGOHIGHLEVELSTATE.fields_by_name['contact_state'].message_type = _CONTACTSTATE
+_LAIKAGOHIGHLEVELCOMMANDSTATE.fields_by_name['command'].message_type = _LAIKAGOHIGHLEVELCOMMAND
+_LAIKAGOHIGHLEVELCOMMANDSTATE.fields_by_name['state'].message_type = _LAIKAGOHIGHLEVELSTATE
+DESCRIPTOR.message_types_by_name['MotorCommand'] = _MOTORCOMMAND
+DESCRIPTOR.message_types_by_name['Led'] = _LED
+DESCRIPTOR.message_types_by_name['LaikagoCommand'] = _LAIKAGOCOMMAND
+DESCRIPTOR.message_types_by_name['LaikagoStateRequest'] = _LAIKAGOSTATEREQUEST
+DESCRIPTOR.message_types_by_name['Imu'] = _IMU
+DESCRIPTOR.message_types_by_name['MotorState'] = _MOTORSTATE
+DESCRIPTOR.message_types_by_name['ContactState'] = _CONTACTSTATE
+DESCRIPTOR.message_types_by_name['LaikagoState'] = _LAIKAGOSTATE
+DESCRIPTOR.message_types_by_name['LaikagoCommandState'] = _LAIKAGOCOMMANDSTATE
+DESCRIPTOR.message_types_by_name['LaikagoHighLevelCommand'] = _LAIKAGOHIGHLEVELCOMMAND
+DESCRIPTOR.message_types_by_name['LaikagoHighLevelState'] = _LAIKAGOHIGHLEVELSTATE
+DESCRIPTOR.message_types_by_name['LaikagoHighLevelStateRequest'] = _LAIKAGOHIGHLEVELSTATEREQUEST
+DESCRIPTOR.message_types_by_name['LaikagoHighLevelCommandState'] = _LAIKAGOHIGHLEVELCOMMANDSTATE
+_sym_db.RegisterFileDescriptor(DESCRIPTOR)
+
+MotorCommand = _reflection.GeneratedProtocolMessageType('MotorCommand', (_message.Message,), {
+ 'DESCRIPTOR' : _MOTORCOMMAND,
+ '__module__' : 'laikago_interface_pb2'
+ # @@protoc_insertion_point(class_scope:minitaur_fluxworks.control.MotorCommand)
+ })
+_sym_db.RegisterMessage(MotorCommand)
+
+Led = _reflection.GeneratedProtocolMessageType('Led', (_message.Message,), {
+ 'DESCRIPTOR' : _LED,
+ '__module__' : 'laikago_interface_pb2'
+ # @@protoc_insertion_point(class_scope:minitaur_fluxworks.control.Led)
+ })
+_sym_db.RegisterMessage(Led)
+
+LaikagoCommand = _reflection.GeneratedProtocolMessageType('LaikagoCommand', (_message.Message,), {
+ 'DESCRIPTOR' : _LAIKAGOCOMMAND,
+ '__module__' : 'laikago_interface_pb2'
+ # @@protoc_insertion_point(class_scope:minitaur_fluxworks.control.LaikagoCommand)
+ })
+_sym_db.RegisterMessage(LaikagoCommand)
+
+LaikagoStateRequest = _reflection.GeneratedProtocolMessageType('LaikagoStateRequest', (_message.Message,), {
+ 'DESCRIPTOR' : _LAIKAGOSTATEREQUEST,
+ '__module__' : 'laikago_interface_pb2'
+ # @@protoc_insertion_point(class_scope:minitaur_fluxworks.control.LaikagoStateRequest)
+ })
+_sym_db.RegisterMessage(LaikagoStateRequest)
+
+Imu = _reflection.GeneratedProtocolMessageType('Imu', (_message.Message,), {
+ 'DESCRIPTOR' : _IMU,
+ '__module__' : 'laikago_interface_pb2'
+ # @@protoc_insertion_point(class_scope:minitaur_fluxworks.control.Imu)
+ })
+_sym_db.RegisterMessage(Imu)
+
+MotorState = _reflection.GeneratedProtocolMessageType('MotorState', (_message.Message,), {
+ 'DESCRIPTOR' : _MOTORSTATE,
+ '__module__' : 'laikago_interface_pb2'
+ # @@protoc_insertion_point(class_scope:minitaur_fluxworks.control.MotorState)
+ })
+_sym_db.RegisterMessage(MotorState)
+
+ContactState = _reflection.GeneratedProtocolMessageType('ContactState', (_message.Message,), {
+ 'DESCRIPTOR' : _CONTACTSTATE,
+ '__module__' : 'laikago_interface_pb2'
+ # @@protoc_insertion_point(class_scope:minitaur_fluxworks.control.ContactState)
+ })
+_sym_db.RegisterMessage(ContactState)
+
+LaikagoState = _reflection.GeneratedProtocolMessageType('LaikagoState', (_message.Message,), {
+ 'DESCRIPTOR' : _LAIKAGOSTATE,
+ '__module__' : 'laikago_interface_pb2'
+ # @@protoc_insertion_point(class_scope:minitaur_fluxworks.control.LaikagoState)
+ })
+_sym_db.RegisterMessage(LaikagoState)
+
+LaikagoCommandState = _reflection.GeneratedProtocolMessageType('LaikagoCommandState', (_message.Message,), {
+ 'DESCRIPTOR' : _LAIKAGOCOMMANDSTATE,
+ '__module__' : 'laikago_interface_pb2'
+ # @@protoc_insertion_point(class_scope:minitaur_fluxworks.control.LaikagoCommandState)
+ })
+_sym_db.RegisterMessage(LaikagoCommandState)
+
+LaikagoHighLevelCommand = _reflection.GeneratedProtocolMessageType('LaikagoHighLevelCommand', (_message.Message,), {
+ 'DESCRIPTOR' : _LAIKAGOHIGHLEVELCOMMAND,
+ '__module__' : 'laikago_interface_pb2'
+ # @@protoc_insertion_point(class_scope:minitaur_fluxworks.control.LaikagoHighLevelCommand)
+ })
+_sym_db.RegisterMessage(LaikagoHighLevelCommand)
+
+LaikagoHighLevelState = _reflection.GeneratedProtocolMessageType('LaikagoHighLevelState', (_message.Message,), {
+ 'DESCRIPTOR' : _LAIKAGOHIGHLEVELSTATE,
+ '__module__' : 'laikago_interface_pb2'
+ # @@protoc_insertion_point(class_scope:minitaur_fluxworks.control.LaikagoHighLevelState)
+ })
+_sym_db.RegisterMessage(LaikagoHighLevelState)
+
+LaikagoHighLevelStateRequest = _reflection.GeneratedProtocolMessageType('LaikagoHighLevelStateRequest', (_message.Message,), {
+ 'DESCRIPTOR' : _LAIKAGOHIGHLEVELSTATEREQUEST,
+ '__module__' : 'laikago_interface_pb2'
+ # @@protoc_insertion_point(class_scope:minitaur_fluxworks.control.LaikagoHighLevelStateRequest)
+ })
+_sym_db.RegisterMessage(LaikagoHighLevelStateRequest)
+
+LaikagoHighLevelCommandState = _reflection.GeneratedProtocolMessageType('LaikagoHighLevelCommandState', (_message.Message,), {
+ 'DESCRIPTOR' : _LAIKAGOHIGHLEVELCOMMANDSTATE,
+ '__module__' : 'laikago_interface_pb2'
+ # @@protoc_insertion_point(class_scope:minitaur_fluxworks.control.LaikagoHighLevelCommandState)
+ })
+_sym_db.RegisterMessage(LaikagoHighLevelCommandState)
+
+
+
+_LAIKAGOCONTROLGRPCINTERFACE = _descriptor.ServiceDescriptor(
+ name='LaikagoControlGrpcInterface',
+ full_name='minitaur_fluxworks.control.LaikagoControlGrpcInterface',
+ file=DESCRIPTOR,
+ index=0,
+ serialized_options=None,
+ create_key=_descriptor._internal_create_key,
+ serialized_start=2657,
+ serialized_end=2894,
+ methods=[
+ _descriptor.MethodDescriptor(
+ name='SendCommand',
+ full_name='minitaur_fluxworks.control.LaikagoControlGrpcInterface.SendCommand',
+ index=0,
+ containing_service=None,
+ input_type=_LAIKAGOCOMMAND,
+ output_type=_LAIKAGOSTATE,
+ serialized_options=None,
+ create_key=_descriptor._internal_create_key,
+ ),
+ _descriptor.MethodDescriptor(
+ name='GetState',
+ full_name='minitaur_fluxworks.control.LaikagoControlGrpcInterface.GetState',
+ index=1,
+ containing_service=None,
+ input_type=_LAIKAGOSTATEREQUEST,
+ output_type=_LAIKAGOSTATE,
+ serialized_options=None,
+ create_key=_descriptor._internal_create_key,
+ ),
+])
+_sym_db.RegisterServiceDescriptor(_LAIKAGOCONTROLGRPCINTERFACE)
+
+DESCRIPTOR.services_by_name['LaikagoControlGrpcInterface'] = _LAIKAGOCONTROLGRPCINTERFACE
+
+
+_LAIKAGOHIGHLEVELCONTROLGRPCINTERFACE = _descriptor.ServiceDescriptor(
+ name='LaikagoHighLevelControlGrpcInterface',
+ full_name='minitaur_fluxworks.control.LaikagoHighLevelControlGrpcInterface',
+ file=DESCRIPTOR,
+ index=1,
+ serialized_options=None,
+ create_key=_descriptor._internal_create_key,
+ serialized_start=2897,
+ serialized_end=3179,
+ methods=[
+ _descriptor.MethodDescriptor(
+ name='SendCommand',
+ full_name='minitaur_fluxworks.control.LaikagoHighLevelControlGrpcInterface.SendCommand',
+ index=0,
+ containing_service=None,
+ input_type=_LAIKAGOHIGHLEVELCOMMAND,
+ output_type=_LAIKAGOHIGHLEVELSTATE,
+ serialized_options=None,
+ create_key=_descriptor._internal_create_key,
+ ),
+ _descriptor.MethodDescriptor(
+ name='GetState',
+ full_name='minitaur_fluxworks.control.LaikagoHighLevelControlGrpcInterface.GetState',
+ index=1,
+ containing_service=None,
+ input_type=_LAIKAGOHIGHLEVELSTATEREQUEST,
+ output_type=_LAIKAGOHIGHLEVELSTATE,
+ serialized_options=None,
+ create_key=_descriptor._internal_create_key,
+ ),
+])
+_sym_db.RegisterServiceDescriptor(_LAIKAGOHIGHLEVELCONTROLGRPCINTERFACE)
+
+DESCRIPTOR.services_by_name['LaikagoHighLevelControlGrpcInterface'] = _LAIKAGOHIGHLEVELCONTROLGRPCINTERFACE
+
+# @@protoc_insertion_point(module_scope)
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/robots/laikago_kinematic_constants.py b/examples/pybullet/gym/pybullet_envs/minitaur/robots/laikago_kinematic_constants.py
new file mode 100644
index 000000000..eae1662da
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/robots/laikago_kinematic_constants.py
@@ -0,0 +1,106 @@
+# Lint as: python3
+"""Defines the LaikagoKinematic robot related constants and URDF specs."""
+
+import collections
+import gin
+
+LAIKAGO_KINEMATIC_URDF_PATH = "robotics/reinforcement_learning/minitaur/robots/data/urdf/laikago/laikago_toes_zup_kinematic.urdf"
+INIT_POSITION = (0, 0, 0.5)
+INIT_ORIENTATION_QUAT = (0, 0, 0, 1)
+INIT_ORIENTATION_RPY = (0, 0, 0)
+
+NUM_LEGS = 4
+# TODO(b/153405332): Use link name instead of joint name to identify the
+# base link.
+BASE_NAMES = ("kinematic_drive_joint_th",)
+
+JOINT_NAMES = (
+ "kinematic_drive_joint_x",
+ "kinematic_drive_joint_y",
+ "kinematic_drive_joint_th",
+ # front right leg
+ "FR_hip_motor_2_chassis_joint",
+ "FR_upper_leg_2_hip_motor_joint",
+ "FR_lower_leg_2_upper_leg_joint",
+ # front left leg
+ "FL_hip_motor_2_chassis_joint",
+ "FL_upper_leg_2_hip_motor_joint",
+ "FL_lower_leg_2_upper_leg_joint",
+ # rear right leg
+ "RR_hip_motor_2_chassis_joint",
+ "RR_upper_leg_2_hip_motor_joint",
+ "RR_lower_leg_2_upper_leg_joint",
+ # rear left leg
+ "RL_hip_motor_2_chassis_joint",
+ "RL_upper_leg_2_hip_motor_joint",
+ "RL_lower_leg_2_upper_leg_joint",
+)
+
+# A default joint pose where the arm is tucked near the base, and head looking
+# forward.
+INIT_ABDUCTION_ANGLE = 0
+INIT_HIP_ANGLE = 0.67
+INIT_KNEE_ANGLE = -1.25
+
+# Note this matches the Laikago SDK/control convention, but is different from
+# URDF's internal joint angles which needs to be computed using the joint
+# offsets and directions. The conversion formula is (sdk_joint_angle + offset) *
+# joint direction.
+INIT_JOINT_ANGLES = collections.OrderedDict(
+ zip(JOINT_NAMES, (0, 0, 0) +
+ (INIT_ABDUCTION_ANGLE, INIT_HIP_ANGLE, INIT_KNEE_ANGLE) * NUM_LEGS))
+
+# Used to convert the robot SDK joint angles to URDF joint angles.
+JOINT_DIRECTIONS = collections.OrderedDict(
+ zip(JOINT_NAMES, (1, 1, 1, -1, 1, 1, 1, 1, 1, -1, 1, 1, 1, 1, 1)))
+
+HIP_JOINT_OFFSET = 0.0
+UPPER_LEG_JOINT_OFFSET = -0.6
+KNEE_JOINT_OFFSET = 0.66
+
+# Used to convert the robot SDK joint angles to URDF joint angles.
+JOINT_OFFSETS = collections.OrderedDict(
+ zip(JOINT_NAMES, [0, 0, 0] +
+ [HIP_JOINT_OFFSET, UPPER_LEG_JOINT_OFFSET, KNEE_JOINT_OFFSET] *
+ NUM_LEGS))
+
+LEG_NAMES = (
+ "front_right",
+ "front_left",
+ "rear_right",
+ "rear_left",
+)
+
+LEG_ORDER = (
+ "front_right",
+ "front_left",
+ "back_right",
+ "back_left",
+)
+
+END_EFFECTOR_NAMES = (
+ "jtoeFR",
+ "jtoeFL",
+ "jtoeRR",
+ "jtoeRL",
+)
+
+MOTOR_NAMES = JOINT_NAMES
+MOTOR_GROUP = collections.OrderedDict((("body_motors", JOINT_NAMES[3:]),))
+
+# Add the gin constants to be used for gin binding in config.
+gin.constant("laikago_kinematic_constants.LAIKAGO_KINEMATIC_URDF_PATH",
+ LAIKAGO_KINEMATIC_URDF_PATH)
+gin.constant("laikago_kinematic_constants.INIT_POSITION", INIT_POSITION)
+gin.constant("laikago_kinematic_constants.INIT_ORIENTATION_QUAT",
+ INIT_ORIENTATION_QUAT)
+gin.constant("laikago_kinematic_constants.INIT_ORIENTATION_RPY",
+ INIT_ORIENTATION_RPY)
+gin.constant("laikago_kinematic_constants.BASE_NAMES", BASE_NAMES)
+gin.constant("laikago_kinematic_constants.INIT_JOINT_ANGLES", INIT_JOINT_ANGLES)
+gin.constant("laikago_kinematic_constants.JOINT_DIRECTIONS", JOINT_DIRECTIONS)
+gin.constant("laikago_kinematic_constants.JOINT_OFFSETS", JOINT_OFFSETS)
+gin.constant("laikago_kinematic_constants.MOTOR_NAMES", MOTOR_NAMES)
+gin.constant("laikago_kinematic_constants.END_EFFECTOR_NAMES",
+ END_EFFECTOR_NAMES)
+gin.constant("laikago_kinematic_constants.MOTOR_GROUP", MOTOR_GROUP)
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/robots/laikago_motor.py b/examples/pybullet/gym/pybullet_envs/minitaur/robots/laikago_motor.py
new file mode 100644
index 000000000..a57c0a964
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/robots/laikago_motor.py
@@ -0,0 +1,149 @@
+"""Motor model for laikago."""
+
+import collections
+import numpy as np
+
+from pybullet_envs.minitaur.robots import robot_config
+
+NUM_MOTORS = 12
+
+MOTOR_COMMAND_DIMENSION = 5
+
+# These values represent the indices of each field in the motor command tuple
+POSITION_INDEX = 0
+POSITION_GAIN_INDEX = 1
+VELOCITY_INDEX = 2
+VELOCITY_GAIN_INDEX = 3
+TORQUE_INDEX = 4
+
+
+class LaikagoMotorModel(object):
+ """A simple motor model for Laikago.
+
+ When in POSITION mode, the torque is calculated according to the difference
+ between current and desired joint angle, as well as the joint velocity.
+ For more information about PD control, please refer to:
+ https://en.wikipedia.org/wiki/PID_controller.
+
+ The model supports a HYBRID mode in which each motor command can be a tuple
+ (desired_motor_angle, position_gain, desired_motor_velocity, velocity_gain,
+ torque).
+
+ """
+
+ def __init__(self,
+ kp=60,
+ kd=1,
+ torque_limits=None,
+ motor_control_mode=robot_config.MotorControlMode.POSITION):
+ self._kp = kp
+ self._kd = kd
+ self._torque_limits = torque_limits
+ if torque_limits is not None:
+ if isinstance(torque_limits, (collections.Sequence, np.ndarray)):
+ self._torque_limits = np.asarray(torque_limits)
+ else:
+ self._torque_limits = np.full(NUM_MOTORS, torque_limits)
+ self._motor_control_mode = motor_control_mode
+ self._strength_ratios = np.full(NUM_MOTORS, 1)
+
+ def set_strength_ratios(self, ratios):
+ """Set the strength of each motors relative to the default value.
+
+ Args:
+ ratios: The relative strength of motor output. A numpy array ranging from
+ 0.0 to 1.0.
+ """
+ self._strength_ratios = ratios
+
+ def set_motor_gains(self, kp, kd):
+ """Set the gains of all motors.
+
+ These gains are PD gains for motor positional control. kp is the
+ proportional gain and kd is the derivative gain.
+
+ Args:
+ kp: proportional gain of the motors.
+ kd: derivative gain of the motors.
+ """
+ self._kp = kp
+ self._kd = kd
+
+ def set_voltage(self, voltage):
+ pass
+
+ def get_voltage(self):
+ return 0.0
+
+ def set_viscous_damping(self, viscous_damping):
+ pass
+
+ def get_viscous_dampling(self):
+ return 0.0
+
+ def convert_to_torque(self,
+ motor_commands,
+ motor_angle,
+ motor_velocity,
+ true_motor_velocity,
+ motor_control_mode=None):
+ """Convert the commands (position control or torque control) to torque.
+
+ Args:
+ motor_commands: The desired motor angle if the motor is in position
+ control mode. The pwm signal if the motor is in torque control mode.
+ motor_angle: The motor angle observed at the current time step. It is
+ actually the true motor angle observed a few milliseconds ago (pd
+ latency).
+ motor_velocity: The motor velocity observed at the current time step, it
+ is actually the true motor velocity a few milliseconds ago (pd latency).
+ true_motor_velocity: The true motor velocity. The true velocity is used to
+ compute back EMF voltage and viscous damping.
+ motor_control_mode: A MotorControlMode enum.
+
+ Returns:
+ actual_torque: The torque that needs to be applied to the motor.
+ observed_torque: The torque observed by the sensor.
+ """
+ del true_motor_velocity
+ if not motor_control_mode:
+ motor_control_mode = self._motor_control_mode
+
+ # No processing for motor torques
+ if motor_control_mode is robot_config.MotorControlMode.TORQUE:
+ assert len(motor_commands) == NUM_MOTORS
+ motor_torques = self._strength_ratios * motor_commands
+ return motor_torques, motor_torques
+
+ desired_motor_angles = None
+ desired_motor_velocities = None
+ kp = None
+ kd = None
+ additional_torques = np.full(NUM_MOTORS, 0)
+ if motor_control_mode is robot_config.MotorControlMode.POSITION:
+ assert len(motor_commands) == NUM_MOTORS
+ kp = self._kp
+ kd = self._kd
+ desired_motor_angles = motor_commands
+ desired_motor_velocities = np.full(NUM_MOTORS, 0)
+ elif motor_control_mode is robot_config.MotorControlMode.HYBRID:
+ # The input should be a 60 dimension vector
+ assert len(motor_commands) == MOTOR_COMMAND_DIMENSION * NUM_MOTORS
+ kp = motor_commands[POSITION_GAIN_INDEX::MOTOR_COMMAND_DIMENSION]
+ kd = motor_commands[VELOCITY_GAIN_INDEX::MOTOR_COMMAND_DIMENSION]
+ desired_motor_angles = motor_commands[
+ POSITION_INDEX::MOTOR_COMMAND_DIMENSION]
+ desired_motor_velocities = motor_commands[
+ VELOCITY_INDEX::MOTOR_COMMAND_DIMENSION]
+ additional_torques = motor_commands[TORQUE_INDEX::MOTOR_COMMAND_DIMENSION]
+ motor_torques = -1 * (kp * (motor_angle - desired_motor_angles)) - kd * (
+ motor_velocity - desired_motor_velocities) + additional_torques
+ motor_torques = self._strength_ratios * motor_torques
+ if self._torque_limits is not None:
+ if len(self._torque_limits) != len(motor_torques):
+ raise ValueError(
+ "Torque limits dimension does not match the number of motors.")
+ motor_torques = np.clip(motor_torques, -1.0 * self._torque_limits,
+ self._torque_limits)
+
+ return motor_torques, motor_torques
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/robots/laikago_v2.py b/examples/pybullet/gym/pybullet_envs/minitaur/robots/laikago_v2.py
new file mode 100644
index 000000000..dcabd05df
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/robots/laikago_v2.py
@@ -0,0 +1,34 @@
+# Lint as: python3
+"""Add the new laikago robot."""
+import gin
+
+from pybullet_envs.minitaur.robots import laikago_constants
+from pybullet_envs.minitaur.robots import quadruped_base
+from pybullet_envs.minitaur.robots import robot_urdf_loader
+
+
+@gin.configurable
+class Laikago(quadruped_base.QuadrupedBase):
+ """The Laikago class that simulates the quadruped from Unitree."""
+
+ def _pre_load(self):
+ """Import the Laikago specific constants.
+ """
+ self._urdf_loader = robot_urdf_loader.RobotUrdfLoader(
+ pybullet_client=self._pybullet_client,
+ urdf_path=laikago_constants.URDF_PATH,
+ enable_self_collision=True,
+ init_base_position=laikago_constants.INIT_POSITION,
+ init_base_orientation_quaternion=laikago_constants.INIT_ORIENTATION,
+ init_joint_angles=laikago_constants.INIT_JOINT_ANGLES,
+ joint_offsets=laikago_constants.JOINT_OFFSETS,
+ joint_directions=laikago_constants.JOINT_DIRECTIONS,
+ motor_names=laikago_constants.MOTOR_NAMES,
+ end_effector_names=laikago_constants.END_EFFECTOR_NAMES,
+ user_group=laikago_constants.MOTOR_GROUP,
+ )
+
+ @classmethod
+ def get_constants(cls):
+ del cls
+ return laikago_constants
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/robots/mini_cheetah.py b/examples/pybullet/gym/pybullet_envs/minitaur/robots/mini_cheetah.py
new file mode 100644
index 000000000..bc8fb1089
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/robots/mini_cheetah.py
@@ -0,0 +1,133 @@
+"""Pybullet simulation of a vision60 robot."""
+import math
+import os
+
+import gin
+import numpy as np
+
+from pybullet_envs.minitaur.robots import laikago_motor
+from pybullet_envs.minitaur.robots import minitaur
+from pybullet_envs.minitaur.robots import robot_config
+
+NUM_MOTORS = 12
+NUM_LEGS = 4
+MOTOR_NAMES = [
+ "torso_to_abduct_fl_j", # Left front abduction (hip0).
+ "abduct_fl_to_thigh_fl_j", # Left front hip (upper0).
+ "thigh_fl_to_knee_fl_j", # Left front knee (lower0).
+ "torso_to_abduct_hl_j", # Left rear abduction (hip1).
+ "abduct_hl_to_thigh_hl_j", # Left rear hip (upper1).
+ "thigh_hl_to_knee_hl_j", # Left rear knee (lower1).
+ "torso_to_abduct_fr_j", # Right front abduction (hip2).
+ "abduct_fr_to_thigh_fr_j", # Right front hip (upper2).
+ "thigh_fr_to_knee_fr_j", # Right front knee (lower2).
+ "torso_to_abduct_hr_j", # Right rear abduction (hip3).
+ "abduct_hr_to_thigh_hr_j", # Right rear hip (upper3).
+ "thigh_hr_to_knee_hr_j", # Right rear knee (lower3).
+]
+_DEFAULT_TORQUE_LIMITS = [12, 18, 12] * 4
+INIT_RACK_POSITION = [0, 0, 1.4]
+INIT_POSITION = [0, 0, 0.4]
+JOINT_DIRECTIONS = np.array([1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1])
+HIP_JOINT_OFFSET = 0.0
+UPPER_LEG_JOINT_OFFSET = 0.0
+KNEE_JOINT_OFFSET = 0.0
+DOFS_PER_LEG = 3
+JOINT_OFFSETS = np.array(
+ [HIP_JOINT_OFFSET, UPPER_LEG_JOINT_OFFSET, KNEE_JOINT_OFFSET] * 4)
+PI = math.pi
+DEFAULT_ABDUCTION_ANGLE = 0.0
+DEFAULT_HIP_ANGLE = -1.1
+DEFAULT_KNEE_ANGLE = 2.3
+# Bases on the readings from 's default pose.
+INIT_MOTOR_ANGLES = [
+ DEFAULT_ABDUCTION_ANGLE, DEFAULT_HIP_ANGLE, DEFAULT_KNEE_ANGLE
+] * NUM_LEGS
+DEFAULT_LOCAL_TOE_POSITIONS = [[0.17, -0.11, -0.16], [0.17, 0.11, -0.16],
+ [-0.20, -0.11, -0.16], [-0.20, 0.11, -0.16]]
+
+
+@gin.configurable
+class MiniCheetah(minitaur.Minitaur):
+ """A simulation for the mini cheetah robot."""
+
+ def __init__(self, **kwargs):
+ if "motor_kp" not in kwargs:
+ kwargs["motor_kp"] = 100.0
+ if "motor_kd" not in kwargs:
+ kwargs["motor_kd"] = 2.0
+ if "motor_torque_limits" not in kwargs:
+ kwargs["motor_torque_limits"] = _DEFAULT_TORQUE_LIMITS
+
+ # The follwing parameters are fixed for the vision60 robot.
+ kwargs["num_motors"] = NUM_MOTORS
+ kwargs["dofs_per_leg"] = DOFS_PER_LEG
+ kwargs["motor_direction"] = JOINT_DIRECTIONS
+ kwargs["motor_offset"] = JOINT_OFFSETS
+ kwargs["motor_overheat_protection"] = False
+ kwargs["motor_model_class"] = laikago_motor.LaikagoMotorModel
+ super(MiniCheetah, self).__init__(**kwargs)
+
+ def _LoadRobotURDF(self):
+ mini_cheetah_urdf_path = "mini_cheetah/mini_cheetah.urdf"
+ if self._self_collision_enabled:
+ self.quadruped = self._pybullet_client.loadURDF(
+ mini_cheetah_urdf_path,
+ self._GetDefaultInitPosition(),
+ self._GetDefaultInitOrientation(),
+ flags=self._pybullet_client.URDF_USE_SELF_COLLISION)
+ else:
+ self.quadruped = self._pybullet_client.loadURDF(
+ mini_cheetah_urdf_path, self._GetDefaultInitPosition(),
+ self._GetDefaultInitOrientation())
+
+ def _SettleDownForReset(self, default_motor_angles, reset_time):
+ self.ReceiveObservation()
+ for _ in range(500):
+ self.ApplyAction(
+ INIT_MOTOR_ANGLES,
+ motor_control_mode=robot_config.MotorControlMode.POSITION)
+ self._pybullet_client.stepSimulation()
+ self.ReceiveObservation()
+ if default_motor_angles is not None:
+ num_steps_to_reset = int(reset_time / self.time_step)
+ for _ in range(num_steps_to_reset):
+ self.ApplyAction(
+ default_motor_angles,
+ motor_control_mode=robot_config.MotorControlMode.POSITION)
+ self._pybullet_client.stepSimulation()
+ self.ReceiveObservation()
+
+ def GetURDFFile(self):
+ return os.path.join(self._urdf_root, "mini_cheetah/mini_cheetah.urdf")
+
+ def ResetPose(self, add_constraint):
+ del add_constraint
+ for name in self._joint_name_to_id:
+ joint_id = self._joint_name_to_id[name]
+ self._pybullet_client.setJointMotorControl2(
+ bodyIndex=self.quadruped,
+ jointIndex=(joint_id),
+ controlMode=self._pybullet_client.VELOCITY_CONTROL,
+ targetVelocity=0,
+ force=0)
+ for name, i in zip(MOTOR_NAMES, range(len(MOTOR_NAMES))):
+ angle = INIT_MOTOR_ANGLES[i]
+ self._pybullet_client.resetJointState(
+ self.quadruped, self._joint_name_to_id[name], angle, targetVelocity=0)
+
+ def _BuildUrdfIds(self):
+ pass
+
+ def _GetMotorNames(self):
+ return MOTOR_NAMES
+
+ def _GetDefaultInitPosition(self):
+ if self._on_rack:
+ return INIT_RACK_POSITION
+ else:
+ return INIT_POSITION
+
+ def _GetDefaultInitOrientation(self):
+ init_orientation = [0, 0, 0, 1.0]
+ return init_orientation
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/robots/mini_cheetah_test.py b/examples/pybullet/gym/pybullet_envs/minitaur/robots/mini_cheetah_test.py
new file mode 100644
index 000000000..94acf54c0
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/robots/mini_cheetah_test.py
@@ -0,0 +1,58 @@
+"""Tests for pybullet_envs.minitaur.robots.mini_cheetah.
+
+blaze test -c opt
+//robotics/reinforcement_learning/minitaur/robots:mini_cheetah_test
+"""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+import math
+import numpy as np
+from pybullet_envs.minitaur.envs import bullet_client
+from pybullet_envs.minitaur.robots import mini_cheetah
+from google3.testing.pybase import googletest
+
+PI = math.pi
+NUM_STEPS = 500
+TIME_STEP = 0.002
+INIT_MOTOR_ANGLES = [0, -0.6, 1.4] * 4
+
+
+class MiniCheetahTest(googletest.TestCase):
+
+ def test_init(self):
+ pybullet_client = bullet_client.BulletClient()
+ pybullet_client.enable_cns()
+ robot = mini_cheetah.MiniCheetah(
+ pybullet_client=pybullet_client, time_step=TIME_STEP, on_rack=True)
+ self.assertIsNotNone(robot)
+
+ def test_static_pose_on_rack(self):
+ pybullet_client = bullet_client.BulletClient()
+ pybullet_client.enable_cns()
+ pybullet_client.resetSimulation()
+ pybullet_client.setPhysicsEngineParameter(numSolverIterations=60)
+ pybullet_client.setTimeStep(TIME_STEP)
+ pybullet_client.setGravity(0, 0, -10)
+
+ robot = (
+ mini_cheetah.MiniCheetah(
+ pybullet_client=pybullet_client,
+ action_repeat=5,
+ time_step=0.002,
+ on_rack=True))
+ robot.Reset(
+ reload_urdf=False,
+ default_motor_angles=INIT_MOTOR_ANGLES,
+ reset_time=0.5)
+ for _ in range(NUM_STEPS):
+ robot.Step(INIT_MOTOR_ANGLES)
+ motor_angles = robot.GetMotorAngles()
+ np.testing.assert_array_almost_equal(
+ motor_angles, INIT_MOTOR_ANGLES, decimal=2)
+
+
+if __name__ == '__main__':
+ googletest.main()
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/robots/minitaur.py b/examples/pybullet/gym/pybullet_envs/minitaur/robots/minitaur.py
new file mode 100644
index 000000000..9494c9ef0
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/robots/minitaur.py
@@ -0,0 +1,1479 @@
+"""This file implements the functionalities of a minitaur using pybullet."""
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+import collections
+import copy
+import logging
+import math
+import re
+import numpy as np
+import gin
+from pybullet_envs.minitaur.robots import minitaur_constants
+from pybullet_envs.minitaur.robots import minitaur_motor
+from pybullet_envs.minitaur.robots import robot_config
+from pybullet_envs.minitaur.robots.safety import safety_checker
+from pybullet_envs.minitaur.robots.safety import safety_error
+from pybullet_envs.minitaur.robots.utilities import action_filter
+from pybullet_envs.minitaur.robots.utilities import kinematics
+
+INIT_POSITION = [0, 0, .2]
+INIT_RACK_POSITION = [0, 0, 1]
+INIT_ORIENTATION = [0, 0, 0, 1]
+KNEE_CONSTRAINT_POINT_RIGHT = [0, 0.005, 0.2]
+KNEE_CONSTRAINT_POINT_LEFT = [0, 0.01, 0.2]
+OVERHEAT_SHUTDOWN_TORQUE = 2.45
+OVERHEAT_SHUTDOWN_TIME = 1.0
+LEG_POSITION = ["front_left", "back_left", "front_right", "back_right"]
+MOTOR_NAMES = [
+ "motor_front_leftL_joint", "motor_front_leftR_joint",
+ "motor_back_leftL_joint", "motor_back_leftR_joint",
+ "motor_front_rightL_joint", "motor_front_rightR_joint",
+ "motor_back_rightL_joint", "motor_back_rightR_joint"
+]
+_CHASSIS_NAME_PATTERN = re.compile(r"chassis\D*center")
+_MOTOR_NAME_PATTERN = re.compile(r"motor\D*joint")
+_KNEE_NAME_PATTERN = re.compile(r"knee\D*")
+_BRACKET_NAME_PATTERN = re.compile(r"motor\D*_bracket_joint")
+_LEG_NAME_PATTERN1 = re.compile(r"hip\D*joint")
+_LEG_NAME_PATTERN2 = re.compile(r"hip\D*link")
+_LEG_NAME_PATTERN3 = re.compile(r"motor\D*link")
+SENSOR_NOISE_STDDEV = (0.0, 0.0, 0.0, 0.0, 0.0)
+MINITAUR_DEFAULT_MOTOR_DIRECTIONS = (-1, -1, -1, -1, 1, 1, 1, 1)
+MINITAUR_DEFAULT_MOTOR_OFFSETS = (0, 0, 0, 0, 0, 0, 0, 0)
+MINITAUR_NUM_MOTORS = 8
+TWO_PI = 2 * math.pi
+MINITAUR_DOFS_PER_LEG = 2
+
+URDF_ROOT = "robotics/reinforcement_learning/minitaur/robots/data/urdf/"
+
+
+def MapToMinusPiToPi(angles):
+ """Maps a list of angles to [-pi, pi].
+
+ Args:
+ angles: A list of angles in rad.
+
+ Returns:
+ A list of angle mapped to [-pi, pi].
+ """
+ mapped_angles = copy.deepcopy(angles)
+ for i in range(len(angles)):
+ mapped_angles[i] = math.fmod(angles[i], TWO_PI)
+ if mapped_angles[i] >= math.pi:
+ mapped_angles[i] -= TWO_PI
+ elif mapped_angles[i] < -math.pi:
+ mapped_angles[i] += TWO_PI
+ return mapped_angles
+
+
+@gin.configurable
+class Minitaur(object):
+ """The minitaur class that simulates a quadruped robot from Ghost Robotics."""
+
+ def __init__(self,
+ pybullet_client,
+ num_motors=MINITAUR_NUM_MOTORS,
+ dofs_per_leg=MINITAUR_DOFS_PER_LEG,
+ urdf_root=URDF_ROOT,
+ time_step=0.01,
+ action_repeat=1,
+ self_collision_enabled=False,
+ motor_control_mode=robot_config.MotorControlMode.POSITION,
+ motor_model_class=minitaur_motor.MotorModel,
+ motor_kp=1.0,
+ motor_kd=0.02,
+ motor_torque_limits=None,
+ pd_latency=0.0,
+ control_latency=0.0,
+ observation_noise_stdev=SENSOR_NOISE_STDDEV,
+ motor_overheat_protection=False,
+ motor_direction=MINITAUR_DEFAULT_MOTOR_DIRECTIONS,
+ motor_offset=MINITAUR_DEFAULT_MOTOR_OFFSETS,
+ on_rack=False,
+ reset_at_current_position=False,
+ sensors=None,
+ safety_config=None,
+ enable_action_interpolation=False,
+ enable_action_filter=False):
+ """Constructs a minitaur and reset it to the initial states.
+
+ Args:
+ pybullet_client: The instance of BulletClient to manage different
+ simulations.
+ num_motors: The number of the motors on the robot.
+ dofs_per_leg: The number of degrees of freedom for each leg.
+ urdf_root: The path to the urdf folder.
+ time_step: The time step of the simulation.
+ action_repeat: The number of ApplyAction() for each control step.
+ self_collision_enabled: Whether to enable self collision.
+ motor_control_mode: Enum. Can either be POSITION, TORQUE, or HYBRID.
+ motor_model_class: We can choose from simple pd model to more accureate DC
+ motor models.
+ motor_kp: proportional gain for the motors.
+ motor_kd: derivative gain for the motors.
+ motor_torque_limits: Torque limits for the motors. Can be a single float
+ or a list of floats specifying different limits for different robots. If
+ not provided, the default limit of the robot is used.
+ pd_latency: The latency of the observations (in seconds) used to calculate
+ PD control. On the real hardware, it is the latency between the
+ microcontroller and the motor controller.
+ control_latency: The latency of the observations (in second) used to
+ calculate action. On the real hardware, it is the latency from the motor
+ controller, the microcontroller to the host (Nvidia TX2).
+ observation_noise_stdev: The standard deviation of a Gaussian noise model
+ for the sensor. It should be an array for separate sensors in the
+ following order [motor_angle, motor_velocity, motor_torque,
+ base_roll_pitch_yaw, base_angular_velocity]
+ motor_overheat_protection: Whether to shutdown the motor that has exerted
+ large torque (OVERHEAT_SHUTDOWN_TORQUE) for an extended amount of time
+ (OVERHEAT_SHUTDOWN_TIME). See ApplyAction() in minitaur.py for more
+ details.
+ motor_direction: A list of direction values, either 1 or -1, to compensate
+ the axis difference of motors between the simulation and the real robot.
+ motor_offset: A list of offset value for the motor angles. This is used to
+ compensate the angle difference between the simulation and the real
+ robot.
+ on_rack: Whether to place the minitaur on rack. This is only used to debug
+ the walking gait. In this mode, the minitaur's base is hanged midair so
+ that its walking gait is clearer to visualize.
+ reset_at_current_position: Whether to reset the minitaur at the current
+ position and orientation. This is for simulating the reset behavior in
+ the real world.
+ sensors: a list of sensors that are attached to the robot.
+ safety_config: A SafetyConfig class to configure the safety layer. If
+ None, the safety layer will be disabled.
+ enable_action_interpolation: Whether to interpolate the current action
+ with the previous action in order to produce smoother motions
+ enable_action_filter: Boolean specifying if a lowpass filter should be
+ used to smooth actions.
+ """
+ self.num_motors = num_motors
+ self.num_legs = self.num_motors // dofs_per_leg
+ self._pybullet_client = pybullet_client
+ self._action_repeat = action_repeat
+ self._urdf_root = urdf_root
+ self._self_collision_enabled = self_collision_enabled
+ self._motor_direction = motor_direction
+ self._motor_offset = motor_offset
+ self._observed_motor_torques = np.zeros(self.num_motors)
+ self._applied_motor_torques = np.zeros(self.num_motors)
+ self._max_force = 3.5
+ self._pd_latency = pd_latency
+ self._control_latency = control_latency
+ self._observation_noise_stdev = observation_noise_stdev
+ self._observation_history = collections.deque(maxlen=100)
+ self._control_observation = []
+ self._chassis_link_ids = [-1]
+ self._leg_link_ids = []
+ self._motor_link_ids = []
+ self._foot_link_ids = []
+ self._motor_overheat_protection = motor_overheat_protection
+ self._on_rack = on_rack
+ self._reset_at_current_position = reset_at_current_position
+ self.SetAllSensors(sensors if sensors is not None else list())
+ self.safety_config = safety_config
+ self._is_safe = True
+ self._safety_checker = None
+
+ self._enable_action_interpolation = enable_action_interpolation
+ self._enable_action_filter = enable_action_filter
+ self._last_action = None
+
+ if not motor_model_class:
+ raise ValueError("Must provide a motor model class!")
+
+ if self._on_rack and self._reset_at_current_position:
+ raise ValueError("on_rack and reset_at_current_position "
+ "cannot be enabled together")
+
+ if isinstance(motor_kp, (collections.Sequence, np.ndarray)):
+ self._motor_kps = np.asarray(motor_kp)
+ else:
+ self._motor_kps = np.full(num_motors, motor_kp)
+
+ if isinstance(motor_kd, (collections.Sequence, np.ndarray)):
+ self._motor_kds = np.asarray(motor_kd)
+ else:
+ self._motor_kds = np.full(num_motors, motor_kd)
+
+ if isinstance(motor_torque_limits, (collections.Sequence, np.ndarray)):
+ self._motor_torque_limits = np.asarray(motor_torque_limits)
+ elif motor_torque_limits is None:
+ self._motor_torque_limits = None
+ else:
+ self._motor_torque_limits = motor_torque_limits
+
+ self._motor_control_mode = motor_control_mode
+ self._motor_model = motor_model_class(
+ kp=motor_kp,
+ kd=motor_kd,
+ torque_limits=self._motor_torque_limits,
+ motor_control_mode=motor_control_mode)
+
+ self.time_step = time_step
+ self._step_counter = 0
+
+ # This also includes the time spent during the Reset motion.
+ self._state_action_counter = 0
+ _, self._init_orientation_inv = self._pybullet_client.invertTransform(
+ position=[0, 0, 0], orientation=self._GetDefaultInitOrientation())
+
+ if self._enable_action_filter:
+ self._action_filter = self._BuildActionFilter()
+ # reset_time=-1.0 means skipping the reset motion.
+ # See Reset for more details.
+ self.Reset(reset_time=-1.0)
+ self.ReceiveObservation()
+
+ return
+
+ def GetTimeSinceReset(self):
+ return self._step_counter * self.time_step
+
+ def _StepInternal(self, action, motor_control_mode=None):
+ self.ApplyAction(action, motor_control_mode)
+ self._pybullet_client.stepSimulation()
+ self.ReceiveObservation()
+ self._state_action_counter += 1
+
+ return
+
+ def Step(self, action):
+ """Steps simulation."""
+ if self._enable_action_filter:
+ action = self._FilterAction(action)
+
+ for i in range(self._action_repeat):
+ proc_action = self.ProcessAction(action, i)
+ self._StepInternal(proc_action)
+ self._step_counter += 1
+
+ self._last_action = action
+ return
+
+ def Terminate(self):
+ pass
+
+ def GetKneeLinkIDs(self):
+ """Get list of IDs for all knee links."""
+ return self._knee_link_ids
+
+ def GetFootLinkIDs(self):
+ """Get list of IDs for all foot links."""
+ return self._foot_link_ids
+
+ def _RecordMassInfoFromURDF(self):
+ """Records the mass information from the URDF file."""
+ self._base_mass_urdf = []
+ for chassis_id in self._chassis_link_ids:
+ self._base_mass_urdf.append(
+ self._pybullet_client.getDynamicsInfo(self.quadruped, chassis_id)[0])
+ self._leg_masses_urdf = []
+ for leg_id in self._leg_link_ids:
+ self._leg_masses_urdf.append(
+ self._pybullet_client.getDynamicsInfo(self.quadruped, leg_id)[0])
+ for motor_id in self._motor_link_ids:
+ self._leg_masses_urdf.append(
+ self._pybullet_client.getDynamicsInfo(self.quadruped, motor_id)[0])
+
+ def _RecordInertiaInfoFromURDF(self):
+ """Record the inertia of each body from URDF file."""
+ self._link_urdf = []
+ num_bodies = self._pybullet_client.getNumJoints(self.quadruped)
+ for body_id in range(-1, num_bodies): # -1 is for the base link.
+ inertia = self._pybullet_client.getDynamicsInfo(self.quadruped,
+ body_id)[2]
+ self._link_urdf.append(inertia)
+ # We need to use id+1 to index self._link_urdf because it has the base
+ # (index = -1) at the first element.
+ self._base_inertia_urdf = [
+ self._link_urdf[chassis_id + 1] for chassis_id in self._chassis_link_ids
+ ]
+ self._leg_inertia_urdf = [
+ self._link_urdf[leg_id + 1] for leg_id in self._leg_link_ids
+ ]
+ self._leg_inertia_urdf.extend(
+ [self._link_urdf[motor_id + 1] for motor_id in self._motor_link_ids])
+
+ def _BuildJointNameToIdDict(self):
+ num_joints = self._pybullet_client.getNumJoints(self.quadruped)
+ self._joint_name_to_id = {}
+ for i in range(num_joints):
+ joint_info = self._pybullet_client.getJointInfo(self.quadruped, i)
+ self._joint_name_to_id[joint_info[1].decode("UTF-8")] = joint_info[0]
+
+ def _BuildUrdfIds(self):
+ """Build the link Ids from its name in the URDF file.
+
+ Raises:
+ ValueError: Unknown category of the joint name.
+ """
+ num_joints = self._pybullet_client.getNumJoints(self.quadruped)
+ self._chassis_link_ids = [-1]
+ # The self._leg_link_ids include both the upper and lower links of the leg.
+ self._leg_link_ids = []
+ self._motor_link_ids = []
+ self._foot_link_ids = []
+ self._bracket_link_ids = []
+ for i in range(num_joints):
+ joint_info = self._pybullet_client.getJointInfo(self.quadruped, i)
+ joint_name = joint_info[1].decode("UTF-8")
+ joint_id = self._joint_name_to_id[joint_name]
+ if _CHASSIS_NAME_PATTERN.match(joint_name):
+ self._chassis_link_ids.append(joint_id)
+ elif _BRACKET_NAME_PATTERN.match(joint_name):
+ self._bracket_link_ids.append(joint_id)
+ elif _MOTOR_NAME_PATTERN.match(joint_name):
+ self._motor_link_ids.append(joint_id)
+ elif _KNEE_NAME_PATTERN.match(joint_name):
+ self._foot_link_ids.append(joint_id)
+ elif (_LEG_NAME_PATTERN1.match(joint_name) or
+ _LEG_NAME_PATTERN2.match(joint_name) or
+ _LEG_NAME_PATTERN3.match(joint_name)):
+ self._leg_link_ids.append(joint_id)
+ else:
+ raise ValueError("Unknown category of joint %s" % joint_name)
+
+ self._leg_link_ids.extend(self._foot_link_ids)
+ self._chassis_link_ids.sort()
+ self._motor_link_ids.sort()
+ self._foot_link_ids.sort()
+ self._leg_link_ids.sort()
+ self._bracket_link_ids.sort()
+
+ def _RemoveDefaultJointDamping(self):
+ num_joints = self._pybullet_client.getNumJoints(self.quadruped)
+ for i in range(num_joints):
+ joint_info = self._pybullet_client.getJointInfo(self.quadruped, i)
+ self._pybullet_client.changeDynamics(
+ joint_info[0], -1, linearDamping=0, angularDamping=0)
+
+ def _BuildMotorIdList(self):
+ self._motor_id_list = [
+ self._joint_name_to_id[motor_name]
+ for motor_name in self._GetMotorNames()
+ ]
+
+ def _CreateRackConstraint(self, init_position, init_orientation):
+ """Create a constraint that keeps the chassis at a fixed frame.
+
+ This frame is defined by init_position and init_orientation.
+
+ Args:
+ init_position: initial position of the fixed frame.
+ init_orientation: initial orientation of the fixed frame in quaternion
+ format [x,y,z,w].
+
+ Returns:
+ Return the constraint id.
+ """
+ fixed_constraint = self._pybullet_client.createConstraint(
+ parentBodyUniqueId=self.quadruped,
+ parentLinkIndex=-1,
+ childBodyUniqueId=-1,
+ childLinkIndex=-1,
+ jointType=self._pybullet_client.JOINT_FIXED,
+ jointAxis=[0, 0, 0],
+ parentFramePosition=[0, 0, 0],
+ childFramePosition=init_position,
+ childFrameOrientation=init_orientation)
+ return fixed_constraint
+
+ def IsObservationValid(self):
+ """Whether the observation is valid for the current time step.
+
+ In simulation, observations are always valid. In real hardware, it may not
+ be valid from time to time when communication error happens between the
+ Nvidia TX2 and the microcontroller.
+
+ Returns:
+ Whether the observation is valid for the current time step.
+ """
+ return True
+
+ def Reset(self, reload_urdf=True, default_motor_angles=None, reset_time=3.0):
+ """Reset the minitaur to its initial states.
+
+ Args:
+ reload_urdf: Whether to reload the urdf file. If not, Reset() just place
+ the minitaur back to its starting position.
+ default_motor_angles: The default motor angles. If it is None, minitaur
+ will hold a default pose (motor angle math.pi / 2) for 100 steps. In
+ torque control mode, the phase of holding the default pose is skipped.
+ reset_time: The duration (in seconds) to hold the default motor angles. If
+ reset_time <= 0 or in torque control mode, the phase of holding the
+ default pose is skipped.
+ """
+ if reload_urdf:
+ self._LoadRobotURDF()
+ if self._on_rack:
+ self.rack_constraint = (
+ self._CreateRackConstraint(self._GetDefaultInitPosition(),
+ self._GetDefaultInitOrientation()))
+ self._BuildJointNameToIdDict()
+ self._BuildUrdfIds()
+ self._RemoveDefaultJointDamping()
+ self._BuildMotorIdList()
+ self._RecordMassInfoFromURDF()
+ self._RecordInertiaInfoFromURDF()
+ self.ResetPose(add_constraint=True)
+ else:
+ self._pybullet_client.resetBasePositionAndOrientation(
+ self.quadruped, self._GetDefaultInitPosition(),
+ self._GetDefaultInitOrientation())
+ self._pybullet_client.resetBaseVelocity(self.quadruped, [0, 0, 0],
+ [0, 0, 0])
+ self.ResetPose(add_constraint=False)
+
+ self._overheat_counter = np.zeros(self.num_motors)
+ self._motor_enabled_list = [True] * self.num_motors
+ self._observation_history.clear()
+ self._step_counter = 0
+ self._state_action_counter = 0
+ self._is_safe = True
+ self._last_action = None
+
+ # Enable the safety layer before we perform any reset motions.
+ if self.safety_config:
+ self._safety_checker = safety_checker.SafetyChecker(self)
+ self._SettleDownForReset(default_motor_angles, reset_time)
+
+ if self._enable_action_filter:
+ self._ResetActionFilter()
+
+ return
+
+ def _LoadRobotURDF(self):
+ """Loads the URDF file for the robot."""
+ urdf_file = self.GetURDFFile()
+ if self._self_collision_enabled:
+ self.quadruped = self._pybullet_client.loadURDF(
+ urdf_file,
+ self._GetDefaultInitPosition(),
+ self._GetDefaultInitOrientation(),
+ flags=self._pybullet_client.URDF_USE_SELF_COLLISION)
+ else:
+ self.quadruped = self._pybullet_client.loadURDF(
+ urdf_file, self._GetDefaultInitPosition(),
+ self._GetDefaultInitOrientation())
+
+ def _SettleDownForReset(self, default_motor_angles, reset_time):
+ """Sets the default motor angles and waits for the robot to settle down.
+
+ The reset is skipped is reset_time is less than zereo.
+
+ Args:
+ default_motor_angles: A list of motor angles that the robot will achieve
+ at the end of the reset phase.
+ reset_time: The time duration for the reset phase.
+ """
+ if reset_time <= 0:
+ return
+
+ # Important to fill the observation buffer.
+ self.ReceiveObservation()
+ for _ in range(100):
+ self._StepInternal(
+ [math.pi / 2] * self.num_motors,
+ motor_control_mode=robot_config.MotorControlMode.POSITION)
+ # Don't continue to reset if a safety error has occurred.
+ if not self._is_safe:
+ return
+
+ if default_motor_angles is None:
+ return
+
+ num_steps_to_reset = int(reset_time / self.time_step)
+ for _ in range(num_steps_to_reset):
+ self._StepInternal(
+ default_motor_angles,
+ motor_control_mode=robot_config.MotorControlMode.POSITION)
+ # Don't continue to reset if a safety error has occurred.
+ if not self._is_safe:
+ return
+
+ def _SetMotorTorqueById(self, motor_id, torque):
+ self._pybullet_client.setJointMotorControl2(
+ bodyIndex=self.quadruped,
+ jointIndex=motor_id,
+ controlMode=self._pybullet_client.TORQUE_CONTROL,
+ force=torque)
+
+ def _SetMotorTorqueByIds(self, motor_ids, torques):
+ self._pybullet_client.setJointMotorControlArray(
+ bodyIndex=self.quadruped,
+ jointIndices=motor_ids,
+ controlMode=self._pybullet_client.TORQUE_CONTROL,
+ forces=torques)
+
+ def _SetDesiredMotorAngleByName(self, motor_name, desired_angle):
+ self._SetDesiredMotorAngleById(self._joint_name_to_id[motor_name],
+ desired_angle)
+
+ def GetURDFFile(self):
+ return "%s/quadruped/minitaur.urdf" % self._urdf_root
+
+ def ResetPose(self, add_constraint):
+ """Reset the pose of the minitaur.
+
+ Args:
+ add_constraint: Whether to add a constraint at the joints of two feet.
+ """
+ for i in range(self.num_legs):
+ self._ResetPoseForLeg(i, add_constraint)
+
+ def _ResetPoseForLeg(self, leg_id, add_constraint):
+ """Reset the initial pose for the leg.
+
+ Args:
+ leg_id: It should be 0, 1, 2, or 3, which represents the leg at
+ front_left, back_left, front_right and back_right.
+ add_constraint: Whether to add a constraint at the joints of two feet.
+ """
+ knee_friction_force = 0
+ half_pi = math.pi / 2.0
+ knee_angle = -2.1834
+
+ leg_position = LEG_POSITION[leg_id]
+ self._pybullet_client.resetJointState(
+ self.quadruped,
+ self._joint_name_to_id["motor_" + leg_position + "L_joint"],
+ self._motor_direction[2 * leg_id] * half_pi,
+ targetVelocity=0)
+ self._pybullet_client.resetJointState(
+ self.quadruped,
+ self._joint_name_to_id["knee_" + leg_position + "L_link"],
+ self._motor_direction[2 * leg_id] * knee_angle,
+ targetVelocity=0)
+ self._pybullet_client.resetJointState(
+ self.quadruped,
+ self._joint_name_to_id["motor_" + leg_position + "R_joint"],
+ self._motor_direction[2 * leg_id + 1] * half_pi,
+ targetVelocity=0)
+ self._pybullet_client.resetJointState(
+ self.quadruped,
+ self._joint_name_to_id["knee_" + leg_position + "R_link"],
+ self._motor_direction[2 * leg_id + 1] * knee_angle,
+ targetVelocity=0)
+ if add_constraint:
+ self._pybullet_client.createConstraint(
+ self.quadruped,
+ self._joint_name_to_id["knee_" + leg_position + "R_link"],
+ self.quadruped,
+ self._joint_name_to_id["knee_" + leg_position + "L_link"],
+ self._pybullet_client.JOINT_POINT2POINT, [0, 0, 0],
+ KNEE_CONSTRAINT_POINT_RIGHT, KNEE_CONSTRAINT_POINT_LEFT)
+
+ # Disable the default motor in pybullet.
+ self._pybullet_client.setJointMotorControl2(
+ bodyIndex=self.quadruped,
+ jointIndex=(self._joint_name_to_id["motor_" + leg_position +
+ "L_joint"]),
+ controlMode=self._pybullet_client.VELOCITY_CONTROL,
+ targetVelocity=0,
+ force=knee_friction_force)
+ self._pybullet_client.setJointMotorControl2(
+ bodyIndex=self.quadruped,
+ jointIndex=(self._joint_name_to_id["motor_" + leg_position +
+ "R_joint"]),
+ controlMode=self._pybullet_client.VELOCITY_CONTROL,
+ targetVelocity=0,
+ force=knee_friction_force)
+
+ self._pybullet_client.setJointMotorControl2(
+ bodyIndex=self.quadruped,
+ jointIndex=(self._joint_name_to_id["knee_" + leg_position + "L_link"]),
+ controlMode=self._pybullet_client.VELOCITY_CONTROL,
+ targetVelocity=0,
+ force=knee_friction_force)
+ self._pybullet_client.setJointMotorControl2(
+ bodyIndex=self.quadruped,
+ jointIndex=(self._joint_name_to_id["knee_" + leg_position + "R_link"]),
+ controlMode=self._pybullet_client.VELOCITY_CONTROL,
+ targetVelocity=0,
+ force=knee_friction_force)
+
+ def GetBasePosition(self):
+ """Get the position of minitaur's base.
+
+ Returns:
+ The position of minitaur's base.
+ """
+ return self._base_position
+
+ def GetBaseVelocity(self):
+ """Get the linear velocity of minitaur's base.
+
+ Returns:
+ The velocity of minitaur's base.
+ """
+ velocity, _ = self._pybullet_client.getBaseVelocity(self.quadruped)
+ return velocity
+
+ def GetTrueBaseRollPitchYaw(self):
+ """Get minitaur's base orientation in euler angle in the world frame.
+
+ Returns:
+ A tuple (roll, pitch, yaw) of the base in world frame.
+ """
+ orientation = self.GetTrueBaseOrientation()
+ roll_pitch_yaw = self._pybullet_client.getEulerFromQuaternion(orientation)
+ return np.asarray(roll_pitch_yaw)
+
+ def GetBaseRollPitchYaw(self):
+ """Get minitaur's base orientation in euler angle in the world frame.
+
+ This function mimicks the noisy sensor reading and adds latency.
+ Returns:
+ A tuple (roll, pitch, yaw) of the base in world frame polluted by noise
+ and latency.
+ """
+ delayed_orientation = np.array(
+ self._control_observation[3 * self.num_motors:3 * self.num_motors + 4])
+ delayed_roll_pitch_yaw = self._pybullet_client.getEulerFromQuaternion(
+ delayed_orientation)
+ roll_pitch_yaw = self._AddSensorNoise(
+ np.array(delayed_roll_pitch_yaw), self._observation_noise_stdev[3])
+ return roll_pitch_yaw
+
+ def GetHipPositionsInBaseFrame(self):
+ """Get the hip joint positions of the robot within its base frame."""
+ raise NotImplementedError("Not implemented for Minitaur.")
+
+ def _EndEffectorIK(self, leg_id, position, position_in_world_frame):
+ """Calculate the joint positions from the end effector position."""
+ assert len(self._foot_link_ids) == self.num_legs
+ toe_id = self._foot_link_ids[leg_id]
+ motors_per_leg = self.num_motors // self.num_legs
+ joint_position_idxs = [
+ i for i in range(leg_id * motors_per_leg, leg_id * motors_per_leg +
+ motors_per_leg)
+ ]
+ joint_angles = kinematics.joint_angles_from_link_position(
+ robot=self,
+ link_position=position,
+ link_id=toe_id,
+ joint_ids=joint_position_idxs,
+ position_in_world_frame=position_in_world_frame)
+ # Joint offset is necessary for Laikago.
+ joint_angles = np.multiply(
+ np.asarray(joint_angles) -
+ np.asarray(self._motor_offset)[joint_position_idxs],
+ self._motor_direction[joint_position_idxs])
+ # Return the joing index (the same as when calling GetMotorAngles) as well
+ # as the angles.
+ return joint_position_idxs, joint_angles.tolist()
+
+ # TODO(b/154361633): Implements an array version of this following function.
+ def ComputeMotorAnglesFromFootWorldPosition(self, leg_id,
+ foot_world_position):
+ """Use IK to compute the motor angles, given the foot link's position.
+
+ Args:
+ leg_id: The leg index.
+ foot_world_position: The foot link's position in the world frame.
+
+ Returns:
+ A tuple. The position indices and the angles for all joints along the
+ leg. The position indices is consistent with the joint orders as returned
+ by GetMotorAngles API.
+ """
+ return self._EndEffectorIK(
+ leg_id, foot_world_position, position_in_world_frame=True)
+
+ def ComputeMotorAnglesFromFootLocalPosition(self, leg_id,
+ foot_local_position):
+ """Use IK to compute the motor angles, given the foot link's local position.
+
+ Args:
+ leg_id: The leg index.
+ foot_local_position: The foot link's position in the base frame.
+
+ Returns:
+ A tuple. The position indices and the angles for all joints along the
+ leg. The position indices is consistent with the joint orders as returned
+ by GetMotorAngles API.
+ """
+ return self._EndEffectorIK(
+ leg_id, foot_local_position, position_in_world_frame=False)
+
+ def ComputeJacobian(self, leg_id):
+ """Compute the Jacobian for a given leg."""
+ # Does not work for Minitaur which has the four bar mechanism for now.
+ assert len(self._foot_link_ids) == self.num_legs
+ return kinematics.compute_jacobian(
+ robot=self,
+ link_id=self._foot_link_ids[leg_id],
+ )
+
+ def MapContactForceToJointTorques(self, leg_id, contact_force):
+ """Maps the foot contact force to the leg joint torques."""
+ jv = self.ComputeJacobian(leg_id)
+ all_motor_torques = np.matmul(contact_force, jv)
+ motor_torques = {}
+ motors_per_leg = self.num_motors // self.num_legs
+ com_dof = 6
+ for joint_id in range(leg_id * motors_per_leg,
+ (leg_id + 1) * motors_per_leg):
+ motor_torques[joint_id] = all_motor_torques[
+ com_dof + joint_id] * self._motor_direction[joint_id]
+
+ return motor_torques
+
+ def GetFootContacts(self):
+ """Get minitaur's foot contact situation with the ground.
+
+ Returns:
+ A list of 4 booleans. The ith boolean is True if leg i is in contact with
+ ground.
+ """
+ contacts = []
+ for leg_idx in range(MINITAUR_NUM_MOTORS // 2):
+ link_id_1 = self._foot_link_ids[leg_idx * 2]
+ link_id_2 = self._foot_link_ids[leg_idx * 2 + 1]
+ contact_1 = bool(
+ self._pybullet_client.getContactPoints(
+ bodyA=0,
+ bodyB=self.quadruped,
+ linkIndexA=-1,
+ linkIndexB=link_id_1))
+ contact_2 = bool(
+ self._pybullet_client.getContactPoints(
+ bodyA=0,
+ bodyB=self.quadruped,
+ linkIndexA=-1,
+ linkIndexB=link_id_2))
+ contacts.append(contact_1 or contact_2)
+ return contacts
+
+ def GetFootPositionsInWorldFrame(self):
+ """Get the robot's foot position in the base frame."""
+ assert len(self._foot_link_ids) == self.num_legs
+ foot_positions = []
+ for foot_id in self.GetFootLinkIDs():
+ foot_positions.append(
+ kinematics.link_position_in_world_frame(
+ robot=self,
+ link_id=foot_id,
+ ))
+ return np.array(foot_positions)
+
+ def GetFootPositionsInBaseFrame(self):
+ """Get the robot's foot position in the base frame."""
+ assert len(self._foot_link_ids) == self.num_legs
+ foot_positions = []
+ for foot_id in self.GetFootLinkIDs():
+ foot_positions.append(
+ kinematics.link_position_in_base_frame(
+ robot=self,
+ link_id=foot_id,
+ ))
+ return np.array(foot_positions)
+
+ def GetTrueMotorAngles(self):
+ """Gets the eight motor angles at the current moment, mapped to [-pi, pi].
+
+ Returns:
+ Motor angles, mapped to [-pi, pi].
+ """
+ motor_angles = [state[0] for state in self._joint_states]
+ motor_angles = np.multiply(
+ np.asarray(motor_angles) - np.asarray(self._motor_offset),
+ self._motor_direction)
+ return motor_angles
+
+ def GetMotorAngles(self):
+ """Gets the eight motor angles.
+
+ This function mimicks the noisy sensor reading and adds latency. The motor
+ angles that are delayed, noise polluted, and mapped to [-pi, pi].
+
+ Returns:
+ Motor angles polluted by noise and latency, mapped to [-pi, pi].
+ """
+ motor_angles = self._AddSensorNoise(
+ np.array(self._control_observation[0:self.num_motors]),
+ self._observation_noise_stdev[0])
+ return MapToMinusPiToPi(motor_angles)
+
+ def GetTrueMotorVelocities(self):
+ """Get the velocity of all eight motors.
+
+ Returns:
+ Velocities of all eight motors.
+ """
+ motor_velocities = [state[1] for state in self._joint_states]
+
+ motor_velocities = np.multiply(motor_velocities, self._motor_direction)
+ return motor_velocities
+
+ def GetMotorVelocities(self):
+ """Get the velocity of all eight motors.
+
+ This function mimicks the noisy sensor reading and adds latency.
+ Returns:
+ Velocities of all eight motors polluted by noise and latency.
+ """
+ return self._AddSensorNoise(
+ np.array(self._control_observation[self.num_motors:2 *
+ self.num_motors]),
+ self._observation_noise_stdev[1])
+
+ def GetTrueMotorTorques(self):
+ """Get the amount of torque the motors are exerting.
+
+ Returns:
+ Motor torques of all eight motors.
+ """
+ return self._observed_motor_torques
+
+ def GetMotorTorques(self):
+ """Get the amount of torque the motors are exerting.
+
+ This function mimicks the noisy sensor reading and adds latency.
+ Returns:
+ Motor torques of all eight motors polluted by noise and latency.
+ """
+ return self._AddSensorNoise(
+ np.array(self._control_observation[2 * self.num_motors:3 *
+ self.num_motors]),
+ self._observation_noise_stdev[2])
+
+ def GetEnergyConsumptionPerControlStep(self):
+ """Get the amount of energy used in last one time step.
+
+ Returns:
+ Energy Consumption based on motor velocities and torques (Nm^2/s).
+ """
+ return np.abs(np.dot(
+ self.GetMotorTorques(),
+ self.GetMotorVelocities())) * self.time_step * self._action_repeat
+
+ def GetTrueBaseOrientation(self):
+ """Get the orientation of minitaur's base, represented as quaternion.
+
+ Returns:
+ The orientation of minitaur's base.
+ """
+ return self._base_orientation
+
+ def GetBaseOrientation(self):
+ """Get the orientation of minitaur's base, represented as quaternion.
+
+ This function mimicks the noisy sensor reading and adds latency.
+ Returns:
+ The orientation of minitaur's base polluted by noise and latency.
+ """
+ return self._pybullet_client.getQuaternionFromEuler(
+ self.GetBaseRollPitchYaw())
+
+ def GetTrueBaseRollPitchYawRate(self):
+ """Get the rate of orientation change of the minitaur's base in euler angle.
+
+ Returns:
+ rate of (roll, pitch, yaw) change of the minitaur's base.
+ """
+ angular_velocity = self._pybullet_client.getBaseVelocity(self.quadruped)[1]
+ orientation = self.GetTrueBaseOrientation()
+ return self.TransformAngularVelocityToLocalFrame(angular_velocity,
+ orientation)
+
+ def TransformAngularVelocityToLocalFrame(self, angular_velocity, orientation):
+ """Transform the angular velocity from world frame to robot's frame.
+
+ Args:
+ angular_velocity: Angular velocity of the robot in world frame.
+ orientation: Orientation of the robot represented as a quaternion.
+
+ Returns:
+ angular velocity of based on the given orientation.
+ """
+ # Treat angular velocity as a position vector, then transform based on the
+ # orientation given by dividing (or multiplying with inverse).
+ # Get inverse quaternion assuming the vector is at 0,0,0 origin.
+ _, orientation_inversed = self._pybullet_client.invertTransform([0, 0, 0],
+ orientation)
+ # Transform the angular_velocity at neutral orientation using a neutral
+ # translation and reverse of the given orientation.
+ relative_velocity, _ = self._pybullet_client.multiplyTransforms(
+ [0, 0, 0], orientation_inversed, angular_velocity,
+ self._pybullet_client.getQuaternionFromEuler([0, 0, 0]))
+ return np.asarray(relative_velocity)
+
+ def GetBaseRollPitchYawRate(self):
+ """Get the rate of orientation change of the minitaur's base in euler angle.
+
+ This function mimicks the noisy sensor reading and adds latency.
+ Returns:
+ rate of (roll, pitch, yaw) change of the minitaur's base polluted by noise
+ and latency.
+ """
+ return self._AddSensorNoise(
+ np.array(self._control_observation[3 * self.num_motors +
+ 4:3 * self.num_motors + 7]),
+ self._observation_noise_stdev[4])
+
+ def GetActionDimension(self):
+ """Get the length of the action list.
+
+ Returns:
+ The length of the action list.
+ """
+ return self.num_motors
+
+ def _ApplyOverheatProtection(self, actual_torque):
+ if self._motor_overheat_protection:
+ for i in range(self.num_motors):
+ if abs(actual_torque[i]) > OVERHEAT_SHUTDOWN_TORQUE:
+ self._overheat_counter[i] += 1
+ else:
+ self._overheat_counter[i] = 0
+ if (self._overheat_counter[i] >
+ OVERHEAT_SHUTDOWN_TIME / self.time_step):
+ self._motor_enabled_list[i] = False
+
+ def ApplyAction(self, motor_commands, motor_control_mode=None):
+ """Apply the motor commands using the motor model.
+
+ Args:
+ motor_commands: np.array. Can be motor angles, torques, hybrid commands,
+ or motor pwms (for Minitaur only).
+ motor_control_mode: A MotorControlMode enum.
+ """
+ self.last_action_time = self._state_action_counter * self.time_step
+ control_mode = motor_control_mode
+ if control_mode is None:
+ control_mode = self._motor_control_mode
+ if self._safety_checker:
+ try:
+ self._safety_checker.check_motor_action(motor_commands, control_mode)
+ except safety_error.SafetyError as e:
+ logging.info("A safety error occurred: %s", e)
+ self._is_safe = False
+ return
+ motor_commands = np.asarray(motor_commands)
+
+ q, qdot = self._GetPDObservation()
+ qdot_true = self.GetTrueMotorVelocities()
+ actual_torque, observed_torque = self._motor_model.convert_to_torque(
+ motor_commands, q, qdot, qdot_true, control_mode)
+
+ # May turn off the motor
+ self._ApplyOverheatProtection(actual_torque)
+
+ # The torque is already in the observation space because we use
+ # GetMotorAngles and GetMotorVelocities.
+ self._observed_motor_torques = observed_torque
+
+ # Transform into the motor space when applying the torque.
+ self._applied_motor_torque = np.multiply(actual_torque,
+ self._motor_direction)
+ motor_ids = []
+ motor_torques = []
+
+ for motor_id, motor_torque, motor_enabled in zip(self._motor_id_list,
+ self._applied_motor_torque,
+ self._motor_enabled_list):
+ if motor_enabled:
+ motor_ids.append(motor_id)
+ motor_torques.append(motor_torque)
+ else:
+ motor_ids.append(motor_id)
+ motor_torques.append(0)
+ self._SetMotorTorqueByIds(motor_ids, motor_torques)
+
+ def ConvertFromLegModel(self, actions):
+ """Convert the actions that use leg model to the real motor actions.
+
+ Args:
+ actions: The theta, phi of the leg model.
+
+ Returns:
+ The eight desired motor angles that can be used in ApplyActions().
+ """
+ motor_angle = copy.deepcopy(actions)
+ scale_for_singularity = 1
+ offset_for_singularity = 1.5
+ half_num_motors = self.num_motors // 2
+ quater_pi = math.pi / 4
+ for i in range(self.num_motors):
+ action_idx = i // 2
+ forward_backward_component = (
+ -scale_for_singularity * quater_pi *
+ (actions[action_idx + half_num_motors] + offset_for_singularity))
+ extension_component = (-1)**i * quater_pi * actions[action_idx]
+ if i >= half_num_motors:
+ extension_component = -extension_component
+ motor_angle[i] = (
+ math.pi + forward_backward_component + extension_component)
+ return motor_angle
+
+ def GetBaseMassesFromURDF(self):
+ """Get the mass of the base from the URDF file."""
+ return self._base_mass_urdf
+
+ def GetBaseInertiasFromURDF(self):
+ """Get the inertia of the base from the URDF file."""
+ return self._base_inertia_urdf
+
+ def GetLegMassesFromURDF(self):
+ """Get the mass of the legs from the URDF file."""
+ return self._leg_masses_urdf
+
+ def GetLegInertiasFromURDF(self):
+ """Get the inertia of the legs from the URDF file."""
+ return self._leg_inertia_urdf
+
+ def SetBaseMasses(self, base_mass):
+ """Set the mass of minitaur's base.
+
+ Args:
+ base_mass: A list of masses of each body link in CHASIS_LINK_IDS. The
+ length of this list should be the same as the length of CHASIS_LINK_IDS.
+
+ Raises:
+ ValueError: It is raised when the length of base_mass is not the same as
+ the length of self._chassis_link_ids.
+ """
+ if len(base_mass) != len(self._chassis_link_ids):
+ raise ValueError(
+ "The length of base_mass {} and self._chassis_link_ids {} are not "
+ "the same.".format(len(base_mass), len(self._chassis_link_ids)))
+ for chassis_id, chassis_mass in zip(self._chassis_link_ids, base_mass):
+ self._pybullet_client.changeDynamics(
+ self.quadruped, chassis_id, mass=chassis_mass)
+
+ def SetLegMasses(self, leg_masses):
+ """Set the mass of the legs.
+
+ A leg includes leg_link and motor. 4 legs contain 16 links (4 links each)
+ and 8 motors. First 16 numbers correspond to link masses, last 8 correspond
+ to motor masses (24 total).
+
+ Args:
+ leg_masses: The leg and motor masses for all the leg links and motors.
+
+ Raises:
+ ValueError: It is raised when the length of masses is not equal to number
+ of links + motors.
+ """
+ if len(leg_masses) != len(self._leg_link_ids) + len(self._motor_link_ids):
+ raise ValueError("The number of values passed to SetLegMasses are "
+ "different than number of leg links and motors.")
+ for leg_id, leg_mass in zip(self._leg_link_ids, leg_masses):
+ self._pybullet_client.changeDynamics(
+ self.quadruped, leg_id, mass=leg_mass)
+ motor_masses = leg_masses[len(self._leg_link_ids):]
+ for link_id, motor_mass in zip(self._motor_link_ids, motor_masses):
+ self._pybullet_client.changeDynamics(
+ self.quadruped, link_id, mass=motor_mass)
+
+ def SetBaseInertias(self, base_inertias):
+ """Set the inertias of minitaur's base.
+
+ Args:
+ base_inertias: A list of inertias of each body link in CHASIS_LINK_IDS.
+ The length of this list should be the same as the length of
+ CHASIS_LINK_IDS.
+
+ Raises:
+ ValueError: It is raised when the length of base_inertias is not the same
+ as the length of self._chassis_link_ids and base_inertias contains
+ negative values.
+ """
+ if len(base_inertias) != len(self._chassis_link_ids):
+ raise ValueError(
+ "The length of base_inertias {} and self._chassis_link_ids {} are "
+ "not the same.".format(
+ len(base_inertias), len(self._chassis_link_ids)))
+ for chassis_id, chassis_inertia in zip(self._chassis_link_ids,
+ base_inertias):
+ for inertia_value in chassis_inertia:
+ if (np.asarray(inertia_value) < 0).any():
+ raise ValueError("Values in inertia matrix should be non-negative.")
+ self._pybullet_client.changeDynamics(
+ self.quadruped, chassis_id, localInertiaDiagonal=chassis_inertia)
+
+ def SetLegInertias(self, leg_inertias):
+ """Set the inertias of the legs.
+
+ A leg includes leg_link and motor. 4 legs contain 16 links (4 links each)
+ and 8 motors. First 16 numbers correspond to link inertia, last 8 correspond
+ to motor inertia (24 total).
+
+ Args:
+ leg_inertias: The leg and motor inertias for all the leg links and motors.
+
+ Raises:
+ ValueError: It is raised when the length of inertias is not equal to
+ the number of links + motors or leg_inertias contains negative values.
+ """
+
+ if len(leg_inertias) != len(self._leg_link_ids) + len(self._motor_link_ids):
+ raise ValueError("The number of values passed to SetLegMasses are "
+ "different than number of leg links and motors.")
+ for leg_id, leg_inertia in zip(self._leg_link_ids, leg_inertias):
+ for inertia_value in leg_inertias:
+ if (np.asarray(inertia_value) < 0).any():
+ raise ValueError("Values in inertia matrix should be non-negative.")
+ self._pybullet_client.changeDynamics(
+ self.quadruped, leg_id, localInertiaDiagonal=leg_inertia)
+
+ motor_inertias = leg_inertias[len(self._leg_link_ids):]
+ for link_id, motor_inertia in zip(self._motor_link_ids, motor_inertias):
+ for inertia_value in motor_inertias:
+ if (np.asarray(inertia_value) < 0).any():
+ raise ValueError("Values in inertia matrix should be non-negative.")
+ self._pybullet_client.changeDynamics(
+ self.quadruped, link_id, localInertiaDiagonal=motor_inertia)
+
+ def SetFootFriction(self, foot_friction):
+ """Set the lateral friction of the feet.
+
+ Args:
+ foot_friction: The lateral friction coefficient of the foot. This value is
+ shared by all four feet.
+ """
+ for link_id in self._foot_link_ids:
+ self._pybullet_client.changeDynamics(
+ self.quadruped, link_id, lateralFriction=foot_friction)
+
+ # TODO(b/73748980): Add more API's to set other contact parameters.
+ def SetFootRestitution(self, foot_restitution):
+ """Set the coefficient of restitution at the feet.
+
+ Args:
+ foot_restitution: The coefficient of restitution (bounciness) of the feet.
+ This value is shared by all four feet.
+ """
+ for link_id in self._foot_link_ids:
+ self._pybullet_client.changeDynamics(
+ self.quadruped, link_id, restitution=foot_restitution)
+
+ def SetJointFriction(self, joint_frictions):
+ for knee_joint_id, friction in zip(self._foot_link_ids, joint_frictions):
+ self._pybullet_client.setJointMotorControl2(
+ bodyIndex=self.quadruped,
+ jointIndex=knee_joint_id,
+ controlMode=self._pybullet_client.VELOCITY_CONTROL,
+ targetVelocity=0,
+ force=friction)
+
+ def GetNumKneeJoints(self):
+ return len(self._foot_link_ids)
+
+ def SetBatteryVoltage(self, voltage):
+ self._motor_model.set_voltage(voltage)
+
+ def SetMotorViscousDamping(self, viscous_damping):
+ self._motor_model.set_viscous_damping(viscous_damping)
+
+ def GetTrueObservation(self):
+ observation = []
+ observation.extend(self.GetTrueMotorAngles())
+ observation.extend(self.GetTrueMotorVelocities())
+ observation.extend(self.GetTrueMotorTorques())
+ observation.extend(self.GetTrueBaseOrientation())
+ observation.extend(self.GetTrueBaseRollPitchYawRate())
+ return observation
+
+ def ReceiveObservation(self):
+ """Receive the observation from sensors.
+
+ This function is called once per step. The observations are only updated
+ when this function is called.
+ """
+ self._joint_states = self._pybullet_client.getJointStates(
+ self.quadruped, self._motor_id_list)
+ self._base_position, orientation = (
+ self._pybullet_client.getBasePositionAndOrientation(self.quadruped))
+ # Computes the relative orientation relative to the robot's
+ # initial_orientation.
+ _, self._base_orientation = self._pybullet_client.multiplyTransforms(
+ positionA=[0, 0, 0],
+ orientationA=orientation,
+ positionB=[0, 0, 0],
+ orientationB=self._init_orientation_inv)
+ self._observation_history.appendleft(self.GetTrueObservation())
+ self._control_observation = self._GetControlObservation()
+ self.last_state_time = self._state_action_counter * self.time_step
+ if self._safety_checker:
+ try:
+ self._safety_checker.check_state()
+ except safety_error.SafetyError as e:
+ logging.info("A safety error occurred: %s", e)
+ self._is_safe = False
+
+ def _GetDelayedObservation(self, latency):
+ """Get observation that is delayed by the amount specified in latency.
+
+ Args:
+ latency: The latency (in seconds) of the delayed observation.
+
+ Returns:
+ observation: The observation which was actually latency seconds ago.
+ """
+ if latency <= 0 or len(self._observation_history) == 1:
+ observation = self._observation_history[0]
+ else:
+ n_steps_ago = int(latency / self.time_step)
+ if n_steps_ago + 1 >= len(self._observation_history):
+ return self._observation_history[-1]
+ remaining_latency = latency - n_steps_ago * self.time_step
+ blend_alpha = remaining_latency / self.time_step
+ observation = (
+ (1.0 - blend_alpha) * np.array(self._observation_history[n_steps_ago])
+ + blend_alpha * np.array(self._observation_history[n_steps_ago + 1]))
+ return observation
+
+ def _GetPDObservation(self):
+ pd_delayed_observation = self._GetDelayedObservation(self._pd_latency)
+ q = pd_delayed_observation[0:self.num_motors]
+ qdot = pd_delayed_observation[self.num_motors:2 * self.num_motors]
+ return (np.array(q), np.array(qdot))
+
+ def _GetControlObservation(self):
+ control_delayed_observation = self._GetDelayedObservation(
+ self._control_latency)
+ return control_delayed_observation
+
+ def _AddSensorNoise(self, sensor_values, noise_stdev):
+ if noise_stdev <= 0:
+ return sensor_values
+ observation = sensor_values + np.random.normal(
+ scale=noise_stdev, size=sensor_values.shape)
+ return observation
+
+ def SetControlLatency(self, latency):
+ """Set the latency of the control loop.
+
+ It measures the duration between sending an action from Nvidia TX2 and
+ receiving the observation from microcontroller.
+
+ Args:
+ latency: The latency (in seconds) of the control loop.
+ """
+ self._control_latency = latency
+
+ def GetControlLatency(self):
+ """Get the control latency.
+
+ Returns:
+ The latency (in seconds) between when the motor command is sent and when
+ the sensor measurements are reported back to the controller.
+ """
+ return self._control_latency
+
+ # TODO(b/73666007): Change the API to SetMotorPGains and SetMotorDGains.
+ def SetMotorGains(self, kp, kd):
+ """Set the gains of all motors.
+
+ These gains are PD gains for motor positional control. kp is the
+ proportional gain and kd is the derivative gain.
+
+ Args:
+ kp: proportional gain(s) of the motors.
+ kd: derivative gain(s) of the motors.
+ """
+ if isinstance(kp, (collections.Sequence, np.ndarray)):
+ self._motor_kps = np.asarray(kp)
+ else:
+ self._motor_kps = np.full(self.num_motors, kp)
+
+ if isinstance(kd, (collections.Sequence, np.ndarray)):
+ self._motor_kds = np.asarray(kd)
+ else:
+ self._motor_kds = np.full(self.num_motors, kd)
+
+ self._motor_model.set_motor_gains(kp, kd)
+
+ # TODO(b/73666007): Change the API to GetMotorPGains and GetMotorDGains.
+ def GetMotorGains(self):
+ """Get the gains of the motor.
+
+ Returns:
+ The proportional gain.
+ The derivative gain.
+ """
+ return self._motor_kps, self._motor_kds
+
+ def GetMotorPositionGains(self):
+ """Get the position gains of the motor.
+
+ Returns:
+ The proportional gain.
+ """
+ return self._motor_kps
+
+ def GetMotorVelocityGains(self):
+ """Get the velocity gains of the motor.
+
+ Returns:
+ The derivative gain.
+ """
+ return self._motor_kds
+
+ def SetMotorStrengthRatio(self, ratio):
+ """Set the strength of all motors relative to the default value.
+
+ Args:
+ ratio: The relative strength. A scalar range from 0.0 to 1.0.
+ """
+ self._motor_model.set_strength_ratios([ratio] * self.num_motors)
+
+ def SetMotorStrengthRatios(self, ratios):
+ """Set the strength of each motor relative to the default value.
+
+ Args:
+ ratios: The relative strength. A numpy array ranging from 0.0 to 1.0.
+ """
+ self._motor_model.set_strength_ratios(ratios)
+
+ def SetTimeSteps(self, action_repeat, simulation_step):
+ """Set the time steps of the control and simulation.
+
+ Args:
+ action_repeat: The number of simulation steps that the same action is
+ repeated.
+ simulation_step: The simulation time step.
+ """
+ self.time_step = simulation_step
+ self._action_repeat = action_repeat
+
+ def _GetMotorNames(self):
+ return MOTOR_NAMES
+
+ def _GetDefaultInitPosition(self):
+ """Returns the init position of the robot.
+
+ It can be either 1) origin (INIT_POSITION), 2) origin with a rack
+ (INIT_RACK_POSITION), or 3) the previous position.
+ """
+ # If we want continuous resetting and is not the first episode.
+ if self._reset_at_current_position and self._observation_history:
+ x, y, _ = self.GetBasePosition()
+ _, _, z = INIT_POSITION
+ return [x, y, z]
+
+ if self._on_rack:
+ return INIT_RACK_POSITION
+ else:
+ return INIT_POSITION
+
+ def _GetDefaultInitOrientation(self):
+ """Returns the init position of the robot.
+
+ It can be either 1) INIT_ORIENTATION or 2) the previous rotation in yaw.
+ """
+ # If we want continuous resetting and is not the first episode.
+ if self._reset_at_current_position and self._observation_history:
+ _, _, yaw = self.GetBaseRollPitchYaw()
+ return self._pybullet_client.getQuaternionFromEuler([0.0, 0.0, yaw])
+ return INIT_ORIENTATION
+
+ @property
+ def chassis_link_ids(self):
+ return self._chassis_link_ids
+
+ def SetAllSensors(self, sensors):
+ """set all sensors to this robot and move the ownership to this robot.
+
+ Args:
+ sensors: a list of sensors to this robot.
+ """
+ for s in sensors:
+ s.set_robot(self)
+ self._sensors = sensors
+
+ def GetAllSensors(self):
+ """get all sensors associated with this robot.
+
+ Returns:
+ sensors: a list of all sensors.
+ """
+ return self._sensors
+
+ def GetSensor(self, name):
+ """get the first sensor with the given name.
+
+ This function return None if a sensor with the given name does not exist.
+
+ Args:
+ name: the name of the sensor we are looking
+
+ Returns:
+ sensor: a sensor with the given name. None if not exists.
+ """
+ for s in self._sensors:
+ if s.get_name() == name:
+ return s
+ return None
+
+ @property
+ def is_safe(self):
+ return self._is_safe
+
+ @property
+ def last_action(self):
+ return self._last_action
+
+ def ProcessAction(self, action, substep_count):
+ """If enabled, interpolates between the current and previous actions.
+
+ Args:
+ action: current action.
+ substep_count: the step count should be between [0, self.__action_repeat).
+
+ Returns:
+ If interpolation is enabled, returns interpolated action depending on
+ the current action repeat substep.
+ """
+ if self._enable_action_interpolation:
+ if self._last_action is not None:
+ prev_action = self._last_action
+ else:
+ prev_action = self.GetMotorAngles()
+
+ lerp = float(substep_count + 1) / self._action_repeat
+ proc_action = prev_action + lerp * (action - prev_action)
+ else:
+ proc_action = action
+
+ return proc_action
+
+ def _BuildActionFilter(self):
+ sampling_rate = 1 / (self.time_step * self._action_repeat)
+ num_joints = self.GetActionDimension()
+ a_filter = action_filter.ActionFilterButter(
+ sampling_rate=sampling_rate, num_joints=num_joints)
+ return a_filter
+
+ def _ResetActionFilter(self):
+ self._action_filter.reset()
+ return
+
+ def _FilterAction(self, action):
+ # initialize the filter history, since resetting the filter will fill
+ # the history with zeros and this can cause sudden movements at the start
+ # of each episode
+ if self._step_counter == 0:
+ default_action = self.GetMotorAngles()
+ self._action_filter.init_history(default_action)
+
+ filtered_action = self._action_filter.filter(action)
+ return filtered_action
+
+ @property
+ def pybullet_client(self):
+ return self._pybullet_client
+
+ @property
+ def joint_states(self):
+ return self._joint_states
+
+ @classmethod
+ def GetConstants(cls):
+ del cls
+ return minitaur_constants
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/robots/minitaur_constants.py b/examples/pybullet/gym/pybullet_envs/minitaur/robots/minitaur_constants.py
new file mode 100644
index 000000000..9a9ed2e69
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/robots/minitaur_constants.py
@@ -0,0 +1,62 @@
+# Lint as: python3
+"""Defines the minitaur robot related constants and URDF specs."""
+
+import collections
+import math
+
+import gin
+
+MINITAUR_URDF_PATH = "quadruped/minitaur_rainbow_dash.urdf"
+
+INIT_POSITION = (0, 0, 0.2)
+INIT_RACK_POSITION = (0, 0, 1)
+INIT_ORIENTATION_QUAT = (0, 0, 0, 1)
+INIT_ORIENTATION_RPY = (0, 0, 0)
+
+NUM_LEGS = 4
+
+JOINT_NAMES = ("motor_front_leftL_joint", "motor_front_leftR_joint",
+ "motor_back_leftL_joint", "motor_back_leftR_joint",
+ "motor_front_rightL_joint", "motor_front_rightR_joint",
+ "motor_back_rightL_joint", "motor_back_rightR_joint")
+
+INIT_JOINT_ANGLES = collections.OrderedDict(
+ zip(JOINT_NAMES, [math.pi / 2, math.pi / 2] * NUM_LEGS))
+
+# Used to convert the robot SDK joint angles to URDF joint angles.
+JOINT_DIRECTIONS = collections.OrderedDict(
+ zip(JOINT_NAMES, (-1, -1, -1, -1, 1, 1, 1, 1)))
+
+# Used to convert the robot SDK joint angles to URDF joint angles.
+JOINT_OFFSETS = collections.OrderedDict(
+ zip(JOINT_NAMES, (0, 0, 0, 0, 0, 0, 0, 0)))
+
+LEG_ORDER = ["front_left", "back_left", "front_right", "back_right"]
+
+END_EFFECTOR_NAMES = (
+ "knee_front_rightR_joint",
+ "knee_front_leftL_joint",
+ "knee_back_rightR_joint",
+ "knee_back_leftL_joint",
+)
+
+MOTOR_NAMES = JOINT_NAMES
+MOTOR_GROUP = collections.OrderedDict((("body_motors", JOINT_NAMES),))
+
+KNEE_CONSTRAINT_POINT_LONG = [0, 0.0045, 0.088]
+KNEE_CONSTRAINT_POINT_SHORT = [0, 0.0045, 0.100]
+
+# Add the gin constants to be used for gin binding in config.
+gin.constant("minitaur_constants.MINITAUR_URDF_PATH", MINITAUR_URDF_PATH)
+gin.constant("minitaur_constants.MINITAUR_INIT_POSITION", INIT_POSITION)
+gin.constant("minitaur_constants.MINITAUR_INIT_ORIENTATION_QUAT",
+ INIT_ORIENTATION_QUAT)
+gin.constant("minitaur_constants.MINITAUR_INIT_ORIENTATION_RPY",
+ INIT_ORIENTATION_RPY)
+gin.constant("minitaur_constants.MINITAUR_INIT_JOINT_ANGLES", INIT_JOINT_ANGLES)
+gin.constant("minitaur_constants.MINITAUR_JOINT_DIRECTIONS", JOINT_DIRECTIONS)
+gin.constant("minitaur_constants.MINITAUR_JOINT_OFFSETS", JOINT_OFFSETS)
+gin.constant("minitaur_constants.MINITAUR_MOTOR_NAMES", MOTOR_NAMES)
+gin.constant("minitaur_constants.MINITAUR_END_EFFECTOR_NAMES",
+ END_EFFECTOR_NAMES)
+gin.constant("minitaur_constants.MINITAUR_MOTOR_GROUP", MOTOR_GROUP)
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/robots/minitaur_motor.py b/examples/pybullet/gym/pybullet_envs/minitaur/robots/minitaur_motor.py
new file mode 100644
index 000000000..2ec0c927e
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/robots/minitaur_motor.py
@@ -0,0 +1,171 @@
+"""This file implements an accurate motor model."""
+
+import numpy as np
+
+from pybullet_envs.minitaur.robots import robot_config
+
+VOLTAGE_CLIPPING = 50
+# TODO(b/73728631): Clamp the pwm signal instead of the OBSERVED_TORQUE_LIMIT.
+OBSERVED_TORQUE_LIMIT = 5.7
+MOTOR_VOLTAGE = 16.0
+MOTOR_RESISTANCE = 0.186
+MOTOR_TORQUE_CONSTANT = 0.0954
+MOTOR_VISCOUS_DAMPING = 0
+MOTOR_SPEED_LIMIT = MOTOR_VOLTAGE / (
+ MOTOR_VISCOUS_DAMPING + MOTOR_TORQUE_CONSTANT)
+NUM_MOTORS = 8
+MOTOR_POS_LB = 0.5
+MOTOR_POS_UB = 2.5
+
+
+class MotorModel(object):
+ """The accurate motor model, which is based on the physics of DC motors.
+
+ The motor model support two types of control: position control and torque
+ control. In position control mode, a desired motor angle is specified, and a
+ torque is computed based on the internal motor model. When the torque control
+ is specified, a pwm signal in the range of [-1.0, 1.0] is converted to the
+ torque.
+
+ The internal motor model takes the following factors into consideration:
+ pd gains, viscous friction, back-EMF voltage and current-torque profile.
+ """
+
+ def __init__(self,
+ kp=1.2,
+ kd=0,
+ torque_limits=None,
+ motor_control_mode=robot_config.MotorControlMode.POSITION):
+ self._kp = kp
+ self._kd = kd
+ self._torque_limits = torque_limits
+ self._motor_control_mode = motor_control_mode
+ self._resistance = MOTOR_RESISTANCE
+ self._voltage = MOTOR_VOLTAGE
+ self._torque_constant = MOTOR_TORQUE_CONSTANT
+ self._viscous_damping = MOTOR_VISCOUS_DAMPING
+ self._current_table = [0, 10, 20, 30, 40, 50, 60]
+ self._torque_table = [0, 1, 1.9, 2.45, 3.0, 3.25, 3.5]
+ self._strength_ratios = [1.0] * NUM_MOTORS
+
+ def set_strength_ratios(self, ratios):
+ """Set the strength of each motors relative to the default value.
+
+ Args:
+ ratios: The relative strength of motor output. A numpy array ranging from
+ 0.0 to 1.0.
+ """
+ self._strength_ratios = np.array(ratios)
+
+ def set_motor_gains(self, kp, kd):
+ """Set the gains of all motors.
+
+ These gains are PD gains for motor positional control. kp is the
+ proportional gain and kd is the derivative gain.
+
+ Args:
+ kp: proportional gain of the motors.
+ kd: derivative gain of the motors.
+ """
+ self._kp = kp
+ self._kd = kd
+
+ def set_voltage(self, voltage):
+ self._voltage = voltage
+
+ def get_voltage(self):
+ return self._voltage
+
+ def set_viscous_damping(self, viscous_damping):
+ self._viscous_damping = viscous_damping
+
+ def get_viscous_dampling(self):
+ return self._viscous_damping
+
+ def convert_to_torque(self,
+ motor_commands,
+ motor_angle,
+ motor_velocity,
+ true_motor_velocity,
+ motor_control_mode=None):
+ """Convert the commands (position control or pwm control) to torque.
+
+ Args:
+ motor_commands: The desired motor angle if the motor is in position
+ control mode. The pwm signal if the motor is in torque control mode.
+ motor_angle: The motor angle observed at the current time step. It is
+ actually the true motor angle observed a few milliseconds ago (pd
+ latency).
+ motor_velocity: The motor velocity observed at the current time step, it
+ is actually the true motor velocity a few milliseconds ago (pd latency).
+ true_motor_velocity: The true motor velocity. The true velocity is used to
+ compute back EMF voltage and viscous damping.
+ motor_control_mode: A MotorControlMode enum.
+
+ Returns:
+ actual_torque: The torque that needs to be applied to the motor.
+ observed_torque: The torque observed by the sensor.
+ """
+ if not motor_control_mode:
+ motor_control_mode = self._motor_control_mode
+
+ if (motor_control_mode is robot_config.MotorControlMode.TORQUE) or (
+ motor_control_mode is robot_config.MotorControlMode.HYBRID):
+ raise ValueError(
+ "{} is not a supported motor control mode".format(motor_control_mode))
+
+ kp = self._kp
+ kd = self._kd
+
+ if motor_control_mode is robot_config.MotorControlMode.PWM:
+ # The following implements a safety controller that softly enforces the
+ # joint angles to remain within safe region: If PD controller targeting
+ # the positive (negative) joint limit outputs a negative (positive)
+ # signal, the corresponding joint violates the joint constraint, so
+ # we should add the PD output to motor_command to bring it back to the
+ # safe region.
+ pd_max = -1 * kp * (motor_angle - MOTOR_POS_UB) - kd / 2. * motor_velocity
+ pd_min = -1 * kp * (motor_angle - MOTOR_POS_LB) - kd / 2. * motor_velocity
+ pwm = motor_commands + np.minimum(pd_max, 0) + np.maximum(pd_min, 0)
+ else:
+ pwm = -1 * kp * (motor_angle - motor_commands) - kd * motor_velocity
+ pwm = np.clip(pwm, -1.0, 1.0)
+ return self._convert_to_torque_from_pwm(pwm, true_motor_velocity)
+
+ def _convert_to_torque_from_pwm(self, pwm, true_motor_velocity):
+ """Convert the pwm signal to torque.
+
+ Args:
+ pwm: The pulse width modulation.
+ true_motor_velocity: The true motor velocity at the current moment. It is
+ used to compute the back EMF voltage and the viscous damping.
+
+ Returns:
+ actual_torque: The torque that needs to be applied to the motor.
+ observed_torque: The torque observed by the sensor.
+ """
+ observed_torque = np.clip(
+ self._torque_constant *
+ (np.asarray(pwm) * self._voltage / self._resistance),
+ -OBSERVED_TORQUE_LIMIT, OBSERVED_TORQUE_LIMIT)
+ if self._torque_limits is not None:
+ observed_torque = np.clip(observed_torque, -1.0 * self._torque_limits,
+ self._torque_limits)
+
+ # Net voltage is clipped at 50V by diodes on the motor controller.
+ voltage_net = np.clip(
+ np.asarray(pwm) * self._voltage -
+ (self._torque_constant + self._viscous_damping) *
+ np.asarray(true_motor_velocity), -VOLTAGE_CLIPPING, VOLTAGE_CLIPPING)
+ current = voltage_net / self._resistance
+ current_sign = np.sign(current)
+ current_magnitude = np.absolute(current)
+ # Saturate torque based on empirical current relation.
+ actual_torque = np.interp(current_magnitude, self._current_table,
+ self._torque_table)
+ actual_torque = np.multiply(current_sign, actual_torque)
+ actual_torque = np.multiply(self._strength_ratios, actual_torque)
+ if self._torque_limits is not None:
+ actual_torque = np.clip(actual_torque, -1.0 * self._torque_limits,
+ self._torque_limits)
+ return actual_torque, observed_torque
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/robots/minitaur_motor_model_v2.py b/examples/pybullet/gym/pybullet_envs/minitaur/robots/minitaur_motor_model_v2.py
new file mode 100644
index 000000000..40927cc7c
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/robots/minitaur_motor_model_v2.py
@@ -0,0 +1,145 @@
+# Lint as: python3
+"""This file implements an accurate motor model."""
+
+from typing import Tuple
+
+import gin
+import numpy as np
+
+from pybullet_envs.minitaur.robots import hybrid_motor_model
+from pybullet_envs.minitaur.robots import robot_config
+
+VOLTAGE_CLIPPING = 50
+# TODO(b/73728631): Clamp the pwm signal instead of the OBSERVED_TORQUE_LIMIT.
+OBSERVED_TORQUE_LIMIT = 5.7
+MOTOR_VOLTAGE = 16.0
+MOTOR_RESISTANCE = 0.186
+MOTOR_TORQUE_CONSTANT = 0.0954
+MOTOR_VISCOUS_DAMPING = 0
+MOTOR_POS_LB = 0.5
+MOTOR_POS_UB = 2.5
+
+
+@gin.configurable
+class MinitaurMotorModel(hybrid_motor_model.HybridMotorModel):
+ """The accurate motor model, which is based on the physics of DC motors.
+
+ The motor model support two types of control: position control and torque
+ control. In position control mode, a desired motor angle is specified, and a
+ torque is computed based on the internal motor model. When the torque control
+ is specified, a pwm signal in the range of [-1.0, 1.0] is converted to the
+ torque.
+
+ The internal motor model takes the following factors into consideration:
+ pd gains, viscous friction, back-EMF voltage and current-torque profile.
+ """
+
+ def __init__(self,
+ num_motors: int,
+ voltage_clipping: float = VOLTAGE_CLIPPING,
+ observed_torque_limit: float = OBSERVED_TORQUE_LIMIT,
+ motor_voltage: float = MOTOR_VOLTAGE,
+ motor_resistance: float = MOTOR_RESISTANCE,
+ motor_torque_constant: float = MOTOR_TORQUE_CONSTANT,
+ motor_viscous_damping: float = MOTOR_VISCOUS_DAMPING,
+ motor_pos_lb: float = MOTOR_POS_LB,
+ motor_pos_ub: float = MOTOR_POS_UB,
+ **kwargs):
+ super(MinitaurMotorModel, self).__init__(num_motors, **kwargs)
+ self._voltage_clipping = voltage_clipping
+ self._observed_torque_limit = observed_torque_limit
+ self._voltage = motor_voltage
+ self._resistance = motor_resistance
+ self._torque_constant = motor_torque_constant
+ self._viscous_damping = motor_viscous_damping
+ self._motor_pos_lb = motor_pos_lb
+ self._motor_pos_ub = motor_pos_ub
+ self._current_table = [0, 10, 20, 30, 40, 50, 60]
+ self._torque_table = [0, 1, 1.9, 2.45, 3.0, 3.25, 3.5]
+
+ def set_voltage(self, voltage):
+ self._voltage = voltage
+
+ def get_voltage(self):
+ return self._voltage
+
+ def set_viscous_damping(self, viscous_damping):
+ self._viscous_damping = viscous_damping
+
+ def get_viscous_dampling(self):
+ return self._viscous_damping
+
+ def get_motor_torques(
+ self,
+ motor_commands: np.ndarray,
+ motor_control_mode=None) -> Tuple[np.ndarray, np.ndarray]:
+ """Convert the commands (position control or pwm control) to torque.
+
+ Args:
+ motor_commands: The desired motor angle if the motor is in position
+ control mode. The pwm signal if the motor is in torque control mode.
+ motor_control_mode: A MotorControlMode enum.
+
+ Returns:
+ actual_torque: The torque that needs to be applied to the motor.
+ observed_torque: The torque observed by the sensor.
+ """
+ if not motor_control_mode:
+ motor_control_mode = self._motor_control_mode
+
+ if (motor_control_mode is robot_config.MotorControlMode.TORQUE) or (
+ motor_control_mode is robot_config.MotorControlMode.HYBRID):
+ raise ValueError(
+ "{} is not a supported motor control mode".format(motor_control_mode))
+
+ motor_angle, motor_velocity = self.get_motor_states()
+ _, true_motor_velocity = self.get_motor_states(latency=0)
+
+ kp = self._kp
+ kd = self._kd
+
+ pwm = -1 * kp * (motor_angle - motor_commands) - kd * motor_velocity
+ pwm = np.clip(pwm, -1.0, 1.0)
+ return self._convert_to_torque_from_pwm(pwm, true_motor_velocity)
+
+ def _convert_to_torque_from_pwm(self, pwm: np.ndarray,
+ true_motor_velocity: np.ndarray):
+ """Convert the pwm signal to torque.
+
+ Args:
+ pwm: The pulse width modulation.
+ true_motor_velocity: The true motor velocity at the current moment. It is
+ used to compute the back EMF voltage and the viscous damping.
+
+ Returns:
+ actual_torque: The torque that needs to be applied to the motor.
+ observed_torque: The torque observed by the sensor.
+ """
+ observed_torque = np.clip(
+ self._torque_constant *
+ (np.asarray(pwm) * self._voltage / self._resistance),
+ -self._observed_torque_limit, self._observed_torque_limit)
+ if (self._torque_lower_limits is not None or
+ self._torque_upper_limits is not None):
+ observed_torque = np.clip(observed_torque, self._torque_lower_limits,
+ self._torque_upper_limits)
+
+ # Net voltage is clipped at 50V by diodes on the motor controller.
+ voltage_net = np.clip(
+ np.asarray(pwm) * self._voltage -
+ (self._torque_constant + self._viscous_damping) *
+ np.asarray(true_motor_velocity), -self._voltage_clipping,
+ self._voltage_clipping)
+ current = voltage_net / self._resistance
+ current_sign = np.sign(current)
+ current_magnitude = np.absolute(current)
+ # Saturate torque based on empirical current relation.
+ actual_torque = np.interp(current_magnitude, self._current_table,
+ self._torque_table)
+ actual_torque = np.multiply(current_sign, actual_torque)
+ actual_torque = np.multiply(self._strength_ratios, actual_torque)
+ if (self._torque_lower_limits is not None or
+ self._torque_upper_limits is not None):
+ actual_torque = np.clip(actual_torque, self._torque_lower_limits,
+ self._torque_upper_limits)
+ return observed_torque, actual_torque
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/robots/minitaur_v2.py b/examples/pybullet/gym/pybullet_envs/minitaur/robots/minitaur_v2.py
new file mode 100644
index 000000000..34d97a50d
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/robots/minitaur_v2.py
@@ -0,0 +1,122 @@
+# Lint as: python3
+"""Pybullet simulation of Minitaur robot."""
+import math
+from typing import Dict, Tuple, Union, Text
+
+from absl import logging
+import gin
+
+from pybullet_envs.minitaur.robots import minitaur_constants
+from pybullet_envs.minitaur.robots import quadruped_base
+from pybullet_envs.minitaur.robots import robot_config
+from pybullet_envs.minitaur.robots import robot_urdf_loader
+
+
+@gin.configurable
+class Minitaur(quadruped_base.QuadrupedBase):
+ """Minitaur simulation model in pyBullet."""
+
+ def _pre_load(self):
+ try:
+ use_constrained_base = gin.query_parameter(
+ "robot_urdf_loader.RobotUrdfLoader.constrained_base")
+ except ValueError:
+ use_constrained_base = False
+ if use_constrained_base:
+ logging.warn(
+ "use_constrained_base is currently not compatible with Minitaur's "
+ "leg constraints."
+ )
+
+ self._urdf_loader = robot_urdf_loader.RobotUrdfLoader(
+ pybullet_client=self._pybullet_client,
+ enable_self_collision=True,
+ urdf_path=minitaur_constants.MINITAUR_URDF_PATH,
+ init_base_position=minitaur_constants.INIT_POSITION,
+ init_base_orientation_quaternion=minitaur_constants
+ .INIT_ORIENTATION_QUAT,
+ init_base_orientation_rpy=minitaur_constants.INIT_ORIENTATION_RPY,
+ init_joint_angles=minitaur_constants.INIT_JOINT_ANGLES,
+ joint_offsets=minitaur_constants.JOINT_OFFSETS,
+ joint_directions=minitaur_constants.JOINT_DIRECTIONS,
+ motor_names=minitaur_constants.MOTOR_NAMES,
+ end_effector_names=minitaur_constants.END_EFFECTOR_NAMES,
+ user_group=minitaur_constants.MOTOR_GROUP,
+ )
+
+ def _on_load(self):
+ """Add hinge constraint for Minitaur's diamond shaped leg after loading."""
+ half_pi = math.pi / 2.0
+ knee_angle = -2.1834
+ for (leg_id, leg_position) in enumerate(minitaur_constants.LEG_ORDER):
+ self._pybullet_client.resetJointState(
+ self._urdf_loader.robot_id,
+ self._joint_id_dict["motor_" + leg_position + "L_joint"],
+ self._motor_directions[2 * leg_id] * half_pi,
+ targetVelocity=0)
+ self._pybullet_client.resetJointState(
+ self._urdf_loader.robot_id,
+ self._joint_id_dict["knee_" + leg_position + "L_joint"],
+ self._motor_directions[2 * leg_id] * knee_angle,
+ targetVelocity=0)
+ self._pybullet_client.resetJointState(
+ self._urdf_loader.robot_id,
+ self._joint_id_dict["motor_" + leg_position + "R_joint"],
+ self._motor_directions[2 * leg_id + 1] * half_pi,
+ targetVelocity=0)
+ self._pybullet_client.resetJointState(
+ self._urdf_loader.robot_id,
+ self._joint_id_dict["knee_" + leg_position + "R_joint"],
+ self._motor_directions[2 * leg_id + 1] * knee_angle,
+ targetVelocity=0)
+
+ if leg_id < 2:
+ self._pybullet_client.createConstraint(
+ self._urdf_loader.robot_id,
+ self._joint_id_dict["knee_" + leg_position + "R_joint"],
+ self._urdf_loader.robot_id,
+ self._joint_id_dict["knee_" + leg_position + "L_joint"],
+ self._pybullet_client.JOINT_POINT2POINT, [0, 0, 0],
+ minitaur_constants.KNEE_CONSTRAINT_POINT_SHORT,
+ minitaur_constants.KNEE_CONSTRAINT_POINT_LONG)
+ else:
+ self._pybullet_client.createConstraint(
+ self._urdf_loader.robot_id,
+ self._joint_id_dict["knee_" + leg_position + "R_joint"],
+ self._urdf_loader.robot_id,
+ self._joint_id_dict["knee_" + leg_position + "L_joint"],
+ self._pybullet_client.JOINT_POINT2POINT, [0, 0, 0],
+ minitaur_constants.KNEE_CONSTRAINT_POINT_LONG,
+ minitaur_constants.KNEE_CONSTRAINT_POINT_SHORT)
+ self.receive_observation()
+
+ def _reset_joint_angles(self,
+ joint_angles: Union[Tuple[float], Dict[Text,
+ float]] = None,
+ num_reset_steps: int = 100):
+ """Resets joint angles of the robot.
+
+ Note that since Minitaur has additional leg constraints on the end
+ effectors, directly setting joint angles will lead to constraint violation.
+ Instead, we apply motor commands to move the motors to the desired position.
+
+ Args:
+ joint_angles: the desired joint angles to reset to.
+ num_reset_steps: number of reset steps.
+ """
+ if joint_angles is None:
+ joint_angles = minitaur_constants.INIT_JOINT_ANGLES
+ actions = joint_angles
+ if isinstance(joint_angles, dict):
+ actions = [
+ joint_angles[joint_name]
+ for joint_name in minitaur_constants.JOINT_NAMES
+ ]
+
+ # TODO(b/157786642): since the simulation clock is not stepped here, this
+ # reset behaves slightly different compared to the old robot class.
+ for _ in range(num_reset_steps):
+ self.apply_action(
+ actions, motor_control_mode=robot_config.MotorControlMode.POSITION)
+ self._pybullet_client.stepSimulation()
+ self.receive_observation()
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/robots/object_controller.py b/examples/pybullet/gym/pybullet_envs/minitaur/robots/object_controller.py
new file mode 100644
index 000000000..2e7dbae81
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/robots/object_controller.py
@@ -0,0 +1,810 @@
+# Lint as: python3
+"""Module for controllers of autonomous objects."""
+
+import abc
+import bisect
+import enum
+from typing import Any, Dict, Optional, Sequence, Text, Tuple, Union
+
+from absl import logging
+import dataclasses
+import gin
+import numpy as np
+
+
+# A constant to be passed into act as parameter t for initial value.
+INIT_TIME = -1.0
+
+# Distance that is deemed close enough in ChaseController.
+_EPS_DISTANCE = 1e-4
+
+ControllerOutput = Tuple[np.ndarray, np.ndarray, Dict[Text, Any]]
+
+ANIMATION_FRAME_NUMBER_KEY = "animation_frame_number"
+
+
+class ControllerBase(metaclass=abc.ABCMeta):
+ """Base class of object controllers.
+
+ Controller is similar to a policy in that its output controls autonomous
+ object just as policy output controls agent. To reflect this similarity,
+ get_action(), the function that "commands" to autonomous object, is named
+ similar to the counterpart in policy.
+ """
+
+ @abc.abstractmethod
+ def get_action(self,
+ time_sec: float,
+ observations: Dict[Text, Any]) -> ControllerOutput:
+ """Returns position, orientation and pose based on time and observations.
+
+ Args:
+ time_sec: Time since simulation reset in seconds. If time < 0, returns
+ initial values and ignores observations.
+ observations: A dict of all observations.
+
+ Returns:
+ Position, orientation and an extra info dict for robot joints, human
+ skeletal pose, etc.
+ """
+
+
+@gin.configurable
+class StationaryController(ControllerBase):
+ """Controller that keeps constant position and orientation."""
+
+ def __init__(self,
+ position: Sequence[float] = None,
+ orientation: Sequence[float] = None):
+ self._position = np.array(position if position is not None else (0, 0, 0))
+ self._orientation = np.array(
+ orientation if orientation is not None else (0, 0, 0, 1))
+
+ def get_action(self,
+ t: float,
+ observations: Dict[Text, Any]) -> ControllerOutput:
+ """Returns constant position orientation."""
+ del t, observations
+ return self._position, self._orientation, {}
+
+
+@gin.configurable
+class CircularMotionController(ControllerBase):
+ """Controller for circular motion.
+
+ The motion trajectory goes around a center in a circle in xy-plane.
+ """
+
+ def __init__(self,
+ center: Sequence[float],
+ radius: float,
+ angular_velocity: float = np.pi,
+ face_travel_direction: bool = False):
+ """Constructor.
+
+ Args:
+ center: Center of circular motion, [x, y, z] in meters.
+ radius: Radius of the circle in meters.
+ angular_velocity: Angular velocity of motion, unit rad/s, e.g. pi means
+ completing a circle in 2 sec.
+ face_travel_direction: If True, object will face direction of motion.
+ """
+
+ self._center = np.array(center)
+ self._radius = radius
+ self._angular_velocity = angular_velocity
+ self._face_travel_direction = face_travel_direction
+
+ def get_action(self,
+ t: float,
+ observations: Dict[Text, Any]) -> ControllerOutput:
+ """Returns position on the circle based on time and constant orientation."""
+ del observations
+
+ t = max(0.0, t)
+ position = np.array(
+ [np.cos(self._angular_velocity * t),
+ np.sin(self._angular_velocity * t),
+ 0]) * self._radius + self._center
+ if self._face_travel_direction:
+ yaw = self._angular_velocity * t + (
+ np.sign(self._angular_velocity) * np.pi / 2)
+ orientation = np.array((0, 0, np.sin(yaw / 2), np.cos(yaw / 2)))
+ else:
+ orientation = np.array((0, 0, 0, 1))
+ return position, orientation, {}
+
+
+class PatrolRepeatMode(enum.Enum):
+ """Enums that defines trajectory repeat mode for patrol type controller."""
+
+ # Trajectory does not repeat. For 3 points a, b, c, the trajectory moves
+ # along a -> b -> c and then stops at c forever.
+ NO_REPEAT = 0
+
+ # Trajectory repeats as a loop. For 3 points a, b, c, the trajectory moves
+ # along a -> b -> c -> a -> b ...
+ LOOP = 1
+
+ # Trajectory repeats back tracking previous point first. For 3 points a, b, c,
+ # the trajectory moves along a -> b -> c -> b -> a -> b -> c ...
+ BACK_TRACK = 2
+
+ # Trajectory repeats by resetting to the initial position after reaching end.
+ # For 3 points a, b, c, the trajectory moves a -> b -> c then immediately
+ # jumps back to a before continue moving along a -> b -> c again.
+ RESET = 3
+
+
+@dataclasses.dataclass
+class PatrolSegmentData:
+ """A data class that describes a patrol segment."""
+
+ # Time in a single cycle to start this segment, range [0, cycle_time).
+ start_time: float
+
+ # Segment start position.
+ start_position: np.ndarray
+
+ # Segment velocity vector.
+ velocity: np.ndarray
+
+ # Orientation quaternion of this segment.
+ orientation: np.ndarray
+
+
+@gin.configurable
+class WayPointPatrolController(ControllerBase):
+ """Controller for patrolling along define waypoints."""
+
+ def __init__(self,
+ points: Sequence[Sequence[float]],
+ yaw_angle: float = 0,
+ face_travel_direction: bool = True,
+ repeat_mode: Union[
+ PatrolRepeatMode, Text] = PatrolRepeatMode.NO_REPEAT,
+ speed_mps: Optional[float] = 1.0,
+ time_points: Optional[Sequence[float]] = None):
+ """Constructor.
+
+ Args:
+ points: List of waypoints, shape Nx3 or Nx2, N is number of points.
+ yaw_angle: Yaw angle of the object in radians.
+ face_travel_direction: If True, yaw angle 'zero' will be redefined to be
+ object's travel direction. Setting yaw_angle to zero with
+ face_travel_direction == True will results in object always facing its
+ travel direction. Non-zero yaw_angle will cause additional yaw offsets.
+ repeat_mode: Behavior of object after reaching the last way point in list.
+ If the value is Text, it is converted to PatrolRepeatMode.
+ speed_mps: Speed in meters per second.
+ time_points: List of times associated with points. These times
+ represent when the object should arrive at the associated waypoint.
+ Optional, but if provided it must have the same length as 'points'.
+ If 'speed_mps' is None, then 'time_points' will be used as-is and there
+ is no maximum segment speed. If 'speed_mps' is also defined, then it
+ serves as a maximum speed value and time points which would result in a
+ segment speed above this value will be altered such that the maximum
+ segment speed is 'speed_mps'.
+ """
+ self._repeat = (repeat_mode if isinstance(repeat_mode, PatrolRepeatMode)
+ else PatrolRepeatMode[repeat_mode])
+ self._yaw_angle = yaw_angle
+ self._face_travel_direction = face_travel_direction
+ self._speed_mps = speed_mps
+
+ if len(points) < 2:
+ raise ValueError(
+ f"Need at least two points in 'points', got {len(points)}")
+
+ if time_points is not None and self._repeat is PatrolRepeatMode.LOOP:
+ raise ValueError("Time points are not compatible with LOOP mode.")
+
+ if (self._repeat is PatrolRepeatMode.NO_REPEAT or
+ self._repeat is PatrolRepeatMode.RESET):
+ augmented_points = points
+
+ if time_points is not None:
+ augmented_time_points = time_points
+ elif self._repeat is PatrolRepeatMode.LOOP:
+ augmented_points = list(points) + [points[0]]
+ elif self._repeat is PatrolRepeatMode.BACK_TRACK:
+ augmented_points = list(points) + list(reversed(points[:-1]))
+
+ if time_points is not None:
+ # Time strictly increases, so add the timepoints again on top
+ # of the last entry. We add the difference between the last time
+ # and the previous elements (in reverse order), on top of the last
+ # element where we left off.
+ augmented_time_points = list(time_points) + \
+ list(time_points[-1] + (time_points[-1] - np.array(time_points[1::-1])))
+ else:
+ raise NotImplementedError(
+ f"Repeat mode {self._repeat} is not supported yet.")
+
+ augmented_points = np.array(augmented_points)
+ # For Nx2 inputs, pad it to Nx3 with default z value of 0.
+ if augmented_points.shape[1] == 2:
+ augmented_points = np.hstack(
+ augmented_points, np.zeros(augmented_points.shape[0], 1))
+ elif augmented_points.shape[1] != 3:
+ raise ValueError("Expect 'points' to be Nx2 or Nx3.")
+
+ t = 0
+ self._segments = []
+
+ if time_points is None:
+ for from_point, to_point in zip(
+ augmented_points[:-1], augmented_points[1:]):
+ segment, t = self._get_patrol_segment_by_speed(from_point, to_point, t)
+ self._segments.append(segment)
+ else:
+ for from_point, to_point, from_time, to_time in zip(
+ augmented_points[:-1], augmented_points[1:],
+ augmented_time_points[:-1], augmented_time_points[1:]):
+ segment, t = self._get_patrol_segment_by_time(from_point, to_point, t,
+ t + (to_time - from_time))
+ self._segments.append(segment)
+
+ self._segment_times = [l.start_time for l in self._segments]
+
+ self._cycle_time = t
+
+ def _get_patrol_segment_by_time(self,
+ from_point,
+ to_point,
+ from_time,
+ to_time):
+ """Returns a PatrolSegmentData for the given points and times."""
+ unit_vector, length = self._get_vector(from_point, to_point)
+ orientation = self._get_orientation(unit_vector)
+
+ if np.isclose(to_time, from_time):
+ speed_mps = 0
+ else:
+ speed_mps = length / (to_time - from_time)
+
+ if self._speed_mps is not None:
+ speed_mps = np.min([self._speed_mps, speed_mps])
+
+ if np.isclose(0, speed_mps):
+ new_to_time = to_time
+ else:
+ new_to_time = np.max([to_time, from_time + (length / speed_mps)])
+
+ segment = PatrolSegmentData(
+ start_time=from_time,
+ start_position=np.array(from_point),
+ velocity=unit_vector * speed_mps,
+ orientation=orientation)
+
+ return segment, new_to_time
+
+ def _get_patrol_segment_by_speed(self, from_point, to_point, current_time):
+ """Returns a PatrolSegmentData for the given points and a constant speed."""
+ unit_vector, length = self._get_vector(from_point, to_point)
+ orientation = self._get_orientation(unit_vector)
+
+ segment = PatrolSegmentData(
+ start_time=current_time,
+ start_position=np.array(from_point),
+ velocity=unit_vector * self._speed_mps,
+ orientation=orientation)
+ time = current_time + length / self._speed_mps
+
+ return segment, time
+
+ def _get_vector(self, from_point, to_point):
+ """Gets the unit vector and length of a from/to point pair."""
+ vector = np.array(to_point) - np.array(from_point)
+ length = np.linalg.norm(vector)
+
+ if length == 0:
+ raise ValueError(f"Length of patrol segment equal to 0, "
+ f"from {from_point} to {to_point}.")
+ unit_vector = vector / length
+
+ return unit_vector, length
+
+ def _get_orientation(self, unit_vector):
+ """Gets the orientation quaternion given a unit vector."""
+ yaw = (np.arctan2(unit_vector[1], unit_vector[0])
+ if self._face_travel_direction else 0) + self._yaw_angle
+
+ orientation = np.array((0, 0, np.sin(yaw / 2), np.cos(yaw / 2)))
+
+ return orientation
+
+ def get_action(self,
+ t: float,
+ observations: Dict[Text, Any]) -> ControllerOutput:
+ """Returns position on, and orientation along the patrol segment."""
+ del observations
+
+ # t < 0 means initial condition, which is the same as the value at t = 0.
+ t = max(0, t)
+
+ if t > self._cycle_time:
+ t = (self._cycle_time if self._repeat is PatrolRepeatMode.NO_REPEAT
+ else np.fmod(t, self._cycle_time))
+
+ segment = self._segments[bisect.bisect_right(self._segment_times, t) - 1]
+ position = (
+ segment.start_position + segment.velocity * (t - segment.start_time))
+
+ return position, segment.orientation.copy(), {}
+
+
+@gin.configurable
+class LinearPatrolController(WayPointPatrolController):
+ """Controller for patrolling along a line segment (back and forth)."""
+
+ def __init__(self,
+ from_point: Sequence[float],
+ to_point: Sequence[float],
+ **kwargs):
+ """Constructor.
+
+ Args:
+ from_point: Starting point of motion, [x, y, z] in meters.
+ to_point: Returning point of motion, [x, y, z] in meters.
+ **kwargs: Keyword arguments to pass onto base class.
+ """
+ super().__init__([from_point, to_point],
+ repeat_mode=PatrolRepeatMode.LOOP,
+ **kwargs)
+
+
+# TODO(b/156126975): migrates this to use difference equation controller.
+@gin.configurable
+class ChaseController(ControllerBase):
+ """Controller for an object to chase another object at certain speed."""
+
+ def __init__(self,
+ self_key: Text,
+ target_key: Text,
+ initial_position: Sequence[float] = (0, 0, 0),
+ initial_orientation: Sequence[float] = (0, 0, 0, 1),
+ speed_mps: float = 1.0,
+ verbose: bool = False):
+ """Constructor.
+
+ Args:
+ self_key: Observation dict key of position of object being controlled.
+ target_key: Observation dict key of position of target object.
+ initial_position: Initial position of the object.
+ initial_orientation: Initial orientation of the object in xyzw quaternion.
+ speed_mps: Speed in meters per second, always positive.
+ verbose: If True, log details of get_action() calculation for debugging.
+ """
+ self._init_position = np.array(initial_position)
+ self._init_orientation = np.array(initial_orientation)
+ self._previous_orientation = self._init_orientation
+ if speed_mps <= 0:
+ raise ValueError(
+ f"'speed_mps' should be a positive value, got {speed_mps}.")
+ self._speed_mps = speed_mps
+
+ self._self_key = self_key
+ self._target_key = target_key
+ self._verbose = verbose
+
+ self._time_sec = 0
+
+ def get_action(self,
+ time_sec: float,
+ observations: Dict[Text, Any]) -> ControllerOutput:
+ """Returns position and orientation of the object being controlled.
+
+ Args:
+ time_sec: Time since simulation reset in seconds. If time < 0, returns
+ initial values and ignores observations.
+ observations: A dict of all observations.
+ """
+ if time_sec < 0:
+ # Initializes internal time.
+ self._time_sec = 0
+ return self._init_position.copy(), self._init_orientation.copy(), {}
+
+ self_position = observations[self._self_key]
+ target_position = observations[self._target_key]
+
+ # Calculates delta vector and projects it to xy-plane.
+ delta_vector = (target_position - self_position) * (1, 1, 0)
+ delta_t = time_sec - self._time_sec
+
+ # Advances internal time.
+ self._time_sec = time_sec
+
+ if self._verbose:
+ with np.printoptions(precision=3, suppress=True):
+ logging.info("t = %.1f, self %s: %s, target %s: %s, v: %s, dt %.1f.",
+ self._t,
+ self._self_key, observations[self._self_key],
+ self._target_key, observations[self._target_key],
+ delta_vector, delta_t)
+
+ # Avoids sigularity when it is close enough. Keeps previous orientation.
+ distance = np.linalg.norm(delta_vector)
+ if distance < _EPS_DISTANCE:
+ return target_position.copy(), self._previous_orientation.copy(), {}
+
+ unit_delta_vector = delta_vector / distance
+ new_position = (unit_delta_vector * min(self._speed_mps * delta_t, distance)
+ + self_position)
+
+ new_yaw = np.arctan2(unit_delta_vector[1], unit_delta_vector[0])
+ new_orientation = np.array((0, 0, np.sin(new_yaw / 2), np.cos(new_yaw / 2)))
+ self._previous_orientation = new_orientation
+
+ return new_position, new_orientation.copy(), {}
+
+
+@gin.configurable
+class AnimationFrameController(ControllerBase):
+ """An extra action controller to control playback of animation sequence."""
+
+ def __init__(self, fps: float = 10.0,
+ pause_between_repeat_sec: float = 0.0,
+ start_time_sec: float = 0.0):
+ """Constructor.
+
+ Args:
+ fps: Frame per second of animation.
+ pause_between_repeat_sec: Pause between repeat in second.
+ start_time_sec: The time when animation starts to play.
+ """
+ self._fps = fps
+ self._total_length = None
+ self._pause_between_repeat_sec = pause_between_repeat_sec
+ self._start_time_sec = start_time_sec
+
+ def set_total_length(self, total_length: int):
+ """Sets total animation frame length."""
+ if total_length <= 0:
+ raise ValueError(
+ f"Total number of frame must be >= 0, got {total_length}.")
+ self._total_length = total_length
+
+ def get_action(self,
+ time_sec: float,
+ observations: Dict[Text, Any]) -> ControllerOutput:
+ """Returns animation frame number with default position and orientation.
+
+ Args:
+ time_sec: Time since simulation reset in seconds. If time < 0, returns
+ initial values and ignores observations.
+ observations: A dict of all observations.
+ """
+ time_sec = max(0, time_sec - self._start_time_sec)
+ frame = int(time_sec * self._fps)
+ if self._total_length:
+ frame = frame % (
+ self._total_length + int(self._pause_between_repeat_sec * self._fps))
+ frame = min(frame, self._total_length - 1)
+ return np.ndarray((0, 0, 0)), np.ndarray((0, 0, 0, 1)), {
+ ANIMATION_FRAME_NUMBER_KEY: frame}
+
+
+@gin.configurable
+class ConversationController(ControllerBase):
+ """Controller for an object that mimics conversational behavior.
+
+ A controlled object is arrayed in a conversation about a center point.
+ When a target object reaches a thresholded distance away from the center,
+ the controlled object will face the target object and move away from
+ the target's intended path along an orthogonal direction vector until it
+ passes.
+ """
+
+ def __init__(self,
+ self_key: Text,
+ target_key: Text,
+ position: Sequence[float] = None,
+ orientation: Sequence[float] = None,
+ conversation_center: Sequence[float] = None,
+ proximity_threshold: float = 1.0,
+ speed_mps: float = 0.1):
+ """Constructor.
+
+ Args:
+ self_key: Observation dict key of position of object being controlled.
+ target_key: Observation dict key of position of target object.
+ position: Initial position of the object.
+ orientation: Initial orientation of the object in xyzw quaternion.
+ conversation_center: Position of the center of the conversation group.
+ proximity_threshold: The distance from the conversation center that the
+ target must reach in order to prompt a response from the controlled
+ object.
+ speed_mps: Speed in meters per second, always positive.
+ """
+ self._self_key = self_key
+ self._target_key = target_key
+ self._position = np.array(position or (0, 0, 0))
+ self._orientation = np.array(orientation or (0, 0, 0, 1))
+ self._conversation_center = np.array(conversation_center or (0, 0, 0))
+ self._proximity_threshold = proximity_threshold
+ self._speed_mps = speed_mps
+
+ self._prev_target_position = None
+ self._wait_position = None
+
+ def _get_wait_position(self, self_position, target_position):
+ """Gets the waiting position for the controlled object.
+
+ This returns the position that the controlled object should move towards
+ to create physical space such that the target object may pass through
+ the conversation space.
+
+ Args:
+ self_position: The current position of the controlled object.
+ target_position: The current position of the target object.
+
+ Returns:
+ An xyz position representing the waiting position that the controlled
+ object should move towards to create space.
+ """
+ # Find an orthogonal projection to the target's path
+ target_path = np.array(target_position - self._prev_target_position)
+ self_path = np.array(self_position - self._prev_target_position)
+
+ unit_target_path = target_path / np.linalg.norm(target_path)
+
+ projected_point = np.dot(self_path, unit_target_path) * unit_target_path
+ projected_point += self._prev_target_position
+
+ # Get the position that lies along the orthogonal projection vector
+ # but in the opposite direction from the target's path and exactly
+ # proximity_threshold distance away.
+ return self._get_position(
+ projected_point,
+ self_position,
+ self._proximity_threshold,
+ self._proximity_threshold)
+
+ def _get_orientation(self, source_position, target_position):
+ """Gets orientation required to face target_position from source_position.
+
+ Args:
+ source_position: The source position where an object would be located.
+ target_position: The target position that an object should face.
+
+ Returns:
+ A xyzw quaternion indicating the orientation.
+ """
+ if np.allclose(source_position, target_position):
+ return self._orientation
+
+ delta_vector = (target_position - source_position) * (1, 1, 0)
+
+ new_yaw = np.arctan2(delta_vector[1], delta_vector[0])
+ new_orientation = np.array(
+ (0, 0, np.sin(new_yaw / 2), np.cos(new_yaw / 2)))
+
+ return new_orientation
+
+ def _get_position(self,
+ source_position,
+ target_position,
+ min_delta,
+ max_delta):
+ """Gets the next position along the vector from source to target.
+
+ This returns the position that should be moved to next which lies along
+ the direction vector from source -> target with a minimum length of
+ min_delta and a maximum distance of max_delta.
+
+ Args:
+ source_position: The current position of the controlled object.
+ target_position: The target position to move to.
+ min_delta: The minimum amount of distance to move.
+ max_delta: The maximum amount of distance to move.
+
+ Returns:
+ An xyz position representing the next position to move to.
+ """
+ delta_vector = (target_position - source_position) * (1, 1, 0)
+ distance = np.linalg.norm(delta_vector)
+
+ # If the distance to the target is greater than the maximum step delta,
+ # then normalize the vector and set it to the max step delta.
+ if distance > max_delta:
+ delta_vector = (delta_vector / distance) * max_delta
+ # If the distance is less than the minimum step delta, then normalize
+ # the vector and set it to the min step delta.
+ elif distance < min_delta:
+ delta_vector = (delta_vector / distance) * min_delta
+
+ new_position = (delta_vector + source_position)
+
+ return new_position
+
+ def _get_target_distance_to_center(self, target_position):
+ """Gets the distance from the target to the conversation center point.
+
+ Args:
+ target_position: The target position.
+
+ Returns:
+ The scalar distance from the target position to the conversation center.
+ """
+ # Calculates delta vector and projects it to xy-plane.
+ delta_vector = (target_position - self._conversation_center) * (1, 1, 0)
+
+ # Compute the length of the delta vector.
+ return np.linalg.norm(delta_vector)
+
+ def get_action(self,
+ t: float,
+ observations: Dict[Text, Any]) -> ControllerOutput:
+ """Gets the position and orientation of the controlled object.
+
+ Args:
+ t: The current time step.
+ observations: Dict containing sensor observations for current time step.
+
+ Returns:
+ The new position and orientation for the controlled object.
+ """
+
+ position = self._position
+ orientation = self._orientation
+
+ # Observations are only available for positive time steps.
+ if t >= 0:
+ self_position = observations[self._self_key]
+ target_position = observations[self._target_key]
+
+ target_distance = self._get_target_distance_to_center(target_position)
+
+ # Check if the target is within the threshold distance of the
+ # conversation center.
+ if(target_distance < self._proximity_threshold and
+ self._prev_target_position is not None):
+
+ # If it is, get the position that the controlled object should move to
+ # in order to create space and get the resulting action/orientation.
+ wait_position = self._get_wait_position(self_position, target_position)
+
+ orientation = self._get_orientation(
+ self_position,
+ target_position)
+
+ position = self._get_position(
+ self_position,
+ wait_position,
+ 0.0,
+ self._speed_mps)
+ else:
+ # Otherwise, get the position/orientation required to move back to
+ # the original position and face the conversation center.
+ orientation = self._get_orientation(
+ self_position,
+ self._conversation_center)
+
+ position = self._get_position(
+ self_position,
+ self._position,
+ 0.0,
+ self._speed_mps)
+
+ self._prev_target_position = target_position
+
+ # Only return a new position along the x/y axis, z should be unaffected.
+ position = np.array([position[0], position[1], self._position[2]])
+ return position, orientation, {}
+
+
+@gin.configurable
+class PauseIfCloseByWrapper(ControllerBase):
+ """A controller wrapper that pauses controller if object is close to others.
+
+ This wrapper works best if the underlying controller is time based. It is
+ intended to be a simple way to stop agent when blocked and is not for
+ reliable collision avoidance.
+ """
+
+ _DEFAULT_PAUSE_DISTANCE_M = 1.0
+
+ def __init__(
+ self,
+ wrapped_controller: ControllerBase,
+ self_pos_key: Text,
+ others_pos_keys: Sequence[Text],
+ pause_distance: Union[float, Sequence[float]] = _DEFAULT_PAUSE_DISTANCE_M,
+ self_yaw_key: Optional[Text] = None,
+ active_front_sector: Optional[float] = None):
+ """Constructor.
+
+ Args:
+ wrapped_controller: The controller being wrapped.
+ self_pos_key: Observation key of self position.
+ others_pos_keys: Observation keys of others' positions.
+ pause_distance: The distance limit before the controller pauses in meters.
+ Can be a float value which applies to all objects specified in
+ others_pos_keys or a Sequence of float values with the same length
+ as others_pos_keys denoting the pause distance for each individual
+ object in the same order in others_pos_keys. Default pause distance is
+ one meter.
+ self_yaw_key: Observation key of self yaw. Required if active_front_sector
+ is specified.
+ active_front_sector: If specified, it defines pie-shaped active region in
+ front of controlled object. The pie-shaped area is symmetric about the
+ forward direction of controlled object with it angle defined by this
+ arg in radians, Only when other objects shows up in this region and
+ pause distance requirement is met, pause is actived.
+ """
+
+ self._controller = wrapped_controller
+ self._pause_start_t = -1
+ self._shift_t = 0
+ self._last_action = None
+ self._self_pos_key = self_pos_key
+ self._others_pos_keys = list(others_pos_keys) # Make a copy.
+
+ if isinstance(pause_distance, float):
+ pause_distance = [pause_distance] * len(others_pos_keys)
+
+ if len(pause_distance) != len(others_pos_keys):
+ raise ValueError(
+ "pause_distance and others_pos_keys must have the same length.")
+ self._pause_distance = list(pause_distance) # Make a copy.
+
+ if active_front_sector is not None and self_yaw_key is None:
+ raise ValueError(
+ "self_yaw_key must be specified if active_front_sector is specified.")
+
+ self._self_yaw_key = self_yaw_key
+ self._active_front_sector = active_front_sector
+
+ def get_action(self,
+ t: float,
+ observations: Dict[Text, Any]) -> ControllerOutput:
+
+ """Gets the position and orientation of the controlled object.
+
+ Args:
+ t: The current time step.
+ observations: Dict containing sensor observations for current time step.
+
+ Returns:
+ The new position and orientation for the controlled object.
+ """
+ if t < 0:
+ self._pause_start_t = -1
+ self._shift_t = 0
+ self._last_action = self._controller.get_action(
+ t, observations)
+ return self._last_action
+
+ if self._should_pause(observations):
+ # Only record the start time of pause.
+ if self._pause_start_t < 0:
+ self._pause_start_t = t
+ return self._last_action
+ else:
+ if self._pause_start_t >= 0:
+ self._shift_t += t - self._pause_start_t
+ self._pause_start_t = -1
+
+ self._last_action = self._controller.get_action(
+ t - self._shift_t, observations)
+ return self._last_action
+
+ def _should_pause(self, observations) -> bool:
+ """Determines whether the controller should pause."""
+ self_position_2d = observations[self._self_pos_key][:2]
+ for pos_key, pause_distance in zip(
+ self._others_pos_keys, self._pause_distance):
+ position_2d = observations[pos_key][:2]
+ vector_2d = position_2d - self_position_2d
+ distance = np.linalg.norm(vector_2d)
+
+ if self._active_front_sector is None:
+ return distance <= pause_distance
+ else:
+ yaw = observations[self._self_yaw_key][0]
+ dot = np.dot(vector_2d / distance, np.array([np.cos(yaw), np.sin(yaw)]))
+ return (distance <= pause_distance and
+ np.arccos(dot) < self._active_front_sector / 2)
+
+ return False
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/robots/quadruped_base.py b/examples/pybullet/gym/pybullet_envs/minitaur/robots/quadruped_base.py
new file mode 100644
index 000000000..960c278ed
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/robots/quadruped_base.py
@@ -0,0 +1,724 @@
+# Lint as: python3
+"""The base class for all quadrupeds."""
+from typing import Any, Callable, Dict, Sequence, Tuple, Text, Union
+import gin
+import gym
+import numpy as np
+
+from pybullet_utils import bullet_client
+from pybullet_envs.minitaur.envs_v2.sensors import sensor as sensor_lib
+from pybullet_envs.minitaur.robots import hybrid_motor_model
+from pybullet_envs.minitaur.robots import robot_base
+from pybullet_envs.minitaur.robots import robot_config
+from pybullet_envs.minitaur.robots import robot_urdf_loader
+from pybullet_envs.minitaur.robots.safety import data_types as safety_data_types
+from pybullet_envs.minitaur.robots.utilities import kinematics_utils
+
+_UNIT_QUATERNION = (0, 0, 0, 1)
+_GRAVITY_ACCELERATION_OFFSET = (0, 0, 10)
+
+
+@gin.configurable
+class QuadrupedBase(robot_base.RobotBase):
+ """The basic quadruped class for both sim and real robots."""
+
+ def __init__(
+ self,
+ pybullet_client: bullet_client.BulletClient,
+ clock: Callable[..., float],
+ motor_control_mode: robot_config.MotorControlMode,
+ motor_limits: robot_config.MotorLimits,
+ motor_model_class: Any = hybrid_motor_model.HybridMotorModel,
+ action_filter: Any = None,
+ sensors: Sequence[sensor_lib.Sensor] = (),
+ safety_config: safety_data_types.SafetyConfig = None,
+ **kwargs,
+ ):
+ """Initializes the class.
+
+ Args:
+ pybullet_client: The PyBullet client.
+ clock: The sim or real clock. The clock function is typically provided by
+ the gym environment.
+ motor_control_mode: Specifies in which mode the motor operates.
+ motor_limits: The motor limits of the robot. Used by the motor_model_class
+ and action space building.
+ motor_model_class: The motor model to use. Not needed for real robots.
+ action_filter: The filter to smooth and/or regulate the actions.
+ sensors: All sensors mounted on the robot.
+ safety_config: The safety setting for the robot.
+ **kwargs: Additional args.
+ """
+
+ self._pybullet_client = pybullet_client
+ self._clock = clock
+ self._motor_control_mode = motor_control_mode
+ self._motor_model_class = motor_model_class
+ self._motor_limits = motor_limits
+ self._action_space = None
+ self._action_names = None
+ self._action_filter = action_filter
+ self._sensors = sensors
+ self._safety_config = safety_config
+ self._urdf_loader = None
+ self._last_base_velocity = np.zeros(3)
+ self._last_observation_time = self._clock()
+ self._last_base_acceleration_world = np.zeros(3)
+ self._last_base_acceleration_accelerometer = np.zeros(3)
+
+ self.load()
+
+ def load(
+ self,
+ base_position: Tuple[float] = None,
+ base_orientation_quaternion: Tuple[float] = None,
+ joint_angles: Union[Dict[Text, float], Tuple[float]] = None,
+ ):
+ """Loads the URDF with the configured pose.
+
+ Args:
+ base_position: The base position after URDF loading. Will use the
+ configured pose in gin if None.
+ base_orientation_quaternion: The base orientation after URDF loading. Will
+ use the configured values in gin if not specified.
+ joint_angles: The desired joint angles after loading. Will use the
+ configured values if None.
+ """
+ # A robot specific pre loading routing.
+ self._pre_load()
+
+ if not self._urdf_loader:
+ self._urdf_loader = robot_urdf_loader.RobotUrdfLoader(
+ pybullet_client=self._pybullet_client)
+
+ # Record the urdf pose at loading, which will be used as the rotation
+ # reference for base rotation computation.
+ self._init_urdf_position, self._init_orientation_quat = (
+ self._pybullet_client.getBasePositionAndOrientation(
+ self._urdf_loader.robot_id))
+ unused_position, self._init_orientation_inv_quat = (
+ self._pybullet_client.invertTransform(
+ position=(0, 0, 0), orientation=self._init_orientation_quat))
+
+ # Joint ids may be different from the motor ids.
+ self._joint_id_dict = self._urdf_loader.get_joint_id_dict()
+ for joint_id in self._joint_id_dict.values():
+ # Disables the default motors in PyBullet.
+ self._pybullet_client.setJointMotorControl2(
+ bodyIndex=self._urdf_loader.robot_id,
+ jointIndex=joint_id,
+ controlMode=self._pybullet_client.VELOCITY_CONTROL,
+ targetVelocity=0,
+ force=0)
+ # Removes the default joint damping in PyBullet.
+ self._pybullet_client.changeDynamics(
+ self._urdf_loader.robot_id,
+ joint_id,
+ linearDamping=0,
+ angularDamping=0)
+
+ # We expect that this is non-empty for all quadrupedes, and should be an
+ # OrderedDict.
+ self._motor_id_dict = self._urdf_loader.get_motor_id_dict()
+ if not self._motor_id_dict:
+ raise ValueError("Motor id dict cannot be empty for quadrupeds.")
+ self._motor_ids = self._motor_id_dict.values()
+ self._num_motors = len(self._motor_id_dict)
+
+ self._build_action_space()
+
+ # Not needed for real robots.
+ if self._motor_model_class:
+ # TODO(b/151664871): Also supports position/velocity limits in the motor
+ # model.
+ self._motor_model = self._motor_model_class(
+ num_motors=self._num_motors,
+ motor_control_mode=self._motor_control_mode,
+ torque_lower_limits=self._motor_limits.torque_lower_limits,
+ torque_upper_limits=self._motor_limits.torque_upper_limits,
+ )
+
+ # Caches the variable for faster computation during stepping.
+ self._motor_direction_dict = self._urdf_loader.get_joint_direction_dict(
+ self._motor_id_dict.keys())
+ self._motor_directions = np.array(list(self._motor_direction_dict.values()))
+
+ self._motor_offset_dict = self._urdf_loader.get_joint_offset_dict(
+ self._motor_id_dict.keys())
+ self._motor_offsets = np.array(list(self._motor_offset_dict.values()))
+
+ # A robot specific routine post loading.
+ self._on_load()
+
+ # Robot sensors may use information from the class. So we initialize them
+ # after the loading is done.
+ for sensor in self._sensors:
+ sensor.set_robot(self)
+
+ def _build_action_space(self):
+ """Builds the action space of the robot using the motor limits."""
+ if self._motor_control_mode == robot_config.MotorControlMode.POSITION:
+ self._action_space = gym.spaces.Box(
+ low=self._motor_limits.angle_lower_limits,
+ high=self._motor_limits.angle_upper_limits,
+ shape=(self._num_motors,),
+ dtype=np.float32) # TODO(b/159160184) Make dtype configurable.
+ self._action_names = tuple(
+ "POSITION_{}".format(motor) for motor in self._motor_id_dict.keys())
+ elif self._motor_control_mode == robot_config.MotorControlMode.TORQUE:
+ self._action_space = gym.spaces.Box(
+ low=self._motor_limits.torque_lower_limits,
+ high=self._motor_limits.torque_upper_limits,
+ shape=(self._num_motors,),
+ dtype=np.float32)
+ self._action_names = tuple(
+ "TORQUE_{}".format(motor) for motor in self._motor_id_dict.keys())
+ elif self._motor_control_mode == robot_config.MotorControlMode.HYBRID:
+ hybrid_action_limits_low = [
+ self._motor_limits.angle_lower_limits, # q
+ # q_dot
+ self._motor_limits.velocity_lower_limits,
+ 0, # kp
+ 0, # kd
+ self._motor_limits.torque_lower_limits
+ ] # tau
+ hybrid_action_limits_high = [
+ self._motor_limits.angle_upper_limits,
+ self._motor_limits.velocity_upper_limits, np.inf, np.inf,
+ self._motor_limits.torque_upper_limits
+ ]
+ space_low = np.full(
+ (self._num_motors, robot_config.HYBRID_ACTION_DIMENSION),
+ hybrid_action_limits_low).ravel()
+ space_high = np.full(
+ (self._num_motors, robot_config.HYBRID_ACTION_DIMENSION),
+ hybrid_action_limits_high).ravel()
+ self._action_space = gym.spaces.Box(
+ low=space_low, high=space_high, dtype=np.float32)
+ self._action_names = tuple(
+ "HYBRID_{}".format(motor) for motor in self._motor_id_dict.keys())
+ else:
+ raise NotImplementedError("Not yet implemented!")
+
+ def _pre_load(self):
+ """Robot specific pre load routine.
+
+ For example, this allows configuration of the URDF loader.
+ """
+ pass
+
+ def _on_load(self):
+ """Robot specific post load routine.
+
+ For example, we need to add add additional hinge constraints to the leg
+ components of Minitaur after loading.
+
+ """
+ pass
+
+ @gin.configurable
+ def reset(
+ self,
+ base_position: Tuple[float] = None,
+ base_orientation_quaternion: Tuple[float] = None,
+ joint_angles: Union[Dict[Text, float], Tuple[float]] = None,
+ save_base_pose: bool = False,
+ **kwargs,
+ ):
+ """Resets the robot base and joint pose without reloading the URDF.
+
+ Base pose resetting only works for simulated robots or visualization of real
+ robots. This routine also updates the initial observation dict.
+
+ Args:
+ base_position: The desired base position. Will use the configured pose in
+ gin if None. Does not affect the position of the real robots in general.
+ base_orientation_quaternion: The base orientation after resetting. Will
+ use the configured values in gin if not specified.
+ joint_angles: The desired joint angles after resetting. Will use the
+ configured values if None.
+ save_base_pose: Save the base position and orientation as the default pose
+ after resetting.
+ **kwargs: Other args for backward compatibility. TODO(b/151975607): Remove
+ after migration.
+ """
+ # Reset the robot's motor model.
+ self._motor_model.reset()
+
+ # Reset the quantities for computing base acceleration.
+ self._last_base_velocity = np.zeros(3)
+ self._last_observation_time = self._clock()
+ self._last_base_acceleration_world = np.zeros(3)
+ self._last_base_acceleration_accelerometer = np.zeros(3)
+
+ # Solves chicken and egg problem. We need to run a control step to obtain
+ # the first motor torques.
+ self._motor_torques = np.zeros(self._num_motors)
+
+ # Receives a set of observation from the robot in case the reset function
+ # needs to use them.
+ self.receive_observation()
+
+ self._reset_base_pose(base_position, base_orientation_quaternion)
+ self._reset_joint_angles(joint_angles)
+
+ if save_base_pose:
+ # Records the base pose at resetting again, in case Reset is called with a
+ # different base orientation. This base pose will be used as zero
+ # rotation reference for base rotation computation.
+ self._init_urdf_position, self._init_orientation_quat = (
+ self._pybullet_client.getBasePositionAndOrientation(
+ self._urdf_loader.robot_id))
+ unused_position, self._init_orientation_inv_quat = (
+ self._pybullet_client.invertTransform(
+ position=(0, 0, 0), orientation=self._init_orientation_quat))
+
+ # Updates the observation at the end of resetting.
+ self.receive_observation()
+ self._time_at_reset = self._clock()
+
+ def GetTimeSinceReset(self):
+ return self._clock() - self._time_at_reset
+
+ def _reset_base_pose(self, position=None, orientation_quat=None):
+ """Resets the pose of the robot's base.
+
+ Base pose resetting only works for simulated robots or visualization of real
+ robots.
+
+ Args:
+ position: The desired base position. Will use the configured pose in gin
+ if None.
+ orientation_quat: The desired base rotation. Will use the configured
+ default pose in None.
+ """
+ self._urdf_loader.reset_base_pose(position, orientation_quat)
+
+ def _reset_joint_angles(self,
+ joint_angles: Union[Tuple[float],
+ Dict[Text, float]] = None):
+ """Resets the joint pose.
+
+ Real robots need to specify their routine to send joint angles. Simulated
+ Minitaur robots also needs to use dynamics to drive the motor joints, due to
+ the additional hinge joints not present in the URDF.
+
+ Args:
+ joint_angles: The joint pose if provided. Will use the robot default pose
+ from configuration.
+ """
+ # TODO(b/148897311): Supports tuple as the input.
+ self._urdf_loader.reset_joint_angles(joint_angles)
+
+ def terminate(self):
+ """The safe exit routine for the robot.
+
+ Only implemented for real robots.
+
+ """
+ pass
+
+ def step(self, action: Any, num_sub_steps: int = 1):
+ """Steps the simulation.
+
+ This is maintained for backward compatibility with the old robot class.
+
+ Args:
+ action: The control command to be executed by the robot.
+ num_sub_steps: Each action can be applied (possibly with interpolation)
+ multiple timesteps, to simulate the elapsed time between two consecutive
+ commands on real robots.
+ """
+ action = self.pre_control_step(action)
+
+ for _ in range(num_sub_steps):
+ # TODO(b/149252003): Add sub sampling.
+ self.apply_action(action)
+ # Timestep is pre-determined at simulation setup.
+ self._pybullet_client.stepSimulation()
+ self.receive_observation()
+
+ self.post_control_step()
+
+ def pre_control_step(self, action: Any, control_timestep: float = None):
+ """Processes the action and updates per control step quantities.
+
+ Args:
+ action: The input control command.
+ control_timestep: The control time step in the environment.
+ TODO(b/153835005), we can remove this once we pass env to the robot.
+
+ Returns:
+ The filtered action.
+ """
+ if self._action_filter:
+ # We assume the filter will create a set of interpolated results.
+ action = self._action_filter.filter(action)
+ return action
+
+ def apply_action(self, motor_commands, motor_control_mode=None):
+
+ # TODO(b/148897311): Supports dict in the future.
+ motor_commands = np.asarray(motor_commands)
+
+ # We always use torque based control at the lowest level for quadrupeds.
+ unused_observed_torques, actual_torques = (
+ self._motor_model.get_motor_torques(motor_commands, motor_control_mode))
+ self._motor_torques = actual_torques
+
+ # Converts the motor torques to URDF joint space, which may have different
+ # directions.
+ applied_motor_torques = np.multiply(actual_torques, self._motor_directions)
+
+ self._pybullet_client.setJointMotorControlArray(
+ bodyIndex=self._urdf_loader.robot_id,
+ jointIndices=self._motor_ids,
+ controlMode=self._pybullet_client.TORQUE_CONTROL,
+ forces=applied_motor_torques)
+
+ def _get_base_roll_pitch_yaw_rate(self):
+ _, angular_velocity = self._pybullet_client.getBaseVelocity(
+ self._urdf_loader.robot_id)
+ return kinematics_utils.rotate_to_base_frame(
+ self._pybullet_client, self.urdf_loader.robot_id, angular_velocity,
+ self._init_orientation_inv_quat)
+
+ def _get_base_velocity(self):
+ base_velocity, _ = self._pybullet_client.getBaseVelocity(
+ self._urdf_loader.robot_id)
+ return base_velocity
+
+ def _update_base_acceleration(self):
+ """Update the base acceleration using finite difference."""
+ if self._last_observation_time < self.timestamp:
+ self._last_base_acceleration_world = (
+ np.array(self._base_velocity) - self._last_base_velocity) / (
+ self.timestamp - self._last_observation_time)
+ _, inv_base_orientation = self.pybullet_client.invertTransform(
+ np.zeros(3), np.array(self.base_orientation_quaternion))
+
+ # An offset is added to the acceleration measured in the world frame
+ # because the accelerometer reading is in the frame of free-falling robot.
+ base_acceleration_accelerometer = self.pybullet_client.multiplyTransforms(
+ np.zeros(3), inv_base_orientation,
+ self._last_base_acceleration_world + _GRAVITY_ACCELERATION_OFFSET,
+ _UNIT_QUATERNION)[0]
+ self._last_base_acceleration_accelerometer = np.array(
+ base_acceleration_accelerometer)
+
+ def receive_observation(self):
+ """Receives the observations for all sensors."""
+ # Update the intrinsic values including the joint angles, joint
+ # velocities, and imu readings.
+ self._base_position, base_orientation_quat = (
+ self._pybullet_client.getBasePositionAndOrientation(
+ self._urdf_loader.robot_id))
+ _, self._base_orientation_quat = self._pybullet_client.multiplyTransforms(
+ positionA=(0, 0, 0),
+ orientationA=self._init_orientation_inv_quat,
+ positionB=(0, 0, 0),
+ orientationB=base_orientation_quat)
+ self._base_velocity = self._get_base_velocity()
+ self._base_roll_pitch_yaw = self._pybullet_client.getEulerFromQuaternion(
+ self._base_orientation_quat)
+
+ self._base_roll_pitch_yaw_rate = self._get_base_roll_pitch_yaw_rate()
+
+ self._joint_states = self._pybullet_client.getJointStates(
+ self._urdf_loader.robot_id, self._motor_ids)
+ self._motor_angles = np.array(
+ [joint_state[0] for joint_state in self._joint_states])
+ self._motor_angles = (self._motor_angles -
+ self._motor_offsets) * self._motor_directions
+
+ self._motor_velocities = np.array(
+ [joint_state[1] for joint_state in self._joint_states])
+ self._motor_velocities = self._motor_velocities * self._motor_directions
+
+ # We use motor models to track the delayed motor positions and velocities
+ # buffer.
+ if self._motor_model:
+ self._motor_model.update(self._clock(), self._motor_angles,
+ self._motor_velocities)
+
+ self._update_base_acceleration()
+ # Update the latest base velocity and timestamp at the end of the API.
+ self._last_base_velocity = np.array(self._base_velocity)
+ self._last_observation_time = self.timestamp
+
+ def post_control_step(self):
+ """Called at the end of a control step outside the action repeat loop."""
+ pass
+
+ # TODO(tingnan): Change from "foot_positions" to "feet_positions".
+ def motor_angles_from_foot_positions(self,
+ foot_positions,
+ position_in_world_frame=False):
+ """Use IK to compute the motor angles, given the feet links' positions.
+
+ Args:
+ foot_positions: The foot links' positions in frame specified by the next
+ parameter. The input is a numpy array of size (4, 3).
+ position_in_world_frame: Whether the foot_positions are specified in the
+ world frame.
+
+ Returns:
+ A tuple. The position indices and the angles for all joints along the
+ leg. The position indices is consistent with the joint orders as returned
+ by GetMotorAngles API.
+ """
+ joint_position_idxs = np.arange(self.num_motors)
+ foot_link_ids = tuple(self._urdf_loader.get_end_effector_id_dict().values())
+ joint_angles = kinematics_utils.joint_angles_from_link_positions(
+ pybullet_client=self.pybullet_client,
+ urdf_id=self.robot_id,
+ link_positions=foot_positions,
+ link_ids=foot_link_ids,
+ joint_dof_ids=joint_position_idxs,
+ positions_are_in_world_frame=position_in_world_frame)
+ joint_angles = np.multiply(
+ np.asarray(joint_angles) - np.asarray(self._motor_offsets),
+ self._motor_directions)
+ return joint_position_idxs, joint_angles
+
+ # TODO(tingnan): Change from "foot_positions" to "feet_positions".
+ def foot_positions(self, position_in_world_frame=False):
+ """Returns the robot's foot positions in the base/world frame."""
+ foot_positions = []
+ foot_link_ids = tuple(self._urdf_loader.get_end_effector_id_dict().values())
+ for foot_id in foot_link_ids:
+ if not position_in_world_frame:
+ foot_positions.append(
+ kinematics_utils.link_position_in_base_frame(
+ pybullet_client=self.pybullet_client,
+ urdf_id=self.robot_id,
+ link_id=foot_id,
+ ))
+ else:
+ foot_positions.append(
+ kinematics_utils.link_position_in_world_frame(
+ pybullet_client=self.pybullet_client,
+ urdf_id=self.robot_id,
+ link_id=foot_id,
+ ))
+ return np.array(foot_positions)
+
+ def feet_contact_forces(self) -> Sequence[np.ndarray]:
+ """Gets the contact forces on all feet.
+
+ Reals robot may use a robot specific implementation. For example, the
+ Laikago will measure each contact force in the corresponding foot's local
+ frame, and this force will not be the total contact force due to the sensor
+ limitation.
+
+ For simulated robots, we wll always report the force in the base frame.
+
+ Returns:
+ A list of foot contact forces.
+ """
+ foot_link_ids = tuple(self._urdf_loader.get_end_effector_id_dict().values())
+ contact_forces = [np.zeros(3) for _ in range(len(foot_link_ids))]
+
+ all_contacts = self._pybullet_client.getContactPoints(
+ bodyA=self._urdf_loader.robot_id)
+
+ for contact in all_contacts:
+ (unused_flag, body_a_id, body_b_id, link_a_id, unused_link_b_id,
+ unused_pos_on_a, unused_pos_on_b, contact_normal_b_to_a, unused_distance,
+ normal_force, friction_1, friction_direction_1, friction_2,
+ friction_direction_2) = contact
+
+ # Ignore self contacts
+ if body_b_id == body_a_id:
+ continue
+
+ if link_a_id in foot_link_ids:
+ normal_force = np.array(contact_normal_b_to_a) * normal_force
+ friction_force = np.array(friction_direction_1) * friction_1 + np.array(
+ friction_direction_2) * friction_2
+ force = normal_force + friction_force
+ local_force = kinematics_utils.rotate_to_base_frame(
+ self._pybullet_client, self.urdf_loader.robot_id, force,
+ self._init_orientation_inv_quat)
+ local_force_norm = np.linalg.norm(local_force)
+ toe_link_order = foot_link_ids.index(link_a_id)
+ if local_force_norm > 0:
+ contact_forces[toe_link_order] += local_force
+ else:
+ continue
+ return contact_forces
+
+ def compute_jacobian_for_one_leg(self, leg_id: int) -> np.ndarray:
+ """Compute the Jacobian for a given leg.
+
+ Args:
+ leg_id: Index of the leg for which the jacobian is computed.
+
+ Returns:
+ The 3 x N transposed Jacobian matrix. where N is the total DoFs of the
+ robot. For a quadruped, the first 6 columns of the matrix corresponds to
+ the CoM translation and rotation. The columns corresponds to a leg can be
+ extracted with indices [6 + leg_id * 3: 6 + leg_id * 3 + 3]. Note that
+ the jacobian is calculated for motors, which takes motor directions into
+ consideration.
+ """
+ com_dof = self._urdf_loader.com_dof
+ foot_link_ids = tuple(self._urdf_loader.get_end_effector_id_dict().values())
+ return kinematics_utils.compute_jacobian(
+ pybullet_client=self.pybullet_client,
+ urdf_id=self.robot_id,
+ link_id=foot_link_ids[leg_id],
+ all_joint_positions=[
+ state[0] for state in self._joint_states
+ ]) * np.concatenate([np.ones(com_dof), self._motor_directions])
+
+ def map_contact_force_to_joint_torques(
+ self, leg_id: int, contact_force: np.ndarray) -> Dict[int, float]:
+ """Maps the foot contact force to the leg joint torques.
+
+ Args:
+ leg_id: Index of the leg for which the jacobian is computed.
+ contact_force: Desired contact force experted by the leg.
+
+ Returns:
+ A dict containing the torques for each motor on the leg.
+ """
+ foot_link_ids = tuple(self._urdf_loader.get_end_effector_id_dict().values())
+ jv = self.compute_jacobian_for_one_leg(leg_id)
+ all_motor_torques = np.matmul(contact_force, jv)
+ motor_torques = {}
+ motors_per_leg = self.num_motors // len(foot_link_ids)
+ com_dof = self._urdf_loader.com_dof
+ for joint_id in range(leg_id * motors_per_leg,
+ (leg_id + 1) * motors_per_leg):
+ motor_torques[joint_id] = all_motor_torques[com_dof + joint_id]
+
+ return motor_torques
+
+ @classmethod
+ def get_constants(cls):
+ raise NotImplementedError("Not yet implemented!")
+
+ @property
+ def timestamp(self):
+ return self._clock()
+
+ @property
+ def action_space(self):
+ return self._action_space
+
+ @property
+ def action_names(self):
+ return self._action_names
+
+ @property
+ def base_orientation_quaternion(self):
+ """Gets the base orientation as a quaternion.
+
+ The base orientation is always relative to the init_orientation, which
+ can be updated by Reset function. This is necessary as many URDF can have an
+ internal frame that is not z-up, so if we don't provide an init_orientation
+ (through Reset), the loaded robot can have its belly facing the horizontal
+ direction.
+
+ Returns:
+ The base orientation in quaternion.
+ """
+ return self._base_orientation_quat
+
+ @property
+ def base_orientation_quaternion_default_frame(self):
+ """Gets the base orientation in the robot's default frame.
+
+ This is the base orientation in whatever frame the robot specifies. For
+ simulated robot this is the URDF's internal frame. For real robot this can
+ be based on the rpy reading determined by the IMU.
+
+ Returns:
+ The base orientation in quaternion in a robot default frame.
+ """
+ _, base_orientation_quat = (
+ self._pybullet_client.getBasePositionAndOrientation(
+ self._urdf_loader.robot_id))
+ return base_orientation_quat
+
+ @property
+ def sensors(self):
+ return self._sensors
+
+ @property
+ def base_roll_pitch_yaw(self):
+ return self._base_roll_pitch_yaw
+
+ @property
+ def base_roll_pitch_yaw_rate(self):
+ return self._base_roll_pitch_yaw_rate
+
+ @property
+ def base_position(self):
+ return self._base_position
+
+ @property
+ def base_velocity(self):
+ return self._base_velocity
+
+ @property
+ def is_safe(self):
+ return True
+
+ @property
+ def num_motors(self):
+ return self._num_motors
+
+ @property
+ def motor_model(self):
+ return self._motor_model
+
+ @property
+ def motor_limits(self) -> robot_config.MotorLimits:
+ return self._motor_limits
+
+ @property
+ def motor_angles(self):
+ return self._motor_angles
+
+ @property
+ def motor_velocities(self):
+ return self._motor_velocities
+
+ @property
+ def motor_torques(self):
+ return self._motor_torques
+
+ @property
+ def pybullet_client(self):
+ return self._pybullet_client
+
+ @property
+ def urdf_loader(self):
+ return self._urdf_loader
+
+ @property
+ def robot_id(self):
+ return self._urdf_loader.robot_id
+
+ @property
+ def initital_orientation_inverse_quaternion(self):
+ return self._init_orientation_inv_quat
+
+ @property
+ def base_acceleration_accelerometer(self):
+ """Get the base acceleration measured by an accelerometer.
+
+ The acceleration is measured in the local frame of a free-falling robot,
+ which is consistent with the robot's IMU measurements. Here the
+ gravitational acceleration is first added to the acceleration in the world
+ frame, which is then converted to the local frame of the robot.
+
+ """
+ return np.array(self._last_base_acceleration_accelerometer)
+
+ @property
+ def base_acceleration(self):
+ """Get the base acceleration in the world frame."""
+ return np.array(self._last_base_acceleration_world)
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/robots/robot_base.py b/examples/pybullet/gym/pybullet_envs/minitaur/robots/robot_base.py
new file mode 100644
index 000000000..75dc127cb
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/robots/robot_base.py
@@ -0,0 +1,133 @@
+# Lint as: python3
+"""The abstract robot class."""
+
+import abc
+from typing import Optional, Sequence
+
+# Action names for robots operating kinematically.
+LINEAR_VELOCITY = "linear_velocity"
+ANGULAR_VELOCITY = "angular_velocity"
+
+
+class RobotBase(metaclass=abc.ABCMeta):
+ """The base class for all robots used in the mobility team."""
+
+ @abc.abstractmethod
+ def reset(
+ self,
+ base_position: Optional[Sequence[float]] = None,
+ base_orientation_quaternion: Optional[Sequence[float]] = None) -> None:
+ """Resets the states (e.g. pose and sensor readings) of the robot.
+
+ This is called at the start of each episode by the environment.
+
+ Args:
+ base_position: Robot base position after reset. If None, robot stay where
+ it was after reset. For robot that does not support reset with position
+ change, a ValueError should be raised.
+ base_orientation_quaternion: Robot base orientation after reset. If None,
+ robot stays in pre-reset orientation. For robot that does not support
+ reset with orientation change, a ValueError should be raised.
+ """
+ pass
+
+ @abc.abstractmethod
+ def terminate(self):
+ """Shuts down the robot."""
+ pass
+
+ @abc.abstractmethod
+ def pre_control_step(self, action):
+ """Processes the input action before the action repeat loop.
+
+ We assume that an action sent to the real robot is sticky, i.e. it will be
+ executed until a new action is received after some time. To simulate this,
+ we introduced the action_repeat parameter, to reflect how many time steps it
+ takes for the policy to generate a new action. That is, for each control
+ step, the simulation contains an inner loop:
+
+ robot.pre_control_step(action) # Smooth or interpolate the action
+ for i in range(action_repeat):
+ robot.apply_action(action)
+ bullet.stepSimulation(time_step) # Step the sim for one time step
+ robot.receive_observation() # Update the sensor observations
+ robot.post_control_step() # Update some internal variables.
+
+ Args:
+ action: Data type depends on the robot. Can be desired motor
+ position/torques for legged robots, or desired velocity/angular velocity
+ for wheeled robots.
+ """
+ pass
+
+ @abc.abstractmethod
+ def apply_action(self, action):
+ """Applies the action to the robot."""
+ pass
+
+ @abc.abstractmethod
+ def receive_observation(self):
+ """Updates the robot sensor readings."""
+ pass
+
+ @abc.abstractmethod
+ def post_control_step(self):
+ """Updates some internal variables such as step counters."""
+ pass
+
+ @property
+ def action_space(self):
+ """The action spec of the robot."""
+ raise NotImplementedError("action_space is not implemented")
+
+ @property
+ @abc.abstractmethod
+ def action_names(self):
+ """Name of each action in the action_space.
+
+ This is a structure of strings with the same shape as the action space,
+ where each string describes the corresponding element of the action space
+ (for example, a kinematic robot might return ("linear_velocity",
+ "angular_velocity")). Used for logging in the safety layer.
+ """
+
+ @property
+ def sensors(self):
+ """Returns the sensors on this robot.
+
+ Sensors are the main interface between the robot class and the gym
+ environment. Sensors can return what the robot can measure (e.g.
+ joint angles, IMU readings), and can represent more general quantities, i.e.
+ the last action taken, that can be part of the observation space.
+ Sensor classes are used by the robot class to the specify its observation
+ space.
+
+ """
+ raise NotImplementedError("sensors property not implemented")
+
+ @property
+ def base_orientation_quaternion(self):
+ """Returns the base pose as a quaternion in format (x, y, z, w).
+
+ These properties differ from the sensor interfaces, as they represent
+ the built-in measurable quantities. We assume most robots have an IMU at
+ its base to measure the base pose. Actually, some sensor classes like the
+ base pose sensor and joint angle sensor will call these built-in methods. In
+ general, how these quantities can be extracted depends on the specific real
+ robots.
+
+ """
+ raise NotImplementedError("base_orientation_quaternion is not implemented")
+
+ @property
+ def base_roll_pitch_yaw(self):
+ """Returns the base roll, pitch, and yaw angles."""
+ raise NotImplementedError("base_roll_pitch_yaw is not implemented")
+
+ @property
+ def base_roll_pitch_yaw_rate(self):
+ raise NotImplementedError("base_roll_pitch_yaw_rate is not implemented")
+
+ @property
+ def base_position(self):
+ raise NotImplementedError("base_position is not implemented")
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/robots/robot_config.py b/examples/pybullet/gym/pybullet_envs/minitaur/robots/robot_config.py
new file mode 100644
index 000000000..255e686d2
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/robots/robot_config.py
@@ -0,0 +1,105 @@
+# Lint as: python3
+"""The configuration parameters for our robots."""
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+import enum
+from typing import Sequence, Union
+import dataclasses
+import gin
+import numpy as np
+
+
+@gin.constants_from_enum
+class MotorControlMode(enum.Enum):
+ """The supported motor control modes."""
+ POSITION = 1,
+
+ # Apply motor torques directly.
+ TORQUE = 2,
+
+ # Apply a tuple (q, qdot, kp, kd, tau) for each motor. Here q, qdot are motor
+ # position and velocities. kp and kd are PD gains. tau is the additional
+ # motor torque. This is the most flexible control mode.
+ HYBRID = 3,
+
+ # PWM mode is only availalbe for Minitaur
+ PWM = 4
+
+
+# TODO(b/127675924): Group other parameters in the named attrib class.
+
+# Each hybrid action is a tuple (position, position_gain, velocity,
+# velocity_gain, torque)
+HYBRID_ACTION_DIMENSION = 5
+
+
+class HybridActionIndex(enum.Enum):
+ # The index of each component within the hybrid action tuple.
+ POSITION = 0
+ POSITION_GAIN = 1
+ VELOCITY = 2
+ VELOCITY_GAIN = 3
+ TORQUE = 4
+
+
+@gin.configurable
+class MotorLimits(object):
+ """The data class for motor limits."""
+
+ def __init__(
+ self,
+ angle_lower_limits: Union[float, Sequence[float]] = float('-inf'),
+ angle_upper_limits: Union[float, Sequence[float]] = float('inf'),
+ velocity_lower_limits: Union[float, Sequence[float]] = float('-inf'),
+ velocity_upper_limits: Union[float, Sequence[float]] = float('inf'),
+ torque_lower_limits: Union[float, Sequence[float]] = float('-inf'),
+ torque_upper_limits: Union[float, Sequence[float]] = float('inf'),
+ ):
+ """Initializes the class."""
+ self.angle_lower_limits = angle_lower_limits
+ self.angle_upper_limits = angle_upper_limits
+ self.velocity_lower_limits = velocity_lower_limits
+ self.velocity_upper_limits = velocity_upper_limits
+ self.torque_lower_limits = torque_lower_limits
+ self.torque_upper_limits = torque_upper_limits
+
+
+@gin.constants_from_enum
+class WheeledRobotControlMode(enum.Enum):
+ """The control mode for wheeled robots."""
+ # Controls the base of the robot (i.e. in kinematic mode.) or the base wheels
+ # using motor commands.
+ BASE = 1
+ # Controls arm only
+ ARM = 2
+ # Controls both base and arm
+ BASE_AND_ARM = 3
+ # Controls both base and head
+ BASE_AND_HEAD = 4
+ # Controls the non-wheel motors. This include arms and heads.
+ BODY = 5
+ # Controls all degrees of freedom, i.e. the base and arm/head simultaneously.
+ ALL = 6
+ # High-level navigation target.
+ NAVIGATION_TARGET = 7
+ # Individually addressable actions for body joints, with nested dict actions.
+ ADDRESSABLE = 8
+
+
+@dataclasses.dataclass
+class TwistActionLimits:
+ """The data class for twist action limits.
+
+ Common abbreviations used in variable names suffix:
+ mps = Meters per Second
+ rps = Radians per Second
+ """
+ max_linear_mps: float
+ min_linear_mps: float
+ max_angular_rps: float
+ min_angular_rps: float
+
+
+
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/robots/robot_urdf_loader.py b/examples/pybullet/gym/pybullet_envs/minitaur/robots/robot_urdf_loader.py
new file mode 100644
index 000000000..2d4bf710a
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/robots/robot_urdf_loader.py
@@ -0,0 +1,371 @@
+# Lint as: python3
+"""Helper class to load and manage the robot URDF file in simulation."""
+
+import collections
+from typing import Dict, Text, Tuple
+
+import gin
+import numpy as np
+
+from pybullet_utils import bullet_client
+
+# Base link does not have a parent joint. So we just use the string "robot_base"
+# for reference. The corresponding link/joint id is always -1 in pybullet.
+ROBOT_BASE = "robot_base"
+
+
+def _sub_dict(joint_name_to_id: Dict[Text, int],
+ joint_names: Tuple[Text]) -> Dict[Text, int]:
+ sub_dict = collections.OrderedDict()
+ if joint_names is None:
+ return sub_dict
+ for name in joint_names:
+ sub_dict[name] = joint_name_to_id[name]
+ return sub_dict
+
+
+def convert_to_urdf_joint_angles(
+ robot_space_joint_angles: np.ndarray,
+ joint_offsets: np.ndarray,
+ joint_directions: np.ndarray,
+):
+ return robot_space_joint_angles * joint_directions + joint_offsets
+
+
+def convert_to_robot_joint_angles(
+ urdf_space_joint_angles: np.ndarray,
+ joint_offsets: np.ndarray,
+ joint_directions: np.ndarray,
+):
+ return (urdf_space_joint_angles - joint_offsets) * joint_directions
+
+
+@gin.configurable
+class RobotUrdfLoader(object):
+ """A abstract class to manage the robot urdf in sim."""
+
+ def __init__(
+ self,
+ pybullet_client: bullet_client.BulletClient,
+ urdf_path: Text,
+ constrained_base: bool = False,
+ enable_self_collision: bool = True,
+ init_base_position: Tuple[float] = (0, 0, 0),
+ init_base_orientation_quaternion: Tuple[float] = None,
+ init_base_orientation_rpy: Tuple[float] = None,
+ base_names: Tuple[Text] = None,
+ init_joint_angles: Dict[Text, float] = None,
+ joint_offsets: Dict[Text, float] = None,
+ joint_directions: Dict[Text, int] = None,
+ motor_names: Tuple[Text] = None,
+ end_effector_names: Tuple[Text] = None,
+ user_group: Dict[Text, Tuple[Text]] = None,
+ ):
+ """Initialize the class.
+
+ Args:
+ pybullet_client: A pybullet client.
+ urdf_path: The path to the URDF to load.
+ constrained_base: Whether to create a FIXED constraint to the base of the
+ URDF. Needs to be True for kinematic robots. This allows us to "hang"
+ the simulated robot in air, and the hanging point can follow arbitrary
+ provided paths.
+ enable_self_collision: Determines if the robot can collide with itself.
+ init_base_position: The base x, y, z after loading.
+ init_base_orientation_quaternion: The base rotation after loading.
+ init_base_orientation_rpy: The base rotation after loading, presented in
+ roll, pitch, yaw.
+ base_names: The base joint names. Used to find additional links that
+ belong to the base. Optional because the base might only contain a
+ single mesh/block, which always has the id of "-1".
+ init_joint_angles: Maps joint name to the desired joint pose after loading
+ URDF. If not provided, will use the URDF default. This can be a subset
+ of all joints in the URDF. This should be in the robot joint convention,
+ which can be different from the URDF convention.
+ joint_offsets: The "zero" position of joint angles in the URDF space.
+ joint_directions: To convert between robot sdk/control and urdf joint
+ convention.
+ motor_names: The motor joint names in the URDF. Typically a subset of all
+ movable joints/DoFs.
+ end_effector_names: A subset of joints specifying the end-effector
+ joint(s). For example for legged robots the end effectors are the toe
+ joints (if provided). For arms this group includes left and right
+ grippers.
+ user_group: User defined joint groups. For example for quadrupeds, we may
+ want to organize all joints according to which leg they belong to.
+ """
+ self._pybullet_client = pybullet_client
+ self._urdf_path = urdf_path
+ self._init_base_position = init_base_position
+ if init_base_orientation_quaternion is not None:
+ self._init_base_orientation_quaternion = init_base_orientation_quaternion
+ else:
+ if init_base_orientation_rpy is None:
+ raise ValueError("Either init_base_orientation_quaterion "
+ "or init_base_orientation_rpy is required")
+ self._init_base_orientation_quaternion = (
+ self._pybullet_client.getQuaternionFromEuler(
+ init_base_orientation_rpy))
+ self._constrained_base = constrained_base
+ self._enable_self_collision = enable_self_collision
+ self._base_names = base_names
+ self._init_joint_angles = init_joint_angles
+ self._joint_offsets = joint_offsets
+ self._joint_directions = joint_directions
+ self._motor_names = motor_names
+ self._end_effector_names = end_effector_names
+ self._user_group = user_group
+ self.load(
+ enable_self_collision=enable_self_collision,
+ init_base_position=init_base_position,
+ init_base_orientation_quaternion=init_base_orientation_quaternion,
+ init_joint_angles=init_joint_angles)
+
+ def get_base_id_dict(self, name: Tuple[Text] = None) -> Dict[Text, int]:
+ if name is None:
+ return self._base_dict
+ return _sub_dict(self._base_dict, name)
+
+ def get_joint_id_dict(self, name: Tuple[Text] = None) -> Dict[Text, int]:
+ if name is None:
+ return self._joint_name_to_id
+ return _sub_dict(self._joint_name_to_id, name)
+
+ def get_link_id_dict(self, name: Tuple[Text] = None) -> Dict[Text, int]:
+ if name is None:
+ return self._link_name_to_id
+ return _sub_dict(self._link_name_to_id, name)
+
+ def get_motor_id_dict(self, name: Tuple[Text] = None) -> Dict[Text, int]:
+ if name is None:
+ return self._motor_dict
+ return _sub_dict(self._motor_dict, name)
+
+ def get_joint_direction_dict(self,
+ name: Tuple[Text] = None) -> Dict[Text, int]:
+ if name is None:
+ return self._joint_directions
+ return _sub_dict(self._joint_directions, name)
+
+ def get_joint_offset_dict(self,
+ name: Tuple[Text] = None) -> Dict[Text, float]:
+ if name is None:
+ return self._joint_offsets
+ return _sub_dict(self._joint_offsets, name)
+
+ def get_end_effector_id_dict(self,
+ name: Tuple[Text] = None) -> Dict[Text, int]:
+ if name is None:
+ return self._end_effector_dict
+ return _sub_dict(self._end_effector_dict, name)
+
+ @property
+ def robot_id(self):
+ """Returns the unique object instance id of this loaded URDF in pybullet.
+
+ Note this is different from all other get_{}_id APIs, which returns the
+ joint/link id within this robot instance.
+
+ Returns:
+ The object id as returned by loadURDF.
+ """
+ return self._robot_id
+
+ @property
+ def all_joint_info(self):
+ return self._all_joint_info
+
+ @property
+ def user_dict(self):
+ return self._user_dict
+
+ @property
+ def motor_names(self):
+ return self._motor_names
+
+ @property
+ def constrained_base(self):
+ return self._constrained_base
+
+ def _build_base_dict(self):
+ """Builds the base joints dictionary.
+
+ In pybullet, a link's id within the robot always equal to its parent joint
+ id. So this base joint dict functionaly is equivalent to the base link dict.
+ The dictionary may only contain {ROBOT_BASE: -1} if self._base_names is
+ empty.
+
+ Returns:
+ The base link (joint) ordered dictionary.
+ """
+ base_dict = collections.OrderedDict()
+ if self._base_names is None:
+ base_dict[ROBOT_BASE] = -1
+ else:
+ base_dict.update(_sub_dict(self._joint_name_to_id, self._base_names))
+ return base_dict
+
+ def _build_user_dict(self):
+ """Builds a dictionary using user defined joint groups."""
+ user_dict = collections.OrderedDict()
+ if self._user_group is None:
+ return user_dict
+ for group_name, group_joint_names in self._user_group.items():
+ user_dict[group_name] = collections.OrderedDict()
+ user_dict[group_name].update(
+ _sub_dict(self._joint_name_to_id, group_joint_names))
+ return user_dict
+
+ def _build_all_joint_dict(self):
+ """Extracts all joints from the URDF.
+
+ Finds all joints (fixed or movable) in the URDF and extracts the info. This
+ includes actuated joints (i.e. motors), and non-actuated joints, e.g. the
+ passive joints in Minitaur's four bar mechanism, and fixed joints connecting
+ the toe and the lower legs, etc.
+
+ Returns:
+ number of joints, all joint information as returned by pybullet, and the
+ joint_name_to_id dictionary.
+
+ """
+ num_joints = self._pybullet_client.getNumJoints(self._robot_id)
+ all_joint_info = [
+ self._pybullet_client.getJointInfo(self._robot_id, i)
+ for i in range(num_joints)
+ ]
+
+ # Remove the default joint dampings to increase sim fidelity.
+ for joint_info in all_joint_info:
+ joint_id = joint_info[0]
+ self._pybullet_client.changeDynamics(
+ joint_id, -1, linearDamping=0, angularDamping=0)
+
+ joint_name_to_id = collections.OrderedDict()
+ link_name_to_id = collections.OrderedDict([(ROBOT_BASE, -1)])
+ for joint_info in all_joint_info:
+ joint_name = joint_info[1].decode("UTF-8")
+ joint_id = joint_info[0]
+ joint_name_to_id[joint_name] = joint_id
+ # Index 12 is the name of the joint's child link, and in PyBullet a child
+ # link id is always equal to its parent joint id.
+ link_name_to_id[joint_info[12].decode("UTF-8")] = joint_id
+
+ return num_joints, all_joint_info, joint_name_to_id, link_name_to_id
+
+ def load(
+ self,
+ enable_self_collision: bool = None,
+ init_base_position: Tuple[float] = None,
+ init_base_orientation_quaternion: Tuple[float] = None,
+ init_joint_angles: Dict[Text, float] = None,
+ ):
+ """Reloads the URDF and rebuilds the dictionaries."""
+ if enable_self_collision is None:
+ enable_self_collision = self._enable_self_collision
+ if init_base_position is None:
+ init_base_position = self._init_base_position
+ if init_base_orientation_quaternion is None:
+ init_base_orientation_quaternion = self._init_base_orientation_quaternion
+
+ self._robot_id = self._load_urdf(enable_self_collision, init_base_position,
+ init_base_orientation_quaternion)
+
+ (self._num_joints, self._all_joint_info, self._joint_name_to_id,
+ self._link_name_to_id) = self._build_all_joint_dict()
+ self._base_dict = self._build_base_dict()
+ self._motor_dict = _sub_dict(self._joint_name_to_id, self._motor_names)
+ self._end_effector_dict = _sub_dict(self._joint_name_to_id,
+ self._end_effector_names)
+ self._user_dict = self._build_user_dict()
+
+ self.reset_base_pose(init_base_position, init_base_orientation_quaternion)
+ self.reset_joint_angles(init_joint_angles)
+
+ def _load_urdf(self, enable_self_collision: bool,
+ init_base_position: Tuple[float],
+ init_base_orientation_quaternion: Tuple[float]) -> int:
+ """Loads the URDF and returns the pybullet id."""
+ try:
+ if enable_self_collision:
+ return self._pybullet_client.loadURDF(
+ self._urdf_path,
+ init_base_position,
+ init_base_orientation_quaternion,
+ useFixedBase=self._constrained_base,
+ flags=self._pybullet_client.URDF_USE_SELF_COLLISION)
+ else:
+ return self._pybullet_client.loadURDF(
+ self._urdf_path,
+ init_base_position,
+ init_base_orientation_quaternion,
+ useFixedBase=self._constrained_base,
+ )
+ except:
+ print("!!!!!!!!!!!!!!!!")
+ print("Error: cannot find file:")
+ print(self._urdf_path)
+ import sys
+ sys.exit(0)
+
+ def reset_joint_angles(self, joint_angles: Dict[Text, float] = None):
+ """Resets the joint angles.
+
+ Resets the joint poses. This is instanteneously and will ignore the physics
+ (e.g. collisions, inertias, etc). Should only be used during the episode
+ reset time. Does not work for real robots (other than changing the
+ visualization). This API has no effect if both the input joint_angles and
+ the self._init_joint_angles are None.
+
+ Args:
+ joint_angles: The joint angles in the robot's joint space.
+
+ Raises:
+ AttributeError if the joint directions and joint offsets are not provided
+ during init.
+ """
+ if self._init_joint_angles is None:
+ return
+
+ if joint_angles is None:
+ joint_angles = self._init_joint_angles
+
+ if self._joint_directions is None or self._joint_offsets is None:
+ raise AttributeError(
+ "joint directions and joint offsets not provided in __init__")
+
+ for joint_name, angle in joint_angles.items():
+ urdf_joint_angle = angle * self._joint_directions[
+ joint_name] + self._joint_offsets[joint_name]
+ self._pybullet_client.resetJointState(
+ self._robot_id,
+ self._joint_name_to_id[joint_name],
+ urdf_joint_angle,
+ targetVelocity=0)
+
+ def reset_base_pose(
+ self,
+ base_position: Tuple[float] = None,
+ base_orientation_quaternion: Tuple[float] = None,
+ ):
+ """Resets the base position and orientation.
+
+ Instanteneously re-position the base pose of the robot. Does not work for
+ real robots except for the visualization.
+
+ Args:
+ base_position: Base x, y, z position.
+ base_orientation_quaternion: Base rotation.
+ """
+ if base_position is None:
+ base_position = self._init_base_position
+ if base_orientation_quaternion is None:
+ base_orientation_quaternion = self._init_base_orientation_quaternion
+ self._pybullet_client.resetBasePositionAndOrientation(
+ self._robot_id, base_position, base_orientation_quaternion)
+ self._pybullet_client.resetBaseVelocity(self._robot_id, (0, 0, 0),
+ (0, 0, 0))
+
+ @property
+ def com_dof(self):
+ return 0 if self._constrained_base else 6
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/robots/safety/__init__.py b/examples/pybullet/gym/pybullet_envs/minitaur/robots/safety/__init__.py
new file mode 100644
index 000000000..e69de29bb
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/robots/safety/data_types.py b/examples/pybullet/gym/pybullet_envs/minitaur/robots/safety/data_types.py
new file mode 100644
index 000000000..853bb567b
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/robots/safety/data_types.py
@@ -0,0 +1,67 @@
+"""Definitions of safety layer data types."""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+import attr
+
+
+@attr.s
+class Bound(object):
+ """Struct for inclusive lower and upper bounds."""
+ lower = attr.ib(type=float, default=0)
+ upper = attr.ib(type=float, default=0)
+
+ @upper.validator # pytype: disable=attribute-error
+ def _upper_greator_equal_to_lower(self, attribute, value):
+ del attribute
+ assert value >= self.lower, (
+ "upper bound {} is less than lower bound {}".format(value, self.lower))
+
+
+@attr.s
+class SafetyConfig(object):
+ """Struct to configure the safety module."""
+ motor_position_bound = attr.ib(type=list)
+ motor_position_gain_bound = attr.ib(type=list)
+ motor_velocity_bound = attr.ib(type=list)
+ motor_velocity_gain_bound = attr.ib(type=list)
+ motor_torque_bound = attr.ib(type=list)
+ timestamp_delta_bound = attr.ib(type=Bound)
+ motor_average_abs_velocity_bound = attr.ib(type=list)
+ motor_average_abs_power_bound = attr.ib(type=list)
+ state_action_timestamp_delta_bound = attr.ib(type=float)
+ motor_delta_position_bound = attr.ib(type=list)
+ motor_average_abs_delta_position_bound = attr.ib(type=list)
+
+
+@attr.s
+class MotorState(object):
+ """A generic type for motor state.
+
+ Motor states are what we can potentially read from the motor encoder or
+ firmware APIs.
+
+ """
+ timestamp = attr.ib(type=float, default=None)
+ position = attr.ib(type=float, default=None)
+ position_gain = attr.ib(type=float, default=None)
+ velocity = attr.ib(type=float, default=None)
+ velocity_gain = attr.ib(type=float, default=None)
+ torque = attr.ib(type=float, default=None)
+
+
+@attr.s
+class MotorAction(object):
+ """A generic type for motor action.
+
+ Motor actions are the potential command structure we can send to the motor.
+ While similar to MotorState, they are logically very different entities.
+ """
+ timestamp = attr.ib(type=float, default=None)
+ position = attr.ib(type=float, default=None)
+ position_gain = attr.ib(type=float, default=None)
+ velocity = attr.ib(type=float, default=None)
+ velocity_gain = attr.ib(type=float, default=None)
+ torque = attr.ib(type=float, default=None)
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/robots/safety/motor_action_validator.py b/examples/pybullet/gym/pybullet_envs/minitaur/robots/safety/motor_action_validator.py
new file mode 100644
index 000000000..567add414
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/robots/safety/motor_action_validator.py
@@ -0,0 +1,128 @@
+"""Validates the motor commands."""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+import typing
+from pybullet_envs.minitaur.robots import robot_config
+from pybullet_envs.minitaur.robots.safety import data_types
+from pybullet_envs.minitaur.robots.safety import utilities
+from pybullet_envs.minitaur.robots.safety.python import moving_window_filter
+
+_DEQUE_SIZE = 200
+
+
+class MotorActionValidator(object):
+ """A safety guard to check motor actions.
+
+ Monitors the commands sent to the motor and detect unsafe behaviors.
+ """
+
+ def __init__(
+ self,
+ motor_id: typing.Any,
+ position_bound: data_types.Bound,
+ position_gain_bound: data_types.Bound,
+ velocity_bound: data_types.Bound,
+ velocity_gain_bound: data_types.Bound,
+ torque_bound: data_types.Bound,
+ timestamp_delta_bound: data_types.Bound,
+ delta_position_bound: data_types.Bound,
+ average_abs_delta_position_bound: data_types.Bound,
+ state_buffer_size: int = _DEQUE_SIZE,
+ ):
+ """Initializes the class.
+
+ Args:
+ motor_id: Unique ID for the motor.
+ position_bound: The lower/upper bound of the motor angle.
+ position_gain_bound: The lower/upper bound of the motor position gain for
+ PD control.
+ velocity_bound: The lower/upper bound of the motor speed.
+ velocity_gain_bound: The lower/upper bound of the motor velocity gain for
+ PD control.
+ torque_bound: The lower/upper bound of the measured motor torque.
+ timestamp_delta_bound: The range of timestamp difference between two
+ consecutively received motor states.
+ delta_position_bound: The bound between the current motor position and the
+ command position, in position control mode.
+ average_abs_delta_position_bound: The bound for average motor position and
+ command poisition difference.
+ state_buffer_size: The buffer size used to calculate the average.
+ """
+ assert state_buffer_size > 1
+ self._last_motor_state = None
+
+ self._motor_id = motor_id
+ self._position_bound = position_bound
+ self._position_gain_bound = position_gain_bound
+ self._velocity_bound = velocity_bound
+ self._velocity_gain_bound = velocity_gain_bound
+ self._torque_bound = torque_bound
+ self._timestamp_delta_bound = timestamp_delta_bound
+ self._delta_position_bound = delta_position_bound
+ self._average_abs_delta_position_bound = average_abs_delta_position_bound
+ self._abs_delta_position_filter = moving_window_filter.MovingWindowFilter(
+ state_buffer_size)
+
+ def on_state(self, new_state: data_types.MotorState):
+ """Updates the last motor state.
+
+ Args:
+ new_state: The latest motor state.
+ """
+ self._last_motor_state = new_state
+
+ def on_action(self, new_action: data_types.MotorAction,
+ control_mode: robot_config.MotorControlMode):
+ """Adds a new motor action and validates it.
+
+ Args:
+ new_action: A new action that will be send to the motor.
+ control_mode: The motor control mode.
+
+ Raises:
+ safety_error.OutOfBoundError: When any of the motor action fields or
+ state-action difference is out of bound.
+ """
+
+ # We first validate the new state.
+
+ motor_str = "motor {} ".format(self._motor_id)
+ if (control_mode == robot_config.MotorControlMode.POSITION or
+ control_mode == robot_config.MotorControlMode.HYBRID):
+ utilities.assert_in_bound(motor_str + "position", new_action.position,
+ self._position_bound)
+ utilities.assert_in_bound(motor_str + "velocity", new_action.velocity,
+ self._velocity_bound)
+ utilities.assert_in_bound(motor_str + "position gain",
+ new_action.position_gain,
+ self._position_gain_bound)
+ utilities.assert_in_bound(motor_str + "velocity gain",
+ new_action.velocity_gain,
+ self._velocity_gain_bound)
+
+ utilities.assert_in_bound(motor_str + "torque", new_action.torque,
+ self._torque_bound)
+
+ if self._last_motor_state is None:
+ return
+
+ delta_time = new_action.timestamp - self._last_motor_state.timestamp
+ utilities.assert_in_bound(motor_str + "state-action timestamp difference",
+ delta_time, self._timestamp_delta_bound)
+
+ # To detect the bang-bang type controller behavior.
+ if (control_mode == robot_config.MotorControlMode.POSITION or
+ control_mode == robot_config.MotorControlMode.HYBRID):
+ delta_position = new_action.position - self._last_motor_state.position
+ utilities.assert_in_bound(motor_str + "state-action position difference",
+ delta_position, self._delta_position_bound)
+
+ average_abs_delta_position = (
+ self._abs_delta_position_filter.CalculateAverage(
+ abs(delta_position)))
+ utilities.assert_in_bound(
+ motor_str + "average state-action position difference",
+ average_abs_delta_position, self._average_abs_delta_position_bound)
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/robots/safety/motor_state_validator.py b/examples/pybullet/gym/pybullet_envs/minitaur/robots/safety/motor_state_validator.py
new file mode 100644
index 000000000..3ccf34c02
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/robots/safety/motor_state_validator.py
@@ -0,0 +1,137 @@
+"""Software safety layer for robot control.
+
+Validates the motor states received from the motor encoder.
+
+"""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+import typing
+
+from pybullet_envs.minitaur.robots.safety import data_types
+from pybullet_envs.minitaur.robots.safety import utilities
+from pybullet_envs.minitaur.robots.safety.python import moving_window_filter
+
+# The default internal buffer size for the MotorStateValidator.
+_DEQUE_SIZE = 200
+
+
+class MotorStateValidator(object):
+ """A safety guard to check motor states.
+
+ Monitors the status of the motor and detects anomalies in the
+ readings. For example, the class will throw safety errors if the motor
+ velocity is too large. Currently we support checking of motor angle, velocity,
+ gain, torque, as well as the timestamp interval.
+
+ Attributes:
+ last_state: The last received motor state.
+ """
+
+ def __init__(
+ self,
+ motor_id: typing.Any,
+ position_bound: data_types.Bound,
+ position_gain_bound: data_types.Bound,
+ velocity_bound: data_types.Bound,
+ velocity_gain_bound: data_types.Bound,
+ torque_bound: data_types.Bound,
+ timestamp_delta_bound: data_types.Bound,
+ average_abs_velocity_bound: data_types.Bound,
+ average_abs_power_bound: data_types.Bound,
+ state_buffer_size: int = _DEQUE_SIZE,
+ ):
+ """Initializes the class.
+
+ Args:
+ motor_id: Unique ID for the motor.
+ position_bound: The lower/upper bound of the motor angle.
+ position_gain_bound: The lower/upper bound of the motor position gain for
+ PD control.
+ velocity_bound: The lower/upper bound of the motor speed.
+ velocity_gain_bound: The lower/upper bound of the motor velocity gain for
+ PD control.
+ torque_bound: The lower/upper bound of the measured motor torque.
+ timestamp_delta_bound: The range of timestamp difference between two
+ consecutively received motor states.
+ average_abs_velocity_bound: The average absolute velocity limit.
+ average_abs_power_bound: The average absolute mechanical power limit.
+ state_buffer_size: The buffer size used to calculate the average.
+ """
+ assert state_buffer_size > 1
+ self.last_state = None
+
+ self._motor_id = motor_id
+ self._position_bound = position_bound
+ self._position_gain_bound = position_gain_bound
+ self._velocity_bound = velocity_bound
+ self._velocity_gain_bound = velocity_gain_bound
+ self._torque_bound = torque_bound
+ self._timestamp_delta_bound = timestamp_delta_bound
+ self._average_abs_velocity_bound = average_abs_velocity_bound
+ self._average_abs_power_bound = average_abs_power_bound
+
+ # For velocity/power, we use a filter to compute their averages
+ # over a small period. This is to avoid the noisy readings giving false
+ # positive.
+ self._abs_velocity_filter = moving_window_filter.MovingWindowFilter(
+ state_buffer_size)
+ self._abs_power_filter = moving_window_filter.MovingWindowFilter(
+ state_buffer_size)
+
+ def on_state(self, new_state: data_types.MotorState):
+ """Adds a new motor state and validates it.
+
+ Will validate both the instantenous state as well as statitical
+ averages.
+
+ Args:
+ new_state: A new state from the motor encoder.
+
+ Raises:
+ safety_error.OutOfBoundError: When any of the motor readings (e.g.
+ position, torque) is out of bound.
+ """
+
+ # We first validate the new state.
+
+ motor_str = "motor {} ".format(self._motor_id)
+ utilities.assert_in_bound(motor_str + "position", new_state.position,
+ self._position_bound)
+ utilities.assert_in_bound(motor_str + "velocity", new_state.velocity,
+ self._velocity_bound)
+ utilities.assert_in_bound(motor_str + "position gain",
+ new_state.position_gain,
+ self._position_gain_bound)
+ utilities.assert_in_bound(motor_str + "velocity gain",
+ new_state.velocity_gain,
+ self._velocity_gain_bound)
+ utilities.assert_in_bound(motor_str + "torque", new_state.torque,
+ self._torque_bound)
+
+ if not self.last_state:
+ self.last_state = new_state
+ return
+
+ last_state = self.last_state
+
+ # Check if the time interval between two received states are large.
+
+ delta_time = new_state.timestamp - last_state.timestamp
+ utilities.assert_in_bound(motor_str + "timestamp", delta_time,
+ self._timestamp_delta_bound)
+
+ average_abs_velocity = self._abs_velocity_filter.CalculateAverage(
+ abs(new_state.velocity))
+ utilities.assert_in_bound(motor_str + "average velocity",
+ average_abs_velocity,
+ self._average_abs_velocity_bound)
+
+ average_abs_power = self._abs_power_filter.CalculateAverage(
+ abs(new_state.velocity * new_state.torque))
+ utilities.assert_in_bound(motor_str + "average power", average_abs_power,
+ self._average_abs_power_bound)
+
+ self.last_state = new_state
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/robots/safety/python/moving_window_filter.py b/examples/pybullet/gym/pybullet_envs/minitaur/robots/safety/python/moving_window_filter.py
new file mode 100644
index 000000000..4249677c5
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/robots/safety/python/moving_window_filter.py
@@ -0,0 +1,68 @@
+"""Moving window filter to smooth out sensor readings."""
+
+import collections
+
+class MovingWindowFilter(object):
+ """A stable O(1) moving filter for incoming data streams.
+
+ We implement the Neumaier's algorithm to calculate the moving window average,
+ which is numerically stable.
+
+ """
+
+ def __init__(self, window_size: int):
+ """Initializes the class.
+
+ Args:
+ window_size: The moving window size.
+ """
+ assert window_size > 0
+ self._window_size = window_size
+ self._value_deque = collections.deque(maxlen=window_size)
+ # The moving window sum.
+ self._sum = 0
+ # The correction term to compensate numerical precision loss during
+ # calculation.
+ self._correction = 0
+
+ def _neumaier_sum(self, value: float):
+ """Update the moving window sum using Neumaier's algorithm.
+
+ For more details please refer to:
+ https://en.wikipedia.org/wiki/Kahan_summation_algorithm#Further_enhancements
+
+ Args:
+ value: The new value to be added to the window.
+ """
+
+ new_sum = self._sum + value
+ if abs(self._sum) >= abs(value):
+ # If self._sum is bigger, low-order digits of value are lost.
+ self._correction += (self._sum - new_sum) + value
+ else:
+ # low-order digits of sum are lost
+ self._correction += (value - new_sum) + self._sum
+
+ self._sum = new_sum
+
+ def calculate_average(self, new_value: float) -> float:
+ """Computes the moving window average in O(1) time.
+
+ Args:
+ new_value: The new value to enter the moving window.
+
+ Returns:
+ The average of the values in the window.
+
+ """
+ deque_len = len(self._value_deque)
+ if deque_len < self._value_deque.maxlen:
+ pass
+ else:
+ # The left most value to be subtracted from the moving sum.
+ self._neumaier_sum(-self._value_deque[0])
+
+ self._neumaier_sum(new_value)
+ self._value_deque.append(new_value)
+
+ return (self._sum + self._correction) / self._window_size
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/robots/safety/safety_checker.py b/examples/pybullet/gym/pybullet_envs/minitaur/robots/safety/safety_checker.py
new file mode 100644
index 000000000..765028c0e
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/robots/safety/safety_checker.py
@@ -0,0 +1,119 @@
+"""The generic safety checking interface.
+
+Defines the generic safety checker class that can detect bad motor states, imu
+states, self-collisions, unsafe motor commands, unusual temperature reading,
+etc. Safety criterions are provided by the robot class.
+"""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+import typing
+from pybullet_envs.minitaur.robots import robot_config
+from pybullet_envs.minitaur.robots.safety import data_types
+from pybullet_envs.minitaur.robots.safety import motor_action_validator
+from pybullet_envs.minitaur.robots.safety import motor_state_validator
+from pybullet_envs.minitaur.robots.safety import utilities
+
+_MOTOR_STATE_BUFFER_SIZE = 500
+
+
+class SafetyChecker(object):
+ """The generic safety checking interface."""
+
+ def __init__(
+ self,
+ robot: typing.Any,
+ ):
+ """Initilaizes the class.
+
+ TODO(b/131377892): Implement other state checkings including the
+ IMU/temperature/contact force if enabled.
+
+ Args:
+ robot: A robot instance such like Minitaur/Laikago/Vision60.
+ """
+ self._robot = robot
+
+ self._motor_state_validators = []
+ self._motor_action_validators = []
+ for i in range(robot.num_motors):
+ self._motor_state_validators.append(
+ motor_state_validator.MotorStateValidator(
+ motor_id=i,
+ position_bound=robot.safety_config.motor_position_bound[i],
+ position_gain_bound=robot.safety_config
+ .motor_position_gain_bound[i],
+ velocity_bound=robot.safety_config.motor_velocity_bound[i],
+ velocity_gain_bound=robot.safety_config
+ .motor_velocity_gain_bound[i],
+ torque_bound=robot.safety_config.motor_torque_bound[i],
+ timestamp_delta_bound=robot.safety_config.timestamp_delta_bound,
+ average_abs_velocity_bound=robot.safety_config
+ .motor_average_abs_velocity_bound[i],
+ average_abs_power_bound=robot.safety_config
+ .motor_average_abs_power_bound[i],
+ state_buffer_size=_MOTOR_STATE_BUFFER_SIZE,
+ ))
+ self._motor_action_validators.append(
+ motor_action_validator.MotorActionValidator(
+ motor_id=i,
+ position_bound=robot.safety_config.motor_position_bound[i],
+ position_gain_bound=robot.safety_config
+ .motor_position_gain_bound[i],
+ velocity_bound=robot.safety_config.motor_velocity_bound[i],
+ velocity_gain_bound=robot.safety_config
+ .motor_velocity_gain_bound[i],
+ torque_bound=robot.safety_config.motor_torque_bound[i],
+ timestamp_delta_bound=robot.safety_config
+ .state_action_timestamp_delta_bound,
+ delta_position_bound=robot.safety_config
+ .motor_delta_position_bound[i],
+ average_abs_delta_position_bound=robot.safety_config
+ .motor_average_abs_delta_position_bound[i],
+ state_buffer_size=_MOTOR_STATE_BUFFER_SIZE,
+ ))
+
+ def check_state(self) -> None:
+ """Validates the state of the robot.
+
+ TODO(b/131377892): Implement other state checkings including the
+ IMU/temperature/contact force if enabled.
+
+ Raises:
+ A safety exception if any state checking (motor/imu/etc) fails.
+ """
+
+ for motor_id, state_validator, action_validator in zip(
+ range(self._robot.num_motors), self._motor_state_validators,
+ self._motor_action_validators):
+ motor_state = data_types.MotorState(
+ timestamp=self._robot.last_state_time,
+ position=self._robot.GetMotorAngles()[motor_id],
+ velocity=self._robot.GetMotorVelocities()[motor_id],
+ position_gain=self._robot.GetMotorPositionGains()[motor_id],
+ velocity_gain=self._robot.GetMotorVelocityGains()[motor_id],
+ torque=self._robot.GetMotorTorques()[motor_id],
+ )
+ state_validator.on_state(motor_state)
+ action_validator.on_state(motor_state)
+
+ def check_motor_action(
+ self,
+ action: typing.Sequence[float],
+ control_mode: robot_config.MotorControlMode,
+ ) -> None:
+ """Validate the action w.r.t to the motor states.
+
+ Args:
+ action: The motor commands sent to the robot.
+ control_mode: The motor control mode.
+
+ Raises:
+ A safety exception if action checking fails.
+ """
+ motor_action_list = utilities.convert_to_motor_action(
+ self._robot, action, control_mode)
+ for motor_id, validator in enumerate(self._motor_action_validators):
+ validator.on_action(motor_action_list[motor_id], control_mode)
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/robots/safety/safety_error.py b/examples/pybullet/gym/pybullet_envs/minitaur/robots/safety/safety_error.py
new file mode 100644
index 000000000..5ff078a6f
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/robots/safety/safety_error.py
@@ -0,0 +1,11 @@
+"""Exception types for safety related error."""
+
+
+class SafetyError(Exception):
+ """The base safety exception."""
+ pass
+
+
+class OutOfBoundError(SafetyError):
+ """Rasied when values like motor position or velocity is out of bound."""
+ pass
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/robots/safety/utilities.py b/examples/pybullet/gym/pybullet_envs/minitaur/robots/safety/utilities.py
new file mode 100644
index 000000000..9703b6bf0
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/robots/safety/utilities.py
@@ -0,0 +1,163 @@
+"""Utilities for safety layers."""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+import collections
+import typing
+from pybullet_envs.minitaur.robots import robot_config
+from pybullet_envs.minitaur.robots.safety import data_types
+from pybullet_envs.minitaur.robots.safety import safety_error
+
+
+def assert_in_bound(name: typing.Text, value: float, bound: data_types.Bound):
+ """Check if the given value is within the provided bound.
+
+ Args:
+ name: The name of the value.
+ value: Number to be checked.
+ bound: Contains the lower and upper bounds. The bound is inclusive.
+
+ Raises:
+ safety_error.OutofBoundError: when the value is outside the bound.
+ """
+ if bound.lower <= value <= bound.upper:
+ return
+ else:
+ raise safety_error.OutOfBoundError("{} is out of bound {} for {}".format(
+ value, bound, name))
+
+
+def convert_to_motor_action(
+ robot: typing.Any,
+ action: typing.Sequence[float],
+ control_mode: robot_config.MotorControlMode,
+):
+ """Converts the input action to generic MotorAction classes.
+
+ Args:
+ robot: An robot instance.
+ action: The motor commands sent to the robot.
+ control_mode: The motor control mode.
+
+ Returns:
+ The list of converted MotorAction instances.
+ """
+ motor_action_list = []
+ if control_mode == robot_config.MotorControlMode.POSITION:
+ for motor_id, position in enumerate(action):
+ motor_action_list.append(
+ data_types.MotorAction(
+ timestamp=robot.last_action_time,
+ position=position,
+ position_gain=robot.GetMotorPositionGains()[motor_id],
+ velocity=0,
+ velocity_gain=robot.GetMotorVelocityGains()[motor_id],
+ torque=0))
+
+ if (control_mode == robot_config.MotorControlMode.TORQUE or
+ control_mode == robot_config.MotorControlMode.PWM):
+ for motor_id, torque in enumerate(action):
+ motor_action_list.append(
+ data_types.MotorAction(
+ timestamp=robot.last_action_time,
+ position=0,
+ position_gain=0,
+ velocity=0,
+ velocity_gain=0,
+ torque=torque))
+
+ if control_mode == robot_config.MotorControlMode.HYBRID:
+ for motor_id in range(robot.num_motors):
+ position_index = (
+ motor_id * robot_config.HYBRID_ACTION_DIMENSION +
+ robot_config.HybridActionIndex.POSITION.value)
+ position_gain_index = (
+ motor_id * robot_config.HYBRID_ACTION_DIMENSION +
+ robot_config.HybridActionIndex.POSITION_GAIN.value)
+ velocity_index = (
+ motor_id * robot_config.HYBRID_ACTION_DIMENSION +
+ robot_config.HybridActionIndex.VELOCITY.value)
+ velocity_gain_index = (
+ motor_id * robot_config.HYBRID_ACTION_DIMENSION +
+ robot_config.HybridActionIndex.VELOCITY_GAIN.value)
+ torque_index = (
+ motor_id * robot_config.HYBRID_ACTION_DIMENSION +
+ robot_config.HybridActionIndex.TORQUE.value)
+ motor_action_list.append(
+ data_types.MotorAction(
+ timestamp=robot.last_action_time,
+ position=action[position_index],
+ position_gain=action[position_gain_index],
+ velocity=action[velocity_index],
+ velocity_gain=action[velocity_gain_index],
+ torque=action[torque_index]))
+
+ return motor_action_list
+
+
+class MovingWindowFilter(object):
+ """A stable O(1) moving filter for incoming data streams.
+
+ We implement the Neumaier's algorithm to calculate the moving window average,
+ which is numerically stable.
+
+ """
+
+ def __init__(self, window_size: int):
+ """Initializes the class.
+
+ Args:
+ window_size: The moving window size.
+ """
+ assert window_size > 0
+ self._window_size = window_size
+ self._value_deque = collections.deque(maxlen=window_size)
+ # The moving window sum.
+ self._sum = 0
+ # The correction term to compensate numerical precision loss during
+ # calculation.
+ self._correction = 0
+
+ def _neumaier_sum(self, value: float):
+ """Update the moving window sum using Neumaier's algorithm.
+
+ For more details please refer to:
+ https://en.wikipedia.org/wiki/Kahan_summation_algorithm#Further_enhancements
+
+ Args:
+ value: The new value to be added to the window.
+ """
+
+ new_sum = self._sum + value
+ if abs(self._sum) >= abs(value):
+ # If self._sum is bigger, low-order digits of value are lost.
+ self._correction += (self._sum - new_sum) + value
+ else:
+ # low-order digits of sum are lost
+ self._correction += (value - new_sum) + self._sum
+
+ self._sum = new_sum
+
+ def calculate_average(self, new_value: float) -> float:
+ """Computes the moving window average in O(1) time.
+
+ Args:
+ new_value: The new value to enter the moving window.
+
+ Returns:
+ The average of the values in the window.
+
+ """
+ deque_len = len(self._value_deque)
+ if deque_len < self._value_deque.maxlen:
+ pass
+ else:
+ # The left most value to be subtracted from the moving sum.
+ self._neumaier_sum(-self._value_deque[0])
+
+ self._neumaier_sum(new_value)
+ self._value_deque.append(new_value)
+
+ return (self._sum + self._correction) / self._window_size
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/robots/time_ordered_buffer.py b/examples/pybullet/gym/pybullet_envs/minitaur/robots/time_ordered_buffer.py
new file mode 100644
index 000000000..52cf6b86d
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/robots/time_ordered_buffer.py
@@ -0,0 +1,263 @@
+# Lint as: python3
+"""The common class to manage the a stream of past data."""
+
+import collections
+from typing import Any, List, Sequence, Union
+import dataclasses
+import gin
+import numpy as np
+
+
+@dataclasses.dataclass
+class BufferTuple:
+ value_0: Any
+ value_1: Any
+ coeff: float
+
+
+FloatOrArray = Union[float, Sequence[float]]
+BufferTupleOrArray = Union[BufferTuple, Sequence[BufferTuple]]
+TIME_IDX = 0
+VALUE_IDX = 1
+
+
+@gin.configurable
+class TimeOrderedBuffer(object):
+ """A buffer to hold and extract history data."""
+
+ def __init__(self,
+ max_buffer_timespan: float,
+ error_on_timestamp_reversal: bool = True,
+ error_on_duplicate_timestamp: bool = True,
+ replace_value_on_duplicate_timestamp: bool = False,
+ ):
+ """Initializes the class.
+
+ Args:
+ max_buffer_timespan: Maximum amount of buffer by time to keep.
+ error_on_timestamp_reversal: Whether to throw error if inverted timestamps
+ are found.
+ error_on_duplicate_timestamp: Whether to throw error if the incoming
+ data has the same timestamp as the latest timestamp in the buffer.
+ replace_value_on_duplicate_timestamp: Whether to keep the new value when
+ a duplicate timestamp has occurred. This only applies if we are not
+ throwing an error on duplicate timestamps.
+ """
+ if max_buffer_timespan < 0:
+ raise ValueError(
+ "Invalid max_buffer_timespan: {}".format(max_buffer_timespan))
+ self._max_buffer_timespan = max_buffer_timespan
+ self._error_on_timestamp_reversal = error_on_timestamp_reversal
+ self._error_on_duplicate_timestamp = error_on_duplicate_timestamp
+ self._replace_value_on_duplicate_timestamp = (
+ replace_value_on_duplicate_timestamp)
+ # TODO(tsangwei): Look for a ring buffer implementation.
+ self._buffer = collections.deque()
+
+ def reset(self):
+ self._buffer.clear()
+
+ def _compute_coeff(self,
+ newer_time: float,
+ older_time: float,
+ target_time: float,
+ ) -> float:
+ """Compute the coefficient value between the two timestamps.
+
+ Args:
+ newer_time: The newer timestamp.
+ older_time: The older timestamp.
+ target_time: Target timestamp that is between the newer and older values.
+
+ Returns:
+ The coefficient as a float.
+ """
+ coeff = 0.0
+ # Prevents divide by 0 error.
+ if newer_time != older_time:
+ assert older_time <= target_time <= newer_time
+ coeff = (newer_time - target_time) / (newer_time - older_time)
+ return coeff
+
+ def _pack_data(self,
+ obs_newer: Any,
+ obs_older: Any,
+ target_time: float,
+ ) -> BufferTuple:
+ """Packs up buffer data as BufferTuple dataclass.
+
+ Args:
+ obs_newer: Timestamp and value of newer observation.
+ obs_older: Timestamp and value of older observation.
+ target_time: Target timestamp of the observation we are looking for.
+
+ Returns:
+ BufferTuple dataclass.
+ """
+ coeff = self._compute_coeff(newer_time=obs_newer[TIME_IDX],
+ older_time=obs_older[TIME_IDX],
+ target_time=target_time)
+ return BufferTuple(value_0=obs_newer[VALUE_IDX],
+ value_1=obs_older[VALUE_IDX],
+ coeff=coeff)
+
+ def _find_values_at(self, timestamp_targets: Sequence[float]) -> List[Any]:
+ """Get the lower/upper bound values for given target timestamp.
+
+ Args:
+ timestamp_targets: Actual timestamp value to match against.
+
+ Returns:
+ List of BufferTuple dataclass.
+ """
+ results = [None] * len(timestamp_targets)
+ oldest_obs = self._buffer[0]
+ latest_obs = self._buffer[-1]
+
+ search_start_idx = None
+ search_end_idx = None
+
+ # Check to make sure we do not try to search for values outside of the
+ # current buffer.
+ for i in range(len(timestamp_targets)):
+ # Oldest observation have the smallest timestamp.
+ if timestamp_targets[i] <= oldest_obs[TIME_IDX]:
+ results[i] = self._pack_data(obs_newer=oldest_obs,
+ obs_older=oldest_obs,
+ target_time=timestamp_targets[i])
+ elif timestamp_targets[i] >= latest_obs[TIME_IDX]:
+ results[i] = self._pack_data(obs_newer=latest_obs,
+ obs_older=latest_obs,
+ target_time=timestamp_targets[i])
+ else:
+ if search_end_idx is None:
+ search_end_idx = i
+ search_start_idx = i
+
+ if search_end_idx is not None:
+ results = self._walkthrough_buffer(timestamp_targets=timestamp_targets,
+ search_start_idx=search_start_idx,
+ search_end_idx=search_end_idx,
+ results=results)
+
+ return results
+
+ def _walkthrough_buffer(self,
+ timestamp_targets: List[float],
+ search_start_idx: int,
+ search_end_idx: int,
+ results: List[BufferTuple],
+ ) -> List[BufferTuple]:
+ """Actual method to walk through the buffer looking for requested values.
+
+ Args:
+ timestamp_targets: List of timestamps to search for in buffer.
+ search_start_idx: Index number for timestamp_targets to start searching
+ from.
+ search_end_idx: Index number for timestamp_targets to stop searching at.
+ results: List of BufferTuple values that covers out of bound results.
+
+ Returns:
+ List of BufferTuple values.
+ """
+ value_older = None
+ target_idx = search_start_idx
+ target_timestamp = timestamp_targets[target_idx]
+ value_older = self._buffer[0]
+
+ # Searching from oldest timestamp to latest timestamp.
+ for value_newer in self._buffer:
+ # Catch edge case scenario where multiple timestamp targets are between
+ # the same two buffer timestamps. (b/157104935)
+ while value_newer[TIME_IDX] >= target_timestamp:
+ # Catch special edge case scenario if using older_obs_blender method.
+ obs_older = value_newer if (
+ value_newer[TIME_IDX] == target_timestamp) else value_older
+ results[target_idx] = self._pack_data(obs_newer=value_newer,
+ obs_older=obs_older,
+ target_time=target_timestamp)
+ if target_idx - 1 >= search_end_idx:
+ target_idx -= 1
+ target_timestamp = timestamp_targets[target_idx]
+ else:
+ return results
+ value_older = value_newer
+
+ return results
+
+ def add(self, timestamp: float, value: Any):
+ """Inserts timestamp and value into buffer.
+
+ Args:
+ timestamp: Timestamp of the data value.
+ value: Data value to be saved into the buffer.
+ """
+ if self._buffer:
+ last_timestamp = self._buffer[-1][TIME_IDX]
+ if last_timestamp == timestamp:
+ if (not np.array_equal(self._buffer[-1][VALUE_IDX], value) and
+ self._error_on_duplicate_timestamp):
+ raise ValueError("Duplicate timestamp detected: {}".format(timestamp))
+ else:
+ # Duplicate message detected.
+ if self._replace_value_on_duplicate_timestamp:
+ self._buffer[-1] = (timestamp, value)
+ return
+ if last_timestamp > timestamp and self._error_on_timestamp_reversal:
+ raise ValueError(
+ "Time reversal detected: new timestamp is {} vs last timestamp {}"
+ .format(timestamp, last_timestamp))
+ # Dropping old buffer data that exceed buffer timespan limit and making
+ # sure the buffer does not go empty.
+ while (len(self._buffer) > 1 and self._max_buffer_timespan <
+ (timestamp - self._buffer[1][TIME_IDX])):
+ self._buffer.popleft()
+ self._buffer.append((timestamp, value))
+
+ def get_delayed_value(self, latency: FloatOrArray) -> BufferTupleOrArray:
+ """Retrieves value in the history buffer according to latency.
+
+ Finds the closest pair of values that are some time (i.e. latency) ago from
+ the most recent timestamp. Suppose the history buffer looks like this:
+
+ [(0, val_x),...,(0.6, val_k-1), (0.7, val_k), (0.8, val_k+1),...,(2, val_0)]
+
+ And the latency is '1.33', then this API will locate the values with
+ timestamps close to 2 - 1.33 = 0.67. So in this case, it will return the
+ pair (0.7, val_k) and (0.6, val_k+1), as well as a blending coefficient
+ which is calculated by (0.7 - 0.67) / (0.7 - 0.6) = 0.3. This blending coeff
+ can be used to linearly interpolate the returned values, i.e. val_1 * (1 -
+ coeff) + val_2 * coeff, if the multiply operator is defined.
+
+ Args:
+ latency: The time interval(s) to look backwards in the history buffer from
+ the most recent timestamp.
+
+ Returns:
+ An array of BufferTuple dataclass.
+
+ Raises:
+ ValueError: if the latency is negative.
+ BufferError: if the buffer is empty.
+ """
+ buffer_len = len(self._buffer)
+ if buffer_len == 0:
+ raise BufferError("The buffer is empty. Have you called 'add'?")
+
+ single_latency = isinstance(latency, (int, float))
+
+ if single_latency:
+ if latency < 0:
+ raise ValueError("Latency cannot be negative.")
+ else:
+ if any(value < 0 for value in latency):
+ raise ValueError("Latency list contains negative values.")
+ if latency != sorted(latency):
+ raise ValueError("Invalid unsorted latency list.")
+
+ target_list = [latency] if single_latency else latency
+ current_time = self._buffer[-1][TIME_IDX]
+ target_list = [current_time - offset for offset in target_list]
+
+ result = self._find_values_at(target_list)
+ return result[0] if single_latency else result
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/robots/timestamp.proto b/examples/pybullet/gym/pybullet_envs/minitaur/robots/timestamp.proto
new file mode 100644
index 000000000..9c4c75cc3
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/robots/timestamp.proto
@@ -0,0 +1,147 @@
+// Protocol Buffers - Google's data interchange format
+// Copyright 2008 Google Inc. All rights reserved.
+// https://developers.google.com/protocol-buffers/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright
+// notice, this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above
+// copyright notice, this list of conditions and the following disclaimer
+// in the documentation and/or other materials provided with the
+// distribution.
+// * Neither the name of Google Inc. nor the names of its
+// contributors may be used to endorse or promote products derived from
+// this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+syntax = "proto3";
+
+package google.protobuf;
+
+option csharp_namespace = "Google.Protobuf.WellKnownTypes";
+option cc_enable_arenas = true;
+option go_package = "google.golang.org/protobuf/types/known/timestamppb";
+option java_package = "com.google.protobuf";
+option java_outer_classname = "TimestampProto";
+option java_multiple_files = true;
+option objc_class_prefix = "GPB";
+
+// A Timestamp represents a point in time independent of any time zone or local
+// calendar, encoded as a count of seconds and fractions of seconds at
+// nanosecond resolution. The count is relative to an epoch at UTC midnight on
+// January 1, 1970, in the proleptic Gregorian calendar which extends the
+// Gregorian calendar backwards to year one.
+//
+// All minutes are 60 seconds long. Leap seconds are "smeared" so that no leap
+// second table is needed for interpretation, using a [24-hour linear
+// smear](https://developers.google.com/time/smear).
+//
+// The range is from 0001-01-01T00:00:00Z to 9999-12-31T23:59:59.999999999Z. By
+// restricting to that range, we ensure that we can convert to and from [RFC
+// 3339](https://www.ietf.org/rfc/rfc3339.txt) date strings.
+//
+// # Examples
+//
+// Example 1: Compute Timestamp from POSIX `time()`.
+//
+// Timestamp timestamp;
+// timestamp.set_seconds(time(NULL));
+// timestamp.set_nanos(0);
+//
+// Example 2: Compute Timestamp from POSIX `gettimeofday()`.
+//
+// struct timeval tv;
+// gettimeofday(&tv, NULL);
+//
+// Timestamp timestamp;
+// timestamp.set_seconds(tv.tv_sec);
+// timestamp.set_nanos(tv.tv_usec * 1000);
+//
+// Example 3: Compute Timestamp from Win32 `GetSystemTimeAsFileTime()`.
+//
+// FILETIME ft;
+// GetSystemTimeAsFileTime(&ft);
+// UINT64 ticks = (((UINT64)ft.dwHighDateTime) << 32) | ft.dwLowDateTime;
+//
+// // A Windows tick is 100 nanoseconds. Windows epoch 1601-01-01T00:00:00Z
+// // is 11644473600 seconds before Unix epoch 1970-01-01T00:00:00Z.
+// Timestamp timestamp;
+// timestamp.set_seconds((INT64) ((ticks / 10000000) - 11644473600LL));
+// timestamp.set_nanos((INT32) ((ticks % 10000000) * 100));
+//
+// Example 4: Compute Timestamp from Java `System.currentTimeMillis()`.
+//
+// long millis = System.currentTimeMillis();
+//
+// Timestamp timestamp = Timestamp.newBuilder().setSeconds(millis / 1000)
+// .setNanos((int) ((millis % 1000) * 1000000)).build();
+//
+//
+// Example 5: Compute Timestamp from Java `Instant.now()`.
+//
+// Instant now = Instant.now();
+//
+// Timestamp timestamp =
+// Timestamp.newBuilder().setSeconds(now.getEpochSecond())
+// .setNanos(now.getNano()).build();
+//
+//
+// Example 6: Compute Timestamp from current time in Python.
+//
+// timestamp = Timestamp()
+// timestamp.GetCurrentTime()
+//
+// # JSON Mapping
+//
+// In JSON format, the Timestamp type is encoded as a string in the
+// [RFC 3339](https://www.ietf.org/rfc/rfc3339.txt) format. That is, the
+// format is "{year}-{month}-{day}T{hour}:{min}:{sec}[.{frac_sec}]Z"
+// where {year} is always expressed using four digits while {month}, {day},
+// {hour}, {min}, and {sec} are zero-padded to two digits each. The fractional
+// seconds, which can go up to 9 digits (i.e. up to 1 nanosecond resolution),
+// are optional. The "Z" suffix indicates the timezone ("UTC"); the timezone
+// is required. A proto3 JSON serializer should always use UTC (as indicated by
+// "Z") when printing the Timestamp type and a proto3 JSON parser should be
+// able to accept both UTC and other timezones (as indicated by an offset).
+//
+// For example, "2017-01-15T01:30:15.01Z" encodes 15.01 seconds past
+// 01:30 UTC on January 15, 2017.
+//
+// In JavaScript, one can convert a Date object to this format using the
+// standard
+// [toISOString()](https://developer.mozilla.org/en-US/docs/Web/JavaScript/Reference/Global_Objects/Date/toISOString)
+// method. In Python, a standard `datetime.datetime` object can be converted
+// to this format using
+// [`strftime`](https://docs.python.org/2/library/time.html#time.strftime) with
+// the time format spec '%Y-%m-%dT%H:%M:%S.%fZ'. Likewise, in Java, one can use
+// the Joda Time's [`ISODateTimeFormat.dateTime()`](
+// http://www.joda.org/joda-time/apidocs/org/joda/time/format/ISODateTimeFormat.html#dateTime%2D%2D
+// ) to obtain a formatter capable of generating timestamps in this format.
+//
+//
+message Timestamp {
+ // Represents seconds of UTC time since Unix epoch
+ // 1970-01-01T00:00:00Z. Must be from 0001-01-01T00:00:00Z to
+ // 9999-12-31T23:59:59Z inclusive.
+ int64 seconds = 1;
+
+ // Non-negative fractions of a second at nanosecond resolution. Negative
+ // second values with fractions must still have non-negative nanos values
+ // that count forward in time. Must be from 0 to 999,999,999
+ // inclusive.
+ int32 nanos = 2;
+}
\ No newline at end of file
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/robots/timestamp_pb2.py b/examples/pybullet/gym/pybullet_envs/minitaur/robots/timestamp_pb2.py
new file mode 100644
index 000000000..b14188991
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/robots/timestamp_pb2.py
@@ -0,0 +1,78 @@
+# -*- coding: utf-8 -*-
+# Generated by the protocol buffer compiler. DO NOT EDIT!
+# source: timestamp.proto
+"""Generated protocol buffer code."""
+from google.protobuf import descriptor as _descriptor
+from google.protobuf import message as _message
+from google.protobuf import reflection as _reflection
+from google.protobuf import symbol_database as _symbol_database
+# @@protoc_insertion_point(imports)
+
+_sym_db = _symbol_database.Default()
+
+
+
+
+DESCRIPTOR = _descriptor.FileDescriptor(
+ name='timestamp.proto',
+ package='google.protobuf',
+ syntax='proto3',
+ serialized_options=b'\n\023com.google.protobufB\016TimestampProtoP\001Z2google.golang.org/protobuf/types/known/timestamppb\370\001\001\242\002\003GPB\252\002\036Google.Protobuf.WellKnownTypes',
+ create_key=_descriptor._internal_create_key,
+ serialized_pb=b'\n\x0ftimestamp.proto\x12\x0fgoogle.protobuf\"+\n\tTimestamp\x12\x0f\n\x07seconds\x18\x01 \x01(\x03\x12\r\n\x05nanos\x18\x02 \x01(\x05\x42\x85\x01\n\x13\x63om.google.protobufB\x0eTimestampProtoP\x01Z2google.golang.org/protobuf/types/known/timestamppb\xf8\x01\x01\xa2\x02\x03GPB\xaa\x02\x1eGoogle.Protobuf.WellKnownTypesb\x06proto3'
+)
+
+
+
+
+_TIMESTAMP = _descriptor.Descriptor(
+ name='Timestamp',
+ full_name='google.protobuf.Timestamp',
+ filename=None,
+ file=DESCRIPTOR,
+ containing_type=None,
+ create_key=_descriptor._internal_create_key,
+ fields=[
+ _descriptor.FieldDescriptor(
+ name='seconds', full_name='google.protobuf.Timestamp.seconds', index=0,
+ number=1, type=3, cpp_type=2, label=1,
+ has_default_value=False, default_value=0,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='nanos', full_name='google.protobuf.Timestamp.nanos', index=1,
+ number=2, type=5, cpp_type=1, label=1,
+ has_default_value=False, default_value=0,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ ],
+ extensions=[
+ ],
+ nested_types=[],
+ enum_types=[
+ ],
+ serialized_options=None,
+ is_extendable=False,
+ syntax='proto3',
+ extension_ranges=[],
+ oneofs=[
+ ],
+ serialized_start=36,
+ serialized_end=79,
+)
+
+DESCRIPTOR.message_types_by_name['Timestamp'] = _TIMESTAMP
+_sym_db.RegisterFileDescriptor(DESCRIPTOR)
+
+Timestamp = _reflection.GeneratedProtocolMessageType('Timestamp', (_message.Message,), {
+ 'DESCRIPTOR' : _TIMESTAMP,
+ '__module__' : 'timestamp_pb2'
+ # @@protoc_insertion_point(class_scope:google.protobuf.Timestamp)
+ })
+_sym_db.RegisterMessage(Timestamp)
+
+
+DESCRIPTOR._options = None
+# @@protoc_insertion_point(module_scope)
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/robots/utilities/__init__.py b/examples/pybullet/gym/pybullet_envs/minitaur/robots/utilities/__init__.py
new file mode 100644
index 000000000..e69de29bb
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/robots/utilities/action_filter.py b/examples/pybullet/gym/pybullet_envs/minitaur/robots/utilities/action_filter.py
new file mode 100644
index 000000000..6fb100cb3
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/robots/utilities/action_filter.py
@@ -0,0 +1,239 @@
+"""Two types of filters which can be applied to policy output sequences.
+
+1. Simple exponential filter
+2. Butterworth filter - lowpass or bandpass
+
+The implementation of the butterworth filter follows scipy's lfilter
+https://github.com/scipy/scipy/blob/v1.2.1/scipy/signal/signaltools.py
+
+We re-implement the logic in order to explicitly manage the y states
+
+The filter implements::
+ a[0]*y[n] = b[0]*x[n] + b[1]*x[n-1] + ... + b[M]*x[n-M]
+ - a[1]*y[n-1] - ... - a[N]*y[n-N]
+
+We assume M == N.
+"""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+import collections
+from absl import logging
+import gin
+import numpy as np
+from scipy.signal import butter
+
+ACTION_FILTER_ORDER = 2
+ACTION_FILTER_LOW_CUT = 0.0
+ACTION_FILTER_HIGH_CUT = 4.0
+
+
+@gin.configurable
+class ActionFilter(object):
+ """Implements a generic lowpass or bandpass action filter."""
+
+ def __init__(self, a, b, order, num_joints, ftype='lowpass'):
+ """Initializes filter.
+
+ Either one per joint or same for all joints.
+
+ Args:
+ a: filter output history coefficients
+ b: filter input coefficients
+ order: filter order
+ num_joints: robot DOF
+ ftype: filter type. 'lowpass' or 'bandpass'
+ """
+ self.num_joints = num_joints
+ if isinstance(a, list):
+ self.a = a
+ self.b = b
+ else:
+ self.a = [a]
+ self.b = [b]
+
+ # Either a set of parameters per joint must be specified as a list
+ # Or one filter is applied to every joint
+ if not ((len(self.a) == len(self.b) == num_joints) or (
+ len(self.a) == len(self.b) == 1)):
+ raise ValueError('Incorrect number of filter values specified')
+
+ # Normalize by a[0]
+ for i in range(len(self.a)):
+ self.b[i] /= self.a[i][0]
+ self.a[i] /= self.a[i][0]
+
+ # Convert single filter to same format as filter per joint
+ if len(self.a) == 1:
+ self.a *= num_joints
+ self.b *= num_joints
+ self.a = np.stack(self.a)
+ self.b = np.stack(self.b)
+
+ if ftype == 'bandpass':
+ assert len(self.b[0]) == len(self.a[0]) == 2 * order + 1
+ self.hist_len = 2 * order
+ elif ftype == 'lowpass':
+ assert len(self.b[0]) == len(self.a[0]) == order + 1
+ self.hist_len = order
+ else:
+ raise ValueError('%s filter type not supported' % (ftype))
+
+ logging.info('Filter shapes: a: %s, b: %s', self.a.shape, self.b.shape)
+ logging.info('Filter type:%s', ftype)
+
+ self.yhist = collections.deque(maxlen=self.hist_len)
+ self.xhist = collections.deque(maxlen=self.hist_len)
+ self.reset()
+
+ def reset(self):
+ """Resets the history buffers to 0."""
+ self.yhist.clear()
+ self.xhist.clear()
+ for _ in range(self.hist_len):
+ self.yhist.appendleft(np.zeros((self.num_joints, 1)))
+ self.xhist.appendleft(np.zeros((self.num_joints, 1)))
+
+ def filter(self, x):
+ """Returns filtered x."""
+ xs = np.concatenate(list(self.xhist), axis=-1)
+ ys = np.concatenate(list(self.yhist), axis=-1)
+ y = np.multiply(x, self.b[:, 0]) + np.sum(
+ np.multiply(xs, self.b[:, 1:]), axis=-1) - np.sum(
+ np.multiply(ys, self.a[:, 1:]), axis=-1)
+ self.xhist.appendleft(x.reshape((self.num_joints, 1)).copy())
+ self.yhist.appendleft(y.reshape((self.num_joints, 1)).copy())
+ return y
+
+ def init_history(self, x):
+ x = np.expand_dims(x, axis=-1)
+ for i in range(self.hist_len):
+ self.xhist[i] = x
+ self.yhist[i] = x
+ return
+
+
+@gin.configurable
+class ActionFilterButter(ActionFilter):
+ """Butterworth filter."""
+
+ def __init__(self,
+ lowcut=None,
+ highcut=None,
+ sampling_rate=None,
+ order=ACTION_FILTER_ORDER,
+ num_joints=None):
+ """Initializes a butterworth filter.
+
+ Either one per joint or same for all joints.
+
+ Args:
+ lowcut: list of strings defining the low cutoff frequencies.
+ The list must contain either 1 element (same filter for all joints)
+ or num_joints elements
+ 0 for lowpass, > 0 for bandpass. Either all values must be 0
+ or all > 0
+ highcut: list of strings defining the high cutoff frequencies.
+ The list must contain either 1 element (same filter for all joints)
+ or num_joints elements
+ All must be > 0
+ sampling_rate: frequency of samples in Hz
+ order: filter order
+ num_joints: robot DOF
+ """
+ self.lowcut = ([float(x) for x in lowcut]
+ if lowcut is not None else [ACTION_FILTER_LOW_CUT])
+ self.highcut = ([float(x) for x in highcut]
+ if highcut is not None else [ACTION_FILTER_HIGH_CUT])
+ if len(self.lowcut) != len(self.highcut):
+ raise ValueError('Number of lowcut and highcut filter values should '
+ 'be the same')
+
+ if sampling_rate is None:
+ raise ValueError('sampling_rate should be provided.')
+
+ if num_joints is None:
+ raise ValueError('num_joints should be provided.')
+
+ if np.any(self.lowcut):
+ if not np.all(self.lowcut):
+ raise ValueError('All the filters must be of the same type: '
+ 'lowpass or bandpass')
+ self.ftype = 'bandpass'
+ else:
+ self.ftype = 'lowpass'
+
+ a_coeffs = []
+ b_coeffs = []
+ for i, (l, h) in enumerate(zip(self.lowcut, self.highcut)):
+ if h <= 0.0:
+ raise ValueError('Highcut must be > 0')
+
+ b, a = self.butter_filter(l, h, sampling_rate, order)
+ logging.info(
+ 'Butterworth filter: joint: %d, lowcut: %f, highcut: %f, '
+ 'sampling rate: %d, order: %d, num joints: %d', i, l, h,
+ sampling_rate, order, num_joints)
+ b_coeffs.append(b)
+ a_coeffs.append(a)
+
+ super(ActionFilterButter, self).__init__(
+ a_coeffs, b_coeffs, order, num_joints, self.ftype)
+
+ def butter_filter(self, lowcut, highcut, fs, order=5):
+ """Returns the coefficients of a butterworth filter.
+
+ If lowcut = 0, the function returns the coefficients of a low pass filter.
+ Otherwise, the coefficients of a band pass filter are returned.
+ Highcut should be > 0
+
+ Args:
+ lowcut: low cutoff frequency
+ highcut: high cutoff frequency
+ fs: sampling rate
+ order: filter order
+ Return:
+ b, a: parameters of a butterworth filter
+ """
+ nyq = 0.5 * fs
+ low = lowcut / nyq
+ high = highcut / nyq
+ if low:
+ b, a = butter(order, [low, high], btype='band')
+ else:
+ b, a = butter(order, [high], btype='low')
+ return b, a
+
+
+class ActionFilterExp(ActionFilter):
+ """Filter by way of simple exponential smoothing.
+
+ y = alpha * x + (1 - alpha) * previous_y
+ """
+
+ def __init__(self, alpha, num_joints):
+ """Initialize the filter.
+
+ Args:
+ alpha: list of strings defining the alphas.
+ The list must contain either 1 element (same filter for all joints)
+ or num_joints elements
+ 0 < alpha <= 1
+ num_joints: robot DOF
+ """
+ self.alphas = [float(x) for x in alpha]
+ logging.info('Exponential filter: alpha: %d', self.alphas)
+
+ a_coeffs = []
+ b_coeffs = []
+ for a in self.alphas:
+ a_coeffs.append(np.asarray([1., a - 1.]))
+ b_coeffs.append(np.asarray([a, 0]))
+
+ order = 1
+ self.ftype = 'lowpass'
+
+ super(ActionFilterExp, self).__init__(
+ a_coeffs, b_coeffs, order, num_joints, self.ftype)
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/robots/utilities/kinematics.py b/examples/pybullet/gym/pybullet_envs/minitaur/robots/utilities/kinematics.py
new file mode 100644
index 000000000..be5824400
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/robots/utilities/kinematics.py
@@ -0,0 +1,127 @@
+"""The inverse kinematic utilities."""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+import numpy as np
+import typing
+
+_IDENTITY_ORIENTATION = (0, 0, 0, 1)
+
+
+def joint_angles_from_link_position(
+ robot: typing.Any,
+ link_position: typing.Sequence[float],
+ link_id: int,
+ joint_ids: typing.Sequence[int],
+ position_in_world_frame=False,
+ base_translation: typing.Sequence[float] = (0, 0, 0),
+ base_rotation: typing.Sequence[float] = (0, 0, 0, 1)):
+ """Uses Inverse Kinematics to calculate joint angles.
+
+ Args:
+ robot: A robot instance.
+ link_position: The (x, y, z) of the link in the body or the world frame,
+ depending on whether the argument position_in_world_frame is true.
+ link_id: The link id as returned from loadURDF.
+ joint_ids: The positional index of the joints. This can be different from
+ the joint unique ids.
+ position_in_world_frame: Whether the input link_position is specified
+ in the world frame or the robot's base frame.
+ base_translation: Additional base translation.
+ base_rotation: Additional base rotation.
+
+ Returns:
+ A list of joint angles.
+ """
+ if not position_in_world_frame:
+ # Projects to local frame.
+ base_position, base_orientation = robot.GetBasePosition(
+ ), robot.GetBaseOrientation()
+ base_position, base_orientation = robot.pybullet_client.multiplyTransforms(
+ base_position, base_orientation, base_translation, base_rotation)
+
+ # Projects to world space.
+ world_link_pos, _ = robot.pybullet_client.multiplyTransforms(
+ base_position, base_orientation, link_position, _IDENTITY_ORIENTATION)
+ else:
+ world_link_pos = link_position
+
+ ik_solver = 0
+ all_joint_angles = robot.pybullet_client.calculateInverseKinematics(
+ robot.quadruped, link_id, world_link_pos, solver=ik_solver)
+
+ # Extract the relevant joint angles.
+ joint_angles = [all_joint_angles[i] for i in joint_ids]
+ return joint_angles
+
+
+def link_position_in_world_frame(
+ robot: typing.Any,
+ link_id: int,
+):
+ """Computes the link's position in the world frame.
+
+ Args:
+ robot: A robot instance.
+ link_id: The link id to calculate its position.
+
+ Returns:
+ The position of the link in the world frame.
+ """
+ return np.array(
+ robot.pybullet_client.getLinkState(robot.quadruped, link_id)[0])
+
+
+def link_position_in_base_frame(
+ robot: typing.Any,
+ link_id: int,
+):
+ """Computes the link's local position in the robot frame.
+
+ Args:
+ robot: A robot instance.
+ link_id: The link to calculate its relative position.
+
+ Returns:
+ The relative position of the link.
+ """
+ base_position, base_orientation = robot.GetBasePosition(
+ ), robot.GetBaseOrientation()
+ inverse_translation, inverse_rotation = robot.pybullet_client.invertTransform(
+ base_position, base_orientation)
+
+ link_state = robot.pybullet_client.getLinkState(robot.quadruped, link_id)
+ link_position = link_state[0]
+ link_local_position, _ = robot.pybullet_client.multiplyTransforms(
+ inverse_translation, inverse_rotation, link_position, (0, 0, 0, 1))
+
+ return np.array(link_local_position)
+
+
+def compute_jacobian(
+ robot: typing.Any,
+ link_id: int,
+):
+ """Computes the Jacobian matrix for the given link.
+
+ Args:
+ robot: A robot instance.
+ link_id: The link id as returned from loadURDF.
+
+ Returns:
+ The 3 x N transposed Jacobian matrix. where N is the total DoFs of the
+ robot. For a quadruped, the first 6 columns of the matrix corresponds to
+ the CoM translation and rotation. The columns corresponds to a leg can be
+ extracted with indices [6 + leg_id * 3: 6 + leg_id * 3 + 3].
+ """
+
+ all_joint_angles = [state[0] for state in robot.joint_states]
+ zero_vec = [0] * len(all_joint_angles)
+ jv, _ = robot.pybullet_client.calculateJacobian(robot.quadruped, link_id,
+ (0, 0, 0), all_joint_angles,
+ zero_vec, zero_vec)
+ jacobian = np.array(jv)
+ assert jacobian.shape[0] == 3
+ return jacobian
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/robots/utilities/kinematics_utils.py b/examples/pybullet/gym/pybullet_envs/minitaur/robots/utilities/kinematics_utils.py
new file mode 100644
index 000000000..088f2bcb9
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/robots/utilities/kinematics_utils.py
@@ -0,0 +1,208 @@
+# Lint as: python3
+"""The inverse kinematic utilities."""
+from typing import Optional, Sequence
+
+import numpy as np
+
+from pybullet_utils import bullet_client
+
+_IDENTITY_ROTATION_QUAT = (0, 0, 0, 1)
+_IK_SOLVER_TYPE = 0
+_LINK_POS_INDEX = 0
+
+
+def joint_angles_from_link_positions(
+ pybullet_client: bullet_client.BulletClient,
+ urdf_id: int,
+ link_ids: Sequence[int],
+ link_positions: Sequence[Sequence[float]],
+ positions_are_in_world_frame: bool = False,
+ joint_dof_ids: Optional[Sequence[int]] = None,
+) -> np.ndarray:
+ """Uses Inverse Kinematics to calculate joint angles.
+
+ Args:
+ pybullet_client: The bullet client.
+ urdf_id: The unique id returned after loading URDF.
+ link_ids: The link ids to compute the IK.
+ link_positions: The (x, y, z) of the links in the body or the world frame,
+ depending on whether the argument link_position_in_world_frame is true.
+ positions_are_in_world_frame: Whether the input link positions are specified
+ in the world frame or the robot's base frame.
+ joint_dof_ids: The degrees of freedom index of the joints we want to extract
+ the angles. This can be different from the joint unique ids. For example,
+ a fixed joint will increase the joint unique id but will not increase the
+ number of degree of freedoms. The joint dof id can be extracted in
+ PyBullet by getJointInfo, which corresponds to the "qIndex" in the
+ returned values. If not specified, will return all movable joint angles.
+
+ Returns:
+ A list of joint angles.
+ """
+ if positions_are_in_world_frame:
+ world_link_positions = link_positions
+ else:
+ # The PyBullet inverse Kinematics Calculation depends on the current URDF
+ # position/orientation, and we cannot pass them to the API. So we have to
+ # always query the current base position/orientation to compute world frame
+ # link positions.
+ urdf_base_position, urdf_base_orientation = (
+ pybullet_client.getBasePositionAndOrientation(urdf_id))
+ world_link_positions = []
+ for link_position in link_positions:
+ world_link_position, _ = pybullet_client.multiplyTransforms(
+ urdf_base_position, urdf_base_orientation, link_position,
+ _IDENTITY_ROTATION_QUAT)
+ world_link_positions.append(world_link_position)
+
+ # Notice that the API expects the link positions in the world frame.
+ all_joint_angles = pybullet_client.calculateInverseKinematics2(
+ urdf_id, link_ids, world_link_positions, solver=_IK_SOLVER_TYPE)
+
+ # Extract the relevant joint angles.
+ if joint_dof_ids is None:
+ return np.array(all_joint_angles)
+
+ return np.array([all_joint_angles[i] for i in joint_dof_ids])
+
+
+def link_position_in_world_frame(
+ pybullet_client: bullet_client.BulletClient,
+ urdf_id: int,
+ link_id: int,
+):
+ """Computes the link's position in the world frame.
+
+ Args:
+ pybullet_client: The bullet client.
+ urdf_id: The unique id returned after loading URDF.
+ link_id: The link id to calculate its position.
+
+ Returns:
+ The position of the link in the world frame.
+ """
+ return np.array(pybullet_client.getLinkState(urdf_id, link_id)[0])
+
+
+def link_position_in_base_frame(
+ pybullet_client: bullet_client.BulletClient,
+ urdf_id: int,
+ link_id: int,
+ base_link_id: Optional[int] = None,
+):
+ """Computes the link's local position in the robot frame.
+
+ Args:
+ pybullet_client: The bullet client.
+ urdf_id: The unique id returned after loading URDF.
+ link_id: The link to calculate its relative position.
+ base_link_id: The link id of the base. For the kinematics robot, such as
+ wheeled_robot_base, three additional joints are added to connect the world
+ and the base. The base_link_id is no longer -1, and need to be passed in.
+
+
+ Returns:
+ The relative position of the link.
+ """
+ if base_link_id is None:
+ base_position, base_orientation = (
+ pybullet_client.getBasePositionAndOrientation(urdf_id))
+ else:
+ base_link_state = pybullet_client.getLinkState(urdf_id, base_link_id)
+ base_position, base_orientation = base_link_state[0], base_link_state[1]
+
+ inverse_translation, inverse_rotation = pybullet_client.invertTransform(
+ base_position, base_orientation)
+
+ link_state = pybullet_client.getLinkState(urdf_id, link_id)
+ link_position = link_state[0]
+ link_local_position, _ = pybullet_client.multiplyTransforms(
+ inverse_translation, inverse_rotation, link_position, (0, 0, 0, 1))
+
+ return np.array(link_local_position)
+
+
+def compute_jacobian(
+ pybullet_client: bullet_client.BulletClient,
+ urdf_id: int,
+ link_id: int,
+ all_joint_positions: Sequence[float],
+ additional_translation: Optional[Sequence[float]] = (0, 0, 0),
+) -> np.ndarray:
+ """Computes the Jacobian matrix for the given point on a link.
+
+ CAVEAT: If during URDF loading process additional rotations are provided, the
+ computed Jacobian are also transformed.
+
+ Args:
+ pybullet_client: The bullet client.
+ urdf_id: The unique id returned after loading URDF.
+ link_id: The link id as returned from loadURDF.
+ all_joint_positions: all the joint positions of the robot. This should
+ include the dummy/kinematic drive joints for the wheeled robot.
+ additional_translation: The additional translation of the point in the link
+ CoM frame.
+
+ Returns:
+ The 3 x N transposed Jacobian matrix. where N is the total DoFs of the
+ robot. For a quadruped, the first 6 columns of the matrix corresponds to
+ the CoM translation and rotation. The columns corresponds to a leg can be
+ extracted with indices [6 + leg_id * 3: 6 + leg_id * 3 + 3].
+ """
+
+ zero_vec = [0] * len(all_joint_positions)
+ jv, _ = pybullet_client.calculateJacobian(
+ urdf_id,
+ link_id,
+ additional_translation,
+ all_joint_positions,
+ objVelocities=zero_vec,
+ objAccelerations=zero_vec)
+ jacobian = np.array(jv)
+ assert jacobian.shape[0] == 3
+ return jacobian
+
+
+def rotate_to_base_frame(
+ pybullet_client: bullet_client.BulletClient,
+ urdf_id: int,
+ vector: Sequence[float],
+ init_orientation_inv_quat: Optional[Sequence[float]] = (0, 0, 0, 1)
+) -> np.ndarray:
+ """Rotates the input vector to the base coordinate systems.
+
+ Note: This is different from world frame to base frame transformation, as we
+ do not apply any translation here.
+
+ Args:
+ pybullet_client: The bullet client.
+ urdf_id: The unique id returned after loading URDF.
+ vector: Input vector in the world frame.
+ init_orientation_inv_quat:
+
+ Returns:
+ A rotated vector in the base frame.
+ """
+ _, base_orientation_quat = (
+ pybullet_client.getBasePositionAndOrientation(urdf_id))
+ _, base_orientation_quat_from_init = pybullet_client.multiplyTransforms(
+ positionA=(0, 0, 0),
+ orientationA=init_orientation_inv_quat,
+ positionB=(0, 0, 0),
+ orientationB=base_orientation_quat)
+ _, inverse_base_orientation = pybullet_client.invertTransform(
+ [0, 0, 0], base_orientation_quat_from_init)
+
+ # PyBullet transforms requires simple list/tuple or it may crash.
+ if isinstance(vector, np.ndarray):
+ vector_list = vector.tolist()
+ else:
+ vector_list = vector
+
+ local_vector, _ = pybullet_client.multiplyTransforms(
+ positionA=(0, 0, 0),
+ orientationA=inverse_base_orientation,
+ positionB=vector_list,
+ orientationB=(0, 0, 0, 1),
+ )
+ return np.array(local_vector)
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/robots/utilities/urdf_utils.py b/examples/pybullet/gym/pybullet_envs/minitaur/robots/utilities/urdf_utils.py
new file mode 100644
index 000000000..f45d4ff86
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/robots/utilities/urdf_utils.py
@@ -0,0 +1,59 @@
+# Lint as: python3
+"""Utilities for robot URDF models."""
+
+from typing import Text
+
+# In the return from pybullet.getJointInfo, the name of the link whose parent is
+# that joint.
+LINK_NAME_INDEX = 12
+# Indication that link_name_to_id should return -1. This is a constant because
+# different URDFs use different names for their base links.
+BASE_LINK = ""
+
+
+def link_name_to_id(link_name: Text, robot_id: int, pybullet_client) -> int:
+ """Returns the pybullet integer link id corresponding to link_name.
+
+ Args:
+ link_name: The name of the link from the URDF. If this is BASE_LINK, returns
+ -1, the link id of the base according to pybullet convention.
+ robot_id: Integer id of the robot to which the link belongs, as returned by
+ pybullet.loadURDF().
+ pybullet_client: Client in which the robot is loaded.
+
+ Returns:
+ Integer id of the link.
+
+ Raises:
+ ValueError if the link_name is not found in the robot.
+ """
+ if link_name == BASE_LINK:
+ return -1
+ link_name_list = []
+ for i in range(pybullet_client.getNumJoints(robot_id)):
+ joint_info = pybullet_client.getJointInfo(robot_id, i)
+ link_name_i = joint_info[LINK_NAME_INDEX].decode("UTF-8")
+ if link_name_i == link_name:
+ return i
+ link_name_list.append(link_name_i)
+ raise ValueError("Link name '{}' not found in URDF. Options are: {}".format(
+ link_name, link_name_list))
+
+
+def set_collision_filter_group_mask(urdf_id: int, group: int, mask: int,
+ pybullet_client):
+ """Sets the collision filter group and mask to the robot.
+
+ TODO(tingnan): Check if this has side effects with self collision flags
+ when loading URDF.
+
+ Args:
+ urdf_id: The URDF id as returned by the loadURDF.
+ group: The collision group the robot is in. By default, all dynamics objects
+ in PyBullet use collision group 1.
+ mask: The collision bit mask to use. See go/pybullet for details.
+ pybullet_client: The bullet client to use.
+ """
+ # We includes "-1" for the base link of the URDF.
+ for link_id in range(-1, pybullet_client.getNumJoints(urdf_id)):
+ pybullet_client.setCollisionFilterGroupMask(urdf_id, link_id, group, mask)
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/robots/vector.proto b/examples/pybullet/gym/pybullet_envs/minitaur/robots/vector.proto
new file mode 100644
index 000000000..92c48f859
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/robots/vector.proto
@@ -0,0 +1,84 @@
+syntax = "proto3";
+
+package robotics.messages;
+
+option cc_enable_arenas = true;
+
+// A four-dimensional double precision vector.
+message Vector4d {
+ double x = 1;
+ double y = 2;
+ double z = 3;
+ double w = 4;
+}
+
+// A four-dimensional single precision vector.
+message Vector4f {
+ float x = 1;
+ float y = 2;
+ float z = 3;
+ float w = 4;
+}
+
+// A four-dimensional integer vector.
+message Vector4i {
+ int64 x = 1;
+ int64 y = 2;
+ int64 z = 3;
+ int64 w = 4;
+}
+
+// A three-dimensional double precision vector.
+message Vector3d {
+ double x = 1;
+ double y = 2;
+ double z = 3;
+}
+
+// A three-dimensional single precision vector.
+message Vector3f {
+ float x = 1;
+ float y = 2;
+ float z = 3;
+}
+
+// A three-dimensional integer vector.
+message Vector3i {
+ int64 x = 1;
+ int64 y = 2;
+ int64 z = 3;
+}
+
+// A two-dimensional double precision vector.
+message Vector2d {
+ double x = 1;
+ double y = 2;
+}
+
+// A two-dimensional single precision vector.
+message Vector2f {
+ float x = 1;
+ float y = 2;
+}
+
+// A two-dimensional integer vector.
+message Vector2i {
+ int64 x = 1;
+ int64 y = 2;
+}
+
+// Double precision vector of arbitrary size.
+message Vectord {
+ repeated double data = 1 [packed = true];
+}
+
+// Single precision vector of arbitrary size.
+message Vectorf {
+ repeated float data = 1 [packed = true];
+}
+
+// Integer vector of arbitrary size.
+message Vectori {
+ repeated int64 data = 1 [packed = true];
+}
+
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/robots/vector_pb2.py b/examples/pybullet/gym/pybullet_envs/minitaur/robots/vector_pb2.py
new file mode 100644
index 000000000..ccaad55b4
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/robots/vector_pb2.py
@@ -0,0 +1,640 @@
+# -*- coding: utf-8 -*-
+# Generated by the protocol buffer compiler. DO NOT EDIT!
+# source: vector.proto
+"""Generated protocol buffer code."""
+from google.protobuf import descriptor as _descriptor
+from google.protobuf import message as _message
+from google.protobuf import reflection as _reflection
+from google.protobuf import symbol_database as _symbol_database
+# @@protoc_insertion_point(imports)
+
+_sym_db = _symbol_database.Default()
+
+
+
+
+DESCRIPTOR = _descriptor.FileDescriptor(
+ name='vector.proto',
+ package='robotics.messages',
+ syntax='proto3',
+ serialized_options=b'\370\001\001',
+ create_key=_descriptor._internal_create_key,
+ serialized_pb=b'\n\x0cvector.proto\x12\x11robotics.messages\"6\n\x08Vector4d\x12\t\n\x01x\x18\x01 \x01(\x01\x12\t\n\x01y\x18\x02 \x01(\x01\x12\t\n\x01z\x18\x03 \x01(\x01\x12\t\n\x01w\x18\x04 \x01(\x01\"6\n\x08Vector4f\x12\t\n\x01x\x18\x01 \x01(\x02\x12\t\n\x01y\x18\x02 \x01(\x02\x12\t\n\x01z\x18\x03 \x01(\x02\x12\t\n\x01w\x18\x04 \x01(\x02\"6\n\x08Vector4i\x12\t\n\x01x\x18\x01 \x01(\x03\x12\t\n\x01y\x18\x02 \x01(\x03\x12\t\n\x01z\x18\x03 \x01(\x03\x12\t\n\x01w\x18\x04 \x01(\x03\"+\n\x08Vector3d\x12\t\n\x01x\x18\x01 \x01(\x01\x12\t\n\x01y\x18\x02 \x01(\x01\x12\t\n\x01z\x18\x03 \x01(\x01\"+\n\x08Vector3f\x12\t\n\x01x\x18\x01 \x01(\x02\x12\t\n\x01y\x18\x02 \x01(\x02\x12\t\n\x01z\x18\x03 \x01(\x02\"+\n\x08Vector3i\x12\t\n\x01x\x18\x01 \x01(\x03\x12\t\n\x01y\x18\x02 \x01(\x03\x12\t\n\x01z\x18\x03 \x01(\x03\" \n\x08Vector2d\x12\t\n\x01x\x18\x01 \x01(\x01\x12\t\n\x01y\x18\x02 \x01(\x01\" \n\x08Vector2f\x12\t\n\x01x\x18\x01 \x01(\x02\x12\t\n\x01y\x18\x02 \x01(\x02\" \n\x08Vector2i\x12\t\n\x01x\x18\x01 \x01(\x03\x12\t\n\x01y\x18\x02 \x01(\x03\"\x1b\n\x07Vectord\x12\x10\n\x04\x64\x61ta\x18\x01 \x03(\x01\x42\x02\x10\x01\"\x1b\n\x07Vectorf\x12\x10\n\x04\x64\x61ta\x18\x01 \x03(\x02\x42\x02\x10\x01\"\x1b\n\x07Vectori\x12\x10\n\x04\x64\x61ta\x18\x01 \x03(\x03\x42\x02\x10\x01\x42\x03\xf8\x01\x01\x62\x06proto3'
+)
+
+
+
+
+_VECTOR4D = _descriptor.Descriptor(
+ name='Vector4d',
+ full_name='robotics.messages.Vector4d',
+ filename=None,
+ file=DESCRIPTOR,
+ containing_type=None,
+ create_key=_descriptor._internal_create_key,
+ fields=[
+ _descriptor.FieldDescriptor(
+ name='x', full_name='robotics.messages.Vector4d.x', index=0,
+ number=1, type=1, cpp_type=5, label=1,
+ has_default_value=False, default_value=float(0),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='y', full_name='robotics.messages.Vector4d.y', index=1,
+ number=2, type=1, cpp_type=5, label=1,
+ has_default_value=False, default_value=float(0),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='z', full_name='robotics.messages.Vector4d.z', index=2,
+ number=3, type=1, cpp_type=5, label=1,
+ has_default_value=False, default_value=float(0),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='w', full_name='robotics.messages.Vector4d.w', index=3,
+ number=4, type=1, cpp_type=5, label=1,
+ has_default_value=False, default_value=float(0),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ ],
+ extensions=[
+ ],
+ nested_types=[],
+ enum_types=[
+ ],
+ serialized_options=None,
+ is_extendable=False,
+ syntax='proto3',
+ extension_ranges=[],
+ oneofs=[
+ ],
+ serialized_start=35,
+ serialized_end=89,
+)
+
+
+_VECTOR4F = _descriptor.Descriptor(
+ name='Vector4f',
+ full_name='robotics.messages.Vector4f',
+ filename=None,
+ file=DESCRIPTOR,
+ containing_type=None,
+ create_key=_descriptor._internal_create_key,
+ fields=[
+ _descriptor.FieldDescriptor(
+ name='x', full_name='robotics.messages.Vector4f.x', index=0,
+ number=1, type=2, cpp_type=6, label=1,
+ has_default_value=False, default_value=float(0),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='y', full_name='robotics.messages.Vector4f.y', index=1,
+ number=2, type=2, cpp_type=6, label=1,
+ has_default_value=False, default_value=float(0),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='z', full_name='robotics.messages.Vector4f.z', index=2,
+ number=3, type=2, cpp_type=6, label=1,
+ has_default_value=False, default_value=float(0),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='w', full_name='robotics.messages.Vector4f.w', index=3,
+ number=4, type=2, cpp_type=6, label=1,
+ has_default_value=False, default_value=float(0),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ ],
+ extensions=[
+ ],
+ nested_types=[],
+ enum_types=[
+ ],
+ serialized_options=None,
+ is_extendable=False,
+ syntax='proto3',
+ extension_ranges=[],
+ oneofs=[
+ ],
+ serialized_start=91,
+ serialized_end=145,
+)
+
+
+_VECTOR4I = _descriptor.Descriptor(
+ name='Vector4i',
+ full_name='robotics.messages.Vector4i',
+ filename=None,
+ file=DESCRIPTOR,
+ containing_type=None,
+ create_key=_descriptor._internal_create_key,
+ fields=[
+ _descriptor.FieldDescriptor(
+ name='x', full_name='robotics.messages.Vector4i.x', index=0,
+ number=1, type=3, cpp_type=2, label=1,
+ has_default_value=False, default_value=0,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='y', full_name='robotics.messages.Vector4i.y', index=1,
+ number=2, type=3, cpp_type=2, label=1,
+ has_default_value=False, default_value=0,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='z', full_name='robotics.messages.Vector4i.z', index=2,
+ number=3, type=3, cpp_type=2, label=1,
+ has_default_value=False, default_value=0,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='w', full_name='robotics.messages.Vector4i.w', index=3,
+ number=4, type=3, cpp_type=2, label=1,
+ has_default_value=False, default_value=0,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ ],
+ extensions=[
+ ],
+ nested_types=[],
+ enum_types=[
+ ],
+ serialized_options=None,
+ is_extendable=False,
+ syntax='proto3',
+ extension_ranges=[],
+ oneofs=[
+ ],
+ serialized_start=147,
+ serialized_end=201,
+)
+
+
+_VECTOR3D = _descriptor.Descriptor(
+ name='Vector3d',
+ full_name='robotics.messages.Vector3d',
+ filename=None,
+ file=DESCRIPTOR,
+ containing_type=None,
+ create_key=_descriptor._internal_create_key,
+ fields=[
+ _descriptor.FieldDescriptor(
+ name='x', full_name='robotics.messages.Vector3d.x', index=0,
+ number=1, type=1, cpp_type=5, label=1,
+ has_default_value=False, default_value=float(0),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='y', full_name='robotics.messages.Vector3d.y', index=1,
+ number=2, type=1, cpp_type=5, label=1,
+ has_default_value=False, default_value=float(0),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='z', full_name='robotics.messages.Vector3d.z', index=2,
+ number=3, type=1, cpp_type=5, label=1,
+ has_default_value=False, default_value=float(0),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ ],
+ extensions=[
+ ],
+ nested_types=[],
+ enum_types=[
+ ],
+ serialized_options=None,
+ is_extendable=False,
+ syntax='proto3',
+ extension_ranges=[],
+ oneofs=[
+ ],
+ serialized_start=203,
+ serialized_end=246,
+)
+
+
+_VECTOR3F = _descriptor.Descriptor(
+ name='Vector3f',
+ full_name='robotics.messages.Vector3f',
+ filename=None,
+ file=DESCRIPTOR,
+ containing_type=None,
+ create_key=_descriptor._internal_create_key,
+ fields=[
+ _descriptor.FieldDescriptor(
+ name='x', full_name='robotics.messages.Vector3f.x', index=0,
+ number=1, type=2, cpp_type=6, label=1,
+ has_default_value=False, default_value=float(0),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='y', full_name='robotics.messages.Vector3f.y', index=1,
+ number=2, type=2, cpp_type=6, label=1,
+ has_default_value=False, default_value=float(0),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='z', full_name='robotics.messages.Vector3f.z', index=2,
+ number=3, type=2, cpp_type=6, label=1,
+ has_default_value=False, default_value=float(0),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ ],
+ extensions=[
+ ],
+ nested_types=[],
+ enum_types=[
+ ],
+ serialized_options=None,
+ is_extendable=False,
+ syntax='proto3',
+ extension_ranges=[],
+ oneofs=[
+ ],
+ serialized_start=248,
+ serialized_end=291,
+)
+
+
+_VECTOR3I = _descriptor.Descriptor(
+ name='Vector3i',
+ full_name='robotics.messages.Vector3i',
+ filename=None,
+ file=DESCRIPTOR,
+ containing_type=None,
+ create_key=_descriptor._internal_create_key,
+ fields=[
+ _descriptor.FieldDescriptor(
+ name='x', full_name='robotics.messages.Vector3i.x', index=0,
+ number=1, type=3, cpp_type=2, label=1,
+ has_default_value=False, default_value=0,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='y', full_name='robotics.messages.Vector3i.y', index=1,
+ number=2, type=3, cpp_type=2, label=1,
+ has_default_value=False, default_value=0,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='z', full_name='robotics.messages.Vector3i.z', index=2,
+ number=3, type=3, cpp_type=2, label=1,
+ has_default_value=False, default_value=0,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ ],
+ extensions=[
+ ],
+ nested_types=[],
+ enum_types=[
+ ],
+ serialized_options=None,
+ is_extendable=False,
+ syntax='proto3',
+ extension_ranges=[],
+ oneofs=[
+ ],
+ serialized_start=293,
+ serialized_end=336,
+)
+
+
+_VECTOR2D = _descriptor.Descriptor(
+ name='Vector2d',
+ full_name='robotics.messages.Vector2d',
+ filename=None,
+ file=DESCRIPTOR,
+ containing_type=None,
+ create_key=_descriptor._internal_create_key,
+ fields=[
+ _descriptor.FieldDescriptor(
+ name='x', full_name='robotics.messages.Vector2d.x', index=0,
+ number=1, type=1, cpp_type=5, label=1,
+ has_default_value=False, default_value=float(0),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='y', full_name='robotics.messages.Vector2d.y', index=1,
+ number=2, type=1, cpp_type=5, label=1,
+ has_default_value=False, default_value=float(0),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ ],
+ extensions=[
+ ],
+ nested_types=[],
+ enum_types=[
+ ],
+ serialized_options=None,
+ is_extendable=False,
+ syntax='proto3',
+ extension_ranges=[],
+ oneofs=[
+ ],
+ serialized_start=338,
+ serialized_end=370,
+)
+
+
+_VECTOR2F = _descriptor.Descriptor(
+ name='Vector2f',
+ full_name='robotics.messages.Vector2f',
+ filename=None,
+ file=DESCRIPTOR,
+ containing_type=None,
+ create_key=_descriptor._internal_create_key,
+ fields=[
+ _descriptor.FieldDescriptor(
+ name='x', full_name='robotics.messages.Vector2f.x', index=0,
+ number=1, type=2, cpp_type=6, label=1,
+ has_default_value=False, default_value=float(0),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='y', full_name='robotics.messages.Vector2f.y', index=1,
+ number=2, type=2, cpp_type=6, label=1,
+ has_default_value=False, default_value=float(0),
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ ],
+ extensions=[
+ ],
+ nested_types=[],
+ enum_types=[
+ ],
+ serialized_options=None,
+ is_extendable=False,
+ syntax='proto3',
+ extension_ranges=[],
+ oneofs=[
+ ],
+ serialized_start=372,
+ serialized_end=404,
+)
+
+
+_VECTOR2I = _descriptor.Descriptor(
+ name='Vector2i',
+ full_name='robotics.messages.Vector2i',
+ filename=None,
+ file=DESCRIPTOR,
+ containing_type=None,
+ create_key=_descriptor._internal_create_key,
+ fields=[
+ _descriptor.FieldDescriptor(
+ name='x', full_name='robotics.messages.Vector2i.x', index=0,
+ number=1, type=3, cpp_type=2, label=1,
+ has_default_value=False, default_value=0,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ _descriptor.FieldDescriptor(
+ name='y', full_name='robotics.messages.Vector2i.y', index=1,
+ number=2, type=3, cpp_type=2, label=1,
+ has_default_value=False, default_value=0,
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ ],
+ extensions=[
+ ],
+ nested_types=[],
+ enum_types=[
+ ],
+ serialized_options=None,
+ is_extendable=False,
+ syntax='proto3',
+ extension_ranges=[],
+ oneofs=[
+ ],
+ serialized_start=406,
+ serialized_end=438,
+)
+
+
+_VECTORD = _descriptor.Descriptor(
+ name='Vectord',
+ full_name='robotics.messages.Vectord',
+ filename=None,
+ file=DESCRIPTOR,
+ containing_type=None,
+ create_key=_descriptor._internal_create_key,
+ fields=[
+ _descriptor.FieldDescriptor(
+ name='data', full_name='robotics.messages.Vectord.data', index=0,
+ number=1, type=1, cpp_type=5, label=3,
+ has_default_value=False, default_value=[],
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=b'\020\001', file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ ],
+ extensions=[
+ ],
+ nested_types=[],
+ enum_types=[
+ ],
+ serialized_options=None,
+ is_extendable=False,
+ syntax='proto3',
+ extension_ranges=[],
+ oneofs=[
+ ],
+ serialized_start=440,
+ serialized_end=467,
+)
+
+
+_VECTORF = _descriptor.Descriptor(
+ name='Vectorf',
+ full_name='robotics.messages.Vectorf',
+ filename=None,
+ file=DESCRIPTOR,
+ containing_type=None,
+ create_key=_descriptor._internal_create_key,
+ fields=[
+ _descriptor.FieldDescriptor(
+ name='data', full_name='robotics.messages.Vectorf.data', index=0,
+ number=1, type=2, cpp_type=6, label=3,
+ has_default_value=False, default_value=[],
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=b'\020\001', file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ ],
+ extensions=[
+ ],
+ nested_types=[],
+ enum_types=[
+ ],
+ serialized_options=None,
+ is_extendable=False,
+ syntax='proto3',
+ extension_ranges=[],
+ oneofs=[
+ ],
+ serialized_start=469,
+ serialized_end=496,
+)
+
+
+_VECTORI = _descriptor.Descriptor(
+ name='Vectori',
+ full_name='robotics.messages.Vectori',
+ filename=None,
+ file=DESCRIPTOR,
+ containing_type=None,
+ create_key=_descriptor._internal_create_key,
+ fields=[
+ _descriptor.FieldDescriptor(
+ name='data', full_name='robotics.messages.Vectori.data', index=0,
+ number=1, type=3, cpp_type=2, label=3,
+ has_default_value=False, default_value=[],
+ message_type=None, enum_type=None, containing_type=None,
+ is_extension=False, extension_scope=None,
+ serialized_options=b'\020\001', file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
+ ],
+ extensions=[
+ ],
+ nested_types=[],
+ enum_types=[
+ ],
+ serialized_options=None,
+ is_extendable=False,
+ syntax='proto3',
+ extension_ranges=[],
+ oneofs=[
+ ],
+ serialized_start=498,
+ serialized_end=525,
+)
+
+DESCRIPTOR.message_types_by_name['Vector4d'] = _VECTOR4D
+DESCRIPTOR.message_types_by_name['Vector4f'] = _VECTOR4F
+DESCRIPTOR.message_types_by_name['Vector4i'] = _VECTOR4I
+DESCRIPTOR.message_types_by_name['Vector3d'] = _VECTOR3D
+DESCRIPTOR.message_types_by_name['Vector3f'] = _VECTOR3F
+DESCRIPTOR.message_types_by_name['Vector3i'] = _VECTOR3I
+DESCRIPTOR.message_types_by_name['Vector2d'] = _VECTOR2D
+DESCRIPTOR.message_types_by_name['Vector2f'] = _VECTOR2F
+DESCRIPTOR.message_types_by_name['Vector2i'] = _VECTOR2I
+DESCRIPTOR.message_types_by_name['Vectord'] = _VECTORD
+DESCRIPTOR.message_types_by_name['Vectorf'] = _VECTORF
+DESCRIPTOR.message_types_by_name['Vectori'] = _VECTORI
+_sym_db.RegisterFileDescriptor(DESCRIPTOR)
+
+Vector4d = _reflection.GeneratedProtocolMessageType('Vector4d', (_message.Message,), {
+ 'DESCRIPTOR' : _VECTOR4D,
+ '__module__' : 'vector_pb2'
+ # @@protoc_insertion_point(class_scope:robotics.messages.Vector4d)
+ })
+_sym_db.RegisterMessage(Vector4d)
+
+Vector4f = _reflection.GeneratedProtocolMessageType('Vector4f', (_message.Message,), {
+ 'DESCRIPTOR' : _VECTOR4F,
+ '__module__' : 'vector_pb2'
+ # @@protoc_insertion_point(class_scope:robotics.messages.Vector4f)
+ })
+_sym_db.RegisterMessage(Vector4f)
+
+Vector4i = _reflection.GeneratedProtocolMessageType('Vector4i', (_message.Message,), {
+ 'DESCRIPTOR' : _VECTOR4I,
+ '__module__' : 'vector_pb2'
+ # @@protoc_insertion_point(class_scope:robotics.messages.Vector4i)
+ })
+_sym_db.RegisterMessage(Vector4i)
+
+Vector3d = _reflection.GeneratedProtocolMessageType('Vector3d', (_message.Message,), {
+ 'DESCRIPTOR' : _VECTOR3D,
+ '__module__' : 'vector_pb2'
+ # @@protoc_insertion_point(class_scope:robotics.messages.Vector3d)
+ })
+_sym_db.RegisterMessage(Vector3d)
+
+Vector3f = _reflection.GeneratedProtocolMessageType('Vector3f', (_message.Message,), {
+ 'DESCRIPTOR' : _VECTOR3F,
+ '__module__' : 'vector_pb2'
+ # @@protoc_insertion_point(class_scope:robotics.messages.Vector3f)
+ })
+_sym_db.RegisterMessage(Vector3f)
+
+Vector3i = _reflection.GeneratedProtocolMessageType('Vector3i', (_message.Message,), {
+ 'DESCRIPTOR' : _VECTOR3I,
+ '__module__' : 'vector_pb2'
+ # @@protoc_insertion_point(class_scope:robotics.messages.Vector3i)
+ })
+_sym_db.RegisterMessage(Vector3i)
+
+Vector2d = _reflection.GeneratedProtocolMessageType('Vector2d', (_message.Message,), {
+ 'DESCRIPTOR' : _VECTOR2D,
+ '__module__' : 'vector_pb2'
+ # @@protoc_insertion_point(class_scope:robotics.messages.Vector2d)
+ })
+_sym_db.RegisterMessage(Vector2d)
+
+Vector2f = _reflection.GeneratedProtocolMessageType('Vector2f', (_message.Message,), {
+ 'DESCRIPTOR' : _VECTOR2F,
+ '__module__' : 'vector_pb2'
+ # @@protoc_insertion_point(class_scope:robotics.messages.Vector2f)
+ })
+_sym_db.RegisterMessage(Vector2f)
+
+Vector2i = _reflection.GeneratedProtocolMessageType('Vector2i', (_message.Message,), {
+ 'DESCRIPTOR' : _VECTOR2I,
+ '__module__' : 'vector_pb2'
+ # @@protoc_insertion_point(class_scope:robotics.messages.Vector2i)
+ })
+_sym_db.RegisterMessage(Vector2i)
+
+Vectord = _reflection.GeneratedProtocolMessageType('Vectord', (_message.Message,), {
+ 'DESCRIPTOR' : _VECTORD,
+ '__module__' : 'vector_pb2'
+ # @@protoc_insertion_point(class_scope:robotics.messages.Vectord)
+ })
+_sym_db.RegisterMessage(Vectord)
+
+Vectorf = _reflection.GeneratedProtocolMessageType('Vectorf', (_message.Message,), {
+ 'DESCRIPTOR' : _VECTORF,
+ '__module__' : 'vector_pb2'
+ # @@protoc_insertion_point(class_scope:robotics.messages.Vectorf)
+ })
+_sym_db.RegisterMessage(Vectorf)
+
+Vectori = _reflection.GeneratedProtocolMessageType('Vectori', (_message.Message,), {
+ 'DESCRIPTOR' : _VECTORI,
+ '__module__' : 'vector_pb2'
+ # @@protoc_insertion_point(class_scope:robotics.messages.Vectori)
+ })
+_sym_db.RegisterMessage(Vectori)
+
+
+DESCRIPTOR._options = None
+_VECTORD.fields_by_name['data']._options = None
+_VECTORF.fields_by_name['data']._options = None
+_VECTORI.fields_by_name['data']._options = None
+# @@protoc_insertion_point(module_scope)
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/vision/imagery.proto b/examples/pybullet/gym/pybullet_envs/minitaur/vision/imagery.proto
new file mode 100644
index 000000000..ef3a639d7
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/vision/imagery.proto
@@ -0,0 +1,72 @@
+syntax = "proto3";
+
+package robotics.reinforcement_learning.minitaur.vision;
+
+import "google/protobuf/timestamp.proto";
+
+// The basic image protobuf. Used for RPC/IPC transmission.
+message Image {
+ // The buffer that contains the actual image data.
+ bytes content = 1;
+
+ // The image storage format. Can be raw or compressed types.
+ enum ImageFormat {
+ IMAGE_FORMAT_UNSPECIFIED = 0;
+
+ // An 32-bit gray-scale image/matrix with row-major (height, width) memroy
+ // layout. Each pixel is a 32-bit floating-point number.
+ IMAGE_FORMAT_GRAY_HW_32F = 1;
+
+ // An 8-bit BGRA raw image format, with HWC memory layout (e.g. the image is
+ // stored as a row-major matrix (height, width) of pixels, with each pixel
+ // an uint8[num_color_channels] packed array. This is the same format as
+ // CV_8UC4.
+ IMAGE_FORMAT_BGRA_HWC_8U = 2;
+
+ // The 16-bit grayscale images with row-major memory layout. This is the
+ // default depth format for intel RealSense cameras. Each pixel is a 16 bit
+ // unsigned integer.
+ IMAGE_FORMAT_GRAY_HW_16U = 3;
+
+ // An 8-bit RGB raw image format, with HWC memory layout. This is the
+ // default color format for intel RealSense cameras.
+ IMAGE_FORMAT_RGB_HWC_8U = 4;
+
+ // TODO(tingnan): Add supports for different image formats like I420 or
+ // JPEG.
+ }
+
+ ImageFormat image_format = 2;
+
+ // The UTC time at which the image is taken.
+ google.protobuf.Timestamp timestamp = 3;
+
+ // Image width and height in pixels. Critical for decoding raw images and
+ // optional for compressed JPEG/PNGs which already embed these information.
+ int32 width_px = 4;
+ int32 height_px = 5;
+}
+
+// A captured frame from camera can combine multiple images from different
+// streams, IR, depth, VGA.
+message CameraFrame {
+ map images = 1;
+ string camera_id = 2;
+}
+
+// Get the latest raw image from camera.
+message GetFrameRequest {
+ // TODO: Also enable camera id in this proto.
+}
+
+// Stacked frames. The imagery service can decide how many frames to transmit
+// for each GetFrameRequest.
+message CameraFrameCollection {
+ repeated CameraFrame frames = 1;
+}
+
+// The capture session start/stop request.
+message CaptureRequest {
+ string run_id = 1;
+ string logging_path = 2;
+}
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/vision/imagery_client.py b/examples/pybullet/gym/pybullet_envs/minitaur/vision/imagery_client.py
new file mode 100644
index 000000000..ca2a2e689
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/vision/imagery_client.py
@@ -0,0 +1,137 @@
+"""The imagery client to connect to the camera job."""
+
+from typing import Any, Dict, Sequence, Text
+import gin
+
+from pybullet_envs.minitaur.fw_bridge import worker_builder
+from pybullet_envs.minitaur.vision import imagery_pb2
+from pybullet_envs.minitaur.vision import imagery_utils
+from google3.third_party.fluxworks.core.fluxworks.python.genericutil_py import fwassert
+from google3.third_party.fluxworks.core.fluxworks.python.genericutil_py import timeutil
+
+_RPC_TIMEOUT = 1 * timeutil.TimeUtil.SEC
+
+_URI_START_CAPTURE = "fwuri://VisionJob/StartCapture"
+_URI_STOP_CAPTURE = "fwuri://VisionJob/StopCapture"
+_URI_GET_FRAME = "fwuri://VisionJob/GetFrame"
+
+
+@gin.configurable
+class ImageryClient(object):
+ """Sends commands and receives states from cameras."""
+
+ def __init__(
+ self,
+ fw_worker=None,
+ rpc_timeout_sec=_RPC_TIMEOUT,
+ ip_address=None,
+ port=None,
+ async_mode=False,
+ start_capture_uri: Text = _URI_START_CAPTURE,
+ stop_capture_uri: Text = _URI_STOP_CAPTURE,
+ get_frame_uri: Text = _URI_GET_FRAME,
+ ):
+ """Initializes the client.
+
+ Args:
+ fw_worker: A FluxWorks worker instance.
+ rpc_timeout_sec: The timeout for any RPC calls from this client.
+ ip_address: The ip address of the camera/vision process. If vision job is
+ also instantiated in the same FluxWorks worker, both ip address and port
+ are not needed.
+ port: The port of the camera/vision process.
+ async_mode: Whether the RPC calls in this client are async or synchronous.
+ Aync mode is only required when you have multiple workers communicating
+ with each other in the same Python process. If worker A is calling
+ worker B's RPC, worker B's RPC is trying to get GIL from it's thread but
+ caller (worker A) already holds the GIL, and this will cause a dead lock
+ if worker A's calls are synchronous. If worker A is calling its own RPC,
+ the same GIL can be used so there is no dead lock, and there is no need
+ for async mode. Async mode will require context switching and thus is a
+ bit slower.
+ start_capture_uri: The FluxWorks URI to start camera capture.
+ stop_capture_uri: The FluxWorks URI to stop camera capture.
+ get_frame_uri: The FluxWorks URI to get camera frames.
+ """
+ self._rpc_timeout_sec = rpc_timeout_sec
+ if fw_worker is None:
+ fw_worker = worker_builder.GetDefaultWorker()
+ self._worker = fw_worker
+
+ # TODO(tingnan): Use a single address and split the string for FW.
+ if ip_address is not None:
+ self._worker.ConnectToWorker(ip_address, port)
+
+ self._async_mode = async_mode
+ self._start_capture_uri = start_capture_uri
+ self._stop_capture_uri = stop_capture_uri
+ self._get_frame_uri = get_frame_uri
+
+ def _convert_camera_frame_to_image_dict(
+ self, camera_frame: imagery_pb2.CameraFrame):
+ """Converts the camera frame to an image dictionary."""
+ # Each camera frame might contain multiple image channels, such as rgb and
+ # depth.
+ images = {}
+ for image_name, image_proto in camera_frame.images.items():
+ image_array = imagery_utils.convert_image_to_array(image_proto)
+ images[image_name] = image_array
+ return images
+
+ def start_capture(self, run_id: Text = "vision"):
+ """Starts the camera capture session.
+
+ Args:
+ run_id: The capture session id. This id will determine the name of the
+ image logs' sub-direcotry.
+ """
+ capture_request = imagery_pb2.CaptureRequest()
+ capture_request.run_id = run_id
+ fwassert.FwAssert.CheckErrorMessage(
+ self._worker.CallOnewayProtoRpc(
+ self._start_capture_uri,
+ capture_request,
+ async_mode=self._async_mode))
+
+ def stop_capture(self):
+ """Concludes the current capture session."""
+ capture_request = imagery_pb2.CaptureRequest()
+ fwassert.FwAssert.CheckErrorMessage(
+ self._worker.CallOnewayProtoRpc(
+ self._stop_capture_uri,
+ capture_request,
+ async_mode=self._async_mode))
+
+ def get_camera_images(self) -> Dict[Text, Sequence[Any]]:
+ """Gets the latest camera images.
+
+ Camera images can only be obtained after self.start_capture() is called.
+
+ Returns:
+ A dictionary of camera frames, with the camera id as the key. Each camera
+ frame may contain multiple streams. For example, on a realsense camera we
+ may have "rgb" and "depth" streams, depending on the configuration.
+ """
+ get_frame_request = imagery_pb2.GetFrameRequest()
+ frame_collection = imagery_pb2.CameraFrameCollection()
+ fwassert.FwAssert.CheckErrorMessage(
+ self._worker.CallRoundtripProtoRpc(
+ self._get_frame_uri,
+ get_frame_request,
+ frame_collection,
+ self._rpc_timeout_sec,
+ async_mode=self._async_mode))
+
+ images_by_camera = {}
+ for camera_frame in frame_collection.frames:
+ camera_id = camera_frame.camera_id
+ # In case we received multiple frames, we apppend them in the order
+ # received.
+ if camera_id in images_by_camera:
+ images_by_camera[camera_id].append(
+ self._convert_camera_frame_to_image_dict(camera_frame))
+ else:
+ images_by_camera[camera_id] = [
+ self._convert_camera_frame_to_image_dict(camera_frame)
+ ]
+ return images_by_camera
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/vision/imagery_utils.py b/examples/pybullet/gym/pybullet_envs/minitaur/vision/imagery_utils.py
new file mode 100644
index 000000000..ccf8da044
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/vision/imagery_utils.py
@@ -0,0 +1,43 @@
+"""Utilities to convert imagery protobufs to other formats."""
+
+import numpy as np
+
+from pybullet_envs.minitaur.vision import imagery_pb2
+
+
+# TODO(b/123306148): Support the conversion from image array to the proto.
+def convert_image_to_array(image):
+ """Converts an Image proto into a numpy array.
+
+ Args:
+ image: An instance of the imagery_pb2.Image proto.
+
+ Returns:
+ A numpy array. For color images (e.g. BGRA), the converted ND array
+ has the format [Height, Width, Channel]. For gray images (e.g. depth), the
+ converted ND array has the format [Height, Width].
+ """
+
+ if image.image_format == imagery_pb2.Image.IMAGE_FORMAT_BGRA_HWC_8U:
+ img_buffer = np.fromstring(image.content, dtype=np.uint8)
+ img = np.reshape(
+ img_buffer, [image.height_px, image.width_px, 4], order="C")
+ return img
+
+ if image.image_format == imagery_pb2.Image.IMAGE_FORMAT_RGB_HWC_8U:
+ img_buffer = np.fromstring(image.content, dtype=np.uint8)
+ img = np.reshape(
+ img_buffer, [image.height_px, image.width_px, 3], order="C")
+ return img
+
+ if image.image_format == imagery_pb2.Image.IMAGE_FORMAT_GRAY_HW_32F:
+ img_buffer = np.fromstring(image.content, dtype=np.float32)
+ img = np.reshape(img_buffer, [image.height_px, image.width_px], order="C")
+ return img
+
+ if image.image_format == imagery_pb2.Image.IMAGE_FORMAT_GRAY_HW_16U:
+ img_buffer = np.fromstring(image.content, dtype=np.uint16)
+ img = np.reshape(img_buffer, [image.height_px, image.width_px], order="C")
+ return img
+
+ raise ValueError("Unsupported image format {}".format(image.image_format))
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/vision/imagery_utils_test.py b/examples/pybullet/gym/pybullet_envs/minitaur/vision/imagery_utils_test.py
new file mode 100644
index 000000000..5b5493f64
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/vision/imagery_utils_test.py
@@ -0,0 +1,84 @@
+"""Tests for imagery_utils."""
+
+from __future__ import absolute_import
+from __future__ import division
+from __future__ import print_function
+
+import struct
+import numpy as np
+
+from pybullet_envs.minitaur.vision import imagery_pb2
+from pybullet_envs.minitaur.vision import imagery_utils
+from google3.testing.pybase import googletest
+
+
+class ImageryUtilsTest(googletest.TestCase):
+
+ def test_convert_bgra_images(self):
+ image = imagery_pb2.Image(
+ height_px=2,
+ width_px=2,
+ image_format=imagery_pb2.Image.IMAGE_FORMAT_BGRA_HWC_8U,
+ content=b'ABCDABCDABCDABCD',
+ )
+
+ image_array = imagery_utils.convert_image_to_array(image)
+
+ self.assertEqual(image_array.dtype, np.uint8)
+ self.assertEqual(image_array.shape, (image.height_px, image.width_px, 4))
+ self.assertEqual(image_array[0, 0, 0], ord('A'))
+ self.assertEqual(image_array[1, 0, 3], ord('D'))
+
+ def test_convert_rgb_images(self):
+ image = imagery_pb2.Image(
+ height_px=2,
+ width_px=2,
+ image_format=imagery_pb2.Image.IMAGE_FORMAT_RGB_HWC_8U,
+ content=b'ABCABCABCABC',
+ )
+
+ image_array = imagery_utils.convert_image_to_array(image)
+
+ self.assertEqual(image_array.dtype, np.uint8)
+ self.assertEqual(image_array.shape, (image.height_px, image.width_px, 3))
+ self.assertEqual(image_array[0, 0, 0], ord('A'))
+ self.assertEqual(image_array[1, 1, 2], ord('C'))
+
+ def test_convert_gray_32bit_images(self):
+ image = imagery_pb2.Image(
+ height_px=2,
+ width_px=3,
+ image_format=imagery_pb2.Image.IMAGE_FORMAT_GRAY_HW_32F,
+ content=b'AAAABBBBCCCCAAAABBBBCCCC',
+ )
+
+ image_array = imagery_utils.convert_image_to_array(image)
+
+ self.assertEqual(image_array.dtype, np.float32)
+ self.assertEqual(image_array.shape, (image.height_px, image.width_px))
+ self.assertEqual(image_array[0, 2], struct.unpack(b'