void mtsPIDQtWidget::timerEvent(QTimerEvent * CMN_UNUSED(event))
{
// make sure we should update the display
if (this->isHidden()) {
return;
}
// get data from the PID
PID.GetStateJoint(PID.StateJoint);
PID.StateJoint.Position().ElementwiseMultiply(UnitFactor);
PID.StateJoint.Velocity().ElementwiseMultiply(UnitFactor);
PID.StateJoint.Effort().ElementwiseMultiply(UnitFactor);
PID.GetStateJointDesired(PID.StateJointDesired);
PID.StateJointDesired.Position().ElementwiseMultiply(UnitFactor);
PID.StateJointDesired.Effort().ElementwiseMultiply(UnitFactor);
// update GUI
QVRCurrentPositionWidget->SetValue(PID.StateJoint.Position());
QVRCurrentEffortWidget->SetValue(PID.StateJoint.Effort());
// display requested joint positions when we are not trying to set it using GUI
if (!DirectControl) {
QVWDesiredPositionWidget->SetValue(PID.StateJointDesired.Position());
QVWDesiredEffortWidget->SetValue(PID.StateJointDesired.Effort());
}
// plot
CurrentPositionSignal->AppendPoint(vctDouble2(PID.StateJoint.Timestamp(),
PID.StateJoint.Position().Element(PlotIndex)));
DesiredPositionSignal->AppendPoint(vctDouble2(PID.StateJointDesired.Timestamp(),
PID.StateJointDesired.Position().Element(PlotIndex)));
CurrentVelocitySignal->AppendPoint(vctDouble2(PID.StateJoint.Timestamp(),
PID.StateJoint.Velocity().Element(PlotIndex)));
// negate effort to plot the same direction
CurrentEffortSignal->AppendPoint(vctDouble2(PID.StateJoint.Timestamp(),
-PID.StateJoint.Effort().Element(PlotIndex)));
DesiredEffortSignal->AppendPoint(vctDouble2(PID.StateJointDesired.Timestamp(),
-PID.StateJointDesired.Effort().Element(PlotIndex)));
QVPlot->updateGL();
}
void sdpPlayerParseStateTableData::LoadDataFromFile(vctPlot2DBase::Signal * SignalHandle ,double TimeStampForSearch, double VisualRange, bool ResetSignalBuffer) {
double MinimumTime, MaxmumTime;
std::string TempLine;
std::vector <std::string> Token;
std::vector <double> TimeIndex;
size_t LeftBoundaryPosition = 0, RightBoundaryPosition = 0;
double LeftBoundaryTime = 0.0, RightBoundaryTime = 0.0;
size_t ElementsNumber =0, SignalBufferSize = 0;
if (ResetSignalBuffer) {
ElementsNumber = SignalHandle->GetNumberOfPoints();
SignalBufferSize = SignalHandle->GetSize();
SignalHandle->SetSize(SignalBufferSize);
}
// Get Max & Minimum Time
std::ifstream inf(Header.FilePath.c_str());
std::getline(inf, TempLine);
Tokenize(TempLine, Token, Header.Delimiter);
MinimumTime = strtod(Token.at(IndexOfTimeField).c_str() , NULL);
TimeBaseOffset = MinimumTime;
inf.close();
Token.clear();
Tokenize(Index.at(Index.size()-1).LineAtIndex, Token, Header.Delimiter);
MaxmumTime =strtod(Token.at(IndexOfTimeField).c_str() , NULL);
MinimumTime -= TimeBaseOffset;
MaxmumTime -= TimeBaseOffset;
LeftBoundaryTime = (TimeStampForSearch-(1.5*VisualRange));
if(TimeStampForSearch!=0)
LeftBoundaryTime -= TimeBaseOffset;
LeftBoundaryTime = (LeftBoundaryTime<MinimumTime)?MinimumTime:LeftBoundaryTime;
RightBoundaryTime = (TimeStampForSearch+(1.5*VisualRange));
if(TimeStampForSearch!=0)
RightBoundaryTime -= TimeBaseOffset;
RightBoundaryTime = (RightBoundaryTime>MaxmumTime)?MaxmumTime:RightBoundaryTime;
if ( RightBoundaryTime < MinimumTime || LeftBoundaryTime > MaxmumTime || LeftBoundaryTime >= RightBoundaryTime)
return;
// find where is the LeftBoundaryPosition & RightBoundaryPosition
// where is TimeStampForSearch postition in index file?
double min, max;
SignalHandle->ComputeDataRangeX(min,max,true);
if (min<= LeftBoundaryTime && (LeftBoundaryTime >= MinimumTime && !ResetSignalBuffer)) {
LeftBoundaryPosition = GetDataPositionFromFile(max+TimeBaseOffset, IndexOfTimeField);
RightBoundaryPosition = GetDataPositionFromFile(RightBoundaryTime+TimeBaseOffset, IndexOfTimeField);
if (LeftBoundaryPosition >= RightBoundaryPosition)
return;
// load Data from File: [LeftBoundaryPosition, RightBoundaryPosition]
inf.open(Header.FilePath.c_str(), std::ios_base::binary | std::ios_base::in);
inf.seekg(LeftBoundaryPosition, std::ios::beg);
double TimeElement = 0, DataElement;
char *StringBuffer = new char [RightBoundaryPosition - LeftBoundaryPosition];
inf.read(StringBuffer, RightBoundaryPosition - LeftBoundaryPosition);
std::string stringvalues(StringBuffer, RightBoundaryPosition - LeftBoundaryPosition);
std::istringstream iss (stringvalues);
inf.close();
while(1) {
std::string TempLine = "aaaa";
std::vector <std::string> Token;
std::getline(iss, TempLine);
Tokenize(TempLine, Token, Header.Delimiter);
if (Token.size() < IndexOfDataField || Token.size() < IndexOfTimeField || iss.eof())
break;
DataElement = strtod(Token.at(IndexOfDataField).c_str() , NULL);
TimeElement = strtod(Token.at(IndexOfTimeField).c_str() , NULL);
// SignalHandle->GetNumberOfPoints(ElementsNumber, SignalBufferSize);
ElementsNumber = SignalHandle->GetNumberOfPoints();
SignalBufferSize = SignalHandle->GetSize();
if (ElementsNumber == SignalBufferSize && (LeftBoundaryTime <= min)) {
// Buffer overflow, we need to add size
SignalBufferSize *= 1.5;
SignalHandle->Resize(SignalBufferSize);
}
TimeElement -= TimeBaseOffset;
SignalHandle->AppendPoint(vctDouble2(TimeElement,DataElement));
SignalHandle->ComputeDataRangeX(min,max,true);
}
// should we resize to a smaller one?
//SignalHandle->GetNumberOfPoints(ElementsNumber, SignalBufferSize);
ElementsNumber = SignalHandle->GetNumberOfPoints();
SignalBufferSize = SignalHandle->GetSize();
if (SignalBufferSize > ElementsNumber*1.5) {
SignalHandle->Resize(ElementsNumber*1.5);
}
delete StringBuffer;
}
else if (min > RightBoundaryTime || max < LeftBoundaryTime) {
// if every thing is not in our range, reset buffer and reload everything
//SignalHandle->GetNumberOfPoints(ElementsNumber, SignalBufferSize);
ElementsNumber = SignalHandle->GetNumberOfPoints();
SignalBufferSize = SignalHandle->GetSize();
SignalHandle->SetSize(SignalBufferSize);
LeftBoundaryPosition = GetDataPositionFromFile(LeftBoundaryTime+TimeBaseOffset ,IndexOfTimeField);
RightBoundaryPosition = GetDataPositionFromFile(RightBoundaryTime+TimeBaseOffset, IndexOfTimeField);
if (LeftBoundaryPosition >= RightBoundaryPosition)
return;
// load Data from File: [LeftBoundaryPosition, RightBoundaryPosition]
inf.open(Header.FilePath.c_str(), std::ios_base::binary | std::ios_base::in);
inf.seekg(LeftBoundaryPosition, std::ios::beg);
double TimeElement = 0, DataElement;
char *StringBuffer = new char [RightBoundaryPosition - LeftBoundaryPosition];
inf.read(StringBuffer, RightBoundaryPosition - LeftBoundaryPosition);
std::string stringvalues(StringBuffer, RightBoundaryPosition - LeftBoundaryPosition);
std::istringstream iss (stringvalues);
inf.close();
while(1) {
std::string TempLine;
std::vector <std::string> Token;
std::getline(iss, TempLine);
Tokenize(TempLine, Token, Header.Delimiter);
if (Token.size() < IndexOfDataField || Token.size() < IndexOfTimeField || iss.eof())
break;
DataElement = strtod(Token.at(IndexOfDataField).c_str() , NULL);
TimeElement = strtod(Token.at(IndexOfTimeField).c_str() , NULL);
//SignalHandle->GetNumberOfPoints(ElementsNumber, SignalBufferSize);
ElementsNumber = SignalHandle->GetNumberOfPoints();
SignalBufferSize = SignalHandle->GetSize();
if (ElementsNumber == SignalBufferSize && (LeftBoundaryTime <= min)) {
// Buffer overflow, we need to add size
SignalBufferSize *= 1.5;
SignalHandle->Resize(SignalBufferSize);
}
TimeElement -= TimeBaseOffset;
SignalHandle->AppendPoint(vctDouble2(TimeElement,DataElement));
SignalHandle->ComputeDataRangeX(min,max,true);
}
// should we resize to a smaller one?
//SignalHandle->GetNumberOfPoints(ElementsNumber, SignalBufferSize);
ElementsNumber = SignalHandle->GetNumberOfPoints();
SignalBufferSize = SignalHandle->GetSize();
if (SignalBufferSize > ElementsNumber*1.5) {
SignalHandle->Resize(ElementsNumber*1.5);
}
delete StringBuffer;
}
else{
// reload only what we need
//SignalHandle->GetNumberOfPoints(ElementsNumber, SignalBufferSize);
ElementsNumber = SignalHandle->GetNumberOfPoints();
SignalBufferSize = SignalHandle->GetSize();
SignalHandle->Resize(SignalBufferSize);
LeftBoundaryPosition = GetDataPositionFromFile(LeftBoundaryTime+TimeBaseOffset ,IndexOfTimeField);
//*********************************************************************************************
// SECTION ONE, prepend data
//*********************************************************************************************
RightBoundaryPosition = GetDataPositionFromFile(min+TimeBaseOffset ,IndexOfTimeField);
{
inf.open(Header.FilePath.c_str(), std::ios_base::binary | std::ios_base::in);
inf.seekg(LeftBoundaryPosition, std::ios::beg);
double TimeElement = 0, DataElement;
char *StringBuffer = new char [RightBoundaryPosition - LeftBoundaryPosition];
inf.read(StringBuffer, RightBoundaryPosition - LeftBoundaryPosition);
std::string stringvalues(StringBuffer, RightBoundaryPosition - LeftBoundaryPosition);
std::istringstream iss (stringvalues);
inf.close();
std::vector <double> TimeDataBuffer;
TimeDataBuffer.clear();
while(1) {
std::string TempLine;
std::vector <std::string> Token;
std::getline(iss, TempLine);
Tokenize(TempLine, Token, Header.Delimiter);
if (Token.size() < IndexOfDataField || Token.size() < IndexOfTimeField || iss.eof())
break;
DataElement = strtod(Token.at(IndexOfDataField).c_str() , NULL);
TimeElement = strtod(Token.at(IndexOfTimeField).c_str() , NULL);
TimeElement -= TimeBaseOffset;
TimeDataBuffer.push_back(TimeElement);
TimeDataBuffer.push_back(DataElement);
}
// reset buffer size for prepend data
//SignalHandle->GetNumberOfPoints(ElementsNumber, SignalBufferSize);
ElementsNumber = SignalHandle->GetNumberOfPoints();
SignalBufferSize = SignalHandle->GetSize();
if ((SignalBufferSize - ElementsNumber) < TimeDataBuffer.size()) {
SignalHandle->Resize(SignalBufferSize+ TimeDataBuffer.size());
}
if (!TimeDataBuffer.empty())
SignalHandle->PrependArray((double*) &TimeDataBuffer[0], TimeDataBuffer.size());
delete StringBuffer;
}
//*********************************************************************************************
// SECTION 2, append data, we let PLAY to load data ahead
//*********************************************************************************************
}
return;
}
void mtsPIDQtWidget::timerEvent(QTimerEvent * CMN_UNUSED(event))
{
// make sure we should update the display
if (this->isHidden()) {
return;
}
// get data from the PID
PID.GetPeriodStatistics(IntervalStatistics);
PID.GetPositionJoint(PID.PositionJointGetParam);
PID.PositionJointGetParam.Position().ElementwiseMultiply(UnitFactor);
PID.GetPositionJointDesired(PID.PositionJointGetDesired);
PID.PositionJointGetDesired.ElementwiseMultiply(UnitFactor);
PID.GetVelocityJoint(PID.VelocityJointGetParam);
PID.VelocityJointGetParam.Velocity().ElementwiseMultiply(UnitFactor);
PID.GetEffortJointDesired(PID.EffortJoint);
// update GUI
QMIntervalStatistics->SetValue(IntervalStatistics);
QVRCurrentPositionWidget->SetValue(PID.PositionJointGetParam.Position());
QVRCurrentEffortWidget->SetValue(PID.EffortJoint);
// display requested joint positions when we are not trying to set it using GUI
if (!DirectControl) {
QVWDesiredPositionWidget->SetValue(PID.PositionJointGetDesired);
}
// plot
CurrentPositionSignal->AppendPoint(vctDouble2(PID.PositionJointGetParam.Timestamp(),
PID.PositionJointGetParam.Position().Element(PlotIndex)));
DesiredPositionSignal->AppendPoint(vctDouble2(PID.PositionJointGetParam.Timestamp(),
PID.PositionJointGetDesired.Element(PlotIndex)));
CurrentVelocitySignal->AppendPoint(vctDouble2(PID.VelocityJointGetParam.Timestamp(),
PID.VelocityJointGetParam.Velocity().Element(PlotIndex)));
DesiredEffortSignal->AppendPoint(vctDouble2(PID.PositionJointGetParam.Timestamp(),
-PID.EffortJoint.Element(PlotIndex))); // negate current to plot the same direction
// UBC log file for PID joint efforts
if(effortLogFile.is_open())
{
struct timespec spec;
clock_gettime(CLOCK_REALTIME, &spec);
time_t s = spec.tv_sec;
long ms = round(spec.tv_nsec / 1.0e6); // Convert nanoseconds to milliseconds
if(ms >= 1000) // this is the grossest conversion ever!!
{
s = s+1;
ms = ms-1000;
}
// pretty gross hack, but didn't want to get into converting long to string then padding it
std::string zeros = "";
if(ms < 10) {
zeros = "00";
} else if(ms < 100) {
zeros = "0";
}
effortLogFile << "PID effort: t = " << (intmax_t)s << "." << zeros << ms << std::endl;
effortLogFile << "effort: " << PID.EffortJoint.Element(0) << " ";
effortLogFile << PID.EffortJoint.Element(1) << " ";
effortLogFile << PID.EffortJoint.Element(2) << " ";
effortLogFile << PID.EffortJoint.Element(3) << " ";
effortLogFile << PID.EffortJoint.Element(4) << " ";
effortLogFile << PID.EffortJoint.Element(5) << " ";
effortLogFile << PID.EffortJoint.Element(6) << std::endl;
/*
effortLogFile << "Joint Pos (deg): " << std::endl;
effortLogFile << "t: " << PID.PositionJointGetParam.Timestamp() << std::endl;
effortLogFile << PID.PositionJointGetParam.Position() << std::endl;
*/
}
QVPlot->updateGL();
}
