Newton-Raphson for Spherical Coordinate Robot
Find: 
Given: x = 689 mm, y = 579 mm, z = 600 mm
Subject to: fx =  cos cos - 689 = 0
fy = sin cos - 579 = 0
fz = sin - 600 = 0

Use: q   generalized coordinates,

f x 
  f y 
f z 
constraint functions
Taylor series expansion of {} about estimate {q}k
 f x f x f x 
    
f x 
f x 
 f f f  
 
 

y
y
y 
{}k+1
f y   f y   

      
f 
f 
 z  k 1  z  k  f z f z f z  


     k
cos  cos 
J    sin  cos 


{}k + [J]k {q}

- [J]k-1 {}k
  sin  cos    cos  sin  
 cos  cos 
sin 
0
  sin  sin  
 cos 

Desired constraint functions:
{}k+1 = 0
Newton-Raphson equation:
{q}k+1 = {q}k - [J]k-1 {}k
k
1
2
3
4
5

q  
 
 
[mm,deg]
600
0
0
689
55
57
974
19
23
1009
40
36
1081
40
33
f x 
  f y 
f 
 z
[mm]
-89
-579
-600
-477
-273
-20
156
-290
-212
-63
-59
-4
0
1
-3
{q}
[J]
[mm/mm, mm/rad]
1
0
0
0.308
0.444
0.842
0.868
0.297
0.399
0.621
0.516
0.590
0.638
0.537
0.552
0
600
0
-306
212
0
-289
845
0
-520
626
0
-580
689
0
0
0
600
–330
-477
372
–367
–126
893
-458
-381
814
-457
-384
901
[J]-1{}
[mm,rad]
-89
-0.965
-1.000
-285
0.636
0.590
-35
-0.364
-0.221
-72
-0.006
0.047
-1
0.001
-0.003
[q]-1{}
[mm,deg]
-89
-55
-57
-285
36
34
-35
-21
-13
-72
0
3
-1
0.034
-0.179