diff --git a/build.sh b/build.sh index 7e06a53ca..cb7e4a26f 100755 --- a/build.sh +++ b/build.sh @@ -73,6 +73,7 @@ install() { echo $password | sudo -s apt autoremove echo $password | sudo -s apt install cmake -y echo $password | sudo -s apt install golang libjpeg-turbo8-dev unzip -y + echo $password | sudo -s apt install wmctrl xdotool -y ## Core renderer echo $password | sudo -s apt install nvidia-cuda-toolkit -y ## Huge, 1121M diff --git a/examples/agents/random_humanoid_camera.py b/examples/agents/random_humanoid_camera.py index dadb22e76..b97885798 100644 --- a/examples/agents/random_humanoid_camera.py +++ b/examples/agents/random_humanoid_camera.py @@ -44,7 +44,7 @@ if __name__ == '__main__': if not done and frame < 60: continue if restart_delay==0: print("score=%0.2f in %i frames" % (score, frame)) - restart_delay = 20 * 4 # 2 sec at 60 fps + restart_delay = 30 * 4 # 2 sec at 60 fps else: restart_delay -= 1 if restart_delay==0: break diff --git a/examples/agents/random_humanoid_sensor.py b/examples/agents/random_humanoid_sensor.py index 5974ac4f2..ac0a2a2a2 100644 --- a/examples/agents/random_humanoid_sensor.py +++ b/examples/agents/random_humanoid_sensor.py @@ -26,12 +26,6 @@ if __name__ == '__main__': env.reset() agent = RandomAgent(env.action_space) ob = None - torsoId = -1 - - for i in range (p.getNumBodies()): - if (p.getBodyInfo(i)[0].decode() == "torso"): - torsoId=i - i = 0 while 1: frame = 0 diff --git a/examples/agents/random_husky_camera.py b/examples/agents/random_husky_camera.py index ab881911a..98c697d89 100644 --- a/examples/agents/random_husky_camera.py +++ b/examples/agents/random_husky_camera.py @@ -21,13 +21,13 @@ class RandomAgent(object): else: action = np.zeros(self.action_space.shape[0]) if (np.random.random() < 0.5): - action[np.random.choice(action.shape[0], 1)] = np.random.randint(0, 2) + action[np.random.choice(action.shape[0], 1)] = np.random.randint(-1, 2) return action if __name__ == '__main__': - env = HuskyCameraEnv(human=True, timestep=1.0/(4 * 22), frame_skip=4, enable_sensors=True, is_discrete = True) + env = HuskyCameraEnv(human=True, timestep=1.0/(4 * 22), frame_skip=4, enable_sensors=True, is_discrete = False) env.reset() - agent = RandomAgent(env.action_space, is_discrete = True) + agent = RandomAgent(env.action_space, is_discrete = False) ob = None while 1: @@ -36,7 +36,7 @@ if __name__ == '__main__': restart_delay = 0 obs = env.reset() while True: - time.sleep(0.03) + time.sleep(0.01) a = agent.act(obs) with Profiler("Agent step function"): obs, r, done, meta = env.step(a) diff --git a/examples/agents/random_husky_sensor.py b/examples/agents/random_husky_sensor.py index 7dd5382ac..54ee83a28 100644 --- a/examples/agents/random_husky_sensor.py +++ b/examples/agents/random_husky_sensor.py @@ -47,7 +47,7 @@ if __name__ == '__main__': if not done and frame < 60: continue if restart_delay==0: print("score=%0.2f in %i frames" % (score, frame)) - restart_delay = 200 * 4 # 2 sec at 60 fps + restart_delay = 20000 * 4 # 2 sec at 60 fps else: restart_delay -= 1 if restart_delay==0: break \ No newline at end of file diff --git a/examples/agents/random_quadruped_camera.py b/examples/agents/random_quadruped_camera.py new file mode 100644 index 000000000..60b2fe8a4 --- /dev/null +++ b/examples/agents/random_quadruped_camera.py @@ -0,0 +1,49 @@ +from __future__ import print_function +import time +import numpy as np +import sys +import gym +from PIL import Image +from realenv.core.render.profiler import Profiler +from realenv.envs.quadruped_env import QuadrupedCameraEnv +import pybullet as p + + +class RandomAgent(object): + """The world's simplest agent""" + def __init__(self, action_space): + self.action_space = action_space + + def act(self, observation, reward=None): + action = np.zeros(self.action_space.shape[0]) + if (np.random.random() < 0.7): + action[np.random.choice(action.shape[0], 1)] = np.random.randint(-1, 2) + return action + +if __name__ == '__main__': + #env = gym.make('HumanoidSensor-v0') + env = QuadrupedCameraEnv(human=True, timestep=1.0/(4 * 22), frame_skip=4, enable_sensors=True) + env.reset() + agent = RandomAgent(env.action_space) + ob = None + + while 1: + frame = 0 + score = 0 + restart_delay = 0 + obs = env.reset() + while True: + time.sleep(0.01) + a = agent.act(obs) + with Profiler("Agent step function"): + obs, r, done, meta = env.step(a) + score += r + frame += 1 + + if not done and frame < 60: continue + if restart_delay==0: + print("score=%0.2f in %i frames" % (score, frame)) + restart_delay = 200000 * 4 # 2 sec at 60 fps + else: + restart_delay -= 1 + if restart_delay==0: break diff --git a/examples/agents/random_quadruped_sensor.py b/examples/agents/random_quadruped_sensor.py new file mode 100644 index 000000000..4d53aa5dc --- /dev/null +++ b/examples/agents/random_quadruped_sensor.py @@ -0,0 +1,49 @@ +from __future__ import print_function +import time +import numpy as np +import sys +import gym +from PIL import Image +from realenv.core.render.profiler import Profiler +from realenv.envs.quadruped_env import QuadrupedSensorEnv +import pybullet as p + + +class RandomAgent(object): + """The world's simplest agent""" + def __init__(self, action_space): + self.action_space = action_space + + def act(self, observation, reward=None): + action = np.zeros(self.action_space.shape[0]) + if (np.random.random() < 0.7): + action[np.random.choice(action.shape[0], 1)] = np.random.randint(-1, 2) + return action + +if __name__ == '__main__': + #env = gym.make('HumanoidSensor-v0') + env = QuadrupedSensorEnv(human=True, timestep=1.0/(4 * 22), frame_skip=4, enable_sensors=True) + env.reset() + agent = RandomAgent(env.action_space) + ob = None + + while 1: + frame = 0 + score = 0 + restart_delay = 0 + obs = env.reset() + while True: + time.sleep(0.01) + a = agent.act(obs) + with Profiler("Agent step function"): + obs, r, done, meta = env.step(a) + score += r + frame += 1 + + if not done and frame < 60: continue + if restart_delay==0: + print("score=%0.2f in %i frames" % (score, frame)) + restart_delay = 200000 * 4 # 2 sec at 60 fps + else: + restart_delay -= 1 + if restart_delay==0: break diff --git a/examples/demo/play_husky_camera.py b/examples/demo/play_husky_camera.py new file mode 100644 index 000000000..af62826fa --- /dev/null +++ b/examples/demo/play_husky_camera.py @@ -0,0 +1,9 @@ +from realenv.envs.husky_env import HuskyCameraEnv +from realenv.utils.play import play + +timestep = 1.0/(4 * 18) +frame_skip = 4 + +if __name__ == '__main__': + env = HuskyCameraEnv(human=True, timestep=timestep, frame_skip=frame_skip, enable_sensors=False, is_discrete = True) + play(env, zoom=4, fps=int( 1.0/(timestep * frame_skip))) \ No newline at end of file diff --git a/examples/demo/play_husky_sensor.py b/examples/demo/play_husky_sensor.py new file mode 100644 index 000000000..07a30255e --- /dev/null +++ b/examples/demo/play_husky_sensor.py @@ -0,0 +1,9 @@ +from realenv.envs.husky_env import HuskySensorEnv +from realenv.utils.play import play + +timestep = 1.0/(4 * 22) +frame_skip = 4 + +if __name__ == '__main__': + env = HuskySensorEnv(human=True, timestep=timestep, frame_skip=frame_skip, enable_sensors=False, is_discrete = True) + play(env, zoom=4, fps=int( 1.0/(timestep * frame_skip))) \ No newline at end of file diff --git a/realenv/client/__init__.py b/realenv/client/__init__.py deleted file mode 100644 index 24d77a46e..000000000 --- a/realenv/client/__init__.py +++ /dev/null @@ -1,2 +0,0 @@ -from realenv.client.vnc_client import VNCClient -from realenv.client.client_actions import client_actions, client_newloc \ No newline at end of file diff --git a/realenv/core/physics/models/husky.urdf b/realenv/core/physics/models/husky.urdf index d3041dbce..7222e8da8 100644 --- a/realenv/core/physics/models/husky.urdf +++ b/realenv/core/physics/models/husky.urdf @@ -76,7 +76,7 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI - + diff --git a/realenv/core/physics/models/quadruped/tmotor.blend b/realenv/core/physics/models/quadruped/tmotor.blend new file mode 100644 index 000000000..ba06c0075 Binary files /dev/null and b/realenv/core/physics/models/quadruped/tmotor.blend differ diff --git a/realenv/core/physics/models/quadruped/tmotor3.mtl b/realenv/core/physics/models/quadruped/tmotor3.mtl new file mode 100644 index 000000000..af216a1cc --- /dev/null +++ b/realenv/core/physics/models/quadruped/tmotor3.mtl @@ -0,0 +1,19 @@ +# Blender MTL File: 'tmotor.blend' +# Material Count: 2 + +newmtl None +Ns 0 +Ka 0.000000 0.000000 0.000000 +Kd 0.8 0.8 0.8 +Ks 0.8 0.8 0.8 +d 1 +illum 2 +map_Kd t-motor.jpg + +newmtl None_NONE +Ns 0 +Ka 0.000000 0.000000 0.000000 +Kd 0.8 0.8 0.8 +Ks 0.8 0.8 0.8 +d 1 +illum 2 diff --git a/realenv/core/physics/motor.py b/realenv/core/physics/motor.py new file mode 100644 index 000000000..fa6b4f2da --- /dev/null +++ b/realenv/core/physics/motor.py @@ -0,0 +1,101 @@ +"""This file implements an accurate motor model.""" +import numpy as np + +VOLTAGE_CLIPPING = 50 +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) + + +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, + torque_control_enabled=False, + kp=1.2, + kd=0): + self._torque_control_enabled = torque_control_enabled + self._kp = kp + self._kd = kd + 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] + + 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, current_motor_angle, + current_motor_velocity): + """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. + current_motor_angle: The motor angle at the current time step. + current_motor_velocity: The motor velocity at the current time step. + Returns: + actual_torque: The torque that needs to be applied to the motor. + observed_torque: The torque observed by the sensor. + """ + if self._torque_control_enabled: + pwm = motor_commands + else: + pwm = (-self._kp * (current_motor_angle - motor_commands) + - self._kd * current_motor_velocity) + pwm = np.clip(pwm, -1.0, 1.0) + return self._convert_to_torque_from_pwm(pwm, current_motor_velocity) + + def _convert_to_torque_from_pwm(self, pwm, current_motor_velocity): + """Convert the pwm signal to torque. + + Args: + pwm: The pulse width modulation. + current_motor_velocity: The motor velocity at the current time step. + 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 * (pwm * self._voltage / self._resistance), + -OBSERVED_TORQUE_LIMIT, OBSERVED_TORQUE_LIMIT) + + # Net voltage is clipped at 50V by diodes on the motor controller. + voltage_net = np.clip(pwm * self._voltage - + (self._torque_constant + self._viscous_damping) + * current_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) + return actual_torque, observed_torque diff --git a/realenv/core/physics/robot_bases.py b/realenv/core/physics/robot_bases.py index fa04ec8d0..0cd484ac7 100644 --- a/realenv/core/physics/robot_bases.py +++ b/realenv/core/physics/robot_bases.py @@ -2,6 +2,7 @@ import pybullet as p import gym, gym.spaces, gym.utils +from realenv.data.datasets import MODEL_SCALING import numpy as np import os, inspect currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe()))) @@ -101,6 +102,7 @@ class MJCFBasedRobot: object_ids = p.loadMJCF(os.path.join(self.physics_model_dir, self.model_file), globalScaling = self.scale) if ".urdf" in self.model_file: object_ids = (p.loadURDF(os.path.join(self.physics_model_dir, self.model_file), globalScaling = self.scale), ) + self.parts, self.jdict, self.ordered_joints, self.robot_body = self.addToScene(object_ids) self.robot_specific_reset() diff --git a/realenv/core/physics/robot_loco_quadruped.py b/realenv/core/physics/robot_loco_quadruped.py new file mode 100644 index 000000000..3e2060585 --- /dev/null +++ b/realenv/core/physics/robot_loco_quadruped.py @@ -0,0 +1,627 @@ +"""This file implements the functionalities of a minitaur using pybullet. + +""" +import copy +import math +import numpy as np +from realenv.core.physics import motor +from realenv.core.physics.robot_locomotors import WalkerBase +from realenv.core.physics.robot_bases import Joint, BodyPart +import os +from gym import spaces +from realenv.data.datasets import get_model_initial_pose +import pybullet as p +from transforms3d.euler import euler2quat + +INIT_POSITION = [0, 0, .2] +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" +] +LEG_LINK_ID = [2, 3, 5, 6, 8, 9, 11, 12, 15, 16, 18, 19, 21, 22, 24, 25] +MOTOR_LINK_ID = [1, 4, 7, 10, 14, 17, 20, 23] +FOOT_LINK_ID = [3, 6, 9, 12, 16, 19, 22, 25] +BASE_LINK_ID = -1 +ACTION_EPS = 0.01 +OBSERVATION_EPS = 0.01 + +def quatWXYZ2quatXYZW(wxyz): + return np.concatenate((wxyz[1:], wxyz[:1])) + + +class Minitaur(object): + """The minitaur class that simulates a quadruped robot from Ghost Robotics. + + """ + + def __init__(self, + #pybullet_client, + urdf_root= os.path.join(os.path.dirname(os.path.abspath(__file__)),"models"), + time_step=0.01, + self_collision_enabled=False, + motor_velocity_limit=np.inf, + pd_control_enabled=False, + accurate_motor_model_enabled=False, + motor_kp=1.0, + motor_kd=0.02, + torque_control_enabled=False, + motor_overheat_protection=False, + on_rack=False, + kd_for_pd_controllers=0.3): + """Constructs a minitaur and reset it to the initial states. + + Args: + pybullet_client: The instance of BulletClient to manage different + simulations. + urdf_root: The path to the urdf folder. + time_step: The time step of the simulation. + self_collision_enabled: Whether to enable self collision. + motor_velocity_limit: The upper limit of the motor velocity. + pd_control_enabled: Whether to use PD control for the motors. + accurate_motor_model_enabled: Whether to use the accurate DC motor model. + motor_kp: proportional gain for the accurate motor model + motor_kd: derivative gain for the acurate motor model + torque_control_enabled: Whether to use the torque control, if set to + False, pose control will be used. + 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. + 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. + kd_for_pd_controllers: kd value for the pd controllers of the motors. + """ + self.num_motors = 8 + self.num_legs = int(self.num_motors / 2) + #p = pybullet_client + self._urdf_root = urdf_root + self._self_collision_enabled = self_collision_enabled + self._motor_velocity_limit = motor_velocity_limit + self._pd_control_enabled = pd_control_enabled + self._motor_direction = [-1, -1, -1, -1, 1, 1, 1, 1] + self._observed_motor_torques = np.zeros(self.num_motors) + self._applied_motor_torques = np.zeros(self.num_motors) + self._max_force = 3.5 + self._accurate_motor_model_enabled = accurate_motor_model_enabled + self._torque_control_enabled = torque_control_enabled + self._motor_overheat_protection = motor_overheat_protection + self._on_rack = on_rack + self.model_file = None + if self._accurate_motor_model_enabled: + self._kp = motor_kp + self._kd = motor_kd + self._motor_model = motor.MotorModel( + torque_control_enabled=self._torque_control_enabled, + kp=self._kp, + kd=self._kd) + elif self._pd_control_enabled: + self._kp = 8 + self._kd = kd_for_pd_controllers + else: + self._kp = 1 + self._kd = 1 + self.time_step = time_step + self.parts, self.jdict, self.ordered_joints, self.robot_body = None, None, None, None + self.robot_name = "quadruped" + #self.Reset() + self.robot_specific_reset() + action_dim = 8 + action_high = np.ones(action_dim) + self.action_space = spaces.Box(-action_high, action_high) + observation_high = ( + self.GetObservationUpperBound() + OBSERVATION_EPS) + observation_low = ( + self.GetObservationLowerBound() - OBSERVATION_EPS) + self.observation_space = spaces.Box(observation_low, observation_high) + + + def _RecordMassInfoFromURDF(self): + self._base_mass_urdf = p.getDynamicsInfo( + self.quadruped, BASE_LINK_ID)[0] + self._leg_masses_urdf = [] + self._leg_masses_urdf.append( + p.getDynamicsInfo(self.quadruped, LEG_LINK_ID[0])[ + 0]) + self._leg_masses_urdf.append( + p.getDynamicsInfo(self.quadruped, MOTOR_LINK_ID[0])[ + 0]) + + def _BuildJointNameToIdDict(self): + num_joints = p.getNumJoints(self.quadruped) + self._joint_name_to_id = {} + for i in range(num_joints): + joint_info = p.getJointInfo(self.quadruped, i) + self._joint_name_to_id[joint_info[1].decode("UTF-8")] = joint_info[0] + + def _BuildMotorIdList(self): + self._motor_id_list = [ + self._joint_name_to_id[motor_name] for motor_name in MOTOR_NAMES + ] + + def reset(self): + self.robot_specific_reset() + + #s = self.calc_state() # optimization: calc_state() can calculate something in self.* for calc_potential() to use + self.eyes = self.parts["eyes"] + #return s + + def robot_specific_reset(self, reload_urdf=True): + """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. + """ + if reload_urdf: + if self._self_collision_enabled: + self.quadruped = p.loadURDF( + "%s/quadruped/minitaur.urdf" % self._urdf_root, + INIT_POSITION, + flags=p.URDF_USE_SELF_COLLISION) + else: + self.quadruped = p.loadURDF( + "%s/quadruped/minitaur.urdf" % self._urdf_root, INIT_POSITION) + self._BuildJointNameToIdDict() + self._BuildMotorIdList() + self._RecordMassInfoFromURDF() + self.ResetPose(add_constraint=True) + if self._on_rack: + p.createConstraint( + self.quadruped, -1, -1, -1, p.JOINT_FIXED, + [0, 0, 0], [0, 0, 0], [0, 0, 1]) + else: + p.resetBasePositionAndOrientation( + self.quadruped, INIT_POSITION, INIT_ORIENTATION) + p.resetBaseVelocity(self.quadruped, [0, 0, 0], + [0, 0, 0]) + self.ResetPose(add_constraint=False) + + self.parts, self.jdict, self.ordered_joints, self.robot_body = self.addToScene((self.quadruped, )) + + self._overheat_counter = np.zeros(self.num_motors) + self._motor_enabled_list = [True] * self.num_motors + + orientation, position = get_model_initial_pose("humanoid") + roll = orientation[0] + pitch = orientation[1] + yaw = orientation[2] + self.robot_body.reset_orientation(quatWXYZ2quatXYZW(euler2quat(roll, pitch, yaw))) + self.robot_body.reset_position(position) + + def _SetMotorTorqueById(self, motor_id, torque): + p.setJointMotorControl2( + bodyIndex=self.quadruped, + jointIndex=motor_id, + controlMode=p.TORQUE_CONTROL, + force=torque) + + def _SetDesiredMotorAngleById(self, motor_id, desired_angle): + p.setJointMotorControl2( + bodyIndex=self.quadruped, + jointIndex=motor_id, + controlMode=p.POSITION_CONTROL, + targetPosition=desired_angle, + positionGain=self._kp, + velocityGain=self._kd, + force=self._max_force) + + def _SetDesiredMotorAngleByName(self, motor_name, desired_angle): + self._SetDesiredMotorAngleById(self._joint_name_to_id[motor_name], + desired_angle) + + def calc_potential(self): + return 0 + + def addToScene(self, bodies): + if self.parts is not None: + parts = self.parts + else: + parts = {} + + if self.jdict is not None: + joints = self.jdict + else: + joints = {} + + if self.ordered_joints is not None: + ordered_joints = self.ordered_joints + else: + ordered_joints = [] + + dump = 0 + for i in range(len(bodies)): + if p.getNumJoints(bodies[i]) == 0: + part_name, robot_name = p.getBodyInfo(bodies[i], 0) + robot_name = robot_name.decode("utf8") + part_name = part_name.decode("utf8") + parts[part_name] = BodyPart(part_name, bodies, i, -1) + for j in range(p.getNumJoints(bodies[i])): + p.setJointMotorControl2(bodies[i],j,p.POSITION_CONTROL,positionGain=0.1,velocityGain=0.1,force=0) + _,joint_name,joint_type,_,_,_,_,_,_,_,_,_,part_name = p.getJointInfo(bodies[i], j) + + joint_name = joint_name.decode("utf8") + part_name = part_name.decode("utf8") + + if dump: print("ROBOT PART '%s'" % part_name) + if dump: print("ROBOT JOINT '%s'" % joint_name) # limits = %+0.2f..%+0.2f effort=%0.3f speed=%0.3f" % ((joint_name,) + j.limits()) ) + + parts[part_name] = BodyPart(part_name, bodies, i, j) + + if part_name == self.robot_name: + self.robot_body = parts[part_name] + + if i == 0 and j == 0 and self.robot_body is None: # if nothing else works, we take this as robot_body + parts[self.robot_name] = BodyPart(self.robot_name, bodies, 0, -1) + self.robot_body = parts[self.robot_name] + + if joint_name[:6] == "ignore": + Joint(joint_name, bodies, i, j).disable_motor() + continue + + if joint_name[:8] != "jointfix" and joint_type != p.JOINT_FIXED: + joints[joint_name] = Joint(joint_name, bodies, i, j) + ordered_joints.append(joints[joint_name]) + + joints[joint_name].power_coef = 100.0 + return parts, joints, ordered_joints, self.robot_body + + + 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] + p.resetJointState( + self.quadruped, + self._joint_name_to_id["motor_" + leg_position + "L_joint"], + self._motor_direction[2 * leg_id] * half_pi, + targetVelocity=0) + p.resetJointState( + self.quadruped, + self._joint_name_to_id["knee_" + leg_position + "L_link"], + self._motor_direction[2 * leg_id] * knee_angle, + targetVelocity=0) + p.resetJointState( + self.quadruped, + self._joint_name_to_id["motor_" + leg_position + "R_joint"], + self._motor_direction[2 * leg_id + 1] * half_pi, + targetVelocity=0) + p.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: + p.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"], + p.JOINT_POINT2POINT, [0, 0, 0], + KNEE_CONSTRAINT_POINT_RIGHT, KNEE_CONSTRAINT_POINT_LEFT) + + if self._accurate_motor_model_enabled or self._pd_control_enabled: + # Disable the default motor in pybullet. + p.setJointMotorControl2( + bodyIndex=self.quadruped, + jointIndex=(self._joint_name_to_id["motor_" + + leg_position + "L_joint"]), + controlMode=p.VELOCITY_CONTROL, + targetVelocity=0, + force=knee_friction_force) + p.setJointMotorControl2( + bodyIndex=self.quadruped, + jointIndex=(self._joint_name_to_id["motor_" + + leg_position + "R_joint"]), + controlMode=p.VELOCITY_CONTROL, + targetVelocity=0, + force=knee_friction_force) + + else: + self._SetDesiredMotorAngleByName( + "motor_" + leg_position + "L_joint", + self._motor_direction[2 * leg_id] * half_pi) + self._SetDesiredMotorAngleByName("motor_" + leg_position + "R_joint", + self._motor_direction[2 * leg_id + + 1] * half_pi) + + p.setJointMotorControl2( + bodyIndex=self.quadruped, + jointIndex=(self._joint_name_to_id["knee_" + leg_position + "L_link"]), + controlMode=p.VELOCITY_CONTROL, + targetVelocity=0, + force=knee_friction_force) + p.setJointMotorControl2( + bodyIndex=self.quadruped, + jointIndex=(self._joint_name_to_id["knee_" + leg_position + "R_link"]), + controlMode=p.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. + """ + position, _ = ( + p.getBasePositionAndOrientation(self.quadruped)) + return position + + def GetBaseOrientation(self): + """Get the orientation of minitaur's base, represented as quaternion. + + Returns: + The orientation of minitaur's base. + """ + _, orientation = ( + p.getBasePositionAndOrientation(self.quadruped)) + return orientation + + def GetActionDimension(self): + """Get the length of the action list. + + Returns: + The length of the action list. + """ + return self.num_motors + + def GetObservationUpperBound(self): + """Get the upper bound of the observation. + + Returns: + The upper bound of an observation. See GetObservation() for the details + of each element of an observation. + """ + upper_bound = np.array([0.0] * self.GetObservationDimension()) + upper_bound[0:self.num_motors] = math.pi # Joint angle. + upper_bound[self.num_motors:2 * self.num_motors] = ( + motor.MOTOR_SPEED_LIMIT) # Joint velocity. + upper_bound[2 * self.num_motors:3 * self.num_motors] = ( + motor.OBSERVED_TORQUE_LIMIT) # Joint torque. + upper_bound[3 * self.num_motors:] = 1.0 # Quaternion of base orientation. + return upper_bound + + def GetObservationLowerBound(self): + """Get the lower bound of the observation.""" + return -self.GetObservationUpperBound() + + def GetObservationDimension(self): + """Get the length of the observation list. + + Returns: + The length of the observation list. + """ + return len(self.GetObservation()) + + def GetObservation(self): + """Get the observations of minitaur. + + It includes the angles, velocities, torques and the orientation of the base. + + Returns: + The observation list. observation[0:8] are motor angles. observation[8:16] + are motor velocities, observation[16:24] are motor torques. + observation[24:28] is the orientation of the base, in quaternion form. + """ + observation = [] + observation.extend(self.GetMotorAngles().tolist()) + observation.extend(self.GetMotorVelocities().tolist()) + observation.extend(self.GetMotorTorques().tolist()) + observation.extend(list(self.GetBaseOrientation())) + return observation + + def ApplyAction(self, motor_commands): + """Set the desired motor angles to the motors of the minitaur. + + The desired motor angles are clipped based on the maximum allowed velocity. + If the pd_control_enabled is True, a torque is calculated according to + the difference between current and desired joint angle, as well as the joint + velocity. This torque is exerted to the motor. For more information about + PD control, please refer to: https://en.wikipedia.org/wiki/PID_controller. + + Args: + motor_commands: The eight desired motor angles. + """ + if self._motor_velocity_limit < np.inf: + current_motor_angle = self.GetMotorAngles() + motor_commands_max = ( + current_motor_angle + self.time_step * self._motor_velocity_limit) + motor_commands_min = ( + current_motor_angle - self.time_step * self._motor_velocity_limit) + motor_commands = np.clip(motor_commands, motor_commands_min, + motor_commands_max) + + if self._accurate_motor_model_enabled or self._pd_control_enabled: + q = self.GetMotorAngles() + qdot = self.GetMotorVelocities() + if self._accurate_motor_model_enabled: + actual_torque, observed_torque = self._motor_model.convert_to_torque( + motor_commands, q, qdot) + 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 + + # 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) + + for motor_id, motor_torque, motor_enabled in zip( + self._motor_id_list, self._applied_motor_torque, + self._motor_enabled_list): + if motor_enabled: + self._SetMotorTorqueById(motor_id, motor_torque) + else: + self._SetMotorTorqueById(motor_id, 0) + else: + torque_commands = -self._kp * (q - motor_commands) - self._kd * qdot + + # The torque is already in the observation space because we use + # GetMotorAngles and GetMotorVelocities. + self._observed_motor_torques = torque_commands + + # Transform into the motor space when applying the torque. + self._applied_motor_torques = np.multiply(self._observed_motor_torques, + self._motor_direction) + + for motor_id, motor_torque in zip(self._motor_id_list, + self._applied_motor_torques): + self._SetMotorTorqueById(motor_id, motor_torque) + else: + motor_commands_with_direction = np.multiply(motor_commands, + self._motor_direction) + for motor_id, motor_command_with_direction in zip( + self._motor_id_list, motor_commands_with_direction): + self._SetDesiredMotorAngleById(motor_id, motor_command_with_direction) + + def GetMotorAngles(self): + """Get the eight motor angles at the current moment. + + Returns: + Motor angles. + """ + motor_angles = [ + p.getJointState(self.quadruped, motor_id)[0] + for motor_id in self._motor_id_list + ] + motor_angles = np.multiply(motor_angles, self._motor_direction) + return motor_angles + + def GetMotorVelocities(self): + """Get the velocity of all eight motors. + + Returns: + Velocities of all eight motors. + """ + motor_velocities = [ + p.getJointState(self.quadruped, motor_id)[1] + for motor_id in self._motor_id_list + ] + motor_velocities = np.multiply(motor_velocities, self._motor_direction) + return motor_velocities + + def GetMotorTorques(self): + """Get the amount of torques the motors are exerting. + + Returns: + Motor torques of all eight motors. + """ + if self._accurate_motor_model_enabled or self._pd_control_enabled: + return self._observed_motor_torques + else: + motor_torques = [ + p.getJointState(self.quadruped, motor_id)[3] + for motor_id in self._motor_id_list + ] + motor_torques = np.multiply(motor_torques, self._motor_direction) + return 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 = int(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 GetBaseMassFromURDF(self): + """Get the mass of the base from the URDF file.""" + return self._base_mass_urdf + + def GetLegMassesFromURDF(self): + """Get the mass of the legs from the URDF file.""" + return self._leg_masses_urdf + + def SetBaseMass(self, base_mass): + p.changeDynamics( + self.quadruped, BASE_LINK_ID, mass=base_mass) + + def SetLegMasses(self, leg_masses): + """Set the mass of the legs. + + A leg includes leg_link and motor. All four leg_links have the same mass, + which is leg_masses[0]. All four motors have the same mass, which is + leg_mass[1]. + + Args: + leg_masses: The leg masses. leg_masses[0] is the mass of the leg link. + leg_masses[1] is the mass of the motor. + """ + for link_id in LEG_LINK_ID: + p.changeDynamics( + self.quadruped, link_id, mass=leg_masses[0]) + for link_id in MOTOR_LINK_ID: + p.changeDynamics( + self.quadruped, link_id, mass=leg_masses[1]) + + 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 FOOT_LINK_ID: + p.changeDynamics( + self.quadruped, link_id, lateralFriction=foot_friction) + + def SetBatteryVoltage(self, voltage): + if self._accurate_motor_model_enabled: + self._motor_model.set_voltage(voltage) + + def SetMotorViscousDamping(self, viscous_damping): + if self._accurate_motor_model_enabled: + self._motor_model.set_viscous_damping(viscous_damping) diff --git a/realenv/core/physics/robot_locomotors.py b/realenv/core/physics/robot_locomotors.py index ca925870a..9d30beb37 100644 --- a/realenv/core/physics/robot_locomotors.py +++ b/realenv/core/physics/robot_locomotors.py @@ -236,11 +236,15 @@ class Husky(WalkerBase): #self.eye_offset_orn = euler2quat(np.pi/2, 0, np.pi/2, axes='sxyz') self.eye_offset_orn = euler2quat(np.pi/2, 0, np.pi/2, axes='sxyz') - self.torque = 0.2 + + self.torque = 0.1 self.action_list = [[self.torque, self.torque, self.torque, self.torque], [-self.torque, -self.torque, -self.torque, -self.torque], [self.torque, -self.torque, self.torque, -self.torque], - [-self.torque, self.torque, -self.torque, self.torque], [0, 0, 0, 0]] + [-self.torque, self.torque, -self.torque, self.torque], + [0, 0, 0, 0]] + + self.setup_keys_to_action() def apply_action(self, action): if self.is_discrete: @@ -260,3 +264,12 @@ class Husky(WalkerBase): def alive_bonus(self, z, pitch): return +1 if z > 0.26 else -1 # 0.25 is central sphere rad, die if it scrapes the ground + + def setup_keys_to_action(self): + self.keys_to_action = { + (ord('s'), ): 0, ## backward + (ord('w'), ): 1, ## forward + (ord('d'), ): 2, ## turn right + (ord('a'), ): 3, ## turn left + (): 4 + } \ No newline at end of file diff --git a/realenv/core/physics/scene_building.py b/realenv/core/physics/scene_building.py index eb8bd9932..7e42ad97b 100644 --- a/realenv/core/physics/scene_building.py +++ b/realenv/core/physics/scene_building.py @@ -25,7 +25,8 @@ class BuildingScene(Scene): print(filename) #visualId = p.createVisualShape(p.GEOM_MESH, fileName=filename, meshScale=original, rgbaColor = [93/255.0,95/255.0, 96/255.0,0.75], specularColor=[0.4, 0.4, 0.4]) boundaryUid = p.createMultiBody(baseCollisionShapeIndex = collisionId, baseVisualShapeIndex = 0) - #p.changeVisualShape(boundaryUid, -1, rgbaColor=[1, 0.2, 0.2, 0.3], specularColor=[1, 1, 1]) + #visualId = p.loadTexture(os.path.join(os.path.dirname(os.path.abspath(__file__)), "tex256.png")) + #p.changeVisualShape(boundaryUid, -1, textureUniqueId=visualId) #self.building_obj = [collisionId] #planeName = os.path.join(pybullet_data.getDataPath(),"mjcf/ground_plane.xml") #self.ground_plane_mjcf = p.loadMJCF(planeName) @@ -33,10 +34,12 @@ class BuildingScene(Scene): p.changeDynamics(boundaryUid, -1, lateralFriction=0.8, spinningFriction=0.1, rollingFriction=0.1) self.building_obj = (boundaryUid, ) #self.building_obj = (int(p.loadURDF(filename)), ) - for i in self.building_obj: + + #for i in self.building_obj: #collisionId = p.createCollisionShape(p.GEOM_MESH, fileName=filename, meshScale=[1, 1, 1], flags=p.GEOM_FORCE_CONCAVE_TRIMESH) - p.changeVisualShape(i,-1,rgbaColor=[93/255.0,95/255.0, 96/255.0,0.75], specularColor=[0.4, 0.4, 0.4]) - + #p.changeVisualShape(boundaryUid, -1, textureUniqueId=visualId) + #p.changeVisualShape(i,-1,rgbaColor=[93/255.0,95/255.0, 96/255.0,0.75], specularColor=[0.4, 0.4, 0.4]) + class SinglePlayerBuildingScene(BuildingScene): multiplayer = False diff --git a/realenv/core/render/show_3d2.py b/realenv/core/render/show_3d2.py index 0b82197d0..e1a8cbc3e 100644 --- a/realenv/core/render/show_3d2.py +++ b/realenv/core/render/show_3d2.py @@ -18,7 +18,7 @@ from numpy import cos, sin from realenv.core.render.profiler import Profiler from multiprocessing import Process -from realenv.data.datasets import ViewDataSet3D, MAKE_VIDEO +from realenv.data.datasets import ViewDataSet3D, MAKE_VIDEO, HIGH_RES_MONITOR, LIVE_DEMO from realenv.core.render.completion import CompletionNet from realenv.learn.completion2 import CompletionNet2 import torch.nn as nn @@ -81,7 +81,6 @@ class PCRenderer: self.target = target self.model = None self.old_topk = set([]) - self.compare_filler = MAKE_VIDEO self.k = 5 self.showsz = 512 @@ -95,7 +94,7 @@ class PCRenderer: self.show_rgb = np.zeros((self.showsz, self.showsz ,3),dtype='uint8') self.show_unfilled = None - if self.compare_filler: + if MAKE_VIDEO: self.show_unfilled = np.zeros((self.showsz, self.showsz, 3),dtype='uint8') @@ -113,16 +112,24 @@ class PCRenderer: self.renderToScreenSetup() def renderToScreenSetup(self): - cv2.namedWindow('show3d') - cv2.namedWindow('target depth') - if self.compare_filler: - cv2.moveWindow('show3d', -30 , self.showsz + LINUX_OFFSET['y_delta']) - cv2.moveWindow('target depth', self.showsz + LINUX_OFFSET['x_delta'] + LINUX_OFFSET['y_delta'], self.showsz + LINUX_OFFSET['y_delta']) - cv2.imshow('show3d', self.show_rgb) - cv2.imshow('target depth', self.show_rgb) - cv2.setMouseCallback('show3d',self._onmouse) - if self.compare_filler: - cv2.namedWindow('show3d unfilled') + cv2.namedWindow('RGB cam') + cv2.namedWindow('Depth cam') + if MAKE_VIDEO: + cv2.moveWindow('RGB cam', -1 , self.showsz + LINUX_OFFSET['y_delta']) + cv2.moveWindow('Depth cam', self.showsz + LINUX_OFFSET['x_delta'] + LINUX_OFFSET['y_delta'], -1) + cv2.namedWindow('RGB unfilled') + cv2.moveWindow('RGB unfilled', self.showsz + LINUX_OFFSET['x_delta'] + LINUX_OFFSET['y_delta'], self.showsz + LINUX_OFFSET['y_delta']) + elif HIGH_RES_MONITOR: + cv2.moveWindow('RGB cam', -1 , self.showsz + LINUX_OFFSET['y_delta']) + cv2.moveWindow('Depth cam', self.showsz + LINUX_OFFSET['x_delta'] + LINUX_OFFSET['y_delta'], self.showsz + LINUX_OFFSET['y_delta']) + + if LIVE_DEMO: + cv2.moveWindow('RGB cam', -1 , 768) + cv2.moveWindow('Depth cam', 512, 768) + + #cv2.imshow('RGB cam', self.show_rgb) + #cv2.imshow('Depth cam', self.show_rgb) + #cv2.setMouseCallback('RGB cam',self._onmouse) def _onmouse(self, *args): @@ -276,7 +283,7 @@ class PCRenderer: #[t.join() for t in threads] _render_pc(opengl_arr) - if self.compare_filler: + if MAKE_VIDEO: show_unfilled[:, :, :] = show[:, :, :] if self.model: tf = transforms.ToTensor() @@ -356,7 +363,7 @@ class PCRenderer: self.render(self.imgs_topk, self.depths_topk, self.render_cpose.astype(np.float32), self.model, self.relative_poses_topk, self.target_poses[0], self.show, self.show_unfilled) self.show_rgb = cv2.cvtColor(self.show, cv2.COLOR_BGR2RGB) - if self.compare_filler: + if MAKE_VIDEO: self.show_unfilled_rgb = cv2.cvtColor(self.show_unfilled, cv2.COLOR_BGR2RGB) return self.show_rgb @@ -372,18 +379,19 @@ class PCRenderer: def _render_depth(depth): #with Profiler("Render Depth"): - cv2.imshow('target depth', depth/16.) + cv2.imshow('Depth cam', depth/16.) + if HIGH_RES_MONITOR and not MAKE_VIDEO: + cv2.moveWindow('Depth cam', self.showsz + LINUX_OFFSET['x_delta'] + LINUX_OFFSET['y_delta'], LINUX_OFFSET['y_delta']) def _render_rgb(rgb): - cv2.imshow('show3d',rgb) - if self.compare_filler: - cv2.moveWindow('show3d', -1 , self.showsz + LINUX_OFFSET['y_delta']) + cv2.imshow('RGB cam',rgb) + if HIGH_RES_MONITOR and not MAKE_VIDEO: + cv2.moveWindow('RGB cam', -1 , self.showsz + LINUX_OFFSET['y_delta']) def _render_rgb_unfilled(unfilled_rgb): - cv2.imshow('show3d unfilled', unfilled_rgb) - if self.compare_filler: - cv2.moveWindow('target depth', self.showsz + LINUX_OFFSET['x_delta'] + LINUX_OFFSET['y_delta'], self.showsz + LINUX_OFFSET['y_delta']) - + assert(MAKE_VIDEO) + cv2.imshow('RGB unfilled', unfilled_rgb) + """ render_threads = [ Process(target=_render_depth, args=(self.target_depth, )), @@ -396,9 +404,8 @@ class PCRenderer: """ _render_depth(self.target_depth) _render_rgb(self.show_rgb) - if self.compare_filler: + if MAKE_VIDEO: _render_rgb_unfilled(self.show_unfilled_rgb) - #cv2.imshow('show3d unfilled', self.show_unfilled_rgb) ## TODO (hzyjerry): does this introduce extra time delay? cv2.waitKey(1) diff --git a/realenv/data/datasets.py b/realenv/data/datasets.py index 65109402b..185820752 100644 --- a/realenv/data/datasets.py +++ b/realenv/data/datasets.py @@ -17,11 +17,15 @@ import json from numpy.linalg import inv import pickle +HIGH_RES_MONITOR = False +MAKE_VIDEO = True +LIVE_DEMO = False -MAKE_VIDEO = False +MODEL_SCALING = 0.7 ## Small model: 11HB6XZSh1Q -## Gates Huang: BbxejD15Etk +## Psych model: BbxejD15Etk +## Gates 1st: sRj553CTHiw MODEL_ID = "11HB6XZSh1Q" IMG_EXTENSIONS = [ @@ -55,12 +59,21 @@ def get_model_initial_pose(robot): if MODEL_ID == "11HB6XZSh1Q": #return [0, 0, 3 * 3.14/2], [-3.38, -7, 1.4] ## living room open area #return [0, 0, 3 * 3.14/2], [-4.8, -5.2, 1.9] ## living room kitchen table - return [0, 0, 3.14/2], [-4.655, -9.038, 1.532] ## living room couch - #return [0, 0, 3.14], [-0.603, -1.24, 2.35] + #return [0, 0, 3.14/2], [-4.655, -9.038, 1.532] ## living room couch + return [0, 0, 3.14], [-0.603, -1.24, 2.35] ## stairs if MODEL_ID == "BbxejD15Etk": return [0, 0, 3 * 3.14/2], [-6.76, -12, 1.4] ## Gates Huang elif robot=="husky": - return [0, 0, 3.14], [-2, 3.5, 0.4] + if MODEL_ID == "11HB6XZSh1Q": + return [0, 0, 3.14], [-2, 3.5, 0.4] ## living room + #return [0, 0, 0], [-0.203, -1.74, 1.8] ## stairs + elif MODEL_ID == "sRj553CTHiw": + return [0, 0, 3.14], [-7, 2.6, 0.8] + elif MODEL_ID == "BbxejD15Etk": + return [0, 0, 3.14], [0, 0, 0.4] + elif robot=="quadruped": + return [0, 0, 3.14], [-2, 3.5, 0.4] ## living room + #return [0, 0, 0], [-0.203, -1.74, 1.8] ## stairs else: return [0, 0, 0], [0, 0, 1.4] diff --git a/realenv/envs/env_bases.py b/realenv/envs/env_bases.py index 5e158be55..1e1756e38 100644 --- a/realenv/envs/env_bases.py +++ b/realenv/envs/env_bases.py @@ -36,7 +36,6 @@ class MJCFBaseEnv(gym.Env): # @self.human # @self.robot self.scene = None - self.physicsClientId=-1 self.camera = Camera() self._seed() self._cam_dist = 3 @@ -47,6 +46,17 @@ class MJCFBaseEnv(gym.Env): self.action_space = self.robot.action_space self.observation_space = self.robot.observation_space + if (self.physicsClientId<0): + self.physicsClientId = p.connect(p.SHARED_MEMORY) + if (self.physicsClientId < 0): + if (self.human): + self.physicsClientId = p.connect(p.GUI) + if MAKE_VIDEO: + #self.set_window(-1, -1, 1024, 512) + self.set_window(-1, -1, 512, 512) + else: + self.physicsClientId = p.connect(p.DIRECT) + def configure(self, args): self.robot.args = args def _seed(self, seed=None): @@ -55,17 +65,11 @@ class MJCFBaseEnv(gym.Env): return [seed] def _reset(self): - if (self.physicsClientId<0): - self.physicsClientId = p.connect(p.SHARED_MEMORY) - if (self.physicsClientId < 0): - if (self.human): - self.physicsClientId = p.connect(p.GUI) - if MAKE_VIDEO: - self.set_window(-1, -1, 1024, 512) - else: - self.physicsClientId = p.connect(p.DIRECT) p.configureDebugVisualizer(p.COV_ENABLE_GUI,0) p.configureDebugVisualizer(p.COV_ENABLE_KEYBOARD_SHORTCUTS, 0) + p.configureDebugVisualizer(p.COV_ENABLE_MOUSE_PICKING, 1) + p.configureDebugVisualizer(p.COV_ENABLE_SHADOWS, 1) + #p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1) if self.scene is None: self.scene = self.create_single_player_scene() @@ -124,17 +128,21 @@ class MJCFBaseEnv(gym.Env): self.physicsClientId = -1 def set_window(self, posX, posY, sizeX, sizeY): - values = { + values = { + 'name': "robot", 'gravity': 0, 'posX': int(posX), 'posY': int(posY), 'sizeX': int(sizeX), 'sizeY': int(sizeY) } - #os.system('wmctrl -r :ACTIVE: -e {},{},{},{},{}'.format(0, posX, posY, sizeX, sizeY)) - cmd = 'wmctrl -r :ACTIVE: -e {gravity},{posX},{posY},{sizeX},{sizeY}'.format(**values) + cmd = 'wmctrl -r \"Bullet Physics\" -e {gravity},{posX},{posY},{sizeX},{sizeY}'.format(**values) os.system(cmd) + cmd = "xdotool search --name \"Bullet Physics\" set_window --name \"robot's world\"" + os.system(cmd) + + def HUD(self, state, a, done): pass diff --git a/realenv/envs/humanoid_env.py b/realenv/envs/humanoid_env.py index 1d18e8ca4..7818aa1cd 100644 --- a/realenv/envs/humanoid_env.py +++ b/realenv/envs/humanoid_env.py @@ -14,6 +14,7 @@ class HumanoidEnv(gym.Env): frame_skip = 20 def __init__(self): self.robot = Humanoid() + self.physicsClientId=-1 self.electricity_cost = 4.25*SensorRobotEnv.electricity_cost self.stall_torque_cost = 4.25*SensorRobotEnv.stall_torque_cost @@ -27,13 +28,17 @@ class HumanoidCameraEnv(HumanoidEnv, CameraRobotEnv): self.enable_sensors = enable_sensors HumanoidEnv.__init__(self) CameraRobotEnv.__init__(self) - self.tracking_camera['yaw'] = 60 - self.tracking_camera['distance'] = 1.5 + #self.tracking_camera['yaw'] = 30 ## living room + #self.tracking_camera['distance'] = 1.5 + #self.tracking_camera['pitch'] = -45 ## stairs + #distance=2.5 ## demo: living room ,kitchen - #distance=1.7 ## demo: stairs + self.tracking_camera['distance'] = 1.7 ## demo: stairs + self.tracking_camera['pitch'] = -45 ## stairs + #yaw = 0 ## demo: living room #yaw = 30 ## demo: kitchen - #yaw = 90 ## demo: stairs + self.tracking_camera['yaw'] = 70 ## demo: stairs class HumanoidSensorEnv(HumanoidEnv, SensorRobotEnv): diff --git a/realenv/envs/husky_env.py b/realenv/envs/husky_env.py index 40f45c017..81943b4c4 100644 --- a/realenv/envs/husky_env.py +++ b/realenv/envs/husky_env.py @@ -13,7 +13,11 @@ class HuskyEnv: 'video.frames_per_second' : 30 } def __init__(self, is_discrete=False): + self.physicsClientId=-1 self.robot = Husky(is_discrete) + + def get_keys_to_action(self): + return self.robot.keys_to_action class HuskyCameraEnv(HuskyEnv, CameraRobotEnv): @@ -26,10 +30,20 @@ class HuskyCameraEnv(HuskyEnv, CameraRobotEnv): self.enable_sensors = enable_sensors HuskyEnv.__init__(self, is_discrete) CameraRobotEnv.__init__(self) - self.tracking_camera['yaw'] = 80 + + #self.tracking_camera['pitch'] = -45 ## stairs + yaw = 90 ## demo: living room + #yaw = 30 ## demo: kitchen + offset = 0.5 + distance = 1.2 ## living room + #self.tracking_camera['yaw'] = 90 ## demo: stairs + + + self.tracking_camera['yaw'] = yaw ## living roon self.tracking_camera['pitch'] = -10 - self.tracking_camera['distance'] = 1.5 - self.tracking_camera['z_offset'] = 0.5 + + self.tracking_camera['distance'] = distance + self.tracking_camera['z_offset'] = offset class HuskySensorEnv(HuskyEnv, SensorRobotEnv): def __init__(self, human=True, timestep=HUMANOID_TIMESTEP, @@ -136,7 +150,16 @@ class HuskySensorEnv(HuskyEnv, SensorRobotEnv): print(sum(self.rewards)) return state, sum(self.rewards), bool(done), {"eye_pos": eye_pos, "eye_quat": eye_quat} + #self.tracking_camera['pitch'] = -45 ## stairs + yaw = 90 ## demo: living room + #yaw = 30 ## demo: kitchen + offset = 0.5 + distance = 1.2 ## living room + #self.tracking_camera['yaw'] = 90 ## demo: stairs - - - + + self.tracking_camera['yaw'] = yaw ## living roon + self.tracking_camera['pitch'] = -10 + + self.tracking_camera['distance'] = distance + self.tracking_camera['z_offset'] = offset \ No newline at end of file diff --git a/realenv/envs/quadruped_env.py b/realenv/envs/quadruped_env.py new file mode 100644 index 000000000..d3347cbf8 --- /dev/null +++ b/realenv/envs/quadruped_env.py @@ -0,0 +1,473 @@ +from realenv.envs.env_modalities import CameraRobotEnv, SensorRobotEnv +from realenv.core.physics.robot_loco_quadruped import Minitaur +import os, inspect +currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe()))) +parentdir = os.path.dirname(os.path.dirname(currentdir)) +os.sys.path.insert(0,parentdir) + +import math +import time +import gym +from gym import spaces +from gym.utils import seeding +import realenv +import numpy as np +import pybullet +from realenv.core.physics.robot_loco_quadruped import Minitaur +from realenv.data.datasets import get_engine_framerate, MAKE_VIDEO + +HUMANOID_TIMESTEP = 1.0/(4 * 22) +HUMANOID_FRAMESKIP = 4 + +NUM_SUBSTEPS = 5 +NUM_MOTORS = 8 +MOTOR_ANGLE_OBSERVATION_INDEX = 0 +MOTOR_VELOCITY_OBSERVATION_INDEX = MOTOR_ANGLE_OBSERVATION_INDEX + NUM_MOTORS +MOTOR_TORQUE_OBSERVATION_INDEX = MOTOR_VELOCITY_OBSERVATION_INDEX + NUM_MOTORS +BASE_ORIENTATION_OBSERVATION_INDEX = MOTOR_TORQUE_OBSERVATION_INDEX + NUM_MOTORS +ACTION_EPS = 0.01 +OBSERVATION_EPS = 0.01 +RENDER_HEIGHT = 720 +RENDER_WIDTH = 960 + +""" +class QuadrupedEnv: + metadata = { + 'render.modes' : ['human', 'rgb_array'], + 'video.frames_per_second' : 30 + } + def __init__(self, is_discrete=False): + self.robot = Minitaur() + + def get_keys_to_action(self): + return self.robot.keys_to_action +""" + + + + +class QuadrupedEnv(gym.Env): + """The gym environment for the minitaur. + + It simulates the locomotion of a minitaur, a quadruped robot. The state space + include the angles, velocities and torques for all the motors and the action + space is the desired motor angle for each motor. The reward function is based + on how far the minitaur walks in 1000 steps and penalizes the energy + expenditure. + + """ + metadata = { + "render.modes": ["human", "rgb_array"], + "video.frames_per_second": 50 + } + + def __init__(self, + is_discrete=False, + urdf_root=os.path.join(os.path.dirname(os.path.abspath(realenv.__file__)), "core", "physics", "models"), + #action_repeat=1, + distance_weight=1.0, + energy_weight=0.005, + shake_weight=0.0, + drift_weight=0.0, + distance_limit=float("inf"), + observation_noise_stdev=0.0, + self_collision_enabled=True, + motor_velocity_limit=np.inf, + pd_control_enabled=False,#not needed to be true if accurate motor model is enabled (has its own better PD) + leg_model_enabled=True, + accurate_motor_model_enabled=True, + motor_kp=1.0, + motor_kd=0.02, + torque_control_enabled=False, + motor_overheat_protection=True, + hard_reset=True, + on_rack=False, + render=False, + kd_for_pd_controllers=0.3, + env_randomizer=None): + """Initialize the minitaur gym environment. + + Args: + urdf_root: The path to the urdf data folder. + action_repeat: The number of simulation steps before actions are applied. + distance_weight: The weight of the distance term in the reward. + energy_weight: The weight of the energy term in the reward. + shake_weight: The weight of the vertical shakiness term in the reward. + drift_weight: The weight of the sideways drift term in the reward. + distance_limit: The maximum distance to terminate the episode. + observation_noise_stdev: The standard deviation of observation noise. + self_collision_enabled: Whether to enable self collision in the sim. + motor_velocity_limit: The velocity limit of each motor. + pd_control_enabled: Whether to use PD controller for each motor. + leg_model_enabled: Whether to use a leg motor to reparameterize the action + space. + accurate_motor_model_enabled: Whether to use the accurate DC motor model. + motor_kp: proportional gain for the accurate motor model. + motor_kd: derivative gain for the accurate motor model. + torque_control_enabled: Whether to use the torque control, if set to + False, pose control will be used. + 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. + hard_reset: Whether to wipe the simulation and load everything when reset + is called. If set to false, reset just place the minitaur back to start + position and set its pose to initial configuration. + 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. + render: Whether to render the simulation. + kd_for_pd_controllers: kd value for the pd controllers of the motors + env_randomizer: An EnvRandomizer to randomize the physical properties + during reset(). + """ + self._time_step = 0.01 + #self._action_repeat = action_repeat + #self._num_bullet_solver_iterations = 300 + self._urdf_root = urdf_root + self._self_collision_enabled = self_collision_enabled + self._motor_velocity_limit = motor_velocity_limit + self._observation = [] + self._env_step_counter = 0 + #self._is_render = render + self._last_base_position = [0, 0, 0] + self._distance_weight = distance_weight + self._energy_weight = energy_weight + self._drift_weight = drift_weight + self._shake_weight = shake_weight + self._distance_limit = distance_limit + self._observation_noise_stdev = observation_noise_stdev + self._action_bound = 1 + self._pd_control_enabled = pd_control_enabled + self._leg_model_enabled = leg_model_enabled + self._accurate_motor_model_enabled = accurate_motor_model_enabled + self._motor_kp = motor_kp + self._motor_kd = motor_kd + self._torque_control_enabled = torque_control_enabled + self._motor_overheat_protection = motor_overheat_protection + self._on_rack = on_rack + self._cam_dist = 1.0 + self._cam_yaw = 0 + self._cam_pitch = -30 + self._hard_reset = True + self._kd_for_pd_controllers = kd_for_pd_controllers + self._last_frame_time = 0.0 + print("urdf_root=" + self._urdf_root) + self._env_randomizer = env_randomizer + if (self.human): + self.physicsClientId = pybullet.connect(pybullet.GUI) + if MAKE_VIDEO: + #self.set_window(-1, -1, 1024, 512) + self.set_window(-1, -1, 512, 512) + else: + self.physicsClientId = pybullet.connect(pybullet.DIRECT) + # PD control needs smaller time step for stability. + #if pd_control_enabled or accurate_motor_model_enabled: + #self._time_step /= NUM_SUBSTEPS + #self._num_bullet_solver_iterations /= NUM_SUBSTEPS + #self._action_repeat *= NUM_SUBSTEPS + + ''' + if self._is_render: + pybullet = bullet_client.BulletClient( + connection_mode=pybullet.GUI) + else: + pybullet = bullet_client.BulletClient() + ''' + self._seed() + #self._reset() + ## TODO (hzyjerry): to avoid calling parent level reset + if self._hard_reset: + #pybullet.resetSimulation() + #pybullet.setPhysicsEngineParameter( + # numSolverIterations=int(self._num_bullet_solver_iterations)) + #pybullet.setTimeStep(self._time_step) + #pybullet.loadURDF("%s/plane.urdf" % self._urdf_root) + #pybullet.setGravity(0, 0, -10) + acc_motor = self._accurate_motor_model_enabled + motor_protect = self._motor_overheat_protection + self.robot = Minitaur( + #pybullet_client=pybullet, + urdf_root=self._urdf_root, + time_step=self._time_step, + self_collision_enabled=self._self_collision_enabled, + motor_velocity_limit=self._motor_velocity_limit, + pd_control_enabled=self._pd_control_enabled, + accurate_motor_model_enabled=acc_motor, + motor_kp=self._motor_kp, + motor_kd=self._motor_kd, + torque_control_enabled=self._torque_control_enabled, + motor_overheat_protection=motor_protect, + on_rack=self._on_rack, + kd_for_pd_controllers=self._kd_for_pd_controllers) + else: + self.robot.Reset(reload_urdf=False) + + if self._env_randomizer is not None: + self._env_randomizer.randomize_env(self) + + self._env_step_counter = 0 + self._last_base_position = [0, 0, 0] + self._objectives = [] + + self.viewer = None + self._hard_reset = hard_reset # This assignment need to be after reset() + + def set_env_randomizer(self, env_randomizer): + self._env_randomizer = env_randomizer + + def configure(self, args): + self._args = args + + def _reset(self): + if self._hard_reset: + #pybullet.resetSimulation() + #pybullet.setPhysicsEngineParameter( + # numSolverIterations=int(self._num_bullet_solver_iterations)) + #pybullet.setTimeStep(self._time_step) + #pybullet.loadURDF("%s/plane.urdf" % self._urdf_root) + #pybullet.setGravity(0, 0, -10) + acc_motor = self._accurate_motor_model_enabled + motor_protect = self._motor_overheat_protection + self.robot = Minitaur( + #pybullet_client=pybullet, + urdf_root=self._urdf_root, + time_step=self._time_step, + self_collision_enabled=self._self_collision_enabled, + motor_velocity_limit=self._motor_velocity_limit, + pd_control_enabled=self._pd_control_enabled, + accurate_motor_model_enabled=acc_motor, + motor_kp=self._motor_kp, + motor_kd=self._motor_kd, + torque_control_enabled=self._torque_control_enabled, + motor_overheat_protection=motor_protect, + on_rack=self._on_rack, + kd_for_pd_controllers=self._kd_for_pd_controllers) + else: + self.robot.Reset(reload_urdf=False) + + if self._env_randomizer is not None: + self._env_randomizer.randomize_env(self) + + self._env_step_counter = 0 + self._last_base_position = [0, 0, 0] + self._objectives = [] + #pybullet.resetDebugVisualizerCamera( + # self._cam_dist, self._cam_yaw, self._cam_pitch, [0, 0, 0]) + + #if not self._torque_control_enabled: + # for _ in range(100): + # if self._pd_control_enabled or self._accurate_motor_model_enabled: + # self.robot.ApplyAction([math.pi / 2] * 8) + #pybullet.stepSimulation() + #return self._noisy_observation() + + def _seed(self, seed=None): + self.np_random, seed = seeding.np_random(seed) + return [seed] + + def _transform_action_to_motor_command(self, action): + if self._leg_model_enabled: + for i, action_component in enumerate(action): + if not (-self._action_bound - ACTION_EPS <= action_component <= + self._action_bound + ACTION_EPS): + raise ValueError( + "{}th action {} out of bounds.".format(i, action_component)) + action = self.robot.ConvertFromLegModel(action) + return action + + def _step(self, action): + """Step forward the simulation, given the action. + + Args: + action: A list of desired motor angles for eight motors. + + Returns: + observations: The angles, velocities and torques of all motors. + 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. + """ + """ + 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._action_repeat * self._time_step - time_spent + if time_to_sleep > 0: + time.sleep(time_to_sleep) + base_pos = self.robot.GetBasePosition() + pybullet.resetDebugVisualizerCamera( + self._cam_dist, self._cam_yaw, self._cam_pitch, base_pos) + """ + action = self._transform_action_to_motor_command(action) + #for _ in range(self._action_repeat): + # self.robot.ApplyAction(action) + # pybullet.stepSimulation() + + self._env_step_counter += 1 + reward = self._reward() + done = self._termination() + return np.array(self._noisy_observation()), reward, done, {} + + def _render(self, mode="rgb_array", close=False): + if mode != "rgb_array": + return np.array([]) + base_pos = self.robot.GetBasePosition() + view_matrix = pybullet.computeViewMatrixFromYawPitchRoll( + cameraTargetPosition=base_pos, + distance=self._cam_dist, + yaw=self._cam_yaw, + pitch=self._cam_pitch, + roll=0, + upAxisIndex=2) + proj_matrix = pybullet.computeProjectionMatrixFOV( + fov=60, aspect=float(RENDER_WIDTH)/RENDER_HEIGHT, + nearVal=0.1, farVal=100.0) + (_, _, px, _, _) = pybullet.getCameraImage( + width=RENDER_WIDTH, height=RENDER_HEIGHT, viewMatrix=view_matrix, + projectionMatrix=proj_matrix, renderer=pybullet.ER_BULLET_HARDWARE_OPENGL) + rgb_array = np.array(px) + rgb_array = rgb_array[:, :, :3] + return rgb_array + + def get_minitaur_motor_angles(self): + """Get the minitaur's motor angles. + + Returns: + A numpy array of motor angles. + """ + return np.array( + self._observation[MOTOR_ANGLE_OBSERVATION_INDEX: + MOTOR_ANGLE_OBSERVATION_INDEX + NUM_MOTORS]) + + def get_minitaur_motor_velocities(self): + """Get the minitaur's motor velocities. + + Returns: + A numpy array of motor velocities. + """ + return np.array( + self._observation[MOTOR_VELOCITY_OBSERVATION_INDEX: + MOTOR_VELOCITY_OBSERVATION_INDEX + NUM_MOTORS]) + + def get_minitaur_motor_torques(self): + """Get the minitaur's motor torques. + + Returns: + A numpy array of motor torques. + """ + return np.array( + self._observation[MOTOR_TORQUE_OBSERVATION_INDEX: + MOTOR_TORQUE_OBSERVATION_INDEX + NUM_MOTORS]) + + def get_minitaur_base_orientation(self): + """Get the minitaur's base orientation, represented by a quaternion. + + Returns: + A numpy array of minitaur's orientation. + """ + return np.array(self._observation[BASE_ORIENTATION_OBSERVATION_INDEX:]) + + def is_fallen(self): + """Decide whether the minitaur has fallen. + + If the up directions between the base and the world is larger (the dot + product is smaller than 0.85) or the base is very low on the ground + (the height is smaller than 0.13 meter), the minitaur is considered fallen. + + Returns: + Boolean value that indicates whether the minitaur has fallen. + """ + orientation = self.robot.GetBaseOrientation() + rot_mat = pybullet.getMatrixFromQuaternion(orientation) + local_up = rot_mat[6:] + pos = self.robot.GetBasePosition() + return (np.dot(np.asarray([0, 0, 1]), np.asarray(local_up)) < 0.85 or + pos[2] < 0.13) + + def _termination(self): + position = self.robot.GetBasePosition() + distance = math.sqrt(position[0]**2 + position[1]**2) + return #self.is_fallen() or# distance > self._distance_limit + + def _reward(self): + current_base_position = self.robot.GetBasePosition() + forward_reward = current_base_position[0] - self._last_base_position[0] + drift_reward = -abs(current_base_position[1] - self._last_base_position[1]) + shake_reward = -abs(current_base_position[2] - self._last_base_position[2]) + self._last_base_position = current_base_position + energy_reward = np.abs( + np.dot(self.robot.GetMotorTorques(), + self.robot.GetMotorVelocities())) * self._time_step + reward = ( + self._distance_weight * forward_reward - + self._energy_weight * energy_reward + self._drift_weight * drift_reward + + self._shake_weight * shake_reward) + self._objectives.append( + [forward_reward, energy_reward, drift_reward, shake_reward]) + return reward + + def get_objectives(self): + return self._objectives + + def _get_observation(self): + self._observation = self.robot.GetObservation() + return self._observation + + def _noisy_observation(self): + self._get_observation() + observation = np.array(self._observation) + if self._observation_noise_stdev > 0: + observation += (np.random.normal( + scale=self._observation_noise_stdev, size=observation.shape) * + self.robot.GetObservationUpperBound()) + return observation + + + + +class QuadrupedCameraEnv(QuadrupedEnv, CameraRobotEnv): + def __init__(self, human=True, timestep=HUMANOID_TIMESTEP, + frame_skip=HUMANOID_FRAMESKIP, enable_sensors=False, + is_discrete=False): + self.human = human + self.timestep = timestep + self.frame_skip = frame_skip + self.enable_sensors = enable_sensors + QuadrupedEnv.__init__(self, is_discrete) + CameraRobotEnv.__init__(self) + + #self.tracking_camera['pitch'] = -45 ## stairs + #yaw = 0 ## demo: living room + #yaw = 30 ## demo: kitchen + + #self.tracking_camera['yaw'] = 90 ## demo: stairs + + self.tracking_camera['yaw'] = 90 ## living roon + self.tracking_camera['pitch'] = -10 + + self.tracking_camera['distance'] = 1.2 + self.tracking_camera['z_offset'] = 0.5 + def _reset(self): + CameraRobotEnv._reset(self) + QuadrupedEnv._reset(self) + +class QuadrupedSensorEnv(QuadrupedEnv, SensorRobotEnv): + def __init__(self, human=True, timestep=HUMANOID_TIMESTEP, + frame_skip=HUMANOID_FRAMESKIP, enable_sensors=False, + is_discrete=False): + self.human = human + self.timestep = timestep + self.frame_skip = frame_skip + QuadrupedEnv.__init__(self, is_discrete) + SensorRobotEnv.__init__(self) + + def _reset(self): + SensorRobotEnv._reset(self) + QuadrupedEnv._reset(self) diff --git a/realenv/utils/__init__.py b/realenv/utils/__init__.py new file mode 100644 index 000000000..01faa4d24 --- /dev/null +++ b/realenv/utils/__init__.py @@ -0,0 +1,2 @@ +#from realenv.client.vnc_client import VNCClient +#from realenv.client.client_actions import client_actions, client_newloc \ No newline at end of file diff --git a/realenv/client/client_actions.py b/realenv/utils/client_actions.py similarity index 100% rename from realenv/client/client_actions.py rename to realenv/utils/client_actions.py diff --git a/realenv/client/constants.py b/realenv/utils/constants.py similarity index 100% rename from realenv/client/constants.py rename to realenv/utils/constants.py diff --git a/realenv/utils/play.py b/realenv/utils/play.py new file mode 100644 index 000000000..17bb1155c --- /dev/null +++ b/realenv/utils/play.py @@ -0,0 +1,209 @@ +import gym +#import pygame +import sys +import time +import matplotlib +import time +import pybullet as p + +''' +try: + matplotlib.use('GTK3Agg') + import matplotlib.pyplot as plt +except Exception: + pass +''' + +#import pyglet.window as pw + +from collections import deque +#from pygame.locals import HWSURFACE, DOUBLEBUF, RESIZABLE, VIDEORESIZE +from threading import Thread + +def display_arr(screen, arr, video_size, transpose): + arr_min, arr_max = arr.min(), arr.max() + arr = 255.0 * (arr - arr_min) / (arr_max - arr_min) + pyg_img = pygame.surfarray.make_surface(arr.swapaxes(0, 1) if transpose else arr) + pyg_img = pygame.transform.scale(pyg_img, video_size) + screen.blit(pyg_img, (0,0)) + +def play(env, transpose=True, fps=30, zoom=None, callback=None, keys_to_action=None): + """Allows one to play the game using keyboard. + + To simply play the game use: + + play(gym.make("Pong-v3")) + play(env) + + Above code works also if env is wrapped, so it's particularly useful in + verifying that the frame-level preprocessing does not render the game + unplayable. + + If you wish to plot real time statistics as you play, you can use + gym.utils.play.PlayPlot. Here's a sample code for plotting the reward + for last 5 second of gameplay. + + def callback(obs_t, obs_tp1, rew, done, info): + return [rew,] + env_plotter = EnvPlotter(callback, 30 * 5, ["reward"]) + + env = gym.make("Pong-v3") + play(env, callback=env_plotter.callback) + + + Arguments + --------- + env: gym.Env + Environment to use for playing. + transpose: bool + If True the output of observation is transposed. + Defaults to true. + fps: int + Maximum number of steps of the environment to execute every second. + Defaults to 30. + zoom: float + Make screen edge this many times bigger + callback: lambda or None + Callback if a callback is provided it will be executed after + every step. It takes the following input: + obs_t: observation before performing action + obs_tp1: observation after performing action + action: action that was executed + rew: reward that was received + done: whether the environemnt is done or not + info: debug info + keys_to_action: dict: tuple(int) -> int or None + Mapping from keys pressed to action performed. + For example if pressed 'w' and space at the same time is supposed + to trigger action number 2 then key_to_action dict would look like this: + + { + # ... + sorted(ord('w'), ord(' ')) -> 2 + # ... + } + If None, default key_to_action mapping for that env is used, if provided. + """ + + obs_s = env.observation_space + #assert type(obs_s) == gym.spaces.box.Box + #assert len(obs_s.shape) == 2 or (len(obs_s.shape) == 3 and obs_s.shape[2] in [1,3]) + + if keys_to_action is None: + if hasattr(env, 'get_keys_to_action'): + keys_to_action = env.get_keys_to_action() + elif hasattr(env.unwrapped, 'get_keys_to_action'): + keys_to_action = env.unwrapped.get_keys_to_action() + #else: + # assert False, env.spec.id + " does not have explicit key to action mapping, " + \ + # "please specify one manually" + relevant_keys = set(sum(map(list, keys_to_action.keys()),[])) + relevant_keys.add(ord('r')) + ''' + if transpose: + video_size = env.observation_space.shape[1], env.observation_space.shape[0] + else: + video_size = env.observation_space.shape[0], env.observation_space.shape[1] + + if zoom is not None: + video_size = int(video_size[0] * zoom), int(video_size[1] * zoom) + ''' + pressed_keys = [] + running = True + env_done = True + + print("sorted pressed keys", tuple(sorted(pressed_keys))) + print("keys to actions", keys_to_action) + + obs = env.reset() + + do_restart = False + while running: + if do_restart: + do_restart = False + env.reset() + continue + if len(pressed_keys) == 0: + action = keys_to_action[()] + obs, rew, env_done, info = env.step(action) + time.sleep(1.0/fps) + for p_key in pressed_keys: + action = keys_to_action[(p_key, )] + prev_obs = obs + obs, rew, env_done, info = env.step(action) + time.sleep(1.0/fps) + if callback is not None: + callback(prev_obs, obs, action, rew, env_done, info) + ''' + if obs is not None: + if len(obs.shape) == 2: + obs = obs[:, :, None] + if obs.shape[2] == 1: + obs = obs.repeat(3, axis=2) + display_arr(screen, obs, transpose=transpose, video_size=video_size) + ''' + # process pygame events + events = p.getKeyboardEvents() + print(events) + key_codes = events.keys() + for key in key_codes: + if key not in relevant_keys: + continue + # test events, set key states + if events[key] == p.KEY_IS_DOWN: + if key not in pressed_keys: + pressed_keys.append(key) + #elif event.key == 27: + # running = False + if events[key] == p.KEY_WAS_RELEASED: + #if event.key in relevant_keys: + if key in pressed_keys: + pressed_keys.remove(key) + + print(ord('r') in key_codes) + if ord('r') in key_codes and events[ord('r')] == p.KEY_IS_DOWN: + do_restart = True + pressed_keys = [] + #print(pressed_keys) + ''' + elif event.type == pygame.QUIT: + running = False + elif event.type == VIDEORESIZE: + video_size = event.size + screen = pygame.display.set_mode(video_size) + print(video_size) + ''' + #time.sleep(1.0/fps) + +class PlayPlot(object): + def __init__(self, callback, horizon_timesteps, plot_names): + self.data_callback = callback + self.horizon_timesteps = horizon_timesteps + self.plot_names = plot_names + + num_plots = len(self.plot_names) + self.fig, self.ax = plt.subplots(num_plots) + if num_plots == 1: + self.ax = [self.ax] + for axis, name in zip(self.ax, plot_names): + axis.set_title(name) + self.t = 0 + self.cur_plot = [None for _ in range(num_plots)] + self.data = [deque(maxlen=horizon_timesteps) for _ in range(num_plots)] + + def callback(self, obs_t, obs_tp1, action, rew, done, info): + points = self.data_callback(obs_t, obs_tp1, action, rew, done, info) + for point, data_series in zip(points, self.data): + data_series.append(point) + self.t += 1 + + xmin, xmax = max(0, self.t - self.horizon_timesteps), self.t + + for i, plot in enumerate(self.cur_plot): + if plot is not None: + plot.remove() + self.cur_plot[i] = self.ax[i].scatter(range(xmin, xmax), list(self.data[i])) + self.ax[i].set_xlim(xmin, xmax) + plt.pause(0.000001) + + diff --git a/realenv/client/remote.yml b/realenv/utils/remote.yml similarity index 100% rename from realenv/client/remote.yml rename to realenv/utils/remote.yml diff --git a/realenv/client/test_client.py b/realenv/utils/test_client.py similarity index 100% rename from realenv/client/test_client.py rename to realenv/utils/test_client.py diff --git a/realenv/client/vnc_client.py b/realenv/utils/vnc_client.py similarity index 100% rename from realenv/client/vnc_client.py rename to realenv/utils/vnc_client.py