Fixing a joint for one solution (multiple solution problems)

[Rollmex]

Hi, I would like to fixe a revolute joint angle when I use the solver ikine_min(M,qlim=True,method='SLSQP') because there is multiple solution and I'm looking for the one with a specific angle from my arm. However the orientation of the fixed joint could be changed by the user in real_time (I use a slider in the Swift environnement as input for the angle value). Is there a option to resolve this problem ?

[petercorke]

Hi @Rollmex, thank you for the question, and I'm sorry for the very long delay: various life events got in the way, and since May I've been doing major work across the Robotics Toolbox, Swift and SpatialGeometry.

ikine_min has since been replaced by the Levenberg-Marquardt solver (ik_LM), which converges well. There isn't a built-in "hold this joint" option yet, but you can get it by replacing that joint in the ETS with a constant, solving for the remaining joints, and putting the fixed value back:

import numpy as np
import roboticstoolbox as rtb

def fix_joint(ets, j, value):
    """Return a copy of an ETS with joint j locked at value."""
    parts, k = [], -1
    for e in ets:
        if e.isjoint:
            k += 1
            if k == j:      # lock this joint
                parts.append(getattr(rtb.ET, e.axis)(-value if e.isflip else value))
                continue
            # fresh joint ET: numbered automatically when the ETS is assembled
            e = getattr(rtb.ET, e.axis)(flip=e.isflip, qlim=e.qlim)
        parts.append(e)
    out = parts[0]
    for p in parts[1:]:
        out = out * p
    return out

robot = rtb.models.Panda()
ets = fix_joint(robot.ets(), 2, 0.8)            # hold joint 2 at 0.8 rad
sol = ets.ik_LM(Tep, q0=q0_without_joint_2)     # solve the other six joints
q = np.insert(sol.q, 2, 0.8)                    # put the fixed joint back

With a slider you can simply rebuild the reduced ETS each time its value changes. I'd like to make this a proper method, ETS.fixjoint(), and I've logged that as #724. On which of the remaining solutions you land, see also the discussion in #671.

If you hit a problem with this on the current release, please post an Issue with a short example.