SimoxCSpace.h
Go to the documentation of this file.
1 /*
2  * This file is part of ArmarX.
3  *
4  * Copyright (C) 2011-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 RobotComponents
19  * @author Raphael Grimm ( raphael dot grimm at kit dot edu )
20  * @date 2015
21  * @copyright http://www.gnu.org/licenses/gpl.txt
22  * GNU General Public License
23  */
24 #pragma once
25 
26 #include <array>
27 
28 #include <Eigen/Core>
29 
30 #include <VirtualRobot/CollisionDetection/CDManager.h>
31 #include <VirtualRobot/SceneObjectSet.h>
32 #include <VirtualRobot/Robot.h>
36 #include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
38 #include <MemoryX/interface/components/WorkingMemoryInterface.h>
39 #include <RobotComponents/interface/components/MotionPlanning/CSpace/SimoxCSpace.h>
40 #include "GlobalCache.h"
41 #include <VirtualRobot/XML/RobotIO.h>
42 
43 #include <atomic>
44 
45 #include "CSpace.h"
46 
48 
49 namespace armarx
50 {
51  class SimoxCSpace;
52  /**
53  * @brief An ice handle for a SimoxCSpace.
54  */
56 
57 
58 
59  /**
60  * @brief The SimoxCSpace contains a set of stationary obstacles and an agent.
61  * The agent can have attached objects.
62  * The objects are loaded from a \ref memoryx::CommonStorageInterfacePrx.
63  * The number of dimensions is determined by the agent's kinematic chains.
64  * @see armarx::SimoxCSpaceBase
65  * @see RobotComponents-Tutorial-SimoxCSpace
66  */
67  class SimoxCSpace :
68  virtual public SimoxCSpaceBase,
69  virtual public CSpace
70  {
71  public:
73 
74  /**
75  * @brief ctor
76  * @param commonStoragePrx The common storage containing the objects.
77  * @param loadVisualizationModel Whether to load the visualization model. (true, if you want to display the object. false if you want to plan)
78  */
79  SimoxCSpace(memoryx::CommonStorageInterfacePrx commonStoragePrx, bool loadVisualizationModel = false, float stationaryObjectMargin = 0.0f);
80 
81  //from setting the cspace up
82  /**
83  * @brief Adds a stationary object to the cspace.
84  * @param obj The object to add.
85  */
86  void addStationaryObject(const StationaryObject& obj, const Ice::Current& = Ice::emptyCurrent)override;
87  virtual void setStationaryObjects(const StationaryObjectList& objList);
88  /**
89  * @brief Adds a stationary agent to the cspace.
90  * @param obj The object to add.
91  */
92  void setAgent(const AgentPlanningInformation& agentInfo, const Ice::Current& = Ice::emptyCurrent)override;
93 
94  /**
95  * @brief Adds all objects except the ones with ids specified in the parameter excludedInstancesIds from the working memory to the cspace.
96  * @param workingMemoryPrx The working memory.
97  * @param excludedInstancesIds the ids of the instances in teh working memory that should not be added
98  */
99  virtual void addObjectsFromWorkingMemory(memoryx::WorkingMemoryInterfacePrx workingMemoryPrx, const std::vector<std::string>& excludedInstancesIds = std::vector<std::string>());
100 
101  /**
102  * @brief Load objects from WorkingMemory and puts them into the mesh cache.
103  * @param workingMemoryPrx
104  */
105  static SimoxCSpacePtr PrefetchWorkingMemoryObjects(memoryx::WorkingMemoryInterfacePrx workingMemoryPrx, memoryx::CommonStorageInterfacePrx commonStoragePrx, RobotStateComponentInterfacePrx rsc);
106 
107  virtual void addPlanarObject(const std::vector<Eigen::Vector3f>& convexHull);
108 
109  //using it
110  /**
111  * @brief Initializes the collision test.
112  */
113  void initCollisionTest(const Ice::Current& = Ice::emptyCurrent)override;
114 
115  /**
116  * @return A clone of this cspace.
117  */
118  CSpaceBasePtr clone(const Ice::Current& = Ice::emptyCurrent) override
119  {
120  return clone(false);
121  }
122 
123  /**
124  * @param loadVisualizationModel Whether the clone should load the visualization model.
125  * @return A clone of this cspace.
126  */
127  virtual CSpaceBasePtr clone(bool loadVisualizationModel);
128 
129  virtual Saba::CSpaceSampledPtr createSimoxCSpace() const;
130 
131  /**
132  * @param cfg The configuration to check.
133  * @return Whether the given configuration is collision free.
134  */
135  bool isCollisionFree(const::std::pair<const Ice::Float*, const Ice::Float*>& cfg, const Ice::Current& = Ice::emptyCurrent)override;
136 
137  /**
138  * @return The cspace's dimension bounds.
139  */
140  FloatRangeSeq getDimensionsBounds(const Ice::Current& = Ice::emptyCurrent) const override;
141  /**
142  * @return The cspace's dimensionality.
143  */
144  Ice::Long getDimensionality(const Ice::Current& = Ice::emptyCurrent) const override
145  {
146  return agentJoints.size();
147  }
148 
149  /**
150  * @brief Sets a configuration to check for. (can be used to display a configuration)
151  * @param it A pointer to the configuration.
152  */
153  virtual void setConfig(const std::vector<float> cfg)
154  {
155  assert(cfg.size() == static_cast<std::size_t>(getDimensionality()));
156  setConfig(cfg.data());
157  }
158  virtual void setConfig(const float*& it);
159  virtual void setConfig(const float*&& it) //deals with vector<float>::data()
160  {
161  setConfig(it);//calls the ref version
162  }
163 
164  //getter
165  /**
166  * @return The stationary objects.
167  */
168  const StationaryObjectList& getStationaryObjects() const
169  {
170  return stationaryObjects;
171  }
172 
173  const AgentPlanningInformation& getAgent() const
174  {
175  return agentInfo;
176  }
177 
178  /**
179  * @return The set of stationary objects' scene objects.
180  */
181  const VirtualRobot::SceneObjectSetPtr& getStationaryObjectSet() const
182  {
183  return stationaryObjectSet;
184  }
185 
187  {
188  return agentSceneObj;
189  }
190 
191  /**
192  * @return The collision checker.
193  */
194  VirtualRobot::CDManager& getCD()
195  {
196  return cd;
197  }
198 
199  virtual Ice::StringSeq getAgentJointNames() const;
200 
201  //utility
202  VectorXf jointMapToVector(const std::map<std::string, float>& jointMap, bool checkForNonexistenJointsInCspace = false) const;
203 
204  virtual TrajectoryPtr pathToTrajectory(const Path& p) const
205  {
206  return pathToTrajectory(p.nodes);
207  }
208  virtual TrajectoryPtr pathToTrajectory(const PathWithCost& p) const
209  {
210  return pathToTrajectory(p.nodes);
211  }
212  virtual TrajectoryPtr pathToTrajectory(const VectorXfSeq& p) const;
213 
214  std::vector<armarx::DebugDrawerLineSet> getTraceVisu(const Path& p, const std::vector<std::string>& nodeNames, float traceStepSize = std::numeric_limits<float>::infinity())
215  {
216  return getTraceVisu(p.nodes, nodeNames, traceStepSize);
217  }
218  std::vector<armarx::DebugDrawerLineSet> getTraceVisu(const PathWithCost& p, const std::vector<std::string>& nodeNames, float traceStepSize = std::numeric_limits<float>::infinity())
219  {
220  return getTraceVisu(p.nodes, nodeNames, traceStepSize);
221  }
222  std::vector<armarx::DebugDrawerLineSet> getTraceVisu(const VectorXfSeq& vecs, const std::vector<std::string>& nodeNames, float traceStepSize = std::numeric_limits<float>::infinity());
223 
225  {
226  return stationaryObjectMargin;
227  }
228 
229  void setStationaryObjectMargin(float stationaryObjectMargin)
230  {
231  if (!isInitialized)
232  {
233  this->stationaryObjectMargin = stationaryObjectMargin;
234  }
235  else
236  {
237  ARMARX_ERROR << "Could not set stationary object margin, because the cSpace is already initialized.";
238  }
239  }
240 
241  //from ice
242  /**
243  * @brief Initializes basic structures after transmission through ice.
244  */
245  void ice_postUnmarshal() override;
246 
247  protected:
248  template <class IceBaseClass, class DerivedClass> friend class armarx::GenericFactory;
249 
250  const std::vector<VirtualRobot::RobotNodePtr>& getAgentJoints() const
251  {
252  return agentJoints;
253  }
254 
255  /**
256  * @brief Default ctor. Used for ice factories.
257  */
258  SimoxCSpace() = default;
259 
260  //data
261  /**
262  * @brief Whether the collision check is initialized
263  */
264  bool isInitialized {false};
265  /**
266  * @brief Whether the visualization model of objects/agents sould be loaded.
267  */
269 
270 
271  /**
272  * @brief The file manager used to load objects.
273  */
275 
276  //cd data
277  /**
278  * @brief The collision checker.
279  */
280  VirtualRobot::CDManager cd;
281 
282  /**
283  * @brief The scene objects for stationary objects.
284  */
285  VirtualRobot::SceneObjectSetPtr stationaryObjectSet {new VirtualRobot::SceneObjectSet{"StationaryObjectSet"}};
286 
287  std::vector<VirtualRobot::RobotNodePtr> agentJoints;
288 
290 
291  std::vector<std::vector<Eigen::Vector3f>> stationaryPlanes;
292 
295 
296 
297  /**
298  * @brief Initializes stationary objects for collision checking.
299  */
300  void initStationaryObjects();
301 
302  /**
303  * @brief Contains information about an agent.
304  */
305  struct AgentData
306  {
307  /**
308  * @brief The agent's collision model. (contains more data but mainly used for the model)
309  */
311  /**
312  * @brief The agent's joints.
313  */
314  std::vector<VirtualRobot::RobotNodePtr> joints;
315  /**
316  * @brief The collision stes.
317  */
318  std::vector<VirtualRobot::SceneObjectSetPtr> collisionSets;
319  };
320 
321  //static functions to load data
322  /**
323  * @brief Initializes an agent.
324  * (Does not need to read from the file system if loadAnyModel is false)
325  * @param agentData Data about the agent.
326  * @param loadAnyModel if unset only the robot structure is loaded.
327  * If set the robot's and objects' collision model are loaded. (depending on
328  * loadVisualizationModel the robot's visualisation model is loaded)
329  */
330  void initAgent(AgentData agentData);
331 
332  void initAgent()
333  {
335  }
336 
338  {
339  initAgent(getAgentDataFromRobot(std::move(robotPtr)));
340  }
341 
342  /**
343  * @param object The object.
344  * @return The given objects manipulation object.
345  */
346  VirtualRobot::ManipulationObjectPtr getSimoxManipulatorObject(const memoryx::ObjectClassBasePtr& object, const memoryx::GridFileManagerPtr& fileManager, bool inflate = true, VirtualRobot::CollisionCheckerPtr const& colChecker = VirtualRobot::CollisionCheckerPtr {}) const;
347 
348  /**
349  * @param object The object.
350  * @param pose The pose to use.
351  * @return The given objects manipulation object moved to the given pose.
352  */
353  VirtualRobot::ManipulationObjectPtr getMovedSimoxManipulatorObject(const memoryx::ObjectClassBasePtr& object, const armarx::PoseBasePtr& pose, memoryx::GridFileManagerPtr& fileManager) const;
354 
355  AgentData getAgentDataAndLoadRobot() const;
356  AgentData getAgentDataFromRobot(VirtualRobot::RobotPtr robotPtr) const;
357  };
358 
359 
360 
361  class SimoxCSpaceWith2DPose;
363  /**
364  * @brief Similar to \ref armarx::SimoxCSpace, but prepends dimensions for translation in x and y and a rotation around z. (use this for path planning)
365  */
367  virtual public SimoxCSpace,
368  virtual public SimoxCSpaceWith2DPoseBase
369  {
370  public:
371  SimoxCSpaceWith2DPose(memoryx::CommonStorageInterfacePrx commonStoragePrx, bool loadVisualizationModel = false, float stationaryObjectMargin = 0.0f);
372 
373  void setPoseBounds(const Vector3fRange& newBounds)
374  {
375  poseBounds = newBounds;
376  }
377 
378  /**
379  * @param loadVisualizationModel Whether the clone should load the visualization model.
380  * @return A clone of this cspace.
381  */
382  CSpaceBasePtr clone(bool loadVisualizationModel) override;
383 
384  /**
385  * @return The cspace's dimension bounds.
386  */
387  FloatRangeSeq getDimensionsBounds(const Ice::Current& = Ice::emptyCurrent) const override;
388  /**
389  * @return The cspace's dimensionality.
390  */
391  Ice::Long getDimensionality(const Ice::Current& = Ice::emptyCurrent) const override
392  {
393  return agentJoints.size() + 3;
394  }
395 
396  Ice::StringSeq getAgentJointNames() const override;
397 
398  void setConfig(const float*& it) override;
399  protected:
400  template <class IceBaseClass, class DerivedClass> friend class armarx::GenericFactory;
401  SimoxCSpaceWith2DPose() = default;
402 
404  };
405 
406  class SimoxCSpaceWith3DPose;
408  /**
409  * @brief Similar to \ref armarx::SimoxCSpace, but prepends dimensions for translation in x, y and z and rotations around x, y and z.
410  * (use this for path planning of flying robots).
411  */
413  virtual public SimoxCSpace,
414  virtual public SimoxCSpaceWith3DPoseBase
415  {
416  public:
417  SimoxCSpaceWith3DPose(memoryx::CommonStorageInterfacePrx commonStoragePrx, bool loadVisualizationModel, float stationaryObjectMargin = 0.0f);
418 
419  void setPoseBounds(const Vector6fRange& newBounds)
420  {
421  poseBounds = newBounds;
422  }
423 
424  /**
425  * @param loadVisualizationModel Whether the clone should load the visualization model.
426  * @return A clone of this cspace.
427  */
428  CSpaceBasePtr clone(bool loadVisualizationModel) override;
429 
430  /**
431  * @return The cspace's dimension bounds.
432  */
433  FloatRangeSeq getDimensionsBounds(const Ice::Current& = Ice::emptyCurrent) const override;
434  /**
435  * @return The cspace's dimensionality.
436  */
437  Ice::Long getDimensionality(const Ice::Current& = Ice::emptyCurrent) const override
438  {
439  return agentJoints.size() + 6;
440  }
441 
442  void setConfig(const float*& it) override;
443 
444  Ice::StringSeq getAgentJointNames() const override;
445  protected:
446  template <class IceBaseClass, class DerivedClass> friend class armarx::GenericFactory;
447  SimoxCSpaceWith3DPose() = default;
448 
450  };
451 
452 }
armarx::SimoxCSpace::getTraceVisu
std::vector< armarx::DebugDrawerLineSet > getTraceVisu(const PathWithCost &p, const std::vector< std::string > &nodeNames, float traceStepSize=std::numeric_limits< float >::infinity())
Definition: SimoxCSpace.h:218
armarx::SimoxCSpaceWith2DPose::setConfig
void setConfig(const float *&it) override
Definition: SimoxCSpace.cpp:871
armarx::SimoxCSpaceWith3DPose::setConfig
void setConfig(const float *&it) override
Definition: SimoxCSpace.cpp:937
armarx::SimoxCSpace::pathToTrajectory
virtual TrajectoryPtr pathToTrajectory(const PathWithCost &p) const
Definition: SimoxCSpace.h:208
armarx::SimoxCSpace::getDimensionsBounds
FloatRangeSeq getDimensionsBounds(const Ice::Current &=Ice::emptyCurrent) const override
Definition: SimoxCSpace.cpp:778
armarx::SimoxCSpace::getAgentDataAndLoadRobot
AgentData getAgentDataAndLoadRobot() const
Definition: SimoxCSpace.cpp:287
armarx::SimoxCSpaceWith3DPose::SimoxCSpaceWith3DPose
SimoxCSpaceWith3DPose()=default
armarx::CSpace::isCollisionFree
bool isCollisionFree(const ::std::pair< const Ice::Float *, const Ice::Float * > &config, const Ice::Current &=Ice::emptyCurrent) override=0
armarx::SimoxCSpace::getAgentDataFromRobot
AgentData getAgentDataFromRobot(VirtualRobot::RobotPtr robotPtr) const
Definition: SimoxCSpace.cpp:370
armarx::SimoxCSpace::AgentData::joints
std::vector< VirtualRobot::RobotNodePtr > joints
The agent's joints.
Definition: SimoxCSpace.h:314
armarx::SimoxCSpace::getAgentSceneObj
const VirtualRobot::RobotPtr & getAgentSceneObj() const
Definition: SimoxCSpace.h:186
armarx::SimoxCSpace::AgentData::agent
VirtualRobot::RobotPtr agent
The agent's collision model.
Definition: SimoxCSpace.h:310
CSpace.h
armarx::SimoxCSpace::pathToTrajectory
virtual TrajectoryPtr pathToTrajectory(const Path &p) const
Definition: SimoxCSpace.h:204
armarx::SimoxCSpace::isInitialized
bool isInitialized
Whether the collision check is initialized.
Definition: SimoxCSpace.h:264
armarx::SimoxCSpace::setStationaryObjectMargin
void setStationaryObjectMargin(float stationaryObjectMargin)
Definition: SimoxCSpace.h:229
armarx::SimoxCSpaceWith2DPose
Similar to armarx::SimoxCSpace, but prepends dimensions for translation in x and y and a rotation aro...
Definition: SimoxCSpace.h:366
armarx::CSpace
Implementation of the slice class CSpaceBase.
Definition: CSpace.h:51
GridFileManager.h
armarx::SimoxCSpaceWith2DPose::globalPoseBuffer
Eigen::Matrix4f globalPoseBuffer
Definition: SimoxCSpace.h:403
armarx::SimoxCSpace::getTraceVisu
std::vector< armarx::DebugDrawerLineSet > getTraceVisu(const Path &p, const std::vector< std::string > &nodeNames, float traceStepSize=std::numeric_limits< float >::infinity())
Definition: SimoxCSpace.h:214
Convex::convexHull
ConvexHull< Point >::type convexHull(const std::vector< Point > &points)
Definition: convexHull.hpp:487
armarx::SimoxCSpaceWith3DPose::globalPoseBuffer
Eigen::Matrix4f globalPoseBuffer
Definition: SimoxCSpace.h:449
FactoryCollectionBase.h
armarx::SimoxCSpace::AgentData::collisionSets
std::vector< VirtualRobot::SceneObjectSetPtr > collisionSets
The collision stes.
Definition: SimoxCSpace.h:318
armarx::SimoxCSpaceWith2DPose::setPoseBounds
void setPoseBounds(const Vector3fRange &newBounds)
Definition: SimoxCSpace.h:373
armarx::SimoxCSpace::getCD
VirtualRobot::CDManager & getCD()
Definition: SimoxCSpace.h:194
armarx::SimoxCSpace::initStationaryObjects
void initStationaryObjects()
Initializes stationary objects for collision checking.
Definition: SimoxCSpace.cpp:221
armarx::SimoxCSpace::addObjectsFromWorkingMemory
virtual void addObjectsFromWorkingMemory(memoryx::WorkingMemoryInterfacePrx workingMemoryPrx, const std::vector< std::string > &excludedInstancesIds=std::vector< std::string >())
Adds all objects except the ones with ids specified in the parameter excludedInstancesIds from the wo...
Definition: SimoxCSpace.cpp:628
armarx::SimoxCSpace::createSimoxCSpace
virtual Saba::CSpaceSampledPtr createSimoxCSpace() const
Definition: SimoxCSpace.cpp:129
IceInternal::Handle< SimoxCSpace >
armarx::SimoxCSpace::getStationaryObjectMargin
float getStationaryObjectMargin() const
Definition: SimoxCSpace.h:224
armarx::SimoxCSpaceWith2DPose::getDimensionsBounds
FloatRangeSeq getDimensionsBounds(const Ice::Current &=Ice::emptyCurrent) const override
Definition: SimoxCSpace.cpp:843
armarx::SimoxCSpace::fileManager
memoryx::GridFileManagerPtr fileManager
The file manager used to load objects.
Definition: SimoxCSpace.h:274
armarx::SimoxCSpaceWith3DPose::getDimensionsBounds
FloatRangeSeq getDimensionsBounds(const Ice::Current &=Ice::emptyCurrent) const override
Definition: SimoxCSpace.cpp:912
armarx::SimoxCSpace::stationaryPlanes
std::vector< std::vector< Eigen::Vector3f > > stationaryPlanes
Definition: SimoxCSpace.h:291
GfxTL::Identity
void Identity(MatrixXX< N, N, T > *a)
Definition: MatrixXX.h:523
armarx::SimoxCSpace::robotCache
GlobalCache< RobotPoolPtr, std::pair< VirtualRobot::RobotIO::RobotDescription, std::string > > robotCache
Definition: SimoxCSpace.h:294
armarx::SimoxCSpace::clone
CSpaceBasePtr clone(const Ice::Current &=Ice::emptyCurrent) override
Definition: SimoxCSpace.h:118
armarx::SimoxCSpace::getDimensionality
Ice::Long getDimensionality(const Ice::Current &=Ice::emptyCurrent) const override
Definition: SimoxCSpace.h:144
armarx::SimoxCSpace::getStationaryObjectSet
const VirtualRobot::SceneObjectSetPtr & getStationaryObjectSet() const
Definition: SimoxCSpace.h:181
armarx::SimoxCSpace::getAgentJointNames
virtual Ice::StringSeq getAgentJointNames() const
Definition: SimoxCSpace.cpp:792
armarx::SimoxCSpace::ice_postUnmarshal
void ice_postUnmarshal() override
Initializes basic structures after transmission through ice.
Definition: SimoxCSpace.cpp:693
armarx::SimoxCSpace::getAgent
const AgentPlanningInformation & getAgent() const
Definition: SimoxCSpace.h:173
GlobalCache.h
armarx::SimoxCSpace::addPlanarObject
virtual void addPlanarObject(const std::vector< Eigen::Vector3f > &convexHull)
Definition: SimoxCSpace.cpp:688
armarx::SimoxCSpace::getAgentJoints
const std::vector< VirtualRobot::RobotNodePtr > & getAgentJoints() const
Definition: SimoxCSpace.h:250
armarx::SimoxCSpace::setAgent
void setAgent(const AgentPlanningInformation &agentInfo, const Ice::Current &=Ice::emptyCurrent) override
Adds a stationary agent to the cspace.
Definition: SimoxCSpace.cpp:157
armarx::SimoxCSpace::initAgent
void initAgent()
Definition: SimoxCSpace.h:332
armarx::VariantType::Long
const VariantTypeId Long
Definition: Variant.h:917
armarx::SimoxCSpace::PrefetchWorkingMemoryObjects
static SimoxCSpacePtr PrefetchWorkingMemoryObjects(memoryx::WorkingMemoryInterfacePrx workingMemoryPrx, memoryx::CommonStorageInterfacePrx commonStoragePrx, RobotStateComponentInterfacePrx rsc)
Load objects from WorkingMemory and puts them into the mesh cache.
Definition: SimoxCSpace.cpp:673
ARMARX_ERROR
#define ARMARX_ERROR
Definition: Logging.h:189
armarx::SimoxCSpace::addStationaryObject
void addStationaryObject(const StationaryObject &obj, const Ice::Current &=Ice::emptyCurrent) override
Adds a stationary object to the cspace.
Definition: SimoxCSpace.cpp:145
armarx::SimoxCSpace::getMovedSimoxManipulatorObject
VirtualRobot::ManipulationObjectPtr getMovedSimoxManipulatorObject(const memoryx::ObjectClassBasePtr &object, const armarx::PoseBasePtr &pose, memoryx::GridFileManagerPtr &fileManager) const
Definition: SimoxCSpace.cpp:609
armarx::SimoxCSpace::initCollisionTest
void initCollisionTest(const Ice::Current &=Ice::emptyCurrent) override
Initializes the collision test.
Definition: SimoxCSpace.cpp:192
armarx::GlobalCache
Definition: GlobalCache.h:38
armarx::SimoxCSpaceWith2DPose::getAgentJointNames
Ice::StringSeq getAgentJointNames() const override
Definition: SimoxCSpace.cpp:860
armarx::SimoxCSpaceWith2DPose::SimoxCSpaceWith2DPose
SimoxCSpaceWith2DPose()=default
armarx::SimoxCSpaceWith3DPose::getAgentJointNames
Ice::StringSeq getAgentJointNames() const override
Definition: SimoxCSpace.cpp:974
armarx::SimoxCSpaceWith3DPose::setPoseBounds
void setPoseBounds(const Vector6fRange &newBounds)
Definition: SimoxCSpace.h:419
armarx::SimoxCSpace::AgentData
Contains information about an agent.
Definition: SimoxCSpace.h:305
armarx::SimoxCSpace::meshCache
GlobalCache< VirtualRobot::ManipulationObjectPtr, std::pair< int, std::string > > meshCache
Definition: SimoxCSpace.h:293
Trajectory.h
armarx::SimoxCSpace::setConfig
virtual void setConfig(const float *&&it)
Definition: SimoxCSpace.h:159
GfxTL::Matrix4f
MatrixXX< 4, 4, float > Matrix4f
Definition: MatrixXX.h:601
armarx::SimoxCSpaceWith2DPose::clone
CSpaceBasePtr clone(bool loadVisualizationModel) override
Definition: SimoxCSpace.cpp:830
armarx::SimoxCSpace::jointMapToVector
VectorXf jointMapToVector(const std::map< std::string, float > &jointMap, bool checkForNonexistenJointsInCspace=false) const
Definition: SimoxCSpace.cpp:803
memoryx::GridFileManagerPtr
std::shared_ptr< GridFileManager > GridFileManagerPtr
Definition: AbstractEntityWrapper.h:32
armarx::SimoxCSpace::stationaryObjectSet
VirtualRobot::SceneObjectSetPtr stationaryObjectSet
The scene objects for stationary objects.
Definition: SimoxCSpace.h:285
armarx::SimoxCSpaceWith3DPose::clone
CSpaceBasePtr clone(bool loadVisualizationModel) override
Definition: SimoxCSpace.cpp:899
armarx::SimoxCSpaceWith3DPose
Similar to armarx::SimoxCSpace, but prepends dimensions for translation in x, y and z and rotations a...
Definition: SimoxCSpace.h:412
IceInternal::ProxyHandle<::IceProxy::armarx::RobotStateComponentInterface >
armarx::SimoxCSpace::setStationaryObjects
virtual void setStationaryObjects(const StationaryObjectList &objList)
Definition: SimoxCSpace.cpp:152
armarx::SimoxCSpace::loadVisualizationModel
bool loadVisualizationModel
Whether the visualization model of objects/agents sould be loaded.
Definition: SimoxCSpace.h:268
armarx::SimoxCSpace::cd
VirtualRobot::CDManager cd
The collision checker.
Definition: SimoxCSpace.h:280
armarx::SimoxCSpace::agentSceneObj
VirtualRobot::RobotPtr agentSceneObj
Definition: SimoxCSpace.h:289
armarx::armem::server::ltm::detail::mixin::Path
std::filesystem::path Path
Definition: DiskStorageMixin.h:17
armarx::SimoxCSpace::initAgent
void initAgent(VirtualRobot::RobotPtr robotPtr)
Definition: SimoxCSpace.h:337
armarx::SimoxCSpace::setConfig
virtual void setConfig(const std::vector< float > cfg)
Sets a configuration to check for.
Definition: SimoxCSpace.h:153
armarx::GenericFactory
Definition: FactoryCollectionBase.h:51
armarx::SimoxCSpace
The SimoxCSpace contains a set of stationary obstacles and an agent.
Definition: SimoxCSpace.h:67
armarx::SimoxCSpace::isCollisionFree
bool isCollisionFree(const ::std::pair< const Ice::Float *, const Ice::Float * > &cfg, const Ice::Current &=Ice::emptyCurrent) override
Definition: SimoxCSpace.cpp:176
armarx::SimoxCSpace::SimoxCSpace
SimoxCSpace()=default
Default ctor.
armarx::SimoxCSpace::getSimoxManipulatorObject
VirtualRobot::ManipulationObjectPtr getSimoxManipulatorObject(const memoryx::ObjectClassBasePtr &object, const memoryx::GridFileManagerPtr &fileManager, bool inflate=true, VirtualRobot::CollisionCheckerPtr const &colChecker=VirtualRobot::CollisionCheckerPtr {}) const
Definition: SimoxCSpace.cpp:550
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:28
armarx::SimoxCSpace::agentJoints
std::vector< VirtualRobot::RobotNodePtr > agentJoints
Definition: SimoxCSpace.h:287
armarx::SimoxCSpaceWith3DPose::getDimensionality
Ice::Long getDimensionality(const Ice::Current &=Ice::emptyCurrent) const override
Definition: SimoxCSpace.h:437
armarx::SimoxCSpace::getStationaryObjects
const StationaryObjectList & getStationaryObjects() const
Definition: SimoxCSpace.h:168
VirtualRobot::RobotPtr
std::shared_ptr< class Robot > RobotPtr
Definition: Bus.h:18
RobotPool.h
armarx::SimoxCSpaceWith2DPose::getDimensionality
Ice::Long getDimensionality(const Ice::Current &=Ice::emptyCurrent) const override
Definition: SimoxCSpace.h:391