void DoubleStanceFeedback::writeToFile(FILE* f){
if (f == NULL)
return;
fprintf( f, "\t\t\t%s COM_SupportCenterFeedback\n", getConLineString(CON_FEEDBACK_START) );
fprintf(f, "Not yet implemented...\n");
fprintf( f, "\t\t\t%s\n", getConLineString(CON_FEEDBACK_END) );
}
/**
This method is used to read the knots of a base trajectory from the file, where they are specified one (knot) on a line
*/
void TrajectoryComponent::writeBaseTrajectory(FILE* f){
if (f == NULL)
return;
fprintf( f, "\t\t\t%s\n", getConLineString(CON_BASE_TRAJECTORY_START) );
for( int i=0; i < baseTraj.getKnotCount(); ++i ) {
fprintf( f, "\t\t\t\t%lf %lf\n", baseTraj.getKnotPosition(i), baseTraj.getKnotValue(i) );
}
fprintf( f, "\t\t\t%s\n", getConLineString(CON_BASE_TRAJECTORY_END) );
}
/**
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 write the knots of a strength trajectory to the file, where they are specified one (knot) on a line
*/
void Trajectory::writeStrengthTrajectory(FILE* f){
if (f == NULL || strengthTraj == NULL)
return;
fprintf( f, "\t\t\t%s\n", getConLineString(CON_STRENGTH_TRAJECTORY_START) );
for( int i=0; i < strengthTraj->getKnotCount(); ++i ) {
fprintf( f, "\t\t\t\t%lf %lf\n", strengthTraj->getKnotPosition(i), strengthTraj->getKnotValue(i) );
}
fprintf( f, "\t\t\t%s\n", getConLineString(CON_STRENGTH_TRAJECTORY_END) );
}
/**
This method is used to write the state parameters to a file
*/
void SimBiConState::writeState(FILE* f, int index){
if (f == NULL)
return;
fprintf( f, "%s %d\n", getConLineString(CON_STATE_START), index );
fprintf( f, "\t%s %s\n", getConLineString(CON_STATE_DESCRIPTION), description );
fprintf( f, "\t%s %d\n", getConLineString(CON_NEXT_STATE), nextStateIndex );
fprintf( f, "\t%s %s\n", getConLineString(CON_TRANSITION_ON),
transitionOnFootContact?"footDown":"timeUp" );
if( reverseStance )
fprintf( f, "\t%s reverse\n", getConLineString(CON_STATE_STANCE) );
else if( keepStance )
fprintf( f, "\t%s same\n", getConLineString(CON_STATE_STANCE) );
else if( stateStance == LEFT_STANCE )
fprintf( f, "\t%s left\n", getConLineString(CON_STATE_STANCE) );
else if( stateStance == RIGHT_STANCE )
fprintf( f, "\t%s right\n", getConLineString(CON_STATE_STANCE) );
fprintf( f, "\t%s %lf\n", getConLineString(CON_STATE_TIME), stateTime );
fprintf( f, "\n" );
if( dTrajX != NULL )
writeTrajectory1D( f, *dTrajX, CON_D_TRAJX_START, CON_D_TRAJX_END );
if( dTrajZ != NULL )
writeTrajectory1D( f, *dTrajZ, CON_D_TRAJZ_START, CON_D_TRAJZ_END );
if( vTrajX != NULL )
writeTrajectory1D( f, *vTrajX, CON_V_TRAJX_START, CON_V_TRAJX_END );
if( vTrajZ != NULL )
writeTrajectory1D( f, *vTrajZ, CON_V_TRAJZ_START, CON_V_TRAJZ_END );
fprintf( f, "\n" );
for( uint i=0; i<sTraj.size(); ++i ) {
fprintf( f, "\n" );
sTraj[i]->writeTrajectory( f );
}
fprintf( f, "%s\n", getConLineString(CON_STATE_END) );
}
/**
This method is used to write a trajectory to a file
*/
void Trajectory::writeTrajectory(FILE* f){
if (f == NULL)
return;
fprintf( f, "\t%s %s\n", getConLineString(CON_TRAJECTORY_START), jName );
if (relToCharFrame)
fprintf(f, "\t%s\n", getConLineString(CON_CHAR_FRAME_RELATIVE));
if( strengthTraj != NULL )
writeStrengthTrajectory( f );
for( uint i=0; i < components.size(); ++i ) {
fprintf( f, "\n" );
components[i]->writeTrajectoryComponent( f );
}
fprintf( f, "\t%s\n", getConLineString(CON_TRAJECTORY_END) );
}
/**
This method is used to write a trajectory to a file
*/
void TrajectoryComponent::writeTrajectoryComponent(FILE* f){
if (f == NULL)
return;
fprintf( f, "\t\t%s\n", getConLineString(CON_TRAJ_COMPONENT) );
fprintf( f, "\t\t\t%s %lf %lf %lf\n", getConLineString(CON_ROTATION_AXIS),
rotationAxis.x, rotationAxis.y, rotationAxis.z );
if( reverseAngleOnLeftStance )
fprintf( f, "\t\t\t%s left\n", getConLineString(CON_REVERSE_ANGLE_ON_STANCE) );
else if( reverseAngleOnRightStance )
fprintf( f, "\t\t\t%s right\n", getConLineString(CON_REVERSE_ANGLE_ON_STANCE) );
if( bFeedback )
bFeedback->writeToFile( f );
writeBaseTrajectory(f);
fprintf( f, "\t\t%s\n", getConLineString(CON_TRAJ_COMPONENT_END) );
}
/**
This method is used to write the details of the current controller to a file
*/
void SimBiController::writeToFile(char* fileName, char* stateFileName){
FILE* f;
if (fileName == NULL || (f = fopen(fileName, "w")) == NULL)
return;
fprintf( f, "%s\n", getConLineString(CON_PD_GAINS_START) );
fprintf( f, "# joint name Kp Kd MaxTorque ScaleX ScaleY ScaleZ\n" );
fprintf( f, " root\t%lf\t%lf\t%lf\t%lf\t%lf\t%lf\n",
rootControlParams.kp,
rootControlParams.kd,
rootControlParams.maxAbsTorque,
rootControlParams.scale.x,
rootControlParams.scale.y,
rootControlParams.scale.z );
writeGains(f);
fprintf( f, "%s\n", getConLineString(CON_PD_GAINS_END) );
fprintf( f, "\n" );
if( stanceHipDamping > 0 ) {
fprintf( f, "%s %lf\n", getConLineString(CON_STANCE_HIP_DAMPING), stanceHipDamping );
fprintf( f, "%s %lf\n", getConLineString(CON_STANCE_HIP_MAX_VELOCITY), stanceHipMaxVelocity );
}
fprintf( f, "\n" );
for( uint i=0; i<states.size(); ++i ) {
fprintf( f, "\n\n" );
states[i]->writeState( f, i );
}
fprintf( f, "\n\n" );
fprintf( f, "%s %d\n", getConLineString(CON_START_AT_STATE), startingState );
fprintf( f, "%s %s\n", getConLineString(CON_STARTING_STANCE),
(stanceFoot == lFoot)?"left":"right" );
if( stateFileName == NULL )
fprintf( f, "%s %s\n", getConLineString(CON_CHARACTER_STATE), initialBipState );
else
fprintf( f, "%s %s\n", getConLineString(CON_CHARACTER_STATE), stateFileName );
fclose(f);
}
/**
This method is used to write the state parameters to a file
*/
void LinearBalanceFeedback::writeToFile(FILE* f){
if (f == NULL)
return;
fprintf( f, "\t\t\t%s linear\n", getConLineString(CON_FEEDBACK_START) );
fprintf( f, "\t\t\t\t%s %lf %lf %lf\n", getConLineString(CON_FEEDBACK_PROJECTION_AXIS),
feedbackProjectionAxis.x,
feedbackProjectionAxis.y,
feedbackProjectionAxis.z );
fprintf( f, "\t\t\t\t%s %lf\n", getConLineString(CON_CD), cd );
fprintf( f, "\t\t\t\t%s %lf\n", getConLineString(CON_CV), cv );
if (dMin > -1000) fprintf( f, "\t\t\t\t%s %lf\n", getConLineString(CON_D_MIN), dMin);
if (dMax < 1000) fprintf( f, "\t\t\t\t%s %lf\n", getConLineString(CON_D_MAX), dMax);
if (vMin > -1000) fprintf( f, "\t\t\t\t%s %lf\n", getConLineString(CON_V_MIN), vMin);
if (vMax < 1000) fprintf( f, "\t\t\t\t%s %lf\n", getConLineString(CON_V_MAX), vMax);
fprintf( f, "\t\t\t%s\n", getConLineString(CON_FEEDBACK_END) );
}
