[Moveit!]控制真实机械臂问题合集（个人向）

Kusa127

已于 2024-03-14 15:55:29 修改

阅读量1.5k

点赞数 8

文章标签：笔记学习

于 2024-03-13 14:50:05 首次发布

本文链接：https://blog.csdn.net/qq_63299892/article/details/136672519

版权

项目场景：

环境：
ubuntu20.04
noetic
硬件：
AR4机械臂

问题描述分析解决

问题1：目标位姿规划无法求解（已解决）

问题描述

首先给出目标位姿规划的代码moveit_ik_demo如下：

#include <string>
#include <ros/ros.h>
#include <moveit/move_group_interface/move_group_interface.h>

int main(int argc, char **argv)
{
    ros::init(argc, argv, "moveit_fk_demo");
    ros::AsyncSpinner spinner(1);
    spinner.start();

    moveit::planning_interface::MoveGroupInterface arm("manipulator");

    //获取终端link的名称
    std::string end_effector_link = arm.getEndEffectorLink();

    //设置目标位置所使用的参考坐标系
    std::string reference_frame = "base_link";
    arm.setPoseReferenceFrame(reference_frame);

    //当运动规划失败后，允许重新规划
    arm.allowReplanning(true);

    //设置位置(单位：米)和姿态（单位：弧度）的允许误差
    arm.setGoalPositionTolerance(0.001);
    arm.setGoalOrientationTolerance(0.01);

    //设置允许的最大速度和加速度
    arm.setMaxAccelerationScalingFactor(0.2);
    arm.setMaxVelocityScalingFactor(0.2);

    // 控制机械臂先回到初始化位置
    arm.setNamedTarget("ready");
    arm.move();
    sleep(1);

    // 设置机器人终端的目标位置
    geometry_msgs::Pose target_pose;
    
    target_pose.orientation.x = 0.70692;
    target_pose.orientation.y = 0.0;
    target_pose.orientation.z = 0.0;    
    target_pose.orientation.w = 0.70729;

    target_pose.position.x = 0.3593;
    target_pose.position.y = 0.0636;
    target_pose.position.z = 0.1787;

    // 设置机器臂当前的状态作为运动初始状态
    arm.setStartStateToCurrentState();

    arm.setPoseTarget(target_pose);

    // 进行运动规划，计算机器人移动到目标的运动轨迹，此时只是计算出轨迹，并不会控制机械臂运动
    moveit::planning_interface::MoveGroupInterface::Plan plan;
    moveit::planning_interface::MoveItErrorCode success = arm.plan(plan);

    ROS_INFO("Plan (pose goal) %s",success?"":"FAILED");   

    //让机械臂按照规划的轨迹开始运动。
    if(success)
      arm.execute(plan);
    sleep(1);

    // 控制机械臂先回到初始化位置
    arm.setNamedTarget("rest");
    arm.move();
    sleep(1);

    ros::shutdown(); 

    return 0;

运行时报错：
在这里插入图片描述

这里的轨迹规划超时了。

问题解决

通过在网上查询相关的问题，得知提高允许的误差就能够成功规划
尝试将代码

    //设置位置(单位：米)和姿态（单位：弧度）的允许误差
    arm.setGoalPositionTolerance(0.001);
    arm.setGoalOrientationTolerance(0.01);

改为

    //设置位置(单位：米)和姿态（单位：弧度）的允许误差
    arm.setGoalPositionTolerance(0.5);
    arm.setGoalOrientationTolerance(0.5);

机械臂成功运行了
但是，经过多次的试验，每次规划出来后的目标位姿相差很大，所以这种方法是不可行的
所以把角度转向了是否是求解器出了问题。我们吸纳把允许误差改回原来的数值
把原本的kdl求解器换成trac_ik求解器
问题解决运行成功
在这里插入图片描述

问题2 执行完目标位姿规划后，再运行笛卡尔坐标系运动时，机械臂无法运行（未解决）

问题描述：

运行笛卡尔空间路径规划例程出现报错
运行程序moveit_circle_demo.cpp如下：

#include <math.h>
#include <ros/ros.h>
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/robot_trajectory/robot_trajectory.h>

int main(int argc, char **argv)
{
    ros::init(argc, argv, "moveit_cartesian_demo");
    ros::AsyncSpinner spinner(1);
    spinner.start();

    moveit::planning_interface::MoveGroupInterface arm("manipulator");

    //获取终端link的名称
    std::string end_effector_link = arm.getEndEffectorLink();

    //设置目标位置所使用的参考坐标系
    std::string reference_frame = "base_link";
    arm.setPoseReferenceFrame(reference_frame);

    //当运动规划失败后，允许重新规划
    arm.allowReplanning(true);

    //设置位置(单位：米)和姿态（单位：弧度）的允许误差
    arm.setGoalPositionTolerance(0.001);
    arm.setGoalOrientationTolerance(0.01);

    //设置允许的最大速度和加速度
    arm.setMaxAccelerationScalingFactor(0.2);
    arm.setMaxVelocityScalingFactor(0.2);

    // 控制机械臂先回到初始化位置
    arm.setNamedTarget("ready");
    arm.move();
    sleep(1);

    // 设置机器人终端的目标位置
    geometry_msgs::Pose target_pose;
    target_pose.orientation.x = -0.482974;
    target_pose.orientation.y = 0.517043;
    target_pose.orientation.z = -0.504953;
    target_pose.orientation.w = -0.494393;

    target_pose.position.x = 0.0;
    target_pose.position.y = -0.501958;
    target_pose.position.z = 0.307887;

    arm.setPoseTarget(target_pose);
    arm.move();

    	std::vector<geometry_msgs::Pose> waypoints;

    //将初始位姿加入路点列表
    //waypoints.push_back(target_pose);

    double centerA = target_pose.position.x;
    double centerB = target_pose.position.z;
    double radius = 0.1;

    for(double th=0.0; th<6.28; th=th+0.01)
    {
        target_pose.position.x = centerA + radius * cos(th);
        target_pose.position.z = centerB + radius * sin(th);
        waypoints.push_back(target_pose);
    }

    // 笛卡尔空间下的路径规划
    moveit_msgs::RobotTrajectory trajectory;
    const double jump_threshold = 0.0;
    const double eef_step = 0.01;
    double fraction = 0.0;
    int maxtries = 100;   //最大尝试规划次数
    int attempts = 0;     //已经尝试规划次数

    while(fraction < 1.0 && attempts < maxtries)
    {
        fraction = arm.computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory);
        attempts++;
        
        if(attempts % 10 == 0)
            ROS_INFO("Still trying after %d attempts...", attempts);
    }
    
    if(fraction == 1)
    {   
        ROS_INFO("Path computed successfully. Moving the arm.");

        // 生成机械臂的运动规划数据
        moveit::planning_interface::MoveGroupInterface::Plan plan;
        plan.trajectory_ = trajectory;

        // 执行运动
        arm.execute(plan);
        sleep(1);
    }
    else
    {
        ROS_INFO("Path planning failed with only %0.6f success after %d attempts.", fraction, maxtries);
    }

    // 控制机械臂先回到初始化位置
    arm.setNamedTarget("ready");
    arm.move();
    sleep(1);

    ros::shutdown(); 
    return 0;
}

程序只能运行到第一段的目标点，无法完成后续的circle轨迹规划

问题分析：

去掉执行目标位姿，直接执行第二段轨迹规划时，出现的问题与问题1相同。当我再次输入规划路径点，并将初始位姿加入路径列表后，虽然轨迹规划能成功，但是真实机械臂无法运行。具体终端显示如下："

在这里插入图片描述

此时rviz中能显示出轨迹，但机械臂无法执行
在这里插入图片描述
轨迹规划代码如下：

    geometry_msgs::Pose target_pose = arm.getCurrentPose(end_effector_link).pose;

    std::vector<geometry_msgs::Pose> waypoints;

    //将初始位姿加入路点列表
    waypoints.push_back(target_pose);

    double centerA = target_pose.position.x;
    double centerB = target_pose.position.z;
    double radius = 0.1;

    for(double th=0.0; th<6.28; th=th+0.01)
    {
        target_pose.position.x = centerA + radius * cos(th);
        target_pose.position.z = centerB + radius * sin(th);
        waypoints.push_back(target_pose);
    }
  
}

所以与问题1相同操作，我注释掉了waypoint.push_back(target_pose)这一句，但不知道为什么要这样做。

    // 设置机器人终端的目标位置
    geometry_msgs::Postarget_pose;
    target_pose.orientation.x = -0.482974;
    target_pose.orientation.y = 0.517043;
    target_pose.orientation.z = -0.504953;
    target_pose.orientation.w = -0.494393;

    target_pose.position.x = 0.0;
    target_pose.position.y = -0.501958;
    target_pose.position.z = 0.307887;

    arm.setPoseTarget(target_pose);
    arm.move();

    	std::vector<geometry_msgs::Pose> waypoints;

    //将初始位姿加入路点列表
    //waypoints.push_back(target_pose);

    double centerA = target_pose.position.x;
    double centerB = target_pose.position.z;
    double radius = 0.1;

    for(double th=0.0; th<6.28; th=th+0.01)
    {
        target_pose.position.x = centerA + radius * cos(th);
        target_pose.position.z = centerB + radius * sin(th);
        waypoints.push_back(target_pose);
    }

这时机械臂运动是成功的。
很好，通过试验我得知了，但我执行笛卡尔空间目标点运动和笛卡尔空间轨迹规划这两个动作是可以单独独立成功运行的（在初始的位姿“ready”下）。通过观察一开始的程序运行的过程中，发现当执行完笛卡尔空间目标点运动的时候，轨迹规划有向circle运动的趋势，这与rviz终端中显示的info结果对应，info中显示只解算出了一部分的点，那么我产生了两个疑问：

我在新的点上没法规划，如果我换成其他的点呢？亦或者试试从其他地方移动到可以成功执行circle的"ready"点上？
解算不出来是否是因为trac_ik求解器有问题，试着换成ik_fast试试？(提前加入初始化的waypoint是不是也能解决？)
第一问由于在轨迹规划之前加入了初始化的start_point导致程序无法运行，在这一次的问题当中我第一段最后提供的就是一个point加入了后续的waypoint路径点，那我把前面的点设置为start_point，后面加入waypoint的点设置为target_point，是否就能避免错误？

疑问1：在疑问三中尝试印证
疑问2：在疑问三中尝试印证
疑问3：
我创建了两个点一个是start_point用于运动到目标位姿，第二个点是target_point用于笛卡尔空间中规划circle
具体的修改的代码如下：

    // 设置机器人终端的目标位置
    geometry_msgs::Pose start_pose;
    start_pose.orientation.x = -0.482974;
    start_pose.orientation.y = 0.517043;
    start_pose.orientation.z = -0.504953;
    start_pose.orientation.w = -0.494393;

    start_pose.position.x = 0.0;
    start_pose.position.y = -0.451958;
    start_pose.position.z = 0.257887;

    arm.setPoseTarget(start_pose);
    arm.move();
    
    
    geometry_msgs::Pose target_pose = arm.getCurrentPose(end_effector_link).pose;
    	std::vector<geometry_msgs::Pose> waypoints;

    //将初始位姿加入路点列表
    //waypoints.push_back(target_pose);

    double centerA = target_pose.position.x;
    double centerB = target_pose.position.z;
    double radius = 0.1;

    for(double th=0.0; th<6.28; th=th+0.01)
    {
        target_pose.position.x = centerA + radius * cos(th);
        target_pose.position.z = centerB + radius * sin(th);
        waypoints.push_back(target_pose);
    }

运行成功了一半，机械臂运动到目标点之后，可以执行circle轨迹，但是circle轨迹运行到了一半，在rviz中出现了一个完美的圆，但是运行到了一半，机械臂突然不跟着轨迹走了，跳过了后半程。合理推测可能是印证了疑问1或疑问2。我更换了第一段的目标点，在其他目标点上，轨迹运行了一半多，估计轨迹规划圆的时候经过了机械臂的奇异点？rviz中显示的轨迹只是通过函数预画出来的，实际机械臂执行不出来？