/** This method is used to read the knots of a strength trajectory from the file, where they are specified one (knot) on a line */ void SimBiConState::readTrajectory1D(FILE* f, Trajectory1D& result, int endingLineType ){ //have a temporary buffer used to read the file line by line... char buffer[200]; double temp1, temp2; //this is where it happens. while (!feof(f)){ //get a line from the file... fgets(buffer, 200, f); char *line = lTrim(buffer); int lineType = getConLineType(line); if( lineType == endingLineType ) //we're done... return; switch (lineType) { case CON_COMMENT: break; case CON_NOT_IMPORTANT: //we expect pairs of numbers, one pair on each row, so see if we have a valid pair if (sscanf(line, "%lf %lf", &temp1, &temp2) == 2){ result.addKnot(temp1, temp2); }else { } break; default: return; } } return; }
/** Update this component to recenter it around the new given D and V trajectories */ void TrajectoryComponent::updateComponent(SimBiController* con, Joint* j, Trajectory1D& newDTrajX, Trajectory1D& newDTrajZ, Trajectory1D& newVTrajX, Trajectory1D& newVTrajZ, Trajectory1D* oldDTrajX, Trajectory1D* oldDTrajZ, Trajectory1D* oldVTrajX, Trajectory1D* oldVTrajZ, int nbSamples ) { if( bFeedback == NULL ) return; double startPhi = 0; double endPhi = 0; if( baseTraj.getKnotCount() > 0 ) { startPhi = min( startPhi, baseTraj.getMinPosition() ); endPhi = min( startPhi, baseTraj.getMaxPosition() ); } if( newDTrajX.getKnotCount() > 0 ) { startPhi = max( startPhi, newDTrajX.getMinPosition() ); endPhi = max( startPhi, newDTrajX.getMaxPosition() ); } Trajectory1D result; Vector3d d0, v0, newD0, newV0; for( int i = 0; i < nbSamples; ++i ) { double interp = (double) i / (nbSamples - 1.0); double phi = startPhi * (1.0 - interp) + endPhi * interp; double baseAngle = 0; if (baseTraj.getKnotCount() > 0) baseAngle = baseTraj.evaluate_catmull_rom(phi); SimBiController::computeDorV( phi, &newDTrajX, &newDTrajZ, LEFT_STANCE, &newD0 ); SimBiController::computeDorV( phi, &newVTrajX, &newVTrajZ, LEFT_STANCE, &newV0 ); SimBiController::computeDorV( phi, oldDTrajX, oldDTrajZ, LEFT_STANCE, &d0 ); SimBiController::computeDorV( phi, oldVTrajX, oldVTrajZ, LEFT_STANCE, &v0 ); double feedback = computeFeedback(con, j, phi, newD0 - d0, newV0 - v0); if (reverseAngleOnLeftStance) baseAngle -= feedback; else baseAngle += feedback; result.addKnot( phi, baseAngle ); } result.simplify_catmull_rom( 0.005 ); baseTraj.copy( result ); }
/** This method is used to read the knots of a strength trajectory from the file, where they are specified one (knot) on a line */ void SimBiConState::readTrajectory1D(FILE* f, Trajectory1D& result, int endingLineType ){ if (f == NULL) throwError("File pointer is NULL - cannot read gain coefficients!!"); //have a temporary buffer used to read the file line by line... char buffer[200]; double temp1, temp2; //this is where it happens. while (!feof(f)){ //get a line from the file... fgets(buffer, 200, f); if (strlen(buffer)>195) throwError("The input file contains a line that is longer than ~200 characters - not allowed"); char *line = lTrim(buffer); int lineType = getConLineType(line); if( lineType == endingLineType ) //we're done... return; switch (lineType) { case CON_COMMENT: break; case CON_NOT_IMPORTANT: //we expect pairs of numbers, one pair on each row, so see if we have a valid pair if (sscanf(line, "%lf %lf", &temp1, &temp2) == 2){ result.addKnot(temp1, temp2); }else tprintf("Ignoring input line: \'%s\'\n", line); break; default: throwError("Incorrect SIMBICON input file: \'%s\' - unexpected line.", buffer); } } throwError("Incorrect SIMBICON input file: Trajectory not closed ", buffer); }