Merge pull request #12 from StanfordVL/contact

Contact -> AprilUpdates

MujocoManip/environment/base.py

@@ -168,6 +168,12 @@ def reset_from_xml_string(self, xml_string):
 
         # return self._get_observation()
 
+    def _check_contact(self):
+        """
+        Returns True if gripper is in contact with an object.
+        """
+        return False
+
     def close(self):
         """
         Do any cleanup necessary here.

MujocoManip/environment/sawyer.py

@@ -100,36 +100,36 @@ def _pre_action(self, action):
             # gravity compensation
             self.sim.data.qfrc_applied[self._ref_joint_vel_indexes] = self.sim.data.qfrc_bias[self._ref_joint_vel_indexes]
 
-        else:
-            if self.use_force_ctrl:
-
-                ### TODO: convert the following in mujoco_py??? ###
+        elif self.use_force_ctrl:
 
-                ### TODO: is this force acting in the end effector frame? If so, we need to translate to base coords... ###
+            ### TODO: convert the following in mujoco_py??? ###
 
-                # note we convert force in base frame to force in world frame
-                # self.physics.named.data.xfrc_applied['right_hand'] = self._convert_base_force_to_world_force(action[:6])
-                self.physics.named.data.xfrc_applied['right_hand'] = action[:6]
+            ### TODO: is this force acting in the end effector frame? If so, we need to translate to base coords... ###
 
-                # gravity compensation
-                self.sim.data.qfrc_applied[self._ref_joint_vel_indexes] = self.sim.data.qfrc_bias[self._ref_joint_vel_indexes]
-            else:
-                action = np.clip(action, -1, 1)    
-                if self.has_gripper:
-                    arm_action = action[:self.mujoco_robot.dof()]
-                    gripper_action_in = action[self.mujoco_robot.dof():self.mujoco_robot.dof()+self.gripper.dof()]
-                    gripper_action_actual = self.gripper.format_action(gripper_action_in)
-                    action = np.concatenate([arm_action, gripper_action_actual])
+            # note we convert force in base frame to force in world frame
+            # self.physics.named.data.xfrc_applied['right_hand'] = self._convert_base_force_to_world_force(action[:6])
+            self.physics.named.data.xfrc_applied['right_hand'] = action[:6]
 
-                # rescale normalized action to control ranges
-                ctrl_range = self.sim.model.actuator_ctrlrange
-                bias = 0.5 * (ctrl_range[:,1] + ctrl_range[:,0])
-                weight = 0.5 * (ctrl_range[:,1] - ctrl_range[:,0])
-                applied_action = bias + weight * action
-                self.sim.data.ctrl[:] = applied_action
+            # gravity compensation
+            self.sim.data.qfrc_applied[self._ref_joint_vel_indexes] = self.sim.data.qfrc_bias[self._ref_joint_vel_indexes]
+
+        else:
+            action = np.clip(action, -1, 1)    
+            if self.has_gripper:
+                arm_action = action[:self.mujoco_robot.dof()]
+                gripper_action_in = action[self.mujoco_robot.dof():self.mujoco_robot.dof()+self.gripper.dof()]
+                gripper_action_actual = self.gripper.format_action(gripper_action_in)
+                action = np.concatenate([arm_action, gripper_action_actual])
+
+            # rescale normalized action to control ranges
+            ctrl_range = self.sim.model.actuator_ctrlrange
+            bias = 0.5 * (ctrl_range[:,1] + ctrl_range[:,0])
+            weight = 0.5 * (ctrl_range[:,1] - ctrl_range[:,0])
+            applied_action = bias + weight * action
+            self.sim.data.ctrl[:] = applied_action
 
-                # gravity compensation
-                self.sim.data.qfrc_applied[self._ref_joint_vel_indexes] = self.sim.data.qfrc_bias[self._ref_joint_vel_indexes]
+            # gravity compensation
+            self.sim.data.qfrc_applied[self._ref_joint_vel_indexes] = self.sim.data.qfrc_bias[self._ref_joint_vel_indexes]
 
     def _post_action(self, action):
         ret = super()._post_action(action)
@@ -308,4 +308,10 @@ def _gripper_visualization(self):
         # By default, don't do any coloring.
         self.sim.model.site_rgba[self.eef_site_id] = [0., 0., 0., 0.]
 
+    def _check_contact(self):
+        """
+        Returns True if the gripper is in contact with another object.
+        """
+        return False
+
 

MujocoManip/environment/sawyer_stack.py

@@ -63,6 +63,15 @@ def __init__(self,
         self.object_names = [di['object_name'] for di in self.object_metadata]
         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.finger_names = self.gripper.contact_geoms()
+
+        # self.sim.data.contact # list, geom1, geom2
+        # self.sim.model._geom_name2id # keys for named shit
+
+        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]
+
     def _load_model(self):
         super()._load_model()
         self.mujoco_robot.place_on([0,0,0])
@@ -121,10 +130,24 @@ def _get_observation(self):
             all_observations += [self._object_pos(i),
                                 self._object_vel(i),
                                 self._target_pos(i)]
-
+            
         di['low-level'] = np.concatenate(all_observations)
         return di
 
+    def _check_contact(self):
+        """
+        Returns True if gripper is in contact with an object.
+        """
+        collision = False
+        for contact in self.sim.data.contact:
+            # print("geom1: {}".format(self.sim.model.geom_id2name(contact.geom1)))
+            # print("geom2: {}".format(self.sim.model.geom_id2name(contact.geom2)))
+            if self.sim.model.geom_id2name(contact.geom1) in self.finger_names or \
+               self.sim.model.geom_id2name(contact.geom2) in self.finger_names:
+                collision = True
+                break
+        return collision
+
     def _check_lose(self):
         object_z = np.concatenate([self._object_pos(i)[2:3] for i in range(self.n_objects)])
         # Object falls off the table

MujocoManip/miscellaneous/mujoco_py_renderer.py

@@ -1,4 +1,5 @@
 from mujoco_py import MjViewer
+from mujoco_py.generated import const
 import glfw
 
 class MujocoPyRenderer():
@@ -8,6 +9,13 @@ def __init__(self, sim):
         """
         self.viewer = MjViewer(sim)
 
+    def set_camera(self, camera_id):
+        """
+        Set the camera view to the specified camera ID.
+        """
+        self.viewer.cam.fixedcamid = camera_id
+        self.viewer.cam.type = const.CAMERA_FIXED
+
     def render(self, *args, **kwargs):
         # safe for multiple calls
         self.viewer.render()

MujocoManip/model/arena.py

@@ -24,7 +24,7 @@ def __init__(self, full_size=(0.8,0.8,0.8), friction=(1, 0.005, 0.0001)):
 		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']")
+		# self.table_visual = self.table_body.find("./geom[@name='table_visual']")
 		self.table_top = self.table_body.find("./site[@name='table_top']")
 
 		self.configure_location()
@@ -37,7 +37,7 @@ def configure_location(self):
 		self.table_body.set('pos', array_to_string(self.center_pos))
 		self.table_collision.set('size', array_to_string(self.half_size))
 		self.table_collision.set('friction', array_to_string(self.friction))
-		self.table_visual.set('size', array_to_string(self.half_size * visual_size_shrink_ratio))
+		# self.table_visual.set('size', array_to_string(self.half_size * visual_size_shrink_ratio))
 
 		self.table_top.set('pos', array_to_string(np.array([0,0,self.half_size[2]])))
 

MujocoManip/model/assets/arena/table_arena.xml

@@ -8,7 +8,7 @@
 		<geom condim="3" material="floorplane" name="floor" pos="0 0 0" size="3 3 .125" type="plane"/>
 		<body name="table" pos="0 0 0.4">
 			<geom pos="0 0 0" size="0.4 0.4 0.4" type="box" name="table_collision" friction="1 0.005 0.0001"/>
-			<geom pos="0 0 0" size="0.399 0.399 0.399" type="box" conaffinity="0" contype="0" group="1" name="table_visual"/>
+			<!-- <geom pos="0 0 0" size="0.399 0.399 0.399" type="box" conaffinity="0" contype="0" group="1" name="table_visual"/> -->
 			<site pos="0 0 0.4" name="table_top" size="0.001 0.001 0.001" rgba="0 0 0 0"/>
 		</body>
 		<light diffuse=".8 .8 .8" dir="0 -.15 -1" directional="false" pos="1 1 4.0" specular="0.3 0.3 0.3"/>

MujocoManip/model/assets/base.xml

@@ -1,6 +1,7 @@
 <!-- This is the base xml for all physics simulations. Set global configs here. -->
 <mujoco model="base">
 	<compiler angle="radian" meshdir="meshes/"/>
+	<option impratio="20" cone="elliptic"/>
 	<size nconmax="1000" njmax="1000"/>
 
 	<asset>

MujocoManip/model/assets/gripper/two_finger_gripper.xml

@@ -44,25 +44,25 @@
                 <!-- original -->
                 <joint name="r_gripper_l_finger_joint" pos="0 0 0" axis="0 1 0" type="slide" limited="true" range="-0.0115 0.020833" damping="100"/>
 
-                <geom quat="0 0 0 -1" type="mesh" contype="0" conaffinity="0" group="1" mesh="standard_narrow" />
+                <geom name="r_gripper_l_finger" quat="0 0 0 -1" type="mesh" contype="0" conaffinity="0" group="1" mesh="standard_narrow" />
 
                 <!-- original -->
                 <!-- <geom size="0.005 0.00675 0.0375" pos="0 0.01725 0.04" quat="0 0 0 -1" type="box" />
                 <geom size="0.005 0.025 0.0085" pos="-0.005 -0.003 0.0083" quat="0 0 0 -1" type="box" /> -->
 
                 <!-- conaffinity, contype changes -->
-                <geom size="0.005 0.00675 0.0375" pos="0 0.01725 0.04" quat="0 0 0 -1" type="box"  conaffinity="1" contype="0"/>
-                <geom size="0.005 0.025 0.0085" pos="-0.005 -0.003 0.0083" quat="0 0 0 -1" type="box"  conaffinity="1" contype="0"/>
+                <geom size="0.005 0.00675 0.0375" pos="0 0.01725 0.04" quat="0 0 0 -1" type="box"  conaffinity="1" contype="0" name="l_finger_g0"/>
+                <geom size="0.005 0.025 0.0085" pos="-0.005 -0.003 0.0083" quat="0 0 0 -1" type="box"  conaffinity="1" contype="0" name="l_finger_g1"/>
 
                 <body name="r_gripper_l_finger_tip" pos="0 0.01725 0.075">
                     <inertial pos="0 0 0" quat="0 0 0 1" mass="0.01" diaginertia="0.01 0.01 0.01" />
                     <geom quat="0 0 0 1" type="mesh" contype="0" conaffinity="0" group="1" mesh="half_round_tip" />
 
                     <!-- original -->
-                    <geom size="0.004 0.004 0.0185" pos="0 -0.0045 -0.015" quat="0 0 0 1" type="box" />
+                    <!-- <geom size="0.004 0.004 0.0185" pos="0 -0.0045 -0.015" quat="0 0 0 1" type="box" /> -->
 
                     <!-- TODO: mess with conaffinity, contype here?? -->
-                    <!-- <geom size="0.004 0.004 0.0185" pos="0 -0.0045 -0.015" quat="0 0 0 1" type="box"  conaffinity="1" contype="0"/> -->
+                    <geom size="0.004 0.004 0.0185" pos="0 -0.0045 -0.015" quat="0 0 0 1" type="box"  conaffinity="1" contype="0" name="l_fingertip_g0" friction="1 0.05 0.01"/>
 
                     <!-- trying condim 4, to mirror openai -->
                     <!-- <geom size="0.004 0.004 0.0185" pos="0 -0.0045 -0.015" quat="0 0 0 1" type="box"  conaffinity="1" contype="0" condim="4" friction="1000 0.05 0.01"/> -->
@@ -85,26 +85,26 @@
                 <!-- original -->
                 <joint name="r_gripper_r_finger_joint" pos="0 0 0" axis="0 1 0" type="slide" limited="true" range="-0.020833 0.0115" damping="100"/>
 
-                <geom type="mesh" contype="0" conaffinity="0" group="1" mesh="standard_narrow" />
+                <geom name="r_gripper_r_finger" type="mesh" contype="0" conaffinity="0" group="1" mesh="standard_narrow" />
 
                 <!-- original -->
                 <!-- <geom size="0.005 0.00675 0.0375" pos="0 -0.01725 0.04" type="box" />
                 <geom size="0.005 0.025 0.0085" pos="0.005 0.003 0.0083" type="box" /> -->
 
                 <!-- conaffinity, contype changes -->
-                <geom size="0.005 0.00675 0.0375" pos="0 -0.01725 0.04" type="box"  conaffinity="1" contype="0"/>
-                <geom size="0.005 0.025 0.0085" pos="0.005 0.003 0.0083" type="box"  conaffinity="1" contype="0"/>
+                <geom size="0.005 0.00675 0.0375" pos="0 -0.01725 0.04" type="box" conaffinity="1" contype="0" name="r_finger_g0"/>
+                <geom size="0.005 0.025 0.0085" pos="0.005 0.003 0.0083" type="box" conaffinity="1" contype="0" name="r_finger_g1"/>
 
 
                 <body name="r_gripper_r_finger_tip" pos="0 -0.01725 0.075">
                     <inertial pos="0 0 0" mass="0.01" diaginertia="0.01 0.01 0.01" />
                     <geom type="mesh" contype="0" conaffinity="0" group="1" mesh="half_round_tip" />
 
                     <!-- original -->
-                    <geom size="0.004 0.004 0.0185" pos="0 0.0045 -0.015" type="box" />
+                    <!-- <geom size="0.004 0.004 0.0185" pos="0 0.0045 -0.015" type="box" /> -->
 
                     <!-- TODO: mess with conaffinity, contype here?? -->
-                    <!-- <geom size="0.004 0.004 0.0185" pos="0 0.0045 -0.015" type="box"  conaffinity="1" contype="0"/> -->
+                    <geom size="0.004 0.004 0.0185" pos="0 0.0045 -0.015" type="box"  conaffinity="1" contype="0" name="r_fingertip_g0" friction="1 0.05 0.01"/>
 
                     <!-- trying condim 4, to mirror openai -->
                     <!-- <geom size="0.004 0.004 0.0185" pos="0 0.0045 -0.015" type="box"  conaffinity="1" contype="0" condim="4" friction="1000 0.05 0.01"/> -->

MujocoManip/model/gripper.py

@@ -33,6 +33,13 @@ def dof(self):
         """
         raise NotImplementedError
 
+    def contact_geoms(self):
+        """
+            Returns a list of names corresponding to the geoms
+            used to determine contact with the gripper.
+        """
+        raise NotImplementedError
+
 
 class TwoFingerGripper(MujocoGripper):
     def __init__(self):
@@ -53,6 +60,9 @@ def joints(self):
     def dof(self):
         return 2
 
+    def contact_geoms(self):
+        return ["r_finger_g0", "r_finger_g1", "l_finger_g0", "l_finger_g1", "r_fingertip_g0", "l_fingertip_g0"]
+
 class PR2Gripper(MujocoGripper):
     def __init__(self):
         super().__init__(xml_path_completion('gripper/pr2_gripper.xml'))
@@ -72,6 +82,9 @@ def joints(self):
     def dof(self):
         return 4
 
+    def contact_geoms(self):
+        raise NotImplementedError
+
 class RobotiqGripper(MujocoGripper):
     def __init__(self):
         super().__init__(xml_path_completion('gripper/robotiq_gripper.xml'))
@@ -99,6 +112,9 @@ def joints(self):
     def dof(self):
         return 6
 
+    def contact_geoms(self):
+        raise NotImplementedError
+
 class PushingGripper(TwoFingerGripper):
     """Same as Two FingerGripper, but always closed"""
     def format_action(self, action):
@@ -107,6 +123,9 @@ def format_action(self, action):
     def dof(self):
         return 0
 
+    def contact_geoms(self):
+        raise NotImplementedError
+
 class RobotiqThreeFingerGripper(MujocoGripper):
     def __init__(self):
         super().__init__(xml_path_completion('gripper/robotiq_gripper_s.xml'))
@@ -138,6 +157,9 @@ def joints(self):
     def dof(self):
         return 11
 
+    def contact_geoms(self):
+        raise NotImplementedError
+
 def gripper_factory(name):
     """Genreator for grippers"""
     if name == "TwoFingerGripper":

MujocoManip/test_gripper.py

@@ -19,14 +19,27 @@
 gripper_body.append(joint(name='gripper_z_joint', type='slide', axis='0 0 1', damping='50'))
 world.merge(gripper, merge_body=False)
 world.worldbody.append(gripper_body)
-world.actuator.append(actuator(joint='gripper_z_joint', act_type='position', name='gripper_z', kp='100'))
+world.actuator.append(actuator(joint='gripper_z_joint', act_type='position', name='gripper_z', kp='500'))
 
 # Add an object for grasping
-mujoco_object = BoxObject(size=[0.01, 0.01, 0.01], rgba=[1, 0, 0, 1]).get_full()
+mujoco_object = BoxObject(size=[0.02, 0.02, 0.02], rgba=[1, 0, 0, 1]).get_full()
 mujoco_object.append(joint(name='object_free_joint', type='free'))
 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
 world.worldbody.append(mujoco_object)
 
+x_ref = BoxObject(size=[0.01, 0.01, 0.01], rgba=[0, 1, 0, 1]).get_visual()
+x_ref.set('pos', '0.2 0 0.105')
+world.worldbody.append(x_ref)
+y_ref = BoxObject(size=[0.01, 0.01, 0.01], rgba=[0, 0, 1, 1]).get_visual()
+y_ref.set('pos', '0 0.2 0.105')
+world.worldbody.append(y_ref)
+
 # Start simulation
 model = world.get_model(mode='mujoco_py')
 
@@ -48,14 +61,22 @@
 gripper_closed = [-0.020833, 0.020833]
 gripper_is_closed = True
 
-seq = [(False, False), (True, False), (True, True), (False, True), (False, False)]
+seq = [(False, False), (True, False), (True, True), (False, True)] + [(False, True)] * 20 + [(False, False)]
 
 sim.set_state(sim_state)
 step = 0
 T = 1000
 while True:
     if step % 100 == 0:
         print('step: {}'.format(step))
+    if step % T == 0:
+        for contact in sim.data.contact:
+            if sim.model.geom_id2name(contact.geom1) == 'floor' and sim.model.geom_id2name(contact.geom2) == 'floor':
+                continue
+            print("geom1: {}".format(sim.model.geom_id2name(contact.geom1)))
+            print("geom2: {}".format(sim.model.geom_id2name(contact.geom2)))
+            print("friction: {}".format(contact.friction))
+            print("normal: {}".format(contact.frame[0:3]))
     if step % T == 0:
         plan = seq[int(step / T) % len(seq)]
         gripper_z_is_low, gripper_is_closed = plan