32 #include "../RobotUnit.h"
33 #include "../SensorValues/SensorValue1DoFActuator.h"
34 #include "../Units/ForceTorqueSubUnit.h"
35 #include "../Units/InertialMeasurementSubUnit.h"
36 #include "../Units/KinematicSubUnit.h"
37 #include "../Units/PlatformSubUnit.h"
38 #include "../Units/TCPControllerSubUnit.h"
39 #include "../Units/TrajectoryControllerSubUnit.h"
52 SetEmergencyStopStateNoReportToTopic(
ControlThread* controlThreadModule,
53 EmergencyStopState state)
55 controlThreadModule->setEmergencyStopStateNoReportToTopic(state);
64 virtual public EmergencyStopMasterInterface
77 const Ice::Current& = Ice::emptyCurrent)
final override
83 ControlThreadAttorneyForRobotUnitEmergencyStopMaster::
112 return "EmergencyStopMaster";
128 std::vector<ManagedIceObjectPtr> unitsCopy;
134 Ice::ObjectProxySeq r;
135 r.reserve(unitsCopy.size());
138 r.emplace_back(unit->getProxy(-1,
true));
151 return unit->getProxy(-1,
true);
156 const EmergencyStopMasterInterfacePtr&
160 return emergencyStopMaster;
182 if (_module<RobotData>().arePlatformAndLocalizationUnitsEnabled())
190 if (_module<RobotData>().arePlatformAndLocalizationUnitsEnabled())
215 ARMARX_INFO <<
"initializing default units...done! "
223 using IfaceT = KinematicUnitInterface;
228 if (
getUnit(IfaceT::ice_staticId()))
242 const std::string& positionControlMode,
243 const std::string& velocityControlMode,
244 const std::string& torqueControlMode,
245 const std::string& pwmControlMode)
254 std::map<std::string, UnitT::ActuatorData> devs;
255 for (
const ControlDevicePtr& controlDevice :
256 _module<Devices>().getControlDevices().
values())
259 const auto& controlDeviceName = controlDevice->getDeviceName();
260 ARMARX_DEBUG <<
"looking at device " << controlDeviceName
261 << controlDevice->getJointControllerToTargetTypeNameMap();
262 if (!controlDevice->hasTag(ControlDeviceTags::CreateNoDefaultController))
264 UnitT::ActuatorData ad;
265 ad.name = controlDeviceName;
267 << _module<Devices>().getSensorDevices().has(controlDeviceName);
268 ad.sensorDeviceIndex =
269 _module<Devices>().getSensorDevices().has(controlDeviceName)
270 ? _module<Devices>().getSensorDevices().index(controlDeviceName)
273 const auto init_controller =
274 [&](
const std::string& requestedControlMode,
auto& ctrl,
const auto* tpptr)
276 auto controlMode = requestedControlMode;
277 using CtargT = std::remove_const_t<std::remove_reference_t<decltype(*tpptr)>>;
278 NJointKinematicUnitPassThroughControllerPtr& pt = ctrl;
280 auto testMode = [&](
const auto& m)
286 if (!testMode(controlMode))
292 auto jointControllers = controlDevice->getJointControllers();
293 for (
const auto* jointController : jointControllers)
295 if (jointController->getControlTarget()->isA<CtargT>())
299 selected_ctrl =
nullptr;
300 ARMARX_INFO <<
"Autodetected two controllers supporting "
301 << requestedControlMode <<
"! autoselection failed";
304 selected_ctrl = jointController;
309 controlMode = selected_ctrl->getControlMode();
310 ARMARX_INFO <<
"Autodetected controller with mode " << controlMode
311 <<
"(the requested mode was " << requestedControlMode
316 if (!controlMode.empty())
318 NJointKinematicUnitPassThroughControllerConfigPtr config =
321 config->deviceName = controlDeviceName;
322 auto ctrl_name =
getName() +
"_" +
"NJointKU_PTCtrl_" + controlDeviceName +
324 std::replace(ctrl_name.begin(), ctrl_name.end(),
'.',
'_');
325 std::replace(ctrl_name.begin(), ctrl_name.end(),
'-',
'_');
326 std::replace(ctrl_name.begin(), ctrl_name.end(),
':',
'_');
327 const NJointControllerBasePtr& nJointCtrl =
328 _module<ControllerManagement>().createNJointController(
329 "NJointKinematicUnitPassThroughController",
334 pt = NJointKinematicUnitPassThroughControllerPtr::dynamicCast(nJointCtrl);
339 positionControlMode, ad.ctrlPos, (ControlTarget1DoFActuatorPosition*)0);
341 velocityControlMode, ad.ctrlVel, (ControlTarget1DoFActuatorVelocity*)0);
342 init_controller(torqueControlMode, ad.ctrlTor, (ControlTarget1DoFActuatorTorque*)0);
348 if (ad.ctrlPos || ad.ctrlTor || ad.ctrlVel)
350 ARMARX_DEBUG <<
"created controllers for " << controlDeviceName
351 <<
" pos/tor/vel " << ad.ctrlPos.get() <<
" / " << ad.ctrlTor.get()
352 <<
" / " << ad.ctrlVel.get();
353 devs[controlDeviceName] = std::move(ad);
359 ARMARX_IMPORTANT <<
"No joint devices found - skipping adding of KinematicUnit";
362 ARMARX_IMPORTANT <<
"Found " << devs.size() <<
" joint devices - adding KinematicUnit";
364 <<
"no 1DoF ControlDevice found with matching SensorDevice";
366 const std::string configName = getProperty<std::string>(
"KinematicUnitName");
367 const std::string confPre =
getConfigDomain() +
"." + configName +
".";
370 properties->setProperty(confPre +
"RobotNodeSetName",
371 _module<RobotData>().getRobotNodetSeName());
372 properties->setProperty(confPre +
"RobotFileName", _module<RobotData>().getRobotFileName());
373 properties->setProperty(confPre +
"RobotFileNameProject",
374 _module<RobotData>().getRobotProjectName());
375 properties->setProperty(confPre +
"TopicPrefix",
376 getProperty<std::string>(
"KinematicUnitNameTopicPrefix"));
379 <<
" using these properties: " << properties->getPropertiesForPrefix(
"");
384 unit->setupData(getProperty<std::string>(
"RobotFileName").getValue(),
385 _module<RobotData>().cloneRobot(),
387 _module<Devices>().getControlModeGroups().groups,
388 _module<Devices>().getControlModeGroups().groupsMerged,
398 using IfaceT = PlatformUnitInterface;
405 if (
getUnit(IfaceT::ice_staticId()))
411 if (_module<RobotData>().getRobotPlatformName().
empty())
413 ARMARX_INFO <<
"no platform unit created since no platform name was given";
417 if (!_module<Devices>().getControlDevices().has(
418 _module<RobotData>().getRobotPlatformName()))
421 <<
"no platform unit created since the platform control device with name '"
422 << _module<RobotData>().getRobotPlatformName() <<
"' was not found";
426 const ControlDevicePtr& controlDevice =
427 _module<Devices>().getControlDevices().at(_module<RobotData>().getRobotPlatformName());
428 const SensorDevicePtr& sensorDevice =
429 _module<Devices>().getSensorDevices().at(_module<RobotData>().getRobotPlatformName());
431 controlDevice->getJointController(ControlModes::HolonomicPlatformVelocity);
438 const std::string configName = getProperty<std::string>(
"PlatformUnitName");
439 const std::string confPre =
getConfigDomain() +
"." + configName +
".";
443 properties->setProperty(confPre +
"PlatformName",
444 _module<RobotData>().getRobotPlatformInstanceName());
446 <<
" using these properties: " << properties->getPropertiesForPrefix(
"");
452 NJointHolonomicPlatformUnitVelocityPassThroughControllerConfigPtr config =
455 config->initialVelocityY = 0;
456 config->initialVelocityRotation = 0;
457 config->platformName = _module<RobotData>().getRobotPlatformName();
458 auto ctrl = NJointHolonomicPlatformUnitVelocityPassThroughControllerPtr::dynamicCast(
459 _module<ControllerManagement>().createNJointController(
460 "NJointHolonomicPlatformUnitVelocityPassThroughController",
461 getName() +
"_" + configName +
"_VelPTContoller",
470 NJointHolonomicPlatformRelativePositionControllerConfigPtr configRelativePositionCtrlCfg =
472 configRelativePositionCtrlCfg->
platformName = _module<RobotData>().getRobotPlatformName();
473 auto ctrlRelativePosition =
474 NJointHolonomicPlatformRelativePositionControllerPtr::dynamicCast(
475 _module<ControllerManagement>().createNJointController(
476 "NJointHolonomicPlatformRelativePositionController",
477 getName() +
"_" + configName +
"_RelativePositionContoller",
478 configRelativePositionCtrlCfg,
483 unit->relativePosCtrl = ctrlRelativePosition;
487 NJointHolonomicPlatformGlobalPositionControllerConfigPtr configGlobalPositionCtrlCfg =
489 configGlobalPositionCtrlCfg->
platformName = _module<RobotData>().getRobotPlatformName();
490 auto ctrlGlobalPosition = NJointHolonomicPlatformGlobalPositionControllerPtr::dynamicCast(
491 _module<ControllerManagement>().createNJointController(
492 "NJointHolonomicPlatformGlobalPositionController",
493 getName() +
"_" + configName +
"_GlobalPositionContoller",
494 configGlobalPositionCtrlCfg,
500 unit->globalPosCtrl = ctrlGlobalPosition;
503 unit->platformSensorIndex = _module<Devices>().getSensorDevices().index(
504 _module<RobotData>().getRobotPlatformName());
517 using IfaceT = LocalizationUnitInterface;
523 if (
getUnit(IfaceT::ice_staticId()))
529 ARMARX_DEBUG <<
"Getting device SensorValueHolonomicPlatformRelativePosition";
531 _module<Devices>().getSensorDevices().has(_module<RobotData>().getRobotPlatformName()))
532 << _module<RobotData>().getRobotPlatformName();
533 const SensorDevicePtr& sensorDeviceRelativePosition =
534 _module<Devices>().getSensorDevices().at(_module<RobotData>().getRobotPlatformName());
544 const std::string configName =
"LocalizationUnit";
545 const std::string confPre =
getConfigDomain() +
"." + configName +
".";
551 <<
" using these properties: " << properties->getPropertiesForPrefix(
"");
556 ARMARX_DEBUG <<
"Setting device GlobalRobotPoseCorrectionSensorDevice";
557 unit->globalPositionCorrectionSensorDevice =
558 dynamic_cast<decltype(unit-
>globalPositionCorrectionSensorDevice)>(
565 const SensorDevicePtr& sensorDeviceGlobalRobotLocalization =
566 _module<Devices>().getSensorDevices().at(
576 unit->globalPositionCorrectionSensorDevice->getSensorValue()
588 using IfaceT = ForceTorqueUnitInterface;
593 if (
getUnit(IfaceT::ice_staticId()))
598 std::vector<ForceTorqueSubUnit::DeviceData> ftdevs;
599 for (
const SensorDevicePtr& ftSensorDevice : _module<Devices>().getSensorDevices().
values())
605 _module<Devices>().getSensorDevices().index(ftSensorDevice->getDeviceName());
606 ftSensorData.
deviceName = ftSensorDevice->getDeviceName();
607 ftSensorData.
frame = ftSensorDevice->getReportingFrame();
609 <<
"The sensor device '" << ftSensorData.
deviceName
611 <<
") reports force torque values but returns an empty string as reporting "
614 <<
"The sensor device '" << ftSensorData.
deviceName
616 <<
") reports force torque values but returns a reporting frame '"
617 << ftSensorData.
frame <<
"' which is not present in the robot '"
618 << _module<RobotData>().getRobotName() <<
"'";
619 ftdevs.emplace_back(std::move(ftSensorData));
625 <<
"no force torque unit created since there are no force torque sensor devices";
629 const std::string configName = getProperty<std::string>(
"ForceTorqueUnitName");
630 const std::string confPre =
getConfigDomain() +
"." + configName +
".";
634 properties->setProperty(confPre +
"AgentName", _module<RobotData>().getRobotName());
635 properties->setProperty(confPre +
"ForceTorqueTopicName",
636 getProperty<std::string>(
"ForceTorqueTopicName").getValue());
638 <<
" using these properties: " << properties->getPropertiesForPrefix(
"");
651 using IfaceT = InertialMeasurementUnitInterface;
656 if (
getUnit(IfaceT::ice_staticId()))
661 std::map<std::string, std::size_t> imudevs;
662 for (
auto i :
getIndices(_module<Devices>().getSensorDevices().keys()))
664 const SensorDevicePtr& sensorDevice = _module<Devices>().getSensorDevices().at(i);
667 imudevs[sensorDevice->getDeviceName()] = i;
673 <<
"no inertial measurement unit created since there are no imu sensor devices";
677 const std::string configName = getProperty<std::string>(
"InertialMeasurementUnitName");
678 const std::string confPre =
getConfigDomain() +
"." + configName +
".";
682 properties->setProperty(confPre +
"IMUTopicName",
683 getProperty<std::string>(
"IMUTopicName").getValue());
686 unit->devs = imudevs;
698 if (!getProperty<bool>(
"CreateTrajectoryPlayer").getValue())
704 if (
getUnit(UnitT::ice_staticId()) || !trajectoryControllerSubUnit)
708 (TrajectoryControllerSubUnitPtr::dynamicCast(trajectoryControllerSubUnit))
709 ->setup(_modulePtr<RobotUnit>());
710 addUnit(trajectoryControllerSubUnit);
711 trajectoryControllerSubUnit =
nullptr;
721 if (!getProperty<bool>(
"CreateTCPControlUnit").getValue())
728 if (
getUnit(UnitT::ice_staticId()))
732 (TCPControllerSubUnitPtr::dynamicCast(tcpControllerSubUnit))
733 ->setup(_modulePtr<RobotUnit>(), _module<RobotData>().cloneRobot());
735 tcpControllerSubUnit =
nullptr;
747 RobotUnitSubUnitPtr rsu = RobotUnitSubUnitPtr::dynamicCast(unit);
750 subUnits.emplace_back(std::move(rsu));
752 units.emplace_back(std::move(unit));
756 Units::_icePropertiesInitialized()
760 const std::string& configDomain =
"ArmarX";
764 const std::string configNameTCPControlUnit =
765 getProperty<std::string>(
"TCPControlUnitName");
766 tcpControllerSubUnit = Component::create<TCPControllerSubUnit, TCPControllerSubUnitPtr>(
767 properties, configNameTCPControlUnit, configDomain);
774 const std::string configNameTrajectoryControllerUnit =
775 getProperty<std::string>(
"TrajectoryControllerUnitName");
776 const std::string confPre =
777 configDomain +
"." + configNameTrajectoryControllerUnit +
".";
778 properties->setProperty(confPre +
"KinematicUnitName",
779 getProperty<std::string>(
"KinematicUnitName").getValue());
780 trajectoryControllerSubUnit =
781 Component::create<TrajectoryControllerSubUnit, TrajectoryControllerSubUnitPtr>(
782 properties, configNameTrajectoryControllerUnit, configDomain);
783 addPropertyUser(PropertyUserPtr::dynamicCast(trajectoryControllerSubUnit));
788 Units::_preFinishRunning()
801 Units::_preOnInitRobotUnit()
804 unitCreateRobot = _module<RobotData>().cloneRobot();
807 emergencyStopMaster =
new RobotUnitEmergencyStopMaster{
808 &_module<ControlThread>(),
809 getProperty<std::string>(
"EmergencyStopTopic").getValue()};
813 obj, getProperty<std::string>(
"EmergencyStopMasterName").
getValue(),
true);
814 auto prx = obj->getProxy(-1);
817 getArmarXManager()->getIceManager()->getIceGridSession()->getAdmin()->addObject(
820 catch (
const IceGrid::ObjectExistsException&)
822 getArmarXManager()->getIceManager()->getIceGridSession()->getAdmin()->updateObject(
825 catch (std::exception& e)
827 ARMARX_WARNING <<
"add the robot unit as emergency stop master to the ice grid "
828 "admin!\nThere was an exception:\n"
836 Units::_postOnExitRobotUnit()
843 ManagedIceObjectPtr::dynamicCast(emergencyStopMaster));
845 getProperty<std::string>(
"EmergencyStopMasterName").
getValue());
860 if (unit->ice_isA(staticIceId))