600字范文 > 机械臂C语言编程 ROS下C++控制UR机械臂

机械臂C语言编程 ROS下C++控制UR机械臂

时间：2023-05-14 17:25:05

描述

ROS系统下，使用moveit和gazebo搭建UR机械臂控制的仿真环境，并使用C++编写一个节点来控制UR机械臂的移动

ROS系统：kinetic

UR5机械臂

电脑系统：Ubuntu16.04

截图和运行效果

gazebo的机械臂会先水平，然后依次移动到两个位置

rviz中的机械臂移动和gazebo机械臂一致，但是会有一个透明效果的机械臂，沿着同样路径非常滞后的移动

代码

1. 特殊位置移动(Moveit中默认的位置)

#include

int main(int argc, char **argv)

{

ros::init(argc, argv, "demo"); //初始化

ros::AsyncSpinner spinner(1); //多线程

spinner.start(); //开启新的线程

moveit::planning_interface::MoveGroupInterface arm("manipulator");//初始化需要使用move group控制的机械臂中的arm group

arm.setGoalJointTolerance(0.001); //允许误差

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

arm.setMaxVelocityScalingFactor(0.2);

arm.setNamedTarget("home"); // 控制机械臂移动到水平(躺下)

arm.move();

sleep(1);

arm.setNamedTarget("up"); //控制机械臂到达竖直位置(竖立)

arm.move();

sleep(1);

ros::shutdown();

return 0;

}

2. 指定位置移动(欧氏空间的位置和四元数)

#include

int main(int argc, char **argv)

{

ros::init(argc, argv, "demo");

ros::AsyncSpinner spinner(1);

spinner.start();

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

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

std::cout<

std::string reference_frame = "/world"; //设置目标位置所使用的参考坐标系

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<

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

arm.move();

sleep(1);

std::vector<:pose> waypoints;

geometry_msgs::Pose pose1;

pose1.position.x = 0.712759;

pose1.position.y = 0.2;

pose1.position.z = 1.16729;

pose1.orientation.x = 0.70717;

pose1.orientation.y = 0.707044;

pose1.orientation.z = 1.42317e-05;

pose1.orientation.w = 7.09771e-05;

waypoints.push_back(pose1);

geometry_msgs::Pose pose2;

pose2.position.x = 0.250989;

pose2.position.y = 0.578917;

pose2.position.z = 1.3212;

pose2.orientation.x = 0.707055;

pose2.orientation.y = 0.707159;

pose2.orientation.z = 4.39101e-05;

pose2.orientation.w = 8.60218e-05;

waypoints.push_back(pose2);

// 笛卡尔空间下的路径规划

moveit_msgs::RobotTrajectory trajectory;

const double jump_threshold = 0.0;

const double eef_step = 0.002;

double fraction = 0.0;

int maxtries = 100; //最大尝试规划次数

int attempts = 0; //已经尝试规划次数

while(fraction < 1.0 && attempts < maxtries)

{

fraction = puteCartesianPath(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);

}

ros::shutdown();

return 0;

}

3. CMakeLists.txt

cmake_minimum_required(VERSION 3.0.2)

project(UR5_control)

add_compile_options(-std=c++11)

find_package(catkin REQUIRED COMPONENTS

roscpp

rospy

std_msgs

moveit_ros_planning_interface

)

find_package(Boost REQUIRED COMPONENTS system)

include_directories(

${catkin_INCLUDE_DIRS}

${Boost_INCLUDE_DIRS}

)

add_executable(UR5_control_node src/UR5_control.cpp)

target_link_libraries(UR5_control_node ${catkin_LIBRARIES})

代码中你需要更改的地方

位姿点geometry_msgs::Pose pose

你可以在rviz界面，使用interactiveMarkers来拖动UR机械臂，可以把它拖动到任何一个位置，然后点击按钮Plan and Execute。这时rviz和gazebo中的机械臂会发生实际移动。

这时你可以再次运行以上程序，程序会把启动时的位姿打印出来，你照抄在代码中即可一些你需要自己设置的参数以“”括起来的字符串们问题及解决方案

1. 路径规划失败

执行这句话fraction = puteCartesianPath(waypoints, eef_step, jump_threshold, trajectory);时，fraction返回的结果是0，代表机械臂路径规划失败，fraction返回的结果是1，代表机械臂路径规划成功

我一直返回的结果就是0，也就是路径规划失败。

排查了一下，最终确认是编译过程中缺少了一个依赖包

解决方案

CMakeLists.txt文件更改

在原基础上添加了一个包moveit_ros_planning_interface

find_package(catkin REQUIRED COMPONENTS

roscpp

rospy

std_msgs

moveit_ros_planning_interface

)

2. Gazebo中的机械臂不动

之前我的代码并不是以上贴出来的样子，比正确的代码多了以下这句错误的话

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

waypoint.push_back(now_pose) // 错误的话，不要添加

多了以上这句话出现的问题就是，在执行arm.execute(plan);这句话的时候，报了如下的错误

[ INFO] [1601195859.013775369, 913.641000000]: ABORTED: Solution found but controller failed during execution

实际表现是：

moveit里的机械臂动了，但是动起来是透明的效果，实体机械臂位置没有更新，gazebo里的机械臂不动。以下这张照片，中间那个机械臂就是我所说的透明的效果，图和文字没关系

问题排查过程

我一直以为，是rviz中的机械臂位置问题，如图

最开始执行代码的时候，机械臂的位置一直是"home"位置，在rviz上可以看到，机械臂的末端是处在水平面位置的，也就是可能存在和水平面的碰撞。执行代码时，rviz机械臂会以透明的效果肉眼可见的范围动一下，gazebo机械臂不动。这让我觉得是rviz认为与地面存在碰撞，而运动失败。

因此我进入了ur5_robot.urdf.xacro文件(请你前往你对应的机械臂描述文件)，更改了机械臂的起始位姿。示例

改为了，这样改变的效果是，机械臂在z方向上上升了1m，也就悬在了空中，这里不做截图展示。

问题没有得到解决。

解决方案

根据我的另外一篇文章ROS下查看rviz中的机械臂位姿，我查阅了机械臂的位姿，突然发现查阅位姿这个命令的终端使用黄色字体提示了这样一句话

[ WARN] [1601210755.347884455, 0.800000000]: goalConnectCallback: Trying to add [/move_group] to goalSubscribers, but it is already in the goalSubscribers list

[ WARN] [1601210755.348000765, 0.800000000]: cancelConnectCallback: Trying to add [/move_group] to cancelSubscribers, but it is already in the cancelSubscribers list

我开始怀疑是路径规划本身的问题。

专业的讲，使用moveit能够控制机械臂运动，没有任何道理使用代码不能让机械臂运动。因此虽然代码提示规划成功，运动失败，我仍然怀疑规划出的路径并不能让机械臂成功移动。

我做了一次大胆的尝试，删除了waypoints中的第一个点，也就是当前位置的push_back。这一次成功运动了，可以运行的整段代码就在上面

后记

是stackflow上都有人说，要在waypoint中加入当前位置，我的参考书中隐隐约约也是这样写的。我现在懒得去真正落实这个问题了，出现这次调试有三个可能

那些要在waypoint中加入当前位置的，是python接口，很多人直接照搬了

moveit代码版本升级的变化

很多人互相在抄，没有跑一跑就把代码搞出来了这一次成功运动了，可以运行的整段代码就在上面

本内容不代表本网观点和政治立场，如有侵犯你的权益请联系我们处理。

网友评论

网友评论仅供其表达个人看法，并不表明网站立场。