Creating ikfast solution cpp for nao

asked 2015-02-13 20:54:47 -0500

evenmarbles
111 ●3 ●6 ●10

Hi,

I am developing on Ubuntu 14.04 and indigo distro working with the Nao robot (I know that this ubuntu version-distro combo still has many issues, however I am stuck with it due to hardware constraints). For my project I need an IK solver, and for that it seemed that MoveIt was the right solution.

I am using nao_description from the nao_robot repository, nao_meshes, and nao_moveit_config.

First of all, one thing I noticed is that when I run roslaunch nao_moveit_config demo.launch, I am getting the following error:

Group 'leg_right' has a mimic joint. Will not initialize dynamics solver

Trying to manipulate the leg_right chain results in crashing the rviz application. However, that may just be a visualization issue.

For the IK, I first tried the default IK solver, KDL, following along the Kinematics tutorial on the MoveIt website, but substituting in Nao's configuration. But it never succeeds to find an IK solution.

Since research was unclear whether KDL is a fit choice for Nao's configuration I decided to try out MoveIt with the ikfast plugin. I followed the instructions. The first issue I ran into was installing OpenRAVE. However I found the following tutorial with which I was able to install OpenRAVE successfully. Next issue was in converting urdf to the collada file with

rosrun collada_urdf urdf_to_collada nao.urdf nao.dae

There seems to be a problem with indigo creating the file, producing the following error:

opt/ros/indigo/share/nao_description/urdf/naoV40_generated_urdf/nao.urdf nao.dae
I/O error : No such file or directory
I/O error : No such file or directory
error : xmlNewTextWriterFilename : cannot open uri

Document successfully written to nao.dae

I was able to overcome this issue by having a friend who is running ros fuerte make the conversion for me.

However, I am now stuck on the step of creating the ikfast solution. When performing the following command:

python ~/git/openrave/python/ikfast.py --robot=nao.rounded.dae --iktype=transform6d --baselink=1 --eelink=8 --savefile= ikfast_arm_hand_left.cpp

[plugindatabase.h:577] Failed to create name ode, interface collisionchecker
[plugindatabase.h:577] Failed to create name bullet, interface collisionchecker
[kinbody.cpp:1275] dof 11 value is not in limits 0.000000e+00<-3.490659e-02
[kinbody.cpp:1264] dof 22 value is not in limits 0.000000e+00<3.490659e-02
/home/astrid/git/openrave/python/ikfast.py:1123: RuntimeWarning: invalid value encountered in divide
  axisangle /= angle
INFO: moved translation Matrix([[0, 0, 0]]) to right end
INFO: moved translation Matrix([[0, 0, 0]]) to left end
INFO: [[1, 0, 0, 0],[0, 1, 0, 0],[0, 0, 1, 253/2000]]
INFO: [[cos(j0), -sin(j0), 0, 0],[sin(j0), cos(j0), 0, 0],[0, 0, 1, 0]]
INFO: [[1, 0, 0, 0],[0, 0, 1, 0],[0, -1, 0, 0]]
INFO: [[cos(j1), -sin(j1), 0, 0],[sin(j1), cos(j1), 0, 0],[0, 0, 1, 0]]
INFO: [[2500000000*sqrt(6252741728462541289)/6252741728462541289, 0, 52361517*sqrt(6252741728462541289)/6252741728462541289, 5871/100000],[52361517*sqrt(6252741728462541289)/6252741728462541289, 0 ...

(more)

edit retag flag offensive close merge delete

add a comment

0

answered 2015-02-13 21:20:13 -0500

evenmarbles
111 ●3 ●6 ●10

updated 2015-02-14 11:03:17 -0500

So apparently the problem was that I did not have the correct/supported version of python-sympy installed. I followed these instructions to prevent the error:

sympy.core.sympify.SympifyError: SympifyError: None

However now I am getting the following error instead:

Traceback (most recent call last):
  File "/home/astrid/git/openrave/python/ikfast.py", line 6382, in <module>
    chaintree = solver.generateIkSolver(options.baselink,options.eelink,options.freeindices,solvefn=solvefn)
  File "/home/astrid/git/openrave/python/ikfast.py", line 1640, in generateIkSolver
    chaintree = solvefn(self, LinksRaw, jointvars, isolvejointvars)
  File "/home/astrid/git/openrave/python/ikfast.py", line 1817, in solveFullIK_Translation3D
    return self._solveFullIK_Translation3D(LinksRaw,jointvars,isolvejointvars,basepos)
  File "/home/astrid/git/openrave/python/ikfast.py", line 1827, in _solveFullIK_Translation3D
    raise self.CannotSolveError('need 3 joints')
__main__.CannotSolveError: 'need 3 joints'

Okay, and with the help of this post I was able to get past this error as well.

So with the command:

python ~/git/openrave/python/ikfast.py --robot=nao.rounded.dae --iktype=translationdirection5d --baselink=1 --eelink=36 --freeindex=32 --savefile=ikfast61_arm_hand_left.cpp

I am now able to create the fastIK cpp file for the arm_hand_left chain. And

python ~/git/openrave/python/ikfast.py --robot=nao.rounded.dae --iktype=transform6d --baselink=1 --eelink=31 --savefile=ikfast_leg_left.cpp

gives me the fastIK cpp file for the leg_left chain.

However, the same does not work for the leg_right chain. When using iktype translationdirection5d I get the error: ValueError: No nonzero pivot found; inversion failed. When using iktype transform6d the error is: CannotSolveError: 'need 6 joints'

So my remaining question is, how to generate the fastIK cpp file for NAO's leg_right chain which contains a mimic joint (which I am assuming is what is causing the problems here).

Any advice and help would be appreciated.

edit flag offensive delete link

add a comment

0

answered 2015-07-19 08:35:26 -0500

vincent78
3 ●6 ●8 ●9

Hi, I followed a very similar process to also get the left arm solver. When running :

python `openrave-config --python-dir`/openravepy/_openravepy_/ikfast.py --robot=/home/vincent/nao/nao.dae --iktype=translationdirection5d --baselink=1 --eelink=35 --savefile=ikfast61_arm_hand_left.cpp

I get (some lines skipped)

[...]
INFO: ikfast translation direction 5d: [j8, j9, j10, j11, j12]
INFO: attempting li/woernle/hiller general ik method
INFO: special structure in equations detected, try to solve through elimination
INFO: [] [j8]
INFO: 'failed to find a variable to solve'
INFO: [] [j9]
INFO: 'failed to find a variable to solve'
INFO: try to solve first two variables pairwise
Traceback (most recent call last):
  File "/usr/lib/python2.7/dist-packages/openravepy/_openravepy_/ikfast.py", line 6121, in <module>
[...]
  File "/usr/lib/python2.7/dist-packages/sympy/core/expr.py", line 129, in __float__
    raise ValueError("Symbolic value, can't compute")
ValueError: Symbolic value, can't compute

I also checked I have the correct version of sympy installed. Any idea what could be wrong ?

edit flag offensive delete link

add a comment

0

answered 2015-02-27 08:47:30 -0500

bigbell
1 ●1

updated 2015-02-28 03:01:55 -0500

I met the same error！

I finally find out that I had too many sympy packages in /usr ,the program may link to wrong version of sympy I guess.

you can run sudo find /usr -name "sympy" in you bash to find out whether if you have redundant packages.

If so , you can delete all sympy packages in /usr and install python-sympy again (you should choose the version you need).

Hope for helping you!

edit flag offensive delete link

Comments

Thanks a lot for your answer ! After installing the latest version of sympy, the error message changed: [...]RuntimeWarning: invalid value encountered in divide axisangle /= angle [...] TypeError: not all arguments converted during string formatting

vincent78 ( 2015-07-22 16:08:13 -0500 )edit

I am getting the same error trying to generate ikfast for lwa4p any sugestions how to solve this problem ?

Chickenman ( 2016-01-30 06:38:57 -0500 )edit

Hey @vincent78 and @Chickenman, I am getting the same error too. Were you able to fix this?

kmohan ( 2016-07-25 16:01:23 -0500 )edit

add a comment

1

answered 2015-02-14 01:55:57 -0500

Vincent Rabaud

1111 ●25 ●20

The leg_right problem is clearly a bug, you should file a bug report. Otherwise, did you follow the tutorials https://github.com/ros-naoqi/nao_move... and clicked on the approximate IK box in RViz ? NAO does not have 6 or more dof so this is required.

edit flag offensive delete link

Comments

Thank you for your reply, Vincent. I reported the bug here.

Furthermore, while I did not follow the tutorial (I wish I had seen it earlier, but somehow in all my search it had eluded me...), I did exactly the steps.

evenmarbles ( 2015-02-14 10:39:34 -0500 )edit

add a comment

Creating ikfast solution cpp for nao

4 Answers

Comments

Comments

Question Tools

Stats

Related questions

Creating ikfast solution cpp for nao edit

4 Answers

Comments

Comments

Question Tools

Stats

Related questions

Creating ikfast solution cpp for nao