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

Commit daa3c3c

Browse files
twahrburg-googletwahrburg-google
committed
pyreach sync 20211102
1 parent 809e8ca commit daa3c3c

20 files changed

+229
-286
lines changed

‎README.md‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -3,6 +3,7 @@
33
### Disclaimer: this is not an officially supported Google product.
44

55
PyReach is the Python client SDK to remotely control robots.
6+
Operator credentials will be approved by invitation only at this time
67

78
## Supported Platform
89

‎pyreach/arm.py‎

Lines changed: 17 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -114,21 +114,23 @@ class ArmState:
114114
robot_mode: Mode of operation of the robot.
115115
"""
116116

117-
time: float
118-
sequence: int
119-
device_type: str
120-
device_name: str
121-
joint_angles: Tuple[float, ...]
122-
pose: core.Pose
123-
force: Tuple[float, ...]
124-
is_protective_stopped: bool
125-
is_emergency_stopped: bool
126-
is_safeguard_stopped: bool
127-
is_reduced_mode: bool
128-
safety_message: str
129-
is_program_running: bool
130-
is_robot_power_on: bool
131-
robot_mode: RobotMode
117+
time: float = 0.0
118+
sequence: int = 0
119+
device_type: str = "robot"
120+
device_name: str = ""
121+
joint_angles: Tuple[float, ...] = (0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
122+
pose: core.Pose = core.Pose(
123+
core.Translation(0.0, 0.0, 0.0), core.AxisAngle.from_tuple(
124+
(0.0, 0.0, 0.0)))
125+
force: Tuple[float, ...] = (0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
126+
is_protective_stopped: bool = False
127+
is_emergency_stopped: bool = False
128+
is_safeguard_stopped: bool = False
129+
is_reduced_mode: bool = False
130+
safety_message: str = ""
131+
is_program_running: bool = False
132+
is_robot_power_on: bool = False
133+
robot_mode: RobotMode = RobotMode.DEFAULT
132134

133135

134136
class Arm(object):

‎pyreach/examples/benchmark_kitting_agent.py‎

Lines changed: 2 additions & 32 deletions
Original file line numberDiff line numberDiff line change
@@ -11,34 +11,7 @@
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-
"""Template for a Kitting agent using the PyReach Gym.
15-
16-
This template demonstrates basic usage of the Kitting Gym environment. It
17-
implements essentially a random policy, performing small moves with a
18-
50% chance. The essence of the policy is in the agent's calculate_action()
19-
function.
20-
21-
== Creating the environment ==
22-
23-
As with all OpenAI Gyms, the Kitting environment is created like this:
24-
25-
with gym.make("benchmark-kitting-v0") as env:
26-
your_code_here(env)
27-
28-
== About the environment ==
29-
30-
The environment provides two functions:
31-
32-
obs = env.reset()
33-
obs, reward, done, info = env.step(action)
34-
35-
The reset() function should be called at the beginning of an episode and will
36-
reset the scene in SIM only.
37-
38-
=== The agent loop ===
39-
40-
See the run() function.
41-
"""
14+
"""Kitting benchmark agent."""
4215

4316
import collections
4417
import time
@@ -91,10 +64,7 @@ def kitting_reward_function(
9164
# pylint: disable=unused-argument
9265
def calculate_action(self, observation: core.Observation,
9366
current_instruction: str) -> core.Action:
94-
"""Do something based on the observation and the current instruction.
95-
96-
This is random for now.
97-
Fill me in!
67+
"""Construct an action that sends an oracle inference request.
9868
9969
Args:
10070
observation: the current observation.

‎pyreach/factory.py‎

Lines changed: 7 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -11,10 +11,10 @@
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
"""Factory interface and concrete factory for connecting to Reach host."""
1615

1716
import importlib
17+
import pathlib
1818
from typing import Dict, Any, Optional, List
1919

2020
import pyreach
@@ -71,12 +71,12 @@ class WebRTCHostFactory(HostFactory):
7171
"""Factory for connecting to Reach using webrtc."""
7272

7373
_robot_id: str
74-
_working_directory: Optional[str]
74+
_working_directory: Optional[pathlib.Path]
7575
_kwargs: Any
7676

7777
def __init__(self,
7878
robot_id: str,
79-
working_directory: Optional[str] = None,
79+
working_directory: Optional[pathlib.Path] = None,
8080
reach_connect_arguments: Optional[List[str]] = None,
8181
**kwargs: Any) -> None:
8282
"""Construct a WebRTCHostFactory object.
@@ -109,15 +109,15 @@ def connect(self) -> pyreach.Host:
109109
raise pyreach.PyReachError()
110110

111111
fn = getattr(mod, 'reach_connect_webrtc')
112-
return fn(self._robot_id, self._working_directory, True,
112+
return fn(self._robot_id, self._working_directory, True, True,
113113
self._reach_connect_arguments, self._kwargs)
114114

115115

116116
class RemoteTCPHostFactory(HostFactory):
117117
"""Factory for connecting to Reach using remote tcp."""
118118

119119
_robot_id: str
120-
_working_directory: Optional[str]
120+
_working_directory: Optional[pathlib.Path]
121121
_connect_host: Optional[str]
122122
_connect_port: Optional[int]
123123
_kwargs: Any
@@ -126,7 +126,7 @@ def __init__(self,
126126
robot_id: str,
127127
connect_host: Optional[str] = None,
128128
connect_port: Optional[int] = 50009,
129-
working_directory: Optional[str] = None,
129+
working_directory: Optional[pathlib.Path] = None,
130130
reach_connect_arguments: Optional[List[str]] = None,
131131
**kwargs: Any) -> None:
132132
"""Construct a RemoteTCPHostFactory object.
@@ -164,7 +164,7 @@ def connect(self) -> pyreach.Host:
164164

165165
fn = getattr(mod, 'reach_connect_remote_tcp')
166166
return fn(self._robot_id, self._connect_host, self._connect_port,
167-
self._working_directory, self._reach_connect_arguments,
167+
self._working_directory, True, self._reach_connect_arguments,
168168
self._kwargs)
169169

170170

‎pyreach/gyms/arm_element.py‎

Lines changed: 4 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -15,8 +15,9 @@
1515
"""Reach arm element used for configuration."""
1616

1717
import dataclasses
18-
from typing import Optional, Tuple
18+
from typing import List, Optional, Tuple
1919

20+
from pyreach import arm
2021
from pyreach.gyms import reach_element
2122

2223

@@ -108,3 +109,5 @@ class ReachArm(reach_element.ReachElement):
108109
controllers: Tuple[str] = ("",)
109110
e_stop_mode: int = ReachStopMode.STOP_ERROR
110111
p_stop_mode: int = ReachStopMode.STOP_ERROR
112+
# Used for unit testing only:
113+
test_states: Optional[List[arm.ArmState]] = None

‎pyreach/gyms/devices/arm_device.py‎

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -26,6 +26,7 @@
2626
import numpy as np # type: ignore
2727

2828
import pyreach
29+
from pyreach import arm as pyreach_arm
2930
from pyreach import core
3031
from pyreach import snapshot as lib_snapshot
3132
from pyreach.gyms import arm_element
@@ -71,6 +72,10 @@ def __init__(self, arm_config: arm_element.ReachArm) -> None:
7172
ik_lib: Optional[str] = arm_config.ik_lib
7273
e_stop_mode: int = arm_config.e_stop_mode
7374
p_stop_mode: int = arm_config.p_stop_mode
75+
# For unit testing only.
76+
test_states: Optional[List[pyreach_arm.ArmState]] = arm_config.test_states
77+
if not test_states:
78+
test_states = []
7479

7580
if not controllers:
7681
raise pyreach.PyReachError("At least one controller must be specified")

‎pyreach/gyms/devices/oracle_device.py‎

Lines changed: 5 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -183,6 +183,7 @@ def _get_latest_prediction(self, host: pyreach.Host) -> pyreach.Prediction:
183183
raise pyreach.PyReachError(
184184
"Internal Error: Oracle tagged request not set")
185185

186+
logging.debug(">>>>>>>>>>>>>>>>Requesting 'prediction'")
186187
with self._timers.select({"!agent*", "!gym*", "host.oracle"}):
187188
prediction = oracle.fetch_prediction(*tagged_request, timeout=30.0)
188189
if prediction:
@@ -367,12 +368,10 @@ def do_action(
367368
self._request = request
368369
self._enable_tagged_request(label)
369370

370-
prediction: Optional[pyreach.Prediction] = self._prediction
371-
if not prediction:
372-
try:
373-
prediction = self._get_latest_prediction(host)
374-
except pyreach.PyReachError as reach_error:
375-
raise pyreach.PyReachError from reach_error
371+
try:
372+
prediction = self._get_latest_prediction(host)
373+
except pyreach.PyReachError as reach_error:
374+
raise pyreach.PyReachError from reach_error
376375
assert prediction
377376

378377
self._selected_point = None

‎pyreach/gyms/devices/text_instructions_device.py‎

Lines changed: 21 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -15,7 +15,7 @@
1515
"""Implementation of PyReach Gym Vacuum Device."""
1616

1717
import sys
18-
from typing import Any, Dict, List, Optional, Tuple
18+
from typing import Any, Dict, Optional, Tuple
1919

2020
import gym # type: ignore
2121
import numpy as np # type: ignore
@@ -27,6 +27,22 @@
2727
from pyreach.gyms.devices import reach_device
2828

2929

30+
def string_to_bytes(string_val: str, bytes_len: int = 1024) -> np.ndarray:
31+
"""String to bytes array (numpy) used when encoding instruction strings.
32+
33+
Args:
34+
string_val: input string. Typically ["text_instruction"]["instructions"].
35+
bytes_len: Returned array length. Will truncate strings that are too long
36+
and pad strings that are too short.
37+
Returns:
38+
Numpy array of length bytes_len encoding the string as bytes.
39+
"""
40+
bytes_val = list(bytes(string_val, "utf-8"))[:bytes_len]
41+
pad_size: int = bytes_len - len(bytes_val)
42+
bytes_val.extend(pad_size * [0])
43+
return np.array(bytes_val)
44+
45+
3046
class ReachDeviceTextInstructions(reach_device.ReachDevice):
3147
"""Represents some text instructions.
3248
@@ -108,7 +124,8 @@ def get_observation(self,
108124
device.fetch_text_instruction()
109125
if self._is_synchronous else device.text_instruction)
110126

111-
instruction_bytes: List[int] = 1024 * [0]
127+
bytes_len = 1024
128+
instruction_bytes: np.ndarray = np.zeros([bytes_len], dtype=np.int64)
112129
ts: float = 0.0
113130
seq: int = 0
114131
counter: int = self._counter
@@ -126,14 +143,12 @@ def get_observation(self,
126143
self._counter = counter
127144

128145
instruction: str = current_text_instruction.instruction
129-
instruction_bytes = list(bytes(instruction, "utf-8"))[:1024]
130-
pad_size: int = 1024 - len(instruction_bytes)
131-
instruction_bytes.extend(pad_size * [0])
146+
instruction_bytes = string_to_bytes(instruction, bytes_len)
132147

133148
observation: gyms_core.Observation = {
134149
"counter": counter,
135150
"ts": ts,
136-
"instruction": np.array(instruction_bytes),
151+
"instruction": instruction_bytes,
137152
}
138153
return observation, (), (lib_snapshot.SnapshotResponse(
139154
counter, "text-instruction", self.config_name,

‎pyreach/gyms/envs/benchmark_kitting.py‎

Lines changed: 27 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -20,11 +20,14 @@
2020

2121
from pyreach.gyms import core
2222
from pyreach.gyms import reach_env
23+
from pyreach.gyms.devices.text_instructions_device import ReachDeviceTextInstructions
2324

2425
KITTING = 3
2526
DEKITTING = 4
2627
UNSET = 0
2728

29+
INSTRUCTION_TIMEOUT_SEC = 20
30+
2831

2932
class KittingBenchmarkEnv(reach_env.ReachEnv): # type: ignore
3033
"""Benchmark environment for the Kitting benchmark.
@@ -134,10 +137,8 @@ def ask_for_new_instruction(
134137

135138
_, _, _, _ = super().step(action)
136139

137-
# Start new task
138-
action = collections.OrderedDict(
139-
{"text_instructions": collections.OrderedDict({"task_enable": 1})})
140-
return super().step(action)
140+
# Start a new task
141+
return self._start_new_task()
141142

142143
def reset(self) -> core.Observation:
143144
"""Resets the benchmark.
@@ -153,14 +154,32 @@ def reset(self) -> core.Observation:
153154
action = collections.OrderedDict(
154155
{"text_instructions": collections.OrderedDict({"task_enable": 0})})
155156
_, _, _, _ = super().step(action)
156-
obs = super().reset()
157+
super().reset()
157158

158159
# Start a new task
160+
obs, _, _, _ = self._start_new_task()
161+
return obs
162+
163+
def _start_new_task(self) -> Tuple[core.Observation, float, bool, Any]:
164+
text_instr_device = self._elements["text_instructions"]
165+
assert isinstance(text_instr_device, ReachDeviceTextInstructions)
166+
instr_obs, _, _ = text_instr_device.get_observation(self._host)
167+
assert isinstance(instr_obs, dict) and "counter" in instr_obs
168+
start_instr_counter = instr_obs["counter"]
159169
action = collections.OrderedDict(
160170
{"text_instructions": collections.OrderedDict({"task_enable": 1})})
161-
obs, _, _, _ = super().step(action)
162-
163-
return obs
171+
start_time = time.time()
172+
while True:
173+
(obs, reward, done, info) = super().step(action)
174+
assert isinstance(obs, dict)
175+
text_instructions = obs["text_instructions"]
176+
assert isinstance(text_instructions, dict)
177+
if text_instructions["counter"] != start_instr_counter:
178+
break
179+
if time.time() - start_time > INSTRUCTION_TIMEOUT_SEC:
180+
raise ValueError("Timeout waiting for valid instruction.")
181+
182+
return obs, reward, done, info
164183

165184

166185
class BenchmarkKittingWrapper(gym.Wrapper):

‎pyreach/gyms/experiment_assigner_test.py‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -57,7 +57,7 @@ class FakeReachEnv(reach_env.ReachEnv):
5757
def __init__(self, **kwargs: Any) -> None:
5858
pyreach_config: Dict[str, reach_env.ReachElement] = {}
5959

60-
mock_host: host.Host = host_mock.HostMock()
60+
mock_host: host.Host = host_mock.HostMock(pyreach_config)
6161
assert isinstance(mock_host, host.Host)
6262
assert isinstance(mock_host, host_mock.HostMock)
6363
super().__init__(pyreach_config=pyreach_config, host=mock_host, **kwargs)

0 commit comments

Comments
 (0)