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