sa1, ca1, sb1, cb1, s1, sp, cp, st, ct, sf, cf, z, y, x, 
-80*cf*cp*st+119*cf*sp*st+34*cf*ct-119*cp*st-200*s1*sb1+80*sf*sp-400*sb1+200*x+424
1000*cb1*s1*sa1-400*cp*sf*st+595*sf*sp*st+2000*cb1*sa1+595*cf*cp+400*cf*sp+170*ct*sf+1000*y-1374
-200*ca1*cb1*s1-400*ca1*cb1-80*cp*ct+119*ct*sp-34*st+200*z
ca1^2+sa1^2-1
cb1^2+sb1^2-1
