喵喵爱编程

这个屌丝很懒，什么也没留下！

热门标签

Gazebo + RViz + Moveit + ur5e机械臂仿真（环境搭建二）_rviz仿真机械臂

作者：喵喵爱编程 | 2024-07-13 09:14:13

踩

rviz仿真机械臂

【前言】本篇文章旨在实现，在gazebo添加摄像头模型以及二维码，以及实现机械臂点动及二维码跟踪。

一、主要步骤

【Gazebo仿真·二】Gazebo + ur5e + RealSense + Aruco 二维码识别与跟随仿真_使用realsense进行gazebo仿真-CSDN博客

二、部分问题解答

2.1 资源获取

链接: https://pan.baidu.com/s/1HGWTH70RynIQVyEwZCqnkQ?pwd=kr4w 提取码: kr4w

建议在Ubuntu中的火狐浏览器直接打开这个链接然后将文件下载下来，节省很多安装时间。但是这只是安装文件，有些插件仍需通过从终端输入指令下载。

我的arm_control中的代码和主要步骤中的链接有了较大改动，将笛卡尔空间规划更换为了关节空间规划。

关节空间的轨迹规划和笛卡尔空间的轨迹规划联系与区别_关节空间轨迹规划与笛卡尔轨迹规划的区别-CSDN博客

2.2 运动学求解器替换

MoveIt! 学习笔记13 - KDL/IKFAST/TRAC-IK运动学求解器区别_moveit 提供了运动学求解器-CSDN博客
 【IKFast】IKFast配置与使用-CSDN博客

如果只是作为大概了解可以将KDL更换为TRACC-IK。在使用主要步骤中arm_control中的arm_control的笛卡尔空间规划的成功率会有提升。但建议还是使用关节空间规划。

关节空间的轨迹规划和笛卡尔空间的轨迹规划联系与区别_关节空间轨迹规划与笛卡尔轨迹规划的区别-CSDN博客

2.3 二维码生成

在Online ArUco markers generator (chev.me)中生成二维码时，要选择Original ArUco，并且确保生成的Marker ID和Marker size与设置中的相同。否则，将无法通过

rostopic echo /aruco_single/pose

识别到二维码的位置信息。

三、代码测试

3.1 常用代码总结


roslaunch ur5e_moveit_config demo_gazebo.launch
roslaunch aruco_ros demo.launch
rostopic echo /aruco_single/pose
rosrun image_view image_view image:=/aruco_single/result
 
rosrun arm_control arm_control
rosrun arm_control move_with_marker

3.2 arm_control 解读


/** 
 * 控制机械臂在关节空间进行移动，并输出移动前后的关节值
 **/
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit_msgs/MoveItErrorCodes.h>
#include <iostream>
 
int main(int argc, char **argv)
{
    ros::init(argc, argv, "arm_control"); // 初始化
    ros::NodeHandle node_handle;
    ros::AsyncSpinner spinner(1); // 多线程
    spinner.start(); // 开启新的线程
 
    moveit::planning_interface::MoveGroupInterface arm("arm"); // 初始化机械臂的arm group
 
    std::string end_effector_link = arm.getEndEffectorLink(); // 获取末端执行器的link名称
    std::cout << "End effector link: " << end_effector_link << std::endl;
 
    std::string reference_frame = "base_link"; // 设置目标位置所使用的参考坐标系
    arm.setPoseReferenceFrame(reference_frame);
 
    arm.allowReplanning(true); // 允许重新规划
    arm.setGoalJointTolerance(0.001);
    arm.setGoalPositionTolerance(0.001); // 设置位置和姿态的允许误差
    arm.setGoalOrientationTolerance(0.01);
    arm.setMaxAccelerationScalingFactor(0.2); // 设置最大加速度
    arm.setMaxVelocityScalingFactor(0.2); // 设置最大速度
 
    // 获取并输出机械臂当前的关节值
    std::vector<double> start_joint_values = arm.getCurrentJointValues();
    std::cout << "Start joint values: ";
    for (double joint_value : start_joint_values) {
        std::cout << joint_value << " ";
    }
    std::cout << std::endl;
 
    // 设置目标关节值
    std::vector<double> joint_group_variables;
 
    // 这里需要根据实际机械臂的关节空间和目标点位来设置关节值
    joint_group_variables.push_back(1.0);
    joint_group_variables.push_back(0.5);
    joint_group_variables.push_back(-0.5);
    joint_group_variables.push_back(1.0);
    joint_group_variables.push_back(0.0);
    joint_group_variables.push_back(0.0);
 
    // 设置关节目标
    arm.setJointValueTarget(joint_group_variables);
 
    // 规划路径
    moveit::planning_interface::MoveGroupInterface::Plan plan;
    moveit::core::MoveItErrorCode error_code = arm.plan(plan);
 
    if (error_code) {
        ROS_INFO("Joint space planning succeeded.");
        arm.execute(plan); // 执行规划的路径
 
        // 移动后的关节值
        std::vector<double> end_joint_values = arm.getCurrentJointValues();
        std::cout << "End joint values: ";
        for (double joint_value : end_joint_values) {
            std::cout << joint_value << " ";
        }
        std::cout << std::endl;
    } 
 
    ros::shutdown();
    return 0;
}

这里使用笛卡尔空间的代码会更直观的帮助我们理解，但是使用关节空间会拥有更高的规划效率。

3.3 move_with_marker 解读

程序的主要逻辑是：

订阅标记位置信息。
当标记位置发生变化时，更新机械臂的目标位置。
规划机械臂到目标位置的路径。
如果规划成功，执行路径；如果失败，重置目标位置为当前位置。
检查机械臂是否到达目标位置，如果没有，重置目标位置。


//问题：二次移动后，不变轴的数据会被赋予上次的数值（已解决）
//问题：二次移动二维码后，不能规划（已解决）
//问题：规划失败后，变量已被提前储存（已解决；规划失败，将变量定为当前数值）
//问题：在已经完成跟踪后，仍然会不停校正位置，从而导致崩溃（已解决）
//问题：在规划成功，但运动失败后，变量已被提前储存（已解决）
//问题：路径规划结果不稳定（已解决）
//目标：增加终端的信息输出（已解决）
 
#include <ros/ros.h>
#include <moveit/move_group_interface/move_group_interface.h>
#include <geometry_msgs/PoseStamped.h>
#include <iostream>
 
class MoveToMarker
{
private:
    ros::NodeHandle nh_;
    ros::Subscriber sub_;
    moveit::planning_interface::MoveGroupInterface arm;
    geometry_msgs::Pose markerPose_; // 私有成员变量，存储目标位置和姿态
    geometry_msgs::Point compare; // 用于比较位置变化的私有成员变量
 
public:
    bool ifMove;
    MoveToMarker(ros::NodeHandle n);
    ~MoveToMarker();
 
    void markerCb(const geometry_msgs::PoseStamped& pose);
    void moveOnce();
};
 
MoveToMarker::~MoveToMarker() {}
 
MoveToMarker::MoveToMarker(ros::NodeHandle n): nh_(n), arm("arm")
{
    this->nh_ = n;
    ifMove = false;
 
    std::string end_effector_link = arm.getEndEffectorLink();
    std::cout << "End effector link: " << end_effector_link << std::endl;
 
    // 初始化机械臂末端位置及姿态
    markerPose_.position.x = 0.1;
    markerPose_.position.y = 0.134;
    markerPose_.position.z = 0.98;
    markerPose_.orientation.x = 0.0;
    markerPose_.orientation.y = 0.707;
    markerPose_.orientation.z = 0.0;
    markerPose_.orientation.w = 0.707;
    
    // 初始化对比值
    compare.x = 0;
    compare.y = 0;
    compare.z = 0;
   
    ros::AsyncSpinner spinner(1);
    spinner.start();
 
    std::string reference_frame = "base_link";
    arm.setPoseReferenceFrame(reference_frame);
 
    arm.allowReplanning(true);
    arm.setGoalJointTolerance(0.001);
    arm.setGoalPositionTolerance(0.001);
    arm.setGoalOrientationTolerance(0.01);
    arm.setMaxAccelerationScalingFactor(0.2);
    arm.setMaxVelocityScalingFactor(0.2);
 
    geometry_msgs::Pose now_pose = arm.getCurrentPose(end_effector_link).pose;
    std::cout << "now Robot position: [x,y,z]: [" << now_pose.position.x << "," << now_pose.position.y << "," << now_pose.position.z << "]" << std::endl;
    std::cout << "now Robot orientation: [x,y,z,w]: [" << now_pose.orientation.x << "," << now_pose.orientation.y << "," << now_pose.orientation.z << "," << now_pose.orientation.w << "]" << std::endl;
 
    arm.setNamedTarget("init_pose"); // 控制机械臂先回到初始化位置
    arm.move();
    sleep(1);
 
    sub_ = n.subscribe("/aruco_single/pose", 1, &MoveToMarker::markerCb, this);
}
 
void MoveToMarker::markerCb(const geometry_msgs::PoseStamped& pose)
{
    // 检查marker位置是否发生变化(通过对比和阈值，避免噪声的干扰)
    // 通过rostopic echo /aruco_single/pose得知，模拟环境噪声变化绝大多数小于0.01
    // 真实情况需要再次评估噪声
    double threshold = 0.05;
    bool position_changed = 
        abs(compare.x - pose.pose.position.x) > threshold ||
        abs(compare.y - pose.pose.position.y) > threshold ||
        abs(compare.z - pose.pose.position.z) > threshold;
 
    if (position_changed) {
        // 位置发生了变化，更新markerPose_为当前机械臂末端的位置
        markerPose_.position.x +=  pose.pose.position.x;
        markerPose_.position.y +=  pose.pose.position.z - 0.8;
        markerPose_.position.z += -pose.pose.position.y;
 
        // 更新姿态信息，如果需要的话
        // markerPose_.orientation = pose.pose.orientation;
 
        // 更新对比数值
        compare.x = pose.pose.position.x;
        compare.y = pose.pose.position.y;
        compare.z = pose.pose.position.z;
 
        ifMove = true; // 设置标志，位置已变化，需要移动
    }
 
    else{
        ifMove = false; // 设置标志，位置未变化，不需要移动
    }
}
 
void MoveToMarker::moveOnce()
{
    ros::AsyncSpinner spinner(1); // 多线程
    spinner.start(); // 开启新的线程
 
    // 直接输出目标位置和姿态
    std::cout << "target position: [x,y,z]: [" << markerPose_.position.x << "," << markerPose_.position.y << "," << markerPose_.position.z << "]" << std::endl;
    std::cout << "target orientation: [x,y,z,w]: [" << markerPose_.orientation.x << "," << markerPose_.orientation.y << "," << markerPose_.orientation.z << "," << markerPose_.orientation.w << "]" << std::endl;
 
    // 设置机械臂的目标位置和姿态
    arm.setPoseTarget(markerPose_);
 
    // 规划路径
    moveit::planning_interface::MoveGroupInterface::Plan plan;
    if (arm.plan(plan)) {
        // 输出执行前机械臂的目标位置
        std::cout << "Moving arm to target position: [x,y,z]: ["
                  << markerPose_.position.x << ", "
                  << markerPose_.position.y << ", "
                  << markerPose_.position.z << "]" << std::endl;
 
        arm.execute(plan);
        ROS_INFO("Joint space planning succeeded.");
 
        // 执行规划的路径
        arm.execute(plan);
 
        // 检查机械臂是否到达目标位置
        geometry_msgs::Pose end_effector_pose = arm.getCurrentPose(arm.getEndEffectorLink()).pose;
        bool reached_target = std::abs(end_effector_pose.position.x - markerPose_.position.x) < 0.05 &&
                              std::abs(end_effector_pose.position.y - markerPose_.position.y) < 0.05 &&
                              std::abs(end_effector_pose.position.z - markerPose_.position.z) < 0.05;
 
        if (reached_target) {
            ROS_INFO("The arm has reached the target position.");
        }
 
        else {
            markerPose_.position = end_effector_pose.position;
        }
 
        ifMove = false; // 设置标志，已完成移动，不需要移动
        // 输出当前机械臂末端的位置
        std::cout << "Current end effector position: [x,y,z]: ["
                  << end_effector_pose.position.x << ", "
                  << end_effector_pose.position.y << ", "
                  << end_effector_pose.position.z << "]" << std::endl;
    } 
 
    else{
        ROS_INFO("Joint space planning failed. Resetting target pose to current end effector position.");
        
        ifMove = false;
 
        // 获取当前机械臂末端的位置
        geometry_msgs::Pose current_pose = arm.getCurrentPose(arm.getEndEffectorLink()).pose;
 
        if (std::abs(markerPose_.position.x - current_pose.position.x) > 0.05 &&
            std::abs(markerPose_.position.y - current_pose.position.y) > 0.05 &&
            std::abs(markerPose_.position.z - current_pose.position.z) > 0.05) {
            markerPose_.position = current_pose.position;
        }
 
        // 可以在这里添加代码来更新markerPose_.orientation为当前机械臂末端的姿态
        // markerPose_.orientation = current_pose.orientation;
 
    }
}
 
int main(int argc, char **argv)
{
    ros::init(argc, argv, "move_with_marker_node");
    ros::NodeHandle n;
 
    MoveToMarker myMove(n);
    ros::WallDuration(5.0).sleep(); // 等待5秒
 
    while (ros::ok()) {
        ros::spinOnce();
        if (myMove.ifMove) {
            myMove.moveOnce();
            myMove.ifMove = false; // 重置标志，避免重复移动
        }
        ros::Duration(0.2).sleep();
    }
 
    return 0;
}

通过不断在gazebo中进行测试，可以发现很多问题。通过分析在什么情况下，会出现不理想的运动，来改善代码。（这里的代码和网盘资源里的move_with_marker比，有一些小的改进）

3.4 路径规划

在catkin_ws/src/robot_gazebo/ur5e_moveit_config/ompl_planning.yaml中更换路径规划器，将RRT换为RRTConnect或者RRTstar（RRT*）

3.5 二次优化代码


//问题：二次移动后，不变轴的数据会被赋予上次的数值（已解决）
//问题：二次移动二维码后，不能规划（已解决）
//问题：规划失败后，变量已被提前储存（已解决；规划失败，将变量定为当前数值）
//问题：在已经完成跟踪后，仍然会不停校正位置，从而导致崩溃（已解决）
//问题：在规划成功，但运动失败后，变量已被提前储存（已解决）
//问题：路径规划结果不稳定（已解决）
//问题：姿态规划算法不正确（未解决）
//目标：增加终端的信息输出（已完成）
//目标：增加位置跟踪功能（已完成)
//目标：增加姿态跟踪功能（未完成)
#include <ros/ros.h>
#include <moveit/move_group_interface/move_group_interface.h>
#include <geometry_msgs/PoseStamped.h>
#include <iostream>
 
class MoveToMarker
{
private:
    ros::NodeHandle nh_;
    ros::Subscriber sub_;
    moveit::planning_interface::MoveGroupInterface arm;
    geometry_msgs::Pose markerPose_; // 私有成员变量，存储目标位置和姿态
    geometry_msgs::Point compare; // 用于比较位置变化的私有成员变量
 
public:
    bool ifMove;
    MoveToMarker(ros::NodeHandle n);
    ~MoveToMarker();
 
    void markerCb(const geometry_msgs::PoseStamped& pose);
    void moveOnce();
};
 
MoveToMarker::~MoveToMarker() {}
 
MoveToMarker::MoveToMarker(ros::NodeHandle n): nh_(n), arm("arm")
{
    this->nh_ = n;
    ifMove = false;
 
    std::string end_effector_link = arm.getEndEffectorLink();
    std::cout << "End effector link: " << end_effector_link << std::endl;
 
    // 初始化机械臂末端位置及姿态
    markerPose_.position.x = 0.1;
    markerPose_.position.y = 0.134;
    markerPose_.position.z = 0.98;
    markerPose_.orientation.x = 0.0;
    markerPose_.orientation.y = 0.707;
    markerPose_.orientation.z = 0.0;
    markerPose_.orientation.w = 0.707;
    
    // 初始化对比值
    compare.x = 0;
    compare.y = 0;
    compare.z = 0;
   
    ros::AsyncSpinner spinner(1);
    spinner.start();
 
    std::string reference_frame = "base_link";
    arm.setPoseReferenceFrame(reference_frame);
 
    arm.allowReplanning(true);
    arm.setGoalJointTolerance(0.001);
    arm.setGoalPositionTolerance(0.001);
    arm.setGoalOrientationTolerance(0.01);
    arm.setMaxAccelerationScalingFactor(0.2);
    arm.setMaxVelocityScalingFactor(0.2);
 
    geometry_msgs::Pose now_pose = arm.getCurrentPose(end_effector_link).pose;
    std::cout << "now Robot position: [x,y,z]: [" << now_pose.position.x << "," << now_pose.position.y << "," << now_pose.position.z << "]" << std::endl;
    std::cout << "now Robot orientation: [x,y,z,w]: [" << now_pose.orientation.x << "," << now_pose.orientation.y << "," << now_pose.orientation.z << "," << now_pose.orientation.w << "]" << std::endl;
 
    arm.setNamedTarget("init_pose"); // 控制机械臂先回到初始化位置
    arm.move();
    sleep(1);
 
    sub_ = n.subscribe("/aruco_single/pose", 1, &MoveToMarker::markerCb, this);
}
 
void MoveToMarker::markerCb(const geometry_msgs::PoseStamped& pose)
{
    // 检查marker位置是否发生变化(通过对比和阈值，避免噪声的干扰)
    // 通过rostopic echo /aruco_single/pose得知，模拟环境噪声变化绝大多数小于0.01
    // 真实情况需要再次评估噪声
    double threshold = 0.05;
    bool position_changed = 
        abs(compare.x - pose.pose.position.x) > threshold ||
        abs(compare.y - pose.pose.position.y) > threshold ||
        abs(compare.z - pose.pose.position.z) > threshold;
 
    if (position_changed) {
        // 位置发生了变化，更新markerPose_为当前机械臂末端的位置
        double pose_offset = 0.8;
 
        markerPose_.position.x +=  pose.pose.position.x;
        markerPose_.position.y +=  pose.pose.position.z - pose_offset;
        markerPose_.position.z += -pose.pose.position.y;
 
        // 更新对比数值
        compare.x = pose.pose.position.x;
        compare.y = pose.pose.position.y;
        compare.z = pose.pose.position.z;
 
        ifMove = true; // 设置标志，位置已变化，需要移动
    }
 
    else{
        ifMove = false; // 设置标志，位置未变化，不需要移动
    }
}
 
void MoveToMarker::moveOnce()
{
    ros::AsyncSpinner spinner(1); // 多线程
    spinner.start(); // 开启新的线程
 
    // 直接输出目标位置和姿态
    std::cout << "target position: [x,y,z]: [" << markerPose_.position.x << "," << markerPose_.position.y << "," << markerPose_.position.z << "]" << std::endl;
    std::cout << "target orientation: [x,y,z,w]: [" << markerPose_.orientation.x << "," << markerPose_.orientation.y << "," << markerPose_.orientation.z << "," << markerPose_.orientation.w << "]" << std::endl;
 
    // 设置机械臂的目标位置和姿态
    arm.setPoseTarget(markerPose_);
 
    // 规划路径
    moveit::planning_interface::MoveGroupInterface::Plan plan;
    if (arm.plan(plan)) {
        // 输出执行前机械臂的目标位置
        std::cout << "Moving arm to target position: [x,y,z]: ["
                  << markerPose_.position.x << ", "
                  << markerPose_.position.y << ", "
                  << markerPose_.position.z << "]" << std::endl;
 
        if (markerPose_.position.z < 0.23) {
            ROS_WARN("The target position is too low: [x,y,z]: [%f, %f, %f]", markerPose_.position.x, markerPose_.position.y, markerPose_.position.z);
            // 这里可以根据需要添加其他逻辑，例如调整目标位置或取消移动
            // 如果目标位置太低，则不执行移动
            arm.setNamedTarget("init_pose"); // 控制机械臂先回到初始化位置
            arm.move();
            ROS_INFO("Moved back to initial position.");
            ifMove = false;
            return; // 退出函数，取消移动
        }
 
        ROS_INFO("Joint space planning succeeded.");
 
        // 执行规划的路径
        arm.execute(plan);
 
        // 检查机械臂是否到达目标位置
        geometry_msgs::Pose end_effector_pose = arm.getCurrentPose(arm.getEndEffectorLink()).pose;
        bool reached_target = std::abs(end_effector_pose.position.x - markerPose_.position.x) < 0.05 &&
                              std::abs(end_effector_pose.position.y - markerPose_.position.y) < 0.05 &&
                              std::abs(end_effector_pose.position.z - markerPose_.position.z) < 0.05;
 
        if (reached_target) {
            ROS_INFO("The arm has reached the target position.");
            ifMove = false; // 设置标志，已完成移动，不需要移动
        }
 
        else {
            // ! 这里会二次运行
            // 输出当前机械臂末端的位置
            std::cout << "Current end effector position: [x,y,z]: ["
                      << end_effector_pose.position.x << ", "
                      << end_effector_pose.position.y << ", "
                      << end_effector_pose.position.z << "]" << std::endl;
 
            markerPose_.position = end_effector_pose.position;
        }
    } 
 
    else{
        ROS_INFO("Joint space planning failed. Resetting target pose to current end effector position.");
        
        ifMove = false;
 
        // 获取当前机械臂末端的位置
        geometry_msgs::Pose current_pose = arm.getCurrentPose(arm.getEndEffectorLink()).pose;
 
        if (std::abs(markerPose_.position.x - current_pose.position.x) > 0.05 &&
            std::abs(markerPose_.position.y - current_pose.position.y) > 0.05 &&
            std::abs(markerPose_.position.z - current_pose.position.z) > 0.05) {
            markerPose_.position = current_pose.position;
        }
    }
}
 
int main(int argc, char **argv)
{
    ros::init(argc, argv, "move_with_marker_node");
    ros::NodeHandle n;
 
    MoveToMarker myMove(n);
    ros::WallDuration(2.0).sleep(); // 等待2秒
 
    while (ros::ok()) {
        ros::spinOnce();
        if (myMove.ifMove) {
            myMove.moveOnce();
            myMove.ifMove = false; // 重置标志，避免重复移动
        }
        ros::Duration(0.2).sleep();
    }
    return 0;
}

四、项目总结

4.1 致学习者

总的来说，这个项目可以作为一个关于ROS和机械臂的入门级项目，对机器人感兴趣或者想要判断未来是否要学习相关领域的同学可以尝试一下。

4.2 未来改进

添加防止碰撞地面功能。（已完成，末端过低，返回初始位置）
完善终端输出的反馈，改善使用体验。（已完成）
添加姿态跟踪的功能，全面实现机械臂跟踪的功能。

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/喵喵爱编程/article/detail/818751