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 | export MYROBOT_NAME="panda" & |
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 | 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" |
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 | arm: |
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 | [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 |
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 |
And compile it.
1 | g++ ikfastdemo.cpp -lstdc++ -llapack -o compute -lrt |
The usage is as follows.
1 | Usage: |
2.1 Painful debug time
I use the following command to calculate a forward kinematic solution.
1 | ./compute fk 1.57 1.57 1.57 1.57 1.57 1.57 1.57 |
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 | <!-- ============ joint 0 =============== --> |
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 | <!--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)--> |
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 | from ..utils import IKFastInfo |
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 | #!/usr/bin/env python |
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 | $python setup.py build |
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.
ikfast-configuration