We have recently completed a self made controller hardware for a Mitsubishi RM-501 Robot (Original controller missing), and we would like to create a IK solver similar as shown in this picture circled in red:
Can someone provide any suggestions where to start?