update readme

.gitignore

@@ -2,4 +2,5 @@ __pycache__/
 workspace/
 logs/
 cache/
-.idea/
+.idea/
+*.txt

README.md

@@ -0,0 +1,112 @@
+# Isaac Sim Agent
+
+Isaac Sim Agent 是一个基于 NVIDIA Isaac Sim 的机器人任务生成和数据采集系统。该系统支持任务模板划分、任务实例生成、数据采集和回放功能。
+
+## 系统架构
+
+系统包含以下主要组件：
+- **任务模板划分器** (TaskTemplatesDivider): 将大型任务模板划分为多个子任务
+- **任务生成器** (TaskGenerator): 从任务模板生成具体的任务实例
+- **数据生成器** (DataGenerator): 在 Isaac Sim 中执行任务并采集数据
+- **数据回放器** (DataReplayer)（暂未接入）: 回放已采集的数据
+- **回放初始化器** (ReplayInitializer)（暂未接入）: 为数据回放准备环境
+
+## 1 安装依赖
+
+安装以下依赖：
+```bash
+pip install pin -i https://pypi.tuna.tsinghua.edu.cn/simple
+pip install numpy==1.26 -i https://pypi.tuna.tsinghua.edu.cn/simple
+pip install scikit-learn -i https://pypi.tuna.tsinghua.edu.cn/simple
+pip install grasp_nms -i https://pypi.tuna.tsinghua.edu.cn/simple
+pip install open3d -i https://pypi.tuna.tsinghua.edu.cn/simple
+pip install git+https://git.hofee.cn/hofee/pyboot.git 
+```
+
+执行 `pb scan` 检查依赖是否完整，若缺少某一依赖，安装对应依赖。
+
+## 2 配置说明
+
+### 2.1 任务模板划分配置 (divide_task_config.yaml)
+```yaml
+runner:
+  workspace:
+    name: "divide_task"
+    root_dir: "workspace"
+  divide:
+    input_task_templates_root_dir: "/path/to/input/task/templates"
+    output_task_templates_dir: ""  # 为空则默认在workspace/divide_task/task_templates
+    output_template_targets_dir: ""  # 为空则默认在workspace/divide_task/template_targets
+    divide_num: 100  # 每个进程划分任务数量
+    total_nums: 10000  # 总任务模板数量
+```
+
+### 2.2 任务生成配置 (generate_task_config.yaml)
+```yaml
+runner:
+  workspace:
+    name: "generate_task"
+    root_dir: "workspace"
+  generate:
+    input_data_root: "/path/to/input/data"
+    input_target_task_templates_path: "workspace/divide_task/template_targets/task_template_target_0.json"
+    output_task_root_dir: ""  # 为空则默认在workspace/generate_task/task_root_dir
+    output_generate_result_path: ""  # 为空则默认在workspace/generate_task/generate_result_path.json
+```
+
+### 2.3 数据生成配置 (generate_data_config.yaml)
+```yaml
+runner:
+  workspace:
+    name: "generate_data"
+    root_dir: "workspace"
+  generate:
+    input_data_root: "/path/to/input/data"
+    input_target_task_path: "workspace/generate_task/generate_result_path.json"
+    server_url: "localhost:50051"  # Isaac Sim 服务器地址
+```
+
+## 3 使用流程
+
+### 3.1 划分任务模板
+在 `configs/divide_task_config.yaml` 里配置任务模板输入目录、整理好的任务模板输出目录（为空则默认在workspace/divide_task/task_templates）、用于多机生产的划分后的Task Template列表（为空则默认在workspace/divide_task/template_targets）
+
+```bash
+pb run divide_task
+```
+
+### 3.2 生成任务实例
+从指定任务模版生成任务实例：
+
+```bash
+pb run generate_task
+```
+
+### 3.3 生成数据
+从指定任务实例表里依次运行任务实例（目前只支持运行同一个任务模板下的多个任务实例）：
+
+```bash
+pb run generate_data
+```
+
+### 3.4 数据回放（暂未接入）
+回放已生成的数据：
+
+```bash
+pb run replay
+```
+
+## 4 工作空间结构
+
+```
+workspace/
+├── divide_task/          # 任务模板划分输出
+│   ├── task_templates/   # 整理后的任务模板
+│   └── template_targets/ # 划分后的任务模板目标
+├── generate_task/        # 任务生成输出
+│   ├── task_root_dir/    # 生成的任务实例
+│   └── generate_result_path.json  # 生成结果路径
+└── generate_data/        # 数据生成输出
+    └── [生成的数据文件]
+```
+

app.py

@@ -1,16 +1,16 @@
 from pyboot.application import PybootApplication
 from runners.task_generator import TaskGenerator
 from runners.data_generator import DataGenerator
-from runners.data_recorder import DataRecorder
+from runners.data_replayer import DataReplayer
 from runners.task_templates_divider import TaskTemplatesDivider
 
-@PybootApplication("agent")
-class AgentApp:
+@PybootApplication("main")
+class MainApp:
     @staticmethod
     def start():
         TaskGenerator(config_path="configs/generate_task_config.yaml").run()
         DataGenerator(config_path="configs/generate_data_config.yaml").run()
-        DataRecorder(config_path="configs/record_data_config.yaml").run()
+        DataReplayer(config_path="configs/record_data_config.yaml").run()
 
 @PybootApplication("divide_task")
 class DivideTaskApp:
@@ -30,8 +30,8 @@ class GenerateDataApp:
     def start():
         DataGenerator(config_path="configs/generate_data_config.yaml").run()
 
-@PybootApplication("record_data")
-class RecordDataApp:
+@PybootApplication("replay")
+class ReplayApp:
     @staticmethod
     def start():
-        DataRecorder(config_path="configs/record_data_config.yaml").run()
+        DataReplayer(config_path="configs/replay_config.yaml").run()

configs/divide_task_config.yaml

@@ -3,8 +3,8 @@ runner:
     name: "divide_task"
     root_dir: "workspace"
   divide:
-    task_templates_root_dir: "/home/ubuntu/Projects/tasks_divide/pick&place"
-    output_task_templates_dir: "/home/ubuntu/Projects/Refactory_isaac_sim/input/templates/task_templates"
-    output_template_targets_dir: "/home/ubuntu/Projects/Refactory_isaac_sim/input/templates/template_targets"
+    input_task_templates_root_dir: "/home/ubuntu/Projects/tasks_divide/pick&place"
+    output_task_templates_dir:
+    output_template_targets_dir:
     divide_num: 100
     total_nums: 10000

configs/generate_data_config.yaml

@@ -0,0 +1,8 @@
+runner:
+  workspace:
+    name: "generate_data"
+    root_dir: "workspace"
+  generate:
+    input_data_root: "/home/ubuntu/Projects/docker_isaac_sim/input/data"
+    input_target_task_path: "workspace/generate_task/generate_result_path.json"
+    server_url: "localhost:50051"

configs/generate_task_config.yaml

@@ -3,4 +3,7 @@ runner:
     name: "generate_task"
     root_dir: "workspace"
   generate:
-    target_task_templates: "task_template_target_0.json"
+    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,41 @@
+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
+from .pull_revolute import PullRevoluteStage
+from .push_revolute import PushRevoluteStage
+from .press import PressPrismaticStage
+from .twist import TwistStage
+
+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': PressPrismaticStage,
+    'touch': PressPrismaticStage,
+    "pull": PullStage,
+    # ---- Articulate ----
+    "pull_revolute": PullRevoluteStage,
+    "push_revolute": PushRevoluteStage,
+    "pull_prismatic":PullStage,
+    "push_prismatic":PushStage,
+    "twist": TwistStage,
+    'press_prismatic': PressPrismaticStage,
+}
+
+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,109 @@
+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 = 'Simple'
+        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,137 @@
+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, active_obj=None, passive_obj=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={}, active_obj=None, passive_obj=None, **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,61 @@
+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={}, active_obj=None, passive_obj=None, **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, active_obj=None, passive_obj=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/press.py

@@ -0,0 +1,37 @@
+import numpy as np
+
+from data_gen_dependencies.action.base import StageTemplate
+
+class PressPrismaticStage(StageTemplate):
+    def __init__(self, active_obj_id, passive_obj_id, active_element=None, passive_element=None, target_pose=None, vector_direction=None, objects=[], **kwargs):
+        super().__init__(active_obj_id, passive_obj_id, active_element, passive_element)
+        self.pre_distance = -0.03
+        
+        self.passive_obj = objects[passive_obj_id]
+        self.active_obj = objects[active_obj_id]
+        self.joint_position_threshold = passive_element.get('joint_position_threshold', 0.7)
+        self.joint_position_threshold = self.joint_position_threshold if self.joint_position_threshold is not None else 0.7
+        self.correspond_joint_id = passive_element.get('correspond_joint_id', None)
+        correspond_joint_info = self.passive_obj.joints_info[self.correspond_joint_id]
+        self.joint_lower_limit = correspond_joint_info["lower_bound"]
+        self.joint_upper_limit = correspond_joint_info["upper_bound"]
+        self.move_distance = -0.005 + abs(self.joint_lower_limit - self.joint_upper_limit) * self.joint_position_threshold
+        self.joint_axis = correspond_joint_info["joint_axis"]
+        self.joint_type = correspond_joint_info["joint_type"]
+        assert self.joint_type == 'prismatic', "joint_type must be prismatic for press action"
+        correspond_joint_info = self.passive_obj.joints_info[self.correspond_joint_id]
+        self.generate_substage(target_pose, vector_direction)
+
+        
+    def generate_substage(self, target_pose, vector_direction):
+        vector_direction = vector_direction / np.linalg.norm(vector_direction)
+        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'])
+        
+        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'])
+        free_delta_pose = np.eye(4)
+        free_delta_pose[2,3] = -0.03
+        self.sub_stages.append([free_delta_pose, "open", np.eye(4), 'local_gripper'])

data_gen_dependencies/action/pull.py

@@ -0,0 +1,57 @@
+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, active_obj=None, passive_obj=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/pull_revolute.py

@@ -0,0 +1,145 @@
+import os
+import numpy as np
+from scipy.spatial.transform import Rotation as R
+from data_gen_dependencies.action.base import StageTemplate
+
+
+class PullRevoluteStage(StageTemplate):
+    DELTA_DISTANCE = 0.003  # meter
+    def __init__(self, active_obj_id, passive_obj_id, active_element=None, passive_element=None, target_pose=np.eye(4), extra_params=None, objects=[], **kwargs):
+        super().__init__(active_obj_id, passive_obj_id, active_element, passive_element)
+        #import ipdb;ipdb.set_trace()
+        self.objects = objects
+        self.passive_obj = objects[passive_obj_id]
+        self.active_obj = objects[active_obj_id]
+        main_part_name= passive_element["part_id"]
+        self.obj_base = os.path.dirname(passive_obj_id)
+        main_part_id = os.path.join(self.obj_base, main_part_name)
+        if main_part_id not in objects:
+            raise ValueError(f"Cannot find main part {main_part_name} in objects")
+        self.root_id = os.path.join(self.obj_base, "root")
+        self.main_part_obj = objects[main_part_id]
+        self.root_obj = objects[self.root_id]
+        self.revolute_radius = np.linalg.norm(self.passive_obj.obj_pose[:3,3] - self.main_part_obj.obj_pose[:3,3])
+        self.revolute_joint_position = self.main_part_obj.obj_pose[:3,3]
+        self.joint_position_threshold = passive_element.get('joint_position_threshold', 0.7)
+        self.correspond_joint_id = passive_element.get('correspond_joint_id', None)
+        correspond_joint_info = self.passive_obj.joints_info[self.correspond_joint_id]
+        
+        self.joint_lower_limit = correspond_joint_info["lower_bound"]
+        self.joint_upper_limit = correspond_joint_info["upper_bound"]
+        self.joint_axis = correspond_joint_info["joint_axis"]
+        self.joint_type = correspond_joint_info["joint_type"]
+        assert self.joint_type == 'revolute', "joint_type must be revolute for pull_revolute action"
+        if self.joint_position_threshold is None:
+            self.joint_position_threshold = 0.7
+        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
+        self.generate_substage(target_pose, vector_direction)
+
+    def generate_substage(self, gripper2obj, vector_direction):
+        vector_direction = vector_direction / np.linalg.norm(vector_direction)
+        total_angle = self.joint_position_threshold * (self.joint_upper_limit - self.joint_lower_limit)
+        arc_length = abs(self.revolute_radius * total_angle)
+        step = int(arc_length / self.DELTA_DISTANCE)
+        from pyboot.utils.log import Log
+        Log.debug(f'PullRevoluteStage: total_angle={total_angle}, arc_length={arc_length}, step={step}')
+        trajectory = self.generate_trajectory(gripper2obj, vector_direction)
+        self.save_visualized_gripper_trajectory(trajectory, "pull_revolute_traj.txt")
+        
+        self.sub_stages.append([trajectory, None, np.eye(4), 'Trajectory'])
+        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 generate_trajectory(self, gripper2passive_obj, init_vector_direction):
+        trajectory = []
+        total_angle = self.joint_position_threshold * (self.joint_upper_limit - self.joint_lower_limit)
+        total_angle *= self.joint_direction 
+
+        arc_length = abs(self.revolute_radius * total_angle)
+        num_steps = int(np.ceil(arc_length / self.DELTA_DISTANCE))
+        if num_steps < 1:
+            num_steps = 1
+
+        delta_theta = total_angle / num_steps
+        rotation_axis = np.array(self.joint_axis, dtype=float)
+        rotation_axis /= np.linalg.norm(rotation_axis) 
+
+        passive_obj_2_root = np.linalg.inv(self.root_obj.obj_pose) @ self.passive_obj.obj_pose
+        gripper_2_root = passive_obj_2_root @ gripper2passive_obj
+        pos0 = gripper_2_root[:3, 3]
+        R0 = gripper_2_root[:3, :3]
+
+        joint_2_root = np.linalg.inv(self.root_obj.obj_pose) @ self.main_part_obj.obj_pose
+
+        pivot = joint_2_root[:3, 3]
+
+        for i in range(1, num_steps + 1):
+            angle = i * delta_theta
+            R_delta = R.from_rotvec(rotation_axis * angle).as_matrix()
+            pos_rot = R_delta @ (pos0 - pivot) + pivot
+            R_new = R_delta @ R0
+
+            waypoint_2_root = np.eye(4)
+            waypoint_2_root[:3, :3] = R_new
+            waypoint_2_root[:3, 3] = pos_rot
+            waypoint_2_obj = np.linalg.inv(passive_obj_2_root) @ waypoint_2_root
+            trajectory.append(waypoint_2_obj)
+        return np.asarray(trajectory)
+
+    def save_visualized_gripper_trajectory(self, gripper_trajectory, save_path):
+        #import ipdb; ipdb.set_trace()
+        N = len(gripper_trajectory)
+        gripper_pts = np.zeros((1, 60, 3))
+        for i in range(10):
+            gripper_pts[0, i] = np.array([0, 0.01, 0.002*i])
+            gripper_pts[0, i+10] = np.array([0, 0.01, 0.002*i + 0.001])
+            gripper_pts[0, i+20] = np.array([0, -0.01, 0.002*i])
+            gripper_pts[0, i+30] = np.array([0, -0.01, 0.002*i + 0.001])
+            gripper_pts[0, i+40] = np.array([0, -0.01 + 0.002 * i, 0])
+        for i in range(5):
+            gripper_pts[0, i+50] = np.array([0, 0, -0.002*i])
+            gripper_pts[0, i+55] = np.array([0, 0, -0.002*i + 0.001])
+        gripper_pts -= np.array([0, 0, 0.002*10 + 0.001])
+        gripper_pts = gripper_pts.repeat(N, axis=0)
+        gripper_trajectory = gripper_trajectory.reshape(N, 1, 4, 4)
+        gripper_trajectory = gripper_trajectory.repeat(60, axis=1)
+        gripper_pts = gripper_pts.reshape(-1, 3)
+        gripper_trajectory = gripper_trajectory.reshape(-1, 4, 4)
+        gripper_pts_2_obj =  (gripper_trajectory[:, :3, :3] @ gripper_pts[..., None]).squeeze(-1)+ gripper_trajectory[:, :3, 3]
+        np.savetxt(save_path, gripper_pts_2_obj, fmt="%.6f")
+    
+    def axis_angle_to_matrix(self, axis, angle):
+
+        axis = axis / np.linalg.norm(axis)
+        K = np.array([[0, -axis[2], axis[1]],
+                      [axis[2], 0, -axis[0]],
+                      [-axis[1], axis[0], 0]])
+        R = np.eye(3) + np.sin(angle) * K + (1 - np.cos(angle)) * (K @ K)
+        T = np.eye(4)
+        T[:3, :3] = R
+        return T
+
+    def check_completion(self, objects):
+        if self.__len__() == 0:
+            return True
+
+        succ = False
+        if self.step_id >= 0:
+            succ = True
+
+        if succ:
+            self.step_id += 1
+
+        return succ

data_gen_dependencies/action/push.py

@@ -0,0 +1,77 @@
+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, active_obj=None, passive_obj=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
+
+
+
+

data_gen_dependencies/action/push_revolute.py

@@ -0,0 +1,146 @@
+import os
+import numpy as np
+from scipy.spatial.transform import Rotation as R
+from data_gen_dependencies.action.base import StageTemplate
+
+
+class PushRevoluteStage(StageTemplate):
+    DELTA_DISTANCE = 0.003  # meter
+    def __init__(self, active_obj_id, passive_obj_id, active_element=None, passive_element=None, target_pose=np.eye(4), extra_params=None, objects=[], **kwargs):
+        super().__init__(active_obj_id, passive_obj_id, active_element, passive_element)
+        #import ipdb;ipdb.set_trace()
+        self.objects = objects
+        self.passive_obj = objects[passive_obj_id]
+        self.active_obj = objects[active_obj_id]
+        main_part_name= passive_element["part_id"]
+        self.obj_base = os.path.dirname(passive_obj_id)
+        main_part_id = os.path.join(self.obj_base, main_part_name)
+        if main_part_id not in objects:
+            raise ValueError(f"Cannot find main part {main_part_name} in objects")
+        self.root_id = os.path.join(self.obj_base, "root")
+        self.main_part_obj = objects[main_part_id]
+        self.root_obj = objects[self.root_id]
+        self.revolute_radius = np.linalg.norm(self.passive_obj.obj_pose[:3,3] - self.main_part_obj.obj_pose[:3,3])
+        self.revolute_joint_position = self.main_part_obj.obj_pose[:3,3]
+        self.joint_position_threshold = passive_element.get('joint_position_threshold', 0.7)
+        self.correspond_joint_id = passive_element.get('correspond_joint_id', None)
+        correspond_joint_info = self.passive_obj.joints_info[self.correspond_joint_id]
+        
+        self.joint_lower_limit = correspond_joint_info["lower_bound"]
+        self.joint_upper_limit = correspond_joint_info["upper_bound"]
+        self.joint_axis = correspond_joint_info["joint_axis"]
+        self.joint_type = correspond_joint_info["joint_type"]
+        assert self.joint_type == 'revolute', "joint_type must be revolute for push_revolute action"
+        if self.joint_position_threshold is None:
+            self.joint_position_threshold = 0.7
+        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
+        self.generate_substage(target_pose, vector_direction)
+
+    def generate_substage(self, gripper2obj, vector_direction):
+        vector_direction = vector_direction / np.linalg.norm(vector_direction)
+        total_angle = self.joint_position_threshold * (self.joint_upper_limit - self.joint_lower_limit)
+        arc_length = abs(self.revolute_radius * total_angle)
+        step = int(arc_length / self.DELTA_DISTANCE)
+        from pyboot.utils.log import Log
+        Log.debug(f'PushRevoluteStage: total_angle={total_angle}, arc_length={arc_length}, step={step}')
+        trajectory = self.generate_trajectory(gripper2obj, vector_direction)
+        self.save_visualized_gripper_trajectory(trajectory, "push_revolute_traj.txt")
+
+        self.sub_stages.append([trajectory, None, np.eye(4), 'Trajectory'])
+        self.sub_stages.append([None, "open", np.eye(4), 'Simple'])
+
+        free_delta_pose = np.eye(4)
+        free_delta_pose[2,3] = -self.DELTA_DISTANCE
+        self.sub_stages.append([free_delta_pose, None, np.eye(4), 'local_gripper'])
+
+
+
+    def generate_trajectory(self, gripper2passive_obj, init_vector_direction):
+        trajectory = []
+        total_angle = self.joint_position_threshold * (self.joint_upper_limit - self.joint_lower_limit)
+        total_angle *= self.joint_direction
+
+        arc_length = abs(self.revolute_radius * total_angle)
+        num_steps = int(np.ceil(arc_length / self.DELTA_DISTANCE))
+        if num_steps < 1:
+            num_steps = 1
+
+        delta_theta = total_angle / num_steps
+        rotation_axis = np.array(self.joint_axis, dtype=float)
+        rotation_axis /= np.linalg.norm(rotation_axis) 
+
+        passive_obj_2_root = np.linalg.inv(self.root_obj.obj_pose) @ self.passive_obj.obj_pose
+        gripper_2_root = passive_obj_2_root @ gripper2passive_obj
+        pos0 = gripper_2_root[:3, 3]
+        R0 = gripper_2_root[:3, :3]
+
+        joint_2_root = np.linalg.inv(self.root_obj.obj_pose) @ self.main_part_obj.obj_pose
+
+        pivot = joint_2_root[:3, 3]
+
+        for i in range(1, num_steps + 1):
+            angle = i * delta_theta
+            R_delta = R.from_rotvec(rotation_axis * angle).as_matrix()
+            pos_rot = R_delta @ (pos0 - pivot) + pivot
+            R_new = R_delta @ R0
+
+            waypoint_2_root = np.eye(4)
+            waypoint_2_root[:3, :3] = R_new
+            waypoint_2_root[:3, 3] = pos_rot
+            waypoint_2_obj = np.linalg.inv(passive_obj_2_root) @ waypoint_2_root
+            trajectory.append(waypoint_2_obj)
+        return np.asarray(trajectory)
+
+    def save_visualized_gripper_trajectory(self, gripper_trajectory, save_path):
+        #import ipdb; ipdb.set_trace()
+        N = len(gripper_trajectory)
+        gripper_pts = np.zeros((1, 60, 3))
+        for i in range(10):
+            gripper_pts[0, i] = np.array([0, 0.01, 0.002*i])
+            gripper_pts[0, i+10] = np.array([0, 0.01, 0.002*i + 0.001])
+            gripper_pts[0, i+20] = np.array([0, -0.01, 0.002*i])
+            gripper_pts[0, i+30] = np.array([0, -0.01, 0.002*i + 0.001])
+            gripper_pts[0, i+40] = np.array([0, -0.01 + 0.002 * i, 0])
+        for i in range(5):
+            gripper_pts[0, i+50] = np.array([0, 0, -0.002*i])
+            gripper_pts[0, i+55] = np.array([0, 0, -0.002*i + 0.001])
+        gripper_pts -= np.array([0, 0, 0.002*10 + 0.001])
+        gripper_pts = gripper_pts.repeat(N, axis=0)
+        gripper_trajectory = gripper_trajectory.reshape(N, 1, 4, 4)
+        gripper_trajectory = gripper_trajectory.repeat(60, axis=1)
+        gripper_pts = gripper_pts.reshape(-1, 3)
+        gripper_trajectory = gripper_trajectory.reshape(-1, 4, 4)
+        gripper_pts_2_obj =  (gripper_trajectory[:, :3, :3] @ gripper_pts[..., None]).squeeze(-1)+ gripper_trajectory[:, :3, 3]
+        np.savetxt(save_path, gripper_pts_2_obj, fmt="%.6f")
+
+    def axis_angle_to_matrix(self, axis, angle):
+
+        axis = axis / np.linalg.norm(axis)
+        K = np.array([[0, -axis[2], axis[1]],
+                      [axis[2], 0, -axis[0]],
+                      [-axis[1], axis[0], 0]])
+        R = np.eye(3) + np.sin(angle) * K + (1 - np.cos(angle)) * (K @ K)
+        T = np.eye(4)
+        T[:3, :3] = R
+        return T
+
+    def check_completion(self, objects):
+        if self.__len__() == 0:
+            return True
+
+        succ = False
+        if self.step_id >= 0:
+            succ = True
+
+        if succ:
+            self.step_id += 1
+
+        return succ

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), active_obj=None, passive_obj=None, **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/action/twist.py

@@ -0,0 +1,66 @@
+import numpy as np
+
+from data_gen_dependencies.action.base import StageTemplate
+
+
+class TwistStage(StageTemplate):
+    DELTA_DISTANCE = 0.01  # meter
+    def __init__(self, active_obj_id, passive_obj_id, active_element=None, passive_element=None, target_pose=np.eye(4), extra_params=None, objects = [], **kwargs):
+        super().__init__(active_obj_id, passive_obj_id, active_element, passive_element)
+        self.passive_obj = objects[passive_obj_id]
+        self.active_obj = objects[active_obj_id]
+        self.joint_position_threshold = passive_element.get('joint_position_threshold', 0.7)
+        if self.joint_position_threshold is None:
+            self.joint_position_threshold = 0.7
+        self.correspond_joint_id = passive_element.get('correspond_joint_id', None)
+        correspond_joint_info = self.passive_obj.joints_info[self.correspond_joint_id]
+        self.joint_lower_limit = correspond_joint_info["lower_bound"]
+        self.joint_upper_limit = correspond_joint_info["upper_bound"]
+        if self.joint_lower_limit is None or self.joint_upper_limit is None:
+            self.twist_degree_range = np.pi
+        else:
+            self.twist_degree_range = abs(self.joint_upper_limit - self.joint_lower_limit)
+        self.joint_axis = correspond_joint_info["joint_axis"]
+        self.joint_type = correspond_joint_info["joint_type"]
+        assert self.joint_type == 'continuous', "joint_type must be continuous for twist action"
+        if self.joint_position_threshold is None:
+            self.joint_position_threshold = 0.7
+        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
+        self.generate_substage(target_pose, vector_direction)
+
+    def generate_substage(self, target_pose, vector_direction):
+        vector_direction = vector_direction / np.linalg.norm(vector_direction)
+        delta_twist_pose = self.axis_angle_to_matrix(np.asarray([0,0,1]), self.twist_degree_range * self.joint_position_threshold * self.joint_direction)
+        self.sub_stages.append([delta_twist_pose, None, np.eye(4), 'local_gripper'])
+
+    def axis_angle_to_matrix(self, axis, angle):
+
+        axis = axis / np.linalg.norm(axis)
+        K = np.array([[0, -axis[2], axis[1]],
+                      [axis[2], 0, -axis[0]],
+                      [-axis[1], axis[0], 0]])
+        R = np.eye(3) + np.sin(angle) * K + (1 - np.cos(angle)) * (K @ K)
+        T = np.eye(4)
+        T[:3, :3] = R
+        return T
+
+    def check_completion(self, objects):
+        if self.__len__() == 0:
+            return True
+
+        succ = False
+        if self.step_id >= 0:
+            succ = True
+
+        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);
+}