float KChainOrientationBodySchema::TryInverseKinematics(const cart_vec_t position){
int i,j;
float cand,rdist=0,tdist=0,k=0.0001; //rotation vs translation weight
CVector3_t stack[MAX_LINKS];
CVector3_t tar,newrod,rod,diff,pos;
CQuat_t q1,q2,q3,q4,rot;
// converting data format
v_copy(position.GetArray(),pos);
q_complete(position.GetArray()+3,rot);
q_inv(rot,q1);
for(i=0;i<nb_joints;i++){
GetQuaternion(i,q2);
q_multiply(q2,q1,q3);
q_copy(q3,q1);
}
for(j=0;j<50;j++){
#ifdef WITH_LAST_LINK
last_link->GetTranslation(rod);
#else
v_clear(rod);
#endif
InverseKinematicsStack(pos,stack);
for(i=nb_joints-1;i>=0;i--){
GetInverseQuaternion(i,q2);
q_multiply(q2,q1,q3); //q1*inv(Ri)
if(IsInverted(i)==-1){
v_copy(stack[i],tar);
Translate(i,rod,newrod); //getting to joint
cand = joints[i]->MinimizePositionAndRotationAngle(tar,newrod,q3,k);
cand = AngleInClosestRange(i,cand);
joints[i]->SetAngle(-cand);// todo to check if it is -
Rotate(i,newrod,rod); // updating "rod"
v_sub(tar,rod,diff);
}
else{
Rotate(i,stack[i+1],tar); //rotating back
cand = joints[i]->MinimizePositionAndRotationAngle(tar,rod,q3,k);
cand = AngleInClosestRange(i,cand);
joints[i]->SetAngle(cand);
Rotate(i,rod,newrod);
Translate(i,newrod,rod);
v_sub(tar,newrod,diff);
}
GetQuaternion(i,q2);
q_multiply(q3,q2,q1);
q_multiply(q2,q3,q4);
rdist = v_length(q4);//rotation distance, only the first 3 components
tdist = v_length(diff);//translation distance
// cout<<"rot "<<rdist<<" pos "<<tdist<<" prod: "<<(1-k)*rdist+k*tdist<<endl;
if(tdist<tol && rdist<rot_tol){return rdist/rot_tol+tdist;}
}
q_multiply(rot,q1,q2);
q_inv(rot,q3);
q_multiply(q2,q3,q1);
}
return rdist/rot_tol + tdist;
}
/*
* EMX shutdown
*/
static void Term_nuke_emx(term *t)
{
/* Move the cursor to bottom of screen */
v_gotoxy(0, 23);
/* Restore the cursor (not necessary) */
v_ctype(curs_start, curs_end);
/* Set attribute to gray on black */
v_attrib(F_WHITE);
/* Clear the screen */
v_clear();
}
int Reaching::SetActualRobPosition(joint_vec_t& angles){
if(AcceptablePosition(angles)){
v4_copy(angles,pos_angle);
v4_set(0,0,0,0,v_angle);
v_set(0,0,0,v_cart);
Angle2Cart(pos_angle,pos_cart);
CVector2ArmConfig(pos_angle,&position);
v_clear(tar_angle);
return 1;
}
else{
return 0;
}
}
/*
* EMX initialization
*/
static void Term_init_emx(term *t)
{
struct _KBDINFO kbdinfo; /* see structure description ?somewhere? */
v_init();
v_getctype(&curs_start, &curs_end);
/* hide cursor (?) XXX XXX XXX */
v_clear();
/* the documentation I (SWD) have implies, in passing, that setting */
/* "binary mode" on the keyboard device will prevent the O/S from */
/* acting on keys such as ^S (pause) and ^P (printer echo). */
/* note also that "KbdSetStatus is ignored for a Vio-windowed application." */
/* so there may well be problems with running this in a window. Damnit. */
/* this is kind of a nasty structure, as you can't just flip a bit */
/* to change binary/ASCII mode, or echo on/off mode... nor can you */
/* clear the whole thing -- certain bits need to be preserved. */
KbdGetStatus(&kbdinfo, (HKBD)0);
kbdinfo.fsMask &= ~ (KEYBOARD_ECHO_ON| /* clear lowest four bits */
KEYBOARD_ECHO_OFF|KEYBOARD_BINARY_MODE|KEYBOARD_ASCII_MODE);
kbdinfo.fsMask |= (KEYBOARD_BINARY_MODE); /* set bit two */
KbdSetStatus(&kbdinfo, (HKBD)0);
#if 1 /* turn off for debug */
signal(SIGHUP, SIG_IGN);
signal(SIGINT, SIG_IGN);
signal(SIGQUIT, SIG_IGN);
/* signal(SIGILL,SIG_IGN); */
/* signal(SIGTRAP,SIG_IGN); */
/* signal(SIGABRT,SIG_IGN); */
/* signal(SIGEMT,SIG_IGN); */
/* signal(SIGFPE,SIG_IGN); */
/* signal(SIGBUS,SIG_IGN); */
/* signal(SIGSEGV,SIG_IGN); */
/* signal(SIGSYS,SIG_IGN); */
signal(SIGPIPE, SIG_IGN);
signal(SIGALRM, SIG_IGN);
/* signal(SIGTERM,SIG_IGN); */
signal(SIGUSR1, SIG_IGN);
signal(SIGUSR2, SIG_IGN);
signal(SIGCHLD, SIG_IGN);
signal(SIGBREAK, SIG_IGN);
#endif
}
int Reaching::LinkDistanceToObject(int link, EnvironmentObject *obj, float *dist,
float *point, cart_vec_t& contact_vector){
cart_vec_t& objPos;
cart_vec_t& lowerLinkExtr;
cart_vec_t& upperLinkExtr;
cart_vec_t& v1,v2,linkv,u,vertexPos,v_tmp,tmp3;
CMatrix4_t ref,tmpmat,tmpmat2;
cart_vec_t& s1,s2;
int i,j,k,dir1,dir2,pindex,min_i;
float alldists[12]; //closest distance to each edge
float allpoints[24]; // closest pair of points on each edge
float min_dist;
int s3[3];
for (i=0;i<12;i++){
alldists[i]=FLT_MAX;
}
switch(link){
case 1:
v_clear(upperLinkExtr);
ElbowPosition(pos_angle,lowerLinkExtr);
break;
case 2:
ElbowPosition(pos_angle,upperLinkExtr);
v_copy(pos_cart,lowerLinkExtr);
break;
default:
cout<<"unvalid link number"<< endl;
}
if(!obj){
return 0;
}
// v_sub(obj->solid->m_position, shoulder_abs_pos,objPos);
Abs2LocalRef(obj->GetPosition(), objPos);
// cout<<"ule: ";coutvec(upperLinkExtr);
// cout<<"lle: ";coutvec(lowerLinkExtr);
// coutvec(objPos);
v_sub(lowerLinkExtr,objPos,v1);
v_sub(upperLinkExtr,objPos,v2);
//m_transpose(obj->solid->m_ref.m_orient,ref); //stupid to do it each time
m_copy(obj->solid->m_ref.m_orient,ref); //stupid to do it each time
v_clear(s3);
//checking when two parallel sides must me checked
for(i=0;i<3;i++){
s1[i] = v_dot(v1, &ref[i*4]);
s2[i] = v_dot(v2, &ref[i*4]);
if (s1[i]*s2[i]>=0){
s3[i] = sign(s1[i]+s2[i]);
}
}
// cout << "s3: ";coutvec(s3);
m_copy(ref,tmpmat);
for(i=0;i<3;i++){
if(s3[i]){
v_sub(lowerLinkExtr,upperLinkExtr,linkv);
v_scale(obj->solid->m_size,-0.5,u);
u[i] *=-s3[i];
v_add(objPos,u,vertexPos);
// cout<<"vPos "<<i<<": ";coutvec(vertexPos);
v_scale(&(ref[i*4]),s3[i],&(tmpmat[i*4]));
// cout<<"norm "<<i<<": ";coutvec((tmpmat+i*4));
m_inverse(tmpmat,tmpmat2);
if(v_dot(&(tmpmat[i*4]),linkv)<0){
v_sub(lowerLinkExtr,vertexPos,v_tmp);
*point = 1;
}
else{
v_sub(upperLinkExtr,vertexPos,v_tmp);
*point = 0;
}
// cout<<"linkv ";coutvec(linkv);
// cout <<"pt "<<*point<<endl;
// #ifdef OLD_AV
// v_copy(linkv,(cart_vec_t&)&tmpmat[i*4]);
// m_inverse(tmpmat,tmpmat2);
// v_sub(upperLinkExtr,vertexPos,tmp2);
// tmp3 should contain the intersection coordinates in
// the referential defined by the edges of the surface
v_transform_normal(v_tmp,tmpmat2,tmp3);
// cout<<"tmp3 ";coutvec(tmp3);
if(tmp3[(i+1)%3]>=0 && tmp3[(i+2)%3]>=0 // the link points to the rectangle
&& tmp3[(i+1)%3]<=obj->solid->m_size[(i+1)%3] &&
tmp3[(i+2)%3]<=obj->solid->m_size[(i+2)%3]){
if(tmp3[i]<0){
return 0; // there is a collision
}
else{
*dist = tmp3[i];
v_copy(&(tmpmat[i*4]),contact_vector);
v_scale(contact_vector,*dist,contact_vector);
return 1;
}
}
}
}
// the link does not point to a rectangle -> look for the edges
v_scale(obj->solid->m_size,-0.5,u);
for(i=0;i<3;i++){// each kind of edge
dir1 = s3[(i+1)%3];
dir2 = s3[(i+2)%3];
for(j=0;j<2;j++){
if(dir1 == 0 || dir1==2*j-1){ //edges of this face must be computed
for(k=0;k<2;k++){
if(dir2 == 0 || dir2==2*k-1){ //edges of this face must be computed
v_copy(u,v_tmp);
v_tmp[(i+1)%3]*=-(2*j-1);
v_tmp[(i+2)%3]*=-(2*k-1);
v_add(objPos,v_tmp,vertexPos);
v_scale(&(ref[4*i]),obj->solid->m_size[i],v1); // edge with the right length
pindex = 4*i+2*j+k;
FindSegmentsNearestPoints(vertexPos,v1,upperLinkExtr,linkv,&(allpoints[2*pindex]),&(allpoints[2*pindex+1]),&(alldists[pindex]));
// cout<<"i:"<<i<<" j:"<<j<<" k:"<<k<<"dist "<<alldists[pindex]<<endl;
// cout<<"v1:";coutvec(v1);
// cout<<"ref:";coutvec((&(ref[4*i])));
// cout<<"vP:";coutvec(vertexPos);
}
}
}
}
}
//looking for the min
min_dist = alldists[0];
min_i = 0;
for(i=1;i<12;i++){
if(alldists[i] < min_dist){
min_dist = alldists[i];
min_i = i;
}
}
// returning the min distance
*dist = min_dist;
*point = allpoints[2*min_i+1];
v_scale(linkv,*point,v1);
v_add(upperLinkExtr,v1,v2); // nearest point on link
k = min_i%2; //retrieving the right edge
j = ((min_i-k)%4)/2;
i = min_i/4;
// cout<<"ijk: "<<i<<" "<<j<<" "<<k<<endl;
v_copy(u,v_tmp);
v_tmp[(i+1)%3]*=-(2*j-1);
v_tmp[(i+2)%3]*=-(2*k-1);
v_add(objPos,v_tmp,vertexPos); // starting vertex of the edge
v_scale(&(ref[3*1]),allpoints[2*min_i]*(obj->solid->m_size[min_i]),v1);
v_add(vertexPos,v1,v1); // nearest point on solid
v_sub(v2,v1,contact_vector);
return 1;
}
static void menu_cb_clear (TextEditor me, EDITOR_ARGS_DIRECT) {
my v_clear ();
}
void NodeStorageBase::clear()
{
v_clear();
}
