Simulator.cpp
Go to the documentation of this file.
1 /*
2  * This file is part of ArmarX.
3  *
4  * Copyright (C) 2013-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 ArmarXSimulation::Simulator
19  * @author Nikolaus ( vahrenkamp at kit dot edu )
20  * @date 2015
21  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
22  * GNU General Public License
23  */
24 
25 #include <filesystem>
26 
27 #include <VirtualRobot/MathTools.h>
28 #include <VirtualRobot/XML/BaseIO.h>
29 #include <VirtualRobot/XML/RobotIO.h>
30 
31 #include "ArmarXCore/core/PackagePath.h"
32 #include <ArmarXCore/core/exceptions/local/ExpressionException.h>
33 #include <ArmarXCore/core/util/StringHelpers.h>
34 #define MAX_SIM_TIME_WARNING 25
35 
36 #include "Simulator.h"
37 
38 #ifdef MUJOCO_PHYSICS_WORLD
39 #include "MujocoPhysicsWorld.h"
40 #endif
41 
42 #include <SimoxUtility/algorithm/string/string_tools.h>
43 #include <SimoxUtility/json.h>
44 #include <VirtualRobot/Scene.h>
45 #include <VirtualRobot/SceneObject.h>
46 #include <VirtualRobot/XML/ObjectIO.h>
47 #include <VirtualRobot/math/Helpers.h>
48 
49 #include <ArmarXCore/core/ArmarXManager.h>
50 #include <ArmarXCore/core/system/ArmarXDataPath.h>
51 #include <ArmarXCore/util/CPPUtility/trace.h>
52 
53 #include <RobotAPI/interface/core/GeometryBase.h>
54 #include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
55 #include <RobotAPI/libraries/ArmarXObjects/Scene.h>
56 #include <RobotAPI/libraries/ArmarXObjects/json_conversions.h>
57 #include <RobotAPI/libraries/core/FramedPose.h>
58 #include <RobotAPI/libraries/core/Pose.h>
59 
60 #include <MemoryX/core/MemoryXCoreObjectFactories.h>
61 #include <MemoryX/libraries/memorytypes/MemoryXTypesObjectFactories.h>
62 #include <MemoryX/libraries/memorytypes/entity/AgentInstance.h>
63 #include <MemoryX/libraries/memorytypes/entity/ObjectClass.h>
64 #include <MemoryX/libraries/memorytypes/entity/ObjectInstance.h>
65 #include <SimDynamics/DynamicsEngine/DynamicsObject.h>
66 #include <SimDynamics/DynamicsEngine/DynamicsRobot.h>
67 #include <sdf/sdf.hh>
68 
69 using namespace armarx;
70 using namespace memoryx;
71 
72 SimulatorPropertyDefinitions::SimulatorPropertyDefinitions(std::string prefix) :
73  ComponentPropertyDefinitions(prefix)
74 {
75  defineOptionalProperty<std::string>(
76  "SimoxSceneFileName", "", "Simox/VirtualRobot scene file name, e.g. myScene.xml");
77  defineOptionalProperty<bool>("FloorPlane", true, "Enable floor plane.");
78  defineOptionalProperty<std::string>("FloorTexture", "", "Texture file for floor.");
79  defineOptionalProperty<std::string>(
80  "LongtermMemory.SnapshotName", "", "Name of snapshot to load the scene")
81  .setCaseInsensitive(true);
82  defineOptionalProperty<bool>(
83  "LoadSnapshotToWorkingMemory", true, "Load the snapshot also into the WorkingMemory");
84  defineOptionalProperty<bool>("LoadAgentsFromSnapshot",
85  false,
86  "Also load the agents from the snapshot into the WorkingMemory");
87 
88  defineOptionalProperty<std::string>(
89  "WorkingMemoryName", "WorkingMemory", "Name of WorkingMemory")
90  .setCaseInsensitive(true);
91  defineOptionalProperty<std::string>(
92  "CommonStorageName", "CommonStorage", "Name of CommonStorage")
93  .setCaseInsensitive(true);
94  defineOptionalProperty<std::string>(
95  "PriorKnowledgeName", "PriorKnowledge", "Name of PriorKnowledge")
96  .setCaseInsensitive(true);
97  defineOptionalProperty<std::string>(
98  "LongtermMemoryName", "LongtermMemory", "Name of LongtermMemory")
99  .setCaseInsensitive(true);
100 
101  defineOptionalProperty<std::string>("Scene.ObjectPackage",
102  "PriorKnowledgeData",
103  "Package to search for object models."
104  "Used when loading scenes from JSON files.");
105  defineOptionalProperty<std::string>("Scene.Path",
106  "",
107  "Path to one or multiple JSON scene file(s) to load.\n"
108  "Example: 'PriorKnowledgeData/scenes/MyScene.json'.\n"
109  "Multiple paths are separated by semicolons (';').\n"
110  "The package to load a scene from is automatically"
111  "extracted from its first path segment.");
112 
113  defineOptionalProperty<SimulatorType>("SimulationType",
114  SimulatorType::Bullet,
115  "Simulation type (Supported: Bullet, Kinematics, Mujoco)")
116  .map("bullet", SimulatorType::Bullet)
117  .map("kinematics", SimulatorType::Kinematics)
118  .map("mujoco", SimulatorType::Mujoco)
119  .setCaseInsensitive(true);
120 
121  for (int i = -1; i < MAX_INITIAL_ROBOT_COUNT - 1; ++i)
122  {
123  std::string postfix;
124  if (i >= 0)
125  {
126  postfix = "_" + ValueToString(i);
127  }
128 
129  defineOptionalProperty<std::string>(
130  "RobotFileName" + postfix,
131  "",
132  "Simox/VirtualRobot robot file name, e.g. robot_model.xml");
133  defineOptionalProperty<std::string>(
134  "RobotInstanceName" + postfix, "", "Name of this robot instance");
135  defineOptionalProperty<bool>(
136  "RobotIsStatic" + postfix,
137  false,
138  "A static robot does not move due to the physical environment (i.e. no gravity). \n"
139  "It is a static (but kinematic) object in the world.");
140  defineOptionalProperty<bool>("ReportRobotPose", false, "Report the global robot pose.");
141  defineOptionalProperty<bool>(
142  "RobotCollisionModel" + postfix, false, "Show the collision model of the robot.");
143  defineOptionalProperty<float>(
144  "InitialRobotPose.x" + postfix, 0.f, "x component of initial robot position (mm)");
145  defineOptionalProperty<float>(
146  "InitialRobotPose.y" + postfix, 0.f, "y component of initial robot position (mm)");
147  defineOptionalProperty<float>(
148  "InitialRobotPose.z" + postfix, 0.f, "z component of initial robot position (mm)");
149  defineOptionalProperty<float>("InitialRobotPose.roll" + postfix,
150  0.f,
151  "Initial robot pose: roll component of RPY angles (radian)");
152  defineOptionalProperty<float>("InitialRobotPose.pitch" + postfix,
153  0.f,
154  "Initial robot pose: pitch component of RPY angles (radian)");
155  defineOptionalProperty<float>("InitialRobotPose.yaw" + postfix,
156  0.f,
157  "Initial robot pose: yaw component of RPY angles (radian)");
158  defineOptionalProperty<std::string>(
159  "InitialRobotConfig" + postfix,
160  "",
161  "Initial robot config as comma separated list (RobotNodeName:value)");
162 
163  defineOptionalProperty<std::string>(
164  "InitialNamedRobotConfig" + postfix,
165  "",
166  "Initial named robot config, e.g., `Home`.");
167 
168  defineOptionalProperty<float>(
169  "RobotScaling" + postfix, 1.0f, "Scaling of the robot (1 = no scaling).");
170  defineOptionalProperty<double>(
171  "RobotControllerPID.p" + postfix, 10.0, "Setup robot controllers: PID paramter p.");
172  defineOptionalProperty<double>(
173  "RobotControllerPID.i" + postfix, 0.0, "Setup robot controllers: PID paramter i.");
174  defineOptionalProperty<double>(
175  "RobotControllerPID.d" + postfix, 0.0, "Setup robot controllers: PID paramter d.");
176  defineOptionalProperty<bool>(
177  "RobotSelfCollisions" + postfix,
178  false,
179  "If false, the robot bodies will not collide with other bodies of this \n"
180  "robot but with all other bodies in the world. \n"
181  "If true, the robot bodies will collide with all other bodies \n"
182  "(This needs an accurately modelled robot model without self collisions \n"
183  "if the robot does not move.)");
184  }
185  defineOptionalProperty<int>(
186  "FixedTimeStepLoopNrSteps",
187  10,
188  "The maximum number of internal simulation loops (fixed time step mode).\n"
189  "After max number of time steps, the remaining time is interpolated.");
190  defineOptionalProperty<int>(
191  "FixedTimeStepStepTimeMS",
192  16,
193  "The simulation's internal timestep (fixed time step mode). \n"
194  "This property determines the precision of the simulation. \n"
195  "Needs to be set to bigger values for slow PCs, but the simulation results might be bad. "
196  "Smaller value for higher precision, but slower calculation.");
197  defineOptionalProperty<int>("StepTimeMS", 25, "The simulation's time progress per step. ");
198  defineOptionalProperty<bool>(
199  "RealTimeMode",
200  false,
201  "If true the timestep is adjusted to the computing power of the host and \n"
202  "the time progress in real time. Can result in bad results on slow PCs. \n"
203  "The property StepTimeMS has then no effect. FixedTimeStepStepTimeMS needs \n"
204  "to be adjusted if real time cannot be achieved on slower PCs.");
205  defineOptionalProperty<float>(
206  "MaxRealTime",
207  1,
208  "If not zero and the real time mode is off, the simulator will not run \n"
209  "\tfaster than real-time * MaxRealTime even if the machine is fast enough \n"
210  "\t(a sleep is inserted to slow down the simulator - the precision remains unchanged).\n"
211  "\tIf set to 2, the simulator will not run faster than double speed. \n"
212  "\tIf set to 0.5, the simulator will not run faster than half speed. \n"
213  "\tIf set to 0, the simulator will run as fast as possible.\n");
214 
215  defineOptionalProperty<bool>(
216  "LogRobot",
217  false,
218  "Enable robot logging. If true, the complete robot state is logged to a file each step.");
219 
220  defineOptionalProperty<int>("ReportVisuFrequency",
221  30,
222  "How often should the visualization data be published. Value is "
223  "given in Hz. (0 to disable)");
224  defineOptionalProperty<std::string>(
225  "ReportVisuTopicName",
226  "SimulatorVisuUpdates",
227  "The topic on which the visualization updates are published.");
228  defineOptionalProperty<int>(
229  "ReportDataFrequency",
230  30,
231  "How often should the robot data be published. Value is given in Hz. (0 to disable)");
232  defineOptionalProperty<std::string>(
233  "FixedObjects",
234  "",
235  "Fixate objects or parts of a robot in the world. Comma separated list. \n"
236  "Define objects by their name, robots can be defined with robotName:RobotNodeName");
237  defineOptionalProperty<bool>("DrawCoMVisu",
238  true,
239  "If true the CoM of each robot is drawn as a circle on the ground "
240  "after each simulation cycle.");
241 }
242 
243 SimulatorPropertyDefinitions::~SimulatorPropertyDefinitions()
244 {
245 }
246 
247 Simulator::Simulator() : SimulatorInterface(), Component()
248 {
249  // create data object
250  //physicsWorld.reset(new BulletPhysicsWorld());
251  //physicsWorldData.reset(new ArmarXPhysicsWorldData());
252 
253  currentComTimeMS = 0;
254  currentSimTimeMS = 0;
255  currentSyncTimeMS = 0;
256  reportVisuTimeMS = 0;
257  timeServerSpeed = 1;
258 
259  publishContacts = false;
260  contactLayerName = "contacts";
261  lastPublishedContacts = 0;
262 }
263 
264 Simulator::~Simulator()
265 {
266  ARMARX_VERBOSE << "~Simulator";
267 }
268 
269 std::string
270 Simulator::getDefaultName() const
271 {
272  return "Simulator";
273 }
274 
275 void
276 Simulator::onInitComponent()
277 {
278  priorKnowledgeName = getProperty<std::string>("PriorKnowledgeName").getValue();
279  commonStorageName = getProperty<std::string>("CommonStorageName").getValue();
280  ARMARX_INFO << "Init component.";
281 
282  std::string snapshotName = getProperty<std::string>("LongtermMemory.SnapshotName").getValue();
283 
284  if (!snapshotName.empty())
285  {
286  ARMARX_LOG << "UsingProxy LongtermMemory for snapshot " << snapshotName;
287  usingProxy(getProperty<std::string>("WorkingMemoryName").getValue());
288  usingProxy(getProperty<std::string>("LongtermMemoryName").getValue());
289  usingProxy(getProperty<std::string>("CommonStorageName").getValue());
290  usingProxy(getProperty<std::string>("PriorKnowledgeName").getValue());
291  }
292  else
293  {
294  ARMARX_LOG << "Not using LongtermMemoryProxy";
295  }
296 
297  bool floorPlane = getProperty<bool>("FloorPlane").getValue();
298  std::string floorTexture = getProperty<std::string>("FloorTexture");
299 
300  SimulatorType simulatorType = SimulatorType::Bullet;
301  try
302  {
303  simulatorType = getProperty<SimulatorType>("SimulationType");
304  }
305  catch (...)
306  {
307  }
308 
309  offeringTopic("GlobalRobotPoseLocalization");
310  offeringTopic("SimulatorResetEvent");
311  getProperty(reportRobotPose, "ReportRobotPose");
312 
313  const int stepTimeMs = getProperty<int>("StepTimeMS");
314  const float maxRealTime = getProperty<float>("MaxRealTime");
315  ARMARX_CHECK_NONNEGATIVE(maxRealTime);
316 
317 
318  switch (simulatorType)
319  {
320  case SimulatorType::Kinematics:
321  {
322  KinematicsWorldPtr kw(new KinematicsWorld());
323  kw->initialize(stepTimeMs, maxRealTime, floorPlane, floorTexture);
324  physicsWorld = kw;
325  }
326  break;
327  case SimulatorType::Bullet:
328  {
329  int bulletFixedTimeStepMS = getProperty<int>("FixedTimeStepStepTimeMS").getValue();
330  int bulletFixedTimeStepMaxNrLoops =
331  getProperty<int>("FixedTimeStepLoopNrSteps").getValue();
332  BulletPhysicsWorldPtr bw(new BulletPhysicsWorld());
333  bw->initialize(stepTimeMs,
334  bulletFixedTimeStepMS,
335  bulletFixedTimeStepMaxNrLoops,
336  maxRealTime,
337  floorPlane,
338  floorTexture);
339  physicsWorld = bw;
340  }
341  break;
342  case SimulatorType::Mujoco:
343  {
344 #ifdef MUJOCO_PHYSICS_WORLD
345  MujocoPhysicsWorldPtr mw(new MujocoPhysicsWorld());
346  // initialize mujoco
347 
348  mw->initialize(stepTimeMs, floorPlane, floorTexture);
349 
350  physicsWorld = mw;
351 #else
352  ARMARX_ERROR << "Simulator type 'mujoco' is not supported, since simulator was built "
353  "without MujocoX."
354  << "\nGet the package MujocoX from: https://gitlab.com/h2t/mujoco .";
355  return;
356 #endif
357  }
358  break;
359  }
360 
361  timeserverProxy = new SimulatorTimeServerProxy(this);
362  getArmarXManager()->addObject(timeserverProxy);
363  offeringTopic(TIME_TOPIC_NAME);
364 }
365 
366 void
367 Simulator::addRobotsFromProperties()
368 {
369  for (int i = -1; i < MAX_INITIAL_ROBOT_COUNT - 1; ++i)
370  {
371  std::string postfix;
372  if (i >= 0)
373  {
374  postfix = "_" + ValueToString(i);
375  }
376  if (!getProperty<std::string>("RobotFileName" + postfix).isSet() ||
377  getProperty<std::string>("RobotFileName" + postfix).getValue().empty())
378  {
379  continue;
380  }
381 
382  std::string robotFile = getProperty<std::string>("RobotFileName" + postfix).getValue();
383  Eigen::Matrix4f gp = Eigen::Matrix4f::Identity();
384 
385  VirtualRobot::MathTools::rpy2eigen4f(
386  getProperty<float>("InitialRobotPose.roll" + postfix).getValue(),
387  getProperty<float>("InitialRobotPose.pitch" + postfix).getValue(),
388  getProperty<float>("InitialRobotPose.yaw" + postfix).getValue(),
389  gp);
390 
391  math::Helpers::Position(gp) =
392  Eigen::Vector3f(getProperty<float>("InitialRobotPose.x" + postfix).getValue(),
393  getProperty<float>("InitialRobotPose.y" + postfix).getValue(),
394  getProperty<float>("InitialRobotPose.z" + postfix).getValue());
395 
396  ARMARX_INFO << "Initial robot pose:\n" << gp;
397  std::map<std::string, float> initConfig;
398 
399  if(getProperty<std::string>("InitialNamedRobotConfig" + postfix).isSet())
400  {
401  std::string confStr =
402  getProperty<std::string>("InitialNamedRobotConfig" + postfix).getValue();
403 
404  ARMARX_INFO << "Using named robot config: " << QUOTED(confStr);
405 
406  ARMARX_CHECK_NOT_EMPTY(robotFile);
407  std::string robFileGlobal = robotFile;
408 
409  if (!ArmarXDataPath::getAbsolutePath(robotFile, robFileGlobal))
410  {
411  ARMARX_ERROR << "Could not find robot file " << robotFile;
412  }
413 
414  const auto robot = VirtualRobot::RobotIO::loadRobot(robFileGlobal, VirtualRobot::BaseIO::eStructure);
415  ARMARX_CHECK_NOT_NULL(robot) << "Could not load robot from file " << QUOTED(robFileGlobal);
416 
417  const std::optional<std::map<std::string, float>> namedConfig =
418  robot->getConfiguration(confStr);
419 
420  ARMARX_CHECK_NOT_NULL(namedConfig)
421  << "Could not find named robot config " << QUOTED(confStr) << " in robot file "
422  << QUOTED(robFileGlobal);
423 
424  initConfig = namedConfig.value();
425 
426  if (getProperty<std::string>("InitialRobotConfig" + postfix).isSet())
427  {
428  ARMARX_WARNING << "Ignoring InitialRobotConfig entry "
429  << getProperty<std::string>("InitialRobotConfig" + postfix).getValue()
430  << " because InitialNamedRobotConfig is set.";
431  }
432 
433  }
434  else if (getProperty<std::string>("InitialRobotConfig" + postfix).isSet())
435  {
436 
437  std::string confStr =
438  getProperty<std::string>("InitialRobotConfig" + postfix).getValue();
439  std::vector<std::string> confList = Split(confStr, ",", true, true);
440  for (auto& pos : confList)
441  {
442  ARMARX_INFO << "Processing '" << pos << "'";
443  std::vector<std::string> d = Split(pos, ":", true, true);
444  if (d.size() != 2)
445  {
446  ARMARX_WARNING << "Ignoring InitialRobotConfig entry " << pos;
447  continue;
448  }
449  std::string name = d.at(0);
450 
451  initConfig[name] = static_cast<float>(atof(d.at(1).c_str()));
452  ARMARX_INFO << "1:'" << name << "', 2:" << initConfig[name];
453  }
454  }
455 
456 
457  double pid_p = getProperty<double>("RobotControllerPID.p" + postfix).getValue();
458  double pid_i = getProperty<double>("RobotControllerPID.i" + postfix).getValue();
459  double pid_d = getProperty<double>("RobotControllerPID.d" + postfix).getValue();
460 
461  std::string robotInstanceName =
462  getProperty<std::string>("RobotInstanceName" + postfix).getValue();
463 
464  bool staticRobot = getProperty<bool>("RobotIsStatic" + postfix).getValue();
465  bool colModelRobot = getProperty<bool>("RobotCollisionModel" + postfix).getValue();
466  float scaling = getProperty<float>("RobotScaling" + postfix).getValue();
467  bool selfCollisions = getProperty<bool>("RobotSelfCollisions" + postfix);
468  if (!robotFile.empty())
469  {
470  std::string robFileGlobal = robotFile;
471 
472  if (!ArmarXDataPath::getAbsolutePath(robotFile, robFileGlobal))
473  {
474  ARMARX_ERROR << "Could not find robot file " << robotFile;
475  }
476  else
477  {
478  ARMARX_INFO << "Adding robot with name '" << robotInstanceName << "' from file "
479  << robFileGlobal;
480  addRobot(robotInstanceName,
481  robFileGlobal,
482  gp,
483  robotFile,
484  pid_p,
485  pid_i,
486  pid_d,
487  staticRobot,
488  scaling,
489  colModelRobot,
490  initConfig,
491  selfCollisions);
492  }
493  }
494  }
495 }
496 
497 void
498 Simulator::addSceneFromFile(const std::string& sceneFile)
499 {
500  std::string sf;
501  if (!ArmarXDataPath::getAbsolutePath(sceneFile, sf))
502  {
503  ARMARX_ERROR << "Could not find scene file " << sceneFile;
504  }
505  else
506  {
507  ARMARX_INFO << "Loading scene " << sf << "...";
508  physicsWorld->addScene(sf);
509  // setup robot topics
510  updateRobotTopics();
511  }
512 }
513 
514 void
515 Simulator::addSceneFromMemorySnapshot(const std::string& snapshotName)
516 {
517  requestFileManager();
518  if (!fileManager)
519  {
520  ARMARX_ERROR << "No connection to memory - cannot add snapshot!";
521  return;
522  }
523  memoryPrx = getProxy<WorkingMemoryInterfacePrx>(
524  getProperty<std::string>("WorkingMemoryName").getValue());
525 
526 
527  priorKnowledgePrx = getProxy<PriorKnowledgeInterfacePrx>(priorKnowledgeName);
528  longtermMemoryPrx = getProxy<LongtermMemoryInterfacePrx>(
529  getProperty<std::string>("LongtermMemoryName").getValue());
530  WorkingMemorySnapshotInterfacePrx snapshotInterfacePrx;
531 
532  if (longtermMemoryPrx && priorKnowledgePrx && memoryPrx)
533  {
534  ARMARX_LOG << "Loading snapshot " << snapshotName;
535 
536  try
537  {
538  snapshotInterfacePrx =
539  longtermMemoryPrx->getWorkingMemorySnapshotListSegment()->openSnapshot(
540  snapshotName);
541  if (getProperty<bool>("LoadSnapshotToWorkingMemory").getValue())
542  {
543  if (snapshotInterfacePrx)
544  {
545  longtermMemoryPrx->loadWorkingMemorySnapshot(snapshotName, memoryPrx);
546  }
547  else
548  {
549  ARMARX_ERROR << "Could not get snapshot segment from longterm memory...";
550  }
551  }
552  }
553  catch (memoryx::SnapshotNotFoundException&)
554  {
555  ARMARX_ERROR << "Could not find snapshot: " << snapshotName;
556  }
557  catch (...)
558  {
559  ARMARX_ERROR << "Failed to load snapshot with name:" << snapshotName;
560  }
561  }
562  else
563  {
564  ARMARX_ERROR << "Could not get PriorKnowledge/LongtermMemory/WorkingMemory Proxy";
565  }
566 
567  if (longtermMemoryPrx && priorKnowledgePrx && fileManager && snapshotInterfacePrx)
568  {
569  ARMARX_LOG << "Found and opened snapshot " << snapshotName;
570  addSnapshot(snapshotInterfacePrx);
571  }
572  else
573  {
574  ARMARX_ERROR << "Failed to open snapshot " << snapshotName;
575  }
576 }
577 
578 void
579 Simulator::addSceneFromSdfFile(const std::string& scenePath)
580 {
581  std::string scenePackage = simox::alg::split(scenePath, "/").at(0);
582  CMakePackageFinder cmakeFinder(scenePackage);
583  if (!cmakeFinder.packageFound())
584  {
585  ARMARX_WARNING << "Could not find CMake package " << scenePackage << ".";
586  return;
587  }
588 
589  std::filesystem::path dataDir = cmakeFinder.getDataDir();
590  std::filesystem::path path = dataDir / scenePath;
591 
592 
593  sdf::SDFPtr sdfFile(new sdf::SDF());
594  sdf::init(sdfFile);
595 
596  std::filesystem::path modelDirectory = path.parent_path();
597 
598  const std::string modelURI = "model://";
599 
600  auto findFileCallback = [this, &modelURI](const std::string& uri) -> std::string
601  {
602  std::stringstream ss;
603  ss << "Find '" << uri << "' ... -> ";
604 
605  const std::string classIDStr = simox::alg::remove_prefix(uri, modelURI);
606  const ObjectID classID = ObjectID::FromString(classIDStr);
607 
608  if (std::optional<ObjectInfo> info = objectFinder.findObject(classID))
609  {
610  if (std::optional<PackageFileLocation> modelPath = info->getModel())
611  {
612  std::filesystem::path result = modelPath->absolutePath.parent_path();
613  ss << result;
614  ARMARX_VERBOSE << ss.str();
615 
616  return result;
617  }
618  }
619  ss << uri;
620  ARMARX_VERBOSE << ss.str();
621 
622  return uri;
623  };
624 
625  sdf::ParserConfig parser;
626  parser.AddURIPath(modelURI, modelDirectory.string());
627  parser.SetFindCallback(findFileCallback);
628 
629  sdf::Root root;
630  sdf::Errors errors = root.Load(path.string(), parser);
631 
632  for (auto const& error : errors)
633  {
634  ARMARX_ERROR << "Error on reading SDF: " << error;
635  }
636 
637  if (!errors.empty())
638  {
639  ARMARX_WARNING << "Could not load SDF from '" << path << "'";
640  return;
641  }
642 
643  if (root.WorldCount() == 0)
644  {
645  ARMARX_WARNING << "SDF does not contain a world";
646  return;
647  }
648 
649  if (root.WorldCount() > 1)
650  {
651  ARMARX_WARNING << "SDF contains multiple worlds";
652  return;
653  }
654 
655  sdf::World* world = root.WorldByIndex(0);
656  auto addSceneObject = [&](sdf::Model* model)
657  {
658  const std::string name = model->Name();
659  if (physicsWorld->hasObject(name))
660  {
661  ARMARX_WARNING << "Object with name \"" << name << "\" already exists.";
662  return;
663  }
664 
665  const std::string uri = model->Uri();
666  if (uri.empty())
667  {
668  ARMARX_WARNING << "No URI set for object \"" << name << "\" in SDF.";
669  return;
670  }
671 
672 
673  // Not clear where the result of the callback called during loading is stored.
674  const std::filesystem::path modelPath = findFileCallback(uri);
675 
676  gz::math::Pose3d pose = model->RawPose();
677 
678  const float meterToMillimeter = 1000.0F;
679 
680  Eigen::Vector3f position;
681  position.x() = static_cast<float>(pose.Pos().X() * meterToMillimeter);
682  position.y() = static_cast<float>(pose.Pos().Y() * meterToMillimeter);
683  position.z() = static_cast<float>(pose.Pos().Z() * meterToMillimeter);
684 
685  Eigen::Quaternionf orientation;
686  orientation.x() = static_cast<float>(pose.Rot().X());
687  orientation.y() = static_cast<float>(pose.Rot().Y());
688  orientation.z() = static_cast<float>(pose.Rot().Z());
689  orientation.w() = static_cast<float>(pose.Rot().W());
690 
691  auto isStatic = model->Static();
692 
693  std::string robotFilename = modelPath.filename().generic_string() + ".urdf";
694  std::filesystem::path robotPath = modelPath / robotFilename;
695 
696  std::string xmlFilename = modelPath.filename().generic_string() + ".xml";
697  std::filesystem::path xmlPath = modelPath / xmlFilename;
698 
699  if (std::filesystem::exists(xmlPath))
700  {
701  ARMARX_INFO << "Loading object '" << name << "' from object XML file " << xmlPath;
702  VirtualRobot::ManipulationObjectPtr simoxObject =
703  VirtualRobot::ObjectIO::loadManipulationObject(xmlPath.string());
704 
705  simoxObject->setGlobalPose(simox::math::pose(position, orientation));
706 
707  VirtualRobot::SceneObject::Physics::SimulationType simType =
708  isStatic ? VirtualRobot::SceneObject::Physics::eKinematic
709  : VirtualRobot::SceneObject::Physics::eDynamic;
710 
711  simoxObject->setSimulationType(simType);
712  simoxObject->setFilename(xmlPath.string());
713  simoxObject->setName(name);
714 
715  physicsWorld->addObstacle(
716  simoxObject, simType, xmlPath.string(), name, {}, scenePackage);
717  }
718  else if (std::filesystem::exists(robotPath))
719  {
720  ARMARX_INFO << "Loading object '" << name << "' from robot URDF file " << xmlPath;
721  VirtualRobot::RobotPtr robot = VirtualRobot::RobotIO::loadRobot(robotPath);
722 
723  robot->setGlobalPose(simox::math::pose(position, orientation));
724 
725  VirtualRobot::SceneObject::Physics::SimulationType simType =
726  isStatic ? VirtualRobot::SceneObject::Physics::eKinematic
727  : VirtualRobot::SceneObject::Physics::eDynamic;
728 
729  robot->setSimulationType(simType);
730  robot->setFilename(robotPath.string());
731  robot->setName(name);
732 
733  physicsWorld->addRobot(robot, 10.0, 0.0, 0.0, robotPath.string(), isStatic);
734  }
735  else
736  {
737  ARMARX_INFO
738  << "Not adding object '" << name
739  << "' because neither object XML nor robot URDF file exist. (Tried object XML = "
740  << xmlPath << " and robot URDF = " << robotPath << ")";
741  }
742  };
743 
744  for (size_t index = 0; index < world->ModelCount(); index++)
745  {
746  addSceneObject(world->ModelByIndex(index));
747  }
748 }
749 
750 void
751 Simulator::addSceneFromJsonFile(const std::string& scenePath)
752 {
753  std::string mutScenePath = scenePath;
754  if (!simox::alg::ends_with(mutScenePath, ".json"))
755  {
756  mutScenePath += ".json";
757  }
758 
759  std::string scenePackage = simox::alg::split(scenePath, "/").at(0);
760  CMakePackageFinder cmakeFinder(scenePackage);
761  if (!cmakeFinder.packageFound())
762  {
763  ARMARX_WARNING << "Could not find CMake package " << scenePackage << ".";
764  return;
765  }
766 
767  std::filesystem::path dataDir = cmakeFinder.getDataDir();
768  std::filesystem::path path = dataDir / scenePath;
769 
770  armarx::objects::Scene scene;
771  try
772  {
773  scene = simox::json::read<armarx::objects::Scene>(path);
774  }
775  catch (const simox::json::error::JsonError& e)
776  {
777  ARMARX_WARNING << "Loading scene snapshot failed: \n" << e.what();
778  return;
779  }
780 
781  std::map<ObjectID, int> idCounters;
782  for (const auto& object : scene.objects)
783  {
784  if (simox::alg::starts_with(object.className, "#"))
785  {
786  // we ignore stuff that is out commented in json
787  continue;
788  }
789 
790  addJsonSceneObject(object, idCounters);
791  }
792 }
793 
794 void
795 Simulator::addJsonSceneObject(const armarx::objects::SceneObject object,
796  std::map<ObjectID, int>& idCounters)
797 {
798  const ObjectID classID = object.getClassID();
799  const std::string name =
800  object.getObjectID()
801  .withInstanceName(object.instanceName.empty() ? std::to_string(idCounters[classID]++)
802  : object.instanceName)
803  .str();
804 
805  if (physicsWorld->hasObject(name))
806  {
807  ARMARX_INFO << "Object with name \"" << name << "\" already exists.";
808  return;
809  }
810 
811  ARMARX_INFO << "Adding object with name \"" << name << "\" to scene.";
812 
813  std::optional<ObjectInfo> objectInfo = objectFinder.findObject(classID);
814  VirtualRobot::ManipulationObjectPtr simoxObject =
815  VirtualRobot::ObjectIO::loadManipulationObject(objectInfo->simoxXML().absolutePath);
816  simoxObject->setGlobalPose(simox::math::pose(object.position, object.orientation));
817 
818  VirtualRobot::SceneObject::Physics::SimulationType simType;
819  if (object.isStatic.has_value())
820  {
821  simType = *object.isStatic ? VirtualRobot::SceneObject::Physics::eKinematic
822  : VirtualRobot::SceneObject::Physics::eDynamic;
823  }
824  else
825  {
826  simType = VirtualRobot::SceneObject::Physics::eKinematic;
827  }
828 
829  PackageFileLocation simoxXml = objectInfo->simoxXML();
830  simoxObject->setSimulationType(simType);
831  simoxObject->setFilename(simoxXml.absolutePath);
832  simoxObject->setName(name);
833 
834  physicsWorld->addObstacle(
835  simoxObject, simType, simoxXml.relativePath, object.className, {}, simoxXml.package);
836 }
837 
838 void
839 Simulator::fixateRobotNode(const std::string& robotName, const std::string& robotNodeName)
840 {
841  ARMARX_VERBOSE << "Fixing robot node" << robotName << "." << robotNodeName;
842  physicsWorld->setRobotNodeSimType(
843  robotName, robotNodeName, VirtualRobot::SceneObject::Physics::eStatic);
844 }
845 
846 void
847 Simulator::fixateObject(const std::string& objectName)
848 {
849  ARMARX_VERBOSE << "Fixing object" << objectName;
850  physicsWorld->setObjectSimType(objectName, VirtualRobot::SceneObject::Physics::eStatic);
851 }
852 
853 void
854 Simulator::initializeData()
855 {
856  ARMARX_INFO << "Initializing simulator...";
857 
858  addRobotsFromProperties();
859 
860  std::string sceneFile = getProperty<std::string>("SimoxSceneFileName").getValue();
861  if (!sceneFile.empty())
862  {
863  addSceneFromFile(sceneFile);
864  }
865 
866  std::string snapshotName = getProperty<std::string>("LongtermMemory.SnapshotName").getValue();
867  if (!snapshotName.empty())
868  {
869  addSceneFromMemorySnapshot(snapshotName);
870  }
871 
872  std::string objectPackage = getProperty<std::string>("Scene.ObjectPackage").getValue();
873  objectFinder = ObjectFinder(objectPackage);
874 
875  std::string scenePaths = getProperty<std::string>("Scene.Path").getValue();
876  if (!scenePaths.empty())
877  {
878  bool trim = true;
879  for (const std::string& scenePath : simox::alg::split(scenePaths, ";", trim))
880  {
881  std::filesystem::path path = scenePath;
882  std::string sdfExtension = ".sdf";
883 
884  if (path.has_extension() && path.extension().string() == sdfExtension)
885  {
886  ARMARX_INFO << "Loading scene from SDF file '" << scenePath << "' ...";
887  addSceneFromSdfFile(scenePath);
888  }
889  else
890  {
891  ARMARX_INFO << "Loading scene from JSON file '" << scenePath << "' ...";
892  addSceneFromJsonFile(scenePath);
893  }
894  }
895  }
896 
897  //check for fixed objects
898  std::string fixedObjects = getProperty<std::string>("FixedObjects").getValue();
899  if (!fixedObjects.empty())
900  {
901  std::vector<std::string> objects = Split(fixedObjects, ",;");
902  for (auto o : objects)
903  {
904  std::vector<std::string> rob = Split(o, ":");
905  if (rob.size() == 2)
906  {
907  // robot
908  fixateRobotNode(rob.at(0), rob.at(1));
909  }
910  else
911  {
912  // object
913  fixateObject(o);
914  }
915  }
916  }
917 
918  physicsWorld->resetSimTime();
919 }
920 
921 SimulatedRobotState
922 Simulator::stateFromRobotInfo(RobotInfo const& r, IceUtil::Time timestamp)
923 {
924  SimulatedRobotState state;
925 
926  state.hasRobot = true;
927  state.timestampInMicroSeconds = timestamp.toMicroSeconds();
928 
929  state.jointAngles = std::move(r.jointAngles);
930  state.jointVelocities = std::move(r.jointVelocities);
931  state.jointTorques = std::move(r.jointTorques);
932 
933  state.pose = new Pose(r.pose);
934  state.linearVelocity = new Vector3(r.linearVelocity);
935  state.angularVelocity = new Vector3(r.angularVelocity);
936 
937  for (auto& forceTorqueSensor : r.forceTorqueSensors)
938  {
939  if (forceTorqueSensor.enable)
940  {
941  ForceTorqueData& ftData = state.forceTorqueValues.emplace_back();
942  ftData.force = new Vector3(forceTorqueSensor.currentForce);
943  ftData.torque = new Vector3(forceTorqueSensor.currentTorque);
944  ftData.sensorName = forceTorqueSensor.sensorName;
945  ftData.nodeName = forceTorqueSensor.robotNodeName;
946  }
947  }
948 
949  return state;
950 }
951 
952 memoryx::GridFileManagerPtr
953 Simulator::requestFileManager()
954 {
955  if (fileManager)
956  {
957  ARMARX_INFO << deactivateSpam(10)
958  << "Was able to find a reference to the CommonStorage. Continue with existing "
959  "connection to memory.";
960  return fileManager;
961  }
962  CommonStorageInterfacePrx commonStoragePrx =
963  getProxy<CommonStorageInterfacePrx>(commonStorageName, false, "", false);
964 
965  if (commonStoragePrx)
966  {
967  ARMARX_INFO << deactivateSpam(10)
968  << "Was able to find a reference to the CommonStorage. Reset GridFSManager and "
969  "continue with memory.";
970  fileManager.reset(new GridFileManager(commonStoragePrx));
971  }
972  else
973  {
974  ARMARX_INFO << deactivateSpam(10)
975  << "Could not get CommonStorage Proxy - continuing without memory";
976  }
977  return fileManager;
978 }
979 
980 void
981 Simulator::onConnectComponent()
982 {
983  ARMARX_VERBOSE << "Connecting ArmarX Simulator";
984 
985  getTopic(globalRobotLocalization, "GlobalRobotPoseLocalization");
986  getTopic(simulatorResetEventTopic, "SimulatorResetEvent");
987 
988  entityDrawerPrx = getTopic<memoryx::EntityDrawerInterfacePrx>("DebugDrawerUpdates");
989 
990  int hzVisu = getProperty<int>("ReportVisuFrequency").getValue();
991  int hzData = getProperty<int>("ReportDataFrequency").getValue();
992  std::string top = getProperty<std::string>("ReportVisuTopicName").getValue();
993 
994  initializeData();
995 
996  ARMARX_DEBUG << "Starting periodic simulation task in ArmarX Simulator";
997 
998  timeTopicPrx = ManagedIceObject::getTopic<TimeServerListenerPrx>(TIME_TOPIC_NAME);
999  start();
1000 
1001  ARMARX_DEBUG << "Starting periodic visualization report task in ArmarX Simulator";
1002 
1003  if (reportVisuTask)
1004  {
1005  reportVisuTask->stop();
1006  }
1007 
1008  if (hzVisu > 0)
1009  {
1010  offeringTopic(top);
1011  ARMARX_INFO << "Connecting to topic " << top;
1012  simulatorVisuUpdateListenerPrx = getTopic<SimulatorListenerInterfacePrx>(top);
1013 
1014  ARMARX_DEBUG << "Creating report visu task";
1015  int cycleTime = 1000 / hzVisu;
1016  reportVisuTask = new PeriodicTask<Simulator>(
1017  this, &Simulator::reportVisuLoop, cycleTime, false, "ArmarXSimulatorReportVisuTask");
1018  reportVisuTask->start();
1019  reportVisuTask->setDelayWarningTolerance(100);
1020  }
1021 
1022  ARMARX_DEBUG << "Starting periodic data report task in ArmarX Simulator";
1023 
1024  if (reportDataTask)
1025  {
1026  reportDataTask->stop();
1027  }
1028 
1029  if (hzData > 0)
1030  {
1031  int cycleTime = 1000 / hzData;
1032  reportDataTask = new PeriodicTask<Simulator>(
1033  this, &Simulator::reportDataLoop, cycleTime, false, "ArmarXSimulatorReportDataTask");
1034  reportDataTask->start();
1035  reportDataTask->setDelayWarningTolerance(100);
1036  }
1037 
1038  ARMARX_DEBUG << "finished connection";
1039 }
1040 
1041 void
1042 Simulator::onDisconnectComponent()
1043 {
1044  ARMARX_VERBOSE << "disconnecting Simulator";
1045 
1046  if (reportDataTask)
1047  {
1048  reportDataTask->stop();
1049  }
1050 
1051  if (reportVisuTask)
1052  {
1053  reportVisuTask->stop();
1054  }
1055 
1056  shutdownSimulationLoop();
1057 
1058  resetData();
1059 }
1060 
1061 void
1062 Simulator::onExitComponent()
1063 {
1064  ARMARX_VERBOSE << "exiting Simulator";
1065 
1066  if (reportDataTask)
1067  {
1068  ARMARX_VERBOSE << "stopping report task";
1069  reportDataTask->stop();
1070  ARMARX_VERBOSE << "stopping report task...Done!";
1071  }
1072 
1073  if (reportVisuTask)
1074  {
1075  ARMARX_VERBOSE << "stopping visu task";
1076  reportVisuTask->stop();
1077  ARMARX_VERBOSE << "stopping visu task...Done!";
1078  }
1079 
1080  shutdownSimulationLoop();
1081  timeserverProxy =
1082  nullptr; // delete the timeserver proxy as it holds a handle to this object, preventing it from being deleted
1083  ARMARX_VERBOSE << "exiting Simulator...Done";
1084 }
1085 
1086 void
1087 Simulator::shutdownSimulationLoop()
1088 {
1089  ARMARX_VERBOSE << "shutting down simulation loop";
1090  simulatorThreadRunning = true;
1091  simulatorThreadShutdown = true;
1092  condSimulatorThreadRunning.notify_all();
1093  ARMARX_VERBOSE << "---- get thread control";
1094  IceUtil::ThreadControl threadControl = getThreadControl();
1095  ARMARX_VERBOSE << "---- get thread control...Done!";
1096  ARMARX_VERBOSE << "---- joining thread control";
1097  if (isAlive())
1098  {
1099  threadControl.join();
1100  }
1101  ARMARX_VERBOSE << "---- joining thread control...Done!";
1102  ARMARX_VERBOSE << "shutting down simulation loop...Done!";
1103 }
1104 
1105 void
1106 Simulator::resetData(bool clearWorkingMemory)
1107 {
1108  ARMARX_INFO << "Deleting data";
1109 
1110  auto lockEngine = physicsWorld->getScopedEngineLock(__FUNCTION__);
1111  auto lockData = physicsWorld->getScopedSyncLock(__FUNCTION__);
1112 
1113  {
1114  ARMARX_INFO << "Clearing class map...";
1115  classInstanceMap.clear();
1116 
1117  physicsWorld->resetData();
1118  }
1119 
1120  if (clearWorkingMemory && memoryPrx)
1121  {
1122  ARMARX_INFO << "Clearing working memory...";
1123 
1124  try
1125  {
1126  memoryPrx->clear();
1127  }
1128  catch (...)
1129  {
1130  ARMARX_WARNING << "Clearing working memory failed...";
1131  }
1132  }
1133  ARMARX_VERBOSE << "Deleting data...Done!";
1134 }
1135 
1136 bool
1137 Simulator::addSnapshot(WorkingMemorySnapshotInterfacePrx snapshotInterfacePrx)
1138 {
1139  ARMARX_VERBOSE << "Adding snapshot from memory";
1140 
1141  if (!snapshotInterfacePrx || !priorKnowledgePrx)
1142  {
1143  ARMARX_ERROR << "Null data?!";
1144  return false;
1145  }
1146 
1147  const std::string segmentName = "objectInstances";
1148 
1149  try
1150  {
1151  PersistentEntitySegmentBasePrx segInstances = snapshotInterfacePrx->getSegment(segmentName);
1152 
1153  if (!segInstances)
1154  {
1155  ARMARX_ERROR << "Segment not found in snapshot: " << segmentName;
1156  return false;
1157  }
1158 
1159  // get classes for IV file loading (instances refer to classes, and classes store IV files)
1160  PersistentObjectClassSegmentBasePrx classesSegmentPrx =
1161  priorKnowledgePrx->getObjectClassesSegment();
1162 
1163  if (!classesSegmentPrx)
1164  {
1165  ARMARX_ERROR << "Classes Segment not found in PriorKnowledge";
1166  return false;
1167  }
1168 
1169 
1170  EntityIdList ids = segInstances->getAllEntityIds();
1171 
1172  int c = 0;
1173 
1174  for (EntityIdList::const_iterator it = ids.begin(); it != ids.end(); ++it)
1175  {
1176  EntityBasePtr snapEntity = segInstances->getEntityById(*it);
1177  ObjectInstancePtr snapInstance = ObjectInstancePtr::dynamicCast(snapEntity);
1178  std::string id = snapInstance->getId();
1179  bool ok = snapInstance->hasAttribute("classes");
1180 
1181  if (!ok)
1182  {
1183  ARMARX_ERROR << "SnapInstance " << id
1184  << " has no classes attribute, could not load iv files, skipping";
1185  continue;
1186  }
1187 
1188  std::string classesName = snapInstance->getAttributeValue("classes")->getString();
1189 
1190  EntityBasePtr classesEntity = classesSegmentPrx->getEntityByName(classesName);
1191 
1192  if (!classesEntity)
1193  {
1194  ARMARX_ERROR << "Classes Segment does not know class with name : " << classesName;
1195  continue;
1196  }
1197 
1198  ObjectClassPtr objectClass = ObjectClassPtr::dynamicCast(classesEntity);
1199 
1200  if (!objectClass)
1201  {
1202  ARMARX_ERROR << "Could not cast class entity : " << classesName;
1203  continue;
1204  }
1205 
1206 
1207  Eigen::Matrix4f m = Eigen::Matrix4f::Identity();
1208 
1209  // Position
1210  FramedPositionPtr objPos = snapInstance->getPosition();
1211 
1212  if (objPos)
1213  {
1214  m.block(0, 3, 3, 1) = objPos->toEigen();
1215  }
1216 
1217  // Orientation
1218  FramedOrientationPtr objOrient = snapInstance->getOrientation();
1219 
1220  if (objOrient)
1221  {
1222  m.block(0, 0, 3, 3) = objOrient->toEigen();
1223  }
1224 
1225  PosePtr poseGlobal(new Pose(m));
1226  memoryx::EntityWrappers::SimoxObjectWrapperPtr sw =
1227  objectClass->addWrapper(new EntityWrappers::SimoxObjectWrapper(fileManager));
1228  VirtualRobot::ManipulationObjectPtr mo = sw->getManipulationObject()->clone(
1229  sw->getManipulationObject()
1230  ->getName()); // maybe not needed: cloning the object to avoid changing content of priorKnowledge
1231 
1232  if (mo)
1233  {
1234  std::string instanceName = snapInstance->getName();
1235  int i = 2;
1236  while (hasObject(instanceName))
1237  {
1238  instanceName = snapInstance->getName() + "_" + to_string(i);
1239  i++;
1240  }
1241  if (instanceName != snapInstance->getName())
1242  {
1243  ARMARX_INFO << "Object instance with name '" << snapInstance->getName()
1244  << "'' already exists - using '" << instanceName << "'";
1245  }
1246  bool isStatic =
1247  mo->getSimulationType() == VirtualRobot::SceneObject::Physics::eStatic;
1248  addObject(objectClass, instanceName, poseGlobal, isStatic);
1249  }
1250  }
1251 
1252  ARMARX_LOG << "Added " << c << " objects";
1253  }
1254  catch (...)
1255  {
1256  ARMARX_WARNING << "addSnapshot failed...";
1257  return false;
1258  }
1259  if (getProperty<bool>("LoadAgentsFromSnapshot").getValue())
1260  {
1261  return loadAgentsFromSnapshot(snapshotInterfacePrx);
1262  }
1263  else
1264  {
1265  return true;
1266  }
1267 }
1268 
1269 bool
1270 Simulator::loadAgentsFromSnapshot(WorkingMemorySnapshotInterfacePrx snapshotInterfacePrx)
1271 {
1272  const std::string segmentName = "agentInstances";
1273  bool result = true;
1274  try
1275  {
1276  PersistentEntitySegmentBasePrx segInstances = snapshotInterfacePrx->getSegment(segmentName);
1277 
1278  if (!segInstances)
1279  {
1280  ARMARX_ERROR << "Segment not found in snapshot: " << segmentName;
1281  return false;
1282  }
1283 
1284  EntityIdList ids = segInstances->getAllEntityIds();
1285 
1286 
1287  for (EntityIdList::const_iterator it = ids.begin(); it != ids.end(); ++it)
1288  {
1289  EntityBasePtr snapEntity = segInstances->getEntityById(*it);
1290  AgentInstancePtr snapAgent = AgentInstancePtr::dynamicCast(snapEntity);
1291 
1292  auto agentName = snapAgent->getName();
1293  std::string agentModelFile = snapAgent->getAgentFilePath();
1294  std::string absModelFile;
1295  if (hasRobot(agentName))
1296  {
1297  ARMARX_INFO << "Robot with name '" << agentName << "' already exists.";
1298  continue;
1299  }
1300  if (!ArmarXDataPath::getAbsolutePath(agentModelFile, absModelFile))
1301  {
1302  ARMARX_WARNING << "Could not load agent '" << agentName
1303  << "'- file not found: " << agentModelFile;
1304  result = false;
1305  continue;
1306  }
1307 
1308 
1309  if (addRobot(agentName, absModelFile, snapAgent->getPose()->toEigen(), agentModelFile)
1310  .empty())
1311  {
1312  result = false;
1313  }
1314  }
1315  }
1316  catch (...)
1317  {
1318  handleExceptions();
1319  }
1320 
1321  return result;
1322 }
1323 
1324 void
1325 Simulator::actuateRobotJoints(const std::string& robotName,
1326  const NameValueMap& angles,
1327  const NameValueMap& velocities,
1328  const Ice::Current&)
1329 {
1330  physicsWorld->actuateRobotJoints(robotName, angles, velocities);
1331 }
1332 
1333 void
1334 Simulator::actuateRobotJointsPos(const std::string& robotName,
1335  const NameValueMap& angles,
1336  const Ice::Current&)
1337 {
1338  physicsWorld->actuateRobotJointsPos(robotName, angles);
1339 }
1340 
1341 void
1342 Simulator::actuateRobotJointsVel(const std::string& robotName,
1343  const NameValueMap& velocities,
1344  const Ice::Current&)
1345 {
1346  physicsWorld->actuateRobotJointsVel(robotName, velocities);
1347 }
1348 
1349 void
1350 Simulator::actuateRobotJointsTorque(const std::string& robotName,
1351  const NameValueMap& torques,
1352  const Ice::Current&)
1353 {
1354  physicsWorld->actuateRobotJointsTorque(robotName, torques);
1355 }
1356 
1357 void
1358 Simulator::setRobotPose(const std::string& robotName,
1359  const PoseBasePtr& globalPose,
1360  const Ice::Current&)
1361 {
1362  VirtualRobot::MathTools::Quaternion qa;
1363  qa.x = globalPose->orientation->qx;
1364  qa.y = globalPose->orientation->qy;
1365  qa.z = globalPose->orientation->qz;
1366  qa.w = globalPose->orientation->qw;
1367  Eigen::Matrix4f gp = VirtualRobot::MathTools::quat2eigen4f(qa);
1368  gp(0, 3) = globalPose->position->x;
1369  gp(1, 3) = globalPose->position->y;
1370  gp(2, 3) = globalPose->position->z;
1371  physicsWorld->setRobotPose(robotName, gp);
1372 }
1373 
1374 void
1375 Simulator::applyForceRobotNode(const std::string& robotName,
1376  const std::string& robotNodeName,
1377  const Vector3BasePtr& force,
1378  const Ice::Current&)
1379 {
1380  Vector3Ptr v3p = Vector3Ptr::dynamicCast(force);
1381  physicsWorld->applyForceRobotNode(robotName, robotNodeName, v3p->toEigen());
1382 }
1383 
1384 void
1385 Simulator::applyTorqueRobotNode(const std::string& robotName,
1386  const std::string& robotNodeName,
1387  const Vector3BasePtr& torque,
1388  const Ice::Current&)
1389 {
1390  Vector3Ptr v3p = Vector3Ptr::dynamicCast(torque);
1391  physicsWorld->applyTorqueRobotNode(robotName, robotNodeName, v3p->toEigen());
1392 }
1393 
1394 void
1395 Simulator::applyForceObject(const std::string& objectName,
1396  const Vector3BasePtr& force,
1397  const Ice::Current&)
1398 {
1399  Vector3Ptr v3p = Vector3Ptr::dynamicCast(force);
1400  physicsWorld->applyForceObject(objectName, v3p->toEigen());
1401 }
1402 
1403 void
1404 Simulator::applyTorqueObject(const std::string& objectName,
1405  const Vector3BasePtr& torque,
1406  const Ice::Current&)
1407 {
1408  Vector3Ptr v3p = Vector3Ptr::dynamicCast(torque);
1409  physicsWorld->applyTorqueObject(objectName, v3p->toEigen());
1410 }
1411 
1412 void
1413 Simulator::addObject(const memoryx::ObjectClassBasePtr& objectClassBase,
1414  const std::string& instanceName,
1415  const PoseBasePtr& globalPose,
1416  bool isStatic,
1417  const Ice::Current&)
1418 {
1419  ARMARX_VERBOSE << "Add object from memory " << instanceName;
1420  if (!requestFileManager())
1421  {
1422  ARMARX_WARNING << "Cannot add object - no connection to common storage";
1423  return;
1424  }
1425  auto lockEngine = physicsWorld->getScopedEngineLock(__FUNCTION__);
1426  auto lockData = physicsWorld->getScopedSyncLock(__FUNCTION__);
1427 
1428  try
1429  {
1430 
1431  ObjectClassPtr objectClass = ObjectClassPtr::dynamicCast(objectClassBase);
1432 
1433  if (physicsWorld->hasObject(instanceName))
1434  {
1435  ARMARX_ERROR << "Object with instance name \"" << instanceName << "\" already exists";
1436  return;
1437  }
1438 
1439  if (!objectClass)
1440  {
1441  ARMARX_ERROR << "Invalid object class; could not create object with instance name "
1442  << instanceName;
1443  return;
1444  }
1445 
1446  memoryx::EntityWrappers::SimoxObjectWrapperPtr sw =
1447  objectClass->addWrapper(new EntityWrappers::SimoxObjectWrapper(fileManager));
1448 
1449  VirtualRobot::ManipulationObjectPtr mo =
1450  sw->getManipulationObject(); //->clone(sw->getManipulationObject()->getName()); // maybe not needed: cloning the object to avoid changing content of priorKnowledge
1451 
1452  if (!mo)
1453  {
1454  ARMARX_ERROR << "Could not retrieve manipulation object of object class "
1455  << objectClass->getName();
1456  return;
1457  }
1458 
1459  //ARMARX_INFO << "Filename mo:" << mo->getVisualization()->getFilename();
1460  //ARMARX_INFO << "Filename sw:" << sw->getManipulationObjectFileName();
1461 
1462  mo->setName(instanceName);
1463 
1464  ARMARX_LOG << "Adding manipulation object " << mo->getName() << " of class "
1465  << objectClass->getName();
1466  classInstanceMap[objectClass->getName()].push_back(instanceName);
1467 
1468  VirtualRobot::MathTools::Quaternion qa;
1469  qa.x = globalPose->orientation->qx;
1470  qa.y = globalPose->orientation->qy;
1471  qa.z = globalPose->orientation->qz;
1472  qa.w = globalPose->orientation->qw;
1473  Eigen::Matrix4f gp = VirtualRobot::MathTools::quat2eigen4f(qa);
1474  gp(0, 3) = globalPose->position->x;
1475  gp(1, 3) = globalPose->position->y;
1476  gp(2, 3) = globalPose->position->z;
1477  mo->setGlobalPose(gp);
1478  VirtualRobot::SceneObject::Physics::SimulationType simType =
1479  (isStatic) ? VirtualRobot::SceneObject::Physics::eKinematic
1480  : VirtualRobot::SceneObject::Physics::eDynamic;
1481  physicsWorld->addObstacle(mo, simType, std::string(), objectClassBase->getName());
1482  }
1483  catch (...)
1484  {
1485  ARMARX_WARNING << "addObject failed: " << GetHandledExceptionString();
1486  }
1487 }
1488 
1489 void
1490 Simulator::addObjectFromFile(const armarx::data::PackagePath& packagePath,
1491  const std::string& instanceName,
1492  const PoseBasePtr& globalPose,
1493  bool isStatic,
1494  const Ice::Current&)
1495 {
1496  ARMARX_INFO << "Add object from packagePath " << packagePath << ": " << instanceName;
1497  //if (!requestFileManager())
1498  //{
1499  // ARMARX_WARNING << "Cannot add object - no connection to common storage";
1500  // return;
1501  //}
1502  auto lockEngine = physicsWorld->getScopedEngineLock(__FUNCTION__);
1503  auto lockData = physicsWorld->getScopedSyncLock(__FUNCTION__);
1504 
1505  ArmarXDataPath::FindPackageAndAddDataPath(packagePath.package);
1506  std::string filename;
1507  if (!ArmarXDataPath::SearchReadableFile(packagePath.path, filename))
1508  {
1509  ARMARX_ERROR << "Could not find file: \"" << packagePath.path << "\".";
1510  return;
1511  }
1512  // armarx::PackagePath boPackagePath(packagePath);
1513  // std::string filename = boPackagePath.toSystemPath();
1514 
1515  try
1516  {
1517  if (physicsWorld->hasObject(instanceName))
1518  {
1519  ARMARX_ERROR << "Object with instance name \"" << instanceName << "\" already exists";
1520  return;
1521  }
1522 
1523 
1524  VirtualRobot::ManipulationObjectPtr mo =
1525  VirtualRobot::ObjectIO::loadManipulationObject(filename);
1526 
1527  if (!mo)
1528  {
1529  ARMARX_ERROR << "Could not retrieve manipulation object from file " << filename;
1530  return;
1531  }
1532 
1533  //ARMARX_INFO << "Filename mo:" << mo->getVisualization()->getFilename();
1534  //ARMARX_INFO << "Filename sw:" << sw->getManipulationObjectFileName();
1535 
1536  mo->setName(instanceName);
1537 
1538  // ARMARX_LOG << "Adding manipulation object " << mo->getName() << " of class " << objectClass->getName();
1539  // classInstanceMap[objectClass->getName()].push_back(instanceName);
1540 
1541  VirtualRobot::MathTools::Quaternion qa;
1542  qa.x = globalPose->orientation->qx;
1543  qa.y = globalPose->orientation->qy;
1544  qa.z = globalPose->orientation->qz;
1545  qa.w = globalPose->orientation->qw;
1546  Eigen::Matrix4f gp = VirtualRobot::MathTools::quat2eigen4f(qa);
1547  gp(0, 3) = globalPose->position->x;
1548  gp(1, 3) = globalPose->position->y;
1549  gp(2, 3) = globalPose->position->z;
1550  mo->setGlobalPose(gp);
1551  VirtualRobot::SceneObject::Physics::SimulationType simType =
1552  (isStatic) ? VirtualRobot::SceneObject::Physics::eKinematic
1553  : VirtualRobot::SceneObject::Physics::eDynamic;
1554  physicsWorld->addObstacle(mo, simType, filename, instanceName, {}, packagePath.package);
1555  }
1556  catch (...)
1557  {
1558  ARMARX_WARNING << "addObject failed: " << GetHandledExceptionString();
1559  }
1560 }
1561 
1562 void
1563 Simulator::addBox(float width,
1564  float height,
1565  float depth,
1566  float massKG,
1567  const DrawColor& color,
1568  const std::string& instanceName,
1569  const PoseBasePtr& globalPose,
1570  bool isStatic,
1571  const Ice::Current&)
1572 {
1573  ARMARX_VERBOSE << "Add box " << instanceName;
1574 
1575  auto lockEngine = physicsWorld->getScopedEngineLock(__FUNCTION__);
1576  auto lockData = physicsWorld->getScopedSyncLock(__FUNCTION__);
1577 
1578  try
1579  {
1580 
1581  if (physicsWorld->hasObject(instanceName))
1582  {
1583  ARMARX_ERROR << "Object with instance name \"" << instanceName << "\" already exists";
1584  return;
1585  }
1586 
1587  if (depth <= 0 || width <= 0 || height <= 0)
1588  {
1589  ARMARX_ERROR << "Invalid properties; could not create object with instance name "
1590  << instanceName;
1591  return;
1592  }
1593 
1594  VirtualRobot::VisualizationFactory::Color c;
1595  c.r = color.r;
1596  c.g = color.g;
1597  c.b = color.b;
1598  c.transparency = color.a;
1599  VirtualRobot::ObstaclePtr o = VirtualRobot::Obstacle::createBox(width, height, depth, c);
1600  o->setMass(massKG);
1601 
1602 
1603  if (!o)
1604  {
1605  ARMARX_ERROR << "Could not create box";
1606  return;
1607  }
1608 
1609  o->setName(instanceName);
1610 
1611  ARMARX_LOG << "Adding box object " << o->getName();
1612 
1613  // not a memoryX object
1614  //classInstanceMap[objectClass->getName()].push_back(instanceName);
1615 
1616  VirtualRobot::MathTools::Quaternion qa;
1617  qa.x = globalPose->orientation->qx;
1618  qa.y = globalPose->orientation->qy;
1619  qa.z = globalPose->orientation->qz;
1620  qa.w = globalPose->orientation->qw;
1621  Eigen::Matrix4f gp = VirtualRobot::MathTools::quat2eigen4f(qa);
1622  gp(0, 3) = globalPose->position->x;
1623  gp(1, 3) = globalPose->position->y;
1624  gp(2, 3) = globalPose->position->z;
1625  o->setGlobalPose(gp);
1626  VirtualRobot::SceneObject::Physics::SimulationType simType =
1627  (isStatic) ? VirtualRobot::SceneObject::Physics::eStatic
1628  : VirtualRobot::SceneObject::Physics::eDynamic;
1629 
1630  BoxVisuPrimitivePtr bp(new BoxVisuPrimitive());
1631  bp->type = Box;
1632  bp->width = width;
1633  bp->height = height;
1634  bp->depth = depth;
1635  bp->massKG = massKG;
1636  bp->color = color;
1637 
1638  physicsWorld->addObstacle(o, simType, std::string(), std::string(), bp);
1639  }
1640  catch (...)
1641  {
1642  ARMARX_WARNING << "addObject failed: " << GetHandledExceptionString();
1643  }
1644 }
1645 
1646 Ice::StringSeq
1647 Simulator::getRobotNames(const Ice::Current&)
1648 {
1649  auto lockEngine =
1650  physicsWorld->getScopedEngineLock(__FUNCTION__); // we are accessing objects and robots
1651  auto lockData = physicsWorld->getScopedSyncLock(__FUNCTION__);
1652  return physicsWorld->getRobotNames();
1653 }
1654 
1655 void
1656 Simulator::removeObject(const std::string& instanceName, const Ice::Current&)
1657 {
1658  ARMARX_VERBOSE << "Remove object " << instanceName;
1659 
1660  auto lockEngine = physicsWorld->getScopedEngineLock(__FUNCTION__);
1661  auto lockData = physicsWorld->getScopedSyncLock(__FUNCTION__);
1662 
1663  try
1664  {
1665  if (!physicsWorld->hasObject(instanceName))
1666  {
1667  ARMARX_ERROR << "Object with instance name \"" << instanceName << "\" does not exist";
1668  return;
1669  }
1670 
1671  ARMARX_LOG << "Removing object " << instanceName;
1672 
1673  if (!physicsWorld->removeObstacle(instanceName))
1674  {
1675  ARMARX_ERROR << "Could not remove object with instance name \"" << instanceName << "\"";
1676  return;
1677  }
1678  }
1679  catch (...)
1680  {
1681  ARMARX_WARNING << "removeObject failed...";
1682  }
1683 }
1684 
1685 void
1686 Simulator::setObjectPose(const std::string& objectName,
1687  const PoseBasePtr& globalPose,
1688  const Ice::Current&)
1689 {
1690  VirtualRobot::MathTools::Quaternion q;
1691  q.x = globalPose->orientation->qx;
1692  q.y = globalPose->orientation->qy;
1693  q.z = globalPose->orientation->qz;
1694  q.w = globalPose->orientation->qw;
1695  Eigen::Matrix4f gp = VirtualRobot::MathTools::quat2eigen4f(q);
1696  gp(0, 3) = globalPose->position->x;
1697  gp(1, 3) = globalPose->position->y;
1698  gp(2, 3) = globalPose->position->z;
1699 
1700  physicsWorld->setObjectPose(objectName, gp);
1701 }
1702 
1703 void
1704 Simulator::setObjectSimulationType(const std::string& objectName,
1705  SimulationType type,
1706  const Ice::Current&)
1707 {
1708  VirtualRobot::SceneObject::Physics::SimulationType st =
1709  VirtualRobot::SceneObject::Physics::eDynamic;
1710  if (type == armarx::Kinematic)
1711  {
1712  st = VirtualRobot::SceneObject::Physics::eKinematic;
1713  }
1714  ARMARX_DEBUG << "setting simulation type of " << objectName << " to " << st;
1715  physicsWorld->setObjectSimType(objectName, st);
1716 }
1717 
1718 void
1719 Simulator::activateObject(const std::string& objectName, const Ice::Current&)
1720 {
1721  physicsWorld->activateObject(objectName);
1722 }
1723 
1724 int
1725 Simulator::getFixedTimeStepMS(const Ice::Current&)
1726 {
1727  return physicsWorld->getFixedTimeStepMS();
1728 }
1729 
1730 float
1731 Simulator::getRobotMass(const std::string& robotName, const Ice::Current&)
1732 {
1733  return physicsWorld->getRobotMass(robotName);
1734 }
1735 
1736 NameValueMap
1737 Simulator::getRobotJointAngles(const std::string& robotName, const Ice::Current&)
1738 {
1739  return physicsWorld->getRobotJointAngles(robotName);
1740 }
1741 
1742 float
1743 Simulator::getRobotJointAngle(const std::string& robotName,
1744  const std::string& nodeName,
1745  const Ice::Current&)
1746 {
1747  return physicsWorld->getRobotJointAngle(robotName, nodeName);
1748 }
1749 
1750 NameValueMap
1751 Simulator::getRobotJointVelocities(const std::string& robotName, const Ice::Current&)
1752 {
1753  return physicsWorld->getRobotJointVelocities(robotName);
1754 }
1755 
1756 float
1757 Simulator::getRobotJointVelocity(const std::string& robotName,
1758  const std::string& nodeName,
1759  const Ice::Current&)
1760 {
1761  return physicsWorld->getRobotJointVelocity(robotName, nodeName);
1762 }
1763 
1764 NameValueMap
1765 Simulator::getRobotJointTorques(const std::string& robotName, const Ice::Current&)
1766 {
1767  return physicsWorld->getRobotJointTorques(robotName);
1768 }
1769 
1770 ForceTorqueDataSeq
1771 Simulator::getRobotForceTorqueSensors(const std::string& robotName, const Ice::Current&)
1772 {
1773  return physicsWorld->getRobotForceTorqueSensors(robotName);
1774 }
1775 
1776 float
1777 Simulator::getRobotJointLimitLo(const std::string& robotName,
1778  const std::string& nodeName,
1779  const Ice::Current&)
1780 {
1781  return physicsWorld->getRobotJointLimitLo(robotName, nodeName);
1782 }
1783 
1784 float
1785 Simulator::getRobotJointLimitHi(const std::string& robotName,
1786  const std::string& nodeName,
1787  const Ice::Current&)
1788 {
1789  return physicsWorld->getRobotJointLimitHi(robotName, nodeName);
1790 }
1791 
1792 PoseBasePtr
1793 Simulator::getRobotPose(const std::string& robotName, const Ice::Current&)
1794 {
1795  PosePtr p(new Pose(physicsWorld->getRobotPose(robotName)));
1796  return p;
1797 }
1798 
1799 float
1800 Simulator::getRobotMaxTorque(const std::string& robotName,
1801  const std::string& nodeName,
1802  const Ice::Current&)
1803 {
1804  return physicsWorld->getRobotMaxTorque(robotName, nodeName);
1805 }
1806 
1807 void
1808 Simulator::setRobotMaxTorque(const std::string& robotName,
1809  const std::string& nodeName,
1810  float maxTorque,
1811  const Ice::Current&)
1812 {
1813  physicsWorld->setRobotMaxTorque(robotName, nodeName, maxTorque);
1814 }
1815 
1816 PoseBasePtr
1817 Simulator::getRobotNodePose(const std::string& robotName,
1818  const std::string& robotNodeName,
1819  const Ice::Current&)
1820 {
1821  PosePtr p(new Pose(physicsWorld->getRobotNodePose(robotName, robotNodeName)));
1822  return p;
1823 }
1824 
1825 Vector3BasePtr
1826 Simulator::getRobotLinearVelocity(const std::string& robotName,
1827  const std::string& nodeName,
1828  const Ice::Current&)
1829 {
1830  Vector3Ptr v(new Vector3(physicsWorld->getRobotLinearVelocity(robotName, nodeName)));
1831  return v;
1832 }
1833 
1834 Vector3BasePtr
1835 Simulator::getRobotAngularVelocity(const std::string& robotName,
1836  const std::string& nodeName,
1837  const Ice::Current&)
1838 {
1839  Vector3Ptr v(new Vector3(physicsWorld->getRobotAngularVelocity(robotName, nodeName)));
1840  return v;
1841 }
1842 
1843 void
1844 Simulator::setRobotLinearVelocity(const std::string& robotName,
1845  const std::string& robotNodeName,
1846  const Vector3BasePtr& vel,
1847  const Ice::Current&)
1848 {
1849  Eigen::Vector3f newVel = Vector3Ptr::dynamicCast(vel)->toEigen();
1850  physicsWorld->setRobotLinearVelocity(robotName, robotNodeName, newVel);
1851 }
1852 
1853 void
1854 Simulator::setRobotAngularVelocity(const std::string& robotName,
1855  const std::string& robotNodeName,
1856  const Vector3BasePtr& vel,
1857  const Ice::Current&)
1858 {
1859  Vector3Ptr newVel = Vector3Ptr::dynamicCast(vel);
1860  physicsWorld->setRobotAngularVelocity(robotName, robotNodeName, newVel->toEigen());
1861 }
1862 
1863 void
1864 Simulator::setRobotLinearVelocityRobotRootFrame(const std::string& robotName,
1865  const std::string& robotNodeName,
1866  const Vector3BasePtr& vel,
1867  const Ice::Current&)
1868 {
1869  Eigen::Vector3f newVel = Vector3Ptr::dynamicCast(vel)->toEigen();
1870  physicsWorld->setRobotLinearVelocityRobotRootFrame(robotName, robotNodeName, newVel);
1871 }
1872 
1873 void
1874 Simulator::setRobotAngularVelocityRobotRootFrame(const std::string& robotName,
1875  const std::string& robotNodeName,
1876  const Vector3BasePtr& vel,
1877  const Ice::Current&)
1878 {
1879  Eigen::Vector3f newVel = Vector3Ptr::dynamicCast(vel)->toEigen();
1880  physicsWorld->setRobotAngularVelocityRobotRootFrame(robotName, robotNodeName, newVel);
1881 }
1882 
1883 bool
1884 Simulator::hasRobot(const std::string& robotName, const Ice::Current&)
1885 {
1886  return physicsWorld->hasRobot(robotName);
1887 }
1888 
1889 bool
1890 Simulator::hasRobotNode(const std::string& robotName,
1891  const std::string& robotNodeName,
1892  const Ice::Current&)
1893 {
1894 
1895  return physicsWorld->hasRobotNode(robotName, robotNodeName);
1896 }
1897 
1898 PoseBasePtr
1899 Simulator::getObjectPose(const std::string& objectName, const Ice::Current&)
1900 {
1901  PosePtr p(new Pose(physicsWorld->getObjectPose(objectName)));
1902  return p;
1903 }
1904 
1905 void
1906 Simulator::objectReleased(const std::string& robotName,
1907  const std::string& robotNodeName,
1908  const std::string& objectName,
1909  const Ice::Current&)
1910 {
1911  ARMARX_VERBOSE << "Object released " << robotName << "," << objectName;
1912  physicsWorld->objectReleased(robotName, robotNodeName, objectName);
1913 }
1914 
1915 void
1916 Simulator::objectGrasped(const std::string& robotName,
1917  const std::string& robotNodeName,
1918  const std::string& objectName,
1919  const Ice::Current&)
1920 {
1921  ARMARX_VERBOSE << "Object grasped" << robotName << "," << objectName;
1922  physicsWorld->objectGrasped(robotName, robotNodeName, objectName);
1923 }
1924 
1925 float
1926 Simulator::getSimTime(const Ice::Current&)
1927 {
1928  return static_cast<float>(physicsWorld->getCurrentSimTime());
1929 }
1930 
1931 SceneVisuData
1932 Simulator::getScene(const Ice::Current&)
1933 {
1934  SceneVisuData res = physicsWorld->copySceneVisuData();
1935  res.priorKnowledgeName = priorKnowledgeName;
1936  res.commonStorageName = commonStorageName;
1937  return res;
1938 }
1939 
1940 SimulatorInformation
1941 Simulator::getSimulatorInformation(const Ice::Current&)
1942 {
1943  auto lockEngine =
1944  physicsWorld->getScopedEngineLock(__FUNCTION__); // we are accessing objects and robots
1945  auto lockData = physicsWorld->getScopedSyncLock(__FUNCTION__);
1946  SimulatorInformation res;
1947  res.comTimeMS = currentComTimeMS;
1948  // res.simTimeStepMeasuredMS = currentSimTimeMS; <-- this is wrong?!
1949  res.syncEngineTimeMS = currentSyncTimeMS;
1950 
1951  std::vector<std::string> robNames = physicsWorld->getRobotNames();
1952  res.nrRobots = static_cast<int>(robNames.size());
1953 
1954  res.nrActuatedJoints = physicsWorld->getRobotJointAngleCount();
1955  res.nrObjects = physicsWorld->getObjectCount();
1956  res.nrContacts = physicsWorld->getContactCount();
1957  res.currentSimulatorTimeSec = getSimTime();
1958  res.simTimeStepMeasuredMS = physicsWorld->getSimulationStepTimeMeasured();
1959  res.simTimeStepDurationMS = physicsWorld->getSimulationStepDuration();
1960 
1961  res.simTimeFactor = 0;
1962  if (res.simTimeStepMeasuredMS > 0)
1963  {
1964  res.simTimeFactor = res.simTimeStepDurationMS / res.simTimeStepMeasuredMS;
1965  }
1966 
1967  std::vector<VirtualRobot::SceneObjectPtr> objects = physicsWorld->getObjects();
1968 
1969  for (VirtualRobot::SceneObjectPtr& o : objects)
1970  {
1971  //ARMARX_INFO << "Object " << o->getName() << ", pose:" << o->getSceneObject()->getGlobalPose();
1972  ObjectVisuData ovd;
1973  ovd.name = o->getName();
1974  PosePtr p(new Pose(o->getGlobalPose()));
1975  ovd.objectPoses[ovd.name] = p;
1976  VirtualRobot::Obstacle* ob = dynamic_cast<VirtualRobot::Obstacle*>(o.get());
1977 
1978  if (ob)
1979  {
1980  ovd.filename = ob->getFilename();
1981  }
1982 
1983  if (o->getSimulationType() == VirtualRobot::SceneObject::Physics::eStatic)
1984  {
1985  ovd.staticObject = true;
1986  }
1987  else
1988  {
1989  ovd.staticObject = false;
1990  }
1991 
1992  res.objects.push_back(ovd);
1993  }
1994 
1995  for (auto& r : robNames)
1996  {
1997  RobotVisuData ovd;
1998  ovd.name = r;
1999  VirtualRobot::RobotPtr rob = physicsWorld->getRobot(r);
2000  if (rob)
2001  {
2002  PosePtr p(new Pose(rob->getGlobalPose()));
2003  ovd.pose = p;
2004  ovd.robotFile = rob->getFilename();
2005  }
2006 
2007  res.robots.push_back(ovd);
2008  }
2009 
2010  return res;
2011 }
2012 
2013 std::string
2014 Simulator::addRobotFromFile(const armarx::data::PackagePath& packagePath, const Ice::Current& c)
2015 {
2016  ArmarXDataPath::FindPackageAndAddDataPath(packagePath.package);
2017 
2018  // legacy
2019  {
2020  std::string filename;
2021  if (ArmarXDataPath::SearchReadableFile(packagePath.path, filename))
2022  {
2023  return addRobot(filename);
2024  }
2025  }
2026 
2027  const PackagePath pp(packagePath);
2028  if (std::filesystem::exists(pp.toSystemPath()))
2029  {
2030  return addRobot(pp.toSystemPath());
2031  }
2032 
2033  ARMARX_ERROR << "Could not find file: \"" << packagePath.path << "\".";
2034  return "";
2035 }
2036 
2037 std::string
2038 Simulator::addRobot(std::string robotInstanceName,
2039  const std::string& robFileGlobal,
2040  Eigen::Matrix4f gp,
2041  const std::string& robFile,
2042  double pid_p,
2043  double pid_i,
2044  double pid_d,
2045  bool isStatic,
2046  float scaling,
2047  bool colModel,
2048  std::map<std::string, float> initConfig,
2049  bool selfCollisions)
2050 {
2051  ARMARX_DEBUG << "Add robot from file " << robFileGlobal;
2052  auto lockEngine = physicsWorld->getScopedEngineLock(__FUNCTION__);
2053  auto lockData = physicsWorld->getScopedSyncLock(__FUNCTION__);
2054 
2055  if (physicsWorld->addRobot(robotInstanceName,
2056  robFileGlobal,
2057  gp,
2058  robFile,
2059  pid_p,
2060  pid_i,
2061  pid_d,
2062  isStatic,
2063  scaling,
2064  colModel,
2065  initConfig,
2066  selfCollisions))
2067  {
2068  // update report topics
2069  if (!updateRobotTopics())
2070  {
2071  ARMARX_ERROR << "topic setup failed";
2072  }
2073  }
2074  else
2075  {
2076  ARMARX_WARNING << "addRobot failed";
2077  return "";
2078  }
2079 
2080  return robotInstanceName;
2081 }
2082 
2083 bool
2084 Simulator::updateRobotTopics()
2085 {
2086  SimulatedWorldData& syncData = physicsWorld->getReportData();
2087 
2088  try
2089  {
2090  for (auto& rob : syncData.robots)
2091  {
2092  // offer robot
2093  std::string top = rob.robotTopicName;
2094  ARMARX_INFO << "Offering topic " << top;
2095  offeringTopic(top);
2096  //if (this->getState() == eManagedIceObjectStarted)
2097  {
2098  ARMARX_INFO << "Connecting to topic " << top;
2099  rob.simulatorRobotListenerPrx = getTopic<SimulatorRobotListenerInterfacePrx>(top);
2100  }
2101 
2102  for (auto& fti : rob.forceTorqueSensors)
2103  {
2104  ARMARX_INFO << "Offering topic " << fti.topicName;
2105  offeringTopic(fti.topicName);
2106 
2107  //if (this->getState() == eManagedIceObjectStarted)
2108  {
2109  ARMARX_INFO << "Connecting to topic " << fti.topicName;
2110  fti.prx = getTopic<SimulatorForceTorqueListenerInterfacePrx>(fti.topicName);
2111  }
2112  }
2113  }
2114  }
2115  catch (...)
2116  {
2117  ARMARX_WARNING << "failed: exception";
2118  return false;
2119  }
2120 
2121  return true;
2122 }
2123 
2124 void
2125 Simulator::simulationLoop()
2126 {
2127  step();
2128 }
2129 
2130 ObjectClassInformationSequence
2131 Simulator::getObjectClassPoses(const std::string& robotName,
2132  const std::string& frameName,
2133  const std::string& className,
2134  const Ice::Current&)
2135 {
2136  auto lockEngine = physicsWorld->getScopedEngineLock(__FUNCTION__);
2137  auto lockData = physicsWorld->getScopedSyncLock(__FUNCTION__);
2138 
2139  ARMARX_DEBUG << "get object class poses";
2140 
2141  ObjectClassInformationSequence result;
2142  std::vector<std::string> objects = classInstanceMap[className];
2143 
2144  for (const auto& object : objects)
2145  {
2146  ObjectClassInformation inf;
2147  // Conversion from seconds to microseconds is 10^6 = 1.0e6 != 10e6 = 1.0e7!!!
2148  inf.timestampMicroSeconds =
2149  static_cast<Ice::Long>(physicsWorld->getCurrentSimTime() * 1.0e6);
2150  inf.className = className;
2151  inf.manipulationObjectName = object;
2152  Eigen::Matrix4f worldPose = physicsWorld->getObjectPose(object);
2153 
2154  if (frameName == armarx::GlobalFrame)
2155  {
2156  inf.pose = new FramedPose(worldPose, armarx::GlobalFrame, "");
2157  }
2158  else
2159  {
2160  inf.pose = physicsWorld->toFramedPose(worldPose, robotName, frameName);
2161  }
2162 
2163  result.push_back(inf);
2164  }
2165 
2166  return result;
2167 }
2168 
2169 DistanceInfo
2170 Simulator::getDistance(const std::string& robotName,
2171  const std::string& robotNodeName,
2172  const std::string& worldObjectName,
2173  const Ice::Current&)
2174 {
2175  return physicsWorld->getDistance(robotName, robotNodeName, worldObjectName);
2176 }
2177 
2178 DistanceInfo
2179 Simulator::getRobotNodeDistance(const std::string& robotName,
2180  const std::string& robotNodeName1,
2181  const std::string& robotNodeName2,
2182  const Ice::Current&)
2183 {
2184  return physicsWorld->getRobotNodeDistance(robotName, robotNodeName1, robotNodeName2);
2185 }
2186 
2187 void
2188 Simulator::showContacts(bool enable, const std::string& layerName, const Ice::Current&)
2189 {
2190  auto lockData = physicsWorld->getScopedSyncLock(__FUNCTION__);
2191  ARMARX_INFO << "showcontacts:" << enable;
2192  publishContacts = enable;
2193  physicsWorld->updateContacts(enable);
2194 
2195  if (enable)
2196  {
2197  contactLayerName = layerName;
2198  }
2199  else
2200  {
2201  lastPublishedContacts = 0;
2202 
2203  try
2204  {
2205  entityDrawerPrx->clearLayer(layerName);
2206  entityDrawerPrx->removeLayer(layerName);
2207  }
2208  catch (...)
2209  {
2210  }
2211  }
2212 }
2213 
2214 void
2215 Simulator::reInitialize(const Ice::Current&)
2216 {
2217  resetData(true);
2218 
2219  initializeData();
2220  simulatorResetEventTopic->simulatorWasReset();
2221 }
2222 
2223 bool
2224 Simulator::hasObject(const std::string& instanceName, const Ice::Current&)
2225 {
2226  return physicsWorld->hasObject(instanceName);
2227 }
2228 
2229 void
2230 Simulator::start(const Ice::Current&)
2231 {
2232  simulatorThreadShutdown = false;
2233  bool notify = !simulatorThreadRunning && this->isAlive();
2234  simulatorThreadRunning = true;
2235 
2236  if (notify)
2237  {
2238  condSimulatorThreadRunning.notify_all();
2239  }
2240 
2241  if (!isAlive())
2242  {
2243  Thread::start();
2244  }
2245 }
2246 
2247 void
2248 Simulator::run()
2249 {
2250  IceUtil::Time startTime;
2251  int sleepTimeMS;
2252  int loopTimeMS = physicsWorld->getFixedTimeStepMS();
2253 
2254  std::unique_lock lock(simulatorRunningMutex);
2255 
2256  while (!simulatorThreadShutdown)
2257  {
2258  while (!simulatorThreadRunning)
2259  {
2260  condSimulatorThreadRunning.wait(lock);
2261  }
2262 
2263  while (simulatorThreadRunning && !simulatorThreadShutdown)
2264  {
2265  startTime = IceUtil::Time::now();
2266  step();
2267  sleepTimeMS = static_cast<int>(
2268  loopTimeMS * (1 / timeServerSpeed) -
2269  static_cast<float>((IceUtil::Time::now() - startTime).toMilliSeconds()));
2270 
2271  if (sleepTimeMS < 0)
2272  {
2273  ARMARX_DEBUG << deactivateSpam(5) << "Simulation step took " << -sleepTimeMS
2274  << " milliseconds too long!";
2275  }
2276  else
2277  {
2278  usleep(static_cast<unsigned int>(sleepTimeMS * 1000)); //TODO: check for EINTR?
2279  }
2280  }
2281  }
2282  ARMARX_VERBOSE << "Exiting Simulator::run()";
2283 }
2284 
2285 void
2286 Simulator::pause(const Ice::Current&)
2287 {
2288  simulatorThreadRunning = false;
2289 }
2290 
2291 void
2292 Simulator::stop(const Ice::Current& c)
2293 {
2294  pause(c);
2295 }
2296 
2297 void
2298 Simulator::setSpeed(Ice::Float newSpeed, const Ice::Current&)
2299 {
2300  timeServerSpeed = static_cast<float>(newSpeed);
2301 }
2302 
2303 Ice::Float
2304 Simulator::getSpeed(const Ice::Current&)
2305 {
2306  return Ice::Float(timeServerSpeed);
2307 }
2308 
2309 Ice::Int
2310 Simulator::getStepTimeMS(const ::Ice::Current&)
2311 {
2312  return physicsWorld->getFixedTimeStepMS();
2313 }
2314 
2315 Ice::Long
2316 Simulator::getTime(const Ice::Current&)
2317 {
2318  return static_cast<long>(physicsWorld->getCurrentSimTime() * 1000);
2319 }
2320 
2321 void
2322 Simulator::step(const Ice::Current&)
2323 {
2324  IceUtil::Time startTime = IceUtil::Time::now();
2325 
2326  if (getProperty<bool>("RealTimeMode"))
2327  {
2328  physicsWorld->stepPhysicsRealTime();
2329  }
2330  else
2331  {
2332  physicsWorld->stepPhysicsFixedTimeStep();
2333  }
2334 
2335  IceUtil::Time durationSim = IceUtil::Time::now() - startTime;
2336 
2337  if (durationSim.toMilliSecondsDouble() > MAX_SIM_TIME_WARNING)
2338  {
2339  ARMARX_INFO << deactivateSpam(5) << "*** Simulation slow !! Simulation time in ms:"
2340  << durationSim.toMilliSecondsDouble();
2341  }
2342 
2343  ARMARX_DEBUG << "*** Step Physics, MS:" << durationSim.toMilliSecondsDouble();
2344 
2345  startTime = IceUtil::Time::now();
2346  // copy bullet content to data objects
2347  physicsWorld->synchronizeSimulationData();
2348  IceUtil::Time durationSync = IceUtil::Time::now() - startTime;
2349 
2350  {
2351  auto lockData = physicsWorld->getScopedSyncLock(__FUNCTION__);
2352  currentSimTimeMS = static_cast<float>(durationSim.toMilliSecondsDouble());
2353  currentSyncTimeMS = static_cast<float>(durationSync.toMilliSecondsDouble());
2354  }
2355  timeTopicPrx->reportTime(getTime());
2356 }
2357 
2358 bool
2359 Simulator::isRunning(const Ice::Current&)
2360 {
2361  return simulatorThreadRunning;
2362 }
2363 
2364 void
2365 Simulator::reportVisuLoop()
2366 {
2367  ARMARX_DEBUG << "report visu updates";
2368 
2369  try
2370  {
2371  IceUtil::Time startTime = IceUtil::Time::now();
2372 
2373  SceneVisuData reportData = physicsWorld->copySceneVisuData();
2374  reportData.priorKnowledgeName = priorKnowledgeName;
2375  reportData.commonStorageName = commonStorageName;
2376 
2377  if (simulatorVisuUpdateListenerPrx)
2378  {
2379  simulatorVisuUpdateListenerPrx->begin_reportSceneUpdated(reportData);
2380  }
2381  else
2382  {
2383  ARMARX_ERROR << deactivateSpam() << "No visu update proxy to send data (so far)";
2384  }
2385 
2386  {
2387  //lock both here!
2388  //otherwise we have the lock order sync->engine which causes a deadlock!
2389  const auto lockEngine = physicsWorld->getScopedEngineLock(__FUNCTION__);
2390  const auto lockSync = physicsWorld->getScopedSyncLock(__FUNCTION__);
2391 
2392  if (publishContacts && entityDrawerPrx)
2393  {
2394  //entityDrawerPrx->clearLayer(contactLayerName);
2395  std::vector<SimDynamics::DynamicsEngine::DynamicsContactInfo> contacts =
2396  physicsWorld->copyContacts();
2397  VirtualRobot::ColorMap cm(VirtualRobot::ColorMap::eHot);
2398  float maxImpulse = 2.0f;
2399  float arrowLength = 50.0f;
2400  int i = 0;
2401  DrawColor dc;
2402 
2403  for (auto& c : contacts)
2404  {
2405  std::stringstream ss;
2406  ss << "Contact_" << i;
2407  /*if (c.objectA)
2408  ss << "_" << c.objectA->getName();
2409  if (c.objectB)
2410  ss << "_" << c.objectB->getName();*/
2411  std::string name = ss.str();
2412  std::string name2 = name + "_2";
2413  std::string objA;
2414 
2415  if (!c.objectAName.empty())
2416  {
2417  objA = c.objectAName; //->getName();
2418  }
2419  else
2420  {
2421  objA = "<unknown>";
2422  }
2423 
2424  std::string objB;
2425 
2426  if (!c.objectBName.empty())
2427  {
2428  objB = c.objectBName; //->getName();
2429  }
2430  else
2431  {
2432  objB = "<unknown>";
2433  }
2434 
2435  ARMARX_DEBUG << "CONTACT " << name << ", objects: " << objA << "-" << objB
2436  << ", impulse:" << c.appliedImpulse;
2437  float intensity = static_cast<float>(c.appliedImpulse);
2438 
2439  if (intensity > maxImpulse)
2440  {
2441  intensity = maxImpulse;
2442  }
2443 
2444  intensity /= maxImpulse;
2445  VirtualRobot::VisualizationFactory::Color colSimox = cm.getColor(intensity);
2446  dc.r = colSimox.r;
2447  dc.g = colSimox.g;
2448  dc.b = colSimox.b;
2449  dc.a = 1.0f;
2450  //Eigen::Vector3f posEndB = c.posGlobalB + c.normalGlobalB.normalized() * arrowLength;
2451  Vector3Ptr p1(new Vector3(c.posGlobalA));
2452  Eigen::Vector3f dir2 = -(c.normalGlobalB);
2453  Vector3Ptr d1(new Vector3(dir2));
2454  Vector3Ptr p2(new Vector3(c.posGlobalB));
2455  Vector3Ptr d2(new Vector3(c.normalGlobalB));
2456 
2457  entityDrawerPrx->setArrowVisu(
2458  contactLayerName, name, p1, d1, dc, arrowLength, 5.0f);
2459  entityDrawerPrx->setArrowVisu(
2460  contactLayerName, name2, p2, d2, dc, arrowLength, 5.0f);
2461  i++;
2462  }
2463 
2464  // remove old contacts
2465  for (int j = i; j < lastPublishedContacts; j++)
2466  {
2467  std::stringstream ss;
2468  ss << "Contact_" << j;
2469  std::string name = ss.str();
2470  std::string name2 = name + "_2";
2471  entityDrawerPrx->removeArrowVisu(contactLayerName, name);
2472  entityDrawerPrx->removeArrowVisu(contactLayerName, name2);
2473  }
2474 
2475  lastPublishedContacts = i;
2476  }
2477 
2478  if (getProperty<bool>("DrawCoMVisu"))
2479  {
2480  for (auto name : physicsWorld->getRobotNames())
2481  {
2482  auto pos = new Vector3(physicsWorld->getRobot(name)->getCoMGlobal());
2483  pos->z = 0.0f;
2484  entityDrawerPrx->setSphereVisu(
2485  "CoM", "GlobalCoM_" + name, pos, DrawColor{0.f, 0.f, 1.f, 0.5f}, 20);
2486  }
2487  }
2488  }
2489  IceUtil::Time duration = IceUtil::Time::now() - startTime;
2490  reportVisuTimeMS = static_cast<float>(duration.toMilliSecondsDouble());
2491  }
2492  catch (...)
2493  {
2494  ARMARX_WARNING << "report updates failed";
2495  ARMARX_WARNING << "Reason" << GetHandledExceptionString();
2496  return;
2497  }
2498 
2499  ARMARX_DEBUG << "report visu updates done, time (ms):" << reportVisuTimeMS;
2500  if (reportVisuTimeMS > 5)
2501  {
2502  ARMARX_INFO << deactivateSpam(10)
2503  << "report visu updates took long, time (ms):" << reportVisuTimeMS;
2504  }
2505 }
2506 
2507 void
2508 Simulator::reportDataLoop()
2509 {
2510  ARMARX_DEBUG_S << "report data updates";
2511 
2512  try
2513  {
2514  IceUtil::Time startTime = IceUtil::Time::now();
2515 
2516  SimulatedWorldData reportData = physicsWorld->copyReportData();
2517 
2518  for (RobotInfo& r : reportData.robots)
2519  {
2520  SimulatedRobotState state = stateFromRobotInfo(r, reportData.timestamp);
2521 
2522  if (reportRobotPose)
2523  {
2524  Ice::Context ctx;
2525  ctx["origin"] = "simulation";
2526 
2527  FrameHeader header;
2528  header.parentFrame = GlobalFrame;
2529  header.frame = physicsWorld->getRobot(r.robotName)->getRootNode()->getName();
2530  header.agent = r.robotName;
2531  header.timestampInMicroSeconds = reportData.timestamp.toMicroSeconds();
2532 
2533  TransformStamped globalRobotPose;
2534  globalRobotPose.header = header;
2535  globalRobotPose.transform = r.pose;
2536 
2537  if (globalRobotLocalization)
2538  {
2539  globalRobotLocalization->reportGlobalRobotPose(globalRobotPose, ctx);
2540  }
2541  }
2542 
2543  if (r.simulatorRobotListenerPrx)
2544  {
2545  r.simulatorRobotListenerPrx->reportState(state);
2546  }
2547  }
2548 
2549  // report world objects
2550  // todo...
2551 
2552  IceUtil::Time duration = IceUtil::Time::now() - startTime;
2553  currentComTimeMS = static_cast<float>(duration.toMilliSecondsDouble());
2554  }
2555  catch (...)
2556  {
2557  ARMARX_WARNING_S << "report updates failed";
2558  ARMARX_WARNING << "Reason" << GetHandledExceptionString();
2559  return;
2560  }
2561 
2562  ARMARX_DEBUG_S << "report data updates done, time (ms):" << currentComTimeMS;
2563 }
2564 
2565 ContactInfoSequence
2566 Simulator::getContacts(const Ice::Current&)
2567 {
2568  ContactInfoSequence result;
2569  auto contacts = this->physicsWorld->copyContacts();
2570  for (auto& contact : contacts)
2571  {
2572  ContactInformation contactInfo;
2573  contactInfo.objectNameA = contact.objectAName;
2574  contactInfo.objectNameB = contact.objectBName;
2575  contactInfo.posGlobalA = new Vector3(contact.posGlobalA);
2576  contactInfo.posGlobalB = new Vector3(contact.posGlobalB);
2577  result.push_back(contactInfo);
2578  }
2579  return result;
2580 }
2581 
2582 void
2583 Simulator::updateContacts(bool enable, const Ice::Current&)
2584 {
2585  this->physicsWorld->updateContacts(enable);
2586 }
2587 
2588 std::string
2589 Simulator::addScaledRobot(const std::string& filename, float scaling, const Ice::Current&)
2590 {
2591  ARMARX_TRACE;
2592  std::string instanceName;
2593  this->physicsWorld->addRobot(
2594  instanceName, filename, Eigen::Matrix4f::Identity(), "", 10.0, 0, 0, false, scaling);
2595  return instanceName;
2596 }
2597 
2598 std::string
2599 Simulator::addScaledRobotName(const std::string& instanceName,
2600  const std::string& filename,
2601  float scaling,
2602  const Ice::Current&)
2603 {
2604  ARMARX_TRACE;
2605  std::string name = instanceName;
2606  this->physicsWorld->addRobot(
2607  name, filename, Eigen::Matrix4f::Identity(), "", 10.0, 0, 0, false, scaling);
2608  return name;
2609 }
2610 
2611 bool
2612 Simulator::removeRobot(const std::string& robotName, const Ice::Current&)
2613 {
2614  return this->physicsWorld->removeRobot(robotName);
2615 }
2616 
2617 std::string
2618 Simulator::addRobot(const std::string& filename, const Ice::Current&)
2619 {
2620  ARMARX_TRACE;
2621  std::string instanceName;
2622  this->physicsWorld->addRobot(
2623  instanceName, filename, Eigen::Matrix4f::Identity(), "", 10.0, 0, 0, false, 1.0f);
2624  return instanceName;
2625 }
2626 
2627 SimulatedRobotState
2628 armarx::Simulator::getRobotState(const std::string& robotName, const Ice::Current&)
2629 {
2630  SimulatedWorldData report = physicsWorld->getReportData();
2631  for (auto& robot : report.robots)
2632  {
2633  if (robot.robotName == robotName)
2634  {
2635  return stateFromRobotInfo(robot, report.timestamp);
2636  }
2637  }
2638 
2639  return SimulatedRobotState();
2640 }
armarx::SimulatedWorldData::robots
EIGEN_MAKE_ALIGNED_OPERATOR_NEW std::vector< RobotInfo > robots
Definition: SimulatedWorld.h:112
armarx::Simulator::addBox
void addBox(float width, float height, float depth, float massKG, const DrawColor &color, const std::string &instanceName, const PoseBasePtr &globalPose, bool isStatic=false, const Ice::Current &c=Ice::emptyCurrent) override
Definition: Simulator.cpp:1563
armarx::Simulator::onExitComponent
void onExitComponent() override
Hook for subclass.
Definition: Simulator.cpp:1062
MujocoPhysicsWorld.h
ARMARX_VERBOSE
#define ARMARX_VERBOSE
Definition: Logging.h:187
armarx::Simulator::Simulator
Simulator()
Definition: Simulator.cpp:247
armarx::RobotInfo::jointTorques
NameValueMap jointTorques
Definition: SimulatedWorld.h:94
armarx::Simulator::setRobotLinearVelocityRobotRootFrame
void setRobotLinearVelocityRobotRootFrame(const std::string &robotName, const std::string &robotNodeName, const Vector3BasePtr &vel, const Ice::Current &c=Ice::emptyCurrent) override
Definition: Simulator.cpp:1864
armarx::Simulator::reportDataLoop
void reportDataLoop()
Definition: Simulator.cpp:2508
ARMARX_CHECK_NONNEGATIVE
#define ARMARX_CHECK_NONNEGATIVE(number)
Check whether number is nonnegative (>= 0). If it is not, throw an ExpressionException with the expre...
Definition: ExpressionException.h:152
armarx::Simulator::reportVisuTask
PeriodicTask< Simulator >::pointer_type reportVisuTask
The report visu task.
Definition: Simulator.h:440
armarx::Simulator::getObjectPose
PoseBasePtr getObjectPose(const std::string &objectName, const Ice::Current &c=Ice::emptyCurrent) override
Definition: Simulator.cpp:1899
armarx::BulletPhysicsWorldPtr
std::shared_ptr< BulletPhysicsWorld > BulletPhysicsWorldPtr
Definition: BulletPhysicsWorld.h:285
armarx::Simulator::reportVisuLoop
void reportVisuLoop()
Definition: Simulator.cpp:2365
armarx::VariantType::Float
const VariantTypeId Float
Definition: Variant.h:919
armarx::Simulator::timeserverProxy
SimulatorTimeServerProxyPtr timeserverProxy
Proxy object to offer the Timeserver proxy.
Definition: Simulator.h:498
armarx::ObjectID
A known object ID of the form "Dataset/ClassName" or "Dataset/ClassName/InstanceName".
Definition: ObjectID.h:10
armarx::ObjectID::withInstanceName
ObjectID withInstanceName(const std::string &instanceName) const
Definition: ObjectID.cpp:83
armarx::Simulator::stop
void stop(const Ice::Current &c=Ice::emptyCurrent) override
Definition: Simulator.cpp:2292
armarx::ObjectID::FromString
static ObjectID FromString(const std::string &idString)
Construct from a string produced by str(), e.g. ("mydataset/myobject", "mydataset/myclass/myinstance"...
Definition: ObjectID.cpp:34
armarx::Simulator::lastPublishedContacts
int lastPublishedContacts
Definition: Simulator.h:488
armarx::Simulator::actuateRobotJointsTorque
void actuateRobotJointsTorque(const std::string &robotName, const NameValueMap &torques, const Ice::Current &c=Ice::emptyCurrent) override
Definition: Simulator.cpp:1350
armarx::Simulator::addRobot
std::string addRobot(const std::string &filename, const Ice::Current &c=Ice::emptyCurrent) override
Definition: Simulator.cpp:2618
armarx::KinematicsWorldPtr
std::shared_ptr< KinematicsWorld > KinematicsWorldPtr
Definition: KinematicsWorld.h:278
armarx::Simulator::onInitComponent
void onInitComponent() override
Pure virtual hook for the subclass.
Definition: Simulator.cpp:276
armarx::navigation::core::Pose
Eigen::Isometry3f Pose
Definition: basic_types.h:31
armarx::ObjectID::getClassID
ObjectID getClassID() const
Return just the class ID without an intance name.
Definition: ObjectID.cpp:71
index
uint8_t index
Definition: EtherCATFrame.h:59
QUOTED
#define QUOTED(x)
Definition: StringHelpers.h:199
armarx::Simulator::setRobotAngularVelocityRobotRootFrame
void setRobotAngularVelocityRobotRootFrame(const std::string &robotName, const std::string &robotNodeName, const Vector3BasePtr &vel, const Ice::Current &c=Ice::emptyCurrent) override
Definition: Simulator.cpp:1874
armarx::VariantType::FramedPose
const VariantTypeId FramedPose
Definition: FramedPose.h:36
ArmarXManager.h
armarx::objects::Scene::objects
std::vector< SceneObject > objects
Definition: Scene.h:57
armarx::Simulator::getRobotPose
PoseBasePtr getRobotPose(const std::string &robotName, const Ice::Current &c=Ice::emptyCurrent) override
Definition: Simulator.cpp:1793
armarx::RobotInfo::jointVelocities
NameValueMap jointVelocities
Definition: SimulatedWorld.h:93
armarx::Simulator::getRobotAngularVelocity
Vector3BasePtr getRobotAngularVelocity(const std::string &robotName, const std::string &nodeName, const Ice::Current &c=Ice::emptyCurrent) override
Definition: Simulator.cpp:1835
GfxTL::Matrix4f
MatrixXX< 4, 4, float > Matrix4f
Definition: MatrixXX.h:650
armarx::Simulator::applyForceObject
void applyForceObject(const std::string &objectName, const Vector3BasePtr &force, const Ice::Current &c=Ice::emptyCurrent) override
Definition: Simulator.cpp:1395
armarx::MujocoPhysicsWorld
The MujocoPhysicsWorld class encapsulates the whole physics simulation and the corresponding data.
Definition: MujocoPhysicsWorld.h:47
armarx::Simulator::run
void run() override
Definition: Simulator.cpp:2248
armarx::ArmarXDataPath::FindPackageAndAddDataPath
static bool FindPackageAndAddDataPath(const std::string &packageName)
Search for the package and add its data path if it was found.
Definition: ArmarXDataPath.cpp:824
Pose.h
ARMARX_CHECK_NOT_NULL
#define ARMARX_CHECK_NOT_NULL(ptr)
This macro evaluates whether ptr is not null and if it turns out to be false it will throw an Express...
Definition: ExpressionException.h:206
armarx::Simulator::condSimulatorThreadRunning
std::condition_variable condSimulatorThreadRunning
Definition: Simulator.h:448
armarx::Simulator::hasRobotNode
bool hasRobotNode(const std::string &robotName, const std::string &robotNodeName, const Ice::Current &c=Ice::emptyCurrent) override
Definition: Simulator.cpp:1890
armarx::Simulator::getRobotMaxTorque
float getRobotMaxTorque(const std::string &robotName, const std::string &nodeName, const Ice::Current &c=Ice::emptyCurrent) override
Definition: Simulator.cpp:1800
armarx::Simulator::getSimulatorInformation
SimulatorInformation getSimulatorInformation(const ::Ice::Current &=Ice::emptyCurrent) override
Definition: Simulator.cpp:1941
MAX_SIM_TIME_WARNING
#define MAX_SIM_TIME_WARNING
Definition: Simulator.cpp:34
armarx::RobotInfo::angularVelocity
Eigen::Vector3f angularVelocity
Definition: SimulatedWorld.h:97
armarx::GlobalFrame
const std::string GlobalFrame
Definition: FramedPose.h:65
armarx::Split
std::vector< std::string > Split(const std::string &source, const std::string &splitBy, bool trimElements=false, bool removeEmptyElements=false)
Definition: StringHelperTemplates.h:36
armarx::SimulatedWorldData::timestamp
IceUtil::Time timestamp
Definition: SimulatedWorld.h:113
armarx::SimulatorPropertyDefinitions::~SimulatorPropertyDefinitions
~SimulatorPropertyDefinitions() override
Definition: Simulator.cpp:243
armarx::ManagedIceObject::getArmarXManager
ArmarXManagerPtr getArmarXManager() const
Returns the ArmarX manager used to add and remove components.
Definition: ManagedIceObject.cpp:360
armarx::Simulator::getSpeed
Ice::Float getSpeed(const Ice::Current &c=Ice::emptyCurrent) override
Definition: Simulator.cpp:2304
trace.h
armarx::SimulatorTimeServerProxy
The SimulatorTimeServerProxy class forwards TimeserverInterface calls to the simulator This is a hack...
Definition: SimulatorTimeServerProxy.h:42
armarx::CMakePackageFinder
The CMakePackageFinder class provides an interface to the CMake Package finder capabilities.
Definition: CMakePackageFinder.h:52
armarx::Simulator::getRobotJointTorques
NameValueMap getRobotJointTorques(const std::string &robotName, const Ice::Current &c=Ice::emptyCurrent) override
Definition: Simulator.cpp:1765
armarx::Simulator::timeServerSpeed
float timeServerSpeed
Scaling factor for the passing of time.
Definition: Simulator.h:461
armarx::RobotInfo::robotName
std::string robotName
Definition: SimulatedWorld.h:87
armarx::Simulator::currentComTimeMS
float currentComTimeMS
Stores the time that was needed for communictaion.
Definition: Simulator.h:454
armarx::SimulatorType::Bullet
@ Bullet
armarx::Simulator::reportVisuTimeMS
std::atomic< float > reportVisuTimeMS
Definition: Simulator.h:470
armarx::Simulator::addObjectFromFile
void addObjectFromFile(const armarx::data::PackagePath &packagePath, const std::string &instanceName, const PoseBasePtr &globalPose, bool isStatic=false, const Ice::Current &c=Ice::emptyCurrent) override
Definition: Simulator.cpp:1490
memoryx
VirtualRobot headers.
Definition: CommonPlacesTester.cpp:48
c
constexpr T c
Definition: UnscentedKalmanFilterTest.cpp:46
armarx::Simulator::getRobotMass
float getRobotMass(const std::string &robotName, const Ice::Current &c=Ice::emptyCurrent) override
Definition: Simulator.cpp:1731
ARMARX_CHECK_NOT_EMPTY
#define ARMARX_CHECK_NOT_EMPTY(c)
Definition: ExpressionException.h:224
armarx::Simulator::addScaledRobotName
std::string addScaledRobotName(const std::string &instanceName, const std::string &filename, float scale, const Ice::Current &c=Ice::emptyCurrent) override
Definition: Simulator.cpp:2599
armarx::Simulator::resetData
void resetData(bool clearWorkingMemory=false)
resetData Clears all data
Definition: Simulator.cpp:1106
ObjectClass.h
armarx::PackageFileLocation
Definition: ObjectInfo.h:22
armarx::Simulator::isRunning
bool isRunning(const Ice::Current &c=Ice::emptyCurrent) override
Definition: Simulator.cpp:2359
armarx::PackageFileLocation::relativePath
std::string relativePath
Relative to the package's data directory.
Definition: ObjectInfo.h:28
armarx::Simulator::actuateRobotJointsPos
void actuateRobotJointsPos(const std::string &robotName, const NameValueMap &angles, const Ice::Current &c=Ice::emptyCurrent) override
Definition: Simulator.cpp:1334
armarx::starts_with
bool starts_with(const std::string &haystack, const std::string &needle)
Definition: StringHelpers.cpp:47
Scene.h
armarx::objects::SceneObject
Definition: Scene.h:37
armarx::Simulator::updateRobotTopics
bool updateRobotTopics()
Definition: Simulator.cpp:2084
armarx::Simulator::actuateRobotJointsVel
void actuateRobotJointsVel(const std::string &robotName, const NameValueMap &velocities, const Ice::Current &c=Ice::emptyCurrent) override
Definition: Simulator.cpp:1342
visionx::imrecman::ok
@ ok
Definition: ImageRecordingManagerInterface.ice:45
armarx::Simulator::actuateRobotJoints
void actuateRobotJoints(const std::string &robotName, const NameValueMap &angles, const NameValueMap &velocities, const Ice::Current &c=Ice::emptyCurrent) override
Definition: Simulator.cpp:1325
armarx::Simulator::hasRobot
bool hasRobot(const std::string &robotName, const Ice::Current &c=Ice::emptyCurrent) override
Definition: Simulator.cpp:1884
StringHelpers.h
armarx::Simulator::getDistance
DistanceInfo getDistance(const std::string &robotName, const std::string &robotNodeName, const std::string &worldObjectName, const Ice::Current &c=Ice::emptyCurrent) override
Definition: Simulator.cpp:2170
armarx::VariantType::Quaternion
const VariantTypeId Quaternion
Definition: Pose.h:39
armarx::Simulator::getRobotJointLimitHi
float getRobotJointLimitHi(const std::string &robotName, const std::string &nodeName, const Ice::Current &c=Ice::emptyCurrent) override
Definition: Simulator.cpp:1785
armarx::Simulator::getObjectClassPoses
ObjectClassInformationSequence getObjectClassPoses(const std::string &robotName, const std::string &frameName, const std::string &className, const Ice::Current &c=Ice::emptyCurrent) override
Definition: Simulator.cpp:2131
armarx::Simulator::pause
void pause(const Ice::Current &c=Ice::emptyCurrent) override
Definition: Simulator.cpp:2286
armarx::Simulator::activateObject
void activateObject(const std::string &objectName, const Ice::Current &c=Ice::emptyCurrent) override
Definition: Simulator.cpp:1719
armarx::Simulator::initializeData
void initializeData()
Definition: Simulator.cpp:854
armarx::Simulator::fileManager
memoryx::GridFileManagerPtr fileManager
Definition: Simulator.h:477
armarx::Simulator::start
void start(const Ice::Current &c=Ice::emptyCurrent) override
Definition: Simulator.cpp:2230
cxxopts::empty
bool empty(const std::string &s)
Definition: cxxopts.hpp:234
armarx::Simulator::contactLayerName
std::string contactLayerName
Definition: Simulator.h:486
armarx::Simulator::requestFileManager
memoryx::GridFileManagerPtr requestFileManager()
Definition: Simulator.cpp:953
IceInternal::Handle< ObjectInstance >
armarx::Simulator::simulatorResetEventTopic
SimulatorResetEventPrx simulatorResetEventTopic
Definition: Simulator.h:499
armarx::Simulator::addSnapshot
bool addSnapshot(memoryx::WorkingMemorySnapshotInterfacePrx snapshotInterfacePrx)
Definition: Simulator.cpp:1137
armarx::Simulator::getRobotNodeDistance
DistanceInfo getRobotNodeDistance(const std::string &robotName, const std::string &robotNodeName1, const std::string &robotNodeName2, const Ice::Current &c=Ice::emptyCurrent) override
Definition: Simulator.cpp:2179
armarx::Simulator::removeRobot
bool removeRobot(const std::string &robotName, const Ice::Current &c=Ice::emptyCurrent)
Definition: Simulator.cpp:2612
armarx::Simulator::setRobotPose
void setRobotPose(const std::string &robotName, const PoseBasePtr &globalPose, const Ice::Current &c=Ice::emptyCurrent) override
Definition: Simulator.cpp:1358
armarx::Simulator::getFixedTimeStepMS
int getFixedTimeStepMS(const Ice::Current &c=Ice::emptyCurrent) override
Definition: Simulator.cpp:1725
armarx::GetHandledExceptionString
std::string GetHandledExceptionString()
Definition: Exception.cpp:165
ARMARX_TRACE
#define ARMARX_TRACE
Definition: trace.h:77
armarx::ObjectFinder
Used to find objects in the ArmarX objects repository [1] (formerly [2]).
Definition: ObjectFinder.h:22
armarx::RobotInfo::jointAngles
NameValueMap jointAngles
Definition: SimulatedWorld.h:92
armarx::Simulator::getRobotJointAngle
float getRobotJointAngle(const std::string &robotName, const std::string &nodeName, const Ice::Current &c=Ice::emptyCurrent) override
Definition: Simulator.cpp:1743
GfxTL::Identity
void Identity(MatrixXX< N, N, T > *a)
Definition: MatrixXX.h:570
Color
uint32_t Color
RGBA color.
Definition: color.h:8
armarx::Simulator::entityDrawerPrx
memoryx::EntityDrawerInterfacePrx entityDrawerPrx
Drawing contacts.
Definition: Simulator.h:483
ARMARX_DEBUG_S
#define ARMARX_DEBUG_S
Definition: Logging.h:205
json_conversions.h
armarx::Simulator::getTime
Ice::Long getTime(const Ice::Current &c=Ice::emptyCurrent) override
Definition: Simulator.cpp:2316
armarx::Simulator::getRobotNodePose
PoseBasePtr getRobotNodePose(const std::string &robotName, const std::string &robotNodeName, const Ice::Current &c=Ice::emptyCurrent) override
Definition: Simulator.cpp:1817
FramedPose.h
armarx::PackagePath::toSystemPath
static std::filesystem::path toSystemPath(const data::PackagePath &pp)
Definition: PackagePath.cpp:47
armarx::ValueToString
std::string ValueToString(const T &value)
Definition: StringHelpers.h:61
ARMARX_DEBUG
#define ARMARX_DEBUG
Definition: Logging.h:184
armarx::SimulatorType
SimulatorType
Definition: Simulator.h:69
ObjectID.h
armarx::Simulator::simulatorThreadRunning
bool simulatorThreadRunning
Definition: Simulator.h:445
MAX_INITIAL_ROBOT_COUNT
#define MAX_INITIAL_ROBOT_COUNT
Definition: Simulator.h:61
armarx::Simulator::currentSimTimeMS
float currentSimTimeMS
Stores the time that was needed to perform the last simulation loop.
Definition: Simulator.h:456
armarx::VariantType::Long
const VariantTypeId Long
Definition: Variant.h:918
armarx::Simulator::setRobotAngularVelocity
void setRobotAngularVelocity(const std::string &robotName, const std::string &robotNodeName, const Vector3BasePtr &vel, const Ice::Current &c=Ice::emptyCurrent) override
Definition: Simulator.cpp:1854
armarx::Simulator::stateFromRobotInfo
SimulatedRobotState stateFromRobotInfo(RobotInfo const &robot, IceUtil::Time timestamp)
Definition: Simulator.cpp:922
armarx::Simulator::getDefaultName
std::string getDefaultName() const override
Retrieve default name of component.
Definition: Simulator.cpp:270
armarx::MujocoPhysicsWorldPtr
std::shared_ptr< MujocoPhysicsWorld > MujocoPhysicsWorldPtr
Definition: MujocoPhysicsWorld.h:324
armarx::Simulator::showContacts
void showContacts(bool enable, const std::string &layerName, const Ice::Current &c=Ice::emptyCurrent) override
Definition: Simulator.cpp:2188
MemoryXCoreObjectFactories.h
filename
std::string filename
Definition: VisualizationRobot.cpp:86
armarx::RobotInfo::forceTorqueSensors
std::vector< ForceTorqueInfo > forceTorqueSensors
Definition: SimulatedWorld.h:99
timestamp
std::string timestamp()
Definition: CartographerAdapter.cpp:85
armarx::Simulator::getContacts
ContactInfoSequence getContacts(const Ice::Current &c=Ice::emptyCurrent) override
Returns a list of all contacts. Note that you must call updateContacts() first to enable contacts han...
Definition: Simulator.cpp:2566
armarx::Simulator::getStepTimeMS
Ice::Int getStepTimeMS(const ::Ice::Current &=Ice::emptyCurrent) override
Definition: Simulator.cpp:2310
armarx::Simulator::priorKnowledgePrx
memoryx::PriorKnowledgeInterfacePrx priorKnowledgePrx
Definition: Simulator.h:475
armarx::SimulatorType::Kinematics
@ Kinematics
armarx::Simulator::addScaledRobot
std::string addScaledRobot(const std::string &filename, float scale, const Ice::Current &c=Ice::emptyCurrent) override
Definition: Simulator.cpp:2589
armarx::Simulator::~Simulator
~Simulator() override
Definition: Simulator.cpp:264
armarx::Simulator::simulatorThreadShutdown
bool simulatorThreadShutdown
Definition: Simulator.h:446
armarx::RobotInfo::linearVelocity
Eigen::Vector3f linearVelocity
Definition: SimulatedWorld.h:96
armarx::Simulator::memoryPrx
memoryx::WorkingMemoryInterfacePrx memoryPrx
Definition: Simulator.h:474
armarx::Simulator::onDisconnectComponent
void onDisconnectComponent() override
Hook for subclass.
Definition: Simulator.cpp:1042
ARMARX_ERROR
#define ARMARX_ERROR
Definition: Logging.h:196
armarx::Simulator::reportDataTask
PeriodicTask< Simulator >::pointer_type reportDataTask
The report data task.
Definition: Simulator.h:442
armarx::Simulator::removeObject
void removeObject(const std::string &instanceName, const Ice::Current &) override
Definition: Simulator.cpp:1656
armarx::Simulator::getRobotJointVelocity
float getRobotJointVelocity(const std::string &robotName, const std::string &nodeName, const Ice::Current &c=Ice::emptyCurrent) override
Definition: Simulator.cpp:1757
armarx::Simulator::getRobotNames
Ice::StringSeq getRobotNames(const Ice::Current &) override
Definition: Simulator.cpp:1647
q
#define q
armarx::Simulator::setRobotLinearVelocity
void setRobotLinearVelocity(const std::string &robotName, const std::string &robotNodeName, const Vector3BasePtr &vel, const Ice::Current &c=Ice::emptyCurrent) override
Definition: Simulator.cpp:1844
ARMARX_WARNING_S
#define ARMARX_WARNING_S
Definition: Logging.h:213
armarx::armem::Time
armarx::core::time::DateTime Time
Definition: forward_declarations.h:13
memoryx::EntityWrappers::SimoxObjectWrapper
SimoxObjectWrapper offers a simplified access to the Simox ManipulationObject (i.e visualization,...
Definition: SimoxObjectWrapper.h:46
armarx::to_string
const std::string & to_string(const std::string &s)
Definition: StringHelpers.h:41
ARMARX_LOG
#define ARMARX_LOG
Definition: Logging.h:165
armarx::Simulator::commonStorageName
std::string commonStorageName
Definition: Simulator.h:473
armarx::RobotInfo::simulatorRobotListenerPrx
SimulatorRobotListenerInterfacePrx simulatorRobotListenerPrx
Definition: SimulatedWorld.h:89
armarx::Component
Baseclass for all ArmarX ManagedIceObjects requiring properties.
Definition: Component.h:91
armarx::Simulator::applyTorqueObject
void applyTorqueObject(const std::string &objectName, const Vector3BasePtr &torque, const Ice::Current &c=Ice::emptyCurrent) override
Definition: Simulator.cpp:1404
armarx::RobotInfo
Definition: SimulatedWorld.h:71
armarx::Simulator::simulatorRunningMutex
std::mutex simulatorRunningMutex
Definition: Simulator.h:447
armarx::objects::Scene
Definition: Scene.h:55
armarx::Simulator::reportRobotPose
bool reportRobotPose
Definition: Simulator.h:466
armarx::Simulator::addRobotFromFile
std::string addRobotFromFile(const armarx::data::PackagePath &packagePath, const Ice::Current &c=Ice::emptyCurrent) override
Definition: Simulator.cpp:2014
armarx::PackageFileLocation::package
std::string package
Name of the ArmarX package.
Definition: ObjectInfo.h:25
ExpressionException.h
armarx::control::common::getValue
T getValue(nlohmann::json &userConfig, nlohmann::json &defaultConfig, const std::string &entryName)
Definition: utils.h:71
armarx::control::njoint_controller::platform::platform_follower_controller::NameValueMap
std::map< std::string, float > NameValueMap
Definition: PlatformFollowerController.h:88
armarx::Simulator::objectReleased
void objectReleased(const std::string &robotName, const std::string &robotNodeName, const std::string &objectName, const Ice::Current &c=Ice::emptyCurrent) override
remove a joint
Definition: Simulator.cpp:1906
armarx::ArmarXDataPath::SearchReadableFile
static bool SearchReadableFile(const std::string &querryFileName, std::string &resultFileName, bool verbose=true)
Definition: ArmarXDataPath.cpp:216
armarx::Simulator::addObject
void addObject(const memoryx::ObjectClassBasePtr &objectClassBase, const std::string &instanceName, const PoseBasePtr &globalPose, bool isStatic=false, const Ice::Current &c=Ice::emptyCurrent) override
Definition: Simulator.cpp:1413
armarx::Simulator::classInstanceMap
std::map< std::string, std::vector< std::string > > classInstanceMap
Stores all instances that belong to a class.
Definition: Simulator.h:491
armarx::Simulator::hasObject
bool hasObject(const std::string &instanceName, const Ice::Current &c=Ice::emptyCurrent) override
Definition: Simulator.cpp:2224
armarx::Simulator::timeTopicPrx
TimeServerListenerPrx timeTopicPrx
Definition: Simulator.h:463
armarx::Simulator::onConnectComponent
void onConnectComponent() override
Pure virtual hook for the subclass.
Definition: Simulator.cpp:981
armarx::ctrlutil::v
double v(double t, double v0, double a0, double j)
Definition: CtrlUtil.h:39
IceStorm::parser
Parser * parser
Definition: Parser.cpp:34
ObjectInstance.h
memoryx::Box
Definition: Box.hpp:32
armarx::Simulator::getRobotJointAngles
NameValueMap getRobotJointAngles(const std::string &robotName, const Ice::Current &c=Ice::emptyCurrent) override
Definition: Simulator.cpp:1737
armarx::ComponentPropertyDefinitions
Default component property definition container.
Definition: Component.h:69
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:181
armarx::Simulator::priorKnowledgeName
std::string priorKnowledgeName
Definition: Simulator.h:472
memoryx::GridFileManagerPtr
std::shared_ptr< GridFileManager > GridFileManagerPtr
Definition: AbstractEntityWrapper.h:33
armarx::Simulator::applyTorqueRobotNode
void applyTorqueRobotNode(const std::string &robotName, const std::string &robotNodeName, const Vector3BasePtr &torque, const Ice::Current &c=Ice::emptyCurrent) override
Definition: Simulator.cpp:1385
armarx::PropertyUser::getProperty
Property< PropertyType > getProperty(const std::string &name)
Property creation and retrieval.
Definition: PropertyUser.h:180
armarx::Simulator::shutdownSimulationLoop
void shutdownSimulationLoop()
stop the simulation and join the simulation thread
Definition: Simulator.cpp:1087
armarx::Simulator::setRobotMaxTorque
void setRobotMaxTorque(const std::string &robotName, const std::string &nodeName, float maxTorque, const Ice::Current &c=Ice::emptyCurrent) override
Definition: Simulator.cpp:1808
armarx::PackageFileLocation::absolutePath
std::filesystem::path absolutePath
The absolute path (in the host's file system).
Definition: ObjectInfo.h:30
armarx::ManagedIceObject::getTopic
TopicProxyType getTopic(const std::string &name)
Returns a proxy of the specified topic.
Definition: ManagedIceObject.h:480
armarx::Quaternion< float, 0 >
armarx::ManagedIceObject::offeringTopic
void offeringTopic(const std::string &name)
Registers a topic for retrival after initialization.
Definition: ManagedIceObject.cpp:300
armarx::BulletPhysicsWorld
The BulletPhysicsWorld class encapsulates the whole physics simulation and the corresponding data.
Definition: BulletPhysicsWorld.h:53
armarx::KinematicsWorld
The KinematicsWorld class encapsulates the kinemtics simulation and the corresponding data....
Definition: KinematicsWorld.h:49
armarx::Simulator::getRobotJointVelocities
NameValueMap getRobotJointVelocities(const std::string &robotName, const Ice::Current &c=Ice::emptyCurrent) override
Definition: Simulator.cpp:1751
armarx::SimulatorPropertyDefinitions::SimulatorPropertyDefinitions
SimulatorPropertyDefinitions(std::string prefix)
Definition: Simulator.cpp:72
armarx::Simulator::getSimTime
float getSimTime(const Ice::Current &c=Ice::emptyCurrent) override
Definition: Simulator.cpp:1926
armarx::ends_with
bool ends_with(const std::string &haystack, const std::string &needle)
Definition: StringHelpers.cpp:53
memoryx::KBM::Vector3
Eigen::Vector3d Vector3
Definition: kbm.h:43
armarx::Simulator::setSpeed
void setSpeed(Ice::Float newSpeed, const Ice::Current &c=Ice::emptyCurrent) override
setSpeed sets the scaling factor for the speed of passing of time e.g.
Definition: Simulator.cpp:2298
armarx::ObjectFinder::findObject
std::optional< ObjectInfo > findObject(const std::string &dataset, const std::string &name) const
Definition: ObjectFinder.cpp:64
armarx::Simulator::reInitialize
void reInitialize(const Ice::Current &c=Ice::emptyCurrent) override
reInitialize Re-initializes the scene. Removes all robots and objects (and, in case the scene was loa...
Definition: Simulator.cpp:2215
scene3D::SceneObjectPtr
boost::intrusive_ptr< SceneObject > SceneObjectPtr
Definition: PointerDefinitions.h:40
armarx::VariantType::Int
const VariantTypeId Int
Definition: Variant.h:917
memoryx::GridFileManager
GridFileManager provides utility functions for working with files in Mongo GridFS and links to them s...
Definition: GridFileManager.h:41
MemoryXTypesObjectFactories.h
AgentInstance.h
armarx::Simulator::setObjectPose
void setObjectPose(const std::string &objectName, const PoseBasePtr &globalPose, const Ice::Current &c=Ice::emptyCurrent) override
Definition: Simulator.cpp:1686
armarx::Simulator::publishContacts
bool publishContacts
Definition: Simulator.h:485
armarx::Logging::deactivateSpam
SpamFilterDataPtr deactivateSpam(float deactivationDurationSec=10.0f, const std::string &identifier="", bool deactivate=true) const
disables the logging for the current line for the given amount of seconds.
Definition: Logging.cpp:99
armarx::Simulator::getRobotLinearVelocity
Vector3BasePtr getRobotLinearVelocity(const std::string &robotName, const std::string &nodeName, const Ice::Current &c=Ice::emptyCurrent) override
Definition: Simulator.cpp:1826
armarx::Simulator::getRobotState
SimulatedRobotState getRobotState(const std::string &robotName, const Ice::Current &) override
Definition: Simulator.cpp:2628
httplib::detail::trim
std::pair< size_t, size_t > trim(const char *b, const char *e, size_t left, size_t right)
Definition: httplib.h:2787
armarx::ArmarXDataPath::getAbsolutePath
static bool getAbsolutePath(const std::string &relativeFilename, std::string &storeAbsoluteFilename, const std::vector< std::string > &additionalSearchPaths={}, bool verbose=true)
Definition: ArmarXDataPath.cpp:109
armarx::PeriodicTask
Definition: ArmarXManager.h:70
armarx::Simulator::getRobotForceTorqueSensors
ForceTorqueDataSeq getRobotForceTorqueSensors(const std::string &robotName, const Ice::Current &c=Ice::emptyCurrent) override
Definition: Simulator.cpp:1771
armarx::navigation::core::Position
Eigen::Vector3f Position
Definition: basic_types.h:36
armarx::Simulator::step
void step(const Ice::Current &c=Ice::emptyCurrent) override
Definition: Simulator.cpp:2322
armarx::handleExceptions
void handleExceptions()
Definition: Exception.cpp:157
armarx::Simulator::physicsWorld
SimulatedWorldPtr physicsWorld
Definition: Simulator.h:450
ARMARX_WARNING
#define ARMARX_WARNING
Definition: Logging.h:193
armarx::Simulator::objectFinder
ObjectFinder objectFinder
Constructed once based on the ObjectPackage property.
Definition: Simulator.h:480
armarx::Simulator::simulatorVisuUpdateListenerPrx
SimulatorListenerInterfacePrx simulatorVisuUpdateListenerPrx
Definition: Simulator.h:468
Simulator.h
armarx::Simulator::objectGrasped
void objectGrasped(const std::string &robotName, const std::string &robotNodeName, const std::string &objectName, const Ice::Current &c=Ice::emptyCurrent) override
create a joint
Definition: Simulator.cpp:1916
ArmarXDataPath.h
armarx::PackagePath
Definition: PackagePath.h:52
armarx::Simulator::getRobotJointLimitLo
float getRobotJointLimitLo(const std::string &robotName, const std::string &nodeName, const Ice::Current &c=Ice::emptyCurrent) override
Definition: Simulator.cpp:1777
armarx::Simulator::globalRobotLocalization
GlobalRobotPoseLocalizationListenerPrx globalRobotLocalization
Definition: Simulator.h:465
armarx::ManagedIceObject::usingProxy
bool usingProxy(const std::string &name, const std::string &endpoints="")
Registers a proxy for retrieval after initialization and adds it to the dependency list.
Definition: ManagedIceObject.cpp:154
armarx::Simulator::getScene
SceneVisuData getScene(const Ice::Current &c=Ice::emptyCurrent) override
Definition: Simulator.cpp:1932
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:27
armarx::RobotInfo::pose
Eigen::Matrix4f pose
Definition: SimulatedWorld.h:83
armarx::ObjectID::str
std::string str() const
Return "dataset/className" or "dataset/className/instanceName".
Definition: ObjectID.cpp:60
VirtualRobot::RobotPtr
std::shared_ptr< class Robot > RobotPtr
Definition: Bus.h:19
armarx::Simulator::currentSyncTimeMS
float currentSyncTimeMS
stores the time that was needed to sync the data
Definition: Simulator.h:458
armarx::split
std::vector< std::string > split(const std::string &source, const std::string &splitBy, bool trimElements=false, bool removeEmptyElements=false)
Definition: StringHelpers.cpp:38
armarx::Simulator::simulationLoop
void simulationLoop()
Definition: Simulator.cpp:2125
armarx::Simulator::loadAgentsFromSnapshot
bool loadAgentsFromSnapshot(memoryx::WorkingMemorySnapshotInterfacePrx snapshotInterfacePrx)
Definition: Simulator.cpp:1270
armarx::Simulator::setObjectSimulationType
void setObjectSimulationType(const std::string &objectName, armarx::SimulationType type, const Ice::Current &c=Ice::emptyCurrent) override
Definition: Simulator.cpp:1704
armarx::SimulatorType::Mujoco
@ Mujoco
PackagePath.h
armarx::SimulatedWorldData
The SimulatedWorldData class This data is queried by the simulated in order to offer it via topics.
Definition: SimulatedWorld.h:107
armarx::Simulator::applyForceRobotNode
void applyForceRobotNode(const std::string &robotName, const std::string &robotNodeName, const Vector3BasePtr &force, const Ice::Current &c=Ice::emptyCurrent) override
Definition: Simulator.cpp:1375
armarx::Simulator::updateContacts
void updateContacts(bool enable, const Ice::Current &c=Ice::emptyCurrent) override
Enables the handling of contacts. If you intend to use getContacts(), call this method with enabled =...
Definition: Simulator.cpp:2583
armarx::Simulator::longtermMemoryPrx
memoryx::LongtermMemoryInterfacePrx longtermMemoryPrx
Definition: Simulator.h:476