RemoteRobot.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 RobotAPI::
19  * @author (stefan dot ulbrich at kit dot edu)
20  * @date
21  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
22  * GNU General Public License
23  */
24 #pragma once
25 
26 #include <VirtualRobot/VirtualRobot.h>
27 #include <VirtualRobot/Robot.h>
28 #include <VirtualRobot/RobotNodeSet.h>
29 #include <VirtualRobot/Nodes/RobotNode.h>
30 #include <RobotAPI/interface/core/RobotState.h>
31 #include <VirtualRobot/Nodes/RobotNodeRevolute.h>
32 #include <VirtualRobot/Nodes/RobotNodePrismatic.h>
33 #include <VirtualRobot/Nodes/RobotNodeFixed.h>
34 #include <VirtualRobot/XML/RobotIO.h>
35 
36 #include <mutex>
37 
38 namespace armarx
39 {
40  // forward declaration of RemoteRobotNode
41  template<class VirtualRobotNodeType> class RemoteRobotNode;
42 
43  /** @brief RemoteRobotNodeInitializer is used to initialize the robot node for a given node type.
44  * For each robot type to be supported make a specialization of the initialize function.
45  * Currently supports: RobotNodeRevolute, RobotNodePrismatic, RobotNodeFixed. Node type specific
46  * initializations go here.
47  */
48  template<typename VirtualRobotNodeType>
50  {
51  static void initialize(RemoteRobotNode<VirtualRobotNodeType>* remoteNode);
52  };
53 
54  // specializations
55  template<>
57  template<>
59  template<>
61 
62  VirtualRobot::CollisionCheckerPtr RemoteRobotNode_getGlobalCollisionChecker();
63 
64 
65  /** @brief Mimics the behaviour of robot nodes while redirecting everything to Ice proxies.
66  * @tparam VirtualRobotNodeType Must be a descendant of VirtualRobot::RobotNode
67  * @details This class is for <b> internal use only</b> as classes cannot be referenced!
68  */
69  template<class VirtualRobotNodeType>
70  class RemoteRobotNode :
71  public VirtualRobotNodeType
72  {
73  friend struct RemoteRobotNodeInitializer<VirtualRobotNodeType>;
74 
75  public:
76  RemoteRobotNode(SharedRobotNodeInterfacePrx node, VirtualRobot::RobotPtr vr) :
77  _node(node)
78  {
79  _node->ref();
80  this->name = _node->getName();
81  this->robot = vr;
82  _node->getJointValueOffest();
83  setJointLimits(_node->getJointLimitLow(), _node->getJointLimitHigh());
84 
85  this->collisionChecker = RemoteRobotNode_getGlobalCollisionChecker();
86 
88  }
89 
90  ~RemoteRobotNode() override;
91 
92  float getJointValue() const override;
93  virtual float getJointLimitHi() const;
94  virtual float getJointLimitLo() const;
95 
97 
98  Eigen::Matrix4f getGlobalPose() const override;
99  Eigen::Matrix4f getPoseInRootFrame() const override;
100  Eigen::Vector3f getPositionInRootFrame() const override;
101  virtual bool hasChildNode(const std::string& child, bool recursive = false) const;
102 
103  std::vector<VirtualRobot::RobotNodePtr> getAllParents(VirtualRobot::RobotNodeSetPtr rns) override;
105  inline SharedRobotNodeInterfacePrx getSharedRobotNode()
106  {
107  return _node;
108  }
109 
110  virtual std::vector<std::string> getChildrenNames() const;
111 
112  protected:
113  ///////////////////////// SETUP ////////////////////////////////////
114  void setJointLimits(float lo, float hi) override;
115  //virtual void setPostJointTransformation(const Eigen::Matrix4f &trafo);
116  virtual void setLocalTransformation(const Eigen::Matrix4f& trafo);
117 
118  virtual std::string getParentName() const;
119  std::vector< VirtualRobot::SceneObjectPtr> getChildren() const override;
120 
121  void updateTransformationMatrices() override;
122  void updateTransformationMatrices(const Eigen::Matrix4f& globalPose) override;
123 
124 
125  virtual bool hasChildNode(const VirtualRobot::RobotNodePtr child, bool recursive = false) const;
126  virtual void addChildNode(VirtualRobot::RobotNodePtr child);
127  virtual bool initialize(VirtualRobot::RobotNodePtr parent, bool initializeChildren = false);
128  virtual void reset();
129  void setGlobalPose(const Eigen::Matrix4f& pose) override;
130  virtual void setJointValue(float q, bool updateTransformations = true, bool clampToLimits = true);
131 
132  SharedRobotNodeInterfacePrx _node;
133  };
134 
135  /** @brief Mimics the behaviour of the VirtualRobot::Robot class while redirecting everything to an Ice proxy.
136  * @details For a description of the members please refer to the Simox documentation (VirtualRobot::Robot).
137  * Note that not the complete interface has been made available yet. These functions are marked as protected.
138  */
140  {
141  public:
143 
144  ~RemoteRobot() override;
145 
146  VirtualRobot::RobotNodePtr getRootNode() const override;
147 
148  bool hasRobotNode(const std::string& robotNodeName) const override;
149  bool hasRobotNode(VirtualRobot::RobotNodePtr) const override;
150 
151  VirtualRobot::RobotNodePtr getRobotNode(const std::string& robotNodeName) const override;
152  void getRobotNodes(std::vector< VirtualRobot::RobotNodePtr >& storeNodes, bool clearVector = true) const override;
153 
154  bool hasRobotNodeSet(const std::string& name) const override;
155  VirtualRobot::RobotNodeSetPtr getRobotNodeSet(const std::string& nodeSetName) const override;
156  void getRobotNodeSets(std::vector<VirtualRobot::RobotNodeSetPtr>& storeNodeSet) const override;
157 
158  /**
159  *
160  * @return Global pose of the robot
161  */
162  Eigen::Matrix4f getGlobalPose() const override;
163 
164  float getScaling();
165 
166 
167 
168  /// Use this method to share the robot instance over Ice.
170 
171  std::string getName() const override;
172 
173  /*!
174  Creates a local robot that is synchronized once but not updated any more. You can use the synchronizeLocalClone method to update the joint values.
175  Note that only the kinematic structure is cloned, but the visualization files, collision models, end effectors, etc are not copied.
176  This means you can use this model for kinematic computations (e.g. coordinate transformations) but not for advanced features like collision detection.
177  In order to get a fully featured robot model you can pass a filename to VirtualRobot::Robot,
178  but then you need to make sure that the loaded model is identical to the model of the remote robot (otherwise errors will occur).
179  In the packages parameter you can pass in ArmarX packages, in which the robot file model might be in.
180  The loadMode specifies in which mode the model should be loaded. Refer to simox for more information (only matters if filename was passed in).
181  @see createLocalCloneFromFile(), synchronizeLocalClone()
182  */
183  static VirtualRobot::RobotPtr createLocalClone(RobotStateComponentInterfacePrx robotStatePrx, const std::string& filename = std::string(), const Ice::StringSeq packages = Ice::StringSeq(), VirtualRobot::RobotIO::RobotDescription loadMode = VirtualRobot::RobotIO::eFull);
184 
185  static VirtualRobot::RobotPtr createLocalClone(SharedRobotInterfacePrx sharedRobotPrx, std::string filename = std::string(), float scaling = 1.0f, const Ice::StringSeq packages = Ice::StringSeq(), VirtualRobot::RobotIO::RobotDescription loadMode = VirtualRobot::RobotIO::eFull);
186 
187  /**
188  * @brief This is a convenience function for createLocalClone, which automatically gets the filename from the RobotStateComponent,
189  * loads robot from the file and syncs it once.
190  * @param robotStatePrx Proxy to the RobotStateComponent
191  * @param loadMode The loadMode specifies in which mode the model should be loaded. Refer to simox for more information.
192  * @return new robot clone
193  * @see createLocalClone(), synchronizeLocalClone()
194  */
195  static VirtualRobot::RobotPtr createLocalCloneFromFile(RobotStateComponentInterfacePrx robotStatePrx, VirtualRobot::RobotIO::RobotDescription loadMode = VirtualRobot::RobotIO::eFull);
196 
197 
198  /*!
199  Use this method to synchronize (i.e. copy the joint values) from the remote robot to the local clone.
200  The local clone must have the identical structure as the remote robot model, otherwise an error will be reported.
201  This is the fastest way to get update a local robot.
202  */
204 
205  static bool synchronizeLocalClone(VirtualRobot::RobotPtr robot, SharedRobotInterfacePrx sharedRobotPrx);
206 
207  /**
208  * @brief Synchronizes a local robot to a robot state at timestamp.
209  * @param robot Local robot that should be updated
210  * @param robotStatePrx Proxy to the RobotStateComponent
211  * @param timestamp Timestamp to which the local robot should be sychronized. Must be in range of the state history of the RobotStateComponent.
212  * @return True if successfully synced.
213  */
215 
216  static bool synchronizeLocalCloneToState(VirtualRobot::RobotPtr robot, RobotStateConfig const& state);
217 
218  // VirtualRobot::RobotPtr getRobotPtr() { return shared_from_this();} // only for debugging
219 
220  //! Clones the structure of this remote robot to a local instance
222 
223  protected:
224 
225  /// Not implemented yet
226  bool hasEndEffector(const std::string& endEffectorName) const override;
227  /// Not implemented yet
228  VirtualRobot::EndEffectorPtr getEndEffector(const std::string& endEffectorName) const override;
229  /// Not implemented yet
230  void getEndEffectors(std::vector<VirtualRobot::EndEffectorPtr>& storeEEF) const override;
231 
232  /// Not implemented yet
233  void setRootNode(VirtualRobot::RobotNodePtr node) override;
234  /// Not implemented yet
235  void registerRobotNode(VirtualRobot::RobotNodePtr node) override;
236  /// Not implemented yet
237  void deregisterRobotNode(VirtualRobot::RobotNodePtr node) override;
238  /// Not implemented yet
239  void registerRobotNodeSet(VirtualRobot::RobotNodeSetPtr nodeSet) override;
240  /// Not implemented yet
241  void deregisterRobotNodeSet(VirtualRobot::RobotNodeSetPtr nodeSet) override;
242  /// Not implemented yet
243  void registerEndEffector(VirtualRobot::EndEffectorPtr endEffector) override;
244  /**
245  * @brief Sets the global pose of the robot.
246  * @param globalPose new global pose
247  * @param applyValues No effect. Will be always applied.
248  */
249  void setGlobalPose(const Eigen::Matrix4f& globalPose, bool applyValues = true) override;
250 
251  VirtualRobot::RobotNodePtr createLocalNode(SharedRobotNodeInterfacePrx remoteNode, std::vector<VirtualRobot::RobotNodePtr>& allNodes, std::map<VirtualRobot::RobotNodePtr, std::vector<std::string> >& childrenMap, VirtualRobot::RobotPtr robo);
252 
253  protected:
255  mutable std::map<std::string, VirtualRobot::RobotNodePtr> _cachedNodes;
256  mutable VirtualRobot::RobotNodePtr _root;
257 
258  static std::recursive_mutex m;
259 
260  static VirtualRobot::RobotNodePtr createRemoteRobotNode(SharedRobotNodeInterfacePrx, VirtualRobot::RobotPtr);
261  };
262 
263  using RemoteRobotPtr = std::shared_ptr<RemoteRobot>;
264 }
265 
armarx::RemoteRobotNode::getJointLimitLo
virtual float getJointLimitLo() const
Definition: RemoteRobotNode.cpp:113
armarx::RemoteRobotNode::reset
virtual void reset()
Definition: RemoteRobotNode.cpp:234
armarx::RemoteRobot::m
static std::recursive_mutex m
Definition: RemoteRobot.h:258
armarx::RemoteRobot::getGlobalPose
Eigen::Matrix4f getGlobalPose() const override
Definition: RemoteRobot.cpp:184
armarx::RemoteRobot::createRemoteRobotNode
static VirtualRobot::RobotNodePtr createRemoteRobotNode(SharedRobotNodeInterfacePrx, VirtualRobot::RobotPtr)
Definition: RemoteRobotNode.cpp:42
armarx::eFull
@ eFull
Definition: MorphingItem.h:35
armarx::RemoteRobot::hasRobotNode
bool hasRobotNode(const std::string &robotNodeName) const override
Definition: RemoteRobot.cpp:91
armarx::RemoteRobot::_robot
SharedRobotInterfacePrx _robot
Definition: RemoteRobot.h:254
armarx::RemoteRobot::deregisterRobotNode
void deregisterRobotNode(VirtualRobot::RobotNodePtr node) override
Not implemented yet.
Definition: RemoteRobot.cpp:543
armarx::RemoteRobot::synchronizeLocalClone
static bool synchronizeLocalClone(VirtualRobot::RobotPtr robot, RobotStateComponentInterfacePrx robotStatePrx)
Definition: RemoteRobot.cpp:448
armarx::RemoteRobotNode::hasChildNode
virtual bool hasChildNode(const std::string &child, bool recursive=false) const
Definition: RemoteRobotNode.cpp:154
armarx::RemoteRobot::createLocalCloneFromFile
static VirtualRobot::RobotPtr createLocalCloneFromFile(RobotStateComponentInterfacePrx robotStatePrx, VirtualRobot::RobotIO::RobotDescription loadMode=VirtualRobot::RobotIO::eFull)
This is a convenience function for createLocalClone, which automatically gets the filename from the R...
Definition: RemoteRobot.cpp:441
armarx::RemoteRobot::getRootNode
VirtualRobot::RobotNodePtr getRootNode() const override
Definition: RemoteRobot.cpp:81
armarx::RemoteRobotNodeInitializer::initialize
static void initialize(RemoteRobotNode< VirtualRobotNodeType > *remoteNode)
armarx::RemoteRobot::getRobotNodes
void getRobotNodes(std::vector< VirtualRobot::RobotNodePtr > &storeNodes, bool clearVector=true) const override
Definition: RemoteRobot.cpp:146
armarx::RemoteRobot::_root
VirtualRobot::RobotNodePtr _root
Definition: RemoteRobot.h:256
armarx::RemoteRobot::getEndEffectors
void getEndEffectors(std::vector< VirtualRobot::EndEffectorPtr > &storeEEF) const override
Not implemented yet.
Definition: RemoteRobot.cpp:540
armarx::RemoteRobotNode::getJointLimitHi
virtual float getJointLimitHi() const
Definition: RemoteRobotNode.cpp:108
armarx::RemoteRobotNode::setLocalTransformation
virtual void setLocalTransformation(const Eigen::Matrix4f &trafo)
Definition: RemoteRobotNode.cpp:188
armarx::RemoteRobotNode::getJointValue
float getJointValue() const override
Definition: RemoteRobotNode.cpp:102
armarx::RemoteRobotNode::getGlobalPose
Eigen::Matrix4f getGlobalPose() const override
Definition: RemoteRobotNode.cpp:130
lo
#define lo(x)
Definition: AbstractInterface.h:43
armarx::RemoteRobot::createLocalNode
VirtualRobot::RobotNodePtr createLocalNode(SharedRobotNodeInterfacePrx remoteNode, std::vector< VirtualRobot::RobotNodePtr > &allNodes, std::map< VirtualRobot::RobotNodePtr, std::vector< std::string > > &childrenMap, VirtualRobot::RobotPtr robo)
Definition: RemoteRobot.cpp:205
armarx::RemoteRobotNodeInitializer
RemoteRobotNodeInitializer is used to initialize the robot node for a given node type.
Definition: RemoteRobot.h:49
armarx::RemoteRobotNode::getChildren
std::vector< VirtualRobot::SceneObjectPtr > getChildren() const override
Definition: RemoteRobotNode.cpp:202
armarx::RemoteRobot::getScaling
float getScaling()
Definition: RemoteRobot.cpp:190
armarx::RemoteRobot::getRobotNodeSet
VirtualRobot::RobotNodeSetPtr getRobotNodeSet(const std::string &nodeSetName) const override
Definition: RemoteRobot.cpp:165
armarx::RemoteRobot::getRobotNode
VirtualRobot::RobotNodePtr getRobotNode(const std::string &robotNodeName) const override
Definition: RemoteRobot.cpp:128
armarx::RemoteRobotNode::getPoseInRootFrame
Eigen::Matrix4f getPoseInRootFrame() const override
Definition: RemoteRobotNode.cpp:135
armarx::RemoteRobot::getName
std::string getName() const override
Definition: RemoteRobot.cpp:200
armarx::RemoteRobot::hasRobotNodeSet
bool hasRobotNodeSet(const std::string &name) const override
Definition: RemoteRobot.cpp:160
armarx::RemoteRobotNode::getSharedRobotNode
SharedRobotNodeInterfacePrx getSharedRobotNode()
Definition: RemoteRobot.h:105
armarx::RemoteRobotNode::_node
SharedRobotNodeInterfacePrx _node
Definition: RemoteRobot.h:132
armarx::RemoteRobot::getRobotNodeSets
void getRobotNodeSets(std::vector< VirtualRobot::RobotNodeSetPtr > &storeNodeSet) const override
Definition: RemoteRobot.cpp:174
armarx::RemoteRobotNode::setJointLimits
void setJointLimits(float lo, float hi) override
Definition: RemoteRobotNode.cpp:160
armarx::armem::human::Robot
@ Robot
Definition: util.h:14
armarx::RemoteRobotNode::setGlobalPose
void setGlobalPose(const Eigen::Matrix4f &pose) override
Definition: RemoteRobotNode.cpp:238
armarx::RemoteRobot::registerEndEffector
void registerEndEffector(VirtualRobot::EndEffectorPtr endEffector) override
Not implemented yet.
Definition: RemoteRobot.cpp:546
armarx::VariantType::Long
const VariantTypeId Long
Definition: Variant.h:917
filename
std::string filename
Definition: VisualizationRobot.cpp:83
armarx::RemoteRobot::getSharedRobot
SharedRobotInterfacePrx getSharedRobot() const
Use this method to share the robot instance over Ice.
Definition: RemoteRobot.cpp:195
armarx::RemoteRobotNode::getAllParents
std::vector< VirtualRobot::RobotNodePtr > getAllParents(VirtualRobot::RobotNodeSetPtr rns) override
Definition: RemoteRobotNode.cpp:165
armarx::RemoteRobotNode::updateTransformationMatrices
void updateTransformationMatrices() override
Definition: RemoteRobotNode.cpp:215
armarx::RemoteRobot::deregisterRobotNodeSet
void deregisterRobotNodeSet(VirtualRobot::RobotNodeSetPtr nodeSet) override
Not implemented yet.
Definition: RemoteRobot.cpp:545
q
#define q
armarx::RemoteRobotNode_getGlobalCollisionChecker
CollisionCheckerPtr RemoteRobotNode_getGlobalCollisionChecker()
Definition: RemoteRobot.cpp:556
armarx::RemoteRobotNode::addChildNode
virtual void addChildNode(VirtualRobot::RobotNodePtr child)
Definition: RemoteRobotNode.cpp:225
armarx::RemoteRobotNode::getLocalTransformation
Eigen::Matrix4f getLocalTransformation() override
Definition: RemoteRobotNode.cpp:124
armarx::RemoteRobotPtr
std::shared_ptr< RemoteRobot > RemoteRobotPtr
Definition: RemoteRobot.h:263
armarx::RemoteRobotNode::getParent
virtual VirtualRobot::SceneObjectPtr getParent()
Definition: RemoteRobotNode.cpp:179
armarx::RemoteRobot
Mimics the behaviour of the VirtualRobot::Robot class while redirecting everything to an Ice proxy.
Definition: RemoteRobot.h:139
armarx::RemoteRobotNode
Mimics the behaviour of robot nodes while redirecting everything to Ice proxies.
Definition: RemoteRobot.h:41
GfxTL::Matrix4f
MatrixXX< 4, 4, float > Matrix4f
Definition: MatrixXX.h:601
armarx::RemoteRobot::_cachedNodes
std::map< std::string, VirtualRobot::RobotNodePtr > _cachedNodes
Definition: RemoteRobot.h:255
armarx::RemoteRobot::~RemoteRobot
~RemoteRobot() override
Definition: RemoteRobot.cpp:64
armarx::RemoteRobot::registerRobotNodeSet
void registerRobotNodeSet(VirtualRobot::RobotNodeSetPtr nodeSet) override
Not implemented yet.
Definition: RemoteRobot.cpp:544
armarx::RemoteRobot::createLocalClone
VirtualRobot::RobotPtr createLocalClone()
Clones the structure of this remote robot to a local instance.
Definition: RemoteRobot.cpp:328
armarx::RemoteRobotNode::RemoteRobotNode
RemoteRobotNode(SharedRobotNodeInterfacePrx node, VirtualRobot::RobotPtr vr)
Definition: RemoteRobot.h:76
armarx::RemoteRobot::synchronizeLocalCloneToTimestamp
static bool synchronizeLocalCloneToTimestamp(VirtualRobot::RobotPtr robot, RobotStateComponentInterfacePrx robotStatePrx, Ice::Long timestamp)
Synchronizes a local robot to a robot state at timestamp.
Definition: RemoteRobot.cpp:487
hi
#define hi(x)
Definition: AbstractInterface.h:42
IceInternal::ProxyHandle< ::IceProxy::armarx::SharedRobotInterface >
armarx::RemoteRobot::synchronizeLocalCloneToState
static bool synchronizeLocalCloneToState(VirtualRobot::RobotPtr robot, RobotStateConfig const &state)
Definition: RemoteRobot.cpp:497
scene3D::SceneObjectPtr
boost::intrusive_ptr< SceneObject > SceneObjectPtr
Definition: PointerDefinitions.h:40
armarx::RemoteRobotNode::getParentName
virtual std::string getParentName() const
Definition: RemoteRobotNode.cpp:197
armarx::RemoteRobotNode::getChildrenNames
virtual std::vector< std::string > getChildrenNames() const
Definition: RemoteRobotNode.cpp:192
armarx::RemoteRobotNode::initialize
virtual bool initialize(VirtualRobot::RobotNodePtr parent, bool initializeChildren=false)
Definition: RemoteRobotNode.cpp:229
armarx::RemoteRobotNode::~RemoteRobotNode
~RemoteRobotNode() override
Definition: RemoteRobotNode.cpp:85
armarx::RemoteRobot::getEndEffector
VirtualRobot::EndEffectorPtr getEndEffector(const std::string &endEffectorName) const override
Not implemented yet.
Definition: RemoteRobot.cpp:535
armarx::RemoteRobot::RemoteRobot
RemoteRobot(SharedRobotInterfacePrx robot)
Definition: RemoteRobot.cpp:57
armarx::RemoteRobotNode::setJointValue
virtual void setJointValue(float q, bool updateTransformations=true, bool clampToLimits=true)
Definition: RemoteRobotNode.cpp:242
armarx::RemoteRobotNode::getPositionInRootFrame
Eigen::Vector3f getPositionInRootFrame() const override
Definition: RemoteRobotNode.cpp:141
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:28
armarx::RemoteRobot::setRootNode
void setRootNode(VirtualRobot::RobotNodePtr node) override
Not implemented yet.
Definition: RemoteRobot.cpp:541
armarx::RemoteRobot::setGlobalPose
void setGlobalPose(const Eigen::Matrix4f &globalPose, bool applyValues=true) override
Sets the global pose of the robot.
Definition: RemoteRobot.cpp:548
VirtualRobot::RobotPtr
std::shared_ptr< class Robot > RobotPtr
Definition: Bus.h:18
armarx::RemoteRobot::registerRobotNode
void registerRobotNode(VirtualRobot::RobotNodePtr node) override
Not implemented yet.
Definition: RemoteRobot.cpp:542
armarx::RemoteRobot::hasEndEffector
bool hasEndEffector(const std::string &endEffectorName) const override
Not implemented yet.
Definition: RemoteRobot.cpp:530