Inverse kinematics undamped case unnecessary multiplications

[kevinzakka]

This is referencing the nullspace_method in inverse_kinematics.py.

In the undamped case, i.e., when regularization_strength = 0.0, the code is doing:

hess_approx = jac_joints.T.dot(jac_joints)
joint_delta = jac_joints.T.dot(delta)
np.linalg.lstsq(hess_approx, joint_delta, rcond=-1)[0]

but should really just be:

np.linalg.lstsq(jac_joints, delta, rcond=-1)[0]

J.T @ J squares the condition number so getting rid of it should improve numerical stability.

[kevinzakka]

@saran-t I've generalized the IK function to sites, geoms and bodies as in dm_robotics. Is this something that could use a PR? I feel like it might not be worth it since IK is only used to position the end-effector at episode initialization. LMK!