void newbaseline ( char *buf /* whatever followed "x X NewBaseline" */ ) { char *p; /* for eliminating white space etc. */ /* * * Called from devcntrl() whenever an "x X NewBaseline" command is recognized. We * assume whatever is in *buf is a set of parametric equations that describe the * new baseline. Equations for x(t), y(t), dx/dt, and dy/dt must be written in * PostScript, bracketed by { and } characters, and supplied in exactly that order. * In particular the equation for x must come first in *buf and it ends up as the * last one on the stack, while the equation for dy/dt comes last (in *buf) and * ends up on the top of the PostScript stack. For example if *buf is given by, * * {} {180 mul 3.1416 div cos} {pop 1} {180 mul 3.1416 div sin neg} * * text will be printed along the curve y = cos(x). * * Angles given in radians must be converted to degrees for the PostScript trig * functions, and things are scaled so that 1 unit maps into 1 inch. In the last * example the cosine curve that describes the baseline has an amplitude of 1 inch. * As another example of this rather confusing syntax if *buf is, * * {} {} {pop 1} {pop 1} * * the baseline will be the 45 degree line y = x. * * When any of the four functions is used they're called with a single number on * the stack that's equal to the current value of the parameter t. The coordinate * system axes run parallel to the PostScript coordinate system that's currently * being used. * */ for ( p = buf; *p; p++ ) /* eliminate trailing '\n' */ if ( *p == '\n' ) { *p = '\0'; break; } /* End if */ for ( p = buf; *p && (*p == ' ' || *p == ':'); p++ ) ; if ( *p != '\0' ) { /* something's there */ endtext(); getbaseline(); fprintf(tf, "mark resolution %s newbaseline\n", p); t_sf(1); resetpos(); } /* End if */ } /* End of newbaseline */
void NextStep:: nextStep( const int & timeCurr ) { sotDEBUGIN(15); const unsigned& sfoot = contactFootSIN( timeCurr ); const MatrixHomogeneous& wMlf = twoHandObserver.leftFootPositionSIN.access( timeCurr ); const MatrixHomogeneous& wMrf = twoHandObserver.rightFootPositionSIN.access( timeCurr ); // actual and reference position of reference frame in fly foot, // position of fly foot in support foot. MatrixHomogeneous ffMref, ffMref0; MatrixHomogeneous sfMff; if( sfoot != 1 ) // --- left foot support --- { ffMref = referencePositionRightSIN.access( timeCurr ); ffMref0 = rfMref0; MatrixHomogeneous sfMw; sfMw = wMlf.inverse(); sfMff = sfMw*wMrf; } else // -- right foot support --- { ffMref = referencePositionLeftSIN.access( timeCurr ); ffMref0 = lfMref0; MatrixHomogeneous sfMw; sfMw = wMrf.inverse(); sfMff = sfMw*wMlf; } // homogeneous transform from ref position of ref frame to // actual position of ref frame. MatrixHomogeneous ref0Mff; ref0Mff = ffMref0.inverse(); MatrixHomogeneous ref0Mref; ref0Mref = ref0Mff * ffMref; // extract the translation part and express it in the support // foot frame. MatrixHomogeneous sfMref0; sfMref0 = sfMff * ffMref0; Vector t_ref0(3); t_ref0 = ref0Mref.translation(); MatrixRotation sfRref0; sfRref0 = sfMref0.linear(); Vector t_sf = sfRref0 * t_ref0; // add it to the position of the fly foot in support foot to // get the new position of fly foot in support foot. Vector pff_sf(3); pff_sf = sfMff.translation(); t_sf += pff_sf; // compute the rotation that transforms ref0 into ref, // express it in the support foot frame. Then get the // associated yaw (rot around z). MatrixRotation ref0Rsf; ref0Rsf = sfRref0.transpose(); MatrixRotation ref0Rref; ref0Rref = ref0Mref.linear(); MatrixRotation tmp = ref0Rref * ref0Rsf; MatrixRotation Rref = sfRref0*tmp; VectorRollPitchYaw rpy; rpy = (Rref.eulerAngles(2,1,0)).reverse(); // get the yaw of the current orientation of the ff wrt sf. // Add it to the previously computed rpy. MatrixRotation sfRff; sfRff = sfMff.linear(); VectorRollPitchYaw rpy_ff; rpy_ff = (sfRff.eulerAngles(2,1,0)).reverse(); rpy += rpy_ff; // Now we can compute and insert the new step (we just need // to express the coordinates of the vector that joins the // support foot to the new fly foot in the coordinate frame of the // new fly foot). // // [dX;dY] = A^t [X;Y] // // where A is the planar rotation matrix of angle theta, [X;Y] // is the planar column-vector joining the support foot to the new fly foot, // expressed in the support foot frame, and [dX;dY] is this same planar // column-vector expressed in the coordinates frame of the new fly foot. // // See the technical report of Olivier Stasse for more details, // on top of page 79. double ns_x = 0, ns_y = 0, ns_theta = 0; if(mode != MODE_1D) { ns_theta = rpy(2) * 180 / 3.14159265; if(fabs(ns_theta) < 10) { ns_theta = 0; rpy(2) = 0; } double x = t_sf(0); double y = t_sf(1); double ctheta = cos(rpy(2)); double stheta = sin(rpy(2)); ns_x = x * ctheta + y * stheta; ns_y = -x * stheta + y * ctheta; ns_theta = rpy(2) * 180 / 3.14159265; if(fabs(ns_theta) < 10){ ns_theta = 0; } } else { ns_x = t_sf(0); if(sfoot != 1){ ns_y = -ZERO_STEP_POSITION_DEFAULT; } else{ ns_y = ZERO_STEP_POSITION_DEFAULT; } ns_theta = 0.; } FootPrint newStep; if(sfoot != 1){ newStep.contact = CONTACT_LEFT_FOOT; } else{ newStep.contact = CONTACT_RIGHT_FOOT; } newStep.x = ns_x; newStep.y = ns_y; newStep.theta = ns_theta; newStep.introductionTime = timeCurr; footPrintList.push_back( newStep ); footPrintList.pop_front(); sotDEBUGOUT(15); }
void picture ( char *buf /* stuff following 'x X PI' command */ ) { int poffset; /* page offset */ int indent; /* indent */ int length; /* line length */ int totrap; /* distance to next trap */ char name[4096]; /* picture file and page string */ char hwo[4096], *p; /* height, width and offset strings */ char flags[4096]; /* miscellaneous stuff */ int page = 1; /* page number pulled from name[] */ double frame[4]; /* height, width, y, and x offsets from hwo[] */ char units; /* scale indicator for frame dimensions */ int whiteout = 0; /* white out the box? */ int outline = 0; /* draw a box around the picture? */ int scaleboth = 0; /* scale both dimensions? */ double adjx = 0.5; /* left-right adjustment */ double adjy = 0.5; /* top-bottom adjustment */ double rot = 0; /* rotation in clockwise degrees */ FILE *fp_in; /* for *name */ int i; /* loop index */ /* * * Called from devcntrl() after an 'x X PI' command is found. The syntax of that * command is: * * x X PI:args * * with args separated by colons and given by: * * poffset * indent * length * totrap * file[(page)] * height[,width[,yoffset[,xoffset]]] * [flags] * * poffset, indent, length, and totrap are given in machine units. height, width, * and offset refer to the picture frame in inches, unless they're followed by * the u scale indicator. flags is a string that provides a little bit of control * over the placement of the picture in the frame. Rotation of the picture, in * clockwise degrees, is set by the a flag. If it's not followed by an angle * the current rotation angle is incremented by 90 degrees, otherwise the angle * is set by the number that immediately follows the a. * */ if ( picflag == OFF ) /* skip it */ return; endtext(); flags[0] = '\0'; /* just to be safe */ if ( sscanf(buf, "%d:%d:%d:%d:%[^:]:%[^:]:%[^:]", &poffset, &indent, &length, &totrap, name, hwo, flags) < 6 ) { error(NON_FATAL, "too few arguments to specify picture"); return; } /* End if */ if ( sscanf(name, "%*[^(](%d", &page) == 1 ) /* grab the page number */ strtok(name, "("); /* and separate it from the name */ if ( (fp_in = picopen(name)) == NULL ) { error(NON_FATAL, "can't open picture file %s", name); return; } /* End if */ frame[0] = frame[1] = -1; /* default frame height, width */ frame[2] = frame[3] = 0; /* and y and x offsets */ for ( i = 0, p = hwo-1; i < 4 && p != NULL; i++, p = strchr(p, ',') ) if ( sscanf(++p, "%lf%c", &frame[i], &units) == 2 ) if ( units == 'i' || units == ',' || units == '\0' ) frame[i] *= res; if ( frame[0] <= 0 ) /* check what we got for height */ frame[0] = totrap; if ( frame[1] <= 0 ) /* and width - check too big?? */ frame[1] = length - indent; frame[3] += poffset + indent; /* real x offset */ for ( i = 0; flags[i]; i++ ) switch ( flags[i] ) { case 'c': adjx = adjy = 0.5; break; /* move to the center */ case 'l': adjx = 0; break; /* left */ case 'r': adjx = 1; break; /* right */ case 't': adjy = 1; break; /* top */ case 'b': adjy = 0; break; /* or bottom justify */ case 'o': outline = 1; break; /* outline the picture */ case 'w': whiteout = 1; break; /* white out the box */ case 's': scaleboth = 1; break; /* scale both dimensions */ case 'a': if ( sscanf(&flags[i+1], "%lf", &rot) != 1 ) rot += 90; } /* End switch */ fprintf(tf, "save mark\n"); fprintf(tf, "[ /NamespacePush pdfmark\n"); ps_include(name, fp_in, tf, page, whiteout, outline, scaleboth, frame[3]+frame[1]/2, -vpos-frame[2]-frame[0]/2, frame[1], frame[0], adjx, adjy, -rot); fprintf(tf, "[ /NamespacePop pdfmark\n"); fprintf(tf, "cleartomark restore\n"); xymove(hpos, vpos); t_sf(1); fclose(fp_in); } /* End of picture */