六维力数据中去除重力、漂移力任务---ros之talker、listener、ur、坑

理论基础

见前面博文
六维力传感器参数辨识方法+matlab代码实现

talker

见前面博文
linux下读取ATI公司力传感器数据&&c++中调用c源文件&&移植到ros+问题解决
最终的talker

/*** 该例程将发布chatter话题，消息类型String*/#include <sstream>
#include "ros/ros.h"
#include "geometry_msgs/WrenchStamped.h"
#include <stdio.h>#ifdef __cplusplus
extern "C"
{#endif
#include "netft.h"
#ifdef __cplusplus
}
#endif
int main(int argc, char **argv)
{// ROS节点初始化ros::init(argc, argv, "talker");// 创建节点句柄ros::NodeHandle n;// 创建一个Publisher，发布名为chatter的topic，消息类型为std_msgs::Stringros::Publisher chatter_pub = n.advertise<geometry_msgs::WrenchStamped>("force0", 1000);// 设置循环的频率ros::Rate loop_rate(100);int nn = 0;int count = 0;while (ros::ok()){// 初始化std_msgs::String类型的消息geometry_msgs::WrenchStamped msg;msg.wrench.force.x = a(argc,argv)[0];msg.wrench.force.y = a(argc,argv)[1];msg.wrench.force.z = a(argc,argv)[2];msg.wrench.torque.x = a(argc,argv)[3];msg.wrench.torque.y = a(argc,argv)[4];msg.wrench.torque.z = a(argc,argv)[5];nn++;// 发布消息
ROS_INFO("[%f]", msg.wrench.force.x);chatter_pub.publish(msg);// 循环等待回调函数ros::spinOnce();// 按照循环频率延时loop_rate.sleep();++count;}return 0;
}

listener

#include <ros/ros.h>
#include "std_msgs/String.h"
#include <sstream>
#include "geometry_msgs/WrenchStamped.h"
#include "sensor_msgs/JointState.h"
#include <cmath>
class SubscribeAndPublish
{
public:  SubscribeAndPublish()  {  pub_ = n_.advertise<geometry_msgs::WrenchStamped>("/force1", 1000);  sub_ = n_.subscribe("force0", 1000, &SubscribeAndPublish::callback1, this);  sub2_ = n_.subscribe("joint_states", 1000, &SubscribeAndPublish::callback2, this);  }
//由正向运动学得到变换矩阵的最后一行元素，用于计算实际的接触力
float *qianxiang(const sensor_msgs::JointState::ConstPtr& a)
{a1 = a->position[0];a2 = a->position[1];a3 = a->position[2];a4 = a->position[3];a5 = a->position[4];a6 = a->position[5];b[0] = -0.5;b[1] = 0.707;b[2] = -0.5;return b;
}void callback1(const geometry_msgs::WrenchStamped::ConstPtr& msg1)  {  force.wrench.force.x = msg1->wrench.force.x/1000000+g*x*qq[0]+g*y*qq[1]-fx;force.wrench.force.y = msg1->wrench.force.y/1000000-g*y*qq[0]+g*x*qq[1]-fy;force.wrench.force.z = msg1->wrench.force.z/1000000+g*qq[2]-fz;force.wrench.torque.x = msg1->wrench.torque.x/1000000+g*y*pz*qq[0]-g*x*pz*qq[1]+g*py*qq[2]-tx;force.wrench.torque.y = msg1->wrench.torque.y/1000000+g*x*pz*qq[0]+g*y*pz*qq[1]-g*px*qq[2]-ty;force.wrench.torque.z = msg1->wrench.torque.z/1000000-g*x*py*qq[0]-g*y*py*qq[1]-g*y*px*qq[0]+g*x*px*qq[1]-tz;pub_.publish(force);ros::Rate loop_rate(10);loop_rate.sleep();}  void callback2(const sensor_msgs::JointState::ConstPtr& msg2){qq[0] = qianxiang(msg2)[0];qq[1] = qianxiang(msg2)[1];qq[2] = qianxiang(msg2)[2];
for(int i=0;i<3;i++)ROS_INFO("[%f]", qq[i]);}
private: float a1;float a2;float a3;float a4;float a5;float a6;float g = 2.96;float fx = 1.82;float fy = -15.41;float fz = -14.18;float tx = 0.071;float ty = 0.349;float tz = 0.000139;float x = 0.34176;float y = -0.24539;float px = -0.001740;float py = 0.00145;float pz = 0.02223;float qq[3];float b[3];ros::NodeHandle n_;   ros::Publisher pub_;  ros::Subscriber sub_;ros::Subscriber sub2_; geometry_msgs::WrenchStamped force;sensor_msgs::JointState joint;};//End of class SubscribeAndPublish  int main(int argc, char **argv)
{  //Initiate ROS  ros::init(argc, argv, "subscribe_and_publish");  //Create an object of class SubscribeAndPublish that will take care of everything  SubscribeAndPublish test;  //ros::spin();ros::MultiThreadedSpinner s(2);  //多线程ros::spin(s);  return 0;
}

这里面坑无数：
1、cmath不需要在cmakelists中添加;
2、boost可以不添加;
3、joint_states类型消息必须要先resize大小，才能用！！！，且消息不能用在函数的返回值中;

ur

好久不用ur，也有不少问题，其中经常性会连接不上，sock傻傻啥的。
1、可能本身电脑网段不对;
2、需要添加reserve_port:=30004，端口号可能不对，这个也是一个点
3、可以在common.launch文件中单独param参数，就不用再输入了。

还有个问题，就是需要将ur和传感器网络放在同一网段下，ur机械臂设置ip的方式：
点击文件，然后退出，就可以设置ur ip地址。

坑

现在的一坑是matlab中计算的正向运动学的R3i，即变换矩阵第三行的元素与论文还中的不一样。

May the force be with you!

六维力数据中去除重力、漂移力任务---ros之talker、listener、ur、坑相关推荐

机械臂六维力传感器重力补偿原理
重力补偿原理 1.力传感器数据分析将六维力传感器三个力分量与力矩分量的零点值分别记为 ( F x 0 , F y 0 , F z 0 ) (F_{x0},F_{y0},F_{z0}) (Fx0,F ...
从 RMS 抖动测量中去除示波器噪声
1 简介我们在此介绍了一种新方法,可在来自测量环境的信号中添加的抖动水平接近或超过信号的固有抖动时,使用实时示波器准确测量抖动. 该方法建立在之前的工作的基础上,该工作结合了测量和建模数据以消除峰峰 ...
高通量数据中批次效应的鉴定和处理（六）- 直接校正表达矩阵
生物信息学习的正确姿势 NGS系列文章包括NGS基础.转录组分析 (Nature重磅综述|关于RNA-seq你想知道的全在这).ChIP-seq分析 (ChIP-seq基本分析流程).单细胞测序分析 ...
用于针对DNN中后门攻击的蒸馏对策和中毒数据的去除
用于针对DNN中后门攻击的蒸馏对策和中毒数据的去除之前的工作现有的防御大多利用后门模型在输入后门数据和正常数据时DNN内部神经元活性分布的不同. [刘, 2018]通过对干净样本输入时神经活性低的 ...
基因表达数据中信息基因和基因调控网络第六周报告
基因表达数据中信息基因和基因调控网络第六周报告本周主要看了<基因芯片技术><基因表达数据的聚类分析>两篇论文,初步了解了基因芯片和聚类分析的含义. 一.基因芯片技术基因芯 ...
Brief Bioinform | 农科院深圳基因组所王怡雯组提出一种去除微生物组数据中批次效应的多元算法框架...
PLSDA-batch:去除微生物组数据中批次效应的多元算法框架 PLSDA-batch: a multivariate framework to correct for batch effects ...
python-演练-数据排除-从学生得分中去除题目源中并不存在的题
需求与情况说明需要删掉学生得分数据中,不存在的题目当前有两个数据题目源数据学生得分数据假设题目源数据 = [a题目,b题目,c题目] 学生得分数据 = [a题目得分,b题目得分,e题目得分 ...
db2去除字段值的空格_DB2表数据中存在空格问题解决办法
--创建测试表 create table administrator.testhyg(id int,name varchar(30),birth date); --插入测试数据 insert into ...
sql中去除重复的数据
总的思路就是先找出表中重复数据中的一条数据,插入临时表中,删除所有的重复数据,然后再将临时表中的数据插入表中.所以重点是如何找出重复数据中的一条数据,有三种情况 1.重复数据完全一样,使用distin ...

六维力数据中去除重力、漂移力任务---ros之talker、listener、ur、坑

理论基础

talker

listener

ur

坑

六维力数据中去除重力、漂移力任务---ros之talker、listener、ur、坑相关推荐

最新文章

热门文章