IK solutions exceed joint limits

[joel0115]

Hi, thanks for your impressive works!
We are new to the robotics, and we have customized a urdf file to describe our 6-DOF arm.
And we found that given an tool center pose (tcp), the PseudoInverseIK will return converged solutions that exceed joint limits. (The argument of joint limit is properly given.)
The values are around 10^1 to 10^2 (should be radius I think).

Surprisingly, after setting the seemingly wrong joint solutions on robot simulators like ManiSkill, we found the corresponding tcp is the same as expected.
Is it reasonable?
What should be further tested?

Thanks in advance!

[LemonPi]

That is quite odd - do you mind sharing code that can replicate this problem?

EDIT: actually looking at the code more, the joint limits are currently only used in the initial configuration generation rather than enforced at each step of the optimization. There are several potential solutions:

• add auxiliary cost function API (needs to be differentiable wrt joint values) that allows enforcing joint angles, collision checks, and so on, and implement a JointAngleCost
• when outputting solutions, wrap the joint angles to [-pi, pi] then project to joint limits

It's not obvious that solution 1 will be better here because it will introduce more local minima as now there's competing cost functions. You can try solution 2 for now but when you post the code I can also implement those solutions and try them out on it.

[joel0115]

I apologize that our urdf file is confidential, and hence I cannot share the structure of it.
Sorry for the inconvienience.
The code using to generating tcp pose and corresponding joint configurations is like the following:

import torch
import pytorch_kinematics as pk
from torch.utils.data import Dataset
from mani_skill import PACKAGE_ASSET_DIR

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

urdf_path = f"..."
chain = pk.build_serial_chain_from_urdf(open(urdf_path).read(), "...", "...").to(device=device)
lim = torch.tensor(chain.get_joint_limits(), device=device)
ik = pk.PseudoInverseIK(chain, max_iterations=200, num_retries=50,
                        joint_limits=lim.T,
                        early_stopping_no_improvement="all",
                        early_stopping_no_improvement_patience=100,
                        debug=False,
                        lr=0.8)

def generate_data(num_samples, x_range, y_range, z_range):
    """
    Generate tcp's x, y, z and compute IK to get corresponding joint configuration
    
    Args:
        num_samples (int): Number of sample
        x_range (tuple): Range of x
        y_range (tuple): Range of y
        z_range (tuple): Range of z
    Returns:
        tensor: (inputs, targets), the tcp's x,y,z and corresponding joint configuration
    """

    inputs = []
    targets = []

    inputs = torch.empty((num_samples, 3), device=device)
    inputs[:, 0] = torch.empty(num_samples, device=device).uniform_(*x_range)
    inputs[:, 1] = torch.empty(num_samples, device=device).uniform_(*y_range)
    inputs[:, 2] = torch.empty(num_samples, device=device).uniform_(*z_range)
    

    tcp_poses = pk.Transform3d(pos=inputs, device='cuda')

    sol = ik.solve(target_poses=tcp_poses)
    # shape: (num_samples, num_retries, DOF)
    # simply picking one solution
    targets = sol.solutions[:, 0]

    return inputs, targets

if __name__ == '__main__':
    inputs, targets = generate_data(2, (0.6, 0.8), (0,0), (0, 0))
    print(inputs)
    print(targets)

And in ManiSkill, I just apply the joint configuration, and check the corresponding tcp like this:

qpos = torch.tensor([...], device=self.device)
self.agent.robot.set_qpos(qpos)
self.scene._gpu_apply_all()
self.scene.px.gpu_update_articulation_kinematics()
self.scene._gpu_fetch_all()

print(self.agent.tcp.pose.p)

Could you please explain how to wrap and project the joint configurations into joint limits?
Thank you so much!