diff --git a/source/ArmarXSimulation/components/SimulationObjectPoseProvider/SimulationObjectPoseProvider.cpp b/source/ArmarXSimulation/components/SimulationObjectPoseProvider/SimulationObjectPoseProvider.cpp
index 71494e3ad679976f782e1bcb11a4953c3be7be7c..92a706add66a1fdf01e807ba8fbe013f4b7f7e9e 100644
--- a/source/ArmarXSimulation/components/SimulationObjectPoseProvider/SimulationObjectPoseProvider.cpp
+++ b/source/ArmarXSimulation/components/SimulationObjectPoseProvider/SimulationObjectPoseProvider.cpp
@@ -218,6 +218,9 @@ namespace armarx
continue;
}
+ ARMARX_DEBUG << deactivateSpam(10) << "Received " << sceneData.objects.size()
+ << " objects and " << sceneData.robots.size() << " robots.";
+
std::vector<objpose::ProvidedObjectPose> providedObjects;
if (properties.requestAllEntities)
@@ -236,8 +239,8 @@ namespace armarx
// Report the poses
try
{
- ARMARX_IMPORTANT << deactivateSpam(5) << "reporting " << providedObjects.size()
- << " object poses";
+ ARMARX_DEBUG << deactivateSpam(10) << "Reporting " << providedObjects.size()
+ << " object poses";
objectPoseTopic->reportObjectPoses(getName(), objpose::toIce(providedObjects));
}
catch (const Ice::LocalException& e)
diff --git a/source/ArmarXSimulation/components/Simulator/SimulatedWorld.cpp b/source/ArmarXSimulation/components/Simulator/SimulatedWorld.cpp
index 6d3d3de85e3b7482ca6402ed2d306cff71193e52..efc529fba2f9714227cc1d261fd00f61e8beab20 100644
--- a/source/ArmarXSimulation/components/Simulator/SimulatedWorld.cpp
+++ b/source/ArmarXSimulation/components/Simulator/SimulatedWorld.cpp
@@ -30,19 +30,19 @@
#include "SimulatedWorld.h"
-#include <VirtualRobot/Nodes/ForceTorqueSensor.h>
+#include <algorithm>
+#include <memory>
+
#include <VirtualRobot/CollisionDetection/CollisionChecker.h>
+#include <VirtualRobot/Nodes/ForceTorqueSensor.h>
+#include <VirtualRobot/XML/ObjectIO.h>
#include <VirtualRobot/XML/RobotIO.h>
#include <VirtualRobot/XML/SceneIO.h>
-#include <VirtualRobot/XML/ObjectIO.h>
+#include <ArmarXCore/core/exceptions/LocalException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/time/TimeUtil.h>
-#include <algorithm>
-
-#include <memory>
-#include <ArmarXCore/core/exceptions/LocalException.h>
using namespace VirtualRobot;
using namespace armarx;
@@ -60,12 +60,14 @@ SimulatedWorld::SimulatedWorld()
collectContacts = false;
}
-void SimulatedWorld::activateObject(const std::string& objectName)
+void
+SimulatedWorld::activateObject(const std::string& objectName)
{
- (void) objectName; // unused
+ (void)objectName; // unused
}
-bool SimulatedWorld::resetData()
+bool
+SimulatedWorld::resetData()
{
ARMARX_DEBUG_S << "Removing all robots & objects";
const bool resetRobots = removeRobots();
@@ -74,7 +76,8 @@ bool SimulatedWorld::resetData()
return resetRobots && resetObstacles;
}
-double SimulatedWorld::getCurrentSimTime()
+double
+SimulatedWorld::getCurrentSimTime()
{
// Return time of last update of robot/object data instead of last step.
// => The caller can rely on robot/object data being synced to this time.
@@ -83,18 +86,20 @@ double SimulatedWorld::getCurrentSimTime()
return currentSyncTimeSec;
}
-void SimulatedWorld::resetSimTime()
+void
+SimulatedWorld::resetSimTime()
{
currentSimTimeSec = 0;
}
-bool SimulatedWorld::removeRobots()
+bool
+SimulatedWorld::removeRobots()
{
ARMARX_DEBUG_S << "Removing all robots";
bool res = true;
- std::vector< std::string > names = getRobotNames();
+ std::vector<std::string> names = getRobotNames();
for (auto n : names)
{
res &= removeRobot(n);
@@ -103,20 +108,29 @@ bool SimulatedWorld::removeRobots()
return res;
}
-
-SimulatedWorldData& SimulatedWorld::getReportData()
+SimulatedWorldData&
+SimulatedWorld::getReportData()
{
return simReportData;
}
-SimulatedWorldData SimulatedWorld::copyReportData()
+SimulatedWorldData
+SimulatedWorld::copyReportData()
{
ScopedRecursiveLockPtr lockSync = getScopedSyncLock(__FUNCTION__);
return simReportData;
}
-
-bool SimulatedWorld::addRobot(RobotPtr robot, double pid_p, double pid_i, double pid_d, const std::string& filename, bool staticRobot, float scaling, bool colModel, bool selfCollisions)
+bool
+SimulatedWorld::addRobot(RobotPtr robot,
+ double pid_p,
+ double pid_i,
+ double pid_d,
+ const std::string& filename,
+ bool staticRobot,
+ float scaling,
+ bool colModel,
+ bool selfCollisions)
{
ARMARX_VERBOSE_S << "Adding robot " << robot->getName() << ", file:" << filename;
assert(robot);
@@ -188,10 +202,14 @@ bool SimulatedWorld::addRobot(RobotPtr robot, double pid_p, double pid_i, double
return true;
}
-void SimulatedWorld::objectReleased(const std::string& robotName, const std::string& robotNodeName, const std::string& objectName)
+void
+SimulatedWorld::objectReleased(const std::string& robotName,
+ const std::string& robotNodeName,
+ const std::string& objectName)
{
ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
- ARMARX_INFO_S << "Object released, robot " << robotName << ", node:" << robotNodeName << ", object:" << objectName;
+ ARMARX_INFO_S << "Object released, robot " << robotName << ", node:" << robotNodeName
+ << ", object:" << objectName;
if (objectReleasedEngine(robotName, robotNodeName, objectName))
{
@@ -201,7 +219,8 @@ void SimulatedWorld::objectReleased(const std::string& robotName, const std::str
while (aoIt != attachedObjects.end())
{
- if (aoIt->robotName == robotName && aoIt->robotNodeName == robotNodeName && aoIt->objectName == objectName)
+ if (aoIt->robotName == robotName && aoIt->robotNodeName == robotNodeName &&
+ aoIt->objectName == objectName)
{
attachedObjects.erase(aoIt);
objectFound = true;
@@ -220,15 +239,20 @@ void SimulatedWorld::objectReleased(const std::string& robotName, const std::str
}
}
-void SimulatedWorld::objectGrasped(const std::string& robotName, const std::string& robotNodeName, const std::string& objectName)
+void
+SimulatedWorld::objectGrasped(const std::string& robotName,
+ const std::string& robotNodeName,
+ const std::string& objectName)
{
ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
- ARMARX_INFO_S << "Object grasped, robot " << robotName << ", node:" << robotNodeName << ", object:" << objectName;
+ ARMARX_INFO_S << "Object grasped, robot " << robotName << ", node:" << robotNodeName
+ << ", object:" << objectName;
// check if object is already grasped
for (const auto& ao : attachedObjects)
{
- if (ao.robotName == robotName && ao.robotNodeName == robotNodeName && ao.objectName == objectName)
+ if (ao.robotName == robotName && ao.robotNodeName == robotNodeName &&
+ ao.objectName == objectName)
{
ARMARX_INFO_S << "Object already attached, skipping...";
return;
@@ -245,14 +269,15 @@ void SimulatedWorld::objectGrasped(const std::string& robotName, const std::stri
g.robotNodeName = robotNodeName;
g.objectName = objectName;
- g.node2objectTransformation = localTransform;//rn->toLocalCoordinateSystem(o->getSceneObject()->getGlobalPose());
+ g.node2objectTransformation =
+ localTransform; //rn->toLocalCoordinateSystem(o->getSceneObject()->getGlobalPose());
attachedObjects.push_back(g);
}
}
-
-bool SimulatedWorld::removeObstacles()
+bool
+SimulatedWorld::removeObstacles()
{
ARMARX_VERBOSE_S << "Removing all obstacles";
bool result = true;
@@ -270,8 +295,8 @@ bool SimulatedWorld::removeObstacles()
return result;
}
-
-bool SimulatedWorld::removeObstacle(const std::string& name)
+bool
+SimulatedWorld::removeObstacle(const std::string& name)
{
ARMARX_VERBOSE_S << "Removing obstacle " << name;
ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
@@ -298,19 +323,27 @@ bool SimulatedWorld::removeObstacle(const std::string& name)
if (!objectFound)
{
- ARMARX_WARNING_S << "Object with name " << name << " not in synchronized list, cannot remove";
+ ARMARX_WARNING_S << "Object with name " << name
+ << " not in synchronized list, cannot remove";
}
}
return true;
}
-
-bool SimulatedWorld::addRobot(std::string& robotInstanceName, const std::string& filename,
- Eigen::Matrix4f pose, const std::string& filenameLocal,
- double pid_p, double pid_i, double pid_d,
- bool staticRobot, float scaling, bool colModel,
- const std::map<std::string, float>& initConfig, bool selfCollisions)
+bool
+SimulatedWorld::addRobot(std::string& robotInstanceName,
+ const std::string& filename,
+ Eigen::Matrix4f pose,
+ const std::string& filenameLocal,
+ double pid_p,
+ double pid_i,
+ double pid_d,
+ bool staticRobot,
+ float scaling,
+ bool colModel,
+ const std::map<std::string, float>& initConfig,
+ bool selfCollisions)
{
ARMARX_TRACE;
ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
@@ -345,7 +378,8 @@ bool SimulatedWorld::addRobot(std::string& robotInstanceName, const std::string&
catch (VirtualRobotException& e)
{
ARMARX_ERROR_S << GetHandledExceptionString()
- << " ERROR while creating robot (file:" << filename << ")\n" << e.what();
+ << " ERROR while creating robot (file:" << filename << ")\n"
+ << e.what();
return false;
}
@@ -408,7 +442,8 @@ bool SimulatedWorld::addRobot(std::string& robotInstanceName, const std::string&
}
ARMARX_TRACE;
- bool success = addRobot(robot, pid_p, pid_i, pid_d, fn, staticRobot, scaling, colModel, selfCollisions);
+ bool success =
+ addRobot(robot, pid_p, pid_i, pid_d, fn, staticRobot, scaling, colModel, selfCollisions);
ARMARX_TRACE;
if (success && initConfig.size() > 0)
@@ -419,12 +454,14 @@ bool SimulatedWorld::addRobot(std::string& robotInstanceName, const std::string&
return success;
}
-VirtualRobot::RobotPtr SimulatedWorld::adaptRobotToWorld(VirtualRobot::RobotPtr robot)
+VirtualRobot::RobotPtr
+SimulatedWorld::adaptRobotToWorld(VirtualRobot::RobotPtr robot)
{
return robot;
}
-bool SimulatedWorld::removeRobot(const std::string& robotName)
+bool
+SimulatedWorld::removeRobot(const std::string& robotName)
{
ARMARX_DEBUG_S << "Removing robot " << robotName;
@@ -489,7 +526,9 @@ bool SimulatedWorld::removeRobot(const std::string& robotName)
return true;
}
-bool SimulatedWorld::addScene(const std::string& filename, VirtualRobot::SceneObject::Physics::SimulationType simType)
+bool
+SimulatedWorld::addScene(const std::string& filename,
+ VirtualRobot::SceneObject::Physics::SimulationType simType)
{
ARMARX_INFO_S << "Loading Scene from " << filename;
ScenePtr scene;
@@ -513,7 +552,8 @@ bool SimulatedWorld::addScene(const std::string& filename, VirtualRobot::SceneOb
return addScene(scene, simType);
}
-bool SimulatedWorld::addScene(ScenePtr scene, VirtualRobot::SceneObject::Physics::SimulationType simType)
+bool
+SimulatedWorld::addScene(ScenePtr scene, VirtualRobot::SceneObject::Physics::SimulationType simType)
{
if (!scene)
{
@@ -557,11 +597,14 @@ bool SimulatedWorld::addScene(ScenePtr scene, VirtualRobot::SceneObject::Physics
return true;
}
-bool SimulatedWorld::addObstacle(const std::string& filename, const Eigen::Matrix4f& pose,
- VirtualRobot::SceneObject::Physics::SimulationType simType,
- const std::string& localFilename)
+bool
+SimulatedWorld::addObstacle(const std::string& filename,
+ const Eigen::Matrix4f& pose,
+ VirtualRobot::SceneObject::Physics::SimulationType simType,
+ const std::string& localFilename)
{
- ARMARX_INFO_S << "Loading obstacle from " << filename << ", (local file:" << localFilename << ")";
+ ARMARX_INFO_S << "Loading obstacle from " << filename << ", (local file:" << localFilename
+ << ")";
ObstaclePtr o;
try
@@ -591,27 +634,27 @@ bool SimulatedWorld::addObstacle(const std::string& filename, const Eigen::Matri
if (fileFound)
{
- ARMARX_IMPORTANT_S << "found local path:" << localFilename << " -> " << absoluteFilename;
+ ARMARX_IMPORTANT_S << "found local path:" << localFilename << " -> "
+ << absoluteFilename;
fn = localFilename;
}
else
{
- ARMARX_IMPORTANT_S << "could not find global path from local path:" << localFilename << " -> usign " << fn;
+ ARMARX_IMPORTANT_S << "could not find global path from local path:" << localFilename
+ << " -> usign " << fn;
}
}
return addObstacle(o, simType, fn);
}
-
-bool SimulatedWorld::addObstacle(
- VirtualRobot::SceneObjectPtr o,
- VirtualRobot::SceneObject::Physics::SimulationType simType,
- const std::string& filename,
- const std::string& objectClassName,
- ObjectVisuPrimitivePtr primitiveData,
- const std::string& project
- )
+bool
+SimulatedWorld::addObstacle(VirtualRobot::SceneObjectPtr o,
+ VirtualRobot::SceneObject::Physics::SimulationType simType,
+ const std::string& filename,
+ const std::string& objectClassName,
+ ObjectVisuPrimitivePtr primitiveData,
+ const std::string& project)
{
if (!o)
{
@@ -627,7 +670,8 @@ bool SimulatedWorld::addObstacle(
//VirtualRobot::SceneObjectPtr clonedObj;
if (hasObject(o->getName()))
{
- ARMARX_ERROR_S << "Object with name " << o->getName() << " already present! Choose a different instance name";
+ ARMARX_ERROR_S << "Object with name " << o->getName()
+ << " already present! Choose a different instance name";
return false;
}
@@ -661,14 +705,13 @@ bool SimulatedWorld::addObstacle(
ovd.updated = true;
simVisuData.objects.push_back(ovd);
-
}
return true;
}
-
-auto SimulatedWorld::getScopedEngineLock(const std::string& callStr) -> ScopedRecursiveLockPtr
+auto
+SimulatedWorld::getScopedEngineLock(const std::string& callStr) -> ScopedRecursiveLockPtr
{
ScopedRecursiveLockPtr l(new ScopedRecursiveLock(*engineMutex));
#ifdef PERFORMANCE_EVALUATION_LOCKS
@@ -686,19 +729,22 @@ auto SimulatedWorld::getScopedEngineLock(const std::string& callStr) -> ScopedRe
engineMtxLastTime[callStr] = IceUtil::Time::now();
if (engineMtxAccTime[callStr] > 1000)
{
- ARMARX_INFO << "engine mutex performance [" << callStr << "]: Nr Calls:" << engineMtxAccCalls[callStr] << ", Calls/Sec:" << (float)engineMtxAccCalls[callStr] / engineMtxAccTime[callStr] * 1000.0f;
+ ARMARX_INFO << "engine mutex performance [" << callStr
+ << "]: Nr Calls:" << engineMtxAccCalls[callStr] << ", Calls/Sec:"
+ << (float)engineMtxAccCalls[callStr] / engineMtxAccTime[callStr] * 1000.0f;
engineMtxAccCalls[callStr] = 1;
engineMtxAccTime[callStr] = 0;
engineMtxLastTime[callStr] = IceUtil::Time::now();
}
}
#else
- (void) callStr; // unused
+ (void)callStr; // unused
#endif
return l;
}
-auto SimulatedWorld::getScopedSyncLock(const std::string& callStr) -> ScopedRecursiveLockPtr
+auto
+SimulatedWorld::getScopedSyncLock(const std::string& callStr) -> ScopedRecursiveLockPtr
{
ScopedRecursiveLockPtr l(new ScopedRecursiveLock(*synchronizeMutex));
#ifdef PERFORMANCE_EVALUATION_LOCKS
@@ -716,20 +762,22 @@ auto SimulatedWorld::getScopedSyncLock(const std::string& callStr) -> ScopedRecu
syncMtxLastTime[callStr] = IceUtil::Time::now();
if (syncMtxAccTime[callStr] > 1000)
{
- ARMARX_INFO << "snyc mutex performance [" << callStr << "]: Nr Calls:" << syncMtxAccCalls[callStr] << ", Calls/Sec:" << (float)syncMtxAccCalls[callStr] / syncMtxAccTime[callStr] * 1000.0f;
+ ARMARX_INFO << "snyc mutex performance [" << callStr
+ << "]: Nr Calls:" << syncMtxAccCalls[callStr] << ", Calls/Sec:"
+ << (float)syncMtxAccCalls[callStr] / syncMtxAccTime[callStr] * 1000.0f;
syncMtxAccCalls[callStr] = 1;
syncMtxAccTime[callStr] = 0;
syncMtxLastTime[callStr] = IceUtil::Time::now();
}
}
#else
- (void) callStr; // unused
+ (void)callStr; // unused
#endif
return l;
}
-
-void SimulatedWorld::setupFloor(bool enable, const std::string& floorTexture)
+void
+SimulatedWorld::setupFloor(bool enable, const std::string& floorTexture)
{
setupFloorEngine(enable, floorTexture);
@@ -740,33 +788,38 @@ void SimulatedWorld::setupFloor(bool enable, const std::string& floorTexture)
}
}
-float SimulatedWorld::getSimulationStepDuration()
+float
+SimulatedWorld::getSimulationStepDuration()
{
return simTimeStepMS;
}
-float SimulatedWorld::getSimulationStepTimeMeasured()
+float
+SimulatedWorld::getSimulationStepTimeMeasured()
{
return simStepExecutionDurationMS;
}
-void SimulatedWorld::enableLogging(const std::string& robotName, const std::string& logFile)
+void
+SimulatedWorld::enableLogging(const std::string& robotName, const std::string& logFile)
{
- (void) robotName, (void) logFile; // unused
+ (void)robotName, (void)logFile; // unused
}
-float SimulatedWorld::getSimTime()
+float
+SimulatedWorld::getSimTime()
{
return simStepExecutionDurationMS;
}
-float SimulatedWorld::getSyncEngineTime()
+float
+SimulatedWorld::getSyncEngineTime()
{
return synchronizeDurationMS;
}
-
-bool SimulatedWorld::synchronizeRobots()
+bool
+SimulatedWorld::synchronizeRobots()
{
// lock engine and data
ScopedRecursiveLockPtr lockEngine = getScopedEngineLock(__FUNCTION__);
@@ -807,8 +860,7 @@ bool SimulatedWorld::synchronizeRobots()
robot.jointVelocities,
robot.jointTorques,
robot.linearVelocity,
- robot.angularVelocity
- );
+ robot.angularVelocity);
if (visuRobotPtr.count(robot.robotName))
{
visuRobotPtr.at(robot.robotName)->jointValues = robot.jointAngles;
@@ -824,8 +876,8 @@ bool SimulatedWorld::synchronizeRobots()
return true;
}
-
-bool SimulatedWorld::synchronizeSimulationData()
+bool
+SimulatedWorld::synchronizeSimulationData()
{
IceUtil::Time startTime = IceUtil::Time::now();
@@ -837,7 +889,9 @@ bool SimulatedWorld::synchronizeSimulationData()
if (durationlock.toMilliSecondsDouble() > 4)
{
- ARMARX_INFO << deactivateSpam(10) << "Engine/Sync locking took long :" << durationlock.toMilliSecondsDouble() << " ms";
+ ARMARX_INFO << deactivateSpam(10)
+ << "Engine/Sync locking took long :" << durationlock.toMilliSecondsDouble()
+ << " ms";
}
simVisuData.timestamp = static_cast<Ice::Long>(getCurrentSimTime() * 1000);
@@ -849,7 +903,8 @@ bool SimulatedWorld::synchronizeSimulationData()
IceUtil::Time durationR = IceUtil::Time::now() - startTimeR;
if (durationR.toMilliSecondsDouble() > 4)
{
- ARMARX_INFO << deactivateSpam(10) << "Robot sync took long:" << durationR.toMilliSecondsDouble() << " ms";
+ ARMARX_INFO << deactivateSpam(10)
+ << "Robot sync took long:" << durationR.toMilliSecondsDouble() << " ms";
}
IceUtil::Time startTimeO = IceUtil::Time::now();
@@ -858,7 +913,8 @@ bool SimulatedWorld::synchronizeSimulationData()
IceUtil::Time durationO = IceUtil::Time::now() - startTimeO;
if (durationO.toMilliSecondsDouble() > 4)
{
- ARMARX_INFO << deactivateSpam(10) << "Object sync took long:" << durationO.toMilliSecondsDouble() << " ms";
+ ARMARX_INFO << deactivateSpam(10)
+ << "Object sync took long:" << durationO.toMilliSecondsDouble() << " ms";
}
// CONTACTS
@@ -872,20 +928,21 @@ bool SimulatedWorld::synchronizeSimulationData()
return true;
}
-void SimulatedWorld::updateContacts(bool enable)
+void
+SimulatedWorld::updateContacts(bool enable)
{
collectContacts = enable;
}
-
-SceneVisuData SimulatedWorld::copySceneVisuData()
+SceneVisuData
+SimulatedWorld::copySceneVisuData()
{
ScopedRecursiveLockPtr lock = getScopedSyncLock(__FUNCTION__);
return simVisuData;
}
-
-int SimulatedWorld::getRobotJointAngleCount()
+int
+SimulatedWorld::getRobotJointAngleCount()
{
ScopedRecursiveLockPtr l = getScopedSyncLock(__FUNCTION__);
@@ -897,13 +954,15 @@ int SimulatedWorld::getRobotJointAngleCount()
return static_cast<int>(simVisuData.robots.at(0).robotNodePoses.size());
}
-int SimulatedWorld::getContactCount()
+int
+SimulatedWorld::getContactCount()
{
ScopedRecursiveLockPtr l = getScopedSyncLock(__FUNCTION__);
return 0;
}
-int SimulatedWorld::getObjectCount()
+int
+SimulatedWorld::getObjectCount()
{
ScopedRecursiveLockPtr l = getScopedSyncLock(__FUNCTION__);
return static_cast<int>(simVisuData.objects.size());
diff --git a/source/ArmarXSimulation/components/Simulator/Simulator.cpp b/source/ArmarXSimulation/components/Simulator/Simulator.cpp
index f8531649d8c9f9e8399bd0da662946bc8455cbaa..abb022dcf19968f215df668674d894c0a855c065 100644
--- a/source/ArmarXSimulation/components/Simulator/Simulator.cpp
+++ b/source/ArmarXSimulation/components/Simulator/Simulator.cpp
@@ -23,6 +23,7 @@
*/
#include <filesystem>
+
#include "ArmarXCore/core/PackagePath.h"
#define MAX_SIM_TIME_WARNING 25
@@ -32,72 +33,84 @@
#include "MujocoPhysicsWorld.h"
#endif
-#include <sdf/sdf.hh>
-
#include <SimoxUtility/algorithm/string/string_tools.h>
#include <SimoxUtility/json.h>
-
-#include <VirtualRobot/math/Helpers.h>
#include <VirtualRobot/Scene.h>
#include <VirtualRobot/SceneObject.h>
#include <VirtualRobot/XML/ObjectIO.h>
+#include <VirtualRobot/math/Helpers.h>
-#include <SimDynamics/DynamicsEngine/DynamicsObject.h>
-#include <SimDynamics/DynamicsEngine/DynamicsRobot.h>
-
-#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/ArmarXManager.h>
+#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
+#include <RobotAPI/interface/core/GeometryBase.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
#include <RobotAPI/libraries/ArmarXObjects/Scene.h>
#include <RobotAPI/libraries/ArmarXObjects/json_conversions.h>
-#include <RobotAPI/interface/core/GeometryBase.h>
-#include <RobotAPI/libraries/core/Pose.h>
#include <RobotAPI/libraries/core/FramedPose.h>
+#include <RobotAPI/libraries/core/Pose.h>
#include <MemoryX/core/MemoryXCoreObjectFactories.h>
#include <MemoryX/libraries/memorytypes/MemoryXTypesObjectFactories.h>
-#include <MemoryX/libraries/memorytypes/entity/ObjectInstance.h>
-#include <MemoryX/libraries/memorytypes/entity/ObjectClass.h>
#include <MemoryX/libraries/memorytypes/entity/AgentInstance.h>
+#include <MemoryX/libraries/memorytypes/entity/ObjectClass.h>
+#include <MemoryX/libraries/memorytypes/entity/ObjectInstance.h>
+#include <SimDynamics/DynamicsEngine/DynamicsObject.h>
+#include <SimDynamics/DynamicsEngine/DynamicsRobot.h>
+#include <sdf/sdf.hh>
using namespace armarx;
using namespace memoryx;
-
SimulatorPropertyDefinitions::SimulatorPropertyDefinitions(std::string prefix) :
ComponentPropertyDefinitions(prefix)
{
- defineOptionalProperty<std::string>("SimoxSceneFileName", "", "Simox/VirtualRobot scene file name, e.g. myScene.xml");
+ defineOptionalProperty<std::string>(
+ "SimoxSceneFileName", "", "Simox/VirtualRobot scene file name, e.g. myScene.xml");
defineOptionalProperty<bool>("FloorPlane", true, "Enable floor plane.");
defineOptionalProperty<std::string>("FloorTexture", "", "Texture file for floor.");
- defineOptionalProperty<std::string>("LongtermMemory.SnapshotName", "", "Name of snapshot to load the scene").setCaseInsensitive(true);
- defineOptionalProperty<bool>("LoadSnapshotToWorkingMemory", true, "Load the snapshot also into the WorkingMemory");
- defineOptionalProperty<bool>("LoadAgentsFromSnapshot", false, "Also load the agents from the snapshot into the WorkingMemory");
+ defineOptionalProperty<std::string>(
+ "LongtermMemory.SnapshotName", "", "Name of snapshot to load the scene")
+ .setCaseInsensitive(true);
+ defineOptionalProperty<bool>(
+ "LoadSnapshotToWorkingMemory", true, "Load the snapshot also into the WorkingMemory");
+ defineOptionalProperty<bool>("LoadAgentsFromSnapshot",
+ false,
+ "Also load the agents from the snapshot into the WorkingMemory");
- defineOptionalProperty<std::string>("WorkingMemoryName", "WorkingMemory", "Name of WorkingMemory").setCaseInsensitive(true);
- defineOptionalProperty<std::string>("CommonStorageName", "CommonStorage", "Name of CommonStorage").setCaseInsensitive(true);
- defineOptionalProperty<std::string>("PriorKnowledgeName", "PriorKnowledge", "Name of PriorKnowledge").setCaseInsensitive(true);
- defineOptionalProperty<std::string>("LongtermMemoryName", "LongtermMemory", "Name of LongtermMemory").setCaseInsensitive(true);
+ defineOptionalProperty<std::string>(
+ "WorkingMemoryName", "WorkingMemory", "Name of WorkingMemory")
+ .setCaseInsensitive(true);
+ defineOptionalProperty<std::string>(
+ "CommonStorageName", "CommonStorage", "Name of CommonStorage")
+ .setCaseInsensitive(true);
+ defineOptionalProperty<std::string>(
+ "PriorKnowledgeName", "PriorKnowledge", "Name of PriorKnowledge")
+ .setCaseInsensitive(true);
+ defineOptionalProperty<std::string>(
+ "LongtermMemoryName", "LongtermMemory", "Name of LongtermMemory")
+ .setCaseInsensitive(true);
- defineOptionalProperty<std::string>("Scene.ObjectPackage", "PriorKnowledgeData",
+ defineOptionalProperty<std::string>("Scene.ObjectPackage",
+ "PriorKnowledgeData",
"Package to search for object models."
"Used when loading scenes from JSON files.");
- defineOptionalProperty<std::string>("Scene.Path", "",
+ defineOptionalProperty<std::string>("Scene.Path",
+ "",
"Path to one or multiple JSON scene file(s) to load.\n"
"Example: 'PriorKnowledgeData/scenes/MyScene.json'.\n"
"Multiple paths are separated by semicolons (';').\n"
"The package to load a scene from is automatically"
"extracted from its first path segment.");
- defineOptionalProperty<SimulatorType>(
- "SimulationType", SimulatorType::Bullet,
- "Simulation type (Supported: Bullet, Kinematics, Mujoco)")
- .map("bullet", SimulatorType::Bullet)
- .map("kinematics", SimulatorType::Kinematics)
- .map("mujoco", SimulatorType::Mujoco)
- .setCaseInsensitive(true);
+ defineOptionalProperty<SimulatorType>("SimulationType",
+ SimulatorType::Bullet,
+ "Simulation type (Supported: Bullet, Kinematics, Mujoco)")
+ .map("bullet", SimulatorType::Bullet)
+ .map("kinematics", SimulatorType::Kinematics)
+ .map("mujoco", SimulatorType::Mujoco)
+ .setCaseInsensitive(true);
for (int i = -1; i < MAX_INITIAL_ROBOT_COUNT - 1; ++i)
{
@@ -107,78 +120,119 @@ SimulatorPropertyDefinitions::SimulatorPropertyDefinitions(std::string prefix) :
postfix = "_" + ValueToString(i);
}
- defineOptionalProperty<std::string>("RobotFileName" + postfix, "", "Simox/VirtualRobot robot file name, e.g. robot_model.xml");
- defineOptionalProperty<std::string>("RobotInstanceName" + postfix, "", "Name of this robot instance");
+ defineOptionalProperty<std::string>(
+ "RobotFileName" + postfix,
+ "",
+ "Simox/VirtualRobot robot file name, e.g. robot_model.xml");
+ defineOptionalProperty<std::string>(
+ "RobotInstanceName" + postfix, "", "Name of this robot instance");
defineOptionalProperty<bool>(
- "RobotIsStatic" + postfix, false,
- "A static robot does not move due to the physical environment (i.e. no gravity). \n"
- "It is a static (but kinematic) object in the world.");
+ "RobotIsStatic" + postfix,
+ false,
+ "A static robot does not move due to the physical environment (i.e. no gravity). \n"
+ "It is a static (but kinematic) object in the world.");
defineOptionalProperty<bool>("ReportRobotPose", false, "Report the global robot pose.");
- defineOptionalProperty<bool>("RobotCollisionModel" + postfix, false, "Show the collision model of the robot.");
- defineOptionalProperty<float>("InitialRobotPose.x" + postfix, 0.f, "x component of initial robot position (mm)");
- defineOptionalProperty<float>("InitialRobotPose.y" + postfix, 0.f, "y component of initial robot position (mm)");
- defineOptionalProperty<float>("InitialRobotPose.z" + postfix, 0.f, "z component of initial robot position (mm)");
- defineOptionalProperty<float>("InitialRobotPose.roll" + postfix, 0.f, "Initial robot pose: roll component of RPY angles (radian)");
- defineOptionalProperty<float>("InitialRobotPose.pitch" + postfix, 0.f, "Initial robot pose: pitch component of RPY angles (radian)");
- defineOptionalProperty<float>("InitialRobotPose.yaw" + postfix, 0.f, "Initial robot pose: yaw component of RPY angles (radian)");
- defineOptionalProperty<std::string>("InitialRobotConfig" + postfix, "", "Initial robot config as comma separated list (RobotNodeName:value)");
- defineOptionalProperty<float>("RobotScaling" + postfix, 1.0f, "Scaling of the robot (1 = no scaling).");
- defineOptionalProperty<double>("RobotControllerPID.p" + postfix, 10.0, "Setup robot controllers: PID paramter p.");
- defineOptionalProperty<double>("RobotControllerPID.i" + postfix, 0.0, "Setup robot controllers: PID paramter i.");
- defineOptionalProperty<double>("RobotControllerPID.d" + postfix, 0.0, "Setup robot controllers: PID paramter d.");
defineOptionalProperty<bool>(
- "RobotSelfCollisions" + postfix, false,
- "If false, the robot bodies will not collide with other bodies of this \n"
- "robot but with all other bodies in the world. \n"
- "If true, the robot bodies will collide with all other bodies \n"
- "(This needs an accurately modelled robot model without self collisions \n"
- "if the robot does not move.)");
+ "RobotCollisionModel" + postfix, false, "Show the collision model of the robot.");
+ defineOptionalProperty<float>(
+ "InitialRobotPose.x" + postfix, 0.f, "x component of initial robot position (mm)");
+ defineOptionalProperty<float>(
+ "InitialRobotPose.y" + postfix, 0.f, "y component of initial robot position (mm)");
+ defineOptionalProperty<float>(
+ "InitialRobotPose.z" + postfix, 0.f, "z component of initial robot position (mm)");
+ defineOptionalProperty<float>("InitialRobotPose.roll" + postfix,
+ 0.f,
+ "Initial robot pose: roll component of RPY angles (radian)");
+ defineOptionalProperty<float>("InitialRobotPose.pitch" + postfix,
+ 0.f,
+ "Initial robot pose: pitch component of RPY angles (radian)");
+ defineOptionalProperty<float>("InitialRobotPose.yaw" + postfix,
+ 0.f,
+ "Initial robot pose: yaw component of RPY angles (radian)");
+ defineOptionalProperty<std::string>(
+ "InitialRobotConfig" + postfix,
+ "",
+ "Initial robot config as comma separated list (RobotNodeName:value)");
+ defineOptionalProperty<float>(
+ "RobotScaling" + postfix, 1.0f, "Scaling of the robot (1 = no scaling).");
+ defineOptionalProperty<double>(
+ "RobotControllerPID.p" + postfix, 10.0, "Setup robot controllers: PID paramter p.");
+ defineOptionalProperty<double>(
+ "RobotControllerPID.i" + postfix, 0.0, "Setup robot controllers: PID paramter i.");
+ defineOptionalProperty<double>(
+ "RobotControllerPID.d" + postfix, 0.0, "Setup robot controllers: PID paramter d.");
+ defineOptionalProperty<bool>(
+ "RobotSelfCollisions" + postfix,
+ false,
+ "If false, the robot bodies will not collide with other bodies of this \n"
+ "robot but with all other bodies in the world. \n"
+ "If true, the robot bodies will collide with all other bodies \n"
+ "(This needs an accurately modelled robot model without self collisions \n"
+ "if the robot does not move.)");
}
defineOptionalProperty<int>(
- "FixedTimeStepLoopNrSteps", 10,
- "The maximum number of internal simulation loops (fixed time step mode).\n"
- "After max number of time steps, the remaining time is interpolated.");
+ "FixedTimeStepLoopNrSteps",
+ 10,
+ "The maximum number of internal simulation loops (fixed time step mode).\n"
+ "After max number of time steps, the remaining time is interpolated.");
defineOptionalProperty<int>(
- "FixedTimeStepStepTimeMS", 16,
- "The simulation's internal timestep (fixed time step mode). \n"
- "This property determines the precision of the simulation. \n"
- "Needs to be set to bigger values for slow PCs, but the simulation results might be bad. "
- "Smaller value for higher precision, but slower calculation.");
+ "FixedTimeStepStepTimeMS",
+ 16,
+ "The simulation's internal timestep (fixed time step mode). \n"
+ "This property determines the precision of the simulation. \n"
+ "Needs to be set to bigger values for slow PCs, but the simulation results might be bad. "
+ "Smaller value for higher precision, but slower calculation.");
defineOptionalProperty<int>("StepTimeMS", 25, "The simulation's time progress per step. ");
defineOptionalProperty<bool>(
- "RealTimeMode", false,
- "If true the timestep is adjusted to the computing power of the host and \n"
- "the time progress in real time. Can result in bad results on slow PCs. \n"
- "The property StepTimeMS has then no effect. FixedTimeStepStepTimeMS needs \n"
- "to be adjusted if real time cannot be achieved on slower PCs.");
+ "RealTimeMode",
+ false,
+ "If true the timestep is adjusted to the computing power of the host and \n"
+ "the time progress in real time. Can result in bad results on slow PCs. \n"
+ "The property StepTimeMS has then no effect. FixedTimeStepStepTimeMS needs \n"
+ "to be adjusted if real time cannot be achieved on slower PCs.");
defineOptionalProperty<float>(
- "MaxRealTime", 1,
- "If not zero and the real time mode is off, the simulator will not run \n"
- "\tfaster than real-time * MaxRealTime even if the machine is fast enough \n"
- "\t(a sleep is inserted to slow down the simulator - the precision remains unchanged).\n"
- "\tIf set to 2, the simulator will not run faster than double speed. \n"
- "\tIf set to 0.5, the simulator will not run faster than half speed. \n"
- "\tIf set to 0, the simulator will run as fast as possible.\n");
-
- defineOptionalProperty<bool>("LogRobot", false, "Enable robot logging. If true, the complete robot state is logged to a file each step.");
-
- defineOptionalProperty<int>("ReportVisuFrequency", 30, "How often should the visualization data be published. Value is given in Hz. (0 to disable)");
- defineOptionalProperty<std::string>("ReportVisuTopicName", "SimulatorVisuUpdates", "The topic on which the visualization updates are published.");
- defineOptionalProperty<int>("ReportDataFrequency", 30, "How often should the robot data be published. Value is given in Hz. (0 to disable)");
- defineOptionalProperty<std::string>(
- "FixedObjects", "",
- "Fixate objects or parts of a robot in the world. Comma separated list. \n"
- "Define objects by their name, robots can be defined with robotName:RobotNodeName");
- defineOptionalProperty<bool>("DrawCoMVisu", true, "If true the CoM of each robot is drawn as a circle on the ground after each simulation cycle.");
+ "MaxRealTime",
+ 1,
+ "If not zero and the real time mode is off, the simulator will not run \n"
+ "\tfaster than real-time * MaxRealTime even if the machine is fast enough \n"
+ "\t(a sleep is inserted to slow down the simulator - the precision remains unchanged).\n"
+ "\tIf set to 2, the simulator will not run faster than double speed. \n"
+ "\tIf set to 0.5, the simulator will not run faster than half speed. \n"
+ "\tIf set to 0, the simulator will run as fast as possible.\n");
+ defineOptionalProperty<bool>(
+ "LogRobot",
+ false,
+ "Enable robot logging. If true, the complete robot state is logged to a file each step.");
+
+ defineOptionalProperty<int>("ReportVisuFrequency",
+ 30,
+ "How often should the visualization data be published. Value is "
+ "given in Hz. (0 to disable)");
+ defineOptionalProperty<std::string>(
+ "ReportVisuTopicName",
+ "SimulatorVisuUpdates",
+ "The topic on which the visualization updates are published.");
+ defineOptionalProperty<int>(
+ "ReportDataFrequency",
+ 30,
+ "How often should the robot data be published. Value is given in Hz. (0 to disable)");
+ defineOptionalProperty<std::string>(
+ "FixedObjects",
+ "",
+ "Fixate objects or parts of a robot in the world. Comma separated list. \n"
+ "Define objects by their name, robots can be defined with robotName:RobotNodeName");
+ defineOptionalProperty<bool>("DrawCoMVisu",
+ true,
+ "If true the CoM of each robot is drawn as a circle on the ground "
+ "after each simulation cycle.");
}
SimulatorPropertyDefinitions::~SimulatorPropertyDefinitions()
-{}
-
+{
+}
-Simulator::Simulator()
- : SimulatorInterface(), Component()
+Simulator::Simulator() : SimulatorInterface(), Component()
{
// create data object
//physicsWorld.reset(new BulletPhysicsWorld());
@@ -200,12 +254,14 @@ Simulator::~Simulator()
ARMARX_VERBOSE << "~Simulator";
}
-std::string Simulator::getDefaultName() const
+std::string
+Simulator::getDefaultName() const
{
return "Simulator";
}
-void Simulator::onInitComponent()
+void
+Simulator::onInitComponent()
{
priorKnowledgeName = getProperty<std::string>("PriorKnowledgeName").getValue();
commonStorageName = getProperty<std::string>("CommonStorageName").getValue();
@@ -223,7 +279,7 @@ void Simulator::onInitComponent()
}
else
{
- ARMARX_LOG << "Not using LongtermMemoryProxy" ;
+ ARMARX_LOG << "Not using LongtermMemoryProxy";
}
bool floorPlane = getProperty<bool>("FloorPlane").getValue();
@@ -235,7 +291,8 @@ void Simulator::onInitComponent()
simulatorType = getProperty<SimulatorType>("SimulationType");
}
catch (...)
- {}
+ {
+ }
offeringTopic("GlobalRobotPoseLocalization");
offeringTopic("SimulatorResetEvent");
@@ -248,38 +305,44 @@ void Simulator::onInitComponent()
switch (simulatorType)
{
- case SimulatorType::Kinematics:
- {
- KinematicsWorldPtr kw(new KinematicsWorld());
- kw->initialize(stepTimeMs, maxRealTime, floorPlane, floorTexture);
- physicsWorld = kw;
- }
+ case SimulatorType::Kinematics:
+ {
+ KinematicsWorldPtr kw(new KinematicsWorld());
+ kw->initialize(stepTimeMs, maxRealTime, floorPlane, floorTexture);
+ physicsWorld = kw;
+ }
break;
- case SimulatorType::Bullet:
- {
- int bulletFixedTimeStepMS = getProperty<int>("FixedTimeStepStepTimeMS").getValue();
- int bulletFixedTimeStepMaxNrLoops = getProperty<int>("FixedTimeStepLoopNrSteps").getValue();
- BulletPhysicsWorldPtr bw(new BulletPhysicsWorld());
- bw->initialize(stepTimeMs, bulletFixedTimeStepMS, bulletFixedTimeStepMaxNrLoops,
- maxRealTime, floorPlane, floorTexture);
- physicsWorld = bw;
- }
+ case SimulatorType::Bullet:
+ {
+ int bulletFixedTimeStepMS = getProperty<int>("FixedTimeStepStepTimeMS").getValue();
+ int bulletFixedTimeStepMaxNrLoops =
+ getProperty<int>("FixedTimeStepLoopNrSteps").getValue();
+ BulletPhysicsWorldPtr bw(new BulletPhysicsWorld());
+ bw->initialize(stepTimeMs,
+ bulletFixedTimeStepMS,
+ bulletFixedTimeStepMaxNrLoops,
+ maxRealTime,
+ floorPlane,
+ floorTexture);
+ physicsWorld = bw;
+ }
break;
- case SimulatorType::Mujoco:
- {
+ case SimulatorType::Mujoco:
+ {
#ifdef MUJOCO_PHYSICS_WORLD
- MujocoPhysicsWorldPtr mw(new MujocoPhysicsWorld());
- // initialize mujoco
+ MujocoPhysicsWorldPtr mw(new MujocoPhysicsWorld());
+ // initialize mujoco
- mw->initialize(stepTimeMs, floorPlane, floorTexture);
+ mw->initialize(stepTimeMs, floorPlane, floorTexture);
- physicsWorld = mw;
+ physicsWorld = mw;
#else
- ARMARX_ERROR << "Simulator type 'mujoco' is not supported, since simulator was built without MujocoX."
- << "\nGet the package MujocoX from: https://gitlab.com/h2t/mujoco .";
- return;
+ ARMARX_ERROR << "Simulator type 'mujoco' is not supported, since simulator was built "
+ "without MujocoX."
+ << "\nGet the package MujocoX from: https://gitlab.com/h2t/mujoco .";
+ return;
#endif
- }
+ }
break;
}
@@ -288,9 +351,8 @@ void Simulator::onInitComponent()
offeringTopic(TIME_TOPIC_NAME);
}
-
-
-void Simulator::addRobotsFromProperties()
+void
+Simulator::addRobotsFromProperties()
{
for (int i = -1; i < MAX_INITIAL_ROBOT_COUNT - 1; ++i)
{
@@ -299,8 +361,8 @@ void Simulator::addRobotsFromProperties()
{
postfix = "_" + ValueToString(i);
}
- if (!getProperty<std::string>("RobotFileName" + postfix).isSet()
- || getProperty<std::string>("RobotFileName" + postfix).getValue().empty())
+ if (!getProperty<std::string>("RobotFileName" + postfix).isSet() ||
+ getProperty<std::string>("RobotFileName" + postfix).getValue().empty())
{
continue;
}
@@ -309,22 +371,23 @@ void Simulator::addRobotsFromProperties()
Eigen::Matrix4f gp = Eigen::Matrix4f::Identity();
VirtualRobot::MathTools::rpy2eigen4f(
- getProperty<float>("InitialRobotPose.roll" + postfix).getValue(),
- getProperty<float>("InitialRobotPose.pitch" + postfix).getValue(),
- getProperty<float>("InitialRobotPose.yaw" + postfix).getValue(),
- gp);
+ getProperty<float>("InitialRobotPose.roll" + postfix).getValue(),
+ getProperty<float>("InitialRobotPose.pitch" + postfix).getValue(),
+ getProperty<float>("InitialRobotPose.yaw" + postfix).getValue(),
+ gp);
- math::Helpers::Position(gp) = Eigen::Vector3f(
- getProperty<float>("InitialRobotPose.x" + postfix).getValue(),
- getProperty<float>("InitialRobotPose.y" + postfix).getValue(),
- getProperty<float>("InitialRobotPose.z" + postfix).getValue());
+ math::Helpers::Position(gp) =
+ Eigen::Vector3f(getProperty<float>("InitialRobotPose.x" + postfix).getValue(),
+ getProperty<float>("InitialRobotPose.y" + postfix).getValue(),
+ getProperty<float>("InitialRobotPose.z" + postfix).getValue());
ARMARX_INFO << "Initial robot pose:\n" << gp;
std::map<std::string, float> initConfig;
if (getProperty<std::string>("InitialRobotConfig" + postfix).isSet())
{
- std::string confStr = getProperty<std::string>("InitialRobotConfig" + postfix).getValue();
+ std::string confStr =
+ getProperty<std::string>("InitialRobotConfig" + postfix).getValue();
std::vector<std::string> confList = Split(confStr, ",", true, true);
for (auto& pos : confList)
{
@@ -347,7 +410,8 @@ void Simulator::addRobotsFromProperties()
double pid_i = getProperty<double>("RobotControllerPID.i" + postfix).getValue();
double pid_d = getProperty<double>("RobotControllerPID.d" + postfix).getValue();
- std::string robotInstanceName = getProperty<std::string>("RobotInstanceName" + postfix).getValue();
+ std::string robotInstanceName =
+ getProperty<std::string>("RobotInstanceName" + postfix).getValue();
bool staticRobot = getProperty<bool>("RobotIsStatic" + postfix).getValue();
bool colModelRobot = getProperty<bool>("RobotCollisionModel" + postfix).getValue();
@@ -363,14 +427,27 @@ void Simulator::addRobotsFromProperties()
}
else
{
- ARMARX_INFO << "Adding robot with name '" << robotInstanceName << "' from file " << robFileGlobal;
- addRobot(robotInstanceName, robFileGlobal, gp, robotFile, pid_p, pid_i, pid_d, staticRobot, scaling, colModelRobot, initConfig, selfCollisions);
+ ARMARX_INFO << "Adding robot with name '" << robotInstanceName << "' from file "
+ << robFileGlobal;
+ addRobot(robotInstanceName,
+ robFileGlobal,
+ gp,
+ robotFile,
+ pid_p,
+ pid_i,
+ pid_d,
+ staticRobot,
+ scaling,
+ colModelRobot,
+ initConfig,
+ selfCollisions);
}
}
}
}
-void Simulator::addSceneFromFile(const std::string& sceneFile)
+void
+Simulator::addSceneFromFile(const std::string& sceneFile)
{
std::string sf;
if (!ArmarXDataPath::getAbsolutePath(sceneFile, sf))
@@ -386,7 +463,8 @@ void Simulator::addSceneFromFile(const std::string& sceneFile)
}
}
-void Simulator::addSceneFromMemorySnapshot(const std::string& snapshotName)
+void
+Simulator::addSceneFromMemorySnapshot(const std::string& snapshotName)
{
requestFileManager();
if (!fileManager)
@@ -394,12 +472,13 @@ void Simulator::addSceneFromMemorySnapshot(const std::string& snapshotName)
ARMARX_ERROR << "No connection to memory - cannot add snapshot!";
return;
}
- memoryPrx = getProxy<WorkingMemoryInterfacePrx>(getProperty<std::string>("WorkingMemoryName").getValue());
-
+ memoryPrx = getProxy<WorkingMemoryInterfacePrx>(
+ getProperty<std::string>("WorkingMemoryName").getValue());
priorKnowledgePrx = getProxy<PriorKnowledgeInterfacePrx>(priorKnowledgeName);
- longtermMemoryPrx = getProxy<LongtermMemoryInterfacePrx>(getProperty<std::string>("LongtermMemoryName").getValue());
+ longtermMemoryPrx = getProxy<LongtermMemoryInterfacePrx>(
+ getProperty<std::string>("LongtermMemoryName").getValue());
WorkingMemorySnapshotInterfacePrx snapshotInterfacePrx;
if (longtermMemoryPrx && priorKnowledgePrx && memoryPrx)
@@ -408,7 +487,9 @@ void Simulator::addSceneFromMemorySnapshot(const std::string& snapshotName)
try
{
- snapshotInterfacePrx = longtermMemoryPrx->getWorkingMemorySnapshotListSegment()->openSnapshot(snapshotName);
+ snapshotInterfacePrx =
+ longtermMemoryPrx->getWorkingMemorySnapshotListSegment()->openSnapshot(
+ snapshotName);
if (getProperty<bool>("LoadSnapshotToWorkingMemory").getValue())
{
if (snapshotInterfacePrx)
@@ -446,7 +527,8 @@ void Simulator::addSceneFromMemorySnapshot(const std::string& snapshotName)
}
}
-void Simulator::addSceneFromSdfFile(const std::string& scenePath)
+void
+Simulator::addSceneFromSdfFile(const std::string& scenePath)
{
std::string scenePackage = simox::alg::split(scenePath, "/").at(0);
CMakePackageFinder cmakeFinder(scenePackage);
@@ -459,15 +541,42 @@ void Simulator::addSceneFromSdfFile(const std::string& scenePath)
std::filesystem::path dataDir = cmakeFinder.getDataDir();
std::filesystem::path path = dataDir / scenePath;
+
sdf::SDFPtr sdfFile(new sdf::SDF());
sdf::init(sdfFile);
std::filesystem::path modelDirectory = path.parent_path();
- std::string modelURI = "model://";
+ const std::string modelURI = "model://";
+
+ auto findFileCallback = [this, &modelURI](const std::string& uri) -> std::string
+ {
+ std::stringstream ss;
+ ss << "Find '" << uri << "' ... -> ";
+
+ const std::string classIDStr = simox::alg::remove_prefix(uri, modelURI);
+ const ObjectID classID = ObjectID::FromString(classIDStr);
+
+ if (std::optional<ObjectInfo> info = objectFinder.findObject(classID))
+ {
+ if (std::optional<PackageFileLocation> modelPath = info->getModel())
+ {
+ std::filesystem::path result = modelPath->absolutePath.parent_path();
+ ss << result;
+ ARMARX_VERBOSE << ss.str();
+
+ return result;
+ }
+ }
+ ss << uri;
+ ARMARX_VERBOSE << ss.str();
+
+ return uri;
+ };
sdf::ParserConfig parser;
parser.AddURIPath(modelURI, modelDirectory.string());
+ parser.SetFindCallback(findFileCallback);
sdf::Root root;
sdf::Errors errors = root.Load(path.string(), parser);
@@ -496,37 +605,25 @@ void Simulator::addSceneFromSdfFile(const std::string& scenePath)
}
sdf::World* world = root.WorldByIndex(0);
-
auto addSceneObject = [&](sdf::Model* model)
{
- auto name = model->Name();
-
+ const std::string name = model->Name();
if (physicsWorld->hasObject(name))
{
ARMARX_WARNING << "Object with name \"" << name << "\" already exists.";
return;
}
- std::string uri = model->Uri();
-
+ const std::string uri = model->Uri();
if (uri.empty())
{
ARMARX_WARNING << "No URI set for object \"" << name << "\" in SDF.";
return;
}
- std::filesystem::path modelPath;
- if (simox::alg::starts_with(uri, modelURI))
- {
- std::filesystem::path localModelPath = uri.substr(modelURI.size(), std::string::npos);
- modelPath = modelDirectory / localModelPath;
- }
- else
- {
- ARMARX_WARNING << "Invalid URI set for object \"" << name << "\" in SDF.";
- return;
- }
+ // Not clear where the result of the callback called during loading is stored.
+ const std::filesystem::path modelPath = findFileCallback(uri);
gz::math::Pose3d pose = model->RawPose();
@@ -545,22 +642,41 @@ void Simulator::addSceneFromSdfFile(const std::string& scenePath)
auto isStatic = model->Static();
- std::string robotName = modelPath.filename().generic_string() + ".urdf";
- std::filesystem::path robotPath = modelPath / robotName;
+ std::string robotFilename = modelPath.filename().generic_string() + ".urdf";
+ std::filesystem::path robotPath = modelPath / robotFilename;
- std::string xmlName = modelPath.filename().generic_string() + ".xml";
- std::filesystem::path xmlPath = modelPath / xmlName;
+ std::string xmlFilename = modelPath.filename().generic_string() + ".xml";
+ std::filesystem::path xmlPath = modelPath / xmlFilename;
- if (std::filesystem::exists(robotPath))
+ if (std::filesystem::exists(xmlPath))
{
+ ARMARX_INFO << "Loading object '" << name << "' from object XML file " << xmlPath;
+ VirtualRobot::ManipulationObjectPtr simoxObject =
+ VirtualRobot::ObjectIO::loadManipulationObject(xmlPath.string());
+
+ simoxObject->setGlobalPose(simox::math::pose(position, orientation));
+
+ VirtualRobot::SceneObject::Physics::SimulationType simType =
+ isStatic ? VirtualRobot::SceneObject::Physics::eKinematic
+ : VirtualRobot::SceneObject::Physics::eDynamic;
+
+ simoxObject->setSimulationType(simType);
+ simoxObject->setFilename(xmlPath.string());
+ simoxObject->setName(name);
+
+ physicsWorld->addObstacle(
+ simoxObject, simType, xmlPath.string(), name, {}, scenePackage);
+ }
+ else if (std::filesystem::exists(robotPath))
+ {
+ ARMARX_INFO << "Loading object '" << name << "' from robot URDF file " << xmlPath;
VirtualRobot::RobotPtr robot = VirtualRobot::RobotIO::loadRobot(robotPath);
robot->setGlobalPose(simox::math::pose(position, orientation));
VirtualRobot::SceneObject::Physics::SimulationType simType =
- isStatic
- ? VirtualRobot::SceneObject::Physics::eKinematic
- : VirtualRobot::SceneObject::Physics::eDynamic;
+ isStatic ? VirtualRobot::SceneObject::Physics::eKinematic
+ : VirtualRobot::SceneObject::Physics::eDynamic;
robot->setSimulationType(simType);
robot->setFilename(robotPath.string());
@@ -568,26 +684,12 @@ void Simulator::addSceneFromSdfFile(const std::string& scenePath)
physicsWorld->addRobot(robot, 10.0, 0.0, 0.0, robotPath.string(), isStatic);
}
- else if (std::filesystem::exists(xmlPath))
+ else
{
- VirtualRobot::ManipulationObjectPtr simoxObject =
- VirtualRobot::ObjectIO::loadManipulationObject(xmlPath.string());
-
- simoxObject->setGlobalPose(simox::math::pose(position, orientation));
-
- VirtualRobot::SceneObject::Physics::SimulationType simType =
- isStatic
- ? VirtualRobot::SceneObject::Physics::eKinematic
- : VirtualRobot::SceneObject::Physics::eDynamic;
-
- simoxObject->setSimulationType(simType);
- simoxObject->setFilename(xmlPath.string());
- simoxObject->setName(name);
-
- physicsWorld->addObstacle(simoxObject, simType,
- xmlPath.string(), name,
- {},scenePackage);
-
+ ARMARX_INFO
+ << "Not adding object '" << name
+ << "' because neither object XML nor robot URDF file exist. (Tried object XML = "
+ << xmlPath << " and robot URDF = " << robotPath << ")";
}
};
@@ -597,7 +699,8 @@ void Simulator::addSceneFromSdfFile(const std::string& scenePath)
}
}
-void Simulator::addSceneFromJsonFile(const std::string& scenePath)
+void
+Simulator::addSceneFromJsonFile(const std::string& scenePath)
{
std::string mutScenePath = scenePath;
if (!simox::alg::ends_with(mutScenePath, ".json"))
@@ -634,16 +737,16 @@ void Simulator::addSceneFromJsonFile(const std::string& scenePath)
}
}
-void Simulator::addJsonSceneObject(
- const armarx::objects::SceneObject object,
- std::map<ObjectID, int>& idCounters)
+void
+Simulator::addJsonSceneObject(const armarx::objects::SceneObject object,
+ std::map<ObjectID, int>& idCounters)
{
const ObjectID classID = object.getClassID();
- const std::string name = object.getObjectID().withInstanceName(
- object.instanceName.empty()
- ? std::to_string(idCounters[classID]++)
- : object.instanceName
- ).str();
+ const std::string name =
+ object.getObjectID()
+ .withInstanceName(object.instanceName.empty() ? std::to_string(idCounters[classID]++)
+ : object.instanceName)
+ .str();
if (physicsWorld->hasObject(name))
{
@@ -661,9 +764,8 @@ void Simulator::addJsonSceneObject(
VirtualRobot::SceneObject::Physics::SimulationType simType;
if (object.isStatic.has_value())
{
- simType = *object.isStatic
- ? VirtualRobot::SceneObject::Physics::eKinematic
- : VirtualRobot::SceneObject::Physics::eDynamic;
+ simType = *object.isStatic ? VirtualRobot::SceneObject::Physics::eKinematic
+ : VirtualRobot::SceneObject::Physics::eDynamic;
}
else
{
@@ -675,23 +777,27 @@ void Simulator::addJsonSceneObject(
simoxObject->setFilename(simoxXml.absolutePath);
simoxObject->setName(name);
- physicsWorld->addObstacle(simoxObject, simType, simoxXml.relativePath, object.className, {},
- simoxXml.package);
+ physicsWorld->addObstacle(
+ simoxObject, simType, simoxXml.relativePath, object.className, {}, simoxXml.package);
}
-void Simulator::fixateRobotNode(const std::string& robotName, const std::string& robotNodeName)
+void
+Simulator::fixateRobotNode(const std::string& robotName, const std::string& robotNodeName)
{
ARMARX_VERBOSE << "Fixing robot node" << robotName << "." << robotNodeName;
- physicsWorld->setRobotNodeSimType(robotName, robotNodeName, VirtualRobot::SceneObject::Physics::eStatic);
+ physicsWorld->setRobotNodeSimType(
+ robotName, robotNodeName, VirtualRobot::SceneObject::Physics::eStatic);
}
-void Simulator::fixateObject(const std::string& objectName)
+void
+Simulator::fixateObject(const std::string& objectName)
{
ARMARX_VERBOSE << "Fixing object" << objectName;
physicsWorld->setObjectSimType(objectName, VirtualRobot::SceneObject::Physics::eStatic);
}
-void Simulator::initializeData()
+void
+Simulator::initializeData()
{
ARMARX_INFO << "Initializing simulator...";
@@ -723,10 +829,12 @@ void Simulator::initializeData()
if (path.has_extension() && path.extension().string() == sdfExtension)
{
+ ARMARX_INFO << "Loading scene from SDF file '" << scenePath << "' ...";
addSceneFromSdfFile(scenePath);
}
else
{
+ ARMARX_INFO << "Loading scene from JSON file '" << scenePath << "' ...";
addSceneFromJsonFile(scenePath);
}
}
@@ -751,13 +859,13 @@ void Simulator::initializeData()
fixateObject(o);
}
}
-
}
physicsWorld->resetSimTime();
}
-SimulatedRobotState Simulator::stateFromRobotInfo(RobotInfo const& r, IceUtil::Time timestamp)
+SimulatedRobotState
+Simulator::stateFromRobotInfo(RobotInfo const& r, IceUtil::Time timestamp)
{
SimulatedRobotState state;
@@ -787,28 +895,36 @@ SimulatedRobotState Simulator::stateFromRobotInfo(RobotInfo const& r, IceUtil::T
return state;
}
-memoryx::GridFileManagerPtr Simulator::requestFileManager()
+memoryx::GridFileManagerPtr
+Simulator::requestFileManager()
{
if (fileManager)
{
- ARMARX_INFO << deactivateSpam(10) << "Was able to find a reference to the CommonStorage. Continue with existing connection to memory.";
+ ARMARX_INFO << deactivateSpam(10)
+ << "Was able to find a reference to the CommonStorage. Continue with existing "
+ "connection to memory.";
return fileManager;
}
- CommonStorageInterfacePrx commonStoragePrx = getProxy<CommonStorageInterfacePrx>(commonStorageName, false, "", false);
+ CommonStorageInterfacePrx commonStoragePrx =
+ getProxy<CommonStorageInterfacePrx>(commonStorageName, false, "", false);
if (commonStoragePrx)
{
- ARMARX_INFO << deactivateSpam(10) << "Was able to find a reference to the CommonStorage. Reset GridFSManager and continue with memory.";
+ ARMARX_INFO << deactivateSpam(10)
+ << "Was able to find a reference to the CommonStorage. Reset GridFSManager and "
+ "continue with memory.";
fileManager.reset(new GridFileManager(commonStoragePrx));
}
else
{
- ARMARX_INFO << deactivateSpam(10) << "Could not get CommonStorage Proxy - continuing without memory";
+ ARMARX_INFO << deactivateSpam(10)
+ << "Could not get CommonStorage Proxy - continuing without memory";
}
return fileManager;
}
-void Simulator::onConnectComponent()
+void
+Simulator::onConnectComponent()
{
ARMARX_VERBOSE << "Connecting ArmarX Simulator";
@@ -838,15 +954,15 @@ void Simulator::onConnectComponent()
if (hzVisu > 0)
{
offeringTopic(top);
- ARMARX_INFO << "Connecting to topic " << top ;
+ ARMARX_INFO << "Connecting to topic " << top;
simulatorVisuUpdateListenerPrx = getTopic<SimulatorListenerInterfacePrx>(top);
ARMARX_DEBUG << "Creating report visu task";
int cycleTime = 1000 / hzVisu;
- reportVisuTask = new PeriodicTask<Simulator>(this, &Simulator::reportVisuLoop, cycleTime, false, "ArmarXSimulatorReportVisuTask");
+ reportVisuTask = new PeriodicTask<Simulator>(
+ this, &Simulator::reportVisuLoop, cycleTime, false, "ArmarXSimulatorReportVisuTask");
reportVisuTask->start();
reportVisuTask->setDelayWarningTolerance(100);
-
}
ARMARX_DEBUG << "Starting periodic data report task in ArmarX Simulator";
@@ -859,7 +975,8 @@ void Simulator::onConnectComponent()
if (hzData > 0)
{
int cycleTime = 1000 / hzData;
- reportDataTask = new PeriodicTask<Simulator>(this, &Simulator::reportDataLoop, cycleTime, false, "ArmarXSimulatorReportDataTask");
+ reportDataTask = new PeriodicTask<Simulator>(
+ this, &Simulator::reportDataLoop, cycleTime, false, "ArmarXSimulatorReportDataTask");
reportDataTask->start();
reportDataTask->setDelayWarningTolerance(100);
}
@@ -867,8 +984,8 @@ void Simulator::onConnectComponent()
ARMARX_DEBUG << "finished connection";
}
-
-void Simulator::onDisconnectComponent()
+void
+Simulator::onDisconnectComponent()
{
ARMARX_VERBOSE << "disconnecting Simulator";
@@ -887,7 +1004,8 @@ void Simulator::onDisconnectComponent()
resetData();
}
-void Simulator::onExitComponent()
+void
+Simulator::onExitComponent()
{
ARMARX_VERBOSE << "exiting Simulator";
@@ -906,11 +1024,13 @@ void Simulator::onExitComponent()
}
shutdownSimulationLoop();
- timeserverProxy = nullptr; // delete the timeserver proxy as it holds a handle to this object, preventing it from being deleted
+ timeserverProxy =
+ nullptr; // delete the timeserver proxy as it holds a handle to this object, preventing it from being deleted
ARMARX_VERBOSE << "exiting Simulator...Done";
}
-void Simulator::shutdownSimulationLoop()
+void
+Simulator::shutdownSimulationLoop()
{
ARMARX_VERBOSE << "shutting down simulation loop";
simulatorThreadRunning = true;
@@ -928,8 +1048,8 @@ void Simulator::shutdownSimulationLoop()
ARMARX_VERBOSE << "shutting down simulation loop...Done!";
}
-
-void Simulator::resetData(bool clearWorkingMemory)
+void
+Simulator::resetData(bool clearWorkingMemory)
{
ARMARX_INFO << "Deleting data";
@@ -959,7 +1079,8 @@ void Simulator::resetData(bool clearWorkingMemory)
ARMARX_VERBOSE << "Deleting data...Done!";
}
-bool Simulator::addSnapshot(WorkingMemorySnapshotInterfacePrx snapshotInterfacePrx)
+bool
+Simulator::addSnapshot(WorkingMemorySnapshotInterfacePrx snapshotInterfacePrx)
{
ARMARX_VERBOSE << "Adding snapshot from memory";
@@ -982,7 +1103,8 @@ bool Simulator::addSnapshot(WorkingMemorySnapshotInterfacePrx snapshotInterfaceP
}
// get classes for IV file loading (instances refer to classes, and classes store IV files)
- PersistentObjectClassSegmentBasePrx classesSegmentPrx = priorKnowledgePrx->getObjectClassesSegment();
+ PersistentObjectClassSegmentBasePrx classesSegmentPrx =
+ priorKnowledgePrx->getObjectClassesSegment();
if (!classesSegmentPrx)
{
@@ -1004,7 +1126,8 @@ bool Simulator::addSnapshot(WorkingMemorySnapshotInterfacePrx snapshotInterfaceP
if (!ok)
{
- ARMARX_ERROR << "SnapInstance " << id << " has no classes attribute, could not load iv files, skipping";
+ ARMARX_ERROR << "SnapInstance " << id
+ << " has no classes attribute, could not load iv files, skipping";
continue;
}
@@ -1046,8 +1169,11 @@ bool Simulator::addSnapshot(WorkingMemorySnapshotInterfacePrx snapshotInterfaceP
}
PosePtr poseGlobal(new Pose(m));
- memoryx::EntityWrappers::SimoxObjectWrapperPtr sw = objectClass->addWrapper(new EntityWrappers::SimoxObjectWrapper(fileManager));
- VirtualRobot::ManipulationObjectPtr mo = sw->getManipulationObject()->clone(sw->getManipulationObject()->getName()); // maybe not needed: cloning the object to avoid changing content of priorKnowledge
+ memoryx::EntityWrappers::SimoxObjectWrapperPtr sw =
+ objectClass->addWrapper(new EntityWrappers::SimoxObjectWrapper(fileManager));
+ VirtualRobot::ManipulationObjectPtr mo = sw->getManipulationObject()->clone(
+ sw->getManipulationObject()
+ ->getName()); // maybe not needed: cloning the object to avoid changing content of priorKnowledge
if (mo)
{
@@ -1060,9 +1186,11 @@ bool Simulator::addSnapshot(WorkingMemorySnapshotInterfacePrx snapshotInterfaceP
}
if (instanceName != snapInstance->getName())
{
- ARMARX_INFO << "Object instance with name '" << snapInstance->getName() << "'' already exists - using '" << instanceName << "'";
+ ARMARX_INFO << "Object instance with name '" << snapInstance->getName()
+ << "'' already exists - using '" << instanceName << "'";
}
- bool isStatic = mo->getSimulationType() == VirtualRobot::SceneObject::Physics::eStatic;
+ bool isStatic =
+ mo->getSimulationType() == VirtualRobot::SceneObject::Physics::eStatic;
addObject(objectClass, instanceName, poseGlobal, isStatic);
}
}
@@ -1084,8 +1212,8 @@ bool Simulator::addSnapshot(WorkingMemorySnapshotInterfacePrx snapshotInterfaceP
}
}
-
-bool Simulator::loadAgentsFromSnapshot(WorkingMemorySnapshotInterfacePrx snapshotInterfacePrx)
+bool
+Simulator::loadAgentsFromSnapshot(WorkingMemorySnapshotInterfacePrx snapshotInterfacePrx)
{
const std::string segmentName = "agentInstances";
bool result = true;
@@ -1117,18 +1245,18 @@ bool Simulator::loadAgentsFromSnapshot(WorkingMemorySnapshotInterfacePrx snapsho
}
if (!ArmarXDataPath::getAbsolutePath(agentModelFile, absModelFile))
{
- ARMARX_WARNING << "Could not load agent '" << agentName << "'- file not found: " << agentModelFile;
+ ARMARX_WARNING << "Could not load agent '" << agentName
+ << "'- file not found: " << agentModelFile;
result = false;
continue;
}
-
- if (addRobot(agentName, absModelFile, snapAgent->getPose()->toEigen(), agentModelFile).empty())
+ if (addRobot(agentName, absModelFile, snapAgent->getPose()->toEigen(), agentModelFile)
+ .empty())
{
result = false;
}
-
}
}
catch (...)
@@ -1138,27 +1266,44 @@ bool Simulator::loadAgentsFromSnapshot(WorkingMemorySnapshotInterfacePrx snapsho
return result;
}
-void Simulator::actuateRobotJoints(const std::string& robotName, const NameValueMap& angles, const NameValueMap& velocities, const Ice::Current&)
+
+void
+Simulator::actuateRobotJoints(const std::string& robotName,
+ const NameValueMap& angles,
+ const NameValueMap& velocities,
+ const Ice::Current&)
{
physicsWorld->actuateRobotJoints(robotName, angles, velocities);
}
-void Simulator::actuateRobotJointsPos(const std::string& robotName, const NameValueMap& angles, const Ice::Current&)
+void
+Simulator::actuateRobotJointsPos(const std::string& robotName,
+ const NameValueMap& angles,
+ const Ice::Current&)
{
physicsWorld->actuateRobotJointsPos(robotName, angles);
}
-void Simulator::actuateRobotJointsVel(const std::string& robotName, const NameValueMap& velocities, const Ice::Current&)
+void
+Simulator::actuateRobotJointsVel(const std::string& robotName,
+ const NameValueMap& velocities,
+ const Ice::Current&)
{
physicsWorld->actuateRobotJointsVel(robotName, velocities);
}
-void Simulator::actuateRobotJointsTorque(const std::string& robotName, const NameValueMap& torques, const Ice::Current&)
+void
+Simulator::actuateRobotJointsTorque(const std::string& robotName,
+ const NameValueMap& torques,
+ const Ice::Current&)
{
physicsWorld->actuateRobotJointsTorque(robotName, torques);
}
-void Simulator::setRobotPose(const std::string& robotName, const PoseBasePtr& globalPose, const Ice::Current&)
+void
+Simulator::setRobotPose(const std::string& robotName,
+ const PoseBasePtr& globalPose,
+ const Ice::Current&)
{
VirtualRobot::MathTools::Quaternion qa;
qa.x = globalPose->orientation->qx;
@@ -1172,31 +1317,50 @@ void Simulator::setRobotPose(const std::string& robotName, const PoseBasePtr& gl
physicsWorld->setRobotPose(robotName, gp);
}
-void Simulator::applyForceRobotNode(const std::string& robotName, const std::string& robotNodeName, const Vector3BasePtr& force, const Ice::Current&)
+void
+Simulator::applyForceRobotNode(const std::string& robotName,
+ const std::string& robotNodeName,
+ const Vector3BasePtr& force,
+ const Ice::Current&)
{
Vector3Ptr v3p = Vector3Ptr::dynamicCast(force);
physicsWorld->applyForceRobotNode(robotName, robotNodeName, v3p->toEigen());
}
-void Simulator::applyTorqueRobotNode(const std::string& robotName, const std::string& robotNodeName, const Vector3BasePtr& torque, const Ice::Current&)
+void
+Simulator::applyTorqueRobotNode(const std::string& robotName,
+ const std::string& robotNodeName,
+ const Vector3BasePtr& torque,
+ const Ice::Current&)
{
Vector3Ptr v3p = Vector3Ptr::dynamicCast(torque);
physicsWorld->applyTorqueRobotNode(robotName, robotNodeName, v3p->toEigen());
}
-void Simulator::applyForceObject(const std::string& objectName, const Vector3BasePtr& force, const Ice::Current&)
+void
+Simulator::applyForceObject(const std::string& objectName,
+ const Vector3BasePtr& force,
+ const Ice::Current&)
{
Vector3Ptr v3p = Vector3Ptr::dynamicCast(force);
physicsWorld->applyForceObject(objectName, v3p->toEigen());
}
-void Simulator::applyTorqueObject(const std::string& objectName, const Vector3BasePtr& torque, const Ice::Current&)
+void
+Simulator::applyTorqueObject(const std::string& objectName,
+ const Vector3BasePtr& torque,
+ const Ice::Current&)
{
Vector3Ptr v3p = Vector3Ptr::dynamicCast(torque);
physicsWorld->applyTorqueObject(objectName, v3p->toEigen());
}
-void Simulator::addObject(const memoryx::ObjectClassBasePtr& objectClassBase, const std::string& instanceName, const PoseBasePtr& globalPose, bool isStatic, const Ice::Current&)
+void
+Simulator::addObject(const memoryx::ObjectClassBasePtr& objectClassBase,
+ const std::string& instanceName,
+ const PoseBasePtr& globalPose,
+ bool isStatic,
+ const Ice::Current&)
{
ARMARX_VERBOSE << "Add object from memory " << instanceName;
if (!requestFileManager())
@@ -1220,17 +1384,21 @@ void Simulator::addObject(const memoryx::ObjectClassBasePtr& objectClassBase, co
if (!objectClass)
{
- ARMARX_ERROR << "Invalid object class; could not create object with instance name " << instanceName;
+ ARMARX_ERROR << "Invalid object class; could not create object with instance name "
+ << instanceName;
return;
}
- memoryx::EntityWrappers::SimoxObjectWrapperPtr sw = objectClass->addWrapper(new EntityWrappers::SimoxObjectWrapper(fileManager));
+ memoryx::EntityWrappers::SimoxObjectWrapperPtr sw =
+ objectClass->addWrapper(new EntityWrappers::SimoxObjectWrapper(fileManager));
- VirtualRobot::ManipulationObjectPtr mo = sw->getManipulationObject();//->clone(sw->getManipulationObject()->getName()); // maybe not needed: cloning the object to avoid changing content of priorKnowledge
+ VirtualRobot::ManipulationObjectPtr mo =
+ sw->getManipulationObject(); //->clone(sw->getManipulationObject()->getName()); // maybe not needed: cloning the object to avoid changing content of priorKnowledge
if (!mo)
{
- ARMARX_ERROR << "Could not retrieve manipulation object of object class " << objectClass->getName();
+ ARMARX_ERROR << "Could not retrieve manipulation object of object class "
+ << objectClass->getName();
return;
}
@@ -1239,7 +1407,8 @@ void Simulator::addObject(const memoryx::ObjectClassBasePtr& objectClassBase, co
mo->setName(instanceName);
- ARMARX_LOG << "Adding manipulation object " << mo->getName() << " of class " << objectClass->getName();
+ ARMARX_LOG << "Adding manipulation object " << mo->getName() << " of class "
+ << objectClass->getName();
classInstanceMap[objectClass->getName()].push_back(instanceName);
VirtualRobot::MathTools::Quaternion qa;
@@ -1253,7 +1422,8 @@ void Simulator::addObject(const memoryx::ObjectClassBasePtr& objectClassBase, co
gp(2, 3) = globalPose->position->z;
mo->setGlobalPose(gp);
VirtualRobot::SceneObject::Physics::SimulationType simType =
- (isStatic) ? VirtualRobot::SceneObject::Physics::eKinematic : VirtualRobot::SceneObject::Physics::eDynamic;
+ (isStatic) ? VirtualRobot::SceneObject::Physics::eKinematic
+ : VirtualRobot::SceneObject::Physics::eDynamic;
physicsWorld->addObstacle(mo, simType, std::string(), objectClassBase->getName());
}
catch (...)
@@ -1262,8 +1432,12 @@ void Simulator::addObject(const memoryx::ObjectClassBasePtr& objectClassBase, co
}
}
-
-void Simulator::addObjectFromFile(const armarx::data::PackagePath& packagePath, const std::string& instanceName, const PoseBasePtr& globalPose, bool isStatic, const Ice::Current&)
+void
+Simulator::addObjectFromFile(const armarx::data::PackagePath& packagePath,
+ const std::string& instanceName,
+ const PoseBasePtr& globalPose,
+ bool isStatic,
+ const Ice::Current&)
{
ARMARX_INFO << "Add object from packagePath " << packagePath << ": " << instanceName;
//if (!requestFileManager())
@@ -1293,7 +1467,8 @@ void Simulator::addObjectFromFile(const armarx::data::PackagePath& packagePath,
}
- VirtualRobot::ManipulationObjectPtr mo = VirtualRobot::ObjectIO::loadManipulationObject(filename);
+ VirtualRobot::ManipulationObjectPtr mo =
+ VirtualRobot::ObjectIO::loadManipulationObject(filename);
if (!mo)
{
@@ -1320,7 +1495,8 @@ void Simulator::addObjectFromFile(const armarx::data::PackagePath& packagePath,
gp(2, 3) = globalPose->position->z;
mo->setGlobalPose(gp);
VirtualRobot::SceneObject::Physics::SimulationType simType =
- (isStatic) ? VirtualRobot::SceneObject::Physics::eKinematic : VirtualRobot::SceneObject::Physics::eDynamic;
+ (isStatic) ? VirtualRobot::SceneObject::Physics::eKinematic
+ : VirtualRobot::SceneObject::Physics::eDynamic;
physicsWorld->addObstacle(mo, simType, filename, instanceName, {}, packagePath.package);
}
catch (...)
@@ -1329,7 +1505,16 @@ void Simulator::addObjectFromFile(const armarx::data::PackagePath& packagePath,
}
}
-void Simulator::addBox(float width, float height, float depth, float massKG, const DrawColor& color, const std::string& instanceName, const PoseBasePtr& globalPose, bool isStatic, const Ice::Current&)
+void
+Simulator::addBox(float width,
+ float height,
+ float depth,
+ float massKG,
+ const DrawColor& color,
+ const std::string& instanceName,
+ const PoseBasePtr& globalPose,
+ bool isStatic,
+ const Ice::Current&)
{
ARMARX_VERBOSE << "Add box " << instanceName;
@@ -1347,7 +1532,8 @@ void Simulator::addBox(float width, float height, float depth, float massKG, con
if (depth <= 0 || width <= 0 || height <= 0)
{
- ARMARX_ERROR << "Invalid properties; could not create object with instance name " << instanceName;
+ ARMARX_ERROR << "Invalid properties; could not create object with instance name "
+ << instanceName;
return;
}
@@ -1384,7 +1570,8 @@ void Simulator::addBox(float width, float height, float depth, float massKG, con
gp(2, 3) = globalPose->position->z;
o->setGlobalPose(gp);
VirtualRobot::SceneObject::Physics::SimulationType simType =
- (isStatic) ? VirtualRobot::SceneObject::Physics::eStatic : VirtualRobot::SceneObject::Physics::eDynamic;
+ (isStatic) ? VirtualRobot::SceneObject::Physics::eStatic
+ : VirtualRobot::SceneObject::Physics::eDynamic;
BoxVisuPrimitivePtr bp(new BoxVisuPrimitive());
bp->type = Box;
@@ -1402,14 +1589,17 @@ void Simulator::addBox(float width, float height, float depth, float massKG, con
}
}
-Ice::StringSeq Simulator::getRobotNames(const Ice::Current&)
+Ice::StringSeq
+Simulator::getRobotNames(const Ice::Current&)
{
- auto lockEngine = physicsWorld->getScopedEngineLock(__FUNCTION__); // we are accessing objects and robots
+ auto lockEngine =
+ physicsWorld->getScopedEngineLock(__FUNCTION__); // we are accessing objects and robots
auto lockData = physicsWorld->getScopedSyncLock(__FUNCTION__);
return physicsWorld->getRobotNames();
}
-void Simulator::removeObject(const std::string& instanceName, const Ice::Current&)
+void
+Simulator::removeObject(const std::string& instanceName, const Ice::Current&)
{
ARMARX_VERBOSE << "Remove object " << instanceName;
@@ -1438,7 +1628,10 @@ void Simulator::removeObject(const std::string& instanceName, const Ice::Current
}
}
-void Simulator::setObjectPose(const std::string& objectName, const PoseBasePtr& globalPose, const Ice::Current&)
+void
+Simulator::setObjectPose(const std::string& objectName,
+ const PoseBasePtr& globalPose,
+ const Ice::Current&)
{
VirtualRobot::MathTools::Quaternion q;
q.x = globalPose->orientation->qx;
@@ -1453,9 +1646,13 @@ void Simulator::setObjectPose(const std::string& objectName, const PoseBasePtr&
physicsWorld->setObjectPose(objectName, gp);
}
-void Simulator::setObjectSimulationType(const std::string& objectName, SimulationType type, const Ice::Current&)
+void
+Simulator::setObjectSimulationType(const std::string& objectName,
+ SimulationType type,
+ const Ice::Current&)
{
- VirtualRobot::SceneObject::Physics::SimulationType st = VirtualRobot::SceneObject::Physics::eDynamic;
+ VirtualRobot::SceneObject::Physics::SimulationType st =
+ VirtualRobot::SceneObject::Physics::eDynamic;
if (type == armarx::Kinematic)
{
st = VirtualRobot::SceneObject::Physics::eKinematic;
@@ -1464,155 +1661,221 @@ void Simulator::setObjectSimulationType(const std::string& objectName, Simulatio
physicsWorld->setObjectSimType(objectName, st);
}
-void Simulator::activateObject(const std::string& objectName, const Ice::Current&)
+void
+Simulator::activateObject(const std::string& objectName, const Ice::Current&)
{
physicsWorld->activateObject(objectName);
}
-int Simulator::getFixedTimeStepMS(const Ice::Current&)
+int
+Simulator::getFixedTimeStepMS(const Ice::Current&)
{
return physicsWorld->getFixedTimeStepMS();
}
-float Simulator::getRobotMass(const std::string& robotName, const Ice::Current&)
+float
+Simulator::getRobotMass(const std::string& robotName, const Ice::Current&)
{
return physicsWorld->getRobotMass(robotName);
}
-NameValueMap Simulator::getRobotJointAngles(const std::string& robotName, const Ice::Current&)
+NameValueMap
+Simulator::getRobotJointAngles(const std::string& robotName, const Ice::Current&)
{
return physicsWorld->getRobotJointAngles(robotName);
}
-float Simulator::getRobotJointAngle(const std::string& robotName, const std::string& nodeName, const Ice::Current&)
+float
+Simulator::getRobotJointAngle(const std::string& robotName,
+ const std::string& nodeName,
+ const Ice::Current&)
{
return physicsWorld->getRobotJointAngle(robotName, nodeName);
}
-NameValueMap Simulator::getRobotJointVelocities(const std::string& robotName, const Ice::Current&)
+NameValueMap
+Simulator::getRobotJointVelocities(const std::string& robotName, const Ice::Current&)
{
return physicsWorld->getRobotJointVelocities(robotName);
}
-float Simulator::getRobotJointVelocity(const std::string& robotName, const std::string& nodeName, const Ice::Current&)
+float
+Simulator::getRobotJointVelocity(const std::string& robotName,
+ const std::string& nodeName,
+ const Ice::Current&)
{
return physicsWorld->getRobotJointVelocity(robotName, nodeName);
}
-NameValueMap Simulator::getRobotJointTorques(const std::string& robotName, const Ice::Current&)
+NameValueMap
+Simulator::getRobotJointTorques(const std::string& robotName, const Ice::Current&)
{
return physicsWorld->getRobotJointTorques(robotName);
}
-ForceTorqueDataSeq Simulator::getRobotForceTorqueSensors(const std::string& robotName, const Ice::Current&)
+ForceTorqueDataSeq
+Simulator::getRobotForceTorqueSensors(const std::string& robotName, const Ice::Current&)
{
return physicsWorld->getRobotForceTorqueSensors(robotName);
}
-float Simulator::getRobotJointLimitLo(const std::string& robotName, const std::string& nodeName, const Ice::Current&)
+float
+Simulator::getRobotJointLimitLo(const std::string& robotName,
+ const std::string& nodeName,
+ const Ice::Current&)
{
return physicsWorld->getRobotJointLimitLo(robotName, nodeName);
}
-float Simulator::getRobotJointLimitHi(const std::string& robotName, const std::string& nodeName, const Ice::Current&)
+float
+Simulator::getRobotJointLimitHi(const std::string& robotName,
+ const std::string& nodeName,
+ const Ice::Current&)
{
return physicsWorld->getRobotJointLimitHi(robotName, nodeName);
}
-PoseBasePtr Simulator::getRobotPose(const std::string& robotName, const Ice::Current&)
+PoseBasePtr
+Simulator::getRobotPose(const std::string& robotName, const Ice::Current&)
{
PosePtr p(new Pose(physicsWorld->getRobotPose(robotName)));
return p;
}
-float Simulator::getRobotMaxTorque(const std::string& robotName, const std::string& nodeName, const Ice::Current&)
+float
+Simulator::getRobotMaxTorque(const std::string& robotName,
+ const std::string& nodeName,
+ const Ice::Current&)
{
return physicsWorld->getRobotMaxTorque(robotName, nodeName);
}
-void Simulator::setRobotMaxTorque(const std::string& robotName, const std::string& nodeName, float maxTorque, const Ice::Current&)
+void
+Simulator::setRobotMaxTorque(const std::string& robotName,
+ const std::string& nodeName,
+ float maxTorque,
+ const Ice::Current&)
{
physicsWorld->setRobotMaxTorque(robotName, nodeName, maxTorque);
}
-PoseBasePtr Simulator::getRobotNodePose(const std::string& robotName, const std::string& robotNodeName, const Ice::Current&)
+PoseBasePtr
+Simulator::getRobotNodePose(const std::string& robotName,
+ const std::string& robotNodeName,
+ const Ice::Current&)
{
PosePtr p(new Pose(physicsWorld->getRobotNodePose(robotName, robotNodeName)));
return p;
}
-Vector3BasePtr Simulator::getRobotLinearVelocity(const std::string& robotName, const std::string& nodeName, const Ice::Current&)
+Vector3BasePtr
+Simulator::getRobotLinearVelocity(const std::string& robotName,
+ const std::string& nodeName,
+ const Ice::Current&)
{
Vector3Ptr v(new Vector3(physicsWorld->getRobotLinearVelocity(robotName, nodeName)));
return v;
}
-Vector3BasePtr Simulator::getRobotAngularVelocity(const std::string& robotName, const std::string& nodeName, const Ice::Current&)
+Vector3BasePtr
+Simulator::getRobotAngularVelocity(const std::string& robotName,
+ const std::string& nodeName,
+ const Ice::Current&)
{
Vector3Ptr v(new Vector3(physicsWorld->getRobotAngularVelocity(robotName, nodeName)));
return v;
}
-void Simulator::setRobotLinearVelocity(const std::string& robotName, const std::string& robotNodeName, const Vector3BasePtr& vel, const Ice::Current&)
+void
+Simulator::setRobotLinearVelocity(const std::string& robotName,
+ const std::string& robotNodeName,
+ const Vector3BasePtr& vel,
+ const Ice::Current&)
{
Eigen::Vector3f newVel = Vector3Ptr::dynamicCast(vel)->toEigen();
physicsWorld->setRobotLinearVelocity(robotName, robotNodeName, newVel);
}
-void Simulator::setRobotAngularVelocity(const std::string& robotName, const std::string& robotNodeName, const Vector3BasePtr& vel, const Ice::Current&)
+void
+Simulator::setRobotAngularVelocity(const std::string& robotName,
+ const std::string& robotNodeName,
+ const Vector3BasePtr& vel,
+ const Ice::Current&)
{
Vector3Ptr newVel = Vector3Ptr::dynamicCast(vel);
physicsWorld->setRobotAngularVelocity(robotName, robotNodeName, newVel->toEigen());
}
-void Simulator::setRobotLinearVelocityRobotRootFrame(const std::string& robotName, const std::string& robotNodeName, const Vector3BasePtr& vel, const Ice::Current&)
+void
+Simulator::setRobotLinearVelocityRobotRootFrame(const std::string& robotName,
+ const std::string& robotNodeName,
+ const Vector3BasePtr& vel,
+ const Ice::Current&)
{
Eigen::Vector3f newVel = Vector3Ptr::dynamicCast(vel)->toEigen();
physicsWorld->setRobotLinearVelocityRobotRootFrame(robotName, robotNodeName, newVel);
}
-void Simulator::setRobotAngularVelocityRobotRootFrame(const std::string& robotName, const std::string& robotNodeName, const Vector3BasePtr& vel, const Ice::Current&)
+void
+Simulator::setRobotAngularVelocityRobotRootFrame(const std::string& robotName,
+ const std::string& robotNodeName,
+ const Vector3BasePtr& vel,
+ const Ice::Current&)
{
Eigen::Vector3f newVel = Vector3Ptr::dynamicCast(vel)->toEigen();
physicsWorld->setRobotAngularVelocityRobotRootFrame(robotName, robotNodeName, newVel);
}
-bool Simulator::hasRobot(const std::string& robotName, const Ice::Current&)
+bool
+Simulator::hasRobot(const std::string& robotName, const Ice::Current&)
{
return physicsWorld->hasRobot(robotName);
}
-bool Simulator::hasRobotNode(const std::string& robotName, const std::string& robotNodeName, const Ice::Current&)
+bool
+Simulator::hasRobotNode(const std::string& robotName,
+ const std::string& robotNodeName,
+ const Ice::Current&)
{
return physicsWorld->hasRobotNode(robotName, robotNodeName);
}
-PoseBasePtr Simulator::getObjectPose(const std::string& objectName, const Ice::Current&)
+PoseBasePtr
+Simulator::getObjectPose(const std::string& objectName, const Ice::Current&)
{
PosePtr p(new Pose(physicsWorld->getObjectPose(objectName)));
return p;
}
-void Simulator::objectReleased(const std::string& robotName, const std::string& robotNodeName, const std::string& objectName, const Ice::Current&)
+void
+Simulator::objectReleased(const std::string& robotName,
+ const std::string& robotNodeName,
+ const std::string& objectName,
+ const Ice::Current&)
{
ARMARX_VERBOSE << "Object released " << robotName << "," << objectName;
physicsWorld->objectReleased(robotName, robotNodeName, objectName);
}
-void Simulator::objectGrasped(const std::string& robotName, const std::string& robotNodeName, const std::string& objectName, const Ice::Current&)
+void
+Simulator::objectGrasped(const std::string& robotName,
+ const std::string& robotNodeName,
+ const std::string& objectName,
+ const Ice::Current&)
{
ARMARX_VERBOSE << "Object grasped" << robotName << "," << objectName;
physicsWorld->objectGrasped(robotName, robotNodeName, objectName);
}
-
-float Simulator::getSimTime(const Ice::Current&)
+float
+Simulator::getSimTime(const Ice::Current&)
{
return static_cast<float>(physicsWorld->getCurrentSimTime());
}
-SceneVisuData Simulator::getScene(const Ice::Current&)
+SceneVisuData
+Simulator::getScene(const Ice::Current&)
{
SceneVisuData res = physicsWorld->copySceneVisuData();
res.priorKnowledgeName = priorKnowledgeName;
@@ -1620,9 +1883,11 @@ SceneVisuData Simulator::getScene(const Ice::Current&)
return res;
}
-SimulatorInformation Simulator::getSimulatorInformation(const Ice::Current&)
+SimulatorInformation
+Simulator::getSimulatorInformation(const Ice::Current&)
{
- auto lockEngine = physicsWorld->getScopedEngineLock(__FUNCTION__); // we are accessing objects and robots
+ auto lockEngine =
+ physicsWorld->getScopedEngineLock(__FUNCTION__); // we are accessing objects and robots
auto lockData = physicsWorld->getScopedSyncLock(__FUNCTION__);
SimulatorInformation res;
res.comTimeMS = currentComTimeMS;
@@ -1691,7 +1956,8 @@ SimulatorInformation Simulator::getSimulatorInformation(const Ice::Current&)
return res;
}
-std::string Simulator::addRobotFromFile(const armarx::data::PackagePath& packagePath, const Ice::Current& c)
+std::string
+Simulator::addRobotFromFile(const armarx::data::PackagePath& packagePath, const Ice::Current& c)
{
ArmarXDataPath::FindPackageAndAddDataPath(packagePath.package);
@@ -1705,7 +1971,7 @@ std::string Simulator::addRobotFromFile(const armarx::data::PackagePath& package
}
const PackagePath pp(packagePath);
- if(std::filesystem::exists(pp.toSystemPath()))
+ if (std::filesystem::exists(pp.toSystemPath()))
{
return addRobot(pp.toSystemPath());
}
@@ -1714,14 +1980,36 @@ std::string Simulator::addRobotFromFile(const armarx::data::PackagePath& package
return "";
}
-
-std::string Simulator::addRobot(std::string robotInstanceName, const std::string& robFileGlobal, Eigen::Matrix4f gp, const std::string& robFile, double pid_p, double pid_i, double pid_d, bool isStatic, float scaling, bool colModel, std::map<std::string, float> initConfig, bool selfCollisions)
+std::string
+Simulator::addRobot(std::string robotInstanceName,
+ const std::string& robFileGlobal,
+ Eigen::Matrix4f gp,
+ const std::string& robFile,
+ double pid_p,
+ double pid_i,
+ double pid_d,
+ bool isStatic,
+ float scaling,
+ bool colModel,
+ std::map<std::string, float> initConfig,
+ bool selfCollisions)
{
ARMARX_DEBUG << "Add robot from file " << robFileGlobal;
auto lockEngine = physicsWorld->getScopedEngineLock(__FUNCTION__);
auto lockData = physicsWorld->getScopedSyncLock(__FUNCTION__);
- if (physicsWorld->addRobot(robotInstanceName, robFileGlobal, gp, robFile, pid_p, pid_i, pid_d, isStatic, scaling, colModel, initConfig, selfCollisions))
+ if (physicsWorld->addRobot(robotInstanceName,
+ robFileGlobal,
+ gp,
+ robFile,
+ pid_p,
+ pid_i,
+ pid_d,
+ isStatic,
+ scaling,
+ colModel,
+ initConfig,
+ selfCollisions))
{
// update report topics
if (!updateRobotTopics())
@@ -1738,7 +2026,8 @@ std::string Simulator::addRobot(std::string robotInstanceName, const std::string
return robotInstanceName;
}
-bool Simulator::updateRobotTopics()
+bool
+Simulator::updateRobotTopics()
{
SimulatedWorldData& syncData = physicsWorld->getReportData();
@@ -1752,7 +2041,7 @@ bool Simulator::updateRobotTopics()
offeringTopic(top);
//if (this->getState() == eManagedIceObjectStarted)
{
- ARMARX_INFO << "Connecting to topic " << top ;
+ ARMARX_INFO << "Connecting to topic " << top;
rob.simulatorRobotListenerPrx = getTopic<SimulatorRobotListenerInterfacePrx>(top);
}
@@ -1763,7 +2052,7 @@ bool Simulator::updateRobotTopics()
//if (this->getState() == eManagedIceObjectStarted)
{
- ARMARX_INFO << "Connecting to topic " << fti.topicName ;
+ ARMARX_INFO << "Connecting to topic " << fti.topicName;
fti.prx = getTopic<SimulatorForceTorqueListenerInterfacePrx>(fti.topicName);
}
}
@@ -1778,15 +2067,17 @@ bool Simulator::updateRobotTopics()
return true;
}
-void Simulator::simulationLoop()
+void
+Simulator::simulationLoop()
{
step();
}
-
-
-ObjectClassInformationSequence Simulator::getObjectClassPoses(
- const std::string& robotName, const std::string& frameName, const std::string& className, const Ice::Current&)
+ObjectClassInformationSequence
+Simulator::getObjectClassPoses(const std::string& robotName,
+ const std::string& frameName,
+ const std::string& className,
+ const Ice::Current&)
{
auto lockEngine = physicsWorld->getScopedEngineLock(__FUNCTION__);
auto lockData = physicsWorld->getScopedSyncLock(__FUNCTION__);
@@ -1800,7 +2091,8 @@ ObjectClassInformationSequence Simulator::getObjectClassPoses(
{
ObjectClassInformation inf;
// Conversion from seconds to microseconds is 10^6 = 1.0e6 != 10e6 = 1.0e7!!!
- inf.timestampMicroSeconds = static_cast<Ice::Long>(physicsWorld->getCurrentSimTime() * 1.0e6);
+ inf.timestampMicroSeconds =
+ static_cast<Ice::Long>(physicsWorld->getCurrentSimTime() * 1.0e6);
inf.className = className;
inf.manipulationObjectName = object;
Eigen::Matrix4f worldPose = physicsWorld->getObjectPose(object);
@@ -1820,20 +2112,26 @@ ObjectClassInformationSequence Simulator::getObjectClassPoses(
return result;
}
-
-DistanceInfo Simulator::getDistance(const std::string& robotName, const std::string& robotNodeName, const std::string& worldObjectName, const Ice::Current&)
+DistanceInfo
+Simulator::getDistance(const std::string& robotName,
+ const std::string& robotNodeName,
+ const std::string& worldObjectName,
+ const Ice::Current&)
{
return physicsWorld->getDistance(robotName, robotNodeName, worldObjectName);
}
-
-DistanceInfo Simulator::getRobotNodeDistance(const std::string& robotName, const std::string& robotNodeName1, const std::string& robotNodeName2, const Ice::Current&)
+DistanceInfo
+Simulator::getRobotNodeDistance(const std::string& robotName,
+ const std::string& robotNodeName1,
+ const std::string& robotNodeName2,
+ const Ice::Current&)
{
return physicsWorld->getRobotNodeDistance(robotName, robotNodeName1, robotNodeName2);
}
-
-void Simulator::showContacts(bool enable, const std::string& layerName, const Ice::Current&)
+void
+Simulator::showContacts(bool enable, const std::string& layerName, const Ice::Current&)
{
auto lockData = physicsWorld->getScopedSyncLock(__FUNCTION__);
ARMARX_INFO << "showcontacts:" << enable;
@@ -1859,8 +2157,8 @@ void Simulator::showContacts(bool enable, const std::string& layerName, const Ic
}
}
-
-void Simulator::reInitialize(const Ice::Current&)
+void
+Simulator::reInitialize(const Ice::Current&)
{
resetData(true);
@@ -1868,14 +2166,14 @@ void Simulator::reInitialize(const Ice::Current&)
simulatorResetEventTopic->simulatorWasReset();
}
-
-bool Simulator::hasObject(const std::string& instanceName, const Ice::Current&)
+bool
+Simulator::hasObject(const std::string& instanceName, const Ice::Current&)
{
return physicsWorld->hasObject(instanceName);
}
-
-void Simulator::start(const Ice::Current&)
+void
+Simulator::start(const Ice::Current&)
{
simulatorThreadShutdown = false;
bool notify = !simulatorThreadRunning && this->isAlive();
@@ -1892,7 +2190,8 @@ void Simulator::start(const Ice::Current&)
}
}
-void Simulator::run()
+void
+Simulator::run()
{
IceUtil::Time startTime;
int sleepTimeMS;
@@ -1911,12 +2210,14 @@ void Simulator::run()
{
startTime = IceUtil::Time::now();
step();
- sleepTimeMS = static_cast<int>(loopTimeMS * (1 / timeServerSpeed)
- - static_cast<float>((IceUtil::Time::now() - startTime).toMilliSeconds()));
+ sleepTimeMS = static_cast<int>(
+ loopTimeMS * (1 / timeServerSpeed) -
+ static_cast<float>((IceUtil::Time::now() - startTime).toMilliSeconds()));
if (sleepTimeMS < 0)
{
- ARMARX_DEBUG << deactivateSpam(5) << "Simulation step took " << -sleepTimeMS << " milliseconds too long!";
+ ARMARX_DEBUG << deactivateSpam(5) << "Simulation step took " << -sleepTimeMS
+ << " milliseconds too long!";
}
else
{
@@ -1927,37 +2228,44 @@ void Simulator::run()
ARMARX_VERBOSE << "Exiting Simulator::run()";
}
-void Simulator::pause(const Ice::Current&)
+void
+Simulator::pause(const Ice::Current&)
{
simulatorThreadRunning = false;
}
-void Simulator::stop(const Ice::Current& c)
+void
+Simulator::stop(const Ice::Current& c)
{
pause(c);
}
-void Simulator::setSpeed(Ice::Float newSpeed, const Ice::Current&)
+void
+Simulator::setSpeed(Ice::Float newSpeed, const Ice::Current&)
{
timeServerSpeed = static_cast<float>(newSpeed);
}
-Ice::Float Simulator::getSpeed(const Ice::Current&)
+Ice::Float
+Simulator::getSpeed(const Ice::Current&)
{
return Ice::Float(timeServerSpeed);
}
-Ice::Int Simulator::getStepTimeMS(const ::Ice::Current&)
+Ice::Int
+Simulator::getStepTimeMS(const ::Ice::Current&)
{
return physicsWorld->getFixedTimeStepMS();
}
-Ice::Long Simulator::getTime(const Ice::Current&)
+Ice::Long
+Simulator::getTime(const Ice::Current&)
{
return static_cast<long>(physicsWorld->getCurrentSimTime() * 1000);
}
-void Simulator::step(const Ice::Current&)
+void
+Simulator::step(const Ice::Current&)
{
IceUtil::Time startTime = IceUtil::Time::now();
@@ -1968,14 +2276,14 @@ void Simulator::step(const Ice::Current&)
else
{
physicsWorld->stepPhysicsFixedTimeStep();
-
}
IceUtil::Time durationSim = IceUtil::Time::now() - startTime;
if (durationSim.toMilliSecondsDouble() > MAX_SIM_TIME_WARNING)
{
- ARMARX_INFO << deactivateSpam(5) << "*** Simulation slow !! Simulation time in ms:" << durationSim.toMilliSecondsDouble();
+ ARMARX_INFO << deactivateSpam(5) << "*** Simulation slow !! Simulation time in ms:"
+ << durationSim.toMilliSecondsDouble();
}
ARMARX_DEBUG << "*** Step Physics, MS:" << durationSim.toMilliSecondsDouble();
@@ -1991,18 +2299,16 @@ void Simulator::step(const Ice::Current&)
currentSyncTimeMS = static_cast<float>(durationSync.toMilliSecondsDouble());
}
timeTopicPrx->reportTime(getTime());
-
-
}
-bool Simulator::isRunning(const Ice::Current&)
+bool
+Simulator::isRunning(const Ice::Current&)
{
return simulatorThreadRunning;
}
-
-
-void Simulator::reportVisuLoop()
+void
+Simulator::reportVisuLoop()
{
ARMARX_DEBUG << "report visu updates";
@@ -2032,7 +2338,8 @@ void Simulator::reportVisuLoop()
if (publishContacts && entityDrawerPrx)
{
//entityDrawerPrx->clearLayer(contactLayerName);
- std::vector<SimDynamics::DynamicsEngine::DynamicsContactInfo> contacts = physicsWorld->copyContacts();
+ std::vector<SimDynamics::DynamicsEngine::DynamicsContactInfo> contacts =
+ physicsWorld->copyContacts();
VirtualRobot::ColorMap cm(VirtualRobot::ColorMap::eHot);
float maxImpulse = 2.0f;
float arrowLength = 50.0f;
@@ -2053,7 +2360,7 @@ void Simulator::reportVisuLoop()
if (!c.objectAName.empty())
{
- objA = c.objectAName;//->getName();
+ objA = c.objectAName; //->getName();
}
else
{
@@ -2064,14 +2371,15 @@ void Simulator::reportVisuLoop()
if (!c.objectBName.empty())
{
- objB = c.objectBName;//->getName();
+ objB = c.objectBName; //->getName();
}
else
{
objB = "<unknown>";
}
- ARMARX_DEBUG << "CONTACT " << name << ", objects: " << objA << "-" << objB << ", impulse:" << c.appliedImpulse;
+ ARMARX_DEBUG << "CONTACT " << name << ", objects: " << objA << "-" << objB
+ << ", impulse:" << c.appliedImpulse;
float intensity = static_cast<float>(c.appliedImpulse);
if (intensity > maxImpulse)
@@ -2092,8 +2400,10 @@ void Simulator::reportVisuLoop()
Vector3Ptr p2(new Vector3(c.posGlobalB));
Vector3Ptr d2(new Vector3(c.normalGlobalB));
- entityDrawerPrx->setArrowVisu(contactLayerName, name, p1, d1, dc, arrowLength, 5.0f);
- entityDrawerPrx->setArrowVisu(contactLayerName, name2, p2, d2, dc, arrowLength, 5.0f);
+ entityDrawerPrx->setArrowVisu(
+ contactLayerName, name, p1, d1, dc, arrowLength, 5.0f);
+ entityDrawerPrx->setArrowVisu(
+ contactLayerName, name2, p2, d2, dc, arrowLength, 5.0f);
i++;
}
@@ -2106,7 +2416,6 @@ void Simulator::reportVisuLoop()
std::string name2 = name + "_2";
entityDrawerPrx->removeArrowVisu(contactLayerName, name);
entityDrawerPrx->removeArrowVisu(contactLayerName, name2);
-
}
lastPublishedContacts = i;
@@ -2118,9 +2427,9 @@ void Simulator::reportVisuLoop()
{
auto pos = new Vector3(physicsWorld->getRobot(name)->getCoMGlobal());
pos->z = 0.0f;
- entityDrawerPrx->setSphereVisu("CoM", "GlobalCoM_" + name, pos, DrawColor {0.f, 0.f, 1.f, 0.5f}, 20);
+ entityDrawerPrx->setSphereVisu(
+ "CoM", "GlobalCoM_" + name, pos, DrawColor{0.f, 0.f, 1.f, 0.5f}, 20);
}
-
}
}
IceUtil::Time duration = IceUtil::Time::now() - startTime;
@@ -2136,11 +2445,13 @@ void Simulator::reportVisuLoop()
ARMARX_DEBUG << "report visu updates done, time (ms):" << reportVisuTimeMS;
if (reportVisuTimeMS > 5)
{
- ARMARX_INFO << deactivateSpam(10) << "report visu updates took long, time (ms):" << reportVisuTimeMS;
+ ARMARX_INFO << deactivateSpam(10)
+ << "report visu updates took long, time (ms):" << reportVisuTimeMS;
}
}
-void Simulator::reportDataLoop()
+void
+Simulator::reportDataLoop()
{
ARMARX_DEBUG_S << "report data updates";
@@ -2169,13 +2480,13 @@ void Simulator::reportDataLoop()
globalRobotPose.header = header;
globalRobotPose.transform = r.pose;
- if(globalRobotLocalization)
+ if (globalRobotLocalization)
{
globalRobotLocalization->reportGlobalRobotPose(globalRobotPose, ctx);
}
}
- if(r.simulatorRobotListenerPrx)
+ if (r.simulatorRobotListenerPrx)
{
r.simulatorRobotListenerPrx->reportState(state);
}
@@ -2197,8 +2508,8 @@ void Simulator::reportDataLoop()
ARMARX_DEBUG_S << "report data updates done, time (ms):" << currentComTimeMS;
}
-
-ContactInfoSequence Simulator::getContacts(const Ice::Current&)
+ContactInfoSequence
+Simulator::getContacts(const Ice::Current&)
{
ContactInfoSequence result;
auto contacts = this->physicsWorld->copyContacts();
@@ -2214,43 +2525,53 @@ ContactInfoSequence Simulator::getContacts(const Ice::Current&)
return result;
}
-
-void Simulator::updateContacts(bool enable, const Ice::Current&)
+void
+Simulator::updateContacts(bool enable, const Ice::Current&)
{
this->physicsWorld->updateContacts(enable);
}
-
-std::string Simulator::addScaledRobot(const std::string& filename, float scaling, const Ice::Current&)
+std::string
+Simulator::addScaledRobot(const std::string& filename, float scaling, const Ice::Current&)
{
ARMARX_TRACE;
std::string instanceName;
- this->physicsWorld->addRobot(instanceName, filename, Eigen::Matrix4f::Identity(), "", 10.0, 0, 0, false, scaling);
+ this->physicsWorld->addRobot(
+ instanceName, filename, Eigen::Matrix4f::Identity(), "", 10.0, 0, 0, false, scaling);
return instanceName;
}
-std::string Simulator::addScaledRobotName(const std::string& instanceName, const std::string& filename, float scaling, const Ice::Current&)
+std::string
+Simulator::addScaledRobotName(const std::string& instanceName,
+ const std::string& filename,
+ float scaling,
+ const Ice::Current&)
{
ARMARX_TRACE;
std::string name = instanceName;
- this->physicsWorld->addRobot(name, filename, Eigen::Matrix4f::Identity(), "", 10.0, 0, 0, false, scaling);
+ this->physicsWorld->addRobot(
+ name, filename, Eigen::Matrix4f::Identity(), "", 10.0, 0, 0, false, scaling);
return name;
}
-bool Simulator::removeRobot(const std::string& robotName, const Ice::Current&)
+bool
+Simulator::removeRobot(const std::string& robotName, const Ice::Current&)
{
return this->physicsWorld->removeRobot(robotName);
}
-std::string Simulator::addRobot(const std::string& filename, const Ice::Current&)
+std::string
+Simulator::addRobot(const std::string& filename, const Ice::Current&)
{
ARMARX_TRACE;
std::string instanceName;
- this->physicsWorld->addRobot(instanceName, filename, Eigen::Matrix4f::Identity(), "", 10.0, 0, 0, false, 1.0f);
+ this->physicsWorld->addRobot(
+ instanceName, filename, Eigen::Matrix4f::Identity(), "", 10.0, 0, 0, false, 1.0f);
return instanceName;
}
-SimulatedRobotState armarx::Simulator::getRobotState(const std::string& robotName, const Ice::Current&)
+SimulatedRobotState
+armarx::Simulator::getRobotState(const std::string& robotName, const Ice::Current&)
{
SimulatedWorldData report = physicsWorld->getReportData();
for (auto& robot : report.robots)