使用Geomagic公司遥操作器Touch遥操作UR5e机器人
应导师要求，给了我一款遥操作器，需要通过该遥操作器来操作UR5e机器人。这款遥操作器Geomagic公司出品的Touch，外型大概长这个样子：
以下是大致思路：使用moveit控制UR5e，同时利用ROS接收Touch遥操作器的数据，然后进行一个ROS内的数据转发，发送给moveit。但是实际开发过程中还是有一些细节的问题。大致总结如下：
1，机器人在实际使用的过程当中，执行movel是需要规划时间的，而遥操作器的消息更新频率为1000HZ。通过和厂家沟通，该频率无法固定。所以，如果按照遥操作器的更新频率来执行movel，必然会遇到频繁的丢步或者延迟问题。体现在操作手感上给人的感觉就是很难使用，比较干涩。
解决办法：降低moveit运动规划器对于遥操作器的采样频率，以此降低运动次数，保证手感。
2，人手在手持遥操作器时会有自然的抖动，同时数据本身也会有小幅度抖动，这些是无法避免的。遥操作器不应该对这些微小抖动很敏感，需要进行数据的滤波处理。
解决办法：设置关键pose，类似于一个pose的滑动窗，对于pose中前后pose的位移使用轴角表示法进行表示，然后判断sigma的大小。如果sigma较大，则认为发生了一次“显著”的位移，才开始移动机器人，这样可以屏蔽较小的抖动。
3，遥操作器和真实机器人UR5e的工作空间对不上，如何处理？
本来是想对于xyz坐标乘以一定的系数，然后发给moveit以此来把两个工作空间对应起来。但是后面听从师兄的经验，捞出遥操作器的移动的位姿转移矩阵，然后取出xyz分量，其他置1，再发回给机器人，优化后感觉更加稳妥。
1：OpenHaptics 环境配置
首先是安装touch驱动。Touch驱动的下载地址：https://support.3dsystems.com/s/article/OpenHaptics-for-Linux-Developer-Edition-v34?language=en_US
在该页面下可以看到有 OpenHaptics 环境配置文件、touch 驱动文件以及驱动安装教程。
将环境配置文件openhaptics_3.4-0-developer-edition-amd64.tar.gz放到主目录下，解压
tar -xzvf openhaptics_3.4-0-developer-edition-amd64.tar.gz然后新开 ternimal ，cd 到解压后的文件夹openhaptics_3.4-0-developer-edition-amd64下，用sudo ./install安装
过程中需要按下任意键重启一次，重启后就安装好啦，安装路径在计算机的/opt目录下。
2：Geomagic Touch 驱动安装
Step1：在主目录下创建一个 touch_driver 文件夹，将下载好的驱动压缩包TouchDriver2019_2_15_Linux.tar.xz放在该文件夹下并解压；
Step2：进入解压缩之后的文件夹下（注意sudo提升权限）
将/touch_driver/TouchDriver2019_2_15_Linux/TouchDriver2019_2_14/bin文件夹下的Touch Setup和Touch diagnostics可执行文件复制到/usr/bin路径下；
将/touch_driver/TouchDriver2019_2_15_Linux/TouchDriver2019_2_14/usr/lib文件夹下的LibPhantomIOLib42.so复制到/usr/lib路径下；
Step3：安装 QT5，不用操作，因为 Ubuntu 18.04 已经安装了；
Step4：创建 3DSystems 目录用以配置和保存配置文件；
sudo mkdir /usr/share/3DSystemsStep5：设置环境变量 GTDD_HOME 映射以上创建的配置文件目录的路径。将以下环境变量添加到/etc/environment以使其永久化。可以直接将环境变量写入/etc/enviroment文件中并保存，在最后一行加入GTDD_HOME="/usr/share/3DSystems"，但是注意，环境变量的生效需要先注销再重新登入ubuntu系统；
Step6：在/usr/share/3DSystems路径下创建 config 文件夹sudo mkdir config；
Step7：给Touch设备赋予权限，打开终端输入sudo chmod 777 /dev/ttyACM0；（注意！！！！！！每次重启以及插拔Touch时都应该重新输入该指令以赋予权限）
Step8：进入驱动的bin目录，输入sudo ./Touch_Setup 进入设置页面，看到自动读取出Touch设备的序列号以及相应信息之后，点击下方的Apply以及OK，会将相应设置写入之前创建的文件夹config之中。（注意这一步是需要sudo权限的）
之后执行sudo ./Touch_Diagnostic
点击向右的黑色箭头
此时的Touch位姿应该和真实机器人位姿相同，如果不同，将Touch的手臂正确插入孔内，待听到卡的一声响声之后，再重新打开可执行文件，再检查位姿是否准确。
以上完成了Touch遥操作器的相关准备工作。
3：下载遥操作器ROS功能包Geomagic_Touch_ROS_Drivers-hydro
下载地址：https://github.com/bharatm11/Geomagic_Touch_ROS_Drivers
按照github页面显示，安装相应依赖。下载后，放入ros工作空间内，catkin_make。如果遇到报错，则检查相关依赖是否安装完整（按照提示，缺什么就下什么，也可以试一下rosdep install --from-paths src --ignore-src -r -y 一键安装一下依赖）
安装完毕后，打开终端输入roslaunch omni_common omni_state.launch，正常打开的界面应该没有报错，界面运行如下：
打开终端输入rostopic list可以看到多了几个前缀为/phantom的话题，输入rostopic echo ***可以打印该话题输出的信息。这也证明了我们遥操作器的功能包使用正常。
4：启动UR5e机器人，并使用moveit连接。
这一步网上教程很多，大家可以自行搜索。
5：启动遥操作器转发
这里我写了一个movel的demo，目前已上传gitee。地址为：https://gitee.com/TeamWIT-RoboMasterAI/demo/tree/master/ROS_Touch_UR5e_demo
可执行文件名为demotouch
这里为了解决上述的排除微小抖动的问题，我使用了一个过滤器，类似pose的滑动窗口，对前后两个关键时刻的pose以及当前pose进行一个维护。函数如下：
bool Posedatefilter(const geometry_msgs::Pose& p_keypose,const geometry_msgs::Pose& p_pose){
xyz_norm = sqrt(pow((p_keypose.position.x - p_pose.position.x),2) + pow((p_keypose.position.y - p_pose.position.y),2) + pow((p_keypose.position.z - p_pose.position.z),2));
msg2tf(p_keypose,tftrans1);
msg2tf(p_pose,tftrans2);
sigma1 = tftrans1.getRotation().angle(tftrans1.getRotation());
sigma2 = tftrans1.getRotation().angle(tftrans2.getRotation());
sigma_norm = sqrt(pow((sigma1-sigma2),2));
if(xyz_norm > 0.01)
{
return true;
}
else
return false;
}
该函数通过判断转换当前的位移为轴角表示，再通过判断标量xyz_norm来判断是否要将位移传给moveit。
void solvedtf(){
tf::Quaternion q;
tf::Vector3 t;
dtf.setIdentity();
t.setX(-(keyframe_PoseStamped.pose.position.x - keyframe_LastPoseStamped.pose.position.x));
t.setY(keyframe_PoseStamped.pose.position.y - keyframe_LastPoseStamped.pose.position.y);
t.setZ(-(keyframe_PoseStamped.pose.position.z - keyframe_LastPoseStamped.pose.position.z));
dtf.setOrigin(t);
tf2msg(start_tf * dtf,robot_states);
}
该函数实现了位移矩阵的传递。
