失眠网 > UR机械臂学习（7-2）：MoveIt简单编程实现机械臂运动——一些参考代码和遇到的问题

UR机械臂学习（7-2）：MoveIt简单编程实现机械臂运动——一些参考代码和遇到的问题

时间：2022-09-25 22:51:18

创建功能包

cd ~/ur_ws/src# 创建功能包 control_robotcatkin_create_pkg control_robot std_msgs rospy roscpproscd control_robot# 新建scripts文件夹（用来放置python程序）mkdir scripts# 新建.py文件touch demo.py# 将.py文件变为可执行文件sudo chmod +x talker.py

注意：

python的程序执行前，要先设置为可执行文件

sudo chmod +x [文件名.py]

更重要的一点，在.py文件的第一行要有：

#!/usr/bin/env python

02 一些参考代码

2.1 代码1（python）

这个是可以正常使用

#!/usr/bin/env python# /22221import rospyimport moveit_commanderrospy.init_node('grasp_object')arm = moveit_commander.MoveGroupCommander("manipulator")# grp = moveit_commander.MoveGroupCommander("gripper")# 表示方式1arm.set_named_target('up')arm.go(wait=True)# 表示方式2arm.set_joint_value_target([-0.21957805043352518, -1.097296859939564, 1.8945345194815335,-2.366067038969164, -1.571228181260084, -1.0061550793898952])arm.go(wait=True)# 表示方式3pose = arm.get_current_pose().posepose.position.z -= 0.05arm.set_pose_target(pose)arm.go(wait=True)# 设置手抓# grp.set_named_target('close')# grp.go(wait=True)pose = arm.get_current_pose().posepose.position.z += 0.05arm.set_pose_target(pose)arm.go(wait=True)

2.2 代码2（python）

原代码rviz在不停的动，但是停止后不动，gazebo不动，说运动规划失败，所以做了修改

#!/usr/bin/env python# -*- coding: utf-8 -*-# /weixin_45839124/article/details/106801986import rospy, sysimport moveit_commanderfrom moveit_commander import MoveGroupCommanderfrom geometry_msgs.msg import Posefrom copy import deepcopyclass MoveItCartesianDemo:def __init__(self):# 初始化move_group的APImoveit_commander.roscpp_initialize(sys.argv)# 初始化ROS节点rospy.init_node('moveit_cartesian_demo', anonymous=True)# 是否需要使用笛卡尔空间的运动规划，获取参数，如果没有设置，则默认为True，即走直线cartesian = rospy.get_param('~cartesian', True)# 初始化需要使用move group控制的机械臂中的arm grouparm = MoveGroupCommander('manipulator')# 当运动规划失败后，允许重新规划arm.allow_replanning(True)# 设置目标位置所使用的参考坐标系arm.set_pose_reference_frame('base_link')# 设置位置(单位：米)和姿态（单位：弧度）的允许误差arm.set_goal_position_tolerance(0.001)arm.set_goal_orientation_tolerance(0.001)# 设置允许的最大速度和加速度arm.set_max_acceleration_scaling_factor(0.5)arm.set_max_velocity_scaling_factor(0.5)# 获取终端link的名称end_effector_link = arm.get_end_effector_link()# 控制机械臂先回到初始化位置arm.set_named_target('home')arm.go()rospy.sleep(1)# 获取当前位姿数据为机械臂运动的起始位姿start_pose = arm.get_current_pose(end_effector_link).pose# 设置机器臂当前的状态作为运动初始状态arm.set_start_state_to_current_state()# 控制机械臂先回到预设位置arm.set_named_target('up')arm.go()rospy.sleep(1)# 关闭并退出moveitmoveit_commander.roscpp_shutdown()moveit_commander.os._exit(0)if __name__ == "__main__":try:MoveItCartesianDemo()except rospy.ROSInterruptException:pass

2.3 代码3（c++）

// /weixin_39312052/article/details/88130730#include <moveit/move_group_interface/move_group_interface.h>#include <moveit/planning_scene_interface/planning_scene_interface.h>#include <moveit_msgs/DisplayRobotState.h> #include <moveit_msgs/DisplayTrajectory.h> #include <moveit_msgs/AttachedCollisionObject.h> #include <moveit_msgs/CollisionObject.h> int main(int argc, char **argv){ros::init(argc, argv, "movo_moveit");ros::NodeHandle node_handle; ros::AsyncSpinner spinner(1);spinner.start();moveit::planning_interface::MoveGroupInterface group("manipulator");//ur3对应moveit中选择的规划部分// 设置发送的数据，对应于moveit中的拖拽geometry_msgs::Pose target_pose1;target_pose1.orientation.x= 0.00196463;target_pose1.orientation.y = 0.7072;target_pose1.orientation.z = 0.707008;target_pose1.orientation.w = -0.00225032;target_pose1.position.x = 0.428326;target_pose1.position.y = 0.257828;target_pose1.position.z = 0.307195;group.setPoseTarget(target_pose1);group.setMaxVelocityScalingFactor(0.02);// 进行运动规划，计算机器人移动到目标的运动轨迹，对应moveit中的plan按钮moveit::planning_interface::MoveGroupInterface::Plan my_plan;bool success = group.plan(my_plan)== moveit::planning_interface::MoveItErrorCode::SUCCESS;;ROS_INFO("Visualizing plan 1 (pose goal) %s",success?"":"FAILED"); //让机械臂按照规划的轨迹开始运动，对应moveit中的execute。if(success)group.execute(my_plan);///第二个位置geometry_msgs::Pose target_pose2;target_pose2.orientation.x= -0.0020266254;target_pose2.orientation.y = -0.706992052889;target_pose2.orientation.z = -0.707215193629;target_pose2.orientation.w = 0.00219085420338;target_pose2.position.x =0.389998894713;target_pose2.position.y =0.31251544014;target_pose2.position.z = 0.361921853036;group.setPoseTarget(target_pose2);group.setMaxVelocityScalingFactor(0.02);// moveit::planning_interface::MoveGroupInterface::Plan my_plan;// 进行运动规划，计算机器人移动到目标的运动轨迹，对应moveit中的plan按钮moveit::planning_interface::MoveGroupInterface::Plan my_plan_1;//bool success = (ptr_group->plan(my_plan_1) == moveit::planning_interface::MoveItErrorCode::SUCCESS;bool success1 = group.plan(my_plan_1)== moveit::planning_interface::MoveItErrorCode::SUCCESS;;ROS_INFO("Visualizing plan 1 (pose goal) %s",success?"":"FAILED"); //让机械臂按照规划的轨迹开始运动，对应moveit中的execute。if(success1)group.execute(my_plan_1);// 第三个位置geometry_msgs::Pose target_pose3;target_pose3.orientation.x= 0.00196463;target_pose3.orientation.y = 0.7072;target_pose3.orientation.z = 0.707008;target_pose3.orientation.w = -0.00225032;target_pose3.position.x = 0.428326;target_pose3.position.y = 0.257828;target_pose3.position.z = 0.307195;group.setPoseTarget(target_pose3);group.setMaxVelocityScalingFactor(0.01);// 进行运动规划，计算机器人移动到目标的运动轨迹，对应moveit中的plan按钮moveit::planning_interface::MoveGroupInterface::Plan my_plan_2;//bool success = (ptr_group->plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS;bool success2 = group.plan(my_plan_2)== moveit::planning_interface::MoveItErrorCode::SUCCESS;;ROS_INFO("Visualizing plan 1 (pose goal) %s",success?"":"FAILED"); //让机械臂按照规划的轨迹开始运动，对应moveit中的execute。if(success2)group.execute(my_plan_2);ros::shutdown(); return 0;}

每次修改后，都要catkin_make

运行

rosrun control_robot demo_node

03 报错及解决

问题1 /usr/bin/env: “python\r”: 没有那个文件或目录

guyue@guyue:~$ rosrun control_robot demo1.py/usr/bin/env: "python\r": 没有那个文件或目录

解决：

这可能是因为这是在windows下创建的txt文件，然后另存为的.py文件。

可以像下面的链接一样修改，也可以touch新建一个.py文件，然后把原文件的东西复制进去。

参考：/qq_24894159/article/details/83957605

问题2 Kinematics solver doesn’t support

启动rviz的时候报错：

[ WARN] [1626783917.175885562, 69.111000000]: Kinematics solver doesn't support #attempts anymore, but only a timeout.Please remove the parameter '/robot_description_kinematics/manipulator/kinematics_solver_attempts' from your configuration.

参考：/ros-planning/moveit/issues/1642

这个一个弃用警告，忽略即可

问题3 Waiting for server

guyue@guyue:~$ rosrun control_robot demo.pyWaiting for server at /scaled_pos_joint_traj_controller/follow_joint_trajectory[FATAL] [1626788950.130852, 875.302000]: Did not find trajectory server. Exiting.guyue@guyue:~$ rosrun control_robot demo.pyWaiting for server at /pos_joint_traj_controller/follow_joint_trajectory[FATAL] [1626789377.320829, 1301.956000]: Did not find trajectory server. Exiting.

问题4 The dependency target does not exist.

CMake Error at control_robot/CMakeLists.txt:155 (add_dependencies):The dependency target "control_robot_generate_messages_cpp" of target"movo_moveit" does not exist.

解决

修改CMakeList.txt，加入下面这些：

add_executable(demo_node src/demo.cpp)target_link_libraries(demo_node ${catkin_LIBRARIES})

问题5 moveit/move_group_interface/move_group.h: 没有那个文件或目录

在运行c++程序时报错

fatal error: moveit/move_group_interface/move_group.h: 没有那个文件或目录#include <moveit/move_group_interface/move_group.h>

解决

CmakeList.txt里：

find_package(catkin REQUIRED COMPONENTSroscpprospystd_msgsmoveit_ros_planning_interfacemoveit_ros_move_group)

cpp文件里

// #include <moveit/move_group_interface/move_group.h> 改为#include <moveit/move_group_interface/move_group_interface.h>

问题6 ‘MoveGroup’ is not a member of ‘moveit::planning_interface’

在运行c++程序时报错

error: ‘MoveGroup’ is not a member of ‘moveit::planning_interface’

解决

// moveit::planning_interface::MoveGroup group("manipulator");//ur5对应moveit中选择的规划部分// 改为moveit::planning_interface::MoveGroupInterface group("manipulator");//ur5对应moveit中选择的规划部分

上面这么多错误，就是搜索，然后把所有的MoveGroup改为MoveGroupInterface