def test_aligned_idx(self):
for i in np.arange(start=1,stop=7):
wrapper = getattr(wrapper_diff, f"Walker2DHopperCase{i}")
env = make_env(robot_body=600, wrappers=[wrapper])()
assert env.observation_space.shape[0]==22, "Should extend Hopper into Walker2D's Observation space"
fake_obs = np.arange(start=0,stop=15)
assert env.observation(fake_obs).tolist()==self.cases[i]
def _get_zipped(method, body_id):
common.args.realign_method = method
env = make_env(robot_body=body_id, wrappers=[ReAlignedWrapper])()
ids = np.arange(len(env.realign_idx))
zipped = list(zip(ids, env.realign_idx))
sorted_zipped = list(zip(ids, sorted(env.realign_idx)))
return zipped, sorted_zipped, env
def load_parameters_from_path(model, model_filename, model_cls, bodies,
default_wrapper):
args = common.args
data, params, pytorch_variables = load_from_zip_file(model_filename)
robot_ids_in_file = []
if args.cnspns:
for parameter_name, module in params['policy'].items():
_match = re.findall(r'pns_sensor_adaptor\.nets\.([0-9]+)\.weight',
parameter_name)
if _match:
_robot_id = _match[0]
print(f"Sensor channel for the policy: {module.shape[0]}")
robot_ids_in_file.append(int(_robot_id))
assert args.cnspns_sensor_channel == module.shape[
0], f"Loading from a model with a different number of sensor channels. Want {args.cnspns_sensor_channel}, the model has {module.shape[0]}."
_match = re.findall(r'pns_motor_adaptor\.nets\.([0-9])+\.weight',
parameter_name)
if _match:
print(f"Motor channel for the policy: {module.shape[1]}")
assert args.cnspns_motor_channel == module.shape[
1], f"Loading from a model with a different number of motor channels. Want {args.cnspns_motor_channel}, the model has {module.shape[1]}."
fake_env = DummyVecEnv([
gym_interface.make_env(robot_body=_robot_id,
wrappers=default_wrapper,
render=False,
dataset_folder=args.body_folder)
for _robot_id in robot_ids_in_file
])
else:
fake_env = None
load_model = model_cls.load(model_filename, fake_env)
if args.cnspns:
for robot_id in robot_ids_in_file:
if robot_id not in bodies:
model.policy.add_net_to_adaptors(robot_id)
for robot_id in bodies:
if robot_id not in robot_ids_in_file:
load_model.policy.add_net_to_adaptors(robot_id)
load_weights = load_model.policy.state_dict()
model.policy.load_state_dict(load_weights)
# model.policy.rebuild()?
print(f"Weights loaded from {model_filename}")
return model
import numpy as np
from common import gym_interface,seeds,utils
default_order = {
"arm": 0,
"arm_left": 1,
"thigh": 2,
"leg": 3,
"foot": 4,
"thigh_left": 5,
"leg_left": 6,
"foot_left": 7,
}
alignments = []
for body_id in np.arange(start=900, stop=908):
env = gym_interface.make_env(robot_body=body_id, render=False)()
env.reset()
custom_order = []
for part_name, part in env.robot.parts.items():
if part_name.startswith("link") or part_name == "torso" or part_name == "floor":
continue
custom_order.append(default_order[part_name])
# Don't argsort! We define order in the order way. (argsort will be perform inside custom alignment class.)
# alignment = np.argsort(custom_order)
alignments.append(custom_order)
def ga_mutate(alignments, n=2, seed=0):
# Mutate individual
# Swap Mutation: https://www.tutorialspoint.com/genetic_algorithms/genetic_algorithms_mutation.htm
# small n means less mutation and less randomness
offspring = copy.deepcopy(alignments)
from common import gym_interface
import pybullet as p
import os
import pybullet_data
import gym
import pybullet_envs
import shutil
import re
import numpy as np
import random
from common import common
common.args.train_bodies = [399]
env = gym_interface.make_env(robot_body=common.args.train_bodies[0],
render=True,
dataset_folder="../input_data/bodies")()
obs = env.reset()
env.env._p.setGravity(0, 0, -0)
env.env._p.removeBody(0)
a = env.action_space.sample()
for i, j in enumerate(env.robot.ordered_joints):
print(i, j.jointIndex, j.joint_name)
a = np.zeros_like(a)
history_actions = []
while True:
# a[2] = (random.random()-0.5)*10
# a[6] = (random.random()-0.5)
from stable_baselines3 import PPO, SAC
from stable_baselines3.common.vec_env import DummyVecEnv, VecNormalize
from common import common
import common.gym_interface as gym_interface
if __name__ == "__main__":
hyperparams = common.load_hyperparameters(conf_name="SAC")
venv = DummyVecEnv([gym_interface.make_env(robot_body=300)])
keys_remove = ["normalize", "n_envs", "n_timesteps", "policy"]
for key in keys_remove:
if key in hyperparams:
del hyperparams[key]
model = SAC('MlpPolicy', venv, verbose=1, seed=common.seed, **hyperparams)
model.save("output_data/tmp/tmp")
model = SAC.load("output_data/tmp/tmp.zip")
model = SAC.load("output_data/models/best_model.zip")
default_wrapper.append(wrapper.ReAlignedWrapper)
elif args.topology_wrapper == "diff":
default_wrapper.append(wrapper_diff.get_wrapper_class())
elif args.topology_wrapper == "MutantWrapper":
default_wrapper.append(wrapper_mut.MutantWrapper)
elif args.topology_wrapper == "CustomAlignWrapper":
default_wrapper.append(wrapper_custom_align.CustomAlignWrapper)
else:
pass # no need for wrapper
for rank_idx, test_body in enumerate(args.test_bodies):
eval_venv = DummyVecEnv([
gym_interface.make_env(rank=rank_idx,
seed=common.seed,
wrappers=default_wrapper,
render=args.render,
force_render=args.render,
robot_body=test_body,
dataset_folder=args.body_folder)
])
if args.vec_normalize:
raise NotImplementedError
# normalize_kwargs["gamma"] = hyperparams["gamma"]
# eval_venv = VecNormalize(eval_venv, **normalize_kwargs)
if args.stack_frames > 1:
eval_venv = VecFrameStack(eval_venv, args.stack_frames)
eval_venv.seed(common.seed)
if args.pns:
model_cls = PNSPPO
import time
import glob
from common import gym_interface
import pybullet as p
import os
import pybullet_data
import gym
import pybullet_envs
import shutil
import re
import numpy as np
import random
env = gym_interface.make_env(robot_body=900, render=True)()
obs = env.reset()
env.env._p.setGravity(0,0,-1)
a = env.action_space.sample()
for i, j in enumerate(env.robot.ordered_joints):
print(i, j.jointIndex, j.joint_name)
a = np.zeros_like(a)
while True:
# a[2] = (random.random()-0.5)*10
a[6] = (random.random()-0.5)
env.step(a)
time.sleep(0.01)
default_wrapper.append(wrapper_mut.MutantWrapper)
elif args.topology_wrapper == "CustomAlignWrapper":
default_wrapper.append(wrapper_custom_align.CustomAlignWrapper)
else:
pass # no need for wrapper
assert len(args.train_bodies) > 0, "No body to train."
if args.with_bodyinfo:
default_wrapper.append(wrapper.BodyinfoWrapper)
print("Making train environments...")
venv = DummyVecEnv([
gym_interface.make_env(
rank=i,
seed=common.seed,
wrappers=default_wrapper,
render=args.render,
robot_body=args.train_bodies[i % len(args.train_bodies)],
dataset_folder=args.body_folder) for i in range(args.num_venvs)
])
normalize_kwargs = {}
if args.vec_normalize:
normalize_kwargs["gamma"] = hyperparams["gamma"]
venv = VecNormalize(venv, **normalize_kwargs)
if args.stack_frames > 1:
venv = VecFrameStack(venv, args.stack_frames)
keys_remove = ["normalize", "n_envs", "n_timesteps", "policy"]
for key in keys_remove:
if body_type==0:
body_type = body//100
else:
assert body_type == body//100, "Training on different body types."
if args.realign_method!="":
default_wrapper.append(wrapper.ReAlignedWrapper)
elif args.topology_wrapper == "diff":
default_wrapper.append(wrapper_diff.get_wrapper_class())
elif args.topology_wrapper == "MutantWrapper":
default_wrapper.append(wrapper_mut.MutantWrapper)
else:
pass # no need for wrapper
for test_body in args.test_bodies:
eval_venv = DummyVecEnv([gym_interface.make_env(rank=0, seed=common.seed, wrappers=default_wrapper, render=args.render,
robot_body=test_body,
dataset_folder="output_data/bodies")])
if args.vec_normalize:
raise NotImplementedError
# normalize_kwargs["gamma"] = hyperparams["gamma"]
# eval_venv = VecNormalize(eval_venv, **normalize_kwargs)
if args.stack_frames > 1:
eval_venv = VecFrameStack(eval_venv, args.stack_frames)
eval_venv.seed(common.seed)
model = PPO.load(args.model_filename)
obs = eval_venv.reset()
print(obs)
g_obs_data = np.zeros(shape=[args.test_steps, obs.shape[1]], dtype=np.float32)
def _get_zipped(method, body_id):
common.args.realign_method = method
env = make_env(robot_body=body_id, wrappers=[ReAlignedWrapper])()
realigned_obs = env.observation(
np.arange(env.observation_space.shape[0]))
return list(zip(realigned_obs, env.realign_idx)), env
import time
import glob
from common import gym_interface
import pybullet as p
import os
import pybullet_data
import gym
import pybullet_envs
import shutil
import re
import numpy as np
import random
env = gym_interface.make_env(robot_body=207, render=True, dataset_folder="output_data/bodies")()
obs = env.reset()
env.env._p.setGravity(0,0,-1)
a = env.action_space.sample()
for i, j in enumerate(env.robot.ordered_joints):
print(i, j.jointIndex, j.joint_name)
a = np.zeros_like(a)
while True:
# a[2] = (random.random()-0.5)*10
a[6] = (random.random()-0.5)
env.step(a)
time.sleep(0.01)
_w = wrapper_pns.make_same_dim_wrapper(obs_dim=28, action_dim=8)
default_wrapper.append(_w)
assert len(args.train_bodies) > 0, "No body to train."
if args.with_bodyinfo:
default_wrapper.append(wrapper.BodyinfoWrapper)
photo_idx = 2
print("Making train environments...")
venv = DummyVecEnv([
gym_interface.make_env(
rank=i,
seed=common.seed,
wrappers=default_wrapper,
render=args.render,
robot_body=args.train_bodies[i % len(args.train_bodies)],
dataset_folder=args.body_folder,
render_index=photo_idx,
) for i in range(args.num_venvs)
])
venv.reset()
p = venv.envs[photo_idx].env.env.env._p
# for i in range(8):
# for j in range(100):
# action = np.zeros(shape=[4,8])
# action[0,i] = 1
# venv.step(action)
# time.sleep(0.01)
for i in range(28):
body_type = body//100
else:
assert body_type == body//100, "Training on different body types."
if args.realign_method!="":
default_wrapper.append(wrapper.ReAlignedWrapper)
elif args.topologies == "diff":
default_wrapper.append(getattr(wrapper_diff, args.wrapper_type))
else:
pass # no need for wrapper
assert len(args.train_bodies) > 0, "No body to train."
if args.with_bodyinfo:
default_wrapper.append(wrapper.BodyinfoWrapper)
print("Making train environments...")
venv = DummyVecEnv([gym_interface.make_env(rank=i, seed=common.seed, wrappers=default_wrapper, render=args.render,
robot_body=args.train_bodies[i % len(args.train_bodies)]) for i in range(args.num_venvs)])
normalize_kwargs = {}
if args.vec_normalize:
normalize_kwargs["gamma"] = hyperparams["gamma"]
venv = VecNormalize(venv, **normalize_kwargs)
if args.stack_frames > 1:
venv = VecFrameStack(venv, args.stack_frames)
keys_remove = ["normalize", "n_envs", "n_timesteps", "policy"]
for key in keys_remove:
if key in hyperparams:
del hyperparams[key]
print("Making eval environments...")
if args.realign_method != "":
default_wrapper.append(wrapper.ReAlignedWrapper)
elif args.topology_wrapper == "diff":
default_wrapper.append(wrapper_diff.get_wrapper_class())
elif args.topology_wrapper == "MutantWrapper":
default_wrapper.append(wrapper_mut.MutantWrapper)
elif args.topology_wrapper == "CustomAlignWrapper":
default_wrapper.append(wrapper_custom_align.CustomAlignWrapper)
else:
pass # no need for wrapper
for test_body in args.test_bodies:
eval_venv = DummyVecEnv([
gym_interface.make_env(rank=0,
seed=common.seed,
wrappers=default_wrapper,
render=args.render,
robot_body=test_body,
dataset_folder="../input_data/bodies")
])
if args.vec_normalize:
raise NotImplementedError
# normalize_kwargs["gamma"] = hyperparams["gamma"]
# eval_venv = VecNormalize(eval_venv, **normalize_kwargs)
if args.stack_frames > 1:
eval_venv = VecFrameStack(eval_venv, args.stack_frames)
eval_venv.seed(common.seed)
model = PPO.load(args.model_filename)
obs = eval_venv.reset()
default_wrapper.append(wrapper.ReAlignedWrapper)
elif args.topology_wrapper == "diff":
default_wrapper.append(wrapper_diff.get_wrapper_class())
else:
pass # no need for wrapper
assert len(args.train_bodies) > 0, "No body to train."
if args.with_bodyinfo:
default_wrapper.append(wrapper.BodyinfoWrapper)
print("Making train environments...")
venv = DummyVecEnv([
gym_interface.make_env(
rank=i,
seed=common.seed,
wrappers=default_wrapper,
render=args.render,
robot_body=args.train_bodies[i % len(args.train_bodies)],
dataset_folder="../input_data/bodies")
for i in range(args.num_venvs)
])
normalize_kwargs = {}
if args.vec_normalize:
normalize_kwargs["gamma"] = hyperparams["gamma"]
venv = VecNormalize(venv, **normalize_kwargs)
if args.stack_frames > 1:
venv = VecFrameStack(venv, args.stack_frames)
keys_remove = ["normalize", "n_envs", "n_timesteps", "policy"]
assert len(args.train_bodies) == 0, "No need for body to train."
default_wrapper = []
if args.misalign_obs:
default_wrapper.append(MisalignedWalkerWrapper)
elif args.random_align_obs:
default_wrapper.append(RandomAlignedWalkerWrapper)
else:
default_wrapper.append(wrapper.WalkerWrapper)
for test_body in args.test_bodies:
eval_venv = DummyVecEnv([
gym_interface.make_env(rank=0,
seed=common.seed,
wrappers=default_wrapper,
render=args.render,
robot_body=test_body)
])
if args.vec_normalize:
raise NotImplementedError
# normalize_kwargs["gamma"] = hyperparams["gamma"]
# eval_venv = VecNormalize(eval_venv, **normalize_kwargs)
if args.stack_frames > 1:
eval_venv = VecFrameStack(eval_venv, args.stack_frames)
eval_venv.seed(common.seed)
model = PPO.load(args.model_filename)
obs = eval_venv.reset()
default_wrapper = []
# if padding zero:
# default_wrapper.append(wrapper.WalkerWrapper)
if args.realign_method != "":
default_wrapper.append(wrapper.ReAlignedWrapper)
assert len(args.train_bodies) > 0, "No body to train."
if args.with_bodyinfo:
default_wrapper.append(wrapper.BodyinfoWrapper)
print("Making train environments...")
venv = DummyVecEnv([
gym_interface.make_env(
rank=i,
seed=common.seed,
wrappers=default_wrapper,
render=args.render,
robot_body=args.train_bodies[i % len(args.train_bodies)])
for i in range(args.num_venvs)
])
normalize_kwargs = {}
if args.vec_normalize:
normalize_kwargs["gamma"] = hyperparams["gamma"]
venv = VecNormalize(venv, **normalize_kwargs)
if args.stack_frames > 1:
venv = VecFrameStack(venv, args.stack_frames)
keys_remove = ["normalize", "n_envs", "n_timesteps", "policy"]
for key in keys_remove:
import re
import numpy as np
import random
from stable_baselines3.common.vec_env import SubprocVecEnv, DummyVecEnv
from common import wrapper_custom_align
from common import common
if __name__ == "__main__":
wrappers = [wrapper_custom_align.CustomAlignWrapper]
_envs = []
for i in range(len(common.args.train_bodies)):
_envs.append(
gym_interface.make_env(rank=i,
wrappers=wrappers,
robot_body=common.args.train_bodies[i],
render=True,
force_render=True))
env = SubprocVecEnv(_envs)
obs = env.reset()
# env.env._p.setGravity(0,0,-1)
a = env.action_space.sample()
# for i, j in enumerate(env.robot.ordered_joints):
# print(i, j.jointIndex, j.joint_name)
a = np.zeros_like(a)
history_actions = []
while True:
