27 #include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
29 #include "../Devices/ControlDevice.h"
30 #include "../Devices/SensorDevice.h"
36 class RTThreadTimingsSensorDevice;
46 defineOptionalProperty<std::size_t>(
47 "NjointController_AllowedExecutionTimePerControlDeviceUntilWarning",
49 "A Warning will be printed, If the execution time in micro seconds of a "
50 "NJointControllerBase "
51 "exceeds this parameter times the number of ControlDevices.");
52 defineOptionalProperty<std::size_t>(
53 "NjointController_AllowedExecutionTimePerControlDeviceUntilError",
55 "An Error will be printed, If the execution time in micro seconds of a "
56 "NJointControllerBase "
57 "exceeds this parameter times the number of ControlDevices.");
58 defineOptionalProperty<bool>(
59 "EnableInverseDynamics",
61 "If true, inverse dynamics are calculated for all joints given in the "
62 "InverseDynamicsRobotJointSets property during each loop of "
63 "the rt control thread.");
64 defineOptionalProperty<std::string>(
"InverseDynamicsRobotJointSets",
66 "Comma separated list of robot nodesets for which "
67 "the inverse dynamics should be calculcated.");
68 defineOptionalProperty<std::string>(
69 "InverseDynamicsRobotBodySet",
71 "Robot nodeset of which the masses should be used for the inverse dynamics");
86 enum class EmergencyStopStateRequest
112 return ModuleBase::Instance<ControlThread>();
120 void _preFinishControlThreadInitialization();
122 void _preOnInitRobotUnit();
132 const Ice::Current& = Ice::emptyCurrent)
override;
157 return controlThreadId;
166 void setEmergencyStopStateNoReportToTopic(EmergencyStopState state);
171 void processEmergencyStopRequest();
285 return rtIsInEmergencyStop_;
295 return rtIsInEmergencyStop_ ? EmergencyStopState::eEmergencyStopActive
296 : EmergencyStopState::eEmergencyStopInactive;
318 std::vector<JointController*>& rtGetActivatedJointControllers();
319 const std::vector<JointController*>& rtGetActivatedJointControllers()
const;
324 std::vector<NJointControllerBase*>& rtGetActivatedNJointControllers();
325 const std::vector<NJointControllerBase*>& rtGetActivatedNJointControllers()
const;
333 std::vector<std::size_t>& rtGetActivatedJointToNJointControllerAssignement();
334 const std::vector<std::size_t>& rtGetActivatedJointToNJointControllerAssignement()
const;
341 const std::vector<JointController*>& rtGetRequestedJointControllers()
const;
346 const std::vector<NJointControllerBase*>& rtGetRequestedNJointControllers()
const;
354 const std::vector<std::size_t>& rtGetRequestedJointToNJointControllerAssignement()
const;
358 void dumpRtControllerSetup(std::ostream& out,
359 const std::string& indent,
360 const std::vector<JointController*>& activeCdevs,
361 const std::vector<std::size_t>& activeJCtrls,
362 const std::vector<NJointControllerBase*>& assignement)
const;
363 std::string dumpRtState()
const;
369 std::atomic_bool rtIsInEmergencyStop_{
false};
371 std::atomic_bool emergencyStop{
false};
376 std::vector<VirtualRobot::RobotNodePtr> rtRobotNodes;
379 std::atomic<std::thread::id> controlThreadId;
382 std::size_t usPerDevUntilWarn;
384 std::size_t usPerDevUntilError;
386 bool rtSwitchControllerSetupChangedControllers{
false};
387 std::size_t numberOfInvalidTargetsInThisIteration{0};
389 std::vector<JointController*> preSwitchSetup_ActivatedJointControllers;
390 std::vector<std::size_t> preSwitchSetup_ActivatedJointToNJointControllerAssignement;
391 std::vector<NJointControllerBase*> preSwitchSetup_ActivatedNJointControllers;
393 std::vector<JointController*> postSwitchSetup_ActivatedJointControllers;
394 std::vector<std::size_t> postSwitchSetup_ActivatedJointToNJointControllerAssignement;
395 std::vector<NJointControllerBase*> postSwitchSetup_ActivatedNJointControllers;
397 std::vector<std::size_t> _activatedSynchronousNJointControllersIdx;
398 std::vector<std::size_t> _activatedAsynchronousNJointControllersIdx;
400 std::atomic<EmergencyStopStateRequest> emergencyStopStateRequest{
401 EmergencyStopStateRequest::RequestNone};
403 std::shared_ptr<DynamicsHelper> dynamicsHelpers;
405 std::size_t _maxControllerCount = 0;