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