IK with initial joint value support

[yswi]

Thanks for your amazing work!
I found it necessary to retrieve the IK solution closest to the current joint config necessary for smoothness and consistencies. Adding an additional cost term to the optimization does the trick for me.

[chungmin99]

Hi! 🥹

I found it necessary to retrieve the IK solution closest to the current joint config necessary for smoothness and consistencies. Adding an additional cost term to the optimization does the trick for me.

Yes, it makes sense that the rest cost helps here -- you could also add a limit_velocity_cost with a high cost weight, so that we're only looking for solutions within some distance from the previous configuration.

Regarding this PR, I do have a light preference to keep solve_ik as short as possible, since it's meant to be the first example for people to see...

Would you be interested in turning this into a new example (e.g., maybe smooth_ik) that extends the existing IK example with these costs? I can also take a pass on this!

[wuisky]

Hi, I want this feature, too.
I have tried yswi's solution, it works good for me.

But i can't work it out when using limit_velocity_cost.
I add cost like:

timesteps = 2
traj_var_prev = robot.joint_var_cls(jnp.arange(0, timesteps - 1))
traj_var_next = robot.joint_var_cls(jnp.arange(1, timesteps))
factors.append(pk.costs.limit_velocity_cost(
    jax.tree.map(lambda x: x[None], robot),
    traj_var_prev,
    traj_var_next,
    weight=100,
    dt=0.1,
))

Do i miss something??