From 02cbc0057154b1ba9ebc56240ff86e06a09ab4d8 Mon Sep 17 00:00:00 2001
From: josiah_wong <84cremebrule@gmail.com>
Date: Sat, 7 Nov 2020 10:59:38 -0800
Subject: [PATCH 01/46] add new papers (#118)
---
docs/references.md | 3 +++
1 file changed, 3 insertions(+)
diff --git a/docs/references.md b/docs/references.md
index e5dd9fe06a..9d2e60c453 100644
--- a/docs/references.md
+++ b/docs/references.md
@@ -13,6 +13,7 @@ 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
## Imitation Learning and Batch (Offline) Reinforcement Learning
@@ -21,6 +22,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
+
## Benchmarks
From 81011ac3f93c21c4771c2462a5c67639b503fcd2 Mon Sep 17 00:00:00 2001
From: josiah_wong <84cremebrule@gmail.com>
Date: Sat, 7 Nov 2020 11:13:03 -0800
Subject: [PATCH 02/46] fix env.model.get_xml() --> env.sim.model.get_xml() so
saved xml is up-to-date
---
robosuite/scripts/collect_human_demonstrations.py | 2 +-
robosuite/scripts/tune_camera.py | 2 +-
2 files changed, 2 insertions(+), 2 deletions(-)
diff --git a/robosuite/scripts/collect_human_demonstrations.py b/robosuite/scripts/collect_human_demonstrations.py
index 8e44917735..b954f4b7cf 100644
--- a/robosuite/scripts/collect_human_demonstrations.py
+++ b/robosuite/scripts/collect_human_demonstrations.py
@@ -75,7 +75,7 @@ def collect_human_trajectory(env, device, arm, env_configuration):
# applied because the data collector wrapper only starts recording
# after the first action has been played.
initial_mjstate = env.sim.get_state().flatten()
- xml_str = env.model.get_xml()
+ xml_str = env.sim.model.get_xml()
env.reset_from_xml_string(xml_str)
env.sim.reset()
env.sim.set_state_from_flattened(initial_mjstate)
diff --git a/robosuite/scripts/tune_camera.py b/robosuite/scripts/tune_camera.py
index 41e6e556ce..4acd1c6caa 100644
--- a/robosuite/scripts/tune_camera.py
+++ b/robosuite/scripts/tune_camera.py
@@ -249,7 +249,7 @@ def print_command(char, info):
)
env.reset()
initial_mjstate = env.sim.get_state().flatten()
- xml = env.model.get_xml()
+ xml = env.sim.model.get_xml()
# add mocap body to camera to be able to move it around
xml = modify_xml_for_camera_movement(xml, camera_name=CAMERA_NAME)
From 1045ee4d134b8fbdab72a90a4f1902fc252e895f Mon Sep 17 00:00:00 2001
From: josiah_wong <84cremebrule@gmail.com>
Date: Sat, 7 Nov 2020 11:22:07 -0800
Subject: [PATCH 03/46] remove redundant proprioceptive obs from object obs in
handover and two arm lift tasks
---
robosuite/environments/two_arm_handover.py | 4 ----
robosuite/environments/two_arm_lift.py | 4 ----
2 files changed, 8 deletions(-)
diff --git a/robosuite/environments/two_arm_handover.py b/robosuite/environments/two_arm_handover.py
index 1bae2d8d78..c776aeb298 100644
--- a/robosuite/environments/two_arm_handover.py
+++ b/robosuite/environments/two_arm_handover.py
@@ -526,8 +526,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 +534,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"],
]
diff --git a/robosuite/environments/two_arm_lift.py b/robosuite/environments/two_arm_lift.py
index 8e73e79d89..1ae9002cde 100644
--- a/robosuite/environments/two_arm_lift.py
+++ b/robosuite/environments/two_arm_lift.py
@@ -449,8 +449,6 @@ 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)
@@ -460,8 +458,6 @@ def _get_observation(self):
[
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"],
From 4a7e250a15c3728e8b2a406772b437ce281ba4d6 Mon Sep 17 00:00:00 2001
From: josiah_wong <84cremebrule@gmail.com>
Date: Sun, 8 Nov 2020 11:52:22 -0800
Subject: [PATCH 04/46] update find_contacts to take in str or list of str
---
robosuite/environments/base.py | 9 +++++++--
1 file changed, 7 insertions(+), 2 deletions(-)
diff --git a/robosuite/environments/base.py b/robosuite/environments/base.py
index 71628e1ba9..1378c2f0d4 100644
--- a/robosuite/environments/base.py
+++ b/robosuite/environments/base.py
@@ -398,12 +398,17 @@ def find_contacts(self, geoms_1, geoms_2):
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): an individual geom name or list of geom names
+ geoms_2 (str or list of str): another individual geom name or list of geom names
Returns:
generator: iterator of all contacts between @geoms_1 and @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]
+ if type(geoms_2) is str:
+ geoms_2 = [geoms_2]
for contact in self.sim.data.contact[0 : self.sim.data.ncon]:
# check contact geom in geoms
c1_in_g1 = self.sim.model.geom_id2name(contact.geom1) in geoms_1
From 40cc21500dc01c34a72ec206484c9542a9140632 Mon Sep 17 00:00:00 2001
From: josiah_wong <84cremebrule@gmail.com>
Date: Sun, 8 Nov 2020 12:05:36 -0800
Subject: [PATCH 05/46] fix _check_gripper_contact (#119), add more accurate
checks for bimanual case
---
robosuite/environments/robot_env.py | 16 ++++++++++------
1 file changed, 10 insertions(+), 6 deletions(-)
diff --git a/robosuite/environments/robot_env.py b/robosuite/environments/robot_env.py
index 6e79fa4630..b5f7274ec5 100644
--- a/robosuite/environments/robot_env.py
+++ b/robosuite/environments/robot_env.py
@@ -418,16 +418,18 @@ def _get_observation(self):
def _check_gripper_contact(self):
"""
- Checks whether each gripper is in contact with an object.
+ Checks whether each gripper is in contact with an object. Note that each entry in the return list corresponds
+ to each robot in the env.robots list. This means that a bimanual robot will have its own nested list within the
+ return list at the corresponding entry.
Returns:
- list of bool: True if the specific gripper is in contact with an object
+ 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 robot.robot_model.arm_type == "single":
if (
self.sim.model.geom_id2name(contact.geom1)
in robot.gripper.contact_geoms
@@ -438,15 +440,17 @@ def _check_gripper_contact(self):
break
# Bimanual case
else:
- for arm in robot.arms:
+ # Set this list entry to be its own list of bools
+ collisions[idx] = [False, False]
+ for i, arm in enumerate(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
+ collisions[idx][i] = True
+ continue
return collisions
def _check_arm_contact(self):
From 9c5efeb66d0b874c207b55db79b6b823d9c4d25a Mon Sep 17 00:00:00 2001
From: josiah_wong <84cremebrule@gmail.com>
Date: Wed, 18 Nov 2020 19:58:59 -0800
Subject: [PATCH 06/46] fix controller limits bug
---
robosuite/controllers/joint_pos.py | 2 +-
robosuite/controllers/joint_tor.py | 2 +-
robosuite/controllers/joint_vel.py | 2 +-
robosuite/controllers/osc.py | 4 ++--
4 files changed, 5 insertions(+), 5 deletions(-)
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 7b5c5c69fd..b7831db4f1 100644
--- a/robosuite/controllers/osc.py
+++ b/robosuite/controllers/osc.py
@@ -173,8 +173,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
From 5b63732bd82130528b669baa43c3fc25e91ba048 Mon Sep 17 00:00:00 2001
From: josiah_wong <84cremebrule@gmail.com>
Date: Wed, 18 Nov 2020 22:18:19 -0800
Subject: [PATCH 07/46] standardize groups for collision / visual geoms,
standardize group colors for collision geoms, refactor generated / xml object
parsing, fix naming conventions in two_arm_lift
---
robosuite/environments/door.py | 3 +-
robosuite/environments/lift.py | 3 +-
robosuite/environments/nut_assembly.py | 5 +-
robosuite/environments/pick_place.py | 19 +-
robosuite/environments/stack.py | 6 +-
robosuite/environments/two_arm_handover.py | 3 +-
robosuite/environments/two_arm_lift.py | 72 +-
robosuite/environments/two_arm_peg_in_hole.py | 4 +-
robosuite/environments/wipe.py | 12 +-
robosuite/models/arenas/arena.py | 8 +-
robosuite/models/arenas/bins_arena.py | 2 -
robosuite/models/arenas/wipe_arena.py | 5 +-
robosuite/models/assets/arenas/bins_arena.xml | 24 +-
robosuite/models/assets/arenas/pegs_arena.xml | 6 +-
.../models/assets/arenas/table_arena.xml | 2 +-
.../grippers/jaco_three_finger_gripper.xml | 20 +-
.../models/assets/grippers/panda_gripper.xml | 8 +-
.../assets/grippers/rethink_gripper.xml | 16 +-
.../assets/grippers/robotiq_gripper_140.xml | 22 +-
.../assets/grippers/robotiq_gripper_85.xml | 22 +-
.../assets/grippers/robotiq_gripper_s.xml | 8 +-
.../models/assets/grippers/wiping_gripper.xml | 70 +-
robosuite/models/assets/objects/bottle.xml | 4 +-
.../models/assets/objects/bread-visual.xml | 11 +-
robosuite/models/assets/objects/bread.xml | 8 +-
.../models/assets/objects/can-visual.xml | 9 +-
robosuite/models/assets/objects/can.xml | 8 +-
.../models/assets/objects/cereal-visual.xml | 15 +-
robosuite/models/assets/objects/cereal.xml | 16 +-
robosuite/models/assets/objects/door.xml | 2 +-
robosuite/models/assets/objects/door_lock.xml | 16 +-
robosuite/models/assets/objects/lemon.xml | 8 +-
.../models/assets/objects/milk-visual.xml | 11 +-
robosuite/models/assets/objects/milk.xml | 8 +-
.../models/assets/objects/plate-with-hole.xml | 16 +-
robosuite/models/assets/objects/round-nut.xml | 20 +-
.../models/assets/objects/square-nut.xml | 12 +-
robosuite/models/base.py | 25 +
robosuite/models/grippers/gripper_model.py | 4 +
robosuite/models/objects/generated_objects.py | 945 ++++++++++--------
robosuite/models/objects/objects.py | 297 ++++--
robosuite/models/objects/xml_objects.py | 50 +-
robosuite/models/robots/robot_model.py | 5 +-
robosuite/models/tasks/manipulation_task.py | 38 +-
robosuite/utils/mjcf_utils.py | 35 +-
45 files changed, 1077 insertions(+), 826 deletions(-)
diff --git a/robosuite/environments/door.py b/robosuite/environments/door.py
index 44588be062..585570f57f 100644
--- a/robosuite/environments/door.py
+++ b/robosuite/environments/door.py
@@ -283,8 +283,7 @@ def _load_model(self):
self.model = ManipulationTask(
mujoco_arena=self.mujoco_arena,
mujoco_robots=[robot.robot_model for robot in self.robots],
- mujoco_objects=self.mujoco_objects,
- visual_objects=None,
+ mujoco_objects=self.mujoco_objects,
initializer=self.placement_initializer,
)
self.model.place_objects()
diff --git a/robosuite/environments/lift.py b/robosuite/environments/lift.py
index bb5d1b52e0..62acec0516 100644
--- a/robosuite/environments/lift.py
+++ b/robosuite/environments/lift.py
@@ -318,8 +318,7 @@ def _load_model(self):
self.model = ManipulationTask(
mujoco_arena=self.mujoco_arena,
mujoco_robots=[robot.robot_model for robot in self.robots],
- mujoco_objects=self.mujoco_objects,
- visual_objects=None,
+ mujoco_objects=self.mujoco_objects,
initializer=self.placement_initializer,
)
self.model.place_objects()
diff --git a/robosuite/environments/nut_assembly.py b/robosuite/environments/nut_assembly.py
index 5628f6c260..0c1c4aa3de 100644
--- a/robosuite/environments/nut_assembly.py
+++ b/robosuite/environments/nut_assembly.py
@@ -466,8 +466,7 @@ def _load_model(self):
self.model = ManipulationTask(
mujoco_arena=self.mujoco_arena,
mujoco_robots=[robot.robot_model for robot in self.robots],
- mujoco_objects=self.mujoco_objects,
- visual_objects=None,
+ mujoco_objects=self.mujoco_objects,
initializer=self.placement_initializer,
)
@@ -495,7 +494,7 @@ def _get_reference(self):
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)))
+ geom_ids.append(self.sim.model.geom_name2id(obj_str + "_{}".format(j)))
self.obj_geom_id[obj_str] = geom_ids
# information of objects
diff --git a/robosuite/environments/pick_place.py b/robosuite/environments/pick_place.py
index 35d960c672..163b497cf8 100644
--- a/robosuite/environments/pick_place.py
+++ b/robosuite/environments/pick_place.py
@@ -430,7 +430,7 @@ def _get_placement_initializer(self):
bin_y_half = self.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:
+ for obj_name in self.collision_objects:
self.placement_initializer.sample_on_top(
obj_name,
@@ -523,7 +523,7 @@ def _load_model(self):
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
+ joints=[], # no free joint for visual objects
)
lst.append((visual_ob_name, visual_ob))
self.visual_objects = OrderedDict(lst)
@@ -537,7 +537,8 @@ def _load_model(self):
)
lst.append((ob_name, ob))
- self.mujoco_objects = OrderedDict(lst)
+ self.collision_objects = OrderedDict(lst)
+ self.mujoco_objects = OrderedDict(**self.visual_objects, **self.collision_objects)
self.n_objects = len(self.mujoco_objects)
# task includes arena, robot, and objects of interest
@@ -545,8 +546,7 @@ def _load_model(self):
self.model = ManipulationTask(
mujoco_arena=self.mujoco_arena,
mujoco_robots=[robot.robot_model for robot in self.robots],
- mujoco_objects=self.mujoco_objects,
- visual_objects=self.visual_objects,
+ mujoco_objects=self.mujoco_objects,
initializer=self.placement_initializer,
)
@@ -579,7 +579,7 @@ def _get_reference(self):
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)
+ self.obj_geom_id[obj_str] = self.sim.model.geom_name2id(obj_str + "_0")
# 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()))
@@ -616,10 +616,11 @@ def _reset_internal(self):
# 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])]))
+ if "visual" not in obj_name.lower():
+ 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())
+ # information of (collision) objects
+ self.object_names = list(self.collision_objects.keys())
self.object_site_ids = [
self.sim.model.site_name2id(ob_name) for ob_name in self.object_names
]
diff --git a/robosuite/environments/stack.py b/robosuite/environments/stack.py
index 8950bf925c..f40e2f84ee 100644
--- a/robosuite/environments/stack.py
+++ b/robosuite/environments/stack.py
@@ -378,9 +378,9 @@ def _load_model(self):
# 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,
+ mujoco_arena=self.mujoco_arena,
+ mujoco_robots=[robot.robot_model for robot in self.robots],
+ mujoco_objects=self.mujoco_objects,
initializer=self.placement_initializer,
)
self.model.place_objects()
diff --git a/robosuite/environments/two_arm_handover.py b/robosuite/environments/two_arm_handover.py
index c776aeb298..6d57f6a10c 100644
--- a/robosuite/environments/two_arm_handover.py
+++ b/robosuite/environments/two_arm_handover.py
@@ -349,8 +349,7 @@ def _load_model(self):
self.model = ManipulationTask(
mujoco_arena=self.mujoco_arena,
mujoco_robots=[robot.robot_model for robot in self.robots],
- mujoco_objects=self.mujoco_objects,
- visual_objects=None,
+ mujoco_objects=self.mujoco_objects,
initializer=self.placement_initializer,
)
self.model.place_objects()
diff --git a/robosuite/environments/two_arm_lift.py b/robosuite/environments/two_arm_lift.py
index 1ae9002cde..a152c7fdc9 100644
--- a/robosuite/environments/two_arm_lift.py
+++ b/robosuite/environments/two_arm_lift.py
@@ -258,8 +258,8 @@ def reward(self, action=None):
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
@@ -268,48 +268,48 @@ def reward(self, action=None):
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()
+ self.robots[0].gripper["left"].important_geoms["left_finger"], self.pot.handle1_geoms
)
)) > 0
_contacts_0_rf = len(list(
self.find_contacts(
- self.robots[0].gripper["left"].important_geoms["right_finger"], self.pot.handle_2_geoms()
+ self.robots[0].gripper["left"].important_geoms["right_finger"], self.pot.handle1_geoms
)
)) > 0
_contacts_1_lf = len(list(
self.find_contacts(
- self.robots[0].gripper["right"].important_geoms["left_finger"], self.pot.handle_1_geoms()
+ self.robots[0].gripper["right"].important_geoms["left_finger"], self.pot.handle0_geoms
)
)) > 0
_contacts_1_rf = len(list(
self.find_contacts(
- self.robots[0].gripper["right"].important_geoms["right_finger"], self.pot.handle_1_geoms()
+ self.robots[0].gripper["right"].important_geoms["right_finger"], self.pot.handle0_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()
+ self.robots[0].gripper.important_geoms["left_finger"], self.pot.handle1_geoms
)
)) > 0
_contacts_0_rf = len(list(
self.find_contacts(
- self.robots[0].gripper.important_geoms["right_finger"], self.pot.handle_2_geoms()
+ self.robots[0].gripper.important_geoms["right_finger"], self.pot.handle1_geoms
)
)) > 0
_contacts_1_lf = len(list(
self.find_contacts(
- self.robots[1].gripper.important_geoms["left_finger"], self.pot.handle_1_geoms()
+ self.robots[1].gripper.important_geoms["left_finger"], self.pot.handle0_geoms
)
)) > 0
_contacts_1_rf = len(list(
self.find_contacts(
- self.robots[1].gripper.important_geoms["right_finger"], self.pot.handle_1_geoms()
+ self.robots[1].gripper.important_geoms["right_finger"], self.pot.handle0_geoms
)
)) > 0
- _g0h_dist = np.linalg.norm(_gripper_0_to_handle)
- _g1h_dist = np.linalg.norm(_gripper_1_to_handle)
+ _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:
@@ -367,15 +367,15 @@ def _load_model(self):
self.mujoco_arena.set_origin([0, 0, 0])
# initialize objects of interest
- self.pot = PotWithHandlesObject(name="pot")
- self.mujoco_objects = OrderedDict([("pot", self.pot)])
+ self.pot_name = "pot"
+ self.pot = PotWithHandlesObject(name=self.pot_name)
+ self.mujoco_objects = OrderedDict([(self.pot_name, self.pot)])
# task includes arena, robot, and objects of interest
self.model = ManipulationTask(
mujoco_arena=self.mujoco_arena,
mujoco_robots=[robot.robot_model for robot in self.robots],
- mujoco_objects=self.mujoco_objects,
- visual_objects=None,
+ mujoco_objects=self.mujoco_objects,
initializer=self.placement_initializer,
)
self.model.place_objects()
@@ -389,11 +389,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_name)
+ self.handle1_site_id = self.sim.model.site_name2id(f"{self.pot_name}_handle0")
+ self.handle0_site_id = self.sim.model.site_name2id(f"{self.pot_name}_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(f"{self.pot_name}_center")
def _reset_internal(self):
"""
@@ -449,19 +449,19 @@ def _get_observation(self):
di["cube_pos"] = cube_pos
di["cube_quat"] = cube_quat
- 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["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"],
]
)
@@ -513,24 +513,24 @@ def _check_robot_configuration(self, robots):
"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):
@@ -579,21 +579,21 @@ def _eef1_xpos(self):
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/two_arm_peg_in_hole.py
index 9cf25454f2..ebd344452c 100644
--- a/robosuite/environments/two_arm_peg_in_hole.py
+++ b/robosuite/environments/two_arm_peg_in_hole.py
@@ -314,13 +314,13 @@ def _load_model(self):
)
# Load hole object
- self.hole_obj = self.hole.get_collision(site=True)
+ self.hole_obj = self.hole.get_object_subtree(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)
# Load peg object
- self.peg_obj = self.peg.get_collision(site=True)
+ self.peg_obj = self.peg.get_object_subtree(site=True)
self.peg_obj.set("pos", array_to_string((0, 0, self.peg_length)))
self.model.merge_asset(self.peg)
diff --git a/robosuite/environments/wipe.py b/robosuite/environments/wipe.py
index 8e4e0c7715..1b8e631b1e 100644
--- a/robosuite/environments/wipe.py
+++ b/robosuite/environments/wipe.py
@@ -422,7 +422,7 @@ def PointInRectangle(X, Y, Z, W, P):
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)
+ new_sensor_active_geom_id = self.sim.model.geom_name2id(sensor_name + "_vis")
# 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
@@ -526,10 +526,12 @@ def _load_model(self):
self.mujoco_objects = OrderedDict()
# 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 = ManipulationTask(
+ mujoco_arena=self.mujoco_arena,
+ mujoco_robots=[robot.robot_model for robot in self.robots],
+ mujoco_objects=self.mujoco_objects,
+ initializer=self.placement_initializer
+ )
self.model.place_objects()
def _reset_internal(self):
diff --git a/robosuite/models/arenas/arena.py b/robosuite/models/arenas/arena.py
index 8bb94d367a..5a68643ddb 100644
--- a/robosuite/models/arenas/arena.py
+++ b/robosuite/models/arenas/arena.py
@@ -1,13 +1,17 @@
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
class Arena(MujocoXML):
"""Base arena class."""
+ def __init__(self, fname):
+ super().__init__(fname)
+ self.recolor_collision_geoms(ENVIRONMENT_COLLISION_COLOR)
+
def set_origin(self, offset):
"""
Applies a constant offset to all objects.
diff --git a/robosuite/models/arenas/bins_arena.py b/robosuite/models/arenas/bins_arena.py
index 3b3f1f5ad1..cfa57af188 100644
--- a/robosuite/models/arenas/bins_arena.py
+++ b/robosuite/models/arenas/bins_arena.py
@@ -17,8 +17,6 @@ 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)
diff --git a/robosuite/models/arenas/wipe_arena.py b/robosuite/models/arenas/wipe_arena.py
index 7e03862b55..92b65f227b 100644
--- a/robosuite/models/arenas/wipe_arena.py
+++ b/robosuite/models/arenas/wipe_arena.py
@@ -94,9 +94,10 @@ def configure_location(self):
density=1,
material=dirt,
friction=friction,
+ obj_type="visual",
)
self.merge_asset(square2)
- visual_c = square2.get_visual(site=True)
+ visual_c = square2.get_object_subtree(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]))
@@ -128,7 +129,7 @@ def reset_arena(self, sim):
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)
+ geom_id = sim.model.geom_name2id(sensor_name + "_vis")
# Determine new position for this sensor
position = np.array([pos[0], pos[1], self.table_half_size[2]])
# Set the current sensor (body) to this new position
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 @@