moveit机械臂C++ server服务端【实现client发送位姿，server控制机械臂运动】【多轨迹连续运动】

最新推荐文章于 2024-08-22 08:57:57 发布

原创

最新推荐文章于 2024-08-22 08:57:57 发布 · 1k 阅读

10 ·

CC 4.0 BY-SA版权

文章标签：

#c++ #机器人

文章讲述了作者如何从PythonAPI切换到C++编写MoveIt服务端，以实现UR5e机械臂的连续轨迹控制，包括简易服务端测试、多轨迹控制以及客户端示例。

之前控制UR5e机械臂都是用MoveIt的Python API来实现的，但MoveIt有些功能仅在C++中实现，例如多轨迹连续运动，由于项目需要，于是开始探索编写C++的MoveIt服务端。

MovePoints.srv

geometry_msgs/Pose[] goal
---
geometry_msgs/Pose current_pose

简易服务端测试

在真正实现服务端代码前，首先编写一个简易的服务端文件，以验证该服务端能够控制机械臂，同时也来说明一下我在这里卡了几天的bug。

#include "ros/ros.h"
#include "robot_control/MovePoints.h"
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit/move_group_interface/move_group_interface.h>
 
#include <moveit_msgs/DisplayRobotState.h> 
#include <moveit_msgs/DisplayTrajectory.h> 
#include <moveit_msgs/AttachedCollisionObject.h> 
#include <moveit_msgs/CollisionObject.h> 

#include <geometry_msgs/Pose.h>
#include <robot_control/MovePoints.h>

ros::MultiThreadedSpinner spinner(2);

bool movePointsCallback(robot_control::MovePoints::Request& req,
                        robot_control::MovePoints::Response& res)
{
   
   
  moveit::planning_interface::MoveGroupInterface group("arm");//ur5对应moveit中选择的规划部分
  group.setGoalJointTolerance(0.001);
  group.setGoalPositionTolerance(0.001);
  group.setGoalOrientationTolerance(0.01);
  //设置允许的最大速度和加速度
  group.setMaxAccelerationScalingFactor(1);
  group.setMaxVelocityScalingFactor(1);
  group.setPoseReferenceFrame("base");
  group.setPlanningTime(5);
  group.allowReplanning(true);
  group.setPlannerId("RRTConnect");
  
  double targetPose[6] = {
   
   -0.12174417293871631+25/180*M_PI, -1.548835405419073, 1.0568126924397783, -2.693364369465602, -2.956528061980836, -1.6631575702179635};
  //关节的向量，赋值
  std::vector<double> joint_group_positions(6);
  joint_group_positions[0] = targetPose[0];
  joint_group_positions[1] = targetPose[1];
  joint_group_positions[2] = targetPose[2];
  joint_group_positions[3] = targetPose[3];
  joint_group_positions[4] = targetPose[4];
  joint_group_positions[5] = targetPose[5];
  
  //将关节值写入
  group.setJointValueTarget(joint_group_positions);
  group.move(); //规划+移动
  sleep(1);

  // 设置发送的数据，对应于moveit中的拖拽
  geometry_msgs::Pose target_pose1;
  
  target_pose1.orientation.x= 1.58480958e-17;
  target_pose1.orientation.y = 9.65925826e-01;
  target_pose1.orientation.z = -2.58819045e-01;
  target_pose1.orientation.w = 5.91458986e-17;
 
  target_pose1.position.x = -0.5434919310337578;
  target_pose1.position.y =  -0.06372072557135472;
  target_pose1.position