void mtsIntuitiveResearchKitConsole::Arm::ConfigureArm(const ArmType armType,
const std::string & configFile,
const double & periodInSeconds,
mtsComponent * existingArm)
{
mtsManagerLocal * componentManager = mtsManagerLocal::GetInstance();
mArmConfigurationFile = configFile;
// for research kit arms, create, add to manager and connect to
// extra IO, PID, etc. For generic arms, do nothing.
switch (armType) {
case ARM_MTM:
{
if (!existingArm) {
mtsIntuitiveResearchKitMTM * master = new mtsIntuitiveResearchKitMTM(Name(), periodInSeconds);
master->Configure(mArmConfigurationFile);
componentManager->AddComponent(master);
}
}
break;
case ARM_PSM:
{
if (!existingArm) {
mtsIntuitiveResearchKitPSM * slave = new mtsIntuitiveResearchKitPSM(Name(), periodInSeconds);
slave->Configure(mArmConfigurationFile);
componentManager->AddComponent(slave);
}
componentManager->Connect(Name(), "Adapter",
IOComponentName(), Name() + "-Adapter");
componentManager->Connect(Name(), "Tool",
IOComponentName(), Name() + "-Tool");
componentManager->Connect(Name(), "ManipClutch",
IOComponentName(), Name() + "-ManipClutch");
}
break;
case ARM_ECM:
{
if (!existingArm) {
mtsIntuitiveResearchKitECM * ecm = new mtsIntuitiveResearchKitECM(Name(), periodInSeconds);
ecm->Configure(mArmConfigurationFile);
componentManager->AddComponent(ecm);
}
componentManager->Connect(Name(), "ManipClutch",
IOComponentName(), Name() + "-ManipClutch");
}
break;
default:
break;
}
// if the arm is a research kit arm
if ((armType == ARM_PSM) || (armType == ARM_MTM) || (armType == ARM_ECM)) {
// Connect arm to IO and PID
componentManager->Connect(Name(), "RobotIO",
IOComponentName(), Name());
componentManager->Connect(Name(), "PID",
PIDComponentName(), "Controller");
}
}
void mtsIntuitiveResearchKitConsole::Arm::ConfigurePID(const std::string & configFile,
const double & periodInSeconds)
{
mPIDComponentName = mName + "-PID";
mPIDConfigurationFile = configFile;
mtsManagerLocal * componentManager = mtsManagerLocal::GetInstance();
mtsPID * pidMaster = new mtsPID(mPIDComponentName,
(periodInSeconds != 0.0) ? periodInSeconds : 1.0 * cmn_s);
pidMaster->Configure(mPIDConfigurationFile);
componentManager->AddComponent(pidMaster);
componentManager->Connect(PIDComponentName(), "RobotJointTorqueInterface", IOComponentName(), Name());
if (periodInSeconds == 0.0) {
componentManager->Connect(PIDComponentName(), "ExecIn",
IOComponentName(), "ExecOut");
}
}
bool mtsIntuitiveResearchKitConsole::Arm::Connect(void)
{
mtsManagerLocal * componentManager = mtsManagerLocal::GetInstance();
// for research kit arms, create, add to manager and connect to
// extra IO, PID, etc. For generic arms, do nothing.
switch (mType) {
case ARM_MTM:
case ARM_MTM_DERIVED:
break;
case ARM_PSM:
case ARM_PSM_DERIVED:
componentManager->Connect(Name(), "Adapter",
IOComponentName(), Name() + "-Adapter");
componentManager->Connect(Name(), "Tool",
IOComponentName(), Name() + "-Tool");
componentManager->Connect(Name(), "ManipClutch",
IOComponentName(), Name() + "-ManipClutch");
break;
case ARM_ECM:
componentManager->Connect(Name(), "ManipClutch",
IOComponentName(), Name() + "-ManipClutch");
break;
case ARM_SUJ:
componentManager->Connect(Name(), "RobotIO",
IOComponentName(), Name());
componentManager->Connect(Name(), "MuxReset",
IOComponentName(), "MuxReset");
componentManager->Connect(Name(), "MuxIncrement",
IOComponentName(), "MuxIncrement");
componentManager->Connect(Name(), "ControlPWM",
IOComponentName(), "ControlPWM");
componentManager->Connect(Name(), "EnablePWM",
IOComponentName(), "EnablePWM");
componentManager->Connect(Name(), "MotorUp",
IOComponentName(), "MotorUp");
componentManager->Connect(Name(), "MotorDown",
IOComponentName(), "MotorDown");
componentManager->Connect(Name(), "SUJ-Clutch-1",
IOComponentName(), "SUJ-Clutch-1");
componentManager->Connect(Name(), "SUJ-Clutch-2",
IOComponentName(), "SUJ-Clutch-2");
componentManager->Connect(Name(), "SUJ-Clutch-3",
IOComponentName(), "SUJ-Clutch-3");
componentManager->Connect(Name(), "SUJ-Clutch-4",
IOComponentName(), "SUJ-Clutch-4");
break;
default:
break;
}
// if the arm is a research kit arm
if ((mType == ARM_PSM) ||
(mType == ARM_MTM) ||
(mType == ARM_PSM_DERIVED) ||
(mType == ARM_MTM_DERIVED) ||
(mType == ARM_ECM)) {
// Connect arm to IO and PID
componentManager->Connect(Name(), "RobotIO",
IOComponentName(), Name());
componentManager->Connect(Name(), "PID",
PIDComponentName(), "Controller");
if ((mBaseFrameComponentName != "") && (mBaseFrameInterfaceName != "")) {
componentManager->Connect(Name(), "BaseFrame", mBaseFrameComponentName, mBaseFrameInterfaceName);
}
}
return true;
}
