Exemplo n.º 1
0
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();
}
Exemplo n.º 3
0
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();
}
Exemplo n.º 4
0
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);
}
Exemplo n.º 5
0
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();
}
Exemplo n.º 6
0
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;
}
Exemplo n.º 10
0
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();
}
Exemplo n.º 12
0
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);
}
Exemplo n.º 13
0
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;
    }
}
Exemplo n.º 14
0
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();
}