EmbodiedAI
/
RoboWaiter
mirror of https://github.com/HPCL-EI/RoboWaiter.git
3
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Update scene.py

This commit is contained in:
Netceor 2023-11-17 16:12:34 +08:00
parent e71b1c217f
commit 5e1801986c
1 changed files with 6 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
robowaiter/scene/scene.py
View File

@ -464,7 +464,6 @@ class Scene:
walk_v = release_pos[:-1] + [180, 180, 0] walk_v = release_pos[:-1] + [180, 180, 0]
if release_pos == [340.0, 900.0, 99.0]: if release_pos == [340.0, 900.0, 99.0]:
walk_v[2] = 130 walk_v[2] = 130
print("walk_v:",walk_v)
action = GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.WalkTo, values=walk_v) action = GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.WalkTo, values=walk_v)
scene = stub.Do(action) scene = stub.Do(action)
print("After Walk Position:", [scene.location.X, scene.location.Y, scene.rotation.Yaw]) print("After Walk Position:", [scene.location.X, scene.location.Y, scene.rotation.Yaw])
@ -513,7 +512,9 @@ class Scene:
obj_info = scene.objects[obj_id] obj_info = scene.objects[obj_id]
obj_x, obj_y, obj_z = obj_info.location.X, obj_info.location.Y, obj_info.location.Z obj_x, obj_y, obj_z = obj_info.location.X, obj_info.location.Y, obj_info.location.Z
if obj_info.name=="CoffeeCup": if obj_info.name=="CoffeeCup":
values = [0,0,0,0,0, -15,0,0,0,0] # obj_x += 1
# obj_y -= 1
# values = [0,0,0,0,0, 10,-25,-45,-45,-45]
# values= [-6, 0, 0, 0, 0, -6, 0, 45, 45, 45] # values= [-6, 0, 0, 0, 0, -6, 0, 45, 45, 45]
stub.Do(GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.Finger, values=values)) stub.Do(GrabSim_pb2.Action(scene=self.sceneID, action=GrabSim_pb2.Action.ActionType.Finger, values=values))
pass pass
@ -545,7 +546,7 @@ class Scene:
# 0-3是躯干，4-6是脖子和头，7-13是左胳膊，14-20是右胳膊 # 0-3是躯干，4-6是脖子和头，7-13是左胳膊，14-20是右胳膊
scene = self.status scene = self.status
angle = [scene.joints[i].angle for i in range(21)] angle = [scene.joints[i].angle for i in range(21)]
angle[0] = 15 angle[0] = 15 # 15
angle[19] = -15 angle[19] = -15
angle[20] = -30 angle[20] = -30
action = GrabSim_pb2.Action(scene=self.sceneID,action=GrabSim_pb2.Action.ActionType.RotateJoints, # 弯腰 action = GrabSim_pb2.Action(scene=self.sceneID,action=GrabSim_pb2.Action.ActionType.RotateJoints, # 弯腰
@ -556,7 +557,7 @@ class Scene:
def release_obj(self,release_pos): def release_obj(self,release_pos):
print("------------------release_obj----------------------") print("------------------release_obj----------------------")
if release_pos==[340.0, 900.0, 99.0]: if release_pos==[340.0, 900.0, 99.0]:
self.ik_control_joints(2, 300.0, 935, release_pos[2]) self.ik_control_joints(2, release_pos[0]-40, release_pos[1]+35, release_pos[2])
time.sleep(2.0) time.sleep(2.0)
else: else:
self.ik_control_joints(2, release_pos[0] - 80, release_pos[1], release_pos[2]) self.ik_control_joints(2, release_pos[0] - 80, release_pos[1], release_pos[2])
@ -725,3 +726,4 @@ class Scene: