ControlDevice.cpp
Go to the documentation of this file.
1 /*
2  * This file is part of ArmarX.
3  *
4  * ArmarX is free software; you can redistribute it and/or modify
5  * it under the terms of the GNU General Public License version 2 as
6  * published by the Free Software Foundation.
7  *
8  * ArmarX is distributed in the hope that it will be useful, but
9  * WITHOUT ANY WARRANTY; without even the implied warranty of
10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
11  * GNU General Public License for more details.
12  *
13  * You should have received a copy of the GNU General Public License
14  * along with this program. If not, see <http://www.gnu.org/licenses/>.
15  *
16  * @package RobotAPI
17  * @author Raphael Grimm ( raphael dot grimm at kit dot edu )
18  * @date 2017
19  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
20  * GNU General Public License
21  */
22 
23 #include "ControlDevice.h"
24 
26 
27 namespace armarx
28 {
29  const ControlDevicePtr ControlDevice::NullPtr{nullptr};
30 
31  JointController*
33  {
34  ARMARX_CHECK_EXPRESSION(jointEmergencyStopController)
35  << "ControlDevice::getJointEmergencyStopController called for a ControlDevice ('"
36  << getDeviceName()
37  << "') without a JointController "
38  "with the ControlMode ControlModes::EmergencyStop"
39  " (add a JointController with this ControlMode)";
40  return jointEmergencyStopController;
41  }
42 
45  {
46  ARMARX_CHECK_EXPRESSION(jointStopMovementController)
47  << "ControlDevice::getJointStopMovementController called for a ControlDevice ('"
48  << getDeviceName()
49  << "') without a JointController "
50  "with the ControlMode ControlModes::StopMovement"
51  " (add a JointController with this ControlMode)";
52  return jointStopMovementController;
53  }
54 
55  void
57  {
58  ARMARX_CHECK_EXPRESSION(jointCtrl)
59  << "Called ControlDevice::rtSetActiveJointController with a nullptr (Don't do this)";
60  if (activeJointController == jointCtrl)
61  {
62  return;
63  }
64  if (jointCtrl->parent != this)
65  {
66  throw std::logic_error{
67  "ControlDevice(" + getDeviceName() +
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)"};
73  }
74  if (activeJointController)
75  {
76  activeJointController->rtDeactivate();
77  }
78  jointCtrl->rtActivate();
79  activeJointController = jointCtrl;
80  }
81 
82  void
83  ControlDevice::rtRun(const IceUtil::Time& sensorValuesTimestamp,
84  const IceUtil::Time& timeSinceLastIteration)
85  {
86  auto activejointCtrl = rtGetActiveJointController();
87  ARMARX_CHECK_EXPRESSION(activejointCtrl);
88  activejointCtrl->rtRun(sensorValuesTimestamp, timeSinceLastIteration);
89  rtWriteTargetValues(sensorValuesTimestamp, timeSinceLastIteration);
90  }
91 
92  void
94  {
95  ARMARX_CHECK_IS_NULL(owner) << "The ControlDevice '" << getDeviceName()
96  << "' was already added to a RobotUnit! Call "
97  "addJointController before calling addControlDevice.";
98  ARMARX_DEBUG << "adding Controller " << jointCtrl;
99  if (!jointCtrl)
100  {
101  throw std::invalid_argument{"ControlDevice(" + getDeviceName() +
102  ")::addJointController: joint controller is nullptr (Don't "
103  "try nullptrs as JointController)"};
104  }
105  if (jointCtrl->parent)
106  {
107  throw std::logic_error{
108  "ControlDevice(" + getDeviceName() +
109  ")::addJointController: tried to add a joint controller multiple times" +
110  "(Don't try to add a JointController multiple to a ControlDevice)"};
111  }
112  ARMARX_DEBUG << "getControlMode of " << jointCtrl;
113  const std::string& mode = jointCtrl->getControlMode();
114  ARMARX_VERBOSE << "adding joint controller for device '" << getDeviceName()
115  << "' with mode '" << mode << "'";
116  if (jointControllers.has(mode))
117  {
118  throw std::invalid_argument{
119  "ControlDevice(" + getDeviceName() +
120  ")::addJointController: joint controller for mode " + mode + " was already added" +
121  "(Don't try to add multiple JointController with the same ControlMode)"};
122  }
123  jointCtrl->parent = this;
124  ARMARX_DEBUG << "resetting target";
125  jointCtrl->rtResetTarget();
126  ARMARX_DEBUG << "resetting target...done!";
127  if (mode == ControlModes::EmergencyStop)
128  {
129  jointEmergencyStopController = jointCtrl;
130  }
131  if (mode == ControlModes::StopMovement)
132  {
133  jointStopMovementController = jointCtrl;
134  }
135  if (!jointCtrl->getControlTarget())
136  {
137  throw std::logic_error{
138  "ControlDevice(" + getDeviceName() +
139  ")::addJointController: The JointController has no ControlTarget. (mode = " + mode +
140  ")" + "(Don't try to add JointController without a ControlTarget)"};
141  }
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";
147  ARMARX_CHECK_IS_NULL(owner);
148  }
149 
152  {
153  return owner;
154  }
155 
156  std::map<std::string, std::string>
158  {
159  std::map<std::string, std::string> map;
160  for (const auto& name : jointControllers.keys())
161  {
162  map[name] = jointControllers.at(name)->getControlTarget()->getControlTargetType();
163  }
164  return map;
165  }
166 
167 } // namespace armarx
ARMARX_VERBOSE
#define ARMARX_VERBOSE
Definition: Logging.h:180
armarx::ControlDevice::NullPtr
static const ControlDevicePtr NullPtr
A static const nullptr in case a const ref to a nullptr needs to be returned.
Definition: ControlDevice.h:67
armarx::DeviceBase::getDeviceName
const std::string & getDeviceName() const
Definition: DeviceBase.h:76
armarx::ControlDevice::rtRun
void rtRun(const IceUtil::Time &sensorValuesTimestamp, const IceUtil::Time &timeSinceLastIteration)
runs the active Joint Controller and write the target values into the control device
Definition: ControlDevice.cpp:83
armarx::ControlDevice::rtWriteTargetValues
virtual void rtWriteTargetValues(const IceUtil::Time &sensorValuesTimestamp, const IceUtil::Time &timeSinceLastIteration)
This is a hook for implementations to write the setpoints to a bus.
Definition: ControlDevice.h:115
armarx::JointController
The JointController class represents one joint in one control mode.
Definition: JointController.h:51
armarx::ControlDevice::getJointEmergencyStopController
JointController * getJointEmergencyStopController()
Definition: ControlDevice.cpp:32
ControlDevice.h
armarx::ControlDevice::rtSetActiveJointController
virtual void rtSetActiveJointController(JointController *jointCtrl)
Activates the given JointController for this device.
Definition: ControlDevice.cpp:56
armarx::ControlDevice::getOwner
RobotUnitModule::Devices * getOwner() const
Definition: ControlDevice.cpp:151
ARMARX_CHECK_IS_NULL
#define ARMARX_CHECK_IS_NULL(ptr)
This macro evaluates whether ptr is null and if it turns out to be false it will throw an ExpressionE...
Definition: ExpressionException.h:194
ARMARX_DEBUG
#define ARMARX_DEBUG
Definition: Logging.h:177
armarx::ControlDevice::getJointControllerToTargetTypeNameMap
std::map< std::string, std::string > getJointControllerToTargetTypeNameMap() const
Definition: ControlDevice.cpp:157
armarx::armem::Time
armarx::core::time::DateTime Time
Definition: forward_declarations.h:13
ExpressionException.h
armarx::RobotUnitModule::Devices
This Module manages sensor and control devices for a RobotUnit and only allows save and sane access.
Definition: RobotUnitModuleDevices.h:67
ARMARX_CHECK_EXPRESSION
#define ARMARX_CHECK_EXPRESSION(expression)
This macro evaluates the expression and if it turns out to be false it will throw an ExpressionExcept...
Definition: ExpressionException.h:73
armarx::ControlDevice::rtGetActiveJointController
JointController * rtGetActiveJointController()
Definition: ControlDevice.h:169
armarx::ControlDevice::addJointController
void addJointController(JointController *jointCtrl)
adds the Joint controller to this ControlDevice
Definition: ControlDevice.cpp:93
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:28
armarx::ControlDevice::getJointStopMovementController
JointController * getJointStopMovementController()
Definition: ControlDevice.cpp:44