配置Movelt ikfast运动学插件

安装DOCKER

wget http://fishros.com/install -O fishros && . fishros

在urdf文件所在工作空间运行DOCKER镜像

xhost + && sudo docker run  -it --rm  -v /tmp/.X11-unix:/tmp/.X11-unix --device /dev/snd -e DISPLAY=unix$DISPLAY  -v `pwd`:`pwd`  -w `pwd` fishros2/openrave

其中--rm选项是指在容器退出后自动删除，防止文件堆积，按需求删留，其他默认即可 

创建工作空间，导入机械臂相关代码。本文使用UR5E机械臂

mkdir -p my_robot
cd my_robot
mkdir src
cd src
git clone -b ros1-legacy https://github.com/IntelRealSense/realsense-ros.git
git clone https://github.com/pal-robotics/ddynamic_reconfigure.git

cd ..
#运行下面的代码,会进入镜像的交互终端,此时不要关闭,我们开始下一步
xhost + && sudo docker run  -it --rm  -v /tmp/.X11-unix:/tmp/.X11-unix --device /dev/snd -e DISPLAY=unix$DISPLAY  -v `pwd`:`pwd`  -w `pwd` fishros2/openrave
#在DOCKER中安装依赖
#依次执行以下命令，来安装rosdepc，一定要给sudo权限，在sudo apt update时有概率失败，多次尝试即可
sudo apt update
sudo apt install python3-pip
sudo pip3 install elirobots transforms3d pytest rosdepc
sudo rosdepc init
sudo rosdepc update
#rosdepc更新提示成功后，进行依赖检查
rosdepc install --from-path src --ignore-src -y -r
#此时会列出编译此工作空间需要但目前没有的依赖，依次复制所有依赖包，以空格间隔依次加在sudo apt-get install命令之后
sudo apt-get install ros-noetic-controller-manager ros-noetic-effort-controllers ros-noetic-gazebo-ros ros-noetic-gazebo-ros-control ros-noetic-joint-state-controller ros-noetic-joint-trajectory-controller ros-noetic-position-controllers ros-noetic-ddynamic-reconfigure ros-noetic-librealsense2 ros-noetic-ur-client-library ros-noetic-pass-through-controllers ros-noetic-scaled-joint-trajectory-controller ros-noetic-speed-scaling-interface ros-noetic-speed-scaling-state-controller ros-noetic-ur-msgs ros-noetic-industrial-robot-status-interface ros-noetic-cartesian-trajectory-controller ros-noetic-force-torque-sensor-controller ros-noetic-industrial-robot-status-controller socat ros-noetic-twist-controller ros-noetic-velocity-controllers
#安装成功后再次检查依赖，会发现依赖已全部安装，可以进行下一步
rosdepc install --from-path src --ignore-src -y -r
#编译
catkin_make
#编译成功后，source环境
source devel/setup.bash

#.生成机械臂dae描述文件
#在终端里输入下面的指令,将URDF文件转换为openrave支持的dae格式,接着修改里面数据的精度信息,5代表小数点后四位.
cd src/universal_robot/ur_description/urdf/
rosrun xacro xacro --inorder -o ur5e.urdf ur5e.xacro
rosrun collada_urdf urdf_to_collada ur5e.urdf ur5e.dae
rosrun moveit_kinematics round_collada_numbers.py ur5e.dae ur5e.dae 5

#转换后可以使用openrave进行可视化(这一步可能会闪退,不过没关系,继续即可)
openrave ur5e.dae

#我们也可以使用命令行来查看机器人的Link和joint
openrave-robot.py ec66_description.dae --info links
openrave-robot.py ec66_description.dae --info joints

#生成IKFAST文件
python `openrave-config --python-dir`/openravepy/_openravepy_/ikfast.py --robot=ur5e.dae --iktype=transform6d --baselink=0 --eelink=9 --savefile=$(pwd)/ikfastur5e.cpp
#结束后可以再当前文件夹生成一个ikfastur5e.cpp文件，这个文件就是针对我们当前的机械臂所生成的快速求解的程序，接着我们可以将其编译成可执行文件进行测试。

编译测试

复制所需的头文件，接着直接使用g++进行编译

cp /usr/local/lib/python2.7/dist-packages/openravepy/_openravepy_/ikfast.h .
g++ ikfastur5e.cpp -o ikfast-ur5e -llapack -std=c++11

编译完成后你应该可以看到一个可执行文件ikfast-ur5e.

输入指令可以看看其使用方法,输入空间中的坐标的旋转矩阵和平移矩阵，即可逆解出关节的角度。

说白了就是你告诉它末端flan坐标系的位置，它告诉你机械臂六个轴在多少度的时候才能到达。

./ikfast-ur5e

 生成ikfast_plugin功能包

重开一个终端不进入DOCKER镜像，使用moveit的工具生成可以被moveit直接调用的功能包。

首先在src目录下创建一个文件夹，我们取名为ikfast_plugins

cd my_robot/src/
mkdir ikfast_plugins
cd ikfast_plugins

此时需要修改 

在my_robot/src下搜ur5e.srdf，第6行改成<robot name="ur5e">
若不修改直接进行命令，会报错显示ur5e与ur5e_robot不匹配

接着我们可以使用moveit的moveit_kinematics下的工具生成代码。

rosrun moveit_kinematics create_ikfast_moveit_plugin.py ur5e manipulator ikfast_plugin_ur5e "world" "flan"  ../universal_robot/ur_description/urdf/ikfastur5e.cpp

 上面需要传几个参数，你可能需要根据你的机械臂做修改

• ur5e 机器人的名称
• manipulator 规划组的名字
• ikfast_plugin_ur5e 功能包的名字
• "world" 运动链的起始link
• "flan" 运动链的结束link
•  ../fmauch_universal_robot/ur_description/urdf/ikfastur5e.cpp上一步生成的配置文件位置（相对目录）

 最终结果

会生成ur5e_manipulator_moveit_ikfast_plugin_description.xml。

<?xml version='1.0' encoding='UTF-8'?>
<library path="lib/ur5e_manipulator_moveit_ikfast_plugin">
  <class name="ur5e_manipulator/IKFastKinematicsPlugin" type="ur5e_manipulator::IKFastKinematicsPlugin" base_class_type="kinematics::KinematicsBase">
    <description>IKFast61 plugin for closed-form kinematics of ur5e manipulator</description>
  </class>
</library>

并且会自动生成 kinematics.yaml文件

manipulator:
  kinematics_solver: ur5e_manipulator/IKFastKinematicsPlugin
  kinematics_solver_search_resolution: 0.005
  kinematics_solver_timeout: 0.005

编译运行改BUG

在DOCKER 镜像空间中编译运行

catkin_make
source devel/setup.bash
roslaunch xxxxx demo.launch robot:=ur5e mode:=fake 

可能会遇到如下错误

/opt/ros/noetic/lib/moveit_ros_move_group/move_group: symbol lookup error: /mnt/d/Code/GitHub/elite_robot_ros/devel/lib//libec66_manipulator_moveit_ikfast_plugin.so: undefined symbol: _ZN16ec66_manipulator17GetFreeParametersEv
[move_group-4] process has died [pid 15625, exit code 127, cmd /opt/ros/noetic/lib/moveit_ros_move_group/move_group --debug __name:=move_group __log:=/home/fishbot/.ros/log/2532bdc0-48a2-11ed-84ef-00155ddc9ce8/move_group-4.log].
log file: /home/fishbot/.ros/log/2532bdc0-48a2-11ed-84ef-00155ddc9ce8/move_group-4*.log

解决办法：

修改 ur5e_manipulator_ikfast_solver.cpp文件，接着修改第392行代码，在其下面加上一行

IKFAST_API int* GetFreeParameters() { return NULL; }

再次编译运行即可