35 <<
"ControlDevice::getJointEmergencyStopController called for a ControlDevice ('"
37 <<
"') without a JointController "
38 "with the ControlMode ControlModes::EmergencyStop"
39 " (add a JointController with this ControlMode)";
40 return jointEmergencyStopController;
47 <<
"ControlDevice::getJointStopMovementController called for a ControlDevice ('"
49 <<
"') without a JointController "
50 "with the ControlMode ControlModes::StopMovement"
51 " (add a JointController with this ControlMode)";
52 return jointStopMovementController;
59 <<
"Called ControlDevice::rtSetActiveJointController with a nullptr (Don't do this)";
60 if (activeJointController == jointCtrl)
64 if (jointCtrl->parent !=
this)
66 throw std::logic_error{
68 ")::rtSetActiveJointController: "
69 "tried to switch to a joint controller from a different ControlDevice "
70 "(You should only call ControlDevice::rtSetActiveJointController with "
71 "JointControllers added to the ControlDevice via "
72 "ControlDevice::addJointController)"};
74 if (activeJointController)
76 activeJointController->rtDeactivate();
78 jointCtrl->rtActivate();
79 activeJointController = jointCtrl;
88 activejointCtrl->rtRun(sensorValuesTimestamp, timeSinceLastIteration);
96 <<
"' was already added to a RobotUnit! Call "
97 "addJointController before calling addControlDevice.";
101 throw std::invalid_argument{
"ControlDevice(" +
getDeviceName() +
102 ")::addJointController: joint controller is nullptr (Don't "
103 "try nullptrs as JointController)"};
105 if (jointCtrl->parent)
107 throw std::logic_error{
109 ")::addJointController: tried to add a joint controller multiple times" +
110 "(Don't try to add a JointController multiple to a ControlDevice)"};
113 const std::string& mode = jointCtrl->getControlMode();
115 <<
"' with mode '" << mode <<
"'";
116 if (jointControllers.has(mode))
118 throw std::invalid_argument{
120 ")::addJointController: joint controller for mode " + mode +
" was already added" +
121 "(Don't try to add multiple JointController with the same ControlMode)"};
123 jointCtrl->parent =
this;
125 jointCtrl->rtResetTarget();
127 if (mode == ControlModes::EmergencyStop)
129 jointEmergencyStopController = jointCtrl;
131 if (mode == ControlModes::StopMovement)
133 jointStopMovementController = jointCtrl;
135 if (!jointCtrl->getControlTarget())
137 throw std::logic_error{
139 ")::addJointController: The JointController has no ControlTarget. (mode = " + mode +
140 ")" +
"(Don't try to add JointController without a ControlTarget)"};
142 const std::hash<std::string> hash{};
143 jointCtrl->controlModeHash = hash(jointCtrl->getControlMode());
144 jointCtrl->hardwareControlModeHash = hash(jointCtrl->getHardwareControlMode());
145 jointControllers.add(mode, std::move(jointCtrl));
146 ARMARX_DEBUG <<
"adding Controller " << jointCtrl <<
"...done";
156 std::map<std::string, std::string>
159 std::map<std::string, std::string> map;
160 for (
const auto& name : jointControllers.keys())
162 map[name] = jointControllers.at(name)->getControlTarget()->getControlTargetType();