void SimBiConState::writeTrajectory1D(std::ofstream& f, Trajectory1D& result, int startingLineType, int endingLineType ){
f << "\t" << getConLineString(startingLineType) << std::endl;
for( int i=0; i < result.getKnotCount(); ++i ) {
f << "\t\t" << result.getKnotPosition(i) << " " << result.getKnotValue(i) << std::endl;
}
f << "\t" << getConLineString(endingLineType) << std::endl;
}
/**
This method is used to write a trajectory to the file
*/
void SimBiConState::writeTrajectory1D(FILE* f, Trajectory1D& result, int startingLineType, int endingLineType ){
if (f == NULL)
return;
fprintf( f, "\t%s\n", getConLineString(startingLineType) );
for( int i=0; i < result.getKnotCount(); ++i ) {
fprintf( f, "\t\t%lf %lf\n", result.getKnotPosition(i), result.getKnotValue(i) );
}
fprintf( f, "\t%s\n", getConLineString(endingLineType) );
}
/**
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);
}
