ROS

MoveIt！Tutorials

The Kinematics and Dynamics Library (KDL)

1. orocos KDL√

Orocos KDL offocial site and git
将运动模型和规范简化为参考系相对运动的几何问题，KDL开发了一个独立于应用程序的框架，用于运动链的建模和计算，为几何对象提供类库（点、框架、线），各种系列的运动链以及它们的运动规范和插值。

• Installation Manual 安装手册
  1.要求
  - Eigen2 (线性代数的C++模板库：矩阵，向量，数值求解器和相关算法)
  - sip 4.7.9 和python
  - camke2.6
  2.安装
  git git clone https://github.com/orocos/orocos_kinematics_dynamics.git
  用ROS或CMake构建源代码：
  
• 用户手册

1. 使用几何图元
2. 运动树
   KDL：：Segments → KDL：：Chain或Tree
3. 运动学和动态求解器

• ChainFkSolverPos √ 递归前向运动求解器
• ChainFkSolverVel
• ChainIkSolverVel
• ChainIkSolverPos

2.urdfdom√

1.安装urdfdom_py（Python implementation of the URDF parser）：

sudo apt-get install ros-kinetic-urdfdom-py

2.如何使用urdfdom解析URDF模型 urdfdom python 教程

3.对编写的arm7.urdf文件，使用以下命令测试:

rosrun pykdl_utils kdl_parser.py `rospack find test`/arm7.urdf

结果为：

URDF non-fixed joints: 9; KDL joints: 9
URDF joints: 11; KDL segments: 11
Root link: world; Random end link: Link6
base_link
Link1
Link2
Link3
Link4
Link5
Link6

3.Python中创建KDL tree√

对应python：
wiki tutorias
pykdl_utils
github下载
PyKDL API 参考
pykdl安装 pykdl_utils参考

KDL运动学函数参考KDLKinematics Class Reference

在当前目录下有一个 test.py 文件，直接执行这个 test.py ./test.py
报错：
can’t read /var/mail/future
原因是：使用 ./ + 文件名，系统默认会调用 bash 执行 test.py 而不是 python 解释器
解决方案：在文件目录下执行 python test.py
结果为：

4.C++中创建KDL tree

Start using the KDL parser

1. Building the KDL parser

 $ rosdep install kdl_parser

这将安装kdl_parser的所有外部依赖项。要构建包，运行以下命令:

 $ rosmake kdl_parser

2. Using in your code

首先，将KDL解析器作为依赖项添加到package.xml文件中:

  <package>
    ...
    <build_depend package="kdl_parser" />
    ...
    <run_depend package="kdl_parser" />
    ...
  </package>

要在c++代码中开始使用KDL解析器，包含以下头文件:

 #include <kdl_parser/kdl_parser.hpp>

从urdf构造KDL树有4种方法:

• From a file
• From the parameter server
• From an xml element
• From a URDF model

kdl_parser Namespace Reference
正逆运动学求解函数参考：从URDF到KDL(C++&Python)
(参考trac_ik_examples)修改package.xml以及CMakeLists.txt文件，添加kdl相关库的依赖（urdf、orocos_kdl、kdl_parser、roscpp）
添加编译生成文件的代码

add_executable(fk_test src/fk.cpp)

target_link_libraries(fk_test
  ${catkin_LIBRARIES}
  ${orocos_kdl_LIBRARIES}
)

install(TARGETS fk_test
  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

还可以编写launch文件启动节点，需要添加launch依赖。（TRAC-IK实验详解）

注意：

1. 运动学正解输出的是4×4的坐标变换矩阵，如需变成位姿、四元数或者欧拉角需要进行额外的转换。
2. 运动学逆解的计算需要输入目标位姿和迭代角度初值，初值设置不要太离谱，不然收敛不了。
3. 编译出错的原因：没有添加库的依赖；这些库之前必须安装好；不必要的程序包移到工作空间外。

5.eigen2

我们对Markdown编辑器进行了一些功能拓展与语法支持，除了标准的Markdown编辑器功能，我们增加了如下几点新功能，帮助你用它写博客：

1. 安装sudo apt-get install libeigen3-dev；默认安装路径为/usr/include/eigen3

   cd /usr/include/eigen3
   ls
   $ Eigen  signature_of_eigen3_matrix_library  unsupported  
   

2. 复制命令 sudo cp Eigen/ .. -R ；将Eigen文件夹放在/usr/include 下
3. 编写文件进行测试 vi myeigen.c；（#在哪个文件夹下进行测试？#）

   #include <iostream>
   #include <Eigen/Dense>
   using Eigen::MatrixXd;
   int main()
   {
   MatrixXd m(2,2);
   m(0,0) = 3;
   m(1,0) = 2.5;
   m(0,1) = -1;
   m(1,1) = m(1,0) + m(0,1);
   std::cout << m << std::endl;
   }
   

4. 编译 g++ myeigen.c -o myeigen
5. 运行 ./myeigen
6. 结果

• matlab正解逆解关节笛卡尔在ROS下的实验

MoveIt！中的运动学插件

KDL：

• 优点：可求解封闭情况下逆运动学
• 缺点：速度慢、可能找不到解

TRAC-IK：

• 基于数值解，比KDL求解效率提高
• 每次求解得到的关节位置不一定相同

IKFAST：由Open RAVE运动规划软件提供

• 基于解析解（可以求解任意复杂运动链的运动学方程，并产生特定语音（如C++）的文件后供使用），可以保证每次求解的一致性
• 比较稳定、速度快

1.TRAC-IK

trac-ik
改进了一般运动链的逆运动学。该项目为Orocos KDL(运动学和动力学库)中常用的逆雅可比矩阵方法提供了一个可选的逆运动学求解器。具体来说，KDL的收敛算法是基于牛顿法的，牛顿法在存在关节极限的情况下不能很好地工作，这在许多机器人平台上是常见的。TRAC-IK同时运行两个IK实现。一个是对KDL基于牛顿的收敛算法的简单扩展，该算法通过随机跳跃来检测和消除由于联合限制而导致的局部极小值。另一个是SQP(序列二次规划)非线性优化方法，它使用拟牛顿方法来更好地处理关节极限。默认情况下，当这两种算法收敛到一个答案时，IK搜索立即返回。此外，还提供了距离、操纵性和避碰性的次要约束，以便接收回“最佳”IK解决方案。

TRAC-IK: An Open-Source Library for Improved Solving of Generic Inverse Kinematics
source code
将KDL替换为TRAC-IK，ROS的软件源中已经集成了TRAC-IK的安装包，可以直接使用以下命令安装：

sudo apt-get install ros-kinetic-trac-ik-kinematics-plugin

然后修改机械臂MoveIt!配置功能包下的config/kinematics.yaml文件：

 kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin

再次运行demo.launch，默认加载的就是TRAC-IK运动学插件了
参考正解KDL逆解TRAC-IK

2.IKFAST

环境配置

安装程序

sudo apt-get install cmake g++ git ipython minizip python-dev python-h5py python-numpy python-scipy qt4-dev-tools

安装依赖库

sudo apt-get install libassimp-dev libavcodec-dev libavformat-dev libavformat-dev libboost-all-dev libboost-date-time-dev libbullet-dev libfaac-dev libglew-dev libgsm1-dev liblapack-dev liblog4cxx-dev libmpfr-dev libode-dev libogg-dev libpcrecpp0v5 libpcre3-dev libqhull-dev libqt4-dev libsoqt-dev-common libsoqt4-dev libswscale-dev libswscale-dev libvorbis-dev libx264-dev libxml2-dev libxvidcore-dev

？安装依赖项collada-dom，从源码安装

git clone https://github.com/rdiankov/collada-dom.git
cd collada-dom && mkdir build && cd build
cmake ..
make -j4
sudo make install

安装OpenSceneGraph-3.4

sudo apt-get install libcairo2-dev libjasper-dev libpoppler-glib-dev libsdl2-dev libtiff5-dev libxrandr-dev
git clone https://github.com/openscenegraph/OpenSceneGraph.git --branch OpenSceneGraph-3.4
cd OpenSceneGraph && mkdir build && cd build
cmake .. -DDESIRED_QT_VERSION=4
make -j4 （make -j$(nproc)）
sudo make install

安装Flexible Collision Library 0.5.0

sudo apt-get install libccd-dev
git clone https://github.com/flexible-collision-library/fcl.git
cd fcl
git checkout 0.5.0  # use FCL 0.5.0
mkdir build && cd build
cmake ..
make -j4 （make -j `nproc`）
sudo make install

安装sympy 0.7.1 (注意版本，若版本不对会产生错误)

sudo apt install python-pip
pip install --upgrade --user sympy==0.7.1
pip install --upgrade pip

删除mpmath

sudo apt remove python-mpmath

安装MoveIt! IKFAST

sudo apt-get install ros-kinetic-moveit-kinematics

安装OpenRave

git clone https://github.com/rdiankov/openrave.git --branch master （git clone --branch latest_stable https://github.com/rdiankov/openrave.git）
git checkout 9c79ea260e1c009b0a6f7c03ec34f59629ccbe2c
cd openrave && mkdir build && cd build
cmake -DODE_USE_MULTITHREAD=ON -DOSG_DIR=/usr/local/lib64/ ..
make -j$(nproc)
sudo make install

注意：不建议采用sudo apt-get install ros-kinetic-openrave，会一并安装mpmath，导致后面生成IKFAET文件失败，因此采用源码安装的方式。

补充：若出现错误RPC failed; curl 56 GnuTLS recv error (-54): Error in the pull function. 可运行下面命令解决。

git config --global http.postBuffer 2097152000

配置IKFast插件

MoveIt! IKFast 安装
源安装：（在catkin工作空间）

git clone https://github.com/ros-planning/moveit_ikfast.git

（1）MYROBOT_NAME（机器人名字） - name of robot as in your URDF

export MYROBOT_NAME="arm7"

（2）若机器人模型为xacro格式需先转为urdf格式

rosrun xacro xacro --inorder -o "$MYROBOT_NAME".urdf "$MYROBOT_NAME".urdf.xacro

（3）机器人模型urdf格式转换为dae格式

rosrun collada_urdf urdf_to_collada "$MYROBOT_NAME".urdf "$MYROBOT_NAME".dae

（4）设置精度为小数点后5位，然后保留备份后重新设置dae格式机器人模型描述文件的精度

export IKFAST_PRECISION="5"//保留精度小数点后5位
cp "$MYROBOT_NAME".dae "$MYROBOT_NAME".backup.dae  # create a backup of your full precision dae.
rosrun moveit_kinematics round_collada_numbers.py "$MYROBOT_NAME".dae "$MYROBOT_NAME".dae "$IKFAST_PRECISION"

（5）查看模型

openrave-robot.py "$MYROBOT_NAME".dae --info links

模型三维结构

openrave "$MYROBOT_NAME".dae

（6）选择IK项（默认Transform6D ）
http://openrave.org/docs/latest_stable/openravepy/ikfast/#ik-types

（7）设置运动规划组

export PLANNING_GROUP="arm"

（8）设置运动规划的关节组，以上面的模型关节数据为基础设置

export BASE_LINK="1"
export EEF_LINK="8"

（9）若关节数量大于6需设置一自由关节，若无则无需设置

export FREE_INDEX="1"

（10）设置IKFAST输出路径

export IKFAST_OUTPUT_PATH=`pwd`/ikfast61_"$PLANNING_GROUP".cpp

（11）生成IKFAST求解文件
6轴

python `openrave-config --python-dir`/openravepy/_openravepy_/ikfast.py --robot="$MYROBOT_NAME".dae --iktype=transform6d --baselink="$BASE_LINK" --eelink="$EEF_LINK" --savefile="$IKFAST_OUTPUT_PATH"

7轴

python `openrave-config --python-dir`/openravepy/_openravepy_/ikfast.py --robot="$MYROBOT_NAME".dae --iktype=transform6d --baselink="$BASE_LINK" --eelink="$EEF_LINK" --freeindex="$FREE_INDEX" --savefile="$IKFAST_OUTPUT_PATH"

备注：生成IKFAST文件时间一般较长，在半个小时左右，若生成失败可以降低精度或者需要修改模型本身。
（12）生成插件

export MOVEIT_IK_PLUGIN_PKG="$MYROBOT_NAME"_ikfast_"$PLANNING_GROUP"_plugin
cd ~/catkin_ws/src
catkin_create_pkg "$MOVEIT_IK_PLUGIN_PKG"
rosrun moveit_kinematics create_ikfast_moveit_plugin.py "$MYROBOT_NAME" "$PLANNING_GROUP" "$MOVEIT_IK_PLUGIN_PKG" "$IKFAST_OUTPUT_PATH"

非ROS方式

python /path/to/create_ikfast_moveit_plugin.py <myrobot_name> <planning_group_name>

编译工作空间

catkin_make

注意：create_ikfast_moveit_plugin.py默认该目录下存在名字为"$MYROBOT_NAME"_moveit_config的功能包如下，该功能包为机器人模型通过moveit_setup_assistant配置生成的功能包。

  plan_pkg = robot_name + '_moveit_config'
  plan_pkg_dir = roslib.packages.get_pkg_dir(plan_pkg)
  print 'Loading robot from \''+plan_pkg+'\' package ... '

使用IKFAST插件

修改使用的插件，cd到"$MYROBOT_NAME"_moveit_config功能包的config文件目录下，修改该目录下的kinematics.yaml文件或者采用下面方式找到该文件。

rosed "$MYROBOT_NAME"_moveit_config/config/kinematics.yaml
 
<planning_group>:
kinematics_solver: <myrobot_name>_<planning_group>_kinematics/IKFastKinematicsPlugin
替换
kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin

配置参考古月居
MoveIt!中的运动学求解器
官方配置
Installing OpenRAVE on Ubuntu 14.04
Installing OpenRAVE on Ubuntu 16.04
IKFast: The Robot Kinematics Compiler
IndustrialTutorialsCreate_a_Fast_IK_Solution

MoveIt!运动学插件IKFAST配置
测试

方式一：运行"$MYROBOT_NAME"_moveit_config功能包的demo.launch检查是否成功。

方式二：编译ikfast61_"$PLANNING_GROUP".cpp文件进行测试。