diff --git a/README.md b/README.md
index 5fed5cfce6..89aae0fa9d 100644
--- a/README.md
+++ b/README.md
@@ -4,6 +4,10 @@
[**[Homepage]**](https://robosuite.ai/) [**[White Paper]**](https://arxiv.org/abs/2009.12293) [**[Documentations]**](https://robosuite.ai/docs/overview.html) [**[ARISE Initiative]**](https://github.com/ARISE-Initiative)
+-------
+## Latest Updates
+[12/17/2020] **v1.1.0**: Refactored infrastructure and standardized model classes for much easier environment prototyping :wrench:
+
-------
**robosuite** is a simulation framework powered by the [MuJoCo](http://mujoco.org/) physics engine for robot learning. It also offers a suite of benchmark environments for reproducible research. The current release (v1.0) features manipulation tasks with feature supports of procedural generation, advanced controllers, teleoperation, etc. This project is part of the broader [Advancing Robot Intelligence through Simulated Environments (ARISE) Initiative](https://github.com/ARISE-Initiative), with the aim of lowering the barriers of entry for cutting-edge research at the intersection of AI and Robotics.
diff --git a/docs/references.md b/docs/references.md
index e5dd9fe06a..dddfc53fd7 100644
--- a/docs/references.md
+++ b/docs/references.md
@@ -13,6 +13,8 @@ A list of references of projects and papers that use **robosuite**. If you would
- [Long-Horizon Visual Planning with Goal-Conditioned Hierarchical Predictors](https://arxiv.org/abs/2006.13205). Karl Pertsch, Oleh Rybkin, Frederik Ebert, Chelsea Finn, Dinesh Jayaraman, Sergey Levine
- [Balance Between Efficient and Effective Learning: Dense2Sparse Reward Shaping for Robot Manipulation with Environment Uncertainty](https://arxiv.org/abs/2003.02740). Yongle Luo, Kun Dong, Lili Zhao, Zhiyong Sun, Chao Zhou, Bo Song
- [Hierarchical 6-DoF Grasping with Approaching Direction Selection](http://rllab.snu.ac.kr/publications/papers/2020_icra_gads.pdf). Yunho Choi, Hogun Kee, Kyungjae Lee, JaeGoo Choy, Junhong Min, Sohee Lee, and Songhwai Oh
+- [Conservative Safety Critics for Exploration](https://arxiv.org/abs/2010.14497). Homanga Bharadhwaj, Aviral Kumar, Nicholas Rhinehart, Sergey Levine, Florian Shkurti, Animesh Garg
+- [Deep Reinforcement Learning for Contact-Rich Skills Using Compliant Movement Primitives](https://arxiv.org/abs/2008.13223). Oren Spector, Miriam Zacksenhouse
## Imitation Learning and Batch (Offline) Reinforcement Learning
@@ -21,6 +23,8 @@ A list of references of projects and papers that use **robosuite**. If you would
- [To Follow or not to Follow: Selective Imitation Learning from Observations](https://arxiv.org/abs/1912.07670). Youngwoon Lee, Edward S. Hu, Zhengyu Yang, Joseph J. Lim
- [Learning Robot Skills with Temporal Variational Inference](https://arxiv.org/abs/2006.16232). Tanmay Shankar, Abhinav Gupta
- [Residual Learning from Demonstration](https://arxiv.org/abs/2008.07682). Todor Davchev, Kevin Sebastian Luck, Michael Burke, Franziska Meier, Stefan Schaal, Subramanian Ramamoorthy
+- [Variational Imitation Learning with Diverse-quality Demonstrations](https://proceedings.icml.cc/static/paper_files/icml/2020/577-Paper.pdf). Voot Tangkaratt, Bo Han, Mohammad Emtiyaz Khan, Masashi Sugiyama
+- [Transformers for One-Shot Visual Imitation](https://corlconf.github.io/paper_463/). Sudeep Dasari, Abhinav Gupta
## Benchmarks
diff --git a/robosuite/__init__.py b/robosuite/__init__.py
index cec6df6a44..6b3db31138 100644
--- a/robosuite/__init__.py
+++ b/robosuite/__init__.py
@@ -1,21 +1,22 @@
from robosuite.environments.base import make
-from robosuite.environments.lift import Lift
-from robosuite.environments.stack import Stack
-from robosuite.environments.nut_assembly import NutAssembly
-from robosuite.environments.pick_place import PickPlace
-from robosuite.environments.door import Door
-from robosuite.environments.wipe import Wipe
-from robosuite.environments.two_arm_lift import TwoArmLift
-from robosuite.environments.two_arm_peg_in_hole import TwoArmPegInHole
-from robosuite.environments.two_arm_handover import TwoArmHandover
+# Manipulation environments
+from robosuite.environments.manipulation.lift import Lift
+from robosuite.environments.manipulation.stack import Stack
+from robosuite.environments.manipulation.nut_assembly import NutAssembly
+from robosuite.environments.manipulation.pick_place import PickPlace
+from robosuite.environments.manipulation.door import Door
+from robosuite.environments.manipulation.wipe import Wipe
+from robosuite.environments.manipulation.two_arm_lift import TwoArmLift
+from robosuite.environments.manipulation.two_arm_peg_in_hole import TwoArmPegInHole
+from robosuite.environments.manipulation.two_arm_handover import TwoArmHandover
from robosuite.environments import ALL_ENVIRONMENTS
from robosuite.controllers import ALL_CONTROLLERS, load_controller_config
from robosuite.robots import ALL_ROBOTS
from robosuite.models.grippers import ALL_GRIPPERS
-__version__ = "1.0.0"
+__version__ = "1.1.0"
__logo__ = """
; / ,--.
["] ["] ,< |__**|
diff --git a/robosuite/controllers/joint_pos.py b/robosuite/controllers/joint_pos.py
index 51d59e59b5..e0a2a52c75 100644
--- a/robosuite/controllers/joint_pos.py
+++ b/robosuite/controllers/joint_pos.py
@@ -120,7 +120,7 @@ def __init__(self,
self.output_min = self.nums2array(output_min, self.control_dim)
# limits
- self.position_limits = qpos_limits
+ self.position_limits = np.array(qpos_limits) if qpos_limits is not None else qpos_limits
# kp kd
self.kp = self.nums2array(kp, self.control_dim)
diff --git a/robosuite/controllers/joint_tor.py b/robosuite/controllers/joint_tor.py
index bf2c90046d..cf1e9fc998 100644
--- a/robosuite/controllers/joint_tor.py
+++ b/robosuite/controllers/joint_tor.py
@@ -87,7 +87,7 @@ def __init__(self,
self.output_min = self.nums2array(output_min, self.control_dim)
# limits (if not specified, set them to actuator limits by default)
- self.torque_limits = torque_limits if torque_limits is not None else self.actuator_limits
+ self.torque_limits = np.array(torque_limits) if torque_limits is not None else self.actuator_limits
# control frequency
self.control_freq = policy_freq
diff --git a/robosuite/controllers/joint_vel.py b/robosuite/controllers/joint_vel.py
index b8f440ec39..9598bb329d 100644
--- a/robosuite/controllers/joint_vel.py
+++ b/robosuite/controllers/joint_vel.py
@@ -104,7 +104,7 @@ def __init__(self,
self.last_joint_vel = np.zeros(self.joint_dim)
# limits
- self.velocity_limits = velocity_limits
+ self.velocity_limits = np.array(velocity_limits) if velocity_limits is not None else None
# control frequency
self.control_freq = policy_freq
diff --git a/robosuite/controllers/osc.py b/robosuite/controllers/osc.py
index 7b32435531..45fefe72d1 100644
--- a/robosuite/controllers/osc.py
+++ b/robosuite/controllers/osc.py
@@ -176,8 +176,8 @@ def __init__(self,
self.control_dim += 6
# limits
- self.position_limits = position_limits
- self.orientation_limits = orientation_limits
+ self.position_limits = np.array(position_limits) if position_limits is not None else position_limits
+ self.orientation_limits = np.array(orientation_limits) if orientation_limits is not None else orientation_limits
# control frequency
self.control_freq = policy_freq
@@ -345,6 +345,13 @@ def run_controller(self):
return self.torques
+ def update_initial_joints(self, initial_joints):
+ # First, update from the superclass method
+ super().update_initial_joints(initial_joints)
+
+ # We also need to reset the goal in case the old goals were set to the initial confguration
+ self.reset_goal()
+
def reset_goal(self):
"""
Resets the goal to the current state of the robot
diff --git a/robosuite/demos/demo_device_control.py b/robosuite/demos/demo_device_control.py
index 2290b39365..14aca05190 100644
--- a/robosuite/demos/demo_device_control.py
+++ b/robosuite/demos/demo_device_control.py
@@ -100,6 +100,7 @@
import robosuite as suite
from robosuite import load_controller_config
from robosuite.utils.input_utils import input2action
+from robosuite.wrappers import VisualizationWrapper
@@ -152,12 +153,14 @@
render_camera="agentview",
ignore_done=True,
use_camera_obs=False,
- gripper_visualizations=True,
reward_shaping=True,
control_freq=20,
hard_reset=False,
)
+ # Wrap this environment in a visualization wrapper
+ env = VisualizationWrapper(env, indicator_configs=None)
+
# Setup printing options for numbers
np.set_printoptions(formatter={'float': lambda x: "{0:0.3f}".format(x)})
diff --git a/robosuite/demos/demo_domain_randomization.py b/robosuite/demos/demo_domain_randomization.py
index 5ef0b01f31..3d80ddd58c 100644
--- a/robosuite/demos/demo_domain_randomization.py
+++ b/robosuite/demos/demo_domain_randomization.py
@@ -2,11 +2,14 @@
Script to showcase domain randomization functionality.
"""
-import numpy as np
+import robosuite.utils.macros as macros
from robosuite.controllers import load_controller_config
from robosuite.utils.input_utils import *
from robosuite.wrappers import DomainRandomizationWrapper
+# We'll use instance randomization so that entire geom groups are randomized together
+macros.USING_INSTANCE_RANDOMIZATION = True
+
if __name__ == "__main__":
# Create dict to hold options that will be passed to env creation call
diff --git a/robosuite/demos/demo_gripper_interaction.py b/robosuite/demos/demo_gripper_interaction.py
index 6da16631a4..3a544fd13a 100644
--- a/robosuite/demos/demo_gripper_interaction.py
+++ b/robosuite/demos/demo_gripper_interaction.py
@@ -29,16 +29,21 @@
# add a gripper
gripper = RethinkGripper()
+ # Create another body with a slider joint to which we'll add this gripper
gripper_body = ET.Element("body")
- for body in gripper.worldbody:
- gripper_body.append(body)
gripper_body.set("pos", "0 0 0.3")
gripper_body.set("quat", "0 0 1 0") # flip z
gripper_body.append(
new_joint(name="gripper_z_joint", type="slide", axis="0 0 1", damping="50")
)
- world.merge(gripper, merge_body=False)
+ # Add all gripper bodies to this higher level body
+ for body in gripper.worldbody:
+ gripper_body.append(body)
+ # Merge the all of the gripper tags except its bodies
+ world.merge(gripper, merge_body=None)
+ # Manually add the higher level body we created
world.worldbody.append(gripper_body)
+ # Create a new actuator to control our slider joint
world.actuator.append(
new_actuator(
joint="gripper_z_joint", act_type="position", name="gripper_z", kp="500"
@@ -51,22 +56,19 @@
size=[0.02, 0.02, 0.02],
rgba=[1, 0, 0, 1],
friction=[1, 0.005, 0.0001]
- ).get_collision()
- mujoco_object.append(new_joint(name="object_free_joint", type="free"))
+ ).get_obj()
+ # Set the position of this object
mujoco_object.set("pos", "0 0 0.11")
- geoms = mujoco_object.findall("./geom")
- for geom in geoms:
- if geom.get("contype"):
- pass
- geom.set("name", "object")
- geom.set("density", "10000") # 1000 for water
+ # Add our object to the world body
world.worldbody.append(mujoco_object)
# add reference objects for x and y axes
- x_ref = BoxObject(name="x_ref", size=[0.01, 0.01, 0.01], rgba=[0, 1, 0, 1]).get_visual()
+ x_ref = BoxObject(name="x_ref", size=[0.01, 0.01, 0.01], rgba=[0, 1, 0, 1], obj_type="visual",
+ joints=None).get_obj()
x_ref.set("pos", "0.2 0 0.105")
world.worldbody.append(x_ref)
- y_ref = BoxObject(name="y_ref", size=[0.01, 0.01, 0.01], rgba=[0, 0, 1, 1]).get_visual()
+ y_ref = BoxObject(name="y_ref", size=[0.01, 0.01, 0.01], rgba=[0, 0, 1, 1], obj_type="visual",
+ joints=None).get_obj()
y_ref.set("pos", "0 0.2 0.105")
world.worldbody.append(y_ref)
@@ -83,6 +85,7 @@
sim.model.get_joint_qvel_addr(x) for x in gravity_corrected
]
+ # Set gripper parameters
gripper_z_id = sim.model.actuator_name2id("gripper_z")
gripper_z_low = 0.07
gripper_z_high = -0.02
@@ -102,11 +105,10 @@
step = 0
T = 500
while True:
-
if step % 100 == 0:
print("step: {}".format(step))
- if step % 100 == 0:
+ # Get contact information
for contact in sim.data.contact[0 : sim.data.ncon]:
geom_name1 = sim.model.geom_id2name(contact.geom1)
@@ -119,6 +121,7 @@
print("friction: {}".format(contact.friction))
print("normal: {}".format(contact.frame[0:3]))
+ # Iterate through gripping trajectory
if step % T == 0:
plan = seq[int(step / T) % len(seq)]
gripper_z_is_low, gripper_is_closed = plan
@@ -128,6 +131,7 @@
)
)
+ # Control gripper
if gripper_z_is_low:
sim.data.ctrl[gripper_z_id] = gripper_z_low
else:
@@ -137,6 +141,7 @@
else:
sim.data.ctrl[gripper_jaw_ids] = gripper_open
+ # Step through sim
sim.step()
sim.data.qfrc_applied[_ref_joint_vel_indexes] = sim.data.qfrc_bias[
_ref_joint_vel_indexes
diff --git a/robosuite/demos/demo_video_recording.py b/robosuite/demos/demo_video_recording.py
index 1026c4254d..6c5027aca1 100644
--- a/robosuite/demos/demo_video_recording.py
+++ b/robosuite/demos/demo_video_recording.py
@@ -10,8 +10,12 @@
import imageio
import numpy as np
+import robosuite.utils.macros as macros
from robosuite import make
+# Set the image convention to opencv so that the images are automatically rendered "right side up" when using imageio
+# (which uses opencv convention)
+macros.IMAGE_CONVENTION = "opencv"
if __name__ == "__main__":
@@ -54,7 +58,7 @@
# dump a frame from every K frames
if i % args.skip_frame == 0:
- frame = obs[args.camera + "_image"][::-1]
+ frame = obs[args.camera + "_image"]
writer.append_data(frame)
print("Saving frame #{}".format(i))
diff --git a/robosuite/devices/spacemouse.py b/robosuite/devices/spacemouse.py
index 4ca33b5d62..4bee4592c2 100644
--- a/robosuite/devices/spacemouse.py
+++ b/robosuite/devices/spacemouse.py
@@ -123,6 +123,10 @@ def __init__(self,
print("Manufacturer: %s" % self.device.get_manufacturer_string())
print("Product: %s" % self.device.get_product_string())
+ # 6-DOF variables
+ self.x, self.y, self.z = 0, 0, 0
+ self.roll, self.pitch, self.yaw = 0, 0, 0
+
self._display_controls()
self.single_click_and_hold = False
@@ -164,6 +168,13 @@ def _reset_internal_state(self):
Resets internal state of controller, except for the reset signal.
"""
self.rotation = np.array([[-1., 0., 0.], [0., 1., 0.], [0., 0., -1.]])
+ # Reset 6-DOF variables
+ self.x, self.y, self.z = 0, 0, 0
+ self.roll, self.pitch, self.yaw = 0, 0, 0
+ # Reset control
+ self._control = np.zeros(6)
+ # Reset grasp
+ self.single_click_and_hold = False
def start_control(self):
"""
@@ -183,7 +194,6 @@ def get_controller_state(self):
"""
dpos = self.control[:3] * 0.005 * self.pos_sensitivity
roll, pitch, yaw = self.control[3:] * 0.005 * self.rot_sensitivity
- self.grasp = self.control_gripper
# convert RPY to an absolute orientation
drot1 = rotation_matrix(angle=-pitch, direction=[1., 0, 0], point=None)[:3, :3]
@@ -196,7 +206,7 @@ def get_controller_state(self):
dpos=dpos,
rotation=self.rotation,
raw_drotation=np.array([roll, pitch, yaw]),
- grasp=self.grasp,
+ grasp=self.control_gripper,
reset=self._reset_state
)
diff --git a/robosuite/environments/base.py b/robosuite/environments/base.py
index 71628e1ba9..abc643bb3f 100644
--- a/robosuite/environments/base.py
+++ b/robosuite/environments/base.py
@@ -3,6 +3,9 @@
from mujoco_py import load_model_from_xml
from robosuite.utils import SimulationError, XMLError, MujocoPyRenderer
+from robosuite.models.base import MujocoModel
+
+import numpy as np
REGISTERED_ENVS = {}
@@ -44,7 +47,7 @@ def __new__(meta, name, bases, class_dict):
cls = super().__new__(meta, name, bases, class_dict)
# List all environments that should not be registered here.
- _unregistered_envs = ["MujocoEnv", "RobotEnv"]
+ _unregistered_envs = ["MujocoEnv", "RobotEnv", "ManipulationEnv", "SingleArmEnv", "TwoArmEnv"]
if cls.__name__ not in _unregistered_envs:
register_env(cls)
@@ -71,6 +74,10 @@ class MujocoEnv(metaclass=EnvMeta):
render_visual_mesh (bool): True if rendering visual meshes
in camera. False otherwise.
+ render_gpu_device_id (int): corresponds to the GPU device id to use for offscreen rendering.
+ Defaults to -1, in which case the device will be inferred from environment variables
+ (GPUS or CUDA_VISIBLE_DEVICES).
+
control_freq (float): how many control signals to receive
in every simulated second. This sets the amount of simulation time
that passes between every action input.
@@ -93,7 +100,8 @@ def __init__(
render_camera="frontview",
render_collision_mesh=False,
render_visual_mesh=True,
- control_freq=10,
+ render_gpu_device_id=-1,
+ control_freq=20,
horizon=1000,
ignore_done=False,
hard_reset=True
@@ -108,6 +116,7 @@ def __init__(
self.render_camera = render_camera
self.render_collision_mesh = render_collision_mesh
self.render_visual_mesh = render_visual_mesh
+ self.render_gpu_device_id = render_gpu_device_id
self.viewer = None
# Simulation-specific attributes
@@ -115,6 +124,7 @@ def __init__(
self.horizon = horizon
self.ignore_done = ignore_done
self.hard_reset = hard_reset
+ self._model_postprocessor = None # Function to post-process model after load_model() call
self.model = None
self.cur_time = None
self.model_timestep = None
@@ -124,6 +134,9 @@ def __init__(
# Load the model
self._load_model()
+ # Post-process model
+ self._postprocess_model()
+
# Initialize the simulation
self._initialize_sim()
@@ -148,10 +161,28 @@ def initialize_time(self, control_freq):
)
self.control_timestep = 1. / control_freq
+ def set_model_postprocessor(self, postprocessor):
+ """
+ Sets the post-processor function that self.model will be passed to after load_model() is called during resets.
+
+ Args:
+ postprocessor (None or function): If set, postprocessing method should take in a Task-based instance and
+ return no arguments.
+ """
+ self._model_postprocessor = postprocessor
+
def _load_model(self):
"""Loads an xml model, puts it in self.model"""
pass
+ def _postprocess_model(self):
+ """
+ Post-processes model after load_model() call. Useful for external objects (e.g.: wrappers) to
+ be able to modify the sim model before it is actually loaded into the simulation
+ """
+ if self._model_postprocessor is not None:
+ self._model_postprocessor(self.model)
+
def _get_reference(self):
"""
Sets up references to important components. A reference is typically an
@@ -173,7 +204,7 @@ def _initialize_sim(self, xml_string=None):
# Create the simulation instance and run a single step to make sure changes have propagated through sim state
self.sim = MjSim(self.mjpy_model)
- self.sim.step()
+ self.sim.forward()
# Setup sim time based on control frequency
self.initialize_time(self.control_freq)
@@ -190,6 +221,7 @@ def reset(self):
if self.hard_reset and not self.deterministic_reset:
self._destroy_viewer()
self._load_model()
+ self._postprocess_model()
self._initialize_sim()
# Else, we only reset the sim internally
else:
@@ -197,6 +229,9 @@ def reset(self):
# Reset necessary robosuite-centric variables
self._reset_internal()
self.sim.forward()
+ # Make sure that all sites are toggled OFF by default
+ self.visualize(vis_settings={vis: False for vis in self._visualizations})
+ # Return new observations
return self._get_observation()
def _reset_internal(self):
@@ -221,7 +256,7 @@ def _reset_internal(self):
elif self.has_offscreen_renderer:
if self.sim._render_context_offscreen is None:
- render_context = MjRenderContextOffscreen(self.sim)
+ render_context = MjRenderContextOffscreen(self.sim, device_id=self.render_gpu_device_id)
self.sim.add_render_context(render_context)
self.sim._render_context_offscreen.vopt.geomgroup[0] = (1 if self.render_collision_mesh else 0)
self.sim._render_context_offscreen.vopt.geomgroup[1] = (1 if self.render_visual_mesh else 0)
@@ -316,6 +351,7 @@ def _post_action(self, action):
# done if number of elapsed timesteps is greater than horizon
self.done = (self.timestep >= self.horizon) and not self.ignore_done
+
return reward, self.done, {}
def reward(self, action):
@@ -351,6 +387,43 @@ def observation_spec(self):
observation = self._get_observation()
return observation
+ def clear_objects(self, object_names):
+ """
+ Clears objects with the name @object_names out of the task space. This is useful
+ for supporting task modes with single types of objects, as in
+ @self.single_object_mode without changing the model definition.
+
+ Args:
+ object_names (str or list of str): Name of object(s) to remove from the task workspace
+ """
+ object_names = {object_names} if type(object_names) is str else set(object_names)
+ for obj in self.model.mujoco_objects:
+ if obj.name in object_names:
+ self.sim.data.set_joint_qpos(obj.joints[0], np.array((10, 10, 10, 1, 0, 0, 0)))
+
+ def visualize(self, vis_settings):
+ """
+ Do any needed visualization here
+
+ Args:
+ vis_settings (dict): Visualization keywords mapped to T/F, determining whether that specific
+ component should be visualized. Should have "env" keyword as well as any other relevant
+ options specified.
+ """
+ # Set visuals for environment objects
+ for obj in self.model.mujoco_objects:
+ obj.set_sites_visibility(sim=self.sim, visible=vis_settings["env"])
+
+ @property
+ def _visualizations(self):
+ """
+ Visualization keywords for this environment
+
+ Returns:
+ set: All components that can be individually visualized for this environment
+ """
+ return {"env"}
+
@property
def action_spec(self):
"""
@@ -365,6 +438,7 @@ def action_spec(self):
def action_dim(self):
"""
Size of the action space
+
Returns:
int: Action space dimension
"""
@@ -393,26 +467,66 @@ def reset_from_xml_string(self, xml_string):
# Turn off deterministic reset
self.deterministic_reset = False
- def find_contacts(self, geoms_1, geoms_2):
+ def check_contact(self, geoms_1, geoms_2=None):
"""
Finds contact between two geom groups.
Args:
- geoms_1 (list of str): a list of geom names
- geoms_2 (list of str): another list of geom names
+ geoms_1 (str or list of str or MujocoModel): an individual geom name or list of geom names or a model. If
+ a MujocoModel is specified, the geoms checked will be its contact_geoms
+ geoms_2 (str or list of str or MujocoModel or None): another individual geom name or list of geom names.
+ If a MujocoModel is specified, the geoms checked will be its contact_geoms. If None, will check
+ any collision with @geoms_1 to any other geom in the environment
Returns:
- generator: iterator of all contacts between @geoms_1 and @geoms_2
- """
- for contact in self.sim.data.contact[0 : self.sim.data.ncon]:
+ bool: True if any geom in @geoms_1 is in contact with any geom in @geoms_2.
+ """
+ # Check if either geoms_1 or geoms_2 is a string, convert to list if so
+ if type(geoms_1) is str:
+ geoms_1 = [geoms_1]
+ elif isinstance(geoms_1, MujocoModel):
+ geoms_1 = geoms_1.contact_geoms
+ if type(geoms_2) is str:
+ geoms_2 = [geoms_2]
+ elif isinstance(geoms_2, MujocoModel):
+ geoms_2 = geoms_2.contact_geoms
+ for contact in self.sim.data.contact[: self.sim.data.ncon]:
# check contact geom in geoms
c1_in_g1 = self.sim.model.geom_id2name(contact.geom1) in geoms_1
- c2_in_g2 = self.sim.model.geom_id2name(contact.geom2) in geoms_2
+ c2_in_g2 = self.sim.model.geom_id2name(contact.geom2) in geoms_2 if geoms_2 is not None else True
# check contact geom in geoms (flipped)
c2_in_g1 = self.sim.model.geom_id2name(contact.geom2) in geoms_1
- c1_in_g2 = self.sim.model.geom_id2name(contact.geom1) in geoms_2
+ c1_in_g2 = self.sim.model.geom_id2name(contact.geom1) in geoms_2 if geoms_2 is not None else True
if (c1_in_g1 and c2_in_g2) or (c1_in_g2 and c2_in_g1):
- yield contact
+ return True
+ return False
+
+ def get_contacts(self, model):
+ """
+ Checks for any contacts with @model (as defined by @model's contact_geoms) and returns the set of
+ geom names currently in contact with that model (excluding the geoms that are part of the model itself).
+
+ Args:
+ model (MujocoModel): Model to check contacts for.
+
+ Returns:
+ set: Unique geoms that are actively in contact with this model.
+
+ Raises:
+ AssertionError: [Invalid input type]
+ """
+ # Make sure model is MujocoModel type
+ assert isinstance(model, MujocoModel), \
+ "Inputted model must be of type MujocoModel; got type {} instead!".format(type(model))
+ contact_set = set()
+ for contact in self.sim.data.contact[: self.sim.data.ncon]:
+ # check contact geom in geoms; add to contact set if match is found
+ g1, g2 = self.sim.model.geom_id2name(contact.geom1), self.sim.model.geom_id2name(contact.geom2)
+ if g1 in model.contact_geoms and g2 not in model.contact_geoms:
+ contact_set.add(g2)
+ elif g2 in model.contact_geoms and g1 not in model.contact_geoms:
+ contact_set.add(g1)
+ return contact_set
def _check_success(self):
"""
diff --git a/robosuite/environments/door.py b/robosuite/environments/manipulation/door.py
similarity index 76%
rename from robosuite/environments/door.py
rename to robosuite/environments/manipulation/door.py
index 44588be062..4465e4a2cc 100644
--- a/robosuite/environments/door.py
+++ b/robosuite/environments/manipulation/door.py
@@ -1,15 +1,15 @@
from collections import OrderedDict
import numpy as np
-from robosuite.environments.robot_env import RobotEnv
-from robosuite.robots import SingleArm
+from robosuite.environments.manipulation.single_arm_env import SingleArmEnv
from robosuite.models.arenas import TableArena
from robosuite.models.objects import DoorObject
-from robosuite.models.tasks import ManipulationTask, UniformRandomSampler
+from robosuite.models.tasks import ManipulationTask
+from robosuite.utils.placement_samplers import UniformRandomSampler
-class Door(RobotEnv):
+class Door(SingleArmEnv):
"""
This class corresponds to the door opening task for a single robot arm.
@@ -18,6 +18,9 @@ class Door(RobotEnv):
(e.g: "Sawyer" would generate one arm; ["Panda", "Panda", "Sawyer"] would generate three robot arms)
Note: Must be a single single-arm robot!
+ env_configuration (str): Specifies how to position the robots within the environment (default is "default").
+ For most single arm environments, this argument has no impact on the robot setup.
+
controller_configs (str or list of dict): If set, contains relevant controller parameters for creating a
custom controller. Else, uses the default controller for this specific task. Should either be single
dict if same controller is to be used for all robots or else it should be a list of the same length as
@@ -29,10 +32,6 @@ class Door(RobotEnv):
overrides the default gripper. Should either be single str if same gripper type is to be used for all
robots or else it should be a list of the same length as "robots" param
- gripper_visualizations (bool or list of bool): True if using gripper visualization.
- Useful for teleoperation. Should either be single bool if gripper visualization is to be used for all
- robots or else it should be a list of the same length as "robots" param
-
initialization_noise (dict or list of dict): Dict containing the initialization noise parameters.
The expected keys and corresponding value types are specified below:
@@ -61,13 +60,10 @@ class Door(RobotEnv):
reward_shaping (bool): if True, use dense rewards.
- placement_initializer (ObjectPositionSampler instance): if provided, will
+ placement_initializer (ObjectPositionSampler): if provided, will
be used to place objects on every reset, else a UniformRandomSampler
is used by default.
- use_indicator_object (bool): if True, sets up an indicator object that
- is useful for debugging.
-
has_renderer (bool): If true, render the simulation state in
a viewer instead of headless mode.
@@ -81,6 +77,10 @@ class Door(RobotEnv):
render_visual_mesh (bool): True if rendering visual meshes in camera. False otherwise.
+ render_gpu_device_id (int): corresponds to the GPU device id to use for offscreen rendering.
+ Defaults to -1, in which case the device will be inferred from environment variables
+ (GPUS or CUDA_VISIBLE_DEVICES).
+
control_freq (float): how many control signals to receive in every second. This sets the amount of
simulation time that passes between every action input.
@@ -119,9 +119,9 @@ class Door(RobotEnv):
def __init__(
self,
robots,
+ env_configuration="default",
controller_configs=None,
gripper_types="default",
- gripper_visualizations=False,
initialization_noise="default",
use_latch=True,
use_camera_obs=True,
@@ -129,13 +129,13 @@ def __init__(
reward_scale=1.0,
reward_shaping=False,
placement_initializer=None,
- use_indicator_object=False,
has_renderer=False,
has_offscreen_renderer=True,
render_camera="frontview",
render_collision_mesh=False,
render_visual_mesh=True,
- control_freq=10,
+ render_gpu_device_id=-1,
+ control_freq=20,
horizon=1000,
ignore_done=False,
hard_reset=True,
@@ -144,9 +144,6 @@ def __init__(
camera_widths=256,
camera_depths=False,
):
- # First, verify that only one robot is being inputted
- self._check_robot_configuration(robots)
-
# settings for table top (hardcoded since it's not an essential part of the environment)
self.table_full_size = (0.8, 0.3, 0.05)
self.table_offset = (-0.2, -0.35, 0.8)
@@ -160,30 +157,22 @@ def __init__(
self.use_object_obs = use_object_obs
# object placement initializer
- if placement_initializer:
- self.placement_initializer = placement_initializer
- else:
- self.placement_initializer = UniformRandomSampler(
- x_range=[0.07, 0.09],
- y_range=[-0.01, 0.01],
- ensure_object_boundary_in_range=False,
- rotation=(-np.pi / 2. - 0.25, -np.pi / 2.),
- rotation_axis='z',
- )
+ self.placement_initializer = placement_initializer
super().__init__(
robots=robots,
+ env_configuration=env_configuration,
controller_configs=controller_configs,
+ mount_types="default",
gripper_types=gripper_types,
- gripper_visualizations=gripper_visualizations,
initialization_noise=initialization_noise,
use_camera_obs=use_camera_obs,
- use_indicator_object=use_indicator_object,
has_renderer=has_renderer,
has_offscreen_renderer=has_offscreen_renderer,
render_camera=render_camera,
render_collision_mesh=render_collision_mesh,
render_visual_mesh=render_visual_mesh,
+ render_gpu_device_id=render_gpu_device_id,
control_freq=control_freq,
horizon=horizon,
ignore_done=ignore_done,
@@ -249,45 +238,57 @@ def _load_model(self):
"""
super()._load_model()
- # Verify the correct robot has been loaded
- assert isinstance(self.robots[0], SingleArm), \
- "Error: Expected one single-armed robot! Got {} type instead.".format(type(self.robots[0]))
-
# Adjust base pose accordingly
xpos = self.robots[0].robot_model.base_xpos_offset["table"](self.table_full_size[0])
self.robots[0].robot_model.set_base_xpos(xpos)
# load model for table top workspace
- self.mujoco_arena = TableArena(
+ mujoco_arena = TableArena(
table_full_size=self.table_full_size,
table_offset=self.table_offset,
)
- if self.use_indicator_object:
- self.mujoco_arena.add_pos_indicator()
# Arena always gets set to zero origin
- self.mujoco_arena.set_origin([0, 0, 0])
+ mujoco_arena.set_origin([0, 0, 0])
+
+ # Modify default agentview camera
+ mujoco_arena.set_camera(
+ camera_name="agentview",
+ pos=[0.5986131746834771, -4.392035683362857e-09, 1.5903500240372423],
+ quat=[0.6380177736282349, 0.3048497438430786, 0.30484986305236816, 0.6380177736282349]
+ )
# initialize objects of interest
- door = DoorObject(
+ self.door = DoorObject(
name="Door",
friction=0.0,
damping=0.1,
lock=self.use_latch,
- joints=[], # ensures that door object does not have a free joint
)
- self.mujoco_objects = OrderedDict([("Door", door)])
- self.n_objects = len(self.mujoco_objects)
+
+ # Create placement initializer
+ if self.placement_initializer is not None:
+ self.placement_initializer.reset()
+ self.placement_initializer.add_objects(self.door)
+ else:
+ self.placement_initializer = UniformRandomSampler(
+ name="ObjectSampler",
+ mujoco_objects=self.door,
+ x_range=[0.07, 0.09],
+ y_range=[-0.01, 0.01],
+ rotation=(-np.pi / 2. - 0.25, -np.pi / 2.),
+ rotation_axis='z',
+ ensure_object_boundary_in_range=False,
+ ensure_valid_placement=True,
+ reference_pos=self.table_offset,
+ )
# task includes arena, robot, and objects of interest
self.model = ManipulationTask(
- mujoco_arena=self.mujoco_arena,
+ mujoco_arena=mujoco_arena,
mujoco_robots=[robot.robot_model for robot in self.robots],
- mujoco_objects=self.mujoco_objects,
- visual_objects=None,
- initializer=self.placement_initializer,
+ mujoco_objects=self.door,
)
- self.model.place_objects()
def _get_reference(self):
"""
@@ -299,20 +300,13 @@ def _get_reference(self):
# Additional object references from this env
self.object_body_ids = dict()
- self.object_body_ids["door"] = self.sim.model.body_name2id("door")
- self.object_body_ids["frame"] = self.sim.model.body_name2id("frame")
- self.object_body_ids["latch"] = self.sim.model.body_name2id("latch")
- self.door_handle_site_id = self.sim.model.site_name2id("door_handle")
- self.hinge_qpos_addr = self.sim.model.get_joint_qpos_addr("door_hinge")
+ self.object_body_ids["door"] = self.sim.model.body_name2id(self.door.door_body)
+ self.object_body_ids["frame"] = self.sim.model.body_name2id(self.door.frame_body)
+ self.object_body_ids["latch"] = self.sim.model.body_name2id(self.door.latch_body)
+ self.door_handle_site_id = self.sim.model.site_name2id(self.door.important_sites["handle"])
+ self.hinge_qpos_addr = self.sim.model.get_joint_qpos_addr(self.door.joints[0])
if self.use_latch:
- self.handle_qpos_addr = self.sim.model.get_joint_qpos_addr("latch_joint")
-
- self.l_finger_geom_ids = [
- self.sim.model.geom_name2id(x) for x in self.robots[0].gripper.important_geoms["left_finger"]
- ]
- self.r_finger_geom_ids = [
- self.sim.model.geom_name2id(x) for x in self.robots[0].gripper.important_geoms["right_finger"]
- ]
+ self.handle_qpos_addr = self.sim.model.get_joint_qpos_addr(self.door.joints[1])
def _reset_internal(self):
"""
@@ -323,11 +317,14 @@ def _reset_internal(self):
# Reset all object positions using initializer sampler if we're not directly loading from an xml
if not self.deterministic_reset:
- # Sample from the placement initializer for the Door object
- door_pos, door_quat = self.model.place_objects()
- door_body_id = self.sim.model.body_name2id("Door")
- self.sim.model.body_pos[door_body_id] = door_pos[0]
- self.sim.model.body_quat[door_body_id] = door_quat[0]
+ # Sample from the placement initializer for all objects
+ object_placements = self.placement_initializer.sample()
+
+ # We know we're only setting a single object (the door), so specifically set its pose
+ door_pos, door_quat, _ = object_placements[self.door.name]
+ door_body_id = self.sim.model.body_name2id(self.door.root_body)
+ self.sim.model.body_pos[door_body_id] = door_pos
+ self.sim.model.body_quat[door_body_id] = door_quat
def _get_observation(self):
"""
@@ -391,50 +388,25 @@ def _check_success(self):
hinge_qpos = self.sim.data.qpos[self.hinge_qpos_addr]
return hinge_qpos > 0.3
- def _visualization(self):
- """
- Do any needed visualization here. Overrides superclass implementations.
- """
-
- # color the gripper site appropriately based on distance to door handle
- if self.robots[0].gripper_visualization:
- # get distance to door handle
- dist = np.sum(
- np.square(
- self._handle_xpos
- - self.sim.data.get_site_xpos(self.robots[0].gripper.visualization_sites["grip_site"])
- )
- )
-
- # set RGBA for the EEF site here
- max_dist = 0.1
- scaled = (1.0 - min(dist / max_dist, 1.)) ** 15
- rgba = np.zeros(4)
- rgba[0] = 1 - scaled
- rgba[1] = scaled
- rgba[3] = 0.5
-
- self.sim.model.site_rgba[self.robots[0].eef_site_id] = rgba
-
- def _check_robot_configuration(self, robots):
+ def visualize(self, vis_settings):
"""
- Sanity check to make sure the inputted robots and configuration is acceptable
+ In addition to super call, visualize gripper site proportional to the distance to the door handle.
Args:
- robots (str or list of str): Robots to instantiate within this env
- """
- if type(robots) is list:
- assert len(robots) == 1, "Error: Only one robot should be inputted for this task!"
-
- @property
- def _eef_xpos(self):
+ vis_settings (dict): Visualization keywords mapped to T/F, determining whether that specific
+ component should be visualized. Should have "grippers" keyword as well as any other relevant
+ options specified.
"""
- Grabs End Effector position
-
- Returns:
- np.array: End effector(x,y,z)
- """
- return np.array(self.sim.data.site_xpos[self.robots[0].eef_site_id])
+ # Run superclass method first
+ super().visualize(vis_settings=vis_settings)
+
+ # Color the gripper visualization site according to its distance to the door handle
+ if vis_settings["grippers"]:
+ self._visualize_gripper_to_target(
+ gripper=self.robots[0].gripper,
+ target=self.door.important_sites["handle"],
+ target_type="site"
+ )
@property
def _handle_xpos(self):
diff --git a/robosuite/environments/lift.py b/robosuite/environments/manipulation/lift.py
similarity index 74%
rename from robosuite/environments/lift.py
rename to robosuite/environments/manipulation/lift.py
index bb5d1b52e0..ec89b1dcdd 100644
--- a/robosuite/environments/lift.py
+++ b/robosuite/environments/manipulation/lift.py
@@ -4,15 +4,15 @@
from robosuite.utils.transform_utils import convert_quat
from robosuite.utils.mjcf_utils import CustomMaterial
-from robosuite.environments.robot_env import RobotEnv
-from robosuite.robots import SingleArm
+from robosuite.environments.manipulation.single_arm_env import SingleArmEnv
from robosuite.models.arenas import TableArena
from robosuite.models.objects import BoxObject
-from robosuite.models.tasks import ManipulationTask, UniformRandomSampler
+from robosuite.models.tasks import ManipulationTask
+from robosuite.utils.placement_samplers import UniformRandomSampler
-class Lift(RobotEnv):
+class Lift(SingleArmEnv):
"""
This class corresponds to the lifting task for a single robot arm.
@@ -21,6 +21,9 @@ class Lift(RobotEnv):
(e.g: "Sawyer" would generate one arm; ["Panda", "Panda", "Sawyer"] would generate three robot arms)
Note: Must be a single single-arm robot!
+ env_configuration (str): Specifies how to position the robots within the environment (default is "default").
+ For most single arm environments, this argument has no impact on the robot setup.
+
controller_configs (str or list of dict): If set, contains relevant controller parameters for creating a
custom controller. Else, uses the default controller for this specific task. Should either be single
dict if same controller is to be used for all robots or else it should be a list of the same length as
@@ -32,10 +35,6 @@ class Lift(RobotEnv):
overrides the default gripper. Should either be single str if same gripper type is to be used for all
robots or else it should be a list of the same length as "robots" param
- gripper_visualizations (bool or list of bool): True if using gripper visualization.
- Useful for teleoperation. Should either be single bool if gripper visualization is to be used for all
- robots or else it should be a list of the same length as "robots" param
-
initialization_noise (dict or list of dict): Dict containing the initialization noise parameters.
The expected keys and corresponding value types are specified below:
@@ -66,13 +65,10 @@ class Lift(RobotEnv):
reward_shaping (bool): if True, use dense rewards.
- placement_initializer (ObjectPositionSampler instance): if provided, will
+ placement_initializer (ObjectPositionSampler): if provided, will
be used to place objects on every reset, else a UniformRandomSampler
is used by default.
- use_indicator_object (bool): if True, sets up an indicator object that
- is useful for debugging.
-
has_renderer (bool): If true, render the simulation state in
a viewer instead of headless mode.
@@ -86,6 +82,10 @@ class Lift(RobotEnv):
render_visual_mesh (bool): True if rendering visual meshes in camera. False otherwise.
+ render_gpu_device_id (int): corresponds to the GPU device id to use for offscreen rendering.
+ Defaults to -1, in which case the device will be inferred from environment variables
+ (GPUS or CUDA_VISIBLE_DEVICES).
+
control_freq (float): how many control signals to receive in every second. This sets the amount of
simulation time that passes between every action input.
@@ -124,9 +124,9 @@ class Lift(RobotEnv):
def __init__(
self,
robots,
+ env_configuration="default",
controller_configs=None,
gripper_types="default",
- gripper_visualizations=False,
initialization_noise="default",
table_full_size=(0.8, 0.8, 0.05),
table_friction=(1., 5e-3, 1e-4),
@@ -135,13 +135,13 @@ def __init__(
reward_scale=1.0,
reward_shaping=False,
placement_initializer=None,
- use_indicator_object=False,
has_renderer=False,
has_offscreen_renderer=True,
render_camera="frontview",
render_collision_mesh=False,
render_visual_mesh=True,
- control_freq=10,
+ render_gpu_device_id=-1,
+ control_freq=20,
horizon=1000,
ignore_done=False,
hard_reset=True,
@@ -150,12 +150,10 @@ def __init__(
camera_widths=256,
camera_depths=False,
):
- # First, verify that only one robot is being inputted
- self._check_robot_configuration(robots)
-
# settings for table top
self.table_full_size = table_full_size
self.table_friction = table_friction
+ self.table_offset = np.array((0, 0, 0.8))
# reward configuration
self.reward_scale = reward_scale
@@ -165,30 +163,22 @@ def __init__(
self.use_object_obs = use_object_obs
# object placement initializer
- if placement_initializer:
- self.placement_initializer = placement_initializer
- else:
- self.placement_initializer = UniformRandomSampler(
- x_range=[-0.03, 0.03],
- y_range=[-0.03, 0.03],
- ensure_object_boundary_in_range=False,
- rotation=None,
- z_offset=0.01,
- )
+ self.placement_initializer = placement_initializer
super().__init__(
robots=robots,
+ env_configuration=env_configuration,
controller_configs=controller_configs,
+ mount_types="default",
gripper_types=gripper_types,
- gripper_visualizations=gripper_visualizations,
initialization_noise=initialization_noise,
use_camera_obs=use_camera_obs,
- use_indicator_object=use_indicator_object,
has_renderer=has_renderer,
has_offscreen_renderer=has_offscreen_renderer,
render_camera=render_camera,
render_collision_mesh=render_collision_mesh,
render_visual_mesh=render_visual_mesh,
+ render_gpu_device_id=render_gpu_device_id,
control_freq=control_freq,
horizon=horizon,
ignore_done=ignore_done,
@@ -241,19 +231,7 @@ def reward(self, action=None):
reward += reaching_reward
# grasping reward
- touch_left_finger = False
- touch_right_finger = False
- for i in range(self.sim.data.ncon):
- c = self.sim.data.contact[i]
- if c.geom1 in self.l_finger_geom_ids and c.geom2 == self.cube_geom_id:
- touch_left_finger = True
- if c.geom1 == self.cube_geom_id and c.geom2 in self.l_finger_geom_ids:
- touch_left_finger = True
- if c.geom1 in self.r_finger_geom_ids and c.geom2 == self.cube_geom_id:
- touch_right_finger = True
- if c.geom1 == self.cube_geom_id and c.geom2 in self.r_finger_geom_ids:
- touch_right_finger = True
- if touch_left_finger and touch_right_finger:
+ if self._check_grasp(gripper=self.robots[0].gripper, object_geoms=self.cube):
reward += 0.25
# Scale reward if requested
@@ -268,25 +246,19 @@ def _load_model(self):
"""
super()._load_model()
- # Verify the correct robot has been loaded
- assert isinstance(self.robots[0], SingleArm), \
- "Error: Expected one single-armed robot! Got {} type instead.".format(type(self.robots[0]))
-
# Adjust base pose accordingly
xpos = self.robots[0].robot_model.base_xpos_offset["table"](self.table_full_size[0])
self.robots[0].robot_model.set_base_xpos(xpos)
# load model for table top workspace
- self.mujoco_arena = TableArena(
+ mujoco_arena = TableArena(
table_full_size=self.table_full_size,
table_friction=self.table_friction,
- table_offset=(0, 0, 0.8),
+ table_offset=self.table_offset,
)
- if self.use_indicator_object:
- self.mujoco_arena.add_pos_indicator()
# Arena always gets set to zero origin
- self.mujoco_arena.set_origin([0, 0, 0])
+ mujoco_arena.set_origin([0, 0, 0])
# initialize objects of interest
tex_attrib = {
@@ -304,25 +276,37 @@ def _load_model(self):
tex_attrib=tex_attrib,
mat_attrib=mat_attrib,
)
- cube = BoxObject(
+ self.cube = BoxObject(
name="cube",
size_min=[0.020, 0.020, 0.020], # [0.015, 0.015, 0.015],
size_max=[0.022, 0.022, 0.022], # [0.018, 0.018, 0.018])
rgba=[1, 0, 0, 1],
material=redwood,
)
- self.mujoco_objects = OrderedDict([("cube", cube)])
- self.n_objects = len(self.mujoco_objects)
+
+ # Create placement initializer
+ if self.placement_initializer is not None:
+ self.placement_initializer.reset()
+ self.placement_initializer.add_objects(self.cube)
+ else:
+ self.placement_initializer = UniformRandomSampler(
+ name="ObjectSampler",
+ mujoco_objects=self.cube,
+ x_range=[-0.03, 0.03],
+ y_range=[-0.03, 0.03],
+ rotation=None,
+ ensure_object_boundary_in_range=False,
+ ensure_valid_placement=True,
+ reference_pos=self.table_offset,
+ z_offset=0.01,
+ )
# task includes arena, robot, and objects of interest
self.model = ManipulationTask(
- mujoco_arena=self.mujoco_arena,
+ mujoco_arena=mujoco_arena,
mujoco_robots=[robot.robot_model for robot in self.robots],
- mujoco_objects=self.mujoco_objects,
- visual_objects=None,
- initializer=self.placement_initializer,
+ mujoco_objects=self.cube,
)
- self.model.place_objects()
def _get_reference(self):
"""
@@ -333,14 +317,7 @@ def _get_reference(self):
super()._get_reference()
# Additional object references from this env
- self.cube_body_id = self.sim.model.body_name2id("cube")
- self.l_finger_geom_ids = [
- self.sim.model.geom_name2id(x) for x in self.robots[0].gripper.important_geoms["left_finger"]
- ]
- self.r_finger_geom_ids = [
- self.sim.model.geom_name2id(x) for x in self.robots[0].gripper.important_geoms["right_finger"]
- ]
- self.cube_geom_id = self.sim.model.geom_name2id("cube")
+ self.cube_body_id = self.sim.model.body_name2id(self.cube.root_body)
def _reset_internal(self):
"""
@@ -352,11 +329,11 @@ def _reset_internal(self):
if not self.deterministic_reset:
# Sample from the placement initializer for all objects
- obj_pos, obj_quat = self.model.place_objects()
+ object_placements = self.placement_initializer.sample()
# Loop through all objects and reset their positions
- for i, (obj_name, _) in enumerate(self.mujoco_objects.items()):
- self.sim.data.set_joint_qpos(obj_name + "_jnt0", np.concatenate([np.array(obj_pos[i]), np.array(obj_quat[i])]))
+ for obj_pos, obj_quat, obj in object_placements.values():
+ self.sim.data.set_joint_qpos(obj.joints[0], np.concatenate([np.array(obj_pos), np.array(obj_quat)]))
def _get_observation(self):
"""
@@ -400,6 +377,22 @@ def _get_observation(self):
return di
+ def visualize(self, vis_settings):
+ """
+ In addition to super call, visualize gripper site proportional to the distance to the cube.
+
+ Args:
+ vis_settings (dict): Visualization keywords mapped to T/F, determining whether that specific
+ component should be visualized. Should have "grippers" keyword as well as any other relevant
+ options specified.
+ """
+ # Run superclass method first
+ super().visualize(vis_settings=vis_settings)
+
+ # Color the gripper visualization site according to its distance to the cube
+ if vis_settings["grippers"]:
+ self._visualize_gripper_to_target(gripper=self.robots[0].gripper, target=self.cube)
+
def _check_success(self):
"""
Check if cube has been lifted.
@@ -408,43 +401,7 @@ def _check_success(self):
bool: True if cube has been lifted
"""
cube_height = self.sim.data.body_xpos[self.cube_body_id][2]
- table_height = self.mujoco_arena.table_offset[2]
+ table_height = self.model.mujoco_arena.table_offset[2]
# cube is higher than the table top above a margin
return cube_height > table_height + 0.04
-
- def _visualization(self):
- """
- Do any needed visualization here. Overrides superclass implementations.
- """
-
- # color the gripper site appropriately based on distance to cube
- if self.robots[0].gripper_visualization:
- # get distance to cube
- cube_site_id = self.sim.model.site_name2id("cube")
- dist = np.sum(
- np.square(
- self.sim.data.site_xpos[cube_site_id]
- - self.sim.data.get_site_xpos(self.robots[0].gripper.visualization_sites["grip_site"])
- )
- )
-
- # set RGBA for the EEF site here
- max_dist = 0.1
- scaled = (1.0 - min(dist / max_dist, 1.)) ** 15
- rgba = np.zeros(4)
- rgba[0] = 1 - scaled
- rgba[1] = scaled
- rgba[3] = 0.5
-
- self.sim.model.site_rgba[self.robots[0].eef_site_id] = rgba
-
- def _check_robot_configuration(self, robots):
- """
- Sanity check to make sure the inputted robots and configuration is acceptable
-
- Args:
- robots (str or list of str): Robots to instantiate within this env
- """
- if type(robots) is list:
- assert len(robots) == 1, "Error: Only one robot should be inputted for this task!"
diff --git a/robosuite/environments/manipulation/manipulation_env.py b/robosuite/environments/manipulation/manipulation_env.py
new file mode 100644
index 0000000000..b11782f82d
--- /dev/null
+++ b/robosuite/environments/manipulation/manipulation_env.py
@@ -0,0 +1,301 @@
+import numpy as np
+
+from robosuite.environments.robot_env import RobotEnv
+from robosuite.models.grippers import GripperModel
+from robosuite.models.base import MujocoModel
+from robosuite.robots import Manipulator, ROBOT_CLASS_MAPPING
+
+
+class ManipulationEnv(RobotEnv):
+ """
+ Initializes a manipulation-specific robot environment in Mujoco.
+
+ Args:
+ robots: Specification for specific robot arm(s) to be instantiated within this env
+ (e.g: "Sawyer" would generate one arm; ["Panda", "Panda", "Sawyer"] would generate three robot arms)
+
+ env_configuration (str): Specifies how to position the robot(s) within the environment. Default is "default",
+ which should be interpreted accordingly by any subclasses.
+
+ controller_configs (str or list of dict): If set, contains relevant controller parameters for creating a
+ custom controller. Else, uses the default controller for this specific task. Should either be single
+ dict if same controller is to be used for all robots or else it should be a list of the same length as
+ "robots" param
+
+ mount_types (None or str or list of str): type of mount, used to instantiate mount models from mount factory.
+ Default is "default", which is the default mount associated with the robot(s) the 'robots' specification.
+ None results in no mount, and any other (valid) model overrides the default mount. Should either be
+ single str if same mount type is to be used for all robots or else it should be a list of the same
+ length as "robots" param
+
+ gripper_types (None or str or list of str): type of gripper, used to instantiate
+ gripper models from gripper factory. Default is "default", which is the default grippers(s) associated
+ with the robot(s) the 'robots' specification. None removes the gripper, and any other (valid) model
+ overrides the default gripper. Should either be single str if same gripper type is to be used for all
+ robots or else it should be a list of the same length as "robots" param
+
+ initialization_noise (dict or list of dict): Dict containing the initialization noise parameters.
+ The expected keys and corresponding value types are specified below:
+
+ :`'magnitude'`: The scale factor of uni-variate random noise applied to each of a robot's given initial
+ joint positions. Setting this value to `None` or 0.0 results in no noise being applied.
+ If "gaussian" type of noise is applied then this magnitude scales the standard deviation applied,
+ If "uniform" type of noise is applied then this magnitude sets the bounds of the sampling range
+ :`'type'`: Type of noise to apply. Can either specify "gaussian" or "uniform"
+
+ Should either be single dict if same noise value is to be used for all robots or else it should be a
+ list of the same length as "robots" param
+
+ :Note: Specifying "default" will automatically use the default noise settings.
+ Specifying None will automatically create the required dict with "magnitude" set to 0.0.
+
+ use_camera_obs (bool): if True, every observation includes rendered image(s)
+
+ has_renderer (bool): If true, render the simulation state in
+ a viewer instead of headless mode.
+
+ has_offscreen_renderer (bool): True if using off-screen rendering
+
+ render_camera (str): Name of camera to render if `has_renderer` is True. Setting this value to 'None'
+ will result in the default angle being applied, which is useful as it can be dragged / panned by
+ the user using the mouse
+
+ render_collision_mesh (bool): True if rendering collision meshes in camera. False otherwise.
+
+ render_visual_mesh (bool): True if rendering visual meshes in camera. False otherwise.
+
+ render_gpu_device_id (int): corresponds to the GPU device id to use for offscreen rendering.
+ Defaults to -1, in which case the device will be inferred from environment variables
+ (GPUS or CUDA_VISIBLE_DEVICES).
+
+ control_freq (float): how many control signals to receive in every second. This sets the amount of
+ simulation time that passes between every action input.
+
+ horizon (int): Every episode lasts for exactly @horizon timesteps.
+
+ ignore_done (bool): True if never terminating the environment (ignore @horizon).
+
+ hard_reset (bool): If True, re-loads model, sim, and render object upon a reset call, else,
+ only calls sim.reset and resets all robosuite-internal variables
+
+ camera_names (str or list of str): name of camera to be rendered. Should either be single str if
+ same name is to be used for all cameras' rendering or else it should be a list of cameras to render.
+
+ :Note: At least one camera must be specified if @use_camera_obs is True.
+
+ :Note: To render all robots' cameras of a certain type (e.g.: "robotview" or "eye_in_hand"), use the
+ convention "all-{name}" (e.g.: "all-robotview") to automatically render all camera images from each
+ robot's camera list).
+
+ camera_heights (int or list of int): height of camera frame. Should either be single int if
+ same height is to be used for all cameras' frames or else it should be a list of the same length as
+ "camera names" param.
+
+ camera_widths (int or list of int): width of camera frame. Should either be single int if
+ same width is to be used for all cameras' frames or else it should be a list of the same length as
+ "camera names" param.
+
+ camera_depths (bool or list of bool): True if rendering RGB-D, and RGB otherwise. Should either be single
+ bool if same depth setting is to be used for all cameras or else it should be a list of the same length as
+ "camera names" param.
+
+ Raises:
+ ValueError: [Camera obs require offscreen renderer]
+ ValueError: [Camera name must be specified to use camera obs]
+ """
+
+ def __init__(
+ self,
+ robots,
+ env_configuration="default",
+ controller_configs=None,
+ mount_types="default",
+ gripper_types="default",
+ initialization_noise=None,
+ use_camera_obs=True,
+ has_renderer=False,
+ has_offscreen_renderer=True,
+ render_camera="frontview",
+ render_collision_mesh=False,
+ render_visual_mesh=True,
+ render_gpu_device_id=-1,
+ control_freq=20,
+ horizon=1000,
+ ignore_done=False,
+ hard_reset=True,
+ camera_names="agentview",
+ camera_heights=256,
+ camera_widths=256,
+ camera_depths=False,
+ ):
+ # Robot info
+ robots = list(robots) if type(robots) is list or type(robots) is tuple else [robots]
+ num_robots = len(robots)
+
+ # Gripper
+ gripper_types = self._input2list(gripper_types, num_robots)
+
+ # Robot configurations to pass to super call
+ robot_configs = [
+ {
+ "gripper_type": gripper_types[idx],
+ }
+ for idx in range(num_robots)
+ ]
+
+ # Run superclass init
+ super().__init__(
+ robots=robots,
+ env_configuration=env_configuration,
+ controller_configs=controller_configs,
+ mount_types=mount_types,
+ initialization_noise=initialization_noise,
+ use_camera_obs=use_camera_obs,
+ has_renderer=has_renderer,
+ has_offscreen_renderer=has_offscreen_renderer,
+ render_camera=render_camera,
+ render_collision_mesh=render_collision_mesh,
+ render_visual_mesh=render_visual_mesh,
+ render_gpu_device_id=render_gpu_device_id,
+ control_freq=control_freq,
+ horizon=horizon,
+ ignore_done=ignore_done,
+ hard_reset=hard_reset,
+ camera_names=camera_names,
+ camera_heights=camera_heights,
+ camera_widths=camera_widths,
+ camera_depths=camera_depths,
+ robot_configs=robot_configs,
+ )
+
+ @property
+ def _visualizations(self):
+ """
+ Visualization keywords for this environment
+
+ Returns:
+ set: All components that can be individually visualized for this environment
+ """
+ vis_set = super()._visualizations
+ vis_set.add("grippers")
+ return vis_set
+
+ def _check_grasp(self, gripper, object_geoms):
+ """
+ Checks whether the specified gripper as defined by @gripper is grasping the specified object in the environment.
+
+ By default, this will return True if at least one geom in both the "left_fingerpad" and "right_fingerpad" geom
+ groups are in contact with any geom specified by @object_geoms. Custom gripper geom groups can be
+ specified with @gripper as well.
+
+ Args:
+ gripper (GripperModel or str or list of str or list of list of str): If a MujocoModel, this is specific
+ gripper to check for grasping (as defined by "left_fingerpad" and "right_fingerpad" geom groups). Otherwise,
+ this sets custom gripper geom groups which together define a grasp. This can be a string
+ (one group of single gripper geom), a list of string (multiple groups of single gripper geoms) or a
+ list of list of string (multiple groups of multiple gripper geoms). At least one geom from each group
+ must be in contact with any geom in @object_geoms for this method to return True.
+ object_geoms (str or list of str or MujocoModel): If a MujocoModel is inputted, will check for any
+ collisions with the model's contact_geoms. Otherwise, this should be specific geom name(s) composing
+ the object to check for contact.
+
+ Returns:
+ bool: True if the gripper is grasping the given object
+ """
+ # Convert object, gripper geoms into standardized form
+ if isinstance(object_geoms, MujocoModel):
+ o_geoms = object_geoms.contact_geoms
+ else:
+ o_geoms = [object_geoms] if type(object_geoms) is str else object_geoms
+ if isinstance(gripper, GripperModel):
+ g_geoms = [gripper.important_geoms["left_fingerpad"], gripper.important_geoms["right_fingerpad"]]
+ elif type(gripper) is str:
+ g_geoms = [[gripper]]
+ else:
+ # Parse each element in the gripper_geoms list accordingly
+ g_geoms = [[g_group] if type(g_group) is str else g_group for g_group in gripper]
+
+ # Search for collisions between each gripper geom group and the object geoms group
+ for g_group in g_geoms:
+ if not self.check_contact(g_group, o_geoms):
+ return False
+ return True
+
+ def _gripper_to_target(self, gripper, target, target_type="body", return_distance=False):
+ """
+ Calculates the (x,y,z) Cartesian distance (target_pos - gripper_pos) from the specified @gripper to the
+ specified @target. If @return_distance is set, will return the Euclidean (scalar) distance instead.
+
+ Args:
+ gripper (MujocoModel): Gripper model to update grip site rgb
+ target (MujocoModel or str): Either a site / geom / body name, or a model that serves as the target.
+ If a model is given, then the root body will be used as the target.
+ target_type (str): One of {"body", "geom", or "site"}, corresponding to the type of element @target
+ refers to.
+ return_distance (bool): If set, will return Euclidean distance instead of Cartesian distance
+
+ Returns:
+ np.array or float: (Cartesian or Euclidean) distance from gripper to target
+ """
+ # Get gripper and target positions
+ gripper_pos = self.sim.data.get_site_xpos(gripper.important_sites["grip_site"])
+ # If target is MujocoModel, grab the correct body as the target and find the target position
+ if isinstance(target, MujocoModel):
+ target_pos = self.sim.data.get_body_xpos(target.root_body)
+ elif target_type == "body":
+ target_pos = self.sim.data.get_body_xpos(target)
+ elif target_type == "site":
+ target_pos = self.sim.data.get_site_xpos(target)
+ else:
+ target_pos = self.sim.data.get_geom_xpos(target)
+ # Calculate distance
+ diff = target_pos - gripper_pos
+ # Return appropriate value
+ return np.linalg.norm(diff) if return_distance else diff
+
+ def _visualize_gripper_to_target(self, gripper, target, target_type="body"):
+ """
+ Colors the grip visualization site proportional to the Euclidean distance to the specified @target.
+ Colors go from red --> green as the gripper gets closer.
+
+ Args:
+ gripper (MujocoModel): Gripper model to update grip site rgb
+ target (MujocoModel or str): Either a site / geom / body name, or a model that serves as the target.
+ If a model is given, then the root body will be used as the target.
+ target_type (str): One of {"body", "geom", or "site"}, corresponding to the type of element @target
+ refers to.
+ """
+ # Get gripper and target positions
+ gripper_pos = self.sim.data.get_site_xpos(gripper.important_sites["grip_site"])
+ # If target is MujocoModel, grab the correct body as the target and find the target position
+ if isinstance(target, MujocoModel):
+ target_pos = self.sim.data.get_body_xpos(target.root_body)
+ elif target_type == "body":
+ target_pos = self.sim.data.get_body_xpos(target)
+ elif target_type == "site":
+ target_pos = self.sim.data.get_site_xpos(target)
+ else:
+ target_pos = self.sim.data.get_geom_xpos(target)
+ # color the gripper site appropriately based on (squared) distance to target
+ dist = np.sum(np.square((target_pos - gripper_pos)))
+ max_dist = 0.1
+ scaled = (1.0 - min(dist / max_dist, 1.)) ** 15
+ rgba = np.zeros(3)
+ rgba[0] = 1 - scaled
+ rgba[1] = scaled
+ self.sim.model.site_rgba[self.sim.model.site_name2id(gripper.important_sites["grip_site"])][:3] = rgba
+
+ def _check_robot_configuration(self, robots):
+ """
+ Sanity check to make sure inputted robots and the corresponding requested task/configuration combo is legal.
+ Should be implemented in every specific task module
+
+ Args:
+ robots (str or list of str): Inputted requested robots at the task-level environment
+ """
+ # Make sure all inputted robots are a manipulation robot
+ if type(robots) is str:
+ robots = [robots]
+ for robot in robots:
+ assert issubclass(ROBOT_CLASS_MAPPING[robot], Manipulator),\
+ "Only manipulator robots supported for manipulation environment!"
diff --git a/robosuite/environments/nut_assembly.py b/robosuite/environments/manipulation/nut_assembly.py
similarity index 63%
rename from robosuite/environments/nut_assembly.py
rename to robosuite/environments/manipulation/nut_assembly.py
index 5628f6c260..d1a37f2d39 100644
--- a/robosuite/environments/nut_assembly.py
+++ b/robosuite/environments/manipulation/nut_assembly.py
@@ -3,15 +3,15 @@
import numpy as np
import robosuite.utils.transform_utils as T
-from robosuite.environments.robot_env import RobotEnv
-from robosuite.robots import SingleArm
+from robosuite.environments.manipulation.single_arm_env import SingleArmEnv
from robosuite.models.arenas import PegsArena
from robosuite.models.objects import SquareNutObject, RoundNutObject
-from robosuite.models.tasks import ManipulationTask, SequentialCompositeSampler
+from robosuite.models.tasks import ManipulationTask
+from robosuite.utils.placement_samplers import SequentialCompositeSampler, UniformRandomSampler
-class NutAssembly(RobotEnv):
+class NutAssembly(SingleArmEnv):
"""
This class corresponds to the nut assembly task for a single robot arm.
@@ -20,6 +20,9 @@ class NutAssembly(RobotEnv):
(e.g: "Sawyer" would generate one arm; ["Panda", "Panda", "Sawyer"] would generate three robot arms)
Note: Must be a single single-arm robot!
+ env_configuration (str): Specifies how to position the robots within the environment (default is "default").
+ For most single arm environments, this argument has no impact on the robot setup.
+
controller_configs (str or list of dict): If set, contains relevant controller parameters for creating a
custom controller. Else, uses the default controller for this specific task. Should either be single
dict if same controller is to be used for all robots or else it should be a list of the same length as
@@ -31,10 +34,6 @@ class NutAssembly(RobotEnv):
overrides the default gripper. Should either be single str if same gripper type is to be used for all
robots or else it should be a list of the same length as "robots" param
- gripper_visualizations (bool or list of bool): True if using gripper visualization.
- Useful for teleoperation. Should either be single bool if gripper visualization is to be used for all
- robots or else it should be a list of the same length as "robots" param
-
initialization_noise (dict or list of dict): Dict containing the initialization noise parameters.
The expected keys and corresponding value types are specified below:
@@ -65,7 +64,7 @@ class NutAssembly(RobotEnv):
reward_shaping (bool): if True, use dense rewards.
- placement_initializer (ObjectPositionSampler instance): if provided, will
+ placement_initializer (ObjectPositionSampler): if provided, will
be used to place objects on every reset, else a UniformRandomSampler
is used by default.
@@ -85,9 +84,6 @@ class NutAssembly(RobotEnv):
which type of nut (round or square) will be spawned on every environment
reset. Only used if @single_object_mode is 2.
- use_indicator_object (bool): if True, sets up an indicator object that
- is useful for debugging.
-
has_renderer (bool): If true, render the simulation state in
a viewer instead of headless mode.
@@ -101,6 +97,10 @@ class NutAssembly(RobotEnv):
render_visual_mesh (bool): True if rendering visual meshes in camera. False otherwise.
+ render_gpu_device_id (int): corresponds to the GPU device id to use for offscreen rendering.
+ Defaults to -1, in which case the device will be inferred from environment variables
+ (GPUS or CUDA_VISIBLE_DEVICES).
+
control_freq (float): how many control signals to receive in every second. This sets the amount of
simulation time that passes between every action input.
@@ -140,9 +140,9 @@ class NutAssembly(RobotEnv):
def __init__(
self,
robots,
+ env_configuration="default",
controller_configs=None,
gripper_types="default",
- gripper_visualizations=False,
initialization_noise="default",
table_full_size=(0.8, 0.8, 0.05),
table_friction=(1, 0.005, 0.0001),
@@ -153,13 +153,13 @@ def __init__(
placement_initializer=None,
single_object_mode=0,
nut_type=None,
- use_indicator_object=False,
has_renderer=False,
has_offscreen_renderer=True,
render_camera="frontview",
render_collision_mesh=False,
render_visual_mesh=True,
- control_freq=10,
+ render_gpu_device_id=-1,
+ control_freq=20,
horizon=1000,
ignore_done=False,
hard_reset=True,
@@ -168,9 +168,6 @@ def __init__(
camera_widths=256,
camera_depths=False,
):
- # First, verify that only one robot is being inputted
- self._check_robot_configuration(robots)
-
# task settings
self.single_object_mode = single_object_mode
self.nut_to_id = {"square": 0, "round": 1}
@@ -186,6 +183,7 @@ def __init__(
# settings for table top
self.table_full_size = table_full_size
self.table_friction = table_friction
+ self.table_offset = np.array((0, 0, 0.82))
# reward configuration
self.reward_scale = reward_scale
@@ -195,46 +193,22 @@ def __init__(
self.use_object_obs = use_object_obs
# object placement initializer
- if placement_initializer:
- self.placement_initializer = placement_initializer
- else:
- # treat sampling of each type of nut differently since we require different
- # sampling ranges for each
- self.placement_initializer = SequentialCompositeSampler()
- self.placement_initializer.sample_on_top(
- "SquareNut0",
- surface_name="table",
- x_range=[-0.115, -0.11],
- y_range=[0.11, 0.225],
- rotation=None,
- rotation_axis='z',
- z_offset=0.02,
- ensure_object_boundary_in_range=False,
- )
- self.placement_initializer.sample_on_top(
- "RoundNut0",
- surface_name="table",
- x_range=[-0.115, -0.11],
- y_range=[-0.225, -0.11],
- rotation=None,
- rotation_axis='z',
- z_offset=0.02,
- ensure_object_boundary_in_range=False,
- )
+ self.placement_initializer = placement_initializer
super().__init__(
robots=robots,
+ env_configuration=env_configuration,
controller_configs=controller_configs,
+ mount_types="default",
gripper_types=gripper_types,
- gripper_visualizations=gripper_visualizations,
initialization_noise=initialization_noise,
use_camera_obs=use_camera_obs,
- use_indicator_object=use_indicator_object,
has_renderer=has_renderer,
has_offscreen_renderer=has_offscreen_renderer,
render_camera=render_camera,
render_collision_mesh=render_collision_mesh,
render_visual_mesh=render_visual_mesh,
+ render_gpu_device_id=render_gpu_device_id,
control_freq=control_freq,
horizon=horizon,
ignore_done=ignore_done,
@@ -306,75 +280,60 @@ def staged_rewards(self):
hover_mult = 0.7
# filter out objects that are already on the correct pegs
- names_to_reach = []
- objs_to_reach = []
- geoms_to_grasp = []
- geoms_by_array = []
-
- for i in range(len(self.ob_inits)):
+ active_nuts = []
+ for i, nut in enumerate(self.nuts):
if self.objects_on_pegs[i]:
continue
- obj_str = str(self.item_names[i]) + "0"
- names_to_reach.append(obj_str)
- objs_to_reach.append(self.obj_body_id[obj_str])
- geoms_to_grasp.extend(self.obj_geom_id[obj_str])
- geoms_by_array.append(self.obj_geom_id[obj_str])
+ active_nuts.append(nut)
- ### reaching reward governed by distance to closest object ###
+ # reaching reward governed by distance to closest object
r_reach = 0.
- if len(objs_to_reach):
- # reaching reward via minimum distance to the handles of the objects (the last geom of each nut)
- geom_ids = [elem[-1] for elem in geoms_by_array]
- target_geom_pos = self.sim.data.geom_xpos[geom_ids]
- gripper_site_pos = self.sim.data.site_xpos[self.robots[0].eef_site_id]
- dists = np.linalg.norm(
- target_geom_pos - gripper_site_pos.reshape(1, -1), axis=1
- )
+ if active_nuts:
+ # reaching reward via minimum distance to the handles of the objects
+ dists = [
+ self._gripper_to_target(
+ gripper=self.robots[0].gripper,
+ target=active_nut.important_sites["handle"],
+ target_type="site",
+ return_distance=True,
+ ) for active_nut in active_nuts
+ ]
r_reach = (1 - np.tanh(10.0 * min(dists))) * reach_mult
- ### grasping reward for touching any objects of interest ###
- touch_left_finger = False
- touch_right_finger = False
- for i in range(self.sim.data.ncon):
- c = self.sim.data.contact[i]
- if c.geom1 in geoms_to_grasp:
- if c.geom2 in self.l_finger_geom_ids:
- touch_left_finger = True
- if c.geom2 in self.r_finger_geom_ids:
- touch_right_finger = True
- elif c.geom2 in geoms_to_grasp:
- if c.geom1 in self.l_finger_geom_ids:
- touch_left_finger = True
- if c.geom1 in self.r_finger_geom_ids:
- touch_right_finger = True
- has_grasp = touch_left_finger and touch_right_finger
- r_grasp = int(has_grasp) * grasp_mult
-
- ### lifting reward for picking up an object ###
+ # grasping reward for touching any objects of interest
+ r_grasp = int(self._check_grasp(
+ gripper=self.robots[0].gripper,
+ object_geoms=[g for active_nut in active_nuts for g in active_nut.contact_geoms])
+ ) * grasp_mult
+
+ # lifting reward for picking up an object
r_lift = 0.
table_pos = np.array(self.sim.data.body_xpos[self.table_body_id])
- if len(objs_to_reach) and r_grasp > 0.:
+ if active_nuts and r_grasp > 0.:
z_target = table_pos[2] + 0.2
- object_z_locs = self.sim.data.body_xpos[objs_to_reach][:, 2]
+ object_z_locs = self.sim.data.body_xpos[[self.obj_body_id[active_nut.name]
+ for active_nut in active_nuts]][:, 2]
z_dists = np.maximum(z_target - object_z_locs, 0.)
r_lift = grasp_mult + (1 - np.tanh(15.0 * min(z_dists))) * (
lift_mult - grasp_mult
)
- ### hover reward for getting object above peg ###
+ # hover reward for getting object above peg
r_hover = 0.
- if len(objs_to_reach):
- r_hovers = np.zeros(len(objs_to_reach))
- for i in range(len(objs_to_reach)):
- if names_to_reach[i].startswith(self.item_names[0]):
- peg_pos = np.array(self.sim.data.body_xpos[self.peg1_body_id])[:2]
- elif names_to_reach[i].startswith(self.item_names[1]):
- peg_pos = np.array(self.sim.data.body_xpos[self.peg2_body_id])[:2]
- else:
- raise Exception(
- "Got invalid object to reach: {}".format(names_to_reach[i])
- )
- ob_xy = self.sim.data.body_xpos[objs_to_reach[i]][:2]
+ if active_nuts:
+ r_hovers = np.zeros(len(active_nuts))
+ peg_body_ids = [self.peg1_body_id, self.peg2_body_id]
+ for i, nut in enumerate(active_nuts):
+ valid_obj = False
+ peg_pos = None
+ for nut_name, idn in self.nut_to_id.items():
+ if nut_name in nut.name.lower():
+ peg_pos = np.array(self.sim.data.body_xpos[peg_body_ids[idn]])[:2]
+ valid_obj = True
+ break
+ if not valid_obj:
+ raise Exception("Got invalid object to reach: {}".format(nut.name))
+ ob_xy = self.sim.data.body_xpos[self.obj_body_id[nut.name]][:2]
dist = np.linalg.norm(peg_pos - ob_xy)
r_hovers[i] = r_lift + (1 - np.tanh(10.0 * dist)) * (
hover_mult - lift_mult
@@ -393,87 +352,76 @@ def on_peg(self, obj_pos, peg_id):
if (
abs(obj_pos[0] - peg_pos[0]) < 0.03
and abs(obj_pos[1] - peg_pos[1]) < 0.03
- and obj_pos[2] < self.mujoco_arena.table_offset[2] + 0.05
+ and obj_pos[2] < self.table_offset[2] + 0.05
):
res = True
return res
- def clear_objects(self, obj):
- """
- Clears objects without the name @obj out of the task space. This is useful
- for supporting task modes with single types of objects, as in
- @self.single_object_mode without changing the model definition.
-
- Args:
- obj (str): Name of object to keep in the task space
- """
- for obj_name, obj_mjcf in self.mujoco_objects.items():
- if obj_name == obj:
- continue
- else:
- sim_state = self.sim.get_state()
- # print(self.sim.model.get_joint_qpos_addr(obj_name))
- sim_state.qpos[self.sim.model.get_joint_qpos_addr(obj_name + "_jnt0")[0]] = 10
- self.sim.set_state(sim_state)
- self.sim.forward()
-
def _load_model(self):
"""
Loads an xml model, puts it in self.model
"""
super()._load_model()
- # Verify the correct robot has been loaded
- assert isinstance(self.robots[0], SingleArm), \
- "Error: Expected one single-armed robot! Got {} type instead.".format(type(self.robots[0]))
-
# Adjust base pose accordingly
xpos = self.robots[0].robot_model.base_xpos_offset["table"](self.table_full_size[0])
self.robots[0].robot_model.set_base_xpos(xpos)
# load model for table top workspace
- self.mujoco_arena = PegsArena(
+ mujoco_arena = PegsArena(
table_full_size=self.table_full_size,
table_friction=self.table_friction,
- table_offset=(0, 0, 0.82)
+ table_offset=self.table_offset,
)
- if self.use_indicator_object:
- self.mujoco_arena.add_pos_indicator()
# Arena always gets set to zero origin
- self.mujoco_arena.set_origin([0, 0, 0])
-
- # define mujoco objects
- self.ob_inits = [SquareNutObject, RoundNutObject]
- self.item_names = ["SquareNut", "RoundNut"]
- self.item_names_org = list(self.item_names)
- self.obj_to_use = (self.item_names[1] + "{}").format(0)
- self.ngeoms = [5, 9]
-
- lst = []
- for i in range(len(self.ob_inits)):
- ob_name = (self.item_names[i] + "0")
- ob = self.ob_inits[i](
- name=ob_name,
- joints=[dict(type="free", damping="0.0005")], # damp the free joint for each object
- )
- lst.append((ob_name, ob))
-
- self.mujoco_objects = OrderedDict(lst)
- self.n_objects = len(self.mujoco_objects)
+ mujoco_arena.set_origin([0, 0, 0])
+
+ # define nuts
+ self.nuts = []
+ nut_names = ("SquareNut", "RoundNut")
+
+ # Create default (SequentialCompositeSampler) sampler if it has not already been specified
+ if self.placement_initializer is None:
+ self.placement_initializer = SequentialCompositeSampler(name="ObjectSampler")
+ for nut_name, default_y_range in zip(nut_names, ([0.11, 0.225], [-0.225, -0.11])):
+ self.placement_initializer.append_sampler(
+ sampler=UniformRandomSampler(
+ name=f"{nut_name}Sampler",
+ x_range=[-0.115, -0.11],
+ y_range=default_y_range,
+ rotation=None,
+ rotation_axis='z',
+ ensure_object_boundary_in_range=False,
+ ensure_valid_placement=True,
+ reference_pos=self.table_offset,
+ z_offset=0.02,
+ )
+ )
+ # Reset sampler before adding any new samplers / objects
+ self.placement_initializer.reset()
+
+ for i, (nut_cls, nut_name) in enumerate(zip(
+ (SquareNutObject, RoundNutObject),
+ nut_names,
+ )):
+ nut = nut_cls(name=nut_name)
+ self.nuts.append(nut)
+ # Add this nut to the placement initializer
+ if isinstance(self.placement_initializer, SequentialCompositeSampler):
+ # assumes we have two samplers so we add nuts to them
+ self.placement_initializer.add_objects_to_sampler(sampler_name=f"{nut_name}Sampler", mujoco_objects=nut)
+ else:
+ # This is assumed to be a flat sampler, so we just add all nuts to this sampler
+ self.placement_initializer.add_objects(nut)
# task includes arena, robot, and objects of interest
self.model = ManipulationTask(
- mujoco_arena=self.mujoco_arena,
+ mujoco_arena=mujoco_arena,
mujoco_robots=[robot.robot_model for robot in self.robots],
- mujoco_objects=self.mujoco_objects,
- visual_objects=None,
- initializer=self.placement_initializer,
+ mujoco_objects=self.nuts,
)
- # set positions of objects
- self.model.place_objects()
-
def _get_reference(self):
"""
Sets up references to important components. A reference is typically an
@@ -490,35 +438,15 @@ def _get_reference(self):
self.peg1_body_id = self.sim.model.body_name2id("peg1")
self.peg2_body_id = self.sim.model.body_name2id("peg2")
- for i in range(len(self.ob_inits)):
- obj_str = str(self.item_names[i]) + "0"
- self.obj_body_id[obj_str] = self.sim.model.body_name2id(obj_str)
- geom_ids = []
- for j in range(self.ngeoms[i]):
- geom_ids.append(self.sim.model.geom_name2id(obj_str + "-{}".format(j)))
- self.obj_geom_id[obj_str] = geom_ids
+ for nut in self.nuts:
+ self.obj_body_id[nut.name] = self.sim.model.body_name2id(nut.root_body)
+ self.obj_geom_id[nut.name] = [self.sim.model.geom_name2id(g) for g in nut.contact_geoms]
# information of objects
- self.object_names = list(self.mujoco_objects.keys())
- self.object_site_ids = [
- self.sim.model.site_name2id(ob_name) for ob_name in self.object_names
- ]
-
- # id of grippers for contact checking
- self.l_finger_geom_ids = [
- self.sim.model.geom_name2id(x) for x in self.robots[0].gripper.important_geoms["left_finger"]
- ]
- self.r_finger_geom_ids = [
- self.sim.model.geom_name2id(x) for x in self.robots[0].gripper.important_geoms["right_finger"]
- ]
- # self.sim.data.contact # list, geom1, geom2
- self.collision_check_geom_names = self.sim.model._geom_name2id.keys()
- self.collision_check_geom_ids = [
- self.sim.model._geom_name2id[k] for k in self.collision_check_geom_names
- ]
+ self.object_site_ids = [self.sim.model.site_name2id(nut.important_sites["handle"]) for nut in self.nuts]
# keep track of which objects are on their corresponding pegs
- self.objects_on_pegs = np.zeros(len(self.ob_inits))
+ self.objects_on_pegs = np.zeros(len(self.nuts))
def _reset_internal(self):
"""
@@ -530,25 +458,21 @@ def _reset_internal(self):
if not self.deterministic_reset:
# Sample from the placement initializer for all objects
- obj_pos, obj_quat = self.model.place_objects()
+ object_placements = self.placement_initializer.sample()
# Loop through all objects and reset their positions
- for i, (obj_name, _) in enumerate(self.mujoco_objects.items()):
- self.sim.data.set_joint_qpos(obj_name + "_jnt0", np.concatenate([np.array(obj_pos[i]), np.array(obj_quat[i])]))
-
- # information of objects
- self.object_names = list(self.mujoco_objects.keys())
- self.object_site_ids = [
- self.sim.model.site_name2id(ob_name) for ob_name in self.object_names
- ]
+ for obj_pos, obj_quat, obj in object_placements.values():
+ self.sim.data.set_joint_qpos(obj.joints[0], np.concatenate([np.array(obj_pos), np.array(obj_quat)]))
# Move objects out of the scene depending on the mode
+ nut_names = {nut.name for nut in self.nuts}
if self.single_object_mode == 1:
- self.obj_to_use = (random.choice(self.item_names) + "{}").format(0)
- self.clear_objects(self.obj_to_use)
+ self.obj_to_use = random.choice(list(nut_names))
elif self.single_object_mode == 2:
- self.obj_to_use = (self.item_names[self.nut_id] + "{}").format(0)
- self.clear_objects(self.obj_to_use)
+ self.obj_to_use = self.nuts[self.nut_id].name
+ if self.single_object_mode in {1, 2}:
+ nut_names.remove(self.obj_to_use)
+ self.clear_objects(list(nut_names))
def _get_observation(self):
"""
@@ -582,13 +506,13 @@ def _get_observation(self):
gripper_pose = T.pose2mat((di[pr + "eef_pos"], di[pr + "eef_quat"]))
world_pose_in_gripper = T.pose_inv(gripper_pose)
- for i in range(len(self.item_names_org)):
+ for i, nut in enumerate(self.nuts):
if self.single_object_mode == 2 and self.nut_id != i:
# skip observations
continue
- obj_str = str(self.item_names_org[i]) + "0"
+ obj_str = nut.name
obj_pos = np.array(self.sim.data.body_xpos[self.obj_body_id[obj_str]])
obj_quat = T.convert_quat(
self.sim.data.body_xquat[self.obj_body_id[obj_str]], to="xyzw"
@@ -609,14 +533,14 @@ def _get_observation(self):
if self.single_object_mode == 1:
# zero out other objs
- for obj_str, obj_mjcf in self.mujoco_objects.items():
- if obj_str == self.obj_to_use:
+ for obj in self.model.mujoco_objects:
+ if obj.name == self.obj_to_use:
continue
else:
- di["{}_pos".format(obj_str)] *= 0.0
- di["{}_quat".format(obj_str)] *= 0.0
- di["{}_to_{}eef_pos".format(obj_str, pr)] *= 0.0
- di["{}_to_{}eef_quat".format(obj_str, pr)] *= 0.0
+ di["{}_pos".format(obj.name)] *= 0.0
+ di["{}_quat".format(obj.name)] *= 0.0
+ di["{}_to_{}eef_pos".format(obj.name, pr)] *= 0.0
+ di["{}_to_{}eef_quat".format(obj.name, pr)] *= 0.0
di["object-state"] = np.concatenate([di[k] for k in object_state_keys])
@@ -631,8 +555,8 @@ def _check_success(self):
"""
# remember objects that are on the correct pegs
gripper_site_pos = self.sim.data.site_xpos[self.robots[0].eef_site_id]
- for i in range(len(self.ob_inits)):
- obj_str = str(self.item_names[i]) + "0"
+ for i, nut in enumerate(self.nuts):
+ obj_str = nut.name
obj_pos = self.sim.data.body_xpos[self.obj_body_id[obj_str]]
dist = np.linalg.norm(gripper_site_pos - obj_pos)
r_reach = 1 - np.tanh(10.0 * dist)
@@ -642,47 +566,38 @@ def _check_success(self):
return np.sum(self.objects_on_pegs) > 0 # need one object on peg
# returns True if all objects are on correct pegs
- return np.sum(self.objects_on_pegs) == len(self.ob_inits)
+ return np.sum(self.objects_on_pegs) == len(self.nuts)
- def _visualization(self):
+ def visualize(self, vis_settings):
"""
- Do any needed visualization here. Overrides superclass implementations.
+ In addition to super call, visualize gripper site proportional to the distance to the closest nut.
+
+ Args:
+ vis_settings (dict): Visualization keywords mapped to T/F, determining whether that specific
+ component should be visualized. Should have "grippers" keyword as well as any other relevant
+ options specified.
"""
+ # Run superclass method first
+ super().visualize(vis_settings=vis_settings)
- # color the gripper site appropriately based on distance to cube
- if self.robots[0].gripper_visualization:
+ # Color the gripper visualization site according to its distance to the closest nut
+ if vis_settings["grippers"]:
# find closest object
- square_dist = lambda x: np.sum(
- np.square(x - self.sim.data.get_site_xpos(self.robots[0].gripper.visualization_sites["grip_site"]))
+ dists = [
+ self._gripper_to_target(
+ gripper=self.robots[0].gripper,
+ target=nut.important_sites["handle"],
+ target_type="site",
+ return_distance=True,
+ ) for nut in self.nuts
+ ]
+ closest_nut_id = np.argmin(dists)
+ # Visualize the distance to this target
+ self._visualize_gripper_to_target(
+ gripper=self.robots[0].gripper,
+ target=self.nuts[closest_nut_id].important_sites["handle"],
+ target_type="site",
)
- dists = np.array(list(map(square_dist, self.sim.data.site_xpos)))
- dists[self.robots[0].eef_site_id] = np.inf # make sure we don't pick the same site
- dists[self.robots[0].eef_cylinder_id] = np.inf
- ob_dists = dists[
- self.object_site_ids
- ] # filter out object sites we care about
- min_dist = np.min(ob_dists)
- ob_id = np.argmin(ob_dists)
-
- # set RGBA for the EEF site here
- max_dist = 0.1
- scaled = (1.0 - min(min_dist / max_dist, 1.)) ** 15
- rgba = np.zeros(4)
- rgba[0] = 1 - scaled
- rgba[1] = scaled
- rgba[3] = 0.5
-
- self.sim.model.site_rgba[self.robots[0].eef_site_id] = rgba
-
- def _check_robot_configuration(self, robots):
- """
- Sanity check to make sure the inputted robots and configuration is acceptable
-
- Args:
- robots (str or list of str): Robots to instantiate within this env
- """
- if type(robots) is list:
- assert len(robots) == 1, "Error: Only one robot should be inputted for this task!"
class NutAssemblySingle(NutAssembly):
diff --git a/robosuite/environments/pick_place.py b/robosuite/environments/manipulation/pick_place.py
similarity index 67%
rename from robosuite/environments/pick_place.py
rename to robosuite/environments/manipulation/pick_place.py
index 35d960c672..c163d82c8d 100644
--- a/robosuite/environments/pick_place.py
+++ b/robosuite/environments/manipulation/pick_place.py
@@ -3,8 +3,7 @@
import numpy as np
import robosuite.utils.transform_utils as T
-from robosuite.environments.robot_env import RobotEnv
-from robosuite.robots import SingleArm
+from robosuite.environments.manipulation.single_arm_env import SingleArmEnv
from robosuite.models.arenas import BinsArena
from robosuite.models.objects import (
@@ -19,10 +18,11 @@
CerealVisualObject,
CanVisualObject,
)
-from robosuite.models.tasks import ManipulationTask, SequentialCompositeSampler
+from robosuite.models.tasks import ManipulationTask
+from robosuite.utils.placement_samplers import SequentialCompositeSampler, UniformRandomSampler
-class PickPlace(RobotEnv):
+class PickPlace(SingleArmEnv):
"""
This class corresponds to the pick place task for a single robot arm.
@@ -31,6 +31,9 @@ class PickPlace(RobotEnv):
(e.g: "Sawyer" would generate one arm; ["Panda", "Panda", "Sawyer"] would generate three robot arms)
Note: Must be a single single-arm robot!
+ env_configuration (str): Specifies how to position the robots within the environment (default is "default").
+ For most single arm environments, this argument has no impact on the robot setup.
+
controller_configs (str or list of dict): If set, contains relevant controller parameters for creating a
custom controller. Else, uses the default controller for this specific task. Should either be single
dict if same controller is to be used for all robots or else it should be a list of the same length as
@@ -42,10 +45,6 @@ class PickPlace(RobotEnv):
overrides the default gripper. Should either be single str if same gripper type is to be used for all
robots or else it should be a list of the same length as "robots" param
- gripper_visualizations (bool or list of bool): True if using gripper visualization.
- Useful for teleoperation. Should either be single bool if gripper visualization is to be used for all
- robots or else it should be a list of the same length as "robots" param
-
initialization_noise (dict or list of dict): Dict containing the initialization noise parameters.
The expected keys and corresponding value types are specified below:
@@ -96,9 +95,6 @@ class PickPlace(RobotEnv):
or "can". Determines which type of object will be spawned on every
environment reset. Only used if @single_object_mode is 2.
- use_indicator_object (bool): if True, sets up an indicator object that
- is useful for debugging.
-
has_renderer (bool): If true, render the simulation state in
a viewer instead of headless mode.
@@ -112,6 +108,10 @@ class PickPlace(RobotEnv):
render_visual_mesh (bool): True if rendering visual meshes in camera. False otherwise.
+ render_gpu_device_id (int): corresponds to the GPU device id to use for offscreen rendering.
+ Defaults to -1, in which case the device will be inferred from environment variables
+ (GPUS or CUDA_VISIBLE_DEVICES).
+
control_freq (float): how many control signals to receive in every second. This sets the amount of
simulation time that passes between every action input.
@@ -151,9 +151,9 @@ class PickPlace(RobotEnv):
def __init__(
self,
robots,
+ env_configuration="default",
controller_configs=None,
gripper_types="default",
- gripper_visualizations=False,
initialization_noise="default",
table_full_size=(0.39, 0.49, 0.82),
table_friction=(1, 0.005, 0.0001),
@@ -165,13 +165,13 @@ def __init__(
reward_shaping=False,
single_object_mode=0,
object_type=None,
- use_indicator_object=False,
has_renderer=False,
has_offscreen_renderer=True,
render_camera="frontview",
render_collision_mesh=False,
render_visual_mesh=True,
- control_freq=10,
+ render_gpu_device_id=-1,
+ control_freq=20,
horizon=1000,
ignore_done=False,
hard_reset=True,
@@ -180,12 +180,10 @@ def __init__(
camera_widths=256,
camera_depths=False,
):
- # First, verify that only one robot is being inputted
- self._check_robot_configuration(robots)
-
# task settings
self.single_object_mode = single_object_mode
self.object_to_id = {"milk": 0, "bread": 1, "cereal": 2, "can": 3}
+ self.obj_names = ["Milk", "Bread", "Cereal", "Can"]
if object_type is not None:
assert (
object_type in self.object_to_id.keys()
@@ -214,17 +212,18 @@ def __init__(
super().__init__(
robots=robots,
+ env_configuration=env_configuration,
controller_configs=controller_configs,
+ mount_types="default",
gripper_types=gripper_types,
- gripper_visualizations=gripper_visualizations,
initialization_noise=initialization_noise,
use_camera_obs=use_camera_obs,
- use_indicator_object=use_indicator_object,
has_renderer=has_renderer,
has_offscreen_renderer=has_offscreen_renderer,
render_camera=render_camera,
render_collision_mesh=render_collision_mesh,
render_visual_mesh=render_visual_mesh,
+ render_gpu_device_id=render_gpu_device_id,
control_freq=control_freq,
horizon=horizon,
ignore_done=ignore_done,
@@ -296,79 +295,66 @@ def staged_rewards(self):
hover_mult = 0.7
# filter out objects that are already in the correct bins
- objs_to_reach = []
- geoms_to_grasp = []
- target_bin_placements = []
- for i in range(len(self.ob_inits)):
+ active_objs = []
+ for i, obj in enumerate(self.objects):
if self.objects_in_bins[i]:
continue
- obj_str = str(self.item_names[i]) + "0"
- objs_to_reach.append(self.obj_body_id[obj_str])
- geoms_to_grasp.append(self.obj_geom_id[obj_str])
- target_bin_placements.append(self.target_bin_placements[i])
- target_bin_placements = np.array(target_bin_placements)
+ active_objs.append(obj)
- ### reaching reward governed by distance to closest object ###
+ # reaching reward governed by distance to closest object
r_reach = 0.
- if len(objs_to_reach):
+ if active_objs:
# get reaching reward via minimum distance to a target object
- target_object_pos = self.sim.data.body_xpos[objs_to_reach]
- gripper_site_pos = self.sim.data.site_xpos[self.robots[0].eef_site_id]
- dists = np.linalg.norm(
- target_object_pos - gripper_site_pos.reshape(1, -1), axis=1
- )
+ dists = [
+ self._gripper_to_target(
+ gripper=self.robots[0].gripper,
+ target=active_obj.root_body,
+ target_type="body",
+ return_distance=True,
+ ) for active_obj in active_objs
+ ]
r_reach = (1 - np.tanh(10.0 * min(dists))) * reach_mult
- ### grasping reward for touching any objects of interest ###
- touch_left_finger = False
- touch_right_finger = False
- for i in range(self.sim.data.ncon):
- c = self.sim.data.contact[i]
- if c.geom1 in geoms_to_grasp:
- bin_id = geoms_to_grasp.index(c.geom1)
- if c.geom2 in self.l_finger_geom_ids:
- touch_left_finger = True
- if c.geom2 in self.r_finger_geom_ids:
- touch_right_finger = True
- elif c.geom2 in geoms_to_grasp:
- bin_id = geoms_to_grasp.index(c.geom2)
- if c.geom1 in self.l_finger_geom_ids:
- touch_left_finger = True
- if c.geom1 in self.r_finger_geom_ids:
- touch_right_finger = True
- has_grasp = touch_left_finger and touch_right_finger
- r_grasp = int(has_grasp) * grasp_mult
-
- ### lifting reward for picking up an object ###
+ # grasping reward for touching any objects of interest
+ r_grasp = int(self._check_grasp(
+ gripper=self.robots[0].gripper,
+ object_geoms=[g for active_obj in active_objs for g in active_obj.contact_geoms])
+ ) * grasp_mult
+
+ # lifting reward for picking up an object
r_lift = 0.
- if len(objs_to_reach) and r_grasp > 0.:
+ if active_objs and r_grasp > 0.:
z_target = self.bin2_pos[2] + 0.25
- object_z_locs = self.sim.data.body_xpos[objs_to_reach][:, 2]
+ object_z_locs = self.sim.data.body_xpos[[self.obj_body_id[active_obj.name]
+ for active_obj in active_objs]][:, 2]
z_dists = np.maximum(z_target - object_z_locs, 0.)
r_lift = grasp_mult + (1 - np.tanh(15.0 * min(z_dists))) * (
lift_mult - grasp_mult
)
- ### hover reward for getting object above bin ###
+ # hover reward for getting object above bin
r_hover = 0.
- if len(objs_to_reach):
+ if active_objs:
+ target_bin_ids = [self.object_to_id[active_obj.name.lower()] for active_obj in active_objs]
# segment objects into left of the bins and above the bins
- object_xy_locs = self.sim.data.body_xpos[objs_to_reach][:, :2]
+ object_xy_locs = self.sim.data.body_xpos[[self.obj_body_id[active_obj.name]
+ for active_obj in active_objs]][:, :2]
y_check = (
- np.abs(object_xy_locs[:, 1] - target_bin_placements[:, 1])
+ np.abs(object_xy_locs[:, 1] - self.target_bin_placements[target_bin_ids, 1])
< self.bin_size[1] / 4.
)
x_check = (
- np.abs(object_xy_locs[:, 0] - target_bin_placements[:, 0])
+ np.abs(object_xy_locs[:, 0] - self.target_bin_placements[target_bin_ids, 0])
< self.bin_size[0] / 4.
)
objects_above_bins = np.logical_and(x_check, y_check)
objects_not_above_bins = np.logical_not(objects_above_bins)
dists = np.linalg.norm(
- target_bin_placements[:, :2] - object_xy_locs, axis=1
+ self.target_bin_placements[target_bin_ids, :2] - object_xy_locs, axis=1
)
- # objects to the left get r_lift added to hover reward, those on the right get max(r_lift) added (to encourage dropping)
- r_hover_all = np.zeros(len(objs_to_reach))
+ # objects to the left get r_lift added to hover reward,
+ # those on the right get max(r_lift) added (to encourage dropping)
+ r_hover_all = np.zeros(len(active_objs))
r_hover_all[objects_above_bins] = lift_mult + (
1 - np.tanh(10.0 * dists[objects_above_bins])
) * (hover_mult - lift_mult)
@@ -400,52 +386,35 @@ def not_in_bin(self, obj_pos, bin_id):
res = False
return res
- def clear_objects(self, obj):
- """
- Clears objects without the name @obj out of the task space. This is useful
- for supporting task modes with single types of objects, as in
- @self.single_object_mode without changing the model definition.
-
- Args:
- obj (str): Name of object to keep in the task space
- """
- for obj_name, obj_mjcf in self.mujoco_objects.items():
- if obj_name == obj:
- continue
- else:
- sim_state = self.sim.get_state()
- # print(self.sim.model.get_joint_qpos_addr(obj_name))
- sim_state.qpos[self.sim.model.get_joint_qpos_addr(obj_name + "_jnt0")[0]] = 10
- self.sim.set_state(sim_state)
- self.sim.forward()
-
def _get_placement_initializer(self):
"""
Helper function for defining placement initializer and object sampling bounds.
"""
- self.placement_initializer = SequentialCompositeSampler()
+ self.placement_initializer = SequentialCompositeSampler(name="ObjectSampler")
# can sample anywhere in bin
- bin_x_half = self.mujoco_arena.table_full_size[0] / 2 - 0.05
- bin_y_half = self.mujoco_arena.table_full_size[1] / 2 - 0.05
+ bin_x_half = self.model.mujoco_arena.table_full_size[0] / 2 - 0.05
+ bin_y_half = self.model.mujoco_arena.table_full_size[1] / 2 - 0.05
# each object should just be sampled in the bounds of the bin (with some tolerance)
- for obj_name in self.mujoco_objects:
-
- self.placement_initializer.sample_on_top(
- obj_name,
- surface_name="table",
+ self.placement_initializer.append_sampler(
+ sampler=UniformRandomSampler(
+ name="CollisionObjectSampler",
+ mujoco_objects=self.objects,
x_range=[-bin_x_half, bin_x_half],
y_range=[-bin_y_half, bin_y_half],
rotation=None,
rotation_axis='z',
- z_offset=0.,
ensure_object_boundary_in_range=True,
+ ensure_valid_placement=True,
+ reference_pos=self.bin1_pos,
+ z_offset=0.,
)
+ )
# each visual object should just be at the center of each target bin
index = 0
- for obj_name in self.visual_objects:
+ for vis_obj in self.visual_objects:
# get center of target bin
bin_x_low = self.bin2_pos[0]
@@ -464,17 +433,20 @@ def _get_placement_initializer(self):
# placement is relative to object bin, so compute difference and send to placement initializer
rel_center = bin_center - self.bin1_pos[:2]
- self.placement_initializer.sample_on_top(
- obj_name,
- surface_name="table",
- x_range=[rel_center[0], rel_center[0]],
- y_range=[rel_center[1], rel_center[1]],
- rotation=0.,
- rotation_axis='z',
- z_offset=self.bin2_pos[2] - self.bin1_pos[2],
- ensure_object_boundary_in_range=False,
+ self.placement_initializer.append_sampler(
+ sampler=UniformRandomSampler(
+ name=f"{vis_obj.name}ObjectSampler",
+ mujoco_objects=vis_obj,
+ x_range=[rel_center[0], rel_center[0]],
+ y_range=[rel_center[1], rel_center[1]],
+ rotation=0.,
+ rotation_axis='z',
+ ensure_object_boundary_in_range=False,
+ ensure_valid_placement=False,
+ reference_pos=self.bin1_pos,
+ z_offset=self.bin2_pos[2] - self.bin1_pos[2],
+ )
)
-
index += 1
def _load_model(self):
@@ -483,77 +455,49 @@ def _load_model(self):
"""
super()._load_model()
- # Verify the correct robot has been loaded
- assert isinstance(self.robots[0], SingleArm), \
- "Error: Expected one single-armed robot! Got {} type instead.".format(type(self.robots[0]))
-
# Adjust base pose accordingly
xpos = self.robots[0].robot_model.base_xpos_offset["bins"]
self.robots[0].robot_model.set_base_xpos(xpos)
# load model for table top workspace
- self.mujoco_arena = BinsArena(
+ mujoco_arena = BinsArena(
bin1_pos=self.bin1_pos,
table_full_size=self.table_full_size,
table_friction=self.table_friction
)
- if self.use_indicator_object:
- self.mujoco_arena.add_pos_indicator()
# Arena always gets set to zero origin
- self.mujoco_arena.set_origin([0, 0, 0])
+ mujoco_arena.set_origin([0, 0, 0])
# store some arena attributes
- self.bin_size = self.mujoco_arena.table_full_size
-
- # define mujoco objects
- self.ob_inits = [MilkObject, BreadObject, CerealObject, CanObject]
- self.vis_inits = [
- MilkVisualObject,
- BreadVisualObject,
- CerealVisualObject,
- CanVisualObject,
- ]
- self.item_names = ["Milk", "Bread", "Cereal", "Can"]
- self.item_names_org = list(self.item_names)
- self.obj_to_use = (self.item_names[0] + "{}").format(0)
-
- lst = []
- for j in range(len(self.vis_inits)):
- visual_ob_name = ("Visual" + self.item_names[j] + "0")
- visual_ob = self.vis_inits[j](
- name=visual_ob_name,
- joints=[], # no free joint for visual objects
- )
- lst.append((visual_ob_name, visual_ob))
- self.visual_objects = OrderedDict(lst)
-
- lst = []
- for i in range(len(self.ob_inits)):
- ob_name = (self.item_names[i] + "0")
- ob = self.ob_inits[i](
- name=ob_name,
- joints=[dict(type="free", damping="0.0005")], # damp the free joint for each object
- )
- lst.append((ob_name, ob))
+ self.bin_size = mujoco_arena.table_full_size
+
+ self.objects = []
+ self.visual_objects = []
+ for vis_obj_cls, obj_name in zip(
+ (MilkVisualObject, BreadVisualObject, CerealVisualObject, CanVisualObject),
+ self.obj_names,
+ ):
+ vis_name = "Visual" + obj_name
+ vis_obj = vis_obj_cls(name=vis_name)
+ self.visual_objects.append(vis_obj)
- self.mujoco_objects = OrderedDict(lst)
- self.n_objects = len(self.mujoco_objects)
+ for obj_cls, obj_name in zip(
+ (MilkObject, BreadObject, CerealObject, CanObject),
+ self.obj_names,
+ ):
+ obj = obj_cls(name=obj_name)
+ self.objects.append(obj)
# task includes arena, robot, and objects of interest
- self._get_placement_initializer()
self.model = ManipulationTask(
- mujoco_arena=self.mujoco_arena,
+ mujoco_arena=mujoco_arena,
mujoco_robots=[robot.robot_model for robot in self.robots],
- mujoco_objects=self.mujoco_objects,
- visual_objects=self.visual_objects,
- initializer=self.placement_initializer,
+ mujoco_objects=self.visual_objects + self.objects,
)
- # set positions of objects
- self.model.place_objects()
-
- # self.model.place_visual()
+ # Generate placement initializer
+ self._get_placement_initializer()
def _get_reference(self):
"""
@@ -567,31 +511,18 @@ def _get_reference(self):
self.obj_body_id = {}
self.obj_geom_id = {}
- # id of grippers for contact checking
- self.l_finger_geom_ids = [
- self.sim.model.geom_name2id(x) for x in self.robots[0].gripper.important_geoms["left_finger"]
- ]
- self.r_finger_geom_ids = [
- self.sim.model.geom_name2id(x) for x in self.robots[0].gripper.important_geoms["right_finger"]
- ]
-
# object-specific ids
- for i in range(len(self.ob_inits)):
- obj_str = str(self.item_names[i]) + "0"
- self.obj_body_id[obj_str] = self.sim.model.body_name2id(obj_str)
- self.obj_geom_id[obj_str] = self.sim.model.geom_name2id(obj_str)
-
- # for checking distance to / contact with objects we want to pick up
- self.target_object_body_ids = list(map(int, self.obj_body_id.values()))
- self.contact_with_object_geom_ids = list(map(int, self.obj_geom_id.values()))
+ for obj in (self.visual_objects + self.objects):
+ self.obj_body_id[obj.name] = self.sim.model.body_name2id(obj.root_body)
+ self.obj_geom_id[obj.name] = [self.sim.model.geom_name2id(g) for g in obj.contact_geoms]
# keep track of which objects are in their corresponding bins
- self.objects_in_bins = np.zeros(len(self.ob_inits))
+ self.objects_in_bins = np.zeros(len(self.objects))
# target locations in bin for each object type
- self.target_bin_placements = np.zeros((len(self.ob_inits), 3))
- for j in range(len(self.ob_inits)):
- bin_id = j
+ self.target_bin_placements = np.zeros((len(self.objects), 3))
+ for i, obj in enumerate(self.objects):
+ bin_id = i
bin_x_low = self.bin2_pos[0]
bin_y_low = self.bin2_pos[1]
if bin_id == 0 or bin_id == 2:
@@ -600,7 +531,7 @@ def _get_reference(self):
bin_y_low -= self.bin_size[1] / 2.
bin_x_low += self.bin_size[0] / 4.
bin_y_low += self.bin_size[1] / 4.
- self.target_bin_placements[j, :] = [bin_x_low, bin_y_low, self.bin2_pos[2]]
+ self.target_bin_placements[i, :] = [bin_x_low, bin_y_low, self.bin2_pos[2]]
def _reset_internal(self):
"""
@@ -612,29 +543,31 @@ def _reset_internal(self):
if not self.deterministic_reset:
# Sample from the placement initializer for all objects
- obj_pos, obj_quat = self.model.place_objects()
+ object_placements = self.placement_initializer.sample()
# Loop through all objects and reset their positions
- for i, (obj_name, _) in enumerate(self.mujoco_objects.items()):
- self.sim.data.set_joint_qpos(obj_name + "_jnt0", np.concatenate([np.array(obj_pos[i]), np.array(obj_quat[i])]))
-
- # information of objects
- self.object_names = list(self.mujoco_objects.keys())
- self.object_site_ids = [
- self.sim.model.site_name2id(ob_name) for ob_name in self.object_names
- ]
+ for obj_pos, obj_quat, obj in object_placements.values():
+ # Set the visual object body locations
+ if "visual" in obj.name.lower():
+ self.sim.model.body_pos[self.obj_body_id[obj.name]] = obj_pos
+ self.sim.model.body_quat[self.obj_body_id[obj.name]] = obj_quat
+ else:
+ # Set the collision object joints
+ self.sim.data.set_joint_qpos(obj.joints[0], np.concatenate([np.array(obj_pos), np.array(obj_quat)]))
# Set the bins to the desired position
self.sim.model.body_pos[self.sim.model.body_name2id("bin1")] = self.bin1_pos
self.sim.model.body_pos[self.sim.model.body_name2id("bin2")] = self.bin2_pos
# Move objects out of the scene depending on the mode
+ obj_names = {obj.name for obj in self.objects}
if self.single_object_mode == 1:
- self.obj_to_use = (random.choice(self.item_names) + "{}").format(0)
- self.clear_objects(self.obj_to_use)
+ self.obj_to_use = random.choice(list(obj_names))
elif self.single_object_mode == 2:
- self.obj_to_use = (self.item_names[self.object_id] + "{}").format(0)
- self.clear_objects(self.obj_to_use)
+ self.obj_to_use = self.objects[self.object_id].name
+ if self.single_object_mode in {1, 2}:
+ obj_names.remove(self.obj_to_use)
+ self.clear_objects(list(obj_names))
def _get_observation(self):
"""
@@ -668,13 +601,13 @@ def _get_observation(self):
gripper_pose = T.pose2mat((di[pr + "eef_pos"], di[pr + "eef_quat"]))
world_pose_in_gripper = T.pose_inv(gripper_pose)
- for i in range(len(self.item_names_org)):
+ for i, obj in enumerate(self.objects):
if self.single_object_mode == 2 and self.object_id != i:
# Skip adding to observations
continue
- obj_str = str(self.item_names_org[i]) + "0"
+ obj_str = obj.name
obj_pos = np.array(self.sim.data.body_xpos[self.obj_body_id[obj_str]])
obj_quat = T.convert_quat(
self.sim.data.body_xquat[self.obj_body_id[obj_str]], to="xyzw"
@@ -696,14 +629,14 @@ def _get_observation(self):
if self.single_object_mode == 1:
# Zero out other objects observations
- for obj_str, obj_mjcf in self.mujoco_objects.items():
- if obj_str == self.obj_to_use:
+ for obj in self.objects:
+ if obj.name == self.obj_to_use:
continue
else:
- di["{}_pos".format(obj_str)] *= 0.0
- di["{}_quat".format(obj_str)] *= 0.0
- di["{}_to_{}eef_pos".format(obj_str, pr)] *= 0.0
- di["{}_to_{}eef_quat".format(obj_str, pr)] *= 0.0
+ di["{}_pos".format(obj.name)] *= 0.0
+ di["{}_quat".format(obj.name)] *= 0.0
+ di["{}_to_{}eef_pos".format(obj.name, pr)] *= 0.0
+ di["{}_to_{}eef_quat".format(obj.name, pr)] *= 0.0
di["object-state"] = np.concatenate([di[k] for k in object_state_keys])
@@ -718,61 +651,50 @@ def _check_success(self):
"""
# remember objects that are in the correct bins
gripper_site_pos = self.sim.data.site_xpos[self.robots[0].eef_site_id]
- for i in range(len(self.ob_inits)):
- obj_str = str(self.item_names[i]) + "0"
+ for i, obj in enumerate(self.objects):
+ obj_str = obj.name
obj_pos = self.sim.data.body_xpos[self.obj_body_id[obj_str]]
dist = np.linalg.norm(gripper_site_pos - obj_pos)
r_reach = 1 - np.tanh(10.0 * dist)
- self.objects_in_bins[i] = int(
- (not self.not_in_bin(obj_pos, i)) and r_reach < 0.6
- )
+ self.objects_in_bins[i] = int((not self.not_in_bin(obj_pos, i)) and r_reach < 0.6)
# returns True if a single object is in the correct bin
- if self.single_object_mode == 1 or self.single_object_mode == 2:
+ if self.single_object_mode in {1, 2}:
return np.sum(self.objects_in_bins) > 0
# returns True if all objects are in correct bins
- return np.sum(self.objects_in_bins) == len(self.ob_inits)
+ return np.sum(self.objects_in_bins) == len(self.objects)
- def _visualization(self):
+ def visualize(self, vis_settings):
"""
- Do any needed visualization here. Overrides superclass implementations.
+ In addition to super call, visualize gripper site proportional to the distance to the closest object.
+
+ Args:
+ vis_settings (dict): Visualization keywords mapped to T/F, determining whether that specific
+ component should be visualized. Should have "grippers" keyword as well as any other relevant
+ options specified.
"""
+ # Run superclass method first
+ super().visualize(vis_settings=vis_settings)
- # color the gripper site appropriately based on distance to cube
- if self.robots[0].gripper_visualization:
+ # Color the gripper visualization site according to its distance to the closest object
+ if vis_settings["grippers"]:
# find closest object
- square_dist = lambda x: np.sum(
- np.square(x - self.sim.data.get_site_xpos(self.robots[0].gripper.visualization_sites["grip_site"]))
+ dists = [
+ self._gripper_to_target(
+ gripper=self.robots[0].gripper,
+ target=obj.root_body,
+ target_type="body",
+ return_distance=True,
+ ) for obj in self.objects
+ ]
+ closest_obj_id = np.argmin(dists)
+ # Visualize the distance to this target
+ self._visualize_gripper_to_target(
+ gripper=self.robots[0].gripper,
+ target=self.objects[closest_obj_id].root_body,
+ target_type="body",
)
- dists = np.array(list(map(square_dist, self.sim.data.site_xpos)))
- dists[self.robots[0].eef_site_id] = np.inf # make sure we don't pick the same site
- dists[self.robots[0].eef_cylinder_id] = np.inf
- ob_dists = dists[
- self.object_site_ids
- ] # filter out object sites we care about
- min_dist = np.min(ob_dists)
- ob_id = np.argmin(ob_dists)
-
- # set RGBA for the EEF site here
- max_dist = 0.1
- scaled = (1.0 - min(min_dist / max_dist, 1.)) ** 15
- rgba = np.zeros(4)
- rgba[0] = 1 - scaled
- rgba[1] = scaled
- rgba[3] = 0.5
-
- self.sim.model.site_rgba[self.robots[0].eef_site_id] = rgba
-
- def _check_robot_configuration(self, robots):
- """
- Sanity check to make sure the inputted robots and configuration is acceptable
-
- Args:
- robots (str or list of str): Robots to instantiate within this env
- """
- if type(robots) is list:
- assert len(robots) == 1, "Error: Only one robot should be inputted for this task!"
class PickPlaceSingle(PickPlace):
diff --git a/robosuite/environments/manipulation/single_arm_env.py b/robosuite/environments/manipulation/single_arm_env.py
new file mode 100644
index 0000000000..5d11f8703f
--- /dev/null
+++ b/robosuite/environments/manipulation/single_arm_env.py
@@ -0,0 +1,69 @@
+import numpy as np
+
+from robosuite.environments.manipulation.manipulation_env import ManipulationEnv
+from robosuite.robots import SingleArm
+from robosuite.utils.transform_utils import mat2quat
+
+
+class SingleArmEnv(ManipulationEnv):
+ """
+ A manipulation environment intended for a single robot arm.
+ """
+ def _load_model(self):
+ """
+ Verifies correct robot model is loaded
+ """
+ super()._load_model()
+
+ # Verify the correct robot has been loaded
+ assert isinstance(self.robots[0], SingleArm), \
+ "Error: Expected one single-armed robot! Got {} type instead.".format(type(self.robots[0]))
+
+ def _check_robot_configuration(self, robots):
+ """
+ Sanity check to make sure the inputted robots and configuration is acceptable
+
+ Args:
+ robots (str or list of str): Robots to instantiate within this env
+ """
+ super()._check_robot_configuration(robots)
+ if type(robots) is list:
+ assert len(robots) == 1, "Error: Only one robot should be inputted for this task!"
+
+ @property
+ def _eef_xpos(self):
+ """
+ Grabs End Effector position
+
+ Returns:
+ np.array: End effector(x,y,z)
+ """
+ return np.array(self.sim.data.site_xpos[self.robots[0].eef_site_id])
+
+ @property
+ def _eef_xmat(self):
+ """
+ End Effector orientation as a rotation matrix
+ Note that this draws the orientation from the "ee" site, NOT the gripper site, since the gripper
+ orientations are inconsistent!
+
+ Returns:
+ np.array: (3,3) End Effector orientation matrix
+ """
+ pf = self.robots[0].robot_model.naming_prefix
+ if self.env_configuration == "bimanual":
+ return np.array(self.sim.data.site_xmat[self.sim.model.site_name2id(pf + "right_ee")]).reshape(3, 3)
+ else:
+ return np.array(self.sim.data.site_xmat[self.sim.model.site_name2id(pf + "ee")]).reshape(3, 3)
+
+ @property
+ def _eef_xquat(self):
+ """
+ End Effector orientation as a (x,y,z,w) quaternion
+ Note that this draws the orientation from the "ee" site, NOT the gripper site, since the gripper
+ orientations are inconsistent!
+
+ Returns:
+ np.array: (x,y,z,w) End Effector quaternion
+ """
+ return mat2quat(self._eef_xmat)
diff --git a/robosuite/environments/stack.py b/robosuite/environments/manipulation/stack.py
similarity index 72%
rename from robosuite/environments/stack.py
rename to robosuite/environments/manipulation/stack.py
index 8950bf925c..856352684c 100644
--- a/robosuite/environments/stack.py
+++ b/robosuite/environments/manipulation/stack.py
@@ -4,15 +4,15 @@
from robosuite.utils.transform_utils import convert_quat
from robosuite.utils.mjcf_utils import CustomMaterial
-from robosuite.environments.robot_env import RobotEnv
-from robosuite.robots import SingleArm
+from robosuite.environments.manipulation.single_arm_env import SingleArmEnv
from robosuite.models.arenas import TableArena
from robosuite.models.objects import BoxObject
-from robosuite.models.tasks import ManipulationTask, UniformRandomSampler
+from robosuite.models.tasks import ManipulationTask
+from robosuite.utils.placement_samplers import UniformRandomSampler
-class Stack(RobotEnv):
+class Stack(SingleArmEnv):
"""
This class corresponds to the stacking task for a single robot arm.
@@ -21,6 +21,9 @@ class Stack(RobotEnv):
(e.g: "Sawyer" would generate one arm; ["Panda", "Panda", "Sawyer"] would generate three robot arms)
Note: Must be a single single-arm robot!
+ env_configuration (str): Specifies how to position the robots within the environment (default is "default").
+ For most single arm environments, this argument has no impact on the robot setup.
+
controller_configs (str or list of dict): If set, contains relevant controller parameters for creating a
custom controller. Else, uses the default controller for this specific task. Should either be single
dict if same controller is to be used for all robots or else it should be a list of the same length as
@@ -32,10 +35,6 @@ class Stack(RobotEnv):
overrides the default gripper. Should either be single str if same gripper type is to be used for all
robots or else it should be a list of the same length as "robots" param
- gripper_visualizations (bool or list of bool): True if using gripper visualization.
- Useful for teleoperation. Should either be single bool if gripper visualization is to be used for all
- robots or else it should be a list of the same length as "robots" param
-
initialization_noise (dict or list of dict): Dict containing the initialization noise parameters.
The expected keys and corresponding value types are specified below:
@@ -66,13 +65,10 @@ class Stack(RobotEnv):
reward_shaping (bool): if True, use dense rewards.
- placement_initializer (ObjectPositionSampler instance): if provided, will
+ placement_initializer (ObjectPositionSampler): if provided, will
be used to place objects on every reset, else a UniformRandomSampler
is used by default.
- use_indicator_object (bool): if True, sets up an indicator object that
- is useful for debugging.
-
has_renderer (bool): If true, render the simulation state in
a viewer instead of headless mode.
@@ -86,6 +82,10 @@ class Stack(RobotEnv):
render_visual_mesh (bool): True if rendering visual meshes in camera. False otherwise.
+ render_gpu_device_id (int): corresponds to the GPU device id to use for offscreen rendering.
+ Defaults to -1, in which case the device will be inferred from environment variables
+ (GPUS or CUDA_VISIBLE_DEVICES).
+
control_freq (float): how many control signals to receive in every second. This sets the amount of
simulation time that passes between every action input.
@@ -124,9 +124,9 @@ class Stack(RobotEnv):
def __init__(
self,
robots,
+ env_configuration="default",
controller_configs=None,
gripper_types="default",
- gripper_visualizations=False,
initialization_noise="default",
table_full_size=(0.8, 0.8, 0.05),
table_friction=(1., 5e-3, 1e-4),
@@ -135,13 +135,13 @@ def __init__(
reward_scale=1.0,
reward_shaping=False,
placement_initializer=None,
- use_indicator_object=False,
has_renderer=False,
has_offscreen_renderer=True,
render_camera="frontview",
render_collision_mesh=False,
render_visual_mesh=True,
- control_freq=10,
+ render_gpu_device_id=-1,
+ control_freq=20,
horizon=1000,
ignore_done=False,
hard_reset=True,
@@ -150,15 +150,10 @@ def __init__(
camera_widths=256,
camera_depths=False,
):
- """
-
- """
- # First, verify that only one robot is being inputted
- self._check_robot_configuration(robots)
-
# settings for table top
self.table_full_size = table_full_size
self.table_friction = table_friction
+ self.table_offset = np.array((0, 0, 0.8))
# reward configuration
self.reward_scale = reward_scale
@@ -168,29 +163,22 @@ def __init__(
self.use_object_obs = use_object_obs
# object placement initializer
- if placement_initializer:
- self.placement_initializer = placement_initializer
- else:
- self.placement_initializer = UniformRandomSampler(
- x_range=[-0.08, 0.08],
- y_range=[-0.08, 0.08],
- ensure_object_boundary_in_range=False,
- rotation=None,
- )
+ self.placement_initializer = placement_initializer
super().__init__(
robots=robots,
+ env_configuration=env_configuration,
controller_configs=controller_configs,
+ mount_types="default",
gripper_types=gripper_types,
- gripper_visualizations=gripper_visualizations,
initialization_noise=initialization_noise,
use_camera_obs=use_camera_obs,
- use_indicator_object=use_indicator_object,
has_renderer=has_renderer,
has_offscreen_renderer=has_offscreen_renderer,
render_camera=render_camera,
render_collision_mesh=render_collision_mesh,
render_visual_mesh=render_visual_mesh,
+ render_gpu_device_id=render_gpu_device_id,
control_freq=control_freq,
horizon=horizon,
ignore_done=ignore_done,
@@ -256,42 +244,21 @@ def staged_rewards(self):
- (float): reward for lifting and aligning
- (float): reward for stacking
"""
- # reaching is successful when the gripper site is close to
- # the center of the cube
+ # reaching is successful when the gripper site is close to the center of the cube
cubeA_pos = self.sim.data.body_xpos[self.cubeA_body_id]
cubeB_pos = self.sim.data.body_xpos[self.cubeB_body_id]
gripper_site_pos = self.sim.data.site_xpos[self.robots[0].eef_site_id]
dist = np.linalg.norm(gripper_site_pos - cubeA_pos)
r_reach = (1 - np.tanh(10.0 * dist)) * 0.25
- # collision checking
- touch_left_finger = False
- touch_right_finger = False
- touch_cubeA_cubeB = False
-
- for i in range(self.sim.data.ncon):
- c = self.sim.data.contact[i]
- if c.geom1 in self.l_finger_geom_ids and c.geom2 == self.cubeA_geom_id:
- touch_left_finger = True
- if c.geom1 == self.cubeA_geom_id and c.geom2 in self.l_finger_geom_ids:
- touch_left_finger = True
- if c.geom1 in self.r_finger_geom_ids and c.geom2 == self.cubeA_geom_id:
- touch_right_finger = True
- if c.geom1 == self.cubeA_geom_id and c.geom2 in self.r_finger_geom_ids:
- touch_right_finger = True
- if c.geom1 == self.cubeA_geom_id and c.geom2 == self.cubeB_geom_id:
- touch_cubeA_cubeB = True
- if c.geom1 == self.cubeB_geom_id and c.geom2 == self.cubeA_geom_id:
- touch_cubeA_cubeB = True
-
- # additional grasping reward
- if touch_left_finger and touch_right_finger:
+ # grasping reward
+ grasping_cubeA = self._check_grasp(gripper=self.robots[0].gripper, object_geoms=self.cubeA)
+ if grasping_cubeA:
r_reach += 0.25
- # lifting is successful when the cube is above the table top
- # by a margin
+ # lifting is successful when the cube is above the table top by a margin
cubeA_height = cubeA_pos[2]
- table_height = self.mujoco_arena.table_offset[2]
+ table_height = self.table_offset[2]
cubeA_lifted = cubeA_height > table_height + 0.04
r_lift = 1.0 if cubeA_lifted else 0.0
@@ -302,11 +269,10 @@ def staged_rewards(self):
)
r_lift += 0.5 * (1 - np.tanh(horiz_dist))
- # stacking is successful when the block is lifted and
- # the gripper is not holding the object
+ # stacking is successful when the block is lifted and the gripper is not holding the object
r_stack = 0
- not_touching = not touch_left_finger and not touch_right_finger
- if not_touching and r_lift > 0 and touch_cubeA_cubeB:
+ cubeA_touching_cubeB = self.check_contact(self.cubeA, self.cubeB)
+ if not grasping_cubeA and r_lift > 0 and cubeA_touching_cubeB:
r_stack = 2.0
return r_reach, r_lift, r_stack
@@ -317,25 +283,19 @@ def _load_model(self):
"""
super()._load_model()
- # Verify the correct robot has been loaded
- assert isinstance(self.robots[0], SingleArm), \
- "Error: Expected one single-armed robot! Got {} type instead.".format(type(self.robots[0]))
-
# Adjust base pose accordingly
xpos = self.robots[0].robot_model.base_xpos_offset["table"](self.table_full_size[0])
self.robots[0].robot_model.set_base_xpos(xpos)
# load model for table top workspace
- self.mujoco_arena = TableArena(
+ mujoco_arena = TableArena(
table_full_size=self.table_full_size,
table_friction=self.table_friction,
- table_offset=(0, 0, 0.8),
+ table_offset=self.table_offset,
)
- if self.use_indicator_object:
- self.mujoco_arena.add_pos_indicator()
# Arena always gets set to zero origin
- self.mujoco_arena.set_origin([0, 0, 0])
+ mujoco_arena.set_origin([0, 0, 0])
# initialize objects of interest
tex_attrib = {
@@ -360,30 +320,44 @@ def _load_model(self):
tex_attrib=tex_attrib,
mat_attrib=mat_attrib,
)
- cubeA = BoxObject(
+ self.cubeA = BoxObject(
name="cubeA",
size_min=[0.02, 0.02, 0.02],
size_max=[0.02, 0.02, 0.02],
rgba=[1, 0, 0, 1],
material=redwood,
)
- cubeB = BoxObject(
+ self.cubeB = BoxObject(
name="cubeB",
size_min=[0.025, 0.025, 0.025],
size_max=[0.025, 0.025, 0.025],
rgba=[0, 1, 0, 1],
material=greenwood,
)
- self.mujoco_objects = OrderedDict([("cubeA", cubeA), ("cubeB", cubeB)])
+ cubes = [self.cubeA, self.cubeB]
+ # Create placement initializer
+ if self.placement_initializer is not None:
+ self.placement_initializer.reset()
+ self.placement_initializer.add_objects(cubes)
+ else:
+ self.placement_initializer = UniformRandomSampler(
+ name="ObjectSampler",
+ mujoco_objects=cubes,
+ x_range=[-0.08, 0.08],
+ y_range=[-0.08, 0.08],
+ rotation=None,
+ ensure_object_boundary_in_range=False,
+ ensure_valid_placement=True,
+ reference_pos=self.table_offset,
+ z_offset=0.01,
+ )
# task includes arena, robot, and objects of interest
self.model = ManipulationTask(
- self.mujoco_arena,
- [robot.robot_model for robot in self.robots],
- self.mujoco_objects,
- initializer=self.placement_initializer,
+ mujoco_arena=mujoco_arena,
+ mujoco_robots=[robot.robot_model for robot in self.robots],
+ mujoco_objects=cubes,
)
- self.model.place_objects()
def _get_reference(self):
"""
@@ -394,24 +368,8 @@ def _get_reference(self):
super()._get_reference()
# Additional object references from this env
- self.cubeA_body_id = self.sim.model.body_name2id("cubeA")
- self.cubeB_body_id = self.sim.model.body_name2id("cubeB")
-
- # information of objects
- self.object_names = list(self.mujoco_objects.keys())
- self.object_site_ids = [
- self.sim.model.site_name2id(ob_name) for ob_name in self.object_names
- ]
-
- # id of grippers for contact checking
- self.l_finger_geom_ids = [
- self.sim.model.geom_name2id(x) for x in self.robots[0].gripper.important_geoms["left_finger"]
- ]
- self.r_finger_geom_ids = [
- self.sim.model.geom_name2id(x) for x in self.robots[0].gripper.important_geoms["right_finger"]
- ]
- self.cubeA_geom_id = self.sim.model.geom_name2id("cubeA")
- self.cubeB_geom_id = self.sim.model.geom_name2id("cubeB")
+ self.cubeA_body_id = self.sim.model.body_name2id(self.cubeA.root_body)
+ self.cubeB_body_id = self.sim.model.body_name2id(self.cubeB.root_body)
def _reset_internal(self):
"""
@@ -423,11 +381,11 @@ def _reset_internal(self):
if not self.deterministic_reset:
# Sample from the placement initializer for all objects
- obj_pos, obj_quat = self.model.place_objects()
+ object_placements = self.placement_initializer.sample()
# Loop through all objects and reset their positions
- for i, (obj_name, _) in enumerate(self.mujoco_objects.items()):
- self.sim.data.set_joint_qpos(obj_name + "_jnt0", np.concatenate([np.array(obj_pos[i]), np.array(obj_quat[i])]))
+ for obj_pos, obj_quat, obj in object_placements.values():
+ self.sim.data.set_joint_qpos(obj.joints[0], np.concatenate([np.array(obj_pos), np.array(obj_quat)]))
def _get_observation(self):
"""
@@ -500,42 +458,18 @@ def _check_success(self):
_, _, r_stack = self.staged_rewards()
return r_stack > 0
- def _visualization(self):
- """
- Do any needed visualization here. Overrides superclass implementations.
- """
-
- # color the gripper site appropriately based on distance to cube
- if self.robots[0].gripper_visualization:
- # find closest object
- square_dist = lambda x: np.sum(
- np.square(x - self.sim.data.get_site_xpos(self.robots[0].gripper.visualization_sites["grip_site"]))
- )
- dists = np.array(list(map(square_dist, self.sim.data.site_xpos)))
- dists[self.robots[0].eef_site_id] = np.inf # make sure we don't pick the same site
- dists[self.robots[0].eef_cylinder_id] = np.inf
- ob_dists = dists[
- self.object_site_ids
- ] # filter out object sites we care about
- min_dist = np.min(ob_dists)
-
- # set RGBA for the EEF site here
- max_dist = 0.1
- scaled = (1.0 - min(min_dist / max_dist, 1.)) ** 15
- rgba = np.zeros(4)
- rgba[0] = 1 - scaled
- rgba[1] = scaled
- rgba[3] = 0.5
-
- self.sim.model.site_rgba[self.robots[0].eef_site_id] = rgba
-
- def _check_robot_configuration(self, robots):
+ def visualize(self, vis_settings):
"""
- Sanity check to make sure the inputted robots and configuration is acceptable
+ In addition to super call, visualize gripper site proportional to the distance to the cube.
Args:
- robots (str or list of str): Robots to instantiate within this env
+ vis_settings (dict): Visualization keywords mapped to T/F, determining whether that specific
+ component should be visualized. Should have "grippers" keyword as well as any other relevant
+ options specified.
"""
- if type(robots) is list:
- assert len(robots) == 1, "Error: Only one robot should be inputted for this task!"
+ # Run superclass method first
+ super().visualize(vis_settings=vis_settings)
+ # Color the gripper visualization site according to its distance to the cube
+ if vis_settings["grippers"]:
+ self._visualize_gripper_to_target(gripper=self.robots[0].gripper, target=self.cubeA)
diff --git a/robosuite/environments/manipulation/two_arm_env.py b/robosuite/environments/manipulation/two_arm_env.py
new file mode 100644
index 0000000000..1b2cb79a3d
--- /dev/null
+++ b/robosuite/environments/manipulation/two_arm_env.py
@@ -0,0 +1,130 @@
+import numpy as np
+
+from robosuite.environments.manipulation.manipulation_env import ManipulationEnv
+from robosuite.utils.robot_utils import check_bimanual
+from robosuite.utils.transform_utils import mat2quat
+
+
+class TwoArmEnv(ManipulationEnv):
+ """
+ A manipulation environment intended for two robot arms.
+ """
+ def _check_robot_configuration(self, robots):
+ """
+ Sanity check to make sure the inputted robots and configuration is acceptable
+
+ Args:
+ robots (str or list of str): Robots to instantiate within this env
+ """
+ super()._check_robot_configuration(robots)
+ robots = robots if type(robots) == list or type(robots) == tuple else [robots]
+ # If default config is used, set env_configuration accordingly
+ if self.env_configuration == "default":
+ self.env_configuration = "bimanual" if check_bimanual(robots[0]) else "single-arm-opposed"
+
+ if self.env_configuration == "single-arm-opposed" or self.env_configuration == "single-arm-parallel":
+ # Specifically two robots should be inputted!
+ is_bimanual = False
+ if type(robots) is not list or len(robots) != 2:
+ raise ValueError("Error: Exactly two single-armed robots should be inputted "
+ "for this task configuration!")
+ elif self.env_configuration == "bimanual":
+ is_bimanual = True
+ # Specifically one robot should be inputted!
+ if type(robots) is list and len(robots) != 1:
+ raise ValueError("Error: Exactly one bimanual robot should be inputted "
+ "for this task configuration!")
+ else:
+ # This is an unknown env configuration, print error
+ raise ValueError("Error: Unknown environment configuration received. Only 'bimanual',"
+ "'single-arm-parallel', and 'single-arm-opposed' are supported. Got: {}"
+ .format(self.env_configuration))
+
+ # Lastly, check to make sure all inputted robot names are of their correct type (bimanual / not bimanual)
+ for robot in robots:
+ if check_bimanual(robot) != is_bimanual:
+ raise ValueError("Error: For {} configuration, expected bimanual check to return {}; "
+ "instead, got {}.".format(self.env_configuration, is_bimanual, check_bimanual(robot)))
+
+ @property
+ def _eef0_xpos(self):
+ """
+ Grab the position of Robot 0's end effector.
+
+ Returns:
+ np.array: (x,y,z) position of EEF0
+ """
+ if self.env_configuration == "bimanual":
+ return np.array(self.sim.data.site_xpos[self.robots[0].eef_site_id["right"]])
+ else:
+ return np.array(self.sim.data.site_xpos[self.robots[0].eef_site_id])
+
+ @property
+ def _eef1_xpos(self):
+ """
+ Grab the position of Robot 1's end effector.
+
+ Returns:
+ np.array: (x,y,z) position of EEF1
+ """
+ if self.env_configuration == "bimanual":
+ return np.array(self.sim.data.site_xpos[self.robots[0].eef_site_id["left"]])
+ else:
+ return np.array(self.sim.data.site_xpos[self.robots[1].eef_site_id])
+
+ @property
+ def _eef0_xmat(self):
+ """
+ End Effector 0 orientation as a rotation matrix
+ Note that this draws the orientation from the "ee" site, NOT the gripper site, since the gripper
+ orientations are inconsistent!
+
+ Returns:
+ np.array: (3,3) orientation matrix for EEF0
+ """
+ pf = self.robots[0].robot_model.naming_prefix
+ if self.env_configuration == "bimanual":
+ return np.array(self.sim.data.site_xmat[self.sim.model.site_name2id(pf + "right_ee")]).reshape(3, 3)
+ else:
+ return np.array(self.sim.data.site_xmat[self.sim.model.site_name2id(pf + "ee")]).reshape(3, 3)
+
+ @property
+ def _eef1_xmat(self):
+ """
+ End Effector 1 orientation as a rotation matrix
+ Note that this draws the orientation from the "ee" site, NOT the gripper site, since the gripper
+ orientations are inconsistent!
+
+ Returns:
+ np.array: (3,3) orientation matrix for EEF1
+ """
+ if self.env_configuration == "bimanual":
+ pf = self.robots[0].robot_model.naming_prefix
+ return np.array(self.sim.data.site_xmat[self.sim.model.site_name2id(pf + "left_ee")]).reshape(3, 3)
+ else:
+ pf = self.robots[1].robot_model.naming_prefix
+ return np.array(self.sim.data.site_xmat[self.sim.model.site_name2id(pf + "ee")]).reshape(3, 3)
+
+ @property
+ def _eef0_xquat(self):
+ """
+ End Effector 0 orientation as a (x,y,z,w) quaternion
+ Note that this draws the orientation from the "ee" site, NOT the gripper site, since the gripper
+ orientations are inconsistent!
+
+ Returns:
+ np.array: (x,y,z,w) quaternion for EEF0
+ """
+ return mat2quat(self._eef0_xmat)
+
+ @property
+ def _eef1_xquat(self):
+ """
+ End Effector 1 orientation as a (x,y,z,w) quaternion
+ Note that this draws the orientation from the "ee" site, NOT the gripper site, since the gripper
+ orientations are inconsistent!
+
+ Returns:
+ np.array: (x,y,z,w) quaternion for EEF1
+ """
+ return mat2quat(self._eef1_xmat)
diff --git a/robosuite/environments/two_arm_handover.py b/robosuite/environments/manipulation/two_arm_handover.py
similarity index 69%
rename from robosuite/environments/two_arm_handover.py
rename to robosuite/environments/manipulation/two_arm_handover.py
index 1bae2d8d78..dbb6fac7bb 100644
--- a/robosuite/environments/two_arm_handover.py
+++ b/robosuite/environments/manipulation/two_arm_handover.py
@@ -1,17 +1,17 @@
from collections import OrderedDict
import numpy as np
-from robosuite.environments.robot_env import RobotEnv
+from robosuite.environments.manipulation.two_arm_env import TwoArmEnv
from robosuite.models.arenas import TableArena
from robosuite.models.objects import HammerObject
-from robosuite.models.tasks import ManipulationTask, UniformRandomSampler
-from robosuite.models.robots import check_bimanual
+from robosuite.models.tasks import ManipulationTask
+from robosuite.utils.placement_samplers import UniformRandomSampler
import robosuite.utils.transform_utils as T
-class TwoArmHandover(RobotEnv):
+class TwoArmHandover(TwoArmEnv):
"""
This class corresponds to the handover task for two robot arms.
@@ -27,7 +27,10 @@ class TwoArmHandover(RobotEnv):
:`'single-arm-parallel'`: Only applicable for multi single arm setups. Sets up the (two) single armed
robots next to each other on the -x side of the table
:`'single-arm-opposed'`: Only applicable for multi single arm setups. Sets up the (two) single armed
- robots opposed from each others on the opposite +/-y sides of the table (Default option)
+ robots opposed from each others on the opposite +/-y sides of the table.
+
+ Note that "default" corresponds to either "bimanual" if a bimanual robot is used or "single-arm-opposed" if two
+ single-arm robots are used.
controller_configs (str or list of dict): If set, contains relevant controller parameters for creating a
custom controller. Else, uses the default controller for this specific task. Should either be single
@@ -40,10 +43,6 @@ class TwoArmHandover(RobotEnv):
overrides the default gripper. Should either be single str if same gripper type is to be used for all
robots or else it should be a list of the same length as "robots" param
- gripper_visualizations (bool or list of bool): True if using gripper visualization.
- Useful for teleoperation. Should either be single bool if gripper visualization is to be used for all
- robots or else it should be a list of the same length as "robots" param
-
initialization_noise (dict or list of dict): Dict containing the initialization noise parameters.
The expected keys and corresponding value types are specified below:
@@ -76,13 +75,10 @@ class TwoArmHandover(RobotEnv):
reward_shaping (bool): if True, use dense rewards.
- placement_initializer (ObjectPositionSampler instance): if provided, will
+ placement_initializer (ObjectPositionSampler): if provided, will
be used to place objects on every reset, else a UniformRandomSampler
is used by default.
- use_indicator_object (bool): if True, sets up an indicator object that
- is useful for debugging.
-
has_renderer (bool): If true, render the simulation state in
a viewer instead of headless mode.
@@ -96,6 +92,10 @@ class TwoArmHandover(RobotEnv):
render_visual_mesh (bool): True if rendering visual meshes in camera. False otherwise.
+ render_gpu_device_id (int): corresponds to the GPU device id to use for offscreen rendering.
+ Defaults to -1, in which case the device will be inferred from environment variables
+ (GPUS or CUDA_VISIBLE_DEVICES).
+
control_freq (float): how many control signals to receive in every second. This sets the amount of
simulation time that passes between every action input.
@@ -139,7 +139,6 @@ def __init__(
env_configuration="single-arm-opposed",
controller_configs=None,
gripper_types="default",
- gripper_visualizations=False,
initialization_noise="default",
prehensile=True,
table_full_size=(0.8, 1.2, 0.05),
@@ -149,13 +148,13 @@ def __init__(
reward_scale=1.0,
reward_shaping=False,
placement_initializer=None,
- use_indicator_object=False,
has_renderer=False,
has_offscreen_renderer=True,
render_camera="frontview",
render_collision_mesh=False,
render_visual_mesh=True,
- control_freq=10,
+ render_gpu_device_id=-1,
+ control_freq=20,
horizon=1000,
ignore_done=False,
hard_reset=True,
@@ -164,10 +163,6 @@ def __init__(
camera_widths=256,
camera_depths=False,
):
- # First, verify that correct number of robots are being inputted
- self.env_configuration = env_configuration
- self._check_robot_configuration(robots)
-
# Task settings
self.prehensile = prehensile
@@ -187,32 +182,22 @@ def __init__(
self.use_object_obs = use_object_obs
# object placement initializer
- if placement_initializer:
- self.placement_initializer = placement_initializer
- else:
- # Set rotation about y-axis if hammer starts on table else rotate about z if it starts in gripper
- rotation_axis = 'y' if self.prehensile else 'z'
- self.placement_initializer = UniformRandomSampler(
- x_range=[-0.1, 0.1],
- y_range=[-0.05, 0.05],
- ensure_object_boundary_in_range=False,
- rotation=None,
- rotation_axis=rotation_axis,
- )
+ self.placement_initializer = placement_initializer
super().__init__(
robots=robots,
+ env_configuration=env_configuration,
controller_configs=controller_configs,
+ mount_types="default",
gripper_types=gripper_types,
- gripper_visualizations=gripper_visualizations,
initialization_noise=initialization_noise,
use_camera_obs=use_camera_obs,
- use_indicator_object=use_indicator_object,
has_renderer=has_renderer,
has_offscreen_renderer=has_offscreen_renderer,
render_camera=render_camera,
render_collision_mesh=render_collision_mesh,
render_visual_mesh=render_visual_mesh,
+ render_gpu_device_id=render_gpu_device_id,
control_freq=control_freq,
horizon=horizon,
ignore_done=ignore_done,
@@ -330,30 +315,50 @@ def _load_model(self):
robot.robot_model.set_base_xpos(xpos)
# load model for table top workspace
- self.mujoco_arena = TableArena(
+ mujoco_arena = TableArena(
table_full_size=self.table_true_size,
table_friction=self.table_friction,
table_offset=self.table_offset
)
- if self.use_indicator_object:
- self.mujoco_arena.add_pos_indicator()
# Arena always gets set to zero origin
- self.mujoco_arena.set_origin([0, 0, 0])
+ mujoco_arena.set_origin([0, 0, 0])
+
+ # Modify default agentview camera
+ mujoco_arena.set_camera(
+ camera_name="agentview",
+ pos=[0.8894354364730311, -3.481824231498976e-08, 1.7383813133506494],
+ quat=[0.6530981063842773, 0.2710406184196472, 0.27104079723358154, 0.6530979871749878]
+ )
# initialize objects of interest
self.hammer = HammerObject(name="hammer")
- self.mujoco_objects = OrderedDict([("hammer", self.hammer)])
+
+ # Create placement initializer
+ if self.placement_initializer is not None:
+ self.placement_initializer.reset()
+ self.placement_initializer.add_objects(self.hammer)
+ else:
+ # Set rotation about y-axis if hammer starts on table else rotate about z if it starts in gripper
+ rotation_axis = 'y' if self.prehensile else 'z'
+ self.placement_initializer = UniformRandomSampler(
+ name="ObjectSampler",
+ mujoco_objects=self.hammer,
+ x_range=[-0.1, 0.1],
+ y_range=[-0.05, 0.05],
+ rotation=None,
+ rotation_axis=rotation_axis,
+ ensure_object_boundary_in_range=False,
+ ensure_valid_placement=True,
+ reference_pos=self.table_offset,
+ )
# task includes arena, robot, and objects of interest
self.model = ManipulationTask(
- mujoco_arena=self.mujoco_arena,
+ mujoco_arena=mujoco_arena,
mujoco_robots=[robot.robot_model for robot in self.robots],
- mujoco_objects=self.mujoco_objects,
- visual_objects=None,
- initializer=self.placement_initializer,
+ mujoco_objects=self.hammer,
)
- self.model.place_objects()
def _get_reference(self):
"""
@@ -364,11 +369,8 @@ def _get_reference(self):
super()._get_reference()
# Hammer object references from this env
- self.hammer_body_id = self.sim.model.body_name2id("hammer")
- self.hammer_handle_geom_id = self.sim.model.geom_name2id("hammer_handle")
- self.hammer_head_geom_id = self.sim.model.geom_name2id("hammer_head")
- self.hammer_face_geom_id = self.sim.model.geom_name2id("hammer_face")
- self.hammer_claw_geom_id = self.sim.model.geom_name2id("hammer_claw")
+ self.hammer_body_id = self.sim.model.body_name2id(self.hammer.root_body)
+ self.hammer_handle_geom_id = self.sim.model.geom_name2id(self.hammer.handle_geoms[0])
# General env references
self.table_top_id = self.sim.model.site_name2id("table_top")
@@ -383,39 +385,38 @@ def _reset_internal(self):
if not self.deterministic_reset:
# Sample from the placement initializer for all objects
- obj_pos, obj_quat = self.model.place_objects()
+ object_placements = self.placement_initializer.sample()
# Loop through all objects and reset their positions
- for i, (obj_name, _) in enumerate(self.mujoco_objects.items()):
+ for obj_pos, obj_quat, obj in object_placements.values():
# If prehensile, set the object normally
if self.prehensile:
- self.sim.data.set_joint_qpos(obj_name + "_jnt0",
- np.concatenate([np.array(obj_pos[i]), np.array(obj_quat[i])]))
+ self.sim.data.set_joint_qpos(obj.joints[0], np.concatenate([np.array(obj_pos), np.array(obj_quat)]))
# Else, set the object in the hand of the robot and loop a few steps to guarantee the robot is grasping
# the object initially
else:
eef_rot_quat = T.mat2quat(T.euler2mat([np.pi - T.mat2euler(self._eef0_xmat)[2], 0, 0]))
- obj_quat[i] = T.quat_multiply(obj_quat[i], eef_rot_quat)
+ obj_quat = T.quat_multiply(obj_quat, eef_rot_quat)
for j in range(100):
# Set object in hand
- self.sim.data.set_joint_qpos(obj_name + "_jnt0",
- np.concatenate([self._eef0_xpos, np.array(obj_quat[i])]))
+ self.sim.data.set_joint_qpos(obj.joints[0],
+ np.concatenate([self._eef0_xpos, np.array(obj_quat)]))
# Close gripper (action = 1) and prevent arm from moving
if self.env_configuration == 'bimanual':
# Execute no-op action with gravity compensation
torques = np.concatenate([self.robots[0].controller["right"].torque_compensation,
self.robots[0].controller["left"].torque_compensation])
- self.sim.data.ctrl[self.robots[0]._ref_joint_torq_actuator_indexes] = torques
+ self.sim.data.ctrl[self.robots[0]._ref_joint_actuator_indexes] = torques
# Execute gripper action
- self.robots[0].grip_action([1], "right")
+ self.robots[0].grip_action(gripper=self.robots[0].gripper["right"], gripper_action=[1])
else:
# Execute no-op action with gravity compensation
- self.sim.data.ctrl[self.robots[0]._ref_joint_torq_actuator_indexes] =\
+ self.sim.data.ctrl[self.robots[0]._ref_joint_actuator_indexes] =\
self.robots[0].controller.torque_compensation
- self.sim.data.ctrl[self.robots[1]._ref_joint_torq_actuator_indexes] = \
+ self.sim.data.ctrl[self.robots[1]._ref_joint_actuator_indexes] = \
self.robots[1].controller.torque_compensation
# Execute gripper action
- self.robots[0].grip_action([1])
+ self.robots[0].grip_action(gripper=self.robots[0].gripper, gripper_action=[1])
# Take forward step
self.sim.step()
@@ -434,59 +435,15 @@ def _get_task_info(self):
# Get height of hammer and table and define height threshold
hammer_angle_offset = (self.hammer.handle_length / 2 + 2*self.hammer.head_halfsize) * np.sin(self._hammer_angle)
hammer_height = self.sim.data.geom_xpos[self.hammer_handle_geom_id][2]\
- - self.hammer.get_top_offset()[2]\
+ - self.hammer.top_offset[2]\
- hammer_angle_offset
table_height = self.sim.data.site_xpos[self.table_top_id][2]
# Check if any Arm's gripper is grasping the hammer handle
-
- # Single bimanual robot setting
- if self.env_configuration == "bimanual":
- _contacts_0_lf = len(list(
- self.find_contacts(
- self.robots[0].gripper["left"].important_geoms["left_finger"], self.hammer.all_geoms
- )
- )) > 0
- _contacts_0_rf = len(list(
- self.find_contacts(
- self.robots[0].gripper["left"].important_geoms["right_finger"], self.hammer.all_geoms
- )
- )) > 0
- _contacts_1_lf = len(list(
- self.find_contacts(
- self.robots[0].gripper["right"].important_geoms["left_finger"], self.hammer.handle_geoms
- )
- )) > 0
- _contacts_1_rf = len(list(
- self.find_contacts(
- self.robots[0].gripper["right"].important_geoms["right_finger"], self.hammer.handle_geoms
- )
- )) > 0
- # Multi single arm setting
- else:
- _contacts_0_lf = len(list(
- self.find_contacts(
- self.robots[0].gripper.important_geoms["left_finger"], self.hammer.all_geoms
- )
- )) > 0
- _contacts_0_rf = len(list(
- self.find_contacts(
- self.robots[0].gripper.important_geoms["right_finger"], self.hammer.all_geoms
- )
- )) > 0
- _contacts_1_lf = len(list(
- self.find_contacts(
- self.robots[1].gripper.important_geoms["left_finger"], self.hammer.handle_geoms
- )
- )) > 0
- _contacts_1_rf = len(list(
- self.find_contacts(
- self.robots[1].gripper.important_geoms["right_finger"], self.hammer.handle_geoms
- )
- )) > 0
-
- arm0_grasp_any = True if _contacts_0_lf and _contacts_0_rf else False
- arm1_grasp_handle = True if _contacts_1_lf and _contacts_1_rf else False
+ (g0, g1) = (self.robots[0].gripper["right"], self.robots[0].gripper["left"]) if \
+ self.env_configuration == "bimanual" else (self.robots[0].gripper, self.robots[1].gripper)
+ arm0_grasp_any = self._check_grasp(gripper=g0, object_geoms=self.hammer)
+ arm1_grasp_handle = self._check_grasp(gripper=g1, object_geoms=self.hammer.handle_geoms)
# Return all relevant values
return arm0_grasp_any, arm1_grasp_handle, hammer_height, table_height
@@ -526,8 +483,6 @@ def _get_observation(self):
di["hammer_quat"] = np.array(self._hammer_quat)
di["handle_xpos"] = np.array(self._handle_xpos)
- di[pr0 + "eef_xpos"] = np.array(self._eef0_xpos)
- di[pr1 + "eef_xpos"] = np.array(self._eef1_xpos)
di[pr0 + "gripper_to_handle"] = np.array(self._gripper_0_to_handle)
di[pr1 + "gripper_to_handle"] = np.array(self._gripper_1_to_handle)
@@ -536,8 +491,6 @@ def _get_observation(self):
di["hammer_pos"],
di["hammer_quat"],
di["handle_xpos"],
- di[pr0 + "eef_xpos"],
- di[pr1 + "eef_xpos"],
di[pr0 + "gripper_to_handle"],
di[pr1 + "gripper_to_handle"],
]
@@ -561,38 +514,6 @@ def _check_success(self):
hammer_height - table_height > self.height_threshold \
else False
- def _check_robot_configuration(self, robots):
- """
- Sanity check to make sure the inputted robots and configuration is acceptable
-
- Args:
- robots (str or list of str): Robots to instantiate within this env
- """
- robots = robots if type(robots) == list or type(robots) == tuple else [robots]
- if self.env_configuration == "single-arm-opposed" or self.env_configuration == "single-arm-parallel":
- # Specifically two robots should be inputted!
- is_bimanual = False
- if type(robots) is not list or len(robots) != 2:
- raise ValueError("Error: Exactly two single-armed robots should be inputted "
- "for this task configuration!")
- elif self.env_configuration == "bimanual":
- is_bimanual = True
- # Specifically one robot should be inputted!
- if type(robots) is list and len(robots) != 1:
- raise ValueError("Error: Exactly one bimanual robot should be inputted "
- "for this task configuration!")
- else:
- # This is an unknown env configuration, print error
- raise ValueError("Error: Unknown environment configuration received. Only 'bimanual',"
- "'single-arm-parallel', and 'single-arm-opposed' are supported. Got: {}"
- .format(self.env_configuration))
-
- # Lastly, check to make sure all inputted robot names are of their correct type (bimanual / not bimanual)
- for robot in robots:
- if check_bimanual(robot) != is_bimanual:
- raise ValueError("Error: For {} configuration, expected bimanual check to return {}; "
- "instead, got {}.".format(self.env_configuration, is_bimanual, check_bimanual(robot)))
-
@property
def _handle_xpos(self):
"""
@@ -603,36 +524,6 @@ def _handle_xpos(self):
"""
return self.sim.data.geom_xpos[self.hammer_handle_geom_id]
- @property
- def _head_xpos(self):
- """
- Grab the position of the hammer head.
-
- Returns:
- np.array: (x,y,z) position of head
- """
- return self.sim.data.geom_xpos[self.hammer_head_geom_id]
-
- @property
- def _face_xpos(self):
- """
- Grab the position of the hammer face.
-
- Returns:
- np.array: (x,y,z) position of face
- """
- return self.sim.data.geom_xpos[self.hammer_face_geom_id]
-
- @property
- def _claw_xpos(self):
- """
- Grab the position of the hammer claw.
-
- Returns:
- np.array: (x,y,z) position of claw
- """
- return self.sim.data.geom_xpos[self.hammer_claw_geom_id]
-
@property
def _hammer_pos(self):
"""
@@ -666,75 +557,6 @@ def _hammer_angle(self):
z_rotated = np.matmul(mat, z_unit)
return np.pi/2 - np.arccos(np.dot(z_unit, z_rotated))
- @property
- def _world_quat(self):
- """
- Grab the world orientation
-
- Returns:
- np.array: (x,y,z,w) world quaternion
- """
- return T.convert_quat(np.array([1, 0, 0, 0]), to="xyzw")
-
- @property
- def _eef0_xpos(self):
- """
- Grab the position of Robot 0's end effector.
-
- Returns:
- np.array: (x,y,z) position of EEF0
- """
- if self.env_configuration == "bimanual":
- return np.array(self.sim.data.site_xpos[self.robots[0].eef_site_id["right"]])
- else:
- return np.array(self.sim.data.site_xpos[self.robots[0].eef_site_id])
-
- @property
- def _eef1_xpos(self):
- """
- Grab the position of Robot 1's end effector.
-
- Returns:
- np.array: (x,y,z) position of EEF1
- """
- if self.env_configuration == "bimanual":
- return np.array(self.sim.data.site_xpos[self.robots[0].eef_site_id["left"]])
- else:
- return np.array(self.sim.data.site_xpos[self.robots[1].eef_site_id])
-
- @property
- def _eef0_xmat(self):
- """
- End Effector 0 orientation as a rotation matrix
- Note that this draws the orientation from the "ee" site, NOT the gripper site, since the gripper
- orientations are inconsistent!
-
- Returns:
- np.array: (3,3) orientation matrix for EEF0
- """
- pf = self.robots[0].robot_model.naming_prefix
- if self.env_configuration == "bimanual":
- return np.array(self.sim.data.site_xmat[self.sim.model.site_name2id(pf + "right_ee")]).reshape(3, 3)
- else:
- return np.array(self.sim.data.site_xmat[self.sim.model.site_name2id(pf + "ee")]).reshape(3, 3)
-
- @property
- def _eef1_xmat(self):
- """
- End Effector 1 orientation as a rotation matrix
- Note that this draws the orientation from the "right_/left_hand" body, NOT the gripper site, since the gripper
- orientations are inconsistent!
-
- Returns:
- np.array: (3,3) orientation matrix for EEF1
- """
- if self.env_configuration == "bimanual":
- pf = self.robots[0].robot_model.naming_prefix
- return np.array(self.sim.data.site_xmat[self.sim.model.site_name2id(pf + "left_ee")]).reshape(3, 3)
- else:
- pf = self.robots[1].robot_model.naming_prefix
- return np.array(self.sim.data.site_xmat[self.sim.model.site_name2id(pf + "ee")]).reshape(3, 3)
-
@property
def _gripper_0_to_handle(self):
"""
diff --git a/robosuite/environments/two_arm_lift.py b/robosuite/environments/manipulation/two_arm_lift.py
similarity index 66%
rename from robosuite/environments/two_arm_lift.py
rename to robosuite/environments/manipulation/two_arm_lift.py
index 8e73e79d89..206f5ff22c 100644
--- a/robosuite/environments/two_arm_lift.py
+++ b/robosuite/environments/manipulation/two_arm_lift.py
@@ -1,17 +1,17 @@
from collections import OrderedDict
import numpy as np
-from robosuite.environments.robot_env import RobotEnv
+from robosuite.environments.manipulation.two_arm_env import TwoArmEnv
from robosuite.models.arenas import TableArena
from robosuite.models.objects import PotWithHandlesObject
-from robosuite.models.tasks import ManipulationTask, UniformRandomSampler
-from robosuite.models.robots import check_bimanual
+from robosuite.models.tasks import ManipulationTask
+from robosuite.utils.placement_samplers import UniformRandomSampler
import robosuite.utils.transform_utils as T
-class TwoArmLift(RobotEnv):
+class TwoArmLift(TwoArmEnv):
"""
This class corresponds to the lifting task for two robot arms.
@@ -27,7 +27,10 @@ class TwoArmLift(RobotEnv):
:`'single-arm-parallel'`: Only applicable for multi single arm setups. Sets up the (two) single armed
robots next to each other on the -x side of the table
:`'single-arm-opposed'`: Only applicable for multi single arm setups. Sets up the (two) single armed
- robots opposed from each others on the opposite +/-y sides of the table (Default option)
+ robots opposed from each others on the opposite +/-y sides of the table.
+
+ Note that "default" corresponds to either "bimanual" if a bimanual robot is used or "single-arm-opposed" if two
+ single-arm robots are used.
controller_configs (str or list of dict): If set, contains relevant controller parameters for creating a
custom controller. Else, uses the default controller for this specific task. Should either be single
@@ -40,10 +43,6 @@ class TwoArmLift(RobotEnv):
overrides the default gripper. Should either be single str if same gripper type is to be used for all
robots or else it should be a list of the same length as "robots" param
- gripper_visualizations (bool or list of bool): True if using gripper visualization.
- Useful for teleoperation. Should either be single bool if gripper visualization is to be used for all
- robots or else it should be a list of the same length as "robots" param
-
initialization_noise (dict or list of dict): Dict containing the initialization noise parameters.
The expected keys and corresponding value types are specified below:
@@ -74,13 +73,10 @@ class TwoArmLift(RobotEnv):
reward_shaping (bool): if True, use dense rewards.
- placement_initializer (ObjectPositionSampler instance): if provided, will
+ placement_initializer (ObjectPositionSampler): if provided, will
be used to place objects on every reset, else a UniformRandomSampler
is used by default.
- use_indicator_object (bool): if True, sets up an indicator object that
- is useful for debugging.
-
has_renderer (bool): If true, render the simulation state in
a viewer instead of headless mode.
@@ -94,6 +90,10 @@ class TwoArmLift(RobotEnv):
render_visual_mesh (bool): True if rendering visual meshes in camera. False otherwise.
+ render_gpu_device_id (int): corresponds to the GPU device id to use for offscreen rendering.
+ Defaults to -1, in which case the device will be inferred from environment variables
+ (GPUS or CUDA_VISIBLE_DEVICES).
+
control_freq (float): how many control signals to receive in every second. This sets the amount of
simulation time that passes between every action input.
@@ -137,7 +137,6 @@ def __init__(
env_configuration="single-arm-opposed",
controller_configs=None,
gripper_types="default",
- gripper_visualizations=False,
initialization_noise="default",
table_full_size=(0.8, 0.8, 0.05),
table_friction=(1., 5e-3, 1e-4),
@@ -146,13 +145,13 @@ def __init__(
reward_scale=1.0,
reward_shaping=False,
placement_initializer=None,
- use_indicator_object=False,
has_renderer=False,
has_offscreen_renderer=True,
render_camera="frontview",
render_collision_mesh=False,
render_visual_mesh=True,
- control_freq=10,
+ render_gpu_device_id=-1,
+ control_freq=20,
horizon=1000,
ignore_done=False,
hard_reset=True,
@@ -161,13 +160,10 @@ def __init__(
camera_widths=256,
camera_depths=False,
):
- # First, verify that correct number of robots are being inputted
- self.env_configuration = env_configuration
- self._check_robot_configuration(robots)
-
# settings for table top
self.table_full_size = table_full_size
self.table_friction = table_friction
+ self.table_offset = np.array((0, 0, 0.8))
# reward configuration
self.reward_scale = reward_scale
@@ -177,29 +173,22 @@ def __init__(
self.use_object_obs = use_object_obs
# object placement initializer
- if placement_initializer:
- self.placement_initializer = placement_initializer
- else:
- self.placement_initializer = UniformRandomSampler(
- x_range=[-0.03, 0.03],
- y_range=[-0.03, 0.03],
- ensure_object_boundary_in_range=False,
- rotation=(-np.pi / 3, np.pi / 3),
- )
+ self.placement_initializer = placement_initializer
super().__init__(
robots=robots,
+ env_configuration=env_configuration,
controller_configs=controller_configs,
+ mount_types="default",
gripper_types=gripper_types,
- gripper_visualizations=gripper_visualizations,
initialization_noise=initialization_noise,
use_camera_obs=use_camera_obs,
- use_indicator_object=use_indicator_object,
has_renderer=has_renderer,
has_offscreen_renderer=has_offscreen_renderer,
render_camera=render_camera,
render_collision_mesh=render_collision_mesh,
render_visual_mesh=render_visual_mesh,
+ render_gpu_device_id=render_gpu_device_id,
control_freq=control_freq,
horizon=horizon,
ignore_done=ignore_done,
@@ -252,73 +241,33 @@ def reward(self, action=None):
# use a shaping reward
elif self.reward_shaping:
# lifting reward
- pot_bottom_height = self.sim.data.site_xpos[self.pot_center_id][2] - self.pot.get_top_offset()[2]
+ pot_bottom_height = self.sim.data.site_xpos[self.pot_center_id][2] - self.pot.top_offset[2]
table_height = self.sim.data.site_xpos[self.table_top_id][2]
elevation = pot_bottom_height - table_height
r_lift = min(max(elevation - 0.05, 0), 0.15)
reward += 10. * direction_coef * r_lift
- _gripper_0_to_handle = self._gripper_0_to_handle
- _gripper_1_to_handle = self._gripper_1_to_handle
+ _gripper0_to_handle0 = self._gripper0_to_handle0
+ _gripper1_to_handle1 = self._gripper1_to_handle1
# gh stands for gripper-handle
# When grippers are far away, tell them to be closer
- # Single bimanual robot setting
- if self.env_configuration == "bimanual":
- _contacts_0_lf = len(list(
- self.find_contacts(
- self.robots[0].gripper["left"].important_geoms["left_finger"], self.pot.handle_2_geoms()
- )
- )) > 0
- _contacts_0_rf = len(list(
- self.find_contacts(
- self.robots[0].gripper["left"].important_geoms["right_finger"], self.pot.handle_2_geoms()
- )
- )) > 0
- _contacts_1_lf = len(list(
- self.find_contacts(
- self.robots[0].gripper["right"].important_geoms["left_finger"], self.pot.handle_1_geoms()
- )
- )) > 0
- _contacts_1_rf = len(list(
- self.find_contacts(
- self.robots[0].gripper["right"].important_geoms["right_finger"], self.pot.handle_1_geoms()
- )
- )) > 0
- # Multi single arm setting
- else:
- _contacts_0_lf = len(list(
- self.find_contacts(
- self.robots[0].gripper.important_geoms["left_finger"], self.pot.handle_2_geoms()
- )
- )) > 0
- _contacts_0_rf = len(list(
- self.find_contacts(
- self.robots[0].gripper.important_geoms["right_finger"], self.pot.handle_2_geoms()
- )
- )) > 0
- _contacts_1_lf = len(list(
- self.find_contacts(
- self.robots[1].gripper.important_geoms["left_finger"], self.pot.handle_1_geoms()
- )
- )) > 0
- _contacts_1_rf = len(list(
- self.find_contacts(
- self.robots[1].gripper.important_geoms["right_finger"], self.pot.handle_1_geoms()
- )
- )) > 0
- _g0h_dist = np.linalg.norm(_gripper_0_to_handle)
- _g1h_dist = np.linalg.norm(_gripper_1_to_handle)
+ # Get contacts
+ (g0, g1) = (self.robots[0].gripper["right"], self.robots[0].gripper["left"]) if \
+ self.env_configuration == "bimanual" else (self.robots[0].gripper, self.robots[1].gripper)
+
+ _g0h_dist = np.linalg.norm(_gripper0_to_handle0)
+ _g1h_dist = np.linalg.norm(_gripper1_to_handle1)
# Grasping reward
- if _contacts_0_lf and _contacts_0_rf:
+ if self._check_grasp(gripper=g0, object_geoms=self.pot.handle0_geoms):
reward += 0.25
# Reaching reward
reward += 0.5 * (1 - np.tanh(10.0 * _g0h_dist))
# Grasping reward
- if _contacts_1_lf and _contacts_1_rf:
+ if self._check_grasp(gripper=g1, object_geoms=self.pot.handle1_geoms):
reward += 0.25
# Reaching reward
reward += 0.5 * (1 - np.tanh(10.0 * _g1h_dist))
@@ -355,30 +304,40 @@ def _load_model(self):
robot.robot_model.set_base_xpos(xpos)
# load model for table top workspace
- self.mujoco_arena = TableArena(
+ mujoco_arena = TableArena(
table_full_size=self.table_full_size,
table_friction=self.table_friction,
- table_offset=(0, 0, 0.8),
+ table_offset=self.table_offset,
)
- if self.use_indicator_object:
- self.mujoco_arena.add_pos_indicator()
# Arena always gets set to zero origin
- self.mujoco_arena.set_origin([0, 0, 0])
+ mujoco_arena.set_origin([0, 0, 0])
# initialize objects of interest
self.pot = PotWithHandlesObject(name="pot")
- self.mujoco_objects = OrderedDict([("pot", self.pot)])
+
+ # Create placement initializer
+ if self.placement_initializer is not None:
+ self.placement_initializer.reset()
+ self.placement_initializer.add_objects(self.pot)
+ else:
+ self.placement_initializer = UniformRandomSampler(
+ name="ObjectSampler",
+ mujoco_objects=self.pot,
+ x_range=[-0.03, 0.03],
+ y_range=[-0.03, 0.03],
+ ensure_object_boundary_in_range=False,
+ ensure_valid_placement=True,
+ reference_pos=self.table_offset,
+ rotation=(np.pi + -np.pi / 3, np.pi + np.pi / 3),
+ )
# task includes arena, robot, and objects of interest
self.model = ManipulationTask(
- mujoco_arena=self.mujoco_arena,
+ mujoco_arena=mujoco_arena,
mujoco_robots=[robot.robot_model for robot in self.robots],
- mujoco_objects=self.mujoco_objects,
- visual_objects=None,
- initializer=self.placement_initializer,
+ mujoco_objects=self.pot,
)
- self.model.place_objects()
def _get_reference(self):
"""
@@ -389,11 +348,11 @@ def _get_reference(self):
super()._get_reference()
# Additional object references from this env
- self.pot_body_id = self.sim.model.body_name2id("pot")
- self.handle_1_site_id = self.sim.model.site_name2id("pot_handle_1")
- self.handle_0_site_id = self.sim.model.site_name2id("pot_handle_2")
+ self.pot_body_id = self.sim.model.body_name2id(self.pot.root_body)
+ self.handle0_site_id = self.sim.model.site_name2id(self.pot.important_sites["handle0"])
+ self.handle1_site_id = self.sim.model.site_name2id(self.pot.important_sites["handle1"])
self.table_top_id = self.sim.model.site_name2id("table_top")
- self.pot_center_id = self.sim.model.site_name2id("pot_center")
+ self.pot_center_id = self.sim.model.site_name2id(self.pot.important_sites["center"])
def _reset_internal(self):
"""
@@ -405,11 +364,11 @@ def _reset_internal(self):
if not self.deterministic_reset:
# Sample from the placement initializer for all objects
- obj_pos, obj_quat = self.model.place_objects()
+ object_placements = self.placement_initializer.sample()
# Loop through all objects and reset their positions
- for i, (obj_name, _) in enumerate(self.mujoco_objects.items()):
- self.sim.data.set_joint_qpos(obj_name + "_jnt0", np.concatenate([np.array(obj_pos[i]), np.array(obj_quat[i])]))
+ for obj_pos, obj_quat, obj in object_placements.values():
+ self.sim.data.set_joint_qpos(obj.joints[0], np.concatenate([np.array(obj_pos), np.array(obj_quat)]))
def _get_observation(self):
"""
@@ -449,28 +408,44 @@ def _get_observation(self):
di["cube_pos"] = cube_pos
di["cube_quat"] = cube_quat
- di[pr0 + "eef_xpos"] = self._eef0_xpos
- di[pr1 + "eef_xpos"] = self._eef1_xpos
- di["handle_0_xpos"] = np.array(self._handle_0_xpos)
- di["handle_1_xpos"] = np.array(self._handle_1_xpos)
- di[pr0 + "gripper_to_handle"] = np.array(self._gripper_0_to_handle)
- di[pr1 + "gripper_to_handle"] = np.array(self._gripper_1_to_handle)
+ di["handle0_xpos"] = np.array(self._handle0_xpos)
+ di["handle1_xpos"] = np.array(self._handle1_xpos)
+ di[pr0 + "gripper_to_handle0"] = np.array(self._gripper0_to_handle0)
+ di[pr1 + "gripper_to_handle1"] = np.array(self._gripper1_to_handle1)
di["object-state"] = np.concatenate(
[
di["cube_pos"],
di["cube_quat"],
- di[pr0 + "eef_xpos"],
- di[pr1 + "eef_xpos"],
- di["handle_0_xpos"],
- di["handle_1_xpos"],
- di[pr0 + "gripper_to_handle"],
- di[pr1 + "gripper_to_handle"],
+ di["handle0_xpos"],
+ di["handle1_xpos"],
+ di[pr0 + "gripper_to_handle0"],
+ di[pr1 + "gripper_to_handle1"],
]
)
return di
+ def visualize(self, vis_settings):
+ """
+ In addition to super call, visualize gripper site proportional to the distance to each handle.
+
+ Args:
+ vis_settings (dict): Visualization keywords mapped to T/F, determining whether that specific
+ component should be visualized. Should have "grippers" keyword as well as any other relevant
+ options specified.
+ """
+ # Run superclass method first
+ super().visualize(vis_settings=vis_settings)
+
+ # Color the gripper visualization site according to its distance to each handle
+ if vis_settings["grippers"]:
+ handles = [self.pot.important_sites[f"handle{i}"] for i in range(2)]
+ grippers = [self.robots[0].gripper[arm] for arm in self.robots[0].arms] if \
+ self.env_configuration == "bimanual" else [robot.gripper for robot in self.robots]
+ for gripper, handle in zip(grippers, handles):
+ self._visualize_gripper_to_target(gripper=gripper, target=handle, target_type="site")
+
def _check_success(self):
"""
Check if pot is successfully lifted
@@ -478,63 +453,31 @@ def _check_success(self):
Returns:
bool: True if pot is lifted
"""
- pot_bottom_height = self.sim.data.site_xpos[self.pot_center_id][2] - self.pot.get_top_offset()[2]
+ pot_bottom_height = self.sim.data.site_xpos[self.pot_center_id][2] - self.pot.top_offset[2]
table_height = self.sim.data.site_xpos[self.table_top_id][2]
# cube is higher than the table top above a margin
return pot_bottom_height > table_height + 0.10
- def _check_robot_configuration(self, robots):
- """
- Sanity check to make sure the inputted robots and configuration is acceptable
-
- Args:
- robots (str or list of str): Robots to instantiate within this env
- """
- robots = robots if type(robots) == list or type(robots) == tuple else [robots]
- if self.env_configuration == "single-arm-opposed" or self.env_configuration == "single-arm-parallel":
- # Specifically two robots should be inputted!
- is_bimanual = False
- if type(robots) is not list or len(robots) != 2:
- raise ValueError("Error: Exactly two single-armed robots should be inputted "
- "for this task configuration!")
- elif self.env_configuration == "bimanual":
- is_bimanual = True
- # Specifically one robot should be inputted!
- if type(robots) is list and len(robots) != 1:
- raise ValueError("Error: Exactly one bimanual robot should be inputted "
- "for this task configuration!")
- else:
- # This is an unknown env configuration, print error
- raise ValueError("Error: Unknown environment configuration received. Only 'bimanual',"
- "'single-arm-parallel', and 'single-arm-opposed' are supported. Got: {}"
- .format(self.env_configuration))
-
- # Lastly, check to make sure all inputted robot names are of their correct type (bimanual / not bimanual)
- for robot in robots:
- if check_bimanual(robot) != is_bimanual:
- raise ValueError("Error: For {} configuration, expected bimanual check to return {}; "
- "instead, got {}.".format(self.env_configuration, is_bimanual, check_bimanual(robot)))
-
@property
- def _handle_0_xpos(self):
+ def _handle0_xpos(self):
"""
Grab the position of the left (blue) hammer handle.
Returns:
np.array: (x,y,z) position of handle
"""
- return self.sim.data.site_xpos[self.handle_0_site_id]
+ return self.sim.data.site_xpos[self.handle0_site_id]
@property
- def _handle_1_xpos(self):
+ def _handle1_xpos(self):
"""
Grab the position of the right (green) hammer handle.
Returns:
np.array: (x,y,z) position of handle
"""
- return self.sim.data.site_xpos[self.handle_1_site_id]
+ return self.sim.data.site_xpos[self.handle1_site_id]
@property
def _pot_quat(self):
@@ -547,57 +490,21 @@ def _pot_quat(self):
return T.convert_quat(self.sim.data.body_xquat[self.pot_body_id], to="xyzw")
@property
- def _world_quat(self):
- """
- Grab the world orientation
-
- Returns:
- np.array: (x,y,z,w) world quaternion
- """
- return T.convert_quat(np.array([1, 0, 0, 0]), to="xyzw")
-
- @property
- def _eef0_xpos(self):
- """
- Grab the position of Robot 0's end effector.
-
- Returns:
- np.array: (x,y,z) position of EEF0
- """
- if self.env_configuration == "bimanual":
- return np.array(self.sim.data.site_xpos[self.robots[0].eef_site_id["left"]])
- else:
- return np.array(self.sim.data.site_xpos[self.robots[0].eef_site_id])
-
- @property
- def _eef1_xpos(self):
- """
- Grab the position of Robot 1's end effector.
-
- Returns:
- np.array: (x,y,z) position of EEF1
- """
- if self.env_configuration == "bimanual":
- return np.array(self.sim.data.site_xpos[self.robots[0].eef_site_id["right"]])
- else:
- return np.array(self.sim.data.site_xpos[self.robots[1].eef_site_id])
-
- @property
- def _gripper_0_to_handle(self):
+ def _gripper0_to_handle0(self):
"""
Calculate vector from the left gripper to the left pot handle.
Returns:
np.array: (dx,dy,dz) distance vector between handle and EEF0
"""
- return self._handle_0_xpos - self._eef0_xpos
+ return self._handle0_xpos - self._eef0_xpos
@property
- def _gripper_1_to_handle(self):
+ def _gripper1_to_handle1(self):
"""
Calculate vector from the right gripper to the right pot handle.
Returns:
np.array: (dx,dy,dz) distance vector between handle and EEF0
"""
- return self._handle_1_xpos - self._eef1_xpos
+ return self._handle1_xpos - self._eef1_xpos
diff --git a/robosuite/environments/two_arm_peg_in_hole.py b/robosuite/environments/manipulation/two_arm_peg_in_hole.py
similarity index 77%
rename from robosuite/environments/two_arm_peg_in_hole.py
rename to robosuite/environments/manipulation/two_arm_peg_in_hole.py
index 9cf25454f2..7c8451d4c1 100644
--- a/robosuite/environments/two_arm_peg_in_hole.py
+++ b/robosuite/environments/manipulation/two_arm_peg_in_hole.py
@@ -1,16 +1,15 @@
import numpy as np
import robosuite.utils.transform_utils as T
-from robosuite.environments.robot_env import RobotEnv
-from robosuite.utils.mjcf_utils import CustomMaterial, array_to_string
+from robosuite.environments.manipulation.two_arm_env import TwoArmEnv
+from robosuite.utils.mjcf_utils import CustomMaterial, array_to_string, find_elements
from robosuite.models.objects import CylinderObject, PlateWithHoleObject
from robosuite.models.arenas import EmptyArena
-from robosuite.models import MujocoWorldBase
-from robosuite.models.robots import check_bimanual
+from robosuite.models.tasks import ManipulationTask
-class TwoArmPegInHole(RobotEnv):
+class TwoArmPegInHole(TwoArmEnv):
"""
This class corresponds to the peg-in-hole task for two robot arms.
@@ -26,22 +25,20 @@ class TwoArmPegInHole(RobotEnv):
:`'single-arm-parallel'`: Only applicable for multi single arm setups. Sets up the (two) single armed
robots next to each other on the -x side of the table
:`'single-arm-opposed'`: Only applicable for multi single arm setups. Sets up the (two) single armed
- robots opposed from each others on the opposite +/-y sides of the table (Default option)
+ robots opposed from each others on the opposite +/-y sides of the table.
+
+ Note that "default" corresponds to either "bimanual" if a bimanual robot is used or "single-arm-opposed" if two
+ single-arm robots are used.
controller_configs (str or list of dict): If set, contains relevant controller parameters for creating a
custom controller. Else, uses the default controller for this specific task. Should either be single
dict if same controller is to be used for all robots or else it should be a list of the same length as
"robots" param
- gripper_types (str or list of str): type of gripper, used to instantiate
- gripper models from gripper factory.
+ gripper_types (str or list of str): type of gripper, used to instantiate gripper models from gripper factory.
For this environment, setting a value other than the default (None) will raise an AssertionError, as
this environment is not meant to be used with any gripper at all.
- gripper_visualizations (bool or list of bool): True if using gripper visualization.
- Useful for teleoperation. Should either be single bool if gripper visualization is to be used for all
- robots or else it should be a list of the same length as "robots" param
-
initialization_noise (dict or list of dict): Dict containing the initialization noise parameters.
The expected keys and corresponding value types are specified below:
@@ -74,9 +71,6 @@ class TwoArmPegInHole(RobotEnv):
peg_length (float): length of the peg
- use_indicator_object (bool): if True, sets up an indicator object that
- is useful for debugging.
-
has_renderer (bool): If true, render the simulation state in
a viewer instead of headless mode.
@@ -90,6 +84,10 @@ class TwoArmPegInHole(RobotEnv):
render_visual_mesh (bool): True if rendering visual meshes in camera. False otherwise.
+ render_gpu_device_id (int): corresponds to the GPU device id to use for offscreen rendering.
+ Defaults to -1, in which case the device will be inferred from environment variables
+ (GPUS or CUDA_VISIBLE_DEVICES).
+
control_freq (float): how many control signals to receive in every second. This sets the amount of
simulation time that passes between every action input.
@@ -134,7 +132,6 @@ def __init__(
env_configuration="single-arm-opposed",
controller_configs=None,
gripper_types=None,
- gripper_visualizations=False,
initialization_noise="default",
use_camera_obs=True,
use_object_obs=True,
@@ -142,13 +139,13 @@ def __init__(
reward_shaping=False,
peg_radius=(0.015, 0.03),
peg_length=0.13,
- use_indicator_object=False,
has_renderer=False,
has_offscreen_renderer=True,
render_camera="frontview",
render_collision_mesh=False,
render_visual_mesh=True,
- control_freq=10,
+ render_gpu_device_id=-1,
+ control_freq=20,
horizon=1000,
ignore_done=False,
hard_reset=True,
@@ -157,10 +154,6 @@ def __init__(
camera_widths=256,
camera_depths=False,
):
- # First, verify that correct number of robots are being inputted
- self.env_configuration = env_configuration
- self._check_robot_configuration(robots)
-
# Assert that the gripper type is None
assert gripper_types is None, "Tried to specify gripper other than None in TwoArmPegInHole environment!"
@@ -177,17 +170,18 @@ def __init__(
super().__init__(
robots=robots,
+ env_configuration=env_configuration,
controller_configs=controller_configs,
+ mount_types="default",
gripper_types=gripper_types,
- gripper_visualizations=gripper_visualizations,
initialization_noise=initialization_noise,
use_camera_obs=use_camera_obs,
- use_indicator_object=use_indicator_object,
has_renderer=has_renderer,
has_offscreen_renderer=has_offscreen_renderer,
render_camera=render_camera,
render_collision_mesh=render_collision_mesh,
render_visual_mesh=render_visual_mesh,
+ render_gpu_device_id=render_gpu_device_id,
control_freq=control_freq,
horizon=horizon,
ignore_done=ignore_done,
@@ -198,7 +192,7 @@ def __init__(
camera_depths=camera_depths,
)
- def reward(self, action):
+ def reward(self, action=None):
"""
Reward function for the task.
@@ -241,8 +235,7 @@ def reward(self, action):
reward += 1 - np.tanh(np.abs(t))
reward += cos
- # if we're not reward shaping, we need to scale our sparse reward so that the max reward is identical
- # to its dense version
+ # if we're not reward shaping, scale sparse reward so that the max reward is identical to its dense version
else:
reward *= 5.0
@@ -278,18 +271,20 @@ def _load_model(self):
robot.robot_model.set_base_xpos(xpos)
# Add arena and robot
- self.model = MujocoWorldBase()
- self.mujoco_arena = EmptyArena()
- if self.use_indicator_object:
- self.mujoco_arena.add_pos_indicator()
- self.model.merge(self.mujoco_arena)
- for robot in self.robots:
- self.model.merge(robot.robot_model)
+ mujoco_arena = EmptyArena()
- # initialize objects of interest
- self.hole = PlateWithHoleObject(
- name="hole",
+ # Arena always gets set to zero origin
+ mujoco_arena.set_origin([0, 0, 0])
+
+ # Modify default agentview camera
+ mujoco_arena.set_camera(
+ camera_name="agentview",
+ pos=[1.0666432116509934, 1.4903257668114777e-08, 2.0563394967349096],
+ quat=[0.6530979871749878, 0.27104058861732483, 0.27104055881500244, 0.6530978679656982]
)
+
+ # initialize objects of interest
+ self.hole = PlateWithHoleObject(name="hole")
tex_attrib = {
"type": "cube",
}
@@ -311,30 +306,40 @@ def _load_model(self):
size_max=(self.peg_radius[1], self.peg_length),
material=greenwood,
rgba=[0, 1, 0, 1],
+ joints=None,
)
# Load hole object
- self.hole_obj = self.hole.get_collision(site=True)
- self.hole_obj.set("quat", "0 0 0.707 0.707")
- self.hole_obj.set("pos", "0.11 0 0.17")
- self.model.merge_asset(self.hole)
+ hole_obj = self.hole.get_obj()
+ hole_obj.set("quat", "0 0 0.707 0.707")
+ hole_obj.set("pos", "0.11 0 0.17")
# Load peg object
- self.peg_obj = self.peg.get_collision(site=True)
- self.peg_obj.set("pos", array_to_string((0, 0, self.peg_length)))
- self.model.merge_asset(self.peg)
+ peg_obj = self.peg.get_obj()
+ peg_obj.set("pos", array_to_string((0, 0, self.peg_length)))
- # Depending on env configuration, append appropriate objects to arms
+ # Append appropriate objects to arms
if self.env_configuration == "bimanual":
- self.model.worldbody.find(".//body[@name='{}']"
- .format(self.robots[0].robot_model.eef_name["left"])).append(self.hole_obj)
- self.model.worldbody.find(".//body[@name='{}']"
- .format(self.robots[0].robot_model.eef_name["right"])).append(self.peg_obj)
+ r_eef, l_eef = [self.robots[0].robot_model.eef_name[arm] for arm in self.robots[0].arms]
+ r_model, l_model = [self.robots[0].robot_model, self.robots[0].robot_model]
else:
- self.model.worldbody.find(".//body[@name='{}']"
- .format(self.robots[1].robot_model.eef_name)).append(self.hole_obj)
- self.model.worldbody.find(".//body[@name='{}']"
- .format(self.robots[0].robot_model.eef_name)).append(self.peg_obj)
+ r_eef, l_eef = [robot.robot_model.eef_name for robot in self.robots]
+ r_model, l_model = [self.robots[0].robot_model, self.robots[1].robot_model]
+ r_body = find_elements(root=r_model.worldbody, tags="body", attribs={"name": r_eef}, return_first=True)
+ l_body = find_elements(root=l_model.worldbody, tags="body", attribs={"name": l_eef}, return_first=True)
+ r_body.append(peg_obj)
+ l_body.append(hole_obj)
+
+ # task includes arena, robot, and objects of interest
+ # We don't add peg and hole directly since they were already appended to the robots
+ self.model = ManipulationTask(
+ mujoco_arena=mujoco_arena,
+ mujoco_robots=[robot.robot_model for robot in self.robots],
+ )
+
+ # Make sure to add relevant assets from peg and hole objects
+ self.model.merge_assets(self.hole)
+ self.model.merge_assets(self.peg)
def _get_reference(self):
"""
@@ -345,8 +350,8 @@ def _get_reference(self):
super()._get_reference()
# Additional object references from this env
- self.hole_body_id = self.sim.model.body_name2id("hole")
- self.peg_body_id = self.sim.model.body_name2id("peg")
+ self.hole_body_id = self.sim.model.body_name2id(self.hole.root_body)
+ self.peg_body_id = self.sim.model.body_name2id(self.peg.root_body)
def _reset_internal(self):
"""
@@ -463,9 +468,7 @@ def _compute_orientation(self):
return (
t,
d,
- abs(
- np.dot(hole_normal, v) / np.linalg.norm(hole_normal) / np.linalg.norm(v)
- ),
+ abs(np.dot(hole_normal, v) / np.linalg.norm(hole_normal) / np.linalg.norm(v)),
)
def _peg_pose_in_hole_frame(self):
@@ -477,13 +480,13 @@ def _peg_pose_in_hole_frame(self):
np.array: (4,4) matrix corresponding to the pose of the peg in the hole frame
"""
# World frame
- peg_pos_in_world = self.sim.data.get_body_xpos("peg")
- peg_rot_in_world = self.sim.data.get_body_xmat("peg").reshape((3, 3))
+ peg_pos_in_world = self.sim.data.get_body_xpos(self.peg.root_body)
+ peg_rot_in_world = self.sim.data.get_body_xmat(self.peg.root_body).reshape((3, 3))
peg_pose_in_world = T.make_pose(peg_pos_in_world, peg_rot_in_world)
# World frame
- hole_pos_in_world = self.sim.data.get_body_xpos("hole")
- hole_rot_in_world = self.sim.data.get_body_xmat("hole").reshape((3, 3))
+ hole_pos_in_world = self.sim.data.get_body_xpos(self.hole.root_body)
+ hole_rot_in_world = self.sim.data.get_body_xmat(self.hole.root_body).reshape((3, 3))
hole_pose_in_world = T.make_pose(hole_pos_in_world, hole_rot_in_world)
world_pose_in_hole = T.pose_inv(hole_pose_in_world)
@@ -492,35 +495,3 @@ def _peg_pose_in_hole_frame(self):
peg_pose_in_world, world_pose_in_hole
)
return peg_pose_in_hole
-
- def _check_robot_configuration(self, robots):
- """
- Sanity check to make sure the inputted robots and configuration is acceptable
-
- Args:
- robots (str or list of str): Robots to instantiate within this env
- """
- robots = robots if type(robots) == list or type(robots) == tuple else [robots]
- if self.env_configuration == "single-arm-opposed" or self.env_configuration == "single-arm-parallel":
- # Specifically two robots should be inputted!
- is_bimanual = False
- if type(robots) is not list or len(robots) != 2:
- raise ValueError("Error: Exactly two single-armed robots should be inputted "
- "for this task configuration!")
- elif self.env_configuration == "bimanual":
- is_bimanual = True
- # Specifically one robot should be inputted!
- if type(robots) is list and len(robots) != 1:
- raise ValueError("Error: Exactly one bimanual robot should be inputted "
- "for this task configuration!")
- else:
- # This is an unknown env configuration, print error
- raise ValueError("Error: Unknown environment configuration received. Only 'bimanual',"
- "'single-arm-parallel', and 'single-arm-opposed' are supported. Got: {}"
- .format(self.env_configuration))
-
- # Lastly, check to make sure all inputted robot names are of their correct type (bimanual / not bimanual)
- for robot in robots:
- if check_bimanual(robot) != is_bimanual:
- raise ValueError("Error: For {} configuration, expected bimanual check to return {}; "
- "instead, got {}.".format(self.env_configuration, is_bimanual, check_bimanual(robot)))
diff --git a/robosuite/environments/wipe.py b/robosuite/environments/manipulation/wipe.py
similarity index 71%
rename from robosuite/environments/wipe.py
rename to robosuite/environments/manipulation/wipe.py
index 8e4e0c7715..f071dd3d72 100644
--- a/robosuite/environments/wipe.py
+++ b/robosuite/environments/manipulation/wipe.py
@@ -1,11 +1,10 @@
from collections import OrderedDict
import numpy as np
-from robosuite.environments.robot_env import RobotEnv
-from robosuite.robots import SingleArm
+from robosuite.environments.manipulation.single_arm_env import SingleArmEnv
from robosuite.models.arenas import WipeArena
-from robosuite.models.tasks import ManipulationTask, UniformRandomSampler
+from robosuite.models.tasks import ManipulationTask
import multiprocessing
@@ -16,36 +15,38 @@
"wipe_contact_reward": 0.01, # reward for contacting something with the wiping tool
"unit_wiped_reward": 50.0, # reward per peg wiped
"ee_accel_penalty": 0, # penalty for large end-effector accelerations
- "excess_force_penalty_mul": 0.01, # penalty for each step that the force is over the safety threshold
+ "excess_force_penalty_mul": 0.05, # penalty for each step that the force is over the safety threshold
"distance_multiplier": 5.0, # multiplier for the dense reward inversely proportional to the mean location of the pegs to wipe
"distance_th_multiplier": 5.0, # multiplier in the tanh function for the aforementioned reward
# settings for table top
- "table_full_size": [0.6, 0.8, 0.05], # Size of tabletop
- "table_offset": [0, 0, 0.8], # Offset of table (z dimension defines max height of table)
- "table_friction": [0.00001, 0.005, 0.0001], # Friction parameters for the table
+ "table_full_size": [0.5, 0.8, 0.05], # Size of tabletop
+ "table_offset": [0.15, 0, 0.9], # Offset of table (z dimension defines max height of table)
+ "table_friction": [0.03, 0.005, 0.0001], # Friction parameters for the table
"table_friction_std": 0, # Standard deviation to sample different friction parameters for the table each episode
"table_height": 0.0, # Additional height of the table over the default location
"table_height_std": 0.0, # Standard deviation to sample different heigths of the table each episode
"line_width": 0.04, # Width of the line to wipe (diameter of the pegs)
"two_clusters": False, # if the dirt to wipe is one continuous line or two
"coverage_factor": 0.6, # how much of the table surface we cover
- "num_sensors": 50, # How many particles of dirt to generate in the environment
+ "num_markers": 100, # How many particles of dirt to generate in the environment
# settings for thresholds
- "contact_threshold": 3, # Minimum eef force to qualify as contact [N]
- "touch_threshold": 5, # force threshold (N) to overcome to change the color of the sensor (wipe the peg)
- "pressure_threshold_max": 70, # maximum force allowed (N)
+ "contact_threshold": 1.0, # Minimum eef force to qualify as contact [N]
+ "pressure_threshold": 0.5, # force threshold (N) to overcome to get increased contact wiping reward
+ "pressure_threshold_max": 60., # maximum force allowed (N)
# misc settings
"print_results": False, # Whether to print results or not
"get_info": False, # Whether to grab info after each env step if not
"use_robot_obs": True, # if we use robot observations (proprioception) as input to the policy
- "early_terminations": False, # Whether we allow for early terminations or not
+ "use_contact_obs": True, # if we use a binary observation for whether robot is in contact or not
+ "early_terminations": True, # Whether we allow for early terminations or not
+ "use_condensed_obj_obs": True, # Whether to use condensed object observation representation (only applicable if obj obs is active)
}
-class Wipe(RobotEnv):
+class Wipe(SingleArmEnv):
"""
This class corresponds to the Wiping task for a single robot arm
@@ -54,6 +55,9 @@ class Wipe(RobotEnv):
(e.g: "Sawyer" would generate one arm; ["Panda", "Panda", "Sawyer"] would generate three robot arms)
Note: Must be a single single-arm robot!
+ env_configuration (str): Specifies how to position the robots within the environment (default is "default").
+ For most single arm environments, this argument has no impact on the robot setup.
+
controller_configs (str or list of dict): If set, contains relevant controller parameters for creating a
custom controller. Else, uses the default controller for this specific task. Should either be single
dict if same controller is to be used for all robots or else it should be a list of the same length as
@@ -64,10 +68,6 @@ class Wipe(RobotEnv):
For this environment, setting a value other than the default ("WipingGripper") will raise an
AssertionError, as this environment is not meant to be used with any other alternative gripper.
- gripper_visualizations (bool or list of bool): True if using gripper visualization.
- Useful for teleoperation. Should either be single bool if gripper visualization is to be used for all
- robots or else it should be a list of the same length as "robots" param
-
initialization_noise (dict or list of dict): Dict containing the initialization noise parameters.
The expected keys and corresponding value types are specified below:
@@ -93,13 +93,6 @@ class Wipe(RobotEnv):
reward_shaping (bool): if True, use dense rewards.
- placement_initializer (ObjectPositionSampler instance): if provided, will
- be used to place objects on every reset, else a UniformRandomSampler
- is used by default.
-
- use_indicator_object (bool): if True, sets up an indicator object that
- is useful for debugging.
-
has_renderer (bool): If true, render the simulation state in
a viewer instead of headless mode.
@@ -113,6 +106,10 @@ class Wipe(RobotEnv):
render_visual_mesh (bool): True if rendering visual meshes in camera. False otherwise.
+ render_gpu_device_id (int): corresponds to the GPU device id to use for offscreen rendering.
+ Defaults to -1, in which case the device will be inferred from environment variables
+ (GPUS or CUDA_VISIBLE_DEVICES).
+
control_freq (float): how many control signals to receive in every second. This sets the amount of
simulation time that passes between every action input.
@@ -157,22 +154,21 @@ class Wipe(RobotEnv):
def __init__(
self,
robots,
+ env_configuration="default",
controller_configs=None,
gripper_types="WipingGripper",
- gripper_visualizations=False,
initialization_noise="default",
use_camera_obs=True,
use_object_obs=True,
reward_scale=1.0,
reward_shaping=True,
- placement_initializer=None,
- use_indicator_object=False,
has_renderer=False,
has_offscreen_renderer=True,
render_camera="frontview",
render_collision_mesh=False,
render_visual_mesh=True,
- control_freq=10,
+ render_gpu_device_id=-1,
+ control_freq=20,
horizon=1000,
ignore_done=False,
hard_reset=True,
@@ -182,9 +178,6 @@ def __init__(
camera_depths=False,
task_config=None,
):
- # First, verify that only one robot is being inputted
- self._check_robot_configuration(robots)
-
# Assert that the gripper type is None
assert gripper_types == "WipingGripper",\
"Tried to specify gripper other than WipingGripper in Wipe environment!"
@@ -207,38 +200,40 @@ def __init__(
# Final reward computation
# So that is better to finish that to stay touching the table for 100 steps
# The 0.5 comes from continuous_distance_reward at 0. If something changes, this may change as well
- self.task_complete_reward = 50 * (self.wipe_contact_reward + 0.5)
+ self.task_complete_reward = self.unit_wiped_reward * (self.wipe_contact_reward + 0.5)
# Verify that the distance multiplier is not greater than the task complete reward
assert self.task_complete_reward > self.distance_multiplier,\
"Distance multiplier cannot be greater than task complete reward!"
# settings for table top
self.table_full_size = self.task_config['table_full_size']
- self.table_offset = self.task_config['table_offset']
- self.table_friction = self.task_config['table_friction']
- self.table_friction_std = self.task_config['table_friction_std']
self.table_height = self.task_config['table_height']
self.table_height_std = self.task_config['table_height_std']
+ delta_height = min(0, np.random.normal(self.table_height, self.table_height_std)) # sample variation in height
+ self.table_offset = np.array(self.task_config['table_offset']) + np.array((0, 0, delta_height))
+ self.table_friction = self.task_config['table_friction']
+ self.table_friction_std = self.task_config['table_friction_std']
self.line_width = self.task_config['line_width']
self.two_clusters = self.task_config['two_clusters']
self.coverage_factor = self.task_config['coverage_factor']
- self.num_sensors = self.task_config['num_sensors']
+ self.num_markers = self.task_config['num_markers']
# settings for thresholds
self.contact_threshold = self.task_config['contact_threshold']
- self.touch_threshold = self.task_config['touch_threshold']
- self.pressure_threshold = self.task_config['touch_threshold']
+ self.pressure_threshold = self.task_config['pressure_threshold']
self.pressure_threshold_max = self.task_config['pressure_threshold_max']
# misc settings
self.print_results = self.task_config['print_results']
self.get_info = self.task_config['get_info']
self.use_robot_obs = self.task_config['use_robot_obs']
+ self.use_contact_obs = self.task_config['use_contact_obs']
self.early_terminations = self.task_config['early_terminations']
+ self.use_condensed_obj_obs = self.task_config['use_condensed_obj_obs']
# Scale reward if desired (see reward method for details)
self.reward_normalization_factor = horizon / \
- (self.num_sensors * self.unit_wiped_reward +
+ (self.num_markers * self.unit_wiped_reward +
horizon * (self.wipe_contact_reward + self.task_complete_reward))
# ee resets
@@ -246,7 +241,7 @@ def __init__(
self.ee_torque_bias = np.zeros(3)
# set other wipe-specific attributes
- self.wiped_sensors = []
+ self.wiped_markers = []
self.collisions = 0
self.f_excess = 0
self.metadata = []
@@ -255,29 +250,20 @@ def __init__(
# whether to include and use ground-truth object states
self.use_object_obs = use_object_obs
- # object placement initializer
- if placement_initializer:
- self.placement_initializer = placement_initializer
- else:
- self.placement_initializer = UniformRandomSampler(
- x_range=[0, 0.2],
- y_range=[0, 0.2],
- ensure_object_boundary_in_range=False,
- rotation=None)
-
super().__init__(
robots=robots,
+ env_configuration=env_configuration,
controller_configs=controller_configs,
+ mount_types="default",
gripper_types=gripper_types,
- gripper_visualizations=gripper_visualizations,
initialization_noise=initialization_noise,
use_camera_obs=use_camera_obs,
- use_indicator_object=use_indicator_object,
has_renderer=has_renderer,
has_offscreen_renderer=has_offscreen_renderer,
render_camera=render_camera,
render_collision_mesh=render_collision_mesh,
render_visual_mesh=render_visual_mesh,
+ render_gpu_device_id=render_gpu_device_id,
control_freq=control_freq,
horizon=horizon,
ignore_done=ignore_done,
@@ -288,7 +274,7 @@ def __init__(
camera_depths=camera_depths,
)
- def reward(self, action):
+ def reward(self, action=None):
"""
Reward function for the task.
@@ -316,7 +302,7 @@ def reward(self, action):
Note that the final per-step reward is normalized given the theoretical best episode return and then scaled:
reward_scale * (horizon /
- (num_sensors * unit_wiped_reward + horizon * (wipe_contact_reward + task_complete_reward)))
+ (num_markers * unit_wiped_reward + horizon * (wipe_contact_reward + task_complete_reward)))
Args:
action (np array): [NOT USED]
@@ -329,18 +315,18 @@ def reward(self, action):
total_force_ee = np.linalg.norm(np.array(self.robots[0].recent_ee_forcetorques.current[:3]))
# Neg Reward from collisions of the arm with the table
- if self._check_arm_contact()[0]:
+ if self.check_contact(self.robots[0].robot_model):
if self.reward_shaping:
reward = self.arm_limit_collision_penalty
self.collisions += 1
- elif self._check_q_limits()[0]:
+ elif self.robots[0].check_q_limits():
if self.reward_shaping:
reward = self.arm_limit_collision_penalty
self.collisions += 1
else:
# If the arm is not colliding or in joint limits, we check if we are wiping
# (we don't want to reward wiping if there are unsafe situations)
- sensors_active_ids = []
+ active_markers = []
# Current 3D location of the corners of the wiping tool in world frame
c_geoms = self.robots[0].gripper.important_geoms["corners"]
@@ -380,75 +366,58 @@ def PointInRectangle(X, Y, Z, W, P):
# Only go into this computation if there are contact points
if self.sim.data.ncon != 0:
- # Check each sensor that is still active
- for sensor_name in self.model.arena.sensor_names:
+ # Check each marker that is still active
+ for marker in self.model.mujoco_arena.markers:
- # Current sensor 3D location in world frame
- # sensor_pos = np.array(
- # self.sim.data.body_xpos[self.sim.model.site_bodyid[self.sim.model.site_name2id(self.model.arena.sensor_site_names[sensor_name])]])
- sensor_pos = np.array(
- self.sim.data.site_xpos[
- self.sim.model.site_name2id(self.model.arena.sensor_site_names[sensor_name])])
+ # Current marker 3D location in world frame
+ marker_pos = np.array(self.sim.data.body_xpos[self.sim.model.body_name2id(marker.root_body)])
# We use the second tool corner as point on the plane and define the vector connecting
- # the sensor position to that point
- v = sensor_pos - corner2_pos
+ # the marker position to that point
+ v = marker_pos - corner2_pos
- # Shortest distance between the center of the sensor and the plane
+ # Shortest distance between the center of the marker and the plane
dist = np.dot(v, n)
- # Projection of the center of the sensor onto the plane
- projected_point = np.array(sensor_pos) - dist * n
+ # Projection of the center of the marker onto the plane
+ projected_point = np.array(marker_pos) - dist * n
- # Positive distances means the center of the sensor is over the plane
- # The plane is aligned with the bottom of the wiper and pointing up, so the sensor would be over it
+ # Positive distances means the center of the marker is over the plane
+ # The plane is aligned with the bottom of the wiper and pointing up, so the marker would be over it
if dist > 0.0:
# Distance smaller than this threshold means we are close to the plane on the upper part
if dist < 0.02:
# Write touching points and projected point in coordinates of the plane
pp_2 = np.array(
[np.dot(projected_point - corner2_pos, v1), np.dot(projected_point - corner2_pos, v2)])
- # Check if sensor is within the tool center:
+ # Check if marker is within the tool center:
if PointInRectangle(pp[0], pp[1], pp[2], pp[3], pp_2):
- parts = sensor_name.split('_')
- sensors_active_ids += [int(parts[1])]
-
- # Obtain the list of currently active (wiped) sensors that where not wiped before
- # These are the sensors we are wiping at this step
- lall = np.where(np.isin(sensors_active_ids, self.wiped_sensors, invert=True))
- new_sensors_active_ids = np.array(sensors_active_ids)[lall]
-
- # Loop through all new sensors we are wiping at this step
- for new_sensor_active_id in new_sensors_active_ids:
- # Grab relevant sensor id info
- sensor_name = self.model.arena.sensor_site_names['contact_' + str(new_sensor_active_id) + '_sensor']
- new_sensor_active_geom_id = self.sim.model.geom_name2id(sensor_name)
- # Make this sensor transparent since we wiped it (alpha = 0)
- self.sim.model.geom_rgba[new_sensor_active_geom_id] = [0, 0, 0, 0]
- # Add this sensor the wiped list
- self.wiped_sensors += [new_sensor_active_id]
+ active_markers.append(marker)
+
+ # Obtain the list of currently active (wiped) markers that where not wiped before
+ # These are the markers we are wiping at this step
+ lall = np.where(np.isin(active_markers, self.wiped_markers, invert=True))
+ new_active_markers = np.array(active_markers)[lall]
+
+ # Loop through all new markers we are wiping at this step
+ for new_active_marker in new_active_markers:
+ # Grab relevant marker id info
+ new_active_marker_geom_id = self.sim.model.geom_name2id(new_active_marker.visual_geoms[0])
+ # Make this marker transparent since we wiped it (alpha = 0)
+ self.sim.model.geom_rgba[new_active_marker_geom_id][3] = 0
+ # Add this marker the wiped list
+ self.wiped_markers.append(new_active_marker)
# Add reward if we're using the dense reward
if self.reward_shaping:
reward += self.unit_wiped_reward
# Additional reward components if using dense rewards
if self.reward_shaping:
- # If we haven't wiped all the sensors yet, add a smooth reward for getting closer
+ # If we haven't wiped all the markers yet, add a smooth reward for getting closer
# to the centroid of the dirt to wipe
- if len(self.wiped_sensors) < len(self.model.arena.sensor_names):
- mean_distance_to_things_to_wipe = 0
- num_non_wiped_sensors = 0
- for sensor_name in self.model.arena.sensor_names:
- parts = sensor_name.split('_')
- sensor_id = int(parts[1])
- if sensor_id not in self.wiped_sensors:
- sensor_pos = np.array(
- self.sim.data.site_xpos[
- self.sim.model.site_name2id(self.model.arena.sensor_site_names[sensor_name])])
- gripper_position = np.array(self.sim.data.site_xpos[self.robots[0].eef_site_id])
- mean_distance_to_things_to_wipe += np.linalg.norm(gripper_position - sensor_pos)
- num_non_wiped_sensors += 1
- mean_distance_to_things_to_wipe /= max(1, num_non_wiped_sensors)
+ if len(self.wiped_markers) < self.num_markers:
+ _, _, mean_pos_to_things_to_wipe = self._get_wipe_information
+ mean_distance_to_things_to_wipe = np.linalg.norm(mean_pos_to_things_to_wipe)
reward += self.distance_multiplier * (
1 - np.tanh(self.distance_th_multiplier * mean_distance_to_things_to_wipe))
@@ -461,28 +430,36 @@ def PointInRectangle(X, Y, Z, W, P):
reward -= self.excess_force_penalty_mul * total_force_ee
self.f_excess += 1
+ # Reward for pressing into table
+ # TODO: Need to include this computation somehow in the scaled reward computation
+ elif total_force_ee > self.pressure_threshold and self.sim.data.ncon > 1:
+ reward += self.wipe_contact_reward + 0.01 * total_force_ee
+ if self.sim.data.ncon > 50:
+ reward += 10. * self.wipe_contact_reward
+
# Penalize large accelerations
reward -= self.ee_accel_penalty * np.mean(abs(self.robots[0].recent_ee_acc.current))
# Final reward if all wiped
- if len(self.wiped_sensors) == len(self.model.arena.sensor_names):
+ if len(self.wiped_markers) == self.num_markers:
reward += self.task_complete_reward
# Printing results
if self.print_results:
- string_to_print = 'Process {pid}, timestep {ts:>4}: reward: {rw:8.4f} wiped sensors: {ws:>3} collisions: {sc:>3} f-excess: {fe:>3}'.format(
- pid=id(multiprocessing.current_process()),
- ts=self.timestep,
- rw=reward,
- ws=len(self.wiped_sensors),
- sc=self.collisions,
- fe=self.f_excess)
+ string_to_print = 'Process {pid}, timestep {ts:>4}: reward: {rw:8.4f}' \
+ 'wiped markers: {ws:>3} collisions: {sc:>3} f-excess: {fe:>3}'.format(
+ pid=id(multiprocessing.current_process()),
+ ts=self.timestep,
+ rw=reward,
+ ws=len(self.wiped_markers),
+ sc=self.collisions,
+ fe=self.f_excess)
print(string_to_print)
# If we're scaling our reward, we normalize the per-step rewards given the theoretical best episode return
# This is equivalent to scaling the reward by:
# reward_scale * (horizon /
- # (num_sensors * unit_wiped_reward + horizon * (wipe_contact_reward + task_complete_reward)))
+ # (num_markers * unit_wiped_reward + horizon * (wipe_contact_reward + task_complete_reward)))
if self.reward_scale:
reward *= self.reward_scale * self.reward_normalization_factor
return reward
@@ -493,10 +470,6 @@ def _load_model(self):
"""
super()._load_model()
- # Verify the correct robot has been loaded
- assert isinstance(self.robots[0], SingleArm), \
- "Error: Expected one single-armed robot! Got {} type instead.".format(type(self.robots[0]))
-
# Adjust base pose accordingly
xpos = self.robots[0].robot_model.base_xpos_offset["table"](self.table_full_size[0])
self.robots[0].robot_model.set_base_xpos(xpos)
@@ -504,51 +477,42 @@ def _load_model(self):
# Get robot's contact geoms
self.robot_contact_geoms = self.robots[0].robot_model.contact_geoms
- # Delta goes down
- delta_height = min(0, np.random.normal(self.table_height, self.table_height_std))
-
- self.mujoco_arena = WipeArena(
+ mujoco_arena = WipeArena(
table_full_size=self.table_full_size,
table_friction=self.table_friction,
- table_offset=np.array(self.table_offset) + np.array((0, 0, delta_height)),
+ table_offset=self.table_offset,
table_friction_std=self.table_friction_std,
coverage_factor=self.coverage_factor,
- num_sensors=self.num_sensors,
+ num_markers=self.num_markers,
line_width=self.line_width,
two_clusters=self.two_clusters
)
- if self.use_indicator_object:
- self.mujoco_arena.add_pos_indicator()
# Arena always gets set to zero origin
- self.mujoco_arena.set_origin([0, 0, 0])
-
- self.mujoco_objects = OrderedDict()
+ mujoco_arena.set_origin([0, 0, 0])
# task includes arena, robot, and objects of interest
- self.model = ManipulationTask(self.mujoco_arena,
- [robot.robot_model for robot in self.robots],
- self.mujoco_objects,
- initializer=self.placement_initializer)
- self.model.place_objects()
+ self.model = ManipulationTask(
+ mujoco_arena=mujoco_arena,
+ mujoco_robots=[robot.robot_model for robot in self.robots],
+ )
def _reset_internal(self):
super()._reset_internal()
# inherited class should reset positions of objects (only if we're not using a deterministic reset)
if not self.deterministic_reset:
- self.model.place_objects()
- self.mujoco_arena.reset_arena(self.sim)
+ self.model.mujoco_arena.reset_arena(self.sim)
# Reset all internal vars for this wipe task
self.timestep = 0
- self.wiped_sensors = []
+ self.wiped_markers = []
self.collisions = 0
self.f_excess = 0
# ee resets - bias at initial state
- self.ee_force_bias = self.robots[0].ee_force
- self.ee_torque_bias = self.robots[0].ee_torque
+ self.ee_force_bias = np.zeros(3)
+ self.ee_torque_bias = np.zeros(3)
def _get_observation(self):
"""
@@ -574,29 +538,42 @@ def _get_observation(self):
pf = self.robots[0].robot_model.naming_prefix
# Add binary contact observation
- di[pf + "contact-obs"] = self._has_gripper_contact
- di[pf + "robot-state"] = np.concatenate((di[pf + "robot-state"], [di[pf + "contact-obs"]]))
+ if self.use_contact_obs:
+ di[pf + "contact-obs"] = self._has_gripper_contact
+ di[pf + "robot-state"] = np.concatenate((di[pf + "robot-state"], [di[pf + "contact-obs"]]))
# object information in the observation
if self.use_object_obs:
- gripper_site_pos = np.array(self.sim.data.site_xpos[self.robots[0].eef_site_id])
- # position of objects to wipe
- acc = np.array([])
- for sensor_name in self.model.arena.sensor_names:
- parts = sensor_name.split('_')
- sensor_id = int(parts[1])
- sensor_pos = np.array(
- self.sim.data.site_xpos[
- self.sim.model.site_name2id(self.model.arena.sensor_site_names[sensor_name])])
- di['sensor' + str(sensor_id) + '_pos'] = sensor_pos
- acc = np.concatenate([acc, di['sensor' + str(sensor_id) + '_pos']])
- di['sensor' + str(sensor_id) + '_wiped'] = [0, 1][sensor_id in self.wiped_sensors]
- acc = np.concatenate([acc, [di['sensor' + str(sensor_id) + '_wiped']]])
- # proprioception
+ gripper_site_pos = self._eef_xpos
+
+ if self.use_condensed_obj_obs:
+ # use implicit representation of wiping objects
+ wipe_radius, wipe_centroid, gripper_to_wipe_centroid = self._get_wipe_information
+ di["proportion_wiped"] = len(self.wiped_markers) / self.num_markers
+ di["wipe_radius"] = wipe_radius
+ di["wipe_centroid"] = wipe_centroid
+ di["object-state"] = np.concatenate([
+ [di["proportion_wiped"]], [di["wipe_radius"]], di["wipe_centroid"],
+ ])
if self.use_robot_obs:
- di['gripper_to_sensor' + str(sensor_id)] = gripper_site_pos - sensor_pos
- acc = np.concatenate([acc, di['gripper_to_sensor' + str(sensor_id)]])
- di['object-state'] = acc
+ # also use ego-centric proprioception
+ di["gripper_to_wipe_centroid"] = gripper_to_wipe_centroid
+ di["object-state"] = np.concatenate([di["object-state"], di["gripper_to_wipe_centroid"]])
+
+ else:
+ # use explicit representation of wiping objects
+ acc = np.array([])
+ for i, marker in enumerate(self.model.mujoco_arena.markers):
+ marker_pos = np.array(self.sim.data.body_xpos[self.sim.model.body_name2id(marker.root_body)])
+ di[f'marker{i}_pos'] = marker_pos
+ acc = np.concatenate([acc, di[f'marker{i}_pos']])
+ di[f'marker{i}_wiped'] = [0, 1][marker in self.wiped_markers]
+ acc = np.concatenate([acc, [di[f'marker{i}_wiped']]])
+ if self.use_robot_obs:
+ # also use ego-centric proprioception
+ di[f'gripper_to_marker{i}'] = gripper_site_pos - marker_pos
+ acc = np.concatenate([acc, di[f'gripper_to_marker{i}']])
+ di['object-state'] = acc
return di
@@ -607,7 +584,7 @@ def _check_success(self):
Returns:
bool: True if completed task
"""
- return True if len(self.wiped_sensors) == len(self.model.arena.sensor_names) else False
+ return True if len(self.wiped_markers) == self.num_markers else False
def _check_terminated(self):
"""
@@ -624,7 +601,7 @@ def _check_terminated(self):
terminated = False
# Prematurely terminate if contacting the table with the arm
- if self._check_arm_contact()[0]:
+ if self.check_contact(self.robots[0].robot_model):
if self.print_results:
print(40 * '-' + " COLLIDED " + 40 * '-')
terminated = True
@@ -636,7 +613,7 @@ def _check_terminated(self):
terminated = True
# Prematurely terminate if contacting the table with the arm
- if self._check_q_limits()[0]:
+ if self.robots[0].check_q_limits():
if self.print_results:
print(40 * '-' + " JOINT LIMIT " + 40 * '-')
terminated = True
@@ -659,11 +636,16 @@ def _post_action(self, action):
"""
reward, done, info = super()._post_action(action)
+ # Update force bias
+ if np.linalg.norm(self.ee_force_bias) == 0:
+ self.ee_force_bias = self.robots[0].ee_force
+ self.ee_torque_bias = self.robots[0].ee_torque
+
if self.get_info:
- info['add_vals'] = ['nwipedsensors', 'colls', 'percent_viapoints_', 'f_excess']
- info['nwipedsensors'] = len(self.wiped_sensors)
+ info['add_vals'] = ['nwipedmarkers', 'colls', 'percent_viapoints_', 'f_excess']
+ info['nwipedmarkers'] = len(self.wiped_markers)
info['colls'] = self.collisions
- info['percent_viapoints_'] = len(self.wiped_sensors) / len(self.model.arena.sensor_names)
+ info['percent_viapoints_'] = len(self.wiped_markers) / self.num_markers
info['f_excess'] = self.f_excess
# allow episode to finish early if allowed
@@ -672,15 +654,28 @@ def _post_action(self, action):
return reward, done, info
- def _check_robot_configuration(self, robots):
- """
- Sanity check to make sure the inputted robots and configuration is acceptable
-
- Args:
- robots (str or list of str): Robots to instantiate within this env
- """
- if type(robots) is list:
- assert len(robots) == 1, "Error: Only one robot should be inputted for this task!"
+ @property
+ def _get_wipe_information(self):
+ """Returns set of wiping information"""
+ mean_pos_to_things_to_wipe = np.zeros(3)
+ wipe_centroid = np.zeros(3)
+ marker_positions = []
+ num_non_wiped_markers = 0
+ if len(self.wiped_markers) < self.num_markers:
+ for marker in self.model.mujoco_arena.markers:
+ if marker not in self.wiped_markers:
+ marker_pos = np.array(self.sim.data.body_xpos[self.sim.model.body_name2id(marker.root_body)])
+ wipe_centroid += marker_pos
+ marker_positions.append(marker_pos)
+ num_non_wiped_markers += 1
+ wipe_centroid /= max(1, num_non_wiped_markers)
+ mean_pos_to_things_to_wipe = wipe_centroid - self._eef_xpos
+ # Radius of circle from centroid capturing all remaining wiping markers
+ max_radius = 0
+ if num_non_wiped_markers > 0:
+ max_radius = np.max(np.linalg.norm(np.array(marker_positions) - wipe_centroid, axis=1))
+ # Return all values
+ return max_radius, wipe_centroid, mean_pos_to_things_to_wipe
@property
def _has_gripper_contact(self):
diff --git a/robosuite/environments/robot_env.py b/robosuite/environments/robot_env.py
index 6e79fa4630..27263589d6 100644
--- a/robosuite/environments/robot_env.py
+++ b/robosuite/environments/robot_env.py
@@ -1,11 +1,10 @@
import numpy as np
-from robosuite.environments.base import MujocoEnv
-
-from robosuite.robots.single_arm import SingleArm
-from robosuite.robots.bimanual import Bimanual
-from robosuite.models.robots import check_bimanual
+import robosuite.utils.macros as macros
+from robosuite.utils.mjcf_utils import IMAGE_CONVENTION_MAPPING
+from robosuite.environments.base import MujocoEnv
+from robosuite.robots import ROBOT_CLASS_MAPPING
from robosuite.controllers import reset_controllers
@@ -14,23 +13,21 @@ class RobotEnv(MujocoEnv):
Initializes a robot environment in Mujoco.
Args:
- robots: Specification for specific robot arm(s) to be instantiated within this env
- (e.g: "Sawyer" would generate one arm; ["Panda", "Panda", "Sawyer"] would generate three robot arms)
+ robots: Specification for specific robot(s) to be instantiated within this env
+
+ env_configuration (str): Specifies how to position the robot(s) within the environment. Default is "default",
+ which should be interpreted accordingly by any subclasses.
controller_configs (str or list of dict): If set, contains relevant controller parameters for creating a
custom controller. Else, uses the default controller for this specific task. Should either be single
dict if same controller is to be used for all robots or else it should be a list of the same length as
"robots" param
- gripper_types (None or str or list of str): type of gripper, used to instantiate
- gripper models from gripper factory. Default is "default", which is the default grippers(s) associated
- with the robot(s) the 'robots' specification. None removes the gripper, and any other (valid) model
- overrides the default gripper. Should either be single str if same gripper type is to be used for all
- robots or else it should be a list of the same length as "robots" param
-
- gripper_visualizations (bool or list of bool): True if using gripper visualization.
- Useful for teleoperation. Should either be single bool if gripper visualization is to be used for all
- robots or else it should be a list of the same length as "robots" param
+ mount_types (None or str or list of str): type of mount, used to instantiate mount models from mount factory.
+ Default is "default", which is the default mount associated with the robot(s) the 'robots' specification.
+ None results in no mount, and any other (valid) model overrides the default mount. Should either be
+ single str if same mount type is to be used for all robots or else it should be a list of the same
+ length as "robots" param
initialization_noise (dict or list of dict): Dict containing the initialization noise parameters.
The expected keys and corresponding value types are specified below:
@@ -49,9 +46,6 @@ class RobotEnv(MujocoEnv):
use_camera_obs (bool): if True, every observation includes rendered image(s)
- use_indicator_object (bool): if True, sets up an indicator object that
- is useful for debugging.
-
has_renderer (bool): If true, render the simulation state in
a viewer instead of headless mode.
@@ -65,6 +59,10 @@ class RobotEnv(MujocoEnv):
render_visual_mesh (bool): True if rendering visual meshes in camera. False otherwise.
+ render_gpu_device_id (int): corresponds to the GPU device id to use for offscreen rendering.
+ Defaults to -1, in which case the device will be inferred from environment variables
+ (GPUS or CUDA_VISIBLE_DEVICES).
+
control_freq (float): how many control signals to receive in every second. This sets the amount of
simulation time that passes between every action input.
@@ -96,6 +94,8 @@ class RobotEnv(MujocoEnv):
bool if same depth setting is to be used for all cameras or else it should be a list of the same length as
"camera names" param.
+ robot_configs (list of dict): Per-robot configurations set from any subclass initializers.
+
Raises:
ValueError: [Camera obs require offscreen renderer]
ValueError: [Camera name must be specified to use camera obs]
@@ -104,18 +104,18 @@ class RobotEnv(MujocoEnv):
def __init__(
self,
robots,
+ env_configuration="default",
+ mount_types="default",
controller_configs=None,
- gripper_types="default",
- gripper_visualizations=False,
initialization_noise=None,
use_camera_obs=True,
- use_indicator_object=False,
has_renderer=False,
has_offscreen_renderer=True,
render_camera="frontview",
render_collision_mesh=False,
render_visual_mesh=True,
- control_freq=10,
+ render_gpu_device_id=-1,
+ control_freq=20,
horizon=1000,
ignore_done=False,
hard_reset=True,
@@ -123,7 +123,12 @@ def __init__(
camera_heights=256,
camera_widths=256,
camera_depths=False,
+ robot_configs=None,
):
+ # First, verify that correct number of robots are being inputted
+ self.env_configuration = env_configuration
+ self._check_robot_configuration(robots)
+
# Robot
robots = list(robots) if type(robots) is list or type(robots) is tuple else [robots]
self.num_robots = len(robots)
@@ -131,16 +136,15 @@ def __init__(
self.robots = self._input2list(None, self.num_robots)
self._action_dim = None
+ # Mount
+ mount_types = self._input2list(mount_types, self.num_robots)
+
# Controller
controller_configs = self._input2list(controller_configs, self.num_robots)
# Initialization Noise
initialization_noise = self._input2list(initialization_noise, self.num_robots)
- # Gripper
- gripper_types = self._input2list(gripper_types, self.num_robots)
- gripper_visualizations = self._input2list(gripper_visualizations, self.num_robots)
-
# Observations -- Ground truth = object_obs, Image data = camera_obs
self.use_camera_obs = use_camera_obs
@@ -160,21 +164,22 @@ def __init__(
if self.use_camera_obs and self.camera_names is None:
raise ValueError("Must specify at least one camera name when using camera obs")
- # Robot configurations
+ # Robot configurations -- update from subclass configs
+ if robot_configs is None:
+ robot_configs = [{} for _ in range(self.num_robots)]
self.robot_configs = [
- {
- "controller_config": controller_configs[idx],
- "initialization_noise": initialization_noise[idx],
- "gripper_type": gripper_types[idx],
- "gripper_visualization": gripper_visualizations[idx],
- "control_freq": control_freq
- }
- for idx in range(self.num_robots)
+ dict(
+ **{
+ "controller_config": controller_configs[idx],
+ "mount_type": mount_types[idx],
+ "initialization_noise": initialization_noise[idx],
+ "control_freq": control_freq
+ },
+ **robot_config,
+ )
+ for idx, robot_config in enumerate(robot_configs)
]
- # whether to use indicator object or not
- self.use_indicator_object = use_indicator_object
-
# Run superclass init
super().__init__(
has_renderer=has_renderer,
@@ -182,12 +187,40 @@ def __init__(
render_camera=render_camera,
render_collision_mesh=render_collision_mesh,
render_visual_mesh=render_visual_mesh,
+ render_gpu_device_id=render_gpu_device_id,
control_freq=control_freq,
horizon=horizon,
ignore_done=ignore_done,
hard_reset=hard_reset,
)
+ def visualize(self, vis_settings):
+ """
+ In addition to super call, visualizes robots.
+
+ Args:
+ vis_settings (dict): Visualization keywords mapped to T/F, determining whether that specific
+ component should be visualized. Should have "robots" keyword as well as any other relevant
+ options specified.
+ """
+ # Run superclass method first
+ super().visualize(vis_settings=vis_settings)
+ # Loop over robots to visualize them independently
+ for robot in self.robots:
+ robot.visualize(vis_settings=vis_settings)
+
+ @property
+ def _visualizations(self):
+ """
+ Visualization keywords for this environment
+
+ Returns:
+ set: All components that can be individually visualized for this environment
+ """
+ vis_set = super()._visualizations
+ vis_set.add("robots")
+ return vis_set
+
@property
def action_spec(self):
"""
@@ -215,17 +248,6 @@ def action_dim(self):
"""
return self._action_dim
- def move_indicator(self, pos):
- """
- Sets 3d position of indicator object to @pos.
-
- Args:
- pos (3-tuple): (x,y,z) values to place the indicator within the env
- """
- if self.use_indicator_object:
- index = self._ref_indicator_pos_low
- self.sim.data.qpos[index : index + 3] = pos
-
@staticmethod
def _input2list(inp, length):
"""
@@ -263,16 +285,6 @@ def _get_reference(self):
robot.reset_sim(self.sim)
robot.setup_references()
- # Indicator object references
- if self.use_indicator_object:
- ind_qpos = self.sim.model.get_joint_qpos_addr("pos_indicator")
- self._ref_indicator_pos_low, self._ref_indicator_pos_high = ind_qpos
-
- ind_qvel = self.sim.model.get_joint_qvel_addr("pos_indicator")
- self._ref_indicator_vel_low, self._ref_indicator_vel_high = ind_qvel
-
- self.indicator_id = self.sim.model.body_name2id("pos_indicator")
-
def _reset_internal(self):
"""
Resets simulation internal configurations.
@@ -348,33 +360,6 @@ def _pre_action(self, action, policy_step=False):
robot.control(robot_action, policy_step=policy_step)
cutoff += robot.action_dim
- # Also update indicator object if necessary
- if self.use_indicator_object:
- # Apply gravity compensation to indicator object too
- self.sim.data.qfrc_applied[
- self._ref_indicator_vel_low: self._ref_indicator_vel_high
- ] = self.sim.data.qfrc_bias[
- self._ref_indicator_vel_low: self._ref_indicator_vel_high]
-
- def _post_action(self, action):
- """
- Run any necessary visualization after running the action
-
- Args:
- action (np.array): Action being passed during this timestep
-
- Returns:
- 3-tuple:
-
- - (float) reward from the environment
- - (bool) whether the current episode is completed or not
- - (dict) empty dict to be filled with information by subclassed method
-
- """
- ret = super()._post_action(action)
- self._visualization()
- return ret
-
def _get_observation(self):
"""
Returns an OrderedDict containing observations [(name_string, np.array), ...].
@@ -399,6 +384,7 @@ def _get_observation(self):
# Loop through cameras and update the observations
if self.use_camera_obs:
+ convention = IMAGE_CONVENTION_MAPPING[macros.IMAGE_CONVENTION]
for (cam_name, cam_w, cam_h, cam_d) in \
zip(self.camera_names, self.camera_widths, self.camera_heights, self.camera_depths):
@@ -410,95 +396,13 @@ def _get_observation(self):
depth=cam_d,
)
if cam_d:
- di[cam_name + "_image"], di[cam_name + "_depth"] = camera_obs
+ rgb, depth = camera_obs
+ di[cam_name + "_image"], di[cam_name + "_depth"] = rgb[::convention], depth[::convention]
else:
- di[cam_name + "_image"] = camera_obs
+ di[cam_name + "_image"] = camera_obs[::convention]
return di
- def _check_gripper_contact(self):
- """
- Checks whether each gripper is in contact with an object.
-
- Returns:
- list of bool: True if the specific gripper is in contact with an object
- """
- collisions = [False] * self.num_robots
- for idx, robot in enumerate(self.robots):
- for contact in self.sim.data.contact[: self.sim.data.ncon]:
- # Single arm case
- if robot.arm_type == "single":
- if (
- self.sim.model.geom_id2name(contact.geom1)
- in robot.gripper.contact_geoms
- or self.sim.model.geom_id2name(contact.geom2)
- in robot.gripper.contact_geoms
- ):
- collisions[idx] = True
- break
- # Bimanual case
- else:
- for arm in robot.arms:
- if (
- self.sim.model.geom_id2name(contact.geom1)
- in robot.gripper[arm].contact_geoms
- or self.sim.model.geom_id2name(contact.geom2)
- in robot.gripper[arm].contact_geoms
- ):
- collisions[idx] = True
- break
- return collisions
-
- def _check_arm_contact(self):
- """
- Checks whether each robot arm is in contact with an object.
-
- Returns:
- list of bool: True if the specific gripper is in contact with an object
- """
- collisions = [False] * self.num_robots
- for idx, robot in enumerate(self.robots):
- for contact in self.sim.data.contact[: self.sim.data.ncon]:
- # Single arm case and Bimanual case are the same
- if (
- self.sim.model.geom_id2name(contact.geom1)
- in robot.robot_model.contact_geoms
- or self.sim.model.geom_id2name(contact.geom2)
- in robot.robot_model.contact_geoms
- ):
- collisions[idx] = True
- break
- return collisions
-
- def _check_q_limits(self):
- """
- Check if each robot arm is either very close or at the joint limits
-
- Returns:
- list of bool: True if the specific arm is near its joint limits
- """
- joint_limits = [False] * self.num_robots
- tolerance = 0.1
- for idx, robot in enumerate(self.robots):
- for (qidx, (q, q_limits)) in enumerate(
- zip(
- self.sim.data.qpos[robot._ref_joint_pos_indexes],
- self.sim.model.jnt_range[robot._ref_joint_indexes]
- )
- ):
- if q_limits[0] != q_limits[1] and not (q_limits[0] + tolerance < q < q_limits[1] - tolerance):
- print("Joint limit reached in joint " + str(qidx))
- joint_limits[idx] = True
- return joint_limits
-
- def _visualization(self):
- """
- Do any needed visualization here
- """
- # Loop over robot grippers to visualize them independently
- for robot in self.robots:
- robot.visualize_gripper()
-
def _load_robots(self):
"""
Instantiates robots and stores them within the self.robots attribute
@@ -506,19 +410,7 @@ def _load_robots(self):
# Loop through robots and instantiate Robot object for each
for idx, (name, config) in enumerate(zip(self.robot_names, self.robot_configs)):
# Create the robot instance
- if not check_bimanual(name):
- self.robots[idx] = SingleArm(
- robot_type=name,
- idn=idx,
- **config
- )
- else:
- self.robots[idx] = Bimanual(
- robot_type=name,
- idn=idx,
- **config
- )
-
+ self.robots[idx] = ROBOT_CLASS_MAPPING[name](robot_type=name, idn=idx, **config)
# Now, load the robot models
self.robots[idx].load_model()
diff --git a/robosuite/models/arenas/arena.py b/robosuite/models/arenas/arena.py
index 8bb94d367a..52646d15bc 100644
--- a/robosuite/models/arenas/arena.py
+++ b/robosuite/models/arenas/arena.py
@@ -1,13 +1,23 @@
import numpy as np
from robosuite.models.base import MujocoXML
-from robosuite.utils.mjcf_utils import array_to_string, string_to_array
-from robosuite.utils.mjcf_utils import new_geom, new_body, new_joint
+from robosuite.utils.mjcf_utils import array_to_string, string_to_array, \
+ new_geom, new_body, new_joint, ENVIRONMENT_COLLISION_COLOR, recolor_collision_geoms, find_elements, new_element
class Arena(MujocoXML):
"""Base arena class."""
+ def __init__(self, fname):
+ super().__init__(fname)
+ # Get references to floor and bottom
+ self.bottom_pos = np.zeros(3)
+ self.floor = self.worldbody.find("./geom[@name='floor']")
+
+ # Recolor all geoms
+ recolor_collision_geoms(root=self.worldbody, rgba=ENVIRONMENT_COLLISION_COLOR,
+ exclude=lambda e: True if e.get("name", None) == "floor" else False)
+
def set_origin(self, offset):
"""
Applies a constant offset to all objects.
@@ -21,18 +31,30 @@ def set_origin(self, offset):
new_pos = cur_pos + offset
node.set("pos", array_to_string(new_pos))
- def add_pos_indicator(self):
- """Adds a new position indicator."""
- body = new_body(name="pos_indicator")
- body.append(
- new_geom(
- "sphere",
- [0.03],
- rgba=[1, 0, 0, 0.5],
- group=1,
- contype="0",
- conaffinity="0",
- )
- )
- body.append(new_joint(type="free", name="pos_indicator"))
- self.worldbody.append(body)
+ def set_camera(self, camera_name, pos, quat, camera_attribs=None):
+ """
+ Sets a camera with @camera_name. If the camera already exists, then this overwrites its pos and quat values.
+
+ Args:
+ camera_name (str): Camera name to search for / create
+ pos (3-array): (x,y,z) coordinates of camera in world frame
+ quat (4-array): (w,x,y,z) quaternion of camera in world frame
+ camera_attribs (dict): If specified, should be additional keyword-mapped attributes for this camera.
+ See http://www.mujoco.org/book/XMLreference.html#camera for exact attribute specifications.
+ """
+ # Determine if camera already exists
+ camera = find_elements(root=self.worldbody, tags="camera", attribs={"name": camera_name}, return_first=True)
+
+ # Compose attributes
+ if camera_attribs is None:
+ camera_attribs = {}
+ camera_attribs["pos"] = array_to_string(pos)
+ camera_attribs["quat"] = array_to_string(quat)
+
+ if camera is None:
+ # If camera doesn't exist, then add a new camera with the specified attributes
+ self.worldbody.append(new_element(tag="camera", name=camera_name, **camera_attribs))
+ else:
+ # Otherwise, we edit all specified attributes in that camera
+ for attrib, value in camera_attribs.items():
+ camera.set(attrib, value)
diff --git a/robosuite/models/arenas/bins_arena.py b/robosuite/models/arenas/bins_arena.py
index 3b3f1f5ad1..6ce439990f 100644
--- a/robosuite/models/arenas/bins_arena.py
+++ b/robosuite/models/arenas/bins_arena.py
@@ -17,15 +17,12 @@ class BinsArena(Arena):
def __init__(
self, bin1_pos=(0.1, -0.5, 0.8), table_full_size=(0.39, 0.49, 0.82), table_friction=(1, 0.005, 0.0001)
):
- """
- """
super().__init__(xml_path_completion("arenas/bins_arena.xml"))
self.table_full_size = np.array(table_full_size)
self.table_half_size = self.table_full_size / 2
self.table_friction = table_friction
- self.floor = self.worldbody.find("./geom[@name='floor']")
self.bin1_body = self.worldbody.find("./body[@name='bin1']")
self.bin2_body = self.worldbody.find("./body[@name='bin2']")
self.table_top_abs = np.array(bin1_pos)
@@ -34,5 +31,4 @@ def __init__(
def configure_location(self):
"""Configures correct locations for this arena"""
- self.bottom_pos = np.array([0, 0, 0])
self.floor.set("pos", array_to_string(self.bottom_pos))
diff --git a/robosuite/models/arenas/empty_arena.py b/robosuite/models/arenas/empty_arena.py
index ae20e02967..e10da831b2 100644
--- a/robosuite/models/arenas/empty_arena.py
+++ b/robosuite/models/arenas/empty_arena.py
@@ -7,4 +7,3 @@ class EmptyArena(Arena):
def __init__(self):
super().__init__(xml_path_completion("arenas/empty_arena.xml"))
- self.floor = self.worldbody.find("./geom[@name='floor']")
diff --git a/robosuite/models/arenas/table_arena.py b/robosuite/models/arenas/table_arena.py
index eb8482d85a..9d7fe52d2f 100644
--- a/robosuite/models/arenas/table_arena.py
+++ b/robosuite/models/arenas/table_arena.py
@@ -32,8 +32,8 @@ def __init__(
self.table_half_size = self.table_full_size / 2
self.table_friction = table_friction
self.table_offset = table_offset
+ self.center_pos = self.bottom_pos + np.array([0, 0, -self.table_half_size[2]]) + self.table_offset
- self.floor = self.worldbody.find("./geom[@name='floor']")
self.table_body = self.worldbody.find("./body[@name='table']")
self.table_collision = self.table_body.find("./geom[@name='table_collision']")
self.table_visual = self.table_body.find("./geom[@name='table_visual']")
@@ -51,10 +51,8 @@ def __init__(
def configure_location(self):
"""Configures correct locations for this arena"""
- self.bottom_pos = np.array([0, 0, 0])
self.floor.set("pos", array_to_string(self.bottom_pos))
- self.center_pos = self.bottom_pos + np.array([0, 0, -self.table_half_size[2]]) + self.table_offset
self.table_body.set("pos", array_to_string(self.center_pos))
self.table_collision.set("size", array_to_string(self.table_half_size))
self.table_collision.set("friction", array_to_string(self.table_friction))
diff --git a/robosuite/models/arenas/wipe_arena.py b/robosuite/models/arenas/wipe_arena.py
index 7e03862b55..cc563e7c42 100644
--- a/robosuite/models/arenas/wipe_arena.py
+++ b/robosuite/models/arenas/wipe_arena.py
@@ -1,12 +1,12 @@
import numpy as np
from robosuite.models.arenas import TableArena
-from robosuite.utils.mjcf_utils import array_to_string, CustomMaterial
+from robosuite.utils.mjcf_utils import CustomMaterial, find_elements
from robosuite.models.objects import CylinderObject
class WipeArena(TableArena):
"""
- Workspace that contains an empty table with tactile sensors on its surface.
+ Workspace that contains an empty table with visual markers on its surface.
Args:
table_full_size (3-tuple): (L,W,H) full dimensions of the table
@@ -14,7 +14,7 @@ class WipeArena(TableArena):
table_offset (3-tuple): (x,y,z) offset from center of arena when placing table.
Note that the z value sets the upper limit of the table
coverage_factor (float): Fraction of table that will be sampled for dirt placement
- num_sensors (int): Number of dirt (peg) particles to generate in a path on the table
+ num_markers (int): Number of dirt (peg) particles to generate in a path on the table
table_friction_std (float): Standard deviation to sample for the peg friction
line_width (float): Diameter of dirt path trace
two_clusters (bool): If set, will generate two separate dirt paths with half the number of sensors in each
@@ -26,7 +26,7 @@ def __init__(
table_friction=(0.01, 0.005, 0.0001),
table_offset=(0, 0, 0.8),
coverage_factor=0.9,
- num_sensors=10,
+ num_markers=10,
table_friction_std=0,
line_width=0.02,
two_clusters=False
@@ -34,10 +34,9 @@ def __init__(
# Tactile table-specific features
self.table_friction_std = table_friction_std
self.line_width = line_width
- self.sensor_names = []
- self.sensor_site_names = {}
+ self.markers = []
self.coverage_factor = coverage_factor
- self.num_sensors = num_sensors
+ self.num_markers = num_markers
self.two_clusters = two_clusters
# Attribute to hold current direction of sampled dirt path
@@ -55,12 +54,6 @@ def configure_location(self):
# Run superclass first
super().configure_location()
- # Determine peg friction
- friction = max(0.001, np.random.normal(self.table_friction[0], self.table_friction_std))
-
- # Grab reference to the table body in the xml
- table_subtree = self.worldbody.find(".//body[@name='{}']".format("table"))
-
# Define start position for drawing the line
pos = self.sample_start_pos()
@@ -79,63 +72,63 @@ def configure_location(self):
mat_name="dirt_mat",
tex_attrib=tex_attrib,
mat_attrib=mat_attrib,
+ shared=True,
)
# Define line(s) drawn on table
- for i in range(self.num_sensors):
+ for i in range(self.num_markers):
# If we're using two clusters, we resample the starting position and direction at the halfway point
- if self.two_clusters and i == int(np.floor(self.num_sensors / 2)):
+ if self.two_clusters and i == int(np.floor(self.num_markers / 2)):
pos = self.sample_start_pos()
- square_name2 = 'contact_'+str(i)
- square2 = CylinderObject(
- name=square_name2,
+ marker_name = f'contact{i}'
+ marker = CylinderObject(
+ name=marker_name,
size=[self.line_width / 2, 0.001],
rgba=[1, 1, 1, 1],
- density=1,
material=dirt,
- friction=friction,
+ obj_type="visual",
+ joints=None,
)
- self.merge_asset(square2)
- visual_c = square2.get_visual(site=True)
- visual_c.set("pos", array_to_string([pos[0], pos[1], self.table_half_size[2]]))
- visual_c.find("site").set("pos", [0, 0, 0.005])
- visual_c.find("site").set("rgba", array_to_string([0, 0, 0, 0]))
- table_subtree.append(visual_c)
-
- sensor_name = square_name2 + "_sensor"
- sensor_site_name = square_name2
- self.sensor_names += [sensor_name]
- self.sensor_site_names[sensor_name] = sensor_site_name
+ # Manually add this object to the arena xml
+ self.merge_assets(marker)
+ table = find_elements(root=self.worldbody, tags="body", attribs={"name": "table"}, return_first=True)
+ table.append(marker.get_obj())
+
+ # Add this marker to our saved list of all markers
+ self.markers.append(marker)
# Add to the current dirt path
pos = self.sample_path_pos(pos)
def reset_arena(self, sim):
"""
- Reset the tactile sensor locations in the environment. Requires @sim (MjSim) reference to be passed in so that
+ Reset the visual marker locations in the environment. Requires @sim (MjSim) reference to be passed in so that
the Mujoco sim can be directly modified
Args:
- sim (MjSim): Simulation instance containing this arena and tactile sensors
+ sim (MjSim): Simulation instance containing this arena and visual markers
"""
- # Sample new initial position and direction for generated sensor paths
+ # Sample new initial position and direction for generated marker paths
pos = self.sample_start_pos()
- # Loop through all sensor collision body / site pairs
- for i, (_, sensor_name) in enumerate(self.sensor_site_names.items()):
+ # Loop through all visual markers
+ for i, marker in enumerate(self.markers):
# If we're using two clusters, we resample the starting position and direction at the halfway point
- if self.two_clusters and i == int(np.floor(self.num_sensors / 2)):
+ if self.two_clusters and i == int(np.floor(self.num_markers / 2)):
pos = self.sample_start_pos()
- # Get IDs to the body, geom, and site of each sensor
- body_id = sim.model.body_name2id(sensor_name)
- geom_id = sim.model.geom_name2id(sensor_name)
- # Determine new position for this sensor
+ # Get IDs to the body, geom, and site of each marker
+ body_id = sim.model.body_name2id(marker.root_body)
+ geom_id = sim.model.geom_name2id(marker.visual_geoms[0])
+ site_id = sim.model.site_name2id(marker.sites[0])
+ # Determine new position for this marker
position = np.array([pos[0], pos[1], self.table_half_size[2]])
- # Set the current sensor (body) to this new position
+ # Set the current marker (body) to this new position
sim.model.body_pos[body_id] = position
- # Reset the sensor visualization -- setting geom rgba to all 1's
- sim.model.geom_rgba[geom_id] = [1, 1, 1, 1]
- # Sample next values in local sensor trajectory
+ # Reset the marker visualization -- setting geom rgba alpha value to 1
+ sim.model.geom_rgba[geom_id][3] = 1
+ # Hide the default visualization site
+ sim.model.site_rgba[site_id][3] = 0
+ # Sample next values in local marker trajectory
pos = self.sample_path_pos(pos)
def sample_start_pos(self):
diff --git a/robosuite/models/assets/arenas/bins_arena.xml b/robosuite/models/assets/arenas/bins_arena.xml
index d26ed7f1f7..31d0d7c8b0 100644
--- a/robosuite/models/assets/arenas/bins_arena.xml
+++ b/robosuite/models/assets/arenas/bins_arena.xml
@@ -26,15 +26,15 @@
-
+
-
+
-
+
-
+
-
+
@@ -43,19 +43,19 @@
-
+
-
+
-
+
-
+
-
+
-
+
-
+
diff --git a/robosuite/models/assets/arenas/pegs_arena.xml b/robosuite/models/assets/arenas/pegs_arena.xml
index 5abcce09f0..dcde695c6c 100644
--- a/robosuite/models/assets/arenas/pegs_arena.xml
+++ b/robosuite/models/assets/arenas/pegs_arena.xml
@@ -33,7 +33,7 @@
-
+
@@ -43,11 +43,11 @@
-
+
-
+
diff --git a/robosuite/models/assets/arenas/table_arena.xml b/robosuite/models/assets/arenas/table_arena.xml
index 1433d1f2d5..1810ded344 100644
--- a/robosuite/models/assets/arenas/table_arena.xml
+++ b/robosuite/models/assets/arenas/table_arena.xml
@@ -29,7 +29,7 @@
-
+
diff --git a/robosuite/models/assets/base.xml b/robosuite/models/assets/base.xml
index 7f863e6062..3de7de2565 100644
--- a/robosuite/models/assets/base.xml
+++ b/robosuite/models/assets/base.xml
@@ -1,6 +1,6 @@
-
+
diff --git a/robosuite/models/assets/demonstrations/door/panda/demo.hdf5 b/robosuite/models/assets/demonstrations/door/panda/demo.hdf5
deleted file mode 100644
index 9b4fe1d84a..0000000000
Binary files a/robosuite/models/assets/demonstrations/door/panda/demo.hdf5 and /dev/null differ
diff --git a/robosuite/models/assets/demonstrations/door/panda/models/model_1.xml b/robosuite/models/assets/demonstrations/door/panda/models/model_1.xml
deleted file mode 100644
index f133bd7407..0000000000
--- a/robosuite/models/assets/demonstrations/door/panda/models/model_1.xml
+++ /dev/null
@@ -1,260 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
\ No newline at end of file
diff --git a/robosuite/models/assets/demonstrations/lift/panda/demo.hdf5 b/robosuite/models/assets/demonstrations/lift/panda/demo.hdf5
deleted file mode 100644
index 7a6f9c6356..0000000000
Binary files a/robosuite/models/assets/demonstrations/lift/panda/demo.hdf5 and /dev/null differ
diff --git a/robosuite/models/assets/demonstrations/lift/panda/models/model_1.xml b/robosuite/models/assets/demonstrations/lift/panda/models/model_1.xml
deleted file mode 100644
index 769d2f7ac7..0000000000
--- a/robosuite/models/assets/demonstrations/lift/panda/models/model_1.xml
+++ /dev/null
@@ -1,232 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
\ No newline at end of file
diff --git a/robosuite/models/assets/demonstrations/nut-assembly/panda/demo.hdf5 b/robosuite/models/assets/demonstrations/nut-assembly/panda/demo.hdf5
deleted file mode 100644
index 4676c405db..0000000000
Binary files a/robosuite/models/assets/demonstrations/nut-assembly/panda/demo.hdf5 and /dev/null differ
diff --git a/robosuite/models/assets/demonstrations/nut-assembly/panda/models/model_1.xml b/robosuite/models/assets/demonstrations/nut-assembly/panda/models/model_1.xml
deleted file mode 100644
index 12052e3ba0..0000000000
--- a/robosuite/models/assets/demonstrations/nut-assembly/panda/models/model_1.xml
+++ /dev/null
@@ -1,262 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
\ No newline at end of file
diff --git a/robosuite/models/assets/demonstrations/pick-place/panda/demo.hdf5 b/robosuite/models/assets/demonstrations/pick-place/panda/demo.hdf5
deleted file mode 100644
index b3e3a3d033..0000000000
Binary files a/robosuite/models/assets/demonstrations/pick-place/panda/demo.hdf5 and /dev/null differ
diff --git a/robosuite/models/assets/demonstrations/pick-place/panda/models/model_1.xml b/robosuite/models/assets/demonstrations/pick-place/panda/models/model_1.xml
deleted file mode 100644
index 20fd11af2e..0000000000
--- a/robosuite/models/assets/demonstrations/pick-place/panda/models/model_1.xml
+++ /dev/null
@@ -1,296 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
\ No newline at end of file
diff --git a/robosuite/models/assets/demonstrations/stack/panda/demo.hdf5 b/robosuite/models/assets/demonstrations/stack/panda/demo.hdf5
deleted file mode 100644
index 9b79b80a33..0000000000
Binary files a/robosuite/models/assets/demonstrations/stack/panda/demo.hdf5 and /dev/null differ
diff --git a/robosuite/models/assets/demonstrations/stack/panda/models/model_1.xml b/robosuite/models/assets/demonstrations/stack/panda/models/model_1.xml
deleted file mode 100644
index 28c16640fd..0000000000
--- a/robosuite/models/assets/demonstrations/stack/panda/models/model_1.xml
+++ /dev/null
@@ -1,232 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
\ No newline at end of file
diff --git a/robosuite/models/assets/grippers/jaco_three_finger_gripper.xml b/robosuite/models/assets/grippers/jaco_three_finger_gripper.xml
index 88c0a62c07..26b745712e 100644
--- a/robosuite/models/assets/grippers/jaco_three_finger_gripper.xml
+++ b/robosuite/models/assets/grippers/jaco_three_finger_gripper.xml
@@ -4,9 +4,11 @@
-
-
-
+
+
+
+
+
@@ -51,14 +53,14 @@
-
-
-
+
+
+
-
+
@@ -66,45 +68,48 @@
-
-
+
+
-
-
-
+
+
+
+
-
-
+
+
-
-
-
+
+
+
+
-
-
+
+
-
-
-
+
+
+
+
diff --git a/robosuite/models/assets/grippers/null_gripper.xml b/robosuite/models/assets/grippers/null_gripper.xml
index 129d5e4d41..b8b43de858 100644
--- a/robosuite/models/assets/grippers/null_gripper.xml
+++ b/robosuite/models/assets/grippers/null_gripper.xml
@@ -4,7 +4,7 @@
-
+
diff --git a/robosuite/models/assets/grippers/panda_gripper.xml b/robosuite/models/assets/grippers/panda_gripper.xml
index c9cf8becc5..dc668c786f 100644
--- a/robosuite/models/assets/grippers/panda_gripper.xml
+++ b/robosuite/models/assets/grippers/panda_gripper.xml
@@ -5,14 +5,6 @@
-
-
-
-
-
-
-
-
@@ -22,30 +14,30 @@
-
+
-
+
-
-
+
+
-
+
-
-
+
+
-
+
diff --git a/robosuite/models/assets/grippers/rethink_gripper.xml b/robosuite/models/assets/grippers/rethink_gripper.xml
index b6835dde59..61aaed3527 100644
--- a/robosuite/models/assets/grippers/rethink_gripper.xml
+++ b/robosuite/models/assets/grippers/rethink_gripper.xml
@@ -4,18 +4,6 @@
-
-
-
-
-
-
-
-
-
-
-
-
@@ -26,35 +14,37 @@
-
-
-
+
+
+
-
+
-
-
-
+
+
+
-
-
+
+
+
-
-
-
+
+
+
-
-
+
+
+
diff --git a/robosuite/models/assets/grippers/robotiq_gripper_140.xml b/robosuite/models/assets/grippers/robotiq_gripper_140.xml
index 379b597ac6..406004855e 100644
--- a/robosuite/models/assets/grippers/robotiq_gripper_140.xml
+++ b/robosuite/models/assets/grippers/robotiq_gripper_140.xml
@@ -49,59 +49,61 @@
-
-
+
+
-
+
-
-
-
-
+
+
+
+
-
-
-
-
+
+
+
+
+
-
-
+
+
-
-
-
-
+
+
+
+
-
-
-
-
+
+
+
+
+
-
-
+
+
diff --git a/robosuite/models/assets/grippers/robotiq_gripper_85.xml b/robosuite/models/assets/grippers/robotiq_gripper_85.xml
index 665b98d293..8a305bc7b3 100644
--- a/robosuite/models/assets/grippers/robotiq_gripper_85.xml
+++ b/robosuite/models/assets/grippers/robotiq_gripper_85.xml
@@ -39,56 +39,58 @@
-
+
-
-
+
+
-
-
-
-
+
+
+
+
-
-
-
-
+
+
+
+
+
-
-
+
+
-
-
-
-
+
+
+
+
-
-
-
-
+
+
+
+
+
-
-
+
+