hofee/gen_data_agent
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

finish data generate

Browse Source
This commit is contained in:
hofee
2025-09-05 19:18:37 +08:00
parent 21fbd5a323
commit 02440ceeb5
21 changed files with 17866 additions and 679 deletions
Download Patch File Download Diff File Expand all files Collapse all files

395
data_gen_dependencies/client.py
View File

@@ -30,7 +30,7 @@ import time
import json
import pinocchio
import os
from pyboot.utils.log import Log
# 目前代码中所有的旋转角度都以角度为单位
class Rpc_Client:
@@ -43,12 +43,12 @@ class Rpc_Client:
grpc.channel_ready_future(self.channel).result(timeout=5)
self.robot_urdf = robot_urdf
except grpc.FutureTimeoutError as e:
print(f"Failed to connect to gRPC server[{i}] ({client_host}): {e}", flush=True)
Log.warning(f"Failed to connect to gRPC server[{i}] ({client_host}): {e}")
time.sleep(3)
if i >= 599:
raise e
except grpc.RpcError as e:
print(f"Failed to connect to gRPC server[{i}] ({client_host}): {e}", flush=True)
Log.warning(f"Failed to connect to gRPC server[{i}] ({client_host}): {e}")
time.sleep(3)
if i >= 599:
raise e
@@ -71,7 +71,7 @@ class Rpc_Client:
"target_pose": target_pose
}
_frame_state: str = json.dumps(frame_state)
print(_frame_state)
#print(_frame_state)
req.frame_state = _frame_state
response = stub.SetFrameState(req)
return response
@@ -423,7 +423,7 @@ class Rpc_Client:
else:
joint_positions.append(joint_data[name])
joint_positions = np.array(joint_positions)
print(joint_positions)
#print(joint_positions)
pinocchio.forwardKinematics(model, data, joint_positions)
J = pinocchio.computeFrameJacobian(model, data, joint_positions, joint_id)
manip = np.sqrt(np.linalg.det(np.dot(J, J.T)))
@@ -552,7 +552,7 @@ class Rpc_Client:
stub = sim_observation_service_pb2_grpc.SimObservationServiceStub(self.channel)
req = sim_observation_service_pb2.SetMaterailReq()
for mat in material_info:
print(mat)
#print(mat)
mat_info = sim_observation_service_pb2.MaterialInfo()
mat_info.object_prim = mat["object_prim"]
mat_info.material_name = mat["material_name"]
@@ -567,7 +567,7 @@ class Rpc_Client:
stub = sim_observation_service_pb2_grpc.SimObservationServiceStub(self.channel)
req = sim_observation_service_pb2.SetLightReq()
for light in light_info:
print(light)
#print(light)
light_cfg = sim_observation_service_pb2.LightCfg()
light_cfg.light_type = light["light_type"]
light_cfg.light_prim = light["light_prim"]
@@ -579,384 +579,3 @@ class Rpc_Client:
req.lights.append(light_cfg)
response = stub.SetLight(req)
return (response)
# 调用示例
def run():
rpc_client = Rpc_Client(client_host="localhost:50051")
try:
while True:
send_msg = input("input Command: ")
if send_msg == "30":
result = rpc_client.DrawLine(
point_list_1=[[np.random.uniform(-5, 5), np.random.uniform(-5, 5), np.random.uniform(-5, 5)]],
point_list_2=[[np.random.uniform(-5, 5), np.random.uniform(-5, 5), np.random.uniform(-5, 5)]],
colors=[[np.random.uniform(0, 1), np.random.uniform(0, 1), np.random.uniform(0, 1)]],
sizes=[np.random.uniform(0, 1)],
name="test"
)
if send_msg == "3111":
result = rpc_client.ClearLine(name="test")
if send_msg == "point":
rpc_client.SetTargetPoint([0, 1, 1])
if send_msg == "test_mat":
rpc_client.SetMaterial([{
"object_prim": "/World/huxing/a_keting/yangbanjian_A_016",
"material_name": "Ash",
"material_path": "materials/wood/Ash"
},
{
"object_prim": "/World/huxing/a_Base/Floor_003",
"material_name": "Ash",
"material_path": "materials/wood/Ash"
}])
rpc_client.SetLight([{
"light_type": "Distant",
"light_prim": "/World/huxing/DistantLight",
"light_temperature": 2000,
"light_intensity": 1000,
"rotation": [1, 0.5, 0.5, 0.5],
"texture": ""
},
{
"light_type": "Dome",
"light_prim": "/World/DomeLight",
"light_temperature": 6500,
"light_intensity": 1000,
"rotation": [1, 0, 0, 0],
"texture": "materials/hdri/abandoned_hall_01_4k.hdr"
},
{
"light_type": "Rect",
"light_prim": "/World/RectLight",
"light_temperature": 4500,
"light_intensity": 1000,
"rotation": [1, 0, 0, 0],
"texture": ""
},
{
"light_type": "Rect",
"light_prim": "/World/RectLight_01",
"light_temperature": 8500,
"light_intensity": 1000,
"rotation": [1, 0, 0, 0],
"texture": ""
}])
if send_msg == "908":
pose_1 = [1, 1, 1], [1, 0, 0, 0]
pose_2 = [1, 0, 1], [1, 0, 0, 0]
trajecory_list = [pose_1, pose_2] * 10
result = rpc_client.SetTrajectoryList(trajecory_list, True)
if send_msg == "31":
target_poses = []
for i in range(1):
# x = np.random.uniform(0.3, 0.7)
# y = np.random.uniform(-0.3, 0.3)
# z = np.random.uniform(0.96, 1.2)
# pose = {
# "position": [x,y,z],
# "rotation": [1,0,0,0]
# }
x = 0.57597359649470348
y = -0.45669529659303565
z = 1.0198275517174573
rw = 0.066194084385260935
rx = 0.70713063274436749
ry = 0.70071350186850612
rz = 0.0677141028599091
pose = {
"position": [x, y, z],
"rotation": [rw, rx, ry, rz]
}
target_poses.append(pose)
result = rpc_client.GetIKStatus(target_poses=target_poses, is_right=False)
print(result)
if send_msg == "1":
# 相机
# A2W
rpc_client.capture_frame(camera_prim_path="/World/Raise_A2/base_link/Head_Camera")
result = rpc_client.capture_semantic_frame(camera_prim_path="/World/Raise_A2/base_link/Head_Camera")
print(result)
if send_msg == "2":
# 臂
x = np.random.uniform(0.3, 0.7)
y = np.random.uniform(-0.3, 0)
z = np.random.uniform(0.1, 0.5)
rpc_client.moveto(target_position=[x, y, z], target_quaternion=np.array([0.0, -0.0, -1.0, 0.0]),
arm_name="left", is_backend=True, ee_interpolation=True)
if send_msg == "20":
result = rpc_client.MultiPlan(robot_name="left", target_poses=[
[[0.5169683509992052, 0.1313259611510117, 1.1018942820728093],
[0.40020943, 0.57116637, 0.69704651, -0.16651593]],
[[0.5610938560120418, 0.048608636026916924, 1.0269891277236924],
[0.40020943, 0.57116637, 0.69704651, -0.16651593]],
[[0.5610938560120418, 0.048608636026916924, 1.2269891277236924],
[0.40020943, 0.57116637, 0.69704651, -0.16651593]]
])
print(result[0])
if send_msg == "44":
import time
# test cabinet
rpc_client.InitRobot("A2D_120s_horizon.json", "A2D.usd", "omnimanip_Simple_Room_01/simple_room.usd",
init_position=[-0.4, 0, -0.55])
rpc_client.reset()
time.sleep(1)
rpc_client.DetachObj()
rpc_client.reset()
time.sleep(1)
rpc_client.add_object(
usd_path="objects/guanglun/storagefurniture/storagefurniture011/Storagefurniture011.usd",
prim_path="/World/Objects/storagefurniture", label_name="test_storagefurniture",
target_position=np.array([
0.64,
-0.3,
0.40]), target_quaternion=np.array([0., 0., 0.70711, 0.70711]),
target_scale=np.array([1, 1, 1]), color=np.array([1, 0, 1]),
material="Plastic", mass=0.01)
time.sleep(1)
rpc_client.set_gripper_state(gripper_command="open", is_right=True, opened_width=1)
target_poses = []
start_time = time.time()
for i in range(50):
x = np.random.uniform(0, 0.2)
y = np.random.uniform(-0.3, 0.3)
z = np.random.uniform(0.96, 1.2)
pose = {
"position": [x, y, z],
"rotation": [1, x, y, z]
}
target_poses.append(pose)
result = rpc_client.GetIKStatus(target_poses=target_poses, is_right=False)
result = rpc_client.GetIKStatus(target_poses=target_poses, is_right=True)
rpc_client.DetachObj()
print("open")
rpc_client.get_object_pose("/World/Objects/storagefurniture/Storagefurniture011_Handle1")
rpc_client.get_object_pose("/World/Objects/storagefurniture/Storagefurniture011_Handle2")
obj_pose = rpc_client.get_object_pose("/World/Objects/storagefurniture/Storagefurniture011_Handle3")
obj_xyz = np.array(
[obj_pose.object_pose.position.x, obj_pose.object_pose.position.y, obj_pose.object_pose.position.z])
ee_pose = rpc_client.GetEEPose(is_right=True)
ee_quat = np.array(
[ee_pose.ee_pose.rpy.rw, ee_pose.ee_pose.rpy.rx, ee_pose.ee_pose.rpy.ry, ee_pose.ee_pose.rpy.rz])
goal_xyz = obj_xyz.copy()
goal_xyz[0] = goal_xyz[0] + 0.3
goal_xyz[2] = goal_xyz[2] + 0.55
print(goal_xyz)
rpc_client.moveto(
target_position=goal_xyz,
target_quaternion=ee_quat,
arm_name="left",
is_backend=False,
ee_interpolation=False,
distance_frame=0.001)
obj_pose = rpc_client.get_object_pose("/World/Objects/storagefurniture/Storagefurniture011_Handle3")
obj_xyz = np.array(
[obj_pose.object_pose.position.x, obj_pose.object_pose.position.y, obj_pose.object_pose.position.z])
ee_pose = rpc_client.GetEEPose(is_right=True)
ee_quat = np.array(
[ee_pose.ee_pose.rpy.rw, ee_pose.ee_pose.rpy.rx, ee_pose.ee_pose.rpy.ry, ee_pose.ee_pose.rpy.rz])
goal_xyz = obj_xyz.copy()
rpc_client.get_object_pose("/World/Objects/storagefurniture/Storagefurniture011_Handle1")
rpc_client.get_object_pose("/World/Objects/storagefurniture/Storagefurniture011_Handle2")
rpc_client.moveto(
target_position=goal_xyz,
target_quaternion=ee_quat,
arm_name="left",
is_backend=True,
ee_interpolation=False,
distance_frame=0.001)
rpc_client.set_gripper_state(gripper_command="close", is_right=True, opened_width=0.02)
time.sleep(1)
obj_pose = rpc_client.get_object_pose("/World/Objects/storagefurniture/Storagefurniture011_Handle3")
obj_xyz = np.array(
[obj_pose.object_pose.position.x, obj_pose.object_pose.position.y, obj_pose.object_pose.position.z])
ee_pose = rpc_client.GetEEPose(is_right=True)
ee_quat = np.array(
[ee_pose.ee_pose.rpy.rw, ee_pose.ee_pose.rpy.rx, ee_pose.ee_pose.rpy.ry, ee_pose.ee_pose.rpy.rz])
goal_xyz = obj_xyz.copy()
goal_xyz[0] = goal_xyz[0] - 0.1
rpc_client.moveto(
target_position=goal_xyz,
target_quaternion=ee_quat,
arm_name="left",
is_backend=True,
ee_interpolation=False,
distance_frame=0.001)
if send_msg == "21":
rpc_client.MultiMove(robot_name="left", plan_index=0)
if send_msg == "22":
rpc_client.MultiMove(robot_name="left", plan_index=1)
if send_msg == "23":
rpc_client.MultiMove(robot_name="left", plan_index=2)
if send_msg == "24":
rpc_client.start_recording()
if send_msg == "3":
# 关节
joint_states = rpc_client.get_joint_positions().states
joint_datas = {}
for joint in joint_states:
joint_datas[joint.name] = joint.position
result = rpc_client.GetManipulability(joint_datas)
print(result)
if send_msg == "4":
# 手
rpc_client.set_gripper_state(gripper_command="open", is_right=True, opened_width=0.1)
if send_msg == "5":
# 物体
import time
# rpc_client.reset()
rpc_client.add_object(usd_path="Collected_cabinet_000/cabinet_000.usd",
prim_path="/World/Objects/cabinet", label_name="test_cabinet",
target_position=np.array([.8, 0., -0.1]),
target_quaternion=np.array([0., 0., 0.70711, 0.70711]),
target_scale=np.array([1, 1, 1]), color=np.array([1, 0, 1]),
material="Plastic", mass=0.01)
time.sleep(1)
result = rpc_client.get_object_joint("/World/Objects/cabinet")
print(result)
# x = np.random.uniform(0.2, 0.64)
# y = np.random.uniform(-0.3, 0.3)
# rpc_client.add_object(usd_path="genie3D/09.usd",
# prim_path="/World/Objects/object8", label_name="test7",
# target_position=np.array([np.random.uniform(0.2, 0.64),np.random.uniform(-0.3, 0.3),1.2]), target_quaternion=np.array([0.0,-0.0,-1.0, 0.0]), target_scale=np.array([0.01,0.01,0.01]), color=np.array([1,1,0]), material="Brass", mass=1)
# rpc_client.add_object(usd_path="genie3D/02/02.usd",
# prim_path="/World/Objects/object2", label_name="test2",
# target_position=np.array([np.random.uniform(0.2, 0.64), np.random.uniform(-0.3, 0.3),1.2]), target_quaternion=np.array([0.0,-0.0,-1.0, 0.0]), target_scale=np.array([0.01,0.01,0.01]), color=np.array([1,1,1]))
# rpc_client.get_object_pose(prim_path="/World/Xform/box_place")
# rpc_client.add_object(usd_path="genie3D/06/06.usd",
# prim_path="/World/Objects/object6", label_name="test6",
# target_position=np.array([np.random.uniform(0.2, 0.64), np.random.uniform(-0.3, 0.3),1.2]), target_quaternion=np.array([0.0,-0.0,-1.0, 0.0]), target_scale=np.array([0.01,0.01,0.01]), color=np.array([1,0,0]))
# rpc_client.get_object_pose(prim_path="/World/Xform/box_pick_00")
# rpc_client.add_object(usd_path="genie3D/01/01.usd",
# prim_path="/World/Objects/object1", label_name="test1",
# target_position=np.array([np.random.uniform(0.2, 0.64), np.random.uniform(-0.3, 0.3),1.2]), target_quaternion=np.array([0.0,-0.0,-1.0, 0.0]), target_scale=np.array([0.01,0.01,0.01]), color=np.array([0,0,1]))
# result = rpc_client.get_object_pose(prim_path="/World/Raise_A2_W_T1/head_link/visuals")
if send_msg == "6":
# 获取全部信息
rpc_client.get_joint_positions()
if send_msg == "7":
rpc_client.set_gripper_state(gripper_command="close", is_right=True, opened_width=0.02)
if send_msg == "8":
result = rpc_client.get_observation(
data_keys={
'camera': {
'camera_prim_list': [
'/World/Raise_A2/base_link/Head_Camera'
],
'render_depth': True,
'render_semantic': True
},
'pose': [
'/World/Raise_A2/base_link/Head_Camera'
],
'joint_position': True,
'gripper': True
}
)
print(result["camera"])
if send_msg == "9":
rpc_client.start_recording(data_keys={
'camera': {
'camera_prim_list': [
'/World/Raise_A2/base_link/Head_Camera'
],
'render_depth': True,
'render_semantic': True
},
'pose': [
'/World/Raise_A2/base_link/Head_Camera'
],
'joint_position': True,
'gripper': True
}, fps=30, task_name="test"
)
time.sleep(1)
rpc_client.stop_recording()
rpc_client.SendTaskStatus(False)
if send_msg == "10":
rpc_client.stop_recording()
if send_msg == "11":
rpc_client.reset()
if send_msg == "13":
result = rpc_client.Exit()
print(result)
if send_msg == '112':
rpc_client.InitRobot(robot_cfg="Franka.json", robot_usd="Franka/franka.usd",
scene_usd="Pick_Place_Franka_Yellow_Table.usd")
if send_msg == '113':
position = [1.2, 0., 1.2]
rotation = [0.61237, 0.35355, 0.35355, 0.61237]
rotation = [0.65328, 0.2706, 0.2706, 0.65328]
width = 640
height = 480
rpc_client.AddCamera("/World/Sensors/Head_Camera", position, rotation, width, height, 18.14756, 20.955,
15.2908, False)
response = rpc_client.capture_frame(camera_prim_path="/World/Sensors/Head_Camera")
# cam_info
cam_info = {
"W": response.color_info.width,
"H": response.color_info.height,
"K": np.array(
[
[response.color_info.fx, 0, response.color_info.ppx],
[0, response.color_info.fy, response.color_info.ppy],
[0, 0, 1],
]
),
"scale": 1,
}
# rgb
# rgb = np.frombuffer(response.color_image.data, dtype=np.uint8).reshape(cam_info["H"], cam_info["W"], 4)[
# :, :, :3
# ]
# import cv2
# cv2.imwrite("head_img.png", rgb)
if send_msg == '114':
position = [0.05, 0., 0.]
rotation = [0.06163, 0.70442, 0.70442, 0.06163]
width = 640
height = 480
rpc_client.AddCamera("/panda/panda_hand/Hand_Camera_1", position, rotation, width, height, 18.14756,
20.955, 15.2908, True)
response = rpc_client.capture_frame(camera_prim_path="/panda/panda_hand/Hand_Camera_1")
# cam_info
cam_info = {
"W": response.color_info.width,
"H": response.color_info.height,
"K": np.array(
[
[response.color_info.fx, 0, response.color_info.ppx],
[0, response.color_info.fy, response.color_info.ppy],
[0, 0, 1],
]
),
"scale": 1,
}
# rgb
rgb = np.frombuffer(response.color_image.data, dtype=np.uint8).reshape(cam_info["H"], cam_info["W"], 4)[
:, :, :3
]
import cv2
cv2.imwrite("hand_img.png", rgb)
except Exception as e:
print("failed.{}".format(e))
return False
if __name__ == '__main__':
run()