中国航天集团实习第一周总结
RViz插件与机械臂控制服务端的开发，实现机械臂的路径规划、运动控制、速度/加速度调
节、空间与关节控制等功能实现
- src/arm/detach_control_plugin/src/node.cpp
- src/arm/detach_control_plugin/src/node.h
bool handle_ARM_PLANN(rviz_arm_control::rviz_arm_control_srvs::Response &res);
bool handle_ARM_MOVE(rviz_arm_control::rviz_arm_control_srvs::Response &res);//3.设置笛卡尔空间
bool handle_ARM_MOVE_POSE(rviz_arm_control::rviz_arm_control_srvs::Request &req,rviz_arm_control::rviz_arm_control_srvs::Response &res);
//4.设置关节空间
bool handle_ARM_MOVE_JOINT(rviz_arm_control::rviz_arm_control_srvs::Request &req,rviz_arm_control::rviz_arm_control_srvs::Response &res);
//5. 设置速度加速度
bool handle_ARM_VEL(rviz_arm_control::rviz_arm_control_srvs::Request &req,rviz_arm_control::rviz_arm_control_srvs::Response &res);//6. 进行复合规划
bool handle_COMPOSITE_PLANN(rviz_arm_control::rviz_arm_control_srvs::Response &res);
//7. 进行复合移动
bool handle_COMPOSITE_MOVE(rviz_arm_control::rviz_arm_control_srvs::Response &res);//8. 设置螺钉位置
bool handle_COMPOSITE_SCREW_POSE(rviz_arm_control::rviz_arm_control_srvs::Request &req,rviz_arm_control::rviz_arm_control_srvs::Response &res);
//9. 设置螺孔位置
bool handle_COMPOSITE_SCREW_HOLES_POSE(rviz_arm_control::rviz_arm_control_srvs::Request &req,rviz_arm_control::rviz_arm_control_srvs::Response &res);//10.设置HOME_POSE位置
bool handle_SET_HOME_POSE(rviz_arm_control::rviz_arm_control_srvs::Request &req,rviz_arm_control::rviz_arm_control_srvs::Response &res);//11.筛选PLANS中的位置并设置
bool handle_SET_PLANS(rviz_arm_control::rviz_arm_control_srvs::Request &req,rviz_arm_control::rviz_arm_control_srvs::Response &res);//12. 设置拆卸螺钉位置
bool handle_COMPOSITE_SCREW_DETACH_POSE(rviz_arm_control::rviz_arm_control_srvs::Request &req,rviz_arm_control::rviz_arm_control_srvs::Response &res);
- 笛卡尔空间就是我们日常生活中常说的三维空间，用三个坐标轴（X、Y、Z）来描述一个点的位置。
- 比如：你在房间里，X轴表示左右，Y轴表示前后，Z轴表示上下。
- 位姿 = 位置（Position）+ 姿态（Orientation）
- 位置：物体在空间中的具体点（比如手的末端在桌子上的哪个点）。
- 姿态：物体在空间中的朝向（比如手掌是朝上、朝下还是侧着）。
机械臂的“目标笛卡尔空间位姿”是什么意思？
- 就是你希望机械臂末端（比如夹爪、吸盘）到达空间中的哪个点，并且以什么朝向到达。
- 这个目标用位置+朝向来描述。
后面是代码的具体实现
其中
复合规划单一规划的区别
1. 单一规划/单一移动
- 单一规划：只针对一个目标点进行路径规划。
- 例子：让机械臂从当前位置移动到A点，先计算一条路径（不动）。
- 单一移动：让机械臂实际运动到这个目标点。
- 例子：让机械臂按照刚才规划好的路径，移动到A点。
特点：
- 只处理一个目标点。
- 适合简单的“点到点”操作，比如抓取一个物体、放到一个地方。
2. 复合规划/复合移动
- 复合规划：针对一组目标点（多个点/多个动作）进行整体路径规划。
- 例子：让机械臂依次经过A、B、C三个点，先整体规划一条经过所有点的路径。
- 复合移动：让机械臂依次运动经过这组目标点。
- 例子：机械臂先到A点，再到B点，再到C点，完成一系列动作。
特点：
- 处理多个目标点或多个动作的“组合任务”。
- 适合复杂工序，比如拧多个螺丝、依次抓取多个物体、拆装流水线等。
自动化装配/拆卸流程
以“自动拧螺钉”为例，流程如下：
- 设置目标点
- 通过视觉/检测，获取所有螺钉和螺孔的位置和朝向。
- 用 handle_COMPOSITE_SCREW_POSE 设置所有螺钉的夹取点。
- 用 handle_COMPOSITE_SCREW_HOLES_POSE 设置所有螺孔的拧紧点。
- 设置HOME位姿
- 用 handle_SET_HOME_POSE 设置机械臂的初始/安全位置。
- 复合路径规划
- 调用 handle_COMPOSITE_PLANN，对所有目标点（夹取点、拧紧点、HOME点）整体规划一条最优路径。
- 复合移动执行
- 调用 handle_COMPOSITE_MOVE，让机械臂依次执行：夹取螺钉 → 移动到螺孔 → 拧紧 → 回HOME。
- 拆卸流程
- 只需把动作类型切换为“Detach”，目标点设置为要拆卸的螺钉位置，流程类似。
通过服务接口把所有目标点和动作类型（夹、拧、拆）都设置好。
- 机械臂控制服务端会：
- 依次读取每个目标点
- 规划一条经过所有点的路径
- 按顺序执行每个动作（比如夹取、拧紧、拆卸等）
- plan阶段：根据操作类型，循环获取目标点并规划路径（夹7次、拧5次、拆6次）。
- move阶段：依次取目标点和对应plan，调用movePlan执行，每步都判断是否到位。
- 拆卸流程还会调用PLC服务控制夹爪/电批等。
- 任一步失败都会提前返回，全部成功才算流程完成。
下面是头文件：
rviz_control.h（调度/业务层）
↓
group_pose.h（目标点/动作组管理）
↓
moveit_control.h（机械臂底层运动控制）
（1）rviz_control.h —— 业务调度与流程控制
- 作用：
负责整个自动化流程的“总指挥”，比如“夹取”、“拧紧”、“拆卸”等复合任务的调度。
- 主要内容：
- 定义了操作类型（夹、拧、拆）
- 负责复合路径规划（plan）和复合动作执行（move）
- 调用 group_pose.h 获取每一步的目标点
- 调用 moveit_control.h 完成具体的路径规划和运动
- 负责与PLC（夹爪/电批等末端执行器）通信
- 举例：
在RViz插件点“自动拧螺丝”，rviz_control.h会负责整个流程的调度。
（2）group_pose.h —— 目标点与动作组管理
- 作用：
管理一组动作的所有关键目标点（比如夹取/拧紧/拆卸的每个关键位姿），并能按顺序返回每个点。
- 主要内容：
- 保存当前操作的基准点（如螺钉/螺孔的空间位置和朝向）
- 提供 getClampPose()、getTightPose()、getDetachPose() 等方法，按顺序返回每个动作的关键点
- 用 getTransformedPoint() 根据基准点和偏移量计算实际空间目标点
- 管理HOME点、动作索引等
- 举例：
你要夹取一个螺钉，group_pose.h会依次给出“HOME→螺钉上方→螺钉后方→螺钉处”等关键点。
（3）moveit_control.h —— 机械臂底层运动控制
- 作用：
封装了MoveIt!的底层接口，负责机械臂的路径规划、运动执行、状态获取等。
- 主要内容：
- 负责路径规划（plan、planCartesianPath）
- 负责运动执行（move、execute）
- 负责设置/获取目标位姿、关节角度
- 负责速度、加速度等参数设置
- 负责判断运动是否完成
- 举例：
你给定一个目标点，moveit_control.h会负责让机械臂安全地运动到那里。
3. 典型调用流程
以“夹取螺钉”为例：
- rviz_control.h
- 你发起“夹取”任务，rviz_control.h 负责调度
- 调用 group_pose.getClampPose() 获取第一个目标点
- 调用 planPose(pose)，内部用 moveit_control.h 进行路径规划
- 循环获取下一个目标点，继续规划
- 规划全部成功后，依次调用 movePlan 执行每一步动作
- 夹取完成后，调用PLC服务控制夹爪
- group_pose.h
- 负责管理和返回夹取流程中的每个关键点
- 通过 getTransformedPoint 计算每个点的全局坐标
- moveit_control.h
- 负责具体的路径规划、运动执行、状态判断等底层操作
4. 总结
- rviz_control.h：总指挥，负责流程调度、复合任务、与PLC通信
- group_pose.h：目标点管家，负责每个动作的关键点管理和空间变换
- moveit_control.h：机械臂司机，负责底层路径规划和运动控制
三者配合，实现了机械臂自动化装配/拆卸的完整闭环。