RobotStateComponentPlugin.h
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::ArmarXObjects::RobotAPIComponentPlugins
17  * @author Raphael Grimm ( raphael dot grimm at kit dot edu )
18  * @date 2019
19  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
20  * GNU General Public License
21  */
22 
23 #pragma once
24 
25 #include <mutex>
26 
27 #include <VirtualRobot/Robot.h>
28 #include <VirtualRobot/XML/RobotIO.h>
29 
31 
33 #include <RobotAPI/interface/core/RobotState.h>
35 
36 
37 namespace armarx::plugins
38 {
39  /**
40  * @defgroup Library-RobotAPIComponentPlugins RobotAPIComponentPlugins
41  * @ingroup RobotAPI
42  * A description of the library RobotAPIComponentPlugins.
43  */
44 
45  /**
46  * @class RobotStateComponentPlugin
47  * @ingroup Library-RobotAPIComponentPlugins
48  * @brief Brief description of class RobotAPIComponentPlugins.
49  *
50  * Detailed description of class RobotAPIComponentPlugins.
51  */
53  {
54  public:
55  struct RobotData;
56  using ComponentPlugin::ComponentPlugin;
57 
58  void setRobotStateComponentName(const std::string& name);
60  const std::string& getRobotStateComponentName() const;
61  void deactivate();
62  //get / add
63  public:
64  bool hasRobot(const std::string& id) const;
65 
67  const std::string& id,
68  const VirtualRobot::RobotPtr& robot,
69  const VirtualRobot::RobotNodeSetPtr& rns = {},
70  const VirtualRobot::RobotNodePtr& node = {});
72  const std::string& id,
73  const VirtualRobot::RobotPtr& robot,
74  const std::string& rnsName,
75  const std::string& nodeName = "");
77  const std::string& id,
78  VirtualRobot::RobotIO::RobotDescription loadMode,
79  const std::string& rnsName = "",
80  const std::string& nodeName = "");
82  const std::string& id,
83  const std::string& filename,
84  const Ice::StringSeq packages,
85  VirtualRobot::RobotIO::RobotDescription loadMode,
86  const std::string& rnsName = "",
87  const std::string& nodeName = "");
88 
89  VirtualRobot::RobotPtr getRobot(const std::string& id) const;
90  RobotData getRobotData(const std::string& id) const;
91  void setRobotRNSAndNode(const std::string& id, const std::string& rnsName, const std::string& nodeName);
92 
93  //querry
94  public:
97  Eigen::Matrix4f transformFromTo(const std::string& from,
98  const std::string& to,
99  const VirtualRobot::RobotPtr& rob);
100 
101  //sync
102  public:
103  bool synchronizeLocalClone(const VirtualRobot::RobotPtr& robot) const;
104  bool synchronizeLocalClone(const VirtualRobot::RobotPtr& robot, Ice::Long timestamp) const;
105  bool synchronizeLocalClone(const VirtualRobot::RobotPtr& robot, const RobotStateConfig& state) const;
106 
107  bool synchronizeLocalClone(const RobotData& rdata) const;
108  bool synchronizeLocalClone(const RobotData& rdata, Ice::Long timestamp) const;
109  bool synchronizeLocalClone(const RobotData& rdata, const RobotStateConfig& state) const;
110 
111  bool synchronizeLocalClone(const std::string& id) const;
112  bool synchronizeLocalClone(const std::string& id, Ice::Long timestamp) const;
113  bool synchronizeLocalClone(const std::string& id, const RobotStateConfig& state) const;
114 
115  //diffik
116  public:
118  const std::string& id,
119  const Eigen::Matrix4f& targetPose,
120  const SimpleDiffIK::Parameters& params = {}) const;
122  const std::string& id,
123  const std::vector<Eigen::Matrix4f>& targets,
124  const Eigen::VectorXf& initialJV,
125  const SimpleDiffIK::Parameters& params = {}) const;
126 
127  //hooks
128  protected:
129  void preOnInitComponent() override;
130  void preOnConnectComponent() override;
131  void postOnDisconnectComponent() override;
132  void postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties) override;
133 
134  //structs
135  public:
136  struct RobotData
137  {
139  VirtualRobot::RobotNodeSetPtr rns;
140  VirtualRobot::RobotNodePtr node;
141 
143  const Eigen::Matrix4f& targetPose,
144  const SimpleDiffIK::Parameters& params = {}) const;
146  const std::vector<Eigen::Matrix4f>& targets,
147  const Eigen::VectorXf& initialJV,
148  const SimpleDiffIK::Parameters& params = {}) const;
149  };
150  //data
151  private:
152  static constexpr auto _propertyName = "RemoteStateComponentName";
153  std::string _robotStateComponentName;
154  RobotStateComponentInterfacePrx _robotStateComponent;
155  mutable std::recursive_mutex _robotsMutex;
156  std::map<std::string, RobotData> _robots;
157  RobotNameHelperPtr _nameHelper;
158  bool _deactivated = false;
159  };
160 }
161 
162 
164 
165 namespace armarx
166 {
168  {
169  public:
171  private:
172  RobotStateComponentPlugin* _robotStateComponentPlugin{nullptr};
173  public:
175 
178 
180 
182  //get / add
183  public:
184  bool hasRobot(const std::string& id) const;
185 
187  const std::string& id,
188  const VirtualRobot::RobotPtr& robot,
189  const VirtualRobot::RobotNodeSetPtr& rns = {},
190  const VirtualRobot::RobotNodePtr& node = {});
192  const std::string& id,
193  const VirtualRobot::RobotPtr& robot,
194  const std::string& rnsName,
195  const std::string& nodeName = "");
197  const std::string& id,
198  VirtualRobot::RobotIO::RobotDescription loadMode,
199  const std::string& rnsName = "",
200  const std::string& nodeName = "");
202  const std::string& id,
203  const std::string& filename,
204  const Ice::StringSeq packages,
205  VirtualRobot::RobotIO::RobotDescription loadMode,
206  const std::string& rnsName = "",
207  const std::string& nodeName = "");
208 
209  template<class...Ts>
210  VirtualRobot::RobotPtr addOrGetRobot(const std::string& id, Ts&& ...ts)
211  {
212  if (hasRobot(id))
213  {
214  return getRobot(id);
215  }
216  return addRobot(id, std::forward<Ts>(ts)...);
217  }
218 
219  VirtualRobot::RobotPtr getRobot(const std::string& id) const;
220  RobotStateComponentPlugin::RobotData getRobotData(const std::string& id) const;
221  void setRobotRNSAndNode(const std::string& id, const std::string& rnsName, const std::string& nodeName);
222 
224 
225  //sync
226  public:
227  bool synchronizeLocalClone(const VirtualRobot::RobotPtr& robot) const;
228  bool synchronizeLocalClone(const VirtualRobot::RobotPtr& robot, Ice::Long timestamp) const;
229  bool synchronizeLocalClone(const VirtualRobot::RobotPtr& robot, const RobotStateConfig& state) const;
230 
231  bool synchronizeLocalClone(const RobotStateComponentPlugin::RobotData& rdata) const;
232  bool synchronizeLocalClone(const RobotStateComponentPlugin::RobotData& rdata, Ice::Long timestamp) const;
233  bool synchronizeLocalClone(const RobotStateComponentPlugin::RobotData& rdata, const RobotStateConfig& state) const;
234 
235  bool synchronizeLocalClone(const std::string& id) const;
236  bool synchronizeLocalClone(const std::string& id, Ice::Long timestamp) const;
237  bool synchronizeLocalClone(const std::string& id, const RobotStateConfig& state) const;
238 
239  //diffik
240  public:
242  const std::string& id,
243  const Eigen::Matrix4f& targetPose,
244  const SimpleDiffIK::Parameters& params = {});
245  SimpleDiffIK::Reachability calculateRobotReachability(
246  const std::string& id,
247  const std::vector<Eigen::Matrix4f> targets,
248  const Eigen::VectorXf& initialJV,
249  const SimpleDiffIK::Parameters& params = {});
250  };
251 }
armarx::RobotStateComponentPluginUser::calculateRobotDiffIK
SimpleDiffIK::Result calculateRobotDiffIK(const std::string &id, const Eigen::Matrix4f &targetPose, const SimpleDiffIK::Parameters &params={})
Definition: RobotStateComponentPlugin.cpp:416
armarx::plugins::RobotStateComponentPlugin::RobotData::robot
VirtualRobot::RobotPtr robot
Definition: RobotStateComponentPlugin.h:138
armarx::SimpleDiffIK::Parameters
Definition: SimpleDiffIK.h:40
armarx::RobotStateComponentPluginUser::getRobotNameHelper
RobotNameHelperPtr getRobotNameHelper() const
Definition: RobotStateComponentPlugin.cpp:321
armarx::SimpleDiffIK::Reachability
Definition: SimpleDiffIK.h:81
armarx::RobotStateComponentPluginUser::getRobotData
RobotStateComponentPlugin::RobotData getRobotData(const std::string &id) const
Definition: RobotStateComponentPlugin.cpp:356
armarx::plugins::RobotStateComponentPlugin::postOnDisconnectComponent
void postOnDisconnectComponent() override
Definition: RobotStateComponentPlugin.cpp:242
armarx::plugins::RobotStateComponentPlugin::RobotData::calculateRobotDiffIK
SimpleDiffIK::Result calculateRobotDiffIK(const Eigen::Matrix4f &targetPose, const SimpleDiffIK::Parameters &params={}) const
Definition: RobotStateComponentPlugin.cpp:258
armarx::plugins::RobotStateComponentPlugin::getRobotStateComponentName
const std::string & getRobotStateComponentName() const
Definition: RobotStateComponentPlugin.cpp:279
armarx::RobotStateComponentPluginUser::setRobotRNSAndNode
void setRobotRNSAndNode(const std::string &id, const std::string &rnsName, const std::string &nodeName)
Definition: RobotStateComponentPlugin.cpp:361
armarx::RobotStateComponentPluginUser::getRobotStateComponent
const RobotStateComponentInterfacePrx & getRobotStateComponent() const
Definition: RobotStateComponentPlugin.cpp:317
armarx::plugins::RobotStateComponentPlugin::setRobotStateComponentName
void setRobotStateComponentName(const std::string &name)
Definition: RobotStateComponentPlugin.cpp:272
armarx::plugins::RobotStateComponentPlugin::setRobotRNSAndNode
void setRobotRNSAndNode(const std::string &id, const std::string &rnsName, const std::string &nodeName)
Definition: RobotStateComponentPlugin.cpp:125
armarx::RobotStateComponentPluginUser::addRobot
VirtualRobot::RobotPtr addRobot(const std::string &id, const VirtualRobot::RobotPtr &robot, const VirtualRobot::RobotNodeSetPtr &rns={}, const VirtualRobot::RobotNodePtr &node={})
Definition: RobotStateComponentPlugin.cpp:331
armarx::RobotStateComponentPluginUser::synchronizeLocalClone
bool synchronizeLocalClone(const VirtualRobot::RobotPtr &robot) const
Definition: RobotStateComponentPlugin.cpp:371
armarx::plugins::RobotStateComponentPlugin::setRobotStateComponent
void setRobotStateComponent(const RobotStateComponentInterfacePrx &rsc)
Definition: RobotStateComponentPlugin.cpp:38
armarx::plugins::RobotStateComponentPlugin::RobotData::node
VirtualRobot::RobotNodePtr node
Definition: RobotStateComponentPlugin.h:140
armarx::plugins::RobotStateComponentPlugin::RobotData::rns
VirtualRobot::RobotNodeSetPtr rns
Definition: RobotStateComponentPlugin.h:139
armarx::plugins::RobotStateComponentPlugin::hasRobot
bool hasRobot(const std::string &id) const
Definition: RobotStateComponentPlugin.cpp:45
armarx::plugins::RobotStateComponentPlugin::synchronizeLocalClone
bool synchronizeLocalClone(const VirtualRobot::RobotPtr &robot) const
Definition: RobotStateComponentPlugin.cpp:155
armarx::plugins::RobotStateComponentPlugin::getRobotData
RobotData getRobotData(const std::string &id) const
Definition: RobotStateComponentPlugin.cpp:118
armarx::plugins::RobotStateComponentPlugin::getRobotNameHelper
RobotNameHelperPtr getRobotNameHelper() const
Definition: RobotStateComponentPlugin.cpp:33
ManagedIceObject.h
armarx::plugins::RobotStateComponentPlugin::transformFromTo
Eigen::Matrix4f transformFromTo(const std::string &from, const std::string &to, const VirtualRobot::RobotPtr &rob)
Definition: RobotStateComponentPlugin.cpp:289
armarx::RobotNameHelperPtr
std::shared_ptr< class RobotNameHelper > RobotNameHelperPtr
Definition: RobotNameHelper.h:34
armarx::plugins::RobotStateComponentPlugin::getRobot
VirtualRobot::RobotPtr getRobot(const std::string &id) const
Definition: RobotStateComponentPlugin.cpp:113
armarx::plugins
This file is part of ArmarX.
Definition: DebugObserverComponentPlugin.cpp:28
armarx::ComponentPlugin
Definition: ComponentPlugin.h:38
armarx::plugins::RobotStateComponentPlugin::calculateRobotReachability
SimpleDiffIK::Reachability calculateRobotReachability(const std::string &id, const std::vector< Eigen::Matrix4f > &targets, const Eigen::VectorXf &initialJV, const SimpleDiffIK::Parameters &params={}) const
Definition: RobotStateComponentPlugin.cpp:208
armarx::RobotStateComponentPluginUser::hasRobot
bool hasRobot(const std::string &id) const
Definition: RobotStateComponentPlugin.cpp:326
armarx::RobotStateComponentPluginUser::getRobotStateComponentPlugin
const RobotStateComponentPlugin & getRobotStateComponentPlugin() const
Definition: RobotStateComponentPlugin.cpp:307
armarx::plugins::RobotStateComponentPlugin::postCreatePropertyDefinitions
void postCreatePropertyDefinitions(PropertyDefinitionsPtr &properties) override
Definition: RobotStateComponentPlugin.cpp:247
armarx::RobotStateComponentPluginUser::RobotStateComponentPluginUser
RobotStateComponentPluginUser()
Definition: RobotStateComponentPlugin.cpp:303
armarx::VariantType::Long
const VariantTypeId Long
Definition: Variant.h:917
SimpleDiffIK.h
armarx::plugins::RobotStateComponentPlugin::RobotData
Definition: RobotStateComponentPlugin.h:136
filename
std::string filename
Definition: VisualizationRobot.cpp:83
armarx::plugins::RobotStateComponentPlugin::addRobot
VirtualRobot::RobotPtr addRobot(const std::string &id, const VirtualRobot::RobotPtr &robot, const VirtualRobot::RobotNodeSetPtr &rns={}, const VirtualRobot::RobotNodePtr &node={})
Definition: RobotStateComponentPlugin.cpp:51
armarx::RobotStateComponentPluginUser::RobotStateComponentPlugin
armarx::plugins::RobotStateComponentPlugin RobotStateComponentPlugin
Definition: RobotStateComponentPlugin.h:170
armarx::RobotStateComponentPluginUser::calculateRobotReachability
SimpleDiffIK::Reachability calculateRobotReachability(const std::string &id, const std::vector< Eigen::Matrix4f > targets, const Eigen::VectorXf &initialJV, const SimpleDiffIK::Parameters &params={})
Definition: RobotStateComponentPlugin.cpp:421
armarx::plugins::RobotStateComponentPlugin::getRobotStateComponent
const RobotStateComponentInterfacePrx & getRobotStateComponent() const
Definition: RobotStateComponentPlugin.cpp:150
armarx::ManagedIceObject
The ManagedIceObject is the base class for all ArmarX objects.
Definition: ManagedIceObject.h:163
GfxTL::Matrix4f
MatrixXX< 4, 4, float > Matrix4f
Definition: MatrixXX.h:601
armarx::SimpleDiffIK::Result
Definition: SimpleDiffIK.h:67
armarx::RobotStateComponentPluginUser::addOrGetRobot
VirtualRobot::RobotPtr addOrGetRobot(const std::string &id, Ts &&...ts)
Definition: RobotStateComponentPlugin.h:210
IceUtil::Handle< class PropertyDefinitionContainer >
armarx::plugins::RobotStateComponentPlugin::preOnInitComponent
void preOnInitComponent() override
Definition: RobotStateComponentPlugin.cpp:217
IceInternal::ProxyHandle<::IceProxy::armarx::RobotStateComponentInterface >
armarx::plugins::RobotStateComponentPlugin::RobotData::calculateRobotReachability
SimpleDiffIK::Reachability calculateRobotReachability(const std::vector< Eigen::Matrix4f > &targets, const Eigen::VectorXf &initialJV, const SimpleDiffIK::Parameters &params={}) const
Definition: RobotStateComponentPlugin.cpp:265
armarx::plugins::RobotStateComponentPlugin::deactivate
void deactivate()
Definition: RobotStateComponentPlugin.cpp:284
armarx::plugins::RobotStateComponentPlugin
Brief description of class RobotAPIComponentPlugins.
Definition: RobotStateComponentPlugin.h:52
armarx::RobotStateComponentPluginUser::getRobot
VirtualRobot::RobotPtr getRobot(const std::string &id) const
Definition: RobotStateComponentPlugin.cpp:351
ComponentPlugin.h
armarx::plugins::RobotStateComponentPlugin::preOnConnectComponent
void preOnConnectComponent() override
Definition: RobotStateComponentPlugin.cpp:230
armarx::plugins::RobotStateComponentPlugin::calculateRobotDiffIK
SimpleDiffIK::Result calculateRobotDiffIK(const std::string &id, const Eigen::Matrix4f &targetPose, const SimpleDiffIK::Parameters &params={}) const
Definition: RobotStateComponentPlugin.cpp:200
armarx::RobotStateComponentPluginUser
Definition: RobotStateComponentPlugin.h:167
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:28
RobotNameHelper.h
VirtualRobot::RobotPtr
std::shared_ptr< class Robot > RobotPtr
Definition: Bus.h:18