Example 4

This example is similar to example 1 except that we deal with the spatial case. The pose of the end-effector is defined by 6 parameters: the coordinates x,y,z of a specific point on the end-effector and three angles p,t,h which define its orientation. We have now 6 linear actuators connecting the end-effector and the ground. The length of the linear actuator are all equal to 60. When expressing the leg lengths as function of the parameters we may extract from the 6 equations a linear system in x,y,z. Solving this system and substituting in the remaining equations leads to a system of three equations in the unknowns p,t,h. The following Maple program enable to compute these equations.

 
with(linalg):
xa1:= -9:ya1:=9:xa2:=9:ya2:=9:
xa3:=12:ya3:=-3:xa4:=3:ya4:=-13:
xa5:=-3:ya5:=-13:xa6:=-12:ya6:=-3:
xb1:= -3:yb1:=7:xb2:=3:yb2:=7:
xb3:=7:yb3:=-1:xb4:=4:yb4:=-6:
xb5:=-4:yb5:=-6:xb6:=-7:yb6:=-1:

rot:=array([[cos(p)*cos(h)-sin(p)*cos(t)*sin(h)
,-cos(p)*sin(h)-sin(p)*cos(t)*cos(h),
sin(p)*sin(t)],[sin(p)*cos(h)+cos(p)*cos(t)*sin(h),
-sin(p)*sin(h)+cos(p)*cos(t)*cos(h),-cos(p)*sin(t)],
[sin(t)*sin(h),sin(t)*cos(h),cos(t)]]):

for i from 1 to 6 do
      OA.i:=array([xa.i,ya.i,0]):
      CBr.i:=array([xb.i,yb.i,0]):
      CB.i:=multiply(rot,CBr.i):
od:

cen:=array([x,y,z]):

for i from 1 to 6 do
      AB.i:=evalm(cen-OA.i+CB.i):
      ro.i:=dotprod(AB.i,AB.i,`orthogonal`):
      ro.i:=simplify(ro.i,trig):
od:
for i from 1 to 6 do eq.i:=ro.i-60^2: od:
er1:=eq2-eq1:er2:=eq3-eq1:er3:=eq4-eq1:er4:=eq5-eq1:er5:=eq6-eq1:
sw:=solve({er1,er2,er3},{x,y,z}):
assign(sw):

en1:=numer(eq1):en2:=numer(er4):en3:=numer(er5):

for i from 1 to 3 do
      en.i:=simplify(en.i,trig):
      en.i:=convert(en.i,horner,[sin(p),cos(p),sin(t),cos(t),sin(h),cos(h)]):
od:

We are looking for a solution in the domain:

$$[4.537856054,4.886921908], [1.570796327,1.745329252], [0.6981317008, 0.8726646262]$$

The system has a solution which is approximatively:

$$4.6616603883, 1.70089818026, 0.86938888189$$

Here we may use the general solving algorithm (section 2.3), the general solving algorithm with the gradient (section 2.4), the general solving algorithm with the gradient and Hessian (section 2.5).

These algorithms are implemented in the test program Test_Solve_General, Test_Solve_Gradient_General, Test_Solve_JH_General (in that later case Kantorovitch theorem is applied only at level 1).