cleaned up merge
This commit is contained in:
Fabian
parent
d313795cec
commit
536e78da23
@ -1,15 +0,0 @@
|
|||||||
from alr_envs import dmc, meta, open_ai
|
|
||||||
from alr_envs.utils.make_env_helpers import make, make_dmp_env, make_promp_env, make_rank
|
|
||||||
from alr_envs.utils import make_dmc
|
|
||||||
|
|
||||||
# Convenience function for all MP environments
|
|
||||||
from .alr import ALL_ALR_MOTION_PRIMITIVE_ENVIRONMENTS
|
|
||||||
from .dmc import ALL_DEEPMIND_MOTION_PRIMITIVE_ENVIRONMENTS
|
|
||||||
from .meta import ALL_METAWORLD_MOTION_PRIMITIVE_ENVIRONMENTS
|
|
||||||
from .open_ai import ALL_GYM_MOTION_PRIMITIVE_ENVIRONMENTS
|
|
||||||
|
|
||||||
ALL_MOTION_PRIMITIVE_ENVIRONMENTS = {
|
|
||||||
key: value + ALL_DEEPMIND_MOTION_PRIMITIVE_ENVIRONMENTS[key] +
|
|
||||||
ALL_GYM_MOTION_PRIMITIVE_ENVIRONMENTS[key] +
|
|
||||||
ALL_METAWORLD_MOTION_PRIMITIVE_ENVIRONMENTS[key]
|
|
||||||
for key, value in ALL_ALR_MOTION_PRIMITIVE_ENVIRONMENTS.items()}
|
|
||||||
@ -1,499 +0,0 @@
|
|||||||
import numpy as np
|
|
||||||
from gym import register
|
|
||||||
|
|
||||||
from . import classic_control, mujoco
|
|
||||||
from .classic_control.hole_reacher.hole_reacher import HoleReacherEnv
|
|
||||||
from .classic_control.simple_reacher.simple_reacher import SimpleReacherEnv
|
|
||||||
from .classic_control.viapoint_reacher.viapoint_reacher import ViaPointReacherEnv
|
|
||||||
from .mujoco.ball_in_a_cup.ball_in_a_cup import ALRBallInACupEnv
|
|
||||||
from .mujoco.ball_in_a_cup.biac_pd import ALRBallInACupPDEnv
|
|
||||||
from .mujoco.reacher.alr_reacher import ALRReacherEnv
|
|
||||||
from .mujoco.reacher.balancing import BalancingEnv
|
|
||||||
|
|
||||||
from .mujoco.table_tennis.tt_gym import MAX_EPISODE_STEPS
|
|
||||||
|
|
||||||
ALL_ALR_MOTION_PRIMITIVE_ENVIRONMENTS = {"DMP": [], "ProMP": []}
|
|
||||||
|
|
||||||
# Classic Control
|
|
||||||
## Simple Reacher
|
|
||||||
register(
|
|
||||||
id='SimpleReacher-v0',
|
|
||||||
entry_point='alr_envs.alr.classic_control:SimpleReacherEnv',
|
|
||||||
max_episode_steps=200,
|
|
||||||
kwargs={
|
|
||||||
"n_links": 2,
|
|
||||||
}
|
|
||||||
)
|
|
||||||
|
|
||||||
register(
|
|
||||||
id='SimpleReacher-v1',
|
|
||||||
entry_point='alr_envs.alr.classic_control:SimpleReacherEnv',
|
|
||||||
max_episode_steps=200,
|
|
||||||
kwargs={
|
|
||||||
"n_links": 2,
|
|
||||||
"random_start": False
|
|
||||||
}
|
|
||||||
)
|
|
||||||
|
|
||||||
register(
|
|
||||||
id='LongSimpleReacher-v0',
|
|
||||||
entry_point='alr_envs.alr.classic_control:SimpleReacherEnv',
|
|
||||||
max_episode_steps=200,
|
|
||||||
kwargs={
|
|
||||||
"n_links": 5,
|
|
||||||
}
|
|
||||||
)
|
|
||||||
|
|
||||||
register(
|
|
||||||
id='LongSimpleReacher-v1',
|
|
||||||
entry_point='alr_envs.alr.classic_control:SimpleReacherEnv',
|
|
||||||
max_episode_steps=200,
|
|
||||||
kwargs={
|
|
||||||
"n_links": 5,
|
|
||||||
"random_start": False
|
|
||||||
}
|
|
||||||
)
|
|
||||||
|
|
||||||
## Viapoint Reacher
|
|
||||||
|
|
||||||
register(
|
|
||||||
id='ViaPointReacher-v0',
|
|
||||||
entry_point='alr_envs.alr.classic_control:ViaPointReacherEnv',
|
|
||||||
max_episode_steps=200,
|
|
||||||
kwargs={
|
|
||||||
"n_links": 5,
|
|
||||||
"allow_self_collision": False,
|
|
||||||
"collision_penalty": 1000
|
|
||||||
}
|
|
||||||
)
|
|
||||||
|
|
||||||
## Hole Reacher
|
|
||||||
register(
|
|
||||||
id='HoleReacher-v0',
|
|
||||||
entry_point='alr_envs.alr.classic_control:HoleReacherEnv',
|
|
||||||
max_episode_steps=200,
|
|
||||||
kwargs={
|
|
||||||
"n_links": 5,
|
|
||||||
"random_start": True,
|
|
||||||
"allow_self_collision": False,
|
|
||||||
"allow_wall_collision": False,
|
|
||||||
"hole_width": None,
|
|
||||||
"hole_depth": 1,
|
|
||||||
"hole_x": None,
|
|
||||||
"collision_penalty": 100,
|
|
||||||
}
|
|
||||||
)
|
|
||||||
|
|
||||||
register(
|
|
||||||
id='HoleReacher-v1',
|
|
||||||
entry_point='alr_envs.alr.classic_control:HoleReacherEnv',
|
|
||||||
max_episode_steps=200,
|
|
||||||
kwargs={
|
|
||||||
"n_links": 5,
|
|
||||||
"random_start": False,
|
|
||||||
"allow_self_collision": False,
|
|
||||||
"allow_wall_collision": False,
|
|
||||||
"hole_width": 0.25,
|
|
||||||
"hole_depth": 1,
|
|
||||||
"hole_x": None,
|
|
||||||
"collision_penalty": 100,
|
|
||||||
}
|
|
||||||
)
|
|
||||||
|
|
||||||
register(
|
|
||||||
id='HoleReacher-v2',
|
|
||||||
entry_point='alr_envs.alr.classic_control:HoleReacherEnv',
|
|
||||||
max_episode_steps=200,
|
|
||||||
kwargs={
|
|
||||||
"n_links": 5,
|
|
||||||
"random_start": False,
|
|
||||||
"allow_self_collision": False,
|
|
||||||
"allow_wall_collision": False,
|
|
||||||
"hole_width": 0.25,
|
|
||||||
"hole_depth": 1,
|
|
||||||
"hole_x": 2,
|
|
||||||
"collision_penalty": 1,
|
|
||||||
}
|
|
||||||
)
|
|
||||||
|
|
||||||
# Mujoco
|
|
||||||
|
|
||||||
## Reacher
|
|
||||||
register(
|
|
||||||
id='ALRReacher-v0',
|
|
||||||
entry_point='alr_envs.alr.mujoco:ALRReacherEnv',
|
|
||||||
max_episode_steps=200,
|
|
||||||
kwargs={
|
|
||||||
"steps_before_reward": 0,
|
|
||||||
"n_links": 5,
|
|
||||||
"balance": False,
|
|
||||||
}
|
|
||||||
)
|
|
||||||
|
|
||||||
register(
|
|
||||||
id='ALRReacherSparse-v0',
|
|
||||||
entry_point='alr_envs.alr.mujoco:ALRReacherEnv',
|
|
||||||
max_episode_steps=200,
|
|
||||||
kwargs={
|
|
||||||
"steps_before_reward": 200,
|
|
||||||
"n_links": 5,
|
|
||||||
"balance": False,
|
|
||||||
}
|
|
||||||
)
|
|
||||||
|
|
||||||
register(
|
|
||||||
id='ALRReacherSparseBalanced-v0',
|
|
||||||
entry_point='alr_envs.alr.mujoco:ALRReacherEnv',
|
|
||||||
max_episode_steps=200,
|
|
||||||
kwargs={
|
|
||||||
"steps_before_reward": 200,
|
|
||||||
"n_links": 5,
|
|
||||||
"balance": True,
|
|
||||||
}
|
|
||||||
)
|
|
||||||
|
|
||||||
register(
|
|
||||||
id='ALRLongReacher-v0',
|
|
||||||
entry_point='alr_envs.alr.mujoco:ALRReacherEnv',
|
|
||||||
max_episode_steps=200,
|
|
||||||
kwargs={
|
|
||||||
"steps_before_reward": 0,
|
|
||||||
"n_links": 7,
|
|
||||||
"balance": False,
|
|
||||||
}
|
|
||||||
)
|
|
||||||
|
|
||||||
register(
|
|
||||||
id='ALRLongReacherSparse-v0',
|
|
||||||
entry_point='alr_envs.alr.mujoco:ALRReacherEnv',
|
|
||||||
max_episode_steps=200,
|
|
||||||
kwargs={
|
|
||||||
"steps_before_reward": 200,
|
|
||||||
"n_links": 7,
|
|
||||||
"balance": False,
|
|
||||||
}
|
|
||||||
)
|
|
||||||
|
|
||||||
register(
|
|
||||||
id='ALRLongReacherSparseBalanced-v0',
|
|
||||||
entry_point='alr_envs.alr.mujoco:ALRReacherEnv',
|
|
||||||
max_episode_steps=200,
|
|
||||||
kwargs={
|
|
||||||
"steps_before_reward": 200,
|
|
||||||
"n_links": 7,
|
|
||||||
"balance": True,
|
|
||||||
}
|
|
||||||
)
|
|
||||||
|
|
||||||
## Balancing Reacher
|
|
||||||
|
|
||||||
register(
|
|
||||||
id='Balancing-v0',
|
|
||||||
entry_point='alr_envs.alr.mujoco:BalancingEnv',
|
|
||||||
max_episode_steps=200,
|
|
||||||
kwargs={
|
|
||||||
"n_links": 5,
|
|
||||||
}
|
|
||||||
)
|
|
||||||
|
|
||||||
## Table Tennis
|
|
||||||
register(id='TableTennis2DCtxt-v0',
|
|
||||||
entry_point='alr_envs.alr.mujoco:TTEnvGym',
|
|
||||||
max_episode_steps=MAX_EPISODE_STEPS,
|
|
||||||
kwargs={'ctxt_dim': 2})
|
|
||||||
|
|
||||||
register(id='TableTennis2DCtxt-v1',
|
|
||||||
entry_point='alr_envs.alr.mujoco:TTEnvGym',
|
|
||||||
max_episode_steps=MAX_EPISODE_STEPS,
|
|
||||||
kwargs={'ctxt_dim': 2, 'fixed_goal': True})
|
|
||||||
|
|
||||||
register(id='TableTennis4DCtxt-v0',
|
|
||||||
entry_point='alr_envs.alr.mujoco:TTEnvGym',
|
|
||||||
max_episode_steps=MAX_EPISODE_STEPS,
|
|
||||||
kwargs={'ctxt_dim': 4})
|
|
||||||
|
|
||||||
## BeerPong
|
|
||||||
difficulties = ["simple", "intermediate", "hard", "hardest"]
|
|
||||||
|
|
||||||
for v, difficulty in enumerate(difficulties):
|
|
||||||
register(
|
|
||||||
id='ALRBeerPong-v{}'.format(v),
|
|
||||||
entry_point='alr_envs.alr.mujoco:ALRBeerBongEnv',
|
|
||||||
max_episode_steps=600,
|
|
||||||
kwargs={
|
|
||||||
"difficulty": difficulty,
|
|
||||||
"reward_type": "staged",
|
|
||||||
}
|
|
||||||
)
|
|
||||||
|
|
||||||
# Motion Primitive Environments
|
|
||||||
|
|
||||||
## Simple Reacher
|
|
||||||
_versions = ["SimpleReacher-v0", "SimpleReacher-v1", "LongSimpleReacher-v0", "LongSimpleReacher-v1"]
|
|
||||||
for _v in _versions:
|
|
||||||
_name = _v.split("-")
|
|
||||||
_env_id = f'{_name[0]}DMP-{_name[1]}'
|
|
||||||
register(
|
|
||||||
id=_env_id,
|
|
||||||
entry_point='alr_envs.utils.make_env_helpers:make_dmp_env_helper',
|
|
||||||
# max_episode_steps=1,
|
|
||||||
kwargs={
|
|
||||||
"name": f"alr_envs:{_v}",
|
|
||||||
"wrappers": [classic_control.simple_reacher.MPWrapper],
|
|
||||||
"mp_kwargs": {
|
|
||||||
"num_dof": 2 if "long" not in _v.lower() else 5,
|
|
||||||
"num_basis": 5,
|
|
||||||
"duration": 2,
|
|
||||||
"alpha_phase": 2,
|
|
||||||
"learn_goal": True,
|
|
||||||
"policy_type": "motor",
|
|
||||||
"weights_scale": 50,
|
|
||||||
"policy_kwargs": {
|
|
||||||
"p_gains": .6,
|
|
||||||
"d_gains": .075
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
)
|
|
||||||
ALL_ALR_MOTION_PRIMITIVE_ENVIRONMENTS["DMP"].append(_env_id)
|
|
||||||
|
|
||||||
_env_id = f'{_name[0]}ProMP-{_name[1]}'
|
|
||||||
register(
|
|
||||||
id=_env_id,
|
|
||||||
entry_point='alr_envs.utils.make_env_helpers:make_promp_env_helper',
|
|
||||||
kwargs={
|
|
||||||
"name": f"alr_envs:{_v}",
|
|
||||||
"wrappers": [classic_control.simple_reacher.MPWrapper],
|
|
||||||
"mp_kwargs": {
|
|
||||||
"num_dof": 2 if "long" not in _v.lower() else 5,
|
|
||||||
"num_basis": 5,
|
|
||||||
"duration": 2,
|
|
||||||
"policy_type": "motor",
|
|
||||||
"weights_scale": 1,
|
|
||||||
"zero_start": True,
|
|
||||||
"policy_kwargs": {
|
|
||||||
"p_gains": .6,
|
|
||||||
"d_gains": .075
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
)
|
|
||||||
ALL_ALR_MOTION_PRIMITIVE_ENVIRONMENTS["ProMP"].append(_env_id)
|
|
||||||
|
|
||||||
# Viapoint reacher
|
|
||||||
register(
|
|
||||||
id='ViaPointReacherDMP-v0',
|
|
||||||
entry_point='alr_envs.utils.make_env_helpers:make_dmp_env_helper',
|
|
||||||
# max_episode_steps=1,
|
|
||||||
kwargs={
|
|
||||||
"name": "alr_envs:ViaPointReacher-v0",
|
|
||||||
"wrappers": [classic_control.viapoint_reacher.MPWrapper],
|
|
||||||
"mp_kwargs": {
|
|
||||||
"num_dof": 5,
|
|
||||||
"num_basis": 5,
|
|
||||||
"duration": 2,
|
|
||||||
"learn_goal": True,
|
|
||||||
"alpha_phase": 2,
|
|
||||||
"policy_type": "velocity",
|
|
||||||
"weights_scale": 50,
|
|
||||||
}
|
|
||||||
}
|
|
||||||
)
|
|
||||||
ALL_ALR_MOTION_PRIMITIVE_ENVIRONMENTS["DMP"].append("ViaPointReacherDMP-v0")
|
|
||||||
|
|
||||||
register(
|
|
||||||
id="ViaPointReacherProMP-v0",
|
|
||||||
entry_point='alr_envs.utils.make_env_helpers:make_promp_env_helper',
|
|
||||||
kwargs={
|
|
||||||
"name": f"alr_envs:ViaPointReacher-v0",
|
|
||||||
"wrappers": [classic_control.viapoint_reacher.MPWrapper],
|
|
||||||
"mp_kwargs": {
|
|
||||||
"num_dof": 5,
|
|
||||||
"num_basis": 5,
|
|
||||||
"duration": 2,
|
|
||||||
"policy_type": "velocity",
|
|
||||||
"weights_scale": 1,
|
|
||||||
"zero_start": True
|
|
||||||
}
|
|
||||||
}
|
|
||||||
)
|
|
||||||
ALL_ALR_MOTION_PRIMITIVE_ENVIRONMENTS["ProMP"].append("ViaPointReacherProMP-v0")
|
|
||||||
|
|
||||||
## Hole Reacher
|
|
||||||
_versions = ["v0", "v1", "v2"]
|
|
||||||
for _v in _versions:
|
|
||||||
_env_id = f'HoleReacherDMP-{_v}'
|
|
||||||
register(
|
|
||||||
id=_env_id,
|
|
||||||
entry_point='alr_envs.utils.make_env_helpers:make_dmp_env_helper',
|
|
||||||
# max_episode_steps=1,
|
|
||||||
kwargs={
|
|
||||||
"name": f"alr_envs:HoleReacher-{_v}",
|
|
||||||
"wrappers": [classic_control.hole_reacher.MPWrapper],
|
|
||||||
"mp_kwargs": {
|
|
||||||
"num_dof": 5,
|
|
||||||
"num_basis": 5,
|
|
||||||
"duration": 2,
|
|
||||||
"learn_goal": True,
|
|
||||||
"alpha_phase": 2.5,
|
|
||||||
"bandwidth_factor": 2,
|
|
||||||
"policy_type": "velocity",
|
|
||||||
"weights_scale": 50,
|
|
||||||
"goal_scale": 0.1
|
|
||||||
}
|
|
||||||
}
|
|
||||||
)
|
|
||||||
ALL_ALR_MOTION_PRIMITIVE_ENVIRONMENTS["DMP"].append(_env_id)
|
|
||||||
|
|
||||||
_env_id = f'HoleReacherProMP-{_v}'
|
|
||||||
register(
|
|
||||||
id=_env_id,
|
|
||||||
entry_point='alr_envs.utils.make_env_helpers:make_promp_env_helper',
|
|
||||||
kwargs={
|
|
||||||
"name": f"alr_envs:HoleReacher-{_v}",
|
|
||||||
"wrappers": [classic_control.hole_reacher.MPWrapper],
|
|
||||||
"mp_kwargs": {
|
|
||||||
"num_dof": 5,
|
|
||||||
"num_basis": 5,
|
|
||||||
"duration": 2,
|
|
||||||
"policy_type": "velocity",
|
|
||||||
"weights_scale": 5,
|
|
||||||
"zero_start": True
|
|
||||||
}
|
|
||||||
}
|
|
||||||
)
|
|
||||||
ALL_ALR_MOTION_PRIMITIVE_ENVIRONMENTS["ProMP"].append(_env_id)
|
|
||||||
|
|
||||||
## ALRReacher
|
|
||||||
_versions = ["ALRReacher-v0", "ALRLongReacher-v0", "ALRReacherSparse-v0", "ALRLongReacherSparse-v0"]
|
|
||||||
for _v in _versions:
|
|
||||||
_name = _v.split("-")
|
|
||||||
_env_id = f'{_name[0]}DMP-{_name[1]}'
|
|
||||||
register(
|
|
||||||
id=_env_id,
|
|
||||||
entry_point='alr_envs.utils.make_env_helpers:make_dmp_env_helper',
|
|
||||||
# max_episode_steps=1,
|
|
||||||
kwargs={
|
|
||||||
"name": f"alr_envs:{_v}",
|
|
||||||
"wrappers": [mujoco.reacher.MPWrapper],
|
|
||||||
"mp_kwargs": {
|
|
||||||
"num_dof": 5 if "long" not in _v.lower() else 7,
|
|
||||||
"num_basis": 2,
|
|
||||||
"duration": 4,
|
|
||||||
"alpha_phase": 2,
|
|
||||||
"learn_goal": True,
|
|
||||||
"policy_type": "motor",
|
|
||||||
"weights_scale": 5,
|
|
||||||
"policy_kwargs": {
|
|
||||||
"p_gains": 1,
|
|
||||||
"d_gains": 0.1
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
)
|
|
||||||
ALL_ALR_MOTION_PRIMITIVE_ENVIRONMENTS["DMP"].append(_env_id)
|
|
||||||
|
|
||||||
_env_id = f'{_name[0]}ProMP-{_name[1]}'
|
|
||||||
register(
|
|
||||||
id=_env_id,
|
|
||||||
entry_point='alr_envs.utils.make_env_helpers:make_promp_env_helper',
|
|
||||||
kwargs={
|
|
||||||
"name": f"alr_envs:{_v}",
|
|
||||||
"wrappers": [mujoco.reacher.MPWrapper],
|
|
||||||
"mp_kwargs": {
|
|
||||||
"num_dof": 5 if "long" not in _v.lower() else 7,
|
|
||||||
"num_basis": 1,
|
|
||||||
"duration": 4,
|
|
||||||
"policy_type": "motor",
|
|
||||||
"weights_scale": 5,
|
|
||||||
"zero_start": True,
|
|
||||||
"policy_kwargs": {
|
|
||||||
"p_gains": 1,
|
|
||||||
"d_gains": 0.1
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
)
|
|
||||||
ALL_ALR_MOTION_PRIMITIVE_ENVIRONMENTS["ProMP"].append(_env_id)
|
|
||||||
|
|
||||||
## Beerpong
|
|
||||||
_versions = ["v0", "v1", "v2", "v3"]
|
|
||||||
for _v in _versions:
|
|
||||||
_env_id = f'BeerpongProMP-{_v}'
|
|
||||||
register(
|
|
||||||
id=_env_id,
|
|
||||||
entry_point='alr_envs.utils.make_env_helpers:make_promp_env_helper',
|
|
||||||
kwargs={
|
|
||||||
"name": f"alr_envs:ALRBeerPong-{_v}",
|
|
||||||
"wrappers": [mujoco.beerpong.MPWrapper],
|
|
||||||
"mp_kwargs": {
|
|
||||||
"num_dof": 7,
|
|
||||||
"num_basis": 2,
|
|
||||||
"duration": 1,
|
|
||||||
"post_traj_time": 2,
|
|
||||||
"policy_type": "motor",
|
|
||||||
"weights_scale": 1,
|
|
||||||
"zero_start": True,
|
|
||||||
"zero_goal": False,
|
|
||||||
"policy_kwargs": {
|
|
||||||
"p_gains": np.array([ 1.5, 5, 2.55, 3, 2., 2, 1.25]),
|
|
||||||
"d_gains": np.array([0.02333333, 0.1, 0.0625, 0.08, 0.03, 0.03, 0.0125])
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
)
|
|
||||||
ALL_ALR_MOTION_PRIMITIVE_ENVIRONMENTS["ProMP"].append(_env_id)
|
|
||||||
|
|
||||||
## Table Tennis
|
|
||||||
ctxt_dim = [2, 4]
|
|
||||||
for _v, cd in enumerate(ctxt_dim):
|
|
||||||
_env_id = f'TableTennisProMP-v{_v}'
|
|
||||||
register(
|
|
||||||
id=_env_id,
|
|
||||||
entry_point='alr_envs.utils.make_env_helpers:make_promp_env_helper',
|
|
||||||
kwargs={
|
|
||||||
"name": "alr_envs:TableTennis{}DCtxt-v0".format(cd),
|
|
||||||
"wrappers": [mujoco.table_tennis.MPWrapper],
|
|
||||||
"mp_kwargs": {
|
|
||||||
"num_dof": 7,
|
|
||||||
"num_basis": 2,
|
|
||||||
"duration": 1.25,
|
|
||||||
"post_traj_time": 4.5,
|
|
||||||
"policy_type": "motor",
|
|
||||||
"weights_scale": 1.0,
|
|
||||||
"zero_start": True,
|
|
||||||
"zero_goal": False,
|
|
||||||
"policy_kwargs": {
|
|
||||||
"p_gains": 0.5*np.array([1.0, 4.0, 2.0, 4.0, 1.0, 4.0, 1.0]),
|
|
||||||
"d_gains": 0.5*np.array([0.1, 0.4, 0.2, 0.4, 0.1, 0.4, 0.1])
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
)
|
|
||||||
ALL_ALR_MOTION_PRIMITIVE_ENVIRONMENTS["ProMP"].append(_env_id)
|
|
||||||
|
|
||||||
register(
|
|
||||||
id='TableTennisProMP-v2',
|
|
||||||
entry_point='alr_envs.utils.make_env_helpers:make_promp_env_helper',
|
|
||||||
kwargs={
|
|
||||||
"name": "alr_envs:TableTennis2DCtxt-v1",
|
|
||||||
"wrappers": [mujoco.table_tennis.MPWrapper],
|
|
||||||
"mp_kwargs": {
|
|
||||||
"num_dof": 7,
|
|
||||||
"num_basis": 2,
|
|
||||||
"duration": 1.,
|
|
||||||
"post_traj_time": 2.5,
|
|
||||||
"policy_type": "motor",
|
|
||||||
"weights_scale": 1,
|
|
||||||
"off": -0.05,
|
|
||||||
"bandwidth_factor": 3.5,
|
|
||||||
"zero_start": True,
|
|
||||||
"zero_goal": False,
|
|
||||||
"policy_kwargs": {
|
|
||||||
"p_gains": 0.5*np.array([1.0, 4.0, 2.0, 4.0, 1.0, 4.0, 1.0]),
|
|
||||||
"d_gains": 0.5*np.array([0.1, 0.4, 0.2, 0.4, 0.1, 0.4, 0.1])
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
)
|
|
||||||
ALL_ALR_MOTION_PRIMITIVE_ENVIRONMENTS["ProMP"].append("TableTennisProMP-v2")
|
|
||||||
@ -1,18 +0,0 @@
|
|||||||
### Classic Control
|
|
||||||
|
|
||||||
## Step-based Environments
|
|
||||||
|Name| Description|Horizon|Action Dimension|Observation Dimension
|
|
||||||
|---|---|---|---|---|
|
|
||||||
|`SimpleReacher-v0`| Simple reaching task (2 links) without any physics simulation. Provides no reward until 150 time steps. This allows the agent to explore the space, but requires precise actions towards the end of the trajectory.| 200 | 2 | 9
|
|
||||||
|`LongSimpleReacher-v0`| Simple reaching task (5 links) without any physics simulation. Provides no reward until 150 time steps. This allows the agent to explore the space, but requires precise actions towards the end of the trajectory.| 200 | 5 | 18
|
|
||||||
|`ViaPointReacher-v0`| Simple reaching task leveraging a via point, which supports self collision detection. Provides a reward only at 100 and 199 for reaching the viapoint and goal point, respectively.| 200 | 5 | 18
|
|
||||||
|`HoleReacher-v0`| 5 link reaching task where the end-effector needs to reach into a narrow hole without collding with itself or walls | 200 | 5 | 18
|
|
||||||
|
|
||||||
## MP Environments
|
|
||||||
|Name| Description|Horizon|Action Dimension|Context Dimension
|
|
||||||
|---|---|---|---|---|
|
|
||||||
|`ViaPointReacherDMP-v0`| A DMP provides a trajectory for the `ViaPointReacher-v0` task. | 200 | 25
|
|
||||||
|`HoleReacherFixedGoalDMP-v0`| A DMP provides a trajectory for the `HoleReacher-v0` task with a fixed goal attractor. | 200 | 25
|
|
||||||
|`HoleReacherDMP-v0`| A DMP provides a trajectory for the `HoleReacher-v0` task. The goal attractor needs to be learned. | 200 | 30
|
|
||||||
|
|
||||||
[//]: |`HoleReacherProMPP-v0`|
|
|
||||||
@ -1,3 +0,0 @@
|
|||||||
from .hole_reacher.hole_reacher import HoleReacherEnv
|
|
||||||
from .simple_reacher.simple_reacher import SimpleReacherEnv
|
|
||||||
from .viapoint_reacher.viapoint_reacher import ViaPointReacherEnv
|
|
||||||
@ -1,146 +0,0 @@
|
|||||||
from typing import Iterable, Union
|
|
||||||
from abc import ABC, abstractmethod
|
|
||||||
import gym
|
|
||||||
import matplotlib.pyplot as plt
|
|
||||||
import numpy as np
|
|
||||||
from gym import spaces
|
|
||||||
from gym.utils import seeding
|
|
||||||
from alr_envs.alr.classic_control.utils import intersect
|
|
||||||
|
|
||||||
|
|
||||||
class BaseReacherEnv(gym.Env, ABC):
|
|
||||||
"""
|
|
||||||
Base class for all reaching environments.
|
|
||||||
"""
|
|
||||||
|
|
||||||
def __init__(self, n_links: int, random_start: bool = True,
|
|
||||||
allow_self_collision: bool = False):
|
|
||||||
super().__init__()
|
|
||||||
self.link_lengths = np.ones(n_links)
|
|
||||||
self.n_links = n_links
|
|
||||||
self._dt = 0.01
|
|
||||||
|
|
||||||
self.random_start = random_start
|
|
||||||
|
|
||||||
self.allow_self_collision = allow_self_collision
|
|
||||||
|
|
||||||
# state
|
|
||||||
self._joints = None
|
|
||||||
self._joint_angles = None
|
|
||||||
self._angle_velocity = None
|
|
||||||
self._acc = None
|
|
||||||
self._start_pos = np.hstack([[np.pi / 2], np.zeros(self.n_links - 1)])
|
|
||||||
self._start_vel = np.zeros(self.n_links)
|
|
||||||
|
|
||||||
# joint limits
|
|
||||||
self.j_min = -np.pi * np.ones(n_links)
|
|
||||||
self.j_max = np.pi * np.ones(n_links)
|
|
||||||
|
|
||||||
self.steps_before_reward = 199
|
|
||||||
|
|
||||||
state_bound = np.hstack([
|
|
||||||
[np.pi] * self.n_links, # cos
|
|
||||||
[np.pi] * self.n_links, # sin
|
|
||||||
[np.inf] * self.n_links, # velocity
|
|
||||||
[np.inf] * 2, # x-y coordinates of target distance
|
|
||||||
[np.inf] # env steps, because reward start after n steps TODO: Maybe
|
|
||||||
])
|
|
||||||
|
|
||||||
self.observation_space = spaces.Box(low=-state_bound, high=state_bound, shape=state_bound.shape)
|
|
||||||
|
|
||||||
self.reward_function = None # Needs to be set in sub class
|
|
||||||
|
|
||||||
# containers for plotting
|
|
||||||
self.metadata = {'render.modes': ["human"]}
|
|
||||||
self.fig = None
|
|
||||||
|
|
||||||
self._steps = 0
|
|
||||||
self.seed()
|
|
||||||
|
|
||||||
@property
|
|
||||||
def dt(self) -> Union[float, int]:
|
|
||||||
return self._dt
|
|
||||||
|
|
||||||
@property
|
|
||||||
def current_pos(self):
|
|
||||||
return self._joint_angles.copy()
|
|
||||||
|
|
||||||
@property
|
|
||||||
def current_vel(self):
|
|
||||||
return self._angle_velocity.copy()
|
|
||||||
|
|
||||||
def reset(self):
|
|
||||||
# Sample only orientation of first link, i.e. the arm is always straight.
|
|
||||||
if self.random_start:
|
|
||||||
first_joint = self.np_random.uniform(np.pi / 4, 3 * np.pi / 4)
|
|
||||||
self._joint_angles = np.hstack([[first_joint], np.zeros(self.n_links - 1)])
|
|
||||||
self._start_pos = self._joint_angles.copy()
|
|
||||||
else:
|
|
||||||
self._joint_angles = self._start_pos
|
|
||||||
|
|
||||||
self._angle_velocity = self._start_vel
|
|
||||||
self._joints = np.zeros((self.n_links + 1, 2))
|
|
||||||
self._update_joints()
|
|
||||||
self._steps = 0
|
|
||||||
|
|
||||||
return self._get_obs().copy()
|
|
||||||
|
|
||||||
@abstractmethod
|
|
||||||
def step(self, action: np.ndarray):
|
|
||||||
"""
|
|
||||||
A single step with action in angular velocity space
|
|
||||||
"""
|
|
||||||
raise NotImplementedError
|
|
||||||
|
|
||||||
def _update_joints(self):
|
|
||||||
"""
|
|
||||||
update joints to get new end-effector position. The other links are only required for rendering.
|
|
||||||
Returns:
|
|
||||||
|
|
||||||
"""
|
|
||||||
angles = np.cumsum(self._joint_angles)
|
|
||||||
x = self.link_lengths * np.vstack([np.cos(angles), np.sin(angles)])
|
|
||||||
self._joints[1:] = self._joints[0] + np.cumsum(x.T, axis=0)
|
|
||||||
|
|
||||||
def _check_self_collision(self):
|
|
||||||
"""Checks whether line segments intersect"""
|
|
||||||
|
|
||||||
if self.allow_self_collision:
|
|
||||||
return False
|
|
||||||
|
|
||||||
if np.any(self._joint_angles > self.j_max) or np.any(self._joint_angles < self.j_min):
|
|
||||||
return True
|
|
||||||
|
|
||||||
link_lines = np.stack((self._joints[:-1, :], self._joints[1:, :]), axis=1)
|
|
||||||
for i, line1 in enumerate(link_lines):
|
|
||||||
for line2 in link_lines[i + 2:, :]:
|
|
||||||
if intersect(line1[0], line1[-1], line2[0], line2[-1]):
|
|
||||||
return True
|
|
||||||
return False
|
|
||||||
|
|
||||||
@abstractmethod
|
|
||||||
def _get_reward(self, action: np.ndarray) -> (float, dict):
|
|
||||||
pass
|
|
||||||
|
|
||||||
@abstractmethod
|
|
||||||
def _get_obs(self) -> np.ndarray:
|
|
||||||
pass
|
|
||||||
|
|
||||||
@abstractmethod
|
|
||||||
def _check_collisions(self) -> bool:
|
|
||||||
pass
|
|
||||||
|
|
||||||
@abstractmethod
|
|
||||||
def _terminate(self, info) -> bool:
|
|
||||||
return False
|
|
||||||
|
|
||||||
def seed(self, seed=None):
|
|
||||||
self.np_random, seed = seeding.np_random(seed)
|
|
||||||
return [seed]
|
|
||||||
|
|
||||||
def close(self):
|
|
||||||
del self.fig
|
|
||||||
|
|
||||||
@property
|
|
||||||
def end_effector(self):
|
|
||||||
return self._joints[self.n_links].T
|
|
||||||
@ -1,37 +0,0 @@
|
|||||||
from abc import ABC
|
|
||||||
|
|
||||||
from gym import spaces
|
|
||||||
import numpy as np
|
|
||||||
from alr_envs.alr.classic_control.base_reacher.base_reacher import BaseReacherEnv
|
|
||||||
|
|
||||||
|
|
||||||
class BaseReacherDirectEnv(BaseReacherEnv, ABC):
|
|
||||||
"""
|
|
||||||
Base class for directly controlled reaching environments
|
|
||||||
"""
|
|
||||||
def __init__(self, n_links: int, random_start: bool = True,
|
|
||||||
allow_self_collision: bool = False):
|
|
||||||
super().__init__(n_links, random_start, allow_self_collision)
|
|
||||||
|
|
||||||
self.max_vel = 2 * np.pi
|
|
||||||
action_bound = np.ones((self.n_links,)) * self.max_vel
|
|
||||||
self.action_space = spaces.Box(low=-action_bound, high=action_bound, shape=action_bound.shape)
|
|
||||||
|
|
||||||
def step(self, action: np.ndarray):
|
|
||||||
"""
|
|
||||||
A single step with action in angular velocity space
|
|
||||||
"""
|
|
||||||
|
|
||||||
self._acc = (action - self._angle_velocity) / self.dt
|
|
||||||
self._angle_velocity = action
|
|
||||||
self._joint_angles = self._joint_angles + self.dt * self._angle_velocity
|
|
||||||
self._update_joints()
|
|
||||||
|
|
||||||
self._is_collided = self._check_collisions()
|
|
||||||
|
|
||||||
reward, info = self._get_reward(action)
|
|
||||||
|
|
||||||
self._steps += 1
|
|
||||||
done = self._terminate(info)
|
|
||||||
|
|
||||||
return self._get_obs().copy(), reward, done, info
|
|
||||||
@ -1,36 +0,0 @@
|
|||||||
from abc import ABC
|
|
||||||
|
|
||||||
from gym import spaces
|
|
||||||
import numpy as np
|
|
||||||
from alr_envs.alr.classic_control.base_reacher.base_reacher import BaseReacherEnv
|
|
||||||
|
|
||||||
|
|
||||||
class BaseReacherTorqueEnv(BaseReacherEnv, ABC):
|
|
||||||
"""
|
|
||||||
Base class for torque controlled reaching environments
|
|
||||||
"""
|
|
||||||
def __init__(self, n_links: int, random_start: bool = True,
|
|
||||||
allow_self_collision: bool = False):
|
|
||||||
super().__init__(n_links, random_start, allow_self_collision)
|
|
||||||
|
|
||||||
self.max_torque = 1000
|
|
||||||
action_bound = np.ones((self.n_links,)) * self.max_torque
|
|
||||||
self.action_space = spaces.Box(low=-action_bound, high=action_bound, shape=action_bound.shape)
|
|
||||||
|
|
||||||
def step(self, action: np.ndarray):
|
|
||||||
"""
|
|
||||||
A single step with action in torque space
|
|
||||||
"""
|
|
||||||
|
|
||||||
self._angle_velocity = self._angle_velocity + self.dt * action
|
|
||||||
self._joint_angles = self._joint_angles + self.dt * self._angle_velocity
|
|
||||||
self._update_joints()
|
|
||||||
|
|
||||||
self._is_collided = self._check_collisions()
|
|
||||||
|
|
||||||
reward, info = self._get_reward(action)
|
|
||||||
|
|
||||||
self._steps += 1
|
|
||||||
done = False
|
|
||||||
|
|
||||||
return self._get_obs().copy(), reward, done, info
|
|
||||||
@ -1 +0,0 @@
|
|||||||
from .mp_wrapper import MPWrapper
|
|
||||||
@ -1,231 +0,0 @@
|
|||||||
from typing import Union
|
|
||||||
|
|
||||||
import gym
|
|
||||||
import matplotlib.pyplot as plt
|
|
||||||
import numpy as np
|
|
||||||
from matplotlib import patches
|
|
||||||
|
|
||||||
from alr_envs.alr.classic_control.base_reacher.base_reacher_direct import BaseReacherDirectEnv
|
|
||||||
|
|
||||||
|
|
||||||
class HoleReacherEnv(BaseReacherDirectEnv):
|
|
||||||
def __init__(self, n_links: int, hole_x: Union[None, float] = None, hole_depth: Union[None, float] = None,
|
|
||||||
hole_width: float = 1., random_start: bool = False, allow_self_collision: bool = False,
|
|
||||||
allow_wall_collision: bool = False, collision_penalty: float = 1000, rew_fct: str = "simple"):
|
|
||||||
|
|
||||||
super().__init__(n_links, random_start, allow_self_collision)
|
|
||||||
|
|
||||||
# provided initial parameters
|
|
||||||
self.initial_x = hole_x # x-position of center of hole
|
|
||||||
self.initial_width = hole_width # width of hole
|
|
||||||
self.initial_depth = hole_depth # depth of hole
|
|
||||||
|
|
||||||
# temp container for current env state
|
|
||||||
self._tmp_x = None
|
|
||||||
self._tmp_width = None
|
|
||||||
self._tmp_depth = None
|
|
||||||
self._goal = None # x-y coordinates for reaching the center at the bottom of the hole
|
|
||||||
|
|
||||||
# action_bound = np.pi * np.ones((self.n_links,))
|
|
||||||
state_bound = np.hstack([
|
|
||||||
[np.pi] * self.n_links, # cos
|
|
||||||
[np.pi] * self.n_links, # sin
|
|
||||||
[np.inf] * self.n_links, # velocity
|
|
||||||
[np.inf], # hole width
|
|
||||||
# [np.inf], # hole depth
|
|
||||||
[np.inf] * 2, # x-y coordinates of target distance
|
|
||||||
[np.inf] # env steps, because reward start after n steps TODO: Maybe
|
|
||||||
])
|
|
||||||
# self.action_space = gym.spaces.Box(low=-action_bound, high=action_bound, shape=action_bound.shape)
|
|
||||||
self.observation_space = gym.spaces.Box(low=-state_bound, high=state_bound, shape=state_bound.shape)
|
|
||||||
|
|
||||||
if rew_fct == "simple":
|
|
||||||
from alr_envs.alr.classic_control.hole_reacher.hr_simple_reward import HolereacherReward
|
|
||||||
self.reward_function = HolereacherReward(allow_self_collision, allow_wall_collision, collision_penalty)
|
|
||||||
elif rew_fct == "vel_acc":
|
|
||||||
from alr_envs.alr.classic_control.hole_reacher.hr_dist_vel_acc_reward import HolereacherReward
|
|
||||||
self.reward_function = HolereacherReward(allow_self_collision, allow_wall_collision, collision_penalty)
|
|
||||||
else:
|
|
||||||
raise ValueError("Unknown reward function {}".format(rew_fct))
|
|
||||||
|
|
||||||
def reset(self):
|
|
||||||
self._generate_hole()
|
|
||||||
self._set_patches()
|
|
||||||
self.reward_function.reset()
|
|
||||||
|
|
||||||
return super().reset()
|
|
||||||
|
|
||||||
def _get_reward(self, action: np.ndarray) -> (float, dict):
|
|
||||||
return self.reward_function.get_reward(self)
|
|
||||||
|
|
||||||
def _terminate(self, info):
|
|
||||||
return info["is_collided"]
|
|
||||||
|
|
||||||
def _generate_hole(self):
|
|
||||||
if self.initial_width is None:
|
|
||||||
width = self.np_random.uniform(0.15, 0.5)
|
|
||||||
else:
|
|
||||||
width = np.copy(self.initial_width)
|
|
||||||
if self.initial_x is None:
|
|
||||||
# sample whole on left or right side
|
|
||||||
direction = self.np_random.choice([-1, 1])
|
|
||||||
# Hole center needs to be half the width away from the arm to give a valid setting.
|
|
||||||
x = direction * self.np_random.uniform(width / 2, 3.5)
|
|
||||||
else:
|
|
||||||
x = np.copy(self.initial_x)
|
|
||||||
if self.initial_depth is None:
|
|
||||||
# TODO we do not want this right now.
|
|
||||||
depth = self.np_random.uniform(1, 1)
|
|
||||||
else:
|
|
||||||
depth = np.copy(self.initial_depth)
|
|
||||||
|
|
||||||
self._tmp_width = width
|
|
||||||
self._tmp_x = x
|
|
||||||
self._tmp_depth = depth
|
|
||||||
self._goal = np.hstack([self._tmp_x, -self._tmp_depth])
|
|
||||||
|
|
||||||
self._line_ground_left = np.array([-self.n_links, 0, x - width / 2, 0])
|
|
||||||
self._line_ground_right = np.array([x + width / 2, 0, self.n_links, 0])
|
|
||||||
self._line_ground_hole = np.array([x - width / 2, -depth, x + width / 2, -depth])
|
|
||||||
self._line_hole_left = np.array([x - width / 2, -depth, x - width / 2, 0])
|
|
||||||
self._line_hole_right = np.array([x + width / 2, -depth, x + width / 2, 0])
|
|
||||||
|
|
||||||
self.ground_lines = np.stack((self._line_ground_left,
|
|
||||||
self._line_ground_right,
|
|
||||||
self._line_ground_hole,
|
|
||||||
self._line_hole_left,
|
|
||||||
self._line_hole_right))
|
|
||||||
|
|
||||||
def _get_obs(self):
|
|
||||||
theta = self._joint_angles
|
|
||||||
return np.hstack([
|
|
||||||
np.cos(theta),
|
|
||||||
np.sin(theta),
|
|
||||||
self._angle_velocity,
|
|
||||||
self._tmp_width,
|
|
||||||
# self._tmp_hole_depth,
|
|
||||||
self.end_effector - self._goal,
|
|
||||||
self._steps
|
|
||||||
]).astype(np.float32)
|
|
||||||
|
|
||||||
def _get_line_points(self, num_points_per_link=1):
|
|
||||||
theta = self._joint_angles[:, None]
|
|
||||||
|
|
||||||
intermediate_points = np.linspace(0, 1, num_points_per_link) if num_points_per_link > 1 else 1
|
|
||||||
accumulated_theta = np.cumsum(theta, axis=0)
|
|
||||||
end_effector = np.zeros(shape=(self.n_links, num_points_per_link, 2))
|
|
||||||
|
|
||||||
x = np.cos(accumulated_theta) * self.link_lengths[:, None] * intermediate_points
|
|
||||||
y = np.sin(accumulated_theta) * self.link_lengths[:, None] * intermediate_points
|
|
||||||
|
|
||||||
end_effector[0, :, 0] = x[0, :]
|
|
||||||
end_effector[0, :, 1] = y[0, :]
|
|
||||||
|
|
||||||
for i in range(1, self.n_links):
|
|
||||||
end_effector[i, :, 0] = x[i, :] + end_effector[i - 1, -1, 0]
|
|
||||||
end_effector[i, :, 1] = y[i, :] + end_effector[i - 1, -1, 1]
|
|
||||||
|
|
||||||
return np.squeeze(end_effector + self._joints[0, :])
|
|
||||||
|
|
||||||
def _check_collisions(self) -> bool:
|
|
||||||
return self._check_self_collision() or self.check_wall_collision()
|
|
||||||
|
|
||||||
def check_wall_collision(self):
|
|
||||||
line_points = self._get_line_points(num_points_per_link=100)
|
|
||||||
|
|
||||||
# all points that are before the hole in x
|
|
||||||
r, c = np.where(line_points[:, :, 0] < (self._tmp_x - self._tmp_width / 2))
|
|
||||||
|
|
||||||
# check if any of those points are below surface
|
|
||||||
nr_line_points_below_surface_before_hole = np.sum(line_points[r, c, 1] < 0)
|
|
||||||
|
|
||||||
if nr_line_points_below_surface_before_hole > 0:
|
|
||||||
return True
|
|
||||||
|
|
||||||
# all points that are after the hole in x
|
|
||||||
r, c = np.where(line_points[:, :, 0] > (self._tmp_x + self._tmp_width / 2))
|
|
||||||
|
|
||||||
# check if any of those points are below surface
|
|
||||||
nr_line_points_below_surface_after_hole = np.sum(line_points[r, c, 1] < 0)
|
|
||||||
|
|
||||||
if nr_line_points_below_surface_after_hole > 0:
|
|
||||||
return True
|
|
||||||
|
|
||||||
# all points that are above the hole
|
|
||||||
r, c = np.where((line_points[:, :, 0] > (self._tmp_x - self._tmp_width / 2)) & (
|
|
||||||
line_points[:, :, 0] < (self._tmp_x + self._tmp_width / 2)))
|
|
||||||
|
|
||||||
# check if any of those points are below surface
|
|
||||||
nr_line_points_below_surface_in_hole = np.sum(line_points[r, c, 1] < -self._tmp_depth)
|
|
||||||
|
|
||||||
if nr_line_points_below_surface_in_hole > 0:
|
|
||||||
return True
|
|
||||||
|
|
||||||
return False
|
|
||||||
|
|
||||||
def render(self, mode='human'):
|
|
||||||
if self.fig is None:
|
|
||||||
# Create base figure once on the beginning. Afterwards only update
|
|
||||||
plt.ion()
|
|
||||||
self.fig = plt.figure()
|
|
||||||
ax = self.fig.add_subplot(1, 1, 1)
|
|
||||||
|
|
||||||
# limits
|
|
||||||
lim = np.sum(self.link_lengths) + 0.5
|
|
||||||
ax.set_xlim([-lim, lim])
|
|
||||||
ax.set_ylim([-1.1, lim])
|
|
||||||
|
|
||||||
self.line, = ax.plot(self._joints[:, 0], self._joints[:, 1], 'ro-', markerfacecolor='k')
|
|
||||||
self._set_patches()
|
|
||||||
self.fig.show()
|
|
||||||
|
|
||||||
self.fig.gca().set_title(
|
|
||||||
f"Iteration: {self._steps}, distance: {np.linalg.norm(self.end_effector - self._goal) ** 2}")
|
|
||||||
|
|
||||||
if mode == "human":
|
|
||||||
|
|
||||||
# arm
|
|
||||||
self.line.set_data(self._joints[:, 0], self._joints[:, 1])
|
|
||||||
|
|
||||||
self.fig.canvas.draw()
|
|
||||||
self.fig.canvas.flush_events()
|
|
||||||
|
|
||||||
elif mode == "partial":
|
|
||||||
if self._steps % 20 == 0 or self._steps in [1, 199] or self._is_collided:
|
|
||||||
# Arm
|
|
||||||
plt.plot(self._joints[:, 0], self._joints[:, 1], 'ro-', markerfacecolor='k',
|
|
||||||
alpha=self._steps / 200)
|
|
||||||
|
|
||||||
def _set_patches(self):
|
|
||||||
if self.fig is not None:
|
|
||||||
# self.fig.gca().patches = []
|
|
||||||
left_block = patches.Rectangle((-self.n_links, -self._tmp_depth),
|
|
||||||
self.n_links + self._tmp_x - self._tmp_width / 2,
|
|
||||||
self._tmp_depth,
|
|
||||||
fill=True, edgecolor='k', facecolor='k')
|
|
||||||
right_block = patches.Rectangle((self._tmp_x + self._tmp_width / 2, -self._tmp_depth),
|
|
||||||
self.n_links - self._tmp_x + self._tmp_width / 2,
|
|
||||||
self._tmp_depth,
|
|
||||||
fill=True, edgecolor='k', facecolor='k')
|
|
||||||
hole_floor = patches.Rectangle((self._tmp_x - self._tmp_width / 2, -self._tmp_depth),
|
|
||||||
self._tmp_width,
|
|
||||||
1 - self._tmp_depth,
|
|
||||||
fill=True, edgecolor='k', facecolor='k')
|
|
||||||
|
|
||||||
# Add the patch to the Axes
|
|
||||||
self.fig.gca().add_patch(left_block)
|
|
||||||
self.fig.gca().add_patch(right_block)
|
|
||||||
self.fig.gca().add_patch(hole_floor)
|
|
||||||
|
|
||||||
|
|
||||||
if __name__ == "__main__":
|
|
||||||
import time
|
|
||||||
env = HoleReacherEnv(5)
|
|
||||||
env.reset()
|
|
||||||
|
|
||||||
for i in range(10000):
|
|
||||||
ac = env.action_space.sample()
|
|
||||||
obs, rew, done, info = env.step(ac)
|
|
||||||
env.render()
|
|
||||||
if done:
|
|
||||||
env.reset()
|
|
||||||
@ -1,60 +0,0 @@
|
|||||||
import numpy as np
|
|
||||||
|
|
||||||
|
|
||||||
class HolereacherReward:
|
|
||||||
def __init__(self, allow_self_collision, allow_wall_collision, collision_penalty):
|
|
||||||
self.collision_penalty = collision_penalty
|
|
||||||
|
|
||||||
# collision
|
|
||||||
self.allow_self_collision = allow_self_collision
|
|
||||||
self.allow_wall_collision = allow_wall_collision
|
|
||||||
self.collision_penalty = collision_penalty
|
|
||||||
self._is_collided = False
|
|
||||||
|
|
||||||
self.reward_factors = np.array((-1, -1e-4, -1e-6, -collision_penalty, 0))
|
|
||||||
|
|
||||||
def reset(self):
|
|
||||||
self._is_collided = False
|
|
||||||
self.collision_dist = 0
|
|
||||||
|
|
||||||
def get_reward(self, env):
|
|
||||||
dist_cost = 0
|
|
||||||
collision_cost = 0
|
|
||||||
time_cost = 0
|
|
||||||
success = False
|
|
||||||
|
|
||||||
self_collision = False
|
|
||||||
wall_collision = False
|
|
||||||
|
|
||||||
if not self._is_collided:
|
|
||||||
if not self.allow_self_collision:
|
|
||||||
self_collision = env._check_self_collision()
|
|
||||||
|
|
||||||
if not self.allow_wall_collision:
|
|
||||||
wall_collision = env.check_wall_collision()
|
|
||||||
|
|
||||||
self._is_collided = self_collision or wall_collision
|
|
||||||
|
|
||||||
self.collision_dist = np.linalg.norm(env.end_effector - env._goal)
|
|
||||||
|
|
||||||
if env._steps == 199: # or self._is_collided:
|
|
||||||
# return reward only in last time step
|
|
||||||
# Episode also terminates when colliding, hence return reward
|
|
||||||
dist = np.linalg.norm(env.end_effector - env._goal)
|
|
||||||
|
|
||||||
success = dist < 0.005 and not self._is_collided
|
|
||||||
dist_cost = dist ** 2
|
|
||||||
collision_cost = self._is_collided * self.collision_dist ** 2
|
|
||||||
time_cost = 199 - env._steps
|
|
||||||
|
|
||||||
info = {"is_success": success,
|
|
||||||
"is_collided": self._is_collided,
|
|
||||||
"end_effector": np.copy(env.end_effector)}
|
|
||||||
|
|
||||||
vel_cost = np.sum(env._angle_velocity ** 2)
|
|
||||||
acc_cost = np.sum(env._acc ** 2)
|
|
||||||
|
|
||||||
reward_features = np.array((dist_cost, vel_cost, acc_cost, collision_cost, time_cost))
|
|
||||||
reward = np.dot(reward_features, self.reward_factors)
|
|
||||||
|
|
||||||
return reward, info
|
|
||||||
@ -1,53 +0,0 @@
|
|||||||
import numpy as np
|
|
||||||
|
|
||||||
|
|
||||||
class HolereacherReward:
|
|
||||||
def __init__(self, allow_self_collision, allow_wall_collision, collision_penalty):
|
|
||||||
self.collision_penalty = collision_penalty
|
|
||||||
|
|
||||||
# collision
|
|
||||||
self.allow_self_collision = allow_self_collision
|
|
||||||
self.allow_wall_collision = allow_wall_collision
|
|
||||||
self.collision_penalty = collision_penalty
|
|
||||||
self._is_collided = False
|
|
||||||
|
|
||||||
self.reward_factors = np.array((-1, -5e-8, -collision_penalty))
|
|
||||||
|
|
||||||
def reset(self):
|
|
||||||
self._is_collided = False
|
|
||||||
|
|
||||||
def get_reward(self, env):
|
|
||||||
dist_cost = 0
|
|
||||||
collision_cost = 0
|
|
||||||
success = False
|
|
||||||
|
|
||||||
self_collision = False
|
|
||||||
wall_collision = False
|
|
||||||
|
|
||||||
if not self.allow_self_collision:
|
|
||||||
self_collision = env._check_self_collision()
|
|
||||||
|
|
||||||
if not self.allow_wall_collision:
|
|
||||||
wall_collision = env.check_wall_collision()
|
|
||||||
|
|
||||||
self._is_collided = self_collision or wall_collision
|
|
||||||
|
|
||||||
if env._steps == 199 or self._is_collided:
|
|
||||||
# return reward only in last time step
|
|
||||||
# Episode also terminates when colliding, hence return reward
|
|
||||||
dist = np.linalg.norm(env.end_effector - env._goal)
|
|
||||||
dist_cost = dist ** 2
|
|
||||||
collision_cost = int(self._is_collided)
|
|
||||||
|
|
||||||
success = dist < 0.005 and not self._is_collided
|
|
||||||
|
|
||||||
info = {"is_success": success,
|
|
||||||
"is_collided": self._is_collided,
|
|
||||||
"end_effector": np.copy(env.end_effector)}
|
|
||||||
|
|
||||||
acc_cost = np.sum(env._acc ** 2)
|
|
||||||
|
|
||||||
reward_features = np.array((dist_cost, acc_cost, collision_cost))
|
|
||||||
reward = np.dot(reward_features, self.reward_factors)
|
|
||||||
|
|
||||||
return reward, info
|
|
||||||
@ -1,43 +0,0 @@
|
|||||||
from typing import Tuple, Union
|
|
||||||
|
|
||||||
import numpy as np
|
|
||||||
|
|
||||||
from mp_env_api import MPEnvWrapper
|
|
||||||
|
|
||||||
|
|
||||||
class MPWrapper(MPEnvWrapper):
|
|
||||||
@property
|
|
||||||
def active_obs(self):
|
|
||||||
return np.hstack([
|
|
||||||
[self.env.random_start] * self.env.n_links, # cos
|
|
||||||
[self.env.random_start] * self.env.n_links, # sin
|
|
||||||
[self.env.random_start] * self.env.n_links, # velocity
|
|
||||||
[self.env.initial_width is None], # hole width
|
|
||||||
# [self.env.hole_depth is None], # hole depth
|
|
||||||
[True] * 2, # x-y coordinates of target distance
|
|
||||||
[False] # env steps
|
|
||||||
])
|
|
||||||
|
|
||||||
# @property
|
|
||||||
# def current_pos(self) -> Union[float, int, np.ndarray, Tuple]:
|
|
||||||
# return self._joint_angles.copy()
|
|
||||||
#
|
|
||||||
# @property
|
|
||||||
# def current_vel(self) -> Union[float, int, np.ndarray, Tuple]:
|
|
||||||
# return self._angle_velocity.copy()
|
|
||||||
|
|
||||||
@property
|
|
||||||
def current_pos(self) -> Union[float, int, np.ndarray, Tuple]:
|
|
||||||
return self.env.current_pos
|
|
||||||
|
|
||||||
@property
|
|
||||||
def current_vel(self) -> Union[float, int, np.ndarray, Tuple]:
|
|
||||||
return self.env.current_vel
|
|
||||||
|
|
||||||
@property
|
|
||||||
def goal_pos(self) -> Union[float, int, np.ndarray, Tuple]:
|
|
||||||
raise ValueError("Goal position is not available and has to be learnt based on the environment.")
|
|
||||||
|
|
||||||
@property
|
|
||||||
def dt(self) -> Union[float, int]:
|
|
||||||
return self.env.dt
|
|
||||||
@ -1 +0,0 @@
|
|||||||
from .mp_wrapper import MPWrapper
|
|
||||||
@ -1,33 +0,0 @@
|
|||||||
from typing import Tuple, Union
|
|
||||||
|
|
||||||
import numpy as np
|
|
||||||
|
|
||||||
from mp_env_api import MPEnvWrapper
|
|
||||||
|
|
||||||
|
|
||||||
class MPWrapper(MPEnvWrapper):
|
|
||||||
@property
|
|
||||||
def active_obs(self):
|
|
||||||
return np.hstack([
|
|
||||||
[self.env.random_start] * self.env.n_links, # cos
|
|
||||||
[self.env.random_start] * self.env.n_links, # sin
|
|
||||||
[self.env.random_start] * self.env.n_links, # velocity
|
|
||||||
[True] * 2, # x-y coordinates of target distance
|
|
||||||
[False] # env steps
|
|
||||||
])
|
|
||||||
|
|
||||||
@property
|
|
||||||
def current_pos(self) -> Union[float, int, np.ndarray, Tuple]:
|
|
||||||
return self.env.current_pos
|
|
||||||
|
|
||||||
@property
|
|
||||||
def current_vel(self) -> Union[float, int, np.ndarray, Tuple]:
|
|
||||||
return self.env.current_vel
|
|
||||||
|
|
||||||
@property
|
|
||||||
def goal_pos(self) -> Union[float, int, np.ndarray, Tuple]:
|
|
||||||
raise ValueError("Goal position is not available and has to be learnt based on the environment.")
|
|
||||||
|
|
||||||
@property
|
|
||||||
def dt(self) -> Union[float, int]:
|
|
||||||
return self.env.dt
|
|
||||||
@ -1,139 +0,0 @@
|
|||||||
from typing import Iterable, Union
|
|
||||||
|
|
||||||
import matplotlib.pyplot as plt
|
|
||||||
import numpy as np
|
|
||||||
from gym import spaces
|
|
||||||
|
|
||||||
from alr_envs.alr.classic_control.base_reacher.base_reacher_torque import BaseReacherTorqueEnv
|
|
||||||
|
|
||||||
|
|
||||||
class SimpleReacherEnv(BaseReacherTorqueEnv):
|
|
||||||
"""
|
|
||||||
Simple Reaching Task without any physics simulation.
|
|
||||||
Returns no reward until 150 time steps. This allows the agent to explore the space, but requires precise actions
|
|
||||||
towards the end of the trajectory.
|
|
||||||
"""
|
|
||||||
|
|
||||||
def __init__(self, n_links: int, target: Union[None, Iterable] = None, random_start: bool = True,
|
|
||||||
allow_self_collision: bool = False,):
|
|
||||||
super().__init__(n_links, random_start, allow_self_collision)
|
|
||||||
|
|
||||||
# provided initial parameters
|
|
||||||
self.inital_target = target
|
|
||||||
|
|
||||||
# temp container for current env state
|
|
||||||
self._goal = None
|
|
||||||
|
|
||||||
self._start_pos = np.zeros(self.n_links)
|
|
||||||
|
|
||||||
self.steps_before_reward = 199
|
|
||||||
|
|
||||||
state_bound = np.hstack([
|
|
||||||
[np.pi] * self.n_links, # cos
|
|
||||||
[np.pi] * self.n_links, # sin
|
|
||||||
[np.inf] * self.n_links, # velocity
|
|
||||||
[np.inf] * 2, # x-y coordinates of target distance
|
|
||||||
[np.inf] # env steps, because reward start after n steps TODO: Maybe
|
|
||||||
])
|
|
||||||
self.observation_space = spaces.Box(low=-state_bound, high=state_bound, shape=state_bound.shape)
|
|
||||||
|
|
||||||
# @property
|
|
||||||
# def start_pos(self):
|
|
||||||
# return self._start_pos
|
|
||||||
|
|
||||||
def reset(self):
|
|
||||||
self._generate_goal()
|
|
||||||
|
|
||||||
return super().reset()
|
|
||||||
|
|
||||||
def _get_reward(self, action: np.ndarray):
|
|
||||||
diff = self.end_effector - self._goal
|
|
||||||
reward_dist = 0
|
|
||||||
|
|
||||||
if not self.allow_self_collision:
|
|
||||||
self._is_collided = self._check_self_collision()
|
|
||||||
|
|
||||||
if self._steps >= self.steps_before_reward:
|
|
||||||
reward_dist -= np.linalg.norm(diff)
|
|
||||||
# reward_dist = np.exp(-0.1 * diff ** 2).mean()
|
|
||||||
# reward_dist = - (diff ** 2).mean()
|
|
||||||
|
|
||||||
reward_ctrl = (action ** 2).sum()
|
|
||||||
reward = reward_dist - reward_ctrl
|
|
||||||
return reward, dict(reward_dist=reward_dist, reward_ctrl=reward_ctrl)
|
|
||||||
|
|
||||||
def _terminate(self, info):
|
|
||||||
return False
|
|
||||||
|
|
||||||
def _get_obs(self):
|
|
||||||
theta = self._joint_angles
|
|
||||||
return np.hstack([
|
|
||||||
np.cos(theta),
|
|
||||||
np.sin(theta),
|
|
||||||
self._angle_velocity,
|
|
||||||
self.end_effector - self._goal,
|
|
||||||
self._steps
|
|
||||||
]).astype(np.float32)
|
|
||||||
|
|
||||||
def _generate_goal(self):
|
|
||||||
|
|
||||||
if self.inital_target is None:
|
|
||||||
|
|
||||||
total_length = np.sum(self.link_lengths)
|
|
||||||
goal = np.array([total_length, total_length])
|
|
||||||
while np.linalg.norm(goal) >= total_length:
|
|
||||||
goal = self.np_random.uniform(low=-total_length, high=total_length, size=2)
|
|
||||||
else:
|
|
||||||
goal = np.copy(self.inital_target)
|
|
||||||
|
|
||||||
self._goal = goal
|
|
||||||
|
|
||||||
def _check_collisions(self) -> bool:
|
|
||||||
return self._check_self_collision()
|
|
||||||
|
|
||||||
def render(self, mode='human'): # pragma: no cover
|
|
||||||
if self.fig is None:
|
|
||||||
# Create base figure once on the beginning. Afterwards only update
|
|
||||||
plt.ion()
|
|
||||||
self.fig = plt.figure()
|
|
||||||
ax = self.fig.add_subplot(1, 1, 1)
|
|
||||||
|
|
||||||
# limits
|
|
||||||
lim = np.sum(self.link_lengths) + 0.5
|
|
||||||
ax.set_xlim([-lim, lim])
|
|
||||||
ax.set_ylim([-lim, lim])
|
|
||||||
|
|
||||||
self.line, = ax.plot(self._joints[:, 0], self._joints[:, 1], 'ro-', markerfacecolor='k')
|
|
||||||
goal_pos = self._goal.T
|
|
||||||
self.goal_point, = ax.plot(goal_pos[0], goal_pos[1], 'gx')
|
|
||||||
self.goal_dist, = ax.plot([self.end_effector[0], goal_pos[0]], [self.end_effector[1], goal_pos[1]], 'g--')
|
|
||||||
|
|
||||||
self.fig.show()
|
|
||||||
|
|
||||||
self.fig.gca().set_title(f"Iteration: {self._steps}, distance: {self.end_effector - self._goal}")
|
|
||||||
|
|
||||||
# goal
|
|
||||||
goal_pos = self._goal.T
|
|
||||||
if self._steps == 1:
|
|
||||||
self.goal_point.set_data(goal_pos[0], goal_pos[1])
|
|
||||||
|
|
||||||
# arm
|
|
||||||
self.line.set_data(self._joints[:, 0], self._joints[:, 1])
|
|
||||||
|
|
||||||
# distance between end effector and goal
|
|
||||||
self.goal_dist.set_data([self.end_effector[0], goal_pos[0]], [self.end_effector[1], goal_pos[1]])
|
|
||||||
|
|
||||||
self.fig.canvas.draw()
|
|
||||||
self.fig.canvas.flush_events()
|
|
||||||
|
|
||||||
|
|
||||||
if __name__ == "__main__":
|
|
||||||
env = SimpleReacherEnv(5)
|
|
||||||
env.reset()
|
|
||||||
for i in range(200):
|
|
||||||
ac = env.action_space.sample()
|
|
||||||
obs, rew, done, info = env.step(ac)
|
|
||||||
|
|
||||||
env.render()
|
|
||||||
if done:
|
|
||||||
break
|
|
||||||
@ -1,21 +0,0 @@
|
|||||||
import numpy as np
|
|
||||||
|
|
||||||
|
|
||||||
def ccw(A, B, C):
|
|
||||||
return (C[1] - A[1]) * (B[0] - A[0]) - (B[1] - A[1]) * (C[0] - A[0]) > 1e-12
|
|
||||||
|
|
||||||
|
|
||||||
def intersect(A, B, C, D):
|
|
||||||
"""
|
|
||||||
Checks whether line segments AB and CD intersect
|
|
||||||
"""
|
|
||||||
return ccw(A, C, D) != ccw(B, C, D) and ccw(A, B, C) != ccw(A, B, D)
|
|
||||||
|
|
||||||
|
|
||||||
def check_self_collision(line_points):
|
|
||||||
"""Checks whether line segments intersect"""
|
|
||||||
for i, line1 in enumerate(line_points):
|
|
||||||
for line2 in line_points[i + 2:, :, :]:
|
|
||||||
if intersect(line1[0], line1[-1], line2[0], line2[-1]):
|
|
||||||
return True
|
|
||||||
return False
|
|
||||||
@ -1 +0,0 @@
|
|||||||
from .mp_wrapper import MPWrapper
|
|
||||||
@ -1,34 +0,0 @@
|
|||||||
from typing import Tuple, Union
|
|
||||||
|
|
||||||
import numpy as np
|
|
||||||
|
|
||||||
from mp_env_api import MPEnvWrapper
|
|
||||||
|
|
||||||
|
|
||||||
class MPWrapper(MPEnvWrapper):
|
|
||||||
@property
|
|
||||||
def active_obs(self):
|
|
||||||
return np.hstack([
|
|
||||||
[self.env.random_start] * self.env.n_links, # cos
|
|
||||||
[self.env.random_start] * self.env.n_links, # sin
|
|
||||||
[self.env.random_start] * self.env.n_links, # velocity
|
|
||||||
[self.env.initial_via_target is None] * 2, # x-y coordinates of via point distance
|
|
||||||
[True] * 2, # x-y coordinates of target distance
|
|
||||||
[False] # env steps
|
|
||||||
])
|
|
||||||
|
|
||||||
@property
|
|
||||||
def current_pos(self) -> Union[float, int, np.ndarray, Tuple]:
|
|
||||||
return self.env.current_pos
|
|
||||||
|
|
||||||
@property
|
|
||||||
def current_vel(self) -> Union[float, int, np.ndarray, Tuple]:
|
|
||||||
return self.env.current_vel
|
|
||||||
|
|
||||||
@property
|
|
||||||
def goal_pos(self) -> Union[float, int, np.ndarray, Tuple]:
|
|
||||||
raise ValueError("Goal position is not available and has to be learnt based on the environment.")
|
|
||||||
|
|
||||||
@property
|
|
||||||
def dt(self) -> Union[float, int]:
|
|
||||||
return self.env.dt
|
|
||||||
@ -1,196 +0,0 @@
|
|||||||
from typing import Iterable, Union
|
|
||||||
|
|
||||||
import gym
|
|
||||||
import matplotlib.pyplot as plt
|
|
||||||
import numpy as np
|
|
||||||
from gym.utils import seeding
|
|
||||||
|
|
||||||
from alr_envs.alr.classic_control.base_reacher.base_reacher_direct import BaseReacherDirectEnv
|
|
||||||
|
|
||||||
|
|
||||||
class ViaPointReacherEnv(BaseReacherDirectEnv):
|
|
||||||
|
|
||||||
def __init__(self, n_links, random_start: bool = False, via_target: Union[None, Iterable] = None,
|
|
||||||
target: Union[None, Iterable] = None, allow_self_collision=False, collision_penalty=1000):
|
|
||||||
|
|
||||||
super().__init__(n_links, random_start, allow_self_collision)
|
|
||||||
|
|
||||||
# provided initial parameters
|
|
||||||
self.intitial_target = target # provided target value
|
|
||||||
self.initial_via_target = via_target # provided via point target value
|
|
||||||
|
|
||||||
# temp container for current env state
|
|
||||||
self._via_point = np.ones(2)
|
|
||||||
self._goal = np.array((n_links, 0))
|
|
||||||
|
|
||||||
# collision
|
|
||||||
self.collision_penalty = collision_penalty
|
|
||||||
|
|
||||||
state_bound = np.hstack([
|
|
||||||
[np.pi] * self.n_links, # cos
|
|
||||||
[np.pi] * self.n_links, # sin
|
|
||||||
[np.inf] * self.n_links, # velocity
|
|
||||||
[np.inf] * 2, # x-y coordinates of via point distance
|
|
||||||
[np.inf] * 2, # x-y coordinates of target distance
|
|
||||||
[np.inf] # env steps, because reward start after n steps
|
|
||||||
])
|
|
||||||
self.observation_space = gym.spaces.Box(low=-state_bound, high=state_bound, shape=state_bound.shape)
|
|
||||||
|
|
||||||
# @property
|
|
||||||
# def start_pos(self):
|
|
||||||
# return self._start_pos
|
|
||||||
|
|
||||||
def reset(self):
|
|
||||||
self._generate_goal()
|
|
||||||
return super().reset()
|
|
||||||
|
|
||||||
def _generate_goal(self):
|
|
||||||
# TODO: Maybe improve this later, this can yield quite a lot of invalid settings
|
|
||||||
|
|
||||||
total_length = np.sum(self.link_lengths)
|
|
||||||
|
|
||||||
# rejection sampled point in inner circle with 0.5*Radius
|
|
||||||
if self.initial_via_target is None:
|
|
||||||
via_target = np.array([total_length, total_length])
|
|
||||||
while np.linalg.norm(via_target) >= 0.5 * total_length:
|
|
||||||
via_target = self.np_random.uniform(low=-0.5 * total_length, high=0.5 * total_length, size=2)
|
|
||||||
else:
|
|
||||||
via_target = np.copy(self.initial_via_target)
|
|
||||||
|
|
||||||
# rejection sampled point in outer circle
|
|
||||||
if self.intitial_target is None:
|
|
||||||
goal = np.array([total_length, total_length])
|
|
||||||
while np.linalg.norm(goal) >= total_length or np.linalg.norm(goal) <= 0.5 * total_length:
|
|
||||||
goal = self.np_random.uniform(low=-total_length, high=total_length, size=2)
|
|
||||||
else:
|
|
||||||
goal = np.copy(self.intitial_target)
|
|
||||||
|
|
||||||
self._via_point = via_target
|
|
||||||
self._goal = goal
|
|
||||||
|
|
||||||
def _get_reward(self, acc):
|
|
||||||
success = False
|
|
||||||
reward = -np.inf
|
|
||||||
|
|
||||||
if not self.allow_self_collision:
|
|
||||||
self._is_collided = self._check_self_collision()
|
|
||||||
|
|
||||||
if not self._is_collided:
|
|
||||||
dist = np.inf
|
|
||||||
# return intermediate reward for via point
|
|
||||||
if self._steps == 100:
|
|
||||||
dist = np.linalg.norm(self.end_effector - self._via_point)
|
|
||||||
# return reward in last time step for goal
|
|
||||||
elif self._steps == 199:
|
|
||||||
dist = np.linalg.norm(self.end_effector - self._goal)
|
|
||||||
|
|
||||||
success = dist < 0.005
|
|
||||||
else:
|
|
||||||
# Episode terminates when colliding, hence return reward
|
|
||||||
dist = np.linalg.norm(self.end_effector - self._goal)
|
|
||||||
reward = -self.collision_penalty
|
|
||||||
|
|
||||||
reward -= dist ** 2
|
|
||||||
reward -= 5e-8 * np.sum(acc ** 2)
|
|
||||||
info = {"is_success": success,
|
|
||||||
"is_collided": self._is_collided,
|
|
||||||
"end_effector": np.copy(self.end_effector)}
|
|
||||||
|
|
||||||
return reward, info
|
|
||||||
|
|
||||||
def _terminate(self, info):
|
|
||||||
return info["is_collided"]
|
|
||||||
|
|
||||||
def _get_obs(self):
|
|
||||||
theta = self._joint_angles
|
|
||||||
return np.hstack([
|
|
||||||
np.cos(theta),
|
|
||||||
np.sin(theta),
|
|
||||||
self._angle_velocity,
|
|
||||||
self.end_effector - self._via_point,
|
|
||||||
self.end_effector - self._goal,
|
|
||||||
self._steps
|
|
||||||
]).astype(np.float32)
|
|
||||||
|
|
||||||
def _check_collisions(self) -> bool:
|
|
||||||
return self._check_self_collision()
|
|
||||||
|
|
||||||
def render(self, mode='human'):
|
|
||||||
goal_pos = self._goal.T
|
|
||||||
via_pos = self._via_point.T
|
|
||||||
|
|
||||||
if self.fig is None:
|
|
||||||
# Create base figure once on the beginning. Afterwards only update
|
|
||||||
plt.ion()
|
|
||||||
self.fig = plt.figure()
|
|
||||||
ax = self.fig.add_subplot(1, 1, 1)
|
|
||||||
|
|
||||||
# limits
|
|
||||||
lim = np.sum(self.link_lengths) + 0.5
|
|
||||||
ax.set_xlim([-lim, lim])
|
|
||||||
ax.set_ylim([-lim, lim])
|
|
||||||
|
|
||||||
self.line, = ax.plot(self._joints[:, 0], self._joints[:, 1], 'ro-', markerfacecolor='k')
|
|
||||||
self.goal_point_plot, = ax.plot(goal_pos[0], goal_pos[1], 'go')
|
|
||||||
self.via_point_plot, = ax.plot(via_pos[0], via_pos[1], 'gx')
|
|
||||||
|
|
||||||
self.fig.show()
|
|
||||||
|
|
||||||
self.fig.gca().set_title(f"Iteration: {self._steps}, distance: {self.end_effector - self._goal}")
|
|
||||||
|
|
||||||
if mode == "human":
|
|
||||||
# goal
|
|
||||||
if self._steps == 1:
|
|
||||||
self.goal_point_plot.set_data(goal_pos[0], goal_pos[1])
|
|
||||||
self.via_point_plot.set_data(via_pos[0], goal_pos[1])
|
|
||||||
|
|
||||||
# arm
|
|
||||||
self.line.set_data(self._joints[:, 0], self._joints[:, 1])
|
|
||||||
|
|
||||||
self.fig.canvas.draw()
|
|
||||||
self.fig.canvas.flush_events()
|
|
||||||
|
|
||||||
elif mode == "partial":
|
|
||||||
if self._steps == 1:
|
|
||||||
# fig, ax = plt.subplots()
|
|
||||||
# Add the patch to the Axes
|
|
||||||
[plt.gca().add_patch(rect) for rect in self.patches]
|
|
||||||
# plt.pause(0.01)
|
|
||||||
|
|
||||||
if self._steps % 20 == 0 or self._steps in [1, 199] or self._is_collided:
|
|
||||||
# Arm
|
|
||||||
plt.plot(self._joints[:, 0], self._joints[:, 1], 'ro-', markerfacecolor='k', alpha=self._steps / 200)
|
|
||||||
# ax.plot(line_points_in_taskspace[:, 0, 0],
|
|
||||||
# line_points_in_taskspace[:, 0, 1],
|
|
||||||
# line_points_in_taskspace[:, -1, 0],
|
|
||||||
# line_points_in_taskspace[:, -1, 1], marker='o', color='k', alpha=t / 200)
|
|
||||||
|
|
||||||
lim = np.sum(self.link_lengths) + 0.5
|
|
||||||
plt.xlim([-lim, lim])
|
|
||||||
plt.ylim([-1.1, lim])
|
|
||||||
plt.pause(0.01)
|
|
||||||
|
|
||||||
elif mode == "final":
|
|
||||||
if self._steps == 199 or self._is_collided:
|
|
||||||
# fig, ax = plt.subplots()
|
|
||||||
|
|
||||||
# Add the patch to the Axes
|
|
||||||
[plt.gca().add_patch(rect) for rect in self.patches]
|
|
||||||
|
|
||||||
plt.xlim(-self.n_links, self.n_links), plt.ylim(-1, self.n_links)
|
|
||||||
# Arm
|
|
||||||
plt.plot(self._joints[:, 0], self._joints[:, 1], 'ro-', markerfacecolor='k')
|
|
||||||
|
|
||||||
plt.pause(0.01)
|
|
||||||
|
|
||||||
if __name__ == "__main__":
|
|
||||||
import time
|
|
||||||
env = ViaPointReacherEnv(5)
|
|
||||||
env.reset()
|
|
||||||
|
|
||||||
for i in range(10000):
|
|
||||||
ac = env.action_space.sample()
|
|
||||||
obs, rew, done, info = env.step(ac)
|
|
||||||
env.render()
|
|
||||||
if done:
|
|
||||||
env.reset()
|
|
||||||
@ -1,15 +0,0 @@
|
|||||||
# Custom Mujoco tasks
|
|
||||||
|
|
||||||
## Step-based Environments
|
|
||||||
|Name| Description|Horizon|Action Dimension|Observation Dimension
|
|
||||||
|---|---|---|---|---|
|
|
||||||
|`ALRReacher-v0`|Modified (5 links) Mujoco gym's `Reacher-v2` (2 links)| 200 | 5 | 21
|
|
||||||
|`ALRReacherSparse-v0`|Same as `ALRReacher-v0`, but the distance penalty is only provided in the last time step.| 200 | 5 | 21
|
|
||||||
|`ALRReacherSparseBalanced-v0`|Same as `ALRReacherSparse-v0`, but the end-effector has to remain upright.| 200 | 5 | 21
|
|
||||||
|`ALRLongReacher-v0`|Modified (7 links) Mujoco gym's `Reacher-v2` (2 links)| 200 | 7 | 27
|
|
||||||
|`ALRLongReacherSparse-v0`|Same as `ALRLongReacher-v0`, but the distance penalty is only provided in the last time step.| 200 | 7 | 27
|
|
||||||
|`ALRLongReacherSparseBalanced-v0`|Same as `ALRLongReacherSparse-v0`, but the end-effector has to remain upright.| 200 | 7 | 27
|
|
||||||
|`ALRBallInACupSimple-v0`| Ball-in-a-cup task where a robot needs to catch a ball attached to a cup at its end-effector. | 4000 | 3 | wip
|
|
||||||
|`ALRBallInACup-v0`| Ball-in-a-cup task where a robot needs to catch a ball attached to a cup at its end-effector | 4000 | 7 | wip
|
|
||||||
|`ALRBallInACupGoal-v0`| Similar to `ALRBallInACupSimple-v0` but the ball needs to be caught at a specified goal position | 4000 | 7 | wip
|
|
||||||
|
|
||||||
@ -1,6 +0,0 @@
|
|||||||
from .reacher.alr_reacher import ALRReacherEnv
|
|
||||||
from .reacher.balancing import BalancingEnv
|
|
||||||
from .ball_in_a_cup.ball_in_a_cup import ALRBallInACupEnv
|
|
||||||
from .ball_in_a_cup.biac_pd import ALRBallInACupPDEnv
|
|
||||||
from .table_tennis.tt_gym import TTEnvGym
|
|
||||||
from .beerpong.beerpong import ALRBeerBongEnv
|
|
||||||
@ -1,21 +0,0 @@
|
|||||||
class AlrReward:
|
|
||||||
"""
|
|
||||||
A base class for non-Markovian reward functions which may need trajectory information to calculate an episodic
|
|
||||||
reward. Call the methods in reset() and step() of the environment.
|
|
||||||
"""
|
|
||||||
|
|
||||||
# methods to override:
|
|
||||||
# ----------------------------
|
|
||||||
def reset(self, *args, **kwargs):
|
|
||||||
"""
|
|
||||||
Reset the reward function, empty state buffers before an episode, set contexts that influence reward, etc.
|
|
||||||
"""
|
|
||||||
raise NotImplementedError
|
|
||||||
|
|
||||||
def compute_reward(self, *args, **kwargs):
|
|
||||||
"""
|
|
||||||
|
|
||||||
Returns: Useful things to return are reward values, success flags or crash flags
|
|
||||||
|
|
||||||
"""
|
|
||||||
raise NotImplementedError
|
|
||||||
@ -1 +0,0 @@
|
|||||||
|
|
||||||
@ -1,361 +0,0 @@
|
|||||||
<mujoco model="wam(v1.31)">
|
|
||||||
<compiler angle="radian" meshdir="../../meshes/wam/" />
|
|
||||||
<option timestep="0.0005" integrator="Euler" />
|
|
||||||
<size njmax="500" nconmax="100" />
|
|
||||||
<default class="main">
|
|
||||||
<joint limited="true" frictionloss="0.001" />
|
|
||||||
<default class="viz">
|
|
||||||
<geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.7 0.7 0.7 1" />
|
|
||||||
</default>
|
|
||||||
<default class="col">
|
|
||||||
<geom type="mesh" contype="0" rgba="0.5 0.6 0.7 1" />
|
|
||||||
</default>
|
|
||||||
</default>
|
|
||||||
<asset>
|
|
||||||
<texture type="2d" name="groundplane" builtin="checker" mark="edge" rgb1="0.25 0.26 0.25" rgb2="0.22 0.22 0.22" markrgb="0.3 0.3 0.3" width="100" height="100" />
|
|
||||||
<material name="MatGnd" texture="groundplane" texrepeat="5 5" specular="1" shininess="0.3" reflectance="1e-05" />
|
|
||||||
<mesh name="base_link_fine" file="base_link_fine.stl" />
|
|
||||||
<mesh name="base_link_convex" file="base_link_convex.stl" />
|
|
||||||
<mesh name="shoulder_link_fine" file="shoulder_link_fine.stl" />
|
|
||||||
<mesh name="shoulder_link_convex_decomposition_p1" file="shoulder_link_convex_decomposition_p1.stl" />
|
|
||||||
<mesh name="shoulder_link_convex_decomposition_p2" file="shoulder_link_convex_decomposition_p2.stl" />
|
|
||||||
<mesh name="shoulder_link_convex_decomposition_p3" file="shoulder_link_convex_decomposition_p3.stl" />
|
|
||||||
<mesh name="shoulder_pitch_link_fine" file="shoulder_pitch_link_fine.stl" />
|
|
||||||
<mesh name="shoulder_pitch_link_convex" file="shoulder_pitch_link_convex.stl" />
|
|
||||||
<mesh name="upper_arm_link_fine" file="upper_arm_link_fine.stl" />
|
|
||||||
<mesh name="upper_arm_link_convex_decomposition_p1" file="upper_arm_link_convex_decomposition_p1.stl" />
|
|
||||||
<mesh name="upper_arm_link_convex_decomposition_p2" file="upper_arm_link_convex_decomposition_p2.stl" />
|
|
||||||
<mesh name="elbow_link_fine" file="elbow_link_fine.stl" />
|
|
||||||
<mesh name="elbow_link_convex" file="elbow_link_convex.stl" />
|
|
||||||
<mesh name="forearm_link_fine" file="forearm_link_fine.stl" />
|
|
||||||
<mesh name="forearm_link_convex_decomposition_p1" file="forearm_link_convex_decomposition_p1.stl" />
|
|
||||||
<mesh name="forearm_link_convex_decomposition_p2" file="forearm_link_convex_decomposition_p2.stl" />
|
|
||||||
<mesh name="wrist_yaw_link_fine" file="wrist_yaw_link_fine.stl" />
|
|
||||||
<mesh name="wrist_yaw_link_convex_decomposition_p1" file="wrist_yaw_link_convex_decomposition_p1.stl" />
|
|
||||||
<mesh name="wrist_yaw_link_convex_decomposition_p2" file="wrist_yaw_link_convex_decomposition_p2.stl" />
|
|
||||||
<mesh name="wrist_pitch_link_fine" file="wrist_pitch_link_fine.stl" />
|
|
||||||
<mesh name="wrist_pitch_link_convex_decomposition_p1" file="wrist_pitch_link_convex_decomposition_p1.stl" />
|
|
||||||
<mesh name="wrist_pitch_link_convex_decomposition_p2" file="wrist_pitch_link_convex_decomposition_p2.stl" />
|
|
||||||
<mesh name="wrist_pitch_link_convex_decomposition_p3" file="wrist_pitch_link_convex_decomposition_p3.stl" />
|
|
||||||
<mesh name="wrist_palm_link_fine" file="wrist_palm_link_fine.stl" />
|
|
||||||
<mesh name="wrist_palm_link_convex" file="wrist_palm_link_convex.stl" />
|
|
||||||
<mesh name="cup1" file="cup_split1.stl" scale="0.001 0.001 0.001" />
|
|
||||||
<mesh name="cup2" file="cup_split2.stl" scale="0.001 0.001 0.001" />
|
|
||||||
<mesh name="cup3" file="cup_split3.stl" scale="0.001 0.001 0.001" />
|
|
||||||
<mesh name="cup4" file="cup_split4.stl" scale="0.001 0.001 0.001" />
|
|
||||||
<mesh name="cup5" file="cup_split5.stl" scale="0.001 0.001 0.001" />
|
|
||||||
<mesh name="cup6" file="cup_split6.stl" scale="0.001 0.001 0.001" />
|
|
||||||
<mesh name="cup7" file="cup_split7.stl" scale="0.001 0.001 0.001" />
|
|
||||||
<mesh name="cup8" file="cup_split8.stl" scale="0.001 0.001 0.001" />
|
|
||||||
<mesh name="cup9" file="cup_split9.stl" scale="0.001 0.001 0.001" />
|
|
||||||
<mesh name="cup10" file="cup_split10.stl" scale="0.001 0.001 0.001" />
|
|
||||||
<mesh name="cup11" file="cup_split11.stl" scale="0.001 0.001 0.001" />
|
|
||||||
<mesh name="cup12" file="cup_split12.stl" scale="0.001 0.001 0.001" />
|
|
||||||
<mesh name="cup13" file="cup_split13.stl" scale="0.001 0.001 0.001" />
|
|
||||||
<mesh name="cup14" file="cup_split14.stl" scale="0.001 0.001 0.001" />
|
|
||||||
<mesh name="cup15" file="cup_split15.stl" scale="0.001 0.001 0.001" />
|
|
||||||
<mesh name="cup16" file="cup_split16.stl" scale="0.001 0.001 0.001" />
|
|
||||||
<mesh name="cup17" file="cup_split17.stl" scale="0.001 0.001 0.001" />
|
|
||||||
<mesh name="cup18" file="cup_split18.stl" scale="0.001 0.001 0.001" />
|
|
||||||
</asset>
|
|
||||||
|
|
||||||
<worldbody>
|
|
||||||
<geom name="ground" size="1.5 2 1" type="plane" material="MatGnd" />
|
|
||||||
<light pos="0.1 0.2 1.3" dir="-0.0758098 -0.32162 -0.985527" directional="true" cutoff="60" exponent="1" diffuse="1 1 1" specular="0.1 0.1 0.1" />
|
|
||||||
|
|
||||||
<body name="wam/base_link" pos="0 0 0.6">
|
|
||||||
<inertial pos="6.93764e-06 0.0542887 0.076438" quat="0.496481 0.503509 -0.503703 0.496255" mass="27.5544" diaginertia="0.432537 0.318732 0.219528" />
|
|
||||||
<geom class="viz" quat="0.707107 0 0 -0.707107" mesh="base_link_fine" />
|
|
||||||
<geom class="col" quat="0.707107 0 0 -0.707107" mesh="base_link_convex" />
|
|
||||||
<body name="wam/shoulder_yaw_link" pos="0 0 0.16" quat="0.707107 0 0 -0.707107">
|
|
||||||
<inertial pos="-0.00443422 -0.00066489 -0.12189" quat="0.999995 0.000984795 0.00270132 0.00136071" mass="10.7677" diaginertia="0.507411 0.462983 0.113271" />
|
|
||||||
<joint name="wam/base_yaw_joint" pos="0 0 0" axis="0 0 1" range="-2.6 2.6" />
|
|
||||||
<geom class="viz" pos="0 0 0.186" mesh="shoulder_link_fine" />
|
|
||||||
<geom class="col" pos="0 0 0.186" mesh="shoulder_link_convex_decomposition_p1" />
|
|
||||||
<geom class="col" pos="0 0 0.186" mesh="shoulder_link_convex_decomposition_p2" />
|
|
||||||
<geom class="col" pos="0 0 0.186" mesh="shoulder_link_convex_decomposition_p3" />
|
|
||||||
<body name="wam/shoulder_pitch_link" pos="0 0 0.184" quat="0.707107 -0.707107 0 0">
|
|
||||||
<inertial pos="-0.00236983 -0.0154211 0.0310561" quat="0.961781 -0.272983 0.0167269 0.0133385" mass="3.87494" diaginertia="0.0214207 0.0167101 0.0126465" />
|
|
||||||
<joint name="wam/shoulder_pitch_joint" pos="0 0 0" axis="0 0 1" range="-1.985 1.985" />
|
|
||||||
<geom class="viz" mesh="shoulder_pitch_link_fine" />
|
|
||||||
<geom class="col" mesh="shoulder_pitch_link_convex" />
|
|
||||||
<body name="wam/upper_arm_link" pos="0 -0.505 0" quat="0.707107 0.707107 0 0">
|
|
||||||
<inertial pos="-0.0382586 3.309e-05 -0.207508" quat="0.705455 0.0381914 0.0383402 0.706686" mass="1.80228" diaginertia="0.0665697 0.0634285 0.00622701" />
|
|
||||||
<joint name="wam/shoulder_yaw_joint" pos="0 0 0" axis="0 0 1" range="-2.8 2.8" />
|
|
||||||
<geom class="viz" pos="0 0 -0.505" mesh="upper_arm_link_fine" />
|
|
||||||
<geom class="col" pos="0 0 -0.505" mesh="upper_arm_link_convex_decomposition_p1" />
|
|
||||||
<geom class="col" pos="0 0 -0.505" mesh="upper_arm_link_convex_decomposition_p2" />
|
|
||||||
<body name="wam/forearm_link" pos="0.045 0 0.045" quat="0.707107 -0.707107 0 0">
|
|
||||||
<inertial pos="0.00498512 -0.132717 -0.00022942" quat="0.546303 0.447151 -0.548676 0.447842" mass="2.40017" diaginertia="0.0196896 0.0152225 0.00749914" />
|
|
||||||
<joint name="wam/elbow_pitch_joint" pos="0 0 0" axis="0 0 1" range="-0.9 3.14159" />
|
|
||||||
<geom class="viz" mesh="elbow_link_fine" />
|
|
||||||
<geom class="col" mesh="elbow_link_convex" />
|
|
||||||
<geom class="viz" pos="-0.045 -0.073 0" quat="0.707388 0.706825 0 0" mesh="forearm_link_fine" />
|
|
||||||
<geom class="col" pos="-0.045 -0.073 0" quat="0.707388 0.706825 0 0" mesh="forearm_link_convex_decomposition_p1" name="forearm_link_convex_decomposition_p1_geom" />
|
|
||||||
<geom class="col" pos="-0.045 -0.073 0" quat="0.707388 0.706825 0 0" mesh="forearm_link_convex_decomposition_p2" name="forearm_link_convex_decomposition_p2_geom" />
|
|
||||||
<body name="wam/wrist_yaw_link" pos="-0.045 0 0" quat="0.707107 0.707107 0 0">
|
|
||||||
<inertial pos="8.921e-05 0.00435824 -0.00511217" quat="0.708528 -0.000120667 0.000107481 0.705683" mass="0.12376" diaginertia="0.0112011 0.0111887 7.58188e-05" />
|
|
||||||
<joint name="wam/wrist_yaw_joint" pos="0 0 0" axis="0 0 1" range="-4.55 1.25" />
|
|
||||||
<geom class="viz" pos="0 0 0.3" mesh="wrist_yaw_link_fine" />
|
|
||||||
<geom class="col" pos="0 0 0.3" mesh="wrist_yaw_link_convex_decomposition_p1" name="wrist_yaw_link_convex_decomposition_p1_geom" />
|
|
||||||
<geom class="col" pos="0 0 0.3" mesh="wrist_yaw_link_convex_decomposition_p2" name="wrist_yaw_link_convex_decomposition_p2_geom" />
|
|
||||||
<body name="wam/wrist_pitch_link" pos="0 0 0.3" quat="0.707107 -0.707107 0 0">
|
|
||||||
<inertial pos="-0.00012262 -0.0246834 -0.0170319" quat="0.994687 -0.102891 0.000824211 -0.00336105" mass="0.417974" diaginertia="0.000555166 0.000463174 0.00023407" />
|
|
||||||
<joint name="wam/wrist_pitch_joint" pos="0 0 0" axis="0 0 1" range="-1.5707 1.5707" />
|
|
||||||
<geom class="viz" mesh="wrist_pitch_link_fine" />
|
|
||||||
<geom class="col" mesh="wrist_pitch_link_convex_decomposition_p1" name="wrist_pitch_link_convex_decomposition_p1_geom" />
|
|
||||||
<geom class="col" mesh="wrist_pitch_link_convex_decomposition_p2" name="wrist_pitch_link_convex_decomposition_p2_geom" />
|
|
||||||
<geom class="col" mesh="wrist_pitch_link_convex_decomposition_p3" name="wrist_pitch_link_convex_decomposition_p3_geom" />
|
|
||||||
<body name="wam/wrist_palm_link" pos="0 -0.06 0" quat="0.707107 0.707107 0 0">
|
|
||||||
<inertial pos="-7.974e-05 -0.00323552 -0.00016313" quat="0.594752 0.382453 0.382453 0.594752" mass="0.0686475" diaginertia="7.408e-05 3.81466e-05 3.76434e-05" />
|
|
||||||
<joint name="wam/palm_yaw_joint" pos="0 0 0" axis="0 0 1" range="-2.7 2.7" />
|
|
||||||
<geom class="viz" pos="0 0 -0.06" mesh="wrist_palm_link_fine" />
|
|
||||||
<geom class="col" pos="0 0 -0.06" mesh="wrist_palm_link_convex" name="wrist_palm_link_convex_geom" />
|
|
||||||
<body name="cup" pos="0 0 0" quat="-0.000203673 0 0 1">
|
|
||||||
<inertial pos="-3.75236e-10 8.27811e-05 0.0947015" quat="0.999945 -0.0104888 0 0" mass="0.132" diaginertia="0.000285643 0.000270485 9.65696e-05" />
|
|
||||||
<geom name="cup_geom1" pos="0 0.05 0.055" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup1" />
|
|
||||||
<geom name="cup_geom2" pos="0 0.05 0.055" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup2" />
|
|
||||||
<geom name="cup_geom3" pos="0 0.05 0.055" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup3" />
|
|
||||||
<geom name="cup_geom4" pos="0 0.05 0.055" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup4" />
|
|
||||||
<geom name="cup_geom5" pos="0 0.05 0.055" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup5" />
|
|
||||||
<geom name="cup_geom6" pos="0 0.05 0.055" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup6" />
|
|
||||||
<geom name="cup_geom7" pos="0 0.05 0.055" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup7" />
|
|
||||||
<geom name="cup_geom8" pos="0 0.05 0.055" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup8" />
|
|
||||||
<geom name="cup_geom9" pos="0 0.05 0.055" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup9" />
|
|
||||||
<geom name="cup_geom10" pos="0 0.05 0.055" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup10" />
|
|
||||||
<geom name="cup_base" pos="0 -0.035 0.1165" euler="-1.57 0 0" type="cylinder" size="0.038 0.0045" solref="-10000 -100"/>
|
|
||||||
<!-- <geom name="cup_base_contact" pos="0 -0.025 0.1165" euler="-1.57 0 0" type="cylinder" size="0.03 0.0005" solref="-10000 -100" rgba="0 0 255 1"/>-->
|
|
||||||
<geom name="cup_base_contact" pos="0 -0.005 0.1165" euler="-1.57 0 0" type="cylinder" size="0.02 0.0005" solref="-10000 -100" rgba="0 0 255 1"/>
|
|
||||||
<geom name="cup_base_contact_below" pos="0 -0.04 0.1165" euler="-1.57 0 0" type="cylinder" size="0.035 0.001" solref="-10000 -100" rgba="255 0 255 1"/>
|
|
||||||
<!-- <geom name="cup_geom11" pos="0 0.05 0.055" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup11" />-->
|
|
||||||
<!-- <geom name="cup_geom12" pos="0 0.05 0.055" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup12" />-->
|
|
||||||
<!-- <geom name="cup_geom13" pos="0 0.05 0.055" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup13" />-->
|
|
||||||
<!-- <geom name="cup_geom14" pos="0 0.05 0.055" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup14" />-->
|
|
||||||
|
|
||||||
<geom name="cup_geom15" pos="0 0.05 0.055" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup15" />
|
|
||||||
<geom name="cup_geom16" pos="0 0.05 0.055" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup16" />
|
|
||||||
<geom name="cup_geom17" pos="0 0.05 0.055" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup17" />
|
|
||||||
<geom name="cup_geom18" pos="0 0.05 0.055" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup18" />
|
|
||||||
<site name="cup_goal" pos="0 0.05 0.1165" rgba="255 0 0 1"/>
|
|
||||||
<site name="cup_goal_final" pos="0 -0.025 0.1165" rgba="0 255 0 1"/>
|
|
||||||
<body name="B0" pos="0 -0.045 0.1165" quat="0.707388 0 0 -0.706825">
|
|
||||||
<inertial pos="0 0 0" quat="0.707107 0 0.707107 0" mass="7.4927e-05" diaginertia="5.87e-10 5.87e-10 3.74635e-11" />
|
|
||||||
<geom name="G0" size="0.001 0.00427" quat="0.707107 0 0.707107 0" type="capsule" rgba="0.8 0.2 0.1 1" />
|
|
||||||
<body name="B1" pos="0.0107 0 0">
|
|
||||||
<inertial pos="0 0 0" quat="0.707107 0 0.707107 0" mass="7.4927e-05" diaginertia="5.87e-10 5.87e-10 3.74635e-11" />
|
|
||||||
<joint name="J0_1" pos="-0.00535 0 0" axis="0 1 0" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
|
||||||
<joint name="J1_1" pos="-0.00535 0 0" axis="0 0 1" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
|
||||||
<geom name="G1" size="0.001 0.00427" quat="0.707107 0 0.707107 0" type="capsule" rgba="0.8 0.2 0.1 1" />
|
|
||||||
<body name="B2" pos="0.0107 0 0">
|
|
||||||
<inertial pos="0 0 0" quat="0.707107 0 0.707107 0" mass="7.4927e-05" diaginertia="5.87e-10 5.87e-10 3.74635e-11" />
|
|
||||||
<joint name="J0_2" pos="-0.00535 0 0" axis="0 1 0" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
|
||||||
<joint name="J1_2" pos="-0.00535 0 0" axis="0 0 1" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
|
||||||
<geom name="G2" size="0.001 0.00427" quat="0.707107 0 0.707107 0" type="capsule" rgba="0.8 0.2 0.1 1" />
|
|
||||||
<body name="B3" pos="0.0107 0 0">
|
|
||||||
<inertial pos="0 0 0" quat="0.707107 0 0.707107 0" mass="7.4927e-05" diaginertia="5.87e-10 5.87e-10 3.74635e-11" />
|
|
||||||
<joint name="J0_3" pos="-0.00535 0 0" axis="0 1 0" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
|
||||||
<joint name="J1_3" pos="-0.00535 0 0" axis="0 0 1" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
|
||||||
<geom name="G3" size="0.001 0.00427" quat="0.707107 0 0.707107 0" type="capsule" rgba="0.8 0.2 0.1 1" />
|
|
||||||
<body name="B4" pos="0.0107 0 0">
|
|
||||||
<inertial pos="0 0 0" quat="0.707107 0 0.707107 0" mass="7.4927e-05" diaginertia="5.87e-10 5.87e-10 3.74635e-11" />
|
|
||||||
<joint name="J0_4" pos="-0.00535 0 0" axis="0 1 0" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
|
||||||
<joint name="J1_4" pos="-0.00535 0 0" axis="0 0 1" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
|
||||||
<geom name="G4" size="0.001 0.00427" quat="0.707107 0 0.707107 0" type="capsule" rgba="0.8 0.2 0.1 1" />
|
|
||||||
<body name="B5" pos="0.0107 0 0">
|
|
||||||
<inertial pos="0 0 0" quat="0.707107 0 0.707107 0" mass="7.4927e-05" diaginertia="5.87e-10 5.87e-10 3.74635e-11" />
|
|
||||||
<joint name="J0_5" pos="-0.00535 0 0" axis="0 1 0" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
|
||||||
<joint name="J1_5" pos="-0.00535 0 0" axis="0 0 1" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
|
||||||
<geom name="G5" size="0.001 0.00427" quat="0.707107 0 0.707107 0" type="capsule" rgba="0.8 0.2 0.1 1" />
|
|
||||||
<body name="B6" pos="0.0107 0 0">
|
|
||||||
<inertial pos="0 0 0" quat="0.707107 0 0.707107 0" mass="7.4927e-05" diaginertia="5.87e-10 5.87e-10 3.74635e-11" />
|
|
||||||
<joint name="J0_6" pos="-0.00535 0 0" axis="0 1 0" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
|
||||||
<joint name="J1_6" pos="-0.00535 0 0" axis="0 0 1" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
|
||||||
<geom name="G6" size="0.001 0.00427" quat="0.707107 0 0.707107 0" type="capsule" rgba="0.8 0.2 0.1 1" />
|
|
||||||
<body name="B7" pos="0.0107 0 0">
|
|
||||||
<inertial pos="0 0 0" quat="0.707107 0 0.707107 0" mass="7.4927e-05" diaginertia="5.87e-10 5.87e-10 3.74635e-11" />
|
|
||||||
<joint name="J0_7" pos="-0.00535 0 0" axis="0 1 0" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
|
||||||
<joint name="J1_7" pos="-0.00535 0 0" axis="0 0 1" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
|
||||||
<geom name="G7" size="0.001 0.00427" quat="0.707107 0 0.707107 0" type="capsule" rgba="0.8 0.2 0.1 1" />
|
|
||||||
<body name="B8" pos="0.0107 0 0">
|
|
||||||
<inertial pos="0 0 0" quat="0.707107 0 0.707107 0" mass="7.4927e-05" diaginertia="5.87e-10 5.87e-10 3.74635e-11" />
|
|
||||||
<joint name="J0_8" pos="-0.00535 0 0" axis="0 1 0" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
|
||||||
<joint name="J1_8" pos="-0.00535 0 0" axis="0 0 1" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
|
||||||
<geom name="G8" size="0.001 0.00427" quat="0.707107 0 0.707107 0" type="capsule" rgba="0.8 0.2 0.1 1" />
|
|
||||||
<body name="B9" pos="0.0107 0 0">
|
|
||||||
<inertial pos="0 0 0" quat="0.707107 0 0.707107 0" mass="7.4927e-05" diaginertia="5.87e-10 5.87e-10 3.74635e-11" />
|
|
||||||
<joint name="J0_9" pos="-0.00535 0 0" axis="0 1 0" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
|
||||||
<joint name="J1_9" pos="-0.00535 0 0" axis="0 0 1" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
|
||||||
<geom name="G9" size="0.001 0.00427" quat="0.707107 0 0.707107 0" type="capsule" rgba="0.8 0.2 0.1 1" />
|
|
||||||
<body name="B10" pos="0.0107 0 0">
|
|
||||||
<inertial pos="0 0 0" quat="0.707107 0 0.707107 0" mass="7.4927e-05" diaginertia="5.87e-10 5.87e-10 3.74635e-11" />
|
|
||||||
<joint name="J0_10" pos="-0.00535 0 0" axis="0 1 0" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
|
||||||
<joint name="J1_10" pos="-0.00535 0 0" axis="0 0 1" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
|
||||||
<geom name="G10" size="0.001 0.00427" quat="0.707107 0 0.707107 0" type="capsule" rgba="0.8 0.2 0.1 1" />
|
|
||||||
<body name="B11" pos="0.0107 0 0">
|
|
||||||
<inertial pos="0 0 0" quat="0.707107 0 0.707107 0" mass="7.4927e-05" diaginertia="5.87e-10 5.87e-10 3.74635e-11" />
|
|
||||||
<joint name="J0_11" pos="-0.00535 0 0" axis="0 1 0" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
|
||||||
<joint name="J1_11" pos="-0.00535 0 0" axis="0 0 1" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
|
||||||
<geom name="G11" size="0.001 0.00427" quat="0.707107 0 0.707107 0" type="capsule" rgba="0.8 0.2 0.1 1" />
|
|
||||||
<body name="B12" pos="0.0107 0 0">
|
|
||||||
<inertial pos="0 0 0" quat="0.707107 0 0.707107 0" mass="7.4927e-05" diaginertia="5.87e-10 5.87e-10 3.74635e-11" />
|
|
||||||
<joint name="J0_12" pos="-0.00535 0 0" axis="0 1 0" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
|
||||||
<joint name="J1_12" pos="-0.00535 0 0" axis="0 0 1" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
|
||||||
<geom name="G12" size="0.001 0.00427" quat="0.707107 0 0.707107 0" type="capsule" rgba="0.8 0.2 0.1 1" />
|
|
||||||
<body name="B13" pos="0.0107 0 0">
|
|
||||||
<inertial pos="0 0 0" quat="0.707107 0 0.707107 0" mass="7.4927e-05" diaginertia="5.87e-10 5.87e-10 3.74635e-11" />
|
|
||||||
<joint name="J0_13" pos="-0.00535 0 0" axis="0 1 0" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
|
||||||
<joint name="J1_13" pos="-0.00535 0 0" axis="0 0 1" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
|
||||||
<geom name="G13" size="0.001 0.00427" quat="0.707107 0 0.707107 0" type="capsule" rgba="0.8 0.2 0.1 1" />
|
|
||||||
<body name="B14" pos="0.0107 0 0">
|
|
||||||
<inertial pos="0 0 0" quat="0.707107 0 0.707107 0" mass="7.4927e-05" diaginertia="5.87e-10 5.87e-10 3.74635e-11" />
|
|
||||||
<joint name="J0_14" pos="-0.00535 0 0" axis="0 1 0" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
|
||||||
<joint name="J1_14" pos="-0.00535 0 0" axis="0 0 1" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
|
||||||
<geom name="G14" size="0.001 0.00427" quat="0.707107 0 0.707107 0" type="capsule" rgba="0.8 0.2 0.1 1" />
|
|
||||||
<body name="B15" pos="0.0107 0 0">
|
|
||||||
<inertial pos="0 0 0" quat="0.707107 0 0.707107 0" mass="7.4927e-05" diaginertia="5.87e-10 5.87e-10 3.74635e-11" />
|
|
||||||
<joint name="J0_15" pos="-0.00535 0 0" axis="0 1 0" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
|
||||||
<joint name="J1_15" pos="-0.00535 0 0" axis="0 0 1" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
|
||||||
<geom name="G15" size="0.001 0.00427" quat="0.707107 0 0.707107 0" type="capsule" rgba="0.8 0.2 0.1 1" />
|
|
||||||
<body name="B16" pos="0.0107 0 0">
|
|
||||||
<inertial pos="0 0 0" quat="0.707107 0 0.707107 0" mass="7.4927e-05" diaginertia="5.87e-10 5.87e-10 3.74635e-11" />
|
|
||||||
<joint name="J0_16" pos="-0.00535 0 0" axis="0 1 0" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
|
||||||
<joint name="J1_16" pos="-0.00535 0 0" axis="0 0 1" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
|
||||||
<geom name="G16" size="0.001 0.00427" quat="0.707107 0 0.707107 0" type="capsule" rgba="0.8 0.2 0.1 1" />
|
|
||||||
<body name="B17" pos="0.0107 0 0">
|
|
||||||
<inertial pos="0 0 0" quat="0.707107 0 0.707107 0" mass="7.4927e-05" diaginertia="5.87e-10 5.87e-10 3.74635e-11" />
|
|
||||||
<joint name="J0_17" pos="-0.00535 0 0" axis="0 1 0" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
|
||||||
<joint name="J1_17" pos="-0.00535 0 0" axis="0 0 1" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
|
||||||
<geom name="G17" size="0.001 0.00427" quat="0.707107 0 0.707107 0" type="capsule" rgba="0.8 0.2 0.1 1" />
|
|
||||||
<body name="B18" pos="0.0107 0 0">
|
|
||||||
<inertial pos="0 0 0" quat="0.707107 0 0.707107 0" mass="7.4927e-05" diaginertia="5.87e-10 5.87e-10 3.74635e-11" />
|
|
||||||
<joint name="J0_18" pos="-0.00535 0 0" axis="0 1 0" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
|
||||||
<joint name="J1_18" pos="-0.00535 0 0" axis="0 0 1" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
|
||||||
<geom name="G18" size="0.001 0.00427" quat="0.707107 0 0.707107 0" type="capsule" rgba="0.8 0.2 0.1 1" />
|
|
||||||
<body name="B19" pos="0.0107 0 0">
|
|
||||||
<inertial pos="0 0 0" quat="0.707107 0 0.707107 0" mass="7.4927e-05" diaginertia="5.87e-10 5.87e-10 3.74635e-11" />
|
|
||||||
<joint name="J0_19" pos="-0.00535 0 0" axis="0 1 0" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
|
||||||
<joint name="J1_19" pos="-0.00535 0 0" axis="0 0 1" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
|
||||||
<geom name="G19" size="0.001 0.00427" quat="0.707107 0 0.707107 0" type="capsule" rgba="0.8 0.2 0.1 1" />
|
|
||||||
<body name="B20" pos="0.0107 0 0">
|
|
||||||
<inertial pos="0 0 0" quat="0.707107 0 0.707107 0" mass="7.4927e-05" diaginertia="5.87e-10 5.87e-10 3.74635e-11" />
|
|
||||||
<joint name="J0_20" pos="-0.00535 0 0" axis="0 1 0" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
|
||||||
<joint name="J1_20" pos="-0.00535 0 0" axis="0 0 1" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
|
||||||
<geom name="G20" size="0.001 0.00427" quat="0.707107 0 0.707107 0" type="capsule" rgba="0.8 0.2 0.1 1" />
|
|
||||||
<body name="B21" pos="0.0107 0 0">
|
|
||||||
<inertial pos="0 0 0" quat="0.707107 0 0.707107 0" mass="7.4927e-05" diaginertia="5.87e-10 5.87e-10 3.74635e-11" />
|
|
||||||
<joint name="J0_21" pos="-0.00535 0 0" axis="0 1 0" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
|
||||||
<joint name="J1_21" pos="-0.00535 0 0" axis="0 0 1" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
|
||||||
<geom name="G21" size="0.001 0.00427" quat="0.707107 0 0.707107 0" type="capsule" rgba="0.8 0.2 0.1 1" />
|
|
||||||
<body name="B22" pos="0.0107 0 0">
|
|
||||||
<inertial pos="0 0 0" quat="0.707107 0 0.707107 0" mass="7.4927e-05" diaginertia="5.87e-10 5.87e-10 3.74635e-11" />
|
|
||||||
<joint name="J0_22" pos="-0.00535 0 0" axis="0 1 0" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
|
||||||
<joint name="J1_22" pos="-0.00535 0 0" axis="0 0 1" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
|
||||||
<geom name="G22" size="0.001 0.00427" quat="0.707107 0 0.707107 0" type="capsule" rgba="0.8 0.2 0.1 1" />
|
|
||||||
<body name="B23" pos="0.0107 0 0">
|
|
||||||
<inertial pos="0 0 0" quat="0.707107 0 0.707107 0" mass="7.4927e-05" diaginertia="5.87e-10 5.87e-10 3.74635e-11" />
|
|
||||||
<joint name="J0_23" pos="-0.00535 0 0" axis="0 1 0" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
|
||||||
<joint name="J1_23" pos="-0.00535 0 0" axis="0 0 1" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
|
||||||
<geom name="G23" size="0.001 0.00427" quat="0.707107 0 0.707107 0" type="capsule" rgba="0.8 0.2 0.1 1" />
|
|
||||||
<body name="B24" pos="0.0107 0 0">
|
|
||||||
<inertial pos="0 0 0" quat="0.707107 0 0.707107 0" mass="7.4927e-05" diaginertia="5.87e-10 5.87e-10 3.74635e-11" />
|
|
||||||
<joint name="J0_24" pos="-0.00535 0 0" axis="0 1 0" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
|
||||||
<joint name="J1_24" pos="-0.00535 0 0" axis="0 0 1" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
|
||||||
<geom name="G24" size="0.001 0.00427" quat="0.707107 0 0.707107 0" type="capsule" rgba="0.8 0.2 0.1 1" />
|
|
||||||
<body name="B25" pos="0.0107 0 0">
|
|
||||||
<inertial pos="0 0 0" quat="0.707107 0 0.707107 0" mass="7.4927e-05" diaginertia="5.87e-10 5.87e-10 3.74635e-11" />
|
|
||||||
<joint name="J0_25" pos="-0.00535 0 0" axis="0 1 0" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
|
||||||
<joint name="J1_25" pos="-0.00535 0 0" axis="0 0 1" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
|
||||||
<geom name="G25" size="0.001 0.00427" quat="0.707107 0 0.707107 0" type="capsule" rgba="0.8 0.2 0.1 1" />
|
|
||||||
<body name="B26" pos="0.0107 0 0">
|
|
||||||
<inertial pos="0 0 0" quat="0.707107 0 0.707107 0" mass="7.4927e-05" diaginertia="5.87e-10 5.87e-10 3.74635e-11" />
|
|
||||||
<joint name="J0_26" pos="-0.00535 0 0" axis="0 1 0" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
|
||||||
<joint name="J1_26" pos="-0.00535 0 0" axis="0 0 1" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
|
||||||
<geom name="G26" size="0.001 0.00427" quat="0.707107 0 0.707107 0" type="capsule" rgba="0.8 0.2 0.1 1" />
|
|
||||||
<body name="B27" pos="0.0107 0 0">
|
|
||||||
<inertial pos="0 0 0" quat="0.707107 0 0.707107 0" mass="7.4927e-05" diaginertia="5.87e-10 5.87e-10 3.74635e-11" />
|
|
||||||
<joint name="J0_27" pos="-0.00535 0 0" axis="0 1 0" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
|
||||||
<joint name="J1_27" pos="-0.00535 0 0" axis="0 0 1" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
|
||||||
<geom name="G27" size="0.001 0.00427" quat="0.707107 0 0.707107 0" type="capsule" rgba="0.8 0.2 0.1 1" />
|
|
||||||
<body name="B28" pos="0.0107 0 0">
|
|
||||||
<inertial pos="0 0 0" quat="0.707107 0 0.707107 0" mass="7.4927e-05" diaginertia="5.87e-10 5.87e-10 3.74635e-11" />
|
|
||||||
<joint name="J0_28" pos="-0.00535 0 0" axis="0 1 0" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
|
||||||
<joint name="J1_28" pos="-0.00535 0 0" axis="0 0 1" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
|
||||||
<geom name="G28" size="0.001 0.00427" quat="0.707107 0 0.707107 0" type="capsule" rgba="0.8 0.2 0.1 1" />
|
|
||||||
<body name="B29" pos="0.0107 0 0">
|
|
||||||
<inertial pos="0 0 0" quat="0.707107 0 0.707107 0" mass="7.4927e-05" diaginertia="5.87e-10 5.87e-10 3.74635e-11" />
|
|
||||||
<joint name="J0_29" pos="-0.00535 0 0" axis="0 1 0" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
|
||||||
<joint name="J1_29" pos="-0.00535 0 0" axis="0 0 1" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
|
||||||
<geom name="G29" size="0.001 0.00427" quat="0.707107 0 0.707107 0" type="capsule" rgba="0.8 0.2 0.1 1" />
|
|
||||||
<body name="ball">
|
|
||||||
<geom name="ball_geom" type="sphere" size="0.02" mass="0.015" rgba="0.8 0.2 0.1 1"/>
|
|
||||||
</body>
|
|
||||||
</body>
|
|
||||||
</body>
|
|
||||||
</body>
|
|
||||||
</body>
|
|
||||||
</body>
|
|
||||||
</body>
|
|
||||||
</body>
|
|
||||||
</body>
|
|
||||||
</body>
|
|
||||||
</body>
|
|
||||||
</body>
|
|
||||||
</body>
|
|
||||||
</body>
|
|
||||||
</body>
|
|
||||||
</body>
|
|
||||||
</body>
|
|
||||||
</body>
|
|
||||||
</body>
|
|
||||||
</body>
|
|
||||||
</body>
|
|
||||||
</body>
|
|
||||||
</body>
|
|
||||||
</body>
|
|
||||||
</body>
|
|
||||||
</body>
|
|
||||||
</body>
|
|
||||||
</body>
|
|
||||||
</body>
|
|
||||||
</body>
|
|
||||||
</body>
|
|
||||||
</body>
|
|
||||||
</body>
|
|
||||||
</body>
|
|
||||||
</body>
|
|
||||||
</body>
|
|
||||||
</body>
|
|
||||||
</body>
|
|
||||||
</body>
|
|
||||||
</body>
|
|
||||||
<!-- <site name="context_point" pos="-0.20869846 -0.66376693 1.18088501" euler="-1.57 0 0" size="0.015" rgba="1 0 0 0.6" type="sphere"/>-->
|
|
||||||
<!-- <site name="context_point1" pos="-0.5 -0.85 0.8165" euler="-1.57 0 0" size="0.015" rgba="0 1 0 0.3" type="sphere"/>-->
|
|
||||||
<!-- <site name="context_point2" pos="-0.5 -0.85 1.4165" euler="-1.57 0 0" size="0.015" rgba="0 1 0 0.3" type="sphere"/>-->
|
|
||||||
<!-- <site name="context_point3" pos="-0.5 -0.35 0.8165" euler="-1.57 0 0" size="0.015" rgba="0 1 0 0.3" type="sphere"/>-->
|
|
||||||
<!-- <site name="context_point4" pos="-0.5 -0.35 1.4165" euler="-1.57 0 0" size="0.015" rgba="0 1 0 0.3" type="sphere"/>-->
|
|
||||||
<!-- <site name="context_point5" pos="0.5 -0.85 0.8165" euler="-1.57 0 0" size="0.015" rgba="0 1 0 0.3" type="sphere"/>-->
|
|
||||||
<!-- <site name="context_point6" pos="0.5 -0.85 1.4165" euler="-1.57 0 0" size="0.015" rgba="0 1 0 0.3" type="sphere"/>-->
|
|
||||||
<!-- <site name="context_point7" pos="0.5 -0.35 0.8165" euler="-1.57 0 0" size="0.015" rgba="0 1 0 0.3" type="sphere"/>-->
|
|
||||||
<!-- <site name="context_point8" pos="0.5 -0.35 1.4165" euler="-1.57 0 0" size="0.015" rgba="0 1 0 0.3" type="sphere"/>-->
|
|
||||||
<!-- <site name="context_space" pos="0 -0.6 1.1165" euler="0 0 0" size="0.5 0.25 0.3" rgba="0 0 1 0.15" type="box"/>-->
|
|
||||||
<camera name="visualization" mode="targetbody" target="wam/wrist_yaw_link" pos="1.5 -0.4 2.2"/>
|
|
||||||
<camera name="experiment" mode="fixed" quat="0.44418059 0.41778323 0.54301123 0.57732103" pos="1.5 -0.3 1.33" />
|
|
||||||
</worldbody>
|
|
||||||
|
|
||||||
<actuator>
|
|
||||||
<!-- <motor ctrllimited="true" ctrlrange="-150 150" joint="wam/base_yaw_joint"/>-->
|
|
||||||
<!-- <motor ctrllimited="true" ctrlrange="-125 125" joint="wam/shoulder_pitch_joint"/>-->
|
|
||||||
<!-- <motor ctrllimited="true" ctrlrange="-40 40" joint="wam/shoulder_yaw_joint"/>-->
|
|
||||||
<!-- <motor ctrllimited="true" ctrlrange="-60 60" joint="wam/elbow_pitch_joint"/>-->
|
|
||||||
<!-- <motor ctrllimited="true" ctrlrange="-5 5" joint="wam/wrist_yaw_joint"/>-->
|
|
||||||
<!-- <motor ctrllimited="true" ctrlrange="-5 5" joint="wam/wrist_pitch_joint"/>-->
|
|
||||||
<!-- <motor ctrllimited="true" ctrlrange="-2 2" joint="wam/palm_yaw_joint"/>-->
|
|
||||||
<motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="150.0" joint="wam/base_yaw_joint"/>
|
|
||||||
<motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="125.0" joint="wam/shoulder_pitch_joint"/>
|
|
||||||
<motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="40.0" joint="wam/shoulder_yaw_joint"/>
|
|
||||||
<motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="60.0" joint="wam/elbow_pitch_joint"/>
|
|
||||||
<motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="5.0" joint="wam/wrist_yaw_joint"/>
|
|
||||||
<motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="5.0" joint="wam/wrist_pitch_joint"/>
|
|
||||||
<motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="2.0" joint="wam/palm_yaw_joint"/>
|
|
||||||
</actuator>
|
|
||||||
</mujoco>
|
|
||||||
@ -1,196 +0,0 @@
|
|||||||
from gym import utils
|
|
||||||
import os
|
|
||||||
import numpy as np
|
|
||||||
from gym.envs.mujoco import MujocoEnv
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
class ALRBallInACupEnv(MujocoEnv, utils.EzPickle):
|
|
||||||
def __init__(self, n_substeps=4, apply_gravity_comp=True, simplified: bool = False,
|
|
||||||
reward_type: str = None, context: np.ndarray = None):
|
|
||||||
utils.EzPickle.__init__(**locals())
|
|
||||||
self._steps = 0
|
|
||||||
|
|
||||||
self.xml_path = os.path.join(os.path.dirname(os.path.abspath(__file__)), "assets", "biac_base.xml")
|
|
||||||
|
|
||||||
self._q_pos = []
|
|
||||||
self._q_vel = []
|
|
||||||
# self.weight_matrix_scale = 50
|
|
||||||
self.max_ctrl = np.array([150., 125., 40., 60., 5., 5., 2.])
|
|
||||||
|
|
||||||
self.j_min = np.array([-2.6, -1.985, -2.8, -0.9, -4.55, -1.5707, -2.7])
|
|
||||||
self.j_max = np.array([2.6, 1.985, 2.8, 3.14159, 1.25, 1.5707, 2.7])
|
|
||||||
|
|
||||||
self.context = context
|
|
||||||
|
|
||||||
alr_mujoco_env.AlrMujocoEnv.__init__(self,
|
|
||||||
self.xml_path,
|
|
||||||
apply_gravity_comp=apply_gravity_comp,
|
|
||||||
n_substeps=n_substeps)
|
|
||||||
self._start_pos = np.array([0.0, 0.58760536, 0.0, 1.36004913, 0.0, -0.32072943, -1.57])
|
|
||||||
self._start_vel = np.zeros(7)
|
|
||||||
|
|
||||||
self.simplified = simplified
|
|
||||||
|
|
||||||
self.sim_time = 8 # seconds
|
|
||||||
self.sim_steps = int(self.sim_time / self.dt)
|
|
||||||
if reward_type == "no_context":
|
|
||||||
from alr_envs.alr.mujoco.ball_in_a_cup.ball_in_a_cup_reward_simple import BallInACupReward
|
|
||||||
reward_function = BallInACupReward
|
|
||||||
elif reward_type == "contextual_goal":
|
|
||||||
from alr_envs.alr.mujoco.ball_in_a_cup.ball_in_a_cup_reward import BallInACupReward
|
|
||||||
reward_function = BallInACupReward
|
|
||||||
else:
|
|
||||||
raise ValueError("Unknown reward type: {}".format(reward_type))
|
|
||||||
self.reward_function = reward_function(self.sim_steps)
|
|
||||||
|
|
||||||
@property
|
|
||||||
def start_pos(self):
|
|
||||||
if self.simplified:
|
|
||||||
return self._start_pos[1::2]
|
|
||||||
else:
|
|
||||||
return self._start_pos
|
|
||||||
|
|
||||||
@property
|
|
||||||
def start_vel(self):
|
|
||||||
if self.simplified:
|
|
||||||
return self._start_vel[1::2]
|
|
||||||
else:
|
|
||||||
return self._start_vel
|
|
||||||
|
|
||||||
@property
|
|
||||||
def current_pos(self):
|
|
||||||
return self.sim.data.qpos[0:7].copy()
|
|
||||||
|
|
||||||
@property
|
|
||||||
def current_vel(self):
|
|
||||||
return self.sim.data.qvel[0:7].copy()
|
|
||||||
|
|
||||||
def reset(self):
|
|
||||||
self.reward_function.reset(None)
|
|
||||||
return super().reset()
|
|
||||||
|
|
||||||
def reset_model(self):
|
|
||||||
init_pos_all = self.init_qpos.copy()
|
|
||||||
init_pos_robot = self._start_pos
|
|
||||||
init_vel = np.zeros_like(init_pos_all)
|
|
||||||
|
|
||||||
self._steps = 0
|
|
||||||
self._q_pos = []
|
|
||||||
self._q_vel = []
|
|
||||||
|
|
||||||
start_pos = init_pos_all
|
|
||||||
start_pos[0:7] = init_pos_robot
|
|
||||||
|
|
||||||
self.set_state(start_pos, init_vel)
|
|
||||||
|
|
||||||
return self._get_obs()
|
|
||||||
|
|
||||||
def step(self, a):
|
|
||||||
reward_dist = 0.0
|
|
||||||
angular_vel = 0.0
|
|
||||||
reward_ctrl = - np.square(a).sum()
|
|
||||||
|
|
||||||
crash = self.do_simulation(a)
|
|
||||||
# joint_cons_viol = self.check_traj_in_joint_limits()
|
|
||||||
|
|
||||||
self._q_pos.append(self.sim.data.qpos[0:7].ravel().copy())
|
|
||||||
self._q_vel.append(self.sim.data.qvel[0:7].ravel().copy())
|
|
||||||
|
|
||||||
ob = self._get_obs()
|
|
||||||
|
|
||||||
if not crash:
|
|
||||||
reward, success, is_collided = self.reward_function.compute_reward(a, self)
|
|
||||||
done = success or self._steps == self.sim_steps - 1 or is_collided
|
|
||||||
self._steps += 1
|
|
||||||
else:
|
|
||||||
reward = -2000
|
|
||||||
success = False
|
|
||||||
is_collided = False
|
|
||||||
done = True
|
|
||||||
return ob, reward, done, dict(reward_dist=reward_dist,
|
|
||||||
reward_ctrl=reward_ctrl,
|
|
||||||
velocity=angular_vel,
|
|
||||||
# traj=self._q_pos,
|
|
||||||
action=a,
|
|
||||||
q_pos=self.sim.data.qpos[0:7].ravel().copy(),
|
|
||||||
q_vel=self.sim.data.qvel[0:7].ravel().copy(),
|
|
||||||
is_success=success,
|
|
||||||
is_collided=is_collided, sim_crash=crash)
|
|
||||||
|
|
||||||
def check_traj_in_joint_limits(self):
|
|
||||||
return any(self.current_pos > self.j_max) or any(self.current_pos < self.j_min)
|
|
||||||
|
|
||||||
# TODO: extend observation space
|
|
||||||
def _get_obs(self):
|
|
||||||
theta = self.sim.data.qpos.flat[:7]
|
|
||||||
return np.concatenate([
|
|
||||||
np.cos(theta),
|
|
||||||
np.sin(theta),
|
|
||||||
# self.get_body_com("target"), # only return target to make problem harder
|
|
||||||
[self._steps],
|
|
||||||
])
|
|
||||||
|
|
||||||
# TODO
|
|
||||||
@property
|
|
||||||
def active_obs(self):
|
|
||||||
return np.hstack([
|
|
||||||
[False] * 7, # cos
|
|
||||||
[False] * 7, # sin
|
|
||||||
# [True] * 2, # x-y coordinates of target distance
|
|
||||||
[False] # env steps
|
|
||||||
])
|
|
||||||
|
|
||||||
# These functions are for the task with 3 joint actuations
|
|
||||||
def extend_des_pos(self, des_pos):
|
|
||||||
des_pos_full = self._start_pos.copy()
|
|
||||||
des_pos_full[1] = des_pos[0]
|
|
||||||
des_pos_full[3] = des_pos[1]
|
|
||||||
des_pos_full[5] = des_pos[2]
|
|
||||||
return des_pos_full
|
|
||||||
|
|
||||||
def extend_des_vel(self, des_vel):
|
|
||||||
des_vel_full = self._start_vel.copy()
|
|
||||||
des_vel_full[1] = des_vel[0]
|
|
||||||
des_vel_full[3] = des_vel[1]
|
|
||||||
des_vel_full[5] = des_vel[2]
|
|
||||||
return des_vel_full
|
|
||||||
|
|
||||||
def render(self, render_mode, **render_kwargs):
|
|
||||||
if render_mode == "plot_trajectory":
|
|
||||||
if self._steps == 1:
|
|
||||||
import matplotlib.pyplot as plt
|
|
||||||
# plt.ion()
|
|
||||||
self.fig, self.axs = plt.subplots(3, 1)
|
|
||||||
|
|
||||||
if self._steps <= 1750:
|
|
||||||
for ax, cp in zip(self.axs, self.current_pos[1::2]):
|
|
||||||
ax.scatter(self._steps, cp, s=2, marker=".")
|
|
||||||
|
|
||||||
# self.fig.show()
|
|
||||||
|
|
||||||
else:
|
|
||||||
super().render(render_mode, **render_kwargs)
|
|
||||||
|
|
||||||
|
|
||||||
if __name__ == "__main__":
|
|
||||||
env = ALRBallInACupEnv()
|
|
||||||
ctxt = np.array([-0.20869846, -0.66376693, 1.18088501])
|
|
||||||
|
|
||||||
env.configure(ctxt)
|
|
||||||
env.reset()
|
|
||||||
# env.render()
|
|
||||||
for i in range(16000):
|
|
||||||
# test with random actions
|
|
||||||
ac = 0.001 * env.action_space.sample()[0:7]
|
|
||||||
# ac = env.start_pos
|
|
||||||
# ac[0] += np.pi/2
|
|
||||||
obs, rew, d, info = env.step(ac)
|
|
||||||
# env.render()
|
|
||||||
|
|
||||||
print(rew)
|
|
||||||
|
|
||||||
if d:
|
|
||||||
break
|
|
||||||
|
|
||||||
env.close()
|
|
||||||
@ -1,42 +0,0 @@
|
|||||||
from typing import Tuple, Union
|
|
||||||
|
|
||||||
import numpy as np
|
|
||||||
|
|
||||||
from mp_env_api import MPEnvWrapper
|
|
||||||
|
|
||||||
|
|
||||||
class BallInACupMPWrapper(MPEnvWrapper):
|
|
||||||
|
|
||||||
@property
|
|
||||||
def active_obs(self):
|
|
||||||
# TODO: @Max Filter observations correctly
|
|
||||||
return np.hstack([
|
|
||||||
[False] * 7, # cos
|
|
||||||
[False] * 7, # sin
|
|
||||||
# [True] * 2, # x-y coordinates of target distance
|
|
||||||
[False] # env steps
|
|
||||||
])
|
|
||||||
|
|
||||||
@property
|
|
||||||
def start_pos(self):
|
|
||||||
if self.simplified:
|
|
||||||
return self._start_pos[1::2]
|
|
||||||
else:
|
|
||||||
return self._start_pos
|
|
||||||
|
|
||||||
@property
|
|
||||||
def current_pos(self) -> Union[float, int, np.ndarray, Tuple]:
|
|
||||||
return self.sim.data.qpos[0:7].copy()
|
|
||||||
|
|
||||||
@property
|
|
||||||
def current_vel(self) -> Union[float, int, np.ndarray, Tuple]:
|
|
||||||
return self.sim.data.qvel[0:7].copy()
|
|
||||||
|
|
||||||
@property
|
|
||||||
def goal_pos(self):
|
|
||||||
# TODO: @Max I think the default value of returning to the start is reasonable here
|
|
||||||
raise ValueError("Goal position is not available and has to be learnt based on the environment.")
|
|
||||||
|
|
||||||
@property
|
|
||||||
def dt(self) -> Union[float, int]:
|
|
||||||
return self.env.dt
|
|
||||||
@ -1,142 +0,0 @@
|
|||||||
import numpy as np
|
|
||||||
from alr_envs.alr.mujoco import alr_reward_fct
|
|
||||||
|
|
||||||
|
|
||||||
class BallInACupReward(alr_reward_fct.AlrReward):
|
|
||||||
def __init__(self, sim_time):
|
|
||||||
self.sim_time = sim_time
|
|
||||||
|
|
||||||
self.collision_objects = ["cup_geom1", "cup_geom2", "wrist_palm_link_convex_geom",
|
|
||||||
"wrist_pitch_link_convex_decomposition_p1_geom",
|
|
||||||
"wrist_pitch_link_convex_decomposition_p2_geom",
|
|
||||||
"wrist_pitch_link_convex_decomposition_p3_geom",
|
|
||||||
"wrist_yaw_link_convex_decomposition_p1_geom",
|
|
||||||
"wrist_yaw_link_convex_decomposition_p2_geom",
|
|
||||||
"forearm_link_convex_decomposition_p1_geom",
|
|
||||||
"forearm_link_convex_decomposition_p2_geom"]
|
|
||||||
|
|
||||||
self.ball_id = None
|
|
||||||
self.ball_collision_id = None
|
|
||||||
self.goal_id = None
|
|
||||||
self.goal_final_id = None
|
|
||||||
self.collision_ids = None
|
|
||||||
|
|
||||||
self.ball_traj = None
|
|
||||||
self.dists = None
|
|
||||||
self.dists_ctxt = None
|
|
||||||
self.dists_final = None
|
|
||||||
self.costs = None
|
|
||||||
|
|
||||||
self.reset(None)
|
|
||||||
|
|
||||||
def reset(self, context):
|
|
||||||
self.ball_traj = np.zeros(shape=(self.sim_time, 3))
|
|
||||||
self.cup_traj = np.zeros(shape=(self.sim_time, 3))
|
|
||||||
self.dists = []
|
|
||||||
self.dists_ctxt = []
|
|
||||||
self.dists_final = []
|
|
||||||
self.costs = []
|
|
||||||
self.context = context
|
|
||||||
self.ball_in_cup = False
|
|
||||||
self.ball_above_threshold = False
|
|
||||||
self.dist_ctxt = 3
|
|
||||||
self.action_costs = []
|
|
||||||
self.cup_angles = []
|
|
||||||
|
|
||||||
def compute_reward(self, action, sim, step):
|
|
||||||
action_cost = np.sum(np.square(action))
|
|
||||||
self.action_costs.append(action_cost)
|
|
||||||
|
|
||||||
stop_sim = False
|
|
||||||
success = False
|
|
||||||
|
|
||||||
self.ball_id = sim.model._body_name2id["ball"]
|
|
||||||
self.ball_collision_id = sim.model._geom_name2id["ball_geom"]
|
|
||||||
self.goal_id = sim.model._site_name2id["cup_goal"]
|
|
||||||
self.goal_final_id = sim.model._site_name2id["cup_goal_final"]
|
|
||||||
self.collision_ids = [sim.model._geom_name2id[name] for name in self.collision_objects]
|
|
||||||
|
|
||||||
if self.check_collision(sim):
|
|
||||||
reward = - 1e-3 * action_cost - 1000
|
|
||||||
stop_sim = True
|
|
||||||
return reward, success, stop_sim
|
|
||||||
|
|
||||||
# Compute the current distance from the ball to the inner part of the cup
|
|
||||||
goal_pos = sim.data.site_xpos[self.goal_id]
|
|
||||||
ball_pos = sim.data.body_xpos[self.ball_id]
|
|
||||||
goal_final_pos = sim.data.site_xpos[self.goal_final_id]
|
|
||||||
self.dists.append(np.linalg.norm(goal_pos - ball_pos))
|
|
||||||
self.dists_final.append(np.linalg.norm(goal_final_pos - ball_pos))
|
|
||||||
self.dists_ctxt.append(np.linalg.norm(ball_pos - self.context))
|
|
||||||
self.ball_traj[step, :] = np.copy(ball_pos)
|
|
||||||
self.cup_traj[step, :] = np.copy(goal_pos) # ?
|
|
||||||
cup_quat = np.copy(sim.data.body_xquat[sim.model._body_name2id["cup"]])
|
|
||||||
self.cup_angles.append(np.arctan2(2 * (cup_quat[0] * cup_quat[1] + cup_quat[2] * cup_quat[3]),
|
|
||||||
1 - 2 * (cup_quat[1] ** 2 + cup_quat[2] ** 2)))
|
|
||||||
|
|
||||||
# Determine the first time when ball is in cup
|
|
||||||
if not self.ball_in_cup:
|
|
||||||
ball_in_cup = self.check_ball_in_cup(sim, self.ball_collision_id)
|
|
||||||
self.ball_in_cup = ball_in_cup
|
|
||||||
if ball_in_cup:
|
|
||||||
dist_to_ctxt = np.linalg.norm(ball_pos - self.context)
|
|
||||||
self.dist_ctxt = dist_to_ctxt
|
|
||||||
|
|
||||||
if step == self.sim_time - 1:
|
|
||||||
t_min_dist = np.argmin(self.dists)
|
|
||||||
angle_min_dist = self.cup_angles[t_min_dist]
|
|
||||||
cost_angle = (angle_min_dist - np.pi / 2) ** 2
|
|
||||||
|
|
||||||
min_dist = np.min(self.dists)
|
|
||||||
dist_final = self.dists_final[-1]
|
|
||||||
# dist_ctxt = self.dists_ctxt[-1]
|
|
||||||
|
|
||||||
# # max distance between ball and cup and cup height at that time
|
|
||||||
# ball_to_cup_diff = self.ball_traj[:, 2] - self.cup_traj[:, 2]
|
|
||||||
# t_max_diff = np.argmax(ball_to_cup_diff)
|
|
||||||
# t_max_ball_height = np.argmax(self.ball_traj[:, 2])
|
|
||||||
# max_ball_height = np.max(self.ball_traj[:, 2])
|
|
||||||
|
|
||||||
# cost = self._get_stage_wise_cost(ball_in_cup, min_dist, dist_final, dist_ctxt)
|
|
||||||
cost = 0.5 * min_dist + 0.5 * dist_final + 0.3 * np.minimum(self.dist_ctxt, 3) + 0.01 * cost_angle
|
|
||||||
reward = np.exp(-2 * cost) - 1e-3 * action_cost
|
|
||||||
# if max_ball_height < self.context[2] or ball_to_cup_diff[t_max_ball_height] < 0:
|
|
||||||
# reward -= 1
|
|
||||||
|
|
||||||
success = dist_final < 0.05 and self.dist_ctxt < 0.05
|
|
||||||
else:
|
|
||||||
reward = - 1e-3 * action_cost
|
|
||||||
success = False
|
|
||||||
|
|
||||||
return reward, success, stop_sim
|
|
||||||
|
|
||||||
def _get_stage_wise_cost(self, ball_in_cup, min_dist, dist_final, dist_to_ctxt):
|
|
||||||
if not ball_in_cup:
|
|
||||||
cost = 3 + 2*(0.5 * min_dist**2 + 0.5 * dist_final**2)
|
|
||||||
else:
|
|
||||||
cost = 2 * dist_to_ctxt ** 2
|
|
||||||
print('Context Distance:', dist_to_ctxt)
|
|
||||||
return cost
|
|
||||||
|
|
||||||
def check_ball_in_cup(self, sim, ball_collision_id):
|
|
||||||
cup_base_collision_id = sim.model._geom_name2id["cup_base_contact"]
|
|
||||||
for coni in range(0, sim.data.ncon):
|
|
||||||
con = sim.data.contact[coni]
|
|
||||||
|
|
||||||
collision = con.geom1 == cup_base_collision_id and con.geom2 == ball_collision_id
|
|
||||||
collision_trans = con.geom1 == ball_collision_id and con.geom2 == cup_base_collision_id
|
|
||||||
|
|
||||||
if collision or collision_trans:
|
|
||||||
return True
|
|
||||||
return False
|
|
||||||
|
|
||||||
def check_collision(self, sim):
|
|
||||||
for coni in range(0, sim.data.ncon):
|
|
||||||
con = sim.data.contact[coni]
|
|
||||||
|
|
||||||
collision = con.geom1 in self.collision_ids and con.geom2 == self.ball_collision_id
|
|
||||||
collision_trans = con.geom1 == self.ball_collision_id and con.geom2 in self.collision_ids
|
|
||||||
|
|
||||||
if collision or collision_trans:
|
|
||||||
return True
|
|
||||||
return False
|
|
||||||
@ -1,116 +0,0 @@
|
|||||||
import numpy as np
|
|
||||||
from alr_envs.alr.mujoco import alr_reward_fct
|
|
||||||
|
|
||||||
|
|
||||||
class BallInACupReward(alr_reward_fct.AlrReward):
|
|
||||||
def __init__(self, env):
|
|
||||||
self.env = env
|
|
||||||
self.collision_objects = ["cup_geom1", "cup_geom2", "cup_base_contact_below",
|
|
||||||
"wrist_palm_link_convex_geom",
|
|
||||||
"wrist_pitch_link_convex_decomposition_p1_geom",
|
|
||||||
"wrist_pitch_link_convex_decomposition_p2_geom",
|
|
||||||
"wrist_pitch_link_convex_decomposition_p3_geom",
|
|
||||||
"wrist_yaw_link_convex_decomposition_p1_geom",
|
|
||||||
"wrist_yaw_link_convex_decomposition_p2_geom",
|
|
||||||
"forearm_link_convex_decomposition_p1_geom",
|
|
||||||
"forearm_link_convex_decomposition_p2_geom"]
|
|
||||||
|
|
||||||
self.ball_id = None
|
|
||||||
self.ball_collision_id = None
|
|
||||||
self.goal_id = None
|
|
||||||
self.goal_final_id = None
|
|
||||||
self.collision_ids = None
|
|
||||||
self._is_collided = False
|
|
||||||
self.collision_penalty = 1000
|
|
||||||
|
|
||||||
self.ball_traj = None
|
|
||||||
self.dists = None
|
|
||||||
self.dists_final = None
|
|
||||||
self.costs = None
|
|
||||||
|
|
||||||
self.reset(None)
|
|
||||||
|
|
||||||
def reset(self, context):
|
|
||||||
# self.sim_time = self.env.sim.dtsim_time
|
|
||||||
self.ball_traj = [] # np.zeros(shape=(self.sim_time, 3))
|
|
||||||
self.dists = []
|
|
||||||
self.dists_final = []
|
|
||||||
self.costs = []
|
|
||||||
self.action_costs = []
|
|
||||||
self.angle_costs = []
|
|
||||||
self.cup_angles = []
|
|
||||||
|
|
||||||
def compute_reward(self, action):
|
|
||||||
self.ball_id = self.env.sim.model._body_name2id["ball"]
|
|
||||||
self.ball_collision_id = self.env.sim.model._geom_name2id["ball_geom"]
|
|
||||||
self.goal_id = self.env.sim.model._site_name2id["cup_goal"]
|
|
||||||
self.goal_final_id = self.env.sim.model._site_name2id["cup_goal_final"]
|
|
||||||
self.collision_ids = [self.env.sim.model._geom_name2id[name] for name in self.collision_objects]
|
|
||||||
|
|
||||||
ball_in_cup = self.check_ball_in_cup(self.env.sim, self.ball_collision_id)
|
|
||||||
|
|
||||||
# Compute the current distance from the ball to the inner part of the cup
|
|
||||||
goal_pos = self.env.sim.data.site_xpos[self.goal_id]
|
|
||||||
ball_pos = self.env.sim.data.body_xpos[self.ball_id]
|
|
||||||
goal_final_pos = self.env.sim.data.site_xpos[self.goal_final_id]
|
|
||||||
self.dists.append(np.linalg.norm(goal_pos - ball_pos))
|
|
||||||
self.dists_final.append(np.linalg.norm(goal_final_pos - ball_pos))
|
|
||||||
# self.ball_traj[self.env._steps, :] = ball_pos
|
|
||||||
self.ball_traj.append(ball_pos)
|
|
||||||
cup_quat = np.copy(self.env.sim.data.body_xquat[self.env.sim.model._body_name2id["cup"]])
|
|
||||||
cup_angle = np.arctan2(2 * (cup_quat[0] * cup_quat[1] + cup_quat[2] * cup_quat[3]),
|
|
||||||
1 - 2 * (cup_quat[1]**2 + cup_quat[2]**2))
|
|
||||||
cost_angle = (cup_angle - np.pi / 2) ** 2
|
|
||||||
self.angle_costs.append(cost_angle)
|
|
||||||
self.cup_angles.append(cup_angle)
|
|
||||||
|
|
||||||
action_cost = np.sum(np.square(action))
|
|
||||||
self.action_costs.append(action_cost)
|
|
||||||
|
|
||||||
self._is_collided = self.check_collision(self.env.sim) or self.env.check_traj_in_joint_limits()
|
|
||||||
|
|
||||||
if self.env._steps == self.env.ep_length - 1 or self._is_collided:
|
|
||||||
t_min_dist = np.argmin(self.dists)
|
|
||||||
angle_min_dist = self.cup_angles[t_min_dist]
|
|
||||||
# cost_angle = (angle_min_dist - np.pi / 2)**2
|
|
||||||
|
|
||||||
|
|
||||||
# min_dist = self.dists[t_min_dist]
|
|
||||||
dist_final = self.dists_final[-1]
|
|
||||||
min_dist_final = np.min(self.dists_final)
|
|
||||||
|
|
||||||
# cost = 0.5 * dist_final + 0.05 * cost_angle # TODO: Increase cost_angle weight # 0.5 * min_dist +
|
|
||||||
# reward = np.exp(-2 * cost) - 1e-2 * action_cost - self.collision_penalty * int(self._is_collided)
|
|
||||||
# reward = - dist_final**2 - 1e-4 * cost_angle - 1e-5 * action_cost - self.collision_penalty * int(self._is_collided)
|
|
||||||
reward = - dist_final**2 - min_dist_final**2 - 1e-4 * cost_angle - 1e-3 * action_cost - self.collision_penalty * int(self._is_collided)
|
|
||||||
success = dist_final < 0.05 and ball_in_cup and not self._is_collided
|
|
||||||
crash = self._is_collided
|
|
||||||
else:
|
|
||||||
reward = - 1e-3 * action_cost - 1e-4 * cost_angle # TODO: increase action_cost weight
|
|
||||||
success = False
|
|
||||||
crash = False
|
|
||||||
|
|
||||||
return reward, success, crash
|
|
||||||
|
|
||||||
def check_ball_in_cup(self, sim, ball_collision_id):
|
|
||||||
cup_base_collision_id = sim.model._geom_name2id["cup_base_contact"]
|
|
||||||
for coni in range(0, sim.data.ncon):
|
|
||||||
con = sim.data.contact[coni]
|
|
||||||
|
|
||||||
collision = con.geom1 == cup_base_collision_id and con.geom2 == ball_collision_id
|
|
||||||
collision_trans = con.geom1 == ball_collision_id and con.geom2 == cup_base_collision_id
|
|
||||||
|
|
||||||
if collision or collision_trans:
|
|
||||||
return True
|
|
||||||
return False
|
|
||||||
|
|
||||||
def check_collision(self, sim):
|
|
||||||
for coni in range(0, sim.data.ncon):
|
|
||||||
con = sim.data.contact[coni]
|
|
||||||
|
|
||||||
collision = con.geom1 in self.collision_ids and con.geom2 == self.ball_collision_id
|
|
||||||
collision_trans = con.geom1 == self.ball_collision_id and con.geom2 in self.collision_ids
|
|
||||||
|
|
||||||
if collision or collision_trans:
|
|
||||||
return True
|
|
||||||
return False
|
|
||||||
@ -1,205 +0,0 @@
|
|||||||
import os
|
|
||||||
|
|
||||||
import gym.envs.mujoco
|
|
||||||
import gym.envs.mujoco as mujoco_env
|
|
||||||
import mujoco_py.builder
|
|
||||||
import numpy as np
|
|
||||||
from gym import utils
|
|
||||||
|
|
||||||
from mp_env_api.mp_wrappers.detpmp_wrapper import DetPMPWrapper
|
|
||||||
from mp_env_api.utils.policies import PDControllerExtend
|
|
||||||
|
|
||||||
|
|
||||||
def make_detpmp_env(**kwargs):
|
|
||||||
name = kwargs.pop("name")
|
|
||||||
_env = gym.make(name)
|
|
||||||
policy = PDControllerExtend(_env, p_gains=kwargs.pop('p_gains'), d_gains=kwargs.pop('d_gains'))
|
|
||||||
kwargs['policy_type'] = policy
|
|
||||||
return DetPMPWrapper(_env, **kwargs)
|
|
||||||
|
|
||||||
|
|
||||||
class ALRBallInACupPDEnv(mujoco_env.MujocoEnv, utils.EzPickle):
|
|
||||||
def __init__(self, frame_skip=4, apply_gravity_comp=True, simplified: bool = False,
|
|
||||||
reward_type: str = None, context: np.ndarray = None):
|
|
||||||
utils.EzPickle.__init__(**locals())
|
|
||||||
self._steps = 0
|
|
||||||
|
|
||||||
self.xml_path = os.path.join(os.path.dirname(os.path.abspath(__file__)), "assets", "biac_base.xml")
|
|
||||||
|
|
||||||
self.max_ctrl = np.array([150., 125., 40., 60., 5., 5., 2.])
|
|
||||||
|
|
||||||
self.j_min = np.array([-2.6, -1.985, -2.8, -0.9, -4.55, -1.5707, -2.7])
|
|
||||||
self.j_max = np.array([2.6, 1.985, 2.8, 3.14159, 1.25, 1.5707, 2.7])
|
|
||||||
|
|
||||||
self.context = context
|
|
||||||
self.apply_gravity_comp = apply_gravity_comp
|
|
||||||
self.simplified = simplified
|
|
||||||
|
|
||||||
self._start_pos = np.array([0.0, 0.58760536, 0.0, 1.36004913, 0.0, -0.32072943, -1.57])
|
|
||||||
self._start_vel = np.zeros(7)
|
|
||||||
|
|
||||||
self.sim_time = 8 # seconds
|
|
||||||
self._dt = 0.02
|
|
||||||
self.ep_length = 4000 # based on 8 seconds with dt = 0.02 int(self.sim_time / self.dt)
|
|
||||||
if reward_type == "no_context":
|
|
||||||
from alr_envs.alr.mujoco.ball_in_a_cup.ball_in_a_cup_reward_simple import BallInACupReward
|
|
||||||
reward_function = BallInACupReward
|
|
||||||
elif reward_type == "contextual_goal":
|
|
||||||
from alr_envs.alr.mujoco.ball_in_a_cup.ball_in_a_cup_reward import BallInACupReward
|
|
||||||
reward_function = BallInACupReward
|
|
||||||
else:
|
|
||||||
raise ValueError("Unknown reward type: {}".format(reward_type))
|
|
||||||
self.reward_function = reward_function(self)
|
|
||||||
|
|
||||||
mujoco_env.MujocoEnv.__init__(self, self.xml_path, frame_skip)
|
|
||||||
|
|
||||||
@property
|
|
||||||
def dt(self):
|
|
||||||
return self._dt
|
|
||||||
|
|
||||||
# TODO: @Max is this even needed?
|
|
||||||
@property
|
|
||||||
def start_vel(self):
|
|
||||||
if self.simplified:
|
|
||||||
return self._start_vel[1::2]
|
|
||||||
else:
|
|
||||||
return self._start_vel
|
|
||||||
|
|
||||||
# def _set_action_space(self):
|
|
||||||
# if self.simplified:
|
|
||||||
# bounds = self.model.actuator_ctrlrange.copy().astype(np.float32)[1::2]
|
|
||||||
# else:
|
|
||||||
# bounds = self.model.actuator_ctrlrange.copy().astype(np.float32)
|
|
||||||
# low, high = bounds.T
|
|
||||||
# self.action_space = spaces.Box(low=low, high=high, dtype=np.float32)
|
|
||||||
# return self.action_space
|
|
||||||
|
|
||||||
def reset(self):
|
|
||||||
self.reward_function.reset(None)
|
|
||||||
return super().reset()
|
|
||||||
|
|
||||||
def reset_model(self):
|
|
||||||
init_pos_all = self.init_qpos.copy()
|
|
||||||
init_pos_robot = self._start_pos
|
|
||||||
init_vel = np.zeros_like(init_pos_all)
|
|
||||||
|
|
||||||
self._steps = 0
|
|
||||||
self._q_pos = []
|
|
||||||
self._q_vel = []
|
|
||||||
|
|
||||||
start_pos = init_pos_all
|
|
||||||
start_pos[0:7] = init_pos_robot
|
|
||||||
|
|
||||||
self.set_state(start_pos, init_vel)
|
|
||||||
|
|
||||||
return self._get_obs()
|
|
||||||
|
|
||||||
def step(self, a):
|
|
||||||
reward_dist = 0.0
|
|
||||||
angular_vel = 0.0
|
|
||||||
reward_ctrl = - np.square(a).sum()
|
|
||||||
|
|
||||||
# if self.simplified:
|
|
||||||
# tmp = np.zeros(7)
|
|
||||||
# tmp[1::2] = a
|
|
||||||
# a = tmp
|
|
||||||
|
|
||||||
if self.apply_gravity_comp:
|
|
||||||
a += self.sim.data.qfrc_bias[:len(a)].copy() / self.model.actuator_gear[:, 0]
|
|
||||||
|
|
||||||
crash = False
|
|
||||||
try:
|
|
||||||
self.do_simulation(a, self.frame_skip)
|
|
||||||
except mujoco_py.builder.MujocoException:
|
|
||||||
crash = True
|
|
||||||
# joint_cons_viol = self.check_traj_in_joint_limits()
|
|
||||||
|
|
||||||
ob = self._get_obs()
|
|
||||||
|
|
||||||
if not crash:
|
|
||||||
reward, success, is_collided = self.reward_function.compute_reward(a)
|
|
||||||
done = success or is_collided # self._steps == self.sim_steps - 1
|
|
||||||
self._steps += 1
|
|
||||||
else:
|
|
||||||
reward = -2000
|
|
||||||
success = False
|
|
||||||
is_collided = False
|
|
||||||
done = True
|
|
||||||
|
|
||||||
return ob, reward, done, dict(reward_dist=reward_dist,
|
|
||||||
reward_ctrl=reward_ctrl,
|
|
||||||
velocity=angular_vel,
|
|
||||||
# traj=self._q_pos,
|
|
||||||
action=a,
|
|
||||||
q_pos=self.sim.data.qpos[0:7].ravel().copy(),
|
|
||||||
q_vel=self.sim.data.qvel[0:7].ravel().copy(),
|
|
||||||
is_success=success,
|
|
||||||
is_collided=is_collided, sim_crash=crash)
|
|
||||||
|
|
||||||
def check_traj_in_joint_limits(self):
|
|
||||||
return any(self.current_pos > self.j_max) or any(self.current_pos < self.j_min)
|
|
||||||
|
|
||||||
# TODO: extend observation space
|
|
||||||
def _get_obs(self):
|
|
||||||
theta = self.sim.data.qpos.flat[:7]
|
|
||||||
return np.concatenate([
|
|
||||||
np.cos(theta),
|
|
||||||
np.sin(theta),
|
|
||||||
# self.get_body_com("target"), # only return target to make problem harder
|
|
||||||
[self._steps],
|
|
||||||
])
|
|
||||||
|
|
||||||
# These functions are for the task with 3 joint actuations
|
|
||||||
def extend_des_pos(self, des_pos):
|
|
||||||
des_pos_full = self._start_pos.copy()
|
|
||||||
des_pos_full[1] = des_pos[0]
|
|
||||||
des_pos_full[3] = des_pos[1]
|
|
||||||
des_pos_full[5] = des_pos[2]
|
|
||||||
return des_pos_full
|
|
||||||
|
|
||||||
def extend_des_vel(self, des_vel):
|
|
||||||
des_vel_full = self._start_vel.copy()
|
|
||||||
des_vel_full[1] = des_vel[0]
|
|
||||||
des_vel_full[3] = des_vel[1]
|
|
||||||
des_vel_full[5] = des_vel[2]
|
|
||||||
return des_vel_full
|
|
||||||
|
|
||||||
def render(self, render_mode, **render_kwargs):
|
|
||||||
if render_mode == "plot_trajectory":
|
|
||||||
if self._steps == 1:
|
|
||||||
import matplotlib.pyplot as plt
|
|
||||||
# plt.ion()
|
|
||||||
self.fig, self.axs = plt.subplots(3, 1)
|
|
||||||
|
|
||||||
if self._steps <= 1750:
|
|
||||||
for ax, cp in zip(self.axs, self.current_pos[1::2]):
|
|
||||||
ax.scatter(self._steps, cp, s=2, marker=".")
|
|
||||||
|
|
||||||
# self.fig.show()
|
|
||||||
|
|
||||||
else:
|
|
||||||
super().render(render_mode, **render_kwargs)
|
|
||||||
|
|
||||||
|
|
||||||
if __name__ == "__main__":
|
|
||||||
env = ALRBallInACupPDEnv(reward_type="no_context", simplified=True)
|
|
||||||
# env = gym.make("alr_envs:ALRBallInACupPDSimpleDetPMP-v0")
|
|
||||||
# ctxt = np.array([-0.20869846, -0.66376693, 1.18088501])
|
|
||||||
|
|
||||||
# env.configure(ctxt)
|
|
||||||
env.reset()
|
|
||||||
env.render("human")
|
|
||||||
for i in range(16000):
|
|
||||||
# test with random actions
|
|
||||||
ac = 0.02 * env.action_space.sample()[0:7]
|
|
||||||
# ac = env.start_pos
|
|
||||||
# ac[0] += np.pi/2
|
|
||||||
obs, rew, d, info = env.step(ac)
|
|
||||||
env.render("human")
|
|
||||||
|
|
||||||
print(rew)
|
|
||||||
|
|
||||||
if d:
|
|
||||||
break
|
|
||||||
|
|
||||||
env.close()
|
|
||||||
@ -1 +0,0 @@
|
|||||||
from .mp_wrapper import MPWrapper
|
|
||||||
@ -1,238 +0,0 @@
|
|||||||
<mujoco model="wam(v1.31)">
|
|
||||||
<compiler angle="radian" meshdir="../../meshes/wam/" />
|
|
||||||
<option timestep="0.0005" integrator="Euler" />
|
|
||||||
<size njmax="500" nconmax="100" />
|
|
||||||
<default class="main">
|
|
||||||
<joint limited="true" frictionloss="0.001" damping="0.07"/>
|
|
||||||
<default class="viz">
|
|
||||||
<geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.7 0.7 0.7 1" />
|
|
||||||
</default>
|
|
||||||
<default class="col">
|
|
||||||
<geom type="mesh" contype="0" rgba="0.5 0.6 0.7 1" />
|
|
||||||
</default>
|
|
||||||
</default>
|
|
||||||
<asset>
|
|
||||||
<texture type="2d" name="groundplane" builtin="checker" mark="edge" rgb1="0.25 0.26 0.25" rgb2="0.22 0.22 0.22" markrgb="0.3 0.3 0.3" width="100" height="100" />
|
|
||||||
<material name="MatGnd" texture="groundplane" texrepeat="5 5" specular="1" shininess="0.3" reflectance="1e-05" />
|
|
||||||
<mesh name="base_link_fine" file="base_link_fine.stl" />
|
|
||||||
<mesh name="base_link_convex" file="base_link_convex.stl" />
|
|
||||||
<mesh name="shoulder_link_fine" file="shoulder_link_fine.stl" />
|
|
||||||
<mesh name="shoulder_link_convex_decomposition_p1" file="shoulder_link_convex_decomposition_p1.stl" />
|
|
||||||
<mesh name="shoulder_link_convex_decomposition_p2" file="shoulder_link_convex_decomposition_p2.stl" />
|
|
||||||
<mesh name="shoulder_link_convex_decomposition_p3" file="shoulder_link_convex_decomposition_p3.stl" />
|
|
||||||
<mesh name="shoulder_pitch_link_fine" file="shoulder_pitch_link_fine.stl" />
|
|
||||||
<mesh name="shoulder_pitch_link_convex" file="shoulder_pitch_link_convex.stl" />
|
|
||||||
<mesh name="upper_arm_link_fine" file="upper_arm_link_fine.stl" />
|
|
||||||
<mesh name="upper_arm_link_convex_decomposition_p1" file="upper_arm_link_convex_decomposition_p1.stl" />
|
|
||||||
<mesh name="upper_arm_link_convex_decomposition_p2" file="upper_arm_link_convex_decomposition_p2.stl" />
|
|
||||||
<mesh name="elbow_link_fine" file="elbow_link_fine.stl" />
|
|
||||||
<mesh name="elbow_link_convex" file="elbow_link_convex.stl" />
|
|
||||||
<mesh name="forearm_link_fine" file="forearm_link_fine.stl" />
|
|
||||||
<mesh name="forearm_link_convex_decomposition_p1" file="forearm_link_convex_decomposition_p1.stl" />
|
|
||||||
<mesh name="forearm_link_convex_decomposition_p2" file="forearm_link_convex_decomposition_p2.stl" />
|
|
||||||
<mesh name="wrist_yaw_link_fine" file="wrist_yaw_link_fine.stl" />
|
|
||||||
<mesh name="wrist_yaw_link_convex_decomposition_p1" file="wrist_yaw_link_convex_decomposition_p1.stl" />
|
|
||||||
<mesh name="wrist_yaw_link_convex_decomposition_p2" file="wrist_yaw_link_convex_decomposition_p2.stl" />
|
|
||||||
<mesh name="wrist_pitch_link_fine" file="wrist_pitch_link_fine.stl" />
|
|
||||||
<mesh name="wrist_pitch_link_convex_decomposition_p1" file="wrist_pitch_link_convex_decomposition_p1.stl" />
|
|
||||||
<mesh name="wrist_pitch_link_convex_decomposition_p2" file="wrist_pitch_link_convex_decomposition_p2.stl" />
|
|
||||||
<mesh name="wrist_pitch_link_convex_decomposition_p3" file="wrist_pitch_link_convex_decomposition_p3.stl" />
|
|
||||||
<mesh name="wrist_palm_link_fine" file="wrist_palm_link_fine.stl" />
|
|
||||||
<mesh name="wrist_palm_link_convex" file="wrist_palm_link_convex.stl" />
|
|
||||||
<mesh name="cup1" file="cup_split1.stl" scale="0.001 0.001 0.001" />
|
|
||||||
<mesh name="cup2" file="cup_split2.stl" scale="0.001 0.001 0.001" />
|
|
||||||
<mesh name="cup3" file="cup_split3.stl" scale="0.001 0.001 0.001" />
|
|
||||||
<mesh name="cup4" file="cup_split4.stl" scale="0.001 0.001 0.001" />
|
|
||||||
<mesh name="cup5" file="cup_split5.stl" scale="0.001 0.001 0.001" />
|
|
||||||
<mesh name="cup6" file="cup_split6.stl" scale="0.001 0.001 0.001" />
|
|
||||||
<mesh name="cup7" file="cup_split7.stl" scale="0.001 0.001 0.001" />
|
|
||||||
<mesh name="cup8" file="cup_split8.stl" scale="0.001 0.001 0.001" />
|
|
||||||
<mesh name="cup9" file="cup_split9.stl" scale="0.001 0.001 0.001" />
|
|
||||||
<mesh name="cup10" file="cup_split10.stl" scale="0.001 0.001 0.001" />
|
|
||||||
<mesh name="cup11" file="cup_split11.stl" scale="0.001 0.001 0.001" />
|
|
||||||
<mesh name="cup12" file="cup_split12.stl" scale="0.001 0.001 0.001" />
|
|
||||||
<mesh name="cup13" file="cup_split13.stl" scale="0.001 0.001 0.001" />
|
|
||||||
<mesh name="cup14" file="cup_split14.stl" scale="0.001 0.001 0.001" />
|
|
||||||
<mesh name="cup15" file="cup_split15.stl" scale="0.001 0.001 0.001" />
|
|
||||||
<mesh name="cup16" file="cup_split16.stl" scale="0.001 0.001 0.001" />
|
|
||||||
<mesh name="cup17" file="cup_split17.stl" scale="0.001 0.001 0.001" />
|
|
||||||
<mesh name="cup18" file="cup_split18.stl" scale="0.001 0.001 0.001" />
|
|
||||||
<mesh name="cup3_table" file="cup_split3.stl" scale="0.00211 0.00211 0.01" />
|
|
||||||
<mesh name="cup4_table" file="cup_split4.stl" scale="0.00211 0.00211 0.01" />
|
|
||||||
<mesh name="cup5_table" file="cup_split5.stl" scale="0.00211 0.00211 0.01" />
|
|
||||||
<mesh name="cup6_table" file="cup_split6.stl" scale="0.00211 0.00211 0.01" />
|
|
||||||
<mesh name="cup7_table" file="cup_split7.stl" scale="0.00211 0.00211 0.01" />
|
|
||||||
<mesh name="cup8_table" file="cup_split8.stl" scale="0.00211 0.00211 0.01" />
|
|
||||||
<mesh name="cup9_table" file="cup_split9.stl" scale="0.00211 0.00211 0.01" />
|
|
||||||
<mesh name="cup10_table" file="cup_split10.stl" scale="0.00211 0.00211 0.01" />
|
|
||||||
<mesh name="cup15_table" file="cup_split15.stl" scale="0.00211 0.00211 0.01" />
|
|
||||||
<mesh name="cup16_table" file="cup_split16.stl" scale="0.00211 0.00211 0.01" />
|
|
||||||
<mesh name="cup17_table" file="cup_split17.stl" scale="0.00211 0.00211 0.01" />
|
|
||||||
<mesh name="cup18_table" file="cup_split18.stl" scale="0.00211 0.00211 0.01" />
|
|
||||||
</asset>
|
|
||||||
|
|
||||||
<worldbody>
|
|
||||||
<geom name="ground" size="5 5 1" type="plane" material="MatGnd" />
|
|
||||||
<light pos="0.1 0.2 1.3" dir="-0.0758098 -0.32162 -0.985527" directional="true" cutoff="60" exponent="1" diffuse="1 1 1" specular="0.1 0.1 0.1" />
|
|
||||||
|
|
||||||
<body name="wam/base_link" pos="0 0 0.6">
|
|
||||||
<inertial pos="6.93764e-06 0.0542887 0.076438" quat="0.496481 0.503509 -0.503703 0.496255" mass="27.5544" diaginertia="0.432537 0.318732 0.219528" />
|
|
||||||
<geom class="viz" quat="0.707107 0 0 -0.707107" mesh="base_link_fine" />
|
|
||||||
<geom class="col" quat="0.707107 0 0 -0.707107" mesh="base_link_convex" name="base_link_convex_geom"/>
|
|
||||||
<body name="wam/shoulder_yaw_link" pos="0 0 0.16" quat="0.707107 0 0 -0.707107">
|
|
||||||
<inertial pos="-0.00443422 -0.00066489 -0.12189" quat="0.999995 0.000984795 0.00270132 0.00136071" mass="10.7677" diaginertia="0.507411 0.462983 0.113271" />
|
|
||||||
<joint name="wam/base_yaw_joint" pos="0 0 0" axis="0 0 1" range="-2.6 2.6" />
|
|
||||||
<geom class="viz" pos="0 0 0.186" mesh="shoulder_link_fine" />
|
|
||||||
<geom class="col" pos="0 0 0.186" mesh="shoulder_link_convex_decomposition_p1" name="shoulder_link_convex_decomposition_p1_geom"/>
|
|
||||||
<geom class="col" pos="0 0 0.186" mesh="shoulder_link_convex_decomposition_p2" name="shoulder_link_convex_decomposition_p2_geom"/>
|
|
||||||
<geom class="col" pos="0 0 0.186" mesh="shoulder_link_convex_decomposition_p3" name="shoulder_link_convex_decomposition_p3_geom"/>
|
|
||||||
<body name="wam/shoulder_pitch_link" pos="0 0 0.184" quat="0.707107 -0.707107 0 0">
|
|
||||||
<inertial pos="-0.00236983 -0.0154211 0.0310561" quat="0.961781 -0.272983 0.0167269 0.0133385" mass="3.87494" diaginertia="0.0214207 0.0167101 0.0126465" />
|
|
||||||
<joint name="wam/shoulder_pitch_joint" pos="0 0 0" axis="0 0 1" range="-1.985 1.985" />
|
|
||||||
<geom class="viz" mesh="shoulder_pitch_link_fine" />
|
|
||||||
<geom class="col" mesh="shoulder_pitch_link_convex" />
|
|
||||||
<body name="wam/upper_arm_link" pos="0 -0.505 0" quat="0.707107 0.707107 0 0">
|
|
||||||
<inertial pos="-0.0382586 3.309e-05 -0.207508" quat="0.705455 0.0381914 0.0383402 0.706686" mass="1.80228" diaginertia="0.0665697 0.0634285 0.00622701" />
|
|
||||||
<joint name="wam/shoulder_yaw_joint" pos="0 0 0" axis="0 0 1" range="-2.8 2.8" />
|
|
||||||
<geom class="viz" pos="0 0 -0.505" mesh="upper_arm_link_fine" />
|
|
||||||
<geom class="col" pos="0 0 -0.505" mesh="upper_arm_link_convex_decomposition_p1" name="upper_arm_link_convex_decomposition_p1_geom"/>
|
|
||||||
<geom class="col" pos="0 0 -0.505" mesh="upper_arm_link_convex_decomposition_p2" name="upper_arm_link_convex_decomposition_p2_geom"/>
|
|
||||||
<body name="wam/forearm_link" pos="0.045 0 0.045" quat="0.707107 -0.707107 0 0">
|
|
||||||
<inertial pos="0.00498512 -0.132717 -0.00022942" quat="0.546303 0.447151 -0.548676 0.447842" mass="2.40017" diaginertia="0.0196896 0.0152225 0.00749914" />
|
|
||||||
<joint name="wam/elbow_pitch_joint" pos="0 0 0" axis="0 0 1" range="-0.9 3.14159" />
|
|
||||||
<geom class="viz" mesh="elbow_link_fine" />
|
|
||||||
<geom class="col" mesh="elbow_link_convex" />
|
|
||||||
<geom class="viz" pos="-0.045 -0.073 0" quat="0.707388 0.706825 0 0" mesh="forearm_link_fine" />
|
|
||||||
<geom class="col" pos="-0.045 -0.073 0" quat="0.707388 0.706825 0 0" mesh="forearm_link_convex_decomposition_p1" name="forearm_link_convex_decomposition_p1_geom" />
|
|
||||||
<geom class="col" pos="-0.045 -0.073 0" quat="0.707388 0.706825 0 0" mesh="forearm_link_convex_decomposition_p2" name="forearm_link_convex_decomposition_p2_geom" />
|
|
||||||
<body name="wam/wrist_yaw_link" pos="-0.045 0 0" quat="0.707107 0.707107 0 0">
|
|
||||||
<inertial pos="8.921e-05 0.00435824 -0.00511217" quat="0.708528 -0.000120667 0.000107481 0.705683" mass="0.12376" diaginertia="0.0112011 0.0111887 7.58188e-05" />
|
|
||||||
<joint name="wam/wrist_yaw_joint" pos="0 0 0" axis="0 0 1" range="-4.55 1.25" />
|
|
||||||
<geom class="viz" pos="0 0 0.3" mesh="wrist_yaw_link_fine" />
|
|
||||||
<geom class="col" pos="0 0 0.3" mesh="wrist_yaw_link_convex_decomposition_p1" name="wrist_yaw_link_convex_decomposition_p1_geom" />
|
|
||||||
<geom class="col" pos="0 0 0.3" mesh="wrist_yaw_link_convex_decomposition_p2" name="wrist_yaw_link_convex_decomposition_p2_geom" />
|
|
||||||
<body name="wam/wrist_pitch_link" pos="0 0 0.3" quat="0.707107 -0.707107 0 0">
|
|
||||||
<inertial pos="-0.00012262 -0.0246834 -0.0170319" quat="0.994687 -0.102891 0.000824211 -0.00336105" mass="0.417974" diaginertia="0.000555166 0.000463174 0.00023407" />
|
|
||||||
<joint name="wam/wrist_pitch_joint" pos="0 0 0" axis="0 0 1" range="-1.5707 1.5707" />
|
|
||||||
<geom class="viz" mesh="wrist_pitch_link_fine" />
|
|
||||||
<geom class="col" mesh="wrist_pitch_link_convex_decomposition_p1" name="wrist_pitch_link_convex_decomposition_p1_geom" />
|
|
||||||
<geom class="col" mesh="wrist_pitch_link_convex_decomposition_p2" name="wrist_pitch_link_convex_decomposition_p2_geom" />
|
|
||||||
<geom class="col" mesh="wrist_pitch_link_convex_decomposition_p3" name="wrist_pitch_link_convex_decomposition_p3_geom" />
|
|
||||||
<body name="wam/wrist_palm_link" pos="0 -0.06 0" quat="0.707107 0.707107 0 0">
|
|
||||||
<inertial pos="-7.974e-05 -0.00323552 -0.00016313" quat="0.594752 0.382453 0.382453 0.594752" mass="0.0686475" diaginertia="7.408e-05 3.81466e-05 3.76434e-05" />
|
|
||||||
<joint name="wam/palm_yaw_joint" pos="0 0 0" axis="0 0 1" range="-2.7 2.7" />
|
|
||||||
<geom class="viz" pos="0 0 -0.06" mesh="wrist_palm_link_fine" />
|
|
||||||
<geom class="col" pos="0 0 -0.06" mesh="wrist_palm_link_convex" name="wrist_palm_link_convex_geom" />
|
|
||||||
<body name="cup" pos="0 0 0" quat="-0.000203673 0 0 1">
|
|
||||||
<inertial pos="-3.75236e-10 8.27811e-05 0.0947015" quat="0.999945 -0.0104888 0 0" mass="0.132" diaginertia="0.000285643 0.000270485 9.65696e-05" />
|
|
||||||
<geom priority="1" name="cup_geom1" pos="0 0.05 0.055" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup1" />
|
|
||||||
<geom priority="1" name="cup_geom2" pos="0 0.05 0.055" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup2" />
|
|
||||||
<geom priority="1" name="cup_geom3" pos="0 0.015 0.055" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup3" />
|
|
||||||
<geom priority="1" name="cup_geom4" pos="0 0.015 0.055" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup4" />
|
|
||||||
<geom priority="1" name="cup_geom5" pos="0 0.015 0.055" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup5" />
|
|
||||||
<geom priority="1" name="cup_geom6" pos="0 0.015 0.055" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup6" />
|
|
||||||
<geom priority="1" name="cup_geom7" pos="0 0.015 0.055" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup7" />
|
|
||||||
<geom priority="1" name="cup_geom8" pos="0 0.015 0.055" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup8" />
|
|
||||||
<geom priority="1" name="cup_geom9" pos="0 0.015 0.055" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup9" />
|
|
||||||
<geom priority="1" name="cup_geom10" pos="0 0.015 0.055" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup10" />
|
|
||||||
<geom name="cup_base" pos="0 -0.05 0.1165" euler="-1.57 0 0" type="cylinder" size="0.038 0.025" solref="-10000 -100"/>
|
|
||||||
<geom name="cup_base_contact" pos="0 -0.005 0.1165" euler="-1.57 0 0" type="cylinder" size="0.03 0.0005" solref="-10000 -100" rgba="0 0 255 1"/>
|
|
||||||
|
|
||||||
<geom priority="1" name="cup_geom15" pos="0 0.015 0.055" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup15" />
|
|
||||||
<geom priority="1" name="cup_geom16" pos="0 0.015 0.055" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup16" />
|
|
||||||
<geom priority="1" name="cup_geom17" pos="0 0.015 0.055" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup17" />
|
|
||||||
<geom priority="1" name="cup_geom18" pos="0 0.015 0.055" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup18" />
|
|
||||||
<site name="cup_robot" pos="0 0.05 0.1165" rgba="255 0 0 1"/>
|
|
||||||
<site name="cup_robot_final" pos="0 -0.025 0.1165" rgba="0 255 0 1"/>
|
|
||||||
</body>
|
|
||||||
</body>
|
|
||||||
</body>
|
|
||||||
</body>
|
|
||||||
</body>
|
|
||||||
</body>
|
|
||||||
</body>
|
|
||||||
</body>
|
|
||||||
</body>
|
|
||||||
<body name="table_body" pos="0 -1.85 0.4025">
|
|
||||||
<geom name="table" type="box" size="0.4 0.6 0.4" rgba="0.8 0.655 0.45 1" solimp="0.999 0.999 0.001"
|
|
||||||
solref="-10000 -100"/>
|
|
||||||
<geom name="table_contact_geom" type="box" size="0.4 0.6 0.01" pos="0 0 0.41" rgba="1.4 0.8 0.45 1" solimp="0.999 0.999 0.001"
|
|
||||||
solref="-10000 -100"/>
|
|
||||||
</body>
|
|
||||||
<geom name="table_robot" type="box" size="0.1 0.1 0.3" pos="0 0.00 0.3025" rgba="0.8 0.655 0.45 1" solimp="0.999 0.999 0.001"
|
|
||||||
solref="-10000 -100"/>
|
|
||||||
<geom name="wall" type="box" quat="1 0 0 0" size="0.4 0.04 1.1" pos="0. -2.45 1.1" rgba="0.8 0.655 0.45 1" solimp="0.999 0.999 0.001"
|
|
||||||
solref="-10000 -100"/>
|
|
||||||
<!-- <geom name="side_wall_left" type="box" quat="1 0 0 0" size="0.04 0.4 0.5" pos="0.45 -2.05 1.1" rgba="0.8 0.655 0.45 1" solimp="0.999 0.999 0.001"-->
|
|
||||||
<!-- solref="-10000 -100"/>-->
|
|
||||||
<!-- <geom name="side_wall_right" type="box" quat="1 0 0 0" size="0.04 0.4 0.5" pos="-0.45 -2.05 1.1" rgba="0.8 0.655 0.45 1" solimp="0.999 0.999 0.001"-->
|
|
||||||
<!-- solref="-10000 -100"/>-->
|
|
||||||
|
|
||||||
<!-- <body name="cup_table" pos="0.32 -1.55 0.84" quat="0.7071068 0.7071068 0 0">-->
|
|
||||||
<!-- <geom priority= "1" type="box" size ="0.1 0.1 0.1" name="cup_base_table" pos="0 0.1 0.001" euler="-1.57 0 0" solref="-10000 -10" mass="10"/>-->
|
|
||||||
<!-- </body>-->
|
|
||||||
|
|
||||||
<body name="cup_table" pos="0.32 -1.55 0.84" quat="0.7071068 0.7071068 0 0">
|
|
||||||
<inertial pos="-3.75236e-10 8.27811e-05 0.0947015" quat="0.999945 -0.0104888 0 0" mass="10.132" diaginertia="0.000285643 0.000270485 9.65696e-05" />
|
|
||||||
<geom priority= "1" name="cup_geom_table3" pos="0 0.1 0.001" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup3_table" mass="10"/>
|
|
||||||
<geom priority= "1" name="cup_geom_table4" pos="0 0.1 0.001" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup4_table" mass="10"/>
|
|
||||||
<geom priority= "1" name="cup_geom_table5" pos="0 0.1 0.001" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup5_table" mass="10"/>
|
|
||||||
<geom priority= "1" name="cup_geom_table6" pos="0 0.1 0.001" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup6_table" mass="10"/>
|
|
||||||
<geom priority= "1" name="cup_geom_table7" pos="0 0.1 0.001" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup7_table" mass="10"/>
|
|
||||||
<geom priority= "1" name="cup_geom_table8" pos="0 0.1 0.001" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup8_table" mass="10"/>
|
|
||||||
<geom priority= "1" name="cup_geom_table9" pos="0 0.1 0.001" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup9_table" mass="10"/>
|
|
||||||
<geom priority= "1" name="cup_geom_table10" pos="0 0.1 0.001" euler="-1.57 0 0" solref="-10000 -010" type="mesh" mesh="cup10_table" mass="10"/>
|
|
||||||
<geom priority= "1" name="cup_base_table" pos="0 -0.035 0.1337249" euler="-1.57 0 0" type="cylinder" size="0.08 0.045" solref="-10000 -100" mass="10"/>
|
|
||||||
<geom priority= "1" name="cup_base_table_contact" pos="0 0.015 0.1337249" euler="-1.57 0 0" type="cylinder" size="0.06 0.0005" solref="-10000 -100" rgba="0 0 255 1" mass="10"/>
|
|
||||||
<geom priority= "1" name="cup_geom_table15" pos="0 0.1 0.001" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup15_table" mass="10"/>
|
|
||||||
<geom priority= "1" name="cup_geom_table16" pos="0 0.1 0.001" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup16_table" mass="10"/>
|
|
||||||
<geom priority= "1" name="cup_geom_table17" pos="0 0.1 0.001" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup17_table" mass="10"/>
|
|
||||||
<geom priority= "1" name="cup_geom1_table8" pos="0 0.1 0.001" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup18_table" mass="10"/>
|
|
||||||
<site name="cup_goal_table" pos="0 0.11 0.1337249" rgba="255 0 0 1"/>
|
|
||||||
<site name="cup_goal_final_table" pos="0.0 0.025 0.1337249" rgba="0 255 0 1"/>
|
|
||||||
<site name="bounce_table" pos="0.0 -0.015 -0.2 " rgba="255 255 0 1"/>
|
|
||||||
|
|
||||||
<!-- <site name="cup_goal_final_table" pos="0.0 -0.025 0.1337249" rgba="0 255 0 1"/>-->
|
|
||||||
</body>
|
|
||||||
<!-- <body name="ball" pos="0.0 -0.813 2.382">-->
|
|
||||||
<body name="ball" pos="0.0 -0.813 2.382">
|
|
||||||
<joint axis="1 0 0" damping="0.0" name="tar:x" pos="0 0 0" stiffness="0" type="slide" frictionloss="0" limited="false"/>
|
|
||||||
<joint axis="0 1 0" damping="0.0" name="tar:y" pos="0 0 0" stiffness="0" type="slide" frictionloss="0" limited="false"/>
|
|
||||||
<joint axis="0 0 1" damping="0.0" name="tar:z" pos="0 0 0" stiffness="0" type="slide" frictionloss="0" limited="false"/>
|
|
||||||
<geom priority= "1" size="0.025 0.025 0.025" type="sphere" condim="4" name="ball_geom" rgba="0.8 0.2 0.1 1" mass="0.1"
|
|
||||||
friction="0.1 0.1 0.1" solimp="0.9 0.95 0.001 0.5 2" solref="-10000 -10"/>
|
|
||||||
<!-- friction="0.1 0.1 0.1" solimp="1 1 0" solref="0.1 0.03"/>-->
|
|
||||||
<site name="target_ball" pos="0 0 0" size="0.02 0.02 0.02" rgba="1 0 0 1" type="sphere"/>
|
|
||||||
</body>
|
|
||||||
<camera name="visualization" mode="targetbody" target="wam/wrist_yaw_link" pos="1.5 -0.4 2.2"/>
|
|
||||||
<camera name="experiment" mode="fixed" quat="0.44418059 0.41778323 0.54301123 0.57732103" pos="1.5 -0.3 1.33" />
|
|
||||||
<site name="test" pos="0.1 0.1 0.1" rgba="0 0 1 1" type="sphere"/>
|
|
||||||
</worldbody>
|
|
||||||
<!-- <actuator>-->
|
|
||||||
<!-- <position ctrlrange="-2.6 2.6" joint="wam/base_yaw_joint" kp="800"/>-->
|
|
||||||
<!-- <position ctrlrange="-1.985 1.985" joint="wam/shoulder_pitch_joint" kp="800"/>-->
|
|
||||||
<!-- <position ctrlrange="-2.8 2.8" joint="wam/shoulder_yaw_joint" kp="800"/>-->
|
|
||||||
<!-- <position ctrlrange="-0.9 3.14159" joint="wam/elbow_pitch_joint" kp="800"/>-->
|
|
||||||
<!-- <position ctrlrange="-4.55 1.25" joint="wam/wrist_yaw_joint" kp="100"/>-->
|
|
||||||
<!-- <position ctrlrange="-1.5707 1.5707" joint="wam/wrist_pitch_joint" kp="2000"/>-->
|
|
||||||
<!-- <position ctrlrange="-2.7 2.7" joint="wam/palm_yaw_joint" kp="100"/>-->
|
|
||||||
<!-- </actuator>-->
|
|
||||||
<!-- <actuator>-->
|
|
||||||
<!-- <motor ctrlrange="-150 150" joint="wam/base_yaw_joint"/>-->
|
|
||||||
<!-- <motor ctrlrange="-125 125" joint="wam/shoulder_pitch_joint"/>-->
|
|
||||||
<!-- <motor ctrlrange="-40 40" joint="wam/shoulder_yaw_joint"/>-->
|
|
||||||
<!-- <motor ctrlrange="-60 60" joint="wam/elbow_pitch_joint"/>-->
|
|
||||||
<!-- <motor ctrlrange="-5 5" joint="wam/wrist_yaw_joint"/>-->
|
|
||||||
<!-- <motor ctrlrange="-5 5" joint="wam/wrist_pitch_joint"/>-->
|
|
||||||
<!-- <motor ctrlrange="-2 2" joint="wam/palm_yaw_joint"/>-->
|
|
||||||
<!-- </actuator>-->
|
|
||||||
<actuator>
|
|
||||||
<motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="150.0" joint="wam/base_yaw_joint"/>
|
|
||||||
<motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="125.0" joint="wam/shoulder_pitch_joint"/>
|
|
||||||
<motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="40.0" joint="wam/shoulder_yaw_joint"/>
|
|
||||||
<motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="60.0" joint="wam/elbow_pitch_joint"/>
|
|
||||||
<motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="5.0" joint="wam/wrist_yaw_joint"/>
|
|
||||||
<motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="5.0" joint="wam/wrist_pitch_joint"/>
|
|
||||||
<motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="2.0" joint="wam/palm_yaw_joint"/>
|
|
||||||
</actuator>
|
|
||||||
</mujoco>
|
|
||||||
@ -1,187 +0,0 @@
|
|||||||
<mujoco model="wam(v1.31)">
|
|
||||||
<compiler angle="radian" meshdir="../../meshes/wam/" />
|
|
||||||
<option timestep="0.005" integrator="Euler" />
|
|
||||||
<size njmax="500" nconmax="100" />
|
|
||||||
<default class="main">
|
|
||||||
<joint limited="true" frictionloss="0.001" damping="0.07"/>
|
|
||||||
<default class="viz">
|
|
||||||
<geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.7 0.7 0.7 1" />
|
|
||||||
</default>
|
|
||||||
<default class="col">
|
|
||||||
<geom type="mesh" contype="0" rgba="0.5 0.6 0.7 1" />
|
|
||||||
</default>
|
|
||||||
</default>
|
|
||||||
<asset>
|
|
||||||
<texture type="2d" name="groundplane" builtin="checker" mark="edge" rgb1="0.25 0.26 0.25" rgb2="0.22 0.22 0.22" markrgb="0.3 0.3 0.3" width="100" height="100" />
|
|
||||||
<material name="MatGnd" texture="groundplane" texrepeat="5 5" specular="1" shininess="0.3" reflectance="1e-05" />
|
|
||||||
<mesh name="base_link_fine" file="base_link_fine.stl" />
|
|
||||||
<mesh name="base_link_convex" file="base_link_convex.stl" />
|
|
||||||
<mesh name="shoulder_link_fine" file="shoulder_link_fine.stl" />
|
|
||||||
<mesh name="shoulder_link_convex_decomposition_p1" file="shoulder_link_convex_decomposition_p1.stl" />
|
|
||||||
<mesh name="shoulder_link_convex_decomposition_p2" file="shoulder_link_convex_decomposition_p2.stl" />
|
|
||||||
<mesh name="shoulder_link_convex_decomposition_p3" file="shoulder_link_convex_decomposition_p3.stl" />
|
|
||||||
<mesh name="shoulder_pitch_link_fine" file="shoulder_pitch_link_fine.stl" />
|
|
||||||
<mesh name="shoulder_pitch_link_convex" file="shoulder_pitch_link_convex.stl" />
|
|
||||||
<mesh name="upper_arm_link_fine" file="upper_arm_link_fine.stl" />
|
|
||||||
<mesh name="upper_arm_link_convex_decomposition_p1" file="upper_arm_link_convex_decomposition_p1.stl" />
|
|
||||||
<mesh name="upper_arm_link_convex_decomposition_p2" file="upper_arm_link_convex_decomposition_p2.stl" />
|
|
||||||
<mesh name="elbow_link_fine" file="elbow_link_fine.stl" />
|
|
||||||
<mesh name="elbow_link_convex" file="elbow_link_convex.stl" />
|
|
||||||
<mesh name="forearm_link_fine" file="forearm_link_fine.stl" />
|
|
||||||
<mesh name="forearm_link_convex_decomposition_p1" file="forearm_link_convex_decomposition_p1.stl" />
|
|
||||||
<mesh name="forearm_link_convex_decomposition_p2" file="forearm_link_convex_decomposition_p2.stl" />
|
|
||||||
<mesh name="wrist_yaw_link_fine" file="wrist_yaw_link_fine.stl" />
|
|
||||||
<mesh name="wrist_yaw_link_convex_decomposition_p1" file="wrist_yaw_link_convex_decomposition_p1.stl" />
|
|
||||||
<mesh name="wrist_yaw_link_convex_decomposition_p2" file="wrist_yaw_link_convex_decomposition_p2.stl" />
|
|
||||||
<mesh name="wrist_pitch_link_fine" file="wrist_pitch_link_fine.stl" />
|
|
||||||
<mesh name="wrist_pitch_link_convex_decomposition_p1" file="wrist_pitch_link_convex_decomposition_p1.stl" />
|
|
||||||
<mesh name="wrist_pitch_link_convex_decomposition_p2" file="wrist_pitch_link_convex_decomposition_p2.stl" />
|
|
||||||
<mesh name="wrist_pitch_link_convex_decomposition_p3" file="wrist_pitch_link_convex_decomposition_p3.stl" />
|
|
||||||
<mesh name="wrist_palm_link_fine" file="wrist_palm_link_fine.stl" />
|
|
||||||
<mesh name="wrist_palm_link_convex" file="wrist_palm_link_convex.stl" />
|
|
||||||
<mesh name="cup1" file="cup_split1.stl" scale="0.001 0.001 0.001" />
|
|
||||||
<mesh name="cup2" file="cup_split2.stl" scale="0.001 0.001 0.001" />
|
|
||||||
<mesh name="cup3" file="cup_split3.stl" scale="0.001 0.001 0.001" />
|
|
||||||
<mesh name="cup4" file="cup_split4.stl" scale="0.001 0.001 0.001" />
|
|
||||||
<mesh name="cup5" file="cup_split5.stl" scale="0.001 0.001 0.001" />
|
|
||||||
<mesh name="cup6" file="cup_split6.stl" scale="0.001 0.001 0.001" />
|
|
||||||
<mesh name="cup7" file="cup_split7.stl" scale="0.001 0.001 0.001" />
|
|
||||||
<mesh name="cup8" file="cup_split8.stl" scale="0.001 0.001 0.001" />
|
|
||||||
<mesh name="cup9" file="cup_split9.stl" scale="0.001 0.001 0.001" />
|
|
||||||
<mesh name="cup10" file="cup_split10.stl" scale="0.001 0.001 0.001" />
|
|
||||||
<mesh name="cup11" file="cup_split11.stl" scale="0.001 0.001 0.001" />
|
|
||||||
<mesh name="cup12" file="cup_split12.stl" scale="0.001 0.001 0.001" />
|
|
||||||
<mesh name="cup13" file="cup_split13.stl" scale="0.001 0.001 0.001" />
|
|
||||||
<mesh name="cup14" file="cup_split14.stl" scale="0.001 0.001 0.001" />
|
|
||||||
<mesh name="cup15" file="cup_split15.stl" scale="0.001 0.001 0.001" />
|
|
||||||
<mesh name="cup16" file="cup_split16.stl" scale="0.001 0.001 0.001" />
|
|
||||||
<mesh name="cup17" file="cup_split17.stl" scale="0.001 0.001 0.001" />
|
|
||||||
<mesh name="cup18" file="cup_split18.stl" scale="0.001 0.001 0.001" />
|
|
||||||
<mesh name="cup3_table" file="cup_split3.stl" scale="0.00211 0.00211 0.01" />
|
|
||||||
<mesh name="cup4_table" file="cup_split4.stl" scale="0.00211 0.00211 0.01" />
|
|
||||||
<mesh name="cup5_table" file="cup_split5.stl" scale="0.00211 0.00211 0.01" />
|
|
||||||
<mesh name="cup6_table" file="cup_split6.stl" scale="0.00211 0.00211 0.01" />
|
|
||||||
<mesh name="cup7_table" file="cup_split7.stl" scale="0.00211 0.00211 0.01" />
|
|
||||||
<mesh name="cup8_table" file="cup_split8.stl" scale="0.00211 0.00211 0.01" />
|
|
||||||
<mesh name="cup9_table" file="cup_split9.stl" scale="0.00211 0.00211 0.01" />
|
|
||||||
<mesh name="cup10_table" file="cup_split10.stl" scale="0.00211 0.00211 0.01" />
|
|
||||||
<mesh name="cup15_table" file="cup_split15.stl" scale="0.00211 0.00211 0.01" />
|
|
||||||
<mesh name="cup16_table" file="cup_split16.stl" scale="0.00211 0.00211 0.01" />
|
|
||||||
<mesh name="cup17_table" file="cup_split17.stl" scale="0.00211 0.00211 0.01" />
|
|
||||||
<mesh name="cup18_table" file="cup_split18.stl" scale="0.00211 0.00211 0.01" />
|
|
||||||
</asset>
|
|
||||||
|
|
||||||
<worldbody>
|
|
||||||
<geom name="ground" size="5 5 1" type="plane" material="MatGnd" />
|
|
||||||
<light pos="0.1 0.2 1.3" dir="-0.0758098 -0.32162 -0.985527" directional="true" cutoff="60" exponent="1" diffuse="1 1 1" specular="0.1 0.1 0.1" />
|
|
||||||
|
|
||||||
<body name="wam/base_link" pos="0 0 0.6">
|
|
||||||
<inertial pos="6.93764e-06 0.0542887 0.076438" quat="0.496481 0.503509 -0.503703 0.496255" mass="27.5544" diaginertia="0.432537 0.318732 0.219528" />
|
|
||||||
<geom class="viz" quat="0.707107 0 0 -0.707107" mesh="base_link_fine" />
|
|
||||||
<geom class="col" quat="0.707107 0 0 -0.707107" mesh="base_link_convex" name="base_link_convex_geom"/>
|
|
||||||
<body name="wam/shoulder_yaw_link" pos="0 0 0.16" quat="0.707107 0 0 -0.707107">
|
|
||||||
<inertial pos="-0.00443422 -0.00066489 -0.12189" quat="0.999995 0.000984795 0.00270132 0.00136071" mass="10.7677" diaginertia="0.507411 0.462983 0.113271" />
|
|
||||||
<joint name="wam/base_yaw_joint" pos="0 0 0" axis="0 0 1" range="-2.6 2.6" />
|
|
||||||
<geom class="viz" pos="0 0 0.186" mesh="shoulder_link_fine" />
|
|
||||||
<geom class="col" pos="0 0 0.186" mesh="shoulder_link_convex_decomposition_p1" name="shoulder_link_convex_decomposition_p1_geom"/>
|
|
||||||
<geom class="col" pos="0 0 0.186" mesh="shoulder_link_convex_decomposition_p2" name="shoulder_link_convex_decomposition_p2_geom"/>
|
|
||||||
<geom class="col" pos="0 0 0.186" mesh="shoulder_link_convex_decomposition_p3" name="shoulder_link_convex_decomposition_p3_geom"/>
|
|
||||||
<body name="wam/shoulder_pitch_link" pos="0 0 0.184" quat="0.707107 -0.707107 0 0">
|
|
||||||
<inertial pos="-0.00236983 -0.0154211 0.0310561" quat="0.961781 -0.272983 0.0167269 0.0133385" mass="3.87494" diaginertia="0.0214207 0.0167101 0.0126465" />
|
|
||||||
<joint name="wam/shoulder_pitch_joint" pos="0 0 0" axis="0 0 1" range="-1.985 1.985" />
|
|
||||||
<geom class="viz" mesh="shoulder_pitch_link_fine" />
|
|
||||||
<geom class="col" mesh="shoulder_pitch_link_convex" />
|
|
||||||
<body name="wam/upper_arm_link" pos="0 -0.505 0" quat="0.707107 0.707107 0 0">
|
|
||||||
<inertial pos="-0.0382586 3.309e-05 -0.207508" quat="0.705455 0.0381914 0.0383402 0.706686" mass="1.80228" diaginertia="0.0665697 0.0634285 0.00622701" />
|
|
||||||
<joint name="wam/shoulder_yaw_joint" pos="0 0 0" axis="0 0 1" range="-2.8 2.8" />
|
|
||||||
<geom class="viz" pos="0 0 -0.505" mesh="upper_arm_link_fine" />
|
|
||||||
<geom class="col" pos="0 0 -0.505" mesh="upper_arm_link_convex_decomposition_p1" name="upper_arm_link_convex_decomposition_p1_geom"/>
|
|
||||||
<geom class="col" pos="0 0 -0.505" mesh="upper_arm_link_convex_decomposition_p2" name="upper_arm_link_convex_decomposition_p2_geom"/>
|
|
||||||
<body name="wam/forearm_link" pos="0.045 0 0.045" quat="0.707107 -0.707107 0 0">
|
|
||||||
<inertial pos="0.00498512 -0.132717 -0.00022942" quat="0.546303 0.447151 -0.548676 0.447842" mass="2.40017" diaginertia="0.0196896 0.0152225 0.00749914" />
|
|
||||||
<joint name="wam/elbow_pitch_joint" pos="0 0 0" axis="0 0 1" range="-0.9 3.14159" />
|
|
||||||
<geom class="viz" mesh="elbow_link_fine" />
|
|
||||||
<geom class="col" mesh="elbow_link_convex" />
|
|
||||||
<geom class="viz" pos="-0.045 -0.073 0" quat="0.707388 0.706825 0 0" mesh="forearm_link_fine" />
|
|
||||||
<geom class="col" pos="-0.045 -0.073 0" quat="0.707388 0.706825 0 0" mesh="forearm_link_convex_decomposition_p1" name="forearm_link_convex_decomposition_p1_geom" />
|
|
||||||
<geom class="col" pos="-0.045 -0.073 0" quat="0.707388 0.706825 0 0" mesh="forearm_link_convex_decomposition_p2" name="forearm_link_convex_decomposition_p2_geom" />
|
|
||||||
<body name="wam/wrist_yaw_link" pos="-0.045 0 0" quat="0.707107 0.707107 0 0">
|
|
||||||
<inertial pos="8.921e-05 0.00435824 -0.00511217" quat="0.708528 -0.000120667 0.000107481 0.705683" mass="0.12376" diaginertia="0.0112011 0.0111887 7.58188e-05" />
|
|
||||||
<joint name="wam/wrist_yaw_joint" pos="0 0 0" axis="0 0 1" range="-4.55 1.25" />
|
|
||||||
<geom class="viz" pos="0 0 0.3" mesh="wrist_yaw_link_fine" />
|
|
||||||
<geom class="col" pos="0 0 0.3" mesh="wrist_yaw_link_convex_decomposition_p1" name="wrist_yaw_link_convex_decomposition_p1_geom" />
|
|
||||||
<geom class="col" pos="0 0 0.3" mesh="wrist_yaw_link_convex_decomposition_p2" name="wrist_yaw_link_convex_decomposition_p2_geom" />
|
|
||||||
<body name="wam/wrist_pitch_link" pos="0 0 0.3" quat="0.707107 -0.707107 0 0">
|
|
||||||
<inertial pos="-0.00012262 -0.0246834 -0.0170319" quat="0.994687 -0.102891 0.000824211 -0.00336105" mass="0.417974" diaginertia="0.000555166 0.000463174 0.00023407" />
|
|
||||||
<joint name="wam/wrist_pitch_joint" pos="0 0 0" axis="0 0 1" range="-1.5707 1.5707" />
|
|
||||||
<geom class="viz" mesh="wrist_pitch_link_fine" />
|
|
||||||
<geom class="col" mesh="wrist_pitch_link_convex_decomposition_p1" name="wrist_pitch_link_convex_decomposition_p1_geom" />
|
|
||||||
<geom class="col" mesh="wrist_pitch_link_convex_decomposition_p2" name="wrist_pitch_link_convex_decomposition_p2_geom" />
|
|
||||||
<geom class="col" mesh="wrist_pitch_link_convex_decomposition_p3" name="wrist_pitch_link_convex_decomposition_p3_geom" />
|
|
||||||
<body name="wam/wrist_palm_link" pos="0 -0.06 0" quat="0.707107 0.707107 0 0">
|
|
||||||
<inertial pos="-7.974e-05 -0.00323552 -0.00016313" quat="0.594752 0.382453 0.382453 0.594752" mass="0.0686475" diaginertia="7.408e-05 3.81466e-05 3.76434e-05" />
|
|
||||||
<joint name="wam/palm_yaw_joint" pos="0 0 0" axis="0 0 1" range="-2.7 2.7" />
|
|
||||||
<geom class="viz" pos="0 0 -0.06" mesh="wrist_palm_link_fine" />
|
|
||||||
<geom class="col" pos="0 0 -0.06" mesh="wrist_palm_link_convex" name="wrist_palm_link_convex_geom" />
|
|
||||||
<site name="init_ball_pos_site" size="0.025 0.025 0.025" pos="0.0 0.0 0.035" rgba="0 1 0 1"/>
|
|
||||||
</body>
|
|
||||||
</body>
|
|
||||||
</body>
|
|
||||||
</body>
|
|
||||||
</body>
|
|
||||||
</body>
|
|
||||||
</body>
|
|
||||||
</body>
|
|
||||||
<body name="table_body" pos="0 -1.85 0.4025">
|
|
||||||
<geom name="table" type="box" size="0.4 0.6 0.4" rgba="0.8 0.655 0.45 1" solimp="0.999 0.999 0.001"
|
|
||||||
solref="-10000 -100"/>
|
|
||||||
<geom name="table_contact_geom" type="box" size="0.4 0.6 0.1" pos="0 0 0.31" rgba="1.4 0.8 0.45 1" solimp="0.999 0.999 0.001"
|
|
||||||
solref="-10000 -100"/>
|
|
||||||
</body>
|
|
||||||
<geom name="table_robot" type="box" size="0.1 0.1 0.3" pos="0 0.00 0.3025" rgba="0.8 0.655 0.45 1" solimp="0.999 0.999 0.001"
|
|
||||||
solref="-10000 -100"/>
|
|
||||||
<geom name="wall" type="box" quat="1 0 0 0" size="0.4 0.04 1.1" pos="0. -2.45 1.1" rgba="0.8 0.655 0.45 1" solimp="0.999 0.999 0.001"
|
|
||||||
solref="-10000 -100"/>
|
|
||||||
|
|
||||||
<body name="cup_table" pos="0.32 -1.55 0.84" quat="0.7071068 0.7071068 0 0">
|
|
||||||
<inertial pos="-3.75236e-10 8.27811e-05 0.0947015" quat="0.999945 -0.0104888 0 0" mass="10.132" diaginertia="0.000285643 0.000270485 9.65696e-05" />
|
|
||||||
<geom priority= "1" name="cup_geom_table3" pos="0 0.1 0.001" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup3_table" mass="10"/>
|
|
||||||
<geom priority= "1" name="cup_geom_table4" pos="0 0.1 0.001" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup4_table" mass="10"/>
|
|
||||||
<geom priority= "1" name="cup_geom_table5" pos="0 0.1 0.001" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup5_table" mass="10"/>
|
|
||||||
<geom priority= "1" name="cup_geom_table6" pos="0 0.1 0.001" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup6_table" mass="10"/>
|
|
||||||
<geom priority= "1" name="cup_geom_table7" pos="0 0.1 0.001" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup7_table" mass="10"/>
|
|
||||||
<geom priority= "1" name="cup_geom_table8" pos="0 0.1 0.001" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup8_table" mass="10"/>
|
|
||||||
<geom priority= "1" name="cup_geom_table9" pos="0 0.1 0.001" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup9_table" mass="10"/>
|
|
||||||
<geom priority= "1" name="cup_geom_table10" pos="0 0.1 0.001" euler="-1.57 0 0" solref="-10000 -010" type="mesh" mesh="cup10_table" mass="10"/>
|
|
||||||
<geom priority= "1" name="cup_base_table" pos="0 -0.035 0.1337249" euler="-1.57 0 0" type="cylinder" size="0.08 0.045" solref="-10000 -100" mass="10"/>
|
|
||||||
<geom priority= "1" name="cup_base_table_contact" pos="0 0.015 0.1337249" euler="-1.57 0 0" type="cylinder" size="0.07 0.01" solref="-10000 -100" rgba="0 0 255 1" mass="10"/>
|
|
||||||
<geom priority= "1" name="cup_geom_table15" pos="0 0.1 0.001" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup15_table" mass="10"/>
|
|
||||||
<geom priority= "1" name="cup_geom_table16" pos="0 0.1 0.001" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup16_table" mass="10"/>
|
|
||||||
<geom priority= "1" name="cup_geom_table17" pos="0 0.1 0.001" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup17_table" mass="10"/>
|
|
||||||
<geom priority= "1" name="cup_geom1_table8" pos="0 0.1 0.001" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup18_table" mass="10"/>
|
|
||||||
<site name="cup_goal_table" pos="0 0.11 0.1337249" rgba="255 0 0 1"/>
|
|
||||||
<site name="cup_goal_final_table" pos="0.0 0.025 0.1337249" rgba="0 255 0 1"/>
|
|
||||||
</body>
|
|
||||||
<body name="ball" pos="0 0 0">
|
|
||||||
<joint axis="1 0 0" damping="0.0" name="tar:x" pos="0 0 0" stiffness="0" type="slide" frictionloss="0" limited="false"/>
|
|
||||||
<joint axis="0 1 0" damping="0.0" name="tar:y" pos="0 0 0" stiffness="0" type="slide" frictionloss="0" limited="false"/>
|
|
||||||
<joint axis="0 0 1" damping="0.0" name="tar:z" pos="0 0 0" stiffness="0" type="slide" frictionloss="0" limited="false"/>
|
|
||||||
<geom priority= "1" size="0.025 0.025 0.025" type="sphere" condim="4" name="ball_geom" rgba="0.8 0.2 0.1 1" mass="0.1"
|
|
||||||
friction="0.1 0.1 0.1" solimp="0.9 0.95 0.001 0.5 2" solref="-10000 -10"/>
|
|
||||||
<site name="target_ball" pos="0 0 0" size="0.02 0.02 0.02" rgba="1 0 0 1" type="sphere"/>
|
|
||||||
</body>
|
|
||||||
<!-- <camera name="visualization" mode="targetbody" target="wam/wrist_yaw_link" pos="1.5 -0.4 2.2"/>-->
|
|
||||||
<!-- <camera name="experiment" mode="fixed" quat="0.44418059 0.41778323 0.54301123 0.57732103" pos="1.5 -0.3 1.33" />-->
|
|
||||||
<camera name="visualization" mode="fixed" euler="2.35 2.0 -0.75" pos="2.0 -0.0 1.85"/>
|
|
||||||
</worldbody>
|
|
||||||
<actuator>
|
|
||||||
<motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="150.0" joint="wam/base_yaw_joint"/>
|
|
||||||
<motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="200.0" joint="wam/shoulder_pitch_joint"/>
|
|
||||||
<motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="50.0" joint="wam/shoulder_yaw_joint"/>
|
|
||||||
<motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="60.0" joint="wam/elbow_pitch_joint"/>
|
|
||||||
<motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="5.0" joint="wam/wrist_yaw_joint"/>
|
|
||||||
<motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="5.0" joint="wam/wrist_pitch_joint"/>
|
|
||||||
<motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="2.0" joint="wam/palm_yaw_joint"/>
|
|
||||||
</actuator>
|
|
||||||
|
|
||||||
</mujoco>
|
|
||||||
@ -1,193 +0,0 @@
|
|||||||
import mujoco_py.builder
|
|
||||||
import os
|
|
||||||
|
|
||||||
import numpy as np
|
|
||||||
from gym import utils
|
|
||||||
from gym.envs.mujoco import MujocoEnv
|
|
||||||
|
|
||||||
|
|
||||||
class ALRBeerBongEnv(MujocoEnv, utils.EzPickle):
|
|
||||||
def __init__(self, frame_skip=1, apply_gravity_comp=True, reward_type: str = "staged", noisy=False,
|
|
||||||
context: np.ndarray = None, difficulty='simple'):
|
|
||||||
self._steps = 0
|
|
||||||
|
|
||||||
self.xml_path = os.path.join(os.path.dirname(os.path.abspath(__file__)), "assets",
|
|
||||||
"beerpong_wo_cup" + ".xml")
|
|
||||||
|
|
||||||
self.j_min = np.array([-2.6, -1.985, -2.8, -0.9, -4.55, -1.5707, -2.7])
|
|
||||||
self.j_max = np.array([2.6, 1.985, 2.8, 3.14159, 1.25, 1.5707, 2.7])
|
|
||||||
|
|
||||||
self.context = context
|
|
||||||
self.apply_gravity_comp = apply_gravity_comp
|
|
||||||
self.add_noise = noisy
|
|
||||||
|
|
||||||
self._start_pos = np.array([0.0, 1.35, 0.0, 1.18, 0.0, -0.786, -1.59])
|
|
||||||
self._start_vel = np.zeros(7)
|
|
||||||
|
|
||||||
self.ball_site_id = 0
|
|
||||||
self.ball_id = 11
|
|
||||||
|
|
||||||
self._release_step = 175 # time step of ball release
|
|
||||||
|
|
||||||
self.sim_time = 3 # seconds
|
|
||||||
self.ep_length = 600 # based on 3 seconds with dt = 0.005 int(self.sim_time / self.dt)
|
|
||||||
self.cup_table_id = 10
|
|
||||||
|
|
||||||
if noisy:
|
|
||||||
self.noise_std = 0.01
|
|
||||||
else:
|
|
||||||
self.noise_std = 0
|
|
||||||
|
|
||||||
if difficulty == 'simple':
|
|
||||||
self.cup_goal_pos = np.array([0, -1.7, 0.840])
|
|
||||||
elif difficulty == 'intermediate':
|
|
||||||
self.cup_goal_pos = np.array([0.3, -1.5, 0.840])
|
|
||||||
elif difficulty == 'hard':
|
|
||||||
self.cup_goal_pos = np.array([-0.3, -2.2, 0.840])
|
|
||||||
elif difficulty == 'hardest':
|
|
||||||
self.cup_goal_pos = np.array([-0.3, -1.2, 0.840])
|
|
||||||
|
|
||||||
if reward_type == "no_context":
|
|
||||||
from alr_envs.alr.mujoco.beerpong.beerpong_reward import BeerPongReward
|
|
||||||
reward_function = BeerPongReward
|
|
||||||
elif reward_type == "staged":
|
|
||||||
from alr_envs.alr.mujoco.beerpong.beerpong_reward_staged import BeerPongReward
|
|
||||||
reward_function = BeerPongReward
|
|
||||||
else:
|
|
||||||
raise ValueError("Unknown reward type: {}".format(reward_type))
|
|
||||||
self.reward_function = reward_function()
|
|
||||||
|
|
||||||
MujocoEnv.__init__(self, self.xml_path, frame_skip)
|
|
||||||
utils.EzPickle.__init__(self)
|
|
||||||
|
|
||||||
@property
|
|
||||||
def start_pos(self):
|
|
||||||
return self._start_pos
|
|
||||||
|
|
||||||
@property
|
|
||||||
def start_vel(self):
|
|
||||||
return self._start_vel
|
|
||||||
|
|
||||||
@property
|
|
||||||
def current_pos(self):
|
|
||||||
return self.sim.data.qpos[0:7].copy()
|
|
||||||
|
|
||||||
@property
|
|
||||||
def current_vel(self):
|
|
||||||
return self.sim.data.qvel[0:7].copy()
|
|
||||||
|
|
||||||
def reset(self):
|
|
||||||
self.reward_function.reset(self.add_noise)
|
|
||||||
return super().reset()
|
|
||||||
|
|
||||||
def reset_model(self):
|
|
||||||
init_pos_all = self.init_qpos.copy()
|
|
||||||
init_pos_robot = self.start_pos
|
|
||||||
init_vel = np.zeros_like(init_pos_all)
|
|
||||||
|
|
||||||
self._steps = 0
|
|
||||||
|
|
||||||
start_pos = init_pos_all
|
|
||||||
start_pos[0:7] = init_pos_robot
|
|
||||||
|
|
||||||
self.set_state(start_pos, init_vel)
|
|
||||||
self.sim.model.body_pos[self.cup_table_id] = self.cup_goal_pos
|
|
||||||
start_pos[7::] = self.sim.data.site_xpos[self.ball_site_id, :].copy()
|
|
||||||
self.set_state(start_pos, init_vel)
|
|
||||||
return self._get_obs()
|
|
||||||
|
|
||||||
def step(self, a):
|
|
||||||
reward_dist = 0.0
|
|
||||||
angular_vel = 0.0
|
|
||||||
reward_ctrl = - np.square(a).sum()
|
|
||||||
|
|
||||||
if self.apply_gravity_comp:
|
|
||||||
a = a + self.sim.data.qfrc_bias[:len(a)].copy() / self.model.actuator_gear[:, 0]
|
|
||||||
try:
|
|
||||||
self.do_simulation(a, self.frame_skip)
|
|
||||||
if self._steps < self._release_step:
|
|
||||||
self.sim.data.qpos[7::] = self.sim.data.site_xpos[self.ball_site_id, :].copy()
|
|
||||||
self.sim.data.qvel[7::] = self.sim.data.site_xvelp[self.ball_site_id, :].copy()
|
|
||||||
elif self._steps == self._release_step and self.add_noise:
|
|
||||||
self.sim.data.qvel[7::] += self.noise_std * np.random.randn(3)
|
|
||||||
crash = False
|
|
||||||
except mujoco_py.builder.MujocoException:
|
|
||||||
crash = True
|
|
||||||
# joint_cons_viol = self.check_traj_in_joint_limits()
|
|
||||||
|
|
||||||
ob = self._get_obs()
|
|
||||||
|
|
||||||
if not crash:
|
|
||||||
reward, reward_infos = self.reward_function.compute_reward(self, a)
|
|
||||||
success = reward_infos['success']
|
|
||||||
is_collided = reward_infos['is_collided']
|
|
||||||
ball_pos = reward_infos['ball_pos']
|
|
||||||
ball_vel = reward_infos['ball_vel']
|
|
||||||
done = is_collided or self._steps == self.ep_length - 1
|
|
||||||
self._steps += 1
|
|
||||||
else:
|
|
||||||
reward = -30
|
|
||||||
reward_infos = dict()
|
|
||||||
success = False
|
|
||||||
is_collided = False
|
|
||||||
done = True
|
|
||||||
ball_pos = np.zeros(3)
|
|
||||||
ball_vel = np.zeros(3)
|
|
||||||
|
|
||||||
infos = dict(reward_dist=reward_dist,
|
|
||||||
reward_ctrl=reward_ctrl,
|
|
||||||
reward=reward,
|
|
||||||
velocity=angular_vel,
|
|
||||||
# traj=self._q_pos,
|
|
||||||
action=a,
|
|
||||||
q_pos=self.sim.data.qpos[0:7].ravel().copy(),
|
|
||||||
q_vel=self.sim.data.qvel[0:7].ravel().copy(),
|
|
||||||
ball_pos=ball_pos,
|
|
||||||
ball_vel=ball_vel,
|
|
||||||
success=success,
|
|
||||||
is_collided=is_collided, sim_crash=crash)
|
|
||||||
infos.update(reward_infos)
|
|
||||||
|
|
||||||
return ob, reward, done, infos
|
|
||||||
|
|
||||||
def check_traj_in_joint_limits(self):
|
|
||||||
return any(self.current_pos > self.j_max) or any(self.current_pos < self.j_min)
|
|
||||||
|
|
||||||
# TODO: extend observation space
|
|
||||||
def _get_obs(self):
|
|
||||||
theta = self.sim.data.qpos.flat[:7]
|
|
||||||
return np.concatenate([
|
|
||||||
np.cos(theta),
|
|
||||||
np.sin(theta),
|
|
||||||
# self.get_body_com("target"), # only return target to make problem harder
|
|
||||||
[self._steps],
|
|
||||||
])
|
|
||||||
|
|
||||||
# TODO
|
|
||||||
@property
|
|
||||||
def active_obs(self):
|
|
||||||
return np.hstack([
|
|
||||||
[False] * 7, # cos
|
|
||||||
[False] * 7, # sin
|
|
||||||
# [True] * 2, # x-y coordinates of target distance
|
|
||||||
[False] # env steps
|
|
||||||
])
|
|
||||||
|
|
||||||
|
|
||||||
if __name__ == "__main__":
|
|
||||||
env = ALRBeerBongEnv(reward_type="staged", difficulty='hardest')
|
|
||||||
|
|
||||||
# env.configure(ctxt)
|
|
||||||
env.reset()
|
|
||||||
env.render("human")
|
|
||||||
for i in range(800):
|
|
||||||
ac = 10 * env.action_space.sample()[0:7]
|
|
||||||
obs, rew, d, info = env.step(ac)
|
|
||||||
env.render("human")
|
|
||||||
|
|
||||||
print(rew)
|
|
||||||
|
|
||||||
if d:
|
|
||||||
break
|
|
||||||
|
|
||||||
env.close()
|
|
||||||
@ -1,171 +0,0 @@
|
|||||||
import numpy as np
|
|
||||||
|
|
||||||
|
|
||||||
class BeerPongReward:
|
|
||||||
def __init__(self):
|
|
||||||
|
|
||||||
self.robot_collision_objects = ["wrist_palm_link_convex_geom",
|
|
||||||
"wrist_pitch_link_convex_decomposition_p1_geom",
|
|
||||||
"wrist_pitch_link_convex_decomposition_p2_geom",
|
|
||||||
"wrist_pitch_link_convex_decomposition_p3_geom",
|
|
||||||
"wrist_yaw_link_convex_decomposition_p1_geom",
|
|
||||||
"wrist_yaw_link_convex_decomposition_p2_geom",
|
|
||||||
"forearm_link_convex_decomposition_p1_geom",
|
|
||||||
"forearm_link_convex_decomposition_p2_geom",
|
|
||||||
"upper_arm_link_convex_decomposition_p1_geom",
|
|
||||||
"upper_arm_link_convex_decomposition_p2_geom",
|
|
||||||
"shoulder_link_convex_decomposition_p1_geom",
|
|
||||||
"shoulder_link_convex_decomposition_p2_geom",
|
|
||||||
"shoulder_link_convex_decomposition_p3_geom",
|
|
||||||
"base_link_convex_geom", "table_contact_geom"]
|
|
||||||
|
|
||||||
self.cup_collision_objects = ["cup_geom_table3", "cup_geom_table4", "cup_geom_table5", "cup_geom_table6",
|
|
||||||
"cup_geom_table7", "cup_geom_table8", "cup_geom_table9", "cup_geom_table10",
|
|
||||||
# "cup_base_table", "cup_base_table_contact",
|
|
||||||
"cup_geom_table15",
|
|
||||||
"cup_geom_table16",
|
|
||||||
"cup_geom_table17", "cup_geom1_table8",
|
|
||||||
# "cup_base_table_contact",
|
|
||||||
# "cup_base_table"
|
|
||||||
]
|
|
||||||
|
|
||||||
|
|
||||||
self.ball_traj = None
|
|
||||||
self.dists = None
|
|
||||||
self.dists_final = None
|
|
||||||
self.costs = None
|
|
||||||
self.action_costs = None
|
|
||||||
self.angle_rewards = None
|
|
||||||
self.cup_angles = None
|
|
||||||
self.cup_z_axes = None
|
|
||||||
self.collision_penalty = 500
|
|
||||||
self.reset(None)
|
|
||||||
|
|
||||||
def reset(self, context):
|
|
||||||
self.ball_traj = []
|
|
||||||
self.dists = []
|
|
||||||
self.dists_final = []
|
|
||||||
self.costs = []
|
|
||||||
self.action_costs = []
|
|
||||||
self.angle_rewards = []
|
|
||||||
self.cup_angles = []
|
|
||||||
self.cup_z_axes = []
|
|
||||||
self.ball_ground_contact = False
|
|
||||||
self.ball_table_contact = False
|
|
||||||
self.ball_wall_contact = False
|
|
||||||
self.ball_cup_contact = False
|
|
||||||
|
|
||||||
def compute_reward(self, env, action):
|
|
||||||
self.ball_id = env.sim.model._body_name2id["ball"]
|
|
||||||
self.ball_collision_id = env.sim.model._geom_name2id["ball_geom"]
|
|
||||||
self.goal_id = env.sim.model._site_name2id["cup_goal_table"]
|
|
||||||
self.goal_final_id = env.sim.model._site_name2id["cup_goal_final_table"]
|
|
||||||
self.cup_collision_ids = [env.sim.model._geom_name2id[name] for name in self.cup_collision_objects]
|
|
||||||
self.cup_table_id = env.sim.model._body_name2id["cup_table"]
|
|
||||||
self.table_collision_id = env.sim.model._geom_name2id["table_contact_geom"]
|
|
||||||
self.wall_collision_id = env.sim.model._geom_name2id["wall"]
|
|
||||||
self.cup_table_collision_id = env.sim.model._geom_name2id["cup_base_table_contact"]
|
|
||||||
self.init_ball_pos_site_id = env.sim.model._site_name2id["init_ball_pos_site"]
|
|
||||||
self.ground_collision_id = env.sim.model._geom_name2id["ground"]
|
|
||||||
self.robot_collision_ids = [env.sim.model._geom_name2id[name] for name in self.robot_collision_objects]
|
|
||||||
|
|
||||||
goal_pos = env.sim.data.site_xpos[self.goal_id]
|
|
||||||
ball_pos = env.sim.data.body_xpos[self.ball_id]
|
|
||||||
ball_vel = env.sim.data.body_xvelp[self.ball_id]
|
|
||||||
goal_final_pos = env.sim.data.site_xpos[self.goal_final_id]
|
|
||||||
self.dists.append(np.linalg.norm(goal_pos - ball_pos))
|
|
||||||
self.dists_final.append(np.linalg.norm(goal_final_pos - ball_pos))
|
|
||||||
|
|
||||||
action_cost = np.sum(np.square(action))
|
|
||||||
self.action_costs.append(action_cost)
|
|
||||||
|
|
||||||
ball_table_bounce = self._check_collision_single_objects(env.sim, self.ball_collision_id,
|
|
||||||
self.table_collision_id)
|
|
||||||
|
|
||||||
if ball_table_bounce: # or ball_cup_table_cont or ball_wall_con
|
|
||||||
self.ball_table_contact = True
|
|
||||||
|
|
||||||
ball_cup_cont = self._check_collision_with_set_of_objects(env.sim, self.ball_collision_id,
|
|
||||||
self.cup_collision_ids)
|
|
||||||
if ball_cup_cont:
|
|
||||||
self.ball_cup_contact = True
|
|
||||||
|
|
||||||
ball_wall_cont = self._check_collision_single_objects(env.sim, self.ball_collision_id, self.wall_collision_id)
|
|
||||||
if ball_wall_cont and not self.ball_table_contact:
|
|
||||||
self.ball_wall_contact = True
|
|
||||||
|
|
||||||
ball_ground_contact = self._check_collision_single_objects(env.sim, self.ball_collision_id,
|
|
||||||
self.ground_collision_id)
|
|
||||||
if ball_ground_contact and not self.ball_table_contact:
|
|
||||||
self.ball_ground_contact = True
|
|
||||||
|
|
||||||
self._is_collided = self._check_collision_with_itself(env.sim, self.robot_collision_ids)
|
|
||||||
if env._steps == env.ep_length - 1 or self._is_collided:
|
|
||||||
|
|
||||||
min_dist = np.min(self.dists)
|
|
||||||
|
|
||||||
ball_in_cup = self._check_collision_single_objects(env.sim, self.ball_collision_id, self.cup_table_collision_id)
|
|
||||||
|
|
||||||
cost_offset = 0
|
|
||||||
|
|
||||||
if self.ball_ground_contact: # or self.ball_wall_contact:
|
|
||||||
cost_offset += 2
|
|
||||||
|
|
||||||
if not self.ball_table_contact:
|
|
||||||
cost_offset += 2
|
|
||||||
|
|
||||||
if not ball_in_cup:
|
|
||||||
cost_offset += 2
|
|
||||||
cost = cost_offset + min_dist ** 2 + 0.5 * self.dists_final[-1] ** 2 + 1e-4 * action_cost # + min_dist ** 2
|
|
||||||
else:
|
|
||||||
if self.ball_cup_contact:
|
|
||||||
cost_offset += 1
|
|
||||||
cost = cost_offset + self.dists_final[-1] ** 2 + 1e-4 * action_cost
|
|
||||||
|
|
||||||
reward = - 1*cost - self.collision_penalty * int(self._is_collided)
|
|
||||||
success = ball_in_cup and not self.ball_ground_contact and not self.ball_wall_contact and not self.ball_cup_contact
|
|
||||||
else:
|
|
||||||
reward = - 1e-4 * action_cost
|
|
||||||
success = False
|
|
||||||
|
|
||||||
infos = {}
|
|
||||||
infos["success"] = success
|
|
||||||
infos["is_collided"] = self._is_collided
|
|
||||||
infos["ball_pos"] = ball_pos.copy()
|
|
||||||
infos["ball_vel"] = ball_vel.copy()
|
|
||||||
infos["action_cost"] = 5e-4 * action_cost
|
|
||||||
|
|
||||||
return reward, infos
|
|
||||||
|
|
||||||
def _check_collision_single_objects(self, sim, id_1, id_2):
|
|
||||||
for coni in range(0, sim.data.ncon):
|
|
||||||
con = sim.data.contact[coni]
|
|
||||||
|
|
||||||
collision = con.geom1 == id_1 and con.geom2 == id_2
|
|
||||||
collision_trans = con.geom1 == id_2 and con.geom2 == id_1
|
|
||||||
|
|
||||||
if collision or collision_trans:
|
|
||||||
return True
|
|
||||||
return False
|
|
||||||
|
|
||||||
def _check_collision_with_itself(self, sim, collision_ids):
|
|
||||||
col_1, col_2 = False, False
|
|
||||||
for j, id in enumerate(collision_ids):
|
|
||||||
col_1 = self._check_collision_with_set_of_objects(sim, id, collision_ids[:j])
|
|
||||||
if j != len(collision_ids) - 1:
|
|
||||||
col_2 = self._check_collision_with_set_of_objects(sim, id, collision_ids[j + 1:])
|
|
||||||
else:
|
|
||||||
col_2 = False
|
|
||||||
collision = True if col_1 or col_2 else False
|
|
||||||
return collision
|
|
||||||
|
|
||||||
def _check_collision_with_set_of_objects(self, sim, id_1, id_list):
|
|
||||||
for coni in range(0, sim.data.ncon):
|
|
||||||
con = sim.data.contact[coni]
|
|
||||||
|
|
||||||
collision = con.geom1 in id_list and con.geom2 == id_1
|
|
||||||
collision_trans = con.geom1 == id_1 and con.geom2 in id_list
|
|
||||||
|
|
||||||
if collision or collision_trans:
|
|
||||||
return True
|
|
||||||
return False
|
|
||||||
@ -1,141 +0,0 @@
|
|||||||
import numpy as np
|
|
||||||
from alr_envs.alr.mujoco import alr_reward_fct
|
|
||||||
|
|
||||||
|
|
||||||
class BeerpongReward(alr_reward_fct.AlrReward):
|
|
||||||
def __init__(self, sim, sim_time):
|
|
||||||
|
|
||||||
self.sim = sim
|
|
||||||
self.sim_time = sim_time
|
|
||||||
|
|
||||||
self.collision_objects = ["cup_geom1", "cup_geom2", "wrist_palm_link_convex_geom",
|
|
||||||
"wrist_pitch_link_convex_decomposition_p1_geom",
|
|
||||||
"wrist_pitch_link_convex_decomposition_p2_geom",
|
|
||||||
"wrist_pitch_link_convex_decomposition_p3_geom",
|
|
||||||
"wrist_yaw_link_convex_decomposition_p1_geom",
|
|
||||||
"wrist_yaw_link_convex_decomposition_p2_geom",
|
|
||||||
"forearm_link_convex_decomposition_p1_geom",
|
|
||||||
"forearm_link_convex_decomposition_p2_geom"]
|
|
||||||
|
|
||||||
self.ball_id = None
|
|
||||||
self.ball_collision_id = None
|
|
||||||
self.goal_id = None
|
|
||||||
self.goal_final_id = None
|
|
||||||
self.collision_ids = None
|
|
||||||
|
|
||||||
self.ball_traj = None
|
|
||||||
self.dists = None
|
|
||||||
self.dists_ctxt = None
|
|
||||||
self.dists_final = None
|
|
||||||
self.costs = None
|
|
||||||
|
|
||||||
self.reset(None)
|
|
||||||
|
|
||||||
def reset(self, context):
|
|
||||||
self.ball_traj = np.zeros(shape=(self.sim_time, 3))
|
|
||||||
self.dists = []
|
|
||||||
self.dists_ctxt = []
|
|
||||||
self.dists_final = []
|
|
||||||
self.costs = []
|
|
||||||
self.action_costs = []
|
|
||||||
self.context = context
|
|
||||||
self.ball_in_cup = False
|
|
||||||
self.dist_ctxt = 5
|
|
||||||
self.bounce_dist = 2
|
|
||||||
self.min_dist = 2
|
|
||||||
self.dist_final = 2
|
|
||||||
self.table_contact = False
|
|
||||||
|
|
||||||
self.ball_id = self.sim.model._body_name2id["ball"]
|
|
||||||
self.ball_collision_id = self.sim.model._geom_name2id["ball_geom"]
|
|
||||||
self.cup_robot_id = self.sim.model._site_name2id["cup_robot_final"]
|
|
||||||
self.goal_id = self.sim.model._site_name2id["cup_goal_table"]
|
|
||||||
self.goal_final_id = self.sim.model._site_name2id["cup_goal_final_table"]
|
|
||||||
self.collision_ids = [self.sim.model._geom_name2id[name] for name in self.collision_objects]
|
|
||||||
self.cup_table_id = self.sim.model._body_name2id["cup_table"]
|
|
||||||
self.bounce_table_id = self.sim.model._site_name2id["bounce_table"]
|
|
||||||
|
|
||||||
def compute_reward(self, action, sim, step):
|
|
||||||
action_cost = np.sum(np.square(action))
|
|
||||||
self.action_costs.append(action_cost)
|
|
||||||
|
|
||||||
stop_sim = False
|
|
||||||
success = False
|
|
||||||
|
|
||||||
if self.check_collision(sim):
|
|
||||||
reward = - 1e-2 * action_cost - 10
|
|
||||||
stop_sim = True
|
|
||||||
return reward, success, stop_sim
|
|
||||||
|
|
||||||
# Compute the current distance from the ball to the inner part of the cup
|
|
||||||
goal_pos = sim.data.site_xpos[self.goal_id]
|
|
||||||
ball_pos = sim.data.body_xpos[self.ball_id]
|
|
||||||
bounce_pos = sim.data.site_xpos[self.bounce_table_id]
|
|
||||||
goal_final_pos = sim.data.site_xpos[self.goal_final_id]
|
|
||||||
self.dists.append(np.linalg.norm(goal_pos - ball_pos))
|
|
||||||
self.dists_final.append(np.linalg.norm(goal_final_pos - ball_pos))
|
|
||||||
self.ball_traj[step, :] = ball_pos
|
|
||||||
|
|
||||||
ball_in_cup = self.check_ball_in_cup(sim, self.ball_collision_id)
|
|
||||||
table_contact = self.check_ball_table_contact(sim, self.ball_collision_id)
|
|
||||||
|
|
||||||
if table_contact and not self.table_contact:
|
|
||||||
self.bounce_dist = np.minimum((np.linalg.norm(bounce_pos - ball_pos)), 2)
|
|
||||||
self.table_contact = True
|
|
||||||
|
|
||||||
if step == self.sim_time - 1:
|
|
||||||
min_dist = np.min(self.dists)
|
|
||||||
self.min_dist = min_dist
|
|
||||||
dist_final = self.dists_final[-1]
|
|
||||||
self.dist_final = dist_final
|
|
||||||
|
|
||||||
cost = 0.33 * min_dist + 0.33 * dist_final + 0.33 * self.bounce_dist
|
|
||||||
reward = np.exp(-2 * cost) - 1e-2 * action_cost
|
|
||||||
success = self.bounce_dist < 0.05 and dist_final < 0.05 and ball_in_cup
|
|
||||||
else:
|
|
||||||
reward = - 1e-2 * action_cost
|
|
||||||
success = False
|
|
||||||
|
|
||||||
return reward, success, stop_sim
|
|
||||||
|
|
||||||
def _get_stage_wise_cost(self, ball_in_cup, min_dist, dist_final, dist_to_ctxt):
|
|
||||||
if not ball_in_cup:
|
|
||||||
cost = 3 + 2*(0.5 * min_dist**2 + 0.5 * dist_final**2)
|
|
||||||
else:
|
|
||||||
cost = 2 * dist_to_ctxt ** 2
|
|
||||||
print('Context Distance:', dist_to_ctxt)
|
|
||||||
return cost
|
|
||||||
|
|
||||||
def check_ball_table_contact(self, sim, ball_collision_id):
|
|
||||||
table_collision_id = sim.model._geom_name2id["table_contact_geom"]
|
|
||||||
for coni in range(0, sim.data.ncon):
|
|
||||||
con = sim.data.contact[coni]
|
|
||||||
collision = con.geom1 == table_collision_id and con.geom2 == ball_collision_id
|
|
||||||
collision_trans = con.geom1 == ball_collision_id and con.geom2 == table_collision_id
|
|
||||||
|
|
||||||
if collision or collision_trans:
|
|
||||||
return True
|
|
||||||
return False
|
|
||||||
|
|
||||||
def check_ball_in_cup(self, sim, ball_collision_id):
|
|
||||||
cup_base_collision_id = sim.model._geom_name2id["cup_base_table_contact"]
|
|
||||||
for coni in range(0, sim.data.ncon):
|
|
||||||
con = sim.data.contact[coni]
|
|
||||||
|
|
||||||
collision = con.geom1 == cup_base_collision_id and con.geom2 == ball_collision_id
|
|
||||||
collision_trans = con.geom1 == ball_collision_id and con.geom2 == cup_base_collision_id
|
|
||||||
|
|
||||||
if collision or collision_trans:
|
|
||||||
return True
|
|
||||||
return False
|
|
||||||
|
|
||||||
def check_collision(self, sim):
|
|
||||||
for coni in range(0, sim.data.ncon):
|
|
||||||
con = sim.data.contact[coni]
|
|
||||||
|
|
||||||
collision = con.geom1 in self.collision_ids and con.geom2 == self.ball_collision_id
|
|
||||||
collision_trans = con.geom1 == self.ball_collision_id and con.geom2 in self.collision_ids
|
|
||||||
|
|
||||||
if collision or collision_trans:
|
|
||||||
return True
|
|
||||||
return False
|
|
||||||
@ -1,158 +0,0 @@
|
|||||||
import numpy as np
|
|
||||||
|
|
||||||
|
|
||||||
class BeerPongReward:
|
|
||||||
def __init__(self):
|
|
||||||
|
|
||||||
self.robot_collision_objects = ["wrist_palm_link_convex_geom",
|
|
||||||
"wrist_pitch_link_convex_decomposition_p1_geom",
|
|
||||||
"wrist_pitch_link_convex_decomposition_p2_geom",
|
|
||||||
"wrist_pitch_link_convex_decomposition_p3_geom",
|
|
||||||
"wrist_yaw_link_convex_decomposition_p1_geom",
|
|
||||||
"wrist_yaw_link_convex_decomposition_p2_geom",
|
|
||||||
"forearm_link_convex_decomposition_p1_geom",
|
|
||||||
"forearm_link_convex_decomposition_p2_geom",
|
|
||||||
"upper_arm_link_convex_decomposition_p1_geom",
|
|
||||||
"upper_arm_link_convex_decomposition_p2_geom",
|
|
||||||
"shoulder_link_convex_decomposition_p1_geom",
|
|
||||||
"shoulder_link_convex_decomposition_p2_geom",
|
|
||||||
"shoulder_link_convex_decomposition_p3_geom",
|
|
||||||
"base_link_convex_geom", "table_contact_geom"]
|
|
||||||
|
|
||||||
self.cup_collision_objects = ["cup_geom_table3", "cup_geom_table4", "cup_geom_table5", "cup_geom_table6",
|
|
||||||
"cup_geom_table7", "cup_geom_table8", "cup_geom_table9", "cup_geom_table10",
|
|
||||||
# "cup_base_table", "cup_base_table_contact",
|
|
||||||
"cup_geom_table15",
|
|
||||||
"cup_geom_table16",
|
|
||||||
"cup_geom_table17", "cup_geom1_table8",
|
|
||||||
# "cup_base_table_contact",
|
|
||||||
# "cup_base_table"
|
|
||||||
]
|
|
||||||
|
|
||||||
|
|
||||||
self.ball_traj = None
|
|
||||||
self.dists = None
|
|
||||||
self.dists_final = None
|
|
||||||
self.costs = None
|
|
||||||
self.action_costs = None
|
|
||||||
self.angle_rewards = None
|
|
||||||
self.cup_angles = None
|
|
||||||
self.cup_z_axes = None
|
|
||||||
self.collision_penalty = 500
|
|
||||||
self.reset(None)
|
|
||||||
|
|
||||||
def reset(self, noisy):
|
|
||||||
self.ball_traj = []
|
|
||||||
self.dists = []
|
|
||||||
self.dists_final = []
|
|
||||||
self.costs = []
|
|
||||||
self.action_costs = []
|
|
||||||
self.angle_rewards = []
|
|
||||||
self.cup_angles = []
|
|
||||||
self.cup_z_axes = []
|
|
||||||
self.ball_ground_contact = False
|
|
||||||
self.ball_table_contact = False
|
|
||||||
self.ball_wall_contact = False
|
|
||||||
self.ball_cup_contact = False
|
|
||||||
self.noisy_bp = noisy
|
|
||||||
self._t_min_final_dist = -1
|
|
||||||
|
|
||||||
def compute_reward(self, env, action):
|
|
||||||
self.ball_id = env.sim.model._body_name2id["ball"]
|
|
||||||
self.ball_collision_id = env.sim.model._geom_name2id["ball_geom"]
|
|
||||||
self.goal_id = env.sim.model._site_name2id["cup_goal_table"]
|
|
||||||
self.goal_final_id = env.sim.model._site_name2id["cup_goal_final_table"]
|
|
||||||
self.cup_collision_ids = [env.sim.model._geom_name2id[name] for name in self.cup_collision_objects]
|
|
||||||
self.cup_table_id = env.sim.model._body_name2id["cup_table"]
|
|
||||||
self.table_collision_id = env.sim.model._geom_name2id["table_contact_geom"]
|
|
||||||
self.wall_collision_id = env.sim.model._geom_name2id["wall"]
|
|
||||||
self.cup_table_collision_id = env.sim.model._geom_name2id["cup_base_table_contact"]
|
|
||||||
self.init_ball_pos_site_id = env.sim.model._site_name2id["init_ball_pos_site"]
|
|
||||||
self.ground_collision_id = env.sim.model._geom_name2id["ground"]
|
|
||||||
self.robot_collision_ids = [env.sim.model._geom_name2id[name] for name in self.robot_collision_objects]
|
|
||||||
|
|
||||||
goal_pos = env.sim.data.site_xpos[self.goal_id]
|
|
||||||
ball_pos = env.sim.data.body_xpos[self.ball_id]
|
|
||||||
ball_vel = env.sim.data.body_xvelp[self.ball_id]
|
|
||||||
goal_final_pos = env.sim.data.site_xpos[self.goal_final_id]
|
|
||||||
self.dists.append(np.linalg.norm(goal_pos - ball_pos))
|
|
||||||
self.dists_final.append(np.linalg.norm(goal_final_pos - ball_pos))
|
|
||||||
|
|
||||||
action_cost = np.sum(np.square(action))
|
|
||||||
self.action_costs.append(action_cost)
|
|
||||||
|
|
||||||
if not self.ball_table_contact:
|
|
||||||
self.ball_table_contact = self._check_collision_single_objects(env.sim, self.ball_collision_id,
|
|
||||||
self.table_collision_id)
|
|
||||||
|
|
||||||
self._is_collided = self._check_collision_with_itself(env.sim, self.robot_collision_ids)
|
|
||||||
if env._steps == env.ep_length - 1 or self._is_collided:
|
|
||||||
|
|
||||||
min_dist = np.min(self.dists)
|
|
||||||
final_dist = self.dists_final[-1]
|
|
||||||
|
|
||||||
ball_in_cup = self._check_collision_single_objects(env.sim, self.ball_collision_id,
|
|
||||||
self.cup_table_collision_id)
|
|
||||||
|
|
||||||
# encourage bounce before falling into cup
|
|
||||||
if not ball_in_cup:
|
|
||||||
if not self.ball_table_contact:
|
|
||||||
reward = 0.2 * (1 - np.tanh(min_dist ** 2)) + 0.1 * (1 - np.tanh(final_dist ** 2))
|
|
||||||
else:
|
|
||||||
reward = (1 - np.tanh(min_dist ** 2)) + 0.5 * (1 - np.tanh(final_dist ** 2))
|
|
||||||
else:
|
|
||||||
if not self.ball_table_contact:
|
|
||||||
reward = 2 * (1 - np.tanh(final_dist ** 2)) + 1 * (1 - np.tanh(min_dist ** 2)) + 1
|
|
||||||
else:
|
|
||||||
reward = 2 * (1 - np.tanh(final_dist ** 2)) + 1 * (1 - np.tanh(min_dist ** 2)) + 3
|
|
||||||
|
|
||||||
# reward = - 1 * cost - self.collision_penalty * int(self._is_collided)
|
|
||||||
success = ball_in_cup
|
|
||||||
crash = self._is_collided
|
|
||||||
else:
|
|
||||||
reward = - 1e-2 * action_cost
|
|
||||||
success = False
|
|
||||||
crash = False
|
|
||||||
|
|
||||||
infos = {}
|
|
||||||
infos["success"] = success
|
|
||||||
infos["is_collided"] = self._is_collided
|
|
||||||
infos["ball_pos"] = ball_pos.copy()
|
|
||||||
infos["ball_vel"] = ball_vel.copy()
|
|
||||||
infos["action_cost"] = action_cost
|
|
||||||
infos["task_reward"] = reward
|
|
||||||
|
|
||||||
return reward, infos
|
|
||||||
|
|
||||||
def _check_collision_single_objects(self, sim, id_1, id_2):
|
|
||||||
for coni in range(0, sim.data.ncon):
|
|
||||||
con = sim.data.contact[coni]
|
|
||||||
|
|
||||||
collision = con.geom1 == id_1 and con.geom2 == id_2
|
|
||||||
collision_trans = con.geom1 == id_2 and con.geom2 == id_1
|
|
||||||
|
|
||||||
if collision or collision_trans:
|
|
||||||
return True
|
|
||||||
return False
|
|
||||||
|
|
||||||
def _check_collision_with_itself(self, sim, collision_ids):
|
|
||||||
col_1, col_2 = False, False
|
|
||||||
for j, id in enumerate(collision_ids):
|
|
||||||
col_1 = self._check_collision_with_set_of_objects(sim, id, collision_ids[:j])
|
|
||||||
if j != len(collision_ids) - 1:
|
|
||||||
col_2 = self._check_collision_with_set_of_objects(sim, id, collision_ids[j + 1:])
|
|
||||||
else:
|
|
||||||
col_2 = False
|
|
||||||
collision = True if col_1 or col_2 else False
|
|
||||||
return collision
|
|
||||||
|
|
||||||
def _check_collision_with_set_of_objects(self, sim, id_1, id_list):
|
|
||||||
for coni in range(0, sim.data.ncon):
|
|
||||||
con = sim.data.contact[coni]
|
|
||||||
|
|
||||||
collision = con.geom1 in id_list and con.geom2 == id_1
|
|
||||||
collision_trans = con.geom1 == id_1 and con.geom2 in id_list
|
|
||||||
|
|
||||||
if collision or collision_trans:
|
|
||||||
return True
|
|
||||||
return False
|
|
||||||
@ -1,166 +0,0 @@
|
|||||||
from gym import utils
|
|
||||||
import os
|
|
||||||
import numpy as np
|
|
||||||
from gym.envs.mujoco import MujocoEnv
|
|
||||||
|
|
||||||
|
|
||||||
class ALRBeerpongEnv(MujocoEnv, utils.EzPickle):
|
|
||||||
def __init__(self, n_substeps=4, apply_gravity_comp=True, reward_function=None):
|
|
||||||
self._steps = 0
|
|
||||||
|
|
||||||
self.xml_path = os.path.join(os.path.dirname(os.path.abspath(__file__)), "assets",
|
|
||||||
"beerpong" + ".xml")
|
|
||||||
|
|
||||||
self.start_pos = np.array([0.0, 1.35, 0.0, 1.18, 0.0, -0.786, -1.59])
|
|
||||||
self.start_vel = np.zeros(7)
|
|
||||||
|
|
||||||
self._q_pos = []
|
|
||||||
self._q_vel = []
|
|
||||||
# self.weight_matrix_scale = 50
|
|
||||||
self.max_ctrl = np.array([150., 125., 40., 60., 5., 5., 2.])
|
|
||||||
self.p_gains = 1 / self.max_ctrl * np.array([200, 300, 100, 100, 10, 10, 2.5])
|
|
||||||
self.d_gains = 1 / self.max_ctrl * np.array([7, 15, 5, 2.5, 0.3, 0.3, 0.05])
|
|
||||||
|
|
||||||
self.j_min = np.array([-2.6, -1.985, -2.8, -0.9, -4.55, -1.5707, -2.7])
|
|
||||||
self.j_max = np.array([2.6, 1.985, 2.8, 3.14159, 1.25, 1.5707, 2.7])
|
|
||||||
|
|
||||||
self.context = None
|
|
||||||
|
|
||||||
# alr_mujoco_env.AlrMujocoEnv.__init__(self,
|
|
||||||
# self.xml_path,
|
|
||||||
# apply_gravity_comp=apply_gravity_comp,
|
|
||||||
# n_substeps=n_substeps)
|
|
||||||
|
|
||||||
self.sim_time = 8 # seconds
|
|
||||||
# self.sim_steps = int(self.sim_time / self.dt)
|
|
||||||
if reward_function is None:
|
|
||||||
from alr_envs.alr.mujoco.beerpong.beerpong_reward_simple import BeerpongReward
|
|
||||||
reward_function = BeerpongReward
|
|
||||||
self.reward_function = reward_function(self.sim, self.sim_steps)
|
|
||||||
self.cup_robot_id = self.sim.model._site_name2id["cup_robot_final"]
|
|
||||||
self.ball_id = self.sim.model._body_name2id["ball"]
|
|
||||||
self.cup_table_id = self.sim.model._body_name2id["cup_table"]
|
|
||||||
# self.bounce_table_id = self.sim.model._body_name2id["bounce_table"]
|
|
||||||
|
|
||||||
MujocoEnv.__init__(self, model_path=self.xml_path, frame_skip=n_substeps)
|
|
||||||
utils.EzPickle.__init__(self)
|
|
||||||
|
|
||||||
@property
|
|
||||||
def current_pos(self):
|
|
||||||
return self.sim.data.qpos[0:7].copy()
|
|
||||||
|
|
||||||
@property
|
|
||||||
def current_vel(self):
|
|
||||||
return self.sim.data.qvel[0:7].copy()
|
|
||||||
|
|
||||||
def configure(self, context):
|
|
||||||
if context is None:
|
|
||||||
context = np.array([0, -2, 0.840])
|
|
||||||
self.context = context
|
|
||||||
self.reward_function.reset(context)
|
|
||||||
|
|
||||||
def reset_model(self):
|
|
||||||
init_pos_all = self.init_qpos.copy()
|
|
||||||
init_pos_robot = self.start_pos
|
|
||||||
init_vel = np.zeros_like(init_pos_all)
|
|
||||||
|
|
||||||
self._steps = 0
|
|
||||||
self._q_pos = []
|
|
||||||
self._q_vel = []
|
|
||||||
|
|
||||||
start_pos = init_pos_all
|
|
||||||
start_pos[0:7] = init_pos_robot
|
|
||||||
# start_pos[7:] = np.copy(self.sim.data.site_xpos[self.cup_robot_id, :]) + np.array([0., 0.0, 0.05])
|
|
||||||
|
|
||||||
self.set_state(start_pos, init_vel)
|
|
||||||
|
|
||||||
ball_pos = np.copy(self.sim.data.site_xpos[self.cup_robot_id, :]) + np.array([0., 0.0, 0.05])
|
|
||||||
self.sim.model.body_pos[self.ball_id] = ball_pos.copy()
|
|
||||||
self.sim.model.body_pos[self.cup_table_id] = self.context.copy()
|
|
||||||
# self.sim.model.body_pos[self.bounce_table_id] = self.context.copy()
|
|
||||||
|
|
||||||
self.sim.forward()
|
|
||||||
|
|
||||||
return self._get_obs()
|
|
||||||
|
|
||||||
def step(self, a):
|
|
||||||
reward_dist = 0.0
|
|
||||||
angular_vel = 0.0
|
|
||||||
reward_ctrl = - np.square(a).sum()
|
|
||||||
action_cost = np.sum(np.square(a))
|
|
||||||
|
|
||||||
crash = self.do_simulation(a, self.frame_skip)
|
|
||||||
joint_cons_viol = self.check_traj_in_joint_limits()
|
|
||||||
|
|
||||||
self._q_pos.append(self.sim.data.qpos[0:7].ravel().copy())
|
|
||||||
self._q_vel.append(self.sim.data.qvel[0:7].ravel().copy())
|
|
||||||
|
|
||||||
ob = self._get_obs()
|
|
||||||
|
|
||||||
if not crash and not joint_cons_viol:
|
|
||||||
reward, success, stop_sim = self.reward_function.compute_reward(a, self.sim, self._steps)
|
|
||||||
done = success or self._steps == self.sim_steps - 1 or stop_sim
|
|
||||||
self._steps += 1
|
|
||||||
else:
|
|
||||||
reward = -10 - 1e-2 * action_cost
|
|
||||||
success = False
|
|
||||||
done = True
|
|
||||||
return ob, reward, done, dict(reward_dist=reward_dist,
|
|
||||||
reward_ctrl=reward_ctrl,
|
|
||||||
velocity=angular_vel,
|
|
||||||
traj=self._q_pos, is_success=success,
|
|
||||||
is_collided=crash or joint_cons_viol)
|
|
||||||
|
|
||||||
def check_traj_in_joint_limits(self):
|
|
||||||
return any(self.current_pos > self.j_max) or any(self.current_pos < self.j_min)
|
|
||||||
|
|
||||||
def extend_des_pos(self, des_pos):
|
|
||||||
des_pos_full = self.start_pos.copy()
|
|
||||||
des_pos_full[1] = des_pos[0]
|
|
||||||
des_pos_full[3] = des_pos[1]
|
|
||||||
des_pos_full[5] = des_pos[2]
|
|
||||||
return des_pos_full
|
|
||||||
|
|
||||||
def extend_des_vel(self, des_vel):
|
|
||||||
des_vel_full = self.start_vel.copy()
|
|
||||||
des_vel_full[1] = des_vel[0]
|
|
||||||
des_vel_full[3] = des_vel[1]
|
|
||||||
des_vel_full[5] = des_vel[2]
|
|
||||||
return des_vel_full
|
|
||||||
|
|
||||||
def _get_obs(self):
|
|
||||||
theta = self.sim.data.qpos.flat[:7]
|
|
||||||
return np.concatenate([
|
|
||||||
np.cos(theta),
|
|
||||||
np.sin(theta),
|
|
||||||
# self.get_body_com("target"), # only return target to make problem harder
|
|
||||||
[self._steps],
|
|
||||||
])
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
if __name__ == "__main__":
|
|
||||||
env = ALRBeerpongEnv()
|
|
||||||
ctxt = np.array([0, -2, 0.840]) # initial
|
|
||||||
|
|
||||||
env.configure(ctxt)
|
|
||||||
env.reset()
|
|
||||||
env.render()
|
|
||||||
for i in range(16000):
|
|
||||||
# test with random actions
|
|
||||||
ac = 0.0 * env.action_space.sample()[0:7]
|
|
||||||
ac[1] = -0.01
|
|
||||||
ac[3] = - 0.01
|
|
||||||
ac[5] = -0.01
|
|
||||||
# ac = env.start_pos
|
|
||||||
# ac[0] += np.pi/2
|
|
||||||
obs, rew, d, info = env.step(ac)
|
|
||||||
env.render()
|
|
||||||
|
|
||||||
print(rew)
|
|
||||||
|
|
||||||
if d:
|
|
||||||
break
|
|
||||||
|
|
||||||
env.close()
|
|
||||||
|
|
||||||
@ -1,39 +0,0 @@
|
|||||||
from typing import Tuple, Union
|
|
||||||
|
|
||||||
import numpy as np
|
|
||||||
|
|
||||||
from mp_env_api.interface_wrappers.mp_env_wrapper import MPEnvWrapper
|
|
||||||
|
|
||||||
|
|
||||||
class MPWrapper(MPEnvWrapper):
|
|
||||||
|
|
||||||
@property
|
|
||||||
def active_obs(self):
|
|
||||||
# TODO: @Max Filter observations correctly
|
|
||||||
return np.hstack([
|
|
||||||
[False] * 7, # cos
|
|
||||||
[False] * 7, # sin
|
|
||||||
# [True] * 2, # x-y coordinates of target distance
|
|
||||||
[False] # env steps
|
|
||||||
])
|
|
||||||
|
|
||||||
@property
|
|
||||||
def start_pos(self):
|
|
||||||
return self._start_pos
|
|
||||||
|
|
||||||
@property
|
|
||||||
def current_pos(self) -> Union[float, int, np.ndarray, Tuple]:
|
|
||||||
return self.sim.data.qpos[0:7].copy()
|
|
||||||
|
|
||||||
@property
|
|
||||||
def current_vel(self) -> Union[float, int, np.ndarray, Tuple]:
|
|
||||||
return self.sim.data.qvel[0:7].copy()
|
|
||||||
|
|
||||||
@property
|
|
||||||
def goal_pos(self):
|
|
||||||
# TODO: @Max I think the default value of returning to the start is reasonable here
|
|
||||||
raise ValueError("Goal position is not available and has to be learnt based on the environment.")
|
|
||||||
|
|
||||||
@property
|
|
||||||
def dt(self) -> Union[float, int]:
|
|
||||||
return self.env.dt
|
|
||||||
@ -1 +0,0 @@
|
|||||||
|
|
||||||
@ -1 +0,0 @@
|
|||||||
|
|
||||||
@ -1,12 +0,0 @@
|
|||||||
<mujocoinclude>
|
|
||||||
<actuator boastype="none">
|
|
||||||
<motor name="wam/shoulder_yaw_link_right_motor" joint="wam/base_yaw_joint_right"/>
|
|
||||||
<motor name="wam/shoulder_pitch_joint_right_motor" joint='wam/shoulder_pitch_joint_right'/>
|
|
||||||
<motor name="wam/shoulder_yaw_joint_right_motor" joint='wam/shoulder_yaw_joint_right'/>
|
|
||||||
<motor name="wam/elbow_pitch_joint_right_motor" joint='wam/elbow_pitch_joint_right'/>
|
|
||||||
<motor name="wam/wrist_yaw_joint_right_motor" joint='wam/wrist_yaw_joint_right'/>
|
|
||||||
<motor name="wam/wrist_pitch_joint_right_motor" joint='wam/wrist_pitch_joint_right'/>
|
|
||||||
<motor name="wam/palm_yaw_joint_right_motor" joint='wam/palm_yaw_joint_right'/>
|
|
||||||
</actuator>
|
|
||||||
</mujocoinclude>
|
|
||||||
|
|
||||||
@ -1,76 +0,0 @@
|
|||||||
<mujocoinclude>
|
|
||||||
<body name="wam/base_link_left" pos="-2.5 0 2" quat="0 1 0 0" childclass="wam">
|
|
||||||
<inertial pos="0 0 0" mass="1" diaginertia="0.1 0.1 0.1"/>
|
|
||||||
<geom class="viz" mesh="base_link_fine" rgba="0.5 0.5 0.5 0"/>
|
|
||||||
<geom class="col" mesh="base_link_convex" rgba="0.5 0.5 0.5 1"/>
|
|
||||||
<body name="wam/shoulder_yaw_link" pos="0 0 0.346">
|
|
||||||
<inertial pos="-0.00443422 -0.00066489 -0.128904" quat="0.69566 0.716713 -0.0354863 0.0334839" mass="5"
|
|
||||||
diaginertia="0.135089 0.113095 0.0904426"/>
|
|
||||||
<joint name="wam/base_yaw_joint" range="-2.6 2.6" damping="1.98"/>
|
|
||||||
<geom class="viz" mesh="shoulder_link_fine" rgba="1 1 1 0"/>
|
|
||||||
<geom class="col" mesh="shoulder_link_convex_decomposition_p1"/>
|
|
||||||
<geom class="col" mesh="shoulder_link_convex_decomposition_p2"/>
|
|
||||||
<geom class="col" mesh="shoulder_link_convex_decomposition_p3"/>
|
|
||||||
<body name="wam/shoulder_pitch_link" pos="0 0 0" quat="0.707107 -0.707107 0 0">
|
|
||||||
<inertial pos="-0.00236981 -0.0154211 0.0310561" quat="0.961794 0.273112 -0.0169316 0.00866592"
|
|
||||||
mass="3.87494" diaginertia="0.0214195 0.0167127 0.0126452"/> <!--seems off-->
|
|
||||||
<joint name="wam/shoulder_pitch_joint" range="-1.985 1.985" damping="0.55"/>
|
|
||||||
<geom class="viz" mesh="shoulder_pitch_link_fine" rgba="1 1 1 0"/>
|
|
||||||
<geom class="col" mesh="shoulder_pitch_link_convex"/>
|
|
||||||
<body name="wam/upper_arm_link" pos="0 0 0" quat="0.707107 0.707107 0 0">
|
|
||||||
<inertial pos="0.00683259 3.309e-005 0.392492" quat="0.647136 0.0170822 0.0143038 0.762049"
|
|
||||||
mass="2.20228" diaginertia="0.0592718 0.0592207 0.00313419"/>
|
|
||||||
<joint name="wam/shoulder_yaw_joint" range="-2.8 2.8" damping="1.65"/>
|
|
||||||
<geom class="viz" mesh="upper_arm_link_fine" rgba="1 1 1 0"/>
|
|
||||||
<geom class="col" mesh="upper_arm_link_convex_decomposition_p1" rgba="0.094 0.48 0.804 1"/>
|
|
||||||
<geom class="col" mesh="upper_arm_link_convex_decomposition_p2" rgba="0.094 0.48 0.804 1"/>
|
|
||||||
<body name="wam/forearm_link" pos="0.045 0 0.55" quat="0.707107 -0.707107 0 0">
|
|
||||||
<inertial pos="-0.0400149 -0.142717 -0.00022942"
|
|
||||||
quat="0.704281 0.706326 0.0180333 0.0690353" mass="0.500168"
|
|
||||||
diaginertia="0.0151047 0.0148285 0.00275805"/>
|
|
||||||
<joint name="wam/elbow_pitch_joint" range="-0.9 3.14159" damping="0.88"/>
|
|
||||||
<geom class="viz" mesh="elbow_link_fine" rgba="1 1 1 0"/>
|
|
||||||
<geom class="col" mesh="elbow_link_convex"/>
|
|
||||||
<geom class="viz" mesh="forearm_link_fine" pos="-.045 -0.0730 0" euler="1.57 0 0" rgba="1 1 1 0"/>
|
|
||||||
<geom class="col" mesh="forearm_link_convex_decomposition_p1" pos="-0.045 -0.0730 0"
|
|
||||||
euler="1.57 0 0" rgba="0.094 0.48 0.804 1"/>
|
|
||||||
<geom class="col" mesh="forearm_link_convex_decomposition_p2" pos="-.045 -0.0730 0"
|
|
||||||
euler="1.57 0 0" rgba="0.094 0.48 0.804 1"/>
|
|
||||||
<body name="wam/wrist_yaw_link" pos="-0.045 -0.3 0" quat="0.707107 0.707107 0 0">
|
|
||||||
<inertial pos="8.921e-005 0.00435824 -0.00511217"
|
|
||||||
quat="0.630602 0.776093 0.00401969 -0.002372" mass="1.05376"
|
|
||||||
diaginertia="0.000555168 0.00046317 0.000234072"/> <!--this is an approximation-->
|
|
||||||
<joint name="wam/wrist_yaw_joint" range="-4.55 1.25" damping="0.55"/>
|
|
||||||
<geom class="viz" mesh="wrist_yaw_link_fine" rgba="1 1 1 0"/>
|
|
||||||
<geom class="col" mesh="wrist_yaw_link_convex_decomposition_p1"/>
|
|
||||||
<geom class="col" mesh="wrist_yaw_link_convex_decomposition_p2"/>
|
|
||||||
<body name="wam/wrist_pitch_link" pos="0 0 0" quat="0.707107 -0.707107 0 0">
|
|
||||||
<inertial pos="-0.00012262 -0.0246834 -0.0170319"
|
|
||||||
quat="0.630602 0.776093 0.00401969 -0.002372" mass="0.517974"
|
|
||||||
diaginertia="0.000555168 0.00046317 0.000234072"/>
|
|
||||||
<joint name="wam/wrist_pitch_joint" range="-1.5707 1.5707" damping="0.11"/>
|
|
||||||
<geom class="viz" mesh="wrist_pitch_link_fine" rgba="1 1 1 0"/>
|
|
||||||
<geom class="col" mesh="wrist_pitch_link_convex_decomposition_p1" rgba="1 0.5 0.313 1"/>
|
|
||||||
<geom class="col" mesh="wrist_pitch_link_convex_decomposition_p2" rgba="1 0.5 0.313 1"/>
|
|
||||||
<geom class="col" mesh="wrist_pitch_link_convex_decomposition_p3" rgba="1 0.5 0.313 1"/>
|
|
||||||
<body name="wam/wrist_palm_link" pos="0 0 0" quat="0.707107 0.707107 0 0">
|
|
||||||
<inertial pos="0 0 0.055" quat="0.707107 0 0 0.707107" mass="0.0828613"
|
|
||||||
diaginertia="0.00020683 0.00010859 0.00010851"/>
|
|
||||||
<joint name="wam/palm_yaw_joint" range="-3 3" damping="0.11"/>
|
|
||||||
<geom class="viz" mesh="wrist_palm_link_fine" rgba="1 1 1 0"/>
|
|
||||||
<geom class="col" mesh="wrist_palm_link_convex"/>
|
|
||||||
<body name="paddle_left" pos="0 0 0.26" childclass="contact_geom">
|
|
||||||
<geom name="bat_left" type="cylinder" size="0.075 0.0015" rgba="1 0 0 1"
|
|
||||||
quat="0.71 0 0.71 0"/>
|
|
||||||
<geom name="handle_left" type="box" size="0.005 0.01 0.05" pos="0 0 -0.08"
|
|
||||||
rgba="1 1 1 1"/>
|
|
||||||
</body>
|
|
||||||
</body>
|
|
||||||
</body>
|
|
||||||
</body>
|
|
||||||
</body>
|
|
||||||
</body>
|
|
||||||
</body>
|
|
||||||
</body>
|
|
||||||
</body>
|
|
||||||
</mujocoinclude>
|
|
||||||
@ -1,95 +0,0 @@
|
|||||||
<mujocoinclue>
|
|
||||||
<body name="wam/base_link_right" pos="2.5 0 2" quat="0 0 1 0" childclass="wam" >
|
|
||||||
<inertial pos="0 0 0" mass="1" diaginertia="0.1 0.1 0.1"/>
|
|
||||||
<geom name="base_link_fine" class="viz" mesh="base_link_fine" rgba="0.5 0.5 0.5 0"/>
|
|
||||||
<geom name="base_link_convex" class="col" mesh="base_link_convex" rgba="0.5 0.5 0.5 1"/>
|
|
||||||
<body name="wam/shoulder_yaw_link_right" pos="0 0 0.346">
|
|
||||||
<inertial pos="-0.00443422 -0.00066489 -0.128904" quat="0.69566 0.716713 -0.0354863 0.0334839" mass="5"
|
|
||||||
diaginertia="0.135089 0.113095 0.0904426"/>
|
|
||||||
<joint name="wam/base_yaw_joint_right" range="-2.6 2.6" damping="1.98"/>
|
|
||||||
<geom name="shoulder_link_fine" class="viz" mesh="shoulder_link_fine" rgba="1 1 1 0"/>
|
|
||||||
<geom name="shoulder_link_convex_decomposition_p1" class="col"
|
|
||||||
mesh="shoulder_link_convex_decomposition_p1"/>
|
|
||||||
<geom name="shoulder_link_convex_decomposition_p2" class="col"
|
|
||||||
mesh="shoulder_link_convex_decomposition_p2"/>
|
|
||||||
<geom name="shoulder_link_convex_decomposition_p3" class="col"
|
|
||||||
mesh="shoulder_link_convex_decomposition_p3"/>
|
|
||||||
<body name="wam/shoulder_pitch_link_right" pos="0 0 0" quat="0.707107 -0.707107 0 0">
|
|
||||||
<inertial pos="-0.00236981 -0.0154211 0.0310561" quat="0.961794 0.273112 -0.0169316 0.00866592"
|
|
||||||
mass="3.87494" diaginertia="0.0214195 0.0167127 0.0126452"/> <!--seems off-->
|
|
||||||
<joint name="wam/shoulder_pitch_joint_right" range="-2 2" damping="0.55"/>
|
|
||||||
<geom name="shoulder_pitch_link_fine" class="viz" mesh="shoulder_pitch_link_fine" rgba="1 1 1 0"/>
|
|
||||||
<geom name="shoulder_pitch_link_convex" class="col" mesh="shoulder_pitch_link_convex"/>
|
|
||||||
<body name="wam/upper_arm_link_right" pos="0 0 0" quat="0.707107 0.707107 0 0">
|
|
||||||
<inertial pos="0.00683259 3.309e-005 0.392492" quat="0.647136 0.0170822 0.0143038 0.762049"
|
|
||||||
mass="2.20228" diaginertia="0.0592718 0.0592207 0.00313419"/>
|
|
||||||
<joint name="wam/shoulder_yaw_joint_right" range="-2.8 2.8" damping="1.65"/>
|
|
||||||
<geom name="upper_arm_link_fine" class="viz" mesh="upper_arm_link_fine" rgba="1 1 1 0"/>
|
|
||||||
<geom name="upper_arm_link_convex_decomposition_p1" class="col"
|
|
||||||
mesh="upper_arm_link_convex_decomposition_p1" rgba="0.094 0.48 0.804 1"/>
|
|
||||||
<geom name="upper_arm_link_convex_decomposition_p2" class="col"
|
|
||||||
mesh="upper_arm_link_convex_decomposition_p2" rgba="0.094 0.48 0.804 1"/>
|
|
||||||
<body name="wam/forearm_link_right" pos="0.045 0 0.55" quat="0.707107 -0.707107 0 0">
|
|
||||||
<inertial pos="-0.0400149 -0.142717 -0.00022942"
|
|
||||||
quat="0.704281 0.706326 0.0180333 0.0690353" mass="0.500168"
|
|
||||||
diaginertia="0.0151047 0.0148285 0.00275805"/>
|
|
||||||
<joint name="wam/elbow_pitch_joint_right" range="-0.9 3.1" damping="0.88"/>
|
|
||||||
<geom name="elbow_link_fine" class="viz" mesh="elbow_link_fine" rgba="1 1 1 0"/>
|
|
||||||
<geom name="elbow_link_convex" class="col" mesh="elbow_link_convex"/>
|
|
||||||
<geom name="forearm_link_fine" class="viz" mesh="forearm_link_fine" pos="-.045 -0.0730 0"
|
|
||||||
euler="1.57 0 0" rgba="1 1 1 0"/>
|
|
||||||
<geom name="forearm_link_convex_decomposition_p1" class="col"
|
|
||||||
mesh="forearm_link_convex_decomposition_p1" pos="-0.045 -0.0730 0"
|
|
||||||
euler="1.57 0 0" rgba="0.094 0.48 0.804 1"/>
|
|
||||||
<geom name="forearm_link_convex_decomposition_p2" class="col"
|
|
||||||
mesh="forearm_link_convex_decomposition_p2" pos="-.045 -0.0730 0"
|
|
||||||
euler="1.57 0 0" rgba="0.094 0.48 0.804 1"/>
|
|
||||||
<body name="wam/wrist_yaw_link_right" pos="-0.045 -0.3 0" quat="0.707107 0.707107 0 0">
|
|
||||||
<inertial pos="8.921e-005 0.00435824 -0.00511217"
|
|
||||||
quat="0.630602 0.776093 0.00401969 -0.002372" mass="1.05376"
|
|
||||||
diaginertia="0.000555168 0.00046317 0.000234072"/> <!--this is an approximation-->
|
|
||||||
<joint name="wam/wrist_yaw_joint_right" range="-4.8 1.3" damping="0.55"/>
|
|
||||||
<geom name="wrist_yaw_link_fine" class="viz" mesh="wrist_yaw_link_fine" rgba="1 1 1 0"/>
|
|
||||||
<geom name="wrist_yaw_link_convex_decomposition_p1" class="col"
|
|
||||||
mesh="wrist_yaw_link_convex_decomposition_p1"/>
|
|
||||||
<geom name="wrist_yaw_link_convex_decomposition_p2" class="col"
|
|
||||||
mesh="wrist_yaw_link_convex_decomposition_p2"/>
|
|
||||||
<body name="wam/wrist_pitch_link_right" pos="0 0 0" quat="0.707107 -0.707107 0 0">
|
|
||||||
<inertial pos="-0.00012262 -0.0246834 -0.0170319"
|
|
||||||
quat="0.630602 0.776093 0.00401969 -0.002372" mass="0.517974"
|
|
||||||
diaginertia="0.000555168 0.00046317 0.000234072"/>
|
|
||||||
<joint name="wam/wrist_pitch_joint_right" range="-1.6 1.6" damping="0.11"/>
|
|
||||||
<geom name="wrist_pitch_link_fine" class="viz" mesh="wrist_pitch_link_fine"
|
|
||||||
rgba="1 1 1 0"/>
|
|
||||||
<geom name="wrist_pitch_link_convex_decomposition_p1" rgba="1 0.5 0.313 1"
|
|
||||||
class="col" mesh="wrist_pitch_link_convex_decomposition_p1"/>
|
|
||||||
<geom name="wrist_pitch_link_convex_decomposition_p2" rgba="1 0.5 0.313 1"
|
|
||||||
class="col" mesh="wrist_pitch_link_convex_decomposition_p2"/>
|
|
||||||
<geom name="wrist_pitch_link_convex_decomposition_p3" rgba="1 0.5 0.313 1"
|
|
||||||
class="col" mesh="wrist_pitch_link_convex_decomposition_p3"/>
|
|
||||||
<body name="wam/wrist_palm_link_right" pos="0 0 0" quat="0.707107 0.707107 0 0">
|
|
||||||
<inertial pos="0 0 0.055" quat="0.707107 0 0 0.707107" mass="0.0828613"
|
|
||||||
diaginertia="0.00020683 0.00010859 0.00010851"/>
|
|
||||||
<joint name="wam/palm_yaw_joint_right" range="-2.2 2.2" damping="0.11"/>
|
|
||||||
<geom name="wrist_palm_link_fine" class="viz" mesh="wrist_palm_link_fine"
|
|
||||||
rgba="1 1 1 0"/>
|
|
||||||
<geom name="wrist_palm_link_convex" class="col" mesh="wrist_palm_link_convex"/>
|
|
||||||
<!-- EE=wam/paddle, configure name to the end effector name-->
|
|
||||||
<body name="EE" pos="0 0 0.26" childclass="contact_geom">
|
|
||||||
<geom name="bat" type="cylinder" size="0.075 0.005" rgba="1 0 0 1"
|
|
||||||
quat="0.71 0 0.71 0"/>
|
|
||||||
<geom name="wam/paddle_handle" type="box" size="0.005 0.01 0.05" pos="0 0 -0.08"
|
|
||||||
rgba="1 1 1 1"/>
|
|
||||||
<!-- Extract information for sampling goals.-->
|
|
||||||
<site name="wam/paddle_center" pos="0 0 0" rgba="1 1 1 1" size="0.00001"/>
|
|
||||||
|
|
||||||
</body>
|
|
||||||
</body>
|
|
||||||
</body>
|
|
||||||
</body>
|
|
||||||
</body>
|
|
||||||
</body>
|
|
||||||
</body>
|
|
||||||
</body>
|
|
||||||
</body>
|
|
||||||
</mujocoinclue>
|
|
||||||
@ -1,38 +0,0 @@
|
|||||||
<mujocoinclude>
|
|
||||||
<body name="table_tennis_table" pos="0 0 0">
|
|
||||||
<geom class="contact_geom" name="table_base_1" type="box" size="0.05 0.05 .375" rgba="1 1 1 1"
|
|
||||||
pos="1 0.7 0.375"/>
|
|
||||||
<geom class="contact_geom" name="table_base_2" type="box" size="0.05 0.05 .375" rgba="1 1 1 1"
|
|
||||||
pos="1 -0.7 0.375"/>
|
|
||||||
<geom class="contact_geom" name="table_base_3" type="box" size="0.05 0.05 .375" rgba="1 1 1 1"
|
|
||||||
pos="-1 -0.7 0.375"/>
|
|
||||||
<geom class="contact_geom" name="table_base_4" type="box" size="0.05 0.05 .375" rgba="1 1 1 1"
|
|
||||||
pos="-1 0.7 0.375"/>
|
|
||||||
<body name="table_top" pos="0 0 0.76">
|
|
||||||
<geom class="contact_geom" name="table_tennis_table" type="box" size="1.37 .7625 .01" rgba="0 0 0.5 1"
|
|
||||||
pos="0 0 0"/>
|
|
||||||
<!-- <geom class="contact_geom" name="table_tennis_table_right_side" type="box" size="0.685 .7625 .01"-->
|
|
||||||
<!-- rgba="0.5 0 0 1" pos="0.685 0 0"/>-->
|
|
||||||
<!-- <geom class="contact_geom" name="table_tennis_table_left_side" type="box" size="0.685 .7625 .01"-->
|
|
||||||
<!-- rgba="0 0.5 0 1" pos="-0.685 0 0"/>-->
|
|
||||||
<site name="left_up_corner" pos="-1.37 .7625 0.01" rgba="1 1 1 1" size="0.00001"/>
|
|
||||||
<site name="middle_up_corner" pos="0 .7625 0.01" rgba="1 1 1 1" size="0.00001"/>
|
|
||||||
<site name="left_down_corner" pos="-1.37 -0.7625 0.01" rgba="1 1 1 1" size="0.00001"/>
|
|
||||||
<site name="middle_down_corner" pos="0 -.7625 0.01" rgba="1 1 1 1" size="0.00001"/>
|
|
||||||
<geom class="contact_geom" name="table_tennis_net" type="box" size="0.01 0.915 0.07625"
|
|
||||||
material="floor_plane"
|
|
||||||
rgba="0 0 1 0.5"
|
|
||||||
pos="0 0 0.08625"/>
|
|
||||||
<geom class="contact_geom" name="left_while_line" type="box" size="1.37 .02 .001" rgba="1 1 1 1"
|
|
||||||
pos="0 -0.7425 0.01"/>
|
|
||||||
<geom class="contact_geom" name="center_while_line" type="box" size="1.37 .01 .001" rgba="1 1 1 1"
|
|
||||||
pos="0 0 0.01"/>
|
|
||||||
<geom class="contact_geom" name="right_while_line" type="box" size="1.37 .02 .001" rgba="1 1 1 1"
|
|
||||||
pos="0 0.7425 0.01"/>
|
|
||||||
<geom class="contact_geom" name="right_side_line" type="box" size="0.02 .7625 .001" rgba="1 1 1 1"
|
|
||||||
pos="1.35 0 0.01"/>
|
|
||||||
<geom class="contact_geom" name="left_side_line" type="box" size="0.02 .7625 .001" rgba="1 1 1 1"
|
|
||||||
pos="-1.35 0 0.01"/>
|
|
||||||
</body>
|
|
||||||
</body>
|
|
||||||
</mujocoinclude>
|
|
||||||
@ -1,10 +0,0 @@
|
|||||||
<mujocoinclude>
|
|
||||||
<body name="target_ball" pos="-1.2 -0.6 1.5">
|
|
||||||
<joint axis="1 0 0" damping="0.0" name="tar:x" pos="0 0 0" stiffness="0" type="slide" frictionloss="0"/>
|
|
||||||
<joint axis="0 1 0" damping="0.0" name="tar:y" pos="0 0 0" stiffness="0" type="slide" frictionloss="0"/>
|
|
||||||
<joint axis="0 0 1" damping="0.0" name="tar:z" pos="0 0 0" stiffness="0" type="slide" frictionloss="0"/>
|
|
||||||
<geom size="0.025 0.025 0.025" type="sphere" condim="4" name="target_ball" rgba="1 1 0 1" mass="0.1"
|
|
||||||
friction="0.1 0.1 0.1" solimp="1 1 0" solref="0.1 0.03"/>
|
|
||||||
<site name="target_ball" pos="0 0 0" size="0.02 0.02 0.02" rgba="1 0 0 1" type="sphere"/>
|
|
||||||
</body>
|
|
||||||
</mujocoinclude>
|
|
||||||
@ -1,80 +0,0 @@
|
|||||||
<mujocoinclude>
|
|
||||||
<body name="test_ball_table" pos="1 0 4">
|
|
||||||
<joint axis="1 0 0" damping="0.0" name="tar:x_test_ball_table" pos="0 0 0" stiffness="0" type="slide"
|
|
||||||
frictionloss="0"/>
|
|
||||||
<joint axis="0 1 0" damping="0.0" name="tar:y_test_ball_table" pos="0 0 0" stiffness="0" type="slide"
|
|
||||||
frictionloss="0"/>
|
|
||||||
<joint axis="0 0 1" damping="0.0" name="tar:z_test_ball_table" pos="0 0 0" stiffness="0" type="slide"
|
|
||||||
frictionloss="0"/>
|
|
||||||
<geom size="0.025 0.025 0.025" type="sphere" condim="4" name="test_ball_table" rgba="0 1 0 1" mass="0.1"
|
|
||||||
friction="0.1 0.1 0.1" solimp="1 1 0" solref="0.1 0.03"/>
|
|
||||||
<site name="test_ball_table" pos="0 0 0" size="0.02 0.02 0.02" rgba="0 1 0 1" type="sphere"/>
|
|
||||||
</body>
|
|
||||||
<body name="test_ball_net" pos="0 0 4">
|
|
||||||
<joint axis="1 0 0" damping="0.0" name="tar:x_test_ball_net" pos="0 0 0" stiffness="0" type="slide"
|
|
||||||
frictionloss="0"/>
|
|
||||||
<joint axis="0 1 0" damping="0.0" name="tar:y_test_ball_net" pos="0 0 0" stiffness="0" type="slide"
|
|
||||||
frictionloss="0"/>
|
|
||||||
<joint axis="0 0 1" damping="0.0" name="tar:z_test_ball_net" pos="0 0 0" stiffness="0" type="slide"
|
|
||||||
frictionloss="0"/>
|
|
||||||
<geom size="0.025 0.025 0.025" type="sphere" condim="4" name="test_ball_net" rgba="1 1 0 1" mass="0.1"
|
|
||||||
friction="0.1 0.1 0.1" solimp="1 1 0" solref="0.1 0.03"/>
|
|
||||||
<site name="test_ball_net" pos="0 0 0" size="0.02 0.02 0.02" rgba="0 1 0 1" type="sphere"/>
|
|
||||||
</body>
|
|
||||||
<body name="test_ball_racket_0" pos="2.54919187 0.81642672 4">
|
|
||||||
<joint axis="1 0 0" damping="0.0" name="tar:x_test_ball_racket_0" pos="0 0 0" stiffness="0" type="slide"
|
|
||||||
frictionloss="0"/>
|
|
||||||
<joint axis="0 1 0" damping="0.0" name="tar:y_test_ball_racket_0" pos="0 0 0" stiffness="0" type="slide"
|
|
||||||
frictionloss="0"/>
|
|
||||||
<joint axis="0 0 1" damping="0.0" name="tar:z_test_ball_racket_0" pos="0 0 0" stiffness="0" type="slide"
|
|
||||||
frictionloss="0"/>
|
|
||||||
<geom size="0.025 0.025 0.025" type="sphere" condim="4" name="test_ball_racket_0" rgba="1 0 1 1" mass="0.1"
|
|
||||||
friction="0.1 0.1 0.1" solimp="1 1 0" solref="0.1 0.03"/>
|
|
||||||
<site name="test_ball_racket_0" pos="0 0 0" size="0.02 0.02 0.02" rgba="0 1 0 1" type="sphere"/>
|
|
||||||
</body>
|
|
||||||
<body name="test_ball_racket_1" pos="2.54919187 0.81642672 4.5">
|
|
||||||
<joint axis="1 0 0" damping="0.0" name="tar:x_test_ball_racket_1" pos="0 0 0" stiffness="0" type="slide"
|
|
||||||
frictionloss="0"/>
|
|
||||||
<joint axis="0 1 0" damping="0.0" name="tar:y_test_ball_racket_1" pos="0 0 0" stiffness="0" type="slide"
|
|
||||||
frictionloss="0"/>
|
|
||||||
<joint axis="0 0 1" damping="0.0" name="tar:z_test_ball_racket_1" pos="0 0 0" stiffness="0" type="slide"
|
|
||||||
frictionloss="0"/>
|
|
||||||
<geom size="0.025 0.025 0.025" type="sphere" condim="4" name="test_ball_racket_1" rgba="1 0 1 1" mass="0.1"
|
|
||||||
friction="0.1 0.1 0.1" solimp="1 1 0" solref="0.1 0.03"/>
|
|
||||||
<site name="test_ball_racket_1" pos="0 0 0" size="0.02 0.02 0.02" rgba="0 1 0 1" type="sphere"/>
|
|
||||||
</body>
|
|
||||||
<body name="test_ball_racket_2" pos="2.54919187 0.81642672 3">
|
|
||||||
<joint axis="1 0 0" damping="0.0" name="tar:x_test_ball_racket_2" pos="0 0 0" stiffness="0" type="slide"
|
|
||||||
frictionloss="0"/>
|
|
||||||
<joint axis="0 1 0" damping="0.0" name="tar:y_test_ball_racket_2" pos="0 0 0" stiffness="0" type="slide"
|
|
||||||
frictionloss="0"/>
|
|
||||||
<joint axis="0 0 1" damping="0.0" name="tar:z_test_ball_racket_2" pos="0 0 0" stiffness="0" type="slide"
|
|
||||||
frictionloss="0"/>
|
|
||||||
<geom size="0.025 0.025 0.025" type="sphere" condim="4" name="test_ball_racket_2" rgba="1 0 1 1" mass="0.1"
|
|
||||||
friction="0.1 0.1 0.1" solimp="1 1 0" solref="0.1 0.03"/>
|
|
||||||
<site name="test_ball_racket" pos="0 0 0" size="0.02 0.02 0.02" rgba="0 1 0 1" type="sphere"/>
|
|
||||||
</body>
|
|
||||||
<body name="test_ball_racket_3" pos="2.54919187 0.81642672 10">
|
|
||||||
<joint axis="1 0 0" damping="0.0" name="tar:x_test_ball_racket_3" pos="0 0 0" stiffness="0" type="slide"
|
|
||||||
frictionloss="0"/>
|
|
||||||
<joint axis="0 1 0" damping="0.0" name="tar:y_test_ball_racket_3" pos="0 0 0" stiffness="0" type="slide"
|
|
||||||
frictionloss="0"/>
|
|
||||||
<joint axis="0 0 1" damping="0.0" name="tar:z_test_ball_racket_3" pos="0 0 0" stiffness="0" type="slide"
|
|
||||||
frictionloss="0"/>
|
|
||||||
<geom size="0.025 0.025 0.025" type="sphere" condim="4" name="test_ball_racket_3" rgba="1 0 1 1" mass="0.1"
|
|
||||||
friction="0.1 0.1 0.1" solimp="1 1 0" solref="0.1 0.03"/>
|
|
||||||
<site name="test_ball_racket_3" pos="0 0 0" size="0.02 0.02 0.02" rgba="0 1 0 1" type="sphere"/>
|
|
||||||
</body>
|
|
||||||
<!-- <body name="test_ball_racket_4" pos="2.54919187 0.81642672 4">-->
|
|
||||||
<!-- <joint axis="1 0 0" damping="0.0" name="tar:x_test_ball_racket_4" pos="0 0 0" stiffness="0" type="slide"-->
|
|
||||||
<!-- frictionloss="0"/>-->
|
|
||||||
<!-- <joint axis="0 1 0" damping="0.0" name="tar:y_test_ball_racket_4" pos="0 0 0" stiffness="0" type="slide"-->
|
|
||||||
<!-- frictionloss="0"/>-->
|
|
||||||
<!-- <joint axis="0 0 1" damping="0.0" name="tar:z_test_ball_racket_4" pos="0 0 0" stiffness="0" type="slide"-->
|
|
||||||
<!-- frictionloss="0"/>-->
|
|
||||||
<!-- <geom size="0.025 0.025 0.025" type="sphere" condim="4" name="test_ball_racket_4" rgba="1 0 0 1" mass="0.1"-->
|
|
||||||
<!-- friction="0.1 0.1 0.1" solimp="1 1 0" solref="0.1 0.03"/>-->
|
|
||||||
<!-- <site name="test_ball_racket_4" pos="0 0 0" size="0.02 0.02 0.02" rgba="0 1 0 1" type="sphere"/>-->
|
|
||||||
<!-- </body>-->
|
|
||||||
|
|
||||||
</mujocoinclude>
|
|
||||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@ -1,19 +0,0 @@
|
|||||||
<mujocoinclude>
|
|
||||||
<actuator>
|
|
||||||
|
|
||||||
<!-- <position ctrlrange="-2.6 2.6" joint="wam/base_yaw_joint_right" kp="100.0" />-->
|
|
||||||
<!-- <position ctrlrange="-1.985 1.985" joint="wam/shoulder_pitch_joint_right" kp="162.0" />-->
|
|
||||||
<!-- <position ctrlrange="-2.8 2.8" joint="wam/shoulder_yaw_joint_right" kp="100.0" />-->
|
|
||||||
<!-- <position ctrlrange="-0.9 3.14159" joint="wam/elbow_pitch_joint_right" kp="122.0" />-->
|
|
||||||
<!-- <position ctrlrange="-4.55 1.25" joint="wam/wrist_yaw_joint_right" kp="100.0" />-->
|
|
||||||
<!-- <position ctrlrange="-1.5707 1.5707" joint="wam/wrist_pitch_joint_right" kp="102.0" />-->
|
|
||||||
<!-- <position ctrlrange="-3 3" joint="wam/palm_yaw_joint_right" kp="100.0" />-->
|
|
||||||
<position ctrlrange="-2.6 2.6" joint="wam/base_yaw_joint_right" kp="151.0"/>
|
|
||||||
<position ctrlrange="-1.985 1.985" joint="wam/shoulder_pitch_joint_right" kp="125.0"/>
|
|
||||||
<position ctrlrange="-2.8 2.8" joint="wam/shoulder_yaw_joint_right" kp="122.0"/>
|
|
||||||
<position ctrlrange="-0.9 3.14159" joint="wam/elbow_pitch_joint_right" kp="121.0"/>
|
|
||||||
<position ctrlrange="-4.55 1.25" joint="wam/wrist_yaw_joint_right" kp="99.0"/>
|
|
||||||
<position ctrlrange="-1.5707 1.5707" joint="wam/wrist_pitch_joint_right" kp="103.0"/>
|
|
||||||
<position ctrlrange="-3 3" joint="wam/palm_yaw_joint_right" kp="99.0"/>
|
|
||||||
</actuator>
|
|
||||||
</mujocoinclude>
|
|
||||||
@ -1,49 +0,0 @@
|
|||||||
<mujocoinclude>
|
|
||||||
<default>
|
|
||||||
<default class="wam">
|
|
||||||
<joint type="hinge" limited="true" pos="0 0 0" axis="0 0 1"/>
|
|
||||||
</default>
|
|
||||||
<default class="viz">
|
|
||||||
<geom type="mesh" contype="0" conaffinity="0" group="1" rgba="1 1 1 1"/>
|
|
||||||
</default>
|
|
||||||
<default class="col">
|
|
||||||
<geom type="mesh" contype="0" conaffinity="1" group="0" rgba="1 1 1 1"/>
|
|
||||||
</default>
|
|
||||||
<default class="contact_geom">
|
|
||||||
<geom condim="4" friction="0.1 0.1 0.1" margin="0" solimp="1 1 0" solref="0.1 0.03"/>
|
|
||||||
<!-- <geom condim="4" friction="0 0 0" margin="0" solimp="1 1 0" solref="0.01 1.1"/>-->
|
|
||||||
|
|
||||||
</default>
|
|
||||||
</default>
|
|
||||||
<asset>
|
|
||||||
<mesh file="base_link_fine.stl"/>
|
|
||||||
<mesh file="base_link_convex.stl"/>
|
|
||||||
<mesh file="shoulder_link_fine.stl"/>
|
|
||||||
<mesh file="shoulder_link_convex_decomposition_p1.stl"/>
|
|
||||||
<mesh file="shoulder_link_convex_decomposition_p2.stl"/>
|
|
||||||
<mesh file="shoulder_link_convex_decomposition_p3.stl"/>
|
|
||||||
<mesh file="shoulder_pitch_link_fine.stl"/>
|
|
||||||
<mesh file="shoulder_pitch_link_convex.stl"/>
|
|
||||||
<mesh file="upper_arm_link_fine.stl"/>
|
|
||||||
<mesh file="upper_arm_link_convex_decomposition_p1.stl"/>
|
|
||||||
<mesh file="upper_arm_link_convex_decomposition_p2.stl"/>
|
|
||||||
<mesh file="elbow_link_fine.stl"/>
|
|
||||||
<mesh file="elbow_link_convex.stl"/>
|
|
||||||
<mesh file="forearm_link_fine.stl"/>
|
|
||||||
<mesh file="forearm_link_convex_decomposition_p1.stl"/>
|
|
||||||
<mesh file="forearm_link_convex_decomposition_p2.stl"/>
|
|
||||||
<mesh file="wrist_yaw_link_fine.stl"/>
|
|
||||||
<mesh file="wrist_yaw_link_convex_decomposition_p1.stl"/>
|
|
||||||
<mesh file="wrist_yaw_link_convex_decomposition_p2.stl"/>
|
|
||||||
<mesh file="wrist_pitch_link_fine.stl"/>
|
|
||||||
<mesh file="wrist_pitch_link_convex_decomposition_p1.stl"/>
|
|
||||||
<mesh file="wrist_pitch_link_convex_decomposition_p2.stl"/>
|
|
||||||
<mesh file="wrist_pitch_link_convex_decomposition_p3.stl"/>
|
|
||||||
<mesh file="wrist_palm_link_fine.stl"/>
|
|
||||||
<mesh file="wrist_palm_link_convex.stl"/>
|
|
||||||
|
|
||||||
<texture builtin="checker" height="512" name="texplane" rgb1=".2 .3 .4" rgb2=".1 0.15 0.2" type="2d"
|
|
||||||
width="512"/>
|
|
||||||
<material name="floor_plane" reflectance="0.5" texrepeat="1 1" texture="texplane" texuniform="true"/>
|
|
||||||
</asset>
|
|
||||||
</mujocoinclude>
|
|
||||||
@ -1,41 +0,0 @@
|
|||||||
<mujoco model="table_tennis(v0.1)">
|
|
||||||
<compiler angle="radian" coordinate="local" meshdir="meshes/" />
|
|
||||||
|
|
||||||
<option gravity="0 0 -9.81" timestep="0.002">
|
|
||||||
<flag warmstart="enable" />
|
|
||||||
</option>
|
|
||||||
|
|
||||||
|
|
||||||
<custom>
|
|
||||||
<numeric data="0 0 0 0 0 0 0" name="START_ANGLES" />
|
|
||||||
</custom>
|
|
||||||
|
|
||||||
|
|
||||||
<include file="shared.xml" />
|
|
||||||
|
|
||||||
<worldbody>
|
|
||||||
<light cutoff="60" diffuse="1 1 1" dir="-.1 -.2 -1.3" directional="true" exponent="1" pos=".1 .2 1.3" specular=".1 .1 .1" />
|
|
||||||
<geom conaffinity="1" contype="1" material="floor_plane" name="floor" pos="0 0 0" size="10 5 1" type="plane" />
|
|
||||||
|
|
||||||
|
|
||||||
<include file="include_table.xml" />
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
<include file="include_barrett_wam_7dof_right.xml" />
|
|
||||||
|
|
||||||
<include file="include_target_ball.xml" />
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
</worldbody>
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
<include file="right_arm_actuator.xml" />
|
|
||||||
|
|
||||||
|
|
||||||
</mujoco>
|
|
||||||
@ -1,244 +0,0 @@
|
|||||||
import numpy as np
|
|
||||||
from gym import spaces
|
|
||||||
from gym.envs.robotics import robot_env, utils
|
|
||||||
# import xml.etree.ElementTree as ET
|
|
||||||
from alr_envs.alr.mujoco.gym_table_tennis.utils.rewards.hierarchical_reward import HierarchicalRewardTableTennis
|
|
||||||
import glfw
|
|
||||||
from alr_envs.alr.mujoco.gym_table_tennis.utils.experiment import ball_initialize
|
|
||||||
from pathlib import Path
|
|
||||||
import os
|
|
||||||
|
|
||||||
|
|
||||||
class TableTennisEnv(robot_env.RobotEnv):
|
|
||||||
"""Class for Table Tennis environment.
|
|
||||||
"""
|
|
||||||
def __init__(self, n_substeps=1,
|
|
||||||
model_path=None,
|
|
||||||
initial_qpos=None,
|
|
||||||
initial_ball_state=None,
|
|
||||||
config=None,
|
|
||||||
reward_obj=None
|
|
||||||
):
|
|
||||||
"""Initializes a new mujoco based Table Tennis environment.
|
|
||||||
|
|
||||||
Args:
|
|
||||||
model_path (string): path to the environments XML file
|
|
||||||
initial_qpos (dict): a dictionary of joint names and values that define the initial
|
|
||||||
n_actions: Number of joints
|
|
||||||
n_substeps (int): number of substeps the simulation runs on every call to step
|
|
||||||
scale (double): limit maximum change in position
|
|
||||||
initial_ball_state: to reset the ball state
|
|
||||||
"""
|
|
||||||
# self.config = config.config
|
|
||||||
if model_path is None:
|
|
||||||
path_cws = Path.cwd()
|
|
||||||
print(path_cws)
|
|
||||||
current_dir = Path(os.path.split(os.path.realpath(__file__))[0])
|
|
||||||
table_tennis_env_xml_path = current_dir / "assets"/"table_tennis_env.xml"
|
|
||||||
model_path = str(table_tennis_env_xml_path)
|
|
||||||
self.config = config
|
|
||||||
action_space = True # self.config['trajectory']['args']['action_space']
|
|
||||||
time_step = 0.002 # self.config['mujoco_sim_env']['args']["time_step"]
|
|
||||||
if initial_qpos is None:
|
|
||||||
initial_qpos = {"wam/base_yaw_joint_right": 1.5,
|
|
||||||
"wam/shoulder_pitch_joint_right": 1,
|
|
||||||
"wam/shoulder_yaw_joint_right": 0,
|
|
||||||
"wam/elbow_pitch_joint_right": 1,
|
|
||||||
"wam/wrist_yaw_joint_right": 1,
|
|
||||||
"wam/wrist_pitch_joint_right": 0,
|
|
||||||
"wam/palm_yaw_joint_right": 0}
|
|
||||||
# initial_qpos = [1.5, 1, 0, 1, 1, 0, 0] # self.config['robot_config']['args']['initial_qpos']
|
|
||||||
|
|
||||||
# TODO should read all configuration in config
|
|
||||||
assert initial_qpos is not None, "Must initialize the initial q position of robot arm"
|
|
||||||
n_actions = 7
|
|
||||||
self.initial_qpos_value = np.array(list(initial_qpos.values())).copy()
|
|
||||||
# self.initial_qpos_value = np.array(initial_qpos)
|
|
||||||
# # change time step in .xml file
|
|
||||||
# tree = ET.parse(model_path)
|
|
||||||
# root = tree.getroot()
|
|
||||||
# for option in root.findall('option'):
|
|
||||||
# option.set("timestep", str(time_step))
|
|
||||||
#
|
|
||||||
# tree.write(model_path)
|
|
||||||
|
|
||||||
super(TableTennisEnv, self).__init__(
|
|
||||||
model_path=model_path, n_substeps=n_substeps, n_actions=n_actions,
|
|
||||||
initial_qpos=initial_qpos)
|
|
||||||
|
|
||||||
if action_space:
|
|
||||||
self.action_space = spaces.Box(low=np.array([-2.6, -2.0, -2.8, -0.9, -4.8, -1.6, -2.2]),
|
|
||||||
high=np.array([2.6, 2.0, 2.8, 3.1, 1.3, 1.6, 2.2]),
|
|
||||||
dtype='float64')
|
|
||||||
else:
|
|
||||||
self.action_space = spaces.Box(low=np.array([-np.inf] * 7),
|
|
||||||
high=np.array([-np.inf] * 7),
|
|
||||||
dtype='float64')
|
|
||||||
self.scale = None
|
|
||||||
self.desired_pos = None
|
|
||||||
self.n_actions = n_actions
|
|
||||||
self.action = None
|
|
||||||
self.time_step = time_step
|
|
||||||
self._dt = time_step
|
|
||||||
self.paddle_center_pos = self.sim.data.get_site_xpos('wam/paddle_center')
|
|
||||||
if reward_obj is None:
|
|
||||||
self.reward_obj = HierarchicalRewardTableTennis()
|
|
||||||
else:
|
|
||||||
self.reward_obj = reward_obj
|
|
||||||
|
|
||||||
if initial_ball_state is not None:
|
|
||||||
self.initial_ball_state = initial_ball_state
|
|
||||||
else:
|
|
||||||
self.initial_ball_state = ball_initialize(random=False)
|
|
||||||
self.target_ball_pos = self.sim.data.get_site_xpos("target_ball")
|
|
||||||
self.racket_center_pos = self.sim.data.get_site_xpos("wam/paddle_center")
|
|
||||||
|
|
||||||
def close(self):
|
|
||||||
if self.viewer is not None:
|
|
||||||
glfw.destroy_window(self.viewer.window)
|
|
||||||
# self.viewer.window.close()
|
|
||||||
self.viewer = None
|
|
||||||
self._viewers = {}
|
|
||||||
|
|
||||||
# GoalEnv methods
|
|
||||||
# ----------------------------
|
|
||||||
def compute_reward(self, achieved_goal, goal, info):
|
|
||||||
# reset the reward, if action valid
|
|
||||||
# right_court_contact_obj = ["target_ball", "table_tennis_table_right_side"]
|
|
||||||
# right_court_contact_detector = self.reward_obj.contact_detection(self, right_court_contact_obj)
|
|
||||||
# if right_court_contact_detector:
|
|
||||||
# print("can detect the table ball contact")
|
|
||||||
self.reward_obj.total_reward = 0
|
|
||||||
# Stage 1 Hitting
|
|
||||||
self.reward_obj.hitting(self)
|
|
||||||
# if not hitted, return the highest reward
|
|
||||||
if not self.reward_obj.goal_achievement:
|
|
||||||
# return self.reward_obj.highest_reward
|
|
||||||
return self.reward_obj.total_reward
|
|
||||||
# # Stage 2 Right Table Contact
|
|
||||||
# self.reward_obj.right_table_contact(self)
|
|
||||||
# if not self.reward_obj.goal_achievement:
|
|
||||||
# return self.reward_obj.highest_reward
|
|
||||||
# # Stage 2 Net Contact
|
|
||||||
# self.reward_obj.net_contact(self)
|
|
||||||
# if not self.reward_obj.goal_achievement:
|
|
||||||
# return self.reward_obj.highest_reward
|
|
||||||
# Stage 3 Opponent court Contact
|
|
||||||
# self.reward_obj.landing_on_opponent_court(self)
|
|
||||||
# if not self.reward_obj.goal_achievement:
|
|
||||||
# print("self.reward_obj.highest_reward: ", self.reward_obj.highest_reward)
|
|
||||||
# TODO
|
|
||||||
self.reward_obj.target_achievement(self)
|
|
||||||
# return self.reward_obj.highest_reward
|
|
||||||
return self.reward_obj.total_reward
|
|
||||||
|
|
||||||
def _reset_sim(self):
|
|
||||||
self.sim.set_state(self.initial_state)
|
|
||||||
[initial_x, initial_y, initial_z, v_x, v_y, v_z] = self.initial_ball_state
|
|
||||||
self.sim.data.set_joint_qpos('tar:x', initial_x)
|
|
||||||
self.sim.data.set_joint_qpos('tar:y', initial_y)
|
|
||||||
self.sim.data.set_joint_qpos('tar:z', initial_z)
|
|
||||||
self.energy_corrected = True
|
|
||||||
self.give_reflection_reward = False
|
|
||||||
|
|
||||||
# velocity is positive direction
|
|
||||||
self.sim.data.set_joint_qvel('tar:x', v_x)
|
|
||||||
self.sim.data.set_joint_qvel('tar:y', v_y)
|
|
||||||
self.sim.data.set_joint_qvel('tar:z', v_z)
|
|
||||||
|
|
||||||
# Apply gravity compensation
|
|
||||||
if self.sim.data.qfrc_applied[:7] is not self.sim.data.qfrc_bias[:7]:
|
|
||||||
self.sim.data.qfrc_applied[:7] = self.sim.data.qfrc_bias[:7]
|
|
||||||
self.sim.forward()
|
|
||||||
return True
|
|
||||||
|
|
||||||
def _env_setup(self, initial_qpos):
|
|
||||||
for name, value in initial_qpos.items():
|
|
||||||
self.sim.data.set_joint_qpos(name, value)
|
|
||||||
|
|
||||||
# Apply gravity compensation
|
|
||||||
if self.sim.data.qfrc_applied[:7] is not self.sim.data.qfrc_bias[:7]:
|
|
||||||
self.sim.data.qfrc_applied[:7] = self.sim.data.qfrc_bias[:7]
|
|
||||||
self.sim.forward()
|
|
||||||
|
|
||||||
# Get the target position
|
|
||||||
self.initial_paddle_center_xpos = self.sim.data.get_site_xpos('wam/paddle_center').copy()
|
|
||||||
self.initial_paddle_center_vel = None # self.sim.get_site_
|
|
||||||
|
|
||||||
def _sample_goal(self):
|
|
||||||
goal = self.initial_paddle_center_xpos[:3] + self.np_random.uniform(-0.2, 0.2, size=3)
|
|
||||||
return goal.copy()
|
|
||||||
|
|
||||||
def _get_obs(self):
|
|
||||||
|
|
||||||
# positions of racket center
|
|
||||||
paddle_center_pos = self.sim.data.get_site_xpos('wam/paddle_center')
|
|
||||||
ball_pos = self.sim.data.get_site_xpos("target_ball")
|
|
||||||
|
|
||||||
dt = self.sim.nsubsteps * self.sim.model.opt.timestep
|
|
||||||
paddle_center_velp = self.sim.data.get_site_xvelp('wam/paddle_center') * dt
|
|
||||||
robot_qpos, robot_qvel = utils.robot_get_obs(self.sim)
|
|
||||||
|
|
||||||
wrist_state = robot_qpos[-3:]
|
|
||||||
wrist_vel = robot_qvel[-3:] * dt # change to a scalar if the gripper is made symmetric
|
|
||||||
|
|
||||||
# achieved_goal = paddle_body_EE_pos
|
|
||||||
obs = np.concatenate([
|
|
||||||
paddle_center_pos, paddle_center_velp, wrist_state, wrist_vel
|
|
||||||
])
|
|
||||||
|
|
||||||
out_dict = {
|
|
||||||
'observation': obs.copy(),
|
|
||||||
'achieved_goal': paddle_center_pos.copy(),
|
|
||||||
'desired_goal': self.goal.copy(),
|
|
||||||
'q_pos': self.sim.data.qpos[:].copy(),
|
|
||||||
"ball_pos": ball_pos.copy(),
|
|
||||||
# "hitting_flag": self.reward_obj.hitting_flag
|
|
||||||
}
|
|
||||||
|
|
||||||
return out_dict
|
|
||||||
|
|
||||||
def _step_callback(self):
|
|
||||||
pass
|
|
||||||
|
|
||||||
def _set_action(self, action):
|
|
||||||
# Apply gravity compensation
|
|
||||||
if self.sim.data.qfrc_applied[:7] is not self.sim.data.qfrc_bias[:7]:
|
|
||||||
self.sim.data.qfrc_applied[:7] = self.sim.data.qfrc_bias[:7]
|
|
||||||
# print("set action process running")
|
|
||||||
assert action.shape == (self.n_actions,)
|
|
||||||
self.action = action.copy() # ensure that we don't change the action outside of this scope
|
|
||||||
pos_ctrl = self.action[:] # limit maximum change in position
|
|
||||||
pos_ctrl = np.clip(pos_ctrl, self.action_space.low, self.action_space.high)
|
|
||||||
|
|
||||||
# get desired trajectory
|
|
||||||
self.sim.data.qpos[:7] = pos_ctrl
|
|
||||||
self.sim.forward()
|
|
||||||
self.desired_pos = self.sim.data.get_site_xpos('wam/paddle_center').copy()
|
|
||||||
|
|
||||||
self.sim.data.ctrl[:] = pos_ctrl
|
|
||||||
|
|
||||||
def _is_success(self, achieved_goal, desired_goal):
|
|
||||||
pass
|
|
||||||
|
|
||||||
|
|
||||||
if __name__ == '__main__':
|
|
||||||
render_mode = "human" # "human" or "partial" or "final"
|
|
||||||
env = TableTennisEnv()
|
|
||||||
env.reset()
|
|
||||||
# env.render(mode=render_mode)
|
|
||||||
|
|
||||||
for i in range(500):
|
|
||||||
# objective.load_result("/tmp/cma")
|
|
||||||
# test with random actions
|
|
||||||
ac = env.action_space.sample()
|
|
||||||
# ac[0] += np.pi/2
|
|
||||||
obs, rew, d, info = env.step(ac)
|
|
||||||
env.render(mode=render_mode)
|
|
||||||
|
|
||||||
print(rew)
|
|
||||||
|
|
||||||
if d:
|
|
||||||
break
|
|
||||||
|
|
||||||
env.close()
|
|
||||||
@ -1,83 +0,0 @@
|
|||||||
import numpy as np
|
|
||||||
from gym.utils import seeding
|
|
||||||
from alr_envs.alr.mujoco.gym_table_tennis.utils.util import read_yaml, read_json
|
|
||||||
from pathlib import Path
|
|
||||||
|
|
||||||
|
|
||||||
def ball_initialize(random=False, scale=False, context_range=None, scale_value=None):
|
|
||||||
if random:
|
|
||||||
if scale:
|
|
||||||
# if scale_value is None:
|
|
||||||
scale_value = context_scale_initialize(context_range)
|
|
||||||
v_x, v_y, v_z = [2.5, 2, 0.5] * scale_value
|
|
||||||
dx = 1
|
|
||||||
dy = 0
|
|
||||||
dz = 0.05
|
|
||||||
else:
|
|
||||||
seed = None
|
|
||||||
np_random, seed = seeding.np_random(seed)
|
|
||||||
dx = np_random.uniform(-0.1, 0.1)
|
|
||||||
dy = np_random.uniform(-0.1, 0.1)
|
|
||||||
dz = np_random.uniform(-0.1, 0.1)
|
|
||||||
|
|
||||||
v_x = np_random.uniform(1.7, 1.8)
|
|
||||||
v_y = np_random.uniform(0.7, 0.8)
|
|
||||||
v_z = np_random.uniform(0.1, 0.2)
|
|
||||||
# print(dx, dy, dz, v_x, v_y, v_z)
|
|
||||||
# else:
|
|
||||||
# dx = -0.1
|
|
||||||
# dy = 0.05
|
|
||||||
# dz = 0.05
|
|
||||||
# v_x = 1.5
|
|
||||||
# v_y = 0.7
|
|
||||||
# v_z = 0.06
|
|
||||||
# initial_x = -0.6 + dx
|
|
||||||
# initial_y = -0.3 + dy
|
|
||||||
# initial_z = 0.8 + dz
|
|
||||||
else:
|
|
||||||
if scale:
|
|
||||||
v_x, v_y, v_z = [2.5, 2, 0.5] * scale_value
|
|
||||||
else:
|
|
||||||
v_x = 2.5
|
|
||||||
v_y = 2
|
|
||||||
v_z = 0.5
|
|
||||||
dx = 1
|
|
||||||
dy = 0
|
|
||||||
dz = 0.05
|
|
||||||
|
|
||||||
initial_x = 0 + dx
|
|
||||||
initial_y = -0.2 + dy
|
|
||||||
initial_z = 0.3 + dz
|
|
||||||
# print("initial ball state: ", initial_x, initial_y, initial_z, v_x, v_y, v_z)
|
|
||||||
initial_ball_state = np.array([initial_x, initial_y, initial_z, v_x, v_y, v_z])
|
|
||||||
return initial_ball_state
|
|
||||||
|
|
||||||
|
|
||||||
def context_scale_initialize(range):
|
|
||||||
"""
|
|
||||||
|
|
||||||
Returns:
|
|
||||||
|
|
||||||
"""
|
|
||||||
low, high = range
|
|
||||||
scale = np.random.uniform(low, high, 1)
|
|
||||||
return scale
|
|
||||||
|
|
||||||
|
|
||||||
def config_handle_generation(config_file_path):
|
|
||||||
"""Generate config handle for multiprocessing
|
|
||||||
|
|
||||||
Args:
|
|
||||||
config_file_path:
|
|
||||||
|
|
||||||
Returns:
|
|
||||||
|
|
||||||
"""
|
|
||||||
cfg_fname = Path(config_file_path)
|
|
||||||
# .json and .yml file
|
|
||||||
if cfg_fname.suffix == ".json":
|
|
||||||
config = read_json(cfg_fname)
|
|
||||||
elif cfg_fname.suffix == ".yml":
|
|
||||||
config = read_yaml(cfg_fname)
|
|
||||||
|
|
||||||
return config
|
|
||||||
@ -1,402 +0,0 @@
|
|||||||
import numpy as np
|
|
||||||
import logging
|
|
||||||
|
|
||||||
|
|
||||||
class HierarchicalRewardTableTennis(object):
|
|
||||||
"""Class for hierarchical reward function for table tennis experiment.
|
|
||||||
|
|
||||||
Return Highest Reward.
|
|
||||||
Reward = 0
|
|
||||||
Step 1: Action Valid. Upper Bound 0
|
|
||||||
[-∞, 0]
|
|
||||||
Reward += -1 * |hit_duration - hit_duration_threshold| * |hit_duration < hit_duration_threshold| * 10
|
|
||||||
Step 2: Hitting. Upper Bound 2
|
|
||||||
if hitting:
|
|
||||||
[0, 2]
|
|
||||||
Reward = 2 * (1 - tanh(|shortest_hitting_dist|))
|
|
||||||
if not hitting:
|
|
||||||
[0, 0.2]
|
|
||||||
Reward = 2 * (1 - tanh(|shortest_hitting_dist|))
|
|
||||||
Step 3: Target Point Achievement. Upper Bound 6
|
|
||||||
[0, 4]
|
|
||||||
if table_contact_detector:
|
|
||||||
Reward += 1
|
|
||||||
Reward += (1 - tanh(|shortest_hitting_dist|)) * 2
|
|
||||||
if contact_coordinate[0] < 0:
|
|
||||||
Reward += 1
|
|
||||||
else:
|
|
||||||
Reward += 0
|
|
||||||
elif:
|
|
||||||
Reward += (1 - tanh(|shortest_hitting_dist|))
|
|
||||||
"""
|
|
||||||
|
|
||||||
def __init__(self):
|
|
||||||
self.reward = None
|
|
||||||
self.goal_achievement = False
|
|
||||||
self.total_reward = 0
|
|
||||||
self.shortest_hitting_dist = 1000
|
|
||||||
self.highest_reward = -1000
|
|
||||||
self.lowest_corner_dist = 100
|
|
||||||
self.right_court_contact_detector = False
|
|
||||||
self.table_contact_detector = False
|
|
||||||
self.floor_contact_detector = False
|
|
||||||
self.radius = 0.025
|
|
||||||
self.min_ball_x_pos = 100
|
|
||||||
self.hit_contact_detector = False
|
|
||||||
self.net_contact_detector = False
|
|
||||||
self.ratio = 1
|
|
||||||
self.lowest_z = 100
|
|
||||||
self.target_flag = False
|
|
||||||
self.dist_target_virtual = 100
|
|
||||||
self.ball_z_pos_lowest = 100
|
|
||||||
self.hitting_flag = False
|
|
||||||
self.hitting_time_point = None
|
|
||||||
self.ctxt_dim = None
|
|
||||||
self.context_range_bounds = None
|
|
||||||
# self.ctxt_out_of_range_punishment = None
|
|
||||||
# self.ctxt_in_side_of_range_punishment = None
|
|
||||||
#
|
|
||||||
# def check_where_invalid(self, ctxt, context_range_bounds, set_to_valid_region=False):
|
|
||||||
# idx_max = []
|
|
||||||
# idx_min = []
|
|
||||||
# for dim in range(self.ctxt_dim):
|
|
||||||
# min_dim = context_range_bounds[0][dim]
|
|
||||||
# max_dim = context_range_bounds[1][dim]
|
|
||||||
# idx_max_c = np.where(ctxt[:, dim] > max_dim)[0]
|
|
||||||
# idx_min_c = np.where(ctxt[:, dim] < min_dim)[0]
|
|
||||||
# if set_to_valid_region:
|
|
||||||
# if idx_max_c.shape[0] != 0:
|
|
||||||
# ctxt[idx_max_c, dim] = max_dim
|
|
||||||
# if idx_min_c.shape[0] != 0:
|
|
||||||
# ctxt[idx_min_c, dim] = min_dim
|
|
||||||
# idx_max.append(idx_max_c)
|
|
||||||
# idx_min.append(idx_min_c)
|
|
||||||
# return idx_max, idx_min, ctxt
|
|
||||||
|
|
||||||
def check_valid(self, scale, context_range_bounds):
|
|
||||||
|
|
||||||
min_dim = context_range_bounds[0][0]
|
|
||||||
max_dim = context_range_bounds[1][0]
|
|
||||||
valid = (scale < max_dim) and (scale > min_dim)
|
|
||||||
return valid
|
|
||||||
|
|
||||||
@classmethod
|
|
||||||
def goal_distance(cls, goal_a, goal_b):
|
|
||||||
assert goal_a.shape == goal_b.shape
|
|
||||||
return np.linalg.norm(goal_a - goal_b, axis=-1)
|
|
||||||
|
|
||||||
def refresh_highest_reward(self):
|
|
||||||
if self.total_reward >= self.highest_reward:
|
|
||||||
self.highest_reward = self.total_reward
|
|
||||||
|
|
||||||
def duration_valid(self):
|
|
||||||
pass
|
|
||||||
|
|
||||||
def huge_value_unstable(self):
|
|
||||||
self.total_reward += -10
|
|
||||||
self.highest_reward = -1
|
|
||||||
|
|
||||||
def context_valid(self, context):
|
|
||||||
valid = self.check_valid(context.copy(), context_range_bounds=self.context_range_bounds)
|
|
||||||
# when using dirac punishments
|
|
||||||
if valid:
|
|
||||||
self.total_reward += 1 # If Action Valid and Context Valid, total_reward = 0
|
|
||||||
else:
|
|
||||||
self.total_reward += 0
|
|
||||||
self.refresh_highest_reward()
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
# If in the ctxt, add 1, otherwise, 0
|
|
||||||
|
|
||||||
def action_valid(self, durations=None):
|
|
||||||
"""Ensure the execution of the robot movement with parameters which are in a valid domain.
|
|
||||||
|
|
||||||
Time should always be positive,
|
|
||||||
the joint position of the robot should be a subset of [−π, π].
|
|
||||||
if all parameters are valid, the robot gets a zero score,
|
|
||||||
otherwise it gets a negative score proportional to how much it is beyond the valid parameter domain.
|
|
||||||
|
|
||||||
Returns:
|
|
||||||
rewards: if valid, reward is equal to 0.
|
|
||||||
if not valid, reward is negative and proportional to the distance beyond the valid parameter domain
|
|
||||||
"""
|
|
||||||
assert durations.shape[0] == 2, "durations type should be np.array and the shape should be 2"
|
|
||||||
# pre_duration = durations[0]
|
|
||||||
hit_duration = durations[1]
|
|
||||||
# pre_duration_thres = 0.01
|
|
||||||
hit_duration_thres = 1
|
|
||||||
# self.goal_achievement = np.all(
|
|
||||||
# [(pre_duration > pre_duration_thres), (hit_duration > hit_duration_thres), (0.3 < pre_duration < 0.6)])
|
|
||||||
self.goal_achievement = (hit_duration > hit_duration_thres)
|
|
||||||
if self.goal_achievement:
|
|
||||||
self.total_reward = -1
|
|
||||||
self.goal_achievement = True
|
|
||||||
else:
|
|
||||||
# self.total_reward += -1 * ((np.abs(pre_duration - pre_duration_thres) * int(
|
|
||||||
# pre_duration < pre_duration_thres) + np.abs(hit_duration - hit_duration_thres) * int(
|
|
||||||
# hit_duration < hit_duration_thres)) * 10)
|
|
||||||
self.total_reward = -1 * ((np.abs(hit_duration - hit_duration_thres) * int(
|
|
||||||
hit_duration < hit_duration_thres)) * 10)
|
|
||||||
self.total_reward += -1
|
|
||||||
self.goal_achievement = False
|
|
||||||
self.refresh_highest_reward()
|
|
||||||
|
|
||||||
def motion_penalty(self, action, high_motion_penalty):
|
|
||||||
"""Protects the robot from high acceleration and dangerous movement.
|
|
||||||
"""
|
|
||||||
if not high_motion_penalty:
|
|
||||||
reward_ctrl = - 0.05 * np.square(action).sum()
|
|
||||||
else:
|
|
||||||
reward_ctrl = - 0.075 * np.square(action).sum()
|
|
||||||
self.total_reward += reward_ctrl
|
|
||||||
self.refresh_highest_reward()
|
|
||||||
self.goal_achievement = True
|
|
||||||
|
|
||||||
def hitting(self, env): # , target_ball_pos, racket_center_pos, hit_contact_detector=False
|
|
||||||
"""Hitting reward calculation
|
|
||||||
|
|
||||||
If racket successfully hit the ball, the reward +1
|
|
||||||
Otherwise calculate the distance between the center of racket and the center of ball,
|
|
||||||
reward = tanh(r/dist) if dist<1 reward almost 2 , if dist >= 1 reward is between [0, 0.2]
|
|
||||||
|
|
||||||
|
|
||||||
Args:
|
|
||||||
env:
|
|
||||||
|
|
||||||
Returns:
|
|
||||||
|
|
||||||
"""
|
|
||||||
|
|
||||||
hit_contact_obj = ["target_ball", "bat"]
|
|
||||||
target_ball_pos = env.target_ball_pos
|
|
||||||
racket_center_pos = env.racket_center_pos
|
|
||||||
# hit contact detection
|
|
||||||
# Record the hitting history
|
|
||||||
self.hitting_flag = False
|
|
||||||
if not self.hit_contact_detector:
|
|
||||||
self.hit_contact_detector = self.contact_detection(env, hit_contact_obj)
|
|
||||||
if self.hit_contact_detector:
|
|
||||||
print("First time detect hitting")
|
|
||||||
self.hitting_flag = True
|
|
||||||
if self.hit_contact_detector:
|
|
||||||
|
|
||||||
# TODO
|
|
||||||
dist = self.goal_distance(target_ball_pos, racket_center_pos)
|
|
||||||
if dist < 0:
|
|
||||||
dist = 0
|
|
||||||
# print("goal distance is:", dist)
|
|
||||||
if dist <= self.shortest_hitting_dist:
|
|
||||||
self.shortest_hitting_dist = dist
|
|
||||||
# print("shortest_hitting_dist is:", self.shortest_hitting_dist)
|
|
||||||
# Keep the shortest hitting distance.
|
|
||||||
dist_reward = 2 * (1 - np.tanh(np.abs(self.shortest_hitting_dist)))
|
|
||||||
|
|
||||||
# TODO sparse
|
|
||||||
# dist_reward = 2
|
|
||||||
|
|
||||||
self.total_reward += dist_reward
|
|
||||||
self.goal_achievement = True
|
|
||||||
|
|
||||||
# if self.hitting_time_point is not None and self.hitting_time_point > 600:
|
|
||||||
# self.total_reward += 1
|
|
||||||
|
|
||||||
else:
|
|
||||||
dist = self.goal_distance(target_ball_pos, racket_center_pos)
|
|
||||||
if dist <= self.shortest_hitting_dist:
|
|
||||||
self.shortest_hitting_dist = dist
|
|
||||||
dist_reward = 1 - np.tanh(self.shortest_hitting_dist)
|
|
||||||
reward = 0.2 * dist_reward # because it does not hit the ball, so multiply 0.2
|
|
||||||
self.total_reward += reward
|
|
||||||
self.goal_achievement = False
|
|
||||||
|
|
||||||
self.refresh_highest_reward()
|
|
||||||
|
|
||||||
@classmethod
|
|
||||||
def relu(cls, x):
|
|
||||||
return np.maximum(0, x)
|
|
||||||
|
|
||||||
# def right_table_contact(self, env):
|
|
||||||
# right_court_contact_obj = ["target_ball", "table_tennis_table_right_side"]
|
|
||||||
# if env.target_ball_pos[0] >= 0 and env.target_ball_pos[2] >= 0.7:
|
|
||||||
# # update right court contact detection
|
|
||||||
# if not self.right_court_contact_detector:
|
|
||||||
# self.right_court_contact_detector = self.contact_detection(env, right_court_contact_obj)
|
|
||||||
# if self.right_court_contact_detector:
|
|
||||||
# self.contact_x_pos = env.target_ball_pos[0]
|
|
||||||
# if self.right_court_contact_detector:
|
|
||||||
# self.total_reward += 1 - norm(0.685, 1).pdf(self.contact_x_pos) # x axis middle of right table
|
|
||||||
# self.goal_achievement = False
|
|
||||||
# else:
|
|
||||||
# self.total_reward += 1
|
|
||||||
# self.goal_achievement = True
|
|
||||||
# # else:
|
|
||||||
# # self.total_reward += 0
|
|
||||||
# # self.goal_achievement = False
|
|
||||||
# self.refresh_highest_reward()
|
|
||||||
|
|
||||||
# def net_contact(self, env):
|
|
||||||
# net_contact_obj = ["target_ball", "table_tennis_net"]
|
|
||||||
# # net_contact_detector = self.contact_detection(env, net_contact_obj)
|
|
||||||
# # ball_x_pos = env.target_ball_pos[0]
|
|
||||||
# # if self.min_ball_x_pos >= ball_x_pos:
|
|
||||||
# # self.min_ball_x_pos = ball_x_pos
|
|
||||||
# # table_left_edge_x_pos = -1.37
|
|
||||||
# # if np.abs(ball_x_pos) <= 0.01: # x threshold of net
|
|
||||||
# # if self.lowest_z >= env.target_ball_pos[2]:
|
|
||||||
# # self.lowest_z = env.target_ball_pos[2]
|
|
||||||
# # # construct a gaussian distribution of z
|
|
||||||
# # z_reward = 4 - norm(0, 0.1).pdf(self.lowest_z - 0.07625) # maximum 4
|
|
||||||
# # self.total_reward += z_reward
|
|
||||||
# # self.total_reward += 2 - np.minimum(1, self.relu(np.abs(self.min_ball_x_pos)))
|
|
||||||
# if not self.net_contact_detector:
|
|
||||||
# self.net_contact_detector = self.contact_detection(env, net_contact_obj)
|
|
||||||
# if self.net_contact_detector:
|
|
||||||
# self.total_reward += 0 # very high cost
|
|
||||||
# self.goal_achievement = False
|
|
||||||
# else:
|
|
||||||
# self.total_reward += 1
|
|
||||||
# self.goal_achievement = True
|
|
||||||
# self.refresh_highest_reward()
|
|
||||||
|
|
||||||
# def landing_on_opponent_court(self, env):
|
|
||||||
# # Very sparse reward
|
|
||||||
# # don't contact the right side court
|
|
||||||
# # right_court_contact_obj = ["target_ball", "table_tennis_table_right_side"]
|
|
||||||
# # right_court_contact_detector = self.contact_detection(env, right_court_contact_obj)
|
|
||||||
# left_court_contact_obj = ["target_ball", "table_tennis_table_left_side"]
|
|
||||||
# # left_court_contact_detector = self.contact_detection(env, left_court_contact_obj)
|
|
||||||
# # record the contact history
|
|
||||||
# # if not self.right_court_contact_detector:
|
|
||||||
# # self.right_court_contact_detector = self.contact_detection(env, right_court_contact_obj)
|
|
||||||
# if not self.table_contact_detector:
|
|
||||||
# self.table_contact_detector = self.contact_detection(env, left_court_contact_obj)
|
|
||||||
#
|
|
||||||
# dist_left_up_corner = self.goal_distance(env.target_ball_pos, env.sim.data.get_site_xpos("left_up_corner"))
|
|
||||||
# dist_middle_up_corner = self.goal_distance(env.target_ball_pos, env.sim.data.get_site_xpos("middle_up_corner"))
|
|
||||||
# dist_left_down_corner = self.goal_distance(env.target_ball_pos, env.sim.data.get_site_xpos("left_down_corner"))
|
|
||||||
# dist_middle_down_corner = self.goal_distance(env.target_ball_pos,
|
|
||||||
# env.sim.data.get_site_xpos("middle_down_corner"))
|
|
||||||
# dist_array = np.array(
|
|
||||||
# [dist_left_up_corner, dist_middle_up_corner, dist_left_down_corner, dist_middle_down_corner])
|
|
||||||
# dist_corner = np.amin(dist_array)
|
|
||||||
# if self.lowest_corner_dist >= dist_corner:
|
|
||||||
# self.lowest_corner_dist = dist_corner
|
|
||||||
#
|
|
||||||
# right_contact_cost = 1
|
|
||||||
# left_contact_reward = 2
|
|
||||||
# dist_left_table_reward = (2 - np.tanh(self.lowest_corner_dist))
|
|
||||||
# # TODO Try multi dimensional gaussian distribution
|
|
||||||
# # contact only the left side court
|
|
||||||
# if self.right_court_contact_detector:
|
|
||||||
# self.total_reward += 0
|
|
||||||
# self.goal_achievement = False
|
|
||||||
# if self.table_contact_detector:
|
|
||||||
# self.total_reward += left_contact_reward
|
|
||||||
# self.goal_achievement = False
|
|
||||||
# else:
|
|
||||||
# self.total_reward += dist_left_table_reward
|
|
||||||
# self.goal_achievement = False
|
|
||||||
# else:
|
|
||||||
# self.total_reward += right_contact_cost
|
|
||||||
# if self.table_contact_detector:
|
|
||||||
# self.total_reward += left_contact_reward
|
|
||||||
# self.goal_achievement = True
|
|
||||||
# else:
|
|
||||||
# self.total_reward += dist_left_table_reward
|
|
||||||
# self.goal_achievement = False
|
|
||||||
# self.refresh_highest_reward()
|
|
||||||
# # if self.left_court_contact_detector and not self.right_court_contact_detector:
|
|
||||||
# # self.total_reward += self.ratio * left_contact_reward
|
|
||||||
# # print("only left court reward return!!!!!!!!!")
|
|
||||||
# # print("contact only left court!!!!!!")
|
|
||||||
# # self.goal_achievement = True
|
|
||||||
# # # no contact with table
|
|
||||||
# # elif not self.right_court_contact_detector and not self.left_court_contact_detector:
|
|
||||||
# # self.total_reward += 0 + self.ratio * dist_left_table_reward
|
|
||||||
# # self.goal_achievement = False
|
|
||||||
# # # contact both side
|
|
||||||
# # elif self.right_court_contact_detector and self.left_court_contact_detector:
|
|
||||||
# # self.total_reward += self.ratio * (left_contact_reward - right_contact_cost) # cost of contact of right court
|
|
||||||
# # self.goal_achievement = False
|
|
||||||
# # # contact only the right side court
|
|
||||||
# # elif self.right_court_contact_detector and not self.left_court_contact_detector:
|
|
||||||
# # self.total_reward += 0 + self.ratio * (
|
|
||||||
# # dist_left_table_reward - right_contact_cost) # cost of contact of right court
|
|
||||||
# # self.goal_achievement = False
|
|
||||||
|
|
||||||
def target_achievement(self, env):
|
|
||||||
target_coordinate = np.array([-0.5, -0.5])
|
|
||||||
# net_contact_obj = ["target_ball", "table_tennis_net"]
|
|
||||||
table_contact_obj = ["target_ball", "table_tennis_table"]
|
|
||||||
floor_contact_obj = ["target_ball", "floor"]
|
|
||||||
|
|
||||||
if 0.78 < env.target_ball_pos[2] < 0.8:
|
|
||||||
dist_target_virtual = np.linalg.norm(env.target_ball_pos[:2] - target_coordinate)
|
|
||||||
if self.dist_target_virtual > dist_target_virtual:
|
|
||||||
self.dist_target_virtual = dist_target_virtual
|
|
||||||
if -0.07 < env.target_ball_pos[0] < 0.07 and env.sim.data.get_joint_qvel('tar:x') < 0:
|
|
||||||
if self.ball_z_pos_lowest > env.target_ball_pos[2]:
|
|
||||||
self.ball_z_pos_lowest = env.target_ball_pos[2].copy()
|
|
||||||
# if not self.net_contact_detector:
|
|
||||||
# self.net_contact_detector = self.contact_detection(env, net_contact_obj)
|
|
||||||
if not self.table_contact_detector:
|
|
||||||
self.table_contact_detector = self.contact_detection(env, table_contact_obj)
|
|
||||||
if not self.floor_contact_detector:
|
|
||||||
self.floor_contact_detector = self.contact_detection(env, floor_contact_obj)
|
|
||||||
if not self.target_flag:
|
|
||||||
# Table Contact Reward.
|
|
||||||
if self.table_contact_detector:
|
|
||||||
self.total_reward += 1
|
|
||||||
# only update when the first contact because of the flag
|
|
||||||
contact_coordinate = env.target_ball_pos[:2].copy()
|
|
||||||
print("contact table ball coordinate: ", env.target_ball_pos)
|
|
||||||
logging.info("contact table ball coordinate: {}".format(env.target_ball_pos))
|
|
||||||
dist_target = np.linalg.norm(contact_coordinate - target_coordinate)
|
|
||||||
self.total_reward += (1 - np.tanh(dist_target)) * 2
|
|
||||||
self.target_flag = True
|
|
||||||
# Net Contact Reward. Precondition: Table Contact exits.
|
|
||||||
if contact_coordinate[0] < 0:
|
|
||||||
print("left table contact")
|
|
||||||
logging.info("~~~~~~~~~~~~~~~left table contact~~~~~~~~~~~~~~~")
|
|
||||||
self.total_reward += 1
|
|
||||||
# TODO Z coordinate reward
|
|
||||||
# self.total_reward += np.maximum(np.tanh(self.ball_z_pos_lowest), 0)
|
|
||||||
self.goal_achievement = True
|
|
||||||
else:
|
|
||||||
print("right table contact")
|
|
||||||
logging.info("~~~~~~~~~~~~~~~right table contact~~~~~~~~~~~~~~~")
|
|
||||||
self.total_reward += 0
|
|
||||||
self.goal_achievement = False
|
|
||||||
# if self.net_contact_detector:
|
|
||||||
# self.total_reward += 0
|
|
||||||
# self.goal_achievement = False
|
|
||||||
# else:
|
|
||||||
# self.total_reward += 1
|
|
||||||
# self.goal_achievement = False
|
|
||||||
# Floor Contact Reward. Precondition: Table Contact exits.
|
|
||||||
elif self.floor_contact_detector:
|
|
||||||
self.total_reward += (1 - np.tanh(self.dist_target_virtual))
|
|
||||||
self.target_flag = True
|
|
||||||
self.goal_achievement = False
|
|
||||||
# No Contact of Floor or Table, flying
|
|
||||||
else:
|
|
||||||
pass
|
|
||||||
# else:
|
|
||||||
# print("Flag is True already")
|
|
||||||
self.refresh_highest_reward()
|
|
||||||
|
|
||||||
def distance_to_target(self):
|
|
||||||
pass
|
|
||||||
|
|
||||||
@classmethod
|
|
||||||
def contact_detection(cls, env, goal_contact):
|
|
||||||
for i in range(env.sim.data.ncon):
|
|
||||||
contact = env.sim.data.contact[i]
|
|
||||||
achieved_geom1_name = env.sim.model.geom_id2name(contact.geom1)
|
|
||||||
achieved_geom2_name = env.sim.model.geom_id2name(contact.geom2)
|
|
||||||
if np.all([(achieved_geom1_name in goal_contact), (achieved_geom2_name in goal_contact)]):
|
|
||||||
print("contact of " + achieved_geom1_name + " " + achieved_geom2_name)
|
|
||||||
return True
|
|
||||||
else:
|
|
||||||
return False
|
|
||||||
@ -1,136 +0,0 @@
|
|||||||
# Copyright 2017 The dm_control Authors.
|
|
||||||
#
|
|
||||||
# Licensed under the Apache License, Version 2.0 (the "License");
|
|
||||||
# you may not use this file except in compliance with the License.
|
|
||||||
# You may obtain a copy of the License at
|
|
||||||
#
|
|
||||||
# http://www.apache.org/licenses/LICENSE-2.0
|
|
||||||
#
|
|
||||||
# Unless required by applicable law or agreed to in writing, software
|
|
||||||
# distributed under the License is distributed on an "AS IS" BASIS,
|
|
||||||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
|
||||||
# See the License for the specific language governing permissions and
|
|
||||||
# limitations under the License.
|
|
||||||
# ============================================================================
|
|
||||||
|
|
||||||
# """Soft indicator function evaluating whether a number is within bounds."""
|
|
||||||
#
|
|
||||||
# from __future__ import absolute_import
|
|
||||||
# from __future__ import division
|
|
||||||
# from __future__ import print_function
|
|
||||||
|
|
||||||
# Internal dependencies.
|
|
||||||
import numpy as np
|
|
||||||
|
|
||||||
# The value returned by tolerance() at `margin` distance from `bounds` interval.
|
|
||||||
_DEFAULT_VALUE_AT_MARGIN = 0.1
|
|
||||||
|
|
||||||
|
|
||||||
def _sigmoids(x, value_at_1, sigmoid):
|
|
||||||
"""Returns 1 when `x` == 0, between 0 and 1 otherwise.
|
|
||||||
|
|
||||||
Args:
|
|
||||||
x: A scalar or numpy array.
|
|
||||||
value_at_1: A float between 0 and 1 specifying the output when `x` == 1.
|
|
||||||
sigmoid: String, choice of sigmoid type.
|
|
||||||
|
|
||||||
Returns:
|
|
||||||
A numpy array with values between 0.0 and 1.0.
|
|
||||||
|
|
||||||
Raises:
|
|
||||||
ValueError: If not 0 < `value_at_1` < 1, except for `linear`, `cosine` and
|
|
||||||
`quadratic` sigmoids which allow `value_at_1` == 0.
|
|
||||||
ValueError: If `sigmoid` is of an unknown type.
|
|
||||||
"""
|
|
||||||
if sigmoid in ('cosine', 'linear', 'quadratic'):
|
|
||||||
if not 0 <= value_at_1 < 1:
|
|
||||||
raise ValueError('`value_at_1` must be nonnegative and smaller than 1, '
|
|
||||||
'got {}.'.format(value_at_1))
|
|
||||||
else:
|
|
||||||
if not 0 < value_at_1 < 1:
|
|
||||||
raise ValueError('`value_at_1` must be strictly between 0 and 1, '
|
|
||||||
'got {}.'.format(value_at_1))
|
|
||||||
|
|
||||||
if sigmoid == 'gaussian':
|
|
||||||
scale = np.sqrt(-2 * np.log(value_at_1))
|
|
||||||
return np.exp(-0.5 * (x*scale)**2)
|
|
||||||
|
|
||||||
elif sigmoid == 'hyperbolic':
|
|
||||||
scale = np.arccosh(1/value_at_1)
|
|
||||||
return 1 / np.cosh(x*scale)
|
|
||||||
|
|
||||||
elif sigmoid == 'long_tail':
|
|
||||||
scale = np.sqrt(1/value_at_1 - 1)
|
|
||||||
return 1 / ((x*scale)**2 + 1)
|
|
||||||
|
|
||||||
elif sigmoid == 'cosine':
|
|
||||||
scale = np.arccos(2*value_at_1 - 1) / np.pi
|
|
||||||
scaled_x = x*scale
|
|
||||||
return np.where(abs(scaled_x) < 1, (1 + np.cos(np.pi*scaled_x))/2, 0.0)
|
|
||||||
|
|
||||||
elif sigmoid == 'linear':
|
|
||||||
scale = 1-value_at_1
|
|
||||||
scaled_x = x*scale
|
|
||||||
return np.where(abs(scaled_x) < 1, 1 - scaled_x, 0.0)
|
|
||||||
|
|
||||||
elif sigmoid == 'quadratic':
|
|
||||||
scale = np.sqrt(1-value_at_1)
|
|
||||||
scaled_x = x*scale
|
|
||||||
return np.where(abs(scaled_x) < 1, 1 - scaled_x**2, 0.0)
|
|
||||||
|
|
||||||
elif sigmoid == 'tanh_squared':
|
|
||||||
scale = np.arctanh(np.sqrt(1-value_at_1))
|
|
||||||
return 1 - np.tanh(x*scale)**2
|
|
||||||
|
|
||||||
else:
|
|
||||||
raise ValueError('Unknown sigmoid type {!r}.'.format(sigmoid))
|
|
||||||
|
|
||||||
|
|
||||||
def tolerance(x, bounds=(0.0, 0.0), margin=0.0, sigmoid='gaussian',
|
|
||||||
value_at_margin=_DEFAULT_VALUE_AT_MARGIN):
|
|
||||||
"""Returns 1 when `x` falls inside the bounds, between 0 and 1 otherwise.
|
|
||||||
|
|
||||||
Args:
|
|
||||||
x: A scalar or numpy array.
|
|
||||||
bounds: A tuple of floats specifying inclusive `(lower, upper)` bounds for
|
|
||||||
the target interval. These can be infinite if the interval is unbounded
|
|
||||||
at one or both ends, or they can be equal to one another if the target
|
|
||||||
value is exact.
|
|
||||||
margin: Float. Parameter that controls how steeply the output decreases as
|
|
||||||
`x` moves out-of-bounds.
|
|
||||||
* If `margin == 0` then the output will be 0 for all values of `x`
|
|
||||||
outside of `bounds`.
|
|
||||||
* If `margin > 0` then the output will decrease sigmoidally with
|
|
||||||
increasing distance from the nearest bound.
|
|
||||||
sigmoid: String, choice of sigmoid type. Valid values are: 'gaussian',
|
|
||||||
'linear', 'hyperbolic', 'long_tail', 'cosine', 'tanh_squared'.
|
|
||||||
value_at_margin: A float between 0 and 1 specifying the output value when
|
|
||||||
the distance from `x` to the nearest bound is equal to `margin`. Ignored
|
|
||||||
if `margin == 0`.
|
|
||||||
|
|
||||||
Returns:
|
|
||||||
A float or numpy array with values between 0.0 and 1.0.
|
|
||||||
|
|
||||||
Raises:
|
|
||||||
ValueError: If `bounds[0] > bounds[1]`.
|
|
||||||
ValueError: If `margin` is negative.
|
|
||||||
"""
|
|
||||||
lower, upper = bounds
|
|
||||||
if lower > upper:
|
|
||||||
raise ValueError('Lower bound must be <= upper bound.')
|
|
||||||
if margin < 0:
|
|
||||||
raise ValueError('`margin` must be non-negative.')
|
|
||||||
|
|
||||||
in_bounds = np.logical_and(lower <= x, x <= upper)
|
|
||||||
if margin == 0:
|
|
||||||
value = np.where(in_bounds, 1.0, 0.0)
|
|
||||||
else:
|
|
||||||
d = np.where(x < lower, lower - x, x - upper) / margin
|
|
||||||
value = np.where(in_bounds, 1.0, _sigmoids(d, value_at_margin, sigmoid))
|
|
||||||
|
|
||||||
return float(value) if np.isscalar(x) else value
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
@ -1,49 +0,0 @@
|
|||||||
import json
|
|
||||||
import yaml
|
|
||||||
import xml.etree.ElementTree as ET
|
|
||||||
from collections import OrderedDict
|
|
||||||
from pathlib import Path
|
|
||||||
|
|
||||||
|
|
||||||
def read_json(fname):
|
|
||||||
fname = Path(fname)
|
|
||||||
with fname.open('rt') as handle:
|
|
||||||
return json.load(handle, object_hook=OrderedDict)
|
|
||||||
|
|
||||||
|
|
||||||
def write_json(content, fname):
|
|
||||||
fname = Path(fname)
|
|
||||||
with fname.open('wt') as handle:
|
|
||||||
json.dump(content, handle, indent=4, sort_keys=False)
|
|
||||||
|
|
||||||
|
|
||||||
def read_yaml(fname):
|
|
||||||
fname = Path(fname)
|
|
||||||
with fname.open('rt') as handle:
|
|
||||||
return yaml.load(handle, Loader=yaml.FullLoader)
|
|
||||||
|
|
||||||
|
|
||||||
def write_yaml(content, fname):
|
|
||||||
fname = Path(fname)
|
|
||||||
with fname.open('wt') as handle:
|
|
||||||
yaml.dump(content, handle)
|
|
||||||
|
|
||||||
|
|
||||||
def config_save(dir_path, config):
|
|
||||||
dir_path = Path(dir_path)
|
|
||||||
config_path_json = dir_path / "config.json"
|
|
||||||
config_path_yaml = dir_path / "config.yml"
|
|
||||||
# .json and .yml file,save 2 version of configuration.
|
|
||||||
write_json(config, config_path_json)
|
|
||||||
write_yaml(config, config_path_yaml)
|
|
||||||
|
|
||||||
|
|
||||||
def change_kp_in_xml(kp_list,
|
|
||||||
model_path="/home/zhou/slow/table_tennis_rl/simulation/gymTableTennis/gym_table_tennis/simple_reacher/robotics/assets/table_tennis/right_arm_actuator.xml"):
|
|
||||||
tree = ET.parse(model_path)
|
|
||||||
root = tree.getroot()
|
|
||||||
# for actuator in root.find("actuator"):
|
|
||||||
for position, kp in zip(root.iter('position'), kp_list):
|
|
||||||
position.set("kp", str(kp))
|
|
||||||
tree.write(model_path)
|
|
||||||
|
|
||||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Some files were not shown because too many files have changed in this diff Show More
Loading…
Reference in New Issue
Block a user