The background of this script is based on Denavit–Hartenberg parameters. And for two adjacent links i and (i-1) on the robot arm, we have the coordinate transformation: where the individual parameters ...