void LinearBalanceFeedback::loadFromFile(FILE* f){
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];
//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);
switch (lineType) {
case CON_FEEDBACK_END:
//we're done...
return;
break;
case CON_COMMENT:
break;
case CON_CV:
if (sscanf(line, "%lf", &this->cv)!=1)
throwError("A cv value must be specified!");
break;
case CON_CD:
if (sscanf(line, "%lf", &this->cd)!=1)
throwError("A cd value must be specified!");
break;
case CON_D_MIN:
if (sscanf(line, "%lf", &this->dMin)!=1)
throwError("A dMin value must be specified!");
break;
case CON_D_MAX:
if (sscanf(line, "%lf", &this->dMax)!=1)
throwError("A dMax value must be specified!");
break;
case CON_V_MIN:
if (sscanf(line, "%lf", &this->vMin)!=1)
throwError("A vMin value must be specified!");
break;
case CON_V_MAX:
if (sscanf(line, "%lf", &this->vMax)!=1)
throwError("A vMax value must be specified!");
break;
case CON_FEEDBACK_PROJECTION_AXIS:
if (sscanf(line, "%lf %lf %lf", &this->feedbackProjectionAxis.x, &this->feedbackProjectionAxis.y, &this->feedbackProjectionAxis.z)!=3)
throwError("The axis for a trajectory is specified by three parameters!");
this->feedbackProjectionAxis.toUnit();
break;
case CON_NOT_IMPORTANT:
tprintf("Ignoring input line: \'%s\'\n", line);
break;
default:
throwError("Incorrect SIMBICON input file: \'%s\' - unexpected line.", buffer);
}
}
throwError("Incorrect SIMBICON input file: No \'/jointTrajectory\' found ", buffer);
}
/**
This method is used to read a trajectory from a file
*/
void Trajectory::readTrajectory(FILE* f){
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];
TrajectoryComponent* newComponent;
//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);
switch (lineType) {
case CON_STRENGTH_TRAJECTORY_START:
//read in the base trajectory
if( strengthTraj != NULL )
throwError( "Two strength trajectory, this is illegal!" );
strengthTraj = new Trajectory1D();
SimBiConState::readTrajectory1D(f, *strengthTraj, CON_STRENGTH_TRAJECTORY_END );
break;
case CON_TRAJECTORY_END:
//we're done...
return;
break;
case CON_CHAR_FRAME_RELATIVE:
relToCharFrame = true;
break;
case CON_COMMENT:
break;
case CON_TRAJ_COMPONENT:
//read in the base trajectory
newComponent = new TrajectoryComponent();
newComponent->readTrajectoryComponent(f);
components.push_back(newComponent);
break;
case CON_NOT_IMPORTANT:
tprintf("Ignoring input line: \'%s\'\n", line);
break;
default:
throwError("Incorrect SIMBICON input file: \'%s\' - unexpected line.", buffer);
}
}
throwError("Incorrect SIMBICON input file: No \'/trajectory\' found ", buffer);
}
/**
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);
}
/**
This method is used to read the state parameters from a file
*/
void SimBiConState::readState(FILE* f, int offset){
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];
Trajectory* tempTraj;
//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);
switch (lineType) {
case CON_STATE_END:
//we're done...
return;
break;
case CON_NEXT_STATE:
if (sscanf(line, "%d", &this->nextStateIndex) != 1)
throwError("An index must be specified when using the \'nextState\' keyword");
this->nextStateIndex += offset;
break;
case CON_STATE_DESCRIPTION:
strcpy(this->description, trim(line));
break;
case CON_STATE_TIME:
if (sscanf(line, "%lf", &stateTime)!=1)
throwError("The time that is expected to be spent in this state needs to be provided.");
break;
case CON_STATE_STANCE:
reverseStance = false;
keepStance = false;
if (strncmp(trim(line), "left",4) == 0)
stateStance = LEFT_STANCE;
else
if (strncmp(trim(line), "right", 5) == 0)
stateStance = RIGHT_STANCE;
else
if (strncmp(trim(line), "reverse", 7) == 0)
reverseStance = true;
else if (strncmp(trim(line), "same", 4) == 0)
keepStance = true;
else
throwError("When using the \'stateStance\' keyword, \'left\', \'right\' or \'reverse\' must be specified.");
break;
case CON_TRANSITION_ON:
transitionOnFootContact = false;
if (strncmp(trim(line), "footDown", 8) == 0)
transitionOnFootContact = true;
else
if (strncmp(trim(line), "timeUp", 6) == 0)
//nothn' to do, since this is the default
;
else
throwError("When using the \'transitionOn\' keyword, \'footDown\' or \'timeUp\' must be specified.");
break;
case CON_TRAJECTORY_START:
//create a new trajectory, and read its information from the file
tempTraj = new Trajectory();
strcpy(tempTraj->jName, trim(line));
tempTraj->readTrajectory(f);
this->sTraj.push_back(tempTraj);
break;
case CON_D_TRAJX_START:
if( dTrajX != NULL )
throwError( "Two dTrajX trajectory, this is illegal!" );
dTrajX = new Trajectory1D();
readTrajectory1D( f, *dTrajX, CON_D_TRAJX_END );
break;
case CON_D_TRAJZ_START:
if( dTrajZ != NULL )
throwError( "Two dTrajZ trajectory, this is illegal!" );
dTrajZ = new Trajectory1D();
readTrajectory1D( f, *dTrajZ, CON_D_TRAJZ_END );
break;
case CON_V_TRAJX_START:
if( vTrajX != NULL )
throwError( "Two vTrajX trajectory, this is illegal!" );
vTrajX = new Trajectory1D();
readTrajectory1D( f, *vTrajX, CON_V_TRAJX_END );
break;
case CON_V_TRAJZ_START:
if( vTrajZ != NULL )
throwError( "Two vTrajZ trajectory, this is illegal!" );
vTrajZ = new Trajectory1D();
readTrajectory1D( f, *vTrajZ, CON_V_TRAJZ_END );
break;
case CON_COMMENT:
break;
case CON_NOT_IMPORTANT:
tprintf("Ignoring input line: \'%s\'\n", line);
break;
default:
throwError("Incorrect SIMBICON input file: \'%s\' - unexpected line.", buffer);
}
}
throwError("Incorrect SIMBICON input file: No \'/State\' found", buffer);
}
/**
This method is used to read a trajectory from a file
*/
void TrajectoryComponent::readTrajectoryComponent(FILE* f){
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];
char tmpString[200];
//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);
switch (lineType) {
case CON_TRAJ_COMPONENT_END:
//we're done...
return;
break;
case CON_COMMENT:
break;
case CON_ROTATION_AXIS:
if (sscanf(line, "%lf %lf %lf", &this->rotationAxis.x, &this->rotationAxis.y, &this->rotationAxis.z)!=3)
throwError("The axis for a trajectory is specified by three parameters!");
this->rotationAxis.toUnit();
break;
case CON_FEEDBACK_START:
//read the kind of feedback that is applicable to this state
if (sscanf(line, "%s", tmpString) != 1)
throwError("The kind of feedback to be used for a trajectory must be specified (e.g. linear)");
delete bFeedback;
bFeedback = NULL;
if (strncmp(tmpString, "linear", 6) == 0){
bFeedback = new LinearBalanceFeedback();
bFeedback->loadFromFile(f);
}else
throwError("Unrecognized type of feedback: \'%s\'", line);
break;
case CON_BASE_TRAJECTORY_START:
//read in the base trajectory
SimBiConState::readTrajectory1D(f, baseTraj, CON_BASE_TRAJECTORY_END);
break;
case CON_REVERSE_ANGLE_ON_STANCE:
if (strncmp(trim(line), "left", 4) == 0)
reverseAngleOnLeftStance = true;
else if (strncmp(trim(line), "right", 5) == 0)
reverseAngleOnRightStance = true;
else
throwError("When using the \'startingStance\' keyword, \'left\' or \'right\' must be specified!");
break;
case CON_NOT_IMPORTANT:
tprintf("Ignoring input line: \'%s\'\n", line);
break;
default:
throwError("Incorrect SIMBICON input file: \'%s\' - unexpected line.", buffer);
}
}
throwError("Incorrect SIMBICON input file: No \'/trajectory\' found ", buffer);
}
/**
This method loads all the pertinent information regarding the simbicon controller from a file.
*/
void SimBiController::loadFromFile(char* fName){
if (fName == NULL)
throwError("NULL file name provided.");
FILE *f = fopen(fName, "r");
if (f == NULL)
throwError("Could not open file: %s", fName);
//to be able to load multiple controllers from multiple files,
//we will use this offset to make sure that the state numbers
//mentioned in each input file are updated correctly
int stateOffset = this->states.size();
SimBiConState* tempState;
int tempStateNr = -1;
//have a temporary buffer used to read the file line by line...
char buffer[200];
//this is where it happens.
while (!feof(f)){
//get a line from the file...
fgets(buffer, 200, f);
if (feof(f))
break;
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);
switch (lineType) {
case CON_PD_GAINS_START:
readGains(f);
break;
case CON_STATE_START:
tempState = new SimBiConState();
sscanf(line, "%d", &tempStateNr);
if (tempStateNr != stateOffset + this->states.size())
throwError("Inccorect state offset specified: %d", tempStateNr);
states.push_back(tempState);
tempState->readState(f, stateOffset);
//now we have to resolve all the joint names (i.e. figure out which joints they apply to).
resolveJoints(tempState);
break;
case CON_STANCE_HIP_DAMPING:
sscanf(line, "%lf", &stanceHipDamping);
break;
case CON_STANCE_HIP_MAX_VELOCITY:
sscanf(line, "%lf", &stanceHipMaxVelocity);
break;
case CON_ROOT_PRED_TORQUE_SCALE:
sscanf(line, "%lf", &rootPredictiveTorqueScale);
break;
case CON_CHARACTER_STATE:
character->loadReducedStateFromFile(trim(line));
strcpy(initialBipState, trim(line));
break;
case CON_START_AT_STATE:
if (sscanf(line, "%d", &tempStateNr) != 1)
throwError("A starting state must be specified!");
transitionToState(tempStateNr);
startingState = tempStateNr;
break;
case CON_COMMENT:
break;
case CON_STARTING_STANCE:
if (strncmp(trim(line), "left", 4) == 0){
setStance(LEFT_STANCE);
startingStance = LEFT_STANCE;
}
else if (strncmp(trim(line), "right", 5) == 0){
setStance(RIGHT_STANCE);
startingStance = RIGHT_STANCE;
}
else
throwError("When using the \'reverseTargetOnStance\' keyword, \'left\' or \'right\' must be specified!");
break;
case CON_NOT_IMPORTANT:
tprintf("Ignoring input line: \'%s\'\n", line);
break;
default:
throwError("Incorrect SIMBICON input file: \'%s\' - unexpected line.", buffer);
}
}
}
