SimoxObjectWrapper.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 MemoryX::Helpers
17 * @author ALexey Kozlov ( kozlov at kit dot edu)
18 * @date 2012
19 * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
20 * GNU General Public License
21 */
22 
23 #pragma once
24 
25 // boost
26 #include <memory>
27 #include <filesystem>
28 
29 // Simox-VirtualRobot
30 #include <VirtualRobot/ManipulationObject.h>
31 #include <VirtualRobot/XML/RobotIO.h>
32 #include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>
33 
34 // MemoryX
37 
39 {
40 
41  /**
42  * SimoxObjectWrapper offers a simplified access to the Simox ManipulationObject
43  * (i.e visualization, collision model, grasps) stored in the attributes of MemoryX entity.
44  *
45  */
47  {
48  public:
50  {
54  };
55 
56  /**
57  * Constructs new SimoxObjectWrapper.
58  *
59  * @param gfm GridFileManager to store/load files
60  * @param loadMode eFull or eCollisionModel. If eFull, then both, visualization and collision model is loaded.
61  * If eCollisionModel then only the collision model will be loaded.
62  *
63  */
64  SimoxObjectWrapper(const GridFileManagerPtr& gfm, VirtualRobot::RobotIO::RobotDescription loadMode = VirtualRobot::RobotIO::eFull);
65  ~SimoxObjectWrapper() override;
66 
67  /**
68  * Retrieve visualization model.
69  *
70  * Corresponding files will be fetched and cached locally, if needed.
71  */
72  VirtualRobot::VisualizationNodePtr getVisualization() const;
73 
74  /**
75  * Retrieve collision model
76  *
77  * Corresponding files will be fetched and cached locally, if needed.
78  */
79  VirtualRobot::CollisionModelPtr getCollisionModel() const;
80 
81  /**
82  * Retrieve the complete Simox ManipulationObject
83  *
84  * Corresponding files will be fetched and cached locally, if needed.
85  */
86  VirtualRobot::ManipulationObjectPtr getManipulationObject() const;
87 
88  /**
89  * Retrieve the Simox MainpulationObjects file name in local cache. File is cached
90  * when this method is called.
91  *
92  */
93  std::string getManipulationObjectFileName() const;
94 
95  /**
96  * Store Simox ManipulationObject in the wrapped entity.
97  *
98  * Namely, this function does the following:
99  * \li 1. Clears previous manipulation object, if any. Corresponding files are deleted from GridFS as well.
100  * \li 2. Saves all files in the GridFS
101  * \li 2.1. Manipulation object XML file
102  * \li 2.2. Visualization model file (normally, *.iv)
103  * \li 2.3. Collision model file (normally, *.iv)
104  * \li 2.4. Any textures used by 2.2. and 2.3.
105  * \li 3. Stores references to the files from 2. in corresponding attributes of the wrapped enity
106  *
107  * @param mo manipulation object
108  * @param filesDBName Mongo database name to store additional files into
109  *
110  * @return MongoDB Id of the stored object
111  */
112  std::string setAndStoreManipulationObject(const VirtualRobot::ManipulationObjectPtr mo, const std::string& filesDBName);
113 
114  /**
115  * Set Simox ManipulationObject in the wrapped entity. The object is not stored to GridFS.
116  *
117  * @param mo manipulation object
118  */
119  void setManipulationObject(const VirtualRobot::ManipulationObjectPtr mo);
120 
121  /**
122  * Create Simox ManipulationObject from an XML file and store it in the wrapped entity.
123  * See setManipulationObject for details.
124  *
125  * @param xmlFName name of XML file with manipulation object definition
126  * @param filesDBName Mongo database name to store additional files into
127  */
128  void setAndStoreManipulationFile(const std::string& xmlFName, const std::string& filesDBName);
129 
130  /**
131  * Create Simox ManipulationObject from an XML file, but do not store it in the wrapped entity.
132  * See setManipulationObject for details.
133  *
134  * @param xmlFName name of XML file with manipulation object definition
135  */
136  void setManipulationFile(const std::string& xmlFName);
137 
138  /**
139  * Create Simox ManipulationObject from separate .iv models and store it in the wrapped entity.
140  * See setManipulationObject for details.
141  *
142  * @param ivFNameVis name of .iv file for visualization model
143  * @param ivFNameCollision name of .iv file for collision model
144  * @param filesDBName Mongo database name to store additional files into
145  */
146  void setAndStoreModelIVFiles(const std::string& ivFNameVis, const std::string& ivFNameCollision, const std::string& filesDBName);
147 
148  /**
149  * Create Simox ManipulationObject from separate .iv models, but do not store it in the wrapped entity.
150  * See setManipulationObject for details.
151  *
152  * @param ivFNameVis name of .iv file for visualization model
153  * @param ivFNameCollision name of .iv file for collision model
154  */
155  void setModelIVFiles(const std::string& ivFNameVis, const std::string& ivFNameCollision);
156 
157  /**
158  * Update the Simox object using attribute values (position, orientation etc) from the entity,
159  * but reusing the existing visualization. This method meant to be used with subsequent observations of
160  * the same entity. For new entity instances, create new SimoxObjectWrapper object instead.
161  *
162  * @param entity entity to get attribute values from
163  */
164  void updateFromEntity(const EntityBasePtr& entity);
165 
166 
167  /**
168  * Update object visualization using the current values of pose and position uncertainty from the wrapped entity
169  */
170  void refreshVisuPose();
171 
172  /**
173  * Set visualization type for position uncertainty.
174  *
175  * @param visuType one of: eEllipse - ellipsoids, full 3d uncertainty shown
176  * eHeatMap - flat colored map, XY uncertainty only
177  * eHeatSurface - 3d surface,
178  */
180 
181  /**
182  * Set parameters for uncertainty visualization
183  *
184  * @param ellipseRadiusInSigmas
185  * @param heatmapMinVariance
186  * @param heatmapDensityThreshold
187  *
188  */
189  void setUncertaintyVisuParams(float ellipseRadiusInSigmas, float heatmapMinVariance,
190  float heatmapDensityThreshold, int heatmapGridSize);
191 
192  /**
193  * Clears the Simox ManipulationObject and optionally removes the corresponding files from GridFS.
194  *
195  * @param removeFiles whether GridFS files (.iv models, textures, MO XML) should be deleted as well
196  */
197  void clear(bool removeFiles = true);
198 
199  /**
200  * @brief GetAllFilenames walks \p node and appends absolute paths of all found SoFile, SoImage, and SoTexture2 node filenames to \p storeFilenames.
201  * @param node SoNode which is analyzed
202  * @param storeFilenames vector to which the found filenames are appended
203  * @param origFile name of the original file used to load \p node (can be used to retrieve absolute paths like texture filenames which are stored as relative paths in Coin3D).
204  */
205  static void GetAllFilenames(SoNode* node, std::vector<std::string>& storeFilenames, const std::string& origFile = "");
206 
207  /**
208  * @brief GetAbsolutePath prepend \p filename with \p origFile and append it to \p storeFilenames (does nothing if \p filename is NULL)
209  * @param filename SbString of the filename to check
210  * @param origFile string containing the original filename
211  * @param storeFilenames vector filename strings to which the result should be appended
212  */
213  static std::string GetAbsolutePath(SbString filename, const std::string& origFile);
214 
215  /**
216  * @brief FindIvTextures scans the given \p ivFName for texture files and returns them in the \p textures string list
217  * @param ivFName path to Inventor file to scan for textures
218  * @param textures vector of strings containing the paths to texture files
219  */
220  static void FindIvTextures(const std::string& ivFName, NameList& textures);
221 
222  /**
223  * Returns the orientation of the object that it should have when it is being put down e.g. onto the table by the robot.
224  */
225  Eigen::Vector3f getPutdownOrientationRPY();
226  /**
227  * Sets the orientation of the object that it should have when it is being put down e.g. onto the table by the robot.
228  */
229  void setPutdownOrientationRPY(Eigen::Vector3f& rpy);
230  Ice::ObjectPtr ice_clone() const override;
231  private:
232  VirtualRobot::CoinVisualizationFactoryPtr coinVisFactory;
233  mutable VirtualRobot::ManipulationObjectPtr simoxObject;
234  VirtualRobot::RobotIO::RobotDescription loadMode;
235  std::filesystem::path cachePath;
236  std::string moTempFile;
237 
238  UncertaintyVisuType uncertaintyVisuType;
239  float ellipseRadiusInSigmas;
240  float heatmapMinVariance;
241  float heatmapDensityThreshold;
242  int heatmapGridSize;
243 
244  void loadSimoxObject() const;
245 
246  std::string cacheAttributeFile(const std::string& attrName, bool preserveOriginalFName = false) const;
247  bool cacheAttributeFiles(const std::string& attrName, bool preserveOriginalFName = false) const;
248 
249  VirtualRobot::ManipulationObjectPtr loadManipulationObjectFile(const std::string& xmlFName) const;
250  VirtualRobot::ManipulationObjectPtr createManipulationObjectFromIvFiles(const std::string& objName,
251  const std::string& ivFNameVis, const std::string& ivFNameCollision) const;
252 
253  std::string storeManipulationObject(const VirtualRobot::ManipulationObjectPtr mo, const std::string& filesDBName);
254  void storeEntityIVFiles(const std::string& visuFName, const std::string& collisionFName, const std::string& filesDBName, bool processTextures = true);
255 
256  VirtualRobot::VisualizationNodePtr getUncertaintyEllipsesVisu(GaussianMixtureDistributionBasePtr posGM, const Eigen::Vector3f& objPos,
257  const VirtualRobot::ColorMap& cmap, float transparency);
258  VirtualRobot::VisualizationNodePtr getUncertaintyHeatVisu(GaussianMixtureDistributionBasePtr posGM, const Eigen::Vector3f& objPos, bool heatSurface,
259  const VirtualRobot::ColorMap& cmap);
260 
261  float evaluate2DGaussian(const Eigen::Vector2f& point, const Eigen::Vector2f& mean, const Eigen::Matrix2f& covInv, float covDetSqrtInv);
262  float evaluate3DGaussian(const Eigen::Vector3f& point, const Eigen::Vector3f& mean, const Eigen::Matrix3f& covInv, float covDet);
263 
264  VirtualRobot::ColorMap getUncertaintyColorMap();
265  float getUncertaintyEllipseTransparency();
266  };
267 
269 
270 }
armarx::eFull
@ eFull
Definition: MorphingItem.h:35
memoryx::EntityWrappers::SimoxObjectWrapper::eHeatSurface
@ eHeatSurface
Definition: SimoxObjectWrapper.h:53
memoryx::EntityWrappers::SimoxObjectWrapper::refreshVisuPose
void refreshVisuPose()
Update object visualization using the current values of pose and position uncertainty from the wrappe...
Definition: SimoxObjectWrapper.cpp:299
memoryx::EntityWrappers::SimoxObjectWrapper::setModelIVFiles
void setModelIVFiles(const std::string &ivFNameVis, const std::string &ivFNameCollision)
Create Simox ManipulationObject from separate .iv models, but do not store it in the wrapped entity.
Definition: SimoxObjectWrapper.cpp:205
memoryx::EntityWrappers::SimoxObjectWrapper::getPutdownOrientationRPY
Eigen::Vector3f getPutdownOrientationRPY()
Returns the orientation of the object that it should have when it is being put down e....
Definition: SimoxObjectWrapper.cpp:886
memoryx::EntityWrappers::SimoxObjectWrapper::setUncertaintyVisuParams
void setUncertaintyVisuParams(float ellipseRadiusInSigmas, float heatmapMinVariance, float heatmapDensityThreshold, int heatmapGridSize)
Set parameters for uncertainty visualization.
Definition: SimoxObjectWrapper.cpp:216
GridFileManager.h
memoryx::EntityWrappers::SimoxObjectWrapper::FindIvTextures
static void FindIvTextures(const std::string &ivFName, NameList &textures)
FindIvTextures scans the given ivFName for texture files and returns them in the textures string list...
Definition: SimoxObjectWrapper.cpp:868
memoryx::EntityWrappers::SimoxObjectWrapper::setManipulationObject
void setManipulationObject(const VirtualRobot::ManipulationObjectPtr mo)
Set Simox ManipulationObject in the wrapped entity.
Definition: SimoxObjectWrapper.cpp:167
memoryx::EntityWrappers::SimoxObjectWrapper::ice_clone
Ice::ObjectPtr ice_clone() const override
Definition: SimoxObjectWrapper.cpp:105
memoryx::EntityWrappers::SimoxObjectWrapper::clear
void clear(bool removeFiles=true)
Clears the Simox ManipulationObject and optionally removes the corresponding files from GridFS.
Definition: SimoxObjectWrapper.cpp:620
memoryx::EntityWrappers::SimoxObjectWrapper::eEllipse
@ eEllipse
Definition: SimoxObjectWrapper.h:51
memoryx::EntityWrappers::SimoxObjectWrapper::getManipulationObject
VirtualRobot::ManipulationObjectPtr getManipulationObject() const
Retrieve the complete Simox ManipulationObject.
Definition: SimoxObjectWrapper.cpp:138
memoryx::EntityWrappers::SimoxObjectWrapper::setAndStoreManipulationObject
std::string setAndStoreManipulationObject(const VirtualRobot::ManipulationObjectPtr mo, const std::string &filesDBName)
Store Simox ManipulationObject in the wrapped entity.
Definition: SimoxObjectWrapper.cpp:161
memoryx::EntityWrappers::SimoxObjectWrapper::SimoxObjectWrapper
SimoxObjectWrapper(const GridFileManagerPtr &gfm, VirtualRobot::RobotIO::RobotDescription loadMode=VirtualRobot::RobotIO::eFull)
Constructs new SimoxObjectWrapper.
Definition: SimoxObjectWrapper.cpp:81
memoryx::EntityWrappers::SimoxObjectWrapper::getCollisionModel
VirtualRobot::CollisionModelPtr getCollisionModel() const
Retrieve collision model.
Definition: SimoxObjectWrapper.cpp:124
IceInternal::Handle< SimoxObjectWrapper >
memoryx::EntityWrappers::SimoxObjectWrapper::getVisualization
VirtualRobot::VisualizationNodePtr getVisualization() const
Retrieve visualization model.
Definition: SimoxObjectWrapper.cpp:110
armarx::mean
std::optional< float > mean(const boost::circular_buffer< NameValueMap > &buffer, const std::string &key)
Definition: KinematicUnitGuiPlugin.cpp:1615
GfxTL::Matrix2f
MatrixXX< 2, 2, float > Matrix2f
Definition: MatrixXX.h:599
memoryx::EntityWrappers::SimoxObjectWrapper::setPutdownOrientationRPY
void setPutdownOrientationRPY(Eigen::Vector3f &rpy)
Sets the orientation of the object that it should have when it is being put down e....
Definition: SimoxObjectWrapper.cpp:908
memoryx::EntityWrappers::AbstractFileEntityWrapper
Definition: AbstractEntityWrapper.h:82
AbstractEntityWrapper.h
filename
std::string filename
Definition: VisualizationRobot.cpp:83
memoryx::EntityWrappers::SimoxObjectWrapper::UncertaintyVisuType
UncertaintyVisuType
Definition: SimoxObjectWrapper.h:49
memoryx::EntityWrappers::SimoxObjectWrapper
SimoxObjectWrapper offers a simplified access to the Simox ManipulationObject (i.e visualization,...
Definition: SimoxObjectWrapper.h:46
memoryx::EntityWrappers::SimoxObjectWrapper::setUncertaintyVisuType
void setUncertaintyVisuType(UncertaintyVisuType visuType)
Set visualization type for position uncertainty.
Definition: SimoxObjectWrapper.cpp:211
memoryx::EntityWrappers::SimoxObjectWrapper::setAndStoreManipulationFile
void setAndStoreManipulationFile(const std::string &xmlFName, const std::string &filesDBName)
Create Simox ManipulationObject from an XML file and store it in the wrapped entity.
Definition: SimoxObjectWrapper.cpp:173
GfxTL::Matrix3f
MatrixXX< 3, 3, float > Matrix3f
Definition: MatrixXX.h:600
memoryx::EntityWrappers::SimoxObjectWrapper::~SimoxObjectWrapper
~SimoxObjectWrapper() override
Definition: SimoxObjectWrapper.cpp:101
memoryx::EntityWrappers
Definition: AbstractEntityWrapper.cpp:28
memoryx::GridFileManagerPtr
std::shared_ptr< GridFileManager > GridFileManagerPtr
Definition: AbstractEntityWrapper.h:32
memoryx::EntityWrappers::SimoxObjectWrapper::eHeatMap
@ eHeatMap
Definition: SimoxObjectWrapper.h:52
memoryx::EntityWrappers::SimoxObjectWrapper::setAndStoreModelIVFiles
void setAndStoreModelIVFiles(const std::string &ivFNameVis, const std::string &ivFNameCollision, const std::string &filesDBName)
Create Simox ManipulationObject from separate .iv models and store it in the wrapped entity.
Definition: SimoxObjectWrapper.cpp:197
memoryx::EntityWrappers::SimoxObjectWrapper::updateFromEntity
void updateFromEntity(const EntityBasePtr &entity)
Update the Simox object using attribute values (position, orientation etc) from the entity,...
Definition: SimoxObjectWrapper.cpp:292
armarx::aron::type::ObjectPtr
std::shared_ptr< Object > ObjectPtr
Definition: Object.h:36
memoryx::EntityWrappers::SimoxObjectWrapper::GetAllFilenames
static void GetAllFilenames(SoNode *node, std::vector< std::string > &storeFilenames, const std::string &origFile="")
GetAllFilenames walks node and appends absolute paths of all found SoFile, SoImage,...
Definition: SimoxObjectWrapper.cpp:917
memoryx::EntityWrappers::SimoxObjectWrapper::getManipulationObjectFileName
std::string getManipulationObjectFileName() const
Retrieve the Simox MainpulationObjects file name in local cache.
Definition: SimoxObjectWrapper.cpp:150
memoryx::EntityWrappers::SimoxObjectWrapper::GetAbsolutePath
static std::string GetAbsolutePath(SbString filename, const std::string &origFile)
GetAbsolutePath prepend filename with origFile and append it to storeFilenames (does nothing if filen...
Definition: SimoxObjectWrapper.cpp:1027
memoryx::EntityWrappers::SimoxObjectWrapper::setManipulationFile
void setManipulationFile(const std::string &xmlFName)
Create Simox ManipulationObject from an XML file, but do not store it in the wrapped entity.
Definition: SimoxObjectWrapper.cpp:187