ikfast-configuration

​ IKFAST is a very powerful tool when a robot requires inverse kinematic solutions, so it’s necessary for us to config a customized robot ikfast plugin from scratch, which is a basic skill for a robotic engineer, I deem.

1. Configure the ikfast for flexiv robot

​ As first search shows, the ikfast is provided by OpenRAVE which is a very complex planning framework. OpenRAVE requires a tricky installing process, and using not supported environment to install it will cause a lot of frustration. Luckily, there is a docker image to help us get rid of such tedious and troublesome work. I fork the repo to better save the docker image. The docker image provides Ubuntu 14.04 with OpenRAVE 0.9.0 and ROS Indigo installed, which can be used to generate the solver code once. You can browse the docker image by the following command.

1
docker pull kamicode/personalrobotics:ros-openrave

​ The script in moveit_kinematics which is called auto_create_ikfast_moveit_plugin.sh will automatically build the docker environment and prepare the OpenRAVE.

​ Since we have the robot’s urdf, we can use the following command to create the ikfast plugin.

1
2
export MYROBOT_NAME="panda" &
rosrun moveit_kinematics auto_create_ikfast_moveit_plugin.sh --iktype Transform6D $MYROBOT_NAME.urdf <planning_group_name> <base_link> <eef_link>

​ In my case, the command is

1
rosrun moveit_kinematics auto_create_ikfast_moveit_plugin.sh --iktype Transform6D --name flexiv <somewhere>/ws_moveit/src/moveit_resources/flexiv_description/robot.urdf arm base link7

​ I specify the –name attribute, because if I use the absolute path, the auto-extracted name may not be correct.

​ The result after several minute is as follows.

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
Running <somewhere>/ws_moveit/src/moveit/moveit_kinematics/ikfast_kinematics_plugin/scripts/create_ikfast_moveit_plugin.py "flexiv" "arm" "flexiv_arm_ikfast_plugin" "base" "link7" "/tmp/ikfast.rn0uJk/.openrave/kinematics.f82de3cf02e1545e44ec052f9f4778ca/ikfast0x10000049.Transform6D.1_2_3_4_5_6_f0.cpp"
Creating IKFastKinematicsPlugin with parameters:
robot_name: flexiv
base_link_name: base
eef_link_name: link7
planning_group_name: arm
ikfast_plugin_pkg: flexiv_arm_ikfast_plugin
ikfast_output_path: /tmp/ikfast.rn0uJk/.openrave/kinematics.f82de3cf02e1545e44ec052f9f4778ca/ikfast0x10000049.Transform6D.1_2_3_4_5_6_f0.cpp
search_mode: OPTIMIZE_MAX_JOINT
srdf_filename: flexiv.srdf
robot_name_in_srdf: flexiv
moveit_config_pkg: flexiv_moveit_config

Found source code generated by IKFast version 73
Failed to find package: flexiv_arm_ikfast_plugin. Will create it in <somewhere>/ws_moveit/src/moveit/moveit_kinematics/flexiv_arm_ikfast_plugin.
Created ikfast header file at '<somewhere>/ws_moveit/src/moveit/moveit_kinematics/flexiv_arm_ikfast_plugin/include/ikfast.h'
Created ikfast plugin file at '<somewhere>/ws_moveit/src/moveit/moveit_kinematics/flexiv_arm_ikfast_plugin/src/flexiv_arm_ikfast_moveit_plugin.cpp'
Created plugin definition at '<somewhere>/ws_moveit/src/moveit/moveit_kinematics/flexiv_arm_ikfast_plugin/flexiv_arm_moveit_ikfast_plugin_description.xml'
Created cmake file at '<somewhere>/ws_moveit/src/moveit/moveit_kinematics/flexiv_arm_ikfast_plugin/CMakeLists.txt'
Wrote package.xml at '<somewhere>/ws_moveit/src/moveit/moveit_kinematics/flexiv_arm_ikfast_plugin/package.xml'
Created update plugin script at '<somewhere>/ws_moveit/src/moveit/moveit_kinematics/flexiv_arm_ikfast_plugin/update_ikfast_plugin.sh'
Modified kinematics.yaml at '<somewhere>/ws_moveit/src/flexiv_moveit_config/config/kinematics.yaml'

​ Notice the upper script has modified the kinematics.yaml in flexiv_moveit_config, which means the robot will use the ikfast to calculate ik.

1
2
3
4
arm:
kinematics_solver: flexiv_arm/IKFastKinematicsPlugin
kinematics_solver_search_resolution: 0.005
kinematics_solver_timeout: 0.005

​ But if we using the following command to visualize the flexiv robot in RViz, we can find an error which means that the moveit can not load the flexiv-ikfast plugin.

1
roslaunch flexiv_moveit_config demo.launch

​ The error is

1
2
[ERROR] [1636036724.761299732]: The kinematics plugin (arm) failed to load. Error: According to the loaded plugin descriptions the class flexiv_arm/IKFastKinematicsPlugin with base class type kinematics::KinematicsBase does not exist. Declared types are  cached_ik_kinematics_plugin/CachedKDLKinematicsPlugin cached_ik_kinematics_plugin/CachedSrvKinematicsPlugin kdl_kinematics_plugin/KDLKinematicsPlugin lma_kinematics_plugin/LMAKinematicsPlugin prbt_manipulator/IKFastKinematicsPlugin srv_kinematics_plugin/SrvKinematicsPlugin
[ERROR] [1636036724.761371349]: Kinematics solver could not be instantiated for joint group arm.

​ It tells us that MoveIt can not find the flexiv-ikfast plugin. So we goto the /ws_moveit/src/moveit/moveit_kinematics to find out why.

​ There is flexiv_arm_ikfast_plugin indeed in the directory, but it seems not be complied. I try to add some lines to the CMakeList.txt in moveit_kinematics directory but failed in compiling. But I find if I copy the flexiv_arm…._plugin out as a single package to compile. Rviz works without informing “plugin not found”.

2. Validate the output ikfast file works

​ Because RViz will pick available ik solver if it can not find configured ikfast plugin, even if the error message has been eliminated, we can not make sure that the RViz planing do use the ikfast. We need to find an another way.

An article proposes another way for us. That is, use the ikfastdemo.cpp to do the validation. We can simply put the ikfastdemo.cpp in the same directory with our ikfast cpp, and make ikfastdemo.cpp inlcude it.

​ So we add two lines to ikfastdemo.cpp.

1
2
#define IK_VERSION 73
#include "flexiv_arm_ikfast_solver.cpp"

​ And compile it.

1
g++ ikfastdemo.cpp -lstdc++ -llapack -o compute -lrt

​ The usage is as follows.

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
Usage: 

./compute fk j0 j1 ... j6
Returns the forward kinematic solution given the joint angles (in radians).


./compute ik t0 t1 t2 qw qi qj qk free0 ...
Returns the ik solutions given the transformation of the end effector specified by a 3x1 translation (tX), and a 1x4 quaternion (w + i + j + k). There are 1 free parameters that have to be specified.


./compute ik r00 r01 r02 t0 r10 r11 r12 t1 r20 r21 r22 t2 free0 ...
Returns the ik solutions given the transformation of the end effector specified by a 3x3 rotation R (rXX), and a 3x1 translation (tX). There are 1 free parameters that have to be specified.

./compute iktiming
For fixed number of iterations, generates random joint angles, then calculates fk, calculates ik, measures average time taken.

./compute iktiming2
For fixed number of iterations, with one set of joint variables, this finds the ik solutions and measures the average time taken.

2.1 Painful debug time

I use the following command to calculate a forward kinematic solution.

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
./compute fk 1.57 1.57 1.57 1.57 1.57 1.57 1.57

Found fk solution for end frame:
Translation: x: -0.484689 y: 0.475605 z: 0.400080

Rotation 0.001591 0.001596 -0.999997
Matrix: 0.999996 0.002387 0.001595
0.002390 -0.999996 -0.001592

Euler angles:
Yaw: -1.572388 (1st: rotation around vertical blue Z-axis in ROS Rviz)
Pitch: 1.568409
Roll: 0.001595

Quaternion: -0.500596 0.500199 0.500597 -0.498606
-0.500596 + 0.500199i + 0.500597j - 0.498606k (alternate convention)

​ To visualize the solution in the RViz, I set the flexiv robot at the same configuration, and add a box indicating the calculation of the ikfast forward kinematics.

​ It seems the base joint need to rotate 180 degree to fit the result of ikfast fk, which is very weird. I spend a lot of time debugging on the problem. What remains unknown is that the RViz motion planning plugin still works well though the ikfast is not giving the correct answer. So it may not use ikfast or experience some downgrade mode because the failure of finding a solution by ikfast plugin.

​ By chance, I find that the yaw angle of the first fixed joint is strange.

1
2
3
4
5
6
<!-- ============ joint 0 =============== -->
<joint name="joint0" type="fixed">
<origin rpy="0 0 -3.1415927" xyz="0 0 0.0"/>
<parent link="world"/>
<child link="base"/>
</joint>

​ It is exactly 180 degree, which fits our guess. So I change the value of that yaw angle to zero. And everything works. But what cause the difference? Remember we do NOT set the virtual joint when configuring the flexiv in MoveIt Setup Assistant because we find the fixed joint in URDF ? If we did that as the tutorial, it will add following lines into our robot.srdf.

1
2
<!--VIRTUAL JOINT: Purpose: this element defines a virtual joint between a robot link and an external frame of reference (considered fixed with respect to the robot)-->
<virtual_joint child_link="panda_link0" name="virtual_joint" parent_frame="world" type="floating"/>

​ So BEAR IN MIND, IT IS A CONVENTION THAT THE VIRTUAL JOINT SHOULD BE WITH RPY=”0 0 0”!!!

2.2 Result

​ Finally, I find both the joint and the box overlap however I change the joint position, which means the forward kinematics of ikfast calculate the correct result !

3. Add ikfast into pybullet-planning module

​ Let’s first have a glance on the directory structure of the pybullet-planning/pybullet_tools/ikfast

​ We need to add flexiv directory which includes

The ik.py defines some basic information of our new robot.

1
2
3
4
5
6
7
from ..utils import IKFastInfo
from ..ikfast import * # For legacy purposes

FLEXIV_URDF = "models/flexiv_description/flexiv.urdf"

FLEXIV_INFO = IKFastInfo(module_name='flexiv.ikfast_flexiv', base_link='base',
ee_link='link7', free_joints=['joint1'])

The setup.py helps us to compile a pybind11 module, which a shared library .so file. The code is mainly modified from the existed ikfast robot directory.

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
#!/usr/bin/env python

from __future__ import print_function
import sys
import os
sys.path.append(os.path.join(os.pardir, os.pardir, os.pardir))
from pybullet_tools.ikfast.compile import compile_ikfast

# Build C++ extension by running: 'python setup.py'
# see: https://docs.python.org/3/extending/building.html

def main():
# lib name template: 'ikfast_<robot name>'
sys.argv[:] = sys.argv[:1] + ['build']
robot_name = 'flexiv'
compile_ikfast(module_name='ikfast_{}'.format(robot_name),
cpp_filename='ikfast_{}.cpp'.format(robot_name))

if __name__ == '__main__':
main()

​ In the original ikfast_flexiv.cpp, to enable the pybind11, we have to add a lot of functions and include “Python.h”. Luckily the author of pybullet-planning has written it for us, so we only to add these lines at the end.

​ Then we simply use the command

1
2
3
4
$python setup.py build
...
...
ikfast module ikfast_flexiv imported successful

​ We can modify the test codes in pybullet-planning examples to check whether our flexiv robot can use ikfast to calculate now.

​ The result shows we did it !

4. Drawbacks

​ When I used ikfast in real application, I found a serious problem that ikfast demanded a high accuracy for the pos and orn of the end-effector. But in practice, due to the observation error and others, many times we just want ikfast to produce a solution with some tolerance rather than output “NO SOLUTION”. But according to my experience and this question, such problem can only solved by randomly sample some 6D pose within our predefined tolerant field manually. But such way will make a high-speed theoretically solvable inverse kinematics solve to be a sample-based ik-solver again and require some wrapper rather than just use the ikfast.cpp file, which becomes cumbersome again. So after consideration, I deem that a theoretically-solvable ik-solve is of more fancy than more use.

Author

Kami-code

Posted on

2021-11-06

Updated on

2022-01-25

Licensed under

Comments