renameing alr module and updating tests
This commit is contained in:
Fabian
@@ -0,0 +1,15 @@
|
||||
# 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
|
||||
|
||||
@@ -0,0 +1,9 @@
|
||||
from .beerpong.beerpong import BeerPongEnv, BeerPongEnvFixedReleaseStep, BeerPongEnvStepBasedEpisodicReward
|
||||
from .ant_jump.ant_jump import AntJumpEnv
|
||||
from .half_cheetah_jump.half_cheetah_jump import ALRHalfCheetahJumpEnv
|
||||
from .hopper_jump.hopper_jump_on_box import HopperJumpOnBoxEnv
|
||||
from .hopper_throw.hopper_throw import ALRHopperThrowEnv
|
||||
from .hopper_throw.hopper_throw_in_basket import ALRHopperThrowInBasketEnv
|
||||
from .reacher.reacher import ReacherEnv
|
||||
from .walker_2d_jump.walker_2d_jump import ALRWalker2dJumpEnv
|
||||
from .hopper_jump.hopper_jump import HopperJumpEnv
|
||||
@@ -0,0 +1 @@
|
||||
from .mp_wrapper import MPWrapper
|
||||
@@ -0,0 +1,102 @@
|
||||
from typing import Tuple, Union, Optional
|
||||
|
||||
import numpy as np
|
||||
from gym.core import ObsType
|
||||
from gym.envs.mujoco.ant_v3 import AntEnv
|
||||
|
||||
MAX_EPISODE_STEPS_ANTJUMP = 200
|
||||
|
||||
|
||||
# TODO: This environment was not tested yet. Do the following todos and test it.
|
||||
# TODO: Right now this environment only considers jumping to a specific height, which is not nice. It should be extended
|
||||
# to the same structure as the Hopper, where the angles are randomized (->contexts) and the agent should jump as heigh
|
||||
# as possible, while landing at a specific target position
|
||||
|
||||
|
||||
class AntJumpEnv(AntEnv):
|
||||
"""
|
||||
Initialization changes to normal Ant:
|
||||
- healthy_reward: 1.0 -> 0.01 -> 0.0 no healthy reward needed - Paul and Marc
|
||||
- ctrl_cost_weight 0.5 -> 0.0
|
||||
- contact_cost_weight: 5e-4 -> 0.0
|
||||
- healthy_z_range: (0.2, 1.0) -> (0.3, float('inf')) !!!!! Does that make sense, limiting height?
|
||||
"""
|
||||
|
||||
def __init__(self,
|
||||
xml_file='ant.xml',
|
||||
ctrl_cost_weight=0.0,
|
||||
contact_cost_weight=0.0,
|
||||
healthy_reward=0.0,
|
||||
terminate_when_unhealthy=True,
|
||||
healthy_z_range=(0.3, float('inf')),
|
||||
contact_force_range=(-1.0, 1.0),
|
||||
reset_noise_scale=0.1,
|
||||
exclude_current_positions_from_observation=True,
|
||||
):
|
||||
self.current_step = 0
|
||||
self.max_height = 0
|
||||
self.goal = 0
|
||||
super().__init__(xml_file, ctrl_cost_weight, contact_cost_weight, healthy_reward, terminate_when_unhealthy,
|
||||
healthy_z_range, contact_force_range, reset_noise_scale,
|
||||
exclude_current_positions_from_observation)
|
||||
|
||||
def step(self, action):
|
||||
self.current_step += 1
|
||||
self.do_simulation(action, self.frame_skip)
|
||||
|
||||
height = self.get_body_com("torso")[2].copy()
|
||||
|
||||
self.max_height = max(height, self.max_height)
|
||||
|
||||
rewards = 0
|
||||
|
||||
ctrl_cost = self.control_cost(action)
|
||||
contact_cost = self.contact_cost
|
||||
|
||||
costs = ctrl_cost + contact_cost
|
||||
|
||||
done = bool(height < 0.3) # fall over -> is the 0.3 value from healthy_z_range? TODO change 0.3 to the value of healthy z angle
|
||||
|
||||
if self.current_step == MAX_EPISODE_STEPS_ANTJUMP or done:
|
||||
# -10 for scaling the value of the distance between the max_height and the goal height; only used when context is enabled
|
||||
# height_reward = -10 * (np.linalg.norm(self.max_height - self.goal))
|
||||
height_reward = -10 * np.linalg.norm(self.max_height - self.goal)
|
||||
# no healthy reward when using context, because we optimize a negative value
|
||||
healthy_reward = 0
|
||||
|
||||
rewards = height_reward + healthy_reward
|
||||
|
||||
obs = self._get_obs()
|
||||
reward = rewards - costs
|
||||
|
||||
info = {
|
||||
'height': height,
|
||||
'max_height': self.max_height,
|
||||
'goal': self.goal
|
||||
}
|
||||
|
||||
return obs, reward, done, info
|
||||
|
||||
def _get_obs(self):
|
||||
return np.append(super()._get_obs(), self.goal)
|
||||
|
||||
def reset(self, *, seed: Optional[int] = None, return_info: bool = False,
|
||||
options: Optional[dict] = None, ) -> Union[ObsType, Tuple[ObsType, dict]]:
|
||||
self.current_step = 0
|
||||
self.max_height = 0
|
||||
# goal heights from 1.0 to 2.5; can be increased, but didnt work well with CMORE
|
||||
self.goal = self.np_random.uniform(1.0, 2.5, 1)
|
||||
return super().reset()
|
||||
|
||||
# reset_model had to be implemented in every env to make it deterministic
|
||||
def reset_model(self):
|
||||
noise_low = -self._reset_noise_scale
|
||||
noise_high = self._reset_noise_scale
|
||||
|
||||
qpos = self.init_qpos # + self.np_random.uniform(low=noise_low, high=noise_high, size=self.model.nq)
|
||||
qvel = self.init_qvel # + self.np_random.uniform(low=noise_low, high=noise_high, size=self.model.nv)
|
||||
|
||||
self.set_state(qpos, qvel)
|
||||
|
||||
observation = self._get_obs()
|
||||
return observation
|
||||
@@ -0,0 +1,81 @@
|
||||
<mujoco model="ant">
|
||||
<compiler angle="degree" coordinate="local" inertiafromgeom="true"/>
|
||||
<option integrator="RK4" timestep="0.01"/>
|
||||
<custom>
|
||||
<numeric data="0.0 0.0 0.55 1.0 0.0 0.0 0.0 0.0 1.0 0.0 -1.0 0.0 -1.0 0.0 1.0" name="init_qpos"/>
|
||||
</custom>
|
||||
<default>
|
||||
<joint armature="1" damping="1" limited="true"/>
|
||||
<geom conaffinity="0" condim="3" density="5.0" friction="1 0.5 0.5" margin="0.01" rgba="0.8 0.6 0.4 1"/>
|
||||
</default>
|
||||
<asset>
|
||||
<texture builtin="gradient" height="100" rgb1="1 1 1" rgb2="0 0 0" type="skybox" width="100"/>
|
||||
<texture builtin="flat" height="1278" mark="cross" markrgb="1 1 1" name="texgeom" random="0.01" rgb1="0.8 0.6 0.4" rgb2="0.8 0.6 0.4" type="cube" width="127"/>
|
||||
<texture builtin="checker" height="100" name="texplane" rgb1="0 0 0" rgb2="0.8 0.8 0.8" type="2d" width="100"/>
|
||||
<material name="MatPlane" reflectance="0.5" shininess="1" specular="1" texrepeat="60 60" texture="texplane"/>
|
||||
<material name="geom" texture="texgeom" texuniform="true"/>
|
||||
</asset>
|
||||
<worldbody>
|
||||
<light cutoff="100" diffuse="1 1 1" dir="-0 0 -1.3" directional="true" exponent="1" pos="0 0 1.3" specular=".1 .1 .1"/>
|
||||
<geom conaffinity="1" condim="3" material="MatPlane" name="floor" pos="0 0 0" rgba="0.8 0.9 0.8 1" size="40 40 40" type="plane"/>
|
||||
<body name="torso" pos="0 0 0.75">
|
||||
<camera name="track" mode="trackcom" pos="0 -3 0.3" xyaxes="1 0 0 0 0 1"/>
|
||||
<geom name="torso_geom" pos="0 0 0" size="0.25" type="sphere"/>
|
||||
<joint armature="0" damping="0" limited="false" margin="0.01" name="root" pos="0 0 0" type="free"/>
|
||||
<body name="front_left_leg" pos="0 0 0">
|
||||
<geom fromto="0.0 0.0 0.0 0.2 0.2 0.0" name="aux_1_geom" size="0.08" type="capsule"/>
|
||||
<body name="aux_1" pos="0.2 0.2 0">
|
||||
<joint axis="0 0 1" name="hip_1" pos="0.0 0.0 0.0" range="-30 30" type="hinge"/>
|
||||
<geom fromto="0.0 0.0 0.0 0.2 0.2 0.0" name="left_leg_geom" size="0.08" type="capsule"/>
|
||||
<body pos="0.2 0.2 0">
|
||||
<joint axis="-1 1 0" name="ankle_1" pos="0.0 0.0 0.0" range="30 70" type="hinge"/>
|
||||
<geom fromto="0.0 0.0 0.0 0.4 0.4 0.0" name="left_ankle_geom" size="0.08" type="capsule"/>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
<body name="front_right_leg" pos="0 0 0">
|
||||
<geom fromto="0.0 0.0 0.0 -0.2 0.2 0.0" name="aux_2_geom" size="0.08" type="capsule"/>
|
||||
<body name="aux_2" pos="-0.2 0.2 0">
|
||||
<joint axis="0 0 1" name="hip_2" pos="0.0 0.0 0.0" range="-30 30" type="hinge"/>
|
||||
<geom fromto="0.0 0.0 0.0 -0.2 0.2 0.0" name="right_leg_geom" size="0.08" type="capsule"/>
|
||||
<body pos="-0.2 0.2 0">
|
||||
<joint axis="1 1 0" name="ankle_2" pos="0.0 0.0 0.0" range="-70 -30" type="hinge"/>
|
||||
<geom fromto="0.0 0.0 0.0 -0.4 0.4 0.0" name="right_ankle_geom" size="0.08" type="capsule"/>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
<body name="back_leg" pos="0 0 0">
|
||||
<geom fromto="0.0 0.0 0.0 -0.2 -0.2 0.0" name="aux_3_geom" size="0.08" type="capsule"/>
|
||||
<body name="aux_3" pos="-0.2 -0.2 0">
|
||||
<joint axis="0 0 1" name="hip_3" pos="0.0 0.0 0.0" range="-30 30" type="hinge"/>
|
||||
<geom fromto="0.0 0.0 0.0 -0.2 -0.2 0.0" name="back_leg_geom" size="0.08" type="capsule"/>
|
||||
<body pos="-0.2 -0.2 0">
|
||||
<joint axis="-1 1 0" name="ankle_3" pos="0.0 0.0 0.0" range="-70 -30" type="hinge"/>
|
||||
<geom fromto="0.0 0.0 0.0 -0.4 -0.4 0.0" name="third_ankle_geom" size="0.08" type="capsule"/>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
<body name="right_back_leg" pos="0 0 0">
|
||||
<geom fromto="0.0 0.0 0.0 0.2 -0.2 0.0" name="aux_4_geom" size="0.08" type="capsule"/>
|
||||
<body name="aux_4" pos="0.2 -0.2 0">
|
||||
<joint axis="0 0 1" name="hip_4" pos="0.0 0.0 0.0" range="-30 30" type="hinge"/>
|
||||
<geom fromto="0.0 0.0 0.0 0.2 -0.2 0.0" name="rightback_leg_geom" size="0.08" type="capsule"/>
|
||||
<body pos="0.2 -0.2 0">
|
||||
<joint axis="1 1 0" name="ankle_4" pos="0.0 0.0 0.0" range="30 70" type="hinge"/>
|
||||
<geom fromto="0.0 0.0 0.0 0.4 -0.4 0.0" name="fourth_ankle_geom" size="0.08" type="capsule"/>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</worldbody>
|
||||
<actuator>
|
||||
<motor ctrllimited="true" ctrlrange="-1.0 1.0" joint="hip_4" gear="150"/>
|
||||
<motor ctrllimited="true" ctrlrange="-1.0 1.0" joint="ankle_4" gear="150"/>
|
||||
<motor ctrllimited="true" ctrlrange="-1.0 1.0" joint="hip_1" gear="150"/>
|
||||
<motor ctrllimited="true" ctrlrange="-1.0 1.0" joint="ankle_1" gear="150"/>
|
||||
<motor ctrllimited="true" ctrlrange="-1.0 1.0" joint="hip_2" gear="150"/>
|
||||
<motor ctrllimited="true" ctrlrange="-1.0 1.0" joint="ankle_2" gear="150"/>
|
||||
<motor ctrllimited="true" ctrlrange="-1.0 1.0" joint="hip_3" gear="150"/>
|
||||
<motor ctrllimited="true" ctrlrange="-1.0 1.0" joint="ankle_3" gear="150"/>
|
||||
</actuator>
|
||||
</mujoco>
|
||||
@@ -0,0 +1,23 @@
|
||||
from typing import Union, Tuple
|
||||
|
||||
import numpy as np
|
||||
|
||||
from alr_envs.black_box.raw_interface_wrapper import RawInterfaceWrapper
|
||||
|
||||
|
||||
class MPWrapper(RawInterfaceWrapper):
|
||||
|
||||
@property
|
||||
def context_mask(self):
|
||||
return np.hstack([
|
||||
[False] * 111, # ant has 111 dimensional observation space !!
|
||||
[True] # goal height
|
||||
])
|
||||
|
||||
@property
|
||||
def current_pos(self) -> Union[float, int, np.ndarray]:
|
||||
return self.env.sim.data.qpos[7:15].copy()
|
||||
|
||||
@property
|
||||
def current_vel(self) -> Union[float, int, np.ndarray, Tuple]:
|
||||
return self.env.sim.data.qvel[6:14].copy()
|
||||
@@ -0,0 +1 @@
|
||||
from .mp_wrapper import MPWrapper
|
||||
@@ -0,0 +1,238 @@
|
||||
<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>
|
||||
@@ -0,0 +1,187 @@
|
||||
<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" 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>
|
||||
@@ -0,0 +1,188 @@
|
||||
<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" 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 -2.8 0.4025">
|
||||
<geom name="table" type="box" size="1.5 1.5 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="1.5 1.5 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="1.5 0.04 1.4" pos="0. -4.3 1.4" 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">-->
|
||||
<body name="cup_table" pos="-1.42 -4.05 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>
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
Binary file not shown.
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
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.
BIN
Binary file not shown.
BIN
Binary file not shown.
Binary file not shown.
Executable
BIN
Binary file not shown.
Executable
BIN
Binary file not shown.
Executable
BIN
Binary file not shown.
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
Executable
BIN
Binary file not shown.
Executable
BIN
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
Executable
BIN
Binary file not shown.
Binary file not shown.
Executable
BIN
Binary file not shown.
Executable
BIN
Binary file not shown.
Binary file not shown.
@@ -0,0 +1,335 @@
|
||||
import os
|
||||
from typing import Optional
|
||||
|
||||
import mujoco_py.builder
|
||||
import numpy as np
|
||||
from gym import utils
|
||||
from gym.envs.mujoco import MujocoEnv
|
||||
|
||||
# XML Variables
|
||||
ROBOT_COLLISION_OBJ = ["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"]
|
||||
|
||||
CUP_COLLISION_OBJ = ["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_geom_table15", "cup_geom_table16", "cup_geom_table17", "cup_geom1_table8"]
|
||||
|
||||
|
||||
class BeerPongEnv(MujocoEnv, utils.EzPickle):
|
||||
def __init__(self, frame_skip=2):
|
||||
self._steps = 0
|
||||
# Small Context -> Easier. Todo: Should we do different versions?
|
||||
# self.xml_path = os.path.join(os.path.dirname(os.path.abspath(__file__)), "assets",
|
||||
# "beerpong_wo_cup" + ".xml")
|
||||
# self._cup_pos_min = np.array([-0.32, -2.2])
|
||||
# self._cup_pos_max = np.array([0.32, -1.2])
|
||||
|
||||
self.xml_path = os.path.join(os.path.dirname(os.path.abspath(__file__)), "assets",
|
||||
"beerpong_wo_cup_big_table" + ".xml")
|
||||
self._cup_pos_min = np.array([-1.42, -4.05])
|
||||
self._cup_pos_max = np.array([1.42, -1.25])
|
||||
|
||||
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.release_step = 100 # time step of ball release
|
||||
self.ep_length = 600 // frame_skip
|
||||
|
||||
self.repeat_action = frame_skip
|
||||
# TODO: If accessing IDs is easier in the (new) official mujoco bindings, remove this
|
||||
self.model = None
|
||||
self.site_id = lambda x: self.model.site_name2id(x)
|
||||
self.body_id = lambda x: self.model.body_name2id(x)
|
||||
self.geom_id = lambda x: self.model.geom_name2id(x)
|
||||
|
||||
# for reward calculation
|
||||
self.dists = []
|
||||
self.dists_final = []
|
||||
self.action_costs = []
|
||||
self.ball_ground_contact_first = False
|
||||
self.ball_table_contact = False
|
||||
self.ball_wall_contact = False
|
||||
self.ball_cup_contact = False
|
||||
self.ball_in_cup = False
|
||||
self.dist_ground_cup = -1 # distance floor to cup if first floor contact
|
||||
|
||||
MujocoEnv.__init__(self, self.xml_path, frame_skip=1, mujoco_bindings="mujoco_py")
|
||||
utils.EzPickle.__init__(self)
|
||||
|
||||
@property
|
||||
def start_pos(self):
|
||||
return self._start_pos
|
||||
|
||||
@property
|
||||
def start_vel(self):
|
||||
return self._start_vel
|
||||
|
||||
def reset(self, *, seed: Optional[int] = None, return_info: bool = False, options: Optional[dict] = None):
|
||||
self.dists = []
|
||||
self.dists_final = []
|
||||
self.action_costs = []
|
||||
self.ball_ground_contact_first = False
|
||||
self.ball_table_contact = False
|
||||
self.ball_wall_contact = False
|
||||
self.ball_cup_contact = False
|
||||
self.ball_in_cup = False
|
||||
self.dist_ground_cup = -1 # distance floor to cup if first floor contact
|
||||
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
|
||||
|
||||
# TODO: Ask Max why we need to set the state twice.
|
||||
self.set_state(start_pos, init_vel)
|
||||
start_pos[7::] = self.data.site_xpos[self.site_id("init_ball_pos"), :].copy()
|
||||
self.set_state(start_pos, init_vel)
|
||||
xy = self.np_random.uniform(self._cup_pos_min, self._cup_pos_max)
|
||||
xyz = np.zeros(3)
|
||||
xyz[:2] = xy
|
||||
xyz[-1] = 0.840
|
||||
self.model.body_pos[self.body_id("cup_table")] = xyz
|
||||
return self._get_obs()
|
||||
|
||||
def step(self, a):
|
||||
crash = False
|
||||
for _ in range(self.repeat_action):
|
||||
applied_action = a + self.data.qfrc_bias[:len(a)].copy() / self.model.actuator_gear[:, 0]
|
||||
try:
|
||||
self.do_simulation(applied_action, self.frame_skip)
|
||||
# self.reward_function.check_contacts(self.sim) # I assume this is not important?
|
||||
if self._steps < self.release_step:
|
||||
self.data.qpos[7::] = self.data.site_xpos[self.site_id("init_ball_pos"), :].copy()
|
||||
self.data.qvel[7::] = self.data.site_xvelp[self.site_id("init_ball_pos"), :].copy()
|
||||
crash = False
|
||||
except mujoco_py.builder.MujocoException:
|
||||
crash = True
|
||||
|
||||
ob = self._get_obs()
|
||||
|
||||
if not crash:
|
||||
reward, reward_infos = self._get_reward(applied_action)
|
||||
is_collided = reward_infos['is_collided']
|
||||
done = is_collided or self._steps == self.ep_length - 1
|
||||
self._steps += 1
|
||||
else:
|
||||
reward = -30
|
||||
done = True
|
||||
reward_infos = {"success": False, "ball_pos": np.zeros(3), "ball_vel": np.zeros(3), "is_collided": False}
|
||||
|
||||
infos = dict(
|
||||
reward=reward,
|
||||
action=a,
|
||||
q_pos=self.data.qpos[0:7].ravel().copy(),
|
||||
q_vel=self.data.qvel[0:7].ravel().copy(), sim_crash=crash,
|
||||
)
|
||||
infos.update(reward_infos)
|
||||
return ob, reward, done, infos
|
||||
|
||||
def _get_obs(self):
|
||||
theta = self.data.qpos.flat[:7]
|
||||
theta_dot = self.data.qvel.flat[:7]
|
||||
ball_pos = self.data.get_body_xpos("ball").copy()
|
||||
cup_goal_diff_final = ball_pos - self.data.get_site_xpos("cup_goal_final_table").copy()
|
||||
cup_goal_diff_top = ball_pos - self.data.get_site_xpos("cup_goal_table").copy()
|
||||
return np.concatenate([
|
||||
np.cos(theta),
|
||||
np.sin(theta),
|
||||
theta_dot,
|
||||
cup_goal_diff_final,
|
||||
cup_goal_diff_top,
|
||||
self.model.body_pos[self.body_id("cup_table")][:2].copy(),
|
||||
# [self._steps], # Use TimeAwareObservation Wrapper instead ....
|
||||
])
|
||||
|
||||
@property
|
||||
def dt(self):
|
||||
return super(BeerPongEnv, self).dt * self.repeat_action
|
||||
|
||||
def _get_reward(self, action):
|
||||
goal_pos = self.data.get_site_xpos("cup_goal_table")
|
||||
ball_pos = self.data.get_body_xpos("ball")
|
||||
ball_vel = self.data.get_body_xvelp("ball")
|
||||
goal_final_pos = self.data.get_site_xpos("cup_goal_final_table")
|
||||
|
||||
self._check_contacts()
|
||||
self.dists.append(np.linalg.norm(goal_pos - ball_pos))
|
||||
self.dists_final.append(np.linalg.norm(goal_final_pos - ball_pos))
|
||||
self.dist_ground_cup = np.linalg.norm(ball_pos - goal_pos) \
|
||||
if self.ball_ground_contact_first and self.dist_ground_cup == -1 else self.dist_ground_cup
|
||||
action_cost = np.sum(np.square(action))
|
||||
self.action_costs.append(np.copy(action_cost))
|
||||
# # ##################### Reward function which does not force to bounce once on the table (quad dist) #########
|
||||
|
||||
# Is this needed?
|
||||
# self._is_collided = self._check_collision_with_itself([self.geom_id(name) for name in CUP_COLLISION_OBJ])
|
||||
|
||||
if self._steps == self.ep_length - 1: # or self._is_collided:
|
||||
min_dist = np.min(self.dists)
|
||||
final_dist = self.dists_final[-1]
|
||||
if self.ball_ground_contact_first:
|
||||
min_dist_coeff, final_dist_coeff, ground_contact_dist_coeff, rew_offset = 1, 0.5, 2, -4
|
||||
else:
|
||||
if not self.ball_in_cup:
|
||||
if not self.ball_table_contact and not self.ball_cup_contact and not self.ball_wall_contact:
|
||||
min_dist_coeff, final_dist_coeff, ground_contact_dist_coeff, rew_offset = 1, 0.5, 0, -4
|
||||
else:
|
||||
min_dist_coeff, final_dist_coeff, ground_contact_dist_coeff, rew_offset = 1, 0.5, 0, -2
|
||||
else:
|
||||
min_dist_coeff, final_dist_coeff, ground_contact_dist_coeff, rew_offset = 0, 1, 0, 0
|
||||
action_cost = 1e-4 * np.mean(action_cost)
|
||||
reward = rew_offset - min_dist_coeff * min_dist ** 2 - final_dist_coeff * final_dist ** 2 - \
|
||||
action_cost - ground_contact_dist_coeff * self.dist_ground_cup ** 2
|
||||
# release step punishment
|
||||
min_time_bound = 0.1
|
||||
max_time_bound = 1.0
|
||||
release_time = self.release_step * self.dt
|
||||
release_time_rew = int(release_time < min_time_bound) * (-30 - 10 * (release_time - min_time_bound) ** 2) + \
|
||||
int(release_time > max_time_bound) * (-30 - 10 * (release_time - max_time_bound) ** 2)
|
||||
reward += release_time_rew
|
||||
success = self.ball_in_cup
|
||||
else:
|
||||
action_cost = 1e-2 * action_cost
|
||||
reward = - action_cost
|
||||
success = False
|
||||
# ##############################################################################################################
|
||||
infos = {"success": success, "ball_pos": ball_pos.copy(),
|
||||
"ball_vel": ball_vel.copy(), "action_cost": action_cost, "task_reward": reward,
|
||||
"table_contact_first": int(not self.ball_ground_contact_first),
|
||||
"is_collided": False} # TODO: Check if is collided is needed
|
||||
return reward, infos
|
||||
|
||||
def _check_contacts(self):
|
||||
if not self.ball_table_contact:
|
||||
self.ball_table_contact = self._check_collision({self.geom_id("ball_geom")},
|
||||
{self.geom_id("table_contact_geom")})
|
||||
if not self.ball_cup_contact:
|
||||
self.ball_cup_contact = self._check_collision({self.geom_id("ball_geom")},
|
||||
{self.geom_id(name) for name in CUP_COLLISION_OBJ})
|
||||
if not self.ball_wall_contact:
|
||||
self.ball_wall_contact = self._check_collision({self.geom_id("ball_geom")},
|
||||
{self.geom_id("wall")})
|
||||
if not self.ball_in_cup:
|
||||
self.ball_in_cup = self._check_collision({self.geom_id("ball_geom")},
|
||||
{self.geom_id("cup_base_table_contact")})
|
||||
if not self.ball_ground_contact_first:
|
||||
if not self.ball_table_contact and not self.ball_cup_contact and not self.ball_wall_contact \
|
||||
and not self.ball_in_cup:
|
||||
self.ball_ground_contact_first = self._check_collision({self.geom_id("ball_geom")},
|
||||
{self.geom_id("ground")})
|
||||
|
||||
# Checks if id_set1 has a collision with id_set2
|
||||
def _check_collision(self, id_set_1, id_set_2):
|
||||
"""
|
||||
If id_set_2 is set to None, it will check for a collision with itself (id_set_1).
|
||||
"""
|
||||
collision_id_set = id_set_2 - id_set_1 if id_set_2 is not None else id_set_1
|
||||
for coni in range(self.data.ncon):
|
||||
con = self.data.contact[coni]
|
||||
if ((con.geom1 in id_set_1 and con.geom2 in collision_id_set) or
|
||||
(con.geom2 in id_set_1 and con.geom1 in collision_id_set)):
|
||||
return True
|
||||
return False
|
||||
|
||||
|
||||
class BeerPongEnvFixedReleaseStep(BeerPongEnv):
|
||||
def __init__(self, frame_skip=2):
|
||||
super().__init__(frame_skip)
|
||||
self.release_step = 62 # empirically evaluated for frame_skip=2!
|
||||
|
||||
|
||||
class BeerPongEnvStepBasedEpisodicReward(BeerPongEnv):
|
||||
def __init__(self, frame_skip=2):
|
||||
super().__init__(frame_skip)
|
||||
self.release_step = 62 # empirically evaluated for frame_skip=2!
|
||||
|
||||
def step(self, a):
|
||||
if self._steps < self.release_step:
|
||||
return super(BeerPongEnvStepBasedEpisodicReward, self).step(a)
|
||||
else:
|
||||
reward = 0
|
||||
done = False
|
||||
while not done:
|
||||
sub_ob, sub_reward, done, sub_infos = super(BeerPongEnvStepBasedEpisodicReward, self).step(
|
||||
np.zeros(a.shape))
|
||||
reward += sub_reward
|
||||
infos = sub_infos
|
||||
ob = sub_ob
|
||||
ob[-1] = self.release_step + 1 # Since we simulate until the end of the episode, PPO does not see the
|
||||
# internal steps and thus, the observation also needs to be set correctly
|
||||
return ob, reward, done, infos
|
||||
|
||||
|
||||
# class ALRBeerBongEnvStepBased(ALRBeerBongEnv):
|
||||
# def __init__(self, frame_skip=1, apply_gravity_comp=True, noisy=False, rndm_goal=False, cup_goal_pos=None):
|
||||
# super().__init__(frame_skip, apply_gravity_comp, noisy, rndm_goal, cup_goal_pos)
|
||||
# self.release_step = 62 # empirically evaluated for frame_skip=2!
|
||||
#
|
||||
# def step(self, a):
|
||||
# if self._steps < self.release_step:
|
||||
# return super(ALRBeerBongEnvStepBased, self).step(a)
|
||||
# else:
|
||||
# reward = 0
|
||||
# done = False
|
||||
# while not done:
|
||||
# sub_ob, sub_reward, done, sub_infos = super(ALRBeerBongEnvStepBased, self).step(np.zeros(a.shape))
|
||||
# if not done or sub_infos['sim_crash']:
|
||||
# reward += sub_reward
|
||||
# else:
|
||||
# ball_pos = self.sim.data.body_xpos[self.sim.model._body_name2id["ball"]].copy()
|
||||
# cup_goal_dist_final = np.linalg.norm(ball_pos - self.sim.data.site_xpos[
|
||||
# self.sim.model._site_name2id["cup_goal_final_table"]].copy())
|
||||
# cup_goal_dist_top = np.linalg.norm(ball_pos - self.sim.data.site_xpos[
|
||||
# self.sim.model._site_name2id["cup_goal_table"]].copy())
|
||||
# if sub_infos['success']:
|
||||
# dist_rew = -cup_goal_dist_final ** 2
|
||||
# else:
|
||||
# dist_rew = -0.5 * cup_goal_dist_final ** 2 - cup_goal_dist_top ** 2
|
||||
# reward = reward - sub_infos['action_cost'] + dist_rew
|
||||
# infos = sub_infos
|
||||
# ob = sub_ob
|
||||
# ob[-1] = self.release_step + 1 # Since we simulate until the end of the episode, PPO does not see the
|
||||
# # internal steps and thus, the observation also needs to be set correctly
|
||||
# return ob, reward, done, infos
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
env = BeerPongEnv(frame_skip=2)
|
||||
env.seed(0)
|
||||
# env = ALRBeerBongEnvStepBased(frame_skip=2)
|
||||
# env = ALRBeerBongEnvStepBasedEpisodicReward(frame_skip=2)
|
||||
# env = ALRBeerBongEnvFixedReleaseStep(frame_skip=2)
|
||||
import time
|
||||
|
||||
env.reset()
|
||||
env.render("human")
|
||||
for i in range(600):
|
||||
# ac = 10 * env.action_space.sample()
|
||||
ac = 0.05 * np.ones(7)
|
||||
obs, rew, d, info = env.step(ac)
|
||||
env.render("human")
|
||||
|
||||
if d:
|
||||
print('reward:', rew)
|
||||
print('RESETTING')
|
||||
env.reset()
|
||||
time.sleep(1)
|
||||
env.close()
|
||||
@@ -0,0 +1,212 @@
|
||||
import os
|
||||
|
||||
import mujoco_py.builder
|
||||
import numpy as np
|
||||
from gym import utils
|
||||
from gym.envs.mujoco import MujocoEnv
|
||||
|
||||
from alr_envs.envs.mujoco.beerpong.deprecated.beerpong_reward_staged import BeerPongReward
|
||||
|
||||
|
||||
class BeerPongEnv(MujocoEnv, utils.EzPickle):
|
||||
def __init__(self, frame_skip=2):
|
||||
self._steps = 0
|
||||
# Small Context -> Easier. Todo: Should we do different versions?
|
||||
# self.xml_path = os.path.join(os.path.dirname(os.path.abspath(__file__)), "assets",
|
||||
# "beerpong_wo_cup" + ".xml")
|
||||
# self._cup_pos_min = np.array([-0.32, -2.2])
|
||||
# self._cup_pos_max = np.array([0.32, -1.2])
|
||||
|
||||
self.xml_path = os.path.join(os.path.dirname(os.path.abspath(__file__)), "../assets",
|
||||
"beerpong_wo_cup_big_table" + ".xml")
|
||||
self._cup_pos_min = np.array([-1.42, -4.05])
|
||||
self._cup_pos_max = np.array([1.42, -1.25])
|
||||
|
||||
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.release_step = 100 # time step of ball release
|
||||
self.ep_length = 600 // frame_skip
|
||||
|
||||
self.reward_function = BeerPongReward()
|
||||
self.repeat_action = frame_skip
|
||||
self.model = None
|
||||
self.site_id = lambda x: self.model.site_name2id(x)
|
||||
self.body_id = lambda x: self.model.body_name2id(x)
|
||||
|
||||
MujocoEnv.__init__(self, self.xml_path, frame_skip=1)
|
||||
utils.EzPickle.__init__(self)
|
||||
|
||||
@property
|
||||
def start_pos(self):
|
||||
return self._start_pos
|
||||
|
||||
@property
|
||||
def start_vel(self):
|
||||
return self._start_vel
|
||||
|
||||
def reset(self):
|
||||
self.reward_function.reset()
|
||||
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
|
||||
|
||||
# TODO: Ask Max why we need to set the state twice.
|
||||
self.set_state(start_pos, init_vel)
|
||||
start_pos[7::] = self.sim.data.site_xpos[self.site_id("init_ball_pos"), :].copy()
|
||||
self.set_state(start_pos, init_vel)
|
||||
xy = self.np_random.uniform(self._cup_pos_min, self._cup_pos_max)
|
||||
xyz = np.zeros(3)
|
||||
xyz[:2] = xy
|
||||
xyz[-1] = 0.840
|
||||
self.sim.model.body_pos[self.body_id("cup_table")] = xyz
|
||||
return self._get_obs()
|
||||
|
||||
def step(self, a):
|
||||
crash = False
|
||||
for _ in range(self.repeat_action):
|
||||
applied_action = a + self.sim.data.qfrc_bias[:len(a)].copy() / self.model.actuator_gear[:, 0]
|
||||
try:
|
||||
self.do_simulation(applied_action, self.frame_skip)
|
||||
self.reward_function.initialize(self)
|
||||
# self.reward_function.check_contacts(self.sim) # I assume this is not important?
|
||||
if self._steps < self.release_step:
|
||||
self.sim.data.qpos[7::] = self.sim.data.site_xpos[self.site_id("init_ball_pos"), :].copy()
|
||||
self.sim.data.qvel[7::] = self.sim.data.site_xvelp[self.site_id("init_ball_pos"), :].copy()
|
||||
crash = False
|
||||
except mujoco_py.builder.MujocoException:
|
||||
crash = True
|
||||
|
||||
ob = self._get_obs()
|
||||
|
||||
if not crash:
|
||||
reward, reward_infos = self.reward_function.compute_reward(self, applied_action)
|
||||
is_collided = reward_infos['is_collided']
|
||||
done = is_collided or self._steps == self.ep_length - 1
|
||||
self._steps += 1
|
||||
else:
|
||||
reward = -30
|
||||
done = True
|
||||
reward_infos = {"success": False, "ball_pos": np.zeros(3), "ball_vel": np.zeros(3), "is_collided": False}
|
||||
|
||||
infos = dict(
|
||||
reward=reward,
|
||||
action=a,
|
||||
q_pos=self.sim.data.qpos[0:7].ravel().copy(),
|
||||
q_vel=self.sim.data.qvel[0:7].ravel().copy(), sim_crash=crash,
|
||||
)
|
||||
infos.update(reward_infos)
|
||||
return ob, reward, done, infos
|
||||
|
||||
def _get_obs(self):
|
||||
theta = self.sim.data.qpos.flat[:7]
|
||||
theta_dot = self.sim.data.qvel.flat[:7]
|
||||
ball_pos = self.data.get_body_xpos("ball").copy()
|
||||
cup_goal_diff_final = ball_pos - self.data.get_site_xpos("cup_goal_final_table").copy()
|
||||
cup_goal_diff_top = ball_pos - self.data.get_site_xpos("cup_goal_table").copy()
|
||||
return np.concatenate([
|
||||
np.cos(theta),
|
||||
np.sin(theta),
|
||||
theta_dot,
|
||||
cup_goal_diff_final,
|
||||
cup_goal_diff_top,
|
||||
self.sim.model.body_pos[self.body_id("cup_table")][:2].copy(),
|
||||
[self._steps],
|
||||
])
|
||||
|
||||
@property
|
||||
def dt(self):
|
||||
return super(BeerPongEnv, self).dt * self.repeat_action
|
||||
|
||||
|
||||
class BeerPongEnvFixedReleaseStep(BeerPongEnv):
|
||||
def __init__(self, frame_skip=2):
|
||||
super().__init__(frame_skip)
|
||||
self.release_step = 62 # empirically evaluated for frame_skip=2!
|
||||
|
||||
|
||||
class BeerPongEnvStepBasedEpisodicReward(BeerPongEnv):
|
||||
def __init__(self, frame_skip=2):
|
||||
super().__init__(frame_skip)
|
||||
self.release_step = 62 # empirically evaluated for frame_skip=2!
|
||||
|
||||
def step(self, a):
|
||||
if self._steps < self.release_step:
|
||||
return super(BeerPongEnvStepBasedEpisodicReward, self).step(a)
|
||||
else:
|
||||
reward = 0
|
||||
done = False
|
||||
while not done:
|
||||
sub_ob, sub_reward, done, sub_infos = super(BeerPongEnvStepBasedEpisodicReward, self).step(
|
||||
np.zeros(a.shape))
|
||||
reward += sub_reward
|
||||
infos = sub_infos
|
||||
ob = sub_ob
|
||||
ob[-1] = self.release_step + 1 # Since we simulate until the end of the episode, PPO does not see the
|
||||
# internal steps and thus, the observation also needs to be set correctly
|
||||
return ob, reward, done, infos
|
||||
|
||||
|
||||
# class ALRBeerBongEnvStepBased(ALRBeerBongEnv):
|
||||
# def __init__(self, frame_skip=1, apply_gravity_comp=True, noisy=False, rndm_goal=False, cup_goal_pos=None):
|
||||
# super().__init__(frame_skip, apply_gravity_comp, noisy, rndm_goal, cup_goal_pos)
|
||||
# self.release_step = 62 # empirically evaluated for frame_skip=2!
|
||||
#
|
||||
# def step(self, a):
|
||||
# if self._steps < self.release_step:
|
||||
# return super(ALRBeerBongEnvStepBased, self).step(a)
|
||||
# else:
|
||||
# reward = 0
|
||||
# done = False
|
||||
# while not done:
|
||||
# sub_ob, sub_reward, done, sub_infos = super(ALRBeerBongEnvStepBased, self).step(np.zeros(a.shape))
|
||||
# if not done or sub_infos['sim_crash']:
|
||||
# reward += sub_reward
|
||||
# else:
|
||||
# ball_pos = self.sim.data.body_xpos[self.sim.model._body_name2id["ball"]].copy()
|
||||
# cup_goal_dist_final = np.linalg.norm(ball_pos - self.sim.data.site_xpos[
|
||||
# self.sim.model._site_name2id["cup_goal_final_table"]].copy())
|
||||
# cup_goal_dist_top = np.linalg.norm(ball_pos - self.sim.data.site_xpos[
|
||||
# self.sim.model._site_name2id["cup_goal_table"]].copy())
|
||||
# if sub_infos['success']:
|
||||
# dist_rew = -cup_goal_dist_final ** 2
|
||||
# else:
|
||||
# dist_rew = -0.5 * cup_goal_dist_final ** 2 - cup_goal_dist_top ** 2
|
||||
# reward = reward - sub_infos['action_cost'] + dist_rew
|
||||
# infos = sub_infos
|
||||
# ob = sub_ob
|
||||
# ob[-1] = self.release_step + 1 # Since we simulate until the end of the episode, PPO does not see the
|
||||
# # internal steps and thus, the observation also needs to be set correctly
|
||||
# return ob, reward, done, infos
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
env = BeerPongEnv(frame_skip=2)
|
||||
env.seed(0)
|
||||
# env = ALRBeerBongEnvStepBased(frame_skip=2)
|
||||
# env = ALRBeerBongEnvStepBasedEpisodicReward(frame_skip=2)
|
||||
# env = ALRBeerBongEnvFixedReleaseStep(frame_skip=2)
|
||||
import time
|
||||
|
||||
env.reset()
|
||||
env.render("human")
|
||||
for i in range(600):
|
||||
# ac = 10 * env.action_space.sample()
|
||||
ac = 0.05 * np.ones(7)
|
||||
obs, rew, d, info = env.step(ac)
|
||||
env.render("human")
|
||||
|
||||
if d:
|
||||
print('reward:', rew)
|
||||
print('RESETTING')
|
||||
env.reset()
|
||||
time.sleep(1)
|
||||
env.close()
|
||||
@@ -0,0 +1,183 @@
|
||||
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_geom_table15",
|
||||
"cup_geom_table16",
|
||||
"cup_geom_table17", "cup_geom1_table8",
|
||||
]
|
||||
|
||||
self.dists = None
|
||||
self.dists_final = None
|
||||
self.action_costs = None
|
||||
self.ball_ground_contact_first = False
|
||||
self.ball_table_contact = False
|
||||
self.ball_wall_contact = False
|
||||
self.ball_cup_contact = False
|
||||
self.ball_in_cup = False
|
||||
self.dist_ground_cup = -1 # distance floor to cup if first floor contact
|
||||
|
||||
# IDs
|
||||
self.ball_collision_id = None
|
||||
self.table_collision_id = None
|
||||
self.wall_collision_id = None
|
||||
self.cup_table_collision_id = None
|
||||
self.ground_collision_id = None
|
||||
self.cup_collision_ids = None
|
||||
self.robot_collision_ids = None
|
||||
self.reset()
|
||||
self.is_initialized = False
|
||||
|
||||
def reset(self):
|
||||
self.dists = []
|
||||
self.dists_final = []
|
||||
self.action_costs = []
|
||||
self.ball_ground_contact_first = False
|
||||
self.ball_table_contact = False
|
||||
self.ball_wall_contact = False
|
||||
self.ball_cup_contact = False
|
||||
self.ball_in_cup = False
|
||||
self.dist_ground_cup = -1 # distance floor to cup if first floor contact
|
||||
|
||||
def initialize(self, env):
|
||||
|
||||
if not self.is_initialized:
|
||||
self.is_initialized = True
|
||||
# TODO: Find a more elegant way to acces to the geom ids in each step -> less code
|
||||
self.ball_collision_id = {env.model.geom_name2id("ball_geom")}
|
||||
# self.ball_collision_id = env.model.geom_name2id("ball_geom")
|
||||
self.table_collision_id = {env.model.geom_name2id("table_contact_geom")}
|
||||
# self.table_collision_id = env.model.geom_name2id("table_contact_geom")
|
||||
self.wall_collision_id = {env.model.geom_name2id("wall")}
|
||||
# self.wall_collision_id = env.model.geom_name2id("wall")
|
||||
self.cup_table_collision_id = {env.model.geom_name2id("cup_base_table_contact")}
|
||||
# self.cup_table_collision_id = env.model.geom_name2id("cup_base_table_contact")
|
||||
self.ground_collision_id = {env.model.geom_name2id("ground")}
|
||||
# self.ground_collision_id = env.model.geom_name2id("ground")
|
||||
self.cup_collision_ids = {env.model.geom_name2id(name) for name in self.cup_collision_objects}
|
||||
# self.cup_collision_ids = [env.model.geom_name2id(name) for name in self.cup_collision_objects]
|
||||
self.robot_collision_ids = [env.model.geom_name2id(name) for name in self.robot_collision_objects]
|
||||
|
||||
def compute_reward(self, env, action):
|
||||
|
||||
goal_pos = env.data.get_site_xpos("cup_goal_table")
|
||||
ball_pos = env.data.get_body_xpos("ball")
|
||||
ball_vel = env.data.get_body_xvelp("ball")
|
||||
goal_final_pos = env.data.get_site_xpos("cup_goal_final_table")
|
||||
|
||||
self.check_contacts(env.sim)
|
||||
self.dists.append(np.linalg.norm(goal_pos - ball_pos))
|
||||
self.dists_final.append(np.linalg.norm(goal_final_pos - ball_pos))
|
||||
self.dist_ground_cup = np.linalg.norm(ball_pos - goal_pos) \
|
||||
if self.ball_ground_contact_first and self.dist_ground_cup == -1 else self.dist_ground_cup
|
||||
action_cost = np.sum(np.square(action))
|
||||
self.action_costs.append(np.copy(action_cost))
|
||||
# # ##################### Reward function which does not force to bounce once on the table (quad dist) #########
|
||||
|
||||
# Is this needed?
|
||||
# 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]
|
||||
if self.ball_ground_contact_first:
|
||||
min_dist_coeff, final_dist_coeff, ground_contact_dist_coeff, rew_offset = 1, 0.5, 2, -4
|
||||
else:
|
||||
if not self.ball_in_cup:
|
||||
if not self.ball_table_contact and not self.ball_cup_contact and not self.ball_wall_contact:
|
||||
min_dist_coeff, final_dist_coeff, ground_contact_dist_coeff, rew_offset = 1, 0.5, 0, -4
|
||||
else:
|
||||
min_dist_coeff, final_dist_coeff, ground_contact_dist_coeff, rew_offset = 1, 0.5, 0, -2
|
||||
else:
|
||||
min_dist_coeff, final_dist_coeff, ground_contact_dist_coeff, rew_offset = 0, 1, 0, 0
|
||||
action_cost = 1e-4 * np.mean(action_cost)
|
||||
reward = rew_offset - min_dist_coeff * min_dist ** 2 - final_dist_coeff * final_dist ** 2 - \
|
||||
action_cost - ground_contact_dist_coeff * self.dist_ground_cup ** 2
|
||||
# release step punishment
|
||||
min_time_bound = 0.1
|
||||
max_time_bound = 1.0
|
||||
release_time = env.release_step * env.dt
|
||||
release_time_rew = int(release_time < min_time_bound) * (-30 - 10 * (release_time - min_time_bound) ** 2) + \
|
||||
int(release_time > max_time_bound) * (-30 - 10 * (release_time - max_time_bound) ** 2)
|
||||
reward += release_time_rew
|
||||
success = self.ball_in_cup
|
||||
else:
|
||||
action_cost = 1e-2 * action_cost
|
||||
reward = - action_cost
|
||||
success = False
|
||||
# ##############################################################################################################
|
||||
infos = {"success": success, "ball_pos": ball_pos.copy(),
|
||||
"ball_vel": ball_vel.copy(), "action_cost": action_cost, "task_reward": reward,
|
||||
"table_contact_first": int(not self.ball_ground_contact_first),
|
||||
"is_collided": False} # TODO: Check if is collided is needed
|
||||
return reward, infos
|
||||
|
||||
def check_contacts(self, sim):
|
||||
if not self.ball_table_contact:
|
||||
self.ball_table_contact = self._check_collision(sim, self.ball_collision_id, self.table_collision_id)
|
||||
if not self.ball_cup_contact:
|
||||
self.ball_cup_contact = self._check_collision(sim, self.ball_collision_id, self.cup_collision_ids)
|
||||
if not self.ball_wall_contact:
|
||||
self.ball_wall_contact = self._check_collision(sim, self.ball_collision_id, self.wall_collision_id)
|
||||
if not self.ball_in_cup:
|
||||
self.ball_in_cup = self._check_collision(sim, self.ball_collision_id, self.cup_table_collision_id)
|
||||
if not self.ball_ground_contact_first:
|
||||
if not self.ball_table_contact and not self.ball_cup_contact and not self.ball_wall_contact \
|
||||
and not self.ball_in_cup:
|
||||
self.ball_ground_contact_first = self._check_collision(sim, self.ball_collision_id,
|
||||
self.ground_collision_id)
|
||||
|
||||
# Checks if id_set1 has a collision with id_set2
|
||||
def _check_collision(self, sim, id_set_1, id_set_2):
|
||||
"""
|
||||
If id_set_2 is set to None, it will check for a collision with itself (id_set_1).
|
||||
"""
|
||||
collision_id_set = id_set_2 - id_set_1 if id_set_2 is not None else id_set_1
|
||||
for coni in range(sim.data.ncon):
|
||||
con = sim.data.contact[coni]
|
||||
if ((con.geom1 in id_set_1 and con.geom2 in collision_id_set) or
|
||||
(con.geom2 in id_set_1 and con.geom1 in collision_id_set)):
|
||||
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
|
||||
|
||||
### This function will not be needed if we really do not need to check for collision with itself
|
||||
# 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
|
||||
@@ -0,0 +1,43 @@
|
||||
from typing import Union, Tuple
|
||||
|
||||
import numpy as np
|
||||
|
||||
from alr_envs.black_box.raw_interface_wrapper import RawInterfaceWrapper
|
||||
|
||||
|
||||
class MPWrapper(RawInterfaceWrapper):
|
||||
|
||||
@property
|
||||
def context_mask(self) -> np.ndarray:
|
||||
return np.hstack([
|
||||
[False] * 7, # cos
|
||||
[False] * 7, # sin
|
||||
[False] * 7, # joint velocities
|
||||
[False] * 3, # cup_goal_diff_final
|
||||
[False] * 3, # cup_goal_diff_top
|
||||
[True] * 2, # xy position of cup
|
||||
# [False] # env steps
|
||||
])
|
||||
|
||||
@property
|
||||
def current_pos(self) -> Union[float, int, np.ndarray, Tuple]:
|
||||
return self.env.data.qpos[0:7].copy()
|
||||
|
||||
@property
|
||||
def current_vel(self) -> Union[float, int, np.ndarray, Tuple]:
|
||||
return self.env.data.qvel[0:7].copy()
|
||||
|
||||
# TODO: Fix this
|
||||
def _episode_callback(self, action: np.ndarray, mp) -> Tuple[np.ndarray, Union[np.ndarray, None]]:
|
||||
if mp.learn_tau:
|
||||
self.env.env.release_step = action[0] / self.env.dt # Tau value
|
||||
return action, None
|
||||
else:
|
||||
return action, None
|
||||
|
||||
def set_context(self, context):
|
||||
xyz = np.zeros(3)
|
||||
xyz[:2] = context
|
||||
xyz[-1] = 0.840
|
||||
self.env.env.model.body_pos[self.env.env.cup_table_id] = xyz
|
||||
return self.get_observation_from_step(self.env.env._get_obs())
|
||||
@@ -0,0 +1 @@
|
||||
from .mp_wrapper import MPWrapper
|
||||
@@ -0,0 +1,62 @@
|
||||
<mujoco model="cheetah">
|
||||
<compiler angle="radian" coordinate="local" inertiafromgeom="true" settotalmass="14"/>
|
||||
<default>
|
||||
<joint armature=".1" damping=".01" limited="true" solimplimit="0 .8 .03" solreflimit=".02 1" stiffness="8"/>
|
||||
<geom conaffinity="0" condim="3" contype="1" friction=".4 .1 .1" rgba="0.8 0.6 .4 1" solimp="0.0 0.8 0.01" solref="0.02 1"/>
|
||||
<motor ctrllimited="true" ctrlrange="-1 1"/>
|
||||
</default>
|
||||
<size nstack="300000" nuser_geom="1"/>
|
||||
<option gravity="0 0 -9.81" timestep="0.01"/>
|
||||
<asset>
|
||||
<texture builtin="gradient" height="100" rgb1="1 1 1" rgb2="0 0 0" type="skybox" width="100"/>
|
||||
<texture builtin="flat" height="1278" mark="cross" markrgb="1 1 1" name="texgeom" random="0.01" rgb1="0.8 0.6 0.4" rgb2="0.8 0.6 0.4" type="cube" width="127"/>
|
||||
<texture builtin="checker" height="100" name="texplane" rgb1="0 0 0" rgb2="0.8 0.8 0.8" type="2d" width="100"/>
|
||||
<material name="MatPlane" reflectance="0.5" shininess="1" specular="1" texrepeat="60 60" texture="texplane"/>
|
||||
<material name="geom" texture="texgeom" texuniform="true"/>
|
||||
</asset>
|
||||
<worldbody>
|
||||
<light cutoff="100" diffuse="1 1 1" dir="-0 0 -1.3" directional="true" exponent="1" pos="0 0 1.3" specular=".1 .1 .1"/>
|
||||
<geom conaffinity="1" condim="3" material="MatPlane" name="floor" pos="0 0 0" rgba="0.8 0.9 0.8 1" size="40 40 40" type="plane"/>
|
||||
<body name="torso" pos="0 0 .7">
|
||||
<camera name="track" mode="trackcom" pos="0 -3 0.3" xyaxes="1 0 0 0 0 1"/>
|
||||
<joint armature="0" axis="1 0 0" damping="0" limited="false" name="rootx" pos="0 0 0" stiffness="0" type="slide"/>
|
||||
<joint armature="0" axis="0 0 1" damping="0" limited="false" name="rootz" pos="0 0 0" stiffness="0" type="slide"/>
|
||||
<joint armature="0" axis="0 1 0" damping="0" limited="false" name="rooty" pos="0 0 0" stiffness="0" type="hinge"/>
|
||||
<geom fromto="-.5 0 0 .5 0 0" name="torso" size="0.046" type="capsule"/>
|
||||
<geom axisangle="0 1 0 .87" name="head" pos=".6 0 .1" size="0.046 .15" type="capsule"/>
|
||||
<!-- <site name='tip' pos='.15 0 .11'/>-->
|
||||
<body name="bthigh" pos="-.5 0 0">
|
||||
<joint axis="0 1 0" damping="6" name="bthigh" pos="0 0 0" range="-.52 1.05" stiffness="240" type="hinge"/>
|
||||
<geom axisangle="0 1 0 -3.8" name="bthigh" pos=".1 0 -.13" size="0.046 .145" type="capsule"/>
|
||||
<body name="bshin" pos=".16 0 -.25">
|
||||
<joint axis="0 1 0" damping="4.5" name="bshin" pos="0 0 0" range="-.785 .785" stiffness="180" type="hinge"/>
|
||||
<geom axisangle="0 1 0 -2.03" name="bshin" pos="-.14 0 -.07" rgba="0.9 0.6 0.6 1" size="0.046 .15" type="capsule"/>
|
||||
<body name="bfoot" pos="-.28 0 -.14">
|
||||
<joint axis="0 1 0" damping="3" name="bfoot" pos="0 0 0" range="-.4 .785" stiffness="120" type="hinge"/>
|
||||
<geom axisangle="0 1 0 -.27" name="bfoot" pos=".03 0 -.097" rgba="0.9 0.6 0.6 1" size="0.046 .094" type="capsule"/>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
<body name="fthigh" pos=".5 0 0">
|
||||
<joint axis="0 1 0" damping="4.5" name="fthigh" pos="0 0 0" range="-1 .7" stiffness="180" type="hinge"/>
|
||||
<geom axisangle="0 1 0 .52" name="fthigh" pos="-.07 0 -.12" size="0.046 .133" type="capsule"/>
|
||||
<body name="fshin" pos="-.14 0 -.24">
|
||||
<joint axis="0 1 0" damping="3" name="fshin" pos="0 0 0" range="-1.2 .87" stiffness="120" type="hinge"/>
|
||||
<geom axisangle="0 1 0 -.6" name="fshin" pos=".065 0 -.09" rgba="0.9 0.6 0.6 1" size="0.046 .106" type="capsule"/>
|
||||
<body name="ffoot" pos=".13 0 -.18">
|
||||
<joint axis="0 1 0" damping="1.5" name="ffoot" pos="0 0 0" range="-.5 .5" stiffness="60" type="hinge"/>
|
||||
<geom axisangle="0 1 0 -.6" name="ffoot" pos=".045 0 -.07" rgba="0.9 0.6 0.6 1" size="0.046 .07" type="capsule"/>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</worldbody>
|
||||
<actuator>
|
||||
<motor gear="120" joint="bthigh" name="bthigh"/>
|
||||
<motor gear="90" joint="bshin" name="bshin"/>
|
||||
<motor gear="60" joint="bfoot" name="bfoot"/>
|
||||
<motor gear="120" joint="fthigh" name="fthigh"/>
|
||||
<motor gear="60" joint="fshin" name="fshin"/>
|
||||
<motor gear="30" joint="ffoot" name="ffoot"/>
|
||||
</actuator>
|
||||
</mujoco>
|
||||
@@ -0,0 +1,86 @@
|
||||
import os
|
||||
from typing import Tuple, Union, Optional
|
||||
|
||||
from gym.core import ObsType
|
||||
from gym.envs.mujoco.half_cheetah_v3 import HalfCheetahEnv
|
||||
import numpy as np
|
||||
|
||||
MAX_EPISODE_STEPS_HALFCHEETAHJUMP = 100
|
||||
|
||||
|
||||
class ALRHalfCheetahJumpEnv(HalfCheetahEnv):
|
||||
"""
|
||||
ctrl_cost_weight 0.1 -> 0.0
|
||||
"""
|
||||
|
||||
def __init__(self,
|
||||
xml_file='cheetah.xml',
|
||||
forward_reward_weight=1.0,
|
||||
ctrl_cost_weight=0.0,
|
||||
reset_noise_scale=0.1,
|
||||
context=True,
|
||||
exclude_current_positions_from_observation=True,
|
||||
max_episode_steps=100):
|
||||
self.current_step = 0
|
||||
self.max_height = 0
|
||||
# self.max_episode_steps = max_episode_steps
|
||||
self.goal = 0
|
||||
self.context = context
|
||||
xml_file = os.path.join(os.path.dirname(__file__), "assets", xml_file)
|
||||
super().__init__(xml_file, forward_reward_weight, ctrl_cost_weight, reset_noise_scale,
|
||||
exclude_current_positions_from_observation)
|
||||
|
||||
def step(self, action):
|
||||
|
||||
self.current_step += 1
|
||||
self.do_simulation(action, self.frame_skip)
|
||||
|
||||
height_after = self.get_body_com("torso")[2]
|
||||
self.max_height = max(height_after, self.max_height)
|
||||
|
||||
## Didnt use fell_over, because base env also has no done condition - Paul and Marc
|
||||
# fell_over = abs(self.sim.data.qpos[2]) > 2.5 # how to figure out if the cheetah fell over? -> 2.5 oke?
|
||||
# TODO: Should a fall over be checked here?
|
||||
done = False
|
||||
|
||||
ctrl_cost = self.control_cost(action)
|
||||
costs = ctrl_cost
|
||||
|
||||
if self.current_step == MAX_EPISODE_STEPS_HALFCHEETAHJUMP:
|
||||
height_goal_distance = -10 * np.linalg.norm(self.max_height - self.goal) + 1e-8 if self.context \
|
||||
else self.max_height
|
||||
rewards = self._forward_reward_weight * height_goal_distance
|
||||
else:
|
||||
rewards = 0
|
||||
|
||||
observation = self._get_obs()
|
||||
reward = rewards - costs
|
||||
info = {
|
||||
'height': height_after,
|
||||
'max_height': self.max_height
|
||||
}
|
||||
|
||||
return observation, reward, done, info
|
||||
|
||||
def _get_obs(self):
|
||||
return np.append(super()._get_obs(), self.goal)
|
||||
|
||||
def reset(self, *, seed: Optional[int] = None, return_info: bool = False,
|
||||
options: Optional[dict] = None, ) -> Union[ObsType, Tuple[ObsType, dict]]:
|
||||
self.max_height = 0
|
||||
self.current_step = 0
|
||||
self.goal = self.np_random.uniform(1.1, 1.6, 1) # 1.1 1.6
|
||||
return super().reset()
|
||||
|
||||
# overwrite reset_model to make it deterministic
|
||||
def reset_model(self):
|
||||
noise_low = -self._reset_noise_scale
|
||||
noise_high = self._reset_noise_scale
|
||||
|
||||
qpos = self.init_qpos # + self.np_random.uniform(low=noise_low, high=noise_high, size=self.model.nq)
|
||||
qvel = self.init_qvel # + self.np_random.uniform(low=noise_low, high=noise_high, size=self.model.nv)
|
||||
|
||||
self.set_state(qpos, qvel)
|
||||
|
||||
observation = self._get_obs()
|
||||
return observation
|
||||
@@ -0,0 +1,22 @@
|
||||
from typing import Tuple, Union
|
||||
|
||||
import numpy as np
|
||||
|
||||
from alr_envs.black_box.raw_interface_wrapper import RawInterfaceWrapper
|
||||
|
||||
|
||||
class MPWrapper(RawInterfaceWrapper):
|
||||
@property
|
||||
def context_mask(self) -> np.ndarray:
|
||||
return np.hstack([
|
||||
[False] * 17,
|
||||
[True] # goal height
|
||||
])
|
||||
|
||||
@property
|
||||
def current_pos(self) -> Union[float, int, np.ndarray]:
|
||||
return self.env.sim.data.qpos[3:9].copy()
|
||||
|
||||
@property
|
||||
def current_vel(self) -> Union[float, int, np.ndarray, Tuple]:
|
||||
return self.env.sim.data.qvel[3:9].copy()
|
||||
@@ -0,0 +1 @@
|
||||
from .mp_wrapper import MPWrapper
|
||||
@@ -0,0 +1,52 @@
|
||||
<mujoco model="hopper">
|
||||
<compiler angle="degree" coordinate="global" inertiafromgeom="true"/>
|
||||
<default>
|
||||
<joint armature="1" damping="1" limited="true"/>
|
||||
<geom conaffinity="1" condim="1" contype="1" margin="0.001" material="geom" rgba="0.8 0.6 .4 1" solimp=".8 .8 .01" solref=".02 1"/>
|
||||
<motor ctrllimited="true" ctrlrange="-.4 .4"/>
|
||||
</default>
|
||||
<option integrator="RK4" timestep="0.002"/>
|
||||
<visual>
|
||||
<map znear="0.02"/>
|
||||
</visual>
|
||||
<worldbody>
|
||||
<light cutoff="100" diffuse="1 1 1" dir="-0 0 -1.3" directional="true" exponent="1" pos="0 0 1.3" specular=".1 .1 .1"/>
|
||||
<geom conaffinity="1" condim="3" name="floor" pos="0 0 0" rgba="0.8 0.9 0.8 1" size="20 20 .125" type="plane" material="MatPlane"/>
|
||||
<body name="torso" pos="0 0 1.25">
|
||||
<camera name="track" mode="trackcom" pos="0 -3 1" xyaxes="1 0 0 0 0 1"/>
|
||||
<joint armature="0" axis="1 0 0" damping="0" limited="false" name="rootx" pos="0 0 0" stiffness="0" type="slide"/>
|
||||
<joint armature="0" axis="0 0 1" damping="0" limited="false" name="rootz" pos="0 0 0" ref="1.25" stiffness="0" type="slide"/>
|
||||
<joint armature="0" axis="0 1 0" damping="0" limited="false" name="rooty" pos="0 0 1.25" stiffness="0" type="hinge"/>
|
||||
<geom friction="0.9" fromto="0 0 1.45 0 0 1.05" name="torso_geom" size="0.05" type="capsule"/>
|
||||
<body name="thigh" pos="0 0 1.05">
|
||||
<joint axis="0 -1 0" name="thigh_joint" pos="0 0 1.05" range="-150 0" type="hinge"/>
|
||||
<geom friction="0.9" fromto="0 0 1.05 0 0 0.6" name="thigh_geom" size="0.05" type="capsule"/>
|
||||
<body name="leg" pos="0 0 0.35">
|
||||
<joint axis="0 -1 0" name="leg_joint" pos="0 0 0.6" range="-150 0" type="hinge"/>
|
||||
<geom friction="0.9" fromto="0 0 0.6 0 0 0.1" name="leg_geom" size="0.04" type="capsule"/>
|
||||
<body name="foot" pos="0.13/2 0 0.1">
|
||||
<site name="foot_site" pos="0 0 0.04" size="0.02 0.02 0.02" rgba="1 0 0 1" type="sphere"/>
|
||||
<joint axis="0 -1 0" name="foot_joint" pos="0 0 0.1" range="-45 45" type="hinge"/>
|
||||
<geom friction="2.0" fromto="-0.13 0 0.1 0.26 0 0.1" name="foot_geom" size="0.06" type="capsule"/>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
<body name="goal_site_body" pos = "0 0 0">
|
||||
<site name="goal_site" pos="0 0 0.0" size="0.02 0.02 0.02" rgba="0 1 0 1" type="sphere"/>
|
||||
</body>
|
||||
</worldbody>
|
||||
<actuator>
|
||||
<motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="200.0" joint="thigh_joint"/>
|
||||
<motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="200.0" joint="leg_joint"/>
|
||||
<motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="200.0" joint="foot_joint"/>
|
||||
</actuator>
|
||||
<asset>
|
||||
<texture type="skybox" builtin="gradient" rgb1=".4 .5 .6" rgb2="0 0 0"
|
||||
width="100" height="100"/>
|
||||
<texture builtin="flat" height="1278" mark="cross" markrgb="1 1 1" name="texgeom" random="0.01" rgb1="0.8 0.6 0.4" rgb2="0.8 0.6 0.4" type="cube" width="127"/>
|
||||
<texture builtin="checker" height="100" name="texplane" rgb1="0 0 0" rgb2="0.8 0.8 0.8" type="2d" width="100"/>
|
||||
<material name="MatPlane" reflectance="0.5" shininess="1" specular="1" texrepeat="60 60" texture="texplane"/>
|
||||
<material name="geom" texture="texgeom" texuniform="true"/>
|
||||
</asset>
|
||||
</mujoco>
|
||||
@@ -0,0 +1,51 @@
|
||||
<mujoco model="hopper">
|
||||
<compiler angle="degree" coordinate="global" inertiafromgeom="true"/>
|
||||
<default>
|
||||
<joint armature="1" damping="1" limited="true"/>
|
||||
<geom conaffinity="1" condim="1" contype="1" margin="0.001" material="geom" rgba="0.8 0.6 .4 1" solimp=".8 .8 .01" solref=".02 1"/>
|
||||
<motor ctrllimited="true" ctrlrange="-.4 .4"/>
|
||||
</default>
|
||||
<option integrator="RK4" timestep="0.002"/>
|
||||
<visual>
|
||||
<map znear="0.02"/>
|
||||
</visual>
|
||||
<worldbody>
|
||||
<light cutoff="100" diffuse="1 1 1" dir="-0 0 -1.3" directional="true" exponent="1" pos="0 0 1.3" specular=".1 .1 .1"/>
|
||||
<geom conaffinity="1" condim="3" name="floor" pos="0 0 0" rgba="0.8 0.9 0.8 1" size="20 20 .125" type="plane" material="MatPlane"/>
|
||||
<body name="torso" pos="0 0 1.25">
|
||||
<camera name="track" mode="trackcom" pos="0 -3 1" xyaxes="1 0 0 0 0 1"/>
|
||||
<joint armature="0" axis="1 0 0" damping="0" limited="false" name="rootx" pos="0 0 0" stiffness="0" type="slide"/>
|
||||
<joint armature="0" axis="0 0 1" damping="0" limited="false" name="rootz" pos="0 0 0" ref="1.25" stiffness="0" type="slide"/>
|
||||
<joint armature="0" axis="0 1 0" damping="0" limited="false" name="rooty" pos="0 0 1.25" stiffness="0" type="hinge"/>
|
||||
<geom friction="0.9" fromto="0 0 1.45 0 0 1.05" name="torso_geom" size="0.05" type="capsule"/>
|
||||
<body name="thigh" pos="0 0 1.05">
|
||||
<joint axis="0 -1 0" name="thigh_joint" pos="0 0 1.05" range="-150 0" type="hinge"/>
|
||||
<geom friction="0.9" fromto="0 0 1.05 0 0 0.6" name="thigh_geom" size="0.05" type="capsule"/>
|
||||
<body name="leg" pos="0 0 0.35">
|
||||
<joint axis="0 -1 0" name="leg_joint" pos="0 0 0.6" range="-150 0" type="hinge"/>
|
||||
<geom friction="0.9" fromto="0 0 0.6 0 0 0.1" name="leg_geom" size="0.04" type="capsule"/>
|
||||
<body name="foot" pos="0.13/2 0 0.1">
|
||||
<joint axis="0 -1 0" name="foot_joint" pos="0 0 0.1" range="-45 45" type="hinge"/>
|
||||
<geom friction="2.0" fromto="-0.13 0 0.1 0.26 0 0.1" name="foot_geom" size="0.06" type="capsule"/>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
<body name="box" pos="1 0 0">
|
||||
<geom friction="1.0" fromto="0.48 0 0 1 0 0" name="basket_ground_geom" size="0.3" type="box" rgba="1 0 0 1"/>
|
||||
</body>
|
||||
</worldbody>
|
||||
<actuator>
|
||||
<motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="200.0" joint="thigh_joint"/>
|
||||
<motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="200.0" joint="leg_joint"/>
|
||||
<motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="200.0" joint="foot_joint"/>
|
||||
</actuator>
|
||||
<asset>
|
||||
<texture type="skybox" builtin="gradient" rgb1=".4 .5 .6" rgb2="0 0 0"
|
||||
width="100" height="100"/>
|
||||
<texture builtin="flat" height="1278" mark="cross" markrgb="1 1 1" name="texgeom" random="0.01" rgb1="0.8 0.6 0.4" rgb2="0.8 0.6 0.4" type="cube" width="127"/>
|
||||
<texture builtin="checker" height="100" name="texplane" rgb1="0 0 0" rgb2="0.8 0.8 0.8" type="2d" width="100"/>
|
||||
<material name="MatPlane" reflectance="0.5" shininess="1" specular="1" texrepeat="60 60" texture="texplane"/>
|
||||
<material name="geom" texture="texgeom" texuniform="true"/>
|
||||
</asset>
|
||||
</mujoco>
|
||||
@@ -0,0 +1,238 @@
|
||||
import copy
|
||||
import os
|
||||
|
||||
import numpy as np
|
||||
from gym.envs.mujoco.hopper_v3 import HopperEnv
|
||||
|
||||
MAX_EPISODE_STEPS_HOPPERJUMP = 250
|
||||
|
||||
|
||||
class HopperJumpEnv(HopperEnv):
|
||||
"""
|
||||
Initialization changes to normal Hopper:
|
||||
- terminate_when_unhealthy: True -> False
|
||||
- healthy_reward: 1.0 -> 2.0
|
||||
- healthy_z_range: (0.7, float('inf')) -> (0.5, float('inf'))
|
||||
- healthy_angle_range: (-0.2, 0.2) -> (-float('inf'), float('inf'))
|
||||
- exclude_current_positions_from_observation: True -> False
|
||||
"""
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
xml_file='hopper_jump.xml',
|
||||
forward_reward_weight=1.0,
|
||||
ctrl_cost_weight=1e-3,
|
||||
healthy_reward=2.0,
|
||||
contact_weight=2.0,
|
||||
height_weight=10.0,
|
||||
dist_weight=3.0,
|
||||
terminate_when_unhealthy=False,
|
||||
healthy_state_range=(-100.0, 100.0),
|
||||
healthy_z_range=(0.5, float('inf')),
|
||||
healthy_angle_range=(-float('inf'), float('inf')),
|
||||
reset_noise_scale=5e-3,
|
||||
exclude_current_positions_from_observation=False,
|
||||
sparse=False,
|
||||
):
|
||||
|
||||
self.sparse = sparse
|
||||
self._height_weight = height_weight
|
||||
self._dist_weight = dist_weight
|
||||
self._contact_weight = contact_weight
|
||||
|
||||
self.max_height = 0
|
||||
self.goal = np.zeros(3, )
|
||||
|
||||
self._steps = 0
|
||||
self.contact_with_floor = False
|
||||
self.init_floor_contact = False
|
||||
self.has_left_floor = False
|
||||
self.contact_dist = None
|
||||
|
||||
xml_file = os.path.join(os.path.dirname(__file__), "assets", xml_file)
|
||||
super().__init__(xml_file, forward_reward_weight, ctrl_cost_weight, healthy_reward, terminate_when_unhealthy,
|
||||
healthy_state_range, healthy_z_range, healthy_angle_range, reset_noise_scale,
|
||||
exclude_current_positions_from_observation)
|
||||
|
||||
# increase initial height
|
||||
self.init_qpos[1] = 1.5
|
||||
|
||||
@property
|
||||
def exclude_current_positions_from_observation(self):
|
||||
return self._exclude_current_positions_from_observation
|
||||
|
||||
def step(self, action):
|
||||
self._steps += 1
|
||||
|
||||
self.do_simulation(action, self.frame_skip)
|
||||
|
||||
height_after = self.get_body_com("torso")[2]
|
||||
site_pos_after = self.data.get_site_xpos('foot_site')
|
||||
self.max_height = max(height_after, self.max_height)
|
||||
|
||||
has_floor_contact = self._is_floor_foot_contact() if not self.contact_with_floor else False
|
||||
|
||||
if not self.init_floor_contact:
|
||||
self.init_floor_contact = has_floor_contact
|
||||
if self.init_floor_contact and not self.has_left_floor:
|
||||
self.has_left_floor = not has_floor_contact
|
||||
if not self.contact_with_floor and self.has_left_floor:
|
||||
self.contact_with_floor = has_floor_contact
|
||||
|
||||
ctrl_cost = self.control_cost(action)
|
||||
costs = ctrl_cost
|
||||
done = False
|
||||
|
||||
goal_dist = np.linalg.norm(site_pos_after - self.goal)
|
||||
if self.contact_dist is None and self.contact_with_floor:
|
||||
self.contact_dist = goal_dist
|
||||
|
||||
rewards = 0
|
||||
if not self.sparse or (self.sparse and self._steps >= MAX_EPISODE_STEPS_HOPPERJUMP):
|
||||
healthy_reward = self.healthy_reward
|
||||
distance_reward = -goal_dist * self._dist_weight
|
||||
height_reward = (self.max_height if self.sparse else self.get_body_com("torso")[2]) * self._height_weight
|
||||
contact_reward = -(self.contact_dist or 5) * self._contact_weight
|
||||
rewards = self._forward_reward_weight * (distance_reward + height_reward + contact_reward + healthy_reward)
|
||||
|
||||
observation = self._get_obs()
|
||||
reward = rewards - costs
|
||||
info = dict(
|
||||
height=height_after,
|
||||
x_pos=site_pos_after,
|
||||
max_height=self.max_height,
|
||||
goal=self.goal[:1],
|
||||
goal_dist=goal_dist,
|
||||
height_rew=self.max_height,
|
||||
healthy_reward=self.healthy_reward * 2,
|
||||
healthy=self.is_healthy,
|
||||
contact_dist=self.contact_dist or 0
|
||||
)
|
||||
return observation, reward, done, info
|
||||
|
||||
def _get_obs(self):
|
||||
goal_dist = self.data.get_site_xpos('foot_site') - self.goal
|
||||
return np.concatenate((super(HopperJumpEnv, self)._get_obs(), goal_dist.copy(), self.goal[:1]))
|
||||
|
||||
def reset_model(self):
|
||||
super(HopperJumpEnv, self).reset_model()
|
||||
|
||||
# self.goal = self.np_random.uniform(0.3, 1.35, 1)[0]
|
||||
self.goal = np.concatenate([self.np_random.uniform(0.3, 1.35, 1), np.zeros(2, )])
|
||||
self.sim.model.body_pos[self.sim.model.body_name2id('goal_site_body')] = self.goal
|
||||
self.max_height = 0
|
||||
self._steps = 0
|
||||
|
||||
noise_low = -np.zeros(self.model.nq)
|
||||
noise_low[3] = -0.5
|
||||
noise_low[4] = -0.2
|
||||
noise_low[5] = 0
|
||||
|
||||
noise_high = np.zeros(self.model.nq)
|
||||
noise_high[3] = 0
|
||||
noise_high[4] = 0
|
||||
noise_high[5] = 0.785
|
||||
|
||||
qpos = (
|
||||
self.np_random.uniform(low=noise_low, high=noise_high, size=self.model.nq) +
|
||||
self.init_qpos
|
||||
)
|
||||
qvel = (
|
||||
# self.np_random.uniform(low=noise_low, high=noise_high, size=self.model.nv) +
|
||||
self.init_qvel
|
||||
)
|
||||
|
||||
self.set_state(qpos, qvel)
|
||||
|
||||
observation = self._get_obs()
|
||||
self.has_left_floor = False
|
||||
self.contact_with_floor = False
|
||||
self.init_floor_contact = False
|
||||
self.contact_dist = None
|
||||
|
||||
return observation
|
||||
|
||||
def _is_floor_foot_contact(self):
|
||||
floor_geom_id = self.model.geom_name2id('floor')
|
||||
foot_geom_id = self.model.geom_name2id('foot_geom')
|
||||
for i in range(self.data.ncon):
|
||||
contact = self.data.contact[i]
|
||||
collision = contact.geom1 == floor_geom_id and contact.geom2 == foot_geom_id
|
||||
collision_trans = contact.geom1 == foot_geom_id and contact.geom2 == floor_geom_id
|
||||
if collision or collision_trans:
|
||||
return True
|
||||
return False
|
||||
|
||||
|
||||
class HopperJumpStepEnv(HopperJumpEnv):
|
||||
|
||||
def __init__(self,
|
||||
xml_file='hopper_jump.xml',
|
||||
forward_reward_weight=1.0,
|
||||
ctrl_cost_weight=1e-3,
|
||||
healthy_reward=1.0,
|
||||
height_weight=3,
|
||||
dist_weight=3,
|
||||
terminate_when_unhealthy=False,
|
||||
healthy_state_range=(-100.0, 100.0),
|
||||
healthy_z_range=(0.5, float('inf')),
|
||||
healthy_angle_range=(-float('inf'), float('inf')),
|
||||
reset_noise_scale=5e-3,
|
||||
exclude_current_positions_from_observation=False
|
||||
):
|
||||
|
||||
self._height_weight = height_weight
|
||||
self._dist_weight = dist_weight
|
||||
super().__init__(xml_file, forward_reward_weight, ctrl_cost_weight, healthy_reward, terminate_when_unhealthy,
|
||||
healthy_state_range, healthy_z_range, healthy_angle_range, reset_noise_scale,
|
||||
exclude_current_positions_from_observation)
|
||||
|
||||
def step(self, action):
|
||||
self._steps += 1
|
||||
|
||||
self.do_simulation(action, self.frame_skip)
|
||||
|
||||
height_after = self.get_body_com("torso")[2]
|
||||
site_pos_after = self.data.get_site_xpos('foot_site')
|
||||
self.max_height = max(height_after, self.max_height)
|
||||
|
||||
ctrl_cost = self.control_cost(action)
|
||||
healthy_reward = self.healthy_reward
|
||||
height_reward = self._height_weight * height_after
|
||||
goal_dist = np.linalg.norm(site_pos_after - np.array([self.goal, 0, 0]))
|
||||
goal_dist_reward = -self._dist_weight * goal_dist
|
||||
dist_reward = self._forward_reward_weight * (goal_dist_reward + height_reward)
|
||||
|
||||
rewards = dist_reward + healthy_reward
|
||||
costs = ctrl_cost
|
||||
done = False
|
||||
|
||||
# This is only for logging the distance to goal when first having the contact
|
||||
has_floor_contact = self._is_floor_foot_contact() if not self.contact_with_floor else False
|
||||
|
||||
if not self.init_floor_contact:
|
||||
self.init_floor_contact = has_floor_contact
|
||||
if self.init_floor_contact and not self.has_left_floor:
|
||||
self.has_left_floor = not has_floor_contact
|
||||
if not self.contact_with_floor and self.has_left_floor:
|
||||
self.contact_with_floor = has_floor_contact
|
||||
|
||||
if self.contact_dist is None and self.contact_with_floor:
|
||||
self.contact_dist = goal_dist
|
||||
|
||||
##############################################################
|
||||
|
||||
observation = self._get_obs()
|
||||
reward = rewards - costs
|
||||
info = {
|
||||
'height': height_after,
|
||||
'x_pos': site_pos_after,
|
||||
'max_height': copy.copy(self.max_height),
|
||||
'goal': copy.copy(self.goal),
|
||||
'goal_dist': goal_dist,
|
||||
'height_rew': height_reward,
|
||||
'healthy_reward': healthy_reward,
|
||||
'healthy': copy.copy(self.is_healthy),
|
||||
'contact_dist': copy.copy(self.contact_dist) or 0
|
||||
}
|
||||
return observation, reward, done, info
|
||||
@@ -0,0 +1,170 @@
|
||||
from gym.envs.mujoco.hopper_v3 import HopperEnv
|
||||
|
||||
import numpy as np
|
||||
import os
|
||||
|
||||
MAX_EPISODE_STEPS_HOPPERJUMPONBOX = 250
|
||||
|
||||
|
||||
class HopperJumpOnBoxEnv(HopperEnv):
|
||||
"""
|
||||
Initialization changes to normal Hopper:
|
||||
- healthy_reward: 1.0 -> 0.01 -> 0.001
|
||||
- healthy_angle_range: (-0.2, 0.2) -> (-float('inf'), float('inf'))
|
||||
"""
|
||||
|
||||
def __init__(self,
|
||||
xml_file='hopper_jump_on_box.xml',
|
||||
forward_reward_weight=1.0,
|
||||
ctrl_cost_weight=1e-3,
|
||||
healthy_reward=0.001,
|
||||
terminate_when_unhealthy=True,
|
||||
healthy_state_range=(-100.0, 100.0),
|
||||
healthy_z_range=(0.7, float('inf')),
|
||||
healthy_angle_range=(-float('inf'), float('inf')),
|
||||
reset_noise_scale=5e-3,
|
||||
context=True,
|
||||
exclude_current_positions_from_observation=True,
|
||||
max_episode_steps=250):
|
||||
self.current_step = 0
|
||||
self.max_height = 0
|
||||
self.max_episode_steps = max_episode_steps
|
||||
self.min_distance = 5000 # what value?
|
||||
self.hopper_on_box = False
|
||||
self.context = context
|
||||
self.box_x = 1
|
||||
xml_file = os.path.join(os.path.dirname(__file__), "assets", xml_file)
|
||||
super().__init__(xml_file, forward_reward_weight, ctrl_cost_weight, healthy_reward, terminate_when_unhealthy,
|
||||
healthy_state_range, healthy_z_range, healthy_angle_range, reset_noise_scale,
|
||||
exclude_current_positions_from_observation)
|
||||
|
||||
def step(self, action):
|
||||
|
||||
self.current_step += 1
|
||||
self.do_simulation(action, self.frame_skip)
|
||||
height_after = self.get_body_com("torso")[2]
|
||||
foot_pos = self.get_body_com("foot")
|
||||
self.max_height = max(height_after, self.max_height)
|
||||
|
||||
vx, vz, vangle = self.sim.data.qvel[0:3]
|
||||
|
||||
s = self.state_vector()
|
||||
fell_over = not (np.isfinite(s).all() and (np.abs(s[2:]) < 100).all() and (height_after > .7))
|
||||
|
||||
box_pos = self.get_body_com("box")
|
||||
box_size = 0.3
|
||||
box_height = 0.3
|
||||
box_center = (box_pos[0] + (box_size / 2), box_pos[1], box_height)
|
||||
foot_length = 0.3
|
||||
foot_center = foot_pos[0] - (foot_length / 2)
|
||||
|
||||
dist = np.linalg.norm(foot_pos - box_center)
|
||||
|
||||
self.min_distance = min(dist, self.min_distance)
|
||||
|
||||
# check if foot is on box
|
||||
is_on_box_x = box_pos[0] <= foot_center <= box_pos[0] + box_size
|
||||
is_on_box_y = True # is y always true because he can only move in x and z direction?
|
||||
is_on_box_z = box_height - 0.02 <= foot_pos[2] <= box_height + 0.02
|
||||
is_on_box = is_on_box_x and is_on_box_y and is_on_box_z
|
||||
if is_on_box: self.hopper_on_box = True
|
||||
|
||||
ctrl_cost = self.control_cost(action)
|
||||
|
||||
costs = ctrl_cost
|
||||
|
||||
done = fell_over or self.hopper_on_box
|
||||
|
||||
if self.current_step >= self.max_episode_steps or done:
|
||||
done = False
|
||||
|
||||
max_height = self.max_height.copy()
|
||||
min_distance = self.min_distance.copy()
|
||||
|
||||
alive_bonus = self._healthy_reward * self.current_step
|
||||
box_bonus = 0
|
||||
rewards = 0
|
||||
|
||||
# TODO explain what we did here for the calculation of the reward
|
||||
if is_on_box:
|
||||
if self.context:
|
||||
rewards -= 100 * vx ** 2 if 100 * vx ** 2 < 1 else 1
|
||||
else:
|
||||
box_bonus = 10
|
||||
rewards += box_bonus
|
||||
# rewards -= dist * dist ???? why when already on box?
|
||||
# reward -= 90 - abs(angle)
|
||||
rewards -= 100 * vx ** 2 if 100 * vx ** 2 < 1 else 1
|
||||
rewards += max_height * 3
|
||||
rewards += alive_bonus
|
||||
|
||||
else:
|
||||
if self.context:
|
||||
rewards = -10 - min_distance
|
||||
rewards += max_height * 3
|
||||
else:
|
||||
# reward -= (dist*dist)
|
||||
rewards -= min_distance * min_distance
|
||||
# rewards -= dist / self.max_distance
|
||||
rewards += max_height
|
||||
rewards += alive_bonus
|
||||
|
||||
else:
|
||||
rewards = 0
|
||||
|
||||
observation = self._get_obs()
|
||||
reward = rewards - costs
|
||||
info = {
|
||||
'height': height_after,
|
||||
'max_height': self.max_height.copy(),
|
||||
'min_distance': self.min_distance,
|
||||
'goal': self.box_x,
|
||||
}
|
||||
|
||||
return observation, reward, done, info
|
||||
|
||||
def _get_obs(self):
|
||||
return np.append(super()._get_obs(), self.box_x)
|
||||
|
||||
def reset(self):
|
||||
|
||||
self.max_height = 0
|
||||
self.min_distance = 5000
|
||||
self.current_step = 0
|
||||
self.hopper_on_box = False
|
||||
if self.context:
|
||||
box_id = self.sim.model.body_name2id("box")
|
||||
self.box_x = self.np_random.uniform(1, 3, 1)
|
||||
self.sim.model.body_pos[box_id] = [self.box_x, 0, 0]
|
||||
return super().reset()
|
||||
|
||||
# overwrite reset_model to make it deterministic
|
||||
def reset_model(self):
|
||||
noise_low = -self._reset_noise_scale
|
||||
noise_high = self._reset_noise_scale
|
||||
|
||||
qpos = self.init_qpos # + self.np_random.uniform(low=noise_low, high=noise_high, size=self.model.nq)
|
||||
qvel = self.init_qvel # + self.np_random.uniform(low=noise_low, high=noise_high, size=self.model.nv)
|
||||
|
||||
self.set_state(qpos, qvel)
|
||||
|
||||
observation = self._get_obs()
|
||||
return observation
|
||||
|
||||
if __name__ == '__main__':
|
||||
render_mode = "human" # "human" or "partial" or "final"
|
||||
env = HopperJumpOnBoxEnv()
|
||||
obs = env.reset()
|
||||
|
||||
for i in range(2000):
|
||||
# objective.load_result("/tmp/cma")
|
||||
# test with random actions
|
||||
ac = env.action_space.sample()
|
||||
obs, rew, d, info = env.step(ac)
|
||||
if i % 10 == 0:
|
||||
env.render(mode=render_mode)
|
||||
if d:
|
||||
print('After ', i, ' steps, done: ', d)
|
||||
env.reset()
|
||||
|
||||
env.close()
|
||||
@@ -0,0 +1,27 @@
|
||||
from typing import Union, Tuple
|
||||
|
||||
import numpy as np
|
||||
|
||||
from alr_envs.black_box.raw_interface_wrapper import RawInterfaceWrapper
|
||||
|
||||
|
||||
class MPWrapper(RawInterfaceWrapper):
|
||||
|
||||
# Random x goal + random init pos
|
||||
@property
|
||||
def context_mask(self):
|
||||
return np.hstack([
|
||||
[False] * (2 + int(not self.exclude_current_positions_from_observation)), # position
|
||||
[True] * 3, # set to true if randomize initial pos
|
||||
[False] * 6, # velocity
|
||||
[True] * 3, # goal distance
|
||||
[True] # goal
|
||||
])
|
||||
|
||||
@property
|
||||
def current_pos(self) -> Union[float, int, np.ndarray, Tuple]:
|
||||
return self.sim.data.qpos[3:6].copy()
|
||||
|
||||
@property
|
||||
def current_vel(self) -> Union[float, int, np.ndarray, Tuple]:
|
||||
return self.sim.data.qvel[3:6].copy()
|
||||
@@ -0,0 +1 @@
|
||||
from .mp_wrapper import MPWrapper
|
||||
@@ -0,0 +1,56 @@
|
||||
<mujoco model="hopper">
|
||||
<compiler angle="degree" coordinate="global" inertiafromgeom="true"/>
|
||||
<default>
|
||||
<joint armature="1" damping="1" limited="true"/>
|
||||
<geom conaffinity="1" condim="1" contype="1" margin="0.001" material="geom" rgba="0.8 0.6 .4 1" solimp=".8 .8 .01" solref=".02 1"/>
|
||||
<motor ctrllimited="true" ctrlrange="-.4 .4"/>
|
||||
</default>
|
||||
<option integrator="RK4" timestep="0.002"/>
|
||||
<visual>
|
||||
<map znear="0.02"/>
|
||||
</visual>
|
||||
<worldbody>
|
||||
<light cutoff="100" diffuse="1 1 1" dir="-0 0 -1.3" directional="true" exponent="1" pos="0 0 1.3" specular=".1 .1 .1"/>
|
||||
<geom conaffinity="1" condim="3" name="floor" pos="0 0 0" rgba="0.8 0.9 0.8 1" size="20 20 .125" type="plane" material="MatPlane"/>
|
||||
<body name="torso" pos="0 0 1.25">
|
||||
<camera name="track" mode="trackcom" pos="0 -3 1" xyaxes="1 0 0 0 0 1"/>
|
||||
<joint armature="0" axis="1 0 0" damping="0" limited="false" name="rootx" pos="0 0 0" stiffness="0" type="slide"/>
|
||||
<joint armature="0" axis="0 0 1" damping="0" limited="false" name="rootz" pos="0 0 0" ref="1.25" stiffness="0" type="slide"/>
|
||||
<joint armature="0" axis="0 1 0" damping="0" limited="false" name="rooty" pos="0 0 1.25" stiffness="0" type="hinge"/>
|
||||
<geom friction="0.9" fromto="0 0 1.45 0 0 1.05" name="torso_geom" size="0.05" type="capsule"/>
|
||||
<body name="thigh" pos="0 0 1.05">
|
||||
<joint axis="0 -1 0" name="thigh_joint" pos="0 0 1.05" range="-150 0" type="hinge"/>
|
||||
<geom friction="0.9" fromto="0 0 1.05 0 0 0.6" name="thigh_geom" size="0.05" type="capsule"/>
|
||||
<body name="leg" pos="0 0 0.35">
|
||||
<joint axis="0 -1 0" name="leg_joint" pos="0 0 0.6" range="-150 0" type="hinge"/>
|
||||
<geom friction="0.9" fromto="0 0 0.6 0 0 0.1" name="leg_geom" size="0.04" type="capsule"/>
|
||||
<body name="foot" pos="0.13/2 0 0.1">
|
||||
<joint axis="0 -1 0" name="foot_joint" pos="0 0 0.1" range="-45 45" type="hinge"/>
|
||||
<geom friction="2.0" fromto="-0.13 0 0.1 0.26 0 0.1" name="foot_geom" size="0.06" type="capsule"/>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
<body name="ball" pos="0 0 1.53">
|
||||
<joint armature="0" axis="1 0 0" damping="0.0" name="tar:x" pos="0 0 1.53" stiffness="0" type="slide" frictionloss="0" limited="false"/>
|
||||
<joint armature="0" axis="0 1 0" damping="0.0" name="tar:y" pos="0 0 1.53" stiffness="0" type="slide" frictionloss="0" limited="false"/>
|
||||
<joint armature="0" axis="0 0 1" damping="0.0" name="tar:z" pos="0 0 1.53" stiffness="0" type="slide" frictionloss="0" limited="false"/>
|
||||
<geom pos="0 0 1.53" 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 1.53" size="0.04 0.04 0.04" rgba="1 0 0 1" type="sphere"/>
|
||||
</body>
|
||||
</worldbody>
|
||||
<actuator>
|
||||
<motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="200.0" joint="thigh_joint"/>
|
||||
<motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="200.0" joint="leg_joint"/>
|
||||
<motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="200.0" joint="foot_joint"/>
|
||||
</actuator>
|
||||
<asset>
|
||||
<texture type="skybox" builtin="gradient" rgb1=".4 .5 .6" rgb2="0 0 0"
|
||||
width="100" height="100"/>
|
||||
<texture builtin="flat" height="1278" mark="cross" markrgb="1 1 1" name="texgeom" random="0.01" rgb1="0.8 0.6 0.4" rgb2="0.8 0.6 0.4" type="cube" width="127"/>
|
||||
<texture builtin="checker" height="100" name="texplane" rgb1="0 0 0" rgb2="0.8 0.8 0.8" type="2d" width="100"/>
|
||||
<material name="MatPlane" reflectance="0.5" shininess="1" specular="1" texrepeat="60 60" texture="texplane"/>
|
||||
<material name="geom" texture="texgeom" texuniform="true"/>
|
||||
</asset>
|
||||
</mujoco>
|
||||
@@ -0,0 +1,132 @@
|
||||
<mujoco model="hopper">
|
||||
<compiler angle="degree" coordinate="global" inertiafromgeom="true"/>
|
||||
<default>
|
||||
<joint armature="1" damping="1" limited="true"/>
|
||||
<geom conaffinity="1" condim="1" contype="1" margin="0.001" material="geom" rgba="0.8 0.6 .4 1" solimp=".8 .8 .01" solref=".02 1"/>
|
||||
<motor ctrllimited="true" ctrlrange="-.4 .4"/>
|
||||
</default>
|
||||
<option integrator="RK4" timestep="0.002"/>
|
||||
<visual>
|
||||
<map znear="0.02"/>
|
||||
</visual>
|
||||
<worldbody>
|
||||
<light cutoff="100" diffuse="1 1 1" dir="-0 0 -1.3" directional="true" exponent="1" pos="0 0 1.3" specular=".1 .1 .1"/>
|
||||
<geom conaffinity="1" condim="3" name="floor" pos="0 0 0" rgba="0.8 0.9 0.8 1" size="20 20 .125" type="plane" material="MatPlane"/>
|
||||
<body name="torso" pos="0 0 1.25">
|
||||
<camera name="track" mode="trackcom" pos="0 -3 1" xyaxes="1 0 0 0 0 1"/>
|
||||
<joint armature="0" axis="1 0 0" damping="0" limited="false" name="rootx" pos="0 0 0" stiffness="0" type="slide"/>
|
||||
<joint armature="0" axis="0 0 1" damping="0" limited="false" name="rootz" pos="0 0 0" ref="1.25" stiffness="0" type="slide"/>
|
||||
<joint armature="0" axis="0 1 0" damping="0" limited="false" name="rooty" pos="0 0 1.25" stiffness="0" type="hinge"/>
|
||||
<geom friction="0.9" fromto="0 0 1.45 0 0 1.05" name="torso_geom" size="0.05" type="capsule"/>
|
||||
<body name="thigh" pos="0 0 1.05">
|
||||
<joint axis="0 -1 0" name="thigh_joint" pos="0 0 1.05" range="-150 0" type="hinge"/>
|
||||
<geom friction="0.9" fromto="0 0 1.05 0 0 0.6" name="thigh_geom" size="0.05" type="capsule"/>
|
||||
<body name="leg" pos="0 0 0.35">
|
||||
<joint axis="0 -1 0" name="leg_joint" pos="0 0 0.6" range="-150 0" type="hinge"/>
|
||||
<geom friction="0.9" fromto="0 0 0.6 0 0 0.1" name="leg_geom" size="0.04" type="capsule"/>
|
||||
<body name="foot" pos="0.13/2 0 0.1">
|
||||
<joint axis="0 -1 0" name="foot_joint" pos="0 0 0.1" range="-45 45" type="hinge"/>
|
||||
<geom friction="2.0" fromto="-0.13 0 0.1 0.26 0 0.1" name="foot_geom" size="0.06" type="capsule"/>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
<body name="ball" pos="0 0 1.53">
|
||||
<joint armature="0" axis="1 0 0" damping="0.0" name="tar:x" pos="0 0 1.53" stiffness="0" type="slide" frictionloss="0" limited="false"/>
|
||||
<joint armature="0" axis="0 1 0" damping="0.0" name="tar:y" pos="0 0 1.53" stiffness="0" type="slide" frictionloss="0" limited="false"/>
|
||||
<joint armature="0" axis="0 0 1" damping="0.0" name="tar:z" pos="0 0 1.53" stiffness="0" type="slide" frictionloss="0" limited="false"/>
|
||||
<geom pos="0 0 1.53" 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 1.53" size="0.04 0.04 0.04" rgba="1 0 0 1" type="sphere"/>
|
||||
</body>
|
||||
<body name="basket_ground" pos="5 0 0">
|
||||
<geom friction="0.9" fromto="5 0 0 5.3 0 0" name="basket_ground_geom" size="0.1 0.4 0.3" type="box"/>
|
||||
<body name="edge1" pos="5 0 0">
|
||||
<geom friction="2.0" fromto="5 0 0 5 0 0.2" name="edge1_geom" size="0.04" type="capsule"/>
|
||||
</body>
|
||||
<body name="edge2" pos="5 0 0.05">
|
||||
<geom friction="2.0" fromto="5 0.05 0 5 0.05 0.2" name="edge2_geom" size="0.04" type="capsule"/>
|
||||
</body>
|
||||
<body name="edge3" pos="5 0 0.1">
|
||||
<geom friction="2.0" fromto="5 0.1 0 5 0.1 0.2" name="edge3_geom" size="0.04" type="capsule"/>
|
||||
</body>
|
||||
<body name="edge4" pos="5 0 0.15">
|
||||
<geom friction="2.0" fromto="5 0.15 0 5 0.15 0.2" name="edge4_geom" size="0.04" type="capsule"/>
|
||||
</body>
|
||||
<body name="edge5" pos="5.05 0 0.15">
|
||||
<geom friction="2.0" fromto="5.05 0.15 0 5.05 0.15 0.2" name="edge5_geom" size="0.04" type="capsule"/>
|
||||
</body>
|
||||
<body name="edge6" pos="5.1 0 0.15">
|
||||
<geom friction="2.0" fromto="5.1 0.15 0 5.1 0.15 0.2" name="edge6_geom" size="0.04" type="capsule"/>
|
||||
</body>
|
||||
<body name="edge7" pos="5.15 0 0.15">
|
||||
<geom friction="2.0" fromto="5.15 0.15 0 5.15 0.15 0.2" name="edge7_geom" size="0.04" type="capsule"/>
|
||||
</body>
|
||||
<body name="edge8" pos="5.2 0 0.15">
|
||||
<geom friction="2.0" fromto="5.2 0.15 0 5.2 0.15 0.2" name="edge8_geom" size="0.04" type="capsule"/>
|
||||
</body>
|
||||
<body name="edge9" pos="5.25 0 0.15">
|
||||
<geom friction="2.0" fromto="5.25 0.15 0 5.25 0.15 0.2" name="edge9_geom" size="0.04" type="capsule"/>
|
||||
</body>
|
||||
<body name="edge10" pos="5.3 0 0.15">
|
||||
<geom friction="2.0" fromto="5.3 0.15 0 5.3 0.15 0.2" name="edge10_geom" size="0.04" type="capsule"/>
|
||||
</body>
|
||||
<body name="edge11" pos="5.3 0 0.1">
|
||||
<geom friction="2.0" fromto="5.3 0.1 0 5.3 0.1 0.2" name="edge11_geom" size="0.04" type="capsule"/>
|
||||
</body>
|
||||
<body name="edge12" pos="5.3 0 0.05">
|
||||
<geom friction="2.0" fromto="5.3 0.05 0 5.3 0.05 0.2" name="edge12_geom" size="0.04" type="capsule"/>
|
||||
</body>
|
||||
<body name="edge13" pos="5.3 0 0.0">
|
||||
<geom friction="2.0" fromto="5.3 0 0 5.3 0 0.2" name="edge13_geom" size="0.04" type="capsule"/>
|
||||
</body>
|
||||
<body name="edge14" pos="5.3 0 -0.05">
|
||||
<geom friction="2.0" fromto="5.3 -0.05 0 5.3 -0.05 0.2" name="edge14_geom" size="0.04" type="capsule"/>
|
||||
</body>
|
||||
<body name="edge15" pos="5.3 0 -0.1">
|
||||
<geom friction="2.0" fromto="5.3 -0.1 0 5.3 -0.1 0.2" name="edge15_geom" size="0.04" type="capsule"/>
|
||||
</body>
|
||||
<body name="edge16" pos="5.3 0 -0.15">
|
||||
<geom friction="2.0" fromto="5.3 -0.15 0 5.3 -0.15 0.2" name="edge16_geom" size="0.04" type="capsule"/>
|
||||
</body>
|
||||
|
||||
<body name="edge20" pos="5.25 0 -0.15">
|
||||
<geom friction="2.0" fromto="5.25 -0.15 0 5.25 -0.15 0.2" name="edge20_geom" size="0.04" type="capsule"/>
|
||||
</body>
|
||||
<body name="edge21" pos="5.2 0 -0.15">
|
||||
<geom friction="2.0" fromto="5.2 -0.15 0 5.2 -0.15 0.2" name="edge21_geom" size="0.04" type="capsule"/>
|
||||
</body>
|
||||
<body name="edge22" pos="5.15 0 -0.15">
|
||||
<geom friction="2.0" fromto="5.15 -0.15 0 5.15 -0.15 0.2" name="edge22_geom" size="0.04" type="capsule"/>
|
||||
</body>
|
||||
<body name="edge23" pos="5.1 0 -0.15">
|
||||
<geom friction="2.0" fromto="5.1 -0.15 0 5.1 -0.15 0.2" name="edge23_geom" size="0.04" type="capsule"/>
|
||||
</body>
|
||||
<body name="edge24" pos="5.05 0 -0.15">
|
||||
<geom friction="2.0" fromto="5.05 -0.15 0 5.05 -0.15 0.2" name="edge24_geom" size="0.04" type="capsule"/>
|
||||
</body>
|
||||
<body name="edge25" pos="5 0 -0.15">
|
||||
<geom friction="2.0" fromto="5 -0.15 0 5 -0.15 0.2" name="edge25_geom" size="0.04" type="capsule"/>
|
||||
</body>
|
||||
<body name="edge26" pos="5 0 -0.1">
|
||||
<geom friction="2.0" fromto="5 -0.1 0 5 -0.1 0.2" name="edge26_geom" size="0.04" type="capsule"/>
|
||||
</body>
|
||||
<body name="edge27" pos="5 0 -0.05">
|
||||
<geom friction="2.0" fromto="5 -0.05 0 5 -0.05 0.2" name="edge27_geom" size="0.04" type="capsule"/>
|
||||
</body>
|
||||
</body>
|
||||
</worldbody>
|
||||
<actuator>
|
||||
<motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="200.0" joint="thigh_joint"/>
|
||||
<motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="200.0" joint="leg_joint"/>
|
||||
<motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="200.0" joint="foot_joint"/>
|
||||
</actuator>
|
||||
<asset>
|
||||
<texture type="skybox" builtin="gradient" rgb1=".4 .5 .6" rgb2="0 0 0"
|
||||
width="100" height="100"/>
|
||||
<texture builtin="flat" height="1278" mark="cross" markrgb="1 1 1" name="texgeom" random="0.01" rgb1="0.8 0.6 0.4" rgb2="0.8 0.6 0.4" type="cube" width="127"/>
|
||||
<texture builtin="checker" height="100" name="texplane" rgb1="0 0 0" rgb2="0.8 0.8 0.8" type="2d" width="100"/>
|
||||
<material name="MatPlane" reflectance="0.5" shininess="1" specular="1" texrepeat="60 60" texture="texplane"/>
|
||||
<material name="geom" texture="texgeom" texuniform="true"/>
|
||||
</asset>
|
||||
</mujoco>
|
||||
@@ -0,0 +1,115 @@
|
||||
import os
|
||||
from typing import Optional
|
||||
|
||||
from gym.envs.mujoco.hopper_v3 import HopperEnv
|
||||
import numpy as np
|
||||
|
||||
MAX_EPISODE_STEPS_HOPPERTHROW = 250
|
||||
|
||||
|
||||
class ALRHopperThrowEnv(HopperEnv):
|
||||
"""
|
||||
Initialization changes to normal Hopper:
|
||||
- healthy_reward: 1.0 -> 0.0 -> 0.1
|
||||
- forward_reward_weight -> 5.0
|
||||
- healthy_angle_range: (-0.2, 0.2) -> (-float('inf'), float('inf'))
|
||||
|
||||
Reward changes to normal Hopper:
|
||||
- velocity: (x_position_after - x_position_before) -> self.get_body_com("ball")[0]
|
||||
"""
|
||||
|
||||
def __init__(self,
|
||||
xml_file='hopper_throw.xml',
|
||||
forward_reward_weight=5.0,
|
||||
ctrl_cost_weight=1e-3,
|
||||
healthy_reward=0.1,
|
||||
terminate_when_unhealthy=True,
|
||||
healthy_state_range=(-100.0, 100.0),
|
||||
healthy_z_range=(0.7, float('inf')),
|
||||
healthy_angle_range=(-float('inf'), float('inf')),
|
||||
reset_noise_scale=5e-3,
|
||||
context=True,
|
||||
exclude_current_positions_from_observation=True,
|
||||
max_episode_steps=250):
|
||||
xml_file = os.path.join(os.path.dirname(__file__), "assets", xml_file)
|
||||
self.current_step = 0
|
||||
self.max_episode_steps = max_episode_steps
|
||||
self.context = context
|
||||
self.goal = 0
|
||||
super().__init__(xml_file, forward_reward_weight, ctrl_cost_weight, healthy_reward, terminate_when_unhealthy,
|
||||
healthy_state_range, healthy_z_range, healthy_angle_range, reset_noise_scale,
|
||||
exclude_current_positions_from_observation)
|
||||
|
||||
def step(self, action):
|
||||
self.current_step += 1
|
||||
self.do_simulation(action, self.frame_skip)
|
||||
ball_pos_after = self.get_body_com("ball")[
|
||||
0] # abs(self.get_body_com("ball")[0]) # use x and y to get point and use euclid distance as reward?
|
||||
ball_pos_after_y = self.get_body_com("ball")[2]
|
||||
|
||||
# done = self.done TODO We should use this, not sure why there is no other termination; ball_landed should be enough, because we only look at the throw itself? - Paul and Marc
|
||||
ball_landed = bool(self.get_body_com("ball")[2] <= 0.05)
|
||||
done = ball_landed
|
||||
|
||||
ctrl_cost = self.control_cost(action)
|
||||
costs = ctrl_cost
|
||||
|
||||
rewards = 0
|
||||
|
||||
if self.current_step >= self.max_episode_steps or done:
|
||||
distance_reward = -np.linalg.norm(ball_pos_after - self.goal) if self.context else \
|
||||
self._forward_reward_weight * ball_pos_after
|
||||
healthy_reward = 0 if self.context else self.healthy_reward * self.current_step
|
||||
|
||||
rewards = distance_reward + healthy_reward
|
||||
|
||||
observation = self._get_obs()
|
||||
reward = rewards - costs
|
||||
info = {
|
||||
'ball_pos': ball_pos_after,
|
||||
'ball_pos_y': ball_pos_after_y,
|
||||
'_steps': self.current_step,
|
||||
'goal': self.goal,
|
||||
}
|
||||
|
||||
return observation, reward, done, info
|
||||
|
||||
def _get_obs(self):
|
||||
return np.append(super()._get_obs(), self.goal)
|
||||
|
||||
def reset(self, *, seed: Optional[int] = None, return_info: bool = False, options: Optional[dict] = None):
|
||||
self.current_step = 0
|
||||
self.goal = self.goal = self.np_random.uniform(2.0, 6.0, 1) # 0.5 8.0
|
||||
return super().reset()
|
||||
|
||||
# overwrite reset_model to make it deterministic
|
||||
def reset_model(self):
|
||||
noise_low = -self._reset_noise_scale
|
||||
noise_high = self._reset_noise_scale
|
||||
|
||||
qpos = self.init_qpos # + self.np_random.uniform(low=noise_low, high=noise_high, size=self.model.nq)
|
||||
qvel = self.init_qvel # + self.np_random.uniform(low=noise_low, high=noise_high, size=self.model.nv)
|
||||
|
||||
self.set_state(qpos, qvel)
|
||||
|
||||
observation = self._get_obs()
|
||||
return observation
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
render_mode = "human" # "human" or "partial" or "final"
|
||||
env = ALRHopperThrowEnv()
|
||||
obs = env.reset()
|
||||
|
||||
for i in range(2000):
|
||||
# objective.load_result("/tmp/cma")
|
||||
# test with random actions
|
||||
ac = env.action_space.sample()
|
||||
obs, rew, d, info = env.step(ac)
|
||||
if i % 10 == 0:
|
||||
env.render(mode=render_mode)
|
||||
if d:
|
||||
print('After ', i, ' steps, done: ', d)
|
||||
env.reset()
|
||||
|
||||
env.close()
|
||||
@@ -0,0 +1,147 @@
|
||||
import os
|
||||
from typing import Optional
|
||||
|
||||
from gym.envs.mujoco.hopper_v3 import HopperEnv
|
||||
|
||||
import numpy as np
|
||||
|
||||
MAX_EPISODE_STEPS_HOPPERTHROWINBASKET = 250
|
||||
|
||||
|
||||
class ALRHopperThrowInBasketEnv(HopperEnv):
|
||||
"""
|
||||
Initialization changes to normal Hopper:
|
||||
- healthy_reward: 1.0 -> 0.0
|
||||
- healthy_angle_range: (-0.2, 0.2) -> (-float('inf'), float('inf'))
|
||||
- hit_basket_reward: - -> 10
|
||||
|
||||
Reward changes to normal Hopper:
|
||||
- velocity: (x_position_after - x_position_before) -> (ball_position_after - ball_position_before)
|
||||
"""
|
||||
|
||||
def __init__(self,
|
||||
xml_file='hopper_throw_in_basket.xml',
|
||||
forward_reward_weight=1.0,
|
||||
ctrl_cost_weight=1e-3,
|
||||
healthy_reward=0.0,
|
||||
hit_basket_reward=10,
|
||||
basket_size=0.3,
|
||||
terminate_when_unhealthy=True,
|
||||
healthy_state_range=(-100.0, 100.0),
|
||||
healthy_z_range=(0.7, float('inf')),
|
||||
healthy_angle_range=(-float('inf'), float('inf')),
|
||||
reset_noise_scale=5e-3,
|
||||
context=True,
|
||||
penalty=0.0,
|
||||
exclude_current_positions_from_observation=True,
|
||||
max_episode_steps=250):
|
||||
self.hit_basket_reward = hit_basket_reward
|
||||
self.current_step = 0
|
||||
self.max_episode_steps = max_episode_steps
|
||||
self.ball_in_basket = False
|
||||
self.basket_size = basket_size
|
||||
self.context = context
|
||||
self.penalty = penalty
|
||||
self.basket_x = 5
|
||||
xml_file = os.path.join(os.path.dirname(__file__), "assets", xml_file)
|
||||
super().__init__(xml_file, forward_reward_weight, ctrl_cost_weight, healthy_reward, terminate_when_unhealthy,
|
||||
healthy_state_range, healthy_z_range, healthy_angle_range, reset_noise_scale,
|
||||
exclude_current_positions_from_observation)
|
||||
|
||||
def step(self, action):
|
||||
|
||||
self.current_step += 1
|
||||
self.do_simulation(action, self.frame_skip)
|
||||
ball_pos = self.get_body_com("ball")
|
||||
|
||||
basket_pos = self.get_body_com("basket_ground")
|
||||
basket_center = (basket_pos[0] + 0.5, basket_pos[1], basket_pos[2])
|
||||
|
||||
is_in_basket_x = ball_pos[0] >= basket_pos[0] and ball_pos[0] <= basket_pos[0] + self.basket_size
|
||||
is_in_basket_y = ball_pos[1] >= basket_pos[1] - (self.basket_size / 2) and ball_pos[1] <= basket_pos[1] + (
|
||||
self.basket_size / 2)
|
||||
is_in_basket_z = ball_pos[2] < 0.1
|
||||
is_in_basket = is_in_basket_x and is_in_basket_y and is_in_basket_z
|
||||
if is_in_basket:
|
||||
self.ball_in_basket = True
|
||||
|
||||
ball_landed = self.get_body_com("ball")[2] <= 0.05
|
||||
done = bool(ball_landed or is_in_basket)
|
||||
|
||||
rewards = 0
|
||||
|
||||
ctrl_cost = self.control_cost(action)
|
||||
|
||||
costs = ctrl_cost
|
||||
|
||||
if self.current_step >= self.max_episode_steps or done:
|
||||
|
||||
if is_in_basket:
|
||||
if not self.context:
|
||||
rewards += self.hit_basket_reward
|
||||
else:
|
||||
dist = np.linalg.norm(ball_pos - basket_center)
|
||||
if self.context:
|
||||
rewards = -10 * dist
|
||||
else:
|
||||
rewards -= (dist * dist)
|
||||
else:
|
||||
# penalty not needed
|
||||
rewards += ((action[
|
||||
:2] > 0) * self.penalty).sum() if self.current_step < 10 else 0 # too much of a penalty?
|
||||
|
||||
observation = self._get_obs()
|
||||
reward = rewards - costs
|
||||
info = {
|
||||
'ball_pos': ball_pos[0],
|
||||
}
|
||||
|
||||
return observation, reward, done, info
|
||||
|
||||
def _get_obs(self):
|
||||
return np.append(super()._get_obs(), self.basket_x)
|
||||
|
||||
def reset(self, *, seed: Optional[int] = None, return_info: bool = False, options: Optional[dict] = None):
|
||||
if self.max_episode_steps == 10:
|
||||
# We have to initialize this here, because the spec is only added after creating the env.
|
||||
self.max_episode_steps = self.spec.max_episode_steps
|
||||
|
||||
self.current_step = 0
|
||||
self.ball_in_basket = False
|
||||
if self.context:
|
||||
basket_id = self.sim.model.body_name2id("basket_ground")
|
||||
self.basket_x = self.np_random.uniform(3, 7, 1)
|
||||
self.sim.model.body_pos[basket_id] = [self.basket_x, 0, 0]
|
||||
return super().reset()
|
||||
|
||||
# overwrite reset_model to make it deterministic
|
||||
def reset_model(self):
|
||||
noise_low = -self._reset_noise_scale
|
||||
noise_high = self._reset_noise_scale
|
||||
|
||||
qpos = self.init_qpos # + self.np_random.uniform(low=noise_low, high=noise_high, size=self.model.nq)
|
||||
qvel = self.init_qvel # + self.np_random.uniform(low=noise_low, high=noise_high, size=self.model.nv)
|
||||
|
||||
self.set_state(qpos, qvel)
|
||||
|
||||
observation = self._get_obs()
|
||||
return observation
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
render_mode = "human" # "human" or "partial" or "final"
|
||||
env = ALRHopperThrowInBasketEnv()
|
||||
obs = env.reset()
|
||||
|
||||
for i in range(2000):
|
||||
# objective.load_result("/tmp/cma")
|
||||
# test with random actions
|
||||
ac = env.action_space.sample()
|
||||
obs, rew, d, info = env.step(ac)
|
||||
if i % 10 == 0:
|
||||
env.render(mode=render_mode)
|
||||
if d:
|
||||
print('After ', i, ' steps, done: ', d)
|
||||
env.reset()
|
||||
|
||||
env.close()
|
||||
@@ -0,0 +1,23 @@
|
||||
from typing import Tuple, Union
|
||||
|
||||
import numpy as np
|
||||
|
||||
from alr_envs.black_box.raw_interface_wrapper import RawInterfaceWrapper
|
||||
|
||||
|
||||
class MPWrapper(RawInterfaceWrapper):
|
||||
|
||||
@property
|
||||
def context_mask(self):
|
||||
return np.hstack([
|
||||
[False] * 17,
|
||||
[True] # goal pos
|
||||
])
|
||||
|
||||
@property
|
||||
def current_pos(self) -> Union[float, int, np.ndarray]:
|
||||
return self.env.data.qpos[3:6].copy()
|
||||
|
||||
@property
|
||||
def current_vel(self) -> Union[float, int, np.ndarray, Tuple]:
|
||||
return self.env.data.qvel[3:6].copy()
|
||||
@@ -0,0 +1 @@
|
||||
from .mp_wrapper import MPWrapper
|
||||
@@ -0,0 +1,57 @@
|
||||
<mujoco model="reacher">
|
||||
<compiler angle="radian" inertiafromgeom="true"/>
|
||||
<default>
|
||||
<joint armature="1" damping="1" limited="true"/>
|
||||
<geom contype="0" friction="1 0.1 0.1" rgba="0.7 0.7 0 1"/>
|
||||
</default>
|
||||
<option gravity="0 0 -9.81" integrator="RK4" timestep="0.01"/>
|
||||
<worldbody>
|
||||
<!-- Arena -->
|
||||
<geom conaffinity="0" contype="0" name="ground" pos="0 0 0" rgba="0.9 0.9 0.9 1" size="1 1 10" type="plane"/>
|
||||
<geom conaffinity="0" fromto="-.6 -.6 .01 .6 -.6 .01" name="sideS" rgba="0.9 0.4 0.6 1" size=".02" type="capsule"/>
|
||||
<geom conaffinity="0" fromto=" .6 -.6 .01 .6 .6 .01" name="sideE" rgba="0.9 0.4 0.6 1" size=".02" type="capsule"/>
|
||||
<geom conaffinity="0" fromto="-.6 .6 .01 .6 .6 .01" name="sideN" rgba="0.9 0.4 0.6 1" size=".02" type="capsule"/>
|
||||
<geom conaffinity="0" fromto="-.6 -.6 .01 -.6 .6 .01" name="sideW" rgba="0.9 0.4 0.6 1" size=".02" type="capsule"/>
|
||||
<!-- Arm -->
|
||||
<geom conaffinity="0" contype="0" fromto="0 0 0 0 0 0.02" name="root" rgba="0.9 0.4 0.6 1" size=".011" type="cylinder"/>
|
||||
<body name="body0" pos="0 0 .01">
|
||||
<geom fromto="0 0 0 0.1 0 0" name="link0" rgba="0.0 0.4 0.6 1" size=".01" type="capsule"/>
|
||||
<joint axis="0 0 1" limited="false" name="joint0" pos="0 0 0" type="hinge"/>
|
||||
<body name="body1" pos="0.1 0 0">
|
||||
<joint axis="0 0 1" limited="false" name="joint1" pos="0 0 0" type="hinge"/>
|
||||
<geom fromto="0 0 0 0.1 0 0" name="link1" rgba="0.0 0.4 0.6 1" size=".01" type="capsule"/>
|
||||
<body name="body2" pos="0.1 0 0">
|
||||
<joint axis="0 0 1" limited="false" name="joint2" pos="0 0 0" type="hinge"/>
|
||||
<geom fromto="0 0 0 0.1 0 0" name="link2" rgba="0.0 0.4 0.6 1" size=".01" type="capsule"/>
|
||||
<body name="body3" pos="0.1 0 0">
|
||||
<joint axis="0 0 1" limited="false" name="joint3" pos="0 0 0" type="hinge"/>
|
||||
<geom fromto="0 0 0 0.1 0 0" name="link3" rgba="0.0 0.4 0.6 1" size=".01" type="capsule"/>
|
||||
<body name="body4" pos="0.1 0 0">
|
||||
<joint axis="0 0 1" limited="true" name="joint4" pos="0 0 0" range="-3.0 3.0" type="hinge"/>
|
||||
<geom fromto="0 0 0 0.1 0 0" name="link4" rgba="0.0 0.4 0.6 1" size=".01" type="capsule"/>
|
||||
<body name="fingertip" pos="0.11 0 0">
|
||||
<geom contype="0" name="fingertip" pos="0 0 0" rgba="0.0 0.8 0.6 1" size=".01" type="sphere"/>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
<!-- Target -->
|
||||
<body name="target" pos=".1 -.1 .01">
|
||||
<!-- <joint armature="0" axis="1 0 0" damping="0" limited="true" name="target_x" pos="0 0 0" range="-.27 .27" ref=".1" stiffness="0" type="slide"/>-->
|
||||
<!-- <joint armature="0" axis="0 1 0" damping="0" limited="true" name="target_y" pos="0 0 0" range="-.27 .27" ref="-.1" stiffness="0" type="slide"/>-->
|
||||
<joint armature="0" axis="1 0 0" damping="0" limited="true" name="target_x" pos="0 0 0" range="-.7 .7" ref=".1" stiffness="0" type="slide"/>
|
||||
<joint armature="0" axis="0 1 0" damping="0" limited="true" name="target_y" pos="0 0 0" range="-.7 .7" ref="-.1" stiffness="0" type="slide"/>
|
||||
<geom conaffinity="0" contype="0" name="target" pos="0 0 0" rgba="0.9 0.2 0.2 1" size=".009" type="sphere"/>
|
||||
</body>
|
||||
<site name="context_space" pos="0 0.0 0.0" euler="0 0 0" size="0.5 0.5 0.01" rgba="0 1 0 0.1" type="box"/>
|
||||
</worldbody>
|
||||
<actuator>
|
||||
<motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="200.0" joint="joint0"/>
|
||||
<motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="200.0" joint="joint1"/>
|
||||
<motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="200.0" joint="joint2"/>
|
||||
<motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="200.0" joint="joint3"/>
|
||||
<motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="200.0" joint="joint4"/>
|
||||
</actuator>
|
||||
</mujoco>
|
||||
@@ -0,0 +1,75 @@
|
||||
<mujoco model="reacher">
|
||||
<compiler angle="radian" inertiafromgeom="true"/>
|
||||
<default>
|
||||
<joint armature="1" damping="1" limited="true"/>
|
||||
<geom contype="0" friction="1 0.1 0.1" rgba="0.7 0.7 0 1"/>
|
||||
</default>
|
||||
<option gravity="0 0 -9.81" integrator="RK4" timestep="0.01"/>
|
||||
<worldbody>
|
||||
<!-- Arena -->
|
||||
<geom conaffinity="0" contype="0" name="ground" pos="0 0 0" rgba="0.9 0.9 0.9 1" size="1 1 10" type="plane"/>
|
||||
<geom conaffinity="0" fromto="-.8 -.8 .01 .8 -.8 .01" name="sideS" rgba="0.9 0.4 0.6 1" size=".02"
|
||||
type="capsule"/>
|
||||
<geom conaffinity="0" fromto=" .8 -.8 .01 .8 .8 .01" name="sideE" rgba="0.9 0.4 0.6 1" size=".02"
|
||||
type="capsule"/>
|
||||
<geom conaffinity="0" fromto="-.8 .8 .01 .8 .8 .01" name="sideN" rgba="0.9 0.4 0.6 1" size=".02"
|
||||
type="capsule"/>
|
||||
<geom conaffinity="0" fromto="-.8 -.8 .01 -.8 .8 .01" name="sideW" rgba="0.9 0.4 0.6 1" size=".02"
|
||||
type="capsule"/>
|
||||
<!-- Arm -->
|
||||
<geom conaffinity="0" contype="0" fromto="0 0 0 0 0 0.02" name="root" rgba="0.9 0.4 0.6 1" size=".011"
|
||||
type="cylinder"/>
|
||||
<body name="body0" pos="0 0 .01">
|
||||
<geom fromto="0 0 0 0.1 0 0" name="link0" rgba="0.0 0.4 0.6 1" size=".01" type="capsule"/>
|
||||
<joint axis="0 0 1" limited="false" name="joint0" pos="0 0 0" type="hinge"/>
|
||||
<body name="body1" pos="0.1 0 0">
|
||||
<joint axis="0 0 1" limited="false" name="joint1" pos="0 0 0" type="hinge"/>
|
||||
<geom fromto="0 0 0 0.1 0 0" name="link1" rgba="0.0 0.4 0.6 1" size=".01" type="capsule"/>
|
||||
<body name="body2" pos="0.1 0 0">
|
||||
<joint axis="0 0 1" limited="false" name="joint2" pos="0 0 0" type="hinge"/>
|
||||
<geom fromto="0 0 0 0.1 0 0" name="link2" rgba="0.0 0.4 0.6 1" size=".01" type="capsule"/>
|
||||
<body name="body3" pos="0.1 0 0">
|
||||
<joint axis="0 0 1" limited="false" name="joint3" pos="0 0 0" type="hinge"/>
|
||||
<geom fromto="0 0 0 0.1 0 0" name="link3" rgba="0.0 0.4 0.6 1" size=".01" type="capsule"/>
|
||||
<body name="body4" pos="0.1 0 0">
|
||||
<joint axis="0 0 1" limited="false" name="joint4" pos="0 0 0" type="hinge"/>
|
||||
<geom fromto="0 0 0 0.1 0 0" name="link4" rgba="0.0 0.4 0.6 1" size=".01" type="capsule"/>
|
||||
<body name="body5" pos="0.1 0 0">
|
||||
<joint axis="0 0 1" limited="false" name="joint5" pos="0 0 0" type="hinge"/>
|
||||
<geom fromto="0 0 0 0.1 0 0" name="link5" rgba="0.0 0.4 0.6 1" size=".01"
|
||||
type="capsule"/>
|
||||
<body name="body6" pos="0.1 0 0">
|
||||
<joint axis="0 0 1" limited="true" name="joint6" pos="0 0 0" range="-3.0 3.0"
|
||||
type="hinge"/>
|
||||
<geom fromto="0 0 0 0.1 0 0" name="link6" rgba="0.0 0.4 0.6 1" size=".01"
|
||||
type="capsule"/>
|
||||
<body name="fingertip" pos="0.11 0 0">
|
||||
<geom contype="0" name="fingertip" pos="0 0 0" rgba="0.0 0.8 0.6 1" size=".01"
|
||||
type="sphere"/>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
<!-- Target -->
|
||||
<body name="target" pos=".1 -.1 .01">
|
||||
<joint armature="0" axis="1 0 0" damping="0" limited="true" name="target_x" pos="0 0 0" range="-.27 .27"
|
||||
ref=".1" stiffness="0" type="slide"/>
|
||||
<joint armature="0" axis="0 1 0" damping="0" limited="true" name="target_y" pos="0 0 0" range="-.27 .27"
|
||||
ref="-.1" stiffness="0" type="slide"/>
|
||||
<geom conaffinity="0" contype="0" name="target" pos="0 0 0" rgba="0.9 0.2 0.2 1" size=".009" type="sphere"/>
|
||||
</body>
|
||||
</worldbody>
|
||||
<actuator>
|
||||
<motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="200.0" joint="joint0"/>
|
||||
<motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="200.0" joint="joint1"/>
|
||||
<motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="200.0" joint="joint2"/>
|
||||
<motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="200.0" joint="joint3"/>
|
||||
<motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="200.0" joint="joint4"/>
|
||||
<motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="200.0" joint="joint5"/>
|
||||
<motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="200.0" joint="joint6"/>
|
||||
</actuator>
|
||||
</mujoco>
|
||||
@@ -0,0 +1,27 @@
|
||||
from typing import Union, Tuple
|
||||
|
||||
import gym
|
||||
import numpy as np
|
||||
|
||||
from alr_envs.black_box.raw_interface_wrapper import RawInterfaceWrapper
|
||||
|
||||
|
||||
class MPWrapper(RawInterfaceWrapper):
|
||||
|
||||
@property
|
||||
def context_mask(self):
|
||||
return np.concatenate([[False] * self.env.n_links, # cos
|
||||
[False] * self.env.n_links, # sin
|
||||
[True] * 2, # goal position
|
||||
[False] * self.env.n_links, # angular velocity
|
||||
[False] * 3, # goal distance
|
||||
# [False], # step
|
||||
])
|
||||
|
||||
@property
|
||||
def current_pos(self) -> Union[float, int, np.ndarray, Tuple]:
|
||||
return self.env.data.qpos.flat[:self.env.n_links]
|
||||
|
||||
@property
|
||||
def current_vel(self) -> Union[float, int, np.ndarray, Tuple]:
|
||||
return self.env.data.qvel.flat[:self.env.n_links]
|
||||
@@ -0,0 +1,105 @@
|
||||
import os
|
||||
|
||||
import numpy as np
|
||||
from gym import utils
|
||||
from gym.envs.mujoco import MujocoEnv
|
||||
|
||||
|
||||
class ReacherEnv(MujocoEnv, utils.EzPickle):
|
||||
"""
|
||||
More general version of the gym mujoco Reacher environment
|
||||
"""
|
||||
|
||||
def __init__(self, sparse: bool = False, n_links: int = 5, ctrl_cost_weight: int = 1):
|
||||
utils.EzPickle.__init__(**locals())
|
||||
|
||||
self._steps = 0
|
||||
self.n_links = n_links
|
||||
|
||||
self.ctrl_cost_weight = ctrl_cost_weight
|
||||
|
||||
self.sparse = sparse
|
||||
self.reward_weight = 1 if not sparse else 200
|
||||
|
||||
file_name = f'reacher_{n_links}links.xml'
|
||||
|
||||
MujocoEnv.__init__(self, os.path.join(os.path.dirname(__file__), "assets", file_name), 2)
|
||||
|
||||
def step(self, action):
|
||||
self._steps += 1
|
||||
|
||||
is_reward = not self.sparse or (self.sparse and self._steps == 200)
|
||||
|
||||
reward_dist = 0.0
|
||||
angular_vel = 0.0
|
||||
if is_reward:
|
||||
reward_dist = self.distance_reward()
|
||||
angular_vel = self.velocity_reward()
|
||||
|
||||
reward_ctrl = -self.ctrl_cost_weight * np.square(action).sum()
|
||||
|
||||
reward = reward_dist + reward_ctrl + angular_vel
|
||||
self.do_simulation(action, self.frame_skip)
|
||||
ob = self._get_obs()
|
||||
done = False
|
||||
|
||||
infos = dict(
|
||||
reward_dist=reward_dist,
|
||||
reward_ctrl=reward_ctrl,
|
||||
velocity=angular_vel,
|
||||
end_effector=self.get_body_com("fingertip").copy(),
|
||||
goal=self.goal if hasattr(self, "goal") else None
|
||||
)
|
||||
|
||||
return ob, reward, done, infos
|
||||
|
||||
def distance_reward(self):
|
||||
vec = self.get_body_com("fingertip") - self.get_body_com("target")
|
||||
return -self.reward_weight * np.linalg.norm(vec)
|
||||
|
||||
def velocity_reward(self):
|
||||
return -10 * np.square(self.data.qvel.flat[:self.n_links]).sum() if self.sparse else 0.0
|
||||
|
||||
def viewer_setup(self):
|
||||
self.viewer.cam.trackbodyid = 0
|
||||
|
||||
def reset_model(self):
|
||||
qpos = (
|
||||
# self.np_random.uniform(low=-0.1, high=0.1, size=self.model.nq) +
|
||||
self.init_qpos.copy()
|
||||
)
|
||||
while True:
|
||||
# full space
|
||||
self.goal = self.np_random.uniform(low=-self.n_links / 10, high=self.n_links / 10, size=2)
|
||||
# I Quadrant
|
||||
# self.goal = self.np_random.uniform(low=0, high=self.n_links / 10, size=2)
|
||||
# II Quadrant
|
||||
# self.goal = np.random.uniform(low=[-self.n_links / 10, 0], high=[0, self.n_links / 10], size=2)
|
||||
# II + III Quadrant
|
||||
# self.goal = np.random.uniform(low=-self.n_links / 10, high=[0, self.n_links / 10], size=2)
|
||||
# I + II Quadrant
|
||||
# self.goal = np.random.uniform(low=[-self.n_links / 10, 0], high=self.n_links, size=2)
|
||||
if np.linalg.norm(self.goal) < self.n_links / 10:
|
||||
break
|
||||
|
||||
qpos[-2:] = self.goal
|
||||
qvel = (
|
||||
# self.np_random.uniform(low=-0.005, high=0.005, size=self.model.nv) +
|
||||
self.init_qvel.copy()
|
||||
)
|
||||
qvel[-2:] = 0
|
||||
self.set_state(qpos, qvel)
|
||||
self._steps = 0
|
||||
|
||||
return self._get_obs()
|
||||
|
||||
def _get_obs(self):
|
||||
theta = self.data.qpos.flat[:self.n_links]
|
||||
target = self.get_body_com("target")
|
||||
return np.concatenate([
|
||||
np.cos(theta),
|
||||
np.sin(theta),
|
||||
target[:2], # x-y of goal position
|
||||
self.data.qvel.flat[:self.n_links], # angular velocity
|
||||
self.get_body_com("fingertip") - target, # goal distance
|
||||
])
|
||||
@@ -0,0 +1 @@
|
||||
from .mp_wrapper import MPWrapper
|
||||
@@ -0,0 +1,64 @@
|
||||
<mujoco model="walker2d">
|
||||
<compiler angle="degree" coordinate="global" inertiafromgeom="true"/>
|
||||
<default>
|
||||
<joint armature="0.01" damping=".1" limited="true"/>
|
||||
<geom conaffinity="0" condim="3" contype="1" density="1000" friction=".7 .1 .1" rgba="0.8 0.6 .4 1"/>
|
||||
</default>
|
||||
<option integrator="RK4" timestep="0.002"/>
|
||||
<worldbody>
|
||||
<light cutoff="100" diffuse="1 1 1" dir="-0 0 -1.3" directional="true" exponent="1" pos="0 0 1.3" specular=".1 .1 .1"/>
|
||||
<geom conaffinity="1" condim="3" name="floor" pos="0 0 0" rgba="0.8 0.9 0.8 1" size="40 40 40" type="plane" material="MatPlane"/>
|
||||
<body name="torso" pos="0 0 1.25">
|
||||
<camera name="track" mode="trackcom" pos="0 -3 1" xyaxes="1 0 0 0 0 1"/>
|
||||
<joint armature="0" axis="1 0 0" damping="0" limited="false" name="rootx" pos="0 0 0" stiffness="0" type="slide"/>
|
||||
<joint armature="0" axis="0 0 1" damping="0" limited="false" name="rootz" pos="0 0 0" ref="1.25" stiffness="0" type="slide"/>
|
||||
<joint armature="0" axis="0 1 0" damping="0" limited="false" name="rooty" pos="0 0 1.25" stiffness="0" type="hinge"/>
|
||||
<geom friction="0.9" fromto="0 0 1.45 0 0 1.05" name="torso_geom" size="0.05" type="capsule"/>
|
||||
<body name="thigh" pos="0 0 1.05">
|
||||
<joint axis="0 -1 0" name="thigh_joint" pos="0 0 1.05" range="-150 0" type="hinge"/>
|
||||
<geom friction="0.9" fromto="0 0 1.05 0 0 0.6" name="thigh_geom" size="0.05" type="capsule"/>
|
||||
<body name="leg" pos="0 0 0.35">
|
||||
<joint axis="0 -1 0" name="leg_joint" pos="0 0 0.6" range="-150 0" type="hinge"/>
|
||||
<geom friction="0.9" fromto="0 0 0.6 0 0 0.1" name="leg_geom" size="0.04" type="capsule"/>
|
||||
<body name="foot" pos="0.2/2 0 0.1">
|
||||
<site name="foot_right_site" pos="0 0 0.04" size="0.02 0.02 0.02" rgba="0 0 1 1" type="sphere"/>
|
||||
<joint axis="0 -1 0" name="foot_joint" pos="0 0 0.1" range="-45 45" type="hinge"/>
|
||||
<geom friction="0.9" fromto="-0.0 0 0.1 0.2 0 0.1" name="foot_geom" size="0.06" type="capsule"/>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
<!-- copied and then replace thigh->thigh_left, leg->leg_left, foot->foot_right -->
|
||||
<body name="thigh_left" pos="0 0 1.05">
|
||||
<joint axis="0 -1 0" name="thigh_left_joint" pos="0 0 1.05" range="-150 0" type="hinge"/>
|
||||
<geom friction="0.9" fromto="0 0 1.05 0 0 0.6" name="thigh_left_geom" rgba=".7 .3 .6 1" size="0.05" type="capsule"/>
|
||||
<body name="leg_left" pos="0 0 0.35">
|
||||
<joint axis="0 -1 0" name="leg_left_joint" pos="0 0 0.6" range="-150 0" type="hinge"/>
|
||||
<geom friction="0.9" fromto="0 0 0.6 0 0 0.1" name="leg_left_geom" rgba=".7 .3 .6 1" size="0.04" type="capsule"/>
|
||||
<body name="foot_left" pos="0.2/2 0 0.1">
|
||||
<site name="foot_left_site" pos="0 0 0.04" size="0.02 0.02 0.02" rgba="1 0 0 1" type="sphere"/>
|
||||
<joint axis="0 -1 0" name="foot_left_joint" pos="0 0 0.1" range="-45 45" type="hinge"/>
|
||||
<geom friction="1.9" fromto="-0.0 0 0.1 0.2 0 0.1" name="foot_left_geom" rgba=".7 .3 .6 1" size="0.06" type="capsule"/>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</worldbody>
|
||||
<actuator>
|
||||
<!-- <motor joint="torso_joint" ctrlrange="-100.0 100.0" isctrllimited="true"/>-->
|
||||
<motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="100" joint="thigh_joint"/>
|
||||
<motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="100" joint="leg_joint"/>
|
||||
<motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="100" joint="foot_joint"/>
|
||||
<motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="100" joint="thigh_left_joint"/>
|
||||
<motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="100" joint="leg_left_joint"/>
|
||||
<motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="100" joint="foot_left_joint"/>
|
||||
<!-- <motor joint="finger2_rot" ctrlrange="-20.0 20.0" isctrllimited="true"/>-->
|
||||
</actuator>
|
||||
<asset>
|
||||
<texture type="skybox" builtin="gradient" rgb1=".4 .5 .6" rgb2="0 0 0"
|
||||
width="100" height="100"/>
|
||||
<texture builtin="flat" height="1278" mark="cross" markrgb="1 1 1" name="texgeom" random="0.01" rgb1="0.8 0.6 0.4" rgb2="0.8 0.6 0.4" type="cube" width="127"/>
|
||||
<texture builtin="checker" height="100" name="texplane" rgb1="0 0 0" rgb2="0.8 0.8 0.8" type="2d" width="100"/>
|
||||
<material name="MatPlane" reflectance="0.5" shininess="1" specular="1" texrepeat="60 60" texture="texplane"/>
|
||||
<material name="geom" texture="texgeom" texuniform="true"/>
|
||||
</asset>
|
||||
</mujoco>
|
||||
@@ -0,0 +1,23 @@
|
||||
from typing import Tuple, Union
|
||||
|
||||
import numpy as np
|
||||
|
||||
from alr_envs.black_box.raw_interface_wrapper import RawInterfaceWrapper
|
||||
|
||||
|
||||
class MPWrapper(RawInterfaceWrapper):
|
||||
|
||||
@property
|
||||
def context_mask(self):
|
||||
return np.hstack([
|
||||
[False] * 17,
|
||||
[True] # goal pos
|
||||
])
|
||||
|
||||
@property
|
||||
def current_pos(self) -> Union[float, int, np.ndarray]:
|
||||
return self.env.data.qpos[3:9].copy()
|
||||
|
||||
@property
|
||||
def current_vel(self) -> Union[float, int, np.ndarray, Tuple]:
|
||||
return self.env.data.qvel[3:9].copy()
|
||||
@@ -0,0 +1,111 @@
|
||||
import os
|
||||
from typing import Optional
|
||||
|
||||
from gym.envs.mujoco.walker2d_v3 import Walker2dEnv
|
||||
import numpy as np
|
||||
|
||||
MAX_EPISODE_STEPS_WALKERJUMP = 300
|
||||
|
||||
|
||||
# TODO: Right now this environment only considers jumping to a specific height, which is not nice. It should be extended
|
||||
# to the same structure as the Hopper, where the angles are randomized (->contexts) and the agent should jump as height
|
||||
# as possible, while landing at a specific target position
|
||||
|
||||
|
||||
class ALRWalker2dJumpEnv(Walker2dEnv):
|
||||
"""
|
||||
healthy reward 1.0 -> 0.005 -> 0.0025 not from alex
|
||||
penalty 10 -> 0 not from alex
|
||||
"""
|
||||
|
||||
def __init__(self,
|
||||
xml_file='walker2d.xml',
|
||||
forward_reward_weight=1.0,
|
||||
ctrl_cost_weight=1e-3,
|
||||
healthy_reward=0.0025,
|
||||
terminate_when_unhealthy=True,
|
||||
healthy_z_range=(0.8, 2.0),
|
||||
healthy_angle_range=(-1.0, 1.0),
|
||||
reset_noise_scale=5e-3,
|
||||
penalty=0,
|
||||
exclude_current_positions_from_observation=True,
|
||||
max_episode_steps=300):
|
||||
self.current_step = 0
|
||||
self.max_episode_steps = max_episode_steps
|
||||
self.max_height = 0
|
||||
self._penalty = penalty
|
||||
self.goal = 0
|
||||
xml_file = os.path.join(os.path.dirname(__file__), "assets", xml_file)
|
||||
super().__init__(xml_file, forward_reward_weight, ctrl_cost_weight, healthy_reward, terminate_when_unhealthy,
|
||||
healthy_z_range, healthy_angle_range, reset_noise_scale,
|
||||
exclude_current_positions_from_observation)
|
||||
|
||||
def step(self, action):
|
||||
self.current_step += 1
|
||||
self.do_simulation(action, self.frame_skip)
|
||||
# pos_after = self.get_body_com("torso")[0]
|
||||
height = self.get_body_com("torso")[2]
|
||||
|
||||
self.max_height = max(height, self.max_height)
|
||||
|
||||
done = bool(height < 0.2)
|
||||
|
||||
ctrl_cost = self.control_cost(action)
|
||||
costs = ctrl_cost
|
||||
rewards = 0
|
||||
if self.current_step >= self.max_episode_steps or done:
|
||||
done = True
|
||||
height_goal_distance = -10 * (np.linalg.norm(self.max_height - self.goal))
|
||||
healthy_reward = self.healthy_reward * self.current_step
|
||||
|
||||
rewards = height_goal_distance + healthy_reward
|
||||
|
||||
observation = self._get_obs()
|
||||
reward = rewards - costs
|
||||
info = {
|
||||
'height': height,
|
||||
'max_height': self.max_height,
|
||||
'goal': self.goal,
|
||||
}
|
||||
|
||||
return observation, reward, done, info
|
||||
|
||||
def _get_obs(self):
|
||||
return np.append(super()._get_obs(), self.goal)
|
||||
|
||||
def reset(self, *, seed: Optional[int] = None, return_info: bool = False, options: Optional[dict] = None):
|
||||
self.current_step = 0
|
||||
self.max_height = 0
|
||||
self.goal = self.np_random.uniform(1.5, 2.5, 1) # 1.5 3.0
|
||||
return super().reset()
|
||||
|
||||
# overwrite reset_model to make it deterministic
|
||||
def reset_model(self):
|
||||
noise_low = -self._reset_noise_scale
|
||||
noise_high = self._reset_noise_scale
|
||||
|
||||
qpos = self.init_qpos # + self.np_random.uniform(low=noise_low, high=noise_high, size=self.model.nq)
|
||||
qvel = self.init_qvel # + self.np_random.uniform(low=noise_low, high=noise_high, size=self.model.nv)
|
||||
|
||||
self.set_state(qpos, qvel)
|
||||
|
||||
observation = self._get_obs()
|
||||
return observation
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
render_mode = "human" # "human" or "partial" or "final"
|
||||
env = ALRWalker2dJumpEnv()
|
||||
obs = env.reset()
|
||||
|
||||
for i in range(6000):
|
||||
# test with random actions
|
||||
ac = env.action_space.sample()
|
||||
obs, rew, d, info = env.step(ac)
|
||||
if i % 10 == 0:
|
||||
env.render(mode=render_mode)
|
||||
if d:
|
||||
print('After ', i, ' steps, done: ', d)
|
||||
env.reset()
|
||||
|
||||
env.close()
|
||||
Reference in New Issue
Block a user