# Copyright (c) 2018-2022, NVIDIA Corporation
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions are met:
#
# 1. Redistributions of source code must retain the above copyright notice, this
# list of conditions and the following disclaimer.
#
# 2. Redistributions in binary form must reproduce the above copyright notice,
# this list of conditions and the following disclaimer in the documentation
# and/or other materials provided with the distribution.
#
# 3. Neither the name of the copyright holder nor the names of its
# contributors may be used to endorse or promote products derived from
# this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
# DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
# FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
# DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
# SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
# OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
import torch
from isaacgym import gymtorch
from rofunc.learning.RofuncRL.tasks.isaacgymenv.ase.humanoid_amp import HumanoidAMP
[docs]class HumanoidASEViewMotionTask(HumanoidAMP):
def __init__(
self,
cfg,
rl_device,
sim_device,
graphics_device_id,
headless,
virtual_screen_capture,
force_render,
):
self.cfg = cfg
control_freq_inv = cfg["env"]["controlFrequencyInv"]
cfg["env"]["controlFrequencyInv"] = 1
cfg["env"]["pdControl"] = False
super().__init__(
cfg=cfg,
rl_device=rl_device,
sim_device=sim_device,
graphics_device_id=graphics_device_id,
headless=headless,
virtual_screen_capture=virtual_screen_capture,
force_render=force_render,
)
self._motion_dt = control_freq_inv * self.sim_params.dt
num_motions = self._motion_lib.num_motions()
self._motion_ids = torch.arange(
self.num_envs, device=self.device, dtype=torch.long
)
self._motion_ids = torch.remainder(self._motion_ids, num_motions)
[docs] def pre_physics_step(self, actions):
self.actions = actions.to(self.device).clone()
# Set the actuation force to zero so that the motion is not affected
# So the action obtaining from the policy is not the real action
forces = torch.zeros_like(self.actions)
force_tensor = gymtorch.unwrap_tensor(forces)
self.gym.set_dof_actuation_force_tensor(self.sim, force_tensor)
return
[docs] def post_physics_step(self):
super().post_physics_step()
self._motion_sync() # Read the real action from the motion data and actuate the robot
return
def _get_humanoid_collision_filter(self):
return 1 # disable self collisions
def _motion_sync(self):
num_motions = self._motion_lib.num_motions()
motion_ids = self._motion_ids
motion_times = self.progress_buf * self._motion_dt
(
root_pos,
root_rot,
dof_pos,
root_vel,
root_ang_vel,
dof_vel,
key_pos,
) = self._motion_lib.get_motion_state(motion_ids, motion_times)
root_vel = torch.zeros_like(root_vel)
root_ang_vel = torch.zeros_like(root_ang_vel)
dof_vel = torch.zeros_like(dof_vel)
env_ids = torch.arange(self.num_envs, dtype=torch.long, device=self.device)
self._set_env_state(
env_ids=env_ids,
root_pos=root_pos,
root_rot=root_rot,
dof_pos=dof_pos,
root_vel=root_vel,
root_ang_vel=root_ang_vel,
dof_vel=dof_vel,
)
env_ids_int32 = self._humanoid_actor_ids[env_ids]
self.gym.set_actor_root_state_tensor_indexed(
self.sim,
gymtorch.unwrap_tensor(self._root_states),
gymtorch.unwrap_tensor(env_ids_int32),
len(env_ids_int32),
)
self.gym.set_dof_state_tensor_indexed(
self.sim,
gymtorch.unwrap_tensor(self._dof_state),
gymtorch.unwrap_tensor(env_ids_int32),
len(env_ids_int32),
)
# TODO use the object pose to let the object move
frame_id = motion_ids.to("cpu").numpy()[0]
object_pose = self._motion_lib.get_object_pose(frame_id)
return
def _compute_reset(self):
motion_lengths = self._motion_lib.get_motion_length(self._motion_ids)
self.reset_buf[:], self._terminate_buf[:] = compute_view_motion_reset(
self.reset_buf, motion_lengths, self.progress_buf, self._motion_dt
)
return
def _reset_actors(self, env_ids):
return
def _reset_env_tensors(self, env_ids):
num_motions = self._motion_lib.num_motions()
self._motion_ids[env_ids] = torch.remainder(
self._motion_ids[env_ids] + self.num_envs, num_motions
)
self.progress_buf[env_ids] = 0
self.reset_buf[env_ids] = 0
self._terminate_buf[env_ids] = 0
return
@torch.jit.script
def compute_view_motion_reset(reset_buf, motion_lengths, progress_buf, dt):
# type: (Tensor, Tensor, Tensor, float) -> Tuple[Tensor, Tensor]
terminated = torch.zeros_like(reset_buf)
motion_times = progress_buf * dt
reset = torch.where(
motion_times > motion_lengths, torch.ones_like(reset_buf), terminated
)
return reset, terminated
