Can't convert DMP to RR

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

Can't convert DMP to RR

ra_
Hello everyone.

I am using OpenRave on a 16.04 without ROS. My main goal is to solve fullik for a Universal Robot UR3.

The robot description was written in the OpenRave XML format specifying its kinematics and taking some details from ROS' urdf for geometry. I used the ROS-industrial .STL files from the universal_robot package.

The geometry was verified in the QT/coin simulator and the frames are aligned with the UR kinematic description[1].

I ran this python file:

import openravepy
from openravepy import Environment, ikfast, raveLogInfo, Viewer
from openravepy.misc import InitOpenRAVELogging

InitOpenRAVELogging()
env = Environment()
kinbody = env.ReadRobotXMLFile('new_ur3.robot.xml')
env.Add(kinbody)

solver = ikfast.IKFastSolver(kinbody=kinbody)
chaintree = solver.generateIkSolver(baselink=0,eelink=6,freeindices=[],solvefn=ikfast.IKFastSolver.solveFullIK_6D)
code = solver.writeIkSolver(chaintree)
open('ik.cpp','w').write(code)

The reason I used "freeindices=[]" is that if I leave it out, I get an error:

Traceback (most recent call last):
  File "test_ik_ur.py", line 23, in <module>
    chaintree = solver.generateIkSolver(baselink=0,eelink=6,solvefn=ikfast.IKFastSolver.solveFullIK_6D)
  File "/usr/local/lib/python2.7/dist-packages/openravepy/_openravepy_/ikfast.py", line 2150, in generateIkSolver
    assert(0)
AssertionError


Running this example first gave me a
mpmath.libmp.libhyper.NoConvergence: Didn't converge in maxsteps=50 steps.
 error, so I followed @baxelrod's instructions and uninstalled mpmath [2].
I have a simpy version 0.7.1.

After I ran it for a few hours I get the following error:

    return dmp_strip([ dmp_convert(c, v, K0, K1) for c in f ], u)
  File "/home/r/.local/lib/python2.7/site-packages/sympy/polys/densebasic.py", line 536, in dmp_convert
    return dmp_strip([ dmp_convert(c, v, K0, K1) for c in f ], u)
  File "/home/r/.local/lib/python2.7/site-packages/sympy/polys/densebasic.py", line 530, in dmp_convert
    return dup_convert(f, K0, K1)
  File "/home/r/.local/lib/python2.7/site-packages/sympy/polys/densebasic.py", line 506, in dup_convert
    return dup_strip([ K1.convert(c, K0) for c in f ])
  File "/home/r/.local/lib/python2.7/site-packages/sympy/polys/domains/domain.py", line 85, in convert
    raise CoercionFailed("can't convert %s of type %s to %s" % (a, K0, K1))
sympy.polys.polyerrors.CoercionFailed: can't convert DMP([[[[mpf('-0.21325')]], [[mpf('-0.24365000000000001')]]], [[[mpf('0.21325'), mpf('0.0')], []]]], RR) of type RR[cj1,cj2,sj1,sj2] to RR

Can someone help me and point me in a direction that helps me setting up a solver for my robot?
I attached my XML file [3] as well, which - if I get it to work - I'd like to make available to others, so that others don't have to do the work I had to do. In my particular example I have a optoforce ft sensor attached to the end effector. Also, weights and center of masses have not been added yet, I first wanted to see if I can get it running like this.

[1]: https://www.universal-robots.com/how-tos-and-faqs/faq/ur-faq/actual-center-of-mass-for-robot-17264/
[2]: http://openrave-users-list.185357.n3.nabble.com/mpmath-not-needed-td4027687.html

[3]:
<Robot>
    <KinBody name="UR3-Opto">

        <!-- 
        Kinematics:
        a = [0, -0.24365, -0.21325, 0, 0, 0]
        d = [0.1519, 0, 0, 0.11235, 0.08535, 0.0819]
        alpha = [1.570796327, 0, 0, 1.570796327, -1.570796327, 0]
        mass = [2, 3.42, 1.26, 0.8, 0.8, 0.35]
        center_of_mass = [[0,-0.02, 0], [0.13, 0, 0.1157], [0.05, 0, 0.0238], [0, 0, 0.01], [0, 0, 0.01], [0, 0, -0.02]] 
        -->

        <Body name="base_link" type="dynamic">
            <Translation>0 0 0</Translation>
            <Geom type="cylinder">
                <render>universal_robot/ur_description/meshes/ur3/collision/base.stl</render>
                <Translation>0 0 0</Translation>
                <rotationmat>-1 0 0 0 0 -1 0 -1 0</rotationmat>
                <transparency>0.8</transparency>
                <diffuseColor>0 0 1</diffuseColor>
            </Geom>
        </Body>
        <Body name="shoulder_link" type="dynamic">
            <Translation>0 0 0</Translation>
            <Geom type="cylinder">
                <render>universal_robot/ur_description/meshes/ur3/collision/shoulder.stl</render>
                <rotationmat>-1 0 0 0 0 -1 0 -1 0</rotationmat>
                <Translation>0.0 0 0.1519</Translation>
                <transparency>0.8</transparency>
                <diffuseColor>0 1 0.0</diffuseColor>
            </Geom>
        </Body>
        <Joint circular="true" name="base_joint" type="hinge">
            <Body>base_link</Body>
            <Body>shoulder_link</Body>
            <offsetfrom>shoulder_link</offsetfrom>
            <axis>0 0 1</axis>
        </Joint>
        <Body name="upper_arm_link" type="dynamic">
            <Translation>0 0 0.1519 </Translation>
            <rotationaxis>1 0 0 90</rotationaxis>
            <Geom type="cylinder">
                <render>universal_robot/ur_description/meshes/ur3/collision/upperarm.stl</render>
                <Translation>0 0 0.120</Translation>
                <rotationaxis>0 0 1 270</rotationaxis>
                <transparency>0.9</transparency>
                <diffuseColor>0.6 0 0.8</diffuseColor>
            </Geom>
        </Body>0
    
    
        <Joint circular="true" name="shoulder_joint" type="hinge">
            <Body>shoulder_link</Body>
            <Body>upper_arm_link</Body>
            <offsetfrom>upper_arm_link</offsetfrom>
            <axis>0 0 1</axis></Joint>
        <Body name="forearm_link" type="dynamic">
            <Translation>-0.24365 0.0 0.1519</Translation>
            <rotationaxis>1 0 0 90</rotationaxis>
            <Geom type="box">
                <render>universal_robot/ur_description/meshes/ur3/collision/forearm.stl</render>
                <transparency>0.9</transparency>
                <diffuseColor>0.4 0.8 0.1</diffuseColor>
                <rotationmat>0 0 -1 -1 0 0 0 1 0</rotationmat>
                <Translation>0 0 0.028</Translation>
            </Geom>
        </Body>
        <Joint circular="true" name="elbow_joint" type="hinge">
            <Body>upper_arm_link</Body>
            <Body>forearm_link</Body>
            <offsetfrom>forearm_link</offsetfrom>
            <axis>0 0 1</axis>
        </Joint>
        <Body name="wrist_1_link" type="dynamic">
            <Translation>-0.4569 0 0.1519</Translation>
            <rotationaxis>1 0 0 90</rotationaxis>
            <Geom type="box">
                <render>universal_robot/ur_description/meshes/ur3/collision/wrist1.stl</render>
                <transparency>0.9</transparency>
                <rotationaxis>1 0 0 90</rotationaxis>
                <diffuseColor>0.1 0.4 1</diffuseColor>
                <Translation>0 0 0.032</Translation>
            </Geom>
        </Body>
        <Joint circular="true" name="wrist_1_joint" type="hinge">
            <Body>forearm_link</Body>
            <Body>wrist_1_link</Body>
            <offsetfrom>wrist_1_link</offsetfrom>
            <axis>0 0 1</axis>
        </Joint>

        <Body name="wrist_2_link" type="dynamic">
            <Translation>-0.4569 -0.11235 0.1519</Translation>
            <rotationaxis>1 0 0 180</rotationaxis>
            <Geom type="box">
                <render>universal_robot/ur_description/meshes/ur3/collision/wrist2.stl</render>
                <transparency>0.8</transparency>
                <diffuseColor>0.4 0.6 0.1</diffuseColor>
                <Translation>0 0.00475 0</Translation>
            </Geom>
        </Body>
        <Joint circular="true" name="wrist_2_joint" type="hinge">
            <Body>wrist_1_link</Body>
            <Body>wrist_2_link</Body>
            <offsetfrom>wrist_2_link</offsetfrom>
            <axis>0 0 1</axis>
        </Joint>
        <Body name="wrist_3_link" type="dynamic">
            <Translation>-0.4569 -0.11235 0.06655</Translation>
            <rotationaxis>1 0 0 90</rotationaxis>
            <Geom type="box">
                <transparency>0.8</transparency>
                <render>universal_robot/ur_description/meshes/ur3/collision/wrist3.stl</render>
                <rotationaxis>1 0 0 90</rotationaxis>
                <diffuseColor>0.8 0.1 0.1</diffuseColor>
            </Geom>
        </Body>
        <Joint circular="true" name="wrist_3_joint" type="hinge">
            <Body>wrist_2_link</Body>
            <Body>wrist_3_link</Body>
            <offsetfrom>wrist_3_link</offsetfrom>
            <axis>0 0 1</axis>
        </Joint>
    </KinBody>
    <AttachedSensor name="Optoforce_FT_6D">
        <link>wrist_3_link</link>
        <translation>0 0 0.035</translation>
        <rotationaxis>0 0 0 0</rotationaxis>
    </AttachedSensor>
</Robot>
Reply | Threaded
Open this post in threaded view
|

Re: Can't convert DMP to RR

sirop
Never seen anything like that, but found by trial and error that the solution space may highly depend on joints resolution.
ra_
Reply | Threaded
Open this post in threaded view
|

Re: Can't convert DMP to RR

ra_
Thank you!

Since it is never really stated anywhere. What is meant with "resolution"?
Reply | Threaded
Open this post in threaded view
|

Re: Can't convert DMP to RR

sirop
ra_ wrote
Thank you!

Since it is never really stated anywhere. What is meant with "resolution"?
Just play with:
robot.SetDOFResolutions??
ra_
Reply | Threaded
Open this post in threaded view
|

Re: Can't convert DMP to RR

ra_
I know where I can set it, its just that I would like to know what it means and how it influences the IK. And why I have to mess with the correct kinematics that I put in in.
Since calculations take several hours to complete (and fail), playing around here is rather tedious.
Reply | Threaded
Open this post in threaded view
|

Re: Can't convert DMP to RR

sirop
ra_ wrote
I know where I can set it, its just that I would like to know what it means and how it influences the IK. And why I have to mess with the correct kinematics that I put in in.
Since calculations take several hours to complete (and fail), playing around here is rather tedious.
If you upload your robot XML together with STL somewhere to github or similar,
I could try it at my place, but I would begin with a simpler example like http://openrave.org/docs/latest_stable/tutorials/openravepy_examples/#inverse-kinematics-transform6d .

Several hours run for a database generation  is pretty much...
ra_
Reply | Threaded
Open this post in threaded view
|

Re: Can't convert DMP to RR

ra_
That would be great!
I included the XML in the first post here. And the STL's I used are from this folder: https://github.com/ros-industrial/universal_robot/tree/kinetic-devel/ur_description/meshes/ur3/collision
Reply | Threaded
Open this post in threaded view
|

Re: Can't convert DMP to RR

sirop
I tried it both on Ubuntu 14(master branch) and Debian 8 ( production branch):
it always gets stucks at
openravepy.ikfast: SimplifyTransform, equation cj3*r12**2*sj1*sj2/(-r22**2 + 1)**(1/2) has rot symbols in denom, so skipping...
openravepy.ikfast: SimplifyTransform, equation -r02**2*sj1*sj2*sj3/(-r22**2 + 1)**(1/2) has rot symbols in denom, so skipping...
openravepy.ikfast: SimplifyTransform, equation cj3*r12**2*sj1*sj2/(-r22**2 + 1)**(1/2) has rot symbols in denom, so skipping...


I was waiting each time at least 30 min but nothing but nothing went further.
ra_
Reply | Threaded
Open this post in threaded view
|

Re: Can't convert DMP to RR

ra_
Yes, that's what I thought would be the end of it, but it continues if you wait log enough.
Reply | Threaded
Open this post in threaded view
|

Re: Can't convert DMP to RR

sirop
Did you try smth. along the lines of this conversation https://github.com/rdiankov/openrave/pull/408 ?

BTW, <AttachedSensor name="Optoforce_FT_6D"> does not seem to be a known OpenRave sensor.
Reply | Threaded
Open this post in threaded view
|

Re: Can't convert DMP to RR

sirop
In reply to this post by ra_
It does not converge with extraprec=20 and maxSteps=250 , but it least it does not take hours to get to this result.
No more than 15 min.