void mtsIntuitiveResearchKitPSM::Init(void)
{
// main initialization from base type
mtsIntuitiveResearchKitArm::Init();
// initialize trajectory data
JointTrajectory.Velocity.SetAll(90.0 * cmnPI_180); // degrees per second
JointTrajectory.Velocity.Element(2) = 0.2; // m per second
JointTrajectory.Acceleration.SetAll(90.0 * cmnPI_180);
JointTrajectory.Acceleration.Element(2) = 0.2; // m per second
JointTrajectory.GoalTolerance.SetAll(3.0 * cmnPI / 180.0); // hard coded to 3 degrees
// high values for engage adapter/tool until these use a proper trajectory generator
PotsToEncodersTolerance.SetAll(100.0 * cmnPI_180); // 100 degrees for rotations
PotsToEncodersTolerance.Element(2) = 20.0 * cmn_mm; // 20 mm
// for tool/adapter engage procedure
EngagingJointSet.SetSize(NumberOfJoints());
mtsInterfaceRequired * interfaceRequired;
// Main interface should have been created by base class init
CMN_ASSERT(RobotInterface);
RobotInterface->AddEventWrite(ClutchEvents.ManipClutch, "ManipClutch", prmEventButton());
RobotInterface->AddEventWrite(ClutchEvents.SUJClutch, "SUJClutch", prmEventButton());
// 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");
}
// Main interface should have been created by base class init
CMN_ASSERT(RobotInterface);
RobotInterface->AddCommandWrite(&mtsIntuitiveResearchKitPSM::SetJawPosition, this, "SetJawPosition");
// Initialize the optimizer
Optimizer = new mtsIntuitiveResearchKitOptimizer(6);
Optimizer->InitializeFollowVF(6,
"FollowVFSlave",
"CurrentSlaveKinematics",
"DesiredSlaveKinematics");
}
void mtsIntuitiveResearchKitECM::Init(void)
{
// main initialization from base type
mtsIntuitiveResearchKitArm::Init();
// initialize trajectory data
JointTrajectory.Velocity.Assign(30.0 * cmnPI_180, // degrees per second
30.0 * cmnPI_180,
0.05, // m per second
20.0 * cmnPI_180);
JointTrajectory.Acceleration.Assign(30.0 * cmnPI_180,
30.0 * cmnPI_180,
0.05,
30.0 * cmnPI_180);
JointTrajectory.GoalTolerance.SetAll(3.0 * cmnPI / 180.0); // hard coded to 3 degrees
PotsToEncodersTolerance.SetAll(10.0 * cmnPI_180); // 10 degrees for rotations
PotsToEncodersTolerance.Element(2) = 20.0 * cmn_mm; // 20 mm
mtsInterfaceRequired * interfaceRequired;
// Main interface should have been created by base class init
CMN_ASSERT(RobotInterface);
RobotInterface->AddEventWrite(ClutchEvents.ManipClutch, "ManipClutch", prmEventButton());
// ManipClutch: digital input button event from ECM
interfaceRequired = AddInterfaceRequired("ManipClutch");
if (interfaceRequired) {
interfaceRequired->AddEventHandlerWrite(&mtsIntuitiveResearchKitECM::EventHandlerManipClutch, this, "Button");
}
}
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;
}
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 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);
}
}
