diff --git a/demo/关节控制.py b/demo/关节控制.py index d6a036f..fbbff16 100644 --- a/demo/关节控制.py +++ b/demo/关节控制.py @@ -4,9 +4,6 @@ import sys import time import grpc -sys.path.append('./') -sys.path.append('../') - import matplotlib.pyplot as plt import numpy as np from mpl_toolkits.axes_grid1 import make_axes_locatable diff --git a/demo/动画控制.py b/demo/动画控制.py index 8ee42cb..457bbbc 100644 --- a/demo/动画控制.py +++ b/demo/动画控制.py @@ -5,9 +5,6 @@ import sys import time import grpc -sys.path.append('./') -sys.path.append('../') - import matplotlib.pyplot as plt import numpy as np from mpl_toolkits.axes_grid1 import make_axes_locatable diff --git a/demo/场景操作.py b/demo/场景操作.py index 6f436e0..cc48eb7 100644 --- a/demo/场景操作.py +++ b/demo/场景操作.py @@ -4,9 +4,6 @@ import sys import time import grpc -sys.path.append('./') -sys.path.append('../') - import matplotlib.pyplot as plt import numpy as np from mpl_toolkits.axes_grid1 import make_axes_locatable diff --git a/demo/导航寻路.py b/demo/导航寻路.py index 5f4fe28..379022f 100644 --- a/demo/导航寻路.py +++ b/demo/导航寻路.py @@ -4,9 +4,6 @@ import sys import time import grpc -sys.path.append('./') -sys.path.append('../') - import matplotlib.pyplot as plt import numpy as np from mpl_toolkits.axes_grid1 import make_axes_locatable diff --git a/demo/文字冒泡.py b/demo/文字冒泡.py index 78bb155..370ffb8 100644 --- a/demo/文字冒泡.py +++ b/demo/文字冒泡.py @@ -5,9 +5,6 @@ import sys import time import grpc -sys.path.append('./') -sys.path.append('../') - import matplotlib.pyplot as plt import numpy as np from mpl_toolkits.axes_grid1 import make_axes_locatable diff --git a/demo/物品操作.py b/demo/物品操作.py index 151e26a..f63a4c7 100644 --- a/demo/物品操作.py +++ b/demo/物品操作.py @@ -4,9 +4,6 @@ import sys import time import grpc -sys.path.append('./') -sys.path.append('../') - import matplotlib.pyplot as plt import numpy as np from mpl_toolkits.axes_grid1 import make_axes_locatable diff --git a/demo/相机操作.py b/demo/相机操作.py index 8389a1a..42e400c 100644 --- a/demo/相机操作.py +++ b/demo/相机操作.py @@ -4,9 +4,6 @@ import sys import time import grpc -sys.path.append('./') -sys.path.append('../') - import matplotlib.pyplot as plt import numpy as np from mpl_toolkits.axes_grid1 import make_axes_locatable diff --git a/demo/行人控制.py b/demo/行人控制.py index 2b5e7da..32efcb2 100644 --- a/demo/行人控制.py +++ b/demo/行人控制.py @@ -4,9 +4,6 @@ import sys import time import grpc -sys.path.append('./') -sys.path.append('../') - import matplotlib.pyplot as plt import numpy as np from mpl_toolkits.axes_grid1 import make_axes_locatable @@ -72,7 +69,9 @@ def clean_walkers(scene_id=0): def walker_test(scene_id=0): add_walker(scene_id) control_walkers(scene_id) + print(sim_client.Observe(GrabSim_pb2.SceneID(value=scene_id)).walkers) remove_walkers(scene_id) + print(sim_client.Observe(GrabSim_pb2.SceneID(value=scene_id)).walkers) clean_walkers(scene_id) return diff --git a/proto/__init__.py b/proto/__init__.py index e69de29..3cca4fd 100644 --- a/proto/__init__.py +++ b/proto/__init__.py @@ -0,0 +1,2 @@ +from . import GrabSim_pb2 +from . import GrabSim_pb2_grpc diff --git a/scene_utils/__init__.py b/scene_utils/__init__.py new file mode 100644 index 0000000..d22ac13 --- /dev/null +++ b/scene_utils/__init__.py @@ -0,0 +1 @@ +from . import control diff --git a/scene_utils/control.py b/scene_utils/control.py new file mode 100644 index 0000000..e27f34c --- /dev/null +++ b/scene_utils/control.py @@ -0,0 +1,128 @@ +import gym +import grpc +from proto import GrabSim_pb2 +from proto import GrabSim_pb2_grpc + +channel = grpc.insecure_channel( + "localhost:30001", + options=[ + ("grpc.max_send_message_length", 1024 * 1024 * 1024), + ("grpc.max_receive_message_length", 1024 * 1024 * 1024), + ], +) +stub = GrabSim_pb2_grpc.GrabSimStub(channel) + + +def init_world(scene_num, mapID): + stub.SetWorld(GrabSim_pb2.BatchMap(count=scene_num, mapID=mapID)) + + +class Scene: + """ + status: + location: Dict[X: float, Y: float] + rotation: Dict[Yaw: float] + joints: List[Dict[name: str, location: Dict[X: float, Y: float, Z: float]]] + fingers: List[Dict[name: str, location: List[3 * Dict[X: float, Y: float, Z: float]]]] + objects[:-1]: List[Dict[name: str, location: Dict[X: float, Y: float, Z: float]]] + objects[-1]: Dict[name: "Hand", boxes: List[Dict[diagonals: List[4 * Dict[X0: float, Y0: float, Z0: float, X1: float, Y1: float, Z1: float]]]]] + walkers: List[name: str, pose: Dict[X: float, Y: float, Yaw: float], speed: float, target: Dict[X: float, Y: float, Yaw: float]] + timestamp: int, timestep: int + collision: str, info: str + """ + + def __init__(self, sceneID): + self.sceneID = sceneID + self.walkerID_set = set() + self.reset() + + @property + def status(self): + return stub.Observe(GrabSim_pb2.SceneID(value=self.sceneID)) + + def reset(self): + stub.Reset(GrabSim_pb2.ResetParams(scene=self.sceneID)) + + def walk_to(self, X, Y, Yaw, velocity, dis_limit): + stub.Do( + GrabSim_pb2.Action( + scene=self.sceneID, + action=GrabSim_pb2.Action.ActionType.WalkTo, + values=[X, Y, Yaw, velocity, dis_limit], + ) + ) + + def reachable_check(self, X, Y, Yaw): + navigation_info = stub.Do( + GrabSim_pb2.Action( + scene=self.sceneID, + action=GrabSim_pb2.Action.ActionType.WalkTo, + values=[X, Y, Yaw], + ) + ).info + if navigation_info == "Unreachable": + return False + else: + return True + + def add_walker(self, X, Y, Yaw): + if self.reachable_check(X, Y, Yaw): + walkerID = 0 if not self.walkerID_set else max(self.walkerID_set) + 1 + stub.AddWalker( + GrabSim_pb2.WalkerList( + walkers=[ + GrabSim_pb2.WalkerList.Walker( + id=walkerID, pose=GrabSim_pb2.Pose(X=X, Y=Y, Yaw=Yaw) + ) + ], + scene=self.sceneID, + ) + ) + self.walkerID_set.add(walkerID) + return walkerID + else: + return None + + def remove_walker(self, *args): + remove_list = [] + for walkerID in args: + if walkerID in self.walkerID_set: + remove_list.append(walkerID) + self.walkerID_set.remove(walkerID) + stub.RemoveWalkers(GrabSim_pb2.RemoveList(IDs=remove_list, scene=self.sceneID)) + + def clean_walker(self): + stub.CleanWalkers(GrabSim_pb2.SceneID(value=self.sceneID)) + + def walker_control_generator(self, walkerID, autowalk, speed, X, Y, Yaw): + return GrabSim_pb2.WalkerControls.WControl( + id=walkerID, + autowalk=autowalk, + speed=speed, + pose=GrabSim_pb2.Pose(X=X, Y=Y, Yaw=Yaw), + ) + + def control_walker(self, control_list): + stub.ControlWalkers( + GrabSim_pb2.WalkerControls(controls=control_list, scene=self.sceneID) + ) + + def control_joints(self, angles): + stub.Do( + GrabSim_pb2.Action( + scene=self.sceneID, + action=GrabSim_pb2.Action.ActionType.RotateJoints, + values=angles, + ) + ) + + def add_object(self, X, Y, Yaw, Z, type): + stub.AddObjects( + GrabSim_pb2.ObjectList( + objects=[ + GrabSim_pb2.ObjectList.Object(x=X, y=Y, yaw=Yaw, z=Z, type=type) + ], + scene=self.sceneID, + ) + ) +