Position-only Inverse Kinematics

[arjung128]

I'd like to move the end-effector along a given trajectory. I only have the 3d positions of the end-effector along the trajectory, not the orientations of the end-effector.

When I try doing IK with the rotation fixed and a small delta in the position, it often does not converge (the error plateaus), which I think is an indication that no valid configuration exists.

How can i do position-only IK?

From #1336:

if you only want to reach some position, just consider the three rows of the Jacobian related to the translation, the three first rows.

I tried zero-ing out the last three rows of the Jacobian (which is of shape [6, 15]), but this did not seem to converge (in fact, the error started increasing). How can I only consider the three rows of the Jacobian related to the translation?

[jcarpent]

Hello @arjung128 ! Could you provide a tiny example?

PS: politeness is more than welcome when you ask for advises

[arjung128]

Hello @jcarpent,

I'm very sorry if I came across as a bit rude, that was not my intention at all. I really appreciate your prompt responses -- the prompt replies are extremely helpful, especially when stuck on something!

I have the robot end-effector pose at t=0:

R =
  0.70356  0.633152   0.32268
0.0875032 -0.527802  0.844848
 0.705228 -0.566166 -0.426743
  p =  0.45032 -0.10624 0.961281

And I want to move the end-effector position a little bit along the x-direction, e.g. to p = 0.50032 -0.10624 0.961281.

When I do IK with the new desired position and the same rotation from t=0 (as shown below) using code from the tutorial, IK is not able to converge:

R =
  0.70356  0.633152   0.32268
0.0875032 -0.527802  0.844848
 0.705228 -0.566166 -0.426743
  p =  0.50032 -0.10624 0.961281

Because the offset is only 0.05 in the x-direction, it seems like this should be reachable for the robot end-effector (based on visualizing the robot in meshcat), but perhaps not reachable with the specified rotation.

Is there a way to do IK given just the desired 3D position (allowing for any rotation of the end-effector)? Either find a valid rotation and then use that for IK, or not consider error in rotation when doing IK?

Thanks in advance for your help!

[traversaro]

Given that you refer to https://github.com/stack-of-tasks/pinocchio/issues/1336, are you sure that you are using the pin.LOCAL_WORLD_ALIGNED jacobian as described in https://github.com/stack-of-tasks/pinocchio/issues/1336#issuecomment-723419697 ?

If I am not wrong, that is the only jacobian that, if multiplied by the robot velocity, returns a quantity where the three rows of the jacobian related to translation actually are the derivative of the origin of the frame expressed in the world frame.

[arjung128]

@traversaro I am using pinocchio.computeJointJacobian() as used in the IK tutorial rather than pinocchio.getJointJacobian(), which doesn't have a localFrame parameter.

[traversaro]

@traversaro I am using pinocchio.computeJointJacobian() as used in the IK tutorial rather than pinocchio.getJointJacobian(), which doesn't have a localFrame parameter.

I am definitely not an expert of Pinocchio internals (so @jcarpent feel free to correct me if I am wrong) but, under this assumptions:

• we have the frame a (inertial/absolute frame) and e (the joint frame of interest), and the transform between this two frames is represented by ᵃRₑ ∈ SO(3) ᵃoₑ ∈ ℝ³
• (.)ᵛ represents the mapping between the skew-symmetrix matrix and the related vector in ℝ³, i.e. such that if u,v ∈ ℝ³, u = (U)ᵛ s.t. U v = u × v
• the dot represent the usual mathematical analysis dot operator (time derivative) and not the Featherstone one then the different velocities are:

I think that the translational part of the jacobian you are using is the jacobian of the quantity ᵃRₑᵀ ᵃȯₑ, while you are interested in the jacobian of ᵃȯₑ . Can you try to pre-multiply the jacobian by ᵃRₑ to obtain the jacobian of ᵃȯₑ and check if that is working? This should be equivalent of using the pin.LOCAL_WORLD_ALIGNED jacobian.

[arjung128]

@traversaro Thanks for such a detailed response!

How can I get ᵃRₑ? Sorry, I am new to Pinocchio and this area in general.

Also, I tried using pinocchio.getJointJacobian() with pin.LOCAL_WORLD_ALIGNED similar to #1336:

while i < n_max_iter:

    pin.forwardKinematics(model, data, x)

    # dummy current SE3 with identity rotation
    cur = SE3.Identity()
    cur.translation = data.oMi[j_idx].translation

    dMi = goal.actInv(cur)
    err = pin.log(dMi).vector / DT
    mu = lm_damping * max(1e-3, err.dot(err))

    jac = pin.getJointJacobian(model, data, j_idx, pin.LOCAL_WORLD_ALIGNED) # pin.WORLD)

    ATA += jac.T @ jac + mu * np.eye(model.nv)
    ATb += -err.T.dot(jac)

    dv = -solve_qp(ATA, ATb)
    x = pin.integrate(model, x, dv * DT)
    x[:7] = q0[:7]

    if not i % 1000:
        print('%d: error = %s' % (i, norm(err*DT))) # err.T))
    i += 1

However, this does not seem to converge either (in fact, doesn't seem to converge for the end-effector pose at t=0 either, let alone the modified pose at t=1). Any ideas what could be going wrong?

[traversaro]

Ack, my intuition failed then. As asked before, then it would be great if you could provide a tiny (self-contained) example. It is not possible to understand what is going on just by the loop you posted, see https://stackoverflow.com/help/minimal-reproducible-example for more details.

[jcarpent]

You can see this example : https://github.com/stack-of-tasks/pinocchio/blob/master/examples/inverse-kinematics.py

Thanks @traversaro for your kind help.

[arjung128]

@traversaro Thanks for your support! Much appreciated.

@jcarpent This is very helpful. I didn't realize setting the rotation to identity would get rid of this constraint. Thanks a lot for your help!