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) ); }