solve dependencies problem

configs/generate_data_config.yaml

@@ -0,0 +1,7 @@
+runner:
+  workspace:
+    name: "generate_data"
+    root_dir: "workspace"
+  generate:
+    input_data_root: "/home/ubuntu/Projects/docker_isaac_sim/input/data"
+    input_target_task_templates_path: "workspace/divide_task/template_targets/task_template_target_0.json"

configs/generate_task_config.yaml

@@ -6,3 +6,4 @@ runner:
     input_data_root: "/home/ubuntu/Projects/docker_isaac_sim/input/data"
     input_target_task_templates_path: "workspace/divide_task/template_targets/task_template_target_0.json"
     output_task_root_dir:
+    output_generate_result_path:

data_gen_dependencies/__init__.py

@@ -0,0 +1,35 @@
+from abc import abstractmethod, ABC
+
+
+class Robot(ABC):
+    @abstractmethod
+    def get_prim_world_pose(self, prim_path):
+        pass
+
+    @abstractmethod
+    def reset(self):
+        pass
+
+    @abstractmethod
+    def open_gripper(self, id="left", width=0.1):
+        pass
+
+    @abstractmethod
+    def close_gripper(self, id="left", force=50):
+        pass
+
+    @abstractmethod
+    def move(self, target, type="pose", speed=None):
+        pass
+
+    @abstractmethod
+    def decode_gripper_pose(self, gripper_pose):
+        pass
+
+    @abstractmethod
+    def get_ee_pose(self, id="right"):
+        pass
+
+    @abstractmethod
+    def check_ik(self, pose, id="right"):
+        pass

data_gen_dependencies/action/__init__.py

@@ -0,0 +1,30 @@
+from .grasp import GraspStage, PickStage, HookStage
+from .place import PlaceStage
+from .insert import InsertStage, HitStage
+from .slide import SlideStage
+from .pour import PourStage
+from .pull import PullStage
+from .push import PushStage, ClickStage
+
+ACTION_STAGE = {
+    "grasp": GraspStage,
+    "pick": PickStage,
+    "hook": HookStage,
+    "place": PlaceStage,
+    "insert": InsertStage,
+    "slide": SlideStage,
+    "shave": NotImplemented,
+    "brush": NotImplemented,
+    "wipe": NotImplemented,
+    "hit": NotImplemented,
+    "pour": PourStage,
+    "push": PushStage,
+    'click': ClickStage,
+    'touch': ClickStage,
+    "pull": PullStage
+}
+
+def build_stage(action):
+    if action not in ACTION_STAGE:
+        raise NotImplementedError
+    return ACTION_STAGE[action]

data_gen_dependencies/action/base.py

@@ -0,0 +1,110 @@
+import copy
+from collections import deque
+
+import numpy as np
+from data_gen_dependencies.data_utils import pose_difference
+
+
+def simple_check_completion(goal, objects, last_statement=None, pos_threshold=0.06, angle_threshold=70, is_grasped=False):
+    active_obj_id, passive_obj_id, target_pose_canonical, gripper_action, transform_world, motion_type = goal
+    if target_pose_canonical is None:
+        return True
+    if gripper_action=='open':
+        return True
+
+    current_pose_world = objects[active_obj_id].obj_pose
+    if len(target_pose_canonical.shape)==3:
+        target_pose_canonical = target_pose_canonical[-1]
+        transform_world = transform_world[-1]
+    target_pose_world = objects[passive_obj_id].obj_pose @ target_pose_canonical
+    if not is_grasped:
+        target_pose_world = np.dot(transform_world, target_pose_world)
+    
+    pos_diff, angle_diff = pose_difference(current_pose_world, target_pose_world)
+    success = (pos_diff < pos_threshold) and (angle_diff < angle_threshold)
+    return success
+
+
+def solve_target_gripper_pose(stage, objects):
+    active_obj_ID, passive_obj_ID, target_pose_canonical, gripper_action, transform_world, motion_type = stage
+    
+    anchor_pose = objects[passive_obj_ID].obj_pose
+    
+    
+    if motion_type=='Trajectory':
+        assert len(target_pose_canonical.shape)==3, 'The target_pose should be a list of poses'
+        target_pose = anchor_pose[np.newaxis, ...] @ target_pose_canonical
+        target_pose = transform_world @ target_pose
+    else:
+        target_pose = anchor_pose @ target_pose_canonical
+        target_pose = transform_world @ target_pose
+    assert 'gripper' in objects, 'The gripper should be the first one in the object list'
+    current_gripper_pose = objects['gripper'].obj_pose
+    
+    if active_obj_ID=='gripper':
+        target_gripper_pose = target_pose
+    else:
+        current_obj_pose = objects[active_obj_ID].obj_pose
+        gripper2obj = np.linalg.inv(current_obj_pose) @ current_gripper_pose
+        if len(target_pose.shape)==3:
+            gripper2obj = gripper2obj[np.newaxis, ...]
+        
+        target_obj_pose = target_pose
+        target_gripper_pose = target_obj_pose @ gripper2obj
+
+    return target_gripper_pose
+
+
+
+class StageTemplate:
+    def __init__(self, active_obj_id, passive_obj_id, active_element, passive_element):
+        self.active_obj_id = active_obj_id
+        self.passive_obj_id = passive_obj_id
+        self.active_element = active_element
+        self.passive_element = passive_element
+
+        self.last_statement = None
+        self.sub_stages = deque()
+        self.step_id = 0
+        self.extra_params = {}
+
+    def generate_substage(self):
+        raise NotImplementedError
+
+    def __len__(self) -> int:
+        return len(self.sub_stages) - self.step_id
+
+    def get_action(self, objects):
+        if self.__len__()==0:
+            return None
+        gripper_pose_canonical, gripper_action, transform_world, motion_type = self.sub_stages[self.step_id]
+
+        if motion_type == 'local_gripper':
+            delta_pose = gripper_pose_canonical
+            gripper_pose = objects['gripper'].obj_pose
+            target_gripper_pose = gripper_pose @ delta_pose
+            motion_type = 'Straight'
+        else:
+
+            if gripper_pose_canonical is None:
+                target_gripper_pose = None
+            else: 
+                goal_datapack = [self.active_obj_id, self.passive_obj_id] + self.sub_stages[self.step_id]
+                target_gripper_pose = solve_target_gripper_pose(goal_datapack, objects)
+            
+            last_statement = {'objects': copy.deepcopy(objects), 'target_gripper_pose': target_gripper_pose}
+            self.last_statement = last_statement
+        return target_gripper_pose, motion_type, gripper_action, self.extra_params.get('arm', 'right')
+    
+
+    def check_completion(self, objects):
+        if self.__len__()==0:
+            return True
+        goal_datapack = [self.active_obj_id, self.passive_obj_id] + self.sub_stages[self.step_id]
+        succ = simple_check_completion(goal_datapack, objects)
+        if succ:
+            self.step_id += 1
+        return succ
+        
+
+

data_gen_dependencies/action/grasp.py

@@ -0,0 +1,136 @@
+import numpy as np
+
+from data_gen_dependencies.action.base import StageTemplate, simple_check_completion, pose_difference
+
+
+class PickStage(StageTemplate):
+    def __init__(self, active_obj_id, passive_obj_id, active_element, passive_element, grasp_pose, extra_params=None, **kwargs):
+        super().__init__(active_obj_id, passive_obj_id, active_element, passive_element)
+        self.grasp_pose_canonical = grasp_pose
+        self.use_pre_grasp = False
+
+        self.extra_params = {} if extra_params is None else extra_params
+        self.pick_up_step = 999
+        self.generate_substage(grasp_pose)
+        
+
+    def generate_substage(self, grasp_pose):
+        pick_up_distance = self.extra_params.get('pick_up_distance', 0.12)
+        pick_up_type = self.extra_params.get('pick_up_type', 'Simple')
+        pick_up_direction = self.extra_params.get('pick_up_direction', 'z')
+        num_against = self.extra_params.get('against', 0)
+        if self.use_pre_grasp:
+            pre_grasp_distance = self.extra_params.get('pre_grasp_distance', 0.08)
+            # sub-stage-0   moveTo pregrasp pose
+            pre_pose = np.array([
+                [1., 0, 0, 0],
+                [0, 1., 0, 0], 
+                [0, 0, 1., -pre_grasp_distance],
+                [0, 0, 0, 1]])
+            pre_grasp_pose = grasp_pose @ pre_pose   
+            self.sub_stages.append([pre_grasp_pose, None, np.eye(4), 'AvoidObs'])
+            # sub-stage-1   moveTo grasp pose   
+            self.sub_stages.append([grasp_pose, 'close', np.eye(4), 'Simple'])
+            self.pick_up_step = 2
+        elif num_against > 0:
+            for index in range(num_against):
+                self.sub_stages.append([grasp_pose, 'open', np.eye(4), 'AvoidObs'])
+            self.sub_stages.append([grasp_pose, 'close', np.eye(4), 'AvoidObs'])
+            self.pick_up_step = num_against + 1 
+            gripper_action = None
+            motion_type = pick_up_type
+            transform_up = np.eye(4)
+            if pick_up_direction == "x":
+                transform_up[0,3] = pick_up_distance 
+                transform_up[2,3] = 0.02
+            elif pick_up_direction == "y":
+                transform_up[1,3] = pick_up_distance
+            else:
+                transform_up[2,3] = pick_up_distance
+            self.sub_stages.append([grasp_pose, gripper_action, transform_up, motion_type])
+        else:
+            # sub-stage-0   moveTo grasp pose
+            self.sub_stages.append([grasp_pose, 'close', np.eye(4), 'AvoidObs'])
+            self.pick_up_step = 1
+
+            # pick-up
+            gripper_action = None
+            motion_type = pick_up_type
+            transform_up = np.eye(4)
+            if pick_up_direction == "x":
+                transform_up[0,3] = pick_up_distance 
+                transform_up[2,3] = 0.02
+            elif pick_up_direction == "y":
+                transform_up[1,3] = pick_up_distance
+            else:
+                transform_up[2,3] = pick_up_distance
+            self.sub_stages.append([grasp_pose, gripper_action, transform_up, motion_type])
+
+
+
+    def check_completion(self, objects):
+        if self.__len__()==0:
+            return True
+        
+        goal_datapack = [self.active_obj_id, self.passive_obj_id] + self.sub_stages[self.step_id]
+
+        succ = True
+        if self.step_id < self.pick_up_step:
+            succ = simple_check_completion(goal_datapack, objects, is_grasped=True)
+
+        if succ:
+            self.step_id += 1
+        return succ
+    
+
+
+
+
+
+
+
+class GraspStage(PickStage):
+    def generate_substage(self, grasp_pose):
+        motion_type = self.extra_params.get('move_type', 'AvoidObs')
+
+        if self.extra_params.get('use_pre_grasp', False):
+            pre_pose = np.array([
+                [1., 0, 0, 0],
+                [0, 1., 0, 0], 
+                [0, 0, 1., -0.08],
+                [0, 0, 0, 1]])
+            pre_grasp_pose = grasp_pose @ pre_pose   
+            gripper_action = None
+            
+            self.sub_stages.append([pre_grasp_pose, None, np.eye(4), motion_type])
+        # sub-stage-1   moveTo grasp pose
+        self.sub_stages.append([grasp_pose, 'close', np.eye(4), motion_type])
+
+        
+
+
+
+    def check_completion(self, objects):
+        if self.__len__()==0:
+            return True
+        
+        goal_datapack = [self.active_obj_id, self.passive_obj_id] + self.sub_stages[self.step_id]
+
+        pos_threshold = self.extra_params.get('position_threshold', 0.06)
+        succ = simple_check_completion(goal_datapack, objects, pos_threshold=pos_threshold, is_grasped=True)
+
+        if self.step_id >= 2:
+            gripper_pose = objects['gripper'].obj_pose
+            grasp_pose = objects[self.passive_obj_id].obj_pose @ self.grasp_pose_canonical
+            pos_diff, _ = pose_difference(gripper_pose, grasp_pose)
+            grasp_succ = pos_diff < pos_threshold
+            succ = succ and grasp_succ
+
+        if succ:
+            self.step_id += 1
+        return succ
+    
+
+class HookStage(PickStage):
+    def generate_substage(self, grasp_pose, *args, **kwargs):
+        self.sub_stages.append([grasp_pose, None, np.eye(4), 'AvoidObs'])

data_gen_dependencies/action/insert.py

@@ -0,0 +1,53 @@
+import numpy as np
+
+from data_gen_dependencies.action.base import StageTemplate, simple_check_completion
+
+
+class InsertStage(StageTemplate):
+    def __init__(self, active_obj_id, passive_obj_id, active_element=None, passive_element=None, target_pose=None, vector_direction=None, extra_params={}, **kwargs):
+        super().__init__(active_obj_id, passive_obj_id, active_element, passive_element)
+
+
+        self.move_distance = 0.01
+        self.generate_substage(target_pose, vector_direction, extra_params)
+        
+
+    def generate_substage(self, target_pose, vector_direction, extra_params={}):
+        insert_pre_distance = extra_params.get('insert_pre_distance', -0.08)
+        insert_motion_type = extra_params.get('insert_motion_type', 'Simple')
+
+        vector_direction = vector_direction / np.linalg.norm(vector_direction)
+
+        # moveTo pre-place position
+        pre_pose = target_pose.copy()
+        pre_pose[:3,3] += vector_direction * insert_pre_distance
+        self.sub_stages.append([pre_pose, None, np.eye(4), 'AvoidObs'])
+        
+        # insert
+        move_pose = target_pose.copy()
+        move_pose[:3,3] += vector_direction * self.move_distance
+        self.sub_stages.append([move_pose, None, np.eye(4), insert_motion_type])
+
+        # open gripper
+        self.sub_stages.append([None, 'open', np.eye(4), None])
+
+    def check_completion(self, objects):
+        if self.__len__()==0:
+            return True
+        goal_datapack = [self.active_obj_id, self.passive_obj_id] + self.sub_stages[self.step_id]
+        succ = simple_check_completion(goal_datapack, objects)
+        if succ:
+            self.step_id += 1
+        return succ
+        
+
+        
+class HitStage(InsertStage):
+    def __init__(self, active_obj_id, passive_obj_id, active_element=None, passive_element=None, target_pose=np.eye(4), vector_direction=None, **kwargs):
+        super(InsertStage, self).__init__(active_obj_id, passive_obj_id, active_element, passive_element)
+
+        self.pre_distance = -0.05
+        self.move_distance = 0.005
+        self.generate_substage(target_pose, vector_direction)
+
+

data_gen_dependencies/action/place.py

@@ -0,0 +1,64 @@
+import numpy as np
+
+from data_gen_dependencies.action.base import StageTemplate, simple_check_completion
+
+
+class PlaceStage(StageTemplate):
+    def __init__(self, active_obj_id, passive_obj_id, active_element=None, passive_element=None, target_pose=None, extra_params={}, **kwargs):
+        super().__init__(active_obj_id, passive_obj_id, active_element, passive_element)
+        self.pre_transform_up =0.12
+        self.place_transform_up = np.array([0, 0, 0.01])
+        self.extra_params = {} if extra_params is None else extra_params
+        self.use_pre_place = self.extra_params.get('use_pre_place', True)
+        self.generate_substage(target_pose)
+        
+        
+ 
+        
+    def generate_substage(self, target_pose):
+        target_pose_canonical = target_pose
+        gripper_cmd = self.extra_params.get('gripper_state', 'open')
+        pre_place_direction = self.extra_params.get('pre_place_direction', 'z')
+        print(gripper_cmd)
+        num_against = self.extra_params.get('against', 0)
+        if self.use_pre_place:
+            # moveTo pre-place position
+            transform_up = np.eye(4)
+            print(transform_up)
+            if pre_place_direction == "x":
+                transform_up[0,3] = self.pre_transform_up
+                transform_up[2,3] = 0.02
+            elif pre_place_direction == "y":
+                transform_up[1,3] = self.pre_transform_up
+            else:
+                transform_up[2,3] = self.pre_transform_up
+            self.sub_stages.append([target_pose_canonical, None, transform_up, 'AvoidObs'])
+            
+            # place
+            palce_transform_up = np.eye(4)
+            palce_transform_up[:3,3] = self.place_transform_up
+            if num_against > 0:
+                for index in range(num_against):
+                    self.sub_stages.append([target_pose_canonical, None, transform_up, 'AvoidObs'])
+                self.pick_up_step = num_against + 1 
+            self.sub_stages.append([target_pose_canonical, gripper_cmd, palce_transform_up, 'Simple'])
+        else:
+            palce_transform_up = np.eye(4)
+            palce_transform_up[:3,3] = self.place_transform_up
+            self.sub_stages.append([target_pose_canonical, None, palce_transform_up, 'AvoidObs'])
+
+            self.sub_stages.append([None, gripper_cmd, np.eye(4), 'Simple'])
+
+    def check_completion(self, objects):
+        if self.__len__()==0:
+            return True
+        goal_datapack = [self.active_obj_id, self.passive_obj_id] + self.sub_stages[self.step_id]
+        succ = True
+        if self.step_id==0:
+            succ = simple_check_completion(goal_datapack, objects)
+        if succ:
+            self.step_id += 1
+        return succ
+        
+
+

data_gen_dependencies/action/pour.py

@@ -0,0 +1,97 @@
+from data_gen_dependencies.action.base import StageTemplate, solve_target_gripper_pose, simple_check_completion
+from data_gen_dependencies.fix_rotation import interpolate_rotation_matrices
+
+import copy
+import numpy as np
+
+
+
+
+
+class PourStage(StageTemplate):
+    def __init__(self, active_obj_id, passive_obj_id, active_element=None, passive_element=None, target_pose=None, current_pose=None, obj2part=None, **kwargs):
+        super().__init__(active_obj_id, passive_obj_id, active_element, passive_element)
+        
+        self.pre_pour_height = 0.25
+        self.pour_heigth = 0.08
+        self.generate_substage(target_pose, current_pose, obj2part)
+        
+
+    def generate_substage(self, target_pose, current_pose, obj2part):
+        target_part_pose = target_pose @ np.linalg.inv(obj2part)
+        current_part_pose = current_pose @ np.linalg.inv(obj2part)
+
+        gripper_action = None
+        transform_pre_pour = np.eye(4)
+        transform_pre_pour[2,3] = self.pre_pour_height    # 8cm above the target pose
+
+
+        # moveTo pre-pour position         
+        pre_pour_part_pose = np.copy(target_part_pose)
+        pre_pour_part_pose[:3, :3] = current_part_pose[:3, :3]
+        # # pre_pour at 2/3 position from current to target
+        # pos_diff = target_part_pose[:3, 3] - current_part_pose[:3, 3]
+        # pre_pour_part_pose[:3, 3] = current_part_pose[:3, 3] + pos_diff * 1/2
+        # pre_pour_part_pose[:3, :3] = current_part_pose[:3, :3]
+        pre_pour_pose = pre_pour_part_pose @ obj2part
+        motion_type = 'AvoidObs'
+        self.sub_stages.append([pre_pour_pose, gripper_action, transform_pre_pour, motion_type])
+
+
+        motion_type = 'Trajectory'
+        rotations = interpolate_rotation_matrices(current_part_pose[:3,:3], target_part_pose[:3,:3], 200)
+        target_part_pose_list = np.tile(target_part_pose, (len(rotations), 1, 1))
+        target_part_pose_list[:, :3, :3] = rotations
+        target_pose_list = target_part_pose_list @ obj2part[np.newaxis, ...]
+
+        transform_pour = np.tile(np.eye(4), (len(rotations), 1, 1))
+        # transform_pour[:, 2, 3] = self.pre_pour_height
+        transform_pour[:, 2, 3] = np.linspace(self.pre_pour_height, self.pour_heigth, len(rotations))
+        # import ipdb; ipdb.set_trace()
+        self.sub_stages.append([target_pose_list, gripper_action, transform_pour, motion_type])
+            
+            
+    def get_action(self, objects):
+        if self.__len__()==0:
+            return None
+        gripper_pose_canonical, gripper_action, transform_world, motion_type = self.sub_stages[self.step_id]
+
+        if gripper_pose_canonical is None:
+            target_gripper_pose = None
+        else: 
+            goal_datapack = [self.active_obj_id, self.passive_obj_id] + self.sub_stages[self.step_id]
+            target_gripper_pose = solve_target_gripper_pose(goal_datapack, objects)
+
+            # current_gripper_pose = objects['gripper'].obj_pose
+            # if self.step_id==0:
+            #     # pre_pour at 5cm away from current to target
+            #     diff_xy_direction = target_gripper_pose[:2, 3] - current_gripper_pose[:2, 3]
+            #     pos_diff = np.linalg.norm(diff_xy_direction) * 0.10
+            #     target_gripper_pose[:2, 3] = current_gripper_pose[:2, 3] + pos_diff
+
+            # elif self.step_id==1:
+            #     target_xyz = target_gripper_pose[-1, :3, 3]
+            #     current_xyz = current_gripper_pose[:3, 3]
+
+            #     xyz_interp = np.linspace(current_xyz, target_xyz, len(target_xyz))
+            #     target_gripper_pose[:, :3, 3] = xyz_interp
+
+            #     import ipdb; ipdb.set_trace()
+        
+        last_statement = {'objects': copy.deepcopy(objects), 'target_gripper_pose': target_gripper_pose}
+        self.last_statement = last_statement
+        return target_gripper_pose, motion_type, gripper_action , "right"
+    
+
+    def check_completion(self, objects):
+        if self.__len__()==0:
+            return True
+        goal_datapack = [self.active_obj_id, self.passive_obj_id] + self.sub_stages[self.step_id]
+        succ = simple_check_completion(goal_datapack, objects)
+
+        
+            
+
+        if succ:
+            self.step_id += 1
+        return succ

data_gen_dependencies/action/pull.py

@@ -0,0 +1,59 @@
+import numpy as np
+
+from data_gen_dependencies.action.base import StageTemplate
+
+
+class PullStage(StageTemplate):
+    def __init__(self, active_obj_id, passive_obj_id, active_element=None, passive_element=None, target_pose=np.eye(4), extra_params=None, **kwargs):
+        super().__init__(active_obj_id, passive_obj_id, active_element, passive_element)
+        self.pull_distance = passive_element['distance']
+        assert self.pull_distance > 0
+        self.joint_position_threshold = passive_element.get('joint_position_threshold', 0.5)
+        assert self.joint_position_threshold >= 0 and self.joint_position_threshold <= 1 
+        self.joint_direction = passive_element.get('joint_direction', 1)
+        assert self.joint_direction in [-1, 1]
+        self.joint_velocity_threshold = passive_element.get('joint_velocity_threshold', 999)
+        vector_direction = passive_element['direction']
+
+        self.extra_params = {} if extra_params is None else extra_params
+        if 'pull_distance' in self.extra_params:
+            self.pull_distance = self.extra_params['pull_distance']
+        self.generate_substage(target_pose, vector_direction)
+
+    def generate_substage(self, target_pose, vector_direction):
+        vector_direction = vector_direction / np.linalg.norm(vector_direction)
+        # moveTo pre-place position
+        move_pose = target_pose.copy()
+        move_pose[:3,3] += vector_direction * self.pull_distance
+        self.sub_stages.append([move_pose, None, np.eye(4), 'Straight'])
+        self.sub_stages.append([None, "open", np.eye(4), 'Simple'])
+
+        free_delta_pose = np.eye(4)
+        free_delta_pose[2,3] = -0.03
+        self.sub_stages.append([free_delta_pose, None, np.eye(4), 'local_gripper'])
+
+        
+
+
+    def check_completion(self, objects):
+        if self.__len__()==0:
+            return True
+        
+        if self.step_id >= 0:
+            joint_position = objects[self.passive_obj_id].joint_position
+            joint_velocity = objects[self.passive_obj_id].joint_velocity
+            lower_bound = objects[self.passive_obj_id].part_joint_limit['lower_bound']
+            upper_bound = objects[self.passive_obj_id].part_joint_limit['upper_bound']
+            if self.joint_direction == 1:
+                succ = joint_position > lower_bound + (upper_bound - lower_bound) * self.joint_position_threshold
+            else:
+                succ = joint_position < lower_bound + (upper_bound - lower_bound) * self.joint_position_threshold
+            succ = succ and joint_velocity < self.joint_velocity_threshold
+
+        if succ:
+            self.step_id += 1
+        # else:
+            # import ipdb;ipdb.set_trace()
+
+        return succ
+        

data_gen_dependencies/action/push.py

@@ -0,0 +1,95 @@
+import numpy as np
+
+from data_gen_dependencies.action.base import StageTemplate, pose_difference
+
+
+class PushStage(StageTemplate):
+    def __init__(self, active_obj_id, passive_obj_id, active_element=None, passive_element=None, target_pose=np.eye(4), vector_direction=None, extra_params=None, **kwargs):
+        super().__init__(active_obj_id, passive_obj_id, active_element, passive_element)
+
+        extra_params = {} if extra_params is None else extra_params
+
+        self.pre_distance = extra_params.get('pre_distance', -0.03)
+        self.move_distance = extra_params.get('move_distance', 0.14)
+
+        vector_direction = vector_direction / np.linalg.norm(vector_direction)
+        self.goal_transform = np.eye(4)
+        self.goal_transform[:3,3] = vector_direction * self.move_distance
+
+        if passive_element is not None:
+            self.joint_position_threshold = passive_element.get('joint_position_threshold', 0.5)
+            assert self.joint_position_threshold >= 0 and self.joint_position_threshold <= 1 
+            self.joint_direction = passive_element.get('joint_direction', 1)
+            assert self.joint_direction in [-1, 1]
+            self.joint_velocity_threshold = passive_element.get('joint_velocity_threshold', 999)
+
+        self.generate_substage(target_pose, vector_direction)
+
+        
+    def generate_substage(self, target_pose, vector_direction):
+        vector_direction = vector_direction / np.linalg.norm(vector_direction)
+        # moveTo pre-place position
+        pre_pose = target_pose.copy()
+        pre_pose[:3,3] += vector_direction * self.pre_distance
+        self.sub_stages.append([pre_pose, 'close', np.eye(4), 'AvoidObs'])
+        
+        # insert
+        move_pose = target_pose.copy()
+        move_pose[:3,3] += vector_direction * self.move_distance
+        self.sub_stages.append([move_pose, None, np.eye(4), 'Straight'])
+
+
+    def check_completion(self, objects):
+        if self.__len__()==0:
+            return True
+
+        # TODO 铰接joitn判定
+
+        if self.step_id==0:
+            succ = True
+
+        if self.step_id>=1:
+            if objects[self.passive_obj_id].is_articulated:
+                joint_position = objects[self.passive_obj_id].joint_position
+                joint_velocity = objects[self.passive_obj_id].joint_velocity
+                lower_bound = objects[self.passive_obj_id].part_joint_limit['lower_bound']
+                upper_bound = objects[self.passive_obj_id].part_joint_limit['upper_bound']
+                if self.joint_direction == 1:
+                    succ = joint_position > lower_bound + (upper_bound - lower_bound) * self.joint_position_threshold
+                else:
+                    succ = joint_position < lower_bound + (upper_bound - lower_bound) * self.joint_position_threshold
+                succ = succ and joint_velocity < self.joint_velocity_threshold
+            else:
+                sub_stage = self.sub_stages[self.step_id]
+        
+                last_pose = self.last_statement['objects'][self.passive_obj_id].obj_pose
+                target_obj_pose = last_pose @ self.goal_transform
+                current_obj_pose = objects[self.passive_obj_id].obj_pose
+                pos_diff, _ = pose_difference(current_obj_pose, target_obj_pose)
+                succ = pos_diff < 0.04
+        
+        if succ:
+            self.step_id += 1
+        return succ
+
+
+class ClickStage(StageTemplate):
+    def __init__(self, active_obj_id, passive_obj_id, active_element=None, passive_element=None, target_pose=None, vector_direction=None, **kwargs):
+        super().__init__(active_obj_id, passive_obj_id, active_element, passive_element)
+        self.pre_distance = -0.03
+        self.move_distance = 0.00
+        self.generate_substage(target_pose, vector_direction)
+
+        
+    def generate_substage(self, target_pose, vector_direction):
+        vector_direction = vector_direction / np.linalg.norm(vector_direction)
+        # moveTo pre-place position
+        pre_pose = target_pose.copy()
+        pre_pose[:3,3] += vector_direction * self.pre_distance
+        self.sub_stages.append([pre_pose, 'close', np.eye(4), 'AvoidObs'])
+        
+        # insert
+        move_pose = target_pose.copy()
+        move_pose[:3,3] += vector_direction * self.move_distance
+        self.sub_stages.append([move_pose, None, np.eye(4), 'Simple'])
+

data_gen_dependencies/action/slide.py

@@ -0,0 +1,47 @@
+import numpy as np
+
+from data_gen_dependencies.action.base import StageTemplate, pose_difference
+
+
+class SlideStage(StageTemplate):
+    def __init__(self, active_obj_id, passive_obj_id, active_element=None, passive_element=None, target_pose=np.eye(4), **kwargs):
+        super().__init__(active_obj_id, passive_obj_id, active_element, passive_element)
+
+        self.pre_distance = 0.0
+        self.move_distance = 0.08
+        vector_direction = passive_element['direction']
+        self.generate_substage(target_pose, vector_direction)
+
+
+
+    def generate_substage(self, target_pose, vector_direction):
+        vector_direction = vector_direction / np.linalg.norm(vector_direction)
+        # slide
+        move_pose = target_pose.copy()
+        move_pose[:3,3] += vector_direction * self.move_distance
+        self.sub_stages.append([move_pose, None, np.eye(4), 'Simple'])
+
+        # open gripper
+        self.sub_stages.append([None, 'open', np.eye(4), None])
+
+    
+
+    def check_completion(self, objects):
+        if self.__len__()==0:
+            return True
+        sub_stage = self.sub_stages[self.step_id]
+        move_transform, gripper_action = sub_stage[0], sub_stage[1]
+        if gripper_action=='open':
+            self.step_id += 1
+            return True
+        last_pose = self.last_statement['objects'][self.passive_obj_id].obj_pose
+        target_obj_pose = last_pose @ move_transform
+        current_obj_pose = objects[self.passive_obj_id].obj_pose
+        pos_diff, _ = pose_difference(current_obj_pose, target_obj_pose)
+        succ = pos_diff < 0.04
+        print(pos_diff, succ)
+        if succ:
+            self.step_id += 1
+        return succ
+
+

data_gen_dependencies/aimdk/protocol/CMakeLists.txt

@@ -0,0 +1,8 @@
+cmake_minimum_required(VERSION 3.16)
+
+# -------------------------------------------------------------
+# Create the project
+
+aimrte_project(NAME aimrt_comm_protocol ROOT_NAMESPACE "aimdk;protocol")
+aimrte_add_subdirectories(.)
+aimrte_project_complete()

data_gen_dependencies/aimdk/protocol/README.md

@@ -0,0 +1,25 @@
+# AimDK 接口协议
+
+## 简介
+
+本仓库定义了 AgiBot 产品的所有跨模块的接口协议，包含对外的部分与仅在内部流通的部分。
+
+本协议的根命名空间是  **aimdk::protocol::** ，我们按不同模块进一步划分了子目录，并以这些模块的名，命名这些子目录及其下一级命名空间。
+
+
+
+## 约定
+
+协议编写规范见 [《研发规划/协议规范》](https://agirobot.feishu.cn/wiki/YBe5wYMtiicci2kBgBxc8KUHngb?fromScene=spaceOverview&theme=LIGHT&contentTheme=DARK) 。
+
+我们需要避免冗余协议内容的出现，若多个模块出现相同的协议内容，若其语义拥有公共属性，应将其放到 common 子协议目录下。
+
+我们将每个协议内容的定义，以注释的形式放在协议源文件中，但在每个子目录（包括本目录）设立 README.md，作为简单的检索之用，
+让阅读者更方便找到所需的协议文件。
+
+
+
+## 模块目录
+
+- [公共部分](./common/README.md)
+- [定位模块（SLAM）](./slam/README.md)

data_gen_dependencies/aimdk/protocol/common/CMakeLists.txt

@@ -0,0 +1,5 @@
+# -------------------------------------------------------------
+# Protocol: aimdk::protocol::common
+# -------------------------------------------------------------
+
+aimrte_protocol()

data_gen_dependencies/aimdk/protocol/common/README.md

@@ -0,0 +1,9 @@
+# AimDK common 协议
+
+## 简介
+
+本目录放置了所有模块公用、复用的协议内容的定义。
+
+## 检索
+
+- [所有消息、请求、响应都有的 header](./header.proto)

data_gen_dependencies/aimdk/protocol/common/battery_state.proto

@@ -0,0 +1,95 @@
+syntax = "proto3";
+package aimdk.protocol;
+
+import "aimdk/protocol/common/timestamp.proto";
+
+
+/**
+ *@brief 电池状态
+ */
+enum PowerSupplyStatus{
+  PowerSupplyStatus_UNDEFINED = 0;
+  PowerSupplyStatus_CHARGING = 1;
+  PowerSupplyStatus_DISCHARGING = 2;
+  PowerSupplyStatus_NOT_CHARGING = 3;
+  PowerSupplyStatus_FULL = 4;
+}
+
+/**
+ *@brief 电池健康状态
+ */
+enum PowerSupplyHealth{
+  PowerSupplyHealth_UNDEFINED= 0;
+  PowerSupplyHealth_GOOD = 1;
+  PowerSupplyHealth_OVERHEAT = 2;
+  PowerSupplyHealth_DEAD = 3;
+  PowerSupplyHealth_OVERVOLTAGE = 4;
+  PowerSupplyHealth_UNSPEC_FAILURE = 5;
+  PowerSupplyHealth_COLD = 6;
+  PowerSupplyHealth_WATCHDOG_TIMER_EXPIRE = 7;
+  PowerSupplyHealth_SAFETY_TIMER_EXPIRE = 8;
+}
+
+
+/**
+ * @brief 电池技术
+ */
+enum PowerSupplyTechnology{
+  PowerSupplyTechnology_UNDEFINED = 0;
+  PowerSupplyTechnology_NIMH = 1;
+  PowerSupplyTechnology_LION = 2;
+  PowerSupplyTechnology_LIPO = 3;
+  PowerSupplyTechnology_LIFE = 4;
+  PowerSupplyTechnology_NICD = 5;
+  PowerSupplyTechnology_LIMN = 6;
+}
+
+/**
+ * @brief 电池状态 对应ROS中的: sensor_msgs/BatteryState.msg
+ */
+message BatteryState{
+    // 时间辍
+    Timestamp timestamp = 1;
+    
+    // 电池电压
+    float voltage = 2;
+
+    // 电流
+    float current = 3;
+
+    // 电量
+    float charge = 4;
+
+    // 电池容量
+    float capacity = 5;
+
+    // 设计容量
+    float design_capacity = 6;
+
+    // 电池百分比
+    float percentage = 7;
+
+    // 电池状态
+    PowerSupplyStatus power_supply_status = 8;
+
+    // 电池健康状态
+    PowerSupplyHealth power_supply_health = 9;
+
+    // 电池技术
+    PowerSupplyTechnology power_supply_technology = 10;
+
+    // 电池单元电压
+    repeated float cell_voltage = 11;
+
+    // 电池位置
+    string location = 12;
+
+    // 电池序列号
+    string serial_number = 13;
+    
+    // 电池功率
+    float power = 14;
+
+    // 电池温度
+    float temperature = 15;
+};

data_gen_dependencies/aimdk/protocol/common/box.proto

@@ -0,0 +1,13 @@
+syntax = "proto3";
+package aimdk.protocol;
+
+
+/**
+ * @brief A box in a 2D space.
+ */
+message Box {
+  float x1 = 1;
+  float y1 = 2;
+  float x2 = 3;
+  float y2 = 4;
+}

data_gen_dependencies/aimdk/protocol/common/common.proto

@@ -0,0 +1,20 @@
+syntax = "proto3";
+package aimdk.protocol;
+
+import public "aimdk/protocol/common/timestamp.proto";
+import public "aimdk/protocol/common/vec2.proto";
+import public "aimdk/protocol/common/vec3.proto";
+import public "aimdk/protocol/common/odometry.proto";
+import public "aimdk/protocol/common/quaternion.proto";
+import public "aimdk/protocol/common/se2_pose.proto";
+import public "aimdk/protocol/common/se3_pose.proto";
+import public "aimdk/protocol/common/se2_velocity.proto";
+import public "aimdk/protocol/common/se3_velocity.proto";
+import public "aimdk/protocol/common/se2_acceleration.proto";
+import public "aimdk/protocol/common/se3_acceleration.proto";
+import public "aimdk/protocol/common/se3_trajectory.proto";
+import public "aimdk/protocol/common/wrench.proto";
+import public "aimdk/protocol/common/twist.proto";
+import public "aimdk/protocol/common/pixel_pose.proto";
+import public "aimdk/protocol/common/image.proto";
+import public "aimdk/protocol/common/rpc.proto";

data_gen_dependencies/aimdk/protocol/common/header.proto

@@ -0,0 +1,71 @@
+syntax = "proto3";
+package aimdk.protocol;
+
+import "aimdk/protocol/common/timestamp.proto";
+
+
+/**
+ * @brief 一般消息的 header
+ */
+message Header {
+  // sequence ID: consecutively increasing ID
+  uint32 seq = 1;
+
+  // timestamp
+  Timestamp timestamp = 2;
+
+  // Frame this data is associated with
+  string frame_id = 3;
+}
+
+/**
+ * @brief RPC 请求体的 header
+ */
+message RequestHeader {
+  // timestamp
+  Timestamp timestamp = 1;
+}
+
+/**
+ * @brief RPC 响应体的 header
+ */
+message ResponseHeader {
+  // 处理结果
+  // - 为0: 成功
+  // - 非0: 失败
+  uint64 code = 1;
+
+  // 处理结果描述
+  string msg  = 2;
+
+  // 时间戳
+  Timestamp timestamp = 3;
+}
+
+/**
+ * @brief 可阻塞的 RPC 请求体的 header
+ */
+message BlockableRequestHeader {
+  // 时间戳
+  Timestamp timestamp = 1;
+
+  // 是否阻塞调用（默认 false）
+  bool blocked = 2;
+}
+
+message BlockableResponseHeader {
+  // 处理结果
+  // - 为0: 成功
+  // - 非0: 失败
+  uint64 code = 1;
+
+  // 处理结果描述
+  string msg  = 2;
+
+  // 时间戳
+  Timestamp timestamp = 3;
+
+  // 请求关联的 rpc id，若设置了非阻塞模式，该字段可用于调用者向执行者定期查询 rpc 执行情况，
+  // 该值需由执行者维护并设置，默认为零无效。
+  uint64 id = 4;
+}

data_gen_dependencies/aimdk/protocol/common/header_pb2.py

@@ -0,0 +1,35 @@
+# -*- coding: utf-8 -*-
+# Generated by the protocol buffer compiler.  DO NOT EDIT!
+# source: aimdk/protocol/common/header.proto
+# Protobuf Python Version: 4.25.1
+"""Generated protocol buffer code."""
+from google.protobuf import descriptor as _descriptor
+from google.protobuf import descriptor_pool as _descriptor_pool
+from google.protobuf import symbol_database as _symbol_database
+from google.protobuf.internal import builder as _builder
+# @@protoc_insertion_point(imports)
+
+_sym_db = _symbol_database.Default()
+
+
+from aimdk.protocol.common import timestamp_pb2 as aimdk_dot_protocol_dot_common_dot_timestamp__pb2
+
+
+DESCRIPTOR = _descriptor_pool.Default().AddSerializedFile(b'\n\"aimdk/protocol/common/header.proto\x12\x0e\x61imdk.protocol\x1a%aimdk/protocol/common/timestamp.proto\"U\n\x06Header\x12\x0b\n\x03seq\x18\x01 \x01(\r\x12,\n\ttimestamp\x18\x02 \x01(\x0b\x32\x19.aimdk.protocol.Timestamp\x12\x10\n\x08\x66rame_id\x18\x03 \x01(\t\"=\n\rRequestHeader\x12,\n\ttimestamp\x18\x01 \x01(\x0b\x32\x19.aimdk.protocol.Timestamp\"Y\n\x0eResponseHeader\x12\x0c\n\x04\x63ode\x18\x01 \x01(\x04\x12\x0b\n\x03msg\x18\x02 \x01(\t\x12,\n\ttimestamp\x18\x03 \x01(\x0b\x32\x19.aimdk.protocol.Timestamp\"W\n\x16\x42lockableRequestHeader\x12,\n\ttimestamp\x18\x01 \x01(\x0b\x32\x19.aimdk.protocol.Timestamp\x12\x0f\n\x07\x62locked\x18\x02 \x01(\x08\"n\n\x17\x42lockableResponseHeader\x12\x0c\n\x04\x63ode\x18\x01 \x01(\x04\x12\x0b\n\x03msg\x18\x02 \x01(\t\x12,\n\ttimestamp\x18\x03 \x01(\x0b\x32\x19.aimdk.protocol.Timestamp\x12\n\n\x02id\x18\x04 \x01(\x04\x62\x06proto3')
+
+_globals = globals()
+_builder.BuildMessageAndEnumDescriptors(DESCRIPTOR, _globals)
+_builder.BuildTopDescriptorsAndMessages(DESCRIPTOR, 'aimdk.protocol.common.header_pb2', _globals)
+if _descriptor._USE_C_DESCRIPTORS == False:
+  DESCRIPTOR._options = None
+  _globals['_HEADER']._serialized_start=93
+  _globals['_HEADER']._serialized_end=178
+  _globals['_REQUESTHEADER']._serialized_start=180
+  _globals['_REQUESTHEADER']._serialized_end=241
+  _globals['_RESPONSEHEADER']._serialized_start=243
+  _globals['_RESPONSEHEADER']._serialized_end=332
+  _globals['_BLOCKABLEREQUESTHEADER']._serialized_start=334
+  _globals['_BLOCKABLEREQUESTHEADER']._serialized_end=421
+  _globals['_BLOCKABLERESPONSEHEADER']._serialized_start=423
+  _globals['_BLOCKABLERESPONSEHEADER']._serialized_end=533
+# @@protoc_insertion_point(module_scope)

data_gen_dependencies/aimdk/protocol/common/image.proto

@@ -0,0 +1,22 @@
+syntax = "proto3";
+package aimdk.protocol;
+
+import "aimdk/protocol/common/timestamp.proto";
+
+
+/**
+ * @brief 给客户端推送视频流的图像数据格式
+ */
+message PubImageData {
+  // 单帧高
+  int32 height = 1;
+
+  // 单帧宽
+  int32 width = 2;
+
+  // 时间戳
+  Timestamp timestamp = 3;
+
+  // 图片数据
+  bytes image_data = 4;
+}

data_gen_dependencies/aimdk/protocol/common/joint.proto

@@ -0,0 +1,100 @@
+syntax = "proto3";
+package aimdk.protocol;
+
+
+// 关节信息
+message JointState {
+  /**
+   * 关节名称
+   */
+  string name = 1;
+
+  /**
+   * 关节消息的序号
+   */
+  uint32 sequence = 2;
+
+  /**
+   * 关节角度，单位：弧度 or m
+   */
+  double position = 3;
+
+  /**
+   * 关节角速度，单位：弧度/秒 or m/s
+   */
+  double velocity = 4;
+
+  /**
+   * 关节扭矩，单位：N or N*m
+   */
+  double effort = 5;
+}
+
+message JointCommand {
+  /**
+   * 关节名称
+   */
+  string name = 1;
+
+  /**
+   * 关节消息的序号
+   */
+  uint32 sequence = 2;
+
+  /**
+   * 关节角度，单位：弧度 or m
+   */
+  double position = 3;
+
+  /**
+   * 关节角速度，单位：弧度/秒 or m/s
+   */
+  double velocity = 4;
+
+  /**
+   * 关节扭矩，单位：N
+   */
+  double effort = 5;
+
+  /**
+   * 阻尼，单位： N·m/rad or N/m
+   */
+  double stiffness = 6;
+
+  /**
+   * 阻尼单位： N·s/m or N·m·s/rad
+   */
+  double damping = 7;
+}
+
+message JointParam {
+  /**
+   * 关节名称
+   */
+  string name = 1;
+
+  /**
+   * 关节位置上限，单位：弧度或者米
+   */
+  double position_upper_limit = 2;
+
+  /**
+   * 关节位置下限，单位：弧度或者米
+   */
+  double position_lower_limit = 3;
+
+  /**
+   * 关节速度限制，单位：弧度/秒或者米/秒
+   */
+  double velocity_limit = 4;
+
+  /**
+   * 关节加速度限制，单位：弧度/秒^2或者米/秒^2
+   */
+  double acceleration_limit = 5;
+
+  /**
+   * 关节扭矩限制，单位：N或者N·m
+   */
+  double effort_limit = 6;
+}

data_gen_dependencies/aimdk/protocol/common/joint_pb2.py

@@ -0,0 +1,30 @@
+# -*- coding: utf-8 -*-
+# Generated by the protocol buffer compiler.  DO NOT EDIT!
+# source: aimdk/protocol/common/joint.proto
+# Protobuf Python Version: 4.25.1
+"""Generated protocol buffer code."""
+from google.protobuf import descriptor as _descriptor
+from google.protobuf import descriptor_pool as _descriptor_pool
+from google.protobuf import symbol_database as _symbol_database
+from google.protobuf.internal import builder as _builder
+# @@protoc_insertion_point(imports)
+
+_sym_db = _symbol_database.Default()
+
+
+
+
+DESCRIPTOR = _descriptor_pool.Default().AddSerializedFile(b'\n!aimdk/protocol/common/joint.proto\x12\x0e\x61imdk.protocol\"`\n\nJointState\x12\x0c\n\x04name\x18\x01 \x01(\t\x12\x10\n\x08sequence\x18\x02 \x01(\r\x12\x10\n\x08position\x18\x03 \x01(\x01\x12\x10\n\x08velocity\x18\x04 \x01(\x01\x12\x0e\n\x06\x65\x66\x66ort\x18\x05 \x01(\x01\"\x86\x01\n\x0cJointCommand\x12\x0c\n\x04name\x18\x01 \x01(\t\x12\x10\n\x08sequence\x18\x02 \x01(\r\x12\x10\n\x08position\x18\x03 \x01(\x01\x12\x10\n\x08velocity\x18\x04 \x01(\x01\x12\x0e\n\x06\x65\x66\x66ort\x18\x05 \x01(\x01\x12\x11\n\tstiffness\x18\x06 \x01(\x01\x12\x0f\n\x07\x64\x61mping\x18\x07 \x01(\x01\"\xa0\x01\n\nJointParam\x12\x0c\n\x04name\x18\x01 \x01(\t\x12\x1c\n\x14position_upper_limit\x18\x02 \x01(\x01\x12\x1c\n\x14position_lower_limit\x18\x03 \x01(\x01\x12\x16\n\x0evelocity_limit\x18\x04 \x01(\x01\x12\x1a\n\x12\x61\x63\x63\x65leration_limit\x18\x05 \x01(\x01\x12\x14\n\x0c\x65\x66\x66ort_limit\x18\x06 \x01(\x01\x62\x06proto3')
+
+_globals = globals()
+_builder.BuildMessageAndEnumDescriptors(DESCRIPTOR, _globals)
+_builder.BuildTopDescriptorsAndMessages(DESCRIPTOR, 'aimdk.protocol.common.joint_pb2', _globals)
+if _descriptor._USE_C_DESCRIPTORS == False:
+  DESCRIPTOR._options = None
+  _globals['_JOINTSTATE']._serialized_start=53
+  _globals['_JOINTSTATE']._serialized_end=149
+  _globals['_JOINTCOMMAND']._serialized_start=152
+  _globals['_JOINTCOMMAND']._serialized_end=286
+  _globals['_JOINTPARAM']._serialized_start=289
+  _globals['_JOINTPARAM']._serialized_end=449
+# @@protoc_insertion_point(module_scope)

data_gen_dependencies/aimdk/protocol/common/odometry.proto

@@ -0,0 +1,17 @@
+syntax = "proto3";
+package aimdk.protocol;
+
+import public "aimdk/protocol/common/timestamp.proto";
+import public "aimdk/protocol/common/se3_pose.proto";
+import public "aimdk/protocol/common/twist.proto";
+
+
+/**
+ * odometry
+ */
+message Odometry {
+  Timestamp timestamp = 1;
+  string child_frame_id = 2;
+  SE3Pose pose = 3;
+  Twist twist = 4;
+}

data_gen_dependencies/aimdk/protocol/common/pixel_pose.proto

@@ -0,0 +1,31 @@
+syntax = "proto3";
+package aimdk.protocol;
+
+
+/**
+ * @brief 2d 像素坐标
+ */
+message Pixel {
+  // 像素横坐标
+  int32 u = 1;
+
+  // 像素纵坐标
+  int32 v = 2;
+}
+
+/**
+ * @brief 2d 像素位姿
+ * @note 坐标系示意
+ *      +--------> u
+ *      | - (angle)       
+ *      |   -    
+ *      |     * 
+ *    v V        
+ */
+message PixelPose {
+  // 2d 像素坐标
+  Pixel position = 1;
+
+  // 角度从u轴转向v轴的角度，单位：弧度
+  double angle = 2;
+}

data_gen_dependencies/aimdk/protocol/common/qr_pose.proto

@@ -0,0 +1,13 @@
+syntax = "proto3";
+package aimdk.protocol;
+
+import public "aimdk/protocol/common/se3_pose.proto";
+
+
+/**
+ * QR位姿
+ */
+message QRPose {
+  int32 qr_code = 1; // 二维码值
+  SE3Pose pose = 2; // 6D位姿
+}

data_gen_dependencies/aimdk/protocol/common/quaternion.proto

@@ -0,0 +1,13 @@
+syntax = "proto3";
+package aimdk.protocol;
+
+
+/**
+ * 四元数
+ */
+message Quaternion {
+  double x = 1;
+  double y = 2;
+  double z = 3;
+  double w = 4;
+}

data_gen_dependencies/aimdk/protocol/common/quaternion_pb2.py

@@ -0,0 +1,26 @@
+# -*- coding: utf-8 -*-
+# Generated by the protocol buffer compiler.  DO NOT EDIT!
+# source: aimdk/protocol/common/quaternion.proto
+# Protobuf Python Version: 4.25.1
+"""Generated protocol buffer code."""
+from google.protobuf import descriptor as _descriptor
+from google.protobuf import descriptor_pool as _descriptor_pool
+from google.protobuf import symbol_database as _symbol_database
+from google.protobuf.internal import builder as _builder
+# @@protoc_insertion_point(imports)
+
+_sym_db = _symbol_database.Default()
+
+
+
+
+DESCRIPTOR = _descriptor_pool.Default().AddSerializedFile(b'\n&aimdk/protocol/common/quaternion.proto\x12\x0e\x61imdk.protocol\"8\n\nQuaternion\x12\t\n\x01x\x18\x01 \x01(\x01\x12\t\n\x01y\x18\x02 \x01(\x01\x12\t\n\x01z\x18\x03 \x01(\x01\x12\t\n\x01w\x18\x04 \x01(\x01\x62\x06proto3')
+
+_globals = globals()
+_builder.BuildMessageAndEnumDescriptors(DESCRIPTOR, _globals)
+_builder.BuildTopDescriptorsAndMessages(DESCRIPTOR, 'aimdk.protocol.common.quaternion_pb2', _globals)
+if _descriptor._USE_C_DESCRIPTORS == False:
+  DESCRIPTOR._options = None
+  _globals['_QUATERNION']._serialized_start=58
+  _globals['_QUATERNION']._serialized_end=114
+# @@protoc_insertion_point(module_scope)

data_gen_dependencies/aimdk/protocol/common/rpc.proto

@@ -0,0 +1,54 @@
+syntax = "proto3";
+package aimdk.protocol;
+
+import public "aimdk/protocol/common/timestamp.proto";
+import public "aimdk/protocol/common/header.proto";
+
+/**
+ * 状态定义
+ */
+enum CommonState {
+  CommonState_UNKNOWN = 0;      // 未知
+  CommonState_SUCCESS = 1;      // 成功
+  CommonState_FAILURE = 2;      // 失败
+  CommonState_ABORTED = 3;      // 中止
+  CommonState_TIMEOUT = 4;      // 超时
+  CommonState_INVALID = 5;      // 无效
+  CommonState_IN_MANUAL = 6;    // 手动
+  CommonState_NOT_READY = 100;  // 未就绪
+  CommonState_PENDING = 200;    // 等待中
+  CommonState_CREATED = 300;    // 已创建
+  CommonState_RUNNING = 400;    // 运行中
+}
+
+/**
+ * 通用请求类型
+ */
+message CommonRequest {
+  RequestHeader header = 1;
+}
+
+/**
+ * 通用返回类型
+ */
+message CommonResponse {
+  ResponseHeader header = 1;
+  CommonState state = 2;
+}
+
+/**
+ * 通用Task请求类型
+ */
+message CommonTaskRequest {
+  RequestHeader header = 1;
+  uint64 task_id = 2;
+}
+
+/**
+ * 通用Task返回类型
+ */
+message CommonTaskResponse {
+  ResponseHeader header = 1;
+  uint64 task_id = 2;
+  CommonState state = 3;
+}

data_gen_dependencies/aimdk/protocol/common/rpc_pb2.py

@@ -0,0 +1,38 @@
+# -*- coding: utf-8 -*-
+# Generated by the protocol buffer compiler.  DO NOT EDIT!
+# source: aimdk/protocol/common/rpc.proto
+# Protobuf Python Version: 4.25.1
+"""Generated protocol buffer code."""
+from google.protobuf import descriptor as _descriptor
+from google.protobuf import descriptor_pool as _descriptor_pool
+from google.protobuf import symbol_database as _symbol_database
+from google.protobuf.internal import builder as _builder
+# @@protoc_insertion_point(imports)
+
+_sym_db = _symbol_database.Default()
+
+
+from aimdk.protocol.common import timestamp_pb2 as aimdk_dot_protocol_dot_common_dot_timestamp__pb2
+from aimdk.protocol.common import header_pb2 as aimdk_dot_protocol_dot_common_dot_header__pb2
+
+from aimdk.protocol.common.timestamp_pb2 import *
+from aimdk.protocol.common.header_pb2 import *
+
+DESCRIPTOR = _descriptor_pool.Default().AddSerializedFile(b'\n\x1f\x61imdk/protocol/common/rpc.proto\x12\x0e\x61imdk.protocol\x1a%aimdk/protocol/common/timestamp.proto\x1a\"aimdk/protocol/common/header.proto\">\n\rCommonRequest\x12-\n\x06header\x18\x01 \x01(\x0b\x32\x1d.aimdk.protocol.RequestHeader\"l\n\x0e\x43ommonResponse\x12.\n\x06header\x18\x01 \x01(\x0b\x32\x1e.aimdk.protocol.ResponseHeader\x12*\n\x05state\x18\x02 \x01(\x0e\x32\x1b.aimdk.protocol.CommonState\"S\n\x11\x43ommonTaskRequest\x12-\n\x06header\x18\x01 \x01(\x0b\x32\x1d.aimdk.protocol.RequestHeader\x12\x0f\n\x07task_id\x18\x02 \x01(\x04\"\x81\x01\n\x12\x43ommonTaskResponse\x12.\n\x06header\x18\x01 \x01(\x0b\x32\x1e.aimdk.protocol.ResponseHeader\x12\x0f\n\x07task_id\x18\x02 \x01(\x04\x12*\n\x05state\x18\x03 \x01(\x0e\x32\x1b.aimdk.protocol.CommonState*\xa7\x02\n\x0b\x43ommonState\x12\x17\n\x13\x43ommonState_UNKNOWN\x10\x00\x12\x17\n\x13\x43ommonState_SUCCESS\x10\x01\x12\x17\n\x13\x43ommonState_FAILURE\x10\x02\x12\x17\n\x13\x43ommonState_ABORTED\x10\x03\x12\x17\n\x13\x43ommonState_TIMEOUT\x10\x04\x12\x17\n\x13\x43ommonState_INVALID\x10\x05\x12\x19\n\x15\x43ommonState_IN_MANUAL\x10\x06\x12\x19\n\x15\x43ommonState_NOT_READY\x10\x64\x12\x18\n\x13\x43ommonState_PENDING\x10\xc8\x01\x12\x18\n\x13\x43ommonState_CREATED\x10\xac\x02\x12\x18\n\x13\x43ommonState_RUNNING\x10\x90\x03P\x00P\x01\x62\x06proto3')
+
+_globals = globals()
+_builder.BuildMessageAndEnumDescriptors(DESCRIPTOR, _globals)
+_builder.BuildTopDescriptorsAndMessages(DESCRIPTOR, 'aimdk.protocol.common.rpc_pb2', _globals)
+if _descriptor._USE_C_DESCRIPTORS == False:
+  DESCRIPTOR._options = None
+  _globals['_COMMONSTATE']._serialized_start=518
+  _globals['_COMMONSTATE']._serialized_end=813
+  _globals['_COMMONREQUEST']._serialized_start=126
+  _globals['_COMMONREQUEST']._serialized_end=188
+  _globals['_COMMONRESPONSE']._serialized_start=190
+  _globals['_COMMONRESPONSE']._serialized_end=298
+  _globals['_COMMONTASKREQUEST']._serialized_start=300
+  _globals['_COMMONTASKREQUEST']._serialized_end=383
+  _globals['_COMMONTASKRESPONSE']._serialized_start=386
+  _globals['_COMMONTASKRESPONSE']._serialized_end=515
+# @@protoc_insertion_point(module_scope)

data_gen_dependencies/aimdk/protocol/common/rpy.proto

@@ -0,0 +1,13 @@
+syntax = "proto3";
+package aimdk.protocol;
+
+
+/**
+ * @brief RPY角度姿态表示
+ */
+message Rpy {
+  double rw = 1;
+  double rx = 2;  // 绕固定轴X旋转角度，单位：rad
+  double ry = 3;  // 绕固定轴Y旋转角度，单位：rad
+  double rz = 4;  // 绕固定轴Z旋转角度，单位：rad
+}

data_gen_dependencies/aimdk/protocol/common/rpy_pb2.py

@@ -0,0 +1,26 @@
+# -*- coding: utf-8 -*-
+# Generated by the protocol buffer compiler.  DO NOT EDIT!
+# source: aimdk/protocol/common/rpy.proto
+# Protobuf Python Version: 4.25.1
+"""Generated protocol buffer code."""
+from google.protobuf import descriptor as _descriptor
+from google.protobuf import descriptor_pool as _descriptor_pool
+from google.protobuf import symbol_database as _symbol_database
+from google.protobuf.internal import builder as _builder
+# @@protoc_insertion_point(imports)
+
+_sym_db = _symbol_database.Default()
+
+
+
+
+DESCRIPTOR = _descriptor_pool.Default().AddSerializedFile(b'\n\x1f\x61imdk/protocol/common/rpy.proto\x12\x0e\x61imdk.protocol\"5\n\x03Rpy\x12\n\n\x02rw\x18\x01 \x01(\x01\x12\n\n\x02rx\x18\x02 \x01(\x01\x12\n\n\x02ry\x18\x03 \x01(\x01\x12\n\n\x02rz\x18\x04 \x01(\x01\x62\x06proto3')
+
+_globals = globals()
+_builder.BuildMessageAndEnumDescriptors(DESCRIPTOR, _globals)
+_builder.BuildTopDescriptorsAndMessages(DESCRIPTOR, 'aimdk.protocol.common.rpy_pb2', _globals)
+if _descriptor._USE_C_DESCRIPTORS == False:
+  DESCRIPTOR._options = None
+  _globals['_RPY']._serialized_start=51
+  _globals['_RPY']._serialized_end=104
+# @@protoc_insertion_point(module_scope)

data_gen_dependencies/aimdk/protocol/common/rpy_pb2_grpc.py

@@ -0,0 +1,4 @@
+# Generated by the gRPC Python protocol compiler plugin. DO NOT EDIT!
+"""Client and server classes corresponding to protobuf-defined services."""
+import grpc
+

data_gen_dependencies/aimdk/protocol/common/se2_acceleration.proto

@@ -0,0 +1,13 @@
+syntax = "proto3";
+package aimdk.protocol;
+
+import public "aimdk/protocol/common/vec2.proto";
+
+
+/**
+ * SE2 加速度
+ */
+message SE2Acceleration {
+  Vec2 linear = 1;   // 线加速度，单位：米/秒^2
+  Vec2 angular = 2;  // 角加速度，单位：弧度/秒^2
+}

data_gen_dependencies/aimdk/protocol/common/se2_pose.proto

@@ -0,0 +1,13 @@
+syntax = "proto3";
+package aimdk.protocol;
+
+import public "aimdk/protocol/common/vec2.proto";
+
+
+/**
+ * SE2位姿
+ */
+message SE2Pose {
+  Vec2 position = 1;  // 2d 位姿，单位：米
+  double angle = 2;   // 角度，单位：弧度
+}

data_gen_dependencies/aimdk/protocol/common/se2_velocity.proto

@@ -0,0 +1,13 @@
+syntax = "proto3";
+package aimdk.protocol;
+
+import public "aimdk/protocol/common/vec2.proto";
+
+
+/**
+ * SE2 速度
+ */
+message SE2Velocity {
+  Vec2 linear = 1;   // 线速度，单位：米/秒
+  Vec2 angular = 2;  // 角速度，单位：弧度/秒
+}

data_gen_dependencies/aimdk/protocol/common/se3_acceleration.proto

@@ -0,0 +1,14 @@
+syntax = "proto3";
+package aimdk.protocol;
+
+import public "aimdk/protocol/common/vec3.proto";
+import public "aimdk/protocol/common/quaternion.proto";
+
+
+/**
+ * SE3 加速度
+ */
+message SE3Acceleration {
+  Vec3 linear = 1;   // 线加速度，单位：米/秒^2
+  Vec3 angular = 2;  // 角加速度，单位：弧度/秒^2
+}

data_gen_dependencies/aimdk/protocol/common/se3_pose.proto

@@ -0,0 +1,23 @@
+syntax = "proto3";
+package aimdk.protocol;
+
+import public "aimdk/protocol/common/vec3.proto";
+import public "aimdk/protocol/common/quaternion.proto";
+import public "aimdk/protocol/common/rpy.proto";
+
+
+/**
+ * SE3位姿（四元数姿态表示）
+ */
+message SE3Pose {
+  Vec3 position = 1;           // 3d 位姿，单位：米
+  Quaternion orientation = 2;  // 四元数
+}
+
+/**
+ * SE3位姿（RPY角度表示）
+ */
+message SE3RpyPose {
+  Vec3 position = 1;       // 3d 位姿，单位：米
+  Rpy rpy = 2;             // RPY角度
+}

data_gen_dependencies/aimdk/protocol/common/se3_pose_pb2.py

@@ -0,0 +1,34 @@
+# -*- coding: utf-8 -*-
+# Generated by the protocol buffer compiler.  DO NOT EDIT!
+# source: aimdk/protocol/common/se3_pose.proto
+# Protobuf Python Version: 4.25.1
+"""Generated protocol buffer code."""
+from google.protobuf import descriptor as _descriptor
+from google.protobuf import descriptor_pool as _descriptor_pool
+from google.protobuf import symbol_database as _symbol_database
+from google.protobuf.internal import builder as _builder
+# @@protoc_insertion_point(imports)
+
+_sym_db = _symbol_database.Default()
+
+
+from aimdk.protocol.common import vec3_pb2 as aimdk_dot_protocol_dot_common_dot_vec3__pb2
+from aimdk.protocol.common import quaternion_pb2 as aimdk_dot_protocol_dot_common_dot_quaternion__pb2
+from aimdk.protocol.common import rpy_pb2 as aimdk_dot_protocol_dot_common_dot_rpy__pb2
+
+from aimdk.protocol.common.vec3_pb2 import *
+from aimdk.protocol.common.quaternion_pb2 import *
+from aimdk.protocol.common.rpy_pb2 import *
+
+DESCRIPTOR = _descriptor_pool.Default().AddSerializedFile(b'\n$aimdk/protocol/common/se3_pose.proto\x12\x0e\x61imdk.protocol\x1a aimdk/protocol/common/vec3.proto\x1a&aimdk/protocol/common/quaternion.proto\x1a\x1f\x61imdk/protocol/common/rpy.proto\"b\n\x07SE3Pose\x12&\n\x08position\x18\x01 \x01(\x0b\x32\x14.aimdk.protocol.Vec3\x12/\n\x0borientation\x18\x02 \x01(\x0b\x32\x1a.aimdk.protocol.Quaternion\"V\n\nSE3RpyPose\x12&\n\x08position\x18\x01 \x01(\x0b\x32\x14.aimdk.protocol.Vec3\x12 \n\x03rpy\x18\x02 \x01(\x0b\x32\x13.aimdk.protocol.RpyP\x00P\x01P\x02\x62\x06proto3')
+
+_globals = globals()
+_builder.BuildMessageAndEnumDescriptors(DESCRIPTOR, _globals)
+_builder.BuildTopDescriptorsAndMessages(DESCRIPTOR, 'aimdk.protocol.common.se3_pose_pb2', _globals)
+if _descriptor._USE_C_DESCRIPTORS == False:
+  DESCRIPTOR._options = None
+  _globals['_SE3POSE']._serialized_start=163
+  _globals['_SE3POSE']._serialized_end=261
+  _globals['_SE3RPYPOSE']._serialized_start=263
+  _globals['_SE3RPYPOSE']._serialized_end=349
+# @@protoc_insertion_point(module_scope)

data_gen_dependencies/aimdk/protocol/common/se3_trajectory.proto

@@ -0,0 +1,25 @@
+syntax = "proto3";
+package aimdk.protocol;
+
+import public "aimdk/protocol/common/se3_acceleration.proto";
+import public "aimdk/protocol/common/se3_velocity.proto";
+import public "aimdk/protocol/common/se3_pose.proto";
+import public "aimdk/protocol/common/timestamp.proto";
+
+
+/**
+ * SE3轨迹点
+ */
+message SE3TrajectoryPoint {
+  Timestamp timestamp = 1;
+  SE3Pose pose = 2;
+  SE3Velocity velocity = 3;
+  SE3Acceleration acceleration = 4;
+}
+
+/**
+ * SE3轨迹
+ */
+message SE3Trajectory {
+  repeated SE3TrajectoryPoint points = 1;
+}

data_gen_dependencies/aimdk/protocol/common/se3_velocity.proto

@@ -0,0 +1,13 @@
+syntax = "proto3";
+package aimdk.protocol;
+
+import public "aimdk/protocol/common/vec3.proto";
+
+
+/**
+ * SE3 速度
+ */
+message SE3Velocity {
+  Vec3 linear = 1;   // 线速度，单位：米/秒
+  Vec3 angular = 2;  // 角速度，单位：弧度/秒
+}

data_gen_dependencies/aimdk/protocol/common/timestamp.proto

@@ -0,0 +1,48 @@
+syntax = "proto3";
+package aimdk.protocol;
+
+
+/**
+ * 时间戳
+ */
+message Timestamp {
+  // Represents seconds of UTC time since Unix epoch
+  // 1970-01-01T00:00:00Z. Must be from 0001-01-01T00:00:00Z to
+  // 9999-12-31T23:59:59Z inclusive.
+  int64 seconds = 1;
+
+  // Non-negative fractions of a second at nanosecond resolution. Negative
+  // second values with fractions must still have non-negative nanos values
+  // that count forward in time. Must be from 0 to 999,999,999
+  // inclusive.
+  int32 nanos = 2;
+
+  // miliseconds since epoch for counts using by frontend.
+  int64 ms_since_epoch = 3;
+}
+
+// # Examples
+//
+// Example 1: Compute Timestamp from POSIX `time()`.
+//
+//     Timestamp timestamp;
+//     timestamp.set_seconds(time(NULL));
+//     timestamp.set_nanos(0);
+//
+// Example 2: Compute Timestamp from POSIX `gettimeofday()`.
+//
+//     struct timeval tv;
+//     gettimeofday(&tv, NULL);
+//
+//     Timestamp timestamp;
+//     timestamp.set_seconds(tv.tv_sec);
+//     timestamp.set_nanos(tv.tv_usec * 1000);
+//
+// Example 3: Compute Timestamp from C++ `std::chrono::system_clock::now()`.
+//
+//     std::chrono::system_clock::time_point tp =
+//       std::chrono::system_clock::now();
+//     std::chrono::duration<long int, std::nano> diff = tp.time_since_epoch();
+//     Timestamp timestamp;
+//     timestamp.set_seconds(diff.count() / 1e9);
+//     timestamp.set_nanos(diff.count() % 1e9);

data_gen_dependencies/aimdk/protocol/common/timestamp_pb2.py

@@ -0,0 +1,26 @@
+# -*- coding: utf-8 -*-
+# Generated by the protocol buffer compiler.  DO NOT EDIT!
+# source: aimdk/protocol/common/timestamp.proto
+# Protobuf Python Version: 4.25.1
+"""Generated protocol buffer code."""
+from google.protobuf import descriptor as _descriptor
+from google.protobuf import descriptor_pool as _descriptor_pool
+from google.protobuf import symbol_database as _symbol_database
+from google.protobuf.internal import builder as _builder
+# @@protoc_insertion_point(imports)
+
+_sym_db = _symbol_database.Default()
+
+
+
+
+DESCRIPTOR = _descriptor_pool.Default().AddSerializedFile(b'\n%aimdk/protocol/common/timestamp.proto\x12\x0e\x61imdk.protocol\"C\n\tTimestamp\x12\x0f\n\x07seconds\x18\x01 \x01(\x03\x12\r\n\x05nanos\x18\x02 \x01(\x05\x12\x16\n\x0ems_since_epoch\x18\x03 \x01(\x03\x62\x06proto3')
+
+_globals = globals()
+_builder.BuildMessageAndEnumDescriptors(DESCRIPTOR, _globals)
+_builder.BuildTopDescriptorsAndMessages(DESCRIPTOR, 'aimdk.protocol.common.timestamp_pb2', _globals)
+if _descriptor._USE_C_DESCRIPTORS == False:
+  DESCRIPTOR._options = None
+  _globals['_TIMESTAMP']._serialized_start=57
+  _globals['_TIMESTAMP']._serialized_end=124
+# @@protoc_insertion_point(module_scope)

data_gen_dependencies/aimdk/protocol/common/twist.proto

@@ -0,0 +1,22 @@
+syntax = "proto3";
+package aimdk.protocol;
+
+import public "aimdk/protocol/common/vec3.proto";
+import public "aimdk/protocol/common/timestamp.proto";
+
+
+/**
+ * 速度
+ */
+message Twist {
+  Vec3 linear = 1;
+  Vec3 angular = 2;
+}
+
+/**
+ * 速度 带时间戳
+ */
+message TwistStamped {
+  Timestamp timestamp = 1;
+  Twist twist = 2;
+}

data_gen_dependencies/aimdk/protocol/common/vec2.proto

@@ -0,0 +1,11 @@
+syntax = "proto3";
+package aimdk.protocol;
+
+
+/**
+ * @brief 二维向量
+ */
+message Vec2 {
+  double x = 1;
+  double y = 2;
+}

data_gen_dependencies/aimdk/protocol/common/vec3.proto

@@ -0,0 +1,12 @@
+syntax = "proto3";
+package aimdk.protocol;
+
+
+/**
+ * @brief 三维向量
+ */
+message Vec3 {
+  double x = 1;
+  double y = 2;
+  double z = 3;
+}

data_gen_dependencies/aimdk/protocol/common/vec3_pb2.py

@@ -0,0 +1,26 @@
+# -*- coding: utf-8 -*-
+# Generated by the protocol buffer compiler.  DO NOT EDIT!
+# source: aimdk/protocol/common/vec3.proto
+# Protobuf Python Version: 4.25.1
+"""Generated protocol buffer code."""
+from google.protobuf import descriptor as _descriptor
+from google.protobuf import descriptor_pool as _descriptor_pool
+from google.protobuf import symbol_database as _symbol_database
+from google.protobuf.internal import builder as _builder
+# @@protoc_insertion_point(imports)
+
+_sym_db = _symbol_database.Default()
+
+
+
+
+DESCRIPTOR = _descriptor_pool.Default().AddSerializedFile(b'\n aimdk/protocol/common/vec3.proto\x12\x0e\x61imdk.protocol\"\'\n\x04Vec3\x12\t\n\x01x\x18\x01 \x01(\x01\x12\t\n\x01y\x18\x02 \x01(\x01\x12\t\n\x01z\x18\x03 \x01(\x01\x62\x06proto3')
+
+_globals = globals()
+_builder.BuildMessageAndEnumDescriptors(DESCRIPTOR, _globals)
+_builder.BuildTopDescriptorsAndMessages(DESCRIPTOR, 'aimdk.protocol.common.vec3_pb2', _globals)
+if _descriptor._USE_C_DESCRIPTORS == False:
+  DESCRIPTOR._options = None
+  _globals['_VEC3']._serialized_start=52
+  _globals['_VEC3']._serialized_end=91
+# @@protoc_insertion_point(module_scope)

data_gen_dependencies/aimdk/protocol/common/wrench.proto

@@ -0,0 +1,22 @@
+syntax = "proto3";
+package aimdk.protocol;
+
+import public "aimdk/protocol/common/vec3.proto";
+import public "aimdk/protocol/common/timestamp.proto";
+
+
+/**
+ * @brief 力矩
+ */
+message Wrench {
+  Vec3 force = 1;   // force 单位：N
+  Vec3 torque = 2;  // torque 单位：N*m
+}
+
+/**
+ * @brief 力矩，带时间戳
+ */
+message WrenchStamped {
+  Timestamp timestamp = 1;
+  Wrench wrench = 2;
+}

data_gen_dependencies/aimdk/protocol/hal/CMakeLists.txt

@@ -0,0 +1,5 @@
+# -------------------------------------------------------------
+# Protocol: aimdk::protocol::hand
+# -------------------------------------------------------------
+
+aimrte_protocol(DEPEND aimdk::protocol::common ROS2_DEPEND std_msgs sensor_msgs)

data_gen_dependencies/aimdk/protocol/hal/hand/agi_claw.proto

@@ -0,0 +1,20 @@
+syntax = "proto3";
+package aimdk.protocol;
+
+
+// 夹爪手部命令
+message AgiClawHandCommand {
+  uint32 cmd = 1;
+  uint32 pos = 2;
+  uint32 force = 3;
+  uint32 clamp_method = 4;
+  uint32 finger_pos = 5;
+}
+
+// 夹爪手部状态
+message AgiClawHandState {
+  uint32 state = 1;
+  uint32 pos = 2;
+  uint32 temperature = 3;
+  uint32 finger_pos = 4;
+}

data_gen_dependencies/aimdk/protocol/hal/hand/agi_claw_pb2.py

@@ -0,0 +1,28 @@
+# -*- coding: utf-8 -*-
+# Generated by the protocol buffer compiler.  DO NOT EDIT!
+# source: aimdk/protocol/hal/hand/agi_claw.proto
+# Protobuf Python Version: 4.25.1
+"""Generated protocol buffer code."""
+from google.protobuf import descriptor as _descriptor
+from google.protobuf import descriptor_pool as _descriptor_pool
+from google.protobuf import symbol_database as _symbol_database
+from google.protobuf.internal import builder as _builder
+# @@protoc_insertion_point(imports)
+
+_sym_db = _symbol_database.Default()
+
+
+
+
+DESCRIPTOR = _descriptor_pool.Default().AddSerializedFile(b'\n&aimdk/protocol/hal/hand/agi_claw.proto\x12\x0e\x61imdk.protocol\"g\n\x12\x41giClawHandCommand\x12\x0b\n\x03\x63md\x18\x01 \x01(\r\x12\x0b\n\x03pos\x18\x02 \x01(\r\x12\r\n\x05\x66orce\x18\x03 \x01(\r\x12\x14\n\x0c\x63lamp_method\x18\x04 \x01(\r\x12\x12\n\nfinger_pos\x18\x05 \x01(\r\"W\n\x10\x41giClawHandState\x12\r\n\x05state\x18\x01 \x01(\r\x12\x0b\n\x03pos\x18\x02 \x01(\r\x12\x13\n\x0btemperature\x18\x03 \x01(\r\x12\x12\n\nfinger_pos\x18\x04 \x01(\rb\x06proto3')
+
+_globals = globals()
+_builder.BuildMessageAndEnumDescriptors(DESCRIPTOR, _globals)
+_builder.BuildTopDescriptorsAndMessages(DESCRIPTOR, 'aimdk.protocol.hal.hand.agi_claw_pb2', _globals)
+if _descriptor._USE_C_DESCRIPTORS == False:
+  DESCRIPTOR._options = None
+  _globals['_AGICLAWHANDCOMMAND']._serialized_start=58
+  _globals['_AGICLAWHANDCOMMAND']._serialized_end=161
+  _globals['_AGICLAWHANDSTATE']._serialized_start=163
+  _globals['_AGICLAWHANDSTATE']._serialized_end=250
+# @@protoc_insertion_point(module_scope)

data_gen_dependencies/aimdk/protocol/hal/hand/hal_hand_service.proto

@@ -0,0 +1,25 @@
+syntax = "proto3";
+package aimdk.protocol;
+
+import public "aimdk/protocol/common/rpc.proto";
+import public "aimdk/protocol/common/header.proto";
+import public "aimdk/protocol/hal/hand/hand.proto";
+
+
+// 获取机器人手部状态响应
+message HandStateResponse {
+  ResponseHeader header = 1;  // 响应头
+  HandState data = 2;         // 手部状态
+}
+
+// 设置机器人手部控制请求
+message HandCommandRequest {
+  RequestHeader header = 1;  // 请求头
+  HandCommand data = 2;      // 手部控制
+}
+
+// HandService
+service HalHandService {
+  rpc GetHandState(CommonRequest) returns (HandStateResponse);
+  rpc SetHandCommand(HandCommandRequest) returns (CommonResponse);
+}

data_gen_dependencies/aimdk/protocol/hal/hand/hal_hand_service_pb2.py

@@ -0,0 +1,60 @@
+# -*- coding: utf-8 -*-
+# Generated by the protocol buffer compiler.  DO NOT EDIT!
+# source: aimdk/protocol/hal/hand/hal_hand_service.proto
+# Protobuf Python Version: 4.25.1
+"""Generated protocol buffer code."""
+from google.protobuf import descriptor as _descriptor
+from google.protobuf import descriptor_pool as _descriptor_pool
+from google.protobuf import symbol_database as _symbol_database
+from google.protobuf.internal import builder as _builder
+# @@protoc_insertion_point(imports)
+
+_sym_db = _symbol_database.Default()
+
+
+from aimdk.protocol.common import rpc_pb2 as aimdk_dot_protocol_dot_common_dot_rpc__pb2
+try:
+  aimdk_dot_protocol_dot_common_dot_timestamp__pb2 = aimdk_dot_protocol_dot_common_dot_rpc__pb2.aimdk_dot_protocol_dot_common_dot_timestamp__pb2
+except AttributeError:
+  aimdk_dot_protocol_dot_common_dot_timestamp__pb2 = aimdk_dot_protocol_dot_common_dot_rpc__pb2.aimdk.protocol.common.timestamp_pb2
+try:
+  aimdk_dot_protocol_dot_common_dot_header__pb2 = aimdk_dot_protocol_dot_common_dot_rpc__pb2.aimdk_dot_protocol_dot_common_dot_header__pb2
+except AttributeError:
+  aimdk_dot_protocol_dot_common_dot_header__pb2 = aimdk_dot_protocol_dot_common_dot_rpc__pb2.aimdk.protocol.common.header_pb2
+from aimdk.protocol.common import header_pb2 as aimdk_dot_protocol_dot_common_dot_header__pb2
+from aimdk.protocol.hal.hand import hand_pb2 as aimdk_dot_protocol_dot_hal_dot_hand_dot_hand__pb2
+try:
+  aimdk_dot_protocol_dot_common_dot_header__pb2 = aimdk_dot_protocol_dot_hal_dot_hand_dot_hand__pb2.aimdk_dot_protocol_dot_common_dot_header__pb2
+except AttributeError:
+  aimdk_dot_protocol_dot_common_dot_header__pb2 = aimdk_dot_protocol_dot_hal_dot_hand_dot_hand__pb2.aimdk.protocol.common.header_pb2
+try:
+  aimdk_dot_protocol_dot_hal_dot_hand_dot_inspire__pb2 = aimdk_dot_protocol_dot_hal_dot_hand_dot_hand__pb2.aimdk_dot_protocol_dot_hal_dot_hand_dot_inspire__pb2
+except AttributeError:
+  aimdk_dot_protocol_dot_hal_dot_hand_dot_inspire__pb2 = aimdk_dot_protocol_dot_hal_dot_hand_dot_hand__pb2.aimdk.protocol.hal.hand.inspire_pb2
+try:
+  aimdk_dot_protocol_dot_hal_dot_hand_dot_jodell__pb2 = aimdk_dot_protocol_dot_hal_dot_hand_dot_hand__pb2.aimdk_dot_protocol_dot_hal_dot_hand_dot_jodell__pb2
+except AttributeError:
+  aimdk_dot_protocol_dot_hal_dot_hand_dot_jodell__pb2 = aimdk_dot_protocol_dot_hal_dot_hand_dot_hand__pb2.aimdk.protocol.hal.hand.jodell_pb2
+try:
+  aimdk_dot_protocol_dot_hal_dot_hand_dot_agi__claw__pb2 = aimdk_dot_protocol_dot_hal_dot_hand_dot_hand__pb2.aimdk_dot_protocol_dot_hal_dot_hand_dot_agi__claw__pb2
+except AttributeError:
+  aimdk_dot_protocol_dot_hal_dot_hand_dot_agi__claw__pb2 = aimdk_dot_protocol_dot_hal_dot_hand_dot_hand__pb2.aimdk.protocol.hal.hand.agi_claw_pb2
+
+from aimdk.protocol.common.rpc_pb2 import *
+from aimdk.protocol.common.header_pb2 import *
+from aimdk.protocol.hal.hand.hand_pb2 import *
+
+DESCRIPTOR = _descriptor_pool.Default().AddSerializedFile(b'\n.aimdk/protocol/hal/hand/hal_hand_service.proto\x12\x0e\x61imdk.protocol\x1a\x1f\x61imdk/protocol/common/rpc.proto\x1a\"aimdk/protocol/common/header.proto\x1a\"aimdk/protocol/hal/hand/hand.proto\"l\n\x11HandStateResponse\x12.\n\x06header\x18\x01 \x01(\x0b\x32\x1e.aimdk.protocol.ResponseHeader\x12\'\n\x04\x64\x61ta\x18\x02 \x01(\x0b\x32\x19.aimdk.protocol.HandState\"n\n\x12HandCommandRequest\x12-\n\x06header\x18\x01 \x01(\x0b\x32\x1d.aimdk.protocol.RequestHeader\x12)\n\x04\x64\x61ta\x18\x02 \x01(\x0b\x32\x1b.aimdk.protocol.HandCommand2\xb8\x01\n\x0eHalHandService\x12P\n\x0cGetHandState\x12\x1d.aimdk.protocol.CommonRequest\x1a!.aimdk.protocol.HandStateResponse\x12T\n\x0eSetHandCommand\x12\".aimdk.protocol.HandCommandRequest\x1a\x1e.aimdk.protocol.CommonResponseP\x00P\x01P\x02\x62\x06proto3')
+
+_globals = globals()
+_builder.BuildMessageAndEnumDescriptors(DESCRIPTOR, _globals)
+_builder.BuildTopDescriptorsAndMessages(DESCRIPTOR, 'aimdk.protocol.hal.hand.hal_hand_service_pb2', _globals)
+if _descriptor._USE_C_DESCRIPTORS == False:
+  DESCRIPTOR._options = None
+  _globals['_HANDSTATERESPONSE']._serialized_start=171
+  _globals['_HANDSTATERESPONSE']._serialized_end=279
+  _globals['_HANDCOMMANDREQUEST']._serialized_start=281
+  _globals['_HANDCOMMANDREQUEST']._serialized_end=391
+  _globals['_HALHANDSERVICE']._serialized_start=394
+  _globals['_HALHANDSERVICE']._serialized_end=578
+# @@protoc_insertion_point(module_scope)

data_gen_dependencies/aimdk/protocol/hal/hand/hal_hand_service_pb2_grpc.py

@@ -0,0 +1,103 @@
+# Generated by the gRPC Python protocol compiler plugin. DO NOT EDIT!
+"""Client and server classes corresponding to protobuf-defined services."""
+import grpc
+
+from aimdk.protocol.common import rpc_pb2 as aimdk_dot_protocol_dot_common_dot_rpc__pb2
+from aimdk.protocol.hal.hand import hal_hand_service_pb2 as aimdk_dot_protocol_dot_hal_dot_hand_dot_hal__hand__service__pb2
+
+
+class HalHandServiceStub(object):
+    """HandService
+    """
+
+    def __init__(self, channel):
+        """Constructor.
+
+        Args:
+            channel: A grpc.Channel.
+        """
+        self.GetHandState = channel.unary_unary(
+                '/aimdk.protocol.HalHandService/GetHandState',
+                request_serializer=aimdk_dot_protocol_dot_common_dot_rpc__pb2.CommonRequest.SerializeToString,
+                response_deserializer=aimdk_dot_protocol_dot_hal_dot_hand_dot_hal__hand__service__pb2.HandStateResponse.FromString,
+                )
+        self.SetHandCommand = channel.unary_unary(
+                '/aimdk.protocol.HalHandService/SetHandCommand',
+                request_serializer=aimdk_dot_protocol_dot_hal_dot_hand_dot_hal__hand__service__pb2.HandCommandRequest.SerializeToString,
+                response_deserializer=aimdk_dot_protocol_dot_common_dot_rpc__pb2.CommonResponse.FromString,
+                )
+
+
+class HalHandServiceServicer(object):
+    """HandService
+    """
+
+    def GetHandState(self, request, context):
+        """Missing associated documentation comment in .proto file."""
+        context.set_code(grpc.StatusCode.UNIMPLEMENTED)
+        context.set_details('Method not implemented!')
+        raise NotImplementedError('Method not implemented!')
+
+    def SetHandCommand(self, request, context):
+        """Missing associated documentation comment in .proto file."""
+        context.set_code(grpc.StatusCode.UNIMPLEMENTED)
+        context.set_details('Method not implemented!')
+        raise NotImplementedError('Method not implemented!')
+
+
+def add_HalHandServiceServicer_to_server(servicer, server):
+    rpc_method_handlers = {
+            'GetHandState': grpc.unary_unary_rpc_method_handler(
+                    servicer.GetHandState,
+                    request_deserializer=aimdk_dot_protocol_dot_common_dot_rpc__pb2.CommonRequest.FromString,
+                    response_serializer=aimdk_dot_protocol_dot_hal_dot_hand_dot_hal__hand__service__pb2.HandStateResponse.SerializeToString,
+            ),
+            'SetHandCommand': grpc.unary_unary_rpc_method_handler(
+                    servicer.SetHandCommand,
+                    request_deserializer=aimdk_dot_protocol_dot_hal_dot_hand_dot_hal__hand__service__pb2.HandCommandRequest.FromString,
+                    response_serializer=aimdk_dot_protocol_dot_common_dot_rpc__pb2.CommonResponse.SerializeToString,
+            ),
+    }
+    generic_handler = grpc.method_handlers_generic_handler(
+            'aimdk.protocol.HalHandService', rpc_method_handlers)
+    server.add_generic_rpc_handlers((generic_handler,))
+
+
+ # This class is part of an EXPERIMENTAL API.
+class HalHandService(object):
+    """HandService
+    """
+
+    @staticmethod
+    def GetHandState(request,
+            target,
+            options=(),
+            channel_credentials=None,
+            call_credentials=None,
+            insecure=False,
+            compression=None,
+            wait_for_ready=None,
+            timeout=None,
+            metadata=None):
+        return grpc.experimental.unary_unary(request, target, '/aimdk.protocol.HalHandService/GetHandState',
+            aimdk_dot_protocol_dot_common_dot_rpc__pb2.CommonRequest.SerializeToString,
+            aimdk_dot_protocol_dot_hal_dot_hand_dot_hal__hand__service__pb2.HandStateResponse.FromString,
+            options, channel_credentials,
+            insecure, call_credentials, compression, wait_for_ready, timeout, metadata)
+
+    @staticmethod
+    def SetHandCommand(request,
+            target,
+            options=(),
+            channel_credentials=None,
+            call_credentials=None,
+            insecure=False,
+            compression=None,
+            wait_for_ready=None,
+            timeout=None,
+            metadata=None):
+        return grpc.experimental.unary_unary(request, target, '/aimdk.protocol.HalHandService/SetHandCommand',
+            aimdk_dot_protocol_dot_hal_dot_hand_dot_hal__hand__service__pb2.HandCommandRequest.SerializeToString,
+            aimdk_dot_protocol_dot_common_dot_rpc__pb2.CommonResponse.FromString,
+            options, channel_credentials,
+            insecure, call_credentials, compression, wait_for_ready, timeout, metadata)

data_gen_dependencies/aimdk/protocol/hal/hand/hand.proto

@@ -0,0 +1,35 @@
+syntax = "proto3";
+package aimdk.protocol;
+
+import public "aimdk/protocol/common/header.proto";
+import public "aimdk/protocol/hal/hand/inspire.proto";
+import public "aimdk/protocol/hal/hand/jodell.proto";
+import public "aimdk/protocol/hal/hand/agi_claw.proto";
+
+
+message SingleHandState {
+  oneof hand {
+    InspireHand inspire = 1;              // 因时手
+    ClawHand claw = 2;                    // jodell夹爪手
+    AgiClawHandState agi_claw_state = 3;  // agi自研夹爪状态
+  }
+}
+
+message SingleHandCommand {
+  oneof hand {
+    InspireHand inspire = 1;              // 因时手
+    ClawHand claw = 2;                    // jodell夹爪手
+    AgiClawHandCommand agi_claw_cmd = 3;  // agi自研夹爪命令
+  }
+}
+
+message HandState {
+  SingleHandState left = 2;   // 左手状态
+  SingleHandState right = 3;  // 右手状态
+}
+
+message HandCommand {
+  SingleHandCommand left = 2;   // 左手指令
+  SingleHandCommand right = 3;  // 右手指令
+}
+

data_gen_dependencies/aimdk/protocol/hal/hand/hand_channel.proto

@@ -0,0 +1,18 @@
+syntax = "proto3";
+package aimdk.protocol;
+
+import public "aimdk/protocol/common/header.proto";
+import public "aimdk/protocol/hal/hand/hand.proto";
+
+
+// 机器人手部状态
+message HandStateChannel {
+  Header header = 1;   // 消息头
+  HandState data = 2;  // 手部状态
+}
+
+// 机器人手部指令
+message HandCommandChannel {
+  Header header = 1;     // 消息头
+  HandCommand data = 2;  // 手部指令
+}

data_gen_dependencies/aimdk/protocol/hal/hand/hand_pb2.py

@@ -0,0 +1,40 @@
+# -*- coding: utf-8 -*-
+# Generated by the protocol buffer compiler.  DO NOT EDIT!
+# source: aimdk/protocol/hal/hand/hand.proto
+# Protobuf Python Version: 4.25.1
+"""Generated protocol buffer code."""
+from google.protobuf import descriptor as _descriptor
+from google.protobuf import descriptor_pool as _descriptor_pool
+from google.protobuf import symbol_database as _symbol_database
+from google.protobuf.internal import builder as _builder
+# @@protoc_insertion_point(imports)
+
+_sym_db = _symbol_database.Default()
+
+
+from aimdk.protocol.common import header_pb2 as aimdk_dot_protocol_dot_common_dot_header__pb2
+from aimdk.protocol.hal.hand import inspire_pb2 as aimdk_dot_protocol_dot_hal_dot_hand_dot_inspire__pb2
+from aimdk.protocol.hal.hand import jodell_pb2 as aimdk_dot_protocol_dot_hal_dot_hand_dot_jodell__pb2
+from aimdk.protocol.hal.hand import agi_claw_pb2 as aimdk_dot_protocol_dot_hal_dot_hand_dot_agi__claw__pb2
+
+from aimdk.protocol.common.header_pb2 import *
+from aimdk.protocol.hal.hand.inspire_pb2 import *
+from aimdk.protocol.hal.hand.jodell_pb2 import *
+from aimdk.protocol.hal.hand.agi_claw_pb2 import *
+
+DESCRIPTOR = _descriptor_pool.Default().AddSerializedFile(b'\n\"aimdk/protocol/hal/hand/hand.proto\x12\x0e\x61imdk.protocol\x1a\"aimdk/protocol/common/header.proto\x1a%aimdk/protocol/hal/hand/inspire.proto\x1a$aimdk/protocol/hal/hand/jodell.proto\x1a&aimdk/protocol/hal/hand/agi_claw.proto\"\xaf\x01\n\x0fSingleHandState\x12.\n\x07inspire\x18\x01 \x01(\x0b\x32\x1b.aimdk.protocol.InspireHandH\x00\x12(\n\x04\x63law\x18\x02 \x01(\x0b\x32\x18.aimdk.protocol.ClawHandH\x00\x12:\n\x0e\x61gi_claw_state\x18\x03 \x01(\x0b\x32 .aimdk.protocol.AgiClawHandStateH\x00\x42\x06\n\x04hand\"\xb1\x01\n\x11SingleHandCommand\x12.\n\x07inspire\x18\x01 \x01(\x0b\x32\x1b.aimdk.protocol.InspireHandH\x00\x12(\n\x04\x63law\x18\x02 \x01(\x0b\x32\x18.aimdk.protocol.ClawHandH\x00\x12:\n\x0c\x61gi_claw_cmd\x18\x03 \x01(\x0b\x32\".aimdk.protocol.AgiClawHandCommandH\x00\x42\x06\n\x04hand\"j\n\tHandState\x12-\n\x04left\x18\x02 \x01(\x0b\x32\x1f.aimdk.protocol.SingleHandState\x12.\n\x05right\x18\x03 \x01(\x0b\x32\x1f.aimdk.protocol.SingleHandState\"p\n\x0bHandCommand\x12/\n\x04left\x18\x02 \x01(\x0b\x32!.aimdk.protocol.SingleHandCommand\x12\x30\n\x05right\x18\x03 \x01(\x0b\x32!.aimdk.protocol.SingleHandCommandP\x00P\x01P\x02P\x03\x62\x06proto3')
+
+_globals = globals()
+_builder.BuildMessageAndEnumDescriptors(DESCRIPTOR, _globals)
+_builder.BuildTopDescriptorsAndMessages(DESCRIPTOR, 'aimdk.protocol.hal.hand.hand_pb2', _globals)
+if _descriptor._USE_C_DESCRIPTORS == False:
+  DESCRIPTOR._options = None
+  _globals['_SINGLEHANDSTATE']._serialized_start=208
+  _globals['_SINGLEHANDSTATE']._serialized_end=383
+  _globals['_SINGLEHANDCOMMAND']._serialized_start=386
+  _globals['_SINGLEHANDCOMMAND']._serialized_end=563
+  _globals['_HANDSTATE']._serialized_start=565
+  _globals['_HANDSTATE']._serialized_end=671
+  _globals['_HANDCOMMAND']._serialized_start=673
+  _globals['_HANDCOMMAND']._serialized_end=785
+# @@protoc_insertion_point(module_scope)

data_gen_dependencies/aimdk/protocol/hal/hand/inspire.proto

@@ -0,0 +1,25 @@
+syntax = "proto3";
+package aimdk.protocol;
+
+
+// 手指
+message Finger {
+  int32 thumb_pos_0 = 1;  // 拇指第一关节
+  int32 thumb_pos_1 = 2;  // 拇指第二关节
+  int32 index_pos = 3;    // 食指
+  int32 middle_pos = 4;   // 中指
+  int32 ring_pos = 5;     // 无名指
+  int32 pinky_pos = 6;    // 小指
+}
+
+// 手腕
+message Wrist {
+  int32 x = 1;  // x轴
+  int32 y = 2;  // y轴
+}
+
+// Inspire手部
+message InspireHand {
+  Finger finger = 1;  // 手指
+  Wrist wrist = 2;    // 手腕
+}

data_gen_dependencies/aimdk/protocol/hal/hand/inspire_pb2.py

@@ -0,0 +1,30 @@
+# -*- coding: utf-8 -*-
+# Generated by the protocol buffer compiler.  DO NOT EDIT!
+# source: aimdk/protocol/hal/hand/inspire.proto
+# Protobuf Python Version: 4.25.1
+"""Generated protocol buffer code."""
+from google.protobuf import descriptor as _descriptor
+from google.protobuf import descriptor_pool as _descriptor_pool
+from google.protobuf import symbol_database as _symbol_database
+from google.protobuf.internal import builder as _builder
+# @@protoc_insertion_point(imports)
+
+_sym_db = _symbol_database.Default()
+
+
+
+
+DESCRIPTOR = _descriptor_pool.Default().AddSerializedFile(b'\n%aimdk/protocol/hal/hand/inspire.proto\x12\x0e\x61imdk.protocol\"~\n\x06\x46inger\x12\x13\n\x0bthumb_pos_0\x18\x01 \x01(\x05\x12\x13\n\x0bthumb_pos_1\x18\x02 \x01(\x05\x12\x11\n\tindex_pos\x18\x03 \x01(\x05\x12\x12\n\nmiddle_pos\x18\x04 \x01(\x05\x12\x10\n\x08ring_pos\x18\x05 \x01(\x05\x12\x11\n\tpinky_pos\x18\x06 \x01(\x05\"\x1d\n\x05Wrist\x12\t\n\x01x\x18\x01 \x01(\x05\x12\t\n\x01y\x18\x02 \x01(\x05\"[\n\x0bInspireHand\x12&\n\x06\x66inger\x18\x01 \x01(\x0b\x32\x16.aimdk.protocol.Finger\x12$\n\x05wrist\x18\x02 \x01(\x0b\x32\x15.aimdk.protocol.Wristb\x06proto3')
+
+_globals = globals()
+_builder.BuildMessageAndEnumDescriptors(DESCRIPTOR, _globals)
+_builder.BuildTopDescriptorsAndMessages(DESCRIPTOR, 'aimdk.protocol.hal.hand.inspire_pb2', _globals)
+if _descriptor._USE_C_DESCRIPTORS == False:
+  DESCRIPTOR._options = None
+  _globals['_FINGER']._serialized_start=57
+  _globals['_FINGER']._serialized_end=183
+  _globals['_WRIST']._serialized_start=185
+  _globals['_WRIST']._serialized_end=214
+  _globals['_INSPIREHAND']._serialized_start=216
+  _globals['_INSPIREHAND']._serialized_end=307
+# @@protoc_insertion_point(module_scope)

data_gen_dependencies/aimdk/protocol/hal/hand/jodell.proto

@@ -0,0 +1,10 @@
+syntax = "proto3";
+package aimdk.protocol;
+
+
+// 夹爪手部
+message ClawHand {
+  uint32 position = 1;
+  uint32 velocity = 2;
+  uint32 force = 3;
+}

data_gen_dependencies/aimdk/protocol/hal/hand/jodell_pb2.py

@@ -0,0 +1,26 @@
+# -*- coding: utf-8 -*-
+# Generated by the protocol buffer compiler.  DO NOT EDIT!
+# source: aimdk/protocol/hal/hand/jodell.proto
+# Protobuf Python Version: 4.25.1
+"""Generated protocol buffer code."""
+from google.protobuf import descriptor as _descriptor
+from google.protobuf import descriptor_pool as _descriptor_pool
+from google.protobuf import symbol_database as _symbol_database
+from google.protobuf.internal import builder as _builder
+# @@protoc_insertion_point(imports)
+
+_sym_db = _symbol_database.Default()
+
+
+
+
+DESCRIPTOR = _descriptor_pool.Default().AddSerializedFile(b'\n$aimdk/protocol/hal/hand/jodell.proto\x12\x0e\x61imdk.protocol\"=\n\x08\x43lawHand\x12\x10\n\x08position\x18\x01 \x01(\r\x12\x10\n\x08velocity\x18\x02 \x01(\r\x12\r\n\x05\x66orce\x18\x03 \x01(\rb\x06proto3')
+
+_globals = globals()
+_builder.BuildMessageAndEnumDescriptors(DESCRIPTOR, _globals)
+_builder.BuildTopDescriptorsAndMessages(DESCRIPTOR, 'aimdk.protocol.hal.hand.jodell_pb2', _globals)
+if _descriptor._USE_C_DESCRIPTORS == False:
+  DESCRIPTOR._options = None
+  _globals['_CLAWHAND']._serialized_start=56
+  _globals['_CLAWHAND']._serialized_end=117
+# @@protoc_insertion_point(module_scope)

data_gen_dependencies/aimdk/protocol/hal/jaka/jaka.proto

@@ -0,0 +1,733 @@
+syntax = "proto3";
+package aimdk.protocol;
+
+import public "aimdk/protocol/common/vec3.proto";
+import public "aimdk/protocol/common/quaternion.proto";
+import public "aimdk/protocol/common/rpy.proto";
+import public "aimdk/protocol/common/se3_pose.proto";
+
+
+/**
+ * @brief 关节位置，单位rad
+ */
+message JointPos {  // Jaka: JointValue
+  repeated double jPos = 1;
+}
+
+/**
+ * @brief 负载
+ */
+message PayLoad {
+  double mass = 1;    // 负载质量，单位：kg
+  Vec3 centroid = 2;  // 负载质心, 单位：mm
+}
+
+/**
+ * @brief 运动模式
+ */
+enum MoveMode {
+  MoveMode_ABS = 0;       // 绝对运动
+  MoveMode_INCR = 1;      // 相对运动
+  MoveMode_CONTINUE = 2;  // 连续运动
+}
+
+/**
+ * @brief 机器人状态
+ */
+message RobotState {
+  int32 estoped = 1;       // 是否急停
+  int32 poweredOn = 2;     // 是否打开电源
+  int32 servoEnabled = 3;  // 是否使能
+}
+
+/**
+ * @brief 机械臂单关节瞬时信息
+ */
+message JointMonitorData {
+  double instCurrent = 1;      // 瞬时电流
+  double instVoltage = 2;      // 瞬时电压
+  double instTemperature = 3;  // 瞬时温度
+  double instVel = 4;          // 瞬时速度 控制器1.7.0.20及以上
+  double instTorq = 5;         // 瞬时力矩
+}
+
+/**
+ * @brief 机械臂关节信息
+ */
+message RobotMonitorData {
+  double scbMajorVersion = 1;                        // scb主版本号
+  double scbMinorVersion = 2;                        // scb小版本号
+  double cabTemperature = 3;                         // 控制器温度
+  double robotAveragePower = 4;                      // 机器人平均电压
+  double robotAverageCurrent = 5;                    // 机器人平均电流
+  repeated JointMonitorData joint_monitor_data = 6;  // 机器人7个关节的监测数据
+}
+
+/**
+ * @brief 力传感器监控信息
+ */
+message TorqSernsorMonitorData {
+  string ip = 1;                   // 力矩传感器ip地址
+  int32 port = 2;                  // 力矩传感器端口号
+  PayLoad payLoad = 3;             // 工具负载
+  int32 status = 4;                // 力矩传感器状态
+  int32 errcode = 5;               // 力矩传感器异常错误码
+  repeated double actTorque = 6;   // 力矩传感器实际接触力值（勾选初始化时）或原始读数值（勾选不初始化时）
+  repeated double torque = 7;      // 力矩传感器原始读数值
+  repeated double realTorque = 8;  // 力矩传感器实际接触力值（不随初始化选项变化）
+}
+
+/**
+ * @brief 机械臂详细状态信息
+ */
+message RobotStatus {
+  int32 errcode = 1;
+  int32 inpos = 2;
+  int32 powered_on = 3;
+  int32 enabled = 4;
+  double rapidrate = 5;
+  int32 protective_stop = 6;
+  int32 emergency_stop = 7;
+  repeated double cartesiantran_pos = 8;
+  repeated double joint_pos = 9;
+  uint32 on_soft_limit = 10;
+  uint32 cur_user_id = 11;
+  int32 drag_status = 12;
+  RobotMonitorData robot_monitor_data = 13;
+  TorqSernsorMonitorData torq_sensor_monitor_data = 14;
+  int32 is_socket_connect = 15;
+}
+
+/**
+ * @brief 可选参数
+ */
+message OptionalCond {
+  int32 executingLineId = 1;
+}
+
+/**
+ * @brief 错误状态
+ */
+message ErrorCode {
+  int32 code = 1;
+  string message = 2;
+}
+
+/**
+ * @brief rpc GetJointPosition() 请求信息
+ */
+message GetJointPositionReq {
+  string robot_name = 1;
+}
+
+/**
+ * @brief rpc GetJointPosition() 响应信息
+ */
+message GetJointPositionRsp {
+  JointPos pos = 1;
+  int32 errcode = 2;
+  string errmsg = 3;
+}
+
+/**
+ * @brief JointMove扩展参数
+ */
+message JointMoveExt {
+  double acc = 1;
+  double tol = 2;
+  OptionalCond option_cond = 3;
+}
+
+/**
+ * @brief rpc JointMove() 请求信息
+ */
+message JointMoveReq {
+  string robot_name = 1;
+  JointPos pos = 2;
+  MoveMode mode = 3;
+  bool is_block = 4;
+  double distance_frame = 5;
+  JointMoveExt ext = 6;
+  bool ee_interpolation = 7;
+}
+
+/**
+ * @brief rpc JointMove() 响应信息
+ */
+message JointMoveRsp {
+  int32 errcode = 1;
+  string errmsg = 2;
+}
+
+/**
+ * @brief LinearMove扩展参数
+ */
+message LinearMoveExt {
+  double acc = 1;
+  double tol = 2;
+  OptionalCond option_cond = 3;
+}
+
+/**
+ * @brief rpc LinearMove() 请求信息
+ */
+message LinearMoveReq {
+  string robot_name = 1;
+  SE3RpyPose pose = 2;
+  MoveMode mode = 3;
+  bool is_block = 4;
+  double distance_frame = 5;
+  LinearMoveExt ext = 6;
+  bool ee_interpolation = 7;
+}
+
+/**
+ * @brief rpc LinearMove() 响应信息
+ */
+message LinearMoveRsp {
+  int32 errcode = 1;
+  string errmsg = 2;
+}
+
+/**
+ * @brief rpc GetRobotState() 请求信息
+ */
+message GetRobotStateReq {
+  string robot_name = 1;
+}
+
+/**
+ * @brief rpc GetRobotState() 响应信息
+ */
+message GetRobotStateRsp {
+  RobotState state = 1;
+  int32 errcode = 2;
+  string errmsg = 3;
+}
+
+/**
+ * @brief rpc GetRobotStatus() 请求信息
+ */
+message GetRobotStatusReq {
+  string robot_name = 1;
+}
+
+/**
+ * @brief rpc GetRobotStatus() 响应信息
+ */
+message GetRobotStatusRsp {
+  RobotStatus status = 1;
+  int32 errcode = 2;
+  string errmsg = 3;
+}
+
+/**
+ * @brief rpc IsInPos() 请求信息
+ */
+message IsInPosReq {
+  string robot_name = 1;
+}
+
+/**
+ * @brief rpc IsInPos() 响应信息
+ */
+message IsInPosRsp {
+  bool is_in_pos = 1;
+  int32 errcode = 2;
+  string errmsg = 3;
+}
+
+/**
+ * @brief rpc IsOnLimit() 请求信息
+ */
+message IsOnLimitReq {
+  string robot_name = 1;
+}
+
+/**
+ * @brief rpc IsOnLimit() 响应信息
+ */
+message IsOnLimitRsp {
+  bool is_on_limit = 1;
+  int32 errcode = 2;
+  string errmsg = 3;
+}
+
+/**
+ * @brief rpc GetTcpPosition() 请求信息
+ */
+message GetTcpPositionReq {
+  string robot_name = 1;
+}
+
+/**
+ * @brief rpc GetTcpPosition() 响应信息
+ */
+message GetTcpPositionRsp {
+  SE3RpyPose pose = 1;
+  int32 errcode = 2;
+  string errmsg = 3;
+}
+
+/**
+ * @brief rpc KineForward() 请求信息
+ */
+message KineForwardReq {
+  string robot_name = 1;
+  JointPos pos = 2;
+}
+
+/**
+ * @brief rpc KineForward() 响应信息
+ */
+message KineForwardRsp {
+  SE3RpyPose pose = 1;
+  int32 errcode = 2;
+  string errmsg = 3;
+}
+
+/**
+ * @brief rpc KineInverse() 请求信息
+ */
+message KineInverseReq {
+  string robot_name = 1;
+  JointPos ref_pos = 2;
+  SE3RpyPose pose = 3;
+}
+
+/**
+ * @brief rpc KineInverse() 响应信息
+ */
+message KineInverseRsp {
+  JointPos pos = 1;
+  int32 errcode = 2;
+  string errmsg = 3;
+}
+
+/**
+ * @brief rpc ClearError() 请求信息
+ */
+message ClearErrorReq {
+  string robot_name = 1;
+}
+
+/**
+ * @brief rpc ClearError() 响应信息
+ */
+message ClearErrorRsp {
+  int32 errcode = 1;
+  string errmsg = 2;
+}
+
+/**
+ * @brief rpc GetLastError() 请求信息
+ */
+message GetLastErrorReq {
+  string robot_name = 1;
+}
+
+/**
+ * @brief rpc GetLastError() 响应信息
+ */
+message GetLastErrorRsp {
+  ErrorCode robot_error = 1;
+  int32 errcode = 2;
+  string errmsg = 3;
+}
+
+/**
+ * @brief rpc IsInCollision() 请求信息
+ */
+message IsInCollisionReq {
+  string robot_name = 1;
+}
+
+/**
+ * @brief rpc IsInCollision() 响应信息
+ */
+message IsInCollisionRsp {
+  bool is_in_collision = 1;
+  int32 errcode = 2;
+  string errmsg = 3;
+}
+
+/**
+ * @brief rpc CollisionRecover() 请求信息
+ */
+message CollisionRecoverReq {
+  string robot_name = 1;
+}
+
+/**
+ * @brief rpc CollisionRecover() 响应信息
+ */
+message CollisionRecoverRsp {
+  int32 errcode = 1;
+  string errmsg = 2;
+}
+
+/**
+ * @brief rpc GetCollisionLevel() 请求信息
+ */
+message GetCollisionLevelReq {
+  string robot_name = 1;
+}
+
+/**
+ * @brief rpc GetCollisionLevel() 响应信息
+ */
+message GetCollisionLevelRsp {
+  int32 level = 1;
+  int32 errcode = 2;
+  string errmsg = 3;
+}
+
+/**
+ * @brief rpc SetCollisionLevel() 请求信息
+ */
+message SetCollisionLevelReq {
+  string robot_name = 1;
+  int32 level = 2;
+}
+
+/**
+ * @brief rpc SetCollisionLevel() 响应信息
+ */
+message SetCollisionLevelRsp {
+  int32 errcode = 1;
+  string errmsg = 2;
+}
+
+/**
+ * @brief rpc EnableServoControl() 请求信息
+ */
+message EnableServoControlReq {
+  string robot_name = 1;
+  bool enable = 2;
+}
+
+/**
+ * @brief rpc EnableServoControl() 响应信息
+ */
+message EnableServoControlRsp {
+  int32 errcode = 1;
+  string errmsg = 2;
+}
+
+/**
+ * @brief rpc EnableTorqueControl() 请求信息
+ */
+message EnableTorqueControlReq {
+  string robot_name = 1;
+  bool enable = 2;
+  int32 period = 3;
+}
+
+/**
+ * @brief rpc EnableTorqueControl() 响应信息
+ */
+message EnableTorqueControlRsp {
+  int32 errcode = 1;
+  string errmsg = 2;
+}
+
+// InstallationAngle
+/**
+ * @brief rpc SetInstallationAngle() 请求信息
+ */
+message SetInstallationAngleReq {
+  string robot_name = 1;
+  double angleX = 2;
+  double angleZ = 3;
+}
+
+/**
+ * @brief rpc SetInstallationAngle() 响应信息
+ */
+message SetInstallationAngleRsp {
+  int32 errcode = 1;
+  string errmsg = 2;
+}
+
+/**
+ * @brief rpc GetInstallationAngle() 请求信息
+ */
+message GetInstallationAngleReq {
+  string robot_name = 1;
+}
+
+/**
+ * @brief rpc GetInstallationAngle() 响应信息
+ */
+message GetInstallationAngleRsp {
+  Quaternion quat = 1;
+  Rpy rpy = 2;
+  int32 errcode = 3;
+  string errmsg = 4;
+}
+
+/**
+ * @brief rpc EnableAdmittanceCtrl() 请求信息
+ */
+message EnableAdmittanceCtrlReq {
+  string robot_name = 1;
+  bool enable_flag = 2;
+}
+
+/**
+ * @brief rpc EnableAdmittanceCtrl() 响应信息
+ */
+message EnableAdmittanceCtrlRsp {
+  int32 errcode = 1;
+  string errmsg = 2;
+}
+
+/**
+ * @brief rpc SetFtCtrlFrame() 请求信息
+ */
+message SetFtCtrlFrameReq {
+  string robot_name = 1;
+  int32 ftFrame = 2;
+}
+
+/**
+ * @brief rpc SetFtCtrlFrame() 响应信息
+ */
+message SetFtCtrlFrameRsp {
+  int32 errcode = 1;
+  string errmsg = 2;
+}
+
+/**
+ * @brief rpc DisableForceControl() 请求信息
+ */
+message DisableForceControlReq {
+  string robot_name = 1;
+}
+
+/**
+ * @brief rpc DisableForceControl() 响应信息
+ */
+message DisableForceControlRsp {
+  int32 errcode = 1;
+  string errmsg = 2;
+}
+
+/**
+ * @brief rpc SetCompliantType() 请求信息
+ */
+message SetCompliantTypeReq {
+  string robot_name = 1;
+  int32 sensor_compensation = 2;
+  int32 compliance_type = 3;
+}
+
+/**
+ * @brief rpc SetCompliantType() 响应信息
+ */
+message SetCompliantTypeRsp {
+  int32 errcode = 1;
+  string errmsg = 2;
+}
+
+/**
+ * @brief rpc SetTorqueSensorMode() 请求信息
+ */
+message SetTorqueSensorModeReq {
+  string robot_name = 1;
+  int32 sensor_mode = 2;
+}
+
+/**
+ * @brief rpc SetTorqueSensorMode() 响应信息
+ */
+message SetTorqueSensorModeRsp {
+  int32 errcode = 1;
+  string errmsg = 2;
+}
+
+/**
+ * @brief rpc SetTorqueSensorMode() 请求信息
+ */
+message SetAdmitCtrlConfigReq {
+  string robot_name = 1;
+  int32 axis = 2;
+  int32 opt = 3;
+  double ftUser = 4;
+  double ftConstant = 5;
+  int32 ftNnormalTrack = 6;
+  double ftReboundFK = 7;
+}
+
+/**
+ * @brief rpc SetAdmitCtrlConfig() 响应信息
+ */
+message SetAdmitCtrlConfigRsp {
+  int32 errcode = 1;
+  string errmsg = 2;
+}
+
+/**
+ * @brief 由 aimrt_hal 中的 jaka_module::JakaModule 模块提供的服务
+ */
+service A2wArmControlService {
+  /**
+   * @brief 获取 jaka_module::JakaModule 收到的关节角度。
+   * @param GetJointPositionReq
+   * @return GetJointPositionRsp
+   */
+  rpc GetJointPosition(GetJointPositionReq) returns (GetJointPositionRsp);
+  /**
+   * @brief jaka_module::JakaModule 执行普通关节运动。
+   * @param JointMoveReq
+   * @return JointMoveRsp
+   */
+  rpc JointMove(JointMoveReq) returns (JointMoveRsp);
+  /**
+   * @brief jaka_module::JakaModule 执行末端线性运动。
+   * @param LinearMoveReq
+   * @return LinearMoveRsp
+   */
+  rpc LinearMove(LinearMoveReq) returns (LinearMoveRsp);
+  /**
+   * @brief 获取 jaka_module::JakaModule 收到的机械臂状态信息。
+   * @param GetRobotStateReq
+   * @return GetRobotStateRsp
+   */
+  rpc GetRobotState(GetRobotStateReq) returns (GetRobotStateRsp);
+  /**
+   * @brief 获取 jaka_module::JakaModule 收到的机械臂详细信息。
+   * @param GetRobotStatusReq
+   * @return GetRobotStatusRsp
+   */
+  rpc GetRobotStatus(GetRobotStatusReq) returns (GetRobotStatusRsp);
+  /**
+   * @brief 获取 jaka_module::JakaModule 收到的机械臂是否到达目标点的状态信息。
+   * @param IsInPosReq
+   * @return IsInPosRsp
+   */
+  rpc IsInPos(IsInPosReq) returns (IsInPosRsp);
+  /**
+   * @brief 获取 jaka_module::JakaModule 收到的机械臂是否限位的信息。
+   * @param IsOnLimitReq
+   * @return IsOnLimitRsp
+   */
+  rpc IsOnLimit(IsOnLimitReq) returns (IsOnLimitRsp);
+  /**
+   * @brief 获取 jaka_module::JakaModule 收到的末端笛卡尔位姿信息。
+   * @param GetTcpPositionReq
+   * @return GetTcpPositionRsp
+   */
+  rpc GetTcpPosition(GetTcpPositionReq) returns (GetTcpPositionRsp);
+  /**
+   * @brief 通过 jaka_module::JakaModule 进行运动学正解。
+   * @param KineForwardReq
+   * @return KineForwardRsp
+   */
+  rpc KineForward(KineForwardReq) returns (KineForwardRsp);
+  /**
+   * @brief 通过 jaka_module::JakaModule 进行运动学逆解。
+   * @param KineInverseReq
+   * @return KineInverseRsp
+   */
+  rpc KineInverse(KineInverseReq) returns (KineInverseRsp);
+  /**
+   * @brief 通过 jaka_module::JakaModule 清理错误。
+   * @param ClearErrorReq
+   * @return ClearErrorRsp
+   */
+  rpc ClearError(ClearErrorReq) returns (ClearErrorRsp);
+  /**
+   * @brief 获取 jaka_module::JakaModule 收到的机械臂错误信息。
+   * @param GetLastErrorReq
+   * @return GetLastErrorRsp
+   */
+  rpc GetLastError(GetLastErrorReq) returns (GetLastErrorRsp);
+  /**
+   * @brief 获取 jaka_module::JakaModule 收到的机械臂是否碰撞的状态信息。
+   * @param IsInCollisionReq
+   * @return IsInCollisionRsp
+   */
+  rpc IsInCollision(IsInCollisionReq) returns (IsInCollisionRsp);
+  /**
+   * @brief 通过 jaka_module::JakaModule 进行碰撞恢复。
+   * @param CollisionRecoverReq
+   * @return CollisionRecoverRsp
+   */
+  rpc CollisionRecover(CollisionRecoverReq) returns (CollisionRecoverRsp);
+  /**
+   * @brief 获取 jaka_module::JakaModule 收到的机械臂碰撞登记。
+   * @param GetCollisionLevelReq
+   * @return GetCollisionLevelRsp
+   */
+  rpc GetCollisionLevel(GetCollisionLevelReq) returns (GetCollisionLevelRsp);
+  /**
+   * @brief 通过 jaka_module::JakaModule 设置机械臂碰撞登记。
+   * @param SetCollisionLevelReq
+   * @return SetCollisionLevelRsp
+   */
+  rpc SetCollisionLevel(SetCollisionLevelReq) returns (SetCollisionLevelRsp);
+  /**
+   * @brief 通过 jaka_module::JakaModule 进行伺服使能控制。
+   * @param EnableServoControlReq
+   * @return EnableServoControlRsp
+   */
+  rpc EnableServoControl(EnableServoControlReq) returns (EnableServoControlRsp);
+  /**
+   * @brief 通过 jaka_module::JakaModule 进行电流环控制。
+   * @param EnableTorqueControlReq
+   * @return EnableTorqueControlRsp
+   */
+  rpc EnableTorqueControl(EnableTorqueControlReq) returns (EnableTorqueControlRsp);
+  /**
+   * @brief 通过 jaka_module::JakaModule 设置安装角度。
+   * @param SetInstallationAngleReq
+   * @return SetInstallationAngleRsp
+   */
+  rpc SetInstallationAngle(SetInstallationAngleReq) returns (SetInstallationAngleRsp);
+  /**
+   * @brief 获取 jaka_module::JakaModule 收到的机械臂安装角度。
+   * @param GetInstallationAngleReq
+   * @return GetInstallationAngleRsp
+   */
+  rpc GetInstallationAngle(GetInstallationAngleReq) returns (GetInstallationAngleRsp);
+  /**
+   * @brief 通过 jaka_module::JakaModule 设置力控拖拽使能。
+   * @param EnableAdmittanceCtrlReq
+   * @return EnableAdmittanceCtrlRsp
+   */
+  rpc EnableAdmittanceCtrl(EnableAdmittanceCtrlReq) returns (EnableAdmittanceCtrlRsp);
+  /**
+   * @brief 通过 jaka_module::JakaModule 设置导纳控制运动坐标系。
+   * @param SetFtCtrlFrameReq
+   * @return SetFtCtrlFrameRsp
+   */
+  rpc SetFtCtrlFrame(SetFtCtrlFrameReq) returns (SetFtCtrlFrameRsp);
+  /**
+   * @brief 通过 jaka_module::JakaModule 关闭力控。
+   * @param DisableForceControlReq
+   * @return DisableForceControlRsp
+   */
+  rpc DisableForceControl(DisableForceControlReq) returns (DisableForceControlRsp);
+
+  /**
+   * @brief 通过 jaka_module::JakaModule 设置力控类型和传感器初始化状态。
+   * @param SetCompliantTypeReq
+   * @return SetCompliantTypeRsp
+   */
+  rpc SetCompliantType(SetCompliantTypeReq) returns (SetCompliantTypeRsp);
+
+  /**
+   * @brief 通过 jaka_module::JakaModule 开启或关闭力矩传感器。
+   * @param SetTorqueSensorModeReq
+   * @return SetTorqueSensorModeRsp
+   */
+  rpc SetTorqueSensorMode(SetTorqueSensorModeReq) returns (SetTorqueSensorModeRsp);
+
+  /**
+   * @brief 通过 jaka_module::JakaModule 设置柔顺控制参数。
+   * @param SetAdmitCtrlConfigReq
+   * @return SetAdmitCtrlConfigRsp
+   */
+  rpc SetAdmitCtrlConfig(SetAdmitCtrlConfigReq) returns (SetAdmitCtrlConfigRsp);
+}

data_gen_dependencies/aimdk/protocol/hal/joint/joint_channel.proto

@@ -0,0 +1,69 @@
+syntax = "proto3";
+package aimdk.protocol;
+
+import public "aimdk/protocol/common/header.proto";
+import public "aimdk/protocol/common/joint.proto";
+import public "aimdk/protocol/common/se3_pose.proto";
+
+message JointStatesChannel {
+  /** 消息头 */
+  Header header = 1;
+
+  /**
+   * 关节状态，包括角度和速度、扭矩
+   */
+  repeated JointState states = 2;
+}
+message IKStatus{
+    repeated string joint_names=1;
+    repeated double joint_positions=2;
+    bool isSuccess=3;
+}
+
+message JointCommandsChannel {
+  /** 消息头 */
+  Header header = 1;
+
+  /**
+   * 关节指令
+   */
+  repeated JointCommand commands = 2;
+}
+
+message GetJointReq {
+  string serial_no = 1;
+}
+message GetJointRsp {
+  repeated JointState states = 1;
+}
+message GetEEPoseReq {
+    bool is_right = 1;
+}
+message GetEEPoseRsp {
+    string prim_path = 1;
+    SE3RpyPose ee_pose = 2;
+}
+message GetIKStatusReq {
+    bool is_right = 1;
+    repeated SE3RpyPose target_pose = 2;
+    bool ObsAvoid = 3;
+}
+message GetIKStatusRsp {
+    repeated IKStatus IKStatus = 2;
+}
+
+
+message SetJointReq {
+  repeated JointCommand commands = 1;
+  bool is_trajectory = 2;
+}
+message SetJointRsp {
+  string errmsg = 1;
+}
+
+service JointControlService {
+  rpc GetJointPosition(GetJointReq) returns(GetJointRsp);
+  rpc SetJointPosition(SetJointReq) returns(SetJointRsp);
+  rpc GetIKStatus(GetIKStatusReq) returns(GetIKStatusRsp);
+  rpc GetEEPose(GetEEPoseReq) returns(GetEEPoseRsp);
+}

data_gen_dependencies/aimdk/protocol/hal/joint/joint_channel_pb2.py

@@ -0,0 +1,66 @@
+# -*- coding: utf-8 -*-
+# Generated by the protocol buffer compiler.  DO NOT EDIT!
+# source: aimdk/protocol/hal/joint/joint_channel.proto
+# Protobuf Python Version: 4.25.1
+"""Generated protocol buffer code."""
+from google.protobuf import descriptor as _descriptor
+from google.protobuf import descriptor_pool as _descriptor_pool
+from google.protobuf import symbol_database as _symbol_database
+from google.protobuf.internal import builder as _builder
+# @@protoc_insertion_point(imports)
+
+_sym_db = _symbol_database.Default()
+
+
+from aimdk.protocol.common import header_pb2 as aimdk_dot_protocol_dot_common_dot_header__pb2
+from aimdk.protocol.common import joint_pb2 as aimdk_dot_protocol_dot_common_dot_joint__pb2
+from aimdk.protocol.common import se3_pose_pb2 as aimdk_dot_protocol_dot_common_dot_se3__pose__pb2
+try:
+  aimdk_dot_protocol_dot_common_dot_vec3__pb2 = aimdk_dot_protocol_dot_common_dot_se3__pose__pb2.aimdk_dot_protocol_dot_common_dot_vec3__pb2
+except AttributeError:
+  aimdk_dot_protocol_dot_common_dot_vec3__pb2 = aimdk_dot_protocol_dot_common_dot_se3__pose__pb2.aimdk.protocol.common.vec3_pb2
+try:
+  aimdk_dot_protocol_dot_common_dot_quaternion__pb2 = aimdk_dot_protocol_dot_common_dot_se3__pose__pb2.aimdk_dot_protocol_dot_common_dot_quaternion__pb2
+except AttributeError:
+  aimdk_dot_protocol_dot_common_dot_quaternion__pb2 = aimdk_dot_protocol_dot_common_dot_se3__pose__pb2.aimdk.protocol.common.quaternion_pb2
+try:
+  aimdk_dot_protocol_dot_common_dot_rpy__pb2 = aimdk_dot_protocol_dot_common_dot_se3__pose__pb2.aimdk_dot_protocol_dot_common_dot_rpy__pb2
+except AttributeError:
+  aimdk_dot_protocol_dot_common_dot_rpy__pb2 = aimdk_dot_protocol_dot_common_dot_se3__pose__pb2.aimdk.protocol.common.rpy_pb2
+
+from aimdk.protocol.common.header_pb2 import *
+from aimdk.protocol.common.joint_pb2 import *
+from aimdk.protocol.common.se3_pose_pb2 import *
+
+DESCRIPTOR = _descriptor_pool.Default().AddSerializedFile(b'\n,aimdk/protocol/hal/joint/joint_channel.proto\x12\x0e\x61imdk.protocol\x1a\"aimdk/protocol/common/header.proto\x1a!aimdk/protocol/common/joint.proto\x1a$aimdk/protocol/common/se3_pose.proto\"h\n\x12JointStatesChannel\x12&\n\x06header\x18\x01 \x01(\x0b\x32\x16.aimdk.protocol.Header\x12*\n\x06states\x18\x02 \x03(\x0b\x32\x1a.aimdk.protocol.JointState\"K\n\x08IKStatus\x12\x13\n\x0bjoint_names\x18\x01 \x03(\t\x12\x17\n\x0fjoint_positions\x18\x02 \x03(\x01\x12\x11\n\tisSuccess\x18\x03 \x01(\x08\"n\n\x14JointCommandsChannel\x12&\n\x06header\x18\x01 \x01(\x0b\x32\x16.aimdk.protocol.Header\x12.\n\x08\x63ommands\x18\x02 \x03(\x0b\x32\x1c.aimdk.protocol.JointCommand\" \n\x0bGetJointReq\x12\x11\n\tserial_no\x18\x01 \x01(\t\"9\n\x0bGetJointRsp\x12*\n\x06states\x18\x01 \x03(\x0b\x32\x1a.aimdk.protocol.JointState\" \n\x0cGetEEPoseReq\x12\x10\n\x08is_right\x18\x01 \x01(\x08\"N\n\x0cGetEEPoseRsp\x12\x11\n\tprim_path\x18\x01 \x01(\t\x12+\n\x07\x65\x65_pose\x18\x02 \x01(\x0b\x32\x1a.aimdk.protocol.SE3RpyPose\"e\n\x0eGetIKStatusReq\x12\x10\n\x08is_right\x18\x01 \x01(\x08\x12/\n\x0btarget_pose\x18\x02 \x03(\x0b\x32\x1a.aimdk.protocol.SE3RpyPose\x12\x10\n\x08ObsAvoid\x18\x03 \x01(\x08\"<\n\x0eGetIKStatusRsp\x12*\n\x08IKStatus\x18\x02 \x03(\x0b\x32\x18.aimdk.protocol.IKStatus\"T\n\x0bSetJointReq\x12.\n\x08\x63ommands\x18\x01 \x03(\x0b\x32\x1c.aimdk.protocol.JointCommand\x12\x15\n\ris_trajectory\x18\x02 \x01(\x08\"\x1d\n\x0bSetJointRsp\x12\x0e\n\x06\x65rrmsg\x18\x01 \x01(\t2\xc9\x02\n\x13JointControlService\x12L\n\x10GetJointPosition\x12\x1b.aimdk.protocol.GetJointReq\x1a\x1b.aimdk.protocol.GetJointRsp\x12L\n\x10SetJointPosition\x12\x1b.aimdk.protocol.SetJointReq\x1a\x1b.aimdk.protocol.SetJointRsp\x12M\n\x0bGetIKStatus\x12\x1e.aimdk.protocol.GetIKStatusReq\x1a\x1e.aimdk.protocol.GetIKStatusRsp\x12G\n\tGetEEPose\x12\x1c.aimdk.protocol.GetEEPoseReq\x1a\x1c.aimdk.protocol.GetEEPoseRspP\x00P\x01P\x02\x62\x06proto3')
+
+_globals = globals()
+_builder.BuildMessageAndEnumDescriptors(DESCRIPTOR, _globals)
+_builder.BuildTopDescriptorsAndMessages(DESCRIPTOR, 'aimdk.protocol.hal.joint.joint_channel_pb2', _globals)
+if _descriptor._USE_C_DESCRIPTORS == False:
+  DESCRIPTOR._options = None
+  _globals['_JOINTSTATESCHANNEL']._serialized_start=173
+  _globals['_JOINTSTATESCHANNEL']._serialized_end=277
+  _globals['_IKSTATUS']._serialized_start=279
+  _globals['_IKSTATUS']._serialized_end=354
+  _globals['_JOINTCOMMANDSCHANNEL']._serialized_start=356
+  _globals['_JOINTCOMMANDSCHANNEL']._serialized_end=466
+  _globals['_GETJOINTREQ']._serialized_start=468
+  _globals['_GETJOINTREQ']._serialized_end=500
+  _globals['_GETJOINTRSP']._serialized_start=502
+  _globals['_GETJOINTRSP']._serialized_end=559
+  _globals['_GETEEPOSEREQ']._serialized_start=561
+  _globals['_GETEEPOSEREQ']._serialized_end=593
+  _globals['_GETEEPOSERSP']._serialized_start=595
+  _globals['_GETEEPOSERSP']._serialized_end=673
+  _globals['_GETIKSTATUSREQ']._serialized_start=675
+  _globals['_GETIKSTATUSREQ']._serialized_end=776
+  _globals['_GETIKSTATUSRSP']._serialized_start=778
+  _globals['_GETIKSTATUSRSP']._serialized_end=838
+  _globals['_SETJOINTREQ']._serialized_start=840
+  _globals['_SETJOINTREQ']._serialized_end=924
+  _globals['_SETJOINTRSP']._serialized_start=926
+  _globals['_SETJOINTRSP']._serialized_end=955
+  _globals['_JOINTCONTROLSERVICE']._serialized_start=958
+  _globals['_JOINTCONTROLSERVICE']._serialized_end=1287
+# @@protoc_insertion_point(module_scope)

data_gen_dependencies/aimdk/protocol/hal/joint/joint_channel_pb2_grpc.py

@@ -0,0 +1,165 @@
+# Generated by the gRPC Python protocol compiler plugin. DO NOT EDIT!
+"""Client and server classes corresponding to protobuf-defined services."""
+import grpc
+
+from aimdk.protocol.hal.joint import joint_channel_pb2 as aimdk_dot_protocol_dot_hal_dot_joint_dot_joint__channel__pb2
+
+
+class JointControlServiceStub(object):
+    """Missing associated documentation comment in .proto file."""
+
+    def __init__(self, channel):
+        """Constructor.
+
+        Args:
+            channel: A grpc.Channel.
+        """
+        self.GetJointPosition = channel.unary_unary(
+                '/aimdk.protocol.JointControlService/GetJointPosition',
+                request_serializer=aimdk_dot_protocol_dot_hal_dot_joint_dot_joint__channel__pb2.GetJointReq.SerializeToString,
+                response_deserializer=aimdk_dot_protocol_dot_hal_dot_joint_dot_joint__channel__pb2.GetJointRsp.FromString,
+                )
+        self.SetJointPosition = channel.unary_unary(
+                '/aimdk.protocol.JointControlService/SetJointPosition',
+                request_serializer=aimdk_dot_protocol_dot_hal_dot_joint_dot_joint__channel__pb2.SetJointReq.SerializeToString,
+                response_deserializer=aimdk_dot_protocol_dot_hal_dot_joint_dot_joint__channel__pb2.SetJointRsp.FromString,
+                )
+        self.GetIKStatus = channel.unary_unary(
+                '/aimdk.protocol.JointControlService/GetIKStatus',
+                request_serializer=aimdk_dot_protocol_dot_hal_dot_joint_dot_joint__channel__pb2.GetIKStatusReq.SerializeToString,
+                response_deserializer=aimdk_dot_protocol_dot_hal_dot_joint_dot_joint__channel__pb2.GetIKStatusRsp.FromString,
+                )
+        self.GetEEPose = channel.unary_unary(
+                '/aimdk.protocol.JointControlService/GetEEPose',
+                request_serializer=aimdk_dot_protocol_dot_hal_dot_joint_dot_joint__channel__pb2.GetEEPoseReq.SerializeToString,
+                response_deserializer=aimdk_dot_protocol_dot_hal_dot_joint_dot_joint__channel__pb2.GetEEPoseRsp.FromString,
+                )
+
+
+class JointControlServiceServicer(object):
+    """Missing associated documentation comment in .proto file."""
+
+    def GetJointPosition(self, request, context):
+        """Missing associated documentation comment in .proto file."""
+        context.set_code(grpc.StatusCode.UNIMPLEMENTED)
+        context.set_details('Method not implemented!')
+        raise NotImplementedError('Method not implemented!')
+
+    def SetJointPosition(self, request, context):
+        """Missing associated documentation comment in .proto file."""
+        context.set_code(grpc.StatusCode.UNIMPLEMENTED)
+        context.set_details('Method not implemented!')
+        raise NotImplementedError('Method not implemented!')
+
+    def GetIKStatus(self, request, context):
+        """Missing associated documentation comment in .proto file."""
+        context.set_code(grpc.StatusCode.UNIMPLEMENTED)
+        context.set_details('Method not implemented!')
+        raise NotImplementedError('Method not implemented!')
+
+    def GetEEPose(self, request, context):
+        """Missing associated documentation comment in .proto file."""
+        context.set_code(grpc.StatusCode.UNIMPLEMENTED)
+        context.set_details('Method not implemented!')
+        raise NotImplementedError('Method not implemented!')
+
+
+def add_JointControlServiceServicer_to_server(servicer, server):
+    rpc_method_handlers = {
+            'GetJointPosition': grpc.unary_unary_rpc_method_handler(
+                    servicer.GetJointPosition,
+                    request_deserializer=aimdk_dot_protocol_dot_hal_dot_joint_dot_joint__channel__pb2.GetJointReq.FromString,
+                    response_serializer=aimdk_dot_protocol_dot_hal_dot_joint_dot_joint__channel__pb2.GetJointRsp.SerializeToString,
+            ),
+            'SetJointPosition': grpc.unary_unary_rpc_method_handler(
+                    servicer.SetJointPosition,
+                    request_deserializer=aimdk_dot_protocol_dot_hal_dot_joint_dot_joint__channel__pb2.SetJointReq.FromString,
+                    response_serializer=aimdk_dot_protocol_dot_hal_dot_joint_dot_joint__channel__pb2.SetJointRsp.SerializeToString,
+            ),
+            'GetIKStatus': grpc.unary_unary_rpc_method_handler(
+                    servicer.GetIKStatus,
+                    request_deserializer=aimdk_dot_protocol_dot_hal_dot_joint_dot_joint__channel__pb2.GetIKStatusReq.FromString,
+                    response_serializer=aimdk_dot_protocol_dot_hal_dot_joint_dot_joint__channel__pb2.GetIKStatusRsp.SerializeToString,
+            ),
+            'GetEEPose': grpc.unary_unary_rpc_method_handler(
+                    servicer.GetEEPose,
+                    request_deserializer=aimdk_dot_protocol_dot_hal_dot_joint_dot_joint__channel__pb2.GetEEPoseReq.FromString,
+                    response_serializer=aimdk_dot_protocol_dot_hal_dot_joint_dot_joint__channel__pb2.GetEEPoseRsp.SerializeToString,
+            ),
+    }
+    generic_handler = grpc.method_handlers_generic_handler(
+            'aimdk.protocol.JointControlService', rpc_method_handlers)
+    server.add_generic_rpc_handlers((generic_handler,))
+
+
+ # This class is part of an EXPERIMENTAL API.
+class JointControlService(object):
+    """Missing associated documentation comment in .proto file."""
+
+    @staticmethod
+    def GetJointPosition(request,
+            target,
+            options=(),
+            channel_credentials=None,
+            call_credentials=None,
+            insecure=False,
+            compression=None,
+            wait_for_ready=None,
+            timeout=None,
+            metadata=None):
+        return grpc.experimental.unary_unary(request, target, '/aimdk.protocol.JointControlService/GetJointPosition',
+            aimdk_dot_protocol_dot_hal_dot_joint_dot_joint__channel__pb2.GetJointReq.SerializeToString,
+            aimdk_dot_protocol_dot_hal_dot_joint_dot_joint__channel__pb2.GetJointRsp.FromString,
+            options, channel_credentials,
+            insecure, call_credentials, compression, wait_for_ready, timeout, metadata)
+
+    @staticmethod
+    def SetJointPosition(request,
+            target,
+            options=(),
+            channel_credentials=None,
+            call_credentials=None,
+            insecure=False,
+            compression=None,
+            wait_for_ready=None,
+            timeout=None,
+            metadata=None):
+        return grpc.experimental.unary_unary(request, target, '/aimdk.protocol.JointControlService/SetJointPosition',
+            aimdk_dot_protocol_dot_hal_dot_joint_dot_joint__channel__pb2.SetJointReq.SerializeToString,
+            aimdk_dot_protocol_dot_hal_dot_joint_dot_joint__channel__pb2.SetJointRsp.FromString,
+            options, channel_credentials,
+            insecure, call_credentials, compression, wait_for_ready, timeout, metadata)
+
+    @staticmethod
+    def GetIKStatus(request,
+            target,
+            options=(),
+            channel_credentials=None,
+            call_credentials=None,
+            insecure=False,
+            compression=None,
+            wait_for_ready=None,
+            timeout=None,
+            metadata=None):
+        return grpc.experimental.unary_unary(request, target, '/aimdk.protocol.JointControlService/GetIKStatus',
+            aimdk_dot_protocol_dot_hal_dot_joint_dot_joint__channel__pb2.GetIKStatusReq.SerializeToString,
+            aimdk_dot_protocol_dot_hal_dot_joint_dot_joint__channel__pb2.GetIKStatusRsp.FromString,
+            options, channel_credentials,
+            insecure, call_credentials, compression, wait_for_ready, timeout, metadata)
+
+    @staticmethod
+    def GetEEPose(request,
+            target,
+            options=(),
+            channel_credentials=None,
+            call_credentials=None,
+            insecure=False,
+            compression=None,
+            wait_for_ready=None,
+            timeout=None,
+            metadata=None):
+        return grpc.experimental.unary_unary(request, target, '/aimdk.protocol.JointControlService/GetEEPose',
+            aimdk_dot_protocol_dot_hal_dot_joint_dot_joint__channel__pb2.GetEEPoseReq.SerializeToString,
+            aimdk_dot_protocol_dot_hal_dot_joint_dot_joint__channel__pb2.GetEEPoseRsp.FromString,
+            options, channel_credentials,
+            insecure, call_credentials, compression, wait_for_ready, timeout, metadata)

data_gen_dependencies/aimdk/protocol/hal/joint/msg/Command.msg

@@ -0,0 +1,7 @@
+string name
+uint32 sequence
+float64 position
+float64 velocity
+float64 effort
+float64 stiffness
+float64 damping

data_gen_dependencies/aimdk/protocol/hal/joint/msg/JointCommand.msg

@@ -0,0 +1,3 @@
+std_msgs/Header header
+
+Command[] joints

data_gen_dependencies/aimdk/protocol/hal/joint/msg/JointState.msg

@@ -0,0 +1,3 @@
+std_msgs/Header header
+
+State[] joints

data_gen_dependencies/aimdk/protocol/hal/joint/msg/State.msg

@@ -0,0 +1,5 @@
+string name
+uint32 sequence
+float64 position
+float64 velocity
+float64 effort

data_gen_dependencies/aimdk/protocol/hal/pose/msg/PoseCommand.msg

@@ -0,0 +1,6 @@
+float64 x
+float64 y
+float64 z
+float64 rx
+float64 ry
+float64 rz

data_gen_dependencies/aimdk/protocol/hal/pose/msg/PoseCommands.msg

@@ -0,0 +1,4 @@
+std_msgs/Header header
+
+string[] names
+PoseCommand[] poses

data_gen_dependencies/aimdk/protocol/hal/pose/msg/PoseState.msg

@@ -0,0 +1,6 @@
+float64 x
+float64 y
+float64 z
+float64 rx
+float64 ry
+float64 rz

data_gen_dependencies/aimdk/protocol/hal/pose/msg/PoseStates.msg

@@ -0,0 +1,5 @@
+std_msgs/Header header
+
+string[] names
+PoseState[] poses
+TorqSensorState[] torqs

data_gen_dependencies/aimdk/protocol/hal/pose/msg/TorqSensorState.msg

@@ -0,0 +1,3 @@
+float64[] act_torque
+float64[] torque
+float64[] real_torque

data_gen_dependencies/aimdk/protocol/hal/sensors/msg/FootSensorState.msg

@@ -0,0 +1,4 @@
+std_msgs/Header header
+
+string[] names
+SensorState[] states

data_gen_dependencies/aimdk/protocol/hal/sensors/msg/LivoxPoint.msg

@@ -0,0 +1,9 @@
+# Livox costom pointcloud format.
+
+uint32 offset_time      # offset time relative to the base time
+float32 x               # X axis, unit:m
+float32 y               # Y axis, unit:m
+float32 z               # Z axis, unit:m
+uint8 reflectivity      # reflectivity, 0~255
+uint8 tag               # livox tag
+uint8 line              # laser number in lidar

data_gen_dependencies/aimdk/protocol/hal/sensors/msg/LivoxPointCloud.msg

@@ -0,0 +1,8 @@
+# Livox publish pointcloud msg format.
+
+std_msgs/Header header    # ROS standard message header
+uint64 timebase           # The time of first point
+uint32 point_num          # Total number of pointclouds
+uint8  lidar_id           # Lidar device id number
+uint8[3]  rsvd            # Reserved use
+LivoxPoint[] points      # Pointcloud data

data_gen_dependencies/aimdk/protocol/hal/sensors/msg/SensorState.msg

@@ -0,0 +1 @@
+int16[] presure

data_gen_dependencies/aimdk/protocol/hal/sensors/rs2_camera.proto

@@ -0,0 +1,98 @@
+syntax = "proto3";
+package aimdk.protocol;
+
+
+/**
+ * @brief 图像类型，包括深度和彩色
+ */
+enum ImageType {
+  ImageType_UNDEFINED = 0;
+  ImageType_DEPTH = 1;
+  ImageType_COLOR = 2;
+}
+
+/**
+ * @brief 图像模型参数
+ */
+enum DistortionType {
+  DistortionType_UNDEFINED = 0;
+  DistortionType_MODIFIED_BROWN_CONRADY = 1;
+  DistortionType_INVERSE_BROWN_CONRADY = 2;
+  DistortionType_FTHETA = 3;
+  DistortionType_BROWN_CONRADY = 4;
+  DistortionType_KANNALA_BRANDT4 = 5;
+  DistortionType_COUNT = 6;
+}
+
+/**
+ * @brief 压缩图像数据
+ */
+message CompressedImage {
+  string format = 2;
+  bytes data = 3;
+}
+
+/**
+ * @brief 相机内参
+ */
+message CameraInfo {
+  int32 width = 1;
+  int32 height = 2;
+  float ppx = 3;
+  float ppy = 4;
+  float fx = 5;
+  float fy = 6;
+  DistortionType model = 7;
+  repeated float coeffs = 8;
+}
+
+/**
+ * @brief rpc GetCameraInfo() 的请求数据
+ */
+message GetCameraInfoRequest {
+  string serial_no = 1;
+  ImageType stream_type = 2;
+}
+
+/**
+ * @brief rpc GetCameraInfo() 的响应数据
+ */
+message GetCameraInfoResponse {
+  repeated CameraInfo info = 1;
+}
+
+/**
+ * @brief rpc GetCameraData() 的请求数据
+ */
+message GetCameraDataRequest {
+  string serial_no = 1;
+}
+
+/**
+ * @brief rpc GetCameraData() 的响应数据
+ */
+message GetCameraDataResponse {
+  string serial_no = 1;
+  CameraInfo color_info = 2;
+  CompressedImage color_image = 3;
+  CameraInfo depth_info = 4;
+  CompressedImage depth_image = 5;
+}
+
+/**
+ * @brief 由 aimrt_hal 中的 realsense_camera_module::RealsenseCameraModule 模块提供的服务
+ */
+service CameraService {
+  /**
+   * @brief 获取 realsense_camera_module::RealsenseCameraModule 收到的相机内参信息。
+   * @param GetCameraInfoRequest 相机序列号
+   * @return GetCameraInfoResponse 相机内参
+   */
+  rpc GetCameraInfo(GetCameraInfoRequest) returns (GetCameraInfoResponse);
+  /**
+   * @brief 获取 realsense_camera_module::RealsenseCameraModule 收到的相机数据，包括相机内参、彩色和深度图像信息。
+   * @param GetCameraDataRequest 相机序列号
+   * @return GetCameraDataResponse 详细数据
+   */
+  rpc GetCameraData(GetCameraDataRequest) returns (GetCameraDataResponse);
+}

data_gen_dependencies/aimdk/protocol/hal/sensors/rs2_camera_pb2.py

@@ -0,0 +1,42 @@
+# -*- coding: utf-8 -*-
+# Generated by the protocol buffer compiler.  DO NOT EDIT!
+# source: protocol/hal/sensors/rs2_camera.proto
+# Protobuf Python Version: 4.25.1
+"""Generated protocol buffer code."""
+from google.protobuf import descriptor as _descriptor
+from google.protobuf import descriptor_pool as _descriptor_pool
+from google.protobuf import symbol_database as _symbol_database
+from google.protobuf.internal import builder as _builder
+# @@protoc_insertion_point(imports)
+
+_sym_db = _symbol_database.Default()
+
+
+
+
+DESCRIPTOR = _descriptor_pool.Default().AddSerializedFile(b'\n%protocol/hal/sensors/rs2_camera.proto\x12\x0e\x61imdk.protocol\"/\n\x0f\x43ompressedImage\x12\x0e\n\x06\x66ormat\x18\x02 \x01(\t\x12\x0c\n\x04\x64\x61ta\x18\x03 \x01(\x0c\"\x9c\x01\n\nCameraInfo\x12\r\n\x05width\x18\x01 \x01(\x05\x12\x0e\n\x06height\x18\x02 \x01(\x05\x12\x0b\n\x03ppx\x18\x03 \x01(\x02\x12\x0b\n\x03ppy\x18\x04 \x01(\x02\x12\n\n\x02\x66x\x18\x05 \x01(\x02\x12\n\n\x02\x66y\x18\x06 \x01(\x02\x12-\n\x05model\x18\x07 \x01(\x0e\x32\x1e.aimdk.protocol.DistortionType\x12\x0e\n\x06\x63oeffs\x18\x08 \x03(\x02\"Y\n\x14GetCameraInfoRequest\x12\x11\n\tserial_no\x18\x01 \x01(\t\x12.\n\x0bstream_type\x18\x02 \x01(\x0e\x32\x19.aimdk.protocol.ImageType\"A\n\x15GetCameraInfoResponse\x12(\n\x04info\x18\x01 \x03(\x0b\x32\x1a.aimdk.protocol.CameraInfo\")\n\x14GetCameraDataRequest\x12\x11\n\tserial_no\x18\x01 \x01(\t\"\xf6\x01\n\x15GetCameraDataResponse\x12\x11\n\tserial_no\x18\x01 \x01(\t\x12.\n\ncolor_info\x18\x02 \x01(\x0b\x32\x1a.aimdk.protocol.CameraInfo\x12\x34\n\x0b\x63olor_image\x18\x03 \x01(\x0b\x32\x1f.aimdk.protocol.CompressedImage\x12.\n\ndepth_info\x18\x04 \x01(\x0b\x32\x1a.aimdk.protocol.CameraInfo\x12\x34\n\x0b\x64\x65pth_image\x18\x05 \x01(\x0b\x32\x1f.aimdk.protocol.CompressedImage*N\n\tImageType\x12\x17\n\x13ImageType_UNDEFINED\x10\x00\x12\x13\n\x0fImageType_DEPTH\x10\x01\x12\x13\n\x0fImageType_COLOR\x10\x02*\xfe\x01\n\x0e\x44istortionType\x12\x1c\n\x18\x44istortionType_UNDEFINED\x10\x00\x12)\n%DistortionType_MODIFIED_BROWN_CONRADY\x10\x01\x12(\n$DistortionType_INVERSE_BROWN_CONRADY\x10\x02\x12\x19\n\x15\x44istortionType_FTHETA\x10\x03\x12 \n\x1c\x44istortionType_BROWN_CONRADY\x10\x04\x12\"\n\x1e\x44istortionType_KANNALA_BRANDT4\x10\x05\x12\x18\n\x14\x44istortionType_COUNT\x10\x06\x32\xcb\x01\n\rCameraService\x12\\\n\rGetCameraInfo\x12$.aimdk.protocol.GetCameraInfoRequest\x1a%.aimdk.protocol.GetCameraInfoResponse\x12\\\n\rGetCameraData\x12$.aimdk.protocol.GetCameraDataRequest\x1a%.aimdk.protocol.GetCameraDataResponseb\x06proto3')
+
+_globals = globals()
+_builder.BuildMessageAndEnumDescriptors(DESCRIPTOR, _globals)
+_builder.BuildTopDescriptorsAndMessages(DESCRIPTOR, 'protocol.hal.sensors.rs2_camera_pb2', _globals)
+if _descriptor._USE_C_DESCRIPTORS == False:
+  DESCRIPTOR._options = None
+  _globals['_IMAGETYPE']._serialized_start=715
+  _globals['_IMAGETYPE']._serialized_end=793
+  _globals['_DISTORTIONTYPE']._serialized_start=796
+  _globals['_DISTORTIONTYPE']._serialized_end=1050
+  _globals['_COMPRESSEDIMAGE']._serialized_start=57
+  _globals['_COMPRESSEDIMAGE']._serialized_end=104
+  _globals['_CAMERAINFO']._serialized_start=107
+  _globals['_CAMERAINFO']._serialized_end=263
+  _globals['_GETCAMERAINFOREQUEST']._serialized_start=265
+  _globals['_GETCAMERAINFOREQUEST']._serialized_end=354
+  _globals['_GETCAMERAINFORESPONSE']._serialized_start=356
+  _globals['_GETCAMERAINFORESPONSE']._serialized_end=421
+  _globals['_GETCAMERADATAREQUEST']._serialized_start=423
+  _globals['_GETCAMERADATAREQUEST']._serialized_end=464
+  _globals['_GETCAMERADATARESPONSE']._serialized_start=467
+  _globals['_GETCAMERADATARESPONSE']._serialized_end=713
+  _globals['_CAMERASERVICE']._serialized_start=1053
+  _globals['_CAMERASERVICE']._serialized_end=1256
+# @@protoc_insertion_point(module_scope)

data_gen_dependencies/aimdk/protocol/hal/sensors/rs2_camera_pb2_grpc.py

@@ -0,0 +1,113 @@
+# Generated by the gRPC Python protocol compiler plugin. DO NOT EDIT!
+"""Client and server classes corresponding to protobuf-defined services."""
+import grpc
+
+from . import rs2_camera_pb2 as protocol_dot_hal_dot_sensors_dot_rs2__camera__pb2
+
+
+class CameraServiceStub(object):
+    """*
+    @brief 由 aimrt_hal 中的 realsense_camera_module::RealsenseCameraModule 模块提供的服务
+    """
+
+    def __init__(self, channel):
+        """Constructor.
+
+        Args:
+            channel: A grpc.Channel.
+        """
+        self.GetCameraInfo = channel.unary_unary(
+                '/aimdk.protocol.CameraService/GetCameraInfo',
+                request_serializer=protocol_dot_hal_dot_sensors_dot_rs2__camera__pb2.GetCameraInfoRequest.SerializeToString,
+                response_deserializer=protocol_dot_hal_dot_sensors_dot_rs2__camera__pb2.GetCameraInfoResponse.FromString,
+                )
+        self.GetCameraData = channel.unary_unary(
+                '/aimdk.protocol.CameraService/GetCameraData',
+                request_serializer=protocol_dot_hal_dot_sensors_dot_rs2__camera__pb2.GetCameraDataRequest.SerializeToString,
+                response_deserializer=protocol_dot_hal_dot_sensors_dot_rs2__camera__pb2.GetCameraDataResponse.FromString,
+                )
+
+
+class CameraServiceServicer(object):
+    """*
+    @brief 由 aimrt_hal 中的 realsense_camera_module::RealsenseCameraModule 模块提供的服务
+    """
+
+    def GetCameraInfo(self, request, context):
+        """*
+        @brief 获取 realsense_camera_module::RealsenseCameraModule 收到的相机内参信息。
+        @param GetCameraInfoRequest 相机序列号
+        @return GetCameraInfoResponse 相机内参
+        """
+        context.set_code(grpc.StatusCode.UNIMPLEMENTED)
+        context.set_details('Method not implemented!')
+        raise NotImplementedError('Method not implemented!')
+
+    def GetCameraData(self, request, context):
+        """*
+        @brief 获取 realsense_camera_module::RealsenseCameraModule 收到的相机数据，包括相机内参、彩色和深度图像信息。
+        @param GetCameraDataRequest 相机序列号
+        @return GetCameraDataResponse 详细数据
+        """
+        context.set_code(grpc.StatusCode.UNIMPLEMENTED)
+        context.set_details('Method not implemented!')
+        raise NotImplementedError('Method not implemented!')
+
+
+def add_CameraServiceServicer_to_server(servicer, server):
+    rpc_method_handlers = {
+            'GetCameraInfo': grpc.unary_unary_rpc_method_handler(
+                    servicer.GetCameraInfo,
+                    request_deserializer=protocol_dot_hal_dot_sensors_dot_rs2__camera__pb2.GetCameraInfoRequest.FromString,
+                    response_serializer=protocol_dot_hal_dot_sensors_dot_rs2__camera__pb2.GetCameraInfoResponse.SerializeToString,
+            ),
+            'GetCameraData': grpc.unary_unary_rpc_method_handler(
+                    servicer.GetCameraData,
+                    request_deserializer=protocol_dot_hal_dot_sensors_dot_rs2__camera__pb2.GetCameraDataRequest.FromString,
+                    response_serializer=protocol_dot_hal_dot_sensors_dot_rs2__camera__pb2.GetCameraDataResponse.SerializeToString,
+            ),
+    }
+    generic_handler = grpc.method_handlers_generic_handler(
+            'aimdk.protocol.CameraService', rpc_method_handlers)
+    server.add_generic_rpc_handlers((generic_handler,))
+
+
+ # This class is part of an EXPERIMENTAL API.
+class CameraService(object):
+    """*
+    @brief 由 aimrt_hal 中的 realsense_camera_module::RealsenseCameraModule 模块提供的服务
+    """
+
+    @staticmethod
+    def GetCameraInfo(request,
+            target,
+            options=(),
+            channel_credentials=None,
+            call_credentials=None,
+            insecure=False,
+            compression=None,
+            wait_for_ready=None,
+            timeout=None,
+            metadata=None):
+        return grpc.experimental.unary_unary(request, target, '/aimdk.protocol.CameraService/GetCameraInfo',
+            protocol_dot_hal_dot_sensors_dot_rs2__camera__pb2.GetCameraInfoRequest.SerializeToString,
+            protocol_dot_hal_dot_sensors_dot_rs2__camera__pb2.GetCameraInfoResponse.FromString,
+            options, channel_credentials,
+            insecure, call_credentials, compression, wait_for_ready, timeout, metadata)
+
+    @staticmethod
+    def GetCameraData(request,
+            target,
+            options=(),
+            channel_credentials=None,
+            call_credentials=None,
+            insecure=False,
+            compression=None,
+            wait_for_ready=None,
+            timeout=None,
+            metadata=None):
+        return grpc.experimental.unary_unary(request, target, '/aimdk.protocol.CameraService/GetCameraData',
+            protocol_dot_hal_dot_sensors_dot_rs2__camera__pb2.GetCameraDataRequest.SerializeToString,
+            protocol_dot_hal_dot_sensors_dot_rs2__camera__pb2.GetCameraDataResponse.FromString,
+            options, channel_credentials,
+            insecure, call_credentials, compression, wait_for_ready, timeout, metadata)

data_gen_dependencies/aimdk/protocol/sim/sim_camera_service.proto

@@ -0,0 +1,25 @@
+syntax = "proto3";
+package aimdk.protocol;
+
+message SemanticMask {
+    string name = 2;
+    bytes data = 3;
+}
+message SemanticLabel {
+    int32 label_id = 2;
+    string label_name = 3;
+}
+
+message GetSemanticRequest {
+    string serial_no = 1;
+}
+message GetSemanticResponse {
+    string serial_no = 1;
+    SemanticMask semantic_mask = 2;
+    repeated SemanticLabel label_dict = 3;
+}
+
+service SimCameraService {
+
+    rpc GetSemanticData(GetSemanticRequest) returns(GetSemanticResponse);
+}

data_gen_dependencies/aimdk/protocol/sim/sim_camera_service_pb2.py

@@ -0,0 +1,34 @@
+# -*- coding: utf-8 -*-
+# Generated by the protocol buffer compiler.  DO NOT EDIT!
+# source: protocol/sim/sim_camera_service.proto
+# Protobuf Python Version: 4.25.1
+"""Generated protocol buffer code."""
+from google.protobuf import descriptor as _descriptor
+from google.protobuf import descriptor_pool as _descriptor_pool
+from google.protobuf import symbol_database as _symbol_database
+from google.protobuf.internal import builder as _builder
+# @@protoc_insertion_point(imports)
+
+_sym_db = _symbol_database.Default()
+
+
+
+
+DESCRIPTOR = _descriptor_pool.Default().AddSerializedFile(b'\n%protocol/sim/sim_camera_service.proto\x12\x0e\x61imdk.protocol\"*\n\x0cSemanticMask\x12\x0c\n\x04name\x18\x02 \x01(\t\x12\x0c\n\x04\x64\x61ta\x18\x03 \x01(\x0c\"5\n\rSemanticLabel\x12\x10\n\x08label_id\x18\x02 \x01(\x05\x12\x12\n\nlabel_name\x18\x03 \x01(\t\"\'\n\x12GetSemanticRequest\x12\x11\n\tserial_no\x18\x01 \x01(\t\"\x90\x01\n\x13GetSemanticResponse\x12\x11\n\tserial_no\x18\x01 \x01(\t\x12\x33\n\rsemantic_mask\x18\x02 \x01(\x0b\x32\x1c.aimdk.protocol.SemanticMask\x12\x31\n\nlabel_dict\x18\x03 \x03(\x0b\x32\x1d.aimdk.protocol.SemanticLabel2n\n\x10SimCameraService\x12Z\n\x0fGetSemanticData\x12\".aimdk.protocol.GetSemanticRequest\x1a#.aimdk.protocol.GetSemanticResponseb\x06proto3')
+
+_globals = globals()
+_builder.BuildMessageAndEnumDescriptors(DESCRIPTOR, _globals)
+_builder.BuildTopDescriptorsAndMessages(DESCRIPTOR, 'protocol.sim.sim_camera_service_pb2', _globals)
+if _descriptor._USE_C_DESCRIPTORS == False:
+  DESCRIPTOR._options = None
+  _globals['_SEMANTICMASK']._serialized_start=57
+  _globals['_SEMANTICMASK']._serialized_end=99
+  _globals['_SEMANTICLABEL']._serialized_start=101
+  _globals['_SEMANTICLABEL']._serialized_end=154
+  _globals['_GETSEMANTICREQUEST']._serialized_start=156
+  _globals['_GETSEMANTICREQUEST']._serialized_end=195
+  _globals['_GETSEMANTICRESPONSE']._serialized_start=198
+  _globals['_GETSEMANTICRESPONSE']._serialized_end=342
+  _globals['_SIMCAMERASERVICE']._serialized_start=344
+  _globals['_SIMCAMERASERVICE']._serialized_end=454
+# @@protoc_insertion_point(module_scope)

data_gen_dependencies/aimdk/protocol/sim/sim_camera_service_pb2_grpc.py

@@ -0,0 +1,66 @@
+# Generated by the gRPC Python protocol compiler plugin. DO NOT EDIT!
+"""Client and server classes corresponding to protobuf-defined services."""
+import grpc
+
+from . import sim_camera_service_pb2 as protocol_dot_sim_dot_sim__camera__service__pb2
+
+
+class SimCameraServiceStub(object):
+    """Missing associated documentation comment in .proto file."""
+
+    def __init__(self, channel):
+        """Constructor.
+
+        Args:
+            channel: A grpc.Channel.
+        """
+        self.GetSemanticData = channel.unary_unary(
+                '/aimdk.protocol.SimCameraService/GetSemanticData',
+                request_serializer=protocol_dot_sim_dot_sim__camera__service__pb2.GetSemanticRequest.SerializeToString,
+                response_deserializer=protocol_dot_sim_dot_sim__camera__service__pb2.GetSemanticResponse.FromString,
+                )
+
+
+class SimCameraServiceServicer(object):
+    """Missing associated documentation comment in .proto file."""
+
+    def GetSemanticData(self, request, context):
+        """Missing associated documentation comment in .proto file."""
+        context.set_code(grpc.StatusCode.UNIMPLEMENTED)
+        context.set_details('Method not implemented!')
+        raise NotImplementedError('Method not implemented!')
+
+
+def add_SimCameraServiceServicer_to_server(servicer, server):
+    rpc_method_handlers = {
+            'GetSemanticData': grpc.unary_unary_rpc_method_handler(
+                    servicer.GetSemanticData,
+                    request_deserializer=protocol_dot_sim_dot_sim__camera__service__pb2.GetSemanticRequest.FromString,
+                    response_serializer=protocol_dot_sim_dot_sim__camera__service__pb2.GetSemanticResponse.SerializeToString,
+            ),
+    }
+    generic_handler = grpc.method_handlers_generic_handler(
+            'aimdk.protocol.SimCameraService', rpc_method_handlers)
+    server.add_generic_rpc_handlers((generic_handler,))
+
+
+ # This class is part of an EXPERIMENTAL API.
+class SimCameraService(object):
+    """Missing associated documentation comment in .proto file."""
+
+    @staticmethod
+    def GetSemanticData(request,
+            target,
+            options=(),
+            channel_credentials=None,
+            call_credentials=None,
+            insecure=False,
+            compression=None,
+            wait_for_ready=None,
+            timeout=None,
+            metadata=None):
+        return grpc.experimental.unary_unary(request, target, '/aimdk.protocol.SimCameraService/GetSemanticData',
+            protocol_dot_sim_dot_sim__camera__service__pb2.GetSemanticRequest.SerializeToString,
+            protocol_dot_sim_dot_sim__camera__service__pb2.GetSemanticResponse.FromString,
+            options, channel_credentials,
+            insecure, call_credentials, compression, wait_for_ready, timeout, metadata)

data_gen_dependencies/aimdk/protocol/sim/sim_gripper_service.proto

@@ -0,0 +1,24 @@
+syntax = "proto3";
+package aimdk.protocol;
+
+import public "aimdk/protocol/common/se3_pose.proto";
+
+message SetGripperStateReq {
+    string gripper_command = 1;
+    bool is_right = 2;
+    double opened_width = 3;
+}
+message SetGripperStateRsp {
+    string msg = 1;
+}
+message GetGripperStateReq {
+    bool is_right = 1;
+}
+message GetGripperStateRsp {
+    string gripper_name = 1
+    SE3RpyPose gripper_pose = 2;
+}
+service SimGripperService {
+    rpc SetGripperState(SetGripperStateReq) returns(SetGripperStateRsp);
+    rpc GetGripperState(GetGripperStateReq) returns(GetGripperStateRsp);
+}

data_gen_dependencies/aimdk/protocol/sim/sim_gripper_service_pb2.py

@@ -0,0 +1,48 @@
+# -*- coding: utf-8 -*-
+# Generated by the protocol buffer compiler.  DO NOT EDIT!
+# source: aimdk/protocol/sim/sim_gripper_service.proto
+# Protobuf Python Version: 4.25.1
+"""Generated protocol buffer code."""
+from google.protobuf import descriptor as _descriptor
+from google.protobuf import descriptor_pool as _descriptor_pool
+from google.protobuf import symbol_database as _symbol_database
+from google.protobuf.internal import builder as _builder
+# @@protoc_insertion_point(imports)
+
+_sym_db = _symbol_database.Default()
+
+
+from aimdk.protocol.common import se3_pose_pb2 as aimdk_dot_protocol_dot_common_dot_se3__pose__pb2
+try:
+  aimdk_dot_protocol_dot_common_dot_vec3__pb2 = aimdk_dot_protocol_dot_common_dot_se3__pose__pb2.aimdk_dot_protocol_dot_common_dot_vec3__pb2
+except AttributeError:
+  aimdk_dot_protocol_dot_common_dot_vec3__pb2 = aimdk_dot_protocol_dot_common_dot_se3__pose__pb2.aimdk.protocol.common.vec3_pb2
+try:
+  aimdk_dot_protocol_dot_common_dot_quaternion__pb2 = aimdk_dot_protocol_dot_common_dot_se3__pose__pb2.aimdk_dot_protocol_dot_common_dot_quaternion__pb2
+except AttributeError:
+  aimdk_dot_protocol_dot_common_dot_quaternion__pb2 = aimdk_dot_protocol_dot_common_dot_se3__pose__pb2.aimdk.protocol.common.quaternion_pb2
+try:
+  aimdk_dot_protocol_dot_common_dot_rpy__pb2 = aimdk_dot_protocol_dot_common_dot_se3__pose__pb2.aimdk_dot_protocol_dot_common_dot_rpy__pb2
+except AttributeError:
+  aimdk_dot_protocol_dot_common_dot_rpy__pb2 = aimdk_dot_protocol_dot_common_dot_se3__pose__pb2.aimdk.protocol.common.rpy_pb2
+
+from aimdk.protocol.common.se3_pose_pb2 import *
+
+DESCRIPTOR = _descriptor_pool.Default().AddSerializedFile(b'\n,aimdk/protocol/sim/sim_gripper_service.proto\x12\x0e\x61imdk.protocol\x1a$aimdk/protocol/common/se3_pose.proto\"U\n\x12SetGripperStateReq\x12\x17\n\x0fgripper_command\x18\x01 \x01(\t\x12\x10\n\x08is_right\x18\x02 \x01(\x08\x12\x14\n\x0copened_width\x18\x03 \x01(\x01\"!\n\x12SetGripperStateRsp\x12\x0b\n\x03msg\x18\x01 \x01(\t\"&\n\x12GetGripperStateReq\x12\x10\n\x08is_right\x18\x01 \x01(\x08\"F\n\x12GetGripperStateRsp\x12\x30\n\x0cgripper_pose\x18\x01 \x01(\x0b\x32\x1a.aimdk.protocol.SE3RpyPose2\xc9\x01\n\x11SimGripperService\x12Y\n\x0fSetGripperState\x12\".aimdk.protocol.SetGripperStateReq\x1a\".aimdk.protocol.SetGripperStateRsp\x12Y\n\x0fGetGripperState\x12\".aimdk.protocol.GetGripperStateReq\x1a\".aimdk.protocol.GetGripperStateRspP\x00\x62\x06proto3')
+
+_globals = globals()
+_builder.BuildMessageAndEnumDescriptors(DESCRIPTOR, _globals)
+_builder.BuildTopDescriptorsAndMessages(DESCRIPTOR, 'aimdk.protocol.sim.sim_gripper_service_pb2', _globals)
+if _descriptor._USE_C_DESCRIPTORS == False:
+  DESCRIPTOR._options = None
+  _globals['_SETGRIPPERSTATEREQ']._serialized_start=102
+  _globals['_SETGRIPPERSTATEREQ']._serialized_end=187
+  _globals['_SETGRIPPERSTATERSP']._serialized_start=189
+  _globals['_SETGRIPPERSTATERSP']._serialized_end=222
+  _globals['_GETGRIPPERSTATEREQ']._serialized_start=224
+  _globals['_GETGRIPPERSTATEREQ']._serialized_end=262
+  _globals['_GETGRIPPERSTATERSP']._serialized_start=264
+  _globals['_GETGRIPPERSTATERSP']._serialized_end=334
+  _globals['_SIMGRIPPERSERVICE']._serialized_start=337
+  _globals['_SIMGRIPPERSERVICE']._serialized_end=538
+# @@protoc_insertion_point(module_scope)

data_gen_dependencies/aimdk/protocol/sim/sim_gripper_service_pb2_grpc.py

@@ -0,0 +1,99 @@
+# Generated by the gRPC Python protocol compiler plugin. DO NOT EDIT!
+"""Client and server classes corresponding to protobuf-defined services."""
+import grpc
+
+from aimdk.protocol.sim import sim_gripper_service_pb2 as aimdk_dot_protocol_dot_sim_dot_sim__gripper__service__pb2
+
+
+class SimGripperServiceStub(object):
+    """Missing associated documentation comment in .proto file."""
+
+    def __init__(self, channel):
+        """Constructor.
+
+        Args:
+            channel: A grpc.Channel.
+        """
+        self.SetGripperState = channel.unary_unary(
+                '/aimdk.protocol.SimGripperService/SetGripperState',
+                request_serializer=aimdk_dot_protocol_dot_sim_dot_sim__gripper__service__pb2.SetGripperStateReq.SerializeToString,
+                response_deserializer=aimdk_dot_protocol_dot_sim_dot_sim__gripper__service__pb2.SetGripperStateRsp.FromString,
+                )
+        self.GetGripperState = channel.unary_unary(
+                '/aimdk.protocol.SimGripperService/GetGripperState',
+                request_serializer=aimdk_dot_protocol_dot_sim_dot_sim__gripper__service__pb2.GetGripperStateReq.SerializeToString,
+                response_deserializer=aimdk_dot_protocol_dot_sim_dot_sim__gripper__service__pb2.GetGripperStateRsp.FromString,
+                )
+
+
+class SimGripperServiceServicer(object):
+    """Missing associated documentation comment in .proto file."""
+
+    def SetGripperState(self, request, context):
+        """Missing associated documentation comment in .proto file."""
+        context.set_code(grpc.StatusCode.UNIMPLEMENTED)
+        context.set_details('Method not implemented!')
+        raise NotImplementedError('Method not implemented!')
+
+    def GetGripperState(self, request, context):
+        """Missing associated documentation comment in .proto file."""
+        context.set_code(grpc.StatusCode.UNIMPLEMENTED)
+        context.set_details('Method not implemented!')
+        raise NotImplementedError('Method not implemented!')
+
+
+def add_SimGripperServiceServicer_to_server(servicer, server):
+    rpc_method_handlers = {
+            'SetGripperState': grpc.unary_unary_rpc_method_handler(
+                    servicer.SetGripperState,
+                    request_deserializer=aimdk_dot_protocol_dot_sim_dot_sim__gripper__service__pb2.SetGripperStateReq.FromString,
+                    response_serializer=aimdk_dot_protocol_dot_sim_dot_sim__gripper__service__pb2.SetGripperStateRsp.SerializeToString,
+            ),
+            'GetGripperState': grpc.unary_unary_rpc_method_handler(
+                    servicer.GetGripperState,
+                    request_deserializer=aimdk_dot_protocol_dot_sim_dot_sim__gripper__service__pb2.GetGripperStateReq.FromString,
+                    response_serializer=aimdk_dot_protocol_dot_sim_dot_sim__gripper__service__pb2.GetGripperStateRsp.SerializeToString,
+            ),
+    }
+    generic_handler = grpc.method_handlers_generic_handler(
+            'aimdk.protocol.SimGripperService', rpc_method_handlers)
+    server.add_generic_rpc_handlers((generic_handler,))
+
+
+ # This class is part of an EXPERIMENTAL API.
+class SimGripperService(object):
+    """Missing associated documentation comment in .proto file."""
+
+    @staticmethod
+    def SetGripperState(request,
+            target,
+            options=(),
+            channel_credentials=None,
+            call_credentials=None,
+            insecure=False,
+            compression=None,
+            wait_for_ready=None,
+            timeout=None,
+            metadata=None):
+        return grpc.experimental.unary_unary(request, target, '/aimdk.protocol.SimGripperService/SetGripperState',
+            aimdk_dot_protocol_dot_sim_dot_sim__gripper__service__pb2.SetGripperStateReq.SerializeToString,
+            aimdk_dot_protocol_dot_sim_dot_sim__gripper__service__pb2.SetGripperStateRsp.FromString,
+            options, channel_credentials,
+            insecure, call_credentials, compression, wait_for_ready, timeout, metadata)
+
+    @staticmethod
+    def GetGripperState(request,
+            target,
+            options=(),
+            channel_credentials=None,
+            call_credentials=None,
+            insecure=False,
+            compression=None,
+            wait_for_ready=None,
+            timeout=None,
+            metadata=None):
+        return grpc.experimental.unary_unary(request, target, '/aimdk.protocol.SimGripperService/GetGripperState',
+            aimdk_dot_protocol_dot_sim_dot_sim__gripper__service__pb2.GetGripperStateReq.SerializeToString,
+            aimdk_dot_protocol_dot_sim_dot_sim__gripper__service__pb2.GetGripperStateRsp.FromString,
+            options, channel_credentials,
+            insecure, call_credentials, compression, wait_for_ready, timeout, metadata)

data_gen_dependencies/aimdk/protocol/sim/sim_object_service.proto

@@ -0,0 +1,57 @@
+syntax = "proto3";
+package aimdk.protocol;
+
+import public "aimdk/protocol/common/se3_pose.proto";
+import public "aimdk/protocol/common/vec3.proto";
+
+message AddObjectReq {
+    string usd_path = 1;        //物体usd资源位置
+    string prim_path = 2;       //物体摆放在场景中的primpath
+    string label_name = 3;      //物体标签名称
+    SE3RpyPose object_pose = 4; //物体放置的位姿
+    Vec3 object_scale = 5;      //物体缩放比例
+    Color object_color = 6;
+    string object_material = 7;
+    double object_mass = 8;
+    bool add_particle = 9;
+    Vec3 particle_position = 10;
+    Vec3 particle_scale = 11;
+    Color particle_color = 12;
+}
+message Color {
+    float r = 1;
+    float g = 2;
+    float b = 3;
+}
+message AddObjectRsp {
+    string prim_path = 1;
+    string label_name = 2;
+}
+message GetObjectPoseReq {
+    string prim_path = 1;
+}
+message GetObjectPoseRsp {
+    string prim_path = 1;
+    SE3RpyPose object_pose = 2;
+}
+message GetObjectJointReq {
+    string prim_path = 1;
+}
+message GetObjectJointRsp {
+    repeated string joint_names = 1;
+    repeated double joint_positions = 2;
+    repeated double joint_velocities = 3;
+}
+message SetTargetPointReq {
+    Vec3 point_position = 1;
+}
+message SetTargetPointRsp {
+    string msg = 1;
+}
+
+service SimObjectService {
+    rpc AddObject(AddObjectReq) returns(AddObjectRsp);
+    rpc GetObjectPose(GetObjectPoseReq) returns(GetObjectPoseRsp);
+    rpc GetObjectJoint(GetObjectJointReq) returns(GetObjectJointRsp);
+    rpc SetTargetPoint(SetTargetPointReq) returns(SetTargetPointRsp);
+}

data_gen_dependencies/aimdk/protocol/sim/sim_object_service_pb2.py

@@ -0,0 +1,60 @@
+# -*- coding: utf-8 -*-
+# Generated by the protocol buffer compiler.  DO NOT EDIT!
+# source: aimdk/protocol/sim/sim_object_service.proto
+# Protobuf Python Version: 4.25.1
+"""Generated protocol buffer code."""
+from google.protobuf import descriptor as _descriptor
+from google.protobuf import descriptor_pool as _descriptor_pool
+from google.protobuf import symbol_database as _symbol_database
+from google.protobuf.internal import builder as _builder
+# @@protoc_insertion_point(imports)
+
+_sym_db = _symbol_database.Default()
+
+
+from aimdk.protocol.common import se3_pose_pb2 as aimdk_dot_protocol_dot_common_dot_se3__pose__pb2
+try:
+  aimdk_dot_protocol_dot_common_dot_vec3__pb2 = aimdk_dot_protocol_dot_common_dot_se3__pose__pb2.aimdk_dot_protocol_dot_common_dot_vec3__pb2
+except AttributeError:
+  aimdk_dot_protocol_dot_common_dot_vec3__pb2 = aimdk_dot_protocol_dot_common_dot_se3__pose__pb2.aimdk.protocol.common.vec3_pb2
+try:
+  aimdk_dot_protocol_dot_common_dot_quaternion__pb2 = aimdk_dot_protocol_dot_common_dot_se3__pose__pb2.aimdk_dot_protocol_dot_common_dot_quaternion__pb2
+except AttributeError:
+  aimdk_dot_protocol_dot_common_dot_quaternion__pb2 = aimdk_dot_protocol_dot_common_dot_se3__pose__pb2.aimdk.protocol.common.quaternion_pb2
+try:
+  aimdk_dot_protocol_dot_common_dot_rpy__pb2 = aimdk_dot_protocol_dot_common_dot_se3__pose__pb2.aimdk_dot_protocol_dot_common_dot_rpy__pb2
+except AttributeError:
+  aimdk_dot_protocol_dot_common_dot_rpy__pb2 = aimdk_dot_protocol_dot_common_dot_se3__pose__pb2.aimdk.protocol.common.rpy_pb2
+from aimdk.protocol.common import vec3_pb2 as aimdk_dot_protocol_dot_common_dot_vec3__pb2
+
+from aimdk.protocol.common.se3_pose_pb2 import *
+from aimdk.protocol.common.vec3_pb2 import *
+
+DESCRIPTOR = _descriptor_pool.Default().AddSerializedFile(b'\n+aimdk/protocol/sim/sim_object_service.proto\x12\x0e\x61imdk.protocol\x1a$aimdk/protocol/common/se3_pose.proto\x1a aimdk/protocol/common/vec3.proto\"\xa3\x03\n\x0c\x41\x64\x64ObjectReq\x12\x10\n\x08usd_path\x18\x01 \x01(\t\x12\x11\n\tprim_path\x18\x02 \x01(\t\x12\x12\n\nlabel_name\x18\x03 \x01(\t\x12/\n\x0bobject_pose\x18\x04 \x01(\x0b\x32\x1a.aimdk.protocol.SE3RpyPose\x12*\n\x0cobject_scale\x18\x05 \x01(\x0b\x32\x14.aimdk.protocol.Vec3\x12+\n\x0cobject_color\x18\x06 \x01(\x0b\x32\x15.aimdk.protocol.Color\x12\x17\n\x0fobject_material\x18\x07 \x01(\t\x12\x13\n\x0bobject_mass\x18\x08 \x01(\x01\x12\x14\n\x0c\x61\x64\x64_particle\x18\t \x01(\x08\x12/\n\x11particle_position\x18\n \x01(\x0b\x32\x14.aimdk.protocol.Vec3\x12,\n\x0eparticle_scale\x18\x0b \x01(\x0b\x32\x14.aimdk.protocol.Vec3\x12-\n\x0eparticle_color\x18\x0c \x01(\x0b\x32\x15.aimdk.protocol.Color\"(\n\x05\x43olor\x12\t\n\x01r\x18\x01 \x01(\x02\x12\t\n\x01g\x18\x02 \x01(\x02\x12\t\n\x01\x62\x18\x03 \x01(\x02\"5\n\x0c\x41\x64\x64ObjectRsp\x12\x11\n\tprim_path\x18\x01 \x01(\t\x12\x12\n\nlabel_name\x18\x02 \x01(\t\"%\n\x10GetObjectPoseReq\x12\x11\n\tprim_path\x18\x01 \x01(\t\"V\n\x10GetObjectPoseRsp\x12\x11\n\tprim_path\x18\x01 \x01(\t\x12/\n\x0bobject_pose\x18\x02 \x01(\x0b\x32\x1a.aimdk.protocol.SE3RpyPose\"&\n\x11GetObjectJointReq\x12\x11\n\tprim_path\x18\x01 \x01(\t\"[\n\x11GetObjectJointRsp\x12\x13\n\x0bjoint_names\x18\x01 \x03(\t\x12\x17\n\x0fjoint_positions\x18\x02 \x03(\x01\x12\x18\n\x10joint_velocities\x18\x03 \x03(\x01\"A\n\x11SetTargetPointReq\x12,\n\x0epoint_position\x18\x01 \x01(\x0b\x32\x14.aimdk.protocol.Vec3\" \n\x11SetTargetPointRsp\x12\x0b\n\x03msg\x18\x01 \x01(\t2\xe0\x02\n\x10SimObjectService\x12G\n\tAddObject\x12\x1c.aimdk.protocol.AddObjectReq\x1a\x1c.aimdk.protocol.AddObjectRsp\x12S\n\rGetObjectPose\x12 .aimdk.protocol.GetObjectPoseReq\x1a .aimdk.protocol.GetObjectPoseRsp\x12V\n\x0eGetObjectJoint\x12!.aimdk.protocol.GetObjectJointReq\x1a!.aimdk.protocol.GetObjectJointRsp\x12V\n\x0eSetTargetPoint\x12!.aimdk.protocol.SetTargetPointReq\x1a!.aimdk.protocol.SetTargetPointRspP\x00P\x01\x62\x06proto3')
+
+_globals = globals()
+_builder.BuildMessageAndEnumDescriptors(DESCRIPTOR, _globals)
+_builder.BuildTopDescriptorsAndMessages(DESCRIPTOR, 'aimdk.protocol.sim.sim_object_service_pb2', _globals)
+if _descriptor._USE_C_DESCRIPTORS == False:
+  DESCRIPTOR._options = None
+  _globals['_ADDOBJECTREQ']._serialized_start=136
+  _globals['_ADDOBJECTREQ']._serialized_end=555
+  _globals['_COLOR']._serialized_start=557
+  _globals['_COLOR']._serialized_end=597
+  _globals['_ADDOBJECTRSP']._serialized_start=599
+  _globals['_ADDOBJECTRSP']._serialized_end=652
+  _globals['_GETOBJECTPOSEREQ']._serialized_start=654
+  _globals['_GETOBJECTPOSEREQ']._serialized_end=691
+  _globals['_GETOBJECTPOSERSP']._serialized_start=693
+  _globals['_GETOBJECTPOSERSP']._serialized_end=779
+  _globals['_GETOBJECTJOINTREQ']._serialized_start=781
+  _globals['_GETOBJECTJOINTREQ']._serialized_end=819
+  _globals['_GETOBJECTJOINTRSP']._serialized_start=821
+  _globals['_GETOBJECTJOINTRSP']._serialized_end=912
+  _globals['_SETTARGETPOINTREQ']._serialized_start=914
+  _globals['_SETTARGETPOINTREQ']._serialized_end=979
+  _globals['_SETTARGETPOINTRSP']._serialized_start=981
+  _globals['_SETTARGETPOINTRSP']._serialized_end=1013
+  _globals['_SIMOBJECTSERVICE']._serialized_start=1016
+  _globals['_SIMOBJECTSERVICE']._serialized_end=1368
+# @@protoc_insertion_point(module_scope)

data_gen_dependencies/aimdk/protocol/sim/sim_object_service_pb2_grpc.py

@@ -0,0 +1,165 @@
+# Generated by the gRPC Python protocol compiler plugin. DO NOT EDIT!
+"""Client and server classes corresponding to protobuf-defined services."""
+import grpc
+
+from aimdk.protocol.sim import sim_object_service_pb2 as aimdk_dot_protocol_dot_sim_dot_sim__object__service__pb2
+
+
+class SimObjectServiceStub(object):
+    """Missing associated documentation comment in .proto file."""
+
+    def __init__(self, channel):
+        """Constructor.
+
+        Args:
+            channel: A grpc.Channel.
+        """
+        self.AddObject = channel.unary_unary(
+                '/aimdk.protocol.SimObjectService/AddObject',
+                request_serializer=aimdk_dot_protocol_dot_sim_dot_sim__object__service__pb2.AddObjectReq.SerializeToString,
+                response_deserializer=aimdk_dot_protocol_dot_sim_dot_sim__object__service__pb2.AddObjectRsp.FromString,
+                )
+        self.GetObjectPose = channel.unary_unary(
+                '/aimdk.protocol.SimObjectService/GetObjectPose',
+                request_serializer=aimdk_dot_protocol_dot_sim_dot_sim__object__service__pb2.GetObjectPoseReq.SerializeToString,
+                response_deserializer=aimdk_dot_protocol_dot_sim_dot_sim__object__service__pb2.GetObjectPoseRsp.FromString,
+                )
+        self.GetObjectJoint = channel.unary_unary(
+                '/aimdk.protocol.SimObjectService/GetObjectJoint',
+                request_serializer=aimdk_dot_protocol_dot_sim_dot_sim__object__service__pb2.GetObjectJointReq.SerializeToString,
+                response_deserializer=aimdk_dot_protocol_dot_sim_dot_sim__object__service__pb2.GetObjectJointRsp.FromString,
+                )
+        self.SetTargetPoint = channel.unary_unary(
+                '/aimdk.protocol.SimObjectService/SetTargetPoint',
+                request_serializer=aimdk_dot_protocol_dot_sim_dot_sim__object__service__pb2.SetTargetPointReq.SerializeToString,
+                response_deserializer=aimdk_dot_protocol_dot_sim_dot_sim__object__service__pb2.SetTargetPointRsp.FromString,
+                )
+
+
+class SimObjectServiceServicer(object):
+    """Missing associated documentation comment in .proto file."""
+
+    def AddObject(self, request, context):
+        """Missing associated documentation comment in .proto file."""
+        context.set_code(grpc.StatusCode.UNIMPLEMENTED)
+        context.set_details('Method not implemented!')
+        raise NotImplementedError('Method not implemented!')
+
+    def GetObjectPose(self, request, context):
+        """Missing associated documentation comment in .proto file."""
+        context.set_code(grpc.StatusCode.UNIMPLEMENTED)
+        context.set_details('Method not implemented!')
+        raise NotImplementedError('Method not implemented!')
+
+    def GetObjectJoint(self, request, context):
+        """Missing associated documentation comment in .proto file."""
+        context.set_code(grpc.StatusCode.UNIMPLEMENTED)
+        context.set_details('Method not implemented!')
+        raise NotImplementedError('Method not implemented!')
+
+    def SetTargetPoint(self, request, context):
+        """Missing associated documentation comment in .proto file."""
+        context.set_code(grpc.StatusCode.UNIMPLEMENTED)
+        context.set_details('Method not implemented!')
+        raise NotImplementedError('Method not implemented!')
+
+
+def add_SimObjectServiceServicer_to_server(servicer, server):
+    rpc_method_handlers = {
+            'AddObject': grpc.unary_unary_rpc_method_handler(
+                    servicer.AddObject,
+                    request_deserializer=aimdk_dot_protocol_dot_sim_dot_sim__object__service__pb2.AddObjectReq.FromString,
+                    response_serializer=aimdk_dot_protocol_dot_sim_dot_sim__object__service__pb2.AddObjectRsp.SerializeToString,
+            ),
+            'GetObjectPose': grpc.unary_unary_rpc_method_handler(
+                    servicer.GetObjectPose,
+                    request_deserializer=aimdk_dot_protocol_dot_sim_dot_sim__object__service__pb2.GetObjectPoseReq.FromString,
+                    response_serializer=aimdk_dot_protocol_dot_sim_dot_sim__object__service__pb2.GetObjectPoseRsp.SerializeToString,
+            ),
+            'GetObjectJoint': grpc.unary_unary_rpc_method_handler(
+                    servicer.GetObjectJoint,
+                    request_deserializer=aimdk_dot_protocol_dot_sim_dot_sim__object__service__pb2.GetObjectJointReq.FromString,
+                    response_serializer=aimdk_dot_protocol_dot_sim_dot_sim__object__service__pb2.GetObjectJointRsp.SerializeToString,
+            ),
+            'SetTargetPoint': grpc.unary_unary_rpc_method_handler(
+                    servicer.SetTargetPoint,
+                    request_deserializer=aimdk_dot_protocol_dot_sim_dot_sim__object__service__pb2.SetTargetPointReq.FromString,
+                    response_serializer=aimdk_dot_protocol_dot_sim_dot_sim__object__service__pb2.SetTargetPointRsp.SerializeToString,
+            ),
+    }
+    generic_handler = grpc.method_handlers_generic_handler(
+            'aimdk.protocol.SimObjectService', rpc_method_handlers)
+    server.add_generic_rpc_handlers((generic_handler,))
+
+
+ # This class is part of an EXPERIMENTAL API.
+class SimObjectService(object):
+    """Missing associated documentation comment in .proto file."""
+
+    @staticmethod
+    def AddObject(request,
+            target,
+            options=(),
+            channel_credentials=None,
+            call_credentials=None,
+            insecure=False,
+            compression=None,
+            wait_for_ready=None,
+            timeout=None,
+            metadata=None):
+        return grpc.experimental.unary_unary(request, target, '/aimdk.protocol.SimObjectService/AddObject',
+            aimdk_dot_protocol_dot_sim_dot_sim__object__service__pb2.AddObjectReq.SerializeToString,
+            aimdk_dot_protocol_dot_sim_dot_sim__object__service__pb2.AddObjectRsp.FromString,
+            options, channel_credentials,
+            insecure, call_credentials, compression, wait_for_ready, timeout, metadata)
+
+    @staticmethod
+    def GetObjectPose(request,
+            target,
+            options=(),
+            channel_credentials=None,
+            call_credentials=None,
+            insecure=False,
+            compression=None,
+            wait_for_ready=None,
+            timeout=None,
+            metadata=None):
+        return grpc.experimental.unary_unary(request, target, '/aimdk.protocol.SimObjectService/GetObjectPose',
+            aimdk_dot_protocol_dot_sim_dot_sim__object__service__pb2.GetObjectPoseReq.SerializeToString,
+            aimdk_dot_protocol_dot_sim_dot_sim__object__service__pb2.GetObjectPoseRsp.FromString,
+            options, channel_credentials,
+            insecure, call_credentials, compression, wait_for_ready, timeout, metadata)
+
+    @staticmethod
+    def GetObjectJoint(request,
+            target,
+            options=(),
+            channel_credentials=None,
+            call_credentials=None,
+            insecure=False,
+            compression=None,
+            wait_for_ready=None,
+            timeout=None,
+            metadata=None):
+        return grpc.experimental.unary_unary(request, target, '/aimdk.protocol.SimObjectService/GetObjectJoint',
+            aimdk_dot_protocol_dot_sim_dot_sim__object__service__pb2.GetObjectJointReq.SerializeToString,
+            aimdk_dot_protocol_dot_sim_dot_sim__object__service__pb2.GetObjectJointRsp.FromString,
+            options, channel_credentials,
+            insecure, call_credentials, compression, wait_for_ready, timeout, metadata)
+
+    @staticmethod
+    def SetTargetPoint(request,
+            target,
+            options=(),
+            channel_credentials=None,
+            call_credentials=None,
+            insecure=False,
+            compression=None,
+            wait_for_ready=None,
+            timeout=None,
+            metadata=None):
+        return grpc.experimental.unary_unary(request, target, '/aimdk.protocol.SimObjectService/SetTargetPoint',
+            aimdk_dot_protocol_dot_sim_dot_sim__object__service__pb2.SetTargetPointReq.SerializeToString,
+            aimdk_dot_protocol_dot_sim_dot_sim__object__service__pb2.SetTargetPointRsp.FromString,
+            options, channel_credentials,
+            insecure, call_credentials, compression, wait_for_ready, timeout, metadata)

data_gen_dependencies/aimdk/protocol/sim/sim_observation_service.proto

@@ -0,0 +1,287 @@
+syntax = "proto3";
+package aimdk.protocol;
+
+import public "aimdk/protocol/common/se3_pose.proto";
+import public "aimdk/protocol/common/joint.proto";
+
+/**
+ * @brief 标签信息
+ */
+message SemanticData {
+    string name = 2;
+    bytes data = 3;
+}
+message SemanticDict {
+    int32 label_id = 2;
+    string label_name = 3;
+}
+/**
+ * @brief 压缩图像数据
+ */
+message CamImage {
+  string format = 2;
+  bytes data = 3;
+}
+
+/**
+ * @brief 相机内参
+ */
+message CamInfo {
+  int32 width = 1;
+  int32 height = 2;
+  float ppx = 3;
+  float ppy = 4;
+  float fx = 5;
+  float fy = 6;
+}
+
+message CameraRsp {
+    CamInfo camera_info = 1;
+    CamImage rgb_camera = 2;
+    CamImage depth_camera = 3;
+    SemanticData semantic_mask = 4;
+    repeated SemanticDict label_dict = 5;
+}
+
+message JointRsp {
+    repeated JointState left_arm = 1;
+    repeated JointState right_arm = 2;
+    repeated JointState body_arm = 3;
+
+}
+
+message ObjectRsp {
+    SE3RpyPose object_pose = 7;
+}
+message GripperRsp {
+    SE3RpyPose left_gripper = 1;
+    SE3RpyPose right_gripper = 2;
+
+}
+
+message CameraRequest{
+    bool render_depth = 1;
+    bool render_semantic = 2;
+    repeated string camera_prim_list = 3;
+}
+
+
+message GripperRequest{
+    bool left = 1;
+    bool right = 2;
+}
+
+
+/**
+ * @brief request
+ */
+message GetObservationReq {
+    bool isCam = 1;
+    CameraRequest CameraReq = 2;
+    bool isJoint = 3;
+    bool isPose = 4;
+    repeated string objectPrims = 5;
+    bool isGripper = 6;
+    GripperRequest gripperReq = 7;
+    bool startRecording = 8;
+    bool stopRecording = 9;
+    int32 fps = 10;
+    string task_name = 11;
+}
+/**
+ * @brief response
+ */
+message GetObservationRsp {
+    repeated CameraRsp camera = 1;
+    repeated ObjectRsp pose = 2;
+    JointRsp joint = 3;
+    GripperRsp gripper = 4;
+    string recordingState = 5;
+}
+
+
+message ResetReq{
+    bool reset = 1;
+}
+message ResetRsp{
+    string msg = 1;
+}
+message AttachReq{
+    repeated string obj_prims = 1;
+    bool is_right = 2; 
+}
+message AttachRsp{
+    string msg = 1;
+}
+message DetachReq{
+    bool detach = 1;
+}
+message DetachRsp{
+    string msg = 1;
+}
+message MultiMoveReq{
+    string robot_name = 1;
+    bool plan = 2;
+    repeated SE3RpyPose poses = 3;
+    CmdPlan cmd_plan = 4;
+}
+message MultiMoveRsp{
+    repeated CmdPlan cmd_plans = 1;
+    string msg = 2;
+}
+message CmdPlan{
+    repeated string joint_names = 1;
+    repeated SinglePlan joint_plans = 2;
+}
+message SinglePlan{
+    repeated float joint_pos = 1;
+}
+message TaskStatusReq{
+    bool isSuccess = 1;
+    repeated int32 failStep =2;
+
+}
+message TaskStatusRsp{
+    string msg = 1;
+}
+
+message ExitReq{
+    bool exit = 1;
+}
+message ExitRsp{
+    string msg = 2;
+}
+
+message GetObjectsOfTypeReq{
+    string obj_type = 1;
+}
+message GetObjectsOfTypeRsp{
+    repeated string prim_paths = 1;
+}
+
+message InitRobotReq{
+    string robot_cfg_file = 1;
+    string robot_usd_path = 2;
+    string scene_usd_path = 3;
+    SE3RpyPose robot_pose = 4;
+    string stand_type = 5;
+    float stand_size_x = 6;
+    float stand_size_y = 7;
+}
+message InitRobotRsp{
+    string msg = 1;
+}
+message AddCameraReq{
+    string camera_prim = 1;
+    SE3RpyPose camera_pose = 2;
+    float focus_length = 3;
+    float horizontal_aperture = 4;
+    float vertical_aperture = 5;
+    int32 width = 6;
+    int32 height = 7;
+    bool is_local = 8;
+}
+message AddCameraRsp{
+    string msg = 1;
+}
+message DrawLineReq{
+    repeated Vec3 point_list_1 = 1;
+    repeated Vec3 point_list_2 = 2;
+    repeated Vec3 colors = 3;
+    repeated float sizes = 4;
+    string name = 5;
+}
+message DrawLineRsp{
+    string msg = 1;
+}
+message ClearLineReq{
+    string name = 1;
+}
+message ClearLineRsp{
+    string msg = 1;
+}
+message ObjectPose {
+    string prim_path = 1;
+    SE3RpyPose pose = 2;
+}
+message ObjectJoint {
+    string prim_path = 1;
+    repeated JointCommand joint_cmd = 2;
+}
+message SetObjectPoseReq {
+    repeated ObjectPose object_pose = 1;
+    repeated JointCommand joint_cmd = 2;
+    repeated ObjectJoint object_joint = 3;
+    
+}
+message SetObjectPoseRsp{
+    string msg = 1;
+}
+
+message SetCameraPoseReq{
+    repeated ObjectPose camera_pose = 1;
+}
+
+message SetCameraPoseRsp{
+    string msg = 1;
+}
+
+message SetTrajectoryListReq {
+  repeated SE3RpyPose trajectory_point = 1;
+  bool is_block = 2;
+}
+message SetTrajectoryListRsp {
+  string msg = 1;
+}
+message SetFrameStateReq {
+    string frame_state = 1;
+}
+message SetFrameStateRsp{
+    string msg = 1;
+}
+message MaterialInfo {
+    string object_prim = 1;
+    string material_name = 2;
+    string material_path = 3;
+    string label_name = 4;
+}
+message SetMaterailReq {
+    repeated MaterialInfo materials = 1;
+} 
+message SetMaterialRsp {
+    string msg = 1;
+}
+message LightCfg {
+    string light_type = 1;
+    string light_prim = 2;
+    float light_temperature = 3;
+    float light_intensity = 4;
+    Rpy light_rotation = 5;
+    string light_texture = 6;
+}
+message SetLightReq {
+    repeated LightCfg lights = 1;
+}
+message SetLightRsp {
+    string msg = 1;
+}
+
+service SimObservationService {
+    rpc GetObservation(GetObservationReq) returns(GetObservationRsp);
+    rpc Reset(ResetReq) returns(ResetRsp);
+    rpc AttachObj(AttachReq) returns(AttachRsp);
+    rpc DetachObj(DetachReq) returns(DetachRsp);
+    rpc MultiMove(MultiMoveReq) returns(MultiMoveRsp);
+    rpc GetObjectsOfType(GetObjectsOfTypeReq) returns(GetObjectsOfTypeRsp);
+    rpc TaskStatus(TaskStatusReq) returns(TaskStatusRsp);
+    rpc Exit(ExitReq) returns(ExitRsp);
+    rpc InitRobot(InitRobotReq) returns(InitRobotRsp);
+    rpc AddCamera(AddCameraReq) returns(AddCameraRsp);
+    rpc DrawLine(DrawLineReq) returns(DrawLineRsp);
+    rpc ClearLine(ClearLineReq) returns(ClearLineRsp);
+    rpc SetObjectPose(SetObjectPoseReq) returns(SetObjectPoseRsp);
+    rpc SetTrajectoryList(SetTrajectoryListReq) returns(SetTrajectoryListRsp);
+    rpc SetFrameState(SetFrameStateReq) returns(SetFrameStateRsp);
+    rpc SetMaterial(SetMaterailReq) returns(SetMaterialRsp);
+    rpc SetLight (SetLightReq) returns (SetLightRsp);
+}

data_gen_dependencies/aimdk/protocol/sim/sim_observation_service_pb2_grpc.py

@@ -0,0 +1,594 @@
+# Generated by the gRPC Python protocol compiler plugin. DO NOT EDIT!
+"""Client and server classes corresponding to protobuf-defined services."""
+import grpc
+
+from aimdk.protocol.sim import sim_observation_service_pb2 as aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2
+
+
+class SimObservationServiceStub(object):
+    """Missing associated documentation comment in .proto file."""
+
+    def __init__(self, channel):
+        """Constructor.
+
+        Args:
+            channel: A grpc.Channel.
+        """
+        self.GetObservation = channel.unary_unary(
+                '/aimdk.protocol.SimObservationService/GetObservation',
+                request_serializer=aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2.GetObservationReq.SerializeToString,
+                response_deserializer=aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2.GetObservationRsp.FromString,
+                )
+        self.Reset = channel.unary_unary(
+                '/aimdk.protocol.SimObservationService/Reset',
+                request_serializer=aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2.ResetReq.SerializeToString,
+                response_deserializer=aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2.ResetRsp.FromString,
+                )
+        self.AttachObj = channel.unary_unary(
+                '/aimdk.protocol.SimObservationService/AttachObj',
+                request_serializer=aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2.AttachReq.SerializeToString,
+                response_deserializer=aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2.AttachRsp.FromString,
+                )
+        self.DetachObj = channel.unary_unary(
+                '/aimdk.protocol.SimObservationService/DetachObj',
+                request_serializer=aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2.DetachReq.SerializeToString,
+                response_deserializer=aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2.DetachRsp.FromString,
+                )
+        self.MultiMove = channel.unary_unary(
+                '/aimdk.protocol.SimObservationService/MultiMove',
+                request_serializer=aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2.MultiMoveReq.SerializeToString,
+                response_deserializer=aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2.MultiMoveRsp.FromString,
+                )
+        self.GetObjectsOfType = channel.unary_unary(
+                '/aimdk.protocol.SimObservationService/GetObjectsOfType',
+                request_serializer=aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2.GetObjectsOfTypeReq.SerializeToString,
+                response_deserializer=aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2.GetObjectsOfTypeRsp.FromString,
+                )
+        self.TaskStatus = channel.unary_unary(
+                '/aimdk.protocol.SimObservationService/TaskStatus',
+                request_serializer=aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2.TaskStatusReq.SerializeToString,
+                response_deserializer=aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2.TaskStatusRsp.FromString,
+                )
+        self.Exit = channel.unary_unary(
+                '/aimdk.protocol.SimObservationService/Exit',
+                request_serializer=aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2.ExitReq.SerializeToString,
+                response_deserializer=aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2.ExitRsp.FromString,
+                )
+        self.InitRobot = channel.unary_unary(
+                '/aimdk.protocol.SimObservationService/InitRobot',
+                request_serializer=aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2.InitRobotReq.SerializeToString,
+                response_deserializer=aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2.InitRobotRsp.FromString,
+                )
+        self.AddCamera = channel.unary_unary(
+                '/aimdk.protocol.SimObservationService/AddCamera',
+                request_serializer=aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2.AddCameraReq.SerializeToString,
+                response_deserializer=aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2.AddCameraRsp.FromString,
+                )
+        self.DrawLine = channel.unary_unary(
+                '/aimdk.protocol.SimObservationService/DrawLine',
+                request_serializer=aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2.DrawLineReq.SerializeToString,
+                response_deserializer=aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2.DrawLineRsp.FromString,
+                )
+        self.ClearLine = channel.unary_unary(
+                '/aimdk.protocol.SimObservationService/ClearLine',
+                request_serializer=aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2.ClearLineReq.SerializeToString,
+                response_deserializer=aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2.ClearLineRsp.FromString,
+                )
+        self.SetObjectPose = channel.unary_unary(
+                '/aimdk.protocol.SimObservationService/SetObjectPose',
+                request_serializer=aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2.SetObjectPoseReq.SerializeToString,
+                response_deserializer=aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2.SetObjectPoseRsp.FromString,
+                )
+        self.SetTrajectoryList = channel.unary_unary(
+                '/aimdk.protocol.SimObservationService/SetTrajectoryList',
+                request_serializer=aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2.SetTrajectoryListReq.SerializeToString,
+                response_deserializer=aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2.SetTrajectoryListRsp.FromString,
+                )
+        self.SetFrameState = channel.unary_unary(
+                '/aimdk.protocol.SimObservationService/SetFrameState',
+                request_serializer=aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2.SetFrameStateReq.SerializeToString,
+                response_deserializer=aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2.SetFrameStateRsp.FromString,
+                )
+        self.SetMaterial = channel.unary_unary(
+                '/aimdk.protocol.SimObservationService/SetMaterial',
+                request_serializer=aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2.SetMaterailReq.SerializeToString,
+                response_deserializer=aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2.SetMaterialRsp.FromString,
+                )
+        self.SetLight = channel.unary_unary(
+                '/aimdk.protocol.SimObservationService/SetLight',
+                request_serializer=aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2.SetLightReq.SerializeToString,
+                response_deserializer=aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2.SetLightRsp.FromString,
+                )
+
+
+class SimObservationServiceServicer(object):
+    """Missing associated documentation comment in .proto file."""
+
+    def GetObservation(self, request, context):
+        """Missing associated documentation comment in .proto file."""
+        context.set_code(grpc.StatusCode.UNIMPLEMENTED)
+        context.set_details('Method not implemented!')
+        raise NotImplementedError('Method not implemented!')
+
+    def Reset(self, request, context):
+        """Missing associated documentation comment in .proto file."""
+        context.set_code(grpc.StatusCode.UNIMPLEMENTED)
+        context.set_details('Method not implemented!')
+        raise NotImplementedError('Method not implemented!')
+
+    def AttachObj(self, request, context):
+        """Missing associated documentation comment in .proto file."""
+        context.set_code(grpc.StatusCode.UNIMPLEMENTED)
+        context.set_details('Method not implemented!')
+        raise NotImplementedError('Method not implemented!')
+
+    def DetachObj(self, request, context):
+        """Missing associated documentation comment in .proto file."""
+        context.set_code(grpc.StatusCode.UNIMPLEMENTED)
+        context.set_details('Method not implemented!')
+        raise NotImplementedError('Method not implemented!')
+
+    def MultiMove(self, request, context):
+        """Missing associated documentation comment in .proto file."""
+        context.set_code(grpc.StatusCode.UNIMPLEMENTED)
+        context.set_details('Method not implemented!')
+        raise NotImplementedError('Method not implemented!')
+
+    def GetObjectsOfType(self, request, context):
+        """Missing associated documentation comment in .proto file."""
+        context.set_code(grpc.StatusCode.UNIMPLEMENTED)
+        context.set_details('Method not implemented!')
+        raise NotImplementedError('Method not implemented!')
+
+    def TaskStatus(self, request, context):
+        """Missing associated documentation comment in .proto file."""
+        context.set_code(grpc.StatusCode.UNIMPLEMENTED)
+        context.set_details('Method not implemented!')
+        raise NotImplementedError('Method not implemented!')
+
+    def Exit(self, request, context):
+        """Missing associated documentation comment in .proto file."""
+        context.set_code(grpc.StatusCode.UNIMPLEMENTED)
+        context.set_details('Method not implemented!')
+        raise NotImplementedError('Method not implemented!')
+
+    def InitRobot(self, request, context):
+        """Missing associated documentation comment in .proto file."""
+        context.set_code(grpc.StatusCode.UNIMPLEMENTED)
+        context.set_details('Method not implemented!')
+        raise NotImplementedError('Method not implemented!')
+
+    def AddCamera(self, request, context):
+        """Missing associated documentation comment in .proto file."""
+        context.set_code(grpc.StatusCode.UNIMPLEMENTED)
+        context.set_details('Method not implemented!')
+        raise NotImplementedError('Method not implemented!')
+
+    def DrawLine(self, request, context):
+        """Missing associated documentation comment in .proto file."""
+        context.set_code(grpc.StatusCode.UNIMPLEMENTED)
+        context.set_details('Method not implemented!')
+        raise NotImplementedError('Method not implemented!')
+
+    def ClearLine(self, request, context):
+        """Missing associated documentation comment in .proto file."""
+        context.set_code(grpc.StatusCode.UNIMPLEMENTED)
+        context.set_details('Method not implemented!')
+        raise NotImplementedError('Method not implemented!')
+
+    def SetObjectPose(self, request, context):
+        """Missing associated documentation comment in .proto file."""
+        context.set_code(grpc.StatusCode.UNIMPLEMENTED)
+        context.set_details('Method not implemented!')
+        raise NotImplementedError('Method not implemented!')
+
+    def SetTrajectoryList(self, request, context):
+        """Missing associated documentation comment in .proto file."""
+        context.set_code(grpc.StatusCode.UNIMPLEMENTED)
+        context.set_details('Method not implemented!')
+        raise NotImplementedError('Method not implemented!')
+
+    def SetFrameState(self, request, context):
+        """Missing associated documentation comment in .proto file."""
+        context.set_code(grpc.StatusCode.UNIMPLEMENTED)
+        context.set_details('Method not implemented!')
+        raise NotImplementedError('Method not implemented!')
+
+    def SetMaterial(self, request, context):
+        """Missing associated documentation comment in .proto file."""
+        context.set_code(grpc.StatusCode.UNIMPLEMENTED)
+        context.set_details('Method not implemented!')
+        raise NotImplementedError('Method not implemented!')
+
+    def SetLight(self, request, context):
+        """Missing associated documentation comment in .proto file."""
+        context.set_code(grpc.StatusCode.UNIMPLEMENTED)
+        context.set_details('Method not implemented!')
+        raise NotImplementedError('Method not implemented!')
+
+
+def add_SimObservationServiceServicer_to_server(servicer, server):
+    rpc_method_handlers = {
+            'GetObservation': grpc.unary_unary_rpc_method_handler(
+                    servicer.GetObservation,
+                    request_deserializer=aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2.GetObservationReq.FromString,
+                    response_serializer=aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2.GetObservationRsp.SerializeToString,
+            ),
+            'Reset': grpc.unary_unary_rpc_method_handler(
+                    servicer.Reset,
+                    request_deserializer=aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2.ResetReq.FromString,
+                    response_serializer=aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2.ResetRsp.SerializeToString,
+            ),
+            'AttachObj': grpc.unary_unary_rpc_method_handler(
+                    servicer.AttachObj,
+                    request_deserializer=aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2.AttachReq.FromString,
+                    response_serializer=aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2.AttachRsp.SerializeToString,
+            ),
+            'DetachObj': grpc.unary_unary_rpc_method_handler(
+                    servicer.DetachObj,
+                    request_deserializer=aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2.DetachReq.FromString,
+                    response_serializer=aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2.DetachRsp.SerializeToString,
+            ),
+            'MultiMove': grpc.unary_unary_rpc_method_handler(
+                    servicer.MultiMove,
+                    request_deserializer=aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2.MultiMoveReq.FromString,
+                    response_serializer=aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2.MultiMoveRsp.SerializeToString,
+            ),
+            'GetObjectsOfType': grpc.unary_unary_rpc_method_handler(
+                    servicer.GetObjectsOfType,
+                    request_deserializer=aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2.GetObjectsOfTypeReq.FromString,
+                    response_serializer=aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2.GetObjectsOfTypeRsp.SerializeToString,
+            ),
+            'TaskStatus': grpc.unary_unary_rpc_method_handler(
+                    servicer.TaskStatus,
+                    request_deserializer=aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2.TaskStatusReq.FromString,
+                    response_serializer=aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2.TaskStatusRsp.SerializeToString,
+            ),
+            'Exit': grpc.unary_unary_rpc_method_handler(
+                    servicer.Exit,
+                    request_deserializer=aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2.ExitReq.FromString,
+                    response_serializer=aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2.ExitRsp.SerializeToString,
+            ),
+            'InitRobot': grpc.unary_unary_rpc_method_handler(
+                    servicer.InitRobot,
+                    request_deserializer=aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2.InitRobotReq.FromString,
+                    response_serializer=aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2.InitRobotRsp.SerializeToString,
+            ),
+            'AddCamera': grpc.unary_unary_rpc_method_handler(
+                    servicer.AddCamera,
+                    request_deserializer=aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2.AddCameraReq.FromString,
+                    response_serializer=aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2.AddCameraRsp.SerializeToString,
+            ),
+            'DrawLine': grpc.unary_unary_rpc_method_handler(
+                    servicer.DrawLine,
+                    request_deserializer=aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2.DrawLineReq.FromString,
+                    response_serializer=aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2.DrawLineRsp.SerializeToString,
+            ),
+            'ClearLine': grpc.unary_unary_rpc_method_handler(
+                    servicer.ClearLine,
+                    request_deserializer=aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2.ClearLineReq.FromString,
+                    response_serializer=aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2.ClearLineRsp.SerializeToString,
+            ),
+            'SetObjectPose': grpc.unary_unary_rpc_method_handler(
+                    servicer.SetObjectPose,
+                    request_deserializer=aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2.SetObjectPoseReq.FromString,
+                    response_serializer=aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2.SetObjectPoseRsp.SerializeToString,
+            ),
+            'SetTrajectoryList': grpc.unary_unary_rpc_method_handler(
+                    servicer.SetTrajectoryList,
+                    request_deserializer=aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2.SetTrajectoryListReq.FromString,
+                    response_serializer=aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2.SetTrajectoryListRsp.SerializeToString,
+            ),
+            'SetFrameState': grpc.unary_unary_rpc_method_handler(
+                    servicer.SetFrameState,
+                    request_deserializer=aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2.SetFrameStateReq.FromString,
+                    response_serializer=aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2.SetFrameStateRsp.SerializeToString,
+            ),
+            'SetMaterial': grpc.unary_unary_rpc_method_handler(
+                    servicer.SetMaterial,
+                    request_deserializer=aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2.SetMaterailReq.FromString,
+                    response_serializer=aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2.SetMaterialRsp.SerializeToString,
+            ),
+            'SetLight': grpc.unary_unary_rpc_method_handler(
+                    servicer.SetLight,
+                    request_deserializer=aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2.SetLightReq.FromString,
+                    response_serializer=aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2.SetLightRsp.SerializeToString,
+            ),
+    }
+    generic_handler = grpc.method_handlers_generic_handler(
+            'aimdk.protocol.SimObservationService', rpc_method_handlers)
+    server.add_generic_rpc_handlers((generic_handler,))
+
+
+ # This class is part of an EXPERIMENTAL API.
+class SimObservationService(object):
+    """Missing associated documentation comment in .proto file."""
+
+    @staticmethod
+    def GetObservation(request,
+            target,
+            options=(),
+            channel_credentials=None,
+            call_credentials=None,
+            insecure=False,
+            compression=None,
+            wait_for_ready=None,
+            timeout=None,
+            metadata=None):
+        return grpc.experimental.unary_unary(request, target, '/aimdk.protocol.SimObservationService/GetObservation',
+            aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2.GetObservationReq.SerializeToString,
+            aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2.GetObservationRsp.FromString,
+            options, channel_credentials,
+            insecure, call_credentials, compression, wait_for_ready, timeout, metadata)
+
+    @staticmethod
+    def Reset(request,
+            target,
+            options=(),
+            channel_credentials=None,
+            call_credentials=None,
+            insecure=False,
+            compression=None,
+            wait_for_ready=None,
+            timeout=None,
+            metadata=None):
+        return grpc.experimental.unary_unary(request, target, '/aimdk.protocol.SimObservationService/Reset',
+            aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2.ResetReq.SerializeToString,
+            aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2.ResetRsp.FromString,
+            options, channel_credentials,
+            insecure, call_credentials, compression, wait_for_ready, timeout, metadata)
+
+    @staticmethod
+    def AttachObj(request,
+            target,
+            options=(),
+            channel_credentials=None,
+            call_credentials=None,
+            insecure=False,
+            compression=None,
+            wait_for_ready=None,
+            timeout=None,
+            metadata=None):
+        return grpc.experimental.unary_unary(request, target, '/aimdk.protocol.SimObservationService/AttachObj',
+            aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2.AttachReq.SerializeToString,
+            aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2.AttachRsp.FromString,
+            options, channel_credentials,
+            insecure, call_credentials, compression, wait_for_ready, timeout, metadata)
+
+    @staticmethod
+    def DetachObj(request,
+            target,
+            options=(),
+            channel_credentials=None,
+            call_credentials=None,
+            insecure=False,
+            compression=None,
+            wait_for_ready=None,
+            timeout=None,
+            metadata=None):
+        return grpc.experimental.unary_unary(request, target, '/aimdk.protocol.SimObservationService/DetachObj',
+            aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2.DetachReq.SerializeToString,
+            aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2.DetachRsp.FromString,
+            options, channel_credentials,
+            insecure, call_credentials, compression, wait_for_ready, timeout, metadata)
+
+    @staticmethod
+    def MultiMove(request,
+            target,
+            options=(),
+            channel_credentials=None,
+            call_credentials=None,
+            insecure=False,
+            compression=None,
+            wait_for_ready=None,
+            timeout=None,
+            metadata=None):
+        return grpc.experimental.unary_unary(request, target, '/aimdk.protocol.SimObservationService/MultiMove',
+            aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2.MultiMoveReq.SerializeToString,
+            aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2.MultiMoveRsp.FromString,
+            options, channel_credentials,
+            insecure, call_credentials, compression, wait_for_ready, timeout, metadata)
+
+    @staticmethod
+    def GetObjectsOfType(request,
+            target,
+            options=(),
+            channel_credentials=None,
+            call_credentials=None,
+            insecure=False,
+            compression=None,
+            wait_for_ready=None,
+            timeout=None,
+            metadata=None):
+        return grpc.experimental.unary_unary(request, target, '/aimdk.protocol.SimObservationService/GetObjectsOfType',
+            aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2.GetObjectsOfTypeReq.SerializeToString,
+            aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2.GetObjectsOfTypeRsp.FromString,
+            options, channel_credentials,
+            insecure, call_credentials, compression, wait_for_ready, timeout, metadata)
+
+    @staticmethod
+    def TaskStatus(request,
+            target,
+            options=(),
+            channel_credentials=None,
+            call_credentials=None,
+            insecure=False,
+            compression=None,
+            wait_for_ready=None,
+            timeout=None,
+            metadata=None):
+        return grpc.experimental.unary_unary(request, target, '/aimdk.protocol.SimObservationService/TaskStatus',
+            aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2.TaskStatusReq.SerializeToString,
+            aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2.TaskStatusRsp.FromString,
+            options, channel_credentials,
+            insecure, call_credentials, compression, wait_for_ready, timeout, metadata)
+
+    @staticmethod
+    def Exit(request,
+            target,
+            options=(),
+            channel_credentials=None,
+            call_credentials=None,
+            insecure=False,
+            compression=None,
+            wait_for_ready=None,
+            timeout=None,
+            metadata=None):
+        return grpc.experimental.unary_unary(request, target, '/aimdk.protocol.SimObservationService/Exit',
+            aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2.ExitReq.SerializeToString,
+            aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2.ExitRsp.FromString,
+            options, channel_credentials,
+            insecure, call_credentials, compression, wait_for_ready, timeout, metadata)
+
+    @staticmethod
+    def InitRobot(request,
+            target,
+            options=(),
+            channel_credentials=None,
+            call_credentials=None,
+            insecure=False,
+            compression=None,
+            wait_for_ready=None,
+            timeout=None,
+            metadata=None):
+        return grpc.experimental.unary_unary(request, target, '/aimdk.protocol.SimObservationService/InitRobot',
+            aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2.InitRobotReq.SerializeToString,
+            aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2.InitRobotRsp.FromString,
+            options, channel_credentials,
+            insecure, call_credentials, compression, wait_for_ready, timeout, metadata)
+
+    @staticmethod
+    def AddCamera(request,
+            target,
+            options=(),
+            channel_credentials=None,
+            call_credentials=None,
+            insecure=False,
+            compression=None,
+            wait_for_ready=None,
+            timeout=None,
+            metadata=None):
+        return grpc.experimental.unary_unary(request, target, '/aimdk.protocol.SimObservationService/AddCamera',
+            aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2.AddCameraReq.SerializeToString,
+            aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2.AddCameraRsp.FromString,
+            options, channel_credentials,
+            insecure, call_credentials, compression, wait_for_ready, timeout, metadata)
+
+    @staticmethod
+    def DrawLine(request,
+            target,
+            options=(),
+            channel_credentials=None,
+            call_credentials=None,
+            insecure=False,
+            compression=None,
+            wait_for_ready=None,
+            timeout=None,
+            metadata=None):
+        return grpc.experimental.unary_unary(request, target, '/aimdk.protocol.SimObservationService/DrawLine',
+            aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2.DrawLineReq.SerializeToString,
+            aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2.DrawLineRsp.FromString,
+            options, channel_credentials,
+            insecure, call_credentials, compression, wait_for_ready, timeout, metadata)
+
+    @staticmethod
+    def ClearLine(request,
+            target,
+            options=(),
+            channel_credentials=None,
+            call_credentials=None,
+            insecure=False,
+            compression=None,
+            wait_for_ready=None,
+            timeout=None,
+            metadata=None):
+        return grpc.experimental.unary_unary(request, target, '/aimdk.protocol.SimObservationService/ClearLine',
+            aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2.ClearLineReq.SerializeToString,
+            aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2.ClearLineRsp.FromString,
+            options, channel_credentials,
+            insecure, call_credentials, compression, wait_for_ready, timeout, metadata)
+
+    @staticmethod
+    def SetObjectPose(request,
+            target,
+            options=(),
+            channel_credentials=None,
+            call_credentials=None,
+            insecure=False,
+            compression=None,
+            wait_for_ready=None,
+            timeout=None,
+            metadata=None):
+        return grpc.experimental.unary_unary(request, target, '/aimdk.protocol.SimObservationService/SetObjectPose',
+            aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2.SetObjectPoseReq.SerializeToString,
+            aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2.SetObjectPoseRsp.FromString,
+            options, channel_credentials,
+            insecure, call_credentials, compression, wait_for_ready, timeout, metadata)
+
+    @staticmethod
+    def SetTrajectoryList(request,
+            target,
+            options=(),
+            channel_credentials=None,
+            call_credentials=None,
+            insecure=False,
+            compression=None,
+            wait_for_ready=None,
+            timeout=None,
+            metadata=None):
+        return grpc.experimental.unary_unary(request, target, '/aimdk.protocol.SimObservationService/SetTrajectoryList',
+            aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2.SetTrajectoryListReq.SerializeToString,
+            aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2.SetTrajectoryListRsp.FromString,
+            options, channel_credentials,
+            insecure, call_credentials, compression, wait_for_ready, timeout, metadata)
+
+    @staticmethod
+    def SetFrameState(request,
+            target,
+            options=(),
+            channel_credentials=None,
+            call_credentials=None,
+            insecure=False,
+            compression=None,
+            wait_for_ready=None,
+            timeout=None,
+            metadata=None):
+        return grpc.experimental.unary_unary(request, target, '/aimdk.protocol.SimObservationService/SetFrameState',
+            aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2.SetFrameStateReq.SerializeToString,
+            aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2.SetFrameStateRsp.FromString,
+            options, channel_credentials,
+            insecure, call_credentials, compression, wait_for_ready, timeout, metadata)
+
+    @staticmethod
+    def SetMaterial(request,
+            target,
+            options=(),
+            channel_credentials=None,
+            call_credentials=None,
+            insecure=False,
+            compression=None,
+            wait_for_ready=None,
+            timeout=None,
+            metadata=None):
+        return grpc.experimental.unary_unary(request, target, '/aimdk.protocol.SimObservationService/SetMaterial',
+            aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2.SetMaterailReq.SerializeToString,
+            aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2.SetMaterialRsp.FromString,
+            options, channel_credentials,
+            insecure, call_credentials, compression, wait_for_ready, timeout, metadata)
+
+    @staticmethod
+    def SetLight(request,
+            target,
+            options=(),
+            channel_credentials=None,
+            call_credentials=None,
+            insecure=False,
+            compression=None,
+            wait_for_ready=None,
+            timeout=None,
+            metadata=None):
+        return grpc.experimental.unary_unary(request, target, '/aimdk.protocol.SimObservationService/SetLight',
+            aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2.SetLightReq.SerializeToString,
+            aimdk_dot_protocol_dot_sim_dot_sim__observation__service__pb2.SetLightRsp.FromString,
+            options, channel_credentials,
+            insecure, call_credentials, compression, wait_for_ready, timeout, metadata)

data_gen_dependencies/base_agent.py

@@ -0,0 +1,307 @@
+# -*- coding: utf-8 -*-
+import json
+import time
+import numpy as np
+from scipy.spatial.transform import Rotation as R, Slerp
+
+from data_gen_dependencies.robot import IsaacSimRpcRobot
+
+
+class BaseAgent(object):
+    def __init__(self, robot: IsaacSimRpcRobot) -> None:
+        self.robot = robot
+        self.reset()
+
+    def reset(self):
+        self.robot.reset()
+        # self.base_camera = "/World/Top_Camera"
+        # self.robot.base_camera = "/World/Top_Camera"
+        # self.data_keys = {
+        #     "camera": {
+        #         "camera_prim_list": [self.base_camera],
+        #         "render_depth": True,
+        #         "render_semantic": True,
+        #     },
+        #     "pose": [self.base_camera],
+        #     "joint_position": True,
+        #     "gripper": True,
+        # }
+        # self.get_observation()
+
+    def add_object(self, object_info: dict):
+        name = object_info["object_id"]
+        usd_path = object_info["model_path"]
+        position = np.array(object_info["position"])
+        quaternion = np.array(object_info["quaternion"])
+        if "scale" not in object_info:
+            object_info["scale"] = 1.0
+            size = object_info.get('size', np.array([]))
+            if isinstance(size, list):
+                size = np.array(size)
+            if size.shape[0] == 0:
+                size = np.array(100)
+
+            if np.max(size) > 10:
+                object_info["scale"] = 0.001
+
+        add_particle = False
+        particle_position = [0, 0, 0]
+        particle_scale = [1, 1, 1]
+        if "add_particle" in object_info:
+            add_particle = True
+            particle_position = object_info["add_particle"]["position"]
+            particle_scale = object_info["add_particle"]["scale"]
+        scale = np.array([object_info["scale"]] * 3)
+        material = "general" if "material" not in object_info else object_info["material"]
+        mass = 0.01 if "mass" not in object_info else object_info["mass"]
+        # if "materialOptions" in object_info:
+        #     if "transparent" in object_info["materialOptions"]:
+        #         material="Glass"
+        self.robot.client.add_object(
+            usd_path=usd_path,
+            prim_path="/World/Objects/%s" % name,
+            label_name=name,
+            target_position=position,
+            target_quaternion=quaternion,
+            target_scale=scale,
+            material=material,
+            color=[1, 1, 1],
+            mass=mass,
+            add_particle=add_particle,
+            particle_position=particle_position,
+            particle_scale=particle_scale
+        )
+
+    def generate_layout(self, task_file, init=True):
+        with open(task_file, "r") as f:
+            task_info = json.load(f)
+
+        # init_position = task_info["init_position"] if "init_position" in task_info else [0] * 39
+        # self.robot.client.set_joint_positions(init_position)
+        if init:
+            for object_info in task_info["objects"]:
+                if "box" not in object_info["name"]:
+                    continue
+                self.add_object(object_info)
+            time.sleep(1)
+
+            for object_info in task_info["objects"]:
+                if "obj" not in object_info["name"]:
+                    continue
+                self.add_object(object_info)
+            time.sleep(1)
+
+        self.task_info = task_info
+        self.robot.target_object = task_info["target_object"]
+        self.arm = task_info["arm"]
+        return task_info
+
+    def execute(self, commands):
+        for command in commands:
+            type = command["action"]
+            content = command["content"]
+            if type == "move" or type == "rotate":
+                if self.robot.move(content) == False:
+                    return False
+                else:
+                    self.last_pose = content["matrix"]
+            elif type == "open_gripper":
+                id = "left" if "gripper" not in content else content["gripper"]
+                width = 0.1 if "width" not in content else content["width"]
+                self.robot.open_gripper(id=id, width=width)
+
+            elif type == "close_gripper":
+                id = "left" if "gripper" not in content else content["gripper"]
+                force = 50 if "force" not in content else content["force"]
+                self.robot.close_gripper(id=id, force=force)
+                if "attach_obj" in content:
+                    self.robot.client.AttachObj(prim_paths=[content["attach_obj"]])
+
+            else:
+                Exception("Not implemented.")
+        return True
+
+    def start_recording(self, task_name, camera_prim_list):
+        print(camera_prim_list)
+        self.robot.client.start_recording(
+            task_name=task_name,
+            fps=30,
+            data_keys={
+                "camera": {
+                    "camera_prim_list": camera_prim_list,
+                    "render_depth": False,
+                    "render_semantic": False,
+                },
+                "pose": ["/World/Raise_A2/gripper_center"],
+                "joint_position": True,
+                "gripper": True,
+            },
+        )
+
+    def stop_recording(self, success):
+        self.robot.client.stop_recording()
+        self.robot.client.SendTaskStatus(success)
+
+    def grasp(
+            self,
+            target_gripper_pose,
+            gripper_id=None,
+            use_pre_grasp=False,
+            use_pick_up=False,
+            grasp_width=0.1,
+    ):
+        gripper_id = "left" if gripper_id is None else gripper_id
+        pick_up_pose = np.copy(target_gripper_pose)
+        pick_up_pose[2, 3] = pick_up_pose[2, 3] + 0.1  # 抓到物体后，先往上方抬10cm
+
+        commands = []
+        commands.append(
+            {
+                "action": "open_gripper",
+                "content": {
+                    "gripper": gripper_id,
+                    "width": grasp_width,
+                },
+            }
+        )
+        if use_pre_grasp:
+            pre_pose = np.array([
+                [1.0, 0, 0, 0],
+                [0, 1.0, 0, 0],
+                [0, 0, 1.0, -0.05],
+                [0, 0, 0, 1]
+            ])
+            pre_grasp_pose = target_gripper_pose @ pre_pose
+            commands.append(
+                {
+                    "action": "move",
+                    "content": {
+                        "matrix": pre_grasp_pose,
+                        "type": "matrix",
+                        "comment": "pre_grasp",
+                        "trajectory_type": "ObsAvoid",
+                    },
+                }
+            )
+
+        grasp_trajectory_type = "Simple" if use_pre_grasp else "ObsAvoid"
+        commands.append(
+            {
+                "action": "move",
+                "content": {
+                    "matrix": target_gripper_pose,
+                    "type": "matrix",
+                    "comment": "grasp",
+                    "trajectory_type": grasp_trajectory_type,
+                },
+            }
+        )
+        commands.append(
+            {
+                "action": "close_gripper",
+                "content": {
+                    "gripper": gripper_id,
+                    "force": 50,
+                },
+            }
+        )
+
+        if use_pick_up:
+            commands.append(
+                {
+                    "action": "move",
+                    "content": {
+                        "matrix": pick_up_pose,
+                        "type": "matrix",
+                        "comment": "pick_up",
+                        "trajectory_type": "Simple",
+                    },
+                }
+            )
+
+        return commands
+
+    def get_observation(self):
+        observation_raw = self.robot.get_observation(self.data_keys)
+        self.observation = observation_raw["camera"][self.robot.base_camera]
+
+    def place(
+            self,
+            target_gripper_pose,
+            current_gripper_pose=None,
+            gripper2part=None,
+            gripper_id=None,
+    ):
+        gripper_id = "left" if gripper_id is None else gripper_id
+
+        pre_place_pose = np.copy(target_gripper_pose)
+        pre_place_pose[2, 3] += 0.05
+
+        commands = [
+            {
+                "action": "move",
+                "content": {
+                    "matrix": pre_place_pose,
+                    "type": "matrix",
+                    "comment": "pre_place",
+                    "trajectory_type": "ObsAvoid",
+                },
+            },
+            {
+                "action": "move",
+                "content": {
+                    "matrix": target_gripper_pose,
+                    "type": "matrix",
+                    "comment": "place",
+                    "trajectory_type": "Simple",
+                },
+            },
+            {
+                "action": "open_gripper",
+                "content": {
+                    "gripper": gripper_id,
+                },
+            },
+        ]
+        return commands
+
+    def pour(
+            self,
+            target_gripper_pose,
+            current_gripper_pose=None,
+            gripper2part=None,
+            gripper_id=None,
+    ):
+        def interpolate_rotation_matrices(rot_matrix1, rot_matrix2, num_interpolations):
+            rot1 = R.from_matrix(rot_matrix1)
+            rot2 = R.from_matrix(rot_matrix2)
+            quat1 = rot1.as_quat()
+            quat2 = rot2.as_quat()
+            times = [0, 1]
+            slerp = Slerp(times, R.from_quat([quat1, quat2]))
+            interp_times = np.linspace(0, 1, num_interpolations)
+            interp_rots = slerp(interp_times)
+            interp_matrices = interp_rots.as_matrix()
+            return interp_matrices
+
+        target_part_pose = target_gripper_pose @ np.linalg.inv(gripper2part)
+        current_part_pose = current_gripper_pose @ np.linalg.inv(gripper2part)
+        commands = []
+        rotations = interpolate_rotation_matrices(current_part_pose[:3, :3], target_part_pose[:3, :3], 5)
+        for i, rotation in enumerate(rotations):
+            target_part_pose_step = np.copy(target_part_pose)
+            target_part_pose_step[:3, :3] = rotation
+            target_gripper_pose_step = target_part_pose_step @ gripper2part
+
+            commands.append(
+                {
+                    "action": "move",
+                    "content": {
+                        "matrix": target_gripper_pose_step,
+                        "type": "matrix",
+                        "comment": "pour_sub_rotate_%d" % i,
+                        "trajectory_type": "Simple",
+                    },
+                }
+            )
+        return commands

data_gen_dependencies/client.py

@@ -0,0 +1,962 @@
+import grpc
+import numpy as np
+import sys
+import os
+
+current_directory = os.path.dirname(os.path.abspath(__file__))
+if current_directory not in sys.path:
+    sys.path.append(current_directory)
+# 相机协议
+from aimdk.protocol.hal.sensors import rs2_camera_pb2
+from aimdk.protocol.hal.sensors import rs2_camera_pb2_grpc
+from aimdk.protocol.sim import sim_camera_service_pb2
+from aimdk.protocol.sim import sim_camera_service_pb2_grpc
+# 臂协议
+from aimdk.protocol.hal.jaka import jaka_pb2
+from aimdk.protocol.hal.jaka import jaka_pb2_grpc
+# 关节协议
+from aimdk.protocol.hal.joint import joint_channel_pb2
+from aimdk.protocol.hal.joint import joint_channel_pb2_grpc
+# 夹爪协议
+from aimdk.protocol.sim import sim_gripper_service_pb2
+from aimdk.protocol.sim import sim_gripper_service_pb2_grpc
+# 物体协议
+from aimdk.protocol.sim import sim_object_service_pb2
+from aimdk.protocol.sim import sim_object_service_pb2_grpc
+# observation协议
+from aimdk.protocol.sim import sim_observation_service_pb2
+from aimdk.protocol.sim import sim_observation_service_pb2_grpc
+import time
+import json
+import pinocchio
+import os
+
+
+# 目前代码中所有的旋转角度都以角度为单位
+class Rpc_Client:
+    def __init__(self, client_host, robot_urdf="A2D_120s.urdf"):
+        for i in range(600):
+            try:
+                self.channel = grpc.insecure_channel(client_host, options=[
+                    ('grpc.max_receive_message_length', 16094304)
+                ])
+                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)
+                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)
+                time.sleep(3)
+                if i >= 599:
+                    raise e
+
+    def set_frame_state(self, action: str, substage_id: int, active_id: str, passive_id: str, if_attached: bool,
+                        set_gripper_open=False, set_gripper_close=False, arm="default", target_pose=None):
+        stub = sim_observation_service_pb2_grpc.SimObservationServiceStub(self.channel)
+        req = sim_observation_service_pb2.SetFrameStateReq()
+        if target_pose is not None:
+            target_pose = target_pose.tolist()
+        frame_state = {
+            "action": action,
+            "substage_id": substage_id,
+            "if_attached": if_attached,
+            "set_gripper_open": set_gripper_open,
+            "set_gripper_close": set_gripper_close,
+            "active_id": active_id,
+            "passive_id": passive_id,
+            "arm": arm,
+            "target_pose": target_pose
+        }
+        _frame_state: str = json.dumps(frame_state)
+        print(_frame_state)
+        req.frame_state = _frame_state
+        response = stub.SetFrameState(req)
+        return response
+
+    def capture_frame(self, camera_prim_path):
+        stub = rs2_camera_pb2_grpc.CameraServiceStub(self.channel)
+        req = rs2_camera_pb2.GetCameraDataRequest()
+        req.serial_no = camera_prim_path
+        response = stub.GetCameraData(req)
+        # print(response.color_info)
+        # if response.color_image.data is not None:
+        #     print(np.frombuffer(response.color_image.data, dtype=np.uint8))
+        #     print(np.frombuffer(response.depth_image.data, dtype=np.float32))
+        return response
+
+    def capture_semantic_frame(self, camera_prim_path):
+        stub = sim_camera_service_pb2_grpc.SimCameraServiceStub(self.channel)
+        req = sim_camera_service_pb2.GetSemanticRequest()
+        req.serial_no = "/World/Raise_A2/base/collisions/camera"
+        req.serial_no = camera_prim_path
+        response = stub.GetSemanticData(req)
+        # if response.semantic_mask.data is not None:
+        #     print(np.frombuffer(response.semantic_mask.data, dtype=np.int16))
+
+        return response
+
+    def moveto(self, target_position, target_quaternion, arm_name, is_backend=True, ee_interpolation=False,
+               distance_frame=0.0008):
+        stub = jaka_pb2_grpc.A2wArmControlServiceStub(self.channel)
+        req = jaka_pb2.LinearMoveReq()
+        req.robot_name = arm_name
+        req.pose.position.x, req.pose.position.y, req.pose.position.z = target_position
+        req.pose.rpy.rw, req.pose.rpy.rx, req.pose.rpy.ry, req.pose.rpy.rz = target_quaternion
+        req.is_block = is_backend
+        req.ee_interpolation = ee_interpolation
+        req.distance_frame = distance_frame
+        response = stub.LinearMove(req)
+        return response
+
+    def set_joint_positions(self, target_joint_position, is_trajectory):
+        stub = joint_channel_pb2_grpc.JointControlServiceStub(self.channel)
+        req = joint_channel_pb2.SetJointReq()
+        req.is_trajectory = is_trajectory
+        for pos in target_joint_position:
+            joint_position = joint_channel_pb2.JointCommand()
+            joint_position.position = pos
+            req.commands.append(joint_position)
+        response = stub.SetJointPosition(req)
+        return response
+
+    def get_joint_positions(self):
+        stub = joint_channel_pb2_grpc.JointControlServiceStub(self.channel)
+        req = joint_channel_pb2.GetJointReq()
+        req.serial_no = "robot"
+        response = stub.GetJointPosition(req)
+        return response
+
+    # usd_path目前值定在data下的目录 如genie3D/01.usd
+    # prim_path需要指定在"/World/Objects/xxx"内，方便reset
+    def add_object(self, usd_path, prim_path, label_name, target_position, target_quaternion, target_scale, color,
+                   material, mass=0.01,
+                   add_particle=True, particle_position=[0, 0, 0], particle_scale=[0.1, 0.1, 0.1],
+                   particle_color=[1, 1, 1]):
+        stub = sim_object_service_pb2_grpc.SimObjectServiceStub(self.channel)
+        req = sim_object_service_pb2.AddObjectReq()
+        req.usd_path = usd_path
+        req.prim_path = prim_path
+        req.label_name = label_name
+        req.object_color.r, req.object_color.g, req.object_color.b = color
+        req.object_pose.position.x, req.object_pose.position.y, req.object_pose.position.z = target_position
+        req.object_pose.rpy.rw, req.object_pose.rpy.rx, req.object_pose.rpy.ry, req.object_pose.rpy.rz = target_quaternion
+        req.object_scale.x, req.object_scale.y, req.object_scale.z = target_scale
+        req.object_material = material
+        req.object_mass = mass
+        req.add_particle = add_particle
+        req.particle_position.x, req.particle_position.y, req.particle_position.z = particle_position
+        req.particle_scale.x, req.particle_scale.y, req.particle_scale.z = particle_scale
+        req.particle_color.r, req.particle_color.g, req.particle_color.b = particle_color
+
+        response = stub.AddObject(req)
+        return response
+
+    def get_object_pose(self, prim_path):
+        stub = sim_object_service_pb2_grpc.SimObjectServiceStub(self.channel)
+        req = sim_object_service_pb2.GetObjectPoseReq()
+        req.prim_path = prim_path
+        response = stub.GetObjectPose(req)
+        return response
+
+    def get_object_joint(self, prim_path):
+        stub = sim_object_service_pb2_grpc.SimObjectServiceStub(self.channel)
+        req = sim_object_service_pb2.GetObjectJointReq()
+        req.prim_path = prim_path
+        response = stub.GetObjectJoint(req)
+        return response
+
+    def set_gripper_state(self, gripper_command, is_right, opened_width):
+        stub = sim_gripper_service_pb2_grpc.SimGripperServiceStub(self.channel)
+        req = sim_gripper_service_pb2.SetGripperStateReq()
+        req.gripper_command = gripper_command
+        req.is_right = is_right
+        req.opened_width = opened_width
+        response = stub.SetGripperState(req)
+        return response
+
+    def get_gripper_state(self, is_right):
+        stub = sim_gripper_service_pb2_grpc.SimGripperServiceStub(self.channel)
+        req = sim_gripper_service_pb2.GetGripperStateReq()
+        req.is_right = is_right
+        response = stub.GetGripperState(req)
+        return response
+
+    # client端获取get某一帧的全部observation
+    def get_observation(self, data_keys):
+        observation = {}
+        stub = sim_observation_service_pb2_grpc.SimObservationServiceStub(self.channel)
+        req = sim_observation_service_pb2.GetObservationReq()
+        if 'camera' in data_keys:
+            req.isCam = True
+            req.CameraReq.render_depth = data_keys['camera']['render_depth']
+            req.CameraReq.render_semantic = data_keys['camera']['render_semantic']
+            for prim in data_keys['camera']['camera_prim_list']:
+                req.CameraReq.camera_prim_list.append(prim)
+        if 'pose' in data_keys:
+            req.isPose = True
+            for pose in data_keys['pose']:
+                req.objectPrims.append(pose)
+        req.isJoint = data_keys['joint_position']
+        req.isGripper = data_keys['gripper']
+        response = stub.GetObservation(req)
+
+        # protobuf转字典
+        camera_datas = {}
+        for camera_data, camera_prim in zip(response.camera, data_keys['camera']['camera_prim_list']):
+            cam_data = {
+                "rgb_camera": np.frombuffer(camera_data.rgb_camera.data, dtype=np.uint8),
+                "depth_camera": np.frombuffer(camera_data.depth_camera.data, dtype=np.float32),
+                "camera_info": {"width": camera_data.camera_info.width,
+                                "height": camera_data.camera_info.height,
+                                "ppx": camera_data.camera_info.ppx,
+                                "ppy": camera_data.camera_info.ppy,
+                                "fx": camera_data.camera_info.fx,
+                                "fy": camera_data.camera_info.fy}
+            }
+            camera_datas[camera_prim] = cam_data
+        joint_datas = {}
+        for joint in response.joint.left_arm:
+            joint_datas[joint.name] = joint.position
+        object_datas = {}
+        if 'pose' in data_keys:
+            for object, obj_name in zip(response.pose, data_keys['pose']):
+                object_data = {
+                    "position": np.array(
+                        [object.object_pose.position.x, object.object_pose.position.y, object.object_pose.position.z]),
+                    "rotation": np.array(
+                        [object.object_pose.rpy.rw, object.object_pose.rpy.rx, object.object_pose.rpy.ry,
+                         object.object_pose.rpy.rz])
+                }
+                object_datas[obj_name] = object_data
+        gripper_datas = {
+            "left": {
+                "position": np.array(
+                    [response.gripper.left_gripper.position.x, response.gripper.left_gripper.position.y,
+                     response.gripper.left_gripper.position.z]),
+                "rotation": np.array([response.gripper.left_gripper.rpy.rw, response.gripper.left_gripper.rpy.rx,
+                                      response.gripper.left_gripper.rpy.ry, response.gripper.left_gripper.rpy.rz])
+
+            },
+            "right": {
+                "position": np.array(
+                    [response.gripper.right_gripper.position.x, response.gripper.right_gripper.position.y,
+                     response.gripper.right_gripper.position.z]),
+                "rotation": np.array([response.gripper.right_gripper.rpy.rw, response.gripper.right_gripper.rpy.rx,
+                                      response.gripper.right_gripper.rpy.ry, response.gripper.right_gripper.rpy.rz])
+            }}
+        observation = {
+            "camera": camera_datas,
+            "joint": joint_datas,
+            "pose": object_datas,
+            "gripper": gripper_datas
+        }
+        return observation
+
+    # client端启动开始录制
+    def start_recording(self, data_keys, fps, task_name):
+        stub = sim_observation_service_pb2_grpc.SimObservationServiceStub(self.channel)
+        req = sim_observation_service_pb2.GetObservationReq()
+        req.startRecording = True
+        req.fps = fps
+        req.task_name = task_name
+        if 'camera' in data_keys:
+            render_depth = data_keys['camera']['render_depth']
+            render_semantic = data_keys['camera']['render_semantic']
+            camera_prim_list = data_keys['camera']['camera_prim_list']
+            req.isCam = True
+            req.CameraReq.render_depth = data_keys['camera']['render_depth']
+            req.CameraReq.render_semantic = data_keys['camera']['render_semantic']
+            for prim in data_keys['camera']['camera_prim_list']:
+                req.CameraReq.camera_prim_list.append(prim)
+        if data_keys['pose']:
+            req.isPose = True
+            for pose in data_keys['pose']:
+                req.objectPrims.append(pose)
+        req.isJoint = data_keys['joint_position']
+        req.isGripper = data_keys['gripper']
+        response = stub.GetObservation(req)
+        return (response)
+
+    # client端启动结束录制
+    def stop_recording(self):
+        stub = sim_observation_service_pb2_grpc.SimObservationServiceStub(self.channel)
+        req = sim_observation_service_pb2.GetObservationReq()
+        req.stopRecording = True
+        response = stub.GetObservation(req)
+        return (response)
+
+    def reset(self):
+        stub = sim_observation_service_pb2_grpc.SimObservationServiceStub(self.channel)
+        req = sim_observation_service_pb2.ResetReq()
+        req.reset = True
+        response = stub.Reset(req)
+        return (response)
+
+    def AttachObj(self, prim_paths, is_right=True):
+        stub = sim_observation_service_pb2_grpc.SimObservationServiceStub(self.channel)
+        req = sim_observation_service_pb2.AttachReq()
+        req.is_right = is_right
+        for prim in prim_paths:
+            req.obj_prims.append(prim)
+        response = stub.AttachObj(req)
+        return (response)
+
+    def DetachObj(self):
+        stub = sim_observation_service_pb2_grpc.SimObservationServiceStub(self.channel)
+        req = sim_observation_service_pb2.DetachReq()
+        req.detach = True
+        response = stub.DetachObj(req)
+        return (response)
+
+    def MultiPlan(self, robot_name, target_poses):
+        stub = sim_observation_service_pb2_grpc.SimObservationServiceStub(self.channel)
+        req = sim_observation_service_pb2.MultiMoveReq()
+        req.plan = True
+        req.robot_name = robot_name
+        req.plans_index = 0
+        for pose in target_poses:
+            _pose = sim_observation_service_pb2.SE3RpyPose()
+            _pose.position.x, _pose.position.y, _pose.position.z = pose[0]
+            _pose.rpy.rw, _pose.rpy.rx, _pose.rpy.ry, _pose.rpy.rz = pose[1]
+            req.poses.append(_pose)
+        response = stub.MultiMove(req)
+        return (response)
+
+    def MultiMove(self, robot_name, cmd_plan):
+        stub = sim_observation_service_pb2_grpc.SimObservationServiceStub(self.channel)
+        req = sim_observation_service_pb2.MultiMoveReq()
+        req.plan = False
+        req.robot_name = robot_name
+        req.cmd_plan = cmd_plan
+        response = stub.MultiMove(req)
+        return (response)
+
+    def SendTaskStatus(self, isSuccess, fail_stage_step):
+        stub = sim_observation_service_pb2_grpc.SimObservationServiceStub(self.channel)
+        req = sim_observation_service_pb2.TaskStatusReq()
+        for step in fail_stage_step:
+            req.failStep.append(step)
+        req.isSuccess = isSuccess
+        response = stub.TaskStatus(req)
+        return (response)
+
+    def Exit(self):
+        stub = sim_observation_service_pb2_grpc.SimObservationServiceStub(self.channel)
+        req = sim_observation_service_pb2.ExitReq()
+        req.exit = True
+        response = stub.Exit(req)
+        return (response)
+
+    def GetEEPose(self, is_right):
+        stub = joint_channel_pb2_grpc.JointControlServiceStub(self.channel)
+        req = joint_channel_pb2.GetEEPoseReq()
+        req.is_right = is_right
+        response = stub.GetEEPose(req)
+        return response
+
+    def GetIKStatus(self, target_poses, is_right, ObsAvoid=False):
+        stub = joint_channel_pb2_grpc.JointControlServiceStub(self.channel)
+        req = joint_channel_pb2.GetIKStatusReq()
+        req.is_right = is_right
+        req.ObsAvoid = ObsAvoid
+        urdf = os.path.dirname(os.path.abspath(__file__)) + "/robot_urdf/" + self.robot_urdf
+        model = pinocchio.buildModelFromUrdf(urdf)
+        data = model.createData()
+        joint_names = []
+        for name in model.names:
+            joint_names.append(str(name))
+
+        for pose in target_poses:
+            _pose = sim_observation_service_pb2.SE3RpyPose()
+            _pose.position.x, _pose.position.y, _pose.position.z = pose["position"]
+            _pose.rpy.rw, _pose.rpy.rx, _pose.rpy.ry, _pose.rpy.rz = pose["rotation"]
+            req.target_pose.append(_pose)
+        response = stub.GetIKStatus(req)
+        result = []
+        for ik_status in response.IKStatus:
+            joint_datas = {}
+            for name, position in zip(ik_status.joint_names, ik_status.joint_positions):
+                joint_datas[name] = position
+
+            joint_positions = []
+            for name in model.names:
+                if name == 'universe':
+                    continue
+                if name not in joint_datas:
+                    joint_positions.append(0)
+                else:
+                    joint_positions.append(joint_datas[name])
+            joint_positions = np.array(joint_positions)
+
+            pinocchio.forwardKinematics(model, data, joint_positions)
+            if ("A2D" in self.robot_urdf):
+                J = pinocchio.computeJointJacobian(model, data, joint_positions, 24)
+            else:
+                J = pinocchio.computeJointJacobian(model, data, joint_positions, 7)
+            manip = np.sqrt(np.linalg.det(np.dot(J, J.T)))
+            result.append({
+                "status": ik_status.isSuccess,
+                "Jacobian": manip,
+                "joint_positions": ik_status.joint_positions,
+                "joint_names": ik_status.joint_names
+            })
+        return result
+
+    def GetManipulability(self, joint_data):
+        urdf = os.path.dirname(os.path.abspath(__file__)) + "/robot_urdf/" + self.robot_urdf
+        model = pinocchio.buildModelFromUrdf(urdf)
+        data = model.createData()
+        joint_id = 24
+        joint_positions = []
+        for name in model.names:
+            if name == 'universe':
+                continue
+            # if name not in joint_data:
+            if name not in ['joint_lift_body', 'joint_body_pitch', 'Joint1_r', 'Joint2_r', 'Joint3_r', 'Joint4_r',
+                            'Joint5_r', 'Joint6_r', 'Joint7_r', 'right_Left_1_Joint', 'right_Left_0_Joint',
+                            'right_Left_Support_Joint', 'right_Right_1_Joint', 'right_Right_0_Joint',
+                            'right_Right_Support_Joint']:
+                joint_positions.append(0)
+            else:
+                joint_positions.append(joint_data[name])
+        joint_positions = np.array(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)))
+        return manip
+
+    def GetObjectsOfType(self, obj_type):
+        stub = sim_observation_service_pb2_grpc.SimObservationServiceStub(self.channel)
+        req = sim_observation_service_pb2.GetObjectsOfTypeReq()
+        req.obj_type = obj_type
+        response = stub.GetObjectsOfType(req)
+        return (response)
+
+    def AddCamera(self, camera_prim, camera_position, camera_rotation, width, height, focus_length, horizontal_aperture,
+                  vertical_aperture, is_local):
+        stub = sim_observation_service_pb2_grpc.SimObservationServiceStub(self.channel)
+        req = sim_observation_service_pb2.AddCameraReq()
+        req.camera_prim = camera_prim
+        req.camera_pose.position.x, req.camera_pose.position.y, req.camera_pose.position.z = camera_position
+        req.camera_pose.rpy.rw, req.camera_pose.rpy.rx, req.camera_pose.rpy.ry, req.camera_pose.rpy.rz = camera_rotation
+        req.focus_length = focus_length
+        req.horizontal_aperture = horizontal_aperture
+        req.vertical_aperture = vertical_aperture
+        req.width = width
+        req.height = height
+        req.is_local = is_local
+        response = stub.AddCamera(req)
+        return (response)
+
+    def InitRobot(self, robot_cfg, robot_usd, scene_usd, init_position=[0, 0, 0], init_rotation=[1, 0, 0, 0],
+                  stand_type="cylinder", stand_size_x=0.1, stand_size_y=0.1):
+        stub = sim_observation_service_pb2_grpc.SimObservationServiceStub(self.channel)
+        req = sim_observation_service_pb2.InitRobotReq()
+        req.robot_cfg_file, req.robot_usd_path, req.scene_usd_path = robot_cfg, robot_usd, scene_usd
+        req.robot_pose.position.x, req.robot_pose.position.y, req.robot_pose.position.z = init_position
+        req.robot_pose.rpy.rw, req.robot_pose.rpy.rx, req.robot_pose.rpy.ry, req.robot_pose.rpy.rz = init_rotation
+        req.stand_type = stand_type
+        req.stand_size_x = stand_size_x
+        req.stand_size_y = stand_size_y
+        response = stub.InitRobot(req)
+        return (response)
+
+    def DrawLine(self, point_list_1, point_list_2, colors, sizes, name):
+        stub = sim_observation_service_pb2_grpc.SimObservationServiceStub(self.channel)
+        req = sim_observation_service_pb2.DrawLineReq()
+        for point in point_list_1:
+            _point = sim_observation_service_pb2.Vec3()
+            _point.x, _point.y, _point.z = point
+            req.point_list_1.append(_point)
+        for point in point_list_2:
+            _point = sim_observation_service_pb2.Vec3()
+            _point.x, _point.y, _point.z = point
+            req.point_list_2.append(_point)
+        for color in colors:
+            _color = sim_observation_service_pb2.Vec3()
+            _color.x, _color.y, _color.z = color
+            req.colors.append(_color)
+        for size in sizes:
+            req.sizes.append(size)
+        req.name = name
+        response = stub.DrawLine(req)
+        return (response)
+
+    def ClearLine(self, name):
+        stub = sim_observation_service_pb2_grpc.SimObservationServiceStub(self.channel)
+        req = sim_observation_service_pb2.ClearLineReq()
+        req.name = name
+        response = stub.ClearLine(req)
+        return (response)
+
+    def SetObjectPose(self, object_info, joint_cmd, object_joint_info=None):
+        stub = sim_observation_service_pb2_grpc.SimObservationServiceStub(self.channel)
+        req = sim_observation_service_pb2.SetObjectPoseReq()
+        for object in object_info:
+            object_pose = sim_observation_service_pb2.ObjectPose()
+            object_pose.prim_path = object["prim_path"]
+            object_pose.pose.position.x, object_pose.pose.position.y, object_pose.pose.position.z = object["position"]
+            object_pose.pose.rpy.rw, object_pose.pose.rpy.rx, object_pose.pose.rpy.ry, object_pose.pose.rpy.rz = object[
+                "rotation"]
+            req.object_pose.append(object_pose)
+        for pos in joint_cmd:
+            cmd = sim_observation_service_pb2.JointCommand()
+            cmd.position = pos
+            req.joint_cmd.append(cmd)
+        if object_joint_info is not None:
+            for joint in object_joint_info:
+                object_joint = sim_observation_service_pb2.ObjectJoint()
+                object_joint.prim_path = joint["prim_path"]
+                for pos in joint["joint_cmd"]:
+                    cmd = sim_observation_service_pb2.JointCommand()
+                    cmd.position = pos
+                    object_joint.joint_cmd.append(cmd)
+                req.object_joint.append(object_joint)
+        response = stub.SetObjectPose(req)
+        return (response)
+
+    def SetCameraPose(self, camera_prim_name, camera_pose):
+        stub = sim_observation_service_pb2_grpc.SimObservationServiceStub(self.channel)
+        req = sim_observation_service_pb2.SetCameraPoseReq()
+        req.camera_prim_name = camera_prim_name
+        req.camera_pose.position.x, req.camera_pose.position.y, req.camera_pose.position.z = camera_pose["position"]
+        req.camera_pose.rpy.rw, req.camera_pose.rpy.rx, req.camera_pose.rpy.ry, req.camera_pose.rpy.rz = camera_pose[
+            "rotation"]
+        response = stub.SetCameraPose(req)
+        return (response)
+
+    def SetTrajectoryList(self, trajectory_list, is_block=False):
+        stub = sim_observation_service_pb2_grpc.SimObservationServiceStub(self.channel)
+        req = sim_observation_service_pb2.SetTrajectoryListReq()
+        req.is_block = is_block
+        for point in trajectory_list:
+            pose = sim_observation_service_pb2.SE3RpyPose()
+            pose.position.x, pose.position.y, pose.position.z = point[0]
+            pose.rpy.rw, pose.rpy.rx, pose.rpy.ry, pose.rpy.rz = point[1]
+            req.trajectory_point.append(pose)
+        response = stub.SetTrajectoryList(req)
+        return response
+
+    def SetTargetPoint(self, position):
+        stub = sim_object_service_pb2_grpc.SimObjectServiceStub(self.channel)
+        req = sim_object_service_pb2.SetTargetPointReq()
+        req.point_position.x, req.point_position.y, req.point_position.z = position
+        response = stub.SetTargetPoint(req)
+        return response
+
+    def SetMaterial(self, material_info):
+        stub = sim_observation_service_pb2_grpc.SimObservationServiceStub(self.channel)
+        req = sim_observation_service_pb2.SetMaterailReq()
+        for mat in material_info:
+            print(mat)
+            mat_info = sim_observation_service_pb2.MaterialInfo()
+            mat_info.object_prim = mat["object_prim"]
+            mat_info.material_name = mat["material_name"]
+            mat_info.material_path = mat["material_path"]
+            if "label_name" in mat:
+                mat_info.label_name = mat["label_name"]
+            req.materials.append(mat_info)
+        response = stub.SetMaterial(req)
+        return (response)
+
+    def SetLight(self, light_info):
+        stub = sim_observation_service_pb2_grpc.SimObservationServiceStub(self.channel)
+        req = sim_observation_service_pb2.SetLightReq()
+        for light in light_info:
+            print(light)
+            light_cfg = sim_observation_service_pb2.LightCfg()
+            light_cfg.light_type = light["light_type"]
+            light_cfg.light_prim = light["light_prim"]
+            light_cfg.light_temperature = light["light_temperature"]
+            light_cfg.light_intensity = light["light_intensity"]
+            light_cfg.light_rotation.rw, light_cfg.light_rotation.rx, light_cfg.light_rotation.ry, light_cfg.light_rotation.rz = \
+            light["rotation"]
+            light_cfg.light_texture = light["texture"]
+            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()