KinematicUnitSimulation.h
Go to the documentation of this file.
1 /*
2  * This file is part of ArmarX.
3  *
4  * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
5  *
6  * ArmarX is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License version 2 as
8  * published by the Free Software Foundation.
9  *
10  * ArmarX is distributed in the hope that it will be useful, but
11  * WITHOUT ANY WARRANTY; without even the implied warranty of
12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13  * GNU General Public License for more details.
14  *
15  * You should have received a copy of the GNU General Public License
16  * along with this program. If not, see <http://www.gnu.org/licenses/>.
17  *
18  * @package ArmarXCore::units
19  * @author Christian Boege (boege dot at kit dot edu)
20  * @date 2011
21  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
22  * GNU General Public License
23  */
24 
25 #pragma once
26 
27 #include "KinematicUnit.h"
28 
32 
33 #include <IceUtil/Time.h>
34 
35 #include <random>
36 #include <mutex>
37 
38 namespace armarx
39 {
40  /**
41  * \class KinematicUnitSimulationJointState.
42  * \brief State of a joint.
43  * \ingroup RobotAPI-SensorActorUnits-util
44  *
45  * Includes the control mode, the joint angle, velocity, torque, and motor temperature.
46  * Controlmode defaults to ePositionControl, all other values default to zero.
47  */
49  {
50  public:
52  {
53  init();
54  }
55 
56  void init()
57  {
58  controlMode = eDisabled;
59  angle = 0.0f;
60  velocity = 0.0f;
61  velocityActual = 0.0f;
62  torque = 0.0f;
63  temperature = 0.0f;
64  }
65 
66  ControlMode controlMode;
67  float angle;
68  float velocity;
70  float torque;
71  float temperature;
72  };
73  using KinematicUnitSimulationJointStates = std::map<std::string, KinematicUnitSimulationJointState>;
74 
75  /**
76  * \class JointInfo.
77  * \brief Additional information about a joint.
78  * \ingroup RobotAPI-SensorActorUnits-util
79  *
80  * Joint info including lower and upper joint limits (rad).
81  * No limits are set by default (lo = hi).
82  */
84  {
85  public:
87  {
88  reset();
89  }
90 
91  void reset()
92  {
93  limitLo = 0.0f;
94  limitHi = 0.0f;
95  }
96  bool hasLimits() const
97  {
98  return (limitLo != limitHi);
99  }
100  bool continuousJoint() const
101  {
102  return !hasLimits() || limitLo - limitHi >= 360.0f;
103  }
104 
105  float limitLo;
106  float limitHi;
107  };
108  using KinematicUnitSimulationJointInfos = std::map<std::string, KinematicUnitSimulationJointInfo>;
109 
110  /**
111  * \class KinematicUnitSimulationPropertyDefinitions
112  * \brief
113  */
115  {
116  public:
118  };
119 
120  /**
121  * \class KinematicUnitSimulation
122  * \brief Simulates robot kinematics
123  * \ingroup RobotAPI-SensorActorUnits-simulation
124  */
126  virtual public KinematicUnit
127  {
128  public:
129  // inherited from Component
130  std::string getDefaultName() const override
131  {
132  return "KinematicUnitSimulation";
133  }
134 
135  void onInitKinematicUnit() override;
136  void onStartKinematicUnit() override;
137  void onExitKinematicUnit() override;
138 
139  void simulationFunction();
140 
141  // proxy implementation
142  void requestJoints(const Ice::StringSeq& joints, const Ice::Current& c = Ice::emptyCurrent) override;
143  void releaseJoints(const Ice::StringSeq& joints, const Ice::Current& c = Ice::emptyCurrent) override;
144  void switchControlMode(const NameControlModeMap& targetJointModes, const Ice::Current& c = Ice::emptyCurrent) override;
145  void setJointAngles(const NameValueMap& targetJointAngles, const Ice::Current& c = Ice::emptyCurrent) override;
146  void setJointVelocities(const NameValueMap& targetJointVelocities, const Ice::Current& c = Ice::emptyCurrent) override;
147  void setJointTorques(const NameValueMap& targetJointTorques, const Ice::Current& c = Ice::emptyCurrent) override;
148 
149  NameControlModeMap getControlModes(const Ice::Current& c = Ice::emptyCurrent) override;
150 
151  /// @warning Not implemented yet!
152  void setJointAccelerations(const NameValueMap& targetJointAccelerations, const Ice::Current& c = Ice::emptyCurrent) override;
153 
154  /// @warning Not implemented yet!
155  void setJointDecelerations(const NameValueMap& targetJointDecelerations, const Ice::Current& c = Ice::emptyCurrent) override;
156 
157  void stop(const Ice::Current& c = Ice::emptyCurrent) override;
158 
159  armarx::NameValueMap getJointAngles(const Ice::Current& c) const override;
160  armarx::NameValueMap getJointVelocities(const Ice::Current& c) const override;
161  Ice::StringSeq getJoints(const Ice::Current& c) const override;
162 
163  DebugInfo getDebugInfo(const Ice::Current& c = Ice::emptyCurrent) const override;
164 
165  /// @see PropertyUser::createPropertyDefinitions()
167 
168  protected:
172  mutable std::mutex jointStatesMutex;
174  float noise;
176  std::normal_distribution<double> distribution;
177  std::default_random_engine generator;
179  std::map<std::string, PIDControllerPtr> positionControlPIDController;
181 
182  mutable std::mutex sensorDataMutex;
185  };
186 }
armarx::KinematicUnitSimulationJointState::velocityActual
float velocityActual
Definition: KinematicUnitSimulation.h:69
armarx::KinematicUnitSimulation::switchControlMode
void switchControlMode(const NameControlModeMap &targetJointModes, const Ice::Current &c=Ice::emptyCurrent) override
Definition: KinematicUnitSimulation.cpp:247
armarx::KinematicUnitSimulationJointInfo::KinematicUnitSimulationJointInfo
KinematicUnitSimulationJointInfo()
Definition: KinematicUnitSimulation.h:86
armarx::KinematicUnitSimulationJointState::velocity
float velocity
Definition: KinematicUnitSimulation.h:68
armarx::KinematicUnitSimulation::requestJoints
void requestJoints(const Ice::StringSeq &joints, const Ice::Current &c=Ice::emptyCurrent) override
Definition: KinematicUnitSimulation.cpp:502
armarx::KinematicUnitSimulation::setJointDecelerations
void setJointDecelerations(const NameValueMap &targetJointDecelerations, const Ice::Current &c=Ice::emptyCurrent) override
Definition: KinematicUnitSimulation.cpp:433
armarx::KinematicUnitSimulationJointState::torque
float torque
Definition: KinematicUnitSimulation.h:70
armarx::KinematicUnit
The KinematicUnit class.
Definition: KinematicUnit.h:76
armarx::KinematicUnitSimulationJointState::temperature
float temperature
Definition: KinematicUnitSimulation.h:71
armarx::KinematicUnitSimulation::getJoints
Ice::StringSeq getJoints(const Ice::Current &c) const override
Definition: KinematicUnitSimulation.cpp:470
armarx::KinematicUnitSimulation::getDefaultName
std::string getDefaultName() const override
Retrieve default name of component.
Definition: KinematicUnitSimulation.h:130
armarx::KinematicUnitSimulation::createPropertyDefinitions
PropertyDefinitionsPtr createPropertyDefinitions() override
Definition: KinematicUnitSimulation.cpp:452
armarx::PropertyDefinitionContainer::prefix
std::string prefix
Prefix of the properties such as namespace, domain, component name, etc.
Definition: PropertyDefinitionContainer.h:333
PeriodicTask.h
armarx::KinematicUnitSimulation::releaseJoints
void releaseJoints(const Ice::StringSeq &joints, const Ice::Current &c=Ice::emptyCurrent) override
Definition: KinematicUnitSimulation.cpp:513
c
constexpr T c
Definition: UnscentedKalmanFilterTest.cpp:43
armarx::KinematicUnitSimulation
Simulates robot kinematics.
Definition: KinematicUnitSimulation.h:125
armarx::navigation::platform_controller::platform_global_trajectory::NameValueMap
std::map< std::string, float > NameValueMap
Definition: PlatformGlobalTrajectoryController.h:93
KinematicUnit.h
armarx::KinematicUnitSimulation::setJointAccelerations
void setJointAccelerations(const NameValueMap &targetJointAccelerations, const Ice::Current &c=Ice::emptyCurrent) override
Definition: KinematicUnitSimulation.cpp:428
armarx::KinematicUnitSimulation::getDebugInfo
DebugInfo getDebugInfo(const Ice::Current &c=Ice::emptyCurrent) const override
Definition: KinematicUnitSimulation.cpp:476
armarx::KinematicUnitSimulation::stop
void stop(const Ice::Current &c=Ice::emptyCurrent) override
Definition: KinematicUnitSimulation.cpp:438
armarx::KinematicUnitSimulation::sensorAngles
NameValueMap sensorAngles
Definition: KinematicUnitSimulation.h:183
armarx::KinematicUnitSimulation::jointStatesMutex
std::mutex jointStatesMutex
Definition: KinematicUnitSimulation.h:172
armarx::KinematicUnitSimulationJointInfo::limitHi
float limitHi
Definition: KinematicUnitSimulation.h:106
armarx::KinematicUnitSimulation::onStartKinematicUnit
void onStartKinematicUnit() override
Definition: KinematicUnitSimulation.cpp:107
armarx::KinematicUnitSimulationJointInfos
std::map< std::string, KinematicUnitSimulationJointInfo > KinematicUnitSimulationJointInfos
Definition: KinematicUnitSimulation.h:108
armarx::KinematicUnitSimulationJointInfo::limitLo
float limitLo
Definition: KinematicUnitSimulation.h:105
armarx::KinematicUnitSimulation::setJointTorques
void setJointTorques(const NameValueMap &targetJointTorques, const Ice::Current &c=Ice::emptyCurrent) override
Definition: KinematicUnitSimulation.cpp:383
armarx::KinematicUnitSimulationJointState::angle
float angle
Definition: KinematicUnitSimulation.h:67
armarx::KinematicUnitSimulationJointState::KinematicUnitSimulationJointState
KinematicUnitSimulationJointState()
Definition: KinematicUnitSimulation.h:51
armarx::KinematicUnitSimulation::simulationFunction
void simulationFunction()
Definition: KinematicUnitSimulation.cpp:120
armarx::KinematicUnitSimulation::getControlModes
NameControlModeMap getControlModes(const Ice::Current &c=Ice::emptyCurrent) override
Definition: KinematicUnitSimulation.cpp:416
armarx::KinematicUnitSimulation::sensorDataMutex
std::mutex sensorDataMutex
Definition: KinematicUnitSimulation.h:182
armarx::KinematicUnitSimulationJointStates
std::map< std::string, KinematicUnitSimulationJointState > KinematicUnitSimulationJointStates
Definition: KinematicUnitSimulation.h:73
armarx::KinematicUnitSimulation::usePDControllerForPosMode
bool usePDControllerForPosMode
Definition: KinematicUnitSimulation.h:178
armarx::KinematicUnitSimulationJointState
State of a joint.
Definition: KinematicUnitSimulation.h:48
armarx::KinematicUnitPropertyDefinitions
Definition: KinematicUnit.h:44
armarx::KinematicUnitSimulation::simulationTask
PeriodicTask< KinematicUnitSimulation >::pointer_type simulationTask
Definition: KinematicUnitSimulation.h:180
armarx::KinematicUnitSimulation::setJointVelocities
void setJointVelocities(const NameValueMap &targetJointVelocities, const Ice::Current &c=Ice::emptyCurrent) override
Definition: KinematicUnitSimulation.cpp:350
armarx::KinematicUnitSimulation::intervalMs
int intervalMs
Definition: KinematicUnitSimulation.h:175
armarx::armem::Time
armarx::core::time::DateTime Time
Definition: forward_declarations.h:13
armarx::KinematicUnitSimulationJointState::init
void init()
Definition: KinematicUnitSimulation.h:56
armarx::KinematicUnitSimulation::sensorVelocities
NameValueMap sensorVelocities
Definition: KinematicUnitSimulation.h:184
PIDController.h
armarx::KinematicUnitSimulationPropertyDefinitions::KinematicUnitSimulationPropertyDefinitions
KinematicUnitSimulationPropertyDefinitions(std::string prefix)
Definition: KinematicUnitSimulation.cpp:44
armarx::KinematicUnitSimulation::onInitKinematicUnit
void onInitKinematicUnit() override
Definition: KinematicUnitSimulation.cpp:57
armarx::KinematicUnitSimulation::distribution
std::normal_distribution< double > distribution
Definition: KinematicUnitSimulation.h:176
armarx::KinematicUnitSimulation::getJointAngles
armarx::NameValueMap getJointAngles(const Ice::Current &c) const override
Definition: KinematicUnitSimulation.cpp:458
armarx::KinematicUnitSimulationJointInfo::reset
void reset()
Definition: KinematicUnitSimulation.h:91
IceUtil::Handle
Definition: forward_declarations.h:29
armarx::KinematicUnitSimulationJointInfo::continuousJoint
bool continuousJoint() const
Definition: KinematicUnitSimulation.h:100
armarx::KinematicUnitSimulationJointState::controlMode
ControlMode controlMode
Definition: KinematicUnitSimulation.h:66
armarx::KinematicUnitSimulation::noise
float noise
Definition: KinematicUnitSimulation.h:174
armarx::KinematicUnitSimulationPropertyDefinitions
Definition: KinematicUnitSimulation.h:114
armarx::KinematicUnitSimulation::jointStates
KinematicUnitSimulationJointStates jointStates
Definition: KinematicUnitSimulation.h:169
armarx::KinematicUnitSimulation::onExitKinematicUnit
void onExitKinematicUnit() override
Definition: KinematicUnitSimulation.cpp:114
armarx::KinematicUnitSimulation::jointInfos
KinematicUnitSimulationJointInfos jointInfos
Definition: KinematicUnitSimulation.h:171
armarx::KinematicUnitSimulation::lastExecutionTime
IceUtil::Time lastExecutionTime
Definition: KinematicUnitSimulation.h:173
armarx::KinematicUnitSimulation::generator
std::default_random_engine generator
Definition: KinematicUnitSimulation.h:177
armarx::KinematicUnitSimulationJointInfo
Definition: KinematicUnitSimulation.h:83
armarx::KinematicUnitSimulation::positionControlPIDController
std::map< std::string, PIDControllerPtr > positionControlPIDController
Definition: KinematicUnitSimulation.h:179
armarx::KinematicUnitSimulationJointInfo::hasLimits
bool hasLimits() const
Definition: KinematicUnitSimulation.h:96
armarx::KinematicUnitSimulation::getJointVelocities
armarx::NameValueMap getJointVelocities(const Ice::Current &c) const override
Definition: KinematicUnitSimulation.cpp:464
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:28
armarx::KinematicUnitSimulation::setJointAngles
void setJointAngles(const NameValueMap &targetJointAngles, const Ice::Current &c=Ice::emptyCurrent) override
Definition: KinematicUnitSimulation.cpp:284
armarx::KinematicUnitSimulation::previousJointStates
KinematicUnitSimulationJointStates previousJointStates
Definition: KinematicUnitSimulation.h:170
ImportExportComponent.h