跳到主要内容

2D/6D视觉抓取

1. 介绍​

灵动操作臂控制系统是一套基于ROS的完整解决方案，专为本公司自主研发的灵动系列六自由度操作臂及夹爪量身打造（参考：https://github.com/Xuanya-Robotics/Alicia-D-ROS1）。该系统提供标准化的 ROS 接口，以弧度为统一角度单位，简化机械臂控制流程，屏蔽底层通信细节，使用户能够专注于上层应用的开发。系统的核心功能包括：

  • 实时控制六自由度机械臂；

  • 精准控制夹爪开合；

  • 提供夹爪独立控制接口（新增功能）；

  • 实时反馈所有关节状态；

  • 提供标准化 ROS 接口；

  • 支持串口自动检测与断线重连；

  • 具备良好的跨平台兼容性。

2. 单操作臂的数据读取和控制​

注意：

  1. 安全第一：始终确保机械臂工作区域内没有障碍物或其他人员；

  2. 角度限制：所有命令应在有效范围内，特别注意夹爪限制在0-100°（机械结构限制）；

  3. 数据单位：所有API使用弧度为单位，不是角度，需要使用math.radians()和math.degrees()进行转换；

  4. 启动顺序：始终先启动控制系统，然后再启动应用程序；

  5. 串口权限：首次使用可能需要配置串口访问权限；

  6. 系统资源：保持ROS核心系统正常运行；

  7. 使用Ctrl+C安全地停止正在运行的节点。

2.1 系统节点​

系统由四个主要节点组成，共同构成完整的控制链路：

节点名称文件功能描述
数据类型节点serial_data_type_node.py将原始数据分类转发至对应处理节点
舵机状态节点servo_states_node.py处理硬件状态数据，转换为标准关节状态
舵机控制节点servo_control_node.py接收标准关节命令，转换为硬件协议格式
串口服务节点serial_server_node负责硬件通信，处理数据帧的收发

数据流向图可参考：

2.2 接口规范​

2.2.1 主要接口​

用户仅需关注以下三个主要接口：

话题名称消息类型方向描述
/arm_joint_stateArmJointState输入(订阅)接收机械臂当前状态
/arm_joint_commandArmJointState输出(发布)仅控制机械臂的6个关节
/gripper_controlFloat32输出(发布)单独控制夹爪

注意：虽然/arm_joint_command消息中包含夹爪字段，但目前系统实现中该字段被忽略。必须使用/gripper_control话题来控制夹爪！

2.2.2 消息类型定义​

系统使用自定义消息类型 ArmJointState：

# 标准机械臂关节状态消息 (弧度单位)
Header header

# 六个主要关节角度 (弧度)
float32 joint1  # 关节1，底座旋转关节
float32 joint2  # 关节2，肩部关节
float32 joint3  # 关节3，肘部关节
float32 joint4  # 关节4，腕部旋转关节
float32 joint5  # 关节5，腕部俯仰关节
float32 joint6  # 关节6，腕部翻转关节

# 夹爪角度 (弧度)
float32 gripper

# 可选的运动控制参数
float32 time    # 运动时间(秒)，默认为0表示立即执行

2.2.3 数据单位与限制​

所有角度值使用弧度作为标准单位：

参数名称角度范围(弧rad)角度范围(度°)备注
joint1~joint6-π 到 +π-180 到 +180超出范围会被自动截断
gripper0 到 1.7450 到 100夹爪特有限制，超出范围会被截断

注意：由于机械结构的限制，夹爪的角度范围为0到1.745弧度（即0到100°）。

2.3 系统部署及启动​

2.3.1 系统依赖安装​

系统依赖：

  • Ubuntu 20.04

  • ROS（Noetic或其他ROS 1版本）

  • Python 3.8

  • rosdepc

rosdepc 安装可参考鱼香ros安装方法:

https://blog.csdn.net/weixin\_45291614/article/details/131530006 

工作空间创建及依赖安装：

# 创建工作空间
mkdir -p ~/alicia_ws/src
cd ~/alicia_ws
git clone https://github.com/Xuanya-Robotics/Alicia-D-ROS1.git ./src/
# 安装库所需的依赖包
./src/install/alicia_amd64_install.sh

设置串口权限

# 方法1：添加用户到dialout组（推荐，永久有效）
sudo usermod -a -G dialout $USER
# 注意：需要注销并重新登录使组权限生效

# 方法2：临时设置串口权限
sudo chmod 666 /dev/ttyUSB*

# 方法3：创建udev规则（永久有效）
echo 'KERNEL=="ttyUSB*", MODE="0666"' | sudo tee /etc/udev/rules.d/99-serial.rules
sudo udevadm control --reload-rules
sudo udevadm trigger

硬件连接验证

  1. 将机械臂通过USB连接至计算机；

  2. 检查串口是否被系统识别；

ls -l /dev/ttyUSB*
  • 确认串口设备名称（如ttyUSB0、ttyUSB1等）。

2.3.2 启动命令​

使用以下命令启动完整控制系统：

roslaunch alicia_duo_driver serial_server.launch

2.3.3 配置参数​

启动时可以配置以下参数：

# 启用调试模式
roslaunch alicia_duo_driver serial_server.launch debug_mode:=true

# 指定串口设备
roslaunch alicia_duo_driver serial_server.launch port:=ttyUSB0

# 设置波特率
roslaunch alicia_duo_driver serial_server.launch baudrate:=921600

# 组合参数
roslaunch alicia_duo_driver serial_server.launch port:=ttyUSB0 debug_mode:=true baudrate:=921600

2.3.4 启动文件参数说明​

参数名称默认值说明
debug_modefalse是否启用调试日志输出
portttyUSB3串口设备名称，位于/dev/下
baudrate921600串口波特率
servo_count9操作臂拥有的舵机数量
outputscreen日志输出位置

2.4 使用方法​

快速测试发送命令：

# 发送所有关节归零的命令
rostopic pub -1 /arm_joint_command alicia_duo_driver/ArmJointState "header:
  stamp: now
  frame_id: ''
joint1: 0.0
joint2: 0.0
joint3: 0.0
joint4: 0.0
joint5: 0.0
joint6: 0.0
gripper: 0.0
time: 0.0"

单独控制夹爪：

# 发送夹爪控制命令（张开50度，约0.873弧度）
rostopic pub -1 /gripper_control std_msgs/Float32 "data: 0.873"

监听关节状态：

rostopic echo /arm_joint_state

2.5 详细示例​

系统提供了一个完整的演示示例，可用于测试和学习：

# 确保系统已启动
roslaunch alicia_duo_driver serial_server.launch

# 运行演示程序
rosrun alicia_duo_driver arm_control_demo.py

演示程序将执行以下测试序列：

  1. 所有关节归零；

  2. 测试关节1的正反向运动；

  3. 测试关节2的正反向运动；

  4. 测试夹爪开合；

  5. 测试复合动作。

2.6 故障排除​

2.6.1 常见问题​

问题描述可能原因解决方法
无法连接到串口权限不足使用sudo chmod 666 /dev/ttyUSB*或添加用户到dialout组
串口连接失败端口名称错误检查dev下的可用设备并指定正确的port参数
通信中断USB连接不稳定使用高质量USB线缆，检查USB接口是否松动
机械臂不响应波特率错误确认波特率设置与硬件一致(默认921600)
运动范围受限软件限制检查代码中的角度限制是否与实际机械匹配
夹爪行为异常超出范围确保夹爪角度在0-100度范围内

2.6.2 诊断方法​

启用调试模式可以获取更多信息:

roslaunch alicia_duo_driver serial_server.launch debug_mode:=true

检查服务节点状态：

rosnode list
rosnode info /servo_control_node

监控原始数据：

rostopic echo /read_serial_data

重要参数参考：

3. 单操作臂的Moveit!与ROS Control控制​

3.1 启动命令​

MoveIt 仿真控制，不涉及连接真实机械臂：

roslaunch alicia_duo_moveit demo.launch

连接真实机械臂的MoveIt 和ROS Control 控制， 请先确保机械臂的ROS 硬件连接已经启动，即已经运行

roslaunch alicia_duo_driver serial_server.launch

之后，在另一个终端启动：

roslaunch alicia_duo_moveit real_robot_control.launch

3.2 MoveIt配置修改​

alicia_duo_moveit ROS 包配置了alicia运动控制规划组和manipulator运动控制规划组。

  • alicia规划组

包含了base_link 到tool0的正逆运动学规划，即包含机械臂基座到夹爪尖端中点的一系列坐标转换关系。

  • manipulator规划组

包含了base_link 到Link6的正逆运动学规划，即包含机械臂基座到机械臂末端的一系列坐标转换关系。

增减规划控制组和其他参数主要是通过MoveIt Assistant配置，运行以下指令启动：

roslaunch moveit_setup_assistant setup_assistant.launch

详细的Moveit!配置教程参见：

Moveit!配置与使用

3.3 ROS 控制器修改​

在实际应用中，我们可以根据机器人硬件和控制需求，灵活配置和自定义 ROS 控制器，包括关节控制器、状态发布器及其对应的控制参数（如 PID 增益），实现对机械臂和夹爪的精准控制。

3.3.1 ros_controllers.yaml​

在启动real_robot_control.launch 后可查询所有的控制器类型

types: 
  - ackermann_steering_controller/AckermannSteeringController
  - diff_drive_controller/DiffDriveController
  - effort_controllers/GripperActionController
  - effort_controllers/JointEffortController
  - effort_controllers/JointGroupEffortController
  - effort_controllers/JointGroupPositionController
  - effort_controllers/JointPositionController
  - effort_controllers/JointTrajectoryController
  - effort_controllers/JointVelocityController
  - force_torque_sensor_controller/ForceTorqueSensorController
  - imu_sensor_controller/ImuSensorController
  - joint_state_controller/JointStateController
  - pos_vel_acc_controllers/JointTrajectoryController
  - pos_vel_controllers/JointTrajectoryController
  - position_controllers/GripperActionController
  - position_controllers/JointGroupPositionController
  - position_controllers/JointPositionController
  - position_controllers/JointTrajectoryController
  - velocity_controllers/JointGroupVelocityController
  - velocity_controllers/JointPositionController
  - velocity_controllers/JointTrajectoryController
  - velocity_controllers/JointVelocityController

其中，real_robot_control.launch 采用的是position_controllers/JointTrajectoryController基于位置的关节轨迹控制。

若要采用其他的ROS控制器，需要修改alicia_duo_moveit/config下的ros_controllers.yaml

controller_list:
  - name: arm_pos_controller
    action_ns: follow_joint_trajectory
    type: FollowJointTrajectory
    default: true
    joints:
      - Joint1
      - Joint2
      - Joint3
      - Joint4
      - Joint5
      - Joint6

arm_pos_controller:
  type: position_controllers/JointTrajectoryController
  joints:
    - Joint1
    - Joint2
    - Joint3
    - Joint4
    - Joint5
    - Joint6
  gains:
    Joint1: {p: 0.2, i: 0.0, d: 0.0} 
    Joint2: {p: 0.2, i: 0.0, d: 0.0}
    Joint3: {p: 0.2, i: 0.0, d: 0.0}
    Joint4: {p: 0.2, i: 0.0, d: 0.0}
    Joint5: {p: 0.2, i: 0.0, d: 0.0}
    Joint6: {p: 0.2, i: 0.0, d: 0.0}

joint_state_controller:
  type: joint_state_controller/JointStateController
  publish_rate: 50

修改action_ns, type为对应的控制其类型及对应的PID参数(请谨慎PID调参， P, I 值过大机械臂可能会振荡或失控)。

3.3.2 alicia_hw.cpp​

在init函数中注册/删除相关的控制器controller_manager接口

bool MyRobotHW::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh)
{
    ROS_INFO("MyRobotHW::init() called, interfaces registered.");

    num_joints_ = 6;
    joint_names_ = {"Joint1", "Joint2", "Joint3", "Joint4", "Joint5", "Joint6"};

    joint_position_.resize(num_joints_, 0.0);
    joint_velocity_.resize(num_joints_, 0.0);
    joint_effort_.resize(num_joints_, 0.0);
    joint_position_command_.resize(num_joints_, 0.0);

    // Initialize PID variables
    p_gains_.resize(num_joints_, 0.0);
    i_gains_.resize(num_joints_, 0.0);
    d_gains_.resize(num_joints_, 0.0);
    position_error_.resize(num_joints_, 0.0);
    position_error_integral_.resize(num_joints_, 0.0);
    position_error_derivative_.resize(num_joints_, 0.0);
    prev_position_error_.resize(num_joints_, 0.0);

    for (int i = 0; i < num_joints_; ++i)
    {
        joint_state_interface_.registerHandle(
            hardware_interface::JointStateHandle(joint_names_[i],
                                                 &joint_position_[i],
                                                 &joint_velocity_[i],
                                                 &joint_effort_[i]));

        position_joint_interface_.registerHandle(
            hardware_interface::JointHandle(joint_state_interface_.getHandle(joint_names_[i]),
                                            &joint_position_command_[i]));
    }

    registerInterface(&joint_state_interface_);
    registerInterface(&position_joint_interface_);

    readpid(root_nh, ros::Time::now(), ros::Duration(0.0)); // Call readpid to initialize PID parameters
    ROS_INFO("Subscribed to /arm_pos_controller/state");
    return true;
}

完成修改后重新编译工作空间：

cd ~/alicia_ws
catkin_make
source devel/setup.bash

3.3.3 PID 控制​

本产品的PID 控制为可选项，对于机械臂的MoveIt规划控制的相应时间有严格要求的用户可增加该功能。

下图是Alicia duo采用MoveIt规划后目标进行线性插值后的关节1的控制响应曲线：

实现机械臂PID控制的步骤主要包含：

  • 修改alicia_hw.cpp;

  • PID 调参；

  • 记录最优的PID参数并修改默认值。

3.3.3.1 修改alicia_hw.cpp;​

将alicia_hw.cpp中的write函数从：

void MyRobotHW::write(const ros::Time& time, const ros::Duration& period)
{
    const int num_interpolation_steps = 15; // 您可以调整这个值
    
    // Create a vector for interpolated positions for this write cycle
    std::vector<double> interpolated_joint_positions(num_joints_);

    // Perform interpolation and send commands
    for (int step = 0; step < num_interpolation_steps; ++step)
    {
        for (int i = 0; i < num_joints_; ++i)
        {
            // Calculate the interpolation factor (alpha) from 0.0 to 1.0
            // For the last step, ensure alpha is exactly 1.0 to reach the target
            double alpha = (num_interpolation_steps > 1) ? 
                           static_cast<double>(step) / (num_interpolation_steps - 1) : 
                           1.0;
            if (step == num_interpolation_steps - 1) { // Ensure last step reaches target
                alpha = 1.0;
            }

            // Linear interpolation: current_actual + alpha * (target_command - current_actual)
            interpolated_joint_positions[i] = joint_position_[i] + alpha * (joint_position_command_[i] - joint_position_[i]);
        }
        
        // Send the interpolated joint positions

        serial_helper_.writeServoCommand(interpolated_joint_positions, gripper_position_command_);
    }
}

替换为

void MyRobotHW::write(const ros::Time& time, const ros::Duration& period)
{
    // PID control

    std::vector<double> current_cycle_position_error(num_joints_);
    {
        std::lock_guard<std::mutex> lock(error_mutex_);
        // Copy the subscribed error to use for this PID cycle
        // This ensures consistency if the callback updates it during calculations
        // and minimizes lock duration.
        current_cycle_position_error = position_error_;
    }

    std::vector<double> control_signals(num_joints_);
    std::vector<double> control_positions(num_joints_);

    if (period.toSec() <= 0.0) { // Avoid division by zero if period is invalid
        ROS_WARN_THROTTLE(1.0, "PID control: period is zero or negative, skipping derivative calculation.");
        for (int i = 0; i < num_joints_; ++i) {
            control_signals[i] = p_gains_[i] * current_cycle_position_error[i]; // Basic P control if period is bad
        }
    } else {
        for (int i = 0; i < num_joints_; ++i)
        {
            // position_error_[i] = joint_position_command_[i] - joint_position_[i]; // REMOVED - Now using error from topic

            double error_for_this_joint_pid = current_cycle_position_error[i] * 1000000;
            // double error_for_this_joint_pid = current_cycle_position_error[i]; 

            // Integral term - accumulates the error from the topic
            position_error_integral_[i] += error_for_this_joint_pid * period.toSec();
            // TODO: Add anti-windup for position_error_integral_[i] if not already implemented

            // Derivative term - rate of change of the error from the topic
            position_error_derivative_[i] = (error_for_this_joint_pid - prev_position_error_[i]) / period.toSec();
            
            // Update previous error with the error used in this cycle's D calculation
            prev_position_error_[i] = error_for_this_joint_pid;

            // PID control logic
            // std::cout << "Joint " << i << " PID: P=" << p_gains_[i] << ", I=" << i_gains_[i] << ", D=" << d_gains_[i] << std::endl;
            control_signals[i] = (p_gains_[i] * error_for_this_joint_pid +
                                 i_gains_[i] * position_error_integral_[i] +
                                 d_gains_[i] * position_error_derivative_[i])/ 1000000;
            // std::cout << "Joint " << i << " Control Signal: " << control_signals[i] << std::endl;
            control_positions[i] = joint_position_[i] + control_signals[i] ; // Calculate the new position
            // std::cout << "Joint " << i << " Control Position: " << control_positions[i] << std::endl;
        }
    }
    serial_helper_.writeServoCommand(control_positions, gripper_position_command_);
    }

完成修改后重新编写ROS 工作空间。

cd ~/alicia_ws
catkin_make
source devel/setup.bash
3.3.3.2 PID调参​

在以下两个指令已经运行之后

roslaunch alicia_duo_driver serial_server.launch
roslaunch alicia_duo_moveit real_robot_control.launch

运行相应的PID实时调参代码

roscd alicia_duo_ros_control
python3 ./scripts/pid_tuner_gui.py

在显示的界面中更改P, I, D的值，请确保即停按钮在手侧以防机械臂失控时断电。

同时建议从关节1开始调参，依次调P, I, D值。

一般D保持为0，P 从小到大调整，调到临界振荡后再将I从0开始调整。

修改完关节后再点击Apply to Joint1完成PID 参数更改。

同时在新终端中运行

rostopic echo /arm_pos_controller/state

可以得到当前ros controller的控制状态：

header: 
  seq: 408705
  stamp: 
    secs: 1746860277
    nsecs: 785812141
  frame_id: ''
joint_names: 
  - Joint1
  - Joint2
  - Joint3
  - Joint4
  - Joint5
  - Joint6
desired: 
  positions: [-0.5262909803639172, -0.01382770622665994, -0.0014948602706659573, 0.010704920874768877, 0.016918329594330865, -0.006143166269781067]
  velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
  accelerations: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
  effort: []
  time_from_start: 
    secs: 1
    nsecs: 705538735
actual: 
  positions: [-0.5230874419212341, -0.023009711876511574, -0.0015339808305725455, 0.01073786523193121, 0.016873788088560104, -0.006135923322290182]
  velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
  accelerations: []
  effort: []
  time_from_start: 
    secs: 2486
    nsecs: 261424456
error: 
  positions: [-0.0032035384426829516, 0.009182005649851721, 3.9120559906624663e-05, -3.294435716227895e-05, 4.4541505770823164e-05, -7.24294749110399e-06]
  velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
  accelerations: []
  effort: []
  time_from_start: 
    secs: -2485
    nsecs: 444114279

根据上面echo出的信息打开rqt_plot观察控制的响应曲线。

rqt_plot

在topic框栏中输入

/arm_pos_controllers/state/actual/positions[0]
/arm_pos_controllers/state/desired/positions[0]

得到关节1的目标位置曲线和当前位置的两个曲线。

之后，使用moveit控制关节运动，观察到响应曲线后点击右上角的暂停按钮以便于分析当前PID参数的控制效果。

以下是P = 1.5, I = 0.018, D =0的暂态响应曲线示例，可根据不同需求调出最优参数。

3.3.3.3 记录最优的PID参数并修改默认值。​

修改alicia_duo_moveit/config/ros_controllers.yamls文件中的gains后保存即可。

controller_list:
  - name: arm_pos_controller
    action_ns: follow_joint_trajectory
    type: FollowJointTrajectory
    default: true
    joints:
      - Joint1
      - Joint2
      - Joint3
      - Joint4
      - Joint5
      - Joint6

arm_pos_controller:
  type: position_controllers/JointTrajectoryController
  joints:
    - Joint1
    - Joint2
    - Joint3
    - Joint4
    - Joint5
    - Joint6
  gains:
    Joint1: {p: 0.2, i: 0.0, d: 0.0} 
    Joint2: {p: 0.2, i: 0.0, d: 0.0}
    Joint3: {p: 0.2, i: 0.0, d: 0.0}
    Joint4: {p: 0.2, i: 0.0, d: 0.0}
    Joint5: {p: 0.2, i: 0.0, d: 0.0}
    Joint6: {p: 0.2, i: 0.0, d: 0.0}

joint_state_controller:
  type: joint_state_controller/JointStateController
  publish_rate: 50

4. 单操作臂的调零​

若观察到启动real_robot_control.launch后rviz中机械臂的运动状态和机械臂的不一致，这主要是机械臂设置的零点位置和Moveit不匹配导致，需要重新调节机械臂的零点。

主要分为两个步骤：

  1. 机械臂进入0力矩状态；

  2. 设置目标姿态；

  3. 运行调零代码。

4.1 步骤1​

运行机械臂的ROS硬件通讯代码：

roslaunch alicia_duo_driver serial_server.launch

在终端中运行指令以0力矩控制（无力矩状态时机械臂无法维持当前姿态，请注意安全）：

rostopic pub /demonstration std_msgs/Bool "data: true"

成功后按ctrl-C 结束发布指令。

4.2 步骤2​

将机械臂摆放到目标零点位置：

1处保持垂直，2处保持45度角，3处保持平行。

4.3 步骤3​

运行调零代码：

cd ~/alicia_ws/src/examples/
python3 alicia_duo_zero_calibration.py

机械臂重新回到满力矩状态后表示完成调零工作。

5. 单操作臂的拖动示教​

5.1 启动命令​

roslaunch alicia_duo_driver serial_server.launch

在另一个终端

roscd alicia_duo_drag_teaching/scripts
python3 pose_record.py

运行pose_record.py前建议捏住机械臂的上端合适位置以防止机械臂0力矩后碰撞桌面。

5.2 停止拖动示教​

停止拖动示教的方式：

  • 达到设定最长时间后自动停止录制；

  • 按键s中断；

其中，设定时长在pose_record.py的PoseRecorder类初始化中修改。


class PoseRecorder:
    """用于记录机器人关节姿态的ROS节点类"""
    
    def __init__(self):
        """初始化姿态记录器节点"""
        rospy.init_node('pose_recorder', anonymous=True)
        
        # 获取记录时长参数
        self.record_duration = 10  # 默认记录10秒
    ...

在完成记录后可键入Enter选择实时姿态还原。

或者键入其他任意键退出，运行：

python3 pose_replicator.py

也可还原姿态，还原的数据是同目录下的pose_data.bag存储的上一个示教记录。

5.3 效果展示​

终端的运行结果如下：

[INFO] [1746864088.321231]: 姿态记录器已初始化，正在监听 /arm_joint_state 主题
[INFO] [1746864088.323308]: 按 's' 键停止记录并保存数据
[INFO] [1746864089.830843]: 已开始记录关节状态，数据将保存到: ....../alicia_duo_drag_teaching/script/pose_data.bag
s
[INFO] [1746864096.982006]: 已停止记录
[INFO] [1746864096.986897]: --------------------------------------------------------------------------------
[INFO] [1746864096.996564]: 主题: /recorded_arm_joint_state
[INFO] [1746864096.998525]: 消息类型: alicia_duo_driver/ArmJointState
[INFO] [1746864097.000337]: 消息数量: 598
[INFO] [1746864097.002062]: 开始时间: 1746864089.8384588
[INFO] [1746864097.003416]: 结束时间: 1746864096.975816
[INFO] [1746864097.004634]: 持续时间: 7.14 秒
[INFO] [1746864097.005741]: --------------------------------------------------------------------------------
[INFO] [1746864097.006784]: 前5条消息预览:
[INFO] [1746864097.014022]: [1746864089.8384588] J1=-0.0966, J2=-0.4019, J3=0.5323, J4=0.0138, J5=0.4878, J6=0.0077, Gripper=0.0102
[INFO] [1746864097.014769]: [1746864089.8501806] J1=-0.0966, J2=-0.4111, J3=0.5369, J4=0.0138, J5=0.4863, J6=0.0077, Gripper=0.0102
[INFO] [1746864097.015336]: [1746864089.8617556] J1=-0.0966, J2=-0.4203, J3=0.5430, J4=0.0138, J5=0.4863, J6=0.0077, Gripper=0.0102
[INFO] [1746864097.015824]: [1746864089.873515] J1=-0.0966, J2=-0.4326, J3=0.5492, J4=0.0138, J5=0.4863, J6=0.0077, Gripper=0.0102
[INFO] [1746864097.016265]: [1746864089.8853006] J1=-0.0966, J2=-0.4433, J3=0.5553, J4=0.0138, J5=0.4863, J6=0.0077, Gripper=0.0102
[INFO] [1746864097.016673]: ... (总共 598 条消息)
[INFO] [1746864097.017077]: --------------------------------------------------------------------------------
[INFO] [1746864097.018954]: 正在关闭拖动示教模式...
[INFO] [1746864098.521311]: --------------------------------------------------------------------------------
[INFO] [1746864098.524080]: 记录已完成，是否要执行姿态还原？
[INFO] [1746864098.526465]: 按 Enter 键启动姿态还原，按其他键退出...
[INFO] [1746864103.597721]: 启动姿态还原...
[INFO] [1746864103.606679]: 姿态还原节点启动命令已发送
[INFO] [1746864103.609799]: 姿态记录器退出...
[INFO] [1746864104.046068]: 姿态复制器初始化完成
[INFO] [1746864104.050654]: 执行速度因子: 1.00
[INFO] [1746864104.086647]: 成功读取 598 个轨迹点
[INFO] [1746864104.087331]: 按 Enter 键开始执行轨迹...
[INFO] [1746864107.390125]: 开始执行轨迹，总时长：7.14秒
[INFO] [1746864115.534549]: 轨迹执行完成
[INFO] [1746864115.537771]: 轨迹执行完成

6. 单操作臂的2D抓取​

6.1 环境搭建​

采用的设备：

  • Alicia Duo单操作臂；

  • ORBBEC（奥比中光）相机；

  • 1.5 cm X 1.5 cm 正方体（被抓取物体）

6.1.1 抓取区域搭建​

使用白纸和黑色胶带按照下图布置抓取工作区域，并测量其长度和宽度以便后续在代码中使用，同时准备一个物体用于 2D 抓取。

6.1.2 相机准备​

通过相机支架或其他工具将固定相机位置，确保相机的视野包含待识别的抓取区域及用于相机标定的Aruco码。

相机需要包含RGB镜头，能够读取出RGB图像。

6.2 相机外参标定​

请参考以下手眼标定教程，完成“眼在外”模式下的相机外参标定：

手眼标定

6.3 2D抓取流程图​

6.4 主要代码修改​

6.4.1 启动机械臂、MoveIt及相机​

roslaunch alicia_duo_driver serial_server.launch 
roslaunch alicia_duo_moveit real_robot_control.launch camera_load:=true2

6.4.2 物体HSV范围选取部分​

打开rqt_image_view保存一张相机读取的RGB图像到~/alicia_ws/src/alicia_duo_grasp_2d/scripts/路径下并命名为image.png后，运行以下代码：

roscd alicia_duo_grasp_2d/scripts/
python3 hsv_select.py

用鼠标多次点击目标物体的表面获取其HSV值范围。

Clicked at (153, 121) - HSV: [ 80 108  71]
Clicked at (160, 127) - HSV: [ 80 115  60]
Clicked at (150, 137) - HSV: [ 29 110 107]
Clicked at (171, 136) - HSV: [72 67 69]

注：HSV: [ 80 108 71]中[ 80 108 71]分别代表了该像素点的Hue, Saturation和Value值

根据获取的HSV区间范围修改obj_detection.py中的hsv_min和hsv_max值（参见以下代码中标黄参数）：

    def detect2DObject(self, image, table_bbox):
        # image = self._convert_to_cv(msg)
        if image is None:
            return 0, 0

        hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
        hsv_min = np.array([35, 50, 50])
        hsv_max = np.array([85, 255, 255])
        mask = cv2.inRange(hsv, hsv_min, hsv_max)

6.4.3 抓取工作区域选定部分​

运行以下代码

roscd alicia_duo_grasp_2d/scripts/
python3 obj_detection.py

在弹出的界面中选取四个点框定后续图像处理的区域后，按Esc退出图像显示：

Image dimensions: 640x360 (width x height)
Click on points to get coordinates. Press 'Esc' to continue.
Pixel coordinates: x=198, y=229
RGB value: [163 163 163]
Pixel coordinates: x=404, y=213
RGB value: [166 167 165]
Pixel coordinates: x=192, y=351
RGB value: [163 163 163]
Pixel coordinates: x=436, y=337
RGB value: [181 181 181]

根据选取的四个像素点的坐标修改obj_detection.py中的select_workspace的默认像素坐标值，修改后可在下次按Esc直接跳过该步骤：


def select_workspace(image, points=None):
    """
    Crop the workspace based on provided or default points.
    :param image: Input image
    :param points: List of (x,y) points defining workspace corners. If None, use defaults.
    :return: cropped image
    """
    # Use default points if none provided
    if points is None:
        # Default points
        points = [[176, 122], [425, 114], [135, 348], [468, 346]]

6.4.4 物体坐标转换部分​

根据设置的抓取工作区域尺寸，修改 length和breadth参数：

if __name__ == '__main__':
    try:
        rospy.init_node('vision_manager_node')
        rospy.loginfo("Starting Vision Manager...")

        length = rospy.get_param("~table_length", 0.34)
        breadth = rospy.get_param("~table_breadth", 0.315)

修改data_process中的position_msg.z的值（参见以下代码中标黄参数，其指代物体到相机的高度，可根据外参标定结果修改）

    def data_process(self):    
        ...
        position_msg = Point()
        position_msg.x = x
        position_msg.y = y
        position_msg.z = 0.5177033416941345  # Set from calibration
        self.position_pub.publish(position_msg)

修改同路径下的obpose.py的以下内容：

    def load_transform_from_yaml(self):
        """从 YAML 文件加载标定变换矩阵"""
        yaml_path = os.path.expanduser("~/.ros/easy_handeye/orbbec_handeyecalibration_eye_on_base.yaml")

替换yaml_path为实际的外参标定结果文件路径。

6.5 启动命令​

6.5.1 单独测试​

依次在4个终端中运行以下代码：

roslaunch alicia_duo_driver serial_server.launch 
roslaunch alicia_duo_moveit real_robot_control.launch camera_load:=true2
python3 obpose.py 
python3 obj_detection.py

在运行obj_detection.py后，运行：

rostopic echo /detected_object_position

可检验识别出的物体在图像坐标系下的x, y, z是否正确。

在运行obpose.py的终端会打印出物体转换到机械臂的基坐标系下的坐标，可大致验证转换后的x, y, z是否在误差允许的范围内，否则需要检查外参结果加载和坐标转换部分的代码或者重新进行外参标定。

在上述验证过程通过后，运行以下抓取代码实现2d抓取：

python3 grasp_2d.py

6.5.2 整体运行​

roslaunch alicia_duo_driver serial_server.launch 
roslaunch alicia_duo_grasp_2d grasp_execution.launch

7. 单操作臂的6D抓取​

7.1 环境搭建​

采用的设备：

  • Alicia Duo单操作臂；

  • ORBBEC（奥比中光）相机；

  • 正方体

7.1.1 抓取物体准备​

准备大小合适的物体用于抓取。

7.1.2 相机准备​

通过相机支架或其他工具将固定相机位置，确保相机的视野包含待识别的抓取区域。

相机需要包含RGB镜头，能够读取出RGB和depth深度图像。

7.2 相机外参标定​

请参考以下手眼标定教程，完成“眼在外”模式下的相机外参标定：

手眼标定

7.3 前置要求​

  • GPU （nvidia-smi查看）

  • Anaconda3 或 Miniconda3

  • CUDA Toolkit (nvcc --verison查看)

7.4 Conda环境配置​

  1. Conda环境创建及基本库安装
conda create -n grasp python=3.8
conda activate grasp
conda install pytorch torchvision torchaudio pytorch-cuda=11.8 -c pytorch -c nvidia
  • MinkowskiEngine安装
cd ~/alicia_ws/src/alicia_duo_grasp_6d
git clone https://github.com/NVIDIA/MinkowskiEngine.git
cd MinkowskiEngine
export CXX=g++-7
python setup.py install --blas_include_dirs=${CONDA_PREFIX}/include --blas=openblas

具体可参考https://github.com/NVIDIA/MinkowskiEngine

  • graspnetAPI安装
pip install graspnetAPI

具体可参考 https://github.com/graspnet/graspnetAPI

  • graspness_implementation安装
cd cd ~/alicia_ws/src
git clone https://github.com/rhett-chen/graspness_implementation ./alicia_duo_grasp_6d/
cd alicia_duo_grasp_6d/pointnet2
python setup.py install # install pointnet2 packg
cd ../knn
python setup.py install # install knn package

具体可参考https://github.com/rhett-chen/graspness\_implementation

7.5 启动命令​

启动机械臂控制及相机：

roslaunch alicia_duo_driver serial_server.launch 
roslaunch alicia_duo_moveit real_robot_control.launch camera_load:=true2

因graspness模型的物体识别和抓取预测已训练完成，可直接调用训练好的checkpoint文件直接预测目标物体位姿及最优抓取位姿。

cd ~/alicia_ws/src/alicia_duo_grasp_6d/
conda activate grasp
python alicia_grasp.py