A runnable reference implementation

Chapters 10.3 and 11.5 give the forward and inverse kinematics in the form that reads most easily. This is the same maths in the form that runs: forward kinematics, the closed-form inverse, and the round-trip self-test in one file, with nothing elided.

Save it as ik_full.py and run it with NumPy installed. It draws 5,000 random joint configurations, puts each through forward kinematics to get a pose, solves that pose, and checks that every returned branch reproduces the original transform. On this machine it prints:

round-trip worst err = 2.274e-13 | no-solution = 0 | mean #sols = 7.32

A worst-case error of $2.3 \times 10^{-13}$ is floating-point noise, no-solution = 0 means no reachable pose was missed, and a mean of 7.3 solutions per pose is what the eight branches of Chapter 11.6 give once the near-singular configurations collapse a pair. An implementation that cannot reproduce these three numbers has something wrong with it, and this is the quickest way to find out.

import numpy as np
a1,a2,a3,a4,a5,a6,a7=110.50,23.42,180.00,43.50,176.35,62.8,45.25
pi=np.pi
DH=[(0.0,-pi/2,a1,a2),(-pi/2,pi,0.0,a3),(pi,pi/2,0.0,-a4),
    (0.0,-pi/2,-a5,0.0),(0.0,pi/2,0.0,0.0),(pi,pi,-a6,-a7)]
L=np.hypot(a4,a5); PHI=np.arctan2(a4,a5)
def Tm(t,al,d,a):
    ct,st,ca,sa=np.cos(t),np.sin(t),np.cos(al),np.sin(al)
    return np.array([[ct,-st*ca,st*sa,a*ct],[st,ct*ca,-ct*sa,a*st],[0,sa,ca,d],[0,0,0,1]])
def fk(q):
    T=np.eye(4);Ts=[]
    for i in range(6):
        o,al,d,a=DH[i];T=T@Tm(q[i]+o,al,d,a);Ts.append(T.copy())
    return T,Ts
def wrap(x): return (x+pi)%(2*pi)-pi

def ik(R,p):
    pw=p+R@np.array([a7,0.0,-a6])
    x,y,z=pw; out=[]
    r=np.hypot(x,y)
    for th1,sgn in ((np.arctan2(y,x),1.0),(wrap(np.arctan2(y,x)+pi),-1.0)):
        u=sgn*r-a2; v=a1-z
        D2=u*u+v*v
        c=(D2-a3*a3-L*L)/(2*a3*L)
        if abs(c)>1: continue
        for s in (+1.0,-1.0):
            B=s*np.arccos(np.clip(c,-1,1))
            A=np.arctan2(v,u)-np.arctan2(L*np.sin(B),a3+L*np.cos(B))
            th2=wrap(A+pi/2); th3=wrap(pi/2-PHI-B)
            _,Ts=fk([th1,th2,th3,0,0,0])
            R36=Ts[2][:3,:3].T@R
            r13,r23,r33,r31,r32=R36[0,2],R36[1,2],R36[2,2],R36[2,0],R36[2,1]
            sf=np.hypot(r13,r23)
            if sf<1e-8:
                th5=0.0 if -r33>0 else pi; th4=0.0
                th6=wrap(np.arctan2(R36[1,0],R36[0,0]))
                out.append([th1,th2,th3,th4,th5,th6]); continue
            th5=np.arctan2(sf,-r33)
            th4=np.arctan2(-r23,-r13); th6=np.arctan2(r32,r31)
            out.append([th1,th2,th3,th4,th5,th6])
            out.append([th1,th2,th3,wrap(th4+pi),-th5,wrap(th6+pi)])
    return out

if __name__=="__main__":
    rng=np.random.default_rng(7); worst=0; nofind=0; counts=[]
    for _ in range(5000):
        q=rng.uniform(-2.5,2.5,6)
        T,_=fk(q); R,p=T[:3,:3],T[:3,3]
        sols=ik(R,p); counts.append(len(sols))
        if not sols: nofind+=1; continue
        best=min(np.abs(fk(s)[0]-T).max() for s in sols)
        worst=max(worst,best)
        # every returned solution must be exact
        allerr=max(np.abs(fk(s)[0]-T).max() for s in sols)
        if allerr>1e-6: print("BAD sol", allerr); break
    print("round-trip worst err = %.3e | no-solution = %d | mean #sols = %.2f"%(worst,nofind,np.mean(counts)))

The variable names are terse where the chapter versions are spelled out, so that the whole solver sits on one screen next to the algebra of Chapter 11. The DH constants are identical to the table in Chapter 10.2.