通过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处的各个关节的角度

巴特西

通过actionlib控制jaco机械臂

最新文章

热门文章