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

Commit 5eb3f1d

Browse files
twahrburg-googletwahrburg-google
committed
Pyreach sync 20211123
1 parent 0f5bc1b commit 5eb3f1d

File tree

15 files changed

+663
-41
lines changed

15 files changed

+663
-41
lines changed

‎pyreach/__init__.py‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -40,6 +40,7 @@
4040
from pyreach.oracle import Prediction
4141
from pyreach.oracle import PredictionPickPlacePoint
4242
from pyreach.oracle import PredictionPoint
43+
from pyreach.sim import Sim
4344
from pyreach.text_instruction import TextInstruction
4445
from pyreach.text_instruction import TextInstructions
4546
from pyreach.vacuum import Vacuum

‎pyreach/common/python/types_gen.py‎

Lines changed: 8 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -8207,16 +8207,16 @@ class Measurement:
82078207

82088208
Measurement is a value for a measurement, with units.
82098209
"""
8210-
seconds: float
8210+
seconds: Optional[float]
82118211

8212-
def __init__(self, seconds: float = 0.0) -> None:
8212+
def __init__(self, seconds: Optional[float] = None) -> None:
82138213
self.seconds = seconds
82148214

82158215
def to_json(self) -> Dict[str, Any]:
82168216
"""Convert type object to JSON."""
82178217
json_data: Dict[str, Any] = dict()
82188218

8219-
if self.seconds:
8219+
if self.seconds is not None:
82208220
assert isinstance(self.seconds, float) or isinstance(self.seconds, int), 'Wrong type for attribute: seconds. Expected: float. Got: ' + str(type(self.seconds)) + '.'
82218221
json_data['seconds'] = self.seconds
82228222

@@ -8225,7 +8225,7 @@ def to_json(self) -> Dict[str, Any]:
82258225
def to_proto(self) -> 'logs_pb2.Measurement':
82268226
"""Convert Measurement to proto."""
82278227
proto = logs_pb2.Measurement()
8228-
if self.seconds:
8228+
if self.seconds is not None:
82298229
proto.seconds = self.seconds
82308230
return proto
82318231

@@ -10902,17 +10902,17 @@ class ReachScriptConst:
1090210902
"""
1090310903
# The value of all pins in a capability.
1090410904
capability: Optional['ReachScriptCapability']
10905-
bool_value: bool
10905+
bool_value: Optional[bool]
1090610906

10907-
def __init__(self, bool_value: bool = False, capability: Optional['ReachScriptCapability'] = None) -> None:
10907+
def __init__(self, bool_value: Optional[bool] = None, capability: Optional['ReachScriptCapability'] = None) -> None:
1090810908
self.bool_value = bool_value
1090910909
self.capability = capability
1091010910

1091110911
def to_json(self) -> Dict[str, Any]:
1091210912
"""Convert type object to JSON."""
1091310913
json_data: Dict[str, Any] = dict()
1091410914

10915-
if self.bool_value:
10915+
if self.bool_value is not None:
1091610916
assert isinstance(self.bool_value, bool), 'Wrong type for attribute: bool_value. Expected: bool. Got: ' + str(type(self.bool_value)) + '.'
1091710917
json_data['boolValue'] = self.bool_value
1091810918

@@ -10927,7 +10927,7 @@ def to_proto(self) -> 'logs_pb2.ReachScriptConst':
1092710927
proto = logs_pb2.ReachScriptConst()
1092810928
if self.capability:
1092910929
proto.capability.CopyFrom(self.capability.to_proto())
10930-
if self.bool_value:
10930+
if self.bool_value is not None:
1093110931
proto.bool_value = self.bool_value
1093210932
return proto
1093310933

‎pyreach/gyms/envs/benchmark_2d_long_horizon_instrs.txt‎

Lines changed: 100 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,100 @@
1+
Put all the objects in a line
2+
Put all the objects in a vertical line
3+
Put all the objects in a vertical line on the left side of the board.
4+
Put all the objects in a vertical line on the right side of the board.
5+
Put everything in a horizontal line
6+
Put everything in a horizontal line near the bottom of the board.
7+
Put everything in a horizontal line near the top of the board.
8+
Arrange objects in a diagonal line.
9+
Arrange the objects in a diagonal line from the top left corner to the bottom right corner.
10+
Arrange the objects in a diagonal line from the top right corner to the bottom left corner.
11+
Separate the objects by color.
12+
Separate the objects into two clumps: one clump of objects that has flat edges, and one clump of objects that has no flat edges.
13+
Surround all of the rest of the objects around the yellow hexagon.
14+
Surround all of the rest of the objects around the yellow heart
15+
Surround all of the rest of the objects around the blue square
16+
Surround all of the rest of the objects around the blue semi-circle
17+
Surround all of the rest of the objects around the green circle
18+
Surround all of the rest of the objects around the green star
19+
Surround all of the rest of the objects around the red circle
20+
Surround all of the rest of the objects around the red star
21+
Put the objects from left to right in the following order: the yellow blocks, then the blue blocks, then the red blocks, then the green blocks.
22+
Put the objects from left to right in the following order: the yellow blocks, then the blue blocks, then the green blocks, then the red blocks.
23+
Put the objects from left to right in the following order: the yellow blocks, then the green blocks, then the red blocks, then the blue blocks.
24+
Put the objects from left to right in the following order: the yellow blocks, then the green blocks, then the blue blocks, then the red blocks.
25+
Put the objects from left to right in the following order: the yellow blocks, then the red blocks, then the blue blocks, then the green blocks.
26+
Put the objects from left to right in the following order: the yellow blocks, then the red blocks, then the green blocks, then the blue blocks.
27+
Put the objects from left to right in the following order: the blue blocks, then the yellow blocks, then the red blocks, then the green blocks.
28+
Put the objects from left to right in the following order: the blue blocks, then the yellow blocks, then the green blocks, then the red blocks.
29+
Put the objects from left to right in the following order: the blue blocks, then the green blocks, then the red blocks, then the yellow blocks.
30+
Put the objects from left to right in the following order: the blue blocks, then the green blocks, then the yellow blocks, then the red blocks.
31+
Put the objects from left to right in the following order: the blue blocks, then the red blocks, then the yellow blocks, then the green blocks.
32+
Put the objects from left to right in the following order: the blue blocks, then the red blocks, then the green blocks, then the yellow blocks.
33+
Put the objects from left to right in the following order: the red blocks, then the yellow blocks, then the blue blocks, then the green blocks.
34+
Put the objects from left to right in the following order: the red blocks, then the yellow blocks, then the green blocks, then the blue blocks.
35+
Put the objects from left to right in the following order: the red blocks, then the green blocks, then the blue blocks, then the yellow blocks.
36+
Put the objects from left to right in the following order: the red blocks, then the green blocks, then the yellow blocks, then the blue blocks.
37+
Put the objects from left to right in the following order: the red blocks, then the blue blocks, then the yellow blocks, then the green blocks.
38+
Put the objects from left to right in the following order: the red blocks, then the blue blocks, then the green blocks, then the yellow blocks.
39+
Put the objects from left to right in the following order: the green blocks, then the blue blocks, then the yellow blocks, then the red blocks.
40+
Put the objects from left to right in the following order: the green blocks, then the blue blocks, then the red blocks, then the yellow blocks.
41+
Put the blue objects in the center, and put the rest of the objects in the top left corner.
42+
Put the green objects in the center, and put the rest of the objects in the top left corner.
43+
Put the red objects in the center, and put the rest of the objects in the top left corner.
44+
Put the yellow objects in the center, and put the rest of the objects in the top left corner.
45+
Put the blue objects in the center, and put the rest of the objects in the top right corner.
46+
Put the green objects in the center, and put the rest of the objects in the top right corner.
47+
Put the red objects in the center, and put the rest of the objects in the top right corner.
48+
Put the yellow objects in the center, and put the rest of the objects in the top right corner.
49+
Put the blue objects in the center, and put the rest of the objects in the bottom left corner.
50+
Put the green objects in the center, and put the rest of the objects in the bottom left corner.
51+
Put the red objects in the center, and put the rest of the objects in the bottom left corner.
52+
Put the yellow objects in the center, and put the rest of the objects in the bottom left corner.
53+
Put the blue objects in the center, and put the rest of the objects in the bottom right corner.
54+
Put the green objects in the center, and put the rest of the objects in the bottom right corner.
55+
Put the red objects in the center, and put the rest of the objects in the bottom right corner.
56+
Put the yellow objects in the center, and put the rest of the objects in the bottom right corner.
57+
On the left side of the scene, make a vertical line with alternating green and red blocks. On the right side of the scene, make a vertical line with alternating blue and yellow blocks.
58+
On the left side of the scene, make a vertical line with alternating yellow and red blocks. On the right side of the scene, make a vertical line with alternating green and blue blocks.
59+
On the left side of the scene, make a vertical line with alternating yellow and green blocks. On the right side of the scene, make a vertical line with alternating red and blue blocks.
60+
On the left side of the scene, make a vertical line with alternating blue and red blocks. On the right side of the scene, make a vertical line with alternating green and yellow blocks.
61+
On the left side of the scene, make a vertical line with alternating blue and green blocks. On the right side of the scene, make a vertical line with alternating red and yellow blocks.
62+
On the left side of the scene, make a vertical line with alternating blue and yellow blocks. On the right side of the scene, make a vertical line with alternating red and green blocks.
63+
Order the blocks from left to right in increasing order of how many "sides" they have. The circle is 1. For example, the square is 4. The semi-circle and heart have 2.
64+
Order the blocks from right to left in increasing order of how many "sides" they have. The circle is 1. For example, the square is 4. The semi-circle and heart have 2.
65+
Order the blocks from top to bottom in increasing order of how many "sides" they have. The circle is 1. For example, the square is 4. The semi-circle and heart have 2.
66+
Order the blocks from bottom to top in increasing order of how many "sides" they have. The circle is 1. For example, the square is 4. The semi-circle and heart have 2.
67+
Put all the objects in the bottom right corner
68+
Put all the objects in the bottom left corner
69+
Put all the objects in the top right corner
70+
Put all the objects in the top left corner
71+
Put the two stars together in the center of the table, and the rest of the objects in the bottom left
72+
Put the two stars together in the center of the table, and the rest of the objects in the bottom right
73+
Put the two stars together in the center of the table, and the rest of the objects in the top left
74+
Put the two stars together in the center of the table, and the rest of the objects in the top right
75+
Put the two circles together in the center of the table, and the rest of the objects in the bottom left
76+
Put the two circles together in the center of the table, and the rest of the objects in the bottom right
77+
Put the two circles together in the center of the table, and the rest of the objects in the top left
78+
Put the two circles together in the center of the table, and the rest of the objects in the top right
79+
Put the two blue blocks together, and put the stars on either side of the blue blocks.
80+
Put the two blue blocks together, and put the circles on either side of the blue blocks.
81+
Put the two yellow blocks together, and put the stars on either side of the yellow blocks.
82+
Put the two yellow blocks together, and put the circles on either side of the yellow blocks.
83+
Order the shapes from left to right in alphabetical order: circle (x2), heart, hexagon, semi-circle, square, star (x2)
84+
Order the shapes from right to left in alphabetical order: circle (x2), heart, hexagon, semi-circle, square, star (x2)
85+
Order the shapes from top to bottom in alphabetical order: circle (x2), heart, hexagon, semi-circle, square, star (x2)
86+
Order the shapes from bottom to top in alphabetical order: circle (x2), heart, hexagon, semi-circle, square, star (x2)
87+
Put all the blocks in a big circle.
88+
Put all the blocks in a big rectangle.
89+
Put all the blocks in a big square.
90+
Arrange the blocks into a big "X" shape.
91+
Arrange the blocks into a big "+" (plus sign) shape.
92+
Arrange the blocks into a big "T" shape.
93+
Arrange the blocks into a "rainbow" (red then yellow then green then blue) in a semi-circle
94+
Make a smiley face out of the blocks, with green eyes.
95+
Make a smiley face out of the blocks, with yellow eyes.
96+
Make a smiley face out of the blocks, with red eyes.
97+
Make a smiley face out of the blocks, with blue eyes.
98+
Make a "G" shape out of the blocks
99+
Make a "4" shape out of the blocks
100+
Spell "Google" with the blocks as best you can

‎pyreach/gyms/envs/integration_fanuc.py‎

Lines changed: 155 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,155 @@
1+
# Copyright 2021 Google LLC
2+
#
3+
# Licensed under the Apache License, Version 2.0 (the "License");
4+
# you may not use this file except in compliance with the License.
5+
# You may obtain a copy of the License at
6+
#
7+
# http://www.apache.org/licenses/LICENSE-2.0
8+
#
9+
# Unless required by applicable law or agreed to in writing, software
10+
# distributed under the License is distributed on an "AS IS" BASIS,
11+
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
# See the License for the specific language governing permissions and
13+
# limitations under the License.
14+
15+
"""Gym environment for the Fanuc Integration Test Workcell."""
16+
17+
import time
18+
from typing import Any, Dict, Tuple
19+
20+
import numpy as np # type: ignore
21+
22+
from pyreach.gyms import core
23+
from pyreach.gyms import reach_env
24+
25+
26+
class IntegrationTestFanucEnv(reach_env.ReachEnv):
27+
"""Integration Test for Fanuc Environment."""
28+
SAFE_JOINT_ANGLES: np.ndarray = np.deg2rad([0, 0, -35, 0, -60, -75])
29+
30+
# pylint: disable=g-bad-todo
31+
# TODO: Grab MAX and MIN joint angles from real workcell
32+
MIN_JOINT_ANGLES = SAFE_JOINT_ANGLES - 5.0
33+
MAX_JOINT_ANGLES = SAFE_JOINT_ANGLES + 5.0
34+
35+
TIMEOUT_PER_INSTRUCTION_SECONDS = 600.0
36+
37+
def __init__(self, is_synchronous: bool = True, **kwargs: Any) -> None:
38+
self.timer_running: bool = False
39+
self.deadline: float = 0.0
40+
self.agent_done_signal = False
41+
response_queue_length: int = 0 if is_synchronous else 2
42+
43+
pyreach_config: Dict[str, reach_env.ReachElement] = {
44+
"arm":
45+
reach_env.ReachArm(
46+
"",
47+
self.MIN_JOINT_ANGLES,
48+
self.MAX_JOINT_ANGLES,
49+
is_synchronous=is_synchronous,
50+
response_queue_length=response_queue_length,
51+
ik_lib="ikfast"),
52+
"camera":
53+
reach_env.ReachColorCamera("realsense", (360, 640)),
54+
"depth_camera":
55+
reach_env.ReachDepthCamera("", (720, 1280), color_enabled=True),
56+
"server":
57+
reach_env.ReachServer("Server"),
58+
"vacuum":
59+
reach_env.ReachVacuum("",
60+
is_synchronous=is_synchronous,
61+
state_enable=True,
62+
vacuum_gauge_enable=True,
63+
vacuum_detect_enable=True),
64+
"annotation":
65+
reach_env.ReachAnnotation("",
66+
is_synchronous=False,
67+
maximum_size=1024),
68+
}
69+
70+
super().__init__(pyreach_config=pyreach_config, **kwargs)
71+
72+
def _movej(self,
73+
joints: np.ndarray) -> Tuple[core.Observation, float, bool, Any]:
74+
action = {
75+
"arm": {
76+
"command": 1,
77+
"joint_angles": joints,
78+
"synchronous": 1,
79+
"velocity": 1.05,
80+
"acceleration": 1.4
81+
}
82+
}
83+
return super().step(action)
84+
85+
def _stow_workcell(self) -> core.Observation:
86+
"""Stow the workcell arm clear of the FOV of the camera.
87+
88+
This will move the workcell arm to a safe joint position.
89+
90+
Returns:
91+
Observation after moving arm to safe joint position
92+
"""
93+
print(f"{time.time():.4f}:ENV: Stowing workcell arm")
94+
95+
obs, _, _, _ = self._movej(self.SAFE_JOINT_ANGLES)
96+
97+
return obs
98+
99+
def reset(self) -> core.Observation:
100+
"""Resets the benchmark.
101+
102+
Returns:
103+
Initial observation.
104+
"""
105+
print(f"{time.time():.4f}:ENV: Resetting the integration test environment")
106+
107+
# End any current task with reset
108+
obs = super().reset()
109+
110+
return obs
111+
112+
def close(self) -> None:
113+
self._stow_workcell()
114+
print(f"{time.time():.4f}:ENV: Closing gym")
115+
super().close()
116+
117+
def step(self,
118+
action: core.Action) -> Tuple[core.Observation, float, bool, Any]:
119+
"""Perform one step."""
120+
observation: core.Observation
121+
reward: float
122+
done: bool
123+
info: Any
124+
125+
observation, reward, done, info = super().step(action)
126+
127+
if not self.timer_running:
128+
self.timer_running = True
129+
self.deadline = time.time() + self.TIMEOUT_PER_INSTRUCTION_SECONDS
130+
131+
if done:
132+
self.timer_running = False
133+
observation = self._stow_workcell()
134+
elif time.time() >= self.deadline:
135+
print(f"{time.time():.4f}:ENV: You ran out of time!")
136+
self.timer_running = False
137+
reward = -1.0
138+
done = True
139+
observation = self._stow_workcell()
140+
141+
return (observation, reward, done, info)
142+
143+
144+
class IntegrationTestFanucSyncEnv(IntegrationTestFanucEnv):
145+
"""Configure a Gym environment with a synchronous arm."""
146+
147+
def __init__(self, **kwargs: Any) -> None:
148+
super().__init__(is_synchronous=True, **kwargs)
149+
150+
151+
class IntegrationTestFanucAsyncEnv(IntegrationTestFanucEnv):
152+
"""Configure a Gym environment with an asynchronous arm."""
153+
154+
def __init__(self, **kwargs: Any) -> None:
155+
super().__init__(is_synchronous=False, **kwargs)

‎pyreach/host.py‎

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -34,6 +34,7 @@
3434
from pyreach import metrics
3535
from pyreach import oracle
3636
from pyreach import playback
37+
from pyreach import sim
3738
from pyreach import text_instruction
3839
from pyreach import vacuum
3940
from pyreach import vnc
@@ -248,6 +249,11 @@ def text_instructions(self) -> text_instruction.TextInstructions:
248249
"""Return the TextInstructions object."""
249250
raise NotImplementedError
250251

252+
@property
253+
def sim(self) -> Optional[sim.Sim]:
254+
"""Access sim object."""
255+
raise NotImplementedError
256+
251257
def get_ping_time(self) -> Optional[float]:
252258
"""Return the latest ping time.
253259

‎pyreach/impl/arm_impl.py‎

Lines changed: 0 additions & 25 deletions
Original file line numberDiff line numberDiff line change
@@ -1148,27 +1148,6 @@ def device_name(self) -> str:
11481148
"""Return the device name."""
11491149
return self._device_name
11501150

1151-
def reset_sim(self,
1152-
callback: Optional[Callable[[core.PyReachStatus], None]] = None,
1153-
finished_callback: Optional[Callable[[], None]] = None,
1154-
timeout: Optional[float] = None) -> None:
1155-
"""Reset the simulator if it is present.
1156-
1157-
Args:
1158-
callback: An optional callback called when new status is available.
1159-
finished_callback: A n optional function that is called when finished.
1160-
timeout: The time in seconds to wait for the simulator reset.
1161-
"""
1162-
tag = utils.generate_tag()
1163-
script = types_gen.CommandData(
1164-
ts=utils.timestamp_now(),
1165-
device_type="script-engine",
1166-
data_type="run-script",
1167-
tag=tag,
1168-
script="sim reset")
1169-
_ = self.send_tagged_request(script)
1170-
return None
1171-
11721151
def fk(self,
11731152
joints: Union[Tuple[float, ...], List[float], np.ndarray],
11741153
apply_tip_adjust_transform: bool = False) -> Optional[core.Pose]:
@@ -1424,10 +1403,6 @@ def supported_controllers(
14241403
"""The supported controllers, or None if not loaded."""
14251404
return self._device.supported_controllers
14261405

1427-
def reset_sim(self) -> None:
1428-
"""Send reset sim command."""
1429-
self._device.reset_sim()
1430-
14311406
@property
14321407
def device_name(self) -> str:
14331408
"""Return the Arm name."""

0 commit comments

Comments
 (0)