JointSimulationDevice.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 ArmarXSimulation::ArmarXObjects::RobotUnitSimulation
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 #include "JointSimulationDevice.h"
23 
25 
26 const float armarx::JointSimulationDevice::nullFloat = 0;
27 
29  armarx::NameValueMap& ctrlpos,
30  armarx::NameValueMap& ctrlvel,
31  armarx::NameValueMap& ctrltor) :
33 {
34  jointCtrlPos.map = &ctrlpos;
35  jointCtrlVel.map = &ctrlvel;
36  jointCtrlTor.map = &ctrltor;
37  jointCtrlESt.map = &ctrlvel;
38  jointCtrlMSt.map = &ctrlvel;
39 
40  jointCtrlPos.val = &jointCtrlPos.targ.position;
41  jointCtrlVel.val = &jointCtrlVel.targ.velocity;
43  jointCtrlESt.val = &nullFloat;
44  jointCtrlMSt.val = &nullFloat;
45 
46  jointCtrlPos.sensVal = &(sensorValue.position);
47  jointCtrlVel.sensVal = &(sensorValue.velocity);
48  jointCtrlTor.sensVal = &(sensorValue.torque);
49  jointCtrlESt.sensVal = &nullFloat;
50  jointCtrlMSt.sensVal = &nullFloat;
51 
57 }
58 
59 void
61 {
62  if (rtGetActiveJointController())
63  {
64  auto curJointCtrl = dynamic_cast<JointSimControllerBase*>(rtGetActiveJointController());
65  curJointCtrl->map->erase(getDeviceName());
66  curJointCtrl->mapVal = nullptr;
67  }
69  auto newJointCtrl = dynamic_cast<JointSimControllerBase*>(jointCtrl);
70  newJointCtrl->mapVal = &(*(newJointCtrl->map))[getDeviceName()];
71  *(newJointCtrl->mapVal) = *(newJointCtrl->sensVal);
72 }
73 
74 void
76  const IceUtil::Time&)
77 {
79  << "JointBaseCtrl(" + getControlMode() + ") has null map target value";
80  ARMARX_CHECK_EXPRESSION(val) << "JointBaseCtrl(" + getControlMode() + ") has null target value";
81  *mapVal = *val;
82 }
armarx::JointSimulationDevice::rtSetActiveJointController
void rtSetActiveJointController(JointController *jointCtrl) override
Activates the given JointController for this device.
Definition: JointSimulationDevice.cpp:60
armarx::JointSimulationDevice::jointCtrlTor
JointSimController< ControlTarget1DoFActuatorTorque > jointCtrlTor
Definition: JointSimulationDevice.h:83
armarx::SensorDeviceTemplate
Definition: SensorDevice.h:111
armarx::JointSimulationDevice::JointSimControllerBase::mapVal
float * mapVal
Definition: JointSimulationDevice.h:48
armarx::ControlDevice
The ControlDevice class represents a logical unit accepting commands via its JointControllers.
Definition: ControlDevice.h:63
armarx::JointSimulationDevice::jointCtrlESt
JointSimController< DummyControlTargetEmergencyStop > jointCtrlESt
Definition: JointSimulationDevice.h:84
armarx::JointSimulationDevice::JointSimController::targ
TargT targ
Definition: JointSimulationDevice.h:59
armarx::JointController
The JointController class represents one joint in one control mode.
Definition: JointController.h:51
armarx::DeviceBase
Definition: DeviceBase.h:29
armarx::JointSimulationDevice::jointCtrlVel
JointSimController< ControlTarget1DoFActuatorVelocity > jointCtrlVel
Definition: JointSimulationDevice.h:82
armarx::JointSimulationDevice::jointCtrlPos
JointSimController< ControlTarget1DoFActuatorPosition > jointCtrlPos
Definition: JointSimulationDevice.h:81
armarx::JointSimulationDevice::JointSimControllerBase::sensVal
const float * sensVal
Definition: JointSimulationDevice.h:47
armarx::ControlDevice::rtSetActiveJointController
virtual void rtSetActiveJointController(JointController *jointCtrl)
Activates the given JointController for this device.
Definition: ControlDevice.cpp:56
armarx::JointSimulationDevice::jointCtrlMSt
JointSimController< DummyControlTargetStopMovement > jointCtrlMSt
Definition: JointSimulationDevice.h:85
armarx::SensorDevice
This class represents some part of the robot providing sensor values.
Definition: SensorDevice.h:59
armarx::JointSimulationDevice::JointSimControllerBase::map
NameValueMap * map
Definition: JointSimulationDevice.h:45
armarx::JointSimulationDevice::JointSimulationDevice
JointSimulationDevice(const std::string &name, NameValueMap &ctrlpos, NameValueMap &ctrlvel, NameValueMap &ctrltor)
Definition: JointSimulationDevice.cpp:28
armarx::JointSimulationDevice::JointSimControllerBase
Definition: JointSimulationDevice.h:43
armarx::armem::Time
armarx::core::time::DateTime Time
Definition: forward_declarations.h:13
armarx::JointSimulationDevice::JointSimControllerBase::rtRun
void rtRun(const IceUtil::Time &, const IceUtil::Time &) override
called when this JointController is run
Definition: JointSimulationDevice.cpp:75
ExpressionException.h
armarx::control::njoint_controller::platform::platform_follower_controller::NameValueMap
std::map< std::string, float > NameValueMap
Definition: PlatformFollowerController.h:88
armarx::SensorDeviceTemplate< SensorValue1DoFActuator >::sensorValue
SensorValue1DoFActuator sensorValue
Definition: SensorDevice.h:123
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
JointSimulationDevice.h
armarx::JointSimulationDevice::JointSimControllerBase::val
const float * val
Definition: JointSimulationDevice.h:46
armarx::ControlDevice::addJointController
void addJointController(JointController *jointCtrl)
adds the Joint controller to this ControlDevice
Definition: ControlDevice.cpp:93