void sdpPlayerExample::Run(void)
{
ProcessQueuedEvents();
ProcessQueuedCommands();
//update the model (load data) etc.
if (State == PLAY) {
double currentTime = TimeServer.GetAbsoluteTimeInSeconds();
Time = currentTime - PlayStartTime.Timestamp() + PlayStartTime.Data;
if (Time > PlayUntilTime) {
Time = PlayUntilTime;
State = STOP;
}
else {
//Load and Prep current data
}
}
//make sure we are at the correct seek position.
else if (State == SEEK) {
//Load and Prep current data
}
else if (State == STOP) {
//do Nothing
}
//now display updated data in the qt thread space.
if (Widget.isVisible()) {
emit QSignalUpdateQT();
}
}
void mtsTeleOperationECM::Run(void)
{
ProcessQueuedCommands();
ProcessQueuedEvents();
// run based on state
mTeleopState.Run();
}
void ireTask::Run(void)
{
ProcessQueuedCommands();
ProcessQueuedEvents();
// Need following call to give the IRE thread some time to execute.
ireFramework::JoinIREShell(0.1);
if (ireFramework::IsFinished())
Kill();
}
void clientTask::Run(void)
{
ProcessQueuedEvents();
value_type parameter;
// read and write back to measure the loop server->client->server
this->ReadServer(parameter);
this->WriteServer(parameter);
// create an event to measure the loop client->server->client
double time;
time = this->TimeServer->GetRelativeTime();
parameter.SetTimestamp(time);
this->TriggerEvent(parameter);
}
void mtsROSBridge::Run(void)
{
ProcessQueuedCommands();
ProcessQueuedEvents();
const PublishersType::iterator end = Publishers.end();
PublishersType::iterator iter;
for (iter = Publishers.begin();
iter != end;
++iter) {
(*iter)->Execute();
}
if (mSpin) ros::spinOnce();
}
void mtsComponentViewer::Run(void)
{
if (!UDrawPipeConnected) {
ConnectToUDrawGraph();
if (UDrawPipeConnected) {
CMN_LOG_CLASS_INIT_VERBOSE << "Run: Sending all info" << std::endl;
// Should flush all events before calling SendAllInfo
SendAllInfo();
}
}
ProcessQueuedCommands();
ProcessQueuedEvents();
if (UDrawPipeConnected)
ProcessResponse();
}
void mtsIntuitiveResearchKitArm::Run(void)
{
ProcessQueuedEvents();
GetRobotData();
switch (RobotState) {
case mtsIntuitiveResearchKitArmTypes::DVRK_UNINITIALIZED:
break;
case mtsIntuitiveResearchKitArmTypes::DVRK_HOMING_POWERING:
RunHomingPower();
break;
case mtsIntuitiveResearchKitArmTypes::DVRK_HOMING_CALIBRATING_ARM:
RunHomingCalibrateArm();
break;
case mtsIntuitiveResearchKitArmTypes::DVRK_ARM_CALIBRATED:
break;
case mtsIntuitiveResearchKitArmTypes::DVRK_READY:
break;
case mtsIntuitiveResearchKitArmTypes::DVRK_POSITION_JOINT:
RunPositionJoint();
break;
case mtsIntuitiveResearchKitArmTypes::DVRK_POSITION_GOAL_JOINT:
RunPositionGoalJoint();
break;
case mtsIntuitiveResearchKitArmTypes::DVRK_POSITION_CARTESIAN:
RunPositionCartesian();
break;
case mtsIntuitiveResearchKitArmTypes::DVRK_POSITION_GOAL_CARTESIAN:
RunPositionGoalCartesian();
break;
case mtsIntuitiveResearchKitArmTypes::DVRK_MANUAL:
break;
default:
RunArmSpecific();
break;
}
RunEvent();
ProcessQueuedCommands();
mCounter++;
CartesianGetPrevious = CartesianGet;
}
void mtsIntuitiveResearchKitPSM::Run(void)
{
Counter++;
ProcessQueuedEvents();
GetRobotData();
switch (RobotState) {
case PSM_UNINITIALIZED:
break;
case PSM_HOMING_POWERING:
RunHomingPower();
break;
case PSM_HOMING_CALIBRATING_ARM:
RunHomingCalibrateArm();
break;
case PSM_ARM_CALIBRATED:
break;
case PSM_ENGAGING_ADAPTER:
RunEngagingAdapter();
break;
case PSM_ADAPTER_ENGAGED:
// choose next state
break;
case PSM_ENGAGING_TOOL:
RunEngagingTool();
break;
case PSM_READY:
case PSM_POSITION_CARTESIAN:
RunPositionCartesian();
break;
case PSM_MANUAL:
break;
default:
break;
}
RunEvent();
ProcessQueuedCommands();
}
void mtsIntuitiveResearchKitMTM::Run(void)
{
ProcessQueuedEvents();
GetRobotData();
switch (RobotState) {
case mtsIntuitiveResearchKitMTMTypes::MTM_UNINITIALIZED:
break;
case mtsIntuitiveResearchKitMTMTypes::MTM_HOMING_POWERING:
RunHomingPower();
break;
case mtsIntuitiveResearchKitMTMTypes::MTM_HOMING_CALIBRATING_ARM:
RunHomingCalibrateArm();
break;
case mtsIntuitiveResearchKitMTMTypes::MTM_HOMING_CALIBRATING_ROLL:
RunHomingCalibrateRoll();
break;
case mtsIntuitiveResearchKitMTMTypes::MTM_READY:
break;
case mtsIntuitiveResearchKitMTMTypes::MTM_POSITION_CARTESIAN:
RunPositionCartesian();
break;
case mtsIntuitiveResearchKitMTMTypes::MTM_GRAVITY_COMPENSATION:
RunGravityCompensation();
break;
case mtsIntuitiveResearchKitMTMTypes::MTM_CLUTCH:
RunClutch();
default:
break;
}
RunEvent();
ProcessQueuedCommands();
// update previous value
CartesianGetPrevious = CartesianGet;
}
void mtsComponentAddLatency::Run(void)
{
ProcessQueuedEvents();
ProcessQueuedCommands();
mtsExecutionResult result;
DelayedReadList::iterator readIterator = DelayedReads.begin();
const DelayedReadList::iterator readEnd = DelayedReads.end();
this->LatencyStateTable.Start();
for (;
readIterator != readEnd;
++readIterator) {
result = (*readIterator)->Execute();
}
this->LatencyStateTable.Advance();
// see if we have some old void/write commands
const double time = mtsComponentManager::GetInstance()->GetTimeServer().GetRelativeTime() - this->Latency;
DelayedVoidList::iterator voidIterator = DelayedVoids.begin();
const DelayedVoidList::iterator voidEnd = DelayedVoids.end();
this->LatencyStateTable.Start();
for (;
voidIterator != voidEnd;
++voidIterator) {
(*voidIterator)->ProcessQueuedCommands(time);
}
DelayedWriteList::iterator writeIterator = DelayedWrites.begin();
const DelayedWriteList::iterator writeEnd = DelayedWrites.end();
this->LatencyStateTable.Start();
for (;
writeIterator != writeEnd;
++writeIterator) {
(*writeIterator)->ProcessQueuedCommands(time);
}
}
void mtsIntuitiveResearchKitConsole::Run(void)
{
ProcessQueuedCommands();
ProcessQueuedEvents();
}
void mtsRobotIO1394QtWidget::timerEvent(QTimerEvent * CMN_UNUSED(event))
{
ProcessQueuedEvents();
// make sure we should update the display
if (!this->isHidden()) {
bool flag;
Robot.IsValid(flag);
if (flag) {
Robot.GetPeriodStatistics(IntervalStatistics);
Robot.GetPowerStatus(PowerStatus);
Robot.GetSafetyRelay(SafetyRelay);
if (NumberOfActuators != 0) {
Actuators.GetAmpEnable(ActuatorAmpEnable);
Actuators.GetAmpStatus(ActuatorAmpStatus);
Robot.GetPosition(JointPosition); // vct
JointPosition.ElementwiseMultiply(UnitFactor); // to degrees or mm
Actuators.GetPositionActuator(ActuatorPositionGet); // prm
ActuatorPosition.Assign(ActuatorPositionGet.Position()); // vct
ActuatorPosition.ElementwiseMultiply(UnitFactor); // to degrees or mm
Robot.GetVelocity(ActuatorVelocity);
ActuatorVelocity.ElementwiseMultiply(UnitFactor); // to degrees or mm
Robot.GetAnalogInputVolts(PotentiometersVolts);
Robot.GetAnalogInputPosSI(PotentiometersPosition);
PotentiometersPosition.ElementwiseMultiply(UnitFactor); // to degrees or mm
Robot.GetActuatorFeedbackCurrent(ActuatorFeedbackCurrent);
ActuatorFeedbackCurrent.Multiply(1000.0); // to mA
Robot.GetActuatorAmpTemperature(ActuatorAmpTemperature);
}
if (NumberOfBrakes != 0) {
Robot.GetBrakeAmpEnable(BrakeAmpEnable);
Robot.GetBrakeAmpStatus(BrakeAmpStatus);
Robot.GetBrakeRequestedCurrent(BrakeRequestedCurrent);
BrakeRequestedCurrent.Multiply(1000.0); // to mA
Robot.GetBrakeFeedbackCurrent(BrakeFeedbackCurrent);
BrakeFeedbackCurrent.Multiply(1000.0); // to mA
Robot.GetBrakeAmpTemperature(BrakeAmpTemperature);
}
} else {
JointPosition.SetAll(DummyValueWhenNotConnected);
ActuatorPosition.SetAll(DummyValueWhenNotConnected);
ActuatorVelocity.SetAll(DummyValueWhenNotConnected);
PotentiometersVolts.SetAll(DummyValueWhenNotConnected);
PotentiometersPosition.SetAll(DummyValueWhenNotConnected);
ActuatorFeedbackCurrent.SetAll(DummyValueWhenNotConnected);
ActuatorAmpTemperature.SetAll(DummyValueWhenNotConnected);
}
DummyValueWhenNotConnected += 0.1;
// display requested current when we are not trying to set it using GUI
if (flag && !DirectControl) {
Robot.GetActuatorRequestedCurrent(ActuatorRequestedCurrent);
ActuatorRequestedCurrent.Multiply(1000.0); // got A, need mA for display
QVWActuatorCurrentSpinBoxWidget->SetValue(ActuatorRequestedCurrent);
QVWActuatorCurrentSliderWidget->SetValue(ActuatorRequestedCurrent);
}
QMIntervalStatistics->SetValue(IntervalStatistics);
if (NumberOfActuators != 0) {
QVRJointPositionWidget->SetValue(JointPosition);
QVRActuatorPositionWidget->SetValue(ActuatorPosition);
QVRActuatorVelocityWidget->SetValue(ActuatorVelocity);
QVRPotVoltsWidget->SetValue(PotentiometersVolts);
QVRPotPositionWidget->SetValue(PotentiometersPosition);
QVRActuatorCurrentFeedbackWidget->SetValue(ActuatorFeedbackCurrent);
QVRActuatorAmpTemperature->SetValue(ActuatorAmpTemperature);
}
if (NumberOfBrakes != 0) {
QVRBrakeCurrentCommandWidget->SetValue(BrakeRequestedCurrent);
QVRBrakeCurrentFeedbackWidget->SetValue(BrakeFeedbackCurrent);
QVRBrakeAmpTemperature->SetValue(BrakeAmpTemperature);
}
UpdateRobotInfo();
}
// ZC: FIX turn off if pid loop exists
Robot.SetWatchdogPeriod(WatchdogPeriodInSeconds);
}
void mtsTeleOperation::Run(void)
{
ProcessQueuedCommands();
ProcessQueuedEvents();
// increment counter
Counter++;
// get master Cartesian position
mtsExecutionResult executionResult;
executionResult = Master.GetPositionCartesian(Master.PositionCartesianCurrent);
if (!executionResult.IsOK()) {
CMN_LOG_CLASS_RUN_ERROR << "Run: call to Master.GetPositionCartesian failed \""
<< executionResult << "\"" << std::endl;
MessageEvents.Error(this->GetName() + ": unable to get cartesian position from master");
this->Enable(false);
}
vctFrm4x4 masterPosition(Master.PositionCartesianCurrent.Position());
// get slave Cartesian position
executionResult = Slave.GetPositionCartesian(Slave.PositionCartesianCurrent);
if (!executionResult.IsOK()) {
CMN_LOG_CLASS_RUN_ERROR << "Run: call to Slave.GetPositionCartesian failed \""
<< executionResult << "\"" << std::endl;
MessageEvents.Error(this->GetName() + ": unable to get cartesian position from slave");
this->Enable(false);
}
/*!
mtsTeleOperation can run in 4 control modes, which is controlled by
footpedal Clutch & OperatorPresent.
Mode 1: OperatorPresent = False, Clutch = False
MTM and PSM stop at their current position. If PSM ManipClutch is
pressed, then the user can manually move PSM.
NOTE: MTM always tries to allign its orientation with PSM's orientation
Mode 2/3: OperatorPresent = False/True, Clutch = True
MTM can move freely in workspace, however its orientation is locked
PSM can not move
Mode 4: OperatorPresent = True, Clutch = False
PSM follows MTM motion
*/
if (IsEnabled
&& Master.PositionCartesianCurrent.Valid()
&& Slave.PositionCartesianCurrent.Valid()) {
// follow mode
if (!IsClutched && IsOperatorPresent) {
// compute master Cartesian motion
vctFrm4x4 masterCartesianMotion;
masterCartesianMotion = Master.CartesianPrevious.Inverse() * masterPosition;
// translation
vct3 masterTranslation;
vct3 slaveTranslation;
if (this->TranslationLocked) {
slaveTranslation = Slave.CartesianPrevious.Translation();
} else {
masterTranslation = (masterPosition.Translation() - Master.CartesianPrevious.Translation());
slaveTranslation = masterTranslation * this->Scale;
slaveTranslation = RegistrationRotation * slaveTranslation + Slave.CartesianPrevious.Translation();
}
// rotation
vctMatRot3 slaveRotation;
if (this->RotationLocked) {
slaveRotation.From(Slave.CartesianPrevious.Rotation());
} else {
slaveRotation = RegistrationRotation * masterPosition.Rotation();
}
// compute desired slave position
vctFrm4x4 slaveCartesianDesired;
slaveCartesianDesired.Translation().Assign(slaveTranslation);
slaveCartesianDesired.Rotation().FromNormalized(slaveRotation);
Slave.PositionCartesianDesired.Goal().FromNormalized(slaveCartesianDesired);
// Slave go this cartesian position
Slave.SetPositionCartesian(Slave.PositionCartesianDesired);
// Gripper
if (Master.GetGripperPosition.IsValid()) {
double gripperPosition;
Master.GetGripperPosition(gripperPosition);
Slave.SetJawPosition(gripperPosition);
} else {
Slave.SetJawPosition(5.0 * cmnPI_180);
}
} else if (!IsClutched && !IsOperatorPresent) {
// Do nothing
}
} else {
CMN_LOG_CLASS_RUN_DEBUG << "mtsTeleOperation disabled" << std::endl;
}
}
void guiTask::Run(void){
ProcessQueuedEvents();
ProcessQueuedCommands();
if(clarityClientUI.beepClicked){
clarityClientUI.beepClicked = false;
mtsInt numberOfBeeps = (int)clarityClientUI.beepCount->value();
NDI.Beep(numberOfBeeps);
}
if(clarityClientUI.initializeClicked){
clarityClientUI.initializeClicked = false;
NDI.Initialize();
NDI.Query();
NDI.Enable();
}
if(clarityClientUI.trackClicked){
clarityClientUI.trackClicked = false;
NDI.Track(trackToggle);
trackToggle = !trackToggle;
}
RobotBase.GetPosition(robotBasePosition);
RobotBaseQuaternion.W() = robotBasePosition.Element(3);
RobotBaseQuaternion.X() = robotBasePosition.Element(4);
RobotBaseQuaternion.Y() = robotBasePosition.Element(5);
RobotBaseQuaternion.Z() = robotBasePosition.Element(6);
RobotBaseFrame.Rotation().FromRaw(RobotBaseQuaternion);
r32 = RobotBaseFrame.Rotation().Element(2,1);
r33 = RobotBaseFrame.Rotation().Element(2,2);
r31 = RobotBaseFrame.Rotation().Element(2,0);
r21 = RobotBaseFrame.Rotation().Element(1,0);
r11 = RobotBaseFrame.Rotation().Element(0,0);
RobotBaseEulerAngles.Element(0) = atan2(r32,r33)*cmn180_PI;
RobotBaseEulerAngles.Element(1) = atan2(-r31,sqrt(pow(r32,2.0) + pow(r33,2.0)))*cmn180_PI;
RobotBaseEulerAngles.Element(2) = atan2(r21,r11)*cmn180_PI;
RobotTip.GetPosition(robotTipPosition);
RobotTipQuaternion.W() = robotTipPosition.Element(3);
RobotTipQuaternion.X() = robotTipPosition.Element(4);
RobotTipQuaternion.Y() = robotTipPosition.Element(5);
RobotTipQuaternion.Z() = robotTipPosition.Element(6);
RobotTipFrame.Rotation().FromRaw(RobotTipQuaternion);
r32 = RobotTipFrame.Rotation().Element(2,1);
r33 = RobotTipFrame.Rotation().Element(2,2);
r31 = RobotTipFrame.Rotation().Element(2,0);
r21 = RobotTipFrame.Rotation().Element(1,0);
r11 = RobotTipFrame.Rotation().Element(0,0);
RobotTipEulerAngles.Element(0) = atan2(r32,r33)*cmn180_PI;
RobotTipEulerAngles.Element(1) = atan2(-r31,sqrt(pow(r32,2.0) + pow(r33,2.0)))*cmn180_PI;
RobotTipEulerAngles.Element(2) = atan2(r21,r11)*cmn180_PI;
for (unsigned int i=0; i<6; i++){
if(robotBasePosition.Element(0) != 999.9){
if(i<3)
clarityClientUI.BasePosition[i]->value(robotBasePosition.Element(i));
else
clarityClientUI.BasePosition[i]->value(RobotBaseEulerAngles.Element(i-3));
}
else
clarityClientUI.BasePosition[i]->value(99999.999);
if(robotTipPosition.Element(0) != 999.9){
if(i<3)
clarityClientUI.TipPosition[i]->value(robotTipPosition.Element(i));
else
clarityClientUI.TipPosition[i]->value(RobotTipEulerAngles.Element(i-3));
}
else
clarityClientUI.TipPosition[i]->value(99999.999);
}
if (Fl::check() == 0)
Kill();
}
