def create_qqsu_setup():
# Environments
env_hparams = dict(dt=1 / 100.0, max_steps=600)
env_real = QQubeSwingUpSim(**env_hparams)
env_real.domain_param = dict(
mass_rot_pole=0.095 * 0.9, # 0.095*0.9 = 0.0855
mass_pend_pole=0.024 * 1.1, # 0.024*1.1 = 0.0264
length_rot_pole=0.085 * 0.9, # 0.085*0.9 = 0.0765
length_pend_pole=0.129 * 1.1, # 0.129*1.1 = 0.1419
)
env_sim = QQubeSwingUpSim(**env_hparams)
randomizer = DomainRandomizer(
NormalDomainParam(name="mass_rot_pole",
mean=0.0,
std=1e-9,
clip_lo=1e-3),
NormalDomainParam(name="mass_pend_pole",
mean=0.0,
std=1e-9,
clip_lo=1e-3),
NormalDomainParam(name="length_rot_pole",
mean=0.0,
std=1e-9,
clip_lo=1e-3),
NormalDomainParam(name="length_pend_pole",
mean=0.0,
std=1e-9,
clip_lo=1e-3),
)
env_sim = DomainRandWrapperLive(env_sim, randomizer)
dp_map = {
0: ("mass_rot_pole", "mean"),
1: ("mass_rot_pole", "std"),
2: ("mass_pend_pole", "mean"),
3: ("mass_pend_pole", "std"),
4: ("length_rot_pole", "mean"),
5: ("length_rot_pole", "std"),
6: ("length_pend_pole", "mean"),
7: ("length_pend_pole", "std"),
}
# trafo_mask = [False, True, False, True, False, True, False, True]
trafo_mask = [True] * 8
env_sim = MetaDomainRandWrapper(env_sim, dp_map)
# Policies (the behavioral policy needs to be deterministic)
behavior_policy = QQubeSwingUpAndBalanceCtrl(env_sim.spec)
prior = DomainRandomizer(
NormalDomainParam(name="mass_rot_pole", mean=0.095, std=0.095 / 10),
NormalDomainParam(name="mass_pend_pole", mean=0.024, std=0.024 / 10),
NormalDomainParam(name="length_rot_pole", mean=0.085, std=0.085 / 10),
NormalDomainParam(name="length_pend_pole", mean=0.129, std=0.129 / 10),
)
ddp_policy = DomainDistrParamPolicy(mapping=dp_map,
trafo_mask=trafo_mask,
prior=prior,
scale_params=False)
return env_sim, env_real, env_hparams, dp_map, behavior_policy, ddp_policy
def create_qq_setup(factor, dt, max_steps, render_mode):
# Set up environment
init_state = np.array([0.1, 0.0, 0.0, 0.0])
env = QQubeSwingUpSim(dt=dt, max_steps=max_steps)
env = ActNormWrapper(env)
# Set up policy
policy = QQubeSwingUpAndBalanceCtrl(env.spec)
# Simulate
ro = rollout(
env,
policy,
reset_kwargs=dict(domain_param=dict(dt=dt), init_state=init_state),
render_mode=render_mode,
max_steps=max_steps,
)
act_500Hz = ro.actions
ro = rollout(
env,
policy,
reset_kwargs=dict(domain_param=dict(dt=dt * factor),
init_state=init_state),
render_mode=render_mode,
max_steps=int(max_steps / factor),
)
act_100Hz = ro.actions
env = DownsamplingWrapper(env, factor)
ro = rollout(
env,
policy,
reset_kwargs=dict(domain_param=dict(dt=dt), init_state=init_state),
render_mode=render_mode,
max_steps=max_steps,
)
act_500Hz_w = ro.actions
# Time in seconds
time_500Hz = np.linspace(0, int(len(act_500Hz) * dt), int(len(act_500Hz)))
time_100Hz = np.linspace(0, int(len(act_100Hz) * dt), int(len(act_100Hz)))
time_500Hz_w = np.linspace(0, int(len(act_500Hz_w) * dt),
int(len(act_500Hz_w)))
# Plot
_, ax = plt.subplots(nrows=1)
ax.plot(time_500Hz, act_500Hz, label="500 Hz (original)")
ax.plot(time_100Hz, act_100Hz, label="100 Hz", ls="--")
ax.plot(time_500Hz_w, act_500Hz_w, label="500 Hz (wrapped)", ls="--")
ax.legend()
ax.set_ylabel(env.act_space.labels)
ax.set_xlabel("time [s]")
def test_bayrn_power(ex_dir, env: SimEnv, bayrn_hparam: dict):
pyrado.set_seed(0)
# Environments and domain randomization
env_real = deepcopy(env)
env_sim = DomainRandWrapperLive(env, create_zero_var_randomizer(env))
dp_map = create_default_domain_param_map_qq()
env_sim = MetaDomainRandWrapper(env_sim, dp_map)
env_real.domain_param = dict(mass_pend_pole=0.024 * 1.1,
mass_rot_pole=0.095 * 1.1)
env_real = wrap_like_other_env(env_real, env_sim)
# Policy and subroutine
policy_hparam = dict(energy_gain=0.587, ref_energy=0.827)
policy = QQubeSwingUpAndBalanceCtrl(env_sim.spec, **policy_hparam)
subrtn_hparam = dict(
max_iter=1,
pop_size=8,
num_init_states_per_domain=1,
num_is_samples=4,
expl_std_init=0.1,
num_workers=1,
)
subrtn = PoWER(ex_dir, env_sim, policy, **subrtn_hparam)
# Set the boundaries for the GP
dp_nom = inner_env(env_sim).get_nominal_domain_param()
ddp_space = BoxSpace(
bound_lo=np.array([
0.8 * dp_nom["mass_pend_pole"], 1e-8,
0.8 * dp_nom["mass_rot_pole"], 1e-8
]),
bound_up=np.array([
1.2 * dp_nom["mass_pend_pole"], 1e-7,
1.2 * dp_nom["mass_rot_pole"], 1e-7
]),
)
# Create algorithm and train
algo = BayRn(ex_dir,
env_sim,
env_real,
subrtn,
ddp_space,
**bayrn_hparam,
num_workers=1)
algo.train()
assert algo.curr_iter == algo.max_iter or algo.stopping_criterion_met()
def create_qq_setup(factor, dt, max_steps):
# Set up environment
init_state = np.array([0.1, 0.0, 0.0, 0.0])
env = QQubeSwingUpSim(dt=dt, max_steps=max_steps)
env = ActNormWrapper(env)
# Set up policy
policy = QQubeSwingUpAndBalanceCtrl(env.spec)
# Simulate
ro = rollout(
env,
policy,
reset_kwargs=dict(domain_param=dict(dt=dt), init_state=init_state),
render_mode=RenderMode(video=True),
max_steps=max_steps,
)
act_500Hz = ro.actions
ro = rollout(
env,
policy,
reset_kwargs=dict(domain_param=dict(dt=dt * factor),
init_state=init_state),
render_mode=RenderMode(video=True),
max_steps=int(max_steps / factor),
)
act_100Hz = ro.actions
act_100Hz_zoh = np.repeat(act_100Hz, 5, axis=0)
env = DownsamplingWrapper(env, factor)
ro = rollout(
env,
policy,
reset_kwargs=dict(domain_param=dict(dt=dt), init_state=init_state),
render_mode=RenderMode(video=True),
max_steps=max_steps,
)
act_500Hz_wrapped = ro.actions
# Plot
_, ax = plt.subplots(nrows=1)
ax.plot(act_500Hz, label="500 Hz (original)")
ax.plot(act_100Hz_zoh, label="100 Hz (zoh)")
ax.plot(act_500Hz_wrapped, label="500 Hz (wrapped)")
ax.legend()
ax.set_ylabel(env.act_space.labels)
ax.set_xlabel("time steps")
plt.show()
def test_env_specific(env: Env):
pyrado.set_seed(0)
if "qbb" in env.name:
policy = QBallBalancerPDCtrl(env.spec)
policy.reset()
elif "qcp" in env.name:
policy = QCartPoleSwingUpAndBalanceCtrl(env.spec)
policy.reset()
elif "qq" in env.name:
policy = QQubeSwingUpAndBalanceCtrl(env.spec)
policy.reset()
else:
raise NotImplementedError
# Sample an observation and do an action 10 times
for _ in range(10):
obs = env.obs_space.sample_uniform()
obs = to.from_numpy(obs).to(dtype=to.get_default_dtype())
act = policy(obs)
assert isinstance(act, to.Tensor)
from pyrado.environments.pysim.quanser_qube import QQubeSwingUpSim
from pyrado.policies.special.environment_specific import QQubeSwingUpAndBalanceCtrl
from pyrado.sampling.rollout import rollout
from pyrado.utils.data_types import RenderMode
if __name__ == '__main__':
plt.rc('text', usetex=True)
# Set up environment
env = QQubeSwingUpSim(dt=1 / 500., max_steps=3500)
env = GaussianObsNoiseWrapper(
env, noise_std=[0., 0., 0., 0., 2.,
0]) # only noise on theta_dot [rad/s]
# Set up policy
policy = QQubeSwingUpAndBalanceCtrl(env.spec)
# Simulate
ro = rollout(env,
policy,
render_mode=RenderMode(text=False, video=False),
eval=True)
# Filter the observations of the last rollout
theta_dot = ro.observations[:, 4]
alpha_dot = ro.observations[:, 5]
theta_dot_filt_3 = gaussian_filter1d(theta_dot, 3)
theta_dot_filt_5 = gaussian_filter1d(theta_dot, 5)
alpha_dot_filt_3 = gaussian_filter1d(alpha_dot, 3)
alpha_dot_filt_5 = gaussian_filter1d(alpha_dot, 5)
length_pend_pole=dp_nom["length_pend_pole"] * 0.8,
length_rot_pole=dp_nom["length_rot_pole"] * 0.9,
gravity_const=dp_nom["gravity_const"] * 1.0,
)
# randomizer = DomainRandomizer(
# NormalDomainParam(name="damping_rot_pole", mean=dp_nom["damping_rot_pole"] * 2.0, std=dp_nom["motor_back_emf"] / 10, clip_lo=0.0),
# NormalDomainParam(name="damping_pend_pole", mean=dp_nom["damping_pend_pole"] * 2.0, std=dp_nom["motor_back_emf"] / 10, clip_lo=0.0),
# NormalDomainParam(name="motor_resistance", mean=dp_nom["motor_resistance"] * 1.1, std=dp_nom["motor_back_emf"] / 50, clip_lo=0.0),
# NormalDomainParam(name="Km", mean=dp_nom["motor_back_emf"] * 0.9, std=dp_nom["motor_back_emf"] / 50, clip_lo=0.0),
# )
# env_real = DomainRandWrapperBuffer(env_real, randomizer)
# env_real.fill_buffer(num_real_rollouts)
# Behavioral policy
policy_hparam = dict(energy_gain=0.587, ref_energy=0.827)
policy = QQubeSwingUpAndBalanceCtrl(env_sim.spec, **policy_hparam)
# Define a mapping: index - domain parameter
# dp_mapping = {0: "act_delay"}
# dp_mapping = {0: "mass_rot_pole", 1: "mass_pend_pole", 2: "length_rot_pole", 3: "length_pend_pole"}
dp_mapping = {
0: "damping_rot_pole",
1: "damping_pend_pole",
2: "motor_resistance",
3: "motor_back_emf",
4: "mass_rot_pole",
5: "mass_pend_pole",
6: "length_rot_pole",
7: "length_pend_pole",
8: "gravity_const",
}
def test_npdr_and_bayessim(
ex_dir,
algo_name: str,
env: SimEnv,
num_segments: int,
len_segments: int,
num_real_rollouts: int,
num_sbi_rounds: int,
use_rec_act: bool,
):
pyrado.set_seed(0)
# Create a fake ground truth target domain
env_real = deepcopy(env)
dp_nom = env.get_nominal_domain_param()
env_real.domain_param = dict(mass_pend_pole=dp_nom["mass_pend_pole"] * 1.2,
length_pend_pole=dp_nom["length_pend_pole"] *
0.8)
# Reduce the number of steps to make this test run faster
env.max_steps = 40
env_real.max_steps = 40
# Policy
policy = QQubeSwingUpAndBalanceCtrl(env.spec)
# Define a mapping: index - domain parameter
dp_mapping = {1: "mass_pend_pole", 2: "length_pend_pole"}
# Prior
prior_hparam = dict(
low=to.tensor(
[dp_nom["mass_pend_pole"] * 0.5,
dp_nom["length_pend_pole"] * 0.5]),
high=to.tensor(
[dp_nom["mass_pend_pole"] * 1.5,
dp_nom["length_pend_pole"] * 1.5]),
)
prior = sbiutils.BoxUniform(**prior_hparam)
# Time series embedding
embedding = BayesSimEmbedding(
env.spec,
RolloutSamplerForSBI.get_dim_data(env.spec),
downsampling_factor=3,
)
# Posterior (normalizing flow)
posterior_hparam = dict(model="maf",
embedding_net=nn.Identity(),
hidden_features=20,
num_transforms=3)
# Policy optimization subroutine
subrtn_policy_hparam = dict(
max_iter=1,
pop_size=2,
num_init_states_per_domain=1,
num_domains=2,
expl_std_init=0.1,
expl_factor=1.1,
num_workers=1,
)
subrtn_policy = HCNormal(ex_dir, env, policy, **subrtn_policy_hparam)
# Algorithm
algo_hparam = dict(
max_iter=1,
num_sim_per_round=20,
num_real_rollouts=num_real_rollouts,
num_sbi_rounds=num_sbi_rounds,
simulation_batch_size=1,
normalize_posterior=False,
num_eval_samples=2,
num_segments=num_segments,
len_segments=len_segments,
use_rec_act=use_rec_act,
stop_on_done=True,
subrtn_sbi_training_hparam=dict(
max_num_epochs=1), # only train for 1 iteration
# subrtn_sbi_sampling_hparam=dict(sample_with_mcmc=True, mcmc_parameters=dict(warmup_steps=20)),
num_workers=1,
)
skip = False
if algo_name == NPDR.name:
algo = NPDR(
save_dir=ex_dir,
env_sim=env,
env_real=env_real,
policy=policy,
dp_mapping=dp_mapping,
prior=prior,
embedding=embedding,
subrtn_sbi_class=SNPE_C,
posterior_hparam=posterior_hparam,
subrtn_policy=subrtn_policy,
**algo_hparam,
)
elif algo_name == BayesSim.name:
# We are not checking multi-round SNPE-A since it has known issues
if algo_hparam["num_sbi_rounds"] > 1:
skip = True
algo = BayesSim(
save_dir=ex_dir,
env_sim=env,
env_real=env_real,
policy=policy,
dp_mapping=dp_mapping,
embedding=embedding,
prior=prior,
subrtn_policy=subrtn_policy,
**algo_hparam,
)
else:
raise NotImplementedError
if not skip:
algo.train()
# Just checking the interface here
assert algo.curr_iter == algo.max_iter
def test_sbi_embedding(
ex_dir,
env: SimEnv,
embedding_name: str,
num_segments: int,
len_segments: int,
stop_on_done: bool,
state_mask_labels: Union[None, List[str]],
act_mask_labels: Union[None, List[str]],
):
pyrado.set_seed(0)
# Reduce the number of steps to make this test run faster
env.max_steps = 80
# Policy
policy = QQubeSwingUpAndBalanceCtrl(env.spec)
# Define a mapping: index - domain parameter
dp_mapping = {1: "mass_pend_pole", 2: "length_pend_pole"}
# Time series embedding
if embedding_name == LastStepEmbedding.name:
embedding = LastStepEmbedding(
env.spec,
RolloutSamplerForSBI.get_dim_data(env.spec),
state_mask_labels=state_mask_labels,
act_mask_labels=act_mask_labels,
)
elif embedding_name == AllStepsEmbedding.name:
embedding = AllStepsEmbedding(
env.spec,
RolloutSamplerForSBI.get_dim_data(env.spec),
env.max_steps,
downsampling_factor=3,
state_mask_labels=state_mask_labels,
act_mask_labels=act_mask_labels,
)
elif embedding_name == DeltaStepsEmbedding.name:
embedding = DeltaStepsEmbedding(
env.spec,
RolloutSamplerForSBI.get_dim_data(env.spec),
env.max_steps,
downsampling_factor=3,
state_mask_labels=state_mask_labels,
act_mask_labels=act_mask_labels,
)
elif embedding_name == BayesSimEmbedding.name:
embedding = BayesSimEmbedding(
env.spec,
RolloutSamplerForSBI.get_dim_data(env.spec),
downsampling_factor=3,
state_mask_labels=state_mask_labels,
act_mask_labels=act_mask_labels,
)
elif embedding_name == DynamicTimeWarpingEmbedding.name:
embedding = DynamicTimeWarpingEmbedding(
env.spec,
RolloutSamplerForSBI.get_dim_data(env.spec),
downsampling_factor=3,
state_mask_labels=state_mask_labels,
act_mask_labels=act_mask_labels,
)
elif embedding_name == RNNEmbedding.name:
embedding = RNNEmbedding(
env.spec,
RolloutSamplerForSBI.get_dim_data(env.spec),
hidden_size=10,
num_recurrent_layers=1,
output_size=1,
len_rollouts=env.max_steps,
downsampling_factor=1,
state_mask_labels=state_mask_labels,
act_mask_labels=act_mask_labels,
)
else:
raise NotImplementedError
sampler = SimRolloutSamplerForSBI(
env,
policy,
dp_mapping,
embedding,
num_segments,
len_segments,
stop_on_done,
rollouts_real=None,
use_rec_act=False,
)
# Test with 7 domain parameter sets
data_sim = sampler(to.abs(to.randn(7, 2)))
assert data_sim.shape == (7, embedding.dim_output)
t_end = 10 # s
# Set seed if desired
pyrado.set_seed(args.seed, verbose=True)
# Environments
env_sim_hparams = dict(dt=1 / 250.0, max_steps=int(t_end * 250))
env_sim = QQubeSwingUpSim(**env_sim_hparams)
# env_sim = ActDelayWrapper(env_sim)
# Create the ground truth target domain and the behavioral policy
if ectl:
env_real = osp.join(pyrado.EVAL_DIR,
f"qq-su_ectrl_250Hz_{t_end}s") # 5s long
policy = QQubeSwingUpAndBalanceCtrl(
env_sim.spec
) # replaced by the recorded actions if use_rec_act=True
else:
env_real = osp.join(pyrado.EVAL_DIR,
f"qq_chrip_10to0Hz_+1.5V_250Hz_{t_end}s")
assert use_rec_act
policy = DummyPolicy(env_sim.spec) # replaced by recorded real actions
# Define a mapping: index - domain parameter
dp_mapping = {
0: "damping_rot_pole",
1: "damping_pend_pole",
2: "motor_resistance",
3: "motor_back_emf",
4: "mass_rot_pole",
5: "mass_pend_pole",