monitor("lezione16-04-26.txt","io");

//---Robot dell'esempio 2, [CLO, p. 300]
//   Exercise 16, p. 317

ring r=(0,l4,l3,l2,a,b,c,s),(c2,s2,c1,s1),lp;
option(redSB);

ideal id=l4*c+l3*(c1*c2-s1*s2)+l2*c1-a,
l4*s+l3*(c1*s2+c2*s1)+l2*s1-b,
c1^2+s1^2-1,c2^2+s2^2-1;

ideal j=std(id);
dim(j);

//---j e' la base di Groebner con parametri.

j;

//---Risolviamo il problema cinematico inverso, cioe'
//   dato il posizionamento e l'orientamento della mano 
//   (a,b,c,s) = (1,1,1,0)
//   e la lunghezza del giunto prismatico 
//   l4 = 1, 1.1, 1.2, 1.3, 1.4, 1.5, 1.6, 1.7, 1.8, 1.9, 2
//   troviamo ogni volta (c1,s1,c2,s2) = elenco l.

LIB "solve.lib";
ring re2=0,(c1,s1,c2,s2),dp;
ring re=0,(c1,s1,c2,s2,l4,a,b,c,s),dp;
setring(re);

//---Robot 2 con l_3 = l_2 = 1:

ideal id=l4*c+c1*c2-s1*s2+c1-a,
l4*s+c1*s2+c2*s1+s1-b,
c1^2+s1^2-1,c2^2+s2^2-1;

std(id);

list l;
int i;
for (i=0; i<=10; i=i+1)
{setring(re);
ideal ids=subst(id,a,1,b,1,l4,1+number(i)/10,c,1,s,0);
setring(re2);
ideal id2=imap(re,ids);
ideal id2s=std(id2);
l=insert(l,solve(id2s,6,0));
};

//---Comandi per esaminare solo la prima soluzione della lista l:

def r1 = l[1];
setring r1;
SOL;

//---Il punto (0,4) e' raggiungibile solo con 
//   l'orientamento della mano (c,s)=(0,1) e in un solo modo:

setring(re);
ideal ids=subst(id,a,0,b,4,l4,2,c,0,s,1);
setring re2;
ideal id2=imap(re,ids);
solve(id2,6,1);

monitor("");
quit;