ROS moveit运动规划

使用moveit中的c++接口进行运动规划

拟解决的问题是已知末端的几个关键位姿，规划一条路径，使得机械臂能够从静止开始经过每个路点直到停止。使用的关键函数是computeCartesianPath
该函数说明如下

double moveit::planning_interface::MoveGroup::computeCartesianPath(	
    const std::vector< geometry_msgs::Pose > & 	waypoints, //路径点
    double  eef_step,//末端最大步距  
    double  jump_threshold,//机械臂最小移动距离，设置0.0则忽略检查
    moveit_msgs::RobotTrajectory &  trajectory,//计算返回路径
    const moveit_msgs::Constraints &  path_constraints,//路径约束
    bool  avoid_collisions = true,//是否检测碰撞
    moveit_msgs::MoveItErrorCodes *  error_code = NULL //返回错误代码
)	

使用该函数的完整代码保存为ur_waypoints_route_plan.cpp,放在工作空间的universal_robots/ur_modern_dirver/src下

#include<moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
int main(int argc, char **argv)
{
	//初始化节点
	ros::init(argc, argv, "ur_waypoints_route_plan", ros::init_options::AnonymousName);
	// 创建一个异步的自旋线程（spinning thread）
	ros::AsyncSpinner spinner(1);
	spinner.start();
	//创建规划组
	moveit::planning_interface::MoveGroupInterface move_group("manipulator");

	// 设置第一个目标位置（路点）
	geometry_msgs::Pose target_pose_1;
	double pose_1[7]={0.51513, 0.5346, 0.42126, 0.52094,0.23946, 0.3401, 0.25042};
	target_pose_1.orientation.x=pose_1[0];
	target_pose_1.orientation.y=pose_1[1];
	target_pose_1.orientation.z=pose_1[2];
	target_pose_1.orientation.w=pose_1[3];
	target_pose_1.position.x = pose_1[4]; 
	target_pose_1.position.y = pose_1[5];
	target_pose_1.position.z = pose_1[6];
	// 设置第二个目标位置（路点）
	double pose_2[7]={0.51546, 0.53436, 0.42154, 0.52064, 0.036189, 0.37489, 0.27121};
	geometry_msgs::Pose target_pose_2;
	target_pose_2.orientation.x=pose_2[0];
	target_pose_2.orientation.y=pose_2[1];
	target_pose_2.orientation.z=pose_2[2];
	target_pose_2.orientation.w=pose_2[3];
	target_pose_2.position.x = pose_2[4]; 
	target_pose_2.position.y = pose_2[5];
	target_pose_2.position.z = pose_2[6];
	// 设置第三个目标位置（路点）
	double pose_3[7]={0.495, 0.56875, 0.51969, 0.40177, -0.072637, 0.59717, 0.24593};
	geometry_msgs::Pose target_pose_3;
	target_pose_3.orientation.x=pose_3[0];
	target_pose_3.orientation.y=pose_3[1];
	target_pose_3.orientation.z=pose_3[2];
	target_pose_3.orientation.w=pose_3[3];
	target_pose_3.position.x = pose_3[4]; 
	target_pose_3.position.y = pose_3[5];
	target_pose_3.position.z = pose_3[6];
	//把目标位置加入路点
	std::vector<geometry_msgs::Pose> waypoints;
	waypoints.push_back(target_pose_1);
	waypoints.push_back(target_pose_2);
	waypoints.push_back(target_pose_3);
	// 笛卡尔空间下的路径规划
	moveit_msgs::RobotTrajectory trajectory;
	const double jump_threshold = 0.0;
	const double eef_step = 0.01;//末端最大步距1cm
	double fraction = move_group.computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory);
	//规划的路径经过的路点成功率
	 ROS_INFO("Visualizing plan (cartesian path) (%.2f%% acheived)",fraction * 100.0);
	// 生成机械臂的运动规划数据
	moveit::planning_interface::MoveGroupInterface::Plan plan;
	plan.trajectory_ = trajectory;

	// 执行运动
	move_group.execute(plan);

	ros::shutdown(); 
	return 0;

}

在CMakeLists.txt文件中添加以下内容

find_package(catkin REQUIRED COMPONENTS
  hardware_interface
  controller_manager
  actionlib
  control_msgs
  geometry_msgs
  roscpp
  sensor_msgs
  std_msgs
  trajectory_msgs
  ur_msgs
  tf
######################新添加
  rospy
  moveit_msgs
  moveit_ros_perception
  moveit_ros_planning_interface
######################结束
)

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

install(TARGETS ur_driver ur_hardware_interface ur_waypoints_route_plan ##这里新加一个ur_waypoints_route_plan
  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

编译通过后，先运行roslaunch ur5_moveit_config demo.launch，然后运行rosrun ur_modern_driver ur_waypoints_route_plan可以看到moveit规划机械臂的路径并运行，仿真运行的视频链接为百度网盘,密码为7me6