创建功能包

cd ~/ur_ws/src# 创建功能包 control_robot
catkin_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# https://www.guyuehome.com/22221import rospy
import moveit_commanderrospy.init_node('grasp_object')
arm = moveit_commander.MoveGroupCommander("manipulator")
# grp = moveit_commander.MoveGroupCommander("gripper")# 表示方式1
arm.set_named_target('up')
arm.go(wait=True)# 表示方式2
arm.set_joint_value_target([-0.21957805043352518, -1.097296859939564, 1.8945345194815335,-2.366067038969164, -1.571228181260084, -1.0061550793898952])
arm.go(wait=True)# 表示方式3
pose = arm.get_current_pose().pose
pose.position.z -= 0.05
arm.set_pose_target(pose)
arm.go(wait=True)# 设置手抓
# grp.set_named_target('close')
# grp.go(wait=True)pose = arm.get_current_pose().pose
pose.position.z += 0.05
arm.set_pose_target(pose)
arm.go(wait=True)

2.2 代码2（python）

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

#!/usr/bin/env python
# -*- coding: utf-8 -*-# https://blog.csdn.net/weixin_45839124/article/details/106801986import rospy, sys
import moveit_commander
from moveit_commander import MoveGroupCommander
from geometry_msgs.msg import Pose
from 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++）

// https://blog.csdn.net/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.00202662520084;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文件，然后把原文件的东西复制进去。
参考：https://blog.csdn.net/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.

参考：https://github.com/ros-planning/moveit/issues/1642
这个一个弃用警告，忽略即可

问题3 Waiting for server

guyue@guyue:~$ rosrun control_robot demo.py
Waiting 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.py
Waiting 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’