ubuntu 20.04安装、测试ikfast

ancientcc

参考

一、安装openrave

这里介绍的安装openrave适用于新安装的ubuntu 20.04。至少ubuntu要没安装过python。

1.1 openrave-installation

下载openrave-installation：https://github.com/crigroup/openrave-installation

强烈建议下载最新版，较老openrave-installation不支持20.04，这里须要下载支持20.04的版本，类似出现了下面语句。

# Check ubuntu version
UBUNTU_VER=$(lsb_release -sr)
if [ ${UBUNTU_VER} != '14.04' ] && [ ${UBUNTU_VER} != '16.04' ] && [ ${UBUNTU_VER} != '18.04' ] \
  && [ ${UBUNTU_VER} != '20.04' ]; then
    echo "ERROR: Unsupported Ubuntu version: ${UBUNTU_VER}"
    echo "  Supported versions are: 14.04, 16.04, 18.04, and 20.04"
    exit 1
fi

下载的*.sh中有出现“pip install ...”，为安全，我是在它们前面加了“sudo”。不加或许也能成功吧。

有了4个*.sh脚本后，按顺序执行命令。

$ ./install-dependencies.sh
$ ./install-osg.sh
$ ./install-fcl.sh
$ ./install-openrave.sh

以上4个sh成功执行后，在终端输入下面命令测试。

$ openrave.py --example hanoi

在终端输入“python”，测试下此时python版本，应该是2.7.x。

二、安装ROS Noetic

安装步骤：Ubuntu install of ROS Noetic

安装成功后，继续安装功能包：collada_urdf。

$ sudo apt install ros-noetic-collada-urdf

注意“PACKAGE”中的横杆。在目录名/opt/ros/noetic/share/collada_urdf，横杆是下横杆，但在apt install，横杆是中横杆。

 

三、用UR5模型，生成C++逆运动学求解源码

3.1 下载UR5模型

下载universal_robot：https://github.com/ros-industrial/universal_robot。本文只用到当中<universal_robot>/ur_description目录，模型ur5_joint_limited_robot.urdf.xacro。

3.2 基于ur_description，创建或加入私有功能包

以创建私有工作包为例，假设要创建的私有功能包名称是moveit_ws。

$ mkdir ~p ~/moveit_ws/src
$ cd ~/moveit_ws/src
$ catkin_init_workspace
$ cd ~/moveit_ws
$ catkin_make
$ source devel/setup.bash

catkin_make成功后，把目录<universal_robot>/ur_description复制到~/moveit_ws/src，再重新catkin_make。

3.3 xacro格式转换成urdf格式

$ cd ~/moveit_ws/src/ur_description/urdf
$ rosrun xacro xacro --inorder -o ur5.urdf ur5_joint_limited_robot.urdf.xacro

转换成功后，可执行“urdf_to_graphiz ur5.urdf”生成ur5.pdf，打开pdf查看该模型整体结构。

3.4 urdf格式转换成dae格式

$ rosrun collada_urdf urdf_to_collada ur5.urdf ur5.dae

设置精度

$ export IKFAST_PRECISION="5"
$ rosrun moveit_kinematics round_collada_numbers.py ur5.dae ur5.dae "$IKFAST_PRECISION"

查看关节数据

$ openrave-robot.py ur5.dae --info links

查看三维模型

$ openrave ur5.dae

3.5 生成IKFast C++文件

$ sudo python `openrave-config --python-dir`/openravepy/_openravepy_/ikfast.py --robot=ur5.dae --iktype=transform6d --baselink=0 --eelink=9 --savefile=$(pwd)/ikfast61.cpp

注意要加“sudo”，sympy模块的路径在“/root/.local/lib/python2.7/site-packages”(0.7.1)，要有管理权限才能读取。没有权限会报找不到sympy模块。

Traceback (most recent call last):
  File "/usr/local/lib/python2.7/dist-packages/openravepy/_openravepy_/ikfast.py", line 179, in <module>
    from sympy import __version__ as sympy_version
ImportError: No module named sympy

上面生成ikfast C++源码的时候，用到了openravepy模块，其安装路径为：/usr/local/lib/python2.7/dist-packages/openravepy。

$ sudo python `openrave-config --python-dir`/openravepy/_openravepy_/ikfast.py --help

运行上面可查看ikfash.py参数。

3.6 使用ikfast61.cpp求逆解

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

注

  • IKFast 算法依赖于LAPACK（Linear Algebra PACKag）库，所以编译的时候要链接 liblapack.so 库；
  • 输入为 3x4 矩阵（rotation 3x3， translation 3x1）
r00 r01 r02 t1 r10 r11 r12 t2 r20 r21 r22 t3

# 转换成 3x4 矩阵
r00  r01  r02  t1
r10  r11  r12  t2
r20  r21  r22  t3

生成 8 组解

$ ./ikfast 0.04071115 -0.99870914 0.03037599 0.4720009 -0.99874455 -0.04156303 -0.02796067 0.12648243 0.0291871 -0.02919955 -0.99914742 0.43451169

3.7 用正向运动学方程进行验证

上面每一组解都表示一个正确的6关节状态。注意最后一个关节ee_fixed_point，虽然类型是fixed，但它有非零(xyz, rpy)，计算时也须要包含该关节。

Eigen::Isometry3d chain_getSegment_pose(const Eigen::Vector3d& xyz, const Eigen::Vector3d& rpy, const Eigen::Vector3d& axis_tag_xyz, double angle)
{
	Eigen::AngleAxisd rollAngle(Eigen::AngleAxisd(rpy(0), Eigen::Vector3d::UnitX()));
	Eigen::AngleAxisd pitchAngle(Eigen::AngleAxisd(rpy(1), Eigen::Vector3d::UnitY()));
	Eigen::AngleAxisd yawAngle(Eigen::AngleAxisd(rpy(2), Eigen::Vector3d::UnitZ()));

	Eigen::Isometry3d F_parent_jnt = Eigen::Isometry3d::Identity();
	F_parent_jnt.rotate(yawAngle * pitchAngle * rollAngle);
	Eigen::Vector3d _axis = F_parent_jnt.rotation() * axis_tag_xyz;
	Eigen::Vector3d axis = _axis / _axis.norm();

	Eigen::Isometry3d joint_pose = Eigen::Isometry3d::Identity();
	joint_pose.rotate(Eigen::AngleAxisd(angle, axis));
	joint_pose.pretranslate(xyz);

	Eigen::Isometry3d f_tip = Eigen::Isometry3d::Identity();
	f_tip.rotate(F_parent_jnt.rotation());

	Eigen::Isometry3d result = joint_pose * f_tip;
	return result;
}

void verify_ur5()
{
	double angle0[6] = {-2.508279331875612, -3.013057342196758, 1.224580428339185, 1.736242820305415, 0.593205998702227, 0.072539035191919};
	std::vector<double*> angles;
	angles.push_back(angle0);
	int at = 0;
	for (std::vector<double*>::const_iterator it = angles.begin(); it != angles.end(); ++ it, at ++) {
		double* angle = *it;
		char buf[128];
		SDL_snprintf(buf, sizeof(buf), "%.15f, %.15f, %.15f, %.15f, %.15f, %.15f",
			angle[0], angle[1], angle[2], angle[3], angle[4], angle[5]);
		SDL_Log("[%i/%i]%s---", at, (int)angles.size(), buf);
		result = Eigen::Isometry3d::Identity();
		// SDL_Log("Joint: shoulder_pan_joint");
		Eigen::Vector3d rpy = Eigen::Vector3d(0.0, 0.0, 0.0);
		Eigen::Vector3d xyz = Eigen::Vector3d(0.0, 0.0, 0.089159);
		Eigen::Vector3d axis_tag_xyz = Eigen::Vector3d(0, 0, 1);
		result = result * chain_getSegment_pose(xyz, rpy, axis_tag_xyz, angle[0]);
		// SDL_Log("Joint: shoulder_lift_joint");
		rpy = Eigen::Vector3d(0.0, 1.5707963267948966, 0.0);
		xyz = Eigen::Vector3d(0.0, 0.13585, 0.0);
		axis_tag_xyz = Eigen::Vector3d(0, 1, 0);
		result = result * chain_getSegment_pose(xyz, rpy, axis_tag_xyz, angle[1]);
		// SDL_Log("Joint: elbow_joint");
		rpy = Eigen::Vector3d(0.0, 0.0, 0.0);
		xyz = Eigen::Vector3d(0.0, -0.1197, 0.425);
		axis_tag_xyz = Eigen::Vector3d(0, 1, 0);
		result = result * chain_getSegment_pose(xyz, rpy, axis_tag_xyz, angle[2]);
		// SDL_Log("Joint: wrist_1_joint");
		rpy = Eigen::Vector3d(0.0, 1.5707963267948966, 0.0);
		xyz = Eigen::Vector3d(0.0, 0.0, 0.39225);
		axis_tag_xyz = Eigen::Vector3d(0, 1, 0);
		result = result * chain_getSegment_pose(xyz, rpy, axis_tag_xyz, angle[3]);
		// SDL_Log("Joint: wrist_2_joint");
		rpy = Eigen::Vector3d(0.0, 0.0, 0.0);
		xyz = Eigen::Vector3d(0.0, 0.093, 0.0);
		axis_tag_xyz = Eigen::Vector3d(0, 0, 1);
		result = result * chain_getSegment_pose(xyz, rpy, axis_tag_xyz, angle[4]);
		// SDL_Log("Joint: wrist_3_joint");
		rpy = Eigen::Vector3d(0.0, 0.0, 0.0);
		xyz = Eigen::Vector3d(0.0, 0.0, 0.09465);
		axis_tag_xyz = Eigen::Vector3d(0, 1, 0);
		result = result * chain_getSegment_pose(xyz, rpy, axis_tag_xyz, angle[5]);
		// SDL_Log("Joint: ee_fixed_joint");
		rpy = Eigen::Vector3d(0.0, 0.0, 1.5707963267948966);
		xyz = Eigen::Vector3d(0.0, 0.0823, 0.0);
		axis_tag_xyz = Eigen::Vector3d(0, 0, 1);
		result = result * chain_getSegment_pose(xyz, rpy, axis_tag_xyz, 0);

		std::stringstream ss;
		ss << "total result's matrix = \n";
		ss << result.matrix();
		SDL_Log("%s", ss.str().c_str());
	}
}

以下是执行后的output输出，可看到计算出的result就是执行“./ikfast”时的参数。

INFO: [0/1]-2.508279331100000, -3.013057342000000, 1.224580428000000, 1.736242820000000, 0.593205998000004294967295, 0.072539035000000---
INFO: total result's matrix = 
 0.0407112  -0.998709  0.0303765   0.472003
 -0.998745  -0.041563 -0.0279603   0.126479
 0.0291867 -0.0292001  -0.999147   0.434511
         0          0          0          1

 

四、生成moveit_ikfast_plugin功能包

$ cd ~/moveit_ws/src
$ export MOVEIT_IK_PLUGIN_PKG="moveit_ikfast_plugin"
$ catkin_create_pkg "$MOVEIT_IK_PLUGIN_PKG"

上面语句的功能是在~/moveit_ws/src添加功能包moveit_ikfast_plugin。下面填充功能包内容，假设成功world-->ee_link的规划组叫“robot_arm”。

$ cd ~/moveit_ws/src/moveit_ikfast_plugin
$ export MYROBOT_NAME="ur5"
$ export PLANNING_GROUP="robot_arm"
$ export IKFAST_OUTPUT_PATH=~/moveit_ws/src/ur_description/urdf/ikfast61.cpp
$ rosrun moveit_kinematics create_ikfast_moveit_plugin.py "$MYROBOT_NAME" "$PLANNING_GROUP" "$MOVEIT_IK_PLUGIN_PKG" "world" "ee_link" "$IKFAST_OUTPUT_PATH"

成功后，会在~/moveit_ws/src/moveit_ikfast_plugin/src生成两个文件。

  • ur5_robot_arm_ikfast_moveit_plugin.cpp。创建一个可用在moveit的，针对“ur5”模型、“robot_arm”规划组，派生于KinematicsBase的运动学求解器。
  • ur5_robot_arm_ikfast_solver.cpp。它的内容就是ikfast61.cpp。

执行catkin_make后，“rospack find moveit_ikfast_plugin”可得到moveit_ikfast_plugin功能包的有效路径。

发布于 2022-04-22 19:39:49

全部评论: 0 条

    写评论: