Merge pull request #151 from ARISE-Initiative/dynamics-randomization

Add dynamics randomization

robosuite/__init__.py

@@ -16,7 +16,7 @@
 from robosuite.robots import ALL_ROBOTS
 from robosuite.models.grippers import ALL_GRIPPERS
 
-__version__ = "1.1.0"
+__version__ = "1.2.0"
 __logo__ = """
       ;     /        ,--.
      ["]   ["]  ,<  |__**|

robosuite/controllers/osc.py

@@ -232,23 +232,26 @@ def set_goal(self, action, set_pos=None, set_ori=None):
         if self.use_delta:
             if delta is not None:
                 scaled_delta = self.scale_action(delta)
-                if not self.use_ori:
+                if not self.use_ori and set_ori is None:
                     # Set default control for ori since user isn't actively controlling ori
-                    set_ori = np.array([[-1., 0., 0.], [0., 1., 0.], [0., 0., -1.]])
+                    set_ori = np.array([[0, -1., 0.], [-1., 0., 0.], [0., 0., 1.]])
             else:
                 scaled_delta = []
         # Else, interpret actions as absolute values
         else:
-            set_pos = delta[:3]
+            if set_pos is None:
+                set_pos = delta[:3]
             # Set default control for ori if we're only using position control
-            set_ori = T.quat2mat(T.axisangle2quat(delta[3:6])) if self.use_ori else \
-                np.array([[-1., 0., 0.], [0., 1., 0.], [0., 0., -1.]])
-            scaled_delta = []
+            if set_ori is None:
+                set_ori = T.quat2mat(T.axisangle2quat(delta[3:6])) if self.use_ori else \
+                    np.array([[0, -1., 0.], [-1., 0., 0.], [0., 0., 1.]])
+            # No scaling of values since these are absolute values
+            scaled_delta = delta
 
-        # We only want to update goal orientation if there is a valid delta ori value
+        # We only want to update goal orientation if there is a valid delta ori value OR if we're using absolute ori
         # use math.isclose instead of numpy because numpy is slow
         bools = [0. if math.isclose(elem, 0.) else 1. for elem in scaled_delta[3:]]
-        if sum(bools) > 0.:
+        if sum(bools) > 0. or set_ori is not None:
             self.goal_ori = set_goal_orientation(scaled_delta[3:],
                                                  self.ee_ori_mat,
                                                  orientation_limit=self.orientation_limits,

robosuite/demos/demo_device_control.py

@@ -116,8 +116,8 @@
     parser.add_argument("--toggle-camera-on-grasp", action="store_true", help="Switch camera angle on gripper action")
     parser.add_argument("--controller", type=str, default="osc", help="Choice of controller. Can be 'ik' or 'osc'")
     parser.add_argument("--device", type=str, default="keyboard")
-    parser.add_argument("--pos-sensitivity", type=float, default=1.5, help="How much to scale position user inputs")
-    parser.add_argument("--rot-sensitivity", type=float, default=1.5, help="How much to scale rotation user inputs")
+    parser.add_argument("--pos-sensitivity", type=float, default=1.0, help="How much to scale position user inputs")
+    parser.add_argument("--rot-sensitivity", type=float, default=1.0, help="How much to scale rotation user inputs")
     args = parser.parse_args()
 
     # Import controller config for EE IK or OSC (pos/ori)

robosuite/demos/demo_sensor_corruption.py

@@ -34,8 +34,8 @@
                         help="Toggle corruption ON / OFF on gripper action")
     parser.add_argument("--controller", type=str, default="osc", help="Choice of controller. Can be 'ik' or 'osc'")
     parser.add_argument("--device", type=str, default="keyboard")
-    parser.add_argument("--pos-sensitivity", type=float, default=1.5, help="How much to scale position user inputs")
-    parser.add_argument("--rot-sensitivity", type=float, default=1.5, help="How much to scale rotation user inputs")
+    parser.add_argument("--pos-sensitivity", type=float, default=1.0, help="How much to scale position user inputs")
+    parser.add_argument("--rot-sensitivity", type=float, default=1.0, help="How much to scale rotation user inputs")
     parser.add_argument("--delay", type=float, default=0.04, help="average delay to use (sec)")
     parser.add_argument("--corruption", type=float, default=20.0, help="Scale of corruption to use (std dev)")
     parser.add_argument("--camera", type=str, default="agentview", help="Name of camera to render")

robosuite/environments/base.py

@@ -252,7 +252,7 @@ def reset(self):
         # 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_observations()
+        return self._get_observations(force_update=True)
 
     def _reset_internal(self):
         """Resets simulation internal configurations."""
@@ -293,24 +293,38 @@ def _reset_internal(self):
         for observable in self._observables.values():
             observable.reset()
 
-    def _update_observables(self):
+    def _update_observables(self, force=False):
         """
         Updates all observables in this environment
+
+        Args:
+            force (bool): If True, will force all the observables to update their internal values to the newest
+                value. This is useful if, e.g., you want to grab observations when directly setting simulation states
+                without actually stepping the simulation.
         """
         for observable in self._observables.values():
-            observable.update(timestep=self.model_timestep, obs_cache=self._obs_cache)
+            observable.update(timestep=self.model_timestep, obs_cache=self._obs_cache, force=force)
 
-    def _get_observations(self):
+    def _get_observations(self, force_update=False):
         """
         Grabs observations from the environment.
 
+        Args:
+            force_update (bool): If True, will force all the observables to update their internal values to the newest
+                value. This is useful if, e.g., you want to grab observations when directly setting simulation states
+                without actually stepping the simulation.
+
         Returns:
             OrderedDict: OrderedDict containing observations [(name_string, np.array), ...]
 
         """
         observations = OrderedDict()
         obs_by_modality = OrderedDict()
 
+        # Force an update if requested
+        if force_update:
+            self._update_observables(force=True)
+
         # Loop through all observables and grab their current observation
         for obs_name, observable in self._observables.items():
             if observable.is_enabled() and observable.is_active():
@@ -572,7 +586,8 @@ def modify_observable(self, observable_name, attribute, modifier):
         Args:
              observable_name (str): Observable to modify
              attribute (str): Observable attribute to modify.
-                Options are {`'sensor'`, `'corrupter'`, `'delayer'`, `'sampling_rate'`, `'enabled'`, `'active'`}
+                Options are {`'sensor'`, `'corrupter'`,`'filter'`,  `'delayer'`, `'sampling_rate'`,
+                `'enabled'`, `'active'`}
              modifier (any): New function / value to replace with for observable. If a function, new signature should
                 match the function being replaced.
         """
@@ -585,6 +600,8 @@ def modify_observable(self, observable_name, attribute, modifier):
             obs.set_sensor(modifier)
         elif attribute == "corrupter":
             obs.set_corrupter(modifier)
+        elif attribute == "filter":
+            obs.set_filter(modifier)
         elif attribute == "delayer":
             obs.set_delayer(modifier)
         elif attribute == "sampling_rate":
@@ -596,7 +613,7 @@ def modify_observable(self, observable_name, attribute, modifier):
         else:
             # Invalid attribute specified
             raise ValueError("Invalid observable attribute specified. Requested: {}, valid options are {}".
-                             format(attribute, {"sensor", "corrupter", "delayer",
+                             format(attribute, {"sensor", "corrupter", "filter", "delayer",
                                                 "sampling_rate", "enabled", "active"}))
 
     def _check_success(self):

robosuite/models/assets/base.xml

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

robosuite/models/assets/grippers/panda_gripper.xml

@@ -7,8 +7,8 @@
         <mesh name="finger_vis2" file="meshes/panda_gripper/finger_longer.stl" />
     </asset>
     <actuator>
-        <position ctrllimited="true" ctrlrange="0.0 0.04" joint="finger_joint1" kp="1000000" name="gripper_finger_joint1" forcelimited="true" forcerange="-20 20"/>
-        <position ctrllimited="true" ctrlrange="-0.04 0.0" joint="finger_joint2" kp="1000000" name="gripper_finger_joint2" forcelimited="true" forcerange="-20 20"/>
+        <position ctrllimited="true" ctrlrange="0.0 0.04" joint="finger_joint1" kp="1000" name="gripper_finger_joint1" forcelimited="true" forcerange="-20 20"/>
+        <position ctrllimited="true" ctrlrange="-0.04 0.0" joint="finger_joint2" kp="1000" name="gripper_finger_joint2" forcelimited="true" forcerange="-20 20"/>
     </actuator>
     <worldbody>
         <body name="right_gripper" pos="0 0 0" quat="0.707107 0 0 -0.707107">
@@ -22,7 +22,7 @@
             <site name="grip_site_cylinder" pos="0 0 0.1399" size="0.005 10" rgba="0 1 0 0.3" type="cylinder" group="1"/>
             <body name="leftfinger" pos="0 0 0.0524" quat="0.707107 0 0 0.707107">
                 <inertial pos="0 0 0.05" mass="0.1" diaginertia="0.01 0.01 0.005" />
-                <joint name="finger_joint1" pos="0 0 0" axis="0 1 0" type="slide" limited="true" range="0.0 0.04" damping="100"/>
+                <joint name="finger_joint1" pos="0 0 0" axis="0 1 0" type="slide" limited="true" range="0.0 0.04" damping="100" armature="1.0" frictionloss="1.0"/>
                 <geom type="mesh" contype="0" conaffinity="0" group="1" mesh="finger_vis" name="finger1_visual" rgba="0.499 0.499 0.499 1" />
                 <geom type="mesh" group="0" conaffinity="1" contype="0" solref="0.02 1" friction="1 0.005 0.0001" condim="4" mesh="finger" name="finger1_collision"/>
                 <body name="finger_joint1_tip" pos="0 0.0085 0.056">
@@ -32,7 +32,7 @@
             </body>
             <body name="rightfinger" pos="0 0 0.0524" quat="0.707107 0 0 0.707107">
                 <inertial pos="0 0 0.05" mass="0.1" diaginertia="0.01 0.01 0.005" />
-                <joint name="finger_joint2" pos="0 0 0" axis="0 1 0" type="slide" limited="true" range="-0.04 0.0" damping="100"/>
+                <joint name="finger_joint2" pos="0 0 0" axis="0 1 0" type="slide" limited="true" range="-0.04 0.0" damping="100" armature="1.0" frictionloss="1.0"/>
                 <geom quat="0 0 0 1" type="mesh" contype="0" conaffinity="0" group="1" mesh="finger_vis" name="finger2_visual" rgba="0.499 0.499 0.499 1" />
                 <geom quat="0 0 0 1" type="mesh" group="0" conaffinity="1" contype="0" solref="0.02 1" friction="1 0.005 0.0001" condim="4" mesh="finger" name="finger2_collision"/>
                 <body name="finger_joint2_tip" pos="0 -0.0085 0.056">

robosuite/models/assets/grippers/rethink_gripper.xml

@@ -6,8 +6,8 @@
         <mesh name="connector_plate" file="meshes/rethink_gripper/connector_plate.stl" />
     </asset>
     <actuator>
-        <position ctrllimited="true" ctrlrange="-0.0115 0.020833" joint="r_finger_joint" kp="10000" name="gripper_r_finger_joint" forcelimited="true" forcerange="-20 20"/>
-        <position ctrllimited="true" ctrlrange="-0.020833 0.0115" joint="l_finger_joint" kp="10000" name="gripper_l_finger_joint" forcelimited="true" forcerange="-20 20"/>
+        <position ctrllimited="true" ctrlrange="-0.0115 0.020833" joint="r_finger_joint" kp="1000" name="gripper_r_finger_joint" forcelimited="true" forcerange="-20 20"/>
+        <position ctrllimited="true" ctrlrange="-0.020833 0.0115" joint="l_finger_joint" kp="1000" name="gripper_l_finger_joint" forcelimited="true" forcerange="-20 20"/>
     </actuator>
 
     <worldbody>
@@ -23,7 +23,7 @@
             <site name="grip_site_cylinder" pos="0 0 0" size="0.005 10" rgba="0 1 0 0.3" type="cylinder" group="1"/>
             <body name="l_finger" pos="0 0.01 0.0444">
                 <inertial pos="0 0 0" quat="0 0 0 -1" mass="0.02" diaginertia="0.01 0.01 0.01" />
-                <joint name="l_finger_joint" pos="0 0 0" axis="0 1 0" type="slide" limited="true" range="-0.0115 0.020833" damping="100"/>
+                <joint name="l_finger_joint" pos="0 0 0" axis="0 1 0" type="slide" limited="true" range="-0.0115 0.020833" damping="100" armature="1.0" frictionloss="1.0"/>
                 <geom name="l_finger" quat="0 0 0 -1" type="mesh" contype="0" conaffinity="0" group="1" mesh="standard_narrow" rgba="0.499 0.499 0.499 1"/>
                 <geom size="0.005 0.00675 0.0375" pos="0 0.01725 0.04" quat="0 0 0 -1" type="box" group="0" conaffinity="1" contype="0" name="l_finger_g0" friction="0 0 0"/>
                 <geom size="0.005 0.025 0.0085" pos="-0.005 -0.003 0.0083" quat="0 0 0 -1" type="box" group="0" conaffinity="1" contype="0" name="l_finger_g1" friction="0 0 0"/>
@@ -36,7 +36,7 @@
             </body>
             <body name="r_finger" pos="0 -0.01 0.0444">
                 <inertial pos="0 0 0" mass="0.02" diaginertia="0.01 0.01 0.01" />
-                <joint name="r_finger_joint" pos="0 0 0" axis="0 1 0" type="slide" limited="true" range="-0.020833 0.0115" damping="100"/>
+                <joint name="r_finger_joint" pos="0 0 0" axis="0 1 0" type="slide" limited="true" range="-0.020833 0.0115" damping="100" armature="1.0" frictionloss="1.0"/>
                 <geom name="r_finger" type="mesh" contype="0" conaffinity="0" group="1" mesh="standard_narrow" rgba="0.499 0.499 0.499 1"/>
                 <geom size="0.005 0.00675 0.0375" pos="0 -0.01725 0.04" type="box" group="0" conaffinity="1" contype="0" name="r_finger_g0" friction="0 0 0"/>
                 <geom size="0.005 0.025 0.0085" pos="0.005 0.003 0.0083" type="box" group="0" conaffinity="1" contype="0" name="r_finger_g1" friction="0 0 0"/>

robosuite/models/robots/robot_model.py

@@ -67,6 +67,12 @@ def __init__(self, fname, idn=0):
         # Get camera names for this robot
         self.cameras = self.get_element_names(self.worldbody, "camera")
 
+        # By default, set small frictionloss and armature values
+        self.set_joint_attribute(attrib="frictionloss", values=0.1 * np.ones(self.dof), force=False)
+        self.set_joint_attribute(attrib="damping", values=0.1 * np.ones(self.dof), force=False)
+        self.set_joint_attribute(attrib="armature",
+                                 values=np.array([5.0 / (i + 1) for i in range(self.dof)]), force=False)
+
     def set_base_xpos(self, pos):
         """
         Places the robot on position @pos.
@@ -87,21 +93,24 @@ def set_base_ori(self, rot):
         rot = mat2quat(euler2mat(rot))[[3,0,1,2]]
         self._elements["root_body"].set("quat", array_to_string(rot))
 
-    def set_joint_attribute(self, attrib, values):
+    def set_joint_attribute(self, attrib, values, force=True):
         """
         Sets joint attributes, e.g.: friction loss, damping, etc.
 
         Args:
             attrib (str): Attribute to set for all joints
             values (n-array): Values to set for each joint
+            force (bool): If True, will automatically override any pre-existing value. Otherwise, if a value already
+                exists for this value, it will be skipped
 
         Raises:
             AssertionError: [Inconsistent dimension sizes]
         """
         assert values.size == len(self._elements["joints"]), "Error setting joint attributes: " + \
             "Values must be same size as joint dimension. Got {}, expected {}!".format(values.size, self.dof)
         for i, joint in enumerate(self._elements["joints"]):
-            joint.set(attrib, array_to_string(np.array([values[i]])))
+            if force or joint.get(attrib, None) is None:
+                joint.set(attrib, array_to_string(np.array([values[i]])))
 
     def add_mount(self, mount):
         """

robosuite/scripts/collect_human_demonstrations.py

@@ -184,8 +184,8 @@ def gather_demonstrations_as_hdf5(directory, out_dir, env_info):
     parser.add_argument("--controller", type=str, default="OSC_POSE",
                         help="Choice of controller. Can be 'IK_POSE' or 'OSC_POSE'")
     parser.add_argument("--device", type=str, default="keyboard")
-    parser.add_argument("--pos-sensitivity", type=float, default=1.5, help="How much to scale position user inputs")
-    parser.add_argument("--rot-sensitivity", type=float, default=1.5, help="How much to scale rotation user inputs")
+    parser.add_argument("--pos-sensitivity", type=float, default=1.0, help="How much to scale position user inputs")
+    parser.add_argument("--rot-sensitivity", type=float, default=1.0, help="How much to scale rotation user inputs")
     args = parser.parse_args()
 
 

robosuite/utils/control_utils.py

@@ -185,7 +185,7 @@ def set_goal_orientation(delta,
         # convert axis-angle value to rotation matrix
         quat_error = trans.axisangle2quat(delta)
         rotation_mat_error = trans.quat2mat(quat_error)
-        goal_orientation = np.dot(rotation_mat_error.T, current_orientation)
+        goal_orientation = np.dot(rotation_mat_error, current_orientation)
 
     # check for orientation limits
     if np.array(orientation_limit).any():

robosuite/utils/input_utils.py

@@ -7,6 +7,7 @@
 import robosuite as suite
 from robosuite.models.robots import *
 from robosuite.robots import *
+from robosuite.devices import *
 import robosuite.utils.transform_utils as T
 
 
@@ -241,11 +242,9 @@ def input2action(device, robot, active_arm="right", env_configuration=None):
     elif controller.name == 'OSC_POSE':
         # Flip z
         drotation[2] = -drotation[2]
-        # Scale rotation for teleoperation (tuned for OSC)
-        drotation *= 75
-        dpos *= 200
-        # Interpret euler angles as (mirrored) scaled axis angle values
-        drotation = -drotation
+        # Scale rotation for teleoperation (tuned for OSC) -- gains tuned for each device
+        drotation = drotation * 50 if isinstance(device, SpaceMouse) else drotation * 1.5
+        dpos = dpos * 125 if isinstance(device, SpaceMouse) else dpos * 75
     else:
         # No other controllers currently supported
         print("Error: Unsupported controller specified -- Robot must have either an IK or OSC-based controller!")