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!! |
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 |
This post has NOT been accepted by the mailing list yet.
In reply to this post by Lucy
Hi Lucy,
Advice: post the significant part of your code (as small as possible) to be more precise here. 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 |
Rather: or sol = linalg.lstsq(J_pos, v) qd = sol[0] |
Whoops, sorry for that! Yes, the mapping is indeed v = dot(J_pos, qd) and the inverse problem is to find qd:
Stéphane Caron
https://scaron.info |
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! |
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? |
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 . |
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])) |
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.] |
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. |
Free forum by Nabble | Edit this page |