From the openrave web site, "OpenRAVE? provides an environment for testing, developing, and deploying motion planning algorithms in real-world robotics applications." For linuxcnc, we are using the ikfast module to create the IK component, and the qtcoin display application to show the robot model.
The modified version adds a c code generator that works with linuxcnc. Use the --outputlang=c to create the c code.
The ikfast updates to linuxcnc can be found here, https://github.com/ftkalcevic/LinuxCNC, in the "ikfast" branch. There is a new src/emc/kinematics/ikfastkins.c module that uses the ikfast generated code. There is also some example configurations in configs/ikfast-demo directory.
The robot model is used in all openrave computations, including the generation of the ikfast source. Therefore it is important that it is created accurately.
The creator of openrave recommends opening an interactive openrave session and continually reloading the model...
$> openrave.py -i In : env.Reset(); env.Load('simple.robot.detailed.xml')
The recommended way of generating the ik code is to use the openrave.py program, but advanced users can use ikfast.py directly. It is best to generate the cpp component first and use that for automatic testing. Automatic testing doesn't work with .c output.
For example, to generate the 3d translation ik...
$> openrave.py --database inversekinematics --robot=../simple.robot.xml --iktyp e=Translation3D --freejoint=Wrist --freejoint=WristR --numiktests=1000 getIndicesFromJointNames [3, 4] ['Wrist', 'WristR'] openravepy.databases.inversekinematics: generate, Generating inverse kinematics for manip arm: Translation3D [0, 1, 2] (this might take up to 10 min) openravepy.databases.inversekinematics: generate, creating ik file /home/frankt/ .openrave/kinematics.587828202130adcfd9bfdcd543fb17e2/ikfast62.Translation3D.0_1 _2_f3_4.cpp ... Lots more output .... openravepy.databases.inversekinematics: save, inversekinematics generation is do ne, compiled shared object: /home/frankt/.openrave/kinematics.587828202130adcfd9 bfdcd543fb17e2/ikfast62.Translation3D.i686.0_1_2_f3_4.so openravepy.databases.inversekinematics: RunFromParser, testing the success rate of robot ../simple.robot.xml [ikfastmodule.cpp:946] FindIKSolution: No ik solution found, i = 0 Joint Val: 0 0 0 0 0 Transform: 855638019 0 0 0.4799 raw ik command: 0 0 0 0 0 0 0 0 0 0 0 0.4799 0.2606354555368424 0.40057338399291 04 [ikfastmodule.cpp:1001] FindIKSolutions: No ik solution found for, i = 0 Joint Val: 0 0 0 0 0 Transform: 855638019 0 0 0.4799 openravepy.databases.inversekinematics: testik, success rate: 0.999000, wrong so lutions: 0.000000, no solutions: 0.001000, missing solution: 0.000000
Specifying --numiktests runs a series of tests with the ik module to confirm it works correctly. I suspect the output shown above is a bug, as the first test coordinate looks dodgey. Running the tests is important because many ik solutions don't provide a very high success rate. If this happens you'll need to look at changing the iktype, or possibly selecting different free joints.
For more details on how to use openrave, visit the openrave website, www.openrave.org
openrave.py can also do performance testing of the ik solution. Specify the --perftiming= option.
$> openrave.py --database inversekinematics --robot=../simple.robot.xml --ikty pe=Translation3D --freejoint=Wrist --freejoint=WristR --perftiming=1000 ... Lots of output ... openravepy.databases.inversekinematics: RunFromParser, running time mean: 0.000 005s, median: 0.000005s, min: 0.000005s, max: 0.000015s
Showing a mean of about 5us to calculate the ik. This is good. It means ikfast produced a fast closed form solution.
Changing iktype to TranslationZAxisAngle4D? gives....
$> openrave.py --database inversekinematics --robot=../simple.robot.xml --ikty pe=TranslationZAxisAngle4D ... Lots of output... openravepy.databases.inversekinematics: RunFromParser, running time mean: 0.0027 76s, median: 0.002728s, min: 0.001594s, max: 0.005023s
These are terrible times - a mean of 2.7ms and max of 5ms means we can't run with a 1kHz servo loop. Slowing the servo loop down to 200Hz will produce poor motion.
The slow times are because ikfast had to use a numerical inverse kinematics solver. Numerical solvers are a slow iterative process. This limitation is because of the geometry of the robot and the selection of joints.
To generate the ikfast code for linuxcnc, just add the --outputlang=c option, eg...
$> openrave.py --database inversekinematics --robot=../simple.robot.xml --iktyp e=Translation3D --freejoint=Wrist --freejoint=WristR --outputlang=c getIndicesFromJointNames [3, 4] ['Wrist', 'WristR'] openravepy.databases.inversekinematics: generate, Generating inverse kinematics for manip arm: Translation3D [0, 1, 2] (this might take up to 10 min) openravepy.databases.inversekinematics: generate, creating ik file /home/frankt/ .openrave/kinematics.587828202130adcfd9bfdcd543fb17e2/ikfast62.Translation3D.0_1 _2_f3_4.c ... Lots more output ... openravepy.ikfast: writeIkSolver, generating c code... openravepy.databases.inversekinematics: generate, compiling ik file to /home/fra nkt/.openrave/kinematics.587828202130adcfd9bfdcd543fb17e2/ikfast62.Translation3D .i686.0_1_2_f3_4.so openravepy.databases.inversekinematics: generate, cannot continue further if out putlang c is not cpp openravepy.databases.inversekinematics: save, inversekinematics generation is do ne, compiled shared object: /home/frankt/.openrave/kinematics.587828202130adcfd9 bfdcd543fb17e2/ikfast62.Translation3D.i686.0_1_2_f3_4.so
Note the name of the output file in the program logs. The directory name and file name are dependant on the robot model. In this case it is...
To use this file in linuxcnc, copy the file to the linuxcnc source tree...
and rename it from .c to .inc. Also copy the file ikfastkins_c.h.
Edit ikfastkins.c in this directory to include the .inc file. There should already be a line like...
Just change that one.
For a simulator build, edit the main make file (src/Makefile?) and uncomment the line...
#IKFASTLIBS := -llapack
A realtime build is a little more tricky; the kinematics object is a kernel object which has tight restrictions: c only (hence the new ikfast c generator) and no external libraries.
To work around this, there is a patch file, ikfast_fortran_kernel_header.patch, in src/emc/kinematics that has to be applied to the kernel headers to change the kernel makefiles to support compiling fortran. Note - this patch file has only been tested against the linux-headers-2.6.32-122-rtai headers.
Then edit the src/Makefile? and uncomment the very long line that looks like ...
#ikfastkins-objs += emc/kinematics/lapack/lapack/blas/daxpy.o emc/kinematics/lap ack/lapack/blas/dcopy.o emc/kinematics/lapack/lapack/blas/ddot.o emc/kinematics/ lapack/lapack/blas/dgemv.o emc/kinematics/lapack/lapack/blas/dger.o emc/kinemati cs/lapack/lapack/blas/dnrm2.o emc/kinematics/lapack/lapack/blas/drot.o emc/kinem atics/lapack/lapack/blas/dscal.o emc/kinematics/lapack/lapack/blas/dswap.o emc/k ...
The kernel build will now compile the required lapack functions and link them directly with the ikfastkins component.
Any free joints that are determined by ikfast are mapped to free axis (ABCUVW) so these joints can be moved when in world coordinate mode.
A call to ComputeIk?() will usually return more than one solution. ikfastkins will compare the current joint positions to each solution and pick the closest. This is a simple method that can cause joints to flip if the wrong solution is chosen.
If no ik solution is found, ikfastkins returns GO_RESULT_ERROR, which is ignored by motmod - the robot stops moving, but motmod doesn't, until either the cartesian movement finds a new ik solution, or the robot backs up out of the "deadzone".
For a fully defined 6DoF? robot, there is no interface to define the end effector orientation. Currently that has to be defined when in joint mode, then the orientation will be maintained when switching to world mode.
In the config/ikfast-demo directory is a simple python hal script to display robot model, openravedisplay. The script uses the openrave libraries so they must be installed.
The program will automatically create hal pins for each joint (openravedisplay.joint%d). These can be connected to the hal joint positions.
loadusr -W ./openravedisplay --robot=wam.robot.env.xml --refreshrate=15 --showtrail net j0 axis.0.joint-pos-fb openravedisplay.joint0 net j1 axis.1.joint-pos-fb openravedisplay.joint1 net j2 axis.2.joint-pos-fb openravedisplay.joint2 net j3 axis.3.joint-pos-fb openravedisplay.joint3 net j4 axis.4.joint-pos-fb openravedisplay.joint4 net j5 axis.5.joint-pos-fb openravedisplay.joint5 net j6 axis.6.joint-pos-fb openravedisplay.joint6
There are some samples in the config/ikfast-demo directory.
Running robotarm.simple will show the robot above. Edit simple.robot.env.xml to change the model. There are 2 ikfast files in src/emc/kinematics that work with this robot...
ikfast62.Translation3D.0_1_2_f3_4.inc - simple 3d translation
ikfast62.TranslationZAxisAngle4D?.0_1_2_3_f4.inc - slow numerical 4d solution
ikfastkins.c must be edited and the #include line changed.
There is also an example of the Barret Wam 7DoF? robot. This robot is included in the openrave installation. The robot can be run using the robotarm.wam script. Remember to edit ikfastkins.c to include the correct ik module (ikfast62.Transform6D.0_1_3_4_5_6_f2.inc)
Barret Wam 7DoF? robot