为了安全，先写一个简单控制三个手指的程序：
根据驱动包内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 !!!
");
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("
Finger values in turn are:
Finger1 = %f
Finger2 = %f
Finger3 = %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 !!!
");
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("
current jointangle:
jonint1: %f
jonint2: %f
jonint3: %f
jonint4: %f
jonint5: %f
jonint6: %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处的各个关节的角度