Inverse Velocity kinematics with OpenRave?

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

Inverse Velocity kinematics with OpenRave?

Lucy
Hi!
Does anyone know a way to solve the inverse velocity kinematics problem with OpenRave?  I would like to transform my end-effector speeds into joint speeds.
I thought that using an IKParameterizationType like "Transform6DVelocity" could be the solution, but I get a "bad type" error when I try to use it.

Any suggestions will be very very appreciated.

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

Re: Inverse Velocity kinematics with OpenRave?

caron
This post has NOT been accepted by the mailing list yet.
Hi Lucy,

On 04/21/2017 11:18 AM, Lucy [via OpenRAVE Users List] wrote:
> I thought that using an IKParameterizationType like
> "Transform6DVelocity" could be the solution, but I get a "bad type"
> error when I try to use it.

Advice: post the significant part of your code (as small as possible) to
be more precise here.

On 04/21/2017 11:18 AM, Lucy [via OpenRAVE Users List] wrote:
> I would like to transform my end-effector speeds into joint speeds.

Are you using a fixed-base or mobile robot?

For a fixed-base robot, Jacobian matrices give you the mapping between
end-effector velocities and positions:

    # pos = your current end-effector world coordinates
    J_pos = robot.ComputeJacobianTranslation(link_index, pos)
    J_ang = robot.ComputeJacobianAxisAngle(link_index)

So, if the current end-effector linear velocity is `v`, the
corresponding joint-angles are given by:

    qd = dot(J_pos, v)

If you have a mobile robot, or a fixed-base robot with multiple
end-effector tasks, you may want to look at Multi-task Inverse
Kinematics <https://scaron.info/teaching/inverse-kinematics.html>.
For instance there is one implemented in the pymanoid library for
OpenRAVE <https://github.com/stephane-caron/pymanoid>.

All the best,

--
Stéphane Caron
https://scaron.info


signature.asc (853 bytes) Download Attachment
Stéphane Caron
https://scaron.info
Reply | Threaded
Open this post in threaded view
|  
Report Content as Inappropriate

Re: Inverse Velocity kinematics with OpenRave?

caron
This post has NOT been accepted by the mailing list yet.
In reply to this post by Lucy
Hi Lucy,
Lucy wrote
I thought that using an IKParameterizationType like "Transform6DVelocity" could be the solution, but I get a "bad type" error when I try to use it.
Advice: post the significant part of your code (as small as possible) to be more precise here.
Lucy wrote
I would like to transform my end-effector speeds into joint speeds.
Are you using a fixed-base or mobile robot?

For a fixed-base robot, Jacobian matrices give you the mapping between end-effector velocities and positions:
    # pos = your current end-effector world coordinates
    J_pos = robot.ComputeJacobianTranslation(link_index, pos)
    J_ang = robot.ComputeJacobianAxisAngle(link_index)
So, if the current end-effector linear velocity is `v`, the corresponding joint-angles are given by:
    qd = dot(J_pos, v)
If you have a mobile robot, or a fixed-base robot with multiple end-effector tasks, you may want to look at Multi-task Inverse Kinematics. For instance there is one implemented in the pymanoid library for OpenRAVE.

All the best,
Stéphane Caron
https://scaron.info
Reply | Threaded
Open this post in threaded view
|  
Report Content as Inappropriate

Re: Inverse Velocity kinematics with OpenRave?

sirop
caron wrote
So, if the current end-effector linear velocity is `v`, the corresponding joint-angles are given by:
    qd = dot(J_pos, v)
Rather:
 qd = dot(J_pos.inv(), v)

or
 
 sol = linalg.lstsq(J_pos, v)
 qd = sol[0]
Reply | Threaded
Open this post in threaded view
|  
Report Content as Inappropriate

Re: Inverse Velocity kinematics with OpenRave?

caron
Whoops, sorry for that! Yes, the mapping is indeed v = dot(J_pos, qd) and the inverse problem is to find qd:
  • If J_pos is invertible then qd = dot(J_pos.inv(), v) as sirop points out;
  • If J_pos is not invertible, the problem is redundant: many qd's yield the same v. Least squares will give you the minimum-norm solution with no constraint (it may violate velocity limits or DOF limits after you apply it);
  • The most general solution to the problem is to solve a QP, which allows you to specify what objective function you want, or to take into account constraints such as DOF or velocity limits.
Stéphane Caron
https://scaron.info
Reply | Threaded
Open this post in threaded view
|  
Report Content as Inappropriate

Re: Inverse Velocity kinematics with OpenRave?

Lucy
In reply to this post by caron
Hi!

Thanks very much for your answer, Caron, and thanks also for the advice.
I'm looking for a solution for fixed-base robots.  I knew that I could use the Jacobian to solve it, but as far as I understand, the jacobian relates in a direct way end-effector speed with joint speeds: v = dot(J_pos, qd) and not joint speeds with end-effector speed. If I wanted to get the speeds in the joints given the end-effector speed, I would need to calculate the inverse of the jacobian, if it was possible, or its pseudoinverse otherwise.

I was looking for a solution that would make me skip these calculations and I suspected that the "Transform6DVelocity" type could be that. However, I haven't found an explanation of it (or examples) in the OpenRave documentation, so it's just a guess (and probably wrong).
I have tried to use it in two of my robots: a very simple 2DOF one, and in 6DOF KR6.
The code I use for the 2DOF robot is:

ikmodel = databases.inversekinematics.InverseKinematicsModel(robot,iktype=IkParameterization.Type.TranslationXY2DVelocity()
if not ikmodel.load():                                                                                            
    ikmodel.autogenerate()  

And the error I get is:

Traceback (most recent call last):
  File "Velocities-Robot2DOF.py", line 21, in <module>
    ikmodel.autogenerate()                                                                                  
  File "/usr/local/lib/python2.7/dist-packages/openravepy/_openravepy_/databases/inversekinematics.py", line 653, in autogenerate
    self.generate(iktype=iktype,freejoints=freejoints,precision=precision,forceikbuild=forceikbuild,outputlang=outputlang,ipython=ipython,ikfastmaxcasedepth=ikfastmaxcasedepth)
  File "/usr/local/lib/python2.7/dist-packages/openravepy/_openravepy_/databases/inversekinematics.py", line 825, in generate
    raise InverseKinematicsError(u'bad type')
InverseKinematicsError: Inverse Kinematics Error
bad type


When I use an iktype "TranslationXY2D" instead of "TranslationXY2DVelocity, I get a solution without problems. That's why I deduce that TranslationXY2DVelocity might not be used for what I'm looking for, or that there is something with it that I'm missing.
I get the same error when I try "Transform6DVelocity" in the 6DOF Kr6

Any ideas about this?

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

Re: Inverse Velocity kinematics with OpenRave?

Lucy
Oh, sorry. I hadn't read the last posts.... Thanks for the suggestions, and forget the first part of my answer...

Any ideas about the Velocity iktypes?
Reply | Threaded
Open this post in threaded view
|  
Report Content as Inappropriate

Re: Inverse Velocity kinematics with OpenRave?

sirop
This post was updated on .
https://github.com/rdiankov/openrave/blob/master/python/databases/inversekinematics.py
does not contain Transform6DVelocity ,
however, https://github.com/rdiankov/openrave/blob/master/test/test_iksolver.py#L233-L237 does contain this IK type.

So my guess is: generate an IK database for Transform6D, but when looking then for solutions use
Transform6DVelocity as in https://github.com/rdiankov/openrave/blob/master/test/test_iksolver.py .
Reply | Threaded
Open this post in threaded view
|  
Report Content as Inappropriate

Re: Inverse Velocity kinematics with OpenRave?

Lucy
Thanks!
Your guess looks quite logical, and it actually doesn't throw erros when I tried it.
However, I must be still missing something, because when I make print(robot.GetDOFVelocities()), l always get a list of zeros.
 The speeds calculated with the Jacobians are different from 0.
Reply | Threaded
Open this post in threaded view
|  
Report Content as Inappropriate

Re: Inverse Velocity kinematics with OpenRave?

sirop
Lucy wrote
Thanks!
Your guess looks quite logical, and it actually doesn't throw erros when I tried it.
However, I must be still missing something, because when I make print(robot.GetDOFVelocities()), l always get a list of zeros.
 The speeds calculated with the Jacobians are different from 0.
I do not know where you use
 robot.GetDOFVelocities() 
.
Have a look at https://github.com/rdiankov/openrave/blob/master/test/test_iksolver.py#L233-L245 again,
especially, at:
            ikparam2 = ikparam.Transform(T)
            ikparamvel2 = ikparamvel.Transform(T)
            newquatvelocity = ikparamvel2.GetValues()[0:4]
            newtransvelocity = ikparamvel2.GetValues()[4:7]
            newangularvelocity = 2*quatMultiply(newquatvelocity, quatInverse(ikparam2.GetValues()[0:4]))


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

Re: Inverse Velocity kinematics with OpenRave?

Lucy
sirop wrote
I do not know where you use
 robot.GetDOFVelocities() 
.
Have a look at https://github.com/rdiankov/openrave/blob/master/test/test_iksolver.py#L233-L245 again,
especially, at:
            ikparam2 = ikparam.Transform(T)
            ikparamvel2 = ikparamvel.Transform(T)
            newquatvelocity = ikparamvel2.GetValues()[0:4]
            newtransvelocity = ikparamvel2.GetValues()[4:7]
            newangularvelocity = 2*quatMultiply(newquatvelocity, quatInverse(ikparam2.GetValues()[0:4]))


Sorry, I should have explained myself better...
I used that code with my 6DOF robot. Exactly the same code and with the same data than in the example, but just up to line 237, as I understand that the rest of the code is for testing. I added a last line: print(robot.GetDOFVelocities()), as I interpret that line 237 (ikparamvel.SetValues(numpy.r_[quatvelocity,transvelocity], IkParameterizationType.Transform6DVelocity)) sets the speed of the end effector, and I wanted to check whether the values of the joints speed were right.

This is the code:

angularvelocity = numpy.array([1,0.5,-0.4])
quatvelocity = 0.5*quatMultiply(numpy.r_[0,angularvelocity],manip.GetIkParameterization(IkParameterizationType.Transform6D).GetValues()[0:4])
transvelocity = numpy.array([0.1,0.2,0.3])
ikparamvel= IkParameterization()
ikparamvel.SetValues(numpy.r_[quatvelocity,transvelocity], IkParameterizationType.Transform6DVelocity)
print(robot.GetDOFVelocities())

But what I got as output was:
[ 0.  0.  0.  0.  0.  0.]

I also tried adding the rest of the code(from line 239 to line 245), just for trying, and I moved "print(robot.GetDOFVelocities())" to the last line, with the same result.

T = matrixFromAxisAngle([0,1,0])
T[0:3,3] = [0.5,0.2,0.8]
ikparam2 = ikparam.Transform(T)
ikparamvel2 = ikparamvel.Transform(T)
newquatvelocity = ikparamvel2.GetValues()[0:4]
newtransvelocity = ikparamvel2.GetValues()[4:7]
newangularvelocity = 2*quatMultiply(newquatvelocity, quatInverse(ikparam2.GetValues()[0:4]))
def transdist(list0,list1):              
    assert(len(list0)==len(list1))
    return numpy.sum([numpy.sum(abs(item0-item1)) for item0, item1 in izip(list0,list1)])
assert(transdist(newangularvelocity[1:4], numpy.dot(T[0:3,0:3],angularvelocity)) <= g_epsilon)
assert(transdist(newtransvelocity, numpy.dot(T[0:3,0:3],transvelocity)) <= g_epsilon)
print(robot.GetDOFVelocities())

Output: [ 0.  0.  0.  0.  0.  0.]
Reply | Threaded
Open this post in threaded view
|  
Report Content as Inappropriate

Re: Inverse Velocity kinematics with OpenRave?

sirop
Lucy wrote
I also tried adding the rest of the code(from line 239 to line 245), just for trying, and I moved "print(robot.GetDOFVelocities())" to the last line, with the same result.

T = matrixFromAxisAngle([0,1,0])
T[0:3,3] = [0.5,0.2,0.8]
ikparam2 = ikparam.Transform(T)
ikparamvel2 = ikparamvel.Transform(T)
ikparam is an object of type IkParameterization(). Just by setting any values of this object you do not change anything yet for robot which is a different object.

Try smth. like:
robot.SetDOFValues(values,manip.GetArmIndices())
robot.SetDOFVelocities(velocities,manip.GetArmIndices())
solutions=ikmodel.manip.FindIKSolutions(ikmodel.manip.GetIkParameterization(ikmodel.iktype),0)
substituting ikmodel.manip.GetIkParameterization(ikmodel.iktype) with a properly parameterized target for Transfrom6DVelocity.
Loading...