Skip to content

Commit

Permalink
UPGRADE: get_sim is moved inside SimScene. We need to call SimScene.g…
Browse files Browse the repository at this point in the history
…et_sim() now. Also cleaning a bunch of stale calls
  • Loading branch information
vikashplus committed Mar 23, 2023
1 parent 68ec799 commit 61b015f
Show file tree
Hide file tree
Showing 9 changed files with 29 additions and 27 deletions.
6 changes: 3 additions & 3 deletions robohive/envs/env_base.py
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,7 @@
from robohive.utils.prompt_utils import prompt, Prompt
import skvideo.io
from sys import platform
from robohive.physics.sim_scene import get_sim
from robohive.physics.sim_scene import SimScene

# TODO
# remove rwd_mode
Expand Down Expand Up @@ -53,8 +53,8 @@ def __init__(self, model_path, obsd_model_path=None, seed=None, env_credits=DEF
self.seed(seed)

# sims
self.sim = get_sim(model_path)
self.sim_obsd = get_sim(obsd_model_path) if obsd_model_path else self.sim
self.sim = SimScene.get_sim(model_path)
self.sim_obsd = SimScene.get_sim(obsd_model_path) if obsd_model_path else self.sim
self.sim.forward()
self.sim_obsd.forward()
ObsVecDict.__init__(self)
Expand Down
6 changes: 3 additions & 3 deletions robohive/envs/fm/franka_ee_pose_v0.py
Original file line number Diff line number Diff line change
Expand Up @@ -8,7 +8,7 @@
import gym
import numpy as np
from robohive.envs import env_base
from robohive.physics.sim_scene import get_sim
from robohive.physics.sim_scene import SimScene
from robohive.utils.xml_utils import reassign_parent
import os
import collections
Expand All @@ -28,7 +28,7 @@ def __init__(self, model_path, obsd_model_path=None, seed=None, **kwargs):
curr_dir = os.path.dirname(os.path.abspath(__file__))

# Process model to use DManus as end effector
raw_sim = get_sim(curr_dir+model_path)
raw_sim = SimScene.get_sim(curr_dir+model_path)
raw_xml = raw_sim.model.get_xml()
processed_xml = reassign_parent(xml_str=raw_xml, receiver_node="panda0_link7", donor_node="ee_mount")
processed_model_path = curr_dir+model_path[:-4]+"_processed.xml"
Expand All @@ -39,7 +39,7 @@ def __init__(self, model_path, obsd_model_path=None, seed=None, **kwargs):
if obsd_model_path == model_path:
processed_obsd_model_path = processed_model_path
elif obsd_model_path:
raw_sim = get_sim(curr_dir+obsd_model_path)
raw_sim = SimScene.get_sim(curr_dir+obsd_model_path)
raw_xml = raw_sim.model.get_xml()
processed_xml = reassign_parent(xml_str=raw_xml, receiver_node="panda0_link7", donor_node="ee_mount")
processed_obsd_model_path = curr_dir+obsd_model_path[:-4]+"_processed.xml"
Expand Down
6 changes: 3 additions & 3 deletions robohive/envs/fm/franka_robotiq_data_v0.py
Original file line number Diff line number Diff line change
Expand Up @@ -8,7 +8,7 @@
import gym
import numpy as np
from robohive.envs import env_base
from robohive.physics.sim_scene import get_sim
from robohive.physics.sim_scene import SimScene
from robohive.utils.xml_utils import reassign_parent
import os
import collections
Expand All @@ -26,7 +26,7 @@ def __init__(self, model_path, obsd_model_path=None, seed=None, **kwargs):
curr_dir = os.path.dirname(os.path.abspath(__file__))

# Process model to use DManus as end effector
raw_sim = get_sim(curr_dir+model_path)
raw_sim = SimScene.get_sim(curr_dir+model_path)
raw_xml = raw_sim.model.get_xml()
processed_xml = reassign_parent(xml_str=raw_xml, receiver_node="panda0_link7", donor_node="ee_mount")
processed_model_path = curr_dir+model_path[:-4]+"_processed.xml"
Expand All @@ -37,7 +37,7 @@ def __init__(self, model_path, obsd_model_path=None, seed=None, **kwargs):
if obsd_model_path == model_path:
processed_obsd_model_path = processed_model_path
elif obsd_model_path:
raw_sim = get_sim(curr_dir+obsd_model_path)
raw_sim = SimScene.get_sim(curr_dir+obsd_model_path)
raw_xml = raw_sim.model.get_xml()
processed_xml = reassign_parent(xml_str=raw_xml, receiver_node="panda0_link7", donor_node="ee_mount")
processed_obsd_model_path = curr_dir+obsd_model_path[:-4]+"_processed.xml"
Expand Down
1 change: 0 additions & 1 deletion robohive/envs/myo/pen_v0.py
Original file line number Diff line number Diff line change
Expand Up @@ -8,7 +8,6 @@
import gym

from robohive.envs.myo.base_v0 import BaseV0
from robohive.envs.env_base import get_sim
from robohive.utils.quat_math import euler2quat
from robohive.utils.vector_math import calculate_cosine
from os import sendfile
Expand Down
1 change: 0 additions & 1 deletion robohive/envs/myo/pose_v0.py
Original file line number Diff line number Diff line change
Expand Up @@ -8,7 +8,6 @@
import numpy as np

from robohive.envs.myo.base_v0 import BaseV0
from robohive.envs.env_base import get_sim

class PoseEnvV0(BaseV0):

Expand Down
22 changes: 13 additions & 9 deletions robohive/physics/sim_scene.py
Original file line number Diff line number Diff line change
Expand Up @@ -15,15 +15,6 @@
import os
from robohive.renderer.renderer import Renderer

# resolve backend and return the sim
def get_sim(model_handle: Any):
sim_backend = os.getenv('sim_backend')
if sim_backend == 'MUJOCO_PY' or sim_backend == None:
return SimScene.create(model_handle=model_handle, backend=SimBackend.MUJOCO_PY)
elif sim_backend == 'MUJOCO':
return SimScene.create(model_handle=model_handle, backend=SimBackend.DM_CONTROL)
else:
raise ValueError("Unknown sim_backend: {}. Available choices: MUJOCO_PY, MUJOCO")

class SimBackend(enum.Enum):
"""Simulation library types."""
Expand Down Expand Up @@ -56,6 +47,19 @@ def create(*args, backend: Union[SimBackend, int], **kwargs) -> 'SimScene':
else:
raise NotImplementedError(backend)


# resolve backend and return the sim
@staticmethod
def get_sim(model_handle: Any) -> 'SimScene':
sim_backend = os.getenv('sim_backend')
if sim_backend == 'MUJOCO_PY' or sim_backend == None:
return SimScene.create(model_handle=model_handle, backend=SimBackend.MUJOCO_PY)
elif sim_backend == 'MUJOCO':
return SimScene.create(model_handle=model_handle, backend=SimBackend.DM_CONTROL)
else:
raise ValueError("Unknown sim_backend: {}. Available choices: MUJOCO_PY, MUJOCO")


def __init__(
self,
model_handle: Any,
Expand Down
4 changes: 2 additions & 2 deletions robohive/robot/robot.py
Original file line number Diff line number Diff line change
Expand Up @@ -5,7 +5,7 @@
License :: Under Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. You may obtain a copy of the License at http://www.apache.org/licenses/LICENSE-2.0 Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License.
================================================= """

from robohive.physics.sim_scene import get_sim
from robohive.physics.sim_scene import SimScene
from robohive.utils.quat_math import quat2euler
from robohive.utils.prompt_utils import prompt
import time
Expand Down Expand Up @@ -71,7 +71,7 @@ def __init__(self,
if mj_sim is None:
# (creates new robot everytime to facilitate parallelization)
prompt("Preparing robot-sim from %s" % model_path)
self.sim =get_sim(model_path=model_path)
self.sim =SimScene.get_sim(model_path=model_path)
else:
# use provided sim
self.sim = mj_sim
Expand Down
6 changes: 3 additions & 3 deletions robohive/sandbox/dmanus_pickplace_script.py
Original file line number Diff line number Diff line change
Expand Up @@ -11,7 +11,7 @@
- python tutorials/get_ik_minjerk_trajectory.py --sim_path envs/arms/franka/assets/franka_busbin_v0.xml\n
"""

from robohive.physics.sim_scene import get_sim
from robohive.physics.sim_scene import SimScene
from robohive.utils.inverse_kinematics import IKResult, qpos_from_site_pose
from robohive.utils.min_jerk import *
from robohive.utils.quat_math import euler2quat, euler2mat
Expand Down Expand Up @@ -39,15 +39,15 @@ def main(sim_path, horizon, frame_skip):

# Process model to use DManus as end effector
curr_dir = os.path.dirname(os.path.abspath(__file__))
raw_sim = get_sim(curr_dir+sim_path)
raw_sim = SimScene.get_sim(curr_dir+sim_path)
raw_xml = raw_sim.model.get_xml()
processed_xml = reassign_parent(xml_str=raw_xml, receiver_node="panda0_link7", donor_node="ee_mount")
processed_model_path = curr_dir+sim_path[:-4]+"_processed.xml"
with open(processed_model_path, 'w') as file:
file.write(processed_xml)

# Prep
sim = get_sim(processed_model_path)
sim = SimScene.get_sim(processed_model_path)
time_horizon = horizon * frame_skip * sim.model.opt.timestep
dt = sim.model.opt.timestep*frame_skip
os.remove(processed_model_path)
Expand Down
4 changes: 2 additions & 2 deletions robohive/tutorials/ik_minjerk_trajectory.py
Original file line number Diff line number Diff line change
Expand Up @@ -11,7 +11,7 @@
- python tutorials/ik_minjerk_trajectory.py --sim_path envs/arms/franka/assets/franka_busbin_v0.xml\n
"""

from robohive.physics.sim_scene import get_sim
from robohive.physics.sim_scene import SimScene
from robohive.utils.inverse_kinematics import qpos_from_site_pose
from robohive.utils.min_jerk import *
from robohive.utils.quat_math import euler2quat
Expand All @@ -28,7 +28,7 @@
@click.option('-h', '--horizon', type=int, help='time (s) to simulate', default=2)
def main(sim_path, horizon):
# Prep
sim = get_sim(model_handle=sim_path)
sim = SimScene.get_sim(model_handle=sim_path)

# setup
target_sid = sim.model.site_name2id("drop_target")
Expand Down

0 comments on commit 61b015f

Please sign in to comment.