通过actionlib控制jaco机械臂

为了安全，先写一个简单控制三个手指的程序：

根据驱动包内kinova_fingers_action.cpp服务器写客户端程序

#include <ros/ros.h>

#include "kinova_driver/kinova_tool_pose_action.h"
#include "kinova_driver/kinova_joint_angles_action.h"
#include "kinova_driver/kinova_fingers_action.h"
#include <actionlib/client/simple_action_client.h>

typedef actionlib::SimpleActionClient<kinova_msgs::ArmJointAnglesAction> ArmJoint_actionlibClient;
typedef actionlib::SimpleActionClient<kinova_msgs::ArmPoseAction> ArmPose_actionlibClient;
typedef actionlib::SimpleActionClient<kinova_msgs::SetFingersPositionAction> Finger_actionlibClient;

std::string kinova_robot_type = "j2n6a300";
std::string Finger_action_address = "/" + kinova_robot_type + "_driver/fingers_action/finger_positions";    //手指控制服务器的地址
std::string finger_position_sub_address = "/" + kinova_robot_type +"_driver/out/finger_position";           //手指位置信息的话题地址

int finger_maxTurn = 6800;              // max thread rotation for one finger
int setFingerPos[3] = {0,0,0};          //设置手指闭合百分比

bool currcent_flag = false;
bool sendflag = true;

typedef struct _fingers
{
  float finger1;
  float finger2;
  float finger3;

} FINGERS;

FINGERS fingers;

void sendFingerGoal(int *p)
{
    Finger_actionlibClient client(Finger_action_address, true);
    kinova_msgs::SetFingersPositionGoal goal;

    goal.fingers.finger1 = (float)p[0] / 100 * finger_maxTurn;
    goal.fingers.finger2 = (float)p[1] / 100 * finger_maxTurn;
    goal.fingers.finger3 = (float)p[2] / 100 * finger_maxTurn;

    ROS_INFO("Waiting for action server to start.");
    client.waitForServer();
    ROS_INFO("Action server started, sending goal.");

    //sendFingerGoal();
    client.sendGoal(goal);

    bool finish_before_timeout = client.waitForResult(ros::Duration(5.0));

    if(finish_before_timeout)
    {
        actionlib::SimpleClientGoalState state = client.getState();
        ROS_INFO("action finish : %s",state.toString().c_str());
    }
    else
    {
        ROS_INFO("TIMEOUT");
    }
}

void currentFingerPoseFeedback(const kinova_msgs::FingerPosition  finger_pose_command)
{
    fingers.finger1= finger_pose_command.finger1;
    fingers.finger2= finger_pose_command.finger2;
    fingers.finger3= finger_pose_command.finger3;
    currcent_flag = true;
}

int main(int argc, char** argv)
{
    if(argc != 4)
    {
        printf("arg error !!! \n");
        return -1;
    }
    else
    {
        setFingerPos[0] = atoi(argv[1]);
        setFingerPos[1] = atoi(argv[2]);
        setFingerPos[2] = atoi(argv[3]);
    }

    ros::init(argc, argv, "finger_control");
    ros::NodeHandle nh;
    ros::Subscriber finger_sub = nh.subscribe(finger_position_sub_address, 10, currentFingerPoseFeedback);

   ros::Rate rate(10);

    while(ros::ok())
    {
        if(sendflag == true)
        {
            sendFingerGoal(setFingerPos);
            sendflag = false;
        }
        if(currcent_flag == true)
        {
            ROS_INFO("\nFinger values in turn are:\n \tFinger1 = %f \n \tFinger2 = %f  \n \tFinger3 = %f",fingers.finger1,fingers.finger2,fingers.finger3);
            ros::shutdown();
        }
        ros::spinOnce();
        rate.sleep();
    } 

    return 0;
}

roslaunch jaco-ros/kinova_bringup/launch/kinova_robot.launch
rosrun kinova_driver kinova_fingers_control 30 30 30

action finish:后边打印出SUCCEEDED则代表这次动作成功

设置参数为百分比制，具体看之前写的环境配置

直接控制机械臂的空间位置就不再写了，以后机械臂机械臂moveit路径规划是控制的每个关节，所以就写一个控制关节的测试程序

#include <ros/ros.h>

#include "kinova_driver/kinova_tool_pose_action.h"
#include "kinova_driver/kinova_joint_angles_action.h"
#include "kinova_driver/kinova_fingers_action.h"
#include <actionlib/client/simple_action_client.h>

typedef actionlib::SimpleActionClient<kinova_msgs::ArmJointAnglesAction> ArmJoint_actionlibClient;
typedef actionlib::SimpleActionClient<kinova_msgs::ArmPoseAction> ArmPose_actionlibClient;
typedef actionlib::SimpleActionClient<kinova_msgs::SetFingersPositionAction> Finger_actionlibClient;

std::string kinova_robot_type = "j2n6a300";
std::string Joint_action_address = "/" + kinova_robot_type + "_driver/joints_action/joint_angles";    //关节控制服务器的地址
std::string joint_angle_sub_address = "/" + kinova_robot_type +"_driver/out/joint_command";           //关节位置信息的话题地址

bool currcent_flag = false;
bool sendflag = true;
int setJointangle[6] = {0,0,0,0,0,0};          //设置关节角度

typedef struct _JOINTS
{
  float joint1;
  float joint2;
  float joint3;
  float joint4;
  float joint5;
  float joint6;

} JOINTS;

JOINTS joints;

void sendJointGoal(int *angle_set)
{
    ArmJoint_actionlibClient client(Joint_action_address, true);
    kinova_msgs::ArmJointAnglesGoal goal;

    goal.angles.joint1 = (float)angle_set[0];
    goal.angles.joint2 = (float)angle_set[1];
    goal.angles.joint3 = (float)angle_set[2];
    goal.angles.joint4 = (float)angle_set[3];
    goal.angles.joint5 = (float)angle_set[4];
    goal.angles.joint6 = (float)angle_set[5];

    ROS_INFO("Waiting for action server to start.");
    client.waitForServer();
    ROS_INFO("Action server started, sending goal.");

    //sendJointGoal();
    client.sendGoal(goal);

    bool finish_before_timeout = client.waitForResult(ros::Duration(20.0));

    if(finish_before_timeout)
    {
        actionlib::SimpleClientGoalState state = client.getState();
        ROS_INFO("action finish : %s",state.toString().c_str());
    }
    else
    {
        ROS_INFO("TIMEOUT");
    }
}

void currentjointangleFeedback(const kinova_msgs::JointAnglesConstPtr & joint_angle_command)
{
    joints.joint1 = joint_angle_command->joint1;
    joints.joint2 = joint_angle_command->joint2;
    joints.joint3 = joint_angle_command->joint3;
    joints.joint4 = joint_angle_command->joint4;
    joints.joint5 = joint_angle_command->joint5;
    joints.joint6 = joint_angle_command->joint6;
    currcent_flag = true;
}

int main(int argc, char** argv)
{
    if(argc != 7)
    {
        printf("arg error !!! \n");
        return -1;
    }
    else
    {
        setJointangle[0] = atoi(argv[1]);
        setJointangle[1] = atoi(argv[2]);
        setJointangle[2] = atoi(argv[3]);
        setJointangle[3] = atoi(argv[4]);
        setJointangle[4] = atoi(argv[5]);
        setJointangle[5] = atoi(argv[6]);
    }

    ros::init(argc, argv, "joint_control");
    ros::NodeHandle nh;
    ros::Subscriber finger_sub = nh.subscribe(joint_angle_sub_address, 10, currentjointangleFeedback);
    ros::Rate rate(10);

    while(ros::ok())
    {
        if(sendflag == true)
        {
            sendJointGoal(setJointangle);
            sendflag = false;
        }
        if(currcent_flag == true)
        {
            ROS_INFO("\ncurrent jointangle: \n \tjonint1: %f  \n \tjonint2: %f  \n \tjonint3: %f  \n \tjonint4: %f  \n \tjonint5: %f \n \tjonint6: %f",
                                        joints.joint1, joints.joint2, joints.joint3, joints.joint4, joints.joint5, joints.joint6);
            ros::shutdown();
        }
        ros::spinOnce();
        rate.sleep();
    } 

    return 0;
}

roslaunch jaco-ros/kinova_bringup/launch/kinova_robot.launch
rosrun kinova_driver joint_angle_control  275 167 57 -119 82 75

这六个角度的参数可不要乱填，有可能会碰着底座，虽然有力矩保护，但也不能乱来

我的这个参数是根据机械臂在HOME处的各个关节的角度