Sawyer arm: raise self.CannotSolveError('need 6 joints') CannotSolveError('need 6 joints')

classic Classic list List threaded Threaded
16 messages Options
Reply | Threaded
Open this post in threaded view
|  
Report Content as Inappropriate

Sawyer arm: raise self.CannotSolveError('need 6 joints') CannotSolveError('need 6 joints')

Pi Robot
Hello,

I'm trying to generate an ikfast solver for our Sawyer arm from Rethink robotics.  I'm working on Ubuntu 14.04 and ROS Indigo and I followed these instructions for installing OpenRave from source.

I then followed these instructions for generating the ikfast solver for the Sawyer arm.  The instructions are for the two-armed Baxter but they are fairly easy to tweak for the Sawyer arm.

For the final step I run:

python `openrave-config --python-dir`/openravepy/_openravepy_/ikfast.py --robot=sawyer_demo_urdf_rounded.dae --iktype=transform6d --baselink=0 --eelink=22 --freeindex=14 --savefile=ikfast61_right_arm_.cpp

Things chug along for about 10 minutes but then abort with the following error:

Traceback (most recent call last):
  File "/usr/local/lib/python2.7/dist-packages/openravepy/_openravepy_/ikfast.py", line 6382, in <module>
    chaintree = solver.generateIkSolver(options.baselink,options.eelink,options.freeindices,solvefn=solvefn)
  File "/usr/local/lib/python2.7/dist-packages/openravepy/_openravepy_/ikfast.py", line 1640, in generateIkSolver
    chaintree = solvefn(self, LinksRaw, jointvars, isolvejointvars)
  File "/usr/local/lib/python2.7/dist-packages/openravepy/_openravepy_/ikfast.py", line 2112, in solveFullIK_6D
    raise self.CannotSolveError('need 6 joints')
__main__.CannotSolveError: 'need 6 joints'

I have attached my URDF file for the Sawyer arm.

--patrick

sawyer_demo_urdf.xml
Reply | Threaded
Open this post in threaded view
|  
Report Content as Inappropriate

Re: Sawyer arm: raise self.CannotSolveError('need 6 joints') CannotSolveError('need 6 joints')

sirop
--baselink=0 --eelink=22

How many joints does this arm have?
Reply | Threaded
Open this post in threaded view
|  
Report Content as Inappropriate

Re: Sawyer arm: raise self.CannotSolveError('need 6 joints') CannotSolveError('need 6 joints')

Pi Robot
It's a 7 DOF arm but there are links for the base and torso:

$ openrave-robot.py sawyer_demo_urdf_rounded.dae --info links

name                index parents            
---------------------------------------------
base                0                        
controller_box      1     base               
pedestal_feet       2     base               
pedestal            3     base               
right_arm_base_link 4     base               
right_l0            5     right_arm_base_link
head                6     right_l0           
screen              7     head               
head_camera         8     head               
right_l1            9     right_l0           
right_l1_2          10    right_l1           
right_l2            11    right_l1           
right_l2_2          12    right_l2           
right_l3            13    right_l2           
right_l4            14    right_l3           
right_arm_itb       15    right_l4           
right_l4_2          16    right_l4           
right_l5            17    right_l4           
right_hand_camera   18    right_l5           
right_l6            19    right_l5           
right_hand          20    right_l6           
right_gripper_base  21    right_hand         
right_gripper       22    right_gripper_base 
right_wrist         23    right_l5           
right_torso_itb     24    right_l0           
torso               25    base               
---------------------------------------------
name                index parents            
Reply | Threaded
Open this post in threaded view
|  
Report Content as Inappropriate

Re: Sawyer arm: raise self.CannotSolveError('need 6 joints') CannotSolveError('need 6 joints')

sirop
Sometimes the conversion from URDF to Collada does not work properly.
Did you check the Collada robot and all its joints in Openrave viewer ?
 
What does robot.GetJoints() print out?


Reply | Threaded
Open this post in threaded view
|  
Report Content as Inappropriate

Re: Sawyer arm: raise self.CannotSolveError('need 6 joints') CannotSolveError('need 6 joints')

Pi Robot
The output from robot.GetJoints() is:

[RaveGetEnvironment(1).GetKinBody('sawyer').GetJoint('right_j0'),
 RaveGetEnvironment(1).GetKinBody('sawyer').GetJoint('head_pan'),
 RaveGetEnvironment(1).GetKinBody('sawyer').GetJoint('right_j1'),
 RaveGetEnvironment(1).GetKinBody('sawyer').GetJoint('right_j2'),
 RaveGetEnvironment(1).GetKinBody('sawyer').GetJoint('right_j3'),
 RaveGetEnvironment(1).GetKinBody('sawyer').GetJoint('right_j4'),
 RaveGetEnvironment(1).GetKinBody('sawyer').GetJoint('right_j5'),
 RaveGetEnvironment(1).GetKinBody('sawyer').GetJoint('right_j6')]

and the robot looks OK in the viewer as shown below:

Reply | Threaded
Open this post in threaded view
|  
Report Content as Inappropriate

Re: Sawyer arm: raise self.CannotSolveError('need 6 joints') CannotSolveError('need 6 joints')

fred91
This post was updated on .
Hi Patrick,

I am using the sawyer arm in openrave too. I import the model using a handy plugin made by the personal robotics guys: https://github.com/personalrobotics/or_urdf

Importantly, I removed all the pedestal links/ joints from the urdf file before running ikfast. The srdf will require some modification too to work with the above plugin. If you would like my modified urdf/ srdf files let me know and I'll email them to you.

I then use the following commands in a python script (generating the ik database this way has some advantages - see ikfast documentation):

`
ikmodel = openravepy.databases.inversekinematics.InverseKinematicsModel(robot, iktype=openravepy.IkParameterization.Type.Transform6D)

if not ikmodel.load():
        ikmodel.autogenerate()

`

If you prefer the command line method, try removing all the pedestal bodies from the urdf file before converting to collada. I believe you don't need to explicitly specify any free indices if you remove the pedestal bodies, ikfast will figure it out for you. Obviously the link ID's will change too.

Another thing to check is that you are using sympy version 0.7.1. Ikfast doesn't seem to play nice with other versions.

Cheers,
Fred
Reply | Threaded
Open this post in threaded view
|  
Report Content as Inappropriate

Re: Sawyer arm: raise self.CannotSolveError('need 6 joints') CannotSolveError('need 6 joints')

sirop
In reply to this post by Pi Robot
Reply | Threaded
Open this post in threaded view
|  
Report Content as Inappropriate

Re: Sawyer arm: raise self.CannotSolveError('need 6 joints') CannotSolveError('need 6 joints')

Pi Robot
In reply to this post by fred91
Hi Fred,

Thanks for the suggestions.  I will try them later today or this weekend or report back to the list.

--patrick
Reply | Threaded
Open this post in threaded view
|  
Report Content as Inappropriate

Re: Sawyer arm: raise self.CannotSolveError('need 6 joints') CannotSolveError('need 6 joints')

Pi Robot
In reply to this post by sirop
Thanks sirop,

I'll give that a try later today or on the weekend and let you know how it goes.

--patrick
Reply | Threaded
Open this post in threaded view
|  
Report Content as Inappropriate

Re: Sawyer arm: raise self.CannotSolveError('need 6 joints') CannotSolveError('need 6 joints')

Pi Robot
In reply to this post by fred91
Hi Fred,

I began to try your suggestion but I think we are working on different problems.  I'm not trying to use the Sawyer arm in OpenRAVE.  Instead, I'm trying to generate an IKFast plugin for use in MoveIt! following these instructions.  (These are for the Baxter robot but they are fairly easy to modify for Sawyer.)

Also, I checked my version of sympy and it is 0.7.1.

--patrick
Reply | Threaded
Open this post in threaded view
|  
Report Content as Inappropriate

Re: Sawyer arm: raise self.CannotSolveError('need 6 joints') CannotSolveError('need 6 joints')

Pi Robot
In reply to this post by sirop
Hi sirop,

I'm not sure how to import the model of my arm since it is in collada format (.dae file).

In the meantime, I reduced my original URDF even further so now it contains only the arm links and joints but I still get the error "__main__.CannotSolveError: 'need 6 joints'" when trying to generate the IKFast plugin.

BTW, I tried generating an IKFast plugin from another URDF I have (with only 6-DOF instead of 7) and it worked fine so at least I known my OpenRAVE and ikfast installations are working.

--patrick
Reply | Threaded
Open this post in threaded view
|  
Report Content as Inappropriate

Re: Sawyer arm: raise self.CannotSolveError('need 6 joints') CannotSolveError('need 6 joints')

sirop
Check this  https://github.com/rdiankov/openrave/blob/master/src/data/youbot1.env.xml#L100-L104
as an example of including a  Collada file.
Reply | Threaded
Open this post in threaded view
|  
Report Content as Inappropriate

Re: Sawyer arm: raise self.CannotSolveError('need 6 joints') CannotSolveError('need 6 joints')

Pi Robot
Thanks for the link.  I gave that a try and now my model loads (see screen capture below) and generating the IK solver hangs (I've been waiting over 30 minutes on an 3.6 Ghz i7 with 64 Gb RAM).  The screen messages look like this:

WARNING: QApplication was not created in the main() thread.
/usr/local/lib/python2.7/dist-packages/openravepy/_openravepy_/ikfast.py:1123: RuntimeWarning: invalid value encountered in divide
  axisangle /= angle
No handlers could be found for logger "openravepy.ikfast"

Seems to hang forever at this point...

My python test file looks like this:

"""Shows how to get all 6D IK solutions.
"""
from openravepy import *
import numpy, time
env = Environment() # create the environment
env.SetViewer('qtcoin') # start the viewer
env.Load('./sawyer_arm_env.xml') # load a scene
robot = env.GetRobots()[0] # get the first robot

manip = robot.SetActiveManipulator('right_arm') # set the manipulator to right_arm
ikmodel = databases.inversekinematics.InverseKinematicsModel(robot,iktype=IkParameterization.Type.Transform6D)
if not ikmodel.load():
    ikmodel.autogenerate()
    
print "IK solver created!"

with env: # lock environment
    Tgoal = numpy.array([[0,-1,0,-0.21],[-1,0,0,0.04],[0,0,-1,0.92],[0,0,0,1]])
    sol = manip.FindIKSolution(Tgoal, IkFilterOptions.CheckEnvCollisions) # get collision-free solution
    with robot: # save robot state
        robot.SetDOFValues(sol,manip.GetArmIndices()) # set the current solution
        Tee = manip.GetEndEffectorTransform()
        env.UpdatePublishedBodies() # allow viewer to update new robot
        time.sleep(10)
     
    raveLogInfo('Tee is: '+repr(Tee))

and my .xml file looks like this:

<Environment>
   <Robot name="Sawyer" file="sawyer_arm_only_rounded.dae">
      <translation>2.7 -1 0.003</translation>
      <RotationAxis>0 0 1 -90</RotationAxis>
        
      <Manipulator name="right_arm">
        <!-- joints>right_j0 right_j1 right_j2 right_j3 right_j4 right_j5 right_j6</joints -->
        <base>right_arm_base_link</base> 
        <effector>right_gripper</effector> 
        <closingdirection>1 1 1 0</closingdirection>   
        <direction>0 0 1</direction>  
     </Manipulator>
   </Robot>
</Environment>

Reply | Threaded
Open this post in threaded view
|  
Report Content as Inappropriate

Re: Sawyer arm: raise self.CannotSolveError('need 6 joints') CannotSolveError('need 6 joints')

sirop
Can be a rounding problem. See
rosrun moveit_ikfast round_collada_numbers.py baxter_arm.right.dae baxter_arm.right.rounded.dae 5
in http://sdk.rethinkrobotics.com/wiki/Custom_IKFast_for_your_Baxter#Rounding_DAEs_for_Each_Arm .


Besides it:

InverseKinematicsModel.generate(iktype=None, freejoints=None, freeinc=None, freeindices=None, precision=None, forceikbuild=True, outputlang=None, ipython=False)

I myself do not know the difference between freejoints and  freeindices .
Reply | Threaded
Open this post in threaded view
|  
Report Content as Inappropriate

Re: Sawyer arm: raise self.CannotSolveError('need 6 joints') CannotSolveError('need 6 joints')

fred91
In reply to this post by Pi Robot
The urdf plugin helps you import the model into openrave as a robot kinbody and then use that robot interface for the ikfast database generator, as you have done in your python script. Either way is fine though.

Anwyay, try change <base>right_arm_base_link</base> to <base>base</base> and <effector>right_gripper</effector> to <effector>right_hand</effector> in your xml file (assuming you are using the urdf file from the RethinkRobotics git repo).

I think your python script is fine as it is.
Reply | Threaded
Open this post in threaded view
|  
Report Content as Inappropriate

Re: Sawyer arm: raise self.CannotSolveError('need 6 joints') CannotSolveError('need 6 joints')

Pi Robot
In reply to this post by fred91
Hi Fred,

We still haven't solved this problem and we have tried everything except the plugin you reference below.  It would be great if you could email me your urdf/srdf files for your Sawyer arm since that might be our only hope!

Thanks!
patrick
Loading...