0
$\begingroup$

Rosanswers logo

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, -2500000000*sqrt(6252741728462541289)/6252741728462541289, -1591/25000],[0, 1, 0, 0]]
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 2108, in solveFullIK_6D
    self.testconsistentvalues = self.computeConsistentValues(jointvars,Tfinal,numsolutions=4)
  File "/home/astrid/git/openrave/python/ikfast.py", line 1687, in computeConsistentValues
    psubs.append((s,v.subs(psubs)))
  File "/usr/lib/python2.7/dist-packages/sympy/core/basic.py", line 949, in subs
    rv = rv._subs(old, new, **kwargs)
  File "/usr/lib/python2.7/dist-packages/sympy/core/cache.py", line 93, in wrapper
    r = func(*args, **kw_args)
  File "/usr/lib/python2.7/dist-packages/sympy/core/basic.py", line 1063, in _subs
    rv = fallback(self, old, new)
  File "/usr/lib/python2.7/dist-packages/sympy/core/basic.py", line 1035, in fallback
    arg = arg._subs(old, new, **hints)
  File "/usr/lib/python2.7/dist-packages/sympy/core/cache.py", line 93, in wrapper
    r = func(*args, **kw_args)
  File "/usr/lib/python2.7/dist-packages/sympy/core/basic.py", line 1061, in _subs
    rv = self._eval_subs(old, new)
  File "/home/astrid/git/openrave/python/ikfast.py", line 301, in Pow_eval_subs
    return Pow(self.base._eval_subs(old, new), self.exp._eval_subs(old, new))
  File "/usr/lib/python2.7/dist-packages/sympy/core/cache.py", line 93, in wrapper
    r = func(*args, **kw_args)
  File "/usr/lib/python2.7/dist-packages/sympy/core/power.py", line 94, in __new__
    b = _sympify(b)
  File "/usr/lib/python2.7/dist-packages/sympy/core/sympify.py", line 346, in _sympify
    return sympify(a, strict=True)
  File "/usr/lib/python2.7/dist-packages/sympy/core/sympify.py", line 239, in sympify
    raise SympifyError(a)
sympy.core.sympify.SympifyError: SympifyError: None

And finally here are my questions:

  1. Can anybody point me into the right direction on how to get around this, maybe even what that error means?

  2. What reasons could there be for the KDL version to never find a solution? Does it have anything to do with the number of DOF's in NAO's chain?

  3. Has anything come out of this discussion? Is there maybe an IKFast class for Nao readily available?

  4. Anybody looked further into this?

Thank you in advance for any help!!


Originally posted by evenmarbles on ROS Answers with karma: 111 on 2015-02-13

Post score: 3

$\endgroup$

2 Answers 2

0
$\begingroup$

Rosanswers logo

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.


Originally posted by evenmarbles with karma: 111 on 2015-02-13

This answer was ACCEPTED on the original site

Post score: 0

$\endgroup$
0
$\begingroup$

Rosanswers logo

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_moveit_config/blob/master/tuto/tuto_moveit.rst#use-moveit and clicked on the approximate IK box in RViz ? NAO does not have 6 or more dof so this is required.


Originally posted by Vincent Rabaud with karma: 1111 on 2015-02-14

This answer was NOT ACCEPTED on the original site

Post score: 1


Original comments

Comment by evenmarbles on 2015-02-14:
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.

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.