void mtsWatchdogClient::AddToRequiredInterface(mtsInterfaceRequired &reqInt) {
// Required interface to match the watchdog on the remote server
reqInt.AddFunction("GetWatchdogState" , Watchdog.ReadState);
reqInt.AddFunction("SetWatchdogState" , Watchdog.WriteState);
reqInt.AddFunction("WatchdogReset" , Watchdog.ResetState);
}
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;
}
}
