From 1625352a4000cf861eb1fced1cb234af12e5f7d4 Mon Sep 17 00:00:00 2001 From: ForceCore Date: Thu, 7 Jan 2021 21:09:35 +0900 Subject: [PATCH] Fix mutable default robot object Caused interference between the training environment and the evaluation environment, if you use stable-baselines3 (where they did the training and evaluation implementation RIGHT) --- .../mujoco/envs/locomotion/humanoid_env.py | 4 ++-- .../envs/mujoco/robots/locomotors/humanoid.py | 2 +- pybulletgym/envs/mujoco/robots/robot_bases.py | 19 ++++++++++++++----- .../envs/locomotion/humanoid_env.py | 4 ++-- .../roboschool/robots/locomotors/humanoid.py | 2 +- .../envs/roboschool/robots/robot_bases.py | 19 ++++++++++++++----- 6 files changed, 34 insertions(+), 16 deletions(-) diff --git a/pybulletgym/envs/mujoco/envs/locomotion/humanoid_env.py b/pybulletgym/envs/mujoco/envs/locomotion/humanoid_env.py index bfd0092..30bd133 100644 --- a/pybulletgym/envs/mujoco/envs/locomotion/humanoid_env.py +++ b/pybulletgym/envs/mujoco/envs/locomotion/humanoid_env.py @@ -3,8 +3,8 @@ class HumanoidMuJoCoEnv(WalkerBaseMuJoCoEnv): - def __init__(self, robot=Humanoid()): - self.robot = robot + def __init__(self, robot=None): + self.robot = robot if robot is not None else Humanoid() WalkerBaseMuJoCoEnv.__init__(self, self.robot) self.electricity_cost = 4.25 * WalkerBaseMuJoCoEnv.electricity_cost self.stall_torque_cost = 4.25 * WalkerBaseMuJoCoEnv.stall_torque_cost diff --git a/pybulletgym/envs/mujoco/robots/locomotors/humanoid.py b/pybulletgym/envs/mujoco/robots/locomotors/humanoid.py index 6f563f6..90eb965 100644 --- a/pybulletgym/envs/mujoco/robots/locomotors/humanoid.py +++ b/pybulletgym/envs/mujoco/robots/locomotors/humanoid.py @@ -7,7 +7,7 @@ class Humanoid(WalkerBase, MJCFBasedRobot): self_collision = True foot_list = ["right_foot", "left_foot"] # "left_hand", "right_hand" - def __init__(self, random_yaw = False, random_lean=False): + def __init__(self, random_yaw=False, random_lean=False): WalkerBase.__init__(self, power=0.41) MJCFBasedRobot.__init__(self, 'humanoid_symmetric.xml', 'torso', action_dim=17, obs_dim=376) # 17 joints, 4 of them important for walking (hip, knee), others may as well be turned off, 17/4 = 4.25 diff --git a/pybulletgym/envs/mujoco/robots/robot_bases.py b/pybulletgym/envs/mujoco/robots/robot_bases.py index 278b578..ed89fbb 100644 --- a/pybulletgym/envs/mujoco/robots/robot_bases.py +++ b/pybulletgym/envs/mujoco/robots/robot_bases.py @@ -140,12 +140,12 @@ class URDFBasedRobot(XmlBasedRobot): Base class for URDF .xml based robots. """ - def __init__(self, model_urdf, robot_name, action_dim, obs_dim, basePosition=[0, 0, 0], baseOrientation=[0, 0, 0, 1], fixed_base=False, self_collision=False): + def __init__(self, model_urdf, robot_name, action_dim, obs_dim, basePosition=None, baseOrientation=None, fixed_base=False, self_collision=False): XmlBasedRobot.__init__(self, robot_name, action_dim, obs_dim, self_collision) self.model_urdf = model_urdf - self.basePosition = basePosition - self.baseOrientation = baseOrientation + self.basePosition = basePosition if basePosition is not None else [0, 0, 0] + self.baseOrientation = baseOrientation if baseOrientation is not None else [0, 0, 0, 1] self.fixed_base = fixed_base def reset(self, bullet_client): @@ -185,9 +185,14 @@ class SDFBasedRobot(XmlBasedRobot): Base class for SDF robots in a Scene. """ - def __init__(self, model_sdf, robot_name, action_dim, obs_dim, basePosition=[0, 0, 0], baseOrientation=[0, 0, 0, 1], fixed_base=False, self_collision=False): + def __init__(self, model_sdf, robot_name, action_dim, obs_dim, basePosition=None, baseOrientation=None, fixed_base=False, self_collision=False): XmlBasedRobot.__init__(self, robot_name, action_dim, obs_dim, self_collision) + if basePosition is None: + basePosition = [0, 0, 0] + if baseOrientation is None: + baseOrientation = [0, 0, 0, 1] + self.model_sdf = model_sdf self.fixed_base = fixed_base @@ -275,7 +280,11 @@ def reset_position(self, position): def reset_orientation(self, orientation): self._p.resetBasePositionAndOrientation(self.bodies[self.bodyIndex], self.get_position(), orientation) - def reset_velocity(self, linearVelocity=[0,0,0], angularVelocity =[0,0,0]): + def reset_velocity(self, linearVelocity=None, angularVelocity=None): + if linearVelocity is None: + linearVelocity = [0, 0, 0] + if angularVelocity is None: + angularVelocity = [0, 0, 0] self._p.resetBaseVelocity(self.bodies[self.bodyIndex], linearVelocity, angularVelocity) def reset_pose(self, position, orientation): diff --git a/pybulletgym/envs/roboschool/envs/locomotion/humanoid_env.py b/pybulletgym/envs/roboschool/envs/locomotion/humanoid_env.py index c37c957..c45332c 100644 --- a/pybulletgym/envs/roboschool/envs/locomotion/humanoid_env.py +++ b/pybulletgym/envs/roboschool/envs/locomotion/humanoid_env.py @@ -3,8 +3,8 @@ class HumanoidBulletEnv(WalkerBaseBulletEnv): - def __init__(self, robot=Humanoid()): - self.robot = robot + def __init__(self, robot=None): + self.robot = robot if robot is not None else Humanoid() WalkerBaseBulletEnv.__init__(self, self.robot) self.electricity_cost = 4.25 * WalkerBaseBulletEnv.electricity_cost self.stall_torque_cost = 4.25 * WalkerBaseBulletEnv.stall_torque_cost diff --git a/pybulletgym/envs/roboschool/robots/locomotors/humanoid.py b/pybulletgym/envs/roboschool/robots/locomotors/humanoid.py index 0341eb6..bf50ba3 100644 --- a/pybulletgym/envs/roboschool/robots/locomotors/humanoid.py +++ b/pybulletgym/envs/roboschool/robots/locomotors/humanoid.py @@ -7,7 +7,7 @@ class Humanoid(WalkerBase, MJCFBasedRobot): self_collision = True foot_list = ["right_foot", "left_foot"] # "left_hand", "right_hand" - def __init__(self, random_yaw = False, random_lean=False): + def __init__(self, random_yaw=False, random_lean=False): WalkerBase.__init__(self, power=0.41) MJCFBasedRobot.__init__(self, 'humanoid_symmetric.xml', 'torso', action_dim=17, obs_dim=44) # 17 joints, 4 of them important for walking (hip, knee), others may as well be turned off, 17/4 = 4.25 diff --git a/pybulletgym/envs/roboschool/robots/robot_bases.py b/pybulletgym/envs/roboschool/robots/robot_bases.py index 8ca94c0..1d2f38d 100644 --- a/pybulletgym/envs/roboschool/robots/robot_bases.py +++ b/pybulletgym/envs/roboschool/robots/robot_bases.py @@ -137,12 +137,12 @@ class URDFBasedRobot(XmlBasedRobot): Base class for URDF .xml based robots. """ - def __init__(self, model_urdf, robot_name, action_dim, obs_dim, basePosition=[0, 0, 0], baseOrientation=[0, 0, 0, 1], fixed_base=False, self_collision=False): + def __init__(self, model_urdf, robot_name, action_dim, obs_dim, basePosition=None, baseOrientation=None, fixed_base=False, self_collision=False): XmlBasedRobot.__init__(self, robot_name, action_dim, obs_dim, self_collision) self.model_urdf = model_urdf - self.basePosition = basePosition - self.baseOrientation = baseOrientation + self.basePosition = basePosition if basePosition is not None else [0, 0, 0] + self.baseOrientation = baseOrientation if baseOrientation is not None else [0, 0, 0, 1] self.fixed_base = fixed_base def reset(self, bullet_client): @@ -183,9 +183,14 @@ class SDFBasedRobot(XmlBasedRobot): Base class for SDF robots in a Scene. """ - def __init__(self, model_sdf, robot_name, action_dim, obs_dim, basePosition=[0, 0, 0], baseOrientation=[0, 0, 0, 1], fixed_base=False, self_collision=False): + def __init__(self, model_sdf, robot_name, action_dim, obs_dim, basePosition=None, baseOrientation=None, fixed_base=False, self_collision=False): XmlBasedRobot.__init__(self, robot_name, action_dim, obs_dim, self_collision) + if basePosition is None: + basePosition = [0, 0, 0] + if baseOrientation is None: + baseOrientation = [0, 0, 0, 1] + self.model_sdf = model_sdf self.fixed_base = fixed_base @@ -271,7 +276,11 @@ def reset_position(self, position): def reset_orientation(self, orientation): self._p.resetBasePositionAndOrientation(self.bodies[self.bodyIndex], self.get_position(), orientation) - def reset_velocity(self, linearVelocity=[0,0,0], angularVelocity =[0,0,0]): + def reset_velocity(self, linearVelocity=None, angularVelocity=None): + if linearVelocity is None: + linearVelocity = [0, 0, 0] + if angularVelocity is None: + angularVelocity = [0, 0, 0] self._p.resetBaseVelocity(self.bodies[self.bodyIndex], linearVelocity, angularVelocity) def reset_pose(self, position, orientation):