通过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机械臂的更多相关文章
- 配置 jaco机械臂 ros环境
---恢复内容开始--- 终于有机械臂了, 首先先下载包 cd ~/catkin_ws/src git clone https://github.com/Kinovarobotics/kinova-r ...
- ROS连接ABB机械臂调试详细教程-ROS(indigo)和ABB RobotStudio 6.03.02-
在ROS industrial介绍中,给出了ROS和常用机械臂的连接方式.具体信息可以参考:http://wiki.ros.org/Industrial ROS连接ABB机械臂调试详细教程-ROS(i ...
- 机械臂——arduino、marlin固件、printrun软件【转】
最近了解到,在市面上大多数机械臂控制都采用的arduino这个开源硬件来控制的,而我发现既然会单片机,就没有必要采用arduino来控制了,arduino只是一种为了简化编程而开发一种软硬件控制平台, ...
- 使用VTK与Python实现机械臂三维模型可视化
三维可视化系统的建立依赖于三维图形平台, 如 OpenGL.VTK.OGRE.OSG等, 传统的方法多采用OpenGL进行底层编程,即对其特有的函数进行定量操作, 需要开发人员熟悉相关函数, 从而造成 ...
- Android版网易云音乐唱片机唱片磁盘旋转及唱片机机械臂动画关键代码实现思路
Android版网易云音乐唱片机唱片磁盘旋转及唱片机机械臂动画关键代码实现思路 先看一看我的代码运行结果. 代码运行起来初始化状态: 点击开始按钮,唱片机的机械臂匀速接近唱片磁盘,同时唱片磁盘也 ...
- 基于 Mathematica 的机器人仿真环境(机械臂篇)[转]
完美的教程,没有之一,收藏学习. 目的 本文手把手教你在 Mathematica 软件中搭建机器人的仿真环境,具体包括以下内容(所使用的版本是 Mathematica 11.1,更早的版本可能缺少某些 ...
- ViperX 300 Robot Arm 机械臂 “5自由度和360°全方位旋转”
- OpenManipulator RM-X52 ROS 开源机械臂
DYNAMIXEL PRO PH54-200-S500-R 简介
- Turtlebot3新手教程:Open-Manipulator机械臂
*本文针对如何结合turtlebot3和Open-Manipulator机械臂做出讲解 测试在Ubuntu 16.04, Linux Mint 18.1和ROS Kinetic Kame下进行 具体步 ...
随机推荐
- 34-python基础-python3-列表删除元素-remove()方法-del语句-pop()方法
1-remove()方法 根据值删除元素. remove()方法传入一个列表中的值,它将从被调用的列表中删除. 如果该值在列表中出现多次,只有第一次出现的值会被删除. 如果要删除的值可能在列表中出现 ...
- LeetCode Arrary Easy 35. Search Insert Position 题解
Description Given a sorted array and a target value, return the index if the target is found. If not ...
- tee - 从标准输入写往文件和标准输出
总览 (SYNOPSIS) tee [OPTION]... [FILE]... 描述 (DESCRIPTION) 把 标准输入 的 数据 复制到 每一个 文件 FILE, 同时 送往 标准输出. -a ...
- 使用Hystrix实现断路器处理
在之前的架构的基础上我们会发现,一旦级别低的服务宕了,会导致调用它的服务也挂掉,这样容易产生级联效应(雪崩效应),为了防止这种情况的出现,我引入了Hystrix来处理,先介绍ribbon使用Hystr ...
- dubbo-源码阅读之服务订阅
配置例子 <?xml version="1.0" encoding="UTF-8"?> <beans xmlns="http://w ...
- vue 通过插槽分发内容
<!DOCTYPE html> <html> <head> <meta charset="UTF-8"> <title> ...
- sqlserver 将店铺表转换成可以用in查询的字符串
create TABLE #tempshopt ( shopid varchar(max) ) set @aSql = 'insert into #tempshopt(shopid) select s ...
- SpringBoot - @ControllerAdvice 处理异常
在Spring 3.2中,新增了@ControllerAdvice.@RestControllerAdvice 注解,可以用于定义@ExceptionHandler.@InitBinder.@Mode ...
- USB之Main item, Local item和Global item 的作用范围与归类
https://doc.micrium.com/display/OSUM50300/USB+Device+HID+Class+Overview report descriptor –> item ...
- loadrunner自定义函数
https://zhangfy068.iteye.com/blog/1614794 Loadruner 有四种实现自定义函数的方式,根据脚本编写方便性进行选择不同的方式. (1)直接引用法: Acti ...