5-Dof Arm - Tuple index out of range error while generating IKfast

classic Classic list List threaded Threaded
9 messages Options
Reply | Threaded
Open this post in threaded view
|

5-Dof Arm - Tuple index out of range error while generating IKfast

tarang27
Hi, I am using the following command to generate an IK solution for my 5 dof robot. I am using a 5dof robot, the image is attached here
 python `openrave-config --python-dir`/openravepy/_openravepy_/ikfast.py --robot=./robots/mybot_v1.dae --iktype=translationdirection5d --baselink=0 --eelink=5 --savefile=ikfast61_code.cpp
I have used the ROS moveit_ikfast packages to convert my URDF to DAE. As evident from the command, I am using the "translationdirection5d" mode. I have installed Openrave from source, using the "latest_stable" branch The program starts running but it fails with the following error
2017-01-05 11:57:40,199 openrave [WARN] [environment-core.h:2423 _SimulationThread] timeout of UpdatePublishedBodies
2017-01-05 11:57:40,209 openrave [WARN] [environment-core.h:2423 _SimulationThread] timeout of UpdatePublishedBodies
Traceback (most recent call last):
  File "/usr/local/lib/python2.7/dist-packages/openravepy/_openravepy_/ikfast.py", line 9521, in 
    chaintree = solver.generateIkSolver(options.baselink,options.eelink,options.freeindices,solvefn=solvefn)
  File "/usr/local/lib/python2.7/dist-packages/openravepy/_openravepy_/ikfast.py", line 2281, in generateIkSolver
    chaintree = solvefn(self, LinksRaw, jointvars, isolvejointvars)
  File "/usr/local/lib/python2.7/dist-packages/openravepy/_openravepy_/ikfast.py", line 2796, in solveFullIK_TranslationDirection5D
    coupledsolutions,usedvars = solvemethod(rawpolyeqs2[index],newsolvejointvars,endbranchtree=[AST.SolverSequence([endbranchtree2])], AllEquationsExtra=AllEquations)
  File "/usr/local/lib/python2.7/dist-packages/openravepy/_openravepy_/ikfast.py", line 5226, in solveLiWoernleHiller
    hassinglevariable |= any([all([__builtin__.sum(monom)==monom[i] for monom in newpeq.monoms()]) for i in range(3)])
IndexError: tuple index out of range
Is there an error with my DAE file? or is the issue in the ikfast code? It'd be great if anyone could help me with this issue.
Reply | Threaded
Open this post in threaded view
|

Re: 5-Dof Arm - Tuple index out of range error while generating IKfast

sirop
Check your Collada file with

openrave.py -i head_chassisRot32bit.dae

and get sure that all joints and manipulators are at the right places.

It is easier to make your own OpenRave Robot XML than to guess what could have gone wrong
during the URDF->Collada conversion. See http://openrave.programmingvision.com/wiki/index.php/Format:XML .
Reply | Threaded
Open this post in threaded view
|

Re: 5-Dof Arm - Tuple index out of range error while generating IKfast

sirop
In reply to this post by tarang27
Link indexing is wrong?

Try
openrave.py -i YourRobot.dae
>robot.GetLinks()
Reply | Threaded
Open this post in threaded view
|

Re: 5-Dof Arm - Tuple index out of range error while generating IKfast

tarang27
Link indexing seems to be fine, this is what I get when I run robot.GetLinks()

In [3]: robot.GetLinks()
Out[3]: 
[RaveGetEnvironment(1).GetKinBody('octobot_urdf').GetLink('base_link'),
 RaveGetEnvironment(1).GetKinBody('octobot_urdf').GetLink('link1'),
 RaveGetEnvironment(1).GetKinBody('octobot_urdf').GetLink('link2'),
 RaveGetEnvironment(1).GetKinBody('octobot_urdf').GetLink('link3'),
 RaveGetEnvironment(1).GetKinBody('octobot_urdf').GetLink('link4'),
 RaveGetEnvironment(1).GetKinBody('octobot_urdf').GetLink('link5')]

Doesn't seem to be an issue with link indexes... 
Also I loaded the collada file in openrave and the display looks good with all joints at the correct positions.

On Tue, Jan 10, 2017 at 2:40 PM, sirop [via OpenRAVE Users List] <[hidden email]> wrote:
Link indexing is wrong?

Try
openrave.py -i YourRobot.dae
>robot.GetLinks()


To unsubscribe from 5-Dof Arm - Tuple index out of range error while generating IKfast, click here.
NAML

Reply | Threaded
Open this post in threaded view
|

Re: 5-Dof Arm - Tuple index out of range error while generating IKfast

sirop
Does your robot model follow the OpneRAVE conventions as explained in
http://openrave.org/docs/latest_stable/geometric_conventions/#robot-conventions?
Reply | Threaded
Open this post in threaded view
|

Re: 5-Dof Arm - Tuple index out of range error while generating IKfast

tarang27
I think it does follow the conventions. 
My dae was generated from a URDF file, using the ROS urdf to collada converter. This URDF was generated from my CAD from another ROS plugin. 
Also, I wrote a URDF file myself with the same joint axes and configuration, and it worked properly, hence the ROS urdf to collada plugin works properly. The only difference between the file which works and this one is that the visual and collision geometries are different. All joint positions are the same

The working URDF had simplified cylinder based geometries, and this one(which gives the above error) had STL geometries. In a URDF, these geometries are only for visual display and collision hence I don't think they may be causing any major errors.

Reply | Threaded
Open this post in threaded view
|

Re: 5-Dof Arm - Tuple index out of range error while generating IKfast

sirop
tarang27 wrote
I think it does follow the conventions.
My dae was generated from a URDF file, using the ROS urdf to collada
converter. This URDF was generated from my CAD from another ROS plugin.
Just because you succeeded to convert URDF to DAE, does not mean that
http://openrave.org/docs/latest_stable/geometric_conventions/#robot-conventions are observed.

I work also with a 5 DOF robot, but had no luck when converting URDF to Collada,
see https://github.com/ros/robot_model/issues/169 .
As for your problem with collision geometry, maybe, this https://github.com/rdiankov/openrave/issues/281#issuecomment-27140984 can help.

Anyway...

I just looked again at the pic of your robot you posted in the very first post.
Suppose, you want to rotate the EE around the Z axis from the initial position as in the pic, which joint would you use?
Is there more than one joint providing this rotation?

If so, than it's worth trying out a 3 DOF IKfast by limiting the "arm" of your manipulator(end effector) to only 3 joints.

If all this fails there is still the good old (pseudo) inverse Jacobian...
Reply | Threaded
Open this post in threaded view
|

Re: 5-Dof Arm - Tuple index out of range error while generating IKfast

sirop
In reply to this post by tarang27
tarang27 wrote
I think it does follow the conventions.
My dae was generated from a URDF file, using the ROS urdf to collada
converter. This URDF was generated from my CAD from another ROS plugin.
Also, I wrote a URDF file myself with the same joint axes and
configuration, and it worked properly, hence the ROS urdf to collada plugin
works properly.
Can you please reveal if you used URDF to Collada converter within Indigo or Kinetic?
We could then close https://github.com/ros/robot_model/issues/169 .
Reply | Threaded
Open this post in threaded view
|

Re: 5-Dof Arm - Tuple index out of range error while generating IKfast

tarang27
I converted it in indigo.

On Fri, Jan 27, 2017 at 2:17 PM, sirop [via OpenRAVE Users List] <[hidden email]> wrote:
tarang27 wrote
I think it does follow the conventions.
My dae was generated from a URDF file, using the ROS urdf to collada
converter. This URDF was generated from my CAD from another ROS plugin.
Also, I wrote a URDF file myself with the same joint axes and
configuration, and it worked properly, hence the ROS urdf to collada plugin
works properly.
Can you please reveal if you used URDF to Collada converter within Indigo or Kinetic?
We could then close https://github.com/ros/robot_model/issues/169 .



To unsubscribe from 5-Dof Arm - Tuple index out of range error while generating IKfast, click here.
NAML