Moveit！编程---magician机械臂

Moveit！编程接口（这里引用了古月老师讲的）

MoveIt！主要给用户提供了两种编程接口，一种Python的接口，另一种是C++的接口，这两种接口的使用非常类似，API基本上保持一致。编写流程：

主要代码：

首先，我们需要创建功能包：

catkin_creat_pkg magician_demo moveit_msgs moveit_ros_perception moveit_ros_planning_interface roscpp rospy trajectory_msgs magician_hardware dobot

正向运动学：

C++

#include <ros/ros.h>
#include <moveit/move_group_interface/move_group_interface.h>
#include <iostream>
 
int main(int argc, char** argv)
{
    setlocale(LC_ALL,"");
	//初始化ROS节点
	ros::init(argc, argv, "magician_moveit_fk_demo");
 

	ros::AsyncSpinner spinner(1);
	spinner.start();
 
	//初始化需要使用move_group控制的机械臂中的arm_group
	moveit::planning_interface::MoveGroupInterface arm_magician("magician_arm");
 
	//设置机械臂运动的允许误差值(单位：弧度)
	arm_magician.setGoalJointTolerance(0.001);
	
	//设置允许的最大速度和加速度
	arm_magician.setMaxAccelerationScalingFactor(0.2);
	arm_magician.setMaxVelocityScalingFactor(0.2);
 
	//控制机械臂先回到初始化位置
	arm_magician.setNamedTarget("home");
	arm_magician.move();
    ROS_INFO("机械臂回到了初始位姿");
	sleep(1);
 
	//设置机械臂的目标位置，使用三轴的位置数据进行描述（单位：弧度）
	double targePose[3] = {0.52, 0.37, 0.47};
	std::vector<double> joint_group_position(3);
	joint_group_position[0] = targePose[0];
	joint_group_position[1] = targePose[1];
	joint_group_position[2] = targePose[2];

 
	//设置完成之后，控制机械臂完成运动
	arm_magician.setJointValueTarget(joint_group_position);
	arm_magician.move();
	sleep(1);
    ROS_INFO("机械臂进行正向运动学规划");
 
	//控制机械臂先回到初始化位置
	arm_magician.setNamedTarget("home");
	arm_magician.move();
    ROS_INFO("机械臂回到了初始位姿");
	sleep(1);
 
	//关闭并退出moveit
	ros::shutdown();
	
	return 0;
}

Python

import rospy, sys
import moveit_commander

class MoveItFkDemo:
    def __init__(self):
        # 初始化move_group的API
        moveit_commander.roscpp_initialize(sys.argv)

        # 初始化ROS节点
        rospy.init_node('moveit_fk_demo', anonymous=True)
 
        # 初始化需要使用move group控制的机械臂中的arm group
        arm = moveit_commander.MoveGroupCommander('magician_arm')
        
        # 设置机械臂运动的允许误差值
        arm.set_goal_joint_tolerance(0.001)

        # 设置允许的最大速度和加速度
        arm.set_max_acceleration_scaling_factor(0.5)
        arm.set_max_velocity_scaling_factor(0.5)
        
        # 控制机械臂先回到初始化位置
        arm.set_named_target('home')
        arm.go()
        rospy.sleep(1)
         
        # 设置机械臂的目标位置，使用六轴的位置数据进行描述（单位：弧度）
        joint_positions = [0.0, 1.052834, 0.454125]
        arm.set_joint_value_target(joint_positions)
                 
        # 控制机械臂完成运动
        arm.go()
        rospy.sleep(1)

        # 控制机械臂先回到初始化位置
        arm.set_named_target('home')
        arm.go()
        rospy.sleep(1)
        
        # 关闭并退出moveit
        moveit_commander.roscpp_shutdown()
        moveit_commander.os._exit(0)

if __name__ == "__main__":
    try:
        MoveItFkDemo()
    except rospy.ROSInterruptException:
        pass

逆向运动学：

C++

#include <string>
#include <ros/ros.h>
#include <moveit/move_group_interface/move_group_interface.h>

int main(int argc, char **argv)
{
    ros::init(argc, argv, "moveit_fk_demo");
    ros::AsyncSpinner spinner(1);
    spinner.start();

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

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

    //设置目标位置所使用的参考坐标系
    std::string reference_frame = "magician_origin";
    arm.setPoseReferenceFrame(reference_frame);

    //当运动规划失败后，允许重新规划
    arm.allowReplanning(true);

    //设置位置(单位：米)和姿态（单位：弧度）的允许误差
    arm.setGoalPositionTolerance(0.001);
    arm.setGoalOrientationTolerance(0.01);

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

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

    // 设置机器人终端的目标位置
    geometry_msgs::Pose target_pose;
    target_pose.orientation.x = 0.0;
    target_pose.orientation.y = 0.0;
    target_pose.orientation.z = 0.0;
    target_pose.orientation.w = 0.1;
 
    target_pose.position.x = 0.13731;
    target_pose.position.y = -0.12658;
    target_pose.position.z = -0.03313;
    
    // 设置机器臂当前的状态作为运动初始状态
    arm.setStartStateToCurrentState();

    arm.setPoseTarget(target_pose);

    // 进行运动规划，计算机器人移动到目标的运动轨迹，此时只是计算出轨迹，并不会控制机械臂运动
    moveit::planning_interface::MoveGroupInterface::Plan plan;
    moveit::planning_interface::MoveItErrorCode success = arm.plan(plan);

    ROS_INFO("Plan (pose goal) %s",success?"":"FAILED");   

    //让机械臂按照规划的轨迹开始运动。
    if(success)
      arm.execute(plan);
    sleep(1);

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

    ros::shutdown(); 

    return 0;
}

Python

import rospy, sys
import moveit_commander
from geometry_msgs.msg import PoseStamped, Pose


class MoveItIkDemo:
    def __init__(self):
        # 初始化move_group的API
        moveit_commander.roscpp_initialize(sys.argv)
        
        # 初始化ROS节点
        rospy.init_node('moveit_ik_demo')
                
        # 初始化需要使用move group控制的机械臂中的arm group
        arm = moveit_commander.MoveGroupCommander('magician_arm')
                
        # 获取终端link的名称
        end_effector_link = arm.get_end_effector_link()
                        
        # 设置目标位置所使用的参考坐标系
        reference_frame = 'magician_origin'
        arm.set_pose_reference_frame(reference_frame)
                
        # 当运动规划失败后，允许重新规划
        arm.allow_replanning(True)
        
        # 设置位置(单位：米)和姿态（单位：弧度）的允许误差
        arm.set_goal_position_tolerance(0.001)
        arm.set_goal_orientation_tolerance(0.01)
       
        # 设置允许的最大速度和加速度
        arm.set_max_acceleration_scaling_factor(0.5)
        arm.set_max_velocity_scaling_factor(0.5)

        # 控制机械臂先回到初始化位置
        arm.set_named_target('home')
        arm.go()
        rospy.sleep(1)
               
        # 设置机械臂工作空间中的目标位姿，位置使用x、y、z坐标描述，
        # 姿态使用四元数描述，基于base_link坐标系
        target_pose = PoseStamped()
        target_pose.header.frame_id = reference_frame
        target_pose.header.stamp = rospy.Time.now()

        target_pose.pose.orientation.x = 0.0
        target_pose.pose.orientation.y = 0.0
        target_pose.pose.orientation.z = 0.0
        target_pose.pose.orientation.w = 0.1
        target_pose.pose.position.x = 0.13731
        target_pose.pose.position.y = -0.12658
        target_pose.pose.position.z = -0.03313

        
        # 设置机器臂当前的状态作为运动初始状态
        arm.set_start_state_to_current_state()
        
        # 设置机械臂终端运动的目标位姿
        arm.set_pose_target(target_pose, end_effector_link)
        
        # 规划运动路径
        traj = arm.plan()
        
        # 按照规划的运动路径控制机械臂运动
        arm.execute(traj,wait=True)
        rospy.sleep(1)

        # 控制机械臂回到初始化位置
        arm.set_named_target('home')
        arm.go()

        # 关闭并退出moveit
        moveit_commander.roscpp_shutdown()
        moveit_commander.os._exit(0)

if __name__ == "__main__":
    MoveItIkDemo()

    
    

Moveit！编程实战

1、控制夹爪的张开与闭合

#include "ros/ros.h"
#include "ros/console.h"
#include "dobot/SetEndEffectorGripper.h"
 
// 1 张开 0闭合
dobot::SetEndEffectorGripper gripflag(int n)
{
    dobot::SetEndEffectorGripper srv;
    if (n == 1) // 张开
    {
        srv.request.enableCtrl = 1;
        srv.request.grip = 0;
        srv.request.isQueued = 1;
    }
    else if (n == 0) // 闭合
    {
        srv.request.enableCtrl = 1;
        srv.request.grip = 1;
        srv.request.isQueued = 1;
    }
    else
    {
        srv.request.enableCtrl = 0;
        srv.request.grip = 0;
        srv.request.isQueued = 0;
    }
    return srv;
}
 
int main(int argc, char **argv)
{
    setlocale(LC_ALL, "");
    ros::init(argc, argv, "DobotGripperClient");
    ros::NodeHandle n;
 
    ros::ServiceClient client;
 
    // 设置夹爪开闭服务
    client = n.serviceClient<dobot::SetEndEffectorGripper>("/DobotServer/SetEndEffectorGripper");
 
    // 闭合
    auto srv = gripflag(0);
    // 张开
    auto srv1 = gripflag(1);
 
    if (client.call(srv1))
    {
        if (srv.response.result == 0)
        {
            ROS_INFO("夹爪已张开");
        }
        else
        {
            ROS_INFO("夹爪已张开");
        }
    }
    else
    {
        ROS_ERROR("Failed to call SetEndEffectorGripper service");
        return -1;
    }
 
    // 等待一段时间
    ros::Duration(2.0).sleep();
 
    if (client.call(srv))
    {
        if (srv.response.result == 0)
        {
            ROS_INFO("夹爪已闭合");
        }
        else
        {
            ROS_INFO("夹爪未能闭合");
        }
    }
    else
    {
        ROS_ERROR("Failed to call SetEndEffectorGripper service");
        return -1;
    }
 
    // 等待一段时间
    ros::Duration(2.0).sleep();
    //关闭控制器
    auto close = gripflag(2);
    client.call(close);
 
 
    ROS_INFO("资源已释放");
    ros::shutdown();
 
    return 0;
}

2、控制机械臂在笛卡尔空间下画圆

#include <math.h>
#include <ros/ros.h>
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/robot_trajectory/robot_trajectory.h>
 
int main(int argc, char **argv)
{
	ros::init(argc, argv, "magician_circle_demo");
	ros::AsyncSpinner spinner(1);
	spinner.start();
 
    moveit::planning_interface::MoveGroupInterface arm("magician_arm");
 
    //获取终端link的名称
    std::string end_effector_link = arm.getEndEffectorLink();
 
    //设置目标位置所使用的参考坐标系
    std::string reference_frame = "magician_origin";
    arm.setPoseReferenceFrame(reference_frame);
 
    //当运动规划失败后，允许重新规划
    arm.allowReplanning(true);
 
    //设置位置(单位：米)和姿态（单位：弧度）的允许误差
    arm.setGoalPositionTolerance(0.0002);
    arm.setGoalOrientationTolerance(0.0001);
 
    //设置允许的最大速度和加速度
    arm.setMaxAccelerationScalingFactor(0.5);
    arm.setMaxVelocityScalingFactor(0.5);
 
    // 控制机械臂先回到初始化位置
    arm.setNamedTarget("home");
    arm.move();
    sleep(1);
 
    // 设置机器人终端的目标位置
    geometry_msgs::Pose target_pose;
    target_pose.orientation.x = 0;
    target_pose.orientation.y = 0;
    target_pose.orientation.z = 0;
    target_pose.orientation.w = 1;
 
    target_pose.position.x = 0.17075;
    target_pose.position.y = 0.0;
    target_pose.position.z = -0.041;
 
    //arm.setPoseTarget(target_pose);
    //arm.move();
 
	std::vector<geometry_msgs::Pose> waypoints;
 
    //将初始位姿加入路点列表
	//waypoints.push_back(target_pose);
 
    double centerA = target_pose.position.x;
    double centerB = target_pose.position.y;
    double radius = 0.04;
 
    for(double th=0.0; th<6.28; th=th+0.001)
    {
		target_pose.position.x = centerA + radius * sin(th);
        target_pose.position.y = centerB + radius * cos(th);
        waypoints.push_back(target_pose);
    }
 
	// 笛卡尔空间下的路径规划
	moveit_msgs::RobotTrajectory trajectory;
	const double jump_threshold = 0.0;
	const double eef_step = 0.0002;
	double fraction = 0.0;
    int maxtries = 100;   //最大尝试规划次数
    int attempts = 0;     //已经尝试规划次数
 
    while(fraction < 1.0 && attempts < maxtries)
    {
        fraction = arm.computeCartesianPath(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);
    }
 
    // 控制机械臂先回到初始化位置
    arm.setNamedTarget("home");
    arm.move();
    sleep(1);
 
	ros::shutdown(); 
	return 0;
}

3、Magician机械臂在ROS环境下的码垛

#include <string>
#include <ros/ros.h>
#include <moveit/move_group_interface/move_group_interface.h>
#include <iostream>
#include "dobot/SetEndEffectorGripper.h"
 
// 1 张开 0闭合
dobot::SetEndEffectorGripper gripflag(int n)
{
    dobot::SetEndEffectorGripper srv;
    if (n == 1) // 张开
    {
        srv.request.enableCtrl = 1;
        srv.request.grip = 0;
        srv.request.isQueued = 1;
    }
    else if (n == 0) // 闭合
    {
        srv.request.enableCtrl = 1;
        srv.request.grip = 1;
        srv.request.isQueued = 1;
    }
    else
    {
        srv.request.enableCtrl = 0;
        srv.request.grip = 0;
        srv.request.isQueued = 0;
    }
    return srv;
}
 
void close(ros::ServiceClient& client)
{
    auto close = gripflag(2);
    client.call(close);
 
}
 
void gripper(ros::ServiceClient& client,dobot::SetEndEffectorGripper& srv){
 
    std::string info[]={"闭合","张开"};
 
 if (client.call(srv))
    {
        if (srv.response.result == 1)
        {
            ROS_INFO(std::string("夹爪"+ info[srv.request.grip]).c_str());
        }
        else
        {
            ROS_INFO(std::string("夹爪"+info[1-srv.request.grip]).c_str());
        }
    }
    else
    {
        ROS_ERROR("Failed to call SetEndEffectorGripper service");
        return;
    }
 
}
 
geometry_msgs::Pose points(float x,float y,float z){
    geometry_msgs::Pose target_pose;
    target_pose.orientation.x = 0.0;
    target_pose.orientation.y = 0.0;
    target_pose.orientation.z = 0.0;
    target_pose.orientation.w = 0.1;
 
    target_pose.position.x = x;
    target_pose.position.y = y;
    target_pose.position.z = z;
 
    return target_pose;
}
 
 
int main(int argc, char **argv)
{
    setlocale(LC_ALL, "");
    ros::init(argc, argv, "magician_palletizing");
    ros::AsyncSpinner spinner(1);
    spinner.start();
 
    ros::NodeHandle n;
 
    ros::ServiceClient client;
    // 设置夹爪开闭服务
    client = n.serviceClient<dobot::SetEndEffectorGripper>("/DobotServer/SetEndEffectorGripper");
    // 闭合
    auto srv = gripflag(0);
    // 张开
    auto srv1 = gripflag(1);
 
    moveit::planning_interface::MoveGroupInterface arm("magician_arm");
 
    //获取终端link的名称
    std::string end_effector_link = arm.getEndEffectorLink();
 
    //设置目标位置所使用的参考坐标系
    std::string reference_frame = "magician_origin";
    arm.setPoseReferenceFrame(reference_frame);
 
    //当运动规划失败后，允许重新规划
    arm.allowReplanning(true);
 
    //设置位置(单位：米)和姿态（单位：弧度）的允许误差
    arm.setGoalPositionTolerance(0.0002);
    arm.setGoalOrientationTolerance(0.001);
 
    //设置允许的最大速度和加速度
    arm.setMaxAccelerationScalingFactor(0.8);
    arm.setMaxVelocityScalingFactor(0.8);
 
    // 控制机械臂先回到初始化位置
    arm.setNamedTarget("home");
    arm.move();
    sleep(1);
 
    //张开
    gripper(client,srv1);
 
    // 设置机器人终端的目标位置
    // geometry_msgs::Pose target_pose;
    // target_pose.orientation.x = 0.0;
    // target_pose.orientation.y = 0.0;
    // target_pose.orientation.z = 0.0;
    // target_pose.orientation.w = 0.1;
 
    // target_pose.position.x = 0.13731;
    // target_pose.position.y = -0.12658;
    // target_pose.position.z = -0.01313;
 
     // 设置机器人终端的目标位置
    auto point0 = points(0.13731,-0.12658,0.01);
 
    // 设置机器臂当前的状态作为运动初始状态
    arm.setStartStateToCurrentState();
 
    arm.setPoseTarget(point0);
 
    // 进行运动规划，计算机器人移动到目标的运动轨迹，此时只是计算出轨迹，并不会控制机械臂运动
    moveit::planning_interface::MoveGroupInterface::Plan plan;
    moveit::planning_interface::MoveItErrorCode success = arm.plan(plan);
 
    ROS_INFO("Plan (pose goal) %s",success?"":"FAILED");   
 
    //让机械臂按照规划的轨迹开始运动。
    if(success){
      arm.execute(plan);
    sleep(1);
    }
 
    // 设置机器人终端的目标位置
    // target_pose.orientation.x = 0.0;
    // target_pose.orientation.y = 0.0;
    // target_pose.orientation.z = 0.0;
    // target_pose.orientation.w = 0.1;
 
    // target_pose.position.x = 0.13731;
    // target_pose.position.y = -0.12658;
    // target_pose.position.z = -0.03313;
    
    auto point1 = points(0.13731,-0.12658,-0.01313);
 
 
    // 设置机器臂当前的状态作为运动初始状态
    arm.setStartStateToCurrentState();
 
    arm.setPoseTarget(point1);
 
    // 进行运动规划，计算机器人移动到目标的运动轨迹，此时只是计算出轨迹，并不会控制机械臂运动
	success = arm.plan(plan);
 
    ROS_INFO("Plan (pose goal) %s",success?"":"FAILED");   
 
    //让机械臂按照规划的轨迹开始运动。
    if(success){
      arm.execute(plan);
    sleep(1);
    }
 
    // 等待一段时间
    ros::Duration(1.0).sleep();
    // 闭合
    gripper(client,srv);
 
    ros::Duration(1.0).sleep();
 
    // 控制机械臂先回到初始化位置
    arm.setNamedTarget("home");
    arm.move();
    sleep(1);
 
    // 设置机器人终端的目标位置
    // target_pose.orientation.x = 0.0;
    // target_pose.orientation.y = 0.0;
    // target_pose.orientation.z = 0.0;
    // target_pose.orientation.w = 0.1;
 
    // target_pose.position.x = 0.13731;
    // target_pose.position.y = 0.13265;
    // target_pose.position.z = -0.03313;
 
    auto point2 = points(0.13731,0.13265,-0.03813);
 
 
    // 设置机器臂当前的状态作为运动初始状态
    arm.setStartStateToCurrentState();
 
    arm.setPoseTarget(point2);
 
    // 进行运动规划，计算机器人移动到目标的运动轨迹，此时只是计算出轨迹，并不会控制机械臂运动
	success = arm.plan(plan);
 
    ROS_INFO("Plan (pose goal) %s",success?"":"FAILED");   
 
    //让机械臂按照规划的轨迹开始运动。
    if(success){
      arm.execute(plan);
    sleep(1);
    }
 
    ros::Duration(1.0).sleep();
    //张开
    gripper(client,srv1);
 
    ros::Duration(1.0).sleep();
 
    // 设置机器人终端的目标位置
    // target_pose.orientation.x = 0.0;
    // target_pose.orientation.y = 0.0;
    // target_pose.orientation.z = 0.0;
    // target_pose.orientation.w = 0.1;
 
    // target_pose.position.x = 0.13731;
    // target_pose.position.y = 0.13265;
    // target_pose.position.z = -0.01313;
 
    auto point3 = points(0.13731,0.13265,0);
 
 
    // 设置机器臂当前的状态作为运动初始状态
    arm.setStartStateToCurrentState();
 
    arm.setPoseTarget(point3);
 
    // 进行运动规划，计算机器人移动到目标的运动轨迹，此时只是计算出轨迹，并不会控制机械臂运动
	  success = arm.plan(plan);
 
    ROS_INFO("Plan (pose goal) %s",success?"":"FAILED");   
 
    //让机械臂按照规划的轨迹开始运动。
    if(success){
      arm.execute(plan);
    sleep(1);
    }
    
    //闭合
    gripper(client,srv);
    ros::Duration(1.0).sleep();
//
    //控制机械臂先回到初始化位置
    arm.setNamedTarget("home");
    arm.move();
    sleep(1);
    //张开
    gripper(client,srv1);
 
    // 设置机器人终端的目标位置
    // target_pose.orientation.x = 0.0;
    // target_pose.orientation.y = 0.0;
    // target_pose.orientation.z = 0.0;
    // target_pose.orientation.w = 0.1;
 
    // target_pose.position.x = 0.13731;
    // target_pose.position.y = -0.15265;
    // target_pose.position.z = -0.01313;
 
    auto point4 = points(0.12731,-0.16265,-0.01313);
    
 
    arm.setStartStateToCurrentState();
 
    arm.setPoseTarget(point4);
 
    // 进行运动规划，计算机器人移动到目标的运动轨迹，此时只是计算出轨迹，并不会控制机械臂运动
	success = arm.plan(plan);
 
    ROS_INFO("Plan (pose goal) %s",success?"":"FAILED");   
 
    //让机械臂按照规划的轨迹开始运动。
    if(success){
      arm.execute(plan);
    sleep(1);
    }
 
    ros::Duration(1.0).sleep();
 
     // 设置机器人终端的目标位置
    // target_pose.orientation.x = 0.0;
    // target_pose.orientation.y = 0.0;
    // target_pose.orientation.z = 0.0;
    // target_pose.orientation.w = 0.1;
 
    // target_pose.position.x = 0.13731;
    // target_pose.position.y = -0.15265;
    // target_pose.position.z = -0.03313;
 
    auto point5 = points(0.12731,-0.16265,-0.04313);
 
    arm.setStartStateToCurrentState();
 
    arm.setPoseTarget(point5);
 
    // 进行运动规划，计算机器人移动到目标的运动轨迹，此时只是计算出轨迹，并不会控制机械臂运动
	success = arm.plan(plan);
 
    ROS_INFO("Plan (pose goal) %s",success?"":"FAILED");   
 
    //让机械臂按照规划的轨迹开始运动。
    if(success){
      arm.execute(plan);
    sleep(1);
    }
 
    ros::Duration(0.5).sleep();
    //闭合
    gripper(client,srv);
 
    ros::Duration(1.0).sleep();
 
    // 设置机器人终端的目标位置
    // target_pose.orientation.x = 0.0;
    // target_pose.orientation.y = 0.0;
    // target_pose.orientation.z = 0.0;
    // target_pose.orientation.w = 0.1;
 
    // target_pose.position.x = 0.13731;
    // target_pose.position.y = -0.15265;
    // target_pose.position.z = -0.01313;
 
    auto point6 = points(0.13731,-0.15265,-0.01313);
 
 
    arm.setStartStateToCurrentState();
 
    arm.setPoseTarget(point6);
 
    // 进行运动规划，计算机器人移动到目标的运动轨迹，此时只是计算出轨迹，并不会控制机械臂运动
	success = arm.plan(plan);
 
    ROS_INFO("Plan (pose goal) %s",success?"":"FAILED");   
 
    //让机械臂按照规划的轨迹开始运动。
    if(success){
      arm.execute(plan);
    sleep(1);
    }
    ros::Duration(1.0).sleep();
 
    // 设置机器人终端的目标位置
    // target_pose.orientation.x = 0.0;
    // target_pose.orientation.y = 0.0;
    // target_pose.orientation.z = 0.0;
    // target_pose.orientation.w = 0.1;
 
    // target_pose.position.x = 0.13731;
    // target_pose.position.y = 0.15265;
    // target_pose.position.z = -0.03313;
 
    auto point7 = points(0.11731,0.11265,-0.04013);
 
 
    arm.setStartStateToCurrentState();
 
    arm.setPoseTarget(point7);
 
    // 进行运动规划，计算机器人移动到目标的运动轨迹，此时只是计算出轨迹，并不会控制机械臂运动
	success = arm.plan(plan);
 
    ROS_INFO("Plan (pose goal) %s",success?"":"FAILED");   
 
    //让机械臂按照规划的轨迹开始运动。
    if(success){
      arm.execute(plan);
    sleep(1);
    }
 
    ros::Duration(0.5).sleep();
   //张开 
    gripper(client,srv1);
 
    ros::Duration(1.0).sleep();
 
    // 设置机器人终端的目标位置
    // target_pose.orientation.x = 0.0;
    // target_pose.orientation.y = 0.0;
    // target_pose.orientation.z = 0.0;
    // target_pose.orientation.w = 0.1;
 
    // target_pose.position.x = 0.13731;
    // target_pose.position.y = 0.15265;
    // target_pose.position.z = -0.01313;
 
    auto point8 = points(0.11731,0.11265,-0.01313);
 
    arm.setStartStateToCurrentState();
 
    arm.setPoseTarget(point8);
 
    // 进行运动规划，计算机器人移动到目标的运动轨迹，此时只是计算出轨迹，并不会控制机械臂运动
	success = arm.plan(plan);
 
    ROS_INFO("Plan (pose goal) %s",success?"":"FAILED");   
 
    //让机械臂按照规划的轨迹开始运动。
    if(success){
      arm.execute(plan);
    sleep(1);
    }
 
    // 控制机械臂先回到初始化位置
    arm.setNamedTarget("home");
    arm.move();
    sleep(1);
 
//
    auto point9 = points(0.13731,-0.12658,-0.01313);
 
    arm.setStartStateToCurrentState();
 
    arm.setPoseTarget(point9);
 
    // 进行运动规划，计算机器人移动到目标的运动轨迹，此时只是计算出轨迹，并不会控制机械臂运动
	  success = arm.plan(plan);
 
    ROS_INFO("Plan (pose goal) %s",success?"":"FAILED");   
 
    //让机械臂按照规划的轨迹开始运动。
    if(success){
      arm.execute(plan);
    sleep(1);
    }
 
    ros::Duration(1.0).sleep();
 
    auto point10 = points(0.13731,-0.12658,-0.04313);
 
    arm.setStartStateToCurrentState();
 
    arm.setPoseTarget(point10);
 
    // 进行运动规划，计算机器人移动到目标的运动轨迹，此时只是计算出轨迹，并不会控制机械臂运动
	success = arm.plan(plan);
 
    ROS_INFO("Plan (pose goal) %s",success?"":"FAILED");   
 
    //让机械臂按照规划的轨迹开始运动。
    if(success){
      arm.execute(plan);
    sleep(1);
    }
 
    ros::Duration(1.0).sleep();
    //闭合
    gripper(client,srv);
 
    ros::Duration(1.0).sleep();
 
    auto point11 = points(0.13731,0.13265,0.01313);
 
    arm.setStartStateToCurrentState();
 
    arm.setPoseTarget(point11);
 
    // 进行运动规划，计算机器人移动到目标的运动轨迹，此时只是计算出轨迹，并不会控制机械臂运动
	success = arm.plan(plan);
 
    ROS_INFO("Plan (pose goal) %s",success?"":"FAILED");   
 
    //让机械臂按照规划的轨迹开始运动。
    if(success){
      arm.execute(plan);
    sleep(1);
    }
 
    ros::Duration(1.0).sleep();
 
    auto point12 = points(0.13731,0.13265,-0.01813);
 
 
    arm.setStartStateToCurrentState();
 
    arm.setPoseTarget(point12);
 
    // 进行运动规划，计算机器人移动到目标的运动轨迹，此时只是计算出轨迹，并不会控制机械臂运动
	success = arm.plan(plan);
 
    ROS_INFO("Plan (pose goal) %s",success?"":"FAILED");   
 
    //让机械臂按照规划的轨迹开始运动。
    if(success){
      arm.execute(plan);
    sleep(1);
    }
 
    ros::Duration(1.0).sleep();
    //张开
    gripper(client,srv1);
 
    ros::Duration(1.0).sleep();
 
    auto point13 = points(0.13731,0.13265,0.00313);
 
 
    arm.setStartStateToCurrentState();
 
    arm.setPoseTarget(point13);
 
    // 进行运动规划，计算机器人移动到目标的运动轨迹，此时只是计算出轨迹，并不会控制机械臂运动
	success = arm.plan(plan);
 
    ROS_INFO("Plan (pose goal) %s",success?"":"FAILED");   
 
    //让机械臂按照规划的轨迹开始运动。
    if(success){
      arm.execute(plan);
    sleep(1);
    }
 
    ros::Duration(1.0).sleep();
 
    // 控制机械臂先回到初始化位置
    arm.setNamedTarget("home");
    arm.move();
    sleep(1);
 
    ros::Duration(1.0).sleep();
 
 
 
 
    //关闭控制器
    close(client);
    ros::shutdown(); 
 
    return 0;
}

4、控制气泵

#include <string>
#include <ros/ros.h>
#include <moveit/move_group_interface/move_group_interface.h>
#include <iostream>
#include "magician_hardware/SetEndEffectorSuctionCup.h"
 
int main(int argc, char **argv)
{
    ros::init(argc, argv, "control_suction_cup_demo");
    ros::AsyncSpinner spinner(1);
    spinner.start();
 
    ros::NodeHandle n;
 
ros::ServiceClient client;
	client = n.serviceClient<magician_hardware::SetEndEffectorSuctionCup>("/DobotServer/SetEndEffectorSuctionCup");
	//设置气泵吸取suck1
	magician_hardware::SetEndEffectorSuctionCup suck1;
	suck1.request.enableCtrl = 1;
	suck1.request.suck =1;
	suck1.request.isQueued = true;
	//设置气泵关闭suck2
	magician_hardware::SetEndEffectorSuctionCup suck2;
	suck2.request.enableCtrl = 0;
	suck2.request.suck =0;
	suck2.request.isQueued = true;



	//开启气泵吸气---关闭夹爪
	client.call(suck1);
 
 
	//关闭气泵
	client.call(suck2);

    ros::shutdown();

    return 0;
}