Robot( const vctDynamicVector<double>& q ) :
mtsTaskPeriodic( "Robot", 0.01, true ){
qout.Position() = q;
mtsInterfaceProvided* input = AddInterfaceProvided( "Input" );
if( input ){
StateTable.AddData( tin, "TorquesInput" );
input->AddCommandWriteState( StateTable, tin, "SetTorqueJoint" );
}
else{
CMN_LOG_RUN_ERROR << "Failed to create interface Input for " << GetName()
<< std::endl;
}
mtsInterfaceProvided* output = AddInterfaceProvided( "Output" );
if( output ){
StateTable.AddData( qout, "PositionsOutput" );
output->AddCommandReadState( StateTable, qout, "GetPositionJoint" );
}
else{
CMN_LOG_RUN_ERROR << "Failed to create interface Output for " << GetName()
<< std::endl;
}
}
C2ServerTask::C2ServerTask(const std::string & taskName, double period):
// base constructor, same task name and period. Set the length of
// state table to 5000
mtsTaskPeriodic(taskName, period, false, 5000)
{
// add ServerData to the StateTable defined in mtsTask
this->StateTable.AddData(ReadValue1, "ReadValue1");
this->StateTable.AddData(ReadValue2, "ReadValue2");
// add one interface, this will create an mtsInterfaceProvided
mtsInterfaceProvided * provided = AddInterfaceProvided("p1");
if (provided) {
provided->AddCommandVoid(&C2ServerTask::Void1, this, "Void");
provided->AddCommandWrite(&C2ServerTask::Write1, this, "Write");
provided->AddCommandReadState(this->StateTable, this->ReadValue1, "Read");
provided->AddCommandQualifiedRead(&C2ServerTask::QualifiedRead1, this, "QualifiedRead");
provided->AddEventVoid(this->EventVoid1, "EventVoid");
provided->AddEventWrite(this->EventWrite1, "EventWrite", mtsDouble(3.14));
}
provided = AddInterfaceProvided("p2");
if (provided) {
provided->AddCommandVoid(&C2ServerTask::Void2, this, "Void");
provided->AddCommandWrite(&C2ServerTask::Write2, this, "Write");
provided->AddCommandReadState(this->StateTable, this->ReadValue2, "Read");
provided->AddCommandQualifiedRead(&C2ServerTask::QualifiedRead2, this, "QualifiedRead");
provided->AddEventVoid(this->EventVoid2, "EventVoid");
provided->AddEventWrite(this->EventWrite2, "EventWrite", mtsDouble(3.14));
}
}
// main constructor
mtsOSGManipulatorTask::mtsOSGManipulatorTask( const std::string& name,
double period,
osaOSGManipulator* manipulator,
osaCPUMask cpumask,
int priority,
InputType inputtype ) :
mtsTaskPeriodic( name, period, true ),
manipulator( manipulator ),
inputtype( inputtype ),
inputp( NULL ),
inputr( NULL ),
output( NULL ),
ctl( NULL ),
cpumask( cpumask ),
priority( priority ){
if( inputtype == mtsOSGManipulatorTask::PROVIDE_INPUT ){
inputp = AddInterfaceProvided( "Input" );
if( inputp ){
StateTable.AddData( prmqin, "PositionJointInput" );
inputp->AddCommandWriteState( StateTable, prmqin, "SetPositionJoint" );
}
else{
CMN_LOG_RUN_ERROR << "Failed to create interface Input for " << GetName()
<< std::endl;
}
}
else{
inputr = AddInterfaceRequired( "Input" );
if( inputr )
{ inputr->AddFunction( "GetPositionJoint", GetPositionJoint ); }
else{
CMN_LOG_RUN_ERROR << "Failed to create interface Input for " << GetName()
<< std::endl;
}
}
output = AddInterfaceProvided( "Output" );
if( output ){
StateTable.AddData( prmqout, "PositionJointOutput" );
StateTable.AddData( prmRtout, "PositionCartesianOutput" );
output->AddCommandReadState( StateTable, prmRtout, "GetPositionCartesian" );
output->AddCommandReadState( StateTable, prmqout, "GetPositionJoint" );
}
else{
CMN_LOG_RUN_ERROR << "Failed to create interface Output for " << GetName()
<< std::endl;
}
}
void mtsMedtronicStealthlink::Init(void)
{
DataMapContainerInsertReturnValue dataMapInsertReturn;
mtsInterfaceProvided * provided = 0;
provided = AddInterfaceProvided("Controller");
if (provided){
//provided->AddCommandRead(&mtsMedtronicStealthlink::GetTool, this, "GetTool");
//provided->AddCommandRead(&mtsMedtronicStealthlink::GetFrame, this, "GetFrame");
}
dataMapInsertReturn = myDataMap.insert(DataMapContainerItem(new MNavStealthLink::SurgicalPlan(),new mtsMedtronicStealthlink::SurgicalPlan()));
if (dataMapInsertReturn.second){
provided = AddInterfaceProvided("SurgicalPlan");
dataMapInsertReturn.first->second->ConfigureInterfaceProvided(provided);
}else{
CMN_LOG_CLASS_INIT_ERROR << "Init: Failed to add SurgicalPlan data to the DataMap " << std::endl;
}
dataMapInsertReturn = myDataMap.insert(DataMapContainerItem(new MNavStealthLink::Registration(),new mtsMedtronicStealthlink::Registration()));
// Add interface for registration, ideally we should standardize such interface commands/payloads
// maybe we should create a separate state table for registration? Would only advance if changed.
if (dataMapInsertReturn.second){
provided = AddInterfaceProvided("Registration");
if(provided){
provided->AddCommandRead(&mtsMedtronicStealthlink::GetRegistrationValid, this, "GetValid");
}
dataMapInsertReturn.first->second->ConfigureInterfaceProvided(provided);
}else{
CMN_LOG_CLASS_INIT_ERROR << "Init: Failed to add Registration data to the DataMap " << std::endl;
}
dataMapInsertReturn = myDataMap.insert(DataMapContainerItem(new MNavStealthLink::Exam(),new mtsMedtronicStealthlink::ExamInformation()));
if (dataMapInsertReturn.second){
provided = AddInterfaceProvided("ExamInformation");
if(provided){
provided->AddCommandVoid(&mtsMedtronicStealthlink::RequestExamInformation, this, "RequestExamInformation");
}
dataMapInsertReturn.first->second->ConfigureInterfaceProvided(provided);
} else {
CMN_LOG_CLASS_INIT_ERROR << "Init: Failed to add Exam data to the DataMap " << std::endl;
}
}
guiTask::guiTask(const char *name) : mtsTaskContinuous(name){
mtsInterfaceRequired * required = AddInterfaceRequired("guiRequiresNDI");
if (required) {
required->AddFunction("Beep", NDI.Beep);
required->AddFunction("PortHandlesInitialize", NDI.Initialize);
required->AddFunction("PortHandlesQuery", NDI.Query);
required->AddFunction("PortHandlesEnable", NDI.Enable);
required->AddFunction("CalibratePivot", NDI.CalibratePivot);
required->AddFunction("ToggleTracking", NDI.Track);
required->AddFunction("ReportStrayMarkers", NDI.ReportStrayMarkers);
}
mtsInterfaceRequired * requiredRobotBase = AddInterfaceRequired("guiRequiresRobotBase");
if (requiredRobotBase) {
requiredRobotBase->AddFunction("GetPosition", RobotBase.GetPosition);
}
mtsInterfaceRequired * requiredRobotTip = AddInterfaceRequired("guiRequiresRobotTip");
if (requiredRobotTip) {
requiredRobotTip->AddFunction("GetPosition", RobotTip.GetPosition);
}
mtsInterfaceProvided * provided = AddInterfaceProvided("guiProvidesClarity");
if (provided) {
provided->AddCommandRead(&guiTask::GetRobotBasePosition, this, "GetRobotBasePosition", vct7());
provided->AddCommandRead(&guiTask::GetRobotTipPosition, this, "GetRobotTipPosition", vct7());
}
trackToggle = true;
robotBasePosition.SetAll(0.0);
robotTipPosition.SetAll(0.0);
}
void svlFilterSplitter::CreateInterfaces()
{
// Add NON-QUEUED provided interface for configuration management
mtsInterfaceProvided* provided = AddInterfaceProvided("Settings", MTS_COMMANDS_SHOULD_NOT_BE_QUEUED);
if (provided) {
provided->AddCommandWrite(&svlFilterSplitter::AddOutputCommand, this, "AddOutput");
}
}
bool mtsIntuitiveResearchKitConsole::AddFootpedalInterfaces(void)
{
// Footpedal events, receive
mtsInterfaceRequired * clutchRequired = AddInterfaceRequired("Clutch");
if (clutchRequired) {
clutchRequired->AddEventHandlerWrite(&mtsIntuitiveResearchKitConsole::ClutchEventHandler, this, "Button");
} else {
return false;
}
mtsInterfaceRequired * cameraRequired = AddInterfaceRequired("Camera");
if (cameraRequired) {
cameraRequired->AddEventHandlerWrite(&mtsIntuitiveResearchKitConsole::CameraEventHandler, this, "Button");
} else {
return false;
}
mtsInterfaceRequired * headRequired = AddInterfaceRequired("OperatorPresent");
if (headRequired) {
headRequired->AddEventHandlerWrite(&mtsIntuitiveResearchKitConsole::OperatorPresentEventHandler, this, "Button");
} else {
return false;
}
// Console events, send
mtsInterfaceProvided * interfaceProvided = AddInterfaceProvided("Clutch");
if (interfaceProvided) {
interfaceProvided->AddEventWrite(ConsoleEvents.Clutch, "Button", prmEventButton());
} else {
return false;
}
interfaceProvided = AddInterfaceProvided("Camera");
if (interfaceProvided) {
interfaceProvided->AddEventWrite(ConsoleEvents.Camera, "Button", prmEventButton());
} else {
return false;
}
interfaceProvided = AddInterfaceProvided("OperatorPresent");
if (interfaceProvided) {
interfaceProvided->AddEventWrite(ConsoleEvents.OperatorPresent, "Button", prmEventButton());
} else {
return false;
}
return true;
}
CameraMotion() : mtsTaskPeriodic( "CameraMotion", 0.01, true ){
Rt.Position().Translation()[2] = 1;
StateTable.AddData( Rt, "PositionOrientation" );
// provide the camera position
mtsInterfaceProvided* output = AddInterfaceProvided( "Output" );
output->AddCommandReadState( StateTable, Rt, "GetPositionCartesian" );
}
void mtsSartoriusSerial::SetupInterface(void)
{
// add weight to state table
StateTable.AddData(this->Weight, "Weight");
// add one interface, this will create an mtsTaskInterface
mtsInterfaceProvided * providedInterface = AddInterfaceProvided("Scale");
if (providedInterface) {
// add command to access state table values to the interface
providedInterface->AddCommandReadState(this->StateTable, this->Weight, "GetWeight");
}
}
void NDISimulator::Construct(void){
mtsInterfaceProvided* provided = AddInterfaceProvided("providesPositionCartesian");
StateTable.AddData(MarkerPosition,"MarkerPosition");
if(provided){
provided->AddCommandReadState(StateTable, MarkerPosition, "GetPositionCartesian");
}
theta = 0;
}
// main constructor
mtsODEManipulatorTask::mtsODEManipulatorTask( const std::string& name,
double period,
osaOSGManipulator* manipulator,
osaCPUMask cpumask,
int priority ) :
mtsTaskPeriodic( name, period, true ),
manipulator( manipulator ),
input( NULL ),
output( NULL ),
ctl( NULL ),
cpumask( cpumask ),
priority( priority ){
input = AddInterfaceProvided( "Input" );
if( input ){
StateTable.AddData( qin, "PositionsInput" );
input->AddCommandWriteState( StateTable, qin, "SetPositionJoint" );
}
else{
CMN_LOG_RUN_ERROR << "Failed to create interface Input for " << GetName()
<< std::endl;
}
output = AddInterfaceProvided( "Output" );
if( output ){
StateTable.AddData( qout, "PositionsOutput" );
output->AddCommandReadState( StateTable, qout, "GetPositionJoint" );
}
else{
CMN_LOG_RUN_ERROR << "Failed to create interface Output for " << GetName()
<< std::endl;
}
}
mtsIntuitiveResearchKitConsole::mtsIntuitiveResearchKitConsole(const std::string & componentName):
mtsTaskFromSignal(componentName, 100)
{
mtsInterfaceProvided * interfaceProvided = AddInterfaceProvided("Main");
if (interfaceProvided) {
interfaceProvided->AddCommandWrite(&mtsIntuitiveResearchKitConsole::SetRobotsControlState, this,
"SetRobotsControlState", std::string(""));
interfaceProvided->AddCommandWrite(&mtsIntuitiveResearchKitConsole::TeleopEnable, this,
"TeleopEnable", false);
interfaceProvided->AddEventWrite(MessageEvents.Error, "Error", std::string(""));
interfaceProvided->AddEventWrite(MessageEvents.Warning, "Warning", std::string(""));
interfaceProvided->AddEventWrite(MessageEvents.Status, "Status", std::string(""));
}
}
void mtsMicroScribeDigitizer::InitComponent(void)
{
mtsInterfaceProvided * provided = AddInterfaceProvided(DigitizerInterfaceName);
if (provided) {
StateTable.AddData(DeviceStatus, "DeviceStatus");
StateTable.AddData(TipPosition, "TipPosition");
StateTable.AddData(TipOrientation, "TipOrientation");
StateTable.AddData(TipOrientationUnitVector, "TipOrientationUnitVector");
StateTable.AddData(ButtonState, "ButtonState");
StateTable.AddData(JointReadings, "JointReadings");
provided->AddCommandReadState(StateTable, DeviceStatus, DigitizerCommandNames::GetDeviceStatus);
provided->AddCommandReadState(StateTable, TipPosition, DigitizerCommandNames::GetTipPosition);
provided->AddCommandReadState(StateTable, TipOrientation, DigitizerCommandNames::GetTipOrientation);
provided->AddCommandReadState(StateTable, TipOrientationUnitVector, DigitizerCommandNames::GetTipOrientationUnitVector);
provided->AddCommandReadState(StateTable, ButtonState, DigitizerCommandNames::GetButtonState);
provided->AddCommandReadState(StateTable, JointReadings, DigitizerCommandNames::GetJointReadings);
provided->AddCommandRead(&mtsMicroScribeDigitizer::GetDigitizerInfo, this, DigitizerCommandNames::GetDigitizerInfo);
provided->AddEventVoid(EventButton1Up, DigitizerEventNames::EventButton1Up);
provided->AddEventVoid(EventButton1Down, DigitizerEventNames::EventButton1Down);
provided->AddEventVoid(EventButton2Up, DigitizerEventNames::EventButton2Up);
provided->AddEventVoid(EventButton2Down, DigitizerEventNames::EventButton2Down);
provided->AddEventVoid(EventDigitizerConnected, DigitizerEventNames::EventDigitizerConnected);
provided->AddEventVoid(EventDigitizerDisconnected, DigitizerEventNames::EventDigitizerDisconnected);
}
// Device initialization
if (ARM_SUCCESS != ArmStart(NULL)) {
// Retry after calling ArmEnd()
ArmEnd();
if (ARM_SUCCESS != ArmStart(NULL)) {
cmnThrow("mtsMicroScribeDigitizer: Startup: Failed to initialize MicroScribe Digitizer");
}
}
// Custom error handler registration
if (ARM_SUCCESS != ArmSetErrorHandlerFunction(NO_HCI_HANDLER, ErrorHandler_NoHCI)) {
cmnThrow("mtsMicroScribeDigitizer: Startup: Failed to register error handler: NO_HCI_HANDLER");
}
if (ARM_SUCCESS != ArmSetErrorHandlerFunction(BAD_PORT_HANDLER, ErrorHandler_BadPort)) {
cmnThrow("mtsMicroScribeDigitizer: Startup: Failed to register error handler: BAD_PORT_HANDLER");
}
if (ARM_SUCCESS != ArmSetErrorHandlerFunction(CANT_OPEN_HANDLER, ErrorHandler_CantOpen)) {
cmnThrow("mtsMicroScribeDigitizer: Startup: Failed to register error handler: CANT_OPEN_HANDLER");
}
if (ARM_SUCCESS != ArmSetErrorHandlerFunction(CANT_BEGIN_HANDLER, ErrorHandler_CantBegin)) {
cmnThrow("mtsMicroScribeDigitizer: Startup: Failed to register error handler: CANT_BEGIN_HANDLER");
}
}
void mtsAtracsysFusionTrack::Construct(void)
{
Internals = new mtsAtracsysFusionTrackInternals();
StateTable.AddData(NumberOfThreeDFiducials, "NumberOfThreeDFiducials");
StateTable.AddData(ThreeDFiducialPosition, "ThreeDFiducialPosition");
mtsInterfaceProvided * provided = AddInterfaceProvided("Controller");
if (provided) {
// provided->AddCommandReadState(StateTable, IsTracking, "IsTracking");
provided->AddCommandReadState(StateTable, NumberOfThreeDFiducials, "GetNumberOfThreeDFiducials");
provided->AddCommandReadState(StateTable, ThreeDFiducialPosition, "GetThreeDFiducialPosition");
}
}
mtsIntuitiveResearchKitConsole::mtsIntuitiveResearchKitConsole(const std::string & componentName):
mtsTaskFromSignal(componentName, 100),
mConfigured(false),
mSUJECMInterfaceRequired(0),
mECMBaseFrameInterfaceProvided(0)
{
mtsInterfaceProvided * interfaceProvided = AddInterfaceProvided("Main");
if (interfaceProvided) {
interfaceProvided->AddCommandWrite(&mtsIntuitiveResearchKitConsole::SetRobotsControlState, this,
"SetRobotsControlState", std::string(""));
interfaceProvided->AddCommandWrite(&mtsIntuitiveResearchKitConsole::TeleopEnable, this,
"TeleopEnable", false);
interfaceProvided->AddEventWrite(MessageEvents.Error, "Error", std::string(""));
interfaceProvided->AddEventWrite(MessageEvents.Warning, "Warning", std::string(""));
interfaceProvided->AddEventWrite(MessageEvents.Status, "Status", std::string(""));
}
mIOComponentName = "io";
mOperatorPresentComponent = mIOComponentName;
mOperatorPresentInterface = "COAG";
}
mtsManagerComponentServer::mtsManagerComponentServer(mtsManagerGlobal * gcm)
: mtsManagerComponentBase(mtsManagerComponentBase::GetNameOfManagerComponentServer()),
GCM(gcm),
InterfaceGCMFunctionMap("InterfaceGCMFunctionMap")
{
// Prevent this component from being created more than once
// MJ: singleton can be implemented instead.
static int instanceCount = 0;
if (instanceCount != 0) {
cmnThrow(std::runtime_error("Error in creating manager component server: it's already created"));
}
gcm->SetMCS(this);
InterfaceGCMFunctionMap.SetOwner(*this);
// For system-wide thread-safe logging
mtsInterfaceProvided * provided = AddInterfaceProvided(
mtsManagerComponentBase::InterfaceNames::InterfaceSystemLoggerProvided);
if (provided) {
provided->AddEventWrite(this->EventPrintLog,
mtsManagerComponentBase::EventNames::PrintLog,
mtsLogMessage());
}
}
serverQtComponent::serverQtComponent(const std::string & componentName):
mtsComponent(componentName),
VoidCounter(0),
ReadValue(0),
QualifiedReadValue(0)
{
// create the cisstMultiTask interface with commands and events
mtsInterfaceProvided * provided = AddInterfaceProvided("Provided");
if (provided) {
provided->AddCommandVoid(&serverQtComponent::Void, this, "Void");
provided->AddCommandWrite(&serverQtComponent::Write, this, "Write");
provided->AddCommandRead(&serverQtComponent::Read, this, "Read");
provided->AddCommandQualifiedRead(&serverQtComponent::QualifiedRead, this, "QualifiedRead");
provided->AddEventVoid(EventVoid, "EventVoid");
provided->AddEventWrite(EventWrite, "EventWrite", mtsInt());
}
// create the user interface
ServerWidget.setupUi(&CentralWidget);
MainWindow.setCentralWidget(&CentralWidget);
MainWindow.setWindowTitle(QString::fromStdString(componentName));
MainWindow.show();
// connect Qt signals to slots
QObject::connect(this, SIGNAL(VoidQSignal(int)),
ServerWidget.VoidValue, SLOT(setNum(int)));
QObject::connect(this, SIGNAL(WriteQSignal(int)),
ServerWidget.WriteValue, SLOT(setNum(int)));
QObject::connect(ServerWidget.ReadSlider, SIGNAL(valueChanged(int)),
this, SLOT(ReadQSlot(int)));
QObject::connect(ServerWidget.QualifiedReadSlider, SIGNAL(valueChanged(int)),
this, SLOT(QualifiedReadQSlot(int)));
QObject::connect(ServerWidget.EventVoidButton, SIGNAL(clicked()),
this, SLOT(EventVoidQSlot()));
QObject::connect(ServerWidget.EventWriteSlider, SIGNAL(sliderMoved(int)),
this, SLOT(EventWriteQSlot(int)));
}
void mtsIntuitiveResearchKitArm::Init(void)
{
mCounter = 0;
IsGoalSet = false;
SetState(mtsIntuitiveResearchKitArmTypes::DVRK_UNINITIALIZED);
// initialize trajectory data
JointGet.SetSize(NumberOfJoints());
JointVelocityGet.SetSize(NumberOfJoints());
JointSet.SetSize(NumberOfJoints());
JointSetParam.Goal().SetSize(NumberOfAxes());
JointTrajectory.Velocity.SetSize(NumberOfJoints());
JointTrajectory.Acceleration.SetSize(NumberOfJoints());
JointTrajectory.Start.SetSize(NumberOfJoints());
JointTrajectory.Goal.SetSize(NumberOfJoints());
JointTrajectory.GoalError.SetSize(NumberOfJoints());
JointTrajectory.GoalTolerance.SetSize(NumberOfJoints());
JointTrajectory.EndTime = 0.0;
PotsToEncodersTolerance.SetSize(NumberOfAxes());
// initialize velocity
CartesianVelocityGetParam.SetVelocityLinear(vct3(0.0));
CartesianVelocityGetParam.SetVelocityAngular(vct3(0.0));
CartesianVelocityGetParam.SetTimestamp(0.0);
CartesianVelocityGetParam.SetValid(false);
// base frame, mostly for cases where no base frame is set by user
BaseFrame = vctFrm4x4::Identity();
BaseFrameValid = true;
// cartesian position are timestamped using timestamps provided by PID
CartesianGetParam.SetAutomaticTimestamp(false);
CartesianGetDesiredParam.SetAutomaticTimestamp(false);
this->StateTable.AddData(CartesianGetLocalParam, "CartesianPositionLocal");
this->StateTable.AddData(CartesianGetLocalDesiredParam, "CartesianPositionLocalDesired");
this->StateTable.AddData(CartesianGetParam, "CartesianPosition");
this->StateTable.AddData(CartesianGetDesiredParam, "CartesianPositionDesired");
this->StateTable.AddData(BaseFrame, "BaseFrame");
this->StateTable.AddData(JointGetParam, "JointPosition");
this->StateTable.AddData(JointGetDesired, "JointPositionDesired");
this->StateTable.AddData(CartesianVelocityGetParam, "CartesianVelocityGetParam");
this->StateTable.AddData(StateJointParam, "StateJoint");
this->StateTable.AddData(StateJointDesiredParam, "StateJointDesired");
// PID
PIDInterface = AddInterfaceRequired("PID");
if (PIDInterface) {
PIDInterface->AddFunction("Enable", PID.Enable);
PIDInterface->AddFunction("GetPositionJoint", PID.GetPositionJoint);
PIDInterface->AddFunction("GetPositionJointDesired", PID.GetPositionJointDesired);
PIDInterface->AddFunction("GetStateJoint", PID.GetStateJoint);
PIDInterface->AddFunction("GetStateJointDesired", PID.GetStateJointDesired);
PIDInterface->AddFunction("SetPositionJoint", PID.SetPositionJoint);
PIDInterface->AddFunction("SetCheckJointLimit", PID.SetCheckJointLimit);
PIDInterface->AddFunction("GetVelocityJoint", PID.GetVelocityJoint);
PIDInterface->AddFunction("EnableTorqueMode", PID.EnableTorqueMode);
PIDInterface->AddFunction("SetTorqueJoint", PID.SetTorqueJoint);
PIDInterface->AddFunction("SetTorqueOffset", PID.SetTorqueOffset);
PIDInterface->AddFunction("EnableTrackingError", PID.EnableTrackingError);
PIDInterface->AddFunction("SetTrackingErrorTolerances", PID.SetTrackingErrorTolerance);
PIDInterface->AddEventHandlerWrite(&mtsIntuitiveResearchKitArm::JointLimitEventHandler, this, "JointLimit");
PIDInterface->AddEventHandlerWrite(&mtsIntuitiveResearchKitArm::ErrorEventHandler, this, "Error");
}
// Robot IO
IOInterface = AddInterfaceRequired("RobotIO");
if (IOInterface) {
IOInterface->AddFunction("GetSerialNumber", RobotIO.GetSerialNumber);
IOInterface->AddFunction("EnablePower", RobotIO.EnablePower);
IOInterface->AddFunction("DisablePower", RobotIO.DisablePower);
IOInterface->AddFunction("GetActuatorAmpStatus", RobotIO.GetActuatorAmpStatus);
IOInterface->AddFunction("GetBrakeAmpStatus", RobotIO.GetBrakeAmpStatus);
IOInterface->AddFunction("BiasEncoder", RobotIO.BiasEncoder);
IOInterface->AddFunction("ResetSingleEncoder", RobotIO.ResetSingleEncoder);
IOInterface->AddFunction("GetAnalogInputPosSI", RobotIO.GetAnalogInputPosSI);
IOInterface->AddFunction("SetActuatorCurrent", RobotIO.SetActuatorCurrent);
IOInterface->AddFunction("UsePotsForSafetyCheck", RobotIO.UsePotsForSafetyCheck);
IOInterface->AddFunction("SetPotsToEncodersTolerance", RobotIO.SetPotsToEncodersTolerance);
IOInterface->AddFunction("BrakeRelease", RobotIO.BrakeRelease);
IOInterface->AddFunction("BrakeEngage", RobotIO.BrakeEngage);
}
// Setup Joints
SUJInterface = AddInterfaceRequired("BaseFrame", MTS_OPTIONAL);
if (SUJInterface) {
SUJInterface->AddEventHandlerWrite(&mtsIntuitiveResearchKitArm::SetBaseFrame, this, "BaseFrameDesired");
SUJInterface->AddEventHandlerWrite(&mtsIntuitiveResearchKitArm::ErrorEventHandler, this, "Error");
}
// Arm
RobotInterface = AddInterfaceProvided("Robot");
if (RobotInterface) {
// Get
RobotInterface->AddCommandReadState(this->StateTable, JointGetParam, "GetPositionJoint");
RobotInterface->AddCommandReadState(this->StateTable, JointGetDesired, "GetPositionJointDesired");
RobotInterface->AddCommandReadState(this->StateTable, StateJointParam, "GetStateJoint");
RobotInterface->AddCommandReadState(this->StateTable, StateJointDesiredParam, "GetStateJointDesired");
RobotInterface->AddCommandReadState(this->StateTable, CartesianGetLocalParam, "GetPositionCartesianLocal");
RobotInterface->AddCommandReadState(this->StateTable, CartesianGetLocalDesiredParam, "GetPositionCartesianLocalDesired");
RobotInterface->AddCommandReadState(this->StateTable, CartesianGetParam, "GetPositionCartesian");
RobotInterface->AddCommandReadState(this->StateTable, CartesianGetDesiredParam, "GetPositionCartesianDesired");
RobotInterface->AddCommandReadState(this->StateTable, BaseFrame, "GetBaseFrame");
RobotInterface->AddCommandReadState(this->StateTable, CartesianVelocityGetParam, "GetVelocityCartesian");
// Set
RobotInterface->AddCommandWrite(&mtsIntuitiveResearchKitArm::SetPositionJoint, this, "SetPositionJoint");
RobotInterface->AddCommandWrite(&mtsIntuitiveResearchKitArm::SetPositionGoalJoint, this, "SetPositionGoalJoint");
RobotInterface->AddCommandWrite(&mtsIntuitiveResearchKitArm::SetPositionCartesian, this, "SetPositionCartesian");
RobotInterface->AddCommandWrite(&mtsIntuitiveResearchKitArm::SetPositionGoalCartesian, this, "SetPositionGoalCartesian");
// Trajectory events
RobotInterface->AddEventWrite(JointTrajectory.GoalReachedEvent, "GoalReached", bool());
// Robot State
RobotInterface->AddCommandWrite(&mtsIntuitiveResearchKitArm::SetRobotControlState,
this, "SetRobotControlState", std::string(""));
RobotInterface->AddCommandRead(&mtsIntuitiveResearchKitArm::GetRobotControlState,
this, "GetRobotControlState", std::string(""));
// Human readable messages
RobotInterface->AddEventWrite(MessageEvents.Status, "Status", std::string(""));
RobotInterface->AddEventWrite(MessageEvents.Warning, "Warning", std::string(""));
RobotInterface->AddEventWrite(MessageEvents.Error, "Error", std::string(""));
RobotInterface->AddEventWrite(MessageEvents.RobotState, "RobotState", std::string(""));
// Stats
RobotInterface->AddCommandReadState(StateTable, StateTable.PeriodStats,
"GetPeriodStatistics");
}
}
bool mtsManagerComponentServer::AddInterfaceGCM(void)
{
// InterfaceGCM's required interface is not created here but is created
// when a manager component client connects to the manager component
// server.
// See mtsManagerComponentServer::AddNewClientProcess()
// for the creation of required interfaces.
// Add provided interface to which InterfaceLCM's required interface connects.
const std::string interfaceName = mtsManagerComponentBase::GetNameOfInterfaceGCMProvided();
mtsInterfaceProvided * provided = AddInterfaceProvided(interfaceName);
if (!provided) {
CMN_LOG_CLASS_INIT_ERROR << "AddInterfaceGCM: failed to add \"GCM\" provided interface: " << interfaceName << std::endl;
return false;
}
provided->AddCommandWriteReturn(&mtsManagerComponentServer::InterfaceGCMCommands_ComponentCreate,
this, mtsManagerComponentBase::CommandNames::ComponentCreate);
provided->AddCommandWrite(&mtsManagerComponentServer::InterfaceGCMCommands_ComponentConfigure,
this, mtsManagerComponentBase::CommandNames::ComponentConfigure);
provided->AddCommandWrite(&mtsManagerComponentServer::InterfaceGCMCommands_ComponentConnect,
this, mtsManagerComponentBase::CommandNames::ComponentConnect);
provided->AddCommandWrite(&mtsManagerComponentServer::InterfaceGCMCommands_ComponentDisconnect,
this, mtsManagerComponentBase::CommandNames::ComponentDisconnect);
provided->AddCommandWrite(&mtsManagerComponentServer::InterfaceGCMCommands_ComponentStart,
this, mtsManagerComponentBase::CommandNames::ComponentStart);
provided->AddCommandWrite(&mtsManagerComponentServer::InterfaceGCMCommands_ComponentStop,
this, mtsManagerComponentBase::CommandNames::ComponentStop);
provided->AddCommandWrite(&mtsManagerComponentServer::InterfaceGCMCommands_ComponentResume,
this, mtsManagerComponentBase::CommandNames::ComponentResume);
provided->AddCommandQualifiedRead(&mtsManagerComponentServer::InterfaceGCMCommands_ComponentGetState,
this, mtsManagerComponentBase::CommandNames::ComponentGetState);
provided->AddCommandRead(&mtsManagerComponentServer::InterfaceGCMCommands_GetNamesOfProcesses,
this, mtsManagerComponentBase::CommandNames::GetNamesOfProcesses);
provided->AddCommandQualifiedRead(&mtsManagerComponentServer::InterfaceGCMCommands_GetNamesOfComponents,
this, mtsManagerComponentBase::CommandNames::GetNamesOfComponents);
provided->AddCommandQualifiedRead(&mtsManagerComponentServer::InterfaceGCMCommands_GetNamesOfInterfaces,
this, mtsManagerComponentBase::CommandNames::GetNamesOfInterfaces);
provided->AddCommandRead(&mtsManagerComponentServer::InterfaceGCMCommands_GetListOfConnections,
this, mtsManagerComponentBase::CommandNames::GetListOfConnections);
provided->AddCommandQualifiedRead(&mtsManagerComponentServer::InterfaceGCMCommands_GetListOfComponentClasses,
this, mtsManagerComponentBase::CommandNames::GetListOfComponentClasses);
provided->AddCommandQualifiedRead(&mtsManagerComponentServer::InterfaceGCMCommands_GetInterfaceProvidedDescription,
this, mtsManagerComponentBase::CommandNames::GetInterfaceProvidedDescription);
provided->AddCommandQualifiedRead(&mtsManagerComponentServer::InterfaceGCMCommands_GetInterfaceRequiredDescription,
this, mtsManagerComponentBase::CommandNames::GetInterfaceRequiredDescription);
provided->AddCommandQualifiedRead(&mtsManagerComponentServer::InterfaceGCMCommands_LoadLibrary,
this, mtsManagerComponentBase::CommandNames::LoadLibrary);
provided->AddCommandWrite(&mtsManagerComponentServer::InterfaceGCMCommands_PrintLog,
this, mtsManagerComponentBase::CommandNames::PrintLog, MTS_COMMAND_NOT_QUEUED);
provided->AddCommandWrite(&mtsManagerComponentServer::InterfaceGCMCommands_EnableLogForwarding,
this, mtsManagerComponentBase::CommandNames::EnableLogForwarding);
provided->AddCommandWrite(&mtsManagerComponentServer::InterfaceGCMCommands_DisableLogForwarding,
this, mtsManagerComponentBase::CommandNames::DisableLogForwarding);
provided->AddCommandQualifiedRead(&mtsManagerComponentServer::InterfaceGCMCommands_GetLogForwardingStates,
this, mtsManagerComponentBase::CommandNames::GetLogForwardingStates);
provided->AddCommandQualifiedRead(&mtsManagerComponentServer::InterfaceGCMCommands_GetAbsoluteTimeDiffs,
this, mtsManagerComponentBase::CommandNames::GetAbsoluteTimeDiffs);
provided->AddEventWrite(this->InterfaceGCMEvents_AddComponent,
mtsManagerComponentBase::EventNames::AddComponent, mtsDescriptionComponent());
// MJ TODO: We may need RemoveComponent command as well
//provided->AddEventWrite(this->InterfaceGCMEvents_AddComponent,
// mtsManagerComponentBase::EventNames::AddComponent, mtsDescriptionComponent());
provided->AddEventWrite(this->InterfaceGCMEvents_AddConnection,
mtsManagerComponentBase::EventNames::AddConnection, mtsDescriptionConnection());
provided->AddEventWrite(this->InterfaceGCMEvents_RemoveConnection,
mtsManagerComponentBase::EventNames::RemoveConnection, mtsDescriptionConnection());
provided->AddEventWrite(this->InterfaceGCMEvents_ChangeState,
mtsManagerComponentBase::EventNames::ChangeState, mtsComponentStateChange());
provided->AddEventVoid(this->InterfaceGCMEvents_MCSReady,
mtsManagerComponentBase::EventNames::MCSReady);
CMN_LOG_CLASS_INIT_VERBOSE << "AddInterfaceGCM: successfully added \"GCM\" interfaces" << std::endl;
return true;
}
void mtsTeleOperationECM::Init(void)
{
// configure state machine
mTeleopState.AddState("SETTING_ARMS_STATE");
mTeleopState.AddState("ENABLED");
mTeleopState.AddAllowedDesiredState("DISABLED");
mTeleopState.AddAllowedDesiredState("ENABLED");
// state change, to convert to string events for users (Qt, ROS)
mTeleopState.SetStateChangedCallback(&mtsTeleOperationECM::StateChanged,
this);
// run for all states
mTeleopState.SetRunCallback(&mtsTeleOperationECM::RunAllStates,
this);
// disabled
mTeleopState.SetTransitionCallback("DISABLED",
&mtsTeleOperationECM::TransitionDisabled,
this);
// setting arms state
mTeleopState.SetEnterCallback("SETTING_ARMS_STATE",
&mtsTeleOperationECM::EnterSettingArmsState,
this);
mTeleopState.SetTransitionCallback("SETTING_ARMS_STATE",
&mtsTeleOperationECM::TransitionSettingArmsState,
this);
// enabled
mTeleopState.SetEnterCallback("ENABLED",
&mtsTeleOperationECM::EnterEnabled,
this);
mTeleopState.SetRunCallback("ENABLED",
&mtsTeleOperationECM::RunEnabled,
this);
mTeleopState.SetTransitionCallback("ENABLED",
&mtsTeleOperationECM::TransitionEnabled,
this);
mScale = 0.2;
mIsClutched = false;
StateTable.AddData(mMTML.PositionCartesianCurrent, "MTMLCartesianPosition");
StateTable.AddData(mMTMR.PositionCartesianCurrent, "MTMRCartesianPosition");
StateTable.AddData(mECM.PositionCartesianCurrent, "ECMCartesianPosition");
mConfigurationStateTable = new mtsStateTable(100, "Configuration");
mConfigurationStateTable->SetAutomaticAdvance(false);
AddStateTable(mConfigurationStateTable);
mConfigurationStateTable->AddData(mScale, "Scale");
mConfigurationStateTable->AddData(mRegistrationRotation, "RegistrationRotation");
mtsInterfaceRequired * interfaceRequired = AddInterfaceRequired("MTML");
if (interfaceRequired) {
interfaceRequired->AddFunction("GetPositionCartesian",
mMTML.GetPositionCartesian);
interfaceRequired->AddFunction("GetVelocityCartesian",
mMTML.GetVelocityCartesian);
interfaceRequired->AddFunction("GetCurrentState",
mMTML.GetCurrentState);
interfaceRequired->AddFunction("GetDesiredState",
mMTML.GetDesiredState);
interfaceRequired->AddFunction("SetDesiredState",
mMTML.SetDesiredState);
interfaceRequired->AddFunction("LockOrientation",
mMTML.LockOrientation);
interfaceRequired->AddFunction("SetWrenchBody",
mMTML.SetWrenchBody);
interfaceRequired->AddFunction("SetWrenchBodyOrientationAbsolute",
mMTML.SetWrenchBodyOrientationAbsolute);
interfaceRequired->AddEventHandlerWrite(&mtsTeleOperationECM::MTMLErrorEventHandler,
this, "Error");
}
interfaceRequired = AddInterfaceRequired("MTMR");
if (interfaceRequired) {
interfaceRequired->AddFunction("GetPositionCartesian",
mMTMR.GetPositionCartesian);
interfaceRequired->AddFunction("GetVelocityCartesian",
mMTMR.GetVelocityCartesian);
interfaceRequired->AddFunction("GetCurrentState",
mMTMR.GetCurrentState);
interfaceRequired->AddFunction("GetDesiredState",
mMTMR.GetDesiredState);
interfaceRequired->AddFunction("SetDesiredState",
mMTMR.SetDesiredState);
interfaceRequired->AddFunction("LockOrientation",
mMTMR.LockOrientation);
interfaceRequired->AddFunction("SetWrenchBody",
mMTMR.SetWrenchBody);
interfaceRequired->AddFunction("SetWrenchBodyOrientationAbsolute",
mMTMR.SetWrenchBodyOrientationAbsolute);
interfaceRequired->AddEventHandlerWrite(&mtsTeleOperationECM::MTMRErrorEventHandler,
this, "Error");
}
interfaceRequired = AddInterfaceRequired("ECM");
if (interfaceRequired) {
// ECM, use PID desired position to make sure there is no jump when engaging
interfaceRequired->AddFunction("GetPositionCartesianDesired",
mECM.GetPositionCartesian);
interfaceRequired->AddFunction("GetStateJointDesired",
mECM.GetStateJointDesired);
interfaceRequired->AddFunction("SetPositionJoint",
mECM.SetPositionJoint);
interfaceRequired->AddFunction("GetCurrentState",
mECM.GetCurrentState);
interfaceRequired->AddFunction("GetDesiredState",
mECM.GetDesiredState);
interfaceRequired->AddFunction("SetDesiredState",
mECM.SetDesiredState);
interfaceRequired->AddEventHandlerWrite(&mtsTeleOperationECM::ECMErrorEventHandler,
this, "Error");
}
// footpedal events
interfaceRequired = AddInterfaceRequired("Clutch");
if (interfaceRequired) {
interfaceRequired->AddEventHandlerWrite(&mtsTeleOperationECM::ClutchEventHandler, this, "Button");
}
mInterface = AddInterfaceProvided("Setting");
if (mInterface) {
mInterface->AddMessageEvents();
// commands
mInterface->AddCommandReadState(StateTable, StateTable.PeriodStats,
"GetPeriodStatistics"); // mtsIntervalStatistics
mInterface->AddCommandWrite(&mtsTeleOperationECM::SetDesiredState, this,
"SetDesiredState", std::string("DISABLED"));
mInterface->AddCommandWrite(&mtsTeleOperationECM::SetScale, this,
"SetScale", 0.5);
mInterface->AddCommandWrite(&mtsTeleOperationECM::SetRegistrationRotation, this,
"SetRegistrationRotation", vctMatRot3());
mInterface->AddCommandReadState(*mConfigurationStateTable,
mScale,
"GetScale");
mInterface->AddCommandReadState(*mConfigurationStateTable,
mRegistrationRotation,
"GetRegistrationRotation");
mInterface->AddCommandReadState(StateTable,
mMTML.PositionCartesianCurrent,
"GetPositionCartesianMTML");
mInterface->AddCommandReadState(StateTable,
mMTMR.PositionCartesianCurrent,
"GetPositionCartesianMTMR");
mInterface->AddCommandReadState(StateTable,
mECM.PositionCartesianCurrent,
"GetPositionCartesianECM");
// events
mInterface->AddEventWrite(MessageEvents.DesiredState,
"DesiredState", std::string(""));
mInterface->AddEventWrite(MessageEvents.CurrentState,
"CurrentState", std::string(""));
mInterface->AddEventWrite(MessageEvents.Following,
"Following", false);
// configuration
mInterface->AddEventWrite(ConfigurationEvents.Scale,
"Scale", 0.5);
}
}
void mtsPIDQtWidget::Init()
{
PID.PositionJointGetParam.Position().SetSize(NumberOfAxis);
PID.PositionJointGetDesired.SetSize(NumberOfAxis);
PID.VelocityJointGetParam.Velocity().SetSize(NumberOfAxis);
PID.EffortJoint.SetSize(NumberOfAxis);
DesiredPosition.SetSize(NumberOfAxis);
DesiredPosition.SetAll(0.0);
DesiredEffort.SetSize(NumberOfAxis);
DesiredEffort.SetAll(0.0);
UnitFactor.SetSize(NumberOfAxis);
UnitFactor.SetAll(1.0);
DirectControl = false;
PlotIndex = 0;
logsEnabled = false;
logEntryIndex = 0;
// Setup cisst interface
mtsInterfaceRequired * interfaceRequired = AddInterfaceRequired("Controller");
if (interfaceRequired) {
interfaceRequired->AddFunction("ResetController", PID.ResetController);
interfaceRequired->AddFunction("Enable", PID.Enable);
interfaceRequired->AddFunction("EnableIO", PID.EnableIO);
interfaceRequired->AddFunction("EnableLogs", PID.EnableLogs);
interfaceRequired->AddFunction("EnableTorqueMode", PID.EnableTorqueMode);
interfaceRequired->AddFunction("SetPositionJoint", PID.SetPositionJoint);
interfaceRequired->AddFunction("SetTorqueJoint", PID.SetEffortJoint);
interfaceRequired->AddFunction("GetPositionJoint", PID.GetPositionJoint);
interfaceRequired->AddFunction("GetVelocityJoint", PID.GetVelocityJoint);
interfaceRequired->AddFunction("GetPositionJointDesired", PID.GetPositionJointDesired);
interfaceRequired->AddFunction("GetEffortJointDesired", PID.GetEffortJointDesired);
interfaceRequired->AddFunction("GetPeriodStatistics", PID.GetPeriodStatistics);
interfaceRequired->AddFunction("GetJointType", PID.GetJointType);
interfaceRequired->AddFunction("GetPGain", PID.GetPGain);
interfaceRequired->AddFunction("GetDGain", PID.GetDGain);
interfaceRequired->AddFunction("GetIGain", PID.GetIGain);
interfaceRequired->AddFunction("SetPGain", PID.SetPGain);
interfaceRequired->AddFunction("SetDGain", PID.SetDGain);
interfaceRequired->AddFunction("SetIGain", PID.SetIGain);
// Events
interfaceRequired->AddEventHandlerWrite(&mtsPIDQtWidget::JointLimitEventHandler, this, "JointLimit");
interfaceRequired->AddEventHandlerWrite(&mtsPIDQtWidget::ErrorEventHandler, this, "Error");
interfaceRequired->AddEventHandlerWrite(&mtsPIDQtWidget::EnableEventHandler, this, "Enabled");
}
if(usingSimulink) {
mtsInterfaceRequired * simulinkQtInterfaceRequired = AddInterfaceRequired("SimulinkQtInterfacePIDCommand");
if (simulinkQtInterfaceRequired) {
simulinkQtInterfaceRequired->AddFunction("EnableSimulinkWidgetFromPID", SimulinkQtWidget.EnableWidget);
simulinkQtInterfaceRequired->AddFunction("EnableSimulinkFromPID", SimulinkQtWidget.Enable);
simulinkQtInterfaceRequired->AddFunction("EnableSimulinkLogsFromPID", SimulinkQtWidget.EnableLogs);
//the below means COMMENCED, as in, is Simulink in control?
simulinkQtInterfaceRequired->AddFunction("IsSimulinkEnabled", SimulinkQtWidget.IsEnabled);
}
mtsInterfaceProvided * simulinkQtInterfaceProvided = AddInterfaceProvided("SimulinkQtInterfaceSimulinkCommand");
if (simulinkQtInterfaceProvided) {
simulinkQtInterfaceProvided->AddCommandWrite(&mtsPIDQtWidget::EnablePIDFromSimulinkQt , this, "EnablePIDFromSimulink", mtsBool());
simulinkQtInterfaceProvided->AddCommandWrite(&mtsPIDQtWidget::EnableLogsFromSimulinkQt , this, "EnablePIDLogsFromSimulink", mtsBool());
}
}
setupUi();
startTimer(TimerPeriodInMilliseconds); // ms
}
mtsMedtronicStealthlink::Tool * mtsMedtronicStealthlink::AddTool(const std::string & stealthName, const std::string & interfaceName)
{
// First, check if tool has already been added
Tool * tool = FindTool(stealthName);
if (tool) {
if (tool->GetInterfaceName() == interfaceName) {
CMN_LOG_CLASS_RUN_WARNING << "AddTool: tool " << stealthName << " already exists with interface "
<< interfaceName << std::endl;
return tool;
}
// We could support having the same tool in multiple interfaces, but we would need to maintain
// an array of CurrentTools, or loop through the entire Tools list in the Run method (to assign the
// MarkerPosition and TooltipPosition).
CMN_LOG_CLASS_RUN_ERROR << "AddTool: tool " << stealthName << " already exists in interface "
<< tool->GetInterfaceName() << ", could not create new interface "
<< interfaceName << std::endl;
return 0;
}
// Next, check if interface has already been added
mtsInterfaceProvided * provided = GetInterfaceProvided(interfaceName);
if (provided) {
CMN_LOG_CLASS_RUN_ERROR << "AddTool: interface " << interfaceName << " already exists." << std::endl;
return 0;
}
// Create the tool and add it to the list
tool = new Tool (stealthName, interfaceName);
Tools.push_back(tool);
DataMapContainerInsertReturnValue dataMapInsertReturn;
MNavStealthLink::DataItem * newItem = 0;
myGenericObject * newObject = tool;
if (strcmp(interfaceName.c_str(),"Frame") == 0){
MNavStealthLink::Frame * newFrame = new MNavStealthLink::Frame();
newFrame->name = stealthName;
newItem = newFrame;
} else if (strcmp(interfaceName.c_str(),"Tool") == 0 ){
MNavStealthLink::Instrument * newTool = new MNavStealthLink::Instrument();
newTool->name = stealthName;
newItem = newTool;
}else{
//warning not a Frame or Tool assuming Tool
MNavStealthLink::Instrument * newTool = new MNavStealthLink::Instrument();
newTool->name = stealthName;
newItem = newTool;
}
dataMapInsertReturn = myDataMap.insert(DataMapContainerItem(newItem,newObject));
if (dataMapInsertReturn.second){
CMN_LOG_CLASS_RUN_VERBOSE << "AddTool: adding " << stealthName << " to interface " << interfaceName << std::endl;
provided = AddInterfaceProvided(interfaceName);
dataMapInsertReturn.first->second->ConfigureInterfaceProvided(provided);
} else {
CMN_LOG_CLASS_INIT_ERROR << "AddTool: Failed to add tool to the DataMap" << std::endl;
}
return tool;
}
bool mtsAtracsysFusionTrack::AddToolIni(const std::string & toolName, const std::string & fileName)
{
// check if this tool already exists
mtsAtracsysFusionTrackTool * tool = Tools.GetItem(toolName);
if (tool) {
CMN_LOG_CLASS_INIT_ERROR << "AddToolIni: " << tool->Name << " already exists" << std::endl;
return false;
}
// make sure we can find and load this tool ini file
ftkError error;
ftkGeometry geometry;
switch (loadGeometry(Internals->Library, Internals->Device, fileName, geometry))
{
case 1:
CMN_LOG_CLASS_INIT_VERBOSE << "AddToolIni: loaded " << fileName << " from installation directory" << std::endl;
case 0:
error = ftkSetGeometry(Internals->Library, Internals->Device, &geometry);
if (error != FTK_OK) {
CMN_LOG_CLASS_INIT_ERROR << "AddToolIni: unable to set geometry for tool " << fileName << " (" << this->GetName() << ")" << std::endl;
return false;
}
break;
default:
CMN_LOG_CLASS_INIT_ERROR << "AddToolIni: error, cannot load geometry file " << fileName << std::endl;
return false;
}
// make sure there is no such geometry Id yet
const mtsAtracsysFusionTrackInternals::GeometryIdToToolMap::const_iterator
toolIterator = Internals->GeometryIdToTool.find(geometry.geometryId);
if (toolIterator != Internals->GeometryIdToTool.end()) {
CMN_LOG_CLASS_INIT_ERROR << "AddToolIni: error, found an existing tool with the same Id " << geometry.geometryId << " for " << fileName << std::endl;
return false;
}
// finally create a cisst tool structure
tool = new mtsAtracsysFusionTrackTool(toolName);
// create an interface for tool
tool->Interface = AddInterfaceProvided(toolName);
if (!tool->Interface) {
CMN_LOG_CLASS_INIT_ERROR << "AddToolIni: " << tool->Name << " already exists" << std::endl;
delete tool;
return false;
}
// regiter newly created tool
this->Tools.AddItem(toolName, tool);
Internals->GeometryIdToTool[geometry.geometryId] = tool;
// add data for this tool and populate tool interface
tool->StateTable.SetAutomaticAdvance(false);
this->AddStateTable(&(tool->StateTable));
tool->StateTable.AddData(tool->Position, "Position");
tool->StateTable.AddData(tool->RegistrationError, "RegistrationError");
tool->Interface->AddCommandReadState(tool->StateTable, tool->Position, "GetPositionCartesian");
tool->Interface->AddCommandReadState(tool->StateTable, tool->RegistrationError, "GetRegistrationError");
tool->Interface->AddCommandReadState(tool->StateTable,
tool->StateTable.PeriodStats,
"GetPeriodStatistics");
return true;
}
void mtsIntuitiveResearchKitMTM::Init(void)
{
logsEnabled = false;
logEntryIndex = 0;
SetState(mtsIntuitiveResearchKitMTMTypes::MTM_UNINITIALIZED);
RobotType = MTM_NULL; SetMTMType();
// initialize gripper state
GripperClosed = false;
// initialize trajectory data
JointGet.SetSize(NumberOfJoints);
JointSet.SetSize(NumberOfJoints);
JointSetParam.Goal().SetSize(NumberOfJoints + 1); // PID treats gripper as joint
JointTrajectory.Velocity.SetSize(NumberOfJoints);
JointTrajectory.Velocity.SetAll(720.0 * cmnPI_180); // degrees per second
JointTrajectory.Acceleration.SetSize(NumberOfJoints);
JointTrajectory.Acceleration.SetAll(720.0 * cmnPI_180);
JointTrajectory.Start.SetSize(NumberOfJoints);
JointTrajectory.Goal.SetSize(NumberOfJoints);
JointTrajectory.GoalError.SetSize(NumberOfJoints);
JointTrajectory.GoalTolerance.SetSize(NumberOfJoints);
JointTrajectory.GoalTolerance.SetAll(3.0 * cmnPI / 180.0); // hard coded to 3 degrees
JointTrajectory.EndTime = 0.0;
this->StateTable.AddData(CartesianGetParam, "CartesianPosition");
this->StateTable.AddData(CartesianGetDesiredParam, "CartesianDesired");
this->StateTable.AddData(JointGetParam, "JointPosition");
this->StateTable.AddData(GripperPosition, "GripperAngle");
// setup cisst interfaces
mtsInterfaceRequired * interfaceRequired;
interfaceRequired = AddInterfaceRequired("PID");
if (interfaceRequired) {
interfaceRequired->AddFunction("Enable", PID.Enable);
interfaceRequired->AddFunction("EnableTorqueMode", PID.EnableTorqueMode);
interfaceRequired->AddFunction("GetPositionJoint", PID.GetPositionJoint);
interfaceRequired->AddFunction("GetPositionJointDesired", PID.GetPositionJointDesired);
interfaceRequired->AddFunction("SetPositionJoint", PID.SetPositionJoint);
interfaceRequired->AddFunction("SetTorqueJoint", PID.SetTorqueJoint);
interfaceRequired->AddFunction("SetCheckJointLimit", PID.SetCheckJointLimit);
interfaceRequired->AddFunction("SetTorqueOffset", PID.SetTorqueOffset);
}
if(usingSimulink) {
mtsInterfaceRequired * simulinkInterfaceRequired = AddInterfaceRequired("SimulinkControlCommand");
if (simulinkInterfaceRequired) {
simulinkInterfaceRequired->AddFunction("SignalTrajEvent", SimulinkController.SignalTrajectoryRunning);
simulinkInterfaceRequired->AddFunction("SetPositionJoint", SimulinkController.SetDesiredJointPosition);
simulinkInterfaceRequired->AddFunction("SetPositionCartDes", SimulinkController.SetDesiredCartesianPosition);
simulinkInterfaceRequired->AddFunction("GetSimControllerType", SimulinkController.GetControllerTypeIsJoint);
}
}
// Robot IO
interfaceRequired = AddInterfaceRequired("RobotIO");
if (interfaceRequired) {
interfaceRequired->AddFunction("EnablePower", RobotIO.EnablePower);
interfaceRequired->AddFunction("DisablePower", RobotIO.DisablePower);
interfaceRequired->AddFunction("GetActuatorAmpStatus", RobotIO.GetActuatorAmpStatus);
interfaceRequired->AddFunction("BiasEncoder", RobotIO.BiasEncoder);
interfaceRequired->AddFunction("SetActuatorCurrent", RobotIO.SetActuatorCurrent);
interfaceRequired->AddFunction("UsePotsForSafetyCheck", RobotIO.UsePotsForSafetyCheck);
interfaceRequired->AddFunction("SetPotsToEncodersTolerance", RobotIO.SetPotsToEncodersTolerance);
interfaceRequired->AddFunction("ResetSingleEncoder", RobotIO.ResetSingleEncoder);
interfaceRequired->AddFunction("GetAnalogInputPosSI", RobotIO.GetAnalogInputPosSI);
}
mtsInterfaceProvided * interfaceProvided = AddInterfaceProvided("Robot");
if (interfaceProvided) {
// Cartesian
interfaceProvided->AddCommandReadState(this->StateTable, JointGetParam, "GetPositionJoint");
interfaceProvided->AddCommandReadState(this->StateTable, CartesianGetParam, "GetPositionCartesian");
interfaceProvided->AddCommandReadState(this->StateTable, CartesianGetDesiredParam, "GetPositionCartesianDesired");
interfaceProvided->AddCommandWrite(&mtsIntuitiveResearchKitMTM::SetPositionCartesian, this, "SetPositionCartesian");
interfaceProvided->AddCommandWrite(&mtsIntuitiveResearchKitMTM::SetWrench, this, "SetWrench");
interfaceProvided->AddCommandRead(&mtsIntuitiveResearchKitMTM::GetRobotCartVelFromJacobian, this, "GetRobotCartVelFromJacobian");
interfaceProvided->AddCommandRead(&mtsIntuitiveResearchKitMTM::GetJointTorqueFromForceTorque, this, "GetJointTorqueFromForceTorque");
// Gripper
interfaceProvided->AddCommandReadState(this->StateTable, GripperPosition, "GetGripperPosition");
interfaceProvided->AddEventVoid(EventTriggers.GripperPinch, "GripperPinchEvent");
interfaceProvided->AddEventWrite(EventTriggers.GripperClosed, "GripperClosedEvent", true);
// Robot State
interfaceProvided->AddCommandWrite(&mtsIntuitiveResearchKitMTM::SetRobotControlState, this, "SetRobotControlState", std::string(""));
interfaceProvided->AddCommandRead(&mtsIntuitiveResearchKitMTM::GetRobotControlState, this, "GetRobotControlState", std::string(""));
interfaceProvided->AddEventWrite(EventTriggers.RobotStatusMsg, "RobotStatusMsg", std::string(""));
interfaceProvided->AddEventWrite(EventTriggers.RobotErrorMsg, "RobotErrorMsg", std::string(""));
// Stats
interfaceProvided->AddCommandReadState(StateTable, StateTable.PeriodStats, "GetPeriodStatistics");
}
}
void mtsTeleOperation::Init(void)
{
Counter = 0;
Scale = 0.2;
// Initialize states
this->IsClutched = false;
this->IsOperatorPresent = false;
this->IsEnabled = false;
Slave.IsManipClutched = false;
this->RotationLocked = false;
this->TranslationLocked = false;
this->StateTable.AddData(Master.PositionCartesianCurrent, "MasterCartesianPosition");
this->StateTable.AddData(Slave.PositionCartesianCurrent, "SlaveCartesianPosition");
this->ConfigurationStateTable = new mtsStateTable(100, "Configuration");
this->ConfigurationStateTable->SetAutomaticAdvance(false);
this->AddStateTable(this->ConfigurationStateTable);
this->ConfigurationStateTable->AddData(this->Scale, "Scale");
this->ConfigurationStateTable->AddData(this->RegistrationRotation, "RegistrationRotation");
this->ConfigurationStateTable->AddData(this->RotationLocked, "RotationLocked");
this->ConfigurationStateTable->AddData(this->TranslationLocked, "TranslationLocked");
// Setup CISST Interface
mtsInterfaceRequired * masterRequired = AddInterfaceRequired("Master");
if (masterRequired) {
masterRequired->AddFunction("GetPositionCartesian", Master.GetPositionCartesian);
masterRequired->AddFunction("SetPositionCartesian", Master.SetPositionCartesian);
masterRequired->AddFunction("SetPositionGoalCartesian", Master.SetPositionGoalCartesian);
masterRequired->AddFunction("GetGripperPosition", Master.GetGripperPosition);
masterRequired->AddFunction("SetRobotControlState", Master.SetRobotControlState);
masterRequired->AddEventHandlerWrite(&mtsTeleOperation::MasterErrorEventHandler, this, "Error");
}
mtsInterfaceRequired * slaveRequired = AddInterfaceRequired("Slave");
if (slaveRequired) {
slaveRequired->AddFunction("GetPositionCartesian", Slave.GetPositionCartesian);
slaveRequired->AddFunction("SetPositionCartesian", Slave.SetPositionCartesian);
slaveRequired->AddFunction("SetJawPosition", Slave.SetJawPosition);
slaveRequired->AddFunction("SetRobotControlState", Slave.SetRobotControlState);
slaveRequired->AddEventHandlerWrite(&mtsTeleOperation::SlaveErrorEventHandler, this, "Error");
slaveRequired->AddEventHandlerWrite(&mtsTeleOperation::SlaveClutchEventHandler, this, "ManipClutch");
}
// Footpedal events
mtsInterfaceRequired * clutchRequired = AddInterfaceRequired("Clutch");
if (clutchRequired) {
clutchRequired->AddEventHandlerWrite(&mtsTeleOperation::ClutchEventHandler, this, "Button");
}
mtsInterfaceRequired * headRequired = AddInterfaceRequired("OperatorPresent");
if (headRequired) {
headRequired->AddEventHandlerWrite(&mtsTeleOperation::OperatorPresentEventHandler, this, "Button");
}
mtsInterfaceProvided * providedSettings = AddInterfaceProvided("Setting");
if (providedSettings) {
// commands
providedSettings->AddCommandReadState(StateTable, StateTable.PeriodStats,
"GetPeriodStatistics"); // mtsIntervalStatistics
providedSettings->AddCommandWrite(&mtsTeleOperation::Enable, this, "Enable", false);
providedSettings->AddCommandWrite(&mtsTeleOperation::SetScale, this, "SetScale", 0.5);
providedSettings->AddCommandWrite(&mtsTeleOperation::SetRegistrationRotation, this,
"SetRegistrationRotation", vctMatRot3());
providedSettings->AddCommandWrite(&mtsTeleOperation::LockRotation, this, "LockRotation", false);
providedSettings->AddCommandWrite(&mtsTeleOperation::LockTranslation, this, "LockTranslation", false);
providedSettings->AddCommandWrite(&mtsTeleOperation::CameraClutchEventHandler, this, "CameraClutch", prmEventButton());
providedSettings->AddCommandReadState(*(this->ConfigurationStateTable), Scale, "GetScale");
providedSettings->AddCommandReadState(*(this->ConfigurationStateTable), RegistrationRotation, "GetRegistrationRotation");
providedSettings->AddCommandReadState(*(this->ConfigurationStateTable), RotationLocked, "GetRotationLocked");
providedSettings->AddCommandReadState(*(this->ConfigurationStateTable), TranslationLocked, "GetTranslationLocked");
providedSettings->AddCommandReadState(this->StateTable, Master.PositionCartesianCurrent, "GetPositionCartesianMaster");
providedSettings->AddCommandReadState(this->StateTable, Slave.PositionCartesianCurrent, "GetPositionCartesianSlave");
// events
providedSettings->AddEventWrite(MessageEvents.Status, "Status", std::string(""));
providedSettings->AddEventWrite(MessageEvents.Warning, "Warning", std::string(""));
providedSettings->AddEventWrite(MessageEvents.Error, "Error", std::string(""));
providedSettings->AddEventWrite(MessageEvents.Enabled, "Enabled", false);
// configuration
providedSettings->AddEventWrite(ConfigurationEvents.Scale, "Scale", 0.5);
providedSettings->AddEventWrite(ConfigurationEvents.RotationLocked, "RotationLocked", false);
providedSettings->AddEventWrite(ConfigurationEvents.TranslationLocked, "TranslationLocked", false);
}
}
SetPoints( const std::string& robotfilename,
const vctFrame4x4<double>& Rtw0,
const vctDynamicVector<double>& qinit ) :
mtsTaskPeriodic( "Robot", 0.01, true ),
manipulator( NULL ),
q( qinit ),
qready( qinit ),
t( 0.0 ),
ctl( NULL ),
mtsEnabled( false ),
online( false ),
input( NULL ),
output( NULL ){
manipulator = new robManipulator( robotfilename, Rtw0 );
Rt = manipulator->ForwardKinematics( q );
x = Rt[0][3];
qready[0] = 0.0;
qready[1] = -cmnPI/2.0;
qready[2] = 0.0;
qready[3] = cmnPI/2.0;
qready[4] = 0.0;
qready[5] = -cmnPI/2.0;
qready[6] = 0.0;
// The control interface: to change the state of the component
ctl = AddInterfaceProvided( "Control" );
if( ctl ){
StateTable.AddData( mtsEnabled, "Enabled" );
ctl->AddCommandWriteState( StateTable, mtsEnabled, "Enable" );
}
else{
CMN_LOG_RUN_ERROR << "Failed to create interface Control for "<< GetName()
<< std::endl;
}
input = AddInterfaceProvided( "Input" );
if( input ){
StateTable.AddData( prmCommand, "Enabled" );
input->AddCommandWriteState(StateTable,prmCommand,"SetPositionCartesian");
}
else{
CMN_LOG_RUN_ERROR << "Failed to create interface Input for " << GetName()
<< std::endl;
}
outputSE3 = AddInterfaceProvided( "OutputSE3" );
if( outputSE3 ){
StateTable.AddData( prmTelemetry, "Telemetry" );
outputSE3->AddCommandReadState( StateTable,
prmTelemetry,
"GetPositionCartesian" );
}
output = AddInterfaceRequired( "Output" );
if( output ){
output->AddFunction( "SetPositionCartesian", SetPositionCartesian );
}
else{
CMN_LOG_RUN_ERROR << "Failed to create interface Output for " << GetName()
<< std::endl;
}
}
void mtsIntuitiveResearchKitPSM::Init(void)
{
IsCartesianGoalSet = false;
Counter = 0;
SetState(PSM_UNINITIALIZED);
DesiredOpenAngle = 0 * cmnPI_180;
// initialize trajectory data
JointCurrent.SetSize(NumberOfJoints);
JointDesired.SetSize(NumberOfJoints);
JointDesiredParam.Goal().SetSize(NumberOfJoints);
JointTrajectory.Start.SetSize(NumberOfJoints);
JointTrajectory.Velocity.SetSize(NumberOfJoints);
JointTrajectory.Acceleration.SetSize(NumberOfJoints);
JointTrajectory.Goal.SetSize(NumberOfJoints);
JointTrajectory.GoalError.SetSize(NumberOfJoints);
JointTrajectory.GoalTolerance.SetSize(NumberOfJoints);
JointTrajectory.GoalTolerance.SetAll(3.0 * cmnPI / 180.0); // hard coded to 3 degrees
JointTrajectory.Zero.SetSize(NumberOfJoints);
EngagingJointSet.SetSize(NumberOfJoints);
this->StateTable.AddData(CartesianCurrentParam, "CartesianPosition");
this->StateTable.AddData(JointCurrentParam, "JointPosition");
// setup CISST Interface
mtsInterfaceRequired * interfaceRequired;
interfaceRequired = AddInterfaceRequired("PID");
if (interfaceRequired) {
interfaceRequired->AddFunction("Enable", PID.Enable);
interfaceRequired->AddFunction("GetPositionJoint", PID.GetPositionJoint);
interfaceRequired->AddFunction("GetPositionJointDesired", PID.GetPositionJointDesired);
interfaceRequired->AddFunction("SetPositionJoint", PID.SetPositionJoint);
interfaceRequired->AddFunction("SetCheckJointLimit", PID.SetCheckJointLimit);
}
// Robot IO
interfaceRequired = AddInterfaceRequired("RobotIO");
if (interfaceRequired) {
interfaceRequired->AddFunction("EnablePower", RobotIO.EnablePower);
interfaceRequired->AddFunction("DisablePower", RobotIO.DisablePower);
interfaceRequired->AddFunction("GetAmpStatus", RobotIO.GetAmpStatus);
interfaceRequired->AddFunction("BiasEncoder", RobotIO.BiasEncoder);
interfaceRequired->AddFunction("SetMotorCurrent", RobotIO.SetMotorCurrent);
}
// Event Adapter engage: digital input button event from PSM
interfaceRequired = AddInterfaceRequired("Adapter");
if (interfaceRequired) {
Adapter.IsPresent = false;
interfaceRequired->AddFunction("GetButton", Adapter.GetButton);
interfaceRequired->AddEventHandlerWrite(&mtsIntuitiveResearchKitPSM::EventHandlerAdapter, this, "Button");
}
// Event Tool engage: digital input button event from PSM
interfaceRequired = AddInterfaceRequired("Tool");
if (interfaceRequired) {
Tool.IsPresent = false;
interfaceRequired->AddFunction("GetButton", Tool.GetButton);
interfaceRequired->AddEventHandlerWrite(&mtsIntuitiveResearchKitPSM::EventHandlerTool, this, "Button");
}
// ManipClutch: digital input button event from PSM
interfaceRequired = AddInterfaceRequired("ManipClutch");
if (interfaceRequired) {
interfaceRequired->AddEventHandlerWrite(&mtsIntuitiveResearchKitPSM::EventHandlerManipClutch, this, "Button");
}
// SUJClutch: digital input button event from PSM
interfaceRequired = AddInterfaceRequired("SUJClutch");
if (interfaceRequired) {
interfaceRequired->AddEventHandlerWrite(&mtsIntuitiveResearchKitPSM::EventHandlerSUJClutch, this, "Button");
}
mtsInterfaceProvided * interfaceProvided = AddInterfaceProvided("Robot");
if (interfaceProvided) {
interfaceProvided->AddCommandReadState(this->StateTable, JointCurrentParam, "GetPositionJoint");
interfaceProvided->AddCommandReadState(this->StateTable, CartesianCurrentParam, "GetPositionCartesian");
interfaceProvided->AddCommandWrite(&mtsIntuitiveResearchKitPSM::SetPositionCartesian, this, "SetPositionCartesian");
interfaceProvided->AddCommandWrite(&mtsIntuitiveResearchKitPSM::SetOpenAngle, this, "SetOpenAngle");
interfaceProvided->AddCommandWrite(&mtsIntuitiveResearchKitPSM::SetRobotControlState,
this, "SetRobotControlState", std::string(""));
interfaceProvided->AddEventWrite(EventTriggers.RobotStatusMsg, "RobotStatusMsg", std::string(""));
interfaceProvided->AddEventWrite(EventTriggers.RobotErrorMsg, "RobotErrorMsg", std::string(""));
interfaceProvided->AddEventWrite(EventTriggers.ManipClutch, "ManipClutchBtn", prmEventButton());
interfaceProvided->AddEventWrite(EventTriggers.SUJClutch, "SUJClutchBtn", prmEventButton());
// Stats
interfaceProvided->AddCommandReadState(StateTable, StateTable.PeriodStats,
"GetPeriodStatistics");
}
}
void mtsIntuitiveResearchKitConsole::Configure(const std::string & filename)
{
mConfigured = false;
std::ifstream jsonStream;
jsonStream.open(filename.c_str());
Json::Value jsonConfig, jsonValue;
Json::Reader jsonReader;
if (!jsonReader.parse(jsonStream, jsonConfig)) {
CMN_LOG_CLASS_INIT_ERROR << "Configure: failed to parse configuration\n"
<< jsonReader.getFormattedErrorMessages();
this->mConfigured = false;
return;
}
// extract path of main json config file to search other files relative to it
cmnPath configPath(cmnPath::GetWorkingDirectory());
std::string fullname = configPath.Find(filename);
std::string configDir = fullname.substr(0, fullname.find_last_of('/'));
configPath.Add(configDir);
// IO default settings
double periodIO = 0.5 * cmn_ms;
int firewirePort = 0;
// get user preferences
jsonValue = jsonConfig["io"];
if (!jsonValue.empty()) {
CMN_LOG_CLASS_INIT_VERBOSE << "Configure: looking for user provided io:period and io:port" << std::endl;
jsonValue = jsonConfig["io"]["period"];
if (!jsonValue.empty()) {
periodIO = jsonValue.asDouble();
}
jsonValue = jsonConfig["io"]["port"];
if (!jsonValue.empty()) {
firewirePort = jsonValue.asInt();
}
} else {
CMN_LOG_CLASS_INIT_VERBOSE << "Configure: using default io:period and io:port" << std::endl;
}
CMN_LOG_CLASS_INIT_VERBOSE << "Configure: period IO is " << periodIO << std::endl
<< "Configure: FireWire port is " << firewirePort << std::endl;
// create IO
mtsRobotIO1394 * io = new mtsRobotIO1394(mIOComponentName, periodIO, firewirePort);
const Json::Value arms = jsonConfig["arms"];
for (unsigned int index = 0; index < arms.size(); ++index) {
if (!ConfigureArmJSON(arms[index], io->GetName(), configPath)) {
CMN_LOG_CLASS_INIT_ERROR << "Configure: failed to configure arms[" << index << "]" << std::endl;
return;
}
}
// loop over all arms to configure IO only
const ArmList::iterator end = mArms.end();
ArmList::iterator iter;
for (iter = mArms.begin(); iter != end; ++iter) {
std::string ioConfig = iter->second->mIOConfigurationFile;
if (ioConfig != "") {
io->Configure(ioConfig);
}
}
mtsComponentManager::GetInstance()->AddComponent(io);
// now can configure PID and Arms
for (iter = mArms.begin(); iter != end; ++iter) {
const std::string pidConfig = iter->second->mPIDConfigurationFile;
if (pidConfig != "") {
iter->second->ConfigurePID(pidConfig);
}
const std::string armConfig = iter->second->mArmConfigurationFile;
if (armConfig != "") {
iter->second->ConfigureArm(iter->second->mType, armConfig);
}
}
bool hasSUJ = false;
bool hasECM = false;
// now load all PSM teleops
const Json::Value psmTeleops = jsonConfig["psm-teleops"];
for (unsigned int index = 0; index < psmTeleops.size(); ++index) {
if (!ConfigurePSMTeleopJSON(psmTeleops[index])) {
CMN_LOG_CLASS_INIT_ERROR << "Configure: failed to configure psm-teleops[" << index << "]" << std::endl;
return;
}
}
// see which event is used for operator present
// find name of button event used to detect if operator is present
mOperatorPresentComponent = jsonConfig["operator-present"]["component"].asString();
mOperatorPresentInterface = jsonConfig["operator-present"]["interface"].asString();
//set defaults
if (mOperatorPresentComponent == "") {
mOperatorPresentComponent = mIOComponentName;
}
if (mOperatorPresentInterface == "") {
mOperatorPresentInterface = "COAG";
}
// look for footpedals in json config
jsonValue = jsonConfig["io"]["has-footpedals"];
if (jsonValue.empty()) {
mHasFootpedals = false;
} else {
mHasFootpedals = jsonValue.asBool();
}
// if we have any teleoperation component, we need to add the interfaces for the foot pedals
if (mTeleops.size() > 0) {
mHasFootpedals = true;
}
if (mHasFootpedals) {
this->AddFootpedalInterfaces();
}
// interface to ecm to get ECM frame and then push to PSM SUJs as base frame
mtsInterfaceRequired * ecmArmInterface = 0;
for (iter = mArms.begin(); iter != end; ++iter) {
if (iter->second->mType == Arm::ARM_ECM) {
hasECM = true;
ecmArmInterface = iter->second->ArmInterfaceRequired;
}
else if (iter->second->mType == Arm::ARM_SUJ) {
hasSUJ = true;
}
}
// add required and provided interfaces to grab positions from ECM SUJ and ECM
if (hasSUJ && hasECM) {
mSUJECMInterfaceRequired = AddInterfaceRequired("BaseFrame");
if (mSUJECMInterfaceRequired) {
mSUJECMInterfaceRequired->AddEventHandlerWrite(&mtsIntuitiveResearchKitConsole::SUJECMBaseFrameHandler, this, "BaseFrameDesired");
}
if (ecmArmInterface) {
ecmArmInterface->AddFunction("GetPositionCartesianLocal", mGetPositionCartesianLocalFromECM);
} else {
CMN_LOG_CLASS_INIT_VERBOSE << "Configure: arm interface not yet added for ECM" << std::endl;
}
mECMBaseFrameInterfaceProvided = AddInterfaceProvided("ECMBaseFrame");
if (mECMBaseFrameInterfaceProvided) {
mECMBaseFrameInterfaceProvided->AddEventWrite(mECMBaseFrameEvent, "BaseFrameDesired", prmPositionCartesianGet());
}
}
// connect arm SUJ clutch button to SUJ
if (hasSUJ) {
for (iter = mArms.begin(); iter != end; ++iter) {
Arm * arm = iter->second;
if ((arm->mType == Arm::ARM_ECM) || (arm->mType == Arm::ARM_PSM)) {
arm->SUJInterfaceRequiredFromIO = this->AddInterfaceRequired("SUJ-" + arm->Name() + "-IO");
arm->SUJInterfaceRequiredFromIO->AddEventHandlerWrite(&Arm::SUJClutchEventHandlerFromIO, arm, "Button");
arm->SUJInterfaceRequiredToSUJ = this->AddInterfaceRequired("SUJ-" + arm->Name());
arm->SUJInterfaceRequiredToSUJ->AddFunction("Clutch", arm->SUJClutch);
} else {
arm->SUJInterfaceRequiredFromIO = 0;
arm->SUJInterfaceRequiredToSUJ = 0;
}
}
}
mConfigured = true;
}
