merge from beta_release

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

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

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

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)

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

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

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

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

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

realenv/client/__init__.py

@@ -1,2 +0,0 @@
-from realenv.client.vnc_client import VNCClient
-from realenv.client.client_actions import client_actions, client_newloc

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
       </geometry>
     </collision>
     <inertial>
-      <mass value="33.455"/>
+      <mass value="30.455"/>
       <origin xyz="-0.08748 -0.00085 0.09947"/>
       <inertia ixx="0.6022" ixy="-0.02364" ixz="-0.1197" iyy="1.7386" iyz="-0.001544" izz="2.0296"/>
     </inertial>

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

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

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

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)

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
+        }

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

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('RGB cam')
-        cv2.namedWindow('target depth')
+        cv2.namedWindow('Depth cam')
-        if self.compare_filler:
+        if MAKE_VIDEO:
-            cv2.moveWindow('show3d', -30 , self.showsz + LINUX_OFFSET['y_delta'])
+            cv2.moveWindow('RGB cam', -1 , 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.moveWindow('Depth cam', self.showsz + LINUX_OFFSET['x_delta'] + LINUX_OFFSET['y_delta'], -1)
-        cv2.imshow('show3d', self.show_rgb)
+            cv2.namedWindow('RGB unfilled')
-        cv2.imshow('target depth', self.show_rgb)
+            cv2.moveWindow('RGB unfilled', self.showsz + LINUX_OFFSET['x_delta'] + LINUX_OFFSET['y_delta'], self.showsz + LINUX_OFFSET['y_delta'])
-        cv2.setMouseCallback('show3d',self._onmouse)
+        elif HIGH_RES_MONITOR:
-        if self.compare_filler:
+            cv2.moveWindow('RGB cam', -1 , self.showsz + LINUX_OFFSET['y_delta'])
-            cv2.namedWindow('show3d unfilled')
+            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)
+            cv2.imshow('RGB cam',rgb)
-            if self.compare_filler:
+            if HIGH_RES_MONITOR and not MAKE_VIDEO:
-                cv2.moveWindow('show3d', -1 , self.showsz + LINUX_OFFSET['y_delta'])
+                cv2.moveWindow('RGB cam', -1 , self.showsz + LINUX_OFFSET['y_delta'])
 
         def _render_rgb_unfilled(unfilled_rgb):
-            cv2.imshow('show3d unfilled', unfilled_rgb)
+            assert(MAKE_VIDEO)
-            if self.compare_filler:
+            cv2.imshow('RGB unfilled', unfilled_rgb)
-                cv2.moveWindow('target depth', self.showsz + LINUX_OFFSET['x_delta'] + LINUX_OFFSET['y_delta'], self.showsz + LINUX_OFFSET['y_delta'])
+            
-
         """
         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)

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/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], [-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]
 

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 \"Bullet Physics\" -e {gravity},{posX},{posY},{sizeX},{sizeY}'.format(**values)
-        cmd = 'wmctrl -r :ACTIVE: -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
 

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['yaw'] = 30    ## living room
-        self.tracking_camera['distance'] = 1.5
+        #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):

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

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)

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

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