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