# Inverse Velocity kinematics with OpenRave? Classic List Threaded 12 messages Open this post in threaded view
|

## Inverse Velocity kinematics with OpenRave?

 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!!
Open this post in threaded view
|

## Re: Inverse Velocity kinematics with OpenRave?

 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 . For instance there is one implemented in the pymanoid library for OpenRAVE . All the best, -- Stéphane Caron https://scaron.info signature.asc (853 bytes) Download Attachment Stéphane Caron https://scaron.info
Open this post in threaded view
|

## Re: Inverse Velocity kinematics with OpenRave?

 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
Open this post in threaded view
|

## Re: Inverse Velocity kinematics with OpenRave?

 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
Open this post in threaded view
|

## Re: Inverse Velocity kinematics with OpenRave?

 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
Open this post in threaded view
|

## Re: Inverse Velocity kinematics with OpenRave?

Open this post in threaded view
|

## Re: Inverse Velocity kinematics with OpenRave?

 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?
Open this post in threaded view
|

## Re: Inverse Velocity kinematics with OpenRave?

 This post was updated on . https://github.com/rdiankov/openrave/blob/master/python/databases/inversekinematics.pydoes 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 .
Open this post in threaded view
|

## Re: Inverse Velocity kinematics with OpenRave?

 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.
Open this post in threaded view
|

## Re: Inverse Velocity kinematics with OpenRave?

 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])) ```
 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.]
 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.