103 lines
4.6 KiB
Python
103 lines
4.6 KiB
Python
|
#!/usr/bin/env python3
|
||
|
|
# -*- encoding: utf-8 -*-
|
||
|
|
import sys
|
||
|
|
import time
|
||
|
|
import grpc
|
||
|
|
|
||
|
|
import matplotlib.pyplot as plt
|
||
|
|
import numpy as np
|
||
|
|
from mpl_toolkits.axes_grid1 import make_axes_locatable
|
||
|
|
|
||
|
|
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)
|
||
|
|
])
|
||
|
|
|
||
|
|
sim_client = GrabSim_pb2_grpc.GrabSimStub(channel)
|
||
|
|
|
||
|
|
def map_test(map_id=0, scene_num=1):
|
||
|
|
initworld = sim_client.Init(GrabSim_pb2.NUL())
|
||
|
|
print(sim_client.AcquireAvailableMaps(GrabSim_pb2.NUL()))
|
||
|
|
initworld = sim_client.SetWorld(GrabSim_pb2.BatchMap(count=scene_num, mapID=map_id))
|
||
|
|
|
||
|
|
def joint_test(scene_id=0):
|
||
|
|
print('------------------joint_test----------------------')
|
||
|
|
action_list = [[0, 0, 0, 0, 0, 30, 0, 0, 0, 0, 0, 0, 0, 0, 36.0, -39.37, 37.2, -92.4, 4.13, -0.62, 0.4],
|
||
|
|
[0, 0, 0, 0, 0, 30, 0, 0, 0, 0, 0, 0, 0, 0, 36.0, -39.62, 34.75, -94.80, 3.22, -0.26, 0.85],
|
||
|
|
[0, 0, 0, 0, 0, 30, 0, 0, 0, 0, 0, 0, 0, 0, 32.63, -32.80, 15.15, -110.70, 6.86, 2.36, 0.40],
|
||
|
|
[0, 0, 0, 0, 0, 30, 0, 0, 0, 0, 0, 0, 0, 0, 28.18, -27.92, 6.75, -115.02, 9.46, 4.28, 1.35],
|
||
|
|
[0, 0, 0, 0, 0, 30, 0, 0, 0, 0, 0, 0, 0, 0, 4.09, -13.15, -11.97, -107.35, 13.08, 8.58, 3.33]]
|
||
|
|
|
||
|
|
for value in action_list:
|
||
|
|
action = GrabSim_pb2.Action(scene=scene_id, action=GrabSim_pb2.Action.ActionType.RotateJoints, values=value)
|
||
|
|
scene = sim_client.Do(action)
|
||
|
|
|
||
|
|
for i in range(8, 21): # arm
|
||
|
|
print(
|
||
|
|
f"{scene.joints[i].name}:{scene.joints[i].angle} location:{scene.joints[i].location.X},{scene.joints[i].location.Y},{scene.joints[i].location.Z}"
|
||
|
|
)
|
||
|
|
print('')
|
||
|
|
for i in range(5, 10): # Right hand
|
||
|
|
print(
|
||
|
|
f"{scene.fingers[i].name} angle:{scene.fingers[i].angle} location:{scene.fingers[i].location[0].X},{scene.fingers[i].location[0].Y},{scene.fingers[i].location[0].Z}"
|
||
|
|
)
|
||
|
|
print('----------------------------------------')
|
||
|
|
time.sleep(0.03)
|
||
|
|
time.sleep(1)
|
||
|
|
|
||
|
|
def gen_obj(scene_id, h=80):
|
||
|
|
print('------------------gen objs----------------------')
|
||
|
|
scene = sim_client.Observe(GrabSim_pb2.SceneID(value=scene_id))
|
||
|
|
ginger_loc = [scene.location.X, scene.location.Y, scene.location.Z]
|
||
|
|
|
||
|
|
obj_list = [
|
||
|
|
GrabSim_pb2.ObjectList.Object(x=ginger_loc[0] - 90, y=ginger_loc[1] + 30, yaw=10, z=h, type=4),
|
||
|
|
GrabSim_pb2.ObjectList.Object(x=ginger_loc[0] - 80, y=ginger_loc[1] + 31, z=h, type=5),
|
||
|
|
GrabSim_pb2.ObjectList.Object(x=ginger_loc[0] - 33, y=ginger_loc[1] - 10.5, z=h+20, type=7),
|
||
|
|
GrabSim_pb2.ObjectList.Object(x=ginger_loc[0] - 70, y=ginger_loc[1] + 33, z=h, type=9),
|
||
|
|
GrabSim_pb2.ObjectList.Object(x=ginger_loc[0] - 60, y=ginger_loc[1] + 34, z=h, type=13)
|
||
|
|
]
|
||
|
|
scene = sim_client.AddObjects(GrabSim_pb2.ObjectList(objects=obj_list, scene=scene_id))
|
||
|
|
print(scene.collision)
|
||
|
|
time.sleep(5)
|
||
|
|
|
||
|
|
def remove_obj(scene_id=0, id_list=[1]):
|
||
|
|
print('------------------remove objs----------------------')
|
||
|
|
remove_obj_list = id_list
|
||
|
|
scene = sim_client.RemoveObjects(GrabSim_pb2.RemoveList(IDs=remove_obj_list, scene=scene_id))
|
||
|
|
print(f"remove objects {id_list}. current obj:")
|
||
|
|
time.sleep(1)
|
||
|
|
|
||
|
|
def clean_obj(scene_id=0):
|
||
|
|
print('------------------clean objs----------------------')
|
||
|
|
scene = sim_client.CleanObjects(GrabSim_pb2.SceneID(value=scene_id))
|
||
|
|
|
||
|
|
def obj_test(scene_id=0):
|
||
|
|
gen_obj(scene_id)
|
||
|
|
# remove_obj(scene_id, id_list=[0])
|
||
|
|
# clean_obj(scene_id)
|
||
|
|
|
||
|
|
def grasp_test(hand_id, obj_scene_id, scene_id=0):
|
||
|
|
action = GrabSim_pb2.Action(scene=scene_id, action=GrabSim_pb2.Action.ActionType.Grasp, values=[hand_id, obj_scene_id])
|
||
|
|
scene = sim_client.Do(action)
|
||
|
|
|
||
|
|
def release_test(hand_id, scene_id=0):
|
||
|
|
action = GrabSim_pb2.Action(scene=scene_id, action=GrabSim_pb2.Action.ActionType.Release, values=[hand_id])
|
||
|
|
scene = sim_client.Do(action)
|
||
|
|
|
||
|
|
if __name__ == '__main__':
|
||
|
|
map_id = 3 # 地图编号: 3: 咖啡厅
|
||
|
|
scene_num = 1 # 场景数量
|
||
|
|
map_test(map_id, scene_num) # 场景加载测试
|
||
|
|
time.sleep(5)
|
||
|
|
|
||
|
|
for i in range(scene_num):
|
||
|
|
print("------------------", i, "----------------------")
|
||
|
|
joint_test(i) # 关节控制测试
|
||
|
|
obj_test(i) # 物品生成测试
|
||
|
|
grasp_test(1, 2) # 抓取物品测试
|
||
|
|
joint_test(i) # 关节控制测试
|
||
|
|
release_test(1) # 释放物品测试
|