Pyreach sync 20220425

twahrburg-google · twahrburg-google · commit 32b6d170946d · 2022-04-25T15:20:51.000-07:00
diff --git a/pyreach/digital_output.py b/pyreach/digital_output.py
@@ -36,6 +36,20 @@ class DigitalOutputState:
   robot_name: str
   pin_states: Tuple[DigitalOutputPinState, ...]
 
+  def get_pin_state(self, name: str) -> Optional[DigitalOutputPinState]:
+    """Get pin state by name.
+
+    Args:
+      name: the name of the pin.
+
+    Returns:
+      The pin, or None if not specified.
+    """
+    for state in self.pin_states:
+      if state.name == name:
+        return state
+    return None
+
 
 class DigitalOutput:
   """Class for interacting with a digital output."""
diff --git a/pyreach/examples/april_led_blinker.py b/pyreach/examples/april_led_blinker.py
@@ -0,0 +1,45 @@
+"""Blink the april-led on and off."""
+
+import time
+from typing import List
+
+from absl import app  # type: ignore
+from absl import flags  # type: ignore
+from pyreach.factory import ConnectionFactory
+
+
+def main(unused_argv: List[str]) -> None:
+  with ConnectionFactory(
+      connection_string=flags.FLAGS.connection_string,
+      enable_streaming=False,
+      robot_types={
+          "april-led": "april-led.urdf"
+      }).connect() as host:
+    april_led = host.arms.get("april-led")
+    assert april_led
+    digital_outputs = april_led.digital_outputs
+    assert digital_outputs
+    digital_outputs_by_type = digital_outputs.get("april-led")
+    assert digital_outputs_by_type
+    output = digital_outputs_by_type.get("")
+    assert output
+    led_state = False
+    while not host.is_closed():
+      print("Set led state", led_state)
+      output.set_pin_state("", led_state)
+      while not host.is_closed():
+        state = output.fetch_state()
+        if not state:
+          return
+        pin_state = state.get_pin_state("")
+        if pin_state and pin_state.state == led_state:
+          break
+      time.sleep(5.0)
+      led_state = not led_state
+
+
+if __name__ == "__main__":
+  flags.DEFINE_string(
+      "connection_string", "", "Connect using a PyReach connection string (see "
+      "connection_string.md for examples and documentation).")
+  app.run(main)
diff --git a/pyreach/gyms/docs/gym_api.md b/pyreach/gyms/docs/gym_api.md
@@ -1235,35 +1235,57 @@ initialized with the with a dictionary of the following structure:
 
 ```
  {
-     "digital_outputs": {
-         "gym_pin_name1":
-             ("reach_name", "capability_type", "reach_pin_name"),
-         "gym_pin_name2":
-             ("reach_name", "capability_type", "reach_pin_name"),
-         # ...
-         "gym_pin_nameN":
-             ("reach_name", "capability_type", "reach_pin_name"),
-     },
-     # Eventually, there will be "digital_inputs", "analog_outputs"
-     # and "analog_inputs", but not for now.
+     "io":
+          io_element.ReachIO(
+              reach_name="reach_io_device_name",
+              is_synchronous=True/False,  # synchronous/asynchronous IO device
+              digital_outputs={  # One dict entry for each digital output
+                  "gym_digital_output1":
+                      io_element.ReachDigitalOutuput(
+                          reach_name="",
+                          capability_type="",
+                          pin_name="")
+                  # ...
+                  "gym_digital_outputN":
+                      io_element.ReachDigitalOutuput(
+                          reach_name="",
+                          capability_type="",
+                          pin_name="")
+              )}
+              # digital_inputs={...}  # Not implemented yet
+              # analog_outputs={...}  # Not implemented yet
+              # analog_inputs={...}  # Not implemented yet
  }
 ```
 
-The `"digital_outputs"` configuration space consists of a dictionary of type
-`Dict[str, Tuple(str, str, str)]` where the key is a user specified name (e.g.
-`"gym_pin_nameI"`) with a three string tuple of the form `("reach_name"",
-"capability_type", "reach_pin_name")`, where
+when the action space is filled in, it will look as follows:
 
-*   `reach_name`: The reach device that hosts the I/O pin.
+```
+    action["io"]["digital_outputs"] = {
+       "digital_output1": 0,  # Turn off,
+       "digital_outputN": 1,  # Turn on,
+    }
+```
 
-*   `capability_type`: The capability type string for the pin. This is specified
+The three values fed into the `io_element.ReachDigitalOutput` are:
+
+*   `reach_name`: The name of the controller that is managing the I/O. This is
+    frequently the same name as the Arm name, but not always.
+
+*   `capability_type`: The capability type string for the pin. This is specifie
     by the robot configuration and is supplied by the operation team.
 
 *   `reach_pin_name`: The specific pin name for the capability. Again, this is
     specified by the robot configuration and is supplied by the operation team.
 
+In practice, trying to the correct values for these 3 strings can be quite
+challenging. A useful trick/hack is to specify random strings for these three
+strings. The I/O will complain about ones that are incorrect *AND* provide a
+list of value values. Thus, tremendously simplifies the task of trying to figure
+what values to used.
+
 Eventually, as other I/O types show up, additional top level configuration
-dictionaries will be added (e.g. `digital_inputs`, etc.)
+dictionary enteries are anticipated (e.g. `digital_inputs`, etc.)
 
 #### Oracle Configuration
 
diff --git a/pyreach/gyms/reach_env.py b/pyreach/gyms/reach_env.py
@@ -143,6 +143,7 @@ def __init__(self,
                host: Optional[pyreach.Host] = None,
                gym_env_id: Optional[str] = None,
                connection_string: Optional[str] = None,
+               robot_types: Optional[Dict[str, str]] = None,
                **kwargs: Any) -> None:
     """Initialize a Reach Gym Environment.
 
@@ -153,6 +154,7 @@ def __init__(self,
       host: A host to use. (Default: None.)
       gym_env_id: ID used to create this gym. Must be specified.
       connection_string: the connection string (see connection_string.md).
+      robot_types: Overrides for robot types.
       **kwargs: Additional keyword arguments.
 
     Raises:
@@ -242,6 +244,8 @@ def __init__(self,
           "enable_streaming": False,
           "arm_default_ik_types": arm_default_ik_types,
       }
+      if robot_types is not None:
+        host_kwargs["robot_types"] = robot_types.copy()
 
       if not host:
         if not connection_string:
diff --git a/pyreach/impl/arm_impl.py b/pyreach/impl/arm_impl.py
@@ -210,6 +210,8 @@ def from_urdf_file(cls, urdf_file: str) -> arm.ArmType:
         "FanucR2000ia165f.urdf", "XArm6.urdf"
     }:
       return ArmTypeImpl(urdf_file, 6)
+    if urdf_file == "april-led.urdf":
+      return ArmTypeImpl(urdf_file, 0)
     raise ValueError("invalid URDF file: " + urdf_file)
 
 
@@ -1232,7 +1234,9 @@ def support_blowoff(self) -> bool:
 
   def set_ik_lib(self, ik_lib: arm.IKLibType) -> None:
     """Set the IK library to be used."""
-    if ik_lib == arm.IKLibType.IKFAST:
+    if self._arm_type.joint_count == 0:
+      self._ik_lib = None
+    elif ik_lib == arm.IKLibType.IKFAST:
       self._ik_lib = IKLibIKFast(self._arm_type.urdf_file)
     elif ik_lib == arm.IKLibType.IKPYBULLET:
       if self._arm_type.urdf_file != "XArm6.urdf":
diff --git a/pyreach/impl/host_impl.py b/pyreach/impl/host_impl.py
@@ -153,16 +153,18 @@ def __init__(
       enable_streaming: bool = True,
       take_control_at_start: bool = True,
       arm_default_ik_types: Optional[Dict[str, arm.IKLibType]] = None,
+      robot_types: Optional[Dict[str, str]] = None,
   ) -> None:
     """Initialize the Host.
 
     Args:
       client: Connection to the client.
-      arm_types: Overrides for arm types.
+      arm_types: Overrides for robot types, deviceName to URDF file mapping.
       enable_streaming: If True, automatically start streaming for Arm,
         ColorCamera and DepthCamera devices.
       take_control_at_start: If True, immediately take control.
       arm_default_ik_types: Default ik type for arms.
+      robot_types: Overrides for robot types, deviceName to URDF file mapping.
 
     Raises:
       Exception: when failed to load config.
@@ -247,6 +249,12 @@ def __init__(
     interfaces = self._config.machine_interfaces
 
     def get_arm_type(name: str) -> Optional[arm.ArmType]:
+      if robot_types and name in robot_types:
+        try:
+          return arm_impl.ArmTypeImpl.from_urdf_file(robot_types[name])
+        except ValueError:
+          logging.warning("Robot type override %s for %s invalid",
+                          robot_types[name], name)
       if arm_types and name in arm_types:
         try:
           return arm_impl.ArmTypeImpl.from_urdf_file(arm_types[name])
