No convergence at franka panda

[PARKBONG]

Hi. I tried to test this repo with Franka Panda.

here is my code. torch_kin_test.py

import os
import torch
import pytorch_kinematics as pk

device = torch.device("cuda" if torch.cuda.is_available() else "cpu")

full_urdf = os.path.dirname(os.path.abspath(__file__)) + "/lula/lula_franka_gen.urdf"
with open(full_urdf, 'rb') as urdf_file:
    urdf_data = urdf_file.read()
    chain = pk.build_serial_chain_from_urdf(urdf_data, "panda_hand")

pos = torch.tensor([0.0, 0.0, 0.0], device=device)
rot = torch.tensor([0.0, 0.0, 0.0], device=device)
rob_tf = pk.Transform3d(pos=pos, rot=rot, device=device)

lim = torch.tensor(chain.get_joint_limits(), device=device)
goal_q = lim[0] + (lim[1] - lim[0]) / 2
goal_in_rob_frame_tf = chain.forward_kinematics(goal_q)

ik = pk.PseudoInverseIK(chain, max_iterations=100, num_retries=10,
                        joint_limits=lim.T,
                        early_stopping_any_converged=True,
                        early_stopping_no_improvement="all",
                        debug=False,
                        lr=0.2)

sol = ik.solve(goal_in_rob_frame_tf)
print(sol.converged)

tensor([[False, False, False, False, False, False, False, False, False, False]], device='cuda:0')

Why does it fail to solve it? Any advice will be very helpful for me. Thanks!

[akashsharma02]

@PARKBONG were you able to resolve this issue? I am facing the same problem

[umicharmlab]

Have you been able to visualize the iterations? or tested changing any of the parameters? That might help us figure out what's going on.

[zhengkai666666]

I am facing the same problem

[williamshen-nz]

I was having this problem with the Franka when I was using a URDF that resulted in 9 actuated joints (7 DOF + gripper). Using a URDF with only 7 actuated joints solved the problem

Here's an example script based on the unit tests: https://gist.github.com/williamshen-nz/abb8bd1aaf91499741aaacb3a87f22dd

URDF I used: https://github.com/NVlabs/curobo/blob/main/src/curobo/content/assets/robot/franka_description/franka_panda.urdf

[LemonPi]

Hi sorry for the long delay. I believe this was an issue where you build a serial chain from a non-linear chain (panda + gripper and hands) and specify the end effector. The problem was the end effector frame would still have its children linking to the non-linear part of the chain later. This would explain why @williamshen-nz using a URDF with only 7 actuated joints worked while the one with 9 didn't. Commit baa6ced4d837e09e51f18dd3c93f41fe7b75f20f should've solved this.

@PARKBONG @akashsharma02 @zhengkai666666 can you try again with 0.7.3? If it persists I can look more into it