Skip to content
This repository was archived by the owner on Oct 4, 2023. It is now read-only.

Commit 4514d3a

Browse files
twahrburg-googletwahrburg-google
committed
Pyreach sync 20220125
1 parent f91753c commit 4514d3a

23 files changed

+537
-336
lines changed

‎pyreach/arm.py‎

Lines changed: 9 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -11,7 +11,6 @@
1111
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
1212
# See the License for the specific language governing permissions and
1313
# limitations under the License.
14-
1514
"""Interface for interacting with a single robot arm.
1615
1716
Supported features
@@ -29,6 +28,7 @@
2928

3029
from pyreach import constraints
3130
from pyreach import core
31+
from pyreach import digital_output
3232

3333

3434
class IKLibType(enum.Enum):
@@ -150,6 +150,14 @@ def arm_type(self) -> ArmType:
150150
"""Return the arm type of the arm."""
151151
raise NotImplementedError
152152

153+
@property
154+
def digital_outputs(
155+
self
156+
) -> core.ImmutableDictionary[core.ImmutableDictionary[
157+
digital_output.DigitalOutput]]:
158+
"""Get the digital outputs for this arm device."""
159+
raise NotImplementedError
160+
153161
@property
154162
def supported_controllers(
155163
self) -> Optional[Tuple[ArmControllerDescription, ...]]:

‎pyreach/gyms/devices/annotation_device.py‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -184,3 +184,7 @@ def validate(self, host: pyreach.Host) -> str:
184184
except pyreach.PyReachError as pyreach_error:
185185
return str(pyreach_error)
186186
return ""
187+
188+
def synchronize(self) -> None:
189+
"""Force the annotation device synchronize its observations."""
190+
pass

‎pyreach/gyms/devices/arm_device.py‎

Lines changed: 48 additions & 34 deletions
Original file line numberDiff line numberDiff line change
@@ -11,7 +11,6 @@
1111
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
1212
# See the License for the specific language governing permissions and
1313
# limitations under the License.
14-
1514
"""Implementation of PyReach Gym Arm Device."""
1615

1716
import copy
@@ -41,17 +40,17 @@ class ReachDeviceArm(reach_device.ReachDevice):
4140
4241
Attributes:
4342
action_space: A Gym action space represented as a Gym Dict Space with
44-
"command", "joint_angles", "pose", "synchronous", "id", "controller"
45-
and "command" should be 0 for do nothing, 1 for set joint angles and 2
46-
for set pose. "joint_angles" should be the desired joint angles in
47-
radians. "pose" should be the desired arm pose. "synchronous" is only
48-
valid when the arm is configured as asynchronous. When set to 1, a
49-
synchronous move is performed. When set to 0 (or not present), an
50-
asynchronous move occurs. "id" is used to keep track of asynchronous
51-
move status. When id is present and positive, each asynchronous move
52-
can be given a unique id (simple counter bumping adequate) to keep
53-
track of the returned status for the move. The most recent returned
54-
statuses are put into the "responses" portion of the arm observation.
43+
"command", "joint_angles", "pose", "synchronous", "id", "controller" and
44+
"command" should be 0 for do nothing, 1 for set joint angles and 2 for set
45+
pose. "joint_angles" should be the desired joint angles in radians. "pose"
46+
should be the desired arm pose. "synchronous" is only valid when the arm
47+
is configured as asynchronous. When set to 1, a synchronous move is
48+
performed. When set to 0 (or not present), an asynchronous move occurs.
49+
"id" is used to keep track of asynchronous move status. When id is present
50+
and positive, each asynchronous move can be given a unique id (simple
51+
counter bumping adequate) to keep track of the returned status for the
52+
move. The most recent returned statuses are put into the "responses"
53+
portion of the arm observation.
5554
observation_space: A Gym observation space represented as a Gym Dict Space
5655
with "ts", "joint_angles", and "pose" fields.
5756
"""
@@ -294,6 +293,7 @@ def get_observation(self,
294293
pyreach_status))
295294
status: str = pyreach_status.status
296295
error: str = pyreach_status.error
296+
message: str = pyreach_status.message
297297
if status == "done":
298298
if error == "timeout":
299299
response = arm_element.ReachResponse.RESPONSE_TIMEOUT
@@ -304,8 +304,8 @@ def get_observation(self,
304304
elif status == "aborted":
305305
response = arm_element.ReachResponse.RESPONSE_ABORTED
306306
else:
307-
logging.warning("Internal Error: Unexpected response '%s' '%s'",
308-
status, error)
307+
logging.warning("Internal Error: Unexpected response '%s' '%s' ' %s'",
308+
status, error, message)
309309
response = arm_element.ReachResponse.RESPONSE_FAILED
310310
if not 0 <= response <= arm_element.ReachResponse.RESPONSE_MAX:
311311
raise pyreach.PyReachError(
@@ -361,6 +361,11 @@ def get_observation(self,
361361
ts, arm_state.sequence),)
362362
return observation, snapshot_reference, tuple(responses)
363363

364+
def synchronize(self) -> None:
365+
"""Synchronously update the arm state."""
366+
if self._arm:
367+
_ = self._arm.fetch_state()
368+
364369
def _get_response_queue(
365370
self
366371
) -> Tuple[gyms_core.Observation, List[lib_snapshot.SnapshotResponse]]:
@@ -594,19 +599,24 @@ def do_action(
594599
timeout=timeout))
595600
return cmd_tuple
596601

597-
# Keep track of the callbacks:
598-
count = self._arm_state_capturer.start(action_id)
602+
callback = None
603+
finished_callback = None
604+
605+
if self._response_queue_length:
606+
# Keep track of the callbacks:
607+
count = self._arm_state_capturer.start(action_id)
599608

600-
def arm_state_callback(arm_state: pyreach.PyReachStatus) -> None:
601-
"""Track asynchronous arm state callbacks."""
602-
self._arm_state_capturer.callback(count, arm_state)
609+
def arm_state_callback(arm_state: pyreach.PyReachStatus) -> None:
610+
"""Track asynchronous arm state callbacks."""
611+
self._arm_state_capturer.callback(count, arm_state)
603612

604-
def arm_state_finished_callback() -> None:
605-
"""Track finished arm state callback."""
606-
self._arm_state_capturer.finished_callback(count)
613+
def arm_state_finished_callback() -> None:
614+
"""Track finished arm state callback."""
615+
self._arm_state_capturer.finished_callback(count)
616+
617+
callback = arm_state_callback
618+
finished_callback = arm_state_finished_callback
607619

608-
callback = arm_state_callback
609-
finished_callback = arm_state_finished_callback
610620
with self._timers.select({"!agent*", "!gym*", "host.arm.to_joints"}):
611621
arm.async_to_joints(
612622
joints,
@@ -662,19 +672,23 @@ def arm_state_finished_callback() -> None:
662672
timeout=timeout)
663673
return cmd_tuple
664674

665-
# Keep track of the callbacks:
666-
count = self._arm_state_capturer.start(action_id)
675+
callback = None
676+
finished_callback = None
677+
678+
if self._response_queue_length:
679+
# Keep track of the callbacks:
680+
count = self._arm_state_capturer.start(action_id)
667681

668-
def arm_state_callback2(arm_state: pyreach.PyReachStatus) -> None:
669-
"""Track asynchronous arm state callbacks."""
670-
self._arm_state_capturer.callback(count, arm_state)
682+
def arm_state_callback2(arm_state: pyreach.PyReachStatus) -> None:
683+
"""Track asynchronous arm state callbacks."""
684+
self._arm_state_capturer.callback(count, arm_state)
671685

672-
def arm_state_finished_callback2() -> None:
673-
"""Track finished arm state callback."""
674-
self._arm_state_capturer.finished_callback(count)
686+
def arm_state_finished_callback2() -> None:
687+
"""Track finished arm state callback."""
688+
self._arm_state_capturer.finished_callback(count)
675689

676-
callback = arm_state_callback2
677-
finished_callback = arm_state_finished_callback2
690+
callback = arm_state_callback2
691+
finished_callback = arm_state_finished_callback2
678692

679693
with self._timers.select({"!agent*", "!gym*", "host.arm.to_pose"}):
680694
arm.async_to_pose(

‎pyreach/gyms/devices/color_camera_device.py‎

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -239,6 +239,11 @@ def get_observation(self,
239239
ts, color_frame.sequence),)
240240
return observation, snapshot_reference, ()
241241

242+
def synchronize(self) -> None:
243+
"""Synchronously fetch an image."""
244+
if self._color_camera:
245+
_ = self._color_camera.fetch_image()
246+
242247
def start_observation(self, host: pyreach.Host) -> bool:
243248
"""Start a synchronous observation."""
244249
camera: pyreach.ColorCamera = self._get_color_camera(host)

‎pyreach/gyms/devices/constraints_device.py‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -125,6 +125,10 @@ def get_observation(self,
125125
self._counter, "constraints", self.config_name,
126126
lib_snapshot.SnapshotReference(0.0, counter)),)
127127

128+
def synchronize(self) -> None:
129+
"""Synchronously fetch constraints."""
130+
pass # Currently constraints don't change.
131+
128132
def do_action(
129133
self, action: gyms_core.Action,
130134
host: pyreach.Host) -> Tuple[lib_snapshot.SnapshotGymAction, ...]:

‎pyreach/gyms/devices/depth_camera_device.py‎

Lines changed: 6 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -277,7 +277,12 @@ def get_observation(self,
277277
"intrinsics": np.array(calibration_camera.intrinsics),
278278
}
279279

280-
return observation, snapshot_reference, ()
280+
return observation, snapshot_reference, ()
281+
282+
def synchronize(self) -> None:
283+
"""Synchronously fetch an image."""
284+
if self._depth_camera:
285+
_ = self._depth_camera.fetch_image()
281286

282287
def start_observation(self, host: pyreach.Host) -> bool:
283288
"""Start a synchronous observation."""

‎pyreach/gyms/devices/force_torque_sensor_device.py‎

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -147,6 +147,11 @@ def get_observation(self,
147147

148148
return observation, snapshot_reference, ()
149149

150+
def synchronize(self) -> None:
151+
"""Synchronously update the arm state."""
152+
if self._force_torque_sensor:
153+
self._force_torque_sensor.fetch_state()
154+
150155
def do_action(
151156
self, action: gyms_core.Action,
152157
host: pyreach.Host) -> Tuple[lib_snapshot.SnapshotGymAction, ...]:

‎pyreach/gyms/devices/oracle_device.py‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -301,6 +301,10 @@ def get_observation(self,
301301
}
302302
return observation, (), ()
303303

304+
def synchronize(self) -> None:
305+
"""Synchronously update the oracle state."""
306+
pass
307+
304308
def start_observation(self, host: pyreach.Host) -> bool:
305309
"""Start a synchronous observation."""
306310
# The gym gets gets the first observation before getting the first action.

‎pyreach/gyms/devices/reach_device.py‎

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -120,6 +120,10 @@ def _add_update_callback(self,
120120
"Internal Error: No ReachDeviceSynchronous object")
121121
reach_synchronous.add_update_callback(add_update_callback, self)
122122

123+
def set_task_synchronize(self, task_synchronize: Callable[[], None]) -> None:
124+
"""Provide a global task synchronize function."""
125+
pass # Most devices will ignore this.
126+
123127
def do_action(
124128
self, action: gyms_core.Action,
125129
host: pyreach.Host) -> Tuple[lib_snapshot.SnapshotGymAction, ...]:
@@ -186,6 +190,10 @@ def set_timers(self, timers: internal.Timers) -> None:
186190
"""Set the reach environment."""
187191
self._timers = timers
188192

193+
def synchronize(self) -> None:
194+
"""Force the device synchronize its observations."""
195+
raise NotImplementedError(f"Device {self._reach_name}; no synchronize()")
196+
189197
def _reshape_image(self, old_image: np.ndarray,
190198
new_shape: Tuple[int, ...]) -> np.ndarray:
191199
"""Return a reshaped image.

‎pyreach/gyms/devices/server_device.py‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -81,6 +81,10 @@ def get_observation(self,
8181
}
8282
return server_observation, (), ()
8383

84+
def synchronize(self) -> None:
85+
"""Synchronously the server."""
86+
pass
87+
8488
def do_action(
8589
self, action: gyms_core.Action,
8690
host: pyreach.Host) -> Tuple[lib_snapshot.SnapshotGymAction, ...]:

0 commit comments

Comments
 (0)