diff --git a/source/RobotComponents/applications/AStarPathPlanner/main.cpp b/source/RobotComponents/applications/AStarPathPlanner/main.cpp
index c86491ca31c0821ab787378974aaf3b6b009b3c5..5ab55b2c7149ec5a6b14dd6523802a2bdc615f0d 100644
--- a/source/RobotComponents/applications/AStarPathPlanner/main.cpp
+++ b/source/RobotComponents/applications/AStarPathPlanner/main.cpp
@@ -20,13 +20,14 @@
* GNU General Public License
*/
-#include <RobotComponents/components/PathPlanner/AStarPathPlanner.h>
-
-#include <ArmarXCore/core/application/Application.h>
#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/application/Application.h>
#include <ArmarXCore/core/logging/Logging.h>
-int main(int argc, char* argv[])
+#include <RobotComponents/components/PathPlanner/AStarPathPlanner.h>
+
+int
+main(int argc, char* argv[])
{
return armarx::runSimpleComponentApp<armarx::AStarPathPlanner>(argc, argv, "AStarPathPlanner");
}
diff --git a/source/RobotComponents/applications/CollisionChecker/main.cpp b/source/RobotComponents/applications/CollisionChecker/main.cpp
index 64ec26c0bea9e290f6b734d4ae88a965c8856e6f..8f08517b018bca76ae9f3ededa3baa5a76d5457e 100644
--- a/source/RobotComponents/applications/CollisionChecker/main.cpp
+++ b/source/RobotComponents/applications/CollisionChecker/main.cpp
@@ -20,13 +20,15 @@
* GNU General Public License
*/
-#include <RobotComponents/components/CollisionCheckerComponent/CollisionCheckerComponent.h>
-
-#include <ArmarXCore/core/application/Application.h>
#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/application/Application.h>
#include <ArmarXCore/core/logging/Logging.h>
-int main(int argc, char* argv[])
+#include <RobotComponents/components/CollisionCheckerComponent/CollisionCheckerComponent.h>
+
+int
+main(int argc, char* argv[])
{
- return armarx::runSimpleComponentApp < armarx::CollisionCheckerComponent > (argc, argv, "CollisionChecker");
+ return armarx::runSimpleComponentApp<armarx::CollisionCheckerComponent>(
+ argc, argv, "CollisionChecker");
}
diff --git a/source/RobotComponents/applications/DHParameterOptimizationLogger/main.cpp b/source/RobotComponents/applications/DHParameterOptimizationLogger/main.cpp
index 6cb5560a50bf3e0c7507fa8420f276f11cf3a5ed..96718fcc537f764a641c572a1a7dadc6f6de954a 100644
--- a/source/RobotComponents/applications/DHParameterOptimizationLogger/main.cpp
+++ b/source/RobotComponents/applications/DHParameterOptimizationLogger/main.cpp
@@ -20,13 +20,15 @@
* GNU General Public License
*/
-#include <RobotComponents/components/DHParameterOptimizationLogger/DHParameterOptimizationLogger.h>
-
-#include <ArmarXCore/core/application/Application.h>
#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/application/Application.h>
#include <ArmarXCore/core/logging/Logging.h>
-int main(int argc, char* argv[])
+#include <RobotComponents/components/DHParameterOptimizationLogger/DHParameterOptimizationLogger.h>
+
+int
+main(int argc, char* argv[])
{
- return armarx::runSimpleComponentApp < armarx::DHParameterOptimizationLogger > (argc, argv, "DHParameterOptimizationLogger");
+ return armarx::runSimpleComponentApp<armarx::DHParameterOptimizationLogger>(
+ argc, argv, "DHParameterOptimizationLogger");
}
diff --git a/source/RobotComponents/applications/DMPComponent/main.cpp b/source/RobotComponents/applications/DMPComponent/main.cpp
index 04179ac989f5a57a025e6992c6bf5fbf442dabce..e1eb6ddcf583b6b7dab782ae30624ac7bba14651 100644
--- a/source/RobotComponents/applications/DMPComponent/main.cpp
+++ b/source/RobotComponents/applications/DMPComponent/main.cpp
@@ -20,13 +20,14 @@
* GNU General Public License
*/
-#include <RobotComponents/components/DMPComponent/DMPComponent.h>
-
-#include <ArmarXCore/core/application/Application.h>
#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/application/Application.h>
#include <ArmarXCore/core/logging/Logging.h>
-int main(int argc, char* argv[])
+#include <RobotComponents/components/DMPComponent/DMPComponent.h>
+
+int
+main(int argc, char* argv[])
{
return armarx::runSimpleComponentApp<armarx::DMPComponent>(argc, argv, "DMPComponent");
}
diff --git a/source/RobotComponents/applications/FunctionApproximator/main.cpp b/source/RobotComponents/applications/FunctionApproximator/main.cpp
index 88988afd7cf9b6b58f246ff8d37c6ebfe3caacae..bbb977efbe2d3eb7d66876d24bdaf77443f49584 100644
--- a/source/RobotComponents/applications/FunctionApproximator/main.cpp
+++ b/source/RobotComponents/applications/FunctionApproximator/main.cpp
@@ -20,13 +20,15 @@
* GNU General Public License
*/
-#include <RobotComponents/components/FunctionApproximator/FunctionApproximator.h>
-
-#include <ArmarXCore/core/application/Application.h>
#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/application/Application.h>
#include <ArmarXCore/core/logging/Logging.h>
-int main(int argc, char* argv[])
+#include <RobotComponents/components/FunctionApproximator/FunctionApproximator.h>
+
+int
+main(int argc, char* argv[])
{
- return armarx::runSimpleComponentApp < armarx::FunctionApproximator > (argc, argv, "FunctionApproximator");
+ return armarx::runSimpleComponentApp<armarx::FunctionApproximator>(
+ argc, argv, "FunctionApproximator");
}
diff --git a/source/RobotComponents/applications/GraspingManager/GraspingManagerApp.h b/source/RobotComponents/applications/GraspingManager/GraspingManagerApp.h
index 19af106e360bc094675e623b69029567a28933f5..b614b3926c2925c1815a7d87f1d1c9f6b2e70c29 100644
--- a/source/RobotComponents/applications/GraspingManager/GraspingManagerApp.h
+++ b/source/RobotComponents/applications/GraspingManager/GraspingManagerApp.h
@@ -26,6 +26,7 @@
#include <ArmarXCore/core/application/Application.h>
+
#include <RobotComponents/components/GraspingManager/GraspingManager.h>
namespace armarx
@@ -36,17 +37,16 @@ namespace armarx
*
* Detailed Description
*/
- class GraspingManagerApp :
- virtual public armarx::Application
+ class GraspingManagerApp : virtual public armarx::Application
{
/**
* @see armarx::Application::setup()
*/
- void setup(const ManagedIceObjectRegistryInterfacePtr& registry,
- Ice::PropertiesPtr properties) override
+ void
+ setup(const ManagedIceObjectRegistryInterfacePtr& registry,
+ Ice::PropertiesPtr properties) override
{
registry->addObject(Component::create<GraspingManager>(properties));
}
};
-}
-
+} // namespace armarx
diff --git a/source/RobotComponents/applications/GraspingManager/main.cpp b/source/RobotComponents/applications/GraspingManager/main.cpp
index 6bdbd4f3153f7b2bc6f63d108473131629060e97..9517ff44bc03d422ed65d4758cb4b572a969f348 100644
--- a/source/RobotComponents/applications/GraspingManager/main.cpp
+++ b/source/RobotComponents/applications/GraspingManager/main.cpp
@@ -22,12 +22,14 @@
* GNU General Public License
*/
-#include "GraspingManagerApp.h"
#include <ArmarXCore/core/logging/Logging.h>
-int main(int argc, char* argv[])
+#include "GraspingManagerApp.h"
+
+int
+main(int argc, char* argv[])
{
- armarx::ApplicationPtr app = armarx::Application::createInstance < armarx::GraspingManagerApp > ();
+ armarx::ApplicationPtr app = armarx::Application::createInstance<armarx::GraspingManagerApp>();
app->setName("GraspingManager");
return app->main(argc, argv);
diff --git a/source/RobotComponents/applications/HumanObstacleDetection/main.cpp b/source/RobotComponents/applications/HumanObstacleDetection/main.cpp
index 28f9e74c83d85d7cb4958a9380c976031e94fb19..dd669c1603ff05692a463ae8be6a61b40fcea18b 100644
--- a/source/RobotComponents/applications/HumanObstacleDetection/main.cpp
+++ b/source/RobotComponents/applications/HumanObstacleDetection/main.cpp
@@ -26,8 +26,8 @@
#include <string>
// ArmarX
-#include <ArmarXCore/core/application/Application.h>
#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/application/Application.h>
#include <ArmarXCore/core/logging/Logging.h>
// Component
@@ -35,7 +35,8 @@
using namespace armarx;
-int main(int argc, char* argv[])
+int
+main(int argc, char* argv[])
{
const std::string name = HumanObstacleDetection::default_name;
return runSimpleComponentApp<HumanObstacleDetection>(argc, argv, name);
diff --git a/source/RobotComponents/applications/LaserScannerObstacleDetection/main.cpp b/source/RobotComponents/applications/LaserScannerObstacleDetection/main.cpp
index 17b15202d2c8ee1de58be44b40623f33240b6505..590137c76e56994fbf2037d1931fba2666c32a3f 100644
--- a/source/RobotComponents/applications/LaserScannerObstacleDetection/main.cpp
+++ b/source/RobotComponents/applications/LaserScannerObstacleDetection/main.cpp
@@ -25,15 +25,16 @@
#include <string>
// ArmarX
-#include <ArmarXCore/core/application/Application.h>
#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/application/Application.h>
// Component
#include <RobotComponents/components/LaserScannerObstacleDetection/LaserScannerObstacleDetection.h>
using namespace armarx;
-int main(int argc, char* argv[])
+int
+main(int argc, char* argv[])
{
const std::string name = LaserScannerObstacleDetection::default_name;
return runSimpleComponentApp<LaserScannerObstacleDetection>(argc, argv, name);
diff --git a/source/RobotComponents/applications/LaserScannerSelfLocalisation/main.cpp b/source/RobotComponents/applications/LaserScannerSelfLocalisation/main.cpp
index 6b4bd1b42301acce1b2c9b8698cf434c74419028..8b9b60330854ba033c504bfcca3f840779688592 100644
--- a/source/RobotComponents/applications/LaserScannerSelfLocalisation/main.cpp
+++ b/source/RobotComponents/applications/LaserScannerSelfLocalisation/main.cpp
@@ -20,13 +20,15 @@
* GNU General Public License
*/
-#include <RobotComponents/components/LaserScannerSelfLocalisation/LaserScannerSelfLocalisation.h>
-
-#include <ArmarXCore/core/application/Application.h>
#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/application/Application.h>
#include <ArmarXCore/core/logging/Logging.h>
-int main(int argc, char* argv[])
+#include <RobotComponents/components/LaserScannerSelfLocalisation/LaserScannerSelfLocalisation.h>
+
+int
+main(int argc, char* argv[])
{
- return armarx::runSimpleComponentApp < armarx::LaserScannerSelfLocalisation > (argc, argv, "LaserScannerSelfLocalisation");
+ return armarx::runSimpleComponentApp<armarx::LaserScannerSelfLocalisation>(
+ argc, argv, "LaserScannerSelfLocalisation");
}
diff --git a/source/RobotComponents/applications/MMMPlayer/main.cpp b/source/RobotComponents/applications/MMMPlayer/main.cpp
index b2caac3fd1f145584c2e2ae8ecad56f385bcbba5..f1f6b68e99d5772b3f1a9f3adf77f744b670ceec 100644
--- a/source/RobotComponents/applications/MMMPlayer/main.cpp
+++ b/source/RobotComponents/applications/MMMPlayer/main.cpp
@@ -22,13 +22,14 @@
* GNU General Public License
*/
-#include <RobotComponents/components/MMMPlayer/MMMPlayer.h>
-
-#include <ArmarXCore/core/application/Application.h>
#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/application/Application.h>
#include <ArmarXCore/core/logging/Logging.h>
-int main(int argc, char* argv[])
+#include <RobotComponents/components/MMMPlayer/MMMPlayer.h>
+
+int
+main(int argc, char* argv[])
{
return armarx::runSimpleComponentApp<armarx::MMMPlayer>(argc, argv, "MMMPlayer");
}
diff --git a/source/RobotComponents/applications/MotionPlanningRemoteObjectNode/main.cpp b/source/RobotComponents/applications/MotionPlanningRemoteObjectNode/main.cpp
index 49b3e7c79fb6b7dcdb671d9b81c86b1b12bc8cc5..5266f98745f2bca2af403265d771be5df51ee16c 100644
--- a/source/RobotComponents/applications/MotionPlanningRemoteObjectNode/main.cpp
+++ b/source/RobotComponents/applications/MotionPlanningRemoteObjectNode/main.cpp
@@ -27,7 +27,9 @@
#include <RobotComponents/components/MotionPlanning/MotionPlanningObjectFactories.h>
-int main(int argc, char* argv[])
+int
+main(int argc, char* argv[])
{
- return armarx::runSimpleComponentApp<armarx::RemoteObjectNode>(argc, argv, "MotionPlanningRemoteObjectNode");
+ return armarx::runSimpleComponentApp<armarx::RemoteObjectNode>(
+ argc, argv, "MotionPlanningRemoteObjectNode");
}
diff --git a/source/RobotComponents/applications/MotionPlanningServer/main.cpp b/source/RobotComponents/applications/MotionPlanningServer/main.cpp
index 4166582984158f24b5d3da275698908240508916..e50fe6bbbe9cd1f83279cfe2702fd91faa804a2b 100644
--- a/source/RobotComponents/applications/MotionPlanningServer/main.cpp
+++ b/source/RobotComponents/applications/MotionPlanningServer/main.cpp
@@ -23,11 +23,13 @@
*/
#include <ArmarXCore/core/application/Application.h>
-#include <RobotComponents/components/MotionPlanning/MotionPlanningServer.h>
#include <RobotComponents/components/MotionPlanning/MotionPlanningObjectFactories.h>
+#include <RobotComponents/components/MotionPlanning/MotionPlanningServer.h>
-int main(int argc, char* argv[])
+int
+main(int argc, char* argv[])
{
- return armarx::runSimpleComponentApp<armarx::MotionPlanningServer>(argc, argv, "MotionPlanningServer");
+ return armarx::runSimpleComponentApp<armarx::MotionPlanningServer>(
+ argc, argv, "MotionPlanningServer");
}
diff --git a/source/RobotComponents/applications/ObjectLocalizationSaliency/main.cpp b/source/RobotComponents/applications/ObjectLocalizationSaliency/main.cpp
index ea815d5cba32d0cbbfc63bdc4466bb3df0cff7c1..656f7fea211f94f12320f20fb75596a7a03073e5 100644
--- a/source/RobotComponents/applications/ObjectLocalizationSaliency/main.cpp
+++ b/source/RobotComponents/applications/ObjectLocalizationSaliency/main.cpp
@@ -20,13 +20,15 @@
* GNU General Public License
*/
-#include <RobotComponents/components/ObjectLocalizationSaliency/ObjectLocalizationSaliency.h>
-
-#include <ArmarXCore/core/application/Application.h>
#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/application/Application.h>
#include <ArmarXCore/core/logging/Logging.h>
-int main(int argc, char* argv[])
+#include <RobotComponents/components/ObjectLocalizationSaliency/ObjectLocalizationSaliency.h>
+
+int
+main(int argc, char* argv[])
{
- return armarx::runSimpleComponentApp < armarx::ObjectLocalizationSaliency > (argc, argv, "ObjectLocalizationSaliency");
+ return armarx::runSimpleComponentApp<armarx::ObjectLocalizationSaliency>(
+ argc, argv, "ObjectLocalizationSaliency");
}
diff --git a/source/RobotComponents/applications/PlannedMotionProvider/main.cpp b/source/RobotComponents/applications/PlannedMotionProvider/main.cpp
index 3651d767fb2351269f5887f3f58df1bb4833594a..49caafb25aa875e7d637e0ea29e9b7b39e6ed1e6 100644
--- a/source/RobotComponents/applications/PlannedMotionProvider/main.cpp
+++ b/source/RobotComponents/applications/PlannedMotionProvider/main.cpp
@@ -20,13 +20,15 @@
* GNU General Public License
*/
-#include <RobotComponents/components/PlannedMotionProvider/PlannedMotionProvider.h>
-
-#include <ArmarXCore/core/application/Application.h>
#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/application/Application.h>
#include <ArmarXCore/core/logging/Logging.h>
-int main(int argc, char* argv[])
+#include <RobotComponents/components/PlannedMotionProvider/PlannedMotionProvider.h>
+
+int
+main(int argc, char* argv[])
{
- return armarx::runSimpleComponentApp < armarx::PlannedMotionProvider > (argc, argv, "PlannedMotionProvider");
+ return armarx::runSimpleComponentApp<armarx::PlannedMotionProvider>(
+ argc, argv, "PlannedMotionProvider");
}
diff --git a/source/RobotComponents/applications/ReflexCombination/main.cpp b/source/RobotComponents/applications/ReflexCombination/main.cpp
index 0b06f83f1c62cbd303c3d799361de071933fd1c6..333218cb6576c59116b0f4a21a734bdd5bed6bcc 100644
--- a/source/RobotComponents/applications/ReflexCombination/main.cpp
+++ b/source/RobotComponents/applications/ReflexCombination/main.cpp
@@ -20,13 +20,15 @@
* GNU General Public License
*/
-#include <RobotComponents/components/ReflexCombination/ReflexCombination.h>
-
-#include <ArmarXCore/core/application/Application.h>
#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/application/Application.h>
#include <ArmarXCore/core/logging/Logging.h>
-int main(int argc, char* argv[])
+#include <RobotComponents/components/ReflexCombination/ReflexCombination.h>
+
+int
+main(int argc, char* argv[])
{
- return armarx::runSimpleComponentApp<armarx::ReflexCombination>(argc, argv, "ReflexCombination");
+ return armarx::runSimpleComponentApp<armarx::ReflexCombination>(
+ argc, argv, "ReflexCombination");
}
diff --git a/source/RobotComponents/applications/RobotIK/main.cpp b/source/RobotComponents/applications/RobotIK/main.cpp
index 57f5788ebf2e92f59f6a815cdd471e6a5e6989f6..ecbff7e7b9e4803be3a91ca7ccbac719891b8b43 100644
--- a/source/RobotComponents/applications/RobotIK/main.cpp
+++ b/source/RobotComponents/applications/RobotIK/main.cpp
@@ -22,13 +22,14 @@
* GNU General Public License
*/
-#include <RobotComponents/components/RobotIK/RobotIK.h>
-
-#include <ArmarXCore/core/application/Application.h>
#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/application/Application.h>
#include <ArmarXCore/core/logging/Logging.h>
-int main(int argc, char* argv[])
+#include <RobotComponents/components/RobotIK/RobotIK.h>
+
+int
+main(int argc, char* argv[])
{
return armarx::runSimpleComponentApp<armarx::RobotIK>(argc, argv, "RobotIK");
}
diff --git a/source/RobotComponents/applications/SimpleEpisodicMemoryKinematicUnitConnector/main.cpp b/source/RobotComponents/applications/SimpleEpisodicMemoryKinematicUnitConnector/main.cpp
index ab0f9c69924eb1051a00c08d936f23bc3c6fd006..dde433cf2ff014e552e10d0572fc9d0cd5c7d6f1 100644
--- a/source/RobotComponents/applications/SimpleEpisodicMemoryKinematicUnitConnector/main.cpp
+++ b/source/RobotComponents/applications/SimpleEpisodicMemoryKinematicUnitConnector/main.cpp
@@ -20,13 +20,15 @@
* GNU General Public License
*/
-#include <RobotComponents/components/SimpleEpisodicMemoryKinematicUnitConnector/SimpleEpisodicMemoryKinematicUnitConnector.h>
-
-#include <ArmarXCore/core/application/Application.h>
#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/application/Application.h>
#include <ArmarXCore/core/logging/Logging.h>
-int main(int argc, char* argv[])
+#include <RobotComponents/components/SimpleEpisodicMemoryKinematicUnitConnector/SimpleEpisodicMemoryKinematicUnitConnector.h>
+
+int
+main(int argc, char* argv[])
{
- return armarx::runSimpleComponentApp < armarx::SimpleEpisodicMemoryKinematicUnitConnector > (argc, argv, "SimpleEpisodicMemoryKinematicUnitConnector");
+ return armarx::runSimpleComponentApp<armarx::SimpleEpisodicMemoryKinematicUnitConnector>(
+ argc, argv, "SimpleEpisodicMemoryKinematicUnitConnector");
}
diff --git a/source/RobotComponents/applications/SimpleEpisodicMemoryPlatformUnitConnector/main.cpp b/source/RobotComponents/applications/SimpleEpisodicMemoryPlatformUnitConnector/main.cpp
index 2d2992f44f14c589b46ce22fe1874e4c31ddfbfc..dd12d0fe40f5ee15501a3ac82d5eba8414cac6ca 100644
--- a/source/RobotComponents/applications/SimpleEpisodicMemoryPlatformUnitConnector/main.cpp
+++ b/source/RobotComponents/applications/SimpleEpisodicMemoryPlatformUnitConnector/main.cpp
@@ -20,13 +20,15 @@
* GNU General Public License
*/
-#include <RobotComponents/components/SimpleEpisodicMemoryPlatformUnitConnector/SimpleEpisodicMemoryPlatformUnitConnector.h>
-
-#include <ArmarXCore/core/application/Application.h>
#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/application/Application.h>
#include <ArmarXCore/core/logging/Logging.h>
-int main(int argc, char* argv[])
+#include <RobotComponents/components/SimpleEpisodicMemoryPlatformUnitConnector/SimpleEpisodicMemoryPlatformUnitConnector.h>
+
+int
+main(int argc, char* argv[])
{
- return armarx::runSimpleComponentApp < armarx::SimpleEpisodicMemoryPlatformUnitConnector > (argc, argv, "SimpleEpisodicMemoryPlatformUnitConnector");
+ return armarx::runSimpleComponentApp<armarx::SimpleEpisodicMemoryPlatformUnitConnector>(
+ argc, argv, "SimpleEpisodicMemoryPlatformUnitConnector");
}
diff --git a/source/RobotComponents/applications/SimpleGraspGenerator/SimpleGraspGeneratorApp.h b/source/RobotComponents/applications/SimpleGraspGenerator/SimpleGraspGeneratorApp.h
index 4d30111c45ab2a6ae519bab23e3e397e6563cb98..7532b304bf11b712932d6575457cb2a1b2f18b28 100644
--- a/source/RobotComponents/applications/SimpleGraspGenerator/SimpleGraspGeneratorApp.h
+++ b/source/RobotComponents/applications/SimpleGraspGenerator/SimpleGraspGeneratorApp.h
@@ -26,6 +26,7 @@
#include <ArmarXCore/core/application/Application.h>
+
#include <RobotComponents/components/SimpleGraspGenerator/SimpleGraspGenerator.h>
namespace armarx
@@ -36,17 +37,16 @@ namespace armarx
*
* Detailed Description
*/
- class SimpleGraspGeneratorApp :
- virtual public armarx::Application
+ class SimpleGraspGeneratorApp : virtual public armarx::Application
{
/**
* @see armarx::Application::setup()
*/
- void setup(const ManagedIceObjectRegistryInterfacePtr& registry,
- Ice::PropertiesPtr properties) override
+ void
+ setup(const ManagedIceObjectRegistryInterfacePtr& registry,
+ Ice::PropertiesPtr properties) override
{
registry->addObject(Component::create<SimpleGraspGenerator>(properties));
}
};
-}
-
+} // namespace armarx
diff --git a/source/RobotComponents/applications/SimpleGraspGenerator/main.cpp b/source/RobotComponents/applications/SimpleGraspGenerator/main.cpp
index 576959eb257409ae42da8d1c644a0ec3a627a535..d5c5db0abbaae92cb3511e919f5a295cbda88db6 100644
--- a/source/RobotComponents/applications/SimpleGraspGenerator/main.cpp
+++ b/source/RobotComponents/applications/SimpleGraspGenerator/main.cpp
@@ -22,12 +22,15 @@
* GNU General Public License
*/
-#include "SimpleGraspGeneratorApp.h"
#include <ArmarXCore/core/logging/Logging.h>
-int main(int argc, char* argv[])
+#include "SimpleGraspGeneratorApp.h"
+
+int
+main(int argc, char* argv[])
{
- armarx::ApplicationPtr app = armarx::Application::createInstance < armarx::SimpleGraspGeneratorApp > ();
+ armarx::ApplicationPtr app =
+ armarx::Application::createInstance<armarx::SimpleGraspGeneratorApp>();
app->setName("SimpleGraspGenerator");
return app->main(argc, argv);
diff --git a/source/RobotComponents/applications/SimpleRobotPlacement/SimpleRobotPlacementApp.h b/source/RobotComponents/applications/SimpleRobotPlacement/SimpleRobotPlacementApp.h
index c69d9d146f111d7d27060c8b737e00409eba5896..f62450c799e71bc4751f9c7c550210e6d3a15147 100644
--- a/source/RobotComponents/applications/SimpleRobotPlacement/SimpleRobotPlacementApp.h
+++ b/source/RobotComponents/applications/SimpleRobotPlacement/SimpleRobotPlacementApp.h
@@ -26,6 +26,7 @@
#include <ArmarXCore/core/application/Application.h>
+
#include <RobotComponents/components/SimpleRobotPlacement/SimpleRobotPlacement.h>
namespace armarx
@@ -36,17 +37,16 @@ namespace armarx
*
* Detailed Description
*/
- class SimpleRobotPlacementApp :
- virtual public armarx::Application
+ class SimpleRobotPlacementApp : virtual public armarx::Application
{
/**
* @see armarx::Application::setup()
*/
- void setup(const ManagedIceObjectRegistryInterfacePtr& registry,
- Ice::PropertiesPtr properties) override
+ void
+ setup(const ManagedIceObjectRegistryInterfacePtr& registry,
+ Ice::PropertiesPtr properties) override
{
registry->addObject(Component::create<SimpleRobotPlacement>(properties));
}
};
-}
-
+} // namespace armarx
diff --git a/source/RobotComponents/applications/SimpleRobotPlacement/main.cpp b/source/RobotComponents/applications/SimpleRobotPlacement/main.cpp
index 2f14690538d4b1fda2307ab4577efe0ba39f12ae..66aeca565b757d169c28edbeab23d3d3378d737d 100644
--- a/source/RobotComponents/applications/SimpleRobotPlacement/main.cpp
+++ b/source/RobotComponents/applications/SimpleRobotPlacement/main.cpp
@@ -22,12 +22,15 @@
* GNU General Public License
*/
-#include "SimpleRobotPlacementApp.h"
#include <ArmarXCore/core/logging/Logging.h>
-int main(int argc, char* argv[])
+#include "SimpleRobotPlacementApp.h"
+
+int
+main(int argc, char* argv[])
{
- armarx::ApplicationPtr app = armarx::Application::createInstance < armarx::SimpleRobotPlacementApp > ();
+ armarx::ApplicationPtr app =
+ armarx::Application::createInstance<armarx::SimpleRobotPlacementApp>();
app->setName("SimpleRobotPlacement");
return app->main(argc, argv);
diff --git a/source/RobotComponents/applications/StaticAgentReporter/main.cpp b/source/RobotComponents/applications/StaticAgentReporter/main.cpp
index fc36bb15c8304e18347d3e9dd095c8190c7c5788..a86250885ffcde97e13a465fe66af46c3424fac3 100644
--- a/source/RobotComponents/applications/StaticAgentReporter/main.cpp
+++ b/source/RobotComponents/applications/StaticAgentReporter/main.cpp
@@ -20,13 +20,15 @@
* GNU General Public License
*/
-#include <RobotComponents/components/StaticAgentReporter/StaticAgentReporter.h>
-
-#include <ArmarXCore/core/application/Application.h>
#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/application/Application.h>
#include <ArmarXCore/core/logging/Logging.h>
-int main(int argc, char* argv[])
+#include <RobotComponents/components/StaticAgentReporter/StaticAgentReporter.h>
+
+int
+main(int argc, char* argv[])
{
- return armarx::runSimpleComponentApp < armarx::StaticAgentReporter > (argc, argv, "StaticAgentReporter");
+ return armarx::runSimpleComponentApp<armarx::StaticAgentReporter>(
+ argc, argv, "StaticAgentReporter");
}
diff --git a/source/RobotComponents/applications/TrajectoryPlayer/main.cpp b/source/RobotComponents/applications/TrajectoryPlayer/main.cpp
index 440b3a2c1ade335ec1dc3489bc50d1d43b7a12d9..6cd8c21ef40d1befa7b4d40ca13ec8e66456d891 100644
--- a/source/RobotComponents/applications/TrajectoryPlayer/main.cpp
+++ b/source/RobotComponents/applications/TrajectoryPlayer/main.cpp
@@ -20,13 +20,14 @@
* GNU General Public License
*/
-#include <RobotComponents/components/TrajectoryPlayer/TrajectoryPlayer.h>
-
-#include <ArmarXCore/core/application/Application.h>
#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/application/Application.h>
#include <ArmarXCore/core/logging/Logging.h>
-int main(int argc, char* argv[])
+#include <RobotComponents/components/TrajectoryPlayer/TrajectoryPlayer.h>
+
+int
+main(int argc, char* argv[])
{
- return armarx::runSimpleComponentApp < armarx::TrajectoryPlayer > (argc, argv, "TrajectoryPlayer");
+ return armarx::runSimpleComponentApp<armarx::TrajectoryPlayer>(argc, argv, "TrajectoryPlayer");
}
diff --git a/source/RobotComponents/applications/ViconMarkerProvider/main.cpp b/source/RobotComponents/applications/ViconMarkerProvider/main.cpp
index eef0908d4e847d7fc468f865548f4dfe5703d33d..132ea12f17af462886a19456d6c8f3460aa14cff 100644
--- a/source/RobotComponents/applications/ViconMarkerProvider/main.cpp
+++ b/source/RobotComponents/applications/ViconMarkerProvider/main.cpp
@@ -20,13 +20,15 @@
* GNU General Public License
*/
-#include <RobotComponents/components/ViconMarkerProvider/ViconMarkerProvider.h>
-
-#include <ArmarXCore/core/application/Application.h>
#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/application/Application.h>
#include <ArmarXCore/core/logging/Logging.h>
-int main(int argc, char* argv[])
+#include <RobotComponents/components/ViconMarkerProvider/ViconMarkerProvider.h>
+
+int
+main(int argc, char* argv[])
{
- return armarx::runSimpleComponentApp < armarx::ViconMarkerProvider > (argc, argv, "ViconMarkerProvider");
+ return armarx::runSimpleComponentApp<armarx::ViconMarkerProvider>(
+ argc, argv, "ViconMarkerProvider");
}
diff --git a/source/RobotComponents/applications/tests/AStarPathPlannerTest/AStarPathPlannerTestComponent.cpp b/source/RobotComponents/applications/tests/AStarPathPlannerTest/AStarPathPlannerTestComponent.cpp
index ac9704e83950064960dc7117f01bbf1a8a0003ff..125c2167b2f9cbf459edae2c54260aa04f3c576a 100644
--- a/source/RobotComponents/applications/tests/AStarPathPlannerTest/AStarPathPlannerTestComponent.cpp
+++ b/source/RobotComponents/applications/tests/AStarPathPlannerTest/AStarPathPlannerTestComponent.cpp
@@ -20,54 +20,60 @@
* GNU General Public License
*/
-#include <chrono>
-
#include "AStarPathPlannerTestComponent.h"
-#include <MemoryX/libraries/memorytypes/entity/ObjectClass.h>
+
+#include <chrono>
#include <MemoryX/core/MemoryXCoreObjectFactories.h>
#include <MemoryX/libraries/memorytypes/MemoryXTypesObjectFactories.h>
+#include <MemoryX/libraries/memorytypes/entity/ObjectClass.h>
#include <MemoryX/libraries/memorytypes/entity/ObjectInstance.h>
#include <MemoryX/libraries/updater/ObjectLocalization/MemoryXUpdaterObjectFactories.h>
-static const std::string layerName
-{
- "AStarPathPlannerTestLayer"
-};
+static const std::string layerName{"AStarPathPlannerTestLayer"};
-void armarx::AStarPathPlannerTestComponent::onInitComponent()
+void
+armarx::AStarPathPlannerTestComponent::onInitComponent()
{
usingProxy(getProperty<std::string>("WorkingMemoryName").getValue());
offeringTopic(getProperty<std::string>("DebugDrawerName").getValue());
usingProxy(getProperty<std::string>("AStarPathPlannerName").getValue());
}
-void armarx::AStarPathPlannerTestComponent::onConnectComponent()
+void
+armarx::AStarPathPlannerTestComponent::onConnectComponent()
{
- workingMemoryPrx = getProxy<memoryx::WorkingMemoryInterfacePrx>(getProperty<std::string>("WorkingMemoryName").getValue());
- debugDrawerPrx = getTopic<armarx::DebugDrawerInterfacePrx>(getProperty<std::string>("DebugDrawerName").getValue());
- aStarPathPlannerPrx = getProxy<armarx::AStarPathPlannerBasePrx>(getProperty<std::string>("AStarPathPlannerName").getValue());
+ workingMemoryPrx = getProxy<memoryx::WorkingMemoryInterfacePrx>(
+ getProperty<std::string>("WorkingMemoryName").getValue());
+ debugDrawerPrx = getTopic<armarx::DebugDrawerInterfacePrx>(
+ getProperty<std::string>("DebugDrawerName").getValue());
+ aStarPathPlannerPrx = getProxy<armarx::AStarPathPlannerBasePrx>(
+ getProperty<std::string>("AStarPathPlannerName").getValue());
//pass all objects from the scene to the planner
auto objInstPrx = workingMemoryPrx->getObjectInstancesSegment();
const auto objIds = objInstPrx->getAllEntityIds();
- armarx::ObjectPositionBaseList objects {};
+ armarx::ObjectPositionBaseList objects{};
for (const auto& id : objIds)
{
const memoryx::EntityBasePtr entityBase = objInstPrx->getEntityById(id);
- const memoryx::ObjectInstanceBasePtr object = memoryx::ObjectInstancePtr::dynamicCast(entityBase);
+ const memoryx::ObjectInstanceBasePtr object =
+ memoryx::ObjectInstancePtr::dynamicCast(entityBase);
assert(object);
const std::string className = object->getMostProbableClass();
- memoryx::ObjectClassList classes = workingMemoryPrx->getPriorKnowledge()->getObjectClassesSegment()->getClassWithSubclasses(className);
+ memoryx::ObjectClassList classes = workingMemoryPrx->getPriorKnowledge()
+ ->getObjectClassesSegment()
+ ->getClassWithSubclasses(className);
if (!classes.size())
{
- ARMARX_INFO << "No classes for most probable class '" << className << "' of object '" << object->getName() << "' with id " << id;
+ ARMARX_INFO << "No classes for most probable class '" << className << "' of object '"
+ << object->getName() << "' with id " << id;
continue;
}
@@ -75,11 +81,13 @@ void armarx::AStarPathPlannerTestComponent::onConnectComponent()
obj.objectClassBase = classes.at(0);
- obj.objectPose = ::armarx::PoseBasePtr {new Pose{object->getPositionBase(), object->getOrientationBase()}};
+ obj.objectPose = ::armarx::PoseBasePtr{
+ new Pose{object->getPositionBase(), object->getOrientationBase()}};
objects.push_back(obj);
- ARMARX_VERBOSE << "Added class '" << className << "' of object '" << object->getName() << "' with id " << id;
+ ARMARX_VERBOSE << "Added class '" << className << "' of object '" << object->getName()
+ << "' with id " << id;
}
ARMARX_INFO << "using " << objects.size() << "objects";
@@ -103,22 +111,22 @@ void armarx::AStarPathPlannerTestComponent::onConnectComponent()
memoryx::AgentInstanceBasePtr agent = agSegPrx->getAgentInstanceById(agent0Id);
- std::string agentCollisionModelName {"Platform"};
+ std::string agentCollisionModelName{"Platform"};
aStarPathPlannerPrx->setAgent(agent, agentCollisionModelName);
aStarPathPlannerPrx->setSafetyMargin(0);
//plan the path
- armarx::Vector3BasePtr from {new armarx::Vector3{1900.f, 2300.f, 0.f}};
+ armarx::Vector3BasePtr from{new armarx::Vector3{1900.f, 2300.f, 0.f}};
//armarx::Vector3BasePtr to {new armarx::Vector3{2500.f, 7000.f, 1.f}};
- armarx::Vector3BasePtr to {new armarx::Vector3{2600.f, 9500.f, 1.f}};
+ armarx::Vector3BasePtr to{new armarx::Vector3{2600.f, 9500.f, 1.f}};
ARMARX_INFO << "Starting path planning";
Vector3BaseList path;
std::size_t iterations = 25;
- std::chrono::high_resolution_clock::duration duration {0};
+ std::chrono::high_resolution_clock::duration duration{0};
for (std::size_t i = 1; i <= iterations; ++i)
{
@@ -128,7 +136,9 @@ void armarx::AStarPathPlannerTestComponent::onConnectComponent()
const auto tEnd = std::chrono::high_resolution_clock::now();
duration += (tEnd - tStart);
}
- ARMARX_INFO << "Path planning done (avg). T[ms] = " << std::chrono::duration_cast<std::chrono::milliseconds>(duration / iterations).count();
+ ARMARX_INFO
+ << "Path planning done (avg). T[ms] = "
+ << std::chrono::duration_cast<std::chrono::milliseconds>(duration / iterations).count();
if (!path.size())
{
@@ -138,14 +148,15 @@ void armarx::AStarPathPlannerTestComponent::onConnectComponent()
//draw the path
- std::stringstream lineName {};
- DrawColor color {1.f, 0.f, 0.f, 1.f};
+ std::stringstream lineName{};
+ DrawColor color{1.f, 0.f, 0.f, 1.f};
for (std::size_t i = 0; i < path.size() - 1; ++i)
{
lineName.str(layerName);
lineName << "from_" << i << "_to_" << i + 1;
- debugDrawerPrx->setLineVisu(layerName, lineName.str(), path.at(i), path.at(i + 1), 5.f, color);
+ debugDrawerPrx->setLineVisu(
+ layerName, lineName.str(), path.at(i), path.at(i + 1), 5.f, color);
//change color to make the first edge distinct
if (!i)
@@ -158,8 +169,8 @@ void armarx::AStarPathPlannerTestComponent::onConnectComponent()
ARMARX_INFO << "Found path whith " << path.size() - 1 << " edges";
}
-
-void armarx::AStarPathPlannerTestComponent::onExitComponent()
+void
+armarx::AStarPathPlannerTestComponent::onExitComponent()
{
debugDrawerPrx->clearLayer(layerName);
debugDrawerPrx->clearDebugLayer();
diff --git a/source/RobotComponents/applications/tests/AStarPathPlannerTest/AStarPathPlannerTestComponent.h b/source/RobotComponents/applications/tests/AStarPathPlannerTest/AStarPathPlannerTestComponent.h
index 4a74125440a87bd1a43c789cd971edc83ec78a02..157fecaf94bd38ec814078c52c458bba79aeef9e 100644
--- a/source/RobotComponents/applications/tests/AStarPathPlannerTest/AStarPathPlannerTestComponent.h
+++ b/source/RobotComponents/applications/tests/AStarPathPlannerTest/AStarPathPlannerTestComponent.h
@@ -24,41 +24,44 @@
#include <ArmarXCore/core/Component.h>
-#include <MemoryX/interface/components/WorkingMemoryInterface.h>
#include <RobotAPI/components/DebugDrawer/DebugDrawerComponent.h>
+
#include <RobotComponents/interface/components/PathPlanner.h>
+#include <MemoryX/interface/components/WorkingMemoryInterface.h>
+
namespace armarx
{
- class AStarPathPlannerTestComponentPropertyDefinitions:
+ class AStarPathPlannerTestComponentPropertyDefinitions :
public armarx::ComponentPropertyDefinitions
{
public:
- AStarPathPlannerTestComponentPropertyDefinitions(std::string prefix):
+ AStarPathPlannerTestComponentPropertyDefinitions(std::string prefix) :
ComponentPropertyDefinitions(prefix)
{
- defineOptionalProperty<std::string>("WorkingMemoryName", "WorkingMemory", "Name of WorkingMemory component");
- defineOptionalProperty<std::string>("DebugDrawerName", "DebugDrawerUpdates", "Name of DebugDrawer component");
- defineOptionalProperty<std::string>("AStarPathPlannerName", "AStarPathPlanner", "Name of AStarPathPlanner component");
+ defineOptionalProperty<std::string>(
+ "WorkingMemoryName", "WorkingMemory", "Name of WorkingMemory component");
+ defineOptionalProperty<std::string>(
+ "DebugDrawerName", "DebugDrawerUpdates", "Name of DebugDrawer component");
+ defineOptionalProperty<std::string>(
+ "AStarPathPlannerName", "AStarPathPlanner", "Name of AStarPathPlanner component");
}
};
/**
* @brief A test component for the AStarPathPlanner
*/
- class AStarPathPlannerTestComponent:
- virtual public Component
+ class AStarPathPlannerTestComponent : virtual public Component
{
public:
/**
* @see PropertyUser::createPropertyDefinitions()
*/
- armarx::PropertyDefinitionsPtr createPropertyDefinitions() override
+ armarx::PropertyDefinitionsPtr
+ createPropertyDefinitions() override
{
- return armarx::PropertyDefinitionsPtr
- {
- new AStarPathPlannerTestComponentPropertyDefinitions{getConfigIdentifier()}
- };
+ return armarx::PropertyDefinitionsPtr{
+ new AStarPathPlannerTestComponentPropertyDefinitions{getConfigIdentifier()}};
}
/**
@@ -76,14 +79,15 @@ namespace armarx
/**
* @see armarx::ManagedIceObject::getDefaultName()
*/
- std::string getDefaultName() const override
+ std::string
+ getDefaultName() const override
{
return "AStarPathPlannerTestComponent";
}
+
private:
memoryx::WorkingMemoryInterfacePrx workingMemoryPrx;
armarx::DebugDrawerInterfacePrx debugDrawerPrx;
armarx::AStarPathPlannerBasePrx aStarPathPlannerPrx;
};
-}
-
+} // namespace armarx
diff --git a/source/RobotComponents/applications/tests/AStarPathPlannerTest/main.cpp b/source/RobotComponents/applications/tests/AStarPathPlannerTest/main.cpp
index 7bc78476e94059c7be84428e5a7851c0570ac27c..edaf3772c411c527f9e4aa4c9129603a5aabb7b5 100644
--- a/source/RobotComponents/applications/tests/AStarPathPlannerTest/main.cpp
+++ b/source/RobotComponents/applications/tests/AStarPathPlannerTest/main.cpp
@@ -20,13 +20,15 @@
* GNU General Public License
*/
-#include "AStarPathPlannerTestComponent.h"
-
-#include <ArmarXCore/core/application/Application.h>
#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/application/Application.h>
#include <ArmarXCore/core/logging/Logging.h>
-int main(int argc, char* argv[])
+#include "AStarPathPlannerTestComponent.h"
+
+int
+main(int argc, char* argv[])
{
- return armarx::runSimpleComponentApp<armarx::AStarPathPlannerTestComponent>(argc, argv, "AStarPathPlannerTestApp");
+ return armarx::runSimpleComponentApp<armarx::AStarPathPlannerTestComponent>(
+ argc, argv, "AStarPathPlannerTestApp");
}
diff --git a/source/RobotComponents/applications/tests/GraspingManagerTest/GraspingManagerTestApp.h b/source/RobotComponents/applications/tests/GraspingManagerTest/GraspingManagerTestApp.h
index 3e6a433d6bef283ca8249606293d6c3228659ef5..eb3fad9b177964225305e02a8560591c81c1039e 100644
--- a/source/RobotComponents/applications/tests/GraspingManagerTest/GraspingManagerTestApp.h
+++ b/source/RobotComponents/applications/tests/GraspingManagerTest/GraspingManagerTestApp.h
@@ -26,25 +26,28 @@
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/ManagedIceObject.h>
+#include <ArmarXCore/util/json/JSONObject.h>
+
+#include <RobotAPI/libraries/core/LinkedPose.h>
+
+#include <RobotComponents/components/GraspingManager/GraspingManager.h>
+
#include <MemoryX/core/MemoryXCoreObjectFactories.h>
-#include <MemoryX/libraries/memorytypes/MemoryXTypesObjectFactories.h>
-#include <MemoryX/interface/components/WorkingMemoryInterface.h>
#include <MemoryX/interface/components/PriorKnowledgeInterface.h>
-#include <RobotComponents/components/GraspingManager/GraspingManager.h>
-#include <RobotAPI/libraries/core/LinkedPose.h>
+#include <MemoryX/interface/components/WorkingMemoryInterface.h>
+#include <MemoryX/libraries/memorytypes/MemoryXTypesObjectFactories.h>
#include <MemoryX/libraries/motionmodels/MotionModelStaticObject.h>
-#include <ArmarXCore/util/json/JSONObject.h>
namespace armarx
{
- class GraspingManagerTestPropertyDefinitions:
- public armarx::ComponentPropertyDefinitions
+ class GraspingManagerTestPropertyDefinitions : public armarx::ComponentPropertyDefinitions
{
public:
- GraspingManagerTestPropertyDefinitions(std::string prefix):
+ GraspingManagerTestPropertyDefinitions(std::string prefix) :
ComponentPropertyDefinitions(prefix)
{
- defineOptionalProperty<std::string>("objectToGrasp", "", "the object for which the grasp should be planned");
+ defineOptionalProperty<std::string>(
+ "objectToGrasp", "", "the object for which the grasp should be planned");
}
};
@@ -62,22 +65,25 @@ namespace armarx
/**
* @see armarx::ManagedIceObject::getDefaultName()
*/
- std::string getDefaultName() const override
+ std::string
+ getDefaultName() const override
{
return "GraspingManagerTest";
}
- armarx::PropertyDefinitionsPtr createPropertyDefinitions() override
+ armarx::PropertyDefinitionsPtr
+ createPropertyDefinitions() override
{
- return armarx::PropertyDefinitionsPtr(new GraspingManagerTestPropertyDefinitions(
- getConfigIdentifier()));
+ return armarx::PropertyDefinitionsPtr(
+ new GraspingManagerTestPropertyDefinitions(getConfigIdentifier()));
}
protected:
/**
* @see armarx::ManagedIceObject::onInitComponent()
*/
- void onInitComponent() override
+ void
+ onInitComponent() override
{
usingProxy("GraspingManager");
usingProxy("WorkingMemory");
@@ -89,11 +95,13 @@ namespace armarx
/**
* @see armarx::ManagedIceObject::onConnectComponent()
*/
- void onConnectComponent() override
+ void
+ onConnectComponent() override
{
std::string objectName = getProperty<std::string>("objectToGrasp").getValue();
wm = getProxy<memoryx::WorkingMemoryInterfacePrx>("WorkingMemory");
- RobotStateComponentInterfacePrx robot = getProxy<RobotStateComponentInterfacePrx>("RobotStateComponent");
+ RobotStateComponentInterfacePrx robot =
+ getProxy<RobotStateComponentInterfacePrx>("RobotStateComponent");
while (true)
{
auto object = wm->getObjectInstancesSegment()->getEntityByName(objectName);
@@ -159,17 +167,17 @@ namespace armarx
/**
* @see armarx::ManagedIceObject::onDisconnectComponent()
*/
- void onDisconnectComponent() override
+ void
+ onDisconnectComponent() override
{
-
}
/**
* @see armarx::ManagedIceObject::onExitComponent()
*/
- void onExitComponent() override
+ void
+ onExitComponent() override
{
-
}
private:
@@ -177,5 +185,4 @@ namespace armarx
memoryx::WorkingMemoryInterfacePrx wm;
GraspingManagerInterfacePrx gm;
};
-}
-
+} // namespace armarx
diff --git a/source/RobotComponents/applications/tests/GraspingManagerTest/main.cpp b/source/RobotComponents/applications/tests/GraspingManagerTest/main.cpp
index 05b124e0235eb0e5feb219f6e5aefe80ba1dc078..52ce6fabb5f446558fa3a495b719cf51d72e6008 100644
--- a/source/RobotComponents/applications/tests/GraspingManagerTest/main.cpp
+++ b/source/RobotComponents/applications/tests/GraspingManagerTest/main.cpp
@@ -22,13 +22,15 @@
* GNU General Public License
*/
-#include "GraspingManagerTestApp.h"
-
-#include <ArmarXCore/core/application/Application.h>
#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/application/Application.h>
#include <ArmarXCore/core/logging/Logging.h>
-int main(int argc, char* argv[])
+#include "GraspingManagerTestApp.h"
+
+int
+main(int argc, char* argv[])
{
- return armarx::runSimpleComponentApp<armarx::GraspingManagerTest>(argc, argv, "GraspingManagerTest");
+ return armarx::runSimpleComponentApp<armarx::GraspingManagerTest>(
+ argc, argv, "GraspingManagerTest");
}
diff --git a/source/RobotComponents/components/CollisionCheckerComponent/CollisionCheckerComponent.cpp b/source/RobotComponents/components/CollisionCheckerComponent/CollisionCheckerComponent.cpp
index c614d25ac8ecf766ba0802dde3ca436684a7f22f..253e3794cb06b7001685cf834fe78be08390f96c 100644
--- a/source/RobotComponents/components/CollisionCheckerComponent/CollisionCheckerComponent.cpp
+++ b/source/RobotComponents/components/CollisionCheckerComponent/CollisionCheckerComponent.cpp
@@ -23,37 +23,38 @@
#include "CollisionCheckerComponent.h"
#include <stdlib.h>
+
#include <cmath>
-#include <sstream>
#include <iterator>
+#include <sstream>
+
+#include <boost/regex.hpp>
+
+#include <SimoxUtility/algorithm/string/string_tools.h>
+#include <VirtualRobot/CollisionDetection/CollisionChecker.h>
+#include <VirtualRobot/RobotNodeSet.h>
+#include <VirtualRobot/SceneObjectSet.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
-#include <MemoryX/libraries/memorytypes/entity/AgentInstance.h>
-#include <MemoryX/libraries/memorytypes/entity/ObjectInstance.h>
-#include <MemoryX/libraries/memorytypes/entity/ObjectClass.h>
-#include <MemoryX/libraries/helpers/VirtualRobotHelpers/SimoxObjectWrapper.h>
-#include <MemoryX/libraries/memorytypes/MemoryXTypesObjectFactories.h>
-#include <MemoryX/core/MemoryXCoreObjectFactories.h>
-
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
-#include <VirtualRobot/CollisionDetection/CollisionChecker.h>
-
-#include <SimoxUtility/algorithm/string/string_tools.h>
-
-#include <boost/regex.hpp>
+#include <MemoryX/core/MemoryXCoreObjectFactories.h>
+#include <MemoryX/libraries/helpers/VirtualRobotHelpers/SimoxObjectWrapper.h>
+#include <MemoryX/libraries/memorytypes/MemoryXTypesObjectFactories.h>
+#include <MemoryX/libraries/memorytypes/entity/AgentInstance.h>
+#include <MemoryX/libraries/memorytypes/entity/ObjectClass.h>
+#include <MemoryX/libraries/memorytypes/entity/ObjectInstance.h>
using namespace armarx;
-CollisionCheckerComponent::CollisionCheckerComponent() :
- connected(false)
+CollisionCheckerComponent::CollisionCheckerComponent() : connected(false)
{
-
}
-void CollisionCheckerComponent::onInitComponent()
+void
+CollisionCheckerComponent::onInitComponent()
{
interval = getProperty<int>("interval").getValue();
@@ -61,43 +62,57 @@ void CollisionCheckerComponent::onInitComponent()
if (useWorkingMemory)
{
workingMemoryName = getProperty<std::string>("WorkingMemoryName").getValue();
- ARMARX_INFO << "Using WorkingMemory \"" << workingMemoryName << "\" to get SceneObjects." << std::endl;
+ ARMARX_INFO << "Using WorkingMemory \"" << workingMemoryName << "\" to get SceneObjects."
+ << std::endl;
usingProxy(workingMemoryName);
usingTopic(getProperty<std::string>("WorkingMemoryListenerTopicName").getValue());
}
else
{
robotStateComponentName = getProperty<std::string>("RobotStateComponentName").getValue();
- ARMARX_INFO << "Using RobotStateComponent \"" << robotStateComponentName << "\" to get SceneObjects." << std::endl;
+ ARMARX_INFO << "Using RobotStateComponent \"" << robotStateComponentName
+ << "\" to get SceneObjects." << std::endl;
usingProxy(robotStateComponentName);
}
std::string collisionPairsString = getProperty<std::string>("CollisionPairs").getValue();
- boost::regex regexCollisionPair("[\\[\\{\\(](([^:,\\[\\{\\(\\]\\}\\)]+:)?([^,\\[\\{\\(\\]\\}\\)]+,|[\\[\\{\\(]([^,\\[\\{\\(\\]\\}\\)]+,)*[^,\\[\\{\\(\\]\\}\\)]+[\\]\\}\\)],)){2}[0-9]*[.]?[0-9]*[\\]\\}\\)]");
+ boost::regex regexCollisionPair(
+ "[\\[\\{\\(](([^:,\\[\\{\\(\\]\\}\\)]+:)?([^,\\[\\{\\(\\]\\}\\)]+,|[\\[\\{\\(]([^,\\[\\{\\("
+ "\\]\\}\\)]+,)*[^,\\[\\{\\(\\]\\}\\)]+[\\]\\}\\)],)){2}[0-9]*[.]?[0-9]*[\\]\\}\\)]");
- boost::sregex_token_iterator iterCollisionPair(collisionPairsString.begin(), collisionPairsString.end(), regexCollisionPair, 0);
+ boost::sregex_token_iterator iterCollisionPair(
+ collisionPairsString.begin(), collisionPairsString.end(), regexCollisionPair, 0);
boost::sregex_token_iterator end;
for (; iterCollisionPair != end; ++iterCollisionPair)
{
std::string collisionPairString = *iterCollisionPair;
- while ((collisionPairString[0] == '{' && collisionPairString[collisionPairString.size() - 1] == '}') ||
- (collisionPairString[0] == '(' && collisionPairString[collisionPairString.size() - 1] == ')') ||
- (collisionPairString[0] == '[' && collisionPairString[collisionPairString.size() - 1] == ']'))
+ while ((collisionPairString[0] == '{' &&
+ collisionPairString[collisionPairString.size() - 1] == '}') ||
+ (collisionPairString[0] == '(' &&
+ collisionPairString[collisionPairString.size() - 1] == ')') ||
+ (collisionPairString[0] == '[' &&
+ collisionPairString[collisionPairString.size() - 1] == ']'))
{
collisionPairString = collisionPairString.substr(1, collisionPairString.size() - 2);
}
- if ((collisionPairString[0] == '{' || collisionPairString[0] == '(' || collisionPairString[0] == '[') &&
- (collisionPairString[collisionPairString.size() - 1] == '}' || collisionPairString[collisionPairString.size() - 1] == ')' || collisionPairString[collisionPairString.size() - 1] == ']'))
+ if ((collisionPairString[0] == '{' || collisionPairString[0] == '(' ||
+ collisionPairString[0] == '[') &&
+ (collisionPairString[collisionPairString.size() - 1] == '}' ||
+ collisionPairString[collisionPairString.size() - 1] == ')' ||
+ collisionPairString[collisionPairString.size() - 1] == ']'))
{
- ARMARX_ERROR << "Could not parse collision pair: " << *iterCollisionPair << ". Ignoring..." << std::endl;
+ ARMARX_ERROR << "Could not parse collision pair: " << *iterCollisionPair
+ << ". Ignoring..." << std::endl;
continue;
}
- boost::regex regex("([^:,\\[\\{\\(\\]\\}\\)]+:)?([\\{\\[\\(][^\\{\\[\\(\\]\\}\\)]+[\\]\\}\\)]|[^\\{\\[\\(\\]\\}\\),]+)");
+ boost::regex regex("([^:,\\[\\{\\(\\]\\}\\)]+:)?([\\{\\[\\(][^\\{\\[\\(\\]\\}\\)]+[\\]\\}"
+ "\\)]|[^\\{\\[\\(\\]\\}\\),]+)");
- boost::sregex_token_iterator iter(collisionPairString.begin(), collisionPairString.end(), regex, 0);
+ boost::sregex_token_iterator iter(
+ collisionPairString.begin(), collisionPairString.end(), regex, 0);
boost::sregex_token_iterator end;
ARMARX_CHECK_EXPRESSION(iter != end);
@@ -120,7 +135,15 @@ void CollisionCheckerComponent::onInitComponent()
++iter;
ARMARX_CHECK_EXPRESSION(iter != end);
- addCollisionPair(robotName1, nodeNames1, usesNodeSet1, nodeSetName1, robotName2, nodeNames2, usesNodeSet2, nodeSetName2, std::atof(((std::string)*iter).c_str()));
+ addCollisionPair(robotName1,
+ nodeNames1,
+ usesNodeSet1,
+ nodeSetName1,
+ robotName2,
+ nodeNames2,
+ usesNodeSet2,
+ nodeSetName2,
+ std::atof(((std::string)*iter).c_str()));
ARMARX_CHECK_EXPRESSION(++iter == end);
}
@@ -133,8 +156,8 @@ void CollisionCheckerComponent::onInitComponent()
useDebugDrawer = getProperty<bool>("UseDebugDrawer").getValue();
}
-
-void CollisionCheckerComponent::onConnectComponent()
+void
+CollisionCheckerComponent::onConnectComponent()
{
{
std::scoped_lock lockData(dataMutex);
@@ -149,17 +172,21 @@ void CollisionCheckerComponent::onConnectComponent()
fileManager.reset(new memoryx::GridFileManager(workingMemoryPrx->getCommonStorage()));
for (memoryx::AgentInstanceBasePtr& agent : agentInstancesPrx->getAllAgentInstances())
{
- RobotStateComponentInterfacePrx robotStateComponentPrx = agent->getSharedRobot()->getRobotStateComponent();
- VirtualRobot::RobotPtr robot = armarx::RemoteRobot::createLocalCloneFromFile(robotStateComponentPrx);
+ RobotStateComponentInterfacePrx robotStateComponentPrx =
+ agent->getSharedRobot()->getRobotStateComponent();
+ VirtualRobot::RobotPtr robot =
+ armarx::RemoteRobot::createLocalCloneFromFile(robotStateComponentPrx);
robots.push_back({robot, robotStateComponentPrx});
}
}
else
{
- RobotStateComponentInterfacePrx robotStateComponentPrx = getProxy<RobotStateComponentInterfacePrx>(robotStateComponentName);
+ RobotStateComponentInterfacePrx robotStateComponentPrx =
+ getProxy<RobotStateComponentInterfacePrx>(robotStateComponentName);
- VirtualRobot::RobotPtr robot = armarx::RemoteRobot::createLocalCloneFromFile(robotStateComponentPrx);
+ VirtualRobot::RobotPtr robot =
+ armarx::RemoteRobot::createLocalCloneFromFile(robotStateComponentPrx);
RobotPair r = {robot, robotStateComponentPrx};
robots.push_back(r);
@@ -176,30 +203,46 @@ void CollisionCheckerComponent::onConnectComponent()
if (!resolveCollisionPair(pair))
{
std::ostringstream nodeNames1Str;
- std::copy(pair.nodeNames1.begin(), pair.nodeNames1.end() - 1, std::ostream_iterator<std::string>(nodeNames1Str, ","));
+ std::copy(pair.nodeNames1.begin(),
+ pair.nodeNames1.end() - 1,
+ std::ostream_iterator<std::string>(nodeNames1Str, ","));
nodeNames1Str << pair.nodeNames1.back();
std::ostringstream nodeNames2Str;
- std::copy(pair.nodeNames2.begin(), pair.nodeNames2.end() - 1, std::ostream_iterator<std::string>(nodeNames2Str, ","));
+ std::copy(pair.nodeNames2.begin(),
+ pair.nodeNames2.end() - 1,
+ std::ostream_iterator<std::string>(nodeNames2Str, ","));
nodeNames2Str << pair.nodeNames2.back();
- ARMARX_WARNING << "Could not resolve collision pair: {" << pair.robotName1 << (pair.robotName1 == "" ? "" : ":") << "(" << nodeNames1Str.str() << ")," << pair.robotName2 << (pair.robotName2 == "" ? "" : ":") << "(" << nodeNames2Str.str() << ")," << pair.warningDistance << "}" << std::endl;
+ ARMARX_WARNING << "Could not resolve collision pair: {" << pair.robotName1
+ << (pair.robotName1 == "" ? "" : ":") << "(" << nodeNames1Str.str()
+ << ")," << pair.robotName2 << (pair.robotName2 == "" ? "" : ":")
+ << "(" << nodeNames2Str.str() << ")," << pair.warningDistance << "}"
+ << std::endl;
}
}
}
- collisionListenerPrx = getTopic<CollisionListenerPrx>(getProperty<std::string>("CollisionListenerTopicName").getValue());
- distanceListenerPrx = getTopic<DistanceListenerPrx>(getProperty<std::string>("DistanceListenerTopicName").getValue());
+ collisionListenerPrx = getTopic<CollisionListenerPrx>(
+ getProperty<std::string>("CollisionListenerTopicName").getValue());
+ distanceListenerPrx = getTopic<DistanceListenerPrx>(
+ getProperty<std::string>("DistanceListenerTopicName").getValue());
if (!reportTask)
{
- reportTask = new PeriodicTask<CollisionCheckerComponent>(this, &CollisionCheckerComponent::reportDistancesAndCollisions, interval, false, "ReportCollisionsTask");
+ reportTask = new PeriodicTask<CollisionCheckerComponent>(
+ this,
+ &CollisionCheckerComponent::reportDistancesAndCollisions,
+ interval,
+ false,
+ "ReportCollisionsTask");
}
reportTask->start();
if (useDebugDrawer)
{
- debugDrawerTopicPrx = getTopic<DebugDrawerInterfacePrx>(getProperty<std::string>("DebugDrawerTopicName").getValue());
+ debugDrawerTopicPrx = getTopic<DebugDrawerInterfacePrx>(
+ getProperty<std::string>("DebugDrawerTopicName").getValue());
if (!debugDrawerTopicPrx)
{
@@ -209,8 +252,8 @@ void CollisionCheckerComponent::onConnectComponent()
}
}
-
-void CollisionCheckerComponent::onDisconnectComponent()
+void
+CollisionCheckerComponent::onDisconnectComponent()
{
{
std::unique_lock lock(connectedMutex);
@@ -233,19 +276,20 @@ void CollisionCheckerComponent::onDisconnectComponent()
}
}
-
-void CollisionCheckerComponent::onExitComponent()
+void
+CollisionCheckerComponent::onExitComponent()
{
-
}
-armarx::PropertyDefinitionsPtr CollisionCheckerComponent::createPropertyDefinitions()
+armarx::PropertyDefinitionsPtr
+CollisionCheckerComponent::createPropertyDefinitions()
{
- return armarx::PropertyDefinitionsPtr(new CollisionCheckerComponentPropertyDefinitions(
- getConfigIdentifier()));
+ return armarx::PropertyDefinitionsPtr(
+ new CollisionCheckerComponentPropertyDefinitions(getConfigIdentifier()));
}
-bool armarx::CollisionCheckerComponent::resolveCollisionPair(SceneObjectPair& pair)
+bool
+armarx::CollisionCheckerComponent::resolveCollisionPair(SceneObjectPair& pair)
{
std::shared_lock lockConnected(connectedMutex);
if (!connected)
@@ -262,7 +306,8 @@ bool armarx::CollisionCheckerComponent::resolveCollisionPair(SceneObjectPair& pa
{
if (robotPair.robot->getName() == pair.robotName1)
{
- VirtualRobot::SceneObjectSetPtr set = robotPair.robot->getRobotNodeSet(pair.nodeSetName1);
+ VirtualRobot::SceneObjectSetPtr set =
+ robotPair.robot->getRobotNodeSet(pair.nodeSetName1);
if (set)
{
for (VirtualRobot::SceneObjectPtr so : set->getSceneObjects())
@@ -272,7 +317,9 @@ bool armarx::CollisionCheckerComponent::resolveCollisionPair(SceneObjectPair& pa
}
else
{
- ARMARX_WARNING << "Robot \"" << pair.robotName1 << "\" has no set with name \"" << pair.nodeSetName1 << "\"" << std::endl;
+ ARMARX_WARNING << "Robot \"" << pair.robotName1
+ << "\" has no set with name \"" << pair.nodeSetName1 << "\""
+ << std::endl;
}
break;
}
@@ -280,7 +327,8 @@ bool armarx::CollisionCheckerComponent::resolveCollisionPair(SceneObjectPair& pa
}
else
{
- ARMARX_WARNING << "No robot name given for RobotNodeSet \"" << pair.nodeSetName1 << "\"" << std::endl;
+ ARMARX_WARNING << "No robot name given for RobotNodeSet \"" << pair.nodeSetName1 << "\""
+ << std::endl;
}
}
@@ -290,12 +338,14 @@ bool armarx::CollisionCheckerComponent::resolveCollisionPair(SceneObjectPair& pa
VirtualRobot::SceneObjectPtr sc = getSceneObject(pair.robotName1, nodeName);
if (!sc)
{
- ARMARX_DEBUG << "failed to get object: \"" << pair.robotName1 << "\" : \"" << nodeName << "\"" << std::endl;
+ ARMARX_DEBUG << "failed to get object: \"" << pair.robotName1 << "\" : \"" << nodeName
+ << "\"" << std::endl;
return false;
}
if (!sc->getCollisionModel())
{
- ARMARX_WARNING << pair.robotName1 << (pair.robotName1 == "" ? "" : ".") << nodeName << " does not have a collision model. Ignoring..." << std::endl;
+ ARMARX_WARNING << pair.robotName1 << (pair.robotName1 == "" ? "" : ".") << nodeName
+ << " does not have a collision model. Ignoring..." << std::endl;
continue;
}
objects1->addSceneObject(sc);
@@ -310,7 +360,8 @@ bool armarx::CollisionCheckerComponent::resolveCollisionPair(SceneObjectPair& pa
{
if (robotPair.robot->getName() == pair.robotName2)
{
- VirtualRobot::SceneObjectSetPtr set = robotPair.robot->getRobotNodeSet(pair.nodeSetName2);
+ VirtualRobot::SceneObjectSetPtr set =
+ robotPair.robot->getRobotNodeSet(pair.nodeSetName2);
if (set)
{
for (VirtualRobot::SceneObjectPtr so : set->getSceneObjects())
@@ -320,7 +371,9 @@ bool armarx::CollisionCheckerComponent::resolveCollisionPair(SceneObjectPair& pa
}
else
{
- ARMARX_WARNING << "Robot \"" << pair.robotName2 << "\" has no set with name \"" << pair.nodeSetName2 << "\"" << std::endl;
+ ARMARX_WARNING << "Robot \"" << pair.robotName2
+ << "\" has no set with name \"" << pair.nodeSetName2 << "\""
+ << std::endl;
}
break;
}
@@ -328,7 +381,8 @@ bool armarx::CollisionCheckerComponent::resolveCollisionPair(SceneObjectPair& pa
}
else
{
- ARMARX_WARNING << "No robot name given for RobotNodeSet \"" << pair.nodeSetName2 << "\"" << std::endl;
+ ARMARX_WARNING << "No robot name given for RobotNodeSet \"" << pair.nodeSetName2 << "\""
+ << std::endl;
}
}
@@ -338,12 +392,14 @@ bool armarx::CollisionCheckerComponent::resolveCollisionPair(SceneObjectPair& pa
VirtualRobot::SceneObjectPtr sc = getSceneObject(pair.robotName2, nodeName);
if (!sc)
{
- ARMARX_DEBUG << "failed to get object: \"" << pair.robotName2 << "\" : \"" << nodeName << "\"" << std::endl;
+ ARMARX_DEBUG << "failed to get object: \"" << pair.robotName2 << "\" : \"" << nodeName
+ << "\"" << std::endl;
return false;
}
if (!sc->getCollisionModel())
{
- ARMARX_WARNING << pair.robotName2 << (pair.robotName2 == "" ? "" : ".") << nodeName << " does not have a collision model. Ignoring..." << std::endl;
+ ARMARX_WARNING << pair.robotName2 << (pair.robotName2 == "" ? "" : ".") << nodeName
+ << " does not have a collision model. Ignoring..." << std::endl;
continue;
}
objects2->addSceneObject(sc);
@@ -359,7 +415,9 @@ bool armarx::CollisionCheckerComponent::resolveCollisionPair(SceneObjectPair& pa
return true;
}
-VirtualRobot::SceneObjectPtr armarx::CollisionCheckerComponent::getSceneObject(const std::string& robotName, const std::string& nodeName)
+VirtualRobot::SceneObjectPtr
+armarx::CollisionCheckerComponent::getSceneObject(const std::string& robotName,
+ const std::string& nodeName)
{
if (robotName.size() == 0 && useWorkingMemory)
{
@@ -392,7 +450,12 @@ VirtualRobot::SceneObjectPtr armarx::CollisionCheckerComponent::getSceneObject(c
return VirtualRobot::SceneObjectPtr();
}
-bool armarx::CollisionCheckerComponent::parseNodeSet(const std::string& setAsString, std::string& robotName, std::vector<std::string>& nodeNames, bool& usesNodeSet, std::string& nodeSetName)
+bool
+armarx::CollisionCheckerComponent::parseNodeSet(const std::string& setAsString,
+ std::string& robotName,
+ std::vector<std::string>& nodeNames,
+ bool& usesNodeSet,
+ std::string& nodeSetName)
{
std::vector<std::string> split = simox::alg::split(setAsString, ":");
@@ -435,7 +498,8 @@ bool armarx::CollisionCheckerComponent::parseNodeSet(const std::string& setAsStr
return true;
}
-VirtualRobot::SceneObjectPtr armarx::CollisionCheckerComponent::getSceneObjectFromWorkingMemory(const std::string& name)
+VirtualRobot::SceneObjectPtr
+armarx::CollisionCheckerComponent::getSceneObjectFromWorkingMemory(const std::string& name)
{
for (VirtualRobot::SceneObjectPtr& sc : workingMemorySceneObjects)
{
@@ -445,7 +509,8 @@ VirtualRobot::SceneObjectPtr armarx::CollisionCheckerComponent::getSceneObjectFr
}
}
const memoryx::EntityBasePtr entityBase = objectInstancesPrx->getEntityByName(name);
- const memoryx::ObjectInstanceBasePtr object = memoryx::ObjectInstancePtr::dynamicCast(entityBase);
+ const memoryx::ObjectInstanceBasePtr object =
+ memoryx::ObjectInstancePtr::dynamicCast(entityBase);
if (!object)
{
@@ -455,24 +520,30 @@ VirtualRobot::SceneObjectPtr armarx::CollisionCheckerComponent::getSceneObjectFr
const std::string className = object->getMostProbableClass();
- memoryx::ObjectClassList classes = workingMemoryPrx->getPriorKnowledge()->getObjectClassesSegment()->getClassWithSubclasses(className);
+ memoryx::ObjectClassList classes =
+ workingMemoryPrx->getPriorKnowledge()->getObjectClassesSegment()->getClassWithSubclasses(
+ className);
if (!classes.size())
{
- ARMARX_INFO_S << "No classes for most probable class '" << className << "' of object '" << object->getName() << "' with name " << name;
+ ARMARX_INFO_S << "No classes for most probable class '" << className << "' of object '"
+ << object->getName() << "' with name " << name;
return VirtualRobot::SceneObjectPtr();
}
memoryx::ObjectClassPtr objectClass = memoryx::ObjectClassPtr::dynamicCast(classes.at(0));
- const PosePtr objectPose = armarx::PosePtr::dynamicCast(armarx::PoseBasePtr {new armarx::Pose{object->getPositionBase(), object->getOrientationBase()}});
+ const PosePtr objectPose = armarx::PosePtr::dynamicCast(armarx::PoseBasePtr{
+ new armarx::Pose{object->getPositionBase(), object->getOrientationBase()}});
if (!objectClass)
{
- ARMARX_ERROR << "Can't use object class with ice id " << classes.at(0)->ice_id() << std::endl;
+ ARMARX_ERROR << "Can't use object class with ice id " << classes.at(0)->ice_id()
+ << std::endl;
return VirtualRobot::SceneObjectPtr();
}
- memoryx::EntityWrappers::SimoxObjectWrapperPtr sw = objectClass->addWrapper(new memoryx::EntityWrappers::SimoxObjectWrapper(fileManager));
+ memoryx::EntityWrappers::SimoxObjectWrapperPtr sw =
+ objectClass->addWrapper(new memoryx::EntityWrappers::SimoxObjectWrapper(fileManager));
VirtualRobot::ManipulationObjectPtr orgMo = sw->getManipulationObject();
ARMARX_CHECK_EXPRESSION(orgMo);
std::string moName = orgMo->getName();
@@ -481,7 +552,8 @@ VirtualRobot::SceneObjectPtr armarx::CollisionCheckerComponent::getSceneObjectFr
//move the object to the given position
if (!objectPose)
{
- ARMARX_ERROR << "Can't convert pose of " << objectClass->getName() << " to armarx::Pose." << std::endl;
+ ARMARX_ERROR << "Can't convert pose of " << objectClass->getName() << " to armarx::Pose."
+ << std::endl;
return VirtualRobot::SceneObjectPtr();
}
mo->setGlobalPose(objectPose->toEigen());
@@ -491,7 +563,8 @@ VirtualRobot::SceneObjectPtr armarx::CollisionCheckerComponent::getSceneObjectFr
return mo;
}
-void armarx::CollisionCheckerComponent::synchronizeObjectsFromWorkingMemory()
+void
+armarx::CollisionCheckerComponent::synchronizeObjectsFromWorkingMemory()
{
std::scoped_lock lockPosition(wmPositionMutex);
for (VirtualRobot::SceneObjectPtr& sc : workingMemorySceneObjects)
@@ -504,12 +577,14 @@ void armarx::CollisionCheckerComponent::synchronizeObjectsFromWorkingMemory()
}
}
-void armarx::CollisionCheckerComponent::reportDistancesAndCollisions()
+void
+armarx::CollisionCheckerComponent::reportDistancesAndCollisions()
{
std::scoped_lock lockData(dataMutex);
for (RobotPair& r : robots)
{
- armarx::RemoteRobot::synchronizeLocalClone(r.robot, r.robotStateComponentPrx->getSynchronizedRobot());
+ armarx::RemoteRobot::synchronizeLocalClone(
+ r.robot, r.robotStateComponentPrx->getSynchronizedRobot());
}
synchronizeObjectsFromWorkingMemory();
@@ -523,11 +598,15 @@ void armarx::CollisionCheckerComponent::reportDistancesAndCollisions()
if (useDebugDrawer && debugDrawerTopicPrx)
{
std::ostringstream nodeNames1Str;
- std::copy(pair.nodeNames1.begin(), pair.nodeNames1.end() - 1, std::ostream_iterator<std::string>(nodeNames1Str, ","));
+ std::copy(pair.nodeNames1.begin(),
+ pair.nodeNames1.end() - 1,
+ std::ostream_iterator<std::string>(nodeNames1Str, ","));
nodeNames1Str << pair.nodeNames1.back();
std::ostringstream nodeNames2Str;
- std::copy(pair.nodeNames2.begin(), pair.nodeNames2.end() - 1, std::ostream_iterator<std::string>(nodeNames2Str, ","));
+ std::copy(pair.nodeNames2.begin(),
+ pair.nodeNames2.end() - 1,
+ std::ostream_iterator<std::string>(nodeNames2Str, ","));
nodeNames2Str << pair.nodeNames2.back();
Eigen::Vector3f p1, p2;
@@ -546,45 +625,87 @@ void armarx::CollisionCheckerComponent::reportDistancesAndCollisions()
color.g = 0.9;
color.b = 0;
}
- debugDrawerTopicPrx->setLineVisu("distanceVisu", pair.robotName1 + (pair.robotName1 == "" ? "" : ": ") + nodeNames1Str.str() + " - " + pair.robotName2 + (pair.robotName2 == "" ? "" : ": ") + nodeNames2Str.str(), p1a, p2a, 1, color);
+ debugDrawerTopicPrx->setLineVisu("distanceVisu",
+ pair.robotName1 + (pair.robotName1 == "" ? "" : ": ") +
+ nodeNames1Str.str() + " - " + pair.robotName2 +
+ (pair.robotName2 == "" ? "" : ": ") +
+ nodeNames2Str.str(),
+ p1a,
+ p2a,
+ 1,
+ color);
}
else
{
distance = collisionChecker->calculateDistance(pair.objects1, pair.objects2);
}
- distanceListenerPrx->reportDistance(pair.robotName1, pair.nodeNames1, pair.robotName2, pair.nodeNames2, distance);
+ distanceListenerPrx->reportDistance(
+ pair.robotName1, pair.nodeNames1, pair.robotName2, pair.nodeNames2, distance);
if (distance <= pair.warningDistance)
{
- collisionListenerPrx->reportCollisionWarning(pair.robotName1, pair.nodeNames1, pair.robotName2, pair.nodeNames2, distance);
+ collisionListenerPrx->reportCollisionWarning(
+ pair.robotName1, pair.nodeNames1, pair.robotName2, pair.nodeNames2, distance);
}
if (collisionChecker->checkCollision(pair.objects1, pair.objects2))
{
- collisionListenerPrx->reportCollision(pair.robotName1, pair.nodeNames1, pair.robotName2, pair.nodeNames2, distance);
+ collisionListenerPrx->reportCollision(
+ pair.robotName1, pair.nodeNames1, pair.robotName2, pair.nodeNames2, distance);
}
}
}
-void CollisionCheckerComponent::addCollisionPair(const std::string& robotName1, const std::vector<std::string>& nodeNames1, const std::string& robotName2, const std::vector<std::string>& nodeNames2, double warningDistance, const Ice::Current&)
+void
+CollisionCheckerComponent::addCollisionPair(const std::string& robotName1,
+ const std::vector<std::string>& nodeNames1,
+ const std::string& robotName2,
+ const std::vector<std::string>& nodeNames2,
+ double warningDistance,
+ const Ice::Current&)
{
- addCollisionPair(robotName1, nodeNames1, false, "", robotName2, nodeNames2, false, "", warningDistance);
+ addCollisionPair(
+ robotName1, nodeNames1, false, "", robotName2, nodeNames2, false, "", warningDistance);
}
-void CollisionCheckerComponent::addCollisionPair(const std::string& robotName1, const std::vector<std::string>& nodeNames1, const bool usesNodeSet1, const std::string& nodeSetName1, const std::string& robotName2, const std::vector<std::string>& nodeNames2, const bool usesNodeSet2, const std::string& nodeSetName2, double warningDistance)
+void
+CollisionCheckerComponent::addCollisionPair(const std::string& robotName1,
+ const std::vector<std::string>& nodeNames1,
+ const bool usesNodeSet1,
+ const std::string& nodeSetName1,
+ const std::string& robotName2,
+ const std::vector<std::string>& nodeNames2,
+ const bool usesNodeSet2,
+ const std::string& nodeSetName2,
+ double warningDistance)
{
if (hasCollisionPair(robotName1, nodeNames1, robotName2, nodeNames2))
{
return;
}
- SceneObjectPair pair = {VirtualRobot::SceneObjectSetPtr(), robotName1, nodeNames1, usesNodeSet1, nodeSetName1, VirtualRobot::SceneObjectSetPtr(), robotName2, nodeNames2, usesNodeSet2, nodeSetName2, warningDistance};
+ SceneObjectPair pair = {VirtualRobot::SceneObjectSetPtr(),
+ robotName1,
+ nodeNames1,
+ usesNodeSet1,
+ nodeSetName1,
+ VirtualRobot::SceneObjectSetPtr(),
+ robotName2,
+ nodeNames2,
+ usesNodeSet2,
+ nodeSetName2,
+ warningDistance};
std::scoped_lock lockData(dataMutex);
resolveCollisionPair(pair);
sceneObjectPairs.push_back(pair);
}
-void CollisionCheckerComponent::removeCollisionPair(const std::string& robotName1, const std::vector<std::string>& nodeNames1, const std::string& robotName2, const std::vector<std::string>& nodeNames2, const Ice::Current&)
+void
+CollisionCheckerComponent::removeCollisionPair(const std::string& robotName1,
+ const std::vector<std::string>& nodeNames1,
+ const std::string& robotName2,
+ const std::vector<std::string>& nodeNames2,
+ const Ice::Current&)
{
std::scoped_lock lockData(dataMutex);
for (auto it = sceneObjectPairs.begin(); it != sceneObjectPairs.end(); it++)
@@ -600,7 +721,12 @@ void CollisionCheckerComponent::removeCollisionPair(const std::string& robotName
}
}
-bool CollisionCheckerComponent::hasCollisionPair(const std::string& robotName1, const std::vector<std::string>& nodeNames1, const std::string& robotName2, const std::vector<std::string>& nodeNames2, const Ice::Current&) const
+bool
+CollisionCheckerComponent::hasCollisionPair(const std::string& robotName1,
+ const std::vector<std::string>& nodeNames1,
+ const std::string& robotName2,
+ const std::vector<std::string>& nodeNames2,
+ const Ice::Current&) const
{
std::scoped_lock lockData(dataMutex);
for (const SceneObjectPair& pair : sceneObjectPairs)
@@ -616,7 +742,13 @@ bool CollisionCheckerComponent::hasCollisionPair(const std::string& robotName1,
return false;
}
-void CollisionCheckerComponent::setWarningDistance(const std::string& robotName1, const std::vector<std::string>& nodeNames1, const std::string& robotName2, const std::vector<std::string>& nodeNames2, double warningDistance, const Ice::Current&)
+void
+CollisionCheckerComponent::setWarningDistance(const std::string& robotName1,
+ const std::vector<std::string>& nodeNames1,
+ const std::string& robotName2,
+ const std::vector<std::string>& nodeNames2,
+ double warningDistance,
+ const Ice::Current&)
{
std::scoped_lock lockData(dataMutex);
for (SceneObjectPair& pair : sceneObjectPairs)
@@ -632,7 +764,12 @@ void CollisionCheckerComponent::setWarningDistance(const std::string& robotName1
}
}
-double CollisionCheckerComponent::getWarningDistance(const std::string& robotName1, const std::vector<std::string>& nodeNames1, const std::string& robotName2, const std::vector<std::string>& nodeNames2, const Ice::Current&) const
+double
+CollisionCheckerComponent::getWarningDistance(const std::string& robotName1,
+ const std::vector<std::string>& nodeNames1,
+ const std::string& robotName2,
+ const std::vector<std::string>& nodeNames2,
+ const Ice::Current&) const
{
std::scoped_lock lockData(dataMutex);
for (const SceneObjectPair& pair : sceneObjectPairs)
@@ -648,40 +785,60 @@ double CollisionCheckerComponent::getWarningDistance(const std::string& robotNam
return INFINITY;
}
-CollisionPairList CollisionCheckerComponent::getAllCollisionPairs(const Ice::Current&) const
+CollisionPairList
+CollisionCheckerComponent::getAllCollisionPairs(const Ice::Current&) const
{
std::scoped_lock lockData(dataMutex);
CollisionPairList list;
for (const SceneObjectPair& pair : sceneObjectPairs)
{
- armarx::CollisionPair p = {pair.robotName1, pair.nodeNames1, pair.robotName2, pair.nodeNames2, pair.warningDistance};
+ armarx::CollisionPair p = {pair.robotName1,
+ pair.nodeNames1,
+ pair.robotName2,
+ pair.nodeNames2,
+ pair.warningDistance};
list.push_back(p);
}
return list;
}
-int CollisionCheckerComponent::getInterval(const Ice::Current&) const
+int
+CollisionCheckerComponent::getInterval(const Ice::Current&) const
{
return interval;
}
-void CollisionCheckerComponent::setInterval(int interval, const Ice::Current&)
+void
+CollisionCheckerComponent::setInterval(int interval, const Ice::Current&)
{
std::shared_lock lockConnected(connectedMutex);
this->interval = interval;
if (connected && reportTask && reportTask->isRunning())
{
reportTask->stop();
- reportTask = new PeriodicTask<CollisionCheckerComponent>(this, &CollisionCheckerComponent::reportDistancesAndCollisions, interval, false, "ReportCollisionsTask");
+ reportTask = new PeriodicTask<CollisionCheckerComponent>(
+ this,
+ &CollisionCheckerComponent::reportDistancesAndCollisions,
+ interval,
+ false,
+ "ReportCollisionsTask");
reportTask->start();
}
else
{
- reportTask = new PeriodicTask<CollisionCheckerComponent>(this, &CollisionCheckerComponent::reportDistancesAndCollisions, interval, false, "ReportCollisionsTask");
+ reportTask = new PeriodicTask<CollisionCheckerComponent>(
+ this,
+ &CollisionCheckerComponent::reportDistancesAndCollisions,
+ interval,
+ false,
+ "ReportCollisionsTask");
}
}
-void CollisionCheckerComponent::reportEntityCreated(const std::string& segmentName, const memoryx::EntityBasePtr& entity, const Ice::Current&)
+void
+CollisionCheckerComponent::reportEntityCreated(const std::string& segmentName,
+ const memoryx::EntityBasePtr& entity,
+ const Ice::Current&)
{
if (segmentName == objectInstancesPrx->getSegmentName())
{
@@ -689,8 +846,12 @@ void CollisionCheckerComponent::reportEntityCreated(const std::string& segmentNa
for (SceneObjectPair& pair : sceneObjectPairs)
{
if ((!pair.objects1 || !pair.objects2) &&
- ((pair.robotName1 == "" && std::find(pair.nodeNames1.begin(), pair.nodeNames1.end(), entity->getName()) != pair.nodeNames1.end()) ||
- (pair.robotName2 == "" && std::find(pair.nodeNames2.begin(), pair.nodeNames2.end(), entity->getName()) != pair.nodeNames2.end())))
+ ((pair.robotName1 == "" &&
+ std::find(pair.nodeNames1.begin(), pair.nodeNames1.end(), entity->getName()) !=
+ pair.nodeNames1.end()) ||
+ (pair.robotName2 == "" &&
+ std::find(pair.nodeNames2.begin(), pair.nodeNames2.end(), entity->getName()) !=
+ pair.nodeNames2.end())))
{
resolveCollisionPair(pair);
}
@@ -699,9 +860,12 @@ void CollisionCheckerComponent::reportEntityCreated(const std::string& segmentNa
else if (segmentName == agentInstancesPrx->getSegmentName())
{
std::scoped_lock lockData(dataMutex);
- memoryx::AgentInstanceBasePtr agent = agentInstancesPrx->getAgentInstanceById(entity->getId());
- RobotStateComponentInterfacePrx robotStateComponentPrx = agent->getSharedRobot()->getRobotStateComponent();
- VirtualRobot::RobotPtr robot = armarx::RemoteRobot::createLocalCloneFromFile(robotStateComponentPrx);
+ memoryx::AgentInstanceBasePtr agent =
+ agentInstancesPrx->getAgentInstanceById(entity->getId());
+ RobotStateComponentInterfacePrx robotStateComponentPrx =
+ agent->getSharedRobot()->getRobotStateComponent();
+ VirtualRobot::RobotPtr robot =
+ armarx::RemoteRobot::createLocalCloneFromFile(robotStateComponentPrx);
robots.push_back({robot, robotStateComponentPrx});
@@ -709,8 +873,7 @@ void CollisionCheckerComponent::reportEntityCreated(const std::string& segmentNa
for (SceneObjectPair& pair : sceneObjectPairs)
{
if ((!pair.objects1 || !pair.objects2) &&
- (pair.robotName1 == entity->getName() ||
- pair.robotName2 == entity->getName()))
+ (pair.robotName1 == entity->getName() || pair.robotName2 == entity->getName()))
{
resolveCollisionPair(pair);
}
@@ -718,31 +881,45 @@ void CollisionCheckerComponent::reportEntityCreated(const std::string& segmentNa
}
}
-void CollisionCheckerComponent::reportEntityUpdated(const std::string& segmentName, const memoryx::EntityBasePtr& entityOld, const memoryx::EntityBasePtr& entityNew, const Ice::Current&)
+void
+CollisionCheckerComponent::reportEntityUpdated(const std::string& segmentName,
+ const memoryx::EntityBasePtr& entityOld,
+ const memoryx::EntityBasePtr& entityNew,
+ const Ice::Current&)
{
if (segmentName == objectInstancesPrx->getSegmentName())
{
std::scoped_lock lockPosition(wmPositionMutex);
- const memoryx::ObjectInstanceBasePtr object = memoryx::ObjectInstancePtr::dynamicCast(entityNew);
- currentPositions[entityNew->getName()] = armarx::PosePtr::dynamicCast(armarx::PoseBasePtr {new armarx::Pose{object->getPositionBase(), object->getOrientationBase()}});
+ const memoryx::ObjectInstanceBasePtr object =
+ memoryx::ObjectInstancePtr::dynamicCast(entityNew);
+ currentPositions[entityNew->getName()] = armarx::PosePtr::dynamicCast(armarx::PoseBasePtr{
+ new armarx::Pose{object->getPositionBase(), object->getOrientationBase()}});
}
}
-void CollisionCheckerComponent::reportEntityRemoved(const std::string& segmentName, const memoryx::EntityBasePtr& entity, const Ice::Current&)
+void
+CollisionCheckerComponent::reportEntityRemoved(const std::string& segmentName,
+ const memoryx::EntityBasePtr& entity,
+ const Ice::Current&)
{
if (segmentName == objectInstancesPrx->getSegmentName())
{
std::scoped_lock lockData(dataMutex);
for (SceneObjectPair& pair : sceneObjectPairs)
{
- if ((pair.robotName1 == "" && std::find(pair.nodeNames1.begin(), pair.nodeNames1.end(), entity->getName()) != pair.nodeNames1.end()) ||
- (pair.robotName2 == "" && std::find(pair.nodeNames2.begin(), pair.nodeNames2.end(), entity->getName()) != pair.nodeNames2.end()))
+ if ((pair.robotName1 == "" &&
+ std::find(pair.nodeNames1.begin(), pair.nodeNames1.end(), entity->getName()) !=
+ pair.nodeNames1.end()) ||
+ (pair.robotName2 == "" &&
+ std::find(pair.nodeNames2.begin(), pair.nodeNames2.end(), entity->getName()) !=
+ pair.nodeNames2.end()))
{
pair.objects1.reset();
pair.objects2.reset();
}
}
- for (auto it = workingMemorySceneObjects.begin(); it != workingMemorySceneObjects.end(); ++it)
+ for (auto it = workingMemorySceneObjects.begin(); it != workingMemorySceneObjects.end();
+ ++it)
{
if ((*it)->getName() == entity->getName())
{
@@ -765,8 +942,7 @@ void CollisionCheckerComponent::reportEntityRemoved(const std::string& segmentNa
for (SceneObjectPair& pair : sceneObjectPairs)
{
- if (pair.robotName1 == entity->getName() ||
- pair.robotName2 == entity->getName())
+ if (pair.robotName1 == entity->getName() || pair.robotName2 == entity->getName())
{
pair.objects1.reset();
pair.objects2.reset();
@@ -775,7 +951,8 @@ void CollisionCheckerComponent::reportEntityRemoved(const std::string& segmentNa
}
}
-void CollisionCheckerComponent::reportSnapshotLoaded(const std::string& segmentName, const Ice::Current&)
+void
+CollisionCheckerComponent::reportSnapshotLoaded(const std::string& segmentName, const Ice::Current&)
{
if (segmentName == objectInstancesPrx->getSegmentName())
{
@@ -790,12 +967,13 @@ void CollisionCheckerComponent::reportSnapshotLoaded(const std::string& segmentN
}
}
-void CollisionCheckerComponent::reportSnapshotCompletelyLoaded(const Ice::Current& c)
+void
+CollisionCheckerComponent::reportSnapshotCompletelyLoaded(const Ice::Current& c)
{
-
}
-void CollisionCheckerComponent::reportMemoryCleared(const std::string& segmentName, const Ice::Current&)
+void
+CollisionCheckerComponent::reportMemoryCleared(const std::string& segmentName, const Ice::Current&)
{
if (segmentName == objectInstancesPrx->getSegmentName())
{
@@ -810,4 +988,3 @@ void CollisionCheckerComponent::reportMemoryCleared(const std::string& segmentNa
}
}
}
-
diff --git a/source/RobotComponents/components/CollisionCheckerComponent/CollisionCheckerComponent.h b/source/RobotComponents/components/CollisionCheckerComponent/CollisionCheckerComponent.h
index a35391b51a2005dd4b7e944b45c2517ebec28b08..4bae57285fee536533d6b354de96911e1d38eaac 100644
--- a/source/RobotComponents/components/CollisionCheckerComponent/CollisionCheckerComponent.h
+++ b/source/RobotComponents/components/CollisionCheckerComponent/CollisionCheckerComponent.h
@@ -23,22 +23,22 @@
#pragma once
+#include <mutex>
+
+#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/SceneObject.h>
+
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
-#include <MemoryX/interface/components/WorkingMemoryInterface.h>
-#include <MemoryX/core/GridFileManager.h>
-
#include <RobotAPI/interface/core/RobotState.h>
#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
#include <RobotAPI/libraries/core/Pose.h>
#include <RobotComponents/interface/components/CollisionCheckerInterface.h>
-#include <VirtualRobot/Robot.h>
-#include <VirtualRobot/SceneObject.h>
-
-#include <mutex>
+#include <MemoryX/core/GridFileManager.h>
+#include <MemoryX/interface/components/WorkingMemoryInterface.h>
namespace armarx
{
@@ -46,23 +46,47 @@ namespace armarx
* @class CollisionCheckerComponentPropertyDefinitions
* @brief
*/
- class CollisionCheckerComponentPropertyDefinitions:
- public armarx::ComponentPropertyDefinitions
+ class CollisionCheckerComponentPropertyDefinitions : public armarx::ComponentPropertyDefinitions
{
public:
- CollisionCheckerComponentPropertyDefinitions(std::string prefix):
+ CollisionCheckerComponentPropertyDefinitions(std::string prefix) :
armarx::ComponentPropertyDefinitions(prefix)
{
- defineRequiredProperty<bool>("UseWorkingMemory", "If true, the WorkingMemory is used to get all SceneObjects. Otherwise a RobotStateComponent is used.");
- defineOptionalProperty<std::string>("WorkingMemoryName", "WorkingMemory", "The name of the WorkingMemory.");
- defineOptionalProperty<std::string>("WorkingMemoryListenerTopicName", "WorkingMemoryUpdates", "The name of the topic to listen for WorkingMemory updates.");
- defineOptionalProperty<std::string>("RobotStateComponentName", "RobotStateComponent", "The name of the RobotStateComponent.");
- defineOptionalProperty<int>("interval", 100, "The interval of time to check for collisions. (in ms)");
- defineOptionalProperty<std::string>("CollisionPairs", "[]", "A list of tripels including two SceneObjects or SceneObject lists and a distance from which to warn. Example: [{Armar3:(Ring L J0,Ring L J1),Armar3:(Ring R J0,Ring R J1),2.0},{Armar3:Index L J0,tableb,3.0}]");
- defineOptionalProperty<std::string>("DistanceListenerTopicName", "DistanceListener", "The topic name to get information about the distances.");
- defineOptionalProperty<std::string>("CollisionListenerTopicName", "CollisionListener", "The topic name to get information about collisions.");
- defineOptionalProperty<bool>("UseDebugDrawer", "false", "If true, the distances are printet using the debug drawer.");
- defineOptionalProperty<std::string>("DebugDrawerTopicName", "DebugDrawerUpdates", "The name of the debug drawer topic.");
+ defineRequiredProperty<bool>("UseWorkingMemory",
+ "If true, the WorkingMemory is used to get all "
+ "SceneObjects. Otherwise a RobotStateComponent is used.");
+ defineOptionalProperty<std::string>(
+ "WorkingMemoryName", "WorkingMemory", "The name of the WorkingMemory.");
+ defineOptionalProperty<std::string>(
+ "WorkingMemoryListenerTopicName",
+ "WorkingMemoryUpdates",
+ "The name of the topic to listen for WorkingMemory updates.");
+ defineOptionalProperty<std::string>("RobotStateComponentName",
+ "RobotStateComponent",
+ "The name of the RobotStateComponent.");
+ defineOptionalProperty<int>(
+ "interval", 100, "The interval of time to check for collisions. (in ms)");
+ defineOptionalProperty<std::string>(
+ "CollisionPairs",
+ "[]",
+ "A list of tripels including two SceneObjects or SceneObject lists and a distance "
+ "from which to warn. Example: [{Armar3:(Ring L J0,Ring L J1),Armar3:(Ring R "
+ "J0,Ring R J1),2.0},{Armar3:Index L J0,tableb,3.0}]");
+ defineOptionalProperty<std::string>(
+ "DistanceListenerTopicName",
+ "DistanceListener",
+ "The topic name to get information about the distances.");
+ defineOptionalProperty<std::string>(
+ "CollisionListenerTopicName",
+ "CollisionListener",
+ "The topic name to get information about collisions.");
+ defineOptionalProperty<bool>(
+ "UseDebugDrawer",
+ "false",
+ "If true, the distances are printet using the debug drawer.");
+ defineOptionalProperty<std::string>("DebugDrawerTopicName",
+ "DebugDrawerUpdates",
+ "The name of the debug drawer topic.");
}
};
@@ -96,6 +120,7 @@ namespace armarx
std::string nodeSetName2;
double warningDistance;
} SceneObjectPair;
+
typedef struct
{
VirtualRobot::RobotPtr robot;
@@ -108,21 +133,45 @@ namespace armarx
/**
* @see armarx::ManagedIceObject::getDefaultName()
*/
- std::string getDefaultName() const override
+ std::string
+ getDefaultName() const override
{
return "CollisionCheckerComponent";
}
- void addCollisionPair(const std::string& robotName1, const std::vector<std::string>& nodeNames1, const std::string& robotName2, const std::vector<std::string>& nodeNames2, double warningDistance, const Ice::Current& = Ice::emptyCurrent) override;
-
- void removeCollisionPair(const std::string& robotName1, const std::vector<std::string>& nodeNames1, const std::string& robotName2, const std::vector<std::string>& nodeNames2, const Ice::Current& = Ice::emptyCurrent) override;
-
- bool hasCollisionPair(const std::string& robotName1, const std::vector<std::string>& nodeNames1, const std::string& robotName2, const std::vector<std::string>& nodeNames2, const Ice::Current& = Ice::emptyCurrent) const override;
-
- void setWarningDistance(const std::string& robotName1, const std::vector<std::string>& nodeNames1, const std::string& robotName2, const std::vector<std::string>& nodeNames2, double warningDistance, const Ice::Current& = Ice::emptyCurrent) override;
- double getWarningDistance(const std::string& robotName1, const std::vector<std::string>& nodeNames1, const std::string& robotName2, const std::vector<std::string>& nodeNames2, const Ice::Current& = Ice::emptyCurrent) const override;
-
- CollisionPairList getAllCollisionPairs(const Ice::Current& = Ice::emptyCurrent) const override;
+ void addCollisionPair(const std::string& robotName1,
+ const std::vector<std::string>& nodeNames1,
+ const std::string& robotName2,
+ const std::vector<std::string>& nodeNames2,
+ double warningDistance,
+ const Ice::Current& = Ice::emptyCurrent) override;
+
+ void removeCollisionPair(const std::string& robotName1,
+ const std::vector<std::string>& nodeNames1,
+ const std::string& robotName2,
+ const std::vector<std::string>& nodeNames2,
+ const Ice::Current& = Ice::emptyCurrent) override;
+
+ bool hasCollisionPair(const std::string& robotName1,
+ const std::vector<std::string>& nodeNames1,
+ const std::string& robotName2,
+ const std::vector<std::string>& nodeNames2,
+ const Ice::Current& = Ice::emptyCurrent) const override;
+
+ void setWarningDistance(const std::string& robotName1,
+ const std::vector<std::string>& nodeNames1,
+ const std::string& robotName2,
+ const std::vector<std::string>& nodeNames2,
+ double warningDistance,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ double getWarningDistance(const std::string& robotName1,
+ const std::vector<std::string>& nodeNames1,
+ const std::string& robotName2,
+ const std::vector<std::string>& nodeNames2,
+ const Ice::Current& = Ice::emptyCurrent) const override;
+
+ CollisionPairList
+ getAllCollisionPairs(const Ice::Current& = Ice::emptyCurrent) const override;
int getInterval(const Ice::Current& = Ice::emptyCurrent) const override;
void setInterval(int interval, const Ice::Current& = Ice::emptyCurrent) override;
@@ -155,20 +204,42 @@ namespace armarx
private:
bool resolveCollisionPair(SceneObjectPair& pair);
- VirtualRobot::SceneObjectPtr getSceneObject(const std::string& robotName, const std::string& nodeName);
- bool parseNodeSet(const std::string& setAsString, std::string& robotName, std::vector<std::string>& nodeNames, bool& usesNodeSet, std::string& nodeSetName);
+ VirtualRobot::SceneObjectPtr getSceneObject(const std::string& robotName,
+ const std::string& nodeName);
+ bool parseNodeSet(const std::string& setAsString,
+ std::string& robotName,
+ std::vector<std::string>& nodeNames,
+ bool& usesNodeSet,
+ std::string& nodeSetName);
void reportDistancesAndCollisions();
VirtualRobot::SceneObjectPtr getSceneObjectFromWorkingMemory(const std::string& name);
void synchronizeObjectsFromWorkingMemory();
- virtual void addCollisionPair(const std::string& robotName1, const std::vector<std::string>& nodeNames1, const bool usesNodeSet1, const std::string& nodeSetName1, const std::string& robotName2, const std::vector<std::string>& nodeNames2, const bool usesNodeSet2, const std::string& nodeSetName2, double warningDistance);
-
- void reportEntityCreated(const std::string& segmentName, const ::memoryx::EntityBasePtr& entity, const ::Ice::Current& = Ice::emptyCurrent) override;
- void reportEntityUpdated(const std::string& segmentName, const ::memoryx::EntityBasePtr& entityOld, const ::memoryx::EntityBasePtr& entityNew, const ::Ice::Current& = Ice::emptyCurrent) override;
- void reportEntityRemoved(const std::string& segmentName, const ::memoryx::EntityBasePtr& entity, const ::Ice::Current& = Ice::emptyCurrent) override;
- void reportSnapshotLoaded(const std::string& segmentName, const ::Ice::Current& = Ice::emptyCurrent) override;
+ virtual void addCollisionPair(const std::string& robotName1,
+ const std::vector<std::string>& nodeNames1,
+ const bool usesNodeSet1,
+ const std::string& nodeSetName1,
+ const std::string& robotName2,
+ const std::vector<std::string>& nodeNames2,
+ const bool usesNodeSet2,
+ const std::string& nodeSetName2,
+ double warningDistance);
+
+ void reportEntityCreated(const std::string& segmentName,
+ const ::memoryx::EntityBasePtr& entity,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
+ void reportEntityUpdated(const std::string& segmentName,
+ const ::memoryx::EntityBasePtr& entityOld,
+ const ::memoryx::EntityBasePtr& entityNew,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
+ void reportEntityRemoved(const std::string& segmentName,
+ const ::memoryx::EntityBasePtr& entity,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
+ void reportSnapshotLoaded(const std::string& segmentName,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
void reportSnapshotCompletelyLoaded(const Ice::Current& c = Ice::emptyCurrent) override;
- void reportMemoryCleared(const std::string& segmentName, const ::Ice::Current& = Ice::emptyCurrent) override;
+ void reportMemoryCleared(const std::string& segmentName,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
bool useWorkingMemory;
@@ -202,5 +273,4 @@ namespace armarx
DebugDrawerInterfacePrx debugDrawerTopicPrx;
bool useDebugDrawer;
};
-}
-
+} // namespace armarx
diff --git a/source/RobotComponents/components/CollisionCheckerComponent/test/CollisionCheckerComponentTest.cpp b/source/RobotComponents/components/CollisionCheckerComponent/test/CollisionCheckerComponentTest.cpp
index 2bd2661928e8b6d2bc7e6607494ced24ae23c2f2..27a78f84ab1b47bebf6d437ae7873d29cfcd6e6f 100644
--- a/source/RobotComponents/components/CollisionCheckerComponent/test/CollisionCheckerComponentTest.cpp
+++ b/source/RobotComponents/components/CollisionCheckerComponent/test/CollisionCheckerComponentTest.cpp
@@ -24,11 +24,11 @@
#define ARMARX_BOOST_TEST
+#include <iostream>
+
#include <RobotComponents/Test.h>
#include <RobotComponents/components/CollisionCheckerComponent/CollisionCheckerComponent.h>
-#include <iostream>
-
BOOST_AUTO_TEST_CASE(testExample)
{
armarx::CollisionCheckerComponent instance;
diff --git a/source/RobotComponents/components/DHParameterOptimizationLogger/DHParameterOptimizationLogger.cpp b/source/RobotComponents/components/DHParameterOptimizationLogger/DHParameterOptimizationLogger.cpp
index 1185749f9c148bca60bf248aa84ba20a16484e06..ed8065483eba6512e028e1ca92aa5d974935c89c 100644
--- a/source/RobotComponents/components/DHParameterOptimizationLogger/DHParameterOptimizationLogger.cpp
+++ b/source/RobotComponents/components/DHParameterOptimizationLogger/DHParameterOptimizationLogger.cpp
@@ -22,26 +22,27 @@
#include "DHParameterOptimizationLogger.h"
-#include <RobotAPI/libraries/core/FramedPose.h>
+#include <ctime>
+#include <iomanip>
-#include <ArmarXCore/core/csv/CsvWriter.h>
-#include <ArmarXCore/core/time/TimeUtil.h>
-#include <ArmarXCore/observers/variant/TimedVariant.h>
-#include <ArmarXCore/core/util/algorithm.h>
+#include <Eigen/Geometry>
#include <VirtualRobot/Nodes/Sensor.h>
+#include <VirtualRobot/RobotNodeSet.h>
-#include <Eigen/Geometry>
+#include <ArmarXCore/core/csv/CsvWriter.h>
+#include <ArmarXCore/core/time/TimeUtil.h>
+#include <ArmarXCore/core/util/algorithm.h>
+#include <ArmarXCore/observers/variant/TimedVariant.h>
-#include <iomanip>
-#include <ctime>
+#include <RobotAPI/libraries/core/FramedPose.h>
using namespace armarx;
namespace fs = std::filesystem;
-
-void DHParameterOptimizationLogger::logData(const Ice::Current&)
+void
+DHParameterOptimizationLogger::logData(const Ice::Current&)
{
std::vector<float> data;
if (_logRepeatAccuracy)
@@ -60,7 +61,8 @@ void DHParameterOptimizationLogger::logData(const Ice::Current&)
ARMARX_DEBUG << "Logging data finished";
}
-void DHParameterOptimizationLogger::logDataWithRepeatAccuracy(const Ice::Current&)
+void
+DHParameterOptimizationLogger::logDataWithRepeatAccuracy(const Ice::Current&)
{
if (!_logRepeatAccuracy)
{
@@ -88,22 +90,25 @@ void DHParameterOptimizationLogger::logDataWithRepeatAccuracy(const Ice::Current
ARMARX_DEBUG << "Logging data with repeat accuracy finished";
}
-void DHParameterOptimizationLogger::startViconLogging(const Ice::Current&)
+void
+DHParameterOptimizationLogger::startViconLogging(const Ice::Current&)
{
_viconLoggingActive = true;
}
-void DHParameterOptimizationLogger::stopViconLogging(const Ice::Current&)
+void
+DHParameterOptimizationLogger::stopViconLogging(const Ice::Current&)
{
_viconLoggingActive = false;
}
-void DHParameterOptimizationLogger::init(const std::string& kinematicChainName,
- const std::map<std::string, std::string>& neckMarkerMapping,
- const std::map<std::string, std::string>& handMarkerMapping,
- const std::string& loggingFileName,
- bool logRepeatAccuracy,
- const Ice::Current&)
+void
+DHParameterOptimizationLogger::init(const std::string& kinematicChainName,
+ const std::map<std::string, std::string>& neckMarkerMapping,
+ const std::map<std::string, std::string>& handMarkerMapping,
+ const std::string& loggingFileName,
+ bool logRepeatAccuracy,
+ const Ice::Current&)
{
ARMARX_CHECK_EXPRESSION(!neckMarkerMapping.empty());
ARMARX_CHECK_EXPRESSION(!handMarkerMapping.empty());
@@ -135,24 +140,26 @@ void DHParameterOptimizationLogger::init(const std::string& kinematicChainName,
for (auto& pair : neckMarkerMapping)
{
auto& i = pair.second;
- ARMARX_CHECK_EXPRESSION(std::find_if(sensors.begin(), sensors.end(), [i](VirtualRobot::SensorPtr s)
- {
- return s->getName() == i;
- }) != sensors.end()) <<
- "Robot does not have a sensor with name '" + i + "'";
+ ARMARX_CHECK_EXPRESSION(std::find_if(sensors.begin(),
+ sensors.end(),
+ [i](VirtualRobot::SensorPtr s)
+ { return s->getName() == i; }) != sensors.end())
+ << "Robot does not have a sensor with name '" + i + "'";
}
for (auto& pair : handMarkerMapping)
{
auto& i = pair.second;
- ARMARX_CHECK_EXPRESSION(std::find_if(sensors.begin(), sensors.end(), [i](VirtualRobot::SensorPtr s)
- {
- return s->getName() == i;
- }) != sensors.end()) <<
- "Robot does not have a sensor with name '" + i + "'";
+ ARMARX_CHECK_EXPRESSION(std::find_if(sensors.begin(),
+ sensors.end(),
+ [i](VirtualRobot::SensorPtr s)
+ { return s->getName() == i; }) != sensors.end())
+ << "Robot does not have a sensor with name '" + i + "'";
}
_kcName = kinematicChainName;
- ARMARX_CHECK_EXPRESSION(localRobot->hasRobotNodeSet(_kcName)) << "Robot " + localRobot->getName() + " does not have a kinematic chain with name " + _kcName;
+ ARMARX_CHECK_EXPRESSION(localRobot->hasRobotNodeSet(_kcName))
+ << "Robot " + localRobot->getName() + " does not have a kinematic chain with name " +
+ _kcName;
_kc = localRobot->getRobotNodeSet(_kcName);
_jointNames = _kc->getNodeNames();
_logRepeatAccuracy = logRepeatAccuracy;
@@ -166,7 +173,9 @@ void DHParameterOptimizationLogger::init(const std::string& kinematicChainName,
_initialized = true;
}
-void DHParameterOptimizationLogger::reportLabeledViconMarkerFrame(const StringVector3fMap& markerPosMap, const Ice::Current&)
+void
+DHParameterOptimizationLogger::reportLabeledViconMarkerFrame(const StringVector3fMap& markerPosMap,
+ const Ice::Current&)
{
// Format of received data: ObjectName:MarkerName x y z
@@ -178,9 +187,12 @@ void DHParameterOptimizationLogger::reportLabeledViconMarkerFrame(const StringVe
copyMap[key.erase(0, i)] = it.second;
Vector3f v = it.second;
// ARMARX_INFO << it.first << " " << v.e0 << " " << v.e1 << " " << v.e2;
- debugObserverPrx->setDebugDatafield("DHParameterOptimization", it.first + "_x", new Variant(v.e0));
- debugObserverPrx->setDebugDatafield("DHParameterOptimization", it.first + "_y", new Variant(v.e1));
- debugObserverPrx->setDebugDatafield("DHParameterOptimization", it.first + "_z", new Variant(v.e2));
+ debugObserverPrx->setDebugDatafield(
+ "DHParameterOptimization", it.first + "_x", new Variant(v.e0));
+ debugObserverPrx->setDebugDatafield(
+ "DHParameterOptimization", it.first + "_y", new Variant(v.e1));
+ debugObserverPrx->setDebugDatafield(
+ "DHParameterOptimization", it.first + "_z", new Variant(v.e2));
}
if (_viconLoggingActive)
@@ -189,16 +201,18 @@ void DHParameterOptimizationLogger::reportLabeledViconMarkerFrame(const StringVe
_viconMarkerBuffer = copyMap;
_timeStampLastViconDataUpdate = TimeUtil::GetTime();
_viconBufferUpdated = true;
-
}
}
-void DHParameterOptimizationLogger::reportUnlabeledViconMarkerFrame(const Vector3fSeq& markerPos, const Ice::Current&)
+void
+DHParameterOptimizationLogger::reportUnlabeledViconMarkerFrame(const Vector3fSeq& markerPos,
+ const Ice::Current&)
{
// Not needed
}
-void DHParameterOptimizationLogger::setupFile(const std::string& fileName)
+void
+DHParameterOptimizationLogger::setupFile(const std::string& fileName)
{
std::string localFileName = fileName;
using namespace std::filesystem;
@@ -225,7 +239,8 @@ void DHParameterOptimizationLogger::setupFile(const std::string& fileName)
std::string pathOrig = getProperty<std::string>("LoggingFilePath").getValue();
if (!pathOrig.empty() && pathOrig[0] == '~')
{
- ARMARX_CHECK_EXPRESSION(pathOrig.size() == 1 || pathOrig[1] == '/') << "LoggingFilePath isviconValues not valid: '~' has to be followed by ’/’";
+ ARMARX_CHECK_EXPRESSION(pathOrig.size() == 1 || pathOrig[1] == '/')
+ << "LoggingFilePath isviconValues not valid: '~' has to be followed by ’/’";
char const* home = std::getenv("HOME");
ARMARX_CHECK_EXPRESSION(home) << "HOME environment variable is not set!";
pathOrig.replace(0, 1, home);
@@ -239,7 +254,8 @@ void DHParameterOptimizationLogger::setupFile(const std::string& fileName)
_filePath = filePath.string();
}
-void DHParameterOptimizationLogger::onInitComponent()
+void
+DHParameterOptimizationLogger::onInitComponent()
{
usingTopic(getProperty<std::string>("ViconDataTopicName").getValue());
usingProxy(getProperty<std::string>("RobotStateComponentName").getValue());
@@ -255,18 +271,20 @@ void DHParameterOptimizationLogger::onInitComponent()
_viconObjectName_hand = getProperty<std::string>("Hand_ViconObjectName").getValue();
}
-
-void DHParameterOptimizationLogger::onConnectComponent()
+void
+DHParameterOptimizationLogger::onConnectComponent()
{
getProxy(robotStateComponent, getProperty<std::string>("RobotStateComponentName").getValue());
getProxy(robotUnitObserver, getProperty<std::string>("RobotUnitObserverName").getValue());
_viconLoggingActive = true;
- debugObserverPrx = getTopic<DebugObserverInterfacePrx>(getProperty<std::string>("DebugObserverTopicName").getValue());
+ debugObserverPrx = getTopic<DebugObserverInterfacePrx>(
+ getProperty<std::string>("DebugObserverTopicName").getValue());
debugDrawerPrx = getTopic<DebugDrawerInterfacePrx>("DebugDrawerUpdates");
- localRobot = RemoteRobot::createLocalCloneFromFile(robotStateComponent, VirtualRobot::RobotIO::RobotDescription::eStructure);
+ localRobot = RemoteRobot::createLocalCloneFromFile(
+ robotStateComponent, VirtualRobot::RobotIO::RobotDescription::eStructure);
std::vector<VirtualRobot::SensorPtr> s = localRobot->getSensors();
for (auto& sp : s)
@@ -275,25 +293,26 @@ void DHParameterOptimizationLogger::onConnectComponent()
}
}
-
-void DHParameterOptimizationLogger::onDisconnectComponent()
+void
+DHParameterOptimizationLogger::onDisconnectComponent()
{
_viconLoggingActive = false;
}
-
-void DHParameterOptimizationLogger::onExitComponent()
+void
+DHParameterOptimizationLogger::onExitComponent()
{
-
}
-armarx::PropertyDefinitionsPtr DHParameterOptimizationLogger::createPropertyDefinitions()
+armarx::PropertyDefinitionsPtr
+DHParameterOptimizationLogger::createPropertyDefinitions()
{
- return armarx::PropertyDefinitionsPtr(new DHParameterOptimizationLoggerPropertyDefinitions(
- getConfigIdentifier()));
+ return armarx::PropertyDefinitionsPtr(
+ new DHParameterOptimizationLoggerPropertyDefinitions(getConfigIdentifier()));
}
-void DHParameterOptimizationLogger::setupHeader()
+void
+DHParameterOptimizationLogger::setupHeader()
{
std::vector<std::string> header;
@@ -370,7 +389,8 @@ void DHParameterOptimizationLogger::setupHeader()
_header = header;
}
-void DHParameterOptimizationLogger::logData(std::vector<float>& data)
+void
+DHParameterOptimizationLogger::logData(std::vector<float>& data)
{
// order is important!! do not change without adapting the order of setting up the header in setupHeader() !!
logViconMarkerPositions(data);
@@ -382,7 +402,8 @@ void DHParameterOptimizationLogger::logData(std::vector<float>& data)
logFTSensorValues(data);
}
-void DHParameterOptimizationLogger::logViconMarkerPositions(std::vector<float>& data)
+void
+DHParameterOptimizationLogger::logViconMarkerPositions(std::vector<float>& data)
{
ARMARX_DEBUG << "Logging viconMarkerPositions";
@@ -429,7 +450,8 @@ void DHParameterOptimizationLogger::logViconMarkerPositions(std::vector<float>&
}
}
-void DHParameterOptimizationLogger::logJointValues(std::vector<float>& data) const
+void
+DHParameterOptimizationLogger::logJointValues(std::vector<float>& data) const
{
ARMARX_DEBUG << "Logging jointValues";
@@ -441,7 +463,8 @@ void DHParameterOptimizationLogger::logJointValues(std::vector<float>& data) con
data.push_back(localRobot->getRobotNode("TorsoJoint")->getJointValue());
}
-void DHParameterOptimizationLogger::logForwardKinematicToViconMarker(std::vector<float>& data) const
+void
+DHParameterOptimizationLogger::logForwardKinematicToViconMarker(std::vector<float>& data) const
{
ARMARX_DEBUG << "Logging forwardKinematicToViconMarker";
@@ -457,7 +480,8 @@ void DHParameterOptimizationLogger::logForwardKinematicToViconMarker(std::vector
}
}
-void DHParameterOptimizationLogger::logErrorBetweenModelAndVicon(std::vector<float>& data)
+void
+DHParameterOptimizationLogger::logErrorBetweenModelAndVicon(std::vector<float>& data)
{
ARMARX_DEBUG << "Logging errorBetweenModelAndVicon";
@@ -482,29 +506,33 @@ void DHParameterOptimizationLogger::logErrorBetweenModelAndVicon(std::vector<flo
Eigen::MatrixXf modelNeckPositions_neckFrame(3, _neckMarker.size());
-
-
-
size_t i = 0;
for (auto& it : _neckMarker)
{
- observedNeckPositions_robotRootFrame(0, i) = _filteredViconMarkerPositions_robotRootFrame.at(it).e0;
- observedNeckPositions_robotRootFrame(1, i) = _filteredViconMarkerPositions_robotRootFrame.at(it).e1;
- observedNeckPositions_robotRootFrame(2, i) = _filteredViconMarkerPositions_robotRootFrame.at(it).e2;
-
- modelNeckPositions_neckFrame(0, i) = neck->getSensor(_neckMarkerMapping.at(it))->getTransformationFrom(neck)(0, 3);
- modelNeckPositions_neckFrame(1, i) = neck->getSensor(_neckMarkerMapping.at(it))->getTransformationFrom(neck)(1, 3);
- modelNeckPositions_neckFrame(2, i) = neck->getSensor(_neckMarkerMapping.at(it))->getTransformationFrom(neck)(2, 3);
+ observedNeckPositions_robotRootFrame(0, i) =
+ _filteredViconMarkerPositions_robotRootFrame.at(it).e0;
+ observedNeckPositions_robotRootFrame(1, i) =
+ _filteredViconMarkerPositions_robotRootFrame.at(it).e1;
+ observedNeckPositions_robotRootFrame(2, i) =
+ _filteredViconMarkerPositions_robotRootFrame.at(it).e2;
+
+ modelNeckPositions_neckFrame(0, i) =
+ neck->getSensor(_neckMarkerMapping.at(it))->getTransformationFrom(neck)(0, 3);
+ modelNeckPositions_neckFrame(1, i) =
+ neck->getSensor(_neckMarkerMapping.at(it))->getTransformationFrom(neck)(1, 3);
+ modelNeckPositions_neckFrame(2, i) =
+ neck->getSensor(_neckMarkerMapping.at(it))->getTransformationFrom(neck)(2, 3);
// visu
- Eigen::Vector3f position = (root->getGlobalPose() * observedNeckPositions_robotRootFrame).block<3, 1>(0, i);
- debugDrawerPrx->setSphereVisu("markerVisu", it, new Vector3(position), DrawColor {0, 1, 1, 1}, 7);
+ Eigen::Vector3f position =
+ (root->getGlobalPose() * observedNeckPositions_robotRootFrame).block<3, 1>(0, i);
+ debugDrawerPrx->setSphereVisu(
+ "markerVisu", it, new Vector3(position), DrawColor{0, 1, 1, 1}, 7);
i++;
}
-
// Calculation of hand pose from vicon data
Eigen::MatrixXf observedHandPositions_robotRootFrame(3, _handMarker.size());
Eigen::MatrixXf modelHandPositions_handFrame(3, _handMarker.size());
@@ -513,43 +541,61 @@ void DHParameterOptimizationLogger::logErrorBetweenModelAndVicon(std::vector<flo
i = 0;
for (auto& it : _handMarker)
{
- observedHandPositions_robotRootFrame(0, i) = _filteredViconMarkerPositions_robotRootFrame.at(it).e0;
- observedHandPositions_robotRootFrame(1, i) = _filteredViconMarkerPositions_robotRootFrame.at(it).e1;
- observedHandPositions_robotRootFrame(2, i) = _filteredViconMarkerPositions_robotRootFrame.at(it).e2;
-
- modelHandPositions_handFrame(0, i) = hand->getSensor(_handMarkerMapping.at(it))->getTransformationFrom(hand)(0, 3);
- modelHandPositions_handFrame(1, i) = hand->getSensor(_handMarkerMapping.at(it))->getTransformationFrom(hand)(1, 3);
- modelHandPositions_handFrame(2, i) = hand->getSensor(_handMarkerMapping.at(it))->getTransformationFrom(hand)(2, 3);
+ observedHandPositions_robotRootFrame(0, i) =
+ _filteredViconMarkerPositions_robotRootFrame.at(it).e0;
+ observedHandPositions_robotRootFrame(1, i) =
+ _filteredViconMarkerPositions_robotRootFrame.at(it).e1;
+ observedHandPositions_robotRootFrame(2, i) =
+ _filteredViconMarkerPositions_robotRootFrame.at(it).e2;
+
+ modelHandPositions_handFrame(0, i) =
+ hand->getSensor(_handMarkerMapping.at(it))->getTransformationFrom(hand)(0, 3);
+ modelHandPositions_handFrame(1, i) =
+ hand->getSensor(_handMarkerMapping.at(it))->getTransformationFrom(hand)(1, 3);
+ modelHandPositions_handFrame(2, i) =
+ hand->getSensor(_handMarkerMapping.at(it))->getTransformationFrom(hand)(2, 3);
// visu
- Eigen::Vector3f position = (root->getGlobalPose() * observedHandPositions_robotRootFrame).block<3, 1>(0, i);
- debugDrawerPrx->setSphereVisu("markerVisu", it, new Vector3(position), DrawColor {0, 1, 0, 1}, 7);
+ Eigen::Vector3f position =
+ (root->getGlobalPose() * observedHandPositions_robotRootFrame).block<3, 1>(0, i);
+ debugDrawerPrx->setSphereVisu(
+ "markerVisu", it, new Vector3(position), DrawColor{0, 1, 0, 1}, 7);
i++;
}
- ARMARX_DEBUG << VAROUT(modelHandPositions_handFrame) << VAROUT(observedHandPositions_robotRootFrame);
- Eigen::Matrix4f observedHandRootPose_robotRootFrame = registration3d(modelHandPositions_handFrame.transpose(), observedHandPositions_robotRootFrame.transpose());
+ ARMARX_DEBUG << VAROUT(modelHandPositions_handFrame)
+ << VAROUT(observedHandPositions_robotRootFrame);
+ Eigen::Matrix4f observedHandRootPose_robotRootFrame =
+ registration3d(modelHandPositions_handFrame.transpose(),
+ observedHandPositions_robotRootFrame.transpose());
_newObservedHandRootPose = observedHandRootPose_robotRootFrame;
_observedHandPosesUpdated = true;
- debugDrawerPrx->setPoseVisu("MarkerVisu", "observedTCP", new Pose(root->getGlobalPose() * observedHandRootPose_robotRootFrame));
+ debugDrawerPrx->setPoseVisu(
+ "MarkerVisu",
+ "observedTCP",
+ new Pose(root->getGlobalPose() * observedHandRootPose_robotRootFrame));
debugDrawerPrx->setPoseVisu("MarkerVisu", "kinematicTCP", new Pose(hand->getGlobalPose()));
// Eigen::Matrix4f viconRelativePose = observedNeckRootPose.inverse() * observedHandRootPose;
- float errorPos = (hand->getPoseInRootFrame().block<3, 1>(0, 3) - observedHandRootPose_robotRootFrame.block<3, 1>(0, 3)).norm();
+ float errorPos = (hand->getPoseInRootFrame().block<3, 1>(0, 3) -
+ observedHandRootPose_robotRootFrame.block<3, 1>(0, 3))
+ .norm();
// Calculate error
// Eigen::Vector3f errorV = viconRelativePose.block<3, 1>(0, 3) - modelRelativePose.block<3, 1>(0, 3);
// float errorPos = errorV.norm();
- Eigen::Matrix3f oriDir = hand->getPoseInRootFrame().block<3, 3>(0, 0) * observedHandRootPose_robotRootFrame.block<3, 3>(0, 0).inverse();
+ Eigen::Matrix3f oriDir = hand->getPoseInRootFrame().block<3, 3>(0, 0) *
+ observedHandRootPose_robotRootFrame.block<3, 3>(0, 0).inverse();
Eigen::AngleAxisf aa(oriDir);
float errorOri = aa.angle();
- ARMARX_INFO << "Error (Hand) in rootFrame between model and vicon ---- Position-Error: " << errorPos << " mm, Orientation-Error: " << errorOri << " rad";
+ ARMARX_INFO << "Error (Hand) in rootFrame between model and vicon ---- Position-Error: "
+ << errorPos << " mm, Orientation-Error: " << errorOri << " rad";
data.push_back(errorPos);
data.push_back(errorOri);
@@ -558,7 +604,8 @@ void DHParameterOptimizationLogger::logErrorBetweenModelAndVicon(std::vector<flo
}
}
-void DHParameterOptimizationLogger::logRawTorqueTicks(std::vector<float>& data) const
+void
+DHParameterOptimizationLogger::logRawTorqueTicks(std::vector<float>& data) const
{
ARMARX_DEBUG << "Logging rawTorqueTicks";
@@ -566,9 +613,14 @@ void DHParameterOptimizationLogger::logRawTorqueTicks(std::vector<float>& data)
{
for (auto& node : _jointNames)
{
- if (robotUnitObserver->existsDataField("SensorDevices", node + "_SensorValueArmar6Actuator_torqueTicks"))
+ if (robotUnitObserver->existsDataField("SensorDevices",
+ node + "_SensorValueArmar6Actuator_torqueTicks"))
{
- data.push_back((float) robotUnitObserver->getDatafieldByName("SensorDevices", node + "_SensorValueArmar6Actuator_torqueTicks")->getInt());
+ data.push_back(
+ (float)robotUnitObserver
+ ->getDatafieldByName("SensorDevices",
+ node + "_SensorValueArmar6Actuator_torqueTicks")
+ ->getInt());
}
else
{
@@ -585,7 +637,8 @@ void DHParameterOptimizationLogger::logRawTorqueTicks(std::vector<float>& data)
}
}
-void DHParameterOptimizationLogger::logTorqueValues(std::vector<float>& data) const
+void
+DHParameterOptimizationLogger::logTorqueValues(std::vector<float>& data) const
{
ARMARX_DEBUG << "Logging torqueValues";
@@ -593,9 +646,13 @@ void DHParameterOptimizationLogger::logTorqueValues(std::vector<float>& data) co
{
for (auto& node : _jointNames)
{
- if (robotUnitObserver->existsDataField("SensorDevices", node + "_SensorValueArmar6Actuator_torque"))
+ if (robotUnitObserver->existsDataField("SensorDevices",
+ node + "_SensorValueArmar6Actuator_torque"))
{
- data.push_back(robotUnitObserver->getDatafieldByName("SensorDevices", node + "_SensorValueArmar6Actuator_torque")->getFloat());
+ data.push_back(robotUnitObserver
+ ->getDatafieldByName("SensorDevices",
+ node + "_SensorValueArmar6Actuator_torque")
+ ->getFloat());
}
else
{
@@ -612,11 +669,12 @@ void DHParameterOptimizationLogger::logTorqueValues(std::vector<float>& data) co
}
}
-void DHParameterOptimizationLogger::logFTSensorValues(std::vector<float>& data) const
+void
+DHParameterOptimizationLogger::logFTSensorValues(std::vector<float>& data) const
{
ARMARX_DEBUG << "Logging FTSensorValues";
- std::string datafieldName ;
+ std::string datafieldName;
if (_kc->getTCP()->getName() == "Hand R TCP")
{
datafieldName = "FT R_FTSensorDataValue_torque";
@@ -630,7 +688,8 @@ void DHParameterOptimizationLogger::logFTSensorValues(std::vector<float>& data)
{
if (robotUnitObserver->existsDataField("SensorDevices", datafieldName))
{
- TimedVariantPtr tv = TimedVariantPtr::dynamicCast(robotUnitObserver->getDatafieldByName("SensorDevices", datafieldName));
+ TimedVariantPtr tv = TimedVariantPtr::dynamicCast(
+ robotUnitObserver->getDatafieldByName("SensorDevices", datafieldName));
ARMARX_CHECK_EXPRESSION(tv->getType() == VariantType::Vector3);
Vector3Ptr v = tv->get<Vector3>();
@@ -653,7 +712,8 @@ void DHParameterOptimizationLogger::logFTSensorValues(std::vector<float>& data)
}
}
-StringVector3fMap DHParameterOptimizationLogger::waitBlockingForAllMarkersFiltered()
+StringVector3fMap
+DHParameterOptimizationLogger::waitBlockingForAllMarkersFiltered()
{
TIMING_START(waitBlockingForAllMarkersFiltered);
std::map<std::string, armarx::rtfilters::AverageFilter> viconDataFilters;
@@ -694,7 +754,9 @@ StringVector3fMap DHParameterOptimizationLogger::waitBlockingForAllMarkersFilter
// Check if all data is valid, if not, continue waiting for next data frame
for (const std::string& s : _neckMarker)
{
- ARMARX_CHECK_EXPRESSION(viconValues.count(s) > 0) << armarx::getMapKeys(viconValues) << "\n" << s;
+ ARMARX_CHECK_EXPRESSION(viconValues.count(s) > 0)
+ << armarx::getMapKeys(viconValues) << "\n"
+ << s;
Vector3f v = viconValues.at(s);
if (v.e0 == 0.0f && v.e1 == 0.0f && v.e2 == 0.0f)
{
@@ -739,7 +801,6 @@ StringVector3fMap DHParameterOptimizationLogger::waitBlockingForAllMarkersFilter
v.e1 = y;
v.e2 = z;
viconValues.insert(std::make_pair(s, v));
-
}
for (const std::string& s : _handMarker)
{
@@ -759,14 +820,15 @@ StringVector3fMap DHParameterOptimizationLogger::waitBlockingForAllMarkersFilter
i++;
}
}
-invalidData:; // rare case where goto is actually useful
+ invalidData:; // rare case where goto is actually useful
}
TIMING_END(waitBlockingForAllMarkersFiltered);
viconDataFilters.clear();
return viconValues;
}
-VirtualRobot::RobotNodePtr DHParameterOptimizationLogger::getNeckMarkerRootNode() const
+VirtualRobot::RobotNodePtr
+DHParameterOptimizationLogger::getNeckMarkerRootNode() const
{
ARMARX_CHECK_EXPRESSION(_kc);
VirtualRobot::RobotNodePtr node;
@@ -780,12 +842,14 @@ VirtualRobot::RobotNodePtr DHParameterOptimizationLogger::getNeckMarkerRootNode(
}
else
{
- ARMARX_ERROR << "Kinematic chain does not have a tcp with name 'Hand R TCP' or 'Hand L TCP'. Maybe the used robot is not Armar-6?!";
+ ARMARX_ERROR << "Kinematic chain does not have a tcp with name 'Hand R TCP' or 'Hand L "
+ "TCP'. Maybe the used robot is not Armar-6?!";
}
return node;
}
-VirtualRobot::RobotNodePtr DHParameterOptimizationLogger::getHandMarkerRootNode() const
+VirtualRobot::RobotNodePtr
+DHParameterOptimizationLogger::getHandMarkerRootNode() const
{
ARMARX_CHECK_EXPRESSION(_kc);
VirtualRobot::RobotNodePtr node;
@@ -799,16 +863,19 @@ VirtualRobot::RobotNodePtr DHParameterOptimizationLogger::getHandMarkerRootNode(
}
else
{
- ARMARX_ERROR << "Kinematic chain does not have a tcp with name 'Hand R TCP' or 'Hand L TCP'. Maybe the used robot is not Armar-6?!";
+ ARMARX_ERROR << "Kinematic chain does not have a tcp with name 'Hand R TCP' or 'Hand L "
+ "TCP'. Maybe the used robot is not Armar-6?!";
}
return node;
}
-std::pair<float, float> DHParameterOptimizationLogger::calculateRepeatError()
+std::pair<float, float>
+DHParameterOptimizationLogger::calculateRepeatError()
{
if (!_observedHandPosesUpdated)
{
- ARMARX_WARNING << "Pose of HandViconMarker have not been updated. No repeat accuracy calculated. (writes -1.0 in log)";
+ ARMARX_WARNING << "Pose of HandViconMarker have not been updated. No repeat accuracy "
+ "calculated. (writes -1.0 in log)";
return std::make_pair(-1.0f, -1.0f);
}
_observedHandPosesUpdated = false;
@@ -824,13 +891,15 @@ std::pair<float, float> DHParameterOptimizationLogger::calculateRepeatError()
Eigen::AngleAxisf aa(oriDir);
float errorOri = aa.angle();
- ARMARX_INFO << "Repeat Accuracy: Error between old pose and new pose ---- Position-Error: " << errorPos << " mm, Orientation-Error: " << errorOri << " rad";
+ ARMARX_INFO << "Repeat Accuracy: Error between old pose and new pose ---- Position-Error: "
+ << errorPos << " mm, Orientation-Error: " << errorOri << " rad";
return std::make_pair(errorPos, errorOri);
}
-
-Eigen::Matrix4f DHParameterOptimizationLogger::registration3d(const Eigen::MatrixXf& A, const Eigen::MatrixXf& B) const
+Eigen::Matrix4f
+DHParameterOptimizationLogger::registration3d(const Eigen::MatrixXf& A,
+ const Eigen::MatrixXf& B) const
{
// std::cout << "A:\n" << A<< std::endl;
@@ -878,7 +947,9 @@ Eigen::Matrix4f DHParameterOptimizationLogger::registration3d(const Eigen::Matri
return T;
}
-StringVector3fMap DHParameterOptimizationLogger::transformViconMarkerPositionsIntoRobotRootFrame(const StringVector3fMap& values) const
+StringVector3fMap
+DHParameterOptimizationLogger::transformViconMarkerPositionsIntoRobotRootFrame(
+ const StringVector3fMap& values) const
{
RemoteRobot::synchronizeLocalClone(localRobot, robotStateComponent);
VirtualRobot::RobotNodePtr neck = getNeckMarkerRootNode();
@@ -892,13 +963,17 @@ StringVector3fMap DHParameterOptimizationLogger::transformViconMarkerPositionsIn
observedNeckPositions_viconRootFrame(0, i) = values.at(it).e0;
observedNeckPositions_viconRootFrame(1, i) = values.at(it).e1;
observedNeckPositions_viconRootFrame(2, i) = values.at(it).e2;
- modelNeckPositions_neckFrame(0, i) = neck->getSensor(_neckMarkerMapping.at(it))->getTransformationFrom(neck)(0, 3);
- modelNeckPositions_neckFrame(1, i) = neck->getSensor(_neckMarkerMapping.at(it))->getTransformationFrom(neck)(1, 3);
- modelNeckPositions_neckFrame(2, i) = neck->getSensor(_neckMarkerMapping.at(it))->getTransformationFrom(neck)(2, 3);
+ modelNeckPositions_neckFrame(0, i) =
+ neck->getSensor(_neckMarkerMapping.at(it))->getTransformationFrom(neck)(0, 3);
+ modelNeckPositions_neckFrame(1, i) =
+ neck->getSensor(_neckMarkerMapping.at(it))->getTransformationFrom(neck)(1, 3);
+ modelNeckPositions_neckFrame(2, i) =
+ neck->getSensor(_neckMarkerMapping.at(it))->getTransformationFrom(neck)(2, 3);
i++;
}
- Eigen::Matrix4f observedNeckRootPose_viconRootFrame = registration3d(modelNeckPositions_neckFrame.transpose(), observedNeckPositions_viconRootFrame.transpose());
+ Eigen::Matrix4f observedNeckRootPose_viconRootFrame = registration3d(
+ modelNeckPositions_neckFrame.transpose(), observedNeckPositions_viconRootFrame.transpose());
localRobot->setGlobalPoseForRobotNode(neck, observedNeckRootPose_viconRootFrame);
Eigen::Matrix4f realRobotToVicon = localRobot->getGlobalPose().inverse();
@@ -916,7 +991,6 @@ StringVector3fMap DHParameterOptimizationLogger::transformViconMarkerPositionsIn
// ARMARX_INFO << VAROUT(realRobotToVicon) << VAROUT(realGlobalRootPose) << VAROUT(localRobot->getGlobalPose()) << VAROUT(neck->getGlobalPose()) << VAROUT(root->getGlobalPose());
-
StringVector3fMap transformedValues;
for (auto& it : values)
{
diff --git a/source/RobotComponents/components/DHParameterOptimizationLogger/DHParameterOptimizationLogger.h b/source/RobotComponents/components/DHParameterOptimizationLogger/DHParameterOptimizationLogger.h
index cda7f5d04842b9d4459956833b56a4b04b9112b7..234cadbf232055b14ad80f66be99e38bf8767639 100644
--- a/source/RobotComponents/components/DHParameterOptimizationLogger/DHParameterOptimizationLogger.h
+++ b/source/RobotComponents/components/DHParameterOptimizationLogger/DHParameterOptimizationLogger.h
@@ -22,17 +22,18 @@
#pragma once
+#include <mutex>
+
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/csv/CsvWriter.h>
-#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
#include <ArmarXCore/observers/Observer.h>
#include <ArmarXCore/observers/filters/rtfilters/AverageFilter.h>
+
#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
+#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
-#include <RobotComponents/interface/components/ViconMarkerProviderInterface.h>
#include <RobotComponents/interface/components/DHParameterOptimizationLoggerInterface.h>
-
-#include <mutex>
+#include <RobotComponents/interface/components/ViconMarkerProviderInterface.h>
namespace armarx
{
@@ -40,23 +41,36 @@ namespace armarx
* @class DHParameterOptimizationLoggerPropertyDefinitions
* @brief
*/
- class DHParameterOptimizationLoggerPropertyDefinitions:
+ class DHParameterOptimizationLoggerPropertyDefinitions :
public armarx::ComponentPropertyDefinitions
{
public:
- DHParameterOptimizationLoggerPropertyDefinitions(std::string prefix):
+ DHParameterOptimizationLoggerPropertyDefinitions(std::string prefix) :
armarx::ComponentPropertyDefinitions(prefix)
{
- defineOptionalProperty<std::string>("ViconDataTopicName", "ViconDataUpdates", "Topic on which the marker data is published by the ViconMarkerProvider");
+ defineOptionalProperty<std::string>(
+ "ViconDataTopicName",
+ "ViconDataUpdates",
+ "Topic on which the marker data is published by the ViconMarkerProvider");
defineOptionalProperty<std::string>("RobotStateComponentName", "RobotStateComponent");
- defineOptionalProperty<std::string>("RobotUnitObserverName", "RobotUnitObserver", "Name of the RobotUnitObserver");
- defineOptionalProperty<std::string>("LoggingFilePath", "~/home/DHLogging", "Path to the logging file");
- defineOptionalProperty<std::string>("DebugObserverTopicName", "DebugObserver", "The topic where updates are send to");
- defineOptionalProperty<int>("FilterWindowSize", 100, "Window size of average filter applied to vicon marker positions.");
- defineRequiredProperty<std::string>("Neck_ViconObjectName", "Name of the object for the neck marker that is used in the vicon system. "
- "This is needed to parse the data received from the vicon system correctly.");
- defineRequiredProperty<std::string>("Hand_ViconObjectName", "Name of the object for the hand marker that is used in the vicon system. "
- "This is needed to parse the data received from the vicon system correctly.");
+ defineOptionalProperty<std::string>(
+ "RobotUnitObserverName", "RobotUnitObserver", "Name of the RobotUnitObserver");
+ defineOptionalProperty<std::string>(
+ "LoggingFilePath", "~/home/DHLogging", "Path to the logging file");
+ defineOptionalProperty<std::string>(
+ "DebugObserverTopicName", "DebugObserver", "The topic where updates are send to");
+ defineOptionalProperty<int>(
+ "FilterWindowSize",
+ 100,
+ "Window size of average filter applied to vicon marker positions.");
+ defineRequiredProperty<std::string>(
+ "Neck_ViconObjectName",
+ "Name of the object for the neck marker that is used in the vicon system. "
+ "This is needed to parse the data received from the vicon system correctly.");
+ defineRequiredProperty<std::string>(
+ "Hand_ViconObjectName",
+ "Name of the object for the hand marker that is used in the vicon system. "
+ "This is needed to parse the data received from the vicon system correctly.");
}
};
@@ -79,7 +93,8 @@ namespace armarx
/**
* @see armarx::ManagedIceObject::getDefaultName()
*/
- std::string getDefaultName() const override
+ std::string
+ getDefaultName() const override
{
return "DHParameterOptimizationLogger";
}
@@ -98,6 +113,7 @@ namespace armarx
const std::string& loggingFileName,
bool logRepeatAccuracy,
const Ice::Current& = Ice::emptyCurrent) override;
+
protected:
/**
* @see armarx::ManagedIceObject::onInitComponent()
@@ -125,8 +141,10 @@ namespace armarx
armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
// ViconMarkerProviderListenerInterface interface
- void reportLabeledViconMarkerFrame(const StringVector3fMap& markerPosMap, const Ice::Current& = Ice::emptyCurrent) override;
- void reportUnlabeledViconMarkerFrame(const Vector3fSeq& markerPos, const Ice::Current& = Ice::emptyCurrent) override;
+ void reportLabeledViconMarkerFrame(const StringVector3fMap& markerPosMap,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ void reportUnlabeledViconMarkerFrame(const Vector3fSeq& markerPos,
+ const Ice::Current& = Ice::emptyCurrent) override;
private:
RobotStateComponentInterfacePrx robotStateComponent;
@@ -180,7 +198,9 @@ namespace armarx
bool _observedHandPosesUpdated;
std::pair<float, float> calculateRepeatError();
- Eigen::Matrix4f registration3d(const Eigen::MatrixXf& matrixA, const Eigen::MatrixXf& matrixB) const;
- StringVector3fMap transformViconMarkerPositionsIntoRobotRootFrame(const StringVector3fMap& values) const;
+ Eigen::Matrix4f registration3d(const Eigen::MatrixXf& matrixA,
+ const Eigen::MatrixXf& matrixB) const;
+ StringVector3fMap
+ transformViconMarkerPositionsIntoRobotRootFrame(const StringVector3fMap& values) const;
};
-}
+} // namespace armarx
diff --git a/source/RobotComponents/components/DHParameterOptimizationLogger/test/DHParameterOptimizationLoggerTest.cpp b/source/RobotComponents/components/DHParameterOptimizationLogger/test/DHParameterOptimizationLoggerTest.cpp
index 8aa83f8418f120cfc8d48c0377b03f13032180dd..91000b3244411ae69b856141a1d954110309f419 100644
--- a/source/RobotComponents/components/DHParameterOptimizationLogger/test/DHParameterOptimizationLoggerTest.cpp
+++ b/source/RobotComponents/components/DHParameterOptimizationLogger/test/DHParameterOptimizationLoggerTest.cpp
@@ -24,11 +24,11 @@
#define ARMARX_BOOST_TEST
+#include <iostream>
+
#include <RobotComponents/Test.h>
#include <RobotComponents/components/DHParameterOptimizationLogger/DHParameterOptimizationLogger.h>
-#include <iostream>
-
BOOST_AUTO_TEST_CASE(testExample)
{
armarx::DHParameterOptimizationLogger instance;
diff --git a/source/RobotComponents/components/DMPComponent/DMPComponent.cpp b/source/RobotComponents/components/DMPComponent/DMPComponent.cpp
index da81e567a25ae6a9086c1dee2650f16125556c55..cb27deecbb3e8c706c347508eda17bb5c27ac59b 100644
--- a/source/RobotComponents/components/DMPComponent/DMPComponent.cpp
+++ b/source/RobotComponents/components/DMPComponent/DMPComponent.cpp
@@ -23,6 +23,7 @@
*/
#include "DMPComponent.h"
+
#include <Ice/ObjectAdapter.h>
#pragma GCC diagnostic push
@@ -32,28 +33,28 @@
#include <dmp/testing/testdataset.h>
#pragma GCC diagnostic pop
-#include <MemoryX/libraries/memorytypes/MemoryXTypesObjectFactories.h>
#include <MemoryX/core/MemoryXCoreObjectFactories.h>
+#include <MemoryX/libraries/memorytypes/MemoryXTypesObjectFactories.h>
using namespace armarx;
-
-
-void DMPComponent::onInitComponent()
+void
+DMPComponent::onInitComponent()
{
ARMARX_INFO << "initializing DMP component";
usingProxy(getProperty<std::string>("LongtermMemoryName").getValue());
- ARMARX_INFO << "successfully initialized DMP component" ;
+ ARMARX_INFO << "successfully initialized DMP component";
}
-
-void DMPComponent::onConnectComponent()
+void
+DMPComponent::onConnectComponent()
{
ARMARX_INFO << "connecting DMP component";
try
{
- longtermMemoryPrx = getProxy<memoryx::LongtermMemoryInterfacePrx>(getProperty<std::string>("LongtermMemoryName").getValue());
+ longtermMemoryPrx = getProxy<memoryx::LongtermMemoryInterfacePrx>(
+ getProperty<std::string>("LongtermMemoryName").getValue());
}
catch (...)
{
@@ -74,19 +75,20 @@ void DMPComponent::onConnectComponent()
ARMARX_INFO << "successfully connected DMP component";
}
-
-void DMPComponent::onDisconnectComponent()
+void
+DMPComponent::onDisconnectComponent()
{
ARMARX_INFO << "disconnecting DMP component";
}
-
-void DMPComponent::onExitComponent()
+void
+DMPComponent::onExitComponent()
{
ARMARX_INFO << "exiting DMP component";
}
-DMPInstanceBasePrx DMPComponent::findInstancePrx(std::string name)
+DMPInstanceBasePrx
+DMPComponent::findInstancePrx(std::string name)
{
DMPInstancePair dmpInstPair = dmpPool.find(name)->second;
DMPInstanceBasePrx result = dmpInstPair.second;
@@ -101,7 +103,8 @@ DMPInstanceBasePrx DMPComponent::findInstancePrx(std::string name)
}
}
-DMPInstancePtr DMPComponent::findInstancePtr(std::string name)
+DMPInstancePtr
+DMPComponent::findInstancePtr(std::string name)
{
DMPInstancePair dmpInstPair = dmpPool.find(name)->second;
DMPInstancePtr result = dmpInstPair.first;
@@ -117,17 +120,22 @@ DMPInstancePtr DMPComponent::findInstancePtr(std::string name)
}
}
-
-DMPInstanceBasePrx DMPComponent::getDMP(const std::string& dmpName, const Ice::Current&)
+DMPInstanceBasePrx
+DMPComponent::getDMP(const std::string& dmpName, const Ice::Current&)
{
return findInstancePrx(dmpName);
}
-DMPInstanceBasePrx DMPComponent::instantiateDMP(const std::string& dmpName, const std::string& DMPType, int kernelSize, const Ice::Current&)
+DMPInstanceBasePrx
+DMPComponent::instantiateDMP(const std::string& dmpName,
+ const std::string& DMPType,
+ int kernelSize,
+ const Ice::Current&)
{
ARMARX_INFO << "instantiate DMP with name " << dmpName;
- boost::serialization::extended_type_info const* eti = boost::serialization::extended_type_info::find(DMPType.c_str());
+ boost::serialization::extended_type_info const* eti =
+ boost::serialization::extended_type_info::find(DMPType.c_str());
DMPInterfacePtr dmp;
@@ -153,9 +161,11 @@ DMPInstanceBasePrx DMPComponent::instantiateDMP(const std::string& dmpName, cons
return instprx;
}
-
// Database related functions
-DMPInstanceBasePrx DMPComponent::getDMPFromDatabase(const std::string& dmpEntityName, const std::string& dmpName, const Ice::Current&)
+DMPInstanceBasePrx
+DMPComponent::getDMPFromDatabase(const std::string& dmpEntityName,
+ const std::string& dmpName,
+ const Ice::Current&)
{
ARMARX_INFO << "getting DMP from database";
@@ -165,7 +175,8 @@ DMPInstanceBasePrx DMPComponent::getDMPFromDatabase(const std::string& dmpEntity
return DMPInstanceBasePrx();
}
- memoryx::DMPEntityPtr dmpEntity = memoryx::DMPEntityPtr::dynamicCast(dmpDataMemoryPrx->getDMPEntityByName(dmpEntityName));
+ memoryx::DMPEntityPtr dmpEntity =
+ memoryx::DMPEntityPtr::dynamicCast(dmpDataMemoryPrx->getDMPEntityByName(dmpEntityName));
std::string textStr = dmpEntity->getDMPtextStr();
@@ -191,7 +202,8 @@ DMPInstanceBasePrx DMPComponent::getDMPFromDatabase(const std::string& dmpEntity
if (dmpPool.find(dmpLocalName) != dmpPool.end())
{
- ARMARX_ERROR << "DMP with DMPName " + dmpLocalName + " exists in DMP pool. It is not allowed to overwrite it.";
+ ARMARX_ERROR << "DMP with DMPName " + dmpLocalName +
+ " exists in DMP pool. It is not allowed to overwrite it.";
return dmpPrx;
}
dmpPtr->setTimeStep(timestep);
@@ -199,13 +211,15 @@ DMPInstanceBasePrx DMPComponent::getDMPFromDatabase(const std::string& dmpEntity
dmpPool.insert(DMPPair(dmpLocalName, DMPInstancePair(dmpPtr, dmpPrx)));
- ARMARX_INFO << "DMP with name: " + dmpEntityName + " is loaded into DMP Pool with dmpName " + dmpLocalName + " and dmpType " + dmpPtr->getDMPType();
+ ARMARX_INFO << "DMP with name: " + dmpEntityName + " is loaded into DMP Pool with dmpName " +
+ dmpLocalName + " and dmpType " + dmpPtr->getDMPType();
ARMARX_INFO << "successfully got dmp from database.";
return dmpPrx;
}
-DMPInstanceBasePrx DMPComponent::getDMPFromDatabaseById(const std::string& dbId, const Ice::Current&)
+DMPInstanceBasePrx
+DMPComponent::getDMPFromDatabaseById(const std::string& dbId, const Ice::Current&)
{
ARMARX_INFO << "getting DMP from database";
@@ -215,14 +229,17 @@ DMPInstanceBasePrx DMPComponent::getDMPFromDatabaseById(const std::string& dbId,
return DMPInstanceBasePrx();
}
- memoryx::DMPEntityPtr dmpEntity = memoryx::DMPEntityPtr::dynamicCast(dmpDataMemoryPrx->getDMPEntityById(dbId));
+ memoryx::DMPEntityPtr dmpEntity =
+ memoryx::DMPEntityPtr::dynamicCast(dmpDataMemoryPrx->getDMPEntityById(dbId));
std::string dmpEntityName = dmpEntity->getDMPName();
return getDMPFromDatabase(dmpEntityName);
-
}
-DMPInstanceBasePrx DMPComponent::getDMPFromFile(const std::string& fileName, const std::string& dmpName, const Ice::Current&)
+DMPInstanceBasePrx
+DMPComponent::getDMPFromFile(const std::string& fileName,
+ const std::string& dmpName,
+ const Ice::Current&)
{
ARMARX_INFO << "getting DMP from file";
@@ -232,7 +249,8 @@ DMPInstanceBasePrx DMPComponent::getDMPFromFile(const std::string& fileName, con
return DMPInstanceBasePrx();
}
- std::string ext = fileName.rfind(".") == fileName.npos ? fileName : fileName.substr(fileName.rfind(".") + 1);
+ std::string ext =
+ fileName.rfind(".") == fileName.npos ? fileName : fileName.substr(fileName.rfind(".") + 1);
std::ifstream file(fileName);
DMP::DMPInterface* readPtr;
@@ -267,24 +285,30 @@ DMPInstanceBasePrx DMPComponent::getDMPFromFile(const std::string& fileName, con
dmpPtr->setCurrentTime(ctime);
if (dmpPool.find(dmpLocalName) != dmpPool.end())
{
- ARMARX_ERROR << "DMP with DMPName " + dmpLocalName + " exists in DMP pool. It is not allowed to overwrite it.";
+ ARMARX_ERROR << "DMP with DMPName " + dmpLocalName +
+ " exists in DMP pool. It is not allowed to overwrite it.";
return dmpPrx;
}
dmpPool.insert(DMPPair(dmpLocalName, DMPInstancePair(dmpPtr, dmpPrx)));
- ARMARX_INFO << "DMP with name: " + dmpName + " is loaded into DMP Pool with dmpName " + dmpLocalName + " and dmpType " + dmpPtr->getDMPType();
+ ARMARX_INFO << "DMP with name: " + dmpName + " is loaded into DMP Pool with dmpName " +
+ dmpLocalName + " and dmpType " + dmpPtr->getDMPType();
ARMARX_INFO << "successfully got dmp from file.";
file.close();
return dmpPrx;
}
-void DMPComponent::storeDMPInFile(const std::string& fileName, const std::string& dmpName, const Ice::Current&)
+void
+DMPComponent::storeDMPInFile(const std::string& fileName,
+ const std::string& dmpName,
+ const Ice::Current&)
{
ARMARX_INFO << "storing DMP in the file";
- std::string ext = fileName.rfind(".") == fileName.npos ? fileName : fileName.substr(fileName.rfind(".") + 1);
+ std::string ext =
+ fileName.rfind(".") == fileName.npos ? fileName : fileName.substr(fileName.rfind(".") + 1);
ofstream ofs(fileName);
DMPInstancePtr dmpPtr = findInstancePtr(dmpName);
@@ -308,7 +332,10 @@ void DMPComponent::storeDMPInFile(const std::string& fileName, const std::string
return;
}
-void DMPComponent::storeDMPInDatabase(const std::string& dmpName, const std::string& dmpEntityName, const ::Ice::Current&)
+void
+DMPComponent::storeDMPInDatabase(const std::string& dmpName,
+ const std::string& dmpEntityName,
+ const ::Ice::Current&)
{
ARMARX_INFO << "storing DMP in the database";
@@ -332,10 +359,10 @@ void DMPComponent::storeDMPInDatabase(const std::string& dmpName, const std::str
dmpEntity->setId(entityID);
ARMARX_INFO << "successfully stored DMP";
-
}
-void DMPComponent::removeDMPFromDatabase(const std::string& dmpEntityName, const ::Ice::Current&)
+void
+DMPComponent::removeDMPFromDatabase(const std::string& dmpEntityName, const ::Ice::Current&)
{
ARMARX_INFO << "removing DMP from database";
@@ -346,7 +373,8 @@ void DMPComponent::removeDMPFromDatabase(const std::string& dmpEntityName, const
return;
}
- memoryx::DMPEntityPtr dmpEntity = memoryx::DMPEntityPtr::dynamicCast(dmpDataMemoryPrx->getDMPEntityByName(dmpEntityName));
+ memoryx::DMPEntityPtr dmpEntity =
+ memoryx::DMPEntityPtr::dynamicCast(dmpDataMemoryPrx->getDMPEntityByName(dmpEntityName));
dmpDataMemoryPrx->removeEntity(dmpEntity->getId());
if (!dmpDataMemoryPrx->hasEntityByName(dmpEntityName))
@@ -355,7 +383,8 @@ void DMPComponent::removeDMPFromDatabase(const std::string& dmpEntityName, const
}
}
-void DMPComponent::removeDMPFromDatabaseById(const std::string& dbId, const Ice::Current&)
+void
+DMPComponent::removeDMPFromDatabaseById(const std::string& dbId, const Ice::Current&)
{
ARMARX_INFO << "removing DMP from database";
@@ -365,7 +394,8 @@ void DMPComponent::removeDMPFromDatabaseById(const std::string& dbId, const Ice:
return;
}
- memoryx::DMPEntityPtr dmpEntity = memoryx::DMPEntityPtr::dynamicCast(dmpDataMemoryPrx->getDMPEntityById(dbId));
+ memoryx::DMPEntityPtr dmpEntity =
+ memoryx::DMPEntityPtr::dynamicCast(dmpDataMemoryPrx->getDMPEntityById(dbId));
dmpDataMemoryPrx->removeEntity(dmpEntity->getId());
if (!dmpDataMemoryPrx->hasEntityById(dbId))
@@ -374,9 +404,9 @@ void DMPComponent::removeDMPFromDatabaseById(const std::string& dbId, const Ice:
}
}
-
// DMP trainner
-void DMPComponent::trainDMP(const std::string& dmpName, const ::Ice::Current&)
+void
+DMPComponent::trainDMP(const std::string& dmpName, const ::Ice::Current&)
{
// learn dmp
ARMARX_INFO << "In Train DMP (name: " << dmpName << ")";
@@ -395,7 +425,11 @@ void DMPComponent::trainDMP(const std::string& dmpName, const ::Ice::Current&)
ARMARX_INFO << "Exit Train DMP (name: " << dmpName << ")";
}
-void DMPComponent::readTrajectoryFromFile(const std::string& dmpName, const std::string& file, double times, const Ice::Current&)
+void
+DMPComponent::readTrajectoryFromFile(const std::string& dmpName,
+ const std::string& file,
+ double times,
+ const Ice::Current&)
{
DMPInstanceBasePrx dmpPrx = findInstancePrx(dmpName);
@@ -409,12 +443,13 @@ void DMPComponent::readTrajectoryFromFile(const std::string& dmpName, const std:
ARMARX_ERROR << "readTrajectoryFromFile: DMP not found.";
return;
}
-
}
-
// convenient setter functions
-void DMPComponent::setDMPState(const std::string& dmpName, const ::armarx::cStateVec& state, const ::Ice::Current&)
+void
+DMPComponent::setDMPState(const std::string& dmpName,
+ const ::armarx::cStateVec& state,
+ const ::Ice::Current&)
{
DMPInstanceBasePrx dmpPrx = findInstancePrx(dmpName);
@@ -429,7 +464,8 @@ void DMPComponent::setDMPState(const std::string& dmpName, const ::armarx::cStat
}
}
-void DMPComponent::setAmpl(const std::string& dmpName, int dim, double value, const Ice::Current&)
+void
+DMPComponent::setAmpl(const std::string& dmpName, int dim, double value, const Ice::Current&)
{
DMPInstanceBasePrx dmpPrx = findInstancePrx(dmpName);
@@ -444,7 +480,8 @@ void DMPComponent::setAmpl(const std::string& dmpName, int dim, double value, co
}
}
-void DMPComponent::setGoal(const std::string& dmpName, const Ice::DoubleSeq& value, const Ice::Current&)
+void
+DMPComponent::setGoal(const std::string& dmpName, const Ice::DoubleSeq& value, const Ice::Current&)
{
DMPInstanceBasePrx dmpPrx = findInstancePrx(dmpName);
@@ -459,7 +496,10 @@ void DMPComponent::setGoal(const std::string& dmpName, const Ice::DoubleSeq& val
}
}
-void DMPComponent::setStartPosition(const std::string& dmpName, const Ice::DoubleSeq& value, const Ice::Current&)
+void
+DMPComponent::setStartPosition(const std::string& dmpName,
+ const Ice::DoubleSeq& value,
+ const Ice::Current&)
{
DMPInstanceBasePrx dmpPrx = findInstancePrx(dmpName);
@@ -473,7 +513,10 @@ void DMPComponent::setStartPosition(const std::string& dmpName, const Ice::Doubl
}
}
-void DMPComponent::setCanonicalValues(const std::string& dmpName, const Ice::DoubleSeq& value, const Ice::Current&)
+void
+DMPComponent::setCanonicalValues(const std::string& dmpName,
+ const Ice::DoubleSeq& value,
+ const Ice::Current&)
{
DMPInstanceBasePrx dmpPrx = findInstancePrx(dmpName);
@@ -488,7 +531,8 @@ void DMPComponent::setCanonicalValues(const std::string& dmpName, const Ice::Dou
}
}
-void DMPComponent::setTemporalFactor(const std::string& dmpName, double tau, const Ice::Current&)
+void
+DMPComponent::setTemporalFactor(const std::string& dmpName, double tau, const Ice::Current&)
{
DMPInstanceBasePrx dmpPrx = findInstancePrx(dmpName);
@@ -502,7 +546,8 @@ void DMPComponent::setTemporalFactor(const std::string& dmpName, double tau, con
}
}
-void DMPComponent::setTimeStep(double ts, const Ice::Current&)
+void
+DMPComponent::setTimeStep(double ts, const Ice::Current&)
{
timestep = ts;
for (DMPMap::iterator it = dmpPool.begin(); it != dmpPool.end(); it++)
@@ -512,7 +557,8 @@ void DMPComponent::setTimeStep(double ts, const Ice::Current&)
}
// convenient getter functions
-double DMPComponent::getAmpl(const std::string& dmpName, int dim, const Ice::Current&)
+double
+DMPComponent::getAmpl(const std::string& dmpName, int dim, const Ice::Current&)
{
DMPInstanceBasePrx dmpPrx = findInstancePrx(dmpName);
@@ -526,10 +572,12 @@ double DMPComponent::getAmpl(const std::string& dmpName, int dim, const Ice::Cur
ARMARX_ERROR << "getAmpl: DMP not found.";
return 0;
}
-
}
-cStateVec DMPComponent::getNextState(const std::string& dmpName, const cStateVec& states, const ::Ice::Current&)
+cStateVec
+DMPComponent::getNextState(const std::string& dmpName,
+ const cStateVec& states,
+ const ::Ice::Current&)
{
cStateVec nextState;
DMPInstanceBasePrx dmpPrx = findInstancePrx(dmpName);
@@ -548,7 +596,8 @@ cStateVec DMPComponent::getNextState(const std::string& dmpName, const cStateVec
return nextState;
}
-cStateVec DMPComponent::getCurrentState(const std::string& dmpName, const Ice::Current&)
+cStateVec
+DMPComponent::getCurrentState(const std::string& dmpName, const Ice::Current&)
{
cStateVec curState;
DMPInstanceBasePrx dmpPrx = findInstancePrx(dmpName);
@@ -565,7 +614,8 @@ cStateVec DMPComponent::getCurrentState(const std::string& dmpName, const Ice::C
return curState;
}
-Ice::DoubleSeq DMPComponent::getCanonicalValues(const std::string& dmpName, const Ice::Current&)
+Ice::DoubleSeq
+DMPComponent::getCanonicalValues(const std::string& dmpName, const Ice::Current&)
{
Ice::DoubleSeq canonicalValues;
@@ -583,7 +633,8 @@ Ice::DoubleSeq DMPComponent::getCanonicalValues(const std::string& dmpName, cons
return canonicalValues;
}
-double DMPComponent::getTemporalFactor(const std::string& dmpName, const Ice::Current&)
+double
+DMPComponent::getTemporalFactor(const std::string& dmpName, const Ice::Current&)
{
double temporalFactor = 0;
@@ -601,17 +652,17 @@ double DMPComponent::getTemporalFactor(const std::string& dmpName, const Ice::Cu
return temporalFactor;
}
-double DMPComponent::getCurrentTime(const Ice::Current&)
+double
+DMPComponent::getCurrentTime(const Ice::Current&)
{
DMPMap::iterator it = dmpPool.begin();
return it->second.first->getCurrentTime();
-
-
}
-double DMPComponent::getDampingFactor(const std::string& dmpName, const Ice::Current&)
+double
+DMPComponent::getDampingFactor(const std::string& dmpName, const Ice::Current&)
{
double dampingFactor = 0;
DMPInstanceBasePrx dmpPrx = findInstancePrx(dmpName);
@@ -627,7 +678,8 @@ double DMPComponent::getDampingFactor(const std::string& dmpName, const Ice::Cur
return dampingFactor;
}
-double DMPComponent::getSpringFactor(const std::string& dmpName, const Ice::Current&)
+double
+DMPComponent::getSpringFactor(const std::string& dmpName, const Ice::Current&)
{
double springFactor{0};
DMPInstanceBasePrx dmpPrx = findInstancePrx(dmpName);
@@ -643,7 +695,8 @@ double DMPComponent::getSpringFactor(const std::string& dmpName, const Ice::Curr
return springFactor;
}
-int DMPComponent::getDMPDim(const std::string& dmpName, const Ice::Current&)
+int
+DMPComponent::getDMPDim(const std::string& dmpName, const Ice::Current&)
{
int dim = -1;
DMPInstanceBasePrx dmpPrx = findInstancePrx(dmpName);
@@ -660,7 +713,8 @@ int DMPComponent::getDMPDim(const std::string& dmpName, const Ice::Current&)
return dim;
}
-string DMPComponent::getDMPType(const std::string& dmpName, const Ice::Current&)
+string
+DMPComponent::getDMPType(const std::string& dmpName, const Ice::Current&)
{
std::string dmpType;
DMPInstanceBasePrx dmpPrx = findInstancePrx(dmpName);
@@ -676,7 +730,8 @@ string DMPComponent::getDMPType(const std::string& dmpName, const Ice::Current&)
return dmpType;
}
-Ice::DoubleSeq DMPComponent::getStartPosition(const std::string& dmpName, const Ice::Current&)
+Ice::DoubleSeq
+DMPComponent::getStartPosition(const std::string& dmpName, const Ice::Current&)
{
Ice::DoubleSeq startPositions;
DMPInstanceBasePrx dmpPrx = findInstancePrx(dmpName);
@@ -692,7 +747,11 @@ Ice::DoubleSeq DMPComponent::getStartPosition(const std::string& dmpName, const
return startPositions;
}
-double DMPComponent::getForceTerm(const std::string& dmpName, const Ice::DoubleSeq& canonicalValues, int dim, const Ice::Current&)
+double
+DMPComponent::getForceTerm(const std::string& dmpName,
+ const Ice::DoubleSeq& canonicalValues,
+ int dim,
+ const Ice::Current&)
{
double forceterm{0};
DMPInstanceBasePrx dmpPrx = findInstancePrx(dmpName);
@@ -709,18 +768,23 @@ double DMPComponent::getForceTerm(const std::string& dmpName, const Ice::DoubleS
return forceterm;
}
-
-
-
-
-Vec2D DMPComponent::calcTrajectory(const std::string& dmpName, double startTime, double timeStep, double endTime, const Ice::DoubleSeq& goal, const cStateVec& states, const Ice::DoubleSeq& canonicalValues, double temporalFactor, const Ice::Current&)
+Vec2D
+DMPComponent::calcTrajectory(const std::string& dmpName,
+ double startTime,
+ double timeStep,
+ double endTime,
+ const Ice::DoubleSeq& goal,
+ const cStateVec& states,
+ const Ice::DoubleSeq& canonicalValues,
+ double temporalFactor,
+ const Ice::Current&)
{
DMPInstanceBasePrx dmpPrx = findInstancePrx(dmpName);
if (dmpPrx)
{
- return dmpPrx->calcTrajectory(startTime, timeStep, endTime, goal, states,
- canonicalValues, temporalFactor);
+ return dmpPrx->calcTrajectory(
+ startTime, timeStep, endTime, goal, states, canonicalValues, temporalFactor);
}
else
{
@@ -729,19 +793,18 @@ Vec2D DMPComponent::calcTrajectory(const std::string& dmpName, double startTime,
return {};
}
-
-
-void DMPComponent::resetTime(const Ice::Current&)
+void
+DMPComponent::resetTime(const Ice::Current&)
{
ctime = 0;
for (DMPMap::iterator it = dmpPool.begin(); it != dmpPool.end(); it++)
{
it->second.first->setCurrentTime(ctime);
}
-
}
-void DMPComponent::resetCanonicalValues(const Ice::Current&)
+void
+DMPComponent::resetCanonicalValues(const Ice::Current&)
{
for (DMPMap::iterator it = dmpPool.begin(); it != dmpPool.end(); it++)
{
@@ -757,14 +820,12 @@ void DMPComponent::resetCanonicalValues(const Ice::Current&)
canvals.push_back(1.0);
dmpPtr->setCanonicalValues(canvals);
}
-
}
-
}
-
// Trajectory related
-Ice::DoubleSeq DMPComponent::getTrajGoal(const std::string& dmpName, const Ice::Current&)
+Ice::DoubleSeq
+DMPComponent::getTrajGoal(const std::string& dmpName, const Ice::Current&)
{
Ice::DoubleSeq trajGoal;
DMPInstanceBasePrx dmpPrx = findInstancePrx(dmpName);
@@ -781,7 +842,8 @@ Ice::DoubleSeq DMPComponent::getTrajGoal(const std::string& dmpName, const Ice::
return trajGoal;
}
-cStateVec DMPComponent::getTrajStartState(const std::string& dmpName, const Ice::Current&)
+cStateVec
+DMPComponent::getTrajStartState(const std::string& dmpName, const Ice::Current&)
{
cStateVec trajStartState;
DMPInstanceBasePrx dmpPrx = findInstancePrx(dmpName);
@@ -796,13 +858,11 @@ cStateVec DMPComponent::getTrajStartState(const std::string& dmpName, const Ice:
}
return trajStartState;
-
}
-
-
// DMP manager
-SVector DMPComponent::getDMPNameList(const Ice::Current&)
+SVector
+DMPComponent::getDMPNameList(const Ice::Current&)
{
SVector nameList;
@@ -814,7 +874,8 @@ SVector DMPComponent::getDMPNameList(const Ice::Current&)
return nameList;
}
-void DMPComponent::eraseDMP(const std::string& dmpName, const Ice::Current&)
+void
+DMPComponent::eraseDMP(const std::string& dmpName, const Ice::Current&)
{
DMPMap::iterator dmpPoolIt = dmpPool.find(dmpName);
@@ -824,7 +885,8 @@ void DMPComponent::eraseDMP(const std::string& dmpName, const Ice::Current&)
}
}
-bool DMPComponent::isDMPExist(const std::string& dmpName, const Ice::Current&)
+bool
+DMPComponent::isDMPExist(const std::string& dmpName, const Ice::Current&)
{
if (dmpPool.find(dmpName) == dmpPool.end())
{
@@ -836,10 +898,9 @@ bool DMPComponent::isDMPExist(const std::string& dmpName, const Ice::Current&)
}
}
-
-
// local functions
-DMPInstanceBasePrx DMPComponent::createDMPInstancePrx(DMPInstancePtr dmpPtr)
+DMPInstanceBasePrx
+DMPComponent::createDMPInstancePrx(DMPInstancePtr dmpPtr)
{
Ice::ObjectAdapterPtr adapter = getObjectAdapter();
Ice::ObjectPrx obj = adapter->addWithUUID(dmpPtr);
@@ -852,4 +913,3 @@ DMPInstanceBasePrx DMPComponent::createDMPInstancePrx(DMPInstancePtr dmpPtr)
// return PropertyDefinitionsPtr(new DMPComponentPropertyDefinitions(
// getConfigIdentifier()));
//}
-
diff --git a/source/RobotComponents/components/DMPComponent/DMPComponent.h b/source/RobotComponents/components/DMPComponent/DMPComponent.h
index 2259a6c129b27f41d5bd3d34de18ad3691df305e..fa7788018fd0a6e75cd0a23f36bea08bbcf5bf43 100644
--- a/source/RobotComponents/components/DMPComponent/DMPComponent.h
+++ b/source/RobotComponents/components/DMPComponent/DMPComponent.h
@@ -25,26 +25,22 @@
#pragma once
-
-#include <RobotComponents/interface/components/DMPComponentBase.h>
-#include <MemoryX/interface/components/WorkingMemoryInterface.h>
-#include <MemoryX/interface/components/LongtermMemoryInterface.h>
-#include <MemoryX/libraries/memorytypes/entity/DMPEntity.h>
-
-#include <MemoryX/interface/memorytypes/MemoryEntities.h>
-#include <MemoryX/interface/memorytypes/MemorySegments.h>
-
-#include <boost/archive/text_oarchive.hpp>
+#include <boost/archive/binary_iarchive.hpp>
+#include <boost/archive/binary_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>
-
+#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/xml_iarchive.hpp>
#include <boost/archive/xml_oarchive.hpp>
+#include <boost/variant/get.hpp>
+#include <boost/variant/variant.hpp>
-#include <boost/archive/binary_iarchive.hpp>
-#include <boost/archive/binary_oarchive.hpp>
+#include <RobotComponents/interface/components/DMPComponentBase.h>
-#include <boost/variant/variant.hpp>
-#include <boost/variant/get.hpp>
+#include <MemoryX/interface/components/LongtermMemoryInterface.h>
+#include <MemoryX/interface/components/WorkingMemoryInterface.h>
+#include <MemoryX/interface/memorytypes/MemoryEntities.h>
+#include <MemoryX/interface/memorytypes/MemorySegments.h>
+#include <MemoryX/libraries/memorytypes/entity/DMPEntity.h>
#pragma GCC diagnostic ignored "-Wunknown-pragmas"
@@ -56,7 +52,6 @@
#include <filesystem>
-
#include <ArmarXCore/core/Component.h>
//#include "dmp/representation/dmp/dmpinterface.h"
//#include "dmp/representation/dmp/basicdmp.h"
@@ -81,15 +76,15 @@ namespace armarx
*/
- class DMPComponentPropertyDefinitions:
- public armarx::ComponentPropertyDefinitions
+ class DMPComponentPropertyDefinitions : public armarx::ComponentPropertyDefinitions
{
public:
- DMPComponentPropertyDefinitions(std::string prefix):
+ DMPComponentPropertyDefinitions(std::string prefix) :
armarx::ComponentPropertyDefinitions(prefix)
{
// defineRequiredProperty<std::string>("LongtermMemoryName", "Description");
- defineOptionalProperty<std::string>("LongtermMemoryName", "LongtermMemory", "Name of the LongtermMemory component");
+ defineOptionalProperty<std::string>(
+ "LongtermMemoryName", "LongtermMemory", "Name of the LongtermMemory component");
}
};
@@ -102,31 +97,26 @@ namespace armarx
* Detailed Description
*/
- using DMPInstancePair = std::pair<DMPInstancePtr, DMPInstanceBasePrx >;
- using DMPPair = std::pair<std::string, std::pair<DMPInstancePtr, DMPInstanceBasePrx> >;
- using DMPMap = std::map<std::string, std::pair<DMPInstancePtr, DMPInstanceBasePrx> >;
+ using DMPInstancePair = std::pair<DMPInstancePtr, DMPInstanceBasePrx>;
+ using DMPPair = std::pair<std::string, std::pair<DMPInstancePtr, DMPInstanceBasePrx>>;
+ using DMPMap = std::map<std::string, std::pair<DMPInstancePtr, DMPInstanceBasePrx>>;
/**
* @ingroup Component-DMPComponent
* @brief The DMPComponent class
*/
- class DMPComponent :
- virtual public Component,
- virtual public DMPComponentBase
+ class DMPComponent : virtual public Component, virtual public DMPComponentBase
{
public:
-
- DMPComponent():
- ctime(0.0),
- timestep(0.001)
+ DMPComponent() : ctime(0.0), timestep(0.001)
{
-
}
/**
* @see armarx::ManagedIceObject::getDefaultName()
*/
- std::string getDefaultName() const override
+ std::string
+ getDefaultName() const override
{
return "DMPComponent";
}
@@ -134,53 +124,92 @@ namespace armarx
/**
* @see PropertyUser::createPropertyDefinitions()
*/
- armarx::PropertyDefinitionsPtr createPropertyDefinitions() override
+ armarx::PropertyDefinitionsPtr
+ createPropertyDefinitions() override
{
- return armarx::PropertyDefinitionsPtr
- {
- new DMPComponentPropertyDefinitions{getConfigIdentifier()}
- };
+ return armarx::PropertyDefinitionsPtr{
+ new DMPComponentPropertyDefinitions{getConfigIdentifier()}};
}
- DMPInstanceBasePrx getDMP(const std::string& dmpName, const Ice::Current& = Ice::emptyCurrent) override;
+ DMPInstanceBasePrx getDMP(const std::string& dmpName,
+ const Ice::Current& = Ice::emptyCurrent) override;
// DMP Database related
- DMPInstanceBasePrx getDMPFromDatabase(const std::string& dmpEntityName, const std::string& dmpName = "UNKNOWN", const Ice::Current& = Ice::emptyCurrent) override;
- DMPInstanceBasePrx getDMPFromDatabaseById(const std::string& dbId, const Ice::Current&) override;
- DMPInstanceBasePrx getDMPFromFile(const std::string& fileName, const std::string& dmpName = "UNKNOWN", const Ice::Current& = Ice::emptyCurrent) override;
-
- void storeDMPInFile(const std::string& fileName, const std::string& dmpName, const Ice::Current&) override;
- void storeDMPInDatabase(const std::string& dmpName, const std::string& name, const ::Ice::Current& = Ice::emptyCurrent) override;
- void removeDMPFromDatabase(const std::string& name, const ::Ice::Current& = Ice::emptyCurrent) override;
+ DMPInstanceBasePrx getDMPFromDatabase(const std::string& dmpEntityName,
+ const std::string& dmpName = "UNKNOWN",
+ const Ice::Current& = Ice::emptyCurrent) override;
+ DMPInstanceBasePrx getDMPFromDatabaseById(const std::string& dbId,
+ const Ice::Current&) override;
+ DMPInstanceBasePrx getDMPFromFile(const std::string& fileName,
+ const std::string& dmpName = "UNKNOWN",
+ const Ice::Current& = Ice::emptyCurrent) override;
+
+ void storeDMPInFile(const std::string& fileName,
+ const std::string& dmpName,
+ const Ice::Current&) override;
+ void storeDMPInDatabase(const std::string& dmpName,
+ const std::string& name,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
+ void removeDMPFromDatabase(const std::string& name,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
void removeDMPFromDatabaseById(const std::string& dbId, const Ice::Current&) override;
//transmit data from client to server (using ice)
- DMPInstanceBasePrx instantiateDMP(const std::string& dmpName, const std::string& DMPType, int kernelSize, const ::Ice::Current& = Ice::emptyCurrent) override;
-
- void setDMPState(const std::string& dmpName, const ::armarx::cStateVec& state, const ::Ice::Current& = Ice::emptyCurrent) override;
- void setGoal(const std::string& dmpName, const Ice::DoubleSeq& value, const Ice::Current& = Ice::emptyCurrent) override;
- void setStartPosition(const std::string& dmpName, const Ice::DoubleSeq& value, const Ice::Current& = Ice::emptyCurrent) override;
- void setCanonicalValues(const std::string& dmpName, const Ice::DoubleSeq& value, const Ice::Current& = Ice::emptyCurrent) override;
-
- void readTrajectoryFromFile(const std::string& dmpName, const std::string& file, double times = 1, const ::Ice::Current& = Ice::emptyCurrent) override;
-
- void trainDMP(const std::string& dmpName, const ::Ice::Current& = Ice::emptyCurrent) override;
- void setAmpl(const std::string& dmpName, int dim, double value, const ::Ice::Current& = Ice::emptyCurrent) override;
- double getAmpl(const std::string& dmpName, int dim, const ::Ice::Current& = Ice::emptyCurrent) override;
-
-
- double getTemporalFactor(const std::string& dmpName, const ::Ice::Current& = Ice::emptyCurrent) override;
- void setTemporalFactor(const std::string& dmpName, double tau, const ::Ice::Current& = Ice::emptyCurrent) override;
-
-
- Vec2D calcTrajectory(const std::string& dmpName, double startTime, double timeStep, double endTime,
+ DMPInstanceBasePrx instantiateDMP(const std::string& dmpName,
+ const std::string& DMPType,
+ int kernelSize,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
+
+ void setDMPState(const std::string& dmpName,
+ const ::armarx::cStateVec& state,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
+ void setGoal(const std::string& dmpName,
+ const Ice::DoubleSeq& value,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ void setStartPosition(const std::string& dmpName,
+ const Ice::DoubleSeq& value,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ void setCanonicalValues(const std::string& dmpName,
+ const Ice::DoubleSeq& value,
+ const Ice::Current& = Ice::emptyCurrent) override;
+
+ void readTrajectoryFromFile(const std::string& dmpName,
+ const std::string& file,
+ double times = 1,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
+
+ void trainDMP(const std::string& dmpName,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
+ void setAmpl(const std::string& dmpName,
+ int dim,
+ double value,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
+ double getAmpl(const std::string& dmpName,
+ int dim,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
+
+
+ double getTemporalFactor(const std::string& dmpName,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
+ void setTemporalFactor(const std::string& dmpName,
+ double tau,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
+
+
+ Vec2D calcTrajectory(const std::string& dmpName,
+ double startTime,
+ double timeStep,
+ double endTime,
const ::Ice::DoubleSeq& goal,
const cStateVec& states,
- const ::Ice::DoubleSeq& canonicalValues, double temporalFactor, const ::Ice::Current& = Ice::emptyCurrent) override;
+ const ::Ice::DoubleSeq& canonicalValues,
+ double temporalFactor,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
// time manager
- double getTimeStep(const ::Ice::Current& = Ice::emptyCurrent) override
+ double
+ getTimeStep(const ::Ice::Current& = Ice::emptyCurrent) override
{
return timestep;
}
@@ -194,28 +223,44 @@ namespace armarx
void resetCanonicalValues(const ::Ice::Current& = Ice::emptyCurrent) override;
- double getDampingFactor(const std::string& dmpName, const ::Ice::Current& = Ice::emptyCurrent) override;
+ double getDampingFactor(const std::string& dmpName,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
- double getSpringFactor(const std::string& dmpName, const ::Ice::Current& = Ice::emptyCurrent) override;
+ double getSpringFactor(const std::string& dmpName,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
- double getForceTerm(const std::string& dmpName, const Ice::DoubleSeq& canonicalValues, int dim, const Ice::Current& = Ice::emptyCurrent) override;
+ double getForceTerm(const std::string& dmpName,
+ const Ice::DoubleSeq& canonicalValues,
+ int dim,
+ const Ice::Current& = Ice::emptyCurrent) override;
//transmit data from server to client (using ice)
- cStateVec getNextState(const std::string& dmpName, const cStateVec& states, const ::Ice::Current& = Ice::emptyCurrent) override;
- cStateVec getCurrentState(const std::string& dmpName, const ::Ice::Current& = Ice::emptyCurrent) override;
- Ice::DoubleSeq getCanonicalValues(const std::string& dmpName, const ::Ice::Current& = Ice::emptyCurrent) override;
- Ice::DoubleSeq getTrajGoal(const std::string& dmpName, const ::Ice::Current& = Ice::emptyCurrent) override;
- ::armarx::cStateVec getTrajStartState(const std::string& dmpName, const ::Ice::Current& = Ice::emptyCurrent) override;
-
- std::string getDMPType(const std::string& dmpName, const ::Ice::Current& = Ice::emptyCurrent) override;
-
- Ice::DoubleSeq getStartPosition(const std::string& dmpName, const ::Ice::Current& = Ice::emptyCurrent) override;
+ cStateVec getNextState(const std::string& dmpName,
+ const cStateVec& states,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
+ cStateVec getCurrentState(const std::string& dmpName,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
+ Ice::DoubleSeq getCanonicalValues(const std::string& dmpName,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
+ Ice::DoubleSeq getTrajGoal(const std::string& dmpName,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
+ ::armarx::cStateVec getTrajStartState(const std::string& dmpName,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
+
+ std::string getDMPType(const std::string& dmpName,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
+
+ Ice::DoubleSeq getStartPosition(const std::string& dmpName,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
SVector getDMPNameList(const ::Ice::Current& = Ice::emptyCurrent) override;
- void eraseDMP(const std::string& dmpName, const ::Ice::Current& = Ice::emptyCurrent) override;
- bool isDMPExist(const std::string& dmpName, const ::Ice::Current& = Ice::emptyCurrent) override;
- int getDMPDim(const std::string& dmpName, const ::Ice::Current& = Ice::emptyCurrent) override;
+ void eraseDMP(const std::string& dmpName,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
+ bool isDMPExist(const std::string& dmpName,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
+ int getDMPDim(const std::string& dmpName,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
double ctime;
@@ -258,8 +303,6 @@ namespace armarx
*/
// virtual armarx::PropertyDefinitionsPtr createPropertyDefinitions();
};
- using DMPComponentPtr = ::IceInternal::Handle< ::armarx::DMPComponent>;
-}
-
-
+ using DMPComponentPtr = ::IceInternal::Handle<::armarx::DMPComponent>;
+} // namespace armarx
diff --git a/source/RobotComponents/components/DMPComponent/DMPInstance.cpp b/source/RobotComponents/components/DMPComponent/DMPInstance.cpp
index 69813c90c2c35352244e72994e889a057546f8d9..59748b4848712f7455b0f948e06fb322aedfe1c0 100644
--- a/source/RobotComponents/components/DMPComponent/DMPInstance.cpp
+++ b/source/RobotComponents/components/DMPComponent/DMPInstance.cpp
@@ -22,13 +22,16 @@
#include "DMPInstance.h"
-#include <dmp/io/MMMConverter.h>
+
#include <Ice/ObjectAdapter.h>
+#include <dmp/io/MMMConverter.h>
+
using namespace armarx;
// tool functions
-DMP::Vec<DMP::DMPState> getDMPStateFromcStateVec(const cStateVec& state)
+DMP::Vec<DMP::DMPState>
+getDMPStateFromcStateVec(const cStateVec& state)
{
DMP::Vec<DMP::DMPState> res;
res.resize(state.size());
@@ -42,7 +45,8 @@ DMP::Vec<DMP::DMPState> getDMPStateFromcStateVec(const cStateVec& state)
return res;
}
-DMP::DVec2d getDVec2dStateFromcStateVec(const cStateVec& state)
+DMP::DVec2d
+getDVec2dStateFromcStateVec(const cStateVec& state)
{
DMP::DVec2d res;
res.resize(state.size());
@@ -55,7 +59,8 @@ DMP::DVec2d getDVec2dStateFromcStateVec(const cStateVec& state)
return res;
}
-armarx::cStateVec getcStateVec(const DMP::Vec<DMP::DMPState>& dmpstate)
+armarx::cStateVec
+getcStateVec(const DMP::Vec<DMP::DMPState>& dmpstate)
{
armarx::cStateVec sv;
sv.resize(dmpstate.size());
@@ -69,7 +74,8 @@ armarx::cStateVec getcStateVec(const DMP::Vec<DMP::DMPState>& dmpstate)
return sv;
}
-armarx::cStateVec getcStateVec(const DMP::DVec2d& dmpstate)
+armarx::cStateVec
+getcStateVec(const DMP::DVec2d& dmpstate)
{
armarx::cStateVec sv;
sv.resize(dmpstate.size());
@@ -83,7 +89,6 @@ armarx::cStateVec getcStateVec(const DMP::DVec2d& dmpstate)
return sv;
}
-
DMPInstance::DMPInstance(DMP::DMPInterfacePtr dmpInter, std::string DMPType)
{
dmp = dmpInter;
@@ -94,7 +99,8 @@ DMPInstance::DMPInstance(DMP::DMPInterfacePtr dmpInter, std::string DMPType)
// state calculation
-cStateVec DMPInstance::getNextState(const cStateVec& states, const Ice::Current&)
+cStateVec
+DMPInstance::getNextState(const cStateVec& states, const Ice::Current&)
{
if (canonicalValues.size() == 0)
{
@@ -105,28 +111,35 @@ cStateVec DMPInstance::getNextState(const cStateVec& states, const Ice::Current&
DMP::DVec2d currentDMPState = getDVec2dStateFromcStateVec(states);
if (currentDMPState.size() == 0)
{
- ARMARX_ERROR << "The current state is not available. Please specify current state with setDMPState().";
+ ARMARX_ERROR << "The current state is not available. Please specify current state with "
+ "setDMPState().";
}
DMP::DVec goal = dmp->getGoals();
- ARMARX_INFO << "CanonicalValues: " << canonicalValues[0]
- << " ctime: " << ctime
+ ARMARX_INFO << "CanonicalValues: " << canonicalValues[0] << " ctime: " << ctime
<< " Timestep: " << timestep;
double temporalFactor = dmp->getTemporalFactor();
- currentDMPState = dmp->calculateTrajectoryPointBase(ctime + timestep, goal, ctime,
- currentDMPState, canonicalValues, temporalFactor);
+ currentDMPState = dmp->calculateTrajectoryPointBase(
+ ctime + timestep, goal, ctime, currentDMPState, canonicalValues, temporalFactor);
ctime += timestep;
- cStateVec nextState = getcStateVec(currentDMPState);
+ cStateVec nextState = getcStateVec(currentDMPState);
setDMPState(nextState);
ARMARX_INFO << "Got next state";
return nextState;
}
-armarx::nStateValues DMPInstance::calcNextState(double t, const Ice::DoubleSeq& goal, double currentT, const Vec2D& currentStates, const Ice::DoubleSeq& canonicalValues, double temporalFactor, const ::Ice::Current&)
+armarx::nStateValues
+DMPInstance::calcNextState(double t,
+ const Ice::DoubleSeq& goal,
+ double currentT,
+ const Vec2D& currentStates,
+ const Ice::DoubleSeq& canonicalValues,
+ double temporalFactor,
+ const ::Ice::Current&)
{
if (canonicalValues.size() == 0)
{
@@ -145,21 +158,21 @@ armarx::nStateValues DMPInstance::calcNextState(double t, const Ice::DoubleSeq&
{
ARMARX_WARNING << "Using goal specified in configs";
goal_v = getGoal();
-
}
DMP::DVec2d curStates;
- for (std::size_t i = 0; i < currentStates.size() ; ++i)
+ for (std::size_t i = 0; i < currentStates.size(); ++i)
{
curStates.emplace_back(DMP::DVec(currentStates.at(i)));
}
DMP::DVec canValues = DMP::DVec(canonicalValues);
ARMARX_INFO_S << VAROUT(temporalFactor);
- DMP::DVec2d tmp_result = dmp->calculateTrajectoryPointBase(t, goal_v, currentT, curStates, canValues, temporalFactor);
+ DMP::DVec2d tmp_result = dmp->calculateTrajectoryPointBase(
+ t, goal_v, currentT, curStates, canValues, temporalFactor);
- Ice::DoubleSeq tmpCanVal;
+ Ice::DoubleSeq tmpCanVal;
for (std::size_t i = 0; i < canValues.size(); i++)
{
tmpCanVal.emplace_back(canValues.at(i));
@@ -170,7 +183,7 @@ armarx::nStateValues DMPInstance::calcNextState(double t, const Ice::DoubleSeq&
result.canonicalValues = tmpCanVal;
Vec2D nState;
- for (std::size_t i = 0; i < tmp_result.size() ; ++i)
+ for (std::size_t i = 0; i < tmp_result.size(); ++i)
{
::Ice::DoubleSeq dblseq = ::Ice::DoubleSeq(tmp_result.at(i));
@@ -182,29 +195,36 @@ armarx::nStateValues DMPInstance::calcNextState(double t, const Ice::DoubleSeq&
return result;
}
-armarx::nStateValues DMPInstance::calcNextStateFromConfig(double t, double currentT, const Vec2D& currentStates, const Ice::DoubleSeq& canonicalValues, const Ice::Current&)
+armarx::nStateValues
+DMPInstance::calcNextStateFromConfig(double t,
+ double currentT,
+ const Vec2D& currentStates,
+ const Ice::DoubleSeq& canonicalValues,
+ const Ice::Current&)
{
DMP::DVec goal = getGoal();
double temporalFactor = getTemporalFactor();
return calcNextState(t, goal, currentT, currentStates, canonicalValues, temporalFactor);
}
-
-
// configuration & parameters
-void DMPInstance::setConfigurationMap(const DMPConfigMap& conf, const Ice::Current&)
+void
+DMPInstance::setConfigurationMap(const DMPConfigMap& conf, const Ice::Current&)
{
- std::map<std::string, paraType > configMap;
- for (::std::map< ::std::string, ::Ice::DoubleSeq>::const_iterator it = conf.begin(); it != conf.end(); ++it)
+ std::map<std::string, paraType> configMap;
+ for (::std::map<::std::string, ::Ice::DoubleSeq>::const_iterator it = conf.begin();
+ it != conf.end();
+ ++it)
{
- configMap[it->first] = (DMP::DVec) it->second;
+ configMap[it->first] = (DMP::DVec)it->second;
dmp->setConfiguration(it->first, it->second);
}
configs = configMap;
}
-configMap DMPInstance::createConfigMap(DMP::Vec<std::string> paraIDs, DMP::Vec<paraType> paraVals)
+configMap
+DMPInstance::createConfigMap(DMP::Vec<std::string> paraIDs, DMP::Vec<paraType> paraVals)
{
if (paraIDs.size() != paraVals.size())
{
@@ -227,7 +247,10 @@ configMap DMPInstance::createConfigMap(DMP::Vec<std::string> paraIDs, DMP::Vec<p
return configs;
}
-void DMPInstance::setParameter(const std::string& paraID, const Ice::DoubleSeq& value, const ::Ice::Current&)
+void
+DMPInstance::setParameter(const std::string& paraID,
+ const Ice::DoubleSeq& value,
+ const ::Ice::Current&)
{
if (configs.find(paraID) == configs.end())
{
@@ -242,22 +265,26 @@ void DMPInstance::setParameter(const std::string& paraID, const Ice::DoubleSeq&
// setter functions
-void DMPInstance::setCanonicalValues(const Ice::DoubleSeq& value, const Ice::Current&)
+void
+DMPInstance::setCanonicalValues(const Ice::DoubleSeq& value, const Ice::Current&)
{
canonicalValues = value;
}
-void DMPInstance::setAmpl(int dim, double value, const Ice::Current&)
+void
+DMPInstance::setAmpl(int dim, double value, const Ice::Current&)
{
dmp->setAmpl(dim, value);
}
-void DMPInstance::setTemporalFactor(double value, const Ice::Current&)
+void
+DMPInstance::setTemporalFactor(double value, const Ice::Current&)
{
dmp->setTemporalFactor(value);
}
-void DMPInstance::setGoal(const Ice::DoubleSeq& value, const Ice::Current&)
+void
+DMPInstance::setGoal(const Ice::DoubleSeq& value, const Ice::Current&)
{
if (configs.find(DMP::Goal) == configs.end())
{
@@ -270,7 +297,8 @@ void DMPInstance::setGoal(const Ice::DoubleSeq& value, const Ice::Current&)
dmp->setConfiguration(DMP::Goal, DMP::DVec(value));
}
-void DMPInstance::setStartPosition(const Ice::DoubleSeq& value, const Ice::Current&)
+void
+DMPInstance::setStartPosition(const Ice::DoubleSeq& value, const Ice::Current&)
{
if (configs.find(DMP::StartPosition) == configs.end())
{
@@ -283,30 +311,34 @@ void DMPInstance::setStartPosition(const Ice::DoubleSeq& value, const Ice::Curre
dmp->setConfiguration(DMP::StartPosition, DMP::DVec(value));
}
-void DMPInstance::setDMPState(const cStateVec& state, const Ice::Current&)
+void
+DMPInstance::setDMPState(const cStateVec& state, const Ice::Current&)
{
dmpState = state;
}
-
// getter function
-double DMPInstance::getAmpl(int dim, const Ice::Current&)
+double
+DMPInstance::getAmpl(int dim, const Ice::Current&)
{
return dmp->getAmpl(dim);
}
-double DMPInstance::getTemporalFactor(const Ice::Current&)
+double
+DMPInstance::getTemporalFactor(const Ice::Current&)
{
return dmp->getTemporalFactor();
}
-double DMPInstance::getForceTerm(const Ice::DoubleSeq& canVal, int dim, const Ice::Current&)
+double
+DMPInstance::getForceTerm(const Ice::DoubleSeq& canVal, int dim, const Ice::Current&)
{
return dmp->_getPerturbationForce(dim, canVal[0]);
}
-Ice::DoubleSeq DMPInstance::getGoal(const Ice::Current&)
+Ice::DoubleSeq
+DMPInstance::getGoal(const Ice::Current&)
{
DMP::DVec goals = dmp->getGoals();
Ice::DoubleSeq res;
@@ -321,7 +353,8 @@ Ice::DoubleSeq DMPInstance::getGoal(const Ice::Current&)
// trajectory related
-Ice::DoubleSeq DMPInstance::getTrajGoal(const ::Ice::Current&)
+Ice::DoubleSeq
+DMPInstance::getTrajGoal(const ::Ice::Current&)
{
Ice::DoubleSeq trajGoal;
for (size_t i = 0; i < trajs[0].dim(); i++)
@@ -330,10 +363,10 @@ Ice::DoubleSeq DMPInstance::getTrajGoal(const ::Ice::Current&)
}
return trajGoal;
-
}
-cStateVec DMPInstance::getTrajStartState(const ::Ice::Current&)
+cStateVec
+DMPInstance::getTrajStartState(const ::Ice::Current&)
{
cStateVec startState;
@@ -350,7 +383,8 @@ cStateVec DMPInstance::getTrajStartState(const ::Ice::Current&)
return startState;
}
-void DMPInstance::readTrajectoryFromFile(const std::string& file, double times, const Ice::Current&)
+void
+DMPInstance::readTrajectoryFromFile(const std::string& file, double times, const Ice::Current&)
{
std::string ext = file.rfind(".") == file.npos ? file : file.substr(file.rfind(".") + 1);
@@ -375,11 +409,9 @@ void DMPInstance::readTrajectoryFromFile(const std::string& file, double times,
dmp->setDim(traj.dim());
ARMARX_INFO << "csv file loaded";
-
}
else if (ext == "vsg")
{
-
}
else
{
@@ -388,16 +420,21 @@ void DMPInstance::readTrajectoryFromFile(const std::string& file, double times,
}
// DMP Trainer
-void DMPInstance::trainDMP(const Ice::Current&)
+void
+DMPInstance::trainDMP(const Ice::Current&)
{
dmp->learnFromTrajectories(trajs);
}
-
-Vec2D DMPInstance::calcTrajectory(double startTime, double timeStep, double endTime,
- const ::Ice::DoubleSeq& goal,
- const cStateVec& states,
- const ::Ice::DoubleSeq& canonicalValues, double temporalFactor, const Ice::Current&)
+Vec2D
+DMPInstance::calcTrajectory(double startTime,
+ double timeStep,
+ double endTime,
+ const ::Ice::DoubleSeq& goal,
+ const cStateVec& states,
+ const ::Ice::DoubleSeq& canonicalValues,
+ double temporalFactor,
+ const Ice::Current&)
{
setDMPState(states);
setGoal(goal);
@@ -426,14 +463,15 @@ Vec2D DMPInstance::calcTrajectory(double startTime, double timeStep, double endT
setCurrentTime(0);
return resultingTrajectory;
-
}
-
-Vec2D DMPInstance::calcTrajectoryV2(const Ice::DoubleSeq& timestamps,
- const Ice::DoubleSeq& goal,
- const Vec2D& initStates,
- const Ice::DoubleSeq& canonicalValues, double temporalFactor, const Ice::Current&)
+Vec2D
+DMPInstance::calcTrajectoryV2(const Ice::DoubleSeq& timestamps,
+ const Ice::DoubleSeq& goal,
+ const Vec2D& initStates,
+ const Ice::DoubleSeq& canonicalValues,
+ double temporalFactor,
+ const Ice::Current&)
{
DMP::DVec timestampsLocal = timestamps;
@@ -454,7 +492,8 @@ Vec2D DMPInstance::calcTrajectoryV2(const Ice::DoubleSeq& timestamps,
for (; it != timestamps.end(); it++)
{
- nStateValues nState = calcNextState(*it, goal, prevTimestamp, previousState, canValues, temporalFactor);
+ nStateValues nState =
+ calcNextState(*it, goal, prevTimestamp, previousState, canValues, temporalFactor);
canValues = nState.canonicalValues;
previousState = nState.nextState;
@@ -470,13 +509,13 @@ Vec2D DMPInstance::calcTrajectoryV2(const Ice::DoubleSeq& timestamps,
return resultingTrajectory;
}
-Vec2D DMPInstance::calcTrajectoryFromConfig(const Ice::DoubleSeq& timestamps,
- const Vec2D& initStates,
- const Ice::DoubleSeq& canonicalValues, const Ice::Current&)
+Vec2D
+DMPInstance::calcTrajectoryFromConfig(const Ice::DoubleSeq& timestamps,
+ const Vec2D& initStates,
+ const Ice::DoubleSeq& canonicalValues,
+ const Ice::Current&)
{
DMP::DVec goal = getGoal();
double temporalFactor = getTemporalFactor();
return calcTrajectoryV2(timestamps, goal, initStates, canonicalValues, temporalFactor);
}
-
-
diff --git a/source/RobotComponents/components/DMPComponent/DMPInstance.h b/source/RobotComponents/components/DMPComponent/DMPInstance.h
index 75cf7bf8717903247ad76505aa1a11537081d899..aa9fabe9dee4fff1d71798cb02fe356d8593c414 100644
--- a/source/RobotComponents/components/DMPComponent/DMPInstance.h
+++ b/source/RobotComponents/components/DMPComponent/DMPInstance.h
@@ -22,31 +22,29 @@
#pragma once
-#include <MMM/MMMCore.h>
-
-#include <dmp/representation/dmp/dmpregistration.h>
+#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/logging/Logging.h>
#include <RobotComponents/interface/components/DMPComponentBase.h>
-#include <ArmarXCore/core/Component.h>
-#include <ArmarXCore/core/logging/Logging.h>
+#include <MMM/MMMCore.h>
+#include <dmp/representation/dmp/dmpregistration.h>
//#include <dmp/representation/dmp/dmpregistration.h>
-#include "dmp/representation/dmp/dmpinterface.h"
+#include "dmp/representation/dmp/adaptive3rdorderdmp.h"
#include "dmp/representation/dmp/basicdmp.h"
#include "dmp/representation/dmp/dmp3rdorder.h"
-#include "dmp/representation/dmp/quaterniondmp.h"
-#include "dmp/representation/dmp/endvelodmp.h"
#include "dmp/representation/dmp/dmp3rdorderforcefield.h"
-#include "dmp/representation/dmp/forcefielddmp.h"
-#include "dmp/representation/dmp/adaptive3rdorderdmp.h"
-#include "dmp/representation/dmp/simpleendvelodmp.h"
+#include "dmp/representation/dmp/dmpinterface.h"
+#include "dmp/representation/dmp/endvelodmp.h"
#include "dmp/representation/dmp/endveloforcefielddmp.h"
#include "dmp/representation/dmp/endveloforcefieldwithobjrepulsiondmp.h"
+#include "dmp/representation/dmp/forcefielddmp.h"
#include "dmp/representation/dmp/periodicdmp.h"
+#include "dmp/representation/dmp/quaterniondmp.h"
+#include "dmp/representation/dmp/simpleendvelodmp.h"
#include "dmp/representation/dmp/taskspacedmp.h"
-
namespace armarx
{
@@ -56,20 +54,20 @@ namespace armarx
#define ARMARX_DMPTYPE_FORCEFIELDDMP boost::serialization::guid<DMP::ForceFieldDMP>()
#define ARMARX_DMPTYPE_ENDVELFORCEFILELDDMP boost::serialization::guid<DMP::EndVeloForceFieldDMP>()
#define ARMARX_DMPTYPE_DMP3RDORDER boost::serialization::guid<DMP::DMP3rdOrder>()
-#define ARMARX_DMPTYPE_DMP3RDORDERFORCEFIELD boost::serialization::guid<DMP::DMP3rdOrderForceField>()
-#define ARMARX_DMPTYPE_ADAPTIVEGOAL3RDORDERDMP boost::serialization::guid<DMP::AdaptiveGoal3rdOrderDMP>()
+#define ARMARX_DMPTYPE_DMP3RDORDERFORCEFIELD \
+ boost::serialization::guid<DMP::DMP3rdOrderForceField>()
+#define ARMARX_DMPTYPE_ADAPTIVEGOAL3RDORDERDMP \
+ boost::serialization::guid<DMP::AdaptiveGoal3rdOrderDMP>()
#define ARMARX_DMPTYPE_QUATERNIONDMP boost::serialization::guid<DMP::QuaternionDMP>()
#define ARMARX_DMPTYPE_PERIODICDMP boost::serialization::guid<DMP::PeriodicDMP>()
#define ARMARX_DMPTYPE_TASKSPACEDMP boost::serialization::guid<DMP::TaskSpaceDMP>()
using paraType = boost::variant<double, DMP::DVec, Eigen::Quaternionf>;
- using configMap = std::map<std::string, paraType >;
- using configPair = std::pair<std::string, paraType >;
+ using configMap = std::map<std::string, paraType>;
+ using configPair = std::pair<std::string, paraType>;
using TrajVec = DMP::Vec<DMP::SampledTrajectoryV2>;
-
-
/**
* @defgroup DMPInstance DMPInstance
* @ingroup RobotComponents-Components
@@ -79,22 +77,18 @@ namespace armarx
* The Instance manages the Configuration of the DMP and can calculate the states of the DMP and the whole Trajectory for all dimensions for different start and target positions.
*
*/
- class DMPInstance :
- virtual public armarx::Logging,
- virtual public DMPInstanceBase
+ class DMPInstance : virtual public armarx::Logging, virtual public DMPInstanceBase
{
public:
/**
* @brief DMPInstance constructs an empty instance of a basicDMP
*/
- DMPInstance():
- dmpType(ARMARX_DMPTYPE_BASICDMP),
- dmp(new DMP::BasicDMP()),
- ctime(0),
- timestep(0.001)
+ DMPInstance() :
+ dmpType(ARMARX_DMPTYPE_BASICDMP), dmp(new DMP::BasicDMP()), ctime(0), timestep(0.001)
- {}
+ {
+ }
DMPInstance(DMP::DMPInterfacePtr dmpInter)
{
@@ -117,37 +111,44 @@ namespace armarx
* @param paraId pointer to the dmp
* @param value type of dmp
*/
- void setParameter(const std::string& paraId, const Ice::DoubleSeq& value, const ::Ice::Current& = Ice::emptyCurrent) override;
+ void setParameter(const std::string& paraId,
+ const Ice::DoubleSeq& value,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
/**
* @brief setGoal set the goal for dmp
* @param value the vector used to specify goal vector
*/
- void setGoal(const Ice::DoubleSeq& value, const ::Ice::Current& = Ice::emptyCurrent) override;
+ void setGoal(const Ice::DoubleSeq& value,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
/**
* @brief setStartPosition set the start position for dmp
* @param value: the vector used to define start position
*/
- void setStartPosition(const Ice::DoubleSeq& value, const ::Ice::Current& = Ice::emptyCurrent) override;
+ void setStartPosition(const Ice::DoubleSeq& value,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
/**
* @brief setDMPState set current state for dmp
* @param state state to set
*/
- void setDMPState(const cStateVec& state, const ::Ice::Current& = Ice::emptyCurrent) override;
+ void setDMPState(const cStateVec& state,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
/**
* @brief setConfigurationMap set dmp configurations using configMap
* @param value: configMap saves all possible configurations for dmp
*/
- void setConfigurationMap(const DMPConfigMap& value, const ::Ice::Current& = Ice::emptyCurrent) override;
+ void setConfigurationMap(const DMPConfigMap& value,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
/**
* @brief setCanonicalValues: set dmp canonicalValues to control the dmp's state
* @param value: current canonical values needed
*/
- void setCanonicalValues(const Ice::DoubleSeq& value, const ::Ice::Current& = Ice::emptyCurrent) override;
+ void setCanonicalValues(const Ice::DoubleSeq& value,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
/**
* @brief setTemporalFactor: set dmp temporal factor
@@ -174,7 +175,9 @@ namespace armarx
* @param canVal: corresponding canonical value
* @param dim: the dimension concerned
*/
- double getForceTerm(const Ice::DoubleSeq& canVal, int dim, const ::Ice::Current& = Ice::emptyCurrent) override;
+ double getForceTerm(const Ice::DoubleSeq& canVal,
+ int dim,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
/**
* @brief getTemporalFactor: get current temporal factor
@@ -184,7 +187,8 @@ namespace armarx
/**
* @brief getCanonicalValues: get current canonical values
*/
- Ice::DoubleSeq getCanonicalValues(const ::Ice::Current& = Ice::emptyCurrent) override
+ Ice::DoubleSeq
+ getCanonicalValues(const ::Ice::Current& = Ice::emptyCurrent) override
{
return canonicalValues;
}
@@ -192,7 +196,8 @@ namespace armarx
/**
* @brief getDampingFactor: get dmp's damping factor
*/
- double getDampingFactor(const ::Ice::Current& = Ice::emptyCurrent) override
+ double
+ getDampingFactor(const ::Ice::Current& = Ice::emptyCurrent) override
{
return dmp->getDampingFactor();
}
@@ -200,7 +205,8 @@ namespace armarx
/**
* @brief getSpringFactor: get dmp's spring factor
*/
- double getSpringFactor(const ::Ice::Current& = Ice::emptyCurrent) override
+ double
+ getSpringFactor(const ::Ice::Current& = Ice::emptyCurrent) override
{
return dmp->getSpringFactor();
}
@@ -208,7 +214,8 @@ namespace armarx
/**
* @brief getDMPType: get dmp type
*/
- std::string getDMPType(const ::Ice::Current& = Ice::emptyCurrent) override
+ std::string
+ getDMPType(const ::Ice::Current& = Ice::emptyCurrent) override
{
return dmp->getDMPType();
}
@@ -216,7 +223,8 @@ namespace armarx
/**
* @brief getDMPDim: get dimesions of dmp
*/
- int getDMPDim(const ::Ice::Current& = Ice::emptyCurrent) override
+ int
+ getDMPDim(const ::Ice::Current& = Ice::emptyCurrent) override
{
return dmp->dim();
}
@@ -224,7 +232,8 @@ namespace armarx
/**
* @brief getStartPosition: get start position of dmp
*/
- Ice::DoubleSeq getStartPosition(const ::Ice::Current& = Ice::emptyCurrent) override
+ Ice::DoubleSeq
+ getStartPosition(const ::Ice::Current& = Ice::emptyCurrent) override
{
return dmp->getStartPositions();
}
@@ -245,26 +254,32 @@ namespace armarx
// DMP trainer
void trainDMP(const ::Ice::Current& = Ice::emptyCurrent) override;
- void readTrajectoryFromFile(const std::string& file, double times = 1, const ::Ice::Current& = Ice::emptyCurrent) override;
-
+ void readTrajectoryFromFile(const std::string& file,
+ double times = 1,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
// dmp calculation
/**
* @brief getNextState: get next state according to the state given
*/
- cStateVec getNextStateWithCurrentState(const ::Ice::Current& = Ice::emptyCurrent) override
+ cStateVec
+ getNextStateWithCurrentState(const ::Ice::Current& = Ice::emptyCurrent) override
{
return getNextState(getCurrentState());
}
+
/**
* @brief getNextState: get next state according to the state given
* @param states: the current state given to the function
*/
- cStateVec getNextState(const cStateVec& states, const ::Ice::Current& = Ice::emptyCurrent) override;
+ cStateVec getNextState(const cStateVec& states,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
+
/**
* @brief getCurrentState: get current state in dmp
*/
- cStateVec getCurrentState(const ::Ice::Current& = Ice::emptyCurrent) override
+ cStateVec
+ getCurrentState(const ::Ice::Current& = Ice::emptyCurrent) override
{
return dmpState;
}
@@ -282,39 +297,67 @@ namespace armarx
* trajectory. The duration scales linear with this factor.
* @return The Trajectory which is created by the DMP, containing all the States of the DMP for each timestep
*/
- Vec2D calcTrajectory(double startTime, double timestep, double endTime,
- const ::Ice::DoubleSeq& goal, const cStateVec& initStates, const ::Ice::DoubleSeq& canonicalValues, double temporalFactor, const Ice::Current& = Ice::emptyCurrent) override;
-
-
+ Vec2D calcTrajectory(double startTime,
+ double timestep,
+ double endTime,
+ const ::Ice::DoubleSeq& goal,
+ const cStateVec& initStates,
+ const ::Ice::DoubleSeq& canonicalValues,
+ double temporalFactor,
+ const Ice::Current& = Ice::emptyCurrent) override;
// local functions
configMap createConfigMap(DMP::Vec<std::string> paraIDs, DMP::Vec<paraType> paraVals);
- DMP::DMPInterfacePtr getDMP()
+ DMP::DMPInterfacePtr
+ getDMP()
{
return dmp;
}
// time manager
- void setTimeStep(double ts)
+ void
+ setTimeStep(double ts)
{
timestep = ts;
}
- void setCurrentTime(double t)
+ void
+ setCurrentTime(double t)
{
ctime = t;
}
- double getCurrentTime()
+ double
+ getCurrentTime()
{
return ctime;
}
- nStateValues calcNextStateFromConfig(double t, double currentT, const Vec2D& currentStates, const Ice::DoubleSeq& canonicalValues, const Ice::Current& = Ice::emptyCurrent) override;
- nStateValues calcNextState(double t, const Ice::DoubleSeq& goal, double currentT, const Vec2D& currentStates, const Ice::DoubleSeq& canonicalValues, double temporalFactor, const::Ice::Current& = Ice::emptyCurrent) override;
- Vec2D calcTrajectoryV2(const Ice::DoubleSeq& timestamps, const Ice::DoubleSeq& goal, const Vec2D& initStates, const Ice::DoubleSeq& canonicalValues, double temporalFactor, const Ice::Current& = Ice::emptyCurrent) override;
- Vec2D calcTrajectoryFromConfig(const Ice::DoubleSeq& timestamps, const Vec2D& initStates, const Ice::DoubleSeq& canonicalValues, const Ice::Current& = Ice::emptyCurrent) override;
+
+ nStateValues calcNextStateFromConfig(double t,
+ double currentT,
+ const Vec2D& currentStates,
+ const Ice::DoubleSeq& canonicalValues,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ nStateValues calcNextState(double t,
+ const Ice::DoubleSeq& goal,
+ double currentT,
+ const Vec2D& currentStates,
+ const Ice::DoubleSeq& canonicalValues,
+ double temporalFactor,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
+ Vec2D calcTrajectoryV2(const Ice::DoubleSeq& timestamps,
+ const Ice::DoubleSeq& goal,
+ const Vec2D& initStates,
+ const Ice::DoubleSeq& canonicalValues,
+ double temporalFactor,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ Vec2D calcTrajectoryFromConfig(const Ice::DoubleSeq& timestamps,
+ const Vec2D& initStates,
+ const Ice::DoubleSeq& canonicalValues,
+ const Ice::Current& = Ice::emptyCurrent) override;
+
protected:
std::string dmpType;
DMP::DMPInterfacePtr dmp;
@@ -327,11 +370,8 @@ namespace armarx
std::string dmpName;
DMP::DVec canonicalValues;
cStateVec dmpState;
-
};
-
using DMPInstancePtr = IceInternal::Handle<DMPInstance>;
-}
-
+} // namespace armarx
diff --git a/source/RobotComponents/components/DMPComponent/test/DMPComponentEnvironment.h b/source/RobotComponents/components/DMPComponent/test/DMPComponentEnvironment.h
index e0b3e598e61190b66b608e384627307482752745..2b9947e2297d911af89968adbd7664b16ff90b01 100644
--- a/source/RobotComponents/components/DMPComponent/test/DMPComponentEnvironment.h
+++ b/source/RobotComponents/components/DMPComponent/test/DMPComponentEnvironment.h
@@ -22,13 +22,15 @@
#pragma once
+#include <ArmarXCore/core/application/Application.h>
+#include <ArmarXCore/core/test/IceTestHelper.h>
+
+#include <RobotComponents/components/DMPComponent/DMPComponent.h>
+
+#include <MemoryX/components/CommonStorage/CommonStorage.h>
#include <MemoryX/components/LongtermMemory/LongtermMemory.h>
#include <MemoryX/components/PriorKnowledge/PriorKnowledge.h>
-#include <MemoryX/components/CommonStorage/CommonStorage.h>
-#include <ArmarXCore/core/test/IceTestHelper.h>
#include <MemoryX/core/MongoTestHelper.h>
-#include <ArmarXCore/core/application/Application.h>
-#include <RobotComponents/components/DMPComponent/DMPComponent.h>
class DMPComponentEnvironment
{
@@ -41,7 +43,8 @@ public:
properties->setProperty("Ice.ThreadPool.Client.SizeMax", "2");
properties->setProperty("Ice.ThreadPool.Server.SizeMax", "2");
properties->setProperty("MemoryX.LongtermMemory.DatabaseName", "memdb");
- properties->setProperty("RobotComponents.DMPComponent.LongtermMemoryName", "LongtermMemory");
+ properties->setProperty("RobotComponents.DMPComponent.LongtermMemoryName",
+ "LongtermMemory");
properties->setProperty("MemoryX.PriorKnowledge.ClassCollections", "testdb.Prior_Objects");
properties->setProperty("MemoryX.PriorKnowledge.RelationCollections", "");
properties->setProperty("MemoryX.CommonStorage.MongoUser", "testuser");
@@ -53,11 +56,14 @@ public:
manager = new TestArmarXManager(testName, iceTestHelper->getCommunicator(), properties);
using namespace memoryx;
- manager->createComponentAndRun<CommonStorage, CommonStorageInterfacePrx>("MemoryX", "CommonStorage");
- manager->createComponentAndRun<PriorKnowledge, PriorKnowledgeInterfacePrx>("MemoryX", "PriorKnowledge");
- ltm = manager->createComponentAndRun<LongtermMemory, LongtermMemoryInterfacePrx>("MemoryX", "LongtermMemory");
- dmp = manager->createComponentAndRun<armarx::DMPComponent, armarx::DMPComponentBasePrx>("ArmarX", "DMPComponent");
-
+ manager->createComponentAndRun<CommonStorage, CommonStorageInterfacePrx>("MemoryX",
+ "CommonStorage");
+ manager->createComponentAndRun<PriorKnowledge, PriorKnowledgeInterfacePrx>(
+ "MemoryX", "PriorKnowledge");
+ ltm = manager->createComponentAndRun<LongtermMemory, LongtermMemoryInterfacePrx>(
+ "MemoryX", "LongtermMemory");
+ dmp = manager->createComponentAndRun<armarx::DMPComponent, armarx::DMPComponentBasePrx>(
+ "ArmarX", "DMPComponent");
}
MongoTestHelper mongoTestHelper;
@@ -68,4 +74,3 @@ public:
};
using DMPComponentEnvironmentPtr = std::shared_ptr<DMPComponentEnvironment>;
-
diff --git a/source/RobotComponents/components/DMPComponent/test/DMPComponentTest.cpp b/source/RobotComponents/components/DMPComponent/test/DMPComponentTest.cpp
index 02fac4e3f5a47d91098814eb46c84ff534e19bf8..283e397a3fff5c7dcb21e0ae1a339e6f9669070d 100644
--- a/source/RobotComponents/components/DMPComponent/test/DMPComponentTest.cpp
+++ b/source/RobotComponents/components/DMPComponent/test/DMPComponentTest.cpp
@@ -26,15 +26,14 @@
#define ARMARX_BOOST_TEST
+#include <iostream>
+
#include <RobotComponents/Test.h>
#include <RobotComponents/components/DMPComponent/DMPComponent.h>
#include <RobotComponents/interface/components/DMPComponentBase.h>
#include "DMPComponentEnvironment.h"
-
-#include <iostream>
-
//::armarx::cStateVec getcStateVec(const DMP::Vec<DMP::DMPState> &dmpstate){
// ::armarx::cStateVec sv;
// sv.resize(dmpstate.size());
@@ -87,7 +86,6 @@ BOOST_AUTO_TEST_CASE(DMPSerialization)
DMPComponentEnvironmentPtr env(new DMPComponentEnvironment("DMPtestenv"));
std::string filename = "/home/zhou/aaa.xml";
env->dmp->getDMPFromFile(filename, "myDMP");
-
}
BOOST_AUTO_TEST_CASE(storeAndremoveDMPTest)
@@ -147,7 +145,6 @@ BOOST_AUTO_TEST_CASE(retrieveDMPTest)
// env->dmp->setDMPState(currentState);
// std::cout << "pos: " << currentState[0].pos << std::endl;
// }
-
}
BOOST_AUTO_TEST_CASE(deleteDMPTest)
diff --git a/source/RobotComponents/components/Feedforward/forwardPredictor.cpp b/source/RobotComponents/components/Feedforward/forwardPredictor.cpp
index 3f4af7bf53c7138ac190b1578e57c1cbd2915936..e30b912fd2aea2d0addb7c9e9814b9cbbcc89501 100644
--- a/source/RobotComponents/components/Feedforward/forwardPredictor.cpp
+++ b/source/RobotComponents/components/Feedforward/forwardPredictor.cpp
@@ -4,13 +4,12 @@
using namespace armarx;
-ForwardPredictor::ForwardPredictor()
- = default;
+ForwardPredictor::ForwardPredictor() = default;
-ForwardPredictor::~ForwardPredictor()
- = default;
+ForwardPredictor::~ForwardPredictor() = default;
-void ForwardPredictor::calc()
+void
+ForwardPredictor::calc()
{
predict_IMU();
@@ -18,16 +17,12 @@ void ForwardPredictor::calc()
predict_optFlow();
}
-void ForwardPredictor::predict_IMU()
+void
+ForwardPredictor::predict_IMU()
{
-
}
-void ForwardPredictor::predict_optFlow()
+void
+ForwardPredictor::predict_optFlow()
{
-
}
-
-
-
-
diff --git a/source/RobotComponents/components/Feedforward/forwardPredictor.h b/source/RobotComponents/components/Feedforward/forwardPredictor.h
index 3ee41e4c1f1fb2f4770e8c728f8f133f75ef938f..60a12f65e70f46f8b9d4a7a7888fc3b590dcdbe2 100644
--- a/source/RobotComponents/components/Feedforward/forwardPredictor.h
+++ b/source/RobotComponents/components/Feedforward/forwardPredictor.h
@@ -1,17 +1,16 @@
#pragma once
-#include "symbolic_routines/mbs_sensor2.h"
-
#include <vector>
+#include "symbolic_routines/mbs_sensor2.h"
+
namespace armarx
{
class ForwardPredictor
{
public:
-
ForwardPredictor();
~ForwardPredictor();
@@ -19,7 +18,6 @@ namespace armarx
std::vector<float> getIMUPred();
private:
-
void calc();
void predict_IMU();
void predict_optFlow();
@@ -32,12 +30,11 @@ namespace armarx
// head IMU prediction
//std::vector<float> orientationQuaternion;
- std::vector<float> gyroscopeRotation_pred; // head velocity in absolute frame (x,y,z) [rad/s]
+ std::vector<float>
+ gyroscopeRotation_pred; // head velocity in absolute frame (x,y,z) [rad/s]
// optical flow prediction
std::vector<float> optFlow_pred; // [x, y] in [deg/s]
-
};
-}
-
+} // namespace armarx
diff --git a/source/RobotComponents/components/Feedforward/symbolic_routines/mbs_sensor2.h b/source/RobotComponents/components/Feedforward/symbolic_routines/mbs_sensor2.h
index eeccd227e47c7c10b544d3c7bee1b1c1c3dbe392..c0e060063a32b1c386961a0aba2f79f3f4b275e5 100644
--- a/source/RobotComponents/components/Feedforward/symbolic_routines/mbs_sensor2.h
+++ b/source/RobotComponents/components/Feedforward/symbolic_routines/mbs_sensor2.h
@@ -12,16 +12,24 @@
//
#pragma once
+
/*--------------------*/
typedef struct MbsSensor
{
- double P[4]; ///< Position vector of the sensor expressed in the inertial frame: \f$P(1:3)=[P_x; P_y; P_z]\f$.
- double R[4][4]; ///< Rotation matrix from the inertial frame to the sensor frame: \f$[\hat{\mathbf{X}}^S]=R(1:3,1:3).[\hat{\mathbf{X}}^0]\f$
- double V[4]; ///< Velocity vector of the sensor expressed in the inertial frame: \f$V(1:3)=[V_x; V_y; V_z]\f$.
- double OM[4]; ///< Angular velocity vector of the sensor expressed in the inertial frame: \f$OM(1:3)=[\omega_x; \omega_y; \omega_z]\f$.
- double A[4]; ///< Acceleration vector of the sensor expressed in the inertial frame: \f$A(1:3)=[A_x; A_y; A_z]\f$.
- double OMP[4]; ///< Angular acceleration vector of the sensor expressed in the inertial frame: \f$OMP(1:3)=[\dot\omega_x; \dot\omega_y; \dot\omega_z]\f$.
+ double P
+ [4]; ///< Position vector of the sensor expressed in the inertial frame: \f$P(1:3)=[P_x; P_y; P_z]\f$.
+ double R
+ [4]
+ [4]; ///< Rotation matrix from the inertial frame to the sensor frame: \f$[\hat{\mathbf{X}}^S]=R(1:3,1:3).[\hat{\mathbf{X}}^0]\f$
+ double V
+ [4]; ///< Velocity vector of the sensor expressed in the inertial frame: \f$V(1:3)=[V_x; V_y; V_z]\f$.
+ double OM
+ [4]; ///< Angular velocity vector of the sensor expressed in the inertial frame: \f$OM(1:3)=[\omega_x; \omega_y; \omega_z]\f$.
+ double A
+ [4]; ///< Acceleration vector of the sensor expressed in the inertial frame: \f$A(1:3)=[A_x; A_y; A_z]\f$.
+ double OMP
+ [4]; ///< Angular acceleration vector of the sensor expressed in the inertial frame: \f$OMP(1:3)=[\dot\omega_x; \dot\omega_y; \dot\omega_z]\f$.
/**
* Jacobian matrix of the sensor:
* \f$J(1:njoint,1:6)=\left[ \frac{\partial V}{\partial \dot q^T} \quad \frac{\partial OM}{\partial \dot q^T} \right]^\top \f$
@@ -32,29 +40,29 @@ typedef struct MbsSensor
} MbsSensor;
#ifdef __cplusplus
-extern "C" {
+extern "C"
+{
#endif
-/*!
+ /*!
* \brief Allocate the Jacobian matrix of the MbsSensor according to the number of joints in the multibody system.
* \param[in,out] psens Pointer to the MbsSensor structure to modify.
* \param[in] njoint Number of joint in the MBS, see MbsData::njoint.
*/
-void allocate_sensor(MbsSensor* psens, int njoint);
+ void allocate_sensor(MbsSensor* psens, int njoint);
-/*!
+ /*!
* \brief Initialize all fields of MbsSensor structure to 0.
* \param[in,out] psens Pointer to the MbsSensor structure to initialize.
* \param[in] njoint Number of joint in the MBS, see MbsData::njoint.
*/
-void init_sensor(MbsSensor* psens, int njoint);
+ void init_sensor(MbsSensor* psens, int njoint);
-/*!
+ /*!
* \brief Free the memory of MbsSensor structure.
* \param[in,out] psens Pointer to the MbsSensor structure to be freed.
*/
-void free_sensor(MbsSensor* psens);
+ void free_sensor(MbsSensor* psens);
#ifdef __cplusplus
}
#endif
-
diff --git a/source/RobotComponents/components/Feedforward/symbolic_routines/mbs_sensor_ArmarIII_simplified_virt.h b/source/RobotComponents/components/Feedforward/symbolic_routines/mbs_sensor_ArmarIII_simplified_virt.h
index 73fb5b1f284ae408cee599b51403bf103661b75c..a649c80a9a73e4f51e12fb6de2812efce1f917dc 100644
--- a/source/RobotComponents/components/Feedforward/symbolic_routines/mbs_sensor_ArmarIII_simplified_virt.h
+++ b/source/RobotComponents/components/Feedforward/symbolic_routines/mbs_sensor_ArmarIII_simplified_virt.h
@@ -1,1397 +1,1397 @@
-double Dz233;
-double Dz243;
-double C4;
-double S4;
-double C5;
-double S5;
-double C6;
-double S6;
-double C7;
-double S7;
-double C9;
-double S9;
-double C10;
-double S10;
-double C11;
-double S11;
-double C13;
-double S13;
-double C14;
-double S14;
-double C15;
-double S15;
-double C17;
-double S17;
-double C18;
-double S18;
-double C20;
-double S20;
-double C21;
-double S21;
-double C22;
-double S22;
-double C25;
-double S25;
-double C27;
-double S27;
-double C28;
-double S28;
-double C29;
-double S29;
-double C30;
-double S30;
-double C31;
-double S31;
-double C32;
-double S32;
-double C33;
-double S33;
-double C34;
-double S34;
-double C35;
-double S35;
-double C36;
-double S36;
-double C37;
-double S37;
-double C38;
-double S38;
-double C39;
-double S39;
-double C40;
-double S40;
-double C41;
-double S41;
-double C42;
-double S42;
-double C43;
-double S43;
-double C44;
-double S44;
-double C46;
-double S46;
-double C48;
-double S48;
-double C49;
-double S49;
-double C50;
-double S50;
-double C51;
-double S51;
-double C53;
-double S53;
-double C55;
-double S55;
-double C56;
-double S56;
-double ROcp0_25;
-double ROcp0_35;
-double ROcp0_85;
-double ROcp0_95;
-double ROcp0_16;
-double ROcp0_26;
-double ROcp0_36;
-double ROcp0_46;
-double ROcp0_56;
-double ROcp0_66;
-double ROcp0_47;
-double ROcp0_57;
-double ROcp0_67;
-double ROcp0_77;
-double ROcp0_87;
-double ROcp0_97;
-double ROcp0_49;
-double ROcp0_59;
-double ROcp0_69;
-double ROcp0_79;
-double ROcp0_89;
-double ROcp0_99;
-double ROcp0_110;
-double ROcp0_210;
-double ROcp0_310;
-double ROcp0_710;
-double ROcp0_810;
-double ROcp0_910;
-double ROcp0_111;
-double ROcp0_211;
-double ROcp0_311;
-double ROcp0_411;
-double ROcp0_511;
-double ROcp0_611;
-double ROcp0_413;
-double ROcp0_513;
-double ROcp0_613;
-double ROcp0_713;
-double ROcp0_813;
-double ROcp0_913;
-double ROcp0_114;
-double ROcp0_214;
-double ROcp0_314;
-double ROcp0_714;
-double ROcp0_814;
-double ROcp0_914;
-double ROcp0_115;
-double ROcp0_215;
-double ROcp0_315;
-double ROcp0_415;
-double ROcp0_515;
-double ROcp0_615;
-double ROcp0_117;
-double ROcp0_217;
-double ROcp0_317;
-double ROcp0_417;
-double ROcp0_517;
-double ROcp0_617;
-double ROcp0_118;
-double ROcp0_218;
-double ROcp0_318;
-double ROcp0_418;
-double ROcp0_518;
-double ROcp0_618;
-double ROcp0_420;
-double ROcp0_520;
-double ROcp0_620;
-double ROcp0_720;
-double ROcp0_820;
-double ROcp0_920;
-double ROcp0_121;
-double ROcp0_221;
-double ROcp0_321;
-double ROcp0_721;
-double ROcp0_821;
-double ROcp0_921;
-double ROcp0_122;
-double ROcp0_222;
-double ROcp0_322;
-double ROcp0_422;
-double ROcp0_522;
-double ROcp0_622;
-double ROcp0_125;
-double ROcp0_225;
-double ROcp0_325;
-double ROcp0_425;
-double ROcp0_525;
-double ROcp0_625;
-double ROcp0_427;
-double ROcp0_527;
-double ROcp0_627;
-double ROcp0_727;
-double ROcp0_827;
-double ROcp0_927;
-double ROcp0_128;
-double ROcp0_228;
-double ROcp0_328;
-double ROcp0_428;
-double ROcp0_528;
-double ROcp0_628;
-double ROcp0_429;
-double ROcp0_529;
-double ROcp0_629;
-double ROcp0_729;
-double ROcp0_829;
-double ROcp0_929;
-double ROcp0_130;
-double ROcp0_230;
-double ROcp0_330;
-double ROcp0_430;
-double ROcp0_530;
-double ROcp0_630;
-double ROcp0_431;
-double ROcp0_531;
-double ROcp0_631;
-double ROcp0_731;
-double ROcp0_831;
-double ROcp0_931;
-double ROcp0_432;
-double ROcp0_532;
-double ROcp0_632;
-double ROcp0_732;
-double ROcp0_832;
-double ROcp0_932;
-double ROcp0_133;
-double ROcp0_233;
-double ROcp0_333;
-double ROcp0_733;
-double ROcp0_833;
-double ROcp0_933;
-double ROcp0_134;
-double ROcp0_234;
-double ROcp0_334;
-double ROcp0_434;
-double ROcp0_534;
-double ROcp0_634;
-double ROcp0_135;
-double ROcp0_235;
-double ROcp0_335;
-double ROcp0_435;
-double ROcp0_535;
-double ROcp0_635;
-double ROcp0_436;
-double ROcp0_536;
-double ROcp0_636;
-double ROcp0_736;
-double ROcp0_836;
-double ROcp0_936;
-double ROcp0_137;
-double ROcp0_237;
-double ROcp0_337;
-double ROcp0_737;
-double ROcp0_837;
-double ROcp0_937;
-double ROcp0_138;
-double ROcp0_238;
-double ROcp0_338;
-double ROcp0_438;
-double ROcp0_538;
-double ROcp0_638;
-double ROcp0_439;
-double ROcp0_539;
-double ROcp0_639;
-double ROcp0_739;
-double ROcp0_839;
-double ROcp0_939;
-double ROcp0_140;
-double ROcp0_240;
-double ROcp0_340;
-double ROcp0_440;
-double ROcp0_540;
-double ROcp0_640;
-double ROcp0_441;
-double ROcp0_541;
-double ROcp0_641;
-double ROcp0_741;
-double ROcp0_841;
-double ROcp0_941;
-double ROcp0_442;
-double ROcp0_542;
-double ROcp0_642;
-double ROcp0_742;
-double ROcp0_842;
-double ROcp0_942;
-double ROcp0_143;
-double ROcp0_243;
-double ROcp0_343;
-double ROcp0_443;
-double ROcp0_543;
-double ROcp0_643;
-double OMcp0_25;
-double OMcp0_35;
-double OMcp0_16;
-double OMcp0_26;
-double OMcp0_36;
-double OPcp0_16;
-double OPcp0_26;
-double OPcp0_36;
-double RLcp0_18;
-double RLcp0_28;
-double RLcp0_38;
-double ORcp0_18;
-double ORcp0_28;
-double ORcp0_38;
-double RLcp0_112;
-double RLcp0_212;
-double RLcp0_312;
-double ORcp0_112;
-double ORcp0_212;
-double ORcp0_312;
-double RLcp0_116;
-double RLcp0_216;
-double RLcp0_316;
-double ORcp0_116;
-double ORcp0_216;
-double ORcp0_316;
-double OMcp0_118;
-double OMcp0_218;
-double OMcp0_318;
-double OPcp0_118;
-double OPcp0_218;
-double OPcp0_318;
-double RLcp0_119;
-double RLcp0_219;
-double RLcp0_319;
-double ORcp0_119;
-double ORcp0_219;
-double ORcp0_319;
-double RLcp0_120;
-double RLcp0_220;
-double RLcp0_320;
-double OMcp0_120;
-double OMcp0_220;
-double OMcp0_320;
-double ORcp0_120;
-double ORcp0_220;
-double ORcp0_320;
-double OMcp0_121;
-double OMcp0_221;
-double OMcp0_321;
-double OMcp0_122;
-double OMcp0_222;
-double OMcp0_322;
-double OPcp0_122;
-double OPcp0_222;
-double OPcp0_322;
-double RLcp0_123;
-double RLcp0_223;
-double RLcp0_323;
-double ORcp0_123;
-double ORcp0_223;
-double ORcp0_323;
-double RLcp0_124;
-double RLcp0_224;
-double RLcp0_324;
-double ORcp0_124;
-double ORcp0_224;
-double ORcp0_324;
-double RLcp0_126;
-double RLcp0_226;
-double RLcp0_326;
-double ORcp0_126;
-double ORcp0_226;
-double ORcp0_326;
-double OMcp0_130;
-double OMcp0_230;
-double OMcp0_330;
-double OMcp0_135;
-double OMcp0_235;
-double OMcp0_335;
-double OMcp0_140;
-double OMcp0_240;
-double OMcp0_340;
-double OPcp0_140;
-double OPcp0_240;
-double OPcp0_340;
-double RLcp0_142;
-double RLcp0_242;
-double RLcp0_342;
-double ORcp0_142;
-double ORcp0_242;
-double ORcp0_342;
-double OMcp0_143;
-double OMcp0_243;
-double OMcp0_343;
-double OPcp0_143;
-double OPcp0_243;
-double OPcp0_343;
-double RLcp0_157;
-double RLcp0_257;
-double RLcp0_357;
-double POcp0_157;
-double POcp0_257;
-double POcp0_357;
-double JTcp0_257_4;
-double JTcp0_357_4;
-double JTcp0_157_5;
-double JTcp0_257_5;
-double JTcp0_357_5;
-double JTcp0_157_6;
-double JTcp0_257_6;
-double JTcp0_357_6;
-double JTcp0_157_7;
-double JTcp0_257_7;
-double JTcp0_357_7;
-double JTcp0_157_9;
-double JTcp0_257_9;
-double JTcp0_357_9;
-double JTcp0_157_10;
-double JTcp0_257_10;
-double JTcp0_357_10;
-double JTcp0_157_11;
-double JTcp0_257_11;
-double JTcp0_357_11;
-double JTcp0_157_13;
-double JTcp0_257_13;
-double JTcp0_357_13;
-double JTcp0_157_14;
-double JTcp0_257_14;
-double JTcp0_357_14;
-double JTcp0_157_15;
-double JTcp0_257_15;
-double JTcp0_357_15;
-double JTcp0_157_17;
-double JTcp0_257_17;
-double JTcp0_357_17;
-double JTcp0_157_18;
-double JTcp0_257_18;
-double JTcp0_357_18;
-double JTcp0_157_20;
-double JTcp0_257_20;
-double JTcp0_357_20;
-double JTcp0_157_21;
-double JTcp0_257_21;
-double JTcp0_357_21;
-double JTcp0_157_22;
-double JTcp0_257_22;
-double JTcp0_357_22;
-double JTcp0_157_25;
-double JTcp0_257_25;
-double JTcp0_357_25;
-double JTcp0_157_27;
-double JTcp0_257_27;
-double JTcp0_357_27;
-double JTcp0_157_28;
-double JTcp0_257_28;
-double JTcp0_357_28;
-double JTcp0_157_29;
-double JTcp0_257_29;
-double JTcp0_357_29;
-double JTcp0_157_30;
-double JTcp0_257_30;
-double JTcp0_357_30;
-double JTcp0_157_31;
-double JTcp0_257_31;
-double JTcp0_357_31;
-double JTcp0_157_32;
-double JTcp0_257_32;
-double JTcp0_357_32;
-double JTcp0_157_33;
-double JTcp0_257_33;
-double JTcp0_357_33;
-double JTcp0_157_34;
-double JTcp0_257_34;
-double JTcp0_357_34;
-double JTcp0_157_35;
-double JTcp0_257_35;
-double JTcp0_357_35;
-double JTcp0_157_36;
-double JTcp0_257_36;
-double JTcp0_357_36;
-double JTcp0_157_37;
-double JTcp0_257_37;
-double JTcp0_357_37;
-double JTcp0_157_38;
-double JTcp0_257_38;
-double JTcp0_357_38;
-double JTcp0_157_39;
-double JTcp0_257_39;
-double JTcp0_357_39;
-double JTcp0_157_40;
-double JTcp0_257_40;
-double JTcp0_357_40;
-double JTcp0_157_41;
-double JTcp0_257_41;
-double JTcp0_357_41;
-double JTcp0_157_42;
-double JTcp0_257_42;
-double JTcp0_357_42;
-double JTcp0_157_43;
-double JTcp0_257_43;
-double JTcp0_357_43;
-double ORcp0_157;
-double ORcp0_257;
-double ORcp0_357;
-double VIcp0_157;
-double VIcp0_257;
-double VIcp0_357;
-double ACcp0_157;
-double ACcp0_257;
-double ACcp0_357;
-double ROcp1_25;
-double ROcp1_35;
-double ROcp1_85;
-double ROcp1_95;
-double ROcp1_16;
-double ROcp1_26;
-double ROcp1_36;
-double ROcp1_46;
-double ROcp1_56;
-double ROcp1_66;
-double ROcp1_47;
-double ROcp1_57;
-double ROcp1_67;
-double ROcp1_77;
-double ROcp1_87;
-double ROcp1_97;
-double ROcp1_49;
-double ROcp1_59;
-double ROcp1_69;
-double ROcp1_79;
-double ROcp1_89;
-double ROcp1_99;
-double ROcp1_110;
-double ROcp1_210;
-double ROcp1_310;
-double ROcp1_710;
-double ROcp1_810;
-double ROcp1_910;
-double ROcp1_111;
-double ROcp1_211;
-double ROcp1_311;
-double ROcp1_411;
-double ROcp1_511;
-double ROcp1_611;
-double ROcp1_413;
-double ROcp1_513;
-double ROcp1_613;
-double ROcp1_713;
-double ROcp1_813;
-double ROcp1_913;
-double ROcp1_114;
-double ROcp1_214;
-double ROcp1_314;
-double ROcp1_714;
-double ROcp1_814;
-double ROcp1_914;
-double ROcp1_115;
-double ROcp1_215;
-double ROcp1_315;
-double ROcp1_415;
-double ROcp1_515;
-double ROcp1_615;
-double ROcp1_117;
-double ROcp1_217;
-double ROcp1_317;
-double ROcp1_417;
-double ROcp1_517;
-double ROcp1_617;
-double ROcp1_118;
-double ROcp1_218;
-double ROcp1_318;
-double ROcp1_418;
-double ROcp1_518;
-double ROcp1_618;
-double ROcp1_420;
-double ROcp1_520;
-double ROcp1_620;
-double ROcp1_720;
-double ROcp1_820;
-double ROcp1_920;
-double ROcp1_121;
-double ROcp1_221;
-double ROcp1_321;
-double ROcp1_721;
-double ROcp1_821;
-double ROcp1_921;
-double ROcp1_122;
-double ROcp1_222;
-double ROcp1_322;
-double ROcp1_422;
-double ROcp1_522;
-double ROcp1_622;
-double ROcp1_125;
-double ROcp1_225;
-double ROcp1_325;
-double ROcp1_425;
-double ROcp1_525;
-double ROcp1_625;
-double ROcp1_427;
-double ROcp1_527;
-double ROcp1_627;
-double ROcp1_727;
-double ROcp1_827;
-double ROcp1_927;
-double ROcp1_128;
-double ROcp1_228;
-double ROcp1_328;
-double ROcp1_428;
-double ROcp1_528;
-double ROcp1_628;
-double ROcp1_429;
-double ROcp1_529;
-double ROcp1_629;
-double ROcp1_729;
-double ROcp1_829;
-double ROcp1_929;
-double ROcp1_130;
-double ROcp1_230;
-double ROcp1_330;
-double ROcp1_430;
-double ROcp1_530;
-double ROcp1_630;
-double ROcp1_431;
-double ROcp1_531;
-double ROcp1_631;
-double ROcp1_731;
-double ROcp1_831;
-double ROcp1_931;
-double ROcp1_432;
-double ROcp1_532;
-double ROcp1_632;
-double ROcp1_732;
-double ROcp1_832;
-double ROcp1_932;
-double ROcp1_133;
-double ROcp1_233;
-double ROcp1_333;
-double ROcp1_733;
-double ROcp1_833;
-double ROcp1_933;
-double ROcp1_134;
-double ROcp1_234;
-double ROcp1_334;
-double ROcp1_434;
-double ROcp1_534;
-double ROcp1_634;
-double ROcp1_135;
-double ROcp1_235;
-double ROcp1_335;
-double ROcp1_435;
-double ROcp1_535;
-double ROcp1_635;
-double ROcp1_436;
-double ROcp1_536;
-double ROcp1_636;
-double ROcp1_736;
-double ROcp1_836;
-double ROcp1_936;
-double ROcp1_137;
-double ROcp1_237;
-double ROcp1_337;
-double ROcp1_737;
-double ROcp1_837;
-double ROcp1_937;
-double ROcp1_138;
-double ROcp1_238;
-double ROcp1_338;
-double ROcp1_438;
-double ROcp1_538;
-double ROcp1_638;
-double ROcp1_439;
-double ROcp1_539;
-double ROcp1_639;
-double ROcp1_739;
-double ROcp1_839;
-double ROcp1_939;
-double ROcp1_140;
-double ROcp1_240;
-double ROcp1_340;
-double ROcp1_440;
-double ROcp1_540;
-double ROcp1_640;
-double ROcp1_441;
-double ROcp1_541;
-double ROcp1_641;
-double ROcp1_741;
-double ROcp1_841;
-double ROcp1_941;
-double ROcp1_442;
-double ROcp1_542;
-double ROcp1_642;
-double ROcp1_742;
-double ROcp1_842;
-double ROcp1_942;
-double ROcp1_143;
-double ROcp1_243;
-double ROcp1_343;
-double ROcp1_443;
-double ROcp1_543;
-double ROcp1_643;
-double ROcp1_444;
-double ROcp1_544;
-double ROcp1_644;
-double ROcp1_744;
-double ROcp1_844;
-double ROcp1_944;
-double ROcp1_446;
-double ROcp1_546;
-double ROcp1_646;
-double ROcp1_746;
-double ROcp1_846;
-double ROcp1_946;
-double ROcp1_448;
-double ROcp1_548;
-double ROcp1_648;
-double ROcp1_748;
-double ROcp1_848;
-double ROcp1_948;
-double ROcp1_149;
-double ROcp1_249;
-double ROcp1_349;
-double ROcp1_449;
-double ROcp1_549;
-double ROcp1_649;
-double ROcp1_450;
-double ROcp1_550;
-double ROcp1_650;
-double ROcp1_750;
-double ROcp1_850;
-double ROcp1_950;
-double ROcp1_451;
-double ROcp1_551;
-double ROcp1_651;
-double ROcp1_751;
-double ROcp1_851;
-double ROcp1_951;
-double ROcp1_153;
-double ROcp1_253;
-double ROcp1_353;
-double ROcp1_453;
-double ROcp1_553;
-double ROcp1_653;
-double OMcp1_25;
-double OMcp1_35;
-double OMcp1_16;
-double OMcp1_26;
-double OMcp1_36;
-double OPcp1_16;
-double OPcp1_26;
-double OPcp1_36;
-double RLcp1_18;
-double RLcp1_28;
-double RLcp1_38;
-double ORcp1_18;
-double ORcp1_28;
-double ORcp1_38;
-double RLcp1_112;
-double RLcp1_212;
-double RLcp1_312;
-double ORcp1_112;
-double ORcp1_212;
-double ORcp1_312;
-double RLcp1_116;
-double RLcp1_216;
-double RLcp1_316;
-double ORcp1_116;
-double ORcp1_216;
-double ORcp1_316;
-double OMcp1_118;
-double OMcp1_218;
-double OMcp1_318;
-double OPcp1_118;
-double OPcp1_218;
-double OPcp1_318;
-double RLcp1_119;
-double RLcp1_219;
-double RLcp1_319;
-double ORcp1_119;
-double ORcp1_219;
-double ORcp1_319;
-double RLcp1_120;
-double RLcp1_220;
-double RLcp1_320;
-double OMcp1_120;
-double OMcp1_220;
-double OMcp1_320;
-double ORcp1_120;
-double ORcp1_220;
-double ORcp1_320;
-double OMcp1_121;
-double OMcp1_221;
-double OMcp1_321;
-double OMcp1_122;
-double OMcp1_222;
-double OMcp1_322;
-double OPcp1_122;
-double OPcp1_222;
-double OPcp1_322;
-double RLcp1_123;
-double RLcp1_223;
-double RLcp1_323;
-double ORcp1_123;
-double ORcp1_223;
-double ORcp1_323;
-double RLcp1_124;
-double RLcp1_224;
-double RLcp1_324;
-double ORcp1_124;
-double ORcp1_224;
-double ORcp1_324;
-double RLcp1_126;
-double RLcp1_226;
-double RLcp1_326;
-double ORcp1_126;
-double ORcp1_226;
-double ORcp1_326;
-double OMcp1_130;
-double OMcp1_230;
-double OMcp1_330;
-double OMcp1_135;
-double OMcp1_235;
-double OMcp1_335;
-double OMcp1_140;
-double OMcp1_240;
-double OMcp1_340;
-double OPcp1_140;
-double OPcp1_240;
-double OPcp1_340;
-double RLcp1_142;
-double RLcp1_242;
-double RLcp1_342;
-double ORcp1_142;
-double ORcp1_242;
-double ORcp1_342;
-double OMcp1_143;
-double OMcp1_243;
-double OMcp1_343;
-double OPcp1_143;
-double OPcp1_243;
-double OPcp1_343;
-double RLcp1_145;
-double RLcp1_245;
-double RLcp1_345;
-double ORcp1_145;
-double ORcp1_245;
-double ORcp1_345;
-double RLcp1_146;
-double RLcp1_246;
-double RLcp1_346;
-double ORcp1_146;
-double ORcp1_246;
-double ORcp1_346;
-double RLcp1_147;
-double RLcp1_247;
-double RLcp1_347;
-double ORcp1_147;
-double ORcp1_247;
-double ORcp1_347;
-double RLcp1_148;
-double RLcp1_248;
-double RLcp1_348;
-double ORcp1_148;
-double ORcp1_248;
-double ORcp1_348;
-double OMcp1_149;
-double OMcp1_249;
-double OMcp1_349;
-double OPcp1_149;
-double OPcp1_249;
-double OPcp1_349;
-double RLcp1_151;
-double RLcp1_251;
-double RLcp1_351;
-double ORcp1_151;
-double ORcp1_251;
-double ORcp1_351;
-double RLcp1_152;
-double RLcp1_252;
-double RLcp1_352;
-double POcp1_152;
-double POcp1_252;
-double POcp1_352;
-double JTcp1_252_4;
-double JTcp1_352_4;
-double JTcp1_152_5;
-double JTcp1_252_5;
-double JTcp1_352_5;
-double JTcp1_152_6;
-double JTcp1_252_6;
-double JTcp1_352_6;
-double JTcp1_152_7;
-double JTcp1_252_7;
-double JTcp1_352_7;
-double JTcp1_152_9;
-double JTcp1_252_9;
-double JTcp1_352_9;
-double JTcp1_152_10;
-double JTcp1_252_10;
-double JTcp1_352_10;
-double JTcp1_152_11;
-double JTcp1_252_11;
-double JTcp1_352_11;
-double JTcp1_152_13;
-double JTcp1_252_13;
-double JTcp1_352_13;
-double JTcp1_152_14;
-double JTcp1_252_14;
-double JTcp1_352_14;
-double JTcp1_152_15;
-double JTcp1_252_15;
-double JTcp1_352_15;
-double JTcp1_152_17;
-double JTcp1_252_17;
-double JTcp1_352_17;
-double JTcp1_152_18;
-double JTcp1_252_18;
-double JTcp1_352_18;
-double JTcp1_152_20;
-double JTcp1_252_20;
-double JTcp1_352_20;
-double JTcp1_152_21;
-double JTcp1_252_21;
-double JTcp1_352_21;
-double JTcp1_152_22;
-double JTcp1_252_22;
-double JTcp1_352_22;
-double JTcp1_152_25;
-double JTcp1_252_25;
-double JTcp1_352_25;
-double JTcp1_152_27;
-double JTcp1_252_27;
-double JTcp1_352_27;
-double JTcp1_152_28;
-double JTcp1_252_28;
-double JTcp1_352_28;
-double JTcp1_152_29;
-double JTcp1_252_29;
-double JTcp1_352_29;
-double JTcp1_152_30;
-double JTcp1_252_30;
-double JTcp1_352_30;
-double JTcp1_152_31;
-double JTcp1_252_31;
-double JTcp1_352_31;
-double JTcp1_152_32;
-double JTcp1_252_32;
-double JTcp1_352_32;
-double JTcp1_152_33;
-double JTcp1_252_33;
-double JTcp1_352_33;
-double JTcp1_152_34;
-double JTcp1_252_34;
-double JTcp1_352_34;
-double JTcp1_152_35;
-double JTcp1_252_35;
-double JTcp1_352_35;
-double JTcp1_152_36;
-double JTcp1_252_36;
-double JTcp1_352_36;
-double JTcp1_152_37;
-double JTcp1_252_37;
-double JTcp1_352_37;
-double JTcp1_152_38;
-double JTcp1_252_38;
-double JTcp1_352_38;
-double JTcp1_152_39;
-double JTcp1_252_39;
-double JTcp1_352_39;
-double JTcp1_152_40;
-double JTcp1_252_40;
-double JTcp1_352_40;
-double JTcp1_152_41;
-double JTcp1_252_41;
-double JTcp1_352_41;
-double JTcp1_152_42;
-double JTcp1_252_42;
-double JTcp1_352_42;
-double JTcp1_152_43;
-double JTcp1_252_43;
-double JTcp1_352_43;
-double JTcp1_152_44;
-double JTcp1_252_44;
-double JTcp1_352_44;
-double JTcp1_152_46;
-double JTcp1_252_46;
-double JTcp1_352_46;
-double JTcp1_152_48;
-double JTcp1_252_48;
-double JTcp1_352_48;
-double JTcp1_152_49;
-double JTcp1_252_49;
-double JTcp1_352_49;
-double JTcp1_152_50;
-double JTcp1_252_50;
-double JTcp1_352_50;
-double JTcp1_152_51;
-double JTcp1_252_51;
-double JTcp1_352_51;
-double ORcp1_152;
-double ORcp1_252;
-double ORcp1_352;
-double VIcp1_152;
-double VIcp1_252;
-double VIcp1_352;
-double ACcp1_152;
-double ACcp1_252;
-double ACcp1_352;
-double OMcp1_153;
-double OMcp1_253;
-double OMcp1_353;
-double OPcp1_153;
-double OPcp1_253;
-double OPcp1_353;
-double ROcp2_25;
-double ROcp2_35;
-double ROcp2_85;
-double ROcp2_95;
-double ROcp2_16;
-double ROcp2_26;
-double ROcp2_36;
-double ROcp2_46;
-double ROcp2_56;
-double ROcp2_66;
-double ROcp2_47;
-double ROcp2_57;
-double ROcp2_67;
-double ROcp2_77;
-double ROcp2_87;
-double ROcp2_97;
-double ROcp2_49;
-double ROcp2_59;
-double ROcp2_69;
-double ROcp2_79;
-double ROcp2_89;
-double ROcp2_99;
-double ROcp2_110;
-double ROcp2_210;
-double ROcp2_310;
-double ROcp2_710;
-double ROcp2_810;
-double ROcp2_910;
-double ROcp2_111;
-double ROcp2_211;
-double ROcp2_311;
-double ROcp2_411;
-double ROcp2_511;
-double ROcp2_611;
-double ROcp2_413;
-double ROcp2_513;
-double ROcp2_613;
-double ROcp2_713;
-double ROcp2_813;
-double ROcp2_913;
-double ROcp2_114;
-double ROcp2_214;
-double ROcp2_314;
-double ROcp2_714;
-double ROcp2_814;
-double ROcp2_914;
-double ROcp2_115;
-double ROcp2_215;
-double ROcp2_315;
-double ROcp2_415;
-double ROcp2_515;
-double ROcp2_615;
-double ROcp2_117;
-double ROcp2_217;
-double ROcp2_317;
-double ROcp2_417;
-double ROcp2_517;
-double ROcp2_617;
-double ROcp2_118;
-double ROcp2_218;
-double ROcp2_318;
-double ROcp2_418;
-double ROcp2_518;
-double ROcp2_618;
-double ROcp2_420;
-double ROcp2_520;
-double ROcp2_620;
-double ROcp2_720;
-double ROcp2_820;
-double ROcp2_920;
-double ROcp2_121;
-double ROcp2_221;
-double ROcp2_321;
-double ROcp2_721;
-double ROcp2_821;
-double ROcp2_921;
-double ROcp2_122;
-double ROcp2_222;
-double ROcp2_322;
-double ROcp2_422;
-double ROcp2_522;
-double ROcp2_622;
-double ROcp2_125;
-double ROcp2_225;
-double ROcp2_325;
-double ROcp2_425;
-double ROcp2_525;
-double ROcp2_625;
-double ROcp2_427;
-double ROcp2_527;
-double ROcp2_627;
-double ROcp2_727;
-double ROcp2_827;
-double ROcp2_927;
-double ROcp2_128;
-double ROcp2_228;
-double ROcp2_328;
-double ROcp2_428;
-double ROcp2_528;
-double ROcp2_628;
-double ROcp2_429;
-double ROcp2_529;
-double ROcp2_629;
-double ROcp2_729;
-double ROcp2_829;
-double ROcp2_929;
-double ROcp2_130;
-double ROcp2_230;
-double ROcp2_330;
-double ROcp2_430;
-double ROcp2_530;
-double ROcp2_630;
-double ROcp2_431;
-double ROcp2_531;
-double ROcp2_631;
-double ROcp2_731;
-double ROcp2_831;
-double ROcp2_931;
-double ROcp2_432;
-double ROcp2_532;
-double ROcp2_632;
-double ROcp2_732;
-double ROcp2_832;
-double ROcp2_932;
-double ROcp2_133;
-double ROcp2_233;
-double ROcp2_333;
-double ROcp2_733;
-double ROcp2_833;
-double ROcp2_933;
-double ROcp2_134;
-double ROcp2_234;
-double ROcp2_334;
-double ROcp2_434;
-double ROcp2_534;
-double ROcp2_634;
-double ROcp2_135;
-double ROcp2_235;
-double ROcp2_335;
-double ROcp2_435;
-double ROcp2_535;
-double ROcp2_635;
-double ROcp2_436;
-double ROcp2_536;
-double ROcp2_636;
-double ROcp2_736;
-double ROcp2_836;
-double ROcp2_936;
-double ROcp2_137;
-double ROcp2_237;
-double ROcp2_337;
-double ROcp2_737;
-double ROcp2_837;
-double ROcp2_937;
-double ROcp2_138;
-double ROcp2_238;
-double ROcp2_338;
-double ROcp2_438;
-double ROcp2_538;
-double ROcp2_638;
-double ROcp2_439;
-double ROcp2_539;
-double ROcp2_639;
-double ROcp2_739;
-double ROcp2_839;
-double ROcp2_939;
-double ROcp2_140;
-double ROcp2_240;
-double ROcp2_340;
-double ROcp2_440;
-double ROcp2_540;
-double ROcp2_640;
-double ROcp2_441;
-double ROcp2_541;
-double ROcp2_641;
-double ROcp2_741;
-double ROcp2_841;
-double ROcp2_941;
-double ROcp2_442;
-double ROcp2_542;
-double ROcp2_642;
-double ROcp2_742;
-double ROcp2_842;
-double ROcp2_942;
-double ROcp2_143;
-double ROcp2_243;
-double ROcp2_343;
-double ROcp2_443;
-double ROcp2_543;
-double ROcp2_643;
-double ROcp2_444;
-double ROcp2_544;
-double ROcp2_644;
-double ROcp2_744;
-double ROcp2_844;
-double ROcp2_944;
-double ROcp2_446;
-double ROcp2_546;
-double ROcp2_646;
-double ROcp2_746;
-double ROcp2_846;
-double ROcp2_946;
-double ROcp2_448;
-double ROcp2_548;
-double ROcp2_648;
-double ROcp2_748;
-double ROcp2_848;
-double ROcp2_948;
-double ROcp2_149;
-double ROcp2_249;
-double ROcp2_349;
-double ROcp2_449;
-double ROcp2_549;
-double ROcp2_649;
-double ROcp2_450;
-double ROcp2_550;
-double ROcp2_650;
-double ROcp2_750;
-double ROcp2_850;
-double ROcp2_950;
-double ROcp2_451;
-double ROcp2_551;
-double ROcp2_651;
-double ROcp2_751;
-double ROcp2_851;
-double ROcp2_951;
-double ROcp2_153;
-double ROcp2_253;
-double ROcp2_353;
-double ROcp2_453;
-double ROcp2_553;
-double ROcp2_653;
-double ROcp2_155;
-double ROcp2_255;
-double ROcp2_355;
-double ROcp2_755;
-double ROcp2_855;
-double ROcp2_955;
-double ROcp2_156;
-double ROcp2_256;
-double ROcp2_356;
-double ROcp2_456;
-double ROcp2_556;
-double ROcp2_656;
-double OMcp2_25;
-double OMcp2_35;
-double OMcp2_16;
-double OMcp2_26;
-double OMcp2_36;
-double OPcp2_16;
-double OPcp2_26;
-double OPcp2_36;
-double RLcp2_18;
-double RLcp2_28;
-double RLcp2_38;
-double ORcp2_18;
-double ORcp2_28;
-double ORcp2_38;
-double RLcp2_112;
-double RLcp2_212;
-double RLcp2_312;
-double ORcp2_112;
-double ORcp2_212;
-double ORcp2_312;
-double RLcp2_116;
-double RLcp2_216;
-double RLcp2_316;
-double ORcp2_116;
-double ORcp2_216;
-double ORcp2_316;
-double OMcp2_118;
-double OMcp2_218;
-double OMcp2_318;
-double OPcp2_118;
-double OPcp2_218;
-double OPcp2_318;
-double RLcp2_119;
-double RLcp2_219;
-double RLcp2_319;
-double ORcp2_119;
-double ORcp2_219;
-double ORcp2_319;
-double RLcp2_120;
-double RLcp2_220;
-double RLcp2_320;
-double OMcp2_120;
-double OMcp2_220;
-double OMcp2_320;
-double ORcp2_120;
-double ORcp2_220;
-double ORcp2_320;
-double OMcp2_121;
-double OMcp2_221;
-double OMcp2_321;
-double OMcp2_122;
-double OMcp2_222;
-double OMcp2_322;
-double OPcp2_122;
-double OPcp2_222;
-double OPcp2_322;
-double RLcp2_123;
-double RLcp2_223;
-double RLcp2_323;
-double ORcp2_123;
-double ORcp2_223;
-double ORcp2_323;
-double RLcp2_124;
-double RLcp2_224;
-double RLcp2_324;
-double ORcp2_124;
-double ORcp2_224;
-double ORcp2_324;
-double RLcp2_126;
-double RLcp2_226;
-double RLcp2_326;
-double ORcp2_126;
-double ORcp2_226;
-double ORcp2_326;
-double OMcp2_130;
-double OMcp2_230;
-double OMcp2_330;
-double OMcp2_135;
-double OMcp2_235;
-double OMcp2_335;
-double OMcp2_140;
-double OMcp2_240;
-double OMcp2_340;
-double OPcp2_140;
-double OPcp2_240;
-double OPcp2_340;
-double RLcp2_142;
-double RLcp2_242;
-double RLcp2_342;
-double ORcp2_142;
-double ORcp2_242;
-double ORcp2_342;
-double OMcp2_143;
-double OMcp2_243;
-double OMcp2_343;
-double OPcp2_143;
-double OPcp2_243;
-double OPcp2_343;
-double RLcp2_145;
-double RLcp2_245;
-double RLcp2_345;
-double ORcp2_145;
-double ORcp2_245;
-double ORcp2_345;
-double RLcp2_146;
-double RLcp2_246;
-double RLcp2_346;
-double ORcp2_146;
-double ORcp2_246;
-double ORcp2_346;
-double RLcp2_147;
-double RLcp2_247;
-double RLcp2_347;
-double ORcp2_147;
-double ORcp2_247;
-double ORcp2_347;
-double RLcp2_148;
-double RLcp2_248;
-double RLcp2_348;
-double ORcp2_148;
-double ORcp2_248;
-double ORcp2_348;
-double OMcp2_149;
-double OMcp2_249;
-double OMcp2_349;
-double OPcp2_149;
-double OPcp2_249;
-double OPcp2_349;
-double RLcp2_151;
-double RLcp2_251;
-double RLcp2_351;
-double ORcp2_151;
-double ORcp2_251;
-double ORcp2_351;
-double RLcp2_152;
-double RLcp2_252;
-double RLcp2_352;
-double ORcp2_152;
-double ORcp2_252;
-double ORcp2_352;
-double OMcp2_153;
-double OMcp2_253;
-double OMcp2_353;
-double OPcp2_153;
-double OPcp2_253;
-double OPcp2_353;
-double RLcp2_154;
-double RLcp2_254;
-double RLcp2_354;
-double POcp2_154;
-double POcp2_254;
-double POcp2_354;
-double JTcp2_254_4;
-double JTcp2_354_4;
-double JTcp2_154_5;
-double JTcp2_254_5;
-double JTcp2_354_5;
-double JTcp2_154_6;
-double JTcp2_254_6;
-double JTcp2_354_6;
+double Dz233;
+double Dz243;
+double C4;
+double S4;
+double C5;
+double S5;
+double C6;
+double S6;
+double C7;
+double S7;
+double C9;
+double S9;
+double C10;
+double S10;
+double C11;
+double S11;
+double C13;
+double S13;
+double C14;
+double S14;
+double C15;
+double S15;
+double C17;
+double S17;
+double C18;
+double S18;
+double C20;
+double S20;
+double C21;
+double S21;
+double C22;
+double S22;
+double C25;
+double S25;
+double C27;
+double S27;
+double C28;
+double S28;
+double C29;
+double S29;
+double C30;
+double S30;
+double C31;
+double S31;
+double C32;
+double S32;
+double C33;
+double S33;
+double C34;
+double S34;
+double C35;
+double S35;
+double C36;
+double S36;
+double C37;
+double S37;
+double C38;
+double S38;
+double C39;
+double S39;
+double C40;
+double S40;
+double C41;
+double S41;
+double C42;
+double S42;
+double C43;
+double S43;
+double C44;
+double S44;
+double C46;
+double S46;
+double C48;
+double S48;
+double C49;
+double S49;
+double C50;
+double S50;
+double C51;
+double S51;
+double C53;
+double S53;
+double C55;
+double S55;
+double C56;
+double S56;
+double ROcp0_25;
+double ROcp0_35;
+double ROcp0_85;
+double ROcp0_95;
+double ROcp0_16;
+double ROcp0_26;
+double ROcp0_36;
+double ROcp0_46;
+double ROcp0_56;
+double ROcp0_66;
+double ROcp0_47;
+double ROcp0_57;
+double ROcp0_67;
+double ROcp0_77;
+double ROcp0_87;
+double ROcp0_97;
+double ROcp0_49;
+double ROcp0_59;
+double ROcp0_69;
+double ROcp0_79;
+double ROcp0_89;
+double ROcp0_99;
+double ROcp0_110;
+double ROcp0_210;
+double ROcp0_310;
+double ROcp0_710;
+double ROcp0_810;
+double ROcp0_910;
+double ROcp0_111;
+double ROcp0_211;
+double ROcp0_311;
+double ROcp0_411;
+double ROcp0_511;
+double ROcp0_611;
+double ROcp0_413;
+double ROcp0_513;
+double ROcp0_613;
+double ROcp0_713;
+double ROcp0_813;
+double ROcp0_913;
+double ROcp0_114;
+double ROcp0_214;
+double ROcp0_314;
+double ROcp0_714;
+double ROcp0_814;
+double ROcp0_914;
+double ROcp0_115;
+double ROcp0_215;
+double ROcp0_315;
+double ROcp0_415;
+double ROcp0_515;
+double ROcp0_615;
+double ROcp0_117;
+double ROcp0_217;
+double ROcp0_317;
+double ROcp0_417;
+double ROcp0_517;
+double ROcp0_617;
+double ROcp0_118;
+double ROcp0_218;
+double ROcp0_318;
+double ROcp0_418;
+double ROcp0_518;
+double ROcp0_618;
+double ROcp0_420;
+double ROcp0_520;
+double ROcp0_620;
+double ROcp0_720;
+double ROcp0_820;
+double ROcp0_920;
+double ROcp0_121;
+double ROcp0_221;
+double ROcp0_321;
+double ROcp0_721;
+double ROcp0_821;
+double ROcp0_921;
+double ROcp0_122;
+double ROcp0_222;
+double ROcp0_322;
+double ROcp0_422;
+double ROcp0_522;
+double ROcp0_622;
+double ROcp0_125;
+double ROcp0_225;
+double ROcp0_325;
+double ROcp0_425;
+double ROcp0_525;
+double ROcp0_625;
+double ROcp0_427;
+double ROcp0_527;
+double ROcp0_627;
+double ROcp0_727;
+double ROcp0_827;
+double ROcp0_927;
+double ROcp0_128;
+double ROcp0_228;
+double ROcp0_328;
+double ROcp0_428;
+double ROcp0_528;
+double ROcp0_628;
+double ROcp0_429;
+double ROcp0_529;
+double ROcp0_629;
+double ROcp0_729;
+double ROcp0_829;
+double ROcp0_929;
+double ROcp0_130;
+double ROcp0_230;
+double ROcp0_330;
+double ROcp0_430;
+double ROcp0_530;
+double ROcp0_630;
+double ROcp0_431;
+double ROcp0_531;
+double ROcp0_631;
+double ROcp0_731;
+double ROcp0_831;
+double ROcp0_931;
+double ROcp0_432;
+double ROcp0_532;
+double ROcp0_632;
+double ROcp0_732;
+double ROcp0_832;
+double ROcp0_932;
+double ROcp0_133;
+double ROcp0_233;
+double ROcp0_333;
+double ROcp0_733;
+double ROcp0_833;
+double ROcp0_933;
+double ROcp0_134;
+double ROcp0_234;
+double ROcp0_334;
+double ROcp0_434;
+double ROcp0_534;
+double ROcp0_634;
+double ROcp0_135;
+double ROcp0_235;
+double ROcp0_335;
+double ROcp0_435;
+double ROcp0_535;
+double ROcp0_635;
+double ROcp0_436;
+double ROcp0_536;
+double ROcp0_636;
+double ROcp0_736;
+double ROcp0_836;
+double ROcp0_936;
+double ROcp0_137;
+double ROcp0_237;
+double ROcp0_337;
+double ROcp0_737;
+double ROcp0_837;
+double ROcp0_937;
+double ROcp0_138;
+double ROcp0_238;
+double ROcp0_338;
+double ROcp0_438;
+double ROcp0_538;
+double ROcp0_638;
+double ROcp0_439;
+double ROcp0_539;
+double ROcp0_639;
+double ROcp0_739;
+double ROcp0_839;
+double ROcp0_939;
+double ROcp0_140;
+double ROcp0_240;
+double ROcp0_340;
+double ROcp0_440;
+double ROcp0_540;
+double ROcp0_640;
+double ROcp0_441;
+double ROcp0_541;
+double ROcp0_641;
+double ROcp0_741;
+double ROcp0_841;
+double ROcp0_941;
+double ROcp0_442;
+double ROcp0_542;
+double ROcp0_642;
+double ROcp0_742;
+double ROcp0_842;
+double ROcp0_942;
+double ROcp0_143;
+double ROcp0_243;
+double ROcp0_343;
+double ROcp0_443;
+double ROcp0_543;
+double ROcp0_643;
+double OMcp0_25;
+double OMcp0_35;
+double OMcp0_16;
+double OMcp0_26;
+double OMcp0_36;
+double OPcp0_16;
+double OPcp0_26;
+double OPcp0_36;
+double RLcp0_18;
+double RLcp0_28;
+double RLcp0_38;
+double ORcp0_18;
+double ORcp0_28;
+double ORcp0_38;
+double RLcp0_112;
+double RLcp0_212;
+double RLcp0_312;
+double ORcp0_112;
+double ORcp0_212;
+double ORcp0_312;
+double RLcp0_116;
+double RLcp0_216;
+double RLcp0_316;
+double ORcp0_116;
+double ORcp0_216;
+double ORcp0_316;
+double OMcp0_118;
+double OMcp0_218;
+double OMcp0_318;
+double OPcp0_118;
+double OPcp0_218;
+double OPcp0_318;
+double RLcp0_119;
+double RLcp0_219;
+double RLcp0_319;
+double ORcp0_119;
+double ORcp0_219;
+double ORcp0_319;
+double RLcp0_120;
+double RLcp0_220;
+double RLcp0_320;
+double OMcp0_120;
+double OMcp0_220;
+double OMcp0_320;
+double ORcp0_120;
+double ORcp0_220;
+double ORcp0_320;
+double OMcp0_121;
+double OMcp0_221;
+double OMcp0_321;
+double OMcp0_122;
+double OMcp0_222;
+double OMcp0_322;
+double OPcp0_122;
+double OPcp0_222;
+double OPcp0_322;
+double RLcp0_123;
+double RLcp0_223;
+double RLcp0_323;
+double ORcp0_123;
+double ORcp0_223;
+double ORcp0_323;
+double RLcp0_124;
+double RLcp0_224;
+double RLcp0_324;
+double ORcp0_124;
+double ORcp0_224;
+double ORcp0_324;
+double RLcp0_126;
+double RLcp0_226;
+double RLcp0_326;
+double ORcp0_126;
+double ORcp0_226;
+double ORcp0_326;
+double OMcp0_130;
+double OMcp0_230;
+double OMcp0_330;
+double OMcp0_135;
+double OMcp0_235;
+double OMcp0_335;
+double OMcp0_140;
+double OMcp0_240;
+double OMcp0_340;
+double OPcp0_140;
+double OPcp0_240;
+double OPcp0_340;
+double RLcp0_142;
+double RLcp0_242;
+double RLcp0_342;
+double ORcp0_142;
+double ORcp0_242;
+double ORcp0_342;
+double OMcp0_143;
+double OMcp0_243;
+double OMcp0_343;
+double OPcp0_143;
+double OPcp0_243;
+double OPcp0_343;
+double RLcp0_157;
+double RLcp0_257;
+double RLcp0_357;
+double POcp0_157;
+double POcp0_257;
+double POcp0_357;
+double JTcp0_257_4;
+double JTcp0_357_4;
+double JTcp0_157_5;
+double JTcp0_257_5;
+double JTcp0_357_5;
+double JTcp0_157_6;
+double JTcp0_257_6;
+double JTcp0_357_6;
+double JTcp0_157_7;
+double JTcp0_257_7;
+double JTcp0_357_7;
+double JTcp0_157_9;
+double JTcp0_257_9;
+double JTcp0_357_9;
+double JTcp0_157_10;
+double JTcp0_257_10;
+double JTcp0_357_10;
+double JTcp0_157_11;
+double JTcp0_257_11;
+double JTcp0_357_11;
+double JTcp0_157_13;
+double JTcp0_257_13;
+double JTcp0_357_13;
+double JTcp0_157_14;
+double JTcp0_257_14;
+double JTcp0_357_14;
+double JTcp0_157_15;
+double JTcp0_257_15;
+double JTcp0_357_15;
+double JTcp0_157_17;
+double JTcp0_257_17;
+double JTcp0_357_17;
+double JTcp0_157_18;
+double JTcp0_257_18;
+double JTcp0_357_18;
+double JTcp0_157_20;
+double JTcp0_257_20;
+double JTcp0_357_20;
+double JTcp0_157_21;
+double JTcp0_257_21;
+double JTcp0_357_21;
+double JTcp0_157_22;
+double JTcp0_257_22;
+double JTcp0_357_22;
+double JTcp0_157_25;
+double JTcp0_257_25;
+double JTcp0_357_25;
+double JTcp0_157_27;
+double JTcp0_257_27;
+double JTcp0_357_27;
+double JTcp0_157_28;
+double JTcp0_257_28;
+double JTcp0_357_28;
+double JTcp0_157_29;
+double JTcp0_257_29;
+double JTcp0_357_29;
+double JTcp0_157_30;
+double JTcp0_257_30;
+double JTcp0_357_30;
+double JTcp0_157_31;
+double JTcp0_257_31;
+double JTcp0_357_31;
+double JTcp0_157_32;
+double JTcp0_257_32;
+double JTcp0_357_32;
+double JTcp0_157_33;
+double JTcp0_257_33;
+double JTcp0_357_33;
+double JTcp0_157_34;
+double JTcp0_257_34;
+double JTcp0_357_34;
+double JTcp0_157_35;
+double JTcp0_257_35;
+double JTcp0_357_35;
+double JTcp0_157_36;
+double JTcp0_257_36;
+double JTcp0_357_36;
+double JTcp0_157_37;
+double JTcp0_257_37;
+double JTcp0_357_37;
+double JTcp0_157_38;
+double JTcp0_257_38;
+double JTcp0_357_38;
+double JTcp0_157_39;
+double JTcp0_257_39;
+double JTcp0_357_39;
+double JTcp0_157_40;
+double JTcp0_257_40;
+double JTcp0_357_40;
+double JTcp0_157_41;
+double JTcp0_257_41;
+double JTcp0_357_41;
+double JTcp0_157_42;
+double JTcp0_257_42;
+double JTcp0_357_42;
+double JTcp0_157_43;
+double JTcp0_257_43;
+double JTcp0_357_43;
+double ORcp0_157;
+double ORcp0_257;
+double ORcp0_357;
+double VIcp0_157;
+double VIcp0_257;
+double VIcp0_357;
+double ACcp0_157;
+double ACcp0_257;
+double ACcp0_357;
+double ROcp1_25;
+double ROcp1_35;
+double ROcp1_85;
+double ROcp1_95;
+double ROcp1_16;
+double ROcp1_26;
+double ROcp1_36;
+double ROcp1_46;
+double ROcp1_56;
+double ROcp1_66;
+double ROcp1_47;
+double ROcp1_57;
+double ROcp1_67;
+double ROcp1_77;
+double ROcp1_87;
+double ROcp1_97;
+double ROcp1_49;
+double ROcp1_59;
+double ROcp1_69;
+double ROcp1_79;
+double ROcp1_89;
+double ROcp1_99;
+double ROcp1_110;
+double ROcp1_210;
+double ROcp1_310;
+double ROcp1_710;
+double ROcp1_810;
+double ROcp1_910;
+double ROcp1_111;
+double ROcp1_211;
+double ROcp1_311;
+double ROcp1_411;
+double ROcp1_511;
+double ROcp1_611;
+double ROcp1_413;
+double ROcp1_513;
+double ROcp1_613;
+double ROcp1_713;
+double ROcp1_813;
+double ROcp1_913;
+double ROcp1_114;
+double ROcp1_214;
+double ROcp1_314;
+double ROcp1_714;
+double ROcp1_814;
+double ROcp1_914;
+double ROcp1_115;
+double ROcp1_215;
+double ROcp1_315;
+double ROcp1_415;
+double ROcp1_515;
+double ROcp1_615;
+double ROcp1_117;
+double ROcp1_217;
+double ROcp1_317;
+double ROcp1_417;
+double ROcp1_517;
+double ROcp1_617;
+double ROcp1_118;
+double ROcp1_218;
+double ROcp1_318;
+double ROcp1_418;
+double ROcp1_518;
+double ROcp1_618;
+double ROcp1_420;
+double ROcp1_520;
+double ROcp1_620;
+double ROcp1_720;
+double ROcp1_820;
+double ROcp1_920;
+double ROcp1_121;
+double ROcp1_221;
+double ROcp1_321;
+double ROcp1_721;
+double ROcp1_821;
+double ROcp1_921;
+double ROcp1_122;
+double ROcp1_222;
+double ROcp1_322;
+double ROcp1_422;
+double ROcp1_522;
+double ROcp1_622;
+double ROcp1_125;
+double ROcp1_225;
+double ROcp1_325;
+double ROcp1_425;
+double ROcp1_525;
+double ROcp1_625;
+double ROcp1_427;
+double ROcp1_527;
+double ROcp1_627;
+double ROcp1_727;
+double ROcp1_827;
+double ROcp1_927;
+double ROcp1_128;
+double ROcp1_228;
+double ROcp1_328;
+double ROcp1_428;
+double ROcp1_528;
+double ROcp1_628;
+double ROcp1_429;
+double ROcp1_529;
+double ROcp1_629;
+double ROcp1_729;
+double ROcp1_829;
+double ROcp1_929;
+double ROcp1_130;
+double ROcp1_230;
+double ROcp1_330;
+double ROcp1_430;
+double ROcp1_530;
+double ROcp1_630;
+double ROcp1_431;
+double ROcp1_531;
+double ROcp1_631;
+double ROcp1_731;
+double ROcp1_831;
+double ROcp1_931;
+double ROcp1_432;
+double ROcp1_532;
+double ROcp1_632;
+double ROcp1_732;
+double ROcp1_832;
+double ROcp1_932;
+double ROcp1_133;
+double ROcp1_233;
+double ROcp1_333;
+double ROcp1_733;
+double ROcp1_833;
+double ROcp1_933;
+double ROcp1_134;
+double ROcp1_234;
+double ROcp1_334;
+double ROcp1_434;
+double ROcp1_534;
+double ROcp1_634;
+double ROcp1_135;
+double ROcp1_235;
+double ROcp1_335;
+double ROcp1_435;
+double ROcp1_535;
+double ROcp1_635;
+double ROcp1_436;
+double ROcp1_536;
+double ROcp1_636;
+double ROcp1_736;
+double ROcp1_836;
+double ROcp1_936;
+double ROcp1_137;
+double ROcp1_237;
+double ROcp1_337;
+double ROcp1_737;
+double ROcp1_837;
+double ROcp1_937;
+double ROcp1_138;
+double ROcp1_238;
+double ROcp1_338;
+double ROcp1_438;
+double ROcp1_538;
+double ROcp1_638;
+double ROcp1_439;
+double ROcp1_539;
+double ROcp1_639;
+double ROcp1_739;
+double ROcp1_839;
+double ROcp1_939;
+double ROcp1_140;
+double ROcp1_240;
+double ROcp1_340;
+double ROcp1_440;
+double ROcp1_540;
+double ROcp1_640;
+double ROcp1_441;
+double ROcp1_541;
+double ROcp1_641;
+double ROcp1_741;
+double ROcp1_841;
+double ROcp1_941;
+double ROcp1_442;
+double ROcp1_542;
+double ROcp1_642;
+double ROcp1_742;
+double ROcp1_842;
+double ROcp1_942;
+double ROcp1_143;
+double ROcp1_243;
+double ROcp1_343;
+double ROcp1_443;
+double ROcp1_543;
+double ROcp1_643;
+double ROcp1_444;
+double ROcp1_544;
+double ROcp1_644;
+double ROcp1_744;
+double ROcp1_844;
+double ROcp1_944;
+double ROcp1_446;
+double ROcp1_546;
+double ROcp1_646;
+double ROcp1_746;
+double ROcp1_846;
+double ROcp1_946;
+double ROcp1_448;
+double ROcp1_548;
+double ROcp1_648;
+double ROcp1_748;
+double ROcp1_848;
+double ROcp1_948;
+double ROcp1_149;
+double ROcp1_249;
+double ROcp1_349;
+double ROcp1_449;
+double ROcp1_549;
+double ROcp1_649;
+double ROcp1_450;
+double ROcp1_550;
+double ROcp1_650;
+double ROcp1_750;
+double ROcp1_850;
+double ROcp1_950;
+double ROcp1_451;
+double ROcp1_551;
+double ROcp1_651;
+double ROcp1_751;
+double ROcp1_851;
+double ROcp1_951;
+double ROcp1_153;
+double ROcp1_253;
+double ROcp1_353;
+double ROcp1_453;
+double ROcp1_553;
+double ROcp1_653;
+double OMcp1_25;
+double OMcp1_35;
+double OMcp1_16;
+double OMcp1_26;
+double OMcp1_36;
+double OPcp1_16;
+double OPcp1_26;
+double OPcp1_36;
+double RLcp1_18;
+double RLcp1_28;
+double RLcp1_38;
+double ORcp1_18;
+double ORcp1_28;
+double ORcp1_38;
+double RLcp1_112;
+double RLcp1_212;
+double RLcp1_312;
+double ORcp1_112;
+double ORcp1_212;
+double ORcp1_312;
+double RLcp1_116;
+double RLcp1_216;
+double RLcp1_316;
+double ORcp1_116;
+double ORcp1_216;
+double ORcp1_316;
+double OMcp1_118;
+double OMcp1_218;
+double OMcp1_318;
+double OPcp1_118;
+double OPcp1_218;
+double OPcp1_318;
+double RLcp1_119;
+double RLcp1_219;
+double RLcp1_319;
+double ORcp1_119;
+double ORcp1_219;
+double ORcp1_319;
+double RLcp1_120;
+double RLcp1_220;
+double RLcp1_320;
+double OMcp1_120;
+double OMcp1_220;
+double OMcp1_320;
+double ORcp1_120;
+double ORcp1_220;
+double ORcp1_320;
+double OMcp1_121;
+double OMcp1_221;
+double OMcp1_321;
+double OMcp1_122;
+double OMcp1_222;
+double OMcp1_322;
+double OPcp1_122;
+double OPcp1_222;
+double OPcp1_322;
+double RLcp1_123;
+double RLcp1_223;
+double RLcp1_323;
+double ORcp1_123;
+double ORcp1_223;
+double ORcp1_323;
+double RLcp1_124;
+double RLcp1_224;
+double RLcp1_324;
+double ORcp1_124;
+double ORcp1_224;
+double ORcp1_324;
+double RLcp1_126;
+double RLcp1_226;
+double RLcp1_326;
+double ORcp1_126;
+double ORcp1_226;
+double ORcp1_326;
+double OMcp1_130;
+double OMcp1_230;
+double OMcp1_330;
+double OMcp1_135;
+double OMcp1_235;
+double OMcp1_335;
+double OMcp1_140;
+double OMcp1_240;
+double OMcp1_340;
+double OPcp1_140;
+double OPcp1_240;
+double OPcp1_340;
+double RLcp1_142;
+double RLcp1_242;
+double RLcp1_342;
+double ORcp1_142;
+double ORcp1_242;
+double ORcp1_342;
+double OMcp1_143;
+double OMcp1_243;
+double OMcp1_343;
+double OPcp1_143;
+double OPcp1_243;
+double OPcp1_343;
+double RLcp1_145;
+double RLcp1_245;
+double RLcp1_345;
+double ORcp1_145;
+double ORcp1_245;
+double ORcp1_345;
+double RLcp1_146;
+double RLcp1_246;
+double RLcp1_346;
+double ORcp1_146;
+double ORcp1_246;
+double ORcp1_346;
+double RLcp1_147;
+double RLcp1_247;
+double RLcp1_347;
+double ORcp1_147;
+double ORcp1_247;
+double ORcp1_347;
+double RLcp1_148;
+double RLcp1_248;
+double RLcp1_348;
+double ORcp1_148;
+double ORcp1_248;
+double ORcp1_348;
+double OMcp1_149;
+double OMcp1_249;
+double OMcp1_349;
+double OPcp1_149;
+double OPcp1_249;
+double OPcp1_349;
+double RLcp1_151;
+double RLcp1_251;
+double RLcp1_351;
+double ORcp1_151;
+double ORcp1_251;
+double ORcp1_351;
+double RLcp1_152;
+double RLcp1_252;
+double RLcp1_352;
+double POcp1_152;
+double POcp1_252;
+double POcp1_352;
+double JTcp1_252_4;
+double JTcp1_352_4;
+double JTcp1_152_5;
+double JTcp1_252_5;
+double JTcp1_352_5;
+double JTcp1_152_6;
+double JTcp1_252_6;
+double JTcp1_352_6;
+double JTcp1_152_7;
+double JTcp1_252_7;
+double JTcp1_352_7;
+double JTcp1_152_9;
+double JTcp1_252_9;
+double JTcp1_352_9;
+double JTcp1_152_10;
+double JTcp1_252_10;
+double JTcp1_352_10;
+double JTcp1_152_11;
+double JTcp1_252_11;
+double JTcp1_352_11;
+double JTcp1_152_13;
+double JTcp1_252_13;
+double JTcp1_352_13;
+double JTcp1_152_14;
+double JTcp1_252_14;
+double JTcp1_352_14;
+double JTcp1_152_15;
+double JTcp1_252_15;
+double JTcp1_352_15;
+double JTcp1_152_17;
+double JTcp1_252_17;
+double JTcp1_352_17;
+double JTcp1_152_18;
+double JTcp1_252_18;
+double JTcp1_352_18;
+double JTcp1_152_20;
+double JTcp1_252_20;
+double JTcp1_352_20;
+double JTcp1_152_21;
+double JTcp1_252_21;
+double JTcp1_352_21;
+double JTcp1_152_22;
+double JTcp1_252_22;
+double JTcp1_352_22;
+double JTcp1_152_25;
+double JTcp1_252_25;
+double JTcp1_352_25;
+double JTcp1_152_27;
+double JTcp1_252_27;
+double JTcp1_352_27;
+double JTcp1_152_28;
+double JTcp1_252_28;
+double JTcp1_352_28;
+double JTcp1_152_29;
+double JTcp1_252_29;
+double JTcp1_352_29;
+double JTcp1_152_30;
+double JTcp1_252_30;
+double JTcp1_352_30;
+double JTcp1_152_31;
+double JTcp1_252_31;
+double JTcp1_352_31;
+double JTcp1_152_32;
+double JTcp1_252_32;
+double JTcp1_352_32;
+double JTcp1_152_33;
+double JTcp1_252_33;
+double JTcp1_352_33;
+double JTcp1_152_34;
+double JTcp1_252_34;
+double JTcp1_352_34;
+double JTcp1_152_35;
+double JTcp1_252_35;
+double JTcp1_352_35;
+double JTcp1_152_36;
+double JTcp1_252_36;
+double JTcp1_352_36;
+double JTcp1_152_37;
+double JTcp1_252_37;
+double JTcp1_352_37;
+double JTcp1_152_38;
+double JTcp1_252_38;
+double JTcp1_352_38;
+double JTcp1_152_39;
+double JTcp1_252_39;
+double JTcp1_352_39;
+double JTcp1_152_40;
+double JTcp1_252_40;
+double JTcp1_352_40;
+double JTcp1_152_41;
+double JTcp1_252_41;
+double JTcp1_352_41;
+double JTcp1_152_42;
+double JTcp1_252_42;
+double JTcp1_352_42;
+double JTcp1_152_43;
+double JTcp1_252_43;
+double JTcp1_352_43;
+double JTcp1_152_44;
+double JTcp1_252_44;
+double JTcp1_352_44;
+double JTcp1_152_46;
+double JTcp1_252_46;
+double JTcp1_352_46;
+double JTcp1_152_48;
+double JTcp1_252_48;
+double JTcp1_352_48;
+double JTcp1_152_49;
+double JTcp1_252_49;
+double JTcp1_352_49;
+double JTcp1_152_50;
+double JTcp1_252_50;
+double JTcp1_352_50;
+double JTcp1_152_51;
+double JTcp1_252_51;
+double JTcp1_352_51;
+double ORcp1_152;
+double ORcp1_252;
+double ORcp1_352;
+double VIcp1_152;
+double VIcp1_252;
+double VIcp1_352;
+double ACcp1_152;
+double ACcp1_252;
+double ACcp1_352;
+double OMcp1_153;
+double OMcp1_253;
+double OMcp1_353;
+double OPcp1_153;
+double OPcp1_253;
+double OPcp1_353;
+double ROcp2_25;
+double ROcp2_35;
+double ROcp2_85;
+double ROcp2_95;
+double ROcp2_16;
+double ROcp2_26;
+double ROcp2_36;
+double ROcp2_46;
+double ROcp2_56;
+double ROcp2_66;
+double ROcp2_47;
+double ROcp2_57;
+double ROcp2_67;
+double ROcp2_77;
+double ROcp2_87;
+double ROcp2_97;
+double ROcp2_49;
+double ROcp2_59;
+double ROcp2_69;
+double ROcp2_79;
+double ROcp2_89;
+double ROcp2_99;
+double ROcp2_110;
+double ROcp2_210;
+double ROcp2_310;
+double ROcp2_710;
+double ROcp2_810;
+double ROcp2_910;
+double ROcp2_111;
+double ROcp2_211;
+double ROcp2_311;
+double ROcp2_411;
+double ROcp2_511;
+double ROcp2_611;
+double ROcp2_413;
+double ROcp2_513;
+double ROcp2_613;
+double ROcp2_713;
+double ROcp2_813;
+double ROcp2_913;
+double ROcp2_114;
+double ROcp2_214;
+double ROcp2_314;
+double ROcp2_714;
+double ROcp2_814;
+double ROcp2_914;
+double ROcp2_115;
+double ROcp2_215;
+double ROcp2_315;
+double ROcp2_415;
+double ROcp2_515;
+double ROcp2_615;
+double ROcp2_117;
+double ROcp2_217;
+double ROcp2_317;
+double ROcp2_417;
+double ROcp2_517;
+double ROcp2_617;
+double ROcp2_118;
+double ROcp2_218;
+double ROcp2_318;
+double ROcp2_418;
+double ROcp2_518;
+double ROcp2_618;
+double ROcp2_420;
+double ROcp2_520;
+double ROcp2_620;
+double ROcp2_720;
+double ROcp2_820;
+double ROcp2_920;
+double ROcp2_121;
+double ROcp2_221;
+double ROcp2_321;
+double ROcp2_721;
+double ROcp2_821;
+double ROcp2_921;
+double ROcp2_122;
+double ROcp2_222;
+double ROcp2_322;
+double ROcp2_422;
+double ROcp2_522;
+double ROcp2_622;
+double ROcp2_125;
+double ROcp2_225;
+double ROcp2_325;
+double ROcp2_425;
+double ROcp2_525;
+double ROcp2_625;
+double ROcp2_427;
+double ROcp2_527;
+double ROcp2_627;
+double ROcp2_727;
+double ROcp2_827;
+double ROcp2_927;
+double ROcp2_128;
+double ROcp2_228;
+double ROcp2_328;
+double ROcp2_428;
+double ROcp2_528;
+double ROcp2_628;
+double ROcp2_429;
+double ROcp2_529;
+double ROcp2_629;
+double ROcp2_729;
+double ROcp2_829;
+double ROcp2_929;
+double ROcp2_130;
+double ROcp2_230;
+double ROcp2_330;
+double ROcp2_430;
+double ROcp2_530;
+double ROcp2_630;
+double ROcp2_431;
+double ROcp2_531;
+double ROcp2_631;
+double ROcp2_731;
+double ROcp2_831;
+double ROcp2_931;
+double ROcp2_432;
+double ROcp2_532;
+double ROcp2_632;
+double ROcp2_732;
+double ROcp2_832;
+double ROcp2_932;
+double ROcp2_133;
+double ROcp2_233;
+double ROcp2_333;
+double ROcp2_733;
+double ROcp2_833;
+double ROcp2_933;
+double ROcp2_134;
+double ROcp2_234;
+double ROcp2_334;
+double ROcp2_434;
+double ROcp2_534;
+double ROcp2_634;
+double ROcp2_135;
+double ROcp2_235;
+double ROcp2_335;
+double ROcp2_435;
+double ROcp2_535;
+double ROcp2_635;
+double ROcp2_436;
+double ROcp2_536;
+double ROcp2_636;
+double ROcp2_736;
+double ROcp2_836;
+double ROcp2_936;
+double ROcp2_137;
+double ROcp2_237;
+double ROcp2_337;
+double ROcp2_737;
+double ROcp2_837;
+double ROcp2_937;
+double ROcp2_138;
+double ROcp2_238;
+double ROcp2_338;
+double ROcp2_438;
+double ROcp2_538;
+double ROcp2_638;
+double ROcp2_439;
+double ROcp2_539;
+double ROcp2_639;
+double ROcp2_739;
+double ROcp2_839;
+double ROcp2_939;
+double ROcp2_140;
+double ROcp2_240;
+double ROcp2_340;
+double ROcp2_440;
+double ROcp2_540;
+double ROcp2_640;
+double ROcp2_441;
+double ROcp2_541;
+double ROcp2_641;
+double ROcp2_741;
+double ROcp2_841;
+double ROcp2_941;
+double ROcp2_442;
+double ROcp2_542;
+double ROcp2_642;
+double ROcp2_742;
+double ROcp2_842;
+double ROcp2_942;
+double ROcp2_143;
+double ROcp2_243;
+double ROcp2_343;
+double ROcp2_443;
+double ROcp2_543;
+double ROcp2_643;
+double ROcp2_444;
+double ROcp2_544;
+double ROcp2_644;
+double ROcp2_744;
+double ROcp2_844;
+double ROcp2_944;
+double ROcp2_446;
+double ROcp2_546;
+double ROcp2_646;
+double ROcp2_746;
+double ROcp2_846;
+double ROcp2_946;
+double ROcp2_448;
+double ROcp2_548;
+double ROcp2_648;
+double ROcp2_748;
+double ROcp2_848;
+double ROcp2_948;
+double ROcp2_149;
+double ROcp2_249;
+double ROcp2_349;
+double ROcp2_449;
+double ROcp2_549;
+double ROcp2_649;
+double ROcp2_450;
+double ROcp2_550;
+double ROcp2_650;
+double ROcp2_750;
+double ROcp2_850;
+double ROcp2_950;
+double ROcp2_451;
+double ROcp2_551;
+double ROcp2_651;
+double ROcp2_751;
+double ROcp2_851;
+double ROcp2_951;
+double ROcp2_153;
+double ROcp2_253;
+double ROcp2_353;
+double ROcp2_453;
+double ROcp2_553;
+double ROcp2_653;
+double ROcp2_155;
+double ROcp2_255;
+double ROcp2_355;
+double ROcp2_755;
+double ROcp2_855;
+double ROcp2_955;
+double ROcp2_156;
+double ROcp2_256;
+double ROcp2_356;
+double ROcp2_456;
+double ROcp2_556;
+double ROcp2_656;
+double OMcp2_25;
+double OMcp2_35;
+double OMcp2_16;
+double OMcp2_26;
+double OMcp2_36;
+double OPcp2_16;
+double OPcp2_26;
+double OPcp2_36;
+double RLcp2_18;
+double RLcp2_28;
+double RLcp2_38;
+double ORcp2_18;
+double ORcp2_28;
+double ORcp2_38;
+double RLcp2_112;
+double RLcp2_212;
+double RLcp2_312;
+double ORcp2_112;
+double ORcp2_212;
+double ORcp2_312;
+double RLcp2_116;
+double RLcp2_216;
+double RLcp2_316;
+double ORcp2_116;
+double ORcp2_216;
+double ORcp2_316;
+double OMcp2_118;
+double OMcp2_218;
+double OMcp2_318;
+double OPcp2_118;
+double OPcp2_218;
+double OPcp2_318;
+double RLcp2_119;
+double RLcp2_219;
+double RLcp2_319;
+double ORcp2_119;
+double ORcp2_219;
+double ORcp2_319;
+double RLcp2_120;
+double RLcp2_220;
+double RLcp2_320;
+double OMcp2_120;
+double OMcp2_220;
+double OMcp2_320;
+double ORcp2_120;
+double ORcp2_220;
+double ORcp2_320;
+double OMcp2_121;
+double OMcp2_221;
+double OMcp2_321;
+double OMcp2_122;
+double OMcp2_222;
+double OMcp2_322;
+double OPcp2_122;
+double OPcp2_222;
+double OPcp2_322;
+double RLcp2_123;
+double RLcp2_223;
+double RLcp2_323;
+double ORcp2_123;
+double ORcp2_223;
+double ORcp2_323;
+double RLcp2_124;
+double RLcp2_224;
+double RLcp2_324;
+double ORcp2_124;
+double ORcp2_224;
+double ORcp2_324;
+double RLcp2_126;
+double RLcp2_226;
+double RLcp2_326;
+double ORcp2_126;
+double ORcp2_226;
+double ORcp2_326;
+double OMcp2_130;
+double OMcp2_230;
+double OMcp2_330;
+double OMcp2_135;
+double OMcp2_235;
+double OMcp2_335;
+double OMcp2_140;
+double OMcp2_240;
+double OMcp2_340;
+double OPcp2_140;
+double OPcp2_240;
+double OPcp2_340;
+double RLcp2_142;
+double RLcp2_242;
+double RLcp2_342;
+double ORcp2_142;
+double ORcp2_242;
+double ORcp2_342;
+double OMcp2_143;
+double OMcp2_243;
+double OMcp2_343;
+double OPcp2_143;
+double OPcp2_243;
+double OPcp2_343;
+double RLcp2_145;
+double RLcp2_245;
+double RLcp2_345;
+double ORcp2_145;
+double ORcp2_245;
+double ORcp2_345;
+double RLcp2_146;
+double RLcp2_246;
+double RLcp2_346;
+double ORcp2_146;
+double ORcp2_246;
+double ORcp2_346;
+double RLcp2_147;
+double RLcp2_247;
+double RLcp2_347;
+double ORcp2_147;
+double ORcp2_247;
+double ORcp2_347;
+double RLcp2_148;
+double RLcp2_248;
+double RLcp2_348;
+double ORcp2_148;
+double ORcp2_248;
+double ORcp2_348;
+double OMcp2_149;
+double OMcp2_249;
+double OMcp2_349;
+double OPcp2_149;
+double OPcp2_249;
+double OPcp2_349;
+double RLcp2_151;
+double RLcp2_251;
+double RLcp2_351;
+double ORcp2_151;
+double ORcp2_251;
+double ORcp2_351;
+double RLcp2_152;
+double RLcp2_252;
+double RLcp2_352;
+double ORcp2_152;
+double ORcp2_252;
+double ORcp2_352;
+double OMcp2_153;
+double OMcp2_253;
+double OMcp2_353;
+double OPcp2_153;
+double OPcp2_253;
+double OPcp2_353;
+double RLcp2_154;
+double RLcp2_254;
+double RLcp2_354;
+double POcp2_154;
+double POcp2_254;
+double POcp2_354;
+double JTcp2_254_4;
+double JTcp2_354_4;
+double JTcp2_154_5;
+double JTcp2_254_5;
+double JTcp2_354_5;
+double JTcp2_154_6;
+double JTcp2_254_6;
+double JTcp2_354_6;
//double JTcp2_154_7;
//double JTcp2_254_7;
//double JTcp2_354_7;
@@ -1416,18 +1416,18 @@ double JTcp2_354_6;
//double JTcp2_154_17;
//double JTcp2_254_17;
//double JTcp2_354_17;
-double JTcp2_154_18;
-double JTcp2_254_18;
-double JTcp2_354_18;
-double JTcp2_154_20;
-double JTcp2_254_20;
-double JTcp2_354_20;
-double JTcp2_154_21;
-double JTcp2_254_21;
-double JTcp2_354_21;
-double JTcp2_154_22;
-double JTcp2_254_22;
-double JTcp2_354_22;
+double JTcp2_154_18;
+double JTcp2_254_18;
+double JTcp2_354_18;
+double JTcp2_154_20;
+double JTcp2_254_20;
+double JTcp2_354_20;
+double JTcp2_154_21;
+double JTcp2_254_21;
+double JTcp2_354_21;
+double JTcp2_154_22;
+double JTcp2_254_22;
+double JTcp2_354_22;
//double JTcp2_154_25;
//double JTcp2_254_25;
//double JTcp2_354_25;
@@ -1440,9 +1440,9 @@ double JTcp2_354_22;
//double JTcp2_154_29;
//double JTcp2_254_29;
//double JTcp2_354_29;
-double JTcp2_154_30;
-double JTcp2_254_30;
-double JTcp2_354_30;
+double JTcp2_154_30;
+double JTcp2_254_30;
+double JTcp2_354_30;
//double JTcp2_154_31;
//double JTcp2_254_31;
//double JTcp2_354_31;
@@ -1455,9 +1455,9 @@ double JTcp2_354_30;
//double JTcp2_154_34;
//double JTcp2_254_34;
//double JTcp2_354_34;
-double JTcp2_154_35;
-double JTcp2_254_35;
-double JTcp2_354_35;
+double JTcp2_154_35;
+double JTcp2_254_35;
+double JTcp2_354_35;
//double JTcp2_154_36;
//double JTcp2_254_36;
//double JTcp2_354_36;
@@ -1470,9 +1470,9 @@ double JTcp2_354_35;
//double JTcp2_154_39;
//double JTcp2_254_39;
//double JTcp2_354_39;
-double JTcp2_154_40;
-double JTcp2_254_40;
-double JTcp2_354_40;
+double JTcp2_154_40;
+double JTcp2_254_40;
+double JTcp2_354_40;
//double JTcp2_154_41;
//double JTcp2_254_41;
//double JTcp2_354_41;
@@ -1491,33 +1491,33 @@ double JTcp2_354_40;
//double JTcp2_154_48;
//double JTcp2_254_48;
//double JTcp2_354_48;
-double JTcp2_154_49;
-double JTcp2_254_49;
-double JTcp2_354_49;
+double JTcp2_154_49;
+double JTcp2_254_49;
+double JTcp2_354_49;
//double JTcp2_154_50;
//double JTcp2_254_50;
//double JTcp2_354_50;
//double JTcp2_154_51;
//double JTcp2_254_51;
//double JTcp2_354_51;
-double JTcp2_154_53;
-double JTcp2_254_53;
-double JTcp2_354_53;
-double ORcp2_154;
-double ORcp2_254;
-double ORcp2_354;
-double VIcp2_154;
-double VIcp2_254;
-double VIcp2_354;
-double ACcp2_154;
-double ACcp2_254;
-double ACcp2_354;
-double OMcp2_155;
-double OMcp2_255;
-double OMcp2_355;
-double OMcp2_156;
-double OMcp2_256;
-double OMcp2_356;
-double OPcp2_156;
-double OPcp2_256;
-double OPcp2_356;
+double JTcp2_154_53;
+double JTcp2_254_53;
+double JTcp2_354_53;
+double ORcp2_154;
+double ORcp2_254;
+double ORcp2_354;
+double VIcp2_154;
+double VIcp2_254;
+double VIcp2_354;
+double ACcp2_154;
+double ACcp2_254;
+double ACcp2_354;
+double OMcp2_155;
+double OMcp2_255;
+double OMcp2_355;
+double OMcp2_156;
+double OMcp2_256;
+double OMcp2_356;
+double OPcp2_156;
+double OPcp2_256;
+double OPcp2_356;
diff --git a/source/RobotComponents/components/Feedforward/symbolic_routines/mbs_sensor_ArmarIV-GazeStab-virt.h b/source/RobotComponents/components/Feedforward/symbolic_routines/mbs_sensor_ArmarIV-GazeStab-virt.h
index 41b40b3b42c96651b03438ce0f263a75e3b73ff9..593738b558552859ad54fff45245c1cb9f6b0145 100644
--- a/source/RobotComponents/components/Feedforward/symbolic_routines/mbs_sensor_ArmarIV-GazeStab-virt.h
+++ b/source/RobotComponents/components/Feedforward/symbolic_routines/mbs_sensor_ArmarIV-GazeStab-virt.h
@@ -1,206 +1,206 @@
-double C4;
-double S4;
-double C5;
-double S5;
-double C6;
-double S6;
-double C7;
-double S7;
-double C8;
-double S8;
-double C9;
-double S9;
-double C10;
-double S10;
-double C11;
-double S11;
-double C12;
-double S12;
-double C13;
-double S13;
-double C15;
-double S15;
-double C16;
-double S16;
-double ROcp0_25;
-double ROcp0_35;
-double ROcp0_85;
-double ROcp0_95;
-double ROcp0_16;
-double ROcp0_26;
-double ROcp0_36;
-double ROcp0_46;
-double ROcp0_56;
-double ROcp0_66;
-double ROcp0_47;
-double ROcp0_57;
-double ROcp0_67;
-double ROcp0_77;
-double ROcp0_87;
-double ROcp0_97;
-double ROcp0_18;
-double ROcp0_28;
-double ROcp0_38;
-double ROcp0_78;
-double ROcp0_88;
-double ROcp0_98;
-double ROcp0_19;
-double ROcp0_29;
-double ROcp0_39;
-double ROcp0_49;
-double ROcp0_59;
-double ROcp0_69;
-double ROcp0_410;
-double ROcp0_510;
-double ROcp0_610;
-double ROcp0_710;
-double ROcp0_810;
-double ROcp0_910;
-double ROcp0_111;
-double ROcp0_211;
-double ROcp0_311;
-double ROcp0_711;
-double ROcp0_811;
-double ROcp0_911;
-double ROcp0_412;
-double ROcp0_512;
-double ROcp0_612;
-double ROcp0_712;
-double ROcp0_812;
-double ROcp0_912;
-double ROcp0_113;
-double ROcp0_213;
-double ROcp0_313;
-double ROcp0_413;
-double ROcp0_513;
-double ROcp0_613;
-double ROcp0_415;
-double ROcp0_515;
-double ROcp0_615;
-double ROcp0_715;
-double ROcp0_815;
-double ROcp0_915;
-double ROcp0_116;
-double ROcp0_216;
-double ROcp0_316;
-double ROcp0_416;
-double ROcp0_516;
-double ROcp0_616;
-double OMcp0_25;
-double OMcp0_35;
-double OMcp0_16;
-double OMcp0_26;
-double OMcp0_36;
-double OMcp0_17;
-double OMcp0_27;
-double OMcp0_37;
-double OMcp0_18;
-double OMcp0_28;
-double OMcp0_38;
-double OPcp0_18;
-double OPcp0_28;
-double OPcp0_38;
-double RLcp0_19;
-double RLcp0_29;
-double RLcp0_39;
-double OMcp0_19;
-double OMcp0_29;
-double OMcp0_39;
-double ORcp0_19;
-double ORcp0_29;
-double ORcp0_39;
-double OPcp0_19;
-double OPcp0_29;
-double OPcp0_39;
-double RLcp0_110;
-double RLcp0_210;
-double RLcp0_310;
-double OMcp0_110;
-double OMcp0_210;
-double OMcp0_310;
-double ORcp0_110;
-double ORcp0_210;
-double ORcp0_310;
-double OPcp0_110;
-double OPcp0_210;
-double OPcp0_310;
-double RLcp0_111;
-double RLcp0_211;
-double RLcp0_311;
-double OMcp0_111;
-double OMcp0_211;
-double OMcp0_311;
-double ORcp0_111;
-double ORcp0_211;
-double ORcp0_311;
-double OPcp0_111;
-double OPcp0_211;
-double OPcp0_311;
-double RLcp0_112;
-double RLcp0_212;
-double RLcp0_312;
-double OMcp0_112;
-double OMcp0_212;
-double OMcp0_312;
-double ORcp0_112;
-double ORcp0_212;
-double ORcp0_312;
-double OMcp0_113;
-double OMcp0_213;
-double OMcp0_313;
-double OPcp0_113;
-double OPcp0_213;
-double OPcp0_313;
-double RLcp0_114;
-double RLcp0_214;
-double RLcp0_314;
-double POcp0_114;
-double POcp0_214;
-double POcp0_314;
-double JTcp0_214_4;
-double JTcp0_314_4;
-double JTcp0_114_5;
-double JTcp0_214_5;
-double JTcp0_314_5;
-double JTcp0_114_6;
-double JTcp0_214_6;
-double JTcp0_314_6;
-double JTcp0_114_7;
-double JTcp0_214_7;
-double JTcp0_314_7;
-double JTcp0_114_8;
-double JTcp0_214_8;
-double JTcp0_314_8;
-double JTcp0_114_9;
-double JTcp0_214_9;
-double JTcp0_314_9;
-double JTcp0_114_10;
-double JTcp0_214_10;
-double JTcp0_314_10;
-double JTcp0_114_11;
-double JTcp0_214_11;
-double JTcp0_314_11;
-double JTcp0_114_12;
-double JTcp0_214_12;
-double JTcp0_314_12;
-double JTcp0_114_13;
-double JTcp0_214_13;
-double JTcp0_314_13;
-double ORcp0_114;
-double ORcp0_214;
-double ORcp0_314;
-double VIcp0_114;
-double VIcp0_214;
-double VIcp0_314;
-double ACcp0_114;
-double ACcp0_214;
-double ACcp0_314;
-double OMcp0_115;
-double OMcp0_215;
-double OMcp0_315;
-double OMcp0_116;
-double OMcp0_216;
-double OMcp0_316;
-double OPcp0_116;
-double OPcp0_216;
-double OPcp0_316;
+double C4;
+double S4;
+double C5;
+double S5;
+double C6;
+double S6;
+double C7;
+double S7;
+double C8;
+double S8;
+double C9;
+double S9;
+double C10;
+double S10;
+double C11;
+double S11;
+double C12;
+double S12;
+double C13;
+double S13;
+double C15;
+double S15;
+double C16;
+double S16;
+double ROcp0_25;
+double ROcp0_35;
+double ROcp0_85;
+double ROcp0_95;
+double ROcp0_16;
+double ROcp0_26;
+double ROcp0_36;
+double ROcp0_46;
+double ROcp0_56;
+double ROcp0_66;
+double ROcp0_47;
+double ROcp0_57;
+double ROcp0_67;
+double ROcp0_77;
+double ROcp0_87;
+double ROcp0_97;
+double ROcp0_18;
+double ROcp0_28;
+double ROcp0_38;
+double ROcp0_78;
+double ROcp0_88;
+double ROcp0_98;
+double ROcp0_19;
+double ROcp0_29;
+double ROcp0_39;
+double ROcp0_49;
+double ROcp0_59;
+double ROcp0_69;
+double ROcp0_410;
+double ROcp0_510;
+double ROcp0_610;
+double ROcp0_710;
+double ROcp0_810;
+double ROcp0_910;
+double ROcp0_111;
+double ROcp0_211;
+double ROcp0_311;
+double ROcp0_711;
+double ROcp0_811;
+double ROcp0_911;
+double ROcp0_412;
+double ROcp0_512;
+double ROcp0_612;
+double ROcp0_712;
+double ROcp0_812;
+double ROcp0_912;
+double ROcp0_113;
+double ROcp0_213;
+double ROcp0_313;
+double ROcp0_413;
+double ROcp0_513;
+double ROcp0_613;
+double ROcp0_415;
+double ROcp0_515;
+double ROcp0_615;
+double ROcp0_715;
+double ROcp0_815;
+double ROcp0_915;
+double ROcp0_116;
+double ROcp0_216;
+double ROcp0_316;
+double ROcp0_416;
+double ROcp0_516;
+double ROcp0_616;
+double OMcp0_25;
+double OMcp0_35;
+double OMcp0_16;
+double OMcp0_26;
+double OMcp0_36;
+double OMcp0_17;
+double OMcp0_27;
+double OMcp0_37;
+double OMcp0_18;
+double OMcp0_28;
+double OMcp0_38;
+double OPcp0_18;
+double OPcp0_28;
+double OPcp0_38;
+double RLcp0_19;
+double RLcp0_29;
+double RLcp0_39;
+double OMcp0_19;
+double OMcp0_29;
+double OMcp0_39;
+double ORcp0_19;
+double ORcp0_29;
+double ORcp0_39;
+double OPcp0_19;
+double OPcp0_29;
+double OPcp0_39;
+double RLcp0_110;
+double RLcp0_210;
+double RLcp0_310;
+double OMcp0_110;
+double OMcp0_210;
+double OMcp0_310;
+double ORcp0_110;
+double ORcp0_210;
+double ORcp0_310;
+double OPcp0_110;
+double OPcp0_210;
+double OPcp0_310;
+double RLcp0_111;
+double RLcp0_211;
+double RLcp0_311;
+double OMcp0_111;
+double OMcp0_211;
+double OMcp0_311;
+double ORcp0_111;
+double ORcp0_211;
+double ORcp0_311;
+double OPcp0_111;
+double OPcp0_211;
+double OPcp0_311;
+double RLcp0_112;
+double RLcp0_212;
+double RLcp0_312;
+double OMcp0_112;
+double OMcp0_212;
+double OMcp0_312;
+double ORcp0_112;
+double ORcp0_212;
+double ORcp0_312;
+double OMcp0_113;
+double OMcp0_213;
+double OMcp0_313;
+double OPcp0_113;
+double OPcp0_213;
+double OPcp0_313;
+double RLcp0_114;
+double RLcp0_214;
+double RLcp0_314;
+double POcp0_114;
+double POcp0_214;
+double POcp0_314;
+double JTcp0_214_4;
+double JTcp0_314_4;
+double JTcp0_114_5;
+double JTcp0_214_5;
+double JTcp0_314_5;
+double JTcp0_114_6;
+double JTcp0_214_6;
+double JTcp0_314_6;
+double JTcp0_114_7;
+double JTcp0_214_7;
+double JTcp0_314_7;
+double JTcp0_114_8;
+double JTcp0_214_8;
+double JTcp0_314_8;
+double JTcp0_114_9;
+double JTcp0_214_9;
+double JTcp0_314_9;
+double JTcp0_114_10;
+double JTcp0_214_10;
+double JTcp0_314_10;
+double JTcp0_114_11;
+double JTcp0_214_11;
+double JTcp0_314_11;
+double JTcp0_114_12;
+double JTcp0_214_12;
+double JTcp0_314_12;
+double JTcp0_114_13;
+double JTcp0_214_13;
+double JTcp0_314_13;
+double ORcp0_114;
+double ORcp0_214;
+double ORcp0_314;
+double VIcp0_114;
+double VIcp0_214;
+double VIcp0_314;
+double ACcp0_114;
+double ACcp0_214;
+double ACcp0_314;
+double OMcp0_115;
+double OMcp0_215;
+double OMcp0_315;
+double OMcp0_116;
+double OMcp0_216;
+double OMcp0_316;
+double OPcp0_116;
+double OPcp0_216;
+double OPcp0_316;
diff --git a/source/RobotComponents/components/Feedforward/symbolic_routines/mbs_sensor_ArmarIV-W-Torso-virt.h b/source/RobotComponents/components/Feedforward/symbolic_routines/mbs_sensor_ArmarIV-W-Torso-virt.h
index 9d0d96ef12ecc037aaea4733a547a73880ee082f..3dd89c7cbd27ba34ca3423632e6afca1809d8f9f 100644
--- a/source/RobotComponents/components/Feedforward/symbolic_routines/mbs_sensor_ArmarIV-W-Torso-virt.h
+++ b/source/RobotComponents/components/Feedforward/symbolic_routines/mbs_sensor_ArmarIV-W-Torso-virt.h
@@ -1,252 +1,252 @@
-double C4;
-double S4;
-double C5;
-double S5;
-double C6;
-double S6;
-double C7;
-double S7;
-double C8;
-double S8;
-double C9;
-double S9;
-double C10;
-double S10;
-double C11;
-double S11;
-double C12;
-double S12;
-double C13;
-double S13;
-double C14;
-double S14;
-double C15;
-double S15;
-double C17;
-double S17;
-double C18;
-double S18;
-double ROcp0_25;
-double ROcp0_35;
-double ROcp0_85;
-double ROcp0_95;
-double ROcp0_26;
-double ROcp0_36;
-double ROcp0_56;
-double ROcp0_66;
-double C6p7;
-double ROcp0_17;
-double ROcp0_27;
-double ROcp0_37;
-double S6p7;
-double ROcp0_47;
-double ROcp0_57;
-double ROcp0_67;
-double ROcp0_48;
-double ROcp0_58;
-double ROcp0_68;
-double ROcp0_78;
-double ROcp0_88;
-double ROcp0_98;
-double ROcp0_49;
-double ROcp0_59;
-double ROcp0_69;
-double ROcp0_79;
-double ROcp0_89;
-double ROcp0_99;
-double ROcp0_110;
-double ROcp0_210;
-double ROcp0_310;
-double ROcp0_710;
-double ROcp0_810;
-double ROcp0_910;
-double ROcp0_111;
-double ROcp0_211;
-double ROcp0_311;
-double ROcp0_411;
-double ROcp0_511;
-double ROcp0_611;
-double ROcp0_412;
-double ROcp0_512;
-double ROcp0_612;
-double ROcp0_712;
-double ROcp0_812;
-double ROcp0_912;
-double ROcp0_113;
-double ROcp0_213;
-double ROcp0_313;
-double ROcp0_713;
-double ROcp0_813;
-double ROcp0_913;
-double ROcp0_414;
-double ROcp0_514;
-double ROcp0_614;
-double ROcp0_714;
-double ROcp0_814;
-double ROcp0_914;
-double ROcp0_115;
-double ROcp0_215;
-double ROcp0_315;
-double ROcp0_415;
-double ROcp0_515;
-double ROcp0_615;
-double ROcp0_417;
-double ROcp0_517;
-double ROcp0_617;
-double ROcp0_717;
-double ROcp0_817;
-double ROcp0_917;
-double ROcp0_118;
-double ROcp0_218;
-double ROcp0_318;
-double ROcp0_418;
-double ROcp0_518;
-double ROcp0_618;
-double OMcp0_25;
-double OMcp0_35;
-double OMcp0_16;
-double OMcp0_26;
-double OMcp0_36;
-double OMcp0_17;
-double OMcp0_27;
-double OMcp0_37;
-double OPcp0_17;
-double OPcp0_27;
-double OPcp0_37;
-double RLcp0_18;
-double RLcp0_28;
-double RLcp0_38;
-double OMcp0_18;
-double OMcp0_28;
-double OMcp0_38;
-double ORcp0_18;
-double ORcp0_28;
-double ORcp0_38;
-double OPcp0_18;
-double OPcp0_28;
-double OPcp0_38;
-double RLcp0_19;
-double RLcp0_29;
-double RLcp0_39;
-double OMcp0_19;
-double OMcp0_29;
-double OMcp0_39;
-double ORcp0_19;
-double ORcp0_29;
-double ORcp0_39;
-double OMcp0_110;
-double OMcp0_210;
-double OMcp0_310;
-double OPcp0_110;
-double OPcp0_210;
-double OPcp0_310;
-double RLcp0_111;
-double RLcp0_211;
-double RLcp0_311;
-double OMcp0_111;
-double OMcp0_211;
-double OMcp0_311;
-double ORcp0_111;
-double ORcp0_211;
-double ORcp0_311;
-double OPcp0_111;
-double OPcp0_211;
-double OPcp0_311;
-double RLcp0_112;
-double RLcp0_212;
-double RLcp0_312;
-double OMcp0_112;
-double OMcp0_212;
-double OMcp0_312;
-double ORcp0_112;
-double ORcp0_212;
-double ORcp0_312;
-double OPcp0_112;
-double OPcp0_212;
-double OPcp0_312;
-double RLcp0_113;
-double RLcp0_213;
-double RLcp0_313;
-double OMcp0_113;
-double OMcp0_213;
-double OMcp0_313;
-double ORcp0_113;
-double ORcp0_213;
-double ORcp0_313;
-double OPcp0_113;
-double OPcp0_213;
-double OPcp0_313;
-double RLcp0_114;
-double RLcp0_214;
-double RLcp0_314;
-double OMcp0_114;
-double OMcp0_214;
-double OMcp0_314;
-double ORcp0_114;
-double ORcp0_214;
-double ORcp0_314;
-double OMcp0_115;
-double OMcp0_215;
-double OMcp0_315;
-double OPcp0_115;
-double OPcp0_215;
-double OPcp0_315;
-double RLcp0_116;
-double RLcp0_216;
-double RLcp0_316;
-double POcp0_116;
-double POcp0_216;
-double POcp0_316;
-double JTcp0_216_4;
-double JTcp0_316_4;
-double JTcp0_116_5;
-double JTcp0_216_5;
-double JTcp0_316_5;
-double JTcp0_116_6;
-double JTcp0_216_6;
-double JTcp0_316_6;
-double JTcp0_116_7;
-double JTcp0_216_7;
-double JTcp0_316_7;
-double JTcp0_116_8;
-double JTcp0_216_8;
-double JTcp0_316_8;
-double JTcp0_116_9;
-double JTcp0_216_9;
-double JTcp0_316_9;
-double JTcp0_116_10;
-double JTcp0_216_10;
-double JTcp0_316_10;
-double JTcp0_116_11;
-double JTcp0_216_11;
-double JTcp0_316_11;
-double JTcp0_116_12;
-double JTcp0_216_12;
-double JTcp0_316_12;
-double JTcp0_116_13;
-double JTcp0_216_13;
-double JTcp0_316_13;
-double JTcp0_116_14;
-double JTcp0_216_14;
-double JTcp0_316_14;
-double JTcp0_116_15;
-double JTcp0_216_15;
-double JTcp0_316_15;
-double ORcp0_116;
-double ORcp0_216;
-double ORcp0_316;
-double VIcp0_116;
-double VIcp0_216;
-double VIcp0_316;
-double ACcp0_116;
-double ACcp0_216;
-double ACcp0_316;
-double OMcp0_117;
-double OMcp0_217;
-double OMcp0_317;
-double OMcp0_118;
-double OMcp0_218;
-double OMcp0_318;
-double OPcp0_118;
-double OPcp0_218;
-double OPcp0_318;
+double C4;
+double S4;
+double C5;
+double S5;
+double C6;
+double S6;
+double C7;
+double S7;
+double C8;
+double S8;
+double C9;
+double S9;
+double C10;
+double S10;
+double C11;
+double S11;
+double C12;
+double S12;
+double C13;
+double S13;
+double C14;
+double S14;
+double C15;
+double S15;
+double C17;
+double S17;
+double C18;
+double S18;
+double ROcp0_25;
+double ROcp0_35;
+double ROcp0_85;
+double ROcp0_95;
+double ROcp0_26;
+double ROcp0_36;
+double ROcp0_56;
+double ROcp0_66;
+double C6p7;
+double ROcp0_17;
+double ROcp0_27;
+double ROcp0_37;
+double S6p7;
+double ROcp0_47;
+double ROcp0_57;
+double ROcp0_67;
+double ROcp0_48;
+double ROcp0_58;
+double ROcp0_68;
+double ROcp0_78;
+double ROcp0_88;
+double ROcp0_98;
+double ROcp0_49;
+double ROcp0_59;
+double ROcp0_69;
+double ROcp0_79;
+double ROcp0_89;
+double ROcp0_99;
+double ROcp0_110;
+double ROcp0_210;
+double ROcp0_310;
+double ROcp0_710;
+double ROcp0_810;
+double ROcp0_910;
+double ROcp0_111;
+double ROcp0_211;
+double ROcp0_311;
+double ROcp0_411;
+double ROcp0_511;
+double ROcp0_611;
+double ROcp0_412;
+double ROcp0_512;
+double ROcp0_612;
+double ROcp0_712;
+double ROcp0_812;
+double ROcp0_912;
+double ROcp0_113;
+double ROcp0_213;
+double ROcp0_313;
+double ROcp0_713;
+double ROcp0_813;
+double ROcp0_913;
+double ROcp0_414;
+double ROcp0_514;
+double ROcp0_614;
+double ROcp0_714;
+double ROcp0_814;
+double ROcp0_914;
+double ROcp0_115;
+double ROcp0_215;
+double ROcp0_315;
+double ROcp0_415;
+double ROcp0_515;
+double ROcp0_615;
+double ROcp0_417;
+double ROcp0_517;
+double ROcp0_617;
+double ROcp0_717;
+double ROcp0_817;
+double ROcp0_917;
+double ROcp0_118;
+double ROcp0_218;
+double ROcp0_318;
+double ROcp0_418;
+double ROcp0_518;
+double ROcp0_618;
+double OMcp0_25;
+double OMcp0_35;
+double OMcp0_16;
+double OMcp0_26;
+double OMcp0_36;
+double OMcp0_17;
+double OMcp0_27;
+double OMcp0_37;
+double OPcp0_17;
+double OPcp0_27;
+double OPcp0_37;
+double RLcp0_18;
+double RLcp0_28;
+double RLcp0_38;
+double OMcp0_18;
+double OMcp0_28;
+double OMcp0_38;
+double ORcp0_18;
+double ORcp0_28;
+double ORcp0_38;
+double OPcp0_18;
+double OPcp0_28;
+double OPcp0_38;
+double RLcp0_19;
+double RLcp0_29;
+double RLcp0_39;
+double OMcp0_19;
+double OMcp0_29;
+double OMcp0_39;
+double ORcp0_19;
+double ORcp0_29;
+double ORcp0_39;
+double OMcp0_110;
+double OMcp0_210;
+double OMcp0_310;
+double OPcp0_110;
+double OPcp0_210;
+double OPcp0_310;
+double RLcp0_111;
+double RLcp0_211;
+double RLcp0_311;
+double OMcp0_111;
+double OMcp0_211;
+double OMcp0_311;
+double ORcp0_111;
+double ORcp0_211;
+double ORcp0_311;
+double OPcp0_111;
+double OPcp0_211;
+double OPcp0_311;
+double RLcp0_112;
+double RLcp0_212;
+double RLcp0_312;
+double OMcp0_112;
+double OMcp0_212;
+double OMcp0_312;
+double ORcp0_112;
+double ORcp0_212;
+double ORcp0_312;
+double OPcp0_112;
+double OPcp0_212;
+double OPcp0_312;
+double RLcp0_113;
+double RLcp0_213;
+double RLcp0_313;
+double OMcp0_113;
+double OMcp0_213;
+double OMcp0_313;
+double ORcp0_113;
+double ORcp0_213;
+double ORcp0_313;
+double OPcp0_113;
+double OPcp0_213;
+double OPcp0_313;
+double RLcp0_114;
+double RLcp0_214;
+double RLcp0_314;
+double OMcp0_114;
+double OMcp0_214;
+double OMcp0_314;
+double ORcp0_114;
+double ORcp0_214;
+double ORcp0_314;
+double OMcp0_115;
+double OMcp0_215;
+double OMcp0_315;
+double OPcp0_115;
+double OPcp0_215;
+double OPcp0_315;
+double RLcp0_116;
+double RLcp0_216;
+double RLcp0_316;
+double POcp0_116;
+double POcp0_216;
+double POcp0_316;
+double JTcp0_216_4;
+double JTcp0_316_4;
+double JTcp0_116_5;
+double JTcp0_216_5;
+double JTcp0_316_5;
+double JTcp0_116_6;
+double JTcp0_216_6;
+double JTcp0_316_6;
+double JTcp0_116_7;
+double JTcp0_216_7;
+double JTcp0_316_7;
+double JTcp0_116_8;
+double JTcp0_216_8;
+double JTcp0_316_8;
+double JTcp0_116_9;
+double JTcp0_216_9;
+double JTcp0_316_9;
+double JTcp0_116_10;
+double JTcp0_216_10;
+double JTcp0_316_10;
+double JTcp0_116_11;
+double JTcp0_216_11;
+double JTcp0_316_11;
+double JTcp0_116_12;
+double JTcp0_216_12;
+double JTcp0_316_12;
+double JTcp0_116_13;
+double JTcp0_216_13;
+double JTcp0_316_13;
+double JTcp0_116_14;
+double JTcp0_216_14;
+double JTcp0_316_14;
+double JTcp0_116_15;
+double JTcp0_216_15;
+double JTcp0_316_15;
+double ORcp0_116;
+double ORcp0_216;
+double ORcp0_316;
+double VIcp0_116;
+double VIcp0_216;
+double VIcp0_316;
+double ACcp0_116;
+double ACcp0_216;
+double ACcp0_316;
+double OMcp0_117;
+double OMcp0_217;
+double OMcp0_317;
+double OMcp0_118;
+double OMcp0_218;
+double OMcp0_318;
+double OPcp0_118;
+double OPcp0_218;
+double OPcp0_318;
diff --git a/source/RobotComponents/components/Feedforward/symbolic_routines/symbolic_routines.h b/source/RobotComponents/components/Feedforward/symbolic_routines/symbolic_routines.h
index 08bc3439efaec8a84fe5dabccaf89665f4522f5e..4218a24872254da84b95799c2c58daad821d35eb 100644
--- a/source/RobotComponents/components/Feedforward/symbolic_routines/symbolic_routines.h
+++ b/source/RobotComponents/components/Feedforward/symbolic_routines/symbolic_routines.h
@@ -2,27 +2,22 @@
#pragma once
#ifdef __cplusplus
-extern "C" {
+extern "C"
+{
#endif
-void mbs_sensor_ArmarIV_GazeStab_virt(MbsSensor* sens,
- double* q,
- double* qd,
- double* qdd,
- int isens);
+ void mbs_sensor_ArmarIV_GazeStab_virt(MbsSensor* sens,
+ double* q,
+ double* qd,
+ double* qdd,
+ int isens);
-void mbs_sensor_ArmarIV_W_Torso_virt(MbsSensor* sens,
- double* q,
- double* qd,
- double* qdd,
- int isens);
+ void
+ mbs_sensor_ArmarIV_W_Torso_virt(MbsSensor* sens, double* q, double* qd, double* qdd, int isens);
-void mbs_sensor_ArmarIII_simplified_virt(MbsSensor* sens,
- double* q_dof,
- double* qd_dof,
- int isens);
+ void
+ mbs_sensor_ArmarIII_simplified_virt(MbsSensor* sens, double* q_dof, double* qd_dof, int isens);
#ifdef __cplusplus
}
#endif
-
diff --git a/source/RobotComponents/components/Feedforward/symbolic_routines/user_hard_param_WO_torso.h b/source/RobotComponents/components/Feedforward/symbolic_routines/user_hard_param_WO_torso.h
index e020e0b672108aac2d1b98e841de578b3b28c12a..e10a38da88897f24a16ab8ba2532b3c9cbd33bed 100644
--- a/source/RobotComponents/components/Feedforward/symbolic_routines/user_hard_param_WO_torso.h
+++ b/source/RobotComponents/components/Feedforward/symbolic_routines/user_hard_param_WO_torso.h
@@ -78,4 +78,3 @@
#define DPT_1_5 0.036
// ============================================================ //
-
diff --git a/source/RobotComponents/components/Feedforward/symbolic_routines/user_hard_param_W_torso.h b/source/RobotComponents/components/Feedforward/symbolic_routines/user_hard_param_W_torso.h
index b25beab2fc25a05467aff0003569175b272d90ed..7f47ae8cc886d0ff6310069d53fdda3dcc5e1bf0 100644
--- a/source/RobotComponents/components/Feedforward/symbolic_routines/user_hard_param_W_torso.h
+++ b/source/RobotComponents/components/Feedforward/symbolic_routines/user_hard_param_W_torso.h
@@ -92,4 +92,3 @@
#define DPT_3_6 0.07265
// ============================================================ //
-
diff --git a/source/RobotComponents/components/Feedforward/symbolic_routines/user_hard_param_armar3.h b/source/RobotComponents/components/Feedforward/symbolic_routines/user_hard_param_armar3.h
index 05095d56f8e0281f397c341d1f0ee80de0b499d8..75623d696c5bb4a1798e96c9d3c107ab3b238fe0 100644
--- a/source/RobotComponents/components/Feedforward/symbolic_routines/user_hard_param_armar3.h
+++ b/source/RobotComponents/components/Feedforward/symbolic_routines/user_hard_param_armar3.h
@@ -139,4 +139,3 @@
#define DPT_3_20 -0.0465
// ============================================================ //
-
diff --git a/source/RobotComponents/components/FunctionApproximator/FunctionApproximator.cpp b/source/RobotComponents/components/FunctionApproximator/FunctionApproximator.cpp
index 2f953b9263ef2ba1477d79a03fabc92857a63641..8ca0c789c9b5dadad48f41474d0bfad4ed12487f 100644
--- a/source/RobotComponents/components/FunctionApproximator/FunctionApproximator.cpp
+++ b/source/RobotComponents/components/FunctionApproximator/FunctionApproximator.cpp
@@ -25,38 +25,37 @@
using namespace armarx;
-
-void FunctionApproximator::onInitComponent()
+void
+FunctionApproximator::onInitComponent()
{
ARMARX_INFO << "initializing DMP component";
-
- ARMARX_INFO << "successfully initialized DMP component" ;
-
+ ARMARX_INFO << "successfully initialized DMP component";
}
-
-void FunctionApproximator::onConnectComponent()
+void
+FunctionApproximator::onConnectComponent()
{
-
}
-
-void FunctionApproximator::onDisconnectComponent()
+void
+FunctionApproximator::onDisconnectComponent()
{
ARMARX_INFO << "disconnecting FunctionApproximator component";
-
}
-
-void FunctionApproximator::onExitComponent()
+void
+FunctionApproximator::onExitComponent()
{
ARMARX_INFO << "exiting FunctionApproximator component";
-
}
-void FunctionApproximator::learn(const std::string& name, const DVector2d& x, const DVector2d& y, const Ice::Current&)
+void
+FunctionApproximator::learn(const std::string& name,
+ const DVector2d& x,
+ const DVector2d& y,
+ const Ice::Current&)
{
DMP::FunctionApproximationInterfacePtr approximator = funcAppPool.find(name)->second;
@@ -87,11 +86,9 @@ void FunctionApproximator::learn(const std::string& name, const DVector2d& x, co
xx[i] = cx;
yy[i] = cy;
-
}
approximator->learn(xx, yy);
-
}
else
{
@@ -100,7 +97,8 @@ void FunctionApproximator::learn(const std::string& name, const DVector2d& x, co
}
}
-Ice::Double FunctionApproximator::predict(const std::string& name, const Ice::DoubleSeq& x, const Ice::Current&)
+Ice::Double
+FunctionApproximator::predict(const std::string& name, const Ice::DoubleSeq& x, const Ice::Current&)
{
DMP::FunctionApproximationInterfacePtr approximator = funcAppPool.find(name)->second;
if (approximator)
@@ -123,7 +121,11 @@ Ice::Double FunctionApproximator::predict(const std::string& name, const Ice::Do
}
}
-void FunctionApproximator::ilearn(const std::string& name, const Ice::DoubleSeq& x, Ice::Double y, const Ice::Current&)
+void
+FunctionApproximator::ilearn(const std::string& name,
+ const Ice::DoubleSeq& x,
+ Ice::Double y,
+ const Ice::Current&)
{
DMP::FunctionApproximationInterfacePtr approximator = funcAppPool.find(name)->second;
if (approximator)
@@ -144,7 +146,11 @@ void FunctionApproximator::ilearn(const std::string& name, const Ice::DoubleSeq&
}
}
-void FunctionApproximator::blearn(const std::string& name, const Ice::DoubleSeq& x, const Ice::DoubleSeq& y, const Ice::Current&)
+void
+FunctionApproximator::blearn(const std::string& name,
+ const Ice::DoubleSeq& x,
+ const Ice::DoubleSeq& y,
+ const Ice::Current&)
{
DMP::FunctionApproximationInterfacePtr approximator = funcAppPool.find(name)->second;
if (approximator)
@@ -164,7 +170,10 @@ void FunctionApproximator::blearn(const std::string& name, const Ice::DoubleSeq&
}
}
-Ice::DoubleSeq FunctionApproximator::bpredict(const std::string& name, const Ice::DoubleSeq& x, const Ice::Current&)
+Ice::DoubleSeq
+FunctionApproximator::bpredict(const std::string& name,
+ const Ice::DoubleSeq& x,
+ const Ice::Current&)
{
DMP::FunctionApproximationInterfacePtr approximator = funcAppPool.find(name)->second;
if (approximator)
@@ -185,17 +194,16 @@ Ice::DoubleSeq FunctionApproximator::bpredict(const std::string& name, const Ice
}
}
-void FunctionApproximator::reset(const Ice::Current&)
+void
+FunctionApproximator::reset(const Ice::Current&)
{
for (FuncAppMap::iterator it = funcAppPool.begin(); it != funcAppPool.end(); ++it)
{
it->second.reset(new DMP::RBFInterpolator<DMP::GaussRadialBasisFunction>(2));
}
-
}
-
//armarx::PropertyDefinitionsPtr FunctionApproximator::createPropertyDefinitions()
//{
// return armarx::PropertyDefinitionsPtr(new FunctionApproximatorPropertyDefinitions(
@@ -203,8 +211,10 @@ void FunctionApproximator::reset(const Ice::Current&)
//}
-
-void armarx::FunctionApproximator::initialize(const std::string& fappName, const Ice::DoubleSeq& /*widthFactors*/, const Ice::Current&)
+void
+armarx::FunctionApproximator::initialize(const std::string& fappName,
+ const Ice::DoubleSeq& /*widthFactors*/,
+ const Ice::Current&)
{
//int dim = widthFactors.size();
@@ -213,8 +223,10 @@ void armarx::FunctionApproximator::initialize(const std::string& fappName, const
funcAppPool.insert(FuncAppPair(fappName, approximator));
}
-
-void armarx::FunctionApproximator::getFunctionApproximatorFromFile(const std::string& funcName, const std::string& name, const Ice::Current&)
+void
+armarx::FunctionApproximator::getFunctionApproximatorFromFile(const std::string& funcName,
+ const std::string& name,
+ const Ice::Current&)
{
std::ifstream file(name);
@@ -229,7 +241,11 @@ void armarx::FunctionApproximator::getFunctionApproximatorFromFile(const std::st
funcAppPool.insert(FuncAppPair(funcName, approximator));
}
-void armarx::FunctionApproximator::getFunctionApproximatorsFromFile(const std::vector<std::string>& funcName, const std::string& filename, const Ice::Current&)
+void
+armarx::FunctionApproximator::getFunctionApproximatorsFromFile(
+ const std::vector<std::string>& funcName,
+ const std::string& filename,
+ const Ice::Current&)
{
std::ifstream file(filename);
@@ -248,8 +264,10 @@ void armarx::FunctionApproximator::getFunctionApproximatorsFromFile(const std::v
}
}
-
-void FunctionApproximator::saveFunctionApproximatorInFile(const std::string& funcName, const std::string& name, const Ice::Current&)
+void
+FunctionApproximator::saveFunctionApproximatorInFile(const std::string& funcName,
+ const std::string& name,
+ const Ice::Current&)
{
std::cout << "save function approximator in file ... " << std::endl;
@@ -264,19 +282,21 @@ void FunctionApproximator::saveFunctionApproximatorInFile(const std::string& fun
file.close();
}
-
-
-void armarx::FunctionApproximator::initializeTest(const std::string& fappName, const Ice::DoubleSeq& widthFactors)
+void
+armarx::FunctionApproximator::initializeTest(const std::string& fappName,
+ const Ice::DoubleSeq& widthFactors)
{
int dim = widthFactors.size();
- DMP::FunctionApproximationInterfacePtr approximator(new DMP::RBFInterpolator<DMP::GaussRadialBasisFunction>(dim, widthFactors));
+ DMP::FunctionApproximationInterfacePtr approximator(
+ new DMP::RBFInterpolator<DMP::GaussRadialBasisFunction>(dim, widthFactors));
funcAppPool.insert(FuncAppPair(fappName, approximator));
}
-
-void FunctionApproximator::saveFunctionApproximatorInFileTest(const std::string& funcName, const std::string& name)
+void
+FunctionApproximator::saveFunctionApproximatorInFileTest(const std::string& funcName,
+ const std::string& name)
{
std::cout << "save function approximator in file ... " << std::endl;
@@ -302,4 +322,3 @@ void FunctionApproximator::saveFunctionApproximatorInFileTest(const std::string&
file.close();
}
-
diff --git a/source/RobotComponents/components/FunctionApproximator/FunctionApproximator.h b/source/RobotComponents/components/FunctionApproximator/FunctionApproximator.h
index a59a606fc99204ad30246880bf8dab8565b1b773..a1f028b4d43c773ccdc02de99a23e1e4e378e2ce 100644
--- a/source/RobotComponents/components/FunctionApproximator/FunctionApproximator.h
+++ b/source/RobotComponents/components/FunctionApproximator/FunctionApproximator.h
@@ -23,21 +23,21 @@
#pragma once
-#include <ArmarXCore/core/Component.h>
-#include <RobotComponents/interface/components/FunctionApproximatorBase.h>
-#include <dmp/functionapproximation/radialbasisfunctioninterpolator.h>
-#include <dmp/functionapproximation/functionapproximation.h>
-#include <dmp/functionapproximation/locallyweightedlearning.h>
-#include <dmp/functionapproximation/functionapproximationregistration.h>
-
-#include <boost/archive/text_oarchive.hpp>
+#include <boost/archive/binary_iarchive.hpp>
+#include <boost/archive/binary_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>
-
+#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/xml_iarchive.hpp>
#include <boost/archive/xml_oarchive.hpp>
-#include <boost/archive/binary_iarchive.hpp>
-#include <boost/archive/binary_oarchive.hpp>
+#include <ArmarXCore/core/Component.h>
+
+#include <RobotComponents/interface/components/FunctionApproximatorBase.h>
+
+#include <dmp/functionapproximation/functionapproximation.h>
+#include <dmp/functionapproximation/functionapproximationregistration.h>
+#include <dmp/functionapproximation/locallyweightedlearning.h>
+#include <dmp/functionapproximation/radialbasisfunctioninterpolator.h>
namespace armarx
{
@@ -45,11 +45,10 @@ namespace armarx
* @class FunctionApproximatorPropertyDefinitions
* @brief
*/
- class FunctionApproximatorPropertyDefinitions:
- public armarx::ComponentPropertyDefinitions
+ class FunctionApproximatorPropertyDefinitions : public armarx::ComponentPropertyDefinitions
{
public:
- FunctionApproximatorPropertyDefinitions(std::string prefix):
+ FunctionApproximatorPropertyDefinitions(std::string prefix) :
armarx::ComponentPropertyDefinitions(prefix)
{
//defineRequiredProperty<std::string>("PropertyName", "Description");
@@ -69,15 +68,14 @@ namespace armarx
* Detailed description of class FunctionApproximator.
*/
using FuncAppMap = std::map<std::string, DMP::FunctionApproximationInterfacePtr>;
- using FuncAppPair = std::pair<std::string, DMP::FunctionApproximationInterfacePtr> ;
+ using FuncAppPair = std::pair<std::string, DMP::FunctionApproximationInterfacePtr>;
+
class FunctionApproximator :
virtual public Component,
virtual public armarx::FunctionApproximatorBase
{
public:
-
- FunctionApproximator():
- dimension(2)
+ FunctionApproximator() : dimension(2)
{
DMP::DVec widthFactor;
widthFactor.push_back(10);
@@ -87,7 +85,8 @@ namespace armarx
/**
* @see armarx::ManagedIceObject::getDefaultName()
*/
- std::string getDefaultName() const override
+ std::string
+ getDefaultName() const override
{
return "FunctionApproximator";
}
@@ -121,32 +120,51 @@ namespace armarx
// FunctionApproximatorBase interface
public:
-
- void initialize(const std::string& fappName, const Ice::DoubleSeq& factors, const Ice::Current&) override;
- void learn(const std::string& name, const DVector2d&, const DVector2d&, const Ice::Current&) override;
- Ice::Double predict(const std::string& name, const Ice::DoubleSeq&, const Ice::Current&) override;
- void ilearn(const std::string& name, const Ice::DoubleSeq&, Ice::Double, const Ice::Current&) override;
- void blearn(const std::string& name, const Ice::DoubleSeq&, const Ice::DoubleSeq&, const Ice::Current&) override;
- Ice::DoubleSeq bpredict(const std::string& name, const Ice::DoubleSeq& x, const Ice::Current&) override;
-
- void getFunctionApproximatorFromFile(const std::string& funcName, const std::string& name, const Ice::Current&) override; //TODO: deserialization;
- void getFunctionApproximatorsFromFile(const std::vector<std::string>& funcNameList, const std::string& filename, const Ice::Current&) override;
- void saveFunctionApproximatorInFile(const std::string& funcName, const std::string& name, const Ice::Current&) override; //TODO: serialization;
+ void initialize(const std::string& fappName,
+ const Ice::DoubleSeq& factors,
+ const Ice::Current&) override;
+ void learn(const std::string& name,
+ const DVector2d&,
+ const DVector2d&,
+ const Ice::Current&) override;
+ Ice::Double
+ predict(const std::string& name, const Ice::DoubleSeq&, const Ice::Current&) override;
+ void ilearn(const std::string& name,
+ const Ice::DoubleSeq&,
+ Ice::Double,
+ const Ice::Current&) override;
+ void blearn(const std::string& name,
+ const Ice::DoubleSeq&,
+ const Ice::DoubleSeq&,
+ const Ice::Current&) override;
+ Ice::DoubleSeq
+ bpredict(const std::string& name, const Ice::DoubleSeq& x, const Ice::Current&) override;
+
+ void getFunctionApproximatorFromFile(const std::string& funcName,
+ const std::string& name,
+ const Ice::Current&) override; //TODO: deserialization;
+ void getFunctionApproximatorsFromFile(const std::vector<std::string>& funcNameList,
+ const std::string& filename,
+ const Ice::Current&) override;
+ void saveFunctionApproximatorInFile(const std::string& funcName,
+ const std::string& name,
+ const Ice::Current&) override; //TODO: serialization;
// void getWeightsFromFile(const std::string & funcName, const std::string& filename, const Ice::Current&);
// void writeWeightsToFile(const std::string & funcName, const std::string& filename, const Ice::Current&);
void reset(const Ice::Current&) override;
- void saveFunctionApproximatorInFileTest(const std::string& funcName, const std::string& name);
+ void saveFunctionApproximatorInFileTest(const std::string& funcName,
+ const std::string& name);
void initializeTest(const std::string& fappName, const Ice::DoubleSeq& widthFactors);
+
private:
FuncAppMap funcAppPool;
int dimension;
int num;
-
};
- using FunctionApproximatorPtr = ::IceInternal::Handle< ::armarx::FunctionApproximator>;
-}
+ using FunctionApproximatorPtr = ::IceInternal::Handle<::armarx::FunctionApproximator>;
+} // namespace armarx
diff --git a/source/RobotComponents/components/FunctionApproximator/test/FunctionApproximatorTest.cpp b/source/RobotComponents/components/FunctionApproximator/test/FunctionApproximatorTest.cpp
index 8b739a09365a9c54e5703da376bc4b734eb6a560..68dd19039e724c6cddd01b3713351eaf84308a48 100644
--- a/source/RobotComponents/components/FunctionApproximator/test/FunctionApproximatorTest.cpp
+++ b/source/RobotComponents/components/FunctionApproximator/test/FunctionApproximatorTest.cpp
@@ -24,11 +24,11 @@
#define ARMARX_BOOST_TEST
+#include <iostream>
+
#include <RobotComponents/Test.h>
#include <RobotComponents/components/FunctionApproximator/FunctionApproximator.h>
-#include <iostream>
-
BOOST_AUTO_TEST_CASE(testExample)
{
armarx::FunctionApproximator instance;
diff --git a/source/RobotComponents/components/GraspSelectionManager/GraspSelectionCriterionBase.cpp b/source/RobotComponents/components/GraspSelectionManager/GraspSelectionCriterionBase.cpp
index 8973ecfb196931cb5f08c977e3a790a8b503e460..c20a168b69e7de742ef0de3619621ab5fb514013 100644
--- a/source/RobotComponents/components/GraspSelectionManager/GraspSelectionCriterionBase.cpp
+++ b/source/RobotComponents/components/GraspSelectionManager/GraspSelectionCriterionBase.cpp
@@ -25,16 +25,17 @@
namespace armarx
{
- GraspSelectionCriterionBase::GraspSelectionCriterionBase()
- = default;
+ GraspSelectionCriterionBase::GraspSelectionCriterionBase() = default;
- void GraspSelectionCriterionBase::onInitComponent()
+ void
+ GraspSelectionCriterionBase::onInitComponent()
{
usingProxy(getProperty<std::string>("GraspSelectionManagerName").getValue());
onInitGraspSelectionCriterion();
}
- void GraspSelectionCriterionBase::onConnectComponent()
+ void
+ GraspSelectionCriterionBase::onConnectComponent()
{
// graspSelectionManager = getProxy<GraspSelectionManagerInterfacePrx>(getProperty<std::string>("GraspSelectionManagerName").getValue());
// graspSelectionManager->registerAsGraspSelectionCriterion(GraspSelectionCriterionInterfacePrx::uncheckedCast(getProxy()));
@@ -42,6 +43,4 @@ namespace armarx
}
-} // namespace spoac
-
-
+} // namespace armarx
diff --git a/source/RobotComponents/components/GraspSelectionManager/GraspSelectionCriterionBase.h b/source/RobotComponents/components/GraspSelectionManager/GraspSelectionCriterionBase.h
index 9957cccc046dccbb100eb0d090086c030f384d6a..b77f01236780e35ac11087a68c4ff8c03d47ebeb 100644
--- a/source/RobotComponents/components/GraspSelectionManager/GraspSelectionCriterionBase.h
+++ b/source/RobotComponents/components/GraspSelectionManager/GraspSelectionCriterionBase.h
@@ -22,21 +22,22 @@
#pragma once
-#include <RobotComponents/interface/components/GraspingManager/GraspSelectionManagerInterface.h>
-
#include <ArmarXCore/core/Component.h>
+#include <RobotComponents/interface/components/GraspingManager/GraspSelectionManagerInterface.h>
+
namespace armarx
{
- class GraspSelectionCriterionPropertyDefinitions:
- public ComponentPropertyDefinitions
+ class GraspSelectionCriterionPropertyDefinitions : public ComponentPropertyDefinitions
{
public:
- GraspSelectionCriterionPropertyDefinitions(std::string prefix):
+ GraspSelectionCriterionPropertyDefinitions(std::string prefix) :
ComponentPropertyDefinitions(prefix)
{
- defineOptionalProperty<std::string>("GraspSelectionManagerName", "GraspSelectionManager", "Name of the GraspSelectionManager proxy");
+ defineOptionalProperty<std::string>("GraspSelectionManagerName",
+ "GraspSelectionManager",
+ "Name of the GraspSelectionManager proxy");
}
};
@@ -66,14 +67,12 @@ namespace armarx
void onInitComponent() override;
void onConnectComponent() override;
- PropertyDefinitionsPtr createPropertyDefinitions() override
+ PropertyDefinitionsPtr
+ createPropertyDefinitions() override
{
return PropertyDefinitionsPtr(
- new GraspSelectionCriterionPropertyDefinitions(
- getConfigIdentifier()));
+ new GraspSelectionCriterionPropertyDefinitions(getConfigIdentifier()));
}
-
};
-}
-
+} // namespace armarx
diff --git a/source/RobotComponents/components/GraspSelectionManager/GraspSelectionManager.cpp b/source/RobotComponents/components/GraspSelectionManager/GraspSelectionManager.cpp
index bc16c3580dbd59cc1b7c8bda9e3bbcc6d332437c..a5a300f9054dbb1ddabfb245414833345d55ecf0 100644
--- a/source/RobotComponents/components/GraspSelectionManager/GraspSelectionManager.cpp
+++ b/source/RobotComponents/components/GraspSelectionManager/GraspSelectionManager.cpp
@@ -24,15 +24,18 @@
using namespace armarx;
-void GraspSelectionManager::onInitComponent()
+void
+GraspSelectionManager::onInitComponent()
{
}
-void GraspSelectionManager::onConnectComponent()
+void
+GraspSelectionManager::onConnectComponent()
{
}
-GeneratedGraspList GraspSelectionManager::filterGrasps(const GeneratedGraspList& grasps, const Ice::Current&)
+GeneratedGraspList
+GraspSelectionManager::filterGrasps(const GeneratedGraspList& grasps, const Ice::Current&)
{
GeneratedGraspList result = grasps;
@@ -44,12 +47,16 @@ GeneratedGraspList GraspSelectionManager::filterGrasps(const GeneratedGraspList&
return result;
}
-void GraspSelectionManager::registerAsGraspSelectionCriterion(const GraspSelectionCriterionInterfacePrx& criterion, const Ice::Current&)
+void
+GraspSelectionManager::registerAsGraspSelectionCriterion(
+ const GraspSelectionCriterionInterfacePrx& criterion,
+ const Ice::Current&)
{
- bool alreadyAdded = std::find_if(criteria.cbegin(), criteria.cend(), [&](const GraspSelectionCriterionInterfacePrx & gsc)
- {
- return gsc->getHash() == criterion->getHash();
- }) != criteria.cend();
+ bool alreadyAdded = std::find_if(criteria.cbegin(),
+ criteria.cend(),
+ [&](const GraspSelectionCriterionInterfacePrx& gsc) {
+ return gsc->getHash() == criterion->getHash();
+ }) != criteria.cend();
if (alreadyAdded)
{
@@ -61,13 +68,15 @@ void GraspSelectionManager::registerAsGraspSelectionCriterion(const GraspSelecti
}
}
-GraspSelectionCriterionInterfaceList GraspSelectionManager::getRegisteredGraspSelectionCriteria(const Ice::Current&)
+GraspSelectionCriterionInterfaceList
+GraspSelectionManager::getRegisteredGraspSelectionCriteria(const Ice::Current&)
{
return criteria;
}
GraspingPlacementList
-GraspSelectionManager::filterPlacements(const GraspingPlacementList& placements, const Ice::Current&)
+GraspSelectionManager::filterPlacements(const GraspingPlacementList& placements,
+ const Ice::Current&)
{
ARMARX_INFO << "length of criteria vector: " << criteria.size();
GraspingPlacementList result = placements;
diff --git a/source/RobotComponents/components/GraspSelectionManager/GraspSelectionManager.h b/source/RobotComponents/components/GraspSelectionManager/GraspSelectionManager.h
index 197cbd01c40675df39ba7d35d5563df904fac650..84cea4a4c1862eecc481ccc1a2029b2102112281 100644
--- a/source/RobotComponents/components/GraspSelectionManager/GraspSelectionManager.h
+++ b/source/RobotComponents/components/GraspSelectionManager/GraspSelectionManager.h
@@ -31,18 +31,16 @@
namespace armarx
{
- class GraspSelectionManagerPropertyDefinitions:
- public ComponentPropertyDefinitions
+ class GraspSelectionManagerPropertyDefinitions : public ComponentPropertyDefinitions
{
public:
- GraspSelectionManagerPropertyDefinitions(std::string prefix):
+ GraspSelectionManagerPropertyDefinitions(std::string prefix) :
ComponentPropertyDefinitions(prefix)
{
// defineOptionalProperty<float>("AskHumanThreshold", 0.7f, "Confidence threshold below which the human will be asked for confirmation of a replacement");
}
};
-
/*!
* \brief The GraspSelectionManager class
*/
@@ -52,15 +50,17 @@ namespace armarx
{
public:
// inherited from Component
- std::string getDefaultName() const override
+ std::string
+ getDefaultName() const override
{
return "GraspSelectionManager";
}
- PropertyDefinitionsPtr createPropertyDefinitions() override
+ PropertyDefinitionsPtr
+ createPropertyDefinitions() override
{
return PropertyDefinitionsPtr(
- new GraspSelectionManagerPropertyDefinitions(getConfigIdentifier()));
+ new GraspSelectionManagerPropertyDefinitions(getConfigIdentifier()));
}
void onInitComponent() override;
@@ -77,17 +77,20 @@ namespace armarx
* @param grasps
* @return list of grasps
*/
- GeneratedGraspList filterGrasps(const GeneratedGraspList& grasps, const Ice::Current& = Ice::emptyCurrent) override;
- GraspingPlacementList filterPlacements(const GraspingPlacementList& placements, const Ice::Current& = Ice::emptyCurrent) override;
+ GeneratedGraspList filterGrasps(const GeneratedGraspList& grasps,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ GraspingPlacementList filterPlacements(const GraspingPlacementList& placements,
+ const Ice::Current& = Ice::emptyCurrent) override;
/**
* @brief Checks if a criterion is already registered; if not, register it with the GraspSelectionManager
* @param criterion
*/
- void registerAsGraspSelectionCriterion(const GraspSelectionCriterionInterfacePrx& criterion, const Ice::Current& = Ice::emptyCurrent) override;
- GraspSelectionCriterionInterfaceList getRegisteredGraspSelectionCriteria(const Ice::Current&) override;
+ void registerAsGraspSelectionCriterion(const GraspSelectionCriterionInterfacePrx& criterion,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ GraspSelectionCriterionInterfaceList
+ getRegisteredGraspSelectionCriteria(const Ice::Current&) override;
//FeedbackPublisherInterface
std::vector<GraspSelectionCriterionInterfacePrx> criteria;
};
-}
-
+} // namespace armarx
diff --git a/source/RobotComponents/components/GraspSelectionManager/GraspSelectionManagerApp.h b/source/RobotComponents/components/GraspSelectionManager/GraspSelectionManagerApp.h
index 7b6adf91a62034fe545ee1d6194db8f3f071331e..f01b7725c7517f750eda85d4216a705e21331b54 100644
--- a/source/RobotComponents/components/GraspSelectionManager/GraspSelectionManagerApp.h
+++ b/source/RobotComponents/components/GraspSelectionManager/GraspSelectionManagerApp.h
@@ -21,21 +21,27 @@
*/
-#include "GraspSelectionManager.h"
#include <ArmarXCore/core/application/Application.h>
+
#include <RobotComponents/components/GraspSelectionManager/selectionCriteria/DummyCriterion.h>
#include <RobotComponents/components/GraspSelectionManager/selectionCriteria/NaturalGraspFilter.h>
+#include "GraspSelectionManager.h"
+
namespace armarx
{
- class GraspSelectionManagerApp :
- virtual public armarx::Application
+ class GraspSelectionManagerApp : virtual public armarx::Application
{
- void setup(const armarx::ManagedIceObjectRegistryInterfacePtr& registry, Ice::PropertiesPtr properties) override
+ void
+ setup(const armarx::ManagedIceObjectRegistryInterfacePtr& registry,
+ Ice::PropertiesPtr properties) override
{
- registry->addObject(armarx::Component::create<GraspSelectionManager>(properties, "GraspSelectionManager"));
- registry->addObject(armarx::Component::create<DummyCriterion>(properties, "DummyCriterion"));
- registry->addObject(armarx::Component::create<NaturalGraspFilter>(properties, "NaturalGraspFilter"));
+ registry->addObject(armarx::Component::create<GraspSelectionManager>(
+ properties, "GraspSelectionManager"));
+ registry->addObject(
+ armarx::Component::create<DummyCriterion>(properties, "DummyCriterion"));
+ registry->addObject(
+ armarx::Component::create<NaturalGraspFilter>(properties, "NaturalGraspFilter"));
}
};
-}
+} // namespace armarx
diff --git a/source/RobotComponents/components/GraspSelectionManager/main.cpp b/source/RobotComponents/components/GraspSelectionManager/main.cpp
index 4674f51a3d8c9fdc53dd4916ce7b694f6711a981..5287e288dd1c703345785b3a5ff8271f9e1a5aee 100644
--- a/source/RobotComponents/components/GraspSelectionManager/main.cpp
+++ b/source/RobotComponents/components/GraspSelectionManager/main.cpp
@@ -22,12 +22,15 @@
* GNU General Public License
*/
-#include "GraspSelectionManagerApp.h"
#include <ArmarXCore/core/logging/Logging.h>
-int main(int argc, char* argv[])
+#include "GraspSelectionManagerApp.h"
+
+int
+main(int argc, char* argv[])
{
- armarx::ApplicationPtr app = armarx::Application::createInstance<armarx::GraspSelectionManagerApp>();
+ armarx::ApplicationPtr app =
+ armarx::Application::createInstance<armarx::GraspSelectionManagerApp>();
app->setName("GraspSelectionManagerApp");
return app->main(argc, argv);
diff --git a/source/RobotComponents/components/GraspSelectionManager/selectionCriteria/DummyCriterion.cpp b/source/RobotComponents/components/GraspSelectionManager/selectionCriteria/DummyCriterion.cpp
index aa1d9815ca20f32da96aafd6615587c170a871d1..00c6a03f642519684ae0ec91ac5047934e2fd0dd 100644
--- a/source/RobotComponents/components/GraspSelectionManager/selectionCriteria/DummyCriterion.cpp
+++ b/source/RobotComponents/components/GraspSelectionManager/selectionCriteria/DummyCriterion.cpp
@@ -22,40 +22,46 @@
#include "DummyCriterion.h"
-#include <MemoryX/libraries/memorytypes/entity/ObjectInstance.h>
-#include <MemoryX/libraries/memorytypes/MemoryXTypesObjectFactories.h>
-#include <MemoryX/core/MemoryXCoreObjectFactories.h>
#include <RobotAPI/libraries/core/FramedPose.h>
+#include <MemoryX/core/MemoryXCoreObjectFactories.h>
+#include <MemoryX/libraries/memorytypes/MemoryXTypesObjectFactories.h>
+#include <MemoryX/libraries/memorytypes/entity/ObjectInstance.h>
+
using namespace armarx;
DummyCriterion::DummyCriterion()
{
}
-void DummyCriterion::onInitGraspSelectionCriterion()
+void
+DummyCriterion::onInitGraspSelectionCriterion()
{
// usingProxy("LongtermMemory");
}
-void DummyCriterion::onConnectGraspSelectionCriterion()
+void
+DummyCriterion::onConnectGraspSelectionCriterion()
{
// This would automatically add the SelectionCriterion
// graspSelectionManager = getProxy<GraspSelectionManagerInterfacePrx>(getProperty<std::string>("GraspSelectionManagerName").getValue());
// graspSelectionManager->registerAsGraspSelectionCriterion(GraspSelectionCriterionInterfacePrx::uncheckedCast(getProxy()));
}
-std::string DummyCriterion::getDefaultName() const
+std::string
+DummyCriterion::getDefaultName() const
{
return "DummyCriterion";
}
-GeneratedGraspList DummyCriterion::filterGrasps(const GeneratedGraspList& grasps, const Ice::Current&)
+GeneratedGraspList
+DummyCriterion::filterGrasps(const GeneratedGraspList& grasps, const Ice::Current&)
{
return grasps;
}
-GraspingPlacementList DummyCriterion::filterPlacements(const GraspingPlacementList& placements, const Ice::Current&)
+GraspingPlacementList
+DummyCriterion::filterPlacements(const GraspingPlacementList& placements, const Ice::Current&)
{
return placements;
}
diff --git a/source/RobotComponents/components/GraspSelectionManager/selectionCriteria/DummyCriterion.h b/source/RobotComponents/components/GraspSelectionManager/selectionCriteria/DummyCriterion.h
index 8aed40fe1d8a33a30fcdbfb6797cee36495914ba..a76243cc6798cfbf64befb6332df4f6e0d03b0d5 100644
--- a/source/RobotComponents/components/GraspSelectionManager/selectionCriteria/DummyCriterion.h
+++ b/source/RobotComponents/components/GraspSelectionManager/selectionCriteria/DummyCriterion.h
@@ -22,17 +22,18 @@
#pragma once
+#include <ArmarXCore/core/Component.h>
+
#include <RobotComponents/components/GraspSelectionManager/GraspSelectionCriterionBase.h>
+
#include <MemoryX/components/CommonPlacesLearner/CommonPlacesLearner.h>
-#include <ArmarXCore/core/Component.h>
namespace armarx
{
- class DummyCriterionPropertyDefinitions:
- public GraspSelectionCriterionPropertyDefinitions
+ class DummyCriterionPropertyDefinitions : public GraspSelectionCriterionPropertyDefinitions
{
public:
- DummyCriterionPropertyDefinitions(std::string prefix):
+ DummyCriterionPropertyDefinitions(std::string prefix) :
GraspSelectionCriterionPropertyDefinitions(prefix)
{
// defineOptionalProperty<std::string>("CommonPlacesLearnerName", "CommonPlacesLearnerGraspSelectionCriterion", "The CommonPlacesLearner to use");
@@ -50,21 +51,24 @@ namespace armarx
void onConnectGraspSelectionCriterion() override;
std::string getDefaultName() const override;
- armarx::PropertyDefinitionsPtr createPropertyDefinitions() override
+ armarx::PropertyDefinitionsPtr
+ createPropertyDefinitions() override
{
return armarx::PropertyDefinitionsPtr(
- new DummyCriterionPropertyDefinitions(
- getConfigIdentifier()));
+ new DummyCriterionPropertyDefinitions(getConfigIdentifier()));
}
public:
- GeneratedGraspList filterGrasps(const GeneratedGraspList& grasps, const Ice::Current&) override;
- GraspingPlacementList filterPlacements(const GraspingPlacementList& placements, const Ice::Current&) override;
- Ice::Int getHash(const Ice::Current&) const override
+ GeneratedGraspList filterGrasps(const GeneratedGraspList& grasps,
+ const Ice::Current&) override;
+ GraspingPlacementList filterPlacements(const GraspingPlacementList& placements,
+ const Ice::Current&) override;
+
+ Ice::Int
+ getHash(const Ice::Current&) const override
{
return 0;
}
};
-} // namespace spoac
-
+} // namespace armarx
diff --git a/source/RobotComponents/components/GraspSelectionManager/selectionCriteria/NaturalGraspFilter.cpp b/source/RobotComponents/components/GraspSelectionManager/selectionCriteria/NaturalGraspFilter.cpp
index 0231deb341b5118b7404d5eced9eb1b5ebf6db08..762ab37fc74482462e21da927342019d357ff98a 100644
--- a/source/RobotComponents/components/GraspSelectionManager/selectionCriteria/NaturalGraspFilter.cpp
+++ b/source/RobotComponents/components/GraspSelectionManager/selectionCriteria/NaturalGraspFilter.cpp
@@ -20,53 +20,61 @@
* GNU General Public License
*/
-#include <functional>
-
#include "NaturalGraspFilter.h"
-#include <MemoryX/libraries/memorytypes/entity/ObjectInstance.h>
-#include <MemoryX/libraries/memorytypes/MemoryXTypesObjectFactories.h>
-#include <MemoryX/core/MemoryXCoreObjectFactories.h>
+#include <functional>
+
#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
+#include <MemoryX/core/MemoryXCoreObjectFactories.h>
+#include <MemoryX/libraries/memorytypes/MemoryXTypesObjectFactories.h>
+#include <MemoryX/libraries/memorytypes/entity/ObjectInstance.h>
+
using namespace armarx;
NaturalGraspFilter::NaturalGraspFilter()
{
}
-void NaturalGraspFilter::onInitGraspSelectionCriterion()
+void
+NaturalGraspFilter::onInitGraspSelectionCriterion()
{
usingProxy("RobotStateComponent");
// usingProxy("LongtermMemory");
}
-void NaturalGraspFilter::onConnectGraspSelectionCriterion()
+void
+NaturalGraspFilter::onConnectGraspSelectionCriterion()
{
getProxy(rsc, "RobotStateComponent");
- localRobot = armarx::RemoteRobot::createLocalCloneFromFile(rsc, VirtualRobot::RobotIO::eCollisionModel);
+ localRobot =
+ armarx::RemoteRobot::createLocalCloneFromFile(rsc, VirtualRobot::RobotIO::eCollisionModel);
// Enabling this would automatically register the SelectionCriterion with the GraspSelectionManager
// graspSelectionManager = getProxy<GraspSelectionManagerInterfacePrx>(getProperty<std::string>("GraspSelectionManagerName").getValue());
// graspSelectionManager->registerAsGraspSelectionCriterion(GraspSelectionCriterionInterfacePrx::uncheckedCast(getProxy()));
}
-std::string NaturalGraspFilter::getDefaultName() const
+std::string
+NaturalGraspFilter::getDefaultName() const
{
return "NaturalGraspFilter";
}
-GeneratedGraspList NaturalGraspFilter::filterGrasps(const GeneratedGraspList& grasps, const Ice::Current&)
+GeneratedGraspList
+NaturalGraspFilter::filterGrasps(const GeneratedGraspList& grasps, const Ice::Current&)
{
GeneratedGraspList result;
for (const GeneratedGrasp& g : grasps)
{
- Eigen::Matrix3f orientation = FramedPosePtr::dynamicCast(g.framedPose)->toGlobalEigen(localRobot).block<3, 3>(0, 0);
+ Eigen::Matrix3f orientation =
+ FramedPosePtr::dynamicCast(g.framedPose)->toGlobalEigen(localRobot).block<3, 3>(0, 0);
Eigen::Vector3f rotatedYAxis = orientation * Eigen::Vector3f::UnitY();
rotatedYAxis.normalize();
if (rotatedYAxis(2) > 0.1)
{
- ARMARX_WARNING << g.graspName << " seems to be an unnatural Grasp; it will not be used further!";
+ ARMARX_WARNING << g.graspName
+ << " seems to be an unnatural Grasp; it will not be used further!";
continue;
}
result.push_back(g);
@@ -84,12 +92,15 @@ NaturalGraspFilter::filterPlacements(const GraspingPlacementList& placements, co
for (const GraspingPlacement& placement : placements)
{
// Note: something goes wrong if auto is used
- Eigen::Matrix3f robotOrientation = FramedPosePtr::dynamicCast(placement.robotPose)->toGlobalEigen(localRobot).block<3, 3>(
- 0, 0);
- Eigen::Matrix3f graspOrientation = FramedPosePtr::dynamicCast(placement.grasp.framedPose)->toGlobalEigen(
- localRobot).block<3, 3>(0, 0);
+ Eigen::Matrix3f robotOrientation = FramedPosePtr::dynamicCast(placement.robotPose)
+ ->toGlobalEigen(localRobot)
+ .block<3, 3>(0, 0);
+ Eigen::Matrix3f graspOrientation = FramedPosePtr::dynamicCast(placement.grasp.framedPose)
+ ->toGlobalEigen(localRobot)
+ .block<3, 3>(0, 0);
Eigen::Matrix3f graspToRobotTransform = robotOrientation.inverse() * graspOrientation;
- Eigen::Vector3f graspXAxisInRobotCoordinates = graspToRobotTransform * Eigen::Vector3f::UnitX();
+ Eigen::Vector3f graspXAxisInRobotCoordinates =
+ graspToRobotTransform * Eigen::Vector3f::UnitX();
graspXAxisInRobotCoordinates.normalize();
float x = graspXAxisInRobotCoordinates(0);
float y = graspXAxisInRobotCoordinates(1);
@@ -116,20 +127,26 @@ NaturalGraspFilter::filterPlacements(const GraspingPlacementList& placements, co
return result;
}
-Ice::Int NaturalGraspFilter::getHash(const Ice::Current&) const
+Ice::Int
+NaturalGraspFilter::getHash(const Ice::Current&) const
{
- return std::hash<std::string> {}(graspSelectionManager->ice_getIdentity().category + "." + graspSelectionManager->ice_getIdentity().name);
+ return std::hash<std::string>{}(graspSelectionManager->ice_getIdentity().category + "." +
+ graspSelectionManager->ice_getIdentity().name);
}
-bool NaturalGraspFilter::vectorIsWithinCircleSegment(const Eigen::Vector2f vector, double angle1, double angle2)
+bool
+NaturalGraspFilter::vectorIsWithinCircleSegment(const Eigen::Vector2f vector,
+ double angle1,
+ double angle2)
{
- ARMARX_CHECK_EXPRESSION(std::abs(angle2 - angle1) < M_PI) <<
- "Works only for circle segments smaller than Pi and is " << std::abs(angle2 - angle1);
+ ARMARX_CHECK_EXPRESSION(std::abs(angle2 - angle1) < M_PI)
+ << "Works only for circle segments smaller than Pi and is " << std::abs(angle2 - angle1);
Eigen::Rotation2Df rot1(angle1);
Eigen::Rotation2Df rot2(angle2);
Eigen::Vector2f boundary1 = rot1 * Eigen::Vector2f::UnitX();
Eigen::Vector2f boundary2 = rot2 * Eigen::Vector2f::UnitX();
- bool vectorIsCounterClockwiseToBoundary1 = boundary1(0) * vector(1) - boundary1(1) * vector(0) > 0;
- bool vectorIsClockwiseToBoundary2 = - boundary2(0) * vector(1) + boundary2(1) * vector(0) > 0;
+ bool vectorIsCounterClockwiseToBoundary1 =
+ boundary1(0) * vector(1) - boundary1(1) * vector(0) > 0;
+ bool vectorIsClockwiseToBoundary2 = -boundary2(0) * vector(1) + boundary2(1) * vector(0) > 0;
return vectorIsCounterClockwiseToBoundary1 && vectorIsClockwiseToBoundary2;
}
diff --git a/source/RobotComponents/components/GraspSelectionManager/selectionCriteria/NaturalGraspFilter.h b/source/RobotComponents/components/GraspSelectionManager/selectionCriteria/NaturalGraspFilter.h
index a77c8ec53b38ed2b7b4ab113ac045c1a7a1d8fac..9c14383dd2b03255bd6942dc7c4e9bbc7b84b24b 100644
--- a/source/RobotComponents/components/GraspSelectionManager/selectionCriteria/NaturalGraspFilter.h
+++ b/source/RobotComponents/components/GraspSelectionManager/selectionCriteria/NaturalGraspFilter.h
@@ -22,17 +22,18 @@
#pragma once
+#include <ArmarXCore/core/Component.h>
+
#include <RobotComponents/components/GraspSelectionManager/GraspSelectionCriterionBase.h>
+
#include <MemoryX/components/CommonPlacesLearner/CommonPlacesLearner.h>
-#include <ArmarXCore/core/Component.h>
namespace armarx
{
- class NaturalGraspFilterPropertyDefinitions:
- public GraspSelectionCriterionPropertyDefinitions
+ class NaturalGraspFilterPropertyDefinitions : public GraspSelectionCriterionPropertyDefinitions
{
public:
- NaturalGraspFilterPropertyDefinitions(std::string prefix):
+ NaturalGraspFilterPropertyDefinitions(std::string prefix) :
GraspSelectionCriterionPropertyDefinitions(prefix)
{
// defineOptionalProperty<std::string>("CommonPlacesLearnerName", "CommonPlacesLearnerGraspSelectionCriterion", "The CommonPlacesLearner to use");
@@ -50,15 +51,16 @@ namespace armarx
void onConnectGraspSelectionCriterion() override;
std::string getDefaultName() const override;
- armarx::PropertyDefinitionsPtr createPropertyDefinitions() override
+ armarx::PropertyDefinitionsPtr
+ createPropertyDefinitions() override
{
return armarx::PropertyDefinitionsPtr(
- new NaturalGraspFilterPropertyDefinitions(
- getConfigIdentifier()));
+ new NaturalGraspFilterPropertyDefinitions(getConfigIdentifier()));
}
public:
- GeneratedGraspList filterGrasps(const GeneratedGraspList& grasps, const Ice::Current&) override;
+ GeneratedGraspList filterGrasps(const GeneratedGraspList& grasps,
+ const Ice::Current&) override;
/*****
* @brief Filters Grasps that are unnatural from the human point of view
@@ -67,14 +69,16 @@ namespace armarx
* direction for the Left Hand) is in the Unit-3Sphere in Robot Coordinates with the left half excluded
* (x < -0.1) and a circle cut out towards the robot; Based on heuristics for naturalness of a grasp.
**/
- GraspingPlacementList filterPlacements(const GraspingPlacementList& placements, const Ice::Current&) override;
+ GraspingPlacementList filterPlacements(const GraspingPlacementList& placements,
+ const Ice::Current&) override;
Ice::Int getHash(const Ice::Current&) const override;
+
private:
RobotStateComponentInterfacePrx rsc;
VirtualRobot::RobotPtr localRobot;
- bool vectorIsWithinCircleSegment(const Eigen::Vector2f vector, double angle1, double angle2);
+ bool
+ vectorIsWithinCircleSegment(const Eigen::Vector2f vector, double angle1, double angle2);
};
-} // namespace spoac
-
+} // namespace armarx
diff --git a/source/RobotComponents/components/GraspingManager/GraspingManager.cpp b/source/RobotComponents/components/GraspingManager/GraspingManager.cpp
index 7f7ffc66f798d6c6d157d43ddca08b0e641655f7..7fa481d25706fe84dc26a0c1440cecd077b376cf 100644
--- a/source/RobotComponents/components/GraspingManager/GraspingManager.cpp
+++ b/source/RobotComponents/components/GraspingManager/GraspingManager.cpp
@@ -23,90 +23,80 @@
*/
#include "GraspingManager.h"
-#include <RobotAPI/libraries/core/Pose.h>
-#include <RobotAPI/libraries/core/FramedPose.h>
-#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
-#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
-#include <ArmarXCore/core/util/StringHelperTemplates.h>
-#include <ArmarXCore/core/util/OnScopeExit.h>
-#include <RobotAPI/libraries/core/Trajectory.h>
-#include <RobotComponents/components/MotionPlanning/CSpace/SimoxCSpace.h>
-#include <RobotComponents/components/MotionPlanning/CSpace/VoxelGridCSpace.h>
-#include <RobotComponents/interface/components/MotionPlanning/MotionPlanningServer.h>
-#include <IceUtil/UUID.h>
-#include <tuple>
#include <atomic>
+#include <tuple>
+
+#include <IceUtil/UUID.h>
+
+#include <SimoxUtility/algorithm/string/string_tools.h>
#include <VirtualRobot/IK/ConstrainedOptimizationIK.h>
#include <VirtualRobot/IK/constraints/CollisionCheckConstraint.h>
#include <VirtualRobot/IK/constraints/OrientationConstraint.h>
#include <VirtualRobot/IK/constraints/PoseConstraint.h>
#include <VirtualRobot/IK/constraints/PositionConstraint.h>
#include <VirtualRobot/IK/constraints/ReferenceConfigurationConstraint.h>
-
+#include <VirtualRobot/MathTools.h>
#include <VirtualRobot/RobotConfig.h>
-#include <RobotComponents/components/GraspSelectionManager/selectionCriteria/NaturalGraspFilter.h>
-#include <RobotAPI/libraries/core/CartesianPositionController.h>
+#include <VirtualRobot/RobotNodeSet.h>
+
+#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
+#include <ArmarXCore/core/util/OnScopeExit.h>
+#include <ArmarXCore/core/util/StringHelperTemplates.h>
+
#include <RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.h>
+#include <RobotAPI/libraries/core/CartesianPositionController.h>
+#include <RobotAPI/libraries/core/FramedPose.h>
+#include <RobotAPI/libraries/core/Pose.h>
+#include <RobotAPI/libraries/core/Trajectory.h>
+#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
-#include <SimoxUtility/algorithm/string/string_tools.h>
+#include <RobotComponents/components/GraspSelectionManager/selectionCriteria/NaturalGraspFilter.h>
+#include <RobotComponents/components/MotionPlanning/CSpace/SimoxCSpace.h>
+#include <RobotComponents/components/MotionPlanning/CSpace/VoxelGridCSpace.h>
+#include <RobotComponents/interface/components/MotionPlanning/MotionPlanningServer.h>
using namespace armarx;
-static const DrawColor COLOR_POSE_LINE
-{
- 0.5f, 1.0f, 0.5f, 0.5f
-};
-static const DrawColor COLOR_POSE_POINT
-{
- 0.0f, 1.0f, 0.0f, 0.5f
-};
-static const DrawColor COLOR_CONFIG_LINE
-{
- 1.0f, 0.5f, 0.5f, 0.5f
-};
-static const DrawColor COLOR_CONFIG_POINT
-{
- 1.0f, 0.0f, 0.0f, 0.5f
-};
-static const DrawColor COLOR_ROBOT
-{
- 0.0f, 0.586f, 0.508f, 1.0f
-};
+static const DrawColor COLOR_POSE_LINE{0.5f, 1.0f, 0.5f, 0.5f};
+static const DrawColor COLOR_POSE_POINT{0.0f, 1.0f, 0.0f, 0.5f};
+static const DrawColor COLOR_CONFIG_LINE{1.0f, 0.5f, 0.5f, 0.5f};
+static const DrawColor COLOR_CONFIG_POINT{1.0f, 0.0f, 0.0f, 0.5f};
+static const DrawColor COLOR_ROBOT{0.0f, 0.586f, 0.508f, 1.0f};
static const float LINE_WIDTH = 5.f;
static const float SPHERE_SIZE = 6.f;
-static const std::map<std::string, std::string> TCP_HAND_MAPPING
-{
- {"TCP R", "handright3a"},
- {"TCP L", "handleft3a"},
- {"Hand L TCP", "left_hand"},
- {"Hand R TCP", "right_hand"}
-};
-
+static const std::map<std::string, std::string> TCP_HAND_MAPPING{{"TCP R", "handright3a"},
+ {"TCP L", "handleft3a"},
+ {"Hand L TCP", "left_hand"},
+ {"Hand R TCP", "right_hand"}};
auto newId = []() mutable
{
- static std::atomic<int> i {0};
+ static std::atomic<int> i{0};
return to_string(i++);
};
-void GraspingManager::onInitComponent()
+void
+GraspingManager::onInitComponent()
{
graspGeneratorName = getProperty<std::string>("GraspGeneratorName").getValue();
robotPlacementName = getProperty<std::string>("RobotPlacementName").getValue();
- robotNodeSetNames = armarx::Split(getProperty<std::string>("RobotNodeSetNames").getValue(), ";");
- reachabilitySpaceFilePaths = armarx::Split(getProperty<std::string>("ReachabilitySpaceFilePaths").getValue(), ";");
+ robotNodeSetNames =
+ armarx::Split(getProperty<std::string>("RobotNodeSetNames").getValue(), ";");
+ reachabilitySpaceFilePaths =
+ armarx::Split(getProperty<std::string>("ReachabilitySpaceFilePaths").getValue(), ";");
globalDescriptionPosition = new Vector3(0, 0, 0);
//@TODO still not sure if this is the way to go
for (auto& path : reachabilitySpaceFilePaths)
{
- std::string packageName = std::filesystem::path {path} .begin()->string();
- ARMARX_CHECK_EXPRESSION(!packageName.empty()) << "Path '" << path << "' could not be parsed correctly";
+ std::string packageName = std::filesystem::path{path}.begin()->string();
+ ARMARX_CHECK_EXPRESSION(!packageName.empty())
+ << "Path '" << path << "' could not be parsed correctly";
armarx::CMakePackageFinder project(packageName);
path = project.getDataDir() + "/" + path;
}
@@ -127,7 +117,8 @@ void GraspingManager::onInitComponent()
usingProxy("PriorKnowledge");
}
-void GraspingManager::onConnectComponent()
+void
+GraspingManager::onConnectComponent()
{
entityDrawer = getTopic<memoryx::EntityDrawerInterfacePrx>("DebugDrawerUpdates");
layerName = getDefaultName();
@@ -138,7 +129,8 @@ void GraspingManager::onConnectComponent()
//getProxy(rik, "RobotIK");
getProxy(rsc, "RobotStateComponent");
- localRobot = armarx::RemoteRobot::createLocalCloneFromFile(rsc, VirtualRobot::RobotIO::eCollisionModel);
+ localRobot =
+ armarx::RemoteRobot::createLocalCloneFromFile(rsc, VirtualRobot::RobotIO::eCollisionModel);
ikRobot = localRobot->clone();
// localRobot->print();
@@ -154,7 +146,8 @@ void GraspingManager::onConnectComponent()
}
cacheCSpace = SimoxCSpace::PrefetchWorkingMemoryObjects(wm, cs, rsc);
- Ice::FloatSeq boundsStrings = Split<float>(getProperty<std::string>("PlanningBoundingBox").getValue(), ",");
+ Ice::FloatSeq boundsStrings =
+ Split<float>(getProperty<std::string>("PlanningBoundingBox").getValue(), ",");
ARMARX_INFO << VAROUT(boundsStrings);
planningBoundingBox.min.e0 = boundsStrings.at(0);
planningBoundingBox.min.e1 = boundsStrings.at(1);
@@ -193,25 +186,26 @@ void GraspingManager::onConnectComponent()
// }
}
-void GraspingManager::onDisconnectComponent()
+void
+GraspingManager::onDisconnectComponent()
{
-
}
-void GraspingManager::onExitComponent()
+void
+GraspingManager::onExitComponent()
{
cacheCSpace = NULL;
}
-GeneratedGraspList GraspingManager::generateGrasps(const std::string& objectInstanceEntityId)
+GeneratedGraspList
+GraspingManager::generateGrasps(const std::string& objectInstanceEntityId)
{
setNextStepDescription("Generating grasps");
ARMARX_VERBOSE << "Step: generate grasps";
GeneratedGraspList grasps = gg->generateGrasps(objectInstanceEntityId);
- std::sort(grasps.begin(), grasps.end(), [](const GeneratedGrasp & l, const GeneratedGrasp & r)
- {
- return l.score < r.score;
- });
+ std::sort(grasps.begin(),
+ grasps.end(),
+ [](const GeneratedGrasp& l, const GeneratedGrasp& r) { return l.score < r.score; });
if (grasps.empty())
{
ARMARX_WARNING << " Step 'generate grasps' generated no grasps";
@@ -221,11 +215,12 @@ GeneratedGraspList GraspingManager::generateGrasps(const std::string& objectInst
return grasps;
}
-std::pair<std::string, Ice::StringSeq> GraspingManager::visualizeGrasp(const GeneratedGrasp& grasp, int id, const DrawColor& color)
+std::pair<std::string, Ice::StringSeq>
+GraspingManager::visualizeGrasp(const GeneratedGrasp& grasp, int id, const DrawColor& color)
{
- float visuSlowdownFactor = getProperty<float>("VisualizationSlowdownFactor").getValue();
+ float visuSlowdownFactor = getProperty<float>("VisualizationSlowdownFactor").getValue();
std::pair<std::string, Ice::StringSeq> result;
- if (visuSlowdownFactor <= 0 || !getProperty<bool>("EnableVisualization"))
+ if (visuSlowdownFactor <= 0 || !getProperty<bool>("EnableVisualization"))
{
return result;
}
@@ -241,9 +236,12 @@ std::pair<std::string, Ice::StringSeq> GraspingManager::visualizeGrasp(const Gen
usleep(500000 * visuSlowdownFactor);
// entityDrawer->setPoseVisu("GeneratedGrasps", "GraspCandidate" + handName + to_string(id), grasp.framedPose);
result.first = "GeneratedGrasps";
- result.second = {"GraspCandidate" + handName + to_string(id), "GraspCandidatePrepose" + handName + to_string(id)};
- entityDrawer->setObjectVisu("GeneratedGrasps", result.second.at(0), objClass, grasp.framedPose);
- entityDrawer->setObjectVisu("GeneratedGrasps", result.second.at(1), objClass, grasp.framedPrePose);
+ result.second = {"GraspCandidate" + handName + to_string(id),
+ "GraspCandidatePrepose" + handName + to_string(id)};
+ entityDrawer->setObjectVisu(
+ "GeneratedGrasps", result.second.at(0), objClass, grasp.framedPose);
+ entityDrawer->setObjectVisu(
+ "GeneratedGrasps", result.second.at(1), objClass, grasp.framedPrePose);
entityDrawer->updateObjectColor("GeneratedGrasps", result.second.at(0), color);
auto darkerColor = color;
@@ -251,7 +249,6 @@ std::pair<std::string, Ice::StringSeq> GraspingManager::visualizeGrasp(const Gen
darkerColor.g *= 0.6f;
darkerColor.b *= 0.6f;
entityDrawer->updateObjectColor("GeneratedGrasps", result.second.at(1), darkerColor);
-
}
else
{
@@ -260,14 +257,18 @@ std::pair<std::string, Ice::StringSeq> GraspingManager::visualizeGrasp(const Gen
return result;
}
-SimoxCSpacePtr GraspingManager::createCSpace()
+SimoxCSpacePtr
+GraspingManager::createCSpace()
{
TIMING_START(CSpaceCreation);
SimoxCSpacePtr cspace;
if (getProperty<bool>("UseVoxelGridCSpace").getValue())
{
- cspace = new VoxelGridCSpace(getProxy<visionx::VoxelGridProviderInterfacePrx>(getProperty<std::string>("VoxelGridProviderName").getValue()), cs);
+ cspace =
+ new VoxelGridCSpace(getProxy<visionx::VoxelGridProviderInterfacePrx>(
+ getProperty<std::string>("VoxelGridProviderName").getValue()),
+ cs);
}
else
{
@@ -279,14 +280,17 @@ SimoxCSpacePtr GraspingManager::createCSpace()
agentData.agentProjectNames = rsc->getArmarXPackages();
agentData.agentRelativeFilePath = rsc->getRobotFilename();
// agentData.kinemaicChainNames = robotNodeSetNames;
- agentData.collisionSetNames = {getProperty<std::string>("RobotCollisionNodeSet").getValue()}; // TODO: Make a mapping between jointset and link set
+ agentData.collisionSetNames = {
+ getProperty<std::string>("RobotCollisionNodeSet")
+ .getValue()}; // TODO: Make a mapping between jointset and link set
cspace->setAgent(agentData);
cspace->initCollisionTest();
TIMING_END(CSpaceCreation);
return cspace;
}
-GeneratedGraspList GraspingManager::filterGrasps(const GeneratedGraspList& grasps)
+GeneratedGraspList
+GraspingManager::filterGrasps(const GeneratedGraspList& grasps)
{
setNextStepDescription("Filtering grasps");
float visuSlowdownFactor = getProperty<float>("VisualizationSlowdownFactor");
@@ -315,7 +319,8 @@ GeneratedGraspList GraspingManager::filterGrasps(const GeneratedGraspList& grasp
return filteredGrasps;
}
-GraspingPlacementList GraspingManager::filterPlacements(const GraspingPlacementList& placements)
+GraspingPlacementList
+GraspingManager::filterPlacements(const GraspingPlacementList& placements)
{
ARMARX_VERBOSE << "Step: filter placements";
GraspingPlacementList filteredPlacements = gsm->filterPlacements(placements);
@@ -327,32 +332,36 @@ GraspingPlacementList GraspingManager::filterPlacements(const GraspingPlacementL
return filteredPlacements;
}
-
-GraspingPlacementList GraspingManager::generateRobotPlacements(const GeneratedGraspList& grasps)
+GraspingPlacementList
+GraspingManager::generateRobotPlacements(const GeneratedGraspList& grasps)
{
// TODO: Why is there a variable number of robot placements for each run??
ARMARX_VERBOSE << "Step: generate robot placements";
GraspingPlacementList graspPlacements = rp->generateRobotPlacementsEx(grasps);
- ARMARX_CHECK_EXPRESSION(!graspPlacements.empty()) << "No placements for the robot platform were found.";
+ ARMARX_CHECK_EXPRESSION(!graspPlacements.empty())
+ << "No placements for the robot platform were found.";
return graspPlacements;
}
-GraspingTrajectory GraspingManager::planMotion(const MotionPlanningData& mpd)
+GraspingTrajectory
+GraspingManager::planMotion(const MotionPlanningData& mpd)
{
- ARMARX_IMPORTANT << "Robot position: " << VAROUT(mpd.globalPoseStart->output()) << VAROUT(mpd.globalPoseGoal->output());
+ ARMARX_IMPORTANT << "Robot position: " << VAROUT(mpd.globalPoseStart->output())
+ << VAROUT(mpd.globalPoseGoal->output());
if (getProperty<bool>("EnableVisualization"))
{
- entityDrawer->setPoseVisu(layerName, "MotionPlanningPlatformTargetPose", mpd.globalPoseGoal);
+ entityDrawer->setPoseVisu(
+ layerName, "MotionPlanningPlatformTargetPose", mpd.globalPoseGoal);
}
Eigen::Vector3f bbcenter;
bbcenter << (planningBoundingBox.max.e0 + planningBoundingBox.min.e0) / 2,
- (planningBoundingBox.max.e1 + planningBoundingBox.min.e1) / 2,
- (planningBoundingBox.max.e2 + planningBoundingBox.min.e2) / 2;
+ (planningBoundingBox.max.e1 + planningBoundingBox.min.e1) / 2,
+ (planningBoundingBox.max.e2 + planningBoundingBox.min.e2) / 2;
Eigen::Vector3f bbSize;
bbSize << planningBoundingBox.max.e0 - planningBoundingBox.min.e0,
- planningBoundingBox.max.e1 - planningBoundingBox.min.e1,
- planningBoundingBox.max.e2 - planningBoundingBox.min.e2;
+ planningBoundingBox.max.e1 - planningBoundingBox.min.e1,
+ planningBoundingBox.max.e2 - planningBoundingBox.min.e2;
// entityDrawer->setBoxVisu(layerName, "PlanningBoundingBox", new Pose(Eigen::Matrix3f::Identity(), Eigen::Vector3f(bbcenter)), new Vector3(bbSize), DrawColor {1, 0, 0, 0.1});
ARMARX_VERBOSE << "Step: Motion Planning";
@@ -362,10 +371,13 @@ GraspingTrajectory GraspingManager::planMotion(const MotionPlanningData& mpd)
}
auto robotFileName = rsc->getRobotFilename();
- auto packageName = std::filesystem::path {robotFileName} .begin()->string();
+ auto packageName = std::filesystem::path{robotFileName}.begin()->string();
auto axPackages = rsc->getArmarXPackages();
- ARMARX_CHECK_EXPRESSION(std::find(axPackages.cbegin(), axPackages.cend(), packageName) != axPackages.cend()) << "Could not determine package name from path '" << robotFileName << "', "
- << "because the determined package name '" << packageName << "' is not in the following list: " << axPackages;
+ ARMARX_CHECK_EXPRESSION(std::find(axPackages.cbegin(), axPackages.cend(), packageName) !=
+ axPackages.cend())
+ << "Could not determine package name from path '" << robotFileName << "', "
+ << "because the determined package name '" << packageName
+ << "' is not in the following list: " << axPackages;
// SimoxCSpacePtr cspace = SimoxCSpacePtr::dynamicCast(this->cspace->clone(false));
SimoxCSpacePtr armCSpace = SimoxCSpacePtr::dynamicCast(cspace->clone());
// armCSpace->addObjectsFromWorkingMemory(wm);
@@ -382,9 +394,11 @@ GraspingTrajectory GraspingManager::planMotion(const MotionPlanningData& mpd)
auto collisionSetMapping = getJointSetCollisionSetMapping();
std::string armCollisionSet = collisionSetMapping.at(mpd.rnsToUse);
- auto robotCol = cspace->getAgentSceneObj()->getRobotNodeSet(getProperty<std::string>("RobotCollisionNodeSet").getValue());
+ auto robotCol = cspace->getAgentSceneObj()->getRobotNodeSet(
+ getProperty<std::string>("RobotCollisionNodeSet").getValue());
auto robotColNodeNames = robotCol->getNodeNames();
- agentData.collisionObjectNames = getRobotNodeSetNodesWithoutAllwaysColliding(robotColNodeNames, armCollisionSet);
+ agentData.collisionObjectNames =
+ getRobotNodeSetNodesWithoutAllwaysColliding(robotColNodeNames, armCollisionSet);
agentData.collisionSetNames = {armCollisionSet};
agentData.initialJointValues = localRobot->getConfig()->getRobotNodeJointValueMap();
@@ -403,10 +417,13 @@ GraspingTrajectory GraspingManager::planMotion(const MotionPlanningData& mpd)
agentData.collisionSetNames = {};
for (auto& pair : collisionSetMapping)
{
- if (std::find(agentData.collisionSetNames.begin(), agentData.collisionSetNames.end(), pair.second) == agentData.collisionSetNames.end())
+ if (std::find(agentData.collisionSetNames.begin(),
+ agentData.collisionSetNames.end(),
+ pair.second) == agentData.collisionSetNames.end())
{
agentData.collisionSetNames.push_back(pair.second);
- agentData.collisionObjectNames = getRobotNodeSetNodesWithoutAllwaysColliding(agentData.collisionObjectNames, pair.second);
+ agentData.collisionObjectNames = getRobotNodeSetNodesWithoutAllwaysColliding(
+ agentData.collisionObjectNames, pair.second);
}
}
@@ -427,9 +444,8 @@ GraspingTrajectory GraspingManager::planMotion(const MotionPlanningData& mpd)
return planningResult;
}
-
-
-void GraspingManager::drawTrajectory(const GraspingTrajectory& t, float visuSlowdownFactor)
+void
+GraspingManager::drawTrajectory(const GraspingTrajectory& t, float visuSlowdownFactor)
{
try
{
@@ -450,10 +466,12 @@ void GraspingManager::drawTrajectory(const GraspingTrajectory& t, float visuSlow
auto robotConfig = rsc->getSynchronizedRobot()->getConfig();
localRobot->setJointValues(robotConfig);
- entityDrawer->setRobotVisu(layerName, robotId, rsc->getRobotFilename(), "RobotAPI", FullModel);
+ entityDrawer->setRobotVisu(
+ layerName, robotId, rsc->getRobotFilename(), "RobotAPI", FullModel);
entityDrawer->updateRobotColor(layerName, robotId, COLOR_ROBOT);
entityDrawer->updateRobotPose(layerName, robotId, new Pose(localRobot->getGlobalPose()));
- entityDrawer->updateRobotConfig(layerName, robotId, localRobot->getConfig()->getRobotNodeJointValueMap());
+ entityDrawer->updateRobotConfig(
+ layerName, robotId, localRobot->getConfig()->getRobotNodeJointValueMap());
ARMARX_INFO << VAROUT(poseTrajectory->output());
@@ -461,17 +479,20 @@ void GraspingManager::drawTrajectory(const GraspingTrajectory& t, float visuSlow
std::vector<PosePtr> poseData;
ARMARX_CHECK_EXPRESSION(poseTrajectory->dim() >= 3) << "dim: " << poseTrajectory->dim();
- std::transform(poseTrajectory->begin(), poseTrajectory->end(), std::back_inserter(poseData), [](const Trajectory::TrajData & data) -> PosePtr
- {
- Eigen::Matrix4f pose = Eigen::Matrix4f::Identity();
- VirtualRobot::MathTools::rpy2eigen4f(0, 0, data.getPosition(2), pose);
- pose(0, 3) = data.getPosition(0);
- pose(1, 3) = data.getPosition(1);
- pose(2, 3) = 1;
- return new Pose(pose);
- });
+ std::transform(poseTrajectory->begin(),
+ poseTrajectory->end(),
+ std::back_inserter(poseData),
+ [](const Trajectory::TrajData& data) -> PosePtr
+ {
+ Eigen::Matrix4f pose = Eigen::Matrix4f::Identity();
+ VirtualRobot::MathTools::rpy2eigen4f(0, 0, data.getPosition(2), pose);
+ pose(0, 3) = data.getPosition(0);
+ pose(1, 3) = data.getPosition(1);
+ pose(2, 3) = 1;
+ return new Pose(pose);
+ });
ARMARX_INFO << __LINE__;
- int stepSize = 1;//std::max<int>(1, poseData.size() / 20);
+ int stepSize = 1; //std::max<int>(1, poseData.size() / 20);
for (auto it = poseData.cbegin(); it != poseData.cend(); it += 1)
{
Vector3Ptr currPos = new Vector3((*it)->toEigen());
@@ -481,7 +502,8 @@ void GraspingManager::drawTrajectory(const GraspingTrajectory& t, float visuSlow
if (nextIt != poseData.cend())
{
Vector3Ptr nextPos = new Vector3((*nextIt)->toEigen());
- entityDrawer->setLineVisu(layerName, newId(), currPos, nextPos, LINE_WIDTH, COLOR_POSE_LINE);
+ entityDrawer->setLineVisu(
+ layerName, newId(), currPos, nextPos, LINE_WIDTH, COLOR_POSE_LINE);
Eigen::Matrix4f pose = localRobot->getGlobalPose();
pose.block<3, 1>(0, 3) = nextPos->toEigen();
entityDrawer->updateRobotPose(layerName, robotId, *nextIt);
@@ -495,16 +517,18 @@ void GraspingManager::drawTrajectory(const GraspingTrajectory& t, float visuSlow
std::vector<NameValueMap> configData;
ARMARX_CHECK_EXPRESSION(configTrajectory->dim() > 0);
auto jointLabels = configTrajectory->getDimensionNames();
- std::transform(configTrajectory->begin(), configTrajectory->end(), std::back_inserter(configData), [&](const Trajectory::TrajData & data) -> NameValueMap
- {
- NameValueMap result;
- for (size_t i = 0; i < jointLabels.size(); ++i)
- {
- result.insert({jointLabels[i], data.getPosition(i)});
- }
- return result;
- });
-
+ std::transform(configTrajectory->begin(),
+ configTrajectory->end(),
+ std::back_inserter(configData),
+ [&](const Trajectory::TrajData& data) -> NameValueMap
+ {
+ NameValueMap result;
+ for (size_t i = 0; i < jointLabels.size(); ++i)
+ {
+ result.insert({jointLabels[i], data.getPosition(i)});
+ }
+ return result;
+ });
VirtualRobot::RobotNodeSetPtr rns = localRobot->getRobotNodeSet(t.rnsToUse);
@@ -512,37 +536,48 @@ void GraspingManager::drawTrajectory(const GraspingTrajectory& t, float visuSlow
const auto tcpName = rns->getTCP()->getName();
std::vector<Vector3Ptr> tcpPoseList;
- std::transform(configData.cbegin(), configData.cend(), std::back_inserter(tcpPoseList), [&](const NameValueMap & config)
- {
- localRobot->setJointValues(config);
- return new Vector3(localRobot->getRobotNode(tcpName)->getGlobalPose());
- });
+ std::transform(configData.cbegin(),
+ configData.cend(),
+ std::back_inserter(tcpPoseList),
+ [&](const NameValueMap& config)
+ {
+ localRobot->setJointValues(config);
+ return new Vector3(localRobot->getRobotNode(tcpName)->getGlobalPose());
+ });
// stepSize = std::max<int>(1, tcpPoseList.size() / 20);
int i = 0;
- for (auto it = tcpPoseList.cbegin(); it != tcpPoseList.cend(); it += stepSize, i += stepSize)
+ for (auto it = tcpPoseList.cbegin(); it != tcpPoseList.cend();
+ it += stepSize, i += stepSize)
{
auto nextIt = std::next(it);
auto currPose = *it;
- entityDrawer->setSphereVisu(layerName, newId(), currPose, COLOR_CONFIG_POINT, SPHERE_SIZE);
+ entityDrawer->setSphereVisu(
+ layerName, newId(), currPose, COLOR_CONFIG_POINT, SPHERE_SIZE);
if (nextIt != tcpPoseList.cend())
{
auto nextPose = *nextIt;
entityDrawer->updateRobotConfig(layerName, robotId, configData.at(i));
- entityDrawer->setLineVisu(layerName, newId(), currPose, nextPose, SPHERE_SIZE, COLOR_CONFIG_LINE);
+ entityDrawer->setLineVisu(
+ layerName, newId(), currPose, nextPose, SPHERE_SIZE, COLOR_CONFIG_LINE);
usleep(1000000 * visuSlowdownFactor / tcpPoseList.size());
}
}
entityDrawer->updateRobotConfig(layerName, robotId, configData.back());
- ARMARX_CHECK_EXPRESSION(TCP_HAND_MAPPING.find(tcpName) != TCP_HAND_MAPPING.end()) << "Unknown TCP '" << tcpName << "'";
- auto handObjectClass = prior->getObjectClassesSegment()->getObjectClassByName(TCP_HAND_MAPPING.at(tcpName));
+ ARMARX_CHECK_EXPRESSION(TCP_HAND_MAPPING.find(tcpName) != TCP_HAND_MAPPING.end())
+ << "Unknown TCP '" << tcpName << "'";
+ auto handObjectClass =
+ prior->getObjectClassesSegment()->getObjectClassByName(TCP_HAND_MAPPING.at(tcpName));
ARMARX_CHECK_EXPRESSION(handObjectClass) << TCP_HAND_MAPPING.at(tcpName);
localRobot->setJointValues(configData.back());
- entityDrawer->setObjectVisu(layerName, handObjectClass->getName(), handObjectClass, new Pose(localRobot->getRobotNode(tcpName)->getGlobalPose()));
+ entityDrawer->setObjectVisu(layerName,
+ handObjectClass->getName(),
+ handObjectClass,
+ new Pose(localRobot->getRobotNode(tcpName)->getGlobalPose()));
entityDrawer->updateObjectColor(layerName, handObjectClass->getName(), COLOR_ROBOT);
}
catch (...)
@@ -551,35 +586,43 @@ void GraspingManager::drawTrajectory(const GraspingTrajectory& t, float visuSlow
}
}
-void GraspingManager::setDescriptionPositionForObject(const std::string& objId)
+void
+GraspingManager::setDescriptionPositionForObject(const std::string& objId)
{
if (!objId.empty())
{
auto objInstance = wm->getObjectInstancesSegment()->getObjectInstanceById(objId);
- FramedPositionPtr p = armarx::FramedPositionPtr::dynamicCast(objInstance->getPositionBase());
+ FramedPositionPtr p =
+ armarx::FramedPositionPtr::dynamicCast(objInstance->getPositionBase());
p->changeToGlobal(localRobot);
p->z += 400;
globalDescriptionPosition = p;
}
}
-void GraspingManager::setNextStepDescription(const std::string& description)
+void
+GraspingManager::setNextStepDescription(const std::string& description)
{
step++;
if (getProperty<bool>("EnableVisualization"))
- entityDrawer->setTextVisu(layerName, "stepDescription", "Step " + to_string(step) + ": " + description, globalDescriptionPosition, DrawColor {0, 1, 0, 1}, 15);
+ entityDrawer->setTextVisu(layerName,
+ "stepDescription",
+ "Step " + to_string(step) + ": " + description,
+ globalDescriptionPosition,
+ DrawColor{0, 1, 0, 1},
+ 15);
}
-void GraspingManager::resetStepDescription()
+void
+GraspingManager::resetStepDescription()
{
step = 0;
globalDescriptionPosition = new Vector3(0, 0, 0);
entityDrawer->removeTextVisu(layerName, "stepDescription");
}
-
-
-std::vector<MotionPlanningData> GraspingManager::calculateIKs(const GraspingPlacementList& graspPlacements)
+std::vector<MotionPlanningData>
+GraspingManager::calculateIKs(const GraspingPlacementList& graspPlacements)
{
// TODO: Why is the left hand grasp always discarded for the IK?
// Eigen::Vector3f rpy;
@@ -609,7 +652,7 @@ std::vector<MotionPlanningData> GraspingManager::calculateIKs(const GraspingPlac
// planningTasks.push_back(taskHandle);
// }
- std::vector<MotionPlanningData>mpdList;
+ std::vector<MotionPlanningData> mpdList;
// VirtualRobot::RobotPtr localRobot = RemoteRobot::createLocalCloneFromFile(rsc, VirtualRobot::RobotIO::eStructure);
Eigen::Vector3f robotPos = localRobot->getGlobalPose().block<3, 1>(0, 3);
@@ -629,64 +672,86 @@ std::vector<MotionPlanningData> GraspingManager::calculateIKs(const GraspingPlac
ARMARX_CHECK_EXPRESSION(!currentConfig.empty());
auto graspVisuId = visualizeGrasp(gp.grasp, i);
Eigen::Matrix4f currentRobotPose = localRobot->getGlobalPose();
- auto desiredRobotPose = PosePtr::dynamicCast(FramedPosePtr::dynamicCast(gp.robotPose)->toGlobal(localRobot));
+ auto desiredRobotPose =
+ PosePtr::dynamicCast(FramedPosePtr::dynamicCast(gp.robotPose)->toGlobal(localRobot));
auto desiredRobotPoseEigen = desiredRobotPose->toEigen();
// objectPose is actually the tcp pose...
// FramedPosePtr desiredTCPPose = FramedPosePtr::dynamicCast(gp.grasp.framedPose);
// TODO: move along gcp z axis
// desiredTCPPose->changeFrame(localRobot, localRobot->getEndEffector(gp.grasp.eefName)->getGCP()->getName());
// desiredTCPPose = FramedPosePtr::dynamicCast(gp.grasp.framedPose)->toGlobal(localRobot);
- auto desiredTCPPose = FramedPosePtr::dynamicCast(gp.grasp.framedPose)->toGlobalEigen(localRobot);
+ auto desiredTCPPose =
+ FramedPosePtr::dynamicCast(gp.grasp.framedPose)->toGlobalEigen(localRobot);
auto desiredTCPPoseRelativeToRobotEigen = desiredRobotPoseEigen.inverse() * desiredTCPPose;
- FramedPosePtr desiredTCPPoseRelativeToRobot {new FramedPose(desiredTCPPoseRelativeToRobotEigen, localRobot->getRootNode()->getName(), localRobot->getName())};
- auto desiredTCPPrepose = FramedPosePtr::dynamicCast(gp.grasp.framedPrePose)->toGlobalEigen(localRobot);
- auto desiredTCPPreposeRelativeToRobotEigen = desiredRobotPoseEigen.inverse() * desiredTCPPrepose;
- FramedPosePtr desiredTCPPreposeRelativeToRobot {new FramedPose(desiredTCPPreposeRelativeToRobotEigen, localRobot->getRootNode()->getName(), localRobot->getName())};
+ FramedPosePtr desiredTCPPoseRelativeToRobot{
+ new FramedPose(desiredTCPPoseRelativeToRobotEigen,
+ localRobot->getRootNode()->getName(),
+ localRobot->getName())};
+ auto desiredTCPPrepose =
+ FramedPosePtr::dynamicCast(gp.grasp.framedPrePose)->toGlobalEigen(localRobot);
+ auto desiredTCPPreposeRelativeToRobotEigen =
+ desiredRobotPoseEigen.inverse() * desiredTCPPrepose;
+ FramedPosePtr desiredTCPPreposeRelativeToRobot{
+ new FramedPose(desiredTCPPreposeRelativeToRobotEigen,
+ localRobot->getRootNode()->getName(),
+ localRobot->getName())};
Ice::StringSeq potentialRNs;
auto tcpName = localRobot->getEndEffector(gp.grasp.eefName)->getTcp()->getName();
for (const auto& rnsName : robotNodeSetNames)
{
- ARMARX_CHECK_EXPRESSION(localRobot->hasRobotNodeSet(rnsName)) << "Could not find RNS '" << rnsName << "' in RNS list of robot " << localRobot->getName();
+ ARMARX_CHECK_EXPRESSION(localRobot->hasRobotNodeSet(rnsName))
+ << "Could not find RNS '" << rnsName << "' in RNS list of robot "
+ << localRobot->getName();
if (localRobot->getRobotNodeSet(rnsName)->getTCP()->getName() == tcpName)
{
potentialRNs.push_back(rnsName);
}
}
- float visuSlowdownFactor = getProperty<float>("VisualizationSlowdownFactor").getValue();
+ float visuSlowdownFactor = getProperty<float>("VisualizationSlowdownFactor").getValue();
if (potentialRNs.empty())
{
- ARMARX_WARNING << "Could not find RNS with tcp '" << tcpName << "'; will not process the corresponding generated grasp...";
+ ARMARX_WARNING << "Could not find RNS with tcp '" << tcpName
+ << "'; will not process the corresponding generated grasp...";
continue;
}
std::string rnsToUse;
- if (/*rik->isFramedPoseReachable(rnsToUse, objectPoseRelativeToRobot) || */true)
+ if (/*rik->isFramedPoseReachable(rnsToUse, objectPoseRelativeToRobot) || */ true)
{
if (getProperty<bool>("EnableVisualization"))
{
if (robotVisuId.empty())
{
robotVisuId = newId();
- entityDrawer->setRobotVisu(layerName, robotVisuId, rsc->getRobotFilename(), simox::alg::join(rsc->getArmarXPackages(), ","), FullModel);
+ entityDrawer->setRobotVisu(layerName,
+ robotVisuId,
+ rsc->getRobotFilename(),
+ simox::alg::join(rsc->getArmarXPackages(), ","),
+ FullModel);
entityDrawer->updateRobotColor(layerName, robotVisuId, COLOR_ROBOT);
}
- entityDrawer->updateRobotColor(layerName, robotVisuId, DrawColor {1.0, 1.0f, 1.0, 0.5});
+ entityDrawer->updateRobotColor(
+ layerName, robotVisuId, DrawColor{1.0, 1.0f, 1.0, 0.5});
entityDrawer->updateRobotPose(layerName, robotVisuId, desiredRobotPose);
- entityDrawer->updateRobotConfig(layerName, robotVisuId, localRobot->getConfig()->getRobotNodeJointValueMap());
+ entityDrawer->updateRobotConfig(
+ layerName, robotVisuId, localRobot->getConfig()->getRobotNodeJointValueMap());
}
- ARMARX_VERBOSE << "Pose " << VAROUT(*desiredTCPPoseRelativeToRobot) << " with RNS '" << potentialRNs << "' is reachable";
+ ARMARX_VERBOSE << "Pose " << VAROUT(*desiredTCPPoseRelativeToRobot) << " with RNS '"
+ << potentialRNs << "' is reachable";
NameValueMap ikSolution;
for (auto& rns : potentialRNs)
{
- ikSolution = calculateSingleIK(rns, gp.grasp.eefName, gp.robotPose, desiredTCPPoseRelativeToRobot, false);
+ ikSolution = calculateSingleIK(
+ rns, gp.grasp.eefName, gp.robotPose, desiredTCPPoseRelativeToRobot, false);
if (ikSolution.empty())
{
continue;
}
- ikSolution = calculateSingleIK(rns, gp.grasp.eefName, gp.robotPose, desiredTCPPreposeRelativeToRobot);
+ ikSolution = calculateSingleIK(
+ rns, gp.grasp.eefName, gp.robotPose, desiredTCPPreposeRelativeToRobot);
if (!ikSolution.empty())
{
rnsToUse = rns;
@@ -702,8 +767,9 @@ std::vector<MotionPlanningData> GraspingManager::calculateIKs(const GraspingPlac
if (getProperty<bool>("EnableVisualization"))
{
usleep(1000000 * visuSlowdownFactor);
- auto graspVisuId = visualizeGrasp(gp.grasp, i, DrawColor {1.0, 0, 0, 1});
- entityDrawer->updateRobotColor(layerName, robotVisuId, DrawColor {1.0, 0.0f, 0.0, 1});
+ auto graspVisuId = visualizeGrasp(gp.grasp, i, DrawColor{1.0, 0, 0, 1});
+ entityDrawer->updateRobotColor(
+ layerName, robotVisuId, DrawColor{1.0, 0.0f, 0.0, 1});
ARMARX_VERBOSE << "...but has no IK solution";
usleep(1000000 * visuSlowdownFactor);
for (auto id : graspVisuId.second)
@@ -722,7 +788,8 @@ std::vector<MotionPlanningData> GraspingManager::calculateIKs(const GraspingPlac
if (getProperty<bool>("EnableVisualization"))
{
usleep(1000000 * visuSlowdownFactor);
- entityDrawer->updateRobotColor(layerName, robotVisuId, DrawColor {0.0, 1.0f, 0.0, 1});
+ entityDrawer->updateRobotColor(
+ layerName, robotVisuId, DrawColor{0.0, 1.0f, 0.0, 1});
entityDrawer->updateRobotPose(layerName, robotVisuId, desiredRobotPose);
entityDrawer->updateRobotConfig(layerName, robotVisuId, ikSolution);
usleep(2000000 * visuSlowdownFactor);
@@ -747,8 +814,9 @@ std::vector<MotionPlanningData> GraspingManager::calculateIKs(const GraspingPlac
{
ARMARX_INFO << "Current config: " << currentConfig;
ARMARX_INFO << "Desired config: " << ikSolution;
- ARMARX_CHECK_EXPRESSION(false) <<
- "calculated configuration contains a joint '" << entry.first << "' whose current value is unknown";
+ ARMARX_CHECK_EXPRESSION(false)
+ << "calculated configuration contains a joint '" << entry.first
+ << "' whose current value is unknown";
}
}
ARMARX_CHECK_EXPRESSION(currentConfig.size() == ikSolution.size());
@@ -756,14 +824,17 @@ std::vector<MotionPlanningData> GraspingManager::calculateIKs(const GraspingPlac
// entityDrawer->setPoseVisu("Poses" , "TCPTargetPose" + to_string(i), objectPoseRelativeToRobot->toGlobal(rsc->getSynchronizedRobot()));
ARMARX_INFO << VAROUT(currentRobotPose) << VAROUT(desiredRobotPose->output());
mpdList.push_back({new FramedPose(currentRobotPose, GlobalFrame, ""),
- desiredRobotPose, currentConfig, ikSolution, rnsToUse, gp.grasp.eefName,
- gp.grasp
- });
-
+ desiredRobotPose,
+ currentConfig,
+ ikSolution,
+ rnsToUse,
+ gp.grasp.eefName,
+ gp.grasp});
}
else
{
- ARMARX_VERBOSE << "Pose " << VAROUT(*desiredTCPPoseRelativeToRobot) << " with RNS '" << potentialRNs << "' not reachable";
+ ARMARX_VERBOSE << "Pose " << VAROUT(*desiredTCPPoseRelativeToRobot) << " with RNS '"
+ << potentialRNs << "' not reachable";
}
if (getProperty<bool>("EnableVisualization"))
for (auto id : graspVisuId.second)
@@ -779,7 +850,12 @@ std::vector<MotionPlanningData> GraspingManager::calculateIKs(const GraspingPlac
return mpdList;
}
-NameValueMap GraspingManager::calculateSingleIK(const std::string& robotNodeSetName, const std::string& eef, const PoseBasePtr& globalRobotPose, const FramedPoseBasePtr& tcpPose, bool checkCollisionFree)
+NameValueMap
+GraspingManager::calculateSingleIK(const std::string& robotNodeSetName,
+ const std::string& eef,
+ const PoseBasePtr& globalRobotPose,
+ const FramedPoseBasePtr& tcpPose,
+ bool checkCollisionFree)
{
TIMING_START(SingleIK);
auto ikRobot = cspace->getAgentSceneObj();
@@ -789,49 +865,71 @@ NameValueMap GraspingManager::calculateSingleIK(const std::string& robotNodeSetN
auto tcp = ikRobot->getEndEffector(eef)->getTcp();
Eigen::Matrix4f targetPose = FramedPosePtr::dynamicCast(tcpPose)->toGlobalEigen(ikRobot);
VirtualRobot::ConstrainedOptimizationIK ik(ikRobot, rns);
- VirtualRobot::PoseConstraintPtr poseConstraint(new VirtualRobot::PoseConstraint(
- ikRobot, rns, tcp, targetPose
- ));
+ VirtualRobot::PoseConstraintPtr poseConstraint(
+ new VirtualRobot::PoseConstraint(ikRobot, rns, tcp, targetPose));
if (checkCollisionFree)
{
auto colSetEnv = cspace->getStationaryObjectSet();
- VirtualRobot::CDManagerPtr cdm(new VirtualRobot::CDManager(cspace->getCD().getCollisionChecker()));
- VirtualRobot::SceneObjectSetPtr sosRns(new VirtualRobot::SceneObjectSet("RNS", cspace->getCD().getCollisionChecker()));
+ VirtualRobot::CDManagerPtr cdm(
+ new VirtualRobot::CDManager(cspace->getCD().getCollisionChecker()));
+ VirtualRobot::SceneObjectSetPtr sosRns(
+ new VirtualRobot::SceneObjectSet("RNS", cspace->getCD().getCollisionChecker()));
std::string armCollisionSet = getJointSetCollisionSetMapping().at(robotNodeSetName);
- ARMARX_INFO << "Using Arm Collision Set '" << armCollisionSet << "' and platform set '" << getProperty<std::string>("RobotCollisionNodeSet").getValue() << "'";
+ ARMARX_INFO << "Using Arm Collision Set '" << armCollisionSet << "' and platform set '"
+ << getProperty<std::string>("RobotCollisionNodeSet").getValue() << "'";
sosRns->addSceneObjects(cspace->getAgentSceneObj()->getRobotNodeSet(armCollisionSet));
cdm->addCollisionModelPair(colSetEnv, sosRns);
- VirtualRobot::SceneObjectSetPtr sosPlatform(new VirtualRobot::SceneObjectSet("platform", cspace->getCD().getCollisionChecker()));
+ VirtualRobot::SceneObjectSetPtr sosPlatform(
+ new VirtualRobot::SceneObjectSet("platform", cspace->getCD().getCollisionChecker()));
- auto robotCol = cspace->getAgentSceneObj()->getRobotNodeSet(getProperty<std::string>("RobotCollisionNodeSet").getValue());
+ auto robotCol = cspace->getAgentSceneObj()->getRobotNodeSet(
+ getProperty<std::string>("RobotCollisionNodeSet").getValue());
auto robotColNodeNames = robotCol->getNodeNames();
- std::vector<std::string> remainingNodeNames = getRobotNodeSetNodesWithoutAllwaysColliding(robotColNodeNames, armCollisionSet);
+ std::vector<std::string> remainingNodeNames =
+ getRobotNodeSetNodesWithoutAllwaysColliding(robotColNodeNames, armCollisionSet);
for (const auto& colNodeName : remainingNodeNames)
{
sosPlatform->addSceneObject(cspace->getAgentSceneObj()->getRobotNode(colNodeName));
}
- ARMARX_INFO << "RobotCollisionNodeSet contains the following nodes after filtering allways colliding nodes:" << std::endl << VAROUT(remainingNodeNames);
+ ARMARX_INFO << "RobotCollisionNodeSet contains the following nodes after filtering allways "
+ "colliding nodes:"
+ << std::endl
+ << VAROUT(remainingNodeNames);
cdm->addCollisionModelPair(sosPlatform, sosRns);
// ik.addConstraint(poseConstraint);
- ik.addConstraint(VirtualRobot::ConstraintPtr(new VirtualRobot::CollisionCheckConstraint(rns, cdm)));
+ ik.addConstraint(
+ VirtualRobot::ConstraintPtr(new VirtualRobot::CollisionCheckConstraint(rns, cdm)));
}
- VirtualRobot::ConstraintPtr posConstraint(new VirtualRobot::PositionConstraint(ikRobot, rns, tcp,
- targetPose.block<3, 1>(0, 3), VirtualRobot::IKSolver::CartesianSelection::All));
+ VirtualRobot::ConstraintPtr posConstraint(
+ new VirtualRobot::PositionConstraint(ikRobot,
+ rns,
+ tcp,
+ targetPose.block<3, 1>(0, 3),
+ VirtualRobot::IKSolver::CartesianSelection::All));
posConstraint->setOptimizationFunctionFactor(1);
- VirtualRobot::ConstraintPtr oriConstraint(new VirtualRobot::OrientationConstraint(ikRobot, rns, tcp,
- targetPose.block<3, 3>(0, 0), VirtualRobot::IKSolver::CartesianSelection::All, VirtualRobot::MathTools::deg2rad(2)));
+ VirtualRobot::ConstraintPtr oriConstraint(
+ new VirtualRobot::OrientationConstraint(ikRobot,
+ rns,
+ tcp,
+ targetPose.block<3, 3>(0, 0),
+ VirtualRobot::IKSolver::CartesianSelection::All,
+ VirtualRobot::MathTools::deg2rad(2)));
oriConstraint->setOptimizationFunctionFactor(1000);
ik.addConstraint(posConstraint);
ik.addConstraint(oriConstraint);
NameValueMap result;
- NaturalIKResult natIKResult = getNaturalIK()->solveIK(eef, FramedPosePtr::dynamicCast(tcpPose)->toRootEigen(ikRobot), true, new armarx::aron::data::dto::Dict());
+ NaturalIKResult natIKResult =
+ getNaturalIK()->solveIK(eef,
+ FramedPosePtr::dynamicCast(tcpPose)->toRootEigen(ikRobot),
+ true,
+ new armarx::aron::data::dto::Dict());
if (!natIKResult.reached)
{
ARMARX_WARNING << "natik failed!";
@@ -839,7 +937,8 @@ NameValueMap GraspingManager::calculateSingleIK(const std::string& robotNodeSetN
}
else
{
- VirtualRobot::ReferenceConfigurationConstraintPtr natIKConstraint(new VirtualRobot::ReferenceConfigurationConstraint(ikRobot, rns));
+ VirtualRobot::ReferenceConfigurationConstraintPtr natIKConstraint(
+ new VirtualRobot::ReferenceConfigurationConstraint(ikRobot, rns));
ARMARX_IMPORTANT << VAROUT(natIKResult.jointValues);
Eigen::VectorXf referenceConfig(natIKResult.jointValues.size());
for (size_t i = 0; i < natIKResult.jointValues.size(); i++)
@@ -849,7 +948,6 @@ NameValueMap GraspingManager::calculateSingleIK(const std::string& robotNodeSetN
ARMARX_IMPORTANT << VAROUT(referenceConfig);
natIKConstraint->setReferenceConfiguration(referenceConfig);
ik.addConstraint(natIKConstraint);
-
}
if (!ik.initialize())
@@ -872,7 +970,6 @@ NameValueMap GraspingManager::calculateSingleIK(const std::string& robotNodeSetN
// result[rns->getNode((int)i)->getName()] = rns->getNode(i)->getJointValue();
// }
//}
-
}
else if (natIKResult.reached)
{
@@ -885,12 +982,11 @@ NameValueMap GraspingManager::calculateSingleIK(const std::string& robotNodeSetN
TIMING_END(SingleIK);
-
return result;
-
}
-StringStringDictionary GraspingManager::getJointSetCollisionSetMapping()
+StringStringDictionary
+GraspingManager::getJointSetCollisionSetMapping()
{
StringStringDictionary result;
auto propString = getProperty<std::string>("JointToLinkSetMapping").getValue();
@@ -904,7 +1000,9 @@ StringStringDictionary GraspingManager::getJointSetCollisionSetMapping()
return result;
}
-GraspingTrajectoryList GraspingManager::generateGraspingTrajectoryListForGraspListInternal(const GeneratedGraspList& grasps)
+GraspingTrajectoryList
+GraspingManager::generateGraspingTrajectoryListForGraspListInternal(
+ const GeneratedGraspList& grasps)
{
std::vector<MotionPlanningData> mpdList = generateIKsInternal(grasps);
if (mpdList.empty())
@@ -926,7 +1024,6 @@ GraspingTrajectoryList GraspingManager::generateGraspingTrajectoryListForGraspLi
GraspingTrajectory gt = planMotion(graspingData);
result.push_back(gt);
-
}
catch (...)
{
@@ -940,7 +1037,9 @@ GraspingTrajectoryList GraspingManager::generateGraspingTrajectoryListForGraspLi
return result;
}
-GraspingTrajectory GraspingManager::generateGraspingTrajectory(const std::string& objectInstanceEntityId, const Ice::Current&)
+GraspingTrajectory
+GraspingManager::generateGraspingTrajectory(const std::string& objectInstanceEntityId,
+ const Ice::Current&)
{
ScopedLock lock(graspManagerMutex);
ARMARX_ON_SCOPE_EXIT
@@ -970,7 +1069,9 @@ GraspingTrajectory GraspingManager::generateGraspingTrajectory(const std::string
return result.front();
}
-GraspingTrajectoryList GraspingManager::generateGraspingTrajectoryList(const std::string& objectInstanceEntityId, const Ice::Current&)
+GraspingTrajectoryList
+GraspingManager::generateGraspingTrajectoryList(const std::string& objectInstanceEntityId,
+ const Ice::Current&)
{
ScopedLock lock(graspManagerMutex);
ARMARX_ON_SCOPE_EXIT
@@ -985,9 +1086,12 @@ GraspingTrajectoryList GraspingManager::generateGraspingTrajectoryList(const std
return generateGraspingTrajectoryListForGraspListInternal(grasps);
}
-GraspingTrajectoryList GraspingManager::generateGraspingTrajectoryListForGraspList(const GeneratedGraspList& grasps, const Ice::Current&)
+GraspingTrajectoryList
+GraspingManager::generateGraspingTrajectoryListForGraspList(const GeneratedGraspList& grasps,
+ const Ice::Current&)
{
- if (grasps.empty()) return GraspingTrajectoryList{};
+ if (grasps.empty())
+ return GraspingTrajectoryList{};
ScopedLock lock(graspManagerMutex);
ARMARX_ON_SCOPE_EXIT
{
@@ -995,7 +1099,8 @@ GraspingTrajectoryList GraspingManager::generateGraspingTrajectoryListForGraspLi
entityDrawer->clearLayer(layerName);
};
RemoteRobot::synchronizeLocalClone(localRobot, rsc);
- FramedPosePtr pose = FramedPosePtr::dynamicCast(grasps.front().framedPose)->toGlobal(localRobot);
+ FramedPosePtr pose =
+ FramedPosePtr::dynamicCast(grasps.front().framedPose)->toGlobal(localRobot);
pose->position->z += 400;
globalDescriptionPosition = Vector3Ptr::dynamicCast(pose->position);
@@ -1015,14 +1120,18 @@ GraspingTrajectoryList GraspingManager::generateGraspingTrajectoryListForGraspLi
return result;
}
-void GraspingManager::visualizeGraspingTrajectory(const GraspingTrajectory& trajectory, float visuSlowdownFactor, const Ice::Current&)
+void
+GraspingManager::visualizeGraspingTrajectory(const GraspingTrajectory& trajectory,
+ float visuSlowdownFactor,
+ const Ice::Current&)
{
drawTrajectory(trajectory, visuSlowdownFactor);
sleep(2 * visuSlowdownFactor);
entityDrawer->removeLayer(layerName);
}
-MotionPlanningDataList GraspingManager::generateIKs(const std::string& objectInstanceEntityId, const Ice::Current&)
+MotionPlanningDataList
+GraspingManager::generateIKs(const std::string& objectInstanceEntityId, const Ice::Current&)
{
ScopedLock lock(graspManagerMutex);
ARMARX_ON_SCOPE_EXIT
@@ -1036,17 +1145,18 @@ MotionPlanningDataList GraspingManager::generateIKs(const std::string& objectIns
return generateIKsInternal(grasps);
}
-GeneratedGraspList GraspingManager::generateGraspsByObjectName(const std::string& objectName, const Ice::Current&)
+GeneratedGraspList
+GraspingManager::generateGraspsByObjectName(const std::string& objectName, const Ice::Current&)
{
GeneratedGraspList grasps = gg->generateGraspsByObjectName(objectName);
- std::sort(grasps.begin(), grasps.end(), [](const GeneratedGrasp & l, const GeneratedGrasp & r)
- {
- return l.score < r.score;
- });
+ std::sort(grasps.begin(),
+ grasps.end(),
+ [](const GeneratedGrasp& l, const GeneratedGrasp& r) { return l.score < r.score; });
return grasps;
}
-MotionPlanningDataList GraspingManager::generateIKsInternal(const GeneratedGraspList& grasps)
+MotionPlanningDataList
+GraspingManager::generateIKsInternal(const GeneratedGraspList& grasps)
{
ikRobot->setConfig(localRobot->getConfig());
auto filteredGrasps = filterGrasps(grasps);
@@ -1060,8 +1170,10 @@ MotionPlanningDataList GraspingManager::generateIKsInternal(const GeneratedGrasp
GeneratedGraspList notDirectGrasps;
for (const auto& g : filteredGrasps)
{
- const Eigen::Vector3f position = FramedPosePtr::dynamicCast(g.framedPose)->getPosition()->toGlobalEigen(localRobot);
- const float distance2D = (position.head(2) - localRobot->getGlobalPose().block<2, 1>(0, 3)).norm();
+ const Eigen::Vector3f position =
+ FramedPosePtr::dynamicCast(g.framedPose)->getPosition()->toGlobalEigen(localRobot);
+ const float distance2D =
+ (position.head(2) - localRobot->getGlobalPose().block<2, 1>(0, 3)).norm();
ARMARX_VERBOSE << "2D Distance to object: " << distance2D;
// if already close to object, try to grasp directly without platform movement
if (distance2D < maxDistance2D)
@@ -1079,7 +1191,9 @@ MotionPlanningDataList GraspingManager::generateIKsInternal(const GeneratedGrasp
if (!notDirectGrasps.empty())
{
auto graspPlacementsNotDirect = generateRobotPlacements(notDirectGrasps);
- graspPlacements.insert(graspPlacements.end(), graspPlacementsNotDirect.begin(), graspPlacementsNotDirect.end());
+ graspPlacements.insert(graspPlacements.end(),
+ graspPlacementsNotDirect.begin(),
+ graspPlacementsNotDirect.end());
}
GraspingPlacementList filteredGraspPlacements = filterPlacements(graspPlacements);
@@ -1088,9 +1202,13 @@ MotionPlanningDataList GraspingManager::generateIKsInternal(const GeneratedGrasp
return calculateIKs(filteredGraspPlacements);
}
-std::vector<std::string> GraspingManager::getRobotNodeSetNodesWithoutAllwaysColliding(const std::vector<std::string>& robotColNodeNames, const std::string& armCollisionSet)
+std::vector<std::string>
+GraspingManager::getRobotNodeSetNodesWithoutAllwaysColliding(
+ const std::vector<std::string>& robotColNodeNames,
+ const std::string& armCollisionSet)
{
- const auto rnsColNames = cspace->getAgentSceneObj()->getRobotNodeSet(armCollisionSet)->getNodeNames();
+ const auto rnsColNames =
+ cspace->getAgentSceneObj()->getRobotNodeSet(armCollisionSet)->getNodeNames();
std::set<std::string> rnsColNamesIncludingIgnore;
rnsColNamesIncludingIgnore.insert(rnsColNames.begin(), rnsColNames.end());
for (const auto& rnsNodeName : rnsColNames)
diff --git a/source/RobotComponents/components/GraspingManager/GraspingManager.h b/source/RobotComponents/components/GraspingManager/GraspingManager.h
index 384edac2cec589cf7bd79f5211a975fb5d6a1895..a8c56ddf040f0483179b5c247adfda2262bd0d01 100644
--- a/source/RobotComponents/components/GraspingManager/GraspingManager.h
+++ b/source/RobotComponents/components/GraspingManager/GraspingManager.h
@@ -24,27 +24,29 @@
#pragma once
+#include <Eigen/Geometry>
+
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/time/TimeKeeper.h>
-#include <RobotComponents/interface/components/GraspingManager/GraspingManagerInterface.h>
+
+#include <RobotAPI/interface/core/RobotState.h>
+#include <RobotAPI/libraries/RobotAPIComponentPlugins/NaturalIKComponentPlugin.h>
+#include <RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h>
+
+#include <RobotComponents/components/MotionPlanning/CSpace/SimoxCSpace.h>
+#include <RobotComponents/components/MotionPlanning/Tasks/CSpaceVisualizerTask/CSpaceVisualizerTask.h>
#include <RobotComponents/interface/components/GraspingManager/GraspGeneratorInterface.h>
#include <RobotComponents/interface/components/GraspingManager/GraspSelectionManagerInterface.h>
+#include <RobotComponents/interface/components/GraspingManager/GraspingManagerInterface.h>
#include <RobotComponents/interface/components/GraspingManager/RobotPlacementInterface.h>
#include <RobotComponents/interface/components/MotionPlanning/MotionPlanningServer.h>
-//#include <RobotComponents/interface/components/RobotIK.h>
#include <RobotComponents/interface/components/PlannedMotionProviderInterface.h>
-#include <RobotComponents/components/MotionPlanning/Tasks/CSpaceVisualizerTask/CSpaceVisualizerTask.h>
-#include <RobotAPI/interface/core/RobotState.h>
-#include <MemoryX/interface/components/WorkingMemoryInterface.h>
+#include <MemoryX/core/MemoryXCoreObjectFactories.h>
#include <MemoryX/interface/components/PriorKnowledgeInterface.h>
+#include <MemoryX/interface/components/WorkingMemoryInterface.h>
#include <MemoryX/interface/gui/EntityDrawerInterface.h>
-#include <MemoryX/core/MemoryXCoreObjectFactories.h>
#include <MemoryX/libraries/memorytypes/MemoryXTypesObjectFactories.h>
-#include <Eigen/Geometry>
-#include <RobotComponents/components/MotionPlanning/CSpace/SimoxCSpace.h>
-#include <RobotAPI/libraries/RobotAPIComponentPlugins/NaturalIKComponentPlugin.h>
-#include <RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h>
namespace armarx
{
@@ -52,31 +54,68 @@ namespace armarx
* @class GraspingManagerPropertyDefinitions
* @brief
*/
- class GraspingManagerPropertyDefinitions:
- public ComponentPropertyDefinitions
+ class GraspingManagerPropertyDefinitions : public ComponentPropertyDefinitions
{
public:
- GraspingManagerPropertyDefinitions(std::string prefix):
+ GraspingManagerPropertyDefinitions(std::string prefix) :
ComponentPropertyDefinitions(prefix)
{
- defineOptionalProperty<std::string>("GraspGeneratorName", "SimpleGraspGenerator", "Name of the GraspGenerator proxy");
- defineOptionalProperty<std::string>("RobotPlacementName", "SimpleRobotPlacement", "Name of the RobotPlacement proxy");
- defineOptionalProperty<std::string>("RobotNodeSetNames", "HipYawRightArm;HipYawLeftArm", "Names of the robot node sets to use for IK calculations (';' delimited)");
- defineOptionalProperty<std::string>("RobotCollisionNodeSet", "Robot", "Name of the collision nodeset used for motion planning", PropertyDefinitionBase::eModifiable);
- defineRequiredProperty<std::string>("JointToLinkSetMapping", "Mapping from joint set to link set, i.e. which collision set to use for which kinematic chain. Format: JointSet1:CollisionSet1;JointSet2:CollisionSet2", PropertyDefinitionBase::eModifiable);
- defineOptionalProperty<std::string>("ReachabilitySpaceFilePaths",
- "Armar3/reachability/reachability_right_hand_smoothened.bin;Armar3/reachability/reachability_left_hand_smoothened.bin",
- "Paths to the reachability space files (needed only if no reachability space is loaded for the chosen RobotNodeSets) (';' delimited)");
- defineOptionalProperty<std::string>("PlanningBoundingBox", "-15000, -4000, -3.1416, 15000, 15000, 3.1416", "x_min, y_min, alpha_min, x_max, y_max, alpha_max");
- defineOptionalProperty<float>("VisualizationSlowdownFactor", 1.0f, "1.0 is a good value for clear visualization, 0 the visualization should not slow down the process", PropertyDefinitionBase::eModifiable);
- defineOptionalProperty<bool>("EnableVisualization", true, "If false no visualization is done.", PropertyDefinitionBase::eModifiable);
- defineOptionalProperty<float>("MaxDistanceForDirectGrasp", 1200.0f, "x-y-Distance from robot base to object for which the grasping manager tries to find a solution without platform movement", PropertyDefinitionBase::eModifiable);
- defineOptionalProperty<bool>("UseVoxelGridCSpace", false, "If true, the VoxelGridCSpace is used instead of the SimoxCSpace.", PropertyDefinitionBase::eModifiable);
- defineOptionalProperty<std::string>("VoxelGridProviderName", "VoxelGridProvider", "Name of the Voxel Grid Provider", PropertyDefinitionBase::eModifiable);
- defineOptionalProperty<bool>("FilterUnnaturalGrasps", false, "If set to true, uses the NaturalGraspFilter to exclude unnatural looking grasps and grasp poses.", PropertyDefinitionBase::eModifiable);
-
-
-
+ defineOptionalProperty<std::string>(
+ "GraspGeneratorName", "SimpleGraspGenerator", "Name of the GraspGenerator proxy");
+ defineOptionalProperty<std::string>(
+ "RobotPlacementName", "SimpleRobotPlacement", "Name of the RobotPlacement proxy");
+ defineOptionalProperty<std::string>(
+ "RobotNodeSetNames",
+ "HipYawRightArm;HipYawLeftArm",
+ "Names of the robot node sets to use for IK calculations (';' delimited)");
+ defineOptionalProperty<std::string>(
+ "RobotCollisionNodeSet",
+ "Robot",
+ "Name of the collision nodeset used for motion planning",
+ PropertyDefinitionBase::eModifiable);
+ defineRequiredProperty<std::string>(
+ "JointToLinkSetMapping",
+ "Mapping from joint set to link set, i.e. which collision set to use for which "
+ "kinematic chain. Format: JointSet1:CollisionSet1;JointSet2:CollisionSet2",
+ PropertyDefinitionBase::eModifiable);
+ defineOptionalProperty<std::string>(
+ "ReachabilitySpaceFilePaths",
+ "Armar3/reachability/reachability_right_hand_smoothened.bin;Armar3/reachability/"
+ "reachability_left_hand_smoothened.bin",
+ "Paths to the reachability space files (needed only if no reachability space is "
+ "loaded for the chosen RobotNodeSets) (';' delimited)");
+ defineOptionalProperty<std::string>("PlanningBoundingBox",
+ "-15000, -4000, -3.1416, 15000, 15000, 3.1416",
+ "x_min, y_min, alpha_min, x_max, y_max, alpha_max");
+ defineOptionalProperty<float>("VisualizationSlowdownFactor",
+ 1.0f,
+ "1.0 is a good value for clear visualization, 0 the "
+ "visualization should not slow down the process",
+ PropertyDefinitionBase::eModifiable);
+ defineOptionalProperty<bool>("EnableVisualization",
+ true,
+ "If false no visualization is done.",
+ PropertyDefinitionBase::eModifiable);
+ defineOptionalProperty<float>(
+ "MaxDistanceForDirectGrasp",
+ 1200.0f,
+ "x-y-Distance from robot base to object for which the grasping manager tries to "
+ "find a solution without platform movement",
+ PropertyDefinitionBase::eModifiable);
+ defineOptionalProperty<bool>(
+ "UseVoxelGridCSpace",
+ false,
+ "If true, the VoxelGridCSpace is used instead of the SimoxCSpace.",
+ PropertyDefinitionBase::eModifiable);
+ defineOptionalProperty<std::string>("VoxelGridProviderName",
+ "VoxelGridProvider",
+ "Name of the Voxel Grid Provider",
+ PropertyDefinitionBase::eModifiable);
+ defineOptionalProperty<bool>("FilterUnnaturalGrasps",
+ false,
+ "If set to true, uses the NaturalGraspFilter to exclude "
+ "unnatural looking grasps and grasp poses.",
+ PropertyDefinitionBase::eModifiable);
}
};
@@ -116,7 +155,8 @@ namespace armarx
/**
* @see armarx::ManagedIceObject::getDefaultName()
*/
- std::string getDefaultName() const override
+ std::string
+ getDefaultName() const override
{
return "GraspingManager";
}
@@ -132,16 +172,27 @@ namespace armarx
* @param c
* @return A list of grasping trajectories in configuration and pose space.
*/
- GraspingTrajectory generateGraspingTrajectory(const std::string& objectInstanceEntityId, const Ice::Current& c = Ice::emptyCurrent) override;
- GraspingTrajectoryList generateGraspingTrajectoryList(const std::string& objectInstanceEntityId, const Ice::Current& c = Ice::emptyCurrent) override;
- GraspingTrajectoryList generateGraspingTrajectoryListForGraspList(const GeneratedGraspList& grasps, const Ice::Current& c = Ice::emptyCurrent) override;
- void visualizeGraspingTrajectory(const GraspingTrajectory& trajectory, float visuSlowdownFactor, const Ice::Current& c = Ice::emptyCurrent) override;
-
- MotionPlanningDataList generateIKs(const std::string& objectInstanceEntityId, const Ice::Current& c = Ice::emptyCurrent) override;
+ GraspingTrajectory
+ generateGraspingTrajectory(const std::string& objectInstanceEntityId,
+ const Ice::Current& c = Ice::emptyCurrent) override;
+ GraspingTrajectoryList
+ generateGraspingTrajectoryList(const std::string& objectInstanceEntityId,
+ const Ice::Current& c = Ice::emptyCurrent) override;
+ GraspingTrajectoryList generateGraspingTrajectoryListForGraspList(
+ const GeneratedGraspList& grasps,
+ const Ice::Current& c = Ice::emptyCurrent) override;
+ void visualizeGraspingTrajectory(const GraspingTrajectory& trajectory,
+ float visuSlowdownFactor,
+ const Ice::Current& c = Ice::emptyCurrent) override;
+
+ MotionPlanningDataList generateIKs(const std::string& objectInstanceEntityId,
+ const Ice::Current& c = Ice::emptyCurrent) override;
///// single step methods /////
- GeneratedGraspList generateGraspsByObjectName(const std::string& objectName, const Ice::Current& = Ice::emptyCurrent) override;
+ GeneratedGraspList
+ generateGraspsByObjectName(const std::string& objectName,
+ const Ice::Current& = Ice::emptyCurrent) override;
protected:
@@ -168,15 +219,21 @@ namespace armarx
/**
* @see PropertyUser::createPropertyDefinitions()
*/
- PropertyDefinitionsPtr createPropertyDefinitions() override
+ PropertyDefinitionsPtr
+ createPropertyDefinitions() override
{
- return PropertyDefinitionsPtr(new GraspingManagerPropertyDefinitions(getConfigIdentifier()));
+ return PropertyDefinitionsPtr(
+ new GraspingManagerPropertyDefinitions(getConfigIdentifier()));
}
/**
* @brief Visualizes the grasp using the DebugDrawer topic
*/
- std::pair<std::string, Ice::StringSeq> visualizeGrasp(const GeneratedGrasp& grasp, int id, const DrawColor& color = DrawColor {1, 1, 1, 1});
+ std::pair<std::string, Ice::StringSeq>
+ visualizeGrasp(const GeneratedGrasp& grasp,
+ int id,
+ const DrawColor& color = DrawColor{1, 1, 1, 1});
+
private:
/**
* @brief Creates a pointer to the SimoxCSpace::SimoxCSpace() and initializes it.
@@ -258,7 +315,11 @@ namespace armarx
* @param checkCollisionFree
* @return NameValueMap of the solved IK
*/
- NameValueMap calculateSingleIK(const ::std::string& robotNodeSetName, const std::string& eef, const PoseBasePtr& globalRobotPose, const ::armarx::FramedPoseBasePtr& tcpPose, bool checkCollisionFree = true);
+ NameValueMap calculateSingleIK(const ::std::string& robotNodeSetName,
+ const std::string& eef,
+ const PoseBasePtr& globalRobotPose,
+ const ::armarx::FramedPoseBasePtr& tcpPose,
+ bool checkCollisionFree = true);
/**
* Wrapper for the property JointToLinkSetMapping
@@ -274,10 +335,13 @@ namespace armarx
* @param objId The object ID. Only used for logging.
* @return The list of valid generated grasps
*/
- GraspingTrajectoryList generateGraspingTrajectoryListForGraspListInternal(const GeneratedGraspList& grasps);
+ GraspingTrajectoryList
+ generateGraspingTrajectoryListForGraspListInternal(const GeneratedGraspList& grasps);
MotionPlanningDataList generateIKsInternal(const GeneratedGraspList& grasps);
- std::vector<std::string> getRobotNodeSetNodesWithoutAllwaysColliding(const std::vector<std::string>& robotColNodeNames, const std::string& armCollisionSet);
+ std::vector<std::string> getRobotNodeSetNodesWithoutAllwaysColliding(
+ const std::vector<std::string>& robotColNodeNames,
+ const std::string& armCollisionSet);
void setDescriptionPositionForObject(const std::string& objId);
void setNextStepDescription(const std::string& description);
@@ -318,5 +382,4 @@ namespace armarx
RobotNameHelperPtr rnh;
};
-}
-
+} // namespace armarx
diff --git a/source/RobotComponents/components/HumanObstacleDetection/HumanObstacleDetection.cpp b/source/RobotComponents/components/HumanObstacleDetection/HumanObstacleDetection.cpp
index bd1a4e9fb72318720dd2b6d9838e119059d823b8..c14b6981bfff8247bed447c3db23fac2fa28dca0 100644
--- a/source/RobotComponents/components/HumanObstacleDetection/HumanObstacleDetection.cpp
+++ b/source/RobotComponents/components/HumanObstacleDetection/HumanObstacleDetection.cpp
@@ -28,8 +28,8 @@
// STD/STL
#include <cmath>
#include <limits>
-#include <string>
#include <map>
+#include <string>
// Simox
#include <VirtualRobot/Nodes/RobotNode.h>
@@ -41,15 +41,14 @@
// ArmarX
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
-#include <RobotAPI/libraries/core/FramedPose.h>
#include <ArmarXCore/core/time/StopWatch.h>
+#include <RobotAPI/libraries/core/FramedPose.h>
+
using namespace armarx;
using namespace visionx;
-const std::string
-HumanObstacleDetection::default_name = "HumanObstacleDetection";
-
+const std::string HumanObstacleDetection::default_name = "HumanObstacleDetection";
HumanObstacleDetection::HumanObstacleDetection() noexcept :
m_enabled(true),
@@ -59,10 +58,8 @@ HumanObstacleDetection::HumanObstacleDetection() noexcept :
m_min_velocity_treshold(15),
m_keypoint_after(IceUtil::Time::milliSeconds(1500))
{
-
}
-
void
HumanObstacleDetection::onInitComponent()
{
@@ -71,7 +68,6 @@ HumanObstacleDetection::onInitComponent()
ARMARX_DEBUG << "Initialized " << getName() << ".";
}
-
void
HumanObstacleDetection::onConnectComponent()
{
@@ -79,14 +75,12 @@ HumanObstacleDetection::onConnectComponent()
const unsigned int periodic_task_interval = 100;
m_check_human_task = new PeriodicTask<HumanObstacleDetection>(
- this,
- &HumanObstacleDetection::checkHumanVisibility, periodic_task_interval);
+ this, &HumanObstacleDetection::checkHumanVisibility, periodic_task_interval);
m_check_human_task->start();
ARMARX_DEBUG << "Connected " << getName() << ".";
}
-
void
HumanObstacleDetection::onDisconnectComponent()
{
@@ -97,7 +91,6 @@ HumanObstacleDetection::onDisconnectComponent()
ARMARX_DEBUG << "Disconnected " << getName() << ".";
}
-
void
HumanObstacleDetection::onExitComponent()
{
@@ -107,10 +100,9 @@ HumanObstacleDetection::onExitComponent()
}
void
-HumanObstacleDetection::report3DKeypoints(
- const armarx::HumanPose3DMap& kpml,
- long timestamp,
- const Ice::Current&)
+HumanObstacleDetection::report3DKeypoints(const armarx::HumanPose3DMap& kpml,
+ long timestamp,
+ const Ice::Current&)
{
ARMARX_DEBUG << "Got human pose";
if (m_onlyUseFirstNResults == 0)
@@ -165,17 +157,20 @@ HumanObstacleDetection::report3DKeypoints(
sw.reset();
- m_obstacle_manager->directly_update_obstacle("human", Eigen::Vector2f(human.posX, human.posY), human.axisLengthX, human.axisLengthY, human.yaw);
+ m_obstacle_manager->directly_update_obstacle("human",
+ Eigen::Vector2f(human.posX, human.posY),
+ human.axisLengthX,
+ human.axisLengthY,
+ human.yaw);
ARMARX_DEBUG << "manager" << sw.stop();
- ARMARX_DEBUG << "Drawing Human Obsacle at position (" << human.posX << ", " << human.posY << "). Needed time: " << IceUtil::Time::now() - start;
+ ARMARX_DEBUG << "Drawing Human Obsacle at position (" << human.posX << ", " << human.posY
+ << "). Needed time: " << IceUtil::Time::now() - start;
}
-
HumanObstacleDetection::OpenPoseResult
-HumanObstacleDetection::find_closest_human(
- const std::vector<Keypoint3DMap>& kpml,
- const IceUtil::Time& timestamp)
+HumanObstacleDetection::find_closest_human(const std::vector<Keypoint3DMap>& kpml,
+ const IceUtil::Time& timestamp)
{
ARMARX_DEBUG << "Find closest human.";
OpenPoseResult closest_human;
@@ -197,7 +192,8 @@ HumanObstacleDetection::find_closest_human(
for (const auto& [key, value] : kpm)
{
- if (std::isfinite(value.x) and value.x != 0 and std::isfinite(value.y) and value.y != 0 and std::isfinite(value.z) and value.z != 0)
+ if (std::isfinite(value.x) and value.x != 0 and std::isfinite(value.y) and
+ value.y != 0 and std::isfinite(value.z) and value.z != 0)
{
sum_confidence += value.confidence;
sum_positions += Eigen::Vector3f(value.globalX, value.globalY, value.globalZ);
@@ -214,9 +210,11 @@ HumanObstacleDetection::find_closest_human(
const Eigen::Vector3f mean_position = sum_positions * (1.0 / amount);
const float mean_confidence = sum_confidence / amount;
const float mean_distance = sum_distance / amount;
- ARMARX_DEBUG << "Found a human at a distance of " << sum_distance << "mm with confidence " << mean_confidence << ".";
+ ARMARX_DEBUG << "Found a human at a distance of " << sum_distance << "mm with confidence "
+ << mean_confidence << ".";
- if (sum_distance < closest_human.distance and mean_confidence >= m_human_confidence_filter_value)
+ if (sum_distance < closest_human.distance and
+ mean_confidence >= m_human_confidence_filter_value)
{
closest_human.mean_position = mean_position;
closest_human.distance = mean_distance;
@@ -230,13 +228,13 @@ HumanObstacleDetection::find_closest_human(
return closest_human;
}
-
HumanObstacleDetection::HumanApproximation
HumanObstacleDetection::approximate_human(const OpenPoseResult& closest_human)
{
ARMARX_DEBUG << "Approximate human";
- std::map<std::string, std::pair<IceUtil::Time, Eigen::Vector2f>> transformed_points_with_timestamp;
+ std::map<std::string, std::pair<IceUtil::Time, Eigen::Vector2f>>
+ transformed_points_with_timestamp;
// Initialize transformed points with old ones.
if (m_pose_buffer_fillctr > 0)
@@ -260,12 +258,13 @@ HumanObstacleDetection::approximate_human(const OpenPoseResult& closest_human)
{
//ARMARX_DEBUG << "kpm[" << key << "] = (" << value.x << ", " << value.y << ", "
// << value.z << ")";
- if (std::isfinite(value.globalX) and value.globalX != 0 and std::isfinite(value.globalY) and value.globalY != 0
- and std::isfinite(value.globalZ) and value.globalZ != 0)
+ if (std::isfinite(value.globalX) and value.globalX != 0 and std::isfinite(value.globalY) and
+ value.globalY != 0 and std::isfinite(value.globalZ) and value.globalZ != 0)
{
Eigen::Vector3f pos{value.globalX, value.globalY, value.globalZ};
- transformed_points_with_timestamp[key] = std::make_pair(closest_human.timestamp, Eigen::Vector2f{pos(0), pos(1)});
+ transformed_points_with_timestamp[key] =
+ std::make_pair(closest_human.timestamp, Eigen::Vector2f{pos(0), pos(1)});
}
}
@@ -297,8 +296,8 @@ HumanObstacleDetection::approximate_human(const OpenPoseResult& closest_human)
max_distance = std::max(max_distance, (keypoint - mean).norm());
}
- ARMARX_DEBUG << "Found positions in current frame [X,Y,Distance]: ("
- << mean(0) << ", " << mean(1) << ", " << max_distance << ").";
+ ARMARX_DEBUG << "Found positions in current frame [X,Y,Distance]: (" << mean(0) << ", "
+ << mean(1) << ", " << max_distance << ").";
if ((last_human_approximation.valid and mean.isZero()) or max_distance == 0.0)
{
// Safety return to avoid 0,0 positions (although a human has been found).
@@ -397,7 +396,6 @@ HumanObstacleDetection::approximate_human(const OpenPoseResult& closest_human)
return mean_frame_human;
}
-
void
HumanObstacleDetection::checkHumanVisibility()
{
@@ -406,7 +404,8 @@ HumanObstacleDetection::checkHumanVisibility()
(IceUtil::Time::now() - last_human_approximation.timestamp) > m_keypoint_after;
if (last_human_approximation.valid and is_data_outdated)
{
- ARMARX_IMPORTANT << "Have not seen a human for " << m_keypoint_after << ". " << "Deleting obstacle.";
+ ARMARX_IMPORTANT << "Have not seen a human for " << m_keypoint_after << ". "
+ << "Deleting obstacle.";
HumanApproximation human;
last_human_approximation = human;
@@ -414,7 +413,6 @@ HumanObstacleDetection::checkHumanVisibility()
}
}
-
void
HumanObstacleDetection::setEnabled(bool enable, const Ice::Current&)
{
@@ -422,7 +420,6 @@ HumanObstacleDetection::setEnabled(bool enable, const Ice::Current&)
m_enabled = enable;
}
-
void
HumanObstacleDetection::enable(const Ice::Current&)
{
@@ -430,7 +427,6 @@ HumanObstacleDetection::enable(const Ice::Current&)
m_enabled = true;
}
-
void
HumanObstacleDetection::disable(const Ice::Current&)
{
@@ -438,15 +434,12 @@ HumanObstacleDetection::disable(const Ice::Current&)
m_enabled = false;
}
-
std::string
-HumanObstacleDetection::getDefaultName()
-const
+HumanObstacleDetection::getDefaultName() const
{
return default_name;
}
-
PropertyDefinitionsPtr
HumanObstacleDetection::createPropertyDefinitions()
{
@@ -454,17 +447,26 @@ HumanObstacleDetection::createPropertyDefinitions()
def->topic<armarx::OpenPose3DListener>("OpenPoseEstimation3D");
- def->component(m_obstacle_manager, "DynamicObstacleManager", "ObstacleManager", "The name of the obstacle manager proxy");
+ def->component(m_obstacle_manager,
+ "DynamicObstacleManager",
+ "ObstacleManager",
+ "The name of the obstacle manager proxy");
def->optional(m_enabled, "ActivateOnStartup", "Activate the component on startup.");
- def->optional(m_onlyUseFirstNResults, "OnlyUseFirstNResults", "Only use the first n results of OpenPose.");
- def->optional(m_keypoint_after, "ForgetOpenPoseKeypointAfter",
+ def->optional(m_onlyUseFirstNResults,
+ "OnlyUseFirstNResults",
+ "Only use the first n results of OpenPose.");
+ def->optional(m_keypoint_after,
+ "ForgetOpenPoseKeypointAfter",
"Forget the OpenPose keypoints after X ms if not visible anymore.");
- def->optional(m_min_velocity_treshold, "MinVelocityThreshold",
+ def->optional(m_min_velocity_treshold,
+ "MinVelocityThreshold",
"Velocities below this value will be considered zero.");
- def->optional(m_human_confidence_filter_value, "HumanConfidenceFilterValue",
+ def->optional(m_human_confidence_filter_value,
+ "HumanConfidenceFilterValue",
"Poses with a mean confidence below this value will be discarded.");
- def->optional(m_warn_distance, "WarnDistance",
- "Distance in [mm] at which a warning should be issued.").setMin(100);
+ def->optional(
+ m_warn_distance, "WarnDistance", "Distance in [mm] at which a warning should be issued.")
+ .setMin(100);
return def;
}
diff --git a/source/RobotComponents/components/HumanObstacleDetection/HumanObstacleDetection.h b/source/RobotComponents/components/HumanObstacleDetection/HumanObstacleDetection.h
index ee720b61dfb59e46df4db94d791bb6f3d2704160..0eae486c7b2c227552c63b96601fd4959b890117 100644
--- a/source/RobotComponents/components/HumanObstacleDetection/HumanObstacleDetection.h
+++ b/source/RobotComponents/components/HumanObstacleDetection/HumanObstacleDetection.h
@@ -57,7 +57,6 @@ namespace armarx
{
private:
-
struct OpenPoseResult
{
bool valid = false;
@@ -82,7 +81,6 @@ namespace armarx
};
public:
-
HumanObstacleDetection() noexcept;
std::string getDefaultName() const override;
@@ -90,12 +88,13 @@ namespace armarx
void setEnabled(bool enable, const Ice::Current& = Ice::emptyCurrent) override;
void enable(const Ice::Current& = Ice::emptyCurrent) override;
void disable(const Ice::Current& = Ice::emptyCurrent) override;
- void report3DKeypoints(const armarx::HumanPose3DMap& kpml, long timestamp, const Ice::Current& = Ice::emptyCurrent) override;
+ void report3DKeypoints(const armarx::HumanPose3DMap& kpml,
+ long timestamp,
+ const Ice::Current& = Ice::emptyCurrent) override;
void checkHumanVisibility();
protected:
-
void onInitComponent() override;
void onConnectComponent() override;
@@ -107,19 +106,15 @@ namespace armarx
armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
private:
+ OpenPoseResult find_closest_human(const std::vector<armarx::Keypoint3DMap>& kpml,
+ const IceUtil::Time& time);
- OpenPoseResult
- find_closest_human(const std::vector<armarx::Keypoint3DMap>& kpml, const IceUtil::Time& time);
-
- HumanApproximation
- approximate_human(const OpenPoseResult& human);
+ HumanApproximation approximate_human(const OpenPoseResult& human);
public:
-
static const std::string default_name;
private:
-
bool m_enabled;
int m_onlyUseFirstNResults;
@@ -142,7 +137,6 @@ namespace armarx
std::mutex m_human_mutex;
std::mutex m_enabled_mutex;
-
};
-}
+} // namespace armarx
diff --git a/source/RobotComponents/components/LaserScannerObstacleDetection/LaserScannerObstacleDetection.cpp b/source/RobotComponents/components/LaserScannerObstacleDetection/LaserScannerObstacleDetection.cpp
index 3a96f0f81607887f51b2ab9510c34595aeba37f6..cf369312aff107f5568f35c458cef666ff795faa 100644
--- a/source/RobotComponents/components/LaserScannerObstacleDetection/LaserScannerObstacleDetection.cpp
+++ b/source/RobotComponents/components/LaserScannerObstacleDetection/LaserScannerObstacleDetection.cpp
@@ -25,15 +25,14 @@
// ArmarX
#include <ArmarXCore/core/Component.h>
+
#include <RobotAPI/interface/core/PoseBase.h>
#include <RobotAPI/libraries/core/Pose.h>
using namespace armarx;
-const std::string
-LaserScannerObstacleDetection::default_name = "LaserScannerObstacleDetection";
-
+const std::string LaserScannerObstacleDetection::default_name = "LaserScannerObstacleDetection";
LaserScannerObstacleDetection::LaserScannerObstacleDetection() noexcept :
m_enabled(true),
@@ -43,7 +42,6 @@ LaserScannerObstacleDetection::LaserScannerObstacleDetection() noexcept :
m_periodic_task_interval(500),
m_accept_lines_after(100)
{
-
}
void
@@ -52,30 +50,30 @@ LaserScannerObstacleDetection::onInitComponent()
m_last_accepted_lines = IceUtil::Time::now();
}
-
void
LaserScannerObstacleDetection::onConnectComponent()
{
- m_eval_obstacles = new PeriodicTask<LaserScannerObstacleDetection>(this,
- &LaserScannerObstacleDetection::eval_obstacles, m_periodic_task_interval);
+ m_eval_obstacles = new PeriodicTask<LaserScannerObstacleDetection>(
+ this, &LaserScannerObstacleDetection::eval_obstacles, m_periodic_task_interval);
m_eval_obstacles->start();
}
-
void
LaserScannerObstacleDetection::onDisconnectComponent()
{
m_eval_obstacles->stop();
}
-
void
LaserScannerObstacleDetection::onExitComponent()
{
}
float
-LaserScannerObstacleDetection::get_distance_from_point_to_line_segment(const Eigen::Vector2f& start, const Eigen::Vector2f& end, const Eigen::Vector2f& p) const
+LaserScannerObstacleDetection::get_distance_from_point_to_line_segment(
+ const Eigen::Vector2f& start,
+ const Eigen::Vector2f& end,
+ const Eigen::Vector2f& p) const
{
// Return minimum distance between line segment l (start = v, end = w) and point p
const Eigen::Vector2f& v = start;
@@ -103,14 +101,15 @@ LaserScannerObstacleDetection::get_distance_from_point_to_line_segment(const Ei
return distance;
}
-
float
-LaserScannerObstacleDetection::get_angle_between_vectors(const Eigen::Vector2f& v1, const Eigen::Vector2f& v2) const
+LaserScannerObstacleDetection::get_angle_between_vectors(const Eigen::Vector2f& v1,
+ const Eigen::Vector2f& v2) const
{
// Returns an angle between 0 and 180 degree (the minimum angle between the two vectors)
const Eigen::Vector2f v1_vec_normed = v1.normalized();
const Eigen::Vector2f v2_vec_normed = v2.normalized();
- const float dot_product_vec = v1_vec_normed(0) * v2_vec_normed(0) + v1_vec_normed(1) * v2_vec_normed(1);
+ const float dot_product_vec =
+ v1_vec_normed(0) * v2_vec_normed(0) + v1_vec_normed(1) * v2_vec_normed(1);
float yaw = acos(dot_product_vec);
if (yaw >= M_PI)
{
@@ -119,8 +118,6 @@ LaserScannerObstacleDetection::get_angle_between_vectors(const Eigen::Vector2f&
return yaw;
}
-
-
void
LaserScannerObstacleDetection::eval_obstacles()
{
@@ -231,10 +228,11 @@ LaserScannerObstacleDetection::eval_obstacles()
lines.erase(lines.begin() + remove2);
lines.erase(lines.begin() + remove1);
}
- }
- while (update_found);
+ } while (update_found);
- ARMARX_DEBUG << "Finished updating the " << lines_before << " lines. Now we have " << lines.size() << " lines. The operation took " << (IceUtil::Time::now() - started).toMilliSeconds() << "ms";
+ ARMARX_DEBUG << "Finished updating the " << lines_before << " lines. Now we have "
+ << lines.size() << " lines. The operation took "
+ << (IceUtil::Time::now() - started).toMilliSeconds() << "ms";
// Try to explain line segments by the obstacles from the obstacle avoidance environment.
std::vector<dynamicobstaclemanager::LineSegment> converted_lines;
@@ -262,7 +260,6 @@ LaserScannerObstacleDetection::eval_obstacles()
ARMARX_DEBUG << "Finished update";
}
-
//void
//LaserScannerObstacleDetection::reportExtractedEdges(
// const std::vector<LineSegment2D>& lines,
@@ -293,7 +290,6 @@ LaserScannerObstacleDetection::setEnabled(bool enable, const Ice::Current&)
m_enabled = enable;
}
-
void
LaserScannerObstacleDetection::enable(const Ice::Current&)
{
@@ -301,7 +297,6 @@ LaserScannerObstacleDetection::enable(const Ice::Current&)
m_enabled = true;
}
-
void
LaserScannerObstacleDetection::disable(const Ice::Current&)
{
@@ -309,37 +304,49 @@ LaserScannerObstacleDetection::disable(const Ice::Current&)
m_enabled = false;
}
-
std::string
LaserScannerObstacleDetection::getDefaultName() const
{
return default_name;
}
-
PropertyDefinitionsPtr
LaserScannerObstacleDetection::createPropertyDefinitions()
{
PropertyDefinitionsPtr defs{new ComponentPropertyDefinitions{getConfigIdentifier()}};
- defs->component(m_obstacle_manager, "DynamicObstacleManager", "ObstacleManager", "The name of the obstacle manager proxy");
+ defs->component(m_obstacle_manager,
+ "DynamicObstacleManager",
+ "ObstacleManager",
+ "The name of the obstacle manager proxy");
defs->topic<LaserScannerFeaturesListener>();
- defs->optional(m_only_submit_first_n_results, "SubmitOnlyFirstNResults", "Only send the first laser scanner result. -1 for all.");
- defs->optional(m_max_distance_to_put_together, "MaxDistanceOfLinesToGroup", "Max distance in mm of two lines to put them together");
- defs->optional(m_max_yaw_difference, "MaxAngleDifferenceOfLinesToGroup", "Max angle difference in yaw of two lines to put them together");
- defs->optional(m_periodic_task_interval, "UpdateInterval", "The interval to check the obstacles");
- defs->optional(m_accept_lines_after, "AcceptLinesAfter", "Accept new Lines for buffer every x ms");
+ defs->optional(m_only_submit_first_n_results,
+ "SubmitOnlyFirstNResults",
+ "Only send the first laser scanner result. -1 for all.");
+ defs->optional(m_max_distance_to_put_together,
+ "MaxDistanceOfLinesToGroup",
+ "Max distance in mm of two lines to put them together");
+ defs->optional(m_max_yaw_difference,
+ "MaxAngleDifferenceOfLinesToGroup",
+ "Max angle difference in yaw of two lines to put them together");
+ defs->optional(
+ m_periodic_task_interval, "UpdateInterval", "The interval to check the obstacles");
+ defs->optional(
+ m_accept_lines_after, "AcceptLinesAfter", "Accept new Lines for buffer every x ms");
defs->optional(m_enabled, "ActivateOnStartup", "Activate the component on startup.");
return defs;
}
-void LaserScannerObstacleDetection::reportExtractedLineSegments(const laser_scanner_feature_extraction::LineSegment2DChainSeq& globalLineSegmentChains, const Ice::Current&)
+void
+LaserScannerObstacleDetection::reportExtractedLineSegments(
+ const laser_scanner_feature_extraction::LineSegment2DChainSeq& globalLineSegmentChains,
+ const Ice::Current&)
{
{
std::lock_guard l{m_enabled_mutex};
- if (! m_enabled or globalLineSegmentChains.empty())
+ if (!m_enabled or globalLineSegmentChains.empty())
{
return;
}
@@ -350,10 +357,11 @@ void LaserScannerObstacleDetection::reportExtractedLineSegments(const laser_scan
std::unique_lock l{m_lines_buffer_mutex};
- ARMARX_DEBUG << "Got " << globalLineSegmentChains.size() << " new extracted edges to buffer.";
+ ARMARX_DEBUG << "Got " << globalLineSegmentChains.size()
+ << " new extracted edges to buffer.";
- m_lines_buffer.insert(m_lines_buffer.end(), globalLineSegmentChains.begin(), globalLineSegmentChains.end());
+ m_lines_buffer.insert(
+ m_lines_buffer.end(), globalLineSegmentChains.begin(), globalLineSegmentChains.end());
m_last_accepted_lines = IceUtil::Time::now();
}
}
-
diff --git a/source/RobotComponents/components/LaserScannerObstacleDetection/LaserScannerObstacleDetection.h b/source/RobotComponents/components/LaserScannerObstacleDetection/LaserScannerObstacleDetection.h
index 880d63aaaecd6e82e319fdf10c944087fb3a78f9..52cd2e1de0141d9e30436deaacb09192aaba372e 100644
--- a/source/RobotComponents/components/LaserScannerObstacleDetection/LaserScannerObstacleDetection.h
+++ b/source/RobotComponents/components/LaserScannerObstacleDetection/LaserScannerObstacleDetection.h
@@ -25,8 +25,8 @@
// STD/STL
-#include <string>
#include <mutex>
+#include <string>
// Eigen
#include <Eigen/Core>
@@ -44,7 +44,6 @@
// ObstacleAvoidance
#include <RobotAPI/interface/components/ObstacleAvoidance/DynamicObstacleManagerInterface.h>
-
namespace armarx
{
@@ -54,11 +53,9 @@ namespace armarx
{
public:
-
static const std::string default_name;
public:
-
LaserScannerObstacleDetection() noexcept;
std::string getDefaultName() const override;
@@ -71,10 +68,11 @@ namespace armarx
// void reportLaserScanPoints(const Vector2fSeq&, const Ice::Current& = Ice::Current()) override { /* nop */ };
// void reportExtractedEdges(const LineSegment2DSeq&, const Ice::Current& = Ice::Current()) override;
- void reportExtractedLineSegments(const laser_scanner_feature_extraction::LineSegment2DChainSeq& globalLineSegmentChains, const Ice::Current& = Ice::Current()) override;
+ void reportExtractedLineSegments(
+ const laser_scanner_feature_extraction::LineSegment2DChainSeq& globalLineSegmentChains,
+ const Ice::Current& = Ice::Current()) override;
protected:
-
void onInitComponent() override;
void onConnectComponent() override;
void onDisconnectComponent() override;
@@ -84,7 +82,9 @@ namespace armarx
private:
void eval_obstacles();
- float get_distance_from_point_to_line_segment(const Eigen::Vector2f& start, const Eigen::Vector2f& end, const Eigen::Vector2f& p) const;
+ float get_distance_from_point_to_line_segment(const Eigen::Vector2f& start,
+ const Eigen::Vector2f& end,
+ const Eigen::Vector2f& p) const;
float get_angle_between_vectors(const Eigen::Vector2f&, const Eigen::Vector2f&) const;
bool m_enabled;
@@ -105,4 +105,4 @@ namespace armarx
DynamicObstacleManagerInterface::ProxyType m_obstacle_manager;
};
-}
+} // namespace armarx
diff --git a/source/RobotComponents/components/LaserScannerSelfLocalisation/LaserScannerSelfLocalisation.cpp b/source/RobotComponents/components/LaserScannerSelfLocalisation/LaserScannerSelfLocalisation.cpp
index 1357a50500592868c3b3cdc884dd12d3a0412a1a..7c786206d75444c9a1a89a8f638f2a25c45c09a9 100644
--- a/source/RobotComponents/components/LaserScannerSelfLocalisation/LaserScannerSelfLocalisation.cpp
+++ b/source/RobotComponents/components/LaserScannerSelfLocalisation/LaserScannerSelfLocalisation.cpp
@@ -22,28 +22,29 @@
#include "LaserScannerSelfLocalisation.h"
-#include <RobotAPI/libraries/core/FramedPose.h>
-#include <RobotAPI/libraries/core/math/MathUtils.h>
-
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/time/TimeUtil.h>
-#include <ArmarXCore/util/json/JSONObject.h>
#include <ArmarXCore/core/util/IceReportSkipper.h>
+#include <ArmarXCore/util/json/JSONObject.h>
+
+#include <RobotAPI/libraries/core/FramedPose.h>
+#include <RobotAPI/libraries/core/math/MathUtils.h>
// These object factories are required, otherwise a runtime error will occur (static global object registration...)
-#include <MemoryX/core/MemoryXCoreObjectFactories.h>
-#include <MemoryX/libraries/helpers/EarlyVisionHelpers/Gaussian.h>
-#include <MemoryX/libraries/memorytypes/MemoryXTypesObjectFactories.h>
+#include <cfloat>
+#include <chrono>
+#include <fstream>
+#include <iomanip>
-#include <SimoxUtility/json/json.hpp>
+#include <Eigen/Geometry>
#include <IceUtil/UUID.h>
-#include <Eigen/Geometry>
-#include <fstream>
-#include <iomanip>
-#include <chrono>
-#include <cfloat>
+#include <SimoxUtility/json/json.hpp>
+
+#include <MemoryX/core/MemoryXCoreObjectFactories.h>
+#include <MemoryX/libraries/helpers/EarlyVisionHelpers/Gaussian.h>
+#include <MemoryX/libraries/memorytypes/MemoryXTypesObjectFactories.h>
using namespace armarx;
@@ -52,20 +53,20 @@ using Line = Eigen::ParametrizedLine<float, 2>;
namespace
{
- bool anyIsNaN(Eigen::Vector2f const& v)
+ bool
+ anyIsNaN(Eigen::Vector2f const& v)
{
- return std::isnan(v.x())
- || std::isnan(v.y());
+ return std::isnan(v.x()) || std::isnan(v.y());
}
- bool anyIsNaN(Eigen::Vector3f const& v)
+ bool
+ anyIsNaN(Eigen::Vector3f const& v)
{
- return std::isnan(v.x())
- || std::isnan(v.y())
- || std::isnan(v.z());
+ return std::isnan(v.x()) || std::isnan(v.y()) || std::isnan(v.z());
}
- std::string readWholeFile(std::string const& filename)
+ std::string
+ readWholeFile(std::string const& filename)
{
std::ifstream t(filename.c_str());
std::string str;
@@ -74,12 +75,12 @@ namespace
str.reserve(t.tellg());
t.seekg(0, std::ios::beg);
- str.assign(std::istreambuf_iterator<char>(t),
- std::istreambuf_iterator<char>());
+ str.assign(std::istreambuf_iterator<char>(t), std::istreambuf_iterator<char>());
return str;
}
- Eigen::Vector2f parsePosition(Json::Value const& pos)
+ Eigen::Vector2f
+ parsePosition(Json::Value const& pos)
{
Eigen::Vector2f result = Eigen::Vector2f::Zero();
if (pos.isArray() && pos.size() == 2)
@@ -98,12 +99,14 @@ namespace
}
else
{
- throw std::runtime_error("Map format error: Unexpected number of elements in 2D vector");
+ throw std::runtime_error(
+ "Map format error: Unexpected number of elements in 2D vector");
}
return result;
}
- std::vector<LineSegment2Df> loadMapFromFile(std::string const& filename)
+ std::vector<LineSegment2Df>
+ loadMapFromFile(std::string const& filename)
{
std::string fullFilename;
if (!ArmarXDataPath::getAbsolutePath(filename, fullFilename))
@@ -155,7 +158,8 @@ namespace
Json::Value const& strip = p["RelativeLineStrip"];
if (!strip.isArray())
{
- throw std::runtime_error("Map format error: Expected an error at property 'RelativeLineStrip'");
+ throw std::runtime_error(
+ "Map format error: Expected an error at property 'RelativeLineStrip'");
}
Eigen::Vector2f startPos = pos;
@@ -163,7 +167,7 @@ namespace
{
Eigen::Vector2f nextPos = startPos + parsePosition(strip[j]);
- result.push_back(LineSegment2Df {lengthUnit * startPos, lengthUnit * nextPos});
+ result.push_back(LineSegment2Df{lengthUnit * startPos, lengthUnit * nextPos});
startPos = nextPos;
}
}
@@ -171,7 +175,8 @@ namespace
return result;
}
- float lineSegmentToPointDistanceSquared(LineSegment2Df const& segment, Eigen::Vector2f point)
+ float
+ lineSegmentToPointDistanceSquared(LineSegment2Df const& segment, Eigen::Vector2f point)
{
// Return minimum distance between line segment and a point
Eigen::Vector2f dir = segment.end - segment.start;
@@ -186,11 +191,12 @@ namespace
// It falls where t = [(p-v) . (w-v)] / |w-v|^2
// We clamp t from [0,1] to handle points outside the segment vw.
float t = std::max(0.0f, std::min(1.0f, (point - segment.start).dot(dir) / l2));
- Eigen::Vector2f projection = segment.start + t * dir; // Projection falls on the segment
+ Eigen::Vector2f projection = segment.start + t * dir; // Projection falls on the segment
return (point - projection).squaredNorm();
}
- float distance(Line const& line, Eigen::Vector2f point)
+ float
+ distance(Line const& line, Eigen::Vector2f point)
{
Eigen::Vector2f normalizedDir = line.direction();
Eigen::Vector2f diff = point - line.origin();
@@ -198,12 +204,14 @@ namespace
return std::fabs(signedDistance);
}
- Line leastSquareLine(Eigen::Vector2f* begin, Eigen::Vector2f* end,
- float* maxError = nullptr,
- float* averageError = nullptr,
- float* as = nullptr,
- float* bs = nullptr,
- float* det = nullptr)
+ Line
+ leastSquareLine(Eigen::Vector2f* begin,
+ Eigen::Vector2f* end,
+ float* maxError = nullptr,
+ float* averageError = nullptr,
+ float* as = nullptr,
+ float* bs = nullptr,
+ float* det = nullptr)
{
float sumx = 0.0f;
float sumy = 0.0f;
@@ -233,16 +241,13 @@ namespace
zeroLine.direction() = Eigen::Vector2f::UnitX();
if (std::abs(calc_as) < 0.001f && std::abs(calc_bs) < 0.001f)
{
- ARMARX_VERBOSE << "Vector is close to zero "
- << VAROUT(calc_as)
- << " " << VAROUT(calc_bs);
+ ARMARX_VERBOSE << "Vector is close to zero " << VAROUT(calc_as) << " "
+ << VAROUT(calc_bs);
return zeroLine;
}
if (numx == 0.0f)
{
- ARMARX_VERBOSE << "numx is zero "
- << VAROUT(begin)
- << " " << VAROUT(end);
+ ARMARX_VERBOSE << "numx is zero " << VAROUT(begin) << " " << VAROUT(end);
return zeroLine;
}
@@ -296,7 +301,11 @@ namespace
return line;
}
- LineSegment2Df leastSquareLineSegmentInOrder(Eigen::Vector2f* begin, Eigen::Vector2f* end, float* maxError, float* averageError)
+ LineSegment2Df
+ leastSquareLineSegmentInOrder(Eigen::Vector2f* begin,
+ Eigen::Vector2f* end,
+ float* maxError,
+ float* averageError)
{
LineSegment2Df segment;
if ((end - begin) < 2)
@@ -335,36 +344,35 @@ namespace
if (anyIsNaN(segment.start))
{
ARMARX_WARNING << "Segment start is NaN: " << VAROUT(segment.start)
- << VAROUT(line.origin())
- << VAROUT(line.direction());
+ << VAROUT(line.origin()) << VAROUT(line.direction());
}
if (anyIsNaN(segment.end))
{
- ARMARX_WARNING << "Segment end is NaN: " << VAROUT(segment.end)
- << VAROUT(line.origin())
+ ARMARX_WARNING << "Segment end is NaN: " << VAROUT(segment.end) << VAROUT(line.origin())
<< VAROUT(line.direction());
}
return segment;
}
- float leastSquareEdge(Eigen::Vector2f* begin, Eigen::Vector2f* end, LineSegment2Df& segment)
+ float
+ leastSquareEdge(Eigen::Vector2f* begin, Eigen::Vector2f* end, LineSegment2Df& segment)
{
- float maxError {0};
- float averageError {0};
+ float maxError{0};
+ float averageError{0};
segment = leastSquareLineSegmentInOrder(begin, end, &maxError, &averageError);
return averageError;
}
-}
-
+} // namespace
-LaserScannerSelfLocalisation::LaserScannerSelfLocalisation()
- : s(0.1) // This frequency will not be used
+LaserScannerSelfLocalisation::LaserScannerSelfLocalisation() :
+ s(0.1) // This frequency will not be used
{
}
-void LaserScannerSelfLocalisation::onInitComponent()
+void
+LaserScannerSelfLocalisation::onInitComponent()
{
reportTopicName = getProperty<std::string>("ReportTopicName");
robotStateComponentName = getProperty<std::string>("RobotStateComponentName");
@@ -372,7 +380,7 @@ void LaserScannerSelfLocalisation::onInitComponent()
laserScannerUnitName = getProperty<std::string>("LaserScannerUnitName");
workingMemoryName = getProperty<std::string>("WorkingMemoryName");
longtermMemoryName = getProperty<std::string>("LongtermMemoryName");
- mapFilename = getProperty<std::string>("MapFilename");
+ mapFilename = getProperty<std::string>("MapFilename");
agentName = getProperty<std::string>("AgentName");
emergencyStopMasterName = getProperty<std::string>("EmergencyStopMasterName");
debugObserverName = getProperty<std::string>("DebugObserverName");
@@ -393,7 +401,8 @@ void LaserScannerSelfLocalisation::onInitComponent()
const auto max = getPropertyAsCSV<float>("PlatformRectangleMax");
std::tie(platformRectMin(0), platformRectMax(0)) = std::minmax(min.at(0), max.at(0));
std::tie(platformRectMin(1), platformRectMax(1)) = std::minmax(min.at(1), max.at(1));
- ARMARX_INFO << "using platform rectangle min / max " << platformRectMin.transpose() << " / " << platformRectMax.transpose();
+ ARMARX_INFO << "using platform rectangle min / max " << platformRectMin.transpose() << " / "
+ << platformRectMax.transpose();
}
@@ -403,8 +412,8 @@ void LaserScannerSelfLocalisation::onInitComponent()
usingProxy(longtermMemoryName);
}
-
-void LaserScannerSelfLocalisation::onConnectComponent()
+void
+LaserScannerSelfLocalisation::onConnectComponent()
{
robotState = getProxy<RobotStateComponentInterfacePrx>(robotStateComponentName);
laserScannerUnit = getProxy<LaserScannerUnitInterfacePrx>(laserScannerUnitName);
@@ -454,7 +463,8 @@ void LaserScannerSelfLocalisation::onConnectComponent()
if (poseAttribute)
{
// So this is how you access a typed attribute, fancy
- FramedPosePtr framedPose = armarx::VariantPtr::dynamicCast(poseAttribute->getValueAt(0))->getClass<armarx::FramedPose>();
+ FramedPosePtr framedPose = armarx::VariantPtr::dynamicCast(poseAttribute->getValueAt(0))
+ ->getClass<armarx::FramedPose>();
if (framedPose)
{
Eigen::Matrix4f globalPose = framedPose->toGlobalEigen(sharedRobot);
@@ -494,7 +504,8 @@ void LaserScannerSelfLocalisation::onConnectComponent()
poseEntityId = selfLocalisationSegment->addEntity(poseEntity);
ARMARX_INFO << "New entity ID: " << poseEntityId;
}
- ARMARX_INFO << "Initial pose: (" << estimatedPose[0] << ", " << estimatedPose[1] << ", " << estimatedPose[2] << ")";
+ ARMARX_INFO << "Initial pose: (" << estimatedPose[0] << ", " << estimatedPose[1] << ", "
+ << estimatedPose[2] << ")";
resetKalmanFilter(estimatedPose);
lastVelocityUpdate = TimeUtil::GetTime();
lastVelocity = Eigen::Vector3f::Zero();
@@ -504,12 +515,16 @@ void LaserScannerSelfLocalisation::onConnectComponent()
int updatePeriod = getProperty<int>("UpdatePeriodInMs");
- task = new PeriodicTask<LaserScannerSelfLocalisation>(this, &LaserScannerSelfLocalisation::updateLocalisation, updatePeriod, "UpdateLocalisation");
+ task = new PeriodicTask<LaserScannerSelfLocalisation>(
+ this,
+ &LaserScannerSelfLocalisation::updateLocalisation,
+ updatePeriod,
+ "UpdateLocalisation");
task->start();
}
-
-void LaserScannerSelfLocalisation::onDisconnectComponent()
+void
+LaserScannerSelfLocalisation::onDisconnectComponent()
{
task->stop();
ARMARX_INFO << "Task stopped";
@@ -519,38 +534,47 @@ void LaserScannerSelfLocalisation::onDisconnectComponent()
}
}
-
-void LaserScannerSelfLocalisation::onExitComponent()
+void
+LaserScannerSelfLocalisation::onExitComponent()
{
-
}
-armarx::PropertyDefinitionsPtr LaserScannerSelfLocalisation::createPropertyDefinitions()
+armarx::PropertyDefinitionsPtr
+LaserScannerSelfLocalisation::createPropertyDefinitions()
{
- armarx::PropertyDefinitionsPtr prop = new LaserScannerSelfLocalisationPropertyDefinitions(
- getConfigIdentifier());
+ armarx::PropertyDefinitionsPtr prop =
+ new LaserScannerSelfLocalisationPropertyDefinitions(getConfigIdentifier());
prop->topic(globalRobotPoseTopic);
return prop;
}
-std::string LaserScannerSelfLocalisation::getReportTopicName(const Ice::Current&)
+std::string
+LaserScannerSelfLocalisation::getReportTopicName(const Ice::Current&)
{
return reportTopicName;
}
-void LaserScannerSelfLocalisation::setAbsolutePose(Ice::Float x, Ice::Float y, Ice::Float theta, const Ice::Current&)
+void
+LaserScannerSelfLocalisation::setAbsolutePose(Ice::Float x,
+ Ice::Float y,
+ Ice::Float theta,
+ const Ice::Current&)
{
{
std::unique_lock lock(setPoseMutex);
setPose = Eigen::Vector3f(x, y, theta);
resetKalmanFilter(*setPose);
-
}
}
-void LaserScannerSelfLocalisation::reportSensorValues(const std::string& device, const std::string& name, const LaserScan& scan, const TimestampBasePtr& timestamp, const Ice::Current&)
+void
+LaserScannerSelfLocalisation::reportSensorValues(const std::string& device,
+ const std::string& name,
+ const LaserScan& scan,
+ const TimestampBasePtr& timestamp,
+ const Ice::Current&)
{
for (LaserScanData& data : scanData)
{
@@ -563,8 +587,8 @@ void LaserScannerSelfLocalisation::reportSensorValues(const std::string& device,
}
}
-
-static Eigen::Vector3f addPose(Eigen::Vector3f pose, Eigen::Vector3f add)
+static Eigen::Vector3f
+addPose(Eigen::Vector3f pose, Eigen::Vector3f add)
{
Eigen::Vector3f result = pose + add;
// Make sure that theta is in the range [-PI,PI]
@@ -572,7 +596,8 @@ static Eigen::Vector3f addPose(Eigen::Vector3f pose, Eigen::Vector3f add)
return result;
}
-static Eigen::Vector3f integratePose(Eigen::Vector3f pose, Eigen::Vector3f v0, Eigen::Vector3f v1, float dt)
+static Eigen::Vector3f
+integratePose(Eigen::Vector3f pose, Eigen::Vector3f v0, Eigen::Vector3f v1, float dt)
{
// Assumption: Linear acceleration between v0 and v1 in dt seconds
// Travelled distance d = v0*dt + 0.5*(v1-v0)*dt = 0.5*dt*(v0+v1)
@@ -589,7 +614,11 @@ static Eigen::Vector3f integratePose(Eigen::Vector3f pose, Eigen::Vector3f v0, E
return addPose(pose, d);
}
-void LaserScannerSelfLocalisation::reportPlatformVelocity(Ice::Float velX, Ice::Float velY, Ice::Float velTheta, const Ice::Current&)
+void
+LaserScannerSelfLocalisation::reportPlatformVelocity(Ice::Float velX,
+ Ice::Float velY,
+ Ice::Float velTheta,
+ const Ice::Current&)
{
IceUtil::Time now = TimeUtil::GetTime();
auto elapsedSeconds = now - lastVelocityUpdate;
@@ -604,20 +633,22 @@ void LaserScannerSelfLocalisation::reportPlatformVelocity(Ice::Float velX, Ice::
std::unique_lock lock(odometryMutex);
- reportedVelocities.push_back(ReportedVelocity {static_cast<float>(elapsedSeconds.toSecondsDouble()), velX, velY, velTheta});
-
+ reportedVelocities.push_back(ReportedVelocity{
+ static_cast<float>(elapsedSeconds.toSecondsDouble()), velX, velY, velTheta});
}
-
-
}
-
-void LaserScannerSelfLocalisation::reportPlatformOdometryPose(Ice::Float, Ice::Float, Ice::Float, const Ice::Current&)
+void
+LaserScannerSelfLocalisation::reportPlatformOdometryPose(Ice::Float,
+ Ice::Float,
+ Ice::Float,
+ const Ice::Current&)
{
// nop
}
-void LaserScannerSelfLocalisation::updateLocalisation()
+void
+LaserScannerSelfLocalisation::updateLocalisation()
{
// Periodic task to update the self localisation
@@ -647,14 +678,13 @@ void LaserScannerSelfLocalisation::updateLocalisation()
for (ReportedVelocity const& report : reportedVelocities)
{
Eigen::Vector3f currentVelocity(report.x, report.y, report.theta);
- Eigen::Vector3f motionPrediction = integratePose(currentPose, lastVelocity, currentVelocity, report.dt);
+ Eigen::Vector3f motionPrediction =
+ integratePose(currentPose, lastVelocity, currentVelocity, report.dt);
if (anyIsNaN(motionPrediction))
{
ARMARX_WARNING << "Motion prediction is NaN: " << motionPrediction;
- ARMARX_WARNING << VAROUT(currentPose)
- << " " << VAROUT(lastVelocity)
- << " " << VAROUT(currentVelocity)
- << " " << VAROUT(report.dt);
+ ARMARX_WARNING << VAROUT(currentPose) << " " << VAROUT(lastVelocity) << " "
+ << VAROUT(currentVelocity) << " " << VAROUT(report.dt);
}
else
{
@@ -691,23 +721,28 @@ void LaserScannerSelfLocalisation::updateLocalisation()
double deltaMeasurement = (now - data.measurementTime).toSecondsDouble();
if (deltaMeasurement < -0.005)
{
- ARMARX_WARNING << deactivateSpam(1) << "Delta between now and measurement time is negative: " << deltaMeasurement
- << " now: " << now.toDateTime() << " measurement: " << data.measurementTime.toDateTime();
+ ARMARX_WARNING << deactivateSpam(1)
+ << "Delta between now and measurement time is negative: "
+ << deltaMeasurement << " now: " << now.toDateTime()
+ << " measurement: " << data.measurementTime.toDateTime();
}
else if (deltaMeasurement > maximalLaserScannerDelay)
{
- ARMARX_WARNING << deactivateSpam(1) << "Delta between now and measurement time has exceeded the limit ("
+ ARMARX_WARNING << deactivateSpam(1)
+ << "Delta between now and measurement time has exceeded the limit ("
<< deltaMeasurement << "s > " << maximalLaserScannerDelay
<< "s). Activating emergency stop!";
- EmergencyStopMasterInterfacePrx emergencyStopMaster = getProxy<EmergencyStopMasterInterfacePrx>(emergencyStopMasterName);
+ EmergencyStopMasterInterfacePrx emergencyStopMaster =
+ getProxy<EmergencyStopMasterInterfacePrx>(emergencyStopMasterName);
if (emergencyStopMaster)
{
emergencyStopMaster->setEmergencyStopState(eEmergencyStopActive);
}
else
{
- ARMARX_ERROR << "Could not activate emergency stop because proxy for EmergencyStopMaster was not found '"
+ ARMARX_ERROR << "Could not activate emergency stop because proxy for "
+ "EmergencyStopMaster was not found '"
<< emergencyStopMasterName << "'";
}
}
@@ -730,7 +765,8 @@ void LaserScannerSelfLocalisation::updateLocalisation()
float distanceSum = 0;
int distanceElements = 0;
int minFrameIndex = std::max(0, stepIndex - halfFrameSize);
- int maxFrameIndex = std::min((int)(data.scan.size()) - 1, stepIndex + halfFrameSize);
+ int maxFrameIndex =
+ std::min((int)(data.scan.size()) - 1, stepIndex + halfFrameSize);
for (int frameIndex = minFrameIndex; frameIndex <= maxFrameIndex; ++frameIndex)
{
float frameDistance = data.scan[frameIndex].distance;
@@ -743,10 +779,10 @@ void LaserScannerSelfLocalisation::updateLocalisation()
}
if (distanceElements == 0)
{
- ARMARX_WARNING << "No elements with distance smaller than merge distance found.";
- ARMARX_WARNING << VAROUT(stepIndex)
- << " " << VAROUT(step.distance)
- << " " << VAROUT(mergeDistance);
+ ARMARX_WARNING
+ << "No elements with distance smaller than merge distance found.";
+ ARMARX_WARNING << VAROUT(stepIndex) << " " << VAROUT(step.distance) << " "
+ << VAROUT(mergeDistance);
}
else
{
@@ -758,24 +794,23 @@ void LaserScannerSelfLocalisation::updateLocalisation()
float x = -distance * sinAngle;
float y = distance * cosAngle;
- Eigen::Vector2f localPoint = (data.pose * Eigen::Vector4f(x, y, 0, 1)).head<2>();
+ Eigen::Vector2f localPoint =
+ (data.pose * Eigen::Vector4f(x, y, 0, 1)).head<2>();
if (anyIsNaN(localPoint))
{
ARMARX_WARNING << "Local point is NaN";
- ARMARX_WARNING << VAROUT(localPoint)
- << " " << VAROUT(x)
- << " " << VAROUT(y)
+ ARMARX_WARNING << VAROUT(localPoint) << " " << VAROUT(x) << " " << VAROUT(y)
<< " " << VAROUT(step.angle);
continue;
}
- if (
- platformRectMin(0) < localPoint(0) && localPoint(0) < platformRectMax(0) &&
- platformRectMin(1) < localPoint(1) && localPoint(1) < platformRectMax(1)
- )
+ if (platformRectMin(0) < localPoint(0) && localPoint(0) < platformRectMax(0) &&
+ platformRectMin(1) < localPoint(1) && localPoint(1) < platformRectMax(1))
{
- ARMARX_INFO << deactivateSpam() << "discarding point inside of the platform rectangle: " << localPoint.transpose();
+ ARMARX_INFO << deactivateSpam()
+ << "discarding point inside of the platform rectangle: "
+ << localPoint.transpose();
continue;
}
@@ -784,10 +819,8 @@ void LaserScannerSelfLocalisation::updateLocalisation()
if (anyIsNaN(globalPoint))
{
ARMARX_WARNING << "Global point is NaN";
- ARMARX_WARNING << VAROUT(globalPoint)
- << " " << VAROUT(x)
- << " " << VAROUT(y)
- << " " << VAROUT(step.angle);
+ ARMARX_WARNING << VAROUT(globalPoint) << " " << VAROUT(x) << " "
+ << VAROUT(y) << " " << VAROUT(step.angle);
}
else
{
@@ -795,7 +828,8 @@ void LaserScannerSelfLocalisation::updateLocalisation()
}
}
}
- ARMARX_DEBUG << deactivateSpam() << "using " << data.points.size() << "/" << data.scan.size();
+ ARMARX_DEBUG << deactivateSpam() << "using " << data.points.size() << "/"
+ << data.scan.size();
}
if (reportPoints)
{
@@ -805,7 +839,7 @@ void LaserScannerSelfLocalisation::updateLocalisation()
{
for (auto& p : data.points)
{
- pointsA.push_back(armarx::Vector2f {p.x(), p.y()});
+ pointsA.push_back(armarx::Vector2f{p.x(), p.y()});
}
}
if (reportTopic)
@@ -837,9 +871,8 @@ void LaserScannerSelfLocalisation::updateLocalisation()
float angle = std::atan2(line.direction().x(), line.direction().y());
if (std::isnan(angle))
{
- ARMARX_VERBOSE << "Edge angle is NaN"
- << VAROUT(beginIndex)
- << " " << VAROUT(endIndex);
+ ARMARX_VERBOSE << "Edge angle is NaN" << VAROUT(beginIndex) << " "
+ << VAROUT(endIndex);
}
else
{
@@ -865,7 +898,8 @@ void LaserScannerSelfLocalisation::updateLocalisation()
// we add them to the edge
while (endIndex < maxEndIndex &&
std::fabs(yaw - angles[endIndex + 1]) < edgeMaxDeltaAngle &&
- (points[endIndex + 1] - points[endIndex]).squaredNorm() < maxDistanceForEdgeSquared)
+ (points[endIndex + 1] - points[endIndex]).squaredNorm() <
+ maxDistanceForEdgeSquared)
{
endIndex++;
}
@@ -879,8 +913,11 @@ void LaserScannerSelfLocalisation::updateLocalisation()
// Extend the front
while (beginIndex > minBeginIndex &&
- (points[beginIndex - 1] - points[beginIndex]).squaredNorm() < maxDistanceForEdgeSquared &&
- leastSquareEdge(points.data() + beginIndex - 1, points.data() + endIndex + 1, edgeLineSegment[resultIndex]) < pointAddingThreshold)
+ (points[beginIndex - 1] - points[beginIndex]).squaredNorm() <
+ maxDistanceForEdgeSquared &&
+ leastSquareEdge(points.data() + beginIndex - 1,
+ points.data() + endIndex + 1,
+ edgeLineSegment[resultIndex]) < pointAddingThreshold)
{
beginIndex--;
resultIndex = !resultIndex;
@@ -889,8 +926,11 @@ void LaserScannerSelfLocalisation::updateLocalisation()
// Extend the back
while (endIndex < maxEndIndex &&
- (points[endIndex] - points[endIndex + 1]).squaredNorm() < maxDistanceForEdgeSquared &&
- leastSquareEdge(points.data() + beginIndex, points.data() + endIndex + 1, edgeLineSegment[resultIndex]) < pointAddingThreshold)
+ (points[endIndex] - points[endIndex + 1]).squaredNorm() <
+ maxDistanceForEdgeSquared &&
+ leastSquareEdge(points.data() + beginIndex,
+ points.data() + endIndex + 1,
+ edgeLineSegment[resultIndex]) < pointAddingThreshold)
{
endIndex++;
resultIndex = !resultIndex;
@@ -899,7 +939,9 @@ void LaserScannerSelfLocalisation::updateLocalisation()
if (!frontExtended && !backExtended)
{
- leastSquareEdge(points.data() + beginIndex, points.data() + endIndex + 1, edgeLineSegment[!resultIndex]);
+ leastSquareEdge(points.data() + beginIndex,
+ points.data() + endIndex + 1,
+ edgeLineSegment[!resultIndex]);
}
// Check for minimum number of points in the edge
@@ -910,7 +952,7 @@ void LaserScannerSelfLocalisation::updateLocalisation()
// Index = resultIndex: contains the line segment which exceeded the error limit
LineSegment2Df& segment = edgeLineSegment[!resultIndex];
Eigen::Vector2f* pointsBegin = points.data() + beginIndex;
- ExtractedEdge edge {segment, pointsBegin, pointsBegin + numberOfPoints};
+ ExtractedEdge edge{segment, pointsBegin, pointsBegin + numberOfPoints};
data.edges.push_back(edge);
++edgeCount;
maximumPointsInEdge = std::max(numberOfPoints, maximumPointsInEdge);
@@ -932,8 +974,8 @@ void LaserScannerSelfLocalisation::updateLocalisation()
for (ExtractedEdge const& e : data.edges)
{
armarx::LineSegment2D s;
- s.start = armarx::Vector2f {e.segment.start.x(), e.segment.start.y()};
- s.end = armarx::Vector2f {e.segment.end.x(), e.segment.end.y()};
+ s.start = armarx::Vector2f{e.segment.start.x(), e.segment.start.y()};
+ s.end = armarx::Vector2f{e.segment.end.x(), e.segment.end.y()};
segments.push_back(s);
}
}
@@ -957,7 +999,8 @@ void LaserScannerSelfLocalisation::updateLocalisation()
float minDistanceSquared = FLT_MAX;
for (LineSegment2Df& segment : map)
{
- float distanceSquared = lineSegmentToPointDistanceSquared(segment, globalPoint);
+ float distanceSquared =
+ lineSegmentToPointDistanceSquared(segment, globalPoint);
if (distanceSquared < minDistanceSquared)
{
minDistanceSquared = distanceSquared;
@@ -997,25 +1040,29 @@ void LaserScannerSelfLocalisation::updateLocalisation()
debugObserver->setDebugDatafield(getName(), "MatchedDataPointRatio", new Variant(matched));
if (matched < minPointsForMatch)
{
- ARMARX_WARNING << deactivateSpam(5) << "Too few data points could be associated with edges of the map (associated: "
- << rowCount << "/" << pointsSize << " = " << matched << ")";
+ ARMARX_WARNING
+ << deactivateSpam(5)
+ << "Too few data points could be associated with edges of the map (associated: "
+ << rowCount << "/" << pointsSize << " = " << matched << ")";
}
else if (rowCount > 10)
{
- b = X.block(0, 0, rowCount, 3).jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(Y.head(rowCount));
+ b = X.block(0, 0, rowCount, 3)
+ .jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV)
+ .solve(Y.head(rowCount));
b *= matchingCorrectionFactor;
if (anyIsNaN(b))
{
- ARMARX_WARNING << deactivateSpam(1) << "Equation solution is NaN "
- << VAROUT(b) << " " << VAROUT(rowCount);
+ ARMARX_WARNING << deactivateSpam(1) << "Equation solution is NaN " << VAROUT(b)
+ << " " << VAROUT(rowCount);
}
}
Eigen::Vector3f newEstimatedPose = addPose(currentPose, b);
if (anyIsNaN(newEstimatedPose))
{
- ARMARX_WARNING << "New estimated pose is NaN "
- << VAROUT(currentPose) << " " << VAROUT(b);
+ ARMARX_WARNING << "New estimated pose is NaN " << VAROUT(currentPose) << " "
+ << VAROUT(b);
}
else
{
@@ -1083,7 +1130,8 @@ void LaserScannerSelfLocalisation::updateLocalisation()
try
{
- if (updateWorkingMemory && s.checkFrequency("workingMemoryUpdate", workingMemoryUpdateFrequency))
+ if (updateWorkingMemory &&
+ s.checkFrequency("workingMemoryUpdate", workingMemoryUpdateFrequency))
{
// This seems to be the way to update the current pose
// Since in simulation another component (SelfLocalizationDynamicSimulation) updates this memory location
@@ -1093,7 +1141,8 @@ void LaserScannerSelfLocalisation::updateLocalisation()
agentInstance->setPose(reportPose);
if (agentsMemory)
{
- std::string robotAgentId = agentsMemory->upsertEntityByName(agentInstance->getName(), agentInstance);
+ std::string robotAgentId =
+ agentsMemory->upsertEntityByName(agentInstance->getName(), agentInstance);
agentInstance->setId(robotAgentId);
}
else
@@ -1133,19 +1182,19 @@ void LaserScannerSelfLocalisation::updateLocalisation()
{
laserScannerFileLoggingData->scanDataHistory[now] = (scanData);
}
-
}
-void LaserScannerSelfLocalisation::updateProperties(bool initial)
+void
+LaserScannerSelfLocalisation::updateProperties(bool initial)
{
-#define ARMARX_LSSL_UPDATE_PROPERTY(name) \
- { \
- auto prop = getProperty<decltype(prop ## name)>(#name); \
- if (initial || prop.isSet()) \
- { \
- prop ## name = prop.getValue(); \
- ARMARX_VERBOSE << VAROUT(prop ## name); \
- } \
+#define ARMARX_LSSL_UPDATE_PROPERTY(name) \
+ { \
+ auto prop = getProperty<decltype(prop##name)>(#name); \
+ if (initial || prop.isSet()) \
+ { \
+ prop##name = prop.getValue(); \
+ ARMARX_VERBOSE << VAROUT(prop##name); \
+ } \
}
try
{
@@ -1162,7 +1211,8 @@ void LaserScannerSelfLocalisation::updateProperties(bool initial)
resetKalmanFilter(estimatedPose);
}
}
- if (std::abs(propVelSensorStdDev - getProperty<float>("VelSensorStdDev").getValue()) > 0.00001)
+ if (std::abs(propVelSensorStdDev - getProperty<float>("VelSensorStdDev").getValue()) >
+ 0.00001)
{
propVelSensorStdDev = getProperty<float>("VelSensorStdDev");
ARMARX_VERBOSE << "new " << VAROUT(propVelSensorStdDev);
@@ -1173,8 +1223,6 @@ void LaserScannerSelfLocalisation::updateProperties(bool initial)
}
-
-
ARMARX_LSSL_UPDATE_PROPERTY(SmoothFrameSize);
ARMARX_LSSL_UPDATE_PROPERTY(SmoothMergeDistance);
ARMARX_LSSL_UPDATE_PROPERTY(MatchingMaxDistance);
@@ -1196,10 +1244,10 @@ void LaserScannerSelfLocalisation::updateProperties(bool initial)
if (!propLoggingFilePath.empty())
{
- if (laserScannerFileLoggingData && propRecordData && !getProperty<bool>("RecordData").getValue())
+ if (laserScannerFileLoggingData && propRecordData &&
+ !getProperty<bool>("RecordData").getValue())
{
writeLogFile();
-
}
if (!propRecordData && getProperty<bool>("RecordData").getValue())
{
@@ -1211,7 +1259,6 @@ void LaserScannerSelfLocalisation::updateProperties(bool initial)
}
useOdometry = propUseOdometry;
-
}
catch (std::exception const& ex)
{
@@ -1221,22 +1268,28 @@ void LaserScannerSelfLocalisation::updateProperties(bool initial)
#undef ARMARX_LSSL_UPDATE_PROPERTY
}
-
-
-void LaserScannerSelfLocalisation::resetKalmanFilter(const Eigen::Vector3f& pose)
+void
+LaserScannerSelfLocalisation::resetKalmanFilter(const Eigen::Vector3f& pose)
{
- ARMARX_INFO << "Position Std Dev: " << propSensorStdDev << " Vel Std Dev: " << propVelSensorStdDev;
+ ARMARX_INFO << "Position Std Dev: " << propSensorStdDev
+ << " Vel Std Dev: " << propVelSensorStdDev;
std::unique_lock lock(kalmanMutex);
- filter.reset(new memoryx::PlatformKalmanFilter(pose.head<2>().cast<double>(), pose(2),
- propSensorStdDev, propSensorStdDev / 100,
- propVelSensorStdDev, propVelSensorStdDev /* no scaling needed since motion noise scaling is done with rad/s*/));
+ filter.reset(new memoryx::PlatformKalmanFilter(
+ pose.head<2>().cast<double>(),
+ pose(2),
+ propSensorStdDev,
+ propSensorStdDev / 100,
+ propVelSensorStdDev,
+ propVelSensorStdDev /* no scaling needed since motion noise scaling is done with rad/s*/));
}
-void LaserScannerSelfLocalisation::writeLogFile()
+void
+LaserScannerSelfLocalisation::writeLogFile()
{
if (!laserScannerFileLoggingData)
{
- ARMARX_INFO << deactivateSpam(1) << "laserScannerFileLoggingData is NULL - cannot write log file";
+ ARMARX_INFO << deactivateSpam(1)
+ << "laserScannerFileLoggingData is NULL - cannot write log file";
return;
}
std::map<IceUtil::Time, std::vector<LaserScanData>> dataMap;
@@ -1268,26 +1321,15 @@ void LaserScannerSelfLocalisation::writeLogFile()
{
const LaserScanData& scan = elem;
Eigen::Quaternionf quat(scan.pose.block<3, 3>(0, 0));
- json["DeviceInfo"][scan.info.device] =
- {
+ json["DeviceInfo"][scan.info.device] = {
{"frame", scan.info.frame},
{"minAngle", scan.info.minAngle},
{"maxAngle", scan.info.maxAngle},
{"stepSize", scan.info.stepSize},
- {
- "sensorPosition", {
- {"x", scan.pose(0, 3)},
- {"y", scan.pose(1, 3)},
- {"z", scan.pose(2, 3)}
- }
- },
- {
- "sensorOrientation", {{"w", quat.w()},
- {"x", quat.x()},
- {"y", quat.y()},
- {"z", quat.z()}
- }
- }
+ {"sensorPosition",
+ {{"x", scan.pose(0, 3)}, {"y", scan.pose(1, 3)}, {"z", scan.pose(2, 3)}}},
+ {"sensorOrientation",
+ {{"w", quat.w()}, {"x", quat.x()}, {"y", quat.y()}, {"z", quat.z()}}}
};
Ice::IntSeq dataPts;
@@ -1309,21 +1351,22 @@ void LaserScannerSelfLocalisation::writeLogFile()
file << std::setw(1) << json << std::endl;
}
-
-LineSegment2DSeq LaserScannerSelfLocalisation::getMap(const Ice::Current&)
+LineSegment2DSeq
+LaserScannerSelfLocalisation::getMap(const Ice::Current&)
{
LineSegment2DSeq result;
for (LineSegment2Df const& segment : map)
{
- armarx::Vector2f start {segment.start.x(), segment.start.y()};
- armarx::Vector2f end {segment.end.x(), segment.end.y()};
- result.push_back(LineSegment2D {start, end});
+ armarx::Vector2f start{segment.start.x(), segment.start.y()};
+ armarx::Vector2f end{segment.end.x(), segment.end.y()};
+ result.push_back(LineSegment2D{start, end});
}
return result;
}
-
-void LaserScannerSelfLocalisation::componentPropertiesUpdated(const std::set<std::string>& changedProperties)
+void
+LaserScannerSelfLocalisation::componentPropertiesUpdated(
+ const std::set<std::string>& changedProperties)
{
updateProperties();
}
diff --git a/source/RobotComponents/components/LaserScannerSelfLocalisation/LaserScannerSelfLocalisation.h b/source/RobotComponents/components/LaserScannerSelfLocalisation/LaserScannerSelfLocalisation.h
index 421a142472571526033c4642d26dabd8035ae24f..54fe55613de7a56b01af40172176fb99df95e59a 100644
--- a/source/RobotComponents/components/LaserScannerSelfLocalisation/LaserScannerSelfLocalisation.h
+++ b/source/RobotComponents/components/LaserScannerSelfLocalisation/LaserScannerSelfLocalisation.h
@@ -22,6 +22,11 @@
#pragma once
+#include <atomic>
+#include <optional>
+
+#include <Eigen/Eigen>
+
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
#include <ArmarXCore/core/util/IceReportSkipper.h>
@@ -29,19 +34,15 @@
#include <RobotAPI/interface/units/LaserScannerUnit.h>
+#include <RobotComponents/interface/components/LaserScannerSelfLocalisation.h>
+
#include <MemoryX/interface/components/LongtermMemoryInterface.h>
#include <MemoryX/interface/components/WorkingMemoryInterface.h>
#include <MemoryX/interface/memorytypes/MemorySegments.h>
-#include <MemoryX/libraries/memorytypes/entity/AgentInstance.h>
-#include <MemoryX/libraries/memorytypes/entity/SimpleEntity.h>
#include <MemoryX/libraries/helpers/EarlyVisionHelpers/KalmanFilter.h>
#include <MemoryX/libraries/helpers/EarlyVisionHelpers/PlatformKalmanFilter.h>
-
-#include <RobotComponents/interface/components/LaserScannerSelfLocalisation.h>
-
-#include <Eigen/Eigen>
-#include <optional>
-#include <atomic>
+#include <MemoryX/libraries/memorytypes/entity/AgentInstance.h>
+#include <MemoryX/libraries/memorytypes/entity/SimpleEntity.h>
namespace armarx
{
@@ -58,7 +59,6 @@ namespace armarx
Eigen::Vector2f* pointsEnd;
};
-
struct LaserScanData
{
Eigen::Matrix4f pose;
@@ -82,57 +82,166 @@ namespace armarx
* @class LaserScannerSelfLocalisationPropertyDefinitions
* @brief
*/
- class LaserScannerSelfLocalisationPropertyDefinitions:
+ class LaserScannerSelfLocalisationPropertyDefinitions :
public armarx::ComponentPropertyDefinitions
{
public:
- LaserScannerSelfLocalisationPropertyDefinitions(std::string prefix):
+ LaserScannerSelfLocalisationPropertyDefinitions(std::string prefix) :
armarx::ComponentPropertyDefinitions(prefix)
{
- defineOptionalProperty<std::string>("ReportTopicName", "LaserScannerSelfLocalisationTopic", "The name of the report topic.");
- defineOptionalProperty<std::string>("RobotStateComponentName", "RobotStateComponent", "The name of the RobotStateComponent. Used to get local transformation of laser scanners");
- defineOptionalProperty<std::string>("PlatformName", "Platform", "Name of the platform to use. This property is used to listen to the platform topic");
- defineOptionalProperty<std::string>("LaserScannerUnitName", "LaserScannerSimulation", "Name of the laser scanner unit to use.");
- defineOptionalProperty<std::string>("WorkingMemoryName", "WorkingMemory", "Name of the WorkingMemory that should be used");
- defineOptionalProperty<std::string>("LongtermMemoryName", "LongtermMemory", "Name of the LongtermMemory component");
- defineOptionalProperty<bool>("UpdateWorkingMemory", true, "Update the working memory with the corrected position (disable in simulation)");
- defineOptionalProperty<std::string>("MapFilename", "RobotComponents/maps/building-5020-kitchen.json", "Floor map (2D) used for global localisation");
- defineOptionalProperty<std::string>("AgentName", "", "Name of the agent instance. If empty, the robot name of the RobotStateComponent will be used");
- defineOptionalProperty<std::string>("EmergencyStopMasterName", "EmergencyStopMaster", "The name used to register as an EmergencyStopMaster");
- defineOptionalProperty<std::string>("DebugObserverName", "DebugObserver", "Name of the topic the DebugObserver listens on");
-
- defineOptionalProperty<int>("UpdatePeriodInMs", 5, "Update period used for the map localisation");
- defineOptionalProperty<int>("UpdatePeriodWorkingMemoryInMs", 30, "Update period used for updating the working memory");
- defineOptionalProperty<int>("UpdatePeriodLongtermMemoryInMs", 30, "Update period used for updating the longterm memory");
- defineOptionalProperty<float>("RobotPositionZ", 0.0f, "The z-coordinate of the reported postion. Laser scanners can only self localize in x,y.");
-
- defineOptionalProperty<float>("MaximalLaserScannerDelay", 0.1, "If no new sensor values have been reported for this amound of seconds, we emit a soft emergency stop");
-
- defineOptionalProperty<int>("SmoothFrameSize", 7, "Frame size used for smoothing of laser scanner input", PropertyDefinitionBase::eModifiable);
- defineOptionalProperty<int>("SmoothMergeDistance", 160, "Distance in mm up to which laser scanner points are merged", PropertyDefinitionBase::eModifiable);
- defineOptionalProperty<float>("MatchingMaxDistance", 300.0f, "Maximal distance in mm up to which points are matched against edges of the map", PropertyDefinitionBase::eModifiable);
- defineOptionalProperty<float>("MatchingMinPoints", 0.01f, "Minimum percentage of points which need to be matched (range [0, 1]). If less points are matched no global correction is applied.", PropertyDefinitionBase::eModifiable);
- defineOptionalProperty<float>("MatchingCorrectionFactor", 0.01f, "This factor is used to apply the calculated map correction (range [0, 1])", PropertyDefinitionBase::eModifiable);
- defineOptionalProperty<float>("EdgeMaxDistance", 600.0f, "Maximum distance between adjacent points up to which they are merged into one edge [mm]", PropertyDefinitionBase::eModifiable);
- defineOptionalProperty<float>("EdgeMaxDeltaAngle", 0.10472f, "Maximum angle delta up to which adjacent points are merged into one edge [rad]", PropertyDefinitionBase::eModifiable);
- defineOptionalProperty<float>("EdgePointAddingThreshold", 10.0f, "Maximum least square error up to which points are added to an edge (extension at the front and back)", PropertyDefinitionBase::eModifiable);
- defineOptionalProperty<int>("EdgeEpsilon", 4, "Half frame size for line regression (angle calculation)", PropertyDefinitionBase::eModifiable);
- defineOptionalProperty<int>("EdgeMinPoints", 30, "Minimum number of points per edge (no edges with less points will be extracted)", PropertyDefinitionBase::eModifiable);
-
- defineOptionalProperty<bool>("UseOdometry", true, "Enable or disable odometry for pose estimation", PropertyDefinitionBase::eModifiable);
- defineOptionalProperty<bool>("UseMapCorrection", true, "Enable or disable map localisation for pose correction", PropertyDefinitionBase::eModifiable);
- defineOptionalProperty<bool>("ReportPoints", false, "Enable or disable the reports of (post-processed) laser scan points", PropertyDefinitionBase::eModifiable);
- defineOptionalProperty<bool>("ReportEdges", true, "Enable or disable the reports of extracted edges", PropertyDefinitionBase::eModifiable);
-
- defineOptionalProperty<float>("SensorStdDev", 100, "Standard deviation in position of the localization result", PropertyDefinitionBase::eModifiable);
- defineOptionalProperty<float>("VelSensorStdDev", 0.1f, "Standard deviation of the reported velocity", PropertyDefinitionBase::eModifiable);
-
- defineOptionalProperty<std::string>("LoggingFilePath", "", "Path to sensor data log file", PropertyDefinitionBase::eModifiable);
- defineOptionalProperty<bool>("RecordData", false, "If true, data is logged. Can be changed online. When set to false online, data is written to file. Otherwise on disconnect.", PropertyDefinitionBase::eModifiable);
-
- defineOptionalProperty<std::string>("PlatformRectangleMin", "0, 0", "Ignores points within the platform rectangle (this is the min x/y point)");
- defineOptionalProperty<std::string>("PlatformRectangleMax", "0, 0", "Ignores points within the platform rectangle (this is the max x/y point)");
-
+ defineOptionalProperty<std::string>("ReportTopicName",
+ "LaserScannerSelfLocalisationTopic",
+ "The name of the report topic.");
+ defineOptionalProperty<std::string>("RobotStateComponentName",
+ "RobotStateComponent",
+ "The name of the RobotStateComponent. Used to get "
+ "local transformation of laser scanners");
+ defineOptionalProperty<std::string>("PlatformName",
+ "Platform",
+ "Name of the platform to use. This property is "
+ "used to listen to the platform topic");
+ defineOptionalProperty<std::string>("LaserScannerUnitName",
+ "LaserScannerSimulation",
+ "Name of the laser scanner unit to use.");
+ defineOptionalProperty<std::string>("WorkingMemoryName",
+ "WorkingMemory",
+ "Name of the WorkingMemory that should be used");
+ defineOptionalProperty<std::string>(
+ "LongtermMemoryName", "LongtermMemory", "Name of the LongtermMemory component");
+ defineOptionalProperty<bool>(
+ "UpdateWorkingMemory",
+ true,
+ "Update the working memory with the corrected position (disable in simulation)");
+ defineOptionalProperty<std::string>("MapFilename",
+ "RobotComponents/maps/building-5020-kitchen.json",
+ "Floor map (2D) used for global localisation");
+ defineOptionalProperty<std::string>("AgentName",
+ "",
+ "Name of the agent instance. If empty, the robot "
+ "name of the RobotStateComponent will be used");
+ defineOptionalProperty<std::string>(
+ "EmergencyStopMasterName",
+ "EmergencyStopMaster",
+ "The name used to register as an EmergencyStopMaster");
+ defineOptionalProperty<std::string>("DebugObserverName",
+ "DebugObserver",
+ "Name of the topic the DebugObserver listens on");
+
+ defineOptionalProperty<int>(
+ "UpdatePeriodInMs", 5, "Update period used for the map localisation");
+ defineOptionalProperty<int>("UpdatePeriodWorkingMemoryInMs",
+ 30,
+ "Update period used for updating the working memory");
+ defineOptionalProperty<int>("UpdatePeriodLongtermMemoryInMs",
+ 30,
+ "Update period used for updating the longterm memory");
+ defineOptionalProperty<float>("RobotPositionZ",
+ 0.0f,
+ "The z-coordinate of the reported postion. Laser "
+ "scanners can only self localize in x,y.");
+
+ defineOptionalProperty<float>("MaximalLaserScannerDelay",
+ 0.1,
+ "If no new sensor values have been reported for this "
+ "amound of seconds, we emit a soft emergency stop");
+
+ defineOptionalProperty<int>("SmoothFrameSize",
+ 7,
+ "Frame size used for smoothing of laser scanner input",
+ PropertyDefinitionBase::eModifiable);
+ defineOptionalProperty<int>(
+ "SmoothMergeDistance",
+ 160,
+ "Distance in mm up to which laser scanner points are merged",
+ PropertyDefinitionBase::eModifiable);
+ defineOptionalProperty<float>(
+ "MatchingMaxDistance",
+ 300.0f,
+ "Maximal distance in mm up to which points are matched against edges of the map",
+ PropertyDefinitionBase::eModifiable);
+ defineOptionalProperty<float>(
+ "MatchingMinPoints",
+ 0.01f,
+ "Minimum percentage of points which need to be matched (range [0, 1]). If less "
+ "points are matched no global correction is applied.",
+ PropertyDefinitionBase::eModifiable);
+ defineOptionalProperty<float>(
+ "MatchingCorrectionFactor",
+ 0.01f,
+ "This factor is used to apply the calculated map correction (range [0, 1])",
+ PropertyDefinitionBase::eModifiable);
+ defineOptionalProperty<float>("EdgeMaxDistance",
+ 600.0f,
+ "Maximum distance between adjacent points up to which "
+ "they are merged into one edge [mm]",
+ PropertyDefinitionBase::eModifiable);
+ defineOptionalProperty<float>(
+ "EdgeMaxDeltaAngle",
+ 0.10472f,
+ "Maximum angle delta up to which adjacent points are merged into one edge [rad]",
+ PropertyDefinitionBase::eModifiable);
+ defineOptionalProperty<float>("EdgePointAddingThreshold",
+ 10.0f,
+ "Maximum least square error up to which points are added "
+ "to an edge (extension at the front and back)",
+ PropertyDefinitionBase::eModifiable);
+ defineOptionalProperty<int>("EdgeEpsilon",
+ 4,
+ "Half frame size for line regression (angle calculation)",
+ PropertyDefinitionBase::eModifiable);
+ defineOptionalProperty<int>(
+ "EdgeMinPoints",
+ 30,
+ "Minimum number of points per edge (no edges with less points will be extracted)",
+ PropertyDefinitionBase::eModifiable);
+
+ defineOptionalProperty<bool>("UseOdometry",
+ true,
+ "Enable or disable odometry for pose estimation",
+ PropertyDefinitionBase::eModifiable);
+ defineOptionalProperty<bool>("UseMapCorrection",
+ true,
+ "Enable or disable map localisation for pose correction",
+ PropertyDefinitionBase::eModifiable);
+ defineOptionalProperty<bool>(
+ "ReportPoints",
+ false,
+ "Enable or disable the reports of (post-processed) laser scan points",
+ PropertyDefinitionBase::eModifiable);
+ defineOptionalProperty<bool>("ReportEdges",
+ true,
+ "Enable or disable the reports of extracted edges",
+ PropertyDefinitionBase::eModifiable);
+
+ defineOptionalProperty<float>(
+ "SensorStdDev",
+ 100,
+ "Standard deviation in position of the localization result",
+ PropertyDefinitionBase::eModifiable);
+ defineOptionalProperty<float>("VelSensorStdDev",
+ 0.1f,
+ "Standard deviation of the reported velocity",
+ PropertyDefinitionBase::eModifiable);
+
+ defineOptionalProperty<std::string>("LoggingFilePath",
+ "",
+ "Path to sensor data log file",
+ PropertyDefinitionBase::eModifiable);
+ defineOptionalProperty<bool>(
+ "RecordData",
+ false,
+ "If true, data is logged. Can be changed online. When set to false online, data is "
+ "written to file. Otherwise on disconnect.",
+ PropertyDefinitionBase::eModifiable);
+
+ defineOptionalProperty<std::string>(
+ "PlatformRectangleMin",
+ "0, 0",
+ "Ignores points within the platform rectangle (this is the min x/y point)");
+ defineOptionalProperty<std::string>(
+ "PlatformRectangleMax",
+ "0, 0",
+ "Ignores points within the platform rectangle (this is the max x/y point)");
}
};
@@ -165,7 +274,8 @@ namespace armarx
/**
* @see armarx::ManagedIceObject::getDefaultName()
*/
- std::string getDefaultName() const override
+ std::string
+ getDefaultName() const override
{
return "LaserScannerSelfLocalisation";
}
@@ -198,16 +308,26 @@ namespace armarx
// LaserScannerSelfLocalisationInterface interface
std::string getReportTopicName(const Ice::Current&) override;
- void setAbsolutePose(Ice::Float x, Ice::Float y, Ice::Float theta, const Ice::Current&) override;
+ void
+ setAbsolutePose(Ice::Float x, Ice::Float y, Ice::Float theta, const Ice::Current&) override;
LineSegment2DSeq getMap(const Ice::Current&) override;
// LaserScannerUnitListener interface
- void reportSensorValues(const std::string& device, const std::string& name, const LaserScan& scan,
- const TimestampBasePtr& timestamp, const Ice::Current&) override;
+ void reportSensorValues(const std::string& device,
+ const std::string& name,
+ const LaserScan& scan,
+ const TimestampBasePtr& timestamp,
+ const Ice::Current&) override;
// PlatformUnitListener interface
- void reportPlatformVelocity(Ice::Float velX, Ice::Float velY, Ice::Float velTheta, const Ice::Current&) override;
- void reportPlatformOdometryPose(Ice::Float, Ice::Float, Ice::Float, const Ice::Current&) override;
+ void reportPlatformVelocity(Ice::Float velX,
+ Ice::Float velY,
+ Ice::Float velTheta,
+ const Ice::Current&) override;
+ void reportPlatformOdometryPose(Ice::Float,
+ Ice::Float,
+ Ice::Float,
+ const Ice::Current&) override;
// Component interface
void componentPropertiesUpdated(const std::set<std::string>& changedProperties) override;
@@ -220,6 +340,7 @@ namespace armarx
void resetKalmanFilter(const Eigen::Vector3f& pose);
Eigen::Vector3f filterPose(const Eigen::Vector3f& pose);
void writeLogFile();
+
private:
std::string reportTopicName;
std::string robotStateComponentName;
@@ -239,26 +360,26 @@ namespace armarx
float maximalLaserScannerDelay = 0.1f;
std::mutex propertyMutex;
- int propSmoothFrameSize;
- int propSmoothMergeDistance;
+ int propSmoothFrameSize;
+ int propSmoothMergeDistance;
float propMatchingMaxDistance;
float propMatchingMinPoints;
float propMatchingCorrectionFactor;
float propEdgeMaxDistance;
float propEdgeMaxDeltaAngle;
float propEdgePointAddingThreshold;
- int propEdgeEpsilon;
- int propEdgeMinPoints;
- bool propReportPoints;
- bool propReportEdges;
- bool propUseOdometry;
- bool propUseMapCorrection;
+ int propEdgeEpsilon;
+ int propEdgeMinPoints;
+ bool propReportPoints;
+ bool propReportEdges;
+ bool propUseOdometry;
+ bool propUseMapCorrection;
float propSensorStdDev;
float propVelSensorStdDev;
std::string propLoggingFilePath;
bool propRecordData = false;
- std::atomic<bool> useOdometry;
+ std::atomic<bool> useOdometry;
std::vector<LineSegment2Df> map;
@@ -307,7 +428,7 @@ namespace armarx
std::map<IceUtil::Time, std::vector<LaserScanData>> scanDataHistory;
};
- std::unique_ptr<LaserScannerFileLoggingData> laserScannerFileLoggingData;
+ std::unique_ptr<LaserScannerFileLoggingData> laserScannerFileLoggingData;
};
-}
+} // namespace armarx
diff --git a/source/RobotComponents/components/LaserScannerSelfLocalisation/test/LaserScannerSelfLocalisationTest.cpp b/source/RobotComponents/components/LaserScannerSelfLocalisation/test/LaserScannerSelfLocalisationTest.cpp
index f6f3cd155ea9a072ea94432ac3af3a88fc427ec4..2b2d92ad9057e70b48f50b5128bac71595bbedc8 100644
--- a/source/RobotComponents/components/LaserScannerSelfLocalisation/test/LaserScannerSelfLocalisationTest.cpp
+++ b/source/RobotComponents/components/LaserScannerSelfLocalisation/test/LaserScannerSelfLocalisationTest.cpp
@@ -24,11 +24,11 @@
#define ARMARX_BOOST_TEST
+#include <iostream>
+
#include <RobotComponents/Test.h>
#include <RobotComponents/components/LaserScannerSelfLocalisation/LaserScannerSelfLocalisation.h>
-#include <iostream>
-
BOOST_AUTO_TEST_CASE(testExample)
{
armarx::LaserScannerSelfLocalisation instance;
diff --git a/source/RobotComponents/components/MMMPlayer/MMMPlayer.cpp b/source/RobotComponents/components/MMMPlayer/MMMPlayer.cpp
index 782191353c6947f8a1e506251f9e635555ec0209..6be3173901b82b73f4dbbf0e7ab64db47e02c59e 100644
--- a/source/RobotComponents/components/MMMPlayer/MMMPlayer.cpp
+++ b/source/RobotComponents/components/MMMPlayer/MMMPlayer.cpp
@@ -23,36 +23,38 @@
*/
#include "MMMPlayer.h"
-#include <MMM/Motion/Legacy/LegacyMotionReaderXML.h>
-#include <MMM/Model/ModelReaderXML.h>
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <Ice/ObjectAdapter.h>
-#include <ArmarXCore/core/system/ArmarXDataPath.h>
+#include <VirtualRobot/MathTools.h>
-#include <ArmarXCore/observers/variant/Variant.h>
-#include <ArmarXCore/interface/observers/VariantBase.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
#include <ArmarXCore/core/time/TimeUtil.h>
-#include <VirtualRobot/MathTools.h>
-#include <RobotAPI/libraries/core/Trajectory.h>
-#include <ArmarXCore/observers/filters/ButterworthFilter.h>
#include <ArmarXCore/core/util/StringHelpers.h>
+#include <ArmarXCore/interface/observers/VariantBase.h>
+#include <ArmarXCore/observers/filters/ButterworthFilter.h>
+#include <ArmarXCore/observers/variant/Variant.h>
+
+#include <RobotAPI/libraries/core/Trajectory.h>
+
#include <RobotComponents/libraries/MMM/MotionFileWrapper.h>
-#include <Ice/ObjectAdapter.h>
+#include <MMM/Model/ModelReaderXML.h>
+#include <MMM/Motion/Legacy/LegacyMotionReaderXML.h>
using namespace armarx;
using namespace MMM;
-
-void MMMPlayer::onInitComponent()
+void
+MMMPlayer::onInitComponent()
{
// offeringTopic("DebugObserver");
}
-
-void MMMPlayer::onConnectComponent()
+void
+MMMPlayer::onConnectComponent()
{
ARMARX_INFO << "ON CONNECT";
std::string armarxProjects = getProperty<std::string>("ArmarXProjects").getValue();
@@ -92,7 +94,8 @@ void MMMPlayer::onConnectComponent()
}*/
}
-void MMMPlayer::load(const std::string& MMMFile, const std::string& projects)
+void
+MMMPlayer::load(const std::string& MMMFile, const std::string& projects)
{
std::unique_lock lock(mmmMutex);
@@ -119,7 +122,11 @@ void MMMPlayer::load(const std::string& MMMFile, const std::string& projects)
ArmarXDataPath::getAbsolutePath(MMMFile, motionPath);
- motionWrapper = MotionFileWrapper::create(motionPath, getProperty<bool>("ApplyButterworthFilter").getValue() ? getProperty<float>("ButterworthFilterCutOffFreq") : 0);
+ motionWrapper =
+ MotionFileWrapper::create(motionPath,
+ getProperty<bool>("ApplyButterworthFilter").getValue()
+ ? getProperty<float>("ButterworthFilterCutOffFreq")
+ : 0);
if (!motionWrapper)
{
terminate();
@@ -130,40 +137,47 @@ void MMMPlayer::load(const std::string& MMMFile, const std::string& projects)
motionData = motionWrapper->getFirstMotionData();
}
-bool MMMPlayer::loadMMMFile(const std::string& MMMFile, const std::string& projects, const Ice::Current&)
+bool
+MMMPlayer::loadMMMFile(const std::string& MMMFile, const std::string& projects, const Ice::Current&)
{
ARMARX_VERBOSE << "loaded trajectory " << MMMFile;
load(MMMFile, projects);
return motionData != nullptr;
}
-bool MMMPlayer::setMotionData(const std::string& motionName, const Ice::Current&)
+bool
+MMMPlayer::setMotionData(const std::string& motionName, const Ice::Current&)
{
motionData = motionWrapper->getMotionData(motionName);
return motionData != nullptr;
}
-int MMMPlayer::getNumberOfFrames(const Ice::Current&)
+int
+MMMPlayer::getNumberOfFrames(const Ice::Current&)
{
- return (int) motionData->numberOfFrames;
+ return (int)motionData->numberOfFrames;
}
-std::string MMMPlayer::getMotionPath(const Ice::Current&)
+std::string
+MMMPlayer::getMotionPath(const Ice::Current&)
{
return motionPath;
}
-std::string MMMPlayer::getModelPath(const Ice::Current&)
+std::string
+MMMPlayer::getModelPath(const Ice::Current&)
{
return motionData->modelPath;
}
-Ice::StringSeq MMMPlayer::getJointNames(const Ice::Current&)
+Ice::StringSeq
+MMMPlayer::getJointNames(const Ice::Current&)
{
return motionData->jointNames;
}
-Ice::StringSeq MMMPlayer::getMotionNames(const Ice::Current&)
+Ice::StringSeq
+MMMPlayer::getMotionNames(const Ice::Current&)
{
if (!motionWrapper)
{
@@ -172,32 +186,36 @@ Ice::StringSeq MMMPlayer::getMotionNames(const Ice::Current&)
return motionWrapper->getMotionNames();
}
-bool MMMPlayer::isMotionLoaded(const Ice::Current&)
+bool
+MMMPlayer::isMotionLoaded(const Ice::Current&)
{
return motionData != nullptr;
}
-void MMMPlayer::onDisconnectComponent()
+void
+MMMPlayer::onDisconnectComponent()
{
-
}
-void MMMPlayer::onExitComponent()
+void
+MMMPlayer::onExitComponent()
{
-
}
-TrajectoryBasePtr MMMPlayer::getJointTraj(const Ice::Current&)
+TrajectoryBasePtr
+MMMPlayer::getJointTraj(const Ice::Current&)
{
return motionData->getJointTrajectory();
}
-TrajectoryBasePtr MMMPlayer::getBasePoseTraj(const Ice::Current&)
+TrajectoryBasePtr
+MMMPlayer::getBasePoseTraj(const Ice::Current&)
{
return motionData->getPoseTrajectory();
}
-PropertyDefinitionsPtr MMMPlayer::createPropertyDefinitions()
+PropertyDefinitionsPtr
+MMMPlayer::createPropertyDefinitions()
{
return PropertyDefinitionsPtr(new MMMPlayerPropertyDefinitions(getConfigIdentifier()));
}
diff --git a/source/RobotComponents/components/MMMPlayer/MMMPlayer.h b/source/RobotComponents/components/MMMPlayer/MMMPlayer.h
index 6e1201448f9227611a7b658dcf96bca96a3ae140..48b74a1974cf73152e66dc2f90a732f2edeadad0 100644
--- a/source/RobotComponents/components/MMMPlayer/MMMPlayer.h
+++ b/source/RobotComponents/components/MMMPlayer/MMMPlayer.h
@@ -24,14 +24,14 @@
#pragma once
-#include <ArmarXCore/core/Component.h>
+#include <mutex>
-#include <RobotComponents/interface/components/MMMPlayerInterface.h>
+#include <ArmarXCore/core/Component.h>
#include <RobotAPI/libraries/core/Pose.h>
#include <RobotAPI/libraries/core/Trajectory.h>
-#include <mutex>
+#include <RobotComponents/interface/components/MMMPlayerInterface.h>
namespace armarx
{
@@ -42,17 +42,24 @@ namespace armarx
* \class MMMPlayerPropertyDefinitions
* \brief
*/
- class MMMPlayerPropertyDefinitions:
- public ComponentPropertyDefinitions
+ class MMMPlayerPropertyDefinitions : public ComponentPropertyDefinitions
{
public:
- MMMPlayerPropertyDefinitions(std::string prefix):
- ComponentPropertyDefinitions(prefix)
+ MMMPlayerPropertyDefinitions(std::string prefix) : ComponentPropertyDefinitions(prefix)
{
- defineOptionalProperty<std::string>("ArmarXProjects", "", "Comma-separated list with names of ArmarXProjects (e.g. 'Armar3,Armar4'). The MMM XML File can be specified relatively to a data path of one of these projects.");
+ defineOptionalProperty<std::string>(
+ "ArmarXProjects",
+ "",
+ "Comma-separated list with names of ArmarXProjects (e.g. 'Armar3,Armar4'). The MMM "
+ "XML File can be specified relatively to a data path of one of these projects.");
//defineOptionalProperty<std::string>("MMMFile", "", "Path to MMM XML File");
- defineOptionalProperty<bool>("ApplyButterworthFilter", false, "If true a butterworth filter is applied on the trajectory.");
- defineOptionalProperty<float>("ButterworthFilterCutOffFreq", 20, "Cut off frequency for the butterworth lowpass filter.");
+ defineOptionalProperty<bool>(
+ "ApplyButterworthFilter",
+ false,
+ "If true a butterworth filter is applied on the trajectory.");
+ defineOptionalProperty<float>("ButterworthFilterCutOffFreq",
+ 20,
+ "Cut off frequency for the butterworth lowpass filter.");
}
};
@@ -70,15 +77,14 @@ namespace armarx
* @ingroup Component-MMMPlayer
* @brief The MMMPlayer class
*/
- class MMMPlayer :
- virtual public armarx::Component,
- public armarx::MMMPlayerInterface
+ class MMMPlayer : virtual public armarx::Component, public armarx::MMMPlayerInterface
{
public:
/**
* @see armarx::ManagedIceObject::getDefaultName()
*/
- std::string getDefaultName() const override
+ std::string
+ getDefaultName() const override
{
return "MMMPlayer";
}
@@ -117,9 +123,12 @@ namespace armarx
std::string motionPath;
std::recursive_mutex mmmMutex;
+
public:
// MMMPlayerInterface
- bool loadMMMFile(const std::string& filename, const std::string& projects, const Ice::Current&) override;
+ bool loadMMMFile(const std::string& filename,
+ const std::string& projects,
+ const Ice::Current&) override;
bool setMotionData(const std::string& motionName, const Ice::Current&) override;
int getNumberOfFrames(const Ice::Current&) override;
std::string getMotionPath(const Ice::Current&) override;
@@ -130,10 +139,8 @@ namespace armarx
TrajectoryBasePtr getJointTraj(const Ice::Current&) override;
TrajectoryBasePtr getBasePoseTraj(const Ice::Current&) override;
-
};
- using MMMPlayerPtr = ::IceInternal::Handle< ::armarx::MMMPlayer>;
-
-}
+ using MMMPlayerPtr = ::IceInternal::Handle<::armarx::MMMPlayer>;
+} // namespace armarx
diff --git a/source/RobotComponents/components/MotionPlanning/CSpace/CSpace.cpp b/source/RobotComponents/components/MotionPlanning/CSpace/CSpace.cpp
index 8addbf4d6920c0afcd00062455b6142acfef8368..64be3c18979813ac3e9851eb0376e517b844a882 100644
--- a/source/RobotComponents/components/MotionPlanning/CSpace/CSpace.cpp
+++ b/source/RobotComponents/components/MotionPlanning/CSpace/CSpace.cpp
@@ -21,20 +21,23 @@
* @copyright http://www.gnu.org/licenses/gpl.txt
* GNU General Public License
*/
-#include <ArmarXCore/core/logging/Logging.h>
-
#include "CSpace.h"
+#include <ArmarXCore/core/logging/Logging.h>
+
namespace armarx
{
- bool CSpace::isValidConfiguration(const ::std::pair<const Ice::Float*, const Ice::Float*>& config, const Ice::Current&) const
+ bool
+ CSpace::isValidConfiguration(const ::std::pair<const Ice::Float*, const Ice::Float*>& config,
+ const Ice::Current&) const
{
ARMARX_CHECK_EXPRESSION(config.second - config.first == getDimensionality());
auto it = config.first;
for (const auto& dim : getDimensionsBounds())
{
- if (*it < dim.min - std::numeric_limits<float>::epsilon() || *it > dim.max + std::numeric_limits<float>::epsilon())
+ if (*it < dim.min - std::numeric_limits<float>::epsilon() ||
+ *it > dim.max + std::numeric_limits<float>::epsilon())
{
return false;
}
@@ -42,4 +45,4 @@ namespace armarx
}
return true;
}
-}
+} // namespace armarx
diff --git a/source/RobotComponents/components/MotionPlanning/CSpace/CSpace.h b/source/RobotComponents/components/MotionPlanning/CSpace/CSpace.h
index 31451a26af20ca302457054f0cd4040e2fe2b53f..63710783de2fb04e7fed2fe3afb032fbead72831 100644
--- a/source/RobotComponents/components/MotionPlanning/CSpace/CSpace.h
+++ b/source/RobotComponents/components/MotionPlanning/CSpace/CSpace.h
@@ -26,9 +26,10 @@
#include <functional>
#include <random>
-#include <RobotComponents/interface/components/MotionPlanning/CSpace/CSpace.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <RobotComponents/interface/components/MotionPlanning/CSpace/CSpace.h>
+
namespace armarx
{
class CSpace;
@@ -48,31 +49,32 @@ namespace armarx
*
* The class provides a function to check whether a configuration is collision free.
*/
- class CSpace :
- virtual public CSpaceBase
+ class CSpace : virtual public CSpaceBase
{
public:
-
-
-
/**
* @param config The config to check.
* @return Whether the given configuration is inside the cspace's bounds.
*/
- bool isValidConfiguration(const ::std::pair<const Ice::Float*, const Ice::Float*>& config, const Ice::Current& = Ice::emptyCurrent) const override;
+ bool isValidConfiguration(const ::std::pair<const Ice::Float*, const Ice::Float*>& config,
+ const Ice::Current& = Ice::emptyCurrent) const override;
//noop functions (can be overwritten in derived classes)
/**
* @brief Initializes collision checking.
* The default implementation is noop.
*/
- void initCollisionTest(const Ice::Current& = Ice::emptyCurrent) override {}
+ void
+ initCollisionTest(const Ice::Current& = Ice::emptyCurrent) override
+ {
+ }
/**
* @param config The config to check.
* @return Whether the given configuration is collision free.
*/
- bool isCollisionFree(const ::std::pair<const Ice::Float*, const Ice::Float*>& config, const Ice::Current& = Ice::emptyCurrent) override = 0;
+ bool isCollisionFree(const ::std::pair<const Ice::Float*, const Ice::Float*>& config,
+ const Ice::Current& = Ice::emptyCurrent) override = 0;
/**
* @return A clone of this object.
@@ -84,7 +86,8 @@ namespace armarx
* @param last The configuration's last element.
* @return Whether the given configuration [first,last) is collision free.
*/
- virtual bool isCollisionFree(const Ice::Float* first, const Ice::Float* last)
+ virtual bool
+ isCollisionFree(const Ice::Float* first, const Ice::Float* last)
{
return isCollisionFree(std::make_pair(first, last));
}
@@ -93,16 +96,19 @@ namespace armarx
* @param config The config to check. (as \ref armarx::VectorXf)
* @return Whether the given configuration is inside the cspace's bounds.
*/
- virtual bool isValidConfiguration(const VectorXf& config) const
+ virtual bool
+ isValidConfiguration(const VectorXf& config) const
{
- return isValidConfiguration(std::make_pair(config.data(), config.data() + config.size()));
+ return isValidConfiguration(
+ std::make_pair(config.data(), config.data() + config.size()));
}
/**
* @param config The config to check. (as \ref armarx::VectorXf)
* @return Whether the given configuration is collision free.
*/
- virtual bool isCollisionFree(const VectorXf& config)
+ virtual bool
+ isCollisionFree(const VectorXf& config)
{
return isCollisionFree(std::make_pair(config.data(), config.data() + config.size()));
}
@@ -110,7 +116,8 @@ namespace armarx
/**
* @return Whether the cspaces scales some axis internally during planning
*/
- virtual bool usesInternalScaling()
+ virtual bool
+ usesInternalScaling()
{
return false;
}
@@ -122,24 +129,24 @@ namespace armarx
*/
using CSpaceAdaptorPtr = IceInternal::Handle<CSpaceAdaptor>;
- class CSpaceAdaptor:
- virtual public CSpace,
- virtual public CSpaceAdaptorBase
+ class CSpaceAdaptor : virtual public CSpace, virtual public CSpaceAdaptorBase
{
public:
- CSpaceAdaptor(CSpaceBasePtr originalCSpace): CSpaceAdaptorBase(originalCSpace)
+ CSpaceAdaptor(CSpaceBasePtr originalCSpace) : CSpaceAdaptorBase(originalCSpace)
{
- if (! originalCSpace)
+ if (!originalCSpace)
{
- throw std::invalid_argument {"the original cspace can't be null"};
+ throw std::invalid_argument{"the original cspace can't be null"};
}
}
- CSpaceBasePtr getOriginalCSpace(const Ice::Current& = Ice::emptyCurrent) const override
+ CSpaceBasePtr
+ getOriginalCSpace(const Ice::Current& = Ice::emptyCurrent) const override
{
return originalCSpace;
}
+
protected:
CSpaceAdaptor() = default;
};
-}
+} // namespace armarx
diff --git a/source/RobotComponents/components/MotionPlanning/CSpace/GlobalCache.h b/source/RobotComponents/components/MotionPlanning/CSpace/GlobalCache.h
index 7a0905daa771f6ee7b16d8599875184c72ae4727..ddcf6b14126afdbed5154fe6cb8253b3b49b7ec5 100644
--- a/source/RobotComponents/components/MotionPlanning/CSpace/GlobalCache.h
+++ b/source/RobotComponents/components/MotionPlanning/CSpace/GlobalCache.h
@@ -23,12 +23,13 @@
*/
#pragma once
-#include <string>
#include <atomic>
#include <map>
-#include <ArmarXCore/core/system/Synchronization.h>
-#include <ArmarXCore/core/logging/Logging.h>
+#include <string>
+
#include <ArmarXCore/core/exceptions/Exception.h>
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/system/Synchronization.h>
#include <ArmarXCore/core/util/StringHelpers.h>
namespace armarx
@@ -43,6 +44,7 @@ namespace armarx
auto lock = getMapLock();
getInstanceCounter()++;
}
+
GlobalCache(const GlobalCache& source)
{
auto lock = getMapLock();
@@ -55,17 +57,21 @@ namespace armarx
getInstanceCounter()--;
if (getInstanceCounter() == 0)
{
- ARMARX_IMPORTANT << "Clearing global cache for type " << armarx::GetTypeString<ObjectTypePtr>(true);
+ ARMARX_IMPORTANT << "Clearing global cache for type "
+ << armarx::GetTypeString<ObjectTypePtr>(true);
getCache().clear();
}
}
- bool hasObject(const Id& id) const
+ bool
+ hasObject(const Id& id) const
{
auto lock = getMapLock();
return getCache().count(id) != 0;
}
- ObjectTypePtr getCacheObject(const Id& id)
+
+ ObjectTypePtr
+ getCacheObject(const Id& id)
{
auto lock = getMapLock();
auto& map = getCache();
@@ -80,7 +86,8 @@ namespace armarx
}
}
- void insertObject(const Id& id, const ObjectTypePtr& obj)
+ void
+ insertObject(const Id& id, const ObjectTypePtr& obj)
{
auto lock = getMapLock();
auto& map = getCache();
@@ -91,28 +98,29 @@ namespace armarx
}
map.insert(std::make_pair(id, obj));
}
+
protected:
- std::atomic<int>& getInstanceCounter()
+ std::atomic<int>&
+ getInstanceCounter()
{
static std::atomic<int> InstanceCounter = {0};
return InstanceCounter;
}
- ScopedLockPtr getMapLock() const
+ ScopedLockPtr
+ getMapLock() const
{
static Mutex mutex;
return ScopedLockPtr(new ScopedLock(mutex));
}
- std::map<Id, ObjectTypePtr>& getCache() const
+ std::map<Id, ObjectTypePtr>&
+ getCache() const
{
static std::map<Id, ObjectTypePtr> map;
return map;
}
-
};
-}
-
-
+} // namespace armarx
diff --git a/source/RobotComponents/components/MotionPlanning/CSpace/ScaledCSpace.cpp b/source/RobotComponents/components/MotionPlanning/CSpace/ScaledCSpace.cpp
index 376ddb671a3b5ad968ff946a2c46158dd1ff6c2e..330c38ef0175ead84458bdee041c31038e2a8a82 100644
--- a/source/RobotComponents/components/MotionPlanning/CSpace/ScaledCSpace.cpp
+++ b/source/RobotComponents/components/MotionPlanning/CSpace/ScaledCSpace.cpp
@@ -24,10 +24,10 @@
#include "ScaledCSpace.h"
-#include <ArmarXCore/core/logging/Logging.h>
-
#include <algorithm>
+#include <ArmarXCore/core/logging/Logging.h>
+
namespace armarx
{
@@ -42,38 +42,41 @@ namespace armarx
{
std::stringstream s;
s << "Different number of scaling factors (" << scalingFactors.size()
- << ") than dimensionality of the original cspace(" << originalCSpace->getDimensionality() << ")";
+ << ") than dimensionality of the original cspace("
+ << originalCSpace->getDimensionality() << ")";
ARMARX_ERROR_S << s.str();
- throw std::invalid_argument {s.str()};
+ throw std::invalid_argument{s.str()};
}
- if (std::any_of(scalingFactors.begin(), scalingFactors.end(), [](float f)
- {
- return f <= 0.f;
- }))
+ if (std::any_of(
+ scalingFactors.begin(), scalingFactors.end(), [](float f) { return f <= 0.f; }))
{
ARMARX_ERROR_S << "One or more factors are <= 0!";
- throw std::invalid_argument {"One or more factors are <= 0!"};
+ throw std::invalid_argument{"One or more factors are <= 0!"};
}
}
- void ScaledCSpace::unscaleConfig(VectorXf& config) const
+ void
+ ScaledCSpace::unscaleConfig(VectorXf& config) const
{
ARMARX_CHECK_EXPRESSION(config.size() == scalingFactors.size());
unscaleToBuffer(config.data(), config);
}
- void ScaledCSpace::unscalePath(Path& path) const
+ void
+ ScaledCSpace::unscalePath(Path& path) const
{
unscalePath(path.nodes);
}
- void ScaledCSpace::unscalePath(PathWithCost& path) const
+ void
+ ScaledCSpace::unscalePath(PathWithCost& path) const
{
unscalePath(path.nodes);
}
- void ScaledCSpace::unscalePath(VectorXfSeq& nodes) const
+ void
+ ScaledCSpace::unscalePath(VectorXfSeq& nodes) const
{
for (auto& cfg : nodes)
{
@@ -81,7 +84,8 @@ namespace armarx
}
}
- void ScaledCSpace::scaleConfig(VectorXf& config) const
+ void
+ ScaledCSpace::scaleConfig(VectorXf& config) const
{
ARMARX_CHECK_EXPRESSION(config.size() == scalingFactors.size());
for (std::size_t i = 0; i < scalingFactors.size(); ++i)
@@ -90,7 +94,8 @@ namespace armarx
}
}
- void ScaledCSpace::scalePath(Path& path) const
+ void
+ ScaledCSpace::scalePath(Path& path) const
{
for (auto& cfg : path.nodes)
{
@@ -98,22 +103,28 @@ namespace armarx
}
}
- bool ScaledCSpace::isCollisionFree(const::std::pair<const Ice::Float*, const Ice::Float*>& cfg, const Ice::Current&)
+ bool
+ ScaledCSpace::isCollisionFree(const ::std::pair<const Ice::Float*, const Ice::Float*>& cfg,
+ const Ice::Current&)
{
- ARMARX_CHECK_EXPRESSION(unscaled.size() == static_cast<std::size_t>(std::distance(cfg.first, cfg.second)));
+ ARMARX_CHECK_EXPRESSION(unscaled.size() ==
+ static_cast<std::size_t>(std::distance(cfg.first, cfg.second)));
unscaleToBuffer(cfg.first, unscaled);
- return originalCSpace->isCollisionFree(std::make_pair(unscaled.data(), unscaled.data() + unscaled.size()));
+ return originalCSpace->isCollisionFree(
+ std::make_pair(unscaled.data(), unscaled.data() + unscaled.size()));
}
- CSpaceBasePtr ScaledCSpace::clone(const Ice::Current&)
+ CSpaceBasePtr
+ ScaledCSpace::clone(const Ice::Current&)
{
- ScaledCSpacePtr cloned {new ScaledCSpace{}};
+ ScaledCSpacePtr cloned{new ScaledCSpace{}};
cloned->scalingFactors = scalingFactors;
cloned->originalCSpace = originalCSpace->clone();
return cloned;
}
- FloatRangeSeq ScaledCSpace::getDimensionsBounds(const Ice::Current&) const
+ FloatRangeSeq
+ ScaledCSpace::getDimensionsBounds(const Ice::Current&) const
{
FloatRangeSeq bounds = originalCSpace->getDimensionsBounds();
@@ -126,7 +137,8 @@ namespace armarx
return bounds;
}
- void ScaledCSpace::unscaleToBuffer(const Ice::Float* cfg, VectorXf& buffer) const
+ void
+ ScaledCSpace::unscaleToBuffer(const Ice::Float* cfg, VectorXf& buffer) const
{
ARMARX_CHECK_EXPRESSION(buffer.size() == scalingFactors.size());
@@ -135,4 +147,4 @@ namespace armarx
buffer.at(i) = cfg[i] / scalingFactors.at(i);
}
}
-}
+} // namespace armarx
diff --git a/source/RobotComponents/components/MotionPlanning/CSpace/ScaledCSpace.h b/source/RobotComponents/components/MotionPlanning/CSpace/ScaledCSpace.h
index decc024e9d9af6c62d4fba39bf6af0c7b3233ce9..e02d0580e9719403d2a55ae44e68e58bcbb62e18 100644
--- a/source/RobotComponents/components/MotionPlanning/CSpace/ScaledCSpace.h
+++ b/source/RobotComponents/components/MotionPlanning/CSpace/ScaledCSpace.h
@@ -26,6 +26,7 @@
#include <ArmarXCore/core/system/FactoryCollectionBase.h>
#include <RobotComponents/interface/components/MotionPlanning/CSpace/ScaledCSpace.h>
+
#include "CSpace.h"
namespace armarx
@@ -40,9 +41,7 @@ namespace armarx
* @brief Takes an other cspace and scales its' dimensions.
* Can be used when lage translations and rotations are planned at the same time.
*/
- class ScaledCSpace :
- virtual public ScaledCSpaceBase,
- virtual public CSpaceAdaptor
+ class ScaledCSpace : virtual public ScaledCSpaceBase, virtual public CSpaceAdaptor
{
public:
using CSpace::isCollisionFree;
@@ -57,7 +56,8 @@ namespace armarx
/**
* @return The dimensions' scaling factors.
*/
- Ice::FloatSeq getScalingFactors(const ::Ice::Current& = Ice::emptyCurrent)override
+ Ice::FloatSeq
+ getScalingFactors(const ::Ice::Current& = Ice::emptyCurrent) override
{
return scalingFactors;
}
@@ -80,14 +80,17 @@ namespace armarx
* @param cfg The configuration to check.
* @return Checks whether the given configuration is collision free.
*/
- bool isCollisionFree(const::std::pair<const Ice::Float*, const Ice::Float*>& cfg, const Ice::Current& = Ice::emptyCurrent)override;
+ bool isCollisionFree(const ::std::pair<const Ice::Float*, const Ice::Float*>& cfg,
+ const Ice::Current& = Ice::emptyCurrent) override;
/**
* @brief Initializes the collision check.
*/
- void initCollisionTest(const Ice::Current& = Ice::emptyCurrent) override
+ void
+ initCollisionTest(const Ice::Current& = Ice::emptyCurrent) override
{
- ARMARX_CHECK_EXPRESSION(static_cast<std::size_t>(originalCSpace->getDimensionality()) == scalingFactors.size());
+ ARMARX_CHECK_EXPRESSION(static_cast<std::size_t>(originalCSpace->getDimensionality()) ==
+ scalingFactors.size());
unscaled.resize(scalingFactors.size());
originalCSpace->initCollisionTest();
}
@@ -105,13 +108,15 @@ namespace armarx
/**
* @return The cspace's dimensionality.
*/
- Ice::Long getDimensionality(const Ice::Current& = Ice::emptyCurrent) const override
+ Ice::Long
+ getDimensionality(const Ice::Current& = Ice::emptyCurrent) const override
{
return scalingFactors.size();
}
protected:
- template <class IceBaseClass, class DerivedClass> friend class armarx::GenericFactory;
+ template <class IceBaseClass, class DerivedClass>
+ friend class armarx::GenericFactory;
/**
* @brief Default ctor. Used for ice factories.
@@ -131,4 +136,4 @@ namespace armarx
*/
VectorXf unscaled;
};
-}
+} // namespace armarx
diff --git a/source/RobotComponents/components/MotionPlanning/CSpace/SimoxCSpace.cpp b/source/RobotComponents/components/MotionPlanning/CSpace/SimoxCSpace.cpp
index 16e7c496658666c2c43c1fea16b271bff05bfbc3..92bd6b2c92b6aeef243fc88473f2585d618cb00f 100644
--- a/source/RobotComponents/components/MotionPlanning/CSpace/SimoxCSpace.cpp
+++ b/source/RobotComponents/components/MotionPlanning/CSpace/SimoxCSpace.cpp
@@ -22,38 +22,41 @@
* GNU General Public License
*/
-#include <exception>
+#include "SimoxCSpace.h"
+
#include <algorithm>
#include <cmath>
+#include <exception>
#include <unordered_map>
#include <unordered_set>
-#include <MotionPlanning/CSpace/CSpaceSampled.h>
+#include <VirtualRobot/CollisionDetection/CDManager.h>
+#include <VirtualRobot/CollisionDetection/CollisionModel.h>
+#include <VirtualRobot/RobotFactory.h>
+#include <VirtualRobot/RobotNodeSet.h>
+#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>
+#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationNode.h>
+#include <VirtualRobot/XML/RobotIO.h>
+
+#include <ArmarXCore/core/system/ArmarXDataPath.h>
+#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
+#include <ArmarXCore/core/util/algorithm.h>
#include <RobotAPI/libraries/core/Pose.h>
+
+#include "../util/Metrics.h"
+#include <MemoryX/core/MemoryXCoreObjectFactories.h>
#include <MemoryX/libraries/helpers/VirtualRobotHelpers/SimoxObjectWrapper.h>
+#include <MemoryX/libraries/memorytypes/MemoryXTypesObjectFactories.h>
#include <MemoryX/libraries/memorytypes/entity/ObjectClass.h>
#include <MemoryX/libraries/memorytypes/entity/ObjectInstance.h>
-#include <MemoryX/libraries/memorytypes/MemoryXTypesObjectFactories.h>
-#include <MemoryX/core/MemoryXCoreObjectFactories.h>
-#include <VirtualRobot/XML/RobotIO.h>
-#include <VirtualRobot/RobotNodeSet.h>
-#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationNode.h>
-#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>
-#include <RobotAPI/libraries/core/Pose.h>
-#include <MemoryX/libraries/helpers/VirtualRobotHelpers/SimoxObjectWrapper.h>
-#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
-#include <ArmarXCore/core/util/algorithm.h>
-#include <ArmarXCore/core/system/ArmarXDataPath.h>
-#include <VirtualRobot/CollisionDetection/CDManager.h>
-#include <VirtualRobot/RobotFactory.h>
#include <MotionPlanning/CSpace/CSpace.h>
-#include "../util/Metrics.h"
-#include "SimoxCSpace.h"
+#include <MotionPlanning/CSpace/CSpaceSampled.h>
namespace armarx
{
- bool ensureCoinIsInitialized()
+ bool
+ ensureCoinIsInitialized()
{
struct Initializer
{
@@ -67,33 +70,35 @@ namespace armarx
//}
}
- bool doStuff()
+ bool
+ doStuff()
{
return true;
}
};
+
static Initializer init;
return init.doStuff(); //suppresses unused variable
}
-
-
- SimoxCSpace::SimoxCSpace(memoryx::CommonStorageInterfacePrx commonStoragePrx, bool loadVisualizationModel, float stationaryObjectMargin):
- loadVisualizationModel {loadVisualizationModel}
+ SimoxCSpace::SimoxCSpace(memoryx::CommonStorageInterfacePrx commonStoragePrx,
+ bool loadVisualizationModel,
+ float stationaryObjectMargin) :
+ loadVisualizationModel{loadVisualizationModel}
{
this->stationaryObjectMargin = stationaryObjectMargin;
//check eigen layout for mapped vectors
Eigen::Matrix4f a;
- ARMARX_CHECK_EXPRESSION(a.data() + 0 == &a(0, 0));
- ARMARX_CHECK_EXPRESSION(a.data() + 1 == &a(1, 0));
- ARMARX_CHECK_EXPRESSION(a.data() + 2 == &a(2, 0));
- ARMARX_CHECK_EXPRESSION(a.data() + 3 == &a(3, 0));
- ARMARX_CHECK_EXPRESSION(a.data() + 4 == &a(0, 1));
- ARMARX_CHECK_EXPRESSION(a.data() + 5 == &a(1, 1));
- ARMARX_CHECK_EXPRESSION(a.data() + 6 == &a(2, 1));
- ARMARX_CHECK_EXPRESSION(a.data() + 7 == &a(3, 1));
- ARMARX_CHECK_EXPRESSION(a.data() + 8 == &a(0, 2));
- ARMARX_CHECK_EXPRESSION(a.data() + 9 == &a(1, 2));
+ ARMARX_CHECK_EXPRESSION(a.data() + 0 == &a(0, 0));
+ ARMARX_CHECK_EXPRESSION(a.data() + 1 == &a(1, 0));
+ ARMARX_CHECK_EXPRESSION(a.data() + 2 == &a(2, 0));
+ ARMARX_CHECK_EXPRESSION(a.data() + 3 == &a(3, 0));
+ ARMARX_CHECK_EXPRESSION(a.data() + 4 == &a(0, 1));
+ ARMARX_CHECK_EXPRESSION(a.data() + 5 == &a(1, 1));
+ ARMARX_CHECK_EXPRESSION(a.data() + 6 == &a(2, 1));
+ ARMARX_CHECK_EXPRESSION(a.data() + 7 == &a(3, 1));
+ ARMARX_CHECK_EXPRESSION(a.data() + 8 == &a(0, 2));
+ ARMARX_CHECK_EXPRESSION(a.data() + 9 == &a(1, 2));
ARMARX_CHECK_EXPRESSION(a.data() + 10 == &a(2, 2));
ARMARX_CHECK_EXPRESSION(a.data() + 11 == &a(3, 2));
ARMARX_CHECK_EXPRESSION(a.data() + 12 == &a(0, 3));
@@ -106,14 +111,16 @@ namespace armarx
if (!commonStorage)
{
ARMARX_ERROR_S << "SimoxCSpace ctor: commonStorage == null";
- throw std::invalid_argument {"SimoxCSpace ctor: commonStorage == null"};
+ throw std::invalid_argument{"SimoxCSpace ctor: commonStorage == null"};
}
}
- CSpaceBasePtr SimoxCSpace::clone(bool loadVisualizationModel)
+ CSpaceBasePtr
+ SimoxCSpace::clone(bool loadVisualizationModel)
{
TIMING_START(SimoxCSpaceClone);
- SimoxCSpacePtr cloned = new SimoxCSpace {commonStorage, loadVisualizationModel, stationaryObjectMargin};
+ SimoxCSpacePtr cloned =
+ new SimoxCSpace{commonStorage, loadVisualizationModel, stationaryObjectMargin};
for (const auto& obj : stationaryObjects)
{
@@ -126,41 +133,51 @@ namespace armarx
return cloned;
}
- Saba::CSpaceSampledPtr SimoxCSpace::createSimoxCSpace() const
+ Saba::CSpaceSampledPtr
+ SimoxCSpace::createSimoxCSpace() const
{
// ARMARX_INFO << "using kinematic chain set: " << agentInfo.kinemaicChainNames.at(0);
- VirtualRobot::CDManagerPtr tmpCd = VirtualRobot::CDManagerPtr(new VirtualRobot::CDManager(cd));
+ VirtualRobot::CDManagerPtr tmpCd =
+ VirtualRobot::CDManagerPtr(new VirtualRobot::CDManager(cd));
// for (VirtualRobot::SceneObjectSetPtr& set : tmpCd->getSceneObjectSets())
// {
// ARMARX_INFO << "set size: " << set->getSize();
// }
- Saba::CSpaceSampledPtr result(new Saba::CSpaceSampled(agentSceneObj, tmpCd,
- agentInfo.kinemaicChainNames.size() > 0 ? agentSceneObj->getRobotNodeSet(agentInfo.kinemaicChainNames.at(0)) : VirtualRobot::RobotNodeSetPtr()));
+ Saba::CSpaceSampledPtr result(new Saba::CSpaceSampled(
+ agentSceneObj,
+ tmpCd,
+ agentInfo.kinemaicChainNames.size() > 0
+ ? agentSceneObj->getRobotNodeSet(agentInfo.kinemaicChainNames.at(0))
+ : VirtualRobot::RobotNodeSetPtr()));
return result;
}
//from SimoxCSpace
- void SimoxCSpace::addStationaryObject(const StationaryObject& obj, const Ice::Current&)
+ void
+ SimoxCSpace::addStationaryObject(const StationaryObject& obj, const Ice::Current&)
{
ARMARX_CHECK_EXPRESSION(obj.objectPose);
ARMARX_CHECK_EXPRESSION(obj.objectClassBase);
stationaryObjects.emplace_back(obj);
}
- void SimoxCSpace::setStationaryObjects(const StationaryObjectList& objList)
+ void
+ SimoxCSpace::setStationaryObjects(const StationaryObjectList& objList)
{
stationaryObjects = objList;
}
- void SimoxCSpace::setAgent(const AgentPlanningInformation& agentInfo, const Ice::Current&)
+ void
+ SimoxCSpace::setAgent(const AgentPlanningInformation& agentInfo, const Ice::Current&)
{
this->agentInfo = agentInfo;
initAgent();
}
- void SimoxCSpace::setConfig(const float*& it)
+ void
+ SimoxCSpace::setConfig(const float*& it)
{
//stationary agents joints
NameValueMap jointValues;
@@ -173,10 +190,9 @@ namespace armarx
agentSceneObj->applyJointValues();
}
- bool SimoxCSpace::isCollisionFree(
- const ::std::pair<const Ice::Float*, const Ice::Float*>& cfg,
- const Ice::Current&
- )
+ bool
+ SimoxCSpace::isCollisionFree(const ::std::pair<const Ice::Float*, const Ice::Float*>& cfg,
+ const Ice::Current&)
{
ARMARX_CHECK_EXPRESSION(std::distance(cfg.first, cfg.second) == getDimensionality());
const Ice::Float* it;
@@ -189,7 +205,8 @@ namespace armarx
return !cd.isInCollision();
}
- void SimoxCSpace::initCollisionTest(const Ice::Current&)
+ void
+ SimoxCSpace::initCollisionTest(const Ice::Current&)
{
if (isInitialized)
{
@@ -218,9 +235,11 @@ namespace armarx
TIMING_END(CSpaceInit);
}
- void SimoxCSpace::initStationaryObjects()
+ void
+ SimoxCSpace::initStationaryObjects()
{
- stationaryObjectSet.reset(new VirtualRobot::SceneObjectSet {"StationaryObjectSet", cd.getCollisionChecker()});
+ stationaryObjectSet.reset(
+ new VirtualRobot::SceneObjectSet{"StationaryObjectSet", cd.getCollisionChecker()});
if (stationaryObjects.size() || stationaryPlanes.size())
{
ARMARX_CHECK_EXPRESSION(fileManager);
@@ -228,7 +247,8 @@ namespace armarx
{
ARMARX_CHECK_EXPRESSION(i == stationaryObjectSet->getSize());
const auto& obj = stationaryObjects.at(i);
- auto&& sceneObj = getMovedSimoxManipulatorObject(obj.objectClassBase, obj.objectPose, fileManager);
+ auto&& sceneObj = getMovedSimoxManipulatorObject(
+ obj.objectClassBase, obj.objectPose, fileManager);
//deactivate visu since it is not needed and moving visu is as expensive as moving the col mod
sceneObj->setUpdateVisualization(false);
stationaryObjectSet->addSceneObject(std::move(sceneObj));
@@ -236,32 +256,39 @@ namespace armarx
for (auto& plane : stationaryPlanes)
{
- VirtualRobot::CoinVisualizationNodePtr visu(new VirtualRobot::CoinVisualizationNode((SoNode*)VirtualRobot::CoinVisualizationFactory::CreatePolygonVisualization(plane)));
- VirtualRobot::SceneObjectPtr s(new VirtualRobot::SceneObject("Plane", visu,
- VirtualRobot::CollisionModelPtr(new VirtualRobot::CollisionModel(visu)),
- VirtualRobot::SceneObject::Physics {}, cd.getCollisionChecker()));
+ VirtualRobot::CoinVisualizationNodePtr visu(new VirtualRobot::CoinVisualizationNode(
+ (SoNode*)VirtualRobot::CoinVisualizationFactory::CreatePolygonVisualization(
+ plane)));
+ VirtualRobot::SceneObjectPtr s(new VirtualRobot::SceneObject(
+ "Plane",
+ visu,
+ VirtualRobot::CollisionModelPtr(new VirtualRobot::CollisionModel(visu)),
+ VirtualRobot::SceneObject::Physics{},
+ cd.getCollisionChecker()));
stationaryObjectSet->addSceneObject(s);
}
if (stationaryObjectSet->getSize())
{
- ARMARX_INFO << "SceneObjects: " << armarx::transform(stationaryObjectSet->getSceneObjects(), +[](VirtualRobot::SceneObjectPtr const & obj)
- {
- return obj->getName();
- });
+ ARMARX_INFO << "SceneObjects: "
+ << armarx::transform(
+ stationaryObjectSet->getSceneObjects(),
+ +[](VirtualRobot::SceneObjectPtr const& obj)
+ { return obj->getName(); });
ARMARX_CHECK_EXPRESSION(!cd.hasSceneObjectSet(stationaryObjectSet));
cd.addCollisionModel(stationaryObjectSet);
}
}
-
}
- void SimoxCSpace::initAgent(AgentData agentData)
+ void
+ SimoxCSpace::initAgent(AgentData agentData)
{
//move agent
if (agentInfo.agentPose) //if none is set the default pose is used
{
- agentData.agent->setGlobalPose(armarx::PosePtr::dynamicCast(agentInfo.agentPose)->toEigen());
+ agentData.agent->setGlobalPose(
+ armarx::PosePtr::dynamicCast(agentInfo.agentPose)->toEigen());
}
agentSceneObj = std::move(agentData.agent);
@@ -271,11 +298,12 @@ namespace armarx
const std::size_t numberOfJoints = agentData.joints.size();
agentJoints.clear();
agentJoints.reserve(numberOfJoints);
- std::move(agentData.joints.begin(), agentData.joints.end(), std::back_inserter(agentJoints));
+ std::move(
+ agentData.joints.begin(), agentData.joints.end(), std::back_inserter(agentJoints));
if (isInitialized)
{
- cd = VirtualRobot::CDManager {agentSceneObj->getCollisionChecker()};
+ cd = VirtualRobot::CDManager{agentSceneObj->getCollisionChecker()};
for (auto& colMod : agentData.collisionSets)
{
cd.addCollisionModel(std::move(colMod));
@@ -284,7 +312,8 @@ namespace armarx
}
}
- SimoxCSpace::AgentData SimoxCSpace::getAgentDataAndLoadRobot() const
+ SimoxCSpace::AgentData
+ SimoxCSpace::getAgentDataAndLoadRobot() const
{
//load agent
VirtualRobot::RobotPtr agent;
@@ -307,32 +336,30 @@ namespace armarx
std::move(packagePaths.begin(), packagePaths.end(), std::back_inserter(paths));
}
- if (!armarx::ArmarXDataPath::getAbsolutePath(agentInfo.agentRelativeFilePath, absoluteFilePath, paths))
+ if (!armarx::ArmarXDataPath::getAbsolutePath(
+ agentInfo.agentRelativeFilePath, absoluteFilePath, paths))
{
std::stringstream s;
- s << "could not find file " << agentInfo.agentRelativeFilePath << " in project " << agentInfo.agentProjectName;
+ s << "could not find file " << agentInfo.agentRelativeFilePath << " in project "
+ << agentInfo.agentProjectName;
ARMARX_ERROR_S << s.str();
- throw std::invalid_argument {s.str()};
+ throw std::invalid_argument{s.str()};
}
// static std::map<std::pair<VirtualRobot::RobotIO::RobotDescription, std::string>, VirtualRobot::RobotPtr> agentCache;
// static armarx::Mutex agentCacheMutex;
// ScopedLock lock(agentCacheMutex);
- auto loadType = isInitialized ?//the cspace is initializing => load models
- (
- loadVisualizationModel ?
- VirtualRobot::RobotIO::eFull :
- VirtualRobot::RobotIO::eCollisionModel
- ) : VirtualRobot::RobotIO::eStructure;
+ auto loadType = isInitialized ? //the cspace is initializing => load models
+ (loadVisualizationModel ? VirtualRobot::RobotIO::eFull
+ : VirtualRobot::RobotIO::eCollisionModel)
+ : VirtualRobot::RobotIO::eStructure;
auto pair = std::make_pair(loadType, absoluteFilePath);
// auto it = robotCache.find(pair);
if (!robotCache.hasObject(pair))
{
- agent = VirtualRobot::RobotIO::loadRobot(
- absoluteFilePath,
- loadType
- );
+ agent = VirtualRobot::RobotIO::loadRobot(absoluteFilePath, loadType);
- ARMARX_DEBUG << "Robot Cache MISS! Path: " << absoluteFilePath << " load type: " << (int)loadType;
+ ARMARX_DEBUG << "Robot Cache MISS! Path: " << absoluteFilePath
+ << " load type: " << (int)loadType;
RobotPoolPtr pool(new RobotPool(agent, 2));
agent = pool->getRobot();
robotCache.insertObject(pair, pool);
@@ -360,14 +387,15 @@ namespace armarx
std::stringstream s;
s << "Can't load agent from: " << absoluteFilePath;
ARMARX_ERROR_S << s.str();
- throw std::invalid_argument {s.str()};
+ throw std::invalid_argument{s.str()};
}
}
return getAgentDataFromRobot(agent);
}
- SimoxCSpace::AgentData SimoxCSpace::getAgentDataFromRobot(VirtualRobot::RobotPtr robotPtr) const
+ SimoxCSpace::AgentData
+ SimoxCSpace::getAgentDataFromRobot(VirtualRobot::RobotPtr robotPtr) const
{
AgentData data;
@@ -379,72 +407,82 @@ namespace armarx
ARMARX_CHECK_EXPRESSION(fileManager);
//attach objects
//use the map to track this association
- std::unordered_map<std::string, std::vector<VirtualRobot::RobotNodePtr>> toCollSetAssociatedObjects;
+ std::unordered_map<std::string, std::vector<VirtualRobot::RobotNodePtr>>
+ toCollSetAssociatedObjects;
toCollSetAssociatedObjects.reserve(agentInfo.collisionSetNames.size());
for (std::size_t i = 0; i < agentInfo.attachedObjects.size(); ++i)
{
const auto& attached = agentInfo.attachedObjects.at(i);
- auto&& sceneObj = getSimoxManipulatorObject(attached.objectClassBase, fileManager, false, robotPtr->getCollisionChecker());
+ auto&& sceneObj = getSimoxManipulatorObject(
+ attached.objectClassBase, fileManager, false, robotPtr->getCollisionChecker());
std::stringstream sceneObjName;
sceneObjName << "attached_obj_" << i << "_" << sceneObj->getName();
sceneObj->setName(sceneObjName.str());
//find node to attach to and attach
- if (! data.agent->hasRobotNode(attached.attachedToRobotNodeName))
+ if (!data.agent->hasRobotNode(attached.attachedToRobotNodeName))
{
std::stringstream s;
- s << "Agent " << data.agent->getName() << " has no node " << attached.attachedToRobotNodeName
- << "to attach object " << attached.objectClassBase->getName();
+ s << "Agent " << data.agent->getName() << " has no node "
+ << attached.attachedToRobotNodeName << "to attach object "
+ << attached.objectClassBase->getName();
ARMARX_ERROR_S << s.str();
- throw std::invalid_argument {s.str()};
+ throw std::invalid_argument{s.str()};
}
- VirtualRobot::RobotNodePtr nodeToAttachTo = data.agent->getRobotNode(attached.attachedToRobotNodeName);
- VirtualRobot::RobotFactory::attach(data.agent, sceneObj, nodeToAttachTo, armarx::PosePtr::dynamicCast(attached.relativeObjectPose)->toEigen());
+ VirtualRobot::RobotNodePtr nodeToAttachTo =
+ data.agent->getRobotNode(attached.attachedToRobotNodeName);
+ VirtualRobot::RobotFactory::attach(
+ data.agent,
+ sceneObj,
+ nodeToAttachTo,
+ armarx::PosePtr::dynamicCast(attached.relativeObjectPose)->toEigen());
//adding to cd
if (attached.associatedCollisionSetName.empty())
{
//add alone
- data.collisionSets.emplace_back(new VirtualRobot::SceneObjectSet {sceneObjName.str(), data.agent->getCollisionChecker()});
- data.collisionSets.back()->addSceneObject(data.agent->getRobotNode(sceneObjName.str()));
+ data.collisionSets.emplace_back(new VirtualRobot::SceneObjectSet{
+ sceneObjName.str(), data.agent->getCollisionChecker()});
+ data.collisionSets.back()->addSceneObject(
+ data.agent->getRobotNode(sceneObjName.str()));
}
else
{
//does the set exist?
- if (
- std::find(
- agentInfo.collisionSetNames.begin(),
- agentInfo.collisionSetNames.end(),
- attached.associatedCollisionSetName
- )
- == agentInfo.collisionSetNames.end()
- )
+ if (std::find(agentInfo.collisionSetNames.begin(),
+ agentInfo.collisionSetNames.end(),
+ attached.associatedCollisionSetName) ==
+ agentInfo.collisionSetNames.end())
{
std::stringstream s;
- s << "Agent " << data.agent->getName() << " has no set " << attached.associatedCollisionSetName
- << " to use as associated collision set for attached object " << attached.objectClassBase->getName();
+ s << "Agent " << data.agent->getName() << " has no set "
+ << attached.associatedCollisionSetName
+ << " to use as associated collision set for attached object "
+ << attached.objectClassBase->getName();
ARMARX_ERROR_S << s.str();
- throw std::invalid_argument {s.str()};
+ throw std::invalid_argument{s.str()};
}
//add to set
- toCollSetAssociatedObjects[attached.associatedCollisionSetName].emplace_back(data.agent->getRobotNode(sceneObjName.str()));
+ toCollSetAssociatedObjects[attached.associatedCollisionSetName].emplace_back(
+ data.agent->getRobotNode(sceneObjName.str()));
}
}
//get collision models
for (const auto& name : agentInfo.collisionSetNames)
{
- if (! data.agent->hasRobotNodeSet(name))
+ if (!data.agent->hasRobotNodeSet(name))
{
std::stringstream s;
s << "Agent " << data.agent->getName() << " has no collision model " << name;
ARMARX_ERROR_S << s.str();
- throw std::invalid_argument {s.str()};
+ throw std::invalid_argument{s.str()};
}
//add set + associated objects for cd
- data.collisionSets.emplace_back(new VirtualRobot::SceneObjectSet {name, data.agent->getCollisionChecker()});
+ data.collisionSets.emplace_back(
+ new VirtualRobot::SceneObjectSet{name, data.agent->getCollisionChecker()});
data.collisionSets.back()->addSceneObjects(data.agent->getRobotNodeSet(name));
for (auto& obj : toCollSetAssociatedObjects[name])
@@ -454,15 +492,17 @@ namespace armarx
}
if (!agentInfo.collisionObjectNames.empty())
{
- data.collisionSets.emplace_back(new VirtualRobot::SceneObjectSet {"collisionObjects", data.agent->getCollisionChecker()});
+ data.collisionSets.emplace_back(new VirtualRobot::SceneObjectSet{
+ "collisionObjects", data.agent->getCollisionChecker()});
for (const auto& name : agentInfo.collisionObjectNames)
{
- if (! data.agent->hasRobotNode(name))
+ if (!data.agent->hasRobotNode(name))
{
std::stringstream s;
- s << "Agent " << data.agent->getName() << " has no collision model " << name;
+ s << "Agent " << data.agent->getName() << " has no collision model "
+ << name;
ARMARX_ERROR_S << s.str();
- throw std::invalid_argument {s.str()};
+ throw std::invalid_argument{s.str()};
}
//add set + associated objects for cd
@@ -478,20 +518,21 @@ namespace armarx
//get joints
//some joints may be blacklisted or already added
- std::set<std::string> blacklist(agentInfo.jointsExcludedFromPlanning.begin(), agentInfo.jointsExcludedFromPlanning.end());
+ std::set<std::string> blacklist(agentInfo.jointsExcludedFromPlanning.begin(),
+ agentInfo.jointsExcludedFromPlanning.end());
//add kinematic chains
for (const auto& name : agentInfo.kinemaicChainNames)
{
- if (! data.agent->hasRobotNodeSet(name))
+ if (!data.agent->hasRobotNodeSet(name))
{
std::stringstream s;
s << "Agent " << data.agent->getName() << " has no kinematic chain " << name;
ARMARX_ERROR_S << s.str();
- throw std::invalid_argument {s.str()};
+ throw std::invalid_argument{s.str()};
}
//add chain
- for (auto& node : * (data.agent->getRobotNodeSet(name)))
+ for (auto& node : *(data.agent->getRobotNodeSet(name)))
{
auto nodeIt = blacklist.find(node->getName());
if (nodeIt != blacklist.end())
@@ -502,11 +543,11 @@ namespace armarx
if (!(node->isTranslationalJoint() || node->isRotationalJoint()))
{
std::stringstream s;
- s << "The node " << node->getName() << " in the kinematic chain "
- << name << " of agent " << data.agent->getName()
+ s << "The node " << node->getName() << " in the kinematic chain " << name
+ << " of agent " << data.agent->getName()
<< " is neither rotational nor tranlational";
ARMARX_ERROR_S << s.str();
- throw std::invalid_argument {s.str()};
+ throw std::invalid_argument{s.str()};
}
data.joints.emplace_back(node);
@@ -521,7 +562,7 @@ namespace armarx
std::stringstream s;
s << "Agent " << data.agent->getName() << " has no node " << name;
ARMARX_ERROR_S << s.str();
- throw std::invalid_argument {s.str()};
+ throw std::invalid_argument{s.str()};
}
auto node = data.agent->getRobotNode(name);
@@ -534,11 +575,10 @@ namespace armarx
if (!(node->isTranslationalJoint() || node->isRotationalJoint()))
{
std::stringstream s;
- s << "The node " << node->getName()
- << " of agent " << data.agent->getName()
+ s << "The node " << node->getName() << " of agent " << data.agent->getName()
<< " is neither rotational nor tranlational";
ARMARX_ERROR_S << s.str();
- throw std::invalid_argument {s.str()};
+ throw std::invalid_argument{s.str()};
}
data.joints.emplace_back(node);
@@ -547,7 +587,12 @@ namespace armarx
return data;
}
- VirtualRobot::ManipulationObjectPtr SimoxCSpace::getSimoxManipulatorObject(const memoryx::ObjectClassBasePtr& object, const memoryx::GridFileManagerPtr& fileManager, bool inflate, VirtualRobot::CollisionCheckerPtr const& colChecker) const
+ VirtualRobot::ManipulationObjectPtr
+ SimoxCSpace::getSimoxManipulatorObject(
+ const memoryx::ObjectClassBasePtr& object,
+ const memoryx::GridFileManagerPtr& fileManager,
+ bool inflate,
+ VirtualRobot::CollisionCheckerPtr const& colChecker) const
{
memoryx::ObjectClassPtr objectClass = memoryx::ObjectClassPtr::dynamicCast(object);
// static boost::mutex meshCacheMutex;
@@ -559,7 +604,7 @@ namespace armarx
std::stringstream s;
s << "Can't use object class with ice id " << object->ice_id();
ARMARX_ERROR_S << s.str();
- throw armarx::InvalidArgumentException {s.str()};
+ throw armarx::InvalidArgumentException{s.str()};
}
{
// boost::mutex::scoped_lock lock(meshCacheMutex);
@@ -568,47 +613,57 @@ namespace armarx
if (meshCache.hasObject(pair))
{
- auto tmpMo = meshCache.getCacheObject(pair);
+ auto tmpMo = meshCache.getCacheObject(pair);
IceUtil::Time start = IceUtil::Time::now();
mo = tmpMo->clone(tmpMo->getName(), colChecker, true);
IceUtil::Time end = IceUtil::Time::now();
- ARMARX_DEBUG << "mesh Cache hit for " << tmpMo->getName() << " - Cloning took " << (end - start).toMilliSeconds();
+ ARMARX_DEBUG << "mesh Cache hit for " << tmpMo->getName() << " - Cloning took "
+ << (end - start).toMilliSeconds();
}
else
{
if (meshCache.hasObject(pairZeroMargin))
{
- auto tmpMo = meshCache.getCacheObject(pairZeroMargin);
- ARMARX_DEBUG << "mesh Cache HALFMISS - mesh inflation needed for " << tmpMo->getName();
+ auto tmpMo = meshCache.getCacheObject(pairZeroMargin);
+ ARMARX_DEBUG << "mesh Cache HALFMISS - mesh inflation needed for "
+ << tmpMo->getName();
mo = tmpMo->clone(tmpMo->getName(), colChecker, true);
}
else
{
- sw = objectClass->addWrapper(new memoryx::EntityWrappers::SimoxObjectWrapper(fileManager, VirtualRobot::RobotIO::eCollisionModel));
+ sw = objectClass->addWrapper(new memoryx::EntityWrappers::SimoxObjectWrapper(
+ fileManager, VirtualRobot::RobotIO::eCollisionModel));
VirtualRobot::ManipulationObjectPtr orgMo = sw->getManipulationObject();
ARMARX_DEBUG << "mesh Cache MISS for " << orgMo->getName();
ARMARX_CHECK_EXPRESSION(orgMo);
std::string moName = orgMo->getName();
- mo = orgMo->clone(moName, colChecker, true);
+ mo = orgMo->clone(moName, colChecker, true);
}
if (this->stationaryObjectMargin != 0.0f && inflate)
{
- ARMARX_DEBUG << deactivateSpam(3, to_string(stationaryObjectMargin)) << "Using " << stationaryObjectMargin << " as margin";
+ ARMARX_DEBUG << deactivateSpam(3, to_string(stationaryObjectMargin)) << "Using "
+ << stationaryObjectMargin << " as margin";
mo->getCollisionModel()->inflateModel(this->stationaryObjectMargin);
}
meshCache.insertObject(pair, mo);
}
-
}
return mo;
}
- VirtualRobot::ManipulationObjectPtr SimoxCSpace::getMovedSimoxManipulatorObject(const memoryx::ObjectClassBasePtr& object, const PoseBasePtr& pose, memoryx::GridFileManagerPtr& fileManager) const
+ VirtualRobot::ManipulationObjectPtr
+ SimoxCSpace::getMovedSimoxManipulatorObject(const memoryx::ObjectClassBasePtr& object,
+ const PoseBasePtr& pose,
+ memoryx::GridFileManagerPtr& fileManager) const
{
- VirtualRobot::ManipulationObjectPtr mo = getSimoxManipulatorObject(object, fileManager, true, const_cast<VirtualRobot::CDManager*>(&cd)->getCollisionChecker());
+ VirtualRobot::ManipulationObjectPtr mo = getSimoxManipulatorObject(
+ object,
+ fileManager,
+ true,
+ const_cast<VirtualRobot::CDManager*>(&cd)->getCollisionChecker());
//move the object to the given position
const auto objectPose = armarx::PosePtr::dynamicCast(pose);
@@ -616,16 +671,20 @@ namespace armarx
if (!objectPose)
{
std::stringstream s;
- s << "Can't convert pose of " << memoryx::ObjectClassPtr::dynamicCast(object)->getName() << " to armarx::Pose.";
+ s << "Can't convert pose of " << memoryx::ObjectClassPtr::dynamicCast(object)->getName()
+ << " to armarx::Pose.";
ARMARX_ERROR_S << s.str();
- throw armarx::InvalidArgumentException {s.str()};
+ throw armarx::InvalidArgumentException{s.str()};
}
mo->setGlobalPose(objectPose->toEigen());
return mo;
}
- void SimoxCSpace::addObjectsFromWorkingMemory(memoryx::WorkingMemoryInterfacePrx workingMemoryPrx, const std::vector<std::string>& excludedInstancesIds /* = std::vector<std::string>() */)
+ void
+ SimoxCSpace::addObjectsFromWorkingMemory(
+ memoryx::WorkingMemoryInterfacePrx workingMemoryPrx,
+ const std::vector<std::string>& excludedInstancesIds /* = std::vector<std::string>() */)
{
TIMING_START(LoadFromWM);
//pass all objects from the scene to the cspace
@@ -636,41 +695,45 @@ namespace armarx
for (const auto& entityBase : objs)
{
auto id = entityBase->getId();
- if (std::find(excludedInstancesIds.cbegin(), excludedInstancesIds.cend(), id) != excludedInstancesIds.cend())
+ if (std::find(excludedInstancesIds.cbegin(), excludedInstancesIds.cend(), id) !=
+ excludedInstancesIds.cend())
{
continue;
}
- const memoryx::ObjectInstancePtr object = memoryx::ObjectInstancePtr::dynamicCast(entityBase);
+ const memoryx::ObjectInstancePtr object =
+ memoryx::ObjectInstancePtr::dynamicCast(entityBase);
auto objPose = object->getPose();
if (objPose->frame != GlobalFrame && !objPose->frame.empty())
{
- objPose = FramedPosePtr::dynamicCast(agentSeg->convertToWorldPose(objPose->agent, objPose));
+ objPose = FramedPosePtr::dynamicCast(
+ agentSeg->convertToWorldPose(objPose->agent, objPose));
}
ARMARX_CHECK_EXPRESSION(object);
const std::string className = object->getMostProbableClass();
- memoryx::ObjectClassList classes = workingMemoryPrx->getPriorKnowledge()->getObjectClassesSegment()->getClassWithSubclasses(className);
+ memoryx::ObjectClassList classes = workingMemoryPrx->getPriorKnowledge()
+ ->getObjectClassesSegment()
+ ->getClassWithSubclasses(className);
if (!classes.size())
{
- ARMARX_INFO_S << "No classes for most probable class '" << className << "' of object '" << object->getName() << "' with id " << id;
+ ARMARX_INFO_S << "No classes for most probable class '" << className
+ << "' of object '" << object->getName() << "' with id " << id;
continue;
}
- addStationaryObject(
- {
- classes.at(0),
- armarx::PoseBasePtr{objPose}
- });
+ addStationaryObject({classes.at(0), armarx::PoseBasePtr{objPose}});
}
TIMING_END(LoadFromWM);
}
-
- SimoxCSpacePtr SimoxCSpace::PrefetchWorkingMemoryObjects(memoryx::WorkingMemoryInterfacePrx workingMemoryPrx, memoryx::CommonStorageInterfacePrx commonStoragePrx, armarx::RobotStateComponentInterfacePrx rsc)
+ SimoxCSpacePtr
+ SimoxCSpace::PrefetchWorkingMemoryObjects(memoryx::WorkingMemoryInterfacePrx workingMemoryPrx,
+ memoryx::CommonStorageInterfacePrx commonStoragePrx,
+ armarx::RobotStateComponentInterfacePrx rsc)
{
SimoxCSpacePtr cspace(new SimoxCSpace(commonStoragePrx));
@@ -685,25 +748,28 @@ namespace armarx
return cspace;
}
- void SimoxCSpace::addPlanarObject(const std::vector<Eigen::Vector3f>& convexHull)
+ void
+ SimoxCSpace::addPlanarObject(const std::vector<Eigen::Vector3f>& convexHull)
{
stationaryPlanes.push_back(convexHull);
}
- void SimoxCSpace::ice_postUnmarshal()
+ void
+ SimoxCSpace::ice_postUnmarshal()
{
if (!commonStorage)
{
ARMARX_ERROR_S << "SimoxCSpace ice_postUnmarshal: commonStorage == null";
- throw std::invalid_argument {"SimoxCSpace ice_postUnmarshal: commonStorage == null"};
+ throw std::invalid_argument{"SimoxCSpace ice_postUnmarshal: commonStorage == null"};
}
//should throw no direct exception since they were prevented when adding an agent
initAgent();
}
- template<class T, class Thrower>
- std::vector<std::vector<T>> transpose(const std::vector<std::vector<T>>& src, Thrower thrower)
+ template <class T, class Thrower>
+ std::vector<std::vector<T>>
+ transpose(const std::vector<std::vector<T>>& src, Thrower thrower)
{
std::size_t numDatapoints = src.size();
if (!numDatapoints)
@@ -727,23 +793,22 @@ namespace armarx
return transposed;
}
- template<class T>
- std::vector<std::vector<T>> transpose(const std::vector<std::vector<T>>& src)
+ template <class T>
+ std::vector<std::vector<T>>
+ transpose(const std::vector<std::vector<T>>& src)
{
- return transpose(
- src,
- [](std::size_t idx, std::size_t szi, std::size_t numDim)
- {
- std::stringstream ss;
- ss << "transpose: Element " << idx
- << " has " << szi << " dimensions. The Element 0 has "
- << numDim << "dimensions.";
- throw std::invalid_argument {ss.str()};
- }
- );
+ return transpose(src,
+ [](std::size_t idx, std::size_t szi, std::size_t numDim)
+ {
+ std::stringstream ss;
+ ss << "transpose: Element " << idx << " has " << szi
+ << " dimensions. The Element 0 has " << numDim << "dimensions.";
+ throw std::invalid_argument{ss.str()};
+ });
}
- TrajectoryPtr SimoxCSpace::pathToTrajectory(const VectorXfSeq& p) const
+ TrajectoryPtr
+ SimoxCSpace::pathToTrajectory(const VectorXfSeq& p) const
{
if (p.empty())
{
@@ -752,16 +817,16 @@ namespace armarx
auto thrower = [](std::size_t idx, std::size_t szi, std::size_t numDim)
{
std::stringstream ss;
- ss << "SimoxCSpace::pathToTrajectory: The datapoint " << idx
- << " has " << szi << " dimensions. The CSpace has "
- << numDim << "dimensions.";
- throw std::invalid_argument {ss.str()};
+ ss << "SimoxCSpace::pathToTrajectory: The datapoint " << idx << " has " << szi
+ << " dimensions. The CSpace has " << numDim << "dimensions.";
+ throw std::invalid_argument{ss.str()};
};
if (static_cast<std::size_t>(getDimensionality()) != p.front().size())
{
thrower(0, p.front().size(), static_cast<std::size_t>(getDimensionality()));
}
- TrajectoryPtr traj = new Trajectory {transpose(p, thrower), Ice::DoubleSeq() /*timestamps*/, getAgentJointNames()};
+ TrajectoryPtr traj = new Trajectory{
+ transpose(p, thrower), Ice::DoubleSeq() /*timestamps*/, getAgentJointNames()};
LimitlessStateSeq limitlessStates;
for (auto& joint : getAgentJoints())
{
@@ -775,21 +840,22 @@ namespace armarx
return traj;
}
- FloatRangeSeq SimoxCSpace::getDimensionsBounds(const Ice::Current&) const
+ FloatRangeSeq
+ SimoxCSpace::getDimensionsBounds(const Ice::Current&) const
{
FloatRangeSeq dims;
dims.reserve(getDimensionality());
- std::transform(
- agentJoints.begin(), agentJoints.end(), std::back_inserter(dims),
- [](const VirtualRobot::RobotNodePtr & joint)
- {
- return FloatRange {joint->getJointLimitLo(), joint->getJointLimitHi()};
- }
- );
+ std::transform(agentJoints.begin(),
+ agentJoints.end(),
+ std::back_inserter(dims),
+ [](const VirtualRobot::RobotNodePtr& joint) {
+ return FloatRange{joint->getJointLimitLo(), joint->getJointLimitHi()};
+ });
return dims;
}
- Ice::StringSeq SimoxCSpace::getAgentJointNames() const
+ Ice::StringSeq
+ SimoxCSpace::getAgentJointNames() const
{
Ice::StringSeq result;
result.reserve(getAgentJoints().size());
@@ -800,7 +866,9 @@ namespace armarx
return result;
}
- VectorXf SimoxCSpace::jointMapToVector(const std::map<std::string, float>& jointMap, bool checkForNonexistenJointsInCspace) const
+ VectorXf
+ SimoxCSpace::jointMapToVector(const std::map<std::string, float>& jointMap,
+ bool checkForNonexistenJointsInCspace) const
{
VectorXf result(getDimensionality(), 0.f);
std::size_t valuesFromMapUsed = 0;
@@ -816,20 +884,26 @@ namespace armarx
}
if (checkForNonexistenJointsInCspace && (valuesFromMapUsed != jointMap.size()))
{
- throw std::invalid_argument {"the joint map contained a joint not contained by the cspace!"};
+ throw std::invalid_argument{
+ "the joint map contained a joint not contained by the cspace!"};
}
return result;
}
- SimoxCSpaceWith2DPose::SimoxCSpaceWith2DPose(memoryx::CommonStorageInterfacePrx commonStoragePrx, bool loadVisualizationModel, float stationaryObjectMargin):
+ SimoxCSpaceWith2DPose::SimoxCSpaceWith2DPose(
+ memoryx::CommonStorageInterfacePrx commonStoragePrx,
+ bool loadVisualizationModel,
+ float stationaryObjectMargin) :
SimoxCSpace(commonStoragePrx, loadVisualizationModel, stationaryObjectMargin)
{
ARMARX_CHECK_EXPRESSION(commonStoragePrx);
}
- CSpaceBasePtr SimoxCSpaceWith2DPose::clone(bool loadVisualizationModel)
+ CSpaceBasePtr
+ SimoxCSpaceWith2DPose::clone(bool loadVisualizationModel)
{
- SimoxCSpaceWith2DPosePtr cloned = new SimoxCSpaceWith2DPose {commonStorage, loadVisualizationModel, stationaryObjectMargin};
+ SimoxCSpaceWith2DPosePtr cloned = new SimoxCSpaceWith2DPose{
+ commonStorage, loadVisualizationModel, stationaryObjectMargin};
for (const auto& obj : stationaryObjects)
{
cloned->addStationaryObject(obj);
@@ -840,14 +914,15 @@ namespace armarx
return cloned;
}
- FloatRangeSeq SimoxCSpaceWith2DPose::getDimensionsBounds(const Ice::Current&) const
+ FloatRangeSeq
+ SimoxCSpaceWith2DPose::getDimensionsBounds(const Ice::Current&) const
{
FloatRangeSeq bounds(getDimensionality());
- bounds.at(0).min = poseBounds.min.e0 ;
- bounds.at(0).max = poseBounds.max.e0 ;
+ bounds.at(0).min = poseBounds.min.e0;
+ bounds.at(0).max = poseBounds.max.e0;
- bounds.at(1).min = poseBounds.min.e1 ;
- bounds.at(1).max = poseBounds.max.e1 ;
+ bounds.at(1).min = poseBounds.min.e1;
+ bounds.at(1).max = poseBounds.max.e1;
bounds.at(2).min = poseBounds.min.e2;
bounds.at(2).max = poseBounds.max.e2;
@@ -857,7 +932,8 @@ namespace armarx
return bounds;
}
- Ice::StringSeq SimoxCSpaceWith2DPose::getAgentJointNames() const
+ Ice::StringSeq
+ SimoxCSpaceWith2DPose::getAgentJointNames() const
{
auto joints = SimoxCSpace::getAgentJointNames();
joints.resize(joints.size() + 3);
@@ -868,10 +944,11 @@ namespace armarx
return joints;
}
- void SimoxCSpaceWith2DPose::setConfig(const float*& it)
+ void
+ SimoxCSpaceWith2DPose::setConfig(const float*& it)
{
- globalPoseBuffer(0, 3) = *(it++) ;
- globalPoseBuffer(1, 3) = *(it++) ;
+ globalPoseBuffer(0, 3) = *(it++);
+ globalPoseBuffer(1, 3) = *(it++);
const auto rot = *(it++);
@@ -890,15 +967,20 @@ namespace armarx
SimoxCSpace::setConfig(it);
}
- SimoxCSpaceWith3DPose::SimoxCSpaceWith3DPose(memoryx::CommonStorageInterfacePrx commonStoragePrx, bool loadVisualizationModel, float stationaryObjectMargin):
+ SimoxCSpaceWith3DPose::SimoxCSpaceWith3DPose(
+ memoryx::CommonStorageInterfacePrx commonStoragePrx,
+ bool loadVisualizationModel,
+ float stationaryObjectMargin) :
SimoxCSpace(commonStoragePrx, loadVisualizationModel, stationaryObjectMargin)
{
ARMARX_CHECK_EXPRESSION(commonStoragePrx);
}
- CSpaceBasePtr SimoxCSpaceWith3DPose::clone(bool loadVisualizationModel)
+ CSpaceBasePtr
+ SimoxCSpaceWith3DPose::clone(bool loadVisualizationModel)
{
- SimoxCSpaceWith3DPosePtr cloned = new SimoxCSpaceWith3DPose {commonStorage, loadVisualizationModel, stationaryObjectMargin};
+ SimoxCSpaceWith3DPosePtr cloned = new SimoxCSpaceWith3DPose{
+ commonStorage, loadVisualizationModel, stationaryObjectMargin};
for (const auto& obj : stationaryObjects)
{
cloned->addStationaryObject(obj);
@@ -909,7 +991,8 @@ namespace armarx
return cloned;
}
- FloatRangeSeq SimoxCSpaceWith3DPose::getDimensionsBounds(const Ice::Current&) const
+ FloatRangeSeq
+ SimoxCSpaceWith3DPose::getDimensionsBounds(const Ice::Current&) const
{
FloatRangeSeq bounds(getDimensionality());
bounds.at(0).min = poseBounds.min.e0;
@@ -934,7 +1017,8 @@ namespace armarx
return bounds;
}
- void SimoxCSpaceWith3DPose::setConfig(const float*& it)
+ void
+ SimoxCSpaceWith3DPose::setConfig(const float*& it)
{
globalPoseBuffer(0, 3) = *(it++);
globalPoseBuffer(1, 3) = *(it++);
@@ -971,7 +1055,8 @@ namespace armarx
SimoxCSpace::setConfig(it);
}
- Ice::StringSeq SimoxCSpaceWith3DPose::getAgentJointNames() const
+ Ice::StringSeq
+ SimoxCSpaceWith3DPose::getAgentJointNames() const
{
auto joints = SimoxCSpace::getAgentJointNames();
joints.resize(joints.size() + 6);
@@ -985,11 +1070,14 @@ namespace armarx
return joints;
}
- std::vector<armarx::DebugDrawerLineSet> SimoxCSpace::getTraceVisu(const armarx::VectorXfSeq& vecs, const std::vector<std::string>& nodeNames, float traceStepSize)
+ std::vector<armarx::DebugDrawerLineSet>
+ SimoxCSpace::getTraceVisu(const armarx::VectorXfSeq& vecs,
+ const std::vector<std::string>& nodeNames,
+ float traceStepSize)
{
if (traceStepSize <= 0)
{
- throw std::invalid_argument {"SimoxCSpace::getTraces: traceStepSize <= 0"};
+ throw std::invalid_argument{"SimoxCSpace::getTraces: traceStepSize <= 0"};
}
std::vector<armarx::DebugDrawerLineSet> traces;
if (nodeNames.empty())
@@ -1008,7 +1096,8 @@ namespace armarx
const auto& name = nodeNames.at(i);
armarx::DebugDrawerLineSet& trace = traces.at(i);
const auto pose = getAgentSceneObj()->getRobotNode(name)->getGlobalPose();
- trace.points.emplace_back(DebugDrawerPointCloudElement {pose(0, 3), pose(1, 3), pose(2, 3)});
+ trace.points.emplace_back(
+ DebugDrawerPointCloudElement{pose(0, 3), pose(1, 3), pose(2, 3)});
};
};
for (std::size_t vecI = 0; vecI < vecs.size() - 1; ++vecI)
@@ -1021,32 +1110,32 @@ namespace armarx
if (dist > traceStepSize)
{
VectorXf vecCfg = vecStart;
- Eigen::Map<const Eigen::VectorXf> eStart {vecStart.data(), getDimensionality()};
- Eigen::Map<const Eigen::VectorXf> eEnd {vecEnd.data(), getDimensionality()};
- Eigen::Map<Eigen::VectorXf> eCfg {vecCfg.data(), getDimensionality()};
+ Eigen::Map<const Eigen::VectorXf> eStart{vecStart.data(), getDimensionality()};
+ Eigen::Map<const Eigen::VectorXf> eEnd{vecEnd.data(), getDimensionality()};
+ Eigen::Map<Eigen::VectorXf> eCfg{vecCfg.data(), getDimensionality()};
Eigen::VectorXf eStep = (eEnd - eStart).normalized() * traceStepSize;
setConfig(vecCfg);
while (dist > traceStepSize)
{
- addPoints();//start
+ addPoints(); //start
eCfg += eStep;
setConfig(vecCfg);
- addPoints();//end
+ addPoints(); //end
dist -= traceStepSize;
}
- addPoints();//start
+ addPoints(); //start
setConfig(vecEnd);
- addPoints();//end
+ addPoints(); //end
}
else
{
setConfig(vecStart);
- addPoints();//start
+ addPoints(); //start
setConfig(vecEnd);
- addPoints();//end
+ addPoints(); //end
}
}
return traces;
}
-}
+} // namespace armarx
diff --git a/source/RobotComponents/components/MotionPlanning/CSpace/SimoxCSpace.h b/source/RobotComponents/components/MotionPlanning/CSpace/SimoxCSpace.h
index a9354889fa4570b84fa781903203396f64413c4a..bea353c605f0e34cbbcb749d40b2ce972cc24f44 100644
--- a/source/RobotComponents/components/MotionPlanning/CSpace/SimoxCSpace.h
+++ b/source/RobotComponents/components/MotionPlanning/CSpace/SimoxCSpace.h
@@ -24,27 +24,28 @@
#pragma once
#include <array>
+#include <atomic>
#include <Eigen/Core>
#include <VirtualRobot/CollisionDetection/CDManager.h>
-#include <VirtualRobot/SceneObjectSet.h>
#include <VirtualRobot/Robot.h>
-#include <MotionPlanning/CSpace/CSpace.h>
+#include <VirtualRobot/SceneObjectSet.h>
+#include <VirtualRobot/XML/RobotIO.h>
+
#include <ArmarXCore/core/system/FactoryCollectionBase.h>
-#include <RobotAPI/libraries/core/Trajectory.h>
+
#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
-#include <MemoryX/core/GridFileManager.h>
-#include <MemoryX/interface/components/WorkingMemoryInterface.h>
-#include <RobotComponents/interface/components/MotionPlanning/CSpace/SimoxCSpace.h>
-#include "GlobalCache.h"
-#include <VirtualRobot/XML/RobotIO.h>
+#include <RobotAPI/libraries/core/RobotPool.h>
+#include <RobotAPI/libraries/core/Trajectory.h>
-#include <atomic>
+#include <RobotComponents/interface/components/MotionPlanning/CSpace/SimoxCSpace.h>
#include "CSpace.h"
-
-#include <RobotAPI/libraries/core/RobotPool.h>
+#include "GlobalCache.h"
+#include <MemoryX/core/GridFileManager.h>
+#include <MemoryX/interface/components/WorkingMemoryInterface.h>
+#include <MotionPlanning/CSpace/CSpace.h>
namespace armarx
{
@@ -54,8 +55,6 @@ namespace armarx
*/
using SimoxCSpacePtr = IceInternal::Handle<SimoxCSpace>;
-
-
/**
* @brief The SimoxCSpace contains a set of stationary obstacles and an agent.
* The agent can have attached objects.
@@ -64,9 +63,7 @@ namespace armarx
* @see armarx::SimoxCSpaceBase
* @see RobotComponents-Tutorial-SimoxCSpace
*/
- class SimoxCSpace :
- virtual public SimoxCSpaceBase,
- virtual public CSpace
+ class SimoxCSpace : virtual public SimoxCSpaceBase, virtual public CSpace
{
public:
using CSpace::isCollisionFree;
@@ -76,33 +73,42 @@ namespace armarx
* @param commonStoragePrx The common storage containing the objects.
* @param loadVisualizationModel Whether to load the visualization model. (true, if you want to display the object. false if you want to plan)
*/
- SimoxCSpace(memoryx::CommonStorageInterfacePrx commonStoragePrx, bool loadVisualizationModel = false, float stationaryObjectMargin = 0.0f);
+ SimoxCSpace(memoryx::CommonStorageInterfacePrx commonStoragePrx,
+ bool loadVisualizationModel = false,
+ float stationaryObjectMargin = 0.0f);
//from setting the cspace up
/**
* @brief Adds a stationary object to the cspace.
* @param obj The object to add.
*/
- void addStationaryObject(const StationaryObject& obj, const Ice::Current& = Ice::emptyCurrent)override;
+ void addStationaryObject(const StationaryObject& obj,
+ const Ice::Current& = Ice::emptyCurrent) override;
virtual void setStationaryObjects(const StationaryObjectList& objList);
/**
* @brief Adds a stationary agent to the cspace.
* @param obj The object to add.
*/
- void setAgent(const AgentPlanningInformation& agentInfo, const Ice::Current& = Ice::emptyCurrent)override;
+ void setAgent(const AgentPlanningInformation& agentInfo,
+ const Ice::Current& = Ice::emptyCurrent) override;
/**
* @brief Adds all objects except the ones with ids specified in the parameter excludedInstancesIds from the working memory to the cspace.
* @param workingMemoryPrx The working memory.
* @param excludedInstancesIds the ids of the instances in teh working memory that should not be added
*/
- virtual void addObjectsFromWorkingMemory(memoryx::WorkingMemoryInterfacePrx workingMemoryPrx, const std::vector<std::string>& excludedInstancesIds = std::vector<std::string>());
+ virtual void addObjectsFromWorkingMemory(
+ memoryx::WorkingMemoryInterfacePrx workingMemoryPrx,
+ const std::vector<std::string>& excludedInstancesIds = std::vector<std::string>());
/**
* @brief Load objects from WorkingMemory and puts them into the mesh cache.
* @param workingMemoryPrx
*/
- static SimoxCSpacePtr PrefetchWorkingMemoryObjects(memoryx::WorkingMemoryInterfacePrx workingMemoryPrx, memoryx::CommonStorageInterfacePrx commonStoragePrx, RobotStateComponentInterfacePrx rsc);
+ static SimoxCSpacePtr
+ PrefetchWorkingMemoryObjects(memoryx::WorkingMemoryInterfacePrx workingMemoryPrx,
+ memoryx::CommonStorageInterfacePrx commonStoragePrx,
+ RobotStateComponentInterfacePrx rsc);
virtual void addPlanarObject(const std::vector<Eigen::Vector3f>& convexHull);
@@ -110,12 +116,13 @@ namespace armarx
/**
* @brief Initializes the collision test.
*/
- void initCollisionTest(const Ice::Current& = Ice::emptyCurrent)override;
+ void initCollisionTest(const Ice::Current& = Ice::emptyCurrent) override;
/**
* @return A clone of this cspace.
*/
- CSpaceBasePtr clone(const Ice::Current& = Ice::emptyCurrent) override
+ CSpaceBasePtr
+ clone(const Ice::Current& = Ice::emptyCurrent) override
{
return clone(false);
}
@@ -132,16 +139,19 @@ namespace armarx
* @param cfg The configuration to check.
* @return Whether the given configuration is collision free.
*/
- bool isCollisionFree(const::std::pair<const Ice::Float*, const Ice::Float*>& cfg, const Ice::Current& = Ice::emptyCurrent)override;
+ bool isCollisionFree(const ::std::pair<const Ice::Float*, const Ice::Float*>& cfg,
+ const Ice::Current& = Ice::emptyCurrent) override;
/**
* @return The cspace's dimension bounds.
*/
FloatRangeSeq getDimensionsBounds(const Ice::Current& = Ice::emptyCurrent) const override;
+
/**
* @return The cspace's dimensionality.
*/
- Ice::Long getDimensionality(const Ice::Current& = Ice::emptyCurrent) const override
+ Ice::Long
+ getDimensionality(const Ice::Current& = Ice::emptyCurrent) const override
{
return agentJoints.size();
}
@@ -150,27 +160,33 @@ namespace armarx
* @brief Sets a configuration to check for. (can be used to display a configuration)
* @param it A pointer to the configuration.
*/
- virtual void setConfig(const std::vector<float> cfg)
+ virtual void
+ setConfig(const std::vector<float> cfg)
{
assert(cfg.size() == static_cast<std::size_t>(getDimensionality()));
setConfig(cfg.data());
}
+
virtual void setConfig(const float*& it);
- virtual void setConfig(const float*&& it) //deals with vector<float>::data()
+
+ virtual void
+ setConfig(const float*&& it) //deals with vector<float>::data()
{
- setConfig(it);//calls the ref version
+ setConfig(it); //calls the ref version
}
//getter
/**
* @return The stationary objects.
*/
- const StationaryObjectList& getStationaryObjects() const
+ const StationaryObjectList&
+ getStationaryObjects() const
{
return stationaryObjects;
}
- const AgentPlanningInformation& getAgent() const
+ const AgentPlanningInformation&
+ getAgent() const
{
return agentInfo;
}
@@ -178,12 +194,14 @@ namespace armarx
/**
* @return The set of stationary objects' scene objects.
*/
- const VirtualRobot::SceneObjectSetPtr& getStationaryObjectSet() const
+ const VirtualRobot::SceneObjectSetPtr&
+ getStationaryObjectSet() const
{
return stationaryObjectSet;
}
- const VirtualRobot::RobotPtr& getAgentSceneObj() const
+ const VirtualRobot::RobotPtr&
+ getAgentSceneObj() const
{
return agentSceneObj;
}
@@ -191,7 +209,8 @@ namespace armarx
/**
* @return The collision checker.
*/
- VirtualRobot::CDManager& getCD()
+ VirtualRobot::CDManager&
+ getCD()
{
return cd;
}
@@ -199,34 +218,52 @@ namespace armarx
virtual Ice::StringSeq getAgentJointNames() const;
//utility
- VectorXf jointMapToVector(const std::map<std::string, float>& jointMap, bool checkForNonexistenJointsInCspace = false) const;
+ VectorXf jointMapToVector(const std::map<std::string, float>& jointMap,
+ bool checkForNonexistenJointsInCspace = false) const;
- virtual TrajectoryPtr pathToTrajectory(const Path& p) const
+ virtual TrajectoryPtr
+ pathToTrajectory(const Path& p) const
{
return pathToTrajectory(p.nodes);
}
- virtual TrajectoryPtr pathToTrajectory(const PathWithCost& p) const
+
+ virtual TrajectoryPtr
+ pathToTrajectory(const PathWithCost& p) const
{
return pathToTrajectory(p.nodes);
}
+
virtual TrajectoryPtr pathToTrajectory(const VectorXfSeq& p) const;
- std::vector<armarx::DebugDrawerLineSet> getTraceVisu(const Path& p, const std::vector<std::string>& nodeNames, float traceStepSize = std::numeric_limits<float>::infinity())
+ std::vector<armarx::DebugDrawerLineSet>
+ getTraceVisu(const Path& p,
+ const std::vector<std::string>& nodeNames,
+ float traceStepSize = std::numeric_limits<float>::infinity())
{
return getTraceVisu(p.nodes, nodeNames, traceStepSize);
}
- std::vector<armarx::DebugDrawerLineSet> getTraceVisu(const PathWithCost& p, const std::vector<std::string>& nodeNames, float traceStepSize = std::numeric_limits<float>::infinity())
+
+ std::vector<armarx::DebugDrawerLineSet>
+ getTraceVisu(const PathWithCost& p,
+ const std::vector<std::string>& nodeNames,
+ float traceStepSize = std::numeric_limits<float>::infinity())
{
return getTraceVisu(p.nodes, nodeNames, traceStepSize);
}
- std::vector<armarx::DebugDrawerLineSet> getTraceVisu(const VectorXfSeq& vecs, const std::vector<std::string>& nodeNames, float traceStepSize = std::numeric_limits<float>::infinity());
- float getStationaryObjectMargin() const
+ std::vector<armarx::DebugDrawerLineSet>
+ getTraceVisu(const VectorXfSeq& vecs,
+ const std::vector<std::string>& nodeNames,
+ float traceStepSize = std::numeric_limits<float>::infinity());
+
+ float
+ getStationaryObjectMargin() const
{
return stationaryObjectMargin;
}
- void setStationaryObjectMargin(float stationaryObjectMargin)
+ void
+ setStationaryObjectMargin(float stationaryObjectMargin)
{
if (!isInitialized)
{
@@ -234,7 +271,8 @@ namespace armarx
}
else
{
- ARMARX_ERROR << "Could not set stationary object margin, because the cSpace is already initialized.";
+ ARMARX_ERROR << "Could not set stationary object margin, because the cSpace is "
+ "already initialized.";
}
}
@@ -245,9 +283,11 @@ namespace armarx
void ice_postUnmarshal() override;
protected:
- template <class IceBaseClass, class DerivedClass> friend class armarx::GenericFactory;
+ template <class IceBaseClass, class DerivedClass>
+ friend class armarx::GenericFactory;
- const std::vector<VirtualRobot::RobotNodePtr>& getAgentJoints() const
+ const std::vector<VirtualRobot::RobotNodePtr>&
+ getAgentJoints() const
{
return agentJoints;
}
@@ -261,11 +301,11 @@ namespace armarx
/**
* @brief Whether the collision check is initialized
*/
- bool isInitialized {false};
+ bool isInitialized{false};
/**
* @brief Whether the visualization model of objects/agents sould be loaded.
*/
- bool loadVisualizationModel {false};
+ bool loadVisualizationModel{false};
/**
@@ -282,7 +322,8 @@ namespace armarx
/**
* @brief The scene objects for stationary objects.
*/
- VirtualRobot::SceneObjectSetPtr stationaryObjectSet {new VirtualRobot::SceneObjectSet{"StationaryObjectSet"}};
+ VirtualRobot::SceneObjectSetPtr stationaryObjectSet{
+ new VirtualRobot::SceneObjectSet{"StationaryObjectSet"}};
std::vector<VirtualRobot::RobotNodePtr> agentJoints;
@@ -290,8 +331,11 @@ namespace armarx
std::vector<std::vector<Eigen::Vector3f>> stationaryPlanes;
- mutable GlobalCache<VirtualRobot::ManipulationObjectPtr, std::pair<int, std::string>> meshCache;
- mutable GlobalCache<RobotPoolPtr, std::pair<VirtualRobot::RobotIO::RobotDescription, std::string>> robotCache;
+ mutable GlobalCache<VirtualRobot::ManipulationObjectPtr, std::pair<int, std::string>>
+ meshCache;
+ mutable GlobalCache<RobotPoolPtr,
+ std::pair<VirtualRobot::RobotIO::RobotDescription, std::string>>
+ robotCache;
/**
@@ -329,12 +373,14 @@ namespace armarx
*/
void initAgent(AgentData agentData);
- void initAgent()
+ void
+ initAgent()
{
initAgent(getAgentDataAndLoadRobot());
}
- void initAgent(VirtualRobot::RobotPtr robotPtr)
+ void
+ initAgent(VirtualRobot::RobotPtr robotPtr)
{
initAgent(getAgentDataFromRobot(std::move(robotPtr)));
}
@@ -343,34 +389,45 @@ namespace armarx
* @param object The object.
* @return The given objects manipulation object.
*/
- VirtualRobot::ManipulationObjectPtr getSimoxManipulatorObject(const memoryx::ObjectClassBasePtr& object, const memoryx::GridFileManagerPtr& fileManager, bool inflate = true, VirtualRobot::CollisionCheckerPtr const& colChecker = VirtualRobot::CollisionCheckerPtr {}) const;
+ VirtualRobot::ManipulationObjectPtr
+ getSimoxManipulatorObject(const memoryx::ObjectClassBasePtr& object,
+ const memoryx::GridFileManagerPtr& fileManager,
+ bool inflate = true,
+ VirtualRobot::CollisionCheckerPtr const& colChecker =
+ VirtualRobot::CollisionCheckerPtr{}) const;
/**
* @param object The object.
* @param pose The pose to use.
* @return The given objects manipulation object moved to the given pose.
*/
- VirtualRobot::ManipulationObjectPtr getMovedSimoxManipulatorObject(const memoryx::ObjectClassBasePtr& object, const armarx::PoseBasePtr& pose, memoryx::GridFileManagerPtr& fileManager) const;
+ VirtualRobot::ManipulationObjectPtr
+ getMovedSimoxManipulatorObject(const memoryx::ObjectClassBasePtr& object,
+ const armarx::PoseBasePtr& pose,
+ memoryx::GridFileManagerPtr& fileManager) const;
AgentData getAgentDataAndLoadRobot() const;
AgentData getAgentDataFromRobot(VirtualRobot::RobotPtr robotPtr) const;
};
-
class SimoxCSpaceWith2DPose;
using SimoxCSpaceWith2DPosePtr = IceInternal::Handle<SimoxCSpaceWith2DPose>;
+
/**
* @brief Similar to \ref armarx::SimoxCSpace, but prepends dimensions for translation in x and y and a rotation around z. (use this for path planning)
*/
- class SimoxCSpaceWith2DPose:
+ class SimoxCSpaceWith2DPose :
virtual public SimoxCSpace,
virtual public SimoxCSpaceWith2DPoseBase
{
public:
- SimoxCSpaceWith2DPose(memoryx::CommonStorageInterfacePrx commonStoragePrx, bool loadVisualizationModel = false, float stationaryObjectMargin = 0.0f);
+ SimoxCSpaceWith2DPose(memoryx::CommonStorageInterfacePrx commonStoragePrx,
+ bool loadVisualizationModel = false,
+ float stationaryObjectMargin = 0.0f);
- void setPoseBounds(const Vector3fRange& newBounds)
+ void
+ setPoseBounds(const Vector3fRange& newBounds)
{
poseBounds = newBounds;
}
@@ -385,10 +442,12 @@ namespace armarx
* @return The cspace's dimension bounds.
*/
FloatRangeSeq getDimensionsBounds(const Ice::Current& = Ice::emptyCurrent) const override;
+
/**
* @return The cspace's dimensionality.
*/
- Ice::Long getDimensionality(const Ice::Current& = Ice::emptyCurrent) const override
+ Ice::Long
+ getDimensionality(const Ice::Current& = Ice::emptyCurrent) const override
{
return agentJoints.size() + 3;
}
@@ -396,8 +455,10 @@ namespace armarx
Ice::StringSeq getAgentJointNames() const override;
void setConfig(const float*& it) override;
+
protected:
- template <class IceBaseClass, class DerivedClass> friend class armarx::GenericFactory;
+ template <class IceBaseClass, class DerivedClass>
+ friend class armarx::GenericFactory;
SimoxCSpaceWith2DPose() = default;
Eigen::Matrix4f globalPoseBuffer = Eigen::Matrix4f::Identity();
@@ -405,18 +466,22 @@ namespace armarx
class SimoxCSpaceWith3DPose;
using SimoxCSpaceWith3DPosePtr = IceInternal::Handle<SimoxCSpaceWith3DPose>;
+
/**
* @brief Similar to \ref armarx::SimoxCSpace, but prepends dimensions for translation in x, y and z and rotations around x, y and z.
* (use this for path planning of flying robots).
*/
- class SimoxCSpaceWith3DPose:
+ class SimoxCSpaceWith3DPose :
virtual public SimoxCSpace,
virtual public SimoxCSpaceWith3DPoseBase
{
public:
- SimoxCSpaceWith3DPose(memoryx::CommonStorageInterfacePrx commonStoragePrx, bool loadVisualizationModel, float stationaryObjectMargin = 0.0f);
+ SimoxCSpaceWith3DPose(memoryx::CommonStorageInterfacePrx commonStoragePrx,
+ bool loadVisualizationModel,
+ float stationaryObjectMargin = 0.0f);
- void setPoseBounds(const Vector6fRange& newBounds)
+ void
+ setPoseBounds(const Vector6fRange& newBounds)
{
poseBounds = newBounds;
}
@@ -431,10 +496,12 @@ namespace armarx
* @return The cspace's dimension bounds.
*/
FloatRangeSeq getDimensionsBounds(const Ice::Current& = Ice::emptyCurrent) const override;
+
/**
* @return The cspace's dimensionality.
*/
- Ice::Long getDimensionality(const Ice::Current& = Ice::emptyCurrent) const override
+ Ice::Long
+ getDimensionality(const Ice::Current& = Ice::emptyCurrent) const override
{
return agentJoints.size() + 6;
}
@@ -442,11 +509,13 @@ namespace armarx
void setConfig(const float*& it) override;
Ice::StringSeq getAgentJointNames() const override;
+
protected:
- template <class IceBaseClass, class DerivedClass> friend class armarx::GenericFactory;
+ template <class IceBaseClass, class DerivedClass>
+ friend class armarx::GenericFactory;
SimoxCSpaceWith3DPose() = default;
Eigen::Matrix4f globalPoseBuffer = Eigen::Matrix4f::Identity();
};
-}
+} // namespace armarx
diff --git a/source/RobotComponents/components/MotionPlanning/CSpace/VoxelGridCSpace.cpp b/source/RobotComponents/components/MotionPlanning/CSpace/VoxelGridCSpace.cpp
index e3fb1f1a6ff55e0c0a2cbaf67a840449941a4a80..4ac3dd4e7e2510986d7d461a7c3e35238969c2bc 100644
--- a/source/RobotComponents/components/MotionPlanning/CSpace/VoxelGridCSpace.cpp
+++ b/source/RobotComponents/components/MotionPlanning/CSpace/VoxelGridCSpace.cpp
@@ -24,52 +24,65 @@
#include "VoxelGridCSpace.h"
#include <Eigen/Core>
-#include <ArmarXCore/interface/core/BasicVectorTypes.h>
+
+#include <VirtualRobot/CollisionDetection/CollisionModel.h>
+#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>
#include <VirtualRobot/Visualization/TriMeshUtils.h>
+#include <VirtualRobot/Visualization/VisualizationFactory.h>
+
#include <ArmarXCore/core/exceptions/user/NotImplementedYetException.h>
#include <ArmarXCore/core/util/algorithm.h>
-#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>
-#include <VirtualRobot/Visualization/VisualizationFactory.h>
+#include <ArmarXCore/interface/core/BasicVectorTypes.h>
using namespace armarx;
-VoxelGridCSpace::VoxelGridCSpace(visionx::VoxelGridProviderInterfacePrx voxelGridProvider, memoryx::CommonStorageInterfacePrx cs, bool loadVisualizationModel, float stationaryObjectMargin)
- : SimoxCSpace(cs, loadVisualizationModel, stationaryObjectMargin)
+VoxelGridCSpace::VoxelGridCSpace(visionx::VoxelGridProviderInterfacePrx voxelGridProvider,
+ memoryx::CommonStorageInterfacePrx cs,
+ bool loadVisualizationModel,
+ float stationaryObjectMargin) :
+ SimoxCSpace(cs, loadVisualizationModel, stationaryObjectMargin)
{
this->voxelGridProvider = voxelGridProvider;
}
-VirtualRobot::SceneObjectPtr VoxelGridCSpace::createGridObstacle() const
+VirtualRobot::SceneObjectPtr
+VoxelGridCSpace::createGridObstacle() const
{
- auto gridPositionsEigen = armarx::transform(gridPositions, +[](armarx::Vector3f const & data)
- {
- return Eigen::Vector3f(data.e0, data.e1, data.e2);
- });
-
-
-
- auto gridMesh = VirtualRobot::TriMeshUtils::CreateSparseBoxGrid(Eigen::Matrix4f::Identity(), gridPositionsEigen,
- gridSize, gridSize, gridSize,
- VirtualRobot::VisualizationFactory::Color::Blue()
- );
+ auto gridPositionsEigen = armarx::transform(
+ gridPositions,
+ +[](armarx::Vector3f const& data) { return Eigen::Vector3f(data.e0, data.e1, data.e2); });
+
+
+ auto gridMesh = VirtualRobot::TriMeshUtils::CreateSparseBoxGrid(
+ Eigen::Matrix4f::Identity(),
+ gridPositionsEigen,
+ gridSize,
+ gridSize,
+ gridSize,
+ VirtualRobot::VisualizationFactory::Color::Blue());
gridMesh->mergeVertices(10);
gridMesh->fattenShrink(stationaryObjectMargin);
Eigen::Matrix4f id = Eigen::Matrix4f::Identity();
- auto visu = VirtualRobot::CoinVisualizationFactory().createTriMeshModelVisualization(gridMesh, id);
- VirtualRobot::SceneObjectPtr obst(new VirtualRobot::SceneObject("PointCloudMeshGrid", visu,
- VirtualRobot::CollisionModelPtr(new VirtualRobot::CollisionModel(visu, "PointCloudMeshGridCollisionModel", agentSceneObj->getCollisionChecker())),
- VirtualRobot::SceneObject::Physics(), const_cast<VirtualRobot::CDManager*>(&cd)->getCollisionChecker()));
+ auto visu =
+ VirtualRobot::CoinVisualizationFactory().createTriMeshModelVisualization(gridMesh, id);
+ VirtualRobot::SceneObjectPtr obst(new VirtualRobot::SceneObject(
+ "PointCloudMeshGrid",
+ visu,
+ VirtualRobot::CollisionModelPtr(new VirtualRobot::CollisionModel(
+ visu, "PointCloudMeshGridCollisionModel", agentSceneObj->getCollisionChecker())),
+ VirtualRobot::SceneObject::Physics(),
+ const_cast<VirtualRobot::CDManager*>(&cd)->getCollisionChecker()));
return obst;
}
-
-
-armarx::CSpaceBasePtr VoxelGridCSpace::clone(const Ice::Current&)
+armarx::CSpaceBasePtr
+VoxelGridCSpace::clone(const Ice::Current&)
{
return VoxelGridCSpaceBasePtr(new VoxelGridCSpace(*this));
}
-void VoxelGridCSpace::initCollisionTest(const Ice::Current& c)
+void
+VoxelGridCSpace::initCollisionTest(const Ice::Current& c)
{
SimoxCSpace::initCollisionTest(c);
if (gridPositions.empty())
@@ -77,29 +90,29 @@ void VoxelGridCSpace::initCollisionTest(const Ice::Current& c)
gridPositions = voxelGridProvider->getFilledGridPositions();
ARMARX_INFO << "Got grid with size: " << gridPositions.size();
gridSize = voxelGridProvider->getGridSize();
-
}
auto obst = createGridObstacle();
ARMARX_INFO << "Adding scene object with grid element count: " << gridPositions.size();
stationaryObjectSet->addSceneObject(obst);
- ARMARX_INFO << "SceneObjects: " << armarx::transform(stationaryObjectSet->getSceneObjects(), +[](VirtualRobot::SceneObjectPtr const & obj)
- {
- return obj->getName();
- });
+ ARMARX_INFO << "SceneObjects: "
+ << armarx::transform(
+ stationaryObjectSet->getSceneObjects(),
+ +[](VirtualRobot::SceneObjectPtr const& obj) { return obj->getName(); });
if (!cd.hasSceneObjectSet(stationaryObjectSet))
{
cd.addCollisionModel(stationaryObjectSet);
}
}
-
-CSpaceBasePtr armarx::VoxelGridCSpace::clone(bool loadVisualizationModel)
+CSpaceBasePtr
+armarx::VoxelGridCSpace::clone(bool loadVisualizationModel)
{
// TIMING_START(SimoxCSpaceClone);
// VoxelGridCSpacePtr cloned = VoxelGridCSpacePtr::dynamicCast(clone(Ice::emptyCurrent));
- VoxelGridCSpacePtr cloned = new VoxelGridCSpace(voxelGridProvider, commonStorage, loadVisualizationModel);
+ VoxelGridCSpacePtr cloned =
+ new VoxelGridCSpace(voxelGridProvider, commonStorage, loadVisualizationModel);
cloned->gridPositions = gridPositions;
cloned->gridSize = gridSize;
cloned->stationaryObjectMargin = stationaryObjectMargin;
diff --git a/source/RobotComponents/components/MotionPlanning/CSpace/VoxelGridCSpace.h b/source/RobotComponents/components/MotionPlanning/CSpace/VoxelGridCSpace.h
index bcbb8be0d61d9d68154dcd9ccbb4bec63ede41ca..b4dd32856d1bbc3041c41bbc233aeec0aa40ca44 100644
--- a/source/RobotComponents/components/MotionPlanning/CSpace/VoxelGridCSpace.h
+++ b/source/RobotComponents/components/MotionPlanning/CSpace/VoxelGridCSpace.h
@@ -22,34 +22,41 @@
* GNU General Public License
*/
#pragma once
-#include "SimoxCSpace.h"
+#include <VirtualRobot/VirtualRobot.h>
-#include <MemoryX/interface/components/CommonStorageInterface.h>
#include <RobotComponents/interface/components/MotionPlanning/CSpace/VoxelGridCSpaceBase.h>
-#include <VirtualRobot/VirtualRobot.h>
+
+#include "SimoxCSpace.h"
+#include <MemoryX/interface/components/CommonStorageInterface.h>
namespace armarx
{
- class VoxelGridCSpace :
- virtual public armarx::VoxelGridCSpaceBase,
- virtual public SimoxCSpace
+ class VoxelGridCSpace : virtual public armarx::VoxelGridCSpaceBase, virtual public SimoxCSpace
{
- template <class IceBaseClass, class DerivedClass> friend class armarx::GenericFactory;
- VoxelGridCSpace() {}
+ template <class IceBaseClass, class DerivedClass>
+ friend class armarx::GenericFactory;
+
+ VoxelGridCSpace()
+ {
+ }
+
public:
- VoxelGridCSpace(visionx::VoxelGridProviderInterfacePrx voxelGridProvider, memoryx::CommonStorageInterfacePrx cs, bool loadVisualizationModel = false, float stationaryObjectMargin = 0.0f);
+ VoxelGridCSpace(visionx::VoxelGridProviderInterfacePrx voxelGridProvider,
+ memoryx::CommonStorageInterfacePrx cs,
+ bool loadVisualizationModel = false,
+ float stationaryObjectMargin = 0.0f);
VirtualRobot::SceneObjectPtr createGridObstacle() const;
// CSpaceBase interface
public:
armarx::CSpaceBasePtr clone(const Ice::Current&) override;
void initCollisionTest(const Ice::Current&) override;
- protected:
+ protected:
// SimoxCSpace interface
public:
CSpaceBasePtr clone(bool loadVisualizationModel) override;
};
- using VoxelGridCSpacePtr = IceInternal::Handle<VoxelGridCSpace>;
-}
+ using VoxelGridCSpacePtr = IceInternal::Handle<VoxelGridCSpace>;
+} // namespace armarx
diff --git a/source/RobotComponents/components/MotionPlanning/MotionPlanningObjectFactories.h b/source/RobotComponents/components/MotionPlanning/MotionPlanningObjectFactories.h
index 714dce7de1fd171ddd26e11763df9f32dca4082d..5db93b14bf076f61e259db30c6d26c7ad0dcee17 100644
--- a/source/RobotComponents/components/MotionPlanning/MotionPlanningObjectFactories.h
+++ b/source/RobotComponents/components/MotionPlanning/MotionPlanningObjectFactories.h
@@ -23,44 +23,35 @@
*/
#pragma once
-#include <vector>
#include <map>
#include <string>
+#include <vector>
-#include <Ice/ObjectFactory.h>
#include <Ice/Ice.h>
+#include <Ice/ObjectFactory.h>
-#include <ArmarXCore/core/system/Synchronization.h>
#include <ArmarXCore/core/logging/Logging.h>
-
#include <ArmarXCore/core/system/FactoryCollectionBase.h>
-#include <MemoryX/libraries/memorytypes/MemoryXTypesObjectFactories.h>
-#include <MemoryX/core/MemoryXCoreObjectFactories.h>
+#include <ArmarXCore/core/system/Synchronization.h>
-#include "Tasks/AdaptiveDynamicDomainInformedRRTStar/Task.h"
+#include <RobotComponents/components/MotionPlanning/CSpace/VoxelGridCSpace.h>
+#include <RobotComponents/components/MotionPlanning/Tasks/BiRRT/Task.h>
+#include <RobotComponents/components/MotionPlanning/Tasks/CSpaceVisualizerTask/CSpaceVisualizerTask.h>
+
+#include "CSpace/ScaledCSpace.h"
+#include "CSpace/SimoxCSpace.h"
+#include "ResourceRequestStrategies/ComputingPowerRequestStrategy.h"
+#include "Tasks/AStar/Task.h"
#include "Tasks/AdaptiveDynamicDomainInformedRRTStar/ManagerNode.h"
+#include "Tasks/AdaptiveDynamicDomainInformedRRTStar/Task.h"
#include "Tasks/AdaptiveDynamicDomainInformedRRTStar/WorkerNode.h"
-
+#include "Tasks/PathCollection/Task.h"
#include "Tasks/RRTConnect/Task.h"
#include "Tasks/RRTConnect/WorkerNode.h"
-
-#include "Tasks/AStar/Task.h"
-
#include "Tasks/RandomShortcutPostprocessor/Task.h"
-
-#include "Tasks/PathCollection/Task.h"
-
-#include <ArmarXCore/core/system/FactoryCollectionBase.h>
+#include <MemoryX/core/MemoryXCoreObjectFactories.h>
#include <MemoryX/libraries/memorytypes/MemoryXTypesObjectFactories.h>
-#include <RobotComponents/components/MotionPlanning/CSpace/VoxelGridCSpace.h>
-#include <RobotComponents/components/MotionPlanning/Tasks/BiRRT/Task.h>
-#include <RobotComponents/components/MotionPlanning/Tasks/CSpaceVisualizerTask/CSpaceVisualizerTask.h>
-#include "CSpace/SimoxCSpace.h"
-#include "CSpace/ScaledCSpace.h"
-
-
-#include "ResourceRequestStrategies/ComputingPowerRequestStrategy.h"
namespace armarx::ObjectFactories
{
/**
@@ -72,7 +63,8 @@ namespace armarx::ObjectFactories
/**
* @return The map of object factories.
*/
- ObjectFactoryMap getFactories() override
+ ObjectFactoryMap
+ getFactories() override
{
ObjectFactoryMap map;
//planning alg
@@ -117,5 +109,4 @@ namespace armarx::ObjectFactories
const FactoryCollectionBaseCleanUp MotionPlanningObjectFactoriesVar =
FactoryCollectionBase::addToPreregistration(new MotionPlanningObjectFactories());
-}
-
+} // namespace armarx::ObjectFactories
diff --git a/source/RobotComponents/components/MotionPlanning/MotionPlanningServer.cpp b/source/RobotComponents/components/MotionPlanning/MotionPlanningServer.cpp
index 7f22a9c87f254f65f20e783132e7fd811d54ae42..194c54ba28042386712bdca11d892795fabd511a 100644
--- a/source/RobotComponents/components/MotionPlanning/MotionPlanningServer.cpp
+++ b/source/RobotComponents/components/MotionPlanning/MotionPlanningServer.cpp
@@ -21,6 +21,8 @@
* @copyright http://www.gnu.org/licenses/gpl.txt
* GNU General Public License
*/
+#include "MotionPlanningServer.h"
+
#include <algorithm>
#include <Ice/ObjectAdapter.h>
@@ -28,19 +30,19 @@
#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>
#include <ArmarXCore/core/ArmarXManager.h>
-#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
-#include <ArmarXCore/core/system/DynamicLibrary.h>
#include <ArmarXCore/core/CoreObjectFactories.h>
-#include <MemoryX/interface/components/PriorKnowledgeInterface.h>
-#include "Tasks/MotionPlanningTask.h"
+#include <ArmarXCore/core/system/DynamicLibrary.h>
+#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
-#include "MotionPlanningServer.h"
+#include "Tasks/MotionPlanningTask.h"
#include "util/PlanningUtil.h"
+#include <MemoryX/interface/components/PriorKnowledgeInterface.h>
namespace armarx
{
- void MotionPlanningServer::onInitComponent()
+ void
+ MotionPlanningServer::onInitComponent()
{
setTag("Server");
@@ -50,14 +52,10 @@ namespace armarx
remoteObjectNodeNames = Split(remNames, ";,");
//del empty names
remoteObjectNodeNames.resize(
- std::distance(
- remoteObjectNodeNames.begin(),
- std::remove_if(remoteObjectNodeNames.begin(), remoteObjectNodeNames.end(), [](const std::string & s)
- {
- return s == "";
- })
- )
- );
+ std::distance(remoteObjectNodeNames.begin(),
+ std::remove_if(remoteObjectNodeNames.begin(),
+ remoteObjectNodeNames.end(),
+ [](const std::string& s) { return s == ""; })));
for (const auto& name : remoteObjectNodeNames)
{
@@ -67,10 +65,11 @@ namespace armarx
//start dispatcher
- dispatcherThread = std::thread {[this]{this->dispatcherTask();}};
+ dispatcherThread = std::thread{[this] { this->dispatcherTask(); }};
}
- void MotionPlanningServer::onConnectComponent()
+ void
+ MotionPlanningServer::onConnectComponent()
{
remoteObjectNodes.clear();
@@ -83,33 +82,38 @@ namespace armarx
{
if (startLocalNode)
{
- localNode = Component::create<RemoteObjectNode>(getIceProperties(), generateSubObjectName("RemoteObjectNode"));
+ localNode = Component::create<RemoteObjectNode>(
+ getIceProperties(), generateSubObjectName("RemoteObjectNode"));
localNode->setCoreCount(localNodePercent * localNode->getDefaultCoreCount());
//registration is required since a proxy is required
getArmarXManager()->addObject(localNode);
- remoteObjectNodes.emplace_back(RemoteObjectNodeInterfacePrx::uncheckedCast(localNode->getProxy(-1)));
+ remoteObjectNodes.emplace_back(
+ RemoteObjectNodeInterfacePrx::uncheckedCast(localNode->getProxy(-1)));
}
else
{
- ARMARX_WARNING_S << "Starting motion planning server without any remote object nodes! (some algorithms may still work)";
+ ARMARX_WARNING_S << "Starting motion planning server without any remote object "
+ "nodes! (some algorithms may still work)";
}
}
auto wm = getProxy<memoryx::WorkingMemoryInterfacePrx>("WorkingMemory", false, "", false);
auto cs = getProxy<memoryx::CommonStorageInterfacePrx>("CommonStorage", false, "", false);
- auto rsc = getProxy<RobotStateComponentInterfacePrx>("RobotStateComponent", false, "", false);
+ auto rsc =
+ getProxy<RobotStateComponentInterfacePrx>("RobotStateComponent", false, "", false);
if (wm && cs & rsc)
{
cacheCSpace = SimoxCSpace::PrefetchWorkingMemoryObjects(wm, cs, rsc);
}
}
- void MotionPlanningServer::onExitComponent()
+ void
+ MotionPlanningServer::onExitComponent()
{
//request kill on dispatcher
dispatcherKillRequest = true;
{
//start aborting current task since this could take a while
- std::lock_guard<std::recursive_mutex> lock {queueMutex};
+ std::lock_guard<std::recursive_mutex> lock{queueMutex};
//abort all queued tasks
for (auto taskId : taskQueue)
@@ -137,7 +141,8 @@ namespace armarx
}
}
- void MotionPlanningServer::dispatcherTask()
+ void
+ MotionPlanningServer::dispatcherTask()
{
decltype(remoteObjectNodes) ronCache;
while (true)
@@ -152,7 +157,7 @@ namespace armarx
TaskAndRemoteHandle handle;
decltype(taskQueue)::value_type taskId; //simple to change taskids later
{
- std::unique_lock<std::recursive_mutex> lock {queueMutex};
+ std::unique_lock<std::recursive_mutex> lock{queueMutex};
if (taskQueue.empty())
{
@@ -172,11 +177,11 @@ namespace armarx
ronCache = remoteObjectNodes;
//what every task prints:
-#define task_info "TASK FAILED!"\
- << "\n\tname: " << handle.task->getTaskName()\
- << "\n\tID: " << taskId\
- << "\n\tice id = " << handle.task->ice_id()\
- << "\n\tstatus " << TaskStatus::toString(handle.task->getTaskStatus())
+#define task_info \
+ "TASK FAILED!" \
+ << "\n\tname: " << handle.task->getTaskName() << "\n\tID: " << taskId \
+ << "\n\tice id = " << handle.task->ice_id() << "\n\tstatus " \
+ << TaskStatus::toString(handle.task->getTaskStatus())
try
{
@@ -188,28 +193,32 @@ namespace armarx
while (t)
{
auto tp = t.get();
- t->addTaskStatusCallback([tp, start, this](TaskStatus::Status s)
- {
- if (s == TaskStatus::eRefining)
+ t->addTaskStatusCallback(
+ [tp, start, this](TaskStatus::Status s)
{
- auto duration = IceUtil::Time::now() - start;
- tp->planningTime = duration.toMicroSeconds();
- }
- if (TaskStatus::finishedRunning(s))
- {
- auto duration = IceUtil::Time::now() - start;
- if (tp->planningTime > 0)
+ if (s == TaskStatus::eRefining)
{
- tp->refiningTime = duration.toMicroSeconds() - tp->planningTime;
+ auto duration = IceUtil::Time::now() - start;
+ tp->planningTime = duration.toMicroSeconds();
}
- else
+ if (TaskStatus::finishedRunning(s))
{
- tp->planningTime = duration.toMicroSeconds();
+ auto duration = IceUtil::Time::now() - start;
+ if (tp->planningTime > 0)
+ {
+ tp->refiningTime = duration.toMicroSeconds() - tp->planningTime;
+ }
+ else
+ {
+ tp->planningTime = duration.toMicroSeconds();
+ }
+ ARMARX_VERBOSE << tp->getTaskName()
+ << ": Planning time: " << tp->planningTime * 0.001f
+ << " ms, " << tp->refiningTime * 0.001f << " ms";
}
- ARMARX_VERBOSE << tp->getTaskName() << ": Planning time: " << tp->planningTime * 0.001f << " ms, " << tp->refiningTime * 0.001f << " ms";
- }
- });
- PostprocessingMotionPlanningTaskPtr tmp = PostprocessingMotionPlanningTaskPtr::dynamicCast(t);
+ });
+ PostprocessingMotionPlanningTaskPtr tmp =
+ PostprocessingMotionPlanningTaskPtr::dynamicCast(t);
if (tmp)
{
t = MotionPlanningTaskPtr::dynamicCast(tmp->previousStep);
@@ -223,15 +232,14 @@ namespace armarx
}
catch (Ice::Exception& e)
{
- ARMARX_ERROR_S << task_info
- << "\n\tWHAT:\n" << e.what()
- << "\n\tSTACK:\n" << e.ice_stackTrace();
+ ARMARX_ERROR_S << task_info << "\n\tWHAT:\n"
+ << e.what() << "\n\tSTACK:\n"
+ << e.ice_stackTrace();
handle.task->setTaskStatus(TaskStatus::eException);
}
catch (std::exception& e)
{
- ARMARX_ERROR_S << task_info
- << "\n\tWHAT:\n" << e.what();
+ ARMARX_ERROR_S << task_info << "\n\tWHAT:\n" << e.what();
handle.task->setTaskStatus(TaskStatus::eException);
}
catch (...)
@@ -245,7 +253,7 @@ namespace armarx
#undef task_info
//task done dequeue it
{
- std::lock_guard<std::recursive_mutex> lock {queueMutex};
+ std::lock_guard<std::recursive_mutex> lock{queueMutex};
taskQueue.erase(taskId);
}
//loop to wait for next task or kill
@@ -253,9 +261,10 @@ namespace armarx
ARMARX_DEBUG << "dispatcher task exit";
}
- bool MotionPlanningServer::deleteTaskById(Ice::Long id)
+ bool
+ MotionPlanningServer::deleteTaskById(Ice::Long id)
{
- std::lock_guard<std::recursive_mutex> lock {queueMutex};
+ std::lock_guard<std::recursive_mutex> lock{queueMutex};
auto it = tasks.find(id);
if (it == tasks.end())
@@ -275,11 +284,12 @@ namespace armarx
return true;
}
- armarx::ClientSideRemoteHandleControlBlockBasePtr MotionPlanningServer::enqueueTask(const MotionPlanningTaskBasePtr& task, const Ice::Current&)
+ armarx::ClientSideRemoteHandleControlBlockBasePtr
+ MotionPlanningServer::enqueueTask(const MotionPlanningTaskBasePtr& task, const Ice::Current&)
{
if (dispatcherKillRequest)
{
- throw armarx::ServerShuttingDown {"MotionPlanningServerComponent"};
+ throw armarx::ServerShuttingDown{"MotionPlanningServerComponent"};
}
MotionPlanningTaskPtr planningTask = MotionPlanningTaskPtr::dynamicCast(task);
@@ -296,24 +306,19 @@ namespace armarx
//add the task
auto remoteHandleHandles = armarx::RemoteHandleControlBlock::create(
- getArmarXManager(),
- MotionPlanningTaskControlInterfacePrx::uncheckedCast(planningTask->getProxy()),
- [id, this, adapter, ident]
- {
- adapter->remove(ident);
- //del from server
- deleteTaskById(id);
- }
- );
+ getArmarXManager(),
+ MotionPlanningTaskControlInterfacePrx::uncheckedCast(planningTask->getProxy()),
+ [id, this, adapter, ident]
+ {
+ adapter->remove(ident);
+ //del from server
+ deleteTaskById(id);
+ });
{
- std::lock_guard<std::recursive_mutex> lock {queueMutex};
+ std::lock_guard<std::recursive_mutex> lock{queueMutex};
ARMARX_CHECK_EXPRESSION(planningTask);
- tasks[id] = TaskAndRemoteHandle
- {
- remoteHandleHandles.directHandle,
- planningTask
- };
+ tasks[id] = TaskAndRemoteHandle{remoteHandleHandles.directHandle, planningTask};
taskQueue.insert(id);
planningTask->postEnqueueing();
}
@@ -323,42 +328,44 @@ namespace armarx
return remoteHandleHandles.clientSideRemoteHandleControlBlock;
}
- Ice::Long MotionPlanningServer::getNumberOfTasks(const Ice::Current&) const
+ Ice::Long
+ MotionPlanningServer::getNumberOfTasks(const Ice::Current&) const
{
- std::lock_guard<std::recursive_mutex> lock {queueMutex};
+ std::lock_guard<std::recursive_mutex> lock{queueMutex};
return tasks.size();
}
- Ice::Long MotionPlanningServer::getNumberOfQueuedTasks(const Ice::Current&) const
+ Ice::Long
+ MotionPlanningServer::getNumberOfQueuedTasks(const Ice::Current&) const
{
- std::lock_guard<std::recursive_mutex> lock {queueMutex};
+ std::lock_guard<std::recursive_mutex> lock{queueMutex};
return taskQueue.size();
}
- TaskInfoSeq MotionPlanningServer::getAllTaskInfo(const Ice::Current&) const
+ TaskInfoSeq
+ MotionPlanningServer::getAllTaskInfo(const Ice::Current&) const
{
- std::lock_guard<std::recursive_mutex> lock {queueMutex};
+ std::lock_guard<std::recursive_mutex> lock{queueMutex};
TaskInfoSeq result;
result.reserve(tasks.size());
- transform(
- tasks.begin(), tasks.end(), std::back_inserter(result),
- [](const decltype(tasks)::value_type & v)
- {
- const TaskAndRemoteHandle& rhs = v.second;
- //ident, status, iceType
- return TaskInfo
- {
- v.first,
- rhs.rh->getManagedObjectProxy()->ice_getIdentity(),
- rhs.task->getTaskStatus(),
- rhs.task->ice_id(),
- rhs.task->getTaskName()
- };
- });
+ transform(tasks.begin(),
+ tasks.end(),
+ std::back_inserter(result),
+ [](const decltype(tasks)::value_type& v)
+ {
+ const TaskAndRemoteHandle& rhs = v.second;
+ //ident, status, iceType
+ return TaskInfo{v.first,
+ rhs.rh->getManagedObjectProxy()->ice_getIdentity(),
+ rhs.task->getTaskStatus(),
+ rhs.task->ice_id(),
+ rhs.task->getTaskName()};
+ });
return result;
}
- ClientSideRemoteHandleControlBlockBasePtr MotionPlanningServer::getCurrentTaskHandle(const Ice::Current&)
+ ClientSideRemoteHandleControlBlockBasePtr
+ MotionPlanningServer::getCurrentTaskHandle(const Ice::Current&)
{
ClientSideRemoteHandleControlBlockBasePtr remoteHandle;
if (currentTask.rh)
@@ -368,16 +375,17 @@ namespace armarx
return remoteHandle;
}
- CSpaceAndPaths MotionPlanningServer::getTaskCSpaceAndPathsById(Ice::Long id, const Ice::Current&)
+ CSpaceAndPaths
+ MotionPlanningServer::getTaskCSpaceAndPathsById(Ice::Long id, const Ice::Current&)
{
CSpaceAndPaths result;
- std::lock_guard<std::recursive_mutex> lock {queueMutex};
+ std::lock_guard<std::recursive_mutex> lock{queueMutex};
auto it = tasks.find(id);
if (it != tasks.end())
{
auto task = MotionPlanningTaskBasePtr::dynamicCast(it->second.task);
- result.cspace = task ? task->getCSpace()->clone() : nullptr;
+ result.cspace = task ? task->getCSpace()->clone() : nullptr;
auto multPathTask = MotionPlanningMultiPathTaskControlInterfacePtr::dynamicCast(task);
if (multPathTask)
{
@@ -395,8 +403,8 @@ namespace armarx
return result;
}
-
- bool MotionPlanningServer::loadLibFromAbsolutePath(const std::string& path)
+ bool
+ MotionPlanningServer::loadLibFromAbsolutePath(const std::string& path)
{
if (loadedLibs.count(path) > 0)
{
@@ -423,11 +431,13 @@ namespace armarx
return false;
}
- ArmarXManager::RegisterKnownObjectFactoriesWithIce(this->getIceManager()->getCommunicator());
+ ArmarXManager::RegisterKnownObjectFactoriesWithIce(
+ this->getIceManager()->getCommunicator());
return true;
}
- bool MotionPlanningServer::loadLibFromPath(const std::string& path, const Ice::Current&)
+ bool
+ MotionPlanningServer::loadLibFromPath(const std::string& path, const Ice::Current&)
{
std::string absPath;
if (ArmarXDataPath::getAbsolutePath(path, absPath))
@@ -441,7 +451,10 @@ namespace armarx
}
}
- bool MotionPlanningServer::loadLibFromPackage(const std::string& package, const std::string& name, const Ice::Current&)
+ bool
+ MotionPlanningServer::loadLibFromPackage(const std::string& package,
+ const std::string& name,
+ const Ice::Current&)
{
CMakePackageFinder finder(package);
if (!finder.packageFound())
@@ -453,7 +466,7 @@ namespace armarx
for (auto libDirPath : armarx::Split(finder.getLibraryPaths(), ";"))
{
std::filesystem::path fullPath = libDirPath;
- fullPath /= "lib" + name + "." + DynamicLibrary::GetSharedLibraryFileExtension();
+ fullPath /= "lib" + name + "." + DynamicLibrary::GetSharedLibraryFileExtension();
if (!std::filesystem::exists(fullPath))
{
fullPath = libDirPath;
@@ -468,7 +481,7 @@ namespace armarx
return true;
}
}
- ARMARX_ERROR << "Could not find library " << name << " in package " << package;
+ ARMARX_ERROR << "Could not find library " << name << " in package " << package;
return false;
}
-}
+} // namespace armarx
diff --git a/source/RobotComponents/components/MotionPlanning/MotionPlanningServer.h b/source/RobotComponents/components/MotionPlanning/MotionPlanningServer.h
index 7a0c04c99a14792c2095bd3a6350a434b7368369..5eb71026af8772d51d4e5ad2a396871c9b349d3f 100644
--- a/source/RobotComponents/components/MotionPlanning/MotionPlanningServer.h
+++ b/source/RobotComponents/components/MotionPlanning/MotionPlanningServer.h
@@ -23,21 +23,19 @@
*/
#pragma once
-#include <unordered_map>
-#include <condition_variable>
#include <atomic>
+#include <condition_variable>
#include <mutex>
#include <thread>
+#include <unordered_map>
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-
-#include <ArmarXCore/core/util/distributed/RemoteHandle/RemoteHandleControlBlock.h>
#include <ArmarXCore/core/system/RemoteObjectNode.h>
+#include <ArmarXCore/core/util/distributed/RemoteHandle/RemoteHandleControlBlock.h>
-#include <RobotComponents/interface/components/MotionPlanning/MotionPlanningServer.h>
#include <RobotComponents/components/MotionPlanning/CSpace/SimoxCSpace.h>
-
+#include <RobotComponents/interface/components/MotionPlanning/MotionPlanningServer.h>
#include "Tasks/MotionPlanningTask.h"
@@ -57,20 +55,26 @@ namespace armarx
/**
* @brief Properties for a MotionPlanningServerComponent
*/
- class MotionPlanningServerPropertyDefinitions:
- public armarx::ComponentPropertyDefinitions
+ class MotionPlanningServerPropertyDefinitions : public armarx::ComponentPropertyDefinitions
{
public:
/**
* @brief ctor
* @param prefix the used prefix for properties
*/
- MotionPlanningServerPropertyDefinitions(std::string prefix):
+ MotionPlanningServerPropertyDefinitions(std::string prefix) :
armarx::ComponentPropertyDefinitions(prefix)
{
- defineOptionalProperty<std::string>("RemoteObjectNodes", "", "CSV of RemoteObjectNode names");
- defineOptionalProperty<bool>("LocalRONStart", true, "start a local remote object node if the CSV RemoteObjectNodes is empty");
- defineOptionalProperty<float>("LocalRONCorePerc", 0.5, "Local remote object node core count percentage. (has to be >0; 0.5 => 50%");
+ defineOptionalProperty<std::string>(
+ "RemoteObjectNodes", "", "CSV of RemoteObjectNode names");
+ defineOptionalProperty<bool>(
+ "LocalRONStart",
+ true,
+ "start a local remote object node if the CSV RemoteObjectNodes is empty");
+ defineOptionalProperty<float>(
+ "LocalRONCorePerc",
+ 0.5,
+ "Local remote object node core count percentage. (has to be >0; 0.5 => 50%");
}
};
@@ -110,10 +114,12 @@ namespace armarx
* @brief Returns the server's default name.
* @return The server's default name.
*/
- std::string getDefaultName() const override
+ std::string
+ getDefaultName() const override
{
return "MotionPlanningServer";
}
+
/**
* @brief Initializes the server and starts the dispatcher thread
*/
@@ -122,10 +128,15 @@ namespace armarx
* @brief Connects to the used RemoteObjectNodes
*/
void onConnectComponent() override;
+
/**
* @brief noop
*/
- void onDisconnectComponent() override {}
+ void
+ onDisconnectComponent() override
+ {
+ }
+
/**
* @brief cleans up and joins the dispatcher
*/
@@ -137,9 +148,11 @@ namespace armarx
* @see PropertyUser::createPropertyDefinitions()
* @return The planning server's PropertyDefinitions
*/
- armarx::PropertyDefinitionsPtr createPropertyDefinitions() override
+ armarx::PropertyDefinitionsPtr
+ createPropertyDefinitions() override
{
- return armarx::PropertyDefinitionsPtr(new MotionPlanningServerPropertyDefinitions(getConfigIdentifier()));
+ return armarx::PropertyDefinitionsPtr(
+ new MotionPlanningServerPropertyDefinitions(getConfigIdentifier()));
}
//from MotionPlanningServerInterface
@@ -148,14 +161,17 @@ namespace armarx
* @param id The id.
* @return The task's cspace and all found paths.
*/
- CSpaceAndPaths getTaskCSpaceAndPathsById(Ice::Long id, const Ice::Current& = Ice::emptyCurrent) override;
+ CSpaceAndPaths getTaskCSpaceAndPathsById(Ice::Long id,
+ const Ice::Current& = Ice::emptyCurrent) override;
/**
* @brief Enqueues a task
* @param task The task.
* @return The task's proxy.
*/
- armarx::ClientSideRemoteHandleControlBlockBasePtr enqueueTask(const MotionPlanningTaskBasePtr& task, const Ice::Current& = Ice::emptyCurrent) override;
+ armarx::ClientSideRemoteHandleControlBlockBasePtr
+ enqueueTask(const MotionPlanningTaskBasePtr& task,
+ const Ice::Current& = Ice::emptyCurrent) override;
/**
* @brief Returns the number of tasks.
@@ -174,10 +190,14 @@ namespace armarx
*/
TaskInfoSeq getAllTaskInfo(const Ice::Current& = Ice::emptyCurrent) const override;
- ClientSideRemoteHandleControlBlockBasePtr getCurrentTaskHandle(const Ice::Current&) override;
+ ClientSideRemoteHandleControlBlockBasePtr
+ getCurrentTaskHandle(const Ice::Current&) override;
- bool loadLibFromPath(const std::string& path, const Ice::Current& = Ice::emptyCurrent) override;
- bool loadLibFromPackage(const std::string& package, const std::string& lib, const Ice::Current& = Ice::emptyCurrent) override;
+ bool loadLibFromPath(const std::string& path,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ bool loadLibFromPackage(const std::string& package,
+ const std::string& lib,
+ const Ice::Current& = Ice::emptyCurrent) override;
protected:
bool deleteTaskById(Ice::Long id);
@@ -189,12 +209,14 @@ namespace armarx
/**
* @brief the next task id.
*/
- std::atomic<Ice::Long> nextTaskId {std::numeric_limits<Ice::Long>::min()};
+ std::atomic<Ice::Long> nextTaskId{std::numeric_limits<Ice::Long>::min()};
+
/**
* @brief Returns a new task id.
* @return A new task id.
*/
- Ice::Long getNewTaskId()
+ Ice::Long
+ getNewTaskId()
{
return nextTaskId++;
}
@@ -269,7 +291,7 @@ namespace armarx
/**
* @brief Whether the dispatcher should shut down
*/
- std::atomic_bool dispatcherKillRequest {false};
+ std::atomic_bool dispatcherKillRequest{false};
std::map<std::string, DynamicLibraryPtr> loadedLibs;
@@ -280,4 +302,4 @@ namespace armarx
bool loadLibFromAbsolutePath(const std::string& path);
};
-}
+} // namespace armarx
diff --git a/source/RobotComponents/components/MotionPlanning/ResourceRequestStrategies/ComputingPowerRequestStrategy.cpp b/source/RobotComponents/components/MotionPlanning/ResourceRequestStrategies/ComputingPowerRequestStrategy.cpp
index 567a7be6e0c3f225c682730edf808345b6154355..c8680987d4e9ddd944162c1871cd8b5898fdd648 100644
--- a/source/RobotComponents/components/MotionPlanning/ResourceRequestStrategies/ComputingPowerRequestStrategy.cpp
+++ b/source/RobotComponents/components/MotionPlanning/ResourceRequestStrategies/ComputingPowerRequestStrategy.cpp
@@ -26,17 +26,17 @@
namespace armarx
{
- bool cprs::And::shouldAllocateComputingPower(const Ice::Current&)
+ bool
+ cprs::And::shouldAllocateComputingPower(const Ice::Current&)
{
- return std::all_of(requestStrategies.begin(), requestStrategies.end(),
- [](ComputingPowerRequestStrategyBasePtr & s)
- {
- return s->shouldAllocateComputingPower();
- }
- );
+ return std::all_of(requestStrategies.begin(),
+ requestStrategies.end(),
+ [](ComputingPowerRequestStrategyBasePtr& s)
+ { return s->shouldAllocateComputingPower(); });
}
- void cprs::ElapsedTime::allocatedComputingPower(const Ice::Current&)
+ void
+ cprs::ElapsedTime::allocatedComputingPower(const Ice::Current&)
{
if (skipping)
{
@@ -44,11 +44,12 @@ namespace armarx
}
else
{
- allocatedLastTime += std::chrono::seconds {timeDeltaInSeconds};
+ allocatedLastTime += std::chrono::seconds{timeDeltaInSeconds};
}
}
- void cprs::TotalNodeCount::allocatedComputingPower(const Ice::Current&)
+ void
+ cprs::TotalNodeCount::allocatedComputingPower(const Ice::Current&)
{
if (skipping)
{
@@ -60,7 +61,8 @@ namespace armarx
}
}
- void cprs::TaskStatus::updateNodeCount(Ice::Long count, const Ice::Current&)
+ void
+ cprs::TaskStatus::updateNodeCount(Ice::Long count, const Ice::Current&)
{
for (auto& strat : strategyPerTaskStatus)
{
@@ -68,7 +70,8 @@ namespace armarx
}
}
- void cprs::TaskStatus::updateTaskStatus(armarx::TaskStatus::Status newStatus, const Ice::Current&)
+ void
+ cprs::TaskStatus::updateTaskStatus(armarx::TaskStatus::Status newStatus, const Ice::Current&)
{
const auto it = strategyPerTaskStatus.find(newStatus);
current = (it == strategyPerTaskStatus.end()) ? nullptr : it->second.get();
@@ -79,7 +82,8 @@ namespace armarx
}
}
- void cprs::TaskStatus::allocatedComputingPower(const Ice::Current&)
+ void
+ cprs::TaskStatus::allocatedComputingPower(const Ice::Current&)
{
for (auto& strat : strategyPerTaskStatus)
{
@@ -87,7 +91,10 @@ namespace armarx
}
}
- void cprs::TaskStatus::updateNodeCreations(Ice::Long nodesCreated, Ice::Long tries, const Ice::Current&)
+ void
+ cprs::TaskStatus::updateNodeCreations(Ice::Long nodesCreated,
+ Ice::Long tries,
+ const Ice::Current&)
{
for (auto& strat : strategyPerTaskStatus)
{
@@ -95,17 +102,22 @@ namespace armarx
}
}
- void cprs::NoNodeCreated::updateNodeCreations(Ice::Long nodesCreated, Ice::Long tries, const Ice::Current&)
+ void
+ cprs::NoNodeCreated::updateNodeCreations(Ice::Long nodesCreated,
+ Ice::Long tries,
+ const Ice::Current&)
{
for (std::size_t i = 0; i < static_cast<std::size_t>(tries); ++i)
{
- backlog.at((currentBacklogIndex + i) % backlogSize) = i < static_cast<std::size_t>(nodesCreated) ? 0 : 1;
+ backlog.at((currentBacklogIndex + i) % backlogSize) =
+ i < static_cast<std::size_t>(nodesCreated) ? 0 : 1;
}
currentBacklogIndex = (currentBacklogIndex + tries) % backlogSize;
}
- void cprs::NoNodeCreated::allocatedComputingPower(const Ice::Current&)
+ void
+ cprs::NoNodeCreated::allocatedComputingPower(const Ice::Current&)
{
const auto perc = std::accumulate(backlog.begin(), backlog.end(), 1.f) / backlog.size();
const auto usedTimeDelta = timeDeltaInSeconds / (1.f + sigma * perc);
@@ -117,11 +129,12 @@ namespace armarx
}
else
{
- allocatedLastTime += std::chrono::seconds {static_cast<std::size_t>(usedTimeDelta)};
+ allocatedLastTime += std::chrono::seconds{static_cast<std::size_t>(usedTimeDelta)};
}
}
- void cprs::CompoundedRequestStrategy::setCurrentStateAsInitialState(const Ice::Current&)
+ void
+ cprs::CompoundedRequestStrategy::setCurrentStateAsInitialState(const Ice::Current&)
{
for (auto& s : requestStrategies)
{
@@ -129,7 +142,8 @@ namespace armarx
}
}
- void cprs::CompoundedRequestStrategy::updateNodeCount(Ice::Long count, const Ice::Current&)
+ void
+ cprs::CompoundedRequestStrategy::updateNodeCount(Ice::Long count, const Ice::Current&)
{
for (auto& s : requestStrategies)
{
@@ -137,7 +151,9 @@ namespace armarx
}
}
- void cprs::CompoundedRequestStrategy::updateTaskStatus(armarx::TaskStatus::Status newStatus, const Ice::Current&)
+ void
+ cprs::CompoundedRequestStrategy::updateTaskStatus(armarx::TaskStatus::Status newStatus,
+ const Ice::Current&)
{
for (auto& s : requestStrategies)
{
@@ -145,7 +161,8 @@ namespace armarx
}
}
- void cprs::CompoundedRequestStrategy::allocatedComputingPower(const Ice::Current&)
+ void
+ cprs::CompoundedRequestStrategy::allocatedComputingPower(const Ice::Current&)
{
for (auto& s : requestStrategies)
{
@@ -153,7 +170,10 @@ namespace armarx
}
}
- void cprs::CompoundedRequestStrategy::updateNodeCreations(Ice::Long nodesCreated, Ice::Long tries, const Ice::Current&)
+ void
+ cprs::CompoundedRequestStrategy::updateNodeCreations(Ice::Long nodesCreated,
+ Ice::Long tries,
+ const Ice::Current&)
{
for (auto& s : requestStrategies)
{
@@ -161,4 +181,4 @@ namespace armarx
}
}
-}
+} // namespace armarx
diff --git a/source/RobotComponents/components/MotionPlanning/ResourceRequestStrategies/ComputingPowerRequestStrategy.h b/source/RobotComponents/components/MotionPlanning/ResourceRequestStrategies/ComputingPowerRequestStrategy.h
index 38f070a8bbe677345cdb4ae09c6245e2e7852a57..ec29cef2beb31fa286aa78fbdf901ea0acc3567b 100644
--- a/source/RobotComponents/components/MotionPlanning/ResourceRequestStrategies/ComputingPowerRequestStrategy.h
+++ b/source/RobotComponents/components/MotionPlanning/ResourceRequestStrategies/ComputingPowerRequestStrategy.h
@@ -37,8 +37,7 @@ namespace armarx::cprs
* @brief Implementation of ComputingPowerRequestStrategyBase
* @see ComputingPowerRequestStrategyBase
*/
- class ComputingPowerRequestStrategy:
- virtual public ComputingPowerRequestStrategyBase
+ class ComputingPowerRequestStrategy : virtual public ComputingPowerRequestStrategyBase
{
public:
/**
@@ -50,7 +49,8 @@ namespace armarx::cprs
* @brief Default implementation. Does nothing.
* @see ComputingPowerRequestStrategyBase::setCurrentStateAsInitialState
*/
- void setCurrentStateAsInitialState(const ::Ice::Current& = Ice::emptyCurrent) override
+ void
+ setCurrentStateAsInitialState(const ::Ice::Current& = Ice::emptyCurrent) override
{
}
@@ -58,14 +58,18 @@ namespace armarx::cprs
* @brief Default implementation. Does nothing.
* @see ComputingPowerRequestStrategyBase::updateNodeCount
*/
- void updateNodeCount(Ice::Long, const Ice::Current& = Ice::emptyCurrent) override
+ void
+ updateNodeCount(Ice::Long, const Ice::Current& = Ice::emptyCurrent) override
{
}
+
/**
* @brief Default implementation. Does nothing.
* @see ComputingPowerRequestStrategyBase::updateTaskStatus
*/
- void updateTaskStatus(armarx::TaskStatus::Status, const Ice::Current& = Ice::emptyCurrent) override
+ void
+ updateTaskStatus(armarx::TaskStatus::Status,
+ const Ice::Current& = Ice::emptyCurrent) override
{
}
@@ -73,7 +77,8 @@ namespace armarx::cprs
* @brief Default implementation. Does nothing.
* @see ComputingPowerRequestStrategyBase::allocatedComputingPower
*/
- void allocatedComputingPower(const Ice::Current& = Ice::emptyCurrent) override
+ void
+ allocatedComputingPower(const Ice::Current& = Ice::emptyCurrent) override
{
}
@@ -81,7 +86,8 @@ namespace armarx::cprs
* @brief Default implementation. Does nothing.
* @see ComputingPowerRequestStrategyBase::updateNodeCreations
*/
- void updateNodeCreations(Ice::Long, Ice::Long, const Ice::Current& = Ice::emptyCurrent) override
+ void
+ updateNodeCreations(Ice::Long, Ice::Long, const Ice::Current& = Ice::emptyCurrent) override
{
}
@@ -93,13 +99,14 @@ namespace armarx::cprs
*/
bool shouldAllocateComputingPower(const ::Ice::Current& = Ice::emptyCurrent) override = 0;
};
+
using ComputingPowerRequestStrategyPtr = IceInternal::Handle<ComputingPowerRequestStrategy>;
/**
* @brief Implementation of CompoundedRequestStrategyBase
* @see CompoundedRequestStrategyBase
*/
- class CompoundedRequestStrategy:
+ class CompoundedRequestStrategy :
virtual public CompoundedRequestStrategyBase,
virtual public ComputingPowerRequestStrategy
{
@@ -108,7 +115,10 @@ namespace armarx::cprs
* @brief Ctor
* @param requestStrategies The strategies forming this compound.
*/
- CompoundedRequestStrategy(const ComputingPowerRequestStrategyBaseList& requestStrategies): CompoundedRequestStrategyBase(requestStrategies) {}
+ CompoundedRequestStrategy(const ComputingPowerRequestStrategyBaseList& requestStrategies) :
+ CompoundedRequestStrategyBase(requestStrategies)
+ {
+ }
/**
* @brief Calls this function for all compounded strategies.
@@ -127,7 +137,8 @@ namespace armarx::cprs
* @see ComputingPowerRequestStrategyBase::updateTaskStatus
* @param newStatus The new status
*/
- void updateTaskStatus(armarx::TaskStatus::Status newStatus, const Ice::Current& = Ice::emptyCurrent) override;
+ void updateTaskStatus(armarx::TaskStatus::Status newStatus,
+ const Ice::Current& = Ice::emptyCurrent) override;
/**
* @brief Calls this function for all compounded strategies.
@@ -141,11 +152,14 @@ namespace armarx::cprs
* @param nodesCreated Number of created nodes.
* @param tries Number of tries to create nodes.
*/
- void updateNodeCreations(Ice::Long nodesCreated, Ice::Long tries, const Ice::Current& = Ice::emptyCurrent) override;
+ void updateNodeCreations(Ice::Long nodesCreated,
+ Ice::Long tries,
+ const Ice::Current& = Ice::emptyCurrent) override;
protected:
//for object factory
- template<class Base, class Derived> friend class armarx::GenericFactory;
+ template <class Base, class Derived>
+ friend class armarx::GenericFactory;
/**
* @brief Ctor used for object factories.
*/
@@ -156,93 +170,97 @@ namespace armarx::cprs
* @brief Implementation of AndBase
* @see AndBase
*/
- class And:
- virtual public AndBase,
- virtual public CompoundedRequestStrategy
+ class And : virtual public AndBase, virtual public CompoundedRequestStrategy
{
public:
/**
* @brief Ctor
* @param requestStrategies The strategies forming this compound.
*/
- And(const ComputingPowerRequestStrategyBaseList& requestStrategies):
- AndBase(requestStrategies),
- CompoundedRequestStrategy(requestStrategies)
- {}
+ And(const ComputingPowerRequestStrategyBaseList& requestStrategies) :
+ AndBase(requestStrategies), CompoundedRequestStrategy(requestStrategies)
+ {
+ }
+
/**
* @brief Returns true if all compounded strategies return true.
* @return True if all compounded strategies return true.
*/
bool shouldAllocateComputingPower(const ::Ice::Current& = Ice::emptyCurrent) override;
+
protected:
//for object factory
- template<class Base, class Derived> friend class armarx::GenericFactory;
+ template <class Base, class Derived>
+ friend class armarx::GenericFactory;
/**
* @brief Ctor used for object factories.
*/
And() = default;
};
+
using AndPtr = IceInternal::Handle<And>;
/**
* @brief Implementation of OrBase
* @see OrBase
*/
- class Or:
- virtual public OrBase,
- virtual public CompoundedRequestStrategy
+ class Or : virtual public OrBase, virtual public CompoundedRequestStrategy
{
public:
/**
* @brief Ctor
* @param requestStrategies The strategies forming this compound.
*/
- Or(const ComputingPowerRequestStrategyBaseList& requestStrategies):
- OrBase(requestStrategies),
- CompoundedRequestStrategy(requestStrategies)
- {}
+ Or(const ComputingPowerRequestStrategyBaseList& requestStrategies) :
+ OrBase(requestStrategies), CompoundedRequestStrategy(requestStrategies)
+ {
+ }
+
/**
* @brief Returns true if any compounded strategies returns true.
* @return True if all compounded strategies return true.
*/
- bool shouldAllocateComputingPower(const ::Ice::Current& = Ice::emptyCurrent) override
+ bool
+ shouldAllocateComputingPower(const ::Ice::Current& = Ice::emptyCurrent) override
{
- return std::any_of(requestStrategies.begin(), requestStrategies.end(),
- [](ComputingPowerRequestStrategyBasePtr & s)
- {
- return s->shouldAllocateComputingPower();
- }
- );
+ return std::any_of(requestStrategies.begin(),
+ requestStrategies.end(),
+ [](ComputingPowerRequestStrategyBasePtr& s)
+ { return s->shouldAllocateComputingPower(); });
}
+
protected:
//for object factory
- template<class Base, class Derived> friend class armarx::GenericFactory;
+ template <class Base, class Derived>
+ friend class armarx::GenericFactory;
/**
* @brief Ctor used for object factories.
*/
Or() = default;
};
+
using OrPtr = IceInternal::Handle<Or>;
/**
* @brief Implementation of NotBase
* @see NotBase
*/
- class Not:
- virtual public NotBase,
- virtual public ComputingPowerRequestStrategy
+ class Not : virtual public NotBase, virtual public ComputingPowerRequestStrategy
{
public:
/**
* @brief Ctor
* @param s The negated strategy.
*/
- Not(const ComputingPowerRequestStrategyBasePtr& s): NotBase(s) {}
+ Not(const ComputingPowerRequestStrategyBasePtr& s) : NotBase(s)
+ {
+ }
/**
* @brief Passes the call to the contained strategy.
*/
- void setCurrentStateAsInitialState(const ::Ice::Current& = Ice::emptyCurrent) override
+ void
+ setCurrentStateAsInitialState(const ::Ice::Current& = Ice::emptyCurrent) override
{
allocStrat->setCurrentStateAsInitialState();
}
@@ -251,7 +269,8 @@ namespace armarx::cprs
* @brief Passes the call to the contained strategy.
* @param count The new node count.
*/
- void updateNodeCount(Ice::Long count, const Ice::Current& = Ice::emptyCurrent) override
+ void
+ updateNodeCount(Ice::Long count, const Ice::Current& = Ice::emptyCurrent) override
{
allocStrat->updateNodeCount(count);
}
@@ -260,7 +279,9 @@ namespace armarx::cprs
* @brief Passes the call to the contained strategy.
* @param newStatus The new task status.
*/
- void updateTaskStatus(armarx::TaskStatus::Status newStatus, const Ice::Current& = Ice::emptyCurrent) override
+ void
+ updateTaskStatus(armarx::TaskStatus::Status newStatus,
+ const Ice::Current& = Ice::emptyCurrent) override
{
allocStrat->updateTaskStatus(newStatus);
}
@@ -268,7 +289,8 @@ namespace armarx::cprs
/**
* @brief Passes the call to the contained strategy.
*/
- void allocatedComputingPower(const Ice::Current& = Ice::emptyCurrent) override
+ void
+ allocatedComputingPower(const Ice::Current& = Ice::emptyCurrent) override
{
allocStrat->allocatedComputingPower();
}
@@ -277,7 +299,8 @@ namespace armarx::cprs
* @brief Returns the negatedresult of the contained strategy.
* @return The negatedresult of the contained strategy.
*/
- bool shouldAllocateComputingPower(const ::Ice::Current& = Ice::emptyCurrent) override
+ bool
+ shouldAllocateComputingPower(const ::Ice::Current& = Ice::emptyCurrent) override
{
return !allocStrat->shouldAllocateComputingPower();
}
@@ -288,67 +311,71 @@ namespace armarx::cprs
* @param nodesCreated Number of created nodes.
* @param tries Number of tries to create nodes.
*/
- void updateNodeCreations(Ice::Long nodesCreated, Ice::Long tries, const Ice::Current& = Ice::emptyCurrent) override
+ void
+ updateNodeCreations(Ice::Long nodesCreated,
+ Ice::Long tries,
+ const Ice::Current& = Ice::emptyCurrent) override
{
allocStrat->updateNodeCreations(nodesCreated, tries);
}
+
protected:
//for object factory
- template<class Base, class Derived> friend class armarx::GenericFactory;
+ template <class Base, class Derived>
+ friend class armarx::GenericFactory;
/**
* @brief Ctor used for object factories.
*/
Not() = default;
};
+
using NotPtr = IceInternal::Handle<Not>;
/**
* @brief Implementation of Always
* @see Always
*/
- class Always:
- virtual public AlwaysBase,
- virtual public ComputingPowerRequestStrategy
+ class Always : virtual public AlwaysBase, virtual public ComputingPowerRequestStrategy
{
public:
/**
* @brief Always return true.
* @return true
*/
- bool shouldAllocateComputingPower(const ::Ice::Current& = Ice::emptyCurrent) override
+ bool
+ shouldAllocateComputingPower(const ::Ice::Current& = Ice::emptyCurrent) override
{
return true;
}
};
+
using AlwaysPtr = IceInternal::Handle<Always>;
/**
* @brief Implementation of NeverBase
* @see NeverBase
*/
- class Never:
- virtual public NeverBase,
- virtual public ComputingPowerRequestStrategy
+ class Never : virtual public NeverBase, virtual public ComputingPowerRequestStrategy
{
public:
/**
* @brief Always return false.
* @return false
*/
- bool shouldAllocateComputingPower(const ::Ice::Current& = Ice::emptyCurrent) override
+ bool
+ shouldAllocateComputingPower(const ::Ice::Current& = Ice::emptyCurrent) override
{
return false;
}
};
+
using NeverPtr = IceInternal::Handle<Never>;
/**
* @brief Implementation of ElapsedTimeBase
* @see ElapsedTimeBase
*/
- class ElapsedTime:
- virtual public ElapsedTimeBase,
- virtual public ComputingPowerRequestStrategy
+ class ElapsedTime : virtual public ElapsedTimeBase, virtual public ComputingPowerRequestStrategy
{
public:
/**
@@ -365,16 +392,16 @@ namespace armarx::cprs
* @param timeDeltaInSeconds The delta to use.(in seconds)
* @param skipping Whether skipping is activated.
*/
- ElapsedTime(Ice::Long timeDeltaInSeconds, bool skipping):
- ElapsedTimeBase(timeDeltaInSeconds, skipping),
- allocatedLastTime {}
+ ElapsedTime(Ice::Long timeDeltaInSeconds, bool skipping) :
+ ElapsedTimeBase(timeDeltaInSeconds, skipping), allocatedLastTime{}
{
}
/**
* @brief Sets the next time to allocate delta seconds from now.
*/
- void setCurrentStateAsInitialState(const ::Ice::Current& = Ice::emptyCurrent) override
+ void
+ setCurrentStateAsInitialState(const ::Ice::Current& = Ice::emptyCurrent) override
{
allocatedLastTime = Clock::now();
}
@@ -390,10 +417,12 @@ namespace armarx::cprs
* @brief Returns true if the last creation timepoint is more than delta seconds away.
* @return Whether more computing power should be allocated.
*/
- bool shouldAllocateComputingPower(const ::Ice::Current& = Ice::emptyCurrent) override
+ bool
+ shouldAllocateComputingPower(const ::Ice::Current& = Ice::emptyCurrent) override
{
- return Clock::now() >= allocatedLastTime + std::chrono::seconds {timeDeltaInSeconds};
+ return Clock::now() >= allocatedLastTime + std::chrono::seconds{timeDeltaInSeconds};
}
+
protected:
/**
* @brief The time point when more power should be allocated again.
@@ -401,22 +430,21 @@ namespace armarx::cprs
TimePoint allocatedLastTime;
//for object factory
- template<class Base, class Derived> friend class armarx::GenericFactory;
+ template <class Base, class Derived>
+ friend class armarx::GenericFactory;
/**
* @brief Ctor used for object factories.
*/
ElapsedTime() = default;
};
- using ElapsedTimePtr = IceInternal::Handle<ElapsedTime>;
+ using ElapsedTimePtr = IceInternal::Handle<ElapsedTime>;
/**
* @brief Implementation of NoNodeCreatedBase
* @see NoNodeCreatedBase
*/
- class NoNodeCreated:
- virtual public NoNodeCreatedBase,
- virtual public ElapsedTime
+ class NoNodeCreated : virtual public NoNodeCreatedBase, virtual public ElapsedTime
{
public:
/**
@@ -426,11 +454,11 @@ namespace armarx::cprs
* @param sig The parameter sigma.
* @param backlogSz The backlog's size.
*/
- NoNodeCreated(Ice::Long timeDeltaInSec, bool skip, float sig, Ice::Long backlogSz):
+ NoNodeCreated(Ice::Long timeDeltaInSec, bool skip, float sig, Ice::Long backlogSz) :
NoNodeCreatedBase(timeDeltaInSec, skip, sig, backlogSz),
ElapsedTime(timeDeltaInSec, skip),
backlog(backlogSz, 0),
- currentBacklogIndex {0}
+ currentBacklogIndex{0}
{
timeDeltaInSeconds = timeDeltaInSec; //somehow does not get set by ctor call
ARMARX_CHECK_EXPRESSION(timeDeltaInSeconds >= 0);
@@ -442,7 +470,9 @@ namespace armarx::cprs
* @param nodesCreated Number of created nodes.
* @param tries Number of tries to create nodes.
*/
- void updateNodeCreations(Ice::Long nodesCreated, Ice::Long tries, const Ice::Current& = Ice::emptyCurrent) override;
+ void updateNodeCreations(Ice::Long nodesCreated,
+ Ice::Long tries,
+ const Ice::Current& = Ice::emptyCurrent) override;
void allocatedComputingPower(const Ice::Current& = Ice::emptyCurrent) override;
@@ -450,18 +480,21 @@ namespace armarx::cprs
* @brief shouldAllocateComputingPower
* @return Whrether
*/
- bool shouldAllocateComputingPower(const ::Ice::Current& = Ice::emptyCurrent) override
+ bool
+ shouldAllocateComputingPower(const ::Ice::Current& = Ice::emptyCurrent) override
{
const auto perc = std::accumulate(backlog.begin(), backlog.end(), 1.f) / backlog.size();
const auto usedTimeDelta = timeDeltaInSeconds / (1.f + sigma * perc);
ARMARX_CHECK_EXPRESSION(usedTimeDelta >= 0);
- return Clock::now() >= allocatedLastTime + std::chrono::seconds {static_cast<std::size_t>(usedTimeDelta)};
+ return Clock::now() >= allocatedLastTime + std::chrono::seconds{
+ static_cast<std::size_t>(usedTimeDelta)};
}
/**
* @brief Nulls the backlog after the object was transmitted through ice.
*/
- void ice_postUnmarshal() override
+ void
+ ice_postUnmarshal() override
{
backlog.assign(backlogSize, 0);
}
@@ -478,19 +511,21 @@ namespace armarx::cprs
std::size_t currentBacklogIndex;
//for object factory
- template<class Base, class Derived> friend class armarx::GenericFactory;
+ template <class Base, class Derived>
+ friend class armarx::GenericFactory;
/**
* @brief Ctor used for object factories.
*/
NoNodeCreated() = default;
};
+
using NoNodeCreatedPtr = IceInternal::Handle<NoNodeCreated>;
/**
* @brief Implementation of TotalNodeCountBase
* @see TotalNodeCountBase
*/
- class TotalNodeCount:
+ class TotalNodeCount :
virtual public TotalNodeCountBase,
virtual public ComputingPowerRequestStrategy
{
@@ -500,10 +535,8 @@ namespace armarx::cprs
* @param nodeCountDelta The delta to use.
* @param skipping Whether skipping is activated.
*/
- TotalNodeCount(Ice::Long nodeCountDelta, bool skipping):
- TotalNodeCountBase(nodeCountDelta, skipping),
- allocateNextTime {0},
- nodeCount {0}
+ TotalNodeCount(Ice::Long nodeCountDelta, bool skipping) :
+ TotalNodeCountBase(nodeCountDelta, skipping), allocateNextTime{0}, nodeCount{0}
{
}
@@ -511,7 +544,8 @@ namespace armarx::cprs
* @brief Updates the internal node count.
* @param newCount The new node count.
*/
- void updateNodeCount(Ice::Long newCount, const Ice::Current& = Ice::emptyCurrent) override
+ void
+ updateNodeCount(Ice::Long newCount, const Ice::Current& = Ice::emptyCurrent) override
{
nodeCount = newCount;
}
@@ -527,7 +561,8 @@ namespace armarx::cprs
* @brief Returns true if the the current count is >= the next creation count.
* @return Whether more computing power should be allocated.
*/
- bool shouldAllocateComputingPower(const ::Ice::Current& = Ice::emptyCurrent) override
+ bool
+ shouldAllocateComputingPower(const ::Ice::Current& = Ice::emptyCurrent) override
{
return nodeCount >= allocateNextTime;
}
@@ -543,30 +578,32 @@ namespace armarx::cprs
Ice::Long nodeCount;
//for object factory
- template<class Base, class Derived> friend class armarx::GenericFactory;
+ template <class Base, class Derived>
+ friend class armarx::GenericFactory;
+
/**
* @brief Ctor used for object factories.
*/
- TotalNodeCount(): allocateNextTime {0}, nodeCount {0} {}
+ TotalNodeCount() : allocateNextTime{0}, nodeCount{0}
+ {
+ }
};
+
using TotalNodeCountPtr = IceInternal::Handle<TotalNodeCount>;
/**
* @brief Implementation of TaskStatusBase
* @see TaskStatusBase
*/
- class TaskStatus:
- virtual public TaskStatusBase,
- virtual public ComputingPowerRequestStrategy
+ class TaskStatus : virtual public TaskStatusBase, virtual public ComputingPowerRequestStrategy
{
public:
/**
* @brief Ctor
* @param strategyPerTaskStatus Task status to sub strategy map.
*/
- TaskStatus(TaskStatusMap strategyPerTaskStatus):
- TaskStatusBase(strategyPerTaskStatus),
- current {nullptr}
+ TaskStatus(TaskStatusMap strategyPerTaskStatus) :
+ TaskStatusBase(strategyPerTaskStatus), current{nullptr}
{
}
@@ -580,7 +617,8 @@ namespace armarx::cprs
* @brief Switches the current strategy and passes this call to all sub strategies.
* @param newStatus
*/
- void updateTaskStatus(armarx::TaskStatus::Status newStatus, const Ice::Current& = Ice::emptyCurrent) override;
+ void updateTaskStatus(armarx::TaskStatus::Status newStatus,
+ const Ice::Current& = Ice::emptyCurrent) override;
/**
* @brief Passes this call to all sub strategies.
@@ -593,17 +631,21 @@ namespace armarx::cprs
* @param nodesCreated Number of created nodes.
* @param tries Number of tries to create nodes.
*/
- void updateNodeCreations(Ice::Long nodesCreated, Ice::Long tries, const Ice::Current& = Ice::emptyCurrent) override;
+ void updateNodeCreations(Ice::Long nodesCreated,
+ Ice::Long tries,
+ const Ice::Current& = Ice::emptyCurrent) override;
/**
* @brief Returns the result of the current selected strategy.
* @return The result of the current selected strategy.
*/
- bool shouldAllocateComputingPower(const ::Ice::Current& = Ice::emptyCurrent) override
+ bool
+ shouldAllocateComputingPower(const ::Ice::Current& = Ice::emptyCurrent) override
{
ARMARX_CHECK_EXPRESSION(current);
return current->shouldAllocateComputingPower();
}
+
protected:
/**
* @brief The current strategy in use.
@@ -611,12 +653,16 @@ namespace armarx::cprs
ComputingPowerRequestStrategyBase* current;
//for object factory
- template<class Base, class Derived> friend class armarx::GenericFactory;
+ template <class Base, class Derived>
+ friend class armarx::GenericFactory;
+
/**
* @brief Ctor used for object factories.
*/
- TaskStatus(): current {nullptr} {}
+ TaskStatus() : current{nullptr}
+ {
+ }
};
- using TaskStatusPtr = IceInternal::Handle<TaskStatus>;
-}
+ using TaskStatusPtr = IceInternal::Handle<TaskStatus>;
+} // namespace armarx::cprs
diff --git a/source/RobotComponents/components/MotionPlanning/Tasks/AStar/Task.cpp b/source/RobotComponents/components/MotionPlanning/Tasks/AStar/Task.cpp
index 5d0c9b601201040015bee30f1b1b7a6ed9005afd..682a14ba5283e7af94472237c2403adc4e9de93d 100644
--- a/source/RobotComponents/components/MotionPlanning/Tasks/AStar/Task.cpp
+++ b/source/RobotComponents/components/MotionPlanning/Tasks/AStar/Task.cpp
@@ -23,14 +23,20 @@
*/
#include "Task.h"
-#include "../../util/CollisionCheckUtil.h"
#include <algorithm>
+#include "../../util/CollisionCheckUtil.h"
+
namespace armarx::astar
{
- Task::Task(const CSpaceBasePtr& cspace, const VectorXf& startCfg, const VectorXf& goalCfg, const std::string& taskName, float dcdStep,
- float gridStepSize, Ice::Long maximalPlanningTimeInSeconds)
+ Task::Task(const CSpaceBasePtr& cspace,
+ const VectorXf& startCfg,
+ const VectorXf& goalCfg,
+ const std::string& taskName,
+ float dcdStep,
+ float gridStepSize,
+ Ice::Long maximalPlanningTimeInSeconds)
{
this->cspace = cspace->clone();
this->startCfg = startCfg;
@@ -40,25 +46,29 @@ namespace armarx::astar
this->maximalPlanningTimeInSeconds = maximalPlanningTimeInSeconds;
this->taskName = taskName;
- if (startCfg.size() != goalCfg.size() || startCfg.size() != static_cast<std::size_t>(this->cspace->getDimensionality()))
+ if (startCfg.size() != goalCfg.size() ||
+ startCfg.size() != static_cast<std::size_t>(this->cspace->getDimensionality()))
{
- throw std::invalid_argument {"start and goal have to be the size of the cspace's dimensionality"};
+ throw std::invalid_argument{
+ "start and goal have to be the size of the cspace's dimensionality"};
}
if (gridStepSize < dcdStep)
{
- throw std::invalid_argument {"the step size of the implicit grid has to be larger than the DCD step size"};
+ throw std::invalid_argument{
+ "the step size of the implicit grid has to be larger than the DCD step size"};
}
}
-
- Path Task::getPath(const Ice::Current&) const
+ Path
+ Task::getPath(const Ice::Current&) const
{
- std::lock_guard<std::mutex> lock {mtx};
+ std::lock_guard<std::mutex> lock{mtx};
return solution;
}
- void Task::run(const RemoteObjectNodePrxList&, const Ice::Current&)
+ void
+ Task::run(const RemoteObjectNodePrxList&, const Ice::Current&)
{
const std::size_t n = cspace->getDimensionality();
setTaskStatus(TaskStatus::ePlanning);
@@ -66,15 +76,20 @@ namespace armarx::astar
//check trivial cases
cspace->initCollisionTest();
- const auto startIsCollisionFree = cspace->isCollisionFree(std::pair<const Ice::Float*, const Ice::Float*> {startCfg.data(), startCfg.data() + startCfg.size()});
- const auto goalIscollisionFree = cspace->isCollisionFree(std::pair<const Ice::Float*, const Ice::Float*> { goalCfg.data(), goalCfg.data() + goalCfg.size()});
+ const auto startIsCollisionFree =
+ cspace->isCollisionFree(std::pair<const Ice::Float*, const Ice::Float*>{
+ startCfg.data(), startCfg.data() + startCfg.size()});
+ const auto goalIscollisionFree =
+ cspace->isCollisionFree(std::pair<const Ice::Float*, const Ice::Float*>{
+ goalCfg.data(), goalCfg.data() + goalCfg.size()});
if (!startIsCollisionFree || !goalIscollisionFree)
{
setTaskStatus(TaskStatus::ePlanningFailed);
return;
}
- const auto distStartEnd = euclideanDistance(startCfg.begin(), startCfg.end(), goalCfg.begin());
+ const auto distStartEnd =
+ euclideanDistance(startCfg.begin(), startCfg.end(), goalCfg.begin());
if (distStartEnd <= dcdStep)
{
@@ -91,25 +106,25 @@ namespace armarx::astar
LongRangeSeq gridBounds;
gridBounds.reserve(n);
const auto&& cspaceBounds = cspace->getDimensionsBounds();
- std::transform(
- cspaceBounds.begin(), cspaceBounds.end(), startCfg.begin(), std::back_inserter(gridBounds),
- [this](const FloatRange & dimBounds, float start)
- {
+ std::transform(cspaceBounds.begin(),
+ cspaceBounds.end(),
+ startCfg.begin(),
+ std::back_inserter(gridBounds),
+ [this](const FloatRange& dimBounds, float start)
+ {
+ const auto distLower = dimBounds.min - start;
+ const auto gridPointsLower = std::trunc(distLower / gridStepSize);
- const auto distLower = dimBounds.min - start;
- const auto gridPointsLower = std::trunc(distLower / gridStepSize);
+ const auto distHigher = dimBounds.max - start;
+ const auto gridPointsHigher = std::trunc(distHigher / gridStepSize);
- const auto distHigher = dimBounds.max - start;
- const auto gridPointsHigher = std::trunc(distHigher / gridStepSize);
-
- return LongRange {static_cast<Ice::Long>(gridPointsLower), static_cast<Ice::Long>(gridPointsHigher)};
-
- }
- );
+ return LongRange{static_cast<Ice::Long>(gridPointsLower),
+ static_cast<Ice::Long>(gridPointsHigher)};
+ });
//calculates all neighbours in the grid (only along the cspace's coordinate axes)
- auto getNeighbours = [gridBounds, n](const GridPointType & p)
+ auto getNeighbours = [gridBounds, n](const GridPointType& p)
{
std::vector<GridPointType> neighbours;
//allow diag neighbours
@@ -120,28 +135,29 @@ namespace armarx::astar
const auto n = neighbours.size();
const auto generateLower = (gridBounds.at(i).min < p.at(i));
const auto generateHigher = (gridBounds.at(i).max > p.at(i));
- neighbours.reserve(n * (1 + generateHigher + generateLower));//shold be already big enough
+ neighbours.reserve(
+ n * (1 + generateHigher + generateLower)); //shold be already big enough
if (generateLower)
{
- std::transform(
- neighbours.begin(), neighbours.begin() + n, std::back_inserter(neighbours),
- [i](typename decltype(neighbours)::value_type v)
- {
- --v.at(i);
- return v;
- }
- );
+ std::transform(neighbours.begin(),
+ neighbours.begin() + n,
+ std::back_inserter(neighbours),
+ [i](typename decltype(neighbours)::value_type v)
+ {
+ --v.at(i);
+ return v;
+ });
}
if (generateHigher)
{
- std::transform(
- neighbours.begin(), neighbours.begin() + n, std::back_inserter(neighbours),
- [i](typename decltype(neighbours)::value_type v)
- {
- ++v.at(i);
- return v;
- }
- );
+ std::transform(neighbours.begin(),
+ neighbours.begin() + n,
+ std::back_inserter(neighbours),
+ [i](typename decltype(neighbours)::value_type v)
+ {
+ ++v.at(i);
+ return v;
+ });
}
}
ARMARX_CHECK_EXPRESSION(p == neighbours.front());
@@ -149,24 +165,20 @@ namespace armarx::astar
};
//define grid conversion / distance meassure
- auto toConfig = [this](const GridPointType & gridPoint)
+ auto toConfig = [this](const GridPointType& gridPoint)
{
VectorXf cfg;
cfg.reserve(gridPoint.size());
- std::transform(
- gridPoint.begin(), gridPoint.end(), startCfg.begin(), std::back_inserter(cfg),
- [this](float g, float s)
- {
- return s + g * gridStepSize;
- }
- );
+ std::transform(gridPoint.begin(),
+ gridPoint.end(),
+ startCfg.begin(),
+ std::back_inserter(cfg),
+ [this](float g, float s) { return s + g * gridStepSize; });
return cfg;
};
- auto distance = [](const GridPointType & a, const GridPointType & b)
- {
- return euclideanDistance(a.begin(), a.end(), b.begin());
- };
+ auto distance = [](const GridPointType& a, const GridPointType& b)
+ { return euclideanDistance(a.begin(), a.end(), b.begin()); };
//project goal to grid
GridPointType goal;
@@ -193,6 +205,7 @@ namespace armarx::astar
float gScore = 0; //path cost
std::size_t predecessor = 0;
};
+
//holds all created nodes
std::deque<NodeData> nodes;
@@ -205,10 +218,10 @@ namespace armarx::astar
std::set<std::size_t> closedSet; //holds ids to nodes (these nodes are processed)
- auto assemblePath = [&](std::size_t toId)//returns the found path to a given node
+ auto assemblePath = [&](std::size_t toId) //returns the found path to a given node
{
std::deque<VectorXf> reversedPath;
- reversedPath.emplace_back(goalCfg);//maybe it was not on a grid point
+ reversedPath.emplace_back(goalCfg); //maybe it was not on a grid point
while (true)
{
auto& currentNode = nodes.at(toId);
@@ -225,18 +238,19 @@ namespace armarx::astar
return path;
};
- auto expandNode = [&](std::size_t idToExpand)
+ auto expandNode = [&](std::size_t idToExpand)
{
const auto& toExpand = nodes.at(idToExpand);
- const auto successors = getNeighbours(toExpand.node);//the first neighbour is the point itself
+ const auto successors =
+ getNeighbours(toExpand.node); //the first neighbour is the point itself
for (std::size_t sucIdx = 1; sucIdx < successors.size(); ++sucIdx)
{
auto& successor = successors.at(sucIdx);
//is the successor already in the open or closed set?
- auto successorIt = std::find_if(nodes.begin(), nodes.end(), [&](const NodeData & elem)
- {
- return successor == elem.node;
- });
+ auto successorIt =
+ std::find_if(nodes.begin(),
+ nodes.end(),
+ [&](const NodeData& elem) { return successor == elem.node; });
auto successorId = std::distance(nodes.begin(), successorIt);
if (closedSet.find(successorId) != closedSet.end())
@@ -253,7 +267,8 @@ namespace armarx::astar
}
//if successorIsInOpenSet the cfg will be moved back (2 moves should be faster than alloc + calc)
- auto successorCfg = successorIsInOpenSet ? std::move(successorIt->cfg) : toConfig(successor);
+ auto successorCfg =
+ successorIsInOpenSet ? std::move(successorIt->cfg) : toConfig(successor);
//check path for collision
if (!isPathCollisionFree(toExpand.cfg, successorCfg))
{
@@ -284,7 +299,8 @@ namespace armarx::astar
};
//wait for done or timeout
- const auto endTime = std::chrono::system_clock::now() + std::chrono::seconds {maximalPlanningTimeInSeconds};
+ const auto endTime =
+ std::chrono::system_clock::now() + std::chrono::seconds{maximalPlanningTimeInSeconds};
while (true)
{
@@ -305,13 +321,11 @@ namespace armarx::astar
setTaskStatus(TaskStatus::ePlanningFailed);
return;
}
- auto currentIdIt = std::min_element(
- openSet.begin(), openSet.end(),
- [&](std::size_t osId1, std::size_t osId2)
- {
- return nodes.at(osId1).fScore < nodes.at(osId2).fScore;
- }
- );
+ auto currentIdIt =
+ std::min_element(openSet.begin(),
+ openSet.end(),
+ [&](std::size_t osId1, std::size_t osId2)
+ { return nodes.at(osId1).fScore < nodes.at(osId2).fScore; });
auto currentId = *currentIdIt;
openSet.erase(currentIdIt);
@@ -328,17 +342,16 @@ namespace armarx::astar
ARMARX_CHECK_EXPRESSION(!"unreachable");
}
-
- bool Task::isPathCollisionFree(const VectorXf& from, const VectorXf& to)
+ bool
+ Task::isPathCollisionFree(const VectorXf& from, const VectorXf& to)
{
return dcdIsPathCollisionFree(
- from, to, dcdStep,
- [this](const VectorXf & cfg)
- {
- return cspace->isCollisionFree({cfg.data(), cfg.data() + cfg.size()});
- },
- false
- );
+ from,
+ to,
+ dcdStep,
+ [this](const VectorXf& cfg) {
+ return cspace->isCollisionFree({cfg.data(), cfg.data() + cfg.size()});
+ },
+ false);
}
-}
-
+} // namespace armarx::astar
diff --git a/source/RobotComponents/components/MotionPlanning/Tasks/AStar/Task.h b/source/RobotComponents/components/MotionPlanning/Tasks/AStar/Task.h
index a476721e2cc920b64c52c5557f7d72ac1e621fbf..59819ce2189bd7d4838d972288c68f093046abbb 100644
--- a/source/RobotComponents/components/MotionPlanning/Tasks/AStar/Task.h
+++ b/source/RobotComponents/components/MotionPlanning/Tasks/AStar/Task.h
@@ -23,18 +23,17 @@
*/
#pragma once
-#include <mutex>
+#include <atomic>
#include <chrono>
-#include<atomic>
+#include <mutex>
#include <ArmarXCore/core/system/FactoryCollectionBase.h>
-
#include <ArmarXCore/core/util/distributed/RemoteHandle/RemoteHandle.h>
#include <ArmarXCore/interface/core/RemoteObjectNode.h>
#include <RobotComponents/interface/components/MotionPlanning/Tasks/AStar/Task.h>
-#include "../../util/Metrics.h"
+#include "../../util/Metrics.h"
#include "../MotionPlanningTask.h"
namespace armarx::astar
@@ -42,9 +41,7 @@ namespace armarx::astar
class Task;
using TaskPtr = IceInternal::Handle<Task>;
- class Task:
- public virtual MotionPlanningTaskWithDefaultMembers,
- public virtual TaskBase
+ class Task : public virtual MotionPlanningTaskWithDefaultMembers, public virtual TaskBase
{
public:
/**
@@ -55,7 +52,7 @@ namespace armarx::astar
* @param dcdStep The step size used for discrete collision checking.
* @param maximalPlanningTimeInSeconds The maximal planning time in seconds.
*/
- Task(//problem
+ Task( //problem
const CSpaceBasePtr& cspace,
const VectorXf& startCfg,
const VectorXf& goalCfg,
@@ -63,17 +60,18 @@ namespace armarx::astar
//general
float dcdStep = 0.01f,
float gridStepSize = 2.5f,
- Ice::Long maximalPlanningTimeInSeconds = 300
- );
+ Ice::Long maximalPlanningTimeInSeconds = 300);
//PlanningControlInterface
/**
* @brief Aborts planning.
*/
- void abortTask(const Ice::Current& = Ice::emptyCurrent) override
+ void
+ abortTask(const Ice::Current& = Ice::emptyCurrent) override
{
taskIsAborted = true;
}
+
/**
* @return The found path. (empty if no path is found)
*/
@@ -88,6 +86,7 @@ namespace armarx::astar
void run(const RemoteObjectNodePrxList&, const Ice::Current& = Ice::emptyCurrent) override;
bool isPathCollisionFree(const VectorXf& from, const VectorXf& to);
+
protected:
/**
* @brief Ctor used by object factories.
@@ -96,14 +95,17 @@ namespace armarx::astar
mutable std::mutex mtx;
Path solution = {{}, "Path"};
- std::atomic_bool taskIsAborted {false};
+ std::atomic_bool taskIsAborted{false};
+
private:
- template<class Base, class Derived> friend class ::armarx::GenericFactory;
+ template <class Base, class Derived>
+ friend class ::armarx::GenericFactory;
};
-}
+} // namespace armarx::astar
+
namespace armarx
{
using AStarTask = astar::Task;
using AStarTaskPtr = IceUtil::Handle<AStarTask>;
using AStarTaskHandle = RemoteHandle<MotionPlanningTaskControlInterfacePrx>;
-}
+} // namespace armarx
diff --git a/source/RobotComponents/components/MotionPlanning/Tasks/AdaptiveDynamicDomainInformedRRTStar/ManagerNode.cpp b/source/RobotComponents/components/MotionPlanning/Tasks/AdaptiveDynamicDomainInformedRRTStar/ManagerNode.cpp
index b53e9bacbf80e63720c2ed915a39c3f9e9dac263..fc1b826930a57eab97712c2bb3a9f4222b02efcf 100644
--- a/source/RobotComponents/components/MotionPlanning/Tasks/AdaptiveDynamicDomainInformedRRTStar/ManagerNode.cpp
+++ b/source/RobotComponents/components/MotionPlanning/Tasks/AdaptiveDynamicDomainInformedRRTStar/ManagerNode.cpp
@@ -22,20 +22,22 @@
* GNU General Public License
*/
-#include <numeric>
+#include "ManagerNode.h"
+
#include <algorithm>
+#include <numeric>
#include <ArmarXCore/core/ArmarXObjectScheduler.h>
-#include "../../util/Samplers.h"
+#include "../../util/Samplers.h"
#include "WorkerNode.h"
-#include "ManagerNode.h"
#include "util.h"
namespace armarx::addirrtstar
{
//from managedIceObject
- void ManagerNode::onInitComponent()
+ void
+ ManagerNode::onInitComponent()
{
setTag("ManagerNode");
ARMARX_VERBOSE_S << "start init";
@@ -47,40 +49,32 @@ namespace armarx::addirrtstar
ARMARX_CHECK_EXPRESSION(!maxWorkersPerRemoteObjectNode.size());
maxWorkersPerRemoteObjectNode.reserve(remoteObjectNodes.size());
- std::transform(
- remoteObjectNodes.begin(), remoteObjectNodes.end(),
- std::back_inserter(maxWorkersPerRemoteObjectNode),
- [](const RemoteObjectNodeInterfacePrx & prx)
- {
- const auto count = static_cast<std::size_t>(prx->getNumberOfCores());
- ARMARX_VERBOSE_S << "ROI " << prx.get() << " has " << count << " cores";
- return count;
- }
- );
+ std::transform(remoteObjectNodes.begin(),
+ remoteObjectNodes.end(),
+ std::back_inserter(maxWorkersPerRemoteObjectNode),
+ [](const RemoteObjectNodeInterfacePrx& prx)
+ {
+ const auto count = static_cast<std::size_t>(prx->getNumberOfCores());
+ ARMARX_VERBOSE_S << "ROI " << prx.get() << " has " << count << " cores";
+ return count;
+ });
//there cant be more worker than remote objects
- const Ice::Long maxROCount = std::accumulate(maxWorkersPerRemoteObjectNode.begin(), maxWorkersPerRemoteObjectNode.end(), 0);
+ const Ice::Long maxROCount = std::accumulate(
+ maxWorkersPerRemoteObjectNode.begin(), maxWorkersPerRemoteObjectNode.end(), 0);
maximalWorkerCount = std::min(maximalWorkerCount, maxROCount);
initialWorkerCount = std::min(initialWorkerCount, maxROCount);
activeWorkerCount = 0;
//init tree with start node
- tree.init(
- FullIceTree
- {
- FullNodeDataListList{
- FullNodeDataList{
- FullNodeData{
- startNode, //config
- NodeId{0, 0}, //parent
- std::numeric_limits<Ice::Float>::infinity(), //radius;
- 0.f //fromParentCost
- }
- }
- },
- Ice::LongSeq{ -1}
- },
- addParams,
- maximalWorkerCount //since this tree does no updates the id is not important
+ tree.init(FullIceTree{FullNodeDataListList{FullNodeDataList{FullNodeData{
+ startNode, //config
+ NodeId{0, 0}, //parent
+ std::numeric_limits<Ice::Float>::infinity(), //radius;
+ 0.f //fromParentCost
+ }}},
+ Ice::LongSeq{-1}},
+ addParams,
+ maximalWorkerCount //since this tree does no updates the id is not important
);
tree.increaseWorkerCountTo(maximalWorkerCount);
@@ -93,15 +87,16 @@ namespace armarx::addirrtstar
ARMARX_VERBOSE_S << "done init";
}
- void ManagerNode::onConnectComponent()
+ void
+ ManagerNode::onConnectComponent()
{
ARMARX_VERBOSE_S << "armarx::addirrtstar::ManagerNode::onConnectComponent()";
//start manager thread
- managerThread = std::thread {[this]{this->managerTask();}};
+ managerThread = std::thread{[this] { this->managerTask(); }};
}
-
- void ManagerNode::onExitComponent()
+ void
+ ManagerNode::onExitComponent()
{
ARMARX_VERBOSE_S << "start onExitComponent()";
killRequest = true;
@@ -110,31 +105,37 @@ namespace armarx::addirrtstar
}
//from ManagerNodeBase
- PathWithCost ManagerNode::getBestPath(const Ice::Current&) const
+ PathWithCost
+ ManagerNode::getBestPath(const Ice::Current&) const
{
- std::lock_guard<std::mutex> lock {treeMutex};
+ std::lock_guard<std::mutex> lock{treeMutex};
auto&& path = tree.getBestPath();
path.nodes.emplace_back(goalNode);
return path;
}
- Ice::Long ManagerNode::getPathCount(const Ice::Current&) const
+
+ Ice::Long
+ ManagerNode::getPathCount(const Ice::Current&) const
{
- std::lock_guard<std::mutex> lock {treeMutex};
+ std::lock_guard<std::mutex> lock{treeMutex};
return tree.getPathCount();
}
- PathWithCost ManagerNode::getNthPathWithCost(Ice::Long n, const Ice::Current&) const
+
+ PathWithCost
+ ManagerNode::getNthPathWithCost(Ice::Long n, const Ice::Current&) const
{
- std::lock_guard<std::mutex> lock {treeMutex};
+ std::lock_guard<std::mutex> lock{treeMutex};
auto&& path = tree.getNthPathWithCost(static_cast<std::size_t>(n));
path.nodes.emplace_back(goalNode);
return path;
}
- PathWithCostSeq ManagerNode::getAllPathsWithCost(const Ice::Current&) const
+ PathWithCostSeq
+ ManagerNode::getAllPathsWithCost(const Ice::Current&) const
{
- std::lock_guard<std::mutex> lock {treeMutex};
+ std::lock_guard<std::mutex> lock{treeMutex};
const auto&& pathCount = tree.getPathCount();
- PathWithCostSeq paths {pathCount};
+ PathWithCostSeq paths{pathCount};
for (std::size_t i = 0; i < pathCount; ++i)
{
@@ -145,10 +146,11 @@ namespace armarx::addirrtstar
return paths;
}
- Update ManagerNode::getUpdate(Ice::Long workerId, Ice::Long updateId, const Ice::Current&) const
+ Update
+ ManagerNode::getUpdate(Ice::Long workerId, Ice::Long updateId, const Ice::Current&) const
{
ARMARX_WARNING_S << "worker requested remote update w/u " << workerId << "/" << updateId;
- std::lock_guard<std::mutex> lock {updateMutex};
+ std::lock_guard<std::mutex> lock{updateMutex};
ARMARX_CHECK_EXPRESSION(workerId >= 0);
ARMARX_CHECK_EXPRESSION(updateId >= 0);
@@ -164,7 +166,8 @@ namespace armarx::addirrtstar
return getRemoteUpdate(uWorkerId, uUpdateId);
}
- FullIceTree ManagerNode::getTree(const Ice::Current&) const
+ FullIceTree
+ ManagerNode::getTree(const Ice::Current&) const
{
//since we a copying the tree and all update ids we need both mutexes
std::lock(treeMutex, updateMutex);
@@ -173,7 +176,8 @@ namespace armarx::addirrtstar
return tree.getIceTree();
}
- void ManagerNode::managerTask()
+ void
+ ManagerNode::managerTask()
{
try
{
@@ -181,13 +185,16 @@ namespace armarx::addirrtstar
//since post unmarshall is of this is called BEFORE post unmarshall of the member cspace is called this assert has to be done here!
const auto dim = static_cast<std::size_t>(cspace->getDimensionality());
ARMARX_CHECK_EXPRESSION(startNode.size() == dim);
- ARMARX_CHECK_EXPRESSION(cspace->isValidConfiguration(std::make_pair(&startNode.front(), &startNode.back() + 1)));
+ ARMARX_CHECK_EXPRESSION(cspace->isValidConfiguration(
+ std::make_pair(&startNode.front(), &startNode.back() + 1)));
ARMARX_CHECK_EXPRESSION(goalNode.size() == dim);
- ARMARX_CHECK_EXPRESSION(cspace->isValidConfiguration(std::make_pair(&goalNode.front(), &goalNode.back() + 1)));
+ ARMARX_CHECK_EXPRESSION(cspace->isValidConfiguration(
+ std::make_pair(&goalNode.front(), &goalNode.back() + 1)));
//calculate rotation matrix
{
- UniformProlateSpheroidDistribution<float> dist(startNode.begin(), startNode.end(), goalNode.begin());
+ UniformProlateSpheroidDistribution<float> dist(
+ startNode.begin(), startNode.end(), goalNode.begin());
rotationMatrix = dist.getRotationMatrix();
}
@@ -206,14 +213,14 @@ namespace armarx::addirrtstar
planningComputingPowerRequestStrategy->updateTaskStatus(TaskStatus::ePlanning);
std::mutex mutexWait;
- std::unique_lock<std::mutex> waitingLock {mutexWait};
+ std::unique_lock<std::mutex> waitingLock{mutexWait};
planningComputingPowerRequestStrategy->setCurrentStateAsInitialState();
while (true)
{
- managerEvent.wait_for(waitingLock, std::chrono::milliseconds {100});
+ managerEvent.wait_for(waitingLock, std::chrono::milliseconds{100});
if (killRequest)
{
@@ -226,7 +233,8 @@ namespace armarx::addirrtstar
if (hasTimeRunOut())
{
status = transitionAtOutoftime(status);
- ARMARX_IMPORTANT << "Planning Task encountered timeout (" << maximalPlanningTimeInSeconds << " seconds)";
+ ARMARX_IMPORTANT << "Planning Task encountered timeout ("
+ << maximalPlanningTimeInSeconds << " seconds)";
break;
}
@@ -253,7 +261,8 @@ namespace armarx::addirrtstar
status = TaskStatus::eRefining;
task->setTaskStatus(status);
ARMARX_IMPORTANT << "Planning Task start refining the found solution";
- planningComputingPowerRequestStrategy->updateTaskStatus(TaskStatus::eRefining);
+ planningComputingPowerRequestStrategy->updateTaskStatus(
+ TaskStatus::eRefining);
}
//update planningComputingPowerRequestStrategy
@@ -261,13 +270,15 @@ namespace armarx::addirrtstar
}
// pause worker processes if maximalWorkerCount was reduced
- while (getActiveWorkerCount() > static_cast<std::size_t>(maximalWorkerCount) && getActiveWorkerCount() > 1)
+ while (getActiveWorkerCount() > static_cast<std::size_t>(maximalWorkerCount) &&
+ getActiveWorkerCount() > 1)
{
workers.at(activeWorkerCount - 1)->pauseWorker(true);
activeWorkerCount--;
}
// unpause/start new worker, if maximalWorkerCount is greater then the number of active workers and new resources are required
- if ((getActiveWorkerCount() < static_cast<std::size_t>(maximalWorkerCount)) && planningComputingPowerRequestStrategy->shouldAllocateComputingPower())
+ if ((getActiveWorkerCount() < static_cast<std::size_t>(maximalWorkerCount)) &&
+ planningComputingPowerRequestStrategy->shouldAllocateComputingPower())
{
if (activeWorkerCount >= workers.size())
{
@@ -291,7 +302,8 @@ namespace armarx::addirrtstar
task->setPaths(getAllPathsWithCost());
//the status may have changed! (if aborted / failed it may be now RefinementAborted)
- if ((status == TaskStatus::ePlanningAborted || status == TaskStatus::ePlanningFailed) && tree.getPathCount())
+ if ((status == TaskStatus::ePlanningAborted || status == TaskStatus::ePlanningFailed) &&
+ tree.getPathCount())
{
//first set the task status
task->setTaskStatus(TaskStatus::eRefining);
@@ -304,21 +316,20 @@ namespace armarx::addirrtstar
ARMARX_VERBOSE_S << "exit managerTask";
}
-#define common_exception_output "EXCEPTION!\n"\
- << "\n\ttask name: " << task->getTaskName()\
- << "\n\tice id = " << task->ice_id()\
- << "\n\told status " << TaskStatus::toString(task->getTaskStatus())
+#define common_exception_output \
+ "EXCEPTION!\n" \
+ << "\n\ttask name: " << task->getTaskName() << "\n\tice id = " << task->ice_id() \
+ << "\n\told status " << TaskStatus::toString(task->getTaskStatus())
catch (Ice::Exception& e)
{
- ARMARX_ERROR_S << common_exception_output
- << "\n\tWHAT:\n" << e.what()
- << "\n\tSTACK:\n" << e.ice_stackTrace();
+ ARMARX_ERROR_S << common_exception_output << "\n\tWHAT:\n"
+ << e.what() << "\n\tSTACK:\n"
+ << e.ice_stackTrace();
task->setTaskStatus(TaskStatus::eException);
}
catch (std::exception& e)
{
- ARMARX_ERROR_S << common_exception_output
- << "\n\tWHAT:\n" << e.what();
+ ARMARX_ERROR_S << common_exception_output << "\n\tWHAT:\n" << e.what();
task->setTaskStatus(TaskStatus::eException);
}
catch (...)
@@ -330,11 +341,10 @@ namespace armarx::addirrtstar
#undef common_exception_output
}
-
-
- void ManagerNode::createNewWorkerOn(std::size_t remoteObjectNodeIndex)
+ void
+ ManagerNode::createNewWorkerOn(std::size_t remoteObjectNodeIndex)
{
- std::lock_guard<std::mutex> lock {workerMutex};
+ std::lock_guard<std::mutex> lock{workerMutex};
ARMARX_VERBOSE_S << "creating worker on " << remoteObjectNodeIndex;
ARMARX_CHECK_EXPRESSION(remoteObjectNodeIndex < remoteObjectNodes.size());
@@ -346,17 +356,17 @@ namespace armarx::addirrtstar
ManagerNodeBasePrx selfProxy = ManagerNodeBasePrx::uncheckedCast(getProxy());
ARMARX_CHECK_EXPRESSION(selfProxy);
- WorkerNodePtr newWorker {new WorkerNode{
- CSpaceBasePtr::dynamicCast(cspace->clone()),
- startNode, goalNode, dcdStep, addParams,
- selfProxy,
- newWorkerId,
- batchSize,
- nodeCountDeltaForGoalConnectionTries,
- updateTopicPrefix,
- rotationMatrix
- }
- };
+ WorkerNodePtr newWorker{new WorkerNode{CSpaceBasePtr::dynamicCast(cspace->clone()),
+ startNode,
+ goalNode,
+ dcdStep,
+ addParams,
+ selfProxy,
+ newWorkerId,
+ batchSize,
+ nodeCountDeltaForGoalConnectionTries,
+ updateTopicPrefix,
+ rotationMatrix}};
std::stringstream newWorkerName;
newWorkerName << newWorker->getName() << ":" << newWorkerId << "@[" << getName() << "]";
//init local structures
@@ -372,7 +382,8 @@ namespace armarx::addirrtstar
planningComputingPowerRequestStrategy->allocatedComputingPower();
}
- void ManagerNode::createNewWorker()
+ void
+ ManagerNode::createNewWorker()
{
if (getWorkerCount() >= static_cast<std::size_t>(maximalWorkerCount))
{
@@ -384,29 +395,38 @@ namespace armarx::addirrtstar
ARMARX_CHECK_EXPRESSION(remoteObjectNodes.size() == maxWorkersPerRemoteObjectNode.size());
std::size_t ronIndex = 0;
- for (; (ronIndex < remoteObjectNodes.size()) &&
- !(workersPerRemoteObjectNode.at(ronIndex) < maxWorkersPerRemoteObjectNode.at(ronIndex)); ++ronIndex);//no body
+ for (;
+ (ronIndex < remoteObjectNodes.size()) && !(workersPerRemoteObjectNode.at(ronIndex) <
+ maxWorkersPerRemoteObjectNode.at(ronIndex));
+ ++ronIndex)
+ ; //no body
ARMARX_CHECK_EXPRESSION(ronIndex < remoteObjectNodes.size());
createNewWorkerOn(ronIndex);
}
//not threadsafe! use only when holding updateMutex
- Update ManagerNode::getRemoteUpdate(std::size_t workerId, std::size_t updateId) const
+ Update
+ ManagerNode::getRemoteUpdate(std::size_t workerId, std::size_t updateId) const
{
- std::lock_guard<std::mutex> lock {workerMutex};
+ std::lock_guard<std::mutex> lock{workerMutex};
ARMARX_WARNING_S << "manager requested remote update w/u " << workerId << "/" << updateId;
ARMARX_CHECK_EXPRESSION(workerId < getWorkerCount());
return workers.at(workerId)->getUpdate(updateId);
}
+
//not threadsafe! use only when holding updateMutex
- bool ManagerNode::hasLocalUpdateRequiresUpdateMutex(std::size_t workerId, std::size_t updateId) const
+ bool
+ ManagerNode::hasLocalUpdateRequiresUpdateMutex(std::size_t workerId, std::size_t updateId) const
{
ARMARX_CHECK_EXPRESSION(workerId < getWorkerCount());
- return (updateId < appliedUpdates.at(workerId).size()) || (tree.hasPendingUpdate(workerId, updateId));
+ return (updateId < appliedUpdates.at(workerId).size()) ||
+ (tree.hasPendingUpdate(workerId, updateId));
}
+
//not threadsafe! use only when holding updateMutex
- const Update& ManagerNode::getLocalUpdateRequiresUpdateMutex(std::size_t workerId, std::size_t updateId) const
+ const Update&
+ ManagerNode::getLocalUpdateRequiresUpdateMutex(std::size_t workerId, std::size_t updateId) const
{
ARMARX_CHECK_EXPRESSION(hasLocalUpdateRequiresUpdateMutex(workerId, updateId));
@@ -419,12 +439,16 @@ namespace armarx::addirrtstar
return tree.getPendingUpdate(workerId, updateId);
}
- void ManagerNode::setWorkersFinalUpdateId(Ice::Long workerId, Ice::Long finalUpdateId, const Ice::Current&)
+ void
+ ManagerNode::setWorkersFinalUpdateId(Ice::Long workerId,
+ Ice::Long finalUpdateId,
+ const Ice::Current&)
{
{
- std::lock_guard<std::mutex> lock {updateMutex};
+ std::lock_guard<std::mutex> lock{updateMutex};
ARMARX_CHECK_EXPRESSION(workerId >= 0);
- ARMARX_CHECK_EXPRESSION(static_cast<std::size_t>(workerId) < workersFinalUpdateId.size());
+ ARMARX_CHECK_EXPRESSION(static_cast<std::size_t>(workerId) <
+ workersFinalUpdateId.size());
ARMARX_CHECK_EXPRESSION(workersFinalUpdateId.at(workerId) == -1);
workersFinalUpdateId.at(workerId) = finalUpdateId;
}
@@ -432,19 +456,21 @@ namespace armarx::addirrtstar
managerEvent.notify_all();
}
- void ManagerNode::updateTree(const Update& u, const Ice::Current&)
+ void
+ ManagerNode::updateTree(const Update& u, const Ice::Current&)
{
- std::lock_guard<std::mutex> lock {updateMutex};
+ std::lock_guard<std::mutex> lock{updateMutex};
tree.addPendingUpdate(u);
managerEvent.notify_all();
}
- void ManagerNode::cleanupAllWorkers()
+ void
+ ManagerNode::cleanupAllWorkers()
{
workersFinalUpdateId.resize(getWorkerCount(), -1);
//kill all
{
- std::lock_guard<std::mutex> lock {workerMutex};
+ std::lock_guard<std::mutex> lock{workerMutex};
for (auto& worker : workers)
{
@@ -460,67 +486,66 @@ namespace armarx::addirrtstar
//sync for update counts
do
{
- managerEvent.wait_for(updateLock, std::chrono::milliseconds {100});
- }
- while (std::any_of(
- workersFinalUpdateId.begin(), workersFinalUpdateId.end(),
- [](const Ice::Long & l)
- {
- return l == -1;
- }
- ));
+ managerEvent.wait_for(updateLock, std::chrono::milliseconds{100});
+ } while (std::any_of(workersFinalUpdateId.begin(),
+ workersFinalUpdateId.end(),
+ [](const Ice::Long& l) { return l == -1; }));
//get all updates
- tree.prepareUpdate(workersFinalUpdateId, updateLock,
- [this](std::size_t workerId, Ice::Long updateId)//remote update getter
- {
- ARMARX_CHECK_EXPRESSION(workerId < getWorkerCount());
- return getRemoteUpdate(workerId, updateId);
- },
- [this](Update && u) //update consumer
- {
- cacheAppliedUpdateRequiresUpdateMutex(std::move(u));
- }
- );
+ tree.prepareUpdate(
+ workersFinalUpdateId,
+ updateLock,
+ [this](std::size_t workerId, Ice::Long updateId) //remote update getter
+ {
+ ARMARX_CHECK_EXPRESSION(workerId < getWorkerCount());
+ return getRemoteUpdate(workerId, updateId);
+ },
+ [this](Update&& u) //update consumer
+ { cacheAppliedUpdateRequiresUpdateMutex(std::move(u)); });
ARMARX_CHECK_EXPRESSION(workersFinalUpdateId.size() == appliedUpdates.size());
for (std::size_t i = 0; i < workersFinalUpdateId.size(); ++i)
{
const auto workerAppliedUpdateCount = appliedUpdates.at(i).size();
- const auto workerUpdateCount = static_cast<std::size_t>(workersFinalUpdateId.at(i) + 1);
+ const auto workerUpdateCount =
+ static_cast<std::size_t>(workersFinalUpdateId.at(i) + 1);
const auto workersLastUpdateId = tree.getAppliedUpdateIds().at(i);
- ARMARX_CHECK_EXPRESSION(workersLastUpdateId == static_cast<Ice::Long>(workerAppliedUpdateCount) - 1);
+ ARMARX_CHECK_EXPRESSION(workersLastUpdateId ==
+ static_cast<Ice::Long>(workerAppliedUpdateCount) - 1);
ARMARX_CHECK_EXPRESSION(workerUpdateCount == workerAppliedUpdateCount);
}
}
//now all required data is stored here
//destroy worker
- std::lock_guard<std::mutex> lock {workerMutex};
+ std::lock_guard<std::mutex> lock{workerMutex};
workers.clear();
}
- void ManagerNode::applyUpdatesNotThreadSafe(std::unique_lock<std::mutex>& updateLock)
+ void
+ ManagerNode::applyUpdatesNotThreadSafe(std::unique_lock<std::mutex>& updateLock)
{
ARMARX_CHECK_EXPRESSION(updateLock);
- tree.applyPendingUpdates(updateLock,
- [this](std::size_t workerId, Ice::Long updateId)//remote update getter
- {
- ARMARX_CHECK_EXPRESSION(workerId < getWorkerCount());
- return getRemoteUpdate(workerId, updateId);
- },
- [this](Update && u) //update consumer
- {
- planningComputingPowerRequestStrategy->updateNodeCreations(u.nodes.size(), batchSize);
+ tree.applyPendingUpdates(
+ updateLock,
+ [this](std::size_t workerId, Ice::Long updateId) //remote update getter
+ {
+ ARMARX_CHECK_EXPRESSION(workerId < getWorkerCount());
+ return getRemoteUpdate(workerId, updateId);
+ },
+ [this](Update&& u) //update consumer
+ {
+ planningComputingPowerRequestStrategy->updateNodeCreations(u.nodes.size(),
+ batchSize);
- cacheAppliedUpdateRequiresUpdateMutex(std::move(u));
- }
- );
+ cacheAppliedUpdateRequiresUpdateMutex(std::move(u));
+ });
ARMARX_CHECK_EXPRESSION(updateLock);
}
- void ManagerNode::cacheAppliedUpdateRequiresUpdateMutex(Update&& u)
+ void
+ ManagerNode::cacheAppliedUpdateRequiresUpdateMutex(Update&& u)
{
//correct worker id
const auto workerId = getUpdatesWorkerId(u);
@@ -535,32 +560,37 @@ namespace armarx::addirrtstar
appliedUpdates.at(workerId).emplace_back(std::move(u));
}
- Ice::Long ManagerNode::getNodeCount(const Ice::Current&) const
+ Ice::Long
+ ManagerNode::getNodeCount(const Ice::Current&) const
{
- std::lock_guard<std::mutex> treeLock {treeMutex};
+ std::lock_guard<std::mutex> treeLock{treeMutex};
return static_cast<Ice::Long>(tree.size());
}
- bool ManagerNode::isPlanningDone()
+ bool
+ ManagerNode::isPlanningDone()
{
std::lock_guard<std::mutex> treeLock(treeMutex, std::adopt_lock);
return tree.getBestCost() <= targetCost;
}
- bool ManagerNode::hasTimeRunOut()
+ bool
+ ManagerNode::hasTimeRunOut()
{
std::lock_guard<std::mutex> treeLock(treeMutex, std::adopt_lock);
- return ((ClockType::now() - timepointStart) >= std::chrono::seconds {maximalPlanningTimeInSeconds});
+ return ((ClockType::now() - timepointStart) >=
+ std::chrono::seconds{maximalPlanningTimeInSeconds});
}
- void ManagerNode::setMaxCpus(Ice::Int maxCpus, const Ice::Current&)
+ void
+ ManagerNode::setMaxCpus(Ice::Int maxCpus, const Ice::Current&)
{
maximalWorkerCount = maxCpus;
}
- Ice::Int ManagerNode::getMaxCpus(const Ice::Current&) const
+ Ice::Int
+ ManagerNode::getMaxCpus(const Ice::Current&) const
{
return maximalWorkerCount;
}
-}
-
+} // namespace armarx::addirrtstar
diff --git a/source/RobotComponents/components/MotionPlanning/Tasks/AdaptiveDynamicDomainInformedRRTStar/ManagerNode.h b/source/RobotComponents/components/MotionPlanning/Tasks/AdaptiveDynamicDomainInformedRRTStar/ManagerNode.h
index e70bd351e7bc6eff404f9083cb7689c02cc9e625..ebeff9f78cf3c5b0b66bc6810e72c35d36e5fc8d 100644
--- a/source/RobotComponents/components/MotionPlanning/Tasks/AdaptiveDynamicDomainInformedRRTStar/ManagerNode.h
+++ b/source/RobotComponents/components/MotionPlanning/Tasks/AdaptiveDynamicDomainInformedRRTStar/ManagerNode.h
@@ -23,26 +23,26 @@
*/
#pragma once
-#include <mutex>
#include <atomic>
-#include <thread>
#include <condition_variable>
+#include <mutex>
+#include <thread>
#include <ArmarXCore/core/ManagedIceObject.h>
-
-
#include <ArmarXCore/core/util/distributed/RemoteHandle/RemoteHandle.h>
-#include "../../util/PlanningUtil.h"
#include <RobotComponents/interface/components/MotionPlanning/Tasks/AdaptiveDynamicDomainInformedRRTStar/ManagerNode.h>
#include <RobotComponents/interface/components/MotionPlanning/Tasks/AdaptiveDynamicDomainInformedRRTStar/WorkerNode.h>
+#include "../../util/PlanningUtil.h"
#include "Tree.h"
namespace armarx
{
- template <class IceBaseClass, class DerivedClass> class GenericFactory;
+ template <class IceBaseClass, class DerivedClass>
+ class GenericFactory;
}
+
namespace armarx::addirrtstar
{
class ManagerNode;
@@ -55,14 +55,13 @@ namespace armarx::addirrtstar
* @brief Manages the planning of the addirrt* algorithm.
* Starts workers and checks whether the planning should end.
*/
- class ManagerNode:
- virtual public ManagerNodeBase,
- virtual public ManagedIceObject
+ class ManagerNode : virtual public ManagerNodeBase, virtual public ManagedIceObject
{
/**
* @brief The clock used to meassure durations.
*/
using ClockType = std::chrono::system_clock;
+
public:
/**
* @brief Ctor.
@@ -98,17 +97,21 @@ namespace armarx::addirrtstar
VectorXf startCfg,
VectorXf goalCfg,
AdaptiveDynamicDomainParameters addParams,
- float targetCost
- ):
- ManagerNodeBase
- (
- task, remoteObjectNodes,
- initialWorkerCount, maximalWorkerCount, planningComputingPowerRequestStrategy,
- dcdStep, maximalPlanningTimeInSeconds,
- cspace,
- startCfg, goalCfg,
- addParams, targetCost, batchSize, nodeCountDeltaForGoalConnectionTries
- )
+ float targetCost) :
+ ManagerNodeBase(task,
+ remoteObjectNodes,
+ initialWorkerCount,
+ maximalWorkerCount,
+ planningComputingPowerRequestStrategy,
+ dcdStep,
+ maximalPlanningTimeInSeconds,
+ cspace,
+ startCfg,
+ goalCfg,
+ addParams,
+ targetCost,
+ batchSize,
+ nodeCountDeltaForGoalConnectionTries)
{
//other variables get initialized in onInitComponent
}
@@ -139,18 +142,22 @@ namespace armarx::addirrtstar
/**
* @brief noop. (debug output)
*/
- void onDisconnectComponent() override
+ void
+ onDisconnectComponent() override
{
ARMARX_VERBOSE_S << "armarx::addirrtstar::ManagerNode::onDisconnectComponent()";
}
+
/**
* @brief Stopps planning and joins the manager thread.
*/
void onExitComponent() override;
+
/**
* @return The components default name.
*/
- std::string getDefaultName() const override
+ std::string
+ getDefaultName() const override
{
return "ADDIRRTStarManagerNode";
}
@@ -159,7 +166,8 @@ namespace armarx::addirrtstar
/**
* @brief Sets the flag to stop planning
*/
- void kill(const Ice::Current& = Ice::emptyCurrent) override
+ void
+ kill(const Ice::Current& = Ice::emptyCurrent) override
{
killRequest = true;
}
@@ -176,7 +184,8 @@ namespace armarx::addirrtstar
* @param index The index.
* @return The path at the given index.
*/
- PathWithCost getNthPathWithCost(Ice::Long n, const Ice::Current& = Ice::emptyCurrent) const override;
+ PathWithCost getNthPathWithCost(Ice::Long n,
+ const Ice::Current& = Ice::emptyCurrent) const override;
/**
* @return All found paths.
*/
@@ -187,7 +196,9 @@ namespace armarx::addirrtstar
* @param updateId The updates sub id.
* @return The requested update. If the update is not cached it will be fetched from the corresponding worker.
*/
- Update getUpdate(Ice::Long workerId, Ice::Long updateId, const Ice::Current& = Ice::emptyCurrent) const override;
+ Update getUpdate(Ice::Long workerId,
+ Ice::Long updateId,
+ const Ice::Current& = Ice::emptyCurrent) const override;
/**
* @return The current tree with all updates applied.
*/
@@ -199,7 +210,9 @@ namespace armarx::addirrtstar
* @param workerId The worker.
* @param finalUpdateId Its final update id.
*/
- void setWorkersFinalUpdateId(Ice::Long workerId, Ice::Long finalUpdateId, const Ice::Current&) override;
+ void setWorkersFinalUpdateId(Ice::Long workerId,
+ Ice::Long finalUpdateId,
+ const Ice::Current&) override;
//from TreeUpdateInterface
/**
@@ -256,7 +269,8 @@ namespace armarx::addirrtstar
* @param updateId The updates sub id.
* @return The requested update from the cache.
*/
- const Update& getLocalUpdateRequiresUpdateMutex(std::size_t workerId, std::size_t updateId) const;
+ const Update& getLocalUpdateRequiresUpdateMutex(std::size_t workerId,
+ std::size_t updateId) const;
//not threadsafe! use only when holding updateMutex
/**
* @brief Returns the requested update fetched from the corresponding worker. (requires the caller to hold the updateMutex)
@@ -296,12 +310,12 @@ namespace armarx::addirrtstar
*/
bool hasTimeRunOut();
-
/**
* @brief getActiveWorkerCount returns the number of currently active workers.
* @return
*/
- std::size_t getActiveWorkerCount() const
+ std::size_t
+ getActiveWorkerCount() const
{
return activeWorkerCount;
}
@@ -310,7 +324,8 @@ namespace armarx::addirrtstar
* @brief Returns the number of currently available workers (both active and paused).
* @return
*/
- std::size_t getWorkerCount() const
+ std::size_t
+ getWorkerCount() const
{
return workers.size();
}
@@ -394,5 +409,4 @@ namespace armarx::addirrtstar
*/
friend class ManagedIceObject;
};
-}
-
+} // namespace armarx::addirrtstar
diff --git a/source/RobotComponents/components/MotionPlanning/Tasks/AdaptiveDynamicDomainInformedRRTStar/Task.cpp b/source/RobotComponents/components/MotionPlanning/Tasks/AdaptiveDynamicDomainInformedRRTStar/Task.cpp
index 1f2793419cddc45a4aa36a77cf9ef4fe7735d2a8..4a78f7cfc9cc36e3fb3847a045800a67b8dc556e 100644
--- a/source/RobotComponents/components/MotionPlanning/Tasks/AdaptiveDynamicDomainInformedRRTStar/Task.cpp
+++ b/source/RobotComponents/components/MotionPlanning/Tasks/AdaptiveDynamicDomainInformedRRTStar/Task.cpp
@@ -22,20 +22,22 @@
* GNU General Public License
*/
+#include "Task.h"
+
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-#include "../../util/Metrics.h"
-#include <RobotComponents/interface/components/MotionPlanning/MotionPlanningServer.h>
#include <RobotComponents/interface/components/MotionPlanning/DataStructures.h>
+#include <RobotComponents/interface/components/MotionPlanning/MotionPlanningServer.h>
-#include "Task.h"
+#include "../../util/Metrics.h"
#include "ManagerNode.h"
namespace armarx::addirrtstar
{
- PathWithCost Task::getBestPath(const Ice::Current&) const
+ PathWithCost
+ Task::getBestPath(const Ice::Current&) const
{
- std::lock_guard<std::recursive_mutex> lock {mutex};
+ std::lock_guard<std::recursive_mutex> lock{mutex};
if (finishedRunning())
{
return paths.at(0);
@@ -51,9 +53,11 @@ namespace armarx::addirrtstar
noPathSentinel.cost = std::numeric_limits<Ice::Float>::infinity();
return noPathSentinel;
}
- Ice::Long Task::getPathCount(const Ice::Current&) const
+
+ Ice::Long
+ Task::getPathCount(const Ice::Current&) const
{
- std::lock_guard<std::recursive_mutex> lock {mutex};
+ std::lock_guard<std::recursive_mutex> lock{mutex};
if (isRunning())
{
@@ -63,13 +67,14 @@ namespace armarx::addirrtstar
return paths.size();
}
- PathWithCost Task::getNthPathWithCost(Ice::Long index, const Ice::Current&) const
+ PathWithCost
+ Task::getNthPathWithCost(Ice::Long index, const Ice::Current&) const
{
- std::lock_guard<std::recursive_mutex> lock {mutex};
+ std::lock_guard<std::recursive_mutex> lock{mutex};
if ((index < 0) || index >= getPathCount())
{
- throw IndexOutOfBoundsException {};
+ throw IndexOutOfBoundsException{};
}
ARMARX_CHECK_EXPRESSION(index >= 0);
@@ -84,9 +89,10 @@ namespace armarx::addirrtstar
return paths.at(index);
}
- PathWithCostSeq Task::getAllPathsWithCost(const Ice::Current&) const
+ PathWithCostSeq
+ Task::getAllPathsWithCost(const Ice::Current&) const
{
- std::lock_guard<std::recursive_mutex> lock {mutex};
+ std::lock_guard<std::recursive_mutex> lock{mutex};
if (isRunning())
{
@@ -97,9 +103,10 @@ namespace armarx::addirrtstar
}
//PlanningControlInterface
- void Task::abortTask(const Ice::Current&)
+ void
+ Task::abortTask(const Ice::Current&)
{
- std::lock_guard<std::recursive_mutex> lock {mutex};
+ std::lock_guard<std::recursive_mutex> lock{mutex};
if (getTaskStatus() == TaskStatus::eQueued || getTaskStatus() == TaskStatus::eNew)
{
@@ -125,7 +132,8 @@ namespace armarx::addirrtstar
}
//PlanningTaskBase
- void Task::run(const RemoteObjectNodePrxList& remoteNodes, const Ice::Current&)
+ void
+ Task::run(const RemoteObjectNodePrxList& remoteNodes, const Ice::Current&)
{
ARMARX_CHECK_EXPRESSION(planningComputingPowerRequestStrategy);
//since post unmarshall is of this is called BEFORE post unmarshall of the member cspace is called this check can only be done here!
@@ -133,7 +141,7 @@ namespace armarx::addirrtstar
ARMARX_CHECK_EXPRESSION(startCfg.size() == dim);
ARMARX_CHECK_EXPRESSION(goalCfg.size() == dim);
- std::unique_lock<std::recursive_mutex> lock {mutex};
+ std::unique_lock<std::recursive_mutex> lock{mutex};
if (getTaskStatus() != TaskStatus::eQueued)
{
@@ -146,27 +154,34 @@ namespace armarx::addirrtstar
ARMARX_CHECK_EXPRESSION(cspace);
cspace->initCollisionTest();
- if (!(cspace->isCollisionFree(std::make_pair(startCfg.data(), startCfg.data() + startCfg.size()))))
+ if (!(cspace->isCollisionFree(
+ std::make_pair(startCfg.data(), startCfg.data() + startCfg.size()))))
{
- ARMARX_VERBOSE_S << "trivial task! failed (start is not collision free): \n" << startCfg;
+ ARMARX_VERBOSE_S << "trivial task! failed (start is not collision free): \n"
+ << startCfg;
setTaskStatus(TaskStatus::ePlanningFailed);
return;
}
- if (!(cspace->isCollisionFree(std::make_pair(goalCfg.data(), goalCfg.data() + startCfg.size()))))
+ if (!(cspace->isCollisionFree(
+ std::make_pair(goalCfg.data(), goalCfg.data() + startCfg.size()))))
{
ARMARX_VERBOSE_S << "trivial task! failed (goal is not collision free): \n" << goalCfg;
setTaskStatus(TaskStatus::ePlanningFailed);
return;
}
- const auto distanceStartGoal = euclideanDistance(startCfg.begin(), startCfg.end(), goalCfg.begin());
+ const auto distanceStartGoal =
+ euclideanDistance(startCfg.begin(), startCfg.end(), goalCfg.begin());
ARMARX_VERBOSE_S << "distance from start to goal = " << distanceStartGoal;
if (distanceStartGoal < dcdStep)
{
- ARMARX_VERBOSE_S << "trivial task! done(the distance from start to goal is smaller than the DCD step size)";
- paths.emplace_back(VectorXfSeq {startCfg, goalCfg}, distanceStartGoal, taskName + "_trivialPath_" + ice_staticId());
+ ARMARX_VERBOSE_S << "trivial task! done(the distance from start to goal is smaller "
+ "than the DCD step size)";
+ paths.emplace_back(VectorXfSeq{startCfg, goalCfg},
+ distanceStartGoal,
+ taskName + "_trivialPath_" + ice_staticId());
setTaskStatus(TaskStatus::eDone);
return;
}
@@ -175,24 +190,29 @@ namespace armarx::addirrtstar
targetCost = std::max(targetCost, distanceStartGoal);
//check params
ARMARX_CHECK_EXPRESSION(remoteNodes.size());
- RemoteObjectNodeInterfacePrx rmObjNode = remoteNodes.at(0); //copy the proxy since it is const
+ RemoteObjectNodeInterfacePrx rmObjNode =
+ remoteNodes.at(0); //copy the proxy since it is const
ARMARX_CHECK_EXPRESSION(rmObjNode);
//create manager node
TaskBasePrx selfDerivedProxy = TaskBasePrx::uncheckedCast(getProxy());
- ManagerNodePtr localManagerNode
- {
- new ManagerNode{
- selfDerivedProxy, remoteNodes,
- initialWorkerCount, maximalWorkerCount, planningComputingPowerRequestStrategy,
- dcdStep, maximalPlanningTimeInSeconds, batchSize, nodeCountDeltaForGoalConnectionTries,
- CSpaceBasePtr::dynamicCast(cspace->clone()),
- startCfg, goalCfg,
- addParams, targetCost
- }
- };
+ ManagerNodePtr localManagerNode{new ManagerNode{selfDerivedProxy,
+ remoteNodes,
+ initialWorkerCount,
+ maximalWorkerCount,
+ planningComputingPowerRequestStrategy,
+ dcdStep,
+ maximalPlanningTimeInSeconds,
+ batchSize,
+ nodeCountDeltaForGoalConnectionTries,
+ CSpaceBasePtr::dynamicCast(cspace->clone()),
+ startCfg,
+ goalCfg,
+ addParams,
+ targetCost}};
//register it
std::stringstream remoteObjectName;
- remoteObjectName << localManagerNode->getName() << "@" << getProxy()->ice_getIdentity().name;
+ remoteObjectName << localManagerNode->getName() << "@"
+ << getProxy()->ice_getIdentity().name;
ARMARX_DEBUG_S << "\n starting manager of task on " << rmObjNode
<< "\n\t remoteObjectName = " << remoteObjectName.str();
@@ -202,7 +222,7 @@ namespace armarx::addirrtstar
setTaskStatus(TaskStatus::ePlanning);
//wait
ARMARX_VERBOSE_S << "waiting for manager";
- managerDone.wait(lock, [this] {return finishedRunning();});
+ managerDone.wait(lock, [this] { return finishedRunning(); });
ARMARX_VERBOSE_S << "manager done";
ARMARX_CHECK_EXPRESSION(finishedRunning());
@@ -213,87 +233,85 @@ namespace armarx::addirrtstar
ARMARX_VERBOSE_S << "done run";
}
- void Task::setPaths(const PathWithCostSeq& newPathList, const Ice::Current&)
+ void
+ Task::setPaths(const PathWithCostSeq& newPathList, const Ice::Current&)
{
- std::lock_guard<std::recursive_mutex> lock {mutex};
+ std::lock_guard<std::recursive_mutex> lock{mutex};
paths = newPathList;
- std::sort(paths.begin(), paths.end(), [](const PathWithCost & lhs, const PathWithCost & rhs)
- {
- return lhs.cost < rhs.cost;
- });
+ std::sort(paths.begin(),
+ paths.end(),
+ [](const PathWithCost& lhs, const PathWithCost& rhs)
+ { return lhs.cost < rhs.cost; });
for (std::size_t i = 0; i < paths.size(); ++i)
{
paths.at(i).pathName = taskName + "_path_" + to_string(i) + ice_staticId();
}
}
- void Task::checkParameters()
+ void
+ Task::checkParameters()
{
ARMARX_DEBUG_S << "\n checking parameters:"
<< "\n\t initialWorkerCount " << initialWorkerCount
- << "\n\t maximalWorkerCount " << maximalWorkerCount
- << "\n\t dcdStep " << dcdStep
- << "\n\t batchSize " << batchSize
- << "\n\t cspace " << cspace.get()
- << "\n\t addParams.alpha " << addParams.alpha
+ << "\n\t maximalWorkerCount " << maximalWorkerCount << "\n\t dcdStep "
+ << dcdStep << "\n\t batchSize " << batchSize << "\n\t cspace "
+ << cspace.get() << "\n\t addParams.alpha " << addParams.alpha
<< "\n\t addParams.initialBorderRadius " << addParams.initialBorderRadius
<< "\n\t addParams.minimalRadius " << addParams.minimalRadius
- << "\n\t targetCost " << targetCost
- << "\n\t dim cspace (-1 == null cspace)" << (cspace ? cspace->getDimensionality() : -1)
- << "\n\t dim start " << startCfg.size()
- << "\n\t dim goal " << goalCfg.size()
- << "\n\t start " << startCfg
- << "\n\t goal " << goalCfg;
+ << "\n\t targetCost " << targetCost << "\n\t dim cspace (-1 == null cspace)"
+ << (cspace ? cspace->getDimensionality() : -1) << "\n\t dim start "
+ << startCfg.size() << "\n\t dim goal " << goalCfg.size() << "\n\t start "
+ << startCfg << "\n\t goal " << goalCfg;
if (!(dcdStep > 0.f))
{
- throw std::invalid_argument {"DCD stepsize <0"};
+ throw std::invalid_argument{"DCD stepsize <0"};
}
if (!(cspace))
{
- throw std::invalid_argument {"cspace == nullptr"};
+ throw std::invalid_argument{"cspace == nullptr"};
}
const auto dim = static_cast<std::size_t>(cspace->getDimensionality());
if (!(startCfg.size() == dim))
{
- throw std::invalid_argument {"Dimensions of cspace and start do not match"};
+ throw std::invalid_argument{"Dimensions of cspace and start do not match"};
}
if (!(goalCfg.size() == dim))
{
- throw std::invalid_argument {"Dimensions of cspace and goal do not match"};
+ throw std::invalid_argument{"Dimensions of cspace and goal do not match"};
}
if (!(addParams.alpha >= 0.f))
{
- throw std::invalid_argument {"addParams.alpha < 0.f"};
+ throw std::invalid_argument{"addParams.alpha < 0.f"};
}
if (!(addParams.minimalRadius > 0.f))
{
- throw std::invalid_argument {"addParams.minimalRadius <= 0.f"};
+ throw std::invalid_argument{"addParams.minimalRadius <= 0.f"};
}
if (!(addParams.initialBorderRadius > addParams.minimalRadius))
{
- throw std::invalid_argument {"addParams.initialBorderRadius <= addParams.minimalRadius"};
+ throw std::invalid_argument{"addParams.initialBorderRadius <= addParams.minimalRadius"};
}
if (nodeCountDeltaForGoalConnectionTries < 1)
{
- throw std::invalid_argument {"nodeCountDeltaForGoalConnectionTries < 1"};
+ throw std::invalid_argument{"nodeCountDeltaForGoalConnectionTries < 1"};
}
if (!planningComputingPowerRequestStrategy)
{
- throw std::invalid_argument {"planningComputingPowerRequestStrategy == nullptr"};
+ throw std::invalid_argument{"planningComputingPowerRequestStrategy == nullptr"};
}
}
- Task::Task(//problem
+ Task::Task( //problem
CSpaceBasePtr cspace,
const cprs::ComputingPowerRequestStrategyBasePtr& planningComputingPowerRequestStrategy,
VectorXf startCfg,
@@ -308,8 +326,7 @@ namespace armarx::addirrtstar
Ice::Long nodeCountDeltaForGoalConnectionTries,
//management
Ice::Long initialWorkerCount,
- Ice::Long maximalWorkerCount
- )
+ Ice::Long maximalWorkerCount)
{
this->cspace = cspace;
this->planningComputingPowerRequestStrategy = planningComputingPowerRequestStrategy;
@@ -328,9 +345,10 @@ namespace armarx::addirrtstar
checkParameters();
}
- Ice::Long Task::getNodeCount(const Ice::Current&) const
+ Ice::Long
+ Task::getNodeCount(const Ice::Current&) const
{
- std::lock_guard<std::recursive_mutex> lock {mutex};
+ std::lock_guard<std::recursive_mutex> lock{mutex};
if (isRunning())
{
@@ -341,14 +359,15 @@ namespace armarx::addirrtstar
return cachedNodeCount;
}
- void Task::setMaxCpus(Ice::Int maxCpus, const Ice::Current&)
+ void
+ Task::setMaxCpus(Ice::Int maxCpus, const Ice::Current&)
{
manager->setMaxCpus(maxCpus);
}
- Ice::Int Task::getMaxCpus(const Ice::Current&) const
+ Ice::Int
+ Task::getMaxCpus(const Ice::Current&) const
{
return manager->getMaxCpus();
}
-}
-
+} // namespace armarx::addirrtstar
diff --git a/source/RobotComponents/components/MotionPlanning/Tasks/AdaptiveDynamicDomainInformedRRTStar/Task.h b/source/RobotComponents/components/MotionPlanning/Tasks/AdaptiveDynamicDomainInformedRRTStar/Task.h
index 73543d114ea1078c103c41e6d667c79cc00f4c2d..b4e932a12b4b1b7d1b269e96db267ad9c72f5810 100644
--- a/source/RobotComponents/components/MotionPlanning/Tasks/AdaptiveDynamicDomainInformedRRTStar/Task.h
+++ b/source/RobotComponents/components/MotionPlanning/Tasks/AdaptiveDynamicDomainInformedRRTStar/Task.h
@@ -23,21 +23,20 @@
*/
#pragma once
-#include <mutex>
#include <atomic>
#include <condition_variable>
+#include <mutex>
#include <ArmarXCore/core/system/FactoryCollectionBase.h>
-
#include <ArmarXCore/core/util/distributed/RemoteHandle/RemoteHandle.h>
-#include <RobotComponents/interface/components/MotionPlanning/Tasks/AdaptiveDynamicDomainInformedRRTStar/Task.h>
#include <RobotComponents/interface/components/MotionPlanning/Tasks/AdaptiveDynamicDomainInformedRRTStar/ManagerNode.h>
+#include <RobotComponents/interface/components/MotionPlanning/Tasks/AdaptiveDynamicDomainInformedRRTStar/Task.h>
+
#include "../../ResourceRequestStrategies/ComputingPowerRequestStrategy.h"
#include "../../util/PlanningUtil.h"
-
-#include "util.h"
#include "../CPRSAwareMotionPlanningTask.h"
+#include "util.h"
namespace armarx::addirrtstar
{
@@ -56,7 +55,7 @@ namespace armarx::addirrtstar
*
* bulks and batches are used as synonyms.
*/
- class Task:
+ class Task :
public virtual cprs::CPRSAwareMotionPlanningTask,
public virtual TaskBase,
public virtual MotionPlanningMultiPathWithCostTaskCI
@@ -77,7 +76,7 @@ namespace armarx::addirrtstar
* @param initialWorkerCount The in itaial number of worker processes.
* @param maximalWorkerCount The maximal number of worker processes.
*/
- Task(//problem
+ Task( //problem
CSpaceBasePtr cspace,
const cprs::ComputingPowerRequestStrategyBasePtr& planningComputingPowerRequestStrategy,
VectorXf startCfg,
@@ -92,20 +91,22 @@ namespace armarx::addirrtstar
Ice::Long nodeCountDeltaForGoalConnectionTries = 50,
//management
Ice::Long initialWorkerCount = 1,
- Ice::Long maximalWorkerCount = std::numeric_limits<Ice::Long>::max()
- );
-
+ Ice::Long maximalWorkerCount = std::numeric_limits<Ice::Long>::max());
- PathWithCost getPathWithCost(const Ice::Current& = Ice::emptyCurrent) const override
+ PathWithCost
+ getPathWithCost(const Ice::Current& = Ice::emptyCurrent) const override
{
return MotionPlanningMultiPathWithCostTaskCI::getPathWithCost();
}
- Path getNthPath(Ice::Long n, const Ice::Current& = Ice::emptyCurrent) const override
+
+ Path
+ getNthPath(Ice::Long n, const Ice::Current& = Ice::emptyCurrent) const override
{
return MotionPlanningMultiPathWithCostTaskCI::getNthPath(n);
}
- Path getPath(const Ice::Current& = Ice::emptyCurrent) const override
+ Path
+ getPath(const Ice::Current& = Ice::emptyCurrent) const override
{
return MotionPlanningWithCostTaskCI::getPath();
}
@@ -123,7 +124,8 @@ namespace armarx::addirrtstar
* @param index The index.
* @return The path at the given index.
*/
- PathWithCost getNthPathWithCost(Ice::Long index, const Ice::Current& = Ice::emptyCurrent) const override;
+ PathWithCost getNthPathWithCost(Ice::Long index,
+ const Ice::Current& = Ice::emptyCurrent) const override;
/**
* @return All found paths.
*/
@@ -140,13 +142,15 @@ namespace armarx::addirrtstar
* @brief Runs the task.
* @param remoteNodes The list of \ref RemoteObjectNodeInterfacePrx used to distribute work to computers.
*/
- void run(const RemoteObjectNodePrxList& remoteNodes, const Ice::Current& = Ice::emptyCurrent) override;
+ void run(const RemoteObjectNodePrxList& remoteNodes,
+ const Ice::Current& = Ice::emptyCurrent) override;
/**
* @brief Used by the manager to store its found paths.
* @param newPathList The paths.
*/
- void setPaths(const PathWithCostSeq& newPathList, const Ice::Current& = Ice::emptyCurrent) override;
+ void setPaths(const PathWithCostSeq& newPathList,
+ const Ice::Current& = Ice::emptyCurrent) override;
/**
* @return The current node count.
@@ -159,7 +163,8 @@ namespace armarx::addirrtstar
Ice::Int getMaxCpus(const Ice::Current& = Ice::emptyCurrent) const override;
protected:
- template<class Base, class Derived> friend class ::armarx::GenericFactory;
+ template <class Base, class Derived>
+ friend class ::armarx::GenericFactory;
/**
* @brief Checks for illegal parameters
@@ -169,9 +174,7 @@ namespace armarx::addirrtstar
/**
* @brief Ctor used by object factories.
*/
- Task():
- TaskBase(),
- cachedNodeCount {0}
+ Task() : TaskBase(), cachedNodeCount{0}
{
}
@@ -203,11 +206,12 @@ namespace armarx::addirrtstar
*/
Ice::Long cachedNodeCount;
};
-}
+} // namespace armarx::addirrtstar
+
namespace armarx
{
using ADDIRRTStarTask = addirrtstar::Task;
using ADDIRRTStarTaskPtr = IceUtil::Handle<ADDIRRTStarTask>;
- using ADDIRRTStarTaskHandle = RemoteHandle<armarx::MotionPlanningMultiPathWithCostTaskControlInterfacePrx>;
-}
-
+ using ADDIRRTStarTaskHandle =
+ RemoteHandle<armarx::MotionPlanningMultiPathWithCostTaskControlInterfacePrx>;
+} // namespace armarx
diff --git a/source/RobotComponents/components/MotionPlanning/Tasks/AdaptiveDynamicDomainInformedRRTStar/Tree.cpp b/source/RobotComponents/components/MotionPlanning/Tasks/AdaptiveDynamicDomainInformedRRTStar/Tree.cpp
index aaf2bcf59b2dbc18fd6ce864a0ca6acc17433696..ce13d8a910bc14894f3ad19812613c9ce96a272a 100644
--- a/source/RobotComponents/components/MotionPlanning/Tasks/AdaptiveDynamicDomainInformedRRTStar/Tree.cpp
+++ b/source/RobotComponents/components/MotionPlanning/Tasks/AdaptiveDynamicDomainInformedRRTStar/Tree.cpp
@@ -22,51 +22,38 @@
* GNU General Public License
*/
+#include "Tree.h"
+
#include <limits>
-#include "../../util/Metrics.h"
+#include <ArmarXCore/core/util/StringHelpers.h>
-#include "Tree.h"
+#include "../../util/Metrics.h"
#include "WorkerNode.h"
-#include <ArmarXCore/core/util/StringHelpers.h>
-
namespace armarx::addirrtstar
{
- const NodeId Tree::ROOT_ID
- {
- 0, 0
- };
+ const NodeId Tree::ROOT_ID{0, 0};
Tree::Tree(const VectorXf& startCfg)
{
- init(
- FullIceTree
- {
- FullNodeDataListList{
- FullNodeDataList{
- FullNodeData{
- startCfg, //config
- ROOT_ID, //parent
- std::numeric_limits<Ice::Float>::infinity(), //radius;
- 0.f //fromParentCost
- }
- }
- },
- Ice::LongSeq{ -1}
- },
- AdaptiveDynamicDomainParameters {1, 1, 1},
- -1 //since this tree does no updates the id is not important
+ init(FullIceTree{FullNodeDataListList{FullNodeDataList{FullNodeData{
+ startCfg, //config
+ ROOT_ID, //parent
+ std::numeric_limits<Ice::Float>::infinity(), //radius;
+ 0.f //fromParentCost
+ }}},
+ Ice::LongSeq{-1}},
+ AdaptiveDynamicDomainParameters{1, 1, 1},
+ -1 //since this tree does no updates the id is not important
);
}
//init
- void Tree::init
- (
- const FullIceTree& iceTree,
- AdaptiveDynamicDomainParameters newaddParams,
- std::size_t newWorkerId
- )
+ void
+ Tree::init(const FullIceTree& iceTree,
+ AdaptiveDynamicDomainParameters newaddParams,
+ std::size_t newWorkerId)
{
addParams = newaddParams;
workerId = newWorkerId;
@@ -105,11 +92,9 @@ namespace armarx::addirrtstar
localNode.radius = iceNode.radius;
localNode.fromParentCost = iceNode.fromParentCost;
//register as child !!This adds root as child of root!!
- getNode(localNode.parent).children.insert(NodeId
- {
- static_cast<Ice::Long>(wId),
- static_cast<Ice::Long>(nId)
- });
+ getNode(localNode.parent)
+ .children.insert(
+ NodeId{static_cast<Ice::Long>(wId), static_cast<Ice::Long>(nId)});
}
}
@@ -119,7 +104,8 @@ namespace armarx::addirrtstar
pushCosts(ROOT_ID);
}
- void Tree::increaseWorkerCountTo(std::size_t count)
+ void
+ Tree::increaseWorkerCountTo(std::size_t count)
{
if (appliedUpdateIds.size() < count)
{
@@ -133,7 +119,8 @@ namespace armarx::addirrtstar
}
//updating
- bool Tree::canApplyUpdate(const Update& u)
+ bool
+ Tree::canApplyUpdate(const Update& u)
{
ARMARX_CHECK_EXPRESSION(u.dependetOnUpdateIds.size() <= appliedUpdateIds.size());
@@ -148,7 +135,8 @@ namespace armarx::addirrtstar
return true;
}
- void Tree::applyUpdate(const Update& u)
+ void
+ Tree::applyUpdate(const Update& u)
{
const auto workerId = getUpdatesWorkerId(u);
ARMARX_CHECK_EXPRESSION(workerId != this->workerId);
@@ -186,9 +174,11 @@ namespace armarx::addirrtstar
++(appliedUpdateIds.at(workerId));
}
- void Tree::sendCurrentUpdate(TreeUpdateInterfacePrx& prx)
+ void
+ Tree::sendCurrentUpdate(TreeUpdateInterfacePrx& prx)
{
- ARMARX_CHECK_EXPRESSION(static_cast<Ice::Long>(getLocalUpdates().size()) - 1 == getCurrentUpdateId());
+ ARMARX_CHECK_EXPRESSION(static_cast<Ice::Long>(getLocalUpdates().size()) - 1 ==
+ getCurrentUpdateId());
getCurrentUpdateNonConst().dependetOnUpdateIds = appliedUpdateIds;
prx->updateTree(getCurrentUpdate());
//begin the next update
@@ -197,7 +187,8 @@ namespace armarx::addirrtstar
++(appliedUpdateIds.at(workerId));
}
- void Tree::applyRadiusUpdate(const RadiusUpdate& u)
+ void
+ Tree::applyRadiusUpdate(const RadiusUpdate& u)
{
if (u.increase)
{
@@ -209,7 +200,8 @@ namespace armarx::addirrtstar
}
}
- void Tree::applyRewireUpdate(const RewireUpdate& rewireUpdate)
+ void
+ Tree::applyRewireUpdate(const RewireUpdate& rewireUpdate)
{
//only rewire if it improves the cost
const NodeId& child = rewireUpdate.child;
@@ -218,9 +210,9 @@ namespace armarx::addirrtstar
const auto costOld = getNode(child).fromStartCost;
const auto costNew = getNode(newParent).fromStartCost + rewireUpdate.fromNewParentCost;
- if (
- costOld < costNew ||
- ((costOld == costNew) && (rewireUpdate.newParent.workerId > getNode(child).parent.workerId)) //corner case
+ if (costOld < costNew ||
+ ((costOld == costNew) &&
+ (rewireUpdate.newParent.workerId > getNode(child).parent.workerId)) //corner case
)
{
//skip this update
@@ -230,7 +222,8 @@ namespace armarx::addirrtstar
doSetNodeParent(child, newParent, rewireUpdate.fromNewParentCost);
}
- void Tree::prepareNextUpdate()
+ void
+ Tree::prepareNextUpdate()
{
localUpdates.emplace_back();
getCurrentUpdateNonConst().workerId = workerId;
@@ -238,7 +231,8 @@ namespace armarx::addirrtstar
}
//changing the tree from the rrt algorithm
- NodeId Tree::addNode(ConfigType cfg, const NodeId& parent, float fromParentCost)
+ NodeId
+ Tree::addNode(ConfigType cfg, const NodeId& parent, float fromParentCost)
{
//add to current tree update
createNewNodeCreationUpdate(cfg, parent, fromParentCost);
@@ -249,25 +243,30 @@ namespace armarx::addirrtstar
}
//change the tree
- void Tree::pushCosts(const NodeId& root)
+ void
+ Tree::pushCosts(const NodeId& root)
{
for (const auto& child : getNode(root).children)
{
- getNode(child).fromStartCost = getNode(child).fromParentCost + getNode(root).fromStartCost;
+ getNode(child).fromStartCost =
+ getNode(child).fromParentCost + getNode(root).fromStartCost;
pushCosts(child);
}
}
- NodeId Tree::doAddNode(ConfigType cfg, const NodeId& parent, float fromParentCost, std::size_t workerId)
+ NodeId
+ Tree::doAddNode(ConfigType cfg,
+ const NodeId& parent,
+ float fromParentCost,
+ std::size_t workerId)
{
NodeId id = getNextNodeIdFor(workerId);
auto& parentNode = getNode(parent);
++nodeCount;
- nodes.at(workerId).emplace_back(NodeType
- {
+ nodes.at(workerId).emplace_back(NodeType{
cfg,
parent,
- std::numeric_limits<float>::infinity(),//radius
+ std::numeric_limits<float>::infinity(), //radius
fromParentCost,
parentNode.fromStartCost + fromParentCost,
std::set<NodeId>{} //children
@@ -278,7 +277,11 @@ namespace armarx::addirrtstar
return id;
}
- void Tree::doSetNodeParent(const NodeId& child, const NodeId& newParent, float fromParentCost, bool updateFromStartCost)
+ void
+ Tree::doSetNodeParent(const NodeId& child,
+ const NodeId& newParent,
+ float fromParentCost,
+ bool updateFromStartCost)
{
const auto oldParent = getNode(child).parent;
getNode(child).parent = newParent;
@@ -295,36 +298,39 @@ namespace armarx::addirrtstar
}
}
- void Tree::doDecreaseRadius(const NodeId& id)
+ void
+ Tree::doDecreaseRadius(const NodeId& id)
{
if (getNode(id).radius < std::numeric_limits<float>::infinity())
{
- getNode(id).radius = std::max(getNode(id).radius * (1.f - addParams.alpha), addParams.minimalRadius);
+ getNode(id).radius =
+ std::max(getNode(id).radius * (1.f - addParams.alpha), addParams.minimalRadius);
}
else
{
- getNode(id).radius = addParams.initialBorderRadius;
+ getNode(id).radius = addParams.initialBorderRadius;
}
}
//querry the tree
- std::vector<std::pair<NodeId, float>> Tree::getKNearestNeighboursAndDistances(const ConfigType& cfg, std::size_t k)
+ std::vector<std::pair<NodeId, float>>
+ Tree::getKNearestNeighboursAndDistances(const ConfigType& cfg, std::size_t k)
{
//change k to fit size
//k = std::min(k, size());
//calc all distances and use nth element + resize + shrik to fit to get the best k
- std::vector<std::pair<NodeId, float>> idDistanceVector {};
+ std::vector<std::pair<NodeId, float>> idDistanceVector{};
idDistanceVector.reserve(size());
for (std::size_t workerLevelIndex = 0; workerLevelIndex < nodes.size(); ++workerLevelIndex)
{
- for (std::size_t nodeLevelIndex = 0; nodeLevelIndex < nodes.at(workerLevelIndex).size(); ++nodeLevelIndex)
+ for (std::size_t nodeLevelIndex = 0; nodeLevelIndex < nodes.at(workerLevelIndex).size();
+ ++nodeLevelIndex)
{
- const NodeId id
- {
- static_cast<Ice::Long>(workerLevelIndex), static_cast<Ice::Long>(nodeLevelIndex)
- };
- const auto dist = WorkerNode::distance(nodes.at(workerLevelIndex).at(nodeLevelIndex).config, cfg);
+ const NodeId id{static_cast<Ice::Long>(workerLevelIndex),
+ static_cast<Ice::Long>(nodeLevelIndex)};
+ const auto dist =
+ WorkerNode::distance(nodes.at(workerLevelIndex).at(nodeLevelIndex).config, cfg);
idDistanceVector.emplace_back(id, dist);
}
}
@@ -339,10 +345,7 @@ namespace armarx::addirrtstar
idDistanceVector.begin() + (k - 1),
idDistanceVector.end(),
[](const std::pair<NodeId, float>& lhs, const std::pair<NodeId, float>& rhs)
- {
- return lhs.second < rhs.second;
- }
- );
+ { return lhs.second < rhs.second; });
idDistanceVector.resize(k);
//we could be using a large amount of memory => try to free it
idDistanceVector.shrink_to_fit();
@@ -351,7 +354,8 @@ namespace armarx::addirrtstar
return idDistanceVector;
}
- Tree::NodeType& Tree::getNode(const NodeId& id)
+ Tree::NodeType&
+ Tree::getNode(const NodeId& id)
{
const auto worker = static_cast<std::size_t>(id.workerId);
const auto node = static_cast<std::size_t>(id.numberOfNode);
@@ -360,7 +364,8 @@ namespace armarx::addirrtstar
return nodes.at(worker).at(node);
}
- const Tree::NodeType& Tree::getNode(const NodeId& id) const
+ const Tree::NodeType&
+ Tree::getNode(const NodeId& id) const
{
const auto worker = static_cast<std::size_t>(id.workerId);
const auto node = static_cast<std::size_t>(id.numberOfNode);
@@ -369,11 +374,12 @@ namespace armarx::addirrtstar
return nodes.at(worker).at(node);
}
- NodeId Tree::getIdOfIndex(std::size_t index) const
+ NodeId
+ Tree::getIdOfIndex(std::size_t index) const
{
if (index >= size())
{
- throw std::out_of_range {"index >= size"};
+ throw std::out_of_range{"index >= size"};
}
Ice::Long worker = 0;
@@ -393,31 +399,40 @@ namespace armarx::addirrtstar
//since this tree should never decrease its size this line of code should never be called!
//it could be called if a thread is concurrently changing the size of nodes,
//causing iterators to be invalidated (thus causing ub while iterating in for(...))
- throw std::logic_error {"Tree size decreased during id calculation!"};
+ throw std::logic_error{"Tree size decreased during id calculation!"};
}
- float Tree::getBestCost() const
+ float
+ Tree::getBestCost() const
{
auto min_elem = getBestCostIt();
- return ((min_elem == goalNodes.end()) ? std::numeric_limits<float>::infinity() : getNode(min_elem->node).fromStartCost + min_elem->costToGoToGoal);
+ return ((min_elem == goalNodes.end())
+ ? std::numeric_limits<float>::infinity()
+ : getNode(min_elem->node).fromStartCost + min_elem->costToGoToGoal);
}
//querry for paths
- PathWithCost Tree::getBestPath() const
+ PathWithCost
+ Tree::getBestPath() const
{
auto min_elem = getBestCostIt();
ARMARX_CHECK_EXPRESSION(min_elem != goalNodes.end());
const auto id = min_elem->node;
- return PathWithCost {getPathTo(id), getNode(id).fromStartCost + min_elem->costToGoToGoal, "bestPath"};
+ return PathWithCost{
+ getPathTo(id), getNode(id).fromStartCost + min_elem->costToGoToGoal, "bestPath"};
}
- PathWithCost Tree::getNthPathWithCost(std::size_t n) const
+
+ PathWithCost
+ Tree::getNthPathWithCost(std::size_t n) const
{
ARMARX_CHECK_EXPRESSION(n < goalNodes.size());
//for client side request the index should be in bounds
- return PathWithCost {getPathTo(goalNodes.at(n).node), getNthPathCost(n), "Path_" + to_string(n)};
+ return PathWithCost{
+ getPathTo(goalNodes.at(n).node), getNthPathCost(n), "Path_" + to_string(n)};
}
- NodeIdList Tree::getNthPathIds(std::size_t n) const
+ NodeIdList
+ Tree::getNthPathIds(std::size_t n) const
{
ARMARX_CHECK_EXPRESSION(n < goalNodes.size());
NodeIdList pathIds;
@@ -437,7 +452,8 @@ namespace armarx::addirrtstar
return pathIds;
}
- VectorXfSeq Tree::getPathTo(NodeId id) const
+ VectorXfSeq
+ Tree::getPathTo(NodeId id) const
{
VectorXfSeq path;
@@ -455,9 +471,10 @@ namespace armarx::addirrtstar
return path;
}
- FullIceTree Tree::getIceTree() const
+ FullIceTree
+ Tree::getIceTree() const
{
- FullNodeDataListList allNodes {};
+ FullNodeDataListList allNodes{};
allNodes.resize(nodes.size());
for (std::size_t workerLevelIndex = 0; workerLevelIndex < nodes.size(); ++workerLevelIndex)
@@ -466,7 +483,8 @@ namespace armarx::addirrtstar
const auto& workerNodes = nodes.at(workerLevelIndex);
iceWorkerNodes.resize(workerNodes.size());
- for (std::size_t nodeLevelIndex = 0; nodeLevelIndex < iceWorkerNodes.size(); ++nodeLevelIndex)
+ for (std::size_t nodeLevelIndex = 0; nodeLevelIndex < iceWorkerNodes.size();
+ ++nodeLevelIndex)
{
const auto& localNode = workerNodes.at(nodeLevelIndex);
FullNodeData& iceNode = iceWorkerNodes.at(nodeLevelIndex);
@@ -477,10 +495,11 @@ namespace armarx::addirrtstar
}
}
- return FullIceTree {allNodes, appliedUpdateIds};
+ return FullIceTree{allNodes, appliedUpdateIds};
}
- void Tree::addPendingUpdate(const Update& u)
+ void
+ Tree::addPendingUpdate(const Update& u)
{
//fill up last spaces with no update (-1)
// increaseWorkerCountTo(u.dependetOnUpdateIds.size());//can't be called here because it invalidates iterators on nodes
@@ -495,5 +514,4 @@ namespace armarx::addirrtstar
pendingUpdatesLookupTable[std::make_pair(nodeId, updateSubId)] = pendingUpdates.size();
pendingUpdates.emplace_back(u);
}
-}
-
+} // namespace armarx::addirrtstar
diff --git a/source/RobotComponents/components/MotionPlanning/Tasks/AdaptiveDynamicDomainInformedRRTStar/Tree.h b/source/RobotComponents/components/MotionPlanning/Tasks/AdaptiveDynamicDomainInformedRRTStar/Tree.h
index 6d026a26985f1e0e5e6c1fdefd92522cf9b6bdb7..1185fed5059f69d13d7428233cc88ad3e4a6ccef 100644
--- a/source/RobotComponents/components/MotionPlanning/Tasks/AdaptiveDynamicDomainInformedRRTStar/Tree.h
+++ b/source/RobotComponents/components/MotionPlanning/Tasks/AdaptiveDynamicDomainInformedRRTStar/Tree.h
@@ -54,6 +54,7 @@ namespace armarx::addirrtstar
* @brief The type of configurations.
*/
using ConfigType = VectorXf;
+
/**
* @brief Represents a node of thr rrt.
*/
@@ -88,7 +89,8 @@ namespace armarx::addirrtstar
/**
* @brief Lookuptable for pending updates.
*/
- using PendingUpdateLookuptableType = std::unordered_map<std::pair<std::size_t, std::size_t>, std::size_t>;
+ using PendingUpdateLookuptableType =
+ std::unordered_map<std::pair<std::size_t, std::size_t>, std::size_t>;
/**
* @brief Iterator for the PendingUpdateLookuptable.
*/
@@ -96,7 +98,8 @@ namespace armarx::addirrtstar
/**
* @brief Const iterator for the PendingUpdateLookuptable.
*/
- using PendingUpdateLookuptableConstIterator = typename PendingUpdateLookuptableType::const_iterator;
+ using PendingUpdateLookuptableConstIterator =
+ typename PendingUpdateLookuptableType::const_iterator;
/**
* @brief Default ctor.
@@ -119,8 +122,7 @@ namespace armarx::addirrtstar
*/
void init(const FullIceTree& iceTree,
AdaptiveDynamicDomainParameters newaddParams,
- std::size_t newWorkerId
- );
+ std::size_t newWorkerId);
/**
* @brief Increases the storage of all data structures to be appropriate for count workers.
@@ -150,10 +152,12 @@ namespace armarx::addirrtstar
* @param prx The proxy to send the update to.
*/
void sendCurrentUpdate(TreeUpdateInterfacePrx& prx);
+
/**
* @return Returns all updates send by this tree.
*/
- const std::deque<Update>& getLocalUpdates() const
+ const std::deque<Update>&
+ getLocalUpdates() const
{
return localUpdates;
}
@@ -163,17 +167,20 @@ namespace armarx::addirrtstar
* @param updateId The update's sub id.
* @return Whether the given update was applied.
*/
- bool hasAppliedUpdate(std::size_t workerId, Ice::Long updateId)
+ bool
+ hasAppliedUpdate(std::size_t workerId, Ice::Long updateId)
{
ARMARX_CHECK_EXPRESSION(workerId < appliedUpdateIds.size());
return updateId <= appliedUpdateIds.at(workerId);
}
+
/**
* @param workerId The update's worker id.
* @param updateId The update's sub id.
* @return Whether the given update is in the list of pending updates.
*/
- bool hasPendingUpdate(std::size_t workerId, std::size_t updateId) const
+ bool
+ hasPendingUpdate(std::size_t workerId, std::size_t updateId) const
{
return findPendingUpdate(workerId, updateId) != pendingUpdatesLookupTable.end();
}
@@ -183,7 +190,8 @@ namespace armarx::addirrtstar
* @param updateId The update's sub id.
* @return Returns the requested update from the list of pending updates.
*/
- const Update& getPendingUpdate(std::size_t workerId, std::size_t updateId) const
+ const Update&
+ getPendingUpdate(std::size_t workerId, std::size_t updateId) const
{
ARMARX_CHECK_EXPRESSION(hasPendingUpdate(workerId, updateId));
return pendingUpdates.at(findPendingUpdate(workerId, updateId)->second);
@@ -194,19 +202,21 @@ namespace armarx::addirrtstar
* If a required update is missing this methode will fail. (no update getter is providet).
* This function is not thread save! Use it only in a serial/protected environment.
*/
- void applyPendingUpdates()
+ void
+ applyPendingUpdates()
{
std::mutex m;
- std::unique_lock<std::mutex> lock {m};
+ std::unique_lock<std::mutex> lock{m};
applyPendingUpdates(
lock,
[](std::size_t w, Ice::Long u)
- {
- ARMARX_ERROR_S << "no remote update getter set but missing an update (w/s): " << w << "/" << u;
- throw std::logic_error {"no remote update getter set but missing an update!"};
- return Update();
- }
- );
+ {
+ ARMARX_ERROR_S
+ << "no remote update getter set but missing an update (w/s): " << w << "/"
+ << u;
+ throw std::logic_error{"no remote update getter set but missing an update!"};
+ return Update();
+ });
}
/**
@@ -215,8 +225,9 @@ namespace armarx::addirrtstar
* @param getRemoteUpdate Function used to request a missing update.
* @see Tree::applyPendingUpdates
*/
- template<class LockType, class RemoteUpdateGetter>
- void applyPendingUpdates(LockType&& treeLock, RemoteUpdateGetter getRemoteUpdate)
+ template <class LockType, class RemoteUpdateGetter>
+ void
+ applyPendingUpdates(LockType&& treeLock, RemoteUpdateGetter getRemoteUpdate)
{
applyPendingUpdates(treeLock, getRemoteUpdate, [](Update&&) {});
}
@@ -228,11 +239,12 @@ namespace armarx::addirrtstar
* @param getRemoteUpdate Function used to request a missing update.
* @param updateConsumer Function used to consume updates after they were added. (will be moved to the consumer)
*/
- template<class LockType, class RemoteUpdateGetter, class UpdateConsumer>
- void applyPendingUpdates(
+ template <class LockType, class RemoteUpdateGetter, class UpdateConsumer>
+ void
+ applyPendingUpdates(
LockType&& treeLock,
RemoteUpdateGetter getRemoteUpdate,
- UpdateConsumer updateConsumer = [](Update&&) {} //no op
+ UpdateConsumer updateConsumer = [](Update&&) {} //no op
)
{
ARMARX_CHECK_EXPRESSION(treeLock);
@@ -258,16 +270,19 @@ namespace armarx::addirrtstar
/**
* @return The update sub id of the current update. (The one currently constructed, not the last send)
*/
- Ice::Long getCurrentUpdateId() const
+ Ice::Long
+ getCurrentUpdateId() const
{
const auto id = static_cast<Ice::Long>(getLocalUpdates().size()) - 1;
ARMARX_CHECK_EXPRESSION(id == appliedUpdateIds.at(workerId) + 1);
return id;
}
+
/**
* @return The update sub id of the last send update.
*/
- Ice::Long getPreviousUpdateId() const
+ Ice::Long
+ getPreviousUpdateId() const
{
return appliedUpdateIds.at(workerId);
}
@@ -275,15 +290,18 @@ namespace armarx::addirrtstar
/**
* @return The currently constructed update.
*/
- const Update& getCurrentUpdate() const
+ const Update&
+ getCurrentUpdate() const
{
return localUpdates.back();
}
+
protected:
/**
* @return The currently constructed update. (non const version for internal use)
*/
- Update& getCurrentUpdateNonConst()
+ Update&
+ getCurrentUpdateNonConst()
{
return localUpdates.back();
}
@@ -293,25 +311,30 @@ namespace armarx::addirrtstar
* @param u The update.
*/
void applyRadiusUpdate(const RadiusUpdate& u);
+
/**
* @brief Applies a node creation update to the tree.
* @param u The update.
* @param workerId The updates source worker.
*/
- void applyNodeCreationUpdate(const NodeCreationUpdate& u, std::size_t workerId)
+ void
+ applyNodeCreationUpdate(const NodeCreationUpdate& u, std::size_t workerId)
{
doAddNode(u.config, u.parent, u.fromParentCost, workerId);
}
+
/**
* @brief Applies a rewire update to the tree.
* @param rewireUpdate The update.
*/
void applyRewireUpdate(const RewireUpdate& rewireUpdate);
+
/**
* @brief Applies multiple goal reached updates to the tree.
* @param newGoalNodes The new nodes that can reach the goal configuration.
*/
- void applyGoalReachedUpdates(const GoalInfoList newGoalNodes)
+ void
+ applyGoalReachedUpdates(const GoalInfoList newGoalNodes)
{
std::copy(newGoalNodes.begin(), newGoalNodes.end(), std::back_inserter(goalNodes));
}
@@ -326,38 +349,49 @@ namespace armarx::addirrtstar
* @param id The node's id.
* @param increaseRadius Whether the node's radius increases.
*/
- void createNewRadiusUpdate(const NodeId& id, bool increaseRadius)
+ void
+ createNewRadiusUpdate(const NodeId& id, bool increaseRadius)
{
- getCurrentUpdateNonConst().radii.emplace_back(RadiusUpdate {id, increaseRadius});
+ getCurrentUpdateNonConst().radii.emplace_back(RadiusUpdate{id, increaseRadius});
}
+
/**
* @brief Adds a new node creation update to the current update.
* @param cfg The new node's configuration.
* @param parent The new node's parent node.
* @param fromParentCost The cost of the edge (node, parent)
*/
- void createNewNodeCreationUpdate(const ConfigType& cfg, const NodeId& parent, float fromParentCost)
+ void
+ createNewNodeCreationUpdate(const ConfigType& cfg,
+ const NodeId& parent,
+ float fromParentCost)
{
- getCurrentUpdateNonConst().nodes.emplace_back(NodeCreationUpdate {cfg, parent, fromParentCost});
+ getCurrentUpdateNonConst().nodes.emplace_back(
+ NodeCreationUpdate{cfg, parent, fromParentCost});
}
+
/**
* @brief Adds a new node creation update to the current update.
* @param child The rewired child node.
* @param newParent The new parent node.
* @param fromParentCost The cost of the edge (child, new parent)
*/
- void createNewRewireUpdate(const NodeId& child, const NodeId& newParent, float fromParentCost)
+ void
+ createNewRewireUpdate(const NodeId& child, const NodeId& newParent, float fromParentCost)
{
- getCurrentUpdateNonConst().rewires.emplace_back(RewireUpdate {child, newParent, fromParentCost});
+ getCurrentUpdateNonConst().rewires.emplace_back(
+ RewireUpdate{child, newParent, fromParentCost});
}
+
/**
* @brief Adds a new goal reached update to the current update.
* @param goal The node that can reach the goal configuration.
* @param costToGoToGoal The cost to reach the goal configuration from the given node)
*/
- void createNewGoalReachedUpdate(const NodeId& goal, float costToGoToGoal)
+ void
+ createNewGoalReachedUpdate(const NodeId& goal, float costToGoToGoal)
{
- getCurrentUpdateNonConst().newGoalNodes.emplace_back(GoalInfo {goal, costToGoToGoal});
+ getCurrentUpdateNonConst().newGoalNodes.emplace_back(GoalInfo{goal, costToGoToGoal});
}
/**
@@ -368,8 +402,12 @@ namespace armarx::addirrtstar
* @param getRemoteUpdate Function used to request a missing update.
* @param updateConsumer Function used to consume updates after they were added. (will be moved to the consumer)
*/
- template<class LockType, class RemoteUpdateGetter, class UpdateConsumer>
- void applyPendingUpdate(std::size_t updateIndex, LockType&& treeLock, RemoteUpdateGetter getRemoteUpdate, UpdateConsumer updateConsumer)
+ template <class LockType, class RemoteUpdateGetter, class UpdateConsumer>
+ void
+ applyPendingUpdate(std::size_t updateIndex,
+ LockType&& treeLock,
+ RemoteUpdateGetter getRemoteUpdate,
+ UpdateConsumer updateConsumer)
{
ARMARX_CHECK_EXPRESSION(treeLock);
ARMARX_CHECK_EXPRESSION(updateIndex < pendingUpdates.size());
@@ -392,9 +430,8 @@ namespace armarx::addirrtstar
const auto updateId = dependetOnUpdateIds.at(updateWorkerId) + 1;
//is it an update from this node or was it already applied?
- if (
- updateWorkerId == workerId || //self update
- hasAppliedUpdate(updateWorkerId, updateId)//update was already applied
+ if (updateWorkerId == workerId || //self update
+ hasAppliedUpdate(updateWorkerId, updateId) //update was already applied
)
{
return;
@@ -419,12 +456,17 @@ namespace armarx::addirrtstar
* @param getRemoteUpdate Function used to request a missing update.
* @param updateConsumer Function used to consume updates after they were added. (will be moved to the consumer)
*/
- template<class LockType, class RemoteUpdateGetter, class UpdateConsumer>
- void prepareUpdate(Ice::LongSeq dependetOnUpdateIds, LockType&& treeLock, RemoteUpdateGetter getRemoteUpdate, UpdateConsumer updateConsumer)
+ template <class LockType, class RemoteUpdateGetter, class UpdateConsumer>
+ void
+ prepareUpdate(Ice::LongSeq dependetOnUpdateIds,
+ LockType&& treeLock,
+ RemoteUpdateGetter getRemoteUpdate,
+ UpdateConsumer updateConsumer)
{
ARMARX_CHECK_EXPRESSION(treeLock);
- for (std::size_t workerNodeId = 0; workerNodeId < dependetOnUpdateIds.size(); ++workerNodeId)
+ for (std::size_t workerNodeId = 0; workerNodeId < dependetOnUpdateIds.size();
+ ++workerNodeId)
{
const auto updateSubId = dependetOnUpdateIds.at(workerNodeId);
@@ -452,7 +494,8 @@ namespace armarx::addirrtstar
Update update = getRemoteUpdate(workerNodeId, updateSubId);
treeLock.lock();
//prepare this update and apply it
- prepareUpdate(update.dependetOnUpdateIds, treeLock, getRemoteUpdate, updateConsumer);
+ prepareUpdate(
+ update.dependetOnUpdateIds, treeLock, getRemoteUpdate, updateConsumer);
ARMARX_CHECK_EXPRESSION(treeLock);
applyUpdate(update);
//we dont need the update anymore. move it to the consumer
@@ -460,13 +503,15 @@ namespace armarx::addirrtstar
}
}
}
+
protected:
/**
* @param workerId The update's worker.
* @param updateId The update's sub id.
* @return An iterator tho the pending update. (If it does not exist to end.)
*/
- PendingUpdateLookuptableIterator findPendingUpdate(std::size_t workerId, std::size_t updateId)
+ PendingUpdateLookuptableIterator
+ findPendingUpdate(std::size_t workerId, std::size_t updateId)
{
return pendingUpdatesLookupTable.find(std::make_pair(workerId, updateId));
}
@@ -476,7 +521,8 @@ namespace armarx::addirrtstar
* @param updateId The update's sub id.
* @return An iterator tho the pending update. (If it does not exist to end.) (const version)
*/
- PendingUpdateLookuptableConstIterator findPendingUpdate(std::size_t workerId, std::size_t updateId) const
+ PendingUpdateLookuptableConstIterator
+ findPendingUpdate(std::size_t workerId, std::size_t updateId) const
{
return pendingUpdatesLookupTable.find(std::make_pair(workerId, updateId));
}
@@ -491,45 +537,54 @@ namespace armarx::addirrtstar
* @return The new node's id.
*/
NodeId addNode(ConfigType cfg, const NodeId& parent, float fromParentCost);
+
/**
* @brief Sets the parent node of child to parent. (costs are updated) (creates an appropriate update)
* @param child The cild node.
* @param newParent The new parent node.
* @param fromParentCost The cost of the edge (child, new parent)
*/
- void setNodeParent(const NodeId& child, const NodeId& newParent, float fromParentCost)
+ void
+ setNodeParent(const NodeId& child, const NodeId& newParent, float fromParentCost)
{
doSetNodeParent(child, newParent, fromParentCost);
createNewRewireUpdate(child, newParent, fromParentCost);
}
+
/**
* @brief Decreases a node's radius.(creates an appropriate update)
* @param id The node.
*/
- void decreaseRadius(const NodeId& id)
+ void
+ decreaseRadius(const NodeId& id)
{
createNewRadiusUpdate(id, false);
doDecreaseRadius(id);
}
+
/**
* @brief Increases a node's radius.(creates an appropriate update)
* @param id The node.
*/
- void increaseRadius(const NodeId& id)
+ void
+ increaseRadius(const NodeId& id)
{
createNewRadiusUpdate(id, true);
doIncreaseRadius(id);
}
+
/**
* @brief Adds a node to the list of nodes that can reach the goal configuration. (creates an appropriate update)
* @param goal The node to add.
* @param costToGoToGoal The cost to reach the gol configuration from the given node.
*/
- void addGoalReached(const NodeId& goal, float costToGoToGoal)
+ void
+ addGoalReached(const NodeId& goal, float costToGoToGoal)
{
doAddGoalReached(goal, costToGoToGoal);
createNewGoalReachedUpdate(goal, costToGoToGoal);
}
+
protected:
//change the tree
/**
@@ -546,7 +601,8 @@ namespace armarx::addirrtstar
* @param workerId The worker creating this node.
* @return The new node's id.
*/
- NodeId doAddNode(ConfigType cfg, const NodeId& parent, float fromParentCost, std::size_t workerId);
+ NodeId
+ doAddNode(ConfigType cfg, const NodeId& parent, float fromParentCost, std::size_t workerId);
/**
* @brief Sets the parent node of child to parent. (costs are updated)
* @param child The cild node.
@@ -554,28 +610,35 @@ namespace armarx::addirrtstar
* @param fromParentCost The cost of the edge (child, new parent)
* @param updateFromStartCost Whether to update the costs of the subtree beneath cild.
*/
- void doSetNodeParent(const NodeId& child, const NodeId& newParent, float fromParentCost, bool updateFromStartCost = true);
+ void doSetNodeParent(const NodeId& child,
+ const NodeId& newParent,
+ float fromParentCost,
+ bool updateFromStartCost = true);
/**
* @brief Decreases a node's radius.
* @param id The node.
*/
void doDecreaseRadius(const NodeId& id);
+
/**
* @brief Increases a node's radius.
* @param id The node.
*/
- void doIncreaseRadius(const NodeId& id)
+ void
+ doIncreaseRadius(const NodeId& id)
{
getNode(id).radius *= (1.f + addParams.alpha); //inf will stay inf
}
+
/**
* @brief Adds a node to the list of nodes that can reach the goal configuration.
* @param goal The node to add.
* @param costToGoToGoal The cost to reach the gol configuration from the given node.
*/
- void doAddGoalReached(const NodeId& goal, float costToGoToGoal)
+ void
+ doAddGoalReached(const NodeId& goal, float costToGoToGoal)
{
- goalNodes.emplace_back(GoalInfo {goal, costToGoToGoal});
+ goalNodes.emplace_back(GoalInfo{goal, costToGoToGoal});
}
//querry the tree
@@ -585,15 +648,19 @@ namespace armarx::addirrtstar
* @param k The number of neighbours.
* @return The k nearest neighbours of and their distance to the given configuration.
*/
- std::vector<std::pair<NodeId, float>> getKNearestNeighboursAndDistances(const ConfigType& cfg, std::size_t k);
+ std::vector<std::pair<NodeId, float>>
+ getKNearestNeighboursAndDistances(const ConfigType& cfg, std::size_t k);
+
/**
* @param cfg The configuration.
* @return The nearest neighbour of and its distance to the given configuration.
*/
- std::pair<NodeId, float> getNearestNeighbourAndDistance(const ConfigType& cfg)
+ std::pair<NodeId, float>
+ getNearestNeighbourAndDistance(const ConfigType& cfg)
{
return getKNearestNeighboursAndDistances(cfg, 1).front();
}
+
/**
* @param id The node's id.
* @return The requested node.
@@ -604,10 +671,12 @@ namespace armarx::addirrtstar
* @return The requested node. (const)
*/
const NodeType& getNode(const NodeId& id) const;
+
/**
* @return All nodes.
*/
- const std::deque<std::deque<NodeType>>& getNodes() const
+ const std::deque<std::deque<NodeType>>&
+ getNodes() const
{
return nodes;
}
@@ -617,42 +686,52 @@ namespace armarx::addirrtstar
* @return The id corresponding to the given node index.
*/
NodeId getIdOfIndex(std::size_t index) const;
+
/**
* @return The number of nodes in the tree.
*/
- std::size_t size() const
+ std::size_t
+ size() const
{
return nodeCount;
}
+
/**
* @param id The node's id.
* @return The requested node.
*/
- NodeType& at(const NodeId& id)
+ NodeType&
+ at(const NodeId& id)
{
return getNode(id);
}
+
/**
* @param id The node's id.
* @return The requested node. (const)
*/
- const NodeType& at(const NodeId& id) const
+ const NodeType&
+ at(const NodeId& id) const
{
return getNode(id);
}
+
/**
* @param index The node's index.
* @return The requested node.
*/
- NodeType& at(std::size_t index)
+ NodeType&
+ at(std::size_t index)
{
return at(getIdOfIndex(index));
}
+
/**
* @param index The node's index.
* @return The requested node. (const)
*/
- const NodeType& at(std::size_t index) const
+ const NodeType&
+ at(std::size_t index) const
{
return at(getIdOfIndex(index));
}
@@ -666,12 +745,17 @@ namespace armarx::addirrtstar
/**
* @return An iterator to the node that can reach the goal configuration and results in the lowest path cost. (end if no path was found)
*/
- typename std::deque<GoalInfo>::const_iterator getBestCostIt() const
+ typename std::deque<GoalInfo>::const_iterator
+ getBestCostIt() const
{
- return std::min_element(goalNodes.begin(), goalNodes.end(), [this](const GoalInfo & fst, const GoalInfo & snd)
- {
- return (getNode(fst.node).fromStartCost + fst.costToGoToGoal) < (getNode(snd.node).fromStartCost + snd.costToGoToGoal);
- });
+ return std::min_element(
+ goalNodes.begin(),
+ goalNodes.end(),
+ [this](const GoalInfo& fst, const GoalInfo& snd)
+ {
+ return (getNode(fst.node).fromStartCost + fst.costToGoToGoal) <
+ (getNode(snd.node).fromStartCost + snd.costToGoToGoal);
+ });
}
//querry for paths
@@ -690,22 +774,19 @@ namespace armarx::addirrtstar
* @param nodeId The node to check.
* @return Whether the given node is a node in the list of nodes able to reach the goal configuration.
*/
- bool hasGoalNode(const NodeId& nodeId) const
+ bool
+ hasGoalNode(const NodeId& nodeId) const
{
- return std::any_of(
- goalNodes.begin(),
- goalNodes.end(),
- [&nodeId](const GoalInfo & info)
- {
- return info.node == nodeId;
- }
- );
+ return std::any_of(goalNodes.begin(),
+ goalNodes.end(),
+ [&nodeId](const GoalInfo& info) { return info.node == nodeId; });
}
/**
* @return The number of paths found.
*/
- std::size_t getPathCount() const
+ std::size_t
+ getPathCount() const
{
return goalNodes.size();
}
@@ -714,11 +795,13 @@ namespace armarx::addirrtstar
* @param n The paths index.
* @return The cost of the given path.
*/
- float getNthPathCost(std::size_t n) const
+ float
+ getNthPathCost(std::size_t n) const
{
ARMARX_CHECK_EXPRESSION(n < goalNodes.size());
return getNode(goalNodes.at(n).node).fromStartCost + goalNodes.at(n).costToGoToGoal;
}
+
/**
* @param n N
* @return The nth path.
@@ -729,6 +812,7 @@ namespace armarx::addirrtstar
* @return The node ids of nodes contained by the given path.
*/
NodeIdList getNthPathIds(std::size_t n) const;
+
protected:
/**
* @param id The node's id.
@@ -741,10 +825,12 @@ namespace armarx::addirrtstar
* @param workerId The worker's id.
* @return The next node id for the given worker.
*/
- NodeId getNextNodeIdFor(std::size_t workerId)
+ NodeId
+ getNextNodeIdFor(std::size_t workerId)
{
ARMARX_CHECK_EXPRESSION(workerId < nodes.size());
- return {static_cast<Ice::Long>(workerId), static_cast<Ice::Long>(nodes.at(workerId).size())};
+ return {static_cast<Ice::Long>(workerId),
+ static_cast<Ice::Long>(nodes.at(workerId).size())};
}
/**
@@ -755,7 +841,8 @@ namespace armarx::addirrtstar
/**
* @return List of ids of applied updates.
*/
- const Ice::LongSeq& getAppliedUpdateIds() const
+ const Ice::LongSeq&
+ getAppliedUpdateIds() const
{
return appliedUpdateIds;
}
@@ -809,4 +896,4 @@ namespace armarx::addirrtstar
*/
PendingUpdateLookuptableType pendingUpdatesLookupTable; //only default init required
};
-}
+} // namespace armarx::addirrtstar
diff --git a/source/RobotComponents/components/MotionPlanning/Tasks/AdaptiveDynamicDomainInformedRRTStar/WorkerNode.cpp b/source/RobotComponents/components/MotionPlanning/Tasks/AdaptiveDynamicDomainInformedRRTStar/WorkerNode.cpp
index 5232d7cb0779f47cefd44ae54ac60b5c1e49e433..50d8f6cf440c42a79938d81ef596dfb62986abd4 100644
--- a/source/RobotComponents/components/MotionPlanning/Tasks/AdaptiveDynamicDomainInformedRRTStar/WorkerNode.cpp
+++ b/source/RobotComponents/components/MotionPlanning/Tasks/AdaptiveDynamicDomainInformedRRTStar/WorkerNode.cpp
@@ -22,19 +22,20 @@
* GNU General Public License
*/
+#include "WorkerNode.h"
+
#include <algorithm>
#include <chrono>
#include <ArmarXCore/core/ArmarXObjectScheduler.h>
#include "../../util/CollisionCheckUtil.h"
-
-#include "WorkerNode.h"
#include "util.h"
namespace armarx::addirrtstar
{
- void WorkerNode::onInitComponent()
+ void
+ WorkerNode::onInitComponent()
{
ARMARX_CHECK_EXPRESSION(manager);
//init values
@@ -48,31 +49,35 @@ namespace armarx::addirrtstar
offeringTopic(updateTopicName);
//start worker
- workerThread = std::thread {[this]{workerTask();}};
+ workerThread = std::thread{[this] { workerTask(); }};
}
- void WorkerNode::onConnectComponent()
+ void
+ WorkerNode::onConnectComponent()
{
globalWorkers = getTopic<TreeUpdateInterfacePrx>(updateTopicName);
}
- void WorkerNode::onExitComponent()
+ void
+ WorkerNode::onExitComponent()
{
//kill worker and join it
killWorker();
workerThread.join();
}
- void WorkerNode::updateTree(const Update& u, const Ice::Current&)
+ void
+ WorkerNode::updateTree(const Update& u, const Ice::Current&)
{
- std::lock_guard<std::mutex> lock {updateMutex};
+ std::lock_guard<std::mutex> lock{updateMutex};
tree.addPendingUpdate(u);
}
- void WorkerNode::workerTask()
+ void
+ WorkerNode::workerTask()
{
//init sampler
- std::vector<std::pair<float, float>> dimensionBounds {};
+ std::vector<std::pair<float, float>> dimensionBounds{};
dimensionBounds.reserve(getDimensionality());
for (const auto& dim : cspace->getDimensionsBounds())
@@ -84,16 +89,13 @@ namespace armarx::addirrtstar
ARMARX_CHECK_EXPRESSION(getDimensionality() == startCfg.size());
sampler.reset(
- new InformedSampler
- {
- typename InformedSampler::DistributionType{
- dimensionBounds.begin(), dimensionBounds.end(),
- startCfg.begin(), startCfg.end(), goalCfg.begin(),
- rotationMatrix
- },
- std::mt19937{std::random_device{}()}
- }
- );
+ new InformedSampler{typename InformedSampler::DistributionType{dimensionBounds.begin(),
+ dimensionBounds.end(),
+ startCfg.begin(),
+ startCfg.end(),
+ goalCfg.begin(),
+ rotationMatrix},
+ std::mt19937{std::random_device{}()}});
rotationMatrix.clear();
//after starting the requred proxies are set.
@@ -128,13 +130,15 @@ namespace armarx::addirrtstar
manager->setWorkersFinalUpdateId(workerId, finalUpdateId);
}
- void WorkerNode::doBatch()
+ void
+ WorkerNode::doBatch()
{
//update max cost for sampler
sampler->getDistribution().setMaximalCost(tree.getBestCost());
//iterate batch
- for (Ice::Long currentBatchIteration = 0; currentBatchIteration < batchSize; ++currentBatchIteration)
+ for (Ice::Long currentBatchIteration = 0; currentBatchIteration < batchSize;
+ ++currentBatchIteration)
{
// //break on kill => sends last update and ends calculation
if (killRequest)
@@ -143,7 +147,7 @@ namespace armarx::addirrtstar
}
doBatchIteration();
- }//for batch
+ } //for batch
//was the goal reached?
//try connect the nearest node of all newly (since the last check) added nodes to the goal
@@ -156,7 +160,9 @@ namespace armarx::addirrtstar
float goalConnectNodeNearestDistance = std::numeric_limits<float>::infinity();
// const Update& u = tree.getCurrentUpdate();
- for (std::size_t numberOfNode = onePastLastGoalConnect; numberOfNode < workersNodes.size(); ++numberOfNode)
+ for (std::size_t numberOfNode = onePastLastGoalConnect;
+ numberOfNode < workersNodes.size();
+ ++numberOfNode)
{
const Tree::NodeType& currNode = workersNodes.at(numberOfNode);
const float currNodeDistance = distance(goalCfg, currNode.config);
@@ -170,7 +176,8 @@ namespace armarx::addirrtstar
if (goalConnectNodeNearestDistance < std::numeric_limits<float>::infinity())
{
- const auto cfgReached = steer(tree.getNode(goalConnectNodeNearestId).config, goalCfg);
+ const auto cfgReached =
+ steer(tree.getNode(goalConnectNodeNearestId).config, goalCfg);
if (cfgReached == tree.getNode(goalConnectNodeNearestId).config)
{
@@ -196,26 +203,26 @@ namespace armarx::addirrtstar
//send update
{
- std::lock_guard<std::mutex> lock {updateMutex};
+ std::lock_guard<std::mutex> lock{updateMutex};
tree.sendCurrentUpdate(globalWorkers);
}
}
- void WorkerNode::applyPendingUpdates()
+ void
+ WorkerNode::applyPendingUpdates()
{
{
//add updates
- tree.applyPendingUpdates(
- std::unique_lock<std::mutex> {updateMutex},
- [this](Ice::Long workerNodeId, Ice::Long updateSubId)
- {
- return getRemoteUpdate(workerNodeId, updateSubId);
- }
- ); //this methode is thread save if the given mutex is correct
+ tree.applyPendingUpdates(std::unique_lock<std::mutex>{updateMutex},
+ [this](Ice::Long workerNodeId, Ice::Long updateSubId)
+ {
+ return getRemoteUpdate(workerNodeId, updateSubId);
+ }); //this methode is thread save if the given mutex is correct
}
}
- void WorkerNode::doBatchIteration()
+ void
+ WorkerNode::doBatchIteration()
{
applyPendingUpdates();
@@ -227,7 +234,7 @@ namespace armarx::addirrtstar
//sample until the sample is in the dynamic domain of nearest
{
- u_int64_t samplingTries {0}; //should never overflow
+ u_int64_t samplingTries{0}; //should never overflow
do
{
//dont get stuck in this loop when killed
@@ -247,10 +254,10 @@ namespace armarx::addirrtstar
}
}
(*sampler)(cfgRnd.data());
- std::tie(nodeNearestId, nodeNearestDistance) = tree.getNearestNeighbourAndDistance(cfgRnd);
+ std::tie(nodeNearestId, nodeNearestDistance) =
+ tree.getNearestNeighbourAndDistance(cfgRnd);
++samplingTries;
- }
- while (nodeNearestDistance > tree.getNode(nodeNearestId).radius);
+ } while (nodeNearestDistance > tree.getNode(nodeNearestId).radius);
}
const auto cfgReached = steer(tree.getNode(nodeNearestId).config, cfgRnd);
@@ -269,9 +276,8 @@ namespace armarx::addirrtstar
addAndRewireConfig(cfgReached, nodeNearestId);
}
- void WorkerNode::addAndRewireConfig(
- const ConfigType& cfgReached,
- const NodeId& nodeNearestId)
+ void
+ WorkerNode::addAndRewireConfig(const ConfigType& cfgReached, const NodeId& nodeNearestId)
{
NodeId nodeBestId;
float costReachedToNodeBest;
@@ -279,36 +285,32 @@ namespace armarx::addirrtstar
std::map<NodeId, bool> isCollisionFreeCache;
- findBestParent(
- nodeNearestId,
- cfgReached,
- //out
- nodeBestId,
- costReachedToNodeBest,
- kNearestIdsAndDistances,
- isCollisionFreeCache
- );
+ findBestParent(nodeNearestId,
+ cfgReached,
+ //out
+ nodeBestId,
+ costReachedToNodeBest,
+ kNearestIdsAndDistances,
+ isCollisionFreeCache);
//add node to the tree
- const auto nodeReachedId = tree.addNode(
- cfgReached, //reached point
- nodeBestId, //parent id
- costReachedToNodeBest//cost parent->node
- );
+ const auto nodeReachedId = tree.addNode(cfgReached, //reached point
+ nodeBestId, //parent id
+ costReachedToNodeBest //cost parent->node
+ );
rewire(nodeReachedId, kNearestIdsAndDistances, isCollisionFreeCache);
}
- void WorkerNode::rewire(
- const NodeId& nodeId,
- const std::vector<std::pair<NodeId, float>>& kNearestIdsAndDistances,
- const std::map<NodeId, bool>& isCollisionFreeCache
- )
+ void
+ WorkerNode::rewire(const NodeId& nodeId,
+ const std::vector<std::pair<NodeId, float>>& kNearestIdsAndDistances,
+ const std::map<NodeId, bool>& isCollisionFreeCache)
{
//find root node
const auto& node = tree.getNode(nodeId);
const ConfigType& cfgNode = node.config;
- const float nodeFromStartCost = node.fromStartCost;
+ const float nodeFromStartCost = node.fromStartCost;
for (const auto& nearIdAndDist : kNearestIdsAndDistances)
{
@@ -326,29 +328,29 @@ namespace armarx::addirrtstar
//check collision
const auto it = isCollisionFreeCache.find(nodeNearId);
- if ((it != isCollisionFreeCache.end() && (*it).second) || isPathCollisionFree(cfgNode, cfgNodeNear))
+ if ((it != isCollisionFreeCache.end() && (*it).second) ||
+ isPathCollisionFree(cfgNode, cfgNodeNear))
{
//rewire node
tree.setNodeParent(nodeNearId, nodeId, costNodeToNear);
}
-
}
}
}
- void WorkerNode::findBestParent(
- const NodeId& nodeNearestId,
- const ConfigType& cfgReached,
- NodeId& outNodeBestId,
- float& outCostReachedToNodeBest,
- std::vector<std::pair<NodeId, float>>& outKNearestIdsAndDistances,
- std::map<NodeId, bool>& outIsCollisionFreeCache
- )
+ void
+ WorkerNode::findBestParent(const NodeId& nodeNearestId,
+ const ConfigType& cfgReached,
+ NodeId& outNodeBestId,
+ float& outCostReachedToNodeBest,
+ std::vector<std::pair<NodeId, float>>& outKNearestIdsAndDistances,
+ std::map<NodeId, bool>& outIsCollisionFreeCache)
{
// //find parent
outNodeBestId = nodeNearestId;
outCostReachedToNodeBest = distance(tree.getNode(outNodeBestId).config, cfgReached);
- auto costPathToReachedOverNodeBest = tree.getNode(nodeNearestId).fromStartCost + outCostReachedToNodeBest;
+ auto costPathToReachedOverNodeBest =
+ tree.getNode(nodeNearestId).fromStartCost + outCostReachedToNodeBest;
outKNearestIdsAndDistances = tree.getKNearestNeighboursAndDistances(cfgReached, getK());
for (const auto& nearIdAndDist : outKNearestIdsAndDistances)
@@ -359,7 +361,8 @@ namespace armarx::addirrtstar
const auto& cfgNodeNear = nodeNear.config;
const auto costReachedToNodeNear = nearIdAndDist.second;
- const auto costPathToReachedOverNodeNear = nodeNear.fromStartCost + costReachedToNodeNear;
+ const auto costPathToReachedOverNodeNear =
+ nodeNear.fromStartCost + costReachedToNodeNear;
if (costPathToReachedOverNodeNear < costPathToReachedOverNodeBest)
{
@@ -379,34 +382,38 @@ namespace armarx::addirrtstar
}
}
- WorkerNode::ConfigType WorkerNode::steer(const ConfigType& from, const ConfigType& to)
+ WorkerNode::ConfigType
+ WorkerNode::steer(const ConfigType& from, const ConfigType& to)
{
ARMARX_CHECK_EXPRESSION(getDimensionality() == from.size());
ARMARX_CHECK_EXPRESSION(getDimensionality() == to.size());
- return dcdSteer(from, to, DCDStepSize, [this](const ConfigType & cfg)
- {
- return isCollisionFree(cfg);
- });
+ return dcdSteer(
+ from, to, DCDStepSize, [this](const ConfigType& cfg) { return isCollisionFree(cfg); });
}
- bool WorkerNode::isPathCollisionFree(const ConfigType& from, const ConfigType& to)
+ bool
+ WorkerNode::isPathCollisionFree(const ConfigType& from, const ConfigType& to)
{
- bool tmp = dcdIsPathCollisionFree(from, to, DCDStepSize, [this](const ConfigType & cfg)
- {
- return isCollisionFree(cfg);
- });//steer(from, to) == to;
+ bool tmp = dcdIsPathCollisionFree(from,
+ to,
+ DCDStepSize,
+ [this](const ConfigType& cfg) {
+ return isCollisionFree(cfg);
+ }); //steer(from, to) == to;
return tmp;
}
- bool WorkerNode::isCollisionFree(const ConfigType& cfg)
+ bool
+ WorkerNode::isCollisionFree(const ConfigType& cfg)
{
bufferCDRange.first = cfg.data();
bufferCDRange.second = bufferCDRange.first + cfg.size();
return cspace->isCollisionFree(bufferCDRange);
}
- std::size_t WorkerNode::getK()
+ std::size_t
+ WorkerNode::getK()
{
const auto dim = static_cast<float>(getDimensionality());
const auto kRRT = std::pow(2.f, dim + 1.f) * M_E * (1.f + 1.f / dim);
@@ -414,16 +421,18 @@ namespace armarx::addirrtstar
return static_cast<std::size_t>(std::ceil(k));
}
- Update WorkerNode::getUpdate(Ice::Long updateId, const Ice::Current&) const
+ Update
+ WorkerNode::getUpdate(Ice::Long updateId, const Ice::Current&) const
{
- std::lock_guard<std::mutex> lock {updateMutex};
+ std::lock_guard<std::mutex> lock{updateMutex};
ARMARX_CHECK_EXPRESSION(updateId >= 0);
ARMARX_CHECK_EXPRESSION(updateId <= tree.getPreviousUpdateId());
return tree.getLocalUpdates().at(static_cast<std::size_t>(updateId));
}
- Update WorkerNode::getRemoteUpdate(Ice::Long workerNodeId, Ice::Long updateSubId)
+ Update
+ WorkerNode::getRemoteUpdate(Ice::Long workerNodeId, Ice::Long updateSubId)
{
return manager->getUpdate(workerNodeId, updateSubId);
}
-}
+} // namespace armarx::addirrtstar
diff --git a/source/RobotComponents/components/MotionPlanning/Tasks/AdaptiveDynamicDomainInformedRRTStar/WorkerNode.h b/source/RobotComponents/components/MotionPlanning/Tasks/AdaptiveDynamicDomainInformedRRTStar/WorkerNode.h
index d46d198421367bfaec949b746bad56829383ee9d..766736715750f7170775eef387f5f13f08c74bdb 100644
--- a/source/RobotComponents/components/MotionPlanning/Tasks/AdaptiveDynamicDomainInformedRRTStar/WorkerNode.h
+++ b/source/RobotComponents/components/MotionPlanning/Tasks/AdaptiveDynamicDomainInformedRRTStar/WorkerNode.h
@@ -24,26 +24,28 @@
#pragma once
#include <atomic>
+#include <deque>
#include <mutex>
#include <thread>
#include <unordered_map>
#include <utility>
#include <vector>
-#include <deque>
#include <ArmarXCore/core/ManagedIceObject.h>
-#include "../../util/Samplers.h"
-#include "../../util/Metrics.h"
-#include "../../util/PlanningUtil.h"
#include <RobotComponents/interface/components/MotionPlanning/Tasks/AdaptiveDynamicDomainInformedRRTStar/WorkerNode.h>
+#include "../../util/Metrics.h"
+#include "../../util/PlanningUtil.h"
+#include "../../util/Samplers.h"
#include "Tree.h"
namespace armarx
{
- template <class IceBaseClass, class DerivedClass> class GenericFactory;
+ template <class IceBaseClass, class DerivedClass>
+ class GenericFactory;
}
+
namespace armarx::addirrtstar
{
class WorkerNode;
@@ -70,9 +72,7 @@ namespace armarx::addirrtstar
*
* since μ(Xfree) can only be estimated the k-NN version is used.
*/
- class WorkerNode:
- virtual public ManagedIceObject,
- virtual public WorkerNodeBase
+ class WorkerNode : virtual public ManagedIceObject, virtual public WorkerNodeBase
{
public:
/**
@@ -80,7 +80,8 @@ namespace armarx::addirrtstar
*/
using ConfigType = Tree::ConfigType;
- static_assert(std::numeric_limits<Ice::Float>::has_infinity, "requires inf in current implementation");
+ static_assert(std::numeric_limits<Ice::Float>::has_infinity,
+ "requires inf in current implementation");
/**
* @brief ctor
@@ -96,29 +97,31 @@ namespace armarx::addirrtstar
* @param topicPrefix The used topic prefix. (for the topic name to distribute updates)
* @param rotationMatrix The rotation matrix for the informed ellipsoid.
*/
- WorkerNode(
- const CSpaceBasePtr& cspace,
- const VectorXf& startCfg,
- const VectorXf& goalCfg,
- Ice::Float DCDStepSize,
- AdaptiveDynamicDomainParameters addParams,
- const ManagerNodeBasePrx& manager,
- Ice::Long workerId,
- Ice::Long batchSize,
- Ice::Long nodeCountDeltaForGoalConnectionTries,
- const std::string& topicPrefix,
- const Ice::FloatSeq& rotationMatrix
-
- ):
- WorkerNodeBase(
- cspace,
- startCfg, goalCfg,
- DCDStepSize,
- addParams,
- manager, workerId, batchSize, nodeCountDeltaForGoalConnectionTries, topicPrefix,
- rotationMatrix
- ),
- onePastLastGoalConnect {0}
+ WorkerNode(const CSpaceBasePtr& cspace,
+ const VectorXf& startCfg,
+ const VectorXf& goalCfg,
+ Ice::Float DCDStepSize,
+ AdaptiveDynamicDomainParameters addParams,
+ const ManagerNodeBasePrx& manager,
+ Ice::Long workerId,
+ Ice::Long batchSize,
+ Ice::Long nodeCountDeltaForGoalConnectionTries,
+ const std::string& topicPrefix,
+ const Ice::FloatSeq& rotationMatrix
+
+ ) :
+ WorkerNodeBase(cspace,
+ startCfg,
+ goalCfg,
+ DCDStepSize,
+ addParams,
+ manager,
+ workerId,
+ batchSize,
+ nodeCountDeltaForGoalConnectionTries,
+ topicPrefix,
+ rotationMatrix),
+ onePastLastGoalConnect{0}
{
}
@@ -142,18 +145,25 @@ namespace armarx::addirrtstar
* @brief Gets a proxy to the topic.
*/
void onConnectComponent() override;
+
/**
* @brief noop
*/
- void onDisconnectComponent() override {}
+ void
+ onDisconnectComponent() override
+ {
+ }
+
/**
* @brief Kills the worker thread (if it is still running) and joins it.
*/
void onExitComponent() override;
+
/**
* @return The component's default name.
*/
- std::string getDefaultName() const override
+ std::string
+ getDefaultName() const override
{
return "ADDIRRTStarWorkerNode";
}
@@ -162,12 +172,14 @@ namespace armarx::addirrtstar
/**
* @brief Signals the worker thread to exit.
*/
- void killWorker(const Ice::Current& = Ice::emptyCurrent) override
+ void
+ killWorker(const Ice::Current& = Ice::emptyCurrent) override
{
killRequest = true;
}
- void pauseWorker(bool pauseFlag, const Ice::Current& = Ice::emptyCurrent) override
+ void
+ pauseWorker(bool pauseFlag, const Ice::Current& = Ice::emptyCurrent) override
{
workerPaused = pauseFlag;
}
@@ -176,7 +188,8 @@ namespace armarx::addirrtstar
* @param updateId The updates id.
* @return This workers update with the given id.
*/
- Update getUpdate(Ice::Long updateId, const Ice::Current& = Ice::emptyCurrent) const override;
+ Update getUpdate(Ice::Long updateId,
+ const Ice::Current& = Ice::emptyCurrent) const override;
//from WorkerUpdateInterface
@@ -184,13 +197,14 @@ namespace armarx::addirrtstar
* @brief Receives an update from other workers and adds it to the queue of pending updates.
* @param u the update.
*/
- void updateTree(const Update& u, const Ice::Current& = Ice::emptyCurrent)override;
+ void updateTree(const Update& u, const Ice::Current& = Ice::emptyCurrent) override;
//from ice
/**
* @brief noop
*/
- void ice_postUnmarshal()override
+ void
+ ice_postUnmarshal() override
{
}
@@ -200,7 +214,8 @@ namespace armarx::addirrtstar
* @param to Second vector.
* @return The euclidian distance between from and to.
*/
- static float distance(const ConfigType& from, const ConfigType& to)
+ static float
+ distance(const ConfigType& from, const ConfigType& to)
{
ARMARX_CHECK_EXPRESSION(from.size() == to.size());
return euclideanDistance(from.data(), from.data() + from.size(), to.data());
@@ -230,13 +245,10 @@ namespace armarx::addirrtstar
/**
* @brief initializes the tree by getting the current tree from the manager
*/
- void initTree()
+ void
+ initTree()
{
- tree.init(
- manager->getTree(),
- addParams,
- workerId
- );
+ tree.init(manager->getTree(), addParams, workerId);
}
/**
@@ -264,7 +276,8 @@ namespace armarx::addirrtstar
/**
* @return The cspace's dimensionality
*/
- std::size_t getDimensionality()
+ std::size_t
+ getDimensionality()
{
return cspace->getDimensionality();
}
@@ -347,8 +360,7 @@ namespace armarx::addirrtstar
NodeId& outNodeBestId,
float& outCostReachedToNodeBest,
std::vector<std::pair<NodeId, float>>& outKNearestIdsAndDistances,
- std::map<NodeId, bool>& outIsCollisionFreeCache
- );
+ std::map<NodeId, bool>& outIsCollisionFreeCache);
/**
* @brief The rewire operation of the rrt* algorithm.
@@ -357,11 +369,9 @@ namespace armarx::addirrtstar
* @param kNearestIdsAndDistances The k nearest nodes and their distance to the root node.
* @param isCollisionFreeCache Cache storing collision checks for paths between nodes from knearest and the root node..
*/
- void rewire(
- const NodeId& nodeId,
- const std::vector<std::pair<NodeId, float>>& kNearestIdsAndDistances,
- const std::map<NodeId, bool>& isCollisionFreeCache
- );
+ void rewire(const NodeId& nodeId,
+ const std::vector<std::pair<NodeId, float>>& kNearestIdsAndDistances,
+ const std::map<NodeId, bool>& isCollisionFreeCache);
/**
* @brief Adds a configuration to the tree.
@@ -392,5 +402,4 @@ namespace armarx::addirrtstar
*/
std::pair<const float*, const float*> bufferCDRange;
};
-}
-
+} // namespace armarx::addirrtstar
diff --git a/source/RobotComponents/components/MotionPlanning/Tasks/AdaptiveDynamicDomainInformedRRTStar/util.cpp b/source/RobotComponents/components/MotionPlanning/Tasks/AdaptiveDynamicDomainInformedRRTStar/util.cpp
index a26666d4152452866b10b60d7c8aa4881edd1671..cab4b0663254b0d38f7b5dad3c332dcd6178ccd7 100644
--- a/source/RobotComponents/components/MotionPlanning/Tasks/AdaptiveDynamicDomainInformedRRTStar/util.cpp
+++ b/source/RobotComponents/components/MotionPlanning/Tasks/AdaptiveDynamicDomainInformedRRTStar/util.cpp
@@ -26,10 +26,11 @@
namespace armarx::addirrtstar
{
- std::string nodeId2String(const NodeId& id)
+ std::string
+ nodeId2String(const NodeId& id)
{
std::stringstream s;
s << "[" << id.workerId << ";" << id.numberOfNode << "]";
return s.str();
}
-}
+} // namespace armarx::addirrtstar
diff --git a/source/RobotComponents/components/MotionPlanning/Tasks/AdaptiveDynamicDomainInformedRRTStar/util.h b/source/RobotComponents/components/MotionPlanning/Tasks/AdaptiveDynamicDomainInformedRRTStar/util.h
index a3107b98319ea14ee9ecc0b1f84e24da4d6913e1..e08a5f14fe47ce2eebf7e7da19593a0e9750fe2e 100644
--- a/source/RobotComponents/components/MotionPlanning/Tasks/AdaptiveDynamicDomainInformedRRTStar/util.h
+++ b/source/RobotComponents/components/MotionPlanning/Tasks/AdaptiveDynamicDomainInformedRRTStar/util.h
@@ -26,6 +26,7 @@
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <RobotComponents/interface/components/MotionPlanning/Tasks/AdaptiveDynamicDomainInformedRRTStar/DataStructures.h>
+
#include "../../util/HashingUtil.h"
ARMARX_OVERLOAD_STD_HASH((armarx::addirrtstar::NodeId), (arg.workerId, arg.numberOfNode))
@@ -39,8 +40,9 @@ namespace armarx::addirrtstar
* @param u The update.
* @return The update's worker id.
*/
- template<class T = std::size_t>
- T getUpdatesWorkerId(const Update& u)
+ template <class T = std::size_t>
+ T
+ getUpdatesWorkerId(const Update& u)
{
ARMARX_CHECK_EXPRESSION(u.workerId >= 0);
return static_cast<std::size_t>(u.workerId);
@@ -51,8 +53,9 @@ namespace armarx::addirrtstar
* @param u The update.
* @return The update's sub id's predecessor.
*/
- template<class T = std::size_t>
- T getUpdatesPreviousUpdateSubId(const Update& u)
+ template <class T = std::size_t>
+ T
+ getUpdatesPreviousUpdateSubId(const Update& u)
{
const auto workerId = getUpdatesWorkerId(u);
ARMARX_CHECK_EXPRESSION(workerId < u.dependetOnUpdateIds.size());
@@ -64,8 +67,9 @@ namespace armarx::addirrtstar
* @param u The update.
* @return The update's sub id.
*/
- template<class T = std::size_t>
- T getUpdatesSubId(const Update& u)
+ template <class T = std::size_t>
+ T
+ getUpdatesSubId(const Update& u)
{
return getUpdatesPreviousUpdateSubId(u) + 1;
}
@@ -76,9 +80,11 @@ namespace armarx::addirrtstar
* @param dependency The dependency to check.
* @return Whether the update update depends on dependency.
*/
- inline bool updateDependsOn(const Update& update, const Update& dependency)
+ inline bool
+ updateDependsOn(const Update& update, const Update& dependency)
{
- return update.dependetOnUpdateIds.at(getUpdatesWorkerId(dependency)) >= getUpdatesSubId<Ice::Long>(dependency);
+ return update.dependetOnUpdateIds.at(getUpdatesWorkerId(dependency)) >=
+ getUpdatesSubId<Ice::Long>(dependency);
}
/**
@@ -86,7 +92,8 @@ namespace armarx::addirrtstar
* @param dcdStepsize The dcd stepsize.
* @return The add parameters.
*/
- inline AdaptiveDynamicDomainParameters generateADDParamsFromDCDStepsize(float dcdStepsize)
+ inline AdaptiveDynamicDomainParameters
+ generateADDParamsFromDCDStepsize(float dcdStepsize)
{
AdaptiveDynamicDomainParameters result;
result.alpha = 0.05f;
@@ -101,5 +108,4 @@ namespace armarx::addirrtstar
* @return The node id as string.
*/
std::string nodeId2String(const NodeId& id);
-}
-
+} // namespace armarx::addirrtstar
diff --git a/source/RobotComponents/components/MotionPlanning/Tasks/BiRRT/Task.cpp b/source/RobotComponents/components/MotionPlanning/Tasks/BiRRT/Task.cpp
index 24c5819ccddc6204e54c5f6f08c9034de8e6c908..f8153954220e6337e33677e200146829e28668b7 100644
--- a/source/RobotComponents/components/MotionPlanning/Tasks/BiRRT/Task.cpp
+++ b/source/RobotComponents/components/MotionPlanning/Tasks/BiRRT/Task.cpp
@@ -23,21 +23,27 @@
*/
#include "Task.h"
+#include <SimoxUtility/algorithm/string/string_tools.h>
+
+#include <ArmarXCore/core/util/OnScopeExit.h>
+
+#include <RobotComponents/components/MotionPlanning/CSpace/ScaledCSpace.h>
+
#include "../../util/CollisionCheckUtil.h"
#include "../../util/Metrics.h"
-
-#include <MotionPlanning/Planner/BiRrt.h>
#include <MotionPlanning/CSpace/CSpacePath.h>
-#include <RobotComponents/components/MotionPlanning/CSpace/ScaledCSpace.h>
-#include <ArmarXCore/core/util/OnScopeExit.h>
+#include <MotionPlanning/Planner/BiRrt.h>
#include <MotionPlanning/PostProcessing/ShortcutProcessor.h>
-#include <SimoxUtility/algorithm/string/string_tools.h>
-
namespace armarx::birrt
{
- Task::Task(const CSpaceBasePtr& cspace, const VectorXf& startCfg, const VectorXf& goalCfg, const std::string& taskName, Ice::Long maximalPlanningTimeInSeconds, float dcdStep)
+ Task::Task(const CSpaceBasePtr& cspace,
+ const VectorXf& startCfg,
+ const VectorXf& goalCfg,
+ const std::string& taskName,
+ Ice::Long maximalPlanningTimeInSeconds,
+ float dcdStep)
{
this->cspace = cspace->clone();
this->startCfg = startCfg;
@@ -46,23 +52,28 @@ namespace armarx::birrt
this->maximalPlanningTimeInSeconds = maximalPlanningTimeInSeconds;
this->taskName = taskName;
- if (startCfg.size() != goalCfg.size() || startCfg.size() != static_cast<std::size_t>(this->cspace->getDimensionality()))
+ if (startCfg.size() != goalCfg.size() ||
+ startCfg.size() != static_cast<std::size_t>(this->cspace->getDimensionality()))
{
- throw std::invalid_argument {"start and goal have to be the size of the cspace's dimensionality"};
+ throw std::invalid_argument{
+ "start and goal have to be the size of the cspace's dimensionality"};
}
}
- void Task::abortTask(const::Ice::Current&)
+ void
+ Task::abortTask(const ::Ice::Current&)
{
}
- Path Task::getPath(const::Ice::Current&) const
+ Path
+ Task::getPath(const ::Ice::Current&) const
{
- std::lock_guard<std::mutex> lock {mtx};
+ std::lock_guard<std::mutex> lock{mtx};
return solution;
}
- void Task::run(const RemoteObjectNodePrxList&, const::Ice::Current&)
+ void
+ Task::run(const RemoteObjectNodePrxList&, const ::Ice::Current&)
{
auto failureOutput = [this]
@@ -90,7 +101,8 @@ namespace armarx::birrt
names.push_back(name);
}
- ss << "scene object set consisting of " << simox::alg::join(names, ", ") << " is in collision\n";
+ ss << "scene object set consisting of " << simox::alg::join(names, ", ")
+ << " is in collision\n";
}
}
return ss.str();
@@ -102,31 +114,41 @@ namespace armarx::birrt
//check trivial cases
cspace->initCollisionTest();
- const auto startIsCollisionFree = cspace->isCollisionFree(std::pair<const Ice::Float*, const Ice::Float*> {startCfg.data(), startCfg.data() + startCfg.size()});
+ const auto startIsCollisionFree =
+ cspace->isCollisionFree(std::pair<const Ice::Float*, const Ice::Float*>{
+ startCfg.data(), startCfg.data() + startCfg.size()});
if (!startIsCollisionFree)
{
- ARMARX_WARNING << "BiRRT failed trivially: start config is in collision: " << startCfg << " Collisions:\n" << failureOutput();
+ ARMARX_WARNING << "BiRRT failed trivially: start config is in collision: " << startCfg
+ << " Collisions:\n"
+ << failureOutput();
setTaskStatus(TaskStatus::ePlanningFailed);
return;
}
- const auto goalIscollisionFree = cspace->isCollisionFree(std::pair<const Ice::Float*, const Ice::Float*> { goalCfg.data(), goalCfg.data() + goalCfg.size()});
+ const auto goalIscollisionFree =
+ cspace->isCollisionFree(std::pair<const Ice::Float*, const Ice::Float*>{
+ goalCfg.data(), goalCfg.data() + goalCfg.size()});
if (!goalIscollisionFree)
{
- ARMARX_WARNING << "BiRRT failed trivially: goal config is in collision" << goalCfg << " Collisions:\n" << failureOutput();
+ ARMARX_WARNING << "BiRRT failed trivially: goal config is in collision" << goalCfg
+ << " Collisions:\n"
+ << failureOutput();
setTaskStatus(TaskStatus::ePlanningFailed);
return;
}
SimoxCSpacePtr origCSpace = getOriginalCSpace();
ARMARX_CHECK_EXPRESSION(origCSpace);
- ARMARX_INFO << "RobotConfig: " << origCSpace->getAgentSceneObj()->getConfig()->getRobotNodeJointValueMap();
+ ARMARX_INFO << "RobotConfig: "
+ << origCSpace->getAgentSceneObj()->getConfig()->getRobotNodeJointValueMap();
Saba::CSpaceSampledPtr sampledcSpace = getOriginalCSpace()->createSimoxCSpace();
ARMARX_CHECK_EXPRESSION(sampledcSpace);
IceUtil::Time start = IceUtil::Time::now();
ARMARX_ON_SCOPE_EXIT
{
- ARMARX_INFO << "BiRRT took : " << (IceUtil::Time::now() - start).toMilliSecondsDouble() << " ms";
+ ARMARX_INFO << "BiRRT took : " << (IceUtil::Time::now() - start).toMilliSecondsDouble()
+ << " ms";
};
ScaledCSpacePtr scaledCSpace = ScaledCSpacePtr::dynamicCast(cspace);
@@ -135,11 +157,14 @@ namespace armarx::birrt
ARMARX_VERBOSE << "unscaling configs " << VAROUT(startCfg) << VAROUT(goalCfg);
scaledCSpace->unscaleConfig(startCfg);
scaledCSpace->unscaleConfig(goalCfg);
- sampledcSpace->setMetricWeights(Eigen::Map<Eigen::VectorXf>(scaledCSpace->getScalingFactors().data(), scaledCSpace->getScalingFactors().size()));
+ sampledcSpace->setMetricWeights(
+ Eigen::Map<Eigen::VectorXf>(scaledCSpace->getScalingFactors().data(),
+ scaledCSpace->getScalingFactors().size()));
ARMARX_VERBOSE << "unscaled configs " << VAROUT(startCfg) << VAROUT(goalCfg);
}
- const auto distStartEnd = euclideanDistance(startCfg.begin(), startCfg.end(), goalCfg.begin());
+ const auto distStartEnd =
+ euclideanDistance(startCfg.begin(), startCfg.end(), goalCfg.begin());
if (distStartEnd <= dcdStep)
{
@@ -150,7 +175,6 @@ namespace armarx::birrt
}
-
Saba::Rrt::RrtMethod mode;
mode = Saba::Rrt::eExtend;
ARMARX_CHECK_EXPRESSION(cspace);
@@ -162,8 +186,7 @@ namespace armarx::birrt
// sampledcSpace->setSamplingSize(samplingSize);
sampledcSpace->setSamplingSizeDCD(dcdStep);
ARMARX_INFO << "SamplingSizeDCD: " << sampledcSpace->getSamplingSizeDCD();
- Saba::BiRrtPtr rrt(new Saba::BiRrt(sampledcSpace,
- mode/*, mode2, samplingSize*/));
+ Saba::BiRrtPtr rrt(new Saba::BiRrt(sampledcSpace, mode /*, mode2, samplingSize*/));
rrt->setStart(Eigen::Map<Eigen::ArrayXf>(startCfg.data(), startCfg.size()));
@@ -185,7 +208,7 @@ namespace armarx::birrt
TIMING_END(ShortCutter);
for (auto& vec : tmpSolution->getPoints())
{
- this->solution.nodes.push_back(VectorXf {vec.data(), vec.data() + vec.rows()});
+ this->solution.nodes.push_back(VectorXf{vec.data(), vec.data() + vec.rows()});
}
ARMARX_VERBOSE << VAROUT(solution.nodes);
if (scaledCSpace)
@@ -225,12 +248,14 @@ namespace armarx::birrt
}
}
- SimoxCSpacePtr Task::getOriginalCSpace() const
+ SimoxCSpacePtr
+ Task::getOriginalCSpace() const
{
SimoxCSpacePtr origCSpace;
if (cspace->ice_isA(CSpaceAdaptorBase::ice_staticId()))
{
- origCSpace = SimoxCSpacePtr::dynamicCast(CSpaceAdaptorBasePtr::dynamicCast(cspace)->getOriginalCSpace());
+ origCSpace = SimoxCSpacePtr::dynamicCast(
+ CSpaceAdaptorBasePtr::dynamicCast(cspace)->getOriginalCSpace());
// sampledcSpace = ScaledCSpacePtr::dynamicCast(cspace)->getOriginal()->createSimoxCSpace();
}
@@ -242,4 +267,4 @@ namespace armarx::birrt
}
-}
+} // namespace armarx::birrt
diff --git a/source/RobotComponents/components/MotionPlanning/Tasks/BiRRT/Task.h b/source/RobotComponents/components/MotionPlanning/Tasks/BiRRT/Task.h
index 4979e85af2053179ab4db65647709ca7e4528daa..602b7fe3cd21ce37ffe58ade3e11894f06668fdf 100644
--- a/source/RobotComponents/components/MotionPlanning/Tasks/BiRRT/Task.h
+++ b/source/RobotComponents/components/MotionPlanning/Tasks/BiRRT/Task.h
@@ -23,16 +23,15 @@
*/
#pragma once
-#include <RobotComponents/components/MotionPlanning/Tasks/MotionPlanningTask.h>
-
-#include <RobotComponents/interface/components/MotionPlanning/Tasks/BiRRT/Task.h>
#include <ArmarXCore/core/system/FactoryCollectionBase.h>
+
#include <RobotComponents/components/MotionPlanning/CSpace/SimoxCSpace.h>
+#include <RobotComponents/components/MotionPlanning/Tasks/MotionPlanningTask.h>
+#include <RobotComponents/interface/components/MotionPlanning/Tasks/BiRRT/Task.h>
namespace armarx::birrt
{
- class Task : public virtual MotionPlanningTaskWithDefaultMembers,
- public virtual TaskBase
+ class Task : public virtual MotionPlanningTaskWithDefaultMembers, public virtual TaskBase
{
public:
Task(const CSpaceBasePtr& cspace,
@@ -45,28 +44,28 @@ namespace armarx::birrt
// MotionPlanningTaskControlInterface interface
public:
- void abortTask(const::Ice::Current&) override;
- Path getPath(const::Ice::Current&) const override;
+ void abortTask(const ::Ice::Current&) override;
+ Path getPath(const ::Ice::Current&) const override;
// MotionPlanningTaskBase interface
public:
- void run(const RemoteObjectNodePrxList&, const::Ice::Current&) override;
+ void run(const RemoteObjectNodePrxList&, const ::Ice::Current&) override;
SimoxCSpacePtr getOriginalCSpace() const;
protected:
-
Task() = default;
mutable std::mutex mtx;
Path solution = {{}, "BiRRT-Path"};
- std::atomic_bool taskIsAborted {false};
- private:
- template<class Base, class Derived> friend class ::armarx::GenericFactory;
+ std::atomic_bool taskIsAborted{false};
+ private:
+ template <class Base, class Derived>
+ friend class ::armarx::GenericFactory;
};
-}
+} // namespace armarx::birrt
namespace armarx
{
@@ -74,4 +73,4 @@ namespace armarx
using BiRRTTaskPtr = IceUtil::Handle<BiRRTTask>;
using BiRRTTaskHandle = RemoteHandle<MotionPlanningTaskControlInterfacePrx>;
-}
+} // namespace armarx
diff --git a/source/RobotComponents/components/MotionPlanning/Tasks/CPRSAwareMotionPlanningTask.h b/source/RobotComponents/components/MotionPlanning/Tasks/CPRSAwareMotionPlanningTask.h
index 277b71a1dfc848ea0872c8aa0b5246fc62322691..ca3e14d25c5750f4e288a6ddd88de96064462e35 100644
--- a/source/RobotComponents/components/MotionPlanning/Tasks/CPRSAwareMotionPlanningTask.h
+++ b/source/RobotComponents/components/MotionPlanning/Tasks/CPRSAwareMotionPlanningTask.h
@@ -38,7 +38,7 @@ namespace armarx::cprs
/**
* @brief Implementation of the slice interface CPRSAwarePlanningTaskBase.
*/
- class CPRSAwareMotionPlanningTask:
+ class CPRSAwareMotionPlanningTask :
virtual public MotionPlanningTaskWithDefaultMembers,
virtual public CPRSAwareMotionPlanningTaskBase
{
@@ -58,13 +58,21 @@ namespace armarx::cprs
Ice::Float dcdStep,
Ice::Long maximalPlanningTimeInSeconds,
const cprs::ComputingPowerRequestStrategyBasePtr& planningComputingPowerRequestStrategy,
- const std::string& taskName
- ):
+ const std::string& taskName) :
MotionPlanningTaskBase(taskName),
- MotionPlanningTaskWithDefaultMembers(startCfg, goalCfg, cspace, dcdStep, maximalPlanningTimeInSeconds, taskName),
- CPRSAwareMotionPlanningTaskBase(
- taskName, startCfg, goalCfg, cspace, dcdStep, maximalPlanningTimeInSeconds,
- planningComputingPowerRequestStrategy)
+ MotionPlanningTaskWithDefaultMembers(startCfg,
+ goalCfg,
+ cspace,
+ dcdStep,
+ maximalPlanningTimeInSeconds,
+ taskName),
+ CPRSAwareMotionPlanningTaskBase(taskName,
+ startCfg,
+ goalCfg,
+ cspace,
+ dcdStep,
+ maximalPlanningTimeInSeconds,
+ planningComputingPowerRequestStrategy)
{
}
@@ -74,5 +82,4 @@ namespace armarx::cprs
*/
CPRSAwareMotionPlanningTask() = default;
};
-}
-
+} // namespace armarx::cprs
diff --git a/source/RobotComponents/components/MotionPlanning/Tasks/CSpaceVisualizerTask/CSpaceVisualizerTask.cpp b/source/RobotComponents/components/MotionPlanning/Tasks/CSpaceVisualizerTask/CSpaceVisualizerTask.cpp
index 07818a698cf68d9cab4d8980e03afbc5da876b7b..4df821269a939b8008f701869d9ced32516c6a56 100644
--- a/source/RobotComponents/components/MotionPlanning/Tasks/CSpaceVisualizerTask/CSpaceVisualizerTask.cpp
+++ b/source/RobotComponents/components/MotionPlanning/Tasks/CSpaceVisualizerTask/CSpaceVisualizerTask.cpp
@@ -28,27 +28,20 @@
using namespace armarx;
-
-CSpaceVisualizerTask::CSpaceVisualizerTask(const SimoxCSpaceWith2DPoseBasePtr& cspace, const VectorXf& robotPlatform2DPose, const std::string& taskName)
+CSpaceVisualizerTask::CSpaceVisualizerTask(const SimoxCSpaceWith2DPoseBasePtr& cspace,
+ const VectorXf& robotPlatform2DPose,
+ const std::string& taskName)
{
- ARMARX_CHECK_EXPRESSION(robotPlatform2DPose.size() == 3) << "The platform pose needs to contain x, y, alpha";
+ ARMARX_CHECK_EXPRESSION(robotPlatform2DPose.size() == 3)
+ << "The platform pose needs to contain x, y, alpha";
this->startCfg = robotPlatform2DPose;
this->goalCfg = VectorXf(3, 0);
this->cspace = cspace->clone();
SimoxCSpaceWith2DPosePtr space = SimoxCSpaceWith2DPosePtr::dynamicCast(this->cspace);
if (space)
{
- space->setPoseBounds(Vector3fRange {{
- startCfg.at(0) - 10000,
- startCfg.at(1) - 10000,
- -M_PI
- },
- {
- startCfg.at(0) + 10000,
- startCfg.at(1) + 10000,
- M_PI
- }
- });
+ space->setPoseBounds(Vector3fRange{{startCfg.at(0) - 10000, startCfg.at(1) - 10000, -M_PI},
+ {startCfg.at(0) + 10000, startCfg.at(1) + 10000, M_PI}});
}
else
{
@@ -56,10 +49,10 @@ CSpaceVisualizerTask::CSpaceVisualizerTask(const SimoxCSpaceWith2DPoseBasePtr& c
}
this->taskName = taskName;
-
}
-Path armarx::CSpaceVisualizerTask::getPath(const Ice::Current&) const
+Path
+armarx::CSpaceVisualizerTask::getPath(const Ice::Current&) const
{
Path p;
p.pathName = "RobotPose";
@@ -68,14 +61,15 @@ Path armarx::CSpaceVisualizerTask::getPath(const Ice::Current&) const
return p;
}
-void CSpaceVisualizerTask::run(const armarx::RemoteObjectNodePrxList&, const Ice::Current&)
+void
+CSpaceVisualizerTask::run(const armarx::RemoteObjectNodePrxList&, const Ice::Current&)
{
setTaskStatus(TaskStatus::ePlanning);
cspace->initCollisionTest();
setTaskStatus(TaskStatus::eDone);
}
-void CSpaceVisualizerTask::ice_postUnmarshal()
+void
+CSpaceVisualizerTask::ice_postUnmarshal()
{
-
}
diff --git a/source/RobotComponents/components/MotionPlanning/Tasks/CSpaceVisualizerTask/CSpaceVisualizerTask.h b/source/RobotComponents/components/MotionPlanning/Tasks/CSpaceVisualizerTask/CSpaceVisualizerTask.h
index 6c647ce18a204f89d91f6effe4a26ed31de8efdc..cae98520385312f78ffca87d00e615e826091548 100644
--- a/source/RobotComponents/components/MotionPlanning/Tasks/CSpaceVisualizerTask/CSpaceVisualizerTask.h
+++ b/source/RobotComponents/components/MotionPlanning/Tasks/CSpaceVisualizerTask/CSpaceVisualizerTask.h
@@ -23,10 +23,11 @@
*/
#pragma once
-#include <RobotComponents/components/MotionPlanning/Tasks/MotionPlanningTask.h>
-#include <RobotComponents/interface/components/MotionPlanning/Tasks/CSpaceVisualizerTask.h>
#include <ArmarXCore/core/system/FactoryCollectionBase.h>
+
#include <RobotComponents/components/MotionPlanning/CSpace/SimoxCSpace.h>
+#include <RobotComponents/components/MotionPlanning/Tasks/MotionPlanningTask.h>
+#include <RobotComponents/interface/components/MotionPlanning/Tasks/CSpaceVisualizerTask.h>
namespace armarx
{
@@ -39,20 +40,20 @@ namespace armarx
public virtual CSpaceVisualizerTaskBase
{
public:
- CSpaceVisualizerTask(//problem
+ CSpaceVisualizerTask( //problem
const SimoxCSpaceWith2DPoseBasePtr& cspace,
const VectorXf& robotPlatform2DPose,
- const std::string& taskName = "CSpaceVisualizerTask"
- );
+ const std::string& taskName = "CSpaceVisualizerTask");
//PlanningControlInterface
/**
* @brief Does not do anything for this task.
*/
- void abortTask(const Ice::Current& = Ice::emptyCurrent) override
+ void
+ abortTask(const Ice::Current& = Ice::emptyCurrent) override
{
-
}
+
/**
* @return Contains the robot pose from the constructor..
*/
@@ -64,19 +65,23 @@ namespace armarx
* @brief Does not do anything for this task.
*/
void run(const RemoteObjectNodePrxList&, const Ice::Current& = Ice::emptyCurrent) override;
+
protected:
/**
* @brief Ctor used by object factories.
*/
CSpaceVisualizerTask() = default;
+
private:
- template<class Base, class Derived> friend class ::armarx::GenericFactory;
+ template <class Base, class Derived>
+ friend class ::armarx::GenericFactory;
// Object interface
public:
void ice_postUnmarshal() override;
};
+
using CSpaceVisualizerTaskPtr = IceUtil::Handle<CSpaceVisualizerTask>;
using CSpaceVisualizerTaskHandle = RemoteHandle<MotionPlanningTaskControlInterfacePrx>;
-}
+} // namespace armarx
diff --git a/source/RobotComponents/components/MotionPlanning/Tasks/MotionPlanningTask.cpp b/source/RobotComponents/components/MotionPlanning/Tasks/MotionPlanningTask.cpp
index 3b3c8bba6b3c175956ece6c5c29ff02c15dd77a8..0cb7049cbf82196fc7690157e81ff0834f1926fa 100644
--- a/source/RobotComponents/components/MotionPlanning/Tasks/MotionPlanningTask.cpp
+++ b/source/RobotComponents/components/MotionPlanning/Tasks/MotionPlanningTask.cpp
@@ -23,18 +23,20 @@
*/
#include "MotionPlanningTask.h"
+
#include "../util/PlanningUtil.h"
namespace armarx
{
- bool MotionPlanningTask::setTaskStatus(TaskStatus::Status newTaskStatus, const Ice::Current&)
+ bool
+ MotionPlanningTask::setTaskStatus(TaskStatus::Status newTaskStatus, const Ice::Current&)
{
//loop for compare and swap
//since states form a DAG and staying on the same state returns from this function,
//this loop will terminate
while (true)
{
- auto currentTaskStatus = taskStatus.load();
+ auto currentTaskStatus = taskStatus.load();
bool invalidTransition = false;
switch (newTaskStatus)
@@ -63,11 +65,9 @@ namespace armarx
}
break;
case TaskStatus::ePlanningAborted:
- if (
- (currentTaskStatus != TaskStatus::eNew) &&
+ if ((currentTaskStatus != TaskStatus::eNew) &&
(currentTaskStatus != TaskStatus::eQueued) &&
- (currentTaskStatus != TaskStatus::ePlanning)
- )
+ (currentTaskStatus != TaskStatus::ePlanning))
{
invalidTransition = true;
}
@@ -85,10 +85,8 @@ namespace armarx
}
break;
case TaskStatus::eDone:
- if (
- (currentTaskStatus != TaskStatus::eRefining) &&
- (currentTaskStatus != TaskStatus::ePlanning)
- )
+ if ((currentTaskStatus != TaskStatus::eRefining) &&
+ (currentTaskStatus != TaskStatus::ePlanning))
{
invalidTransition = true;
}
@@ -103,8 +101,7 @@ namespace armarx
if (currentTaskStatus != newTaskStatus)
{
ARMARX_WARNING_S << "Tried invalid transition from "
- << TaskStatus::toString(currentTaskStatus)
- << " to "
+ << TaskStatus::toString(currentTaskStatus) << " to "
<< TaskStatus::toString(newTaskStatus);
}
return false;
@@ -132,4 +129,4 @@ namespace armarx
}
}
}
-}
+} // namespace armarx
diff --git a/source/RobotComponents/components/MotionPlanning/Tasks/MotionPlanningTask.h b/source/RobotComponents/components/MotionPlanning/Tasks/MotionPlanningTask.h
index 68fbe468219354b265dfb408bf4c00874752cb8b..9e8c4576541e8b3d79d2e7c7adcf701327d8ea66 100644
--- a/source/RobotComponents/components/MotionPlanning/Tasks/MotionPlanningTask.h
+++ b/source/RobotComponents/components/MotionPlanning/Tasks/MotionPlanningTask.h
@@ -29,15 +29,16 @@
#include <Ice/ObjectAdapter.h>
-#include <RobotComponents/interface/components/MotionPlanning/Tasks/MotionPlanningTask.h>
-#include "../MotionPlanningServer.h"
-#include "MotionPlanningTaskControlInterface.h"
-
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/ManagedIceObject.h>
-#include <ArmarXCore/core/util/distributed/RemoteHandle/RemoteHandle.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/util/distributed/AMDCallbackCollection.h>
+#include <ArmarXCore/core/util/distributed/RemoteHandle/RemoteHandle.h>
+
+#include <RobotComponents/interface/components/MotionPlanning/Tasks/MotionPlanningTask.h>
+
+#include "../MotionPlanningServer.h"
+#include "MotionPlanningTaskControlInterface.h"
namespace armarx
{
@@ -46,14 +47,17 @@ namespace armarx
using MotionPlanningTaskHandle = RemoteHandle<MotionPlanningTaskControlInterfacePrx>;
class MotionPlanningTaskWithDefaultMembers;
- using MotionPlanningTaskWithDefaultMembersPtr = IceInternal::Handle<MotionPlanningTaskWithDefaultMembers>;
+ using MotionPlanningTaskWithDefaultMembersPtr =
+ IceInternal::Handle<MotionPlanningTaskWithDefaultMembers>;
using MotionPlanningTaskHandle = RemoteHandle<MotionPlanningTaskControlInterfacePrx>;
class PostprocessingMotionPlanningTask;
- using PostprocessingMotionPlanningTaskPtr = IceInternal::Handle<PostprocessingMotionPlanningTask>;
- using PostprocessingMotionPlanningTaskHandle = RemoteHandle<MotionPlanningTaskControlInterfacePrx>;
+ using PostprocessingMotionPlanningTaskPtr =
+ IceInternal::Handle<PostprocessingMotionPlanningTask>;
+ using PostprocessingMotionPlanningTaskHandle =
+ RemoteHandle<MotionPlanningTaskControlInterfacePrx>;
- class MotionPlanningTask:
+ class MotionPlanningTask :
virtual public MotionPlanningTaskBase,
virtual public MotionPlanningTaskCI
{
@@ -63,58 +67,80 @@ namespace armarx
MotionPlanningTask() = default;
~MotionPlanningTask() override = default;
+
/**
* @brief Called by the planning server after the task was enqueued.
* Override this function to prepare some work (e.g. setting the status from eNew to eQueued)
*/
- virtual void postEnqueueing()
+ virtual void
+ postEnqueueing()
{
setTaskStatus(TaskStatus::eQueued);
}
- virtual void onPlanningDone() {}
- virtual void onRefiningDone() {}
+ virtual void
+ onPlanningDone()
+ {
+ }
- MotionPlanningTaskBasePrx& getProxy()
+ virtual void
+ onRefiningDone()
+ {
+ }
+
+ MotionPlanningTaskBasePrx&
+ getProxy()
{
return selfProxy;
}
- bool setTaskStatus(TaskStatus::Status newTaskStatus, const Ice::Current& = Ice::emptyCurrent) override;
+ bool setTaskStatus(TaskStatus::Status newTaskStatus,
+ const Ice::Current& = Ice::emptyCurrent) override;
- TaskStatus::Status getTaskStatus(const Ice::Current& = Ice::emptyCurrent) const override
+ TaskStatus::Status
+ getTaskStatus(const Ice::Current& = Ice::emptyCurrent) const override
{
return taskStatus;
}
- virtual void registerAtIceAdapter(Ice::ObjectAdapterPtr& adapter, const Ice::Identity ident)
+ virtual void
+ registerAtIceAdapter(Ice::ObjectAdapterPtr& adapter, const Ice::Identity ident)
{
if (selfProxy)
{
- throw std::logic_error {"Task already registered as: category: " + selfProxy->ice_getIdentity().category + ", name: " + selfProxy->ice_getIdentity().name};
+ throw std::logic_error{"Task already registered as: category: " +
+ selfProxy->ice_getIdentity().category +
+ ", name: " + selfProxy->ice_getIdentity().name};
}
selfProxy = MotionPlanningTaskBasePrx::uncheckedCast(adapter->add(this, ident));
}
- std::string getTaskName(const Ice::Current& = Ice::emptyCurrent) const override
+ std::string
+ getTaskName(const Ice::Current& = Ice::emptyCurrent) const override
{
return taskName;
}
- void addTaskStatusCallback(std::function<void(TaskStatus::Status)> cb)
+ void
+ addTaskStatusCallback(std::function<void(TaskStatus::Status)> cb)
{
taskStatusCallbacks.emplace_back(cb);
}
- Ice::Long getPlanningTime(const Ice::Current&) const override
+ Ice::Long
+ getPlanningTime(const Ice::Current&) const override
{
return planningTime;
}
- Ice::Long getRefiningTime(const Ice::Current&) const override
+
+ Ice::Long
+ getRefiningTime(const Ice::Current&) const override
{
return refiningTime;
}
- Ice::Long getRunningTime(const Ice::Current&) const override
+
+ Ice::Long
+ getRunningTime(const Ice::Current&) const override
{
return planningTime + refiningTime;
}
@@ -134,12 +160,11 @@ namespace armarx
* @brief A self proxy. (may be required for callbacks)
*/
MotionPlanningTaskBasePrx selfProxy;
- std::atomic<TaskStatus::Status> taskStatus {TaskStatus::eNew};
+ std::atomic<TaskStatus::Status> taskStatus{TaskStatus::eNew};
std::deque<std::function<void(TaskStatus::Status)>> taskStatusCallbacks;
};
-
- class MotionPlanningTaskWithDefaultMembers:
+ class MotionPlanningTaskWithDefaultMembers :
virtual public MotionPlanningTaskWithDefaultMembersBase,
virtual public MotionPlanningTask
{
@@ -151,24 +176,27 @@ namespace armarx
* @param dcdStep the dcd step size
* @param maximalPlanningTimeInSeconds the maximal time in seconds
*/
- MotionPlanningTaskWithDefaultMembers(
- const VectorXf& startCfg,
- const VectorXf& goalCfg,
- const CSpaceBasePtr& cspace,
- Ice::Float dcdStep,
- Ice::Long maximalPlanningTimeInSeconds,
- const std::string& taskName
- ):
+ MotionPlanningTaskWithDefaultMembers(const VectorXf& startCfg,
+ const VectorXf& goalCfg,
+ const CSpaceBasePtr& cspace,
+ Ice::Float dcdStep,
+ Ice::Long maximalPlanningTimeInSeconds,
+ const std::string& taskName) :
MotionPlanningTaskBase(taskName),
- MotionPlanningTaskWithDefaultMembersBase(taskName, startCfg, goalCfg, cspace, dcdStep, maximalPlanningTimeInSeconds)
+ MotionPlanningTaskWithDefaultMembersBase(taskName,
+ startCfg,
+ goalCfg,
+ cspace,
+ dcdStep,
+ maximalPlanningTimeInSeconds)
{
}
-
/**
* @return The task's start configuration.
*/
- VectorXf getStart(const Ice::Current& = Ice::emptyCurrent) const override
+ VectorXf
+ getStart(const Ice::Current& = Ice::emptyCurrent) const override
{
return startCfg;
}
@@ -176,7 +204,8 @@ namespace armarx
/**
* @return The task's start configuration.
*/
- VectorXf getGoal(const Ice::Current& = Ice::emptyCurrent) const override
+ VectorXf
+ getGoal(const Ice::Current& = Ice::emptyCurrent) const override
{
return goalCfg;
}
@@ -184,7 +213,8 @@ namespace armarx
/**
* @return The task's CSpace .
*/
- CSpaceBasePtr getCSpace(const Ice::Current& = Ice::emptyCurrent) const override
+ CSpaceBasePtr
+ getCSpace(const Ice::Current& = Ice::emptyCurrent) const override
{
return cspace;
}
@@ -192,7 +222,8 @@ namespace armarx
/**
* @return The task's .
*/
- float getDcdStep(const Ice::Current& = Ice::emptyCurrent) const override
+ float
+ getDcdStep(const Ice::Current& = Ice::emptyCurrent) const override
{
return dcdStep;
}
@@ -200,7 +231,8 @@ namespace armarx
/**
* @return The task's .
*/
- Ice::Long getMaximalPlanningTimeInSeconds(const Ice::Current& = Ice::emptyCurrent) const override
+ Ice::Long
+ getMaximalPlanningTimeInSeconds(const Ice::Current& = Ice::emptyCurrent) const override
{
return maximalPlanningTimeInSeconds;
}
@@ -209,25 +241,28 @@ namespace armarx
MotionPlanningTaskWithDefaultMembers() = default;
};
- class PostprocessingMotionPlanningTask:
+ class PostprocessingMotionPlanningTask :
virtual public MotionPlanningTask,
virtual public PostprocessingMotionPlanningTaskBase
{
public:
- PostprocessingMotionPlanningTask(const MotionPlanningTaskBasePtr& previousStep, const std::string& taskName):
+ PostprocessingMotionPlanningTask(const MotionPlanningTaskBasePtr& previousStep,
+ const std::string& taskName) :
MotionPlanningTaskBase(taskName),
PostprocessingMotionPlanningTaskBase(taskName, previousStep)
{
ARMARX_CHECK_EXPRESSION(this->previousStep);
}
- CSpaceBasePtr getCSpace(const Ice::Current& = Ice::emptyCurrent) const override
+ CSpaceBasePtr
+ getCSpace(const Ice::Current& = Ice::emptyCurrent) const override
{
ARMARX_CHECK_EXPRESSION(previousStep->getCSpace());
return previousStep->getCSpace();
}
- void registerAtIceAdapter(Ice::ObjectAdapterPtr& adapter, const Ice::Identity ident) override
+ void
+ registerAtIceAdapter(Ice::ObjectAdapterPtr& adapter, const Ice::Identity ident) override
{
MotionPlanningTask::registerAtIceAdapter(adapter, ident);
@@ -235,11 +270,13 @@ namespace armarx
ARMARX_CHECK_EXPRESSION(prev);
Ice::Identity subIdent;
- subIdent.name = ManagedIceObject::generateSubObjectName(ident.category + ident.name, "PreviousStep_" + previousStep->ice_id());
+ subIdent.name = ManagedIceObject::generateSubObjectName(
+ ident.category + ident.name, "PreviousStep_" + previousStep->ice_id());
prev->registerAtIceAdapter(adapter, subIdent);
}
- void postEnqueueing() override
+ void
+ postEnqueueing() override
{
MotionPlanningTask::postEnqueueing();
MotionPlanningTaskPtr::dynamicCast(previousStep)->postEnqueueing();
@@ -248,4 +285,4 @@ namespace armarx
protected:
PostprocessingMotionPlanningTask() = default;
};
-}
+} // namespace armarx
diff --git a/source/RobotComponents/components/MotionPlanning/Tasks/MotionPlanningTaskControlInterface.cpp b/source/RobotComponents/components/MotionPlanning/Tasks/MotionPlanningTaskControlInterface.cpp
index 036e008d17e76562f8b3ec7c142859fb1ce919f9..66727f125b18ea9cfa365fb943ba834ed2e1759f 100644
--- a/source/RobotComponents/components/MotionPlanning/Tasks/MotionPlanningTaskControlInterface.cpp
+++ b/source/RobotComponents/components/MotionPlanning/Tasks/MotionPlanningTaskControlInterface.cpp
@@ -23,30 +23,36 @@
*/
#include "MotionPlanningTaskControlInterface.h"
+
#include "../util/PlanningUtil.h"
namespace armarx
{
- PathSeq MotionPlanningMultiPathWithCostTaskCI::getAllPaths(const Ice::Current&) const
+ PathSeq
+ MotionPlanningMultiPathWithCostTaskCI::getAllPaths(const Ice::Current&) const
{
PathSeq result;
auto paths = getAllPathsWithCost();
result.reserve(paths.size());
- std::transform(paths.begin(), paths.end(), std::back_inserter(result), [](PathWithCost & p)
- {
- return Path {std::move(p.nodes), std::move(p.pathName)};
- });
+ std::transform(paths.begin(),
+ paths.end(),
+ std::back_inserter(result),
+ [](PathWithCost& p) {
+ return Path{std::move(p.nodes), std::move(p.pathName)};
+ });
return result;
}
- bool MotionPlanningTaskCI::isRunning(const Ice::Current&) const
+ bool
+ MotionPlanningTaskCI::isRunning(const Ice::Current&) const
{
return TaskStatus::isRunning(getTaskStatus());
}
- bool MotionPlanningTaskCI::finishedRunning(const Ice::Current&) const
+ bool
+ MotionPlanningTaskCI::finishedRunning(const Ice::Current&) const
{
return TaskStatus::finishedRunning(getTaskStatus());
}
-}
+} // namespace armarx
diff --git a/source/RobotComponents/components/MotionPlanning/Tasks/MotionPlanningTaskControlInterface.h b/source/RobotComponents/components/MotionPlanning/Tasks/MotionPlanningTaskControlInterface.h
index b678a0568382f36b163506a077d0763e42d1a09b..525c2f8271b83f5a8ddfeb8572c68c8a6d873a69 100644
--- a/source/RobotComponents/components/MotionPlanning/Tasks/MotionPlanningTaskControlInterface.h
+++ b/source/RobotComponents/components/MotionPlanning/Tasks/MotionPlanningTaskControlInterface.h
@@ -23,13 +23,13 @@
*/
#pragma once
-#include <RobotComponents/interface/components/MotionPlanning/Tasks/MotionPlanningTask.h>
#include <ArmarXCore/core/util/distributed/AMDCallbackCollection.h>
+#include <RobotComponents/interface/components/MotionPlanning/Tasks/MotionPlanningTask.h>
+
namespace armarx
{
- class MotionPlanningTaskCI:
- virtual public MotionPlanningTaskControlInterface
+ class MotionPlanningTaskCI : virtual public MotionPlanningTaskControlInterface
{
public:
// virtual void abortTask(const Ice::Current& = Ice::emptyCurrent) = 0;
@@ -37,14 +37,22 @@ namespace armarx
// virtual TaskStatus::Status getTaskStatus(const Ice::Current& = Ice::emptyCurrent) const = 0;
- void waitForFinishedPlanning_async(const AMD_MotionPlanningTaskControlInterface_waitForFinishedPlanningPtr& cb, const Ice::Current& = Ice::emptyCurrent) override
+ void
+ waitForFinishedPlanning_async(
+ const AMD_MotionPlanningTaskControlInterface_waitForFinishedPlanningPtr& cb,
+ const Ice::Current& = Ice::emptyCurrent) override
{
waitForFinishedPlanning.addCallback(cb);
}
- void waitForFinishedRunning_async(const AMD_MotionPlanningTaskControlInterface_waitForFinishedRunningPtr& cb, const Ice::Current& = Ice::emptyCurrent) override
+
+ void
+ waitForFinishedRunning_async(
+ const AMD_MotionPlanningTaskControlInterface_waitForFinishedRunningPtr& cb,
+ const Ice::Current& = Ice::emptyCurrent) override
{
waitForFinishedRunning.addCallback(cb);
}
+
/**
* @brief Returns whether the task is currently planning.
* @return Whether the task is currently planning.
@@ -56,37 +64,39 @@ namespace armarx
* @return Whether the task has finished planning.
*/
bool finishedRunning(const Ice::Current& = Ice::emptyCurrent) const override;
+
protected:
AMDCallbackCollection<> waitForFinishedPlanning;
AMDCallbackCollection<> waitForFinishedRunning;
};
- class MotionPlanningMultiPathTaskCI:
- virtual public MotionPlanningMultiPathTaskControlInterface
+ class MotionPlanningMultiPathTaskCI : virtual public MotionPlanningMultiPathTaskControlInterface
{
public:
// virtual Ice::Long getPathCount(const Ice::Current& = Ice::emptyCurrent) const = 0;
// virtual Path getNthPath(Ice::Long n, const Ice::Current& = Ice::emptyCurrent) const = 0;
// virtual PathSeq getAllPaths(const Ice::Current& = Ice::emptyCurrent) const = 0;
- Path getPath(const Ice::Current& = Ice::emptyCurrent) const override
+ Path
+ getPath(const Ice::Current& = Ice::emptyCurrent) const override
{
return getNthPath(0);
}
};
- class MotionPlanningWithCostTaskCI:
- virtual public MotionPlanningWithCostTaskControlInterface
+ class MotionPlanningWithCostTaskCI : virtual public MotionPlanningWithCostTaskControlInterface
{
public:
PathWithCost getPathWithCost(const Ice::Current& = Ice::emptyCurrent) const override = 0;
- Path getPath(const Ice::Current& = Ice::emptyCurrent) const override
+
+ Path
+ getPath(const Ice::Current& = Ice::emptyCurrent) const override
{
auto p = getPathWithCost();
return {std::move(p.nodes), std::move(p.pathName)};
}
};
- class MotionPlanningMultiPathWithCostTaskCI:
+ class MotionPlanningMultiPathWithCostTaskCI :
virtual public MotionPlanningMultiPathWithCostTaskControlInterface,
virtual public MotionPlanningMultiPathTaskCI,
virtual public MotionPlanningWithCostTaskCI
@@ -98,21 +108,25 @@ namespace armarx
// virtual PathWithCostSeq getAllPathsWithCost(const Ice::Current& = Ice::emptyCurrent) const = 0;
//default impl
- Path getPath(const Ice::Current& = Ice::emptyCurrent) const override
+ Path
+ getPath(const Ice::Current& = Ice::emptyCurrent) const override
{
return MotionPlanningWithCostTaskCI::getPath();
}
- PathWithCost getPathWithCost(const Ice::Current& = Ice::emptyCurrent) const override
+ PathWithCost
+ getPathWithCost(const Ice::Current& = Ice::emptyCurrent) const override
{
return getNthPathWithCost(0);
}
- Path getNthPath(Ice::Long n, const Ice::Current& = Ice::emptyCurrent) const override
+
+ Path
+ getNthPath(Ice::Long n, const Ice::Current& = Ice::emptyCurrent) const override
{
auto p = getNthPathWithCost(n);
return {std::move(p.nodes), std::move(p.pathName)};
}
- PathSeq getAllPaths(const Ice::Current& = Ice::emptyCurrent) const override;
+ PathSeq getAllPaths(const Ice::Current& = Ice::emptyCurrent) const override;
};
-}
+} // namespace armarx
diff --git a/source/RobotComponents/components/MotionPlanning/Tasks/PathCollection/Task.cpp b/source/RobotComponents/components/MotionPlanning/Tasks/PathCollection/Task.cpp
index a477e9cae0cfa5a327c45c4f7779e857be04260f..2f9c92ab0a9f402232458af3bf37f555cfd7227d 100644
--- a/source/RobotComponents/components/MotionPlanning/Tasks/PathCollection/Task.cpp
+++ b/source/RobotComponents/components/MotionPlanning/Tasks/PathCollection/Task.cpp
@@ -26,11 +26,11 @@
namespace armarx::pathcol
{
- void Task::run(const RemoteObjectNodePrxList&, const Ice::Current&)
+ void
+ Task::run(const RemoteObjectNodePrxList&, const Ice::Current&)
{
setTaskStatus(TaskStatus::ePlanning);
setTaskStatus(TaskStatus::eRefining);
setTaskStatus(TaskStatus::eDone);
}
-}
-
+} // namespace armarx::pathcol
diff --git a/source/RobotComponents/components/MotionPlanning/Tasks/PathCollection/Task.h b/source/RobotComponents/components/MotionPlanning/Tasks/PathCollection/Task.h
index ddff5d2730469c8a98de994bfa77563fb0206014..3c7dbf5b84257fae49cf38849c0c7ad872dfc0b9 100644
--- a/source/RobotComponents/components/MotionPlanning/Tasks/PathCollection/Task.h
+++ b/source/RobotComponents/components/MotionPlanning/Tasks/PathCollection/Task.h
@@ -23,24 +23,24 @@
*/
#pragma once
-#include <mutex>
+#include <atomic>
#include <chrono>
-#include<atomic>
+#include <mutex>
#include <ArmarXCore/core/util/distributed/RemoteHandle/RemoteHandle.h>
#include <ArmarXCore/interface/core/RemoteObjectNode.h>
#include <RobotComponents/interface/components/MotionPlanning/Tasks/PathCollection/Task.h>
-#include "../MotionPlanningTask.h"
#include "../../util/Metrics.h"
+#include "../MotionPlanningTask.h"
namespace armarx::pathcol
{
class Task;
using TaskPtr = IceInternal::Handle<Task>;
- class Task:
+ class Task :
public virtual MotionPlanningTask,
public virtual TaskBase,
public virtual MotionPlanningMultiPathWithCostTaskCI
@@ -53,25 +53,31 @@ namespace armarx::pathcol
~Task() override = default;
-
- PathWithCost getPathWithCost(const Ice::Current& = Ice::emptyCurrent) const override
+ PathWithCost
+ getPathWithCost(const Ice::Current& = Ice::emptyCurrent) const override
{
return MotionPlanningMultiPathWithCostTaskCI::getPathWithCost();
}
- Path getNthPath(Ice::Long n, const Ice::Current& = Ice::emptyCurrent) const override
+
+ Path
+ getNthPath(Ice::Long n, const Ice::Current& = Ice::emptyCurrent) const override
{
return MotionPlanningMultiPathWithCostTaskCI::getNthPath(n);
}
- Path getPath(const Ice::Current& = Ice::emptyCurrent) const override
+
+ Path
+ getPath(const Ice::Current& = Ice::emptyCurrent) const override
{
return MotionPlanningWithCostTaskCI::getPath();
}
- void abortTask(const Ice::Current& = Ice::emptyCurrent) override
+ void
+ abortTask(const Ice::Current& = Ice::emptyCurrent) override
{
}
- CSpaceBasePtr getCSpace(const Ice::Current& = Ice::emptyCurrent) const override
+ CSpaceBasePtr
+ getCSpace(const Ice::Current& = Ice::emptyCurrent) const override
{
return cspace;
}
@@ -82,20 +88,26 @@ namespace armarx::pathcol
*/
void run(const RemoteObjectNodePrxList&, const Ice::Current& = Ice::emptyCurrent) override;
- Ice::Long getMaximalPlanningTimeInSeconds(const Ice::Current& = Ice::emptyCurrent) const override
+ Ice::Long
+ getMaximalPlanningTimeInSeconds(const Ice::Current& = Ice::emptyCurrent) const override
{
return maximalPlanningTimeInSeconds;
}
- Ice::Long getPathCount(const Ice::Current& = Ice::emptyCurrent) const override
+ Ice::Long
+ getPathCount(const Ice::Current& = Ice::emptyCurrent) const override
{
return paths.size();
}
- PathWithCost getBestPath(const Ice::Current& = Ice::emptyCurrent) const override
+
+ PathWithCost
+ getBestPath(const Ice::Current& = Ice::emptyCurrent) const override
{
return getNthPathWithCost(0);
}
- PathWithCost getNthPathWithCost(Ice::Long n, const Ice::Current& = Ice::emptyCurrent) const override
+
+ PathWithCost
+ getNthPathWithCost(Ice::Long n, const Ice::Current& = Ice::emptyCurrent) const override
{
if (static_cast<std::size_t>(n) < paths.size())
{
@@ -103,29 +115,37 @@ namespace armarx::pathcol
}
return {{}, std::numeric_limits<float>::infinity(), "Path_" + to_string(n)};
}
- PathWithCostSeq getAllPathsWithCost(const Ice::Current& = Ice::emptyCurrent) const override
+
+ PathWithCostSeq
+ getAllPathsWithCost(const Ice::Current& = Ice::emptyCurrent) const override
{
return paths;
}
- void addPath(Path p)
+ void
+ addPath(Path p)
{
float len = 0;
for (std::size_t i = 0; i + 1 < p.nodes.size(); ++i)
{
- len += euclideanDistance(p.nodes.at(i).begin(), p.nodes.at(i).end(), p.nodes.at(i + 1).begin());
+ len += euclideanDistance(
+ p.nodes.at(i).begin(), p.nodes.at(i).end(), p.nodes.at(i + 1).begin());
}
- addPath(PathWithCost {std::move(p.nodes), len, std::move(p.pathName)});
+ addPath(PathWithCost{std::move(p.nodes), len, std::move(p.pathName)});
}
- void addPath(PathWithCost p)
+
+ void
+ addPath(PathWithCost p)
{
paths.emplace_back(std::move(p));
}
};
-}
+} // namespace armarx::pathcol
+
namespace armarx
{
using PathCollection = pathcol::Task;
using PathCollectionPtr = IceUtil::Handle<PathCollection>;
- using PathCollectionHandle = RemoteHandle<MotionPlanningMultiPathWithCostTaskControlInterfacePrx>;
-}
+ using PathCollectionHandle =
+ RemoteHandle<MotionPlanningMultiPathWithCostTaskControlInterfacePrx>;
+} // namespace armarx
diff --git a/source/RobotComponents/components/MotionPlanning/Tasks/RRTConnect/Task.cpp b/source/RobotComponents/components/MotionPlanning/Tasks/RRTConnect/Task.cpp
index 4dd0eb762095acc114a62e2ec550af7be2c5e208..08ace11b44ffa60afaf99931515343528e2e091f 100644
--- a/source/RobotComponents/components/MotionPlanning/Tasks/RRTConnect/Task.cpp
+++ b/source/RobotComponents/components/MotionPlanning/Tasks/RRTConnect/Task.cpp
@@ -28,7 +28,13 @@
namespace armarx::rrtconnect
{
- Task::Task(const CSpaceBasePtr& cspace, const VectorXf& startCfg, const VectorXf& goalCfg, const std::string& taskName, Ice::Long maximalPlanningTimeInSeconds, float dcdStep, Ice::Long workerCount)
+ Task::Task(const CSpaceBasePtr& cspace,
+ const VectorXf& startCfg,
+ const VectorXf& goalCfg,
+ const std::string& taskName,
+ Ice::Long maximalPlanningTimeInSeconds,
+ float dcdStep,
+ Ice::Long workerCount)
{
this->cspace = cspace;
this->startCfg = startCfg;
@@ -39,53 +45,66 @@ namespace armarx::rrtconnect
this->taskName = taskName;
}
- void Task::abortTask(const Ice::Current&)
+ void
+ Task::abortTask(const Ice::Current&)
{
- std::lock_guard<std::mutex> lock {mtx};
- setTaskStatus((getTaskStatus() != TaskStatus::eDone) ? TaskStatus::ePlanningAborted : TaskStatus::eDone);
+ std::lock_guard<std::mutex> lock{mtx};
+ setTaskStatus((getTaskStatus() != TaskStatus::eDone) ? TaskStatus::ePlanningAborted
+ : TaskStatus::eDone);
//worker will be aborted in run()
doneCV.notify_all();
}
- Path Task::getPath(const Ice::Current&) const
+ Path
+ Task::getPath(const Ice::Current&) const
{
- std::lock_guard<std::mutex> lock {mtx};
+ std::lock_guard<std::mutex> lock{mtx};
return solution;
}
- void Task::workerHasAborted(Ice::Long workerId, const Ice::Current&)
+ void
+ Task::workerHasAborted(Ice::Long workerId, const Ice::Current&)
{
- std::lock_guard<std::mutex> lock {mtx};
+ std::lock_guard<std::mutex> lock{mtx};
ARMARX_CHECK_EXPRESSION(static_cast<std::size_t>(workerId) < workerAbortedFlags.size());
workerAbortedFlags.at(workerId) = true;
}
- void Task::run(const RemoteObjectNodePrxList& remoteNodes, const Ice::Current&)
+ void
+ Task::run(const RemoteObjectNodePrxList& remoteNodes, const Ice::Current&)
{
ARMARX_CHECK_EXPRESSION(cspace);
- ARMARX_CHECK_EXPRESSION(std::all_of(remoteNodes.begin(), remoteNodes.end(), [](const RemoteObjectNodeInterfacePrx & n)
- {
- return n;
- }));
+ ARMARX_CHECK_EXPRESSION(std::all_of(remoteNodes.begin(),
+ remoteNodes.end(),
+ [](const RemoteObjectNodeInterfacePrx& n)
+ { return n; }));
//init
workerAbortedFlags.assign(workerCount, false);
setTaskStatus(TaskStatus::ePlanning);
const std::string topicName = "RRTConnectUpdateTopic:" + getProxy()->ice_getIdentity().name;
//check trivial cases
- const auto startIsCollisionFree = cspace->isCollisionFree(std::pair<const Ice::Float*, const Ice::Float*> {startCfg.data(), startCfg.data() + startCfg.size()});
+ const auto startIsCollisionFree =
+ cspace->isCollisionFree(std::pair<const Ice::Float*, const Ice::Float*>{
+ startCfg.data(), startCfg.data() + startCfg.size()});
if (!startIsCollisionFree)
{
setTaskStatus(TaskStatus::ePlanningFailed);
- ARMARX_WARNING << "MotionPlanningTask " << taskName << " trivially failed! start cfg in collision. start:\n" << startCfg;
+ ARMARX_WARNING << "MotionPlanningTask " << taskName
+ << " trivially failed! start cfg in collision. start:\n"
+ << startCfg;
return;
}
- const auto goalIscollisionFree = cspace->isCollisionFree(std::pair<const Ice::Float*, const Ice::Float*> { goalCfg.data(), goalCfg.data() + goalCfg.size()});
+ const auto goalIscollisionFree =
+ cspace->isCollisionFree(std::pair<const Ice::Float*, const Ice::Float*>{
+ goalCfg.data(), goalCfg.data() + goalCfg.size()});
if (!goalIscollisionFree)
{
setTaskStatus(TaskStatus::ePlanningFailed);
- ARMARX_WARNING << "MotionPlanningTask " << taskName << " trivially failed! goal cfg in collision. goal:\n" << goalCfg;
+ ARMARX_WARNING << "MotionPlanningTask " << taskName
+ << " trivially failed! goal cfg in collision. goal:\n"
+ << goalCfg;
return;
}
@@ -94,13 +113,15 @@ namespace armarx::rrtconnect
solution.nodes.emplace_back(startCfg);
solution.nodes.emplace_back(goalCfg);
solution.pathName = taskName + "_trivialPath_" + ice_staticId();
- ARMARX_WARNING << "MotionPlanningTask " << taskName << " trivially succeeded! (start and goal less than dcd step size apart!)";
+ ARMARX_WARNING
+ << "MotionPlanningTask " << taskName
+ << " trivially succeeded! (start and goal less than dcd step size apart!)";
setTaskStatus(TaskStatus::eDone);
return;
}
//start workers
- std::unique_lock<std::mutex> lock {mtx};
+ std::unique_lock<std::mutex> lock{mtx};
std::vector<RemoteHandle<WorkerNodeBasePrx>> workers;
WorkerNodeBasePrxList workerProxies;
@@ -112,18 +133,16 @@ namespace armarx::rrtconnect
auto maxWorkerForNode = ron->getNumberOfCores();
for (Ice::Long i = 0; i < maxWorkerForNode && workerToStart; ++i)
{
- WorkerNodeBasePtr worker {new WorkerNode{
- cspace->clone(),
- startCfg,
- goalCfg,
- dcdStep,
- TaskBasePrx::uncheckedCast(getProxy()),
- topicName,
- static_cast<Ice::Long>(workers.size()),
- workerCount
- }
- };
- const std::string workerName = ManagedIceObject::generateSubObjectName(taskName, "RRTConnectWorker:" + to_string(workers.size()));
+ WorkerNodeBasePtr worker{new WorkerNode{cspace->clone(),
+ startCfg,
+ goalCfg,
+ dcdStep,
+ TaskBasePrx::uncheckedCast(getProxy()),
+ topicName,
+ static_cast<Ice::Long>(workers.size()),
+ workerCount}};
+ const std::string workerName = ManagedIceObject::generateSubObjectName(
+ taskName, "RRTConnectWorker:" + to_string(workers.size()));
workers.emplace_back(ron->registerRemoteHandledObject(workerName, worker));
workerProxies.emplace_back(*(workers.back()));
--workerToStart;
@@ -141,11 +160,12 @@ namespace armarx::rrtconnect
}
//wait for done or timeout
- const auto endTime = std::chrono::system_clock::now() + std::chrono::seconds {maximalPlanningTimeInSeconds};
+ const auto endTime =
+ std::chrono::system_clock::now() + std::chrono::seconds{maximalPlanningTimeInSeconds};
while (true)
{
- doneCV.wait_for(lock, std::chrono::milliseconds {10});
+ doneCV.wait_for(lock, std::chrono::milliseconds{10});
if (endTime < std::chrono::system_clock::now())
{
setTaskStatus(TaskStatus::ePlanningFailed);
@@ -158,32 +178,31 @@ namespace armarx::rrtconnect
}
//wait for all stopped
- while (std::any_of(workerAbortedFlags.begin(), workerAbortedFlags.end(), [](bool b)
- {
- return !b;
- }))
+ while (std::any_of(
+ workerAbortedFlags.begin(), workerAbortedFlags.end(), [](bool b) { return !b; }))
{
for (auto& w : workers)
{
w->abort();
}
- doneCV.wait_for(lock, std::chrono::milliseconds {10});
+ doneCV.wait_for(lock, std::chrono::milliseconds{10});
}
}
- void Task::setPath(const Path& path, const Ice::Current&)
+ void
+ Task::setPath(const Path& path, const Ice::Current&)
{
- std::lock_guard<std::mutex> lock {mtx};
+ std::lock_guard<std::mutex> lock{mtx};
solution = path;
solution.pathName = taskName + "_path_" + ice_staticId();
setTaskStatus(TaskStatus::eDone);
doneCV.notify_all();
}
- void Task::postEnqueueing()
+ void
+ Task::postEnqueueing()
{
MotionPlanningTaskWithDefaultMembers::postEnqueueing();
cspace->initCollisionTest();
}
-}
-
+} // namespace armarx::rrtconnect
diff --git a/source/RobotComponents/components/MotionPlanning/Tasks/RRTConnect/Task.h b/source/RobotComponents/components/MotionPlanning/Tasks/RRTConnect/Task.h
index fb50d7b9f08361e00704244c5285d1d8f808ec91..d0ceb8e642b0d238c5967b7add69f010391db78a 100644
--- a/source/RobotComponents/components/MotionPlanning/Tasks/RRTConnect/Task.h
+++ b/source/RobotComponents/components/MotionPlanning/Tasks/RRTConnect/Task.h
@@ -23,30 +23,26 @@
*/
#pragma once
-#include <mutex>
#include <chrono>
#include <condition_variable>
+#include <mutex>
#include <ArmarXCore/core/system/FactoryCollectionBase.h>
-
-
#include <ArmarXCore/core/util/distributed/RemoteHandle/RemoteHandle.h>
#include <ArmarXCore/interface/core/RemoteObjectNode.h>
#include <RobotComponents/interface/components/MotionPlanning/Tasks/RRTConnect/Task.h>
-#include "../../util/Metrics.h"
-#include "WorkerNode.h"
+#include "../../util/Metrics.h"
#include "../MotionPlanningTask.h"
+#include "WorkerNode.h"
namespace armarx::rrtconnect
{
class Task;
using TaskPtr = IceInternal::Handle<Task>;
- class Task:
- public virtual MotionPlanningTaskWithDefaultMembers,
- public virtual TaskBase
+ class Task : public virtual MotionPlanningTaskWithDefaultMembers, public virtual TaskBase
{
public:
/**
@@ -58,7 +54,7 @@ namespace armarx::rrtconnect
* @param maximalPlanningTimeInSeconds The maximal planning time in seconds.
* @param workerCount The maximal number of computation nodes used.
*/
- Task(//problem
+ Task( //problem
const CSpaceBasePtr& cspace,
const VectorXf& startCfg,
const VectorXf& goalCfg,
@@ -67,8 +63,7 @@ namespace armarx::rrtconnect
Ice::Long maximalPlanningTimeInSeconds = 300,
float dcdStep = 0.01f,
//management
- Ice::Long workerCount = 4
- );
+ Ice::Long workerCount = 4);
//PlanningControlInterface
/**
@@ -87,7 +82,8 @@ namespace armarx::rrtconnect
* @brief Runs the task.
* @param remoteNodes The list of \ref RemoteObjectNodeInterfacePrx used to distribute work to computers.
*/
- void run(const RemoteObjectNodePrxList& remoteNodes, const Ice::Current& = Ice::emptyCurrent) override;
+ void run(const RemoteObjectNodePrxList& remoteNodes,
+ const Ice::Current& = Ice::emptyCurrent) override;
void setPath(const Path& path, const Ice::Current& = Ice::emptyCurrent) override;
@@ -114,13 +110,14 @@ namespace armarx::rrtconnect
std::vector<bool> workerAbortedFlags;
private:
- template<class Base, class Derived> friend class ::armarx::GenericFactory;
+ template <class Base, class Derived>
+ friend class ::armarx::GenericFactory;
};
-}
+} // namespace armarx::rrtconnect
namespace armarx
{
using RRTConnectTask = rrtconnect::Task;
using RRTConnectTaskPtr = IceUtil::Handle<RRTConnectTask>;
using RRTConnectTaskHandle = RemoteHandle<MotionPlanningTaskControlInterfacePrx>;
-}
+} // namespace armarx
diff --git a/source/RobotComponents/components/MotionPlanning/Tasks/RRTConnect/Tree.cpp b/source/RobotComponents/components/MotionPlanning/Tasks/RRTConnect/Tree.cpp
index 222296094d2bbc50d16250f6c02f6d3e03dd6246..3d10a2762a6957a00a4caa15d93540c8b485cab9 100644
--- a/source/RobotComponents/components/MotionPlanning/Tasks/RRTConnect/Tree.cpp
+++ b/source/RobotComponents/components/MotionPlanning/Tasks/RRTConnect/Tree.cpp
@@ -22,6 +22,8 @@
* GNU General Public License
*/
+#include "Tree.h"
+
#include <algorithm>
#include <limits>
@@ -29,14 +31,10 @@
#include "../../util/Metrics.h"
-#include "Tree.h"
-
-const armarx::rrtconnect::NodeId armarx::rrtconnect::Tree::ROOT_NODE_ID
-{
- 0, 0
-};
+const armarx::rrtconnect::NodeId armarx::rrtconnect::Tree::ROOT_NODE_ID{0, 0};
-void armarx::rrtconnect::Tree::setRoot(const armarx::rrtconnect::Tree::ConfigType& root)
+void
+armarx::rrtconnect::Tree::setRoot(const armarx::rrtconnect::Tree::ConfigType& root)
{
ARMARX_CHECK_EXPRESSION(!nodes.empty());
if (nodes.at(0).empty())
@@ -49,7 +47,8 @@ void armarx::rrtconnect::Tree::setRoot(const armarx::rrtconnect::Tree::ConfigTyp
}
}
-std::vector<armarx::rrtconnect::Tree::ConfigType> armarx::rrtconnect::Tree::getReversedPathTo(armarx::rrtconnect::NodeId nodeId) const
+std::vector<armarx::rrtconnect::Tree::ConfigType>
+armarx::rrtconnect::Tree::getReversedPathTo(armarx::rrtconnect::NodeId nodeId) const
{
std::vector<ConfigType> path;
while (nodeId != ROOT_NODE_ID)
@@ -62,14 +61,16 @@ std::vector<armarx::rrtconnect::Tree::ConfigType> armarx::rrtconnect::Tree::getR
return path;
}
-std::vector<armarx::rrtconnect::Tree::ConfigType> armarx::rrtconnect::Tree::getPathTo(const armarx::rrtconnect::NodeId& nodeId) const
+std::vector<armarx::rrtconnect::Tree::ConfigType>
+armarx::rrtconnect::Tree::getPathTo(const armarx::rrtconnect::NodeId& nodeId) const
{
std::vector<ConfigType> path = getReversedPathTo(nodeId);
std::reverse(path.begin(), path.end());
return path;
}
-armarx::rrtconnect::NodeId armarx::rrtconnect::Tree::getNearestNeighbour(const armarx::rrtconnect::Tree::ConfigType& cfg)
+armarx::rrtconnect::NodeId
+armarx::rrtconnect::Tree::getNearestNeighbour(const armarx::rrtconnect::Tree::ConfigType& cfg)
{
float minDistance = std::numeric_limits<float>::infinity();
NodeId nearesNode;
@@ -78,9 +79,11 @@ armarx::rrtconnect::NodeId armarx::rrtconnect::Tree::getNearestNeighbour(const a
const auto& workerNodes = nodes.at(workerId);
for (std::size_t nodeSubId = 0; nodeSubId < workerNodes.size(); ++nodeSubId)
{
- NodeId currentNodeId {static_cast<Ice::Long>(workerId), static_cast<Ice::Long>(nodeSubId)};
+ NodeId currentNodeId{static_cast<Ice::Long>(workerId),
+ static_cast<Ice::Long>(nodeSubId)};
const auto& node = getNode(currentNodeId);
- float currentDistance = euclideanDistance(cfg.data(), cfg.data() + cfg.size(), node.cfg.data());
+ float currentDistance =
+ euclideanDistance(cfg.data(), cfg.data() + cfg.size(), node.cfg.data());
if (currentDistance < minDistance)
{
minDistance = currentDistance;
diff --git a/source/RobotComponents/components/MotionPlanning/Tasks/RRTConnect/Tree.h b/source/RobotComponents/components/MotionPlanning/Tasks/RRTConnect/Tree.h
index ac549108682411302bfa2883ba4af8d0bebbc19e..2a437a5ff56bc1d96d8d34a9e7417790724e3d85 100644
--- a/source/RobotComponents/components/MotionPlanning/Tasks/RRTConnect/Tree.h
+++ b/source/RobotComponents/components/MotionPlanning/Tasks/RRTConnect/Tree.h
@@ -36,10 +36,9 @@ namespace armarx::rrtconnect
{
using ConfigType = VectorXf;
- NodeType(ConfigType cfg, NodeId parent):
- cfg {std::move(cfg)},
- parent(parent)
- {}
+ NodeType(ConfigType cfg, NodeId parent) : cfg{std::move(cfg)}, parent(parent)
+ {
+ }
ConfigType cfg;
NodeId parent;
@@ -50,11 +49,11 @@ namespace armarx::rrtconnect
public:
using ConfigType = VectorXf;
- Tree(): nodes(1)
+ Tree() : nodes(1)
{
}
- Tree(std::size_t maximalWorkerCount): nodes(maximalWorkerCount)
+ Tree(std::size_t maximalWorkerCount) : nodes(maximalWorkerCount)
{
}
@@ -63,28 +62,34 @@ namespace armarx::rrtconnect
* This value has to be set before the tree's useage. and should never be set twice.
* @param count The new count.
*/
- void setMaximalWorkerCount(std::size_t count)
+ void
+ setMaximalWorkerCount(std::size_t count)
{
nodes.resize(count);
}
void setRoot(const ConfigType& root);
- const std::deque<std::deque<NodeType>>& getNodes() const
+ const std::deque<std::deque<NodeType>>&
+ getNodes() const
{
return nodes;
}
std::vector<ConfigType> getReversedPathTo(NodeId nodeId) const;
std::vector<ConfigType> getPathTo(const NodeId& nodeId) const;
- const NodeType& getNode(const NodeId& nodeId) const
+
+ const NodeType&
+ getNode(const NodeId& nodeId) const
{
ARMARX_CHECK_EXPRESSION(static_cast<std::size_t>(nodeId.workerId) < nodes.size());
- ARMARX_CHECK_EXPRESSION(static_cast<std::size_t>(nodeId.nodeSubId) < nodes.at(nodeId.workerId).size());
+ ARMARX_CHECK_EXPRESSION(static_cast<std::size_t>(nodeId.nodeSubId) <
+ nodes.at(nodeId.workerId).size());
return nodes.at(nodeId.workerId).at(nodeId.nodeSubId);
}
- void addNode(const ConfigType& cfg, const NodeId& parent, std::size_t workerId)
+ void
+ addNode(const ConfigType& cfg, const NodeId& parent, std::size_t workerId)
{
ARMARX_CHECK_EXPRESSION(workerId < nodes.size());
nodes.at(workerId).emplace_back(cfg, parent);
@@ -93,12 +98,14 @@ namespace armarx::rrtconnect
NodeId getNearestNeighbour(const ConfigType& cfg);
- std::size_t getSize() const
+ std::size_t
+ getSize() const
{
return size;
}
- void applyUpdate(const PerTreeUpdate& u, Ice::Long workerId)
+ void
+ applyUpdate(const PerTreeUpdate& u, Ice::Long workerId)
{
for (const auto& n : u.nodes)
{
@@ -111,5 +118,4 @@ namespace armarx::rrtconnect
std::deque<std::deque<NodeType>> nodes;
std::size_t size = 0;
};
-}
-
+} // namespace armarx::rrtconnect
diff --git a/source/RobotComponents/components/MotionPlanning/Tasks/RRTConnect/Updater.cpp b/source/RobotComponents/components/MotionPlanning/Tasks/RRTConnect/Updater.cpp
index d0c05f52734773e64b7563f5574ed30f50bbece9..b6bc490bb92e148b23ed10ac3c5071ee7ad63090 100644
--- a/source/RobotComponents/components/MotionPlanning/Tasks/RRTConnect/Updater.cpp
+++ b/source/RobotComponents/components/MotionPlanning/Tasks/RRTConnect/Updater.cpp
@@ -25,26 +25,30 @@
#include <ArmarXCore/core/logging/Logging.h>
-void armarx::rrtconnect::Updater::setWorkerId(Ice::Long newId)
+void
+armarx::rrtconnect::Updater::setWorkerId(Ice::Long newId)
{
ARMARX_CHECK_EXPRESSION(newId >= 0);
workerId = newId;
setWorkerCount(workerId + 1);
}
-void armarx::rrtconnect::Updater::addPendingUpdate(const armarx::rrtconnect::Update& u)
+void
+armarx::rrtconnect::Updater::addPendingUpdate(const armarx::rrtconnect::Update& u)
{
- pendingUpdateLookupTable[UpdateId {u}] = pendingUpdates.size();
+ pendingUpdateLookupTable[UpdateId{u}] = pendingUpdates.size();
pendingUpdates.emplace_back(u);
}
-void armarx::rrtconnect::Updater::clearPendingUpdates()
+void
+armarx::rrtconnect::Updater::clearPendingUpdates()
{
pendingUpdateLookupTable.clear();
pendingUpdates.clear();
}
-void armarx::rrtconnect::Updater::applyUpdate(const armarx::rrtconnect::Update& u)
+void
+armarx::rrtconnect::Updater::applyUpdate(const armarx::rrtconnect::Update& u)
{
ARMARX_CHECK_EXPRESSION(!hasAppliedUpdate(u));
ARMARX_CHECK_EXPRESSION(canApplyUpdate(u));
@@ -58,17 +62,18 @@ void armarx::rrtconnect::Updater::applyUpdate(const armarx::rrtconnect::Update&
++appliedUpdateIds.at(u.workerId);
}
-bool armarx::rrtconnect::Updater::canApplyUpdate(const armarx::rrtconnect::Update& u)
+bool
+armarx::rrtconnect::Updater::canApplyUpdate(const armarx::rrtconnect::Update& u)
{
- for (Ice::Long workerId = 0; static_cast<std::size_t>(workerId) < u.dependetOnUpdateIds.size(); ++workerId)
+ for (Ice::Long workerId = 0; static_cast<std::size_t>(workerId) < u.dependetOnUpdateIds.size();
+ ++workerId)
{
if (!hasAppliedUpdate(workerId, u.dependetOnUpdateIds.at(workerId)))
{
- const UpdateId currUpdateId
- {
- u
- };
- ARMARX_ERROR_S << "[worker " << workerId << "] missing update " << workerId << "/" << u.dependetOnUpdateIds.at(workerId) << " for update " << currUpdateId.workerId << "/" << currUpdateId.updateSubId;
+ const UpdateId currUpdateId{u};
+ ARMARX_ERROR_S << "[worker " << workerId << "] missing update " << workerId << "/"
+ << u.dependetOnUpdateIds.at(workerId) << " for update "
+ << currUpdateId.workerId << "/" << currUpdateId.updateSubId;
return false;
}
}
diff --git a/source/RobotComponents/components/MotionPlanning/Tasks/RRTConnect/Updater.h b/source/RobotComponents/components/MotionPlanning/Tasks/RRTConnect/Updater.h
index ae37ce837d9f5e08fd7be800f9ecbe5d493a061b..fab319e7c1db5b77e2962418b947da9f9feb9b56 100644
--- a/source/RobotComponents/components/MotionPlanning/Tasks/RRTConnect/Updater.h
+++ b/source/RobotComponents/components/MotionPlanning/Tasks/RRTConnect/Updater.h
@@ -24,37 +24,39 @@
#pragma once
#include <functional>
-#include <vector>
#include <map>
#include <mutex>
+#include <vector>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <RobotComponents/interface/components/MotionPlanning/Tasks/RRTConnect/DataStructures.h>
#include "Tree.h"
+
namespace armarx::rrtconnect
{
struct UpdateId
{
- UpdateId(Ice::Long workerId = -1, Ice::Long updateSubId = -1):
- workerId {workerId},
- updateSubId {updateSubId}
- {}
-
- UpdateId(const Update& u):
- workerId {u.workerId},
- updateSubId
+ UpdateId(Ice::Long workerId = -1, Ice::Long updateSubId = -1) :
+ workerId{workerId}, updateSubId{updateSubId}
+ {
+ }
+
+ UpdateId(const Update& u) :
+ workerId{u.workerId},
+ updateSubId{((workerId >= 0) &&
+ (static_cast<std::size_t>(workerId) < u.dependetOnUpdateIds.size()))
+ ? u.dependetOnUpdateIds.at(workerId) + 1
+ : -1}
{
- ((workerId >= 0) && (static_cast<std::size_t>(workerId) < u.dependetOnUpdateIds.size())) ?
- u.dependetOnUpdateIds.at(workerId) + 1 :
- -1
}
- {}
- bool operator <(const UpdateId& other) const
+ bool
+ operator<(const UpdateId& other) const
{
- return (workerId < other.workerId) || ((workerId == other.workerId) && (updateSubId < other.updateSubId));
+ return (workerId < other.workerId) ||
+ ((workerId == other.workerId) && (updateSubId < other.updateSubId));
}
Ice::Long workerId;
@@ -66,8 +68,8 @@ namespace armarx::rrtconnect
public:
Updater() = default;
-
- void setWorkerCount(std::size_t count)
+ void
+ setWorkerCount(std::size_t count)
{
if (appliedUpdateIds.size() < count)
{
@@ -77,7 +79,8 @@ namespace armarx::rrtconnect
void setWorkerId(Ice::Long newId);
- void setTrees(const std::vector<std::reference_wrapper<Tree>>& newTrees)
+ void
+ setTrees(const std::vector<std::reference_wrapper<Tree>>& newTrees)
{
trees = newTrees;
}
@@ -87,17 +90,20 @@ namespace armarx::rrtconnect
void clearPendingUpdates();
- bool hasPendingUpdate(const UpdateId& id) const
+ bool
+ hasPendingUpdate(const UpdateId& id) const
{
return pendingUpdateLookupTable.find(id) != pendingUpdateLookupTable.end();
}
- Update& getPendingUpdate(const UpdateId& id)
+ Update&
+ getPendingUpdate(const UpdateId& id)
{
return getPendingUpdate(pendingUpdateLookupTable.at(id));
}
- Update& getPendingUpdate(std::size_t id)
+ Update&
+ getPendingUpdate(std::size_t id)
{
return pendingUpdates.at(id);
}
@@ -105,14 +111,18 @@ namespace armarx::rrtconnect
//applying
void applyUpdate(const Update& u);
- template<class LockType, class RemoteUpdateGetter>
- void applyPendingUpdates(LockType&& lock, RemoteUpdateGetter getRemoteUpdate)
+ template <class LockType, class RemoteUpdateGetter>
+ void
+ applyPendingUpdates(LockType&& lock, RemoteUpdateGetter getRemoteUpdate)
{
applyPendingUpdates(lock, getRemoteUpdate, [](Update&&) {});
}
- template<class LockType, class RemoteUpdateGetter, class UpdateConsumer>
- void applyPendingUpdates(LockType&& lock, RemoteUpdateGetter getRemoteUpdate, UpdateConsumer updateConsumer)
+ template <class LockType, class RemoteUpdateGetter, class UpdateConsumer>
+ void
+ applyPendingUpdates(LockType&& lock,
+ RemoteUpdateGetter getRemoteUpdate,
+ UpdateConsumer updateConsumer)
{
ARMARX_CHECK_EXPRESSION(lock);
//we want i for an assert after the loop
@@ -131,8 +141,12 @@ namespace armarx::rrtconnect
clearPendingUpdates();
}
- template<class LockType, class RemoteUpdateGetter, class UpdateConsumer>
- void applyPendingUpdate(Update& u, LockType&& lock, RemoteUpdateGetter getRemoteUpdate, UpdateConsumer updateConsumer)
+ template <class LockType, class RemoteUpdateGetter, class UpdateConsumer>
+ void
+ applyPendingUpdate(Update& u,
+ LockType&& lock,
+ RemoteUpdateGetter getRemoteUpdate,
+ UpdateConsumer updateConsumer)
{
ARMARX_CHECK_EXPRESSION(lock);
ARMARX_CHECK_EXPRESSION(u.workerId >= 0);
@@ -143,12 +157,13 @@ namespace armarx::rrtconnect
return;
}
- ARMARX_CHECK_EXPRESSION(static_cast<std::size_t>(u.workerId) < u.dependetOnUpdateIds.size());
+ ARMARX_CHECK_EXPRESSION(static_cast<std::size_t>(u.workerId) <
+ u.dependetOnUpdateIds.size());
//is it an update from this node or was it already applied?
if (
//u.workerId == workerId || //self update //now handled by has applied update
- hasAppliedUpdate(u)//update was already applied
+ hasAppliedUpdate(u) //update was already applied
)
{
return;
@@ -168,24 +183,22 @@ namespace armarx::rrtconnect
u.dependetOnUpdateIds.clear();
}
- template<class LockType, class RemoteUpdateGetter, class UpdateConsumer>
- void prepareUpdate(
- Ice::LongSeq dependetOnUpdateIds,
- LockType&& lock,
- RemoteUpdateGetter getRemoteUpdate,
- UpdateConsumer updateConsumer
- )
+ template <class LockType, class RemoteUpdateGetter, class UpdateConsumer>
+ void
+ prepareUpdate(Ice::LongSeq dependetOnUpdateIds,
+ LockType&& lock,
+ RemoteUpdateGetter getRemoteUpdate,
+ UpdateConsumer updateConsumer)
{
ARMARX_CHECK_EXPRESSION(lock);
- for (Ice::Long workerNodeId = 0; static_cast<std::size_t>(workerNodeId) < dependetOnUpdateIds.size(); ++workerNodeId)
+ for (Ice::Long workerNodeId = 0;
+ static_cast<std::size_t>(workerNodeId) < dependetOnUpdateIds.size();
+ ++workerNodeId)
{
const auto updateSubId = dependetOnUpdateIds.at(workerNodeId);
- const UpdateId uId
- {
- workerNodeId, updateSubId
- };
+ const UpdateId uId{workerNodeId, updateSubId};
if (hasAppliedUpdate(uId))
{
@@ -198,7 +211,8 @@ namespace armarx::rrtconnect
if (hasPendingUpdate(uId))
{
//local update => recurse
- applyPendingUpdate(getPendingUpdate(uId), lock, getRemoteUpdate, updateConsumer);
+ applyPendingUpdate(
+ getPendingUpdate(uId), lock, getRemoteUpdate, updateConsumer);
}
else
{
@@ -208,7 +222,8 @@ namespace armarx::rrtconnect
Update update = getRemoteUpdate(workerNodeId, updateSubId);
lock.lock();
//prepare this update and apply it
- prepareUpdate(update.dependetOnUpdateIds, lock, getRemoteUpdate, updateConsumer);
+ prepareUpdate(
+ update.dependetOnUpdateIds, lock, getRemoteUpdate, updateConsumer);
ARMARX_CHECK_EXPRESSION(lock);
ARMARX_CHECK_EXPRESSION(canApplyUpdate(update));
applyUpdate(update);
@@ -221,20 +236,25 @@ namespace armarx::rrtconnect
}
//applied
- bool hasAppliedUpdate(Ice::Long workerId, Ice::Long updateSubId) const
+ bool
+ hasAppliedUpdate(Ice::Long workerId, Ice::Long updateSubId) const
{
- return (this->workerId == workerId) || ((static_cast<std::size_t>(workerId) < appliedUpdateIds.size()) ?
- updateSubId <= appliedUpdateIds.at(workerId) : false);
+ return (this->workerId == workerId) ||
+ ((static_cast<std::size_t>(workerId) < appliedUpdateIds.size())
+ ? updateSubId <= appliedUpdateIds.at(workerId)
+ : false);
}
- bool hasAppliedUpdate(const UpdateId& id) const
+ bool
+ hasAppliedUpdate(const UpdateId& id) const
{
return hasAppliedUpdate(id.workerId, id.updateSubId);
}
bool canApplyUpdate(const Update& u);
- const std::vector<Ice::Long>& getAppliedUpdateIds() const
+ const std::vector<Ice::Long>&
+ getAppliedUpdateIds() const
{
return appliedUpdateIds;
}
@@ -248,4 +268,4 @@ namespace armarx::rrtconnect
std::vector<Ice::Long> appliedUpdateIds;
Ice::Long workerId = -1;
};
-}
+} // namespace armarx::rrtconnect
diff --git a/source/RobotComponents/components/MotionPlanning/Tasks/RRTConnect/WorkerNode.cpp b/source/RobotComponents/components/MotionPlanning/Tasks/RRTConnect/WorkerNode.cpp
index b136aaf0a3ee9c886f2dab29bacf8070f5e3dbae..7dae41efb33623423bc4fa74fef443c58971234e 100644
--- a/source/RobotComponents/components/MotionPlanning/Tasks/RRTConnect/WorkerNode.cpp
+++ b/source/RobotComponents/components/MotionPlanning/Tasks/RRTConnect/WorkerNode.cpp
@@ -23,10 +23,13 @@
*/
#include "WorkerNode.h"
+
#include "../../CSpace/CSpace.h"
+
namespace armarx::rrtconnect
{
- void WorkerNode::onInitComponent()
+ void
+ WorkerNode::onInitComponent()
{
abortRequest = false;
usingTopic(topicName);
@@ -36,77 +39,79 @@ namespace armarx::rrtconnect
ARMARX_CHECK_EXPRESSION(!workerThread.joinable());
}
- void WorkerNode::onConnectComponent()
+ void
+ WorkerNode::onConnectComponent()
{
globalWorkers = getTopic<TreeUpdateInterfacePrx>(topicName);
if (!workerThread.joinable())
{
- workerThread = std::thread
- {
- [this]
- {
- try
- {
- workerTask();
- }
- catch (Ice::Exception& e)
- {
- ARMARX_ERROR_S << "The worker thread of worker " << workerId << " had an Ice::Exception! \nStack trace:\n"
- << e.ice_stackTrace();
- }
- }
- };
+ workerThread = std::thread{[this]
+ {
+ try
+ {
+ workerTask();
+ }
+ catch (Ice::Exception& e)
+ {
+ ARMARX_ERROR_S
+ << "The worker thread of worker " << workerId
+ << " had an Ice::Exception! \nStack trace:\n"
+ << e.ice_stackTrace();
+ }
+ }};
}
}
- void WorkerNode::onExitComponent()
+ void
+ WorkerNode::onExitComponent()
{
abort();
workerThread.join();
}
- Update WorkerNode::getUpdate(Ice::Long updateId, const Ice::Current&) const
+ Update
+ WorkerNode::getUpdate(Ice::Long updateId, const Ice::Current&) const
{
- std::lock_guard<std::mutex> lock {updateMutex};
- ARMARX_VERBOSE_S << "[worker " << workerId
- << "] request for update " << updateId << " (of " << localUpdates.size() << ")";
+ std::lock_guard<std::mutex> lock{updateMutex};
+ ARMARX_VERBOSE_S << "[worker " << workerId << "] request for update " << updateId << " (of "
+ << localUpdates.size() << ")";
return localUpdates.at(updateId);
}
- void WorkerNode::setWorkerNodes(const WorkerNodeBasePrxList& workers, const Ice::Current&)
+ void
+ WorkerNode::setWorkerNodes(const WorkerNodeBasePrxList& workers, const Ice::Current&)
{
ARMARX_CHECK_EXPRESSION(!doApplyUpdates);
{
- std::lock_guard<std::mutex> lock {updateMutex};
+ std::lock_guard<std::mutex> lock{updateMutex};
this->workers = workers;
updater.setWorkerCount(workers.size());
}
doApplyUpdates = true;
}
- void WorkerNode::updateTree(const Update& u, const Ice::Current&)
+ void
+ WorkerNode::updateTree(const Update& u, const Ice::Current&)
{
- std::lock_guard<std::mutex> lock {updateMutex};
+ std::lock_guard<std::mutex> lock{updateMutex};
updater.addPendingUpdate(u);
}
- void WorkerNode::applyUpdates()
+ void
+ WorkerNode::applyUpdates()
{
if (doApplyUpdates)
{
- std::unique_lock<std::mutex> lock {updateMutex};
- updater.applyPendingUpdates(
- lock,
- [this](Ice::Long workerId, Ice::Long updateId)
- {
- return getRemoteUpdate(workerId, updateId);
- }
- );
+ std::unique_lock<std::mutex> lock{updateMutex};
+ updater.applyPendingUpdates(lock,
+ [this](Ice::Long workerId, Ice::Long updateId)
+ { return getRemoteUpdate(workerId, updateId); });
ARMARX_CHECK_EXPRESSION(lock);
}
}
- void WorkerNode::ice_postUnmarshal()
+ void
+ WorkerNode::ice_postUnmarshal()
{
treeFromStart.setMaximalWorkerCount(workerCount);
treeFromGoal.setMaximalWorkerCount(workerCount);
@@ -114,42 +119,38 @@ namespace armarx::rrtconnect
treeFromGoal.setRoot(goalCfg);
}
- void WorkerNode::workerTask()
+ void
+ WorkerNode::workerTask()
{
cspace->initCollisionTest();
//init sampler
const auto cspaceDims = cspace->getDimensionsBounds();
- std::vector<std::pair<float, float>> dimensionBounds {};
+ std::vector<std::pair<float, float>> dimensionBounds{};
dimensionBounds.reserve(getDimensionality());
- std::transform(
- cspaceDims.begin(), cspaceDims.end(),
- std::back_inserter(dimensionBounds),
- [](const FloatRange & dim)
- {
- return std::make_pair(dim.min, dim.max);
- }
- );
+ std::transform(cspaceDims.begin(),
+ cspaceDims.end(),
+ std::back_inserter(dimensionBounds),
+ [](const FloatRange& dim) { return std::make_pair(dim.min, dim.max); });
sampler.reset(
- new CuboidSampler
- {
- typename CuboidSampler::DistributionType{dimensionBounds.begin(), dimensionBounds.end()},
- typename CuboidSampler::GeneratorType{std::random_device{}()}
- }
- );
+ new CuboidSampler{typename CuboidSampler::DistributionType{dimensionBounds.begin(),
+ dimensionBounds.end()},
+ typename CuboidSampler::GeneratorType{std::random_device{}()}});
//util functions
auto cspaceDerived = CSpacePtr::dynamicCast(cspace);
- auto steer = [cspaceDerived, this](const ConfigType & from, const ConfigType & to)
+ auto steer = [cspaceDerived, this](const ConfigType& from, const ConfigType& to)
{
- ARMARX_CHECK_EXPRESSION(static_cast<std::size_t>(cspaceDerived->getDimensionality()) == from.size());
+ ARMARX_CHECK_EXPRESSION(static_cast<std::size_t>(cspaceDerived->getDimensionality()) ==
+ from.size());
ARMARX_CHECK_EXPRESSION(from.size() == to.size());
- return dcdSteer(from, to, DCDStepSize, [cspaceDerived](const ConfigType & cfg)
- {
- return cspaceDerived->isCollisionFree(cfg);
- });
+ return dcdSteer(from,
+ to,
+ DCDStepSize,
+ [cspaceDerived](const ConfigType& cfg)
+ { return cspaceDerived->isCollisionFree(cfg); });
};
//init trees
treeFromStart.setMaximalWorkerCount(workerCount);
@@ -197,10 +198,10 @@ namespace armarx::rrtconnect
{
//found path. set path + exit
const auto startTreeId = fromStartIsPrimaryTree ? reachId : reachSecondId;
- const auto goalTreeId = fromStartIsPrimaryTree ? reachSecondId : reachId;
+ const auto goalTreeId = fromStartIsPrimaryTree ? reachSecondId : reachId;
VectorXfSeq p = treeFromStart.getPathTo(startTreeId);
- p.pop_back();//pop the middle node (it would be added 2 times)
+ p.pop_back(); //pop the middle node (it would be added 2 times)
VectorXfSeq pPart2 = treeFromGoal.getReversedPathTo(goalTreeId);
std::move(pPart2.begin(), pPart2.end(), std::back_inserter(p));
@@ -220,40 +221,52 @@ namespace armarx::rrtconnect
task->workerHasAborted(workerId);
}
- void WorkerNode::sendUpdate()
+ void
+ WorkerNode::sendUpdate()
{
- std::lock_guard<std::mutex> lock {updateMutex};
+ std::lock_guard<std::mutex> lock{updateMutex};
//set dependent updates + id
ARMARX_CHECK_EXPRESSION(!localUpdates.empty());
- ARMARX_CHECK_EXPRESSION(static_cast<std::size_t>(workerId) < updater.getAppliedUpdateIds().size());
+ ARMARX_CHECK_EXPRESSION(static_cast<std::size_t>(workerId) <
+ updater.getAppliedUpdateIds().size());
localUpdates.back().workerId = workerId;
localUpdates.back().dependetOnUpdateIds = updater.getAppliedUpdateIds();
//current update id = #updates -1
- ARMARX_CHECK_EXPRESSION(static_cast<std::size_t>(workerId) < localUpdates.back().dependetOnUpdateIds.size());
- localUpdates.back().dependetOnUpdateIds.at(workerId) = static_cast<Ice::Long>(localUpdates.size()) - 2;
+ ARMARX_CHECK_EXPRESSION(static_cast<std::size_t>(workerId) <
+ localUpdates.back().dependetOnUpdateIds.size());
+ localUpdates.back().dependetOnUpdateIds.at(workerId) =
+ static_cast<Ice::Long>(localUpdates.size()) - 2;
globalWorkers->updateTree(localUpdates.back());
//next update
prepareNextUpdate();
}
- void WorkerNode::prepareNextUpdate()
+ void
+ WorkerNode::prepareNextUpdate()
{
localUpdates.emplace_back();
localUpdates.back().updatesPerTree.resize(2);
}
- NodeId WorkerNode::addNode(const WorkerNode::ConfigType& cfg, const NodeId& parent, bool addToPrimaryTree)
+ NodeId
+ WorkerNode::addNode(const WorkerNode::ConfigType& cfg,
+ const NodeId& parent,
+ bool addToPrimaryTree)
{
cspace->initCollisionTest();
auto& tree = addToPrimaryTree ? getPrimaryTree() : getSecondaryTree();
tree.addNode(cfg, parent, workerId);
const auto treeId = addToPrimaryTree ? getPrimaryTreeId() : getSecondaryTreeId();
- ARMARX_CHECK_EXPRESSION(static_cast<std::size_t>(treeId) < localUpdates.back().updatesPerTree.size());
- localUpdates.back().updatesPerTree.at(treeId).nodes.emplace_back(NodeCreationUpdate {cfg, parent});
+ ARMARX_CHECK_EXPRESSION(static_cast<std::size_t>(treeId) <
+ localUpdates.back().updatesPerTree.size());
+ localUpdates.back().updatesPerTree.at(treeId).nodes.emplace_back(
+ NodeCreationUpdate{cfg, parent});
ARMARX_CHECK_EXPRESSION(workerId < static_cast<Ice::Long>(tree.getNodes().size()));
- return {workerId, static_cast<Ice::Long>(tree.getNodes().at(workerId).size()) - 1}; //wid + index of new node
+ return {workerId,
+ static_cast<Ice::Long>(tree.getNodes().at(workerId).size()) -
+ 1}; //wid + index of new node
}
-}
+} // namespace armarx::rrtconnect
diff --git a/source/RobotComponents/components/MotionPlanning/Tasks/RRTConnect/WorkerNode.h b/source/RobotComponents/components/MotionPlanning/Tasks/RRTConnect/WorkerNode.h
index 5ecea4211121e902230432bac50116d735afa666..ff9043598b3b80dbcb124be2c269102fd6217c86 100644
--- a/source/RobotComponents/components/MotionPlanning/Tasks/RRTConnect/WorkerNode.h
+++ b/source/RobotComponents/components/MotionPlanning/Tasks/RRTConnect/WorkerNode.h
@@ -23,25 +23,27 @@
*/
#pragma once
-#include <mutex>
-#include <thread>
+#include <algorithm>
#include <atomic>
#include <memory>
-#include <algorithm>
+#include <mutex>
+#include <thread>
#include <ArmarXCore/core/ManagedIceObject.h>
#include <RobotComponents/interface/components/MotionPlanning/Tasks/RRTConnect/WorkerNode.h>
-#include "../../util/Samplers.h"
-#include "../../util/CollisionCheckUtil.h"
+#include "../../util/CollisionCheckUtil.h"
+#include "../../util/Samplers.h"
#include "Tree.h"
#include "Updater.h"
namespace armarx
{
- template <class IceBaseClass, class DerivedClass> class GenericFactory;
+ template <class IceBaseClass, class DerivedClass>
+ class GenericFactory;
}
+
namespace armarx::rrtconnect
{
class WorkerNode;
@@ -50,9 +52,7 @@ namespace armarx::rrtconnect
*/
using WorkerNodePtr = IceInternal::Handle<WorkerNode>;
- class WorkerNode:
- virtual public armarx::ManagedIceObject,
- virtual public WorkerNodeBase
+ class WorkerNode : virtual public armarx::ManagedIceObject, virtual public WorkerNodeBase
{
public:
using ConfigType = VectorXf;
@@ -76,16 +76,21 @@ namespace armarx::rrtconnect
void onInitComponent() override;
void onConnectComponent() override;
void onExitComponent() override;
- std::string getDefaultName() const override
+
+ std::string
+ getDefaultName() const override
{
return "RRTConnectWorkerNode";
}
//from WorkerNodeBase
- Update getUpdate(Ice::Long updateId, const Ice::Current& = Ice::emptyCurrent) const override;
- void setWorkerNodes(const WorkerNodeBasePrxList& workers, const Ice::Current& = Ice::emptyCurrent) override;
+ Update getUpdate(Ice::Long updateId,
+ const Ice::Current& = Ice::emptyCurrent) const override;
+ void setWorkerNodes(const WorkerNodeBasePrxList& workers,
+ const Ice::Current& = Ice::emptyCurrent) override;
- void abort(const ::Ice::Current& = Ice::emptyCurrent) override
+ void
+ abort(const ::Ice::Current& = Ice::emptyCurrent) override
{
abortRequest = true;
}
@@ -110,10 +115,11 @@ namespace armarx::rrtconnect
* @param updateSubId The update's sub id.
* @return The update fetched from the manager node.
*/
- Update getRemoteUpdate(Ice::Long workerNodeId, Ice::Long updateSubId)
+ Update
+ getRemoteUpdate(Ice::Long workerNodeId, Ice::Long updateSubId)
{
- ARMARX_VERBOSE_S << "[worker " << workerId
- << "] requesting update " << workerNodeId << " / " << updateSubId;
+ ARMARX_VERBOSE_S << "[worker " << workerId << "] requesting update " << workerNodeId
+ << " / " << updateSubId;
ARMARX_CHECK_EXPRESSION(static_cast<std::size_t>(workerNodeId) < workers.size());
return workers.at(workerNodeId)->getUpdate(updateSubId);
}
@@ -128,11 +134,14 @@ namespace armarx::rrtconnect
NodeId addNode(const ConfigType& cfg, const NodeId& parent, bool addToPrimaryTree);
- NodeId addNodeToPrimaryTree(const ConfigType& cfg, const NodeId& parent)
+ NodeId
+ addNodeToPrimaryTree(const ConfigType& cfg, const NodeId& parent)
{
return addNode(cfg, parent, true);
}
- NodeId addNodeToSecondaryTree(const ConfigType& cfg, const NodeId& parent)
+
+ NodeId
+ addNodeToSecondaryTree(const ConfigType& cfg, const NodeId& parent)
{
return addNode(cfg, parent, false);
}
@@ -140,30 +149,38 @@ namespace armarx::rrtconnect
/**
* @return The cspace's dimensionality
*/
- std::size_t getDimensionality()
+ std::size_t
+ getDimensionality()
{
return cspace->getDimensionality();
}
- Tree& getPrimaryTree()
+ Tree&
+ getPrimaryTree()
{
return fromStartIsPrimaryTree ? treeFromStart : treeFromGoal;
}
- Tree& getSecondaryTree()
+
+ Tree&
+ getSecondaryTree()
{
return (!fromStartIsPrimaryTree) ? treeFromStart : treeFromGoal;
}
- Ice::Byte getPrimaryTreeId() const
+ Ice::Byte
+ getPrimaryTreeId() const
{
return fromStartIsPrimaryTree ? 0 : 1;
}
- Ice::Byte getSecondaryTreeId() const
+
+ Ice::Byte
+ getSecondaryTreeId() const
{
return fromStartIsPrimaryTree ? 1 : 0;
}
- void swapTrees()
+ void
+ swapTrees()
{
fromStartIsPrimaryTree ^= true;
}
@@ -175,7 +192,7 @@ namespace armarx::rrtconnect
mutable std::mutex updateMutex;
Updater updater;
- std::atomic_bool doApplyUpdates {false};
+ std::atomic_bool doApplyUpdates{false};
WorkerNodeBasePrxList workers;
std::thread workerThread;
@@ -189,5 +206,4 @@ namespace armarx::rrtconnect
std::unique_ptr<CuboidSampler> sampler;
void prepareNextUpdate();
};
-}
-
+} // namespace armarx::rrtconnect
diff --git a/source/RobotComponents/components/MotionPlanning/Tasks/RandomShortcutPostprocessor/Task.cpp b/source/RobotComponents/components/MotionPlanning/Tasks/RandomShortcutPostprocessor/Task.cpp
index be3d4df8e9b400561f731489abebfa76f8161b10..abb9d148de6b7934798af6a840f884925125bbf2 100644
--- a/source/RobotComponents/components/MotionPlanning/Tasks/RandomShortcutPostprocessor/Task.cpp
+++ b/source/RobotComponents/components/MotionPlanning/Tasks/RandomShortcutPostprocessor/Task.cpp
@@ -22,12 +22,17 @@
* GNU General Public License
*/
-#include <random>
#include "Task.h"
-#include "../../util/CollisionCheckUtil.h"
+
+#include <random>
+
+#include <VirtualRobot/RobotNodeSet.h>
+
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include "../../util/CollisionCheckUtil.h"
#include <MotionPlanning/CSpace/CSpaceSampled.h>
-#include <VirtualRobot/RobotNodeSet.h>
+
namespace armarx::rngshortcut
{
Task::Task(MotionPlanningTaskBasePtr previousStep,
@@ -36,45 +41,54 @@ namespace armarx::rngshortcut
Ice::Float dcdStep,
Ice::Long maxTries,
Ice::Float minShortcutImprovementRatio,
- Ice::Float minPathImprovementRatio):
+ Ice::Float minPathImprovementRatio) :
MotionPlanningTaskBase(taskName),
PostprocessingMotionPlanningTaskBase(taskName, previousStep),
PostprocessingMotionPlanningTask(previousStep, taskName),
- TaskBase(taskName, previousStep, dcdStep, maxTries, maxTimeForPostprocessingInSeconds, minShortcutImprovementRatio, minPathImprovementRatio)
+ TaskBase(taskName,
+ previousStep,
+ dcdStep,
+ maxTries,
+ maxTimeForPostprocessingInSeconds,
+ minShortcutImprovementRatio,
+ minPathImprovementRatio)
{
- if (! this->previousStep)
+ if (!this->previousStep)
{
- throw std::invalid_argument {"previousStep is NULL"};
+ throw std::invalid_argument{"previousStep is NULL"};
}
if (this->dcdStep <= 0)
{
- throw std::invalid_argument {"dcdStep <= 0"};
+ throw std::invalid_argument{"dcdStep <= 0"};
}
if (this->maxTimeForPostprocessingInSeconds <= 0)
{
- throw std::invalid_argument {"maxTimeForPostprocessingInSeconds <= 0"};
+ throw std::invalid_argument{"maxTimeForPostprocessingInSeconds <= 0"};
}
if (this->minShortcutImprovementRatio >= 1)
{
- throw std::invalid_argument {"minShortcutImprovementRatio >=1"};
+ throw std::invalid_argument{"minShortcutImprovementRatio >=1"};
}
if (this->minShortcutImprovementRatio < 0.01)
{
- ARMARX_VERBOSE_S << "Changed minShortcutImprovementRatio from " << this->minShortcutImprovementRatio << " to " << 0.01;
+ ARMARX_VERBOSE_S << "Changed minShortcutImprovementRatio from "
+ << this->minShortcutImprovementRatio << " to " << 0.01;
this->minShortcutImprovementRatio = 0.01;
}
if (this->minPathImprovementRatio >= 1)
{
- throw std::invalid_argument {"minPathImprovementRatio >=1"};
+ throw std::invalid_argument{"minPathImprovementRatio >=1"};
}
if (this->minPathImprovementRatio < 0.001)
{
- ARMARX_VERBOSE_S << "Changed minShortcutImprovementRatio from " << this->minShortcutImprovementRatio << " to " << 0.001;
+ ARMARX_VERBOSE_S << "Changed minShortcutImprovementRatio from "
+ << this->minShortcutImprovementRatio << " to " << 0.001;
this->minPathImprovementRatio = 0.001;
}
}
- void Task::abortTask(const Ice::Current&)
+ void
+ Task::abortTask(const Ice::Current&)
{
if (!previousStep->finishedRunning())
{
@@ -83,24 +97,27 @@ namespace armarx::rngshortcut
taskIsAborted = true;
}
- void Task::run(const RemoteObjectNodePrxList& nodes, const Ice::Current&)
+ void
+ Task::run(const RemoteObjectNodePrxList& nodes, const Ice::Current&)
{
auto previousStep = MotionPlanningTaskPtr::dynamicCast(this->previousStep);
ARMARX_CHECK_EXPRESSION(previousStep);
previousStep->addTaskStatusCallback(
[this](TaskStatus::Status s)
- {
- if (s != TaskStatus::eDone)
{
- setTaskStatus(s);
- }
- }
- );
- auto endTime = std::chrono::high_resolution_clock::now() + std::chrono::seconds {getMaximalPlanningTimeInSeconds()};
+ if (s != TaskStatus::eDone)
+ {
+ setTaskStatus(s);
+ }
+ });
+ auto endTime = std::chrono::high_resolution_clock::now() +
+ std::chrono::seconds{getMaximalPlanningTimeInSeconds()};
previousStep->run(nodes);
- endTime = std::min(endTime, std::chrono::high_resolution_clock::now() + std::chrono::seconds {maxTimeForPostprocessingInSeconds});
+ endTime = std::min(endTime,
+ std::chrono::high_resolution_clock::now() +
+ std::chrono::seconds{maxTimeForPostprocessingInSeconds});
{
- std::lock_guard<std::mutex> lock {mtx};
+ std::lock_guard<std::mutex> lock{mtx};
paths.reserve(2);
auto p = previousStep->getPath();
paths.emplace_back(p.nodes, 0, "smooth_" + p.pathName);
@@ -120,7 +137,8 @@ namespace armarx::rngshortcut
{
auto& from = paths.front().nodes.at(i);
auto& to = paths.front().nodes.at(i + 1);
- lengths.emplace_back(lengths.back() + euclideanDistance(from.begin(), from.end(), to.begin()));
+ lengths.emplace_back(lengths.back() +
+ euclideanDistance(from.begin(), from.end(), to.begin()));
}
};
updateLengths();
@@ -145,11 +163,11 @@ namespace armarx::rngshortcut
getCSpace()->initCollisionTest();
- std::mt19937 gen {std::random_device{}()};
+ std::mt19937 gen{std::random_device{}()};
Ice::Long iteration = 0;
for (; iteration < maxTries;)
- //don't increment the iteration count here.
- // we only want to count iterations that do collision checking.
+ //don't increment the iteration count here.
+ // we only want to count iterations that do collision checking.
{
ARMARX_CHECK_EXPRESSION(paths.front().nodes.size() == lengths.size());
//stop?
@@ -179,9 +197,10 @@ namespace armarx::rngshortcut
std::size_t i = 0;
//search node i with: segmentPoints.first <= pathLenght(start,node_i)
- for (; i < lengths.size() && segmentOffsets.first > lengths.at(i); ++i);
+ for (; i < lengths.size() && segmentOffsets.first > lengths.at(i); ++i)
+ ;
takeLastOPTE = i;
- if (takeLastOPTE + 1 == lengths.size())
+ if (takeLastOPTE + 1 == lengths.size())
{
//already at the end
continue;
@@ -196,8 +215,10 @@ namespace armarx::rngshortcut
else
{
ARMARX_CHECK_EXPRESSION(takeLastOPTE);
- const auto fromSubSegLen = lengths.at(takeLastOPTE) - lengths.at(takeLastOPTE - 1);
- const auto fromSubSegLenPart = segmentOffsets.first - lengths.at(takeLastOPTE - 1);
+ const auto fromSubSegLen =
+ lengths.at(takeLastOPTE) - lengths.at(takeLastOPTE - 1);
+ const auto fromSubSegLenPart =
+ segmentOffsets.first - lengths.at(takeLastOPTE - 1);
if (fromSubSegLenPart < 1e-5)
{
//distance to prev node is low
@@ -209,26 +230,27 @@ namespace armarx::rngshortcut
{
const auto fromSubSegLenRatio = fromSubSegLenPart / fromSubSegLen;
- ARMARX_CHECK_EXPRESSION(from.size() == paths.front().nodes.at(takeLastOPTE - 1).size());
- ARMARX_CHECK_EXPRESSION(from.size() == paths.front().nodes.at(takeLastOPTE).size());
+ ARMARX_CHECK_EXPRESSION(from.size() ==
+ paths.front().nodes.at(takeLastOPTE - 1).size());
+ ARMARX_CHECK_EXPRESSION(from.size() ==
+ paths.front().nodes.at(takeLastOPTE).size());
- std::transform(
- paths.front().nodes.at(takeLastOPTE - 1).begin(),
- paths.front().nodes.at(takeLastOPTE - 1).end(),
- paths.front().nodes.at(takeLastOPTE).begin(),
- from.begin(),
- [fromSubSegLenRatio](float from, float to)
- {
- return (1.f - fromSubSegLenRatio) * from + fromSubSegLenRatio * to;
- }
- );
+ std::transform(paths.front().nodes.at(takeLastOPTE - 1).begin(),
+ paths.front().nodes.at(takeLastOPTE - 1).end(),
+ paths.front().nodes.at(takeLastOPTE).begin(),
+ from.begin(),
+ [fromSubSegLenRatio](float from, float to) {
+ return (1.f - fromSubSegLenRatio) * from +
+ fromSubSegLenRatio * to;
+ });
}
}
- for (; i < lengths.size() && segmentOffsets.second > lengths.at(i); ++i);
+ for (; i < lengths.size() && segmentOffsets.second > lengths.at(i); ++i)
+ ;
takeAgainFirstIdx = i;
ARMARX_CHECK_EXPRESSION(takeAgainFirstIdx);
- if (takeAgainFirstIdx >= lengths.size())
+ if (takeAgainFirstIdx >= lengths.size())
{
//to the end
takeAgainFirstIdx = paths.front().nodes.size();
@@ -236,10 +258,12 @@ namespace armarx::rngshortcut
}
else
{
- ARMARX_CHECK_EXPRESSION(takeAgainFirstIdx < lengths.size());
+ ARMARX_CHECK_EXPRESSION(takeAgainFirstIdx < lengths.size());
//to either some node in the middle
- const auto toSubSegLen = lengths.at(takeAgainFirstIdx) - lengths.at(takeAgainFirstIdx - 1);
- const auto toSubSegLenPart = segmentOffsets.second - lengths.at(takeAgainFirstIdx - 1);
+ const auto toSubSegLen =
+ lengths.at(takeAgainFirstIdx) - lengths.at(takeAgainFirstIdx - 1);
+ const auto toSubSegLenPart =
+ segmentOffsets.second - lengths.at(takeAgainFirstIdx - 1);
if (lengths.at(takeAgainFirstIdx) - toSubSegLenPart < 1e-5)
{
//snap to node in path
@@ -250,18 +274,17 @@ namespace armarx::rngshortcut
{
const auto toSubSegLenRatio = toSubSegLenPart / toSubSegLen;
ARMARX_CHECK_EXPRESSION(takeAgainFirstIdx > 0);
- ARMARX_CHECK_EXPRESSION(to.size() == paths.front().nodes.at(takeAgainFirstIdx - 1).size());
- ARMARX_CHECK_EXPRESSION(to.size() == paths.front().nodes.at(takeAgainFirstIdx).size());
+ ARMARX_CHECK_EXPRESSION(
+ to.size() == paths.front().nodes.at(takeAgainFirstIdx - 1).size());
+ ARMARX_CHECK_EXPRESSION(to.size() ==
+ paths.front().nodes.at(takeAgainFirstIdx).size());
std::transform(
paths.front().nodes.at(takeAgainFirstIdx - 1).begin(),
paths.front().nodes.at(takeAgainFirstIdx - 1).end(),
paths.front().nodes.at(takeAgainFirstIdx).begin(),
to.begin(),
[toSubSegLenRatio](float from, float to)
- {
- return (1.f - toSubSegLenRatio) * from + toSubSegLenRatio * to;
- }
- );
+ { return (1.f - toSubSegLenRatio) * from + toSubSegLenRatio * to; });
}
}
}
@@ -285,14 +308,19 @@ namespace armarx::rngshortcut
ARMARX_CHECK_EXPRESSION(takeLastOPTE < takeAgainFirstIdx);
VectorXfSeq newPostprocessedSolution;
- newPostprocessedSolution.reserve(paths.front().nodes.size() + 1); //max one node longer (upper bound for new length
- std::copy(paths.front().nodes.begin(), paths.front().nodes.begin() + takeLastOPTE, std::back_inserter(newPostprocessedSolution));
+ newPostprocessedSolution.reserve(paths.front().nodes.size() +
+ 1); //max one node longer (upper bound for new length
+ std::copy(paths.front().nodes.begin(),
+ paths.front().nodes.begin() + takeLastOPTE,
+ std::back_inserter(newPostprocessedSolution));
newPostprocessedSolution.emplace_back(std::move(from));
newPostprocessedSolution.emplace_back(std::move(to));
- std::copy(paths.front().nodes.begin() + takeAgainFirstIdx, paths.front().nodes.end(), std::back_inserter(newPostprocessedSolution));
+ std::copy(paths.front().nodes.begin() + takeAgainFirstIdx,
+ paths.front().nodes.end(),
+ std::back_inserter(newPostprocessedSolution));
{
- std::lock_guard<std::mutex> lock {mtx};
+ std::lock_guard<std::mutex> lock{mtx};
paths.front().nodes = std::move(newPostprocessedSolution);
}
//the path changed
@@ -302,30 +330,35 @@ namespace armarx::rngshortcut
setTaskStatus(TaskStatus::eDone);
}
- Ice::Long Task::getPathCount(const Ice::Current&) const
+ Ice::Long
+ Task::getPathCount(const Ice::Current&) const
{
- std::lock_guard<std::mutex> lock {mtx};
+ std::lock_guard<std::mutex> lock{mtx};
return paths.size();
}
- PathWithCost Task::getBestPath(const Ice::Current&) const
+ PathWithCost
+ Task::getBestPath(const Ice::Current&) const
{
return getNthPathWithCost(0);
}
- PathWithCost Task::getNthPathWithCost(Ice::Long n, const Ice::Current&) const
+ PathWithCost
+ Task::getNthPathWithCost(Ice::Long n, const Ice::Current&) const
{
- std::lock_guard<std::mutex> lock {mtx};
+ std::lock_guard<std::mutex> lock{mtx};
return paths.at(n);
}
- PathWithCostSeq Task::getAllPathsWithCost(const Ice::Current&) const
+ PathWithCostSeq
+ Task::getAllPathsWithCost(const Ice::Current&) const
{
- std::lock_guard<std::mutex> lock {mtx};
+ std::lock_guard<std::mutex> lock{mtx};
return paths;
}
- bool Task::isPathCollisionFree(const VectorXf& from, const VectorXf& to)
+ bool
+ Task::isPathCollisionFree(const VectorXf& from, const VectorXf& to)
{
// it is usually either a simox cspace or an adapter, if not use standard interpolation
@@ -333,7 +366,8 @@ namespace armarx::rngshortcut
SimoxCSpacePtr simoxcspace = SimoxCSpacePtr::dynamicCast(getCSpace());
if (!simoxcspace)
{
- CSpaceAdaptorBasePtr simoxcspaceadapter = CSpaceAdaptorBasePtr::dynamicCast(getCSpace());
+ CSpaceAdaptorBasePtr simoxcspaceadapter =
+ CSpaceAdaptorBasePtr::dynamicCast(getCSpace());
if (simoxcspaceadapter)
{
simoxcspace = SimoxCSpacePtr::dynamicCast(simoxcspaceadapter->getOriginalCSpace());
@@ -342,52 +376,54 @@ namespace armarx::rngshortcut
if (simoxcspace)
{
// do robot specific interpolation
- VirtualRobot::RobotNodeSetPtr rns = VirtualRobot::RobotNodeSet::createRobotNodeSet(simoxcspace->getAgentSceneObj(), "tmp",
- simoxcspace->getAgentJointNames());
- Saba::CSpaceSampled tmpCSpace(simoxcspace->getAgentSceneObj(), VirtualRobot::CDManagerPtr(new VirtualRobot::CDManager(simoxcspace->getCD())), rns);
- auto distance = [&, this](Eigen::VectorXf const & from, Eigen::VectorXf const & to)
- {
- return tmpCSpace.calcDist(from, to);
- };
+ VirtualRobot::RobotNodeSetPtr rns = VirtualRobot::RobotNodeSet::createRobotNodeSet(
+ simoxcspace->getAgentSceneObj(), "tmp", simoxcspace->getAgentJointNames());
+ Saba::CSpaceSampled tmpCSpace(
+ simoxcspace->getAgentSceneObj(),
+ VirtualRobot::CDManagerPtr(new VirtualRobot::CDManager(simoxcspace->getCD())),
+ rns);
+ auto distance = [&, this](Eigen::VectorXf const& from, Eigen::VectorXf const& to)
+ { return tmpCSpace.calcDist(from, to); };
- auto interpolation = [&, this](Eigen::VectorXf const & from, Eigen::VectorXf const & to, float step)
- {
- return tmpCSpace.interpolate(from, to, step);
- };
+ auto interpolation =
+ [&, this](Eigen::VectorXf const& from, Eigen::VectorXf const& to, float step)
+ { return tmpCSpace.interpolate(from, to, step); };
return dcdIsPathCollisionFree(
- from, to, dcdStep,
- [this](const VectorXf & cfg)
- {
- return getCSpace()->isCollisionFree({cfg.data(), cfg.data() + cfg.size()});
- },
- false,
- std::optional<decltype(distance)>(distance),
- std::optional<decltype(interpolation)>(interpolation)
- );
+ from,
+ to,
+ dcdStep,
+ [this](const VectorXf& cfg) {
+ return getCSpace()->isCollisionFree({cfg.data(), cfg.data() + cfg.size()});
+ },
+ false,
+ std::optional<decltype(distance)>(distance),
+ std::optional<decltype(interpolation)>(interpolation));
}
else
{
return dcdIsPathCollisionFree(
- from, to, dcdStep,
- [this](const VectorXf & cfg)
- {
- return getCSpace()->isCollisionFree({cfg.data(), cfg.data() + cfg.size()});
- },
- false
- );
+ from,
+ to,
+ dcdStep,
+ [this](const VectorXf& cfg) {
+ return getCSpace()->isCollisionFree({cfg.data(), cfg.data() + cfg.size()});
+ },
+ false);
}
-
}
- std::pair<float, float> Task::calcOffsets(float length, std::mt19937& gen)
+ std::pair<float, float>
+ Task::calcOffsets(float length, std::mt19937& gen)
{
- std::uniform_real_distribution<float> distA {0, std::nextafter(length, std::numeric_limits<float>::max())};
+ std::uniform_real_distribution<float> distA{
+ 0, std::nextafter(length, std::numeric_limits<float>::max())};
const auto a = distA(gen);
const auto minDelta = minPathImprovementRatio * length;
- std::uniform_real_distribution<float> distB {0, std::nextafter(length - 2 * minDelta, std::numeric_limits<float>::max())};
+ std::uniform_real_distribution<float> distB{
+ 0, std::nextafter(length - 2 * minDelta, std::numeric_limits<float>::max())};
const auto bSample = distB(gen);
const auto b = (bSample <= (a - minDelta)) ? bSample : bSample + 2 * minDelta;
return std::minmax(a, b);
}
-}
+} // namespace armarx::rngshortcut
diff --git a/source/RobotComponents/components/MotionPlanning/Tasks/RandomShortcutPostprocessor/Task.h b/source/RobotComponents/components/MotionPlanning/Tasks/RandomShortcutPostprocessor/Task.h
index 5bb3802d5b3b4c81a7d1308546ba6aa6ee11cac0..f710532d1e074f22c3ca56931d1a31bb6bc50af9 100644
--- a/source/RobotComponents/components/MotionPlanning/Tasks/RandomShortcutPostprocessor/Task.h
+++ b/source/RobotComponents/components/MotionPlanning/Tasks/RandomShortcutPostprocessor/Task.h
@@ -23,19 +23,18 @@
*/
#pragma once
-#include <mutex>
-#include <chrono>
#include <atomic>
+#include <chrono>
+#include <mutex>
#include <random>
#include <ArmarXCore/core/system/FactoryCollectionBase.h>
-
#include <ArmarXCore/core/util/distributed/RemoteHandle/RemoteHandle.h>
#include <ArmarXCore/interface/core/RemoteObjectNode.h>
#include <RobotComponents/interface/components/MotionPlanning/Tasks/RandomShortcutPostprocessor/Task.h>
-#include "../../util/Metrics.h"
+#include "../../util/Metrics.h"
#include "../MotionPlanningTask.h"
namespace armarx::rngshortcut
@@ -43,32 +42,34 @@ namespace armarx::rngshortcut
class Task;
using TaskPtr = IceInternal::Handle<Task>;
- class Task:
+ class Task :
public virtual PostprocessingMotionPlanningTask,
public virtual TaskBase,
public virtual MotionPlanningMultiPathWithCostTaskCI
{
public:
- Task(
- MotionPlanningTaskBasePtr previousStep,
- const std::string& taskName = "RandomShortcutPostprocessorTask",
- Ice::Long maxTimeForPostprocessingInSeconds = 6000,
- Ice::Float dcdStep = 0.01,
- Ice::Long maxTries = 10000,
- Ice::Float minShortcutImprovementRatio = 0.1,
- Ice::Float minPathImprovementRatio = 0.01
- );
-
-
- PathWithCost getPathWithCost(const Ice::Current& = Ice::emptyCurrent) const override
+ Task(MotionPlanningTaskBasePtr previousStep,
+ const std::string& taskName = "RandomShortcutPostprocessorTask",
+ Ice::Long maxTimeForPostprocessingInSeconds = 6000,
+ Ice::Float dcdStep = 0.01,
+ Ice::Long maxTries = 10000,
+ Ice::Float minShortcutImprovementRatio = 0.1,
+ Ice::Float minPathImprovementRatio = 0.01);
+
+ PathWithCost
+ getPathWithCost(const Ice::Current& = Ice::emptyCurrent) const override
{
return MotionPlanningMultiPathWithCostTaskCI::getPathWithCost();
}
- Path getNthPath(Ice::Long n, const Ice::Current& = Ice::emptyCurrent) const override
+
+ Path
+ getNthPath(Ice::Long n, const Ice::Current& = Ice::emptyCurrent) const override
{
return MotionPlanningMultiPathWithCostTaskCI::getNthPath(n);
}
- Path getPath(const Ice::Current& = Ice::emptyCurrent) const override
+
+ Path
+ getPath(const Ice::Current& = Ice::emptyCurrent) const override
{
return MotionPlanningWithCostTaskCI::getPath();
}
@@ -86,16 +87,19 @@ namespace armarx::rngshortcut
* @brief Runs the task.
* @param remoteNodes The list of \ref RemoteObjectNodeInterfacePrx used to distribute work to computers.
*/
- void run(const RemoteObjectNodePrxList& nodes, const Ice::Current& = Ice::emptyCurrent) override;
+ void run(const RemoteObjectNodePrxList& nodes,
+ const Ice::Current& = Ice::emptyCurrent) override;
- Ice::Long getMaximalPlanningTimeInSeconds(const Ice::Current& = Ice::emptyCurrent) const override
+ Ice::Long
+ getMaximalPlanningTimeInSeconds(const Ice::Current& = Ice::emptyCurrent) const override
{
return previousStep->getMaximalPlanningTimeInSeconds();
}
Ice::Long getPathCount(const Ice::Current& = Ice::emptyCurrent) const override;
PathWithCost getBestPath(const Ice::Current& = Ice::emptyCurrent) const override;
- PathWithCost getNthPathWithCost(Ice::Long n, const Ice::Current& = Ice::emptyCurrent) const override;
+ PathWithCost getNthPathWithCost(Ice::Long n,
+ const Ice::Current& = Ice::emptyCurrent) const override;
PathWithCostSeq getAllPathsWithCost(const Ice::Current& = Ice::emptyCurrent) const override;
bool isPathCollisionFree(const VectorXf& from, const VectorXf& to);
@@ -111,14 +115,18 @@ namespace armarx::rngshortcut
mutable std::mutex mtx;
Path postprocessedSolution;
PathWithCostSeq paths;
- std::atomic_bool taskIsAborted {false};
+ std::atomic_bool taskIsAborted{false};
+
private:
- template<class Base, class Derived> friend class ::armarx::GenericFactory;
+ template <class Base, class Derived>
+ friend class ::armarx::GenericFactory;
};
-}
+} // namespace armarx::rngshortcut
+
namespace armarx
{
using RandomShortcutPostprocessorTask = rngshortcut::Task;
using RandomShortcutPostprocessorTaskPtr = IceUtil::Handle<RandomShortcutPostprocessorTask>;
- using RandomShortcutPostprocessorTaskHandle = RemoteHandle<MotionPlanningMultiPathWithCostTaskControlInterfacePrx>;
-}
+ using RandomShortcutPostprocessorTaskHandle =
+ RemoteHandle<MotionPlanningMultiPathWithCostTaskControlInterfacePrx>;
+} // namespace armarx
diff --git a/source/RobotComponents/components/MotionPlanning/Tasks/SimoxTask/Task.cpp b/source/RobotComponents/components/MotionPlanning/Tasks/SimoxTask/Task.cpp
index 5e3b6c87e8dc2fafc26a50b5a4858842ac5446b4..e1904f37e5e8ca50ea419b6b95bbe8182f648c2e 100644
--- a/source/RobotComponents/components/MotionPlanning/Tasks/SimoxTask/Task.cpp
+++ b/source/RobotComponents/components/MotionPlanning/Tasks/SimoxTask/Task.cpp
@@ -30,7 +30,6 @@ namespace armarx
Task::Task()
{
-
}
} // namespace simoxtask
diff --git a/source/RobotComponents/components/MotionPlanning/Tasks/SimoxTask/Task.h b/source/RobotComponents/components/MotionPlanning/Tasks/SimoxTask/Task.h
index a05c5ed67796ad2c5edc8533d5b95ba45720fab4..ff430c27a5c9d1daaaccbb5062829f34146b221d 100644
--- a/source/RobotComponents/components/MotionPlanning/Tasks/SimoxTask/Task.h
+++ b/source/RobotComponents/components/MotionPlanning/Tasks/SimoxTask/Task.h
@@ -23,7 +23,6 @@
*/
#pragma once
-
namespace armarx::simoxtask
{
@@ -33,4 +32,4 @@ namespace armarx::simoxtask
Task();
};
-}
+} // namespace armarx::simoxtask
diff --git a/source/RobotComponents/components/MotionPlanning/util/CollisionCheckUtil.h b/source/RobotComponents/components/MotionPlanning/util/CollisionCheckUtil.h
index 92d0e51883acc065151478e10ac79cccb17377fe..025d06da377f618c779fe5fc084d0696628bd11d 100644
--- a/source/RobotComponents/components/MotionPlanning/util/CollisionCheckUtil.h
+++ b/source/RobotComponents/components/MotionPlanning/util/CollisionCheckUtil.h
@@ -23,12 +23,12 @@
*/
#pragma once
+#include <optional>
+
#include <Eigen/Core>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-#include <optional>
-
namespace armarx
{
/**
@@ -42,15 +42,19 @@ namespace armarx
* If no such point is reachable exactly from is returned.
* If to is reachable exactly to is returned.
*/
- template<class RealType, class CollisionChecker, class ConfigType>
- ConfigType dcdSteer(const ConfigType& from, const ConfigType& to, RealType dcdStepSize, CollisionChecker isCollisionFree)
+ template <class RealType, class CollisionChecker, class ConfigType>
+ ConfigType
+ dcdSteer(const ConfigType& from,
+ const ConfigType& to,
+ RealType dcdStepSize,
+ CollisionChecker isCollisionFree)
{
- using EigenVector = Eigen::Matrix< RealType, Eigen::Dynamic, 1>;
+ using EigenVector = Eigen::Matrix<RealType, Eigen::Dynamic, 1>;
ARMARX_CHECK_EXPRESSION(from.size() == to.size());
const auto size = static_cast<long int>(from.size()); //eigen wants long int
//get eigen views
- Eigen::Map<const EigenVector> eFrom {from.data(), size};
- Eigen::Map<const EigenVector> eTo {to.data(), size};
+ Eigen::Map<const EigenVector> eFrom{from.data(), size};
+ Eigen::Map<const EigenVector> eTo{to.data(), size};
EigenVector step = eTo - eFrom;
const auto length = step.norm();
@@ -58,12 +62,13 @@ namespace armarx
//will truncate in most cases => we will need to check the destination step for collision
ARMARX_CHECK_EXPRESSION(length / dcdStepSize < std::numeric_limits<RealType>::infinity());
- ARMARX_CHECK_EXPRESSION(length / dcdStepSize < static_cast<RealType>(std::numeric_limits<std::size_t>::max()));
+ ARMARX_CHECK_EXPRESSION(length / dcdStepSize <
+ static_cast<RealType>(std::numeric_limits<std::size_t>::max()));
const std::size_t stepsToTake = static_cast<std::size_t>(length / dcdStepSize);
ConfigType stepToCheck(from);
ARMARX_CHECK_EXPRESSION(stepToCheck.size() == from.size());
- Eigen::Map<EigenVector> eStepToCheck {stepToCheck.data(), size};
+ Eigen::Map<EigenVector> eStepToCheck{stepToCheck.data(), size};
std::size_t stepsTaken = 0;
for (; stepsTaken < stepsToTake; ++stepsTaken)
@@ -97,9 +102,6 @@ namespace armarx
return stepToCheck;
}
-
-
-
//startCfg is assumed to be collision free
/**
* @brief Returns whether the line startCfg to to is collision free.
@@ -110,26 +112,37 @@ namespace armarx
* @param toIsCollisionFree Whether to is collision free
* @return Whether the line startCfg to to is collision free.
*/
- template<class RealType, class CollisionChecker, class ConfigType, class Distance = std::function<float(Eigen::VectorXf, Eigen::VectorXf)>,
- class Interpolate = std::function<Eigen::VectorXf(Eigen::VectorXf, Eigen::VectorXf, float)>>
- bool dcdIsPathCollisionFree(const ConfigType& from, const ConfigType& to, RealType dcdStepSize, CollisionChecker isCollisionFree, bool toIsCollisionFree = true,
- const std::optional<Distance>& distanceLambda = std::optional<Distance>(),
- const std::optional<Interpolate>& interpolationLambda = std::optional<Interpolate>(), std::vector<ConfigType>* resultPath = NULL)
+ template <
+ class RealType,
+ class CollisionChecker,
+ class ConfigType,
+ class Distance = std::function<float(Eigen::VectorXf, Eigen::VectorXf)>,
+ class Interpolate = std::function<Eigen::VectorXf(Eigen::VectorXf, Eigen::VectorXf, float)>>
+ bool
+ dcdIsPathCollisionFree(
+ const ConfigType& from,
+ const ConfigType& to,
+ RealType dcdStepSize,
+ CollisionChecker isCollisionFree,
+ bool toIsCollisionFree = true,
+ const std::optional<Distance>& distanceLambda = std::optional<Distance>(),
+ const std::optional<Interpolate>& interpolationLambda = std::optional<Interpolate>(),
+ std::vector<ConfigType>* resultPath = NULL)
{
if (!(toIsCollisionFree || isCollisionFree(to)))
{
return false;
}
- using EigenVector = Eigen::Matrix< RealType, Eigen::Dynamic, 1>;
+ using EigenVector = Eigen::Matrix<RealType, Eigen::Dynamic, 1>;
ARMARX_CHECK_EXPRESSION(from.size() == to.size());
const auto size = static_cast<long int>(from.size()); //eigen wants long int
//get eigen views
- Eigen::Map<const EigenVector> eFrom {from.data(), size};
- Eigen::Map<const EigenVector> eTo {to.data(), size};
+ Eigen::Map<const EigenVector> eFrom{from.data(), size};
+ Eigen::Map<const EigenVector> eTo{to.data(), size};
EigenVector connecting = eTo - eFrom;
std::vector<float> tmp(size);
- Eigen::Map<EigenVector> eTmp {tmp.data(), size};
+ Eigen::Map<EigenVector> eTmp{tmp.data(), size};
float distanceFromTo;
if (distanceLambda)
{
@@ -139,7 +152,8 @@ namespace armarx
{
distanceFromTo = connecting.norm();
}
- ARMARX_INFO << deactivateSpam(1) << VAROUT(dcdStepSize) << VAROUT(distanceFromTo) << VAROUT(eFrom) << VAROUT(eTo);
+ ARMARX_INFO << deactivateSpam(1) << VAROUT(dcdStepSize) << VAROUT(distanceFromTo)
+ << VAROUT(eFrom) << VAROUT(eTo);
if (distanceFromTo <= dcdStepSize)
{
if (resultPath)
@@ -184,10 +198,9 @@ namespace armarx
}
//increment steps
stepsToCheck = stepsToCheck << 1;
- }
- while (checkedStepFactor > dcdAsDistanceFactor);
+ } while (checkedStepFactor > dcdAsDistanceFactor);
//checked everything and no collision
return true;
}
-}
+} // namespace armarx
diff --git a/source/RobotComponents/components/MotionPlanning/util/HashingUtil.h b/source/RobotComponents/components/MotionPlanning/util/HashingUtil.h
index 2799fbdb7d5325e371c0a05c598ca6504fb47f22..41f0b97297d90e95cb093b9615e7316b60947d4a 100644
--- a/source/RobotComponents/components/MotionPlanning/util/HashingUtil.h
+++ b/source/RobotComponents/components/MotionPlanning/util/HashingUtil.h
@@ -46,19 +46,20 @@
* ARMARX_OVERLOAD_STD_HASH((foo), (arg.a, arg.b))
* The parentheses are required!
*/
-#define ARMARX_OVERLOAD_STD_HASH(Type, Members)\
- namespace std\
- {\
- template<>\
- struct hash< ARMARX_STRIP_PARENTHESES(Type) >\
- {\
- using argument_type = ARMARX_STRIP_PARENTHESES(Type);\
- using result_type = std::size_t;\
- result_type operator()(argument_type const& arg) const\
- {\
- return armarx::hash_all( ARMARX_STRIP_PARENTHESES(Members) );\
- }\
- };\
+#define ARMARX_OVERLOAD_STD_HASH(Type, Members) \
+ namespace std \
+ { \
+ template <> \
+ struct hash<ARMARX_STRIP_PARENTHESES(Type)> \
+ { \
+ using argument_type = ARMARX_STRIP_PARENTHESES(Type); \
+ using result_type = std::size_t; \
+ result_type \
+ operator()(argument_type const& arg) const \
+ { \
+ return armarx::hash_all(ARMARX_STRIP_PARENTHESES(Members)); \
+ } \
+ }; \
}
/**
@@ -70,19 +71,20 @@
* for vector<int, AllocatorType> : ARMARX_OVERLOAD_STD_HASH_FOR_ITERABLE((), (vector<int, AllocatorType>))
* for all vectors: ARMARX_OVERLOAD_STD_HASH_FOR_ITERABLE((class T, class A), (vector<T, A>))
*/
-#define ARMARX_OVERLOAD_STD_HASH_FOR_ITERABLE(TemplateArgs, Type)\
- namespace std\
- {\
- template< ARMARX_STRIP_PARENTHESES(TemplateArgs) >\
- struct hash< ARMARX_STRIP_PARENTHESES(Type) >\
- {\
- using argument_type = ARMARX_STRIP_PARENTHESES(Type);\
- using result_type = std::size_t;\
- result_type operator()(argument_type const& arg) const\
- {\
- return boost::hash_range(arg.begin(), arg.end());\
- }\
- };\
+#define ARMARX_OVERLOAD_STD_HASH_FOR_ITERABLE(TemplateArgs, Type) \
+ namespace std \
+ { \
+ template <ARMARX_STRIP_PARENTHESES(TemplateArgs)> \
+ struct hash<ARMARX_STRIP_PARENTHESES(Type)> \
+ { \
+ using argument_type = ARMARX_STRIP_PARENTHESES(Type); \
+ using result_type = std::size_t; \
+ result_type \
+ operator()(argument_type const& arg) const \
+ { \
+ return boost::hash_range(arg.begin(), arg.end()); \
+ } \
+ }; \
}
namespace armarx
@@ -92,8 +94,9 @@ namespace armarx
* @param hashed The accumulated hash.
* @param t0 The value to hash.
*/
- template<class T0>
- void do_hash(std::size_t& hashed, const T0& t0)
+ template <class T0>
+ void
+ do_hash(std::size_t& hashed, const T0& t0)
{
//replace this call to change combination methode of hash values and the used hash function
//(currently uses boost::hash_value)
@@ -106,8 +109,9 @@ namespace armarx
* @param t0 The first value to hash.
* @param ts Further values to hash.
*/
- template<class T0, class...Ts>
- void do_hash(std::size_t& hashed, const T0& t0, const Ts& ...ts)
+ template <class T0, class... Ts>
+ void
+ do_hash(std::size_t& hashed, const T0& t0, const Ts&... ts)
{
do_hash(hashed, t0);
do_hash(hashed, ts...);
@@ -118,23 +122,25 @@ namespace armarx
* @param ts The parameters to hash
* @return The hash of all parameters.
*/
- template<class...Ts>
- std::size_t hash_all(const Ts& ...ts)
+ template <class... Ts>
+ std::size_t
+ hash_all(const Ts&... ts)
{
std::size_t hashed = 0;
do_hash(hashed, ts...);
return hashed;
}
-}
+} // namespace armarx
//basic overloads from boost
#include <tuple>
+
namespace std
{
/**
* @brief Enables hashing of std::tuple.
*/
- template<class...Ts>
+ template <class... Ts>
struct hash<tuple<Ts...>>
{
/**
@@ -145,25 +151,28 @@ namespace std
* @brief the hash operator's result type
*/
using result_type = std::size_t;
+
/**
* @brief Hashes a tuple
* @param the tuple to hash
* @return The tuple's hash
*/
- result_type operator()(argument_type const& arg) const
+ result_type
+ operator()(argument_type const& arg) const
{
return hash_all(arg);
}
};
-}
+} // namespace std
#include <utility>
+
namespace std
{
/**
* @brief Enables hashing of std::pair.
*/
- template<class A, class B>
+ template <class A, class B>
struct hash<pair<A, B>>
{
/**
@@ -180,12 +189,13 @@ namespace std
* @param the pair to hash
* @return The pair's hash
*/
- result_type operator()(argument_type const& arg) const
+ result_type
+ operator()(argument_type const& arg) const
{
return armarx::hash_all(arg.first, arg.second);
}
};
-}
+} // namespace std
#include <vector>
/**
@@ -224,4 +234,3 @@ ARMARX_OVERLOAD_STD_HASH_FOR_ITERABLE((class K, class V, class C, class A), (map
* @brief Enables hashing of std::multimap.
*/
ARMARX_OVERLOAD_STD_HASH_FOR_ITERABLE((class K, class V, class C, class A), (multimap<K, V, C, A>))
-
diff --git a/source/RobotComponents/components/MotionPlanning/util/Metrics.h b/source/RobotComponents/components/MotionPlanning/util/Metrics.h
index 37e9f623c5fa5d0ccb96fd9670d3fab1ff244a8b..0cc2ca887c8b2bd1fcc96881ecb7b4da413e5502 100644
--- a/source/RobotComponents/components/MotionPlanning/util/Metrics.h
+++ b/source/RobotComponents/components/MotionPlanning/util/Metrics.h
@@ -36,8 +36,12 @@ namespace armarx
* @param firstw First weight.
* @return The squared euclidean distance with weighted with the given vector.
*/
- template<class IteratorType1, class IteratorType2, class IteratorType3>
- float euclideanDistanceWeightedSquared(IteratorType1 first1, IteratorType1 last1, IteratorType2 first2, IteratorType3 firstw)
+ template <class IteratorType1, class IteratorType2, class IteratorType3>
+ float
+ euclideanDistanceWeightedSquared(IteratorType1 first1,
+ IteratorType1 last1,
+ IteratorType2 first2,
+ IteratorType3 firstw)
{
float akk = 0;
@@ -56,8 +60,9 @@ namespace armarx
* @param first2 First value of the second vector.
* @return The squared euclidean distance.
*/
- template<class IteratorType1, class IteratorType2>
- float euclideanDistanceSquared(IteratorType1 first1, IteratorType1 last1, IteratorType2 first2)
+ template <class IteratorType1, class IteratorType2>
+ float
+ euclideanDistanceSquared(IteratorType1 first1, IteratorType1 last1, IteratorType2 first2)
{
float akk = 0;
@@ -77,8 +82,12 @@ namespace armarx
* @param firstw First weight.
* @return The euclidean distance with weighted with the given vector.
*/
- template<class IteratorType>
- float euclideanDistanceWeighted(IteratorType first1, IteratorType last1, IteratorType first2, IteratorType firstw)
+ template <class IteratorType>
+ float
+ euclideanDistanceWeighted(IteratorType first1,
+ IteratorType last1,
+ IteratorType first2,
+ IteratorType firstw)
{
return std::sqrt(euclideanDistanceWeightedSquared(first1, last1, first2, firstw));
}
@@ -90,9 +99,10 @@ namespace armarx
* @param first2 First value of the second vector.
* @return The euclidean distance.
*/
- template<class IteratorType1, class IteratorType2>
- float euclideanDistance(IteratorType1 first1, IteratorType1 last1, IteratorType2 first2)
+ template <class IteratorType1, class IteratorType2>
+ float
+ euclideanDistance(IteratorType1 first1, IteratorType1 last1, IteratorType2 first2)
{
return std::sqrt(euclideanDistanceSquared(first1, last1, first2));
}
-}
+} // namespace armarx
diff --git a/source/RobotComponents/components/MotionPlanning/util/PlanningUtil.cpp b/source/RobotComponents/components/MotionPlanning/util/PlanningUtil.cpp
index 99f6d4be30964901af79eacf766221de4424cadf..3c68628b9ab07d599ac608f468e1403daa973e83 100644
--- a/source/RobotComponents/components/MotionPlanning/util/PlanningUtil.cpp
+++ b/source/RobotComponents/components/MotionPlanning/util/PlanningUtil.cpp
@@ -21,13 +21,14 @@
* @copyright http://www.gnu.org/licenses/gpl.txt
* GNU General Public License
*/
-#include <stdexcept>
-
#include "PlanningUtil.h"
+#include <stdexcept>
+
namespace armarx::TaskStatus
{
- std::string toString(Status status)
+ std::string
+ toString(Status status)
{
switch (status)
{
@@ -64,7 +65,8 @@ namespace armarx::TaskStatus
}
//status information
- bool isRunning(Status status)
+ bool
+ isRunning(Status status)
{
switch (status)
{
@@ -88,10 +90,11 @@ namespace armarx::TaskStatus
//this point should never be reached!
//if it is a new status was added without changing this function
- throw std::logic_error {__FILE__ " in line " + std::to_string(__LINE__) + toString(status)};
+ throw std::logic_error{__FILE__ " in line " + std::to_string(__LINE__) + toString(status)};
}
- bool finishedRunning(Status status)
+ bool
+ finishedRunning(Status status)
{
switch (status)
{
@@ -115,11 +118,12 @@ namespace armarx::TaskStatus
//this point should never be reached!
//if it is a new status was added without changing this function
- throw std::logic_error {__FILE__ " in line " + std::to_string(__LINE__) + toString(status)};
+ throw std::logic_error{__FILE__ " in line " + std::to_string(__LINE__) + toString(status)};
}
//status transition
- Status transitionAtKill(Status status)
+ Status
+ transitionAtKill(Status status)
{
switch (status)
{
@@ -143,11 +147,13 @@ namespace armarx::TaskStatus
}
std::stringstream ss;
- ss << "planningStatusTransitionAtKill: transition from " << toString(status) << " is illegal";
- throw std::invalid_argument {ss.str()};
+ ss << "planningStatusTransitionAtKill: transition from " << toString(status)
+ << " is illegal";
+ throw std::invalid_argument{ss.str()};
}
- Status transitionAtOutoftime(Status status)
+ Status
+ transitionAtOutoftime(Status status)
{
switch (status)
{
@@ -171,11 +177,13 @@ namespace armarx::TaskStatus
}
std::stringstream ss;
- ss << "planningStatusTransitionAtOutoftime: transition from " << toString(status) << " is illegal";
- throw std::invalid_argument {ss.str()};
+ ss << "planningStatusTransitionAtOutoftime: transition from " << toString(status)
+ << " is illegal";
+ throw std::invalid_argument{ss.str()};
}
- Status transitionAtDone(Status status)
+ Status
+ transitionAtDone(Status status)
{
switch (status)
{
@@ -198,8 +206,8 @@ namespace armarx::TaskStatus
}
std::stringstream ss;
- ss << "planningStatusTransitionAtDone: transition from " << toString(status) << " is illegal";
- throw std::invalid_argument {ss.str()};
+ ss << "planningStatusTransitionAtDone: transition from " << toString(status)
+ << " is illegal";
+ throw std::invalid_argument{ss.str()};
}
-}
-
+} // namespace armarx::TaskStatus
diff --git a/source/RobotComponents/components/MotionPlanning/util/PlanningUtil.h b/source/RobotComponents/components/MotionPlanning/util/PlanningUtil.h
index 43c7f012958889758f79bd2bf2b09f6debe24502..de47007a4adf03d71236f24749b6571b5a54deb2 100644
--- a/source/RobotComponents/components/MotionPlanning/util/PlanningUtil.h
+++ b/source/RobotComponents/components/MotionPlanning/util/PlanningUtil.h
@@ -23,8 +23,8 @@
*/
#pragma once
-#include <string>
#include <chrono>
+#include <string>
#include <RobotComponents/interface/components/MotionPlanning/DataStructures.h>
@@ -49,7 +49,7 @@ namespace armarx::TaskStatus
* @param status The status to check.
* @return Whether the given task status describes a state where planning is done (may be failed)
*/
- bool finishedRunning(Status status) ;
+ bool finishedRunning(Status status);
//status transition
/**
@@ -71,5 +71,4 @@ namespace armarx::TaskStatus
*/
Status transitionAtDone(Status status);
-}
-
+} // namespace armarx::TaskStatus
diff --git a/source/RobotComponents/components/MotionPlanning/util/Samplers.h b/source/RobotComponents/components/MotionPlanning/util/Samplers.h
index 2072bf3d37a17b354492b3fab02d9cf479e848d5..5a6ffe6f931324ec3c2402acf7141eecde0515bb 100644
--- a/source/RobotComponents/components/MotionPlanning/util/Samplers.h
+++ b/source/RobotComponents/components/MotionPlanning/util/Samplers.h
@@ -24,26 +24,26 @@
#pragma once
#include <random>
+#include <tuple>
#include <type_traits>
#include <vector>
-#include <tuple>
#include <Eigen/Core>
-#include <Eigen/SVD>
#include <Eigen/LU>
-
-#include "Volume.h"
-#include "Metrics.h"
+#include <Eigen/SVD>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include "Metrics.h"
+#include "Volume.h"
+
namespace armarx
{
/**
* @brief Stores a distribution and a generator.
* The call operator passes the generator to the distribution
*/
- template<class Distribution, class Generator>
+ template <class Distribution, class Generator>
class Sampler
{
public:
@@ -54,7 +54,8 @@ namespace armarx
/**
* @brief Type of the stored generator
*/
- using GeneratorType = Generator ;
+ using GeneratorType = Generator;
+
protected:
/**
* @brief The stored distribution.
@@ -64,15 +65,14 @@ namespace armarx
* @brief The stored generator
*/
Generator generator;
+
public:
/**
* @brief Ctor
* @param dist The distribution to store.
* @param gen The generator to store.
*/
- Sampler(Distribution&& dist, Generator&& gen):
- distribution {dist},
- generator {gen}
+ Sampler(Distribution&& dist, Generator&& gen) : distribution{dist}, generator{gen}
{
}
@@ -81,9 +81,9 @@ namespace armarx
* @param args Additional parameters to pass to the distribution.
* @return The return value of the distribution.
*/
- template<class...Args>
- auto operator()(Args&& ...args)
- ->decltype(distribution(std::forward<Args>(args)..., generator))
+ template <class... Args>
+ auto
+ operator()(Args&&... args) -> decltype(distribution(std::forward<Args>(args)..., generator))
{
return distribution(std::forward<Args>(args)..., generator);
}
@@ -92,7 +92,8 @@ namespace armarx
* @brief Returns the stored distribution.
* @return The stored distribution.
*/
- Distribution& getDistribution()
+ Distribution&
+ getDistribution()
{
return distribution;
}
@@ -101,7 +102,8 @@ namespace armarx
* @brief Returns the stored generator.
* @return The stored generator.
*/
- Generator& getGenerator()
+ Generator&
+ getGenerator()
{
return generator;
}
@@ -112,15 +114,14 @@ namespace armarx
*
* (using the method described in doi>10.1145/377939.377946 (https://dl.acm.org/citation.cfm?doid=377939.377946))
*/
- template<class RealType>
+ template <class RealType>
class UniformUnitSphereSurfaceDistribution
{
public:
/**
* @brief Ctor
*/
- UniformUnitSphereSurfaceDistribution():
- normDist {0.0, 0.1}
+ UniformUnitSphereSurfaceDistribution() : normDist{0.0, 0.1}
{
}
@@ -130,8 +131,9 @@ namespace armarx
* @param last One past the last value to fill.
* @param gen The generator to use.
*/
- template<class Iterator, class Generator>
- void operator()(const Iterator& first, const Iterator& last, Generator& gen)
+ template <class Iterator, class Generator>
+ void
+ operator()(const Iterator& first, const Iterator& last, Generator& gen)
{
RealType length = 0;
@@ -148,6 +150,7 @@ namespace armarx
*it /= length;
}
}
+
protected:
/**
* @brief The internally used normal distribution.
@@ -160,16 +163,14 @@ namespace armarx
*
* https://math.stackexchange.com/questions/87230/picking-random-points-in-the-volume-of-sphere-with-uniform-probability/87238#87238
*/
- template<class RealType>
+ template <class RealType>
class UniformUnitSphereDistribution
{
public:
/**
* @brief Ctor
*/
- UniformUnitSphereDistribution():
- normDist {0.0, 0.1},
- radDist {0.0, 1.0}
+ UniformUnitSphereDistribution() : normDist{0.0, 0.1}, radDist{0.0, 1.0}
{
}
@@ -182,8 +183,9 @@ namespace armarx
* @param last One past the last value to fill.
* @param gen The generator to use.
*/
- template<class Iterator, class Generator>
- void operator()(const Iterator& first, const Iterator& last, Generator& gen)
+ template <class Iterator, class Generator>
+ void
+ operator()(const Iterator& first, const Iterator& last, Generator& gen)
{
const RealType dim = std::distance(first, last);
RealType length = 0;
@@ -202,6 +204,7 @@ namespace armarx
*it = ((*it) / length) * scalingFactor;
}
}
+
protected:
/**
* @brief The internal used normal distribution for samples of the sphere surfaces.
@@ -216,7 +219,7 @@ namespace armarx
/**
* @brief Uniform distribution of an cuboid space.
*/
- template<class RealType>
+ template <class RealType>
class UniformCuboidDistribution
{
public:
@@ -231,7 +234,7 @@ namespace armarx
* @param boundsFirst The bounds for the first dimension.
* @param boundsLast One past the bounds for the last dimension.
*/
- template<class Iterator>
+ template <class Iterator>
UniformCuboidDistribution(Iterator boundsFirst, Iterator boundsLast)
{
volume = 1;
@@ -244,8 +247,7 @@ namespace armarx
//set distribution
spaceDist.emplace_back(
boundsFirst->first,
- std::nextafter(boundsFirst->second, std::numeric_limits<RealType>::max())
- );
+ std::nextafter(boundsFirst->second, std::numeric_limits<RealType>::max()));
}
}
@@ -255,8 +257,9 @@ namespace armarx
* @param first The first value to fill.
* @param gen The used generator.
*/
- template<class Iterator, class Generator>
- void operator()(Iterator first, Generator& gen)
+ template <class Iterator, class Generator>
+ void
+ operator()(Iterator first, Generator& gen)
{
for (std::size_t i = 0; i < spaceDist.size(); ++i, ++first)
{
@@ -270,8 +273,9 @@ namespace armarx
* @param first The first value of the point.
* @return Whether the point is contained by the cuboid.
*/
- template<class Iterator>
- bool isInBounds(Iterator first)
+ template <class Iterator>
+ bool
+ isInBounds(Iterator first)
{
for (std::size_t i = 0; i < spaceDist.size(); ++i, ++first)
{
@@ -288,7 +292,8 @@ namespace armarx
* @brief Returns the cuboid's volume.
* @return The cuboid's volume.
*/
- RealType getVolume() const
+ RealType
+ getVolume() const
{
return volume;
}
@@ -297,11 +302,13 @@ namespace armarx
* @brief Returns the cuboid's dimensionality.
* @return The cuboid's dimensionality.
*/
- template<class T = std::size_t>
- T getDimensionality() const
+ template <class T = std::size_t>
+ T
+ getDimensionality() const
{
return static_cast<T>(spaceDist.size());
}
+
protected:
/**
* @brief The cuboid's volume.
@@ -314,10 +321,11 @@ namespace armarx
};
using CuboidSampler = Sampler<UniformCuboidDistribution<float>, std::mt19937>;
+
/**
* @brief Uniform distribution of a prolate hyper spheroid.
*/
- template<class RealType = float>
+ template <class RealType = float>
class UniformProlateSpheroidDistribution
{
protected:
@@ -325,11 +333,12 @@ namespace armarx
/**
* @brief Internal used Eigen vector type.
*/
- using VecType = Eigen::Matrix<RealType, Eigen::Dynamic, 1 >;
+ using VecType = Eigen::Matrix<RealType, Eigen::Dynamic, 1>;
/**
* @brief Internal used Eigen matrix type.
*/
using MatType = Eigen::Matrix<RealType, Eigen::Dynamic, Eigen::Dynamic>;
+
public:
/**
* @brief Constructs a uiform distribution of a prolate hyper spheroid with the focal points [focalAFirst, focalALast)
@@ -338,21 +347,20 @@ namespace armarx
* @param focalALast One past the last value of the first focal point.
* @param focalBFirst The first value of the second focal point.
*/
- template<class ValIter>
- UniformProlateSpheroidDistribution(
- ValIter focalAFirst, ValIter focalALast,
- ValIter focalBFirst
- ):
- sphereDistribution {},
+ template <class ValIter>
+ UniformProlateSpheroidDistribution(ValIter focalAFirst,
+ ValIter focalALast,
+ ValIter focalBFirst) :
+ sphereDistribution{},
tmpVal(std::distance(focalAFirst, focalALast)),
//distanceFocalPoints,
- polarDiameter {std::numeric_limits<RealType>::infinity()},
- volumeProlateSpheroid {std::numeric_limits<RealType>::infinity()},
+ polarDiameter{std::numeric_limits<RealType>::infinity()},
+ volumeProlateSpheroid{std::numeric_limits<RealType>::infinity()},
focalPointA(getDimensionality()),
focalPointB(getDimensionality()),
centre(getDimensionality()),
- rotation {getDimensionality(), getDimensionality()},
- scaleThenRotate {getDimensionality(), getDimensionality()}
+ rotation{getDimensionality(), getDimensionality()},
+ scaleThenRotate{getDimensionality(), getDimensionality()}
{
setFocalPoints(focalAFirst, focalBFirst);
}
@@ -366,28 +374,29 @@ namespace armarx
* @param rotationMatrix The used rotation matrix (the values should be ordered to be conform with eigens mapping of matrices to vectors.
* (as defined by Eigen::Map<Eigen::Matrix<RealType, dimensionality, dimensionality>>)
*/
- template<class ValIter>
- UniformProlateSpheroidDistribution(
- ValIter focalAFirst, ValIter focalALast,
- ValIter focalBFirst,
- const std::vector<RealType>& rotationMatrix
- ):
- sphereDistribution {},
+ template <class ValIter>
+ UniformProlateSpheroidDistribution(ValIter focalAFirst,
+ ValIter focalALast,
+ ValIter focalBFirst,
+ const std::vector<RealType>& rotationMatrix) :
+ sphereDistribution{},
tmpVal(std::distance(focalAFirst, focalALast)),
//distanceFocalPoints,
- polarDiameter {std::numeric_limits<RealType>::infinity()},
- volumeProlateSpheroid {std::numeric_limits<RealType>::infinity()},
+ polarDiameter{std::numeric_limits<RealType>::infinity()},
+ volumeProlateSpheroid{std::numeric_limits<RealType>::infinity()},
focalPointA(getDimensionality()),
focalPointB(getDimensionality()),
centre(getDimensionality()),
- rotation {getDimensionality(), getDimensionality()},
- scaleThenRotate {getDimensionality(), getDimensionality()}
+ rotation{getDimensionality(), getDimensionality()},
+ scaleThenRotate{getDimensionality(), getDimensionality()}
{
setFocalPoints(focalAFirst, focalBFirst, false);
- ARMARX_CHECK_EXPRESSION(rotationMatrix.size() == getDimensionality() * getDimensionality());
+ ARMARX_CHECK_EXPRESSION(rotationMatrix.size() ==
+ getDimensionality() * getDimensionality());
- rotation = Eigen::Map<const MatType>(rotationMatrix.data(), getDimensionality(), getDimensionality());
+ rotation = Eigen::Map<const MatType>(
+ rotationMatrix.data(), getDimensionality(), getDimensionality());
}
/**
@@ -395,11 +404,12 @@ namespace armarx
* @param first The first value to fill.
* @param gen The used generator.
*/
- template<class Generator>
- void operator()(RealType* first, Generator& gen)
+ template <class Generator>
+ void
+ operator()(RealType* first, Generator& gen)
{
sphereDistribution(tmpVal.begin(), tmpVal.end(), gen);
- Eigen::Map<VecType> tmpVec {tmpVal.data(), getDimensionality<long int>()};
+ Eigen::Map<VecType> tmpVec{tmpVal.data(), getDimensionality<long int>()};
Eigen::Map<VecType>(first, getDimensionality()) = scaleThenRotate * tmpVec + centre;
ARMARX_CHECK_EXPRESSION(isInBounds(first));
@@ -409,8 +419,9 @@ namespace armarx
* @brief Returns the dimensionality of the distribution.
* @return The dimensionality of the distribution.
*/
- template<class T = std::size_t>
- T getDimensionality() const
+ template <class T = std::size_t>
+ T
+ getDimensionality() const
{
return static_cast<T>(tmpVal.size());
}
@@ -420,7 +431,8 @@ namespace armarx
* @param first The first value of the point to check.
* @return Whether [first, first + dimensionality) is a point contained by the prolate hyper spheroid.
*/
- bool isInBounds(RealType* first) const
+ bool
+ isInBounds(RealType* first) const
{
const auto srcVec = Eigen::Map<VecType>(first, getDimensionality());
//dont use std::numeric_limits<RealType>::epsilon() since its about 1e-7 and all the floating errors add up
@@ -432,7 +444,8 @@ namespace armarx
* @brief Returns the prolate hyper spheroid's volume.
* @return The prolate hyper spheroid's volume.
*/
- RealType getVolume() const
+ RealType
+ getVolume() const
{
return volumeProlateSpheroid;
}
@@ -441,7 +454,8 @@ namespace armarx
* @brief Returns the distance between the prolate hyper spheroid's focal points.
* @return The distance between the prolate hyper spheroid's focal points.
*/
- RealType getDistanceFocalPoints() const
+ RealType
+ getDistanceFocalPoints() const
{
return distanceFocalPoints;
}
@@ -450,7 +464,8 @@ namespace armarx
* @brief Returns the prolate hyper spheroid's polar diameter.
* @return The prolate hyper spheroid's polar diameter.
*/
- RealType getPolarDiameter() const
+ RealType
+ getPolarDiameter() const
{
return polarDiameter;
}
@@ -459,22 +474,25 @@ namespace armarx
* @brief Sets the prolate hyper spheroid's polar diameter.
* @param The new prolate hyper spheroid's polar diameter.
*/
- void setPolarDiameter(RealType newPolarDiameter)
+ void
+ setPolarDiameter(RealType newPolarDiameter)
{
ARMARX_CHECK_EXPRESSION(newPolarDiameter < std::numeric_limits<RealType>::infinity());
- Eigen::Map<VecType> tmpVec {tmpVal.data(), getDimensionality<long int>()};
+ Eigen::Map<VecType> tmpVec{tmpVal.data(), getDimensionality<long int>()};
tmpVec(0) = newPolarDiameter / 2.0;
const auto polarDiameterQuad = std::pow(newPolarDiameter, 2.0);
const auto distanceFocalPointsQuad = std::pow(distanceFocalPoints, 2.0);
- const auto equatorialRadius = std::sqrt(polarDiameterQuad - distanceFocalPointsQuad) / 2.0;
+ const auto equatorialRadius =
+ std::sqrt(polarDiameterQuad - distanceFocalPointsQuad) / 2.0;
for (std::size_t i = 1; i < getDimensionality(); ++i) //requires this-> (2 phase lookup)
{
tmpVec(i) = equatorialRadius;
}
- volumeProlateSpheroid = volumeOfHyperspheroid<RealType>(getDimensionality(), newPolarDiameter / 2.0, equatorialRadius);
+ volumeProlateSpheroid = volumeOfHyperspheroid<RealType>(
+ getDimensionality(), newPolarDiameter / 2.0, equatorialRadius);
//calculate compound transformation
scaleThenRotate = rotation * tmpVec.asDiagonal();
polarDiameter = newPolarDiameter;
@@ -484,9 +502,10 @@ namespace armarx
* @brief Returns the rotation matrix stored in a vector.
* @return The rotation matrix stored in a vector.
*/
- std::vector<RealType> getRotationMatrix() const
+ std::vector<RealType>
+ getRotationMatrix() const
{
- std::vector<RealType> copy(getDimensionality()*getDimensionality());
+ std::vector<RealType> copy(getDimensionality() * getDimensionality());
Eigen::Map<MatType>(copy.data(), getDimensionality(), getDimensionality()) = rotation;
return copy;
}
@@ -498,10 +517,13 @@ namespace armarx
* @param focalBFirst The first value of the second focal point.
* @param updateRotationMatrix Whether the rotation matrix should be calculated.
*/
- template<class IteratorType>
- void setFocalPoints(IteratorType focalAFirst, IteratorType focalBFirst, bool updateRotationMatrix = true)
+ template <class IteratorType>
+ void
+ setFocalPoints(IteratorType focalAFirst,
+ IteratorType focalBFirst,
+ bool updateRotationMatrix = true)
{
- Eigen::Map<VecType> tmpVec {tmpVal.data(), getDimensionality<long int>()};
+ Eigen::Map<VecType> tmpVec{tmpVal.data(), getDimensionality<long int>()};
//reset
distanceFocalPoints = 0.0;
@@ -532,7 +554,8 @@ namespace armarx
* Variable names (mostly) represent the names from the paper
* @param normalizedFocalPointAToB corresponds to a_1 from the paper
*/
- void refreshRotationMatrix(const VecType& normalizedFocalPointAToB)
+ void
+ refreshRotationMatrix(const VecType& normalizedFocalPointAToB)
{
const auto n = getDimensionality();
ARMARX_CHECK_EXPRESSION(normalizedFocalPointAToB.size() >= 0);
@@ -558,7 +581,8 @@ namespace armarx
Eigen::JacobiSVD<MatType, Eigen::NoQRPreconditioner> jacoby(
//M = a_1 * <first column of the identity matrix>^T
(normalizedFocalPointAToB * (MatType::Identity(n, n).col(0).transpose())),
- Eigen::ComputeFullV | Eigen::ComputeFullU // we need U and V and we dont want thin matrices
+ Eigen::ComputeFullV |
+ Eigen::ComputeFullU // we need U and V and we dont want thin matrices
);
//C = U * diag{1,...,1, det(U)* det(V)} * V^T
@@ -622,7 +646,7 @@ namespace armarx
*
* (as described in DOI>10.1109/IROS.2014.6942976)
*/
- template<class RealType = float>
+ template <class RealType = float>
class UniformInformedProlateSpheroidDistribution
{
public:
@@ -638,20 +662,20 @@ namespace armarx
* @param rotationMatrix The used rotation matrix (the values should be ordered to be conform with eigens mapping of matrices to vectors.
* (as defined by Eigen::Map<Eigen::Matrix<RealType, dimensionality, dimensionality>>)
*/
- template<class ValIter, class BoundsIter>
- UniformInformedProlateSpheroidDistribution(
- BoundsIter boundsFirst, BoundsIter boundsLast,
- ValIter focalAFirst, ValIter focalALast,
- ValIter focalBFirst,
- const std::vector<RealType>& rotationMatrix
- ):
- spheroidDistribution {focalAFirst, focalALast, focalBFirst, rotationMatrix},
- cuboidDistribution {boundsFirst, boundsLast},
- cMax {std::numeric_limits<RealType>::infinity()}
+ template <class ValIter, class BoundsIter>
+ UniformInformedProlateSpheroidDistribution(BoundsIter boundsFirst,
+ BoundsIter boundsLast,
+ ValIter focalAFirst,
+ ValIter focalALast,
+ ValIter focalBFirst,
+ const std::vector<RealType>& rotationMatrix) :
+ spheroidDistribution{focalAFirst, focalALast, focalBFirst, rotationMatrix},
+ cuboidDistribution{boundsFirst, boundsLast},
+ cMax{std::numeric_limits<RealType>::infinity()}
{
if (spheroidDistribution.getDimensionality() != cuboidDistribution.getDimensionality())
{
- throw std::logic_error {"differend bounds for spheroid and cuboid"};
+ throw std::logic_error{"differend bounds for spheroid and cuboid"};
}
}
@@ -665,19 +689,19 @@ namespace armarx
* @param focalALast One past the last value of the first focal point.
* @param focalBFirst The first value of the second focal point.
*/
- template<class ValIter, class BoundsIter>
- UniformInformedProlateSpheroidDistribution(
- BoundsIter boundsFirst, BoundsIter boundsLast,
- ValIter focalAFirst, ValIter focalALast,
- ValIter focalBFirst
- ):
- spheroidDistribution {focalAFirst, focalALast, focalBFirst},
- cuboidDistribution {boundsFirst, boundsLast},
- cMax {std::numeric_limits<RealType>::infinity()}
+ template <class ValIter, class BoundsIter>
+ UniformInformedProlateSpheroidDistribution(BoundsIter boundsFirst,
+ BoundsIter boundsLast,
+ ValIter focalAFirst,
+ ValIter focalALast,
+ ValIter focalBFirst) :
+ spheroidDistribution{focalAFirst, focalALast, focalBFirst},
+ cuboidDistribution{boundsFirst, boundsLast},
+ cMax{std::numeric_limits<RealType>::infinity()}
{
if (spheroidDistribution.getDimensionality() != cuboidDistribution.getDimensionality())
{
- throw std::logic_error {"differend bounds for spheroid and cuboid"};
+ throw std::logic_error{"differend bounds for spheroid and cuboid"};
}
}
@@ -686,8 +710,9 @@ namespace armarx
* @param first The first value to fill.
* @param gen The used generator.
*/
- template<class Generator>
- void operator()(RealType* first, Generator& gen)
+ template <class Generator>
+ void
+ operator()(RealType* first, Generator& gen)
{
if (cMax < std::numeric_limits<RealType>::infinity())
{
@@ -697,8 +722,7 @@ namespace armarx
do
{
cuboidDistribution(first, gen);
- }
- while (!spheroidDistribution.isInBounds(first));
+ } while (!spheroidDistribution.isInBounds(first));
}
else
{
@@ -706,8 +730,7 @@ namespace armarx
do
{
spheroidDistribution(first, gen);
- }
- while (!cuboidDistribution.isInBounds(first));
+ } while (!cuboidDistribution.isInBounds(first));
}
}
else
@@ -722,9 +745,11 @@ namespace armarx
* @param max The new maximal cost to set.
* @return Whether the new cost was lower than the old cost.
*/
- bool setMaximalCost(RealType max)
+ bool
+ setMaximalCost(RealType max)
{
- max = std::max(max, spheroidDistribution.getDistanceFocalPoints()); //in case of floating error
+ max = std::max(
+ max, spheroidDistribution.getDistanceFocalPoints()); //in case of floating error
if (!(max < cMax))
{
@@ -749,18 +774,19 @@ namespace armarx
* @brief Returns the distributions volume. It is the minimum of the prolate hyper spheroid's and the cuboid's volumes.
* @return The distributions volume.
*/
- RealType getVolume() const
+ RealType
+ getVolume() const
{
return std::min(cuboidDistribution.getVolume(), spheroidDistribution.getVolume());
}
-
/**
* @brief Returns the distributions dimensionality.
* @return The distributions dimensionality.
*/
- template<class T = std::size_t>
- T getDimensionality() const
+ template <class T = std::size_t>
+ T
+ getDimensionality() const
{
return cuboidDistribution.template getDimensionality<T>();
}
@@ -770,7 +796,8 @@ namespace armarx
* @param first The first value of the point to check.
* @return Whether [first, first + dimensionality) is a point contained by the prolate hyper spheroid and the cuboid.
*/
- bool isInBounds(const RealType* first) const
+ bool
+ isInBounds(const RealType* first) const
{
return cuboidDistribution.isInBounds(first) && spheroidDistribution.isInBounds(first);
}
@@ -793,5 +820,6 @@ namespace armarx
/**
* @brief Typedef for a sampler as required by informed rrt*.
*/
- using InformedSampler = Sampler<UniformInformedProlateSpheroidDistribution<float>, std::mt19937>;
-}
+ using InformedSampler =
+ Sampler<UniformInformedProlateSpheroidDistribution<float>, std::mt19937>;
+} // namespace armarx
diff --git a/source/RobotComponents/components/MotionPlanning/util/Volume.h b/source/RobotComponents/components/MotionPlanning/util/Volume.h
index 81ebba374bc0063a88cad1e0d11e60730f2cfb81..ecd9c9bd3188fb05b91c743031cd03a5341e4e03 100644
--- a/source/RobotComponents/components/MotionPlanning/util/Volume.h
+++ b/source/RobotComponents/components/MotionPlanning/util/Volume.h
@@ -32,8 +32,9 @@ namespace armarx
* @param n The dimensionality of the sphere.
* @return The volume of a n dimensional unit hypersphere.
*/
- template<class RealType>
- RealType volumeOfUnitHypersphere(std::size_t n)
+ template <class RealType>
+ RealType
+ volumeOfUnitHypersphere(std::size_t n)
{
const auto nReal = static_cast<RealType>(n);
const auto piSqrt = std::sqrt(M_PI);
@@ -47,8 +48,9 @@ namespace armarx
* @param radius The radius of the sphere.
* @return The volume of a n dimensional hypersphere with radius radius.
*/
- template<class RealType>
- RealType volumeOfHypersphere(std::size_t n, RealType radius)
+ template <class RealType>
+ RealType
+ volumeOfHypersphere(std::size_t n, RealType radius)
{
const auto nReal = static_cast<RealType>(n);
const auto piSqrt = std::sqrt(M_PI);
@@ -62,8 +64,9 @@ namespace armarx
* @param endRadii One past the last radius.
* @return The volume of an hyperellipsoid with the radii contained by the range [beginRadii, endRadii)
*/
- template<class RealType, class IteratorType>
- RealType volumeOfHyperellipsoid(IteratorType beginRadii, IteratorType endRadii)
+ template <class RealType, class IteratorType>
+ RealType
+ volumeOfHyperellipsoid(IteratorType beginRadii, IteratorType endRadii)
{
const auto nReal = static_cast<RealType>(std::distance(beginRadii, endRadii));
const auto divisor = std::tgamma(nReal / 2.0 + 1.0);
@@ -86,12 +89,14 @@ namespace armarx
* @param equatorialRadius The equatorial radius of the hyperspheroid.
* @return The volume of the hyperspheroid.
*/
- template<class RealType>
- RealType volumeOfHyperspheroid(std::size_t n, RealType polarRadius, RealType equatorialRadius)
+ template <class RealType>
+ RealType
+ volumeOfHyperspheroid(std::size_t n, RealType polarRadius, RealType equatorialRadius)
{
const auto nReal = static_cast<RealType>(n);
const auto divisor = std::tgamma(nReal / 2.0 + 1.0);
const auto piSqrt = std::sqrt(M_PI);
- return std::pow(piSqrt, nReal) * std::pow(equatorialRadius, nReal - 1.0) * polarRadius / divisor;
+ return std::pow(piSqrt, nReal) * std::pow(equatorialRadius, nReal - 1.0) * polarRadius /
+ divisor;
}
-}
+} // namespace armarx
diff --git a/source/RobotComponents/components/ObjectLocalizationSaliency/ObjectLocalizationSaliency.cpp b/source/RobotComponents/components/ObjectLocalizationSaliency/ObjectLocalizationSaliency.cpp
index 09ead15fe56847ecc08d5c8e18ac3da7b6cc6332..8a8e25f172b3ff0602604c3810140f3a55bca064 100644
--- a/source/RobotComponents/components/ObjectLocalizationSaliency/ObjectLocalizationSaliency.cpp
+++ b/source/RobotComponents/components/ObjectLocalizationSaliency/ObjectLocalizationSaliency.cpp
@@ -25,12 +25,13 @@
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
+
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
using namespace armarx;
-
-void ObjectLocalizationSaliency::onInitComponent()
+void
+ObjectLocalizationSaliency::onInitComponent()
{
usingProxy(getProperty<std::string>("ViewSelectionName").getValue());
usingTopic(getProperty<std::string>("ViewSelectionName").getValue() + "Observer");
@@ -56,7 +57,8 @@ void ObjectLocalizationSaliency::onInitComponent()
if (ArmarXDataPath::getAbsolutePath(graphFileName, graphFileName))
{
saliencyEgosphereGraph = new CIntensityGraph(graphFileName);
- ARMARX_VERBOSE << "Created egosphere graph with " << saliencyEgosphereGraph->getNodes()->size() << "nodes";
+ ARMARX_VERBOSE << "Created egosphere graph with "
+ << saliencyEgosphereGraph->getNodes()->size() << "nodes";
graphLookupTable = new CGraphPyramidLookupTable(9, 18);
graphLookupTable->buildLookupTable(saliencyEgosphereGraph);
nodeVisitedForObject.resize(saliencyEgosphereGraph->getNodes()->size());
@@ -73,21 +75,33 @@ void ObjectLocalizationSaliency::onInitComponent()
// this is robot model specific: offset from the used head coordinate system to the actual
// head center where the eyes are assumed to be located. Here it is correct for ARMAR-III
offsetToHeadCenter << 0, 0, 150;
- processorTask = new PeriodicTask<ObjectLocalizationSaliency>(this, &ObjectLocalizationSaliency::process, 1000.f / getProperty<float>("UpdateFrequency").getValue(), false, "ViewSelectionCalculation", false);
+ processorTask = new PeriodicTask<ObjectLocalizationSaliency>(
+ this,
+ &ObjectLocalizationSaliency::process,
+ 1000.f / getProperty<float>("UpdateFrequency").getValue(),
+ false,
+ "ViewSelectionCalculation",
+ false);
processorTask->setDelayWarningTolerance(2600);
}
-
-void ObjectLocalizationSaliency::onConnectComponent()
+void
+ObjectLocalizationSaliency::onConnectComponent()
{
- viewSelection = getProxy<ViewSelectionInterfacePrx>(getProperty<std::string>("ViewSelectionName").getValue());
+ viewSelection = getProxy<ViewSelectionInterfacePrx>(
+ getProperty<std::string>("ViewSelectionName").getValue());
viewSelection = viewSelection->ice_compress(true);
- robotStateComponent = getProxy<RobotStateComponentInterfacePrx>(getProperty<std::string>("RobotStateComponentName").getValue());
-
- memoryProxy = getProxy<memoryx::WorkingMemoryInterfacePrx>(getProperty<std::string>("WorkingMemoryName").getValue());
- priorKnowledge = getProxy<memoryx::PriorKnowledgeInterfacePrx>(getProperty<std::string>("PriorKnowledgeName").getValue());
- objectInstancesProxy = memoryx::ObjectInstanceMemorySegmentBasePrx::uncheckedCast(memoryProxy->getSegment("objectInstances"));
- objectClassesProxy = memoryx::ObjectClassMemorySegmentBasePrx::uncheckedCast(memoryProxy->getSegment("objectClasses"));
+ robotStateComponent = getProxy<RobotStateComponentInterfacePrx>(
+ getProperty<std::string>("RobotStateComponentName").getValue());
+
+ memoryProxy = getProxy<memoryx::WorkingMemoryInterfacePrx>(
+ getProperty<std::string>("WorkingMemoryName").getValue());
+ priorKnowledge = getProxy<memoryx::PriorKnowledgeInterfacePrx>(
+ getProperty<std::string>("PriorKnowledgeName").getValue());
+ objectInstancesProxy = memoryx::ObjectInstanceMemorySegmentBasePrx::uncheckedCast(
+ memoryProxy->getSegment("objectInstances"));
+ objectClassesProxy = memoryx::ObjectClassMemorySegmentBasePrx::uncheckedCast(
+ memoryProxy->getSegment("objectClasses"));
robot = RemoteRobot::createLocalClone(robotStateComponent);
@@ -97,13 +111,16 @@ void ObjectLocalizationSaliency::onConnectComponent()
for (std::string& id : ids)
{
- memoryx::ObjectClassPtr objClass = memoryx::ObjectClassPtr::dynamicCast(objClassSegment->getEntityById(id));
+ memoryx::ObjectClassPtr objClass =
+ memoryx::ObjectClassPtr::dynamicCast(objClassSegment->getEntityById(id));
if (objClass)
{
- memoryx::EntityWrappers::ObjectRecognitionWrapperPtr wrapper = objClass->addWrapper(new memoryx::EntityWrappers::ObjectRecognitionWrapper());
+ memoryx::EntityWrappers::ObjectRecognitionWrapperPtr wrapper =
+ objClass->addWrapper(new memoryx::EntityWrappers::ObjectRecognitionWrapper());
- if (!wrapper->getRecognitionMethod().empty() && wrapper->getRecognitionMethod() != "<none>")
+ if (!wrapper->getRecognitionMethod().empty() &&
+ wrapper->getRecognitionMethod() != "<none>")
{
recognizableObjClasses[objClass->getName()] = objClass;
ARMARX_INFO << objClass->getName() << " has " << wrapper->getRecognitionMethod();
@@ -117,23 +134,23 @@ void ObjectLocalizationSaliency::onConnectComponent()
}
}
-
-void ObjectLocalizationSaliency::onDisconnectComponent()
+void
+ObjectLocalizationSaliency::onDisconnectComponent()
{
onDeactivateAutomaticViewSelection();
}
-
-void ObjectLocalizationSaliency::onExitComponent()
+void
+ObjectLocalizationSaliency::onExitComponent()
{
delete graphLookupTable;
delete saliencyEgosphereGraph;
delete randomNoiseGraph;
}
-
-void ObjectLocalizationSaliency::process()
+void
+ObjectLocalizationSaliency::process()
{
std::unique_lock<std::mutex> lock(mutex);
@@ -170,8 +187,8 @@ void ObjectLocalizationSaliency::process()
lastDiff = (stopTime - startTime);
}
-
-void ObjectLocalizationSaliency::generateObjectLocalizationSaliency()
+void
+ObjectLocalizationSaliency::generateObjectLocalizationSaliency()
{
// reset saliency and visited nodes
@@ -196,13 +213,17 @@ void ObjectLocalizationSaliency::generateObjectLocalizationSaliency()
for (size_t i = 0; i < objectInstances.size(); i++)
{
- const memoryx::ObjectInstancePtr object = memoryx::ObjectInstancePtr::dynamicCast(objectInstances.at(i));
+ const memoryx::ObjectInstancePtr object =
+ memoryx::ObjectInstancePtr::dynamicCast(objectInstances.at(i));
if (object)
{
- if (recognizableObjClasses.count(object->getMostProbableClass()) > 0 && !object->getPosition()->getFrame().empty())
+ if (recognizableObjClasses.count(object->getMostProbableClass()) > 0 &&
+ !object->getPosition()->getFrame().empty())
{
- if (objectClassesProxy->hasEntityByName(object->getMostProbableClass())) // should be true if and only if object has been requested
+ if (objectClassesProxy->hasEntityByName(
+ object
+ ->getMostProbableClass())) // should be true if and only if object has been requested
{
FramedPositionPtr position = object->getPosition();
position->changeFrame(robot, headFrameName);
@@ -220,7 +241,8 @@ void ObjectLocalizationSaliency::generateObjectLocalizationSaliency()
}
else
{
- ARMARX_DEBUG << "Discarding " << object->getName() << " which is at " << position->output() << " ObjectDistance " << objDist;
+ ARMARX_DEBUG << "Discarding " << object->getName() << " which is at "
+ << position->output() << " ObjectDistance " << objDist;
}
}
}
@@ -228,7 +250,6 @@ void ObjectLocalizationSaliency::generateObjectLocalizationSaliency()
}
-
// generate new random noise
if (randomNoiseLevel > 0)
{
@@ -236,22 +257,29 @@ void ObjectLocalizationSaliency::generateObjectLocalizationSaliency()
}
- const float probabilityToAddALostObject = (numberOfLostObjects > 0) ? getProperty<float>("ProbabilityToLookForALostObject").getValue() / numberOfLostObjects : 0;
+ const float probabilityToAddALostObject =
+ (numberOfLostObjects > 0)
+ ? getProperty<float>("ProbabilityToLookForALostObject").getValue() / numberOfLostObjects
+ : 0;
// add localization necessities to sphere of possible view directions
for (size_t i = 0; i < localizableObjects.size(); i++)
{
const memoryx::ObjectInstancePtr object = localizableObjects.at(i);
- if ((object->getExistenceCertainty() >= 0.5) || (rand() % 100000 < 100000 * probabilityToAddALostObject))
+ if ((object->getExistenceCertainty() >= 0.5) ||
+ (rand() % 100000 < 100000 * probabilityToAddALostObject))
{
const FramedPositionPtr position = localizableObjectPositions.at(i);
- memoryx::MultivariateNormalDistributionPtr uncertaintyGaussian = memoryx::MultivariateNormalDistributionPtr::dynamicCast(object->getPositionUncertainty());
+ memoryx::MultivariateNormalDistributionPtr uncertaintyGaussian =
+ memoryx::MultivariateNormalDistributionPtr::dynamicCast(
+ object->getPositionUncertainty());
if (!uncertaintyGaussian)
{
- uncertaintyGaussian = memoryx::MultivariateNormalDistribution::CreateDefaultDistribution(1000000);
+ uncertaintyGaussian =
+ memoryx::MultivariateNormalDistribution::CreateDefaultDistribution(1000000);
}
float priority = object->getLocalizationPriority() / 100.0;
@@ -265,32 +293,51 @@ void ObjectLocalizationSaliency::generateObjectLocalizationSaliency()
float shortDistance = 0.5f * maxObjectDistance;
if (objDist < shortDistance)
{
- modifiedHalfCameraOpeningAngle = (objDist - 0.1f * shortDistance) / (0.9f * shortDistance) * halfCameraOpeningAngle;
+ modifiedHalfCameraOpeningAngle = (objDist - 0.1f * shortDistance) /
+ (0.9f * shortDistance) *
+ halfCameraOpeningAngle;
modifiedHalfCameraOpeningAngle = std::max(0.0f, modifiedHalfCameraOpeningAngle);
}
TSphereCoord positionInSphereCoordinates;
Eigen::Vector3f vec = position->toEigen() - offsetToHeadCenter;
MathTools::convert(vec, positionInSphereCoordinates);
- int closestNodeIndex = graphLookupTable->getClosestNode(positionInSphereCoordinates);
- addSaliencyRecursive(closestNodeIndex, saliency, positionInSphereCoordinates, i, modifiedHalfCameraOpeningAngle);
+ int closestNodeIndex =
+ graphLookupTable->getClosestNode(positionInSphereCoordinates);
+ addSaliencyRecursive(closestNodeIndex,
+ saliency,
+ positionInSphereCoordinates,
+ i,
+ modifiedHalfCameraOpeningAngle);
}
}
}
}
-
-void ObjectLocalizationSaliency::addSaliencyRecursive(const int currentNodeIndex, const float saliency, const TSphereCoord objectSphereCoord, const int objectIndex, const float maxDistanceOnArc)
+void
+ObjectLocalizationSaliency::addSaliencyRecursive(const int currentNodeIndex,
+ const float saliency,
+ const TSphereCoord objectSphereCoord,
+ const int objectIndex,
+ const float maxDistanceOnArc)
{
// distance on arc between object projection center and node,
// normalized by the maximal viewing angle of the camera (=> in [0,1])
- float normalizedDistance = MathTools::getDistanceOnArc(objectSphereCoord, saliencyEgosphereGraph->getNodes()->at(currentNodeIndex)->getPosition()) / maxDistanceOnArc;
+ float normalizedDistance =
+ MathTools::getDistanceOnArc(
+ objectSphereCoord,
+ saliencyEgosphereGraph->getNodes()->at(currentNodeIndex)->getPosition()) /
+ maxDistanceOnArc;
// increase value of node
- float newValue = ((CIntensityNode*)saliencyEgosphereGraph->getNodes()->at(currentNodeIndex))->getIntensity()
- + (1.0f - deviationFromCameraCenterFactor * normalizedDistance * normalizedDistance) * saliency;
- ((CIntensityNode*)saliencyEgosphereGraph->getNodes()->at(currentNodeIndex))->setIntensity(newValue);
+ float newValue =
+ ((CIntensityNode*)saliencyEgosphereGraph->getNodes()->at(currentNodeIndex))
+ ->getIntensity() +
+ (1.0f - deviationFromCameraCenterFactor * normalizedDistance * normalizedDistance) *
+ saliency;
+ ((CIntensityNode*)saliencyEgosphereGraph->getNodes()->at(currentNodeIndex))
+ ->setIntensity(newValue);
// mark node as visited for this object
nodeVisitedForObject.at(currentNodeIndex) = objectIndex;
@@ -304,9 +351,13 @@ void ObjectLocalizationSaliency::addSaliencyRecursive(const int currentNodeIndex
if (nodeVisitedForObject.at(neighbourIndex) != objectIndex)
{
- if (MathTools::getDistanceOnArc(objectSphereCoord, saliencyEgosphereGraph->getNodes()->at(neighbourIndex)->getPosition()) <= maxDistanceOnArc)
+ if (MathTools::getDistanceOnArc(
+ objectSphereCoord,
+ saliencyEgosphereGraph->getNodes()->at(neighbourIndex)->getPosition()) <=
+ maxDistanceOnArc)
{
- addSaliencyRecursive(neighbourIndex, saliency, objectSphereCoord, objectIndex, maxDistanceOnArc);
+ addSaliencyRecursive(
+ neighbourIndex, saliency, objectSphereCoord, objectIndex, maxDistanceOnArc);
}
else
{
@@ -316,8 +367,10 @@ void ObjectLocalizationSaliency::addSaliencyRecursive(const int currentNodeIndex
}
}
-
-void ObjectLocalizationSaliency::generateRandomNoise(std::vector<memoryx::ObjectInstancePtr>& localizableObjects, memoryx::EntityBaseList& objectInstances)
+void
+ObjectLocalizationSaliency::generateRandomNoise(
+ std::vector<memoryx::ObjectInstancePtr>& localizableObjects,
+ memoryx::EntityBaseList& objectInstances)
{
// if there are requested objects that were not localized yet, the random noise component will be bigger
@@ -329,7 +382,9 @@ void ObjectLocalizationSaliency::generateRandomNoise(std::vector<memoryx::Object
bool isInInstancesList = false;
for (size_t j = 0; j < objectInstances.size(); j++)
{
- if (requestedObjectClasses.at(i)->getName() == memoryx::ObjectInstanceBasePtr::dynamicCast(objectInstances.at(j))->getMostProbableClass())
+ if (requestedObjectClasses.at(i)->getName() ==
+ memoryx::ObjectInstanceBasePtr::dynamicCast(objectInstances.at(j))
+ ->getMostProbableClass())
{
isInInstancesList = true;
break;
@@ -351,33 +406,36 @@ void ObjectLocalizationSaliency::generateRandomNoise(std::vector<memoryx::Object
}
else
{
- ARMARX_DEBUG << "There are objects requested, and some but not all of them have been localized already";
+ ARMARX_DEBUG << "There are objects requested, and some but not all of them have been "
+ "localized already";
setRandomNoise(centralHeadTiltAngle + 20, 2.0f);
}
}
else
{
- ARMARX_DEBUG << "There are no requested objects that were not localized yet. requestedObjectClasses.size() = " << requestedObjectClasses.size()
- << ", objectInstances.size() = " << objectInstances.size();// << ", difference.size() = " << difference.size();
+ ARMARX_DEBUG << "There are no requested objects that were not localized yet. "
+ "requestedObjectClasses.size() = "
+ << requestedObjectClasses.size() << ", objectInstances.size() = "
+ << objectInstances.size(); // << ", difference.size() = " << difference.size();
setRandomNoise(centralHeadTiltAngle, 1.0f);
}
-
}
-
-
-void ObjectLocalizationSaliency::nextViewTarget(Ice::Long timestamp, const Ice::Current& c)
+void
+ObjectLocalizationSaliency::nextViewTarget(Ice::Long timestamp, const Ice::Current& c)
{
std::lock_guard<std::mutex> lock(mutex);
next = IceUtil::Time::milliSeconds(timestamp);
}
-
-void ObjectLocalizationSaliency::setRandomNoise(const float centralAngleForVerticalDirection, const float directionVariabilityFactor)
+void
+ObjectLocalizationSaliency::setRandomNoise(const float centralAngleForVerticalDirection,
+ const float directionVariabilityFactor)
{
- Eigen::Vector3f currentViewTargetEigen(1000, 0, 0); // virtual central gaze goes into x direction
+ Eigen::Vector3f currentViewTargetEigen(
+ 1000, 0, 0); // virtual central gaze goes into x direction
FramedPosition currentViewTarget(currentViewTargetEigen, cameraFrameName, robot->getName());
currentViewTarget.changeFrame(robot, headFrameName);
currentViewTargetEigen = currentViewTarget.toEigen() - offsetToHeadCenter;
@@ -392,22 +450,23 @@ void ObjectLocalizationSaliency::setRandomNoise(const float centralAngleForVerti
for (size_t i = 0; i < nodes->size(); i++)
{
- CIntensityNode* node = (CIntensityNode*) nodes->at(i);
+ CIntensityNode* node = (CIntensityNode*)nodes->at(i);
TSphereCoord nodeCoord = node->getPosition();
float distanceOnSphere = MathTools::getDistanceOnArc(currentViewDirection, nodeCoord);
- float distanceFactor = 1.0f - distanceWeight * distanceOnSphere / M_PI; // prefer directions close to current direction
- float verticalCenterFactor = 1.0f - centralityWeight * fabs(nodeCoord.fPhi - centralAngleForVerticalDirection) / 90.0f; // prefer directions that look to the center
+ float distanceFactor = 1.0f - distanceWeight * distanceOnSphere /
+ M_PI; // prefer directions close to current direction
+ float verticalCenterFactor =
+ 1.0f - centralityWeight * fabs(nodeCoord.fPhi - centralAngleForVerticalDirection) /
+ 90.0f; // prefer directions that look to the center
float horizontalCenterFactor = 1.0f - centralityWeight * fabs(nodeCoord.fTheta) / 120.0f;
- node->setIntensity(verticalCenterFactor * horizontalCenterFactor * distanceFactor * noiseFactor * (rand() % randomFactor));
+ node->setIntensity(verticalCenterFactor * horizontalCenterFactor * distanceFactor *
+ noiseFactor * (rand() % randomFactor));
}
-
}
-
-armarx::PropertyDefinitionsPtr ObjectLocalizationSaliency::createPropertyDefinitions()
+armarx::PropertyDefinitionsPtr
+ObjectLocalizationSaliency::createPropertyDefinitions()
{
- return armarx::PropertyDefinitionsPtr(new ObjectLocalizationSaliencyPropertyDefinitions(
- getConfigIdentifier()));
+ return armarx::PropertyDefinitionsPtr(
+ new ObjectLocalizationSaliencyPropertyDefinitions(getConfigIdentifier()));
}
-
-
diff --git a/source/RobotComponents/components/ObjectLocalizationSaliency/ObjectLocalizationSaliency.h b/source/RobotComponents/components/ObjectLocalizationSaliency/ObjectLocalizationSaliency.h
index 678d97786c093f01d0dbcc6faea6b3fbb6cae151..48c326e16ac841f40fcb5b7bfd50004e2aa3cf61 100644
--- a/source/RobotComponents/components/ObjectLocalizationSaliency/ObjectLocalizationSaliency.h
+++ b/source/RobotComponents/components/ObjectLocalizationSaliency/ObjectLocalizationSaliency.h
@@ -24,32 +24,27 @@
#pragma once
-#include <ArmarXCore/core/Component.h>
+#include <condition_variable>
+#include <mutex>
+#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
-
-#include <RobotAPI/interface/components/ViewSelectionInterface.h>
#include <RobotAPI/components/EarlyVisionGraph/GraphPyramidLookupTable.h>
#include <RobotAPI/components/EarlyVisionGraph/IntensityGraph.h>
#include <RobotAPI/components/EarlyVisionGraph/MathTools.h>
+#include <RobotAPI/interface/components/ViewSelectionInterface.h>
+#include <MemoryX/core/MemoryXCoreObjectFactories.h>
+#include <MemoryX/interface/components/WorkingMemoryInterface.h>
#include <MemoryX/interface/memorytypes/MemorySegments.h>
#include <MemoryX/interface/workingmemory/AbstractWorkingMemoryInterface.h>
-#include <MemoryX/interface/components/WorkingMemoryInterface.h>
-
-#include <MemoryX/core/MemoryXCoreObjectFactories.h>
-
-#include <MemoryX/libraries/memorytypes/entity/ObjectInstance.h>
-#include <MemoryX/libraries/memorytypes/entity/ObjectClass.h>
+#include <MemoryX/libraries/helpers/ObjectRecognitionHelpers/ObjectRecognitionWrapper.h>
#include <MemoryX/libraries/memorytypes/MemoryXTypesObjectFactories.h>
-#include <MemoryX/libraries/updater/ObjectLocalization/MemoryXUpdaterObjectFactories.h>
+#include <MemoryX/libraries/memorytypes/entity/ObjectClass.h>
+#include <MemoryX/libraries/memorytypes/entity/ObjectInstance.h>
#include <MemoryX/libraries/observers/ObjectMemoryObserver.h>
-#include <MemoryX/libraries/helpers/ObjectRecognitionHelpers/ObjectRecognitionWrapper.h>
-
-#include <mutex>
-#include <condition_variable>
-
+#include <MemoryX/libraries/updater/ObjectLocalization/MemoryXUpdaterObjectFactories.h>
namespace armarx
{
@@ -57,28 +52,65 @@ namespace armarx
* @class ObjectLocalizationSaliencyPropertyDefinitions
* @brief
*/
- class ObjectLocalizationSaliencyPropertyDefinitions:
+ class ObjectLocalizationSaliencyPropertyDefinitions :
public armarx::ComponentPropertyDefinitions
{
public:
- ObjectLocalizationSaliencyPropertyDefinitions(std::string prefix):
+ ObjectLocalizationSaliencyPropertyDefinitions(std::string prefix) :
armarx::ComponentPropertyDefinitions(prefix)
{
- defineOptionalProperty<std::string>("RobotStateComponentName", "RobotStateComponent", "Name of the robot state component that should be used");
- defineOptionalProperty<std::string>("HeadIKUnitName", "HeadIKUnit", "Name of the head IK unit component that should be used");
- defineOptionalProperty<std::string>("WorkingMemoryName", "WorkingMemory", "Name of the WorkingMemory component that should be used");
- defineOptionalProperty<std::string>("PriorKnowledgeName", "PriorKnowledge", "Name of the PriorKnowledge component that should be used");
- defineOptionalProperty<std::string>("ViewSelectionName", "ViewSelection", "Name of the ViewSelection component that should be used");
- defineOptionalProperty<std::string>("CameraFrameName", "VirtualCentralGaze", "Name of the frame of the head base in the robot model");
-
- defineOptionalProperty<std::string>("HeadFrameName", "Head Base", "Name of the frame of the head base in the robot model");
- defineOptionalProperty<float>("RandomNoiseLevel", 1.0f, "Maximum for the random noise that will be added to the localization necessities");
- defineOptionalProperty<float>("HalfCameraOpeningAngle", 12.0 * M_PI / 180.0, "0.5 * minimal opening angles of the used cameras");
- defineOptionalProperty<float>("MaxObjectDistance", 1500.f, "Maximum distance of objects the head to be considered for observation");
- defineOptionalProperty<float>("CentralHeadTiltAngle", 110.0f, "Defines the height direction that will be considered 'central' in the reachable area of the head (in degrees). Default is looking 20 degrees downwards");
- defineOptionalProperty<float>("ProbabilityToLookForALostObject", 0.03f, "Probability that one of the objects that have been seen but could later not been localized again will be included in the view selection");
- defineOptionalProperty<float>("UpdateFrequency", 10.0f, "Frequency of the saliency update");
-
+ defineOptionalProperty<std::string>(
+ "RobotStateComponentName",
+ "RobotStateComponent",
+ "Name of the robot state component that should be used");
+ defineOptionalProperty<std::string>(
+ "HeadIKUnitName",
+ "HeadIKUnit",
+ "Name of the head IK unit component that should be used");
+ defineOptionalProperty<std::string>(
+ "WorkingMemoryName",
+ "WorkingMemory",
+ "Name of the WorkingMemory component that should be used");
+ defineOptionalProperty<std::string>(
+ "PriorKnowledgeName",
+ "PriorKnowledge",
+ "Name of the PriorKnowledge component that should be used");
+ defineOptionalProperty<std::string>(
+ "ViewSelectionName",
+ "ViewSelection",
+ "Name of the ViewSelection component that should be used");
+ defineOptionalProperty<std::string>(
+ "CameraFrameName",
+ "VirtualCentralGaze",
+ "Name of the frame of the head base in the robot model");
+
+ defineOptionalProperty<std::string>(
+ "HeadFrameName",
+ "Head Base",
+ "Name of the frame of the head base in the robot model");
+ defineOptionalProperty<float>(
+ "RandomNoiseLevel",
+ 1.0f,
+ "Maximum for the random noise that will be added to the localization necessities");
+ defineOptionalProperty<float>("HalfCameraOpeningAngle",
+ 12.0 * M_PI / 180.0,
+ "0.5 * minimal opening angles of the used cameras");
+ defineOptionalProperty<float>(
+ "MaxObjectDistance",
+ 1500.f,
+ "Maximum distance of objects the head to be considered for observation");
+ defineOptionalProperty<float>(
+ "CentralHeadTiltAngle",
+ 110.0f,
+ "Defines the height direction that will be considered 'central' in the reachable "
+ "area of the head (in degrees). Default is looking 20 degrees downwards");
+ defineOptionalProperty<float>(
+ "ProbabilityToLookForALostObject",
+ 0.03f,
+ "Probability that one of the objects that have been seen but could later not been "
+ "localized again will be included in the view selection");
+ defineOptionalProperty<float>(
+ "UpdateFrequency", 10.0f, "Frequency of the saliency update");
}
};
@@ -101,12 +133,14 @@ namespace armarx
/**
* @see armarx::ManagedIceObject::getDefaultName()
*/
- std::string getDefaultName() const override
+ std::string
+ getDefaultName() const override
{
return "ObjectLocalizationSaliency";
}
- void onActivateAutomaticViewSelection(const Ice::Current& c = Ice::emptyCurrent) override
+ void
+ onActivateAutomaticViewSelection(const Ice::Current& c = Ice::emptyCurrent) override
{
ARMARX_LOG << "enabling object saliency computation";
@@ -120,11 +154,10 @@ namespace armarx
{
ARMARX_INFO << "view selection already active";
}
-
-
}
- void onDeactivateAutomaticViewSelection(const Ice::Current& c = Ice::emptyCurrent) override
+ void
+ onDeactivateAutomaticViewSelection(const Ice::Current& c = Ice::emptyCurrent) override
{
ARMARX_LOG << "disabling object saliency computation";
@@ -134,8 +167,8 @@ namespace armarx
}
}
-
- void nextViewTarget(Ice::Long timestamp, const Ice::Current& c = Ice::emptyCurrent) override;
+ void nextViewTarget(Ice::Long timestamp,
+ const Ice::Current& c = Ice::emptyCurrent) override;
protected:
@@ -170,11 +203,16 @@ namespace armarx
void generateObjectLocalizationSaliency();
- void addSaliencyRecursive(const int currentNodeIndex, const float saliency, const TSphereCoord objectSphereCoord, const int objectIndex, const float maxDistanceOnArc);
-
- void setRandomNoise(const float centralAngleForVerticalDirection, const float directionVariabilityFactor = 1.0f);
- void generateRandomNoise(std::vector<memoryx::ObjectInstancePtr>& localizableObjects, memoryx::EntityBaseList& objectInstances);
+ void addSaliencyRecursive(const int currentNodeIndex,
+ const float saliency,
+ const TSphereCoord objectSphereCoord,
+ const int objectIndex,
+ const float maxDistanceOnArc);
+ void setRandomNoise(const float centralAngleForVerticalDirection,
+ const float directionVariabilityFactor = 1.0f);
+ void generateRandomNoise(std::vector<memoryx::ObjectInstancePtr>& localizableObjects,
+ memoryx::EntityBaseList& objectInstances);
std::mutex mutex;
@@ -213,7 +251,5 @@ namespace armarx
float randomNoiseLevel;
std::string headFrameName, cameraFrameName;
-
};
-}
-
+} // namespace armarx
diff --git a/source/RobotComponents/components/ObjectLocalizationSaliency/test/ObjectLocalizationSaliencyTest.cpp b/source/RobotComponents/components/ObjectLocalizationSaliency/test/ObjectLocalizationSaliencyTest.cpp
index 712bad02c086b7baa312c359ce5349203412b302..f2e09906e6d307d34dbea4f0246db318e45d59d8 100644
--- a/source/RobotComponents/components/ObjectLocalizationSaliency/test/ObjectLocalizationSaliencyTest.cpp
+++ b/source/RobotComponents/components/ObjectLocalizationSaliency/test/ObjectLocalizationSaliencyTest.cpp
@@ -24,11 +24,11 @@
#define ARMARX_BOOST_TEST
+#include <iostream>
+
#include <RobotComponents/Test.h>
#include <RobotComponents/components/ObjectLocalizationSaliency/ObjectLocalizationSaliency.h>
-#include <iostream>
-
BOOST_AUTO_TEST_CASE(testExample)
{
armarx::ObjectLocalizationSaliency instance;
diff --git a/source/RobotComponents/components/PathPlanner/AStarPathPlanner.cpp b/source/RobotComponents/components/PathPlanner/AStarPathPlanner.cpp
index 340802d85fcc61e2db1ffb4ab707d7bbb81e6f79..6b481a2cb54afbc404bdf7f401e552d8b064cee9 100644
--- a/source/RobotComponents/components/PathPlanner/AStarPathPlanner.cpp
+++ b/source/RobotComponents/components/PathPlanner/AStarPathPlanner.cpp
@@ -25,116 +25,131 @@
#include <cmath>
#include <map>
-armarx::Vector3IBase operator +(const armarx::Vector3IBase& lhs, const armarx::Vector3IBase& rhs)
+armarx::Vector3IBase
+operator+(const armarx::Vector3IBase& lhs, const armarx::Vector3IBase& rhs)
{
- return armarx::Vector3IBase {lhs.x + rhs.x, lhs.y + rhs.y, lhs.z + rhs.z};
+ return armarx::Vector3IBase{lhs.x + rhs.x, lhs.y + rhs.y, lhs.z + rhs.z};
}
-
-armarx::AStarPathPlanner::AStarPathPlanner():
- AStarPathPlannerBase(
- 250.f, //tStepSize
-{
- 0.f, 5500.f
-}, //xBoundingRange
-{0.f, 11000.f}, //yBoundingRange
-{
- { +1, 0, 0}, //step in x
- { -1, 0, 0}, //step in -x
- {0, +1, 0}, //step in x
- {0, -1, 0}, //step in -y
- {0, 0, +1}, //rotate +
- {0, 0, -1}, //rotate -
- { +1, +1, 0}, //step in +x, +y
- { +1, -1, 0}, //step in +x, -y
- { -1, +1, 0}, //step in -x, +y
- { -1, -1, 0} //step in -x, -y
-}, //neighbourDeltas
-100000000.f, //rotationFactorForHeuristic
-1.f //distanceFactorForHeuristic
-),
-stepSizeDeg {5}
+armarx::AStarPathPlanner::AStarPathPlanner() :
+ AStarPathPlannerBase(250.f, //tStepSize
+ {0.f, 5500.f}, //xBoundingRange
+ {0.f, 11000.f}, //yBoundingRange
+ {
+ {+1, 0, 0}, //step in x
+ {-1, 0, 0}, //step in -x
+ {0, +1, 0}, //step in x
+ {0, -1, 0}, //step in -y
+ {0, 0, +1}, //rotate +
+ {0, 0, -1}, //rotate -
+ {+1, +1, 0}, //step in +x, +y
+ {+1, -1, 0}, //step in +x, -y
+ {-1, +1, 0}, //step in -x, +y
+ {-1, -1, 0} //step in -x, -y
+ }, //neighbourDeltas
+ 100000000.f, //rotationFactorForHeuristic
+ 1.f //distanceFactorForHeuristic
+ ),
+ stepSizeDeg{5}
{
}
-
//slice impl from PathPlannerBase
-void armarx::AStarPathPlanner::setTranslationtStepSize(::Ice::Float step, const ::Ice::Current&)
+void
+armarx::AStarPathPlanner::setTranslationtStepSize(::Ice::Float step, const ::Ice::Current&)
{
if (step < 0)
{
std::stringstream s;
s << "Invalid translation step size: " << step << " < 0.0";
ARMARX_ERROR_S << s.str();
- throw armarx::InvalidArgumentException {s.str()};
+ throw armarx::InvalidArgumentException{s.str()};
}
tStepSize = step;
}
-void armarx::AStarPathPlanner::setRotationStepSize(::Ice::Float step, const ::Ice::Current&)
+void
+armarx::AStarPathPlanner::setRotationStepSize(::Ice::Float step, const ::Ice::Current&)
{
//step is in rad
//we need values in [0, 360) but enforce this at a later point
//(we add stepSizeDeg so we have to normalize the result to [0, 360) anyway.
stepSizeDeg = static_cast<int>(step / M_PI * 180);
-
}
-void armarx::AStarPathPlanner::setBoundingXRange(const ::armarx::FloatRange& range, const ::Ice::Current&)
+void
+armarx::AStarPathPlanner::setBoundingXRange(const ::armarx::FloatRange& range,
+ const ::Ice::Current&)
{
if (range.max < range.min)
{
std::stringstream s;
- s << "Invalid bounding range for x dimension: min = " << range.min << ", max = " << range.max;
+ s << "Invalid bounding range for x dimension: min = " << range.min
+ << ", max = " << range.max;
ARMARX_ERROR_S << s.str();
- throw armarx::InvalidArgumentException {s.str()};
+ throw armarx::InvalidArgumentException{s.str()};
}
xBoundingRange = range;
}
-void armarx::AStarPathPlanner::setBoundingYRange(const ::armarx::FloatRange& range, const ::Ice::Current&)
+void
+armarx::AStarPathPlanner::setBoundingYRange(const ::armarx::FloatRange& range,
+ const ::Ice::Current&)
{
if (range.max < range.min)
{
std::stringstream s;
- s << "Invalid bounding range for y dimension: min = " << range.min << ", max = " << range.max;
+ s << "Invalid bounding range for y dimension: min = " << range.min
+ << ", max = " << range.max;
ARMARX_ERROR_S << s.str();
- throw armarx::InvalidArgumentException {s.str()};
+ throw armarx::InvalidArgumentException{s.str()};
}
yBoundingRange = range;
}
-void armarx::AStarPathPlanner::setNeighbourSteps(const ::armarx::Vector3IBaseList& neighbours, const ::Ice::Current&)
+void
+armarx::AStarPathPlanner::setNeighbourSteps(const ::armarx::Vector3IBaseList& neighbours,
+ const ::Ice::Current&)
{
neighbourDeltas = neighbours;
}
-void armarx::AStarPathPlanner::setRotationFactorForHeuristic(::Ice::Float factor, const ::Ice::Current&)
+void
+armarx::AStarPathPlanner::setRotationFactorForHeuristic(::Ice::Float factor, const ::Ice::Current&)
{
rotationFactorForHeuristic = factor;
}
-void armarx::AStarPathPlanner::setDistanceFactorForHeuristic(::Ice::Float factor, const ::Ice::Current&)
+void
+armarx::AStarPathPlanner::setDistanceFactorForHeuristic(::Ice::Float factor, const ::Ice::Current&)
{
distanceFactorForHeuristic = factor;
}
-float armarx::AStarPathPlanner::heuristic(const armarx::Vector3& from, const armarx::Vector3& to) const
+float
+armarx::AStarPathPlanner::heuristic(const armarx::Vector3& from, const armarx::Vector3& to) const
{
const Eigen::Vector3f delta = to.toEigen() - from.toEigen();
- return distanceFactorForHeuristic * std::hypot(delta(0), delta(1)) + rotationFactorForHeuristic * std::abs(delta(2));
+ return distanceFactorForHeuristic * std::hypot(delta(0), delta(1)) +
+ rotationFactorForHeuristic * std::abs(delta(2));
}
-float armarx::AStarPathPlanner::heuristic(const armarx::Vector3IBase& from, const armarx::Vector3IBase& to) const
+float
+armarx::AStarPathPlanner::heuristic(const armarx::Vector3IBase& from,
+ const armarx::Vector3IBase& to) const
{
- return distanceFactorForHeuristic * std::hypot(to.x - from.x, to.y - from.y) + rotationFactorForHeuristic * std::abs(to.z - from.z);
+ return distanceFactorForHeuristic * std::hypot(to.x - from.x, to.y - from.y) +
+ rotationFactorForHeuristic * std::abs(to.z - from.z);
}
//slice impl from PathPlannerBase
-::armarx::Vector3BaseList armarx::AStarPathPlanner::getPath(const ::armarx::Vector3BasePtr& from, const ::armarx::Vector3BasePtr& to, const ::Ice::Current&) const
+::armarx::Vector3BaseList
+armarx::AStarPathPlanner::getPath(const ::armarx::Vector3BasePtr& from,
+ const ::armarx::Vector3BasePtr& to,
+ const ::Ice::Current&) const
{
/*
* in this context the values of a Vector3IBase are interpreted as:
@@ -144,43 +159,44 @@ float armarx::AStarPathPlanner::heuristic(const armarx::Vector3IBase& from, cons
* so (0, 0, 0) is the starting point from
*/
- if (!(isPositionValid(Vector3 {from->x, from->y, from->z}) && isPositionValid(Vector3 {to->x, to->y, to->z})))
+ if (!(isPositionValid(Vector3{from->x, from->y, from->z}) &&
+ isPositionValid(Vector3{to->x, to->y, to->z})))
{
- return Vector3BaseList {};
+ return Vector3BaseList{};
}
assert(from);
assert(to);
assert(agent);
- ARMARX_VERBOSE << "Planning path from (" << from->x << ", " << from->y << ", " << from->z << ") to (" << to->x << ", " << to->y << ", " << to->z << ")";
+ ARMARX_VERBOSE << "Planning path from (" << from->x << ", " << from->y << ", " << from->z
+ << ") to (" << to->x << ", " << to->y << ", " << to->z << ")";
- auto toGlobal = [&](const armarx::Vector3IBase & node)
+ auto toGlobal = [&](const armarx::Vector3IBase& node)
{
- return armarx::Vector3
- {
- from->x + node.x * tStepSize,
- from->y + node.y * tStepSize,
- from->z + /*deg to rad*/ static_cast<float>(node.z* M_PI / 180)
- };
+ return armarx::Vector3{from->x + node.x * tStepSize,
+ from->y + node.y * tStepSize,
+ from->z + /*deg to rad*/ static_cast<float>(node.z * M_PI / 180)};
};
- auto getNeighbours = [&](const armarx::Vector3IBase & node)
+ auto getNeighbours = [&](const armarx::Vector3IBase& node)
{
armarx::Vector3IBaseList neighbours;
for (const auto& delta : neighbourDeltas)
{
- armarx::Vector3IBase neighbour {node.x + delta.x, node.y + delta.y,
- (((node.z + delta.z * stepSizeDeg) % 360) + 360) % 360 /*is rotation in deg (in [0, 360))*/
- };
+ armarx::Vector3IBase neighbour{
+ node.x + delta.x,
+ node.y + delta.y,
+ (((node.z + delta.z * stepSizeDeg) % 360) + 360) %
+ 360 /*is rotation in deg (in [0, 360))*/
+ };
const armarx::Vector3 globalNeighbour = toGlobal(neighbour);
if (globalNeighbour.x <= xBoundingRange.max &&
globalNeighbour.x >= xBoundingRange.min &&
globalNeighbour.y <= yBoundingRange.max &&
- globalNeighbour.y >= yBoundingRange.min &&
- isPositionValid(globalNeighbour))
+ globalNeighbour.y >= yBoundingRange.min && isPositionValid(globalNeighbour))
{
neighbours.push_back(neighbour);
}
@@ -199,17 +215,14 @@ float armarx::AStarPathPlanner::heuristic(const armarx::Vector3IBase& from, cons
// debugDrawer->setSphereDebugLayerVisu(s.str(), positionToDraw, color, 30.f);
// };
- const armarx::Vector3IBase start
- {
- 0, 0, 0
- };
+ const armarx::Vector3IBase start{0, 0, 0};
- armarx::Vector3 fromToDelta {to->x - from->x, to->y - from->y, to->z - from->z};
+ armarx::Vector3 fromToDelta{to->x - from->x, to->y - from->y, to->z - from->z};
- armarx::Vector3IBase goal {static_cast<int>(std::lround(fromToDelta.x / tStepSize)),
- static_cast<int>(std::lround(fromToDelta.y / tStepSize)),
- ((static_cast<int>(fromToDelta.z / M_PI * 180) % 360) + 360) % 360
- };//we need values in [0, 360)
+ armarx::Vector3IBase goal{static_cast<int>(std::lround(fromToDelta.x / tStepSize)),
+ static_cast<int>(std::lround(fromToDelta.y / tStepSize)),
+ ((static_cast<int>(fromToDelta.z / M_PI * 180) % 360) + 360) %
+ 360}; //we need values in [0, 360)
//if goal.z == 10, start.z == 0 and stepSizeDeg == 20 we can't reach the goal.z value => we have to round goal.z
int goalZMod = goal.z % stepSizeDeg;
@@ -219,9 +232,9 @@ float armarx::AStarPathPlanner::heuristic(const armarx::Vector3IBase& from, cons
const armarx::Vector3Ptr globalGoalPtr = armarx::Vector3Ptr::dynamicCast(to);
assert(globalGoalPtr);
- ::armarx::Vector3BaseList path {};
- std::vector<armarx::Vector3IBase> openSet {};
- std::vector<armarx::Vector3IBase> closedSet {};
+ ::armarx::Vector3BaseList path{};
+ std::vector<armarx::Vector3IBase> openSet{};
+ std::vector<armarx::Vector3IBase> closedSet{};
openSet.push_back(start);
@@ -242,12 +255,10 @@ float armarx::AStarPathPlanner::heuristic(const armarx::Vector3IBase& from, cons
{
//find best
auto currentIT =
- std::min_element(openSet.begin(), openSet.end(),
- [&](const armarx::Vector3IBase & a, const armarx::Vector3IBase & b)
- {
- return fScore.at(a) < fScore.at(b);
- }
- );
+ std::min_element(openSet.begin(),
+ openSet.end(),
+ [&](const armarx::Vector3IBase& a, const armarx::Vector3IBase& b)
+ { return fScore.at(a) < fScore.at(b); });
assert(currentIT != openSet.end());
const armarx::Vector3IBase current = *currentIT;
@@ -260,7 +271,8 @@ float armarx::AStarPathPlanner::heuristic(const armarx::Vector3IBase& from, cons
while (cameFromNode != start)
{
- armarx::Vector3BasePtr globalCameFromNodePtr = armarx::Vector3BasePtr {new armarx::Vector3{toGlobal(cameFromNode)}};
+ armarx::Vector3BasePtr globalCameFromNodePtr =
+ armarx::Vector3BasePtr{new armarx::Vector3{toGlobal(cameFromNode)}};
path.insert(path.begin(), globalCameFromNodePtr);
cameFromNode = cameFrom.at(cameFromNode);
}
@@ -308,5 +320,3 @@ float armarx::AStarPathPlanner::heuristic(const armarx::Vector3IBase& from, cons
return path;
}
-
-
diff --git a/source/RobotComponents/components/PathPlanner/AStarPathPlanner.h b/source/RobotComponents/components/PathPlanner/AStarPathPlanner.h
index bedd29d1af61c37f7217cf5bd1e32510df59903f..fc0fe3cb7997c5914f5c69b2b84d54f31717e6af 100644
--- a/source/RobotComponents/components/PathPlanner/AStarPathPlanner.h
+++ b/source/RobotComponents/components/PathPlanner/AStarPathPlanner.h
@@ -48,9 +48,7 @@ namespace armarx
* @ingroup Component-AStarPathPlanner
* @brief The AStarPathPlanner class
*/
- class AStarPathPlanner:
- virtual public PathPlanner,
- virtual public AStarPathPlannerBase
+ class AStarPathPlanner : virtual public PathPlanner, virtual public AStarPathPlannerBase
{
public:
AStarPathPlanner();
@@ -58,26 +56,37 @@ namespace armarx
/**
* @see armarx::ManagedIceObject::getDefaultName()
*/
- std::string getDefaultName() const override
+ std::string
+ getDefaultName() const override
{
return "AStarPathPlanner";
}
+
//slice impl from PathPlannerBase
- ::armarx::Vector3BaseList getPath(const ::armarx::Vector3BasePtr& from, const ::armarx::Vector3BasePtr& to, const ::Ice::Current& = Ice::emptyCurrent) const override;
+ ::armarx::Vector3BaseList getPath(const ::armarx::Vector3BasePtr& from,
+ const ::armarx::Vector3BasePtr& to,
+ const ::Ice::Current& = Ice::emptyCurrent) const override;
//slice impl from PathPlannerBase
- void setTranslationtStepSize(::Ice::Float step, const ::Ice::Current& = Ice::emptyCurrent) override;
- void setRotationStepSize(::Ice::Float step, const ::Ice::Current& = Ice::emptyCurrent) override;
+ void setTranslationtStepSize(::Ice::Float step,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
+ void setRotationStepSize(::Ice::Float step,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
- void setBoundingXRange(const ::armarx::FloatRange& range, const ::Ice::Current& = Ice::emptyCurrent) override;
+ void setBoundingXRange(const ::armarx::FloatRange& range,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
- void setBoundingYRange(const ::armarx::FloatRange& range, const ::Ice::Current& = Ice::emptyCurrent) override;
+ void setBoundingYRange(const ::armarx::FloatRange& range,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
- void setNeighbourSteps(const ::armarx::Vector3IBaseList& neighbours, const ::Ice::Current& = Ice::emptyCurrent) override;
+ void setNeighbourSteps(const ::armarx::Vector3IBaseList& neighbours,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
- void setRotationFactorForHeuristic(::Ice::Float factor, const ::Ice::Current& = Ice::emptyCurrent) override;
+ void setRotationFactorForHeuristic(::Ice::Float factor,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
- void setDistanceFactorForHeuristic(::Ice::Float factor, const ::Ice::Current& = Ice::emptyCurrent) override;
+ void setDistanceFactorForHeuristic(::Ice::Float factor,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
/**
* @brief The heuristic used for A*.
@@ -96,5 +105,5 @@ namespace armarx
int stepSizeDeg;
};
- using AStarPathPlannerPtr = ::IceInternal::Handle< ::armarx::AStarPathPlanner>;
-}
+ using AStarPathPlannerPtr = ::IceInternal::Handle<::armarx::AStarPathPlanner>;
+} // namespace armarx
diff --git a/source/RobotComponents/components/PathPlanner/PathPlanner.cpp b/source/RobotComponents/components/PathPlanner/PathPlanner.cpp
index d31881751b1c594bc3632ca693f97ae6fc0ed9ca..19b3a9a9eada1bbdf35b64e07d1450ec2722d251 100644
--- a/source/RobotComponents/components/PathPlanner/PathPlanner.cpp
+++ b/source/RobotComponents/components/PathPlanner/PathPlanner.cpp
@@ -22,46 +22,50 @@
#include "PathPlanner.h"
+#include <VirtualRobot/CollisionDetection/CollisionChecker.h>
+#include <VirtualRobot/XML/RobotIO.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
+#include <MemoryX/core/MemoryXCoreObjectFactories.h>
#include <MemoryX/interface/memorytypes/MemoryEntities.h>
-#include <MemoryX/libraries/memorytypes/entity/ObjectClass.h>
#include <MemoryX/libraries/helpers/VirtualRobotHelpers/SimoxObjectWrapper.h>
#include <MemoryX/libraries/memorytypes/MemoryXTypesObjectFactories.h>
-#include <MemoryX/core/MemoryXCoreObjectFactories.h>
-
-#include <VirtualRobot/XML/RobotIO.h>
-#include <VirtualRobot/CollisionDetection/CollisionChecker.h>
+#include <MemoryX/libraries/memorytypes/entity/ObjectClass.h>
-armarx::PathPlanner::PathPlanner():
- safetyMargin {50.f},
- objects {}
+armarx::PathPlanner::PathPlanner() : safetyMargin{50.f}, objects{}
{
}
-void armarx::PathPlanner::onInitComponent()
+void
+armarx::PathPlanner::onInitComponent()
{
usingProxy(getProperty<std::string>("WorkingMemoryName").getValue());
// offeringTopic(getProperty<std::string>("DebugDrawerName").getValue());
}
-void armarx::PathPlanner::onConnectComponent()
+void
+armarx::PathPlanner::onConnectComponent()
{
- workingMemoryPrx = getProxy<memoryx::WorkingMemoryInterfacePrx>(getProperty<std::string>("WorkingMemoryName").getValue());
+ workingMemoryPrx = getProxy<memoryx::WorkingMemoryInterfacePrx>(
+ getProperty<std::string>("WorkingMemoryName").getValue());
commonStoragePrx = workingMemoryPrx->getCommonStorage();
fileManager.reset(new memoryx::GridFileManager(commonStoragePrx));
// debugDrawer = getTopic<armarx::DebugDrawerInterfacePrx>(getProperty<std::string>("DebugDrawerName").getValue());
}
-void armarx::PathPlanner::setCollisionObjects(const ::armarx::ObjectPositionBaseList& list, const ::Ice::Current&)
+void
+armarx::PathPlanner::setCollisionObjects(const ::armarx::ObjectPositionBaseList& list,
+ const ::Ice::Current&)
{
clearCollisionObjects();
addCollisionObjects(list);
}
-void armarx::PathPlanner::addCollisionObjects(const ::armarx::ObjectPositionBaseList& list, const ::Ice::Current&)
+void
+armarx::PathPlanner::addCollisionObjects(const ::armarx::ObjectPositionBaseList& list,
+ const ::Ice::Current&)
{
ARMARX_VERBOSE << "Adding " << list.size() << " collision objects";
@@ -75,20 +79,23 @@ void armarx::PathPlanner::addCollisionObjects(const ::armarx::ObjectPositionBase
CollisionObjectData newObject;
newObject.object = elem.objectClassBase;
- ARMARX_VERBOSE << "Object: name " << newObject.object->getName() << ", id " << newObject.object->getId();
+ ARMARX_VERBOSE << "Object: name " << newObject.object->getName() << ", id "
+ << newObject.object->getId();
- memoryx::ObjectClassPtr objectClass = memoryx::ObjectClassPtr::dynamicCast(elem.objectClassBase);
+ memoryx::ObjectClassPtr objectClass =
+ memoryx::ObjectClassPtr::dynamicCast(elem.objectClassBase);
if (!objectClass)
{
std::stringstream s;
s << "Can't use object class with ice id " << elem.objectClassBase->ice_id();
ARMARX_ERROR_S << s.str();
- throw armarx::InvalidArgumentException {s.str()};
+ throw armarx::InvalidArgumentException{s.str()};
}
ARMARX_VERBOSE << "Adding SimoxObjectWrapper";
- memoryx::EntityWrappers::SimoxObjectWrapperPtr sw = objectClass->addWrapper(new memoryx::EntityWrappers::SimoxObjectWrapper(fileManager));
+ memoryx::EntityWrappers::SimoxObjectWrapperPtr sw =
+ objectClass->addWrapper(new memoryx::EntityWrappers::SimoxObjectWrapper(fileManager));
ARMARX_VERBOSE << "Getting ManipulationObject name and object";
VirtualRobot::ManipulationObjectPtr orgMo = sw->getManipulationObject();
std::string moName = orgMo->getName();
@@ -104,7 +111,7 @@ void armarx::PathPlanner::addCollisionObjects(const ::armarx::ObjectPositionBase
std::stringstream s;
s << "Can't convert pose of " << objectClass->getName() << " to armarx::Pose.";
ARMARX_ERROR_S << s.str();
- throw armarx::InvalidArgumentException {s.str()};
+ throw armarx::InvalidArgumentException{s.str()};
}
mo->setGlobalPose(objectPose->toEigen());
@@ -120,16 +127,21 @@ void armarx::PathPlanner::addCollisionObjects(const ::armarx::ObjectPositionBase
//copy
std::copy(newObjects.begin(), newObjects.end(), std::back_inserter(objects));
- ARMARX_VERBOSE << "Added " << newObjects.size() << " collision objects. (Total: " << objects.size() << ")";
+ ARMARX_VERBOSE << "Added " << newObjects.size()
+ << " collision objects. (Total: " << objects.size() << ")";
}
-void armarx::PathPlanner::clearCollisionObjects(const ::Ice::Current&)
+void
+armarx::PathPlanner::clearCollisionObjects(const ::Ice::Current&)
{
ARMARX_VERBOSE << "Cleared " << objects.size() << " objects.";
objects.clear();
}
-void armarx::PathPlanner::setAgent(const ::memoryx::AgentInstanceBasePtr& newAgent, const std::string& agentColModelName, const ::Ice::Current&)
+void
+armarx::PathPlanner::setAgent(const ::memoryx::AgentInstanceBasePtr& newAgent,
+ const std::string& agentColModelName,
+ const ::Ice::Current&)
{
ARMARX_VERBOSE << "Setting agent '" << newAgent->getName() << "' with id " << newAgent->getId();
//get agent
@@ -139,7 +151,8 @@ void armarx::PathPlanner::setAgent(const ::memoryx::AgentInstanceBasePtr& newAge
{
ARMARX_ERROR << "Could not find robot file " << agentFilePath << std::flush;
- ARMARX_VERBOSE << "Searched in " << ArmarXDataPath::getDataPaths().size() << " additional paths";
+ ARMARX_VERBOSE << "Searched in " << ArmarXDataPath::getDataPaths().size()
+ << " additional paths";
for (const auto& path : ArmarXDataPath::getDataPaths())
{
@@ -159,42 +172,45 @@ void armarx::PathPlanner::setAgent(const ::memoryx::AgentInstanceBasePtr& newAge
std::stringstream s;
s << "Can't load agent from: " << agentFilePath;
ARMARX_ERROR_S << s.str();
- throw armarx::InvalidArgumentException {s.str()};
+ throw armarx::InvalidArgumentException{s.str()};
}
//get collision model
- if (!agent2->hasRobotNode(agentColModelName) || !agent2->getRobotNode(agentColModelName)->getCollisionModel())
+ if (!agent2->hasRobotNode(agentColModelName) ||
+ !agent2->getRobotNode(agentColModelName)->getCollisionModel())
{
std::stringstream s;
s << "Agent has no collision model with name " << agentColModelName;
ARMARX_ERROR_S << s.str();
- throw armarx::InvalidArgumentException {s.str()};
+ throw armarx::InvalidArgumentException{s.str()};
}
agent = agent2;
- agentCollisionModel = agent->getRobotNode(agentColModelName)->getCollisionModel();//->clone();
+ agentCollisionModel = agent->getRobotNode(agentColModelName)->getCollisionModel(); //->clone();
agentZCoord = agent->getGlobalPose()(2, 3);
ARMARX_VERBOSE << "Setting agent...done";
}
-void armarx::PathPlanner::setSafetyMargin(::Ice::Float margin, const ::Ice::Current&)
+void
+armarx::PathPlanner::setSafetyMargin(::Ice::Float margin, const ::Ice::Current&)
{
if (margin < 0)
{
std::stringstream s;
s << "Invalid margin: " << margin << " < 0.0";
ARMARX_ERROR_S << s.str();
- throw armarx::InvalidArgumentException {s.str()};
+ throw armarx::InvalidArgumentException{s.str()};
}
safetyMargin = margin;
}
-bool armarx::PathPlanner::isPositionValid(armarx::Vector3 position) const
+bool
+armarx::PathPlanner::isPositionValid(armarx::Vector3 position) const
{
if (!agentCollisionModel)
{
- throw std::logic_error {"no agent collision model"};
+ throw std::logic_error{"no agent collision model"};
}
if (objects.empty())
@@ -204,7 +220,8 @@ bool armarx::PathPlanner::isPositionValid(armarx::Vector3 position) const
//set pose
Eigen::Matrix4f pose = Eigen::Matrix4f::Identity();
- pose.block<3, 3>(0, 0) = VirtualRobot::MathTools::axisangle2eigen3f(Eigen::Vector3f {0.f, 0.f, 1.f}, position.z);
+ pose.block<3, 3>(0, 0) =
+ VirtualRobot::MathTools::axisangle2eigen3f(Eigen::Vector3f{0.f, 0.f, 1.f}, position.z);
pose(0, 3) = position.x;
pose(1, 3) = position.y;
pose(3, 3) = agentZCoord;
@@ -214,9 +231,9 @@ bool armarx::PathPlanner::isPositionValid(armarx::Vector3 position) const
{
for (const auto& object : objects)
{
- float dist = VirtualRobot::CollisionChecker::getGlobalCollisionChecker()->calculateDistance(
- agentCollisionModel,
- object.colModel);
+ float dist =
+ VirtualRobot::CollisionChecker::getGlobalCollisionChecker()->calculateDistance(
+ agentCollisionModel, object.colModel);
if (dist <= safetyMargin)
{
@@ -230,9 +247,7 @@ bool armarx::PathPlanner::isPositionValid(armarx::Vector3 position) const
for (const auto& object : objects)
{
bool col = VirtualRobot::CollisionChecker::getGlobalCollisionChecker()->checkCollision(
- agentCollisionModel,
- object.colModel
- );
+ agentCollisionModel, object.colModel);
if (col)
{
diff --git a/source/RobotComponents/components/PathPlanner/PathPlanner.h b/source/RobotComponents/components/PathPlanner/PathPlanner.h
index a87f39e39afee62733295affe8b8d601c5d0bdf4..c73c03e54801bb9c3ec0b70d27f844fd6b15b750 100644
--- a/source/RobotComponents/components/PathPlanner/PathPlanner.h
+++ b/source/RobotComponents/components/PathPlanner/PathPlanner.h
@@ -22,6 +22,9 @@
#pragma once
+#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/SceneObject.h>
+
#include <ArmarXCore/core/Component.h>
#include <RobotAPI/libraries/core/Pose.h>
@@ -31,9 +34,6 @@
#include <MemoryX/core/entity/AbstractEntityWrapper.h>
#include <MemoryX/interface/components/WorkingMemoryInterface.h>
-#include <VirtualRobot/SceneObject.h>
-#include <VirtualRobot/Robot.h>
-
//include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
namespace armarx
@@ -42,19 +42,17 @@ namespace armarx
* @brief Holds properties for PathPlanner.
* @see PathPlanner
*/
- class PathPlannerPropertyDefinitions:
- public armarx::ComponentPropertyDefinitions
+ class PathPlannerPropertyDefinitions : public armarx::ComponentPropertyDefinitions
{
public:
- PathPlannerPropertyDefinitions(std::string prefix):
- ComponentPropertyDefinitions(prefix)
+ PathPlannerPropertyDefinitions(std::string prefix) : ComponentPropertyDefinitions(prefix)
{
- defineOptionalProperty<std::string>("WorkingMemoryName", "WorkingMemory", "Name of the WorkingMemory component");
+ defineOptionalProperty<std::string>(
+ "WorkingMemoryName", "WorkingMemory", "Name of the WorkingMemory component");
// defineOptionalProperty<std::string>("DebugDrawerName", "DebugDrawerUpdates", "Name of the DebugDrawer component");
}
};
-
/**
* @defgroup Component-PathPlanner PathPlanner
* @ingroup RobotComponents-Components
@@ -70,9 +68,7 @@ namespace armarx
* @ingroup Component-PathPlanner
* @brief The PathPlanner class
*/
- class PathPlanner:
- virtual public Component,
- virtual public PathPlannerBase
+ class PathPlanner : virtual public Component, virtual public PathPlannerBase
{
public:
PathPlanner();
@@ -80,12 +76,11 @@ namespace armarx
/**
* @see PropertyUser::createPropertyDefinitions()
*/
- armarx::PropertyDefinitionsPtr createPropertyDefinitions() override
+ armarx::PropertyDefinitionsPtr
+ createPropertyDefinitions() override
{
- return armarx::PropertyDefinitionsPtr
- {
- new PathPlannerPropertyDefinitions{getConfigIdentifier()}
- };
+ return armarx::PropertyDefinitionsPtr{
+ new PathPlannerPropertyDefinitions{getConfigIdentifier()}};
}
/**
@@ -99,14 +94,22 @@ namespace armarx
void onConnectComponent() override;
//from slice interface PathPlannerBase
- void setCollisionObjects(const ::armarx::ObjectPositionBaseList& list, const ::Ice::Current& = Ice::emptyCurrent) override;
- void addCollisionObjects(const ::armarx::ObjectPositionBaseList& list, const ::Ice::Current& = Ice::emptyCurrent) override;
+ void setCollisionObjects(const ::armarx::ObjectPositionBaseList& list,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
+ void addCollisionObjects(const ::armarx::ObjectPositionBaseList& list,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
void clearCollisionObjects(const ::Ice::Current& = Ice::emptyCurrent) override;
- void setAgent(const ::memoryx::AgentInstanceBasePtr& newAgent, const std::string& agentColModelName, const ::Ice::Current& = Ice::emptyCurrent) override;
- void setSafetyMargin(::Ice::Float margin, const ::Ice::Current& = Ice::emptyCurrent) override;
+ void setAgent(const ::memoryx::AgentInstanceBasePtr& newAgent,
+ const std::string& agentColModelName,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
+ void setSafetyMargin(::Ice::Float margin,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
- ::armarx::Vector3BaseList getPath(const ::armarx::Vector3BasePtr&, const ::armarx::Vector3BasePtr&, const ::Ice::Current& = Ice::emptyCurrent) const override = 0;
+ ::armarx::Vector3BaseList
+ getPath(const ::armarx::Vector3BasePtr&,
+ const ::armarx::Vector3BasePtr&,
+ const ::Ice::Current& = Ice::emptyCurrent) const override = 0;
bool isPositionValid(armarx::Vector3 position) const;
@@ -132,7 +135,6 @@ namespace armarx
// armarx::DebugDrawerInterfacePrx debugDrawer;
};
- using PathPlannerPtr = ::IceInternal::Handle< ::armarx::PathPlanner>;
-
-}
+ using PathPlannerPtr = ::IceInternal::Handle<::armarx::PathPlanner>;
+} // namespace armarx
diff --git a/source/RobotComponents/components/PlannedMotionProvider/PlannedMotionProvider.cpp b/source/RobotComponents/components/PlannedMotionProvider/PlannedMotionProvider.cpp
index 74494386e9e5c156903e70a1f86b0d92a9a081c0..1dfb49875182bef4eea0be85ea6e662d8a5892e8 100644
--- a/source/RobotComponents/components/PlannedMotionProvider/PlannedMotionProvider.cpp
+++ b/source/RobotComponents/components/PlannedMotionProvider/PlannedMotionProvider.cpp
@@ -21,35 +21,40 @@
*/
#include "PlannedMotionProvider.h"
+
+#include <VirtualRobot/MathTools.h>
#include <VirtualRobot/Nodes/RobotNode.h>
+#include <VirtualRobot/RobotNodeSet.h>
+
#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotComponents/components/MotionPlanning/Tasks/RRTConnect/Task.h>
//#include <RobotComponents/components/MotionPlanning/Tasks/BiRRT/Task.h>
-#include <RobotComponents/components/MotionPlanning/Tasks/RandomShortcutPostprocessor/Task.h>
-#include <RobotComponents/components/MotionPlanning/CSpace/SimoxCSpace.h>
-#include <RobotComponents/components/MotionPlanning/CSpace/ScaledCSpace.h>
-#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
#include <RobotAPI/libraries/core/math/MathUtils.h>
+#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
+#include <RobotComponents/components/MotionPlanning/CSpace/ScaledCSpace.h>
+#include <RobotComponents/components/MotionPlanning/CSpace/SimoxCSpace.h>
+#include <RobotComponents/components/MotionPlanning/Tasks/RandomShortcutPostprocessor/Task.h>
namespace armarx
{
- GraspingTrajectory PlannedMotionProvider::planMotion(const SimoxCSpaceBasePtr& cSpaceBase,
- const SimoxCSpaceBasePtr& cSpacePlatformBase,
- const MotionPlanningData& mpd, const Ice::Current& c)
+ GraspingTrajectory
+ PlannedMotionProvider::planMotion(const SimoxCSpaceBasePtr& cSpaceBase,
+ const SimoxCSpaceBasePtr& cSpacePlatformBase,
+ const MotionPlanningData& mpd,
+ const Ice::Current& c)
{
TrajectoryPtr jointTrajectory = calculateJointTrajectory(cSpaceBase, mpd, c);
TrajectoryPtr platformTrajectory = calculatePlatformTrajectory(cSpacePlatformBase, mpd, c);
- return {platformTrajectory,
- jointTrajectory, mpd.rnsToUse, mpd.endeffector,
- mpd.grasp
- };
+ return {platformTrajectory, jointTrajectory, mpd.rnsToUse, mpd.endeffector, mpd.grasp};
}
- GraspingTrajectory PlannedMotionProvider::planMotionParallel(const SimoxCSpaceBasePtr& cSpaceBase,
- const SimoxCSpaceBasePtr& cSpacePlatformBase,
- const MotionPlanningData& mpd, const Ice::Current& c)
+ GraspingTrajectory
+ PlannedMotionProvider::planMotionParallel(const SimoxCSpaceBasePtr& cSpaceBase,
+ const SimoxCSpaceBasePtr& cSpacePlatformBase,
+ const MotionPlanningData& mpd,
+ const Ice::Current& c)
{
VectorXf startCfg, goalCfg, startPos, goalPos;
ScaledCSpacePtr scaledJointCSpace, scaledPlatformCSpace;
@@ -57,24 +62,27 @@ namespace armarx
SimoxCSpacePtr cSpace = SimoxCSpacePtr::dynamicCast(cSpaceBase);
SimoxCSpacePtr cSpacePlatform = SimoxCSpacePtr::dynamicCast(cSpacePlatformBase);
prepareJointCSpace(cSpace, mpd, &startCfg, &goalCfg, &scaledJointCSpace);
- preparePlatformCSpace(cSpacePlatform, mpd, &startPos, &goalPos, &scaledPlatformCSpace, &rpyAgent);
+ preparePlatformCSpace(
+ cSpacePlatform, mpd, &startPos, &goalPos, &scaledPlatformCSpace, &rpyAgent);
ARMARX_INFO << "Starting BiRRT for Joints and Platform";
// TODO: This is not async. Why?? Are the tasks not started when they are enqueued?
- RemoteHandle<MotionPlanningTaskControlInterfacePrx> rspJointHandle = startBiRRT(scaledJointCSpace, startCfg, goalCfg, 0.01);
- RemoteHandle<MotionPlanningTaskControlInterfacePrx> rspPlatformHandle = startBiRRT(scaledPlatformCSpace, startPos, goalPos, 0.1);
+ RemoteHandle<MotionPlanningTaskControlInterfacePrx> rspJointHandle =
+ startBiRRT(scaledJointCSpace, startCfg, goalCfg, 0.01);
+ RemoteHandle<MotionPlanningTaskControlInterfacePrx> rspPlatformHandle =
+ startBiRRT(scaledPlatformCSpace, startPos, goalPos, 0.1);
ARMARX_INFO << "Collecting the jointTask";
Path jointTrajectoryPath = finishBiRRT(rspJointHandle, scaledJointCSpace, "Joint");
ARMARX_INFO << "Collecting the platformTask";
Path posTrajectoryPath = finishBiRRT(rspPlatformHandle, scaledPlatformCSpace, "Platform");
// BiRRTTask;
- auto transformToGlobal = [&](armarx::VectorXf & pos2D)
+ auto transformToGlobal = [&](armarx::VectorXf& pos2D)
{
- Eigen::Vector2f globalPos = Eigen::Rotation2Df(rpyAgent(2)) * Eigen::Vector2f(pos2D.at(0), pos2D.at(1));
+ Eigen::Vector2f globalPos =
+ Eigen::Rotation2Df(rpyAgent(2)) * Eigen::Vector2f(pos2D.at(0), pos2D.at(1));
pos2D.at(0) = globalPos[0] + cSpacePlatform->getAgent().agentPose->position->x;
pos2D.at(1) = globalPos[1] + cSpacePlatform->getAgent().agentPose->position->y;
pos2D.at(2) = math::MathUtils::angleModPI(pos2D.at(2) + rpyAgent(2));
-
};
for (auto& e : posTrajectoryPath.nodes)
{
@@ -86,91 +94,99 @@ namespace armarx
return {platformTrajectory, jointTrajectory, mpd.rnsToUse, mpd.endeffector, mpd.grasp};
}
- void PlannedMotionProvider::onInitComponent()
+ void
+ PlannedMotionProvider::onInitComponent()
{
usingProxy("MotionPlanningServer");
usingProxy("RobotStateComponent");
maxPostProcessingSteps = 50;
-
}
-
- void PlannedMotionProvider::onConnectComponent()
+ void
+ PlannedMotionProvider::onConnectComponent()
{
getProxy(mps, "MotionPlanningServer");
getProxy(robotStateComponent, "RobotStateComponent");
- localRobot = RemoteRobot::createLocalCloneFromFile(robotStateComponent, VirtualRobot::RobotIO::eStructure);
+ localRobot = RemoteRobot::createLocalCloneFromFile(robotStateComponent,
+ VirtualRobot::RobotIO::eStructure);
}
-
- void PlannedMotionProvider::onDisconnectComponent()
+ void
+ PlannedMotionProvider::onDisconnectComponent()
{
planningTasks.clear();
}
-
- void PlannedMotionProvider::onExitComponent()
+ void
+ PlannedMotionProvider::onExitComponent()
{
-
}
- armarx::PropertyDefinitionsPtr PlannedMotionProvider::createPropertyDefinitions()
+ armarx::PropertyDefinitionsPtr
+ PlannedMotionProvider::createPropertyDefinitions()
{
- return armarx::PropertyDefinitionsPtr(new PlannedMotionProviderPropertyDefinitions(
- getConfigIdentifier()));
+ return armarx::PropertyDefinitionsPtr(
+ new PlannedMotionProviderPropertyDefinitions(getConfigIdentifier()));
}
- TrajectoryBasePtr PlannedMotionProvider::planJointMotion(const armarx::SimoxCSpaceBasePtr& cSpaceBase,
- const armarx::MotionPlanningData& mpd,
- const Ice::Current& c)
+ TrajectoryBasePtr
+ PlannedMotionProvider::planJointMotion(const armarx::SimoxCSpaceBasePtr& cSpaceBase,
+ const armarx::MotionPlanningData& mpd,
+ const Ice::Current& c)
{
TrajectoryPtr trajectory = calculateJointTrajectory(cSpaceBase, mpd, c);
return TrajectoryBasePtr::dynamicCast(trajectory);
}
- TrajectoryBasePtr PlannedMotionProvider::planPlatformMotion(const armarx::SimoxCSpaceBasePtr& cSpacePlatformBase,
- const armarx::MotionPlanningData& mpd,
- const Ice::Current& c)
+ TrajectoryBasePtr
+ PlannedMotionProvider::planPlatformMotion(const armarx::SimoxCSpaceBasePtr& cSpacePlatformBase,
+ const armarx::MotionPlanningData& mpd,
+ const Ice::Current& c)
{
TrajectoryPtr trajectory = calculatePlatformTrajectory(cSpacePlatformBase, mpd, c);
return TrajectoryBasePtr::dynamicCast(trajectory);
}
- TrajectoryPtr PlannedMotionProvider::calculateJointTrajectory(const SimoxCSpaceBasePtr& cSpaceBase,
- const MotionPlanningData& mpd, const Ice::Current& c)
+ TrajectoryPtr
+ PlannedMotionProvider::calculateJointTrajectory(const SimoxCSpaceBasePtr& cSpaceBase,
+ const MotionPlanningData& mpd,
+ const Ice::Current& c)
{
VectorXf startCfg;
VectorXf goalCfg;
ScaledCSpacePtr scaledJointCSpace;
SimoxCSpacePtr cSpace = SimoxCSpacePtr::dynamicCast(cSpaceBase);
prepareJointCSpace(cSpace, mpd, &startCfg, &goalCfg, &scaledJointCSpace);
- RemoteHandle<MotionPlanningTaskControlInterfacePrx> rspHandle = startBiRRT(scaledJointCSpace, startCfg, goalCfg, 0.01);
+ RemoteHandle<MotionPlanningTaskControlInterfacePrx> rspHandle =
+ startBiRRT(scaledJointCSpace, startCfg, goalCfg, 0.01);
Path jointTrajectoryPath = finishBiRRT(rspHandle, scaledJointCSpace, "Joint");
return cSpace->pathToTrajectory(jointTrajectoryPath);
}
-
- TrajectoryPtr PlannedMotionProvider::calculatePlatformTrajectory(const SimoxCSpaceBasePtr& cSpacePlatformBase,
- const MotionPlanningData& mpd,
- const Ice::Current& c)
+ TrajectoryPtr
+ PlannedMotionProvider::calculatePlatformTrajectory(const SimoxCSpaceBasePtr& cSpacePlatformBase,
+ const MotionPlanningData& mpd,
+ const Ice::Current& c)
{
VectorXf startPos;
VectorXf goalPos;
ScaledCSpacePtr scaledPlatformCSpace;
Eigen::Vector3f rpyAgent;
SimoxCSpacePtr cSpacePlatform = SimoxCSpacePtr::dynamicCast(cSpacePlatformBase);
- preparePlatformCSpace(cSpacePlatform, mpd, &startPos, &goalPos, &scaledPlatformCSpace, &rpyAgent);
+ preparePlatformCSpace(
+ cSpacePlatform, mpd, &startPos, &goalPos, &scaledPlatformCSpace, &rpyAgent);
// BiRRTTask;
- RemoteHandle<MotionPlanningTaskControlInterfacePrx> rspHandle = startBiRRT(scaledPlatformCSpace, startPos, goalPos, 0.1);
+ RemoteHandle<MotionPlanningTaskControlInterfacePrx> rspHandle =
+ startBiRRT(scaledPlatformCSpace, startPos, goalPos, 0.1);
Path posTrajectoryPath = finishBiRRT(rspHandle, scaledPlatformCSpace, "Platform");
- auto transformToGlobal = [&](armarx::VectorXf & pos2D)
+ auto transformToGlobal = [&](armarx::VectorXf& pos2D)
{
- Eigen::Vector2f globalPos = Eigen::Rotation2Df(rpyAgent(2)) * Eigen::Vector2f(pos2D.at(0), pos2D.at(1));
+ Eigen::Vector2f globalPos =
+ Eigen::Rotation2Df(rpyAgent(2)) * Eigen::Vector2f(pos2D.at(0), pos2D.at(1));
pos2D.at(0) = globalPos[0] + cSpacePlatform->getAgent().agentPose->position->x;
pos2D.at(1) = globalPos[1] + cSpacePlatform->getAgent().agentPose->position->y;
pos2D.at(2) = math::MathUtils::angleModPI(pos2D.at(2) + rpyAgent(2));
-
};
for (auto& e : posTrajectoryPath.nodes)
{
@@ -180,15 +196,21 @@ namespace armarx
return cSpacePlatform->pathToTrajectory(posTrajectoryPath);
}
- RemoteHandle<MotionPlanningTaskControlInterfacePrx> PlannedMotionProvider::startBiRRT(const ScaledCSpacePtr scaledCSpace,
- const armarx::VectorXf startPos,
- const armarx::VectorXf goalPos, float dcdStep, bool doRandomShortcutPostProcessing)
+ RemoteHandle<MotionPlanningTaskControlInterfacePrx>
+ PlannedMotionProvider::startBiRRT(const ScaledCSpacePtr scaledCSpace,
+ const armarx::VectorXf startPos,
+ const armarx::VectorXf goalPos,
+ float dcdStep,
+ bool doRandomShortcutPostProcessing)
{
// BiRRTTask;
- MotionPlanningTaskBasePtr taskRRT = new BiRRTTask {scaledCSpace, startPos, goalPos,
- getDefaultName() + ".birrt" + IceUtil::generateUUID(),
- 60, dcdStep
- };
+ MotionPlanningTaskBasePtr taskRRT =
+ new BiRRTTask{scaledCSpace,
+ startPos,
+ goalPos,
+ getDefaultName() + ".birrt" + IceUtil::generateUUID(),
+ 60,
+ dcdStep};
RemoteHandle<MotionPlanningTaskControlInterfacePrx> handle;
if (!doRandomShortcutPostProcessing)
{
@@ -197,22 +219,27 @@ namespace armarx
else
{
handle = mps->enqueueTask(
- new RandomShortcutPostprocessorTask(taskRRT, "RRTSmoothing" + IceUtil::generateUUID(),
- 1, dcdStep, maxPostProcessingSteps));
+ new RandomShortcutPostprocessorTask(taskRRT,
+ "RRTSmoothing" + IceUtil::generateUUID(),
+ 1,
+ dcdStep,
+ maxPostProcessingSteps));
}
planningTasks.push_back(handle);
return handle;
}
- Path PlannedMotionProvider::finishBiRRT(RemoteHandle<MotionPlanningTaskControlInterfacePrx> handle,
- const ScaledCSpacePtr scaledCSpace,
- const std::string roboPart)
+ Path
+ PlannedMotionProvider::finishBiRRT(RemoteHandle<MotionPlanningTaskControlInterfacePrx> handle,
+ const ScaledCSpacePtr scaledCSpace,
+ const std::string roboPart)
{
ARMARX_CHECK_EXPRESSION(handle);
handle->waitForFinishedRunning();
- ARMARX_IMPORTANT << roboPart + " trajectory planning took "
- << IceUtil::Time::microSeconds(handle->getRunningTime()).toMilliSecondsDouble()
- << " ms";
+ ARMARX_IMPORTANT
+ << roboPart + " trajectory planning took "
+ << IceUtil::Time::microSeconds(handle->getRunningTime()).toMilliSecondsDouble()
+ << " ms";
if (handle->getTaskStatus() == armarx::TaskStatus::ePlanningFailed)
{
@@ -220,8 +247,9 @@ namespace armarx
}
ARMARX_INFO << "RRTConnectTask Planning "
- << ((bool)(handle->getTaskStatus() == armarx::TaskStatus::ePlanningFailed) ? "failed"
- : "succeeded");
+ << ((bool)(handle->getTaskStatus() == armarx::TaskStatus::ePlanningFailed)
+ ? "failed"
+ : "succeeded");
ARMARX_INFO << "RRTConnectTask Planning status: " << handle->getTaskStatus();
auto posTrajectoryPath = handle->getPath();
@@ -230,31 +258,40 @@ namespace armarx
return posTrajectoryPath;
}
- void PlannedMotionProvider::preparePlatformCSpace(SimoxCSpacePtr cSpacePlatform, const MotionPlanningData& mpd,
- VectorXf* storeStart, VectorXf* storeGoal,
- ScaledCSpacePtr* storeScaledCSpace, Eigen::Vector3f* storeRpyAgent)
+ void
+ PlannedMotionProvider::preparePlatformCSpace(SimoxCSpacePtr cSpacePlatform,
+ const MotionPlanningData& mpd,
+ VectorXf* storeStart,
+ VectorXf* storeGoal,
+ ScaledCSpacePtr* storeScaledCSpace,
+ Eigen::Vector3f* storeRpyAgent)
{
- cSpacePlatform->setStationaryObjectMargin(getProperty<float>("PlatformMotionSafetyMargin").getValue());
+ cSpacePlatform->setStationaryObjectMargin(
+ getProperty<float>("PlatformMotionSafetyMargin").getValue());
ARMARX_CHECK_EXPRESSION(cSpacePlatform);
Eigen::Vector3f rpy, rpyAgent;
- VirtualRobot::MathTools::eigen4f2rpy(PosePtr::dynamicCast(mpd.globalPoseStart)->toEigen(), rpy);
- VirtualRobot::MathTools::eigen4f2rpy(PosePtr::dynamicCast(cSpacePlatform->getAgent().agentPose)->toEigen(),
- rpyAgent);
- armarx::VectorXf startPos {mpd.globalPoseStart->position->x - cSpacePlatform->getAgent().agentPose->position->x,
- mpd.globalPoseStart->position->y - cSpacePlatform->getAgent().agentPose->position->y,
- math::MathUtils::angleModPI(rpy(2) - rpyAgent(2))
- };
-
- Eigen::Vector2f localStartPos = Eigen::Rotation2Df(-rpyAgent(2)) * Eigen::Vector2f(startPos.at(0), startPos.at(1));
+ VirtualRobot::MathTools::eigen4f2rpy(PosePtr::dynamicCast(mpd.globalPoseStart)->toEigen(),
+ rpy);
+ VirtualRobot::MathTools::eigen4f2rpy(
+ PosePtr::dynamicCast(cSpacePlatform->getAgent().agentPose)->toEigen(), rpyAgent);
+ armarx::VectorXf startPos{
+ mpd.globalPoseStart->position->x - cSpacePlatform->getAgent().agentPose->position->x,
+ mpd.globalPoseStart->position->y - cSpacePlatform->getAgent().agentPose->position->y,
+ math::MathUtils::angleModPI(rpy(2) - rpyAgent(2))};
+
+ Eigen::Vector2f localStartPos =
+ Eigen::Rotation2Df(-rpyAgent(2)) * Eigen::Vector2f(startPos.at(0), startPos.at(1));
startPos.at(0) = localStartPos(0);
startPos.at(1) = localStartPos(1);
- VirtualRobot::MathTools::eigen4f2rpy(PosePtr::dynamicCast(mpd.globalPoseGoal)->toEigen(), rpy);
- armarx::VectorXf goalPos {mpd.globalPoseGoal->position->x - cSpacePlatform->getAgent().agentPose->position->x,
- mpd.globalPoseGoal->position->y - cSpacePlatform->getAgent().agentPose->position->y,
- math::MathUtils::angleModPI(rpy(2) - rpyAgent(2))
- };
- Eigen::Vector2f localGoalPos = Eigen::Rotation2Df(-rpyAgent(2)) * Eigen::Vector2f(goalPos.at(0), goalPos.at(1));
+ VirtualRobot::MathTools::eigen4f2rpy(PosePtr::dynamicCast(mpd.globalPoseGoal)->toEigen(),
+ rpy);
+ armarx::VectorXf goalPos{
+ mpd.globalPoseGoal->position->x - cSpacePlatform->getAgent().agentPose->position->x,
+ mpd.globalPoseGoal->position->y - cSpacePlatform->getAgent().agentPose->position->y,
+ math::MathUtils::angleModPI(rpy(2) - rpyAgent(2))};
+ Eigen::Vector2f localGoalPos =
+ Eigen::Rotation2Df(-rpyAgent(2)) * Eigen::Vector2f(goalPos.at(0), goalPos.at(1));
goalPos.at(0) = localGoalPos(0);
goalPos.at(1) = localGoalPos(1);
@@ -269,15 +306,19 @@ namespace armarx
*storeRpyAgent = rpyAgent;
}
- void PlannedMotionProvider::prepareJointCSpace(SimoxCSpacePtr cSpace, const MotionPlanningData& mpd,
- VectorXf* storeStart, VectorXf* storeGoal,
- ScaledCSpacePtr* storeScaledCSpace)
+ void
+ PlannedMotionProvider::prepareJointCSpace(SimoxCSpacePtr cSpace,
+ const MotionPlanningData& mpd,
+ VectorXf* storeStart,
+ VectorXf* storeGoal,
+ ScaledCSpacePtr* storeScaledCSpace)
{
cSpace->setStationaryObjectMargin(getProperty<float>("JointMotionSafetyMargin").getValue());
ARMARX_CHECK_EXPRESSION(cSpace);
ARMARX_INFO << "Entered prepareJointCSpace";
Ice::FloatSeq cSpaceScaling;
- for (VirtualRobot::RobotNodePtr node : localRobot->getRobotNodeSet(mpd.rnsToUse)->getAllRobotNodes())
+ for (VirtualRobot::RobotNodePtr node :
+ localRobot->getRobotNodeSet(mpd.rnsToUse)->getAllRobotNodes())
{
cSpaceScaling.push_back(node->isTranslationalJoint() ? 0.001f : 1.0f);
}
@@ -294,7 +335,5 @@ namespace armarx
*storeStart = startCfg;
*storeGoal = goalCfg;
*storeScaledCSpace = scaledJointCSpace;
-
}
-}
-
+} // namespace armarx
diff --git a/source/RobotComponents/components/PlannedMotionProvider/PlannedMotionProvider.h b/source/RobotComponents/components/PlannedMotionProvider/PlannedMotionProvider.h
index e426cd6e972c011a46449d09aded5bbfcf85068f..6d7312cef1a2e719c5dea4592a1f915bf27be767 100644
--- a/source/RobotComponents/components/PlannedMotionProvider/PlannedMotionProvider.h
+++ b/source/RobotComponents/components/PlannedMotionProvider/PlannedMotionProvider.h
@@ -26,9 +26,9 @@
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/util/distributed/RemoteHandle/RemoteHandle.h>
+#include <RobotComponents/components/MotionPlanning/MotionPlanningObjectFactories.h>
#include <RobotComponents/interface/components/MotionPlanning/MotionPlanningServer.h>
#include <RobotComponents/interface/components/PlannedMotionProviderInterface.h>
-#include <RobotComponents/components/MotionPlanning/MotionPlanningObjectFactories.h>
namespace armarx
{
@@ -36,16 +36,17 @@ namespace armarx
* @class PlannedMotionProviderPropertyDefinitions
* @brief
*/
- class PlannedMotionProviderPropertyDefinitions :
- public armarx::ComponentPropertyDefinitions
+ class PlannedMotionProviderPropertyDefinitions : public armarx::ComponentPropertyDefinitions
{
public:
PlannedMotionProviderPropertyDefinitions(std::string prefix) :
armarx::ComponentPropertyDefinitions(prefix)
{
//defineRequiredProperty<std::string>("PropertyName", "Description");
- defineOptionalProperty<float>("JointMotionSafetyMargin", 80.0f, "Safety margin for joint arm motion in mm");
- defineOptionalProperty<float>("PlatformMotionSafetyMargin", 100.0f, "Safety margin for platform motion in mm");
+ defineOptionalProperty<float>(
+ "JointMotionSafetyMargin", 80.0f, "Safety margin for joint arm motion in mm");
+ defineOptionalProperty<float>(
+ "PlatformMotionSafetyMargin", 100.0f, "Safety margin for platform motion in mm");
}
};
@@ -68,21 +69,24 @@ namespace armarx
/**
* @see armarx::ManagedIceObject::getDefaultName()
*/
- std::string getDefaultName() const override
+ std::string
+ getDefaultName() const override
{
return "PlannedMotionProvider";
}
- GraspingTrajectory
- planMotion(const SimoxCSpaceBasePtr& cSpaceBase, const SimoxCSpaceBasePtr& cSpacePlatformBase,
- const MotionPlanningData& mpd, const Ice::Current& c = Ice::emptyCurrent) override;
+ GraspingTrajectory planMotion(const SimoxCSpaceBasePtr& cSpaceBase,
+ const SimoxCSpaceBasePtr& cSpacePlatformBase,
+ const MotionPlanningData& mpd,
+ const Ice::Current& c = Ice::emptyCurrent) override;
GraspingTrajectory planMotionParallel(const SimoxCSpaceBasePtr& cSpaceBase,
const SimoxCSpaceBasePtr& cSpacePlatformBase,
const MotionPlanningData& mpd,
const Ice::Current& c) override;
// does it make sense to return a GraspingTrajectory
- TrajectoryBasePtr planJointMotion(const SimoxCSpaceBasePtr& cSpaceBase, const MotionPlanningData& mpd,
+ TrajectoryBasePtr planJointMotion(const SimoxCSpaceBasePtr& cSpaceBase,
+ const MotionPlanningData& mpd,
const Ice::Current& c = Ice::emptyCurrent) override;
TrajectoryBasePtr planPlatformMotion(const SimoxCSpaceBasePtr& cSpacePlatformBase,
@@ -124,22 +128,33 @@ namespace armarx
// how to make those public?
- TrajectoryPtr calculateJointTrajectory(const SimoxCSpaceBasePtr& cSpaceBase, const MotionPlanningData& mpd,
+ TrajectoryPtr calculateJointTrajectory(const SimoxCSpaceBasePtr& cSpaceBase,
+ const MotionPlanningData& mpd,
const Ice::Current& c = Ice::emptyCurrent);
TrajectoryPtr calculatePlatformTrajectory(const SimoxCSpaceBasePtr& cSpacePlatformBase,
- const MotionPlanningData& mpd,
- const Ice::Current& c = Ice::emptyCurrent);
- void preparePlatformCSpace(SimoxCSpacePtr cSpacePlatform, const MotionPlanningData& mpd,
- VectorXf* storeStart, VectorXf* storeGoal,
- ScaledCSpacePtr* storeScaledCSpace, Eigen::Vector3f* storeRpyAgent);
- void prepareJointCSpace(SimoxCSpacePtr cSpace, const MotionPlanningData& mpd,
- VectorXf* storeStart, VectorXf* storeGoal, ScaledCSpacePtr* storeScaledCSpace);
-
- RemoteHandle<MotionPlanningTaskControlInterfacePrx> startBiRRT(ScaledCSpacePtr scaledCSpace, VectorXf startPos,
- VectorXf goalPos, float dcdStep, bool doRandomShortcutPostProcessing = false);
- Path finishBiRRT(RemoteHandle<MotionPlanningTaskControlInterfacePrx> handle, const ScaledCSpacePtr scaledCSpace,
+ const MotionPlanningData& mpd,
+ const Ice::Current& c = Ice::emptyCurrent);
+ void preparePlatformCSpace(SimoxCSpacePtr cSpacePlatform,
+ const MotionPlanningData& mpd,
+ VectorXf* storeStart,
+ VectorXf* storeGoal,
+ ScaledCSpacePtr* storeScaledCSpace,
+ Eigen::Vector3f* storeRpyAgent);
+ void prepareJointCSpace(SimoxCSpacePtr cSpace,
+ const MotionPlanningData& mpd,
+ VectorXf* storeStart,
+ VectorXf* storeGoal,
+ ScaledCSpacePtr* storeScaledCSpace);
+
+ RemoteHandle<MotionPlanningTaskControlInterfacePrx>
+ startBiRRT(ScaledCSpacePtr scaledCSpace,
+ VectorXf startPos,
+ VectorXf goalPos,
+ float dcdStep,
+ bool doRandomShortcutPostProcessing = false);
+ Path finishBiRRT(RemoteHandle<MotionPlanningTaskControlInterfacePrx> handle,
+ const ScaledCSpacePtr scaledCSpace,
const std::string roboPart);
};
-}
-
+} // namespace armarx
diff --git a/source/RobotComponents/components/PlannedMotionProvider/test/PlannedMotionProviderTest.cpp b/source/RobotComponents/components/PlannedMotionProvider/test/PlannedMotionProviderTest.cpp
index e14f228adab532045635aad116d6c119e6cea150..a612a6b8228afb491e1879ccb451cdd6576397b5 100644
--- a/source/RobotComponents/components/PlannedMotionProvider/test/PlannedMotionProviderTest.cpp
+++ b/source/RobotComponents/components/PlannedMotionProvider/test/PlannedMotionProviderTest.cpp
@@ -24,11 +24,11 @@
#define ARMARX_BOOST_TEST
+#include <iostream>
+
#include <RobotComponents/Test.h>
#include <RobotComponents/components/PlannedMotionProvider/PlannedMotionProvider.h>
-#include <iostream>
-
BOOST_AUTO_TEST_CASE(testExample)
{
armarx::PlannedMotionProvider instance;
diff --git a/source/RobotComponents/components/ReflexCombination/ReflexCombination.cpp b/source/RobotComponents/components/ReflexCombination/ReflexCombination.cpp
index 2d0539d69c5eed256d72fa2597d95be2136b746e..216b664020506545860cfa047ecc3f500d057ffa 100644
--- a/source/RobotComponents/components/ReflexCombination/ReflexCombination.cpp
+++ b/source/RobotComponents/components/ReflexCombination/ReflexCombination.cpp
@@ -27,8 +27,8 @@
using namespace armarx;
-
-void ReflexCombination::onInitComponent()
+void
+ReflexCombination::onInitComponent()
{
usingProxy(getProperty<std::string>("KinematicUnitName").getValue());
usingProxy(getProperty<std::string>("RobotStateName").getValue());
@@ -65,15 +65,8 @@ void ReflexCombination::onInitComponent()
eye_yaw_right = "Eye_Right";
neck_roll = "Neck_2_Roll";
- headJointNames =
- {
- "Neck_1_Pitch",
- neck_roll,
- "Neck_3_Yaw",
- eye_pitch_left,
- eye_yaw_right,
- eye_yaw_left
- };
+ headJointNames = {
+ "Neck_1_Pitch", neck_roll, "Neck_3_Yaw", eye_pitch_left, eye_yaw_right, eye_yaw_left};
}
velocityBased = getProperty<bool>("VelocityBasedControl").getValue();
@@ -95,7 +88,6 @@ void ReflexCombination::onInitComponent()
jointIKWeight = getProperty<float>("JointIK").getValue();
-
kp = getProperty<float>("kp").getValue();
ki = getProperty<float>("ki").getValue();
kd = getProperty<float>("kd").getValue();
@@ -114,7 +106,8 @@ void ReflexCombination::onInitComponent()
okr->setPIDValues(kp, ki, kd);
- execCombineReflexesTask = new PeriodicTask<ReflexCombination>(this, &ReflexCombination::combineReflexes, 5, false, "ReflexCombinationTask");
+ execCombineReflexesTask = new PeriodicTask<ReflexCombination>(
+ this, &ReflexCombination::combineReflexes, 5, false, "ReflexCombinationTask");
execCombineReflexesTask->setDelayWarningTolerance(2);
@@ -128,11 +121,11 @@ void ReflexCombination::onInitComponent()
IMU_GyroFilters["Z"] = new armarx::filters::MedianFilter(sampling_size);
IMU_GyroFilters["Z"]->update(0, new Variant((float)0.0));
- double frequency = 10.; // cut off frequency
+ double frequency = 10.; // cut off frequency
int sampleRate = 100;
- double resonance = 1.0; // should be between 0.1 and sqrt(2)
+ double resonance = 1.0; // should be between 0.1 and sqrt(2)
- std::map<std::string, float> filterProperties {{"minSampleTimeDelta", 100 * 1000.0}};
+ std::map<std::string, float> filterProperties{{"minSampleTimeDelta", 100 * 1000.0}};
for (auto& joint_name : headJointNames)
{
@@ -141,36 +134,40 @@ void ReflexCombination::onInitComponent()
VelocityFilters[joint_name]->setProperties(filterProperties);
VelocityFilters[joint_name]->update(0, new Variant((double)0.0));
- PreVelocityFilters[joint_name] = new armarx::filters::ButterworthFilter(frequency, sampleRate, Lowpass, resonance);
+ PreVelocityFilters[joint_name] =
+ new armarx::filters::ButterworthFilter(frequency, sampleRate, Lowpass, resonance);
PreVelocityFilters[joint_name]->setInitialValue(0.);
PreVelocityFilters[joint_name]->update(0, new Variant((double)0.0));
-
}
frequency = 5.0;
sampleRate = 100;
resonance = 1.0;
- FlowFilters["X"] = new armarx::filters::ButterworthFilter(frequency, sampleRate, Lowpass, resonance);
+ FlowFilters["X"] =
+ new armarx::filters::ButterworthFilter(frequency, sampleRate, Lowpass, resonance);
FlowFilters["X"]->setInitialValue(0.);
FlowFilters["X"]->update(0, new Variant((float)0.0));
- FlowFilters["Y"] = new armarx::filters::ButterworthFilter(frequency, sampleRate, Lowpass, resonance);
+ FlowFilters["Y"] =
+ new armarx::filters::ButterworthFilter(frequency, sampleRate, Lowpass, resonance);
FlowFilters["Y"]->setInitialValue(0.);
FlowFilters["Y"]->update(0, new Variant((float)0.0));
-
t_init = armarx::TimeUtil::GetTime().toMilliSecondsDouble() / 1000.;
-
}
-
-void ReflexCombination::onConnectComponent()
+void
+ReflexCombination::onConnectComponent()
{
- kinUnitPrx = getProxy<KinematicUnitInterfacePrx>(getProperty<std::string>("KinematicUnitName").getValue());
- robotStateComponent = getProxy<RobotStateComponentInterfacePrx>(getProperty<std::string>("RobotStateName").getValue());
+ kinUnitPrx = getProxy<KinematicUnitInterfacePrx>(
+ getProperty<std::string>("KinematicUnitName").getValue());
+ robotStateComponent = getProxy<RobotStateComponentInterfacePrx>(
+ getProperty<std::string>("RobotStateName").getValue());
- debugObserver = getTopic<DebugObserverInterfacePrx>(getProperty<std::string>("DebugObserverName").getValue());
- imageSourceSelection = getProxy<ImageSourceSelectionInterfacePrx>(getProperty<std::string>("ImageSourceSelectionName").getValue());
+ debugObserver = getTopic<DebugObserverInterfacePrx>(
+ getProperty<std::string>("DebugObserverName").getValue());
+ imageSourceSelection = getProxy<ImageSourceSelectionInterfacePrx>(
+ getProperty<std::string>("ImageSourceSelectionName").getValue());
drawer = getTopic<DebugDrawerInterfacePrx>("DebugDrawerUpdates");
@@ -188,40 +185,45 @@ void ReflexCombination::onConnectComponent()
if ((combinationMethod == Reafference) && !jointIKEnabled)
{
- ARMARX_WARNING << "Reafference combination cannot work without IK method enabled. Setting to weighted sum imnstead";
+ ARMARX_WARNING << "Reafference combination cannot work without IK method enabled. Setting "
+ "to weighted sum imnstead";
combinationMethod = WeightedSum;
}
execCombineReflexesTask->start();
-
}
-
-void ReflexCombination::onDisconnectComponent()
+void
+ReflexCombination::onDisconnectComponent()
{
execCombineReflexesTask->stop();
}
-
-void ReflexCombination::onExitComponent()
+void
+ReflexCombination::onExitComponent()
{
delete vor;
delete jointIK;
delete okr;
-
}
-armarx::PropertyDefinitionsPtr ReflexCombination::createPropertyDefinitions()
+armarx::PropertyDefinitionsPtr
+ReflexCombination::createPropertyDefinitions()
{
- return armarx::PropertyDefinitionsPtr(new ReflexCombinationPropertyDefinitions(
- getConfigIdentifier()));
+ return armarx::PropertyDefinitionsPtr(
+ new ReflexCombinationPropertyDefinitions(getConfigIdentifier()));
}
-void ReflexCombination::updateWeights(float vorWeight, float okrWeight, float jointIKWeight, const Ice::Current& c)
+void
+ReflexCombination::updateWeights(float vorWeight,
+ float okrWeight,
+ float jointIKWeight,
+ const Ice::Current& c)
{
- ARMARX_LOG << "new reflex weights vor:" << vorWeight << " okr:" << okrWeight << " jointik:" << jointIKWeight;
+ ARMARX_LOG << "new reflex weights vor:" << vorWeight << " okr:" << okrWeight
+ << " jointik:" << jointIKWeight;
StringVariantBaseMap debugValues;
@@ -242,7 +244,8 @@ void ReflexCombination::updateWeights(float vorWeight, float okrWeight, float jo
jointIKEnabled = (jointIKWeight > 0);
okrEnabled = (okrWeight > 0);
- ARMARX_LOG << "enabled reflexes vor:" << vorEnabled << " okr:" << okrEnabled << " jointik:" << jointIKEnabled;
+ ARMARX_LOG << "enabled reflexes vor:" << vorEnabled << " okr:" << okrEnabled
+ << " jointik:" << jointIKEnabled;
vor->setEnabled(vorEnabled);
okr->setEnabled(okrEnabled);
@@ -251,7 +254,8 @@ void ReflexCombination::updateWeights(float vorWeight, float okrWeight, float jo
ARMARX_LOG << "reflexes enabled.";
}
-void ReflexCombination::setReafferenceMethod(bool isReafference, const Ice::Current& c)
+void
+ReflexCombination::setReafferenceMethod(bool isReafference, const Ice::Current& c)
{
if (isReafference)
{
@@ -263,13 +267,15 @@ void ReflexCombination::setReafferenceMethod(bool isReafference, const Ice::Curr
}
}
-void ReflexCombination::combineReflexes()
+void
+ReflexCombination::combineReflexes()
{
std::scoped_lock lock(mutex);
- if (!(vorEnabled || jointIKEnabled || okrEnabled))
+ if (!(vorEnabled || jointIKEnabled || okrEnabled))
{
- ARMARX_LOG << deactivateSpam(1) << "at least one reflex must be enabled to stabilize the gaze!";
+ ARMARX_LOG << deactivateSpam(1)
+ << "at least one reflex must be enabled to stabilize the gaze!";
return;
}
@@ -292,18 +298,18 @@ void ReflexCombination::combineReflexes()
// logging to debug observed via single channel using the map
StringVariantBaseMap mapValues;
- mapValues["optFlow_predX"] = new Variant(jointIK->optFlow_pred[0]);
- mapValues["optFlow_predY"] = new Variant(jointIK->optFlow_pred[1]);
- mapValues["meanOptFlow_pred"] = new Variant((float)jointIK->mean_optFl_pred);
- mapValues["imu_gyro_predX"] = new Variant(jointIK->gyroscopeRotation_pred[0]);
- mapValues["imu_gyro_predY"] = new Variant(jointIK->gyroscopeRotation_pred[1]);
- mapValues["imu_gyro_predZ"] = new Variant(jointIK->gyroscopeRotation_pred[2]);
- mapValues["vel_filtered"] = new Variant(VelocityFilters[eye_yaw_left]->getValue()->getDouble());
- mapValues["flow_filteredX"] = new Variant(FlowFilters["X"]->getValue()->getDouble());
- mapValues["flow_filteredY"] = new Variant(FlowFilters["Y"]->getValue()->getDouble());
- mapValues["imu_filteredX"] = new Variant(IMU_GyroFilters["X"]->getValue()->getFloat());
- mapValues["imu_filteredY"] = new Variant(IMU_GyroFilters["Y"]->getValue()->getFloat());
- mapValues["imu_filteredZ"] = new Variant(IMU_GyroFilters["Z"]->getValue()->getFloat());
+ mapValues["optFlow_predX"] = new Variant(jointIK->optFlow_pred[0]);
+ mapValues["optFlow_predY"] = new Variant(jointIK->optFlow_pred[1]);
+ mapValues["meanOptFlow_pred"] = new Variant((float)jointIK->mean_optFl_pred);
+ mapValues["imu_gyro_predX"] = new Variant(jointIK->gyroscopeRotation_pred[0]);
+ mapValues["imu_gyro_predY"] = new Variant(jointIK->gyroscopeRotation_pred[1]);
+ mapValues["imu_gyro_predZ"] = new Variant(jointIK->gyroscopeRotation_pred[2]);
+ mapValues["vel_filtered"] = new Variant(VelocityFilters[eye_yaw_left]->getValue()->getDouble());
+ mapValues["flow_filteredX"] = new Variant(FlowFilters["X"]->getValue()->getDouble());
+ mapValues["flow_filteredY"] = new Variant(FlowFilters["Y"]->getValue()->getDouble());
+ mapValues["imu_filteredX"] = new Variant(IMU_GyroFilters["X"]->getValue()->getFloat());
+ mapValues["imu_filteredY"] = new Variant(IMU_GyroFilters["Y"]->getValue()->getFloat());
+ mapValues["imu_filteredZ"] = new Variant(IMU_GyroFilters["Z"]->getValue()->getFloat());
debugObserver->setDebugChannel("ForwardPredictor", mapValues);
/*
@@ -383,7 +389,7 @@ void ReflexCombination::combineReflexes()
// set the unset velocities to 0
if (velocityBased)
{
- for (auto const& headJoint : headJointNames)
+ for (auto const& headJoint : headJointNames)
{
if (resultJointValues.find(headJoint) == resultJointValues.end())
{
@@ -398,12 +404,13 @@ void ReflexCombination::combineReflexes()
try
{
double kp = 5.;
- resultJointValues["Eye_Left"] = resultJointValues["Eye_Right"] + kp * (PreVelocityFilters["Eye_Right"]->getValue()->getDouble() - PreVelocityFilters["Eye_Left"]->getValue()->getDouble());
-
+ resultJointValues["Eye_Left"] =
+ resultJointValues["Eye_Right"] +
+ kp * (PreVelocityFilters["Eye_Right"]->getValue()->getDouble() -
+ PreVelocityFilters["Eye_Left"]->getValue()->getDouble());
}
catch (...)
{
-
}
}
@@ -422,7 +429,8 @@ void ReflexCombination::combineReflexes()
if (neckPerturbation && velocityBased)
{
controlModes["Neck_3_Yaw"] = eVelocityControl;
- resultJointValues["Neck_3_Yaw"] = 4. * cos((armarx::TimeUtil::GetTime().toMilliSecondsDouble() / 1000. - t_init));
+ resultJointValues["Neck_3_Yaw"] =
+ 4. * cos((armarx::TimeUtil::GetTime().toMilliSecondsDouble() / 1000. - t_init));
}
try
@@ -475,11 +483,13 @@ void ReflexCombination::combineReflexes()
{
ARMARX_IMPORTANT << "Setting joint values failed!";
}
-
}
-
-void ReflexCombination::reportPlatformVelocity(Ice::Float currentPlatformVelocityX, Ice::Float currentPlatformVelocityY, Ice::Float currentPlatformVelocityRotation, const Ice::Current& c)
+void
+ReflexCombination::reportPlatformVelocity(Ice::Float currentPlatformVelocityX,
+ Ice::Float currentPlatformVelocityY,
+ Ice::Float currentPlatformVelocityRotation,
+ const Ice::Current& c)
{
if (jointIKEnabled)
{
@@ -487,11 +497,19 @@ void ReflexCombination::reportPlatformVelocity(Ice::Float currentPlatformVelocit
}
}
-void armarx::ReflexCombination::reportControlModeChanged(const NameControlModeMap&, Ice::Long timestamp, bool, const Ice::Current&)
+void
+armarx::ReflexCombination::reportControlModeChanged(const NameControlModeMap&,
+ Ice::Long timestamp,
+ bool,
+ const Ice::Current&)
{
}
-void armarx::ReflexCombination::reportJointAngles(const NameValueMap& values, Ice::Long timestamp, bool valueChanged, const Ice::Current& c)
+void
+armarx::ReflexCombination::reportJointAngles(const NameValueMap& values,
+ Ice::Long timestamp,
+ bool valueChanged,
+ const Ice::Current& c)
{
std::scoped_lock lock(mutex);
@@ -527,7 +545,6 @@ void armarx::ReflexCombination::reportJointAngles(const NameValueMap& values, Ic
{
okr->start();
}
-
}
else
{
@@ -552,11 +569,16 @@ void armarx::ReflexCombination::reportJointAngles(const NameValueMap& values, Ic
for (auto& joint_name : headJointNames)
{
PreVelocityFilters[joint_name]->update(timestamp, new Variant(tmp[joint_name]));
- VelocityFilters[joint_name]->update(timestamp, new Variant(PreVelocityFilters[joint_name]->getValue()->getDouble()));
+ VelocityFilters[joint_name]->update(
+ timestamp, new Variant(PreVelocityFilters[joint_name]->getValue()->getDouble()));
}
}
-void armarx::ReflexCombination::reportJointVelocities(const NameValueMap& velocities, Ice::Long timestamp, bool valueChanged, const Ice::Current& c)
+void
+armarx::ReflexCombination::reportJointVelocities(const NameValueMap& velocities,
+ Ice::Long timestamp,
+ bool valueChanged,
+ const Ice::Current& c)
{
std::scoped_lock lock(mutex);
@@ -570,7 +592,6 @@ void armarx::ReflexCombination::reportJointVelocities(const NameValueMap& veloci
}
-
if (vorEnabled)
{
vor->reportJointVelocities(velocities_filtered, valueChanged, c);
@@ -583,37 +604,61 @@ void armarx::ReflexCombination::reportJointVelocities(const NameValueMap& veloci
{
if (combinationMethod == Reafference && jointIKEnabled)
{
- velocities_filtered[eye_yaw_left] = 0.; // no need of removing self induced velocities as already removed by the prediction
+ velocities_filtered[eye_yaw_left] =
+ 0.; // no need of removing self induced velocities as already removed by the prediction
velocities_filtered[eye_yaw_right] = 0.;
velocities_filtered[eye_pitch_left] = 0.;
}
okr->reportJointVelocities(velocities_filtered, valueChanged, timestamp, c);
}
-
}
-void armarx::ReflexCombination::reportJointTorques(const NameValueMap&, Ice::Long timestamp, bool, const Ice::Current&)
+void
+armarx::ReflexCombination::reportJointTorques(const NameValueMap&,
+ Ice::Long timestamp,
+ bool,
+ const Ice::Current&)
{
}
-void armarx::ReflexCombination::reportJointAccelerations(const NameValueMap&, Ice::Long timestamp, bool, const Ice::Current&)
+void
+armarx::ReflexCombination::reportJointAccelerations(const NameValueMap&,
+ Ice::Long timestamp,
+ bool,
+ const Ice::Current&)
{
}
-void armarx::ReflexCombination::reportJointCurrents(const NameValueMap&, Ice::Long timestamp, bool, const Ice::Current&)
+void
+armarx::ReflexCombination::reportJointCurrents(const NameValueMap&,
+ Ice::Long timestamp,
+ bool,
+ const Ice::Current&)
{
}
-void armarx::ReflexCombination::reportJointMotorTemperatures(const NameValueMap&, Ice::Long timestamp, bool, const Ice::Current&)
+void
+armarx::ReflexCombination::reportJointMotorTemperatures(const NameValueMap&,
+ Ice::Long timestamp,
+ bool,
+ const Ice::Current&)
{
-
}
-void armarx::ReflexCombination::reportJointStatuses(const NameStatusMap&, Ice::Long timestamp, bool, const Ice::Current&)
+void
+armarx::ReflexCombination::reportJointStatuses(const NameStatusMap&,
+ Ice::Long timestamp,
+ bool,
+ const Ice::Current&)
{
}
-void armarx::ReflexCombination::reportNewOpticalFlow(Ice::Float x, Ice::Float y, Ice::Float deltaT, Ice::Long timestamp, const Ice::Current& c)
+void
+armarx::ReflexCombination::reportNewOpticalFlow(Ice::Float x,
+ Ice::Float y,
+ Ice::Float deltaT,
+ Ice::Long timestamp,
+ const Ice::Current& c)
{
std::scoped_lock lock(mutex);
@@ -647,8 +692,10 @@ void armarx::ReflexCombination::reportNewOpticalFlow(Ice::Float x, Ice::Float y,
}
}
-
-void armarx::ReflexCombination::reportHeadTargetChanged(const NameValueMap& targetJointAngles, const FramedPositionBasePtr& targetPosition, const Ice::Current&)
+void
+armarx::ReflexCombination::reportHeadTargetChanged(const NameValueMap& targetJointAngles,
+ const FramedPositionBasePtr& targetPosition,
+ const Ice::Current&)
{
std::scoped_lock lock(mutex);
@@ -672,11 +719,14 @@ void armarx::ReflexCombination::reportHeadTargetChanged(const NameValueMap& targ
{
okr->stop();
}
-
-
}
-void armarx::ReflexCombination::reportSensorValues(const std::string& device, const std::string& name, const IMUData& values, const TimestampBasePtr& timestamp, const Ice::Current& c)
+void
+armarx::ReflexCombination::reportSensorValues(const std::string& device,
+ const std::string& name,
+ const IMUData& values,
+ const TimestampBasePtr& timestamp,
+ const Ice::Current& c)
{
@@ -690,9 +740,12 @@ void armarx::ReflexCombination::reportSensorValues(const std::string& device, co
*/
if (IMU_GyroFilters["X"] && IMU_GyroFilters["Y"] && IMU_GyroFilters["Z"])
{
- IMU_GyroFilters["X"]->update(timestamp->timestamp, new Variant((float) values.gyroscopeRotation[0]));
- IMU_GyroFilters["Y"]->update(timestamp->timestamp, new Variant((float) values.gyroscopeRotation[1]));
- IMU_GyroFilters["Z"]->update(timestamp->timestamp, new Variant((float) values.gyroscopeRotation[2]));
+ IMU_GyroFilters["X"]->update(timestamp->timestamp,
+ new Variant((float)values.gyroscopeRotation[0]));
+ IMU_GyroFilters["Y"]->update(timestamp->timestamp,
+ new Variant((float)values.gyroscopeRotation[1]));
+ IMU_GyroFilters["Z"]->update(timestamp->timestamp,
+ new Variant((float)values.gyroscopeRotation[2]));
}
IMUData values2 = values;
@@ -722,9 +775,8 @@ void armarx::ReflexCombination::reportSensorValues(const std::string& device, co
}
}
-
-
-void armarx::ReflexCombination::setImageQuality(float quality)
+void
+armarx::ReflexCombination::setImageQuality(float quality)
{
Ice::StringSeq imageSources = getProperty<Ice::StringSeq>("ImageProviders");
ARMARX_CHECK_GREATER_EQUAL(imageSources.size(), 2) << imageSources;
@@ -738,12 +790,20 @@ void armarx::ReflexCombination::setImageQuality(float quality)
}
}
-void armarx::ReflexCombination::reportNewTrackingError(Ice::Float pixelX, Ice::Float pixelY, Ice::Float angleX, Ice::Float angleY, const Ice::Current&)
+void
+armarx::ReflexCombination::reportNewTrackingError(Ice::Float pixelX,
+ Ice::Float pixelY,
+ Ice::Float angleX,
+ Ice::Float angleY,
+ const Ice::Current&)
{
okr->reportNewTrackingError(pixelX, pixelY, angleX, angleY);
}
-
-void armarx::ReflexCombination::reportPlatformOdometryPose(Ice::Float, Ice::Float, Ice::Float, const Ice::Current&)
+void
+armarx::ReflexCombination::reportPlatformOdometryPose(Ice::Float,
+ Ice::Float,
+ Ice::Float,
+ const Ice::Current&)
{
}
diff --git a/source/RobotComponents/components/ReflexCombination/ReflexCombination.h b/source/RobotComponents/components/ReflexCombination/ReflexCombination.h
index 0fe6ea4adcf7db12bdd37efda74000983897f572..5b7e0c817b3c4a40d8a9aa4ee18df881687d7dba 100644
--- a/source/RobotComponents/components/ReflexCombination/ReflexCombination.h
+++ b/source/RobotComponents/components/ReflexCombination/ReflexCombination.h
@@ -23,52 +23,52 @@
#pragma once
-#include "reflex.h"
-#include "vor.h"
-#include "jointik.h"
-#include "feedforward.h"
-#include "okr.h"
+#include <VirtualRobot/IK/GazeIK.h>
+#include <VirtualRobot/Nodes/RobotNode.h>
+#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/RobotNodeSet.h>
+#include <VirtualRobot/VirtualRobot.h>
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/observers/DebugObserver.h>
+#include <ArmarXCore/observers/filters/ButterworthFilter.h>
+#include <ArmarXCore/observers/filters/DerivationFilter.h>
+#include <ArmarXCore/observers/filters/MedianFilter.h>
-#include <VirtualRobot/VirtualRobot.h>
-#include <VirtualRobot/Robot.h>
-#include <VirtualRobot/Nodes/RobotNode.h>
-#include <VirtualRobot/RobotNodeSet.h>
-#include <VirtualRobot/IK/GazeIK.h>
-
+#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
#include <RobotAPI/libraries/core/Pose.h>
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
-#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
-
#include <VisionX/interface/components/ImageSourceSelectionInterface.h>
#include <RobotComponents/interface/GazeStabilizationInterface.h>
-#include <ArmarXCore/observers/filters/DerivationFilter.h>
-#include <ArmarXCore/observers/filters/ButterworthFilter.h>
-#include <ArmarXCore/observers/filters/MedianFilter.h>
+
+#include "feedforward.h"
+#include "jointik.h"
+#include "okr.h"
+#include "reflex.h"
+#include "vor.h"
namespace armarx
{
enum COMBINATION_METHOD
{
- WeightedSum, Reafference
+ WeightedSum,
+ Reafference
};
/**
* @class ReflexCombinationPropertyDefinitions
* @brief
*/
- class ReflexCombinationPropertyDefinitions:
- public armarx::ComponentPropertyDefinitions
+ class ReflexCombinationPropertyDefinitions : public armarx::ComponentPropertyDefinitions
{
public:
- ReflexCombinationPropertyDefinitions(std::string prefix):
+ ReflexCombinationPropertyDefinitions(std::string prefix) :
armarx::ComponentPropertyDefinitions(prefix)
{
- defineOptionalProperty<bool>("VelocityBasedControl", true, "Use velocity based reflexes");
+ defineOptionalProperty<bool>(
+ "VelocityBasedControl", true, "Use velocity based reflexes");
defineOptionalProperty<float>("kp", 1.0, "p part of regulator");
defineOptionalProperty<float>("ki", 0.0, "i part of regulator");
@@ -77,23 +77,49 @@ namespace armarx
defineOptionalProperty<float>("VOR", 0, "Vestibulo-ocular reflex weight");
defineOptionalProperty<float>("OKR", 1, "Opto-kinetic reflex weight");
defineOptionalProperty<float>("JointIK", 0, "Joint IK weight");
- defineOptionalProperty<float>("AdvancedVOR", 0, "Advanced Vestibulo-ocular reflex weight");
-
- defineOptionalProperty<std::string>("KinematicUnitName", "Armar3KinematicUnit", "Name of the KinematicUnit of the Robot");
- defineOptionalProperty<std::string>("IMUObserverName", "InertialMeasurementUnitObserver", "Name of the InertialMeasurementUnitObserver of the Robot");
- defineOptionalProperty<std::string>("RobotStateName", "RobotStateComponent", "Name of the RobotState of the Robot");
- defineOptionalProperty<std::string>("HeadIKName", "IKVirtualGazeNoEyes", "Name of the HeadIK");
- defineOptionalProperty<std::string>("RobotNodeSetName", "Robot", "Name of the RobotNodeSet");
-
- defineOptionalProperty<std::string>("ImageSourceSelectionName", "ImageSourceSelection", "Name of the ImageSourceSelection component");
- defineOptionalProperty<Ice::StringSeq>("ImageProviders", {"Armar3ImageProvider", "Armar3FovealImageProvider"}, "Name of the image provider to choose from. If image quality is high, the second provider will be used.");
-
- defineOptionalProperty<std::string>("DebugObserverName", "DebugObserver", "Name of the topic the DebugObserver listens on");
-
- defineOptionalProperty<std::string>("CalibrationFile", "ArmarXSimulation/camera_simulator_640x480.txt", "Camera calibration file, will be made available in the SLICE interface");
-
- defineOptionalProperty<bool>("NeckPerturbation", false, "Add sinusoidal motion in the neck joint to test the reflexes");
- defineOptionalProperty<bool>("reafferenceCombination", false, "Combined reflexes with IK based on the reafference principle (sensory cancelation)");
+ defineOptionalProperty<float>(
+ "AdvancedVOR", 0, "Advanced Vestibulo-ocular reflex weight");
+
+ defineOptionalProperty<std::string>("KinematicUnitName",
+ "Armar3KinematicUnit",
+ "Name of the KinematicUnit of the Robot");
+ defineOptionalProperty<std::string>(
+ "IMUObserverName",
+ "InertialMeasurementUnitObserver",
+ "Name of the InertialMeasurementUnitObserver of the Robot");
+ defineOptionalProperty<std::string>(
+ "RobotStateName", "RobotStateComponent", "Name of the RobotState of the Robot");
+ defineOptionalProperty<std::string>(
+ "HeadIKName", "IKVirtualGazeNoEyes", "Name of the HeadIK");
+ defineOptionalProperty<std::string>(
+ "RobotNodeSetName", "Robot", "Name of the RobotNodeSet");
+
+ defineOptionalProperty<std::string>("ImageSourceSelectionName",
+ "ImageSourceSelection",
+ "Name of the ImageSourceSelection component");
+ defineOptionalProperty<Ice::StringSeq>(
+ "ImageProviders",
+ {"Armar3ImageProvider", "Armar3FovealImageProvider"},
+ "Name of the image provider to choose from. If image quality is high, the second "
+ "provider will be used.");
+
+ defineOptionalProperty<std::string>("DebugObserverName",
+ "DebugObserver",
+ "Name of the topic the DebugObserver listens on");
+
+ defineOptionalProperty<std::string>(
+ "CalibrationFile",
+ "ArmarXSimulation/camera_simulator_640x480.txt",
+ "Camera calibration file, will be made available in the SLICE interface");
+
+ defineOptionalProperty<bool>(
+ "NeckPerturbation",
+ false,
+ "Add sinusoidal motion in the neck joint to test the reflexes");
+ defineOptionalProperty<bool>("reafferenceCombination",
+ false,
+ "Combined reflexes with IK based on the reafference "
+ "principle (sensory cancelation)");
}
};
@@ -123,14 +149,19 @@ namespace armarx
/**
* @see armarx::ManagedIceObject::getDefaultName()
*/
- std::string getDefaultName() const override
+ std::string
+ getDefaultName() const override
{
return "ReflexCombination";
}
- void updateWeights(float vor, float okr, float jointIK, const Ice::Current& c = Ice::emptyCurrent) override;
+ void updateWeights(float vor,
+ float okr,
+ float jointIK,
+ const Ice::Current& c = Ice::emptyCurrent) override;
- void setReafferenceMethod(bool isReafference, const Ice::Current& c = Ice::emptyCurrent) override;
+ void setReafferenceMethod(bool isReafference,
+ const Ice::Current& c = Ice::emptyCurrent) override;
protected:
@@ -161,7 +192,6 @@ namespace armarx
private:
-
void setImageQuality(float quality);
std::mutex mutex;
@@ -202,28 +232,60 @@ namespace armarx
bool newHeadTarget;
//RobotStateComponentInterfacePrx robotStateComponent;
- std::map<std::string, IceInternal::Handle<armarx::filters::DerivationFilter>> VelocityFilters;
- std::map<std::string, IceInternal::Handle<armarx::filters::MedianFilter>> IMU_GyroFilters;
- std::map<std::string, IceInternal::Handle<armarx::filters::ButterworthFilter>> PreVelocityFilters;
- std::map<std::string, IceInternal::Handle<armarx::filters::ButterworthFilter>> FlowFilters;
+ std::map<std::string, IceInternal::Handle<armarx::filters::DerivationFilter>>
+ VelocityFilters;
+ std::map<std::string, IceInternal::Handle<armarx::filters::MedianFilter>> IMU_GyroFilters;
+ std::map<std::string, IceInternal::Handle<armarx::filters::ButterworthFilter>>
+ PreVelocityFilters;
+ std::map<std::string, IceInternal::Handle<armarx::filters::ButterworthFilter>> FlowFilters;
double t_init;
// PlatformUnitListener interface
public:
- void reportPlatformVelocity(Ice::Float currentPlatformVelocityX, Ice::Float currentPlatformVelocityY, Ice::Float currentPlatformVelocityRotation, const Ice::Current& c = Ice::emptyCurrent) override;
- void reportPlatformOdometryPose(Ice::Float, Ice::Float, Ice::Float, const Ice::Current&) override;
+ void reportPlatformVelocity(Ice::Float currentPlatformVelocityX,
+ Ice::Float currentPlatformVelocityY,
+ Ice::Float currentPlatformVelocityRotation,
+ const Ice::Current& c = Ice::emptyCurrent) override;
+ void reportPlatformOdometryPose(Ice::Float,
+ Ice::Float,
+ Ice::Float,
+ const Ice::Current&) override;
// KinematicUnitListener interface
public:
- void reportControlModeChanged(const NameControlModeMap&, Ice::Long timestamp, bool, const Ice::Current& c = Ice::emptyCurrent) override;
- void reportJointAngles(const NameValueMap& values, Ice::Long timestamp, bool valueChanged, const Ice::Current& c = Ice::emptyCurrent) override;
- void reportJointVelocities(const NameValueMap& values, Ice::Long timestamp, bool valueChanged, const Ice::Current& c = Ice::emptyCurrent) override;
- void reportJointTorques(const NameValueMap&, Ice::Long timestamp, bool, const Ice::Current& c = Ice::emptyCurrent) override;
- void reportJointAccelerations(const NameValueMap&, Ice::Long timestamp, bool, const Ice::Current& c = Ice::emptyCurrent) override;
- void reportJointCurrents(const NameValueMap&, Ice::Long timestamp, bool, const Ice::Current& c = Ice::emptyCurrent) override;
- void reportJointMotorTemperatures(const NameValueMap&, Ice::Long timestamp, bool, const Ice::Current& c = Ice::emptyCurrent) override;
- void reportJointStatuses(const NameStatusMap&, Ice::Long timestamp, bool, const Ice::Current& c = Ice::emptyCurrent) override;
+ void reportControlModeChanged(const NameControlModeMap&,
+ Ice::Long timestamp,
+ bool,
+ const Ice::Current& c = Ice::emptyCurrent) override;
+ void reportJointAngles(const NameValueMap& values,
+ Ice::Long timestamp,
+ bool valueChanged,
+ const Ice::Current& c = Ice::emptyCurrent) override;
+ void reportJointVelocities(const NameValueMap& values,
+ Ice::Long timestamp,
+ bool valueChanged,
+ const Ice::Current& c = Ice::emptyCurrent) override;
+ void reportJointTorques(const NameValueMap&,
+ Ice::Long timestamp,
+ bool,
+ const Ice::Current& c = Ice::emptyCurrent) override;
+ void reportJointAccelerations(const NameValueMap&,
+ Ice::Long timestamp,
+ bool,
+ const Ice::Current& c = Ice::emptyCurrent) override;
+ void reportJointCurrents(const NameValueMap&,
+ Ice::Long timestamp,
+ bool,
+ const Ice::Current& c = Ice::emptyCurrent) override;
+ void reportJointMotorTemperatures(const NameValueMap&,
+ Ice::Long timestamp,
+ bool,
+ const Ice::Current& c = Ice::emptyCurrent) override;
+ void reportJointStatuses(const NameStatusMap&,
+ Ice::Long timestamp,
+ bool,
+ const Ice::Current& c = Ice::emptyCurrent) override;
private:
@@ -232,26 +294,39 @@ namespace armarx
// OpticalFlowListener interface
public:
- void reportNewOpticalFlow(Ice::Float, Ice::Float, Ice::Float, Ice::Long, const Ice::Current&) override;
+ void reportNewOpticalFlow(Ice::Float,
+ Ice::Float,
+ Ice::Float,
+ Ice::Long,
+ const Ice::Current&) override;
// HeadIKUnitListener interface
public:
- void reportHeadTargetChanged(const NameValueMap& targetJointAngles, const FramedPositionBasePtr& targetPosition, const Ice::Current& c = Ice::emptyCurrent) override;
+ void reportHeadTargetChanged(const NameValueMap& targetJointAngles,
+ const FramedPositionBasePtr& targetPosition,
+ const Ice::Current& c = Ice::emptyCurrent) override;
// InertialMeasurementUnitListener interface
public:
- void reportSensorValues(const std::string& device, const std::string& name, const IMUData& values, const TimestampBasePtr& timestamp, const Ice::Current& c = Ice::emptyCurrent) override;
+ void reportSensorValues(const std::string& device,
+ const std::string& name,
+ const IMUData& values,
+ const TimestampBasePtr& timestamp,
+ const Ice::Current& c = Ice::emptyCurrent) override;
// HeadIKUnitListener interface
public:
- void reportHeadTargetChanged(const Ice::Current&)
+ void
+ reportHeadTargetChanged(const Ice::Current&)
{
}
// TrackingErrorListener interface
public:
- void reportNewTrackingError(Ice::Float pixelX, Ice::Float pixelY, Ice::Float angleX, Ice::Float angleY, const Ice::Current&) override;
-
-
+ void reportNewTrackingError(Ice::Float pixelX,
+ Ice::Float pixelY,
+ Ice::Float angleX,
+ Ice::Float angleY,
+ const Ice::Current&) override;
};
-}
+} // namespace armarx
diff --git a/source/RobotComponents/components/ReflexCombination/feedforward.cpp b/source/RobotComponents/components/ReflexCombination/feedforward.cpp
index df504ecb2d130c4023fca82fe70dd731c17a8e95..c06d02392c21694eef39a64e1a31c969fd78181e 100644
--- a/source/RobotComponents/components/ReflexCombination/feedforward.cpp
+++ b/source/RobotComponents/components/ReflexCombination/feedforward.cpp
@@ -25,7 +25,8 @@
using namespace armarx;
-void FeedforwardReflex::calc()
+void
+FeedforwardReflex::calc()
{
if (!update_input_fromArmarX(stabilizer->input))
@@ -38,15 +39,17 @@ void FeedforwardReflex::calc()
stabilizer->control_loop();
- optFlow_pred = stabilizer->getOptFlowPred();
- mean_optFl_pred = stabilizer->getMeanOptFlPred();
- gyroscopeRotation_pred = stabilizer->getIMUPred();
+ optFlow_pred = stabilizer->getOptFlowPred();
+ mean_optFl_pred = stabilizer->getMeanOptFlPred();
+ gyroscopeRotation_pred = stabilizer->getIMUPred();
update_output_toArmarX(stabilizer->output);
-
}
-void FeedforwardReflex::setRobot(std::string nodeSetName, std::string headIKName, RobotStateComponentInterfacePrx robotStateComponent)
+void
+FeedforwardReflex::setRobot(std::string nodeSetName,
+ std::string headIKName,
+ RobotStateComponentInterfacePrx robotStateComponent)
{
std::scoped_lock lock(mutex);
@@ -60,7 +63,7 @@ void FeedforwardReflex::setRobot(std::string nodeSetName, std::string headIKName
{
gs_options = new GazeStabOptions(1);
}
- else // armar3
+ else // armar3
{
gs_options = new GazeStabOptions(2);
}
@@ -74,32 +77,22 @@ void FeedforwardReflex::setRobot(std::string nodeSetName, std::string headIKName
if (armar4)
{
- headJointNames =
- {
- "Neck_1_joint",
- "Neck_2_joint",
- "Neck_3_joint",
- "Head_1_joint",
- "Head_2_joint",
- "EyeR_1_joint",
- "EyeR_2_joint"
- };
+ headJointNames = {"Neck_1_joint",
+ "Neck_2_joint",
+ "Neck_3_joint",
+ "Head_1_joint",
+ "Head_2_joint",
+ "EyeR_1_joint",
+ "EyeR_2_joint"};
}
else // armar3
{
- headJointNames =
- {
- "Neck_1_Pitch",
- "Neck_2_Roll",
- "Neck_3_Yaw",
- "Cameras",
- "Eye_Right"
- };
+ headJointNames = {"Neck_1_Pitch", "Neck_2_Roll", "Neck_3_Yaw", "Cameras", "Eye_Right"};
}
-
}
-bool FeedforwardReflex::update_input_fromArmarX(GazeStabInput* gs_input)
+bool
+FeedforwardReflex::update_input_fromArmarX(GazeStabInput* gs_input)
{
std::scoped_lock lock(dataMutex);
@@ -107,7 +100,7 @@ bool FeedforwardReflex::update_input_fromArmarX(GazeStabInput* gs_input)
//Check if all sensor values have at least been reported once to avoid segfaults
if (!(reportedJointAnglesBool && reportedJointVelocitiesBool)) // TODO:: && globalPos))
{
- ARMARX_WARNING_S << "Try to access gaze stab input that haven't been reported";
+ ARMARX_WARNING_S << "Try to access gaze stab input that haven't been reported";
return false;
}
@@ -115,13 +108,17 @@ bool FeedforwardReflex::update_input_fromArmarX(GazeStabInput* gs_input)
if (armar4)
{
- PoseBasePtr globalPose = robotStateComponent->getSynchronizedRobot()->getRobotNode("Torso_Yaw_joint")->getGlobalPose();
+ PoseBasePtr globalPose = robotStateComponent->getSynchronizedRobot()
+ ->getRobotNode("Torso_Yaw_joint")
+ ->getGlobalPose();
- Eigen::Quaternionf quaternion = QuaternionPtr::dynamicCast(globalPose->orientation)->toEigenQuaternion();
+ Eigen::Quaternionf quaternion =
+ QuaternionPtr::dynamicCast(globalPose->orientation)->toEigenQuaternion();
Eigen::Vector3f rpy = quaternionToRPY(quaternion);
- gs_input->q_LB[0] = globalPose->position->x; // should be probably converted from mm to m (TBC)
+ gs_input->q_LB[0] =
+ globalPose->position->x; // should be probably converted from mm to m (TBC)
gs_input->q_LB[1] = globalPose->position->y;
gs_input->q_LB[2] = globalPose->position->z;
@@ -148,7 +145,8 @@ bool FeedforwardReflex::update_input_fromArmarX(GazeStabInput* gs_input)
{
PoseBasePtr globalPose = robotStateComponent->getSynchronizedRobot()->getGlobalPose();
- Eigen::Quaternionf quaternion = QuaternionPtr::dynamicCast(globalPose->orientation)->toEigenQuaternion();
+ Eigen::Quaternionf quaternion =
+ QuaternionPtr::dynamicCast(globalPose->orientation)->toEigenQuaternion();
Eigen::Vector3f rpy = quaternionToRPY(quaternion);
gs_input->q_LB[0] = globalPose->position->x / 1000.;
@@ -172,7 +170,7 @@ bool FeedforwardReflex::update_input_fromArmarX(GazeStabInput* gs_input)
gs_input->qd_LB_des[4] = 0.;
//gs_input->qd_LB_des[5] = 0.; // theta
- gs_input->qd_LB_des[6] = 0.; // yaw platform (unused here...)
+ gs_input->qd_LB_des[6] = 0.; // yaw platform (unused here...)
gs_input->qd_LB_des[7] = -1 * this->reportedJointVelocities["Hip Pitch"];
gs_input->qd_LB_des[8] = -1 * this->reportedJointVelocities["Hip Roll"];
gs_input->qd_LB_des[9] = this->reportedJointVelocities["Hip Yaw"];
@@ -182,7 +180,6 @@ bool FeedforwardReflex::update_input_fromArmarX(GazeStabInput* gs_input)
{
gs_input->qd_LB[i] = gs_input->qd_LB_des[i];
}
-
}
for (size_t i = 0; i < headJointNames.size(); i++)
@@ -193,8 +190,8 @@ bool FeedforwardReflex::update_input_fromArmarX(GazeStabInput* gs_input)
if (armar4)
{
- gs_input->q_UB[2] += M_PI; // shift between models
- gs_input->q_UB[0] *= -1; // sense of rotation between models
+ gs_input->q_UB[2] += M_PI; // shift between models
+ gs_input->q_UB[0] *= -1; // sense of rotation between models
gs_input->q_UB[1] *= -1;
gs_input->q_UB[6] *= -1;
}
@@ -206,10 +203,10 @@ bool FeedforwardReflex::update_input_fromArmarX(GazeStabInput* gs_input)
gs_input->tsim = (armarx::TimeUtil::GetTime() - startTime).toSecondsDouble();
return true;
-
}
-void FeedforwardReflex::update_output_toArmarX(GazeStabOutput* gs_output)
+void
+FeedforwardReflex::update_output_toArmarX(GazeStabOutput* gs_output)
{
std::map<std::string, float> targetJointAngles;
@@ -220,23 +217,22 @@ void FeedforwardReflex::update_output_toArmarX(GazeStabOutput* gs_output)
}
if (armar4)
{
- targetJointAngles[headJointNames[0]] *= -1; // sense of rotation between models
+ targetJointAngles[headJointNames[0]] *= -1; // sense of rotation between models
targetJointAngles[headJointNames[1]] *= -1;
targetJointAngles[headJointNames[6]] *= -1;
}
else // armar 3
{
targetJointAngles["Eye_Left"] = targetJointAngles["Eye_Right"];
-
}
std::scoped_lock lock(mutex);
jointAngles.swap(targetJointAngles);
-
}
-void FeedforwardReflex::setBools(bool armar4, bool velocityBased)
+void
+FeedforwardReflex::setBools(bool armar4, bool velocityBased)
{
std::scoped_lock lock(mutex);
@@ -244,16 +240,21 @@ void FeedforwardReflex::setBools(bool armar4, bool velocityBased)
this->velocityBased = velocityBased;
}
-void FeedforwardReflex::reportJointAngles(const NameValueMap& values, bool valueChanged, const Ice::Current& c)
+void
+FeedforwardReflex::reportJointAngles(const NameValueMap& values,
+ bool valueChanged,
+ const Ice::Current& c)
{
std::scoped_lock lock(dataMutex);
reportedJointAnglesBool = true;
this->reportedJointAngles = values;
-
}
-void FeedforwardReflex::reportJointVelocities(const NameValueMap& values, bool valueChanged, const Ice::Current& c)
+void
+FeedforwardReflex::reportJointVelocities(const NameValueMap& values,
+ bool valueChanged,
+ const Ice::Current& c)
{
std::scoped_lock lock(dataMutex);
@@ -261,16 +262,17 @@ void FeedforwardReflex::reportJointVelocities(const NameValueMap& values, bool v
this->reportedJointVelocities = values;
}
-void FeedforwardReflex::reportPlatformVelocity(float x, float y, float a)
+void
+FeedforwardReflex::reportPlatformVelocity(float x, float y, float a)
{
stabilizer->input->qd_LB_des[0] = x / 1000.;
stabilizer->input->qd_LB_des[1] = y / 1000.;
stabilizer->input->qd_LB_des[5] = a;
}
-
-
-void FeedforwardReflex::reportHeadTargetChanged(const NameValueMap& targetJointAngles, const FramedPositionBasePtr& targetPosition)
+void
+FeedforwardReflex::reportHeadTargetChanged(const NameValueMap& targetJointAngles,
+ const FramedPositionBasePtr& targetPosition)
{
std::scoped_lock lock(dataMutex);
@@ -279,16 +281,14 @@ void FeedforwardReflex::reportHeadTargetChanged(const NameValueMap& targetJointA
Eigen::Vector3f x = globalPos->toEigen() / 1000.0f;
stabilizer->input->pos_target = x.cast<double>();
-
}
-void FeedforwardReflex::onStop()
+void
+FeedforwardReflex::onStop()
{
std::scoped_lock lock(dataMutex);
// globalPos = {};
reportedJointVelocitiesBool = false;
reportedJointAnglesBool = false;
-
-
}
diff --git a/source/RobotComponents/components/ReflexCombination/feedforward.h b/source/RobotComponents/components/ReflexCombination/feedforward.h
index de9544f5de0f52c9837116d85aad503661fcd501..994cbeebd791276f22ec5d7612391f468f485009 100644
--- a/source/RobotComponents/components/ReflexCombination/feedforward.h
+++ b/source/RobotComponents/components/ReflexCombination/feedforward.h
@@ -23,16 +23,14 @@
*/
#pragma once
-#include "reflex.h"
-#include "ReflexCombination.h"
#include <Eigen/Core>
#include <Eigen/Geometry>
-#include <VirtualRobot/VirtualRobot.h>
-#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/IK/GazeIK.h>
#include <VirtualRobot/Nodes/RobotNode.h>
+#include <VirtualRobot/Robot.h>
#include <VirtualRobot/RobotNodeSet.h>
-#include <VirtualRobot/IK/GazeIK.h>
+#include <VirtualRobot/VirtualRobot.h>
#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
@@ -40,9 +38,13 @@
#include <RobotComponents/components/Feedforward/GazeStabilization.hh>
#include <RobotComponents/components/Feedforward/forwardPredictor.h>
+#include "ReflexCombination.h"
+#include "reflex.h"
+
namespace armarx
{
class ReflexCombination;
+
class FeedforwardReflex : virtual public Reflex
{
public:
@@ -56,6 +58,7 @@ namespace armarx
gyroscopeRotation_pred.resize(3);
onStop();
}
+
~FeedforwardReflex() override
{
if (stabilizer)
@@ -69,19 +72,24 @@ namespace armarx
}
}
- void setRobot(std::string nodeSetName, std::string headIKName, RobotStateComponentInterfacePrx robotStateComponent);
+ void setRobot(std::string nodeSetName,
+ std::string headIKName,
+ RobotStateComponentInterfacePrx robotStateComponent);
void setBools(bool armar4, bool velocityBased);
- bool update_input_fromArmarX(GazeStabInput* gs_input);
- void update_output_toArmarX(GazeStabOutput* gs_output);
+ bool update_input_fromArmarX(GazeStabInput* gs_input);
+ void update_output_toArmarX(GazeStabOutput* gs_output);
- void reportJointAngles(const NameValueMap& values, bool valueChanged, const Ice::Current& c);
- void reportJointVelocities(const NameValueMap& values, bool valueChanged, const Ice::Current& c);
+ void
+ reportJointAngles(const NameValueMap& values, bool valueChanged, const Ice::Current& c);
+ void
+ reportJointVelocities(const NameValueMap& values, bool valueChanged, const Ice::Current& c);
void reportPlatformVelocity(float x, float y, float a);
- void reportHeadTargetChanged(const NameValueMap& targetJointAngles, const FramedPositionBasePtr& targetPosition);
-
+ void reportHeadTargetChanged(const NameValueMap& targetJointAngles,
+ const FramedPositionBasePtr& targetPosition);
- std::string getName() const override
+ std::string
+ getName() const override
{
return "FeedforwardReflex";
}
@@ -89,17 +97,17 @@ namespace armarx
// optical flow prediction
std::vector<float> optFlow_pred; // [x, y] in [deg/s]
double mean_optFl_pred; // mean distance of the optical flow [deg/s]
- std::vector<float> gyroscopeRotation_pred; // head IMU velocity in head attached frame (x,y,z) [rad/s]
+ std::vector<float>
+ gyroscopeRotation_pred; // head IMU velocity in head attached frame (x,y,z) [rad/s]
protected:
-
void onStop() override;
void calc() override;
private:
VirtualRobot::RobotPtr robot;
- std::vector< VirtualRobot::RobotNodePtr > allRobotNodeSet;
+ std::vector<VirtualRobot::RobotNodePtr> allRobotNodeSet;
RobotStateComponentInterfacePrx robotStateComponent;
std::string headIKName;
@@ -116,6 +124,5 @@ namespace armarx
bool reportedJointAnglesBool, reportedJointVelocitiesBool;
bool armar4, velocityBased;
NameValueMap reportedJointAngles, reportedJointVelocities;
-
};
-}
+} // namespace armarx
diff --git a/source/RobotComponents/components/ReflexCombination/jointik.cpp b/source/RobotComponents/components/ReflexCombination/jointik.cpp
index a98310d9fa0d140fd8b3a21899f62f50c18301f1..ef2ebfa820b088c48b34a0940db94140df72670d 100644
--- a/source/RobotComponents/components/ReflexCombination/jointik.cpp
+++ b/source/RobotComponents/components/ReflexCombination/jointik.cpp
@@ -27,7 +27,8 @@
using namespace armarx;
-void JointIK::calc()
+void
+JointIK::calc()
{
std::scoped_lock lock(dataMutex);
@@ -50,7 +51,8 @@ void JointIK::calc()
{
continue;
}
- newValues[name] = reportedJointAngles[name] + (((float) interval) / 1000.0) * reportedJointVelocities[name];
+ newValues[name] = reportedJointAngles[name] +
+ (((float)interval) / 1000.0) * reportedJointVelocities[name];
}
robot->setJointValues(newValues);
@@ -66,7 +68,8 @@ void JointIK::calc()
//TODO VirtualCentralGaze in Armar4 nicht vorhanden
if (kinematicChain->getNode(i)->getName().compare("VirtualCentralGaze") == 0)
{
- virtualJoint = std::dynamic_pointer_cast<VirtualRobot::RobotNodePrismatic>(kinematicChain->getNode(i));
+ virtualJoint = std::dynamic_pointer_cast<VirtualRobot::RobotNodePrismatic>(
+ kinematicChain->getNode(i));
}
}
@@ -86,7 +89,6 @@ void JointIK::calc()
if (jointName.compare("VirtualCentralGaze") != 0)
{
targetJointAngles[jointName] = kinematicChain->getNode(i)->getJointValue();
-
}
}
@@ -96,17 +98,19 @@ void JointIK::calc()
}
else
{
- ARMARX_WARNING_S << "HeadIK no solution found";
+ ARMARX_WARNING_S << "HeadIK no solution found";
}
-
-
}
-void JointIK::setRobot(std::string nodeSetName, std::string headIKName, RobotStateComponentInterfacePrx robotStateComponent)
+void
+JointIK::setRobot(std::string nodeSetName,
+ std::string headIKName,
+ RobotStateComponentInterfacePrx robotStateComponent)
{
std::scoped_lock lock(mutex);
- this->robot = RemoteRobot::createLocalCloneFromFile(robotStateComponent, VirtualRobot::RobotIO::RobotDescription::eFull);
+ this->robot = RemoteRobot::createLocalCloneFromFile(
+ robotStateComponent, VirtualRobot::RobotIO::RobotDescription::eFull);
VirtualRobot::RobotNodeSetPtr nodeSetPtr = this->robot->getRobotNodeSet(nodeSetName);
this->allRobotNodeSet = nodeSetPtr->getAllRobotNodes();
@@ -114,7 +118,8 @@ void JointIK::setRobot(std::string nodeSetName, std::string headIKName, RobotSta
this->robotStateComponent = robotStateComponent;
}
-void JointIK::setBools(bool armar4, bool velocityBased)
+void
+JointIK::setBools(bool armar4, bool velocityBased)
{
std::scoped_lock lock(mutex);
@@ -122,16 +127,17 @@ void JointIK::setBools(bool armar4, bool velocityBased)
this->velocityBased = velocityBased;
}
-void JointIK::reportJointAngles(const NameValueMap& values, bool valueChanged, const Ice::Current& c)
+void
+JointIK::reportJointAngles(const NameValueMap& values, bool valueChanged, const Ice::Current& c)
{
std::scoped_lock lock(dataMutex);
reportedJointAnglesBool = true;
this->reportedJointAngles = values;
-
}
-void JointIK::reportJointVelocities(const NameValueMap& values, bool valueChanged, const Ice::Current& c)
+void
+JointIK::reportJointVelocities(const NameValueMap& values, bool valueChanged, const Ice::Current& c)
{
std::scoped_lock lock(dataMutex);
@@ -139,22 +145,21 @@ void JointIK::reportJointVelocities(const NameValueMap& values, bool valueChange
this->reportedJointVelocities = values;
}
-
-
-void JointIK::reportHeadTargetChanged(const NameValueMap& targetJointAngles, const FramedPositionBasePtr& targetPosition)
+void
+JointIK::reportHeadTargetChanged(const NameValueMap& targetJointAngles,
+ const FramedPositionBasePtr& targetPosition)
{
std::scoped_lock lock(dataMutex);
globalPos = FramedPositionPtr::dynamicCast(targetPosition);
}
-void JointIK::onStop()
+void
+JointIK::onStop()
{
std::scoped_lock lock(dataMutex);
// globalPos = {};
reportedJointVelocitiesBool = false;
reportedJointAnglesBool = false;
-
-
}
diff --git a/source/RobotComponents/components/ReflexCombination/jointik.h b/source/RobotComponents/components/ReflexCombination/jointik.h
index 9d5a9b82d929d634ff27863946c229eed4db25a5..c0e8e8fa305de6a1ef208c6ff9770fd93a757f5e 100644
--- a/source/RobotComponents/components/ReflexCombination/jointik.h
+++ b/source/RobotComponents/components/ReflexCombination/jointik.h
@@ -23,23 +23,25 @@
*/
#pragma once
-#include "reflex.h"
-#include "ReflexCombination.h"
#include <Eigen/Core>
#include <Eigen/Geometry>
-#include <VirtualRobot/VirtualRobot.h>
-#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/IK/GazeIK.h>
#include <VirtualRobot/Nodes/RobotNode.h>
+#include <VirtualRobot/Robot.h>
#include <VirtualRobot/RobotNodeSet.h>
-#include <VirtualRobot/IK/GazeIK.h>
+#include <VirtualRobot/VirtualRobot.h>
#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
+#include "ReflexCombination.h"
+#include "reflex.h"
+
namespace armarx
{
class ReflexCombination;
+
class JointIK : virtual public Reflex
{
public:
@@ -47,31 +49,38 @@ namespace armarx
{
onStop();
}
- ~JointIK() override {}
- void setRobot(std::string nodeSetName, std::string headIKName, RobotStateComponentInterfacePrx robotStateComponent);
- void setBools(bool armar4, bool velocityBased);
+ ~JointIK() override
+ {
+ }
- void reportJointAngles(const NameValueMap& values, bool valueChanged, const Ice::Current& c);
- void reportJointVelocities(const NameValueMap& values, bool valueChanged, const Ice::Current& c);
+ void setRobot(std::string nodeSetName,
+ std::string headIKName,
+ RobotStateComponentInterfacePrx robotStateComponent);
+ void setBools(bool armar4, bool velocityBased);
- void reportHeadTargetChanged(const NameValueMap& targetJointAngles, const FramedPositionBasePtr& targetPosition);
+ void
+ reportJointAngles(const NameValueMap& values, bool valueChanged, const Ice::Current& c);
+ void
+ reportJointVelocities(const NameValueMap& values, bool valueChanged, const Ice::Current& c);
+ void reportHeadTargetChanged(const NameValueMap& targetJointAngles,
+ const FramedPositionBasePtr& targetPosition);
- std::string getName() const override
+ std::string
+ getName() const override
{
return "JointIK";
}
protected:
-
void onStop() override;
void calc() override;
private:
VirtualRobot::RobotPtr robot;
- std::vector< VirtualRobot::RobotNodePtr > allRobotNodeSet;
+ std::vector<VirtualRobot::RobotNodePtr> allRobotNodeSet;
RobotStateComponentInterfacePrx robotStateComponent;
std::string headIKName;
@@ -82,6 +91,5 @@ namespace armarx
bool reportedJointAnglesBool, reportedJointVelocitiesBool;
bool armar4, velocityBased;
NameValueMap reportedJointAngles, reportedJointVelocities;
-
};
-}
+} // namespace armarx
diff --git a/source/RobotComponents/components/ReflexCombination/okr.cpp b/source/RobotComponents/components/ReflexCombination/okr.cpp
index 42cc59d088bfec7260c9b4b5f9f0803f266740af..3c030d152135951f5be2f31a10de2e8704f522bc 100644
--- a/source/RobotComponents/components/ReflexCombination/okr.cpp
+++ b/source/RobotComponents/components/ReflexCombination/okr.cpp
@@ -25,7 +25,8 @@
using namespace armarx;
-void OKR::calc()
+void
+OKR::calc()
{
std::scoped_lock lock(dataMutex);
@@ -78,13 +79,12 @@ void OKR::calc()
// gain < 1 is to avoid unstability with the head
headJoints[eye_pitch_left] = 0.0; //0.8 * (flowY) - kp * errorY;
- headJoints[eye_yaw_right] = -0.8 * (flowX);// + kp * errorX;
+ headJoints[eye_yaw_right] = -0.8 * (flowX); // + kp * errorX;
headJoints[eye_yaw_left] = headJoints[eye_yaw_right];
// uncomment for baseline
//headJoints[eye_pitch_left] = headJoints[eye_yaw_left] = headJoints[eye_yaw_right] = 0.;
-
}
else
{
@@ -104,7 +104,6 @@ void OKR::calc()
headJoints[eye_yaw_right] = (reportedJointAngles[eye_yaw_left] - flowX);
jointModes[eye_yaw_right] = ePositionControl;
-
}
// mutexNodeSet.unlock();
@@ -114,7 +113,8 @@ void OKR::calc()
jointAngles.swap(headJoints);
}
-void OKR::removeSelfInducedOpticalFlow()
+void
+OKR::removeSelfInducedOpticalFlow()
{
NameValueMap velocities;
@@ -133,7 +133,8 @@ void OKR::removeSelfInducedOpticalFlow()
if (timestamp <= flow_time + flowT)
{
velocities = kv.second;
- jointVelocities.erase(kv.first); // erase the values belonging to the previous optical flow
+ jointVelocities.erase(
+ kv.first); // erase the values belonging to the previous optical flow
}
else
{
@@ -145,15 +146,19 @@ void OKR::removeSelfInducedOpticalFlow()
if (velocities.find(eye_yaw_right) == velocities.end()) // no velocity found
{
velocities = jointVelocities.rbegin()->second;
- ARMARX_WARNING << deactivateSpam(1) << "No self-induced velocity found";
+ ARMARX_WARNING << deactivateSpam(1) << "No self-induced velocity found";
}
- flowY = flowY + velocities[eye_pitch_left] ;
- flowX = flowX - velocities[eye_yaw_right] ;
-
+ flowY = flowY + velocities[eye_pitch_left];
+ flowX = flowX - velocities[eye_yaw_right];
}
-void OKR::setJointNames(std::string eye_pitch_left, std::string eye_pitch_right, std::string eye_yaw_left, std::string eye_yaw_right, std::string neck_roll)
+void
+OKR::setJointNames(std::string eye_pitch_left,
+ std::string eye_pitch_right,
+ std::string eye_yaw_left,
+ std::string eye_yaw_right,
+ std::string neck_roll)
{
std::scoped_lock lock(mutex);
this->eye_pitch_left = eye_pitch_left;
@@ -163,7 +168,8 @@ void OKR::setJointNames(std::string eye_pitch_left, std::string eye_pitch_right,
this->neck_roll = neck_roll;
}
-void OKR::setPIDValues(float kp, float ki, float kd)
+void
+OKR::setPIDValues(float kp, float ki, float kd)
{
std::scoped_lock lock(mutex);
@@ -172,7 +178,8 @@ void OKR::setPIDValues(float kp, float ki, float kd)
this->kd = kd;
}
-void OKR::setBools(bool armar4, bool velocityBased)
+void
+OKR::setBools(bool armar4, bool velocityBased)
{
std::scoped_lock lock(mutex);
@@ -180,7 +187,11 @@ void OKR::setBools(bool armar4, bool velocityBased)
this->velocityBased = velocityBased;
}
-std::vector<float> OKR::pid(std::vector<float> error, std::vector<float> kp, std::vector<float> ki, std::vector<float> kd)
+std::vector<float>
+OKR::pid(std::vector<float> error,
+ std::vector<float> kp,
+ std::vector<float> ki,
+ std::vector<float> kd)
{
if (error.size() != kp.size() || kp.size() != ki.size() || ki.size() != kd.size())
{
@@ -202,16 +213,20 @@ std::vector<float> OKR::pid(std::vector<float> error, std::vector<float> kp, std
return y;
}
-void OKR::reportJointAngles(const NameValueMap& values, bool valueChanged, const Ice::Current& c)
+void
+OKR::reportJointAngles(const NameValueMap& values, bool valueChanged, const Ice::Current& c)
{
std::scoped_lock lock(dataMutex);
reportedJointAnglesBool = true;
this->reportedJointAngles = values;
-
}
-void OKR::reportJointVelocities(const NameValueMap& values, bool valueChanged, long timestamp, const Ice::Current& c)
+void
+OKR::reportJointVelocities(const NameValueMap& values,
+ bool valueChanged,
+ long timestamp,
+ const Ice::Current& c)
{
std::scoped_lock lock(dataMutex);
@@ -220,11 +235,10 @@ void OKR::reportJointVelocities(const NameValueMap& values, bool valueChanged, l
NameValueMap test(values);
jointVelocities[timestamp] = test;
-
-
}
-void OKR::reportNewOpticalFlow(float x, float y, float deltaT, long timestamp)
+void
+OKR::reportNewOpticalFlow(float x, float y, float deltaT, long timestamp)
{
std::scoped_lock lock(dataMutex);
@@ -236,7 +250,11 @@ void OKR::reportNewOpticalFlow(float x, float y, float deltaT, long timestamp)
flow_time = timestamp;
}
-void OKR::reportNewTrackingError(Ice::Float pixelX, Ice::Float pixelY, Ice::Float angleX, Ice::Float angleY)
+void
+OKR::reportNewTrackingError(Ice::Float pixelX,
+ Ice::Float pixelY,
+ Ice::Float angleX,
+ Ice::Float angleY)
{
std::scoped_lock lock(dataMutex);
@@ -244,8 +262,8 @@ void OKR::reportNewTrackingError(Ice::Float pixelX, Ice::Float pixelY, Ice::Floa
this->errorY = angleY;
}
-
-void OKR::onStop()
+void
+OKR::onStop()
{
std::scoped_lock lock(dataMutex);
diff --git a/source/RobotComponents/components/ReflexCombination/okr.h b/source/RobotComponents/components/ReflexCombination/okr.h
index 054de79b2643e59c8e5761281c0918b71d3e17e0..1452c21b6bbc311426346fb67eadba2e6986c796 100644
--- a/source/RobotComponents/components/ReflexCombination/okr.h
+++ b/source/RobotComponents/components/ReflexCombination/okr.h
@@ -23,13 +23,13 @@
*/
#pragma once
-#include "reflex.h"
#include "ReflexCombination.h"
-
+#include "reflex.h"
namespace armarx
{
class ReflexCombination;
+
class OKR : virtual public Reflex
{
public:
@@ -37,9 +37,13 @@ namespace armarx
{
flow_time = 0.;
}
- ~OKR() override {}
- std::string getName() const override
+ ~OKR() override
+ {
+ }
+
+ std::string
+ getName() const override
{
return "OKR";
}
@@ -49,7 +53,10 @@ namespace armarx
void calc() override;
void onStop() override;
- std::vector<float> pid(std::vector<float> error, std::vector<float> kp, std::vector<float> ki, std::vector<float> kd);
+ std::vector<float> pid(std::vector<float> error,
+ std::vector<float> kp,
+ std::vector<float> ki,
+ std::vector<float> kd);
void removeSelfInducedOpticalFlow();
private:
@@ -69,14 +76,24 @@ namespace armarx
std::map<long, NameValueMap> jointVelocities;
public:
- void reportJointAngles(const NameValueMap& values, bool valueChanged, const Ice::Current& c);
- void reportJointVelocities(const NameValueMap& values, bool valueChanged, long timestamp, const Ice::Current& c);
+ void
+ reportJointAngles(const NameValueMap& values, bool valueChanged, const Ice::Current& c);
+ void reportJointVelocities(const NameValueMap& values,
+ bool valueChanged,
+ long timestamp,
+ const Ice::Current& c);
- void setJointNames(std::string eye_pitch_left, std::string eye_pitch_right, std::string eye_yaw_left, std::string eye_yaw_right, std::string neck_roll);
+ void setJointNames(std::string eye_pitch_left,
+ std::string eye_pitch_right,
+ std::string eye_yaw_left,
+ std::string eye_yaw_right,
+ std::string neck_roll);
void setPIDValues(float kp, float ki, float kd);
void setBools(bool armar4, bool velocityBased);
void reportNewOpticalFlow(float x, float y, float deltaT, long timestamp);
- void reportNewTrackingError(Ice::Float pixelX, Ice::Float pixelY, Ice::Float angleX, Ice::Float angleY);
+ void reportNewTrackingError(Ice::Float pixelX,
+ Ice::Float pixelY,
+ Ice::Float angleX,
+ Ice::Float angleY);
};
-}
-
+} // namespace armarx
diff --git a/source/RobotComponents/components/ReflexCombination/reflex.h b/source/RobotComponents/components/ReflexCombination/reflex.h
index 23232bdd2a2b660e0de72d9a6244139241f8ab7b..32ca7376199b1dfc2b25d7b2d8633688bfbfbd7d 100644
--- a/source/RobotComponents/components/ReflexCombination/reflex.h
+++ b/source/RobotComponents/components/ReflexCombination/reflex.h
@@ -23,16 +23,17 @@
*/
#pragma once
-#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
+#include <mutex>
#include <Eigen/Core>
#include <Eigen/Geometry>
-#include <mutex>
+#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
namespace armarx
{
class ReflexCombination;
+
class Reflex
{
public:
@@ -40,7 +41,8 @@ namespace armarx
{
this->interval = interval;
- task = new PeriodicTask<Reflex>(this, &Reflex::calc, interval, false, "HeadStabilizationTask");
+ task = new PeriodicTask<Reflex>(
+ this, &Reflex::calc, interval, false, "HeadStabilizationTask");
task->setDelayWarningTolerance(5);
}
@@ -49,8 +51,8 @@ namespace armarx
stop();
}
-
- void start()
+ void
+ start()
{
if (!task->isRunning())
{
@@ -58,7 +60,8 @@ namespace armarx
}
}
- void stop()
+ void
+ stop()
{
if (task->isRunning())
{
@@ -70,7 +73,8 @@ namespace armarx
jointAngles.clear();
}
- void setEnabled(bool enabled)
+ void
+ setEnabled(bool enabled)
{
if (enabled)
{
@@ -82,7 +86,8 @@ namespace armarx
}
}
- std::map<std::string, float> getJoints()
+ std::map<std::string, float>
+ getJoints()
{
std::scoped_lock lock(mutex);
@@ -96,13 +101,14 @@ namespace armarx
return result;
}
-
- float getWeight() const
+ float
+ getWeight() const
{
return weight;
}
- void setWeight(float weight)
+ void
+ setWeight(float weight)
{
this->weight = weight;
}
@@ -111,14 +117,16 @@ namespace armarx
protected:
-
- Eigen::Vector3f quaternionToRPY(Eigen::Quaternionf q)
+ Eigen::Vector3f
+ quaternionToRPY(Eigen::Quaternionf q)
{
Eigen::Vector3f euler;
- euler(0) = -std::atan2(2.0 * (q.w() * q.x() - q.y() * q.z()), 1.0 - 2.0 * (q.x() * q.x() + q.y() * q.y())); //roll
- euler(1) = std::asin(2.0 * (q.w() * q.y() - q.z() * q.x())); //pitch
- euler(2) = std::atan2(2.0 * (q.w() * q.z() + q.x() * q.y()), 1.0 - 2.0 * (q.y() * q.y() + q.z() * q.z())); //yaw
+ euler(0) = -std::atan2(2.0 * (q.w() * q.x() - q.y() * q.z()),
+ 1.0 - 2.0 * (q.x() * q.x() + q.y() * q.y())); //roll
+ euler(1) = std::asin(2.0 * (q.w() * q.y() - q.z() * q.x())); //pitch
+ euler(2) = std::atan2(2.0 * (q.w() * q.z() + q.x() * q.y()),
+ 1.0 - 2.0 * (q.y() * q.y() + q.z() * q.z())); //yaw
return euler;
}
@@ -129,7 +137,7 @@ namespace armarx
std::mutex mutex;
- std::map<std::string, float> jointAngles;
+ std::map<std::string, float> jointAngles;
int interval;
@@ -141,13 +149,10 @@ namespace armarx
private:
-
PeriodicTask<Reflex>::pointer_type task;
float weight;
-
};
-
using ReflexPtr = std::shared_ptr<Reflex>;
-}
+} // namespace armarx
diff --git a/source/RobotComponents/components/ReflexCombination/test/ReflexCombinationTest.cpp b/source/RobotComponents/components/ReflexCombination/test/ReflexCombinationTest.cpp
index 60bbb8ea585737df28665486d1631eb67e202813..a501c8c0dfda0ad7a0b012839a8b7a2a37ec257f 100644
--- a/source/RobotComponents/components/ReflexCombination/test/ReflexCombinationTest.cpp
+++ b/source/RobotComponents/components/ReflexCombination/test/ReflexCombinationTest.cpp
@@ -24,11 +24,11 @@
#define ARMARX_BOOST_TEST
+#include <iostream>
+
#include <RobotComponents/Test.h>
#include <RobotComponents/components/ReflexCombination/ReflexCombination.h>
-#include <iostream>
-
BOOST_AUTO_TEST_CASE(testExample)
{
armarx::ReflexCombination instance;
diff --git a/source/RobotComponents/components/ReflexCombination/vor.cpp b/source/RobotComponents/components/ReflexCombination/vor.cpp
index 367630ca63e2562c64cdbe1fe24cacbc1423f0a0..ebf9576eaddc4ef5bc806f5940ca14d349b8fa51 100644
--- a/source/RobotComponents/components/ReflexCombination/vor.cpp
+++ b/source/RobotComponents/components/ReflexCombination/vor.cpp
@@ -25,7 +25,8 @@
using namespace armarx;
-void VOR::calc()
+void
+VOR::calc()
{
std::scoped_lock lock(dataMutex);
@@ -92,13 +93,15 @@ void VOR::calc()
// headJoints[eye_yaw_left] = (reportedJointVelocities[eye_yaw_left] + y[2]);
// headJoints[eye_yaw_right] = (reportedJointVelocities[eye_yaw_left] + y[2]);
- headJoints[eye_pitch_left] = -1 * rotationVelocity[1] + kp * (rpy[1] - reportedJointAngles[eye_pitch_left]) ;
+ headJoints[eye_pitch_left] =
+ -1 * rotationVelocity[1] + kp * (rpy[1] - reportedJointAngles[eye_pitch_left]);
headJoints[eye_pitch_left] = 0.;
- headJoints[eye_yaw_left] = rotationVelocity[2] + kp * (rpy[2] - reportedJointAngles[eye_yaw_left]);
- headJoints[eye_yaw_right] = rotationVelocity[2] + kp * (rpy[2] - reportedJointAngles[eye_yaw_right]);
+ headJoints[eye_yaw_left] =
+ rotationVelocity[2] + kp * (rpy[2] - reportedJointAngles[eye_yaw_left]);
+ headJoints[eye_yaw_right] =
+ rotationVelocity[2] + kp * (rpy[2] - reportedJointAngles[eye_yaw_right]);
//headJoints[neck_roll] = (reportedJointVelocities[neck_roll] + y[0]);
-
}
else
{
@@ -110,7 +113,7 @@ void VOR::calc()
rpy[0] = -rpy[0];
}
headJoints[eye_yaw_left] = 1 * rpy[2];
- headJoints[eye_yaw_right] = 1 * rpy[2];
+ headJoints[eye_yaw_right] = 1 * rpy[2];
//headJoints[neck_roll] = 1 * (rpy[0] + reportedJointAngles[neck_roll]);
}
@@ -119,7 +122,8 @@ void VOR::calc()
jointAngles.swap(headJoints);
}
-void VOR::reportSensorValues(const IMUData& values)
+void
+VOR::reportSensorValues(const IMUData& values)
{
std::scoped_lock lock(dataMutex);
@@ -139,10 +143,14 @@ void VOR::reportSensorValues(const IMUData& values)
initialQuaternion = Eigen::Quaternionf(q[0], q[1], q[2], q[3]);
}
-
}
-void VOR::setJointNames(std::string eye_pitch_left, std::string eye_pitch_right, std::string eye_yaw_left, std::string eye_yaw_right, std::string neck_roll)
+void
+VOR::setJointNames(std::string eye_pitch_left,
+ std::string eye_pitch_right,
+ std::string eye_yaw_left,
+ std::string eye_yaw_right,
+ std::string neck_roll)
{
std::scoped_lock lock(mutex);
@@ -153,7 +161,8 @@ void VOR::setJointNames(std::string eye_pitch_left, std::string eye_pitch_right,
this->neck_roll = neck_roll;
}
-void VOR::setPIDValues(float kp, float ki, float kd)
+void
+VOR::setPIDValues(float kp, float ki, float kd)
{
std::scoped_lock lock(mutex);
@@ -162,7 +171,8 @@ void VOR::setPIDValues(float kp, float ki, float kd)
this->kd = kd;
}
-void VOR::setBools(bool armar4, bool velocityBased)
+void
+VOR::setBools(bool armar4, bool velocityBased)
{
std::scoped_lock lock(mutex);
@@ -170,9 +180,11 @@ void VOR::setBools(bool armar4, bool velocityBased)
this->velocityBased = velocityBased;
}
-
-
-std::vector<float> VOR::pid(std::vector<float> error, std::vector<float> kp, std::vector<float> ki, std::vector<float> kd)
+std::vector<float>
+VOR::pid(std::vector<float> error,
+ std::vector<float> kp,
+ std::vector<float> ki,
+ std::vector<float> kd)
{
if (error.size() != kp.size() || kp.size() != ki.size() || ki.size() != kd.size())
{
@@ -195,8 +207,8 @@ std::vector<float> VOR::pid(std::vector<float> error, std::vector<float> kp, std
return y;
}
-
-void armarx::VOR::onStop()
+void
+armarx::VOR::onStop()
{
std::scoped_lock lock(dataMutex);
@@ -213,22 +225,22 @@ void armarx::VOR::onStop()
err_sum.swap(this->err_sum);
}
-
-void armarx::VOR::reportJointAngles(const NameValueMap& values, bool valueChanged, const Ice::Current& c)
+void
+armarx::VOR::reportJointAngles(const NameValueMap& values, bool valueChanged, const Ice::Current& c)
{
std::scoped_lock lock(dataMutex);
reportedJointAnglesBool = true;
this->reportedJointAngles = values;
-
}
-void armarx::VOR::reportJointVelocities(const NameValueMap& values, bool valueChanged, const Ice::Current& c)
+void
+armarx::VOR::reportJointVelocities(const NameValueMap& values,
+ bool valueChanged,
+ const Ice::Current& c)
{
std::scoped_lock lock(dataMutex);
reportedJointVelocitiesBool = true;
this->reportedJointVelocities = values;
-
}
-
diff --git a/source/RobotComponents/components/ReflexCombination/vor.h b/source/RobotComponents/components/ReflexCombination/vor.h
index bfd10691ff8e4e6e0cc1c177d31c7c25e1e3ea08..b52bebbf9d2a47659c8f03fbe108b3043635cbc4 100644
--- a/source/RobotComponents/components/ReflexCombination/vor.h
+++ b/source/RobotComponents/components/ReflexCombination/vor.h
@@ -23,16 +23,18 @@
*/
#pragma once
-#include "reflex.h"
-#include "ReflexCombination.h"
-
-#include <RobotAPI/components/units/InertialMeasurementUnit.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
+#include <RobotAPI/components/units/InertialMeasurementUnit.h>
+
+#include "ReflexCombination.h"
+#include "reflex.h"
+
namespace armarx
{
class ReflexCombination;
+
class VOR : virtual public Reflex
{
public:
@@ -40,20 +42,28 @@ namespace armarx
{
onStop();
}
- ~VOR() override {}
+
+ ~VOR() override
+ {
+ }
+
void reportSensorValues(const IMUData& values);
- void setJointNames(std::string eye_pitch_left, std::string eye_pitch_right, std::string eye_yaw_left, std::string eye_yaw_right, std::string neck_roll);
+ void setJointNames(std::string eye_pitch_left,
+ std::string eye_pitch_right,
+ std::string eye_yaw_left,
+ std::string eye_yaw_right,
+ std::string neck_roll);
void setPIDValues(float kp, float ki, float kd);
void setBools(bool armar4, bool velocityBased);
- std::string getName() const override
+ std::string
+ getName() const override
{
return "VOR";
}
// Reflex interface
private:
-
void calc() override;
void onStop() override;
@@ -61,7 +71,8 @@ namespace armarx
private:
std::mutex dataMutex;
- bool reportedSensorValues, reportedJointAnglesBool, reportedJointVelocitiesBool, initialOrientation;
+ bool reportedSensorValues, reportedJointAnglesBool, reportedJointVelocitiesBool,
+ initialOrientation;
bool armar4, velocityBased;
float kp, ki, kd;
std::vector<float> err_old = std::vector<float>(3);
@@ -75,13 +86,18 @@ namespace armarx
// KinematicUnitListener interface
public:
- void reportJointAngles(const NameValueMap& values, bool valueChanged, const Ice::Current& c);
- void reportJointVelocities(const NameValueMap& values, bool valueChanged, const Ice::Current& c);
+ void
+ reportJointAngles(const NameValueMap& values, bool valueChanged, const Ice::Current& c);
+ void
+ reportJointVelocities(const NameValueMap& values, bool valueChanged, const Ice::Current& c);
- void reportHeadTargetChanged(const NameValueMap& targetJointAngles, const FramedPositionBasePtr& targetPosition);
+ void reportHeadTargetChanged(const NameValueMap& targetJointAngles,
+ const FramedPositionBasePtr& targetPosition);
private:
- std::vector<float> pid(std::vector<float> error, std::vector<float> kp, std::vector<float> ki, std::vector<float> kd);
+ std::vector<float> pid(std::vector<float> error,
+ std::vector<float> kp,
+ std::vector<float> ki,
+ std::vector<float> kd);
};
-}
-
+} // namespace armarx
diff --git a/source/RobotComponents/components/RobotIK/RobotIK.cpp b/source/RobotComponents/components/RobotIK/RobotIK.cpp
index 359ee59748322647bb9eba65488d829e940d20f7..91e3e5561d2ad30b1a9b69d743bd7d0d797e5c20 100644
--- a/source/RobotComponents/components/RobotIK/RobotIK.cpp
+++ b/source/RobotComponents/components/RobotIK/RobotIK.cpp
@@ -24,41 +24,41 @@
#include "RobotIK.h"
-#include <VirtualRobot/XML/RobotIO.h>
+#include <algorithm>
+#include <filesystem>
+#include <set>
+
+#include <VirtualRobot/IK/CoMIK.h>
+#include <VirtualRobot/IK/ConstrainedOptimizationIK.h>
+#include <VirtualRobot/IK/HierarchicalIK.h>
+#include <VirtualRobot/IK/JointLimitAvoidanceJacobi.h>
+#include <VirtualRobot/IK/constraints/JointLimitAvoidanceConstraint.h>
+#include <VirtualRobot/IK/constraints/OrientationConstraint.h>
+#include <VirtualRobot/IK/constraints/PoseConstraint.h>
+#include <VirtualRobot/IK/constraints/PositionConstraint.h>
+#include <VirtualRobot/MathTools.h>
#include <VirtualRobot/Nodes/RobotNode.h>
#include <VirtualRobot/RobotNodeSet.h>
-#include <VirtualRobot/Workspace/Reachability.h>
#include <VirtualRobot/Workspace/Manipulability.h>
-#include <VirtualRobot/IK/CoMIK.h>
-#include <VirtualRobot/IK/JointLimitAvoidanceJacobi.h>
-#include <VirtualRobot/IK/HierarchicalIK.h>
-#include <VirtualRobot/IK/ConstrainedOptimizationIK.h>
-
-#include <filesystem>
+#include <VirtualRobot/Workspace/Reachability.h>
+#include <VirtualRobot/XML/RobotIO.h>
-#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/ArmarXManager.h>
#include <ArmarXCore/core/ArmarXObjectScheduler.h>
-#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
-#include <RobotAPI/libraries/core/FramedPose.h>
-#include <VirtualRobot/IK/constraints/PoseConstraint.h>
-#include <VirtualRobot/IK/constraints/JointLimitAvoidanceConstraint.h>
-#include <VirtualRobot/IK/constraints/OrientationConstraint.h>
-#include <VirtualRobot/IK/constraints/PositionConstraint.h>
+#include <ArmarXCore/core/system/ArmarXDataPath.h>
-#include <algorithm>
-#include <set>
+#include <RobotAPI/libraries/core/FramedPose.h>
+#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
using namespace VirtualRobot;
using namespace Eigen;
using namespace Ice;
-
-
namespace armarx
{
- void RobotIK::onInitComponent()
+ void
+ RobotIK::onInitComponent()
{
_robotFile = getProperty<std::string>("RobotFileName").getValue();
@@ -67,7 +67,8 @@ namespace armarx
throw UserException("Could not find robot file " + _robotFile);
}
- this->_robotModel = VirtualRobot::RobotIO::loadRobot(_robotFile, VirtualRobot::RobotIO::eStructure);
+ this->_robotModel =
+ VirtualRobot::RobotIO::loadRobot(_robotFile, VirtualRobot::RobotIO::eStructure);
if (this->_robotModel)
{
@@ -87,17 +88,20 @@ namespace armarx
if (spacesStr != "")
{
- std::string spacesFolder = getProperty<std::string>("ReachabilitySpacesFolder").getValue();
+ std::string spacesFolder =
+ getProperty<std::string>("ReachabilitySpacesFolder").getValue();
for (auto& space : spaces)
{
- ARMARX_INFO << "Initially loading reachability space '" << (spacesFolder + "/" + space) << "'";
+ ARMARX_INFO << "Initially loading reachability space '"
+ << (spacesFolder + "/" + space) << "'";
std::string absolutePath;
if (!ArmarXDataPath::getAbsolutePath(spacesFolder + "/" + space, absolutePath))
{
- ARMARX_ERROR << "Could not load reachability map '" << (spacesFolder + "/" + space) << "'";
+ ARMARX_ERROR << "Could not load reachability map '"
+ << (spacesFolder + "/" + space) << "'";
continue;
}
@@ -109,10 +113,11 @@ namespace armarx
offeringTopic("DebugDrawerUpdates");
}
-
- void RobotIK::onConnectComponent()
+ void
+ RobotIK::onConnectComponent()
{
- _robotStateComponentPrx = getProxy<RobotStateComponentInterfacePrx>(getProperty<std::string>("RobotStateComponentName").getValue());
+ _robotStateComponentPrx = getProxy<RobotStateComponentInterfacePrx>(
+ getProperty<std::string>("RobotStateComponentName").getValue());
std::string robFile_remote = _robotStateComponentPrx->getRobotFilename();
ArmarXDataPath::getAbsolutePath(robFile_remote, robFile_remote);
@@ -121,34 +126,41 @@ namespace armarx
if (robFile_remote.compare(_robotFile) != 0)
{
ARMARX_WARNING << "The robot state component uses the robot model " << robFile_remote
- << " This component, however, uses " << _robotFile << " Both models must be identical!";
+ << " This component, however, uses " << _robotFile
+ << " Both models must be identical!";
}
_synchronizedRobot = _robotStateComponentPrx->getSynchronizedRobot();
debugDrawer = getTopic<armarx::DebugDrawerInterfacePrx>("DebugDrawerUpdates");
-
}
- void RobotIK::onDisconnectComponent()
+ void
+ RobotIK::onDisconnectComponent()
{
}
- PropertyDefinitionsPtr RobotIK::createPropertyDefinitions()
+ PropertyDefinitionsPtr
+ RobotIK::createPropertyDefinitions()
{
- return PropertyDefinitionsPtr(new RobotIKPropertyDefinitions(
- getConfigIdentifier()));
+ return PropertyDefinitionsPtr(new RobotIKPropertyDefinitions(getConfigIdentifier()));
}
- NameValueMap RobotIK::computeIKFramedPose(const std::string& robotNodeSetName,
- const FramedPoseBasePtr& tcpPose, armarx::CartesianSelection cs, const Ice::Current&)
+ NameValueMap
+ RobotIK::computeIKFramedPose(const std::string& robotNodeSetName,
+ const FramedPoseBasePtr& tcpPose,
+ armarx::CartesianSelection cs,
+ const Ice::Current&)
{
NameValueMap ikSolution;
computeIK(robotNodeSetName, toGlobalPose(tcpPose), cs, ikSolution);
return ikSolution;
}
- NameValueMap RobotIK::computeIKGlobalPose(const std::string& robotNodeSetName,
- const PoseBasePtr& tcpPose, armarx::CartesianSelection cs, const Ice::Current&)
+ NameValueMap
+ RobotIK::computeIKGlobalPose(const std::string& robotNodeSetName,
+ const PoseBasePtr& tcpPose,
+ armarx::CartesianSelection cs,
+ const Ice::Current&)
{
NameValueMap ikSolution;
Pose globalTcpPose(tcpPose->position, tcpPose->orientation);
@@ -156,8 +168,11 @@ namespace armarx
return ikSolution;
}
- ExtendedIKResult RobotIK::computeExtendedIKGlobalPose(const std::string& robotNodeSetName,
- const PoseBasePtr& tcpPose, armarx::CartesianSelection cs, const Ice::Current&)
+ ExtendedIKResult
+ RobotIK::computeExtendedIKGlobalPose(const std::string& robotNodeSetName,
+ const PoseBasePtr& tcpPose,
+ armarx::CartesianSelection cs,
+ const Ice::Current&)
{
ExtendedIKResult ikSolution;
Pose globalTcpPose(tcpPose->position, tcpPose->orientation);
@@ -165,7 +180,10 @@ namespace armarx
return ikSolution;
}
- NameValueMap RobotIK::computeCoMIK(const std::string& robotNodeSetJoints, const CoMIKDescriptor& desc, const Ice::Current&)
+ NameValueMap
+ RobotIK::computeCoMIK(const std::string& robotNodeSetJoints,
+ const CoMIKDescriptor& desc,
+ const Ice::Current&)
{
// Make sure we have valid input parameters
if (!_robotModel->hasRobotNodeSet(robotNodeSetJoints))
@@ -187,7 +205,8 @@ namespace armarx
// Create and initialize ik solver
RobotNodeSetPtr joints = _robotModel->getRobotNodeSet(robotNodeSetJoints);
- CoMIK comIkSolver(joints, _robotModel->getRobotNodeSet(desc.robotNodeSetBodies), coordSystem);
+ CoMIK comIkSolver(
+ joints, _robotModel->getRobotNodeSet(desc.robotNodeSetBodies), coordSystem);
Eigen::VectorXf goal(2);
goal(0) = desc.gx;
goal(1) = desc.gy;
@@ -210,16 +229,21 @@ namespace armarx
return result;
}
- NameValueMap RobotIK::computeHierarchicalDeltaIK(const std::string& robotNodeSetName,
- const IKTasks& iktasks, const CoMIKDescriptor& comIK, float stepSize,
- bool avoidJointLimits, bool enableCenterOfMass, const Ice::Current&)
+ NameValueMap
+ RobotIK::computeHierarchicalDeltaIK(const std::string& robotNodeSetName,
+ const IKTasks& iktasks,
+ const CoMIKDescriptor& comIK,
+ float stepSize,
+ bool avoidJointLimits,
+ bool enableCenterOfMass,
+ const Ice::Current&)
{
using PriorityJacobiProviderPair = std::pair<int, JacobiProviderPtr>;
- auto lowerPriorityCompare = [](const PriorityJacobiProviderPair & a, const PriorityJacobiProviderPair & b)
- {
- return a.first < b.first;
- };
- std::multiset<PriorityJacobiProviderPair, decltype(lowerPriorityCompare)> jacobiProviders(lowerPriorityCompare);
+ auto lowerPriorityCompare =
+ [](const PriorityJacobiProviderPair& a, const PriorityJacobiProviderPair& b)
+ { return a.first < b.first; };
+ std::multiset<PriorityJacobiProviderPair, decltype(lowerPriorityCompare)> jacobiProviders(
+ lowerPriorityCompare);
if (!_robotModel->hasRobotNodeSet(robotNodeSetName))
{
@@ -244,7 +268,8 @@ namespace armarx
coordSystem = _robotModel->getRobotNode(ikTask.coordSysName);
}
- DifferentialIKPtr diffIK(new DifferentialIK(rns, coordSystem, convertInverseJacobiMethod(ikTask.ijm)));
+ DifferentialIKPtr diffIK(
+ new DifferentialIK(rns, coordSystem, convertInverseJacobiMethod(ikTask.ijm)));
Pose globalTcpPose(ikTask.tcpGoal->position, ikTask.tcpGoal->orientation);
RobotNodePtr tcp = _robotModel->getRobotNode(ikTask.tcpName);
diffIK->setGoal(globalTcpPose.toEigen(), tcp, convertCartesianSelection(ikTask.csel));
@@ -257,7 +282,8 @@ namespace armarx
{
if (!_robotModel->hasRobotNodeSet(comIK.robotNodeSetBodies))
{
- throw UserException("Unknown robot node set for bodies: " + comIK.robotNodeSetBodies);
+ throw UserException("Unknown robot node set for bodies: " +
+ comIK.robotNodeSetBodies);
}
RobotNodePtr coordSystem = RobotNodePtr();
@@ -267,7 +293,8 @@ namespace armarx
coordSystem = _robotModel->getRobotNode(comIK.coordSysName);
}
- CoMIKPtr comIkSolver(new CoMIK(rns, _robotModel->getRobotNodeSet(comIK.robotNodeSetBodies)));
+ CoMIKPtr comIkSolver(
+ new CoMIK(rns, _robotModel->getRobotNodeSet(comIK.robotNodeSetBodies)));
Eigen::VectorXf goal(2);
goal(0) = comIK.gx;
goal(1) = comIK.gy;
@@ -303,8 +330,15 @@ namespace armarx
return result;
}
- bool RobotIK::createReachabilitySpace(const std::string& chainName, const std::string& coordinateSystem, float stepTranslation,
- float stepRotation, const WorkspaceBounds& minBounds, const WorkspaceBounds& maxBounds, int numSamples, const Ice::Current&)
+ bool
+ RobotIK::createReachabilitySpace(const std::string& chainName,
+ const std::string& coordinateSystem,
+ float stepTranslation,
+ float stepRotation,
+ const WorkspaceBounds& minBounds,
+ const WorkspaceBounds& maxBounds,
+ int numSamples,
+ const Ice::Current&)
{
std::lock_guard<std::recursive_mutex> cacheLock(_accessReachabilityCacheMutex);
@@ -317,8 +351,10 @@ namespace armarx
//VirtualRobot::WorkspaceRepresentationPtr reachability(new Reachability(_robotModel));
VirtualRobot::WorkspaceRepresentationPtr reachability(new Manipulability(_robotModel));
- float minBoundsArray[] = {minBounds.x, minBounds.y, minBounds.z, minBounds.ro, minBounds.pi, minBounds.ya};
- float maxBoundsArray[] = {maxBounds.x, maxBounds.y, maxBounds.z, maxBounds.ro, maxBounds.pi, maxBounds.ya};
+ float minBoundsArray[] = {
+ minBounds.x, minBounds.y, minBounds.z, minBounds.ro, minBounds.pi, minBounds.ya};
+ float maxBoundsArray[] = {
+ maxBounds.x, maxBounds.y, maxBounds.z, maxBounds.ro, maxBounds.pi, maxBounds.ya};
std::lock_guard<std::recursive_mutex> robotLock(_modifyRobotModelMutex);
@@ -331,12 +367,22 @@ namespace armarx
return false;
}
- reachability->initialize(_robotModel->getRobotNodeSet(chainName), stepTranslation, stepRotation, minBoundsArray, maxBoundsArray,
- VirtualRobot::SceneObjectSetPtr(), VirtualRobot::SceneObjectSetPtr(), _robotModel->getRobotNode(coordinateSystem));
+ reachability->initialize(_robotModel->getRobotNodeSet(chainName),
+ stepTranslation,
+ stepRotation,
+ minBoundsArray,
+ maxBoundsArray,
+ VirtualRobot::SceneObjectSetPtr(),
+ VirtualRobot::SceneObjectSetPtr(),
+ _robotModel->getRobotNode(coordinateSystem));
}
else
{
- reachability->initialize(_robotModel->getRobotNodeSet(chainName), stepTranslation, stepRotation, minBoundsArray, maxBoundsArray);
+ reachability->initialize(_robotModel->getRobotNodeSet(chainName),
+ stepTranslation,
+ stepRotation,
+ minBoundsArray,
+ maxBoundsArray);
ARMARX_WARNING << "Using global coordinate system to create reachability space.";
}
@@ -347,17 +393,17 @@ namespace armarx
return true;
}
- bool RobotIK::defineRobotNodeSet(const std::string& name, const NodeNameList& nodes,
- const std::string& tcpName, const std::string& rootNodeName, const Ice::Current&)
+ bool
+ RobotIK::defineRobotNodeSet(const std::string& name,
+ const NodeNameList& nodes,
+ const std::string& tcpName,
+ const std::string& rootNodeName,
+ const Ice::Current&)
{
- auto stringsCompareEqual = [](const std::string & a, const std::string & b)
- {
- return a.compare(b) == 0;
- };
- auto stringsCompareSmaller = [](const std::string & a, const std::string & b)
- {
- return a.compare(b) <= 0;
- };
+ auto stringsCompareEqual = [](const std::string& a, const std::string& b)
+ { return a.compare(b) == 0; };
+ auto stringsCompareSmaller = [](const std::string& a, const std::string& b)
+ { return a.compare(b) <= 0; };
// First check if there is already a set with the given name
// We need to lock here, to make sure we are not adding similar named sets at the same time.
std::lock_guard<std::recursive_mutex> lock(_editRobotNodeSetsMutex);
@@ -383,41 +429,54 @@ namespace armarx
std::sort(nodeNames.begin(), nodeNames.end(), stringsCompareSmaller);
std::vector<std::string> inputNodeNames(nodes);
std::sort(inputNodeNames.begin(), inputNodeNames.end(), stringsCompareSmaller);
- std::pair< std::vector<std::string>::iterator, std::vector<std::string>::iterator > mismatch;
- mismatch = std::mismatch(nodeNames.begin(), nodeNames.end(), inputNodeNames.begin(), stringsCompareEqual);
- setsIdentical &= mismatch.first == nodeNames.end() && mismatch.second == inputNodeNames.end();
+ std::pair<std::vector<std::string>::iterator, std::vector<std::string>::iterator>
+ mismatch;
+ mismatch = std::mismatch(
+ nodeNames.begin(), nodeNames.end(), inputNodeNames.begin(), stringsCompareEqual);
+ setsIdentical &=
+ mismatch.first == nodeNames.end() && mismatch.second == inputNodeNames.end();
return setsIdentical;
}
// Else we can add the new robot node set
- RobotNodeSetPtr rns = RobotNodeSet::createRobotNodeSet(_robotModel, name, nodes, rootNodeName, tcpName, true);
+ RobotNodeSetPtr rns =
+ RobotNodeSet::createRobotNodeSet(_robotModel, name, nodes, rootNodeName, tcpName, true);
return _robotModel->hasRobotNodeSet(name);
}
- std::string RobotIK::getRobotFilename(const Ice::Current&) const
+ std::string
+ RobotIK::getRobotFilename(const Ice::Current&) const
{
return _robotFile;
}
- bool RobotIK::hasReachabilitySpace(const std::string& chainName, const Ice::Current&) const
+ bool
+ RobotIK::hasReachabilitySpace(const std::string& chainName, const Ice::Current&) const
{
std::lock_guard<std::recursive_mutex> lock(_accessReachabilityCacheMutex);
return _reachabilities.count(chainName) > 0;
}
- bool RobotIK::isFramedPoseReachable(const std::string& chainName, const FramedPoseBasePtr& tcpPose, const Ice::Current&) const
+ bool
+ RobotIK::isFramedPoseReachable(const std::string& chainName,
+ const FramedPoseBasePtr& tcpPose,
+ const Ice::Current&) const
{
return isReachable(chainName, toReachabilityMapFrame(tcpPose, chainName));
}
- bool RobotIK::isPoseReachable(const std::string& chainName, const PoseBasePtr& tcpPose, const Ice::Current&) const
+ bool
+ RobotIK::isPoseReachable(const std::string& chainName,
+ const PoseBasePtr& tcpPose,
+ const Ice::Current&) const
{
Pose globalTcpPose(tcpPose->position, tcpPose->orientation);
return isReachable(chainName, globalTcpPose.toEigen());
}
- bool RobotIK::loadReachabilitySpace(const std::string& filename, const Ice::Current&)
+ bool
+ RobotIK::loadReachabilitySpace(const std::string& filename, const Ice::Current&)
{
VirtualRobot::WorkspaceRepresentationPtr newSpace;
bool success = false;
@@ -468,7 +527,8 @@ namespace armarx
}
else
{
- ARMARX_WARNING << "Reachability map for kinematic chain '" << chainName << "' already loaded";
+ ARMARX_WARNING << "Reachability map for kinematic chain '" << chainName
+ << "' already loaded";
return false;
}
}
@@ -480,7 +540,10 @@ namespace armarx
return true;
}
- bool RobotIK::saveReachabilitySpace(const std::string& robotNodeSet, const std::string& filename, const Ice::Current&) const
+ bool
+ RobotIK::saveReachabilitySpace(const std::string& robotNodeSet,
+ const std::string& filename,
+ const Ice::Current&) const
{
std::lock_guard<std::recursive_mutex> lock(_accessReachabilityCacheMutex);
bool success = false;
@@ -506,32 +569,43 @@ namespace armarx
return success;
}
- void RobotIK::computeIK(const std::string& robotNodeSetName, const Eigen::Matrix4f& tcpPose,
- armarx::CartesianSelection cs, NameValueMap& ikSolution)
+ void
+ RobotIK::computeIK(const std::string& robotNodeSetName,
+ const Eigen::Matrix4f& tcpPose,
+ armarx::CartesianSelection cs,
+ NameValueMap& ikSolution)
{
if (!_robotModel->hasRobotNodeSet(robotNodeSetName))
{
- throw UserException("The robot model does not contain the robot node set " + robotNodeSetName);
+ throw UserException("The robot model does not contain the robot node set " +
+ robotNodeSetName);
}
RobotNodeSetPtr rns = _robotModel->getRobotNodeSet(robotNodeSetName);
computeIK(rns, tcpPose, cs, ikSolution);
}
- void RobotIK::computeIK(const std::string& robotNodeSetName, const Eigen::Matrix4f& tcpPose,
- armarx::CartesianSelection cs, ExtendedIKResult& ikSolution)
+ void
+ RobotIK::computeIK(const std::string& robotNodeSetName,
+ const Eigen::Matrix4f& tcpPose,
+ armarx::CartesianSelection cs,
+ ExtendedIKResult& ikSolution)
{
if (!_robotModel->hasRobotNodeSet(robotNodeSetName))
{
- throw UserException("The robot model does not contain the robot node set " + robotNodeSetName);
+ throw UserException("The robot model does not contain the robot node set " +
+ robotNodeSetName);
}
RobotNodeSetPtr rns = _robotModel->getRobotNodeSet(robotNodeSetName);
computeIK(rns, tcpPose, cs, ikSolution);
}
- void RobotIK::computeIK(VirtualRobot::RobotNodeSetPtr nodeSet, const Eigen::Matrix4f& tcpPose,
- armarx::CartesianSelection cs, NameValueMap& ikSolution)
+ void
+ RobotIK::computeIK(VirtualRobot::RobotNodeSetPtr nodeSet,
+ const Eigen::Matrix4f& tcpPose,
+ armarx::CartesianSelection cs,
+ NameValueMap& ikSolution)
{
ikSolution.clear();
@@ -561,26 +635,40 @@ namespace armarx
// Lock is automatically released
}
- bool RobotIK::solveIK(const Eigen::Matrix4f& tcpPose, armarx::CartesianSelection cs, VirtualRobot::RobotNodeSetPtr nodeSet)
+ bool
+ RobotIK::solveIK(const Eigen::Matrix4f& tcpPose,
+ armarx::CartesianSelection cs,
+ VirtualRobot::RobotNodeSetPtr nodeSet)
{
// VirtualRobot::ConstraintPtr poseConstraint(new VirtualRobot::PoseConstraint(_robotModel, nodeSet, nodeSet->getTCP(),
// tcpPose, convertCartesianSelection(cs)));
std::lock_guard<std::recursive_mutex> lock(_modifyRobotModelMutex);
- VirtualRobot::ConstraintPtr posConstraint(new VirtualRobot::PositionConstraint(_robotModel, nodeSet, nodeSet->getTCP(),
- tcpPose.block<3, 1>(0, 3), convertCartesianSelection(cs)));
+ VirtualRobot::ConstraintPtr posConstraint(
+ new VirtualRobot::PositionConstraint(_robotModel,
+ nodeSet,
+ nodeSet->getTCP(),
+ tcpPose.block<3, 1>(0, 3),
+ convertCartesianSelection(cs)));
posConstraint->setOptimizationFunctionFactor(1);
- VirtualRobot::ConstraintPtr oriConstraint(new VirtualRobot::OrientationConstraint(_robotModel, nodeSet, nodeSet->getTCP(),
- tcpPose.block<3, 3>(0, 0), convertCartesianSelection(cs), VirtualRobot::MathTools::deg2rad(2)));
+ VirtualRobot::ConstraintPtr oriConstraint(
+ new VirtualRobot::OrientationConstraint(_robotModel,
+ nodeSet,
+ nodeSet->getTCP(),
+ tcpPose.block<3, 3>(0, 0),
+ convertCartesianSelection(cs),
+ VirtualRobot::MathTools::deg2rad(2)));
oriConstraint->setOptimizationFunctionFactor(1000);
Eigen::VectorXf jointConfig;
nodeSet->getJointValues(jointConfig);
- VirtualRobot::ConstraintPtr referenceConfigConstraint(new VirtualRobot::ReferenceConfigurationConstraint(_robotModel, nodeSet, jointConfig));
+ VirtualRobot::ConstraintPtr referenceConfigConstraint(
+ new VirtualRobot::ReferenceConfigurationConstraint(_robotModel, nodeSet, jointConfig));
referenceConfigConstraint->setOptimizationFunctionFactor(0.1);
- VirtualRobot::ConstraintPtr jointLimitAvoidanceConstraint(new VirtualRobot::JointLimitAvoidanceConstraint(_robotModel, nodeSet));
+ VirtualRobot::ConstraintPtr jointLimitAvoidanceConstraint(
+ new VirtualRobot::JointLimitAvoidanceConstraint(_robotModel, nodeSet));
jointLimitAvoidanceConstraint->setOptimizationFunctionFactor(0.1);
// Instantiate solver and generate IK solution
@@ -604,8 +692,11 @@ namespace armarx
return success;
}
- void RobotIK::computeIK(VirtualRobot::RobotNodeSetPtr nodeSet, const Eigen::Matrix4f& tcpPose,
- armarx::CartesianSelection cs, ExtendedIKResult& ikSolution)
+ void
+ RobotIK::computeIK(VirtualRobot::RobotNodeSetPtr nodeSet,
+ const Eigen::Matrix4f& tcpPose,
+ armarx::CartesianSelection cs,
+ ExtendedIKResult& ikSolution)
{
ikSolution.jointAngles.clear();
// For the rest of this function we need to lock access to the robot,
@@ -625,22 +716,30 @@ namespace armarx
}
//Calculate error
- ikSolution.errorPosition = (nodeSet->getTCP()->getGlobalPose().block<3, 1>(0, 3) - tcpPose.block<3, 1>(0, 3)).norm();
- ikSolution.errorOrientation = Eigen::AngleAxisf(nodeSet->getTCP()->getGlobalPose().block<3, 3>(0, 0) * tcpPose.block<3, 3>(0, 0).inverse()).angle();
+ ikSolution.errorPosition =
+ (nodeSet->getTCP()->getGlobalPose().block<3, 1>(0, 3) - tcpPose.block<3, 1>(0, 3))
+ .norm();
+ ikSolution.errorOrientation =
+ Eigen::AngleAxisf(nodeSet->getTCP()->getGlobalPose().block<3, 3>(0, 0) *
+ tcpPose.block<3, 3>(0, 0).inverse())
+ .angle();
ikSolution.isReachable = success;
// Lock is automatically released
}
-
- Eigen::Matrix4f RobotIK::toGlobalPose(const FramedPoseBasePtr& tcpPose) const
+ Eigen::Matrix4f
+ RobotIK::toGlobalPose(const FramedPoseBasePtr& tcpPose) const
{
- FramedPose framedTcpPose(tcpPose->position, tcpPose->orientation, tcpPose->frame, tcpPose->agent);
+ FramedPose framedTcpPose(
+ tcpPose->position, tcpPose->orientation, tcpPose->frame, tcpPose->agent);
FramedPosePtr globalTcpPose = framedTcpPose.toGlobal(_synchronizedRobot);
return globalTcpPose->toEigen();
}
- Eigen::Matrix4f RobotIK::toReachabilityMapFrame(const FramedPoseBasePtr& tcpPose, const std::string& chainName) const
+ Eigen::Matrix4f
+ RobotIK::toReachabilityMapFrame(const FramedPoseBasePtr& tcpPose,
+ const std::string& chainName) const
{
FramedPosePtr p = FramedPosePtr::dynamicCast(tcpPose);
@@ -657,13 +756,15 @@ namespace armarx
}
else
{
- ARMARX_WARNING << "Could not convert TCP pose to reachability map frame: Map not found.";
+ ARMARX_WARNING
+ << "Could not convert TCP pose to reachability map frame: Map not found.";
}
return p->toEigen();
}
- bool RobotIK::isReachable(const std::string& setName, const Eigen::Matrix4f& tcpPose) const
+ bool
+ RobotIK::isReachable(const std::string& setName, const Eigen::Matrix4f& tcpPose) const
{
ARMARX_INFO << "Checking reachability for kinematic chain '" << setName << "': " << tcpPose;
@@ -676,12 +777,14 @@ namespace armarx
}
else
{
- ARMARX_WARNING << "Could not find reachability map for kinematic chain '" << setName << "'";
+ ARMARX_WARNING << "Could not find reachability map for kinematic chain '" << setName
+ << "'";
return false;
}
}
- VirtualRobot::IKSolver::CartesianSelection RobotIK::convertCartesianSelection(armarx::CartesianSelection cs) const
+ VirtualRobot::IKSolver::CartesianSelection
+ RobotIK::convertCartesianSelection(armarx::CartesianSelection cs) const
{
switch (cs)
{
@@ -707,7 +810,8 @@ namespace armarx
return VirtualRobot::IKSolver::CartesianSelection::All;
}
- VirtualRobot::JacobiProvider::InverseJacobiMethod RobotIK::convertInverseJacobiMethod(armarx::InverseJacobiMethod aenum) const
+ VirtualRobot::JacobiProvider::InverseJacobiMethod
+ RobotIK::convertInverseJacobiMethod(armarx::InverseJacobiMethod aenum) const
{
switch (aenum)
{
@@ -724,13 +828,11 @@ namespace armarx
return VirtualRobot::JacobiProvider::InverseJacobiMethod::eSVD;
}
- void RobotIK::synchRobot() const
+ void
+ RobotIK::synchRobot() const
{
std::lock_guard<std::recursive_mutex> lock(_modifyRobotModelMutex);
RemoteRobot::synchronizeLocalClone(_robotModel, _synchronizedRobot);
}
-}
-
-
-
+} // namespace armarx
diff --git a/source/RobotComponents/components/RobotIK/RobotIK.h b/source/RobotComponents/components/RobotIK/RobotIK.h
index 07be1fc836577ef96cf1e76fe488b59e1040ade1..5d2e77df645a9b57d0016e4c11a44e7269dcad41 100644
--- a/source/RobotComponents/components/RobotIK/RobotIK.h
+++ b/source/RobotComponents/components/RobotIK/RobotIK.h
@@ -24,18 +24,19 @@
#pragma once
+#include <mutex>
+
+#include <VirtualRobot/IK/GenericIKSolver.h>
+
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/application/properties/Properties.h>
#include <ArmarXCore/core/system/ImportExportComponent.h>
-#include <RobotAPI/libraries/core/RobotAPIObjectFactories.h>
-#include <RobotComponents/interface/components/RobotIK.h>
-#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
#include <RobotAPI/interface/core/RobotState.h>
+#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
+#include <RobotAPI/libraries/core/RobotAPIObjectFactories.h>
-#include <VirtualRobot/IK/GenericIKSolver.h>
-
-#include <mutex>
+#include <RobotComponents/interface/components/RobotIK.h>
namespace armarx
{
@@ -43,19 +44,26 @@ namespace armarx
* \class RobotIKPropertyDefinition
* \brief
*/
- class RobotIKPropertyDefinitions:
- public ComponentPropertyDefinitions
+ class RobotIKPropertyDefinitions : public ComponentPropertyDefinitions
{
public:
- RobotIKPropertyDefinitions(std::string prefix):
- ComponentPropertyDefinitions(prefix)
+ RobotIKPropertyDefinitions(std::string prefix) : ComponentPropertyDefinitions(prefix)
{
- defineRequiredProperty<std::string>("RobotFileName", "Filename of VirtualRobot robot model (e.g. robot_model.xml)");
+ defineRequiredProperty<std::string>(
+ "RobotFileName", "Filename of VirtualRobot robot model (e.g. robot_model.xml)");
//Define optional properties
- defineOptionalProperty<std::string>("RobotStateComponentName", "RobotStateComponent", "Name of the robot state component that should be used");
- defineOptionalProperty<std::string>("ReachabilitySpacesFolder", "Path to a folder containing reachability spaces");
- defineOptionalProperty<int>("NumIKTrials", 10, "Number of trials to find an ik solution");
- defineOptionalProperty<std::string>("InitialReachabilitySpaces", "", "Reachability spaces to load initially (semi-colon separated)");
+ defineOptionalProperty<std::string>(
+ "RobotStateComponentName",
+ "RobotStateComponent",
+ "Name of the robot state component that should be used");
+ defineOptionalProperty<std::string>("ReachabilitySpacesFolder",
+ "Path to a folder containing reachability spaces");
+ defineOptionalProperty<int>(
+ "NumIKTrials", 10, "Number of trials to find an ik solution");
+ defineOptionalProperty<std::string>(
+ "InitialReachabilitySpaces",
+ "",
+ "Reachability spaces to load initially (semi-colon separated)");
}
};
@@ -119,7 +127,6 @@ namespace armarx
virtual public RobotIKInterface
{
public:
-
/**
* \return the robot xml filename as specified in the configuration
*/
@@ -130,7 +137,8 @@ namespace armarx
*/
PropertyDefinitionsPtr createPropertyDefinitions() override;
- std::string getDefaultName() const override
+ std::string
+ getDefaultName() const override
{
return "RobotIK";
}
@@ -149,7 +157,9 @@ namespace armarx
* @return A map that maps each joint name to its value in the found IK solution.
*/
NameValueMap computeIKFramedPose(const std::string& robotNodeSetName,
- const FramedPoseBasePtr& tcpPose, armarx::CartesianSelection cs, const Ice::Current& = Ice::emptyCurrent) override;
+ const FramedPoseBasePtr& tcpPose,
+ armarx::CartesianSelection cs,
+ const Ice::Current& = Ice::emptyCurrent) override;
/**
* @brief Computes a single IK solution for the given robot node set and desired global TCP pose.
@@ -164,7 +174,9 @@ namespace armarx
* @return A map that maps each joint name to its value in the found IK solution.
*/
NameValueMap computeIKGlobalPose(const std::string& robotNodeSetName,
- const PoseBasePtr& tcpPose, armarx::CartesianSelection cs, const Ice::Current& = Ice::emptyCurrent) override;
+ const PoseBasePtr& tcpPose,
+ armarx::CartesianSelection cs,
+ const Ice::Current& = Ice::emptyCurrent) override;
/**
* @brief Computes a single IK solution, error and reachability for the given robot node set and desired global TCP pose.
@@ -178,8 +190,11 @@ namespace armarx
* the orientation only or all.
* @return A map that maps each joint name to its value in the found IK solution, the reachability and computational error.
*/
- ExtendedIKResult computeExtendedIKGlobalPose(const std::string& robotNodeSetName,
- const PoseBasePtr& tcpPose, armarx::CartesianSelection cs, const Ice::Current& = Ice::emptyCurrent) override;
+ ExtendedIKResult
+ computeExtendedIKGlobalPose(const std::string& robotNodeSetName,
+ const PoseBasePtr& tcpPose,
+ armarx::CartesianSelection cs,
+ const Ice::Current& = Ice::emptyCurrent) override;
/**
* @brief Computes an IK solution for the given robot joints such that the center of mass lies above the
@@ -192,7 +207,9 @@ namespace armarx
*
* @return The ik-solution. Returns an empty vector if there is no solution.
*/
- NameValueMap computeCoMIK(const std::string& robotNodeSetJoints, const CoMIKDescriptor& desc, const Ice::Current& = Ice::emptyCurrent) override;
+ NameValueMap computeCoMIK(const std::string& robotNodeSetJoints,
+ const CoMIKDescriptor& desc,
+ const Ice::Current& = Ice::emptyCurrent) override;
/**
* @brief Computes a configuration gradient in order to solve several tasks/constraints simultaneously.
@@ -218,8 +235,12 @@ namespace armarx
* @return A configuration gradient...
*/
NameValueMap computeHierarchicalDeltaIK(const std::string& robotNodeSet,
- const IKTasks& iktasks, const CoMIKDescriptor& comIK,
- float stepSize, bool avoidJointLimits, bool enableCenterOfMass, const Ice::Current& = Ice::emptyCurrent) override;
+ const IKTasks& iktasks,
+ const CoMIKDescriptor& comIK,
+ float stepSize,
+ bool avoidJointLimits,
+ bool enableCenterOfMass,
+ const Ice::Current& = Ice::emptyCurrent) override;
/**
* @brief Creates a new reachability space for the given robot node set.
@@ -241,8 +262,14 @@ namespace armarx
* @return True iff the a reachability space for the given robot node set is available after execution of this function.
* False in case of a failure, e.g. there is no chain with the given name.
*/
- bool createReachabilitySpace(const std::string& chainName, const std::string& coordinateSystem, float stepTranslation, float stepRotation,
- const WorkspaceBounds& minBounds, const WorkspaceBounds& maxBounds, int numSamples, const Ice::Current& = Ice::emptyCurrent) override;
+ bool createReachabilitySpace(const std::string& chainName,
+ const std::string& coordinateSystem,
+ float stepTranslation,
+ float stepRotation,
+ const WorkspaceBounds& minBounds,
+ const WorkspaceBounds& maxBounds,
+ int numSamples,
+ const Ice::Current& = Ice::emptyCurrent) override;
/**
* @brief Defines a new robot node set.
@@ -255,8 +282,11 @@ namespace armarx
*
* @return True, iff chain was added or already exists. False, iff a different chain with similar name already exists.
*/
- bool defineRobotNodeSet(const std::string& name, const NodeNameList& nodes,
- const std::string& tcpName, const std::string& rootNode, const Ice::Current& = Ice::emptyCurrent) override;
+ bool defineRobotNodeSet(const std::string& name,
+ const NodeNameList& nodes,
+ const std::string& tcpName,
+ const std::string& rootNode,
+ const Ice::Current& = Ice::emptyCurrent) override;
/**
* @brief Returns whether this component has a reachability space for the given robot node set.
@@ -265,7 +295,8 @@ namespace armarx
* @param chainName Name of the robot node set.
* @return True if there is a reachability space available, else false.
*/
- bool hasReachabilitySpace(const std::string& chainName, const Ice::Current& = Ice::emptyCurrent) const override;
+ bool hasReachabilitySpace(const std::string& chainName,
+ const Ice::Current& = Ice::emptyCurrent) const override;
/**
* @brief Returns whether a given framed pose is currently reachable by the TCP of the given robot node set.
@@ -281,7 +312,9 @@ namespace armarx
* @return True, if the pose is reachable by the TCP of the given chain. False, if it is not reachable or
* there is no reachability space for the given chain available.
*/
- bool isFramedPoseReachable(const std::string& chainName, const FramedPoseBasePtr& tcpPose, const Ice::Current& = Ice::emptyCurrent) const override;
+ bool isFramedPoseReachable(const std::string& chainName,
+ const FramedPoseBasePtr& tcpPose,
+ const Ice::Current& = Ice::emptyCurrent) const override;
/**
* @brief Returns whether a given global pose is currently reachable by the TCP of the given robot node set.
@@ -296,7 +329,9 @@ namespace armarx
* @return True, if the pose is reachable by the TCP of the given chain. False, if it is not reachable or
* there is no reachability space for the given chain available.
*/
- bool isPoseReachable(const std::string& chainName, const PoseBasePtr& tcpPose, const Ice::Current& = Ice::emptyCurrent) const override;
+ bool isPoseReachable(const std::string& chainName,
+ const PoseBasePtr& tcpPose,
+ const Ice::Current& = Ice::emptyCurrent) const override;
/**
* @brief Loads the reachability space from the given file.
@@ -308,7 +343,8 @@ namespace armarx
* where this component is running.
* @return True iff loading the reachability space was successful.
*/
- bool loadReachabilitySpace(const std::string& filename, const Ice::Current& = Ice::emptyCurrent) override;
+ bool loadReachabilitySpace(const std::string& filename,
+ const Ice::Current& = Ice::emptyCurrent) override;
/**
* @brief Saves a previously created reachability space of the given robot node set.
@@ -319,7 +355,9 @@ namespace armarx
* @param filename The filename if the file(must be an accessible path for this component) you wish to save the space in.
* @return True iff saving was successful.
*/
- bool saveReachabilitySpace(const std::string& robotNodeSet, const std::string& filename, const Ice::Current& = Ice::emptyCurrent) const override;
+ bool saveReachabilitySpace(const std::string& robotNodeSet,
+ const std::string& filename,
+ const Ice::Current& = Ice::emptyCurrent) const override;
protected:
@@ -332,27 +370,40 @@ namespace armarx
void onDisconnectComponent() override;
private:
- bool solveIK(const Eigen::Matrix4f& tcpPose, armarx::CartesianSelection cs, VirtualRobot::RobotNodeSetPtr nodeSet);
- void computeIK(const std::string& robotNodeSetName, const Eigen::Matrix4f& tcpPose,
- armarx::CartesianSelection cs, NameValueMap& iksolution);
-
- void computeIK(VirtualRobot::RobotNodeSetPtr nodeSet, const Eigen::Matrix4f& tcpPose,
- armarx::CartesianSelection cs, NameValueMap& iksolution);
-
- void computeIK(const std::string& robotNodeSetName, const Eigen::Matrix4f& tcpPose,
- armarx::CartesianSelection cs, ExtendedIKResult& iksolution);
-
- void computeIK(VirtualRobot::RobotNodeSetPtr nodeSet, const Eigen::Matrix4f& tcpPose,
- armarx::CartesianSelection cs, ExtendedIKResult& iksolution);
+ bool solveIK(const Eigen::Matrix4f& tcpPose,
+ armarx::CartesianSelection cs,
+ VirtualRobot::RobotNodeSetPtr nodeSet);
+ void computeIK(const std::string& robotNodeSetName,
+ const Eigen::Matrix4f& tcpPose,
+ armarx::CartesianSelection cs,
+ NameValueMap& iksolution);
+
+ void computeIK(VirtualRobot::RobotNodeSetPtr nodeSet,
+ const Eigen::Matrix4f& tcpPose,
+ armarx::CartesianSelection cs,
+ NameValueMap& iksolution);
+
+ void computeIK(const std::string& robotNodeSetName,
+ const Eigen::Matrix4f& tcpPose,
+ armarx::CartesianSelection cs,
+ ExtendedIKResult& iksolution);
+
+ void computeIK(VirtualRobot::RobotNodeSetPtr nodeSet,
+ const Eigen::Matrix4f& tcpPose,
+ armarx::CartesianSelection cs,
+ ExtendedIKResult& iksolution);
Eigen::Matrix4f toGlobalPose(const FramedPoseBasePtr& tcpPose) const;
- Eigen::Matrix4f toReachabilityMapFrame(const FramedPoseBasePtr& tcpPose, const std::string& chainName) const;
+ Eigen::Matrix4f toReachabilityMapFrame(const FramedPoseBasePtr& tcpPose,
+ const std::string& chainName) const;
bool isReachable(const std::string& setName, const Eigen::Matrix4f& tcpPose) const;
- VirtualRobot::IKSolver::CartesianSelection convertCartesianSelection(armarx::CartesianSelection cs) const;
+ VirtualRobot::IKSolver::CartesianSelection
+ convertCartesianSelection(armarx::CartesianSelection cs) const;
- VirtualRobot::JacobiProvider::InverseJacobiMethod convertInverseJacobiMethod(armarx::InverseJacobiMethod aenum) const;
+ VirtualRobot::JacobiProvider::InverseJacobiMethod
+ convertInverseJacobiMethod(armarx::InverseJacobiMethod aenum) const;
void synchRobot() const;
@@ -373,10 +424,10 @@ namespace armarx
DebugDrawerInterfacePrx debugDrawer;
// Reachability cache: not thread safe, always lock!
- using ReachabilityCacheType = std::map<std::string, VirtualRobot::WorkspaceRepresentationPtr>;
+ using ReachabilityCacheType =
+ std::map<std::string, VirtualRobot::WorkspaceRepresentationPtr>;
ReachabilityCacheType _reachabilities;
int _numIKTrials;
};
-}
-
+} // namespace armarx
diff --git a/source/RobotComponents/components/RobotIK/test/RobotIKTest.cpp b/source/RobotComponents/components/RobotIK/test/RobotIKTest.cpp
index 7eeab79fe24132af71ac2347882f590bb6ea3761..23cb601f7852f4cf6288cf84f642d33d3825b10e 100644
--- a/source/RobotComponents/components/RobotIK/test/RobotIKTest.cpp
+++ b/source/RobotComponents/components/RobotIK/test/RobotIKTest.cpp
@@ -30,20 +30,21 @@
#define EPSILON_ROT 0.1f
#define NUMBER_THREADS 10
-#include <RobotComponents/Test.h>
-#include <RobotComponents/components/RobotIK/RobotIK.h>
-#include <RobotComponents/components/RobotIK/test/RobotIKTestEnvironment.h>
+#include <iostream>
+#include <thread>
#include <VirtualRobot/RobotNodeSet.h>
-#include <iostream>
-#include <thread>
+#include <RobotComponents/Test.h>
+#include <RobotComponents/components/RobotIK/RobotIK.h>
+#include <RobotComponents/components/RobotIK/test/RobotIKTestEnvironment.h>
// #include <chrono>
// #include <ctime>
-#include <ratio>
#include <random>
+#include <ratio>
-bool comparePosition(const Eigen::Matrix4f& m1, const Eigen::Matrix4f& m2)
+bool
+comparePosition(const Eigen::Matrix4f& m1, const Eigen::Matrix4f& m2)
{
bool equal = true;
@@ -60,7 +61,8 @@ bool comparePosition(const Eigen::Matrix4f& m1, const Eigen::Matrix4f& m2)
return equal;
}
-bool compareOrientation(const Eigen::Matrix4f& m1, const Eigen::Matrix4f& m2)
+bool
+compareOrientation(const Eigen::Matrix4f& m1, const Eigen::Matrix4f& m2)
{
bool equal = true;
@@ -80,7 +82,10 @@ bool compareOrientation(const Eigen::Matrix4f& m1, const Eigen::Matrix4f& m2)
return equal;
}
-bool compareResults(const Eigen::Matrix4f& m1, const Eigen::Matrix4f& m2, armarx::CartesianSelection csel)
+bool
+compareResults(const Eigen::Matrix4f& m1,
+ const Eigen::Matrix4f& m2,
+ armarx::CartesianSelection csel)
{
bool success;
@@ -101,20 +106,28 @@ bool compareResults(const Eigen::Matrix4f& m1, const Eigen::Matrix4f& m2, armarx
return success;
}
-void synchRobot(VirtualRobot::RobotPtr localRobot, armarx::RobotStateComponentInterfacePrx robotState)
+void
+synchRobot(VirtualRobot::RobotPtr localRobot, armarx::RobotStateComponentInterfacePrx robotState)
{
VirtualRobot::RobotConfigPtr robotConfig = localRobot->getConfig();
armarx::SharedRobotInterfacePrx sharedRobot = robotState->getSynchronizedRobot();
for (auto& roboNode : robotConfig->getNodes())
{
- armarx::SharedRobotNodeInterfacePrx sharedRoboNode = sharedRobot->getRobotNode(roboNode->getName());
+ armarx::SharedRobotNodeInterfacePrx sharedRoboNode =
+ sharedRobot->getRobotNode(roboNode->getName());
float val = sharedRoboNode->getJointValue();
localRobot->getRobotNode(roboNode->getName())->setJointValue(val);
}
}
-void testIKGlobalPose(const std::string& setName, armarx::PosePtr pose, bool& success, armarx::CartesianSelection csel,
- VirtualRobot::RobotPtr localRobot, armarx::RobotIKInterfacePrx robotIK, armarx::RobotStateComponentInterfacePrx sharedRobot)
+void
+testIKGlobalPose(const std::string& setName,
+ armarx::PosePtr pose,
+ bool& success,
+ armarx::CartesianSelection csel,
+ VirtualRobot::RobotPtr localRobot,
+ armarx::RobotIKInterfacePrx robotIK,
+ armarx::RobotStateComponentInterfacePrx sharedRobot)
{
// Test component
synchRobot(localRobot, sharedRobot);
@@ -144,8 +157,14 @@ void testIKGlobalPose(const std::string& setName, armarx::PosePtr pose, bool& su
}
}
-void testIKFramedPose(const std::string& setName, armarx::FramedPosePtr fpose, bool& success, armarx::CartesianSelection csel,
- VirtualRobot::RobotPtr localRobot, armarx::RobotIKInterfacePrx robotIK, armarx::RobotStateComponentInterfacePrx roboState)
+void
+testIKFramedPose(const std::string& setName,
+ armarx::FramedPosePtr fpose,
+ bool& success,
+ armarx::CartesianSelection csel,
+ VirtualRobot::RobotPtr localRobot,
+ armarx::RobotIKInterfacePrx robotIK,
+ armarx::RobotStateComponentInterfacePrx roboState)
{
armarx::FramedPosePtr globalPose = fpose->toGlobal(roboState->getSynchronizedRobot());
Eigen::Matrix4f poseGlobal = globalPose->toEigen();
@@ -169,30 +188,25 @@ void testIKFramedPose(const std::string& setName, armarx::FramedPosePtr fpose, b
}
}
-void doGlobalPoseTests(const RobotIKTestEnvironment& testEnv, bool& finished)
+void
+doGlobalPoseTests(const RobotIKTestEnvironment& testEnv, bool& finished)
{
// Create test input
std::string nodeSetName1("TorsoLeftArm");
Eigen::Matrix4f poseMatrix1;
- poseMatrix1 << 1.0f, 0.0f, 0.0f, 400.0f,
- 0.0f, 1.0f, 0.0f, 600.0f,
- 0.0f, 0.0f, 1.0f, 900.0f,
- 0.0f, 0.0f, 0.0f, 1.0f;
+ poseMatrix1 << 1.0f, 0.0f, 0.0f, 400.0f, 0.0f, 1.0f, 0.0f, 600.0f, 0.0f, 0.0f, 1.0f, 900.0f,
+ 0.0f, 0.0f, 0.0f, 1.0f;
armarx::PosePtr pose1 = new armarx::Pose(poseMatrix1);
// Create test input 2
std::string nodeSetName2 = "TorsoRightArm";
Eigen::Matrix4f poseMatrix2;
- poseMatrix2 << 1.0f, 0.0f, 0.0f, 400.0f,
- 0.0f, -1.0f, 0.0f, 100.0f,
- 0.0f, 0.0f, -1.0f, 600.0f,
- 0.0f, 0.0f, 0.0f, 1.0f;
+ poseMatrix2 << 1.0f, 0.0f, 0.0f, 400.0f, 0.0f, -1.0f, 0.0f, 100.0f, 0.0f, 0.0f, -1.0f, 600.0f,
+ 0.0f, 0.0f, 0.0f, 1.0f;
armarx::PosePtr pose2 = new armarx::Pose(poseMatrix2);
Eigen::Matrix4f unreachablePoseMatrix;
- unreachablePoseMatrix << 1.0f, 0.0f, 0.0f, 1000.0f,
- 0.0f, 1.0f, 0.0f, 1000.0f,
- 0.0f, 0.0f, 1.0f, 1300.0f,
- 0.0f, 0.0f, 0.0f, 1.0f;
+ unreachablePoseMatrix << 1.0f, 0.0f, 0.0f, 1000.0f, 0.0f, 1.0f, 0.0f, 1000.0f, 0.0f, 0.0f, 1.0f,
+ 1300.0f, 0.0f, 0.0f, 0.0f, 1.0f;
armarx::PosePtr unreachablePose = new armarx::Pose(unreachablePoseMatrix);
bool testSuccesses[3 * NUMBER_THREADS];
@@ -202,14 +216,32 @@ void doGlobalPoseTests(const RobotIKTestEnvironment& testEnv, bool& finished)
for (unsigned int tid = 0; tid < NUMBER_THREADS; ++tid)
{
VirtualRobot::RobotPtr localRobot1 = testEnv._robotModel->clone("LocalRobot1");
- threads.push_back(std::thread(testIKGlobalPose, std::ref(nodeSetName1), pose1,
- std::ref(testSuccesses[3 * tid]), armarx::CartesianSelection::ePosition, localRobot1, testEnv._robotIK, testEnv._robotStateComponent));
+ threads.push_back(std::thread(testIKGlobalPose,
+ std::ref(nodeSetName1),
+ pose1,
+ std::ref(testSuccesses[3 * tid]),
+ armarx::CartesianSelection::ePosition,
+ localRobot1,
+ testEnv._robotIK,
+ testEnv._robotStateComponent));
VirtualRobot::RobotPtr localRobot2 = testEnv._robotModel->clone("LocalRobot2");
- threads.push_back(std::thread(testIKGlobalPose, std::ref(nodeSetName2), pose2,
- std::ref(testSuccesses[3 * tid + 1]), armarx::CartesianSelection::eAll, localRobot2, testEnv._robotIK, testEnv._robotStateComponent));
+ threads.push_back(std::thread(testIKGlobalPose,
+ std::ref(nodeSetName2),
+ pose2,
+ std::ref(testSuccesses[3 * tid + 1]),
+ armarx::CartesianSelection::eAll,
+ localRobot2,
+ testEnv._robotIK,
+ testEnv._robotStateComponent));
VirtualRobot::RobotPtr localRobot3 = testEnv._robotModel->clone("LocalRobot3");
- threads.push_back(std::thread(testIKGlobalPose, std::ref(nodeSetName2), unreachablePose,
- std::ref(testSuccesses[3 * tid + 2]), armarx::CartesianSelection::eAll, localRobot3, testEnv._robotIK, testEnv._robotStateComponent));
+ threads.push_back(std::thread(testIKGlobalPose,
+ std::ref(nodeSetName2),
+ unreachablePose,
+ std::ref(testSuccesses[3 * tid + 2]),
+ armarx::CartesianSelection::eAll,
+ localRobot3,
+ testEnv._robotIK,
+ testEnv._robotStateComponent));
}
for (size_t tid = 0; tid < threads.size(); ++tid)
@@ -229,17 +261,17 @@ void doGlobalPoseTests(const RobotIKTestEnvironment& testEnv, bool& finished)
finished = true;
}
-void doFramedPoseTests(const RobotIKTestEnvironment& testEnv, bool& finished)
+void
+doFramedPoseTests(const RobotIKTestEnvironment& testEnv, bool& finished)
{
// Create test input
std::string nodeSetName("TorsoLeftArm");
std::string frame("TCP L");
Eigen::Matrix4f poseMatrix;
- poseMatrix << 0.86603f, -0.5f, 0.0f, 50.0f,
- 0.5f, 0.86603f, 0.0f, 60.0f,
- 0.0f, 0.0f, 1.0f, 40.0f,
- 0.0f, 0.0f, 0.0f, 1.0f;
- armarx::FramedPosePtr pose = new armarx::FramedPose(poseMatrix, frame, testEnv._robotStateComponent->getRobotName());
+ poseMatrix << 0.86603f, -0.5f, 0.0f, 50.0f, 0.5f, 0.86603f, 0.0f, 60.0f, 0.0f, 0.0f, 1.0f,
+ 40.0f, 0.0f, 0.0f, 0.0f, 1.0f;
+ armarx::FramedPosePtr pose =
+ new armarx::FramedPose(poseMatrix, frame, testEnv._robotStateComponent->getRobotName());
bool testSuccesses[NUMBER_THREADS];
std::vector<std::thread> threads;
@@ -247,8 +279,14 @@ void doFramedPoseTests(const RobotIKTestEnvironment& testEnv, bool& finished)
for (unsigned int tid = 0; tid < NUMBER_THREADS; ++tid)
{
VirtualRobot::RobotPtr localRobot = testEnv._robotModel->clone("LocalRobot");
- threads.push_back(std::thread(testIKFramedPose, std::ref(nodeSetName), pose,
- std::ref(testSuccesses[tid]), armarx::CartesianSelection::eAll, localRobot, testEnv._robotIK, testEnv._robotStateComponent));
+ threads.push_back(std::thread(testIKFramedPose,
+ std::ref(nodeSetName),
+ pose,
+ std::ref(testSuccesses[tid]),
+ armarx::CartesianSelection::eAll,
+ localRobot,
+ testEnv._robotIK,
+ testEnv._robotStateComponent));
}
for (size_t tid = 0; tid < threads.size(); ++tid)
@@ -264,7 +302,8 @@ void doFramedPoseTests(const RobotIKTestEnvironment& testEnv, bool& finished)
finished = true;
}
-void doReachabilityTests(const RobotIKTestEnvironment& testEnv, bool& finished)
+void
+doReachabilityTests(const RobotIKTestEnvironment& testEnv, bool& finished)
{
std::string nodeSetName("TorsoLeftArm");
std::string frame("TCP L");
@@ -273,16 +312,12 @@ void doReachabilityTests(const RobotIKTestEnvironment& testEnv, bool& finished)
// Define two global poses
// First, a reachable pose
Eigen::Matrix4f matrix;
- matrix << 0.275582f, 0.950628f, 0.142693f, -363.636f,
- 0.892777f, -0.198076f, -0.404616f, 181.818f,
- -0.356376f, 0.238898f, -0.903285f, 727.273f,
- 1.0f, 1.0f, 1.0f, 1.0f;
+ matrix << 0.275582f, 0.950628f, 0.142693f, -363.636f, 0.892777f, -0.198076f, -0.404616f,
+ 181.818f, -0.356376f, 0.238898f, -0.903285f, 727.273f, 1.0f, 1.0f, 1.0f, 1.0f;
armarx::PosePtr reachablePose = new armarx::Pose(matrix);
// Second, a unreachable pose
- matrix << 1.0f, 0.0f, 0.0f, 1000.0f,
- 0.0f, 1.0f, 0.0f, 1000.0f,
- 0.0f, 0.0f, 1.0f, 1300.0f,
- 0.0f, 0.0f, 0.0f, 1.0f;
+ matrix << 1.0f, 0.0f, 0.0f, 1000.0f, 0.0f, 1.0f, 0.0f, 1000.0f, 0.0f, 0.0f, 1.0f, 1300.0f, 0.0f,
+ 0.0f, 0.0f, 1.0f;
armarx::PosePtr unreachablePose = new armarx::Pose(matrix);
// Now define two framed poses
// Again, first reachable
@@ -292,11 +327,10 @@ void doReachabilityTests(const RobotIKTestEnvironment& testEnv, bool& finished)
// 0.0f, 0.0f, 0.0f, 1.0f;
// armarx::FramedPosePtr reachableFramedPose = new armarx::FramedPose(matrix, frame, testEnv._robotStateComponent->getRobotName());
// and an unreachable
- matrix << 0.86603f, -0.5f, 0.0f, 1000.0f,
- 0.5f, 0.86603f, 0.0f, 600.0f,
- 0.0f, 0.0f, 1.0f, 400.0f,
- 0.0f, 0.0f, 0.0f, 1.0f;
- armarx::FramedPosePtr unreachableFramedPose = new armarx::FramedPose(matrix, frame, testEnv._robotStateComponent->getRobotName());
+ matrix << 0.86603f, -0.5f, 0.0f, 1000.0f, 0.5f, 0.86603f, 0.0f, 600.0f, 0.0f, 0.0f, 1.0f,
+ 400.0f, 0.0f, 0.0f, 0.0f, 1.0f;
+ armarx::FramedPosePtr unreachableFramedPose =
+ new armarx::FramedPose(matrix, frame, testEnv._robotStateComponent->getRobotName());
// To create a reachability space we need workspace bounds.
armarx::WorkspaceBounds minBounds;
@@ -325,7 +359,8 @@ void doReachabilityTests(const RobotIKTestEnvironment& testEnv, bool& finished)
// BOOST_CHECK(!testEnv._robotIK->isFramedPoseReachable(nodeSetName, reachableFramedPose));
BOOST_CHECK(!testEnv._robotIK->isFramedPoseReachable(nodeSetName, unreachableFramedPose));
- bool createdReachabilitySpace = testEnv._robotIK->createReachabilitySpace(nodeSetName, baseFrame, 200.0f, 0.3f, minBounds, maxBounds, 1000000);
+ bool createdReachabilitySpace = testEnv._robotIK->createReachabilitySpace(
+ nodeSetName, baseFrame, 200.0f, 0.3f, minBounds, maxBounds, 1000000);
// This should work
BOOST_CHECK(createdReachabilitySpace);
@@ -342,10 +377,10 @@ void doReachabilityTests(const RobotIKTestEnvironment& testEnv, bool& finished)
BOOST_TEST_MESSAGE(savepath);
BOOST_CHECK(testEnv._robotIK->saveReachabilitySpace(nodeSetName, savepath));
finished = true;
-
}
-void doLoadReachabilityTest(const RobotIKTestEnvironment& testEnv, bool& finished)
+void
+doLoadReachabilityTest(const RobotIKTestEnvironment& testEnv, bool& finished)
{
std::string nodeSetName("TorsoLeftArm");
std::string frame("TCP L");
@@ -354,23 +389,18 @@ void doLoadReachabilityTest(const RobotIKTestEnvironment& testEnv, bool& finishe
// Define two global poses
// First, a reachable pose
Eigen::Matrix4f matrix;
- matrix << 0.275582f, 0.950628f, 0.142693f, -363.636f,
- 0.892777f, -0.198076f, -0.404616f, 181.818f,
- -0.356376f, 0.238898f, -0.903285f, 727.273f,
- 1.0f, 1.0f, 1.0f, 1.0f;
+ matrix << 0.275582f, 0.950628f, 0.142693f, -363.636f, 0.892777f, -0.198076f, -0.404616f,
+ 181.818f, -0.356376f, 0.238898f, -0.903285f, 727.273f, 1.0f, 1.0f, 1.0f, 1.0f;
armarx::PosePtr reachablePose = new armarx::Pose(matrix);
// Second, a unreachable pose
- matrix << 1.0f, 0.0f, 0.0f, 1000.0f,
- 0.0f, 1.0f, 0.0f, 1000.0f,
- 0.0f, 0.0f, 1.0f, 1300.0f,
- 0.0f, 0.0f, 0.0f, 1.0f;
+ matrix << 1.0f, 0.0f, 0.0f, 1000.0f, 0.0f, 1.0f, 0.0f, 1000.0f, 0.0f, 0.0f, 1.0f, 1300.0f, 0.0f,
+ 0.0f, 0.0f, 1.0f;
armarx::PosePtr unreachablePose = new armarx::Pose(matrix);
- matrix << 0.86603f, -0.5f, 0.0f, 1000.0f,
- 0.5f, 0.86603f, 0.0f, 600.0f,
- 0.0f, 0.0f, 1.0f, 400.0f,
- 0.0f, 0.0f, 0.0f, 1.0f;
- armarx::FramedPosePtr unreachableFramedPose = new armarx::FramedPose(matrix, frame, testEnv._robotStateComponent->getRobotName());
+ matrix << 0.86603f, -0.5f, 0.0f, 1000.0f, 0.5f, 0.86603f, 0.0f, 600.0f, 0.0f, 0.0f, 1.0f,
+ 400.0f, 0.0f, 0.0f, 0.0f, 1.0f;
+ armarx::FramedPosePtr unreachableFramedPose =
+ new armarx::FramedPose(matrix, frame, testEnv._robotStateComponent->getRobotName());
// To create a reachability space we need workspace bounds.
armarx::WorkspaceBounds minBounds;
@@ -390,7 +420,8 @@ void doLoadReachabilityTest(const RobotIKTestEnvironment& testEnv, bool& finishe
maxBounds.ya = 3.15f;
// Now start testing
- testEnv._robotIK->createReachabilitySpace(nodeSetName, baseFrame, 200.0f, 0.3f, minBounds, maxBounds, 1000000);
+ testEnv._robotIK->createReachabilitySpace(
+ nodeSetName, baseFrame, 200.0f, 0.3f, minBounds, maxBounds, 1000000);
// Now we should have a reachability space:
BOOST_CHECK(testEnv._robotIK->hasReachabilitySpace(nodeSetName));
@@ -404,7 +435,8 @@ void doLoadReachabilityTest(const RobotIKTestEnvironment& testEnv, bool& finishe
finished = true;
}
-void doDefineNodeSetTests(const RobotIKTestEnvironment& testEnv, bool& finished)
+void
+doDefineNodeSetTests(const RobotIKTestEnvironment& testEnv, bool& finished)
{
std::string setName1("TestSet1");
std::string tcpName("TCP L");
@@ -418,21 +450,27 @@ void doDefineNodeSetTests(const RobotIKTestEnvironment& testEnv, bool& finished)
VirtualRobot::RobotPtr localRobot = testEnv._robotModel->clone("MyLocalRobot");
- bool defineSuccessful = testEnv._robotIK->defineRobotNodeSet(setName1, jointList, tcpName, rootNode);
+ bool defineSuccessful =
+ testEnv._robotIK->defineRobotNodeSet(setName1, jointList, tcpName, rootNode);
BOOST_CHECK(defineSuccessful);
- VirtualRobot::RobotNodeSet::createRobotNodeSet(localRobot, setName1, jointList, rootNode, tcpName, true);
+ VirtualRobot::RobotNodeSet::createRobotNodeSet(
+ localRobot, setName1, jointList, rootNode, tcpName, true);
Eigen::Matrix4f poseMatrix1;
- poseMatrix1 << -0.258819f, -0.965926f, 0.0f, -315.0f,
- 0.0f, 0.0f, 1.0f, 550.0f,
- -0.965926f, 0.258819f, 0.0f, 1077.5f,
- 0.0f, 0.0f, 0.0f, 1.0f;
+ poseMatrix1 << -0.258819f, -0.965926f, 0.0f, -315.0f, 0.0f, 0.0f, 1.0f, 550.0f, -0.965926f,
+ 0.258819f, 0.0f, 1077.5f, 0.0f, 0.0f, 0.0f, 1.0f;
armarx::PosePtr pose1 = new armarx::Pose(poseMatrix1);
bool ikSuccess = false;
- testIKGlobalPose(setName1, pose1, ikSuccess, armarx::CartesianSelection::ePosition, localRobot, testEnv._robotIK, testEnv._robotStateComponent);
+ testIKGlobalPose(setName1,
+ pose1,
+ ikSuccess,
+ armarx::CartesianSelection::ePosition,
+ localRobot,
+ testEnv._robotIK,
+ testEnv._robotStateComponent);
BOOST_CHECK(ikSuccess);
// Redefine should always return true.
@@ -441,7 +479,8 @@ void doDefineNodeSetTests(const RobotIKTestEnvironment& testEnv, bool& finished)
// Redefine with different parameters, however, always false
armarx::NodeNameList modifiedJointList(jointList);
modifiedJointList.pop_back();
- BOOST_CHECK(!testEnv._robotIK->defineRobotNodeSet(setName1, modifiedJointList, tcpName, rootNode));
+ BOOST_CHECK(
+ !testEnv._robotIK->defineRobotNodeSet(setName1, modifiedJointList, tcpName, rootNode));
std::string tcpName2("TCP R");
BOOST_CHECK(!testEnv._robotIK->defineRobotNodeSet(setName1, jointList, tcpName2, rootNode));
std::string rootNode2("Shoulder 1 L");
@@ -449,7 +488,8 @@ void doDefineNodeSetTests(const RobotIKTestEnvironment& testEnv, bool& finished)
finished = true;
}
-void doRobotIKComputeCoMIKTests(const RobotIKTestEnvironment& testEnv, bool& finished)
+void
+doRobotIKComputeCoMIKTests(const RobotIKTestEnvironment& testEnv, bool& finished)
{
std::string completeRobotSetname("Robot");
armarx::CoMIKDescriptor comdesc;
@@ -470,12 +510,15 @@ void doRobotIKComputeCoMIKTests(const RobotIKTestEnvironment& testEnv, bool& fin
// BOOST_TEST_MESSAGE("desired com " << comdesc.gx << " " << comdesc.gy);
// BOOST_TEST_MESSAGE("is com " << resultCoM[0] << " " << resultCoM[1]);
- BOOST_CHECK((comdesc.gx - resultCoM[0]) * (comdesc.gx - resultCoM[0]) <= comdesc.tolerance * comdesc.tolerance);
- BOOST_CHECK((comdesc.gy - resultCoM[1]) * (comdesc.gy - resultCoM[1]) <= comdesc.tolerance * comdesc.tolerance);
+ BOOST_CHECK((comdesc.gx - resultCoM[0]) * (comdesc.gx - resultCoM[0]) <=
+ comdesc.tolerance * comdesc.tolerance);
+ BOOST_CHECK((comdesc.gy - resultCoM[1]) * (comdesc.gy - resultCoM[1]) <=
+ comdesc.tolerance * comdesc.tolerance);
finished = true;
}
-void doRobotIKComputeHierarchicalDeltaIKTests(const RobotIKTestEnvironment& testEnv, bool& finished)
+void
+doRobotIKComputeHierarchicalDeltaIKTests(const RobotIKTestEnvironment& testEnv, bool& finished)
{
std::string jointsSetName("TorsoBothArms");
armarx::IKTasks ikTasks;
@@ -486,10 +529,8 @@ void doRobotIKComputeHierarchicalDeltaIKTests(const RobotIKTestEnvironment& test
leftArmGoal.csel = armarx::CartesianSelection::ePosition;
leftArmGoal.ijm = armarx::InverseJacobiMethod::eSVD;
Eigen::Matrix4f poseMatrix1;
- poseMatrix1 << 1.0f, 0.0f, 0.0f, -285.0f,
- 0.0f, 1.0f, 0.0f, 540.0f,
- 0.0f, 0.0f, 1.0f, 1125.0f,
- 0.0f, 0.0f, 0.0f, 1.0f;
+ poseMatrix1 << 1.0f, 0.0f, 0.0f, -285.0f, 0.0f, 1.0f, 0.0f, 540.0f, 0.0f, 0.0f, 1.0f, 1125.0f,
+ 0.0f, 0.0f, 0.0f, 1.0f;
armarx::PosePtr pose1 = new armarx::Pose(poseMatrix1);
leftArmGoal.tcpGoal = pose1;
ikTasks.push_back(leftArmGoal);
@@ -502,10 +543,8 @@ void doRobotIKComputeHierarchicalDeltaIKTests(const RobotIKTestEnvironment& test
rightArmGoal.csel = armarx::CartesianSelection::ePosition;
rightArmGoal.ijm = armarx::InverseJacobiMethod::eSVD;
Eigen::Matrix4f poseMatrix2;
- poseMatrix2 << 1.0f, 0.0f, 0.0f, 400.0f,
- 0.0f, 1.0f, 0.0f, 100.0f,
- 0.0f, 0.0f, 1.0f, 900.0f,
- 0.0f, 0.0f, 0.0f, 1.0f;
+ poseMatrix2 << 1.0f, 0.0f, 0.0f, 400.0f, 0.0f, 1.0f, 0.0f, 100.0f, 0.0f, 0.0f, 1.0f, 900.0f,
+ 0.0f, 0.0f, 0.0f, 1.0f;
armarx::PosePtr pose2 = new armarx::Pose(poseMatrix2);
rightArmGoal.tcpGoal = pose2;
ikTasks.push_back(rightArmGoal);
@@ -524,10 +563,15 @@ void doRobotIKComputeHierarchicalDeltaIKTests(const RobotIKTestEnvironment& test
armarx::NameValueMap ikGradient;
BOOST_TEST_MESSAGE(poseMatrix1);
- while (!compareResults(poseMatrix1, localRobot->getRobotNodeSet("TorsoLeftArm")->getTCP()->getGlobalPose(), armarx::CartesianSelection::ePosition)
- || !compareResults(poseMatrix2, localRobot->getRobotNodeSet("TorsoRightArm")->getTCP()->getGlobalPose(), armarx::CartesianSelection::ePosition))
+ while (!compareResults(poseMatrix1,
+ localRobot->getRobotNodeSet("TorsoLeftArm")->getTCP()->getGlobalPose(),
+ armarx::CartesianSelection::ePosition) ||
+ !compareResults(poseMatrix2,
+ localRobot->getRobotNodeSet("TorsoRightArm")->getTCP()->getGlobalPose(),
+ armarx::CartesianSelection::ePosition))
{
- ikGradient = testEnv._robotIK->computeHierarchicalDeltaIK(jointsSetName, ikTasks, comdesc, stepSize, true, true);
+ ikGradient = testEnv._robotIK->computeHierarchicalDeltaIK(
+ jointsSetName, ikTasks, comdesc, stepSize, true, true);
BOOST_CHECK(ikGradient.size() > 0);
VirtualRobot::RobotNodeSetPtr rns = localRobot->getRobotNodeSet(jointsSetName);
@@ -541,18 +585,27 @@ void doRobotIKComputeHierarchicalDeltaIKTests(const RobotIKTestEnvironment& test
newConfig.insert(armarx::NameValueMap::value_type(node->getName(), val + delta));
}
localRobot->setJointValues(newConfig);
- testEnv._robotStateComponent->reportJointAngles(newConfig, IceUtil::Time::now().toMicroSeconds(), true);
+ testEnv._robotStateComponent->reportJointAngles(
+ newConfig, IceUtil::Time::now().toMicroSeconds(), true);
}
- BOOST_CHECK(compareResults(poseMatrix1, localRobot->getRobotNodeSet("TorsoLeftArm")->getTCP()->getGlobalPose(), armarx::CartesianSelection::ePosition));
- BOOST_CHECK(compareResults(poseMatrix2, localRobot->getRobotNodeSet("TorsoRightArm")->getTCP()->getGlobalPose(), armarx::CartesianSelection::ePosition));
+ BOOST_CHECK(
+ compareResults(poseMatrix1,
+ localRobot->getRobotNodeSet("TorsoLeftArm")->getTCP()->getGlobalPose(),
+ armarx::CartesianSelection::ePosition));
+ BOOST_CHECK(
+ compareResults(poseMatrix2,
+ localRobot->getRobotNodeSet("TorsoRightArm")->getTCP()->getGlobalPose(),
+ armarx::CartesianSelection::ePosition));
Eigen::Vector3f resultCoM = localRobot->getRobotNodeSet(completeRobotSetname)->getCoM();
BOOST_TEST_MESSAGE("desired com " << comdesc.gx << " " << comdesc.gy);
BOOST_TEST_MESSAGE("is com " << resultCoM[0] << " " << resultCoM[1]);
- BOOST_CHECK((comdesc.gx - resultCoM[0]) * (comdesc.gx - resultCoM[0]) <= comdesc.tolerance * comdesc.tolerance);
- BOOST_CHECK((comdesc.gy - resultCoM[1]) * (comdesc.gy - resultCoM[1]) <= comdesc.tolerance * comdesc.tolerance);
+ BOOST_CHECK((comdesc.gx - resultCoM[0]) * (comdesc.gx - resultCoM[0]) <=
+ comdesc.tolerance * comdesc.tolerance);
+ BOOST_CHECK((comdesc.gy - resultCoM[1]) * (comdesc.gy - resultCoM[1]) <=
+ comdesc.tolerance * comdesc.tolerance);
finished = true;
}
diff --git a/source/RobotComponents/components/RobotIK/test/RobotIKTestEnvironment.h b/source/RobotComponents/components/RobotIK/test/RobotIKTestEnvironment.h
index e2097d25a127755693f945c876162db22e9b1dc6..3e9ca7b3701528de14cfa948c3ce2b02467ae13f 100644
--- a/source/RobotComponents/components/RobotIK/test/RobotIKTestEnvironment.h
+++ b/source/RobotComponents/components/RobotIK/test/RobotIKTestEnvironment.h
@@ -20,21 +20,24 @@
* GNU General Public License
*/
-#include <RobotComponents/components/RobotIK/RobotIK.h>
-#include <RobotAPI/components/RobotState/RobotStateComponent.h>
+#include <VirtualRobot/XML/RobotIO.h>
-#include <ArmarXCore/core/test/IceTestHelper.h>
#include <ArmarXCore/core/application/Application.h>
#include <ArmarXCore/core/system/FactoryCollectionBase.h>
+#include <ArmarXCore/core/test/IceTestHelper.h>
-#include <VirtualRobot/XML/RobotIO.h>
+#include <RobotAPI/components/RobotState/RobotStateComponent.h>
#include <RobotComponents/Test.h>
+#include <RobotComponents/components/RobotIK/RobotIK.h>
class RobotIKTestEnvironment
{
public:
- RobotIKTestEnvironment(const std::string& testName, bool loadSpaces = false, int registryPort = 11220, bool addObjects = true)
+ RobotIKTestEnvironment(const std::string& testName,
+ bool loadSpaces = false,
+ int registryPort = 11220,
+ bool addObjects = true)
{
Ice::PropertiesPtr properties = Ice::createProperties();
armarx::Application::LoadDefaultConfig(properties);
@@ -52,7 +55,8 @@ public:
std::string fn = "RobotAPI/robots/Armar3/ArmarIII.xml";
armarx::ArmarXDataPath::getAbsolutePath(fn, fn);
- std::string robotFilename = fn;//test::getCmakeValue("PROJECT_DATA_DIR") + "/RobotAPI/tests/robotmodel/ArmarIII.xml";
+ std::string robotFilename =
+ fn; //test::getCmakeValue("PROJECT_DATA_DIR") + "/RobotAPI/tests/robotmodel/ArmarIII.xml";
properties->setProperty("ArmarX.RobotStateComponent.RobotFileName", robotFilename);
properties->setProperty("ArmarX.RobotStateComponent.ObjectName", "RobotStateComponent");
properties->setProperty("ArmarX.RobotStateComponent.AgentName", "Armar3");
@@ -60,12 +64,14 @@ public:
properties->setProperty("ArmarX.RobotIK.RobotFileName", robotFilename);
properties->setProperty("ArmarX.RobotIK.RobotStateComponentName", "RobotStateComponent");
- _spaceSavePath = armarx::test::getCmakeValue("PROJECT_BINARY_DIR") + "/Testing/Temporary/spaces/";
+ _spaceSavePath =
+ armarx::test::getCmakeValue("PROJECT_BINARY_DIR") + "/Testing/Temporary/spaces/";
if (loadSpaces)
{
properties->setProperty("ArmarX.RobotIK.ReachabilitySpacesFolder", _spaceSavePath);
- properties->setProperty("ArmarX.RobotIK.InitialReachabilitySpaces", "testReachabilitySpace.bin");
+ properties->setProperty("ArmarX.RobotIK.InitialReachabilitySpaces",
+ "testReachabilitySpace.bin");
}
_iceTestHelper = new IceTestHelper(registryPort, registryPort + 1);
@@ -75,11 +81,17 @@ public:
if (addObjects)
{
- _robotStateComponent = _manager->createComponentAndRun<armarx::RobotStateComponent, armarx::RobotStateComponentInterfacePrx>("ArmarX", "RobotStateComponent");
- _robotIK = _manager->createComponentAndRun<armarx::RobotIK, armarx::RobotIKInterfacePrx>("ArmarX", "RobotIK");
+ _robotStateComponent =
+ _manager->createComponentAndRun<armarx::RobotStateComponent,
+ armarx::RobotStateComponentInterfacePrx>(
+ "ArmarX", "RobotStateComponent");
+ _robotIK =
+ _manager->createComponentAndRun<armarx::RobotIK, armarx::RobotIKInterfacePrx>(
+ "ArmarX", "RobotIK");
}
- _robotModel = VirtualRobot::RobotIO::loadRobot(robotFilename, VirtualRobot::RobotIO::eStructure);
+ _robotModel =
+ VirtualRobot::RobotIO::loadRobot(robotFilename, VirtualRobot::RobotIO::eStructure);
}
~RobotIKTestEnvironment()
@@ -96,4 +108,3 @@ public:
};
using RobotIKTestEnvironmentPtr = std::shared_ptr<RobotIKTestEnvironment>;
-
diff --git a/source/RobotComponents/components/SimpleEpisodicMemoryKinematicUnitConnector/SimpleEpisodicMemoryKinematicUnitConnector.cpp b/source/RobotComponents/components/SimpleEpisodicMemoryKinematicUnitConnector/SimpleEpisodicMemoryKinematicUnitConnector.cpp
index 616a3e5e327f7b40994408ce39c66896a8a73454..d81ef67fba9bf6d865ca63fe9f39a3c7ae2e1e5a 100644
--- a/source/RobotComponents/components/SimpleEpisodicMemoryKinematicUnitConnector/SimpleEpisodicMemoryKinematicUnitConnector.cpp
+++ b/source/RobotComponents/components/SimpleEpisodicMemoryKinematicUnitConnector/SimpleEpisodicMemoryKinematicUnitConnector.cpp
@@ -25,56 +25,58 @@
namespace armarx
{
- armarx::PropertyDefinitionsPtr SimpleEpisodicMemoryKinematicUnitConnector::createPropertyDefinitions()
+ armarx::PropertyDefinitionsPtr
+ SimpleEpisodicMemoryKinematicUnitConnector::createPropertyDefinitions()
{
- armarx::PropertyDefinitionsPtr def{new armarx::ComponentPropertyDefinitions{getConfigIdentifier()}};
+ armarx::PropertyDefinitionsPtr def{
+ new armarx::ComponentPropertyDefinitions{getConfigIdentifier()}};
def->topic<KinematicUnitListener>("RealRobotState", "KinematicUnitName");
def->optional(frequency, "UpdateFrequency", "Frequency of updates in Hz");
return def;
}
- std::string SimpleEpisodicMemoryKinematicUnitConnector::getDefaultName() const
+ std::string
+ SimpleEpisodicMemoryKinematicUnitConnector::getDefaultName() const
{
return "SimpleEpisodicMemoryKinematicUnitConnector";
}
SimpleEpisodicMemoryKinematicUnitConnector::SimpleEpisodicMemoryKinematicUnitConnector() :
- frequency(10),
- updated(false),
- timestampLastUpdateInMs(0)
+ frequency(10), updated(false), timestampLastUpdateInMs(0)
{
-
}
-
- void SimpleEpisodicMemoryKinematicUnitConnector::onInitComponent()
+ void
+ SimpleEpisodicMemoryKinematicUnitConnector::onInitComponent()
{
usingProxy(m_simple_episodic_memory_proxy_name);
}
-
- void SimpleEpisodicMemoryKinematicUnitConnector::onConnectComponent()
+ void
+ SimpleEpisodicMemoryKinematicUnitConnector::onConnectComponent()
{
getProxy(m_simple_episodic_memory, m_simple_episodic_memory_proxy_name);
- periodic_task = new PeriodicTask<SimpleEpisodicMemoryKinematicUnitConnector>(this,
- &SimpleEpisodicMemoryKinematicUnitConnector::checkAndSendToMemory, (1.0f / frequency * 1000));
+ periodic_task = new PeriodicTask<SimpleEpisodicMemoryKinematicUnitConnector>(
+ this,
+ &SimpleEpisodicMemoryKinematicUnitConnector::checkAndSendToMemory,
+ (1.0f / frequency * 1000));
periodic_task->start();
}
-
- void SimpleEpisodicMemoryKinematicUnitConnector::onDisconnectComponent()
+ void
+ SimpleEpisodicMemoryKinematicUnitConnector::onDisconnectComponent()
{
periodic_task->stop();
}
-
- void SimpleEpisodicMemoryKinematicUnitConnector::onExitComponent()
+ void
+ SimpleEpisodicMemoryKinematicUnitConnector::onExitComponent()
{
-
}
- void SimpleEpisodicMemoryKinematicUnitConnector::checkAndSendToMemory()
+ void
+ SimpleEpisodicMemoryKinematicUnitConnector::checkAndSendToMemory()
{
std::lock_guard l(jointAngle_mutex);
std::lock_guard ll(jointVelocity_mutex);
@@ -132,14 +134,22 @@ namespace armarx
updated = false;
timestampLastUpdateInMs = 0;
-
}
- void SimpleEpisodicMemoryKinematicUnitConnector::reportControlModeChanged(const NameControlModeMap&, Ice::Long, bool updated, const Ice::Current&)
+ void
+ SimpleEpisodicMemoryKinematicUnitConnector::reportControlModeChanged(const NameControlModeMap&,
+ Ice::Long,
+ bool updated,
+ const Ice::Current&)
{
//ARMARX_DEBUG << "Received reportControlModeChanged";
}
- void SimpleEpisodicMemoryKinematicUnitConnector::reportJointAngles(const NameValueMap& data, Ice::Long ts, bool updated, const Ice::Current&)
+
+ void
+ SimpleEpisodicMemoryKinematicUnitConnector::reportJointAngles(const NameValueMap& data,
+ Ice::Long ts,
+ bool updated,
+ const Ice::Current&)
{
std::lock_guard l(jointAngle_mutex);
double received = IceUtil::Time::now().toMilliSecondsDouble();
@@ -154,7 +164,12 @@ namespace armarx
this->timestampLastUpdateInMs = received;
}
}
- void SimpleEpisodicMemoryKinematicUnitConnector::reportJointVelocities(const NameValueMap& data, Ice::Long, bool updated, const Ice::Current&)
+
+ void
+ SimpleEpisodicMemoryKinematicUnitConnector::reportJointVelocities(const NameValueMap& data,
+ Ice::Long,
+ bool updated,
+ const Ice::Current&)
{
std::lock_guard l(jointVelocity_mutex);
double received = IceUtil::Time::now().toMilliSecondsDouble();
@@ -169,7 +184,12 @@ namespace armarx
this->timestampLastUpdateInMs = received;
}
}
- void SimpleEpisodicMemoryKinematicUnitConnector::reportJointTorques(const NameValueMap& data, Ice::Long, bool updated, const Ice::Current&)
+
+ void
+ SimpleEpisodicMemoryKinematicUnitConnector::reportJointTorques(const NameValueMap& data,
+ Ice::Long,
+ bool updated,
+ const Ice::Current&)
{
std::lock_guard l(jointTorque_mutex);
double received = IceUtil::Time::now().toMilliSecondsDouble();
@@ -184,7 +204,12 @@ namespace armarx
this->timestampLastUpdateInMs = received;
}
}
- void SimpleEpisodicMemoryKinematicUnitConnector::reportJointAccelerations(const NameValueMap& data, Ice::Long updated, bool, const Ice::Current&)
+
+ void
+ SimpleEpisodicMemoryKinematicUnitConnector::reportJointAccelerations(const NameValueMap& data,
+ Ice::Long updated,
+ bool,
+ const Ice::Current&)
{
std::lock_guard l(jointAcceleration_mutex);
double received = IceUtil::Time::now().toMilliSecondsDouble();
@@ -199,7 +224,12 @@ namespace armarx
this->timestampLastUpdateInMs = received;
}
}
- void SimpleEpisodicMemoryKinematicUnitConnector::reportJointCurrents(const NameValueMap& data, Ice::Long, bool updated, const Ice::Current&)
+
+ void
+ SimpleEpisodicMemoryKinematicUnitConnector::reportJointCurrents(const NameValueMap& data,
+ Ice::Long,
+ bool updated,
+ const Ice::Current&)
{
std::lock_guard l(jointCurrent_mutex);
double received = IceUtil::Time::now().toMilliSecondsDouble();
@@ -214,7 +244,13 @@ namespace armarx
this->timestampLastUpdateInMs = received;
}
}
- void SimpleEpisodicMemoryKinematicUnitConnector::reportJointMotorTemperatures(const NameValueMap& data, Ice::Long, bool updated, const Ice::Current&)
+
+ void
+ SimpleEpisodicMemoryKinematicUnitConnector::reportJointMotorTemperatures(
+ const NameValueMap& data,
+ Ice::Long,
+ bool updated,
+ const Ice::Current&)
{
std::lock_guard l(jointTemperature_mutex);
double received = IceUtil::Time::now().toMilliSecondsDouble();
@@ -229,7 +265,12 @@ namespace armarx
this->timestampLastUpdateInMs = received;
}
}
- void SimpleEpisodicMemoryKinematicUnitConnector::reportJointStatuses(const NameStatusMap& data, Ice::Long, bool updated, const Ice::Current&)
+
+ void
+ SimpleEpisodicMemoryKinematicUnitConnector::reportJointStatuses(const NameStatusMap& data,
+ Ice::Long,
+ bool updated,
+ const Ice::Current&)
{
std::lock_guard l(jointEnabled_mutex);
double received = IceUtil::Time::now().toMilliSecondsDouble();
@@ -244,4 +285,4 @@ namespace armarx
this->timestampLastUpdateInMs = received;
}
}
-}
+} // namespace armarx
diff --git a/source/RobotComponents/components/SimpleEpisodicMemoryKinematicUnitConnector/SimpleEpisodicMemoryKinematicUnitConnector.h b/source/RobotComponents/components/SimpleEpisodicMemoryKinematicUnitConnector/SimpleEpisodicMemoryKinematicUnitConnector.h
index b784b0418ecc844201103b8e11caf42260a315aa..acf6559848704ae0efb5bec63a3c4240f9759b48 100644
--- a/source/RobotComponents/components/SimpleEpisodicMemoryKinematicUnitConnector/SimpleEpisodicMemoryKinematicUnitConnector.h
+++ b/source/RobotComponents/components/SimpleEpisodicMemoryKinematicUnitConnector/SimpleEpisodicMemoryKinematicUnitConnector.h
@@ -23,13 +23,13 @@
#pragma once
-#include <eigen3/Eigen/Core>
-
#include <ArmarXCore/core/Component.h>
-#include <MemoryX/components/SimpleEpisodicMemory/SimpleEpisodicMemoryConnector.h>
+#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
+
#include <RobotAPI/interface/units/KinematicUnitInterface.h>
-#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
+#include <MemoryX/components/SimpleEpisodicMemory/SimpleEpisodicMemoryConnector.h>
+#include <eigen3/Eigen/Core>
namespace armarx
{
@@ -43,20 +43,21 @@ namespace armarx
SimpleEpisodicMemoryKinematicUnitConnector();
- void reportControlModeChanged(const NameControlModeMap&, Ice::Long, bool, const Ice::Current&);
+ void
+ reportControlModeChanged(const NameControlModeMap&, Ice::Long, bool, const Ice::Current&);
void reportJointAngles(const NameValueMap&, Ice::Long, bool, const Ice::Current&);
void reportJointVelocities(const NameValueMap&, Ice::Long, bool, const Ice::Current&);
void reportJointTorques(const NameValueMap&, Ice::Long, bool, const Ice::Current&);
void reportJointAccelerations(const NameValueMap&, Ice::Long, bool, const Ice::Current&);
void reportJointCurrents(const NameValueMap&, Ice::Long, bool, const Ice::Current&);
- void reportJointMotorTemperatures(const NameValueMap&, Ice::Long, bool, const Ice::Current&);
+ void
+ reportJointMotorTemperatures(const NameValueMap&, Ice::Long, bool, const Ice::Current&);
void reportJointStatuses(const NameStatusMap&, Ice::Long, bool, const Ice::Current&);
private:
void checkAndSendToMemory();
protected:
-
/// @see armarx::ManagedIceObject::onInitComponent()
void onInitComponent() override;
@@ -93,6 +94,5 @@ namespace armarx
std::mutex jointTemperature_mutex;
std::map<std::string, bool> jointEnabledMap;
std::mutex jointEnabled_mutex;
-
};
-}
+} // namespace armarx
diff --git a/source/RobotComponents/components/SimpleEpisodicMemoryKinematicUnitConnector/test/SimpleEpisodicMemoryKinematicUnitConnectorTest.cpp b/source/RobotComponents/components/SimpleEpisodicMemoryKinematicUnitConnector/test/SimpleEpisodicMemoryKinematicUnitConnectorTest.cpp
index 9f2e97cb7be52a157fd6516789a412567299b3ff..64443fdd22f231bdc62caa1c73c25022843a5728 100644
--- a/source/RobotComponents/components/SimpleEpisodicMemoryKinematicUnitConnector/test/SimpleEpisodicMemoryKinematicUnitConnectorTest.cpp
+++ b/source/RobotComponents/components/SimpleEpisodicMemoryKinematicUnitConnector/test/SimpleEpisodicMemoryKinematicUnitConnectorTest.cpp
@@ -24,11 +24,11 @@
#define ARMARX_BOOST_TEST
+#include <iostream>
+
#include <RobotComponents/Test.h>
#include <RobotComponents/components/SimpleEpisodicMemoryKinematicUnitConnector/SimpleEpisodicMemoryKinematicUnitConnector.h>
-#include <iostream>
-
BOOST_AUTO_TEST_CASE(testExample)
{
armarx::SimpleEpisodicMemoryKinematicUnitConnector instance;
diff --git a/source/RobotComponents/components/SimpleEpisodicMemoryPlatformUnitConnector/SimpleEpisodicMemoryPlatformUnitConnector.cpp b/source/RobotComponents/components/SimpleEpisodicMemoryPlatformUnitConnector/SimpleEpisodicMemoryPlatformUnitConnector.cpp
index 78035fbf1d5a68a25d41d19137b06d7fee994d33..a08ab2acd486ea7326e76c796d341158002c3ada 100644
--- a/source/RobotComponents/components/SimpleEpisodicMemoryPlatformUnitConnector/SimpleEpisodicMemoryPlatformUnitConnector.cpp
+++ b/source/RobotComponents/components/SimpleEpisodicMemoryPlatformUnitConnector/SimpleEpisodicMemoryPlatformUnitConnector.cpp
@@ -22,20 +22,22 @@
#include "SimpleEpisodicMemoryPlatformUnitConnector.h"
-
namespace armarx
{
- armarx::PropertyDefinitionsPtr SimpleEpisodicMemoryPlatformUnitConnector::createPropertyDefinitions()
+ armarx::PropertyDefinitionsPtr
+ SimpleEpisodicMemoryPlatformUnitConnector::createPropertyDefinitions()
{
- armarx::PropertyDefinitionsPtr def{new armarx::ComponentPropertyDefinitions{getConfigIdentifier()}};
+ armarx::PropertyDefinitionsPtr def{
+ new armarx::ComponentPropertyDefinitions{getConfigIdentifier()}};
def->topic<PlatformUnitListener>("Armar6PlatformUnit", "PlatformUnitName");
def->optional(frequency, "UpdateFrequency", "Frequency of updates in Hz");
return def;
}
- std::string SimpleEpisodicMemoryPlatformUnitConnector::getDefaultName() const
+ std::string
+ SimpleEpisodicMemoryPlatformUnitConnector::getDefaultName() const
{
return "SimpleEpisodicMemoryPlatformUnitConnector";
}
@@ -47,37 +49,38 @@ namespace armarx
updated_target(false),
timestampLastUpdateInMs_target(0)
{
-
}
-
- void SimpleEpisodicMemoryPlatformUnitConnector::onInitComponent()
+ void
+ SimpleEpisodicMemoryPlatformUnitConnector::onInitComponent()
{
usingProxy(m_simple_episodic_memory_proxy_name);
}
-
- void SimpleEpisodicMemoryPlatformUnitConnector::onConnectComponent()
+ void
+ SimpleEpisodicMemoryPlatformUnitConnector::onConnectComponent()
{
getProxy(m_simple_episodic_memory, m_simple_episodic_memory_proxy_name);
- periodic_task = new PeriodicTask<SimpleEpisodicMemoryPlatformUnitConnector>(this,
- &SimpleEpisodicMemoryPlatformUnitConnector::checkAndSendToMemory, (1.0f / frequency * 1000));
+ periodic_task = new PeriodicTask<SimpleEpisodicMemoryPlatformUnitConnector>(
+ this,
+ &SimpleEpisodicMemoryPlatformUnitConnector::checkAndSendToMemory,
+ (1.0f / frequency * 1000));
periodic_task->start();
}
-
- void SimpleEpisodicMemoryPlatformUnitConnector::onDisconnectComponent()
+ void
+ SimpleEpisodicMemoryPlatformUnitConnector::onDisconnectComponent()
{
periodic_task->stop();
}
-
- void SimpleEpisodicMemoryPlatformUnitConnector::onExitComponent()
+ void
+ SimpleEpisodicMemoryPlatformUnitConnector::onExitComponent()
{
-
}
- void SimpleEpisodicMemoryPlatformUnitConnector::checkAndSendToMemory()
+ void
+ SimpleEpisodicMemoryPlatformUnitConnector::checkAndSendToMemory()
{
std::lock_guard l(platformPose_mutex);
std::lock_guard ll(platformTarget_mutex);
@@ -123,8 +126,9 @@ namespace armarx
}
}
-
- void SimpleEpisodicMemoryPlatformUnitConnector::reportPlatformPose(const PlatformPose& pose, const Ice::Current&)
+ void
+ SimpleEpisodicMemoryPlatformUnitConnector::reportPlatformPose(const PlatformPose& pose,
+ const Ice::Current&)
{
std::lock_guard l(platformPose_mutex);
std::lock_guard u(updatedMutex_pose);
@@ -136,7 +140,11 @@ namespace armarx
timestampLastUpdateInMs_pose = received;
}
- void SimpleEpisodicMemoryPlatformUnitConnector::reportNewTargetPose(Ice::Float t1, Ice::Float t2, Ice::Float t3, const Ice::Current&)
+ void
+ SimpleEpisodicMemoryPlatformUnitConnector::reportNewTargetPose(Ice::Float t1,
+ Ice::Float t2,
+ Ice::Float t3,
+ const Ice::Current&)
{
std::lock_guard l(platformTarget_mutex);
std::lock_guard u(updatedMutex_target);
@@ -148,7 +156,11 @@ namespace armarx
timestampLastUpdateInMs_target = received;
}
- void SimpleEpisodicMemoryPlatformUnitConnector::reportPlatformVelocity(Ice::Float v1, Ice::Float v2, Ice::Float v3, const Ice::Current&)
+ void
+ SimpleEpisodicMemoryPlatformUnitConnector::reportPlatformVelocity(Ice::Float v1,
+ Ice::Float v2,
+ Ice::Float v3,
+ const Ice::Current&)
{
std::lock_guard l(platformAcceleration_mutex);
std::lock_guard u(updatedMutex_pose);
@@ -160,8 +172,12 @@ namespace armarx
timestampLastUpdateInMs_pose = received;
}
- void SimpleEpisodicMemoryPlatformUnitConnector::reportPlatformOdometryPose(Ice::Float, Ice::Float, Ice::Float, const Ice::Current&)
+ void
+ SimpleEpisodicMemoryPlatformUnitConnector::reportPlatformOdometryPose(Ice::Float,
+ Ice::Float,
+ Ice::Float,
+ const Ice::Current&)
{
}
-}
+} // namespace armarx
diff --git a/source/RobotComponents/components/SimpleEpisodicMemoryPlatformUnitConnector/SimpleEpisodicMemoryPlatformUnitConnector.h b/source/RobotComponents/components/SimpleEpisodicMemoryPlatformUnitConnector/SimpleEpisodicMemoryPlatformUnitConnector.h
index d9cdda112331b78ae6748578c9b9e2b80c2d59a8..01c40cb58524db20c110818f1a3a61fb3594d8a6 100644
--- a/source/RobotComponents/components/SimpleEpisodicMemoryPlatformUnitConnector/SimpleEpisodicMemoryPlatformUnitConnector.h
+++ b/source/RobotComponents/components/SimpleEpisodicMemoryPlatformUnitConnector/SimpleEpisodicMemoryPlatformUnitConnector.h
@@ -25,11 +25,11 @@
#include <Eigen/Core>
#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
#include <RobotAPI/interface/units/PlatformUnitInterface.h>
-#include <MemoryX/components/SimpleEpisodicMemory/SimpleEpisodicMemoryConnector.h>
-#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
+#include <MemoryX/components/SimpleEpisodicMemory/SimpleEpisodicMemoryConnector.h>
namespace armarx
{
@@ -52,7 +52,6 @@ namespace armarx
void reportPlatformOdometryPose(Ice::Float, Ice::Float, Ice::Float, const Ice::Current&);
protected:
-
/// @see armarx::ManagedIceObject::onInitComponent()
void onInitComponent() override;
@@ -97,6 +96,5 @@ namespace armarx
float acc_y;
float acc_rot;
std::mutex platformAcceleration_mutex;
-
};
-}
+} // namespace armarx
diff --git a/source/RobotComponents/components/SimpleEpisodicMemoryPlatformUnitConnector/test/SimpleEpisodicMemoryPlatformUnitConnectorTest.cpp b/source/RobotComponents/components/SimpleEpisodicMemoryPlatformUnitConnector/test/SimpleEpisodicMemoryPlatformUnitConnectorTest.cpp
index 058530dfc464ee4c70d7e53f12cd264ffa5c4af3..f5dcb515feb9c635b52444a6b67000eee4fcbd3d 100644
--- a/source/RobotComponents/components/SimpleEpisodicMemoryPlatformUnitConnector/test/SimpleEpisodicMemoryPlatformUnitConnectorTest.cpp
+++ b/source/RobotComponents/components/SimpleEpisodicMemoryPlatformUnitConnector/test/SimpleEpisodicMemoryPlatformUnitConnectorTest.cpp
@@ -24,11 +24,11 @@
#define ARMARX_BOOST_TEST
+#include <iostream>
+
#include <RobotComponents/Test.h>
#include <RobotComponents/components/SimpleEpisodicMemoryPlatformUnitConnector/SimpleEpisodicMemoryPlatformUnitConnector.h>
-#include <iostream>
-
BOOST_AUTO_TEST_CASE(testExample)
{
armarx::SimpleEpisodicMemoryPlatformUnitConnector instance;
diff --git a/source/RobotComponents/components/SimpleGraspGenerator/SimpleGraspGenerator.cpp b/source/RobotComponents/components/SimpleGraspGenerator/SimpleGraspGenerator.cpp
index 26f149666f4182047964bfefcc0370e196441053..d20e699c9335f4dc979b0f9249fd8b89f9451952 100644
--- a/source/RobotComponents/components/SimpleGraspGenerator/SimpleGraspGenerator.cpp
+++ b/source/RobotComponents/components/SimpleGraspGenerator/SimpleGraspGenerator.cpp
@@ -23,20 +23,26 @@
*/
#include "SimpleGraspGenerator.h"
-#include <MemoryX/libraries/memorytypes/entity/ObjectInstance.h>
-#include <MemoryX/libraries/memorytypes/MemoryXTypesObjectFactories.h>
-#include <MemoryX/core/MemoryXCoreObjectFactories.h>
+
+#include <VirtualRobot/Grasping/Grasp.h>
+#include <VirtualRobot/Grasping/GraspSet.h>
+
+#include <ArmarXCore/core/util/StringHelpers.h>
+
#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/libraries/core/LinkedPose.h>
-#include <VirtualRobot/Grasping/GraspSet.h>
-#include <MemoryX/libraries/helpers/VirtualRobotHelpers/SimoxObjectWrapper.h>
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
-#include <ArmarXCore/core/util/StringHelpers.h>
+
+#include <MemoryX/core/MemoryXCoreObjectFactories.h>
+#include <MemoryX/libraries/helpers/VirtualRobotHelpers/SimoxObjectWrapper.h>
+#include <MemoryX/libraries/memorytypes/MemoryXTypesObjectFactories.h>
+#include <MemoryX/libraries/memorytypes/entity/ObjectInstance.h>
using namespace armarx;
using namespace memoryx;
-void SimpleGraspGenerator::onInitComponent()
+void
+SimpleGraspGenerator::onInitComponent()
{
usingProxy("WorkingMemory");
usingProxy("PriorKnowledge");
@@ -45,7 +51,8 @@ void SimpleGraspGenerator::onInitComponent()
offeringTopic("DebugDrawerUpdates");
}
-void SimpleGraspGenerator::onConnectComponent()
+void
+SimpleGraspGenerator::onConnectComponent()
{
getProxy(rsc, "RobotStateComponent");
getProxy(wm, "WorkingMemory");
@@ -53,22 +60,25 @@ void SimpleGraspGenerator::onConnectComponent()
objectInstances = wm->getObjectInstancesSegment();
objectClasses = prior->getObjectClassesSegment();
- fileManager = memoryx::GridFileManagerPtr(new memoryx::GridFileManager(prior->getCommonStorage()));
+ fileManager =
+ memoryx::GridFileManagerPtr(new memoryx::GridFileManager(prior->getCommonStorage()));
debugDrawerPrx = getTopic<armarx::DebugDrawerInterfacePrx>("DebugDrawerUpdates");
robot = RemoteRobot::createLocalCloneFromFile(rsc, VirtualRobot::RobotIO::eStructure);
-
}
-void SimpleGraspGenerator::onDisconnectComponent()
+void
+SimpleGraspGenerator::onDisconnectComponent()
{
}
-void SimpleGraspGenerator::onExitComponent()
+void
+SimpleGraspGenerator::onExitComponent()
{
}
-StringStringDictionary SimpleGraspGenerator::getTcpGcpMapping()
+StringStringDictionary
+SimpleGraspGenerator::getTcpGcpMapping()
{
StringStringDictionary mapping;
auto entries = Split(getProperty<std::string>("TCPtoGCPMapping"), ";", true, true);
@@ -81,17 +91,23 @@ StringStringDictionary SimpleGraspGenerator::getTcpGcpMapping()
return mapping;
}
-GeneratedGraspList SimpleGraspGenerator::generateGrasps(const std::string& objectInstanceEntityId, const Ice::Current& c)
+GeneratedGraspList
+SimpleGraspGenerator::generateGrasps(const std::string& objectInstanceEntityId,
+ const Ice::Current& c)
{
GeneratedGraspList result;
// int counter = 0;
- ObjectInstancePtr instance = ObjectInstancePtr::dynamicCast(objectInstances->getEntityById(objectInstanceEntityId));
+ ObjectInstancePtr instance =
+ ObjectInstancePtr::dynamicCast(objectInstances->getEntityById(objectInstanceEntityId));
ARMARX_CHECK_EXPRESSION(instance) << "no instance with id '" << objectInstanceEntityId << "'";
- ObjectClassPtr objectClass = ObjectClassPtr::dynamicCast(objectClasses->getEntityByName(instance->getMostProbableClass()));
- ARMARX_CHECK_EXPRESSION(objectClass) << "no object class with name '" << instance->getMostProbableClass() << "' found ";
- memoryx::EntityWrappers::SimoxObjectWrapperPtr simoxWrapper = objectClass->addWrapper(new memoryx::EntityWrappers::SimoxObjectWrapper(fileManager));
+ ObjectClassPtr objectClass = ObjectClassPtr::dynamicCast(
+ objectClasses->getEntityByName(instance->getMostProbableClass()));
+ ARMARX_CHECK_EXPRESSION(objectClass)
+ << "no object class with name '" << instance->getMostProbableClass() << "' found ";
+ memoryx::EntityWrappers::SimoxObjectWrapperPtr simoxWrapper =
+ objectClass->addWrapper(new memoryx::EntityWrappers::SimoxObjectWrapper(fileManager));
ARMARX_CHECK_EXPRESSION(simoxWrapper);
VirtualRobot::ManipulationObjectPtr mo = simoxWrapper->getManipulationObject();
ARMARX_CHECK_EXPRESSION(mo);
@@ -102,12 +118,15 @@ GeneratedGraspList SimpleGraspGenerator::generateGrasps(const std::string& objec
if (instance->hasLocalizationTimestamp())
{
- RemoteRobot::synchronizeLocalCloneToTimestamp(robot, rsc, instance->getLocalizationTimestamp().toMicroSeconds());
- ARMARX_INFO << "object has timestamp " << instance->getLocalizationTimestamp().toDateTime() << " and frame " << objectFramedPose->frame << " with robot at " << robot->getGlobalPose();
+ RemoteRobot::synchronizeLocalCloneToTimestamp(
+ robot, rsc, instance->getLocalizationTimestamp().toMicroSeconds());
+ ARMARX_INFO << "object has timestamp " << instance->getLocalizationTimestamp().toDateTime()
+ << " and frame " << objectFramedPose->frame << " with robot at "
+ << robot->getGlobalPose();
objectFramedPose->changeToGlobal(robot);
}
else
- // dangerous - robot might have moved!
+ // dangerous - robot might have moved!
{
objectFramedPose->changeToGlobal(rsc->getSynchronizedRobot());
}
@@ -127,9 +146,9 @@ GeneratedGraspList SimpleGraspGenerator::generateGrasps(const std::string& objec
// debugDrawerPrx->setSphereDebugLayerVisu("objectSphere", objectPos, objectColor, 50.0f);
auto tcpGcpMapping = getTcpGcpMapping();
- std::string graspNameInfix = getProperty<std::string>("GraspNameInfix");
+ std::string graspNameInfix = getProperty<std::string>("GraspNameInfix");
int countGrasps = 0;
- auto robotType = getProperty<std::string>("RobotType").getValue();
+ auto robotType = getProperty<std::string>("RobotType").getValue();
for (const VirtualRobot::GraspSetPtr& gs : mo->getAllGraspSets())
{
if (gs->getRobotType() != robotType)
@@ -141,30 +160,40 @@ GeneratedGraspList SimpleGraspGenerator::generateGrasps(const std::string& objec
ARMARX_INFO << "Found Grasp: " << g->getName() << " for eef: " << g->getEefName();
if (!Contains(g->getName(), graspNameInfix, true))
{
- ARMARX_INFO << "grasp name does not contain infix " << graspNameInfix << " - skipping it";
+ ARMARX_INFO << "grasp name does not contain infix " << graspNameInfix
+ << " - skipping it";
continue;
}
if (!robot->hasEndEffector(g->getEefName()))
{
- ARMARX_INFO << "Endeffector " << g->getEefName() << " does not exist in this robot - skipping grasps";
+ ARMARX_INFO << "Endeffector " << g->getEefName()
+ << " does not exist in this robot - skipping grasps";
continue;
}
auto globalTcpPose = g->getTcpPoseGlobal(globalObjectPose);
- FramedPosePtr framedTcpPose {new FramedPose(globalTcpPose, objectFramedPose->frame, "")};
+ FramedPosePtr framedTcpPose{new FramedPose(globalTcpPose, objectFramedPose->frame, "")};
// calculate prepose
- auto gcpName = tcpGcpMapping.at(robot->getEndEffector(g->getEefName())->getTcp()->getName());
+ auto gcpName =
+ tcpGcpMapping.at(robot->getEndEffector(g->getEefName())->getTcp()->getName());
auto gcpNode = robot->getRobotNode(gcpName);
ARMARX_CHECK_EXPRESSION(gcpNode) << gcpName;
Eigen::Vector3f preposeOffsetGCPFrame = Eigen::Vector3f::Zero();
preposeOffsetGCPFrame(2) = -getProperty<float>("PreposeOffset").getValue();
- Eigen::Vector3f preposeOffsetTCPFrame = robot->getEndEffector(g->getEefName())->getTcp()->transformTo(gcpNode, preposeOffsetGCPFrame);
+ Eigen::Vector3f preposeOffsetTCPFrame =
+ robot->getEndEffector(g->getEefName())
+ ->getTcp()
+ ->transformTo(gcpNode, preposeOffsetGCPFrame);
Eigen::Matrix4f preposeOffsetGlobal = globalTcpPose;
- preposeOffsetGlobal.block<3, 1>(0, 3) += globalTcpPose.block<3, 3>(0, 0) * preposeOffsetTCPFrame;
+ preposeOffsetGlobal.block<3, 1>(0, 3) +=
+ globalTcpPose.block<3, 3>(0, 0) * preposeOffsetTCPFrame;
ARMARX_INFO << " before up offset: " << preposeOffsetGlobal;
preposeOffsetGlobal(2, 3) += getProperty<float>("UpwardsOffset").getValue();
- ARMARX_INFO << VAROUT(preposeOffsetGCPFrame) << "\n" << VAROUT(preposeOffsetTCPFrame) << "\n" << VAROUT(preposeOffsetGlobal) << VAROUT(globalTcpPose);
- FramedPosePtr framedTcpPrepose = new FramedPose(preposeOffsetGlobal, armarx::GlobalFrame, "");
+ ARMARX_INFO << VAROUT(preposeOffsetGCPFrame) << "\n"
+ << VAROUT(preposeOffsetTCPFrame) << "\n"
+ << VAROUT(preposeOffsetGlobal) << VAROUT(globalTcpPose);
+ FramedPosePtr framedTcpPrepose =
+ new FramedPose(preposeOffsetGlobal, armarx::GlobalFrame, "");
// prepose->changeFrame(robot, gcpName);
result.push_back({1.f, g->getEefName(), g->getName(), framedTcpPose, framedTcpPrepose});
@@ -174,15 +203,19 @@ GeneratedGraspList SimpleGraspGenerator::generateGrasps(const std::string& objec
if (result.empty())
{
- ARMARX_WARNING << "No grasps defined for object class '" << instance->getMostProbableClass() << "'";
+ ARMARX_WARNING << "No grasps defined for object class '" << instance->getMostProbableClass()
+ << "'";
}
ARMARX_INFO << "Found " << result.size() << " grasps";
return result;
}
-GeneratedGraspList SimpleGraspGenerator::generateGraspsByObjectName(const std::string& objectName, const Ice::Current& c)
+GeneratedGraspList
+SimpleGraspGenerator::generateGraspsByObjectName(const std::string& objectName,
+ const Ice::Current& c)
{
- memoryx::ObjectInstanceBasePtr objectInstance = wm->getObjectInstancesSegment()->getObjectInstanceByName(objectName);
+ memoryx::ObjectInstanceBasePtr objectInstance =
+ wm->getObjectInstancesSegment()->getObjectInstanceByName(objectName);
std::string id = objectInstance->getId();
ARMARX_IMPORTANT << "objectName: " << objectName << "; instanceID: " << id;
return generateGrasps(id);
diff --git a/source/RobotComponents/components/SimpleGraspGenerator/SimpleGraspGenerator.h b/source/RobotComponents/components/SimpleGraspGenerator/SimpleGraspGenerator.h
index c0b8de90637d8aa40e8ff5b31c58c59aa345e037..09ec23a36731ec3deb19e135d60fadac863fa3f8 100644
--- a/source/RobotComponents/components/SimpleGraspGenerator/SimpleGraspGenerator.h
+++ b/source/RobotComponents/components/SimpleGraspGenerator/SimpleGraspGenerator.h
@@ -24,15 +24,17 @@
#pragma once
+#include <Eigen/Geometry>
+
#include <ArmarXCore/core/Component.h>
-#include <RobotComponents/interface/components/GraspingManager/GraspGeneratorInterface.h>
-#include <MemoryX/interface/memorytypes/MemorySegments.h>
-#include <MemoryX/interface/components/WorkingMemoryInterface.h>
-#include <MemoryX/core/GridFileManager.h>
#include <RobotAPI/components/DebugDrawer/DebugDrawerComponent.h>
-#include <Eigen/Geometry>
+#include <RobotComponents/interface/components/GraspingManager/GraspGeneratorInterface.h>
+
+#include <MemoryX/core/GridFileManager.h>
+#include <MemoryX/interface/components/WorkingMemoryInterface.h>
+#include <MemoryX/interface/memorytypes/MemorySegments.h>
namespace armarx
{
@@ -40,21 +42,38 @@ namespace armarx
* @class SimpleGraspGeneratorPropertyDefinitions
* @brief
*/
- class SimpleGraspGeneratorPropertyDefinitions:
- public ComponentPropertyDefinitions
+ class SimpleGraspGeneratorPropertyDefinitions : public ComponentPropertyDefinitions
{
public:
- SimpleGraspGeneratorPropertyDefinitions(std::string prefix):
+ SimpleGraspGeneratorPropertyDefinitions(std::string prefix) :
ComponentPropertyDefinitions(prefix)
{
- defineOptionalProperty<std::string>("GraspNameInfix", "Grasp", "All grasp candidates without this infix are filtered out. Case-insensitive.");
- defineOptionalProperty<float>("VisualizationSlowdownFactor", 1.0f, "1.0 is a good value for clear visualization, 0 the visualization should not slow down the process", PropertyDefinitionBase::eModifiable);
- defineOptionalProperty<bool>("EnableVisualization", true, "If false no visualization is done.", PropertyDefinitionBase::eModifiable);
- defineOptionalProperty<float>("PreposeOffset", 70.0f, "Offset of proposed grasp along negative z axis", PropertyDefinitionBase::eModifiable);
- defineOptionalProperty<float>("UpwardsOffset", 50.0f, "Offset of proposed grasp in upwards direction", PropertyDefinitionBase::eModifiable);
- defineRequiredProperty<std::string>("TCPtoGCPMapping", "Format: TCP L:GCP L;TCP R:GCP R", PropertyDefinitionBase::eModifiable);
- defineOptionalProperty<std::string>("RobotType", "Armar3", "Type of robot for which the grasps should be generated");
-
+ defineOptionalProperty<std::string>(
+ "GraspNameInfix",
+ "Grasp",
+ "All grasp candidates without this infix are filtered out. Case-insensitive.");
+ defineOptionalProperty<float>("VisualizationSlowdownFactor",
+ 1.0f,
+ "1.0 is a good value for clear visualization, 0 the "
+ "visualization should not slow down the process",
+ PropertyDefinitionBase::eModifiable);
+ defineOptionalProperty<bool>("EnableVisualization",
+ true,
+ "If false no visualization is done.",
+ PropertyDefinitionBase::eModifiable);
+ defineOptionalProperty<float>("PreposeOffset",
+ 70.0f,
+ "Offset of proposed grasp along negative z axis",
+ PropertyDefinitionBase::eModifiable);
+ defineOptionalProperty<float>("UpwardsOffset",
+ 50.0f,
+ "Offset of proposed grasp in upwards direction",
+ PropertyDefinitionBase::eModifiable);
+ defineRequiredProperty<std::string>("TCPtoGCPMapping",
+ "Format: TCP L:GCP L;TCP R:GCP R",
+ PropertyDefinitionBase::eModifiable);
+ defineOptionalProperty<std::string>(
+ "RobotType", "Armar3", "Type of robot for which the grasps should be generated");
}
};
@@ -71,15 +90,14 @@ namespace armarx
* @ingroup Component-SimpleGraspGenerator
* @brief The SimpleGraspGenerator class
*/
- class SimpleGraspGenerator :
- virtual public Component,
- virtual public GraspGeneratorInterface
+ class SimpleGraspGenerator : virtual public Component, virtual public GraspGeneratorInterface
{
public:
/**
* @see armarx::ManagedIceObject::getDefaultName()
*/
- std::string getDefaultName() const override
+ std::string
+ getDefaultName() const override
{
return "SimpleGraspGenerator";
}
@@ -93,8 +111,11 @@ namespace armarx
*
* @return List of generatedGrasps
*/
- GeneratedGraspList generateGrasps(const std::string& objectInstanceEntityId, const Ice::Current& c = Ice::emptyCurrent) override;
- GeneratedGraspList generateGraspsByObjectName(const std::string& objectName, const Ice::Current& c = Ice::emptyCurrent) override;
+ GeneratedGraspList generateGrasps(const std::string& objectInstanceEntityId,
+ const Ice::Current& c = Ice::emptyCurrent) override;
+ GeneratedGraspList
+ generateGraspsByObjectName(const std::string& objectName,
+ const Ice::Current& c = Ice::emptyCurrent) override;
protected:
/**
@@ -120,13 +141,14 @@ namespace armarx
/**
* @see PropertyUser::createPropertyDefinitions()
*/
- PropertyDefinitionsPtr createPropertyDefinitions() override
+ PropertyDefinitionsPtr
+ createPropertyDefinitions() override
{
- return PropertyDefinitionsPtr(new SimpleGraspGeneratorPropertyDefinitions(getConfigIdentifier()));
+ return PropertyDefinitionsPtr(
+ new SimpleGraspGeneratorPropertyDefinitions(getConfigIdentifier()));
}
private:
-
/**
* @brief Wrapper for the getter of the property TCPtoGCPMapping
*
@@ -147,5 +169,4 @@ namespace armarx
armarx::DebugDrawerInterfacePrx debugDrawerPrx;
};
-}
-
+} // namespace armarx
diff --git a/source/RobotComponents/components/SimpleRobotPlacement/SimpleRobotPlacement.cpp b/source/RobotComponents/components/SimpleRobotPlacement/SimpleRobotPlacement.cpp
index d6241e0522e14b3c95b334805585bbdc15d82a18..28a6d556b6b53612bde660aed005586a92da078f 100644
--- a/source/RobotComponents/components/SimpleRobotPlacement/SimpleRobotPlacement.cpp
+++ b/source/RobotComponents/components/SimpleRobotPlacement/SimpleRobotPlacement.cpp
@@ -24,74 +24,114 @@
*/
#include "SimpleRobotPlacement.h"
-#include <MemoryX/libraries/memorytypes/entity/ObjectInstance.h>
-#include <MemoryX/libraries/memorytypes/MemoryXTypesObjectFactories.h>
-#include <MemoryX/core/MemoryXCoreObjectFactories.h>
-#include <RobotAPI/libraries/core/FramedPose.h>
-#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
+
+#include <VirtualRobot/Grasping/Grasp.h>
#include <VirtualRobot/Grasping/GraspSet.h>
-#include <MemoryX/libraries/helpers/VirtualRobotHelpers/SimoxObjectWrapper.h>
+#include <VirtualRobot/ManipulationObject.h>
+#include <VirtualRobot/MathTools.h>
+#include <VirtualRobot/Workspace/Manipulability.h>
+
+#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
#include <ArmarXCore/core/util/algorithm.h>
-#include <ArmarXCore/core/system/ArmarXDataPath.h>
-#include <VirtualRobot/Grasping/Grasp.h>
-#include <VirtualRobot/Workspace/Manipulability.h>
-#include <VirtualRobot/ManipulationObject.h>
+#include <RobotAPI/libraries/core/FramedPose.h>
+#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
+
+#include <MemoryX/core/MemoryXCoreObjectFactories.h>
+#include <MemoryX/libraries/helpers/VirtualRobotHelpers/SimoxObjectWrapper.h>
+#include <MemoryX/libraries/memorytypes/MemoryXTypesObjectFactories.h>
#include <MemoryX/libraries/memorytypes/entity/AgentInstance.h>
+#include <MemoryX/libraries/memorytypes/entity/ObjectInstance.h>
//#include <RobotComponents/components/MotionPlanning/CSpace/ScaledCSpace.h>
#include <RobotComponents/components/MotionPlanning/CSpace/SimoxCSpace.h>
#include <RobotComponents/components/MotionPlanning/CSpace/VoxelGridCSpace.h>
//#include <RobotComponents/components/MotionPlanning/Tasks/RRTConnect/Task.h>
+#include <math.h>
+
#include <IceUtil/UUID.h>
-#include <RobotComponents/interface/components/MotionPlanning/MotionPlanningServer.h>
+
+#include <SimoxUtility/algorithm/string/string_tools.h>
#include <VirtualRobot/RobotConfig.h>
-#include <VisionX/interface/components/VoxelGridProviderInterface.h>
#include <VirtualRobot/math/Helpers.h>
-#include <SimoxUtility/algorithm/string/string_tools.h>
+#include <VisionX/interface/components/VoxelGridProviderInterface.h>
-#include <math.h>
+#include <RobotComponents/interface/components/MotionPlanning/MotionPlanningServer.h>
using namespace armarx;
using namespace memoryx;
using namespace VirtualRobot;
-static const DrawColor COLOR_ROBOT
-{
- 1.0f, 1.0f, 0.5f, 1.0f
-};
+static const DrawColor COLOR_ROBOT{1.0f, 1.0f, 0.5f, 1.0f};
-SimpleRobotPlacementPropertyDefinitions::SimpleRobotPlacementPropertyDefinitions(std::string prefix):
+SimpleRobotPlacementPropertyDefinitions::SimpleRobotPlacementPropertyDefinitions(
+ std::string prefix) :
ComponentPropertyDefinitions(prefix)
{
defineOptionalProperty<std::string>("RobotName", "Armar3", "Name of the robot to use");
- defineOptionalProperty<std::string>("RobotFilePath", "RobotAPI/robots/Armar3/ArmarIII.xml", "File path of the robot to use");
- defineOptionalProperty<std::string>("CollisionModel", "PlatformTorsoHeadColModel", "Collisionmodel of the robot");
- defineOptionalProperty<std::string>("WorkspaceFilePaths",
- "Armar3/reachability/reachability_right_hand_smoothened.bin;Armar3/reachability/reachability_left_hand_smoothened.bin",
- "Paths to manipulability and reachability files (';' delimited)");
+ defineOptionalProperty<std::string>(
+ "RobotFilePath", "RobotAPI/robots/Armar3/ArmarIII.xml", "File path of the robot to use");
+ defineOptionalProperty<std::string>(
+ "CollisionModel", "PlatformTorsoHeadColModel", "Collisionmodel of the robot");
+ defineOptionalProperty<std::string>(
+ "WorkspaceFilePaths",
+ "Armar3/reachability/reachability_right_hand_smoothened.bin;Armar3/reachability/"
+ "reachability_left_hand_smoothened.bin",
+ "Paths to manipulability and reachability files (';' delimited)");
// defineOptionalProperty<bool>("VisualizeCollisionSpace",
// false,
// "If true adds cspace task to MotionPlanning Server");
defineOptionalProperty<float>("MinimumDistanceToEnvironment",
0.0f,
- "Minimum distance to the environment for all robot placements. Much faster if set to zero.");
- defineOptionalProperty<float>("VisualizationSlowdownFactor", 1.0f, "1.0 is a good value for clear visualization, 0 the visualization should not slow down the process", PropertyDefinitionBase::eModifiable);
- defineOptionalProperty<bool>("EnableVisualization", true, "If false no visualization is done.", PropertyDefinitionBase::eModifiable);
- defineOptionalProperty<int>("PlacmentsPerGrasp", 3, "Number of robot placement that will be generated for each grasp", PropertyDefinitionBase::eModifiable);
- defineOptionalProperty<float>("MinManipulabilityDecreaseFactor", 0.9f, "Min initial manipulability in relation to max manipulabity value and factor by which this threshold is decreased each trial", PropertyDefinitionBase::eModifiable);
-
- defineOptionalProperty<bool>("UseVoxelGridCSpace", false, "If true, the VoxelGridCSpace is used instead of the SimoxCSpace.", PropertyDefinitionBase::eModifiable);
- defineOptionalProperty<std::string>("VoxelGridProviderName", "VoxelGridProvider", "Name of the Voxel Grid Provider", PropertyDefinitionBase::eModifiable);
-
- defineOptionalProperty<std::string>("EefNamePreferenceFilter", "R", "Prefer grasps where eef name contains this name by setting grasp success probability to 1. Set to empty string to disable.");
- defineOptionalProperty<float>("MinPlacementDistance", 400, "Minimum Distance for a Placement", PropertyDefinitionBase::eModifiable);
- defineOptionalProperty<float>("MaxPlacementDistance", 1000, "Maximum Distance for a Placement", PropertyDefinitionBase::eModifiable);
-
+ "Minimum distance to the environment for all robot placements. "
+ "Much faster if set to zero.");
+ defineOptionalProperty<float>("VisualizationSlowdownFactor",
+ 1.0f,
+ "1.0 is a good value for clear visualization, 0 the "
+ "visualization should not slow down the process",
+ PropertyDefinitionBase::eModifiable);
+ defineOptionalProperty<bool>("EnableVisualization",
+ true,
+ "If false no visualization is done.",
+ PropertyDefinitionBase::eModifiable);
+ defineOptionalProperty<int>("PlacmentsPerGrasp",
+ 3,
+ "Number of robot placement that will be generated for each grasp",
+ PropertyDefinitionBase::eModifiable);
+ defineOptionalProperty<float>(
+ "MinManipulabilityDecreaseFactor",
+ 0.9f,
+ "Min initial manipulability in relation to max manipulabity value and factor by which this "
+ "threshold is decreased each trial",
+ PropertyDefinitionBase::eModifiable);
+
+ defineOptionalProperty<bool>("UseVoxelGridCSpace",
+ false,
+ "If true, the VoxelGridCSpace is used instead of the SimoxCSpace.",
+ PropertyDefinitionBase::eModifiable);
+ defineOptionalProperty<std::string>("VoxelGridProviderName",
+ "VoxelGridProvider",
+ "Name of the Voxel Grid Provider",
+ PropertyDefinitionBase::eModifiable);
+
+ defineOptionalProperty<std::string>(
+ "EefNamePreferenceFilter",
+ "R",
+ "Prefer grasps where eef name contains this name by setting grasp success probability to "
+ "1. Set to empty string to disable.");
+ defineOptionalProperty<float>("MinPlacementDistance",
+ 400,
+ "Minimum Distance for a Placement",
+ PropertyDefinitionBase::eModifiable);
+ defineOptionalProperty<float>("MaxPlacementDistance",
+ 1000,
+ "Maximum Distance for a Placement",
+ PropertyDefinitionBase::eModifiable);
}
-void SimpleRobotPlacement::onInitComponent()
+void
+SimpleRobotPlacement::onInitComponent()
{
drawRobotId = "local_robot_";
visuLayerName = "SimpleRobotPlacement";
@@ -104,12 +144,15 @@ void SimpleRobotPlacement::onInitComponent()
ARMARX_ERROR << "Could not find robot file: " << robotFilePath;
}
- workspaceFilePaths = armarx::Split(getProperty<std::string>("WorkspaceFilePaths").getValue(), ";");
+ workspaceFilePaths =
+ armarx::Split(getProperty<std::string>("WorkspaceFilePaths").getValue(), ";");
// retrieve absolute workspace file paths
for (std::string& path : workspaceFilePaths)
{
- std::string packageName = std::filesystem::path {path} .begin()->string();
- ARMARX_CHECK_EXPRESSION(!packageName.empty()) << "Workspace file path '" << path << "' could not be parsed correctly, because package name is empty";
+ std::string packageName = std::filesystem::path{path}.begin()->string();
+ ARMARX_CHECK_EXPRESSION(!packageName.empty())
+ << "Workspace file path '" << path
+ << "' could not be parsed correctly, because package name is empty";
armarx::CMakePackageFinder project(packageName);
path = project.getDataDir() + "/" + path;
if (!std::filesystem::exists(path))
@@ -128,7 +171,8 @@ void SimpleRobotPlacement::onInitComponent()
offeringTopic("DebugDrawerUpdates");
}
-void SimpleRobotPlacement::onConnectComponent()
+void
+SimpleRobotPlacement::onConnectComponent()
{
srand(IceUtil::Time::now().toSeconds());
getProxy(wm, "WorkingMemory");
@@ -141,31 +185,38 @@ void SimpleRobotPlacement::onConnectComponent()
agentInstances = wm->getAgentInstancesSegment();
objectClasses = prior->getObjectClassesSegment();
- fileManager = memoryx::GridFileManagerPtr(new memoryx::GridFileManager(prior->getCommonStorage()));
+ fileManager =
+ memoryx::GridFileManagerPtr(new memoryx::GridFileManager(prior->getCommonStorage()));
debugDrawerPrx = getTopic<armarx::DebugDrawerInterfacePrx>("DebugDrawerUpdates");
entityDrawerPrx = getTopic<memoryx::EntityDrawerInterfacePrx>("DebugDrawerUpdates");
- cacheCSpace = SimoxCSpace::PrefetchWorkingMemoryObjects(wm, prior->getCommonStorage(), robotStateComponentPrx);
+ cacheCSpace = SimoxCSpace::PrefetchWorkingMemoryObjects(
+ wm, prior->getCommonStorage(), robotStateComponentPrx);
loadRobot();
loadWorkspaces();
}
-void SimpleRobotPlacement::onDisconnectComponent()
+void
+SimpleRobotPlacement::onDisconnectComponent()
{
}
-void SimpleRobotPlacement::onExitComponent()
+void
+SimpleRobotPlacement::onExitComponent()
{
}
-GraspingPlacementList SimpleRobotPlacement::generateRobotPlacements(const GeneratedGraspList& grasps, const Ice::Current&)
+GraspingPlacementList
+SimpleRobotPlacement::generateRobotPlacements(const GeneratedGraspList& grasps, const Ice::Current&)
{
// TIMING_START(RobotPlacement);
planningTasks.clear();
GraspingPlacementList result;
- AgentInstancePtr agent = AgentInstancePtr::dynamicCast(agentInstances->getAgentInstanceByName(robotName));
- std::shared_ptr<RemoteRobot> remoteRobot(new RemoteRobot(robotStateComponentPrx->getSynchronizedRobot()));
+ AgentInstancePtr agent =
+ AgentInstancePtr::dynamicCast(agentInstances->getAgentInstanceByName(robotName));
+ std::shared_ptr<RemoteRobot> remoteRobot(
+ new RemoteRobot(robotStateComponentPrx->getSynchronizedRobot()));
robot->setGlobalPose(remoteRobot->getGlobalPose());
// first, we get rid of all generated grasps, whose tcp is not given in any of the preloaded workspaces
@@ -177,7 +228,10 @@ GraspingPlacementList SimpleRobotPlacement::generateRobotPlacements(const Genera
RemoteRobot::synchronizeLocalClone(robot, robotStateComponentPrx);
if (getProperty<bool>("UseVoxelGridCSpace").getValue())
{
- cspace = new VoxelGridCSpace(getProxy<visionx::VoxelGridProviderInterfacePrx>(getProperty<std::string>("VoxelGridProviderName").getValue()), prior->getCommonStorage());
+ cspace =
+ new VoxelGridCSpace(getProxy<visionx::VoxelGridProviderInterfacePrx>(
+ getProperty<std::string>("VoxelGridProviderName").getValue()),
+ prior->getCommonStorage());
}
else
{
@@ -192,11 +246,13 @@ GraspingPlacementList SimpleRobotPlacement::generateRobotPlacements(const Genera
agentData.collisionSetNames = {colModel};
agentData.initialJointValues = robot->getConfig()->getRobotNodeJointValueMap();
cspace->setAgent(agentData);
- cspace->setStationaryObjectMargin(getProperty<float>("MinimumDistanceToEnvironment").getValue());
+ cspace->setStationaryObjectMargin(
+ getProperty<float>("MinimumDistanceToEnvironment").getValue());
cspace->initCollisionTest();
int placementsPerGrasp = getProperty<int>("PlacmentsPerGrasp");
- ARMARX_VERBOSE << "Searching " << placementsPerGrasp << " poses for " << filteredGrasps.size() << " grasps";
+ ARMARX_VERBOSE << "Searching " << placementsPerGrasp << " poses for " << filteredGrasps.size()
+ << " grasps";
for (const auto& g : filteredGrasps)
{
Eigen::Matrix4f graspPose = FramedPosePtr::dynamicCast(g.framedPose)->toGlobalEigen(robot);
@@ -237,14 +293,15 @@ GraspingPlacementList SimpleRobotPlacement::generateRobotPlacements(const Genera
float z = newPoseEigen(2, 3);
newPoseEigen.block<3, 1>(0, 3) = Eigen::Vector3f::Zero();
Eigen::Affine3f transform;
- transform = Eigen::Translation<float, 3>(x, y, z) * Eigen::AngleAxisf(platformRotation, Eigen::Vector3f(0, 0, 1));
+ transform = Eigen::Translation<float, 3>(x, y, z) *
+ Eigen::AngleAxisf(platformRotation, Eigen::Vector3f(0, 0, 1));
newPoseEigen = transform * newPoseEigen;
// drawRobot(newPoseEigen);
armarx::FramedPosePtr resultPose(new FramedPose(newPoseEigen, GlobalFrame, ""));
ARMARX_INFO << "Inserting robot placement: " << resultPose->output();
- placementCandidates[score] = GraspingPlacement {g, resultPose, 0};
+ placementCandidates[score] = GraspingPlacement{g, resultPose, 0};
}
if (!placementCandidates.empty())
{
@@ -260,13 +317,15 @@ GraspingPlacementList SimpleRobotPlacement::generateRobotPlacements(const Genera
std::string EefNamePreferenceFilter = getProperty<std::string>("EefNamePreferenceFilter");
for (GraspingPlacement& gp : result)
{
- Eigen::Matrix4f mat = inverseRobotPose * FramedPosePtr::dynamicCast(gp.robotPose)->toEigen();
+ Eigen::Matrix4f mat =
+ inverseRobotPose * FramedPosePtr::dynamicCast(gp.robotPose)->toEigen();
Eigen::AngleAxisf aa(mat.block<3, 3>(0, 0));
float distanceLhs = mat.block<3, 1>(0, 3).norm() * aa.angle() * 50;
// secondhands demo...
- float handPreferenceScore = Contains(gp.grasp.eefName, EefNamePreferenceFilter, true) ? 0.f : 1.f;
- orderedMap [ {handPreferenceScore, distanceLhs}] = gp;
+ float handPreferenceScore =
+ Contains(gp.grasp.eefName, EefNamePreferenceFilter, true) ? 0.f : 1.f;
+ orderedMap[{handPreferenceScore, distanceLhs}] = gp;
}
// TIMING_END(RobotPlacement);
@@ -280,23 +339,28 @@ struct PlacementInfo
Eigen::Matrix4f graspPose;
std::vector<SimpleRobotPlacement::RobotPlacement> placements;
VirtualRobot::WorkspaceGridPtr reachGrid;
- void sortplacements()
+
+ void
+ sortplacements()
{
- std::sort(placements.begin(), placements.end(), [](auto & a, auto & b)
- {
- return a.score > b.score;
- });
+ std::sort(placements.begin(),
+ placements.end(),
+ [](auto& a, auto& b) { return a.score > b.score; });
}
+
std::pair<float, float> sortingInfo{0, 0};
armarx::FramedPosePtr resultPose;
};
-GraspingPlacementList SimpleRobotPlacement::generateRobotPlacementsEx(const GeneratedGraspList& grasps, const Ice::Current&)
+GraspingPlacementList
+SimpleRobotPlacement::generateRobotPlacementsEx(const GeneratedGraspList& grasps,
+ const Ice::Current&)
{
// TIMING_START(RobotPlacement);
planningTasks.clear();
GraspingPlacementList result;
- AgentInstancePtr agent = AgentInstancePtr::dynamicCast(agentInstances->getAgentInstanceByName(robotName));
+ AgentInstancePtr agent =
+ AgentInstancePtr::dynamicCast(agentInstances->getAgentInstanceByName(robotName));
RemoteRobotPtr remoteRobot(new RemoteRobot(robotStateComponentPrx->getSynchronizedRobot()));
robot->setGlobalPose(remoteRobot->getGlobalPose());
@@ -309,7 +373,10 @@ GraspingPlacementList SimpleRobotPlacement::generateRobotPlacementsEx(const Gene
RemoteRobot::synchronizeLocalClone(robot, robotStateComponentPrx);
if (getProperty<bool>("UseVoxelGridCSpace").getValue())
{
- cspace = new VoxelGridCSpace(getProxy<visionx::VoxelGridProviderInterfacePrx>(getProperty<std::string>("VoxelGridProviderName").getValue()), prior->getCommonStorage());
+ cspace =
+ new VoxelGridCSpace(getProxy<visionx::VoxelGridProviderInterfacePrx>(
+ getProperty<std::string>("VoxelGridProviderName").getValue()),
+ prior->getCommonStorage());
}
else
{
@@ -324,11 +391,13 @@ GraspingPlacementList SimpleRobotPlacement::generateRobotPlacementsEx(const Gene
agentData.collisionSetNames = {colModel};
agentData.initialJointValues = robot->getConfig()->getRobotNodeJointValueMap();
cspace->setAgent(agentData);
- cspace->setStationaryObjectMargin(getProperty<float>("MinimumDistanceToEnvironment").getValue());
+ cspace->setStationaryObjectMargin(
+ getProperty<float>("MinimumDistanceToEnvironment").getValue());
cspace->initCollisionTest();
int placementsPerGrasp = getProperty<int>("PlacmentsPerGrasp");
- ARMARX_VERBOSE << "Searching " << placementsPerGrasp << " poses for " << filteredGrasps.size() << " grasps";
+ ARMARX_VERBOSE << "Searching " << placementsPerGrasp << " poses for " << filteredGrasps.size()
+ << " grasps";
std::vector<PlacementInfo> pis;
for (const auto& g : filteredGrasps)
@@ -346,7 +415,8 @@ GraspingPlacementList SimpleRobotPlacement::generateRobotPlacementsEx(const Gene
{
for (PlacementInfo& pi : pis)
{
- std::vector<SimpleRobotPlacement::RobotPlacement> placements = getSuitablePositions(pi.grasp, pi.reachGrid, pi.graspPose, 0.1, 10, 100);
+ std::vector<SimpleRobotPlacement::RobotPlacement> placements =
+ getSuitablePositions(pi.grasp, pi.reachGrid, pi.graspPose, 0.1, 10, 100);
pi.placements.insert(pi.placements.end(), placements.begin(), placements.end());
}
//requiredManipulabilityFraction *= minManipulabilityDecreaseFactor;
@@ -366,16 +436,17 @@ GraspingPlacementList SimpleRobotPlacement::generateRobotPlacementsEx(const Gene
ARMARX_IMPORTANT << VAROUT(rp.score);
}
RobotPlacement rp = pi.placements.at(0);
- Eigen::Matrix4f robotpose = ::math::Helpers::CreatePose(Eigen::Vector3f(rp.x, rp.y, 0), Eigen::AngleAxisf(rp.z, Eigen::Vector3f::UnitZ()).toRotationMatrix());
+ Eigen::Matrix4f robotpose = ::math::Helpers::CreatePose(
+ Eigen::Vector3f(rp.x, rp.y, 0),
+ Eigen::AngleAxisf(rp.z, Eigen::Vector3f::UnitZ()).toRotationMatrix());
pi.resultPose = new FramedPose(robotpose, GlobalFrame, "");
- pi.sortingInfo.first = Contains(pi.grasp.eefName, EefNamePreferenceFilter, true) ? 0.f : 1.f;
+ pi.sortingInfo.first =
+ Contains(pi.grasp.eefName, EefNamePreferenceFilter, true) ? 0.f : 1.f;
pi.sortingInfo.second = -rp.score;
}
entityDrawerPrx->removeLayer(visuLayerName);
- std::sort(pis.begin(), pis.end(), [](auto & a, auto & b)
- {
- return a.sortingInfo < b.sortingInfo;
- });
+ std::sort(
+ pis.begin(), pis.end(), [](auto& a, auto& b) { return a.sortingInfo < b.sortingInfo; });
for (PlacementInfo& pi : pis)
{
if (pi.placements.size() > 0)
@@ -389,10 +460,14 @@ GraspingPlacementList SimpleRobotPlacement::generateRobotPlacementsEx(const Gene
}
}
return result;
-
}
-GraspingPlacementList SimpleRobotPlacement::generateRobotPlacementsForGraspPose(const std::string& endEffectorName, const FramedPoseBasePtr& target, const PlanarObstacleList& planarObstacles, const ConvexHull& placementArea, const Ice::Current& c)
+GraspingPlacementList
+SimpleRobotPlacement::generateRobotPlacementsForGraspPose(const std::string& endEffectorName,
+ const FramedPoseBasePtr& target,
+ const PlanarObstacleList& planarObstacles,
+ const ConvexHull& placementArea,
+ const Ice::Current& c)
{
planningTasks.clear();
GraspingPlacementList result;
@@ -406,7 +481,8 @@ GraspingPlacementList SimpleRobotPlacement::generateRobotPlacementsForGraspPose(
FramedPosePtr target_pose = FramedPosePtr::dynamicCast(target);
target_pose->changeToGlobal(robotStateComponentPrx->getSynchronizedRobot());
- std::shared_ptr<RemoteRobot> remoteRobot(new RemoteRobot(robotStateComponentPrx->getSynchronizedRobot()));
+ std::shared_ptr<RemoteRobot> remoteRobot(
+ new RemoteRobot(robotStateComponentPrx->getSynchronizedRobot()));
robot->setGlobalPose(remoteRobot->getGlobalPose());
RemoteRobot::synchronizeLocalClone(robot, robotStateComponentPrx);
@@ -423,7 +499,10 @@ GraspingPlacementList SimpleRobotPlacement::generateRobotPlacementsForGraspPose(
// Initialize Simox collision space
if (getProperty<bool>("UseVoxelGridCSpace").getValue())
{
- cspace = new VoxelGridCSpace(getProxy<visionx::VoxelGridProviderInterfacePrx>(getProperty<std::string>("VoxelGridProviderName").getValue()), prior->getCommonStorage());
+ cspace =
+ new VoxelGridCSpace(getProxy<visionx::VoxelGridProviderInterfacePrx>(
+ getProperty<std::string>("VoxelGridProviderName").getValue()),
+ prior->getCommonStorage());
}
else
{
@@ -431,7 +510,8 @@ GraspingPlacementList SimpleRobotPlacement::generateRobotPlacementsForGraspPose(
cspace->addObjectsFromWorkingMemory(wm);
}
cspace->setAgent(agentData);
- cspace->setStationaryObjectMargin(getProperty<float>("MinimumDistanceToEnvironment").getValue());
+ cspace->setStationaryObjectMargin(
+ getProperty<float>("MinimumDistanceToEnvironment").getValue());
for (auto& obstacle : planarObstacles)
@@ -440,7 +520,8 @@ GraspingPlacementList SimpleRobotPlacement::generateRobotPlacementsForGraspPose(
for (auto& p : obstacle)
{
- plane.push_back(FramedPositionPtr::dynamicCast(p)->toGlobalEigen(robotStateComponentPrx->getSynchronizedRobot()));
+ plane.push_back(FramedPositionPtr::dynamicCast(p)->toGlobalEigen(
+ robotStateComponentPrx->getSynchronizedRobot()));
}
cspace->addPlanarObject(plane);
@@ -454,7 +535,8 @@ GraspingPlacementList SimpleRobotPlacement::generateRobotPlacementsForGraspPose(
g.framedPose = target_pose;
int placmentsPerGrasp = getProperty<int>("PlacmentsPerGrasp");
- ARMARX_INFO << "Searching " << placmentsPerGrasp << " poses for EEF pose " << target_pose->toEigen();
+ ARMARX_INFO << "Searching " << placmentsPerGrasp << " poses for EEF pose "
+ << target_pose->toEigen();
// Construct placement area as convex hull
VirtualRobot::MathTools::ConvexHull2DPtr placementArea_ch;
@@ -482,7 +564,14 @@ GraspingPlacementList SimpleRobotPlacement::generateRobotPlacementsForGraspPose(
float xGoal, yGoal, platformRotation;
int score;
- getSuitablePosition(g, grid, target_pose->toEigen(), xGoal, yGoal, platformRotation, score, placementArea_ch);
+ getSuitablePosition(g,
+ grid,
+ target_pose->toEigen(),
+ xGoal,
+ yGoal,
+ platformRotation,
+ score,
+ placementArea_ch);
if (xGoal == 0 && yGoal == 0 && platformRotation == 0)
{
@@ -501,18 +590,20 @@ GraspingPlacementList SimpleRobotPlacement::generateRobotPlacementsForGraspPose(
float z = newPoseEigen(2, 3);
newPoseEigen.block<3, 1>(0, 3) = Eigen::Vector3f::Zero();
Eigen::Affine3f transform;
- transform = Eigen::Translation<float, 3>(x, y, z) * Eigen::AngleAxisf(platformRotation, Eigen::Vector3f(0, 0, 1));
+ transform = Eigen::Translation<float, 3>(x, y, z) *
+ Eigen::AngleAxisf(platformRotation, Eigen::Vector3f(0, 0, 1));
newPoseEigen = transform * newPoseEigen;
armarx::FramedPosePtr resultPose(new FramedPose(newPoseEigen, GlobalFrame, ""));
- result.push_back(GraspingPlacement {g, resultPose, score});
+ result.push_back(GraspingPlacement{g, resultPose, score});
}
entityDrawerPrx->removeLayer(visuLayerName);
return result;
}
-VirtualRobot::WorkspaceRepresentationPtr SimpleRobotPlacement::getWorkspaceRepresentation(GeneratedGrasp const& g)
+VirtualRobot::WorkspaceRepresentationPtr
+SimpleRobotPlacement::getWorkspaceRepresentation(GeneratedGrasp const& g)
{
for (VirtualRobot::WorkspaceRepresentationPtr workspace : workspaces)
@@ -526,7 +617,8 @@ VirtualRobot::WorkspaceRepresentationPtr SimpleRobotPlacement::getWorkspaceRepre
return VirtualRobot::WorkspaceRepresentationPtr();
}
-VirtualRobot::WorkspaceGridPtr SimpleRobotPlacement::createWorkspaceGrid(GeneratedGrasp g, const Eigen::Matrix4f& globalObjectPose)
+VirtualRobot::WorkspaceGridPtr
+SimpleRobotPlacement::createWorkspaceGrid(GeneratedGrasp g, const Eigen::Matrix4f& globalObjectPose)
{
static int counter = 0;
std::string graspName = "some_random_grasp_" + to_string(counter++);
@@ -539,10 +631,12 @@ VirtualRobot::WorkspaceGridPtr SimpleRobotPlacement::createWorkspaceGrid(Generat
// dummy grasp
Eigen::Matrix4f tcpPoseGlobal = FramedPosePtr::dynamicCast(g.framedPose)->toGlobalEigen(robot);
- Eigen::Matrix4f tcpPrePoseGlobal = FramedPosePtr::dynamicCast(g.framedPrePose)->toGlobalEigen(robot);
+ Eigen::Matrix4f tcpPrePoseGlobal =
+ FramedPosePtr::dynamicCast(g.framedPrePose)->toGlobalEigen(robot);
Eigen::Matrix4f objectPoseInTcpFrame = tcpPoseGlobal.inverse() * globalObjectPose;
VirtualRobot::GraspPtr dummyGrasp(new Grasp(graspName, robotType, eef, objectPoseInTcpFrame));
- VirtualRobot::GraspPtr dummyPrepose(new Grasp(graspName, robotType, eef, tcpPrePoseGlobal.inverse() * globalObjectPose));
+ VirtualRobot::GraspPtr dummyPrepose(
+ new Grasp(graspName, robotType, eef, tcpPrePoseGlobal.inverse() * globalObjectPose));
VirtualRobot::WorkspaceRepresentationPtr ws;
// find a workspace whose tcp is equal to the tcp of the generated grasp
@@ -552,13 +646,15 @@ VirtualRobot::WorkspaceGridPtr SimpleRobotPlacement::createWorkspaceGrid(Generat
Eigen::Vector3f minBB, maxBB;
ws->getWorkspaceExtends(minBB, maxBB);
VirtualRobot::WorkspaceGridPtr reachGridPrepose;
- reachGridPrepose.reset(new WorkspaceGrid(minBB(0), maxBB(0), minBB(1), maxBB(1), ws->getDiscretizeParameterTranslation()));
+ reachGridPrepose.reset(new WorkspaceGrid(
+ minBB(0), maxBB(0), minBB(1), maxBB(1), ws->getDiscretizeParameterTranslation()));
reachGridPrepose->setGridPosition(globalObjectPose(0, 3), globalObjectPose(1, 3));
reachGridPrepose->fillGridData(ws, dummyObject, dummyPrepose, robot->getRootNode());
// reachGrid->fillGridData(ws, dummyObject, dummyPrepose, robot->getRootNode());
VirtualRobot::WorkspaceGridPtr reachGridGrasp;
- reachGridGrasp.reset(new WorkspaceGrid(minBB(0), maxBB(0), minBB(1), maxBB(1), ws->getDiscretizeParameterTranslation()));
+ reachGridGrasp.reset(new WorkspaceGrid(
+ minBB(0), maxBB(0), minBB(1), maxBB(1), ws->getDiscretizeParameterTranslation()));
reachGridGrasp->setGridPosition(globalObjectPose(0, 3), globalObjectPose(1, 3));
ARMARX_INFO << " grasp pose: " << dummyGrasp->getTcpPoseGlobal(globalObjectPose);
ARMARX_INFO << " prepose pose: " << dummyPrepose->getTcpPoseGlobal(globalObjectPose);
@@ -571,7 +667,8 @@ VirtualRobot::WorkspaceGridPtr SimpleRobotPlacement::createWorkspaceGrid(Generat
// visualizedGrid.reset(new WorkspaceGrid(minBB(0), maxBB(0), minBB(1), maxBB(1), ws->getDiscretizeParameterTranslation()));
TIMING_START(GridMerge);
- visualizedGrid = reachGridGrasp;//VirtualRobot::WorkspaceGrid::MergeWorkspaceGrids({reachGridGrasp, reachGridPrepose});
+ visualizedGrid =
+ reachGridGrasp; //VirtualRobot::WorkspaceGrid::MergeWorkspaceGrids({reachGridGrasp, reachGridPrepose});
TIMING_END(GridMerge);
// visualizedGrid->setGridPosition(globalObjectPose(0, 3), globalObjectPose(1, 3));
// }
@@ -587,7 +684,16 @@ VirtualRobot::WorkspaceGridPtr SimpleRobotPlacement::createWorkspaceGrid(Generat
// return VirtualRobot::WorkspaceGrid::MergeWorkspaceGrids({reachGridGrasp, reachGridPrepose});
}
-bool SimpleRobotPlacement::getSuitablePosition(const GeneratedGrasp& g, VirtualRobot::WorkspaceGridPtr reachGrid, const Eigen::Matrix4f& globalObjectPose, float& storeGlobalX, float& storeGlobalY, float& storeGlobalYaw, int& score, const VirtualRobot::MathTools::ConvexHull2DPtr& placementArea)
+bool
+SimpleRobotPlacement::getSuitablePosition(
+ const GeneratedGrasp& g,
+ VirtualRobot::WorkspaceGridPtr reachGrid,
+ const Eigen::Matrix4f& globalObjectPose,
+ float& storeGlobalX,
+ float& storeGlobalY,
+ float& storeGlobalYaw,
+ int& score,
+ const VirtualRobot::MathTools::ConvexHull2DPtr& placementArea)
{
// robot pose
@@ -610,7 +716,12 @@ bool SimpleRobotPlacement::getSuitablePosition(const GeneratedGrasp& g, VirtualR
auto collisionCheckVisu = "collisionCheckRobotVisu";
if (getProperty<bool>("EnableVisualization"))
{
- entityDrawerPrx->setRobotVisu(visuLayerName, collisionCheckVisu, robotStateComponentPrx->getRobotFilename(), simox::alg::join(robotStateComponentPrx->getArmarXPackages(), ","), FullModel);
+ entityDrawerPrx->setRobotVisu(
+ visuLayerName,
+ collisionCheckVisu,
+ robotStateComponentPrx->getRobotFilename(),
+ simox::alg::join(robotStateComponentPrx->getArmarXPackages(), ","),
+ FullModel);
auto config = robotStateComponentPrx->getSynchronizedRobot()->getConfig();
ARMARX_INFO << "Using config: " << config;
entityDrawerPrx->updateRobotConfig(visuLayerName, collisionCheckVisu, config);
@@ -618,7 +729,8 @@ bool SimpleRobotPlacement::getSuitablePosition(const GeneratedGrasp& g, VirtualR
float visuSlowdownFactor = getProperty<float>("VisualizationSlowdownFactor");
auto wsr = getWorkspaceRepresentation(g);
- float minManipulabilityDecreaseFactor = getProperty<float>("MinManipulabilityDecreaseFactor").getValue();
+ float minManipulabilityDecreaseFactor =
+ getProperty<float>("MinManipulabilityDecreaseFactor").getValue();
int maxTrials = 1000;
bool success = false;
@@ -640,11 +752,14 @@ bool SimpleRobotPlacement::getSuitablePosition(const GeneratedGrasp& g, VirtualR
minRequiredEntry *= minManipulabilityDecreaseFactor;
minRequiredEntry = std::max<int>(minRequiredEntry, maxEntry * 0.1f);
int entries;
- if (!reachGrid->getRandomPos(minRequiredEntry, storeGlobalX, storeGlobalY, dummyGrasps, 100, &entries))
+ if (!reachGrid->getRandomPos(
+ minRequiredEntry, storeGlobalX, storeGlobalY, dummyGrasps, 100, &entries))
{
continue;
}
- float distance2D = (Eigen::Vector2f(storeGlobalX, storeGlobalY) - globalObjectPose.block<2, 1>(0, 3)).norm();
+ float distance2D =
+ (Eigen::Vector2f(storeGlobalX, storeGlobalY) - globalObjectPose.block<2, 1>(0, 3))
+ .norm();
if (distance2D > maxDistance2D)
{
// ARMARX_INFO << VAROUT(globalObjectPose) << " 2d: " << globalObjectPose.block<2, 1>(0, 3) << " candidate: " << Eigen::Vector2f(storeGlobalX, storeGlobalY);
@@ -657,9 +772,12 @@ bool SimpleRobotPlacement::getSuitablePosition(const GeneratedGrasp& g, VirtualR
continue;
}
- if (placementArea != nullptr && !VirtualRobot::MathTools::isInside(Eigen::Vector2f(storeGlobalX, storeGlobalY), placementArea))
+ if (placementArea != nullptr &&
+ !VirtualRobot::MathTools::isInside(Eigen::Vector2f(storeGlobalX, storeGlobalY),
+ placementArea))
{
- ARMARX_INFO << "Generated placement (" << storeGlobalX << ", " << storeGlobalY << ") lies outside the permitted area => Retry.";
+ ARMARX_INFO << "Generated placement (" << storeGlobalX << ", " << storeGlobalY
+ << ") lies outside the permitted area => Retry.";
continue;
}
@@ -682,13 +800,18 @@ bool SimpleRobotPlacement::getSuitablePosition(const GeneratedGrasp& g, VirtualR
}
tmpRobotPoseGlobal.block<3, 1>(0, 3) = Eigen::Vector3f::Zero();
Eigen::Affine3f transform;
- transform = Eigen::Translation<float, 3>(x, y, z) * Eigen::AngleAxisf(storeGlobalYaw, Eigen::Vector3f(0, 0, 1));
+ transform = Eigen::Translation<float, 3>(x, y, z) *
+ Eigen::AngleAxisf(storeGlobalYaw, Eigen::Vector3f(0, 0, 1));
tmpRobotPoseGlobal = transform * tmpRobotPoseGlobal;
robot->setGlobalPose(tmpRobotPoseGlobal);
cspace->getAgentSceneObj()->setGlobalPose(tmpRobotPoseGlobal);
int max = wsr->getMaxEntry();
- ARMARX_VERBOSE << "Candidate pose: " << 100. * wsr->getEntry(FramedPosePtr::dynamicCast(g.framedPrePose)->toEigen()) / max << "% min. required: " << minRequiredEntry << " trial: " << counter * 100 / maxTrials << "%";
+ ARMARX_VERBOSE
+ << "Candidate pose: "
+ << 100. * wsr->getEntry(FramedPosePtr::dynamicCast(g.framedPrePose)->toEigen()) / max
+ << "% min. required: " << minRequiredEntry << " trial: " << counter * 100 / maxTrials
+ << "%";
if (!wsr->isCovered(FramedPosePtr::dynamicCast(g.framedPrePose)->toEigen()))
{
ARMARX_INFO << "not reachable";
@@ -709,8 +832,9 @@ bool SimpleRobotPlacement::getSuitablePosition(const GeneratedGrasp& g, VirtualR
//if (!collision)
if (getProperty<bool>("EnableVisualization"))
{
- updateRobot(collisionCheckVisu, tmpRobotPoseGlobal,
- collision ? DrawColor {1.0, 0.0, 0.0, 1} : DrawColor {0.0, 1.0, 0.0, 1});
+ updateRobot(collisionCheckVisu,
+ tmpRobotPoseGlobal,
+ collision ? DrawColor{1.0, 0.0, 0.0, 1} : DrawColor{0.0, 1.0, 0.0, 1});
usleep(500000 * visuSlowdownFactor);
}
@@ -751,7 +875,14 @@ bool SimpleRobotPlacement::getSuitablePosition(const GeneratedGrasp& g, VirtualR
return success;
}
-std::vector<SimpleRobotPlacement::RobotPlacement> SimpleRobotPlacement::getSuitablePositions(const GeneratedGrasp& g, WorkspaceGridPtr reachGrid, const Eigen::Matrix4f& globalObjectPose, float requiredReachabilityFraction, int requiredCount, int maxIterations, const MathTools::ConvexHull2DPtr& placementArea)
+std::vector<SimpleRobotPlacement::RobotPlacement>
+SimpleRobotPlacement::getSuitablePositions(const GeneratedGrasp& g,
+ WorkspaceGridPtr reachGrid,
+ const Eigen::Matrix4f& globalObjectPose,
+ float requiredReachabilityFraction,
+ int requiredCount,
+ int maxIterations,
+ const MathTools::ConvexHull2DPtr& placementArea)
{
std::vector<SimpleRobotPlacement::RobotPlacement> placements;
@@ -774,7 +905,12 @@ std::vector<SimpleRobotPlacement::RobotPlacement> SimpleRobotPlacement::getSuita
auto collisionCheckVisu = "collisionCheckRobotVisu";
if (getProperty<bool>("EnableVisualization"))
{
- entityDrawerPrx->setRobotVisu(visuLayerName, collisionCheckVisu, robotStateComponentPrx->getRobotFilename(), simox::alg::join(robotStateComponentPrx->getArmarXPackages(), ","), FullModel);
+ entityDrawerPrx->setRobotVisu(
+ visuLayerName,
+ collisionCheckVisu,
+ robotStateComponentPrx->getRobotFilename(),
+ simox::alg::join(robotStateComponentPrx->getArmarXPackages(), ","),
+ FullModel);
auto config = robotStateComponentPrx->getSynchronizedRobot()->getConfig();
ARMARX_INFO << "Using config: " << config;
entityDrawerPrx->updateRobotConfig(visuLayerName, collisionCheckVisu, config);
@@ -788,11 +924,14 @@ std::vector<SimpleRobotPlacement::RobotPlacement> SimpleRobotPlacement::getSuita
int entries;
float storeGlobalX, storeGlobalY;
- if (!reachGrid->getRandomPos(minRequiredEntry, storeGlobalX, storeGlobalY, dummyGrasps, 100, &entries))
+ if (!reachGrid->getRandomPos(
+ minRequiredEntry, storeGlobalX, storeGlobalY, dummyGrasps, 100, &entries))
{
continue;
}
- float distance2D = (Eigen::Vector2f(storeGlobalX, storeGlobalY) - globalObjectPose.block<2, 1>(0, 3)).norm();
+ float distance2D =
+ (Eigen::Vector2f(storeGlobalX, storeGlobalY) - globalObjectPose.block<2, 1>(0, 3))
+ .norm();
if (distance2D > 1000)
{
// ARMARX_INFO << VAROUT(globalObjectPose) << " 2d: " << globalObjectPose.block<2, 1>(0, 3) << " candidate: " << Eigen::Vector2f(storeGlobalX, storeGlobalY);
@@ -805,9 +944,12 @@ std::vector<SimpleRobotPlacement::RobotPlacement> SimpleRobotPlacement::getSuita
continue;
}
- if (placementArea != nullptr && !VirtualRobot::MathTools::isInside(Eigen::Vector2f(storeGlobalX, storeGlobalY), placementArea))
+ if (placementArea != nullptr &&
+ !VirtualRobot::MathTools::isInside(Eigen::Vector2f(storeGlobalX, storeGlobalY),
+ placementArea))
{
- ARMARX_INFO << "Generated placement (" << storeGlobalX << ", " << storeGlobalY << ") lies outside the permitted area => Retry.";
+ ARMARX_INFO << "Generated placement (" << storeGlobalX << ", " << storeGlobalY
+ << ") lies outside the permitted area => Retry.";
continue;
}
@@ -830,13 +972,17 @@ std::vector<SimpleRobotPlacement::RobotPlacement> SimpleRobotPlacement::getSuita
}
tmpRobotPoseGlobal.block<3, 1>(0, 3) = Eigen::Vector3f::Zero();
Eigen::Affine3f transform;
- transform = Eigen::Translation<float, 3>(x, y, z) * Eigen::AngleAxisf(storeGlobalYaw, Eigen::Vector3f(0, 0, 1));
+ transform = Eigen::Translation<float, 3>(x, y, z) *
+ Eigen::AngleAxisf(storeGlobalYaw, Eigen::Vector3f(0, 0, 1));
tmpRobotPoseGlobal = transform * tmpRobotPoseGlobal;
robot->setGlobalPose(tmpRobotPoseGlobal);
cspace->getAgentSceneObj()->setGlobalPose(tmpRobotPoseGlobal);
int max = wsr->getMaxEntry();
- ARMARX_VERBOSE << "Candidate pose: " << 100. * wsr->getEntry(FramedPosePtr::dynamicCast(g.framedPrePose)->toEigen()) / max << "% min. required: " << minRequiredEntry;
+ ARMARX_VERBOSE
+ << "Candidate pose: "
+ << 100. * wsr->getEntry(FramedPosePtr::dynamicCast(g.framedPrePose)->toEigen()) / max
+ << "% min. required: " << minRequiredEntry;
if (!wsr->isCovered(FramedPosePtr::dynamicCast(g.framedPrePose)->toEigen()))
{
ARMARX_INFO << "not reachable";
@@ -857,8 +1003,9 @@ std::vector<SimpleRobotPlacement::RobotPlacement> SimpleRobotPlacement::getSuita
//if (!collision)
if (getProperty<bool>("EnableVisualization"))
{
- updateRobot(collisionCheckVisu, tmpRobotPoseGlobal,
- collision ? DrawColor {1.0, 0.0, 0.0, 1} : DrawColor {0.0, 1.0, 0.0, 1});
+ updateRobot(collisionCheckVisu,
+ tmpRobotPoseGlobal,
+ collision ? DrawColor{1.0, 0.0, 0.0, 1} : DrawColor{0.0, 1.0, 0.0, 1});
usleep(500000 * visuSlowdownFactor);
}
@@ -890,7 +1037,9 @@ std::vector<SimpleRobotPlacement::RobotPlacement> SimpleRobotPlacement::getSuita
return placements;
}
-float SimpleRobotPlacement::getPlatformRotation(const Eigen::Matrix4f& frameGlobal, const Eigen::Matrix4f& globalTarget)
+float
+SimpleRobotPlacement::getPlatformRotation(const Eigen::Matrix4f& frameGlobal,
+ const Eigen::Matrix4f& globalTarget)
{
Eigen::Matrix4f localTarget = frameGlobal.inverse() * globalTarget;
float x = localTarget(0, 3);
@@ -901,7 +1050,8 @@ float SimpleRobotPlacement::getPlatformRotation(const Eigen::Matrix4f& frameGlob
return alpha;
}
-void SimpleRobotPlacement::loadRobot()
+void
+SimpleRobotPlacement::loadRobot()
{
robot = VirtualRobot::RobotIO::loadRobot(robotFilePath);
if (!robot)
@@ -910,11 +1060,13 @@ void SimpleRobotPlacement::loadRobot()
return;
}
- std::shared_ptr<RemoteRobot> remoteRobot(new RemoteRobot(robotStateComponentPrx->getSynchronizedRobot()));
+ std::shared_ptr<RemoteRobot> remoteRobot(
+ new RemoteRobot(robotStateComponentPrx->getSynchronizedRobot()));
robot->setGlobalPose(remoteRobot->getGlobalPose());
}
-void SimpleRobotPlacement::loadWorkspaces()
+void
+SimpleRobotPlacement::loadWorkspaces()
{
for (std::string wsFile : workspaceFilePaths)
{
@@ -962,7 +1114,8 @@ void SimpleRobotPlacement::loadWorkspaces()
}
}
-bool SimpleRobotPlacement::hasWorkspace(std::string tcp)
+bool
+SimpleRobotPlacement::hasWorkspace(std::string tcp)
{
for (VirtualRobot::WorkspaceRepresentationPtr ws : workspaces)
{
@@ -974,7 +1127,8 @@ bool SimpleRobotPlacement::hasWorkspace(std::string tcp)
return false;
}
-GeneratedGraspList SimpleRobotPlacement::filterGrasps(const GeneratedGraspList grasps)
+GeneratedGraspList
+SimpleRobotPlacement::filterGrasps(const GeneratedGraspList grasps)
{
GeneratedGraspList filteredGrasps = grasps;
GeneratedGraspList::iterator it = filteredGrasps.begin();
@@ -984,7 +1138,8 @@ GeneratedGraspList SimpleRobotPlacement::filterGrasps(const GeneratedGraspList g
auto tcpName = robot->getEndEffector(g.eefName)->getTcp()->getName();
if (!hasWorkspace(tcpName))
{
- ARMARX_VERBOSE << "Removing grasp because tcp " << tcpName << " is not available in workspace";
+ ARMARX_VERBOSE << "Removing grasp because tcp " << tcpName
+ << " is not available in workspace";
it = filteredGrasps.erase(it);
}
else
@@ -995,15 +1150,21 @@ GeneratedGraspList SimpleRobotPlacement::filterGrasps(const GeneratedGraspList g
return filteredGrasps;
}
-void SimpleRobotPlacement::drawNewRobot(Eigen::Matrix4f globalPose)
+void
+SimpleRobotPlacement::drawNewRobot(Eigen::Matrix4f globalPose)
{
if (!getProperty<bool>("EnableVisualization"))
{
return;
}
- static int suffix = 0;
+ static int suffix = 0;
std::string id = drawRobotId + to_string(suffix++);
- entityDrawerPrx->setRobotVisu(visuLayerName, id, robotStateComponentPrx->getRobotFilename(), simox::alg::join(robotStateComponentPrx->getArmarXPackages(), ","), FullModel);
+ entityDrawerPrx->setRobotVisu(
+ visuLayerName,
+ id,
+ robotStateComponentPrx->getRobotFilename(),
+ simox::alg::join(robotStateComponentPrx->getArmarXPackages(), ","),
+ FullModel);
auto config = robotStateComponentPrx->getSynchronizedRobot()->getConfig();
ARMARX_INFO << "Using config: " << config;
@@ -1011,7 +1172,8 @@ void SimpleRobotPlacement::drawNewRobot(Eigen::Matrix4f globalPose)
updateRobot(id, globalPose, COLOR_ROBOT);
}
-void SimpleRobotPlacement::updateRobot(std::string id, Eigen::Matrix4f globalPose, DrawColor color)
+void
+SimpleRobotPlacement::updateRobot(std::string id, Eigen::Matrix4f globalPose, DrawColor color)
{
if (!getProperty<bool>("EnableVisualization"))
{
@@ -1021,19 +1183,23 @@ void SimpleRobotPlacement::updateRobot(std::string id, Eigen::Matrix4f globalPos
entityDrawerPrx->updateRobotPose(visuLayerName, id, new Pose(globalPose));
}
-void SimpleRobotPlacement::drawWorkspaceGrid(const GeneratedGrasp& grasp, const std::string& objectInstanceEntityId)
+void
+SimpleRobotPlacement::drawWorkspaceGrid(const GeneratedGrasp& grasp,
+ const std::string& objectInstanceEntityId)
{
if (!getProperty<bool>("EnableVisualization"))
{
return;
}
- ObjectInstancePtr instance = ObjectInstancePtr::dynamicCast(objectInstances->getEntityById(objectInstanceEntityId));
+ ObjectInstancePtr instance =
+ ObjectInstancePtr::dynamicCast(objectInstances->getEntityById(objectInstanceEntityId));
Eigen::Matrix4f objectPose = instance->getPose()->toGlobalEigen(robot);
drawWorkspaceGrid(createWorkspaceGrid(grasp, objectPose));
}
-void SimpleRobotPlacement::drawWorkspaceGrid(VirtualRobot::WorkspaceGridPtr reachGrid)
+void
+SimpleRobotPlacement::drawWorkspaceGrid(VirtualRobot::WorkspaceGridPtr reachGrid)
{
if (!getProperty<bool>("EnableVisualization"))
{
@@ -1084,11 +1250,13 @@ void SimpleRobotPlacement::drawWorkspaceGrid(VirtualRobot::WorkspaceGridPtr reac
voxelColor.b = color.b;
voxelColor.a = 0.5;
- batch->setBoxVisu(visuLayerName, "reachGridVoxel_" + to_string(counter++),
- tmpPose, dimensions, voxelColor);
+ batch->setBoxVisu(visuLayerName,
+ "reachGridVoxel_" + to_string(counter++),
+ tmpPose,
+ dimensions,
+ voxelColor);
}
}
}
batch->ice_flushBatchRequests();
}
-
diff --git a/source/RobotComponents/components/SimpleRobotPlacement/SimpleRobotPlacement.h b/source/RobotComponents/components/SimpleRobotPlacement/SimpleRobotPlacement.h
index bc70981d0481701c7f569b5090082ea99ec3b42a..e77ad35b81edd17170e2c6a20c6982b642df7d06 100644
--- a/source/RobotComponents/components/SimpleRobotPlacement/SimpleRobotPlacement.h
+++ b/source/RobotComponents/components/SimpleRobotPlacement/SimpleRobotPlacement.h
@@ -25,37 +25,38 @@
#pragma once
+#include <VirtualRobot/CollisionDetection/CDManager.h>
+#include <VirtualRobot/SceneObjectSet.h>
+#include <VirtualRobot/Workspace/Manipulability.h>
+#include <VirtualRobot/Workspace/Reachability.h>
+#include <VirtualRobot/Workspace/WorkspaceGrid.h>
+
#include <ArmarXCore/core/Component.h>
-#include <RobotComponents/interface/components/GraspingManager/RobotPlacementInterface.h>
-#include <MemoryX/interface/memorytypes/MemorySegments.h>
-#include <MemoryX/interface/components/WorkingMemoryInterface.h>
-#include <MemoryX/core/GridFileManager.h>
+
+#include <RobotAPI/components/DebugDrawer/DebugDrawerComponent.h>
#include <RobotAPI/interface/core/RobotState.h>
-#include <VirtualRobot/Workspace/WorkspaceGrid.h>
+#include <RobotComponents/components/MotionPlanning/CSpace/SimoxCSpace.h>
+#include <RobotComponents/interface/components/GraspingManager/RobotPlacementInterface.h>
#include <RobotComponents/interface/components/RobotIK.h>
-#include <VirtualRobot/Workspace/Manipulability.h>
-#include <VirtualRobot/Workspace/Reachability.h>
-#include <VirtualRobot/SceneObjectSet.h>
-#include <RobotAPI/components/DebugDrawer/DebugDrawerComponent.h>
+#include <MemoryX/core/GridFileManager.h>
+#include <MemoryX/interface/components/WorkingMemoryInterface.h>
#include <MemoryX/interface/gui/EntityDrawerInterface.h>
-#include <VirtualRobot/CollisionDetection/CDManager.h>
-#include <RobotComponents/components/MotionPlanning/CSpace/SimoxCSpace.h>
+#include <MemoryX/interface/memorytypes/MemorySegments.h>
//#include <RobotComponents/interface/components/MotionPlanning/Tasks/MotionPlanningTask.h>
//#include <RobotComponents/components/MotionPlanning/Tasks/AStar/Task.h>
-#include <RobotComponents/components/MotionPlanning/Tasks/CSpaceVisualizerTask/CSpaceVisualizerTask.h>
-
#include <Eigen/Geometry>
+#include <RobotComponents/components/MotionPlanning/Tasks/CSpaceVisualizerTask/CSpaceVisualizerTask.h>
+
namespace armarx
{
/**
* @class SimpleRobotPlacementPropertyDefinitions
* @brief
*/
- class SimpleRobotPlacementPropertyDefinitions:
- public ComponentPropertyDefinitions
+ class SimpleRobotPlacementPropertyDefinitions : public ComponentPropertyDefinitions
{
public:
SimpleRobotPlacementPropertyDefinitions(std::string prefix);
@@ -83,15 +84,14 @@ namespace armarx
* @ingroup Component-SimpleRobotPlacement
* @brief refer to \ref Component-SimpleRobotPlacement
*/
- class SimpleRobotPlacement :
- virtual public Component,
- virtual public RobotPlacementInterface
+ class SimpleRobotPlacement : virtual public Component, virtual public RobotPlacementInterface
{
public:
/**
* @see armarx::ManagedIceObject::getDefaultName()
*/
- std::string getDefaultName() const override
+ std::string
+ getDefaultName() const override
{
return "SimpleRobotPlacement";
}
@@ -116,7 +116,9 @@ namespace armarx
* @param c
* @return A list of possible robot placements
*/
- GraspingPlacementList generateRobotPlacements(const GeneratedGraspList& grasps, const Ice::Current& c = Ice::emptyCurrent) override;
+ GraspingPlacementList
+ generateRobotPlacements(const GeneratedGraspList& grasps,
+ const Ice::Current& c = Ice::emptyCurrent) override;
/**
* @brief Computes a list of candidate robot placements for a grasp pose.
@@ -129,10 +131,17 @@ namespace armarx
* @param c Ice::Current
* @return A list of possible robot placements
*/
- GraspingPlacementList generateRobotPlacementsForGraspPose(const std::string& endEffectorName, const FramedPoseBasePtr& target, const PlanarObstacleList& planarObstacles, const ConvexHull& placementArea, const Ice::Current& c = Ice::emptyCurrent) override;
+ GraspingPlacementList
+ generateRobotPlacementsForGraspPose(const std::string& endEffectorName,
+ const FramedPoseBasePtr& target,
+ const PlanarObstacleList& planarObstacles,
+ const ConvexHull& placementArea,
+ const Ice::Current& c = Ice::emptyCurrent) override;
- GraspingPlacementList generateRobotPlacementsEx(const GeneratedGraspList& grasps, const Ice::Current& c = Ice::emptyCurrent) override;
+ GraspingPlacementList
+ generateRobotPlacementsEx(const GeneratedGraspList& grasps,
+ const Ice::Current& c = Ice::emptyCurrent) override;
protected:
/**
@@ -158,9 +167,11 @@ namespace armarx
/**
* @see PropertyUser::createPropertyDefinitions()
*/
- PropertyDefinitionsPtr createPropertyDefinitions() override
+ PropertyDefinitionsPtr
+ createPropertyDefinitions() override
{
- return PropertyDefinitionsPtr(new SimpleRobotPlacementPropertyDefinitions(getConfigIdentifier()));
+ return PropertyDefinitionsPtr(
+ new SimpleRobotPlacementPropertyDefinitions(getConfigIdentifier()));
}
/**
@@ -169,7 +180,9 @@ namespace armarx
* @param g generated Grasps
* @return Pointer to the WorkspaceRepresentation
*/
- VirtualRobot::WorkspaceRepresentationPtr getWorkspaceRepresentation(const GeneratedGrasp& g);
+ VirtualRobot::WorkspaceRepresentationPtr
+ getWorkspaceRepresentation(const GeneratedGrasp& g);
+
private:
memoryx::WorkingMemoryInterfacePrx wm;
memoryx::WorkingMemoryEntitySegmentBasePrx objectInstances;
@@ -202,12 +215,29 @@ namespace armarx
std::vector<CSpaceVisualizerTaskHandle> planningTasks;
- VirtualRobot::WorkspaceGridPtr createWorkspaceGrid(GeneratedGrasp g, const Eigen::Matrix4f& globalObjectPose);
+ VirtualRobot::WorkspaceGridPtr createWorkspaceGrid(GeneratedGrasp g,
+ const Eigen::Matrix4f& globalObjectPose);
void drawWorkspaceGrid(VirtualRobot::WorkspaceGridPtr reachGrid);
// finds a collision free position, tries to maximize the position score
- bool getSuitablePosition(const GeneratedGrasp& g, VirtualRobot::WorkspaceGridPtr reachGrid, const Eigen::Matrix4f& globalGraspPose, float& storeGlobalX, float& storeGlobalY, float& storeGlobalYaw, int& score, const VirtualRobot::MathTools::ConvexHull2DPtr& placementArea = VirtualRobot::MathTools::ConvexHull2DPtr());
+ bool getSuitablePosition(const GeneratedGrasp& g,
+ VirtualRobot::WorkspaceGridPtr reachGrid,
+ const Eigen::Matrix4f& globalGraspPose,
+ float& storeGlobalX,
+ float& storeGlobalY,
+ float& storeGlobalYaw,
+ int& score,
+ const VirtualRobot::MathTools::ConvexHull2DPtr& placementArea =
+ VirtualRobot::MathTools::ConvexHull2DPtr());
- std::vector<RobotPlacement> getSuitablePositions(const GeneratedGrasp& g, VirtualRobot::WorkspaceGridPtr reachGrid, const Eigen::Matrix4f& globalGraspPose, float requiredReachabilityFraction, int requiredCount, int maxIterations, const VirtualRobot::MathTools::ConvexHull2DPtr& placementArea = VirtualRobot::MathTools::ConvexHull2DPtr());
+ std::vector<RobotPlacement>
+ getSuitablePositions(const GeneratedGrasp& g,
+ VirtualRobot::WorkspaceGridPtr reachGrid,
+ const Eigen::Matrix4f& globalGraspPose,
+ float requiredReachabilityFraction,
+ int requiredCount,
+ int maxIterations,
+ const VirtualRobot::MathTools::ConvexHull2DPtr& placementArea =
+ VirtualRobot::MathTools::ConvexHull2DPtr());
// loads the robot given in the properties for collision testing
void loadRobot();
// loads the workspaces with the file paths given in the properties
@@ -217,11 +247,12 @@ namespace armarx
// filters all generated grasps, whose tcp is not given in any of the preloaded workspaces
GeneratedGraspList filterGrasps(const GeneratedGraspList grasps);
// returns the rotation needed for the robot to face the given target pose
- float getPlatformRotation(const Eigen::Matrix4f& frameGlobal, const Eigen::Matrix4f& globalTarget);
+ float getPlatformRotation(const Eigen::Matrix4f& frameGlobal,
+ const Eigen::Matrix4f& globalTarget);
void updateRobot(std::string id, Eigen::Matrix4f globalPose, DrawColor color);
void drawNewRobot(Eigen::Matrix4f globalPose);
- void drawWorkspaceGrid(const ::armarx::GeneratedGrasp&, const std::string& objectInstanceEntityId);
+ void drawWorkspaceGrid(const ::armarx::GeneratedGrasp&,
+ const std::string& objectInstanceEntityId);
};
-}
-
+} // namespace armarx
diff --git a/source/RobotComponents/components/StaticAgentReporter/StaticAgentReporter.cpp b/source/RobotComponents/components/StaticAgentReporter/StaticAgentReporter.cpp
index 1db505403dfe61e4f96590e5ff2f87948baf4ce3..0f741f44a9ec0f3cf1370981677d8aa9af9c0c89 100644
--- a/source/RobotComponents/components/StaticAgentReporter/StaticAgentReporter.cpp
+++ b/source/RobotComponents/components/StaticAgentReporter/StaticAgentReporter.cpp
@@ -27,8 +27,8 @@
using namespace armarx;
-
-void StaticAgentReporter::onInitComponent()
+void
+StaticAgentReporter::onInitComponent()
{
usingProxy(getProperty<std::string>("RobotStateComponentName").getValue());
@@ -38,14 +38,18 @@ void StaticAgentReporter::onInitComponent()
}
}
-
-void StaticAgentReporter::onConnectComponent()
+void
+StaticAgentReporter::onConnectComponent()
{
- robotStateComponent = getProxy<RobotStateComponentInterfacePrx>(getProperty<std::string>("RobotStateComponentName").getValue());
- robot = RemoteRobot::createLocalCloneFromFile(robotStateComponent, VirtualRobot::RobotIO::RobotDescription::eStructure);
+ robotStateComponent = getProxy<RobotStateComponentInterfacePrx>(
+ getProperty<std::string>("RobotStateComponentName").getValue());
+ robot = RemoteRobot::createLocalCloneFromFile(
+ robotStateComponent, VirtualRobot::RobotIO::RobotDescription::eStructure);
if (!getProperty<std::string>("WorkingMemoryName").getValue().empty())
{
- memoryx::WorkingMemoryInterfacePrx workingMemory = getProxy<memoryx::WorkingMemoryInterfacePrx>(getProperty<std::string>("WorkingMemoryName").getValue());
+ memoryx::WorkingMemoryInterfacePrx workingMemory =
+ getProxy<memoryx::WorkingMemoryInterfacePrx>(
+ getProperty<std::string>("WorkingMemoryName").getValue());
agentInstanceSegment = workingMemory->getAgentInstancesSegment();
robotAgent = new memoryx::AgentInstance(robotStateComponent->getRobotName());
@@ -54,26 +58,25 @@ void StaticAgentReporter::onConnectComponent()
robotAgent->setAgentFilePath(robotStateComponent->getRobotFilename());
robotAgent->setPose(new FramedPose(Eigen::Matrix4f::Identity(), GlobalFrame, ""));
- std::string robotAgentId = agentInstanceSegment->upsertEntityByName(robotAgent->getName(), robotAgent);
+ std::string robotAgentId =
+ agentInstanceSegment->upsertEntityByName(robotAgent->getName(), robotAgent);
robotAgent->setId(robotAgentId);
}
}
-
-void StaticAgentReporter::onDisconnectComponent()
+void
+StaticAgentReporter::onDisconnectComponent()
{
-
}
-
-void StaticAgentReporter::onExitComponent()
+void
+StaticAgentReporter::onExitComponent()
{
-
}
-armarx::PropertyDefinitionsPtr StaticAgentReporter::createPropertyDefinitions()
+armarx::PropertyDefinitionsPtr
+StaticAgentReporter::createPropertyDefinitions()
{
- return armarx::PropertyDefinitionsPtr(new StaticAgentReporterPropertyDefinitions(
- getConfigIdentifier()));
+ return armarx::PropertyDefinitionsPtr(
+ new StaticAgentReporterPropertyDefinitions(getConfigIdentifier()));
}
-
diff --git a/source/RobotComponents/components/StaticAgentReporter/StaticAgentReporter.h b/source/RobotComponents/components/StaticAgentReporter/StaticAgentReporter.h
index 663bfa41b16a2e2f3f4f5c08590f80c9ca17ec91..9fa8376eaee6f205da3c78c0b55ac7915f0e4985 100644
--- a/source/RobotComponents/components/StaticAgentReporter/StaticAgentReporter.h
+++ b/source/RobotComponents/components/StaticAgentReporter/StaticAgentReporter.h
@@ -34,16 +34,21 @@ namespace armarx
* @class StaticAgentReporterPropertyDefinitions
* @brief
*/
- class StaticAgentReporterPropertyDefinitions:
- public armarx::ComponentPropertyDefinitions
+ class StaticAgentReporterPropertyDefinitions : public armarx::ComponentPropertyDefinitions
{
public:
- StaticAgentReporterPropertyDefinitions(std::string prefix):
+ StaticAgentReporterPropertyDefinitions(std::string prefix) :
armarx::ComponentPropertyDefinitions(prefix)
{
- defineOptionalProperty<std::string>("RobotStateComponentName", "RobotStateComponent", "Name of the RobotStateComponent that should be used");
- defineOptionalProperty<std::string>("WorkingMemoryName", "WorkingMemory", "Name of the WorkingMemory that should be used. Leave empty to not use the working memory and update the robotstate directly");
-
+ defineOptionalProperty<std::string>(
+ "RobotStateComponentName",
+ "RobotStateComponent",
+ "Name of the RobotStateComponent that should be used");
+ defineOptionalProperty<std::string>(
+ "WorkingMemoryName",
+ "WorkingMemory",
+ "Name of the WorkingMemory that should be used. Leave empty to not use the working "
+ "memory and update the robotstate directly");
}
};
@@ -58,14 +63,14 @@ namespace armarx
*
* Detailed description of class StaticAgentReporter.
*/
- class StaticAgentReporter :
- virtual public armarx::Component
+ class StaticAgentReporter : virtual public armarx::Component
{
public:
/**
* @see armarx::ManagedIceObject::getDefaultName()
*/
- std::string getDefaultName() const override
+ std::string
+ getDefaultName() const override
{
return "StaticAgentReporter";
}
@@ -97,8 +102,6 @@ namespace armarx
armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
private:
-
-
RobotStateComponentInterfacePrx robotStateComponent;
memoryx::AgentInstancesSegmentBasePrx agentInstanceSegment;
@@ -106,5 +109,4 @@ namespace armarx
VirtualRobot::RobotPtr robot;
};
-}
-
+} // namespace armarx
diff --git a/source/RobotComponents/components/StaticAgentReporter/test/StaticAgentReporterTest.cpp b/source/RobotComponents/components/StaticAgentReporter/test/StaticAgentReporterTest.cpp
index fabe686a69a48d68615bb4e3da69ae837fc8bd49..29976d9cbbbae119df3868b9b3261a398d7965f8 100644
--- a/source/RobotComponents/components/StaticAgentReporter/test/StaticAgentReporterTest.cpp
+++ b/source/RobotComponents/components/StaticAgentReporter/test/StaticAgentReporterTest.cpp
@@ -24,11 +24,11 @@
#define ARMARX_BOOST_TEST
+#include <iostream>
+
#include <RobotComponents/Test.h>
#include <RobotComponents/components/StaticAgentReporter/StaticAgentReporter.h>
-#include <iostream>
-
BOOST_AUTO_TEST_CASE(testExample)
{
armarx::StaticAgentReporter instance;
diff --git a/source/RobotComponents/components/TrajectoryPlayer/TrajectoryPlayer.cpp b/source/RobotComponents/components/TrajectoryPlayer/TrajectoryPlayer.cpp
index 84125a4f01f3bd16c2c8c75ed6e014eb2fd8b0ad..72bbb42d8b13ba663a108e76305480dd03889232 100644
--- a/source/RobotComponents/components/TrajectoryPlayer/TrajectoryPlayer.cpp
+++ b/source/RobotComponents/components/TrajectoryPlayer/TrajectoryPlayer.cpp
@@ -21,9 +21,11 @@
*/
#include "TrajectoryPlayer.h"
+
+#include <SimoxUtility/math/convert.h>
+
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
-#include <SimoxUtility/math/convert.h>
using namespace armarx;
@@ -32,7 +34,8 @@ using namespace armarx;
#define STATE_VELOCITY 1
#define STATE_ACCELERATION 2
-bool TrajectoryPlayer::startTrajectoryPlayer(const Ice::Current&)
+bool
+TrajectoryPlayer::startTrajectoryPlayer(const Ice::Current&)
{
direction = 1;
timeOffset = 0;
@@ -41,14 +44,19 @@ bool TrajectoryPlayer::startTrajectoryPlayer(const Ice::Current&)
if (isPreview)
{
ARMARX_INFO << "robot file name : " << kinematicUnit->getRobotFilename();
- debugDrawer->setRobotVisu("Preview", "previewRobot", kinematicUnit->getRobotFilename(), armarxProject, armarx::CollisionModel);
- debugDrawer->updateRobotColor("Preview", "previewRobot", DrawColor {0, 1, 0, 0.5});
+ debugDrawer->setRobotVisu("Preview",
+ "previewRobot",
+ kinematicUnit->getRobotFilename(),
+ armarxProject,
+ armarx::CollisionModel);
+ debugDrawer->updateRobotColor("Preview", "previewRobot", DrawColor{0, 1, 0, 0.5});
}
try
{
- task = new PeriodicTask<TrajectoryPlayer>(this, &TrajectoryPlayer::run, 10, false, "TrajectoryPlayerTask", false);
+ task = new PeriodicTask<TrajectoryPlayer>(
+ this, &TrajectoryPlayer::run, 10, false, "TrajectoryPlayerTask", false);
paused = false;
firstRound = true;
currentTime = 0;
@@ -63,11 +71,10 @@ bool TrajectoryPlayer::startTrajectoryPlayer(const Ice::Current&)
ARMARX_WARNING << "Failed to start MMMPLayer task";
return false;
}
-
-
}
-bool TrajectoryPlayer::pauseTrajectoryPlayer(const Ice::Current&)
+bool
+TrajectoryPlayer::pauseTrajectoryPlayer(const Ice::Current&)
{
std::unique_lock lock(motionMutex);
@@ -86,7 +93,8 @@ bool TrajectoryPlayer::pauseTrajectoryPlayer(const Ice::Current&)
return paused;
}
-bool TrajectoryPlayer::stopTrajectoryPlayer(const Ice::Current&)
+bool
+TrajectoryPlayer::stopTrajectoryPlayer(const Ice::Current&)
{
std::unique_lock lock(motionMutex);
paused = true;
@@ -122,7 +130,8 @@ bool TrajectoryPlayer::stopTrajectoryPlayer(const Ice::Current&)
return false;
}
-void TrajectoryPlayer::updateTargetValues()
+void
+TrajectoryPlayer::updateTargetValues()
{
StringVariantBaseMap debugTargetValues;
StringVariantBaseMap debugVelocityValues;
@@ -134,7 +143,7 @@ void TrajectoryPlayer::updateTargetValues()
int maxDerivative = 1;
// ARMARX_INFO << deactivateSpam(1) << VAROUT(currentTime);
- std::vector<Ice::DoubleSeq > states = jointTraj->getAllStates(currentTime, maxDerivative);
+ std::vector<Ice::DoubleSeq> states = jointTraj->getAllStates(currentTime, maxDerivative);
for (size_t i = 0; i < jointNames.size(); ++i)
{
@@ -174,10 +183,9 @@ void TrajectoryPlayer::updateTargetValues()
//ARMARX_INFO << "*" << (jointName) << ": targetPosValue: " << targetPosValue << ", targetVel:" << (targetVel) << ", pid-cv:" << cv << ", result vel:" << (cv + targetVel);
targetVel += cv;
}
-
}
- targetVel = std::min<double>(maxVel / 180.0 * M_PI, targetVel);
- targetVel = std::max<double>(-1 * maxVel / 180.0 * M_PI, targetVel);
+ targetVel = std::min<double>(maxVel / 180.0 * M_PI, targetVel);
+ targetVel = std::max<double>(-1 * maxVel / 180.0 * M_PI, targetVel);
debugVelocityValues[jointName] = new Variant(targetVel);
}
else
@@ -189,7 +197,7 @@ void TrajectoryPlayer::updateTargetValues()
if (robotPoseUnitEnabled)
{
- std::vector<Ice::DoubleSeq > pose = basePoseTraj->getAllStates(currentTime, 0);
+ std::vector<Ice::DoubleSeq> pose = basePoseTraj->getAllStates(currentTime, 0);
Eigen::Vector3f position(pose[0][0], pose[1][0], pose[2][0]);
Eigen::Quaternionf orientation(pose[3][0], pose[4][0], pose[5][0], pose[6][0]);
@@ -209,14 +217,10 @@ void TrajectoryPlayer::updateTargetValues()
debugObserver->setDebugChannel("targetJointAngles", debugTargetValues);
debugObserver->setDebugChannel("targetVelocity", debugVelocityValues);
-
-
-
}
-
-
-bool TrajectoryPlayer::resetTrajectoryPlayer(bool moveToFrameZeroPose, const Ice::Current&)
+bool
+TrajectoryPlayer::resetTrajectoryPlayer(bool moveToFrameZeroPose, const Ice::Current&)
{
std::unique_lock lock(motionMutex);
currentTime = 0;
@@ -230,15 +234,15 @@ bool TrajectoryPlayer::resetTrajectoryPlayer(bool moveToFrameZeroPose, const Ice
return true;
}
-bool TrajectoryPlayer::setJointsInUse(const std::string& jointName, bool inUse, const Ice::Current&)
+bool
+TrajectoryPlayer::setJointsInUse(const std::string& jointName, bool inUse, const Ice::Current&)
{
jointNamesUsed[jointName] = inUse;
return jointNamesUsed[jointName];
}
-
-
-void TrajectoryPlayer::loadJointTraj(const TrajectoryBasePtr& trajs, const Ice::Current&)
+void
+TrajectoryPlayer::loadJointTraj(const TrajectoryBasePtr& trajs, const Ice::Current&)
{
std::unique_lock lock(motionMutex);
// get model filename
@@ -264,10 +268,13 @@ void TrajectoryPlayer::loadJointTraj(const TrajectoryBasePtr& trajs, const Ice::
}
ArmarXDataPath::getAbsolutePath(kinematicUnit->getRobotFilename(), modelFileName);
- localModel = VirtualRobot::RobotIO::loadRobot(modelFileName, VirtualRobot::RobotIO::RobotDescription::eStructure);
- if (getProperty<std::string>("CustomRootNode").isSet() && !getProperty<std::string>("CustomRootNode").getValue().empty())
+ localModel = VirtualRobot::RobotIO::loadRobot(
+ modelFileName, VirtualRobot::RobotIO::RobotDescription::eStructure);
+ if (getProperty<std::string>("CustomRootNode").isSet() &&
+ !getProperty<std::string>("CustomRootNode").getValue().empty())
{
- customRootNode = localModel->getRobotNode(getProperty<std::string>("CustomRootNode").getValue());
+ customRootNode =
+ localModel->getRobotNode(getProperty<std::string>("CustomRootNode").getValue());
}
else
{
@@ -294,7 +301,8 @@ void TrajectoryPlayer::loadJointTraj(const TrajectoryBasePtr& trajs, const Ice::
if (jointNames.size() != jointTraj->dim())
{
- ARMARX_ERROR << "Not all trajectory dimensions are named !!! (would cause problems when using kinematicUnit)";
+ ARMARX_ERROR << "Not all trajectory dimensions are named !!! (would cause problems when "
+ "using kinematicUnit)";
return;
}
@@ -348,23 +356,22 @@ void TrajectoryPlayer::loadJointTraj(const TrajectoryBasePtr& trajs, const Ice::
ARMARX_INFO << "Setting null velocities: " << nullVelocities;
kinematicUnit->switchControlMode(modes);
kinematicUnit->setJointVelocities(nullVelocities);
-
}
-void TrajectoryPlayer::loadBasePoseTraj(const TrajectoryBasePtr& trajs, const Ice::Current&)
+void
+TrajectoryPlayer::loadBasePoseTraj(const TrajectoryBasePtr& trajs, const Ice::Current&)
{
basePoseTraj = TrajectoryPtr::dynamicCast(trajs);
}
-
-
-void TrajectoryPlayer::setLoopPlayback(bool loop, const Ice::Current&)
+void
+TrajectoryPlayer::setLoopPlayback(bool loop, const Ice::Current&)
{
loopPlayback = loop;
}
-
-void TrajectoryPlayer::setIsVelocityControl(bool isVelocity, const Ice::Current&)
+void
+TrajectoryPlayer::setIsVelocityControl(bool isVelocity, const Ice::Current&)
{
// std::unique_lock lock(motionMutex);
@@ -389,21 +396,23 @@ void TrajectoryPlayer::setIsVelocityControl(bool isVelocity, const Ice::Current&
{
kinematicUnit->switchControlMode(modes);
}
- catch (...) { }
-
+ catch (...)
+ {
+ }
}
-
-
-
-void TrajectoryPlayer::reportJointAngles(const NameValueMap& angles, Ice::Long timestamp, bool, const Ice::Current&)
+void
+TrajectoryPlayer::reportJointAngles(const NameValueMap& angles,
+ Ice::Long timestamp,
+ bool,
+ const Ice::Current&)
{
std::unique_lock lock(jointAnglesMutex);
latestJointAngles = angles;
}
-
-void TrajectoryPlayer::onInitComponent()
+void
+TrajectoryPlayer::onInitComponent()
{
offeringTopic("DebugObserver");
usingProxy(getProperty<std::string>("KinematicUnitName").getValue());
@@ -419,43 +428,47 @@ void TrajectoryPlayer::onInitComponent()
offeringTopic("DebugDrawerUpdates");
}
-
-void TrajectoryPlayer::onConnectComponent()
+void
+TrajectoryPlayer::onConnectComponent()
{
- kinematicUnit = getProxy<KinematicUnitInterfacePrx>(getProperty<std::string>("KinematicUnitName").getValue());
+ kinematicUnit = getProxy<KinematicUnitInterfacePrx>(
+ getProperty<std::string>("KinematicUnitName").getValue());
debugObserver = getTopic<DebugObserverInterfacePrx>("DebugObserver");
debugDrawer = getTopic<armarx::DebugDrawerInterfacePrx>("DebugDrawerUpdates");
armarxProject = getProperty<std::string>("ArmarXProjects").getValue();
robotPoseUnitEnabled = getProperty<bool>("EnableRobotPoseUnit").getValue();
- Eigen::Vector3f position(getProperty<float>("Offset.x").getValue(), getProperty<float>("Offset.y").getValue(), getProperty<float>("Offset.z").getValue());
- Eigen::Vector3f orientation(getProperty<float>("Offset.roll").getValue(), getProperty<float>("Offset.pitch").getValue(), getProperty<float>("Offset.yaw").getValue());
+ Eigen::Vector3f position(getProperty<float>("Offset.x").getValue(),
+ getProperty<float>("Offset.y").getValue(),
+ getProperty<float>("Offset.z").getValue());
+ Eigen::Vector3f orientation(getProperty<float>("Offset.roll").getValue(),
+ getProperty<float>("Offset.pitch").getValue(),
+ getProperty<float>("Offset.yaw").getValue());
offset = simox::math::pos_rpy_to_mat4f(position, orientation);
ARMARX_INFO << "Trajectory Player Offset " << offset;
targetRobotPose = new Pose();
-
}
-
-void TrajectoryPlayer::onDisconnectComponent()
+void
+TrajectoryPlayer::onDisconnectComponent()
{
-
}
-
-void TrajectoryPlayer::onExitComponent()
+void
+TrajectoryPlayer::onExitComponent()
{
-
}
-armarx::PropertyDefinitionsPtr TrajectoryPlayer::createPropertyDefinitions()
+armarx::PropertyDefinitionsPtr
+TrajectoryPlayer::createPropertyDefinitions()
{
- return armarx::PropertyDefinitionsPtr(new TrajectoryPlayerPropertyDefinitions(
- getConfigIdentifier()));
+ return armarx::PropertyDefinitionsPtr(
+ new TrajectoryPlayerPropertyDefinitions(getConfigIdentifier()));
}
-void TrajectoryPlayer::run()
+void
+TrajectoryPlayer::run()
{
if (!jointTraj)
{
@@ -539,8 +552,8 @@ void TrajectoryPlayer::run()
int maxDerivative = 1;
// ARMARX_INFO << deactivateSpam(1) << VAROUT(currentTime);
- std::vector<Ice::DoubleSeq > states = jointTraj->getAllStates(currentTime, maxDerivative);
-
+ std::vector<Ice::DoubleSeq> states =
+ jointTraj->getAllStates(currentTime, maxDerivative);
for (size_t i = 0; i < jointNames.size(); ++i)
@@ -550,7 +563,8 @@ void TrajectoryPlayer::run()
if (jointNamesUsed[jointName])
{
// update targetPositionValues
- auto& targetPosValue = targetPositionValues[jointName] = states[i][STATE_POSITION];
+ auto& targetPosValue = targetPositionValues[jointName] =
+ states[i][STATE_POSITION];
auto it = jointOffets.find(jointName);
if (it != jointOffets.end())
{
@@ -593,10 +607,9 @@ void TrajectoryPlayer::run()
}*/
targetVel += cv;
}
-
}
- targetVel = std::min<double>(maxVel / 180.0 * M_PI, targetVel);
- targetVel = std::max<double>(-1 * maxVel / 180.0 * M_PI, targetVel);
+ targetVel = std::min<double>(maxVel / 180.0 * M_PI, targetVel);
+ targetVel = std::max<double>(-1 * maxVel / 180.0 * M_PI, targetVel);
targetVel *= direction;
debugVelocityValues[jointName] = new Variant(targetVel);
}
@@ -609,7 +622,7 @@ void TrajectoryPlayer::run()
if (robotPoseUnitEnabled)
{
- std::vector<Ice::DoubleSeq > pose = basePoseTraj->getAllStates(currentTime, 0);
+ std::vector<Ice::DoubleSeq> pose = basePoseTraj->getAllStates(currentTime, 0);
Eigen::Vector3f position(pose[0][0], pose[1][0], pose[2][0]);
Eigen::Quaternionf orientation(pose[3][0], pose[4][0], pose[5][0], pose[6][0]);
@@ -624,7 +637,6 @@ void TrajectoryPlayer::run()
{
targetRobotPose = new Pose(p);
}
-
}
}
@@ -646,13 +658,11 @@ void TrajectoryPlayer::run()
if (robotPoseUnitEnabled)
{
- RobotPoseUnitInterfacePrx robotPoseUnitPrx
- = getProxy<RobotPoseUnitInterfacePrx>(getProperty<std::string>("RobotPoseUnitName").getValue());
+ RobotPoseUnitInterfacePrx robotPoseUnitPrx = getProxy<RobotPoseUnitInterfacePrx>(
+ getProperty<std::string>("RobotPoseUnitName").getValue());
robotPoseUnitPrx->moveTo(PoseBasePtr::dynamicCast(targetRobotPose), 0.001f, 0.001f);
}
-
-
}
else
{
@@ -664,7 +674,8 @@ void TrajectoryPlayer::run()
if (robotPoseUnitEnabled)
{
- debugDrawer->updateRobotPose("Preview", "previewRobot", PoseBasePtr::dynamicCast(targetRobotPose));
+ debugDrawer->updateRobotPose(
+ "Preview", "previewRobot", PoseBasePtr::dynamicCast(targetRobotPose));
}
}
@@ -674,7 +685,6 @@ void TrajectoryPlayer::run()
sleep(1);
return;
}
-
}
{
@@ -684,14 +694,11 @@ void TrajectoryPlayer::run()
debugObserver->setDebugChannel("targetJointAngles", debugTargetValues);
debugObserver->setDebugChannel("targetVelocity", debugVelocityValues);
-
}
-
-
-
}
-void TrajectoryPlayer::updatePIDController(const NameValueMap& angles)
+void
+TrajectoryPlayer::updatePIDController(const NameValueMap& angles)
{
if (!isVelocityControl)
{
@@ -713,12 +720,11 @@ void TrajectoryPlayer::updatePIDController(const NameValueMap& angles)
if (it == PIDs.end())
{
PIDs[name] = PIDControllerPtr(new PIDController(getProperty<float>("Kp").getValue(),
- getProperty<float>("Ki").getValue(),
- getProperty<float>("Kd").getValue(),
- std::numeric_limits<double>::max(),
- std::numeric_limits<double>::max(),
- limitlessMap[name]
- ));
+ getProperty<float>("Ki").getValue(),
+ getProperty<float>("Kd").getValue(),
+ std::numeric_limits<double>::max(),
+ std::numeric_limits<double>::max(),
+ limitlessMap[name]));
ARMARX_INFO << "Creating PID for " << name << " is limitless:" << limitlessMap[name];
it = PIDs.find(name);
}
@@ -729,8 +735,8 @@ void TrajectoryPlayer::updatePIDController(const NameValueMap& angles)
}
}
-
-bool TrajectoryPlayer::checkJointsLimit()
+bool
+TrajectoryPlayer::checkJointsLimit()
{
if (!localModel)
@@ -770,16 +776,14 @@ bool TrajectoryPlayer::checkJointsLimit()
else
{
debugDrawer->updateRobotNodeColor("Preview", "previewRobot", jointName, normColor);
-
}
-
-
}
return true;
}
-bool TrajectoryPlayer::checkSelfCollision()
+bool
+TrajectoryPlayer::checkSelfCollision()
{
if (!localModel)
{
@@ -793,4 +797,3 @@ bool TrajectoryPlayer::checkSelfCollision()
return true;
}
-
diff --git a/source/RobotComponents/components/TrajectoryPlayer/TrajectoryPlayer.h b/source/RobotComponents/components/TrajectoryPlayer/TrajectoryPlayer.h
index eb5d85d76452f7a1026b73ce6400e303d1eb0396..b919af34e5f3f8ef7417fbdbc0b1a1326203a813 100644
--- a/source/RobotComponents/components/TrajectoryPlayer/TrajectoryPlayer.h
+++ b/source/RobotComponents/components/TrajectoryPlayer/TrajectoryPlayer.h
@@ -23,65 +23,93 @@
#pragma once
-#include <ArmarXCore/core/Component.h>
-#include <RobotAPI/interface/components/TrajectoryPlayerInterface.h>
-#include <RobotAPI/interface/units/RobotPoseUnitInterface.h>
-
-#include <RobotAPI/libraries/core/RobotAPIObjectFactories.h>
+#include <mutex>
-#include <RobotAPI/libraries/core/Trajectory.h>
+#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
-#include <RobotAPI/libraries/core/PIDController.h>
-
#include <ArmarXCore/interface/observers/ObserverInterface.h>
-#include <RobotAPI/libraries/core/Pose.h>
#include <RobotAPI/components/DebugDrawer/DebugDrawerComponent.h>
+#include <RobotAPI/interface/components/TrajectoryPlayerInterface.h>
+#include <RobotAPI/interface/units/RobotPoseUnitInterface.h>
+#include <RobotAPI/libraries/core/PIDController.h>
+#include <RobotAPI/libraries/core/Pose.h>
+#include <RobotAPI/libraries/core/RobotAPIObjectFactories.h>
+#include <RobotAPI/libraries/core/Trajectory.h>
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
-#include <mutex>
-
-
namespace armarx
{
/**
* @class TrajectoryPlayerPropertyDefinitions
* @brief
*/
- class TrajectoryPlayerPropertyDefinitions:
- public armarx::ComponentPropertyDefinitions
+ class TrajectoryPlayerPropertyDefinitions : public armarx::ComponentPropertyDefinitions
{
public:
- TrajectoryPlayerPropertyDefinitions(std::string prefix):
+ TrajectoryPlayerPropertyDefinitions(std::string prefix) :
armarx::ComponentPropertyDefinitions(prefix)
{
- defineRequiredProperty<std::string>("KinematicUnitName", "Name of the KinematicUnit to which the joint values should be sent");
- defineRequiredProperty<std::string>("KinematicTopicName", "Name of the KinematicUnit reporting topic");
-
- defineOptionalProperty<std::string>("ArmarXProjects", "Armar4", "Comma-separated list with names of ArmarXProjects (e.g. 'Armar3,Armar4'). The MMM XML File can be specified relatively to a data path of one of these projects.");
- defineOptionalProperty<float>("FPS", 100.0f, "FPS with which the recording should be executed. Velocities will be adapted.");
- defineOptionalProperty<bool>("LoopPlayback", false, "Specify whether motion should be repeated after execution");
- defineOptionalProperty<bool>("isVelocityControl", true, "Specify whether the PID Controller should be used (==velocity control)");
+ defineRequiredProperty<std::string>(
+ "KinematicUnitName",
+ "Name of the KinematicUnit to which the joint values should be sent");
+ defineRequiredProperty<std::string>("KinematicTopicName",
+ "Name of the KinematicUnit reporting topic");
+
+ defineOptionalProperty<std::string>(
+ "ArmarXProjects",
+ "Armar4",
+ "Comma-separated list with names of ArmarXProjects (e.g. 'Armar3,Armar4'). The MMM "
+ "XML File can be specified relatively to a data path of one of these projects.");
+ defineOptionalProperty<float>(
+ "FPS",
+ 100.0f,
+ "FPS with which the recording should be executed. Velocities will be adapted.");
+ defineOptionalProperty<bool>(
+ "LoopPlayback", false, "Specify whether motion should be repeated after execution");
+ defineOptionalProperty<bool>(
+ "isVelocityControl",
+ true,
+ "Specify whether the PID Controller should be used (==velocity control)");
defineOptionalProperty<float>("Kp", 1.f, "Proportional gain value of PID Controller");
defineOptionalProperty<float>("Ki", 0.00001f, "Integral gain value of PID Controller");
defineOptionalProperty<float>("Kd", 0.0f, "Differential gain value of PID Controller");
- defineOptionalProperty<float>("absMaximumVelocity", 120.0f, "The PID will never set a velocity above this threshold (degree/s)");
- defineOptionalProperty<std::string>("RobotNodeSetName", "RobotState", "The name of the robot node set to be used (only need for PIDController mode)");
- defineOptionalProperty<std::string>("CustomRootNode", "", "If this value is set, the root pose of the motion is set for this node instead of the root joint.");
-
- defineOptionalProperty<bool>("EnableRobotPoseUnit", false, "Specify whether RobotPoseUnit should be used to move the robot's position. Only useful in simulation.");
- defineOptionalProperty<std::string>("RobotPoseUnitName", "RobotPoseUnit", "Name of the RobotPoseUnit to which the robot position should be sent");
- defineOptionalProperty<float>("Offset.x", 0.f, "x component of initial robot position (mm)");
- defineOptionalProperty<float>("Offset.y", 0.f, "y component of initial robot position (mm)");
- defineOptionalProperty<float>("Offset.z", 0.f, "z component of initial robot position (mm)");
- defineOptionalProperty<float>("Offset.roll", 0.f, "Initial robot pose: roll component of RPY angles (radian)");
- defineOptionalProperty<float>("Offset.pitch", 0.f, "Initial robot pose: pitch component of RPY angles (radian)");
- defineOptionalProperty<float>("Offset.yaw", 0.f, "Initial robot pose: yaw component of RPY angles (radian)");
-
+ defineOptionalProperty<float>(
+ "absMaximumVelocity",
+ 120.0f,
+ "The PID will never set a velocity above this threshold (degree/s)");
+ defineOptionalProperty<std::string>(
+ "RobotNodeSetName",
+ "RobotState",
+ "The name of the robot node set to be used (only need for PIDController mode)");
+ defineOptionalProperty<std::string>("CustomRootNode",
+ "",
+ "If this value is set, the root pose of the motion "
+ "is set for this node instead of the root joint.");
+
+ defineOptionalProperty<bool>("EnableRobotPoseUnit",
+ false,
+ "Specify whether RobotPoseUnit should be used to move the "
+ "robot's position. Only useful in simulation.");
+ defineOptionalProperty<std::string>(
+ "RobotPoseUnitName",
+ "RobotPoseUnit",
+ "Name of the RobotPoseUnit to which the robot position should be sent");
+ defineOptionalProperty<float>(
+ "Offset.x", 0.f, "x component of initial robot position (mm)");
+ defineOptionalProperty<float>(
+ "Offset.y", 0.f, "y component of initial robot position (mm)");
+ defineOptionalProperty<float>(
+ "Offset.z", 0.f, "z component of initial robot position (mm)");
+ defineOptionalProperty<float>(
+ "Offset.roll", 0.f, "Initial robot pose: roll component of RPY angles (radian)");
+ defineOptionalProperty<float>(
+ "Offset.pitch", 0.f, "Initial robot pose: pitch component of RPY angles (radian)");
+ defineOptionalProperty<float>(
+ "Offset.yaw", 0.f, "Initial robot pose: yaw component of RPY angles (radian)");
}
};
-
/**
* @defgroup Component-TrajectoryPlayer TrajectoryPlayer
* @ingroup RobotComponents-Components
@@ -102,7 +130,8 @@ namespace armarx
/**
* @see armarx::ManagedIceObject::getDefaultName()
*/
- std::string getDefaultName() const override
+ std::string
+ getDefaultName() const override
{
return "TrajectoryPlayer";
}
@@ -113,7 +142,8 @@ namespace armarx
bool stopTrajectoryPlayer(const Ice::Current&) override;
bool resetTrajectoryPlayer(bool moveToFrameZeroPose, const Ice::Current&) override;
void setLoopPlayback(bool loop, const Ice::Current&) override;
- void setIsVelocityControl(bool isVelocity, const ::Ice::Current& = Ice::emptyCurrent) override;
+ void setIsVelocityControl(bool isVelocity,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
// virtual void load(const TrajSource& trajs,
// double start = 0.0,
@@ -129,21 +159,25 @@ namespace armarx
// const ::Ice::StringSeq& joints = std::vector<std::string>(),
// const ::Ice::Current& = Ice::emptyCurrent);
- void loadJointTraj(const TrajectoryBasePtr& trajs, const ::Ice::Current& = Ice::emptyCurrent) override;
- void loadBasePoseTraj(const TrajectoryBasePtr& trajs, const ::Ice::Current& = Ice::emptyCurrent) override;
-
+ void loadJointTraj(const TrajectoryBasePtr& trajs,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
+ void loadBasePoseTraj(const TrajectoryBasePtr& trajs,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
- double getCurrentTime(const Ice::Current&) override
+ double
+ getCurrentTime(const Ice::Current&) override
{
return currentTime;
}
- double getEndTime(const Ice::Current&) override
+ double
+ getEndTime(const Ice::Current&) override
{
return endTime;
}
- void setEndTime(double end, const Ice::Current&) override
+ void
+ setEndTime(double end, const Ice::Current&) override
{
if (endTime == end)
{
@@ -155,44 +189,87 @@ namespace armarx
basePoseTraj = basePoseTraj->normalize(0, endTime);
}
-
- double getTrajEndTime(const Ice::Current&) override
+ double
+ getTrajEndTime(const Ice::Current&) override
{
return trajEndTime;
}
- void setIsPreview(bool isPreview, const Ice::Current&) override
+ void
+ setIsPreview(bool isPreview, const Ice::Current&) override
{
this->isPreview = isPreview;
}
- void enableRobotPoseUnit(bool isRobotPose, const Ice::Current&) override
+ void
+ enableRobotPoseUnit(bool isRobotPose, const Ice::Current&) override
{
robotPoseUnitEnabled = isRobotPose;
}
- void considerConstraints(bool, const Ice::Current&) override
+ void
+ considerConstraints(bool, const Ice::Current&) override
{
ARMARX_WARNING << "NYI TrajectoryPlayer::considerConstraints()";
}
bool setJointsInUse(const std::string& jointName, bool inUse, const Ice::Current&) override;
+
// KinematicUnitListener interface
- void reportControlModeChanged(const NameControlModeMap&, Ice::Long, bool, const Ice::Current&) override {}
- void reportJointAngles(const NameValueMap& angles, Ice::Long, bool, const Ice::Current&) override;
- void reportJointVelocities(const NameValueMap&, Ice::Long, bool, const Ice::Current&) override {}
- void reportJointTorques(const NameValueMap&, Ice::Long, bool, const Ice::Current&) override {}
- void reportJointAccelerations(const NameValueMap&, Ice::Long, bool, const Ice::Current&) override {}
- void reportJointCurrents(const NameValueMap&, Ice::Long, bool, const Ice::Current&) override {}
- void reportJointMotorTemperatures(const NameValueMap&, Ice::Long, bool, const Ice::Current&) override {}
- void reportJointStatuses(const NameStatusMap&, Ice::Long, bool, const Ice::Current&) override {}
-
- void setOffset(const Eigen::Matrix4f& offset, const Ice::Current&) override
+ void
+ reportControlModeChanged(const NameControlModeMap&,
+ Ice::Long,
+ bool,
+ const Ice::Current&) override
+ {
+ }
+
+ void reportJointAngles(const NameValueMap& angles,
+ Ice::Long,
+ bool,
+ const Ice::Current&) override;
+
+ void
+ reportJointVelocities(const NameValueMap&, Ice::Long, bool, const Ice::Current&) override
+ {
+ }
+
+ void
+ reportJointTorques(const NameValueMap&, Ice::Long, bool, const Ice::Current&) override
+ {
+ }
+
+ void
+ reportJointAccelerations(const NameValueMap&, Ice::Long, bool, const Ice::Current&) override
+ {
+ }
+
+ void
+ reportJointCurrents(const NameValueMap&, Ice::Long, bool, const Ice::Current&) override
+ {
+ }
+
+ void
+ reportJointMotorTemperatures(const NameValueMap&,
+ Ice::Long,
+ bool,
+ const Ice::Current&) override
+ {
+ }
+
+ void
+ reportJointStatuses(const NameStatusMap&, Ice::Long, bool, const Ice::Current&) override
+ {
+ }
+
+ void
+ setOffset(const Eigen::Matrix4f& offset, const Ice::Current&) override
{
this->offset = offset;
}
void updateTargetValues();
+
protected:
/**
* @see armarx::ManagedIceObject::onInitComponent()
@@ -286,9 +363,8 @@ namespace armarx
bool paused;
bool firstRound;
VirtualRobot::RobotNodePtr customRootNode;
-
};
- using TrajectoryPlayerPtr = ::IceInternal::Handle< ::armarx::TrajectoryPlayer>;
+ using TrajectoryPlayerPtr = ::IceInternal::Handle<::armarx::TrajectoryPlayer>;
-}
+} // namespace armarx
diff --git a/source/RobotComponents/components/TrajectoryPlayer/test/TrajectoryPlayerTest.cpp b/source/RobotComponents/components/TrajectoryPlayer/test/TrajectoryPlayerTest.cpp
index 5cfc8d7ce8e536f0649b7e6d52bc4c4cf35aec42..5d1439dd8ba22dab37d385e472ef58192355677b 100644
--- a/source/RobotComponents/components/TrajectoryPlayer/test/TrajectoryPlayerTest.cpp
+++ b/source/RobotComponents/components/TrajectoryPlayer/test/TrajectoryPlayerTest.cpp
@@ -24,11 +24,11 @@
#define ARMARX_BOOST_TEST
+#include <iostream>
+
#include <RobotComponents/Test.h>
#include <RobotComponents/components/TrajectoryPlayer/TrajectoryPlayer.h>
-#include <iostream>
-
BOOST_AUTO_TEST_CASE(testExample)
{
armarx::TrajectoryPlayer instance;
diff --git a/source/RobotComponents/components/ViconMarkerProvider/ViconMarkerProvider.cpp b/source/RobotComponents/components/ViconMarkerProvider/ViconMarkerProvider.cpp
index 43295a6fc92c73fe99aae7ddd6e385050d683da9..0a305174b7771a64048347cde370846965092afe 100644
--- a/source/RobotComponents/components/ViconMarkerProvider/ViconMarkerProvider.cpp
+++ b/source/RobotComponents/components/ViconMarkerProvider/ViconMarkerProvider.cpp
@@ -25,18 +25,20 @@
using namespace armarx;
-
-void ViconMarkerProvider::onInitComponent()
+void
+ViconMarkerProvider::onInitComponent()
{
offeringTopic(getProperty<std::string>("ViconDataTopicName").getValue());
offeringTopic(getProperty<std::string>("DebugDrawerTopicName").getValue());
}
-
-void ViconMarkerProvider::onConnectComponent()
+void
+ViconMarkerProvider::onConnectComponent()
{
- listener = getTopic<ViconMarkerProviderListenerInterfacePrx>(getProperty<std::string>("ViconDataTopicName").getValue());
- debugDrawer = getTopic<DebugDrawerInterfacePrx>(getProperty<std::string>("DebugDrawerTopicName").getValue());
+ listener = getTopic<ViconMarkerProviderListenerInterfacePrx>(
+ getProperty<std::string>("ViconDataTopicName").getValue());
+ debugDrawer = getTopic<DebugDrawerInterfacePrx>(
+ getProperty<std::string>("DebugDrawerTopicName").getValue());
std::string address = getProperty<std::string>("Hostname").getValue();
address += ":" + to_string(getProperty<int>("Port").getValue());
@@ -50,11 +52,13 @@ void ViconMarkerProvider::onConnectComponent()
{
ARMARX_INFO << "Connected to VICON server. Setting up configuration...";
- if (dsClient.EnableMarkerData().Result && dsClient.EnableUnlabeledMarkerData().Result && dsClient.SetStreamMode(ViconDataStreamSDK::CPP::StreamMode::ClientPull).Result)
+ if (dsClient.EnableMarkerData().Result && dsClient.EnableUnlabeledMarkerData().Result &&
+ dsClient.SetStreamMode(ViconDataStreamSDK::CPP::StreamMode::ClientPull).Result)
{
ARMARX_INFO << "Setup complete. Starting polling thread.";
- publisherThread = new RunningTask<ViconMarkerProvider>(this, &ViconMarkerProvider::publish, "ViconPublishThread");
+ publisherThread = new RunningTask<ViconMarkerProvider>(
+ this, &ViconMarkerProvider::publish, "ViconPublishThread");
publisherThread->start();
}
else
@@ -74,20 +78,20 @@ void ViconMarkerProvider::onConnectComponent()
}
}
-
-void ViconMarkerProvider::onDisconnectComponent()
+void
+ViconMarkerProvider::onDisconnectComponent()
{
publisherThread->stop();
dsClient.Disconnect();
}
-
-void ViconMarkerProvider::onExitComponent()
+void
+ViconMarkerProvider::onExitComponent()
{
-
}
-void ViconMarkerProvider::publish()
+void
+ViconMarkerProvider::publish()
{
while (publisherThread->isRunning())
{
@@ -100,7 +104,8 @@ void ViconMarkerProvider::publish()
// Wait for frame (blocking call)
if (dsClient.GetFrame().Result != ViconDataStreamSDK::CPP::Result::Success)
{
- ARMARX_WARNING << "Failed to receive frame, will retry in one second! (Is VICON Nexus currently running in online or playback mode?)";
+ ARMARX_WARNING << "Failed to receive frame, will retry in one second! (Is VICON Nexus "
+ "currently running in online or playback mode?)";
sleep(1);
continue;
}
@@ -121,7 +126,8 @@ void ViconMarkerProvider::publish()
for (int i = 0; i < subjectMarkerCount; ++i)
{
std::string markerName = dsClient.GetMarkerName(subjectName, i).MarkerName;
- double* translation = dsClient.GetMarkerGlobalTranslation(subjectName, markerName).Translation;
+ double* translation =
+ dsClient.GetMarkerGlobalTranslation(subjectName, markerName).Translation;
Vector3f v;
v.e0 = translation[0];
@@ -175,15 +181,16 @@ void ViconMarkerProvider::publish()
listener->reportUnlabeledViconMarkerFrame(unlabeledMarkers);
- ARMARX_DEBUG << "Frame processed: " << subjectCount << " subjects, " << labeledMarkerCount << " labeled markers, " << unlabeledMarkers.size() << " unlabeled markers";
+ ARMARX_DEBUG << "Frame processed: " << subjectCount << " subjects, " << labeledMarkerCount
+ << " labeled markers, " << unlabeledMarkers.size() << " unlabeled markers";
debugDrawer->setColoredPointCloudVisu("ViconMarkerProvider", "MarkerCloud", pointCloud);
}
}
-armarx::PropertyDefinitionsPtr ViconMarkerProvider::createPropertyDefinitions()
+armarx::PropertyDefinitionsPtr
+ViconMarkerProvider::createPropertyDefinitions()
{
- return armarx::PropertyDefinitionsPtr(new ViconMarkerProviderPropertyDefinitions(
- getConfigIdentifier()));
+ return armarx::PropertyDefinitionsPtr(
+ new ViconMarkerProviderPropertyDefinitions(getConfigIdentifier()));
}
-
diff --git a/source/RobotComponents/components/ViconMarkerProvider/ViconMarkerProvider.h b/source/RobotComponents/components/ViconMarkerProvider/ViconMarkerProvider.h
index 080db821e4b107b2bebf39d28363687deeb0ef59..f4044630ef3a7d14f6123ed36ed83342e052c60e 100644
--- a/source/RobotComponents/components/ViconMarkerProvider/ViconMarkerProvider.h
+++ b/source/RobotComponents/components/ViconMarkerProvider/ViconMarkerProvider.h
@@ -22,30 +22,34 @@
#pragma once
-#include <DataStreamClient.h>
-
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/services/tasks/RunningTask.h>
-#include <RobotComponents/interface/components/ViconMarkerProviderInterface.h>
+
#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
+#include <RobotComponents/interface/components/ViconMarkerProviderInterface.h>
+
+#include <DataStreamClient.h>
+
namespace armarx
{
/**
* @class ViconMarkerProviderPropertyDefinitions
* @brief
*/
- class ViconMarkerProviderPropertyDefinitions:
- public armarx::ComponentPropertyDefinitions
+ class ViconMarkerProviderPropertyDefinitions : public armarx::ComponentPropertyDefinitions
{
public:
- ViconMarkerProviderPropertyDefinitions(std::string prefix):
+ ViconMarkerProviderPropertyDefinitions(std::string prefix) :
armarx::ComponentPropertyDefinitions(prefix)
{
defineRequiredProperty<std::string>("Hostname", "IP or hostname of the Vicon PC");
defineOptionalProperty<int>("Port", 801, "Port used of the Vicon SDK on the Vicon PC");
- defineOptionalProperty<std::string>("ViconDataTopicName", "ViconDataUpdates", "Topic on which the marker data is published");
- defineOptionalProperty<std::string>("DebugDrawerTopicName", "DebugDrawerUpdates", "Topic for debug drawer");
+ defineOptionalProperty<std::string>("ViconDataTopicName",
+ "ViconDataUpdates",
+ "Topic on which the marker data is published");
+ defineOptionalProperty<std::string>(
+ "DebugDrawerTopicName", "DebugDrawerUpdates", "Topic for debug drawer");
}
};
@@ -60,14 +64,14 @@ namespace armarx
*
* Detailed description of class ViconMarkerProvider.
*/
- class ViconMarkerProvider :
- virtual public armarx::Component
+ class ViconMarkerProvider : virtual public armarx::Component
{
public:
/**
* @see armarx::ManagedIceObject::getDefaultName()
*/
- virtual std::string getDefaultName() const
+ virtual std::string
+ getDefaultName() const
{
return "ViconMarkerProvider";
}
@@ -105,5 +109,4 @@ namespace armarx
ViconDataStreamSDK::CPP::Client dsClient;
DebugDrawerInterfacePrx debugDrawer;
};
-}
-
+} // namespace armarx
diff --git a/source/RobotComponents/components/ViconMarkerProvider/test/ViconMarkerProviderTest.cpp b/source/RobotComponents/components/ViconMarkerProvider/test/ViconMarkerProviderTest.cpp
index b68ca4ae6b13fa9d13ae6331a6d69e15ab4db815..64b6ac07c52707b18d29cece0700cd09f3b6ee25 100644
--- a/source/RobotComponents/components/ViconMarkerProvider/test/ViconMarkerProviderTest.cpp
+++ b/source/RobotComponents/components/ViconMarkerProvider/test/ViconMarkerProviderTest.cpp
@@ -24,11 +24,11 @@
#define ARMARX_BOOST_TEST
+#include <iostream>
+
#include <RobotComponents/Test.h>
#include <RobotComponents/components/ViconMarkerProvider/ViconMarkerProvider.h>
-#include <iostream>
-
BOOST_AUTO_TEST_CASE(testExample)
{
armarx::ViconMarkerProvider instance;
diff --git a/source/RobotComponents/components/laser_scanner_timestamp_test/LaserScannerTimestampTest.cpp b/source/RobotComponents/components/laser_scanner_timestamp_test/LaserScannerTimestampTest.cpp
index 9e529f372a0d1703b30019253f256f4b24b759bc..987e2b311dfaebe4be1663a1ac384a0a43cba22e 100644
--- a/source/RobotComponents/components/laser_scanner_timestamp_test/LaserScannerTimestampTest.cpp
+++ b/source/RobotComponents/components/laser_scanner_timestamp_test/LaserScannerTimestampTest.cpp
@@ -22,25 +22,22 @@
#include "LaserScannerTimestampTest.h"
+
#include <cstddef>
#include <ArmarXCore/core/time/Metronome.h>
-
#include <ArmarXCore/observers/variant/TimestampVariant.h>
-
-
namespace RobotComponents
{
- const std::string
- Component::defaultName = "LaserScannerTimestampTest";
-
+ const std::string Component::defaultName = "LaserScannerTimestampTest";
armarx::PropertyDefinitionsPtr
Component::createPropertyDefinitions()
{
- armarx::PropertyDefinitionsPtr def = new armarx::ComponentPropertyDefinitions(getConfigIdentifier());
+ armarx::PropertyDefinitionsPtr def =
+ new armarx::ComponentPropertyDefinitions(getConfigIdentifier());
def->optional(
properties.topicName, "TopicName", "Name of the laserscanner topic to report to.");
@@ -49,35 +46,54 @@ namespace RobotComponents
def->optional(properties.useTopic, "useTopic", "If enabled, will send data via topic.");
-
- def->required(properties.frame, "frame", "Coordinate frame of the laserscanner.");
- def->required(properties.freq, "frequency", "Default frequency of newly generated and published laser scans [Hz]");
-
- def->required(properties.min, "minOffset", "Smallest allowed offset of wrong timestamps [ms]. May be negative. Offset for wrong generated timestamp randomly chosen from interval [minOffset,maxOffset]");
- def->required(properties.max, "maxOffset", "Greatest allowed offset of wrong timestamps [ms]. May be negative. Offset for wrong generated timestamp randomly chosen from interval [minOffset,maxOffset]");
+ def->required(properties.frame, "frame", "Coordinate frame of the laserscanner.");
- def->required(properties.correctBatchSize, "MaxCorrectBatchSize", "Maximum size of consecutive generated correct timestamps. Batch size of correct timestamps is randomly chosen from interval [1,MaxCorrectBatchSize]");
- def->required(properties.wrongBatchSize, "MaxWrongBatchSize", "Maximum size of consecutive generated wrong timestamps. Batch size of wrong timestamps is randomly chosen from interval [1,MaxWrongBatchSize");
+ def->required(properties.freq,
+ "frequency",
+ "Default frequency of newly generated and published laser scans [Hz]");
+
+ def->required(
+ properties.min,
+ "minOffset",
+ "Smallest allowed offset of wrong timestamps [ms]. May be negative. Offset for wrong "
+ "generated timestamp randomly chosen from interval [minOffset,maxOffset]");
+ def->required(
+ properties.max,
+ "maxOffset",
+ "Greatest allowed offset of wrong timestamps [ms]. May be negative. Offset for wrong "
+ "generated timestamp randomly chosen from interval [minOffset,maxOffset]");
+
+ def->required(
+ properties.correctBatchSize,
+ "MaxCorrectBatchSize",
+ "Maximum size of consecutive generated correct timestamps. Batch size of correct "
+ "timestamps is randomly chosen from interval [1,MaxCorrectBatchSize]");
+ def->required(properties.wrongBatchSize,
+ "MaxWrongBatchSize",
+ "Maximum size of consecutive generated wrong timestamps. Batch size of wrong "
+ "timestamps is randomly chosen from interval [1,MaxWrongBatchSize");
return def;
}
-
void
Component::onInitComponent()
{
// Topics and properties defined above are automagically registered.
- if (properties.min >= properties.max) {
+ if (properties.min >= properties.max)
+ {
ARMARX_ERROR << "following statement should be false but is not: min >= max";
}
- if (properties.correctBatchSize < 1) {
+ if (properties.correctBatchSize < 1)
+ {
ARMARX_ERROR << "following statement should be false but is not: correctBatchSize < 1";
}
- if (properties.wrongBatchSize < 1) {
+ if (properties.wrongBatchSize < 1)
+ {
ARMARX_ERROR << "following statement should be false but is not: wrongBatchSize < 1";
}
@@ -88,7 +104,6 @@ namespace RobotComponents
}
}
-
void
Component::onConnectComponent()
{
@@ -117,14 +132,16 @@ namespace RobotComponents
constexpr std::size_t nSamples = 1000;
armarx::LaserScan scanData;
- for(std::size_t i = 0; i < nSamples; i++)
+ for (std::size_t i = 0; i < nSamples; i++)
{
- scanData.emplace_back(armarx::LaserScanStep {2 * M_PI / nSamples, 2500.0F, 1.0f} );
+ scanData.emplace_back(armarx::LaserScanStep{2 * M_PI / nSamples, 2500.0F, 1.0f});
}
armarx::DateTime scanTime = armarx::DateTime::Now();
- if (!correctScans) {
- armarx::Duration scanOffset = armarx::Duration::MilliSeconds(randInRange(properties.min, properties.max));
+ if (!correctScans)
+ {
+ armarx::Duration scanOffset =
+ armarx::Duration::MilliSeconds(randInRange(properties.min, properties.max));
scanTime += scanOffset;
ARMARX_INFO << "Using offset of " << scanOffset.toMilliSeconds() << "ms.";
}
@@ -132,8 +149,10 @@ namespace RobotComponents
armarx::TimestampVariantPtr scanT(new armarx::TimestampVariant(scanTime));
- if (laserScanUnitListenerTopic) {
- laserScanUnitListenerTopic->reportSensorValues(properties.frame, properties.frame, scanData, scanT);
+ if (laserScanUnitListenerTopic)
+ {
+ laserScanUnitListenerTopic->reportSensorValues(
+ properties.frame, properties.frame, scanData, scanT);
}
// if(laserScanUnitListenerPrx)
@@ -142,11 +161,15 @@ namespace RobotComponents
// }
scansUntilToggle--;
- if (scansUntilToggle <= 0) {
- if (correctScans) {
+ if (scansUntilToggle <= 0)
+ {
+ if (correctScans)
+ {
scansUntilToggle = randInRange(1, properties.wrongBatchSize);
ARMARX_INFO << "Now sending wrong timestamps";
- } else {
+ }
+ else
+ {
scansUntilToggle = randInRange(1, properties.correctBatchSize);
ARMARX_INFO << "Now sending correct timestamps";
}
@@ -157,7 +180,9 @@ namespace RobotComponents
}
}
- int Component::randInRange(int min, int max) {
+ int
+ Component::randInRange(int min, int max)
+ {
int size = (max + 1) - min;
int rand = std::rand();
@@ -170,24 +195,21 @@ namespace RobotComponents
runTask->stop();
}
-
void
Component::onExitComponent()
{
}
-
std::string
Component::getDefaultName() const
{
return Component::defaultName;
}
-
std::string
Component::GetDefaultName()
{
return Component::defaultName;
}
-} // namespace RobotComponents::components::laser_scanner_timestamp_test
+} // namespace RobotComponents
diff --git a/source/RobotComponents/components/laser_scanner_timestamp_test/LaserScannerTimestampTest.h b/source/RobotComponents/components/laser_scanner_timestamp_test/LaserScannerTimestampTest.h
index 63172d6dc14dbe11d6ad8b9d89ee799e31df227c..94a2b6e9355353ca62ecbd9b35bfb86fc7f40bf7 100644
--- a/source/RobotComponents/components/laser_scanner_timestamp_test/LaserScannerTimestampTest.h
+++ b/source/RobotComponents/components/laser_scanner_timestamp_test/LaserScannerTimestampTest.h
@@ -25,17 +25,15 @@
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/services/tasks/RunningTask.h>
-#include <RobotAPI/interface/units/LaserScannerUnit.h>
+#include <RobotAPI/interface/units/LaserScannerUnit.h>
namespace RobotComponents
{
- class Component :
- virtual public armarx::Component
+ class Component : virtual public armarx::Component
{
public:
-
/// @see armarx::ManagedIceObject::getDefaultName()
std::string getDefaultName() const override;
@@ -44,7 +42,6 @@ namespace RobotComponents
protected:
-
/// @see PropertyUser::createPropertyDefinitions()
armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
@@ -62,12 +59,11 @@ namespace RobotComponents
private:
void run();
-
+
int randInRange(int min, int max);
private:
-
static const std::string defaultName;
struct Properties
@@ -85,6 +81,7 @@ namespace RobotComponents
int correctBatchSize;
int wrongBatchSize;
};
+
Properties properties;
armarx::LaserScannerUnitListenerPrx laserScanUnitListenerTopic;
@@ -92,4 +89,4 @@ namespace RobotComponents
armarx::RunningTask<Component>::pointer_type runTask;
};
-} // namespace RobotComponents::components::laser_scanner_timestamp_test
+} // namespace RobotComponents
diff --git a/source/RobotComponents/gui-plugins/HandEyeCalibration/DisplayWidget.cpp b/source/RobotComponents/gui-plugins/HandEyeCalibration/DisplayWidget.cpp
index 75e351f81f38c2075ad25d65cc5efc1f945553db..a72276d9c30b1389a029e64b39512aba86c702ae 100644
--- a/source/RobotComponents/gui-plugins/HandEyeCalibration/DisplayWidget.cpp
+++ b/source/RobotComponents/gui-plugins/HandEyeCalibration/DisplayWidget.cpp
@@ -24,15 +24,14 @@
#include <QGridLayout>
-#include <Inventor/nodes/SoSeparator.h>
-
-#include <Inventor/nodes/SoPickStyle.h>
-#include <Inventor/nodes/SoMaterial.h>
+#include <Inventor/events/SoLocation2Event.h>
+#include <Inventor/events/SoMouseButtonEvent.h>
#include <Inventor/nodes/SoDrawStyle.h>
-#include <Inventor/nodes/SoVertexProperty.h>
#include <Inventor/nodes/SoLineSet.h>
-#include <Inventor/events/SoMouseButtonEvent.h>
-#include <Inventor/events/SoLocation2Event.h>
+#include <Inventor/nodes/SoMaterial.h>
+#include <Inventor/nodes/SoPickStyle.h>
+#include <Inventor/nodes/SoSeparator.h>
+#include <Inventor/nodes/SoVertexProperty.h>
namespace armarx
{
@@ -61,12 +60,17 @@ namespace armarx
delete display;
}
- Display* DisplayWidget::getDisplay()
+ Display*
+ DisplayWidget::getDisplay()
{
return display;
}
- Display::Display(QWidget* widget) : SoQtExaminerViewer(widget), sceneRootNode(new SoSeparator), contentRootNode(new SoSeparator), camera(new SoPerspectiveCamera)
+ Display::Display(QWidget* widget) :
+ SoQtExaminerViewer(widget),
+ sceneRootNode(new SoSeparator),
+ contentRootNode(new SoSeparator),
+ camera(new SoPerspectiveCamera)
{
this->setBackgroundColor(SbColor(80 / 255.0f, 80 / 255.0f, 80 / 255.0f));
this->setAccumulationBuffer(true);
@@ -104,17 +108,20 @@ namespace armarx
}
}
- SoSeparator* Display::getRootNode()
+ SoSeparator*
+ Display::getRootNode()
{
return contentRootNode;
}
- void Display::cameraViewAll()
+ void
+ Display::cameraViewAll()
{
camera->viewAll(contentRootNode, SbViewportRegion());
}
- void Display::cameraViewNode(SoNode* node, const float slack)
+ void
+ Display::cameraViewNode(SoNode* node, const float slack)
{
camera->viewAll(node, SbViewportRegion(), slack);
}
@@ -246,4 +253,4 @@ namespace armarx
// return SoQtExaminerViewer::processSoEvent(event);
// }
-}
+} // namespace armarx
diff --git a/source/RobotComponents/gui-plugins/HandEyeCalibration/DisplayWidget.h b/source/RobotComponents/gui-plugins/HandEyeCalibration/DisplayWidget.h
index 20b5233e480df3babed38fa43eb27320faeeed16..90394e6fbcda8571c0f35b311b753d8f7ef9e584 100644
--- a/source/RobotComponents/gui-plugins/HandEyeCalibration/DisplayWidget.h
+++ b/source/RobotComponents/gui-plugins/HandEyeCalibration/DisplayWidget.h
@@ -26,9 +26,9 @@
#include <QWidget>
/* Coin headers */
+#include <Inventor/Qt/viewers/SoQtExaminerViewer.h>
#include <Inventor/actions/SoGLRenderAction.h>
#include <Inventor/nodes/SoPerspectiveCamera.h>
-#include <Inventor/Qt/viewers/SoQtExaminerViewer.h>
namespace armarx
{
@@ -61,4 +61,4 @@ namespace armarx
private:
Display* display;
};
-}
+} // namespace armarx
diff --git a/source/RobotComponents/gui-plugins/HandEyeCalibration/HandEyeCalibrationGuiPlugin.cpp b/source/RobotComponents/gui-plugins/HandEyeCalibration/HandEyeCalibrationGuiPlugin.cpp
index 3cc4e42746a1ddc026f849839cd9dff974ffafd2..d3bff058429d638766faddc3c2a40f5dad340883 100644
--- a/source/RobotComponents/gui-plugins/HandEyeCalibration/HandEyeCalibrationGuiPlugin.cpp
+++ b/source/RobotComponents/gui-plugins/HandEyeCalibration/HandEyeCalibrationGuiPlugin.cpp
@@ -28,5 +28,5 @@ using namespace armarx;
HandEyeCalibrationGuiPlugin::HandEyeCalibrationGuiPlugin()
{
- addWidget < HandEyeCalibrationWidgetController > ();
+ addWidget<HandEyeCalibrationWidgetController>();
}
diff --git a/source/RobotComponents/gui-plugins/HandEyeCalibration/HandEyeCalibrationGuiPlugin.h b/source/RobotComponents/gui-plugins/HandEyeCalibration/HandEyeCalibrationGuiPlugin.h
index 50a2e549d4e16c66994d2ec0560760bb29406ee2..d96faff93e743980c2eb7cf21331c3a67335fb82 100644
--- a/source/RobotComponents/gui-plugins/HandEyeCalibration/HandEyeCalibrationGuiPlugin.h
+++ b/source/RobotComponents/gui-plugins/HandEyeCalibration/HandEyeCalibrationGuiPlugin.h
@@ -22,8 +22,9 @@
#pragma once
#include <ArmarXCore/core/system/ImportExportComponent.h>
-#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
+
#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
namespace armarx
{
@@ -34,8 +35,7 @@ namespace armarx
*
* Detailed description
*/
- class ARMARXCOMPONENT_IMPORT_EXPORT HandEyeCalibrationGuiPlugin:
- public armarx::ArmarXGuiPlugin
+ class ARMARXCOMPONENT_IMPORT_EXPORT HandEyeCalibrationGuiPlugin : public armarx::ArmarXGuiPlugin
{
Q_OBJECT
Q_INTERFACES(ArmarXGuiInterface)
@@ -47,4 +47,4 @@ namespace armarx
*/
HandEyeCalibrationGuiPlugin();
};
-}
+} // namespace armarx
diff --git a/source/RobotComponents/gui-plugins/HandEyeCalibration/HandEyeCalibrationWidgetController.cpp b/source/RobotComponents/gui-plugins/HandEyeCalibration/HandEyeCalibrationWidgetController.cpp
index f4af0d33384c473536d329865bd571ef9de3354a..153d0cc13a84fd2c2aa17a0afccb0315bb1b4154 100644
--- a/source/RobotComponents/gui-plugins/HandEyeCalibration/HandEyeCalibrationWidgetController.cpp
+++ b/source/RobotComponents/gui-plugins/HandEyeCalibration/HandEyeCalibrationWidgetController.cpp
@@ -22,16 +22,8 @@
#include "HandEyeCalibrationWidgetController.h"
-#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
-#include <VisionX/interface/components/Calibration.h>
-
-#include <ArmarXCore/util/json/JSONObject.h>
-#include <ArmarXCore/core/util/StringHelpers.h>
-#include <ArmarXCore/core/application/Application.h>
-#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
-#include <RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLogger.h>
-#include <RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLoggerEntry.h>
-#include <RobotAPI/libraries/core/Pose.h>
+#include <cctype>
+#include <iomanip>
#include <QClipboard>
#include <QFileDialog>
@@ -41,50 +33,62 @@
#include <pcl/common/transforms.h>
#include <pcl/filters/filter.h>
#include <pcl/filters/passthrough.h>
+#include <VirtualRobot/MathTools.h>
-#include <iomanip>
-#include <cctype>
+#include <ArmarXCore/core/application/Application.h>
+#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
+#include <ArmarXCore/core/util/StringHelpers.h>
+#include <ArmarXCore/util/json/JSONObject.h>
+
+#include <RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLogger.h>
+#include <RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLoggerEntry.h>
+#include <RobotAPI/libraries/core/Pose.h>
+#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
+
+#include <VisionX/interface/components/Calibration.h>
const QString defaultFileName("HandEyeCalib_Data.json");
using namespace armarx;
-HandEyeCalibrationWidgetController::HandEyeCalibrationWidgetController()
- = default;
+HandEyeCalibrationWidgetController::HandEyeCalibrationWidgetController() = default;
-HandEyeCalibrationWidgetController::~HandEyeCalibrationWidgetController()
- = default;
+HandEyeCalibrationWidgetController::~HandEyeCalibrationWidgetController() = default;
-
-void HandEyeCalibrationWidgetController::loadSettings(QSettings* settings)
+void
+HandEyeCalibrationWidgetController::loadSettings(QSettings* settings)
{
-
}
-void HandEyeCalibrationWidgetController::saveSettings(QSettings* settings)
+void
+HandEyeCalibrationWidgetController::saveSettings(QSettings* settings)
{
-
}
-QPointer<QDialog> HandEyeCalibrationWidgetController::getConfigDialog(QWidget* parent)
+QPointer<QDialog>
+HandEyeCalibrationWidgetController::getConfigDialog(QWidget* parent)
{
if (!dialog)
{
dialog = new SimpleConfigDialog(parent);
- dialog->addProxyFinder<visionx::PointCloudProviderInterfacePrx>({"PointCloudProvider", "PointCloudProvider", "*"});
- dialog->addProxyFinder<RobotStateComponentInterfacePrx>({"RobotStateComponent", "RobotStateComponent", "RobotState*"});
+ dialog->addProxyFinder<visionx::PointCloudProviderInterfacePrx>(
+ {"PointCloudProvider", "PointCloudProvider", "*"});
+ dialog->addProxyFinder<RobotStateComponentInterfacePrx>(
+ {"RobotStateComponent", "RobotStateComponent", "RobotState*"});
}
return qobject_cast<SimpleConfigDialog*>(dialog);
}
-void HandEyeCalibrationWidgetController::configured()
+void
+HandEyeCalibrationWidgetController::configured()
{
providerName = dialog->getProxyName("PointCloudProvider");
robotStateComponentName = dialog->getProxyName("RobotStateComponent");
}
-void HandEyeCalibrationWidgetController::onInitPointCloudProcessor()
+void
+HandEyeCalibrationWidgetController::onInitPointCloudProcessor()
{
ARMARX_INFO << "onInitPointCloudProcessor";
@@ -93,14 +97,16 @@ void HandEyeCalibrationWidgetController::onInitPointCloudProcessor()
usingProxy(robotStateComponentName);
}
-void HandEyeCalibrationWidgetController::initUI()
+void
+HandEyeCalibrationWidgetController::initUI()
{
widget.setupUi(getWidget());
widget.lineEdit_file->setText(QDir::currentPath() + "/" + defaultFileName);
getWidget()->setEnabled(false);
}
-void HandEyeCalibrationWidgetController::saveDatapoint()
+void
+HandEyeCalibrationWidgetController::saveDatapoint()
{
if (activeEndEffector)
{
@@ -118,25 +124,26 @@ void HandEyeCalibrationWidgetController::saveDatapoint()
logger.log(entry);
logger.close();
- QMessageBox::information(getWidget(),
- tr("HandEyeCalibration"),
- tr("Datapoint successful written to file."));
+ QMessageBox::information(
+ getWidget(), tr("HandEyeCalibration"), tr("Datapoint successful written to file."));
}
}
-void HandEyeCalibrationWidgetController::selectFile()
+void
+HandEyeCalibrationWidgetController::selectFile()
{
QString fileName = QFileDialog::getOpenFileName(getWidget(),
- tr("Select file for saving data points"),
- QDir::currentPath(),
- tr("JSON-File (*.json);; All Files (*)"));
+ tr("Select file for saving data points"),
+ QDir::currentPath(),
+ tr("JSON-File (*.json);; All Files (*)"));
if (!fileName.isEmpty())
{
widget.lineEdit_file->setText(fileName);
}
}
-VirtualRobot::RobotPtr HandEyeCalibrationWidgetController::getEEFRobot(VirtualRobot::EndEffectorPtr eef) const
+VirtualRobot::RobotPtr
+HandEyeCalibrationWidgetController::getEEFRobot(VirtualRobot::EndEffectorPtr eef) const
{
if (eef && robotEEFMap.count(eef->getName()) == 1)
{
@@ -145,7 +152,9 @@ VirtualRobot::RobotPtr HandEyeCalibrationWidgetController::getEEFRobot(VirtualRo
return VirtualRobot::RobotPtr();
}
-Eigen::Matrix4f HandEyeCalibrationWidgetController::getOffsetMatrixForEEF(const VirtualRobot::EndEffectorPtr eef) const
+Eigen::Matrix4f
+HandEyeCalibrationWidgetController::getOffsetMatrixForEEF(
+ const VirtualRobot::EndEffectorPtr eef) const
{
if (eef && robotEEFMap.count(eef->getName()) == 1)
{
@@ -154,7 +163,9 @@ Eigen::Matrix4f HandEyeCalibrationWidgetController::getOffsetMatrixForEEF(const
return Eigen::Matrix4f::Identity();
}
-void HandEyeCalibrationWidgetController::setOffsetMatrixForEEF(const VirtualRobot::EndEffectorPtr eef, const Eigen::Matrix4f& offset)
+void
+HandEyeCalibrationWidgetController::setOffsetMatrixForEEF(const VirtualRobot::EndEffectorPtr eef,
+ const Eigen::Matrix4f& offset)
{
if (eef && robotEEFMap.count(eef->getName()) == 1)
{
@@ -162,11 +173,13 @@ void HandEyeCalibrationWidgetController::setOffsetMatrixForEEF(const VirtualRobo
}
}
-void HandEyeCalibrationWidgetController::onConnectPointCloudProcessor()
+void
+HandEyeCalibrationWidgetController::onConnectPointCloudProcessor()
{
ARMARX_INFO << "onConnectPointCloudProcessor";
- robotStateComponentPrx = getProxy<RobotStateComponentInterfacePrx>(robotStateComponentName, true);
+ robotStateComponentPrx =
+ getProxy<RobotStateComponentInterfacePrx>(robotStateComponentName, true);
usingPointCloudProvider(providerName);
providerInfo = getPointCloudProvider(providerName, true);
@@ -180,37 +193,83 @@ void HandEyeCalibrationWidgetController::onConnectPointCloudProcessor()
}
catch (...)
{
- ARMARX_ERROR << "Exception while creating local clone of robot in onConnectPointCLoudProcessor()";
+ ARMARX_ERROR
+ << "Exception while creating local clone of robot in onConnectPointCLoudProcessor()";
handleExceptions();
}
// Setup periodic tasks
- taskEEFManipulation = new PeriodicTask<HandEyeCalibrationWidgetController>(this, &HandEyeCalibrationWidgetController::taskEEFManipulationCB, 30.0f);
- taskLocalRobotUpdate = new PeriodicTask<HandEyeCalibrationWidgetController>(this, &HandEyeCalibrationWidgetController::taskLocalRobotUpdateCB, 50.0f);
+ taskEEFManipulation = new PeriodicTask<HandEyeCalibrationWidgetController>(
+ this, &HandEyeCalibrationWidgetController::taskEEFManipulationCB, 30.0f);
+ taskLocalRobotUpdate = new PeriodicTask<HandEyeCalibrationWidgetController>(
+ this, &HandEyeCalibrationWidgetController::taskLocalRobotUpdateCB, 50.0f);
// Setup visualizations
QMetaObject::invokeMethod(this, "connectQt", Qt::BlockingQueuedConnection);
// Finally connecting the signals and slots
connect(this, SIGNAL(pointCloudUpdated()), this, SLOT(processPointCloud()));
- connect(widget.comboBox_endEffectorSelection, SIGNAL(currentIndexChanged(QString)), this, SLOT(activeEndEffectorChanged(QString)));
- connect(widget.horizontalSlider_croppingBox, SIGNAL(valueChanged(int)), this, SLOT(slider_croppingRange_changed(int)));
- connect(widget.checkBox_croppingActive, SIGNAL(stateChanged(int)), this, SLOT(cb_croppingActive_changed(int)));
-
- connect(widget.checkBox_offsetInverted, SIGNAL(toggled(bool)), this, SLOT(cb_inverted_changed(bool)));
- connect(widget.pushButton_copyToClipboard, SIGNAL(clicked()), this, SLOT(btn_copyToClipboard_pressed()));
- connect(widget.pushButton_resetEndeffector, SIGNAL(clicked()), this, SLOT(btn_resetToModel_pressed()));
-
- connect(widget.doubleSpinBox_camera_x, SIGNAL(valueChanged(double)), this, SLOT(cameraNodeTransformationChanged(double)));
- connect(widget.doubleSpinBox_camera_y, SIGNAL(valueChanged(double)), this, SLOT(cameraNodeTransformationChanged(double)));
- connect(widget.doubleSpinBox_camera_z, SIGNAL(valueChanged(double)), this, SLOT(cameraNodeTransformationChanged(double)));
- connect(widget.doubleSpinBox_camera_roll, SIGNAL(valueChanged(double)), this, SLOT(cameraNodeTransformationChanged(double)));
- connect(widget.doubleSpinBox_camera_pitch, SIGNAL(valueChanged(double)), this, SLOT(cameraNodeTransformationChanged(double)));
- connect(widget.doubleSpinBox_camera_yaw, SIGNAL(valueChanged(double)), this, SLOT(cameraNodeTransformationChanged(double)));
-
- connect(widget.pushButton_backgroundColor, SIGNAL(clicked()), this, SLOT(btn_backgroundColor_pressed()));
- connect(widget.pushButton_pointcloudColor, SIGNAL(clicked()), this, SLOT(btn_pointCloudColor_pressed()));
- connect(widget.spinBox_pointSize, SIGNAL(valueChanged(int)), this, SLOT(sB_pointSize_changed(int)));
+ connect(widget.comboBox_endEffectorSelection,
+ SIGNAL(currentIndexChanged(QString)),
+ this,
+ SLOT(activeEndEffectorChanged(QString)));
+ connect(widget.horizontalSlider_croppingBox,
+ SIGNAL(valueChanged(int)),
+ this,
+ SLOT(slider_croppingRange_changed(int)));
+ connect(widget.checkBox_croppingActive,
+ SIGNAL(stateChanged(int)),
+ this,
+ SLOT(cb_croppingActive_changed(int)));
+
+ connect(widget.checkBox_offsetInverted,
+ SIGNAL(toggled(bool)),
+ this,
+ SLOT(cb_inverted_changed(bool)));
+ connect(widget.pushButton_copyToClipboard,
+ SIGNAL(clicked()),
+ this,
+ SLOT(btn_copyToClipboard_pressed()));
+ connect(widget.pushButton_resetEndeffector,
+ SIGNAL(clicked()),
+ this,
+ SLOT(btn_resetToModel_pressed()));
+
+ connect(widget.doubleSpinBox_camera_x,
+ SIGNAL(valueChanged(double)),
+ this,
+ SLOT(cameraNodeTransformationChanged(double)));
+ connect(widget.doubleSpinBox_camera_y,
+ SIGNAL(valueChanged(double)),
+ this,
+ SLOT(cameraNodeTransformationChanged(double)));
+ connect(widget.doubleSpinBox_camera_z,
+ SIGNAL(valueChanged(double)),
+ this,
+ SLOT(cameraNodeTransformationChanged(double)));
+ connect(widget.doubleSpinBox_camera_roll,
+ SIGNAL(valueChanged(double)),
+ this,
+ SLOT(cameraNodeTransformationChanged(double)));
+ connect(widget.doubleSpinBox_camera_pitch,
+ SIGNAL(valueChanged(double)),
+ this,
+ SLOT(cameraNodeTransformationChanged(double)));
+ connect(widget.doubleSpinBox_camera_yaw,
+ SIGNAL(valueChanged(double)),
+ this,
+ SLOT(cameraNodeTransformationChanged(double)));
+
+ connect(widget.pushButton_backgroundColor,
+ SIGNAL(clicked()),
+ this,
+ SLOT(btn_backgroundColor_pressed()));
+ connect(widget.pushButton_pointcloudColor,
+ SIGNAL(clicked()),
+ this,
+ SLOT(btn_pointCloudColor_pressed()));
+ connect(
+ widget.spinBox_pointSize, SIGNAL(valueChanged(int)), this, SLOT(sB_pointSize_changed(int)));
connect(widget.pushButton_changeFile, SIGNAL(clicked()), this, SLOT(selectFile()));
connect(widget.pushButton_saveDatapoint, SIGNAL(clicked()), this, SLOT(saveDatapoint()));
@@ -220,7 +279,8 @@ void HandEyeCalibrationWidgetController::onConnectPointCloudProcessor()
enableMainWidgetAsync(true);
}
-void HandEyeCalibrationWidgetController::connectQt()
+void
+HandEyeCalibrationWidgetController::connectQt()
{
pointCloudVisu = new PointCloudVisualization();
widget.displayWidget->getDisplay()->getRootNode()->addChild(pointCloudVisu);
@@ -242,7 +302,8 @@ void HandEyeCalibrationWidgetController::connectQt()
disableGuiElements();
}
-void HandEyeCalibrationWidgetController::onDisconnectPointCloudProcessor()
+void
+HandEyeCalibrationWidgetController::onDisconnectPointCloudProcessor()
{
ARMARX_INFO << "onDisconnectPointCloudProcessor";
@@ -258,7 +319,8 @@ void HandEyeCalibrationWidgetController::onDisconnectPointCloudProcessor()
providerBuffer.reset();
}
-void HandEyeCalibrationWidgetController::disconnectQt()
+void
+HandEyeCalibrationWidgetController::disconnectQt()
{
disableGuiElements();
@@ -271,12 +333,14 @@ void HandEyeCalibrationWidgetController::disconnectQt()
QObject::disconnect(widget.comboBox_endEffectorSelection, 0, 0, 0);
}
-void HandEyeCalibrationWidgetController::onExitPointCloudProcessor()
+void
+HandEyeCalibrationWidgetController::onExitPointCloudProcessor()
{
ARMARX_INFO << "onExitPointCloudProcessor";
}
-void HandEyeCalibrationWidgetController::setupEndEffectorSelection(VirtualRobot::RobotPtr robot)
+void
+HandEyeCalibrationWidgetController::setupEndEffectorSelection(VirtualRobot::RobotPtr robot)
{
try
{
@@ -293,30 +357,34 @@ void HandEyeCalibrationWidgetController::setupEndEffectorSelection(VirtualRobot:
{
widget.comboBox_endEffectorSelection->addItem(QString::fromStdString(e->getName()));
VirtualRobot::RobotPtr r = e->createEefRobot(e->getName(), e->getName());
- r->setGlobalPoseForRobotNode(r->getRobotNode(e->getTcp()->getName()), e->getTcp()->getGlobalPose());
- robotEEFMap.insert(std::make_pair(e->getName(), std::make_pair(r, Eigen::Matrix4f::Identity())));
+ r->setGlobalPoseForRobotNode(r->getRobotNode(e->getTcp()->getName()),
+ e->getTcp()->getGlobalPose());
+ robotEEFMap.insert(
+ std::make_pair(e->getName(), std::make_pair(r, Eigen::Matrix4f::Identity())));
}
widget.comboBox_endEffectorSelection->setCurrentIndex(-1);
}
-bool HandEyeCalibrationWidgetController::findStringIC(const std::string& strHaystack, const std::string& strNeedle) const
+bool
+HandEyeCalibrationWidgetController::findStringIC(const std::string& strHaystack,
+ const std::string& strNeedle) const
{
- auto it = std::search(
- strHaystack.begin(), strHaystack.end(),
- strNeedle.begin(), strNeedle.end(),
- [](char ch1, char ch2)
- {
- return std::toupper(ch1) == std::toupper(ch2);
- }
- );
+ auto it =
+ std::search(strHaystack.begin(),
+ strHaystack.end(),
+ strNeedle.begin(),
+ strNeedle.end(),
+ [](char ch1, char ch2) { return std::toupper(ch1) == std::toupper(ch2); });
return (it != strHaystack.end());
}
-void HandEyeCalibrationWidgetController::setupCameraNodeSlider(VirtualRobot::RobotPtr robot)
+void
+HandEyeCalibrationWidgetController::setupCameraNodeSlider(VirtualRobot::RobotPtr robot)
{
if (robot)
{
- ARMARX_CHECK_EXPRESSION(robot->hasRobotNode(referenceFrame)) << "Robot " + robot->getName() + " does not have node with name " + referenceFrame;
+ ARMARX_CHECK_EXPRESSION(robot->hasRobotNode(referenceFrame))
+ << "Robot " + robot->getName() + " does not have node with name " + referenceFrame;
widget.label_cameraNodeName->setText(QString::fromStdString((referenceFrame)));
cameraNode = robot->getRobotNode(referenceFrame);
@@ -331,7 +399,8 @@ void HandEyeCalibrationWidgetController::setupCameraNodeSlider(VirtualRobot::Rob
}
}
-VirtualRobot::RobotPtr HandEyeCalibrationWidgetController::loadRobotFromFile() const
+VirtualRobot::RobotPtr
+HandEyeCalibrationWidgetController::loadRobotFromFile() const
{
Ice::StringSeq includePaths;
@@ -350,7 +419,8 @@ VirtualRobot::RobotPtr HandEyeCalibrationWidgetController::loadRobotFromFile() c
CMakePackageFinder project(projectName);
auto pathsString = project.getDataDir();
Ice::StringSeq projectIncludePaths = armarx::Split(pathsString, ";,", true, true);
- includePaths.insert(includePaths.end(), projectIncludePaths.begin(), projectIncludePaths.end());
+ includePaths.insert(
+ includePaths.end(), projectIncludePaths.begin(), projectIncludePaths.end());
}
}
catch (...)
@@ -372,12 +442,14 @@ VirtualRobot::RobotPtr HandEyeCalibrationWidgetController::loadRobotFromFile() c
}
}
-std::string HandEyeCalibrationWidgetController::getProviderFrame() const
+std::string
+HandEyeCalibrationWidgetController::getProviderFrame() const
{
std::string frame = "Global";
try
{
- visionx::ReferenceFrameInterfacePrx refFramePrx = visionx::ReferenceFrameInterfacePrx::checkedCast(providerInfo.proxy);
+ visionx::ReferenceFrameInterfacePrx refFramePrx =
+ visionx::ReferenceFrameInterfacePrx::checkedCast(providerInfo.proxy);
if (refFramePrx)
{
frame = refFramePrx->getReferenceFrame();
@@ -390,7 +462,10 @@ std::string HandEyeCalibrationWidgetController::getProviderFrame() const
return frame;
}
-std::string HandEyeCalibrationWidgetController::formatTransformationMatrix(Eigen::Matrix4f mat, int decimalPlacesOrientation, int decimalPlacesPosition) const
+std::string
+HandEyeCalibrationWidgetController::formatTransformationMatrix(Eigen::Matrix4f mat,
+ int decimalPlacesOrientation,
+ int decimalPlacesPosition) const
{
std::stringstream ss;
ss.imbue(std::locale::classic());
@@ -430,7 +505,8 @@ std::string HandEyeCalibrationWidgetController::formatTransformationMatrix(Eigen
return ss.str();
}
-void HandEyeCalibrationWidgetController::enableGuiElements()
+void
+HandEyeCalibrationWidgetController::enableGuiElements()
{
widget.horizontalSlider_croppingBox->setEnabled(true);
widget.horizontalSlider_x->setEnabled(true);
@@ -460,7 +536,8 @@ void HandEyeCalibrationWidgetController::enableGuiElements()
showPointCloud = true;
}
-void HandEyeCalibrationWidgetController::disableGuiElements()
+void
+HandEyeCalibrationWidgetController::disableGuiElements()
{
widget.horizontalSlider_croppingBox->setEnabled(false);
widget.horizontalSlider_x->setEnabled(false);
@@ -490,11 +567,13 @@ void HandEyeCalibrationWidgetController::disableGuiElements()
showPointCloud = false;
}
-void HandEyeCalibrationWidgetController::updateManipulatorVisualization()
+void
+HandEyeCalibrationWidgetController::updateManipulatorVisualization()
{
if (activeEndEffector && manipulatorVisu)
{
- Eigen::Matrix4f modelPoseInRoot = localRobot->getRobotNode(activeEndEffector->getTcp()->getName())->getPoseInRootFrame();
+ Eigen::Matrix4f modelPoseInRoot =
+ localRobot->getRobotNode(activeEndEffector->getTcp()->getName())->getPoseInRootFrame();
Eigen::Matrix4f offset = getOffsetMatrixForEEF(activeEndEffector);
// Apply offset in root frame (multiplication order matters!!)
Eigen::Matrix4f pointCloudInRoot = offset * modelPoseInRoot;
@@ -504,22 +583,26 @@ void HandEyeCalibrationWidgetController::updateManipulatorVisualization()
manipulatorVisu->setUserDesiredPose(pointCloudGlobal);
// Update the local eefRobot
- getEEFRobot(activeEndEffector)->setGlobalPoseForRobotNode(activeEndEffector->getTcp(), pointCloudGlobal);
+ getEEFRobot(activeEndEffector)
+ ->setGlobalPoseForRobotNode(activeEndEffector->getTcp(), pointCloudGlobal);
// Update Matrix label
if (widget.checkBox_offsetInverted->isChecked())
{
offset = offset.inverse();
}
- widget.label_offsetMatrix->setText(QString::fromStdString(formatTransformationMatrix(offset)));
+ widget.label_offsetMatrix->setText(
+ QString::fromStdString(formatTransformationMatrix(offset)));
}
}
-void HandEyeCalibrationWidgetController::process()
+void
+HandEyeCalibrationWidgetController::process()
{
if (!waitForPointClouds(providerName, 500))
{
- ARMARX_WARNING << "Timeout or error in waiting for pointclouds from provider: " << providerName;
+ ARMARX_WARNING << "Timeout or error in waiting for pointclouds from provider: "
+ << providerName;
}
else
{
@@ -529,7 +612,8 @@ void HandEyeCalibrationWidgetController::process()
}
}
-void HandEyeCalibrationWidgetController::processPointCloud()
+void
+HandEyeCalibrationWidgetController::processPointCloud()
{
if (showPointCloud && pointcloudRobot)
{
@@ -545,7 +629,8 @@ void HandEyeCalibrationWidgetController::processPointCloud()
try
{
- RemoteRobot::synchronizeLocalCloneToTimestamp(pointcloudRobot, robotStateComponentPrx, info->timeProvided);
+ RemoteRobot::synchronizeLocalCloneToTimestamp(
+ pointcloudRobot, robotStateComponentPrx, info->timeProvided);
}
catch (...)
{
@@ -563,12 +648,14 @@ void HandEyeCalibrationWidgetController::processPointCloud()
Eigen::Matrix4f eefGlobal = manipulatorVisu->getUserDesiredPose();
Eigen::Matrix4f eefInReferenceFrame = sourceFrameGlobalPose.inverse() * eefGlobal;
- float cropRange = (float) widget.horizontalSlider_croppingBox->value();
+ float cropRange = (float)widget.horizontalSlider_croppingBox->value();
Eigen::Vector3f cropMin;
- cropMin << eefInReferenceFrame(0, 3) - cropRange, eefInReferenceFrame(1, 3) - cropRange, eefInReferenceFrame(2, 3) - cropRange;
+ cropMin << eefInReferenceFrame(0, 3) - cropRange, eefInReferenceFrame(1, 3) - cropRange,
+ eefInReferenceFrame(2, 3) - cropRange;
Eigen::Vector3f cropMax;
- cropMax << eefInReferenceFrame(0, 3) + cropRange, eefInReferenceFrame(1, 3) + cropRange, eefInReferenceFrame(2, 3) + cropRange;
+ cropMax << eefInReferenceFrame(0, 3) + cropRange, eefInReferenceFrame(1, 3) + cropRange,
+ eefInReferenceFrame(2, 3) + cropRange;
pcl::PassThrough<PointT> pass;
@@ -603,7 +690,8 @@ void HandEyeCalibrationWidgetController::processPointCloud()
}
}
-void HandEyeCalibrationWidgetController::cb_inverted_changed(bool checked)
+void
+HandEyeCalibrationWidgetController::cb_inverted_changed(bool checked)
{
if (checked)
{
@@ -615,7 +703,8 @@ void HandEyeCalibrationWidgetController::cb_inverted_changed(bool checked)
}
}
-void HandEyeCalibrationWidgetController::btn_copyToClipboard_pressed()
+void
+HandEyeCalibrationWidgetController::btn_copyToClipboard_pressed()
{
if (activeEndEffector)
{
@@ -636,34 +725,43 @@ void HandEyeCalibrationWidgetController::btn_copyToClipboard_pressed()
}
}
-void HandEyeCalibrationWidgetController::btn_resetToModel_pressed()
+void
+HandEyeCalibrationWidgetController::btn_resetToModel_pressed()
{
if (activeEndEffector)
{
setOffsetMatrixForEEF(activeEndEffector, Eigen::Matrix4f::Identity());
updateManipulatorVisualization();
- widget.displayWidget->getDisplay()->cameraViewNode((SoNode*) manipulatorVisu, 1.0);
+ widget.displayWidget->getDisplay()->cameraViewNode((SoNode*)manipulatorVisu, 1.0);
}
}
-void HandEyeCalibrationWidgetController::btn_backgroundColor_pressed()
+void
+HandEyeCalibrationWidgetController::btn_backgroundColor_pressed()
{
QColor selectedColor = this->colorDialog.getColor();
- widget.displayWidget->getDisplay()->setBackgroundColor(SbColor(selectedColor.red() / 255.0f, selectedColor.green() / 255.0f, selectedColor.blue() / 255.0f));
+ widget.displayWidget->getDisplay()->setBackgroundColor(SbColor(selectedColor.red() / 255.0f,
+ selectedColor.green() / 255.0f,
+ selectedColor.blue() / 255.0f));
}
-void HandEyeCalibrationWidgetController::btn_pointCloudColor_pressed()
+void
+HandEyeCalibrationWidgetController::btn_pointCloudColor_pressed()
{
QColor selectedColor = this->colorDialog.getColor();
- this->pointCloudVisu->setDrawColor(SbColor(selectedColor.red() / 255.0f, selectedColor.green() / 255.0f, selectedColor.blue() / 255.0f));
+ this->pointCloudVisu->setDrawColor(SbColor(selectedColor.red() / 255.0f,
+ selectedColor.green() / 255.0f,
+ selectedColor.blue() / 255.0f));
}
-void HandEyeCalibrationWidgetController::sB_pointSize_changed(int value)
+void
+HandEyeCalibrationWidgetController::sB_pointSize_changed(int value)
{
this->pointCloudVisu->setPointSize(value);
}
-void HandEyeCalibrationWidgetController::cameraNodeTransformationChanged(double value)
+void
+HandEyeCalibrationWidgetController::cameraNodeTransformationChanged(double value)
{
if (cameraNode)
{
@@ -671,39 +769,41 @@ void HandEyeCalibrationWidgetController::cameraNodeTransformationChanged(double
QObject* obj = sender();
if (obj == widget.doubleSpinBox_camera_x)
{
- t(0) = (float) value;
+ t(0) = (float)value;
}
else if (obj == widget.doubleSpinBox_camera_y)
{
- t(1) = (float) value;
+ t(1) = (float)value;
}
else if (obj == widget.doubleSpinBox_camera_z)
{
- t(2) = (float) value;
+ t(2) = (float)value;
}
else if (obj == widget.doubleSpinBox_camera_roll)
{
- t(3) = VirtualRobot::MathTools::deg2rad((float) value);
+ t(3) = VirtualRobot::MathTools::deg2rad((float)value);
}
else if (obj == widget.doubleSpinBox_camera_pitch)
{
- t(4) = VirtualRobot::MathTools::deg2rad((float) value);
+ t(4) = VirtualRobot::MathTools::deg2rad((float)value);
}
else if (obj == widget.doubleSpinBox_camera_yaw)
{
- t(5) = VirtualRobot::MathTools::deg2rad((float) value);
+ t(5) = VirtualRobot::MathTools::deg2rad((float)value);
}
- Eigen::Matrix4f transformation = VirtualRobot::MathTools::posrpy2eigen4f(t(0), t(1), t(2), t(3), t(4), t(5));
+ Eigen::Matrix4f transformation =
+ VirtualRobot::MathTools::posrpy2eigen4f(t(0), t(1), t(2), t(3), t(4), t(5));
cameraNode->setLocalTransformation(transformation);
}
}
-void HandEyeCalibrationWidgetController::taskEEFManipulationCB()
+void
+HandEyeCalibrationWidgetController::taskEEFManipulationCB()
{
float x[3];
- x[0] = (float) widget.horizontalSlider_x->value();
- x[1] = (float) widget.horizontalSlider_y->value();
- x[2] = (float) widget.horizontalSlider_z->value();
+ x[0] = (float)widget.horizontalSlider_x->value();
+ x[1] = (float)widget.horizontalSlider_y->value();
+ x[2] = (float)widget.horizontalSlider_z->value();
if (activeEndEffector)
{
Eigen::Matrix4f offsetInRoot = Eigen::Matrix4f::Identity();
@@ -715,7 +815,8 @@ void HandEyeCalibrationWidgetController::taskEEFManipulationCB()
}
}
-void HandEyeCalibrationWidgetController::taskLocalRobotUpdateCB()
+void
+HandEyeCalibrationWidgetController::taskLocalRobotUpdateCB()
{
if (!robotStateComponentPrx)
{
@@ -731,7 +832,8 @@ void HandEyeCalibrationWidgetController::taskLocalRobotUpdateCB()
}
}
-void HandEyeCalibrationWidgetController::activeEndEffectorChanged(QString endEffectorName)
+void
+HandEyeCalibrationWidgetController::activeEndEffectorChanged(QString endEffectorName)
{
if (localRobot && manipulatorVisu)
{
@@ -742,23 +844,26 @@ void HandEyeCalibrationWidgetController::activeEndEffectorChanged(QString endEff
{
activeEndEffector = robotEEFMap.at(eefName).first->getEndEffector(eefName);
manipulatorVisu->setVisualization(activeEndEffector);
- widget.displayWidget->getDisplay()->cameraViewNode((SoNode*) manipulatorVisu, 1.0);
+ widget.displayWidget->getDisplay()->cameraViewNode((SoNode*)manipulatorVisu, 1.0);
enableGuiElements();
}
else
{
- ARMARX_ERROR << "Robot " << localRobot->getName() << " does not have endeffector with name " << eefName;
+ ARMARX_ERROR << "Robot " << localRobot->getName()
+ << " does not have endeffector with name " << eefName;
}
}
}
}
-void HandEyeCalibrationWidgetController::slider_croppingRange_changed(int value)
+void
+HandEyeCalibrationWidgetController::slider_croppingRange_changed(int value)
{
this->widget.label_croppingBoxValue->setText(QString::fromStdString(ValueToString(value)));
}
-void HandEyeCalibrationWidgetController::cb_croppingActive_changed(int state)
+void
+HandEyeCalibrationWidgetController::cb_croppingActive_changed(int state)
{
if (state == Qt::Unchecked)
{
@@ -769,5 +874,3 @@ void HandEyeCalibrationWidgetController::cb_croppingActive_changed(int state)
this->croppingActive = false;
}
}
-
-
diff --git a/source/RobotComponents/gui-plugins/HandEyeCalibration/HandEyeCalibrationWidgetController.h b/source/RobotComponents/gui-plugins/HandEyeCalibration/HandEyeCalibrationWidgetController.h
index 0edbc8f14fb7abf42914c4242b988f5cded8a2a4..42a319f50a27e98c5704f43b845bbeb150d02f23 100644
--- a/source/RobotComponents/gui-plugins/HandEyeCalibration/HandEyeCalibrationWidgetController.h
+++ b/source/RobotComponents/gui-plugins/HandEyeCalibration/HandEyeCalibrationWidgetController.h
@@ -21,30 +21,32 @@
*/
#pragma once
-#include <RobotComponents/gui-plugins/HandEyeCalibration/ui_HandEyeCalibrationWidget.h>
-#include <Inventor/sensors/SoTimerSensor.h>
+#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
+#include <ArmarXCore/core/system/ImportExportComponent.h>
-#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
#include <ArmarXGui/libraries/SimpleConfigDialog/SimpleConfigDialog.h>
-#include <ArmarXCore/core/system/ImportExportComponent.h>
-#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
#include <RobotAPI/interface/core/RobotState.h>
+#include <RobotComponents/gui-plugins/HandEyeCalibration/ui_HandEyeCalibrationWidget.h>
+
+#include <Inventor/sensors/SoTimerSensor.h>
+
//Point Cloud Processor
#include <VisionX/components/pointcloud_core/PointCloudProcessor.h>
//VisionX
-#include <VisionX/interface/core/PointCloudProviderInterface.h>
#include <VisionX/components/pointcloud_core/PointCloudProvider.h>
+#include <VisionX/interface/core/PointCloudProviderInterface.h>
// Qt
#include <QColorDialog>
#include <QFile>
-#include "PointCloudVisualization.h"
#include "ManipulatorVisualization.h"
+#include "PointCloudVisualization.h"
namespace armarx
{
@@ -66,9 +68,8 @@ namespace armarx
*
* Detailed description
*/
- class ARMARXCOMPONENT_IMPORT_EXPORT
- HandEyeCalibrationWidgetController:
- public armarx::ArmarXComponentWidgetControllerTemplate < HandEyeCalibrationWidgetController >,
+ class ARMARXCOMPONENT_IMPORT_EXPORT HandEyeCalibrationWidgetController :
+ public armarx::ArmarXComponentWidgetControllerTemplate<HandEyeCalibrationWidgetController>,
public visionx::PointCloudProcessor
{
Q_OBJECT
@@ -98,10 +99,12 @@ namespace armarx
* Returns the Widget name displayed in the ArmarXGui to create an
* instance of this class.
*/
- static QString GetWidgetName()
+ static QString
+ GetWidgetName()
{
return "RobotControl.HandEyeCalibrationGUI";
}
+
QPointer<QDialog> getConfigDialog(QWidget* parent = 0) override;
void configured() override;
@@ -117,8 +120,11 @@ namespace armarx
void setupCameraNodeSlider(VirtualRobot::RobotPtr robot);
VirtualRobot::RobotPtr loadRobotFromFile() const;
std::string getProviderFrame() const;
- void cropOriginalPointCloud(const pcl::PointCloud<PointT>& cloud, const VirtualRobot::EndEffectorPtr eef) const;
- std::string formatTransformationMatrix(Eigen::Matrix4f mat, int decimalPlacesOrientation = 6, int decimalPlacesPosition = 2) const;
+ void cropOriginalPointCloud(const pcl::PointCloud<PointT>& cloud,
+ const VirtualRobot::EndEffectorPtr eef) const;
+ std::string formatTransformationMatrix(Eigen::Matrix4f mat,
+ int decimalPlacesOrientation = 6,
+ int decimalPlacesPosition = 2) const;
void enableGuiElements();
void disableGuiElements();
void updateManipulatorVisualization();
@@ -176,8 +182,10 @@ namespace armarx
VirtualRobot::EndEffectorPtr activeEndEffector;
std::map<std::string, std::pair<VirtualRobot::RobotPtr, Eigen::Matrix4f>> robotEEFMap;
VirtualRobot::RobotPtr getEEFRobot(VirtualRobot::EndEffectorPtr eef) const;
- Eigen::Matrix4f getOffsetMatrixForEEF(const VirtualRobot::EndEffectorPtr eef) const; // OffsetMatrix is stored from Model to Pointcloud
- void setOffsetMatrixForEEF(const VirtualRobot::EndEffectorPtr eef, const Eigen::Matrix4f& offset);
+ Eigen::Matrix4f getOffsetMatrixForEEF(const VirtualRobot::EndEffectorPtr eef)
+ const; // OffsetMatrix is stored from Model to Pointcloud
+ void setOffsetMatrixForEEF(const VirtualRobot::EndEffectorPtr eef,
+ const Eigen::Matrix4f& offset);
bool croppingActive = true;
@@ -190,8 +198,5 @@ namespace armarx
QColorDialog colorDialog;
VirtualRobot::RobotNodePtr cameraNode;
-
};
-}
-
-
+} // namespace armarx
diff --git a/source/RobotComponents/gui-plugins/HandEyeCalibration/ManipulatorVisualization.cpp b/source/RobotComponents/gui-plugins/HandEyeCalibration/ManipulatorVisualization.cpp
index 44e02e8dcbe3c1d544d80fd024b15a93b0f6fa0e..cf95c3fb8a45add21976e48c2589ff9275df6d97 100644
--- a/source/RobotComponents/gui-plugins/HandEyeCalibration/ManipulatorVisualization.cpp
+++ b/source/RobotComponents/gui-plugins/HandEyeCalibration/ManipulatorVisualization.cpp
@@ -23,22 +23,25 @@
#include "ManipulatorVisualization.h"
//Virtual Robot includes
-#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
-#include <VirtualRobot/XML/RobotIO.h>
#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>
+#include <VirtualRobot/XML/RobotIO.h>
+
+#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
/* Coin includes */
+#include <iostream>
+
+#include <Inventor/SbViewportRegion.h>
#include <Inventor/actions/SoGetMatrixAction.h>
#include <Inventor/actions/SoSearchAction.h>
-#include <Inventor/SbViewportRegion.h>
#include <Inventor/nodes/SoCube.h>
-
-#include <iostream>
-
using namespace armarx;
-ManipulatorVisualization::ManipulatorVisualization() : isVisualizing(false), hasEndEffectorVisualizer(false), localTransformation(Eigen::Matrix4f::Identity())
+ManipulatorVisualization::ManipulatorVisualization() :
+ isVisualizing(false),
+ hasEndEffectorVisualizer(false),
+ localTransformation(Eigen::Matrix4f::Identity())
{
this->ref();
}
@@ -49,7 +52,8 @@ ManipulatorVisualization::~ManipulatorVisualization()
this->unref();
}
-void ManipulatorVisualization::setVisualization(VirtualRobot::EndEffectorPtr endEffector)
+void
+ManipulatorVisualization::setVisualization(VirtualRobot::EndEffectorPtr endEffector)
{
//Completely forget anything we displayed earlier
//This also works when this is the first time we display something
@@ -67,7 +71,8 @@ void ManipulatorVisualization::setVisualization(VirtualRobot::EndEffectorPtr end
if (this->hasEndEffectorVisualizer)
{
VirtualRobot::RobotPtr endEffectorRobot = endEffector->createEefRobot("", "");
- VirtualRobot::CoinVisualizationPtr endEffectorVisualization = endEffectorRobot->getVisualization<VirtualRobot::CoinVisualization>();
+ VirtualRobot::CoinVisualizationPtr endEffectorVisualization =
+ endEffectorRobot->getVisualization();
this->addChild(endEffectorVisualization->getCoinVisualization());
}
else
@@ -119,7 +124,8 @@ void ManipulatorVisualization::setVisualization(VirtualRobot::EndEffectorPtr end
if (this->hasEndEffectorVisualizer)
{
//Stores the local transformation from the endeffector base to the TCP node
- this->localTransformation = endEffector->getBase()->getTransformationTo(endEffector->getTcp());
+ this->localTransformation =
+ endEffector->getBase()->getTransformationTo(endEffector->getTcp());
globalMat = endEffector->getBase()->getGlobalPose();
}
@@ -135,7 +141,8 @@ void ManipulatorVisualization::setVisualization(VirtualRobot::EndEffectorPtr end
this->isVisualizing = true;
}
-void ManipulatorVisualization::removeVisualization()
+void
+ManipulatorVisualization::removeVisualization()
{
//Remove all children and reset manip pointer
//This should bring ref counter of Inventor down to zero and free memory
@@ -147,7 +154,8 @@ void ManipulatorVisualization::removeVisualization()
this->localTransformation = Eigen::Matrix4f::Identity();
}
-void ManipulatorVisualization::setColor(float r, float g, float b)
+void
+ManipulatorVisualization::setColor(float r, float g, float b)
{
if (this->getIsVisualizing())
{
@@ -155,15 +163,16 @@ void ManipulatorVisualization::setColor(float r, float g, float b)
}
}
-Eigen::Matrix4f ManipulatorVisualization::getUserDesiredPose()
+Eigen::Matrix4f
+ManipulatorVisualization::getUserDesiredPose()
{
if (this->getIsVisualizing())
{
SoGetMatrixAction* action = new SoGetMatrixAction(SbViewportRegion());
SoSearchAction sa;
sa.setNode(manip.get());
- sa.setSearchingAll(TRUE); // Search all nodes
- SoBaseKit::setSearchingChildren(TRUE); // Even inside nodekits
+ sa.setSearchingAll(TRUE); // Search all nodes
+ SoBaseKit::setSearchingChildren(TRUE); // Even inside nodekits
sa.apply(this);
action->apply(sa.getPath());
@@ -205,7 +214,8 @@ Eigen::Matrix4f ManipulatorVisualization::getUserDesiredPose()
return Eigen::Matrix4f::Identity();
}
-std::string ManipulatorVisualization::getUserDesiredPoseString()
+std::string
+ManipulatorVisualization::getUserDesiredPoseString()
{
Eigen::Matrix4f mat = this->getUserDesiredPose();
@@ -215,7 +225,8 @@ std::string ManipulatorVisualization::getUserDesiredPoseString()
return buffer.str();
}
-void ManipulatorVisualization::setUserDesiredPose(Eigen::Matrix4f globalPose)
+void
+ManipulatorVisualization::setUserDesiredPose(Eigen::Matrix4f globalPose)
{
if (manip)
{
diff --git a/source/RobotComponents/gui-plugins/HandEyeCalibration/ManipulatorVisualization.h b/source/RobotComponents/gui-plugins/HandEyeCalibration/ManipulatorVisualization.h
index e245ca8b21b3a2de063d8a1c80411b42c68a1eb9..d9ed3670237646e7eb9ed3a7674d5e7c5782f45e 100644
--- a/source/RobotComponents/gui-plugins/HandEyeCalibration/ManipulatorVisualization.h
+++ b/source/RobotComponents/gui-plugins/HandEyeCalibration/ManipulatorVisualization.h
@@ -23,9 +23,9 @@
#pragma once
//Coin includes
-#include <Inventor/nodes/SoSeparator.h>
#include <Inventor/manips/SoTransformerManip.h>
#include <Inventor/nodes/SoMaterial.h>
+#include <Inventor/nodes/SoSeparator.h>
//VirtualRobot
#include <VirtualRobot/Robot.h>
@@ -39,11 +39,15 @@ namespace armarx
//Let boost use referencing of Inventor to manage objects memory
//and not its own shared pointer referencing
using SoTransformerManipPtr = boost::intrusive_ptr<SoTransformerManip>;
- inline void intrusive_ptr_add_ref(SoTransformerManip* obj)
+
+ inline void
+ intrusive_ptr_add_ref(SoTransformerManip* obj)
{
obj->ref();
}
- inline void intrusive_ptr_release(SoTransformerManip* obj)
+
+ inline void
+ intrusive_ptr_release(SoTransformerManip* obj)
{
obj->unref();
}
@@ -63,7 +67,8 @@ namespace armarx
std::string getUserDesiredPoseString();
void setUserDesiredPose(Eigen::Matrix4f globalPose);
- bool getIsVisualizing() const
+ bool
+ getIsVisualizing() const
{
return isVisualizing;
}
@@ -75,6 +80,4 @@ namespace armarx
bool hasEndEffectorVisualizer;
Eigen::Matrix4f localTransformation;
};
-}
-
-
+} // namespace armarx
diff --git a/source/RobotComponents/gui-plugins/HandEyeCalibration/PointCloudVisualization.cpp b/source/RobotComponents/gui-plugins/HandEyeCalibration/PointCloudVisualization.cpp
index 73498d0951946830b9eec16ad32ec2e19a8eeb41..1223a83470bff0e55f5ce8df06ce8d0c725b2e11 100644
--- a/source/RobotComponents/gui-plugins/HandEyeCalibration/PointCloudVisualization.cpp
+++ b/source/RobotComponents/gui-plugins/HandEyeCalibration/PointCloudVisualization.cpp
@@ -23,16 +23,16 @@
#include "PointCloudVisualization.h"
//Coin includes
-#include <Inventor/nodes/SoMaterial.h>
-#include <Inventor/nodes/SoMaterialBinding.h>
+#include <ArmarXCore/core/logging/Logging.h>
+
+#include <Inventor/SbVec3f.h>
#include <Inventor/nodes/SoCoordinate3.h>
#include <Inventor/nodes/SoDrawStyle.h>
+#include <Inventor/nodes/SoMaterial.h>
+#include <Inventor/nodes/SoMaterialBinding.h>
#include <Inventor/nodes/SoPointSet.h>
-#include <Inventor/SbVec3f.h>
-#include <Inventor/nodes/SoTransform.h>
#include <Inventor/nodes/SoSphere.h>
-
-#include <ArmarXCore/core/logging/Logging.h>
+#include <Inventor/nodes/SoTransform.h>
using namespace armarx;
@@ -47,7 +47,8 @@ PointCloudVisualization::~PointCloudVisualization()
this->unref();
}
-void PointCloudVisualization::setVisualization(pcl::PointCloud<PointT>::ConstPtr cloud)
+void
+PointCloudVisualization::setVisualization(pcl::PointCloud<PointT>::ConstPtr cloud)
{
// Clear all previous visualization
this->removeAllChildren();
@@ -69,9 +70,9 @@ void PointCloudVisualization::setVisualization(pcl::PointCloud<PointT>::ConstPtr
{
float colorArray[3];
- colorArray[0] = (float) p.r / 256.0f;
- colorArray[1] = (float) p.g / 256.0f;
- colorArray[2] = (float) p.b / 256.0f;
+ colorArray[0] = (float)p.r / 256.0f;
+ colorArray[1] = (float)p.g / 256.0f;
+ colorArray[2] = (float)p.b / 256.0f;
colorContainer.setValue(colorArray);
}
@@ -113,13 +114,15 @@ void PointCloudVisualization::setVisualization(pcl::PointCloud<PointT>::ConstPtr
this->addChild(pointSet);
}
-void PointCloudVisualization::setDrawColor(SbColor color)
+void
+PointCloudVisualization::setDrawColor(SbColor color)
{
this->color = color;
this->useOriginalColors = false;
}
-void PointCloudVisualization::setPointSize(int size)
+void
+PointCloudVisualization::setPointSize(int size)
{
if (size < 1)
{
@@ -128,7 +131,8 @@ void PointCloudVisualization::setPointSize(int size)
this->pointSize = size;
}
-void PointCloudVisualization::resetDrawColor()
+void
+PointCloudVisualization::resetDrawColor()
{
this->useOriginalColors = true;
}
diff --git a/source/RobotComponents/gui-plugins/HandEyeCalibration/PointCloudVisualization.h b/source/RobotComponents/gui-plugins/HandEyeCalibration/PointCloudVisualization.h
index c1be2e228935cabdabaced7ce19d36ea55c9aead..1cb889d55a403a2dba0bd26405ef1383bc354ab0 100644
--- a/source/RobotComponents/gui-plugins/HandEyeCalibration/PointCloudVisualization.h
+++ b/source/RobotComponents/gui-plugins/HandEyeCalibration/PointCloudVisualization.h
@@ -22,13 +22,13 @@
#pragma once
//PCL
-#include <pcl/point_types.h>
-#include <pcl/point_cloud.h>
#include <pcl/filters/filter.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
//Coin includes
-#include <Inventor/nodes/SoSeparator.h>
#include <Inventor/SbColor.h>
+#include <Inventor/nodes/SoSeparator.h>
namespace armarx
{
@@ -44,9 +44,10 @@ namespace armarx
void setDrawColor(SbColor color);
void setPointSize(int size);
void resetDrawColor();
+
private:
SbColor color;
bool useOriginalColors = true;
int pointSize = 1;
};
-}
+} // namespace armarx
diff --git a/source/RobotComponents/gui-plugins/LaserScannerSelfLocalisation/LaserScannerSelfLocalisationGuiPlugin.cpp b/source/RobotComponents/gui-plugins/LaserScannerSelfLocalisation/LaserScannerSelfLocalisationGuiPlugin.cpp
index 5958db4d5161e1e4e9bc67957be33df68fb3381b..dfc19c0e7d5da9400b457618a1f13039e9a6a118 100644
--- a/source/RobotComponents/gui-plugins/LaserScannerSelfLocalisation/LaserScannerSelfLocalisationGuiPlugin.cpp
+++ b/source/RobotComponents/gui-plugins/LaserScannerSelfLocalisation/LaserScannerSelfLocalisationGuiPlugin.cpp
@@ -28,5 +28,5 @@ using namespace armarx;
LaserScannerSelfLocalisationGuiPlugin::LaserScannerSelfLocalisationGuiPlugin()
{
- addWidget < LaserScannerSelfLocalisationWidgetController > ();
+ addWidget<LaserScannerSelfLocalisationWidgetController>();
}
diff --git a/source/RobotComponents/gui-plugins/LaserScannerSelfLocalisation/LaserScannerSelfLocalisationGuiPlugin.h b/source/RobotComponents/gui-plugins/LaserScannerSelfLocalisation/LaserScannerSelfLocalisationGuiPlugin.h
index ad24a004b2595702303d519e79b174f52ec91399..9ab8a5f9f0c4dd8b1fd89af1536c1ad572639219 100644
--- a/source/RobotComponents/gui-plugins/LaserScannerSelfLocalisation/LaserScannerSelfLocalisationGuiPlugin.h
+++ b/source/RobotComponents/gui-plugins/LaserScannerSelfLocalisation/LaserScannerSelfLocalisationGuiPlugin.h
@@ -23,8 +23,9 @@
#pragma once
#include <ArmarXCore/core/system/ImportExportComponent.h>
-#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
+
#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
namespace armarx
{
@@ -35,7 +36,7 @@ namespace armarx
*
* Detailed description
*/
- class ARMARXCOMPONENT_IMPORT_EXPORT LaserScannerSelfLocalisationGuiPlugin:
+ class ARMARXCOMPONENT_IMPORT_EXPORT LaserScannerSelfLocalisationGuiPlugin :
public armarx::ArmarXGuiPlugin
{
Q_OBJECT
@@ -48,5 +49,4 @@ namespace armarx
*/
LaserScannerSelfLocalisationGuiPlugin();
};
-}
-
+} // namespace armarx
diff --git a/source/RobotComponents/gui-plugins/LaserScannerSelfLocalisation/LaserScannerSelfLocalisationWidgetController.cpp b/source/RobotComponents/gui-plugins/LaserScannerSelfLocalisation/LaserScannerSelfLocalisationWidgetController.cpp
index be85a72764838a40a9bb6ecd7eeb239c34453c4d..f192bc0595dc9d853092a1e4c858e776a9eef2ef 100644
--- a/source/RobotComponents/gui-plugins/LaserScannerSelfLocalisation/LaserScannerSelfLocalisationWidgetController.cpp
+++ b/source/RobotComponents/gui-plugins/LaserScannerSelfLocalisation/LaserScannerSelfLocalisationWidgetController.cpp
@@ -22,55 +22,50 @@
#include "LaserScannerSelfLocalisationWidgetController.h"
-#include <QSpinBox>
-#include <QDoubleSpinBox>
+#include <cfloat>
+#include <string>
+
#include <QCheckBox>
+#include <QDoubleSpinBox>
+#include <QSpinBox>
#include <QTimer>
-#include <string>
-#include <cfloat>
-
using namespace armarx;
-LaserScannerSelfLocalisationWidgetController::LaserScannerSelfLocalisationWidgetController()
- : setPoseState(SetPoseState::None)
+LaserScannerSelfLocalisationWidgetController::LaserScannerSelfLocalisationWidgetController() :
+ setPoseState(SetPoseState::None)
{
widget.setupUi(getWidget());
painterWidget = new QPainterWidget();
painterWidget->setupUi(widget.frame);
- painterWidget->setPaintCallback([this](QPainter & painter)
- {
- onPaintCanvas(painter, painterWidget->size());
- });
+ painterWidget->setPaintCallback([this](QPainter& painter)
+ { onPaintCanvas(painter, painterWidget->size()); });
paintTimerId = startTimer(16);
}
-
LaserScannerSelfLocalisationWidgetController::~LaserScannerSelfLocalisationWidgetController()
{
-
}
-
-void LaserScannerSelfLocalisationWidgetController::loadSettings(QSettings* settings)
+void
+LaserScannerSelfLocalisationWidgetController::loadSettings(QSettings* settings)
{
-
}
-void LaserScannerSelfLocalisationWidgetController::saveSettings(QSettings* settings)
+void
+LaserScannerSelfLocalisationWidgetController::saveSettings(QSettings* settings)
{
-
}
-
-void LaserScannerSelfLocalisationWidgetController::onInitComponent()
+void
+LaserScannerSelfLocalisationWidgetController::onInitComponent()
{
usingProxy(localisationName);
}
-
-void LaserScannerSelfLocalisationWidgetController::onConnectComponent()
+void
+LaserScannerSelfLocalisationWidgetController::onConnectComponent()
{
localisation = getProxy<LaserScannerSelfLocalisationInterfacePrx>(localisationName);
localisationManager = getProxy<armarx::ArmarXManagerInterfacePrx>(localisationName + "Manager");
@@ -79,14 +74,34 @@ void LaserScannerSelfLocalisationWidgetController::onConnectComponent()
map = localisation->getMap();
- connect(widget.smoothingFrameSize, SIGNAL(valueChanged(int)), this, SLOT(onSpinBoxChanged(int)));
- connect(widget.smoothingMergeDistance, SIGNAL(valueChanged(int)), this, SLOT(onSpinBoxChanged(int)));
- connect(widget.matchingMaxDistance, SIGNAL(valueChanged(double)), this, SLOT(onSpinBoxChanged(double)));
- connect(widget.matchingMinPoints, SIGNAL(valueChanged(double)), this, SLOT(onSpinBoxChanged(double)));
- connect(widget.matchingCorrectionFactor, SIGNAL(valueChanged(double)), this, SLOT(onSpinBoxChanged(double)));
- connect(widget.edgeMaxDistance, SIGNAL(valueChanged(double)), this, SLOT(onSpinBoxChanged(double)));
- connect(widget.edgeMaxDeltaAngle, SIGNAL(valueChanged(double)), this, SLOT(onSpinBoxChanged(double)));
- connect(widget.edgePointAddingThreshold, SIGNAL(valueChanged(double)), this, SLOT(onSpinBoxChanged(double)));
+ connect(
+ widget.smoothingFrameSize, SIGNAL(valueChanged(int)), this, SLOT(onSpinBoxChanged(int)));
+ connect(widget.smoothingMergeDistance,
+ SIGNAL(valueChanged(int)),
+ this,
+ SLOT(onSpinBoxChanged(int)));
+ connect(widget.matchingMaxDistance,
+ SIGNAL(valueChanged(double)),
+ this,
+ SLOT(onSpinBoxChanged(double)));
+ connect(widget.matchingMinPoints,
+ SIGNAL(valueChanged(double)),
+ this,
+ SLOT(onSpinBoxChanged(double)));
+ connect(widget.matchingCorrectionFactor,
+ SIGNAL(valueChanged(double)),
+ this,
+ SLOT(onSpinBoxChanged(double)));
+ connect(
+ widget.edgeMaxDistance, SIGNAL(valueChanged(double)), this, SLOT(onSpinBoxChanged(double)));
+ connect(widget.edgeMaxDeltaAngle,
+ SIGNAL(valueChanged(double)),
+ this,
+ SLOT(onSpinBoxChanged(double)));
+ connect(widget.edgePointAddingThreshold,
+ SIGNAL(valueChanged(double)),
+ this,
+ SLOT(onSpinBoxChanged(double)));
connect(widget.edgeEpsilon, SIGNAL(valueChanged(int)), this, SLOT(onSpinBoxChanged(int)));
connect(widget.edgeMinPoints, SIGNAL(valueChanged(int)), this, SLOT(onSpinBoxChanged(int)));
connect(widget.useMapCorrection, SIGNAL(stateChanged(int)), this, SLOT(onSpinBoxChanged(int)));
@@ -94,8 +109,10 @@ void LaserScannerSelfLocalisationWidgetController::onConnectComponent()
connect(widget.reportPoints, SIGNAL(stateChanged(int)), this, SLOT(onSpinBoxChanged(int)));
connect(widget.reportEdges, SIGNAL(stateChanged(int)), this, SLOT(onSpinBoxChanged(int)));
connect(widget.setPoseButton, SIGNAL(clicked()), this, SLOT(onSetPoseClick()));
- connect(widget.sensorStdDev, SIGNAL(valueChanged(double)), this, SLOT(onSpinBoxChanged(double)));
- connect(widget.velSensorStdDev, SIGNAL(valueChanged(double)), this, SLOT(onSpinBoxChanged(double)));
+ connect(
+ widget.sensorStdDev, SIGNAL(valueChanged(double)), this, SLOT(onSpinBoxChanged(double)));
+ connect(
+ widget.velSensorStdDev, SIGNAL(valueChanged(double)), this, SLOT(onSpinBoxChanged(double)));
connect(painterWidget, SIGNAL(mousePressed(QPoint)), this, SLOT(onCanvasClick(QPoint)));
@@ -104,18 +121,20 @@ void LaserScannerSelfLocalisationWidgetController::onConnectComponent()
usingTopic(localisation->getReportTopicName());
}
-
-QPointer<QDialog> armarx::LaserScannerSelfLocalisationWidgetController::getConfigDialog(QWidget* parent)
+QPointer<QDialog>
+armarx::LaserScannerSelfLocalisationWidgetController::getConfigDialog(QWidget* parent)
{
if (!dialog)
{
dialog = new SimpleConfigDialog(parent);
- dialog->addProxyFinder<LaserScannerSelfLocalisationInterfacePrx>({"LaserScannerSelfLocalisation", "", "*"});
+ dialog->addProxyFinder<LaserScannerSelfLocalisationInterfacePrx>(
+ {"LaserScannerSelfLocalisation", "", "*"});
}
return qobject_cast<SimpleConfigDialog*>(dialog);
}
-void armarx::LaserScannerSelfLocalisationWidgetController::configured()
+void
+armarx::LaserScannerSelfLocalisationWidgetController::configured()
{
if (dialog)
{
@@ -123,7 +142,8 @@ void armarx::LaserScannerSelfLocalisationWidgetController::configured()
}
}
-void LaserScannerSelfLocalisationWidgetController::onPaintCanvas(QPainter& painter, QSize size)
+void
+LaserScannerSelfLocalisationWidgetController::onPaintCanvas(QPainter& painter, QSize size)
{
float maxX = 0.0f;
float minX = FLT_MAX;
@@ -148,21 +168,21 @@ void LaserScannerSelfLocalisationWidgetController::onPaintCanvas(QPainter& paint
float scaleFactor = 0.95f * std::min(scaleFactorX, scaleFactorY);
float invScaleFactor = 1.0f / scaleFactor;
- auto toCanvas = [ = ](armarx::Vector2f p)
+ auto toCanvas = [=](armarx::Vector2f p)
{
float xCoord = scaleFactor * (p.e0 - minX);
float yCoord = scaleFactor * (p.e1 - minY);
return QPoint((int)xCoord, (int)(size.height() - 1 - yCoord));
};
- auto toCanvasE = [ = ](Eigen::Vector2f p)
+ auto toCanvasE = [=](Eigen::Vector2f p)
{
float xCoord = scaleFactor * (p.x() - minX);
float yCoord = scaleFactor * (p.y() - minY);
return QPoint((int)xCoord, (int)(size.height() - 1 - yCoord));
};
- auto fromCanvas = [ = ](QPoint p)
+ auto fromCanvas = [=](QPoint p)
{
float globalX = (invScaleFactor * p.x()) + minX;
float globalY = invScaleFactor * (size.height() - 1 - p.y()) + minY;
@@ -183,7 +203,8 @@ void LaserScannerSelfLocalisationWidgetController::onPaintCanvas(QPainter& paint
try
{
- ARMARX_INFO << "Set global pose to (" << globalPos.x() << ", " << globalPos.y() << ", " << theta << ")";
+ ARMARX_INFO << "Set global pose to (" << globalPos.x() << ", " << globalPos.y() << ", "
+ << theta << ")";
localisation->setAbsolutePose(globalPos.x(), globalPos.y(), theta);
}
catch (std::exception const& ex)
@@ -223,15 +244,19 @@ void LaserScannerSelfLocalisationWidgetController::onPaintCanvas(QPainter& paint
Eigen::Vector2f robotPos(poseX, poseY);
float robotTheta = poseTheta;
- armarx::Vector2f estRobotPos {robotPos.x(), robotPos.y()};
- Eigen::Vector2f estRobotEnd = robotPos + 500.0f * Eigen::Vector2f(Eigen::Rotation2Df(robotTheta) * Eigen::Vector2f::UnitY());
- armarx::Vector2f estRobotEnda {estRobotEnd.x(), estRobotEnd.y()};
+ armarx::Vector2f estRobotPos{robotPos.x(), robotPos.y()};
+ Eigen::Vector2f estRobotEnd =
+ robotPos +
+ 500.0f * Eigen::Vector2f(Eigen::Rotation2Df(robotTheta) * Eigen::Vector2f::UnitY());
+ armarx::Vector2f estRobotEnda{estRobotEnd.x(), estRobotEnd.y()};
painter.setBrush(QColor(0, 255, 255));
painter.drawEllipse(toCanvas(estRobotPos), 10, 10);
painter.drawLine(toCanvas(estRobotPos), toCanvas(estRobotEnda));
painter.setBrush(QColor(255, 255, 0, 25));
float posUncertaintyAverage = (poseUncertaintyX + poseUncertaintyY) * 0.5;
- painter.drawEllipse(toCanvas(estRobotPos), (int)(posUncertaintyAverage * scaleFactor), (int)(posUncertaintyAverage * scaleFactor));
+ painter.drawEllipse(toCanvas(estRobotPos),
+ (int)(posUncertaintyAverage * scaleFactor),
+ (int)(posUncertaintyAverage * scaleFactor));
qpoints.clear();
qedges.clear();
@@ -273,8 +298,11 @@ void LaserScannerSelfLocalisationWidgetController::onPaintCanvas(QPainter& paint
painter.drawLines(qedges.data(), qedges.size());
}
-
-void armarx::LaserScannerSelfLocalisationWidgetController::reportCorrectedPose(Ice::Float x, Ice::Float y, Ice::Float theta, const Ice::Current&)
+void
+armarx::LaserScannerSelfLocalisationWidgetController::reportCorrectedPose(Ice::Float x,
+ Ice::Float y,
+ Ice::Float theta,
+ const Ice::Current&)
{
poseX = x;
poseY = y;
@@ -283,18 +311,23 @@ void armarx::LaserScannerSelfLocalisationWidgetController::reportCorrectedPose(I
emit newDataReported();
}
-void LaserScannerSelfLocalisationWidgetController::reportPoseUncertainty(Ice::Float x, Ice::Float y, Ice::Float theta, const Ice::Current&)
+void
+LaserScannerSelfLocalisationWidgetController::reportPoseUncertainty(Ice::Float x,
+ Ice::Float y,
+ Ice::Float theta,
+ const Ice::Current&)
{
poseUncertaintyX = x;
poseUncertaintyY = y;
poseUncertaintyTheta = theta;
emit newDataReported();
-
}
-
-void armarx::LaserScannerSelfLocalisationWidgetController::reportLaserScanPoints(const Vector2fSeq& globalPoints, const Ice::Current&)
+void
+armarx::LaserScannerSelfLocalisationWidgetController::reportLaserScanPoints(
+ const Vector2fSeq& globalPoints,
+ const Ice::Current&)
{
{
ScopedLock guard(pointsMutex);
@@ -303,7 +336,10 @@ void armarx::LaserScannerSelfLocalisationWidgetController::reportLaserScanPoints
emit newDataReported();
}
-void armarx::LaserScannerSelfLocalisationWidgetController::reportExtractedEdges(const LineSegment2DSeq& globalEdges, const Ice::Current&)
+void
+armarx::LaserScannerSelfLocalisationWidgetController::reportExtractedEdges(
+ const LineSegment2DSeq& globalEdges,
+ const Ice::Current&)
{
{
ScopedLock guard(pointsMutex);
@@ -312,31 +348,38 @@ void armarx::LaserScannerSelfLocalisationWidgetController::reportExtractedEdges(
emit newDataReported();
}
-void LaserScannerSelfLocalisationWidgetController::onSpinBoxChanged(int value)
+void
+LaserScannerSelfLocalisationWidgetController::onSpinBoxChanged(int value)
{
updateProperties();
}
-void LaserScannerSelfLocalisationWidgetController::onSpinBoxChanged(double value)
+void
+LaserScannerSelfLocalisationWidgetController::onSpinBoxChanged(double value)
{
updateProperties();
}
-void LaserScannerSelfLocalisationWidgetController::onNewDataReported()
+void
+LaserScannerSelfLocalisationWidgetController::onNewDataReported()
{
- QString poseString = QString("Pose: ") + QString::number(poseX) + ", " + QString::number(poseY)
- + ", " + QString::number(poseTheta) + "\nUncertainty: " + QString::number(poseUncertaintyX, 'g', 4) + ", " + QString::number(poseUncertaintyY, 'g', 4)
- + ", " + QString::number(poseUncertaintyTheta, 'g', 4);
+ QString poseString = QString("Pose: ") + QString::number(poseX) + ", " +
+ QString::number(poseY) + ", " + QString::number(poseTheta) +
+ "\nUncertainty: " + QString::number(poseUncertaintyX, 'g', 4) + ", " +
+ QString::number(poseUncertaintyY, 'g', 4) + ", " +
+ QString::number(poseUncertaintyTheta, 'g', 4);
widget.poseLabel->setText(poseString);
}
-void LaserScannerSelfLocalisationWidgetController::onSetPoseClick()
+void
+LaserScannerSelfLocalisationWidgetController::onSetPoseClick()
{
setPoseState = SetPoseState::Position;
ARMARX_INFO << "Starting to set new position";
}
-void LaserScannerSelfLocalisationWidgetController::onCanvasClick(QPoint p)
+void
+LaserScannerSelfLocalisationWidgetController::onCanvasClick(QPoint p)
{
if (setPoseState == SetPoseState::Position)
{
@@ -352,7 +395,8 @@ void LaserScannerSelfLocalisationWidgetController::onCanvasClick(QPoint p)
}
}
-static std::string floatToString(float v)
+static std::string
+floatToString(float v)
{
std::ostringstream s;
s.imbue(std::locale::classic());
@@ -360,7 +404,8 @@ static std::string floatToString(float v)
return s.str();
}
-static float stringToFloat(std::string const& string)
+static float
+stringToFloat(std::string const& string)
{
std::istringstream s(string);
s.imbue(std::locale::classic());
@@ -371,7 +416,8 @@ static float stringToFloat(std::string const& string)
const std::string PROPERTIES_PREFIX = "ArmarX.LaserScannerSelfLocalisation.";
-void LaserScannerSelfLocalisationWidgetController::updateProperties()
+void
+LaserScannerSelfLocalisationWidgetController::updateProperties()
{
if (!localisationAdmin)
{
@@ -402,10 +448,13 @@ void LaserScannerSelfLocalisationWidgetController::updateProperties()
properties[PROPERTIES_PREFIX + "SmoothMergeDistance"] = to_string(smoothingMergeDistance);
properties[PROPERTIES_PREFIX + "MatchingMaxDistance"] = floatToString(matchingMaxDistance);
properties[PROPERTIES_PREFIX + "MatchingMinPoints"] = floatToString(matchingMinPoints);
- properties[PROPERTIES_PREFIX + "MatchingCorrectionFactor"] = floatToString(matchingCorrectionFactor);
+ properties[PROPERTIES_PREFIX + "MatchingCorrectionFactor"] =
+ floatToString(matchingCorrectionFactor);
properties[PROPERTIES_PREFIX + "EdgeMaxDistance"] = floatToString(edgeMaxDistance);
- properties[PROPERTIES_PREFIX + "EdgeMaxDeltaAngle"] = floatToString(edgeMaxDeltaAngle * M_PI / 180.0);
- properties[PROPERTIES_PREFIX + "EdgePointAddingThreshold"] = floatToString(edgePointAddingThreshold);
+ properties[PROPERTIES_PREFIX + "EdgeMaxDeltaAngle"] =
+ floatToString(edgeMaxDeltaAngle * M_PI / 180.0);
+ properties[PROPERTIES_PREFIX + "EdgePointAddingThreshold"] =
+ floatToString(edgePointAddingThreshold);
properties[PROPERTIES_PREFIX + "EdgeEpsilon"] = to_string(edgeEpsilon);
properties[PROPERTIES_PREFIX + "EdgeMinPoints"] = to_string(edgeMinPoints);
properties[PROPERTIES_PREFIX + "UseMapCorrection"] = useMapCorrection ? "1" : "0";
@@ -420,19 +469,26 @@ void LaserScannerSelfLocalisationWidgetController::updateProperties()
ARMARX_INFO << "done updating LaserScannerSelfLocalisationWidget properties";
}
-void LaserScannerSelfLocalisationWidgetController::readProperties()
+void
+LaserScannerSelfLocalisationWidgetController::readProperties()
{
ARMARX_IMPORTANT << "Trying to read properties";
ObjectPropertyInfos properties = localisationManager->getObjectPropertyInfos(localisationName);
int smoothingFrameSize = std::stoi(properties[PROPERTIES_PREFIX + "SmoothFrameSize"].value);
- int smoothingMergeDistance = std::stoi(properties[PROPERTIES_PREFIX + "SmoothMergeDistance"].value);
- float matchingMaxDistance = stringToFloat(properties[PROPERTIES_PREFIX + "MatchingMaxDistance"].value);
- float matchingMinPoints = stringToFloat(properties[PROPERTIES_PREFIX + "MatchingMinPoints"].value);
- float matchingCorrectionFactor = stringToFloat(properties[PROPERTIES_PREFIX + "MatchingCorrectionFactor"].value);
+ int smoothingMergeDistance =
+ std::stoi(properties[PROPERTIES_PREFIX + "SmoothMergeDistance"].value);
+ float matchingMaxDistance =
+ stringToFloat(properties[PROPERTIES_PREFIX + "MatchingMaxDistance"].value);
+ float matchingMinPoints =
+ stringToFloat(properties[PROPERTIES_PREFIX + "MatchingMinPoints"].value);
+ float matchingCorrectionFactor =
+ stringToFloat(properties[PROPERTIES_PREFIX + "MatchingCorrectionFactor"].value);
float edgeMaxDistance = stringToFloat(properties[PROPERTIES_PREFIX + "EdgeMaxDistance"].value);
- float edgeMaxDeltaAngle = 180.0 / M_PI * stringToFloat(properties[PROPERTIES_PREFIX + "EdgeMaxDeltaAngle"].value);
- float edgePointAddingThreshold = stringToFloat(properties[PROPERTIES_PREFIX + "EdgePointAddingThreshold"].value);
+ float edgeMaxDeltaAngle =
+ 180.0 / M_PI * stringToFloat(properties[PROPERTIES_PREFIX + "EdgeMaxDeltaAngle"].value);
+ float edgePointAddingThreshold =
+ stringToFloat(properties[PROPERTIES_PREFIX + "EdgePointAddingThreshold"].value);
int edgeEpsilon = std::stoi(properties[PROPERTIES_PREFIX + "EdgeEpsilon"].value);
int edgeMinPoints = std::stoi(properties[PROPERTIES_PREFIX + "EdgeMinPoints"].value);
bool useMapCorrection = properties[PROPERTIES_PREFIX + "UseMapCorrection"].value == "true";
@@ -460,7 +516,8 @@ void LaserScannerSelfLocalisationWidgetController::readProperties()
widget.velSensorStdDev->setValue(velSensorStdDev);
}
-void LaserScannerSelfLocalisationWidgetController::timerEvent(QTimerEvent*)
+void
+LaserScannerSelfLocalisationWidgetController::timerEvent(QTimerEvent*)
{
painterWidget->update();
}
diff --git a/source/RobotComponents/gui-plugins/LaserScannerSelfLocalisation/LaserScannerSelfLocalisationWidgetController.h b/source/RobotComponents/gui-plugins/LaserScannerSelfLocalisation/LaserScannerSelfLocalisationWidgetController.h
index 9604e139746a15302279812207257da6887f6f5a..9a0dd788b6a4de9e1e4f9cb3e0deea6ee62439e7 100644
--- a/source/RobotComponents/gui-plugins/LaserScannerSelfLocalisation/LaserScannerSelfLocalisationWidgetController.h
+++ b/source/RobotComponents/gui-plugins/LaserScannerSelfLocalisation/LaserScannerSelfLocalisationWidgetController.h
@@ -22,20 +22,21 @@
#pragma once
-#include <RobotComponents/gui-plugins/LaserScannerSelfLocalisation/ui_LaserScannerSelfLocalisationWidget.h>
-#include <RobotComponents/gui-plugins/LaserScannerSelfLocalisation/QPainterWidget.h>
-#include <RobotComponents/interface/components/LaserScannerSelfLocalisation.h>
+#include <atomic>
-#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
-#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
-#include <ArmarXGui/libraries/SimpleConfigDialog/SimpleConfigDialog.h>
+#include <Eigen/Eigen>
-#include <ArmarXCore/interface/core/ArmarXManagerInterface.h>
#include <ArmarXCore/core/system/ImportExportComponent.h>
#include <ArmarXCore/core/system/Synchronization.h>
+#include <ArmarXCore/interface/core/ArmarXManagerInterface.h>
-#include <Eigen/Eigen>
-#include <atomic>
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
+#include <ArmarXGui/libraries/SimpleConfigDialog/SimpleConfigDialog.h>
+
+#include <RobotComponents/gui-plugins/LaserScannerSelfLocalisation/QPainterWidget.h>
+#include <RobotComponents/gui-plugins/LaserScannerSelfLocalisation/ui_LaserScannerSelfLocalisationWidget.h>
+#include <RobotComponents/interface/components/LaserScannerSelfLocalisation.h>
namespace armarx
{
@@ -85,7 +86,8 @@ namespace armarx
* Returns the Widget name displayed in the ArmarXGui to create an
* instance of this class.
*/
- QString getWidgetName() const override
+ QString
+ getWidgetName() const override
{
return "RobotControl.LaserScannerSelfLocalisation";
}
@@ -107,8 +109,14 @@ namespace armarx
void onPaintCanvas(QPainter& painter, QSize size);
// LaserScannerSelfLocalisationListener interface
- void reportCorrectedPose(Ice::Float x, Ice::Float y, Ice::Float theta, const Ice::Current&) override;
- void reportPoseUncertainty(Ice::Float x, Ice::Float y, Ice::Float theta, const Ice::Current&) override;
+ void reportCorrectedPose(Ice::Float x,
+ Ice::Float y,
+ Ice::Float theta,
+ const Ice::Current&) override;
+ void reportPoseUncertainty(Ice::Float x,
+ Ice::Float y,
+ Ice::Float theta,
+ const Ice::Current&) override;
//void reportExtractedEdges(const LineSegment2DSeq& edges, const Ice::Current&) override;
void reportLaserScanPoints(const Vector2fSeq& globalPoints, const Ice::Current&) override;
@@ -174,5 +182,4 @@ namespace armarx
QPoint setPosition;
QPoint setOrientation;
};
-}
-
+} // namespace armarx
diff --git a/source/RobotComponents/gui-plugins/LaserScannerSelfLocalisation/QPainterWidget.h b/source/RobotComponents/gui-plugins/LaserScannerSelfLocalisation/QPainterWidget.h
index 7e665948fa6990675356ee946fd02f8e773879b1..398b792752f14e9a42309d3869f4153b506caa0a 100644
--- a/source/RobotComponents/gui-plugins/LaserScannerSelfLocalisation/QPainterWidget.h
+++ b/source/RobotComponents/gui-plugins/LaserScannerSelfLocalisation/QPainterWidget.h
@@ -22,12 +22,12 @@
#pragma once
-#include <QWidget>
-#include <QPainter>
+#include <functional>
+
#include <QLayout>
#include <QMouseEvent>
-
-#include <functional>
+#include <QPainter>
+#include <QWidget>
namespace armarx
{
@@ -41,7 +41,8 @@ namespace armarx
void mousePressed(QPoint p);
public:
- void setupUi(QWidget* parent = 0)
+ void
+ setupUi(QWidget* parent = 0)
{
parent->layout()->addWidget(this);
@@ -59,7 +60,8 @@ namespace armarx
QMetaObject::connectSlotsByName(this);
}
- void paintEvent(QPaintEvent*) override
+ void
+ paintEvent(QPaintEvent*) override
{
if (callback)
{
@@ -68,12 +70,14 @@ namespace armarx
}
}
- void mousePressEvent(QMouseEvent* event) override
+ void
+ mousePressEvent(QMouseEvent* event) override
{
emit mousePressed(event->pos());
}
- void setPaintCallback(PaintCallback const& callback)
+ void
+ setPaintCallback(PaintCallback const& callback)
{
this->callback = callback;
}
@@ -81,5 +85,4 @@ namespace armarx
private:
PaintCallback callback;
};
-}
-
+} // namespace armarx
diff --git a/source/RobotComponents/gui-plugins/MMMPlayerPlugin/MMMPlayerConfigDialog.cpp b/source/RobotComponents/gui-plugins/MMMPlayerPlugin/MMMPlayerConfigDialog.cpp
index 95cbffcd4a768c9e7752efb3400bc35f0355cd58..4af30657cdc9c9640f227ac5727a4c40d7c7eb40 100644
--- a/source/RobotComponents/gui-plugins/MMMPlayerPlugin/MMMPlayerConfigDialog.cpp
+++ b/source/RobotComponents/gui-plugins/MMMPlayerPlugin/MMMPlayerConfigDialog.cpp
@@ -22,25 +22,25 @@
*/
#include "MMMPlayerConfigDialog.h"
-#include <RobotComponents/gui-plugins/MMMPlayerPlugin/ui_MMMPlayerConfigDialog.h>
-#include <IceUtil/UUID.h>
-#include <QPushButton>
+#include <filesystem>
+
#include <QMessageBox>
+#include <QPushButton>
-#include <RobotAPI/interface/core/RobotState.h>
+#include <IceUtil/UUID.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
-#include <filesystem>
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
-#include <RobotComponents/interface/components/MMMPlayerInterface.h>
#include <RobotAPI/interface/components/TrajectoryPlayerInterface.h>
+#include <RobotAPI/interface/core/RobotState.h>
+#include <RobotComponents/gui-plugins/MMMPlayerPlugin/ui_MMMPlayerConfigDialog.h>
+#include <RobotComponents/interface/components/MMMPlayerInterface.h>
armarx::MMMPlayerConfigDialog::MMMPlayerConfigDialog(QWidget* parent) :
- QDialog(parent),
- ui(new Ui::MMMPlayerConfigDialog)
+ QDialog(parent), ui(new Ui::MMMPlayerConfigDialog)
{
ui->setupUi(this);
connect(ui->buttonBox, SIGNAL(accepted()), this, SLOT(verifyConfiguration()));
@@ -62,19 +62,21 @@ armarx::MMMPlayerConfigDialog::MMMPlayerConfigDialog(QWidget* parent) :
needtoCreate = false;
}
-void armarx::MMMPlayerConfigDialog::onInitComponent()
+void
+armarx::MMMPlayerConfigDialog::onInitComponent()
{
kinematicUnitComponentProxyFinder->setIceManager(this->getIceManager());
mmmPlayerComponentProxyFinder->setIceManager(this->getIceManager());
trajPlayerComponentProxyFinder->setIceManager(this->getIceManager());
}
-void armarx::MMMPlayerConfigDialog::onConnectComponent()
+void
+armarx::MMMPlayerConfigDialog::onConnectComponent()
{
-
}
-void armarx::MMMPlayerConfigDialog::onExitComponent()
+void
+armarx::MMMPlayerConfigDialog::onExitComponent()
{
QObject::disconnect();
@@ -85,8 +87,8 @@ armarx::MMMPlayerConfigDialog::~MMMPlayerConfigDialog()
delete ui;
}
-
-void armarx::MMMPlayerConfigDialog::verifyConfiguration()
+void
+armarx::MMMPlayerConfigDialog::verifyConfiguration()
{
if (kinematicUnitComponentProxyFinder->getSelectedProxyName().trimmed().length() == 0)
{
@@ -105,7 +107,8 @@ void armarx::MMMPlayerConfigDialog::verifyConfiguration()
}
}
-void armarx::MMMPlayerConfigDialog::reject()
+void
+armarx::MMMPlayerConfigDialog::reject()
{
if (getIceManager())
{
@@ -113,6 +116,4 @@ void armarx::MMMPlayerConfigDialog::reject()
}
QDialog::reject();
-
}
-
diff --git a/source/RobotComponents/gui-plugins/MMMPlayerPlugin/MMMPlayerConfigDialog.h b/source/RobotComponents/gui-plugins/MMMPlayerPlugin/MMMPlayerConfigDialog.h
index 1a11a0c9659f3e2be552284f72c2d4c69cd2f063..15e081d5377bbc71aeda0938da2c3f199c7dd46d 100644
--- a/source/RobotComponents/gui-plugins/MMMPlayerPlugin/MMMPlayerConfigDialog.h
+++ b/source/RobotComponents/gui-plugins/MMMPlayerPlugin/MMMPlayerConfigDialog.h
@@ -23,11 +23,12 @@
#pragma once
-#include <ArmarXGui/libraries/ArmarXGuiBase/widgets/IceProxyFinder.h>
+#include <QDialog>
+#include <QFileDialog>
+
#include <ArmarXCore/core/ManagedIceObject.h>
-#include <QFileDialog>
-#include <QDialog>
+#include <ArmarXGui/libraries/ArmarXGuiBase/widgets/IceProxyFinder.h>
namespace Ui
{
@@ -36,8 +37,7 @@ namespace Ui
namespace armarx
{
- class MMMPlayerConfigDialog :
- public QDialog, virtual public ManagedIceObject
+ class MMMPlayerConfigDialog : public QDialog, virtual public ManagedIceObject
{
Q_OBJECT
friend class MMMPlayerWidget;
@@ -47,7 +47,8 @@ namespace armarx
~MMMPlayerConfigDialog() override;
// inherited from ManagedIceObject
- std::string getDefaultName() const override
+ std::string
+ getDefaultName() const override
{
return "MMMPlayerConfigDialog" + uuid;
}
@@ -74,7 +75,5 @@ namespace armarx
bool needtoCreate;
std::string uuid;
-
};
-}
-
+} // namespace armarx
diff --git a/source/RobotComponents/gui-plugins/MMMPlayerPlugin/MMMPlayerGuiPlugin.cpp b/source/RobotComponents/gui-plugins/MMMPlayerPlugin/MMMPlayerGuiPlugin.cpp
index ff6a3fbf0d6d45453204d248a8140a07f7b6cdc0..afb0392b52e4ec82249e374158bac764c05a4f1c 100644
--- a/source/RobotComponents/gui-plugins/MMMPlayerPlugin/MMMPlayerGuiPlugin.cpp
+++ b/source/RobotComponents/gui-plugins/MMMPlayerPlugin/MMMPlayerGuiPlugin.cpp
@@ -22,26 +22,29 @@
* GNU General Public License
*/
#include "MMMPlayerGuiPlugin.h"
-#include "MMMPlayerConfigDialog.h"
-#include <RobotComponents/gui-plugins/MMMPlayerPlugin/ui_MMMPlayerConfigDialog.h>
+
+#include <IceUtil/Time.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/observers/variant/SingleTypeVariantList.h>
-#include <ArmarXCore/util/variants/eigen3/MatrixVariant.h>
#include <ArmarXCore/observers/variant/TimestampVariant.h>
-#include <IceUtil/Time.h>
+#include <ArmarXCore/util/variants/eigen3/MatrixVariant.h>
+
+#include <RobotComponents/gui-plugins/MMMPlayerPlugin/ui_MMMPlayerConfigDialog.h>
+
+#include "MMMPlayerConfigDialog.h"
// Qt headers
-#include <QInputDialog>
-#include <Qt>
-#include <QtGlobal>
-#include <QPushButton>
+#include <filesystem>
+
#include <QComboBox>
+#include <QInputDialog>
#include <QMenu>
-
-#include <filesystem>
#include <QMessageBox>
+#include <QPushButton>
+#include <Qt>
+#include <QtGlobal>
#include <RobotComponents/components/MMMPlayer/MMMPlayer.h>
#include <RobotComponents/components/TrajectoryPlayer/TrajectoryPlayer.h>
@@ -71,7 +74,6 @@ MMMPlayerWidget::MMMPlayerWidget()
jointList = new QListWidget();
updateTimer = new QTimer(this);
-
}
/*
@@ -97,13 +99,17 @@ void MMMPlayerWidget::configured()
*/
-void MMMPlayerWidget::onInitComponent()
+void
+MMMPlayerWidget::onInitComponent()
{
isComponentCreated = false;
if (dialog->needtoCreate)
{
- std::string kinematicUnitName = dialog->kinematicUnitComponentProxyFinder->getSelectedProxyName().trimmed().toStdString();
+ std::string kinematicUnitName =
+ dialog->kinematicUnitComponentProxyFinder->getSelectedProxyName()
+ .trimmed()
+ .toStdString();
std::string kinematicTopic = dialog->kinematicTopicName;
Ice::StringSeq pros;
@@ -127,22 +133,25 @@ void MMMPlayerWidget::onInitComponent()
getArmarXManager()->addObject(mptr);
isComponentCreated = true;
-
}
- usingProxy(dialog->mmmPlayerComponentProxyFinder->getSelectedProxyName().trimmed().toStdString());
- usingProxy(dialog->trajPlayerComponentProxyFinder->getSelectedProxyName().trimmed().toStdString());
+ usingProxy(
+ dialog->mmmPlayerComponentProxyFinder->getSelectedProxyName().trimmed().toStdString());
+ usingProxy(
+ dialog->trajPlayerComponentProxyFinder->getSelectedProxyName().trimmed().toStdString());
// ui.frameSlider->setEnabled(false);
connectSlots();
}
-
-void MMMPlayerWidget::onConnectComponent()
+void
+MMMPlayerWidget::onConnectComponent()
{
- MMMLoader = getProxy<MMMPlayerInterfacePrx>(dialog->mmmPlayerComponentProxyFinder->getSelectedProxyName().trimmed().toStdString());
- trajPlayer = getProxy<TrajectoryPlayerInterfacePrx>(dialog->trajPlayerComponentProxyFinder->getSelectedProxyName().trimmed().toStdString());
+ MMMLoader = getProxy<MMMPlayerInterfacePrx>(
+ dialog->mmmPlayerComponentProxyFinder->getSelectedProxyName().trimmed().toStdString());
+ trajPlayer = getProxy<TrajectoryPlayerInterfacePrx>(
+ dialog->trajPlayerComponentProxyFinder->getSelectedProxyName().trimmed().toStdString());
configFile = MMMLoader->getMotionPath();
ui.editConfigFile->setText(QString::fromStdString(configFile));
@@ -162,15 +171,18 @@ void MMMPlayerWidget::onConnectComponent()
//on_enableRobotPoseUnit_toggled(false);
}
-void MMMPlayerWidget::onExitComponent()
+void
+MMMPlayerWidget::onExitComponent()
{
}
-void MMMPlayerWidget::onDisconnectComponent()
+void
+MMMPlayerWidget::onDisconnectComponent()
{
}
-bool MMMPlayerWidget::onClose()
+bool
+MMMPlayerWidget::onClose()
{
if (getIceManager())
@@ -179,27 +191,29 @@ bool MMMPlayerWidget::onClose()
if (isComponentCreated)
{
- getIceManager()->removeObject(dialog->mmmPlayerComponentProxyFinder->getSelectedProxyName().trimmed().toStdString());
+ getIceManager()->removeObject(
+ dialog->mmmPlayerComponentProxyFinder->getSelectedProxyName()
+ .trimmed()
+ .toStdString());
}
}
return ArmarXComponentWidgetController::onClose();
-
-
}
-
-void MMMPlayerWidget::loadSettings(QSettings* settings)
+void
+MMMPlayerWidget::loadSettings(QSettings* settings)
{
}
-
-void MMMPlayerWidget::saveSettings(QSettings* settings)
+void
+MMMPlayerWidget::saveSettings(QSettings* settings)
{
}
-void MMMPlayerWidget::loadTrajPlayer()
+void
+MMMPlayerWidget::loadTrajPlayer()
{
TrajectoryBasePtr t = MMMLoader->getJointTraj();
TrajectoryPtr tr = IceInternal::Handle<Trajectory>::dynamicCast(t);
@@ -210,8 +224,8 @@ void MMMPlayerWidget::loadTrajPlayer()
// trajPlayer->setEndTime(trajPlayer->getTrajEndTime() / ui.speedSpinBox->value());
}
-
-void MMMPlayerWidget::connectSlots()
+void
+MMMPlayerWidget::connectSlots()
{
connect(ui.initializeButton, SIGNAL(clicked()), this, SLOT(on_initializeButton_clicked()));
connect(ui.startButton, SIGNAL(clicked()), this, SLOT(on_startButton_clicked()));
@@ -226,13 +240,24 @@ void MMMPlayerWidget::connectSlots()
// connect(ui.frameSlider, SIGNAL(valueChanged(int)), this, SLOT(setFrameNumber(int)));
// connect(ui.loopPlayback, SIGNAL(toggled(bool)), this, SLOT(on_loopPlayback_toggled(bool)));
connect(ui.loopPlayback, SIGNAL(toggled(bool)), this, SLOT(on_loopPlayback_toggled(bool)));
- connect(ui.enableRobotPoseUnit, SIGNAL(toggled(bool)), this, SLOT(on_enableRobotPoseUnit_toggled(bool)));
- connect(ui.speedSpinBox, SIGNAL(valueChanged(double)), this, SLOT(on_spinBoxFPS_valueChanged(double)));
- connect(ui.controlMode, SIGNAL(currentIndexChanged(int)), this, SLOT(on_controlMode_changed(int)));
- connect(jointList, SIGNAL(itemChanged(QListWidgetItem*)), this, SLOT(jointListChanged(QListWidgetItem*)));
+ connect(ui.enableRobotPoseUnit,
+ SIGNAL(toggled(bool)),
+ this,
+ SLOT(on_enableRobotPoseUnit_toggled(bool)));
+ connect(ui.speedSpinBox,
+ SIGNAL(valueChanged(double)),
+ this,
+ SLOT(on_spinBoxFPS_valueChanged(double)));
+ connect(
+ ui.controlMode, SIGNAL(currentIndexChanged(int)), this, SLOT(on_controlMode_changed(int)));
+ connect(jointList,
+ SIGNAL(itemChanged(QListWidgetItem*)),
+ this,
+ SLOT(jointListChanged(QListWidgetItem*)));
}
-void armarx::MMMPlayerWidget::on_initializeButton_clicked()
+void
+armarx::MMMPlayerWidget::on_initializeButton_clicked()
{
// set position control mode and move to first frame as initial pose
@@ -243,13 +268,14 @@ void armarx::MMMPlayerWidget::on_initializeButton_clicked()
updateSlider();
}
-void armarx::MMMPlayerWidget::setSpeed(double value)
+void
+armarx::MMMPlayerWidget::setSpeed(double value)
{
trajPlayer->setEndTime(trajPlayer->getTrajEndTime() / value);
-
}
-void armarx::MMMPlayerWidget::on_startButton_clicked()
+void
+armarx::MMMPlayerWidget::on_startButton_clicked()
{
trajPlayer->setIsPreview(false);
setSpeed(ui.speedSpinBox->value());
@@ -274,14 +300,14 @@ void armarx::MMMPlayerWidget::on_startButton_clicked()
ui.frameSlider->blockSignals(true);
ui.status->setText("Real");
-
}
}
-void armarx::MMMPlayerWidget::on_previewButton_clicked()
+void
+armarx::MMMPlayerWidget::on_previewButton_clicked()
{
trajPlayer->setIsPreview(true);
- ARMARX_INFO << "speed spin box val: " << ui.speedSpinBox->value();
+ ARMARX_INFO << "speed spin box val: " << ui.speedSpinBox->value();
setSpeed(ui.speedSpinBox->value());
trajPlayer->resetTrajectoryPlayer(false);
bool previewed = trajPlayer->startTrajectoryPlayer();
@@ -306,7 +332,8 @@ void armarx::MMMPlayerWidget::on_previewButton_clicked()
}
}
-void MMMPlayerWidget::on_pauseButton_clicked()
+void
+MMMPlayerWidget::on_pauseButton_clicked()
{
bool paused = trajPlayer->pauseTrajectoryPlayer();
@@ -325,7 +352,8 @@ void MMMPlayerWidget::on_pauseButton_clicked()
ui.frameSlider->blockSignals(!paused);
}
-void armarx::MMMPlayerWidget::on_stopButton_clicked()
+void
+armarx::MMMPlayerWidget::on_stopButton_clicked()
{
bool stopped = trajPlayer->stopTrajectoryPlayer();
@@ -348,7 +376,8 @@ void armarx::MMMPlayerWidget::on_stopButton_clicked()
}
}
-void MMMPlayerWidget::configFileSelected()
+void
+MMMPlayerWidget::configFileSelected()
{
configFile = (this->fileDialog2->selectedFiles()[0]).toStdString();
ui.editConfigFile->setText(this->fileDialog2->selectedFiles()[0]);
@@ -365,10 +394,12 @@ void MMMPlayerWidget::configFileSelected()
}
}
-void MMMPlayerWidget::setupMotionList()
+void
+MMMPlayerWidget::setupMotionList()
{
Ice::StringSeq motionNames = MMMLoader->getMotionNames();
- QObject::disconnect(ui.motionName, SIGNAL(currentIndexChanged(int)), this, SLOT(motionChanged(int)));
+ QObject::disconnect(
+ ui.motionName, SIGNAL(currentIndexChanged(int)), this, SLOT(motionChanged(int)));
ui.motionName->clear();
for (size_t i = 0; i < motionNames.size(); ++i)
@@ -383,7 +414,8 @@ void MMMPlayerWidget::setupMotionList()
}
}
-void MMMPlayerWidget::setupJointList()
+void
+MMMPlayerWidget::setupJointList()
{
Ice::StringSeq jointNames = MMMLoader->getJointNames();
jointList->blockSignals(true);
@@ -399,11 +431,12 @@ void MMMPlayerWidget::setupJointList()
jointList->blockSignals(false);
}
-void MMMPlayerWidget::updateSlider()
+void
+MMMPlayerWidget::updateSlider()
{
double currentTime = trajPlayer->getCurrentTime();
double endTime = trajPlayer->getEndTime();
- double posPer = currentTime / endTime ;
+ double posPer = currentTime / endTime;
int sliderPos = posPer * 100;
@@ -413,7 +446,8 @@ void MMMPlayerWidget::updateSlider()
ui.currentFrame->display(currentTime);
}
-void MMMPlayerWidget::jointListChanged(QListWidgetItem* joint)
+void
+MMMPlayerWidget::jointListChanged(QListWidgetItem* joint)
{
bool jointState;
@@ -436,7 +470,8 @@ void MMMPlayerWidget::jointListChanged(QListWidgetItem* joint)
}
}
-void MMMPlayerWidget::motionChanged(int)
+void
+MMMPlayerWidget::motionChanged(int)
{
std::string motionName = ui.motionName->currentText().toStdString();
ARMARX_INFO << "Choosing motion with name '" << motionName << "'";
@@ -449,24 +484,24 @@ void MMMPlayerWidget::motionChanged(int)
}
}
-
-void MMMPlayerWidget::on_spinBoxFPS_valueChanged(double value)
+void
+MMMPlayerWidget::on_spinBoxFPS_valueChanged(double value)
{
if (value <= 0)
{
return;
}
-
}
-
-void MMMPlayerWidget::on_loopPlayback_toggled(bool state)
+void
+MMMPlayerWidget::on_loopPlayback_toggled(bool state)
{
trajPlayer->setLoopPlayback(state);
ui.loopPlayback->setChecked(state);
}
-void MMMPlayerWidget::on_controlMode_changed(int controlMode)
+void
+MMMPlayerWidget::on_controlMode_changed(int controlMode)
{
ui.controlMode->setCurrentIndex(controlMode);
@@ -480,7 +515,8 @@ void MMMPlayerWidget::on_controlMode_changed(int controlMode)
}
}
-void armarx::MMMPlayerWidget::on_enableRobotPoseUnit_toggled(bool state)
+void
+armarx::MMMPlayerWidget::on_enableRobotPoseUnit_toggled(bool state)
{
trajPlayer->enableRobotPoseUnit(state);
ui.enableRobotPoseUnit->setChecked(state);
diff --git a/source/RobotComponents/gui-plugins/MMMPlayerPlugin/MMMPlayerGuiPlugin.h b/source/RobotComponents/gui-plugins/MMMPlayerPlugin/MMMPlayerGuiPlugin.h
index a732636e210051f8ab333a041846f2fc3797b173..80f9bb9b79c481fab26195fedcb963cb71d447a2 100644
--- a/source/RobotComponents/gui-plugins/MMMPlayerPlugin/MMMPlayerGuiPlugin.h
+++ b/source/RobotComponents/gui-plugins/MMMPlayerPlugin/MMMPlayerGuiPlugin.h
@@ -25,33 +25,33 @@
#pragma once
/* ArmarX headers */
-#include <RobotComponents/gui-plugins/MMMPlayerPlugin/ui_MMMPlayerGuiPlugin.h>
-
-#include "MMMPlayerConfigDialog.h"
#include <ArmarXCore/core/Component.h>
-#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
-#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
-#include <ArmarXCore/observers/Observer.h>
#include <ArmarXCore/core/application/Application.h>
#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/observers/Observer.h>
-#include <RobotAPI/libraries/core/RobotAPIObjectFactories.h>
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
-/* Qt headers */
-#include <QMainWindow>
-#include <QTimer>
-#include <RobotComponents/interface/components/MMMPlayerInterface.h>
+#include <RobotAPI/libraries/core/RobotAPIObjectFactories.h>
+#include <RobotComponents/gui-plugins/MMMPlayerPlugin/ui_MMMPlayerGuiPlugin.h>
-#include <RobotAPI/interface/components/TrajectoryPlayerInterface.h>
-#include <RobotAPI/libraries/core/Trajectory.h>
+#include "MMMPlayerConfigDialog.h"
+/* Qt headers */
#include <string>
-#include <QLayout>
+
#include <QFileDialog>
+#include <QLayout>
#include <QListWidget>
+#include <QMainWindow>
+#include <QTimer>
+#include <RobotAPI/interface/components/TrajectoryPlayerInterface.h>
+#include <RobotAPI/libraries/core/Trajectory.h>
+#include <RobotComponents/interface/components/MMMPlayerInterface.h>
namespace armarx
{
@@ -63,15 +63,16 @@ namespace armarx
\see MMMPlayerWidget
*/
- class MMMPlayerGuiPlugin :
- public ArmarXGuiPlugin
+ class MMMPlayerGuiPlugin : public ArmarXGuiPlugin
{
Q_OBJECT
Q_INTERFACES(ArmarXGuiInterface)
Q_PLUGIN_METADATA(IID "ArmarXGuiInterface/1.00")
public:
MMMPlayerGuiPlugin();
- QString getPluginName() override
+
+ QString
+ getPluginName() override
{
return "MMMPlayerGuiPlugin";
}
@@ -84,14 +85,15 @@ namespace armarx
\ingroup RobotAPI-ArmarXGuiPlugins ArmarXGuiPlugins
\see MMMPlayerGuiPlugin
*/
- class MMMPlayerWidget :
- public ArmarXComponentWidgetControllerTemplate<MMMPlayerWidget>
+ class MMMPlayerWidget : public ArmarXComponentWidgetControllerTemplate<MMMPlayerWidget>
{
Q_OBJECT
public:
MMMPlayerWidget();
+
~MMMPlayerWidget()
- {}
+ {
+ }
// inherited from Component
void onInitComponent() override;
@@ -100,16 +102,20 @@ namespace armarx
void onDisconnectComponent() override;
// inherited of ArmarXWidget
- static QString GetWidgetName()
+ static QString
+ GetWidgetName()
{
return "RobotControl.MMMPlayerGUI";
}
- static QIcon GetWidgetIcon()
+
+ static QIcon
+ GetWidgetIcon()
{
return QIcon(":icons/MMM.png");
}
- QPointer<QDialog> getConfigDialog(QWidget* parent = 0) override
+ QPointer<QDialog>
+ getConfigDialog(QWidget* parent = 0) override
{
if (!dialog)
{
@@ -117,13 +123,11 @@ namespace armarx
}
return qobject_cast<MMMPlayerConfigDialog*>(dialog);
-
}
bool onClose() override;
-
void loadSettings(QSettings* settings) override;
void saveSettings(QSettings* settings) override;
// void configured();
@@ -172,6 +176,7 @@ namespace armarx
Ui::MMMPlayerGuiPlugin ui;
MMMPlayerInterfacePrx MMMLoader;
TrajectoryPlayerInterfacePrx trajPlayer;
+
private:
QPointer<QWidget> __widget;
QPointer<MMMPlayerConfigDialog> dialog;
@@ -181,6 +186,6 @@ namespace armarx
QTimer* updateTimer;
bool isComponentCreated;
};
- //using MMMPlayerGuiPluginPtr = boost::shared_ptr<MMMPlayerWidget>;
-}
+ //using MMMPlayerGuiPluginPtr = boost::shared_ptr<MMMPlayerWidget>;
+} // namespace armarx
diff --git a/source/RobotComponents/gui-plugins/MotionPlanning/MotionPlanningServer/MotionPlanningServerConfigDialog.cpp b/source/RobotComponents/gui-plugins/MotionPlanning/MotionPlanningServer/MotionPlanningServerConfigDialog.cpp
index 63b4515922a156778a6335bc04b7fda95ac7b189..e90b9f941cbab32a231e5d710c967c1f8698981c 100644
--- a/source/RobotComponents/gui-plugins/MotionPlanning/MotionPlanningServer/MotionPlanningServerConfigDialog.cpp
+++ b/source/RobotComponents/gui-plugins/MotionPlanning/MotionPlanningServer/MotionPlanningServerConfigDialog.cpp
@@ -21,16 +21,14 @@
* @copyright http://www.gnu.org/licenses/gpl.txt
* GNU General Public License
*/
-#include <IceUtil/UUID.h>
-
#include "MotionPlanningServerConfigDialog.h"
+#include <IceUtil/UUID.h>
namespace armarx
{
MotionPlanningServerConfigDialog::MotionPlanningServerConfigDialog(QWidget* parent) :
- QDialog(parent),
- uuid(IceUtil::generateUUID())
+ QDialog(parent), uuid(IceUtil::generateUUID())
{
ui.setupUi(this);
finder = new armarx::IceProxyFinder<MotionPlanningServerInterfacePrx>(this);
@@ -38,17 +36,20 @@ namespace armarx
ui.horizontalLayout->addWidget(finder);
}
- void MotionPlanningServerConfigDialog::onInitComponent()
+ void
+ MotionPlanningServerConfigDialog::onInitComponent()
{
finder->setIceManager(getIceManager());
}
- void MotionPlanningServerConfigDialog::onConnectComponent()
+ void
+ MotionPlanningServerConfigDialog::onConnectComponent()
{
}
- std::string MotionPlanningServerConfigDialog::getDefaultName() const
+ std::string
+ MotionPlanningServerConfigDialog::getDefaultName() const
{
return "MotionPlanningServerConfigDialog" + uuid;
}
-}
+} // namespace armarx
diff --git a/source/RobotComponents/gui-plugins/MotionPlanning/MotionPlanningServer/MotionPlanningServerConfigDialog.h b/source/RobotComponents/gui-plugins/MotionPlanning/MotionPlanningServer/MotionPlanningServerConfigDialog.h
index 736e9a124d49d9a0fbb5b4d7fbaa9df7ff10bb0e..e0f6f586cc3c5856bce785f360d948c02ce9cdd7 100644
--- a/source/RobotComponents/gui-plugins/MotionPlanning/MotionPlanningServer/MotionPlanningServerConfigDialog.h
+++ b/source/RobotComponents/gui-plugins/MotionPlanning/MotionPlanningServer/MotionPlanningServerConfigDialog.h
@@ -28,7 +28,6 @@
#include <ArmarXGui/libraries/ArmarXGuiBase/widgets/IceProxyFinder.h>
#include <RobotComponents/components/MotionPlanning/MotionPlanningServer.h>
-
#include <RobotComponents/gui-plugins/MotionPlanning/MotionPlanningServer/ui_MotionPlanningServerConfigDialog.h>
namespace armarx
@@ -37,9 +36,7 @@ namespace armarx
* @brief The config dialog of the palnning server gui.
* It has a proxy finder for a planning server
*/
- class MotionPlanningServerConfigDialog :
- public QDialog,
- virtual public armarx::ManagedIceObject
+ class MotionPlanningServerConfigDialog : public QDialog, virtual public armarx::ManagedIceObject
{
Q_OBJECT
@@ -86,4 +83,4 @@ namespace armarx
*/
std::string getDefaultName() const override;
};
-}
+} // namespace armarx
diff --git a/source/RobotComponents/gui-plugins/MotionPlanning/MotionPlanningServer/MotionPlanningServerWidgetController.cpp b/source/RobotComponents/gui-plugins/MotionPlanning/MotionPlanningServer/MotionPlanningServerWidgetController.cpp
index 803596f3ed444ce32726f82bdee1098b06fc1721..df488b84bdc1f5fde18e58ccefbefd3f68a2d231 100644
--- a/source/RobotComponents/gui-plugins/MotionPlanning/MotionPlanningServer/MotionPlanningServerWidgetController.cpp
+++ b/source/RobotComponents/gui-plugins/MotionPlanning/MotionPlanningServer/MotionPlanningServerWidgetController.cpp
@@ -22,6 +22,8 @@
* GNU General Public License
*/
+#include "MotionPlanningServerWidgetController.h"
+
#include <string>
#include <QPushButton>
@@ -29,47 +31,58 @@
#include <RobotComponents/components/MotionPlanning/util/PlanningUtil.h>
-#include "MotionPlanningServerWidgetController.h"
-
using namespace armarx;
-MotionPlanningServerWidgetController::MotionPlanningServerWidgetController():
- motionPlanningServerProxyName {"MotionPlanningServer"}
+MotionPlanningServerWidgetController::MotionPlanningServerWidgetController() :
+ motionPlanningServerProxyName{"MotionPlanningServer"}
{
widget.setupUi(getWidget());
taskList = new MotionPlanningServerTaskList{};
widget.verticalLayout->addWidget(taskList);
}
-void MotionPlanningServerWidgetController::loadSettings(QSettings* settings)
+void
+MotionPlanningServerWidgetController::loadSettings(QSettings* settings)
{
- motionPlanningServerProxyName = settings->value("motionPlanningServerProxyName", QString::fromStdString(motionPlanningServerProxyName)).toString().toStdString();
+ motionPlanningServerProxyName =
+ settings
+ ->value("motionPlanningServerProxyName",
+ QString::fromStdString(motionPlanningServerProxyName))
+ .toString()
+ .toStdString();
}
-void MotionPlanningServerWidgetController::saveSettings(QSettings* settings)
+void
+MotionPlanningServerWidgetController::saveSettings(QSettings* settings)
{
- settings->setValue("motionPlanningServerProxyName", QString::fromStdString(motionPlanningServerProxyName));
+ settings->setValue("motionPlanningServerProxyName",
+ QString::fromStdString(motionPlanningServerProxyName));
}
-void MotionPlanningServerWidgetController::onInitComponent()
+void
+MotionPlanningServerWidgetController::onInitComponent()
{
usingProxy(motionPlanningServerProxyName);
}
-void MotionPlanningServerWidgetController::onConnectComponent()
+void
+MotionPlanningServerWidgetController::onConnectComponent()
{
- taskList->setMotionPlanningServer(getProxy<MotionPlanningServerInterfacePrx>(motionPlanningServerProxyName));
+ taskList->setMotionPlanningServer(
+ getProxy<MotionPlanningServerInterfacePrx>(motionPlanningServerProxyName));
taskList->enableAutoUpdate(true);
}
-void MotionPlanningServerWidgetController::onDisconnectComponent()
+void
+MotionPlanningServerWidgetController::onDisconnectComponent()
{
taskList->clearList();
taskList->setMotionPlanningServer(nullptr);
taskList->enableAutoUpdate(false);
}
-QPointer<QDialog> MotionPlanningServerWidgetController::getConfigDialog(QWidget* parent)
+QPointer<QDialog>
+MotionPlanningServerWidgetController::getConfigDialog(QWidget* parent)
{
if (!dialog)
{
@@ -79,8 +92,8 @@ QPointer<QDialog> MotionPlanningServerWidgetController::getConfigDialog(QWidget*
return qobject_cast<MotionPlanningServerConfigDialog*>(dialog);
}
-void MotionPlanningServerWidgetController::configured()
+void
+MotionPlanningServerWidgetController::configured()
{
motionPlanningServerProxyName = dialog->finder->getSelectedProxyName().toStdString();
}
-
diff --git a/source/RobotComponents/gui-plugins/MotionPlanning/MotionPlanningServer/MotionPlanningServerWidgetController.h b/source/RobotComponents/gui-plugins/MotionPlanning/MotionPlanningServer/MotionPlanningServerWidgetController.h
index 69fd39cf60f7a966308c027fe9a19dca0a756dc9..a60631a037930921c2fb218489e870e1a50bad88 100644
--- a/source/RobotComponents/gui-plugins/MotionPlanning/MotionPlanningServer/MotionPlanningServerWidgetController.h
+++ b/source/RobotComponents/gui-plugins/MotionPlanning/MotionPlanningServer/MotionPlanningServerWidgetController.h
@@ -24,18 +24,18 @@
#pragma once
-#include "MotionPlanningServerConfigDialog.h"
-#include <RobotComponents/gui-plugins/MotionPlanning/MotionPlanningServer/ui_MotionPlanningServerWidget.h>
-
-#include "../QtWidgets/MotionPlanningServerTaskList.h"
-
-#include <RobotComponents/interface/components/MotionPlanning/MotionPlanningServer.h>
-
#include <QPointer>
#include <ArmarXCore/core/system/ImportExportComponent.h>
-#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
+
#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
+
+#include <RobotComponents/gui-plugins/MotionPlanning/MotionPlanningServer/ui_MotionPlanningServerWidget.h>
+#include <RobotComponents/interface/components/MotionPlanning/MotionPlanningServer.h>
+
+#include "../QtWidgets/MotionPlanningServerTaskList.h"
+#include "MotionPlanningServerConfigDialog.h"
namespace armarx
{
@@ -53,8 +53,7 @@ namespace armarx
* \li The ice type
* \li The internal id used by the \ref armarx::MotionPlanningServer. (this can be used for debugging) Tasks are processed in ascending order.
*/
- class ARMARXCOMPONENT_IMPORT_EXPORT
- MotionPlanningServerWidgetController:
+ class ARMARXCOMPONENT_IMPORT_EXPORT MotionPlanningServerWidgetController :
public ArmarXComponentWidgetControllerTemplate<MotionPlanningServerWidgetController>
{
Q_OBJECT
@@ -86,7 +85,8 @@ namespace armarx
* @return The Widget name displayed in the ArmarXGui to create an
* instance of this class.
*/
- static QString GetWidgetName()
+ static QString
+ GetWidgetName()
{
return "MotionPlanning.MotionPlanningServerGui";
}
@@ -135,4 +135,4 @@ namespace armarx
QPointer<MotionPlanningServerTaskList> taskList;
};
-}
+} // namespace armarx
diff --git a/source/RobotComponents/gui-plugins/MotionPlanning/MotionPlanningWidgetsPlugin.cpp b/source/RobotComponents/gui-plugins/MotionPlanning/MotionPlanningWidgetsPlugin.cpp
index e8856ecbe325456f86b134f9266eb7ca28f405ab..3b2586ccaa467eec8b5884981c3581a408e3765e 100644
--- a/source/RobotComponents/gui-plugins/MotionPlanning/MotionPlanningWidgetsPlugin.cpp
+++ b/source/RobotComponents/gui-plugins/MotionPlanning/MotionPlanningWidgetsPlugin.cpp
@@ -24,8 +24,8 @@
#include "MotionPlanningWidgetsPlugin.h"
-#include "SimoxCSpaceVisualizer/SimoxCSpaceVisualizerWidgetController.h"
#include "MotionPlanningServer/MotionPlanningServerWidgetController.h"
+#include "SimoxCSpaceVisualizer/SimoxCSpaceVisualizerWidgetController.h"
namespace armarx
{
@@ -35,4 +35,4 @@ namespace armarx
addWidget<SimoxCSpaceVisualizerWidgetController>();
addWidget<MotionPlanningServerWidgetController>();
}
-}
+} // namespace armarx
diff --git a/source/RobotComponents/gui-plugins/MotionPlanning/MotionPlanningWidgetsPlugin.h b/source/RobotComponents/gui-plugins/MotionPlanning/MotionPlanningWidgetsPlugin.h
index 1d4ae872b8a9a81097ff3b72b036bb9cc3a3891e..23566d41d5faf306b40db9e7aa95f7a33bffa715 100644
--- a/source/RobotComponents/gui-plugins/MotionPlanning/MotionPlanningWidgetsPlugin.h
+++ b/source/RobotComponents/gui-plugins/MotionPlanning/MotionPlanningWidgetsPlugin.h
@@ -24,10 +24,10 @@
#pragma once
-#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
-
#include <ArmarXCore/core/system/ImportExportComponent.h>
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
+
namespace armarx
{
/**
@@ -36,8 +36,7 @@ namespace armarx
* @see SimoxCSpaceVisualizerWidgetController
* @see MotionPlanningServerWidgetController
*/
- class ARMARXCOMPONENT_IMPORT_EXPORT MotionPlanningPlugin :
- public ArmarXGuiPlugin
+ class ARMARXCOMPONENT_IMPORT_EXPORT MotionPlanningPlugin : public ArmarXGuiPlugin
{
Q_OBJECT
Q_INTERFACES(ArmarXGuiInterface)
@@ -45,5 +44,4 @@ namespace armarx
public:
MotionPlanningPlugin();
};
-}
-
+} // namespace armarx
diff --git a/source/RobotComponents/gui-plugins/MotionPlanning/QtWidgets/IndexedQCheckBox.cpp b/source/RobotComponents/gui-plugins/MotionPlanning/QtWidgets/IndexedQCheckBox.cpp
index 46518716459c9397b77d970e8fffca847a27689a..fa28055ca487332a130bb91e91369ce320e15da9 100644
--- a/source/RobotComponents/gui-plugins/MotionPlanning/QtWidgets/IndexedQCheckBox.cpp
+++ b/source/RobotComponents/gui-plugins/MotionPlanning/QtWidgets/IndexedQCheckBox.cpp
@@ -22,4 +22,3 @@
* GNU General Public License
*/
#include "IndexedQCheckBox.h"
-
diff --git a/source/RobotComponents/gui-plugins/MotionPlanning/QtWidgets/IndexedQCheckBox.h b/source/RobotComponents/gui-plugins/MotionPlanning/QtWidgets/IndexedQCheckBox.h
index b77b87dfdf31f723c6f62340b379d578e593da23..4479544d274905ca59658b841cb625f1b389b50c 100644
--- a/source/RobotComponents/gui-plugins/MotionPlanning/QtWidgets/IndexedQCheckBox.h
+++ b/source/RobotComponents/gui-plugins/MotionPlanning/QtWidgets/IndexedQCheckBox.h
@@ -39,21 +39,19 @@ namespace armarx
* @param index The index to emit,
* @param parent The widget's parent.
*/
- IndexedQCheckBox(int index, QWidget* parent = nullptr):
- QCheckBox {parent},
- index {index}
+ IndexedQCheckBox(int index, QWidget* parent = nullptr) : QCheckBox{parent}, index{index}
{
connect(this, SIGNAL(stateChanged(int)), this, SLOT(emitStateChangedWithIndex(int)));
}
+
/**
* @brief ctor.
* @param index The index to emit,
* @param The checkboxes' label.
* @param parent The widget's parent.
*/
- IndexedQCheckBox(int index, const QString& text, QWidget* parent = nullptr):
- QCheckBox {text, parent},
- index {index}
+ IndexedQCheckBox(int index, const QString& text, QWidget* parent = nullptr) :
+ QCheckBox{text, parent}, index{index}
{
connect(this, SIGNAL(stateChanged(int)), this, SLOT(emitStateChangedWithIndex(int)));
}
@@ -66,11 +64,13 @@ namespace armarx
*/
void stateChangedIndex(int index, Qt::CheckState state);
private slots:
+
/**
* @brief Emitts the slot stateChangedIndex.
* @param state The check state.
*/
- void emitStateChangedWithIndex(int state)
+ void
+ emitStateChangedWithIndex(int state)
{
emit stateChangedIndex(index, static_cast<Qt::CheckState>(state));
}
@@ -81,4 +81,4 @@ namespace armarx
*/
int index;
};
-}
+} // namespace armarx
diff --git a/source/RobotComponents/gui-plugins/MotionPlanning/QtWidgets/MotionPlanningServerTaskList.cpp b/source/RobotComponents/gui-plugins/MotionPlanning/QtWidgets/MotionPlanningServerTaskList.cpp
index 6a486768988c62b63f61656182c3ec154f0ccaf7..6c3f1ee535204a1a46ef090c4a134d1de2d62cbb 100644
--- a/source/RobotComponents/gui-plugins/MotionPlanning/QtWidgets/MotionPlanningServerTaskList.cpp
+++ b/source/RobotComponents/gui-plugins/MotionPlanning/QtWidgets/MotionPlanningServerTaskList.cpp
@@ -22,17 +22,16 @@
* GNU General Public License
*/
#include "MotionPlanningServerTaskList.h"
-#include <RobotComponents/gui-plugins/MotionPlanning/QtWidgets/ui_MotionPlanningServerTaskList.h>
#include <Ice/LocalException.h>
#include <RobotComponents/components/MotionPlanning/util/PlanningUtil.h>
+#include <RobotComponents/gui-plugins/MotionPlanning/QtWidgets/ui_MotionPlanningServerTaskList.h>
using namespace armarx;
MotionPlanningServerTaskList::MotionPlanningServerTaskList(QWidget* parent) :
- QWidget(parent),
- ui(new Ui::MotionPlanningServerTaskList)
+ QWidget(parent), ui(new Ui::MotionPlanningServerTaskList)
{
ui->setupUi(this);
startTimer(100);
@@ -44,7 +43,8 @@ MotionPlanningServerTaskList::~MotionPlanningServerTaskList()
delete ui;
}
-std::optional<Ice::Long> MotionPlanningServerTaskList::getSelectedId()
+std::optional<Ice::Long>
+MotionPlanningServerTaskList::getSelectedId()
{
const auto&& rowsSelection = ui->tableWidget->selectionModel()->selectedRows();
if (rowsSelection.empty())
@@ -55,7 +55,8 @@ std::optional<Ice::Long> MotionPlanningServerTaskList::getSelectedId()
return ui->tableWidget->item(rowIndex, 4)->text().toLong();
}
-void MotionPlanningServerTaskList::updateList()
+void
+MotionPlanningServerTaskList::updateList()
{
auto selected = getSelectedId();
clearList();
@@ -73,16 +74,21 @@ void MotionPlanningServerTaskList::updateList()
for (std::size_t row = 0; row < data.size(); ++row)
{
const auto& datum = data.at(row);
- ui->tableWidget->setItem(row, 0, new QTableWidgetItem {QString::fromStdString(datum.taskName)});
- ui->tableWidget->setItem(row, 1, new QTableWidgetItem {QString::fromStdString(toString(datum.status))});
- ui->tableWidget->setItem(row, 2, new QTableWidgetItem
- {
- datum.taskIdent.category.empty() ?
- QString::fromStdString(datum.taskIdent.name) :
- QString::fromStdString(datum.taskIdent.category) + '\n' + QString::fromStdString(datum.taskIdent.name)
- });
- ui->tableWidget->setItem(row, 3, new QTableWidgetItem {QString::fromStdString(datum.typeId)});
- ui->tableWidget->setItem(row, 4, new QTableWidgetItem {QString::number(datum.internalId)});
+ ui->tableWidget->setItem(
+ row, 0, new QTableWidgetItem{QString::fromStdString(datum.taskName)});
+ ui->tableWidget->setItem(
+ row, 1, new QTableWidgetItem{QString::fromStdString(toString(datum.status))});
+ ui->tableWidget->setItem(
+ row,
+ 2,
+ new QTableWidgetItem{datum.taskIdent.category.empty()
+ ? QString::fromStdString(datum.taskIdent.name)
+ : QString::fromStdString(datum.taskIdent.category) + '\n' +
+ QString::fromStdString(datum.taskIdent.name)});
+ ui->tableWidget->setItem(
+ row, 3, new QTableWidgetItem{QString::fromStdString(datum.typeId)});
+ ui->tableWidget->setItem(
+ row, 4, new QTableWidgetItem{QString::number(datum.internalId)});
if (selected && *selected == datum.internalId)
{
rowToSelect = row;
@@ -107,17 +113,20 @@ void MotionPlanningServerTaskList::updateList()
}
}
-void MotionPlanningServerTaskList::clearList()
+void
+MotionPlanningServerTaskList::clearList()
{
ui->tableWidget->setRowCount(0);
}
-void MotionPlanningServerTaskList::enableAutoUpdate(bool enabled)
+void
+MotionPlanningServerTaskList::enableAutoUpdate(bool enabled)
{
ui->checkBoxAutoRefresh->setChecked(enabled);
}
-void MotionPlanningServerTaskList::timerEvent(QTimerEvent*)
+void
+MotionPlanningServerTaskList::timerEvent(QTimerEvent*)
{
if (ui->checkBoxAutoRefresh->isChecked())
{
diff --git a/source/RobotComponents/gui-plugins/MotionPlanning/QtWidgets/MotionPlanningServerTaskList.h b/source/RobotComponents/gui-plugins/MotionPlanning/QtWidgets/MotionPlanningServerTaskList.h
index 50db9ded800e2ecbc40aa73eeb2cb21446232cec..d0b0bb8cf6fb9b9d6e01464426587f98bdf62281 100644
--- a/source/RobotComponents/gui-plugins/MotionPlanning/QtWidgets/MotionPlanningServerTaskList.h
+++ b/source/RobotComponents/gui-plugins/MotionPlanning/QtWidgets/MotionPlanningServerTaskList.h
@@ -23,12 +23,12 @@
*/
#pragma once
-#include <RobotComponents/interface/components/MotionPlanning/MotionPlanningServer.h>
+#include <optional>
#include <QModelIndex>
#include <QWidget>
-#include <optional>
+#include <RobotComponents/interface/components/MotionPlanning/MotionPlanningServer.h>
namespace Ui
{
@@ -47,7 +47,8 @@ namespace armarx
std::optional<Ice::Long> getSelectedId();
- void setMotionPlanningServer(const MotionPlanningServerInterfacePrx& planningServerPrx)
+ void
+ setMotionPlanningServer(const MotionPlanningServerInterfacePrx& planningServerPrx)
{
planningServerProxy = planningServerPrx;
}
@@ -64,4 +65,4 @@ namespace armarx
Ui::MotionPlanningServerTaskList* ui;
MotionPlanningServerInterfacePrx planningServerProxy;
};
-}
+} // namespace armarx
diff --git a/source/RobotComponents/gui-plugins/MotionPlanning/SimoxCSpaceVisualizer/SimoxCSpaceVisualizerWidgetController.cpp b/source/RobotComponents/gui-plugins/MotionPlanning/SimoxCSpaceVisualizer/SimoxCSpaceVisualizerWidgetController.cpp
index fbacabe08ce084de689862c8f1c1a8a7310f0429..ffbbb1f1d20676c0b8e1d8adb585fa0052cdfed3 100644
--- a/source/RobotComponents/gui-plugins/MotionPlanning/SimoxCSpaceVisualizer/SimoxCSpaceVisualizerWidgetController.cpp
+++ b/source/RobotComponents/gui-plugins/MotionPlanning/SimoxCSpaceVisualizer/SimoxCSpaceVisualizerWidgetController.cpp
@@ -22,41 +22,43 @@
* GNU General Public License
*/
-#include <utility>
+#include "SimoxCSpaceVisualizerWidgetController.h"
+
#include <unordered_set>
+#include <utility>
-#include <QWidget>
-#include <QString>
-#include <QSlider>
-#include <QLabel>
-#include <QDoubleSpinBox>
-#include <QSpinBox>
#include <QComboBox>
-#include <QPushButton>
+#include <QDoubleSpinBox>
+#include <QFileDialog>
#include <QGroupBox>
+#include <QLabel>
+#include <QPushButton>
+#include <QSlider>
+#include <QSpinBox>
+#include <QString>
#include <QTableWidget>
#include <QTableWidgetItem>
-#include <QFileDialog>
+#include <QWidget>
+#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/RobotNodeSet.h>
#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualization.h>
#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>
#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationNode.h>
-#include <VirtualRobot/Robot.h>
-#include <VirtualRobot/RobotNodeSet.h>
+#include <VirtualRobot/Visualization/Visualization.h>
-#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
#include <ArmarXCore/core/ArmarXManager.h>
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
+
#include <RobotComponents/components/MotionPlanning/CSpace/ScaledCSpace.h>
-#include <RobotComponents/interface/components/MotionPlanning/Tasks/MotionPlanningTask.h>
#include <RobotComponents/components/MotionPlanning/MotionPlanningObjectFactories.h>
-#include <VirtualRobot/Visualization/Visualization.h>
-#include <MotionPlanning/CSpace/CSpaceSampled.h>
+#include <RobotComponents/interface/components/MotionPlanning/Tasks/MotionPlanningTask.h>
-#include "SimoxCSpaceVisualizerWidgetController.h"
-#include <VirtualRobot/Robot.h>
+#include <MotionPlanning/CSpace/CSpaceSampled.h>
-void clearLayout(QLayout* layout, bool deleteWidgets = true)
+void
+clearLayout(QLayout* layout, bool deleteWidgets = true)
{
while (QLayoutItem* item = layout->takeAt(0))
{
@@ -77,18 +79,9 @@ void clearLayout(QLayout* layout, bool deleteWidgets = true)
using namespace armarx;
-static const float EDGE_WIDTH
-{
- 1.f
-};
-static const armarx::DrawColor EDGE_COLOR_TREE
-{
- 0.f, 1.f, 0.f, 1.f
-};
-static const armarx::DrawColor EDGE_COLOR_PATH
-{
- 0.f, 0.f, 1.f, 1.f
-};
+static const float EDGE_WIDTH{1.f};
+static const armarx::DrawColor EDGE_COLOR_TREE{0.f, 1.f, 0.f, 1.f};
+static const armarx::DrawColor EDGE_COLOR_PATH{0.f, 0.f, 1.f, 1.f};
SimoxCSpaceVisualizerWidgetController::SimoxCSpaceVisualizerWidgetController()
{
@@ -103,7 +96,7 @@ SimoxCSpaceVisualizerWidgetController::SimoxCSpaceVisualizerWidgetController()
//add list for server tasks
- motionPlanningServerTaskList = new MotionPlanningServerTaskList {};
+ motionPlanningServerTaskList = new MotionPlanningServerTaskList{};
widget.loadFromServerLayout->addWidget(motionPlanningServerTaskList);
updateCollisionStateTimer.setSingleShot(true);
@@ -111,22 +104,32 @@ SimoxCSpaceVisualizerWidgetController::SimoxCSpaceVisualizerWidgetController()
connect(&updateCollisionStateTimer, SIGNAL(timeout()), this, SLOT(updateCollisionState()));
}
-void SimoxCSpaceVisualizerWidgetController::loadSettings(QSettings* settings)
+void
+SimoxCSpaceVisualizerWidgetController::loadSettings(QSettings* settings)
{
- motionPlanningServerProxyName = settings->value("motionPlanningServerProxyName", QString::fromStdString(motionPlanningServerProxyName)).toString().toStdString();
+ motionPlanningServerProxyName =
+ settings
+ ->value("motionPlanningServerProxyName",
+ QString::fromStdString(motionPlanningServerProxyName))
+ .toString()
+ .toStdString();
}
-void SimoxCSpaceVisualizerWidgetController::saveSettings(QSettings* settings)
+void
+SimoxCSpaceVisualizerWidgetController::saveSettings(QSettings* settings)
{
- settings->setValue("motionPlanningServerProxyName", QString::fromStdString(motionPlanningServerProxyName));
+ settings->setValue("motionPlanningServerProxyName",
+ QString::fromStdString(motionPlanningServerProxyName));
}
-QString SimoxCSpaceVisualizerWidgetController::getWidgetName() const
+QString
+SimoxCSpaceVisualizerWidgetController::getWidgetName() const
{
return "MotionPlanning.SimoxCSpaceVisualizer";
}
-void SimoxCSpaceVisualizerWidgetController::onInitComponent()
+void
+SimoxCSpaceVisualizerWidgetController::onInitComponent()
{
visuRoot = new SoSeparator();
visuRoot->ref();
@@ -139,7 +142,8 @@ void SimoxCSpaceVisualizerWidgetController::onInitComponent()
// create the debugdrawer
std::string debugDrawerComponentName = generateSubObjectName("DebugDrawer");
ARMARX_INFO << "Creating debug drawer component " << debugDrawerComponentName;
- debugDrawer = Component::create<armarx::DebugDrawerComponent>(getIceProperties(), debugDrawerComponentName);
+ debugDrawer = Component::create<armarx::DebugDrawerComponent>(getIceProperties(),
+ debugDrawerComponentName);
if (mutex3D)
{
@@ -163,25 +167,30 @@ void SimoxCSpaceVisualizerWidgetController::onInitComponent()
usingProxy(motionPlanningServerProxyName);
}
-QPointer<QDialog> SimoxCSpaceVisualizerWidgetController::getConfigDialog(QWidget* parent)
+QPointer<QDialog>
+SimoxCSpaceVisualizerWidgetController::getConfigDialog(QWidget* parent)
{
if (!dialog)
{
dialog = new SimpleConfigDialog(parent);
- dialog->addProxyFinder<MotionPlanningServerInterfacePrx>({"MotionPlanningServer", "The MotionPlanningServer (empty for none)", "*Server"});
+ dialog->addProxyFinder<MotionPlanningServerInterfacePrx>(
+ {"MotionPlanningServer", "The MotionPlanningServer (empty for none)", "*Server"});
}
return qobject_cast<SimpleConfigDialog*>(dialog);
}
-void SimoxCSpaceVisualizerWidgetController::onConnectComponent()
+void
+SimoxCSpaceVisualizerWidgetController::onConnectComponent()
{
- motionPlanningServerProxy = getProxy<MotionPlanningServerInterfacePrx>(motionPlanningServerProxyName);
+ motionPlanningServerProxy =
+ getProxy<MotionPlanningServerInterfacePrx>(motionPlanningServerProxyName);
motionPlanningServerTaskList->setMotionPlanningServer(motionPlanningServerProxy);
motionPlanningServerTaskList->enableAutoUpdate(true);
resetSimoxCSpace();
}
-SoNode* SimoxCSpaceVisualizerWidgetController::getScene()
+SoNode*
+SimoxCSpaceVisualizerWidgetController::getScene()
{
if (visuRoot)
{
@@ -191,7 +200,8 @@ SoNode* SimoxCSpaceVisualizerWidgetController::getScene()
return visuRoot;
}
-void SimoxCSpaceVisualizerWidgetController::setMutex3D(RecursiveMutexPtr const& mutex3D)
+void
+SimoxCSpaceVisualizerWidgetController::setMutex3D(RecursiveMutexPtr const& mutex3D)
{
this->mutex3D = mutex3D;
@@ -201,25 +211,39 @@ void SimoxCSpaceVisualizerWidgetController::setMutex3D(RecursiveMutexPtr const&
}
}
-void SimoxCSpaceVisualizerWidgetController::connectSlots()
+void
+SimoxCSpaceVisualizerWidgetController::connectSlots()
{
connect(widget.pushButtonLoadTask, SIGNAL(clicked()), this, SLOT(loadTask()));
connect(widget.checkBoxShowTreeEdges, SIGNAL(clicked()), this, SLOT(updateEdgeVisibility()));
connect(widget.checkBoxDrawTrace, SIGNAL(clicked(bool)), this, SLOT(drawEdges(bool)));
- connect(widget.doubleSpinBoxTraceStep, SIGNAL(valueChanged(double)), this, SLOT(traceStepChanged()));
+ connect(widget.doubleSpinBoxTraceStep,
+ SIGNAL(valueChanged(double)),
+ this,
+ SLOT(traceStepChanged()));
connect(widget.pushButtonHideAllPaths, SIGNAL(clicked()), this, SLOT(hideAllPaths()));
connect(widget.pushButtonSetConfig, SIGNAL(clicked()), this, SLOT(setCurrentConfig()));
connect(widget.spinBoxPathNumber, SIGNAL(valueChanged(int)), this, SLOT(setCurrentPath()));
- connect(widget.sliderPathPosition, SIGNAL(valueChanged(double)), this, SLOT(setCurrentPathPosition()));
- connect(widget.sliderPathPosition, SIGNAL(valueChanged(double)), this, SLOT(setCurrentPathPosition()));
- connect(widget.checkBoxCollisionNodeHighlighting, SIGNAL(toggled(bool)), this, SLOT(highlightCollisionNodes(bool)));
+ connect(widget.sliderPathPosition,
+ SIGNAL(valueChanged(double)),
+ this,
+ SLOT(setCurrentPathPosition()));
+ connect(widget.sliderPathPosition,
+ SIGNAL(valueChanged(double)),
+ this,
+ SLOT(setCurrentPathPosition()));
+ connect(widget.checkBoxCollisionNodeHighlighting,
+ SIGNAL(toggled(bool)),
+ this,
+ SLOT(highlightCollisionNodes(bool)));
startTimer(timerPeriod);
}
-void SimoxCSpaceVisualizerWidgetController::traceStepChanged()
+void
+SimoxCSpaceVisualizerWidgetController::traceStepChanged()
{
if (widget.checkBoxDrawTrace->isChecked())
{
@@ -227,22 +251,26 @@ void SimoxCSpaceVisualizerWidgetController::traceStepChanged()
}
}
-void SimoxCSpaceVisualizerWidgetController::updateEdgeVisibility()
+void
+SimoxCSpaceVisualizerWidgetController::updateEdgeVisibility()
{
for (std::size_t pIdx = 0; pIdx < paths.size(); ++pIdx)
{
auto&& path = paths.at(pIdx);
- debugDrawer->enableLayerVisu(PathData::getEdgeLayerName(pIdx), path.visible && widget.checkBoxShowTreeEdges->isChecked());
+ debugDrawer->enableLayerVisu(PathData::getEdgeLayerName(pIdx),
+ path.visible && widget.checkBoxShowTreeEdges->isChecked());
}
}
-void SimoxCSpaceVisualizerWidgetController::loadTask()
+void
+SimoxCSpaceVisualizerWidgetController::loadTask()
{
//switch here if new sources are added
loadTaskFromServer();
}
-void SimoxCSpaceVisualizerWidgetController::setCurrentPath()
+void
+SimoxCSpaceVisualizerWidgetController::setCurrentPath()
{
resetCurrentPath();
int newPath = widget.spinBoxPathNumber->value() - 1;
@@ -261,7 +289,8 @@ void SimoxCSpaceVisualizerWidgetController::setCurrentPath()
setCurrentPathPosition();
}
-void SimoxCSpaceVisualizerWidgetController::setCurrentPathPosition()
+void
+SimoxCSpaceVisualizerWidgetController::setCurrentPathPosition()
{
if (!widget.spinBoxPathNumber->value())
{
@@ -272,7 +301,8 @@ void SimoxCSpaceVisualizerWidgetController::setCurrentPathPosition()
const auto& pathAcc = getCurrentPath().accPathLength;
const auto val = widget.sliderPathPosition->value();
widget.labelCurrentPathCurrentLen->setText(QString::number(val));
- auto indexTo = std::distance(pathAcc.begin(), std::upper_bound(pathAcc.begin(), pathAcc.end(), val));
+ auto indexTo =
+ std::distance(pathAcc.begin(), std::upper_bound(pathAcc.begin(), pathAcc.end(), val));
widget.labelCurrentPathCurrentNode->setText(QString::number(indexTo));
assert(indexTo >= 0);
@@ -292,7 +322,7 @@ void SimoxCSpaceVisualizerWidgetController::setCurrentPathPosition()
auto indexFrom = indexTo - 1;
auto cfgFrom = path.at(indexFrom);
- auto cfgTo = path.at(indexTo);
+ auto cfgTo = path.at(indexTo);
auto t = (val - pathAcc.at(indexFrom)) / (pathAcc.at(indexTo) - pathAcc.at(indexFrom));
VectorXf cfgToSet;
@@ -310,28 +340,30 @@ void SimoxCSpaceVisualizerWidgetController::setCurrentPathPosition()
if (simoxcspace)
{
// do robot specific interpolation
- VirtualRobot::RobotNodeSetPtr rns = VirtualRobot::RobotNodeSet::createRobotNodeSet(simoxcspace->getAgentSceneObj(), "tmp",
- simoxcspace->getAgentJointNames());
- Saba::CSpaceSampled tmpCSpace(simoxcspace->getAgentSceneObj(), VirtualRobot::CDManagerPtr(new VirtualRobot::CDManager(simoxcspace->getCD())), rns);
+ VirtualRobot::RobotNodeSetPtr rns = VirtualRobot::RobotNodeSet::createRobotNodeSet(
+ simoxcspace->getAgentSceneObj(), "tmp", simoxcspace->getAgentJointNames());
+ Saba::CSpaceSampled tmpCSpace(
+ simoxcspace->getAgentSceneObj(),
+ VirtualRobot::CDManagerPtr(new VirtualRobot::CDManager(simoxcspace->getCD())),
+ rns);
Eigen::VectorXf from = Eigen::Map<Eigen::VectorXf>(cfgFrom.data(), cfgFrom.size());
Eigen::VectorXf to = Eigen::Map<Eigen::VectorXf>(cfgTo.data(), cfgTo.size());
auto config = tmpCSpace.interpolate(from, to, t);
- cfgToSet = (VectorXf {config.data(), config.data() + config.rows()});
-
+ cfgToSet = (VectorXf{config.data(), config.data() + config.rows()});
}
else
{
- std::transform(cfgFrom.begin(), cfgFrom.end(), cfgTo.begin(), std::back_inserter(cfgToSet),
- [t](float from, float to)
- {
- return t * to + (1 - t) * from;
- }
- );
+ std::transform(cfgFrom.begin(),
+ cfgFrom.end(),
+ cfgTo.begin(),
+ std::back_inserter(cfgToSet),
+ [t](float from, float to) { return t * to + (1 - t) * from; });
}
setAndApplyConfig(cfgToSet);
}
-void SimoxCSpaceVisualizerWidgetController::setCurrentConfig()
+void
+SimoxCSpaceVisualizerWidgetController::setCurrentConfig()
{
if (!cspaceVisu)
{
@@ -342,7 +374,8 @@ void SimoxCSpaceVisualizerWidgetController::setCurrentConfig()
for (std::size_t i = 0; i < cfg.size(); ++i)
{
- cfg.at(i) = dynamic_cast<QDoubleSpinBox*>(widget.tableWidgetDims->cellWidget(i, 2))->value();
+ cfg.at(i) =
+ dynamic_cast<QDoubleSpinBox*>(widget.tableWidgetDims->cellWidget(i, 2))->value();
}
ARMARX_DEBUG_S << "Setting visu config to: " << cfg;
@@ -354,14 +387,16 @@ void SimoxCSpaceVisualizerWidgetController::setCurrentConfig()
updateCollisionStateTimer.start(100);
}
-void SimoxCSpaceVisualizerWidgetController::updateCollisionState()
+void
+SimoxCSpaceVisualizerWidgetController::updateCollisionState()
{
widget.labelCurrentMinObjDist->setText(QString::number(cspaceVisu->getCD().getDistance()));
- widget.labelCurrentCollisionState->setText(cspaceVisu->getCD().isInCollision() ? QString {"in collision"}: QString {"no collision"});
-
+ widget.labelCurrentCollisionState->setText(
+ cspaceVisu->getCD().isInCollision() ? QString{"in collision"} : QString{"no collision"});
}
-void SimoxCSpaceVisualizerWidgetController::drawEdges(bool drawTrace)
+void
+SimoxCSpaceVisualizerWidgetController::drawEdges(bool drawTrace)
{
std::vector<std::string> nodeNames;
nodeNames.reserve(robotNodes.size());
@@ -386,7 +421,9 @@ void SimoxCSpaceVisualizerWidgetController::drawEdges(bool drawTrace)
}
//extract poses for each node and cfg
- const float traceStep = drawTrace ? static_cast<float>(widget.doubleSpinBoxTraceStep->value()) : std::numeric_limits<float>::infinity();
+ const float traceStep = drawTrace
+ ? static_cast<float>(widget.doubleSpinBoxTraceStep->value())
+ : std::numeric_limits<float>::infinity();
auto traces = cspaceUtil->getTraceVisu(p, nodeNames, traceStep);
//draw
@@ -402,15 +439,18 @@ void SimoxCSpaceVisualizerWidgetController::drawEdges(bool drawTrace)
}
}
-void SimoxCSpaceVisualizerWidgetController::setVisibilityPath(int index, Qt::CheckState state)
+void
+SimoxCSpaceVisualizerWidgetController::setVisibilityPath(int index, Qt::CheckState state)
{
auto&& path = paths.at(index);
const bool vis = (state == Qt::Checked);
path.visible = vis;
- debugDrawer->enableLayerVisu(PathData::getEdgeLayerName(index), vis && widget.checkBoxShowTreeEdges->isChecked());
+ debugDrawer->enableLayerVisu(PathData::getEdgeLayerName(index),
+ vis && widget.checkBoxShowTreeEdges->isChecked());
}
-void SimoxCSpaceVisualizerWidgetController::hideAllPaths()
+void
+SimoxCSpaceVisualizerWidgetController::hideAllPaths()
{
for (int i = 0; static_cast<std::size_t>(i) < paths.size(); ++i)
{
@@ -418,7 +458,8 @@ void SimoxCSpaceVisualizerWidgetController::hideAllPaths()
}
}
-void SimoxCSpaceVisualizerWidgetController::loadTaskFromServer()
+void
+SimoxCSpaceVisualizerWidgetController::loadTaskFromServer()
{
if (!motionPlanningServerProxy)
{
@@ -464,13 +505,15 @@ void SimoxCSpaceVisualizerWidgetController::loadTaskFromServer()
}
}
-void SimoxCSpaceVisualizerWidgetController::soSeparatorCleanUpAndRemoveFromRoot(SoSeparator*& toRemove)
+void
+SimoxCSpaceVisualizerWidgetController::soSeparatorCleanUpAndRemoveFromRoot(SoSeparator*& toRemove)
{
soSeparatorRemoveFromRoot(toRemove);
soSeparatorCleanUp(toRemove);
}
-void SimoxCSpaceVisualizerWidgetController::soSeparatorCleanUp(SoSeparator*& toClear)
+void
+SimoxCSpaceVisualizerWidgetController::soSeparatorCleanUp(SoSeparator*& toClear)
{
std::unique_lock lock(*mutex3D);
@@ -482,7 +525,8 @@ void SimoxCSpaceVisualizerWidgetController::soSeparatorCleanUp(SoSeparator*& toC
}
}
-void SimoxCSpaceVisualizerWidgetController::soSeparatorRemoveFromRoot(SoSeparator* toRemove)
+void
+SimoxCSpaceVisualizerWidgetController::soSeparatorRemoveFromRoot(SoSeparator* toRemove)
{
std::unique_lock lock(*mutex3D);
@@ -497,7 +541,8 @@ void SimoxCSpaceVisualizerWidgetController::soSeparatorRemoveFromRoot(SoSeparato
}
}
-void SimoxCSpaceVisualizerWidgetController::soSeparatorAddToRoot(SoSeparator* toAdd)
+void
+SimoxCSpaceVisualizerWidgetController::soSeparatorAddToRoot(SoSeparator* toAdd)
{
std::unique_lock lock(*mutex3D);
@@ -512,7 +557,8 @@ void SimoxCSpaceVisualizerWidgetController::soSeparatorAddToRoot(SoSeparator* to
}
}
-void SimoxCSpaceVisualizerWidgetController::soSeparatorToggleRootChild(SoSeparator* toToggle)
+void
+SimoxCSpaceVisualizerWidgetController::soSeparatorToggleRootChild(SoSeparator* toToggle)
{
std::unique_lock lock(*mutex3D);
@@ -531,7 +577,8 @@ void SimoxCSpaceVisualizerWidgetController::soSeparatorToggleRootChild(SoSeparat
}
}
-void SimoxCSpaceVisualizerWidgetController::soSeparatorRootChildVis(SoSeparator* child, bool visible)
+void
+SimoxCSpaceVisualizerWidgetController::soSeparatorRootChildVis(SoSeparator* child, bool visible)
{
std::unique_lock lock(*mutex3D);
@@ -556,7 +603,8 @@ void SimoxCSpaceVisualizerWidgetController::soSeparatorRootChildVis(SoSeparator*
}
}
-void SimoxCSpaceVisualizerWidgetController::setSimoxCSpace(SimoxCSpacePtr newCSpace)
+void
+SimoxCSpaceVisualizerWidgetController::setSimoxCSpace(SimoxCSpacePtr newCSpace)
{
resetSimoxCSpace();
cspaceUtil = SimoxCSpacePtr::dynamicCast(newCSpace->clone());
@@ -576,15 +624,16 @@ void SimoxCSpaceVisualizerWidgetController::setSimoxCSpace(SimoxCSpacePtr newCSp
const std::string& name = jointNames.at(i);
const FloatRange& bounds = dimensions.at(i);
- widget.tableWidgetDims->setItem(i, 0, new QTableWidgetItem {QString::fromStdString(name)});
+ widget.tableWidgetDims->setItem(i, 0, new QTableWidgetItem{QString::fromStdString(name)});
- widget.tableWidgetDims->setItem(i, 1, new QTableWidgetItem {QString::number(bounds.min)});
- auto spinbox = new QDoubleSpinBox {};
+ widget.tableWidgetDims->setItem(i, 1, new QTableWidgetItem{QString::number(bounds.min)});
+ auto spinbox = new QDoubleSpinBox{};
spinbox->setRange(bounds.min, bounds.max);
widget.tableWidgetDims->setCellWidget(i, 2, spinbox);
- widget.tableWidgetDims->setItem(i, 3, new QTableWidgetItem {QString::number(bounds.max)});
+ widget.tableWidgetDims->setItem(i, 3, new QTableWidgetItem{QString::number(bounds.max)});
spinbox->setSingleStep((bounds.max - bounds.min) / 10000.0);
- connect(spinbox, SIGNAL(valueChanged(double)), widget.pushButtonSetConfig, SIGNAL(clicked()));
+ connect(
+ spinbox, SIGNAL(valueChanged(double)), widget.pushButtonSetConfig, SIGNAL(clicked()));
}
visuObjStat.resize(cspaceVisu->getStationaryObjectSet()->getSize());
@@ -593,14 +642,15 @@ void SimoxCSpaceVisualizerWidgetController::setSimoxCSpace(SimoxCSpacePtr newCSp
std::unique_lock lock(*mutex3D);
auto robot = cspaceVisu->getAgentSceneObj();
robot->setUpdateVisualization(true);
- robotVisu = robot->getVisualization<VirtualRobot::CoinVisualization>();
+ robotVisu = robot->getVisualization();
ARMARX_IMPORTANT << "Setting visu of robot " << robot.get();
auto robotCoinVisu = robotVisu->getCoinVisualization(true);
- highlightCollisionNodes(widget.checkBoxCollisionNodeHighlighting->checkState() == Qt::Checked);
+ highlightCollisionNodes(widget.checkBoxCollisionNodeHighlighting->checkState() ==
+ Qt::Checked);
visuAgent->removeAllChildren();
visuAgent->addChild(robotCoinVisu);
//add to table
- auto boxVis = new QCheckBox {QString::fromStdString(robot->getName())};
+ auto boxVis = new QCheckBox{QString::fromStdString(robot->getName())};
boxVis->setChecked(true);
connect(boxVis, SIGNAL(stateChanged(int)), this, SLOT(setVisibilityAgent(int)));
@@ -619,15 +669,19 @@ void SimoxCSpaceVisualizerWidgetController::setSimoxCSpace(SimoxCSpacePtr newCSp
sep->ref();
sceneObj->setUpdateVisualization(true);
sceneObj->setUpdateCollisionModel(true);
- auto* visu = dynamic_cast<VirtualRobot::CoinVisualizationNode*>(sceneObj->getVisualization(VirtualRobot::SceneObject::Collision).get());
+ auto* visu = dynamic_cast<VirtualRobot::CoinVisualizationNode*>(
+ sceneObj->getVisualization(VirtualRobot::SceneObject::Collision).get());
auto coin = visu->getCoinVisualization();
ARMARX_INFO << sceneObj->getName() << " visu: " << visu << " coin: " << coin;
sep->addChild(coin);
soSeparatorAddToRoot(sep);
//add to table
- auto boxVis = new IndexedQCheckBox {i, QString::fromStdString(sceneObj->getName())};
+ auto boxVis = new IndexedQCheckBox{i, QString::fromStdString(sceneObj->getName())};
boxVis->setChecked(true);
- connect(boxVis, SIGNAL(stateChangedIndex(int, Qt::CheckState)), this, SLOT(setVisibilityObjectStationary(int, Qt::CheckState)));
+ connect(boxVis,
+ SIGNAL(stateChangedIndex(int, Qt::CheckState)),
+ this,
+ SLOT(setVisibilityObjectStationary(int, Qt::CheckState)));
widget.scrollAreaWidgetContentsObj->layout()->addWidget(boxVis);
}
@@ -635,24 +689,22 @@ void SimoxCSpaceVisualizerWidgetController::setSimoxCSpace(SimoxCSpacePtr newCSp
//add list of nodes
auto nodes = cspaceUtil->getAgentSceneObj()->getRobotNodes();
robotNodes.clear();
- std::transform(
- nodes.begin(), nodes.end(), std::back_inserter(robotNodes),
- [](VirtualRobot::RobotNodePtr & nptr)
- {
- return std::pair<VirtualRobot::RobotNodePtr, bool> {std::move(nptr), false};
- }
- );
+ std::transform(nodes.begin(),
+ nodes.end(),
+ std::back_inserter(robotNodes),
+ [](VirtualRobot::RobotNodePtr& nptr) {
+ return std::pair<VirtualRobot::RobotNodePtr, bool>{std::move(nptr), false};
+ });
//tcps of used kinematic chains are visible per default
std::set<std::string> visTcps;
const auto& kinematicChainNames = cspaceUtil->getAgent().kinemaicChainNames;
std::transform(
- kinematicChainNames.begin(), kinematicChainNames.end(), std::inserter(visTcps, visTcps.begin()),
- [this](const std::string & name)
- {
- return cspaceUtil->getAgentSceneObj()->getRobotNodeSet(name)->getTCP()->getName();
- }
- );
+ kinematicChainNames.begin(),
+ kinematicChainNames.end(),
+ std::inserter(visTcps, visTcps.begin()),
+ [this](const std::string& name)
+ { return cspaceUtil->getAgentSceneObj()->getRobotNodeSet(name)->getTCP()->getName(); });
for (int i = 0; i < static_cast<int>(robotNodes.size()); ++i)
{
@@ -661,9 +713,12 @@ void SimoxCSpaceVisualizerWidgetController::setSimoxCSpace(SimoxCSpacePtr newCSp
bool visible = visTcps.find(name) != visTcps.end();
robotNodes.at(i).second = visible;
- auto boxVis = new IndexedQCheckBox {i, QString::fromStdString(node->getName())};
+ auto boxVis = new IndexedQCheckBox{i, QString::fromStdString(node->getName())};
boxVis->setChecked(visible);
- connect(boxVis, SIGNAL(stateChangedIndex(int, Qt::CheckState)), this, SLOT(toggleRobotNodePath(int, Qt::CheckState)));
+ connect(boxVis,
+ SIGNAL(stateChangedIndex(int, Qt::CheckState)),
+ this,
+ SLOT(toggleRobotNodePath(int, Qt::CheckState)));
widget.scrollAreaWidgetContentsJoints->layout()->addWidget(boxVis);
}
@@ -671,39 +726,41 @@ void SimoxCSpaceVisualizerWidgetController::setSimoxCSpace(SimoxCSpacePtr newCSp
setEnabledSimoxCSpace();
}
-void SimoxCSpaceVisualizerWidgetController::toggleRobotNodePath(int index, Qt::CheckState)
+void
+SimoxCSpaceVisualizerWidgetController::toggleRobotNodePath(int index, Qt::CheckState)
{
robotNodes.at(index).second ^= 1;
drawEdges();
}
-void SimoxCSpaceVisualizerWidgetController::setPaths(std::vector<Path> newPaths)
+void
+SimoxCSpaceVisualizerWidgetController::setPaths(std::vector<Path> newPaths)
{
resetPaths();
assert(paths.empty());
- std::transform(
- newPaths.begin(), newPaths.end(), std::back_inserter(paths),
- [](Path & p)
- {
- PathData dat;
- dat.path = std::move(p.nodes);
+ std::transform(newPaths.begin(),
+ newPaths.end(),
+ std::back_inserter(paths),
+ [](Path& p)
+ {
+ PathData dat;
+ dat.path = std::move(p.nodes);
- float lengthAcc = 0;
- dat.accPathLength.emplace_back(0);
- for (std::size_t i = 1; i < dat.path.size(); ++i)
- {
- const auto& cfg1 = dat.path.at(i - 1);
- const auto& cfg2 = dat.path.at(i);
+ float lengthAcc = 0;
+ dat.accPathLength.emplace_back(0);
+ for (std::size_t i = 1; i < dat.path.size(); ++i)
+ {
+ const auto& cfg1 = dat.path.at(i - 1);
+ const auto& cfg2 = dat.path.at(i);
- lengthAcc += euclideanDistance(cfg1.begin(), cfg1.end(), cfg2.begin());
- dat.accPathLength.emplace_back(lengthAcc);
- }
+ lengthAcc += euclideanDistance(cfg1.begin(), cfg1.end(), cfg2.begin());
+ dat.accPathLength.emplace_back(lengthAcc);
+ }
- return dat;
- }
- );
+ return dat;
+ });
widget.spinBoxPathNumber->setMaximum(paths.size());
@@ -711,11 +768,17 @@ void SimoxCSpaceVisualizerWidgetController::setPaths(std::vector<Path> newPaths)
for (std::size_t i = 0; i < paths.size(); ++i)
{
const auto& p = paths.at(i);
- widget.tableWidgetPaths->setItem(i, 1, new QTableWidgetItem {QString::number(p.path.size())});
- widget.tableWidgetPaths->setItem(i, 2, new QTableWidgetItem {QString::number(p.getLength())});
-
- auto boxVis = new IndexedQCheckBox {static_cast<int>(i), QString::fromStdString(newPaths.at(i).pathName)};
- connect(boxVis, SIGNAL(stateChangedIndex(int, Qt::CheckState)), this, SLOT(setVisibilityPath(int, Qt::CheckState)));
+ widget.tableWidgetPaths->setItem(
+ i, 1, new QTableWidgetItem{QString::number(p.path.size())});
+ widget.tableWidgetPaths->setItem(
+ i, 2, new QTableWidgetItem{QString::number(p.getLength())});
+
+ auto boxVis = new IndexedQCheckBox{static_cast<int>(i),
+ QString::fromStdString(newPaths.at(i).pathName)};
+ connect(boxVis,
+ SIGNAL(stateChangedIndex(int, Qt::CheckState)),
+ this,
+ SLOT(setVisibilityPath(int, Qt::CheckState)));
boxVis->setChecked(true);
widget.tableWidgetPaths->setCellWidget(i, 0, boxVis);
}
@@ -726,7 +789,8 @@ void SimoxCSpaceVisualizerWidgetController::setPaths(std::vector<Path> newPaths)
updateEdgeVisibility();
}
-void SimoxCSpaceVisualizerWidgetController::setAndApplyConfig(const VectorXf& cfg)
+void
+SimoxCSpaceVisualizerWidgetController::setAndApplyConfig(const VectorXf& cfg)
{
if (widget.tableWidgetDims->columnCount() < 2)
{
@@ -743,7 +807,8 @@ void SimoxCSpaceVisualizerWidgetController::setAndApplyConfig(const VectorXf& cf
setCurrentConfig();
}
-void SimoxCSpaceVisualizerWidgetController::resetSimoxCSpace()
+void
+SimoxCSpaceVisualizerWidgetController::resetSimoxCSpace()
{
resetPaths();
setEnabledSimoxCSpace(false);
@@ -767,7 +832,8 @@ void SimoxCSpaceVisualizerWidgetController::resetSimoxCSpace()
cspaceVisu = nullptr;
}
-void SimoxCSpaceVisualizerWidgetController::resetPaths()
+void
+SimoxCSpaceVisualizerWidgetController::resetPaths()
{
setEnabledPaths(false);
resetCurrentPath();
@@ -777,20 +843,23 @@ void SimoxCSpaceVisualizerWidgetController::resetPaths()
paths.clear();
}
-void SimoxCSpaceVisualizerWidgetController::setEnabledSimoxCSpace(bool active)
+void
+SimoxCSpaceVisualizerWidgetController::setEnabledSimoxCSpace(bool active)
{
widget.groupBoxCurCfg->setEnabled(active);
widget.groupBoxVisPath->setEnabled(active);
widget.groupBoxVisObj->setEnabled(active);
}
-void SimoxCSpaceVisualizerWidgetController::setEnabledPaths(bool active)
+void
+SimoxCSpaceVisualizerWidgetController::setEnabledPaths(bool active)
{
widget.groupBoxPaths->setEnabled(active);
widget.groupBoxPathOpt->setEnabled(active);
}
-void SimoxCSpaceVisualizerWidgetController::timerEvent(QTimerEvent*)
+void
+SimoxCSpaceVisualizerWidgetController::timerEvent(QTimerEvent*)
{
const auto step = widget.doubleSpinBoxMovePerMs->value() * timerPeriod;
@@ -800,7 +869,8 @@ void SimoxCSpaceVisualizerWidgetController::timerEvent(QTimerEvent*)
}
}
-void SimoxCSpaceVisualizerWidgetController::highlightCollisionNodes(bool enabled)
+void
+SimoxCSpaceVisualizerWidgetController::highlightCollisionNodes(bool enabled)
{
if (!cspaceVisu)
{
@@ -830,8 +900,7 @@ void SimoxCSpaceVisualizerWidgetController::highlightCollisionNodes(bool enabled
for (auto& name : nodesToHighlight)
{
auto node = robot->getRobotNode(name);
- ARMARX_INFO << "highlighting: " << node->getName() << ": " << enabled;
+ ARMARX_INFO << "highlighting: " << node->getName() << ": " << enabled;
node->highlight(robotVisu, enabled);
-
}
}
diff --git a/source/RobotComponents/gui-plugins/MotionPlanning/SimoxCSpaceVisualizer/SimoxCSpaceVisualizerWidgetController.h b/source/RobotComponents/gui-plugins/MotionPlanning/SimoxCSpaceVisualizer/SimoxCSpaceVisualizerWidgetController.h
index ef85dee2e618d50ec31199419be764848cf6c12a..61c838c66fa4023fc25d7890aaad7d0f818cc0df 100644
--- a/source/RobotComponents/gui-plugins/MotionPlanning/SimoxCSpaceVisualizer/SimoxCSpaceVisualizerWidgetController.h
+++ b/source/RobotComponents/gui-plugins/MotionPlanning/SimoxCSpaceVisualizer/SimoxCSpaceVisualizerWidgetController.h
@@ -24,31 +24,34 @@
#pragma once
-#include <vector>
#include <deque>
-#include <unordered_map>
-#include <string>
#include <memory>
+#include <string>
#include <tuple>
+#include <unordered_map>
+#include <vector>
-#include <QPointer>
#include <QCheckBox>
+#include <QPointer>
#include <QTimer>
-#include <Inventor/nodes/SoSeparator.h>
#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualization.h>
-#include <RobotAPI/components/DebugDrawer/DebugDrawerComponent.h>
+#include <ArmarXCore/core/system/ImportExportComponent.h>
+
#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXWidgetController.h>
-#include <ArmarXCore/core/system/ImportExportComponent.h>
+#include <RobotAPI/components/DebugDrawer/DebugDrawerComponent.h>
+
+#include <RobotComponents/components/MotionPlanning/CSpace/ScaledCSpace.h>
#include <RobotComponents/components/MotionPlanning/CSpace/SimoxCSpace.h>
#include <RobotComponents/components/MotionPlanning/util/CollisionCheckUtil.h>
+#include <RobotComponents/components/MotionPlanning/util/Metrics.h>
+
#include "../QtWidgets/IndexedQCheckBox.h"
#include "../QtWidgets/MotionPlanningServerTaskList.h"
-#include <RobotComponents/components/MotionPlanning/CSpace/ScaledCSpace.h>
-#include <RobotComponents/components/MotionPlanning/util/Metrics.h>
+#include <Inventor/nodes/SoSeparator.h>
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wpedantic"
@@ -97,8 +100,7 @@ namespace armarx
* @image html mplan_simox_cspace_gui-set_pose.png
* To set a pose, edit the joint values in the bottom left.
*/
- class ARMARXCOMPONENT_IMPORT_EXPORT
- SimoxCSpaceVisualizerWidgetController:
+ class ARMARXCOMPONENT_IMPORT_EXPORT SimoxCSpaceVisualizerWidgetController :
public armarx::ArmarXComponentWidgetController
{
Q_OBJECT
@@ -115,16 +117,19 @@ namespace armarx
/**
* @return The path's length.
*/
- float getLength() const
+ float
+ getLength() const
{
return accPathLength.back();
}
+
/**
* @brief The path's length up to the i-th node.
*/
std::vector<float> accPathLength;
- static std::string getEdgeLayerName(std::size_t i)
+ static std::string
+ getEdgeLayerName(std::size_t i)
{
return "PathEdges::" + to_string(i);
}
@@ -170,7 +175,8 @@ namespace armarx
/**
* @brief Callback called when the config dialog is closed.
*/
- void configured() override
+ void
+ configured() override
{
motionPlanningServerProxyName = dialog->getProxyName("MotionPlanningServer");
}
@@ -180,7 +186,8 @@ namespace armarx
*/
void onConnectComponent() override;
- void onDisconnectComponent() override
+ void
+ onDisconnectComponent() override
{
motionPlanningServerTaskList->enableAutoUpdate(false);
resetSimoxCSpace();
@@ -189,12 +196,12 @@ namespace armarx
/**
* @brief Performs cleanup on exit.
*/
- void onExitComponent() override
+ void
+ onExitComponent() override
{
resetSimoxCSpace();
soSeparatorCleanUpAndRemoveFromRoot(visuAgent);
soSeparatorCleanUp(visuRoot);
-
}
/**
@@ -216,7 +223,8 @@ namespace armarx
/**
* @return The current user selected solution path. (exception if there is none)
*/
- PathData& getCurrentPath()
+ PathData&
+ getCurrentPath()
{
return paths.at(widget.spinBoxPathNumber->value() - 1);
}
@@ -241,7 +249,9 @@ namespace armarx
//draw
void drawEdges(bool drawTrace);
- void drawEdges()
+
+ void
+ drawEdges()
{
drawEdges(widget.checkBoxDrawTrace->isChecked());
}
@@ -251,7 +261,8 @@ namespace armarx
* @brief Sets the visibility of the given stationary object.
* @param index The object's index.
*/
- void setVisibilityObjectStationary(int index, Qt::CheckState state)
+ void
+ setVisibilityObjectStationary(int index, Qt::CheckState state)
{
soSeparatorRootChildVis(visuObjStat.at(index), state == Qt::Checked);
}
@@ -260,7 +271,8 @@ namespace armarx
* @brief Sets the visibility of the given moveable object.
* @param index The object's index.
*/
- void setVisibilityAgent(int state)
+ void
+ setVisibilityAgent(int state)
{
soSeparatorRootChildVis(visuAgent, state == Qt::Checked);
}
@@ -340,7 +352,8 @@ namespace armarx
void resetPaths();
- void resetCurrentPath()
+ void
+ resetCurrentPath()
{
widget.sliderPathPosition->setScale(0, 0);
}
@@ -405,9 +418,6 @@ namespace armarx
*/
std::vector<std::pair<VirtualRobot::RobotNodePtr, bool>> robotNodes;
QTimer updateCollisionStateTimer;
- const long timerPeriod
- {
- 30
- };
+ const long timerPeriod{30};
};
-}
+} // namespace armarx
diff --git a/source/RobotComponents/gui-plugins/RobotIKPlugin/ManipulatorVisualization.cpp b/source/RobotComponents/gui-plugins/RobotIKPlugin/ManipulatorVisualization.cpp
index 50c3b5ea98fc35dd2bb86582e97813a9daa93f5c..9c7618bba55e101418decde51934f19d4c1db4bc 100644
--- a/source/RobotComponents/gui-plugins/RobotIKPlugin/ManipulatorVisualization.cpp
+++ b/source/RobotComponents/gui-plugins/RobotIKPlugin/ManipulatorVisualization.cpp
@@ -24,20 +24,24 @@
#include "ManipulatorVisualization.h"
//Virtual Robot includes
-#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
-#include <VirtualRobot/XML/RobotIO.h>
+#include <VirtualRobot/RobotNodeSet.h>
#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>
+#include <VirtualRobot/XML/RobotIO.h>
+
+#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
/* Coin includes */
+#include <iostream>
+
+#include <Inventor/SbViewportRegion.h>
#include <Inventor/actions/SoGetMatrixAction.h>
#include <Inventor/actions/SoSearchAction.h>
-#include <Inventor/SbViewportRegion.h>
#include <Inventor/nodes/SoCube.h>
-
-#include <iostream>
-
-ManipulatorVisualization::ManipulatorVisualization() : isVisualizing(false), hasEndEffectorVisualizer(false), localTransformation(Eigen::Matrix4f::Identity())
+ManipulatorVisualization::ManipulatorVisualization() :
+ isVisualizing(false),
+ hasEndEffectorVisualizer(false),
+ localTransformation(Eigen::Matrix4f::Identity())
{
//Im gonna live forever :)
this->ref();
@@ -49,7 +53,9 @@ ManipulatorVisualization::~ManipulatorVisualization()
this->unref();
}
-void ManipulatorVisualization::setVisualization(VirtualRobot::RobotPtr robot, VirtualRobot::RobotNodeSetPtr nodeSet)
+void
+ManipulatorVisualization::setVisualization(VirtualRobot::RobotPtr robot,
+ VirtualRobot::RobotNodeSetPtr nodeSet)
{
//Completely forget anything we displayed earlier
//This also works when this is the first time we display something
@@ -82,7 +88,8 @@ void ManipulatorVisualization::setVisualization(VirtualRobot::RobotPtr robot, Vi
if (this->hasEndEffectorVisualizer)
{
VirtualRobot::RobotPtr endEffectorRobot = endEffector->createEefRobot("", "");
- VirtualRobot::CoinVisualizationPtr endEffectorVisualization = endEffectorRobot->getVisualization<VirtualRobot::CoinVisualization>();
+ VirtualRobot::CoinVisualizationPtr endEffectorVisualization =
+ endEffectorRobot->getVisualization();
this->addChild(endEffectorVisualization->getCoinVisualization());
}
else
@@ -137,7 +144,8 @@ void ManipulatorVisualization::setVisualization(VirtualRobot::RobotPtr robot, Vi
this->isVisualizing = true;
}
-void ManipulatorVisualization::removeVisualization()
+void
+ManipulatorVisualization::removeVisualization()
{
//Remove all children and reset manip pointer
//This should bring ref counter of Inventor down to zero and free memory
@@ -146,10 +154,11 @@ void ManipulatorVisualization::removeVisualization()
this->isVisualizing = false;
this->hasEndEffectorVisualizer = false;
- this->localTransformation = Eigen::Matrix4f::Identity();
+ this->localTransformation = Eigen::Matrix4f::Identity();
}
-void ManipulatorVisualization::setColor(float r, float g, float b)
+void
+ManipulatorVisualization::setColor(float r, float g, float b)
{
if (this->getIsVisualizing())
{
@@ -157,7 +166,8 @@ void ManipulatorVisualization::setColor(float r, float g, float b)
}
}
-void ManipulatorVisualization::addManipFinishCallback(SoDraggerCB* func, void* data)
+void
+ManipulatorVisualization::addManipFinishCallback(SoDraggerCB* func, void* data)
{
if (this->getIsVisualizing())
{
@@ -165,7 +175,8 @@ void ManipulatorVisualization::addManipFinishCallback(SoDraggerCB* func, void* d
}
}
-void ManipulatorVisualization::addManipMovedCallback(SoDraggerCB* func, void* data)
+void
+ManipulatorVisualization::addManipMovedCallback(SoDraggerCB* func, void* data)
{
if (this->getIsVisualizing())
{
@@ -173,15 +184,16 @@ void ManipulatorVisualization::addManipMovedCallback(SoDraggerCB* func, void* da
}
}
-Eigen::Matrix4f ManipulatorVisualization::getUserDesiredPose()
+Eigen::Matrix4f
+ManipulatorVisualization::getUserDesiredPose()
{
if (this->getIsVisualizing())
{
SoGetMatrixAction* action = new SoGetMatrixAction(SbViewportRegion());
SoSearchAction sa;
sa.setNode(manip.get());
- sa.setSearchingAll(TRUE); // Search all nodes
- SoBaseKit::setSearchingChildren(TRUE); // Even inside nodekits
+ sa.setSearchingAll(TRUE); // Search all nodes
+ SoBaseKit::setSearchingChildren(TRUE); // Even inside nodekits
sa.apply(this);
action->apply(sa.getPath());
@@ -223,7 +235,8 @@ Eigen::Matrix4f ManipulatorVisualization::getUserDesiredPose()
return Eigen::Matrix4f::Identity();
}
-std::string ManipulatorVisualization::getUserDesiredPoseString()
+std::string
+ManipulatorVisualization::getUserDesiredPoseString()
{
Eigen::Matrix4f mat = this->getUserDesiredPose();
@@ -233,7 +246,8 @@ std::string ManipulatorVisualization::getUserDesiredPoseString()
return buffer.str();
}
-void ManipulatorVisualization::setUserDesiredPose(Eigen::Matrix4f globalPose)
+void
+ManipulatorVisualization::setUserDesiredPose(Eigen::Matrix4f globalPose)
{
if (this->hasEndEffectorVisualizer)
{
@@ -244,6 +258,3 @@ void ManipulatorVisualization::setUserDesiredPose(Eigen::Matrix4f globalPose)
globalPose(2, 3) /= 1000;
manip->setMatrix(VirtualRobot::CoinVisualizationFactory::getSbMatrix(globalPose));
}
-
-
-
diff --git a/source/RobotComponents/gui-plugins/RobotIKPlugin/ManipulatorVisualization.h b/source/RobotComponents/gui-plugins/RobotIKPlugin/ManipulatorVisualization.h
index 9f73be9fe63ccf383826426eda756c1c19c0c77f..42bce32072759b6582a1cdea05cd04df408eb23b 100644
--- a/source/RobotComponents/gui-plugins/RobotIKPlugin/ManipulatorVisualization.h
+++ b/source/RobotComponents/gui-plugins/RobotIKPlugin/ManipulatorVisualization.h
@@ -24,9 +24,9 @@
#pragma once
//Coin includes
-#include <Inventor/nodes/SoSeparator.h>
#include <Inventor/manips/SoTransformerManip.h>
#include <Inventor/nodes/SoMaterial.h>
+#include <Inventor/nodes/SoSeparator.h>
//VirtualRobot
#include <VirtualRobot/Robot.h>
@@ -39,11 +39,15 @@
//Let boost use referencing of Inventor to manage objects memory
//and not its own shared pointer referencing
using SoTransformerManipPtr = boost::intrusive_ptr<SoTransformerManip>;
-inline void intrusive_ptr_add_ref(SoTransformerManip* obj)
+
+inline void
+intrusive_ptr_add_ref(SoTransformerManip* obj)
{
obj->ref();
}
-inline void intrusive_ptr_release(SoTransformerManip* obj)
+
+inline void
+intrusive_ptr_release(SoTransformerManip* obj)
{
obj->unref();
}
@@ -66,7 +70,8 @@ public:
std::string getUserDesiredPoseString();
void setUserDesiredPose(Eigen::Matrix4f globalPose);
- bool getIsVisualizing() const
+ bool
+ getIsVisualizing() const
{
return isVisualizing;
}
@@ -78,4 +83,3 @@ private:
bool hasEndEffectorVisualizer;
Eigen::Matrix4f localTransformation;
};
-
diff --git a/source/RobotComponents/gui-plugins/RobotIKPlugin/RobotIKGuiPlugin.cpp b/source/RobotComponents/gui-plugins/RobotIKPlugin/RobotIKGuiPlugin.cpp
index 1e27a7dfecea55ff9b71c6c16642c64a40eb5337..722167aa02538d48384031bf8171e1b5be9f57cd 100644
--- a/source/RobotComponents/gui-plugins/RobotIKPlugin/RobotIKGuiPlugin.cpp
+++ b/source/RobotComponents/gui-plugins/RobotIKPlugin/RobotIKGuiPlugin.cpp
@@ -22,26 +22,31 @@
*/
#include "RobotIKGuiPlugin.h"
+
#include "RobotViewer.h"
/* ArmarX includes */
-#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/ArmarXObjectScheduler.h>
-#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
#include <ArmarXCore/core/application/Application.h>
+#include <ArmarXCore/core/system/ArmarXDataPath.h>
+#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
+
#include <RobotAPI/libraries/core/CartesianPositionController.h>
#include <RobotAPI/libraries/core/CartesianVelocityController.h>
/* Virtual Robot includes */
-#include <VirtualRobot/XML/RobotIO.h>
+#include <VirtualRobot/RobotNodeSet.h>
#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>
+#include <VirtualRobot/XML/RobotIO.h>
/* Coin includes */
+#include <QClipboard>
+
+#include <ArmarXCore/util/json/JSONObject.h>
+
+#include <Inventor/SbViewportRegion.h>
#include <Inventor/actions/SoGetMatrixAction.h>
#include <Inventor/actions/SoSearchAction.h>
-#include <Inventor/SbViewportRegion.h>
-#include <ArmarXCore/util/json/JSONObject.h>
-#include <QClipboard>
// Gui Includes
#include "SetDesiredPoseDialog.h"
@@ -56,7 +61,8 @@ armarx::RobotIKGuiPlugin::RobotIKGuiPlugin()
addWidget<RobotIKWidgetController>();
}
-armarx::RobotIKWidgetController::RobotIKWidgetController() : manipulatorMoved(false), startUpCameraPositioningFlag(true)
+armarx::RobotIKWidgetController::RobotIKWidgetController() :
+ manipulatorMoved(false), startUpCameraPositioningFlag(true)
{
@@ -70,7 +76,8 @@ armarx::RobotIKWidgetController::RobotIKWidgetController() : manipulatorMoved(fa
textFieldUpdateSensor = NULL;
}
-void armarx::RobotIKWidgetController::onInitComponent()
+void
+armarx::RobotIKWidgetController::onInitComponent()
{
ARMARX_INFO << "RobotIKWidgetController on init";
QMetaObject::invokeMethod(this, "initWidget");
@@ -80,7 +87,8 @@ void armarx::RobotIKWidgetController::onInitComponent()
usingProxy(kinematicUnitComponentName);
}
-void armarx::RobotIKWidgetController::onConnectComponent()
+void
+armarx::RobotIKWidgetController::onConnectComponent()
{
//Get all proxies
robotStateComponentPrx = getProxy<RobotStateComponentInterfacePrx>(robotStateComponentName);
@@ -109,12 +117,13 @@ void armarx::RobotIKWidgetController::onConnectComponent()
this->ui.robotViewer->getRobotViewer()->getRootNode()->addChild((SoNode*)visualization);
//Get visualization for our robot and add it to scene graph
- currentRobotVisualization = robot->getVisualization<VirtualRobot::CoinVisualization>();
- this->ui.robotViewer->getRobotViewer()->getRootNode()->addChild(currentRobotVisualization->getCoinVisualization());
+ currentRobotVisualization = robot->getVisualization();
+ this->ui.robotViewer->getRobotViewer()->getRootNode()->addChild(
+ currentRobotVisualization->getCoinVisualization());
// Get visualization for preview robot
previewRobot = loadRobot(getIncludePaths());
- previewRobotVisualization = previewRobot->getVisualization<VirtualRobot::CoinVisualization>();
+ previewRobotVisualization = previewRobot->getVisualization();
// Setup cartesianControlRobot
cartesianControlRobot = previewRobot->clone();
@@ -157,7 +166,8 @@ void armarx::RobotIKWidgetController::onConnectComponent()
enableMainWidgetAsync(true);
}
-void armarx::RobotIKWidgetController::onDisconnect()
+void
+armarx::RobotIKWidgetController::onDisconnect()
{
//Stop timers
SoSensorManager* sensor_mgr = SoDB::getSensorManager();
@@ -195,50 +205,60 @@ void armarx::RobotIKWidgetController::onDisconnect()
textFieldUpdateSensor = NULL;
}
-void armarx::RobotIKWidgetController::onDisconnectComponent()
+void
+armarx::RobotIKWidgetController::onDisconnectComponent()
{
QMetaObject::invokeMethod(this, "onDisconnect");
}
-void armarx::RobotIKWidgetController::onExitComponent()
+void
+armarx::RobotIKWidgetController::onExitComponent()
{
// enableMainWidgetAsync(false);
}
-QPointer<QDialog> armarx::RobotIKWidgetController::getConfigDialog(QWidget* parent)
+QPointer<QDialog>
+armarx::RobotIKWidgetController::getConfigDialog(QWidget* parent)
{
if (!dialog)
{
dialog = new SimpleConfigDialog(parent);
- dialog->addProxyFinder<RobotStateComponentInterfacePrx>({"RobotStateComponent", "", "RobotState*"});
- dialog->addProxyFinder<KinematicUnitInterfacePrx>({"KinematicUnit", "", "*KinematicUnit|KinematicUnit*"});
+ dialog->addProxyFinder<RobotStateComponentInterfacePrx>(
+ {"RobotStateComponent", "", "RobotState*"});
+ dialog->addProxyFinder<KinematicUnitInterfacePrx>(
+ {"KinematicUnit", "", "*KinematicUnit|KinematicUnit*"});
dialog->addProxyFinder<RobotIKInterfacePrx>({"RobotIK", "", "RobotIK*"});
}
return qobject_cast<SimpleConfigDialog*>(dialog);
}
-void armarx::RobotIKWidgetController::loadSettings(QSettings* settings)
+void
+armarx::RobotIKWidgetController::loadSettings(QSettings* settings)
{
robotStateComponentName = settings->value("robotStateComponentName").toString().toStdString();
robotIKComponentName = settings->value("robotIKComponentName").toString().toStdString();
- kinematicUnitComponentName = settings->value("kinematicUnitComponentName").toString().toStdString();
+ kinematicUnitComponentName =
+ settings->value("kinematicUnitComponentName").toString().toStdString();
}
-void armarx::RobotIKWidgetController::saveSettings(QSettings* settings)
+void
+armarx::RobotIKWidgetController::saveSettings(QSettings* settings)
{
settings->setValue("robotStateComponentName", robotStateComponentName.c_str());
settings->setValue("robotIKComponentName", robotIKComponentName.c_str());
settings->setValue("kinematicUnitComponentName", kinematicUnitComponentName.c_str());
}
-void armarx::RobotIKWidgetController::configured()
+void
+armarx::RobotIKWidgetController::configured()
{
robotStateComponentName = dialog->getProxyName("RobotStateComponent");
robotIKComponentName = dialog->getProxyName("RobotIK");
kinematicUnitComponentName = dialog->getProxyName("KinematicUnit");
}
-void armarx::RobotIKWidgetController::initWidget()
+void
+armarx::RobotIKWidgetController::initWidget()
{
ARMARX_INFO << "RobotIKWidgetController initWidget";
//Initialize Gui
@@ -251,14 +271,18 @@ void armarx::RobotIKWidgetController::initWidget()
this->ui.reachableLabel->setStyleSheet("QLabel { color : red; }");
}
-void armarx::RobotIKWidgetController::solveIK()
+void
+armarx::RobotIKWidgetController::solveIK()
{
- robotIKPrx->begin_computeExtendedIKGlobalPose(this->ui.comboBox->currentText().toStdString(), new armarx::Pose(visualization->getUserDesiredPose()),
- convertOption(this->ui.cartesianselection->currentText().toStdString()),
- Ice::newCallback(this, &RobotIKWidgetController::ikCallbackExecuteMotion));
+ robotIKPrx->begin_computeExtendedIKGlobalPose(
+ this->ui.comboBox->currentText().toStdString(),
+ new armarx::Pose(visualization->getUserDesiredPose()),
+ convertOption(this->ui.cartesianselection->currentText().toStdString()),
+ Ice::newCallback(this, &RobotIKWidgetController::ikCallbackExecuteMotion));
}
-void armarx::RobotIKWidgetController::ikCallbackExecuteMotion(const Ice::AsyncResultPtr& r)
+void
+armarx::RobotIKWidgetController::ikCallbackExecuteMotion(const Ice::AsyncResultPtr& r)
{
RobotIKInterfacePrx prx = RobotIKInterfacePrx::uncheckedCast(r->getProxy());
ExtendedIKResult solution = prx->end_computeExtendedIKGlobalPose(r);
@@ -269,7 +293,9 @@ void armarx::RobotIKWidgetController::ikCallbackExecuteMotion(const Ice::AsyncRe
QMetaObject::invokeMethod(this, "updateSolutionDisplay");
QMetaObject::invokeMethod(this, "executeMotion");
}
-void armarx::RobotIKWidgetController::ikCallbackDisplaySolution(const Ice::AsyncResultPtr& r)
+
+void
+armarx::RobotIKWidgetController::ikCallbackDisplaySolution(const Ice::AsyncResultPtr& r)
{
RobotIKInterfacePrx prx = RobotIKInterfacePrx::uncheckedCast(r->getProxy());
@@ -285,9 +311,8 @@ void armarx::RobotIKWidgetController::ikCallbackDisplaySolution(const Ice::Async
QMetaObject::invokeMethod(this, "updateSolutionDisplay");
}
-
-
-void armarx::RobotIKWidgetController::kinematicChainChanged(const QString& arg1)
+void
+armarx::RobotIKWidgetController::kinematicChainChanged(const QString& arg1)
{
//An item has been selected, so we can allow the user to use the ui now
//The manipulator will be set to the position of the current tcp, so this pose
@@ -320,7 +345,8 @@ void armarx::RobotIKWidgetController::kinematicChainChanged(const QString& arg1)
armarx::RemoteRobot::synchronizeLocalClone(previewRobot, robotStateComponentPrx);
}
-void armarx::RobotIKWidgetController::caertesianSelectionChanged(const QString& arg1)
+void
+armarx::RobotIKWidgetController::caertesianSelectionChanged(const QString& arg1)
{
//If there is a manip in the scene we pretend it just moved to update color etc.
if (visualization->getIsVisualizing())
@@ -329,13 +355,15 @@ void armarx::RobotIKWidgetController::caertesianSelectionChanged(const QString&
}
}
-void armarx::RobotIKWidgetController::resetManip()
+void
+armarx::RobotIKWidgetController::resetManip()
{
//Triggers reset of manipulator in kinematicChainChanged
kinematicChainChanged(this->ui.comboBox->currentText());
}
-void armarx::RobotIKWidgetController::updateSolutionDisplay()
+void
+armarx::RobotIKWidgetController::updateSolutionDisplay()
{
if (getState() != eManagedIceObjectStarted)
{
@@ -361,12 +389,13 @@ void armarx::RobotIKWidgetController::updateSolutionDisplay()
}
//Display calculated error
- ui.errorValue->setText("Calculated error: " + QString::number(currentSolution.errorPosition, 'f', 1)
- + " mm " + QString::number(currentSolution.errorOrientation * 180 / M_PI, 'f', 2) + " deg");
-
+ ui.errorValue->setText(
+ "Calculated error: " + QString::number(currentSolution.errorPosition, 'f', 1) + " mm " +
+ QString::number(currentSolution.errorOrientation * 180 / M_PI, 'f', 2) + " deg");
}
-void armarx::RobotIKWidgetController::showCurrentRobotChanged(bool checked)
+void
+armarx::RobotIKWidgetController::showCurrentRobotChanged(bool checked)
{
auto visualizationRoot = this->ui.robotViewer->getRobotViewer()->getRootNode();
if (checked)
@@ -385,7 +414,8 @@ void armarx::RobotIKWidgetController::showCurrentRobotChanged(bool checked)
}
}
-void armarx::RobotIKWidgetController::showPreviewRobotChanged(bool checked)
+void
+armarx::RobotIKWidgetController::showPreviewRobotChanged(bool checked)
{
auto visualizationRoot = this->ui.robotViewer->getRobotViewer()->getRootNode();
if (checked)
@@ -404,20 +434,50 @@ void armarx::RobotIKWidgetController::showPreviewRobotChanged(bool checked)
}
}
-void armarx::RobotIKWidgetController::connectSlots()
+void
+armarx::RobotIKWidgetController::connectSlots()
{
connect(ui.resetManip, SIGNAL(clicked()), this, SLOT(resetManip()), Qt::UniqueConnection);
connect(ui.pushButton, SIGNAL(clicked()), this, SLOT(executeMotion()), Qt::UniqueConnection);
- connect(ui.comboBox, SIGNAL(currentIndexChanged(QString)), this, SLOT(kinematicChainChanged(QString)), Qt::UniqueConnection);
- connect(ui.cartesianselection, SIGNAL(currentIndexChanged(QString)), this, SLOT(caertesianSelectionChanged(QString)), Qt::UniqueConnection);
- connect(ui.checkBox, SIGNAL(toggled(bool)), this, SLOT(autoFollowChanged(bool)), Qt::UniqueConnection);
- connect(ui.btnCopyCurrentPoseToClipboard, SIGNAL(clicked()), this, SLOT(on_btnCopyCurrentPoseToClipboard_clicked()), Qt::UniqueConnection);
- connect(ui.checkBox_showCurrentRobot, SIGNAL(toggled(bool)), this, SLOT(showCurrentRobotChanged(bool)), Qt::UniqueConnection);
- connect(ui.checkBox_showPreviewRobot, SIGNAL(toggled(bool)), this, SLOT(showPreviewRobotChanged(bool)), Qt::UniqueConnection);
- connect(ui.pushButton_setDesiredPose, SIGNAL(clicked()), this, SLOT(on_btnSetDesiredPose_clicked()), Qt::UniqueConnection);
-}
-
-Ice::StringSeq armarx::RobotIKWidgetController::getIncludePaths() const
+ connect(ui.comboBox,
+ SIGNAL(currentIndexChanged(QString)),
+ this,
+ SLOT(kinematicChainChanged(QString)),
+ Qt::UniqueConnection);
+ connect(ui.cartesianselection,
+ SIGNAL(currentIndexChanged(QString)),
+ this,
+ SLOT(caertesianSelectionChanged(QString)),
+ Qt::UniqueConnection);
+ connect(ui.checkBox,
+ SIGNAL(toggled(bool)),
+ this,
+ SLOT(autoFollowChanged(bool)),
+ Qt::UniqueConnection);
+ connect(ui.btnCopyCurrentPoseToClipboard,
+ SIGNAL(clicked()),
+ this,
+ SLOT(on_btnCopyCurrentPoseToClipboard_clicked()),
+ Qt::UniqueConnection);
+ connect(ui.checkBox_showCurrentRobot,
+ SIGNAL(toggled(bool)),
+ this,
+ SLOT(showCurrentRobotChanged(bool)),
+ Qt::UniqueConnection);
+ connect(ui.checkBox_showPreviewRobot,
+ SIGNAL(toggled(bool)),
+ this,
+ SLOT(showPreviewRobotChanged(bool)),
+ Qt::UniqueConnection);
+ connect(ui.pushButton_setDesiredPose,
+ SIGNAL(clicked()),
+ this,
+ SLOT(on_btnSetDesiredPose_clicked()),
+ Qt::UniqueConnection);
+}
+
+Ice::StringSeq
+armarx::RobotIKWidgetController::getIncludePaths() const
{
Ice::StringSeq includePaths;
@@ -436,7 +496,8 @@ Ice::StringSeq armarx::RobotIKWidgetController::getIncludePaths() const
CMakePackageFinder project(projectName);
auto pathsString = project.getDataDir();
Ice::StringSeq projectIncludePaths = armarx::Split(pathsString, ",;", true, true);
- includePaths.insert(includePaths.end(), projectIncludePaths.begin(), projectIncludePaths.end());
+ includePaths.insert(
+ includePaths.end(), projectIncludePaths.begin(), projectIncludePaths.end());
}
}
catch (...)
@@ -447,7 +508,8 @@ Ice::StringSeq armarx::RobotIKWidgetController::getIncludePaths() const
return includePaths;
}
-VirtualRobot::RobotPtr armarx::RobotIKWidgetController::loadRobot(Ice::StringSeq includePaths)
+VirtualRobot::RobotPtr
+armarx::RobotIKWidgetController::loadRobot(Ice::StringSeq includePaths)
{
try
{
@@ -462,7 +524,8 @@ VirtualRobot::RobotPtr armarx::RobotIKWidgetController::loadRobot(Ice::StringSeq
}
}
-void armarx::RobotIKWidgetController::manipFinishCallback(void* data, SoDragger* dragger)
+void
+armarx::RobotIKWidgetController::manipFinishCallback(void* data, SoDragger* dragger)
{
RobotIKWidgetController* controller = static_cast<RobotIKWidgetController*>(data);
if (controller)
@@ -471,18 +534,21 @@ void armarx::RobotIKWidgetController::manipFinishCallback(void* data, SoDragger*
}
}
-void armarx::RobotIKWidgetController::manipMovedCallback(void* data, SoDragger* dragger)
+void
+armarx::RobotIKWidgetController::manipMovedCallback(void* data, SoDragger* dragger)
{
RobotIKWidgetController* controller = static_cast<RobotIKWidgetController*>(data);
if (controller)
{
controller->manipulatorMoved = true;
- QMetaObject::invokeMethod(controller, "showPreviewRobotChanged", Qt::QueuedConnection, Q_ARG(bool, false));
+ QMetaObject::invokeMethod(
+ controller, "showPreviewRobotChanged", Qt::QueuedConnection, Q_ARG(bool, false));
}
}
-void armarx::RobotIKWidgetController::robotUpdateTimerCB(void* data, SoSensor* sensor)
+void
+armarx::RobotIKWidgetController::robotUpdateTimerCB(void* data, SoSensor* sensor)
{
RobotIKWidgetController* controller = static_cast<RobotIKWidgetController*>(data);
@@ -493,7 +559,8 @@ void armarx::RobotIKWidgetController::robotUpdateTimerCB(void* data, SoSensor* s
try
{
- armarx::RemoteRobot::synchronizeLocalClone(controller->robot, controller->robotStateComponentPrx);
+ armarx::RemoteRobot::synchronizeLocalClone(controller->robot,
+ controller->robotStateComponentPrx);
if (controller->startUpCameraPositioningFlag)
{
@@ -501,10 +568,13 @@ void armarx::RobotIKWidgetController::robotUpdateTimerCB(void* data, SoSensor* s
controller->startUpCameraPositioningFlag = false;
}
}
- catch (...) {};
+ catch (...)
+ {
+ };
}
-void armarx::RobotIKWidgetController::textFieldUpdateTimerCB(void* data, SoSensor* sensor)
+void
+armarx::RobotIKWidgetController::textFieldUpdateTimerCB(void* data, SoSensor* sensor)
{
RobotIKWidgetController* controller = static_cast<RobotIKWidgetController*>(data);
@@ -515,22 +585,26 @@ void armarx::RobotIKWidgetController::textFieldUpdateTimerCB(void* data, SoSenso
if (controller->visualization->getIsVisualizing())
{
- Eigen::Matrix4f tcpMatrix = controller->robot->getRobotNodeSet(controller->ui.comboBox->currentText().toStdString())->getTCP()->getPoseInRootFrame();
- FramedPose actualPose(tcpMatrix,
- controller->robot->getRootNode()->getName(),
- controller->robot->getName());
+ Eigen::Matrix4f tcpMatrix =
+ controller->robot->getRobotNodeSet(controller->ui.comboBox->currentText().toStdString())
+ ->getTCP()
+ ->getPoseInRootFrame();
+ FramedPose actualPose(
+ tcpMatrix, controller->robot->getRootNode()->getName(), controller->robot->getName());
//Set text label to tcp matrix
controller->ui.currentPoseMatrix->setText(QString::fromStdString(actualPose.output()));
//Set text label for desired tcp pose
- FramedPose desired(controller->robot->getRootNode()->toLocalCoordinateSystem(controller->visualization->getUserDesiredPose()),
+ FramedPose desired(controller->robot->getRootNode()->toLocalCoordinateSystem(
+ controller->visualization->getUserDesiredPose()),
controller->robot->getRootNode()->getName(),
controller->robot->getName());
controller->ui.desiredPoseMatrix->setText(QString::fromStdString(desired.output()));
}
}
-void armarx::RobotIKWidgetController::autoFollowSensorTimerCB(void* data, SoSensor* sensor)
+void
+armarx::RobotIKWidgetController::autoFollowSensorTimerCB(void* data, SoSensor* sensor)
{
RobotIKWidgetController* controller = static_cast<RobotIKWidgetController*>(data);
@@ -541,20 +615,24 @@ void armarx::RobotIKWidgetController::autoFollowSensorTimerCB(void* data, SoSens
}
}
-void armarx::RobotIKWidgetController::startIKSolving(RobotIKWidgetController* controller)
+void
+armarx::RobotIKWidgetController::startIKSolving(RobotIKWidgetController* controller)
{
if (controller)
{
controller->ui.reachableLabel->setText(QString::fromStdString("Calculating..."));
controller->ui.reachableLabel->setStyleSheet("QLabel { color : black; }");
- controller->robotIKPrx->begin_computeExtendedIKGlobalPose(controller->ui.comboBox->currentText().toStdString(),
- new armarx::Pose(controller->visualization->getUserDesiredPose()),
- controller->convertOption(controller->ui.cartesianselection->currentText().toStdString()),
- Ice::newCallback(controller, &RobotIKWidgetController::ikCallbackDisplaySolution));
+ controller->robotIKPrx->begin_computeExtendedIKGlobalPose(
+ controller->ui.comboBox->currentText().toStdString(),
+ new armarx::Pose(controller->visualization->getUserDesiredPose()),
+ controller->convertOption(
+ controller->ui.cartesianselection->currentText().toStdString()),
+ Ice::newCallback(controller, &RobotIKWidgetController::ikCallbackDisplaySolution));
}
}
-armarx::ExtendedIKResult armarx::RobotIKWidgetController::improveCurrentSolution(armarx::ExtendedIKResult& solution)
+armarx::ExtendedIKResult
+armarx::RobotIKWidgetController::improveCurrentSolution(armarx::ExtendedIKResult& solution)
{
if (!ui.checkBox_cartesianControl->isChecked())
{
@@ -567,7 +645,8 @@ armarx::ExtendedIKResult armarx::RobotIKWidgetController::improveCurrentSolution
}
VirtualRobot::IKSolver::CartesianSelection vrmode;
- armarx::CartesianSelection mode = convertOption(ui.cartesianselection->currentText().toStdString());
+ armarx::CartesianSelection mode =
+ convertOption(ui.cartesianselection->currentText().toStdString());
if (mode == CartesianSelection::eAll)
{
vrmode = VirtualRobot::IKSolver::CartesianSelection::All;
@@ -587,16 +666,19 @@ armarx::ExtendedIKResult armarx::RobotIKWidgetController::improveCurrentSolution
armarx::RemoteRobot::synchronizeLocalClone(cartesianControlRobot, robotStateComponentPrx);
cartesianControlRobot->setJointValues(solution.jointAngles);
- VirtualRobot::RobotNodeSetPtr kc = cartesianControlRobot->getRobotNodeSet(ui.comboBox->currentText().toStdString());
+ VirtualRobot::RobotNodeSetPtr kc =
+ cartesianControlRobot->getRobotNodeSet(ui.comboBox->currentText().toStdString());
VirtualRobot::RobotNodePtr tcp = kc->getTCP();
Eigen::Matrix4f globalTargetTCPPose = visualization->getUserDesiredPose();
- Eigen::Matrix4f localTarget = cartesianControlRobot->getRootNode()->toLocalCoordinateSystem(globalTargetTCPPose);
+ Eigen::Matrix4f localTarget =
+ cartesianControlRobot->getRootNode()->toLocalCoordinateSystem(globalTargetTCPPose);
CartesianPositionControllerPtr cartesianPositionController;
cartesianPositionController.reset(new CartesianPositionController(tcp));
CartesianVelocityControllerPtr cartesianVelocityController;
- cartesianVelocityController.reset(new CartesianVelocityController(kc, tcp, VirtualRobot::JacobiProvider::eSVD));
+ cartesianVelocityController.reset(
+ new CartesianVelocityController(kc, tcp, VirtualRobot::JacobiProvider::eSVD));
float errorOriInitial = cartesianPositionController->getOrientationError(localTarget);
@@ -608,13 +690,16 @@ armarx::ExtendedIKResult armarx::RobotIKWidgetController::improveCurrentSolution
for (int i = 0; i < CARTESIAN_CONTROLLER_LOOP_AMOUNT; i++)
{
Eigen::VectorXf tcpVelocities = cartesianPositionController->calculate(localTarget, vrmode);
- Eigen::VectorXf nullspaceVel = cartesianVelocityController->calculateJointLimitAvoidance(); // calculateNullspaceVelocity(tcpVelocities, 1.0f, vrmode);
+ Eigen::VectorXf nullspaceVel =
+ cartesianVelocityController
+ ->calculateJointLimitAvoidance(); // calculateNullspaceVelocity(tcpVelocities, 1.0f, vrmode);
float nullspaceLen = nullspaceVel.norm();
if (nullspaceLen > stepLength)
{
nullspaceVel = nullspaceVel / nullspaceLen * stepLength;
}
- Eigen::VectorXf jointVelocities = cartesianVelocityController->calculate(tcpVelocities, nullspaceVel, vrmode);
+ Eigen::VectorXf jointVelocities =
+ cartesianVelocityController->calculate(tcpVelocities, nullspaceVel, vrmode);
//jointVelocities = jointVelocities * stepLength;
float len = jointVelocities.norm();
@@ -645,12 +730,14 @@ armarx::ExtendedIKResult armarx::RobotIKWidgetController::improveCurrentSolution
return solution;
}
-armarx::ExtendedIKResult armarx::RobotIKWidgetController::getIKSolution() const
+armarx::ExtendedIKResult
+armarx::RobotIKWidgetController::getIKSolution() const
{
return ExtendedIKResult();
}
-armarx::CartesianSelection armarx::RobotIKWidgetController::convertOption(std::string option) const
+armarx::CartesianSelection
+armarx::RobotIKWidgetController::convertOption(std::string option) const
{
if (option == "Orientation and Position")
{
@@ -680,14 +767,16 @@ armarx::CartesianSelection armarx::RobotIKWidgetController::convertOption(std::s
return eAll;
}
-void armarx::RobotIKWidgetController::updatePreviewRobot()
+void
+armarx::RobotIKWidgetController::updatePreviewRobot()
{
armarx::RemoteRobot::synchronizeLocalClone(previewRobot, robotStateComponentPrx);
previewRobot->setJointValues(currentSolution.jointAngles);
showPreviewRobotChanged(ui.checkBox_showPreviewRobot->isChecked());
}
-void armarx::RobotIKWidgetController::on_btnSetDesiredPose_clicked()
+void
+armarx::RobotIKWidgetController::on_btnSetDesiredPose_clicked()
{
SetDesiredPoseDialog d;
@@ -701,7 +790,6 @@ void armarx::RobotIKWidgetController::on_btnSetDesiredPose_clicked()
else if (newDesiredPose->agent.empty())
{
ARMARX_WARNING << "Agent of a FramedPose cannot be empty!";
-
}
else
{
@@ -718,12 +806,15 @@ void armarx::RobotIKWidgetController::on_btnSetDesiredPose_clicked()
}
}
-void armarx::RobotIKWidgetController::executeMotion()
+void
+armarx::RobotIKWidgetController::executeMotion()
{
if (currentSolution.isReachable)
{
//Switch all control modes to ePositionControl
- std::vector<VirtualRobot::RobotNodePtr> rn = robot->getRobotNodeSet(this->ui.comboBox->currentText().toStdString())->getAllRobotNodes();
+ std::vector<VirtualRobot::RobotNodePtr> rn =
+ robot->getRobotNodeSet(this->ui.comboBox->currentText().toStdString())
+ ->getAllRobotNodes();
NameControlModeMap jointModes;
NameValueMap jointAngles;
@@ -738,7 +829,8 @@ void armarx::RobotIKWidgetController::executeMotion()
}
}
-void armarx::RobotIKWidgetController::autoFollowChanged(bool checked)
+void
+armarx::RobotIKWidgetController::autoFollowChanged(bool checked)
{
if (checked)
{
@@ -755,14 +847,18 @@ void armarx::RobotIKWidgetController::autoFollowChanged(bool checked)
}
}
-void armarx::RobotIKWidgetController::on_btnCopyCurrentPoseToClipboard_clicked()
+void
+armarx::RobotIKWidgetController::on_btnCopyCurrentPoseToClipboard_clicked()
{
if (visualization->getIsVisualizing())
{
- Eigen::Matrix4f tcpMatrix = robot->getRobotNodeSet(ui.comboBox->currentText().toStdString())->getTCP()->getPoseInRootFrame();
- FramedPosePtr pose = new FramedPose(tcpMatrix, robot->getRootNode()->getName(), robot->getName());
+ Eigen::Matrix4f tcpMatrix = robot->getRobotNodeSet(ui.comboBox->currentText().toStdString())
+ ->getTCP()
+ ->getPoseInRootFrame();
+ FramedPosePtr pose =
+ new FramedPose(tcpMatrix, robot->getRootNode()->getName(), robot->getName());
JSONObjectPtr obj = new JSONObject();
obj->serializeIceObject(pose);
@@ -771,5 +867,3 @@ void armarx::RobotIKWidgetController::on_btnCopyCurrentPoseToClipboard_clicked()
clipboard->setText(QString::fromStdString(obj->asString(true)));
}
}
-
-
diff --git a/source/RobotComponents/gui-plugins/RobotIKPlugin/RobotIKGuiPlugin.h b/source/RobotComponents/gui-plugins/RobotIKPlugin/RobotIKGuiPlugin.h
index b6c117be91e43bb1e164c010bc8a9aef33e094a6..06968e7b043f48a869fa843c8871f83986ac09de 100644
--- a/source/RobotComponents/gui-plugins/RobotIKPlugin/RobotIKGuiPlugin.h
+++ b/source/RobotComponents/gui-plugins/RobotIKPlugin/RobotIKGuiPlugin.h
@@ -31,12 +31,15 @@
//ArmarX includes
#include <ArmarXCore/core/system/Synchronization.h>
-#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
+
#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
-#include <RobotComponents/interface/components/RobotIK.h>
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
+#include <ArmarXGui/libraries/SimpleConfigDialog/SimpleConfigDialog.h>
+
#include <RobotAPI/interface/core/RobotState.h>
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
-#include <ArmarXGui/libraries/SimpleConfigDialog/SimpleConfigDialog.h>
+
+#include <RobotComponents/interface/components/RobotIK.h>
//Qt includes
#include <QObject>
@@ -46,11 +49,10 @@
#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualization.h>
//Inventor includes
-#include <Inventor/sensors/SoTimerSensor.h>
#include <Inventor/manips/SoTransformerManip.h>
-#include <Inventor/nodes/SoSphere.h>
#include <Inventor/nodes/SoMaterial.h>
-
+#include <Inventor/nodes/SoSphere.h>
+#include <Inventor/sensors/SoTimerSensor.h>
namespace armarx
{
@@ -63,7 +65,8 @@ namespace armarx
public:
RobotIKGuiPlugin();
- QString getPluginName() override
+ QString
+ getPluginName() override
{
return "RobotIKGuiPlugin";
}
@@ -93,13 +96,17 @@ namespace armarx
*/
// * \ingroup RobotAPI-ArmarXGuiPlugins ArmarXGuiPlugins
- class RobotIKWidgetController : public ArmarXComponentWidgetControllerTemplate<RobotIKWidgetController>
+ class RobotIKWidgetController :
+ public ArmarXComponentWidgetControllerTemplate<RobotIKWidgetController>
{
Q_OBJECT
public:
RobotIKWidgetController();
- virtual ~RobotIKWidgetController() {}
+
+ virtual ~RobotIKWidgetController()
+ {
+ }
// inherited from Component
void onInitComponent() override;
@@ -108,10 +115,12 @@ namespace armarx
void onExitComponent() override;
// inherited of ArmarXWidget
- static QString GetWidgetName()
+ static QString
+ GetWidgetName()
{
return "RobotControl.RobotIK";
}
+
QPointer<QDialog> getConfigDialog(QWidget* parent = 0) override;
void loadSettings(QSettings* settings) override;
void saveSettings(QSettings* settings) override;
@@ -187,7 +196,7 @@ namespace armarx
VirtualRobot::CoinVisualizationPtr previewRobotVisualization;
VirtualRobot::CoinVisualizationPtr currentRobotVisualization;
};
+
using RobotIKGuiPluginPtr = std::shared_ptr<RobotIKWidgetController>;
using CoinVisualizationPtr = boost::shared_ptr<VirtualRobot::CoinVisualization>;
-}
-
+} // namespace armarx
diff --git a/source/RobotComponents/gui-plugins/RobotIKPlugin/RobotViewer.cpp b/source/RobotComponents/gui-plugins/RobotIKPlugin/RobotViewer.cpp
index e4c9fc22a4b4dace732e385b4865bb179d882aa8..263cf8a02e8f79660bf801e3c65b9e636facf1df 100644
--- a/source/RobotComponents/gui-plugins/RobotIKPlugin/RobotViewer.cpp
+++ b/source/RobotComponents/gui-plugins/RobotIKPlugin/RobotViewer.cpp
@@ -23,17 +23,20 @@
*/
#include "RobotViewer.h"
-#include <Inventor/nodes/SoSeparator.h>
-#include <Inventor/nodes/SoPickStyle.h>
-#include <Inventor/nodes/SoMaterial.h>
+#include <Inventor/events/SoLocation2Event.h>
+#include <Inventor/events/SoMouseButtonEvent.h>
#include <Inventor/nodes/SoDrawStyle.h>
-#include <Inventor/nodes/SoVertexProperty.h>
#include <Inventor/nodes/SoLineSet.h>
+#include <Inventor/nodes/SoMaterial.h>
+#include <Inventor/nodes/SoPickStyle.h>
+#include <Inventor/nodes/SoSeparator.h>
+#include <Inventor/nodes/SoVertexProperty.h>
-#include <Inventor/events/SoMouseButtonEvent.h>
-#include <Inventor/events/SoLocation2Event.h>
-
-armarx::RobotViewer::RobotViewer(QWidget* widget) : SoQtExaminerViewer(widget), sceneRootNode(new SoSeparator), contentRootNode(new SoSeparator), camera(new SoPerspectiveCamera)
+armarx::RobotViewer::RobotViewer(QWidget* widget) :
+ SoQtExaminerViewer(widget),
+ sceneRootNode(new SoSeparator),
+ contentRootNode(new SoSeparator),
+ camera(new SoPerspectiveCamera)
{
this->setBackgroundColor(SbColor(150 / 255.0f, 150 / 255.0f, 150 / 255.0f));
this->setAccumulationBuffer(this->getAccumulationBuffer());
@@ -159,12 +162,16 @@ armarx::RobotViewer::RobotViewer(QWidget* widget) : SoQtExaminerViewer(widget),
for (int n = 1; n < GRIDSUBSIZE; n++)
{
- subVertices->vertex.set1Value(subverticescounter++, GRIDSIZE * GRIDUNIT, (float)i + (float)n * delta, 0);
- subVertices->vertex.set1Value(subverticescounter++, -(GRIDSIZE * GRIDUNIT), (float)i + (float)n * delta, 0);
+ subVertices->vertex.set1Value(
+ subverticescounter++, GRIDSIZE * GRIDUNIT, (float)i + (float)n * delta, 0);
+ subVertices->vertex.set1Value(
+ subverticescounter++, -(GRIDSIZE * GRIDUNIT), (float)i + (float)n * delta, 0);
subLines->numVertices.set1Value((subverticescounter - 1) / 2, 2);
- subVertices->vertex.set1Value(subverticescounter++, (float)i + (float)n * delta, GRIDSIZE * GRIDUNIT, 0);
- subVertices->vertex.set1Value(subverticescounter++, (float)i + (float)n * delta, -(GRIDSIZE * GRIDUNIT), 0);
+ subVertices->vertex.set1Value(
+ subverticescounter++, (float)i + (float)n * delta, GRIDSIZE * GRIDUNIT, 0);
+ subVertices->vertex.set1Value(
+ subverticescounter++, (float)i + (float)n * delta, -(GRIDSIZE * GRIDUNIT), 0);
subLines->numVertices.set1Value((subverticescounter - 1) / 2, 2);
}
}
@@ -200,25 +207,28 @@ armarx::RobotViewer::~RobotViewer()
sceneRootNode->unref();
}
-SoSeparator* armarx::RobotViewer::getRootNode()
+SoSeparator*
+armarx::RobotViewer::getRootNode()
{
return this->contentRootNode;
}
-void armarx::RobotViewer::cameraViewAll()
+void
+armarx::RobotViewer::cameraViewAll()
{
camera->viewAll(this->contentRootNode, SbViewportRegion());
}
//Override the default navigation behaviour of the SoQtExaminerViewer
-SbBool armarx::RobotViewer::processSoEvent(const SoEvent* const event)
+SbBool
+armarx::RobotViewer::processSoEvent(const SoEvent* const event)
{
const SoType type(event->getTypeId());
//Remapping mouse press events
if (type.isDerivedFrom(SoMouseButtonEvent::getClassTypeId()))
{
- SoMouseButtonEvent* const ev = (SoMouseButtonEvent*) event;
+ SoMouseButtonEvent* const ev = (SoMouseButtonEvent*)event;
const int button = ev->getButton();
const SbBool press = ev->getState() == SoButtonEvent::DOWN ? TRUE : FALSE;
@@ -271,8 +281,8 @@ SbBool armarx::RobotViewer::processSoEvent(const SoEvent* const event)
on to make SoQtExaminerViewer allow zooming.*/
//Swap BUTTON4 and BUTTON5 because zooming out while scrolling up is just retarded
- ev->setButton(button == SoMouseButtonEvent::BUTTON4 ?
- SoMouseButtonEvent::BUTTON5 : SoMouseButtonEvent::BUTTON4);
+ ev->setButton(button == SoMouseButtonEvent::BUTTON4 ? SoMouseButtonEvent::BUTTON5
+ : SoMouseButtonEvent::BUTTON4);
//Zooming is allowed, so just pass it and temporarily set viewing mode on, if it is not already
//(otherwise coin gives us warning messages...)
@@ -302,7 +312,7 @@ SbBool armarx::RobotViewer::processSoEvent(const SoEvent* const event)
// Keyboard handling
if (type.isDerivedFrom(SoKeyboardEvent::getClassTypeId()))
{
- const SoKeyboardEvent* const ev = (const SoKeyboardEvent*) event;
+ const SoKeyboardEvent* const ev = (const SoKeyboardEvent*)event;
/*The escape key and super key (windows key) is used to switch between
viewing modes. We need to disable this behaviour completely.*/
diff --git a/source/RobotComponents/gui-plugins/RobotIKPlugin/RobotViewer.h b/source/RobotComponents/gui-plugins/RobotIKPlugin/RobotViewer.h
index 50103dcd89bcc1cf727027809358d6bed1e0d1e0..358d851358d390aba8a58e5b3a85d8b1a6ef3d1d 100644
--- a/source/RobotComponents/gui-plugins/RobotIKPlugin/RobotViewer.h
+++ b/source/RobotComponents/gui-plugins/RobotIKPlugin/RobotViewer.h
@@ -24,8 +24,8 @@
#pragma once
/* Coin headers */
-#include <Inventor/nodes/SoPerspectiveCamera.h>
#include <Inventor/Qt/viewers/SoQtExaminerViewer.h>
+#include <Inventor/nodes/SoPerspectiveCamera.h>
namespace armarx
{
@@ -36,11 +36,11 @@ namespace armarx
~RobotViewer() override;
SoSeparator* getRootNode();
void cameraViewAll();
+
private:
SbBool processSoEvent(const SoEvent* const event) override;
SoSeparator* sceneRootNode;
SoSeparator* contentRootNode;
SoPerspectiveCamera* camera;
};
-}
-
+} // namespace armarx
diff --git a/source/RobotComponents/gui-plugins/RobotIKPlugin/RobotViewerWidget.cpp b/source/RobotComponents/gui-plugins/RobotIKPlugin/RobotViewerWidget.cpp
index 11d4271f905777fbce13a4cf32750577fd15c019..1c452c1328f8ba0b4990cbcd898e8470b2117418 100644
--- a/source/RobotComponents/gui-plugins/RobotIKPlugin/RobotViewerWidget.cpp
+++ b/source/RobotComponents/gui-plugins/RobotIKPlugin/RobotViewerWidget.cpp
@@ -44,13 +44,12 @@ armarx::RobotViewerWidget::RobotViewerWidget(QWidget* parent) : QWidget(parent)
grid->addWidget(view1, 0, 0, 1, 2);
}
-
armarx::RobotViewerWidget::~RobotViewerWidget()
{
-
}
-RobotViewerPtr armarx::RobotViewerWidget::getRobotViewer()
+RobotViewerPtr
+armarx::RobotViewerWidget::getRobotViewer()
{
return viewer;
}
diff --git a/source/RobotComponents/gui-plugins/RobotIKPlugin/RobotViewerWidget.h b/source/RobotComponents/gui-plugins/RobotIKPlugin/RobotViewerWidget.h
index d5139bfa37056f9fd9251a3b8091a4bd93da2d52..a48129b0c50c0794018086354262d33cd6dac377 100644
--- a/source/RobotComponents/gui-plugins/RobotIKPlugin/RobotViewerWidget.h
+++ b/source/RobotComponents/gui-plugins/RobotIKPlugin/RobotViewerWidget.h
@@ -24,12 +24,12 @@
#pragma once
/* Qt headers */
+#include <memory>
+
#include <QWidget>
#include "RobotViewer.h"
-#include <memory>
-
using RobotViewerPtr = std::shared_ptr<armarx::RobotViewer>;
namespace armarx
@@ -61,5 +61,4 @@ namespace armarx
private:
RobotViewerPtr viewer;
};
-}
-
+} // namespace armarx
diff --git a/source/RobotComponents/gui-plugins/RobotIKPlugin/SetDesiredPoseDialog.cpp b/source/RobotComponents/gui-plugins/RobotIKPlugin/SetDesiredPoseDialog.cpp
index 2039cfdf366a198cdf596e0107250cba592d6bcb..343ff19d4e5a238a9ec9c3e1d8e152eb897ab1c5 100644
--- a/source/RobotComponents/gui-plugins/RobotIKPlugin/SetDesiredPoseDialog.cpp
+++ b/source/RobotComponents/gui-plugins/RobotIKPlugin/SetDesiredPoseDialog.cpp
@@ -1,22 +1,26 @@
#include "SetDesiredPoseDialog.h"
-#include <RobotComponents/gui-plugins/RobotIKPlugin/ui_SetDesiredPoseDialog.h>
-
-#include <ArmarXCore/core/util/StringHelpers.h>
#include <QMessageBox>
+
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/util/StringHelpers.h>
+
+#include <RobotComponents/gui-plugins/RobotIKPlugin/ui_SetDesiredPoseDialog.h>
-static std::vector<std::string> expectedKeys({"agent", "frame", "qw", "qx", "qy", "qz", "x", "y", "z"});
+static std::vector<std::string>
+ expectedKeys({"agent", "frame", "qw", "qx", "qy", "qz", "x", "y", "z"});
SetDesiredPoseDialog::SetDesiredPoseDialog(QWidget* parent) :
- QDialog(parent),
- ui(new Ui::SetDesiredPoseDialog)
+ QDialog(parent), ui(new Ui::SetDesiredPoseDialog)
{
ui->setupUi(this);
connect(ui->pushButton_formatJSON, SIGNAL(clicked()), this, SLOT(formatInput()));
connect(ui->plainTextEdit, SIGNAL(textChanged()), this, SLOT(checkJSON()));
- connect(ui->buttonBox->button(QDialogButtonBox::Ok), SIGNAL(clicked()), this, SLOT(parseInputAndSetPose()));
+ connect(ui->buttonBox->button(QDialogButtonBox::Ok),
+ SIGNAL(clicked()),
+ this,
+ SLOT(parseInputAndSetPose()));
checkJSON();
}
@@ -26,12 +30,14 @@ SetDesiredPoseDialog::~SetDesiredPoseDialog()
delete ui;
}
-armarx::FramedPosePtr SetDesiredPoseDialog::getDesiredPose()
+armarx::FramedPosePtr
+SetDesiredPoseDialog::getDesiredPose()
{
return result;
}
-void SetDesiredPoseDialog::checkJSON()
+void
+SetDesiredPoseDialog::checkJSON()
{
std::string text = ui->plainTextEdit->toPlainText().toUtf8().data();
@@ -79,17 +85,20 @@ void SetDesiredPoseDialog::checkJSON()
ui->label_jsonValid->setStyleSheet("QLabel { color : red; }");
if (errorMsgPart.empty())
{
- ui->label_jsonValid->setText("JSON-Format: not valid, Error at " + QString(parser.getlongerrposstr().c_str()));
+ ui->label_jsonValid->setText("JSON-Format: not valid, Error at " +
+ QString(parser.getlongerrposstr().c_str()));
}
else
{
- ui->label_jsonValid->setText("JSON-Format: not valid, Missing following keys: \n{ " + QString(errorMsgPart.c_str()) + " }");
+ ui->label_jsonValid->setText("JSON-Format: not valid, Missing following keys: \n{ " +
+ QString(errorMsgPart.c_str()) + " }");
}
}
}
-void SetDesiredPoseDialog::formatInput()
+void
+SetDesiredPoseDialog::formatInput()
{
std::string text = ui->plainTextEdit->toPlainText().toUtf8().data();
if (text.empty())
@@ -127,21 +136,31 @@ void SetDesiredPoseDialog::formatInput()
}
}
-void SetDesiredPoseDialog::parseInputAndSetPose()
+void
+SetDesiredPoseDialog::parseInputAndSetPose()
{
std::string text = ui->plainTextEdit->toPlainText().toUtf8().data();
armarx::JsonObjectPtr json;
ARMARX_CHECK_EXPRESSION(stringToJSON(text, json));
- float x = armarx::toFloat((std::dynamic_pointer_cast<armarx::JsonValue>(json->get("x")))->rawValue());
- float y = armarx::toFloat((std::dynamic_pointer_cast<armarx::JsonValue>(json->get("y")))->rawValue());
- float z = armarx::toFloat((std::dynamic_pointer_cast<armarx::JsonValue>(json->get("z")))->rawValue());
- float qw = armarx::toFloat((std::dynamic_pointer_cast<armarx::JsonValue>(json->get("qw")))->rawValue());
- float qx = armarx::toFloat((std::dynamic_pointer_cast<armarx::JsonValue>(json->get("qx")))->rawValue());
- float qy = armarx::toFloat((std::dynamic_pointer_cast<armarx::JsonValue>(json->get("qy")))->rawValue());
- float qz = armarx::toFloat((std::dynamic_pointer_cast<armarx::JsonValue>(json->get("qz")))->rawValue());
- std::string agent = (std::dynamic_pointer_cast<armarx::JsonValue>(json->get("agent")))->asString();
- std::string frame = (std::dynamic_pointer_cast<armarx::JsonValue>(json->get("frame")))->asString();
+ float x =
+ armarx::toFloat((std::dynamic_pointer_cast<armarx::JsonValue>(json->get("x")))->rawValue());
+ float y =
+ armarx::toFloat((std::dynamic_pointer_cast<armarx::JsonValue>(json->get("y")))->rawValue());
+ float z =
+ armarx::toFloat((std::dynamic_pointer_cast<armarx::JsonValue>(json->get("z")))->rawValue());
+ float qw = armarx::toFloat(
+ (std::dynamic_pointer_cast<armarx::JsonValue>(json->get("qw")))->rawValue());
+ float qx = armarx::toFloat(
+ (std::dynamic_pointer_cast<armarx::JsonValue>(json->get("qx")))->rawValue());
+ float qy = armarx::toFloat(
+ (std::dynamic_pointer_cast<armarx::JsonValue>(json->get("qy")))->rawValue());
+ float qz = armarx::toFloat(
+ (std::dynamic_pointer_cast<armarx::JsonValue>(json->get("qz")))->rawValue());
+ std::string agent =
+ (std::dynamic_pointer_cast<armarx::JsonValue>(json->get("agent")))->asString();
+ std::string frame =
+ (std::dynamic_pointer_cast<armarx::JsonValue>(json->get("frame")))->asString();
armarx::Vector3Ptr pos = new armarx::Vector3(x, y, z);
armarx::QuaternionPtr quat = new armarx::Quaternion(qw, qx, qy, qz);
@@ -153,8 +172,8 @@ void SetDesiredPoseDialog::parseInputAndSetPose()
}
}
-
-bool SetDesiredPoseDialog::stringToJSON(std::string string, armarx::JsonObjectPtr& result) const
+bool
+SetDesiredPoseDialog::stringToJSON(std::string string, armarx::JsonObjectPtr& result) const
{
armarx::StructuralJsonParser parser(string, false);
parser.parse();
diff --git a/source/RobotComponents/gui-plugins/RobotIKPlugin/SetDesiredPoseDialog.h b/source/RobotComponents/gui-plugins/RobotIKPlugin/SetDesiredPoseDialog.h
index fab21b6ddade9c31064d499dd737f2d6f2ecea82..0e0b19ab4bdafbcd3bfc53fd0a57fa2b52ea1e0e 100644
--- a/source/RobotComponents/gui-plugins/RobotIKPlugin/SetDesiredPoseDialog.h
+++ b/source/RobotComponents/gui-plugins/RobotIKPlugin/SetDesiredPoseDialog.h
@@ -2,9 +2,11 @@
#define SETDESIREDPOSEDIALOG_H
#include <QDialog>
-#include <RobotAPI/libraries/core/FramedPose.h>
+
#include <ArmarXGui/libraries/StructuralJson/StructuralJsonParser.h>
+#include <RobotAPI/libraries/core/FramedPose.h>
+
namespace Ui
{
class SetDesiredPoseDialog;
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/AbstractController.h b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/AbstractController.h
index 9be1cd60bcb52a9da036d70b65ffbec5bf484b54..682ec1265ddafb43a013ff0ea0e169dcee5e0d44 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/AbstractController.h
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/AbstractController.h
@@ -21,9 +21,11 @@
*/
#ifndef ABSTRACTCONTROLLER_H
#define ABSTRACTCONTROLLER_H
-#include <QObject>
-#include <QErrorMessage>
#include <memory>
+
+#include <QErrorMessage>
+#include <QObject>
+
#include "ArmarXCore/core/logging/Logging.h"
namespace armarx
@@ -59,6 +61,6 @@ namespace armarx
};
using AbstractControllerPtr = std::shared_ptr<AbstractController>;
-}
+} // namespace armarx
#endif // ABSTRACTCONTROLLER_H
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/EnvironmentController.cpp b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/EnvironmentController.cpp
index 29892bcf095e7633aec42ce8ea14a8f3b96f1894..73f145716bc3c4ba136903cc4233dc61c6c13582 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/EnvironmentController.cpp
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/EnvironmentController.cpp
@@ -1,45 +1,56 @@
#include "EnvironmentController.h"
+
#include <string>
+
#include <QFileDialog>
+#include <QMessageBox>
+
#include <VirtualRobot/XML/RobotIO.h>
-#include <ArmarXCore/core/system/ArmarXDataPath.h>
+
#include <ArmarXCore/core/ArmarXObjectScheduler.h>
-#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
#include <ArmarXCore/core/application/Application.h>
+#include <ArmarXCore/core/system/ArmarXDataPath.h>
+#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
+
#include "../KinematicSolver.h"
-#include <QMessageBox>
namespace armarx
{
- void armarx::EnvironmentController::onInitComponent()
+ void
+ armarx::EnvironmentController::onInitComponent()
{
environment = std::make_shared<Environment>();
ARMARX_INFO << "RobotTrajectoryDesigner: EnvironmentController on init";
}
- void EnvironmentController::onConnectComponent()
+ void
+ EnvironmentController::onConnectComponent()
{
ARMARX_INFO << "RobotTrajectoryDesigner: EnvironmentController on connect";
}
- void EnvironmentController::onDisconnectComponent()
+ void
+ EnvironmentController::onDisconnectComponent()
{
ARMARX_INFO << "RobotTrajectoryDesigner: EnvironmentController on disconnect";
}
- void EnvironmentController::onExitComponent()
+ void
+ EnvironmentController::onExitComponent()
{
ARMARX_INFO << "RobotTrajectoryDesigner: EnvironmentController on exit";
}
- void EnvironmentController::loadArmar3()
+ void
+ EnvironmentController::loadArmar3()
{
std::string robotFile = "/RobotAPI/robots/Armar3/ArmarIII.xml";
CMakePackageFinder finder("RobotAPI");
if (finder.packageFound())
{
- environment->setRobot(VirtualRobot::RobotIO::loadRobot(finder.getDataDir() + robotFile));
+ environment->setRobot(
+ VirtualRobot::RobotIO::loadRobot(finder.getDataDir() + robotFile));
}
else
{
@@ -48,7 +59,8 @@ namespace armarx
emit environmentChanged(environment);
}
- void EnvironmentController::loadRobotFromProxy(std::string robotFileName)
+ void
+ EnvironmentController::loadRobotFromProxy(std::string robotFileName)
{
environment->setRobot(VirtualRobot::RobotIO::loadRobot(robotFileName));
emit environmentChanged(environment);
@@ -60,10 +72,15 @@ namespace armarx
onConnectComponent();
}
- void EnvironmentController::openRobotFileDialog()
+ void
+ EnvironmentController::openRobotFileDialog()
{
QMessageBox::StandardButton reply;
- reply = QMessageBox::question(0, "Select robot", "If you change the robot all trajectories are lost.\nDo you want to change?", QMessageBox::Yes | QMessageBox::No);
+ reply = QMessageBox::question(
+ 0,
+ "Select robot",
+ "If you change the robot all trajectories are lost.\nDo you want to change?",
+ QMessageBox::Yes | QMessageBox::No);
if (reply == QMessageBox::No)
{
return;
@@ -82,14 +99,16 @@ namespace armarx
{
QMessageBox* toManyFilesSelected = new QMessageBox;
toManyFilesSelected->setWindowTitle(QString::fromStdString("Information"));
- toManyFilesSelected->setText(QString::fromStdString("To many files selected.\nAs default the first file was taken."));
+ toManyFilesSelected->setText(QString::fromStdString(
+ "To many files selected.\nAs default the first file was taken."));
toManyFilesSelected->exec();
}
if (!fileNames.empty())
{
try
{
- VirtualRobot::RobotPtr robot = VirtualRobot::RobotIO::loadRobot(fileNames[0].toStdString());
+ VirtualRobot::RobotPtr robot =
+ VirtualRobot::RobotIO::loadRobot(fileNames[0].toStdString());
environment->setRobot(robot);
emit environmentChanged(environment);
KinematicSolver::reset();
@@ -99,9 +118,10 @@ namespace armarx
{
QMessageBox* selectRobotFailed = new QMessageBox;
selectRobotFailed->setWindowTitle(QString::fromStdString("Error Message"));
- selectRobotFailed->setText(QString::fromStdString("The selected file is not a robot xml file."));
+ selectRobotFailed->setText(
+ QString::fromStdString("The selected file is not a robot xml file."));
selectRobotFailed->exec();
}
}
}
-}
+} // namespace armarx
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/EnvironmentController.h b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/EnvironmentController.h
index fb231d5f9e348a5a218d116c71441bce3efbf0ff..7712e667146248313f50ee962d2cd4c3c6166171 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/EnvironmentController.h
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/EnvironmentController.h
@@ -1,9 +1,10 @@
#ifndef ENVIRONMENTCONTROLLER_H
#define ENVIRONMENTCONTROLLER_H
-#include "AbstractController.h"
-#include "../Environment.h"
#include <RobotAPI/interface/core/RobotState.h>
+#include "../Environment.h"
+#include "AbstractController.h"
+
namespace armarx
{
class EnvironmentController : public AbstractController
@@ -66,6 +67,6 @@ namespace armarx
};
using EnvironmentControllerPtr = std::shared_ptr<EnvironmentController>;
-}
+} // namespace armarx
#endif // ENVIRONMENTCONTROLLER_H
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/ExportDialogController.cpp b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/ExportDialogController.cpp
index 71411fc7ac3d17fca715d79a3d3c2e22a9df6345..37cf49e543ae3507b380d12fe7f87a8ebeb3e46b 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/ExportDialogController.cpp
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/ExportDialogController.cpp
@@ -1,9 +1,11 @@
#include "ExportDialogController.h"
+
#include <QMessageBox>
namespace armarx
{
- void ExportDialogController::onInitComponent()
+ void
+ ExportDialogController::onInitComponent()
{
exportDialog = new QDialog;
ui.setupUi(exportDialog);
@@ -11,7 +13,8 @@ namespace armarx
ARMARX_INFO << "RobotTrajectoryDesigner: ExportDialogController on init";
}
- void ExportDialogController::onConnectComponent()
+ void
+ ExportDialogController::onConnectComponent()
{
QObject::connect(ui.cancel, SIGNAL(clicked()), exportDialog, SLOT(reject()));
QObject::connect(ui.exportMMM, SIGNAL(clicked()), exportDialog, SLOT(accept()));
@@ -19,12 +22,14 @@ namespace armarx
QObject::connect(ui.selcetFPS, SIGNAL(valueChanged(int)), this, SLOT(setFPS(int)));
}
- void ExportDialogController::onDisconnectComponent()
+ void
+ ExportDialogController::onDisconnectComponent()
{
ARMARX_INFO << "RobotTrajectoryDesigner: ExportDialogController on disconnect";
}
- void ExportDialogController::onExitComponent()
+ void
+ ExportDialogController::onExitComponent()
{
ARMARX_INFO << "RobotTrajectoryDesigner: ExportDialogController on exit";
}
@@ -35,24 +40,29 @@ namespace armarx
onConnectComponent();
}
- void ExportDialogController::open()
+ void
+ ExportDialogController::open()
{
//fps = ui.selcetFPS->value();
exportDialog->setModal(true);
exportDialog->exec();
}
- void ExportDialogController::setFPS(int fps)
+ void
+ ExportDialogController::setFPS(int fps)
{
this->fps = fps;
}
- void ExportDialogController::exportTrajectory(std::vector<armarx::DesignerTrajectoryPtr> trajectories)
+ void
+ ExportDialogController::exportTrajectory(
+ std::vector<armarx::DesignerTrajectoryPtr> trajectories)
{
this->trajectories = trajectories;
}
- void ExportDialogController::exportMMM()
+ void
+ ExportDialogController::exportMMM()
{
if (environment == NULL)
{
@@ -66,7 +76,8 @@ namespace armarx
{
QMessageBox* exportMMMfailed = new QMessageBox();
exportMMMfailed->setWindowTitle(QString::fromStdString("Error Message"));
- exportMMMfailed->setText(QString::fromStdString("Export failed.\nNo trajectory is implemented."));
+ exportMMMfailed->setText(
+ QString::fromStdString("Export failed.\nNo trajectory is implemented."));
exportMMMfailed->exec();
return;
}
@@ -85,7 +96,8 @@ namespace armarx
}
}
- void ExportDialogController::exportTrajectory()
+ void
+ ExportDialogController::exportTrajectory()
{
/*if (environment == NULL)
{
@@ -120,12 +132,14 @@ namespace armarx
throw armarx::NotImplementedYetException();
}
- void ExportDialogController::environmentChanged(EnvironmentPtr environment)
+ void
+ ExportDialogController::environmentChanged(EnvironmentPtr environment)
{
this->environment = environment;
}
- QString ExportDialogController::saveToFile()
+ QString
+ ExportDialogController::saveToFile()
{
QFileDialog dialog(0);
dialog.setFileMode(QFileDialog::AnyFile);
@@ -147,4 +161,4 @@ namespace armarx
}
-}
+} // namespace armarx
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/ExportDialogController.h b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/ExportDialogController.h
index 5a4323b1d20627e41198d6e54d8af60ce505247a..005e8ddc6cdc765a7eb2c3bdddbf1b0a8fb6cd1a 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/ExportDialogController.h
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/ExportDialogController.h
@@ -1,15 +1,16 @@
#ifndef EXPORTDIALOGCONTROLLER_H
#define EXPORTDIALOGCONTROLLER_H
-#include "AbstractController.h"
#include <QDialog>
-#include "../View/ui_ExportDialog.h"
-#include "../Model/DesignerTrajectory.h"
#include <QErrorMessage>
-#include <QString>
-#include <QFileDialog>
#include <QFile>
-#include "../ImportExport/MMMExporter.h"
+#include <QFileDialog>
+#include <QString>
+
#include "../Environment.h"
+#include "../ImportExport/MMMExporter.h"
+#include "../Model/DesignerTrajectory.h"
+#include "../View/ui_ExportDialog.h"
+#include "AbstractController.h"
namespace armarx
{
@@ -111,6 +112,6 @@ namespace armarx
};
using ExportDialogControllerPtr = std::shared_ptr<ExportDialogController>;
-}
+} // namespace armarx
#endif // EXPORTDIALOGCONTROLLER_H
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/ImportDialogController.cpp b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/ImportDialogController.cpp
index 52af7768cb9edd354fc5f495b1f50e2994ce2b7c..47f0afde784d5c364ed519d3549859a2ff7bb46e 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/ImportDialogController.cpp
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/ImportDialogController.cpp
@@ -1,27 +1,32 @@
#include "ImportDialogController.h"
+
#include <QFile>
#include <QFileDialog>
#include <QMessageBox>
namespace armarx
{
- void ImportDialogController::onInitComponent()
+ void
+ ImportDialogController::onInitComponent()
{
ARMARX_INFO << "RobotTrajectoryDesigner: ExportDialogController on init";
}
- void ImportDialogController::onConnectComponent()
+ void
+ ImportDialogController::onConnectComponent()
{
ARMARX_INFO << "RobotTrajectoryDesigner: ExportDialogController on connect";
}
- void ImportDialogController::onDisconnectComponent()
+ void
+ ImportDialogController::onDisconnectComponent()
{
ARMARX_INFO << "RobotTrajectoryDesigner: ExportDialogController on disconnect";
}
- void ImportDialogController::onExitComponent()
+ void
+ ImportDialogController::onExitComponent()
{
ARMARX_INFO << "RobotTrajectoryDesigner: ExportDialogController on exit";
}
@@ -32,7 +37,8 @@ namespace armarx
onConnectComponent();
}
- void ImportDialogController::open()
+ void
+ ImportDialogController::open()
{
if (environment == NULL)
{
@@ -51,7 +57,8 @@ namespace armarx
{
QMessageBox* toManyFilesSelected = new QMessageBox;
toManyFilesSelected->setWindowTitle(QString::fromStdString("Information"));
- toManyFilesSelected->setText(QString::fromStdString("To many files selected.\nAs default the first file was taken."));
+ toManyFilesSelected->setText(QString::fromStdString(
+ "To many files selected.\nAs default the first file was taken."));
toManyFilesSelected->exec();
}
if (!fileNames.empty())
@@ -59,7 +66,8 @@ namespace armarx
try
{
MMMImporterPtr mmmImporter = std::make_shared<MMMImporter>(environment);
- std::vector<DesignerTrajectoryPtr> trajectories = mmmImporter->importTrajectory(fileNames[0].toStdString());
+ std::vector<DesignerTrajectoryPtr> trajectories =
+ mmmImporter->importTrajectory(fileNames[0].toStdString());
if (!trajectories.empty())
{
for (DesignerTrajectoryPtr trajectory : trajectories)
@@ -83,16 +91,18 @@ namespace armarx
}
}
- void ImportDialogController::environmentChanged(EnvironmentPtr environment)
+ void
+ ImportDialogController::environmentChanged(EnvironmentPtr environment)
{
this->environment = environment;
}
- void ImportDialogController::helpExceptionMessageBox(std::string errorMessage)
+ void
+ ImportDialogController::helpExceptionMessageBox(std::string errorMessage)
{
QMessageBox* errorMessageBox = new QMessageBox;
errorMessageBox->setWindowTitle(QString::fromStdString("Error Message"));
errorMessageBox->setText(QString::fromStdString(errorMessage));
errorMessageBox->exec();
}
-}
+} // namespace armarx
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/ImportDialogController.h b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/ImportDialogController.h
index 9251a1292a0e27cba2244ae44020e2c37320597e..a67a59fa13f307d3fb27d0c6e48ab66fe2483b48 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/ImportDialogController.h
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/ImportDialogController.h
@@ -1,8 +1,8 @@
#ifndef IMPORTDIALOGCONTROLLER_H
#define IMPORTDIALOGCONTROLLER_H
-#include "AbstractController.h"
-#include "../Model/DesignerTrajectory.h"
#include "../ImportExport/MMMImporter.h"
+#include "../Model/DesignerTrajectory.h"
+#include "AbstractController.h"
namespace armarx
{
@@ -60,6 +60,7 @@ namespace armarx
* @brief Notifies other controllers about the import of a trajectory
*/
void import(DesignerTrajectoryPtr trajectory);
+
private:
EnvironmentPtr environment;
@@ -67,6 +68,6 @@ namespace armarx
};
using ImportDialogControllerPtr = std::shared_ptr<ImportDialogController>;
-}
+} // namespace armarx
#endif // IMPORTDIALOGCONTROLLER_H
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/MementoController.cpp b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/MementoController.cpp
index f7d8ed98b9b9592fbbebb73cd6d57c1ab98bf220..d5065266c4757e94aa1bd4932c241395fb712cc1 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/MementoController.cpp
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/MementoController.cpp
@@ -23,7 +23,8 @@
namespace armarx
{
- void MementoController::onInitComponent()
+ void
+ MementoController::onInitComponent()
{
ARMARX_INFO << "RobotTrajectoryDesigner: MementoController on init";
@@ -31,7 +32,8 @@ namespace armarx
enableRedoButton(false);
}
- void MementoController::onConnectComponent()
+ void
+ MementoController::onConnectComponent()
{
// Undo lastly executed operation
QObject::connect(undoButton, SIGNAL(clicked()), this, SLOT(undoOperation()));
@@ -40,51 +42,58 @@ namespace armarx
QObject::connect(redoButton, SIGNAL(clicked()), this, SLOT(redoOperation()));
}
- void MementoController::onDisconnectComponent()
+ void
+ MementoController::onDisconnectComponent()
{
ARMARX_INFO << "RobotTrajectoryDesigner: MementoController on disconnect";
}
- void MementoController::onExitComponent()
+ void
+ MementoController::onExitComponent()
{
ARMARX_INFO << "RobotTrajectoryDesigner: MementoController on exit";
}
MementoController::MementoController(QPushButton* undoButton, QPushButton* redoButton) :
- undoButton(undoButton),
- redoButton(redoButton)
+ undoButton(undoButton), redoButton(redoButton)
{
onInitComponent();
onConnectComponent();
}
- QPushButton* MementoController::getUndoButton()
+ QPushButton*
+ MementoController::getUndoButton()
{
return this->undoButton;
}
- QPushButton* MementoController::getRedoButton()
+ QPushButton*
+ MementoController::getRedoButton()
{
return this->redoButton;
}
- void MementoController::undoOperation()
+ void
+ MementoController::undoOperation()
{
emit undo();
}
- void MementoController::redoOperation()
+ void
+ MementoController::redoOperation()
{
emit redo();
}
- void MementoController::enableRedoButton(bool enable)
+ void
+ MementoController::enableRedoButton(bool enable)
{
this->redoButton->setEnabled(enable);
this->redoBool = enable;
}
- void MementoController::enableRedoButtonVisualization(bool enable)
+ void
+ MementoController::enableRedoButtonVisualization(bool enable)
{
if (redoBool)
{
@@ -92,8 +101,9 @@ namespace armarx
}
}
- void MementoController::enableUndoButton(bool enable)
+ void
+ MementoController::enableUndoButton(bool enable)
{
this->undoButton->setEnabled(enable);
}
-}
+} // namespace armarx
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/MementoController.h b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/MementoController.h
index 3751ddbda816eb54163a437b981185ccb62a3090..03de51436c35ee0dca3db0e5adfcb41ee8c9ad42 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/MementoController.h
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/MementoController.h
@@ -21,9 +21,10 @@
*/
#ifndef MEMENTOCONTROLLER_H
#define MEMENTOCONTROLLER_H
-#include "AbstractController.h"
#include <QPushButton>
+#include "AbstractController.h"
+
namespace armarx
{
/**
@@ -125,6 +126,6 @@ namespace armarx
};
using MementoControllerPtr = std::shared_ptr<MementoController>;
-}
+} // namespace armarx
#endif // MEMENTOCONTROLLER_H
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/RobotVisualizationController.cpp b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/RobotVisualizationController.cpp
index 3d3e04a167b09d6becc1a6e126c1e6aa9573f68c..2e6c2138d1d0ef91ea5b0cd0816eea1da81b5d81 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/RobotVisualizationController.cpp
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/RobotVisualizationController.cpp
@@ -21,24 +21,19 @@
*/
#include "RobotVisualizationController.h"
-#include "RobotAPI/libraries/core/Trajectory.h"
-
-#include "RobotAPI/libraries/core/TrajectoryController.h"
-
-#include "VirtualRobot/XML/RobotIO.h"
+#include <QGridLayout>
-#include "VirtualRobot/RobotNodeSet.h"
+#include <ArmarXCore/core/logging/Logging.h>
-#include "../KinematicSolver.h"
+#include "RobotAPI/libraries/core/Trajectory.h"
+#include "RobotAPI/libraries/core/TrajectoryController.h"
#include "../Environment.h"
-
+#include "../KinematicSolver.h"
#include "../Util/OrientationConversion.h"
-
#include "../Visualization/DesignerTrajectoryPlayer.h"
-#include <ArmarXCore/core/logging/Logging.h>
-
-#include <QGridLayout>
+#include "VirtualRobot/RobotNodeSet.h"
+#include "VirtualRobot/XML/RobotIO.h"
#define ROBOT_UPDATE_TIMER_MS 33
@@ -57,10 +52,10 @@ RobotVisualizationController::RobotVisualizationController(QWidget* parent) : pa
RobotVisualizationController::~RobotVisualizationController()
{
-
}
-void RobotVisualizationController::onInitComponent()
+void
+RobotVisualizationController::onInitComponent()
{
//initialize Attributes
this->cs = IKSolver::CartesianSelection::All;
@@ -74,7 +69,12 @@ void RobotVisualizationController::onInitComponent()
QWidget* viewerParent = new QWidget();
this->viewer = RobotVisualizationPtr(new CoinRobotViewerAdapter(viewerParent));
this->viewSplitter = new RobotVisualizationWidget(0, viewerParent, viewer);
- dynamic_cast<QGridLayout*>(parent->layout())->addWidget(viewSplitter, 0, 0, 1, 3);//this needs to be done in order to let Toolbar and view overlap
+ dynamic_cast<QGridLayout*>(parent->layout())
+ ->addWidget(viewSplitter,
+ 0,
+ 0,
+ 1,
+ 3); //this needs to be done in order to let Toolbar and view overlap
if (environment != NULL && environment->getRobot())
{
robotChanged(environment->getRobot());
@@ -83,32 +83,39 @@ void RobotVisualizationController::onInitComponent()
ARMARX_INFO << "RobotTrajectoryDesigner: RobotVisualizationController on init component";
}
-void RobotVisualizationController::onConnectComponent()
+void
+RobotVisualizationController::onConnectComponent()
{
ARMARX_INFO << "RobotTrajectoryDesigner: RobotVisualizationController on connect component";
}
-void RobotVisualizationController::onDisconnectComponent()
+void
+RobotVisualizationController::onDisconnectComponent()
{
ARMARX_INFO << "RobotTrajectoryDesigner: RobotVisualizationController on disconnect";
}
-void RobotVisualizationController::onExitComponent()
+void
+RobotVisualizationController::onExitComponent()
{
ARMARX_INFO << "RobotTrajectoryDesigner: RobotVisualizationController on exit components";
}
-void RobotVisualizationController::refresh()
+void
+RobotVisualizationController::refresh()
{
//UPDATE WAYPOINT SELECTION
if (selectedWayPoint != this->viewer->getSelectedWaypoint())
{
selectedWayPoint = this->viewer->getSelectedWaypoint();
emit wayPointSelected(selectedWayPoint);
- if (currentTrajectory && static_cast<unsigned>(viewer->getSelectedWaypoint()) < currentTrajectory->getAllUserWaypoints().size())
+ if (currentTrajectory && static_cast<unsigned>(viewer->getSelectedWaypoint()) <
+ currentTrajectory->getAllUserWaypoints().size())
{
- std::vector<double> angles = currentTrajectory->getUserWaypoint(viewer->getSelectedWaypoint())->getJointAngles();
- viewer->setManipulator(currentTrajectory->getRns(), std::vector<float>(angles.begin(), angles.end()));
+ std::vector<double> angles =
+ currentTrajectory->getUserWaypoint(viewer->getSelectedWaypoint())->getJointAngles();
+ viewer->setManipulator(currentTrajectory->getRns(),
+ std::vector<float>(angles.begin(), angles.end()));
}
}
//UPDATE MANIPULATOR MOVEMENT
@@ -120,7 +127,11 @@ void RobotVisualizationController::refresh()
KinematicSolverPtr ks = KinematicSolver::getInstance(NULL, robot);
PoseBasePtr destination = PoseBasePtr(new Pose(viewer->getManipulatorPose()));
std::string currentRnsName = selectedKinematicChain->getName();
- std::vector<float> jointAngles = ks->solveRecursiveIK(this->robot->getRobotNodeSet(currentRnsName), robot->getRobotNodeSet(currentRnsName)->getJointValues(), destination, cs);
+ std::vector<float> jointAngles =
+ ks->solveRecursiveIK(this->robot->getRobotNodeSet(currentRnsName),
+ robot->getRobotNodeSet(currentRnsName)->getJointValues(),
+ destination,
+ cs);
if (jointAngles.size() != 0 && iKCallback)
{
robot->getRobotNodeSet(currentRnsName)->setJointValues(jointAngles);
@@ -133,7 +144,8 @@ void RobotVisualizationController::refresh()
}
}
-void RobotVisualizationController::addConnection(RobotVisualizationControllerPtr ctr)
+void
+RobotVisualizationController::addConnection(RobotVisualizationControllerPtr ctr)
{
viewer->setObserver(ctr);
}
@@ -141,12 +153,14 @@ void RobotVisualizationController::addConnection(RobotVisualizationControllerPtr
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/// PARALLEL VIEWS
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-void RobotVisualizationController::addView()
+void
+RobotVisualizationController::addView()
{
this->viewSplitter->addWidget();
}
-void RobotVisualizationController::removeView()
+void
+RobotVisualizationController::removeView()
{
this->viewSplitter->removeWidget();
}
@@ -155,13 +169,15 @@ void RobotVisualizationController::removeView()
/// UPDATING OF VISUALIZATION
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-void RobotVisualizationController::environmentChanged(EnvironmentPtr environment)
+void
+RobotVisualizationController::environmentChanged(EnvironmentPtr environment)
{
this->environment = environment;
robotChanged(environment->getRobot());
}
-void RobotVisualizationController::robotChanged(VirtualRobot::RobotPtr robot)
+void
+RobotVisualizationController::robotChanged(VirtualRobot::RobotPtr robot)
{
this->robot = robot;
if (viewer)
@@ -173,10 +189,10 @@ void RobotVisualizationController::robotChanged(VirtualRobot::RobotPtr robot)
this->selectedWayPoint = 0;
this->selectedTransition = -1;
this->setCamera(0);
-
}
-void RobotVisualizationController::updateViews(armarx::DesignerTrajectoryPtr trajectory)
+void
+RobotVisualizationController::updateViews(armarx::DesignerTrajectoryPtr trajectory)
{
/*for (RobotVisualizationPtr visu : viewers)
{
@@ -204,7 +220,8 @@ void RobotVisualizationController::updateViews(armarx::DesignerTrajectoryPtr tra
for (UserWaypointPtr currentPoint : currentTrajectory->getAllUserWaypoints())
{
- VirtualRobot::EndEffectorPtr endEffector = robot->getEndEffector(trajectory->getRns()->getTCP()->getName());
+ VirtualRobot::EndEffectorPtr endEffector =
+ robot->getEndEffector(trajectory->getRns()->getTCP()->getName());
if (!endEffector)
{
std::vector<VirtualRobot::EndEffectorPtr> eefs;
@@ -218,19 +235,27 @@ void RobotVisualizationController::updateViews(armarx::DesignerTrajectoryPtr tra
}
}
}
- Pose localPose = Pose(currentPoint->getPose()->position, currentPoint->getPose()->orientation);
+ Pose localPose =
+ Pose(currentPoint->getPose()->position, currentPoint->getPose()->orientation);
if (endEffector)
{
- viewer->addWaypointVisualization(i, PosePtr(new Pose(trajectory->getRns()->getKinematicRoot()->toGlobalCoordinateSystem(localPose.toEigen()))), endEffector);
+ viewer->addWaypointVisualization(
+ i,
+ PosePtr(new Pose(trajectory->getRns()->getKinematicRoot()->toGlobalCoordinateSystem(
+ localPose.toEigen()))),
+ endEffector);
//visu->addWaypointVisualization(i, currentPoint->getPose(), robot->getEndEffector(robot->getRobotNodeSet(trajectory->getRns()->getName())->getTCP()->getName()));
}
else
{
- viewer->addWaypointVisualization(i, PosePtr(new Pose(trajectory->getRns()->getKinematicRoot()->toGlobalCoordinateSystem(localPose.toEigen()))), NULL);
+ viewer->addWaypointVisualization(
+ i,
+ PosePtr(new Pose(trajectory->getRns()->getKinematicRoot()->toGlobalCoordinateSystem(
+ localPose.toEigen()))),
+ NULL);
}
i++;
-
}
@@ -259,16 +284,14 @@ void RobotVisualizationController::updateViews(armarx::DesignerTrajectoryPtr tra
//ARMARX_INFO << "DEBUG END";
}
-
-
-
-
-void RobotVisualizationController::displayAllWayPoints(bool display)
+void
+RobotVisualizationController::displayAllWayPoints(bool display)
{
//TODO find out sphere size
}
-void RobotVisualizationController::selectedTransitionChanged(int index)
+void
+RobotVisualizationController::selectedTransitionChanged(int index)
{
KinematicSolverPtr ks = KinematicSolver::getInstance(NULL, robot);
TrajectoryPtr selectedTransition = currentTrajectory->getTransition(index)->getTrajectory();
@@ -284,9 +307,12 @@ void RobotVisualizationController::selectedTransitionChanged(int index)
/*for (RobotVisualizationPtr visu : viewers)*/
{
- if (currentTrajectory->getNrOfUserWaypoints() > static_cast<unsigned>(this->selectedTransition + 1) && this->selectedTransition >= 0)
+ if (currentTrajectory->getNrOfUserWaypoints() >
+ static_cast<unsigned>(this->selectedTransition + 1) &&
+ this->selectedTransition >= 0)
{
- TrajectoryPtr deselectedTransition = currentTrajectory->getTransition(this->selectedTransition)->getTrajectory();
+ TrajectoryPtr deselectedTransition =
+ currentTrajectory->getTransition(this->selectedTransition)->getTrajectory();
std::vector<PoseBasePtr> oldCurve = std::vector<PoseBasePtr>();
for (const Trajectory::TrajData& element : *deselectedTransition)
@@ -304,7 +330,8 @@ void RobotVisualizationController::selectedTransitionChanged(int index)
this->selectedTransition = index;
}
-void RobotVisualizationController::clearView()
+void
+RobotVisualizationController::clearView()
{
viewer->clearTrajectory();
this->currentTrajectory = NULL;
@@ -313,13 +340,16 @@ void RobotVisualizationController::clearView()
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/// IK CALLBACK METHODS
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-void RobotVisualizationController::cartesianSelectionChanged(VirtualRobot::IKSolver::CartesianSelection cs)
+void
+RobotVisualizationController::cartesianSelectionChanged(
+ VirtualRobot::IKSolver::CartesianSelection cs)
{
this->cs = IKSolver::CartesianSelection::All;
refresh();
}
-void RobotVisualizationController::kinematicChainChanged(RobotNodeSetPtr rns)
+void
+RobotVisualizationController::kinematicChainChanged(RobotNodeSetPtr rns)
{
viewer->removeAllWaypointVisualizations();
//IKCallback
@@ -329,13 +359,15 @@ void RobotVisualizationController::kinematicChainChanged(RobotNodeSetPtr rns)
if (selectedKinematicChain)
{
std::vector<float> jointAngles;
- jointAngles = std::vector<float>(robot->getRobotNodeSet(this->selectedKinematicChain->getName())->getJointValues());
+ jointAngles = std::vector<float>(
+ robot->getRobotNodeSet(this->selectedKinematicChain->getName())->getJointValues());
viewer->setManipulator(this->selectedKinematicChain, jointAngles);
}
this->updateViews(NULL);
}
-void RobotVisualizationController::setIKCallbackEnabled(bool enabled)
+void
+RobotVisualizationController::setIKCallbackEnabled(bool enabled)
{
iKCallback = enabled;
}
@@ -343,12 +375,14 @@ void RobotVisualizationController::setIKCallbackEnabled(bool enabled)
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/// CAMERA UPDATING METHODS
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-void RobotVisualizationController::setCamera()
+void
+RobotVisualizationController::setCamera()
{
setCamera(0);
}
-void RobotVisualizationController::setCamera(int perspective)
+void
+RobotVisualizationController::setCamera(int perspective)
{
Eigen::Vector3f position;
Eigen::Vector3f pointAtA = Eigen::Vector3f(0, 0, 0);
@@ -356,35 +390,35 @@ void RobotVisualizationController::setCamera(int perspective)
switch (perspective)
{
- case 0 :
+ case 0:
//HIGH ANGLE
position = Eigen::Vector3f(0, 2, 3.5);
pointAtB = Eigen::Vector3f(0, 0, 5);
break;
- case 1 :
+ case 1:
//TOP
position = Eigen::Vector3f(0, 0, 4);
pointAtB = Eigen::Vector3f(0, -360, 0);
break;
- case 2 :
+ case 2:
//FRONT
position = Eigen::Vector3f(0, 5, 2);
pointAtB = Eigen::Vector3f(0, 0, 2);
break;
- case 3 :
+ case 3:
//BACK
position = Eigen::Vector3f(0, -5, 2);
pointAtB = Eigen::Vector3f(0, 0, 2);
break;
- case 4 :
+ case 4:
//LEFT
position = Eigen::Vector3f(5, 0, 2);
pointAtB = Eigen::Vector3f(0, 0, 2);
break;
- case 5 :
+ case 5:
//RIGHT
position = Eigen::Vector3f(-5, 0, 2);
pointAtB = Eigen::Vector3f(0, 0, 2);
@@ -397,10 +431,10 @@ void RobotVisualizationController::setCamera(int perspective)
/*RobotVisualizationPtr expectedViewer = viewers[0];
expectedViewer->setCamera(position, pointAtA, pointAtB);
expectedViewer->updateRobotVisualization();*/
-
}
-void RobotVisualizationController::selectedWayPointChanged(int index)
+void
+RobotVisualizationController::selectedWayPointChanged(int index)
{
viewer->setSelectedWaypoint(index);
if (currentTrajectory)
@@ -409,7 +443,8 @@ void RobotVisualizationController::selectedWayPointChanged(int index)
std::vector<double> jA = currentTrajectory->getUserWaypoint(index)->getJointAngles();
std::vector<float> jointAngles = std::vector<float>(jA.begin(), jA.end());
viewer->setManipulator(currentTrajectory->getRns(), jointAngles);
- this->cartesianSelectionChanged(currentTrajectory->getUserWaypoint(index)->getIKSelection());
+ this->cartesianSelectionChanged(
+ currentTrajectory->getUserWaypoint(index)->getIKSelection());
}
}
@@ -417,14 +452,16 @@ void RobotVisualizationController::selectedWayPointChanged(int index)
/// PLAY TRAJECTORY
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-void RobotVisualizationController::playTrajectory()
+void
+RobotVisualizationController::playTrajectory()
{
player = DesignerTrajectoryPlayerPtr(new DesignerTrajectoryPlayer(this->viewer, this->robot));
player->addTrajectory(currentTrajectory);
this->playerStarter();
}
-void RobotVisualizationController::playTrajectories(std::vector<DesignerTrajectoryPtr> trajectories)
+void
+RobotVisualizationController::playTrajectories(std::vector<DesignerTrajectoryPtr> trajectories)
{
player = DesignerTrajectoryPlayerPtr(new DesignerTrajectoryPlayer(this->viewer, this->robot));
for (DesignerTrajectoryPtr current : trajectories)
@@ -434,19 +471,23 @@ void RobotVisualizationController::playTrajectories(std::vector<DesignerTrajecto
this->playerStarter();
}
-void RobotVisualizationController::trajectoryPlayerStopped()
+void
+RobotVisualizationController::trajectoryPlayerStopped()
{
player = NULL;
KinematicSolver::getInstance(NULL, NULL)->syncRobotPrx();
updateViews(currentTrajectory);
setIKCallbackEnabled(true);
- viewer->setManipulator(selectedKinematicChain, robot->getRobotNodeSet(selectedKinematicChain->getName())->getJointValues());
+ viewer->setManipulator(
+ selectedKinematicChain,
+ robot->getRobotNodeSet(selectedKinematicChain->getName())->getJointValues());
playerRunning = false;
emit trajectoryPlayerNotPlaying(!playerRunning);
emit trajectoryPlayerPlaying(playerRunning);
}
-void RobotVisualizationController::playerStarter()
+void
+RobotVisualizationController::playerStarter()
{
if (playerRunning || !selectedKinematicChain)
{
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/RobotVisualizationController.h b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/RobotVisualizationController.h
index 231cca9649ddd1a76f6fed56a994b8bff4a17a52..41fdaf82b7ffd0da5f4736fbffe967c74d861fd2 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/RobotVisualizationController.h
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/RobotVisualizationController.h
@@ -22,19 +22,21 @@
#ifndef ROBOTVIEWERCONTROLLER_H
#define ROBOTVIEWERCONTROLLER_H
-#include "AbstractController.h"
-#include "RobotAPI/interface/core/PoseBase.h"
#include <Eigen/Eigen>
+
+#include "RobotAPI/interface/core/PoseBase.h"
#include "RobotAPI/libraries/core/Trajectory.h"
-#include <../Visualization/RobotVisualization.h>
-#include "../Visualization/DesignerTrajectoryPlayer.h"
+
+#include "../Environment.h"
#include "../Model/DesignerTrajectory.h"
-#include "VirtualRobot/IK/IKSolver.h"
#include "../Visualization/CoinRobotViewerAdapter.h"
+#include "../Visualization/DesignerTrajectoryPlayer.h"
#include "../Visualization/RobotVisualizationWidget.h"
-#include <Inventor/sensors/SoTimerSensor.h>
#include "../Visualization/VisualizationObserver.h"
-#include "../Environment.h"
+#include "AbstractController.h"
+#include "VirtualRobot/IK/IKSolver.h"
+#include <../Visualization/RobotVisualization.h>
+#include <Inventor/sensors/SoTimerSensor.h>
namespace armarx
{
@@ -147,8 +149,6 @@ namespace armarx
void setIKCallbackEnabled(bool enabled);
-
-
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/// CAMERA UPDATING METHODS
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
@@ -188,8 +188,6 @@ namespace armarx
void trajectoryPlayerStopped();
-
-
signals:
/**
* @brief wayPointSelected informs all relevant controllers that the currently selected Waypoit has been changed via click on the waypoint in the RobotViewer
@@ -219,7 +217,6 @@ namespace armarx
void trajectoryPlayerPlaying(bool playing);
private:
-
RobotVisualizationPtr viewer;
QWidget* parent;
RobotVisualizationWidget* viewSplitter;
@@ -237,8 +234,7 @@ namespace armarx
void playerStarter();
};
-
using RobotVisualizationControllerPtr = std::shared_ptr<RobotVisualizationController>;
-}
+} // namespace armarx
#endif
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/SettingController.cpp b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/SettingController.cpp
index afe99401ba139c938d3bf4ceba93c0960f77fd85..a935cdfe625b6cb7f6f55ad317eeec20243f5b32 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/SettingController.cpp
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/SettingController.cpp
@@ -20,13 +20,17 @@
* GNU General Public License
*/
#include "SettingController.h"
+
+#include <QFileDialog>
+
#include <VirtualRobot/XML/RobotIO.h>
+
#include "../KinematicSolver.h"
-#include <QFileDialog>
namespace armarx
{
- void SettingController::onInitComponent()
+ void
+ SettingController::onInitComponent()
{
ARMARX_INFO << "RobotTrajectoryDesigner: SettingController on init";
@@ -36,68 +40,86 @@ namespace armarx
enableIKSolutionButton(false);
}
- void SettingController::onConnectComponent()
+ void
+ SettingController::onConnectComponent()
{
ARMARX_INFO << "RobotTrajectoryDesigner: SettingController on connect";
// Robot selection button: clicked
QObject::connect(guiSettingTab->getSettingTab()->robotSelectionButton,
- SIGNAL(clicked()), this, SLOT(openRobotSelectionDialog()));
+ SIGNAL(clicked()),
+ this,
+ SLOT(openRobotSelectionDialog()));
// Shortcut button: clicked
QObject::connect(guiSettingTab->getSettingTab()->shortcutButton,
- SIGNAL(clicked()), this, SLOT(openShortcutDialog()));
+ SIGNAL(clicked()),
+ this,
+ SLOT(openShortcutDialog()));
// TCP combo box: index changed
QObject::connect(guiSettingTab->getSettingTab()->tcpComboBox,
SIGNAL(activated(int)),
- this, SLOT(selectTCP(int)));
+ this,
+ SLOT(selectTCP(int)));
// New IK solution button: clicked
QObject::connect(guiSettingTab->getSettingTab()->newIKSolutionButton,
- SIGNAL(clicked()), this, SLOT(newIKSolution()));
+ SIGNAL(clicked()),
+ this,
+ SLOT(newIKSolution()));
// Active collision model changed
QObject::connect(guiSettingTab->getSettingTab()->collisionModelComboBox,
- SIGNAL(activated(int)), this, SLOT(selectActiveCM(int)));
+ SIGNAL(activated(int)),
+ this,
+ SLOT(selectActiveCM(int)));
// Other used collision models changed
QObject::connect(guiSettingTab->getSettingTab()->collisionModelList,
SIGNAL(itemChanged(QListWidgetItem*)),
- this, SLOT(setCollisionModelList(QListWidgetItem*)));
+ this,
+ SLOT(setCollisionModelList(QListWidgetItem*)));
// Export
QObject::connect(guiSettingTab->getSettingTab()->convertToMMMButton,
- SIGNAL(clicked()), this, SLOT(convertToMMMSlot()));
+ SIGNAL(clicked()),
+ this,
+ SLOT(convertToMMMSlot()));
// Import
QObject::connect(guiSettingTab->getSettingTab()->importButton,
- SIGNAL(clicked()), this, SLOT(openImportDialog()));
+ SIGNAL(clicked()),
+ this,
+ SLOT(openImportDialog()));
}
- void SettingController::onDisconnectComponent()
+ void
+ SettingController::onDisconnectComponent()
{
ARMARX_INFO << "RobotTrajectoryDesigner: SettingController on disconnect";
}
- void SettingController::onExitComponent()
+ void
+ SettingController::onExitComponent()
{
ARMARX_INFO << "RobotTrajectoryDesigner: SettingController on exit";
}
- SettingController::SettingController(SettingTabPtr settingTab) :
- guiSettingTab(settingTab)
+ SettingController::SettingController(SettingTabPtr settingTab) : guiSettingTab(settingTab)
{
onConnectComponent();
onInitComponent();
}
- SettingTabPtr SettingController::getGuiSettingTab()
+ SettingTabPtr
+ SettingController::getGuiSettingTab()
{
return this->guiSettingTab;
}
- void SettingController::setGuiSettingTab(SettingTabPtr guiSettingTab)
+ void
+ SettingController::setGuiSettingTab(SettingTabPtr guiSettingTab)
{
if (guiSettingTab != NULL)
{
@@ -105,17 +127,20 @@ namespace armarx
}
}
- void SettingController::openRobotSelectionDialog()
+ void
+ SettingController::openRobotSelectionDialog()
{
emit openRobotSelection();
}
- void SettingController::openShortcutDialog()
+ void
+ SettingController::openShortcutDialog()
{
emit openShortcut();
}
- void SettingController::selectTCP(int index)
+ void
+ SettingController::selectTCP(int index)
{
/*
* If index is valid kinematic chain, enable widgets and send signal,
@@ -124,8 +149,7 @@ namespace armarx
if (index > 0)
{
enableWidgets(true);
- emit changedTCP(this->guiSettingTab->getSettingTab()->
- tcpComboBox->currentText());
+ emit changedTCP(this->guiSettingTab->getSettingTab()->tcpComboBox->currentText());
}
else
{
@@ -133,7 +157,8 @@ namespace armarx
}
}
- void SettingController::selectTCP(QString tcp)
+ void
+ SettingController::selectTCP(QString tcp)
{
QComboBox* combobox = guiSettingTab->getSettingTab()->tcpComboBox;
int index = combobox->findText(tcp);
@@ -145,7 +170,8 @@ namespace armarx
}
}
- void SettingController::selectActiveCM(int index)
+ void
+ SettingController::selectActiveCM(int index)
{
QListWidget* cmList = this->guiSettingTab->getSettingTab()->collisionModelList;
if (index > 0)
@@ -169,10 +195,12 @@ namespace armarx
{
cmList->setEnabled(false);
}
- emit setActiveColModelName(guiSettingTab->getSettingTab()->collisionModelComboBox->itemText(index));
+ emit setActiveColModelName(
+ guiSettingTab->getSettingTab()->collisionModelComboBox->itemText(index));
}
- void SettingController::setCollisionModelList(QListWidgetItem* item)
+ void
+ SettingController::setCollisionModelList(QListWidgetItem* item)
{
if (item != NULL)
{
@@ -180,8 +208,7 @@ namespace armarx
QComboBox* cm = guiSettingTab->getSettingTab()->collisionModelComboBox;
// Check if tcp is selected and active collision model is selected
- if ((tcp->currentIndex() > 0)
- && (cm->currentIndex() > 0))
+ if ((tcp->currentIndex() > 0) && (cm->currentIndex() > 0))
{
QListWidget* models = guiSettingTab->getSettingTab()->collisionModelList;
QStringList bodyCollisionModels;
@@ -199,22 +226,26 @@ namespace armarx
}
}
- void SettingController::exportTrajectorySlot()
+ void
+ SettingController::exportTrajectorySlot()
{
emit exportTrajectory();
}
- void SettingController::convertToMMMSlot()
+ void
+ SettingController::convertToMMMSlot()
{
emit convertToMMM();
}
- void SettingController::openImportDialog()
+ void
+ SettingController::openImportDialog()
{
emit openImport();
}
- void SettingController::setLanguage(int index)
+ void
+ SettingController::setLanguage(int index)
{
/*
QVariant language = guiSettingTab->getSettingTab()->languageComboBox->
@@ -226,18 +257,21 @@ namespace armarx
*/
}
- void SettingController::enableIKSolutionButton(bool enable)
+ void
+ SettingController::enableIKSolutionButton(bool enable)
{
this->guiSettingTab->getSettingTab()->newIKSolutionButton->setEnabled(enable);
}
- void SettingController::enableExportButtons(bool enable)
+ void
+ SettingController::enableExportButtons(bool enable)
{
this->guiSettingTab->getSettingTab()->convertToMMMButton->setEnabled(enable);
//this->guiSettingTab->getSettingTab()->exportTrajectoryButton->setEnabled(enable);
}
- void SettingController::enableImportTCPCollision(bool enable)
+ void
+ SettingController::enableImportTCPCollision(bool enable)
{
this->guiSettingTab->getSettingTab()->collisionModelComboBox->setEnabled(enable);
this->guiSettingTab->getSettingTab()->collisionModelList->setEnabled(enable);
@@ -245,29 +279,38 @@ namespace armarx
this->guiSettingTab->getSettingTab()->importButton->setEnabled(enable);
}
- void SettingController::newIKSolution()
+ void
+ SettingController::newIKSolution()
{
VirtualRobot::RobotPtr robot = environment->getRobot();
KinematicSolverPtr kc = KinematicSolver::getInstance(NULL, robot);
- VirtualRobot::RobotNodeSetPtr activeKinematicChain = robot->getRobotNodeSet(guiSettingTab->getSettingTab()->tcpComboBox->currentText().toStdString());
- std::vector<double> newJA = kc->solveIK(activeKinematicChain, PoseBasePtr(new Pose(activeKinematicChain->getTCP()->getGlobalPose())), VirtualRobot::IKSolver::CartesianSelection::All, 50);
+ VirtualRobot::RobotNodeSetPtr activeKinematicChain = robot->getRobotNodeSet(
+ guiSettingTab->getSettingTab()->tcpComboBox->currentText().toStdString());
+ std::vector<double> newJA =
+ kc->solveIK(activeKinematicChain,
+ PoseBasePtr(new Pose(activeKinematicChain->getTCP()->getGlobalPose())),
+ VirtualRobot::IKSolver::CartesianSelection::All,
+ 50);
if (newJA.size() != 0)
{
activeKinematicChain->setJointValues(std::vector<float>(newJA.begin(), newJA.end()));
}
}
- void SettingController::retranslateGui()
+ void
+ SettingController::retranslateGui()
{
- throw ("not yet implemented");
+ throw("not yet implemented");
}
- void SettingController::enableSelectRobotButton(bool enable)
+ void
+ SettingController::enableSelectRobotButton(bool enable)
{
guiSettingTab->getSettingTab()->robotSelectionButton->setEnabled(enable);
}
- void SettingController::environmentChanged(EnvironmentPtr environment)
+ void
+ SettingController::environmentChanged(EnvironmentPtr environment)
{
guiSettingTab->getSettingTab()->tcpComboBox->clear();
guiSettingTab->getSettingTab()->collisionModelComboBox->clear();
@@ -280,11 +323,11 @@ namespace armarx
initCMList(robot);
}
-
/************************************************************************************/
/* Private functions */
/************************************************************************************/
- void SettingController::enableWidgets(bool enable)
+ void
+ SettingController::enableWidgets(bool enable)
{
this->guiSettingTab->getSettingTab()->collisionModelComboBox->setEnabled(enable);
this->guiSettingTab->getSettingTab()->collisionModelList->setEnabled(enable);
@@ -292,7 +335,8 @@ namespace armarx
//this->guiSettingTab->getSettingTab()->importButton->setEnabled(enable);
}
- void SettingController::initTCPComboBox(VirtualRobot::RobotPtr robot)
+ void
+ SettingController::initTCPComboBox(VirtualRobot::RobotPtr robot)
{
QComboBox* tcpComboBox = this->guiSettingTab->getSettingTab()->tcpComboBox;
@@ -328,10 +372,10 @@ namespace armarx
}
}
- void SettingController::initCMComboBox(VirtualRobot::RobotPtr robot)
+ void
+ SettingController::initCMComboBox(VirtualRobot::RobotPtr robot)
{
- QComboBox* cmComboBox = this->guiSettingTab->getSettingTab()->
- collisionModelComboBox;
+ QComboBox* cmComboBox = this->guiSettingTab->getSettingTab()->collisionModelComboBox;
// Set strong focus and add wheel event filter
cmComboBox->setFocusPolicy(Qt::StrongFocus);
@@ -364,7 +408,8 @@ namespace armarx
}
}
- void SettingController::initCMList(VirtualRobot::RobotPtr robot)
+ void
+ SettingController::initCMList(VirtualRobot::RobotPtr robot)
{
QListWidget* cmList = this->guiSettingTab->getSettingTab()->collisionModelList;
if (robot)
@@ -377,7 +422,8 @@ namespace armarx
{
if (s->isKinematicChain())
{
- QListWidgetItem* item = new QListWidgetItem(QString::fromStdString(s->getName()));
+ QListWidgetItem* item =
+ new QListWidgetItem(QString::fromStdString(s->getName()));
item->setFlags(item->flags() | Qt::ItemIsUserCheckable);
item->setCheckState(Qt::Unchecked);
ARMARX_DEBUG << "Add item " << s->getName() << " to collision model list";
@@ -392,4 +438,4 @@ namespace armarx
}
}
-}
+} // namespace armarx
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/SettingController.h b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/SettingController.h
index 2aa876deb13a14901d242b8e0cd51daa17e6d0d3..226157a2e57334d71fa105d00a75022773c0f3dd 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/SettingController.h
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/SettingController.h
@@ -21,16 +21,17 @@
*/
#ifndef SETTINGCONTROLLER_H
#define SETTINGCONTROLLER_H
-#include "AbstractController.h"
-#include "../View/SettingTab.h"
-#include "../Environment.h"
-#include "VirtualRobot/RobotNodeSet.h"
-#include "../Util/WheelEventFilter.h"
-
#include <memory>
+
#include <QComboBox>
#include <QVariant>
+#include "../Environment.h"
+#include "../Util/WheelEventFilter.h"
+#include "../View/SettingTab.h"
+#include "AbstractController.h"
+#include "VirtualRobot/RobotNodeSet.h"
+
namespace armarx
{
/**
@@ -288,7 +289,7 @@ namespace armarx
};
using SettingControllerPtr = std::shared_ptr<SettingController>;
-}
+} // namespace armarx
#endif // SETTINGCONTROLLER_H
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/ShortcutController.cpp b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/ShortcutController.cpp
index fc1d80d1daea73ac867b6829a63b3093945aeff1..981025a72ec84818eae810263e4f9c72a2c9bc5f 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/ShortcutController.cpp
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/ShortcutController.cpp
@@ -1,10 +1,13 @@
#include "ShortcutController.h"
+
#include "qapplication.h"
+
namespace armarx
{
- void ShortcutController::onInitComponent()
+ void
+ ShortcutController::onInitComponent()
{
- deactivateSetWaypoint = new QShortcut(QKeySequence(Qt::LeftButton), this->parent);
+ deactivateSetWaypoint = new QShortcut(QKeySequence(Qt::LeftButton), this->parent);
setWaypoint = new QShortcut(QKeySequence(Qt::CTRL + Qt::Key_S), this->parent);
deleteWaypoint = new QShortcut(QKeySequence(Qt::CTRL + Qt::Key_D), this->parent);
changeWaypointShortcut = new QShortcut(QKeySequence(Qt::CTRL + Qt::Key_W), this->parent);
@@ -30,7 +33,8 @@ namespace armarx
ARMARX_INFO << "RobotTrajectoryDesigner: ShortcutController on init";
}
- void ShortcutController::onConnectComponent()
+ void
+ ShortcutController::onConnectComponent()
{
//Connects Gui
QObject::connect(ui.okButton, SIGNAL(clicked()), shortcutDialog, SLOT(accept()));
@@ -38,27 +42,38 @@ namespace armarx
QObject::connect(deactivateSetWaypoint, SIGNAL(activated()), this, SLOT(disableSet()));
QObject::connect(setWaypoint, SIGNAL(activated()), this, SLOT(addedWaypointSlot()));
QObject::connect(deleteWaypoint, SIGNAL(activated()), this, SLOT(deletedWaypointSlot()));
- QObject::connect(changeWaypointShortcut, SIGNAL(activated()), this, SLOT(changeWaypointSlot()));
+ QObject::connect(
+ changeWaypointShortcut, SIGNAL(activated()), this, SLOT(changeWaypointSlot()));
QObject::connect(playPreview, SIGNAL(activated()), this, SLOT(playPreviewSlot()));
QObject::connect(playPreviewAll, SIGNAL(activated()), this, SLOT(playPreviewAllSlot()));
QObject::connect(stopPreview, SIGNAL(activated()), this, SLOT(stopPreviewSlot()));
- QObject::connect(setPerspectiveTop, SIGNAL(activated()), this, SLOT(changedPerspectiveTopSlot()));
- QObject::connect(setPerspectiveFront, SIGNAL(activated()), this, SLOT(changedPerspectiveFrontSlot()));
- QObject::connect(setPerspectiveBack, SIGNAL(activated()), this, SLOT(changedPerspectiveBackSlot()));
- QObject::connect(setPerspectiveLeft, SIGNAL(activated()), this, SLOT(changedPerspectiveLeftSlot()));
- QObject::connect(setPerspectiveRight, SIGNAL(activated()), this, SLOT(changedPerspectiveRightSlot()));
- QObject::connect(setPerspectiveHighAngle, SIGNAL(activated()), this, SLOT(changedPerspectiveHighAngleSlot()));
+ QObject::connect(
+ setPerspectiveTop, SIGNAL(activated()), this, SLOT(changedPerspectiveTopSlot()));
+ QObject::connect(
+ setPerspectiveFront, SIGNAL(activated()), this, SLOT(changedPerspectiveFrontSlot()));
+ QObject::connect(
+ setPerspectiveBack, SIGNAL(activated()), this, SLOT(changedPerspectiveBackSlot()));
+ QObject::connect(
+ setPerspectiveLeft, SIGNAL(activated()), this, SLOT(changedPerspectiveLeftSlot()));
+ QObject::connect(
+ setPerspectiveRight, SIGNAL(activated()), this, SLOT(changedPerspectiveRightSlot()));
+ QObject::connect(setPerspectiveHighAngle,
+ SIGNAL(activated()),
+ this,
+ SLOT(changedPerspectiveHighAngleSlot()));
QObject::connect(undoShortcut, SIGNAL(activated()), this, SLOT(undoOperation()));
QObject::connect(redoShortcut, SIGNAL(activated()), this, SLOT(redoOperation()));
ARMARX_INFO << "RobotTrajectoryDesigner: ShortcutController on connect";
}
- void ShortcutController::onDisconnectComponent()
+ void
+ ShortcutController::onDisconnectComponent()
{
ARMARX_INFO << "RobotTrajectoryDesigner: ShortcutController on disconnect";
}
- void ShortcutController::onExitComponent()
+ void
+ ShortcutController::onExitComponent()
{
ARMARX_INFO << "RobotTrajectoryDesigner: ShortcutController on exit";
}
@@ -71,12 +86,14 @@ namespace armarx
onConnectComponent();
}
- int ShortcutController::getCurrentWaypoint()
+ int
+ ShortcutController::getCurrentWaypoint()
{
return currentWaypoint;
}
- void ShortcutController::setCurrentWaypoint(int currentWaypoint)
+ void
+ ShortcutController::setCurrentWaypoint(int currentWaypoint)
{
if (currentWaypoint >= 0)
{
@@ -84,49 +101,58 @@ namespace armarx
}
}
- void ShortcutController::open()
+ void
+ ShortcutController::open()
{
shortcutDialog->setModal(true);
shortcutDialog->exec();
}
- void ShortcutController::enablePreviewAllShortcut(bool enable)
+ void
+ ShortcutController::enablePreviewAllShortcut(bool enable)
{
playPreviewAll->setEnabled(enable);
}
- void ShortcutController::enableStopPreviewShortcut(bool enable)
+ void
+ ShortcutController::enableStopPreviewShortcut(bool enable)
{
stopPreview->setEnabled(enable);
}
- void ShortcutController::enableRedoShortcut(bool enable)
+ void
+ ShortcutController::enableRedoShortcut(bool enable)
{
redoShortcut->setEnabled(enable);
}
- void ShortcutController::enableUndoShortcut(bool enable)
+ void
+ ShortcutController::enableUndoShortcut(bool enable)
{
undoShortcut->setEnabled(enable);
}
- void ShortcutController::enableDeleteChangeShortcut(bool enable)
+ void
+ ShortcutController::enableDeleteChangeShortcut(bool enable)
{
deleteWaypoint->setEnabled(enable);
changeWaypointShortcut->setEnabled(enable);
}
- void ShortcutController::enableAddShortcut(bool enable)
+ void
+ ShortcutController::enableAddShortcut(bool enable)
{
setWaypoint->setEnabled(enable);
}
- void ShortcutController::enablePreviewShortcut(bool enable)
+ void
+ ShortcutController::enablePreviewShortcut(bool enable)
{
playPreview->setEnabled(enable);
}
- void ShortcutController::addedWaypointSlot()
+ void
+ ShortcutController::addedWaypointSlot()
{
//cannot add a waypoint while mouse is held, as programm crashes when manipulator is moved while adding waypoints
if (QApplication::mouseButtons() == Qt::NoButton)
@@ -139,78 +165,93 @@ namespace armarx
}
}
- void ShortcutController::deletedWaypointSlot()
+ void
+ ShortcutController::deletedWaypointSlot()
{
emit deletedWaypoint(currentWaypoint);
}
- void ShortcutController::changeWaypointSlot()
+ void
+ ShortcutController::changeWaypointSlot()
{
emit changeWaypoint(currentWaypoint);
}
- void ShortcutController::playPreviewSlot()
+ void
+ ShortcutController::playPreviewSlot()
{
emit playPreviewSignal();
}
- void ShortcutController::playPreviewAllSlot()
+ void
+ ShortcutController::playPreviewAllSlot()
{
emit playPreviewAllSignal();
}
- void ShortcutController::stopPreviewSlot()
+ void
+ ShortcutController::stopPreviewSlot()
{
emit stopPreviewSignal();
}
- void ShortcutController::changedPerspectiveHighAngleSlot()
+ void
+ ShortcutController::changedPerspectiveHighAngleSlot()
{
emit changedPerspective(0);
}
- void ShortcutController::changedPerspectiveTopSlot()
+ void
+ ShortcutController::changedPerspectiveTopSlot()
{
emit changedPerspective(1);
}
- void ShortcutController::changedPerspectiveFrontSlot()
+ void
+ ShortcutController::changedPerspectiveFrontSlot()
{
emit changedPerspective(2);
}
- void ShortcutController::changedPerspectiveBackSlot()
+ void
+ ShortcutController::changedPerspectiveBackSlot()
{
emit changedPerspective(3);
}
- void ShortcutController::changedPerspectiveLeftSlot()
+ void
+ ShortcutController::changedPerspectiveLeftSlot()
{
emit changedPerspective(4);
}
- void ShortcutController::changedPerspectiveRightSlot()
+ void
+ ShortcutController::changedPerspectiveRightSlot()
{
emit changedPerspective(5);
}
- void ShortcutController::undoOperation()
+ void
+ ShortcutController::undoOperation()
{
emit undo();
}
- void ShortcutController::redoOperation()
+ void
+ ShortcutController::redoOperation()
{
emit redo();
}
- void ShortcutController::enableSet()
+ void
+ ShortcutController::enableSet()
{
this->enableAddShortcut(true);
}
- void ShortcutController::disableSet()
+ void
+ ShortcutController::disableSet()
{
this->enableAddShortcut(false);
}
-}
+} // namespace armarx
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/ShortcutController.h b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/ShortcutController.h
index 69318ac838611f85c537b876a590568adbf38d8b..259de1391035cae6c7e82d869e5c4533cd4c67a9 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/ShortcutController.h
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/ShortcutController.h
@@ -1,12 +1,12 @@
#ifndef SHORTCUTCONTROLLER_H
#define SHORTCUTCONTROLLER_H
+#include <QKeySequence>
#include <QShortcut>
#include <QString>
-#include <QKeySequence>
-#include "AbstractController.h"
-#include "../ui_RobotTrajectoryDesignerGuiPluginWidget.h"
-#include "../View/ui_ShortcutDialog.h"
+#include "../View/ui_ShortcutDialog.h"
+#include "../ui_RobotTrajectoryDesignerGuiPluginWidget.h"
+#include "AbstractController.h"
namespace armarx
{
@@ -195,13 +195,11 @@ namespace armarx
QShortcut* redoShortcut;
Ui::ShortcutDialog ui;
QDialog* shortcutDialog;
-
-
-
};
+
using ShortcutControllerPtr = std::shared_ptr<ShortcutController>;
-}
+} // namespace armarx
#endif // SHORTCUTCONTROLLER_H
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/TCPInformationController.cpp b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/TCPInformationController.cpp
index 82dc6079345cb030d67ac13c168076a810472ec6..12d128aad1c9dd1f388ee16d64d873a4040bb62a 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/TCPInformationController.cpp
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/TCPInformationController.cpp
@@ -20,28 +20,34 @@
* GNU General Public License
*/
#include "TCPInformationController.h"
+
#include <iomanip> // setprecision
#include <sstream> // stringstream
+
namespace armarx
{
- void TCPInformationController::onInitComponent()
+ void
+ TCPInformationController::onInitComponent()
{
ARMARX_INFO << "RobotTrajectoryDesigner: TCPInformationController on init";
clearPoseLabels();
}
- void TCPInformationController::onConnectComponent()
+ void
+ TCPInformationController::onConnectComponent()
{
ARMARX_INFO << "RobotTrajectoryDesigner: TCPInformationController on connect";
}
- void TCPInformationController::onDisconnectComponent()
+ void
+ TCPInformationController::onDisconnectComponent()
{
ARMARX_INFO << "RobotTrajectoryDesigner: TCPInformationController on disconnect";
}
- void TCPInformationController::onExitComponent()
+ void
+ TCPInformationController::onExitComponent()
{
ARMARX_INFO << "RobotTrajectoryDesigner: TCPInformationController on exit";
}
@@ -53,12 +59,14 @@ namespace armarx
onConnectComponent();
}
- TCPInformationPtr TCPInformationController::getGuiTCPInformation()
+ TCPInformationPtr
+ TCPInformationController::getGuiTCPInformation()
{
return this->guiTCPInformation;
}
- void TCPInformationController::setGuiTCPInformation(TCPInformationPtr guiTCPInformation)
+ void
+ TCPInformationController::setGuiTCPInformation(TCPInformationPtr guiTCPInformation)
{
if (guiTCPInformation != NULL)
{
@@ -66,21 +74,20 @@ namespace armarx
}
}
-
- void TCPInformationController::setCurrentPose(Eigen::Matrix4f pose)
+ void
+ TCPInformationController::setCurrentPose(Eigen::Matrix4f pose)
{
- setLineEditText(guiTCPInformation->getTCPInformationTab()->currentPoseLayout,
- pose);
+ setLineEditText(guiTCPInformation->getTCPInformationTab()->currentPoseLayout, pose);
}
- void TCPInformationController::setDesiredPose(Eigen::Matrix4f pose)
+ void
+ TCPInformationController::setDesiredPose(Eigen::Matrix4f pose)
{
- setLineEditText(guiTCPInformation->getTCPInformationTab()->desiredPoseLayout,
- pose);
-
+ setLineEditText(guiTCPInformation->getTCPInformationTab()->desiredPoseLayout, pose);
}
- void TCPInformationController::setReachable(bool reachable)
+ void
+ TCPInformationController::setReachable(bool reachable)
{
std::string reachableString = "";
if (reachable)
@@ -91,24 +98,26 @@ namespace armarx
{
reachableString = "Pose NOT reachable!";
}
- guiTCPInformation->getTCPInformationTab()->reachableLabel->setText(QString::fromStdString(reachableString));
+ guiTCPInformation->getTCPInformationTab()->reachableLabel->setText(
+ QString::fromStdString(reachableString));
}
- void TCPInformationController::clearPoseLabels()
+ void
+ TCPInformationController::clearPoseLabels()
{
Eigen::Matrix4f zero;
- setLineEditText(guiTCPInformation->getTCPInformationTab()->currentPoseLayout,
- zero);
- setLineEditText(guiTCPInformation->getTCPInformationTab()->desiredPoseLayout,
- zero);
+ setLineEditText(guiTCPInformation->getTCPInformationTab()->currentPoseLayout, zero);
+ setLineEditText(guiTCPInformation->getTCPInformationTab()->desiredPoseLayout, zero);
}
- void TCPInformationController::retranslateGui()
+ void
+ TCPInformationController::retranslateGui()
{
- throw ("not yet implemented");
+ throw("not yet implemented");
}
- void TCPInformationController::setLineEditText(QGridLayout* gridLayout, Eigen::Matrix4f pose)
+ void
+ TCPInformationController::setLineEditText(QGridLayout* gridLayout, Eigen::Matrix4f pose)
{
int columns = gridLayout->columnCount();
int rows = gridLayout->rowCount();
@@ -142,4 +151,4 @@ namespace armarx
}
}
-}
+} // namespace armarx
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/TCPInformationController.h b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/TCPInformationController.h
index 2aa3dc706850644c9d613de32b15ba2f6df46207..75b77ef8b0bec5a0cff3e43a9999a2cb18ca5c86 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/TCPInformationController.h
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/TCPInformationController.h
@@ -21,14 +21,16 @@
*/
#ifndef TCPINFORMATIONCONTROLLER_H
#define TCPINFORMATIONCONTROLLER_H
-#include "AbstractController.h"
-#include "../View/TCPInformationTab.h"
#include <memory>
-#include <Eigen/Core>
#include <QLayoutItem>
#include <QLineEdit>
+#include <Eigen/Core>
+
+#include "../View/TCPInformationTab.h"
+#include "AbstractController.h"
+
namespace armarx
{
@@ -128,7 +130,7 @@ namespace armarx
};
using TCPInformationControllerPtr = std::shared_ptr<TCPInformationController>;
-}
+} // namespace armarx
#endif // TCPINFORMATIONCONTROLLER_H
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/TCPSelectionController.cpp b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/TCPSelectionController.cpp
index f96e7f906b862bf115331c85e148ced4f2298a70..edffd598b09c5d49a8a9d79c535f065c74fc63b6 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/TCPSelectionController.cpp
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/TCPSelectionController.cpp
@@ -20,11 +20,13 @@
* GNU General Public License
*/
#include "TCPSelectionController.h"
+
#include "../Environment.h"
namespace armarx
{
- void TCPSelectionController::onInitComponent()
+ void
+ TCPSelectionController::onInitComponent()
{
ARMARX_INFO << "RobotTrajectoryDesigner: TCPSelectionController on init";
@@ -36,21 +38,24 @@ namespace armarx
currentTCPCombobox->installEventFilter(new WheelEventFilter(this));
}
- void TCPSelectionController::onConnectComponent()
+ void
+ TCPSelectionController::onConnectComponent()
{
ARMARX_INFO << "RobotTrajectoryDesigner: TCPSelectionController on connect";
// Select trajectory: update selected trajectory
- QObject::connect(currentTCPCombobox, SIGNAL(activated(int)),
- this, SLOT(updateSelectedTCP(int)));
+ QObject::connect(
+ currentTCPCombobox, SIGNAL(activated(int)), this, SLOT(updateSelectedTCP(int)));
}
- void TCPSelectionController::onDisconnectComponent()
+ void
+ TCPSelectionController::onDisconnectComponent()
{
ARMARX_INFO << "RobotTrajectoryDesigner: TCPSelectionController on disconnect";
}
- void TCPSelectionController::onExitComponent()
+ void
+ TCPSelectionController::onExitComponent()
{
ARMARX_INFO << "RobotTrajectoryDesigner: TCPSelectionController on exit";
}
@@ -62,12 +67,14 @@ namespace armarx
onConnectComponent();
}
- QComboBox* TCPSelectionController::getCurrentTCPCombobox()
+ QComboBox*
+ TCPSelectionController::getCurrentTCPCombobox()
{
return this->currentTCPCombobox;
}
- void TCPSelectionController::setCurrentTCPCombobox(QComboBox* currentTCPCombobox)
+ void
+ TCPSelectionController::setCurrentTCPCombobox(QComboBox* currentTCPCombobox)
{
if (currentTCPCombobox != NULL)
{
@@ -75,7 +82,8 @@ namespace armarx
}
}
- void TCPSelectionController::updateSelectedTCP(int index)
+ void
+ TCPSelectionController::updateSelectedTCP(int index)
{
if (index >= 0)
{
@@ -83,7 +91,8 @@ namespace armarx
}
}
- void TCPSelectionController::updateSelectedTCP(QString trajectory)
+ void
+ TCPSelectionController::updateSelectedTCP(QString trajectory)
{
int index = currentTCPCombobox->findText(trajectory);
if (index != -1)
@@ -93,7 +102,8 @@ namespace armarx
}
}
- void TCPSelectionController::addTrajectory(QString trajectory)
+ void
+ TCPSelectionController::addTrajectory(QString trajectory)
{
if (trajectory != NULL)
{
@@ -108,7 +118,8 @@ namespace armarx
}
}
- void TCPSelectionController::removeTrajectory(QString trajectory)
+ void
+ TCPSelectionController::removeTrajectory(QString trajectory)
{
int index = currentTCPCombobox->findText(trajectory);
@@ -122,14 +133,16 @@ namespace armarx
}
}
- void TCPSelectionController::enableSelectedTCP(bool enable)
+ void
+ TCPSelectionController::enableSelectedTCP(bool enable)
{
this->currentTCPCombobox->setEnabled(enable);
}
- void TCPSelectionController::removeAllTrajectories()
+ void
+ TCPSelectionController::removeAllTrajectories()
{
currentTCPCombobox->clear();
currentTCPCombobox->setEnabled(false);
}
-}
+} // namespace armarx
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/TCPSelectionController.h b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/TCPSelectionController.h
index 8c76cd6040fa9892606512088f9a01dac34ddc7c..6f613bdb424e82817bdd4db526a61c9091be4a45 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/TCPSelectionController.h
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/TCPSelectionController.h
@@ -21,11 +21,11 @@
*/
#ifndef TCPSELECTIONCONTROLLER_H
#define TCPSELECTIONCONTROLLER_H
-#include "AbstractController.h"
-#include "../Util/WheelEventFilter.h"
-
#include <QComboBox>
+#include "../Util/WheelEventFilter.h"
+#include "AbstractController.h"
+
namespace armarx
{
/**
@@ -39,7 +39,6 @@ namespace armarx
Q_OBJECT
public:
-
/**
* @brief @see AbstractController
*/
@@ -130,6 +129,6 @@ namespace armarx
};
using TCPSelectionControllerPtr = std::shared_ptr<TCPSelectionController>;
-}
+} // namespace armarx
#endif // TCPSELECTIONCONTROLLER_H
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/Test/ExportDialogControllerTest.cpp b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/Test/ExportDialogControllerTest.cpp
index ef25bb5b2f9a76cb541dd2b739ff391569f41ed4..90f604b237a3fcc19e50eb46a78b24a40ee1505b 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/Test/ExportDialogControllerTest.cpp
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/Test/ExportDialogControllerTest.cpp
@@ -1,13 +1,15 @@
#define BOOST_TEST_MODULE ArmarX::RobotTrajectoryDesigner::Controller::Exportdialogcontroller
#define ARMARX_BOOST_TEST
-#include "../../../../../build/testing/RobotTrajectoryDesigner/Test.h"
-
#include "../ExportDialogController.h"
-#include "../../View/ExportDialog.h"
+
+#include <iostream>
#include <memory>
+
#include <VirtualRobot/XML/RobotIO.h>
-#include <iostream>
+
+#include "../../../../../build/testing/RobotTrajectoryDesigner/Test.h"
+#include "../../View/ExportDialog.h"
using namespace armarx;
using namespace VirtualRobot;
@@ -18,8 +20,8 @@ struct F
F()
{
BOOST_TEST_MESSAGE("Setup fixture with export dialog and controller");
-
}
+
~F()
{
BOOST_TEST_MESSAGE("Teardown fixture");
@@ -31,5 +33,4 @@ struct F
BOOST_FIXTURE_TEST_CASE(ExportDialogControllerTest, F)
{
-
}
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/Test/ImportDialogControllerTest.cpp b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/Test/ImportDialogControllerTest.cpp
index edd0f4aaa0bb4e7b514b1b11bcdeabad507f1d1f..e496819713c839a35a2461310f92c00b17881413 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/Test/ImportDialogControllerTest.cpp
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/Test/ImportDialogControllerTest.cpp
@@ -1,12 +1,14 @@
#define BOOST_TEST_MODULE ArmarX::RobotTrajectoryDesigner::Controller::Importdialogcontroller
#define ARMARX_BOOST_TEST
-#include "../../../../../build/testing/RobotTrajectoryDesigner/Test.h"
-
#include "../ImportDialogController.h"
+
+#include <iostream>
#include <memory>
+
#include <VirtualRobot/XML/RobotIO.h>
-#include <iostream>
+
+#include "../../../../../build/testing/RobotTrajectoryDesigner/Test.h"
using namespace armarx;
using namespace VirtualRobot;
@@ -17,8 +19,8 @@ struct F
F()
{
BOOST_TEST_MESSAGE("Setup fixture with import dialog and controller");
-
}
+
~F()
{
BOOST_TEST_MESSAGE("Teardown fixture");
@@ -29,5 +31,4 @@ struct F
BOOST_FIXTURE_TEST_CASE(ImportDialogControllerTest, F)
{
-
}
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/Test/RobotVisualizationControllerTest.cpp b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/Test/RobotVisualizationControllerTest.cpp
index c6e61171e2f17fb674eed22394ddd5b7a92f6ba2..c3b1955e7c3e513224417174d5577578fc6bfcbd 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/Test/RobotVisualizationControllerTest.cpp
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/Test/RobotVisualizationControllerTest.cpp
@@ -1,12 +1,14 @@
#define BOOST_TEST_MODULE ArmarX::RobotTrajectoryDesigner::Controller::Robotvisualizationcontroller
#define ARMARX_BOOST_TEST
-#include "../../../../../build/testing/RobotTrajectoryDesigner/Test.h"
-
#include "../RobotVisualizationController.h"
+
+#include <iostream>
#include <memory>
+
#include <VirtualRobot/XML/RobotIO.h>
-#include <iostream>
+
+#include "../../../../../build/testing/RobotTrajectoryDesigner/Test.h"
using namespace armarx;
using namespace VirtualRobot;
@@ -17,8 +19,8 @@ struct F
F()
{
BOOST_TEST_MESSAGE("Setup fixture with export dialog and controller");
-
}
+
~F()
{
BOOST_TEST_MESSAGE("Teardown fixture");
@@ -29,5 +31,4 @@ struct F
BOOST_FIXTURE_TEST_CASE(RobotVisualizationControllerTest, F)
{
-
}
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/Test/SettingControllerTest.cpp b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/Test/SettingControllerTest.cpp
index 17a2c1345644cfd64f94256615514678519e2f45..0e6e80f40a232e2b78a2689a4a4931844552fde3 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/Test/SettingControllerTest.cpp
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/Test/SettingControllerTest.cpp
@@ -1,18 +1,21 @@
#define BOOST_TEST_MODULE ArmarX::RobotTrajectoryDesigner::Controller::SettingController
#define ARMARX_BOOST_TEST
-#include "../../../../../build/testing/RobotTrajectoryDesigner/Test.h"
-
#include "../SettingController.h"
-#include "ui_SettingTab.h"
+
+#include <iostream>
+#include <memory>
+
#include <QApplication>
-#include <QWidget>
#include <QComboBox>
#include <QMainWindow>
-#include "../View/SettingTab.h"
-#include <memory>
+#include <QWidget>
+
#include <VirtualRobot/XML/RobotIO.h>
-#include <iostream>
+
+#include "../../../../../build/testing/RobotTrajectoryDesigner/Test.h"
+#include "../View/SettingTab.h"
+#include "ui_SettingTab.h"
using namespace armarx;
using namespace VirtualRobot;
@@ -28,6 +31,7 @@ struct F
//settingController = make_shared<SettingController>(settingTab);
//tcp = settingTab->getSettingTab()->tcpComboBox;
}
+
~F()
{
BOOST_TEST_MESSAGE("Teardown fixture");
@@ -41,8 +45,8 @@ struct F
BOOST_FIXTURE_TEST_CASE(initTCPComboBoxTest, F)
{
- RobotPtr robot = RobotIO::loadRobot(
- "../../../../../data/RobotTrajectoryDesigner/Resources/ArmarIII.xml");
+ RobotPtr robot =
+ RobotIO::loadRobot("../../../../../data/RobotTrajectoryDesigner/Resources/ArmarIII.xml");
Environment* environment = new Environment();
environment->setRobot(robot);
// Environment::setRobot(robot);
@@ -69,4 +73,3 @@ BOOST_FIXTURE_TEST_CASE(initTCPComboBoxTest, F)
// BOOST_CHECK(rns->contains(QString::fromStdString("PlatformYawTorsoLeftArm")));
// BOOST_CHECK(rns->contains(QString::fromStdString("PlatformTorsoRightArm")));
}
-
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/Test/TCPInformationControllerTest.cpp b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/Test/TCPInformationControllerTest.cpp
index 1c16f5e6d3139e6d7b526bc2bd2d5d137f43534e..014e93913209354f9a0bf5f4a3325d90a8043cfc 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/Test/TCPInformationControllerTest.cpp
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/Test/TCPInformationControllerTest.cpp
@@ -1,13 +1,15 @@
#define BOOST_TEST_MODULE ArmarX::RobotTrajectoryDesigner::Controller::TCPInformationcontroller
#define ARMARX_BOOST_TEST
-#include "../../../../../build/testing/RobotTrajectoryDesigner/Test.h"
-
#include "../TCPInformationController.h"
-#include "../../View/TCPInformationTab.h"
+
+#include <iostream>
#include <memory>
+
#include <VirtualRobot/XML/RobotIO.h>
-#include <iostream>
+
+#include "../../../../../build/testing/RobotTrajectoryDesigner/Test.h"
+#include "../../View/TCPInformationTab.h"
using namespace armarx;
using namespace VirtualRobot;
@@ -18,8 +20,8 @@ struct F
F()
{
BOOST_TEST_MESSAGE("Setup fixture with TCP information tab and controller");
-
}
+
~F()
{
BOOST_TEST_MESSAGE("Teardown fixture");
@@ -31,5 +33,4 @@ struct F
BOOST_FIXTURE_TEST_CASE(TCPInformationControllerTest, F)
{
-
}
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/Test/TCPSelectionControllerTest.cpp b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/Test/TCPSelectionControllerTest.cpp
index 56b1c0f3208de174a375711d0c0522031aed479d..c5ef52ac689876e5a80abd9d082e17d987e7222b 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/Test/TCPSelectionControllerTest.cpp
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/Test/TCPSelectionControllerTest.cpp
@@ -1,12 +1,14 @@
#define BOOST_TEST_MODULE ArmarX::RobotTrajectoryDesigner::Controller::TCPSelectioncontroller
#define ARMARX_BOOST_TEST
-#include "../../../../../build/testing/RobotTrajectoryDesigner/Test.h"
-
#include "../TCPSelectionController.h"
+
+#include <iostream>
#include <memory>
+
#include <VirtualRobot/XML/RobotIO.h>
-#include <iostream>
+
+#include "../../../../../build/testing/RobotTrajectoryDesigner/Test.h"
using namespace armarx;
using namespace VirtualRobot;
@@ -17,8 +19,8 @@ struct F
F()
{
BOOST_TEST_MESSAGE("Setup fixture with TCP selection tab and controller");
-
}
+
~F()
{
BOOST_TEST_MESSAGE("Teardown fixture");
@@ -29,5 +31,4 @@ struct F
BOOST_FIXTURE_TEST_CASE(TCPSelectionControllerTest, F)
{
-
}
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/Test/TrajectoryControllerTest.cpp b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/Test/TrajectoryControllerTest.cpp
index 330d4e26fffb0c73c00caf6087de050b13843ae7..f12d7ea8d31a14eb85fcbce5f80455e63046d7c6 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/Test/TrajectoryControllerTest.cpp
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/Test/TrajectoryControllerTest.cpp
@@ -1,12 +1,14 @@
#define BOOST_TEST_MODULE ArmarX::RobotTrajectoryDesigner::Controller::Trajectorycontroller
#define ARMARX_BOOST_TEST
-#include "../../../../../build/testing/RobotTrajectoryDesigner/Test.h"
-
#include "../TrajectoryController.h"
+
+#include <iostream>
#include <memory>
+
#include <VirtualRobot/XML/RobotIO.h>
-#include <iostream>
+
+#include "../../../../../build/testing/RobotTrajectoryDesigner/Test.h"
using namespace armarx;
using namespace VirtualRobot;
@@ -17,8 +19,8 @@ struct F
F()
{
BOOST_TEST_MESSAGE("Setup fixture with trajectory and controller");
-
}
+
~F()
{
BOOST_TEST_MESSAGE("Teardown fixture");
@@ -29,5 +31,4 @@ struct F
BOOST_FIXTURE_TEST_CASE(TrajectoryControllerTest, F)
{
-
}
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/Test/TransitionControllerTest.cpp b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/Test/TransitionControllerTest.cpp
index dc15154a6b3e20a443e8f42d9c04c732dd52cc36..359fc475c2114cf48d74f61b81b96b116bb02d97 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/Test/TransitionControllerTest.cpp
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/Test/TransitionControllerTest.cpp
@@ -1,13 +1,15 @@
#define BOOST_TEST_MODULE ArmarX::RobotTrajectoryDesigner::Controller::Transitioncontroller
#define ARMARX_BOOST_TEST
-#include "../../../../../build/testing/RobotTrajectoryDesigner/Test.h"
-
#include "../TransitionController.h"
-#include "../../View/TransitionTab.h"
+
+#include <iostream>
#include <memory>
+
#include <VirtualRobot/XML/RobotIO.h>
-#include <iostream>
+
+#include "../../../../../build/testing/RobotTrajectoryDesigner/Test.h"
+#include "../../View/TransitionTab.h"
using namespace armarx;
using namespace VirtualRobot;
@@ -18,8 +20,8 @@ struct F
F()
{
BOOST_TEST_MESSAGE("Setup fixture with transition tab and controller");
-
}
+
~F()
{
BOOST_TEST_MESSAGE("Teardown fixture");
@@ -32,5 +34,4 @@ struct F
BOOST_FIXTURE_TEST_CASE(TransitionControllerTest, F)
{
-
}
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/Test/WaypointControllerTest.cpp b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/Test/WaypointControllerTest.cpp
index e6e0e7b7dfe3f2b471662994c60d0a5779b5a163..92016425f23b9a990edf1b28c3f4375b4632665e 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/Test/WaypointControllerTest.cpp
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/Test/WaypointControllerTest.cpp
@@ -1,13 +1,15 @@
#define BOOST_TEST_MODULE ArmarX::RobotTrajectoryDesigner::Controller::Waypointcontroller
#define ARMARX_BOOST_TEST
-#include "../../../../../build/testing/RobotTrajectoryDesigner/Test.h"
-
#include "../WaypointController.h"
-#include "../../View/WaypointTab.h"
+
+#include <iostream>
#include <memory>
+
#include <VirtualRobot/XML/RobotIO.h>
-#include <iostream>
+
+#include "../../../../../build/testing/RobotTrajectoryDesigner/Test.h"
+#include "../../View/WaypointTab.h"
using namespace armarx;
using namespace VirtualRobot;
@@ -18,8 +20,8 @@ struct F
F()
{
BOOST_TEST_MESSAGE("Setup fixture with waypoint tab and controller");
-
}
+
~F()
{
BOOST_TEST_MESSAGE("Teardown fixture");
@@ -32,5 +34,4 @@ struct F
BOOST_FIXTURE_TEST_CASE(WaypointControllerTest, F)
{
-
}
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/ToolBarController.cpp b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/ToolBarController.cpp
index 3462c4bee8c76f56e44a2db22f9861acb5c5baab..bddb35b4f6a91c2cdb1af86a2958df0bcd68828b 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/ToolBarController.cpp
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/ToolBarController.cpp
@@ -23,7 +23,8 @@
namespace armarx
{
- void ToolBarController::onInitComponent()
+ void
+ ToolBarController::onInitComponent()
{
ARMARX_INFO << "RobotTrajectoryDesigner: ToolBarController on init";
enableDeleteChangeButton(false);
@@ -33,70 +34,87 @@ namespace armarx
enableStopButton(false);
}
- void ToolBarController::onConnectComponent()
+ void
+ ToolBarController::onConnectComponent()
{
ARMARX_INFO << "RobotTrajectoryDesigner: ToolBarController on connect";
// Set waypoint: clicked
- QObject::connect(guiToolbar.get()->getToolBar()->setWaypointButton, SIGNAL(clicked()),
- this, SLOT(setWaypoint()));
+ QObject::connect(guiToolbar.get()->getToolBar()->setWaypointButton,
+ SIGNAL(clicked()),
+ this,
+ SLOT(setWaypoint()));
// Delete waypoint: clicked
- QObject::connect(guiToolbar.get()->getToolBar()->deleteWaypointButton, SIGNAL(clicked()),
- this, SLOT(deleteWaypoint()));
+ QObject::connect(guiToolbar.get()->getToolBar()->deleteWaypointButton,
+ SIGNAL(clicked()),
+ this,
+ SLOT(deleteWaypoint()));
// Change Waypoint: clicked
- QObject::connect(guiToolbar.get()->getToolBar()->changeWaypointButton, SIGNAL(clicked()),
- this, SLOT(changeWaypoint()));
+ QObject::connect(guiToolbar.get()->getToolBar()->changeWaypointButton,
+ SIGNAL(clicked()),
+ this,
+ SLOT(changeWaypoint()));
// Play preview: clicked
- QObject::connect(guiToolbar.get()->getToolBar()->playPreviewButton, SIGNAL(clicked()),
- this, SLOT(playPreview()));
+ QObject::connect(guiToolbar.get()->getToolBar()->playPreviewButton,
+ SIGNAL(clicked()),
+ this,
+ SLOT(playPreview()));
// Play all preview: clicked
- QObject::connect(guiToolbar.get()->getToolBar()->playPreviewAllButton, SIGNAL(clicked()),
- this, SLOT(playAllPreview()));
+ QObject::connect(guiToolbar.get()->getToolBar()->playPreviewAllButton,
+ SIGNAL(clicked()),
+ this,
+ SLOT(playAllPreview()));
// Stop preview: clicked
- QObject::connect(guiToolbar.get()->getToolBar()->stopPreviewButton, SIGNAL(clicked()),
- this, SLOT(stopPreview()));
-
+ QObject::connect(guiToolbar.get()->getToolBar()->stopPreviewButton,
+ SIGNAL(clicked()),
+ this,
+ SLOT(stopPreview()));
}
- void ToolBarController::onDisconnectComponent()
+ void
+ ToolBarController::onDisconnectComponent()
{
ARMARX_INFO << "RobotTrajectoryDesigner: ToolBarController on disconnect";
}
- void ToolBarController::onExitComponent()
+ void
+ ToolBarController::onExitComponent()
{
ARMARX_INFO << "RobotTrajectoryDesigner: ToolBarController on exit";
}
ToolBarController::ToolBarController(ToolBarPtr guiToolbar) :
- guiToolbar(guiToolbar),
- currentWaypoint(0)
+ guiToolbar(guiToolbar), currentWaypoint(0)
{
onInitComponent();
onConnectComponent();
}
- ToolBarPtr ToolBarController::getGuiToolbar()
+ ToolBarPtr
+ ToolBarController::getGuiToolbar()
{
return this->guiToolbar;
}
- void ToolBarController::setGuiToolbar(ToolBarPtr guiToolbar)
+ void
+ ToolBarController::setGuiToolbar(ToolBarPtr guiToolbar)
{
this->guiToolbar = guiToolbar;
}
- int ToolBarController::getCurrentWaypoint()
+ int
+ ToolBarController::getCurrentWaypoint()
{
return this->currentWaypoint;
}
- void ToolBarController::setCurrentWaypoint(int currentWaypoint)
+ void
+ ToolBarController::setCurrentWaypoint(int currentWaypoint)
{
if (currentWaypoint >= 0)
{
@@ -104,65 +122,77 @@ namespace armarx
}
}
- void ToolBarController::setWaypoint()
+ void
+ ToolBarController::setWaypoint()
{
emit addedWaypoint(this->currentWaypoint);
}
- void ToolBarController::deleteWaypoint()
+ void
+ ToolBarController::deleteWaypoint()
{
emit deletedWaypoint(this->currentWaypoint);
}
- void ToolBarController::changeWaypoint()
+ void
+ ToolBarController::changeWaypoint()
{
emit changeWaypoint(this->currentWaypoint);
}
- void ToolBarController::playAllPreview()
+ void
+ ToolBarController::playAllPreview()
{
emit notifyAllPreview();
}
- void ToolBarController::playPreview()
+ void
+ ToolBarController::playPreview()
{
emit notifyPreview();
}
- void ToolBarController::stopPreview()
+ void
+ ToolBarController::stopPreview()
{
emit notifyStopPreview();
}
- void ToolBarController::retranslateGui()
+ void
+ ToolBarController::retranslateGui()
{
- throw ("not yet implemented");
+ throw("not yet implemented");
}
- void ToolBarController::enableDeleteChangeButton(bool enable)
+ void
+ ToolBarController::enableDeleteChangeButton(bool enable)
{
guiToolbar->getToolBar()->changeWaypointButton->setEnabled(enable);
guiToolbar->getToolBar()->deleteWaypointButton->setEnabled(enable);
}
- void ToolBarController::enableAddButton(bool enable)
+ void
+ ToolBarController::enableAddButton(bool enable)
{
guiToolbar->getToolBar()->setWaypointButton->setEnabled(enable);
}
- void ToolBarController::enablePreviewAllButton(bool enable)
+ void
+ ToolBarController::enablePreviewAllButton(bool enable)
{
guiToolbar->getToolBar()->playPreviewAllButton->setEnabled(enable);
}
- void ToolBarController::enablePreviewButton(bool enable)
+ void
+ ToolBarController::enablePreviewButton(bool enable)
{
guiToolbar->getToolBar()->playPreviewButton->setEnabled(enable);
}
- void ToolBarController::enableStopButton(bool enable)
+ void
+ ToolBarController::enableStopButton(bool enable)
{
guiToolbar->getToolBar()->stopPreviewButton->setEnabled(enable);
}
-}
+} // namespace armarx
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/ToolBarController.h b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/ToolBarController.h
index 7c71d037f4e1b0b8c326042d660f325d48abbc86..093cc0a5880bad82206b72519e28246d014b6400 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/ToolBarController.h
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/ToolBarController.h
@@ -21,8 +21,8 @@
*/
#ifndef TOOLBARCONTROLLER_H
#define TOOLBARCONTROLLER_H
-#include "AbstractController.h"
#include "../View/ToolBar.h"
+#include "AbstractController.h"
namespace armarx
{
@@ -200,6 +200,6 @@ namespace armarx
};
using ToolBarControllerPtr = std::shared_ptr<ToolBarController>;
-}
+} // namespace armarx
#endif // TOOLBARCONTROLLER_H
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/TrajectoryController.cpp b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/TrajectoryController.cpp
index d67f2672f3526b16fe37bd29de745636ec105246..444eb5ea0c8c6bceba860fcf226c4a84f03ed80c 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/TrajectoryController.cpp
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/TrajectoryController.cpp
@@ -1,27 +1,34 @@
#include "TrajectoryController.h"
-#include "../Util/OrientationConversion.h"
+
#include <iostream>
+
#include <QMessageBox>
+
+#include "../Util/OrientationConversion.h"
using namespace std;
namespace armarx
{
- void TrajectoryController::onInitComponent()
+ void
+ TrajectoryController::onInitComponent()
{
ARMARX_INFO << "RobotTrajectoryDesigner: TrajectoryController on init";
}
- void TrajectoryController::onConnectComponent()
+ void
+ TrajectoryController::onConnectComponent()
{
ARMARX_INFO << "RobotTrajectoryDesigner: TrajectoryController on connect";
}
- void TrajectoryController::onDisconnectComponent()
+ void
+ TrajectoryController::onDisconnectComponent()
{
ARMARX_INFO << "RobotTrajectoryDesigner: TrajectoryController on disconnect";
}
- void TrajectoryController::onExitComponent()
+ void
+ TrajectoryController::onExitComponent()
{
ARMARX_INFO << "RobotTrajectoryDesigner: TrajectoryController on exit";
}
@@ -34,14 +41,16 @@ namespace armarx
onConnectComponent();
}
- void TrajectoryController::updateTCP(QString tcp)
+ void
+ TrajectoryController::updateTCP(QString tcp)
{
if (environment->getRobot() == NULL || holder == NULL)
{
ARMARX_INFO << "Robot or holder is NULL";
return;
}
- VirtualRobot::RobotNodeSetPtr helpRns = environment->getRobot()->getRobotNodeSet(tcp.toStdString());
+ VirtualRobot::RobotNodeSetPtr helpRns =
+ environment->getRobot()->getRobotNodeSet(tcp.toStdString());
if (manager != NULL && !manager->getIsInitialized())
{
holder->deleteDesignerTrajectoryManager(rns->getName());
@@ -73,7 +82,8 @@ namespace armarx
{
QMessageBox* rnsIsPartOfExistingRns = new QMessageBox;
rnsIsPartOfExistingRns->setWindowTitle(QString::fromStdString("Error Message"));
- rnsIsPartOfExistingRns->setText(QString::fromStdString("Selected TCP is part of an existing TCP."));
+ rnsIsPartOfExistingRns->setText(
+ QString::fromStdString("Selected TCP is part of an existing TCP."));
rnsIsPartOfExistingRns->exec();
emit updateSelectedTCP(QString::fromStdString(rns->getName()));
}
@@ -89,13 +99,14 @@ namespace armarx
helpEnableUndoRedo();
}
- void TrajectoryController::updateTransition(int transition, int it)
+ void
+ TrajectoryController::updateTransition(int transition, int it)
{
if (manager)
{
try
{
- manager->setTransitionInterpolation(transition, (InterpolationType) it);
+ manager->setTransitionInterpolation(transition, (InterpolationType)it);
emit showTrajectory(manager->getDesignerTrajectory());
changeTransitionWaypointGui(manager->getDesignerTrajectory());
helpEnableUndoRedo();
@@ -104,19 +115,22 @@ namespace armarx
catch (LocalException& e)
{
helpExceptionMessageBox(e.getReason());
- TransitionPtr transitionPtr = manager->getDesignerTrajectory()->getTransition(transition);
+ TransitionPtr transitionPtr =
+ manager->getDesignerTrajectory()->getTransition(transition);
helpChangeTransitionGui(transitionPtr, transition);
}
catch (InvalidArgumentException& e)
{
helpExceptionMessageBox(e.reason);
- TransitionPtr transitionPtr = manager->getDesignerTrajectory()->getTransition(transition);
+ TransitionPtr transitionPtr =
+ manager->getDesignerTrajectory()->getTransition(transition);
helpChangeTransitionGui(transitionPtr, transition);
}
}
}
- void TrajectoryController::updateTransition(int transition, double duration)
+ void
+ TrajectoryController::updateTransition(int transition, double duration)
{
if (manager)
{
@@ -130,13 +144,15 @@ namespace armarx
}
catch (InvalidArgumentException& e)
{
- TransitionPtr transitionPtr = manager->getDesignerTrajectory()->getTransition(transition);
+ TransitionPtr transitionPtr =
+ manager->getDesignerTrajectory()->getTransition(transition);
helpChangeTransitionGui(transitionPtr, transition);
}
}
}
- void TrajectoryController::updateWaypoint(int waypoint, std::vector<double> values)
+ void
+ TrajectoryController::updateWaypoint(int waypoint, std::vector<double> values)
{
if (manager)
{
@@ -150,24 +166,30 @@ namespace armarx
}
catch (LocalException& e)
{
- UserWaypointPtr waypointPtr = manager->getDesignerTrajectory()->getUserWaypoint(waypoint);
+ UserWaypointPtr waypointPtr =
+ manager->getDesignerTrajectory()->getUserWaypoint(waypoint);
helpChangeWaypointGui(waypointPtr, waypoint);
}
catch (InvalidArgumentException& e)
{
- UserWaypointPtr waypointPtr = manager->getDesignerTrajectory()->getUserWaypoint(waypoint);
+ UserWaypointPtr waypointPtr =
+ manager->getDesignerTrajectory()->getUserWaypoint(waypoint);
helpChangeWaypointGui(waypointPtr, waypoint);
}
}
}
- void TrajectoryController::updateWaypoint(int waypoint)
+ void
+ TrajectoryController::updateWaypoint(int waypoint)
{
if (manager)
{
try
{
- manager->editWaypointPoseBase(waypoint, *new PoseBasePtr(new Pose(rns->getKinematicRoot()->toLocalCoordinateSystem(rns->getTCP()->getGlobalPose()))));
+ manager->editWaypointPoseBase(
+ waypoint,
+ *new PoseBasePtr(new Pose(rns->getKinematicRoot()->toLocalCoordinateSystem(
+ rns->getTCP()->getGlobalPose()))));
emit showTrajectory(manager->getDesignerTrajectory());
changeTransitionWaypointGui(manager->getDesignerTrajectory());
helpEnableUndoRedo();
@@ -176,7 +198,6 @@ namespace armarx
catch (LocalException& e)
{
helpExceptionMessageBox(e.getReason());
-
}
catch (InvalidArgumentException& e)
{
@@ -185,7 +206,8 @@ namespace armarx
}
}
- void TrajectoryController::updateWaypoint(int waypoint, PoseBasePtr newPoseBase)
+ void
+ TrajectoryController::updateWaypoint(int waypoint, PoseBasePtr newPoseBase)
{
if (manager)
{
@@ -200,47 +222,55 @@ namespace armarx
catch (LocalException& e)
{
helpExceptionMessageBox(e.getReason());
- UserWaypointPtr waypointPtr = manager->getDesignerTrajectory()->getUserWaypoint(waypoint);
+ UserWaypointPtr waypointPtr =
+ manager->getDesignerTrajectory()->getUserWaypoint(waypoint);
helpChangeWaypointGui(waypointPtr, waypoint);
}
catch (InvalidArgumentException& e)
{
helpExceptionMessageBox(e.reason);
- UserWaypointPtr waypointPtr = manager->getDesignerTrajectory()->getUserWaypoint(waypoint);
+ UserWaypointPtr waypointPtr =
+ manager->getDesignerTrajectory()->getUserWaypoint(waypoint);
helpChangeWaypointGui(waypointPtr, waypoint);
}
}
}
- void TrajectoryController::updateWaypoint(int waypoint, int cartesianSelection)
+ void
+ TrajectoryController::updateWaypoint(int waypoint, int cartesianSelection)
{
if (manager)
{
try
{
- manager->editWaypointIKSelection(waypoint, (VirtualRobot::IKSolver::CartesianSelection) cartesianSelection);
+ manager->editWaypointIKSelection(
+ waypoint, (VirtualRobot::IKSolver::CartesianSelection)cartesianSelection);
emit showTrajectory(manager->getDesignerTrajectory());
changeTransitionWaypointGui(manager->getDesignerTrajectory());
- emit cartesianSelectionChanged(manager->getDesignerTrajectory()->getUserWaypoint(waypoint)->getIKSelection());
+ emit cartesianSelectionChanged(
+ manager->getDesignerTrajectory()->getUserWaypoint(waypoint)->getIKSelection());
helpEnableUndoRedo();
helpCollision();
}
catch (LocalException& e)
{
helpExceptionMessageBox(e.getReason());
- UserWaypointPtr waypointPtr = manager->getDesignerTrajectory()->getUserWaypoint(waypoint);
+ UserWaypointPtr waypointPtr =
+ manager->getDesignerTrajectory()->getUserWaypoint(waypoint);
helpChangeWaypointGui(waypointPtr, waypoint);
}
catch (InvalidArgumentException& e)
{
helpExceptionMessageBox(e.reason);
- UserWaypointPtr waypointPtr = manager->getDesignerTrajectory()->getUserWaypoint(waypoint);
+ UserWaypointPtr waypointPtr =
+ manager->getDesignerTrajectory()->getUserWaypoint(waypoint);
helpChangeWaypointGui(waypointPtr, waypoint);
}
}
}
- void TrajectoryController::updateWaypoint(int waypoint, bool isBreakpoint)
+ void
+ TrajectoryController::updateWaypoint(int waypoint, bool isBreakpoint)
{
if (manager == NULL)
{
@@ -257,18 +287,21 @@ namespace armarx
catch (LocalException& e)
{
helpExceptionMessageBox(e.getReason());
- UserWaypointPtr waypointPtr = manager->getDesignerTrajectory()->getUserWaypoint(waypoint);
+ UserWaypointPtr waypointPtr =
+ manager->getDesignerTrajectory()->getUserWaypoint(waypoint);
helpChangeWaypointGui(waypointPtr, waypoint);
}
catch (InvalidArgumentException& e)
{
helpExceptionMessageBox(e.reason);
- UserWaypointPtr waypointPtr = manager->getDesignerTrajectory()->getUserWaypoint(waypoint);
+ UserWaypointPtr waypointPtr =
+ manager->getDesignerTrajectory()->getUserWaypoint(waypoint);
helpChangeWaypointGui(waypointPtr, waypoint);
}
}
- void TrajectoryController::addWaypoint(int waypoint, bool insertAfter)
+ void
+ TrajectoryController::addWaypoint(int waypoint, bool insertAfter)
{
if (manager == NULL)
{
@@ -281,22 +314,29 @@ namespace armarx
std::vector<double> jointValues;
for (double newValue : rns->getJointValues())
{
- jointValues.push_back((double) newValue);
+ jointValues.push_back((double)newValue);
}
manager->initializeDesignerTrajectory(jointValues);
emit showTrajectory(manager->getDesignerTrajectory());
- helpAddWaypointGui(manager->getDesignerTrajectory()->getUserWaypoint(waypoint), waypoint);
+ helpAddWaypointGui(manager->getDesignerTrajectory()->getUserWaypoint(waypoint),
+ waypoint);
emit newTrajectory(QString::fromStdString(rns->getName()));
emit updateSelectedTCP(QString::fromStdString(rns->getName()));
}
- else if (helpWaypoint == manager->getDesignerTrajectory()->getNrOfUserWaypoints() - static_cast<unsigned>(1) && insertAfter)
+ else if (helpWaypoint == manager->getDesignerTrajectory()->getNrOfUserWaypoints() -
+ static_cast<unsigned>(1) &&
+ insertAfter)
{
try
{
- manager->addWaypoint(*new PoseBasePtr(new Pose(rns->getKinematicRoot()->toLocalCoordinateSystem(rns->getTCP()->getGlobalPose()))));
+ manager->addWaypoint(
+ *new PoseBasePtr(new Pose(rns->getKinematicRoot()->toLocalCoordinateSystem(
+ rns->getTCP()->getGlobalPose()))));
emit showTrajectory(manager->getDesignerTrajectory());
- helpAddWaypointGui(manager->getDesignerTrajectory()->getUserWaypoint(waypoint + 1), waypoint + 1);
- helpAddTransitionGui(manager->getDesignerTrajectory()->getTransition(waypoint), waypoint);
+ helpAddWaypointGui(manager->getDesignerTrajectory()->getUserWaypoint(waypoint + 1),
+ waypoint + 1);
+ helpAddTransitionGui(manager->getDesignerTrajectory()->getTransition(waypoint),
+ waypoint);
changeTransitionWaypointGui(manager->getDesignerTrajectory());
helpCollision();
}
@@ -309,14 +349,21 @@ namespace armarx
helpExceptionMessageBox(e.reason);
}
}
- else if (helpWaypoint < manager->getDesignerTrajectory()->getNrOfUserWaypoints() - static_cast<unsigned>(1) && waypoint >= 0 && insertAfter)
+ else if (helpWaypoint < manager->getDesignerTrajectory()->getNrOfUserWaypoints() -
+ static_cast<unsigned>(1) &&
+ waypoint >= 0 && insertAfter)
{
try
{
- manager->insertWaypoint(waypoint + 1, *new PoseBasePtr(new Pose(rns->getKinematicRoot()->toLocalCoordinateSystem(rns->getTCP()->getGlobalPose()))));
+ manager->insertWaypoint(
+ waypoint + 1,
+ *new PoseBasePtr(new Pose(rns->getKinematicRoot()->toLocalCoordinateSystem(
+ rns->getTCP()->getGlobalPose()))));
emit showTrajectory(manager->getDesignerTrajectory());
- helpAddWaypointGui(manager->getDesignerTrajectory()->getUserWaypoint(waypoint + 1), waypoint + 1);
- helpAddTransitionGui(manager->getDesignerTrajectory()->getTransition(waypoint + 1), waypoint + 1);
+ helpAddWaypointGui(manager->getDesignerTrajectory()->getUserWaypoint(waypoint + 1),
+ waypoint + 1);
+ helpAddTransitionGui(manager->getDesignerTrajectory()->getTransition(waypoint + 1),
+ waypoint + 1);
changeTransitionWaypointGui(manager->getDesignerTrajectory());
helpCollision();
}
@@ -325,14 +372,20 @@ namespace armarx
helpExceptionMessageBox(e.getReason());
}
}
- else if (helpWaypoint < manager->getDesignerTrajectory()->getNrOfUserWaypoints() && waypoint >= 0 && !insertAfter)
+ else if (helpWaypoint < manager->getDesignerTrajectory()->getNrOfUserWaypoints() &&
+ waypoint >= 0 && !insertAfter)
{
try
{
- manager->insertWaypoint(waypoint, *new PoseBasePtr(new Pose(rns->getKinematicRoot()->toLocalCoordinateSystem(rns->getTCP()->getGlobalPose()))));
+ manager->insertWaypoint(
+ waypoint,
+ *new PoseBasePtr(new Pose(rns->getKinematicRoot()->toLocalCoordinateSystem(
+ rns->getTCP()->getGlobalPose()))));
emit showTrajectory(manager->getDesignerTrajectory());
- helpAddWaypointGui(manager->getDesignerTrajectory()->getUserWaypoint(waypoint), waypoint);
- helpAddTransitionGui(manager->getDesignerTrajectory()->getTransition(waypoint), waypoint);
+ helpAddWaypointGui(manager->getDesignerTrajectory()->getUserWaypoint(waypoint),
+ waypoint);
+ helpAddTransitionGui(manager->getDesignerTrajectory()->getTransition(waypoint),
+ waypoint);
changeTransitionWaypointGui(manager->getDesignerTrajectory());
helpCollision();
}
@@ -349,7 +402,8 @@ namespace armarx
helpEnableUndoRedo();
}
- void TrajectoryController::deleteWaypoint(int waypoint)
+ void
+ TrajectoryController::deleteWaypoint(int waypoint)
{
if (manager == NULL)
{
@@ -378,7 +432,8 @@ namespace armarx
helpExceptionMessageBox(e.reason);
}
}
- else if (helpWaypoint == manager->getDesignerTrajectory()->getNrOfUserWaypoints() - static_cast<unsigned>(1))
+ else if (helpWaypoint == manager->getDesignerTrajectory()->getNrOfUserWaypoints() -
+ static_cast<unsigned>(1))
{
try
{
@@ -398,7 +453,9 @@ namespace armarx
helpExceptionMessageBox(e.reason);
}
}
- else if (waypoint >= 0 && helpWaypoint < manager->getDesignerTrajectory()->getNrOfUserWaypoints() - static_cast<unsigned>(1))
+ else if (waypoint >= 0 &&
+ helpWaypoint < manager->getDesignerTrajectory()->getNrOfUserWaypoints() -
+ static_cast<unsigned>(1))
{
try
{
@@ -422,14 +479,18 @@ namespace armarx
helpEnableButtons();
}
- void TrajectoryController::import(DesignerTrajectoryPtr designerTrajectory)
+ void
+ TrajectoryController::import(DesignerTrajectoryPtr designerTrajectory)
{
if (holder)
{
if (holder->rnsIsPartOfExistingRns(designerTrajectory->getRns()->getName()))
{
QMessageBox::StandardButton reply;
- reply = QMessageBox::question(0, "Import", "Do you want to override the existing Trajectory?", QMessageBox::Yes | QMessageBox::No);
+ reply = QMessageBox::question(0,
+ "Import",
+ "Do you want to override the existing Trajectory?",
+ QMessageBox::Yes | QMessageBox::No);
if (reply == QMessageBox::Yes)
{
emit removeTrajectory(QString::fromStdString(rns->getName()));
@@ -454,40 +515,47 @@ namespace armarx
}
}
- void TrajectoryController::exportTrajectory(int fps)
+ void
+ TrajectoryController::exportTrajectory(int fps)
{
if (manager)
{
std::vector<DesignerTrajectoryPtr> designerTrajectories;
for (VirtualRobot::RobotNodeSetPtr s : environment->getRobot()->getRobotNodeSets())
{
- if (holder->rnsExists(s->getName()) && holder->getTrajectoryManager(s->getName())->getIsInitialized())
+ if (holder->rnsExists(s->getName()) &&
+ holder->getTrajectoryManager(s->getName())->getIsInitialized())
{
//holder->getTrajectoryManager(s)->setFPS(fps);
- designerTrajectories.push_back(holder->getTrajectoryManager(s->getName())->getDesignerTrajectory());
+ designerTrajectories.push_back(
+ holder->getTrajectoryManager(s->getName())->getDesignerTrajectory());
}
}
emit exportTrajectory(designerTrajectories);
}
}
- void TrajectoryController::exportTrajectory()
+ void
+ TrajectoryController::exportTrajectory()
{
if (manager)
{
std::vector<DesignerTrajectoryPtr> designerTrajectories;
for (VirtualRobot::RobotNodeSetPtr s : environment->getRobot()->getRobotNodeSets())
{
- if (holder->rnsExists(s->getName()) && holder->getTrajectoryManager(s->getName())->getIsInitialized())
+ if (holder->rnsExists(s->getName()) &&
+ holder->getTrajectoryManager(s->getName())->getIsInitialized())
{
- designerTrajectories.push_back(holder->getTrajectoryManager(s->getName())->getDesignerTrajectory());
+ designerTrajectories.push_back(
+ holder->getTrajectoryManager(s->getName())->getDesignerTrajectory());
}
}
emit exportTrajectory(designerTrajectories);
}
}
- void TrajectoryController::undo()
+ void
+ TrajectoryController::undo()
{
if (manager)
{
@@ -503,7 +571,8 @@ namespace armarx
helpCollision();
}
- void TrajectoryController::redo()
+ void
+ TrajectoryController::redo()
{
if (manager)
{
@@ -519,20 +588,23 @@ namespace armarx
helpCollision();
}
- void TrajectoryController::playTrajectories()
+ void
+ TrajectoryController::playTrajectories()
{
std::vector<DesignerTrajectoryPtr> designerTrajectories;
for (VirtualRobot::RobotNodeSetPtr s : environment->getRobot()->getRobotNodeSets())
{
if (holder->rnsExists(s->getName()))
{
- designerTrajectories.push_back(holder->getTrajectoryManager(s->getName())->getDesignerTrajectory());
+ designerTrajectories.push_back(
+ holder->getTrajectoryManager(s->getName())->getDesignerTrajectory());
}
}
emit playTrajectories(designerTrajectories);
}
- void TrajectoryController::environmentChanged(EnvironmentPtr environment)
+ void
+ TrajectoryController::environmentChanged(EnvironmentPtr environment)
{
this->environment = environment;
holder = std::make_shared<DesignerTrajectoryHolder>(environment);
@@ -544,13 +616,15 @@ namespace armarx
helpEnableButtons();
}
- void TrajectoryController::setActiveColModelName(QString activeColModelName)
+ void
+ TrajectoryController::setActiveColModelName(QString activeColModelName)
{
this->activeColModelName = activeColModelName.toStdString();
helpCollision();
}
- void TrajectoryController::setBodyColModelsNames(QStringList bodyColModelsNames)
+ void
+ TrajectoryController::setBodyColModelsNames(QStringList bodyColModelsNames)
{
this->bodyColModelsNames.clear();
for (QString name : bodyColModelsNames)
@@ -564,7 +638,8 @@ namespace armarx
//private methods
//=============================================================================================
- std::vector<double> TrajectoryController::parseToCoordinate(PoseBasePtr pose)
+ std::vector<double>
+ TrajectoryController::parseToCoordinate(PoseBasePtr pose)
{
float x = pose->position->x;
float y = pose->position->y;
@@ -578,23 +653,27 @@ namespace armarx
return {x, y, z, eulerAngle[0], eulerAngle[1], eulerAngle[2]};
}
- PoseBasePtr TrajectoryController::parseToPoseBasePtr(std::vector<double> values)
+ PoseBasePtr
+ TrajectoryController::parseToPoseBasePtr(std::vector<double> values)
{
Vector3Ptr position = new Vector3(values[0], values[1], values[2]);
- QuaternionBasePtr orientation = OrientationConversion::toQuaternion(values[3], values[4], values[5]);
+ QuaternionBasePtr orientation =
+ OrientationConversion::toQuaternion(values[3], values[4], values[5]);
return new FramedPose(position, orientation, "", "");
}
//=============================================================================================
- void TrajectoryController::helpAddWaypointGui(UserWaypointPtr waypoint, int waypointIndex)
+ void
+ TrajectoryController::helpAddWaypointGui(UserWaypointPtr waypoint, int waypointIndex)
{
std::vector<double> values = parseToCoordinate(waypoint->getPose());
bool isBreakpoint = waypoint->getIsTimeOptimalBreakpoint();
emit addWaypointGui(waypointIndex, values, waypoint->getIKSelection(), isBreakpoint);
}
- void TrajectoryController::helpAddTransitionGui(TransitionPtr transition, int transitionIndex)
+ void
+ TrajectoryController::helpAddTransitionGui(TransitionPtr transition, int transitionIndex)
{
double duration = transition->getUserDuration();
double startTime = transition->getStart()->getUserTimestamp();
@@ -602,7 +681,8 @@ namespace armarx
emit addTransitionGui(transitionIndex, duration, startTime, interpolation);
}
- void TrajectoryController::addTransitionWaypointGui(DesignerTrajectoryPtr trajectory)
+ void
+ TrajectoryController::addTransitionWaypointGui(DesignerTrajectoryPtr trajectory)
{
if (trajectory)
{
@@ -622,12 +702,12 @@ namespace armarx
{
return;
}
-
}
//=============================================================================================
- void TrajectoryController::helpChangeTransitionGui(TransitionPtr transition, int transitionIndex)
+ void
+ TrajectoryController::helpChangeTransitionGui(TransitionPtr transition, int transitionIndex)
{
double duration = transition->getUserDuration();
double startTime = transition->getStart()->getUserTimestamp();
@@ -635,14 +715,16 @@ namespace armarx
emit changeTransitionGui(transitionIndex, duration, startTime, interpolation);
}
- void TrajectoryController::helpChangeWaypointGui(UserWaypointPtr waypoint, int waypointIndex)
+ void
+ TrajectoryController::helpChangeWaypointGui(UserWaypointPtr waypoint, int waypointIndex)
{
std::vector<double> values = parseToCoordinate(waypoint->getPose());
bool isBreakpoint = waypoint->getIsTimeOptimalBreakpoint();
emit changeWaypointGui(waypointIndex, values, waypoint->getIKSelection(), isBreakpoint);
}
- void TrajectoryController::changeTransitionWaypointGui(DesignerTrajectoryPtr trajectory)
+ void
+ TrajectoryController::changeTransitionWaypointGui(DesignerTrajectoryPtr trajectory)
{
for (unsigned int i = 0; i < trajectory->getNrOfUserWaypoints(); i++)
{
@@ -659,7 +741,8 @@ namespace armarx
//=============================================================================================
- void TrajectoryController::helpImportDesignerTrajectory(DesignerTrajectoryPtr designerTrajectory)
+ void
+ TrajectoryController::helpImportDesignerTrajectory(DesignerTrajectoryPtr designerTrajectory)
{
removeTransitionWaypointGui();
holder->import(designerTrajectory, true);
@@ -674,7 +757,8 @@ namespace armarx
//=============================================================================================
- void TrajectoryController::helpCreateDesignerTrajectoryManager()
+ void
+ TrajectoryController::helpCreateDesignerTrajectoryManager()
{
holder->createDesignerTrajectoryManager(rns->getName());
manager = holder->getTrajectoryManager(rns->getName());
@@ -683,14 +767,18 @@ namespace armarx
//=============================================================================================
- bool TrajectoryController::helpEnableExportPreviewAll()
+ bool
+ TrajectoryController::helpEnableExportPreviewAll()
{
bool help = false;
for (VirtualRobot::RobotNodeSetPtr s : environment->getRobot()->getRobotNodeSets())
{
- if (holder->rnsExists(s->getName()) && holder->getTrajectoryManager(s->getName())->getIsInitialized())
+ if (holder->rnsExists(s->getName()) &&
+ holder->getTrajectoryManager(s->getName())->getIsInitialized())
{
- if (holder->getTrajectoryManager(s->getName())->getDesignerTrajectory()->getNrOfUserWaypoints() > 1)
+ if (holder->getTrajectoryManager(s->getName())
+ ->getDesignerTrajectory()
+ ->getNrOfUserWaypoints() > 1)
{
help = true;
}
@@ -703,7 +791,8 @@ namespace armarx
return help;
}
- void TrajectoryController::helpEnableUndoRedo()
+ void
+ TrajectoryController::helpEnableUndoRedo()
{
if (manager != NULL)
{
@@ -717,7 +806,8 @@ namespace armarx
}
}
- void TrajectoryController::helpEnableButtons()
+ void
+ TrajectoryController::helpEnableButtons()
{
emit enableExportButtons(helpEnableExportPreviewAll());
emit enablePreviewAll(helpEnableExportPreviewAll());
@@ -762,7 +852,8 @@ namespace armarx
}
}
- void TrajectoryController::helpExceptionMessageBox(std::string errorMessage)
+ void
+ TrajectoryController::helpExceptionMessageBox(std::string errorMessage)
{
QMessageBox* errorMessageBox = new QMessageBox;
errorMessageBox->setWindowTitle(QString::fromStdString("Error Message"));
@@ -772,7 +863,8 @@ namespace armarx
//=============================================================================================
- void TrajectoryController::helpCollision()
+ void
+ TrajectoryController::helpCollision()
{
if (holder == NULL)
{
@@ -787,4 +879,4 @@ namespace armarx
changeInterpolationFailed->exec();
}
}
-}
+} // namespace armarx
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/TrajectoryController.h b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/TrajectoryController.h
index a43c32dc8ace20465e93baf8878d5103573c3992..d2dcfe4aa9c347ab6fee15c970660fa64436328b 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/TrajectoryController.h
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/TrajectoryController.h
@@ -1,8 +1,8 @@
#ifndef TRAJECTORYCONTROLLER_H
#define TRAJECTORYCONTROLLER_H
-#include "AbstractController.h"
#include "../Manager/DesignerTrajectoryHolder.h"
#include "../Manager/DesignerTrajectoryManager.h"
+#include "AbstractController.h"
namespace armarx
{
@@ -178,7 +178,9 @@ namespace armarx
* @param constraints Constraints of the waypoint: isBreakpoint, ikConstraints
* @param waypoint Index of the waypoint
*/
- void changeWaypointGui(int index, std::vector<double> values, int cartesianSelection,
+ void changeWaypointGui(int index,
+ std::vector<double> values,
+ int cartesianSelection,
bool isBreakpoint);
/**
@@ -197,7 +199,10 @@ namespace armarx
* @param constraints Constraints of the waypoint: isBreakpoint, ikConstraints
* @param waypoint Index of the waypoint
*/
- void addWaypointGui(int waypoint, std::vector<double> values, int cartesianSelection, bool isBreakpoint);
+ void addWaypointGui(int waypoint,
+ std::vector<double> values,
+ int cartesianSelection,
+ bool isBreakpoint);
/**
* @brief Notifies other controllers about the deletion of a given waypoint
@@ -332,7 +337,7 @@ namespace armarx
* @param pose PoseBase to convert
* @return Vector containing the resulting coordinates and angles
*/
- std::vector<double>parseToCoordinate(PoseBasePtr pose);
+ std::vector<double> parseToCoordinate(PoseBasePtr pose);
PoseBasePtr parseToPoseBasePtr(std::vector<double> values);
@@ -357,7 +362,7 @@ namespace armarx
void helpCollision();
DesignerTrajectoryManagerPtr manager;
- DesignerTrajectoryHolderPtr holder;
+ DesignerTrajectoryHolderPtr holder;
VirtualRobot::RobotNodeSetPtr rns;
EnvironmentPtr environment;
std::string activeColModelName;
@@ -365,6 +370,6 @@ namespace armarx
};
using TrajectoryControllerPtr = std::shared_ptr<TrajectoryController>;
-}
+} // namespace armarx
#endif // TRAJECTORYCONTROLLER_H
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/TransitionController.cpp b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/TransitionController.cpp
index 9a7dabb8c6f6935a6465d5b5d883dee6ec9883e8..7013424fac551812092734f565105f2ee8e3f7a5 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/TransitionController.cpp
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/TransitionController.cpp
@@ -20,13 +20,16 @@
* GNU General Public License
*/
#include "TransitionController.h"
-#include "../Interpolation/InterpolationType.h"
+
#include <iostream>
+#include "../Interpolation/InterpolationType.h"
+
namespace armarx
{
- void TransitionController::onInitComponent()
+ void
+ TransitionController::onInitComponent()
{
ARMARX_INFO << "RobotTrajectoryDesigner: TransitionController on init";
@@ -34,46 +37,49 @@ namespace armarx
initInterpolationComboBox();
// Set single selection and clear transition list
- this->guiTransitionTab->getTransitionTab()->transitionList->
- setSelectionMode(QAbstractItemView::SingleSelection);
+ this->guiTransitionTab->getTransitionTab()->transitionList->setSelectionMode(
+ QAbstractItemView::SingleSelection);
this->guiTransitionTab->getTransitionTab()->transitionList->clear();
// Add double validator to duration line edit
- QDoubleValidator* validator = new QDoubleValidator(
- -1000.000,
- 1000.000,
- 3,
- guiTransitionTab.get());
+ QDoubleValidator* validator =
+ new QDoubleValidator(-1000.000, 1000.000, 3, guiTransitionTab.get());
validator->setNotation(QDoubleValidator::StandardNotation);
this->guiTransitionTab->getTransitionTab()->durationLineEdit->setValidator(validator);
}
- void TransitionController::onConnectComponent()
+ void
+ TransitionController::onConnectComponent()
{
ARMARX_INFO << "RobotTrajectoryDesigner: TransitionController on connect";
// Transition list: changed selected item
QObject::connect(guiTransitionTab->getTransitionTab()->transitionList,
- SIGNAL(itemClicked(QListWidgetItem*)), this,
+ SIGNAL(itemClicked(QListWidgetItem*)),
+ this,
SLOT(updateSelectedTransition(QListWidgetItem*)));
// Transition duration: changed duration
QObject::connect(guiTransitionTab->getTransitionTab()->durationLineEdit,
SIGNAL(editingFinished()),
- this, SLOT(setDurationValue()));
+ this,
+ SLOT(setDurationValue()));
// Transition interpolation: changed interpolation
QObject::connect(guiTransitionTab->getTransitionTab()->interpolationComboBox,
SIGNAL(activated(int)),
- this, SLOT(setInterpolation(int)));
+ this,
+ SLOT(setInterpolation(int)));
}
- void TransitionController::onDisconnectComponent()
+ void
+ TransitionController::onDisconnectComponent()
{
ARMARX_INFO << "RobotTrajectoryDesigner: TransitionController on disconnect";
}
- void TransitionController::onExitComponent()
+ void
+ TransitionController::onExitComponent()
{
ARMARX_INFO << "RobotTrajectoryDesigner: TransitionController on exit";
}
@@ -85,12 +91,14 @@ namespace armarx
onConnectComponent();
}
- TransitionTabPtr TransitionController::getGuiTransitionTab()
+ TransitionTabPtr
+ TransitionController::getGuiTransitionTab()
{
return this->guiTransitionTab;
}
- void TransitionController::setGuiTransitionTab(TransitionTabPtr guiTransitionTab)
+ void
+ TransitionController::setGuiTransitionTab(TransitionTabPtr guiTransitionTab)
{
if (guiTransitionTab != NULL)
{
@@ -98,11 +106,8 @@ namespace armarx
}
}
- void TransitionController::addTransition(
- int index,
- double duration,
- double start,
- int interpolation)
+ void
+ TransitionController::addTransition(int index, double duration, double start, int interpolation)
{
// Create struct representing transition
struct GuiTransition transition;
@@ -117,7 +122,8 @@ namespace armarx
data.setValue(transition);
// add data to item and insert
- QListWidgetItem* item = new QListWidgetItem(QString(("Transition: " + std::to_string(index)).c_str()));
+ QListWidgetItem* item =
+ new QListWidgetItem(QString(("Transition: " + std::to_string(index)).c_str()));
item->setData(Qt::UserRole, data);
QListWidget* transitions = guiTransitionTab->getTransitionTab()->transitionList;
@@ -127,7 +133,8 @@ namespace armarx
changeTextListWidgetItems();
}
- void TransitionController::removeTransition(int index)
+ void
+ TransitionController::removeTransition(int index)
{
QListWidget* transitions = guiTransitionTab->getTransitionTab()->transitionList;
@@ -139,49 +146,51 @@ namespace armarx
changeTextListWidgetItems();
}
- void TransitionController::updateSelectedTransition(QListWidgetItem* item)
+ void
+ TransitionController::updateSelectedTransition(QListWidgetItem* item)
{
GuiTransition transition = item->data(Qt::UserRole).value<GuiTransition>();
// Duration line edit
- guiTransitionTab->getTransitionTab()->durationLineEdit->
- setText(QString::number(transition.duration));
+ guiTransitionTab->getTransitionTab()->durationLineEdit->setText(
+ QString::number(transition.duration));
// Start value label
- guiTransitionTab->getTransitionTab()->startValueLabel->
- setText(QString::number(transition.start));
+ guiTransitionTab->getTransitionTab()->startValueLabel->setText(
+ QString::number(transition.start));
// End value label
- guiTransitionTab->getTransitionTab()->EndValueLabel->
- setText(QString::number(transition.start + transition.duration));
+ guiTransitionTab->getTransitionTab()->EndValueLabel->setText(
+ QString::number(transition.start + transition.duration));
// Interpolation combo box
- guiTransitionTab->getTransitionTab()->interpolationComboBox->
- setCurrentIndex(transition.it);
+ guiTransitionTab->getTransitionTab()->interpolationComboBox->setCurrentIndex(transition.it);
// Notify other controllers about changes of the selected transition
QListWidget* transitions = guiTransitionTab->getTransitionTab()->transitionList;
emit selectedTransitionChanged(transitions->currentIndex().row());
-
}
- void TransitionController::updateSelectedTransition(int index)
+ void
+ TransitionController::updateSelectedTransition(int index)
{
- GuiTransition transition = guiTransitionTab->getTransitionTab()->transitionList->
- item(index)->data(Qt::UserRole).value<GuiTransition>();
+ GuiTransition transition = guiTransitionTab->getTransitionTab()
+ ->transitionList->item(index)
+ ->data(Qt::UserRole)
+ .value<GuiTransition>();
// Duration line edit
- guiTransitionTab->getTransitionTab()->durationLineEdit->
- setText(QString::number(transition.duration));
+ guiTransitionTab->getTransitionTab()->durationLineEdit->setText(
+ QString::number(transition.duration));
// Start value label
- guiTransitionTab->getTransitionTab()->startValueLabel->
- setText(QString::number(transition.start));
+ guiTransitionTab->getTransitionTab()->startValueLabel->setText(
+ QString::number(transition.start));
// End value label
- guiTransitionTab->getTransitionTab()->EndValueLabel->
- setText(QString::number(transition.start + transition.duration));
+ guiTransitionTab->getTransitionTab()->EndValueLabel->setText(
+ QString::number(transition.start + transition.duration));
// Interpolation combo box
- guiTransitionTab->getTransitionTab()->interpolationComboBox->
- setCurrentIndex(transition.it);
+ guiTransitionTab->getTransitionTab()->interpolationComboBox->setCurrentIndex(transition.it);
}
- void TransitionController::setDurationValue()
+ void
+ TransitionController::setDurationValue()
{
if (this->guiTransitionTab->getTransitionTab()->durationLineEdit->hasAcceptableInput())
{
@@ -192,8 +201,8 @@ namespace armarx
if (item != NULL)
{
GuiTransition transition = item->data(Qt::UserRole).value<GuiTransition>();
- double dur = this->guiTransitionTab->getTransitionTab()->
- durationLineEdit->text().toDouble();
+ double dur =
+ this->guiTransitionTab->getTransitionTab()->durationLineEdit->text().toDouble();
// Check if duration of item has changed
if ((dur >= 0) && (dur != transition.duration))
@@ -201,11 +210,11 @@ namespace armarx
emit changedDuration(transitions->currentRow(), dur);
}
}
-
}
}
- void TransitionController::setInterpolation(int index)
+ void
+ TransitionController::setInterpolation(int index)
{
QListWidget* transitions = guiTransitionTab->getTransitionTab()->transitionList;
QListWidgetItem* item = transitions->currentItem();
@@ -222,18 +231,16 @@ namespace armarx
}
}
- void TransitionController::enableAll(bool enable)
+ void
+ TransitionController::enableAll(bool enable)
{
this->guiTransitionTab->getTransitionTab()->durationLineEdit->setEnabled(enable);
this->guiTransitionTab->getTransitionTab()->interpolationComboBox->setEnabled(enable);
this->guiTransitionTab->getTransitionTab()->transitionList->setEnabled(enable);
}
- void TransitionController::setTransitionData(
- int index,
- double duration,
- double start,
- int it)
+ void
+ TransitionController::setTransitionData(int index, double duration, double start, int it)
{
// Create struct holding relevant data
struct GuiTransition transition;
@@ -260,12 +267,14 @@ namespace armarx
}
}
- void TransitionController::retranslateGui()
+ void
+ TransitionController::retranslateGui()
{
- throw ("not yet implemented");
+ throw("not yet implemented");
}
- void TransitionController::clearTransitionList()
+ void
+ TransitionController::clearTransitionList()
{
// Clear transition list
this->guiTransitionTab->getTransitionTab()->transitionList->clear();
@@ -278,7 +287,8 @@ namespace armarx
this->guiTransitionTab->getTransitionTab()->interpolationComboBox->setCurrentIndex(0);
}
- bool TransitionController::contains(QListWidget* list, GuiTransition transition)
+ bool
+ TransitionController::contains(QListWidget* list, GuiTransition transition)
{
for (int i = 0; i < list->count(); i++)
{
@@ -290,10 +300,10 @@ namespace armarx
return false;
}
- void TransitionController::initInterpolationComboBox()
+ void
+ TransitionController::initInterpolationComboBox()
{
- QComboBox* interpolation = guiTransitionTab->getTransitionTab()->
- interpolationComboBox;
+ QComboBox* interpolation = guiTransitionTab->getTransitionTab()->interpolationComboBox;
// Set strong focus and add wheel event filter
interpolation->setFocusPolicy(Qt::StrongFocus);
@@ -307,12 +317,14 @@ namespace armarx
interpolation->setCurrentIndex(0);
}
- void TransitionController::changeTextListWidgetItems()
+ void
+ TransitionController::changeTextListWidgetItems()
{
QListWidget* transitions = this->guiTransitionTab->getTransitionTab()->transitionList;
for (int i = 0; i < transitions->count(); i++)
{
- transitions->item(i)->setText(QString::fromStdString(("Transition: " + std::to_string(i))));
+ transitions->item(i)->setText(
+ QString::fromStdString(("Transition: " + std::to_string(i))));
}
}
-}
+} // namespace armarx
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/TransitionController.h b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/TransitionController.h
index e5a4e4d977615d4dfd8747b6365e56740fa3b1e4..503233d8877b217c3d32e61a2faa78d4fca63cfc 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/TransitionController.h
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/TransitionController.h
@@ -21,15 +21,14 @@
*/
#ifndef TRANSITIONCONTROLLER_H
#define TRANSITIONCONTROLLER_H
-#include "AbstractController.h"
-#include "../View/TransitionTab.h"
-#include "../Util/WheelEventFilter.h"
-
+#include <QDoubleValidator>
#include <QListWidget>
#include <QMetaType>
#include <QVariant>
-#include <QDoubleValidator>
+#include "../Util/WheelEventFilter.h"
+#include "../View/TransitionTab.h"
+#include "AbstractController.h"
namespace armarx
{
@@ -48,11 +47,11 @@ namespace armarx
* @param transition GuiTransition to compare with
* @return true, iff all values of both GuiTransitions are equal, otherwise false
*/
- bool operator==(GuiTransition transition)
+ bool
+ operator==(GuiTransition transition)
{
- return (duration == transition.duration)
- && (start == transition.start)
- && (it == transition.it);
+ return (duration == transition.duration) && (start == transition.start) &&
+ (it == transition.it);
}
/**
@@ -60,11 +59,10 @@ namespace armarx
* are greater than or equal to zero
* @return true iff the transition is valid
*/
- bool isValid()
+ bool
+ isValid()
{
- return (duration >= 0)
- && (start >= 0)
- && (it >= 0);
+ return (duration >= 0) && (start >= 0) && (it >= 0);
}
} GuiTransition;
@@ -232,7 +230,7 @@ namespace armarx
};
using TransitionControllerPtr = std::shared_ptr<TransitionController>;
-}
+} // namespace armarx
Q_DECLARE_METATYPE(armarx::GuiTransition)
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/ViewController.cpp b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/ViewController.cpp
index a58775c74a422d86c9dc254ef61d81cd5f8fd38b..26e0b5087aa372ce440a21974345f0cf95a8dbd6 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/ViewController.cpp
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/ViewController.cpp
@@ -23,7 +23,8 @@
namespace armarx
{
- void ViewController::onInitComponent()
+ void
+ ViewController::onInitComponent()
{
ARMARX_INFO << "RobotTrajectoryDesigner: ViewController on disconnect";
@@ -33,27 +34,35 @@ namespace armarx
enableAddRemoveViewButton();
}
- void ViewController::onConnectComponent()
+ void
+ ViewController::onConnectComponent()
{
ARMARX_INFO << "RobotTrajectoryDesigner: ViewController on connect";
// Set perspective (index)
QObject::connect(guiPerspectives->getPerspectives()->perspectiveComboBox,
SIGNAL(activated(int)),
- this, SLOT(setViewPerspective(int)));
+ this,
+ SLOT(setViewPerspective(int)));
QObject::connect(guiPerspectives->getPerspectives()->addViewButton,
- SIGNAL(clicked()), this, SLOT(addViewSlot()));
+ SIGNAL(clicked()),
+ this,
+ SLOT(addViewSlot()));
QObject::connect(guiPerspectives->getPerspectives()->deleteViewButton,
- SIGNAL(clicked()), this, SLOT(removeViewSlot()));
+ SIGNAL(clicked()),
+ this,
+ SLOT(removeViewSlot()));
}
- void ViewController::onDisconnectComponent()
+ void
+ ViewController::onDisconnectComponent()
{
ARMARX_INFO << "RobotTrajectoryDesigner: ViewController on disconnect";
}
- void ViewController::onExitComponent()
+ void
+ ViewController::onExitComponent()
{
ARMARX_INFO << "RobotTrajectoryDesigner: ViewController on exit";
}
@@ -65,41 +74,48 @@ namespace armarx
onConnectComponent();
}
- PerspectivesPtr ViewController::getGuiPerspectives()
+ PerspectivesPtr
+ ViewController::getGuiPerspectives()
{
return this->guiPerspectives;
}
- void ViewController::setGuiPerspectives(PerspectivesPtr guiPerspectives)
+ void
+ ViewController::setGuiPerspectives(PerspectivesPtr guiPerspectives)
{
this->guiPerspectives = guiPerspectives;
}
- void ViewController::setViewPerspective(int index)
+ void
+ ViewController::setViewPerspective(int index)
{
emit changedPerspective(index);
}
- void ViewController::retranslateGui()
+ void
+ ViewController::retranslateGui()
{
- throw ("not yet implemented");
+ throw("not yet implemented");
}
- void ViewController::addViewSlot()
+ void
+ ViewController::addViewSlot()
{
numberViews++;
emit addView();
enableAddRemoveViewButton();
}
- void ViewController::removeViewSlot()
+ void
+ ViewController::removeViewSlot()
{
numberViews--;
emit removeView();
enableAddRemoveViewButton();
}
- void ViewController::initPerspectivesCombobox()
+ void
+ ViewController::initPerspectivesCombobox()
{
QComboBox* perspectives = guiPerspectives->getPerspectives()->perspectiveComboBox;
@@ -119,7 +135,8 @@ namespace armarx
perspectives->setCurrentIndex(0);
}
- void ViewController::enableAddRemoveViewButton()
+ void
+ ViewController::enableAddRemoveViewButton()
{
if (numberViews == 1)
{
@@ -137,4 +154,4 @@ namespace armarx
guiPerspectives->getPerspectives()->deleteViewButton->setEnabled(true);
}
}
-}
+} // namespace armarx
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/ViewController.h b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/ViewController.h
index 5e94fe31c4ef5c897b78b69cbcfe0d3ce232d02a..b14248395aa5e01745b813e5cb63a0941b215f0d 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/ViewController.h
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/ViewController.h
@@ -21,13 +21,14 @@
*/
#ifndef VIEWCONTROLLER_H
#define VIEWCONTROLLER_H
-#include "AbstractController.h"
-#include "../View/Perspectives.h"
-#include "../Util/WheelEventFilter.h"
-#include <QMouseEvent>
+#include <QCheckBox>
#include <QComboBox>
+#include <QMouseEvent>
#include <QPushButton>
-#include <QCheckBox>
+
+#include "../Util/WheelEventFilter.h"
+#include "../View/Perspectives.h"
+#include "AbstractController.h"
namespace armarx
{
@@ -129,6 +130,6 @@ namespace armarx
};
using ViewControllerPtr = std::shared_ptr<ViewController>;
-}
+} // namespace armarx
#endif // VIEWCONTROLLER_H
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/WaypointController.cpp b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/WaypointController.cpp
index ba11ed0a2cceeb19bad93c8f2dfa220eae10f5be..122fdb738fd932ad0869899cad4d3e8221d026a1 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/WaypointController.cpp
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/WaypointController.cpp
@@ -20,11 +20,13 @@
* GNU General Public License
*/
#include "WaypointController.h"
+
#include "VirtualRobot/IK/IKSolver.h"
namespace armarx
{
- void WaypointController::onInitComponent()
+ void
+ WaypointController::onInitComponent()
{
ARMARX_INFO << "RobotTrajectoryDesigner: WaypointController on init";
@@ -34,77 +36,91 @@ namespace armarx
initValidator(-10000.000, 10000.000, 3);
// Set single selection and clear waypoint list
- this->guiWaypointTab->getWaypointTab()->waypointList->
- setSelectionMode(QAbstractItemView::SingleSelection);
+ this->guiWaypointTab->getWaypointTab()->waypointList->setSelectionMode(
+ QAbstractItemView::SingleSelection);
this->guiWaypointTab->getWaypointTab()->waypointList->clear();
}
- void WaypointController::onConnectComponent()
+ void
+ WaypointController::onConnectComponent()
{
ARMARX_INFO << "RobotTrajectoryDesigner: WaypointController on connect";
// Waypoint: update selected waypoint
QObject::connect(guiWaypointTab->getWaypointTab()->waypointList,
SIGNAL(itemClicked(QListWidgetItem*)),
- this, SLOT(updateSelectedWaypoint(QListWidgetItem*)));
+ this,
+ SLOT(updateSelectedWaypoint(QListWidgetItem*)));
// Waypoint: change x-coordinate
QObject::connect(guiWaypointTab->getWaypointTab()->xPositionLineEdit,
SIGNAL(editingFinished()),
- this, SLOT(setXCoordinate()));
+ this,
+ SLOT(setXCoordinate()));
// Waypoint: change y-coordinate
QObject::connect(guiWaypointTab->getWaypointTab()->yPositionLineEdit,
SIGNAL(editingFinished()),
- this, SLOT(setYCoordinate()));
+ this,
+ SLOT(setYCoordinate()));
// Waypoint: change x-coordinate
QObject::connect(guiWaypointTab->getWaypointTab()->zPositionLineEdit,
SIGNAL(editingFinished()),
- this, SLOT(setZCoordinate()));
+ this,
+ SLOT(setZCoordinate()));
// Waypoint: change euler angle roll
QObject::connect(guiWaypointTab->getWaypointTab()->eulerRLineEdit,
SIGNAL(editingFinished()),
- this, SLOT(setEulerAngleR()));
+ this,
+ SLOT(setEulerAngleR()));
// Waypoint: change euler angle pitch
QObject::connect(guiWaypointTab->getWaypointTab()->eulerPLineEdit,
SIGNAL(editingFinished()),
- this, SLOT(setEulerAngleP()));
+ this,
+ SLOT(setEulerAngleP()));
// Waypoint: change euler angle yaw
QObject::connect(guiWaypointTab->getWaypointTab()->eulerYLineEdit,
SIGNAL(editingFinished()),
- this, SLOT(setEulerAngleY()));
+ this,
+ SLOT(setEulerAngleY()));
// Waypoint: change cartesian selection
QObject::connect(guiWaypointTab->getWaypointTab()->ikSelectionComboBox,
SIGNAL(activated(int)),
- this, SLOT(setCartesianSelection(int)));
+ this,
+ SLOT(setCartesianSelection(int)));
// Waypoint: is breakpoint
QObject::connect(guiWaypointTab->getWaypointTab()->isBreakpointCheckBox,
SIGNAL(clicked(bool)),
- this, SLOT(setBreakpoint(bool)));
+ this,
+ SLOT(setBreakpoint(bool)));
// Waypoint: add waypoint
QObject::connect(guiWaypointTab->getWaypointTab()->insertButton,
SIGNAL(clicked()),
- this, SLOT(addWaypoint()));
+ this,
+ SLOT(addWaypoint()));
// Waypoint: remove waypoint
QObject::connect(guiWaypointTab->getWaypointTab()->deleteButton,
SIGNAL(clicked()),
- this, SLOT(removeWaypoint()));
+ this,
+ SLOT(removeWaypoint()));
}
- void WaypointController::onDisconnectComponent()
+ void
+ WaypointController::onDisconnectComponent()
{
ARMARX_INFO << "RobotTrajectoryDesigner: WaypointController on disconnect";
}
- void WaypointController::onExitComponent()
+ void
+ WaypointController::onExitComponent()
{
ARMARX_INFO << "RobotTrajectoryDesigner: WaypointController on exit";
}
@@ -116,12 +132,14 @@ namespace armarx
onConnectComponent();
}
- WaypointTabPtr WaypointController::getGuiWaypointTab()
+ WaypointTabPtr
+ WaypointController::getGuiWaypointTab()
{
return this->guiWaypointTab;
}
- void WaypointController::setGuiWaypointTab(WaypointTabPtr guiWaypointTab)
+ void
+ WaypointController::setGuiWaypointTab(WaypointTabPtr guiWaypointTab)
{
if (guiWaypointTab != NULL)
{
@@ -129,11 +147,11 @@ namespace armarx
}
}
- void WaypointController::addWaypoint(
- int index,
- std::vector<double> values,
- int cartesianSelection,
- bool isBreakpoint)
+ void
+ WaypointController::addWaypoint(int index,
+ std::vector<double> values,
+ int cartesianSelection,
+ bool isBreakpoint)
{
if (values.size() == 6)
{
@@ -148,7 +166,8 @@ namespace armarx
data.setValue(waypoint);
// add data to item and insert
- QListWidgetItem* item = new QListWidgetItem(QString::fromStdString("Waypoint: " + std::to_string(index)));
+ QListWidgetItem* item =
+ new QListWidgetItem(QString::fromStdString("Waypoint: " + std::to_string(index)));
item->setData(Qt::UserRole, data);
QListWidget* waypoints = guiWaypointTab->getWaypointTab()->waypointList;
@@ -164,7 +183,8 @@ namespace armarx
changeTextListWidgetItems();
}
- void WaypointController::removeWaypoint(int index)
+ void
+ WaypointController::removeWaypoint(int index)
{
QListWidget* waypoints = guiWaypointTab->getWaypointTab()->waypointList;
@@ -182,7 +202,8 @@ namespace armarx
changeTextListWidgetItems();
}
- void WaypointController::updateSelectedWaypoint(QListWidgetItem* item)
+ void
+ WaypointController::updateSelectedWaypoint(QListWidgetItem* item)
{
GuiWaypoint waypoint = item->data(Qt::UserRole).value<GuiWaypoint>();
@@ -197,7 +218,8 @@ namespace armarx
emit setCurrentIndexRobotVisualization(row);
}
- void WaypointController::updateSelectedWaypoint(int index)
+ void
+ WaypointController::updateSelectedWaypoint(int index)
{
QListWidgetItem* item = guiWaypointTab->getWaypointTab()->waypointList->item(index);
GuiWaypoint waypoint = item->data(Qt::UserRole).value<GuiWaypoint>();
@@ -211,7 +233,8 @@ namespace armarx
emit setCurrentIndex(index);
}
- void WaypointController::setXCoordinate()
+ void
+ WaypointController::setXCoordinate()
{
// Check whether input is valid
if (guiWaypointTab->getWaypointTab()->xPositionLineEdit->hasAcceptableInput())
@@ -223,12 +246,10 @@ namespace armarx
if (item != NULL)
{
GuiWaypoint waypoint = item->data(Qt::UserRole).value<GuiWaypoint>();
- double x = guiWaypointTab->getWaypointTab()->
- xPositionLineEdit->text().toDouble();
+ double x = guiWaypointTab->getWaypointTab()->xPositionLineEdit->text().toDouble();
// Check if x-coordinate of waypoint has changed
- if ((waypoint.values.size() == 6)
- && (x != waypoint.values[0]))
+ if ((waypoint.values.size() == 6) && (x != waypoint.values[0]))
{
std::vector<double> values = waypoint.values;
values[0] = x;
@@ -242,7 +263,8 @@ namespace armarx
}
}
- void WaypointController::setYCoordinate()
+ void
+ WaypointController::setYCoordinate()
{
// Check whether input is valid
if (guiWaypointTab->getWaypointTab()->yPositionLineEdit->hasAcceptableInput())
@@ -254,12 +276,10 @@ namespace armarx
if (item != NULL)
{
GuiWaypoint waypoint = item->data(Qt::UserRole).value<GuiWaypoint>();
- double y = guiWaypointTab->getWaypointTab()->
- yPositionLineEdit->text().toDouble();
+ double y = guiWaypointTab->getWaypointTab()->yPositionLineEdit->text().toDouble();
// Check if x-coordinate of waypoint has changed
- if ((waypoint.values.size() == 6)
- && (y != waypoint.values[1]))
+ if ((waypoint.values.size() == 6) && (y != waypoint.values[1]))
{
std::vector<double> values = waypoint.values;
values[1] = y;
@@ -273,7 +293,8 @@ namespace armarx
}
}
- void WaypointController::setZCoordinate()
+ void
+ WaypointController::setZCoordinate()
{
// Check whether input is valid
if (guiWaypointTab->getWaypointTab()->zPositionLineEdit->hasAcceptableInput())
@@ -285,12 +306,10 @@ namespace armarx
if (item != NULL)
{
GuiWaypoint waypoint = item->data(Qt::UserRole).value<GuiWaypoint>();
- double z = guiWaypointTab->getWaypointTab()->
- xPositionLineEdit->text().toDouble();
+ double z = guiWaypointTab->getWaypointTab()->xPositionLineEdit->text().toDouble();
// Check if x-coordinate of waypoint has changed
- if ((waypoint.values.size() == 6)
- && (z != waypoint.values[2]))
+ if ((waypoint.values.size() == 6) && (z != waypoint.values[2]))
{
std::vector<double> values = waypoint.values;
values[2] = z;
@@ -304,7 +323,8 @@ namespace armarx
}
}
- void WaypointController::setEulerAngleR()
+ void
+ WaypointController::setEulerAngleR()
{
// Check whether input is valid
if (guiWaypointTab->getWaypointTab()->eulerRLineEdit->hasAcceptableInput())
@@ -316,12 +336,10 @@ namespace armarx
if (item != NULL)
{
GuiWaypoint waypoint = item->data(Qt::UserRole).value<GuiWaypoint>();
- double r = guiWaypointTab->getWaypointTab()->
- eulerRLineEdit->text().toDouble();
+ double r = guiWaypointTab->getWaypointTab()->eulerRLineEdit->text().toDouble();
// Check if x-coordinate of waypoint has changed
- if ((waypoint.values.size() == 6)
- && (r != waypoint.values[3]))
+ if ((waypoint.values.size() == 6) && (r != waypoint.values[3]))
{
std::vector<double> values = waypoint.values;
values[3] = r;
@@ -335,7 +353,8 @@ namespace armarx
}
}
- void WaypointController::setEulerAngleP()
+ void
+ WaypointController::setEulerAngleP()
{
// Check whether input is valid
if (guiWaypointTab->getWaypointTab()->eulerPLineEdit->hasAcceptableInput())
@@ -347,12 +366,10 @@ namespace armarx
if (item != NULL)
{
GuiWaypoint waypoint = item->data(Qt::UserRole).value<GuiWaypoint>();
- double p = guiWaypointTab->getWaypointTab()->
- eulerPLineEdit->text().toDouble();
+ double p = guiWaypointTab->getWaypointTab()->eulerPLineEdit->text().toDouble();
// Check if x-coordinate of waypoint has changed
- if ((waypoint.values.size() == 6)
- && (p != waypoint.values[4]))
+ if ((waypoint.values.size() == 6) && (p != waypoint.values[4]))
{
std::vector<double> values = waypoint.values;
values[4] = p;
@@ -366,7 +383,8 @@ namespace armarx
}
}
- void WaypointController::setEulerAngleY()
+ void
+ WaypointController::setEulerAngleY()
{
// Check whether input is valid
if (guiWaypointTab->getWaypointTab()->eulerYLineEdit->hasAcceptableInput())
@@ -378,12 +396,10 @@ namespace armarx
if (item != NULL)
{
GuiWaypoint waypoint = item->data(Qt::UserRole).value<GuiWaypoint>();
- double y = guiWaypointTab->getWaypointTab()->
- eulerYLineEdit->text().toDouble();
+ double y = guiWaypointTab->getWaypointTab()->eulerYLineEdit->text().toDouble();
// Check if x-coordinate of waypoint has changed
- if ((waypoint.values.size() == 6)
- && (y != waypoint.values[5]))
+ if ((waypoint.values.size() == 6) && (y != waypoint.values[5]))
{
std::vector<double> values = waypoint.values;
values[5] = y;
@@ -397,7 +413,8 @@ namespace armarx
}
}
- void WaypointController::setCartesianSelection(int cs)
+ void
+ WaypointController::setCartesianSelection(int cs)
{
QListWidget* waypoints = guiWaypointTab->getWaypointTab()->waypointList;
QListWidgetItem* item = waypoints->currentItem();
@@ -408,8 +425,9 @@ namespace armarx
GuiWaypoint waypoint = item->data(Qt::UserRole).value<GuiWaypoint>();
// Check if x-coordinate of waypoint has changed
- int cartesianSelection = guiWaypointTab->getWaypointTab()->
- ikSelectionComboBox->itemData(cs, Qt::UserRole).toInt();
+ int cartesianSelection = guiWaypointTab->getWaypointTab()
+ ->ikSelectionComboBox->itemData(cs, Qt::UserRole)
+ .toInt();
if (cartesianSelection != waypoint.cartesianSelection)
{
emit changedWaypoint(waypoints->currentRow(), cartesianSelection);
@@ -417,7 +435,8 @@ namespace armarx
}
}
- void WaypointController::setBreakpoint(bool isBreakpoint)
+ void
+ WaypointController::setBreakpoint(bool isBreakpoint)
{
QListWidget* waypoints = guiWaypointTab->getWaypointTab()->waypointList;
QListWidgetItem* item = waypoints->currentItem();
@@ -435,11 +454,11 @@ namespace armarx
}
}
- void WaypointController::setWaypointData(
- int index,
- std::vector<double> values,
- int cartesianSelection,
- bool isBreakpoint)
+ void
+ WaypointController::setWaypointData(int index,
+ std::vector<double> values,
+ int cartesianSelection,
+ bool isBreakpoint)
{
// Create struct holding relevant data
struct GuiWaypoint waypoint;
@@ -464,33 +483,30 @@ namespace armarx
updateWaypointElements(waypoint);
}
}
-
}
- void WaypointController::addWaypoint()
+ void
+ WaypointController::addWaypoint()
{
if (guiWaypointTab->getWaypointTab()->waypointList->count() == 0)
{
emit addedWaypoint(guiWaypointTab->getWaypointTab()->waypointList->count(),
- guiWaypointTab->getWaypointTab()->
- insertAfterButton->isChecked());
+ guiWaypointTab->getWaypointTab()->insertAfterButton->isChecked());
}
else if (guiWaypointTab->getWaypointTab()->waypointList->currentRow() == -1)
{
emit addedWaypoint(guiWaypointTab->getWaypointTab()->waypointList->count() - 1,
- guiWaypointTab->getWaypointTab()->
- insertAfterButton->isChecked());
+ guiWaypointTab->getWaypointTab()->insertAfterButton->isChecked());
}
else
{
- emit addedWaypoint(guiWaypointTab->getWaypointTab()->
- waypointList->currentRow(),
- guiWaypointTab->getWaypointTab()->
- insertAfterButton->isChecked());
+ emit addedWaypoint(guiWaypointTab->getWaypointTab()->waypointList->currentRow(),
+ guiWaypointTab->getWaypointTab()->insertAfterButton->isChecked());
}
}
- void WaypointController::removeWaypoint()
+ void
+ WaypointController::removeWaypoint()
{
QListWidget* waypoints = this->guiWaypointTab->getWaypointTab()->waypointList;
if (waypoints->count() > 0)
@@ -500,12 +516,14 @@ namespace armarx
}
}
- void WaypointController::retranslateGui()
+ void
+ WaypointController::retranslateGui()
{
ARMARX_INFO << "not yet implemented";
}
- void WaypointController::clearWaypointList()
+ void
+ WaypointController::clearWaypointList()
{
// Clear waypoint list
this->guiWaypointTab->getWaypointTab()->waypointList->clear();
@@ -527,21 +545,24 @@ namespace armarx
emit setCurrentIndex(0);
}
- void WaypointController::enableDeleteButton(bool enable)
+ void
+ WaypointController::enableDeleteButton(bool enable)
{
guiWaypointTab->getWaypointTab()->deleteButton->setEnabled(enable);
guiWaypointTab->getWaypointTab()->ikSelectionComboBox->setEnabled(enable);
guiWaypointTab->getWaypointTab()->isBreakpointCheckBox->setEnabled(enable);
}
- void WaypointController::enableAddButton(bool enable)
+ void
+ WaypointController::enableAddButton(bool enable)
{
guiWaypointTab->getWaypointTab()->insertButton->setEnabled(enable);
guiWaypointTab->getWaypointTab()->insertAfterButton->setEnabled(enable);
guiWaypointTab->getWaypointTab()->insertBeforeButton->setEnabled(enable);
}
- void WaypointController::enableWaypointListLineEdit(bool enable)
+ void
+ WaypointController::enableWaypointListLineEdit(bool enable)
{
guiWaypointTab->getWaypointTab()->eulerPLineEdit->setEnabled(enable);
guiWaypointTab->getWaypointTab()->eulerRLineEdit->setEnabled(enable);
@@ -555,7 +576,8 @@ namespace armarx
/************************************************************************************/
/* Private Functions */
/************************************************************************************/
- void WaypointController::initCSComboBox()
+ void
+ WaypointController::initCSComboBox()
{
QComboBox* cs = guiWaypointTab->getWaypointTab()->ikSelectionComboBox;
@@ -576,58 +598,56 @@ namespace armarx
cs->setCurrentIndex(5);
}
- void WaypointController::initValidator(double bottom, double top, int decimals)
+ void
+ WaypointController::initValidator(double bottom, double top, int decimals)
{
- QDoubleValidator* validator = new QDoubleValidator(bottom,
- top,
- decimals,
- guiWaypointTab.get());
+ QDoubleValidator* validator =
+ new QDoubleValidator(bottom, top, decimals, guiWaypointTab.get());
validator->setNotation(QDoubleValidator::StandardNotation);
- this->guiWaypointTab->getWaypointTab()->xPositionLineEdit->
- setValidator(validator);
- this->guiWaypointTab->getWaypointTab()->yPositionLineEdit->
- setValidator(validator);
- this->guiWaypointTab->getWaypointTab()->zPositionLineEdit->
- setValidator(validator);
+ this->guiWaypointTab->getWaypointTab()->xPositionLineEdit->setValidator(validator);
+ this->guiWaypointTab->getWaypointTab()->yPositionLineEdit->setValidator(validator);
+ this->guiWaypointTab->getWaypointTab()->zPositionLineEdit->setValidator(validator);
this->guiWaypointTab->getWaypointTab()->eulerRLineEdit->setValidator(validator);
this->guiWaypointTab->getWaypointTab()->eulerPLineEdit->setValidator(validator);
this->guiWaypointTab->getWaypointTab()->eulerYLineEdit->setValidator(validator);
}
- void WaypointController::updateWaypointElements(GuiWaypoint waypoint)
+ void
+ WaypointController::updateWaypointElements(GuiWaypoint waypoint)
{
if (waypoint.values.size() == 6)
{
// x-coordinate line edit
- guiWaypointTab->getWaypointTab()->xPositionLineEdit->
- setText(QString::number(waypoint.values[0]));
+ guiWaypointTab->getWaypointTab()->xPositionLineEdit->setText(
+ QString::number(waypoint.values[0]));
// y-coordinate line edit
- guiWaypointTab->getWaypointTab()->yPositionLineEdit->
- setText(QString::number(waypoint.values[1]));
+ guiWaypointTab->getWaypointTab()->yPositionLineEdit->setText(
+ QString::number(waypoint.values[1]));
// z-coordinate line edit
- guiWaypointTab->getWaypointTab()->zPositionLineEdit->
- setText(QString::number(waypoint.values[2]));
+ guiWaypointTab->getWaypointTab()->zPositionLineEdit->setText(
+ QString::number(waypoint.values[2]));
// roll euler angle line edit
- guiWaypointTab->getWaypointTab()->eulerRLineEdit->
- setText(QString::number(waypoint.values[3]));
+ guiWaypointTab->getWaypointTab()->eulerRLineEdit->setText(
+ QString::number(waypoint.values[3]));
// pitch euler angle line edit
- guiWaypointTab->getWaypointTab()->eulerPLineEdit->
- setText(QString::number(waypoint.values[4]));
+ guiWaypointTab->getWaypointTab()->eulerPLineEdit->setText(
+ QString::number(waypoint.values[4]));
// yaw euler angle line edit
- guiWaypointTab->getWaypointTab()->eulerYLineEdit->
- setText(QString::number(waypoint.values[5]));
+ guiWaypointTab->getWaypointTab()->eulerYLineEdit->setText(
+ QString::number(waypoint.values[5]));
// cartesian selection combo box
- guiWaypointTab->getWaypointTab()->ikSelectionComboBox->
- setCurrentIndex(guiWaypointTab->getWaypointTab()->ikSelectionComboBox->
- findData(QVariant(waypoint.cartesianSelection)));
+ guiWaypointTab->getWaypointTab()->ikSelectionComboBox->setCurrentIndex(
+ guiWaypointTab->getWaypointTab()->ikSelectionComboBox->findData(
+ QVariant(waypoint.cartesianSelection)));
// is breakpoint check box
- guiWaypointTab->getWaypointTab()->isBreakpointCheckBox->
- setChecked(waypoint.isBreakpoint);
+ guiWaypointTab->getWaypointTab()->isBreakpointCheckBox->setChecked(
+ waypoint.isBreakpoint);
}
}
- void WaypointController::changeTextListWidgetItems()
+ void
+ WaypointController::changeTextListWidgetItems()
{
QListWidget* waypoints = this->guiWaypointTab->getWaypointTab()->waypointList;
for (int i = 0; i < waypoints->count(); i++)
@@ -635,4 +655,4 @@ namespace armarx
waypoints->item(i)->setText(QString::fromStdString(("Waypoint: " + std::to_string(i))));
}
}
-}
+} // namespace armarx
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/WaypointController.h b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/WaypointController.h
index 12f12b911fda749a57d325b5b7f8a7fef3595258..7f79f16e55e8190874af52495c11e05ae25c1641 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/WaypointController.h
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Controller/WaypointController.h
@@ -21,14 +21,14 @@
*/
#ifndef WAYPOINTCONTROLLER_H
#define WAYPOINTCONTROLLER_H
-#include "AbstractController.h"
-#include "../View/WaypointTab.h"
-#include "../Util/WheelEventFilter.h"
-
+#include <QDoubleValidator>
#include <QListWidget>
#include <QMetaType>
#include <QVariant>
-#include <QDoubleValidator>
+
+#include "../Util/WheelEventFilter.h"
+#include "../View/WaypointTab.h"
+#include "AbstractController.h"
namespace armarx
{
@@ -108,7 +108,9 @@ namespace armarx
* @param cartesianSelection Integer representing the cartesian selection
* @param isBreakpoint Boolean determining whether the waypoint is a breakpoint
*/
- void addWaypoint(int index, std::vector<double> values, int cartesianSelection,
+ void addWaypoint(int index,
+ std::vector<double> values,
+ int cartesianSelection,
bool isBreakpoint);
/**
@@ -131,7 +133,9 @@ namespace armarx
* roll, pitch, yaw euler angles
* @param constraints Constraints of the waypoint: isBreakpoint, ikConstraints
*/
- void setWaypointData(int index, std::vector<double> values, int cartesianSelection,
+ void setWaypointData(int index,
+ std::vector<double> values,
+ int cartesianSelection,
bool isBreakpoint);
/**
@@ -306,7 +310,7 @@ namespace armarx
};
using WaypointControllerPtr = std::shared_ptr<WaypointController>;
-}
+} // namespace armarx
Q_DECLARE_METATYPE(armarx::GuiWaypoint)
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Environment.cpp b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Environment.cpp
index 29e56bc0d79c5be001d95e7087817ed101eb670a..a0234dd5a09720cd5e252b0fe7928594ca4b39e7 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Environment.cpp
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Environment.cpp
@@ -1,4 +1,5 @@
#include "Environment.h"
+
#include <ArmarXCore/core/exceptions/user/NotImplementedYetException.h>
#include <ArmarXCore/core/logging/LogSender.h>
#include <ArmarXCore/core/logging/Logging.h>
@@ -10,7 +11,8 @@ Environment::Environment()
this->scene = VirtualRobot::ScenePtr(new VirtualRobot::Scene("empty"));
}
-VirtualRobot::RobotPtr Environment::getRobot()
+VirtualRobot::RobotPtr
+Environment::getRobot()
{
if (robot != nullptr)
{
@@ -22,7 +24,8 @@ VirtualRobot::RobotPtr Environment::getRobot()
}
}
-VirtualRobot::RobotPtr Environment::getCDRobot()
+VirtualRobot::RobotPtr
+Environment::getCDRobot()
{
if (cdRobot != nullptr)
{
@@ -34,13 +37,15 @@ VirtualRobot::RobotPtr Environment::getCDRobot()
}
}
-void Environment::setRobot(const VirtualRobot::RobotPtr& value)
+void
+Environment::setRobot(const VirtualRobot::RobotPtr& value)
{
robot = value;
cdRobot = robot->clone();
}
-VirtualRobot::ScenePtr Environment::getScene()
+VirtualRobot::ScenePtr
+Environment::getScene()
{
if (scene != nullptr)
{
@@ -52,7 +57,8 @@ VirtualRobot::ScenePtr Environment::getScene()
}
}
-void Environment::setScene(const VirtualRobot::ScenePtr& value)
+void
+Environment::setScene(const VirtualRobot::ScenePtr& value)
{
scene = value;
}
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Environment.h b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Environment.h
index e1b41e1648cf3ce3fcab5b966962b750e09daf8a..b801e5660e34db8bba3ae7dd474f2254631e0fad 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Environment.h
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Environment.h
@@ -4,7 +4,6 @@
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/Scene.h>
-
namespace armarx
{
class Environment
@@ -26,7 +25,8 @@ namespace armarx
void setScene(const VirtualRobot::ScenePtr& value);
};
+
using EnvironmentPtr = std::shared_ptr<Environment>;
-}
+} // namespace armarx
#endif // ENVIRONMENT_H
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/ImportExport/MMMExporter.cpp b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/ImportExport/MMMExporter.cpp
index ffa6aa98fd05d12911496af3f0211972405e5fc0..fe56596ebd459045aff039fa952ace299b8af77e 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/ImportExport/MMMExporter.cpp
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/ImportExport/MMMExporter.cpp
@@ -21,27 +21,34 @@
*/
#include "MMMExporter.h"
+
+#include <clocale>
+
#include <boost/make_shared.hpp>
+
+#include <QString>
+#include <QtXml>
+
+#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
+
#include "../Model/DesignerTrajectory.h"
#include "../Model/UserWaypoint.h"
-#include <MMM/Motion/Plugin/ModelPosePlugin/ModelPoseSensor.h>
-#include <MMM/Motion/Plugin/KinematicPlugin/KinematicSensor.h>
-#include <MMM/Motion/Motion.h>
-#include <MMM/Motion/MotionWriterXML.h>
#include <MMM/Model/Model.h>
#include <MMM/Model/ModelReaderXML.h>
+#include <MMM/Motion/Motion.h>
+#include <MMM/Motion/MotionWriterXML.h>
+#include <MMM/Motion/Plugin/KinematicPlugin/KinematicSensor.h>
+#include <MMM/Motion/Plugin/ModelPosePlugin/ModelPoseSensor.h>
#include <MMM/RapidXML/rapidxml.hpp>
-#include <QString>
-#include <QtXml>
-#include <clocale>
-#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
armarx::MMMExporter::MMMExporter(EnvironmentPtr environment)
{
this->environment = environment;
}
-void armarx::MMMExporter::exportTrajectory(std::vector<armarx::DesignerTrajectoryPtr> trajectories, const std::string file)
+void
+armarx::MMMExporter::exportTrajectory(std::vector<armarx::DesignerTrajectoryPtr> trajectories,
+ const std::string file)
{
//Ensure that locale is en - punctuation for doubles in exported file
setlocale(LC_NUMERIC, "en_US.UTF-8");
@@ -54,12 +61,18 @@ void armarx::MMMExporter::exportTrajectory(std::vector<armarx::DesignerTrajector
MMM::RapidXMLWriter writer;
MMM::RapidXMLWriterNodePtr metaDataNode = writer.createRootNode("RobotTrajectoryDesignerData");
metaDataNode->append_string_node("Robot", environment->getRobot()->getFilename());
- MMM::Motion* kinematicMotion = new MMM::Motion("RobotTrajectoryDesigner", modelptr, modelptr, nullptr);
+ MMM::Motion* kinematicMotion =
+ new MMM::Motion("RobotTrajectoryDesigner", modelptr, modelptr, nullptr);
MMM::ModelPoseSensor kinematicModelPoseSensor = MMM::ModelPoseSensor("Model");
- MMM::ModelPoseSensorMeasurementPtr kinematicModelPoseSensorMeasurement = boost::make_shared<MMM::ModelPoseSensorMeasurement>(MMM::ModelPoseSensorMeasurement(0, Eigen::Vector3f(1, 1, 1), MMM::Math::matrix4fToEulerXZY(MMM::Math::quat2eigen4f(0, 0, 0, 0))));
+ MMM::ModelPoseSensorMeasurementPtr kinematicModelPoseSensorMeasurement =
+ boost::make_shared<MMM::ModelPoseSensorMeasurement>(MMM::ModelPoseSensorMeasurement(
+ 0,
+ Eigen::Vector3f(1, 1, 1),
+ MMM::Math::matrix4fToEulerXZY(MMM::Math::quat2eigen4f(0, 0, 0, 0))));
kinematicModelPoseSensor.addSensorMeasurement(kinematicModelPoseSensorMeasurement);
- kinematicMotion->addSensor(boost::make_shared<MMM::ModelPoseSensor>(kinematicModelPoseSensor), 0);
+ kinematicMotion->addSensor(boost::make_shared<MMM::ModelPoseSensor>(kinematicModelPoseSensor),
+ 0);
MMM::MotionPtr mptr(kinematicMotion);
motions.push_back(mptr);
@@ -70,8 +83,10 @@ void armarx::MMMExporter::exportTrajectory(std::vector<armarx::DesignerTrajector
std::string location(TRAJECTORYDESIGNER_SOURCEDIR);
location += "/data/RobotTrajectoryDesigner/Resources/UserWaypoint.xml";
MMM::ModelPtr modelptr = modelReader->loadModel(location);
- MMM::Motion* m = new MMM::Motion(trajectory->getRns()->getName(), modelptr, modelptr, nullptr);
- MMM::ModelPoseSensor modelPoseSensor = MMM::ModelPoseSensor(trajectory->getRns()->getName());
+ MMM::Motion* m =
+ new MMM::Motion(trajectory->getRns()->getName(), modelptr, modelptr, nullptr);
+ MMM::ModelPoseSensor modelPoseSensor =
+ MMM::ModelPoseSensor(trajectory->getRns()->getName());
//Get all userWayPoints
std::vector<armarx::UserWaypointPtr> userWayPoints = trajectory->getAllUserWaypoints();
/*
@@ -83,7 +98,8 @@ void armarx::MMMExporter::exportTrajectory(std::vector<armarx::DesignerTrajector
*/
//Add the userWaypoints to the ModelPoseSensor
//ARMARX_INFO << "Now creating ModelPoses";
- MMM::RapidXMLWriterNodePtr trajectoryNode = metaDataNode->append_node(trajectory->getRns()->getName());
+ MMM::RapidXMLWriterNodePtr trajectoryNode =
+ metaDataNode->append_node(trajectory->getRns()->getName());
MMM::RapidXMLWriterNodePtr userWaypointsNode = trajectoryNode->append_node("UserWaypoints");
MMM::RapidXMLWriterNodePtr transitionsNode = trajectoryNode->append_node("Transitions");
for (unsigned int index = 0; index < userWayPoints.size(); index++)
@@ -93,41 +109,70 @@ void armarx::MMMExporter::exportTrajectory(std::vector<armarx::DesignerTrajector
armarx::Vector3BasePtr pos = userWayPoint->getPose()->position;
Eigen::Vector3f position = Eigen::Vector3f(pos->x, pos->y, pos->z);
armarx::QuaternionBasePtr quaternion = userWayPoint->getPose()->orientation;
- Eigen::Matrix4f matrix = MMM::Math::quat2eigen4f(quaternion->qx, quaternion->qy, quaternion->qz, quaternion->qw);
+ Eigen::Matrix4f matrix = MMM::Math::quat2eigen4f(
+ quaternion->qx, quaternion->qy, quaternion->qz, quaternion->qw);
Eigen::Vector3f orientation = MMM::Math::matrix4fToEulerXZY(matrix);
//Create a measurement containing the reached pose
- MMM::ModelPoseSensorMeasurementPtr measurement = boost::make_shared<MMM::ModelPoseSensorMeasurement>(MMM::ModelPoseSensorMeasurement(userWayPoint->getUserTimestamp(), position, orientation));
+ MMM::ModelPoseSensorMeasurementPtr measurement =
+ boost::make_shared<MMM::ModelPoseSensorMeasurement>(MMM::ModelPoseSensorMeasurement(
+ userWayPoint->getUserTimestamp(), position, orientation));
//Add the measurement to the sensor
modelPoseSensor.addSensorMeasurement(measurement);
- MMM::RapidXMLWriterNodePtr userpointNode = userWaypointsNode->append_node("UserWaypoint");
+ MMM::RapidXMLWriterNodePtr userpointNode =
+ userWaypointsNode->append_node("UserWaypoint");
std::string jointAngleString;
for (unsigned int j = 0; j < userWayPoints[index]->getJointAngles().size(); j++)
{
- jointAngleString += boost::lexical_cast<std::string>(userWayPoints[index]->getJointAngles()[j]) + " ";
+ jointAngleString +=
+ boost::lexical_cast<std::string>(userWayPoints[index]->getJointAngles()[j]) +
+ " ";
}
- MMM::RapidXMLWriterNodePtr jointAngleNode = userpointNode->append_string_node("JointAngles", jointAngleString.substr(0, jointAngleString.size() - 1));
- MMM::RapidXMLWriterNodePtr timeOptimalTimeStampNode = userpointNode->append_string_node("TimeOptimalTimeStamp", boost::lexical_cast<std::string>((userWayPoints[index]->getTimeOptimalTimestamp())));
- MMM::RapidXMLWriterNodePtr breakpointNode = userpointNode->append_string_node("IsBreakPoint", std::to_string((int)(userWayPoints[index]->getIsTimeOptimalBreakpoint())));
- MMM::RapidXMLWriterNodePtr cartesianSelectionNode = userpointNode->append_string_node("CartesianSelection", std::to_string((int)(userWayPoints[index]->getIKSelection())));
+ MMM::RapidXMLWriterNodePtr jointAngleNode = userpointNode->append_string_node(
+ "JointAngles", jointAngleString.substr(0, jointAngleString.size() - 1));
+ MMM::RapidXMLWriterNodePtr timeOptimalTimeStampNode = userpointNode->append_string_node(
+ "TimeOptimalTimeStamp",
+ boost::lexical_cast<std::string>(
+ (userWayPoints[index]->getTimeOptimalTimestamp())));
+ MMM::RapidXMLWriterNodePtr breakpointNode = userpointNode->append_string_node(
+ "IsBreakPoint",
+ std::to_string((int)(userWayPoints[index]->getIsTimeOptimalBreakpoint())));
+ MMM::RapidXMLWriterNodePtr cartesianSelectionNode = userpointNode->append_string_node(
+ "CartesianSelection",
+ std::to_string((int)(userWayPoints[index]->getIKSelection())));
}
- for (unsigned int index = 0; index < trajectory->getNrOfUserWaypoints() - 1; index ++)
+ for (unsigned int index = 0; index < trajectory->getNrOfUserWaypoints() - 1; index++)
{
MMM::RapidXMLWriterNodePtr transitionNode = transitionsNode->append_node("Transition");
- MMM::RapidXMLWriterNodePtr timeOptimalDurationNode = transitionNode->append_string_node("TimeOptimalDuration", boost::lexical_cast<std::string>(trajectory->getTransition(index)->getTimeOptimalDuration()));
- MMM::RapidXMLWriterNodePtr userDurationNode = transitionNode->append_string_node("UserDuration", boost::lexical_cast<std::string>(trajectory->getTransition(index)->getUserDuration()));
- MMM::RapidXMLWriterNodePtr interpolationTypeNode = transitionNode->append_string_node("InterpolationType", std::to_string((int)(trajectory->getTransition(index)->getInterpolationType())));
+ MMM::RapidXMLWriterNodePtr timeOptimalDurationNode = transitionNode->append_string_node(
+ "TimeOptimalDuration",
+ boost::lexical_cast<std::string>(
+ trajectory->getTransition(index)->getTimeOptimalDuration()));
+ MMM::RapidXMLWriterNodePtr userDurationNode = transitionNode->append_string_node(
+ "UserDuration",
+ boost::lexical_cast<std::string>(
+ trajectory->getTransition(index)->getUserDuration()));
+ MMM::RapidXMLWriterNodePtr interpolationTypeNode = transitionNode->append_string_node(
+ "InterpolationType",
+ std::to_string((int)(trajectory->getTransition(index)->getInterpolationType())));
}
- MMM::RapidXMLWriterNodePtr interBreakpointTrajectoriesNode = trajectoryNode->append_node("InterBreakpointTrajectories");
- for (unsigned int index = 0; index < trajectory->getInterBreakpointTrajectories().size(); index++)
+ MMM::RapidXMLWriterNodePtr interBreakpointTrajectoriesNode =
+ trajectoryNode->append_node("InterBreakpointTrajectories");
+ for (unsigned int index = 0; index < trajectory->getInterBreakpointTrajectories().size();
+ index++)
{
- MMM::RapidXMLWriterNodePtr interBreakpointTrajectoryNode = interBreakpointTrajectoriesNode->append_node("InterBreakpointTrajectory");
- MMM::KinematicSensorPtr interBreakpointSensor(new MMM::KinematicSensor(trajectory->getRns()->getNodeNames(), trajectory->getRns()->getName()));
+ MMM::RapidXMLWriterNodePtr interBreakpointTrajectoryNode =
+ interBreakpointTrajectoriesNode->append_node("InterBreakpointTrajectory");
+ MMM::KinematicSensorPtr interBreakpointSensor(new MMM::KinematicSensor(
+ trajectory->getRns()->getNodeNames(), trajectory->getRns()->getName()));
for (double t : trajectory->getInterBreakpointTrajectories()[index]->getTimestamps())
{
- std::vector<double> positions = trajectory->getInterBreakpointTrajectories()[index]->getStates(t, 0);
- const Eigen::VectorXd vectord = Eigen::VectorXd::Map(positions.data(), positions.size());
+ std::vector<double> positions =
+ trajectory->getInterBreakpointTrajectories()[index]->getStates(t, 0);
+ const Eigen::VectorXd vectord =
+ Eigen::VectorXd::Map(positions.data(), positions.size());
const Eigen::VectorXf vector = vectord.cast<float>();
- MMM::KinematicSensorMeasurement* measurement = new MMM::KinematicSensorMeasurement(t, vector);
+ MMM::KinematicSensorMeasurement* measurement =
+ new MMM::KinematicSensorMeasurement(t, vector);
MMM::KinematicSensorMeasurementPtr measurementptr(measurement);
interBreakpointSensor->addSensorMeasurement(measurementptr);
}
@@ -141,15 +186,18 @@ void armarx::MMMExporter::exportTrajectory(std::vector<armarx::DesignerTrajector
MMM::MotionPtr mptr(m);
motions.push_back(mptr);
- MMM::KinematicSensorPtr kinematicSensor(new MMM::KinematicSensor(trajectory->getRns()->getNodeNames(), trajectory->getRns()->getName()));
- /*100FPS*///for (double t = 0; t < trajectory->getFinalTrajectory()->getLength(); t += 0.01)
+ MMM::KinematicSensorPtr kinematicSensor(new MMM::KinematicSensor(
+ trajectory->getRns()->getNodeNames(), trajectory->getRns()->getName()));
+ /*100FPS*/ //for (double t = 0; t < trajectory->getFinalTrajectory()->getLength(); t += 0.01)
//Model implements fps conversion for finalTrajectory
for (double t : trajectory->getFinalTrajectory()->getTimestamps())
{
std::vector<double> positions = trajectory->getFinalTrajectory()->getStates(t, 0);
- const Eigen::VectorXd vectord = Eigen::VectorXd::Map(positions.data(), positions.size());
+ const Eigen::VectorXd vectord =
+ Eigen::VectorXd::Map(positions.data(), positions.size());
const Eigen::VectorXf vector = vectord.cast<float>();
- MMM::KinematicSensorMeasurement* measurement = new MMM::KinematicSensorMeasurement(t, vector);
+ MMM::KinematicSensorMeasurement* measurement =
+ new MMM::KinematicSensorMeasurement(t, vector);
MMM::KinematicSensorMeasurementPtr measurementptr(measurement);
kinematicSensor->addSensorMeasurement(measurementptr);
}
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/ImportExport/MMMExporter.h b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/ImportExport/MMMExporter.h
index 8165f35439c8ecbfcf6b0d00305619ec43c9f428..3525c19fefdba0459a34f7461e26a32df8ec190d 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/ImportExport/MMMExporter.h
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/ImportExport/MMMExporter.h
@@ -21,19 +21,22 @@
*/
#ifndef MMMEXPORTER_H
#define MMMEXPORTER_H
-#include "../Model/DesignerTrajectory.h"
#include "../Environment.h"
+#include "../Model/DesignerTrajectory.h"
+
namespace armarx
{
class MMMExporter;
+
/**
* @class MMMExporter
* @brief Exports instances of the DesignerTrajectory class to .xml-files in MMM format.
*/
- class MMMExporter/* : public DesignerExporter*/
+ class MMMExporter /* : public DesignerExporter*/
{
private:
EnvironmentPtr environment;
+
public:
MMMExporter(EnvironmentPtr environment);
/**
@@ -41,9 +44,10 @@ namespace armarx
* @param trajectory Trajectory to export.
* @param fps Frequency of the measurements in measurements per second.
*/
- void exportTrajectory(std::vector<armarx::DesignerTrajectoryPtr> trajectories, const std::string file);
+ void exportTrajectory(std::vector<armarx::DesignerTrajectoryPtr> trajectories,
+ const std::string file);
};
using MMMExporterPtr = std::shared_ptr<MMMExporter>;
-}
+} // namespace armarx
#endif // MMMEXPORTER_H
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/ImportExport/MMMImporter.cpp b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/ImportExport/MMMImporter.cpp
index 1857370902886c5afdfad5b0f78941336007b70a..dc1d144897d581b061af64760945fcc80efc4ceb 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/ImportExport/MMMImporter.cpp
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/ImportExport/MMMImporter.cpp
@@ -20,20 +20,24 @@
* GNU General Public License
*/
#include "MMMImporter.h"
-#include <MMM/Motion/Plugin/KinematicPlugin/KinematicSensor.h>
-#include <MMM/Motion/Plugin/ModelPosePlugin/ModelPoseSensor.h>
+
+#include <iostream>
+#include <sstream>
+
+#include <boost/lexical_cast.hpp>
+#include <boost/make_shared.hpp>
+
+#include <VirtualRobot/RobotNodeSet.h>
+
#include "../Environment.h"
#include "../KinematicSolver.h"
-#include <MMM/RapidXML/rapidxml.hpp>
-#include <VirtualRobot/RobotNodeSet.h>
#include "../Model/UserWaypoint.h"
-#include <boost/make_shared.hpp>
-#include <MMM/RapidXML/RapidXMLReader.h>
#include <MMM/Motion/MotionReaderXML.h>
-#include <sstream>
-#include <iostream>
-#include <boost/lexical_cast.hpp>
+#include <MMM/Motion/Plugin/KinematicPlugin/KinematicSensor.h>
#include <MMM/Motion/Plugin/KinematicPlugin/KinematicSensorFactory.h>
+#include <MMM/Motion/Plugin/ModelPosePlugin/ModelPoseSensor.h>
+#include <MMM/RapidXML/RapidXMLReader.h>
+#include <MMM/RapidXML/rapidxml.hpp>
static constexpr const char* NODE_METADATA = "RobotTrajectoryDesignerData";
static constexpr const char* NODE_USERWAYPOINTS = "UserWaypoints";
@@ -56,10 +60,10 @@ static constexpr const char* NODE_ROBOT = "Robot";
armarx::MMMImporter::MMMImporter(armarx::EnvironmentPtr environment) : environment(environment)
{
-
}
-std::vector<armarx::DesignerTrajectoryPtr> armarx::MMMImporter::importTrajectory(std::string file)
+std::vector<armarx::DesignerTrajectoryPtr>
+armarx::MMMImporter::importTrajectory(std::string file)
{
//ARMARX_INFO << "Creating MotionReader";
std::vector<std::string> libpaths;
@@ -82,7 +86,8 @@ std::vector<armarx::DesignerTrajectoryPtr> armarx::MMMImporter::importTrajectory
}
for (MMM::MotionPtr motion : motions)
{
- if (motion->getSensorsByType("ModelPoseSensor").size() > 1) //Assert that one ModelPoseSensor or none (one RobotNodeset or none) are affected
+ if (motion->getSensorsByType("ModelPoseSensor").size() >
+ 1) //Assert that one ModelPoseSensor or none (one RobotNodeset or none) are affected
{
//ARMARX_INFO << "Motion with multiple RobotNodeSets";
//Ignore every sensor except the first one
@@ -93,7 +98,8 @@ std::vector<armarx::DesignerTrajectoryPtr> armarx::MMMImporter::importTrajectory
{
break;
}
- MMM::ModelPoseSensor mp = dynamic_cast<MMM::ModelPoseSensor&>(*sensor); //Will throw if sensor is not a modelposesensor
+ MMM::ModelPoseSensor mp = dynamic_cast<MMM::ModelPoseSensor&>(
+ *sensor); //Will throw if sensor is not a modelposesensor
MMM::ModelPoseSensorPtr modelPoseSensor = boost::make_shared<MMM::ModelPoseSensor>(mp);
if (!environment->getRobot()->hasRobotNodeSet(motion->getName()))
{
@@ -102,12 +108,16 @@ std::vector<armarx::DesignerTrajectoryPtr> armarx::MMMImporter::importTrajectory
continue;
}
//ARMARX_INFO << motion->getName() << " being imported";
- VirtualRobot::RobotNodeSetPtr rns = environment->getRobot()->getRobotNodeSet(motion->getName());
- MMM::RapidXMLReaderNodePtr trajectoryNode = metaDataNode->first_node(motion->getName().c_str());
- MMM::RapidXMLReaderNodePtr userWaypointsNode = trajectoryNode->first_node(NODE_USERWAYPOINTS);
+ VirtualRobot::RobotNodeSetPtr rns =
+ environment->getRobot()->getRobotNodeSet(motion->getName());
+ MMM::RapidXMLReaderNodePtr trajectoryNode =
+ metaDataNode->first_node(motion->getName().c_str());
+ MMM::RapidXMLReaderNodePtr userWaypointsNode =
+ trajectoryNode->first_node(NODE_USERWAYPOINTS);
MMM::RapidXMLReaderNodePtr transitionsNode = trajectoryNode->first_node(NODE_TRANSITIONS);
- MMM::RapidXMLReaderNodePtr userWaypointNode = userWaypointsNode->first_node(NODE_USERWAYPOINT);
+ MMM::RapidXMLReaderNodePtr userWaypointNode =
+ userWaypointsNode->first_node(NODE_USERWAYPOINT);
MMM::RapidXMLReaderNodePtr transitionNode = transitionsNode->first_node(NODE_TRANSITION);
std::vector<float> times = modelPoseSensor->getTimesteps();
std::vector<UserWaypointPtr> userPoints;
@@ -119,13 +129,14 @@ std::vector<armarx::DesignerTrajectoryPtr> armarx::MMMImporter::importTrajectory
{
userWaypointNode = userWaypointNode->next_sibling(NODE_USERWAYPOINT);
}
- MMM::ModelPoseSensorMeasurementPtr measurement = modelPoseSensor->getDerivedMeasurement(t);
-
+ MMM::ModelPoseSensorMeasurementPtr measurement =
+ modelPoseSensor->getDerivedMeasurement(t);
//Get metaData
//ARMARX_INFO << "Getting meta data";
- MMM::RapidXMLReaderNodePtr jointAnglesNode = userWaypointNode->first_node(NODE_JOINTANGLES);
+ MMM::RapidXMLReaderNodePtr jointAnglesNode =
+ userWaypointNode->first_node(NODE_JOINTANGLES);
std::string jointAngleValues = jointAnglesNode->value();
std::vector<double> jointAngles;
std::istringstream js(jointAngleValues);
@@ -141,24 +152,30 @@ std::vector<armarx::DesignerTrajectoryPtr> armarx::MMMImporter::importTrajectory
userPoint->setUserTimestamp(t);
userPoint->setJointAngles(jointAngles);
//ARMARX_INFO << "Reading timeOptimalTimeStamp";
- MMM::RapidXMLReaderNodePtr timeOptimalTimeStampNode = userWaypointNode->first_node(NODE_TIMEOPTIMALTIMESTAMP);
- userPoint->setTimeOptimalTimestamp(boost::lexical_cast<double>(timeOptimalTimeStampNode->value()));
+ MMM::RapidXMLReaderNodePtr timeOptimalTimeStampNode =
+ userWaypointNode->first_node(NODE_TIMEOPTIMALTIMESTAMP);
+ userPoint->setTimeOptimalTimestamp(
+ boost::lexical_cast<double>(timeOptimalTimeStampNode->value()));
//ARMARX_INFO << "Reading ikSelection";
- MMM::RapidXMLReaderNodePtr ikSelectionNode = userWaypointNode->first_node(NODE_IKSELECTION);
+ MMM::RapidXMLReaderNodePtr ikSelectionNode =
+ userWaypointNode->first_node(NODE_IKSELECTION);
userPoint->setIKSelection((IKSelection)(std::stoi(ikSelectionNode->value())));
//ARMARX_INFO << "Reading isTimeOptimalBreakpoint";
- MMM::RapidXMLReaderNodePtr isTimeOptimalBreakPointNode = userWaypointNode->first_node(NODE_ISBREAKPOINT);
- userPoint->setIsTimeOptimalBreakpoint(boost::lexical_cast<bool>(isTimeOptimalBreakPointNode->value()));
+ MMM::RapidXMLReaderNodePtr isTimeOptimalBreakPointNode =
+ userWaypointNode->first_node(NODE_ISBREAKPOINT);
+ userPoint->setIsTimeOptimalBreakpoint(
+ boost::lexical_cast<bool>(isTimeOptimalBreakPointNode->value()));
if (boost::lexical_cast<bool>(isTimeOptimalBreakPointNode->value()))
{
if (t > 0 && t != times.back())
{
- breakpoints ++;
+ breakpoints++;
}
}
userPoints.push_back(userPoint);
}
- DesignerTrajectoryPtr trajectory = std::make_shared<DesignerTrajectory>(DesignerTrajectory(userPoints[0], rns));
+ DesignerTrajectoryPtr trajectory =
+ std::make_shared<DesignerTrajectory>(DesignerTrajectory(userPoints[0], rns));
for (unsigned int i = 0; i < userPoints.size(); i++)
{
if (i > 0)
@@ -175,35 +192,47 @@ std::vector<armarx::DesignerTrajectoryPtr> armarx::MMMImporter::importTrajectory
}
TransitionPtr transition = trajectory->getTransition(i);
//ARMARX_INFO << "Reading timeOptimalDuration";
- MMM::RapidXMLReaderNodePtr timeOptimalDurationNode = transitionNode->first_node(NODE_TIMEOPTIMALDURATION);
- transition->setTimeOptimalDuration(boost::lexical_cast<double>(timeOptimalDurationNode->value()));
+ MMM::RapidXMLReaderNodePtr timeOptimalDurationNode =
+ transitionNode->first_node(NODE_TIMEOPTIMALDURATION);
+ transition->setTimeOptimalDuration(
+ boost::lexical_cast<double>(timeOptimalDurationNode->value()));
//ARMARX_INFO << "Reading userDuration";
- MMM::RapidXMLReaderNodePtr userDurationNode = transitionNode->first_node(NODE_USERDURATION);
+ MMM::RapidXMLReaderNodePtr userDurationNode =
+ transitionNode->first_node(NODE_USERDURATION);
transition->setUserDuration(boost::lexical_cast<double>(userDurationNode->value()));
//ARMARX_INFO << "Reading interpolationType";
- MMM::RapidXMLReaderNodePtr interpolationTypeNode = transitionNode->first_node(NODE_INTERPOLATIONTYPE);
- transition->setInterpolationType((InterpolationType)(std::stoi(interpolationTypeNode->value())));
+ MMM::RapidXMLReaderNodePtr interpolationTypeNode =
+ transitionNode->first_node(NODE_INTERPOLATIONTYPE);
+ transition->setInterpolationType(
+ (InterpolationType)(std::stoi(interpolationTypeNode->value())));
}
//ARMARX_INFO << "Processing interbreakpoint trajectories";
std::vector<armarx::TrajectoryPtr> interBreakpointTrajectories;
- MMM::RapidXMLReaderNodePtr interBreakpointTrajectoriesNode = trajectoryNode->first_node(NODE_INTERBREAKPOINTTRAJECTORIES);
- MMM::RapidXMLReaderNodePtr interBreakpointTrajectoryNode = interBreakpointTrajectoriesNode->first_node(NODE_INTERBREAKPOINTTRAJECTORY);
+ MMM::RapidXMLReaderNodePtr interBreakpointTrajectoriesNode =
+ trajectoryNode->first_node(NODE_INTERBREAKPOINTTRAJECTORIES);
+ MMM::RapidXMLReaderNodePtr interBreakpointTrajectoryNode =
+ interBreakpointTrajectoriesNode->first_node(NODE_INTERBREAKPOINTTRAJECTORY);
//ARMARX_INFO << breakpoints << " interbreakpoint trajectories";
for (unsigned int i = 0; i < breakpoints; i++)
{
if (i > 0)
{
- interBreakpointTrajectoryNode = interBreakpointTrajectoryNode->next_sibling(NODE_INTERBREAKPOINTTRAJECTORY);
+ interBreakpointTrajectoryNode =
+ interBreakpointTrajectoryNode->next_sibling(NODE_INTERBREAKPOINTTRAJECTORY);
}
MMM::KinematicSensorFactory factory;
- MMM::SensorPtr sensorPtr = factory.createSensor(interBreakpointTrajectoryNode->first_node(MMM::XML::NODE_SENSOR));
- MMM::KinematicSensor interBreakpointTrajectorySensor = dynamic_cast<MMM::KinematicSensor&>(*sensorPtr);
- std::vector<float> interBreakpointTimeStamps = interBreakpointTrajectorySensor.getTimesteps();
+ MMM::SensorPtr sensorPtr = factory.createSensor(
+ interBreakpointTrajectoryNode->first_node(MMM::XML::NODE_SENSOR));
+ MMM::KinematicSensor interBreakpointTrajectorySensor =
+ dynamic_cast<MMM::KinematicSensor&>(*sensorPtr);
+ std::vector<float> interBreakpointTimeStamps =
+ interBreakpointTrajectorySensor.getTimesteps();
std::vector<std::vector<double>> nodeData;
//Initialize the vectors for the respective joints, fill in the first timestamp (0)
for (unsigned int i = 0; i < rns->getSize(); i++)
{
- MMM::KinematicSensorMeasurementPtr measurement = interBreakpointTrajectorySensor.getDerivedMeasurement(0);
+ MMM::KinematicSensorMeasurementPtr measurement =
+ interBreakpointTrajectorySensor.getDerivedMeasurement(0);
nodeData.push_back({measurement->getJointAngles()[i]});
}
//Fill in the data for all other timestamps into the now existing joint values
@@ -213,14 +242,16 @@ std::vector<armarx::DesignerTrajectoryPtr> armarx::MMMImporter::importTrajectory
{
continue;
}
- MMM::KinematicSensorMeasurementPtr measurement = interBreakpointTrajectorySensor.getDerivedMeasurement(t);
+ MMM::KinematicSensorMeasurementPtr measurement =
+ interBreakpointTrajectorySensor.getDerivedMeasurement(t);
Eigen::VectorXf floatPositions = measurement->getJointAngles();
std::vector<double> doublePositions;
for (int j = 0; j < floatPositions.size(); j++)
{
doublePositions.push_back(floatPositions[j]);
}
- Eigen::VectorXd positions = Eigen::VectorXd::Map(&doublePositions[0], doublePositions.size());
+ Eigen::VectorXd positions =
+ Eigen::VectorXd::Map(&doublePositions[0], doublePositions.size());
for (unsigned int i = 0; i < positions.size(); i++)
{
nodeData[i].push_back(positions[i]);
@@ -229,7 +260,8 @@ std::vector<armarx::DesignerTrajectoryPtr> armarx::MMMImporter::importTrajectory
//Get the correct names for the dimensions
Ice::StringSeq dimensionNames = rns->getNodeNames();
//Construct the armarx Trajectory from the given data
- std::vector<double>doubleTimeStamps(interBreakpointTimeStamps.begin(), interBreakpointTimeStamps.end());
+ std::vector<double> doubleTimeStamps(interBreakpointTimeStamps.begin(),
+ interBreakpointTimeStamps.end());
TrajectoryPtr traj = new Trajectory(nodeData, doubleTimeStamps, dimensionNames);
interBreakpointTrajectories.push_back(traj);
}
@@ -249,5 +281,5 @@ std::vector<armarx::DesignerTrajectoryPtr> armarx::MMMImporter::importTrajectory
//ARMARX_INFO << "Trajectory with " << designerTrajectory->getAllUserWaypoints().size() << " points";
}
return trajectories;
- throw ("Not implemented");
+ throw("Not implemented");
}
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/ImportExport/MMMImporter.h b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/ImportExport/MMMImporter.h
index 7481308bfee92faaa05b18147482d268f249a917..df3262c85b9c541cc39ec96f52db9dc162b98cc3 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/ImportExport/MMMImporter.h
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/ImportExport/MMMImporter.h
@@ -22,12 +22,13 @@
#ifndef MMMIMPORTER_H
#define MMMIMPORTER_H
-#include "../Model/DesignerTrajectory.h"
#include "../Environment.h"
+#include "../Model/DesignerTrajectory.h"
namespace armarx
{
class MMMImporter;
+
/**
* @class MMMImporter
* @brief Imports instances of the DesignerTrajectory class from a file in MMM format.
@@ -46,5 +47,5 @@ namespace armarx
};
using MMMImporterPtr = std::shared_ptr<MMMImporter>;
-}
+} // namespace armarx
#endif // MMMIMPORTER_H
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/ImportExport/TrajectoryExporter.cpp b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/ImportExport/TrajectoryExporter.cpp
index 8b8002117b78902c4db257cc5c018a51a19ac757..a870e5492a2efe3ff0efb6618c3c01a83a889658 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/ImportExport/TrajectoryExporter.cpp
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/ImportExport/TrajectoryExporter.cpp
@@ -20,17 +20,23 @@
* GNU General Public License
*/
#include "TrajectoryExporter.h"
-#include <RobotAPI/libraries/core/Trajectory.h>
-#include <iostream>
+
#include <fstream>
+#include <iostream>
+
+#include <Ice/CommunicatorF.h>
+
#include <ArmarXCore/interface/observers/Serialization.h>
#include <ArmarXCore/interface/serialization/JSONSerialization.h>
#include <ArmarXCore/util/json/JSONObject.h>
-#include <Ice/CommunicatorF.h>
+
+#include <RobotAPI/libraries/core/Trajectory.h>
//@Liran ich (Tim) habe es von DesignerTrajectory zu DesignerTrajectoryPtr geändert. Ich bekomme es so von dem Model
//fps habe ich wie besprochen entfernt
-void armarx::TrajectoryExporter::exportTrajectory(std::vector<DesignerTrajectoryPtr> trajectories, const std::string file)
+void
+armarx::TrajectoryExporter::exportTrajectory(std::vector<DesignerTrajectoryPtr> trajectories,
+ const std::string file)
{
const Ice::Current& c = Ice::emptyCurrent;
auto communicator = c.adapter.get()->getCommunicator();
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/ImportExport/TrajectoryExporter.h b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/ImportExport/TrajectoryExporter.h
index 3e5179c84f0e49488dd7dee0eed6d3bcd58ca96e..0c42a81e319c6c1dab1073d867b185cb7582129a 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/ImportExport/TrajectoryExporter.h
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/ImportExport/TrajectoryExporter.h
@@ -22,17 +22,20 @@
#ifndef TRAJECTORYEXPORTER_H
#define TRAJECTORYEXPORTER_H
#include <Ice/Ice.h>
-#include "DesignerExporter.h"
+
#include "../Model/DesignerTrajectory.h"
+#include "DesignerExporter.h"
+
namespace armarx
{
class TrajectoryExporter;
+
/**
* @class TrajectoryExporter
* @brief Exports instances of the DesignerTrajectory class to files as serialized instance of the Trajectory class.
* UserWaypoint information is not exported.
*/
- class TrajectoryExporter// : public DesignerExporter
+ class TrajectoryExporter // : public DesignerExporter
{
public:
/**
@@ -41,10 +44,10 @@ namespace armarx
* @param fps fsequency of the measurements in measurements per second.
* @param c The Ice::Current instance
*/
- void exportTrajectory(std::vector<DesignerTrajectoryPtr> trajectories, const std::string file);
+ void exportTrajectory(std::vector<DesignerTrajectoryPtr> trajectories,
+ const std::string file);
};
using TrajectoryExporterPtr = std::shared_ptr<TrajectoryExporter>;
-}
+} // namespace armarx
#endif // TRAJECTORYEXPORTER_H
-
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/ImportExport/TrajectoryImporter.cpp b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/ImportExport/TrajectoryImporter.cpp
index cf3ab66a6324ac597e8e63eb040fdee1fd5593f1..2d39bca3e5d1cff74536d441444375b64ce497d4 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/ImportExport/TrajectoryImporter.cpp
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/ImportExport/TrajectoryImporter.cpp
@@ -20,22 +20,28 @@
* GNU General Public License
*/
#include "TrajectoryImporter.h"
-#include <RobotAPI/libraries/core/Trajectory.h>
-#include <iostream>
+
+#include <algorithm>
#include <fstream>
+#include <iostream>
+
+#include <Ice/CommunicatorF.h>
+
#include <ArmarXCore/interface/observers/Serialization.h>
#include <ArmarXCore/interface/serialization/JSONSerialization.h>
#include <ArmarXCore/util/json/JSONObject.h>
-#include <Ice/CommunicatorF.h>
+
+#include <RobotAPI/libraries/core/Trajectory.h>
+
#include "../KinematicSolver.h"
-#include <algorithm>
-armarx::TrajectoryImporter::TrajectoryImporter(armarx::EnvironmentPtr environment) : environment(environment)
+armarx::TrajectoryImporter::TrajectoryImporter(armarx::EnvironmentPtr environment) :
+ environment(environment)
{
-
}
-std::vector<armarx::DesignerTrajectoryPtr> armarx::TrajectoryImporter::importTrajectory(std::string file)
+std::vector<armarx::DesignerTrajectoryPtr>
+armarx::TrajectoryImporter::importTrajectory(std::string file)
{
//Use initialized ice communicator instead of initializing a new one
const Ice::Current& c = Ice::emptyCurrent;
@@ -68,15 +74,17 @@ std::vector<armarx::DesignerTrajectoryPtr> armarx::TrajectoryImporter::importTra
VirtualRobot::RobotNodeSetPtr rns;
//Get the correct rns
- for (VirtualRobot::RobotNodeSetPtr set : environment->getRobot()->getRobotNodeSets()) //Check if Robot has an rns that has EXACTLY the needed nodes.
+ for (VirtualRobot::RobotNodeSetPtr set :
+ environment->getRobot()
+ ->getRobotNodeSets()) //Check if Robot has an rns that has EXACTLY the needed nodes.
{
-next:
+ next:
std::vector<VirtualRobot::RobotNodePtr> nodes = set->getAllRobotNodes();
for (VirtualRobot::RobotNodePtr node : nodeSet)
{
if (std::find(nodes.begin(), nodes.end(), node) == nodes.end())
{
- goto next; //continue next;
+ goto next; //continue next;
}
}
if (rns->getAllRobotNodes().size() == nodes.size())
@@ -100,13 +108,16 @@ next:
{
jointAngles = element.getPositions();
}
- PoseBasePtr pose = KinematicSolver::getInstance(environment->getScene(), environment->getRobot())->doForwardKinematic(rns, jointAngles);
+ PoseBasePtr pose =
+ KinematicSolver::getInstance(environment->getScene(), environment->getRobot())
+ ->doForwardKinematic(rns, jointAngles);
UserWaypointPtr userPoint = std::make_shared<UserWaypoint>(UserWaypoint(pose));
//Make the points timeOptimalBreakPoints, so the calculation does not automatically get called
userPoint->setIsTimeOptimalBreakpoint(true);
points.push_back(userPoint);
}
- DesignerTrajectoryPtr result = std::make_shared<DesignerTrajectory>(DesignerTrajectory(points[0], rns));
+ DesignerTrajectoryPtr result =
+ std::make_shared<DesignerTrajectory>(DesignerTrajectory(points[0], rns));
for (unsigned int i = 1; i < points.size(); i++)
{
result->addLastUserWaypoint(points[i]);
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/ImportExport/TrajectoryImporter.h b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/ImportExport/TrajectoryImporter.h
index d18005d46d8be01cf2beff133d31c6511b0040f8..399bf4042b06596602368e65e385811383cc8e5b 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/ImportExport/TrajectoryImporter.h
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/ImportExport/TrajectoryImporter.h
@@ -22,11 +22,14 @@
#ifndef TRAJECTORYIMPORTER_H
#define TRAJECTORYIMPORTER_H
#include <Ice/Ice.h>
-#include "DesignerImporter.h"
+
#include "../Environment.h"
+#include "DesignerImporter.h"
+
namespace armarx
{
class TrajectoryImporter;
+
/**
* @class TrajectoryImporter
* @brief Imports instances of the DesignerTrajectory class out of a serialized instance of the Trajectory class from a target file.
@@ -45,5 +48,5 @@ namespace armarx
};
using TrajectoryImporterPtr = std::shared_ptr<TrajectoryImporter>;
-}
+} // namespace armarx
#endif // TRAJECTORYIMPORTER_H
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Interpolation/AbstractInterpolation.cpp b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Interpolation/AbstractInterpolation.cpp
index 02fecad55c6cf2eb3ec2123aef879c2a9d3d69bd..3ffbe80cbe93e6dbc97360494cd2ca51dc5b183a 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Interpolation/AbstractInterpolation.cpp
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Interpolation/AbstractInterpolation.cpp
@@ -24,8 +24,8 @@
///ARMARX-INCLUDES
-#include <RobotAPI/libraries/core/math/MathUtils.h>
#include <RobotAPI/libraries/core/FramedPose.h>
+#include <RobotAPI/libraries/core/math/MathUtils.h>
#include "../Util/OrientationConversion.h"
#include "../exceptions/InterpolationNotDefinedException.h"
@@ -33,14 +33,16 @@
using namespace armarx;
-const QuaternionBasePtr AbstractInterpolation::calculateOrientationAt(double time)
+const QuaternionBasePtr
+AbstractInterpolation::calculateOrientationAt(double time)
{
if (time < 0 || time > 1)
{
throw new exceptions::local::InterpolationNotDefinedException(time);
}
int segment = time * (getNumberOfControlPoints() - 1);
- double segmentRelativeTime = (time - (1.0 / (controlPoints.size() - 1.0)) * segment) * (controlPoints.size() - 1);
+ double segmentRelativeTime =
+ (time - (1.0 / (controlPoints.size() - 1.0)) * segment) * (controlPoints.size() - 1);
PoseBasePtr current = this->controlPoints.at(segment);
PoseBasePtr next = this->controlPoints.at(segment + 1);
@@ -50,25 +52,28 @@ const QuaternionBasePtr AbstractInterpolation::calculateOrientationAt(double tim
return temp;
}
-int AbstractInterpolation::getNumberOfControlPoints()
+int
+AbstractInterpolation::getNumberOfControlPoints()
{
return controlPoints.size();
}
-void AbstractInterpolation::init(const std::vector<PoseBasePtr> cp)
+void
+AbstractInterpolation::init(const std::vector<PoseBasePtr> cp)
{
for (unsigned int i = 0; i < cp.size(); i++)
{
this->controlPoints.push_back(deepCopy(cp[i]));
-
}
}
-PoseBasePtr AbstractInterpolation::deepCopy(PoseBasePtr org)
+PoseBasePtr
+AbstractInterpolation::deepCopy(PoseBasePtr org)
{
- QuaternionPtr tempOri = QuaternionPtr(new Quaternion(org->orientation->qw, org->orientation->qx, org->orientation->qy, org->orientation->qz));
- Vector3BasePtr tempPos = Vector3BasePtr(new Vector3(org->position->x, org->position->y, org->position->z));
+ QuaternionPtr tempOri = QuaternionPtr(new Quaternion(
+ org->orientation->qw, org->orientation->qx, org->orientation->qy, org->orientation->qz));
+ Vector3BasePtr tempPos =
+ Vector3BasePtr(new Vector3(org->position->x, org->position->y, org->position->z));
return *new PoseBasePtr(new Pose(tempPos, tempOri));
}
-
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Interpolation/AbstractInterpolation.h b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Interpolation/AbstractInterpolation.h
index e2e0305820822c21bc0246801406eec83faf6e16..4f071a3d7381d73dbb80293f6042c6c72d932388 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Interpolation/AbstractInterpolation.h
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Interpolation/AbstractInterpolation.h
@@ -36,7 +36,6 @@ namespace armarx
class AbstractInterpolation
{
public:
-
/**
* @brief getPoseAt returns the Pose defined by f(time)
* @param time a time between 0 and 1 with getPoseAt(0) being the startingPose and getPoseAt(1) being the ending Pose
@@ -74,7 +73,7 @@ namespace armarx
static PoseBasePtr deepCopy(PoseBasePtr org);
};
- using AbstractInterpolationPtr = std::shared_ptr <AbstractInterpolation>;
-}
+ using AbstractInterpolationPtr = std::shared_ptr<AbstractInterpolation>;
+} // namespace armarx
#endif
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Interpolation/InterpolationSegmentFactory.cpp b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Interpolation/InterpolationSegmentFactory.cpp
index 2dcd791e60302c1e5e0ae4056f6d92d10ec412c4..ebc4ecc36bc96d98d14f3d90b74300041881b648 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Interpolation/InterpolationSegmentFactory.cpp
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Interpolation/InterpolationSegmentFactory.cpp
@@ -20,27 +20,35 @@
* GNU General Public License
*/
#include "InterpolationSegmentFactory.h"
-#include "LinearInterpolation.h"
-#include "SplineInterpolation.h"
+
#include <ArmarXCore/core/logging/Logging.h>
+
#include <RobotAPI/libraries/core/FramedPose.h>
+#include "LinearInterpolation.h"
+#include "SplineInterpolation.h"
+
///EXCEPTIONS
+#include "../exceptions/ControlPointsInterpolationMatchException.h"
#include "../exceptions/InterpolationNotDefinedException.h"
#include "../exceptions/NoInterpolationPossibleException.h"
-#include "../exceptions/ControlPointsInterpolationMatchException.h"
using namespace armarx;
using namespace VirtualRobot;
-std::vector<AbstractInterpolationPtr> InterpolationSegmentFactory::produceInterpolationSegments(std::vector<PoseBasePtr> controlPoints, std::vector<InterpolationType> interpolations)
+std::vector<AbstractInterpolationPtr>
+InterpolationSegmentFactory::produceInterpolationSegments(
+ std::vector<PoseBasePtr> controlPoints,
+ std::vector<InterpolationType> interpolations)
{
if (controlPoints.size() != interpolations.size() + 1)
{
- throw new exceptions::local::ControlPointsInterpolationMatchException(controlPoints.size(), interpolations.size());
+ throw new exceptions::local::ControlPointsInterpolationMatchException(
+ controlPoints.size(), interpolations.size());
}
- std::vector<AbstractInterpolationPtr> producedInterpolations = std::vector<AbstractInterpolationPtr>();
+ std::vector<AbstractInterpolationPtr> producedInterpolations =
+ std::vector<AbstractInterpolationPtr>();
std::vector<int> interPolationChangePoints = std::vector<int>();
interPolationChangePoints.push_back(0);
@@ -82,11 +90,11 @@ std::vector<AbstractInterpolationPtr> InterpolationSegmentFactory::produceInterp
interPolationChangePoints.push_back(i);
}
current = interpolations[i];
-
}
-
}
- if (interpolations.size() >= 2 && interpolations.at(interpolations.size() - 1) == eSplineInterpolation && interpolations.at(interpolations.size() - 2) != eSplineInterpolation)
+ if (interpolations.size() >= 2 &&
+ interpolations.at(interpolations.size() - 1) == eSplineInterpolation &&
+ interpolations.at(interpolations.size() - 2) != eSplineInterpolation)
{
interpolations.pop_back();
interpolations.push_back(eLinearInterpolation);
@@ -108,7 +116,7 @@ std::vector<AbstractInterpolationPtr> InterpolationSegmentFactory::produceInterp
}
}*/
//Produce the InterpolationSegments and add them to producedInterpolationsVector
- for (unsigned int j = 1; j < interPolationChangePoints.size(); j++)
+ for (unsigned int j = 1; j < interPolationChangePoints.size(); j++)
{
std::vector<PoseBasePtr> currentCP = std::vector<PoseBasePtr>();
for (int i = interPolationChangePoints.at(j - 1); i <= interPolationChangePoints.at(j); i++)
@@ -129,7 +137,9 @@ std::vector<AbstractInterpolationPtr> InterpolationSegmentFactory::produceInterp
}
std::vector<PoseBasePtr> currentCP = std::vector<PoseBasePtr>();
- for (unsigned int i = interPolationChangePoints.at(interPolationChangePoints.size() - 1); i < controlPoints.size(); i++)
+ for (unsigned int i = interPolationChangePoints.at(interPolationChangePoints.size() - 1);
+ i < controlPoints.size();
+ i++)
{
currentCP.push_back(controlPoints.at(i));
}
@@ -147,48 +157,65 @@ std::vector<AbstractInterpolationPtr> InterpolationSegmentFactory::produceInterp
return producedInterpolations;
}
-
-std::vector<AbstractInterpolationPtr> InterpolationSegmentFactory::produceLinearInterpolationSegments(std::vector<PoseBasePtr> controlPoints)
+std::vector<AbstractInterpolationPtr>
+InterpolationSegmentFactory::produceLinearInterpolationSegments(
+ std::vector<PoseBasePtr> controlPoints)
{
- std::vector<AbstractInterpolationPtr> producedInterpolations = std::vector<AbstractInterpolationPtr>();
+ std::vector<AbstractInterpolationPtr> producedInterpolations =
+ std::vector<AbstractInterpolationPtr>();
for (unsigned int i = 0; i < controlPoints.size() - 1; i++)
{
std::vector<PoseBasePtr> currentCP = std::vector<PoseBasePtr>();
currentCP.push_back(controlPoints.at(i));
currentCP.push_back(controlPoints.at(i + 1));
- AbstractInterpolationPtr interpolation = std::shared_ptr<AbstractInterpolation>(new LinearInterpolation(currentCP));
+ AbstractInterpolationPtr interpolation =
+ std::shared_ptr<AbstractInterpolation>(new LinearInterpolation(currentCP));
producedInterpolations.push_back(interpolation);
}
return producedInterpolations;
}
-std::vector<AbstractInterpolationPtr> InterpolationSegmentFactory::produceSplineInterpolationSegments(std::vector<PoseBasePtr> controlPoints)
+std::vector<AbstractInterpolationPtr>
+InterpolationSegmentFactory::produceSplineInterpolationSegments(
+ std::vector<PoseBasePtr> controlPoints)
{
- std::vector<AbstractInterpolationPtr> producedInterpolations = std::vector<AbstractInterpolationPtr>();
- SplineInterpolationPtr parent = *new std::shared_ptr<SplineInterpolation>(new SplineInterpolation(controlPoints));//das hier muss genauso aussehen
+ std::vector<AbstractInterpolationPtr> producedInterpolations =
+ std::vector<AbstractInterpolationPtr>();
+ SplineInterpolationPtr parent = *new std::shared_ptr<SplineInterpolation>(
+ new SplineInterpolation(controlPoints)); //das hier muss genauso aussehen
for (unsigned int i = 0; i < controlPoints.size() - 1; i++)
{
- AbstractInterpolationPtr interpolation = std::shared_ptr<AbstractInterpolation>(parent->getInterPolationSegment(i));
+ AbstractInterpolationPtr interpolation =
+ std::shared_ptr<AbstractInterpolation>(parent->getInterPolationSegment(i));
producedInterpolations.push_back(interpolation);
}
return producedInterpolations;
}
-AbstractInterpolationPtr InterpolationSegmentFactory::produceSplineInterpolationSegment(std::vector<PoseBasePtr> controlPoints, PoseBasePtr segmentStart)
+AbstractInterpolationPtr
+InterpolationSegmentFactory::produceSplineInterpolationSegment(
+ std::vector<PoseBasePtr> controlPoints,
+ PoseBasePtr segmentStart)
{
- SplineInterpolationPtr parent = *new std::shared_ptr<SplineInterpolation>(new SplineInterpolation(controlPoints));//das hier muss genauso aussehen
+ SplineInterpolationPtr parent = *new std::shared_ptr<SplineInterpolation>(
+ new SplineInterpolation(controlPoints)); //das hier muss genauso aussehen
- AbstractInterpolationPtr interpolation = std::shared_ptr<AbstractInterpolation>(parent->getInterPolationSegment(segmentStart));
+ AbstractInterpolationPtr interpolation =
+ std::shared_ptr<AbstractInterpolation>(parent->getInterPolationSegment(segmentStart));
return interpolation;
}
+
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/// \brief InterpolationSegmentFactory::optimizeControlPoints
/// \param controlPoints
/// \param selections
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-void InterpolationSegmentFactory::optimizeControlPoints(std::vector<PoseBasePtr>& controlPoints, std::vector<IKSolver::CartesianSelection>& selections)
+void
+InterpolationSegmentFactory::optimizeControlPoints(
+ std::vector<PoseBasePtr>& controlPoints,
+ std::vector<IKSolver::CartesianSelection>& selections)
{
/*for(PoseBasePtr pose: controlPoints) {
ARMARX_WARNING << Pose(pose->position, pose->orientation).toEigen();
@@ -197,22 +224,26 @@ void InterpolationSegmentFactory::optimizeControlPoints(std::vector<PoseBasePtr>
ARMARX_WARNING << std::to_string(sel);
}*/
- for (unsigned int i = 0; i < selections.size() - 1; i++)
+ for (unsigned int i = 0; i < selections.size() - 1; i++)
{
//ARMARX_INFO << "OPTIMIZE";
if (InterpolationSegmentFactory::isDominantOver(selections[i + 1], selections[i]))
{
- std::vector<IKSolver::CartesianSelection> followingDominations = std::vector<IKSolver::CartesianSelection>();
+ std::vector<IKSolver::CartesianSelection> followingDominations =
+ std::vector<IKSolver::CartesianSelection>();
followingDominations.push_back(selections[i + 1]);
int followingDominationsStart = i;
bool isBreaking;
do
{
//look left until an equal or fully dominating cartesian selection appears
- for (int j = i ; j >= 0; j--)
+ for (int j = i; j >= 0; j--)
{
- if ((InterpolationSegmentFactory::isDominantOver(selections[j], selections[i + 1]) && !InterpolationSegmentFactory::isDominantOver(selections[i + 1], selections[j]))
- || selections[i + 1] == selections[j])
+ if ((InterpolationSegmentFactory::isDominantOver(selections[j],
+ selections[i + 1]) &&
+ !InterpolationSegmentFactory::isDominantOver(selections[i + 1],
+ selections[j])) ||
+ selections[i + 1] == selections[j])
{
//ARMARX_INFO << std::to_string(selections[j]) + " is dominant over " + std::to_string(selections[i + 1]);
@@ -221,21 +252,27 @@ void InterpolationSegmentFactory::optimizeControlPoints(std::vector<PoseBasePtr>
int dominanceIntervalSize = i + 1 - j;
PoseBasePtr dominanceIntervalStart = controlPoints[j];
PoseBasePtr dominanceIntervalEnd = controlPoints[i + 1];
- std::vector<AbstractInterpolationPtr> li = produceLinearInterpolationSegments({dominanceIntervalStart, dominanceIntervalEnd});
+ std::vector<AbstractInterpolationPtr> li =
+ produceLinearInterpolationSegments(
+ {dominanceIntervalStart, dominanceIntervalEnd});
int counter = 1;
for (unsigned int k = j + 1; k < i + 1; k++)
{
- controlPoints[k] = optimizePose(controlPoints[k], li[0]->getPoseAt(double(1.0 / dominanceIntervalSize) * counter), selections[k], selections[i + 1]);
+ controlPoints[k] = optimizePose(
+ controlPoints[k],
+ li[0]->getPoseAt(double(1.0 / dominanceIntervalSize) * counter),
+ selections[k],
+ selections[i + 1]);
counter++;
//ARMARX_INFO << std::to_string(double(1.0 / dominanceIntervalSize) * k) + " SET POINT AT------" + std::to_string(k) + " with " + std::to_string(selections[i + 1]);
}
break;
-
}
followingDominations.push_back(selections[j]);
}
- if (i + 2 < selections.size() && isDominantOver(selections[i + 2], selections[i + 1]))
+ if (i + 2 < selections.size() &&
+ isDominantOver(selections[i + 2], selections[i + 1]))
{
isBreaking = false;
i++;
@@ -246,14 +283,14 @@ void InterpolationSegmentFactory::optimizeControlPoints(std::vector<PoseBasePtr>
isBreaking = true;
}
- }
- while (!isBreaking);
+ } while (!isBreaking);
/*ARMARX_WARNING << "FOLLOWING DOMINATIONS START ";
for(IKSolver::CartesianSelection sel: followingDominations) {
ARMARX_WARNING << std::to_string(sel);
}
ARMARX_WARNING << "FOLLOWING DOMINATIONS END ";*/
- IKSolver::CartesianSelection dominatingSelection = getSmallestDominating(followingDominations);
+ IKSolver::CartesianSelection dominatingSelection =
+ getSmallestDominating(followingDominations);
for (unsigned int m = followingDominationsStart; m <= i + 1; m++)
{
//ARMARX_WARNING << "SET SELECTION " + std::to_string(m) + " with " + std::to_string(dominatingSelection);
@@ -262,7 +299,6 @@ void InterpolationSegmentFactory::optimizeControlPoints(std::vector<PoseBasePtr>
}
}
//ARMARX_INFO << "Optimizing done";
-
}
ARMARX_INFO << "Optimizing done";
@@ -275,7 +311,8 @@ void InterpolationSegmentFactory::optimizeControlPoints(std::vector<PoseBasePtr>
}*/
}
-bool InterpolationSegmentFactory::needsOptimizing(std::vector<IKSolver::CartesianSelection>& selections)
+bool
+InterpolationSegmentFactory::needsOptimizing(std::vector<IKSolver::CartesianSelection>& selections)
{
for (unsigned int i = 0; i < selections.size() - 1; i++)
{
@@ -288,20 +325,27 @@ bool InterpolationSegmentFactory::needsOptimizing(std::vector<IKSolver::Cartesia
return false;
}
-bool InterpolationSegmentFactory::isDominantOver(VirtualRobot::IKSolver::CartesianSelection current, VirtualRobot::IKSolver::CartesianSelection other)
+bool
+InterpolationSegmentFactory::isDominantOver(VirtualRobot::IKSolver::CartesianSelection current,
+ VirtualRobot::IKSolver::CartesianSelection other)
{
- if (other == IKSolver::CartesianSelection::X || other == IKSolver::CartesianSelection::Y || other == IKSolver::CartesianSelection::Z || other == IKSolver::CartesianSelection::Orientation)
+ if (other == IKSolver::CartesianSelection::X || other == IKSolver::CartesianSelection::Y ||
+ other == IKSolver::CartesianSelection::Z ||
+ other == IKSolver::CartesianSelection::Orientation)
{
return current != other;
}
else if (other == IKSolver::CartesianSelection::Position)
{
- return (current == IKSolver::CartesianSelection::Orientation || current == IKSolver::CartesianSelection::All);
+ return (current == IKSolver::CartesianSelection::Orientation ||
+ current == IKSolver::CartesianSelection::All);
}
return false;
}
-IKSolver::CartesianSelection InterpolationSegmentFactory::getSmallestDominating(std::vector<VirtualRobot::IKSolver::CartesianSelection> selections)
+IKSolver::CartesianSelection
+InterpolationSegmentFactory::getSmallestDominating(
+ std::vector<VirtualRobot::IKSolver::CartesianSelection> selections)
{
bool isInhomogenous = false;
ARMARX_INFO << "Vector start";
@@ -320,7 +364,8 @@ IKSolver::CartesianSelection InterpolationSegmentFactory::getSmallestDominating(
}
if (max <= 4 && isInhomogenous)
{
- ARMARX_INFO << "SMALLEST DOMINATING" + std::to_string(IKSolver::CartesianSelection::Position);
+ ARMARX_INFO << "SMALLEST DOMINATING" +
+ std::to_string(IKSolver::CartesianSelection::Position);
return IKSolver::CartesianSelection::Position;
}
else if (max == IKSolver::CartesianSelection::Orientation)
@@ -329,11 +374,13 @@ IKSolver::CartesianSelection InterpolationSegmentFactory::getSmallestDominating(
{
if (currentSelection != IKSolver::CartesianSelection::Orientation)
{
- ARMARX_INFO << "SMALLEST DOMINATING" + std::to_string(IKSolver::CartesianSelection::All);
+ ARMARX_INFO << "SMALLEST DOMINATING" +
+ std::to_string(IKSolver::CartesianSelection::All);
return IKSolver::CartesianSelection::All;
}
}
- ARMARX_INFO << "SMALLEST DOMINATING" + std::to_string(IKSolver::CartesianSelection::Orientation);
+ ARMARX_INFO << "SMALLEST DOMINATING" +
+ std::to_string(IKSolver::CartesianSelection::Orientation);
return IKSolver::CartesianSelection::Orientation;
}
else
@@ -341,10 +388,13 @@ IKSolver::CartesianSelection InterpolationSegmentFactory::getSmallestDominating(
ARMARX_INFO << "SMALLEST DOMINATING" + std::to_string(max);
return static_cast<IKSolver::CartesianSelection>(max);
}
-
}
-PoseBasePtr InterpolationSegmentFactory::optimizePose(PoseBasePtr original, PoseBasePtr corrected, IKSolver::CartesianSelection selectionOriginal, IKSolver::CartesianSelection selectionCorrecting)
+PoseBasePtr
+InterpolationSegmentFactory::optimizePose(PoseBasePtr original,
+ PoseBasePtr corrected,
+ IKSolver::CartesianSelection selectionOriginal,
+ IKSolver::CartesianSelection selectionCorrecting)
{
//ARMARX_WARNING << Pose(original->position, original->orientation).toEigen();
//ARMARX_WARNING << Pose(corrected->position, corrected->orientation).toEigen();
@@ -352,11 +402,20 @@ PoseBasePtr InterpolationSegmentFactory::optimizePose(PoseBasePtr original, Pose
switch (selectionCorrecting)
{
case (IKSolver::CartesianSelection::X):
- firstStep = PoseBasePtr(new Pose(Vector3BasePtr(new Vector3(corrected->position->x, original->position->y, original->position->z)), original->orientation));
+ firstStep = PoseBasePtr(
+ new Pose(Vector3BasePtr(new Vector3(
+ corrected->position->x, original->position->y, original->position->z)),
+ original->orientation));
case (IKSolver::CartesianSelection::Y):
- firstStep = PoseBasePtr(new Pose(Vector3BasePtr(new Vector3(original->position->x, corrected->position->y, original->position->z)), original->orientation));
+ firstStep = PoseBasePtr(
+ new Pose(Vector3BasePtr(new Vector3(
+ original->position->x, corrected->position->y, original->position->z)),
+ original->orientation));
case (IKSolver::CartesianSelection::Z):
- firstStep = PoseBasePtr(new Pose(Vector3BasePtr(new Vector3(original->position->x, original->position->y, corrected->position->z)), original->orientation));
+ firstStep = PoseBasePtr(
+ new Pose(Vector3BasePtr(new Vector3(
+ original->position->x, original->position->y, corrected->position->z)),
+ original->orientation));
case (IKSolver::CartesianSelection::Position):
firstStep = PoseBasePtr(new Pose(corrected->position, original->orientation));
case (IKSolver::CartesianSelection::Orientation):
@@ -368,11 +427,20 @@ PoseBasePtr InterpolationSegmentFactory::optimizePose(PoseBasePtr original, Pose
switch (selectionOriginal)
{
case (IKSolver::CartesianSelection::X):
- return PoseBasePtr(new Pose(Vector3BasePtr(new Vector3(original->position->x, firstStep->position->y, firstStep->position->z)), firstStep->orientation));
+ return PoseBasePtr(new Pose(Vector3BasePtr(new Vector3(original->position->x,
+ firstStep->position->y,
+ firstStep->position->z)),
+ firstStep->orientation));
case (IKSolver::CartesianSelection::Y):
- return PoseBasePtr(new Pose(Vector3BasePtr(new Vector3(firstStep->position->x, original->position->y, firstStep->position->z)), firstStep->orientation));
+ return PoseBasePtr(new Pose(Vector3BasePtr(new Vector3(firstStep->position->x,
+ original->position->y,
+ firstStep->position->z)),
+ firstStep->orientation));
case (IKSolver::CartesianSelection::Z):
- return PoseBasePtr(new Pose(Vector3BasePtr(new Vector3(firstStep->position->x, firstStep->position->y, original->position->z)), firstStep->orientation));
+ return PoseBasePtr(new Pose(Vector3BasePtr(new Vector3(firstStep->position->x,
+ firstStep->position->y,
+ original->position->z)),
+ firstStep->orientation));
case (IKSolver::CartesianSelection::Position):
return PoseBasePtr(new Pose(original->position, firstStep->orientation));
case (IKSolver::CartesianSelection::Orientation):
@@ -383,6 +451,3 @@ PoseBasePtr InterpolationSegmentFactory::optimizePose(PoseBasePtr original, Pose
return corrected;
}
}
-
-
-
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Interpolation/InterpolationSegmentFactory.h b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Interpolation/InterpolationSegmentFactory.h
index 81150dd6f8122fbbb6e6e0cc6f088735bbef727b..0f7e562bbe54a8d72b7e34dfd65043c2df723d24 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Interpolation/InterpolationSegmentFactory.h
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Interpolation/InterpolationSegmentFactory.h
@@ -22,9 +22,10 @@
#ifndef INTERPOLATIONSEGMENTFACTORY_H
#define INTERPOLATIONSEGMENTFACTORY_H
+#include <VirtualRobot/IK/AdvancedIKSolver.h>
+
#include "AbstractInterpolation.h"
#include "InterpolationType.h"
-#include <VirtualRobot/IK/AdvancedIKSolver.h>
namespace armarx
{
@@ -43,21 +44,25 @@ namespace armarx
* @param interpolations the InterpolationTypes of the Interpolation segments (warning: at least 2 segments in a row have to have spline intepolation)
* @return a vector of |controlPoints| - 1 AbstractInterpolations between the control points
*/
- static std::vector<AbstractInterpolationPtr> produceInterpolationSegments(std::vector<PoseBasePtr> controlPoints, std::vector<InterpolationType> interpolations);
+ static std::vector<AbstractInterpolationPtr>
+ produceInterpolationSegments(std::vector<PoseBasePtr> controlPoints,
+ std::vector<InterpolationType> interpolations);
/**
* @brief produceInterpolationSegments constructs a vector of LinearInterpolations
* @param controlPoints the Poses the interpolation curves go through; segment i starts at controlPoint i and ends at control point i + 1
* @return a vector of |controlPoints| - 1 LinearInterpolations between the control points
*/
- static std::vector<AbstractInterpolationPtr> produceLinearInterpolationSegments(std::vector<PoseBasePtr> controlPoints);
+ static std::vector<AbstractInterpolationPtr>
+ produceLinearInterpolationSegments(std::vector<PoseBasePtr> controlPoints);
/**
* @brief produceInterpolationSegments constructs a vector of SplineInterpolations
* @param controlPoints the Poses the interpolation curves go through; segment i starts at controlPoint i and ends at control point i + 1
* @return a vector of |controlPoints| - 1 SplineInterpolations between the control points
*/
- static std::vector<AbstractInterpolationPtr> produceSplineInterpolationSegments(std::vector<PoseBasePtr> controlPoints);
+ static std::vector<AbstractInterpolationPtr>
+ produceSplineInterpolationSegments(std::vector<PoseBasePtr> controlPoints);
/**
* @brief produceSplineInterpolationSegment constructs the splineInterpolationSegment of a splineInterpolation between given controlPoints
@@ -65,20 +70,25 @@ namespace armarx
* @param segmentStart the first Point in the Interval
* @return a SplineInterpolationSegment between segmentStart and the following control point
*/
- static AbstractInterpolationPtr produceSplineInterpolationSegment(std::vector<PoseBasePtr> controlPoints, PoseBasePtr segmentStart);
+ static AbstractInterpolationPtr
+ produceSplineInterpolationSegment(std::vector<PoseBasePtr> controlPoints,
+ PoseBasePtr segmentStart);
/**
* @brief optimizeControlPoints changes the cartian selections and control points so that the IKSolving produces a smooth Trajectory
* @param controlPoints the controlPoints to change to make the Trajectory smooth
* @param selections the cartesian selections to optimize
*/
- static void optimizeControlPoints(std::vector<PoseBasePtr>& controlPoints, std::vector<VirtualRobot::IKSolver::CartesianSelection>& selections);
+ static void
+ optimizeControlPoints(std::vector<PoseBasePtr>& controlPoints,
+ std::vector<VirtualRobot::IKSolver::CartesianSelection>& selections);
/**
* @brief needsOptimizing returns true if there is a CartesianSelection at i that dominates a CartesianSelection at i - 1
* @param selections the cartesian selection that should be reached
* @return true if the cartesian Selection need optimizing for a smooth trajectory
*/
- static bool needsOptimizing(std::vector<VirtualRobot::IKSolver::CartesianSelection>& selections);
+ static bool
+ needsOptimizing(std::vector<VirtualRobot::IKSolver::CartesianSelection>& selections);
/**
* @brief isDominantOver returns true when current selection dominates other selection
* Definition: A CartesianSelection dominates another CartesianSelection when additional Information is needed to solve curren after solving other
@@ -87,14 +97,16 @@ namespace armarx
* @param other the prior cartesian selection
* @return true if current dominates other
*/
- static bool isDominantOver(VirtualRobot::IKSolver::CartesianSelection current, VirtualRobot::IKSolver::CartesianSelection other);
+ static bool isDominantOver(VirtualRobot::IKSolver::CartesianSelection current,
+ VirtualRobot::IKSolver::CartesianSelection other);
/**
* @brief getSmallestDominating returns the cartesian selection that dominates all other cartesian selections in selections
* Examples: (X,Y,Y,Y,Z) -> Pos ; (X,Y, Ori) -> All; (Ori,Ori) -> Ori ; (X,Z,Pos) -> Pos
* @param selections the selections to get the smallest dominating selection from
* @return the smmallestDominating Selection
*/
- static VirtualRobot::IKSolver::CartesianSelection getSmallestDominating(std::vector<VirtualRobot::IKSolver::CartesianSelection> selections);
+ static VirtualRobot::IKSolver::CartesianSelection
+ getSmallestDominating(std::vector<VirtualRobot::IKSolver::CartesianSelection> selections);
/**
* @brief optimizePose changes the original pose so that the interpolation is smove when it changes from selectionOriginal to selectionCorrected
* @param original the original Pose
@@ -103,8 +115,12 @@ namespace armarx
* @param selectionCorrected the cartesian selection associated with the corrected Pose
* @return
*/
- static PoseBasePtr optimizePose(armarx::PoseBasePtr original, armarx::PoseBasePtr corrected, VirtualRobot::IKSolver::CartesianSelection selectionOriginal, VirtualRobot::IKSolver::CartesianSelection selectionCorrected);
+ static PoseBasePtr
+ optimizePose(armarx::PoseBasePtr original,
+ armarx::PoseBasePtr corrected,
+ VirtualRobot::IKSolver::CartesianSelection selectionOriginal,
+ VirtualRobot::IKSolver::CartesianSelection selectionCorrected);
};
-}
+} // namespace armarx
#endif
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Interpolation/InterpolationType.h b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Interpolation/InterpolationType.h
index f0f6df9d0ce919511f42c4a79f5ff716f5a0620e..ef3b540c14f9f3629540d6686c935f4a7712c0a0 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Interpolation/InterpolationType.h
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Interpolation/InterpolationType.h
@@ -34,6 +34,6 @@ namespace armarx
eLinearInterpolation,
eSplineInterpolation
};
-}
+} // namespace armarx
#endif
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Interpolation/LinearInterpolation.cpp b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Interpolation/LinearInterpolation.cpp
index d1577d2919fa73da2fd7aa77d31cb11a658837e3..ca3a03475982b846eff311cfdf52f6e204159295 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Interpolation/LinearInterpolation.cpp
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Interpolation/LinearInterpolation.cpp
@@ -30,6 +30,7 @@
#include <RobotAPI/libraries/core/FramedPose.h>
using namespace armarx;
+
LinearInterpolation::LinearInterpolation(std::vector<PoseBasePtr> controlPoints)
{
if (controlPoints.size() < 2)
@@ -48,8 +49,8 @@ LinearInterpolation::LinearInterpolation(std::vector<PoseBasePtr> controlPoints)
}
}
-
-PoseBasePtr LinearInterpolation::getPoseAt(double time)
+PoseBasePtr
+LinearInterpolation::getPoseAt(double time)
{
if (time < 0 || time > 1)
{
@@ -61,14 +62,15 @@ PoseBasePtr LinearInterpolation::getPoseAt(double time)
{
return LinearInterpolation::deepCopy(controlPoints[controlPoints.size() - 1]);
}
- double segmentRelativeTime = (time - (1.0 / (controlPoints.size() - 1.0)) * segment) * (controlPoints.size() - 1);
+ double segmentRelativeTime =
+ (time - (1.0 / (controlPoints.size() - 1.0)) * segment) * (controlPoints.size() - 1);
PoseBasePtr current = controlPoints[segment];
- Eigen::Vector3f start = Eigen::Vector3f(current->position->x, current->position->y, current->position->z);
+ Eigen::Vector3f start =
+ Eigen::Vector3f(current->position->x, current->position->y, current->position->z);
Eigen::Vector3f transition = connectingVector[segment];
transition *= segmentRelativeTime;
Eigen::Vector3f position = start + transition;
- return *new PoseBasePtr(new Pose(*new Vector3BasePtr(new Vector3(position)), calculateOrientationAt(time)));
+ return *new PoseBasePtr(
+ new Pose(*new Vector3BasePtr(new Vector3(position)), calculateOrientationAt(time)));
}
-
-
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Interpolation/LinearInterpolation.h b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Interpolation/LinearInterpolation.h
index 845f1ab613ae44eb2a6ceb3ef941530e286443a8..826be821ba7ed3833efeba302c64adee69c22425 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Interpolation/LinearInterpolation.h
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Interpolation/LinearInterpolation.h
@@ -22,11 +22,10 @@
#ifndef LINEARINTERPOLATION_H
#define LINEARINTERPOLATION_H
-#include "AbstractInterpolation.h"
-
-
#include <Eigen/Eigen>
+#include "AbstractInterpolation.h"
+
namespace armarx
{
/**
@@ -54,9 +53,9 @@ namespace armarx
std::vector<Eigen::Vector3f> connectingVector;
};
- using LinearInterpolationPtr = std::shared_ptr <LinearInterpolation>;
+ using LinearInterpolationPtr = std::shared_ptr<LinearInterpolation>;
-}
+} // namespace armarx
#endif
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Interpolation/SplineInterPolationSegment.cpp b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Interpolation/SplineInterPolationSegment.cpp
index cacd3c28454f19a1c2f377e0ef7ca15300a633ad..b7da348cf72dcc2c5caff8504c9ae04ee229a8b5 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Interpolation/SplineInterPolationSegment.cpp
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Interpolation/SplineInterPolationSegment.cpp
@@ -21,7 +21,8 @@
*/
#include "SplineInterPolationSegment.h"
-PoseBase Interpolation::SplineInterPolationSegment::getPoseAt(double time)
+PoseBase
+Interpolation::SplineInterPolationSegment::getPoseAt(double time)
{
// TODO - implement SplineInterPolationSegment::getPoseAt
throw "Not yet implemented";
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Interpolation/SplineInterPolationSegment.h b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Interpolation/SplineInterPolationSegment.h
index 352e61a4df1dff7c0bdb0f26e32e2bf4bae25843..346fe03d0fa3f1f46594dc84c760220b3d493daa 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Interpolation/SplineInterPolationSegment.h
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Interpolation/SplineInterPolationSegment.h
@@ -34,6 +34,6 @@ namespace Interpolation
public:
PoseBase getPoseAt(double time);
};
-}
+} // namespace Interpolation
#endif
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Interpolation/SplineInterpolation.cpp b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Interpolation/SplineInterpolation.cpp
index dfbb8845b80cd2bb0543c7f3a893051b8b713fdd..e1c540f2f52e0a6d8ad56fbc78b559c4331de25d 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Interpolation/SplineInterpolation.cpp
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Interpolation/SplineInterpolation.cpp
@@ -20,6 +20,7 @@
* GNU General Public License
*/
#include "SplineInterpolation.h"
+
#include "SplineInterpolationSegment.h"
///ARMARX-INCLUDES
@@ -44,7 +45,9 @@ SplineInterpolation::SplineInterpolation(std::vector<PoseBasePtr> controlPoints)
alglib::pspline2build(getCoordinateArray(1), controlPoints.size(), 2, 0, yInterpolation);
alglib::pspline2build(getCoordinateArray(2), controlPoints.size(), 2, 0, zInterpolation);
}
-PoseBasePtr SplineInterpolation::getPoseAt(double time)
+
+PoseBasePtr
+SplineInterpolation::getPoseAt(double time)
{
if (time < 0 || time > 1)
{
@@ -65,10 +68,10 @@ PoseBasePtr SplineInterpolation::getPoseAt(double time)
Vector3BasePtr position = new Vector3(x, y, z);
QuaternionBasePtr base = this->calculateOrientationAt(time);
return *new PoseBasePtr(new Pose(position, base));
-
}
-AbstractInterpolationPtr SplineInterpolation::getInterPolationSegment(PoseBasePtr start)
+AbstractInterpolationPtr
+SplineInterpolation::getInterPolationSegment(PoseBasePtr start)
{
int number = 0;
for (unsigned int i = 0; i < controlPoints.size(); i++)
@@ -79,17 +82,19 @@ AbstractInterpolationPtr SplineInterpolation::getInterPolationSegment(PoseBasePt
break;
}
}
- return *new AbstractInterpolationPtr(new SplineInterpolationSegment(number, AbstractInterpolationPtr(this)));
+ return *new AbstractInterpolationPtr(
+ new SplineInterpolationSegment(number, AbstractInterpolationPtr(this)));
}
-AbstractInterpolationPtr SplineInterpolation::getInterPolationSegment(int segmentNumber)
+AbstractInterpolationPtr
+SplineInterpolation::getInterPolationSegment(int segmentNumber)
{
- return *new AbstractInterpolationPtr(new SplineInterpolationSegment(segmentNumber, AbstractInterpolationPtr(this)));
+ return *new AbstractInterpolationPtr(
+ new SplineInterpolationSegment(segmentNumber, AbstractInterpolationPtr(this)));
}
-
-
-alglib::real_2d_array SplineInterpolation::getCoordinateArray(int coordinate)
+alglib::real_2d_array
+SplineInterpolation::getCoordinateArray(int coordinate)
{
std::string transformation = "[";
unsigned int i = 0;
@@ -108,7 +113,8 @@ alglib::real_2d_array SplineInterpolation::getCoordinateArray(int coordinate)
temp = current->position->z;
break;
}
- transformation = transformation + "[" + std::to_string(temp) + "," + std::to_string(i / this->controlPoints.size()) + "]";
+ transformation = transformation + "[" + std::to_string(temp) + "," +
+ std::to_string(i / this->controlPoints.size()) + "]";
if (i != controlPoints.size() - 1)
{
transformation += ",";
@@ -117,7 +123,6 @@ alglib::real_2d_array SplineInterpolation::getCoordinateArray(int coordinate)
}
transformation += "]";
const char* c = transformation.c_str();
- real_2d_array input = *new real_2d_array(c);//TODO implement
+ real_2d_array input = *new real_2d_array(c); //TODO implement
return input;
}
-
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Interpolation/SplineInterpolation.h b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Interpolation/SplineInterpolation.h
index 611d67cdeb397ccf1b9eb71bbe9e01cb50175067..801e14fb1c6d8d221d9d2807a157d94b7d338f68 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Interpolation/SplineInterpolation.h
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Interpolation/SplineInterpolation.h
@@ -23,10 +23,8 @@
#define SPLINEINTERPOLATION_H
#include "AbstractInterpolation.h"
-
#include <interpolation.h>
-
namespace armarx
{
/**
@@ -69,10 +67,9 @@ namespace armarx
alglib::pspline2interpolant xInterpolation;
alglib::pspline2interpolant yInterpolation;
alglib::pspline2interpolant zInterpolation;
-
};
using SplineInterpolationPtr = std::shared_ptr<SplineInterpolation>;
-}
+} // namespace armarx
#endif
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Interpolation/SplineInterpolationSegment.cpp b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Interpolation/SplineInterpolationSegment.cpp
index 3db4df6dc36ae29405ba35209a5a61147dcc012c..2bca68797c142315e031810e9752b5e1a82a2018 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Interpolation/SplineInterpolationSegment.cpp
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Interpolation/SplineInterpolationSegment.cpp
@@ -27,7 +27,7 @@ using namespace armarx;
SplineInterpolationSegment::SplineInterpolationSegment(int number, AbstractInterpolationPtr parent)
{
- if (number >= parent->getNumberOfControlPoints() - 1 || number < 0)
+ if (number >= parent->getNumberOfControlPoints() - 1 || number < 0)
{
throw new exceptions::local::InterpolationNotDefinedException(number);
}
@@ -36,7 +36,8 @@ SplineInterpolationSegment::SplineInterpolationSegment(int number, AbstractInter
this->poseAccesStart = poseAccesFactor * number;
}
-PoseBasePtr SplineInterpolationSegment::getPoseAt(double time)
+PoseBasePtr
+SplineInterpolationSegment::getPoseAt(double time)
{
if (time < 0 || time > 1)
{
@@ -45,7 +46,8 @@ PoseBasePtr SplineInterpolationSegment::getPoseAt(double time)
return parent->getPoseAt(poseAccesStart + poseAccesFactor * time);
}
-int SplineInterpolationSegment::getNumberOfControlPoints()
+int
+SplineInterpolationSegment::getNumberOfControlPoints()
{
return 2;
}
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Interpolation/SplineInterpolationSegment.h b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Interpolation/SplineInterpolationSegment.h
index 7274659cd10f0872f4816b8bd15dfbebc879f006..a5e1de3d013a0b4b75d3799d2032db213538d507 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Interpolation/SplineInterpolationSegment.h
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Interpolation/SplineInterpolationSegment.h
@@ -24,7 +24,6 @@
#include "SplineInterpolation.h"
-
namespace armarx
{
/**
@@ -59,6 +58,6 @@ namespace armarx
};
using SplineInterpolationSegmentPtr = std::shared_ptr<SplineInterpolationSegment>;
-}
+} // namespace armarx
#endif
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Interpolation/Tests/InterpolationSegmentFactoryTest.cpp b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Interpolation/Tests/InterpolationSegmentFactoryTest.cpp
index dfba3ff172f59c2bf08c19e38c159a403e8ef606..6fa705602695585b7d7207caf02c08755cc9f5b3 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Interpolation/Tests/InterpolationSegmentFactoryTest.cpp
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Interpolation/Tests/InterpolationSegmentFactoryTest.cpp
@@ -1,20 +1,22 @@
#define BOOST_TEST_MODULE ArmarX::RobotTrajectoryDesigner::InterpolationSegmentFactory
#define ARMARX_BOOST_TEST
-#include <RobotComponents/Test.h>
-
-#include "../InterpolationType.h"
#include "../InterpolationSegmentFactory.h"
-#include "../exceptions/NoInterpolationPossibleException.h"
-#include "../exceptions/InterpolationNotDefinedException.h"
#include <boost/test/unit_test.hpp>
+#include <RobotAPI/libraries/core/Pose.h>
+
+#include <RobotComponents/Test.h>
+
#include "../../Interpolation/LinearInterpolation.h"
#include "../../Util/OrientationConversion.h"
-#include <RobotAPI/libraries/core/Pose.h>
+#include "../InterpolationType.h"
+#include "../exceptions/InterpolationNotDefinedException.h"
+#include "../exceptions/NoInterpolationPossibleException.h"
using namespace armarx;
+
BOOST_AUTO_TEST_CASE(basicTest)
{
Vector3BasePtr pos1 = new Vector3(2, 4, 6);
@@ -61,21 +63,22 @@ BOOST_AUTO_TEST_CASE(basicTest)
QuaternionBasePtr ori11 = OrientationConversion::toQuaternion(0, 0, 0);
PoseBasePtr pose11 = new Pose(pos11, ori11);
- std::vector<PoseBasePtr> cp = {pose1, pose2, pose3, pose4, pose5, pose6, pose7, pose8, pose9, pose10, pose11};
+ std::vector<PoseBasePtr> cp = {
+ pose1, pose2, pose3, pose4, pose5, pose6, pose7, pose8, pose9, pose10, pose11};
- std::vector<InterpolationType> interpolations = {InterpolationType::eSplineInterpolation,
- InterpolationType::eSplineInterpolation,
- InterpolationType::eLinearInterpolation,
- InterpolationType::eLinearInterpolation,
- InterpolationType::eLinearInterpolation,
- InterpolationType::eLinearInterpolation,
- InterpolationType::eLinearInterpolation,
- InterpolationType::eSplineInterpolation,
- InterpolationType::eSplineInterpolation,
- InterpolationType::eSplineInterpolation
- };
+ std::vector<InterpolationType> interpolations = {InterpolationType::eSplineInterpolation,
+ InterpolationType::eSplineInterpolation,
+ InterpolationType::eLinearInterpolation,
+ InterpolationType::eLinearInterpolation,
+ InterpolationType::eLinearInterpolation,
+ InterpolationType::eLinearInterpolation,
+ InterpolationType::eLinearInterpolation,
+ InterpolationType::eSplineInterpolation,
+ InterpolationType::eSplineInterpolation,
+ InterpolationType::eSplineInterpolation};
- std::vector<AbstractInterpolationPtr> ip = InterpolationSegmentFactory::produceInterpolationSegments(cp, interpolations);
+ std::vector<AbstractInterpolationPtr> ip =
+ InterpolationSegmentFactory::produceInterpolationSegments(cp, interpolations);
AbstractInterpolationPtr ips1 = ip[0];
AbstractInterpolationPtr ips2 = ip[1];
@@ -157,21 +160,20 @@ BOOST_AUTO_TEST_CASE(basicTest)
BOOST_CHECK_EQUAL(ips10->getPoseAt(1)->position->x, 0);
BOOST_CHECK_EQUAL(ips10->getPoseAt(1)->position->y, 0);
BOOST_CHECK_EQUAL(ips10->getPoseAt(1)->position->z, 0);
-
-
}
-bool is_critical(exceptions::local::NoInterpolationPossibleException const& ex)
+bool
+is_critical(exceptions::local::NoInterpolationPossibleException const& ex)
{
return true;
}
-bool is_critical2(exceptions::local::InterpolationNotDefinedException const& ex)
+bool
+is_critical2(exceptions::local::InterpolationNotDefinedException const& ex)
{
return true;
}
-
BOOST_AUTO_TEST_CASE(wrongSplineTest)
{
@@ -219,44 +221,42 @@ BOOST_AUTO_TEST_CASE(wrongSplineTest)
QuaternionBasePtr ori11 = OrientationConversion::toQuaternion(0, 0, 0);
PoseBasePtr pose11 = new Pose(pos11, ori11);
- std::vector<PoseBasePtr> cp = {pose1, pose2, pose3, pose4, pose5, pose6, pose7, pose8, pose9, pose10, pose11};
-
- std::vector<InterpolationType> interpolations = {InterpolationType::eSplineInterpolation,
- InterpolationType::eLinearInterpolation,
- InterpolationType::eLinearInterpolation,
- InterpolationType::eLinearInterpolation,
- InterpolationType::eLinearInterpolation,
- InterpolationType::eLinearInterpolation,
- InterpolationType::eLinearInterpolation,
- InterpolationType::eSplineInterpolation,
- InterpolationType::eSplineInterpolation,
- InterpolationType::eSplineInterpolation
- };
-
-
- interpolations = {InterpolationType::eSplineInterpolation,
- InterpolationType::eSplineInterpolation,
- InterpolationType::eLinearInterpolation,
- InterpolationType::eLinearInterpolation,
- InterpolationType::eSplineInterpolation,
- InterpolationType::eLinearInterpolation,
- InterpolationType::eLinearInterpolation,
- InterpolationType::eSplineInterpolation,
- InterpolationType::eSplineInterpolation,
- InterpolationType::eSplineInterpolation
- };
-
- interpolations = {InterpolationType::eSplineInterpolation,
- InterpolationType::eLinearInterpolation,
- InterpolationType::eLinearInterpolation,
- InterpolationType::eLinearInterpolation,
- InterpolationType::eLinearInterpolation,
- InterpolationType::eLinearInterpolation,
- InterpolationType::eLinearInterpolation,
- InterpolationType::eLinearInterpolation,
- InterpolationType::eLinearInterpolation,
- InterpolationType::eSplineInterpolation
- };
+ std::vector<PoseBasePtr> cp = {
+ pose1, pose2, pose3, pose4, pose5, pose6, pose7, pose8, pose9, pose10, pose11};
+
+ std::vector<InterpolationType> interpolations = {InterpolationType::eSplineInterpolation,
+ InterpolationType::eLinearInterpolation,
+ InterpolationType::eLinearInterpolation,
+ InterpolationType::eLinearInterpolation,
+ InterpolationType::eLinearInterpolation,
+ InterpolationType::eLinearInterpolation,
+ InterpolationType::eLinearInterpolation,
+ InterpolationType::eSplineInterpolation,
+ InterpolationType::eSplineInterpolation,
+ InterpolationType::eSplineInterpolation};
+
+
+ interpolations = {InterpolationType::eSplineInterpolation,
+ InterpolationType::eSplineInterpolation,
+ InterpolationType::eLinearInterpolation,
+ InterpolationType::eLinearInterpolation,
+ InterpolationType::eSplineInterpolation,
+ InterpolationType::eLinearInterpolation,
+ InterpolationType::eLinearInterpolation,
+ InterpolationType::eSplineInterpolation,
+ InterpolationType::eSplineInterpolation,
+ InterpolationType::eSplineInterpolation};
+
+ interpolations = {InterpolationType::eSplineInterpolation,
+ InterpolationType::eLinearInterpolation,
+ InterpolationType::eLinearInterpolation,
+ InterpolationType::eLinearInterpolation,
+ InterpolationType::eLinearInterpolation,
+ InterpolationType::eLinearInterpolation,
+ InterpolationType::eLinearInterpolation,
+ InterpolationType::eLinearInterpolation,
+ InterpolationType::eLinearInterpolation,
+ InterpolationType::eSplineInterpolation};
//If no exception is thrown here InterpolationSegmentFactory automatically replaces wrong spline Interpolations with LinearInterpolations
}
@@ -271,7 +271,8 @@ BOOST_AUTO_TEST_CASE(basicDominanceTest)
{
poses.push_back(pose1);
}
- std::vector<VirtualRobot::IKSolver::CartesianSelection> selections = std::vector<VirtualRobot::IKSolver::CartesianSelection>();
+ std::vector<VirtualRobot::IKSolver::CartesianSelection> selections =
+ std::vector<VirtualRobot::IKSolver::CartesianSelection>();
selections.push_back(VirtualRobot::IKSolver::CartesianSelection::All);
selections.push_back(VirtualRobot::IKSolver::CartesianSelection::Position);
selections.push_back(VirtualRobot::IKSolver::CartesianSelection::Orientation);
@@ -295,8 +296,6 @@ BOOST_AUTO_TEST_CASE(basicDominanceTest)
BOOST_CHECK_EQUAL(selections[7], VirtualRobot::IKSolver::CartesianSelection::All);
BOOST_CHECK_EQUAL(selections[8], VirtualRobot::IKSolver::CartesianSelection::All);
BOOST_CHECK_EQUAL(selections[9], VirtualRobot::IKSolver::CartesianSelection::All);
-
-
}
BOOST_AUTO_TEST_CASE(advancedDominanceTest)
@@ -310,7 +309,8 @@ BOOST_AUTO_TEST_CASE(advancedDominanceTest)
{
poses.push_back(pose1);
}
- std::vector<VirtualRobot::IKSolver::CartesianSelection> selections = std::vector<VirtualRobot::IKSolver::CartesianSelection>();
+ std::vector<VirtualRobot::IKSolver::CartesianSelection> selections =
+ std::vector<VirtualRobot::IKSolver::CartesianSelection>();
selections.push_back(VirtualRobot::IKSolver::CartesianSelection::All);
selections.push_back(VirtualRobot::IKSolver::CartesianSelection::Orientation);
selections.push_back(VirtualRobot::IKSolver::CartesianSelection::All);
@@ -350,7 +350,8 @@ BOOST_AUTO_TEST_CASE(reverseOrderDominanceTest)
{
poses.push_back(pose1);
}
- std::vector<VirtualRobot::IKSolver::CartesianSelection> selections = std::vector<VirtualRobot::IKSolver::CartesianSelection>();
+ std::vector<VirtualRobot::IKSolver::CartesianSelection> selections =
+ std::vector<VirtualRobot::IKSolver::CartesianSelection>();
selections.push_back(VirtualRobot::IKSolver::CartesianSelection::X);
selections.push_back(VirtualRobot::IKSolver::CartesianSelection::Orientation);
selections.push_back(VirtualRobot::IKSolver::CartesianSelection::Orientation);
@@ -394,12 +395,4 @@ BOOST_AUTO_TEST_CASE(reverseOrderDominanceTest)
BOOST_CHECK_EQUAL(selections[17], VirtualRobot::IKSolver::CartesianSelection::Position);
BOOST_CHECK_EQUAL(selections[18], VirtualRobot::IKSolver::CartesianSelection::Position);
BOOST_CHECK_EQUAL(selections[19], VirtualRobot::IKSolver::CartesianSelection::X);
-
-
}
-
-
-
-
-
-
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Interpolation/Tests/LinearInterpolationTest.cpp b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Interpolation/Tests/LinearInterpolationTest.cpp
index 872346f8b01d0c8fbc846381197da8c58dccf8c7..2aa2e89db81d15c9980f30af743dc4583a00f9bf 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Interpolation/Tests/LinearInterpolationTest.cpp
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Interpolation/Tests/LinearInterpolationTest.cpp
@@ -2,16 +2,17 @@
#define ARMARX_BOOST_TEST
+#include "../../Interpolation/LinearInterpolation.h"
+
+#include <RobotAPI/libraries/core/FramedPose.h>
+#include <RobotAPI/libraries/core/Pose.h>
+
#include <RobotComponents/Test.h>
-#include "../../Interpolation/LinearInterpolation.h"
#include "../../Util/OrientationConversion.h"
-#include <RobotAPI/libraries/core/Pose.h>
-#include <RobotAPI/libraries/core/FramedPose.h>
using namespace armarx;
using namespace std;
-
BOOST_AUTO_TEST_CASE(basicTest)
{
Vector3BasePtr pos1 = new Vector3(2, 4, 6);
@@ -101,10 +102,8 @@ BOOST_AUTO_TEST_CASE(samePointTwiceTest)
BOOST_CHECK_EQUAL(ip->getPoseAt(0.8997)->position->x, 2);
BOOST_CHECK_EQUAL(ip->getPoseAt(0.8997)->position->y, 4);
BOOST_CHECK_EQUAL(ip->getPoseAt(0.8997)->position->z, 6);
-
}
-
BOOST_AUTO_TEST_CASE(rotateAtPlaceTest)
{
Vector3BasePtr pos1 = new Vector3(2, 4, 6);
@@ -150,8 +149,6 @@ BOOST_AUTO_TEST_CASE(rotateAtPlaceTest)
BOOST_CHECK_EQUAL(ip->getPoseAt(1)->orientation->qx, q4->qx);
BOOST_CHECK_EQUAL(ip->getPoseAt(1)->orientation->qy, q4->qy);
BOOST_CHECK_EQUAL(ip->getPoseAt(1)->orientation->qz, q4->qz);
-
-
}
BOOST_AUTO_TEST_CASE(startIsEndTest)
@@ -186,8 +183,6 @@ BOOST_AUTO_TEST_CASE(startIsEndTest)
BOOST_CHECK_EQUAL(ip->getPoseAt(0.75)->position->x, 1);
BOOST_CHECK_EQUAL(ip->getPoseAt(0.75)->position->y, 1);
BOOST_CHECK_EQUAL(ip->getPoseAt(0.75)->position->z, -1);
-
-
}
BOOST_AUTO_TEST_CASE(tenPointPositionTest)
@@ -255,7 +250,8 @@ BOOST_AUTO_TEST_CASE(tenPointPositionTest)
* E
* R
*/
- std::vector<PoseBasePtr> cp = {pose1, pose2, pose3, pose4, pose5, pose6, pose7, pose8, pose9, pose10, pose11};
+ std::vector<PoseBasePtr> cp = {
+ pose1, pose2, pose3, pose4, pose5, pose6, pose7, pose8, pose9, pose10, pose11};
LinearInterpolationPtr ip = *new LinearInterpolationPtr(new LinearInterpolation(cp));
BOOST_CHECK_EQUAL(ip->getPoseAt(0)->position->x, 2);
@@ -291,8 +287,4 @@ BOOST_AUTO_TEST_CASE(tenPointPositionTest)
BOOST_CHECK_EQUAL(ip->getPoseAt(1)->position->x, 0);
BOOST_CHECK_EQUAL(ip->getPoseAt(1)->position->y, 0);
BOOST_CHECK_EQUAL(ip->getPoseAt(1)->position->z, 0);
-
-
-
}
-
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Interpolation/Tests/SplineInterpolationSegmentTest.cpp b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Interpolation/Tests/SplineInterpolationSegmentTest.cpp
index 740abc5b01d034f65dafbfe3564b30b09504a6bb..a1c49548e031d960ed6be859aa02f3788ba638aa 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Interpolation/Tests/SplineInterpolationSegmentTest.cpp
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Interpolation/Tests/SplineInterpolationSegmentTest.cpp
@@ -1,14 +1,14 @@
#define BOOST_TEST_MODULE ArmarX::RobotTrajectoryDesigner::SplineInterpolationSegment
#define ARMARX_BOOST_TEST
+#include <RobotAPI/libraries/core/FramedPose.h>
+#include <RobotAPI/libraries/core/Pose.h>
+
#include <RobotComponents/Test.h>
#include "../../Interpolation/LinearInterpolation.h"
-#include "../../Util/OrientationConversion.h"
#include "../../Interpolation/SplineInterpolation.h"
-
-#include <RobotAPI/libraries/core/FramedPose.h>
-#include <RobotAPI/libraries/core/Pose.h>
+#include "../../Util/OrientationConversion.h"
using namespace armarx;
@@ -106,11 +106,8 @@ BOOST_AUTO_TEST_CASE(rotateAtPlaceTest)
BOOST_CHECK_EQUAL(ips3->getPoseAt(1)->orientation->qx, q4->qx);
BOOST_CHECK_EQUAL(ips3->getPoseAt(1)->orientation->qy, q4->qy);
BOOST_CHECK_EQUAL(ips3->getPoseAt(1)->orientation->qz, q4->qz);
-
-
}
-
BOOST_AUTO_TEST_CASE(startIsEndTest)
{
Vector3BasePtr pos1 = new Vector3(2, 4, 6);
@@ -143,11 +140,8 @@ BOOST_AUTO_TEST_CASE(startIsEndTest)
BOOST_CHECK_EQUAL(ips2->getPoseAt(1)->position->x, 2);
BOOST_CHECK_EQUAL(ips2->getPoseAt(1)->position->y, 4);
BOOST_CHECK_EQUAL(ips2->getPoseAt(1)->position->z, 6);
-
-
}
-
BOOST_AUTO_TEST_CASE(tenPointPositionTest)
{
Vector3BasePtr pos1 = new Vector3(2, 4, 6);
@@ -194,7 +188,8 @@ BOOST_AUTO_TEST_CASE(tenPointPositionTest)
QuaternionBasePtr ori11 = OrientationConversion::toQuaternion(0, 0, 0);
PoseBasePtr pose11 = new Pose(pos11, ori11);
- std::vector<PoseBasePtr> cp = {pose1, pose2, pose3, pose4, pose5, pose6, pose7, pose8, pose9, pose10, pose11};
+ std::vector<PoseBasePtr> cp = {
+ pose1, pose2, pose3, pose4, pose5, pose6, pose7, pose8, pose9, pose10, pose11};
SplineInterpolationPtr ip = *new SplineInterpolationPtr(new SplineInterpolation(cp));
AbstractInterpolationPtr ips1 = ip->getInterPolationSegment(0);
AbstractInterpolationPtr ips2 = ip->getInterPolationSegment(1);
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Interpolation/Tests/SplineInterpolationTest.cpp b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Interpolation/Tests/SplineInterpolationTest.cpp
index 9b289ebf1e89377d002f415d75c40751c8921c16..a87be03642d02d60b4abee92c2a7e5e2951f80c8 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Interpolation/Tests/SplineInterpolationTest.cpp
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Interpolation/Tests/SplineInterpolationTest.cpp
@@ -2,14 +2,14 @@
#define ARMARX_BOOST_TEST
-#include <RobotComponents/Test.h>
+#include "../../Interpolation/SplineInterpolation.h"
+#include <RobotAPI/libraries/core/Pose.h>
-#include "../exceptions/NoInterpolationPossibleException.h"
+#include <RobotComponents/Test.h>
-#include "../../Interpolation/SplineInterpolation.h"
#include "../../Util/OrientationConversion.h"
-#include <RobotAPI/libraries/core/Pose.h>
+#include "../exceptions/NoInterpolationPossibleException.h"
using namespace armarx;
@@ -44,12 +44,12 @@ BOOST_AUTO_TEST_CASE(basicTest)
BOOST_CHECK_EQUAL(ip->getPoseAt(1)->position->z, -2);
}
-bool is_critical(exceptions::local::NoInterpolationPossibleException const& ex)
+bool
+is_critical(exceptions::local::NoInterpolationPossibleException const& ex)
{
return true;
}
-
BOOST_AUTO_TEST_CASE(rotateAtPlaceTest)
{
Vector3BasePtr pos1 = new Vector3(2, 4, 6);
@@ -95,11 +95,8 @@ BOOST_AUTO_TEST_CASE(rotateAtPlaceTest)
BOOST_CHECK_EQUAL(ip->getPoseAt(1)->orientation->qx, q4->qx);
BOOST_CHECK_EQUAL(ip->getPoseAt(1)->orientation->qy, q4->qy);
BOOST_CHECK_EQUAL(ip->getPoseAt(1)->orientation->qz, q4->qz);
-
-
}
-
BOOST_AUTO_TEST_CASE(startIsEndTest)
{
Vector3BasePtr pos1 = new Vector3(2, 4, 6);
@@ -174,7 +171,8 @@ BOOST_AUTO_TEST_CASE(tenPointPositionTest)
QuaternionBasePtr ori11 = OrientationConversion::toQuaternion(0, 0, 0);
PoseBasePtr pose11 = new Pose(pos11, ori11);
- std::vector<PoseBasePtr> cp = {pose1, pose2, pose3, pose4, pose5, pose6, pose7, pose8, pose9, pose10, pose11};
+ std::vector<PoseBasePtr> cp = {
+ pose1, pose2, pose3, pose4, pose5, pose6, pose7, pose8, pose9, pose10, pose11};
SplineInterpolationPtr ip = *new SplineInterpolationPtr(new SplineInterpolation(cp));
BOOST_CHECK_EQUAL(ip->getPoseAt(0)->position->x, 2);
@@ -211,4 +209,3 @@ BOOST_AUTO_TEST_CASE(tenPointPositionTest)
BOOST_CHECK_EQUAL(ip->getPoseAt(1)->position->y, 0);
BOOST_CHECK_EQUAL(ip->getPoseAt(1)->position->z, 0);
}
-
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/KinematicSolver.cpp b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/KinematicSolver.cpp
index dc7399d56215656fa0cb9f45452fa484b230ba08..c48800492691119a107907090954588ef323fc06 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/KinematicSolver.cpp
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/KinematicSolver.cpp
@@ -20,15 +20,18 @@
* GNU General Public License
*/
#include "KinematicSolver.h"
-#include "VirtualRobot/Workspace/Reachability.h"
+
+#include "RobotAPI/libraries/core/math/MathUtils.h"
+
+#include "RobotComponents/components/RobotIK/RobotIK.h"
+
+#include "Interpolation/LinearInterpolation.h"
#include "MotionPlanning/CSpace/CSpaceSampled.h"
#include "MotionPlanning/Planner/BiRrt.h"
#include "MotionPlanning/Planner/Rrt.h"
-#include "VirtualRobot/CollisionDetection/CDManager.h"
#include "MotionPlanning/PostProcessing/ShortcutProcessor.h"
-#include "RobotAPI/libraries/core/math/MathUtils.h"
-#include "RobotComponents/components/RobotIK/RobotIK.h"
-#include "Interpolation/LinearInterpolation.h"
+#include "VirtualRobot/CollisionDetection/CDManager.h"
+#include "VirtualRobot/Workspace/Reachability.h"
using namespace armarx;
@@ -47,10 +50,14 @@ KinematicSolver::KinematicSolver(ScenePtr scenario, RobotPtr robot)
//initialize IKCalibrations
this->IKCalibrationMap = std::map<std::string, IKCalibration>();
- this->IKCalibrationMap.insert(std::pair<std::string, IKCalibration>("Armar3", IKCalibration::createIKCalibration(1000, 0.005, 500, 0.3, 100, 1.0)));
- this->IKCalibrationMap.insert(std::pair<std::string, IKCalibration>("Armar4", IKCalibration::createIKCalibration(10000, 0.005, 100, 0.6, 100, 1.0)));
- this->IKCalibrationMap.insert(std::pair<std::string, IKCalibration>("Armar6", IKCalibration::createIKCalibration(100, 0.5, 500, 1.1, 500, 1.1)));
- this->IKCalibrationMap.insert(std::pair<std::string, IKCalibration>("Custom", IKCalibration::createIKCalibration(100, 100, 100, 100, 100, 100)));
+ this->IKCalibrationMap.insert(std::pair<std::string, IKCalibration>(
+ "Armar3", IKCalibration::createIKCalibration(1000, 0.005, 500, 0.3, 100, 1.0)));
+ this->IKCalibrationMap.insert(std::pair<std::string, IKCalibration>(
+ "Armar4", IKCalibration::createIKCalibration(10000, 0.005, 100, 0.6, 100, 1.0)));
+ this->IKCalibrationMap.insert(std::pair<std::string, IKCalibration>(
+ "Armar6", IKCalibration::createIKCalibration(100, 0.5, 500, 1.1, 500, 1.1)));
+ this->IKCalibrationMap.insert(std::pair<std::string, IKCalibration>(
+ "Custom", IKCalibration::createIKCalibration(100, 100, 100, 100, 100, 100)));
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///ALTERNATIVE WITH REACHABILITY MAPS
@@ -73,7 +80,13 @@ KinematicSolver::KinematicSolver(ScenePtr scenario, RobotPtr robot)
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
}
-KinematicSolver::IKCalibration KinematicSolver::IKCalibration::createIKCalibration(int smallIterations, double smallStepSize, int mediumIterations, double mediumStepSize, int largeIterations, double largeStepSize)
+KinematicSolver::IKCalibration
+KinematicSolver::IKCalibration::createIKCalibration(int smallIterations,
+ double smallStepSize,
+ int mediumIterations,
+ double mediumStepSize,
+ int largeIterations,
+ double largeStepSize)
{
IKCalibration calibration;
calibration.smallIterations = smallIterations;
@@ -85,11 +98,11 @@ KinematicSolver::IKCalibration KinematicSolver::IKCalibration::createIKCalibrati
return calibration;
}
-
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///SINGLETON-FEATURES////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-KinematicSolverPtr KinematicSolver::getInstance(ScenePtr scenario = NULL, RobotPtr robot = NULL)
+KinematicSolverPtr
+KinematicSolver::getInstance(ScenePtr scenario = NULL, RobotPtr robot = NULL)
{
if (singleton == 0)
{
@@ -100,19 +113,23 @@ KinematicSolverPtr KinematicSolver::getInstance(ScenePtr scenario = NULL, RobotP
return KinematicSolver::singleton;
}
-void KinematicSolver::reset()
+void
+KinematicSolver::reset()
{
ARMARX_INFO << "KinematicSolver reset";
singleton = 0;
}
-
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///IK////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-std::vector<double> KinematicSolver::solveIK(RobotNodeSetPtr kc, PoseBasePtr globalPose, IKSolver::CartesianSelection selection, int iterations)
+std::vector<double>
+KinematicSolver::solveIK(RobotNodeSetPtr kc,
+ PoseBasePtr globalPose,
+ IKSolver::CartesianSelection selection,
+ int iterations)
{
IKCalibration calibration = calibrate();
syncRobotPrx(kc);
@@ -128,7 +145,7 @@ std::vector<double> KinematicSolver::solveIK(RobotNodeSetPtr kc, PoseBasePtr glo
float rn = 1.0f / (float)RAND_MAX;
for (unsigned int i = 0; i < kcPrx->getSize(); i++)
{
- RobotNodePtr ro = kcPrx->getNode(i);
+ RobotNodePtr ro = kcPrx->getNode(i);
float r = (float)rand() * rn;
float v = ro->getJointLimitLo() + (ro->getJointLimitHi() - ro->getJointLimitLo()) * r;
jv.push_back(v);
@@ -137,57 +154,78 @@ std::vector<double> KinematicSolver::solveIK(RobotNodeSetPtr kc, PoseBasePtr glo
//finished randomly setting starting Pose
//solve actual IK
- GenericIKSolverPtr ikSolver = this->IKSetup(calibration.largeStepSize, calibration.largeIterations, 10.0, 0.50, kcPrx);
+ GenericIKSolverPtr ikSolver = this->IKSetup(
+ calibration.largeStepSize, calibration.largeIterations, 10.0, 0.50, kcPrx);
Pose targetPose = Pose(globalPose->position, globalPose->orientation);
std::vector<float> output = ikSolver->solveNoRNSUpdate(targetPose.toEigen(), selection);
jointValues = std::vector<double>(output.begin(), output.end());
failureCounter++;
- }
- while (jointValues.size() == 0 && failureCounter <= iterations); //randomly resets the robot Pose
+ } while (jointValues.size() == 0 &&
+ failureCounter <= iterations); //randomly resets the robot Pose
return jointValues;
}
-std::vector<double> KinematicSolver::solveIKRelative(RobotNodeSetPtr kc, PoseBasePtr framedPose, IKSolver::CartesianSelection selection)
+std::vector<double>
+KinematicSolver::solveIKRelative(RobotNodeSetPtr kc,
+ PoseBasePtr framedPose,
+ IKSolver::CartesianSelection selection)
{
PosePtr localPose = PosePtr(new Pose(framedPose->position, framedPose->orientation));
- PoseBasePtr globalPose = PoseBasePtr(new Pose(kc->getKinematicRoot()->toGlobalCoordinateSystem(localPose->toEigen())));
+ PoseBasePtr globalPose = PoseBasePtr(
+ new Pose(kc->getKinematicRoot()->toGlobalCoordinateSystem(localPose->toEigen())));
return this->solveIK(kc, globalPose, selection, 50);
}
-
-std::vector<float> KinematicSolver::solveRecursiveIK(RobotNodeSetPtr kc, std::vector<float> startingPoint, PoseBasePtr globalDestination, VirtualRobot::IKSolver::CartesianSelection selection)
+std::vector<float>
+KinematicSolver::solveRecursiveIK(RobotNodeSetPtr kc,
+ std::vector<float> startingPoint,
+ PoseBasePtr globalDestination,
+ VirtualRobot::IKSolver::CartesianSelection selection)
{
IKCalibration calibration = calibrate();
syncRobotPrx(kc);
RobotNodeSetPtr kcPrx = robotProxy->getRobotNodeSet(kc->getName());
kcPrx->setJointValues(startingPoint);
- GenericIKSolverPtr ikSolver = this->IKSetup(calibration.mediumStepSize, calibration.mediumIterations, 10.0, 1.0, kcPrx);
+ GenericIKSolverPtr ikSolver =
+ this->IKSetup(calibration.mediumStepSize, calibration.mediumIterations, 10.0, 1.0, kcPrx);
Pose targetPose = Pose(globalDestination->position, globalDestination->orientation);
std::vector<float> output = ikSolver->solveNoRNSUpdate(targetPose.toEigen(), selection);
if (output.size() == 0)
{
- std::vector<double> random = this->solveIK(kc, PoseBasePtr(new Pose(targetPose.toEigen())), selection, 5);
+ std::vector<double> random =
+ this->solveIK(kc, PoseBasePtr(new Pose(targetPose.toEigen())), selection, 5);
output = std::vector<float>(random.begin(), random.end());
}
return output;
}
-std::vector<float> KinematicSolver::solveRecursiveIKRelative(RobotNodeSetPtr kc, std::vector<float> startingPoint, PoseBasePtr framedDestination, IKSolver::CartesianSelection selection)
+std::vector<float>
+KinematicSolver::solveRecursiveIKRelative(RobotNodeSetPtr kc,
+ std::vector<float> startingPoint,
+ PoseBasePtr framedDestination,
+ IKSolver::CartesianSelection selection)
{
syncRobotPrx(kc);
- PosePtr localPose = PosePtr(new Pose(framedDestination->position, framedDestination->orientation));
+ PosePtr localPose =
+ PosePtr(new Pose(framedDestination->position, framedDestination->orientation));
//std::cout << localPose->toEigen();
- PoseBasePtr globalPose = PoseBasePtr(new Pose(kc->getKinematicRoot()->toGlobalCoordinateSystem(localPose->toEigen())));
+ PoseBasePtr globalPose = PoseBasePtr(
+ new Pose(kc->getKinematicRoot()->toGlobalCoordinateSystem(localPose->toEigen())));
return this->solveRecursiveIK(kc, startingPoint, globalPose, selection);
}
-std::vector<double> KinematicSolver::solveRecursiveIK(RobotNodeSetPtr kc, std::vector<double> startingPoint, PoseBasePtr globalDestination, IKSolver::CartesianSelection selection)
+std::vector<double>
+KinematicSolver::solveRecursiveIK(RobotNodeSetPtr kc,
+ std::vector<double> startingPoint,
+ PoseBasePtr globalDestination,
+ IKSolver::CartesianSelection selection)
{
IKCalibration calibration = calibrate();
syncRobotPrx(kc);
RobotNodeSetPtr kcPrx = robotProxy->getRobotNodeSet(kc->getName());
kcPrx->setJointValues(std::vector<float>(startingPoint.begin(), startingPoint.end()));
- GenericIKSolverPtr ikSolver = this->IKSetup(calibration.smallStepSize, calibration.smallIterations, 10, 0.5, kcPrx);
+ GenericIKSolverPtr ikSolver =
+ this->IKSetup(calibration.smallStepSize, calibration.smallIterations, 10, 0.5, kcPrx);
Pose targetPose = Pose(globalDestination->position, globalDestination->orientation);
//std::cout << targetPose.toEigen();
std::vector<float> output = ikSolver->solveNoRNSUpdate(targetPose.toEigen(), selection);
@@ -195,17 +233,29 @@ std::vector<double> KinematicSolver::solveRecursiveIK(RobotNodeSetPtr kc, std::v
return outputd;
}
-std::vector<double> KinematicSolver::solveRecursiveIKRelative(RobotNodeSetPtr kc, std::vector<double> startingPoint, PoseBasePtr framedDestination, IKSolver::CartesianSelection selection)
+std::vector<double>
+KinematicSolver::solveRecursiveIKRelative(RobotNodeSetPtr kc,
+ std::vector<double> startingPoint,
+ PoseBasePtr framedDestination,
+ IKSolver::CartesianSelection selection)
{
syncRobotPrx(kc);
- PosePtr localPose = PosePtr(new Pose(framedDestination->position, framedDestination->orientation));
- PoseBasePtr globalPose = PoseBasePtr(new Pose(kc->getKinematicRoot()->toGlobalCoordinateSystem(localPose->toEigen())));
+ PosePtr localPose =
+ PosePtr(new Pose(framedDestination->position, framedDestination->orientation));
+ PoseBasePtr globalPose = PoseBasePtr(
+ new Pose(kc->getKinematicRoot()->toGlobalCoordinateSystem(localPose->toEigen())));
return this->solveRecursiveIK(kc, startingPoint, globalPose, selection);
}
-std::vector<std::vector<double>> KinematicSolver::solveRecursiveIK(RobotNodeSetPtr kc, std::vector<double> startingPoint, std::vector<PoseBasePtr> globalDestinations, std::vector<IKSolver::CartesianSelection> selections)
+std::vector<std::vector<double>>
+KinematicSolver::solveRecursiveIK(RobotNodeSetPtr kc,
+ std::vector<double> startingPoint,
+ std::vector<PoseBasePtr> globalDestinations,
+ std::vector<IKSolver::CartesianSelection> selections)
{
- if (!isReachable(kc, globalDestinations[globalDestinations.size() - 1], selections[selections.size() - 1]))
+ if (!isReachable(kc,
+ globalDestinations[globalDestinations.size() - 1],
+ selections[selections.size() - 1]))
{
throw LocalException("Desired Pose is not Reachable");
}
@@ -222,15 +272,19 @@ std::vector<std::vector<double>> KinematicSolver::solveRecursiveIK(RobotNodeSetP
int failureCounter = 0;
int singularities = 0;
std::vector<std::vector<double>> result = std::vector<std::vector<double>>();
- std::vector<float> currentStart = std::vector<float>(startingPoint.begin(), startingPoint.end());
+ std::vector<float> currentStart =
+ std::vector<float>(startingPoint.begin(), startingPoint.end());
//solve ik step-by-step
for (PoseBasePtr currentDest : globalDestinations)
{
- std::vector<double> temp = std::vector<double>(currentStart.begin(), currentStart.end());
+ std::vector<double> temp =
+ std::vector<double>(currentStart.begin(), currentStart.end());
if (i != 0)
{
- PosePtr priorPose = PosePtr(new Pose(globalDestinations.at(i - 1)->position, globalDestinations.at(i - 1)->orientation));
- PosePtr currentPose = PosePtr(new Pose(currentDest->position, currentDest->orientation));
+ PosePtr priorPose = PosePtr(new Pose(globalDestinations.at(i - 1)->position,
+ globalDestinations.at(i - 1)->orientation));
+ PosePtr currentPose =
+ PosePtr(new Pose(currentDest->position, currentDest->orientation));
if (priorPose->toEigen() == currentPose->toEigen() && temp.size() != 0)
{
@@ -238,7 +292,8 @@ std::vector<std::vector<double>> KinematicSolver::solveRecursiveIK(RobotNodeSetP
continue;
}
}
- std::vector<double> currentResult = KinematicSolver::getInstance()->solveRecursiveIK(kc, temp, currentDest, selections.at(i));
+ std::vector<double> currentResult = KinematicSolver::getInstance()->solveRecursiveIK(
+ kc, temp, currentDest, selections.at(i));
if (currentResult.size() == 0)
{
failureCounter++;
@@ -281,15 +336,19 @@ std::vector<std::vector<double>> KinematicSolver::solveRecursiveIK(RobotNodeSetP
PoseBasePtr endPose = PoseBasePtr(new Pose(node->getGlobalPose()));
std::vector<PoseBasePtr> cp = {startPose, endPose};
IKCalibration calibration = calibrate();
- AbstractInterpolationPtr ip = AbstractInterpolationPtr(new LinearInterpolation(cp));
+ AbstractInterpolationPtr ip =
+ AbstractInterpolationPtr(new LinearInterpolation(cp));
for (int i = 0; i < 25; i++)
{
PoseBasePtr poseBase = ip->getPoseAt(i * 0.04);
- PosePtr posePose = PosePtr(new Pose(poseBase->position, poseBase->orientation));
- GenericIKSolverPtr ikSolver = this->IKSetup(calibration.smallStepSize, calibration.smallIterations, 5, 0.5, kcPrx);
+ PosePtr posePose =
+ PosePtr(new Pose(poseBase->position, poseBase->orientation));
+ GenericIKSolverPtr ikSolver = this->IKSetup(
+ calibration.smallStepSize, calibration.smallIterations, 5, 0.5, kcPrx);
DifferentialIKPtr dIK = ikSolver->getDifferentialIK();
dIK->initialize();
- PosePtr pose = PosePtr(new Pose(currentDest->position, currentDest->orientation));
+ PosePtr pose =
+ PosePtr(new Pose(currentDest->position, currentDest->orientation));
dIK->setGoal(posePose->toEigen(), node, IKSolver::CartesianSelection::X);
dIK->setGoal(pose->toEigen(), kcPrx->getTCP(), sel);
@@ -304,8 +363,10 @@ std::vector<std::vector<double>> KinematicSolver::solveRecursiveIK(RobotNodeSetP
}
else
{
- dIK->setGoal(posePose->toEigen(), node, IKSolver::CartesianSelection::Y);
- kcPrx->setJointValues(std::vector<float>(nowStart.begin(), nowStart.end()));
+ dIK->setGoal(
+ posePose->toEigen(), node, IKSolver::CartesianSelection::Y);
+ kcPrx->setJointValues(
+ std::vector<float>(nowStart.begin(), nowStart.end()));
if (dIK->solveIK(0.4))
{
//ARMARX_INFO << "IK SOLUTION FOUND " + std::to_string(i);
@@ -317,8 +378,10 @@ std::vector<std::vector<double>> KinematicSolver::solveRecursiveIK(RobotNodeSetP
{
if (dIK->solveIK(0.4))
{
- dIK->setGoal(posePose->toEigen(), node, IKSolver::CartesianSelection::Z);
- kcPrx->setJointValues(std::vector<float>(nowStart.begin(), nowStart.end()));
+ dIK->setGoal(
+ posePose->toEigen(), node, IKSolver::CartesianSelection::Z);
+ kcPrx->setJointValues(
+ std::vector<float>(nowStart.begin(), nowStart.end()));
//ARMARX_INFO << "IK SOLUTION FOUND " + std::to_string(i);
std::vector<float> ca = kcPrx->getJointValues();
nowStart = std::vector<double>(ca.begin(), ca.end());
@@ -350,11 +413,9 @@ std::vector<std::vector<double>> KinematicSolver::solveRecursiveIK(RobotNodeSetP
ARMARX_INFO << result[result.size() - 1];
}
}*/
-
}
currentStart = std::vector<float>(nowStart.begin(), nowStart.end());
}
-
}
else
@@ -362,7 +423,6 @@ std::vector<std::vector<double>> KinematicSolver::solveRecursiveIK(RobotNodeSetP
result.push_back(currentResult);
currentStart = std::vector<float>(currentResult.begin(), currentResult.end());
}
-
}
tries++;
if (minSingularities <= 0 || singularities <= minSingularities)
@@ -374,16 +434,20 @@ std::vector<std::vector<double>> KinematicSolver::solveRecursiveIK(RobotNodeSetP
return bestResult;
}
-
-
-std::vector<std::vector<double>> KinematicSolver::solveRecursiveIKNoMotionPlanning(RobotNodeSetPtr kc, std::vector<double> startingPoint, std::vector<PoseBasePtr> globalDestinations, std::vector<IKSolver::CartesianSelection> selections)
+std::vector<std::vector<double>>
+KinematicSolver::solveRecursiveIKNoMotionPlanning(
+ RobotNodeSetPtr kc,
+ std::vector<double> startingPoint,
+ std::vector<PoseBasePtr> globalDestinations,
+ std::vector<IKSolver::CartesianSelection> selections)
{
syncRobotPrx(kc);
//setup data
int failureCounter = 0;
RobotNodeSetPtr kcPrx = robotProxy->getRobotNodeSet(kc->getName());
std::vector<std::vector<double>> result = std::vector<std::vector<double>>();
- std::vector<float> currentStart = std::vector<float>(startingPoint.begin(), startingPoint.end());
+ std::vector<float> currentStart =
+ std::vector<float>(startingPoint.begin(), startingPoint.end());
int i = 0;
//solve ik step-by-step
@@ -392,8 +456,10 @@ std::vector<std::vector<double>> KinematicSolver::solveRecursiveIKNoMotionPlanni
std::vector<double> temp = std::vector<double>(currentStart.begin(), currentStart.end());
if (i != 0)
{
- PosePtr priorPose = PosePtr(new Pose(globalDestinations.at(i - 1)->position, globalDestinations.at(i - 1)->orientation));
- PosePtr currentPose = PosePtr(new Pose(currentDest->position, currentDest->orientation));
+ PosePtr priorPose = PosePtr(new Pose(globalDestinations.at(i - 1)->position,
+ globalDestinations.at(i - 1)->orientation));
+ PosePtr currentPose =
+ PosePtr(new Pose(currentDest->position, currentDest->orientation));
if (priorPose->toEigen() == currentPose->toEigen() && temp.size() != 0)
{
result.push_back(temp);
@@ -401,7 +467,8 @@ std::vector<std::vector<double>> KinematicSolver::solveRecursiveIKNoMotionPlanni
continue;
}
}
- std::vector<double> currentResult = KinematicSolver::getInstance()->solveRecursiveIK(kc, temp, currentDest, selections.at(i));
+ std::vector<double> currentResult = KinematicSolver::getInstance()->solveRecursiveIK(
+ kc, temp, currentDest, selections.at(i));
i++;
if (currentResult.size() == 0)
{
@@ -410,9 +477,11 @@ std::vector<std::vector<double>> KinematicSolver::solveRecursiveIKNoMotionPlanni
currentStart = std::vector<float>(temp.begin(), temp.end());
if (failureCounter > (double)globalDestinations.size() / 20.0)
{
- throw LocalException("Too, many faults at current calculation: " + std::to_string(failureCounter) + " Poses not reachable - only " + std::to_string((double)globalDestinations.size() / 5.0) + " allowed!");
+ throw LocalException(
+ "Too, many faults at current calculation: " + std::to_string(failureCounter) +
+ " Poses not reachable - only " +
+ std::to_string((double)globalDestinations.size() / 5.0) + " allowed!");
}
-
}
else
{
@@ -424,13 +493,15 @@ std::vector<std::vector<double>> KinematicSolver::solveRecursiveIKNoMotionPlanni
return result;
}
-
-
-
-std::vector<std::vector<double>> KinematicSolver::solveRecursiveIK(VirtualRobot::RobotNodeSetPtr kc, std::vector<double> startingPoint, std::vector<PoseBasePtr> globalDestinations, IKSolver::CartesianSelection selection)
+std::vector<std::vector<double>>
+KinematicSolver::solveRecursiveIK(VirtualRobot::RobotNodeSetPtr kc,
+ std::vector<double> startingPoint,
+ std::vector<PoseBasePtr> globalDestinations,
+ IKSolver::CartesianSelection selection)
{
syncRobotPrx(kc);
- std::vector<IKSolver::CartesianSelection> selections = std::vector<IKSolver::CartesianSelection>();
+ std::vector<IKSolver::CartesianSelection> selections =
+ std::vector<IKSolver::CartesianSelection>();
for (unsigned int i = 0; i < globalDestinations.size(); i++)
{
selections.push_back(selection);
@@ -438,40 +509,48 @@ std::vector<std::vector<double>> KinematicSolver::solveRecursiveIK(VirtualRobot:
return this->solveRecursiveIK(kc, startingPoint, globalDestinations, selections);
}
-std::vector<std::vector<double> > KinematicSolver::solveRecursiveIKRelative(RobotNodeSetPtr kc, std::vector<double> startingPoint, std::vector<PoseBasePtr> framedDestinations, std::vector<IKSolver::CartesianSelection> selections)
+std::vector<std::vector<double>>
+KinematicSolver::solveRecursiveIKRelative(RobotNodeSetPtr kc,
+ std::vector<double> startingPoint,
+ std::vector<PoseBasePtr> framedDestinations,
+ std::vector<IKSolver::CartesianSelection> selections)
{
syncRobotPrx(kc);
std::vector<PoseBasePtr> globalDestinations = std::vector<PoseBasePtr>();
for (PoseBasePtr framedDestination : framedDestinations)
{
- PosePtr localPose = PosePtr(new Pose(framedDestination->position, framedDestination->orientation));
+ PosePtr localPose =
+ PosePtr(new Pose(framedDestination->position, framedDestination->orientation));
//std::cout << localPose->toEigen();
- PoseBasePtr globalPose = PoseBasePtr(new Pose(kc->getKinematicRoot()->toGlobalCoordinateSystem(localPose->toEigen())));
+ PoseBasePtr globalPose = PoseBasePtr(
+ new Pose(kc->getKinematicRoot()->toGlobalCoordinateSystem(localPose->toEigen())));
globalDestinations.push_back(globalPose);
}
return this->solveRecursiveIK(kc, startingPoint, globalDestinations, selections);
}
-
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///FORWARD KINEMATIC & REACHABILITY///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-PoseBasePtr KinematicSolver::doForwardKinematic(RobotNodeSetPtr kc, std::vector<double> jointAngles)
+PoseBasePtr
+KinematicSolver::doForwardKinematic(RobotNodeSetPtr kc, std::vector<double> jointAngles)
{
syncRobotPrx(kc);
RobotNodeSetPtr kcPrx = this->setupProxy(kc, jointAngles);
return PoseBasePtr(new Pose(kcPrx->getTCP()->getGlobalPose()));
}
-PoseBasePtr KinematicSolver::doForwardKinematicRelative(RobotNodeSetPtr kc, std::vector<double> jointAngles)
+PoseBasePtr
+KinematicSolver::doForwardKinematicRelative(RobotNodeSetPtr kc, std::vector<double> jointAngles)
{
syncRobotPrx(kc);
RobotNodeSetPtr kcPrx = this->setupProxy(kc, jointAngles);
- return PoseBasePtr(new Pose(kcPrx->getKinematicRoot()->toLocalCoordinateSystem(kcPrx->getTCP()->getGlobalPose())));
+ return PoseBasePtr(new Pose(
+ kcPrx->getKinematicRoot()->toLocalCoordinateSystem(kcPrx->getTCP()->getGlobalPose())));
}
-
-bool KinematicSolver::isReachable(RobotNodeSetPtr rns, PoseBasePtr pose, IKSolver::CartesianSelection cs)
+bool
+KinematicSolver::isReachable(RobotNodeSetPtr rns, PoseBasePtr pose, IKSolver::CartesianSelection cs)
{
syncRobotPrx(rns);
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
@@ -493,7 +572,8 @@ bool KinematicSolver::isReachable(RobotNodeSetPtr rns, PoseBasePtr pose, IKSolve
return this->solveIK(rns, pose, cs, 50).size() != 0;
}
-void KinematicSolver::syncRobotPrx(RobotNodeSetPtr rns)
+void
+KinematicSolver::syncRobotPrx(RobotNodeSetPtr rns)
{
if (currentRns != rns->getName())
{
@@ -502,7 +582,8 @@ void KinematicSolver::syncRobotPrx(RobotNodeSetPtr rns)
}
}
-void KinematicSolver::syncRobotPrx()
+void
+KinematicSolver::syncRobotPrx()
{
ARMARX_INFO << "Syncing Proxy";
for (RobotNodePtr node : robot->getRobotNodes())
@@ -514,15 +595,22 @@ void KinematicSolver::syncRobotPrx()
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///CONVINIENCE-METHODS///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-GenericIKSolverPtr KinematicSolver::IKSetup(double jacStepSize, int IKIterations, double vectorError, double orientationError, RobotNodeSetPtr kc)
+GenericIKSolverPtr
+KinematicSolver::IKSetup(double jacStepSize,
+ int IKIterations,
+ double vectorError,
+ double orientationError,
+ RobotNodeSetPtr kc)
{
- GenericIKSolverPtr ikSolver = GenericIKSolverPtr(new GenericIKSolver(kc, JacobiProvider::eSVDDamped));
+ GenericIKSolverPtr ikSolver =
+ GenericIKSolverPtr(new GenericIKSolver(kc, JacobiProvider::eSVDDamped));
ikSolver->setupJacobian(jacStepSize, IKIterations);
ikSolver->setMaximumError(vectorError, orientationError);
return ikSolver;
}
-RobotNodeSetPtr KinematicSolver::setupProxy(RobotNodeSetPtr kc, std::vector<double> jointAngles)
+RobotNodeSetPtr
+KinematicSolver::setupProxy(RobotNodeSetPtr kc, std::vector<double> jointAngles)
{
RobotNodeSetPtr kcPrx = robotProxy->getRobotNodeSet(kc->getName());
std::vector<float> jointValues = std::vector<float>(jointAngles.begin(), jointAngles.end());
@@ -530,10 +618,12 @@ RobotNodeSetPtr KinematicSolver::setupProxy(RobotNodeSetPtr kc, std::vector<dou
return kcPrx;
}
-KinematicSolver::IKCalibration KinematicSolver::calibrate()
+KinematicSolver::IKCalibration
+KinematicSolver::calibrate()
{
- if (robot->getName() != "Armar3" && robot->getName() != "Armar4" && robot->getName() != "Armar6")
+ if (robot->getName() != "Armar3" && robot->getName() != "Armar4" &&
+ robot->getName() != "Armar6")
{
return this->IKCalibrationMap.at("Armar3");
}
@@ -543,13 +633,18 @@ KinematicSolver::IKCalibration KinematicSolver::calibrate()
}
}
-double KinematicSolver::getMaxDistance(Eigen::Vector3f destination, std::vector<std::vector<double> > jointAngles, RobotNodeSetPtr rns)
+double
+KinematicSolver::getMaxDistance(Eigen::Vector3f destination,
+ std::vector<std::vector<double>> jointAngles,
+ RobotNodeSetPtr rns)
{
double max = 0;
for (std::vector<double> ja : jointAngles)
{
PoseBasePtr pose = doForwardKinematicRelative(rns, ja);
- double distance = sqrt(pow(pose->position->x - destination[0], 2) + pow(pose->position->y - destination[1], 2) + pow(pose->position->z - destination[2], 2));
+ double distance = sqrt(pow(pose->position->x - destination[0], 2) +
+ pow(pose->position->y - destination[1], 2) +
+ pow(pose->position->z - destination[2], 2));
if (distance > max)
{
max = distance;
@@ -558,7 +653,10 @@ double KinematicSolver::getMaxDistance(Eigen::Vector3f destination, std::vector<
return max;
}
-int KinematicSolver::getFurthestNode(RobotNodeSetPtr rns, Eigen::VectorXf startConfig, Eigen::VectorXf endConfig)
+int
+KinematicSolver::getFurthestNode(RobotNodeSetPtr rns,
+ Eigen::VectorXf startConfig,
+ Eigen::VectorXf endConfig)
{
double max = 0;
int best = rns->getSize() - 1;
@@ -592,5 +690,4 @@ int KinematicSolver::getFurthestNode(RobotNodeSetPtr rns, Eigen::VectorXf startC
}
}
return best;
-
}
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/KinematicSolver.h b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/KinematicSolver.h
index 789bbd69ab969bf4a67c190935ddf5f46fcda8ba..39b936090fc761f53b5fac69e10abe6a72abffc2 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/KinematicSolver.h
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/KinematicSolver.h
@@ -30,16 +30,17 @@
#include <Eigen/Geometry>
///VIRTUAL-ROBOT-INCLUDES
-#include <VirtualRobot/RobotNodeSet.h>
-#include <VirtualRobot/Robot.h>
-#include <VirtualRobot/IK/GenericIKSolver.h>
#include <VirtualRobot/IK/DifferentialIK.h>
-#include "VirtualRobot/IK/AdvancedIKSolver.h"
+#include <VirtualRobot/IK/GenericIKSolver.h>
+#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/RobotNodeSet.h>
+
#include "MotionPlanning/CSpace/CSpacePath.h"
+#include "VirtualRobot/IK/AdvancedIKSolver.h"
///STANDARD-INCLUDES
-#include <memory>
#include <map>
+#include <memory>
namespace armarx
{
@@ -67,11 +68,15 @@ namespace armarx
double mediumStepSize;
int largeIterations;
double largeStepSize;
- static IKCalibration createIKCalibration(int smallIterations, double smallStepSize, int mediumIterations, double mediumStepSize, int largeIterations, double largeStepSize);
+ static IKCalibration createIKCalibration(int smallIterations,
+ double smallStepSize,
+ int mediumIterations,
+ double mediumStepSize,
+ int largeIterations,
+ double largeStepSize);
};
public:
-
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///SINGLETON-FEATURES////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
@@ -82,7 +87,8 @@ namespace armarx
* @param robot the robot for which the Kinematic Solution is calculated
* @return the single instance of KinematicSolver
*/
- static std::shared_ptr<KinematicSolver> getInstance(VirtualRobot::ScenePtr scenario, VirtualRobot::RobotPtr robot);
+ static std::shared_ptr<KinematicSolver> getInstance(VirtualRobot::ScenePtr scenario,
+ VirtualRobot::RobotPtr robot);
/**
* @brief reset
@@ -110,7 +116,10 @@ namespace armarx
* @param selection the attributes of the pose that are considered when solving the IK problem, e.g. position, orientation or all
* @return an mostly random IK solution for the given PoseBase and kinematic Chain
*/
- std::vector<double> solveIK(VirtualRobot::RobotNodeSetPtr kc, armarx::PoseBasePtr pose, VirtualRobot::IKSolver::CartesianSelection selection, int iterations);
+ std::vector<double> solveIK(VirtualRobot::RobotNodeSetPtr kc,
+ armarx::PoseBasePtr pose,
+ VirtualRobot::IKSolver::CartesianSelection selection,
+ int iterations);
/**
* @brief solveIK returns a random solution for an inverse Kinematic problem with no consideration of the current pose of the robot
@@ -119,7 +128,9 @@ namespace armarx
* @param selection the attributes of the pose that are considered when solving the IK problem, e.g. position, orientation or all
* @return an mostly random IK solution for the given PoseBase and kinematic Chain RELATIVE TO THE ROBOTS KINEMATIC ROOT
*/
- std::vector<double> solveIKRelative(VirtualRobot::RobotNodeSetPtr kc, armarx::PoseBasePtr framedPose, VirtualRobot::IKSolver::CartesianSelection selection);
+ std::vector<double> solveIKRelative(VirtualRobot::RobotNodeSetPtr kc,
+ armarx::PoseBasePtr framedPose,
+ VirtualRobot::IKSolver::CartesianSelection selection);
/**
* @brief solveRecursiveIK solves IK based on a starting point and an End point based on one step
@@ -129,7 +140,10 @@ namespace armarx
* @param selection the attributes of the pose that are considered when solving the IK problem, e.g. position, orientation or all
* @return an mostly random IK solution for the given PoseBase and kinematic Chain in form joint angle configuration
*/
- std::vector<float> solveRecursiveIK(VirtualRobot::RobotNodeSetPtr kc, std::vector<float> startingPoint, PoseBasePtr globalDestination, VirtualRobot::IKSolver::CartesianSelection selection);
+ std::vector<float> solveRecursiveIK(VirtualRobot::RobotNodeSetPtr kc,
+ std::vector<float> startingPoint,
+ PoseBasePtr globalDestination,
+ VirtualRobot::IKSolver::CartesianSelection selection);
/**
* @brief solveRecursiveIK solves IK based on a starting point and an End point based on one step
@@ -139,7 +153,11 @@ namespace armarx
* @param selection the attributes of the pose that are considered when solving the IK problem, e.g. position, orientation or all
* @return an mostly random IK solution for the given PoseBase and kinematic Chain in form joint angle configuration
*/
- std::vector<float> solveRecursiveIKRelative(VirtualRobot::RobotNodeSetPtr kc, std::vector<float> startingPoint, PoseBasePtr framedDestination, VirtualRobot::IKSolver::CartesianSelection selection);
+ std::vector<float>
+ solveRecursiveIKRelative(VirtualRobot::RobotNodeSetPtr kc,
+ std::vector<float> startingPoint,
+ PoseBasePtr framedDestination,
+ VirtualRobot::IKSolver::CartesianSelection selection);
/**
@@ -150,7 +168,10 @@ namespace armarx
* @param selection the attributes of the pose that are considered when solving the IK problem, e.g. position, orientation or all
* @return an mostly random IK solution for the given PoseBase and kinematic Chain in form joint angle configuration
*/
- std::vector<double> solveRecursiveIK(VirtualRobot::RobotNodeSetPtr kc, std::vector<double> startingPoint, PoseBasePtr globalDestination, VirtualRobot::IKSolver::CartesianSelection selection);
+ std::vector<double> solveRecursiveIK(VirtualRobot::RobotNodeSetPtr kc,
+ std::vector<double> startingPoint,
+ PoseBasePtr globalDestination,
+ VirtualRobot::IKSolver::CartesianSelection selection);
/**
* @brief solveRecursiveIK solves IK based on a starting point and an End point based on one step
@@ -160,7 +181,11 @@ namespace armarx
* @param selection the attributes of the pose that are considered when solving the IK problem, e.g. position, orientation or all
* @return an mostly random IK solution for the given PoseBase and kinematic Chain in form joint angle configuration
*/
- std::vector<double> solveRecursiveIKRelative(VirtualRobot::RobotNodeSetPtr kc, std::vector<double> startingPoint, PoseBasePtr framedDestination, VirtualRobot::IKSolver::CartesianSelection selection);
+ std::vector<double>
+ solveRecursiveIKRelative(VirtualRobot::RobotNodeSetPtr kc,
+ std::vector<double> startingPoint,
+ PoseBasePtr framedDestination,
+ VirtualRobot::IKSolver::CartesianSelection selection);
/**
* @brief solveRecursiveIK solves IK based on a starting point and an End point based on one step
@@ -170,11 +195,23 @@ namespace armarx
* @param selection the attributes of the pose that are considered when solving the IK problem, e.g. position, orientation or all
* @return an mostly random IK solution for the given PoseBase and kinematic Chain in form joint angle configuration
*/
- std::vector<std::vector<double>> solveRecursiveIK(VirtualRobot::RobotNodeSetPtr kc, std::vector<double> startingPoint, std::vector<PoseBasePtr> globalDestinations, std::vector<VirtualRobot::IKSolver::CartesianSelection> selections);
-
- std::vector<std::vector<double>> solveRecursiveIKNoMotionPlanning(VirtualRobot::RobotNodeSetPtr kc, std::vector<double> startingPoint, std::vector<PoseBasePtr> globalDestinations, std::vector<VirtualRobot::IKSolver::CartesianSelection> selections);
-
- std::vector<std::vector<double>> solveRecursiveIK(VirtualRobot::RobotNodeSetPtr kc, std::vector<double> startingPoint, std::vector<PoseBasePtr> globalDestinations, VirtualRobot::IKSolver::CartesianSelection selection);
+ std::vector<std::vector<double>>
+ solveRecursiveIK(VirtualRobot::RobotNodeSetPtr kc,
+ std::vector<double> startingPoint,
+ std::vector<PoseBasePtr> globalDestinations,
+ std::vector<VirtualRobot::IKSolver::CartesianSelection> selections);
+
+ std::vector<std::vector<double>> solveRecursiveIKNoMotionPlanning(
+ VirtualRobot::RobotNodeSetPtr kc,
+ std::vector<double> startingPoint,
+ std::vector<PoseBasePtr> globalDestinations,
+ std::vector<VirtualRobot::IKSolver::CartesianSelection> selections);
+
+ std::vector<std::vector<double>>
+ solveRecursiveIK(VirtualRobot::RobotNodeSetPtr kc,
+ std::vector<double> startingPoint,
+ std::vector<PoseBasePtr> globalDestinations,
+ VirtualRobot::IKSolver::CartesianSelection selection);
/**
* @brief solveRecursiveIK solves IK based on a starting point and an End point based on one step
@@ -184,7 +221,11 @@ namespace armarx
* @param selection the attributes of the pose that are considered when solving the IK problem, e.g. position, orientation or all
* @return an mostly random IK solution for the given PoseBase and kinematic Chain in form joint angle configuration
*/
- std::vector<std::vector<double>> solveRecursiveIKRelative(VirtualRobot::RobotNodeSetPtr kc, std::vector<double> startingPoint, std::vector<PoseBasePtr> framedDestinations, std::vector<VirtualRobot::IKSolver::CartesianSelection> selections);
+ std::vector<std::vector<double>> solveRecursiveIKRelative(
+ VirtualRobot::RobotNodeSetPtr kc,
+ std::vector<double> startingPoint,
+ std::vector<PoseBasePtr> framedDestinations,
+ std::vector<VirtualRobot::IKSolver::CartesianSelection> selections);
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///FORWARD KINEMATIC & REACHABILITY///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
@@ -195,7 +236,8 @@ namespace armarx
* @param jointAngles the joint angles which the robot should have
* @return a vector of joint angle configurations to reach destination 1,..., n in exactly the same order as the destinations
*/
- PoseBasePtr doForwardKinematic(VirtualRobot::RobotNodeSetPtr kc, std::vector<double> jointAngles);
+ PoseBasePtr doForwardKinematic(VirtualRobot::RobotNodeSetPtr kc,
+ std::vector<double> jointAngles);
/**
* @brief doForwardKinematic executes the given jointAngles on the loaded Robot
@@ -203,14 +245,17 @@ namespace armarx
* @param jointAngles the joint angles which the robot should have
* @return a vector of joint angle configurations to reach destination 1,..., n in exactly the same order as the destinations
*/
- PoseBasePtr doForwardKinematicRelative(VirtualRobot::RobotNodeSetPtr kc, std::vector<double> jointAngles);
+ PoseBasePtr doForwardKinematicRelative(VirtualRobot::RobotNodeSetPtr kc,
+ std::vector<double> jointAngles);
/**
* @brief isReachable calculates whether the PoseBase pose is reachable by the loaded robot or not
* @param pose the pose that should be reachable by the robot
* @return returns true if the pose is reachable, false otherwise
*/
- bool isReachable(VirtualRobot::RobotNodeSetPtr rns, PoseBasePtr globalPose, VirtualRobot::IKSolver::CartesianSelection cs);
+ bool isReachable(VirtualRobot::RobotNodeSetPtr rns,
+ PoseBasePtr globalPose,
+ VirtualRobot::IKSolver::CartesianSelection cs);
private:
@@ -220,20 +265,27 @@ namespace armarx
std::map<std::string, IKCalibration> IKCalibrationMap;
//std::map<std::string, VirtualRobot::ReachabilityPtr> reachabilityMap;
KinematicSolver(VirtualRobot::ScenePtr scenario, VirtualRobot::RobotPtr robot);
- VirtualRobot::GenericIKSolverPtr IKSetup(double jacStepSize, int IKIterations, double vectorError, double orientationError, VirtualRobot::RobotNodeSetPtr kc);
- VirtualRobot::RobotNodeSetPtr setupProxy(VirtualRobot::RobotNodeSetPtr kc, std::vector<double> jointAngles);
+ VirtualRobot::GenericIKSolverPtr IKSetup(double jacStepSize,
+ int IKIterations,
+ double vectorError,
+ double orientationError,
+ VirtualRobot::RobotNodeSetPtr kc);
+ VirtualRobot::RobotNodeSetPtr setupProxy(VirtualRobot::RobotNodeSetPtr kc,
+ std::vector<double> jointAngles);
std::string currentRns;
IKCalibration calibrate();
- double getMaxDistance(Eigen::Vector3f destination, std::vector<std::vector<double>> jointAngles, VirtualRobot::RobotNodeSetPtr rns);
- static int getFurthestNode(VirtualRobot::RobotNodeSetPtr rns, Eigen::VectorXf startConfig, Eigen::VectorXf endConfig);
+ double getMaxDistance(Eigen::Vector3f destination,
+ std::vector<std::vector<double>> jointAngles,
+ VirtualRobot::RobotNodeSetPtr rns);
+ static int getFurthestNode(VirtualRobot::RobotNodeSetPtr rns,
+ Eigen::VectorXf startConfig,
+ Eigen::VectorXf endConfig);
//Saba::RrtPtr getMotionPlanning(Eigen::VectorXf start, Eigen::VectorXf end, VirtualRobot::RobotNodeSetPtr rns);
-
-
};
using KinematicSolverPtr = std::shared_ptr<KinematicSolver>;
-}
+} // namespace armarx
#endif
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Manager/DesignerTrajectoryHolder.cpp b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Manager/DesignerTrajectoryHolder.cpp
index 91c26af69694fac9a16b6b2486db43443384fa8b..ac1803ea965c01b72d33a3b109e7d3a549a61b7c 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Manager/DesignerTrajectoryHolder.cpp
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Manager/DesignerTrajectoryHolder.cpp
@@ -33,8 +33,8 @@ using namespace armarx;
////////////////////////////////////////////////////////////////////////////////////////////////////
///
-DesignerTrajectoryHolder::DesignerTrajectoryHolder(EnvironmentPtr environment)
- : environment(environment)
+DesignerTrajectoryHolder::DesignerTrajectoryHolder(EnvironmentPtr environment) :
+ environment(environment)
{
}
@@ -42,18 +42,17 @@ DesignerTrajectoryHolder::DesignerTrajectoryHolder(EnvironmentPtr environment)
/// Public functions
////////////////////////////////////////////////////////////////////////////////////////////////////
///
-bool DesignerTrajectoryHolder::createDesignerTrajectoryManager(std::string rnsName)
+bool
+DesignerTrajectoryHolder::createDesignerTrajectoryManager(std::string rnsName)
{
// check, if the RobotNodeSet-name already exists as key
//if (designerTrajectories.find(rnsName) == designerTrajectories.end())
if (!rnsExists(rnsName) && !rnsIsPartOfExistingRns(rnsName))
{
// RobotNodeSet-name does not exist as key
- designerTrajectories.insert(
- std::make_pair(
- rnsName,
- DesignerTrajectoryManagerPtr(
- new DesignerTrajectoryManager(rnsName, environment))));
+ designerTrajectories.insert(std::make_pair(
+ rnsName,
+ DesignerTrajectoryManagerPtr(new DesignerTrajectoryManager(rnsName, environment))));
// environment->getRobot()->getRobotNodeSet(rnsName)))));
return true;
}
@@ -63,7 +62,8 @@ bool DesignerTrajectoryHolder::createDesignerTrajectoryManager(std::string rnsNa
}
}
-bool DesignerTrajectoryHolder::deleteDesignerTrajectoryManager(std::string rnsName)
+bool
+DesignerTrajectoryHolder::deleteDesignerTrajectoryManager(std::string rnsName)
{
// check, if the RobotNodeSet-name already exists as key
//if (designerTrajectories.find(rnsName) != designerTrajectories.end())
@@ -87,7 +87,8 @@ bool DesignerTrajectoryHolder::deleteDesignerTrajectoryManager(std::string rnsNa
}
}
-DesignerTrajectoryManagerPtr DesignerTrajectoryHolder::getTrajectoryManager(std::string rnsName)
+DesignerTrajectoryManagerPtr
+DesignerTrajectoryHolder::getTrajectoryManager(std::string rnsName)
{
if (rnsExists(rnsName))
{
@@ -99,12 +100,13 @@ DesignerTrajectoryManagerPtr DesignerTrajectoryHolder::getTrajectoryManager(std:
}
}
-bool DesignerTrajectoryHolder::import(DesignerTrajectoryPtr designerTrajectory, bool force = true)
+bool
+DesignerTrajectoryHolder::import(DesignerTrajectoryPtr designerTrajectory, bool force = true)
{
VirtualRobot::RobotNodeSetPtr tmpRns = designerTrajectory->getRns();
- DesignerTrajectoryManagerPtr tmpDTM(new DesignerTrajectoryManager(tmpRns->getName(),
- environment));
+ DesignerTrajectoryManagerPtr tmpDTM(
+ new DesignerTrajectoryManager(tmpRns->getName(), environment));
tmpDTM.get()->import(designerTrajectory);
// check, if the the RobotNodeSet-name already exists as key
@@ -126,7 +128,8 @@ bool DesignerTrajectoryHolder::import(DesignerTrajectoryPtr designerTrajectory,
}
}
-bool DesignerTrajectoryHolder::rnsExists(std::string rnsName)
+bool
+DesignerTrajectoryHolder::rnsExists(std::string rnsName)
{
// check, if the RobotNodeSet-name already exists as key
if (designerTrajectories.find(rnsName) != designerTrajectories.end())
@@ -141,26 +144,26 @@ bool DesignerTrajectoryHolder::rnsExists(std::string rnsName)
}
}
-bool DesignerTrajectoryHolder::rnsIsPartOfExistingRns(std::string rnsName)
+bool
+DesignerTrajectoryHolder::rnsIsPartOfExistingRns(std::string rnsName)
{
// save all existing RobotNodeSets as vector<vector<string>>
// one RobotNodeSet is representet as vector of its RobotNodes-name
std::vector<std::vector<std::string>> rnssNodeNames;
- for (std::map<std::string, DesignerTrajectoryManagerPtr>::iterator it
- = designerTrajectories.begin();
+ for (std::map<std::string, DesignerTrajectoryManagerPtr>::iterator it =
+ designerTrajectories.begin();
it != designerTrajectories.end();
++it)
{
VirtualRobot::RobotNodeSetPtr rnsptr = environment->getRobot()->getRobotNodeSet(it->first);
std::vector<std::string> tmpRnsNodesNames = rnsptr->getNodeNames();
rnssNodeNames.push_back(tmpRnsNodesNames);
-
}
// get nodes of given rns
- std::vector<std::string> givenRnsNodeNames
- = environment->getRobot()->getRobotNodeSet(rnsName)->getNodeNames();
+ std::vector<std::string> givenRnsNodeNames =
+ environment->getRobot()->getRobotNodeSet(rnsName)->getNodeNames();
// check if givenRnsNodeNames is a subset of any rnsNodesNames
for (std::vector<std::string> rnsNodeNames : rnssNodeNames)
@@ -170,8 +173,10 @@ bool DesignerTrajectoryHolder::rnsIsPartOfExistingRns(std::string rnsName)
sort(rnsNodeNames.begin(), rnsNodeNames.end());
sort(givenRnsNodeNames.begin(), givenRnsNodeNames.end());
- std::set_intersection(rnsNodeNames.begin(), rnsNodeNames.end(),
- givenRnsNodeNames.begin(), givenRnsNodeNames.end(),
+ std::set_intersection(rnsNodeNames.begin(),
+ rnsNodeNames.end(),
+ givenRnsNodeNames.begin(),
+ givenRnsNodeNames.end(),
std::back_inserter(intersection));
if (intersection.size() != 0)
@@ -183,9 +188,10 @@ bool DesignerTrajectoryHolder::rnsIsPartOfExistingRns(std::string rnsName)
return false;
}
-bool DesignerTrajectoryHolder::isInCollision(std::string activeColModelName,
- std::vector<std::string> bodyColModelsNames,
- unsigned int fps)
+bool
+DesignerTrajectoryHolder::isInCollision(std::string activeColModelName,
+ std::vector<std::string> bodyColModelsNames,
+ unsigned int fps)
{
TrajectoryPtr t = mergeTrajectories(fps);
VirtualRobot::RobotPtr cdRob = environment->getCDRobot();
@@ -202,8 +208,7 @@ bool DesignerTrajectoryHolder::isInCollision(std::string activeColModelName,
}
VirtualRobot::SceneObjectSetPtr sosActiveColModel(new VirtualRobot::SceneObjectSet());
- sosActiveColModel->addSceneObjects(
- cdRob->getRobotNodeSet(activeColModelName));
+ sosActiveColModel->addSceneObjects(cdRob->getRobotNodeSet(activeColModelName));
// add sosActiveColModel to CDManager
cdm->addCollisionModel(sosActiveColModel);
@@ -218,8 +223,7 @@ bool DesignerTrajectoryHolder::isInCollision(std::string activeColModelName,
}
VirtualRobot::SceneObjectSetPtr sosBodyColModel(new VirtualRobot::SceneObjectSet());
- sosBodyColModel->addSceneObjects(
- cdRob->getRobotNodeSet(bodyColModelName));
+ sosBodyColModel->addSceneObjects(cdRob->getRobotNodeSet(bodyColModelName));
// add sosBodyColModel to CDManager
cdm->addCollisionModel(sosBodyColModel);
@@ -241,10 +245,7 @@ bool DesignerTrajectoryHolder::isInCollision(std::string activeColModelName,
std::vector<float> jointValues(jointValuesDouble.begin(), jointValuesDouble.end());
VirtualRobot::RobotConfigPtr robConf(new VirtualRobot::RobotConfig(
- cdRob,
- "config at t=" + std::to_string(time),
- dimNames,
- jointValues));
+ cdRob, "config at t=" + std::to_string(time), dimNames, jointValues));
////////////////////////////////////////////////////////////////////////////////////////////
// Set RobotConfig to CDRobot
@@ -265,7 +266,8 @@ bool DesignerTrajectoryHolder::isInCollision(std::string activeColModelName,
return false;
}
-TrajectoryPtr DesignerTrajectoryHolder::mergeTrajectories(unsigned int fps)
+TrajectoryPtr
+DesignerTrajectoryHolder::mergeTrajectories(unsigned int fps)
{
std::vector<TrajectoryPtr> trajectories;
std::vector<double> endTimes;
@@ -273,8 +275,8 @@ TrajectoryPtr DesignerTrajectoryHolder::mergeTrajectories(unsigned int fps)
std::vector<double> timestamps;
TrajectoryPtr finalTrajectory(new Trajectory());
- for (std::map<std::string, DesignerTrajectoryManagerPtr>::iterator it
- = designerTrajectories.begin();
+ for (std::map<std::string, DesignerTrajectoryManagerPtr>::iterator it =
+ designerTrajectories.begin();
it != designerTrajectories.end();
++it)
{
@@ -319,7 +321,8 @@ TrajectoryPtr DesignerTrajectoryHolder::mergeTrajectories(unsigned int fps)
}
}
- finalTrajectory->addDimension(dimData, timestamps, trajectories[t]->getDimensionName(dim));
+ finalTrajectory->addDimension(
+ dimData, timestamps, trajectories[t]->getDimensionName(dim));
}
}
return finalTrajectory;
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Manager/DesignerTrajectoryHolder.h b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Manager/DesignerTrajectoryHolder.h
index 590d9eca183a29fedf9b9236641603c3845df6ea..2d5a2412cf82e0091f477abb127b6583d1613309 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Manager/DesignerTrajectoryHolder.h
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Manager/DesignerTrajectoryHolder.h
@@ -29,9 +29,8 @@
#include <VirtualRobot/RobotNodeSet.h>
-#include "DesignerTrajectoryManager.h"
#include "../Model/DesignerTrajectory.h"
-
+#include "DesignerTrajectoryManager.h"
//using DesignerTrajectoryPtr = std::shared_ptr<armarx::DesignerTrajectory>;
@@ -58,12 +57,12 @@ namespace armarx
}
};*/
- std::map < std::string, DesignerTrajectoryManagerPtr>
- designerTrajectories;
+ std::map<std::string, DesignerTrajectoryManagerPtr> designerTrajectories;
EnvironmentPtr environment;
TrajectoryPtr mergeTrajectories(unsigned int fps);
+
public:
////////////////////////////////////////////////////////////////////////////////////////////
/// Constructor(s)
@@ -149,13 +148,12 @@ namespace armarx
bool isInCollision(std::string activeColModelName,
std::vector<std::string> bodyColModelsNames,
unsigned int fps);
-
};
////////////////////////////////////////////////////////////////////////////////////////////////
/// Typedefinitions
////////////////////////////////////////////////////////////////////////////////////////////////
using DesignerTrajectoryHolderPtr = std::shared_ptr<armarx::DesignerTrajectoryHolder>;
-}
+} // namespace armarx
#endif // DESIGNERTRAJECTORYHOLDER_H
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Manager/DesignerTrajectoryManager.cpp b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Manager/DesignerTrajectoryManager.cpp
index 33eac934d572aa8fe3aba6af1bb10ad494d4f2d7..9dfc12272c57df54557c8898db3e3016eba437d5 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Manager/DesignerTrajectoryManager.cpp
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Manager/DesignerTrajectoryManager.cpp
@@ -30,57 +30,55 @@ using namespace armarx;
/// Struct-functions
////////////////////////////////////////////////////////////////////////////////////////////
-void DesignerTrajectoryManager::ManipulationInterval::addFirstUserWaypoint(UserWaypointPtr uw)
+void
+DesignerTrajectoryManager::ManipulationInterval::addFirstUserWaypoint(UserWaypointPtr uw)
{
userWaypointsList.push_back(uw);
}
-void DesignerTrajectoryManager::ManipulationInterval::pushFront(UserWaypointPtr uw, TransitionPtr t)
+void
+DesignerTrajectoryManager::ManipulationInterval::pushFront(UserWaypointPtr uw, TransitionPtr t)
{
userWaypointsList.push_front(uw);
transitionsList.push_front(t);
lowerIntervalLimit--;
}
-void DesignerTrajectoryManager::ManipulationInterval::pushBack(UserWaypointPtr uw, TransitionPtr t)
+void
+DesignerTrajectoryManager::ManipulationInterval::pushBack(UserWaypointPtr uw, TransitionPtr t)
{
userWaypointsList.push_back(uw);
transitionsList.push_back(t);
upperIntervalLimit++;
}
-void DesignerTrajectoryManager::ManipulationInterval::addBreakpointIndex(int index)
+void
+DesignerTrajectoryManager::ManipulationInterval::addBreakpointIndex(int index)
{
breakpointIndicesSet.insert(index);
}
-void DesignerTrajectoryManager::ManipulationInterval::finishSearch()
+void
+DesignerTrajectoryManager::ManipulationInterval::finishSearch()
{
// copy userWaypointsList to userWaypoints vector
userWaypoints.reserve(userWaypointsList.size());
std::copy(
- userWaypointsList.begin(),
- userWaypointsList.end(),
- std::back_inserter(userWaypoints));
+ userWaypointsList.begin(), userWaypointsList.end(), std::back_inserter(userWaypoints));
// copy transitionsList to transitions vector
transitions.reserve(transitionsList.size());
- std::copy(
- transitionsList.begin(),
- transitionsList.end(),
- std::back_inserter(transitions));
+ std::copy(transitionsList.begin(), transitionsList.end(), std::back_inserter(transitions));
// copy breakpointInicesSet to breakpointIndices vector
breakpointIndices.reserve(breakpointIndicesSet.size());
- std::copy(
- breakpointIndicesSet.begin(),
- breakpointIndicesSet.end(),
- std::back_inserter(breakpointIndices)
- );
+ std::copy(breakpointIndicesSet.begin(),
+ breakpointIndicesSet.end(),
+ std::back_inserter(breakpointIndices));
}
-UserWaypointPtr DesignerTrajectoryManager::ManipulationInterval::getUserWaypointByRealIndex(
- unsigned int i) const
+UserWaypointPtr
+DesignerTrajectoryManager::ManipulationInterval::getUserWaypointByRealIndex(unsigned int i) const
{
if (i - lowerIntervalLimit < userWaypoints.size())
{
@@ -91,10 +89,10 @@ UserWaypointPtr DesignerTrajectoryManager::ManipulationInterval::getUserWaypoint
// modify Exception?
throw OutOfRangeException();
}
-
}
-UserWaypointPtr DesignerTrajectoryManager::ManipulationInterval::getUserWaypointByZeroBasedIndex(
+UserWaypointPtr
+DesignerTrajectoryManager::ManipulationInterval::getUserWaypointByZeroBasedIndex(
unsigned int i) const
{
if (i < userWaypoints.size())
@@ -108,8 +106,8 @@ UserWaypointPtr DesignerTrajectoryManager::ManipulationInterval::getUserWaypoint
}
}
-TransitionPtr DesignerTrajectoryManager::ManipulationInterval::getTransitionByRealIndex(
- unsigned int i) const
+TransitionPtr
+DesignerTrajectoryManager::ManipulationInterval::getTransitionByRealIndex(unsigned int i) const
{
if (i - lowerIntervalLimit < transitions.size())
{
@@ -122,8 +120,8 @@ TransitionPtr DesignerTrajectoryManager::ManipulationInterval::getTransitionByRe
}
}
-TransitionPtr DesignerTrajectoryManager::ManipulationInterval::getTransitionByZeroBasedIndex(
- unsigned int i) const
+TransitionPtr
+DesignerTrajectoryManager::ManipulationInterval::getTransitionByZeroBasedIndex(unsigned int i) const
{
if (i < transitions.size())
{
@@ -136,11 +134,11 @@ TransitionPtr DesignerTrajectoryManager::ManipulationInterval::getTransitionByZe
}
}
-std::vector<std::vector<double>> DesignerTrajectoryManager::ManipulationInterval::
- getUserWaypointsIKSolutions(
- std::vector<std::vector<double>>& ikSolutions,
- unsigned int intervalStart,
- unsigned int intervalEnd)
+std::vector<std::vector<double>>
+DesignerTrajectoryManager::ManipulationInterval::getUserWaypointsIKSolutions(
+ std::vector<std::vector<double>>& ikSolutions,
+ unsigned int intervalStart,
+ unsigned int intervalEnd)
{
std::vector<std::vector<double>> result;
for (unsigned int& newIndex : newIndexOfUserWaypoint)
@@ -159,13 +157,12 @@ std::vector<std::vector<double>> DesignerTrajectoryManager::ManipulationInterval
return result;
}
-void DesignerTrajectoryManager::ManipulationInterval::applyJointAnglesOfUserWaypoints(
+void
+DesignerTrajectoryManager::ManipulationInterval::applyJointAnglesOfUserWaypoints(
std::vector<std::vector<double>> ikSolution)
{
std::vector<std::vector<double>> jointAngles = getUserWaypointsIKSolutions(
- ikSolution,
- newIndexOfUserWaypoint[0],
- newIndexOfUserWaypoint[userWaypoints.size() - 1]);
+ ikSolution, newIndexOfUserWaypoint[0], newIndexOfUserWaypoint[userWaypoints.size() - 1]);
int count = 0;
for (UserWaypointPtr w : userWaypoints)
@@ -178,7 +175,8 @@ void DesignerTrajectoryManager::ManipulationInterval::applyJointAnglesOfUserWayp
/// Auxiliary functions
////////////////////////////////////////////////////////////////////////////////////////////////////
-void DesignerTrajectoryManager::saveState()
+void
+DesignerTrajectoryManager::saveState()
{
//cut off the last mementos behind the current memento
if (mementos.currentMemento < mementos.mementoList.size() - 1)
@@ -188,8 +186,8 @@ void DesignerTrajectoryManager::saveState()
if (designerTrajectory)
{
- mementos.mementoList.push_back(std::shared_ptr<DesignerTrajectory>(
- new DesignerTrajectory(*designerTrajectory)));
+ mementos.mementoList.push_back(
+ std::shared_ptr<DesignerTrajectory>(new DesignerTrajectory(*designerTrajectory)));
}
else
{
@@ -277,8 +275,8 @@ DesignerTrajectoryManager::calculateManipulationInterval(unsigned int manipulati
return mi;
}
-std::vector<AbstractInterpolationPtr> DesignerTrajectoryManager::getInterpolationObjects(
- ManipulationInterval& mi)
+std::vector<AbstractInterpolationPtr>
+DesignerTrajectoryManager::getInterpolationObjects(ManipulationInterval& mi)
{
std::vector<PoseBasePtr> poses;
for (UserWaypointPtr& uwp : mi.userWaypoints)
@@ -295,7 +293,8 @@ std::vector<AbstractInterpolationPtr> DesignerTrajectoryManager::getInterpolatio
return InterpolationSegmentFactory::produceInterpolationSegments(poses, interpolationTypes);
}
-void DesignerTrajectoryManager::calculateInterpolatedPoints(
+void
+DesignerTrajectoryManager::calculateInterpolatedPoints(
std::vector<AbstractInterpolationPtr> interpolationObjects,
unsigned int fps,
ManipulationInterval& mi)
@@ -359,8 +358,9 @@ void DesignerTrajectoryManager::calculateInterpolatedPoints(
}
}
-bool DesignerTrajectoryManager::checkTransitionReachability(
- ManipulationInterval& mi, unsigned int transitionIndex)
+bool
+DesignerTrajectoryManager::checkTransitionReachability(ManipulationInterval& mi,
+ unsigned int transitionIndex)
{
for (PoseBasePtr pose : mi.interpolatedTransitions[transitionIndex])
{
@@ -377,8 +377,8 @@ bool DesignerTrajectoryManager::checkTransitionReachability(
return true;
}
-std::vector<std::vector<double>> DesignerTrajectoryManager::calculateIKSolutions(
- ManipulationInterval& mi)
+std::vector<std::vector<double>>
+DesignerTrajectoryManager::calculateIKSolutions(ManipulationInterval& mi)
{
// IKSolutions
std::vector<std::vector<double>> ikSolutions;
@@ -390,8 +390,7 @@ std::vector<std::vector<double>> DesignerTrajectoryManager::calculateIKSolutions
// motion planning insertion indices
std::vector<InsertTransition> mpInserts;
- for (unsigned int transitionIndex = 0;
- transitionIndex < mi.interpolatedTransitions.size();
+ for (unsigned int transitionIndex = 0; transitionIndex < mi.interpolatedTransitions.size();
transitionIndex++)
{
if (!checkTransitionReachability(mi, transitionIndex))
@@ -403,20 +402,16 @@ std::vector<std::vector<double>> DesignerTrajectoryManager::calculateIKSolutions
mpInserts.push_back(mpInsert);
//calculate IKSolution (jump to end point of this transition)
- std::vector<double> start
- = {ikSolutions.back()};
+ std::vector<double> start = {ikSolutions.back()};
- std::vector<PoseBasePtr> destinations
- = {mi.interpolatedTransitions[transitionIndex].back()};
+ std::vector<PoseBasePtr> destinations = {
+ mi.interpolatedTransitions[transitionIndex].back()};
- std::vector<VirtualRobot::IKSolver::CartesianSelection> selections
- = {mi.userWaypoints[transitionIndex + 1]->getIKSelection()};
+ std::vector<VirtualRobot::IKSolver::CartesianSelection> selections = {
+ mi.userWaypoints[transitionIndex + 1]->getIKSelection()};
- std::vector<std::vector<double>> tmpIkSolutions = kinSolver->solveRecursiveIKRelative(
- rns,
- start,
- destinations,
- selections);
+ std::vector<std::vector<double>> tmpIkSolutions =
+ kinSolver->solveRecursiveIKRelative(rns, start, destinations, selections);
// insert tmpIkSolutions into ikSolutions
ikSolutions.insert(ikSolutions.end(), tmpIkSolutions.begin(), tmpIkSolutions.end());
@@ -432,13 +427,12 @@ std::vector<std::vector<double>> DesignerTrajectoryManager::calculateIKSolutions
// this transition is reachable
// calculate IKSolution of this transition
// get start
- std::vector<double> start
- = {ikSolutions.back()};
+ std::vector<double> start = {ikSolutions.back()};
// get destinations: all poses from interpolatedTransition except for the first
- std::vector<PoseBasePtr> destinations(
- mi.interpolatedTransitions[transitionIndex].begin() + 1,
- mi.interpolatedTransitions[transitionIndex].end());
+ std::vector<PoseBasePtr> destinations(mi.interpolatedTransitions[transitionIndex].begin() +
+ 1,
+ mi.interpolatedTransitions[transitionIndex].end());
// get selection
std::vector<VirtualRobot::IKSolver::CartesianSelection> selections;
@@ -453,11 +447,8 @@ std::vector<std::vector<double>> DesignerTrajectoryManager::calculateIKSolutions
selections.push_back(mi.userWaypoints[transitionIndex + 1]->getIKSelection());
// call ikSolver
- std::vector<std::vector<double>> tmpIkSolutions = kinSolver->solveRecursiveIKRelative(
- rns,
- start,
- destinations,
- selections);
+ std::vector<std::vector<double>> tmpIkSolutions =
+ kinSolver->solveRecursiveIKRelative(rns, start, destinations, selections);
// END: calculate IKSolution of this transition
// check for tmpIkSolutions for collision
@@ -471,7 +462,8 @@ std::vector<std::vector<double>> DesignerTrajectoryManager::calculateIKSolutions
return ikSolutions;
}
-std::vector<TimedTrajectory> DesignerTrajectoryManager::calculateTimeOptimalTrajectories(
+std::vector<TimedTrajectory>
+DesignerTrajectoryManager::calculateTimeOptimalTrajectories(
std::vector<std::vector<double>> ikSolutions,
ManipulationInterval& mi)
{
@@ -483,10 +475,10 @@ std::vector<TimedTrajectory> DesignerTrajectoryManager::calculateTimeOptimalTraj
* mi.breakpointIndices are the indices before the interpolated points were inserted.
*/
int intervalStart = mi.breakpointIndices[i];
- int intervalEnd = mi.breakpointIndices[i + 1];
+ int intervalEnd = mi.breakpointIndices[i + 1];
std::vector<std::vector<double>> ikSolutionsInterval(
- ikSolutions.begin() + mi.newIndexOfUserWaypoint[intervalStart],
- ikSolutions.begin() + 1 + mi.newIndexOfUserWaypoint[intervalEnd]);
+ ikSolutions.begin() + mi.newIndexOfUserWaypoint[intervalStart],
+ ikSolutions.begin() + 1 + mi.newIndexOfUserWaypoint[intervalEnd]);
/*
* Because intervalStart/intervalEnd are the breakpoint indices BEFORE interpolated
@@ -494,31 +486,28 @@ std::vector<TimedTrajectory> DesignerTrajectoryManager::calculateTimeOptimalTraj
* their new index (retrieved with mi.newIndexOfUserWaypoint[...]) is needed as parameter
* for the function mi.getUserWaypointsIKSolutions(..., ..., ...)
*/
- std::vector<std::vector<double>> userWaypointsIKSolution
- = mi.getUserWaypointsIKSolutions(
- ikSolutions,
- mi.newIndexOfUserWaypoint[intervalStart],
- mi.newIndexOfUserWaypoint[intervalEnd]);
-
- result.push_back(
- DesignerTrajectoryCalculator(environment).calculateTimeOptimalTrajectory(
- ikSolutionsInterval,
- userWaypointsIKSolution,
- rns,
- MAX_DEVIATION));
+ std::vector<std::vector<double>> userWaypointsIKSolution =
+ mi.getUserWaypointsIKSolutions(ikSolutions,
+ mi.newIndexOfUserWaypoint[intervalStart],
+ mi.newIndexOfUserWaypoint[intervalEnd]);
+
+ result.push_back(DesignerTrajectoryCalculator(environment)
+ .calculateTimeOptimalTrajectory(
+ ikSolutionsInterval, userWaypointsIKSolution, rns, MAX_DEVIATION));
}
return result;
}
-bool DesignerTrajectoryManager::undo()
+bool
+DesignerTrajectoryManager::undo()
{
if (mementos.currentMemento != 0)
{
if (*(std::next(mementos.mementoList.begin(), mementos.currentMemento - 1)))
{
- designerTrajectory = DesignerTrajectoryPtr(new DesignerTrajectory(*(*(std::next(mementos.mementoList.begin(),
- mementos.currentMemento - 1)))));
+ designerTrajectory = DesignerTrajectoryPtr(new DesignerTrajectory(
+ *(*(std::next(mementos.mementoList.begin(), mementos.currentMemento - 1)))));
isInitialized = true;
}
else
@@ -534,14 +523,15 @@ bool DesignerTrajectoryManager::undo()
}
}
-bool DesignerTrajectoryManager::redo()
+bool
+DesignerTrajectoryManager::redo()
{
if (mementos.currentMemento < mementos.mementoList.size() - 1)
{
if (*std::next(mementos.mementoList.begin(), mementos.currentMemento + 1))
{
- designerTrajectory = DesignerTrajectoryPtr(new DesignerTrajectory(*(*(std::next(mementos.mementoList.begin(),
- mementos.currentMemento + 1)))));
+ designerTrajectory = DesignerTrajectoryPtr(new DesignerTrajectory(
+ *(*(std::next(mementos.mementoList.begin(), mementos.currentMemento + 1)))));
}
else
{
@@ -557,12 +547,14 @@ bool DesignerTrajectoryManager::redo()
}
}
-bool DesignerTrajectoryManager::getIsInitialized()
+bool
+DesignerTrajectoryManager::getIsInitialized()
{
return isInitialized;
}
-void DesignerTrajectoryManager::theUniversalMethod(unsigned int index)
+void
+DesignerTrajectoryManager::theUniversalMethod(unsigned int index)
{
if (designerTrajectory->getNrOfUserWaypoints() > 1)
{
@@ -580,7 +572,8 @@ void DesignerTrajectoryManager::theUniversalMethod(unsigned int index)
for (UserWaypointPtr uwp : mi.userWaypoints)
{
poses.push_back(uwp->getPose());
- posesBackup.push_back(PoseBasePtr(new Pose(Pose(uwp->getPose()->position, uwp->getPose()->orientation).toEigen())));
+ posesBackup.push_back(PoseBasePtr(
+ new Pose(Pose(uwp->getPose()->position, uwp->getPose()->orientation).toEigen())));
selections.push_back(uwp->getIKSelection());
selectionsBackup.push_back(uwp->getIKSelection());
}
@@ -604,9 +597,7 @@ void DesignerTrajectoryManager::theUniversalMethod(unsigned int index)
}
calculateInterpolatedPoints(
- InterpolationSegmentFactory::produceInterpolationSegments(
- poses,
- interpolations),
+ InterpolationSegmentFactory::produceInterpolationSegments(poses, interpolations),
DEFAULT_FRAME_COUNT,
mi);
//calculateInterpolatedPoints(getInterpolationObjects(mi), DEFAULT_FRAME_COUNT, mi);
@@ -627,8 +618,8 @@ void DesignerTrajectoryManager::theUniversalMethod(unsigned int index)
mi.applyJointAnglesOfUserWaypoints(ikSolutions);
//set interBreakpointsTrajectories////////////////////////////////////////////////////
- std::vector<TimedTrajectory> timedTrajectories = calculateTimeOptimalTrajectories(
- ikSolutions, mi);
+ std::vector<TimedTrajectory> timedTrajectories =
+ calculateTimeOptimalTrajectories(ikSolutions, mi);
//search how many interBreakPointTrajectories are behind and before the new
//interBreakpointTrajectories
int countBefore = 0;
@@ -648,8 +639,9 @@ void DesignerTrajectoryManager::theUniversalMethod(unsigned int index)
if (mi.upperIntervalLimit != designerTrajectory->getNrOfUserWaypoints() - 1)
{
countAfter++;
- for (unsigned int i = mi.upperIntervalLimit + 1; i < designerTrajectory->
- getNrOfUserWaypoints() - 1; i++)
+ for (unsigned int i = mi.upperIntervalLimit + 1;
+ i < designerTrajectory->getNrOfUserWaypoints() - 1;
+ i++)
{
countAfter++;
}
@@ -660,8 +652,7 @@ void DesignerTrajectoryManager::theUniversalMethod(unsigned int index)
std::vector<TrajectoryPtr> newInterBreakPointTrajectories;
newInterBreakPointTrajectories.insert(newInterBreakPointTrajectories.end(),
InterBreakPointTrajectories.begin(),
- InterBreakPointTrajectories.begin() +
- countBefore);
+ InterBreakPointTrajectories.begin() + countBefore);
// set new inter breakpoint trajectories
for (TimedTrajectory& t : timedTrajectories)
@@ -678,14 +669,14 @@ void DesignerTrajectoryManager::theUniversalMethod(unsigned int index)
//set newTimeOptimalDuration of each Transition///////////////////////////////////////
//and timeOptimalTimestamp of each UserWaypoint///////////////////////////////////////
//and Trajectories of all changed Transitions ////////////////////////////////////////
- TrajectoryPtr traj = designerTrajectory ->getTimeOptimalTrajectory();
+ TrajectoryPtr traj = designerTrajectory->getTimeOptimalTrajectory();
std::vector<double> timestamps = traj->getTimestamps();
unsigned int trajCount = 0;
//search for the timestamp of first changed point of traj
while (trajCount < timestamps.size() &&
- designerTrajectory->getUserWaypoint(mi.breakpointIndices[0])->getTimeOptimalTimestamp()
- > timestamps[trajCount])
+ designerTrajectory->getUserWaypoint(mi.breakpointIndices[0])
+ ->getTimeOptimalTimestamp() > timestamps[trajCount])
{
trajCount++;
}
@@ -707,9 +698,8 @@ void DesignerTrajectoryManager::theUniversalMethod(unsigned int index)
for (unsigned int i = mi.breakpointIndices[t]; i < mi.breakpointIndices[t + 1]; i++)
{
TransitionPtr trans = designerTrajectory->getTransition(i);
- trans->
- setTimeOptimalDuration(timedUserWaypoints[count + 1] -
- timedUserWaypoints[count]);
+ trans->setTimeOptimalDuration(timedUserWaypoints[count + 1] -
+ timedUserWaypoints[count]);
count++;
}
@@ -729,7 +719,8 @@ void DesignerTrajectoryManager::theUniversalMethod(unsigned int index)
}
//add all jointAngles for all timestamps at transition
- while (trajCountEnd < timestamps.size() && timestamps[trajCountEnd] < end->getTimeOptimalTimestamp())
+ while (trajCountEnd < timestamps.size() &&
+ timestamps[trajCountEnd] < end->getTimeOptimalTimestamp())
{
newTimestamps.push_back(timestamps[trajCountEnd] - timestamps[trajCount]);
for (unsigned int dim = 0; dim < traj->dim(); dim++)
@@ -740,7 +731,8 @@ void DesignerTrajectoryManager::theUniversalMethod(unsigned int index)
trajCountEnd++;
}
trajCount = trajCountEnd;
- TrajectoryPtr newTraj = TrajectoryPtr(new Trajectory(newTrajData, newTimestamps, traj->getDimensionNames()));
+ TrajectoryPtr newTraj = TrajectoryPtr(
+ new Trajectory(newTrajData, newTimestamps, traj->getDimensionNames()));
trans->setTrajectory(newTraj);
}
}
@@ -748,7 +740,8 @@ void DesignerTrajectoryManager::theUniversalMethod(unsigned int index)
//shift all timeOptimaltimestamps of the userWaypoints behind last BreakpointIndices
//it goes through transitions (getNrOfUserWaypoints - 2 = NrOfTransitions - 1)
for (unsigned int i = mi.breakpointIndices.back();
- i <= designerTrajectory->getNrOfUserWaypoints() - 2; i++)
+ i <= designerTrajectory->getNrOfUserWaypoints() - 2;
+ i++)
{
//the setter of transitions updates the timeOptimalTimestamps of end- and start-
//Userwaypoint
@@ -757,22 +750,25 @@ void DesignerTrajectoryManager::theUniversalMethod(unsigned int index)
}
-
//update UserTimestamps of all UserWaypoints
for (unsigned int i = 0; i < designerTrajectory->getNrOfUserWaypoints() - 2; i++)
{
- double time = designerTrajectory->getTransition(i)->getStart()->getUserTimestamp()
- + designerTrajectory->getTransition(i)->getUserDuration();
+ double time = designerTrajectory->getTransition(i)->getStart()->getUserTimestamp() +
+ designerTrajectory->getTransition(i)->getUserDuration();
designerTrajectory->getTransition(i)->getEnd()->setUserTimestamp(time);
}
}
saveState();
}
-std::vector<double> DesignerTrajectoryManager::getNewIkSolutionOfFirstPoint(PoseBasePtr oldStart, PoseBasePtr newStart, std::vector<double> jointAnglesOldStart)
+std::vector<double>
+DesignerTrajectoryManager::getNewIkSolutionOfFirstPoint(PoseBasePtr oldStart,
+ PoseBasePtr newStart,
+ std::vector<double> jointAnglesOldStart)
{
//Resolve recursive IK from oldStart to NewStart
- std::vector<AbstractInterpolationPtr> ip = InterpolationSegmentFactory::produceLinearInterpolationSegments({oldStart, newStart});
+ std::vector<AbstractInterpolationPtr> ip =
+ InterpolationSegmentFactory::produceLinearInterpolationSegments({oldStart, newStart});
std::vector<PoseBasePtr> poses;
std::vector<VirtualRobot::IKSolver::CartesianSelection> selections;
@@ -786,7 +782,8 @@ std::vector<double> DesignerTrajectoryManager::getNewIkSolutionOfFirstPoint(Pose
}
poses.push_back(newStart);
selections.push_back(VirtualRobot::IKSolver::CartesianSelection::All);
- std::vector<std::vector<double>> ikSolutions = kinSolver->solveRecursiveIKRelative(rns, jointAnglesOldStart, poses, selections);
+ std::vector<std::vector<double>> ikSolutions =
+ kinSolver->solveRecursiveIKRelative(rns, jointAnglesOldStart, poses, selections);
return ikSolutions.back();
}
@@ -795,12 +792,10 @@ std::vector<double> DesignerTrajectoryManager::getNewIkSolutionOfFirstPoint(Pose
/// Constructor(s)
////////////////////////////////////////////////////////////////////////////////////////////////////
-DesignerTrajectoryManager::DesignerTrajectoryManager(
- std::string rnsName, EnvironmentPtr environment)
- : kinSolver(KinematicSolver::getInstance(
- environment->getScene(),
- environment->getRobot())),
- environment(environment)
+DesignerTrajectoryManager::DesignerTrajectoryManager(std::string rnsName,
+ EnvironmentPtr environment) :
+ kinSolver(KinematicSolver::getInstance(environment->getScene(), environment->getRobot())),
+ environment(environment)
{
rns = environment->getRobot()->getRobotNodeSet(rnsName);
}
@@ -809,8 +804,8 @@ DesignerTrajectoryManager::DesignerTrajectoryManager(
/// Public Functions
////////////////////////////////////////////////////////////////////////////////////////////////////
-void DesignerTrajectoryManager::initializeDesignerTrajectory(
- std::vector<double>& jointAngles)
+void
+DesignerTrajectoryManager::initializeDesignerTrajectory(std::vector<double>& jointAngles)
{
PoseBasePtr pb = kinSolver->doForwardKinematic(rns, jointAngles);
Pose pose = Pose(pb->position, pb->orientation);
@@ -830,14 +825,15 @@ void DesignerTrajectoryManager::initializeDesignerTrajectory(
}
else
{
- mementos.mementoList.push_back(DesignerTrajectoryPtr(
- new DesignerTrajectory(*designerTrajectory)));
+ mementos.mementoList.push_back(
+ DesignerTrajectoryPtr(new DesignerTrajectory(*designerTrajectory)));
}
isInitialized = true;
}
-void DesignerTrajectoryManager::addWaypoint(const PoseBasePtr pb)
+void
+DesignerTrajectoryManager::addWaypoint(const PoseBasePtr pb)
{
if (isInitialized)
{
@@ -854,7 +850,8 @@ void DesignerTrajectoryManager::addWaypoint(const PoseBasePtr pb)
}
}
-void DesignerTrajectoryManager::insertWaypoint(unsigned int index, const PoseBasePtr pb)
+void
+DesignerTrajectoryManager::insertWaypoint(unsigned int index, const PoseBasePtr pb)
{
if (isInitialized)
{
@@ -870,7 +867,8 @@ void DesignerTrajectoryManager::insertWaypoint(unsigned int index, const PoseBas
else
{
PoseBasePtr oldStart = designerTrajectory->getUserWaypoint(0)->getPose();
- std::vector<double> jointAngles = getNewIkSolutionOfFirstPoint(oldStart, pb, designerTrajectory->getUserWaypoint(0)->getJointAngles());
+ std::vector<double> jointAngles = getNewIkSolutionOfFirstPoint(
+ oldStart, pb, designerTrajectory->getUserWaypoint(0)->getJointAngles());
if (jointAngles.size() == 0)
{
return;
@@ -888,7 +886,8 @@ void DesignerTrajectoryManager::insertWaypoint(unsigned int index, const PoseBas
}
}
-void DesignerTrajectoryManager::editWaypointPoseBase(unsigned int index, const PoseBasePtr pb)
+void
+DesignerTrajectoryManager::editWaypointPoseBase(unsigned int index, const PoseBasePtr pb)
{
if (isInitialized)
{
@@ -904,8 +903,9 @@ void DesignerTrajectoryManager::editWaypointPoseBase(unsigned int index, const P
if (designerTrajectory->getNrOfUserWaypoints() > 1)
{
newJointAngles = getNewIkSolutionOfFirstPoint(
- designerTrajectory->getUserWaypoint(1)->getPose(), pb,
- designerTrajectory->getUserWaypoint(1)->getJointAngles());
+ designerTrajectory->getUserWaypoint(1)->getPose(),
+ pb,
+ designerTrajectory->getUserWaypoint(1)->getJointAngles());
if (newJointAngles.size() == 0)
{
return;
@@ -917,9 +917,9 @@ void DesignerTrajectoryManager::editWaypointPoseBase(unsigned int index, const P
{
//newJointAngles = kinSolver->solveIK(rns, pb, designerTrajectory->getUserWaypoint(0)->getIKSelection(), 50);
newJointAngles = getNewIkSolutionOfFirstPoint(
- designerTrajectory->getUserWaypoint(0)->getPose(),
- pb,
- designerTrajectory->getUserWaypoint(0)->getJointAngles());
+ designerTrajectory->getUserWaypoint(0)->getPose(),
+ pb,
+ designerTrajectory->getUserWaypoint(0)->getJointAngles());
if (newJointAngles.size() == 0)
{
return;
@@ -941,7 +941,10 @@ void DesignerTrajectoryManager::editWaypointPoseBase(unsigned int index, const P
}
}
-void DesignerTrajectoryManager::editWaypointIKSelection(unsigned int index, VirtualRobot::IKSolver::CartesianSelection ikSelection)
+void
+DesignerTrajectoryManager::editWaypointIKSelection(
+ unsigned int index,
+ VirtualRobot::IKSolver::CartesianSelection ikSelection)
{
if (isInitialized)
{
@@ -957,7 +960,8 @@ void DesignerTrajectoryManager::editWaypointIKSelection(unsigned int index, Virt
}
}
-void DesignerTrajectoryManager::deleteWaypoint(unsigned int index)
+void
+DesignerTrajectoryManager::deleteWaypoint(unsigned int index)
{
if (isInitialized)
{
@@ -1005,7 +1009,8 @@ void DesignerTrajectoryManager::deleteWaypoint(unsigned int index)
}
}
-void DesignerTrajectoryManager::setTransitionInterpolation(unsigned int index, InterpolationType it)
+void
+DesignerTrajectoryManager::setTransitionInterpolation(unsigned int index, InterpolationType it)
{
if (isInitialized)
{
@@ -1021,7 +1026,8 @@ void DesignerTrajectoryManager::setTransitionInterpolation(unsigned int index, I
}
}
-void DesignerTrajectoryManager::setWaypointAsBreakpoint(unsigned int index, bool b)
+void
+DesignerTrajectoryManager::setWaypointAsBreakpoint(unsigned int index, bool b)
{
if (isInitialized)
{
@@ -1037,7 +1043,8 @@ void DesignerTrajectoryManager::setWaypointAsBreakpoint(unsigned int index, bool
}
}
-void DesignerTrajectoryManager::setTransitionUserDuration(unsigned int index, double duration)
+void
+DesignerTrajectoryManager::setTransitionUserDuration(unsigned int index, double duration)
{
if (isInitialized)
{
@@ -1060,14 +1067,14 @@ void DesignerTrajectoryManager::setTransitionUserDuration(unsigned int index, do
}
}
-DesignerTrajectoryPtr DesignerTrajectoryManager::getDesignerTrajectory() const
+DesignerTrajectoryPtr
+DesignerTrajectoryManager::getDesignerTrajectory() const
{
if (isInitialized)
{
//return DesignerTrajectoryPtr(new DesignerTrajectory(*designerTrajectory));
- return DesignerTrajectoryPtr(new DesignerTrajectory(*(*std::next(mementos.mementoList.begin(),
- mementos.currentMemento))));
-
+ return DesignerTrajectoryPtr(new DesignerTrajectory(
+ *(*std::next(mementos.mementoList.begin(), mementos.currentMemento))));
}
else
{
@@ -1075,9 +1082,11 @@ DesignerTrajectoryPtr DesignerTrajectoryManager::getDesignerTrajectory() const
}
}
-bool DesignerTrajectoryManager::import(DesignerTrajectoryPtr newDesignerTrajectory)
+bool
+DesignerTrajectoryManager::import(DesignerTrajectoryPtr newDesignerTrajectory)
{
- if (newDesignerTrajectory->getNrOfUserWaypoints() > 1 && newDesignerTrajectory->getRns()->getName() == rns->getName())
+ if (newDesignerTrajectory->getNrOfUserWaypoints() > 1 &&
+ newDesignerTrajectory->getRns()->getName() == rns->getName())
{
TrajectoryPtr trajectory = newDesignerTrajectory->getTimeOptimalTrajectory();
@@ -1110,7 +1119,8 @@ bool DesignerTrajectoryManager::import(DesignerTrajectoryPtr newDesignerTrajecto
}
}
- TrajectoryPtr shiftedTraj(new Trajectory(nodeData, newTimestamps, traj->getDimensionNames()));
+ TrajectoryPtr shiftedTraj(
+ new Trajectory(nodeData, newTimestamps, traj->getDimensionNames()));
trans->setTrajectory(shiftedTraj);
}
designerTrajectory = newDesignerTrajectory;
@@ -1121,8 +1131,8 @@ bool DesignerTrajectoryManager::import(DesignerTrajectoryPtr newDesignerTrajecto
}
else
{
- mementos.mementoList.push_back(DesignerTrajectoryPtr(
- new DesignerTrajectory(*designerTrajectory)));
+ mementos.mementoList.push_back(
+ DesignerTrajectoryPtr(new DesignerTrajectory(*designerTrajectory)));
}
isInitialized = true;
return true;
@@ -1130,14 +1140,17 @@ bool DesignerTrajectoryManager::import(DesignerTrajectoryPtr newDesignerTrajecto
return false;
}
-bool DesignerTrajectoryManager::undoPossible()
+bool
+DesignerTrajectoryManager::undoPossible()
{
return mementos.currentMemento > 0;
}
-bool DesignerTrajectoryManager::redoPossible()
+bool
+DesignerTrajectoryManager::redoPossible()
{
- if (mementos.mementoList.size() != 0 && mementos.currentMemento < mementos.mementoList.size() - 1)
+ if (mementos.mementoList.size() != 0 &&
+ mementos.currentMemento < mementos.mementoList.size() - 1)
{
return true;
}
@@ -1146,4 +1159,3 @@ bool DesignerTrajectoryManager::redoPossible()
return false;
}
}
-
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Manager/DesignerTrajectoryManager.h b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Manager/DesignerTrajectoryManager.h
index ccf564a1c9c60ceb7228b0009e193cb044148863..273c15f46a55def6d957fa32f5ab25d2a0ad6c92 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Manager/DesignerTrajectoryManager.h
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Manager/DesignerTrajectoryManager.h
@@ -29,18 +29,21 @@
#include <set>
#include <Eigen/Core>
+
+#include <ArmarXCore/core/util/PropagateConst.h>
+
#include <RobotAPI/interface/core/PoseBase.h>
+
#include <RobotComponents/interface/components/RobotIK.h>
-#include <ArmarXCore/core/util/PropagateConst.h>
-#include <MotionPlanning/Planner/BiRrt.h>
-#include <MotionPlanning/CSpace/CSpaceSampled.h>
-#include "../Interpolation/InterpolationType.h"
-#include "../Model/DesignerTrajectory.h"
-#include "../KinematicSolver.h"
#include "../Environment.h"
#include "../Interpolation/InterpolationSegmentFactory.h"
+#include "../Interpolation/InterpolationType.h"
+#include "../KinematicSolver.h"
+#include "../Model/DesignerTrajectory.h"
#include "../TrajectoryCalculation/DesignerTrajectoryCalculator.h"
+#include <MotionPlanning/CSpace/CSpaceSampled.h>
+#include <MotionPlanning/Planner/BiRrt.h>
namespace armarx
{
@@ -109,10 +112,10 @@ namespace armarx
TransitionPtr getTransitionByRealIndex(unsigned int i) const;
TransitionPtr getTransitionByZeroBasedIndex(unsigned int i) const;
- std::vector<std::vector<double>> getUserWaypointsIKSolutions(
- std::vector<std::vector<double>>& ikSolutions,
- unsigned int intervalStart,
- unsigned int intervalEnd);
+ std::vector<std::vector<double>>
+ getUserWaypointsIKSolutions(std::vector<std::vector<double>>& ikSolutions,
+ unsigned int intervalStart,
+ unsigned int intervalEnd);
void applyJointAnglesOfUserWaypoints(std::vector<std::vector<double>> ikSolution);
};
@@ -126,7 +129,7 @@ namespace armarx
unsigned int insertionIndex;
};
- struct Mementos
+ struct Mementos
{
std::list<DesignerTrajectoryPtr> mementoList;
unsigned int currentMemento = 0;
@@ -190,8 +193,7 @@ namespace armarx
* @param manipulationIndex index where the manipulation takes place
* @return interval which is affected by the manipulation
*/
- ManipulationInterval calculateManipulationInterval(
- unsigned int manipulationIndex);
+ ManipulationInterval calculateManipulationInterval(unsigned int manipulationIndex);
/**
* @brief getInterpolationObjects calculates the interpolation object for each transition
@@ -201,8 +203,7 @@ namespace armarx
*
* @see InterpolationSegmentFactory
*/
- std::vector<AbstractInterpolationPtr> getInterpolationObjects(
- ManipulationInterval& mi);
+ std::vector<AbstractInterpolationPtr> getInterpolationObjects(ManipulationInterval& mi);
/**
* @brief Calculates the interpolated points between two userwaypoints according to the
@@ -211,10 +212,9 @@ namespace armarx
* @param fps frames per second
* @param ManipulationInterval object to insert the interpolated points into
*/
- void calculateInterpolatedPoints(
- std::vector<AbstractInterpolationPtr> interpolationObjects,
- unsigned int fps,
- ManipulationInterval& mi);
+ void calculateInterpolatedPoints(std::vector<AbstractInterpolationPtr> interpolationObjects,
+ unsigned int fps,
+ ManipulationInterval& mi);
/**
* @brief Checks if the transition with the given index is reachable
@@ -258,13 +258,16 @@ namespace armarx
* @param mi ManipulationInterval
* @return returns vector of TimedTrajectories
*/
- std::vector<TimedTrajectory> calculateTimeOptimalTrajectories(
- std::vector<std::vector<double>> ikSolutions,
- ManipulationInterval& mi);
+ std::vector<TimedTrajectory>
+ calculateTimeOptimalTrajectories(std::vector<std::vector<double>> ikSolutions,
+ ManipulationInterval& mi);
void theUniversalMethod(unsigned int index);
- std::vector<double> getNewIkSolutionOfFirstPoint(PoseBasePtr oldStart, PoseBasePtr newStart, std::vector<double> jointAnglesOldStart);
+ std::vector<double> getNewIkSolutionOfFirstPoint(PoseBasePtr oldStart,
+ PoseBasePtr newStart,
+ std::vector<double> jointAnglesOldStart);
+
public:
////////////////////////////////////////////////////////////////////////////////////////////
/// Constructor(s)
@@ -315,7 +318,8 @@ namespace armarx
* @param index index of the UserWaypoint to edit
* @param ikSelection new IKSelection
*/
- void editWaypointIKSelection(unsigned int index, VirtualRobot::IKSolver::CartesianSelection ikSelection);
+ void editWaypointIKSelection(unsigned int index,
+ VirtualRobot::IKSolver::CartesianSelection ikSelection);
/**
* @brief Deletes a UserWaypoint from this manager's DesignerTrajectory.
@@ -390,7 +394,6 @@ namespace armarx
bool undoPossible();
bool redoPossible();
-
};
////////////////////////////////////////////////////////////////////////////////////////////////
@@ -400,6 +403,6 @@ namespace armarx
// old typedef (can be deleted, if it is not used anymore)
using DesignerTrajectoryManagerStdPtr = std::shared_ptr<armarx::DesignerTrajectoryManager>;
-}
+} // namespace armarx
#endif // DESIGNERTRAJECTORYMANAGER_H
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Manager/Test/DesignerTrajectoryHolderTest.cpp b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Manager/Test/DesignerTrajectoryHolderTest.cpp
index b4cfd59741881319be6f08f52a2ca589468dbfad..bc3e9b9393e946cc449df5331f1792c6511859bc 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Manager/Test/DesignerTrajectoryHolderTest.cpp
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Manager/Test/DesignerTrajectoryHolderTest.cpp
@@ -1,13 +1,13 @@
#define BOOST_TEST_MODULE ArmarX::RobotTrajectoryDesigner::DesignerTrajectoryHolder
#define ARMARX_BOOST_TEST
-#include <RobotComponents/Test.h>
-
+#include "../DesignerTrajectoryHolder.h"
#include <VirtualRobot/XML/RobotIO.h>
+
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
-#include "../DesignerTrajectoryHolder.h"
+#include <RobotComponents/Test.h>
using namespace armarx;
using namespace VirtualRobot;
@@ -20,8 +20,8 @@ public:
PoseBasePtr pose;
};
-PosePkg createPosePkg(float x, float y, float z,
- float qw, float qx, float qy, float qz)
+PosePkg
+createPosePkg(float x, float y, float z, float qw, float qx, float qy, float qz)
{
PosePkg posePkg;
posePkg.pos = Vector3BasePtr(new Vector3(x, y, z));
@@ -31,12 +31,12 @@ PosePkg createPosePkg(float x, float y, float z,
return posePkg;
}
-PoseBasePtr poseToLocal(PoseBasePtr pose, RobotPtr robot, std::string& rnsName)
+PoseBasePtr
+poseToLocal(PoseBasePtr pose, RobotPtr robot, std::string& rnsName)
{
return PoseBasePtr(
- new Pose(
- robot->getRobotNodeSet(rnsName)->getKinematicRoot()->toLocalCoordinateSystem(
- (new Pose(pose->position, pose->orientation))->toEigen())));
+ new Pose(robot->getRobotNodeSet(rnsName)->getKinematicRoot()->toLocalCoordinateSystem(
+ (new Pose(pose->position, pose->orientation))->toEigen())));
}
BOOST_AUTO_TEST_CASE(basicTest)
@@ -52,7 +52,7 @@ BOOST_AUTO_TEST_CASE(basicTest)
else
{
robot = RobotIO::loadRobot(
- "../../../../../data/RobotTrajectoryDesigner/Resources/ArmarIII.xml");
+ "../../../../../data/RobotTrajectoryDesigner/Resources/ArmarIII.xml");
}
EnvironmentPtr environment = std::make_shared<Environment>();
@@ -88,7 +88,7 @@ BOOST_AUTO_TEST_CASE(collisionDetectionNoCollisionTest)
else
{
robot = RobotIO::loadRobot(
- "../../../../../data/RobotTrajectoryDesigner/Resources/ArmarIII.xml");
+ "../../../../../data/RobotTrajectoryDesigner/Resources/ArmarIII.xml");
}
KinematicSolverPtr kc = KinematicSolver::getInstance(NULL, robot);
@@ -114,15 +114,38 @@ BOOST_AUTO_TEST_CASE(collisionDetectionNoCollisionTest)
DesignerTrajectoryManagerPtr dtm = dth->getTrajectoryManager(leftArmName);
-
-
// Create Poses
- std::vector<double> p1ja = { 0.464789, 0.357171, -0.303694, -0.067161, -0.445988, -0.204292, -0.070153};
-
- PosePkg pp4 = createPosePkg(-316.302246093750, 777.949218750, 1194.246459960938, 0.5907033681869507, -0.5503066778182983, 0.4992305040359497, 0.3146440684795380);
- PosePkg pp2 = createPosePkg(-182.23925781250, 580.074218750, 1034.98925781250, 0.4338901340961456, -0.4268467724323273, 0.5642792582511902, 0.5577904582023621);//reachable
- PosePkg pp3 = createPosePkg(-226.792480468750, 580.723144531250, 1186.157348632812, 0.4336481690406799, -0.4273631870746613, 0.5638203620910645, 0.5580471754074097);//reachable
- PosePkg pp5 = createPosePkg(-348.304718, 580.476440, 712.264465, 0.4453609585762024, -0.4538218379020691, 0.5429607033729553, 0.5485371351242065);//reachable
+ std::vector<double> p1ja = {
+ 0.464789, 0.357171, -0.303694, -0.067161, -0.445988, -0.204292, -0.070153};
+
+ PosePkg pp4 = createPosePkg(-316.302246093750,
+ 777.949218750,
+ 1194.246459960938,
+ 0.5907033681869507,
+ -0.5503066778182983,
+ 0.4992305040359497,
+ 0.3146440684795380);
+ PosePkg pp2 = createPosePkg(-182.23925781250,
+ 580.074218750,
+ 1034.98925781250,
+ 0.4338901340961456,
+ -0.4268467724323273,
+ 0.5642792582511902,
+ 0.5577904582023621); //reachable
+ PosePkg pp3 = createPosePkg(-226.792480468750,
+ 580.723144531250,
+ 1186.157348632812,
+ 0.4336481690406799,
+ -0.4273631870746613,
+ 0.5638203620910645,
+ 0.5580471754074097); //reachable
+ PosePkg pp5 = createPosePkg(-348.304718,
+ 580.476440,
+ 712.264465,
+ 0.4453609585762024,
+ -0.4538218379020691,
+ 0.5429607033729553,
+ 0.5485371351242065); //reachable
PoseBasePtr p1 = kc->doForwardKinematic(robot->getRobotNodeSet(leftArmName), p1ja);
PoseBasePtr p2 = poseToLocal(pp2.pose, robot, leftArmName);
@@ -157,7 +180,7 @@ BOOST_AUTO_TEST_CASE(collisionDetectionCollisionTest)
else
{
robot = RobotIO::loadRobot(
- "../../../../../data/RobotTrajectoryDesigner/Resources/ArmarIII.xml");
+ "../../../../../data/RobotTrajectoryDesigner/Resources/ArmarIII.xml");
}
KinematicSolverPtr kc = KinematicSolver::getInstance(NULL, robot);
@@ -182,11 +205,30 @@ BOOST_AUTO_TEST_CASE(collisionDetectionCollisionTest)
DesignerTrajectoryManagerPtr dtm = dth->getTrajectoryManager(leftArmName);
// Create Poses
- std::vector<double> p1ja = { 0.315176, -0.0275324, 1.00421, 0.0182586, 0.127499, -0.0255035, -0.299747};
- PosePkg pp1 = createPosePkg(56.5798, 233.545, 992.508, 0.4453609585762024, -0.4538218379020691, 0.5429607033729553, 0.5485371351242065);//Colliding Pose
- PosePkg pp2 = createPosePkg(-95.4907, 761.478, 1208.03, 0.4338901340961456, -0.4268467724323273, 0.5642792582511902, 0.5577904582023621);//Not Colliding Pose
-
- PosePkg pp9 = createPosePkg(-182.23925781250, 580.074218750, 1034.98925781250, 0.4338901340961456, -0.4268467724323273, 0.5642792582511902, 0.5577904582023621);//reachable
+ std::vector<double> p1ja = {
+ 0.315176, -0.0275324, 1.00421, 0.0182586, 0.127499, -0.0255035, -0.299747};
+ PosePkg pp1 = createPosePkg(56.5798,
+ 233.545,
+ 992.508,
+ 0.4453609585762024,
+ -0.4538218379020691,
+ 0.5429607033729553,
+ 0.5485371351242065); //Colliding Pose
+ PosePkg pp2 = createPosePkg(-95.4907,
+ 761.478,
+ 1208.03,
+ 0.4338901340961456,
+ -0.4268467724323273,
+ 0.5642792582511902,
+ 0.5577904582023621); //Not Colliding Pose
+
+ PosePkg pp9 = createPosePkg(-182.23925781250,
+ 580.074218750,
+ 1034.98925781250,
+ 0.4338901340961456,
+ -0.4268467724323273,
+ 0.5642792582511902,
+ 0.5577904582023621); //reachable
PoseBasePtr p1 = poseToLocal(pp1.pose, robot, leftArmName);
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Manager/Test/DesignerTrajectoryManagerTest.cpp b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Manager/Test/DesignerTrajectoryManagerTest.cpp
index a84a2c81f9035e065508d115227fe3d199bed000..10ec1d7b9666e86ea20708a5f26473ff8f06d866 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Manager/Test/DesignerTrajectoryManagerTest.cpp
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Manager/Test/DesignerTrajectoryManagerTest.cpp
@@ -2,15 +2,18 @@
#define ARMARX_BOOST_TEST
-#include <RobotComponents/Test.h>
#include "../Manager/DesignerTrajectoryManager.h"
+
#include <VirtualRobot/XML/RobotIO.h>
-#include "../Util/OrientationConversion.h"
-#include "../Interpolation/LinearInterpolation.h"
#include <ArmarXCore/core/logging/LogSender.h>
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
+#include <RobotComponents/Test.h>
+
+#include "../Interpolation/LinearInterpolation.h"
+#include "../Util/OrientationConversion.h"
+
using namespace armarx;
using namespace VirtualRobot;
using namespace std;
@@ -23,8 +26,8 @@ public:
PoseBasePtr pose;
};
-PosePkg createPosePkg(float x, float y, float z,
- float qw, float qx, float qy, float qz)
+PosePkg
+createPosePkg(float x, float y, float z, float qw, float qx, float qy, float qz)
{
PosePkg posePkg;
posePkg.pos = Vector3BasePtr(new Vector3(x, y, z));
@@ -34,12 +37,14 @@ PosePkg createPosePkg(float x, float y, float z,
return posePkg;
}
-double roundTo(double d, int accuracy)
+double
+roundTo(double d, int accuracy)
{
return floor(d * pow10(accuracy)) / pow10(accuracy);
}
-bool equalPoseBase(PoseBasePtr p1, PoseBasePtr p2)
+bool
+equalPoseBase(PoseBasePtr p1, PoseBasePtr p2)
{
int accuracy = 5;
@@ -80,7 +85,8 @@ bool equalPoseBase(PoseBasePtr p1, PoseBasePtr p2)
return true;
}
-bool equalTrajectory(armarx::TrajectoryPtr t1, armarx::TrajectoryPtr t2)
+bool
+equalTrajectory(armarx::TrajectoryPtr t1, armarx::TrajectoryPtr t2)
{
// check dimension
if (t1->dim() != t2->dim())
@@ -106,7 +112,8 @@ bool equalTrajectory(armarx::TrajectoryPtr t1, armarx::TrajectoryPtr t2)
return true;
}
-bool equalUserWaypoint(armarx::UserWaypointPtr u1, armarx::UserWaypointPtr u2)
+bool
+equalUserWaypoint(armarx::UserWaypointPtr u1, armarx::UserWaypointPtr u2)
{
// check poses
if (equalPoseBase(u1->getPose(), u2->getPose()) == false)
@@ -147,7 +154,8 @@ bool equalUserWaypoint(armarx::UserWaypointPtr u1, armarx::UserWaypointPtr u2)
return true;
}
-bool equalTransition(armarx::TransitionPtr t1, armarx::TransitionPtr t2)
+bool
+equalTransition(armarx::TransitionPtr t1, armarx::TransitionPtr t2)
{
// check start user waypoints
if (!equalUserWaypoint(t1->getStart(), t2->getStart()))
@@ -188,7 +196,8 @@ bool equalTransition(armarx::TransitionPtr t1, armarx::TransitionPtr t2)
return true;
}
-bool equalDesignerTrajectory(DesignerTrajectoryPtr dt1, DesignerTrajectoryPtr dt2)
+bool
+equalDesignerTrajectory(DesignerTrajectoryPtr dt1, DesignerTrajectoryPtr dt2)
{
// check inter breakpoint trajectories
std::vector<armarx::TrajectoryPtr> dt1_interBPT = dt1->getInterBreakpointTrajectories();
@@ -242,18 +251,30 @@ bool equalDesignerTrajectory(DesignerTrajectoryPtr dt1, DesignerTrajectoryPtr dt
return true;
}
-PoseBasePtr poseToLocal(PoseBasePtr pose, RobotPtr robot, std::string& rnsName)
+PoseBasePtr
+poseToLocal(PoseBasePtr pose, RobotPtr robot, std::string& rnsName)
{
return PoseBasePtr(
- new Pose(
- robot->getRobotNodeSet(rnsName)->getKinematicRoot()->toLocalCoordinateSystem(
- (new Pose(pose->position, pose->orientation))->toEigen())));
+ new Pose(robot->getRobotNodeSet(rnsName)->getKinematicRoot()->toLocalCoordinateSystem(
+ (new Pose(pose->position, pose->orientation))->toEigen())));
}
BOOST_AUTO_TEST_CASE(testEqualityOfPoseBases)
{
- PosePkg pp4 = createPosePkg(-316.302246093750, 777.949218750, 1194.246459960938, 0.5907033681869507, -0.5503066778182983, 0.4992305040359497, 0.3146440684795380);
- PosePkg pp2 = createPosePkg(-182.23925781250, 580.074218750, 1034.98925781250, 0.4338901340961456, -0.4268467724323273, 0.5642792582511902, 0.5577904582023621);//reachable
+ PosePkg pp4 = createPosePkg(-316.302246093750,
+ 777.949218750,
+ 1194.246459960938,
+ 0.5907033681869507,
+ -0.5503066778182983,
+ 0.4992305040359497,
+ 0.3146440684795380);
+ PosePkg pp2 = createPosePkg(-182.23925781250,
+ 580.074218750,
+ 1034.98925781250,
+ 0.4338901340961456,
+ -0.4268467724323273,
+ 0.5642792582511902,
+ 0.5577904582023621); //reachable
BOOST_CHECK(equalPoseBase(pp2.pose, pp2.pose));
BOOST_CHECK(equalPoseBase(pp4.pose, pp4.pose));
@@ -261,7 +282,13 @@ BOOST_AUTO_TEST_CASE(testEqualityOfPoseBases)
BOOST_CHECK(!equalPoseBase(pp4.pose, pp2.pose));
// pose with minimal difference to pp4
- PosePkg pp4_1 = createPosePkg(-316.302246093, 777.9492187, 1194.246459960746, 0.5907046681869507, -0.55030667789183983, 0.4992304564358497, 0.3146440488984465480);
+ PosePkg pp4_1 = createPosePkg(-316.302246093,
+ 777.9492187,
+ 1194.246459960746,
+ 0.5907046681869507,
+ -0.55030667789183983,
+ 0.4992304564358497,
+ 0.3146440488984465480);
BOOST_CHECK(equalPoseBase(pp4.pose, pp4_1.pose));
}
@@ -278,7 +305,7 @@ BOOST_AUTO_TEST_CASE(wrongUsage)
else
{
robot = RobotIO::loadRobot(
- "../../../../../data/RobotTrajectoryDesigner/Resources/ArmarIII.xml");
+ "../../../../../data/RobotTrajectoryDesigner/Resources/ArmarIII.xml");
}
@@ -288,8 +315,7 @@ BOOST_AUTO_TEST_CASE(wrongUsage)
//(5): Wrist 1 L, (6): Wrist 2 L
// Create Pose Packages
- PosePkg pp1_r = createPosePkg(4111, 7280, 1067,
- 0.6987, 0.1106, 0.7, -0.0969);
+ PosePkg pp1_r = createPosePkg(4111, 7280, 1067, 0.6987, 0.1106, 0.7, -0.0969);
vector<double> pp1_r_ja = {0.0540, 0.0398, 0.3851, 0.1253, -0.2440, 0.1100, -0.0342};
@@ -297,7 +323,8 @@ BOOST_AUTO_TEST_CASE(wrongUsage)
InterpolationType interpolationType = InterpolationType::eLinearInterpolation;
EnvironmentPtr environment = std::shared_ptr<Environment>(new Environment());
environment->setRobot(robot);
- DesignerTrajectoryManagerPtr m = std::shared_ptr<DesignerTrajectoryManager>(new DesignerTrajectoryManager(rns->getName(), environment));
+ DesignerTrajectoryManagerPtr m = std::shared_ptr<DesignerTrajectoryManager>(
+ new DesignerTrajectoryManager(rns->getName(), environment));
//m.initializeDesignerTrajectory(pp1_r_ja);
/////////////////////////////////////////////////////////////////////////////////////
@@ -325,7 +352,7 @@ BOOST_AUTO_TEST_CASE(basicTest)
else
{
robot = RobotIO::loadRobot(
- "../../../../../data/RobotTrajectoryDesigner/Resources/ArmarIII.xml");
+ "../../../../../data/RobotTrajectoryDesigner/Resources/ArmarIII.xml");
}
KinematicSolverPtr kc = KinematicSolver::getInstance(NULL, robot);
@@ -336,15 +363,41 @@ BOOST_AUTO_TEST_CASE(basicTest)
// Create DesignerTrajectoryManager
EnvironmentPtr environment = std::make_shared<Environment>();
environment->setRobot(robot);
- DesignerTrajectoryManagerPtr dtm = std::make_shared<DesignerTrajectoryManager>(rns->getName(), environment);
+ DesignerTrajectoryManagerPtr dtm =
+ std::make_shared<DesignerTrajectoryManager>(rns->getName(), environment);
// Create Poses
- std::vector<double> p1ja = { 0.464789, 0.357171, -0.303694, -0.067161, -0.445988, -0.204292, -0.070153};
-
- PosePkg pp4 = createPosePkg(-316.302246093750, 777.949218750, 1194.246459960938, 0.5907033681869507, -0.5503066778182983, 0.4992305040359497, 0.3146440684795380);
- PosePkg pp2 = createPosePkg(-182.23925781250, 580.074218750, 1034.98925781250, 0.4338901340961456, -0.4268467724323273, 0.5642792582511902, 0.5577904582023621);//reachable
- PosePkg pp3 = createPosePkg(-226.792480468750, 580.723144531250, 1186.157348632812, 0.4336481690406799, -0.4273631870746613, 0.5638203620910645, 0.5580471754074097);//reachable
- PosePkg pp5 = createPosePkg(-348.304718, 580.476440, 712.264465, 0.4453609585762024, -0.4538218379020691, 0.5429607033729553, 0.5485371351242065);//reachable
+ std::vector<double> p1ja = {
+ 0.464789, 0.357171, -0.303694, -0.067161, -0.445988, -0.204292, -0.070153};
+
+ PosePkg pp4 = createPosePkg(-316.302246093750,
+ 777.949218750,
+ 1194.246459960938,
+ 0.5907033681869507,
+ -0.5503066778182983,
+ 0.4992305040359497,
+ 0.3146440684795380);
+ PosePkg pp2 = createPosePkg(-182.23925781250,
+ 580.074218750,
+ 1034.98925781250,
+ 0.4338901340961456,
+ -0.4268467724323273,
+ 0.5642792582511902,
+ 0.5577904582023621); //reachable
+ PosePkg pp3 = createPosePkg(-226.792480468750,
+ 580.723144531250,
+ 1186.157348632812,
+ 0.4336481690406799,
+ -0.4273631870746613,
+ 0.5638203620910645,
+ 0.5580471754074097); //reachable
+ PosePkg pp5 = createPosePkg(-348.304718,
+ 580.476440,
+ 712.264465,
+ 0.4453609585762024,
+ -0.4538218379020691,
+ 0.5429607033729553,
+ 0.5485371351242065); //reachable
std::string rnsName = rns->getName();
PoseBasePtr p1 = poseToLocal(kc->doForwardKinematic(rns, p1ja), robot, rnsName);
@@ -390,18 +443,17 @@ BOOST_AUTO_TEST_CASE(basicTest)
BOOST_CHECK(dt->getUserWaypoint(3)->getJointAngles().size() != 0);
-
BOOST_CHECK_EQUAL(dt->getTransition(0)->getTimeOptimalDuration(),
- dt->getTransition(0)->getEnd()->getTimeOptimalTimestamp()
- - dt->getTransition(0)->getStart()->getTimeOptimalTimestamp());
+ dt->getTransition(0)->getEnd()->getTimeOptimalTimestamp() -
+ dt->getTransition(0)->getStart()->getTimeOptimalTimestamp());
BOOST_CHECK_EQUAL(dt->getTransition(1)->getTimeOptimalDuration(),
- dt->getTransition(1)->getEnd()->getTimeOptimalTimestamp()
- - dt->getTransition(1)->getStart()->getTimeOptimalTimestamp());
+ dt->getTransition(1)->getEnd()->getTimeOptimalTimestamp() -
+ dt->getTransition(1)->getStart()->getTimeOptimalTimestamp());
BOOST_CHECK_EQUAL(dt->getTransition(2)->getTimeOptimalDuration(),
- dt->getTransition(2)->getEnd()->getTimeOptimalTimestamp()
- - dt->getTransition(2)->getStart()->getTimeOptimalTimestamp());
+ dt->getTransition(2)->getEnd()->getTimeOptimalTimestamp() -
+ dt->getTransition(2)->getStart()->getTimeOptimalTimestamp());
BOOST_CHECK(dt->getTimeOptimalTrajectory()->size() > 1);
@@ -453,7 +505,7 @@ BOOST_AUTO_TEST_CASE(undoRedoTest1)
else
{
robot = RobotIO::loadRobot(
- "../../../../../data/RobotTrajectoryDesigner/Resources/ArmarIII.xml");
+ "../../../../../data/RobotTrajectoryDesigner/Resources/ArmarIII.xml");
}
KinematicSolverPtr kc = KinematicSolver::getInstance(NULL, robot);
@@ -464,15 +516,41 @@ BOOST_AUTO_TEST_CASE(undoRedoTest1)
// Create DesignerTrajectoryManager
EnvironmentPtr environment = std::make_shared<Environment>();
environment->setRobot(robot);
- DesignerTrajectoryManagerPtr dtm = std::make_shared<DesignerTrajectoryManager>(rns->getName(), environment);
+ DesignerTrajectoryManagerPtr dtm =
+ std::make_shared<DesignerTrajectoryManager>(rns->getName(), environment);
// Create Poses
- std::vector<double> p1ja = { 0.464789, 0.357171, -0.303694, -0.067161, -0.445988, -0.204292, -0.070153};
-
- PosePkg pp4 = createPosePkg(-316.302246093750, 777.949218750, 1194.246459960938, 0.5907033681869507, -0.5503066778182983, 0.4992305040359497, 0.3146440684795380);
- PosePkg pp2 = createPosePkg(-182.23925781250, 580.074218750, 1034.98925781250, 0.4338901340961456, -0.4268467724323273, 0.5642792582511902, 0.5577904582023621);//reachable
- PosePkg pp3 = createPosePkg(-226.792480468750, 580.723144531250, 1186.157348632812, 0.4336481690406799, -0.4273631870746613, 0.5638203620910645, 0.5580471754074097);//reachable
- PosePkg pp5 = createPosePkg(-348.304718, 580.476440, 712.264465, 0.4453609585762024, -0.4538218379020691, 0.5429607033729553, 0.5485371351242065);//reachable
+ std::vector<double> p1ja = {
+ 0.464789, 0.357171, -0.303694, -0.067161, -0.445988, -0.204292, -0.070153};
+
+ PosePkg pp4 = createPosePkg(-316.302246093750,
+ 777.949218750,
+ 1194.246459960938,
+ 0.5907033681869507,
+ -0.5503066778182983,
+ 0.4992305040359497,
+ 0.3146440684795380);
+ PosePkg pp2 = createPosePkg(-182.23925781250,
+ 580.074218750,
+ 1034.98925781250,
+ 0.4338901340961456,
+ -0.4268467724323273,
+ 0.5642792582511902,
+ 0.5577904582023621); //reachable
+ PosePkg pp3 = createPosePkg(-226.792480468750,
+ 580.723144531250,
+ 1186.157348632812,
+ 0.4336481690406799,
+ -0.4273631870746613,
+ 0.5638203620910645,
+ 0.5580471754074097); //reachable
+ PosePkg pp5 = createPosePkg(-348.304718,
+ 580.476440,
+ 712.264465,
+ 0.4453609585762024,
+ -0.4538218379020691,
+ 0.5429607033729553,
+ 0.5485371351242065); //reachable
std::string rnsName = rns->getName();
PoseBasePtr p1 = poseToLocal(kc->doForwardKinematic(rns, p1ja), robot, rnsName);
@@ -500,8 +578,6 @@ BOOST_AUTO_TEST_CASE(undoRedoTest1)
dtm->undo();
-
-
//BOOST_CHECK(equalDesignerTrajectory(dtm->getDesignerTrajectory(), dtptr1));
//dtm->redo();
//BOOST_CHECK(equalDesignerTrajectory(dtm->getDesignerTrajectory(), dtptr2));
@@ -522,7 +598,7 @@ BOOST_AUTO_TEST_CASE(undoRedoTest2)
else
{
robot = RobotIO::loadRobot(
- "../../../../../data/RobotTrajectoryDesigner/Resources/ArmarIII.xml");
+ "../../../../../data/RobotTrajectoryDesigner/Resources/ArmarIII.xml");
}
KinematicSolverPtr kc = KinematicSolver::getInstance(NULL, robot);
@@ -533,15 +609,41 @@ BOOST_AUTO_TEST_CASE(undoRedoTest2)
// Create DesignerTrajectoryManager
EnvironmentPtr environment = std::make_shared<Environment>();
environment->setRobot(robot);
- DesignerTrajectoryManagerPtr dtm = std::make_shared<DesignerTrajectoryManager>(rns->getName(), environment);
+ DesignerTrajectoryManagerPtr dtm =
+ std::make_shared<DesignerTrajectoryManager>(rns->getName(), environment);
// Create Poses
- std::vector<double> p1ja = { 0.464789, 0.357171, -0.303694, -0.067161, -0.445988, -0.204292, -0.070153};
-
- PosePkg pp4 = createPosePkg(-316.302246093750, 777.949218750, 1194.246459960938, 0.5907033681869507, -0.5503066778182983, 0.4992305040359497, 0.3146440684795380);
- PosePkg pp2 = createPosePkg(-182.23925781250, 580.074218750, 1034.98925781250, 0.4338901340961456, -0.4268467724323273, 0.5642792582511902, 0.5577904582023621);//reachable
- PosePkg pp3 = createPosePkg(-226.792480468750, 580.723144531250, 1186.157348632812, 0.4336481690406799, -0.4273631870746613, 0.5638203620910645, 0.5580471754074097);//reachable
- PosePkg pp5 = createPosePkg(-348.304718, 580.476440, 712.264465, 0.4453609585762024, -0.4538218379020691, 0.5429607033729553, 0.5485371351242065);//reachable
+ std::vector<double> p1ja = {
+ 0.464789, 0.357171, -0.303694, -0.067161, -0.445988, -0.204292, -0.070153};
+
+ PosePkg pp4 = createPosePkg(-316.302246093750,
+ 777.949218750,
+ 1194.246459960938,
+ 0.5907033681869507,
+ -0.5503066778182983,
+ 0.4992305040359497,
+ 0.3146440684795380);
+ PosePkg pp2 = createPosePkg(-182.23925781250,
+ 580.074218750,
+ 1034.98925781250,
+ 0.4338901340961456,
+ -0.4268467724323273,
+ 0.5642792582511902,
+ 0.5577904582023621); //reachable
+ PosePkg pp3 = createPosePkg(-226.792480468750,
+ 580.723144531250,
+ 1186.157348632812,
+ 0.4336481690406799,
+ -0.4273631870746613,
+ 0.5638203620910645,
+ 0.5580471754074097); //reachable
+ PosePkg pp5 = createPosePkg(-348.304718,
+ 580.476440,
+ 712.264465,
+ 0.4453609585762024,
+ -0.4538218379020691,
+ 0.5429607033729553,
+ 0.5485371351242065); //reachable
PoseBasePtr p1 = kc->doForwardKinematic(rns, p1ja);
std::string rnsName = rns->getName();
@@ -586,7 +688,7 @@ BOOST_AUTO_TEST_CASE(breakpointTest)
else
{
robot = RobotIO::loadRobot(
- "../../../../../data/RobotTrajectoryDesigner/Resources/ArmarIII.xml");
+ "../../../../../data/RobotTrajectoryDesigner/Resources/ArmarIII.xml");
}
KinematicSolverPtr kc = KinematicSolver::getInstance(NULL, robot);
@@ -597,15 +699,41 @@ BOOST_AUTO_TEST_CASE(breakpointTest)
// Create DesignerTrajectoryManager
EnvironmentPtr environment = std::make_shared<Environment>();
environment->setRobot(robot);
- DesignerTrajectoryManagerPtr dtm = std::make_shared<DesignerTrajectoryManager>(rns->getName(), environment);
+ DesignerTrajectoryManagerPtr dtm =
+ std::make_shared<DesignerTrajectoryManager>(rns->getName(), environment);
// Create Poses
- std::vector<double> p1ja = { 0.464789, 0.357171, -0.303694, -0.067161, -0.445988, -0.204292, -0.070153};
-
- PosePkg pp4 = createPosePkg(-316.302246093750, 777.949218750, 1194.246459960938, 0.5907033681869507, -0.5503066778182983, 0.4992305040359497, 0.3146440684795380);
- PosePkg pp2 = createPosePkg(-182.23925781250, 580.074218750, 1034.98925781250, 0.4338901340961456, -0.4268467724323273, 0.5642792582511902, 0.5577904582023621);//reachable
- PosePkg pp3 = createPosePkg(-226.792480468750, 580.723144531250, 1186.157348632812, 0.4336481690406799, -0.4273631870746613, 0.5638203620910645, 0.5580471754074097);//reachable
- PosePkg pp5 = createPosePkg(-348.304718, 580.476440, 712.264465, 0.4453609585762024, -0.4538218379020691, 0.5429607033729553, 0.5485371351242065);//reachable
+ std::vector<double> p1ja = {
+ 0.464789, 0.357171, -0.303694, -0.067161, -0.445988, -0.204292, -0.070153};
+
+ PosePkg pp4 = createPosePkg(-316.302246093750,
+ 777.949218750,
+ 1194.246459960938,
+ 0.5907033681869507,
+ -0.5503066778182983,
+ 0.4992305040359497,
+ 0.3146440684795380);
+ PosePkg pp2 = createPosePkg(-182.23925781250,
+ 580.074218750,
+ 1034.98925781250,
+ 0.4338901340961456,
+ -0.4268467724323273,
+ 0.5642792582511902,
+ 0.5577904582023621); //reachable
+ PosePkg pp3 = createPosePkg(-226.792480468750,
+ 580.723144531250,
+ 1186.157348632812,
+ 0.4336481690406799,
+ -0.4273631870746613,
+ 0.5638203620910645,
+ 0.5580471754074097); //reachable
+ PosePkg pp5 = createPosePkg(-348.304718,
+ 580.476440,
+ 712.264465,
+ 0.4453609585762024,
+ -0.4538218379020691,
+ 0.5429607033729553,
+ 0.5485371351242065); //reachable
std::string rnsName = rns->getName();
PoseBasePtr p1 = poseToLocal(kc->doForwardKinematic(rns, p1ja), robot, rnsName);
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Model/DesignerTrajectory.cpp b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Model/DesignerTrajectory.cpp
index e34f3818fb856ebaa9dc4cb808ad686898f0152f..f920ffe57d04e03d366e983bf44972b2fe7f7fcd 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Model/DesignerTrajectory.cpp
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Model/DesignerTrajectory.cpp
@@ -1,15 +1,18 @@
#include "DesignerTrajectory.h"
+
#include <iostream>
using namespace std;
using namespace armarx;
-VirtualRobot::RobotNodeSetPtr DesignerTrajectory::getRns()
+VirtualRobot::RobotNodeSetPtr
+DesignerTrajectory::getRns()
{
return rns;
}
-void DesignerTrajectory::setRns(const VirtualRobot::RobotNodeSetPtr& value)
+void
+DesignerTrajectory::setRns(const VirtualRobot::RobotNodeSetPtr& value)
{
if (value != nullptr)
{
@@ -21,13 +24,14 @@ void DesignerTrajectory::setRns(const VirtualRobot::RobotNodeSetPtr& value)
}
}
-std::vector<TrajectoryPtr> DesignerTrajectory::getInterBreakpointTrajectories()
+std::vector<TrajectoryPtr>
+DesignerTrajectory::getInterBreakpointTrajectories()
{
return interBreakpointTrajectories;
}
-void DesignerTrajectory::setInterBreakpointTrajectories(
- const std::vector<TrajectoryPtr>& value)
+void
+DesignerTrajectory::setInterBreakpointTrajectories(const std::vector<TrajectoryPtr>& value)
{
if (value.size() != 0)
{
@@ -35,14 +39,13 @@ void DesignerTrajectory::setInterBreakpointTrajectories(
}
else
{
- throw InvalidArgumentException(
- "Can not set std::vector<TrajectoryPtr> interBreakpointTrajectories with empty vector ");
+ throw InvalidArgumentException("Can not set std::vector<TrajectoryPtr> "
+ "interBreakpointTrajectories with empty vector ");
}
}
-
armarx::DesignerTrajectory::DesignerTrajectory(UserWaypointPtr& firstPoint,
- VirtualRobot::RobotNodeSetPtr newRns)
+ VirtualRobot::RobotNodeSetPtr newRns)
{
if (newRns != nullptr)
{
@@ -50,7 +53,8 @@ armarx::DesignerTrajectory::DesignerTrajectory(UserWaypointPtr& firstPoint,
}
else
{
- throw InvalidArgumentException("Can not construct DesignerTrajectory with VirtualRobot::RobotNodeSetPtr rns = nullptr");
+ throw InvalidArgumentException("Can not construct DesignerTrajectory with "
+ "VirtualRobot::RobotNodeSetPtr rns = nullptr");
}
if (firstPoint != nullptr)
{
@@ -58,7 +62,8 @@ armarx::DesignerTrajectory::DesignerTrajectory(UserWaypointPtr& firstPoint,
}
else
{
- throw InvalidArgumentException("Can not construct DesignerTrajectory with UserWaypointPtr firstPoint = nullptr");
+ throw InvalidArgumentException(
+ "Can not construct DesignerTrajectory with UserWaypointPtr firstPoint = nullptr");
}
std::vector<Ice::DoubleSeq> data;
@@ -76,12 +81,11 @@ armarx::DesignerTrajectory::DesignerTrajectory(UserWaypointPtr& firstPoint,
throw InvalidArgumentException("firstPoint of a DesignerTrajectory must have jointAngles");
}
//firstPoint has eveytime timestamp 0
- interBreakpointTrajectories.push_back(TrajectoryPtr(new Trajectory(data,
- {0}, rns->getNodeNames())));
+ interBreakpointTrajectories.push_back(
+ TrajectoryPtr(new Trajectory(data, {0}, rns->getNodeNames())));
}
-DesignerTrajectory::DesignerTrajectory(const DesignerTrajectory& source) :
- rns(source.rns)
+DesignerTrajectory::DesignerTrajectory(const DesignerTrajectory& source) : rns(source.rns)
{
std::vector<TrajectoryPtr> trajectoriesTmp;
std::vector<UserWaypointPtr> userWaypointsTmp;
@@ -112,7 +116,8 @@ DesignerTrajectory::DesignerTrajectory(const DesignerTrajectory& source) :
transitions = transitionsTmp;
}
-void DesignerTrajectory::addFirstUserWaypoint(UserWaypointPtr& point)
+void
+DesignerTrajectory::addFirstUserWaypoint(UserWaypointPtr& point)
{
if (point != nullptr)
{
@@ -120,19 +125,18 @@ void DesignerTrajectory::addFirstUserWaypoint(UserWaypointPtr& point)
if (userWaypoints.size() > 1)
{
- transitions.insert(transitions.begin(), TransitionPtr(
- new Transition(userWaypoints[0], userWaypoints[1])));
-
+ transitions.insert(transitions.begin(),
+ TransitionPtr(new Transition(userWaypoints[0], userWaypoints[1])));
}
}
else
{
throw InvalidArgumentException("Can not add UserWaypoint with point = nullptr");
}
-
}
-void DesignerTrajectory::addLastUserWaypoint(UserWaypointPtr& point)
+void
+DesignerTrajectory::addLastUserWaypoint(UserWaypointPtr& point)
{
if (point != nullptr)
{
@@ -140,8 +144,7 @@ void DesignerTrajectory::addLastUserWaypoint(UserWaypointPtr& point)
if (userWaypoints.size() > 1)
{
transitions.push_back(TransitionPtr(new Transition(
- userWaypoints[userWaypoints.size() - 2],
- userWaypoints[userWaypoints.size() - 1])));
+ userWaypoints[userWaypoints.size() - 2], userWaypoints[userWaypoints.size() - 1])));
}
}
else
@@ -150,8 +153,8 @@ void DesignerTrajectory::addLastUserWaypoint(UserWaypointPtr& point)
}
}
-void DesignerTrajectory::insertUserWaypoint(
- UserWaypointPtr& point, unsigned int index)
+void
+DesignerTrajectory::insertUserWaypoint(UserWaypointPtr& point, unsigned int index)
{
if (point != nullptr)
{
@@ -161,10 +164,9 @@ void DesignerTrajectory::insertUserWaypoint(
if (userWaypoints.size() > 1)
{
- transitions.insert(transitions.begin() + index - 1,
- TransitionPtr(
- new Transition(userWaypoints[index - 1],
- userWaypoints[index])));
+ transitions.insert(
+ transitions.begin() + index - 1,
+ TransitionPtr(new Transition(userWaypoints[index - 1], userWaypoints[index])));
transitions[index]->setStart(userWaypoints[index]);
}
}
@@ -184,10 +186,10 @@ void DesignerTrajectory::insertUserWaypoint(
{
throw InvalidArgumentException("Can not add UserWaypoint with point = nullptr");
}
-
}
-void DesignerTrajectory::deleteUserWaypoint(unsigned int index)
+void
+DesignerTrajectory::deleteUserWaypoint(unsigned int index)
{
if (index < userWaypoints.size())
{
@@ -208,8 +210,7 @@ void DesignerTrajectory::deleteUserWaypoint(unsigned int index)
transitions.erase(transitions.begin() + index - 1);
transitions.insert(transitions.begin() + index - 1,
std::shared_ptr<Transition>(
- new Transition(userWaypoints[index - 1],
- userWaypoints[index])));
+ new Transition(userWaypoints[index - 1], userWaypoints[index])));
}
}
else
@@ -218,13 +219,14 @@ void DesignerTrajectory::deleteUserWaypoint(unsigned int index)
}
}
-
-unsigned int DesignerTrajectory::getNrOfUserWaypoints() const
+unsigned int
+DesignerTrajectory::getNrOfUserWaypoints() const
{
return userWaypoints.size();
}
-UserWaypointPtr DesignerTrajectory::getUserWaypoint(unsigned int index)
+UserWaypointPtr
+DesignerTrajectory::getUserWaypoint(unsigned int index)
{
if (index < userWaypoints.size())
{
@@ -234,10 +236,10 @@ UserWaypointPtr DesignerTrajectory::getUserWaypoint(unsigned int index)
{
throw IndexOutOfBoundsException("getUserWaypoint");
}
-
}
-TransitionPtr DesignerTrajectory::getTransition(unsigned int index)
+TransitionPtr
+DesignerTrajectory::getTransition(unsigned int index)
{
if (index < transitions.size())
{
@@ -247,21 +249,22 @@ TransitionPtr DesignerTrajectory::getTransition(unsigned int index)
{
throw IndexOutOfBoundsException();
}
-
}
-TrajectoryPtr DesignerTrajectory::getTimeOptimalTrajectory()
+TrajectoryPtr
+DesignerTrajectory::getTimeOptimalTrajectory()
{
std::vector<Ice::DoubleSeq> dimensionDatas = getDimensionDatas();
Ice::DoubleSeq timestamps = getAllTimestamps();
- TrajectoryPtr tmp = TrajectoryPtr(new Trajectory(dimensionDatas, timestamps,
- interBreakpointTrajectories[0]->getDimensionNames()));
+ TrajectoryPtr tmp = TrajectoryPtr(new Trajectory(
+ dimensionDatas, timestamps, interBreakpointTrajectories[0]->getDimensionNames()));
setLimitless(tmp, rns);
return tmp;
}
-TrajectoryPtr DesignerTrajectory::getTrajectorySegment(unsigned int index)
+TrajectoryPtr
+DesignerTrajectory::getTrajectorySegment(unsigned int index)
{
if (index < interBreakpointTrajectories.size())
{
@@ -271,12 +274,12 @@ TrajectoryPtr DesignerTrajectory::getTrajectorySegment(unsigned int index)
{
throw IndexOutOfBoundsException();
}
-
}
-std::vector<UserWaypointPtr> DesignerTrajectory::getAllUserWaypoints() const
+std::vector<UserWaypointPtr>
+DesignerTrajectory::getAllUserWaypoints() const
{
- std::vector <UserWaypointPtr> tmp;
+ std::vector<UserWaypointPtr> tmp;
for (unsigned int i = 0; i < userWaypoints.size(); i++)
{
@@ -285,12 +288,14 @@ std::vector<UserWaypointPtr> DesignerTrajectory::getAllUserWaypoints() const
return tmp;
}
-std::vector<UserWaypointPtr> DesignerTrajectory::getAllUserWaypoints()
+std::vector<UserWaypointPtr>
+DesignerTrajectory::getAllUserWaypoints()
{
return userWaypoints;
}
-TrajectoryPtr DesignerTrajectory::getFinalTrajectory() //const
+TrajectoryPtr
+DesignerTrajectory::getFinalTrajectory() //const
{
if (userWaypoints.size() > 1)
{
@@ -334,12 +339,10 @@ TrajectoryPtr DesignerTrajectory::getFinalTrajectory() //const
tmpDif = tmpDif + newDuration - oldDuration;
timestamps[k + 1] = timestamps[k] + newDuration;
}
- TrajectoryPtr traj = TrajectoryPtr(new Trajectory(dimensionDatas,
- timestamps,
- interBreakpointTrajectories[0]->getDimensionNames()));
+ TrajectoryPtr traj = TrajectoryPtr(new Trajectory(
+ dimensionDatas, timestamps, interBreakpointTrajectories[0]->getDimensionNames()));
setLimitless(traj, rns);
return traj;
-
}
else
{
@@ -347,9 +350,9 @@ TrajectoryPtr DesignerTrajectory::getFinalTrajectory() //const
}
}
-
//////////////private/////////////////////////////////////////////////////////////////////
-std::vector<std::vector<double>> DesignerTrajectory::getDimensionDatas()
+std::vector<std::vector<double>>
+DesignerTrajectory::getDimensionDatas()
{
std::vector<Ice::DoubleSeq> dimensionDatas;
@@ -371,9 +374,8 @@ std::vector<std::vector<double>> DesignerTrajectory::getDimensionDatas()
{
//get all jointAngles
std::vector<double> newDatas = t->getDimensionData(i);
- dimensionDatas[i].insert(dimensionDatas[i].end(),
- newDatas.begin() + 1,
- newDatas.end());
+ dimensionDatas[i].insert(
+ dimensionDatas[i].end(), newDatas.begin() + 1, newDatas.end());
}
}
return dimensionDatas;
@@ -384,8 +386,8 @@ std::vector<std::vector<double>> DesignerTrajectory::getDimensionDatas()
}
}
-
-std::vector<double> DesignerTrajectory::getAllTimestamps()
+std::vector<double>
+DesignerTrajectory::getAllTimestamps()
{
std::vector<double> timestamps;
@@ -411,7 +413,8 @@ std::vector<double> DesignerTrajectory::getAllTimestamps()
}
}
-void DesignerTrajectory::setLimitless(TrajectoryPtr traj, VirtualRobot::RobotNodeSetPtr rns)
+void
+DesignerTrajectory::setLimitless(TrajectoryPtr traj, VirtualRobot::RobotNodeSetPtr rns)
{
//set Limitless state for smooth interpolation
LimitlessStateSeq states;
@@ -425,4 +428,3 @@ void DesignerTrajectory::setLimitless(TrajectoryPtr traj, VirtualRobot::RobotNod
}
traj->setLimitless(states);
}
-
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Model/DesignerTrajectory.h b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Model/DesignerTrajectory.h
index d370b4eef62d4684c22348b1979ed41ac8f9c802..4296a2ebd2acae27a2fd5927ebd9445f9358e89c 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Model/DesignerTrajectory.h
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Model/DesignerTrajectory.h
@@ -26,10 +26,9 @@
#include <VirtualRobot/RobotNodeSet.h>
-#include "UserWaypoint.h"
-#include "Transition.h"
-
+#include "Transition.h"
+#include "UserWaypoint.h"
namespace armarx
{
@@ -50,8 +49,7 @@ namespace armarx
public:
- DesignerTrajectory(UserWaypointPtr& firstPoint,
- VirtualRobot::RobotNodeSetPtr newRns);
+ DesignerTrajectory(UserWaypointPtr& firstPoint, VirtualRobot::RobotNodeSetPtr newRns);
/**
* @brief Deep copy constructor of designerTrajectory
@@ -160,9 +158,9 @@ namespace armarx
* @param value interBreakpointTrajectories
*/
void setInterBreakpointTrajectories(const std::vector<TrajectoryPtr>& value);
-
};
+
using DesignerTrajectoryPtr = std::shared_ptr<DesignerTrajectory>;
-}
+} // namespace armarx
#endif // DESIGNERTRAJECTORY_H
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Model/Test/DesignerTrajectoryTests.cpp b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Model/Test/DesignerTrajectoryTests.cpp
index f644732faba2bcbfc81347f6fedf145d925411ae..acdeef9e62a6d47c541e2d059e92d700cd4dd4b3 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Model/Test/DesignerTrajectoryTests.cpp
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Model/Test/DesignerTrajectoryTests.cpp
@@ -1,26 +1,28 @@
#define BOOST_TEST_MODULE ArmarX::RobotTrajectoryDesigner::DesignerTrajectory
#define ARMARX_BOOST_TEST
+#include <VirtualRobot/XML/RobotIO.h>
+
+#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
+
#include <RobotComponents/Test.h>
#include "../DesignerTrajectory.h"
#include "../Util/OrientationConversion.h"
-#include <VirtualRobot/XML/RobotIO.h>
-#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
using namespace armarx;
namespace
{
- bool equalPose(PoseBasePtr p1, PoseBasePtr p2)
+ bool
+ equalPose(PoseBasePtr p1, PoseBasePtr p2)
{
QuaternionPtr q1 = QuaternionPtr::dynamicCast(p1->orientation);
QuaternionPtr q2 = QuaternionPtr::dynamicCast(p2->orientation);
Vector3Ptr pos1 = Vector3Ptr::dynamicCast(p1->position);
Vector3Ptr pos2 = Vector3Ptr::dynamicCast(p2->position);
- if (q1->toEigen() == q2->toEigen() &&
- pos1->toEigen() == pos2->toEigen())
+ if (q1->toEigen() == q2->toEigen() && pos1->toEigen() == pos2->toEigen())
{
return true;
}
@@ -30,18 +32,21 @@ namespace
}
}
- bool equalUserWaypoints(UserWaypointPtr w1, UserWaypointPtr w2)
+ bool
+ equalUserWaypoints(UserWaypointPtr w1, UserWaypointPtr w2)
{
return (w1->getIKSelection() == w2->getIKSelection() &&
w1->getIsTimeOptimalBreakpoint() == w2->getIsTimeOptimalBreakpoint() &&
w1->getJointAngles() == w2->getJointAngles() &&
equalPose(w1->getPose(), w2->getPose()) &&
w1->getTimeOptimalTimestamp() == w1->getTimeOptimalTimestamp() &&
- w1->getUserTimestamp() == w2->getUserTimestamp()) ? true : false ;
+ w1->getUserTimestamp() == w2->getUserTimestamp())
+ ? true
+ : false;
}
-}
+} // namespace
BOOST_AUTO_TEST_CASE(basicTest)
{
@@ -95,7 +100,7 @@ BOOST_AUTO_TEST_CASE(basicTest)
//add UserWaypoints and test Transition
dt1.addFirstUserWaypoint(w2);
dt1.insertUserWaypoint(w3, 1);
- dt1.addLastUserWaypoint(w4); // Userwaypoints should be w2,w3,w1,w4
+ dt1.addLastUserWaypoint(w4); // Userwaypoints should be w2,w3,w1,w4
BOOST_CHECK_EQUAL(dt1.getTransition(0)->getStart(), w2);
BOOST_CHECK_EQUAL(dt1.getTransition(0)->getEnd(), w3);
@@ -106,7 +111,8 @@ BOOST_AUTO_TEST_CASE(basicTest)
//delete UserWaypoint
dt1.deleteUserWaypoint(1);
- BOOST_CHECK_THROW(dt1.getTransition(2), IndexOutOfBoundsException); //there should be not three transitions
+ BOOST_CHECK_THROW(dt1.getTransition(2),
+ IndexOutOfBoundsException); //there should be not three transitions
BOOST_CHECK_EQUAL(dt1.getTransition(0)->getStart(), w2);
BOOST_CHECK_EQUAL(dt1.getTransition(0)->getEnd(), w1);
BOOST_CHECK_EQUAL(dt1.getNrOfUserWaypoints(), 3);
@@ -132,42 +138,33 @@ BOOST_AUTO_TEST_CASE(getTimeOptimalTraj)
else
{
robot = VirtualRobot::RobotIO::loadRobot(
- "../../../../../data/RobotTrajectoryDesigner/Resources/ArmarIII.xml");
+ "../../../../../data/RobotTrajectoryDesigner/Resources/ArmarIII.xml");
}
VirtualRobot::RobotNodeSetPtr rns = robot->getRobotNodeSet(robot->getRobotNodeSetNames()[0]);
DesignerTrajectory dt1 = DesignerTrajectory(w1, rns);
//Init three Trajectories
- std::vector<std::vector<double>> data1 =
- {
- {1, 2, 3, 4, 5},
- {1, 2, 3, 4, 5},
- {1, 2, 3, 4, 5},
- {1, 2, 3, 4, 5},
- {1, 2, 3, 4, 5},
- {1, 2, 3, 4, 5},
- {1, 2, 3, 4, 5}
- };
- std::vector<std::vector<double>> data2 =
- {
- {5, 6, 7, 8, 9},
- {5, 6, 7, 8, 9},
- {5, 6, 7, 8, 9},
- {5, 6, 7, 8, 9},
- {5, 6, 7, 8, 9},
- {5, 6, 7, 8, 9},
- {5, 6, 7, 8, 9}
- };
- std::vector<std::vector<double>> data3 =
- {
- {9, 10, 11, 12, 13},
- {9, 10, 11, 12, 13},
- {9, 10, 11, 12, 13},
- {9, 10, 11, 12, 13},
- {9, 10, 11, 12, 13},
- {9, 10, 11, 12, 13},
- {9, 10, 11, 12, 13}
- };
+ std::vector<std::vector<double>> data1 = {{1, 2, 3, 4, 5},
+ {1, 2, 3, 4, 5},
+ {1, 2, 3, 4, 5},
+ {1, 2, 3, 4, 5},
+ {1, 2, 3, 4, 5},
+ {1, 2, 3, 4, 5},
+ {1, 2, 3, 4, 5}};
+ std::vector<std::vector<double>> data2 = {{5, 6, 7, 8, 9},
+ {5, 6, 7, 8, 9},
+ {5, 6, 7, 8, 9},
+ {5, 6, 7, 8, 9},
+ {5, 6, 7, 8, 9},
+ {5, 6, 7, 8, 9},
+ {5, 6, 7, 8, 9}};
+ std::vector<std::vector<double>> data3 = {{9, 10, 11, 12, 13},
+ {9, 10, 11, 12, 13},
+ {9, 10, 11, 12, 13},
+ {9, 10, 11, 12, 13},
+ {9, 10, 11, 12, 13},
+ {9, 10, 11, 12, 13},
+ {9, 10, 11, 12, 13}};
Ice::DoubleSeq timestamps1 = {0, 1, 2, 3, 4};
Ice::DoubleSeq timestamps2 = {0, 1, 2, 3, 4};
@@ -183,16 +180,13 @@ BOOST_AUTO_TEST_CASE(getTimeOptimalTraj)
BOOST_CHECK_EQUAL(dt1.getInterBreakpointTrajectories().size(), 3);
- std::vector<std::vector<double>> data4 =
- {
- {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13},
- {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13},
- {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13},
- {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13},
- {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13},
- {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13},
- {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13}
- };
+ std::vector<std::vector<double>> data4 = {{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13},
+ {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13},
+ {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13},
+ {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13},
+ {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13},
+ {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13},
+ {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13}};
Ice::DoubleSeq timestamps4 = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12};
TrajectoryPtr traj4(new Trajectory(data4, timestamps4, dimensionNames));
@@ -222,10 +216,3 @@ BOOST_AUTO_TEST_CASE(getTimeOptimalTraj)
BOOST_AUTO_TEST_CASE(getFinalTraj)
{
}
-
-
-
-
-
-
-
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Model/Test/TransitionTests.cpp b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Model/Test/TransitionTests.cpp
index 8e1fea04e6c26d2940e968a0698c36fa629d02ca..4466d103f6ada84137e92b580709cd5ebb392be3 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Model/Test/TransitionTests.cpp
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Model/Test/TransitionTests.cpp
@@ -8,6 +8,7 @@
#include "../Util/OrientationConversion.h"
using namespace armarx;
+
BOOST_AUTO_TEST_CASE(basicTest)
{
//Init
@@ -45,7 +46,6 @@ BOOST_AUTO_TEST_CASE(basicTest)
BOOST_CHECK_THROW(t1.setTimeOptimalDuration(-1), InvalidArgumentException);
}
-
BOOST_AUTO_TEST_CASE(deepCopyTest)
{
//Init
@@ -80,7 +80,8 @@ BOOST_AUTO_TEST_CASE(deepCopyTest)
t1.getStart()->setUserTimestamp(2);
t1.getEnd()->setTimeOptimalTimestamp(10);
t1.getEnd()->setUserTimestamp(20);
- Transition t2 = Transition(t1, UserWaypointPtr(new UserWaypoint(*t1.getStart())),
+ Transition t2 = Transition(t1,
+ UserWaypointPtr(new UserWaypoint(*t1.getStart())),
UserWaypointPtr(new UserWaypoint(*t1.getEnd())));
@@ -114,4 +115,3 @@ BOOST_AUTO_TEST_CASE(deepCopyTest)
BOOST_CHECK_EQUAL(t1.getEnd()->getUserTimestamp(), 40);
BOOST_CHECK_EQUAL(t1.getEnd()->getTimeOptimalTimestamp(), 30);
}
-
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Model/Test/UserWaypointTests.cpp b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Model/Test/UserWaypointTests.cpp
index 10512a2e6bc1890399ceed1d44297294cfb1e5df..0e7495f32211974d333964b9adf306b5f5204acf 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Model/Test/UserWaypointTests.cpp
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Model/Test/UserWaypointTests.cpp
@@ -8,6 +8,7 @@
#include "../Util/OrientationConversion.h"
using namespace armarx;
+
BOOST_AUTO_TEST_CASE(basicTest)
{
// consturct a UserWaypoint and tests getter
@@ -23,7 +24,6 @@ BOOST_AUTO_TEST_CASE(basicTest)
BOOST_CHECK_EQUAL(w1.getIsTimeOptimalBreakpoint(), false);
-
//check getPose
BOOST_CHECK_EQUAL(w1.getPose(), pose1);
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Model/Tests/DesignerTrajectoryTests.cpp b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Model/Tests/DesignerTrajectoryTests.cpp
index 7e03600add08426efcb0d12eb0c5aa9030e85f41..9c41d5bf3ed5a93f673e0ebc02a7c76387b34bd2 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Model/Tests/DesignerTrajectoryTests.cpp
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Model/Tests/DesignerTrajectoryTests.cpp
@@ -1,24 +1,25 @@
#define BOOST_TEST_MODULE ArmarX::RobotTrajectoryDesigner::DesignerTrajectory
#define ARMARX_BOOST_TEST
+#include <VirtualRobot/XML/RobotIO.h>
+
#include "../../../../../build/testing/RobotTrajectoryDesigner/Test.h"
#include "../DesignerTrajectory.h"
#include "../Util/OrientationConversion.h"
-#include <VirtualRobot/XML/RobotIO.h>
using namespace armarx;
namespace
{
- bool equalPose(PoseBasePtr p1, PoseBasePtr p2)
+ bool
+ equalPose(PoseBasePtr p1, PoseBasePtr p2)
{
QuaternionPtr q1 = QuaternionPtr::dynamicCast(p1->orientation);
QuaternionPtr q2 = QuaternionPtr::dynamicCast(p2->orientation);
Vector3Ptr pos1 = Vector3Ptr::dynamicCast(p1->position);
Vector3Ptr pos2 = Vector3Ptr::dynamicCast(p2->position);
- if (q1->toEigen() == q2->toEigen() &&
- pos1->toEigen() == pos2->toEigen())
+ if (q1->toEigen() == q2->toEigen() && pos1->toEigen() == pos2->toEigen())
{
return true;
}
@@ -28,18 +29,21 @@ namespace
}
}
- bool equalUserWaypoints(UserWaypointPtr w1, UserWaypointPtr w2)
+ bool
+ equalUserWaypoints(UserWaypointPtr w1, UserWaypointPtr w2)
{
return (w1->getIKSelection() == w2->getIKSelection() &&
w1->getIsTimeOptimalBreakpoint() == w2->getIsTimeOptimalBreakpoint() &&
w1->getJointAngles() == w2->getJointAngles() &&
equalPose(w1->getPose(), w2->getPose()) &&
w1->getTimeOptimalTimestamp() == w1->getTimeOptimalTimestamp() &&
- w1->getUserTimestamp() == w2->getUserTimestamp()) ? true : false ;
+ w1->getUserTimestamp() == w2->getUserTimestamp())
+ ? true
+ : false;
}
-}
+} // namespace
BOOST_AUTO_TEST_CASE(basicTest)
{
@@ -67,7 +71,7 @@ BOOST_AUTO_TEST_CASE(basicTest)
UserWaypointPtr w4 = UserWaypointPtr(new UserWaypoint(pose4));
VirtualRobot::RobotPtr robot = VirtualRobot::RobotIO::loadRobot(
- "../../../../../data/RobotTrajectoryDesigner/Resources/ArmarIII.xml");
+ "../../../../../data/RobotTrajectoryDesigner/Resources/ArmarIII.xml");
VirtualRobot::RobotNodeSetPtr rns = robot->getRobotNodeSet(robot->getRobotNodeSetNames()[0]);
@@ -87,7 +91,7 @@ BOOST_AUTO_TEST_CASE(basicTest)
//add UserWaypoints and test Transition
dt1.addFirstUserWaypoint(w2);
dt1.insertUserWaypoint(w3, 1);
- dt1.addLastUserWaypoint(w4); // Userwaypoints should be w2,w3,w1,w4
+ dt1.addLastUserWaypoint(w4); // Userwaypoints should be w2,w3,w1,w4
BOOST_CHECK_EQUAL(dt1.getTransition(0)->getStart(), w2);
BOOST_CHECK_EQUAL(dt1.getTransition(0)->getEnd(), w3);
@@ -98,7 +102,8 @@ BOOST_AUTO_TEST_CASE(basicTest)
//delete UserWaypoint
dt1.deleteUserWaypoint(1);
- BOOST_CHECK_THROW(dt1.getTransition(2), IndexOutOfBoundsException); //there should be not three transitions
+ BOOST_CHECK_THROW(dt1.getTransition(2),
+ IndexOutOfBoundsException); //there should be not three transitions
BOOST_CHECK_EQUAL(dt1.getTransition(0)->getStart(), w2);
BOOST_CHECK_EQUAL(dt1.getTransition(0)->getEnd(), w1);
BOOST_CHECK_EQUAL(dt1.getNrOfUserWaypoints(), 3);
@@ -112,41 +117,32 @@ BOOST_AUTO_TEST_CASE(getTimeOptimalTraj)
PoseBasePtr pose1 = PoseBasePtr(new Pose(pos1, ori1));
UserWaypointPtr w1 = UserWaypointPtr(new UserWaypoint(pose1));
VirtualRobot::RobotPtr robot = VirtualRobot::RobotIO::loadRobot(
- "../../../../../data/RobotTrajectoryDesigner/Resources/ArmarIII.xml");
+ "../../../../../data/RobotTrajectoryDesigner/Resources/ArmarIII.xml");
VirtualRobot::RobotNodeSetPtr rns = robot->getRobotNodeSet(robot->getRobotNodeSetNames()[0]);
DesignerTrajectory dt1 = DesignerTrajectory(w1, rns);
//Init three Trajectories
- std::vector<std::vector<double>> data1 =
- {
- {1, 2, 3, 4, 5},
- {1, 2, 3, 4, 5},
- {1, 2, 3, 4, 5},
- {1, 2, 3, 4, 5},
- {1, 2, 3, 4, 5},
- {1, 2, 3, 4, 5},
- {1, 2, 3, 4, 5}
- };
- std::vector<std::vector<double>> data2 =
- {
- {5, 6, 7, 8, 9},
- {5, 6, 7, 8, 9},
- {5, 6, 7, 8, 9},
- {5, 6, 7, 8, 9},
- {5, 6, 7, 8, 9},
- {5, 6, 7, 8, 9},
- {5, 6, 7, 8, 9}
- };
- std::vector<std::vector<double>> data3 =
- {
- {9, 10, 11, 12, 13},
- {9, 10, 11, 12, 13},
- {9, 10, 11, 12, 13},
- {9, 10, 11, 12, 13},
- {9, 10, 11, 12, 13},
- {9, 10, 11, 12, 13},
- {9, 10, 11, 12, 13}
- };
+ std::vector<std::vector<double>> data1 = {{1, 2, 3, 4, 5},
+ {1, 2, 3, 4, 5},
+ {1, 2, 3, 4, 5},
+ {1, 2, 3, 4, 5},
+ {1, 2, 3, 4, 5},
+ {1, 2, 3, 4, 5},
+ {1, 2, 3, 4, 5}};
+ std::vector<std::vector<double>> data2 = {{5, 6, 7, 8, 9},
+ {5, 6, 7, 8, 9},
+ {5, 6, 7, 8, 9},
+ {5, 6, 7, 8, 9},
+ {5, 6, 7, 8, 9},
+ {5, 6, 7, 8, 9},
+ {5, 6, 7, 8, 9}};
+ std::vector<std::vector<double>> data3 = {{9, 10, 11, 12, 13},
+ {9, 10, 11, 12, 13},
+ {9, 10, 11, 12, 13},
+ {9, 10, 11, 12, 13},
+ {9, 10, 11, 12, 13},
+ {9, 10, 11, 12, 13},
+ {9, 10, 11, 12, 13}};
Ice::DoubleSeq timestamps1 = {0, 1, 2, 3, 4};
Ice::DoubleSeq timestamps2 = {0, 1, 2, 3, 4};
@@ -162,16 +158,13 @@ BOOST_AUTO_TEST_CASE(getTimeOptimalTraj)
BOOST_CHECK_EQUAL(dt1.getInterBreakpointTrajectories().size(), 3);
- std::vector<std::vector<double>> data4 =
- {
- {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13},
- {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13},
- {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13},
- {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13},
- {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13},
- {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13},
- {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13}
- };
+ std::vector<std::vector<double>> data4 = {{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13},
+ {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13},
+ {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13},
+ {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13},
+ {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13},
+ {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13},
+ {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13}};
Ice::DoubleSeq timestamps4 = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12};
TrajectoryPtr traj4(new Trajectory(data4, timestamps4, dimensionNames));
@@ -201,10 +194,3 @@ BOOST_AUTO_TEST_CASE(getTimeOptimalTraj)
BOOST_AUTO_TEST_CASE(getFinalTraj)
{
}
-
-
-
-
-
-
-
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Model/Tests/TransitionTests.cpp b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Model/Tests/TransitionTests.cpp
index 9cfc04132ca3183e8c7cf73e60e19d64e93a3e59..3e74c0f65e398d46ecce03a8a79224f3cd39eaa0 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Model/Tests/TransitionTests.cpp
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Model/Tests/TransitionTests.cpp
@@ -6,6 +6,7 @@
#include "../Util/OrientationConversion.h"
using namespace armarx;
+
BOOST_AUTO_TEST_CASE(basicTest)
{
//Init
@@ -43,7 +44,6 @@ BOOST_AUTO_TEST_CASE(basicTest)
BOOST_CHECK_THROW(t1.setTimeOptimalDuration(-1), InvalidArgumentException);
}
-
BOOST_AUTO_TEST_CASE(deepCopyTest)
{
//Init
@@ -111,4 +111,3 @@ BOOST_AUTO_TEST_CASE(deepCopyTest)
BOOST_CHECK_EQUAL(t1.getEnd()->getUserTimestamp(), 40);
BOOST_CHECK_EQUAL(t1.getEnd()->getTimeOptimalTimestamp(), 30);
}
-
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Model/Tests/UserWaypointTests.cpp b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Model/Tests/UserWaypointTests.cpp
index 4ee1af0972edc043632fe7e267cb772d53b8cf54..69da093c8b7c14213436305aa19aaa60dd06a4c7 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Model/Tests/UserWaypointTests.cpp
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Model/Tests/UserWaypointTests.cpp
@@ -6,6 +6,7 @@
#include "../Util/OrientationConversion.h"
using namespace armarx;
+
BOOST_AUTO_TEST_CASE(basicTest)
{
// consturct a UserWaypoint and tests getter
@@ -21,7 +22,6 @@ BOOST_AUTO_TEST_CASE(basicTest)
BOOST_CHECK_EQUAL(w1.getIsTimeOptimalBreakpoint(), false);
-
//check getPose
BOOST_CHECK_EQUAL(w1.getPose(), pose1);
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Model/Transition.cpp b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Model/Transition.cpp
index d8b0951603d240bdddf035e5a1c6c2516f364dbe..4034cd6b7ae8393a5efd3f4ea00b0db73ede9b0d 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Model/Transition.cpp
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Model/Transition.cpp
@@ -2,18 +2,21 @@
using namespace armarx;
-double Transition::getUserDuration() const
+double
+Transition::getUserDuration() const
{
return userDuration;
}
-void Transition::setUserDuration(double value)
+void
+Transition::setUserDuration(double value)
{
if (value >= 0)
{
if (value < timeOptimalDuration)
{
- throw InvalidArgumentException("User duration must be greater than or equal timeOptimalDuration");
+ throw InvalidArgumentException(
+ "User duration must be greater than or equal timeOptimalDuration");
}
else
{
@@ -27,18 +30,20 @@ void Transition::setUserDuration(double value)
}
}
-double Transition::getTimeOptimalDuration() const
+double
+Transition::getTimeOptimalDuration() const
{
return timeOptimalDuration;
}
-void Transition::setTimeOptimalDuration(double value)
+void
+Transition::setTimeOptimalDuration(double value)
{
if (value >= 0)
{
timeOptimalDuration = value;
- getEnd()->setTimeOptimalTimestamp(getStart()->getTimeOptimalTimestamp()
- + timeOptimalDuration);
+ getEnd()->setTimeOptimalTimestamp(getStart()->getTimeOptimalTimestamp() +
+ timeOptimalDuration);
if (userDuration == 0 || userDuration < timeOptimalDuration)
{
setUserDuration(timeOptimalDuration);
@@ -50,32 +55,38 @@ void Transition::setTimeOptimalDuration(double value)
}
}
-InterpolationType Transition::getInterpolationType() const
+InterpolationType
+Transition::getInterpolationType() const
{
return interpolationType;
}
-void Transition::setInterpolationType(const InterpolationType& value)
+void
+Transition::setInterpolationType(const InterpolationType& value)
{
interpolationType = value;
}
-TrajectoryPtr Transition::getTrajectory()
+TrajectoryPtr
+Transition::getTrajectory()
{
return trajectory;
}
-void Transition::setTrajectory(const TrajectoryPtr& value)
+void
+Transition::setTrajectory(const TrajectoryPtr& value)
{
trajectory = value;
}
-UserWaypointPtr Transition::getStart()
+UserWaypointPtr
+Transition::getStart()
{
return start;
}
-void Transition::setStart(const UserWaypointPtr& value)
+void
+Transition::setStart(const UserWaypointPtr& value)
{
if (value != nullptr)
{
@@ -87,12 +98,14 @@ void Transition::setStart(const UserWaypointPtr& value)
}
}
-UserWaypointPtr Transition::getEnd()
+UserWaypointPtr
+Transition::getEnd()
{
return end;
}
-void Transition::setEnd(const UserWaypointPtr& value)
+void
+Transition::setEnd(const UserWaypointPtr& value)
{
if (value != nullptr)
{
@@ -104,8 +117,8 @@ void Transition::setEnd(const UserWaypointPtr& value)
}
}
-armarx::Transition::Transition(UserWaypointPtr& newStart, UserWaypointPtr& newEnd)
- : interpolationType(eLinearInterpolation)
+armarx::Transition::Transition(UserWaypointPtr& newStart, UserWaypointPtr& newEnd) :
+ interpolationType(eLinearInterpolation)
{
if (newStart != nullptr)
{
@@ -127,10 +140,13 @@ armarx::Transition::Transition(UserWaypointPtr& newStart, UserWaypointPtr& newEn
userDuration = 0;
}
-Transition::Transition(const Transition& source, const UserWaypointPtr newStart, const UserWaypointPtr newEnd) :
+Transition::Transition(const Transition& source,
+ const UserWaypointPtr newStart,
+ const UserWaypointPtr newEnd) :
start(newStart),
end(newEnd),
- timeOptimalDuration(source.timeOptimalDuration), userDuration(source.userDuration),
+ timeOptimalDuration(source.timeOptimalDuration),
+ userDuration(source.userDuration),
interpolationType(source.interpolationType),
trajectory(IceInternal::Handle<Trajectory>(new Trajectory(*source.trajectory)))
{
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Model/Transition.h b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Model/Transition.h
index 449ab36566eaf6b5ac063ff88d88bef8058a187f..f298a567505548e3ec0e9b5723bd71e5c4999735 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Model/Transition.h
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Model/Transition.h
@@ -25,9 +25,9 @@
#define TRANSITION_H
#include <RobotAPI/libraries/core/Trajectory.h>
-#include "UserWaypoint.h"
-#include "../Interpolation/InterpolationType.h"
+#include "../Interpolation/InterpolationType.h"
+#include "UserWaypoint.h"
namespace armarx
{
@@ -45,7 +45,6 @@ namespace armarx
TrajectoryPtr trajectory = TrajectoryPtr(new Trajectory());
-
public:
/**
* @brief Transition
@@ -57,7 +56,9 @@ namespace armarx
* @brief Deep copy constructor of the class Transition.
* @param source to copy
*/
- Transition(const Transition& source, const UserWaypointPtr newStart, const UserWaypointPtr newEnd);
+ Transition(const Transition& source,
+ const UserWaypointPtr newStart,
+ const UserWaypointPtr newEnd);
///////GET///////////////////////////////////////////////////////////////////////
/**
@@ -138,9 +139,10 @@ namespace armarx
*/
void setEnd(const UserWaypointPtr& value);
};
+
using TransitionPtr = std::shared_ptr<Transition>;
-}
+} // namespace armarx
#endif // TRANSITION_H
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Model/UserWaypoint.cpp b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Model/UserWaypoint.cpp
index 2d9cbe04306174de95de9ff7774db828480b55a5..072ddd49eea0f2b8e0cb7793dab3dc610ce7db96 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Model/UserWaypoint.cpp
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Model/UserWaypoint.cpp
@@ -2,12 +2,14 @@
using namespace armarx;
-std::vector<double> UserWaypoint::getJointAngles() const
+std::vector<double>
+UserWaypoint::getJointAngles() const
{
return jointAngles;
}
-void UserWaypoint::setJointAngles(const std::vector<double>& value)
+void
+UserWaypoint::setJointAngles(const std::vector<double>& value)
{
if (value.size() != 0)
{
@@ -19,32 +21,38 @@ void UserWaypoint::setJointAngles(const std::vector<double>& value)
}
}
-IKSelection UserWaypoint::getIKSelection() const
+IKSelection
+UserWaypoint::getIKSelection() const
{
return iKSelection;
}
-void UserWaypoint::setIKSelection(const IKSelection& value)
+void
+UserWaypoint::setIKSelection(const IKSelection& value)
{
iKSelection = value;
}
-bool UserWaypoint::getIsTimeOptimalBreakpoint() const
+bool
+UserWaypoint::getIsTimeOptimalBreakpoint() const
{
return isTimeOptimalBreakpoint;
}
-void UserWaypoint::setIsTimeOptimalBreakpoint(bool value)
+void
+UserWaypoint::setIsTimeOptimalBreakpoint(bool value)
{
isTimeOptimalBreakpoint = value;
}
-double UserWaypoint::getTimeOptimalTimestamp() const
+double
+UserWaypoint::getTimeOptimalTimestamp() const
{
return timeOptimalTimestamp;
}
-void UserWaypoint::setTimeOptimalTimestamp(double value)
+void
+UserWaypoint::setTimeOptimalTimestamp(double value)
{
if (value >= 0)
{
@@ -60,12 +68,14 @@ void UserWaypoint::setTimeOptimalTimestamp(double value)
}
}
-double UserWaypoint::getUserTimestamp() const
+double
+UserWaypoint::getUserTimestamp() const
{
return userTimestamp;
}
-void UserWaypoint::setUserTimestamp(double value)
+void
+UserWaypoint::setUserTimestamp(double value)
{
if (value >= timeOptimalTimestamp)
{
@@ -73,24 +83,28 @@ void UserWaypoint::setUserTimestamp(double value)
}
else
{
- throw InvalidArgumentException("User timestamp must be greater than or equal timeOptimal timestamp");
+ throw InvalidArgumentException(
+ "User timestamp must be greater than or equal timeOptimal timestamp");
}
}
-PoseBasePtr UserWaypoint::getPose()
+PoseBasePtr
+UserWaypoint::getPose()
{
return pose;
}
-void UserWaypoint::setPose(const PoseBasePtr& value)
+void
+UserWaypoint::setPose(const PoseBasePtr& value)
{
pose = value;
}
-UserWaypoint::UserWaypoint(PoseBasePtr newPose):
+UserWaypoint::UserWaypoint(PoseBasePtr newPose) :
iKSelection(VirtualRobot::IKSolver::CartesianSelection::All),
isTimeOptimalBreakpoint(false),
- timeOptimalTimestamp(0), userTimestamp(0)
+ timeOptimalTimestamp(0),
+ userTimestamp(0)
{
pose = newPose;
}
@@ -98,10 +112,9 @@ UserWaypoint::UserWaypoint(PoseBasePtr newPose):
UserWaypoint::UserWaypoint(const UserWaypoint& source) :
pose(IceInternal::Handle<PoseBase>(new Pose(*PosePtr::dynamicCast(source.pose)))),
jointAngles(source.jointAngles),
- iKSelection(source.iKSelection), isTimeOptimalBreakpoint(source.isTimeOptimalBreakpoint),
- timeOptimalTimestamp(source.timeOptimalTimestamp), userTimestamp(source.userTimestamp)
+ iKSelection(source.iKSelection),
+ isTimeOptimalBreakpoint(source.isTimeOptimalBreakpoint),
+ timeOptimalTimestamp(source.timeOptimalTimestamp),
+ userTimestamp(source.userTimestamp)
{
}
-
-
-
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Model/UserWaypoint.h b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Model/UserWaypoint.h
index 0b021dc83baa6886d23ca78064562cfb235be5f9..f2a4a19e2681666d82cbf450d9bc9e64f0658085 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Model/UserWaypoint.h
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Model/UserWaypoint.h
@@ -24,18 +24,23 @@
#ifndef USERWAYPOINT_H
#define USERWAYPOINT_H
-#include <RobotAPI/interface/core/PoseBase.h>
-#include <Eigen/Core>
-#include <RobotComponents/interface/components/RobotIK.h>
#include <memory>
+
+#include <Eigen/Core>
+
#include <VirtualRobot/IK/GenericIKSolver.h>
+
#include <ArmarXCore/core/util/PropagateConst.h>
+
+#include <RobotAPI/interface/core/PoseBase.h>
#include <RobotAPI/libraries/core/FramedPose.h>
+#include <RobotComponents/interface/components/RobotIK.h>
namespace armarx
{
using IKSelection = VirtualRobot::IKSolver::CartesianSelection;
+
/**
* @brief The UserWaypoint class represents a waypoint of the trajectory.
*/
@@ -132,8 +137,8 @@ namespace armarx
* @param user timestamp
*/
void setUserTimestamp(double value);
-
};
+
using UserWaypointPtr = std::shared_ptr<UserWaypoint>;
-}
+} // namespace armarx
#endif // USERWAYPOINT_H
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/RobotTrajectoryDesignerGuiPluginGuiPlugin.cpp b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/RobotTrajectoryDesignerGuiPluginGuiPlugin.cpp
index 7a853af6f32ef5fb1ac05d228489ab501b012271..8c0acaaf755a780699c3e0955972a764cdc8fb40 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/RobotTrajectoryDesignerGuiPluginGuiPlugin.cpp
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/RobotTrajectoryDesignerGuiPluginGuiPlugin.cpp
@@ -28,8 +28,9 @@ using namespace armarx;
RobotTrajectoryDesignerGuiPluginGuiPlugin::RobotTrajectoryDesignerGuiPluginGuiPlugin()
{
- addWidget < RobotTrajectoryDesignerGuiPluginWidgetController > ();
+ addWidget<RobotTrajectoryDesignerGuiPluginWidgetController>();
}
#if QT_VERSION < QT_VERSION_CHECK(5, 0, 0)
-Q_EXPORT_PLUGIN2(armarx_gui_RobotTrajectoryDesignerGuiPluginGuiPlugin, RobotTrajectoryDesignerGuiPluginGuiPlugin);
+Q_EXPORT_PLUGIN2(armarx_gui_RobotTrajectoryDesignerGuiPluginGuiPlugin,
+ RobotTrajectoryDesignerGuiPluginGuiPlugin);
#endif
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/RobotTrajectoryDesignerGuiPluginGuiPlugin.h b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/RobotTrajectoryDesignerGuiPluginGuiPlugin.h
index a9dfa80ba0fe71eaf3a777f764c9027582d871cc..b16f54b8b66f9d2ccf16252c55d2fa826b5441ca 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/RobotTrajectoryDesignerGuiPluginGuiPlugin.h
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/RobotTrajectoryDesignerGuiPluginGuiPlugin.h
@@ -24,8 +24,9 @@
#define _ARMARX_RobotTrajectoryDesigner_RobotTrajectoryDesignerGuiPlugin_GuiPlugin_H
#include <ArmarXCore/core/system/ImportExportComponent.h>
-#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
+
#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
namespace armarx
{
@@ -36,7 +37,7 @@ namespace armarx
*
* Detailed description
*/
- class ARMARXCOMPONENT_IMPORT_EXPORT RobotTrajectoryDesignerGuiPluginGuiPlugin:
+ class ARMARXCOMPONENT_IMPORT_EXPORT RobotTrajectoryDesignerGuiPluginGuiPlugin :
public armarx::ArmarXGuiPlugin
{
Q_INTERFACES(ArmarXGuiInterface)
@@ -48,6 +49,6 @@ namespace armarx
*/
RobotTrajectoryDesignerGuiPluginGuiPlugin();
};
-}
+} // namespace armarx
#endif
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/RobotTrajectoryDesignerGuiPluginWidgetController.cpp b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/RobotTrajectoryDesignerGuiPluginWidgetController.cpp
index 5e03c9155bef306ee2d4c8b94fab577830ff0ccc..9f1f128b911d6bb468469fca124e8a32f6431d5d 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/RobotTrajectoryDesignerGuiPluginWidgetController.cpp
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/RobotTrajectoryDesignerGuiPluginWidgetController.cpp
@@ -23,10 +23,12 @@
#include "RobotTrajectoryDesignerGuiPluginWidgetController.h"
#include <string>
+
#include <QCoreApplication>
-#include <VirtualRobot/XML/RobotIO.h>
#include <QShortcut>
+#include <VirtualRobot/XML/RobotIO.h>
+
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
@@ -37,36 +39,35 @@ RobotTrajectoryDesignerGuiPluginWidgetController::RobotTrajectoryDesignerGuiPlug
{
}
-
-RobotTrajectoryDesignerGuiPluginWidgetController::~RobotTrajectoryDesignerGuiPluginWidgetController()
+RobotTrajectoryDesignerGuiPluginWidgetController::
+ ~RobotTrajectoryDesignerGuiPluginWidgetController()
{
-
}
-
-void RobotTrajectoryDesignerGuiPluginWidgetController::loadSettings(QSettings* settings)
+void
+RobotTrajectoryDesignerGuiPluginWidgetController::loadSettings(QSettings* settings)
{
-
}
-void RobotTrajectoryDesignerGuiPluginWidgetController::saveSettings(QSettings* settings)
+void
+RobotTrajectoryDesignerGuiPluginWidgetController::saveSettings(QSettings* settings)
{
-
}
-
-void RobotTrajectoryDesignerGuiPluginWidgetController::onInitComponent()
+void
+RobotTrajectoryDesignerGuiPluginWidgetController::onInitComponent()
{
QMetaObject::invokeMethod(this, "initWidget");
}
-
-void RobotTrajectoryDesignerGuiPluginWidgetController::onConnectComponent()
+void
+RobotTrajectoryDesignerGuiPluginWidgetController::onConnectComponent()
{
QMetaObject::invokeMethod(this, "connectWidget");
}
-void RobotTrajectoryDesignerGuiPluginWidgetController::onDisconnectComponent()
+void
+RobotTrajectoryDesignerGuiPluginWidgetController::onDisconnectComponent()
{
viewControllerPtr.reset();
toolBarControllerPtr.reset();
@@ -87,25 +88,27 @@ void RobotTrajectoryDesignerGuiPluginWidgetController::onDisconnectComponent()
KinematicSolver::reset();
}
-void RobotTrajectoryDesignerGuiPluginWidgetController::initWidget()
+void
+RobotTrajectoryDesignerGuiPluginWidgetController::initWidget()
{
widget.setupUi(getWidget());
// Create shared pointers for each member, initialize each controller
this->exportDialogControllerPtr = std::make_shared<ExportDialogController>();
this->importDialogControllerPtr = std::make_shared<ImportDialogController>();
- this->mementoControllerPtr = std::make_shared<MementoController>(
- widget.undoButton, widget.redoButton);
- this->robotVisualizationControllerPtr = std::make_shared<RobotVisualizationController>(widget.robotVisualization);
+ this->mementoControllerPtr =
+ std::make_shared<MementoController>(widget.undoButton, widget.redoButton);
+ this->robotVisualizationControllerPtr =
+ std::make_shared<RobotVisualizationController>(widget.robotVisualization);
this->robotVisualizationControllerPtr->addConnection(this->robotVisualizationControllerPtr);
SettingTabPtr settingTabPtr(widget.settingTab);
this->settingControllerPtr = std::make_shared<SettingController>(settingTabPtr);
this->shortcutControllerPtr = std::make_shared<ShortcutController>(getWidget().data());
TCPInformationPtr tcpInformationPtr(widget.tcpInformationTab);
- this->tcpInformationControllerPtr = std::make_shared<TCPInformationController>(
- tcpInformationPtr);
- this->tcpSelectionControllerPtr = std::make_shared<TCPSelectionController>(
- widget.currentTPComboBox);
+ this->tcpInformationControllerPtr =
+ std::make_shared<TCPInformationController>(tcpInformationPtr);
+ this->tcpSelectionControllerPtr =
+ std::make_shared<TCPSelectionController>(widget.currentTPComboBox);
ToolBarPtr toolBarPtr(widget.toolBar);
this->toolBarControllerPtr = std::make_shared<ToolBarController>(toolBarPtr);
@@ -113,8 +116,7 @@ void RobotTrajectoryDesignerGuiPluginWidgetController::initWidget()
this->trajectoryControllerPtr = std::make_shared<TrajectoryController>();
TransitionTabPtr transitionTabPtr(widget.transitionTab);
- this->transitionControllerPtr = std::make_shared<TransitionController>(
- transitionTabPtr);
+ this->transitionControllerPtr = std::make_shared<TransitionController>(transitionTabPtr);
PerspectivesPtr perspectivesPtr(widget.perspectives);
this->viewControllerPtr = std::make_shared<ViewController>(perspectivesPtr);
widget.perspectives->raise();
@@ -124,313 +126,534 @@ void RobotTrajectoryDesignerGuiPluginWidgetController::initWidget()
this->environmentControllerPtr = std::make_shared<EnvironmentController>();
}
-void RobotTrajectoryDesignerGuiPluginWidgetController::connectWidget()
+void
+RobotTrajectoryDesignerGuiPluginWidgetController::connectWidget()
{
//=============================================================================================
//EnvironmentController Signals
- QObject::connect(environmentControllerPtr.get(), SIGNAL(environmentChanged(EnvironmentPtr)),
- exportDialogControllerPtr.get(), SLOT(environmentChanged(EnvironmentPtr)));
- QObject::connect(environmentControllerPtr.get(), SIGNAL(environmentChanged(EnvironmentPtr)),
- importDialogControllerPtr.get(), SLOT(environmentChanged(EnvironmentPtr)));
- QObject::connect(environmentControllerPtr.get(), SIGNAL(environmentChanged(EnvironmentPtr)),
- robotVisualizationControllerPtr.get(), SLOT(environmentChanged(EnvironmentPtr)));
- QObject::connect(environmentControllerPtr.get(), SIGNAL(environmentChanged(EnvironmentPtr)),
- trajectoryControllerPtr.get(), SLOT(environmentChanged(EnvironmentPtr)));
- QObject::connect(environmentControllerPtr.get(), SIGNAL(environmentChanged(EnvironmentPtr)),
- settingControllerPtr.get(), SLOT(environmentChanged(EnvironmentPtr)));
+ QObject::connect(environmentControllerPtr.get(),
+ SIGNAL(environmentChanged(EnvironmentPtr)),
+ exportDialogControllerPtr.get(),
+ SLOT(environmentChanged(EnvironmentPtr)));
+ QObject::connect(environmentControllerPtr.get(),
+ SIGNAL(environmentChanged(EnvironmentPtr)),
+ importDialogControllerPtr.get(),
+ SLOT(environmentChanged(EnvironmentPtr)));
+ QObject::connect(environmentControllerPtr.get(),
+ SIGNAL(environmentChanged(EnvironmentPtr)),
+ robotVisualizationControllerPtr.get(),
+ SLOT(environmentChanged(EnvironmentPtr)));
+ QObject::connect(environmentControllerPtr.get(),
+ SIGNAL(environmentChanged(EnvironmentPtr)),
+ trajectoryControllerPtr.get(),
+ SLOT(environmentChanged(EnvironmentPtr)));
+ QObject::connect(environmentControllerPtr.get(),
+ SIGNAL(environmentChanged(EnvironmentPtr)),
+ settingControllerPtr.get(),
+ SLOT(environmentChanged(EnvironmentPtr)));
//=============================================================================================
//ExportDialogController Signals
- QObject::connect(exportDialogControllerPtr.get(), SIGNAL(exportDesignerTrajectory(int)),
- trajectoryControllerPtr.get(), SLOT(exportTrajectory(int)));
- QObject::connect(exportDialogControllerPtr.get(), SIGNAL(exportDesignerTrajectory()),
- trajectoryControllerPtr.get(), SLOT(exportTrajectory()));
+ QObject::connect(exportDialogControllerPtr.get(),
+ SIGNAL(exportDesignerTrajectory(int)),
+ trajectoryControllerPtr.get(),
+ SLOT(exportTrajectory(int)));
+ QObject::connect(exportDialogControllerPtr.get(),
+ SIGNAL(exportDesignerTrajectory()),
+ trajectoryControllerPtr.get(),
+ SLOT(exportTrajectory()));
//=============================================================================================
//ImportDialogController Signals
- QObject::connect(importDialogControllerPtr.get(), SIGNAL(import(DesignerTrajectoryPtr)),
- trajectoryControllerPtr.get(), SLOT(import(DesignerTrajectoryPtr)));
+ QObject::connect(importDialogControllerPtr.get(),
+ SIGNAL(import(DesignerTrajectoryPtr)),
+ trajectoryControllerPtr.get(),
+ SLOT(import(DesignerTrajectoryPtr)));
//=============================================================================================
//MementoController Signals
- QObject::connect(mementoControllerPtr.get(), SIGNAL(undo()), trajectoryControllerPtr.get(), SLOT(undo()));
- QObject::connect(mementoControllerPtr.get(), SIGNAL(redo()), trajectoryControllerPtr.get(), SLOT(redo()));
+ QObject::connect(
+ mementoControllerPtr.get(), SIGNAL(undo()), trajectoryControllerPtr.get(), SLOT(undo()));
+ QObject::connect(
+ mementoControllerPtr.get(), SIGNAL(redo()), trajectoryControllerPtr.get(), SLOT(redo()));
//=============================================================================================
//RobotVisualizationController Signals
- QObject::connect(robotVisualizationControllerPtr.get(), SIGNAL(manipulatorChanged(Eigen::Matrix4f)),
- tcpInformationControllerPtr.get(), SLOT(setDesiredPose(Eigen::Matrix4f)));
- QObject::connect(robotVisualizationControllerPtr.get(), SIGNAL(tcpPoseChanged(Eigen::Matrix4f)),
- tcpInformationControllerPtr.get(), SLOT(setCurrentPose(Eigen::Matrix4f)));
- QObject::connect(robotVisualizationControllerPtr.get(), SIGNAL(poseReachable(bool)),
- tcpInformationControllerPtr.get(), SLOT(setReachable(bool)));
-
- QObject::connect(robotVisualizationControllerPtr.get(), SIGNAL(wayPointSelected(int)),
- waypointControllerPtr.get(), SLOT(updateSelectedWaypoint(int)));
-
- QObject::connect(robotVisualizationControllerPtr.get(), SIGNAL(trajectoryPlayerNotPlaying(bool)),
- waypointControllerPtr.get(), SLOT(enableDeleteButton(bool)));
- QObject::connect(robotVisualizationControllerPtr.get(), SIGNAL(trajectoryPlayerNotPlaying(bool)),
- waypointControllerPtr.get(), SLOT(enableAddButton(bool)));
- QObject::connect(robotVisualizationControllerPtr.get(), SIGNAL(trajectoryPlayerNotPlaying(bool)),
- waypointControllerPtr.get(), SLOT(enableWaypointListLineEdit(bool)));
-
- QObject::connect(robotVisualizationControllerPtr.get(), SIGNAL(trajectoryPlayerNotPlaying(bool)),
- waypointControllerPtr.get(), SLOT(enableAddButton(bool)));
-
- QObject::connect(robotVisualizationControllerPtr.get(), SIGNAL(trajectoryPlayerNotPlaying(bool)),
- toolBarControllerPtr.get(), SLOT(enableAddButton(bool)));
- QObject::connect(robotVisualizationControllerPtr.get(), SIGNAL(trajectoryPlayerNotPlaying(bool)),
- toolBarControllerPtr.get(), SLOT(enableDeleteChangeButton(bool)));
- QObject::connect(robotVisualizationControllerPtr.get(), SIGNAL(trajectoryPlayerNotPlaying(bool)),
- toolBarControllerPtr.get(), SLOT(enablePreviewAllButton(bool)));
- QObject::connect(robotVisualizationControllerPtr.get(), SIGNAL(trajectoryPlayerNotPlaying(bool)),
- toolBarControllerPtr.get(), SLOT(enablePreviewButton(bool)));
- QObject::connect(robotVisualizationControllerPtr.get(), SIGNAL(trajectoryPlayerPlaying(bool)),
- toolBarControllerPtr.get(), SLOT(enableStopButton(bool)));
-
- QObject::connect(robotVisualizationControllerPtr.get(), SIGNAL(trajectoryPlayerNotPlaying(bool)),
- settingControllerPtr.get(), SLOT(enableSelectRobotButton(bool)));
- QObject::connect(robotVisualizationControllerPtr.get(), SIGNAL(trajectoryPlayerNotPlaying(bool)),
- settingControllerPtr.get(), SLOT(enableExportButtons(bool)));
- QObject::connect(robotVisualizationControllerPtr.get(), SIGNAL(trajectoryPlayerNotPlaying(bool)),
- settingControllerPtr.get(), SLOT(enableImportTCPCollision(bool)));
-
- QObject::connect(robotVisualizationControllerPtr.get(), SIGNAL(trajectoryPlayerNotPlaying(bool)),
- mementoControllerPtr.get(), SLOT(enableRedoButtonVisualization(bool)));
- QObject::connect(robotVisualizationControllerPtr.get(), SIGNAL(trajectoryPlayerNotPlaying(bool)),
- mementoControllerPtr.get(), SLOT(enableUndoButton(bool)));
-
- QObject::connect(robotVisualizationControllerPtr.get(), SIGNAL(trajectoryPlayerNotPlaying(bool)),
- shortcutControllerPtr.get(), SLOT(enableAddShortcut(bool)));
- QObject::connect(robotVisualizationControllerPtr.get(), SIGNAL(trajectoryPlayerNotPlaying(bool)),
- shortcutControllerPtr.get(), SLOT(enableDeleteChangeShortcut(bool)));
- QObject::connect(robotVisualizationControllerPtr.get(), SIGNAL(trajectoryPlayerNotPlaying(bool)),
- shortcutControllerPtr.get(), SLOT(enablePreviewAllShortcut(bool)));
- QObject::connect(robotVisualizationControllerPtr.get(), SIGNAL(trajectoryPlayerNotPlaying(bool)),
- shortcutControllerPtr.get(), SLOT(enablePreviewShortcut(bool)));
- QObject::connect(robotVisualizationControllerPtr.get(), SIGNAL(trajectoryPlayerNotPlaying(bool)),
- shortcutControllerPtr.get(), SLOT(enableRedoShortcut(bool)));
- QObject::connect(robotVisualizationControllerPtr.get(), SIGNAL(trajectoryPlayerNotPlaying(bool)),
- shortcutControllerPtr.get(), SLOT(enableUndoShortcut(bool)));
- QObject::connect(robotVisualizationControllerPtr.get(), SIGNAL(trajectoryPlayerPlaying(bool)),
- shortcutControllerPtr.get(), SLOT(enableStopPreviewShortcut(bool)));
-
- QObject::connect(robotVisualizationControllerPtr.get(), SIGNAL(trajectoryPlayerNotPlaying(bool)),
- tcpSelectionControllerPtr.get(), SLOT(enableSelectedTCP(bool)));
-
- QObject::connect(robotVisualizationControllerPtr.get(), SIGNAL(trajectoryPlayerNotPlaying(bool)),
- transitionControllerPtr.get(), SLOT(enableAll(bool)));
+ QObject::connect(robotVisualizationControllerPtr.get(),
+ SIGNAL(manipulatorChanged(Eigen::Matrix4f)),
+ tcpInformationControllerPtr.get(),
+ SLOT(setDesiredPose(Eigen::Matrix4f)));
+ QObject::connect(robotVisualizationControllerPtr.get(),
+ SIGNAL(tcpPoseChanged(Eigen::Matrix4f)),
+ tcpInformationControllerPtr.get(),
+ SLOT(setCurrentPose(Eigen::Matrix4f)));
+ QObject::connect(robotVisualizationControllerPtr.get(),
+ SIGNAL(poseReachable(bool)),
+ tcpInformationControllerPtr.get(),
+ SLOT(setReachable(bool)));
+
+ QObject::connect(robotVisualizationControllerPtr.get(),
+ SIGNAL(wayPointSelected(int)),
+ waypointControllerPtr.get(),
+ SLOT(updateSelectedWaypoint(int)));
+
+ QObject::connect(robotVisualizationControllerPtr.get(),
+ SIGNAL(trajectoryPlayerNotPlaying(bool)),
+ waypointControllerPtr.get(),
+ SLOT(enableDeleteButton(bool)));
+ QObject::connect(robotVisualizationControllerPtr.get(),
+ SIGNAL(trajectoryPlayerNotPlaying(bool)),
+ waypointControllerPtr.get(),
+ SLOT(enableAddButton(bool)));
+ QObject::connect(robotVisualizationControllerPtr.get(),
+ SIGNAL(trajectoryPlayerNotPlaying(bool)),
+ waypointControllerPtr.get(),
+ SLOT(enableWaypointListLineEdit(bool)));
+
+ QObject::connect(robotVisualizationControllerPtr.get(),
+ SIGNAL(trajectoryPlayerNotPlaying(bool)),
+ waypointControllerPtr.get(),
+ SLOT(enableAddButton(bool)));
+
+ QObject::connect(robotVisualizationControllerPtr.get(),
+ SIGNAL(trajectoryPlayerNotPlaying(bool)),
+ toolBarControllerPtr.get(),
+ SLOT(enableAddButton(bool)));
+ QObject::connect(robotVisualizationControllerPtr.get(),
+ SIGNAL(trajectoryPlayerNotPlaying(bool)),
+ toolBarControllerPtr.get(),
+ SLOT(enableDeleteChangeButton(bool)));
+ QObject::connect(robotVisualizationControllerPtr.get(),
+ SIGNAL(trajectoryPlayerNotPlaying(bool)),
+ toolBarControllerPtr.get(),
+ SLOT(enablePreviewAllButton(bool)));
+ QObject::connect(robotVisualizationControllerPtr.get(),
+ SIGNAL(trajectoryPlayerNotPlaying(bool)),
+ toolBarControllerPtr.get(),
+ SLOT(enablePreviewButton(bool)));
+ QObject::connect(robotVisualizationControllerPtr.get(),
+ SIGNAL(trajectoryPlayerPlaying(bool)),
+ toolBarControllerPtr.get(),
+ SLOT(enableStopButton(bool)));
+
+ QObject::connect(robotVisualizationControllerPtr.get(),
+ SIGNAL(trajectoryPlayerNotPlaying(bool)),
+ settingControllerPtr.get(),
+ SLOT(enableSelectRobotButton(bool)));
+ QObject::connect(robotVisualizationControllerPtr.get(),
+ SIGNAL(trajectoryPlayerNotPlaying(bool)),
+ settingControllerPtr.get(),
+ SLOT(enableExportButtons(bool)));
+ QObject::connect(robotVisualizationControllerPtr.get(),
+ SIGNAL(trajectoryPlayerNotPlaying(bool)),
+ settingControllerPtr.get(),
+ SLOT(enableImportTCPCollision(bool)));
+
+ QObject::connect(robotVisualizationControllerPtr.get(),
+ SIGNAL(trajectoryPlayerNotPlaying(bool)),
+ mementoControllerPtr.get(),
+ SLOT(enableRedoButtonVisualization(bool)));
+ QObject::connect(robotVisualizationControllerPtr.get(),
+ SIGNAL(trajectoryPlayerNotPlaying(bool)),
+ mementoControllerPtr.get(),
+ SLOT(enableUndoButton(bool)));
+
+ QObject::connect(robotVisualizationControllerPtr.get(),
+ SIGNAL(trajectoryPlayerNotPlaying(bool)),
+ shortcutControllerPtr.get(),
+ SLOT(enableAddShortcut(bool)));
+ QObject::connect(robotVisualizationControllerPtr.get(),
+ SIGNAL(trajectoryPlayerNotPlaying(bool)),
+ shortcutControllerPtr.get(),
+ SLOT(enableDeleteChangeShortcut(bool)));
+ QObject::connect(robotVisualizationControllerPtr.get(),
+ SIGNAL(trajectoryPlayerNotPlaying(bool)),
+ shortcutControllerPtr.get(),
+ SLOT(enablePreviewAllShortcut(bool)));
+ QObject::connect(robotVisualizationControllerPtr.get(),
+ SIGNAL(trajectoryPlayerNotPlaying(bool)),
+ shortcutControllerPtr.get(),
+ SLOT(enablePreviewShortcut(bool)));
+ QObject::connect(robotVisualizationControllerPtr.get(),
+ SIGNAL(trajectoryPlayerNotPlaying(bool)),
+ shortcutControllerPtr.get(),
+ SLOT(enableRedoShortcut(bool)));
+ QObject::connect(robotVisualizationControllerPtr.get(),
+ SIGNAL(trajectoryPlayerNotPlaying(bool)),
+ shortcutControllerPtr.get(),
+ SLOT(enableUndoShortcut(bool)));
+ QObject::connect(robotVisualizationControllerPtr.get(),
+ SIGNAL(trajectoryPlayerPlaying(bool)),
+ shortcutControllerPtr.get(),
+ SLOT(enableStopPreviewShortcut(bool)));
+
+ QObject::connect(robotVisualizationControllerPtr.get(),
+ SIGNAL(trajectoryPlayerNotPlaying(bool)),
+ tcpSelectionControllerPtr.get(),
+ SLOT(enableSelectedTCP(bool)));
+
+ QObject::connect(robotVisualizationControllerPtr.get(),
+ SIGNAL(trajectoryPlayerNotPlaying(bool)),
+ transitionControllerPtr.get(),
+ SLOT(enableAll(bool)));
//=============================================================================================
//SettingController Signals
- QObject::connect(settingControllerPtr.get(), SIGNAL(changedTCP(QString)),
- trajectoryControllerPtr.get(), SLOT(updateTCP(QString)));
- QObject::connect(settingControllerPtr.get(), SIGNAL(setActiveColModelName(QString)),
- trajectoryControllerPtr.get(), SLOT(setActiveColModelName(QString)));
- QObject::connect(settingControllerPtr.get(), SIGNAL(setBodyColModelsNames(QStringList)),
- trajectoryControllerPtr.get(), SLOT(setBodyColModelsNames(QStringList)));
-
- QObject::connect(settingControllerPtr.get(), SIGNAL(openRobotSelection()),
- environmentControllerPtr.get(), SLOT(openRobotFileDialog()));
-
- QObject::connect(settingControllerPtr.get(), SIGNAL(openShortcut()),
- shortcutControllerPtr.get(), SLOT(open()));
-
- QObject::connect(settingControllerPtr.get(), SIGNAL(convertToMMM()),
- exportDialogControllerPtr.get(), SLOT(exportMMM()));
-
- QObject::connect(settingControllerPtr.get(), SIGNAL(openImport()),
- importDialogControllerPtr.get(), SLOT(open()));
+ QObject::connect(settingControllerPtr.get(),
+ SIGNAL(changedTCP(QString)),
+ trajectoryControllerPtr.get(),
+ SLOT(updateTCP(QString)));
+ QObject::connect(settingControllerPtr.get(),
+ SIGNAL(setActiveColModelName(QString)),
+ trajectoryControllerPtr.get(),
+ SLOT(setActiveColModelName(QString)));
+ QObject::connect(settingControllerPtr.get(),
+ SIGNAL(setBodyColModelsNames(QStringList)),
+ trajectoryControllerPtr.get(),
+ SLOT(setBodyColModelsNames(QStringList)));
+
+ QObject::connect(settingControllerPtr.get(),
+ SIGNAL(openRobotSelection()),
+ environmentControllerPtr.get(),
+ SLOT(openRobotFileDialog()));
+
+ QObject::connect(settingControllerPtr.get(),
+ SIGNAL(openShortcut()),
+ shortcutControllerPtr.get(),
+ SLOT(open()));
+
+ QObject::connect(settingControllerPtr.get(),
+ SIGNAL(convertToMMM()),
+ exportDialogControllerPtr.get(),
+ SLOT(exportMMM()));
+
+ QObject::connect(settingControllerPtr.get(),
+ SIGNAL(openImport()),
+ importDialogControllerPtr.get(),
+ SLOT(open()));
//=============================================================================================
//ShortcutController Signals
- QObject::connect(shortcutControllerPtr.get(), SIGNAL(addedWaypoint(int, bool)),
- trajectoryControllerPtr.get(), SLOT(addWaypoint(int, bool)));
- QObject::connect(shortcutControllerPtr.get(), SIGNAL(deletedWaypoint(int)),
- trajectoryControllerPtr.get(), SLOT(deleteWaypoint(int)));
- QObject::connect(shortcutControllerPtr.get(), SIGNAL(changeWaypoint(int)),
- trajectoryControllerPtr.get(), SLOT(updateWaypoint(int)));
- QObject::connect(shortcutControllerPtr.get(), SIGNAL(undo()),
- trajectoryControllerPtr.get(), SLOT(undo()));
- QObject::connect(shortcutControllerPtr.get(), SIGNAL(redo()),
- trajectoryControllerPtr.get(), SLOT(redo()));
- QObject::connect(shortcutControllerPtr.get(), SIGNAL(playPreviewAllSignal()),
- trajectoryControllerPtr.get(), SLOT(playTrajectories()));
-
- QObject::connect(shortcutControllerPtr.get(), SIGNAL(playPreviewSignal()),
- robotVisualizationControllerPtr.get(), SLOT(playTrajectory()));
- QObject::connect(shortcutControllerPtr.get(), SIGNAL(changedPerspective(int)),
- robotVisualizationControllerPtr.get(), SLOT(setCamera(int)));
-
- QObject::connect(shortcutControllerPtr.get(), SIGNAL(addedWaypointPressed(bool)),
- robotVisualizationControllerPtr.get(), SLOT(enableLeftMouse(bool)));
- QObject::connect(shortcutControllerPtr.get(), SIGNAL(stopPreviewSignal()),
- robotVisualizationControllerPtr.get(), SLOT(trajectoryPlayerStopped()));
+ QObject::connect(shortcutControllerPtr.get(),
+ SIGNAL(addedWaypoint(int, bool)),
+ trajectoryControllerPtr.get(),
+ SLOT(addWaypoint(int, bool)));
+ QObject::connect(shortcutControllerPtr.get(),
+ SIGNAL(deletedWaypoint(int)),
+ trajectoryControllerPtr.get(),
+ SLOT(deleteWaypoint(int)));
+ QObject::connect(shortcutControllerPtr.get(),
+ SIGNAL(changeWaypoint(int)),
+ trajectoryControllerPtr.get(),
+ SLOT(updateWaypoint(int)));
+ QObject::connect(
+ shortcutControllerPtr.get(), SIGNAL(undo()), trajectoryControllerPtr.get(), SLOT(undo()));
+ QObject::connect(
+ shortcutControllerPtr.get(), SIGNAL(redo()), trajectoryControllerPtr.get(), SLOT(redo()));
+ QObject::connect(shortcutControllerPtr.get(),
+ SIGNAL(playPreviewAllSignal()),
+ trajectoryControllerPtr.get(),
+ SLOT(playTrajectories()));
+
+ QObject::connect(shortcutControllerPtr.get(),
+ SIGNAL(playPreviewSignal()),
+ robotVisualizationControllerPtr.get(),
+ SLOT(playTrajectory()));
+ QObject::connect(shortcutControllerPtr.get(),
+ SIGNAL(changedPerspective(int)),
+ robotVisualizationControllerPtr.get(),
+ SLOT(setCamera(int)));
+
+ QObject::connect(shortcutControllerPtr.get(),
+ SIGNAL(addedWaypointPressed(bool)),
+ robotVisualizationControllerPtr.get(),
+ SLOT(enableLeftMouse(bool)));
+ QObject::connect(shortcutControllerPtr.get(),
+ SIGNAL(stopPreviewSignal()),
+ robotVisualizationControllerPtr.get(),
+ SLOT(trajectoryPlayerStopped()));
//=============================================================================================
//TCPSelectionController Signals
- QObject::connect(tcpSelectionControllerPtr.get(), SIGNAL(changedSelectedTCP(QString)),
- trajectoryControllerPtr.get(), SLOT(updateTCP(QString)));
- QObject::connect(tcpSelectionControllerPtr.get(), SIGNAL(changedSelectedTCP(QString)),
- settingControllerPtr.get(), SLOT(selectTCP(QString)));
+ QObject::connect(tcpSelectionControllerPtr.get(),
+ SIGNAL(changedSelectedTCP(QString)),
+ trajectoryControllerPtr.get(),
+ SLOT(updateTCP(QString)));
+ QObject::connect(tcpSelectionControllerPtr.get(),
+ SIGNAL(changedSelectedTCP(QString)),
+ settingControllerPtr.get(),
+ SLOT(selectTCP(QString)));
//=============================================================================================
//ToolBarController Signals
- QObject::connect(toolBarControllerPtr.get(), SIGNAL(addedWaypoint(int, bool)),
- trajectoryControllerPtr.get(), SLOT(addWaypoint(int, bool)));
- QObject::connect(toolBarControllerPtr.get(), SIGNAL(deletedWaypoint(int)),
- trajectoryControllerPtr.get(), SLOT(deleteWaypoint(int)));
- QObject::connect(toolBarControllerPtr.get(), SIGNAL(changeWaypoint(int)),
- trajectoryControllerPtr.get(), SLOT(updateWaypoint(int)));
-
- QObject::connect(toolBarControllerPtr.get(), SIGNAL(notifyAllPreview()),
- trajectoryControllerPtr.get(), SLOT(playTrajectories()));
-
- QObject::connect(toolBarControllerPtr.get(), SIGNAL(notifyPreview()),
- robotVisualizationControllerPtr.get(), SLOT(playTrajectory()));
- QObject::connect(toolBarControllerPtr.get(), SIGNAL(notifyStopPreview()),
- robotVisualizationControllerPtr.get(), SLOT(trajectoryPlayerStopped()));
+ QObject::connect(toolBarControllerPtr.get(),
+ SIGNAL(addedWaypoint(int, bool)),
+ trajectoryControllerPtr.get(),
+ SLOT(addWaypoint(int, bool)));
+ QObject::connect(toolBarControllerPtr.get(),
+ SIGNAL(deletedWaypoint(int)),
+ trajectoryControllerPtr.get(),
+ SLOT(deleteWaypoint(int)));
+ QObject::connect(toolBarControllerPtr.get(),
+ SIGNAL(changeWaypoint(int)),
+ trajectoryControllerPtr.get(),
+ SLOT(updateWaypoint(int)));
+
+ QObject::connect(toolBarControllerPtr.get(),
+ SIGNAL(notifyAllPreview()),
+ trajectoryControllerPtr.get(),
+ SLOT(playTrajectories()));
+
+ QObject::connect(toolBarControllerPtr.get(),
+ SIGNAL(notifyPreview()),
+ robotVisualizationControllerPtr.get(),
+ SLOT(playTrajectory()));
+ QObject::connect(toolBarControllerPtr.get(),
+ SIGNAL(notifyStopPreview()),
+ robotVisualizationControllerPtr.get(),
+ SLOT(trajectoryPlayerStopped()));
//=============================================================================================
//TrajectroyController Signals
- QObject::connect(trajectoryControllerPtr.get(), SIGNAL(addWaypointGui(int, std::vector<double>, int, bool)),
- waypointControllerPtr.get(), SLOT(addWaypoint(int, std::vector<double>, int, bool)));
- QObject::connect(trajectoryControllerPtr.get(), SIGNAL(changeWaypointGui(int, std::vector<double>, int, bool)),
- waypointControllerPtr.get(), SLOT(setWaypointData(int, std::vector<double>, int, bool)));
- QObject::connect(trajectoryControllerPtr.get(), SIGNAL(removeWaypointGui(int)),
- waypointControllerPtr.get(), SLOT(removeWaypoint(int)));
- QObject::connect(trajectoryControllerPtr.get(), SIGNAL(removeTransitionWaypointGui()),
- waypointControllerPtr.get(), SLOT(clearWaypointList()));
- QObject::connect(trajectoryControllerPtr.get(), SIGNAL(enableDeleteChange(bool)),
- waypointControllerPtr.get(), SLOT(enableDeleteButton(bool)));
- QObject::connect(trajectoryControllerPtr.get(), SIGNAL(enableAdd(bool)),
- waypointControllerPtr.get(), SLOT(enableAddButton(bool)));
-
- QObject::connect(trajectoryControllerPtr.get(), SIGNAL(addTransitionGui(int, double, double, int)),
- transitionControllerPtr.get(), SLOT(addTransition(int, double, double, int)));
- QObject::connect(trajectoryControllerPtr.get(), SIGNAL(changeTransitionGui(int, double, double, int)),
- transitionControllerPtr.get(), SLOT(setTransitionData(int, double, double, int)));
- QObject::connect(trajectoryControllerPtr.get(), SIGNAL(removeTransitionGui(int)),
- transitionControllerPtr.get(), SLOT(removeTransition(int)));
- QObject::connect(trajectoryControllerPtr.get(), SIGNAL(removeTransitionWaypointGui()),
- transitionControllerPtr.get(), SLOT(clearTransitionList()));
-
- QObject::connect(trajectoryControllerPtr.get(), SIGNAL(newTrajectory(QString)),
- tcpSelectionControllerPtr.get(), SLOT(addTrajectory(QString)));
- QObject::connect(trajectoryControllerPtr.get(), SIGNAL(updateSelectedTCP(QString)),
- tcpSelectionControllerPtr.get(), SLOT(updateSelectedTCP(QString)));
- QObject::connect(trajectoryControllerPtr.get(), SIGNAL(removeTrajectory(QString)),
- tcpSelectionControllerPtr.get(), SLOT(removeTrajectory(QString)));
- QObject::connect(trajectoryControllerPtr.get(), SIGNAL(removeAllTrajectories()),
- tcpSelectionControllerPtr.get(), SLOT(removeAllTrajectories()));
-
- QObject::connect(trajectoryControllerPtr.get(), SIGNAL(updateSelectedTCP(QString)),
- settingControllerPtr.get(), SLOT(selectTCP(QString)));
- QObject::connect(trajectoryControllerPtr.get(), SIGNAL(enableIKSolutionButton(bool)),
- settingControllerPtr.get(), SLOT(enableIKSolutionButton(bool)));
- QObject::connect(trajectoryControllerPtr.get(), SIGNAL(enableExportButtons(bool)),
- settingControllerPtr.get(), SLOT(enableExportButtons(bool)));
-
- QObject::connect(trajectoryControllerPtr.get(), SIGNAL(enableRedo(bool)),
- mementoControllerPtr.get(), SLOT(enableRedoButton(bool)));
- QObject::connect(trajectoryControllerPtr.get(), SIGNAL(enableUndo(bool)),
- mementoControllerPtr.get(), SLOT(enableUndoButton(bool)));
-
- QObject::connect(trajectoryControllerPtr.get(), SIGNAL(exportTrajectory(std::vector<DesignerTrajectoryPtr>)),
- exportDialogControllerPtr.get(), SLOT(exportTrajectory(std::vector<DesignerTrajectoryPtr>)));
-
- QObject::connect(trajectoryControllerPtr.get(), SIGNAL(showTrajectory(DesignerTrajectoryPtr)),
- robotVisualizationControllerPtr.get(), SLOT(updateViews(DesignerTrajectoryPtr)));
- QObject::connect(trajectoryControllerPtr.get(), SIGNAL(removeTransitionWaypointGui()),
- robotVisualizationControllerPtr.get(), SLOT(clearView()));
- QObject::connect(trajectoryControllerPtr.get(), SIGNAL(cartesianSelectionChanged(VirtualRobot::IKSolver::CartesianSelection())),
- robotVisualizationControllerPtr.get(), SLOT(cartesianSelectionChanged(VirtualRobot::IKSolver::CartesianSelection())));
- QObject::connect(trajectoryControllerPtr.get(), SIGNAL(playTrajectories(std::vector<DesignerTrajectoryPtr>)),
- robotVisualizationControllerPtr.get(), SLOT(playTrajectories(std::vector<DesignerTrajectoryPtr>)));
- QObject::connect(trajectoryControllerPtr.get(), SIGNAL(rnsChanged(VirtualRobot::RobotNodeSetPtr)),
- robotVisualizationControllerPtr.get(), SLOT(kinematicChainChanged(VirtualRobot::RobotNodeSetPtr)));
-
- QObject::connect(trajectoryControllerPtr.get(), SIGNAL(enableDeleteChange(bool)),
- toolBarControllerPtr.get(), SLOT(enableDeleteChangeButton(bool)));
- QObject::connect(trajectoryControllerPtr.get(), SIGNAL(enableAdd(bool)),
- toolBarControllerPtr.get(), SLOT(enableAddButton(bool)));
- QObject::connect(trajectoryControllerPtr.get(), SIGNAL(enablePreview(bool)),
- toolBarControllerPtr.get(), SLOT(enablePreviewButton(bool)));
- QObject::connect(trajectoryControllerPtr.get(), SIGNAL(enablePreviewAll(bool)),
- toolBarControllerPtr.get(), SLOT(enablePreviewAllButton(bool)));
-
- QObject::connect(trajectoryControllerPtr.get(), SIGNAL(enableDeleteChange(bool)),
- shortcutControllerPtr.get(), SLOT(enableDeleteChangeShortcut(bool)));
- QObject::connect(trajectoryControllerPtr.get(), SIGNAL(enableAdd(bool)),
- shortcutControllerPtr.get(), SLOT(enableAddShortcut(bool)));
- QObject::connect(trajectoryControllerPtr.get(), SIGNAL(enablePreview(bool)),
- shortcutControllerPtr.get(), SLOT(enablePreviewShortcut(bool)));
- QObject::connect(trajectoryControllerPtr.get(), SIGNAL(enablePreviewAll(bool)),
- shortcutControllerPtr.get(), SLOT(enablePreviewAllShortcut(bool)));
- QObject::connect(trajectoryControllerPtr.get(), SIGNAL(enableRedo(bool)),
- shortcutControllerPtr.get(), SLOT(enableRedoShortcut(bool)));
- QObject::connect(trajectoryControllerPtr.get(), SIGNAL(enableUndo(bool)),
- shortcutControllerPtr.get(), SLOT(enableUnoShortcut(bool)));
+ QObject::connect(trajectoryControllerPtr.get(),
+ SIGNAL(addWaypointGui(int, std::vector<double>, int, bool)),
+ waypointControllerPtr.get(),
+ SLOT(addWaypoint(int, std::vector<double>, int, bool)));
+ QObject::connect(trajectoryControllerPtr.get(),
+ SIGNAL(changeWaypointGui(int, std::vector<double>, int, bool)),
+ waypointControllerPtr.get(),
+ SLOT(setWaypointData(int, std::vector<double>, int, bool)));
+ QObject::connect(trajectoryControllerPtr.get(),
+ SIGNAL(removeWaypointGui(int)),
+ waypointControllerPtr.get(),
+ SLOT(removeWaypoint(int)));
+ QObject::connect(trajectoryControllerPtr.get(),
+ SIGNAL(removeTransitionWaypointGui()),
+ waypointControllerPtr.get(),
+ SLOT(clearWaypointList()));
+ QObject::connect(trajectoryControllerPtr.get(),
+ SIGNAL(enableDeleteChange(bool)),
+ waypointControllerPtr.get(),
+ SLOT(enableDeleteButton(bool)));
+ QObject::connect(trajectoryControllerPtr.get(),
+ SIGNAL(enableAdd(bool)),
+ waypointControllerPtr.get(),
+ SLOT(enableAddButton(bool)));
+
+ QObject::connect(trajectoryControllerPtr.get(),
+ SIGNAL(addTransitionGui(int, double, double, int)),
+ transitionControllerPtr.get(),
+ SLOT(addTransition(int, double, double, int)));
+ QObject::connect(trajectoryControllerPtr.get(),
+ SIGNAL(changeTransitionGui(int, double, double, int)),
+ transitionControllerPtr.get(),
+ SLOT(setTransitionData(int, double, double, int)));
+ QObject::connect(trajectoryControllerPtr.get(),
+ SIGNAL(removeTransitionGui(int)),
+ transitionControllerPtr.get(),
+ SLOT(removeTransition(int)));
+ QObject::connect(trajectoryControllerPtr.get(),
+ SIGNAL(removeTransitionWaypointGui()),
+ transitionControllerPtr.get(),
+ SLOT(clearTransitionList()));
+
+ QObject::connect(trajectoryControllerPtr.get(),
+ SIGNAL(newTrajectory(QString)),
+ tcpSelectionControllerPtr.get(),
+ SLOT(addTrajectory(QString)));
+ QObject::connect(trajectoryControllerPtr.get(),
+ SIGNAL(updateSelectedTCP(QString)),
+ tcpSelectionControllerPtr.get(),
+ SLOT(updateSelectedTCP(QString)));
+ QObject::connect(trajectoryControllerPtr.get(),
+ SIGNAL(removeTrajectory(QString)),
+ tcpSelectionControllerPtr.get(),
+ SLOT(removeTrajectory(QString)));
+ QObject::connect(trajectoryControllerPtr.get(),
+ SIGNAL(removeAllTrajectories()),
+ tcpSelectionControllerPtr.get(),
+ SLOT(removeAllTrajectories()));
+
+ QObject::connect(trajectoryControllerPtr.get(),
+ SIGNAL(updateSelectedTCP(QString)),
+ settingControllerPtr.get(),
+ SLOT(selectTCP(QString)));
+ QObject::connect(trajectoryControllerPtr.get(),
+ SIGNAL(enableIKSolutionButton(bool)),
+ settingControllerPtr.get(),
+ SLOT(enableIKSolutionButton(bool)));
+ QObject::connect(trajectoryControllerPtr.get(),
+ SIGNAL(enableExportButtons(bool)),
+ settingControllerPtr.get(),
+ SLOT(enableExportButtons(bool)));
+
+ QObject::connect(trajectoryControllerPtr.get(),
+ SIGNAL(enableRedo(bool)),
+ mementoControllerPtr.get(),
+ SLOT(enableRedoButton(bool)));
+ QObject::connect(trajectoryControllerPtr.get(),
+ SIGNAL(enableUndo(bool)),
+ mementoControllerPtr.get(),
+ SLOT(enableUndoButton(bool)));
+
+ QObject::connect(trajectoryControllerPtr.get(),
+ SIGNAL(exportTrajectory(std::vector<DesignerTrajectoryPtr>)),
+ exportDialogControllerPtr.get(),
+ SLOT(exportTrajectory(std::vector<DesignerTrajectoryPtr>)));
+
+ QObject::connect(trajectoryControllerPtr.get(),
+ SIGNAL(showTrajectory(DesignerTrajectoryPtr)),
+ robotVisualizationControllerPtr.get(),
+ SLOT(updateViews(DesignerTrajectoryPtr)));
+ QObject::connect(trajectoryControllerPtr.get(),
+ SIGNAL(removeTransitionWaypointGui()),
+ robotVisualizationControllerPtr.get(),
+ SLOT(clearView()));
+ QObject::connect(
+ trajectoryControllerPtr.get(),
+ SIGNAL(cartesianSelectionChanged(VirtualRobot::IKSolver::CartesianSelection())),
+ robotVisualizationControllerPtr.get(),
+ SLOT(cartesianSelectionChanged(VirtualRobot::IKSolver::CartesianSelection())));
+ QObject::connect(trajectoryControllerPtr.get(),
+ SIGNAL(playTrajectories(std::vector<DesignerTrajectoryPtr>)),
+ robotVisualizationControllerPtr.get(),
+ SLOT(playTrajectories(std::vector<DesignerTrajectoryPtr>)));
+ QObject::connect(trajectoryControllerPtr.get(),
+ SIGNAL(rnsChanged(VirtualRobot::RobotNodeSetPtr)),
+ robotVisualizationControllerPtr.get(),
+ SLOT(kinematicChainChanged(VirtualRobot::RobotNodeSetPtr)));
+
+ QObject::connect(trajectoryControllerPtr.get(),
+ SIGNAL(enableDeleteChange(bool)),
+ toolBarControllerPtr.get(),
+ SLOT(enableDeleteChangeButton(bool)));
+ QObject::connect(trajectoryControllerPtr.get(),
+ SIGNAL(enableAdd(bool)),
+ toolBarControllerPtr.get(),
+ SLOT(enableAddButton(bool)));
+ QObject::connect(trajectoryControllerPtr.get(),
+ SIGNAL(enablePreview(bool)),
+ toolBarControllerPtr.get(),
+ SLOT(enablePreviewButton(bool)));
+ QObject::connect(trajectoryControllerPtr.get(),
+ SIGNAL(enablePreviewAll(bool)),
+ toolBarControllerPtr.get(),
+ SLOT(enablePreviewAllButton(bool)));
+
+ QObject::connect(trajectoryControllerPtr.get(),
+ SIGNAL(enableDeleteChange(bool)),
+ shortcutControllerPtr.get(),
+ SLOT(enableDeleteChangeShortcut(bool)));
+ QObject::connect(trajectoryControllerPtr.get(),
+ SIGNAL(enableAdd(bool)),
+ shortcutControllerPtr.get(),
+ SLOT(enableAddShortcut(bool)));
+ QObject::connect(trajectoryControllerPtr.get(),
+ SIGNAL(enablePreview(bool)),
+ shortcutControllerPtr.get(),
+ SLOT(enablePreviewShortcut(bool)));
+ QObject::connect(trajectoryControllerPtr.get(),
+ SIGNAL(enablePreviewAll(bool)),
+ shortcutControllerPtr.get(),
+ SLOT(enablePreviewAllShortcut(bool)));
+ QObject::connect(trajectoryControllerPtr.get(),
+ SIGNAL(enableRedo(bool)),
+ shortcutControllerPtr.get(),
+ SLOT(enableRedoShortcut(bool)));
+ QObject::connect(trajectoryControllerPtr.get(),
+ SIGNAL(enableUndo(bool)),
+ shortcutControllerPtr.get(),
+ SLOT(enableUnoShortcut(bool)));
//=============================================================================================
//TransitionController Signals
- QObject::connect(transitionControllerPtr.get(), SIGNAL(changedDuration(int, double)),
- trajectoryControllerPtr.get(), SLOT(updateTransition(int, double)));
- QObject::connect(transitionControllerPtr.get(), SIGNAL(changedInterpolation(int, int)),
- trajectoryControllerPtr.get(), SLOT(updateTransition(int, int)));
+ QObject::connect(transitionControllerPtr.get(),
+ SIGNAL(changedDuration(int, double)),
+ trajectoryControllerPtr.get(),
+ SLOT(updateTransition(int, double)));
+ QObject::connect(transitionControllerPtr.get(),
+ SIGNAL(changedInterpolation(int, int)),
+ trajectoryControllerPtr.get(),
+ SLOT(updateTransition(int, int)));
- QObject::connect(transitionControllerPtr.get(), SIGNAL(selectedTransitionChanged(int)),
- robotVisualizationControllerPtr.get(), SLOT(selectedTransitionChanged(int)));
+ QObject::connect(transitionControllerPtr.get(),
+ SIGNAL(selectedTransitionChanged(int)),
+ robotVisualizationControllerPtr.get(),
+ SLOT(selectedTransitionChanged(int)));
//=============================================================================================
//ViewController Signals
- QObject::connect(viewControllerPtr.get(), SIGNAL(changedPerspective(int)),
- robotVisualizationControllerPtr.get(), SLOT(setCamera(int)));
- QObject::connect(viewControllerPtr.get(), SIGNAL(addView()),
- robotVisualizationControllerPtr.get(), SLOT(addView()));
- QObject::connect(viewControllerPtr.get(), SIGNAL(removeView()),
- robotVisualizationControllerPtr.get(), SLOT(removeView()));
+ QObject::connect(viewControllerPtr.get(),
+ SIGNAL(changedPerspective(int)),
+ robotVisualizationControllerPtr.get(),
+ SLOT(setCamera(int)));
+ QObject::connect(viewControllerPtr.get(),
+ SIGNAL(addView()),
+ robotVisualizationControllerPtr.get(),
+ SLOT(addView()));
+ QObject::connect(viewControllerPtr.get(),
+ SIGNAL(removeView()),
+ robotVisualizationControllerPtr.get(),
+ SLOT(removeView()));
//=============================================================================================
//WaypointController Signals
- QObject::connect(waypointControllerPtr.get(), SIGNAL(changedWaypoint(int, std::vector<double>)),
- trajectoryControllerPtr.get(), SLOT(updateWaypoint(int, std::vector<double>)));
- QObject::connect(waypointControllerPtr.get(), SIGNAL(changedWaypoint(int, int)),
- trajectoryControllerPtr.get(), SLOT(updateWaypoint(int, int)));
- QObject::connect(waypointControllerPtr.get(), SIGNAL(changedWaypoint(int, bool)),
- trajectoryControllerPtr.get(), SLOT(updateWaypoint(int, bool)));
- QObject::connect(waypointControllerPtr.get(), SIGNAL(addedWaypoint(int, bool)),
- trajectoryControllerPtr.get(), SLOT(addWaypoint(int, bool)));
- QObject::connect(waypointControllerPtr.get(), SIGNAL(deletedWaypoint(int)),
- trajectoryControllerPtr.get(), SLOT(deleteWaypoint(int)));
- QObject::connect(waypointControllerPtr.get(), SIGNAL(enableIKSolutionButton(bool)),
- settingControllerPtr.get(), SLOT(enableIKSolutionButton(bool)));
- QObject::connect(waypointControllerPtr.get(), SIGNAL(setCurrentIndex(int)),
- toolBarControllerPtr.get(), SLOT(setCurrentWaypoint(int)));
- QObject::connect(waypointControllerPtr.get(), SIGNAL(setCurrentIndex(int)),
- shortcutControllerPtr.get(), SLOT(setCurrentWaypoint(int)));
-
- QObject::connect(waypointControllerPtr.get(), SIGNAL(setCurrentIndexRobotVisualization(int)),
- robotVisualizationControllerPtr.get(), SLOT(selectedWayPointChanged(int)));
+ QObject::connect(waypointControllerPtr.get(),
+ SIGNAL(changedWaypoint(int, std::vector<double>)),
+ trajectoryControllerPtr.get(),
+ SLOT(updateWaypoint(int, std::vector<double>)));
+ QObject::connect(waypointControllerPtr.get(),
+ SIGNAL(changedWaypoint(int, int)),
+ trajectoryControllerPtr.get(),
+ SLOT(updateWaypoint(int, int)));
+ QObject::connect(waypointControllerPtr.get(),
+ SIGNAL(changedWaypoint(int, bool)),
+ trajectoryControllerPtr.get(),
+ SLOT(updateWaypoint(int, bool)));
+ QObject::connect(waypointControllerPtr.get(),
+ SIGNAL(addedWaypoint(int, bool)),
+ trajectoryControllerPtr.get(),
+ SLOT(addWaypoint(int, bool)));
+ QObject::connect(waypointControllerPtr.get(),
+ SIGNAL(deletedWaypoint(int)),
+ trajectoryControllerPtr.get(),
+ SLOT(deleteWaypoint(int)));
+ QObject::connect(waypointControllerPtr.get(),
+ SIGNAL(enableIKSolutionButton(bool)),
+ settingControllerPtr.get(),
+ SLOT(enableIKSolutionButton(bool)));
+ QObject::connect(waypointControllerPtr.get(),
+ SIGNAL(setCurrentIndex(int)),
+ toolBarControllerPtr.get(),
+ SLOT(setCurrentWaypoint(int)));
+ QObject::connect(waypointControllerPtr.get(),
+ SIGNAL(setCurrentIndex(int)),
+ shortcutControllerPtr.get(),
+ SLOT(setCurrentWaypoint(int)));
+
+ QObject::connect(waypointControllerPtr.get(),
+ SIGNAL(setCurrentIndexRobotVisualization(int)),
+ robotVisualizationControllerPtr.get(),
+ SLOT(selectedWayPointChanged(int)));
//=============================================================================================
//Load Robot
- RobotStateComponentInterfacePrx robotStateComponent = getProxy<RobotStateComponentInterfacePrx>("RobotStateComponent", false, "", false);
+ RobotStateComponentInterfacePrx robotStateComponent =
+ getProxy<RobotStateComponentInterfacePrx>("RobotStateComponent", false, "", false);
if (robotStateComponent)
{
environmentControllerPtr->loadRobotFromProxy(robotStateComponent->getRobotFilename());
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/RobotTrajectoryDesignerGuiPluginWidgetController.h b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/RobotTrajectoryDesignerGuiPluginWidgetController.h
index bd651baac0b7da4942366098eef8177c5f5c3202..9f6c099b8b1c2baa6f78dcedeaf67f8ec4f17903 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/RobotTrajectoryDesignerGuiPluginWidgetController.h
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/RobotTrajectoryDesignerGuiPluginWidgetController.h
@@ -23,7 +23,13 @@
#ifndef _ARMARX_RobotTrajectoryDesigner_RobotTrajectoryDesignerGuiPlugin_WidgetController_H
#define _ARMARX_RobotTrajectoryDesigner_RobotTrajectoryDesignerGuiPlugin_WidgetController_H
+#include <ArmarXCore/core/system/ImportExportComponent.h>
+
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
+
#include <RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/ui_RobotTrajectoryDesignerGuiPluginWidget.h>
+
#include "View/Perspectives.h"
#include "View/SettingTab.h"
#include "View/TCPInformationTab.h"
@@ -31,17 +37,15 @@
#include "View/TransitionTab.h"
#include "View/WaypointTab.h"
-#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
-#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
-
-#include <ArmarXCore/core/system/ImportExportComponent.h>
-
//#include "Controller/CollisionDialogController.h"
//#include "Controller/LanguageController.h"
#include "Controller/MementoController.h"
//#include "Controller/PopUpController.h"
#include "Controller/RobotVisualizationController.h"
//#include "Controller/ScenarioDialogController.h"
+#include "Controller/EnvironmentController.h"
+#include "Controller/ExportDialogController.h"
+#include "Controller/ImportDialogController.h"
#include "Controller/SettingController.h"
#include "Controller/ShortcutController.h"
#include "Controller/TCPInformationController.h"
@@ -51,10 +55,6 @@
#include "Controller/TransitionController.h"
#include "Controller/ViewController.h"
#include "Controller/WaypointController.h"
-#include "Controller/ExportDialogController.h"
-#include "Controller/ImportDialogController.h"
-#include "Controller/EnvironmentController.h"
-
namespace armarx
{
@@ -73,8 +73,7 @@ namespace armarx
* \brief Main controller of the GUI plugin, handling all other subcontrollers.
* Detailed description
*/
- class ARMARXCOMPONENT_IMPORT_EXPORT
- RobotTrajectoryDesignerGuiPluginWidgetController:
+ class ARMARXCOMPONENT_IMPORT_EXPORT RobotTrajectoryDesignerGuiPluginWidgetController :
public armarx::ArmarXComponentWidgetController
{
Q_OBJECT
@@ -104,7 +103,8 @@ namespace armarx
* Returns the Widget name displayed in the ArmarXGui to create an
* instance of this class.
*/
- virtual QString getWidgetName() const override
+ virtual QString
+ getWidgetName() const override
{
return "RobotTrajectoryDesigner";
}
@@ -152,6 +152,6 @@ namespace armarx
VirtualRobot::RobotPtr requestRobot(std::string robotFile);
};
-}
+} // namespace armarx
#endif
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/RobotTrajectoryDesignerTestEnvironment.h b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/RobotTrajectoryDesignerTestEnvironment.h
index 4df9323a361b66c04c23e437c8ab09702250f778..2d72d622b100886586b6a883dbbd208d8065342e 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/RobotTrajectoryDesignerTestEnvironment.h
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/RobotTrajectoryDesignerTestEnvironment.h
@@ -1,14 +1,17 @@
-#include <RobotTrajectoryDesigner/gui-plugins/RobotTrajectoryDesignerGuiPlugin/RobotTrajectoryDesignerGuiPluginGuiPlugin.h>
-#include <ArmarXCore/core/test/IceTestHelper.h>
+#include <VirtualRobot/XML/RobotIO.h>
+
#include <ArmarXCore/core/application/Application.h>
#include <ArmarXCore/core/system/FactoryCollectionBase.h>
+#include <ArmarXCore/core/test/IceTestHelper.h>
-#include <VirtualRobot/XML/RobotIO.h>
+#include <RobotTrajectoryDesigner/gui-plugins/RobotTrajectoryDesignerGuiPlugin/RobotTrajectoryDesignerGuiPluginGuiPlugin.h>
class RobotTrajectoryDesignerTestEnvironment
{
public:
- RobotTrajectoryDesignerTestEnvironment(const std::string& testName, int registryPort = 11220, bool addObjects = true)
+ RobotTrajectoryDesignerTestEnvironment(const std::string& testName,
+ int registryPort = 11220,
+ bool addObjects = true)
{
Ice::PropertiesPtr properties = Ice::createProperties();
armarx::Application::LoadDefaultConfig(properties);
@@ -33,7 +36,8 @@ public:
if (loadSpaces)
{
properties->setProperty("ArmarX.RobotIK.ReachabilitySpacesFolder", _spaceSavePath);
- properties->setProperty("ArmarX.RobotIK.InitialReachabilitySpaces", "testReachabilitySpace.bin");
+ properties->setProperty("ArmarX.RobotIK.InitialReachabilitySpaces",
+ "testReachabilitySpace.bin");
}
_iceTestHelper = new IceTestHelper(registryPort, registryPort + 1);
@@ -44,20 +48,26 @@ public:
if (addObjects)
{
// This is how you create components.
- _robotTrajectoryDesigner = _manager->createComponentAndRun<YourComponent, YourComponentInterfacePrx>("ArmarX", "RobotTrajectoryDesigner");
+ _robotTrajectoryDesigner =
+ _manager->createComponentAndRun<YourComponent, YourComponentInterfacePrx>(
+ "ArmarX", "RobotTrajectoryDesigner");
}
- _robotModel = VirtualRobot::RobotIO::loadRobot(robotFilename, VirtualRobot::RobotIO::eStructure);
+ _robotModel =
+ VirtualRobot::RobotIO::loadRobot(robotFilename, VirtualRobot::RobotIO::eStructure);
}
+
~RobotTrajectoryDesignerTestEnvironment()
{
_manager->shutdown();
}
+
// In your tests, you can access your component through this proxy
RobotTrajectoryDesignerInterfacePrx _robotTrajectoryDesigner;
TestArmarXManagerPtr _manager;
IceTestHelperPtr _iceTestHelper;
VirtualRobot::RobotPtr _robotModel;
-
};
-using RobotTrajectoryDesignerTestEnvironmentPtr = std::shared_ptr<RobotTrajectoryDesignerTestEnvironment>;
+
+using RobotTrajectoryDesignerTestEnvironmentPtr =
+ std::shared_ptr<RobotTrajectoryDesignerTestEnvironment>;
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Tests/KinematicSolverTest.cpp b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Tests/KinematicSolverTest.cpp
index bb885ab592c054f0e1d17758161ff1f7c487a80c..fccd301df978312824688e04a6ec381aa2f4ff42 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Tests/KinematicSolverTest.cpp
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Tests/KinematicSolverTest.cpp
@@ -2,22 +2,20 @@
#define ARMARX_BOOST_TEST
-#include <RobotComponents/Test.h>
-
-#include <RobotAPI/libraries/core/Pose.h>
-#include <RobotAPI/libraries/core/FramedPose.h>
-
#include "../KinematicSolver.h"
#include <VirtualRobot/XML/RobotIO.h>
-#include "../Util/OrientationConversion.h"
+#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
-#include "../Interpolation/LinearInterpolation.h"
+#include <RobotAPI/libraries/core/FramedPose.h>
+#include <RobotAPI/libraries/core/Pose.h>
-#include "../Interpolation/InterpolationSegmentFactory.h"
+#include <RobotComponents/Test.h>
-#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
+#include "../Interpolation/InterpolationSegmentFactory.h"
+#include "../Interpolation/LinearInterpolation.h"
+#include "../Util/OrientationConversion.h"
using namespace armarx;
@@ -36,8 +34,8 @@ public:
PoseBasePtr pose;
};
-PosePkg createPosePkg(float x, float y, float z,
- float qw, float qx, float qy, float qz)
+PosePkg
+createPosePkg(float x, float y, float z, float qw, float qx, float qy, float qz)
{
PosePkg posePkg;
posePkg.pos = Vector3BasePtr(new Vector3(x, y, z));
@@ -47,38 +45,47 @@ PosePkg createPosePkg(float x, float y, float z,
return posePkg;
}
-bool isEqualRounded(PoseBasePtr current, PoseBasePtr other, double delta)
+bool
+isEqualRounded(PoseBasePtr current, PoseBasePtr other, double delta)
{
- if (current->orientation->qw - other->orientation->qw > delta && other->orientation->qw - current->orientation->qw > delta)
+ if (current->orientation->qw - other->orientation->qw > delta &&
+ other->orientation->qw - current->orientation->qw > delta)
{
return false;
}
- if (current->orientation->qx - other->orientation->qx > delta && other->orientation->qx - current->orientation->qx > delta)
+ if (current->orientation->qx - other->orientation->qx > delta &&
+ other->orientation->qx - current->orientation->qx > delta)
{
return false;
}
- if (current->orientation->qy - other->orientation->qy > delta && other->orientation->qy - current->orientation->qy > delta)
+ if (current->orientation->qy - other->orientation->qy > delta &&
+ other->orientation->qy - current->orientation->qy > delta)
{
return false;
}
- if (current->orientation->qz - other->orientation->qz > delta && other->orientation->qz - current->orientation->qz > delta)
+ if (current->orientation->qz - other->orientation->qz > delta &&
+ other->orientation->qz - current->orientation->qz > delta)
{
return false;
}
- if (current->position->x - other->position->x > delta && other->position->x - current->position->x > delta)
+ if (current->position->x - other->position->x > delta &&
+ other->position->x - current->position->x > delta)
{
return false;
}
- if (current->position->y - other->position->y > delta && other->position->y - current->position->y > delta)
+ if (current->position->y - other->position->y > delta &&
+ other->position->y - current->position->y > delta)
{
return false;
}
- if (current->position->z - other->position->z > delta && other->position->z - current->position->z > delta)
+ if (current->position->z - other->position->z > delta &&
+ other->position->z - current->position->z > delta)
{
return false;
}
return true;
}
+
/////////////////////////////////////////////////////////////////////////////////////////////////////////////
/// Tests basic functionality of Kineamtic Solver
/////////////////////////////////////////////////////////////////////////////////////////////////////////////
@@ -112,14 +119,18 @@ BOOST_AUTO_TEST_CASE(singletonTest)
}
std::cout << robot->getRobotNodeSet("TorsoRightArm")->getAllRobotNodes().size();
KinematicSolverPtr ks1 = KinematicSolver::getInstance(NULL, robot);
- ks1->doForwardKinematic(robot->getRobotNodeSet("TorsoRightArm"), {2, 3, 4, 5, 0, 1, 2, 3, 3, 4});
+ ks1->doForwardKinematic(robot->getRobotNodeSet("TorsoRightArm"),
+ {2, 3, 4, 5, 0, 1, 2, 3, 3, 4});
KinematicSolverPtr ks2 = KinematicSolver::getInstance(NULL, NULL);
- ks2->doForwardKinematic(robot->getRobotNodeSet("TorsoRightArm"), {2, 3, 4, 5, 0, 1, 2, 3, 3, 4});
+ ks2->doForwardKinematic(robot->getRobotNodeSet("TorsoRightArm"),
+ {2, 3, 4, 5, 0, 1, 2, 3, 3, 4});
KinematicSolverPtr ks3 = KinematicSolver::getInstance(NULL, robot);
- ks2->doForwardKinematic(robot->getRobotNodeSet("TorsoRightArm"), {2, 3, 4, 5, 0, 1, 2, 3, 3, 4});
+ ks2->doForwardKinematic(robot->getRobotNodeSet("TorsoRightArm"),
+ {2, 3, 4, 5, 0, 1, 2, 3, 3, 4});
KinematicSolver::reset();
//shouldnt crash after reset
- ks1->doForwardKinematic(robot->getRobotNodeSet("TorsoRightArm"), {2, 3, 4, 5, 0, 1, 2, 3, 3, 4});
+ ks1->doForwardKinematic(robot->getRobotNodeSet("TorsoRightArm"),
+ {2, 3, 4, 5, 0, 1, 2, 3, 3, 4});
}
/////////////////////////////////////////////////////////////////////////////////////////////////////////////
@@ -137,8 +148,14 @@ BOOST_AUTO_TEST_CASE(backAndForthTest)
robot = VirtualRobot::RobotIO::loadRobot(finder.getDataDir() + robotFile);
}
KinematicSolverPtr ks1 = KinematicSolver::getInstance(NULL, robot);
- PoseBasePtr pose = PoseBasePtr(new Pose(new Vector3(-300, 750, 1250), OrientationConversion::toQuaternion(0, 0, 0)));
- PoseBasePtr result = ks1->doForwardKinematic(robot->getRobotNodeSet("TorsoLeftArm"), ks1->solveIK(robot->getRobotNodeSet("TorsoLeftArm"), pose, IKSolver::CartesianSelection::Position, 50));
+ PoseBasePtr pose = PoseBasePtr(
+ new Pose(new Vector3(-300, 750, 1250), OrientationConversion::toQuaternion(0, 0, 0)));
+ PoseBasePtr result =
+ ks1->doForwardKinematic(robot->getRobotNodeSet("TorsoLeftArm"),
+ ks1->solveIK(robot->getRobotNodeSet("TorsoLeftArm"),
+ pose,
+ IKSolver::CartesianSelection::Position,
+ 50));
BOOST_CHECK_CLOSE(pose->position->x, result->position->x, 1);
BOOST_CHECK_CLOSE(pose->position->y, result->position->y, 1);
BOOST_CHECK_CLOSE(pose->position->z, result->position->z, 1);
@@ -194,13 +211,21 @@ BOOST_AUTO_TEST_CASE(recursiveIKLinear)
std::vector<PoseBasePtr> myPoints = std::vector<PoseBasePtr>();
- std::vector<IKSolver::CartesianSelection> mySelection = std::vector<IKSolver::CartesianSelection>();
+ std::vector<IKSolver::CartesianSelection> mySelection =
+ std::vector<IKSolver::CartesianSelection>();
for (double d = 0; d < 1; d = d + 0.01f)
{
myPoints.push_back(ip->getPoseAt(d));
mySelection.push_back(IKSolver::CartesianSelection::All);
}
- std::vector<std::vector<double>> result = ks->solveRecursiveIK(robot->getRobotNodeSet("TorsoLeftArm"), ks->solveIK(robot->getRobotNodeSet("TorsoLeftArm"), pose1, IKSolver::CartesianSelection::Position, 50), myPoints, mySelection);
+ std::vector<std::vector<double>> result =
+ ks->solveRecursiveIK(robot->getRobotNodeSet("TorsoLeftArm"),
+ ks->solveIK(robot->getRobotNodeSet("TorsoLeftArm"),
+ pose1,
+ IKSolver::CartesianSelection::Position,
+ 50),
+ myPoints,
+ mySelection);
/*for (vector<double> tempResult : result)
{
for (double angle : tempResult)
@@ -210,8 +235,6 @@ BOOST_AUTO_TEST_CASE(recursiveIKLinear)
//std::cout << "----------------------------------\n";*/
}
-
-
/////////////////////////////////////////////////////////////////////////////////////////////////////////////
/// Tests recursive IK method in "realistic setting - e.g. for calclating the Pose necessarry for spline
/// interpolation.
@@ -293,14 +316,36 @@ REACHABLE END
*/
BOOST_AUTO_TEST_CASE(recursiveIKThreeCP)
{
- std::vector<double> ja = { 0.464789, 0.357171, -0.303694, -0.067161, -0.445988, -0.204292, -0.070153};
- PosePkg p1 = createPosePkg(-348.304718, 580.476440, 712.264465, 0.4453609585762024, -0.4538218379020691, 0.5429607033729553, 0.5485371351242065);//reachable
- PosePkg p2 = createPosePkg(-182.23925781250, 580.074218750, 1034.98925781250, 0.4338901340961456, -0.4268467724323273, 0.5642792582511902, 0.5577904582023621);//reachable
- PosePkg p3 = createPosePkg(-226.792480468750, 580.723144531250, 1186.157348632812, 0.4336481690406799, -0.4273631870746613, 0.5638203620910645, 0.5580471754074097);//reachable
-
- std::vector<AbstractInterpolationPtr> ip = InterpolationSegmentFactory::produceLinearInterpolationSegments({p1.pose, p2.pose, p3.pose});
+ std::vector<double> ja = {
+ 0.464789, 0.357171, -0.303694, -0.067161, -0.445988, -0.204292, -0.070153};
+ PosePkg p1 = createPosePkg(-348.304718,
+ 580.476440,
+ 712.264465,
+ 0.4453609585762024,
+ -0.4538218379020691,
+ 0.5429607033729553,
+ 0.5485371351242065); //reachable
+ PosePkg p2 = createPosePkg(-182.23925781250,
+ 580.074218750,
+ 1034.98925781250,
+ 0.4338901340961456,
+ -0.4268467724323273,
+ 0.5642792582511902,
+ 0.5577904582023621); //reachable
+ PosePkg p3 = createPosePkg(-226.792480468750,
+ 580.723144531250,
+ 1186.157348632812,
+ 0.4336481690406799,
+ -0.4273631870746613,
+ 0.5638203620910645,
+ 0.5580471754074097); //reachable
+
+ std::vector<AbstractInterpolationPtr> ip =
+ InterpolationSegmentFactory::produceLinearInterpolationSegments(
+ {p1.pose, p2.pose, p3.pose});
std::vector<PoseBasePtr> poses = std::vector<PoseBasePtr>();
- std::vector<IKSolver::CartesianSelection> mySelection = std::vector<IKSolver::CartesianSelection>();
+ std::vector<IKSolver::CartesianSelection> mySelection =
+ std::vector<IKSolver::CartesianSelection>();
for (AbstractInterpolationPtr current : ip)
{
for (int i = 0; i < 100; i++)
@@ -322,7 +367,8 @@ BOOST_AUTO_TEST_CASE(recursiveIKThreeCP)
KinematicSolverPtr ks = KinematicSolver::getInstance(NULL, robot);
//Look whther first Pose is reachable
PoseBasePtr result2 = ks->doForwardKinematicRelative(robot->getRobotNodeSet("LeftArm"), ja);
- std::vector<std::vector<double>> result = ks->solveRecursiveIK(robot->getRobotNodeSet("LeftArm"), ja, poses, mySelection);
+ std::vector<std::vector<double>> result =
+ ks->solveRecursiveIK(robot->getRobotNodeSet("LeftArm"), ja, poses, mySelection);
for (vector<double> tempResult : result)
{
for (double angle : tempResult)
@@ -333,7 +379,6 @@ BOOST_AUTO_TEST_CASE(recursiveIKThreeCP)
}
}
-
/////////////////////////////////////////////////////////////////////////////////////////////////////////////
/// Tests recursive IK method in "realistic setting - e.g. for calclating the Pose necessarry for spline
/// interpolation.
@@ -409,14 +454,36 @@ BOOST_AUTO_TEST_CASE(recursiveIKThreeCP)
*/
BOOST_AUTO_TEST_CASE(recursiveIKThreeCPWithRotation)
{
- std::vector<double> ja = { -0.818259, 0.428277, 0.136299, -0.608422, 1.064518, -0.227969, -0.289433};
- PosePkg p1 = createPosePkg(-337.719116, 927.767395, 907.007935, -0.03198593109846115, 0.01920612715184689, 0.7076730132102966, 0.7055543065071106);
- PosePkg p2 = createPosePkg(-290.5917968750, 636.383300781250, 1363.315063476562, 0.6405168175697327, -0.3970025181770325, -0.3363495767116547, -0.5647976398468018);
- PosePkg p3 = createPosePkg(-316.302246093750, 777.949218750, 1194.246459960938, 0.5907033681869507, -0.5503066778182983, 0.4992305040359497, 0.3146440684795380);
-
- std::vector<AbstractInterpolationPtr> ip = InterpolationSegmentFactory::produceLinearInterpolationSegments({p1.pose, p2.pose, p3.pose});
+ std::vector<double> ja = {
+ -0.818259, 0.428277, 0.136299, -0.608422, 1.064518, -0.227969, -0.289433};
+ PosePkg p1 = createPosePkg(-337.719116,
+ 927.767395,
+ 907.007935,
+ -0.03198593109846115,
+ 0.01920612715184689,
+ 0.7076730132102966,
+ 0.7055543065071106);
+ PosePkg p2 = createPosePkg(-290.5917968750,
+ 636.383300781250,
+ 1363.315063476562,
+ 0.6405168175697327,
+ -0.3970025181770325,
+ -0.3363495767116547,
+ -0.5647976398468018);
+ PosePkg p3 = createPosePkg(-316.302246093750,
+ 777.949218750,
+ 1194.246459960938,
+ 0.5907033681869507,
+ -0.5503066778182983,
+ 0.4992305040359497,
+ 0.3146440684795380);
+
+ std::vector<AbstractInterpolationPtr> ip =
+ InterpolationSegmentFactory::produceLinearInterpolationSegments(
+ {p1.pose, p2.pose, p3.pose});
std::vector<PoseBasePtr> poses = std::vector<PoseBasePtr>();
- std::vector<IKSolver::CartesianSelection> mySelection = std::vector<IKSolver::CartesianSelection>();
+ std::vector<IKSolver::CartesianSelection> mySelection =
+ std::vector<IKSolver::CartesianSelection>();
for (AbstractInterpolationPtr current : ip)
{
for (int i = 0; i < 50; i++)
@@ -438,7 +505,8 @@ BOOST_AUTO_TEST_CASE(recursiveIKThreeCPWithRotation)
//Look whther first Pose is reachable
PoseBasePtr result2 = ks->doForwardKinematicRelative(robot->getRobotNodeSet("LeftArm"), ja);
//std::cout << "XXXXXXXXX" + std::to_string(result2->position->x) + "," + std::to_string(result2->position->y) + "," + std::to_string(result2->position->z) + "\n";
- std::vector<std::vector<double>> result = ks->solveRecursiveIK(robot->getRobotNodeSet("LeftArm"), ja, poses, mySelection);
+ std::vector<std::vector<double>> result =
+ ks->solveRecursiveIK(robot->getRobotNodeSet("LeftArm"), ja, poses, mySelection);
for (vector<double> tempResult : result)
{
for (double angle : tempResult)
@@ -505,13 +573,28 @@ UNREACHABLE START
*/
BOOST_AUTO_TEST_CASE(unreachableStart)
{
- std::vector<double> ja = { 0.464789, 0.357171, -0.303694, -0.067161, -0.445988, -0.204292, -0.070153};
- PosePkg p1 = createPosePkg(-348.304718, 580.476440, 712.264465, 0.4453609585762024, -0.4538218379020691, 0.5429607033729553, 0.5485371351242065);//unreachable
- PosePkg p2 = createPosePkg(-182.23925781250, 580.074218750, 1034.98925781250, 0.4338901340961456, -0.4268467724323273, 0.5642792582511902, 0.5577904582023621);//reachable
-
- std::vector<AbstractInterpolationPtr> ip = InterpolationSegmentFactory::produceLinearInterpolationSegments({p1.pose, p2.pose});
+ std::vector<double> ja = {
+ 0.464789, 0.357171, -0.303694, -0.067161, -0.445988, -0.204292, -0.070153};
+ PosePkg p1 = createPosePkg(-348.304718,
+ 580.476440,
+ 712.264465,
+ 0.4453609585762024,
+ -0.4538218379020691,
+ 0.5429607033729553,
+ 0.5485371351242065); //unreachable
+ PosePkg p2 = createPosePkg(-182.23925781250,
+ 580.074218750,
+ 1034.98925781250,
+ 0.4338901340961456,
+ -0.4268467724323273,
+ 0.5642792582511902,
+ 0.5577904582023621); //reachable
+
+ std::vector<AbstractInterpolationPtr> ip =
+ InterpolationSegmentFactory::produceLinearInterpolationSegments({p1.pose, p2.pose});
std::vector<PoseBasePtr> poses = std::vector<PoseBasePtr>();
- std::vector<IKSolver::CartesianSelection> mySelection = std::vector<IKSolver::CartesianSelection>();
+ std::vector<IKSolver::CartesianSelection> mySelection =
+ std::vector<IKSolver::CartesianSelection>();
for (AbstractInterpolationPtr current : ip)
{
for (int i = 0; i < 50; i++)
@@ -533,7 +616,8 @@ BOOST_AUTO_TEST_CASE(unreachableStart)
KinematicSolverPtr ks = KinematicSolver::getInstance(NULL, robot);
//Look whther first Pose is reachable
PoseBasePtr result2 = ks->doForwardKinematicRelative(robot->getRobotNodeSet("LeftArm"), ja);
- std::vector<std::vector<double>> result = ks->solveRecursiveIK(robot->getRobotNodeSet("LeftArm"), ja, poses, mySelection);
+ std::vector<std::vector<double>> result =
+ ks->solveRecursiveIK(robot->getRobotNodeSet("LeftArm"), ja, poses, mySelection);
for (vector<double> tempResult : result)
{
for (double angle : tempResult)
@@ -544,7 +628,6 @@ BOOST_AUTO_TEST_CASE(unreachableStart)
}
}
-
/////////////////////////////////////////////////////////////////////////////////////////////////////////////
/// Tests recursive IK method in "realistic setting - e.g. for calclating the Pose necessarry for spline
/// interpolation.
@@ -601,13 +684,28 @@ UNREACHABLE END
*/
BOOST_AUTO_TEST_CASE(UnreachableEnd)
{
- std::vector<double> ja = { -0.763830, -0.244323, 0.619645, -0.281197, 0.040509, 0.444159, 0.536926};
- PosePkg p1 = createPosePkg(4.825809, 788.516541, 1083.528442, 0.4453609585762024, -0.4538218379020691, 0.5429607033729553, 0.5485371351242065);//reachable
- PosePkg p2 = createPosePkg(-93.015136718750, 784.5754394531250, 500.448608398438, 0.4338901340961456, -0.4268467724323273, 0.5642792582511902, 0.5577904582023621);//unreachable
-
- std::vector<AbstractInterpolationPtr> ip = InterpolationSegmentFactory::produceLinearInterpolationSegments({p1.pose, p2.pose});
+ std::vector<double> ja = {
+ -0.763830, -0.244323, 0.619645, -0.281197, 0.040509, 0.444159, 0.536926};
+ PosePkg p1 = createPosePkg(4.825809,
+ 788.516541,
+ 1083.528442,
+ 0.4453609585762024,
+ -0.4538218379020691,
+ 0.5429607033729553,
+ 0.5485371351242065); //reachable
+ PosePkg p2 = createPosePkg(-93.015136718750,
+ 784.5754394531250,
+ 500.448608398438,
+ 0.4338901340961456,
+ -0.4268467724323273,
+ 0.5642792582511902,
+ 0.5577904582023621); //unreachable
+
+ std::vector<AbstractInterpolationPtr> ip =
+ InterpolationSegmentFactory::produceLinearInterpolationSegments({p1.pose, p2.pose});
std::vector<PoseBasePtr> poses = std::vector<PoseBasePtr>();
- std::vector<IKSolver::CartesianSelection> mySelection = std::vector<IKSolver::CartesianSelection>();
+ std::vector<IKSolver::CartesianSelection> mySelection =
+ std::vector<IKSolver::CartesianSelection>();
for (AbstractInterpolationPtr current : ip)
{
for (int i = 0; i < 50; i++)
@@ -716,15 +814,37 @@ ENREACHABLE MID
/////////////////////////////////////////////////////////////////////////////////////////////////////////////
BOOST_AUTO_TEST_CASE(unreachableMid)
{
- std::vector<double> ja = { -0.763830, -0.244323, 0.619645, -0.281197, 0.040509, 0.444159, 0.536926};
- PosePkg p1 = createPosePkg(4.825809, 788.516541, 1083.528442, 0.4453609585762024, -0.4538218379020691, 0.5429607033729553, 0.5485371351242065);//reachable
- PosePkg p2 = createPosePkg(-93.015136718750, 784.5754394531250, 500.448608398438, 0.4338901340961456, -0.4268467724323273, 0.5642792582511902, 0.5577904582023621);//unreachable
- PosePkg p3 = createPosePkg(4.825809, 788.516541, 1083.528442, 0.4453609585762024, -0.4538218379020691, 0.5429607033729553, 0.5485371351242065);//reachable
-
-
- std::vector<AbstractInterpolationPtr> ip = InterpolationSegmentFactory::produceLinearInterpolationSegments({p1.pose, p2.pose, p3.pose});
+ std::vector<double> ja = {
+ -0.763830, -0.244323, 0.619645, -0.281197, 0.040509, 0.444159, 0.536926};
+ PosePkg p1 = createPosePkg(4.825809,
+ 788.516541,
+ 1083.528442,
+ 0.4453609585762024,
+ -0.4538218379020691,
+ 0.5429607033729553,
+ 0.5485371351242065); //reachable
+ PosePkg p2 = createPosePkg(-93.015136718750,
+ 784.5754394531250,
+ 500.448608398438,
+ 0.4338901340961456,
+ -0.4268467724323273,
+ 0.5642792582511902,
+ 0.5577904582023621); //unreachable
+ PosePkg p3 = createPosePkg(4.825809,
+ 788.516541,
+ 1083.528442,
+ 0.4453609585762024,
+ -0.4538218379020691,
+ 0.5429607033729553,
+ 0.5485371351242065); //reachable
+
+
+ std::vector<AbstractInterpolationPtr> ip =
+ InterpolationSegmentFactory::produceLinearInterpolationSegments(
+ {p1.pose, p2.pose, p3.pose});
std::vector<PoseBasePtr> poses = std::vector<PoseBasePtr>();
- std::vector<IKSolver::CartesianSelection> mySelection = std::vector<IKSolver::CartesianSelection>();
+ std::vector<IKSolver::CartesianSelection> mySelection =
+ std::vector<IKSolver::CartesianSelection>();
for (AbstractInterpolationPtr current : ip)
{
for (int i = 0; i < 50; i++)
@@ -790,14 +910,29 @@ GlobalPose
/////////////////////////////////////////////////////////////////////////////////////////////////////////////
BOOST_AUTO_TEST_CASE(collisionStart)
{
- std::vector<double> ja = { 0.315176, -0.0275324, 1.00421, 0.0182586, 0.127499, -0.0255035, -0.299747};
- PosePkg p1 = createPosePkg(56.5798, 233.545, 992.508, 0.4453609585762024, -0.4538218379020691, 0.5429607033729553, 0.5485371351242065);//Colliding Pose
- PosePkg p2 = createPosePkg(-95.4907, 761.478, 1208.03, 0.4338901340961456, -0.4268467724323273, 0.5642792582511902, 0.5577904582023621);//Not Colliding Pose
-
-
- std::vector<AbstractInterpolationPtr> ip = InterpolationSegmentFactory::produceLinearInterpolationSegments({p1.pose, p2.pose});
+ std::vector<double> ja = {
+ 0.315176, -0.0275324, 1.00421, 0.0182586, 0.127499, -0.0255035, -0.299747};
+ PosePkg p1 = createPosePkg(56.5798,
+ 233.545,
+ 992.508,
+ 0.4453609585762024,
+ -0.4538218379020691,
+ 0.5429607033729553,
+ 0.5485371351242065); //Colliding Pose
+ PosePkg p2 = createPosePkg(-95.4907,
+ 761.478,
+ 1208.03,
+ 0.4338901340961456,
+ -0.4268467724323273,
+ 0.5642792582511902,
+ 0.5577904582023621); //Not Colliding Pose
+
+
+ std::vector<AbstractInterpolationPtr> ip =
+ InterpolationSegmentFactory::produceLinearInterpolationSegments({p1.pose, p2.pose});
std::vector<PoseBasePtr> poses = std::vector<PoseBasePtr>();
- std::vector<IKSolver::CartesianSelection> mySelection = std::vector<IKSolver::CartesianSelection>();
+ std::vector<IKSolver::CartesianSelection> mySelection =
+ std::vector<IKSolver::CartesianSelection>();
for (AbstractInterpolationPtr current : ip)
{
for (int i = 0; i < 50; i++)
@@ -819,8 +954,11 @@ BOOST_AUTO_TEST_CASE(collisionStart)
KinematicSolverPtr ks = KinematicSolver::getInstance(NULL, robot);
//Look whther first Pose is reachable
PoseBasePtr result2 = ks->doForwardKinematic(robot->getRobotNodeSet("LeftArm"), ja);
- std::cout << "XXXXXXXXX" + std::to_string(result2->position->x) + "," + std::to_string(result2->position->y) + "," + std::to_string(result2->position->z) + "\n";
- std::vector<std::vector<double>> result = ks->solveRecursiveIK(robot->getRobotNodeSet("LeftArm"), ja, poses, mySelection);
+ std::cout << "XXXXXXXXX" + std::to_string(result2->position->x) + "," +
+ std::to_string(result2->position->y) + "," +
+ std::to_string(result2->position->z) + "\n";
+ std::vector<std::vector<double>> result =
+ ks->solveRecursiveIK(robot->getRobotNodeSet("LeftArm"), ja, poses, mySelection);
/*for (vector<double> tempResult : result)
{
for (double angle : tempResult)
@@ -871,14 +1009,29 @@ jointAngles
}*/
BOOST_AUTO_TEST_CASE(CollisonMid)
{
- std::vector<double> ja = { -0.0475329, -0.0275045, 0.790436, -0.205607, 0.561071, -0.322912, -0.256206};
- PosePkg p1 = createPosePkg(3.02088, 389.723, 938.033, 0.4453609585762024, -0.4538218379020691, 0.5429607033729553, 0.5485371351242065);//no collision here
+ std::vector<double> ja = {
+ -0.0475329, -0.0275045, 0.790436, -0.205607, 0.561071, -0.322912, -0.256206};
+ PosePkg p1 = createPosePkg(3.02088,
+ 389.723,
+ 938.033,
+ 0.4453609585762024,
+ -0.4538218379020691,
+ 0.5429607033729553,
+ 0.5485371351242065); //no collision here
//in between there is a collision with robot body
- PosePkg p2 = createPosePkg(-167.276, -187.905, 999.493, 0.4338901340961456, -0.4268467724323273, 0.5642792582511902, 0.5577904582023621);//no one here either
-
- std::vector<AbstractInterpolationPtr> ip = InterpolationSegmentFactory::produceLinearInterpolationSegments({p1.pose, p2.pose});
+ PosePkg p2 = createPosePkg(-167.276,
+ -187.905,
+ 999.493,
+ 0.4338901340961456,
+ -0.4268467724323273,
+ 0.5642792582511902,
+ 0.5577904582023621); //no one here either
+
+ std::vector<AbstractInterpolationPtr> ip =
+ InterpolationSegmentFactory::produceLinearInterpolationSegments({p1.pose, p2.pose});
std::vector<PoseBasePtr> poses = std::vector<PoseBasePtr>();
- std::vector<IKSolver::CartesianSelection> mySelection = std::vector<IKSolver::CartesianSelection>();
+ std::vector<IKSolver::CartesianSelection> mySelection =
+ std::vector<IKSolver::CartesianSelection>();
for (AbstractInterpolationPtr current : ip)
{
for (int i = 0; i < 50; i++)
@@ -900,7 +1053,8 @@ BOOST_AUTO_TEST_CASE(CollisonMid)
KinematicSolverPtr ks = KinematicSolver::getInstance(NULL, robot);
//Look whther first Pose is reachable
PoseBasePtr result2 = ks->doForwardKinematicRelative(robot->getRobotNodeSet("LeftArm"), ja);
- std::vector<std::vector<double>> result = ks->solveRecursiveIK(robot->getRobotNodeSet("LeftArm"), ja, poses, mySelection);
+ std::vector<std::vector<double>> result =
+ ks->solveRecursiveIK(robot->getRobotNodeSet("LeftArm"), ja, poses, mySelection);
/*for (vector<double> tempResult : result)
{
for (double angle : tempResult)
@@ -923,14 +1077,28 @@ BOOST_AUTO_TEST_CASE(CollisonMid)
BOOST_AUTO_TEST_CASE(NoMovement)
{
std::cout << "NO MOVEMENT START";
- std::vector<double> ja = { -0.0475329, -0.0275045, 0.790436, -0.205607, 0.561071, -0.322912, -0.256206};
- PosePkg p1 = createPosePkg(3.02088, 389.723, 938.033, 0.4453609585762024, -0.4538218379020691, 0.5429607033729553, 0.5485371351242065);//no collision here
- PosePkg p2 = createPosePkg(3.02088, 389.723, 938.033, 0.4453609585762024, -0.4538218379020691, 0.5429607033729553, 0.5485371351242065);//no collision here
+ std::vector<double> ja = {
+ -0.0475329, -0.0275045, 0.790436, -0.205607, 0.561071, -0.322912, -0.256206};
+ PosePkg p1 = createPosePkg(3.02088,
+ 389.723,
+ 938.033,
+ 0.4453609585762024,
+ -0.4538218379020691,
+ 0.5429607033729553,
+ 0.5485371351242065); //no collision here
+ PosePkg p2 = createPosePkg(3.02088,
+ 389.723,
+ 938.033,
+ 0.4453609585762024,
+ -0.4538218379020691,
+ 0.5429607033729553,
+ 0.5485371351242065); //no collision here
std::vector<PoseBasePtr> cp = {p1.pose, p2.pose};
LinearInterpolationPtr ip = *new LinearInterpolationPtr(new LinearInterpolation(cp));
std::vector<PoseBasePtr> poses = std::vector<PoseBasePtr>();
- std::vector<IKSolver::CartesianSelection> mySelection = std::vector<IKSolver::CartesianSelection>();
+ std::vector<IKSolver::CartesianSelection> mySelection =
+ std::vector<IKSolver::CartesianSelection>();
for (int i = 0; i < 50; i++)
{
poses.push_back(ip->getPoseAt(i / 50.0));
@@ -949,7 +1117,8 @@ BOOST_AUTO_TEST_CASE(NoMovement)
}
KinematicSolverPtr ks = KinematicSolver::getInstance(NULL, robot);
//Look whther first Pose is reachable
- std::vector<std::vector<double>> result = ks->solveRecursiveIK(robot->getRobotNodeSet("LeftArm"), ja, poses, mySelection);
+ std::vector<std::vector<double>> result =
+ ks->solveRecursiveIK(robot->getRobotNodeSet("LeftArm"), ja, poses, mySelection);
for (vector<double> tempResult : result)
{
for (double angle : tempResult)
@@ -960,10 +1129,3 @@ BOOST_AUTO_TEST_CASE(NoMovement)
}
std::cout << "NO MOVEMENT END";
}
-
-
-
-
-
-
-
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/TrajectoryCalculation/DesignerTrajectoryCalculator.cpp b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/TrajectoryCalculation/DesignerTrajectoryCalculator.cpp
index 2648ce1face1c67b198bd71018bbec9bc5a621c1..09dd90151f64576ac4b4108c060f91f7826133cd 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/TrajectoryCalculation/DesignerTrajectoryCalculator.cpp
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/TrajectoryCalculation/DesignerTrajectoryCalculator.cpp
@@ -1,16 +1,26 @@
#include "DesignerTrajectoryCalculator.h"
-#include "TimedTrajectoryFactory.h"
-#include "PathFactory.h"
+
#include <VirtualRobot/TimeOptimalTrajectory/Path.h>
+
#include <RobotAPI/libraries/core/TrajectoryController.h>
+#include "PathFactory.h"
+#include "TimedTrajectoryFactory.h"
+
#define TRAJECTORYCALCULATION_TIMESTEP 0.001
-armarx::DesignerTrajectoryCalculator::DesignerTrajectoryCalculator(EnvironmentPtr environment) : environment(environment)
-{
+armarx::DesignerTrajectoryCalculator::DesignerTrajectoryCalculator(EnvironmentPtr environment) :
+ environment(environment)
+{
}
-armarx::TimedTrajectory armarx::DesignerTrajectoryCalculator::calculateTimeOptimalTrajectory(std::vector<std::vector<double> >& trajectory, std::vector<std::vector<double> >& userPoints, VirtualRobot::RobotNodeSetPtr rns, double maxDeviation, double accuracyFactor)
+armarx::TimedTrajectory
+armarx::DesignerTrajectoryCalculator::calculateTimeOptimalTrajectory(
+ std::vector<std::vector<double>>& trajectory,
+ std::vector<std::vector<double>>& userPoints,
+ VirtualRobot::RobotNodeSetPtr rns,
+ double maxDeviation,
+ double accuracyFactor)
{
//ARMARX_INFO << "Entering calculation";
//ARMARX_INFO << trajectory.size() << " jointangles in trajectory";
@@ -28,7 +38,7 @@ armarx::TimedTrajectory armarx::DesignerTrajectoryCalculator::calculateTimeOptim
//If the path length is 0, create a pseudo trajectory containing the start position once for every userPoint
std::vector<double> timestamps;
//ARMARX_INFO << "Filling timestamps";
- for (unsigned int i = 0; i < userPoints.size(); i ++)
+ for (unsigned int i = 0; i < userPoints.size(); i++)
{
timestamps.push_back(0.01 * i);
}
@@ -38,7 +48,6 @@ armarx::TimedTrajectory armarx::DesignerTrajectoryCalculator::calculateTimeOptim
for (unsigned int i = 0; i < userPoints[0].size(); i++)
{
nodeData.push_back({userPoints[0][i]});
-
}
for (unsigned int i = 1; i < userPoints.size(); i++)
{
@@ -107,7 +116,6 @@ armarx::TimedTrajectory armarx::DesignerTrajectoryCalculator::calculateTimeOptim
}
//ARMARX_INFO << jA;
newPathVector.push_back(jA);
-
}
//ARMARX_INFO << newPathVector;
VirtualRobot::Path unfoldedPath = armarx::PathFactory::createPath(newPathVector, maxDeviation);
@@ -135,13 +143,15 @@ armarx::TimedTrajectory armarx::DesignerTrajectoryCalculator::calculateTimeOptim
}
}
const Eigen::VectorXd maxVelocity = Eigen::VectorXd::Map(&mVelocity[0], mVelocity.size());
- const Eigen::VectorXd maxAcceleration = Eigen::VectorXd::Map(&mAcceleration[0], mAcceleration.size());
+ const Eigen::VectorXd maxAcceleration =
+ Eigen::VectorXd::Map(&mAcceleration[0], mAcceleration.size());
//Set the timestep to the trajectorycalculation timestep constant for now
double timeStep = TRAJECTORYCALCULATION_TIMESTEP;
//Calculate the TimeOptimalTrajectory using Simox, and bring the accuracyFactor into account
//ARMARX_INFO << "Calling Simox TimeOptimalTrajectory";
- VirtualRobot::TimeOptimalTrajectory timeOptimalTrajectory = VirtualRobot::TimeOptimalTrajectory(unfoldedPath, maxVelocity, maxAcceleration, timeStep / accuracyFactor);
+ VirtualRobot::TimeOptimalTrajectory timeOptimalTrajectory = VirtualRobot::TimeOptimalTrajectory(
+ unfoldedPath, maxVelocity, maxAcceleration, timeStep / accuracyFactor);
//If simox failed finding a trajectory, signal this
if (timeOptimalTrajectory.isValid() == 0)
{
@@ -149,12 +159,16 @@ armarx::TimedTrajectory armarx::DesignerTrajectoryCalculator::calculateTimeOptim
throw InvalidArgumentException("Input trajectory can not generate a continuous path");
}
//As simox sometimes returns valid on trajectories with nonreal or negative duration, check this too
- if (timeOptimalTrajectory.getDuration() <= 0 || std::isnan(timeOptimalTrajectory.getDuration()) || timeOptimalTrajectory.getDuration() >= HUGE_VAL)
+ if (timeOptimalTrajectory.getDuration() <= 0 ||
+ std::isnan(timeOptimalTrajectory.getDuration()) ||
+ timeOptimalTrajectory.getDuration() >= HUGE_VAL)
{
//ARMARX_INFO << "Invalid trajectory duration";
- throw InvalidArgumentException("Input trajectory can not generate a continuous path with positive real duration");
+ throw InvalidArgumentException(
+ "Input trajectory can not generate a continuous path with positive real duration");
}
//ARMARX_INFO << "TimeOptimalTrajectory has duration of " << timeOptimalTrajectory.getDuration();
//Return the result
- return armarx::TimedTrajectoryFactory::createTimedTrajectory(timeOptimalTrajectory, userPoints, rns, maxDeviation);
+ return armarx::TimedTrajectoryFactory::createTimedTrajectory(
+ timeOptimalTrajectory, userPoints, rns, maxDeviation);
}
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/TrajectoryCalculation/DesignerTrajectoryCalculator.h b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/TrajectoryCalculation/DesignerTrajectoryCalculator.h
index e5c25ca2952c70df9b6e6966417d84cee90992e4..adb68a1150bb4a2cdfc5e6a8b15ce4cca613108a 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/TrajectoryCalculation/DesignerTrajectoryCalculator.h
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/TrajectoryCalculation/DesignerTrajectoryCalculator.h
@@ -2,13 +2,16 @@
#define DESIGNERTRAJECTORYCALCULATOR_H
#include <Eigen/Core>
-#include "TimedTrajectory.h"
+
#include <VirtualRobot/Robot.h>
+
#include "../Environment.h"
+#include "TimedTrajectory.h"
namespace armarx
{
class DesignerTrajectoryCalculator;
+
/**
* @class DesignerTrajectoryCalculator
* @brief Offers functionality to create TimedTrajectories from supplied Trajectories and points set by the user.
@@ -19,7 +22,6 @@ namespace armarx
EnvironmentPtr environment;
public:
-
//TimedTrajectory calculateTimeOptimalTrajectory(std::vector<Eigen::VectorXf>& trajectory, std::vector<Eigen::VectorXf>& userPoints, double maxDeviation);
/*
* HOW TO CONVERT vector<vector<double>> to vector<Eigen::VectorXf>
@@ -45,7 +47,11 @@ namespace armarx
* @param accuracyFactor The factor with which to scale the accuracy of the time optimal trajectory.
* @return The created TimedTrajectory.
*/
- TimedTrajectory calculateTimeOptimalTrajectory(std::vector<std::vector<double>>& trajectory, std::vector<std::vector<double>>& userPoints, VirtualRobot::RobotNodeSetPtr rns, double maxDeviation, double accuracyFactor = 1.0);
+ TimedTrajectory calculateTimeOptimalTrajectory(std::vector<std::vector<double>>& trajectory,
+ std::vector<std::vector<double>>& userPoints,
+ VirtualRobot::RobotNodeSetPtr rns,
+ double maxDeviation,
+ double accuracyFactor = 1.0);
};
-}
+} // namespace armarx
#endif // DESIGNERTRAJECTORYCALCULATOR_H
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/TrajectoryCalculation/PathFactory.cpp b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/TrajectoryCalculation/PathFactory.cpp
index 00223522a9ac6c8f807ddf5945cd1edf6657ece1..f1b8118e12ea6203d5be6da27bc473f3018442fe 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/TrajectoryCalculation/PathFactory.cpp
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/TrajectoryCalculation/PathFactory.cpp
@@ -1,6 +1,7 @@
#include "PathFactory.h"
-VirtualRobot::Path armarx::PathFactory::createPath(std::vector<std::vector<double>>& nodes, double maxDeviation)
+VirtualRobot::Path
+armarx::PathFactory::createPath(std::vector<std::vector<double>>& nodes, double maxDeviation)
{
//Convert the input vector of vectors to a list of Eigen::VectorXd
std::list<Eigen::VectorXd> path;
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/TrajectoryCalculation/PathFactory.h b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/TrajectoryCalculation/PathFactory.h
index 466878465368179cea7d89b4ccdcadd39108fe59..30bef87835939d20ec51dc9a9b0acd116bffb32d 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/TrajectoryCalculation/PathFactory.h
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/TrajectoryCalculation/PathFactory.h
@@ -23,12 +23,15 @@
#define PATHFACTORY_H
#include <Eigen/Core>
+
#include <VirtualRobot/TimeOptimalTrajectory/Path.h>
+
#include "../Model/DesignerTrajectory.h"
namespace armarx
{
class PathFactory;
+
/**
* @class PathFactory
* @brief Creates a Simox VirtualRobot::Path from a set of nodes representing joint angles and a maximum deviation parameter.
@@ -42,8 +45,8 @@ namespace armarx
* @param maxDeviation The maximum deviation of the points along the path.
* @return The created path.
*/
- static VirtualRobot::Path createPath(std::vector<std::vector<double>>& nodes, double maxDeviation);
+ static VirtualRobot::Path createPath(std::vector<std::vector<double>>& nodes,
+ double maxDeviation);
};
-}
+} // namespace armarx
#endif // PATHFACTORY_H
-
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/TrajectoryCalculation/Test/DesignerTrajectoryCalculatorTest.cpp b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/TrajectoryCalculation/Test/DesignerTrajectoryCalculatorTest.cpp
index 5c210860636043385295f8a4c011021d2bcba791..920778a839e3a6504018529450b2e971b79a117b 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/TrajectoryCalculation/Test/DesignerTrajectoryCalculatorTest.cpp
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/TrajectoryCalculation/Test/DesignerTrajectoryCalculatorTest.cpp
@@ -1,12 +1,10 @@
#define BOOST_TEST_MODULE ArmarX::RobotTrajectoryDesigner::DesignerTrajectoryCalculator
#define ARMARX_BOOST_TEST
-#include "../../../../../build/testing/RobotTrajectoryDesigner/Test.h"
#include "../DesignerTrajectoryCalculator.h"
-using namespace armarx;
-
-
+#include "../../../../../build/testing/RobotTrajectoryDesigner/Test.h"
+using namespace armarx;
//Check if trajectories are calculated correctly
BOOST_AUTO_TEST_CASE(TrajectoryCalculationTest)
@@ -14,6 +12,3 @@ BOOST_AUTO_TEST_CASE(TrajectoryCalculationTest)
BOOST_CHECK_EQUAL(1, 1);
//BOOST_CHECK_THROW(dt1.getTrajectorySegment(2), IndexOutOfBoundsException);
}
-
-
-
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/TrajectoryCalculation/Test/PathFactoryTest.cpp b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/TrajectoryCalculation/Test/PathFactoryTest.cpp
index d3cec5b631c72d016e5029c2736832add9d3d312..05a59b8c21e45b8ffe7259fef1221deeebeb27f1 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/TrajectoryCalculation/Test/PathFactoryTest.cpp
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/TrajectoryCalculation/Test/PathFactoryTest.cpp
@@ -7,8 +7,8 @@ using namespace armarx;
BOOST_AUTO_TEST_CASE(empty)
{
-
}
+
/*
//Check incorrect nodes parameters
BOOST_AUTO_TEST_CASE(NodesTest)
@@ -20,5 +20,3 @@ BOOST_AUTO_TEST_CASE(CorrectPathTest)
{
//BOOST_CHECK_EQUAL(traj5->getDimensionData(0), traj4->getDimensionData(0));
}*/
-
-
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/TrajectoryCalculation/Test/TimedTrajectoryFactoryTest.cpp b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/TrajectoryCalculation/Test/TimedTrajectoryFactoryTest.cpp
index 6e6d2f17ec870c8e6f2fcc1b346050f3635b7775..8f916bfa1a3a064615bd8fdda6549395f8682f0c 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/TrajectoryCalculation/Test/TimedTrajectoryFactoryTest.cpp
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/TrajectoryCalculation/Test/TimedTrajectoryFactoryTest.cpp
@@ -1,16 +1,17 @@
#define BOOST_TEST_MODULE ArmarX::RobotTrajectoryDesigner::TimedTrajectoryFactory
#define ARMARX_BOOST_TEST
+#include "../TimedTrajectoryFactory.h"
+
#include "../../../../../build/testing/RobotTrajectoryDesigner/Test.h"
#include "../Environment.h"
-#include "../TimedTrajectoryFactory.h"
#include "../PathFactory.h"
using namespace armarx;
//Check if the trajectory contained in the returned timedtrajectory is correct
BOOST_AUTO_TEST_CASE(CorrectTrajectoryTest)
{
- BOOST_CHECK_EQUAL(1, 1);/*
+ BOOST_CHECK_EQUAL(1, 1); /*
VirtualRobot::RobotNodeSetPtr rns = Environment::getRobot()->getRobotNodeSets()[0];
std::vector<std::vector<double>> data;
for(unsigned int i = 0; i < 10; i++){
@@ -26,6 +27,7 @@ BOOST_AUTO_TEST_CASE(CorrectTrajectoryTest)
VirtualRobot::TimeOptimalTrajectory t = VirtualRobot::TimeOptimalTrajectory(p, maxVelocity, maxAcceleration, 0.001);
TimedTrajectory trajectory = TimedTrajectoryFactory::createTimedTrajectory(t, data, rns, 0.0);*/
}
+
/*
//Check if the userPoints contained in the returned timedtrajectory are correct
BOOST_AUTO_TEST_CASE(CorrectUserPointsTest){
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/TrajectoryCalculation/TimedTrajectory.cpp b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/TrajectoryCalculation/TimedTrajectory.cpp
index 1a48e907c82a38cd14f533f8d910b1bce7da168a..5f3c2403a0e67dc1f330b27a3260cb24746429db 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/TrajectoryCalculation/TimedTrajectory.cpp
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/TrajectoryCalculation/TimedTrajectory.cpp
@@ -1,18 +1,22 @@
#include "TimedTrajectory.h"
+
#include <memory.h>
-armarx::TimedTrajectory::TimedTrajectory(armarx::TrajectoryPtr trajectory, std::vector<double> userPoints)
+armarx::TimedTrajectory::TimedTrajectory(armarx::TrajectoryPtr trajectory,
+ std::vector<double> userPoints)
{
this->trajectory = trajectory;
this->userPoints = userPoints;
}
-const armarx::TrajectoryPtr armarx::TimedTrajectory::getTrajectory() const
+const armarx::TrajectoryPtr
+armarx::TimedTrajectory::getTrajectory() const
{
return trajectory;
}
-std::vector<double> armarx::TimedTrajectory::getUserPoints() const
+std::vector<double>
+armarx::TimedTrajectory::getUserPoints() const
{
return userPoints;
}
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/TrajectoryCalculation/TimedTrajectory.h b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/TrajectoryCalculation/TimedTrajectory.h
index 78df90e985a6e483af3cae35113d6aa4a99d5c74..317ba1f62fc11890fdb19edc5917adf1c9468f37 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/TrajectoryCalculation/TimedTrajectory.h
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/TrajectoryCalculation/TimedTrajectory.h
@@ -1,9 +1,10 @@
#ifndef TIMEDTRAJECTORY_H
#define TIMEDTRAJECTORY_H
-#include <RobotAPI/libraries/core/Trajectory.h>
#include <ArmarXCore/core/exceptions/user/NotImplementedYetException.h>
+#include <RobotAPI/libraries/core/Trajectory.h>
+
namespace armarx
{
/**
@@ -17,6 +18,7 @@ namespace armarx
TrajectoryPtr trajectory;
//Mapped times of the Trajectory reaching userpoints.
std::vector<double> userPoints;
+
public:
/**
* @brief Creates a TimedTrajectory out of the supplied Trajectory and userpoints.
@@ -36,5 +38,5 @@ namespace armarx
*/
std::vector<double> getUserPoints() const;
};
-}
+} // namespace armarx
#endif // TIMEDTRAJECTORY_H
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/TrajectoryCalculation/TimedTrajectoryFactory.cpp b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/TrajectoryCalculation/TimedTrajectoryFactory.cpp
index cb1eeecc7eab558ed82932922e694f40bc3a2754..6f6da3f6e5bcfa08a16373c1de030322e5511c80 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/TrajectoryCalculation/TimedTrajectoryFactory.cpp
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/TrajectoryCalculation/TimedTrajectoryFactory.cpp
@@ -6,7 +6,12 @@
#define TIMEDTRAJECTORY_FPS 100
-armarx::TimedTrajectory armarx::TimedTrajectoryFactory::createTimedTrajectory(VirtualRobot::TimeOptimalTrajectory& trajectory, std::vector<std::vector<double>>& userPoints, VirtualRobot::RobotNodeSetPtr rns, double maxDeviation)
+armarx::TimedTrajectory
+armarx::TimedTrajectoryFactory::createTimedTrajectory(
+ VirtualRobot::TimeOptimalTrajectory& trajectory,
+ std::vector<std::vector<double>>& userPoints,
+ VirtualRobot::RobotNodeSetPtr rns,
+ double maxDeviation)
{
//ARMARX_INFO << "Creating timedTrajectory";
@@ -116,15 +121,17 @@ armarx::TimedTrajectory armarx::TimedTrajectoryFactory::createTimedTrajectory(Vi
//ARMARX_ERROR << traj->toEigen();
int pass = 1;
//Arbitrary interrupt condition
- for (pass = 1; pass < 10; pass ++)
+ for (pass = 1; pass < 10; pass++)
{
std::vector<double> times;
unsigned int i = 0;
//Map the user Points to the respective timestamps that their position is approximately reached within
- for (double t = 0; t < trajectory.getDuration(); t += timestep / TIMEDTRAJECTORY_PROBEFACTOR)
+ for (double t = 0; t < trajectory.getDuration();
+ t += timestep / TIMEDTRAJECTORY_PROBEFACTOR)
{
- Eigen::VectorXd jointAngles = Eigen::VectorXd::Map(&userPoints[i][0], userPoints[i].size());
+ Eigen::VectorXd jointAngles =
+ Eigen::VectorXd::Map(&userPoints[i][0], userPoints[i].size());
/*for (int i = 0; i < jointAngles.rows(); i++)
{
ARMARX_INFO << "Point" << i << "jointAngles [" << i << "] : " << jointAngles[i];
@@ -170,10 +177,11 @@ armarx::TimedTrajectory armarx::TimedTrajectoryFactory::createTimedTrajectory(Vi
continue;
}
//Return the result and the number of passes that were needed for the calculation
- pass == 1 ? ARMARX_INFO << pass << " pass needed to find mapping" : ARMARX_INFO << pass << " passes needed to find mapping";
+ pass == 1 ? ARMARX_INFO << pass << " pass needed to find mapping"
+ : ARMARX_INFO << pass << " passes needed to find mapping";
return TimedTrajectory(traj, times);
}
- ARMARX_INFO << "More than" << pass << "passes needed for calculation";
+ ARMARX_INFO << "More than" << pass << "passes needed for calculation";
//ARMARX_INFO << "Canceled calculation with maxDeviation of " << maxDeviation;
throw InvalidArgumentException("No TimedTrajectory could be created");
}
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/TrajectoryCalculation/TimedTrajectoryFactory.h b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/TrajectoryCalculation/TimedTrajectoryFactory.h
index b778bc25f85a311f224544484c8dd6ff532d20c0..bdd3df38b163a7dd7f05a6f6a6b885cee51bc01a 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/TrajectoryCalculation/TimedTrajectoryFactory.h
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/TrajectoryCalculation/TimedTrajectoryFactory.h
@@ -3,11 +3,13 @@
#include <VirtualRobot/RobotNodeSet.h>
#include <VirtualRobot/TimeOptimalTrajectory/TimeOptimalTrajectory.h>
+
#include "TimedTrajectory.h"
namespace armarx
{
class TimedTrajectoryFactory;
+
/**
* @class TimedTrajectoryFactory
* @brief Creates a TimedTrajectory out of a TimeOptimalTrajectory supplied by Simox methods.
@@ -22,7 +24,11 @@ namespace armarx
* @param maxDeviation The maximum deviation with which the userPoints may vary from the input TimedTrajectory.
* @return A TimedTrajectory which contains the timestamps of the supplied userPoints.
*/
- static TimedTrajectory createTimedTrajectory(VirtualRobot::TimeOptimalTrajectory& trajectory, std::vector<std::vector<double>>& userPoints, VirtualRobot::RobotNodeSetPtr rns, double maxDeviation);
+ static TimedTrajectory
+ createTimedTrajectory(VirtualRobot::TimeOptimalTrajectory& trajectory,
+ std::vector<std::vector<double>>& userPoints,
+ VirtualRobot::RobotNodeSetPtr rns,
+ double maxDeviation);
};
-}
+} // namespace armarx
#endif // TIMEDTRAJECTORYFACTORY_H
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Util/OrientationConversion.cpp b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Util/OrientationConversion.cpp
index 6d925fedeb1e652c21e8c80d998563d3681627ff..dc644ccc16574c2ca249bc855a699d1826b5ce64 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Util/OrientationConversion.cpp
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Util/OrientationConversion.cpp
@@ -27,10 +27,10 @@
using namespace armarx;
-
namespace armarx
{
- std::vector<double> OrientationConversion::toEulerAngle(QuaternionBasePtr q)
+ std::vector<double>
+ OrientationConversion::toEulerAngle(QuaternionBasePtr q)
{
// roll (x-axis rotation)
double sinr = +2.0 * (q->qw * q->qx + q->qy * q->qz);
@@ -41,7 +41,7 @@ namespace armarx
double pitch;
if (fabs(sinp) >= 1)
{
- pitch = copysign(M_PI / 2, sinp); // use 90 degrees if out of range
+ pitch = copysign(M_PI / 2, sinp); // use 90 degrees if out of range
}
else
{
@@ -54,7 +54,8 @@ namespace armarx
return {roll, pitch, yaw};
}
- QuaternionBasePtr OrientationConversion::toQuaternion(const double roll, const double pitch, const double yaw)
+ QuaternionBasePtr
+ OrientationConversion::toQuaternion(const double roll, const double pitch, const double yaw)
{
QuaternionBasePtr q = QuaternionBasePtr(new FramedOrientation());
//Abbreviations for the various angular functions
@@ -71,12 +72,14 @@ namespace armarx
return q;
}
- Eigen::Quaternion<double> OrientationConversion::toEigen(QuaternionBasePtr q)
+ Eigen::Quaternion<double>
+ OrientationConversion::toEigen(QuaternionBasePtr q)
{
return Eigen::Quaternion<double>(q->qw, q->qx, q->qy, q->qz);
}
- QuaternionBasePtr OrientationConversion::toArmarX(Eigen::Quaternion<double> q)
+ QuaternionBasePtr
+ OrientationConversion::toArmarX(Eigen::Quaternion<double> q)
{
FramedOrientation temp = *new FramedOrientation();
temp.qw = q.w();
@@ -85,4 +88,4 @@ namespace armarx
temp.qz = q.z();
return QuaternionBasePtr(new FramedOrientation(temp));
}
-}
+} // namespace armarx
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Util/OrientationConversion.h b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Util/OrientationConversion.h
index afabbf12e242abe839df5766ed718844800c1858..7d2cc63f77ddc62487c9f5d69c269a2675649585 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Util/OrientationConversion.h
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Util/OrientationConversion.h
@@ -25,12 +25,12 @@
#define ORIENTATIONCONVERSION_H
+#include <memory>
+
#include "RobotAPI/interface/core/PoseBase.h"
#include "Eigen/Geometry"
-#include <memory>
-
namespace armarx
{
@@ -39,12 +39,13 @@ namespace armarx
public:
static std::vector<double> toEulerAngle(QuaternionBasePtr q);
- static QuaternionBasePtr toQuaternion(const double roll, const double pitch, const double yaw);
+ static QuaternionBasePtr
+ toQuaternion(const double roll, const double pitch, const double yaw);
static Eigen::Quaternion<double> toEigen(QuaternionBasePtr q);
static QuaternionBasePtr toArmarX(Eigen::Quaternion<double> q);
};
-}
+} // namespace armarx
#endif
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Util/Tests/OrientationConversionTest.cpp b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Util/Tests/OrientationConversionTest.cpp
index e5ac83cd9df52e74abad9bfa151e849857471e19..a53e49762eda130bc3e3a586873c593827ff2e00 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Util/Tests/OrientationConversionTest.cpp
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Util/Tests/OrientationConversionTest.cpp
@@ -1,15 +1,16 @@
#define BOOST_TEST_MODULE ArmarX::RobotTrajectoryDesigner::OrientationConversionTest
#define ARMARX_BOOST_TEST
-#include "../../../../../build/testing/RobotTrajectoryDesigner/Test.h"
-
-#include "RobotTrajectoryDesigner/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Interpolation/LinearInterpolation.h"
#include "RobotTrajectoryDesigner/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Util/OrientationConversion.h"
-#include "RobotAPI/libraries/core/Pose.h"
#include "RobotAPI/libraries/core/FramedPose.h"
+#include "RobotAPI/libraries/core/Pose.h"
+
+#include "../../../../../build/testing/RobotTrajectoryDesigner/Test.h"
+#include "RobotTrajectoryDesigner/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Interpolation/LinearInterpolation.h"
using namespace armarx;
using namespace boost::test_tools;
+
BOOST_AUTO_TEST_CASE(eulerToQuatTest1)
{
//Init
@@ -24,11 +25,12 @@ BOOST_AUTO_TEST_CASE(eulerToQuatTest2)
{
//Init
QuaternionBasePtr ori = OrientationConversion::toQuaternion(3, 1, -2);
- BOOST_REQUIRE_CLOSE_FRACTION(ori->qw, -0.369, 1);
+ BOOST_REQUIRE_CLOSE_FRACTION(ori->qw, -0.369, 1);
BOOST_REQUIRE_CLOSE(ori->qx, 0.502, 1);
BOOST_REQUIRE_CLOSE(ori->qy, -0.718, 1);
BOOST_REQUIRE_CLOSE(ori->qz, -0.311, 1);
}
+
BOOST_AUTO_TEST_CASE(eulerToQuatTest3)
{
//Init
@@ -39,9 +41,6 @@ BOOST_AUTO_TEST_CASE(eulerToQuatTest3)
BOOST_REQUIRE_CLOSE(ori->qz, 0.772, 1);
}
-
-
-
BOOST_AUTO_TEST_CASE(quatToEulerTest1)
{
//Init
@@ -61,7 +60,7 @@ BOOST_AUTO_TEST_CASE(quatToEulerTest2)
//Init
QuaternionBasePtr ori = *new QuaternionBasePtr(new FramedOrientation());
ori->qw = 0.658;
- ori->qx = 0.164;
+ ori->qx = 0.164;
ori->qy = -0.329;
ori->qz = 0.658;
std::vector<double> euler = OrientationConversion::toEulerAngle(ori);
@@ -78,5 +77,3 @@ BOOST_AUTO_TEST_CASE(twoConversionsTest)
BOOST_REQUIRE_CLOSE(euler[1], 0.2, 1);
BOOST_REQUIRE_CLOSE(euler[2], 0.3, 1);
}
-
-
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Util/WheelEventFilter.cpp b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Util/WheelEventFilter.cpp
index 40ae80d5f9cb17fcc14ca4ef69348a9b1f202adf..d7c5ca2001e8bdd9c62a89d65799b23b8ed2a867 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Util/WheelEventFilter.cpp
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Util/WheelEventFilter.cpp
@@ -23,15 +23,14 @@
namespace armarx
{
- WheelEventFilter::WheelEventFilter(QObject* parent) :
- QObject(parent)
+ WheelEventFilter::WheelEventFilter(QObject* parent) : QObject(parent)
{
}
- bool WheelEventFilter::eventFilter(QObject* obj, QEvent* e)
+ bool
+ WheelEventFilter::eventFilter(QObject* obj, QEvent* e)
{
- if (e->type() == QEvent::Wheel
- && qobject_cast<QComboBox*>(obj))
+ if (e->type() == QEvent::Wheel && qobject_cast<QComboBox*>(obj))
{
if (qobject_cast<QComboBox*>(obj)->focusPolicy() == Qt::WheelFocus)
{
@@ -49,4 +48,4 @@ namespace armarx
return QObject::eventFilter(obj, e);
}
}
-}
+} // namespace armarx
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Util/WheelEventFilter.h b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Util/WheelEventFilter.h
index 2a82a8d95030fa8718cb47005c1c4e56fcd27c36..b2feebecf3407e17c5a5b376131fb577f1d3daab 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Util/WheelEventFilter.h
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Util/WheelEventFilter.h
@@ -22,9 +22,9 @@
#ifndef WHEELEVENTFILTER_H
#define WHEELEVENTFILTER_H
-#include <QObject>
-#include <QEvent>
#include <QComboBox>
+#include <QEvent>
+#include <QObject>
namespace armarx
{
@@ -42,6 +42,6 @@ namespace armarx
*/
bool eventFilter(QObject* obj, QEvent* e) override;
};
-}
+} // namespace armarx
#endif // WHEELEVENTFILTER_H
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/View/ExportDialog.cpp b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/View/ExportDialog.cpp
index 9ade30e8dc8a662c3ef407f593aa093a7b2eeee9..0f493268df77f98ab66fb98e31e528430a1da232 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/View/ExportDialog.cpp
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/View/ExportDialog.cpp
@@ -1,9 +1,8 @@
#include "ExportDialog.h"
+
#include "ui_ExportDialog.h"
-ExportDialog::ExportDialog(QWidget* parent) :
- QDialog(parent),
- ui(new Ui::ExportDialog)
+ExportDialog::ExportDialog(QWidget* parent) : QDialog(parent), ui(new Ui::ExportDialog)
{
ui->setupUi(this);
}
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/View/Perspectives.cpp b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/View/Perspectives.cpp
index 96d2d046826dc44c01712231e7c4326d0855ab4e..238935caacc1dcad104a78b460f3b0cbbf08fbca 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/View/Perspectives.cpp
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/View/Perspectives.cpp
@@ -21,19 +21,19 @@
*/
#include "Perspectives.h"
-Perspectives::Perspectives(QWidget* parent) :
- QWidget(parent),
- perspectives(new Ui::Perspectives)
+Perspectives::Perspectives(QWidget* parent) : QWidget(parent), perspectives(new Ui::Perspectives)
{
perspectives->setupUi(this);
}
-Ui::Perspectives* Perspectives::getPerspectives()
+Ui::Perspectives*
+Perspectives::getPerspectives()
{
return this->perspectives;
}
-void Perspectives::setPerspectives(Ui::Perspectives* perspectives)
+void
+Perspectives::setPerspectives(Ui::Perspectives* perspectives)
{
this->perspectives = perspectives;
}
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/View/Perspectives.h b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/View/Perspectives.h
index fde976599b5ba8c2686e01f99f1d47befc73e380..fb21f5facc434aa2eb5175fdffb88080f06c1215 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/View/Perspectives.h
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/View/Perspectives.h
@@ -23,11 +23,13 @@
#define PERSPECTIVES_H
-#include <RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/View/ui_Perspectives.h>
-#include <QWidget>
-#include <QListWidget>
#include <memory>
+#include <QListWidget>
+#include <QWidget>
+
+#include <RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/View/ui_Perspectives.h>
+
namespace Ui
{
class Perspectives;
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/View/SettingTab.cpp b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/View/SettingTab.cpp
index 9b354f4bc25d698fac815850df3a128327dd8c5b..8e6ee236e98a3fd71595a4baae7d5f6343fbd0d5 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/View/SettingTab.cpp
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/View/SettingTab.cpp
@@ -21,19 +21,19 @@
*/
#include "SettingTab.h"
-SettingTab::SettingTab(QWidget* parent) :
- QWidget(parent),
- settingTab(new Ui::SettingTab)
+SettingTab::SettingTab(QWidget* parent) : QWidget(parent), settingTab(new Ui::SettingTab)
{
settingTab->setupUi(this);
}
-Ui::SettingTab* SettingTab::getSettingTab()
+Ui::SettingTab*
+SettingTab::getSettingTab()
{
return this->settingTab;
}
-void SettingTab::setSettingTab(Ui::SettingTab* settingTab)
+void
+SettingTab::setSettingTab(Ui::SettingTab* settingTab)
{
this->settingTab = settingTab;
}
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/View/SettingTab.h b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/View/SettingTab.h
index a01c5beda78464de7168d1bcd175d71b88fef00e..282a57a9f76109f0d4945884ccda288f19b65615 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/View/SettingTab.h
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/View/SettingTab.h
@@ -22,12 +22,14 @@
#ifndef SETTINGTAB_H
#define SETTINGTAB_H
-#include <RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/View/ui_SettingTab.h>
-#include <QWidget>
-#include <QListWidget>
+#include <memory>
+
#include <QComboBox>
+#include <QListWidget>
#include <QPushButton>
-#include <memory>
+#include <QWidget>
+
+#include <RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/View/ui_SettingTab.h>
namespace Ui
{
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/View/TCPInformationTab.cpp b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/View/TCPInformationTab.cpp
index 64b53a741560d5359b2cecb77f1dfae07e0768fd..68b96483121f02730814021fceca0f00b30f337f 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/View/TCPInformationTab.cpp
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/View/TCPInformationTab.cpp
@@ -22,18 +22,19 @@
#include "TCPInformationTab.h"
TCPInformationTab::TCPInformationTab(QWidget* parent) :
- QWidget(parent),
- tcpInformationTab(new Ui::TCPInformationTab)
+ QWidget(parent), tcpInformationTab(new Ui::TCPInformationTab)
{
tcpInformationTab->setupUi(this);
}
-Ui::TCPInformationTab* TCPInformationTab::getTCPInformationTab()
+Ui::TCPInformationTab*
+TCPInformationTab::getTCPInformationTab()
{
return this->tcpInformationTab;
}
-void TCPInformationTab::setTCPInformationTab(Ui::TCPInformationTab* tcpInformationTab)
+void
+TCPInformationTab::setTCPInformationTab(Ui::TCPInformationTab* tcpInformationTab)
{
this->tcpInformationTab = tcpInformationTab;
}
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/View/TCPInformationTab.h b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/View/TCPInformationTab.h
index ae49b228b254886d459dcb7eae493a6eac0df1d8..001458ce4d7f9cc534a09f97f966af83ac530bec 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/View/TCPInformationTab.h
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/View/TCPInformationTab.h
@@ -22,11 +22,13 @@
#ifndef TCPINFORMATIONTAB_H
#define TCPINFORMATIONTAB_H
-#include <RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/View/ui_TCPInformationTab.h>
-#include <QWidget>
-#include <QGridLayout>
#include <memory>
+#include <QGridLayout>
+#include <QWidget>
+
+#include <RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/View/ui_TCPInformationTab.h>
+
namespace Ui
{
class TCPInformationTab;
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/View/ToolBar.cpp b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/View/ToolBar.cpp
index 44712f9bc6f89bde1e50f3383aa3fafd11e76103..b3a0a6ed98d3acc2208b1197163f1e515c5e0bea 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/View/ToolBar.cpp
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/View/ToolBar.cpp
@@ -21,19 +21,19 @@
*/
#include "ToolBar.h"
-ToolBar::ToolBar(QWidget* parent) :
- QWidget(parent),
- toolBar(new Ui::ToolBar)
+ToolBar::ToolBar(QWidget* parent) : QWidget(parent), toolBar(new Ui::ToolBar)
{
toolBar->setupUi(this);
}
-Ui::ToolBar* ToolBar::getToolBar()
+Ui::ToolBar*
+ToolBar::getToolBar()
{
return this->toolBar;
}
-void ToolBar::setToolBar(Ui::ToolBar* toolBar)
+void
+ToolBar::setToolBar(Ui::ToolBar* toolBar)
{
this->toolBar = toolBar;
}
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/View/ToolBar.h b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/View/ToolBar.h
index f983f8315c5996180e31805ab76cc76bf0d7113f..5ef82c373d7f1a8c50b358f9d618375664626f54 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/View/ToolBar.h
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/View/ToolBar.h
@@ -22,10 +22,12 @@
#ifndef ToolBar_H
#define ToolBar_H
-#include <RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/View/ui_ToolBar.h>
-#include <QWidget>
#include <memory>
+#include <QWidget>
+
+#include <RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/View/ui_ToolBar.h>
+
namespace Ui
{
class ToolBar;
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/View/TransitionTab.cpp b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/View/TransitionTab.cpp
index 7517d5e33cb18fcf8f270b3fe64ab47160998c60..baf1bd99b7f93616cdd254a575eb25349083fee3 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/View/TransitionTab.cpp
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/View/TransitionTab.cpp
@@ -22,18 +22,19 @@
#include "TransitionTab.h"
TransitionTab::TransitionTab(QWidget* parent) :
- QWidget(parent),
- transitionTab(new Ui::TransitionTab)
+ QWidget(parent), transitionTab(new Ui::TransitionTab)
{
transitionTab->setupUi(this);
}
-Ui::TransitionTab* TransitionTab::getTransitionTab()
+Ui::TransitionTab*
+TransitionTab::getTransitionTab()
{
return this->transitionTab;
}
-void TransitionTab::setTransitionTab(Ui::TransitionTab* transitionTab)
+void
+TransitionTab::setTransitionTab(Ui::TransitionTab* transitionTab)
{
this->transitionTab = transitionTab;
}
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/View/TransitionTab.h b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/View/TransitionTab.h
index 8d9e1263e475a3bec2a5fbb509abb25a121ea4e4..2e10e14eba802c98a7fe0b32580c626d0c258230 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/View/TransitionTab.h
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/View/TransitionTab.h
@@ -22,11 +22,13 @@
#ifndef TRANSITIONTAB_H
#define TRANSITIONTAB_H
-#include <RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/View/ui_TransitionTab.h>
-#include <QWidget>
-#include <QListWidget>
#include <memory>
+#include <QListWidget>
+#include <QWidget>
+
+#include <RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/View/ui_TransitionTab.h>
+
namespace Ui
{
class TransitionTab;
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/View/WaypointTab.cpp b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/View/WaypointTab.cpp
index 88e85ebe409d47938ae1c9dea7fc28bf104b0ddf..8a9b3f13c71223d9d4edd7b84984db5109408568 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/View/WaypointTab.cpp
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/View/WaypointTab.cpp
@@ -21,19 +21,19 @@
*/
#include "WaypointTab.h"
-WaypointTab::WaypointTab(QWidget* parent) :
- QWidget(parent),
- waypointTab(new Ui::WaypointTab)
+WaypointTab::WaypointTab(QWidget* parent) : QWidget(parent), waypointTab(new Ui::WaypointTab)
{
waypointTab->setupUi(this);
}
-Ui::WaypointTab* WaypointTab::getWaypointTab()
+Ui::WaypointTab*
+WaypointTab::getWaypointTab()
{
return this->waypointTab;
}
-void WaypointTab::setWaypointTab(Ui::WaypointTab* waypointTab)
+void
+WaypointTab::setWaypointTab(Ui::WaypointTab* waypointTab)
{
this->waypointTab = waypointTab;
}
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/View/WaypointTab.h b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/View/WaypointTab.h
index 2fe57a8077cbeba9e2133c5ee8f1f9a2345ce635..dc9e970891c577003b1eeb22f3eaf490bddf535d 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/View/WaypointTab.h
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/View/WaypointTab.h
@@ -22,11 +22,13 @@
#ifndef WAYPOINTTAB_H
#define WAYPOINTTAB_H
-#include <RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/ui_WaypointTab.h>
-#include <QWidget>
-#include <QListWidget>
#include <memory>
+#include <QListWidget>
+#include <QWidget>
+
+#include <RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/ui_WaypointTab.h>
+
namespace Ui
{
class WaypointTab;
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Visualization/AbstractManipulatorVisualization.h b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Visualization/AbstractManipulatorVisualization.h
index 90dc05f398e99db05933d953879867c074d60529..27cf45f5181410992d7d48e1510c92a8b26bb8cd 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Visualization/AbstractManipulatorVisualization.h
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Visualization/AbstractManipulatorVisualization.h
@@ -22,13 +22,14 @@
#ifndef ABSTRACTMANIPULATORVISUALIZATION_H
#define ABSTRACTMANIPULATORVISUALIZATION_H
+#include <memory>
+
#include <Eigen/Eigen>
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/RobotNodeSet.h>
-#include <RobotAPI/interface/core/PoseBase.h>
-#include <memory>
+#include <RobotAPI/interface/core/PoseBase.h>
namespace armarx
{
@@ -41,11 +42,12 @@ namespace armarx
{
public:
virtual Eigen::MatrixXf getUserDesiredPose() = 0;
- virtual void setVisualization(VirtualRobot::RobotPtr robot, VirtualRobot::RobotNodeSetPtr nodeSet) = 0;
+ virtual void setVisualization(VirtualRobot::RobotPtr robot,
+ VirtualRobot::RobotNodeSetPtr nodeSet) = 0;
};
using AbstractManipulatorVisualizationPtr = std::shared_ptr<AbstractManipulatorVisualization>;
-}
+} // namespace armarx
#endif
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Visualization/AdvancedCoinVisualizationFactory.cpp b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Visualization/AdvancedCoinVisualizationFactory.cpp
index fa2fb6e9b56d6400e4045fce47edc5decd258b4d..0ff45ea1de8c5775c460330184d04a8ab93a2199 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Visualization/AdvancedCoinVisualizationFactory.cpp
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Visualization/AdvancedCoinVisualizationFactory.cpp
@@ -2,6 +2,7 @@
///ARMARX_INCLUDES
#include "ArmarXCore/core/logging/Logging.h"
+
#include "RobotAPI/libraries/core/Pose.h"
///VIRTUALROBOT_INCLUDES
@@ -14,15 +15,18 @@
using namespace armarx;
using namespace VirtualRobot;
-
AdvancedCoinVisualizationFactory::AdvancedCoinVisualizationFactory()
{
component = CoinVisualizationFactory();
}
-VisualizationNodePtr AdvancedCoinVisualizationFactory::createCurve(const std::vector<PoseBasePtr> points, bool highlight)
+
+VisualizationNodePtr
+AdvancedCoinVisualizationFactory::createCurve(const std::vector<PoseBasePtr> points, bool highlight)
{
- std::vector<VirtualRobot::VisualizationNodePtr> visualizations = std::vector<VirtualRobot::VisualizationNodePtr>();
- CoinVisualizationNodePtr parent = CoinVisualizationNodePtr(new CoinVisualizationNode(new SoSeparator()));
+ std::vector<VirtualRobot::VisualizationNodePtr> visualizations =
+ std::vector<VirtualRobot::VisualizationNodePtr>();
+ CoinVisualizationNodePtr parent =
+ CoinVisualizationNodePtr(new CoinVisualizationNode(new SoSeparator()));
for (unsigned int i = 0; i < points.size() - 1; i++)
{
PosePtr pose1 = PosePtr(new Pose(points[i]->position, points[i]->orientation));
@@ -30,11 +34,13 @@ VisualizationNodePtr AdvancedCoinVisualizationFactory::createCurve(const std::ve
VisualizationNodePtr node;
if (!highlight)
{
- node = CoinVisualizationFactory().createLine(pose1->toEigen(), pose2->toEigen(), 7.375, 254.0, 1, 1); //blue transition
+ node = CoinVisualizationFactory().createLine(
+ pose1->toEigen(), pose2->toEigen(), 7.375, 254.0, 1, 1); //blue transition
}
else
{
- node = CoinVisualizationFactory().createLine(pose1->toEigen(), pose2->toEigen(), 7.375, 0, 254.0, 0); //red transition
+ node = CoinVisualizationFactory().createLine(
+ pose1->toEigen(), pose2->toEigen(), 7.375, 0, 254.0, 0); //red transition
}
parent->attachVisualization("", node);
}
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Visualization/AdvancedCoinVisualizationFactory.h b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Visualization/AdvancedCoinVisualizationFactory.h
index 6ab16ecaaf996bce56e420d9b8352538aa5b22b8..f8ef7ba7d8ed1ea73ae322d331af1b9f77c60a91 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Visualization/AdvancedCoinVisualizationFactory.h
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Visualization/AdvancedCoinVisualizationFactory.h
@@ -22,12 +22,10 @@
#ifndef ADVANCEDCOINVISUALIZATIONFACTORY_H
#define ADVANCEDCOINVISUALIZATIONFACTORY_H
-#include "AdvancedVisualizationFactory.h"
-
-
-
#include <memory>
+#include "AdvancedVisualizationFactory.h"
+
namespace armarx
{
class AdvancedCoinVisualizationFactory : public AdvancedVisualizationFactory
@@ -41,13 +39,14 @@ namespace armarx
*/
AdvancedCoinVisualizationFactory();
- VirtualRobot::VisualizationNodePtr createCurve(const std::vector<PoseBasePtr> points, bool highlight) override;
+ VirtualRobot::VisualizationNodePtr createCurve(const std::vector<PoseBasePtr> points,
+ bool highlight) override;
//CoinManipulatorVisualizationAdapterPtr createManipulator();
};
using AdvancedCoinVisualizationFactoryPtr = std::shared_ptr<AdvancedCoinVisualizationFactory>;
-}
+} // namespace armarx
#endif
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Visualization/AdvancedVisualizationFactory.cpp b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Visualization/AdvancedVisualizationFactory.cpp
index 3d0dcb2d9e70c854fc94fdf16da32810f918bd22..feb18e7960cd86f3ff73b70e2a5cff81b70eaeee 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Visualization/AdvancedVisualizationFactory.cpp
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Visualization/AdvancedVisualizationFactory.cpp
@@ -8,10 +8,11 @@
using namespace armarx;
using namespace VirtualRobot;
-
-VisualizationNodePtr AdvancedVisualizationFactory::createCurve(std::vector<PoseBasePtr> points, bool highlight)
+VisualizationNodePtr
+AdvancedVisualizationFactory::createCurve(std::vector<PoseBasePtr> points, bool highlight)
{
- std::vector<VirtualRobot::VisualizationNodePtr> visualizations = std::vector<VirtualRobot::VisualizationNodePtr>();
+ std::vector<VirtualRobot::VisualizationNodePtr> visualizations =
+ std::vector<VirtualRobot::VisualizationNodePtr>();
VisualizationNodePtr parent = VisualizationNodePtr(new VisualizationNode());
for (unsigned int i = 0; i < points.size() - 1; i++)
{
@@ -22,4 +23,3 @@ VisualizationNodePtr AdvancedVisualizationFactory::createCurve(std::vector<PoseB
}
return parent;
}
-
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Visualization/AdvancedVisualizationFactory.h b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Visualization/AdvancedVisualizationFactory.h
index 1a26a18016fdfe185885d0a95f4b6e29f73ef275..7a982508d5f87f2942471c9107f960ffdedc9996 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Visualization/AdvancedVisualizationFactory.h
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Visualization/AdvancedVisualizationFactory.h
@@ -24,24 +24,17 @@
#include <memory>
+#include <VirtualRobot/Primitive.h>
#include <VirtualRobot/Robot.h>
-
#include <VirtualRobot/Visualization/VisualizationFactory.h>
-
-#include "AbstractManipulatorVisualization.h"
-
#include <VirtualRobot/Visualization/VisualizationNode.h>
-#include <VirtualRobot/Primitive.h>
-
+#include "AbstractManipulatorVisualization.h"
+#include "VirtualRobot/BoundingBox.h"
#include "VirtualRobot/MathTools.h"
-
#include "VirtualRobot/Trajectory.h"
-
#include "VirtualRobot/Visualization/TriMeshModel.h"
-#include "VirtualRobot/BoundingBox.h"
-
namespace armarx
{
/**
@@ -60,135 +53,285 @@ namespace armarx
* @param robot
* @return
*/
- AbstractManipulatorVisualizationPtr createManipulator(VirtualRobot::RobotNodeSetPtr kc, VirtualRobot::RobotPtr robot);
+ AbstractManipulatorVisualizationPtr createManipulator(VirtualRobot::RobotNodeSetPtr kc,
+ VirtualRobot::RobotPtr robot);
- virtual void init(int& argc, char* argv[], const std::string& appName) override
+ virtual void
+ init(int& argc, char* argv[], const std::string& appName) override
{
}
- virtual VirtualRobot::VisualizationNodePtr getVisualizationFromPrimitives(const std::vector<VirtualRobot::Primitive::PrimitivePtr>& primitives, bool boundingBox = false, Color color = Color::Gray()) override
+ virtual VirtualRobot::VisualizationNodePtr
+ getVisualizationFromPrimitives(
+ const std::vector<VirtualRobot::Primitive::PrimitivePtr>& primitives,
+ bool boundingBox = false,
+ Color color = Color::Gray()) override
{
return component.getVisualizationFromPrimitives(primitives, boundingBox, color);
}
- virtual VirtualRobot::VisualizationNodePtr getVisualizationFromFile(const std::string& filename, bool boundingBox = false, float scaleX = 1.0f, float scaleY = 1.0f, float scaleZ = 1.0f) override
+
+ virtual VirtualRobot::VisualizationNodePtr
+ getVisualizationFromFile(const std::string& filename,
+ bool boundingBox = false,
+ float scaleX = 1.0f,
+ float scaleY = 1.0f,
+ float scaleZ = 1.0f) override
{
- return component.getVisualizationFromFile(filename, boundingBox, scaleX, scaleY, scaleZ);
+ return component.getVisualizationFromFile(
+ filename, boundingBox, scaleX, scaleY, scaleZ);
}
- virtual VirtualRobot::VisualizationNodePtr getVisualizationFromFile(const std::ifstream& ifs, bool boundingBox = false, float scaleX = 1.0f, float scaleY = 1.0f, float scaleZ = 1.0f) override
+
+ virtual VirtualRobot::VisualizationNodePtr
+ getVisualizationFromFile(const std::ifstream& ifs,
+ bool boundingBox = false,
+ float scaleX = 1.0f,
+ float scaleY = 1.0f,
+ float scaleZ = 1.0f) override
{
return component.getVisualizationFromFile(ifs, boundingBox, scaleX, scaleY, scaleZ);
}
- virtual VirtualRobot::VisualizationNodePtr createBox(float width, float height, float depth, float colorR = 0.5f, float colorG = 0.5f, float colorB = 0.5f) override
+ virtual VirtualRobot::VisualizationNodePtr
+ createBox(float width,
+ float height,
+ float depth,
+ float colorR = 0.5f,
+ float colorG = 0.5f,
+ float colorB = 0.5f) override
{
return component.createBox(width, height, depth, colorR, colorG, colorB);
}
- virtual VirtualRobot::VisualizationNodePtr createLine(const Eigen::Vector3f& from, const Eigen::Vector3f& to, float width = 1.0f, float colorR = 0.5f, float colorG = 0.5f, float colorB = 0.5f) override
+
+ virtual VirtualRobot::VisualizationNodePtr
+ createLine(const Eigen::Vector3f& from,
+ const Eigen::Vector3f& to,
+ float width = 1.0f,
+ float colorR = 0.5f,
+ float colorG = 0.5f,
+ float colorB = 0.5f) override
{
return component.createLine(from, to, width, colorR, colorG, colorB);
}
- virtual VirtualRobot::VisualizationNodePtr createLine(const Eigen::Matrix4f& from, const Eigen::Matrix4f& to, float width = 1.0f, float colorR = 0.5f, float colorG = 0.5f, float colorB = 0.5f) override
+
+ virtual VirtualRobot::VisualizationNodePtr
+ createLine(const Eigen::Matrix4f& from,
+ const Eigen::Matrix4f& to,
+ float width = 1.0f,
+ float colorR = 0.5f,
+ float colorG = 0.5f,
+ float colorB = 0.5f) override
{
return component.createLine(from, to, width, colorR, colorG, colorB);
}
- virtual VirtualRobot::VisualizationNodePtr createSphere(float radius, float colorR = 0.5f, float colorG = 0.5f, float colorB = 0.5f) override
+
+ virtual VirtualRobot::VisualizationNodePtr
+ createSphere(float radius,
+ float colorR = 0.5f,
+ float colorG = 0.5f,
+ float colorB = 0.5f) override
{
return component.createSphere(radius, colorR, colorG, colorB);
}
- virtual VirtualRobot::VisualizationNodePtr createCircle(float radius, float circleCompletion, float width, float colorR = 1.0f, float colorG = 0.5f, float colorB = 0.5f, size_t numberOfCircleParts = 30) override
+
+ virtual VirtualRobot::VisualizationNodePtr
+ createCircle(float radius,
+ float circleCompletion,
+ float width,
+ float colorR = 1.0f,
+ float colorG = 0.5f,
+ float colorB = 0.5f,
+ size_t numberOfCircleParts = 30) override
{
- return component.createCircle(radius, circleCompletion, width, colorR, colorG, colorB, numberOfCircleParts);
+ return component.createCircle(
+ radius, circleCompletion, width, colorR, colorG, colorB, numberOfCircleParts);
}
- virtual VirtualRobot::VisualizationNodePtr createTorus(float radius, float tubeRadius, float completion = 1.0f, float colorR = 0.5f, float colorG = 0.5f, float colorB = 0.5f, float transparency = 0.0f, int sides = 8, int rings = 30) override
+ virtual VirtualRobot::VisualizationNodePtr
+ createTorus(float radius,
+ float tubeRadius,
+ float completion = 1.0f,
+ float colorR = 0.5f,
+ float colorG = 0.5f,
+ float colorB = 0.5f,
+ float transparency = 0.0f,
+ int sides = 8,
+ int rings = 30) override
{
- return component.createTorus(radius, tubeRadius, completion, colorR, colorG, colorB, transparency, sides, rings);
+ return component.createTorus(
+ radius, tubeRadius, completion, colorR, colorG, colorB, transparency, sides, rings);
}
- virtual VirtualRobot::VisualizationNodePtr createCircleArrow(float radius, float tubeRadius, float completion = 1, float colorR = 0.5f, float colorG = 0.5f, float colorB = 0.5f, float transparency = 0.0f, int sides = 8, int rings = 30) override
+ virtual VirtualRobot::VisualizationNodePtr
+ createCircleArrow(float radius,
+ float tubeRadius,
+ float completion = 1,
+ float colorR = 0.5f,
+ float colorG = 0.5f,
+ float colorB = 0.5f,
+ float transparency = 0.0f,
+ int sides = 8,
+ int rings = 30) override
{
- return component.createCircleArrow(radius, tubeRadius, completion, colorR, colorG, colorB, transparency, sides, rings);
+ return component.createCircleArrow(
+ radius, tubeRadius, completion, colorR, colorG, colorB, transparency, sides, rings);
}
- virtual VirtualRobot::VisualizationNodePtr createCylinder(float radius, float height, float colorR = 0.5f, float colorG = 0.5f, float colorB = 0.5f) override
+ virtual VirtualRobot::VisualizationNodePtr
+ createCylinder(float radius,
+ float height,
+ float colorR = 0.5f,
+ float colorG = 0.5f,
+ float colorB = 0.5f) override
{
return component.createCylinder(radius, height, colorR, colorG, colorB);
}
- virtual VirtualRobot::VisualizationNodePtr createCoordSystem(float scaling = 1.0f, std::string* text = NULL, float axisLength = 100.0f, float axisSize = 3.0f, int nrOfBlocks = 10) override
+
+ virtual VirtualRobot::VisualizationNodePtr
+ createCoordSystem(float scaling = 1.0f,
+ std::string* text = NULL,
+ float axisLength = 100.0f,
+ float axisSize = 3.0f,
+ int nrOfBlocks = 10) override
{
return component.createCoordSystem(scaling, text, axisLength, axisSize, nrOfBlocks);
}
- virtual VirtualRobot::VisualizationNodePtr createBoundingBox(const VirtualRobot::BoundingBox& bbox, bool wireFrame = false) override
+
+ virtual VirtualRobot::VisualizationNodePtr
+ createBoundingBox(const VirtualRobot::BoundingBox& bbox, bool wireFrame = false) override
{
return component.createBoundingBox(bbox, wireFrame);
}
- virtual VirtualRobot::VisualizationNodePtr createVertexVisualization(const Eigen::Vector3f& position, float radius, float transparency, float colorR = 0.5f, float colorG = 0.5f, float colorB = 0.5f) override
+
+ virtual VirtualRobot::VisualizationNodePtr
+ createVertexVisualization(const Eigen::Vector3f& position,
+ float radius,
+ float transparency,
+ float colorR = 0.5f,
+ float colorG = 0.5f,
+ float colorB = 0.5f) override
{
- return component.createVertexVisualization(position, radius, transparency, colorR, colorG, colorB);
+ return component.createVertexVisualization(
+ position, radius, transparency, colorR, colorG, colorB);
}
- virtual VirtualRobot::VisualizationNodePtr createTriMeshModelVisualization(VirtualRobot::TriMeshModelPtr model, Eigen::Matrix4f& pose, float scaleX = 1.0f, float scaleY = 1.0f, float scaleZ = 1.0f) override
+ virtual VirtualRobot::VisualizationNodePtr
+ createTriMeshModelVisualization(VirtualRobot::TriMeshModelPtr model,
+ Eigen::Matrix4f& pose,
+ float scaleX = 1.0f,
+ float scaleY = 1.0f,
+ float scaleZ = 1.0f) override
{
return component.createTriMeshModelVisualization(model, pose, scaleX, scaleY, scaleZ);
}
- virtual VirtualRobot::VisualizationNodePtr createTriMeshModelVisualization(VirtualRobot::TriMeshModelPtr model, bool showNormals, Eigen::Matrix4f& pose, bool showLines = true) override
+ virtual VirtualRobot::VisualizationNodePtr
+ createTriMeshModelVisualization(VirtualRobot::TriMeshModelPtr model,
+ bool showNormals,
+ Eigen::Matrix4f& pose,
+ bool showLines = true) override
{
return component.createTriMeshModelVisualization(model, showNormals, pose, showLines);
}
- virtual VirtualRobot::VisualizationNodePtr createPlane(const Eigen::Vector3f& position, const Eigen::Vector3f& normal, float extend, float transparency, float colorR = 0.5f, float colorG = 0.5f, float colorB = 0.5f) override
+
+ virtual VirtualRobot::VisualizationNodePtr
+ createPlane(const Eigen::Vector3f& position,
+ const Eigen::Vector3f& normal,
+ float extend,
+ float transparency,
+ float colorR = 0.5f,
+ float colorG = 0.5f,
+ float colorB = 0.5f) override
{
- return component.createPlane(position, normal, extend, transparency, colorR, colorG, colorB);
+ return component.createPlane(
+ position, normal, extend, transparency, colorR, colorG, colorB);
}
- virtual VirtualRobot::VisualizationNodePtr createPlane(const VirtualRobot::MathTools::Plane& plane, float extend, float transparency, float colorR = 0.5f, float colorG = 0.5f, float colorB = 0.5f) override
+
+ virtual VirtualRobot::VisualizationNodePtr
+ createPlane(const VirtualRobot::MathTools::Plane& plane,
+ float extend,
+ float transparency,
+ float colorR = 0.5f,
+ float colorG = 0.5f,
+ float colorB = 0.5f) override
{
return component.createPlane(plane, extend, transparency, colorR, colorG, colorB);
}
- virtual VirtualRobot::VisualizationNodePtr createArrow(const Eigen::Vector3f& n, float length = 50.0f, float width = 2.0f, const Color& color = Color::Gray()) override
+
+ virtual VirtualRobot::VisualizationNodePtr
+ createArrow(const Eigen::Vector3f& n,
+ float length = 50.0f,
+ float width = 2.0f,
+ const Color& color = Color::Gray()) override
{
return component.createArrow(n, length, width, color);
}
- virtual VirtualRobot::VisualizationNodePtr createTrajectory(VirtualRobot::TrajectoryPtr t, Color colorNode = Color::Blue(), Color colorLine = Color::Gray(), float nodeSize = 15.0f, float lineSize = 4.0f) override
+
+ virtual VirtualRobot::VisualizationNodePtr
+ createTrajectory(VirtualRobot::TrajectoryPtr t,
+ Color colorNode = Color::Blue(),
+ Color colorLine = Color::Gray(),
+ float nodeSize = 15.0f,
+ float lineSize = 4.0f) override
{
return component.createTrajectory(t, colorNode, colorLine, nodeSize, lineSize);
}
- virtual VirtualRobot::VisualizationNodePtr createText(const std::string& text, bool billboard = false, float scaling = 1.0f, Color c = Color::Black(), float offsetX = 20.0f, float offsetY = 20.0f, float offsetZ = 0.0f) override
+
+ virtual VirtualRobot::VisualizationNodePtr
+ createText(const std::string& text,
+ bool billboard = false,
+ float scaling = 1.0f,
+ Color c = Color::Black(),
+ float offsetX = 20.0f,
+ float offsetY = 20.0f,
+ float offsetZ = 0.0f) override
{
return component.createText(text, billboard, scaling, c, offsetX, offsetY, offsetZ);
}
- virtual VirtualRobot::VisualizationNodePtr createEllipse(float x, float y, float z, bool showAxes = true, float axesHeight = 4.0f, float axesWidth = 8.0f) override
+ virtual VirtualRobot::VisualizationNodePtr
+ createEllipse(float x,
+ float y,
+ float z,
+ bool showAxes = true,
+ float axesHeight = 4.0f,
+ float axesWidth = 8.0f) override
{
return component.createEllipse(x, y, z, showAxes, axesHeight, axesWidth);
}
- virtual void applyDisplacement(VirtualRobot::VisualizationNodePtr o, Eigen::Matrix4f& m) override {}
- virtual VirtualRobot::VisualizationNodePtr createVisualization() override
+ virtual void
+ applyDisplacement(VirtualRobot::VisualizationNodePtr o, Eigen::Matrix4f& m) override
+ {
+ }
+
+ virtual VirtualRobot::VisualizationNodePtr
+ createVisualization() override
{
return component.createVisualization();
}
- virtual VirtualRobot::VisualizationNodePtr createUnitedVisualization(const std::vector<VirtualRobot::VisualizationNodePtr>& visualizations) const override
+ virtual VirtualRobot::VisualizationNodePtr
+ createUnitedVisualization(
+ const std::vector<VirtualRobot::VisualizationNodePtr>& visualizations) const override
{
return component.createUnitedVisualization(visualizations);
}
+
/**
* @brief createCurve creates the visualization of a curve that goes through the Waypoints in transition
* @param transition the curve/transition to visualize
* @param highligt true when the Transition should be highlighted(red) , false if not (blue)
* @return the visualization node in the visualization method of this factory
*/
- virtual VirtualRobot::VisualizationNodePtr createCurve(const std::vector<PoseBasePtr> transition, bool highligt);
-
+ virtual VirtualRobot::VisualizationNodePtr
+ createCurve(const std::vector<PoseBasePtr> transition, bool highligt);
protected:
VisualizationFactory component;
-
-
};
using AdvancedVisualizationFactoryPtr = std::shared_ptr<AdvancedVisualizationFactory>;
-}
+} // namespace armarx
#endif
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Visualization/CoinManipulatorVisualizationAdapter.cpp b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Visualization/CoinManipulatorVisualizationAdapter.cpp
index 3568250a71403c4c065a821b4523d0d04a8fbd1c..07dd6122b7444d3f31475eefc4a09f4370836036 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Visualization/CoinManipulatorVisualizationAdapter.cpp
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Visualization/CoinManipulatorVisualizationAdapter.cpp
@@ -26,11 +26,9 @@
#include "../KinematicSolver.h"
-
using namespace armarx;
using namespace VirtualRobot;
-
CoinManipulatorVisualizationAdapter::CoinManipulatorVisualizationAdapter()
{
adaptedManipulation = new ManipulatorVisualization();
@@ -38,27 +36,31 @@ CoinManipulatorVisualizationAdapter::CoinManipulatorVisualizationAdapter()
CoinManipulatorVisualizationAdapter::~CoinManipulatorVisualizationAdapter()
{
-
}
-void CoinManipulatorVisualizationAdapter::setVisualization(RobotPtr robot, RobotNodeSetPtr nodeSet)
+void
+CoinManipulatorVisualizationAdapter::setVisualization(RobotPtr robot, RobotNodeSetPtr nodeSet)
{
adaptedManipulation->setVisualization(robot, nodeSet);
}
-Eigen::MatrixXf CoinManipulatorVisualizationAdapter::getUserDesiredPose()
+Eigen::MatrixXf
+CoinManipulatorVisualizationAdapter::getUserDesiredPose()
{
return adaptedManipulation->getUserDesiredPose();
}
-SoSeparator* CoinManipulatorVisualizationAdapter::getVisualization()
+SoSeparator*
+CoinManipulatorVisualizationAdapter::getVisualization()
{
- return (SoSeparator*) adaptedManipulation;
+ return (SoSeparator*)adaptedManipulation;
}
-void CoinManipulatorVisualizationAdapter::attachCallback(SoDraggerCB* finish, SoDraggerCB* moved, void* data)
+void
+CoinManipulatorVisualizationAdapter::attachCallback(SoDraggerCB* finish,
+ SoDraggerCB* moved,
+ void* data)
{
adaptedManipulation->addManipFinishCallback(finish, data);
adaptedManipulation->addManipMovedCallback(moved, data);
}
-
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Visualization/CoinManipulatorVisualizationAdapter.h b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Visualization/CoinManipulatorVisualizationAdapter.h
index cd3f5e98ae9b11fd6c25dc3f7deb9a9f73ab6b87..a7300f783feeb71cbc6ec7eed454cab2eabdc996 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Visualization/CoinManipulatorVisualizationAdapter.h
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Visualization/CoinManipulatorVisualizationAdapter.h
@@ -22,24 +22,17 @@
#ifndef COINMANIPULATORVISUALIZATIONADAPTER_H
#define COINMANIPULATORVISUALIZATIONADAPTER_H
-#include "AbstractManipulatorVisualization.h"
-
-#include "ManipulatorVisualization.h"
+#include <memory>
#include <Eigen/Eigen>
#include <VirtualRobot/Robot.h>
-
#include <VirtualRobot/RobotNodeSet.h>
+#include "AbstractManipulatorVisualization.h"
+#include "ManipulatorVisualization.h"
#include <Inventor/nodes/SoSeparator.h>
-#include <memory>
-
-
-
-
-
namespace armarx
{
/**
@@ -61,7 +54,8 @@ namespace armarx
ManipulatorVisualization* adaptedManipulation;
};
- using CoinManipulatorVisualizationAdapterPtr = std::shared_ptr<CoinManipulatorVisualizationAdapter>;
-}
+ using CoinManipulatorVisualizationAdapterPtr =
+ std::shared_ptr<CoinManipulatorVisualizationAdapter>;
+} // namespace armarx
#endif
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Visualization/CoinRobotViewerAdapter.cpp b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Visualization/CoinRobotViewerAdapter.cpp
index ef7b4b31c0522cfcdff05a7cd02f330b0012ed53..8aec20dfdd3731800d3a87f7741f0631f51e6e86 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Visualization/CoinRobotViewerAdapter.cpp
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Visualization/CoinRobotViewerAdapter.cpp
@@ -20,52 +20,38 @@
* GNU General Public License
*/
-#include "../Util/OrientationConversion.h"
#include "CoinRobotViewerAdapter.h"
-#include "Inventor/draggers/SoDragger.h"
-
-#include "Inventor/nodes/SoSeparator.h"
-
-#include <Inventor/sensors/SoTimerSensor.h>
-
-#include "VirtualRobot/Visualization/CoinVisualization/CoinVisualization.h"
-
-#include "VirtualRobot/Visualization/CoinVisualization/CoinVisualizationNode.h"
-
-#include "VirtualRobot/Scene.h"
-
#include "ArmarXCore/core/logging/Logging.h"
#include "RobotAPI/libraries/core/Pose.h"
-#include <Inventor/SoInteraction.h>
-
-#include <Inventor/actions/SoLineHighlightRenderAction.h>
-
+#include "../Util/OrientationConversion.h"
+#include "Inventor/draggers/SoDragger.h"
+#include "Inventor/nodes/SoSeparator.h"
+#include "VirtualRobot/Scene.h"
+#include "VirtualRobot/Visualization/CoinVisualization/CoinVisualization.h"
+#include "VirtualRobot/Visualization/CoinVisualization/CoinVisualizationNode.h"
#include <Inventor/Qt/SoQt.h>
-
#include <Inventor/SoFullPath.h>
-
-#include <Inventor/nodes/SoPickStyle.h>
-
+#include <Inventor/SoInteraction.h>
+#include <Inventor/actions/SoLineHighlightRenderAction.h>
+#include <Inventor/misc/SoContextHandler.h>
#include <Inventor/nodes/SoCube.h>
-
+#include <Inventor/nodes/SoPickStyle.h>
#include <Inventor/nodes/SoTranslation.h>
-
-#include <Inventor/misc/SoContextHandler.h>
+#include <Inventor/sensors/SoTimerSensor.h>
#define ROBOT_UPDATE_TIMER_MS 333
#define AUTO_FOLLOW_UPDATE 50
-
using namespace VirtualRobot;
using namespace armarx;
using namespace Qt;
-CoinRobotViewerAdapter::CoinRobotViewerAdapter(QWidget* widget): RobotVisualization()
+CoinRobotViewerAdapter::CoinRobotViewerAdapter(QWidget* widget) : RobotVisualization()
{
@@ -137,7 +123,8 @@ CoinRobotViewerAdapter::~CoinRobotViewerAdapter()
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/// METHODS FOR VISUALIZATION SETUP
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-void CoinRobotViewerAdapter::addRobotVisualization(RobotPtr robot, QString selectedChain = "")
+void
+CoinRobotViewerAdapter::addRobotVisualization(RobotPtr robot, QString selectedChain = "")
{
if (robot)
{
@@ -157,7 +144,8 @@ void CoinRobotViewerAdapter::addRobotVisualization(RobotPtr robot, QString selec
SoPickStyle* pickable = new SoPickStyle();
pickable->style = SoPickStyle::SHAPE;
viewer->getRootNode()->addChild(unpickable);
- CoinVisualizationPtr robotViewerVisualization = this->robot->getVisualization<VirtualRobot::CoinVisualization>();
+ CoinVisualizationPtr robotViewerVisualization =
+ this->robot->getVisualization<VirtualRobot::CoinVisualization>();
this->viewer->getRootNode()->addChild(robotViewerVisualization->getCoinVisualization());
RobotNodeSetPtr nodeset = robot->getRobotNodeSet(selectedChain.toStdString());
//manipulator->setVisualization(robot, nodeset);
@@ -175,23 +163,30 @@ void CoinRobotViewerAdapter::addRobotVisualization(RobotPtr robot, QString selec
}
}
-void CoinRobotViewerAdapter::addSceneVisualization(ScenePtr scene)
+void
+CoinRobotViewerAdapter::addSceneVisualization(ScenePtr scene)
{
- CoinVisualizationPtr sceneVisualization = scene->getVisualization<VirtualRobot::CoinVisualization>();
+ CoinVisualizationPtr sceneVisualization =
+ scene->getVisualization<VirtualRobot::CoinVisualization>();
this->viewer->getRootNode()->addChild(sceneVisualization->getCoinVisualization());
}
-void CoinRobotViewerAdapter::setCamera(Eigen::VectorXf position, Eigen::VectorXf pointAtA, Eigen::VectorXf pointAtB)
+void
+CoinRobotViewerAdapter::setCamera(Eigen::VectorXf position,
+ Eigen::VectorXf pointAtA,
+ Eigen::VectorXf pointAtB)
{
camera->position.setValue(position[0], position[1], position[2]);
- camera->pointAt(SbVec3f(pointAtA[0], pointAtA[1], pointAtA[2]), SbVec3f(pointAtB[0], pointAtB[1], pointAtB[2]));
+ camera->pointAt(SbVec3f(pointAtA[0], pointAtA[1], pointAtA[2]),
+ SbVec3f(pointAtB[0], pointAtB[1], pointAtB[2]));
camera->viewAll(viewer->getRootNode(), SbViewportRegion());
}
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/// METHODS FOR TRANSITIONS
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-void CoinRobotViewerAdapter::addTransitionVisualization(int index, std::vector<PoseBasePtr> transition)
+void
+CoinRobotViewerAdapter::addTransitionVisualization(int index, std::vector<PoseBasePtr> transition)
{
VisualizationNodePtr node = this->factory->createCurve(transition, false);
CoinVisualizationNodePtr visu = boost::dynamic_pointer_cast<CoinVisualizationNode>(node);
@@ -200,14 +195,17 @@ void CoinRobotViewerAdapter::addTransitionVisualization(int index, std::vector<P
transitions.insert(newTransition, index);
}
-void CoinRobotViewerAdapter::removeTransitionVisualization(int index)
+void
+CoinRobotViewerAdapter::removeTransitionVisualization(int index)
{
SoNode* toDelete = transitions[index];
viewer->getRootNode()->removeChild(toDelete);
transitions.remove(index);
}
-void CoinRobotViewerAdapter::highlightTransitionVisualization(int index, std::vector<PoseBasePtr> transition)
+void
+CoinRobotViewerAdapter::highlightTransitionVisualization(int index,
+ std::vector<PoseBasePtr> transition)
{
removeTransitionVisualization(index);
VisualizationNodePtr node = this->factory->createCurve(transition, true);
@@ -228,23 +226,29 @@ void CoinRobotViewerAdapter::highlightTransitionVisualization(int index, std::ve
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/// INHERITED BY OBSERVER
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-Eigen::Matrix4f CoinRobotViewerAdapter::getManipulatorPose()
+Eigen::Matrix4f
+CoinRobotViewerAdapter::getManipulatorPose()
{
return manipulator->getUserDesiredPose();
}
-int CoinRobotViewerAdapter::getSelectedWaypoint()
+int
+CoinRobotViewerAdapter::getSelectedWaypoint()
{
//ARMARX_INFO << std::to_string(this->selectedWaypoint);
return this->selectedWaypoint;
}
-AdvancedVisualizationFactoryPtr CoinRobotViewerAdapter::createAdvancedVisualizationFactory()
+AdvancedVisualizationFactoryPtr
+CoinRobotViewerAdapter::createAdvancedVisualizationFactory()
{
return std::make_shared<AdvancedCoinVisualizationFactory>(AdvancedCoinVisualizationFactory());
}
-void CoinRobotViewerAdapter::addWaypointVisualization(int index, PoseBasePtr waypoint, EndEffectorPtr tcp)
+void
+CoinRobotViewerAdapter::addWaypointVisualization(int index,
+ PoseBasePtr waypoint,
+ EndEffectorPtr tcp)
{
SoNode* newWaypoint;
@@ -256,7 +260,8 @@ void CoinRobotViewerAdapter::addWaypointVisualization(int index, PoseBasePtr way
endEffectorRobot->setGlobalPoseForRobotNode(eef->getTcp(), p.toEigen());
eef->getTcp()->setGlobalPoseNoChecks(p.toEigen());
- newWaypoint = CoinVisualizationFactory::getCoinVisualization(endEffectorRobot, SceneObject::VisualizationType::Full, true);
+ newWaypoint = CoinVisualizationFactory::getCoinVisualization(
+ endEffectorRobot, SceneObject::VisualizationType::Full, true);
}
else
{
@@ -265,7 +270,9 @@ void CoinRobotViewerAdapter::addWaypointVisualization(int index, PoseBasePtr way
cube->height = 0.09;
cube->depth = 0.09;
SoTranslation* cubePoint = new SoTranslation();
- cubePoint->translation.setValue(waypoint->position->x / 1000.0, waypoint->position->y / 1000.0, waypoint->position->z / 1000.0);
+ cubePoint->translation.setValue(waypoint->position->x / 1000.0,
+ waypoint->position->y / 1000.0,
+ waypoint->position->z / 1000.0);
SoSeparator* parent = new SoSeparator();
parent->addChild(cubePoint);
parent->addChild(cube);
@@ -282,7 +289,8 @@ void CoinRobotViewerAdapter::addWaypointVisualization(int index, PoseBasePtr way
viewer->getRootNode()->deselectAll();
}
-void CoinRobotViewerAdapter::removeWaypointVisualization(int index)
+void
+CoinRobotViewerAdapter::removeWaypointVisualization(int index)
{
SoNode* toDelete = wayPoints[index];
viewer->getRootNode()->removeChild(toDelete);
@@ -290,16 +298,18 @@ void CoinRobotViewerAdapter::removeWaypointVisualization(int index)
viewer->getRootNode()->deselectAll();
}
-void CoinRobotViewerAdapter::removeAllWaypointVisualizations()
+void
+CoinRobotViewerAdapter::removeAllWaypointVisualizations()
{
while (wayPoints.getLength() != 0)
{
removeWaypointVisualization(0);
}
- wayPointCounter = 0;//just to be sure
+ wayPointCounter = 0; //just to be sure
}
-void CoinRobotViewerAdapter::setSelectedWaypoint(int index)
+void
+CoinRobotViewerAdapter::setSelectedWaypoint(int index)
{
this->selectedWaypoint = index;
refreshSelectedPoint();
@@ -308,7 +318,9 @@ void CoinRobotViewerAdapter::setSelectedWaypoint(int index)
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/// SETTING MANIPULATOR
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-void CoinRobotViewerAdapter::setManipulator(VirtualRobot::RobotNodeSetPtr kc, std::vector<float>jointAngles)
+void
+CoinRobotViewerAdapter::setManipulator(VirtualRobot::RobotNodeSetPtr kc,
+ std::vector<float> jointAngles)
{
if (!kc)
{
@@ -324,22 +336,26 @@ void CoinRobotViewerAdapter::setManipulator(VirtualRobot::RobotNodeSetPtr kc, st
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/// UPDATING OF VISUALIZATION
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-void CoinRobotViewerAdapter::updateRobotVisualization()
+void
+CoinRobotViewerAdapter::updateRobotVisualization()
{
viewer->render();
}
-void CoinRobotViewerAdapter::enableVisualization()
+void
+CoinRobotViewerAdapter::enableVisualization()
{
viewer->show();
}
-void CoinRobotViewerAdapter::disableVisualization()
+void
+CoinRobotViewerAdapter::disableVisualization()
{
viewer->hide();
}
-void CoinRobotViewerAdapter::clearTrajectory()
+void
+CoinRobotViewerAdapter::clearTrajectory()
{
while (transitions.getLength() != 0)
{
@@ -349,12 +365,13 @@ void CoinRobotViewerAdapter::clearTrajectory()
{
this->removeWaypointVisualization(0);
}
-
}
-RobotVisualizationPtr CoinRobotViewerAdapter::reproduce(QWidget* parent)
+RobotVisualizationPtr
+CoinRobotViewerAdapter::reproduce(QWidget* parent)
{
- std::shared_ptr<CoinRobotViewerAdapter> reproduction = std::make_shared<CoinRobotViewerAdapter>(parent);
+ std::shared_ptr<CoinRobotViewerAdapter> reproduction =
+ std::make_shared<CoinRobotViewerAdapter>(parent);
reproduction->viewer->getRootNode()->addChild(this->viewer->getRootNode());
reproduction->camera = reproduction->viewer->getCamera();
Eigen::Vector3f position;
@@ -367,15 +384,14 @@ RobotVisualizationPtr CoinRobotViewerAdapter::reproduce(QWidget* parent)
return reproduction;
}
-
-
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///CALLBACK METHODS
/// ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-void CoinRobotViewerAdapter::manipFinishCallback(void* data, SoDragger* dragger)
+void
+CoinRobotViewerAdapter::manipFinishCallback(void* data, SoDragger* dragger)
{
CoinRobotViewerAdapter* controller = static_cast<CoinRobotViewerAdapter*>(data);
if (controller)
@@ -387,7 +403,8 @@ void CoinRobotViewerAdapter::manipFinishCallback(void* data, SoDragger* dragger)
}
}
-void CoinRobotViewerAdapter::manipMovedCallback(void* data, SoDragger* dragger)
+void
+CoinRobotViewerAdapter::manipMovedCallback(void* data, SoDragger* dragger)
{
CoinRobotViewerAdapter* controller = static_cast<CoinRobotViewerAdapter*>(data);
if (controller)
@@ -400,7 +417,8 @@ void CoinRobotViewerAdapter::manipMovedCallback(void* data, SoDragger* dragger)
}
}
-void CoinRobotViewerAdapter::autoFollowSensorTimerCB(void* data, SoSensor* sensor)
+void
+CoinRobotViewerAdapter::autoFollowSensorTimerCB(void* data, SoSensor* sensor)
{
CoinRobotViewerAdapter* controller = static_cast<CoinRobotViewerAdapter*>(data);
@@ -413,7 +431,8 @@ void CoinRobotViewerAdapter::autoFollowSensorTimerCB(void* data, SoSensor* senso
}
}
-void CoinRobotViewerAdapter::made_selection(void* data, SoPath* path)
+void
+CoinRobotViewerAdapter::made_selection(void* data, SoPath* path)
{
CoinRobotViewerAdapter* controller = static_cast<CoinRobotViewerAdapter*>(data);
if (controller)
@@ -430,7 +449,8 @@ void CoinRobotViewerAdapter::made_selection(void* data, SoPath* path)
}
}
-void CoinRobotViewerAdapter::unmade_selection(void* data, SoPath* path)
+void
+CoinRobotViewerAdapter::unmade_selection(void* data, SoPath* path)
{
CoinRobotViewerAdapter* controller = static_cast<CoinRobotViewerAdapter*>(data);
if (!controller || !controller->robot)
@@ -439,7 +459,8 @@ void CoinRobotViewerAdapter::unmade_selection(void* data, SoPath* path)
}
}
-SoPath* CoinRobotViewerAdapter::pickFilterCB(void* data, const SoPickedPoint* pick)
+SoPath*
+CoinRobotViewerAdapter::pickFilterCB(void* data, const SoPickedPoint* pick)
{
CoinRobotViewerAdapter* controller = static_cast<CoinRobotViewerAdapter*>(data);
if (!controller || !controller->robot)
@@ -469,14 +490,16 @@ SoPath* CoinRobotViewerAdapter::pickFilterCB(void* data, const SoPickedPoint* pi
return new SoPath();
}
-void CoinRobotViewerAdapter::refreshSelectedPoint()
+void
+CoinRobotViewerAdapter::refreshSelectedPoint()
{
SoNode* selectedPoint = static_cast<SoNode*>(wayPoints.get(this->selectedWaypoint));
this->viewer->getRootNode()->deselectAll();
this->viewer->getRootNode()->select(selectedPoint);
}
-void CoinRobotViewerAdapter::robotUpdateTimerCB(void* data, SoSensor* sensor)
+void
+CoinRobotViewerAdapter::robotUpdateTimerCB(void* data, SoSensor* sensor)
{
//static_cast<SoPerspectiveCamera*>(viewer->getCamera()->);
CoinRobotViewerAdapter* controller = static_cast<CoinRobotViewerAdapter*>(data);
@@ -500,7 +523,6 @@ void CoinRobotViewerAdapter::robotUpdateTimerCB(void* data, SoSensor* sensor)
{
controller->observer.lock()->refresh();
}
-
}
}
}
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Visualization/CoinRobotViewerAdapter.h b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Visualization/CoinRobotViewerAdapter.h
index 297ee3f312a7095726560f44aff9dd71b695649e..48cdeb690e1d4a2d8886e054850566dfdc8b66b8 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Visualization/CoinRobotViewerAdapter.h
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Visualization/CoinRobotViewerAdapter.h
@@ -22,16 +22,16 @@
#ifndef COINROBOTVIEWERADAPTER_H
#define COINROBOTVIEWERADAPTER_H
-#include "RobotVisualization.h"
#include "AdvancedCoinVisualizationFactory.h"
-#include "RobotViewer.h"
#include "ManipulatorVisualization.h"
+#include "RobotViewer.h"
+#include "RobotVisualization.h"
///COIN3D-INCLUDES
-#include "Inventor/draggers/SoDragger.h"
#include "Inventor/SoPath.h"
-#include "Inventor/nodes/SoSelection.h"
#include "Inventor/SoPickedPoint.h"
+#include "Inventor/draggers/SoDragger.h"
+#include "Inventor/nodes/SoSelection.h"
namespace armarx
{
@@ -41,7 +41,6 @@ namespace armarx
class CoinRobotViewerAdapter : public RobotVisualization
{
public:
-
/**
* @brief CoinRobotViewerAdapter
* @param widget the parent widget on the gui
@@ -53,17 +52,22 @@ namespace armarx
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void addRobotVisualization(VirtualRobot::RobotPtr robot, QString selectedChain) override;
void addSceneVisualization(VirtualRobot::ScenePtr scene) override;
- void setCamera(const Eigen::VectorXf position, const Eigen::VectorXf pointAtA, const Eigen::VectorXf pointAtB) override;
+ void setCamera(const Eigen::VectorXf position,
+ const Eigen::VectorXf pointAtA,
+ const Eigen::VectorXf pointAtB) override;
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/// METHODS FOR TRANSITIONS
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void addTransitionVisualization(int index, std::vector<PoseBasePtr> transition) override;
- void highlightTransitionVisualization(int index, std::vector<PoseBasePtr> transition) override;
+ void highlightTransitionVisualization(int index,
+ std::vector<PoseBasePtr> transition) override;
void removeTransitionVisualization(int index) override;
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/// METHODS FOR WAYPOINTS
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
- void addWaypointVisualization(int index, PoseBasePtr waypoint, VirtualRobot::EndEffectorPtr tcp) override;
+ void addWaypointVisualization(int index,
+ PoseBasePtr waypoint,
+ VirtualRobot::EndEffectorPtr tcp) override;
void removeWaypointVisualization(int index) override;
void removeAllWaypointVisualizations() override;
void setSelectedWaypoint(int index) override;
@@ -73,7 +77,8 @@ namespace armarx
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/// SETTING MANIPULATOR
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
- void setManipulator(VirtualRobot::RobotNodeSetPtr kc, std::vector<float>jointAngles) override;
+ void setManipulator(VirtualRobot::RobotNodeSetPtr kc,
+ std::vector<float> jointAngles) override;
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/// UPDATING OF VISUALIZATION
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
@@ -92,10 +97,9 @@ namespace armarx
AdvancedVisualizationFactoryPtr createAdvancedVisualizationFactory() override;
private:
-
AdvancedCoinVisualizationFactoryPtr factory; //produces Visualizations
std::shared_ptr<RobotViewer> viewer; //adaptedClass
- SoDragger* dragger;//muss vor manipCallback initialisiert werden
+ SoDragger* dragger; //muss vor manipCallback initialisiert werden
void refreshSelectedPoint();
SoNodeList transitions;
SoNodeList wayPoints;
@@ -115,7 +119,7 @@ namespace armarx
SoTimerSensor* robotUpdateSensor;
static void robotUpdateTimerCB(void* data, SoSensor* sensor);
bool startUpCameraPositioningFlag = true;
- SoCamera* camera;//pointer to camera of viewer to change perspective easyly
+ SoCamera* camera; //pointer to camera of viewer to change perspective easyly
//Selection utils
SoSelection* selected;
@@ -123,7 +127,7 @@ namespace armarx
static void unmade_selection(void* data, SoPath* path);
static SoPath* pickFilterCB(void* data, const SoPickedPoint* pick);
};
-}
+} // namespace armarx
#endif
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Visualization/DesignerTrajectoryPlayer.cpp b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Visualization/DesignerTrajectoryPlayer.cpp
index df10316cfcd45d53b532d761f21c33144fce10ab..4834ff127623ce38c5360a20f25a1972a20040ae 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Visualization/DesignerTrajectoryPlayer.cpp
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Visualization/DesignerTrajectoryPlayer.cpp
@@ -4,11 +4,11 @@
#include <QTimer>
-
using namespace armarx;
using namespace VirtualRobot;
-DesignerTrajectoryPlayer::DesignerTrajectoryPlayer(RobotVisualizationPtr viewer, VirtualRobot::RobotPtr robot)
+DesignerTrajectoryPlayer::DesignerTrajectoryPlayer(RobotVisualizationPtr viewer,
+ VirtualRobot::RobotPtr robot)
{
this->viewer = viewer;
this->robot = robot;
@@ -18,7 +18,8 @@ DesignerTrajectoryPlayer::DesignerTrajectoryPlayer(RobotVisualizationPtr viewer,
this->timeOptimalTrajectories = std::vector<TrajectoryPtr>();
}
-void DesignerTrajectoryPlayer::updateLoop()
+void
+DesignerTrajectoryPlayer::updateLoop()
{
bool endReached = true;
int i = 0;
@@ -45,15 +46,16 @@ void DesignerTrajectoryPlayer::updateLoop()
{
emit finishedPlayback();
}
-
}
-void DesignerTrajectoryPlayer::addTrajectory(armarx::DesignerTrajectoryPtr trajectory)
+void
+DesignerTrajectoryPlayer::addTrajectory(armarx::DesignerTrajectoryPtr trajectory)
{
this->trajectories.push_back(trajectory);
}
-void DesignerTrajectoryPlayer::startPlayback()
+void
+DesignerTrajectoryPlayer::startPlayback()
{
//Setup timer
timer = std::shared_ptr<QTimer>(new QTimer(this));
@@ -78,9 +80,8 @@ void DesignerTrajectoryPlayer::startPlayback()
timer->start(1000.0 / fps);
}
-void DesignerTrajectoryPlayer::setFPS(int fps)
+void
+DesignerTrajectoryPlayer::setFPS(int fps)
{
this->fps = fps;
}
-
-
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Visualization/DesignerTrajectoryPlayer.h b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Visualization/DesignerTrajectoryPlayer.h
index 602dad2929c1420fb7db3c13d2fec7e6338f83ad..4babdc96cd42a408a9414b786fa94b4eaaa466c1 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Visualization/DesignerTrajectoryPlayer.h
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Visualization/DesignerTrajectoryPlayer.h
@@ -23,14 +23,11 @@
#define DESIGNERTRAJECTORYPLAYER_H
//this include needs to be here so there is not problem with a Q_FOREACH macro
-#include "../Controller/AbstractController.h"
-
#include <memory>
-#include "RobotVisualization.h"
+#include "../Controller/AbstractController.h"
#include "../Model/DesignerTrajectory.h"
-
-
+#include "RobotVisualization.h"
namespace armarx
{
@@ -79,13 +76,10 @@ namespace armarx
std::vector<TrajectoryPtr> timeOptimalTrajectories;
double currentTime;
int fps;
-
-
};
- using DesignerTrajectoryPlayerPtr = std::shared_ptr<DesignerTrajectoryPlayer>;
-}
-
+ using DesignerTrajectoryPlayerPtr = std::shared_ptr<DesignerTrajectoryPlayer>;
+} // namespace armarx
#endif
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Visualization/ManipulatorVisualization.cpp b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Visualization/ManipulatorVisualization.cpp
index a0c2783c753e951cffe9bc5c1d1400766fac6974..41abc500c7acc82dcf01e57bb9af6a6694855fe6 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Visualization/ManipulatorVisualization.cpp
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Visualization/ManipulatorVisualization.cpp
@@ -24,20 +24,23 @@
#include "ManipulatorVisualization.h"
//Virtual Robot includes
-#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
-#include <VirtualRobot/XML/RobotIO.h>
#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>
+#include <VirtualRobot/XML/RobotIO.h>
+
+#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
/* Coin includes */
+#include <iostream>
+
+#include <Inventor/SbViewportRegion.h>
#include <Inventor/actions/SoGetMatrixAction.h>
#include <Inventor/actions/SoSearchAction.h>
-#include <Inventor/SbViewportRegion.h>
#include <Inventor/nodes/SoCube.h>
-
-#include <iostream>
-
-ManipulatorVisualization::ManipulatorVisualization() : isVisualizing(false), hasEndEffectorVisualizer(false), localTransformation(Eigen::Matrix4f::Identity())
+ManipulatorVisualization::ManipulatorVisualization() :
+ isVisualizing(false),
+ hasEndEffectorVisualizer(false),
+ localTransformation(Eigen::Matrix4f::Identity())
{
//Im gonna live forever :)
this->ref();
@@ -49,7 +52,9 @@ ManipulatorVisualization::~ManipulatorVisualization()
this->unref();
}
-void ManipulatorVisualization::setVisualization(VirtualRobot::RobotPtr robot, VirtualRobot::RobotNodeSetPtr nodeSet)
+void
+ManipulatorVisualization::setVisualization(VirtualRobot::RobotPtr robot,
+ VirtualRobot::RobotNodeSetPtr nodeSet)
{
//Completely forget anything we displayed earlier
//This also works when this is the first time we display something
@@ -82,7 +87,8 @@ void ManipulatorVisualization::setVisualization(VirtualRobot::RobotPtr robot, Vi
if (this->hasEndEffectorVisualizer)
{
VirtualRobot::RobotPtr endEffectorRobot = endEffector->createEefRobot("", "");
- CoinVisualizationPtr endEffectorVisualization = endEffectorRobot->getVisualization<VirtualRobot::CoinVisualization>();
+ CoinVisualizationPtr endEffectorVisualization =
+ endEffectorRobot->getVisualization<VirtualRobot::CoinVisualization>();
this->addChild(endEffectorVisualization->getCoinVisualization());
}
else
@@ -137,7 +143,8 @@ void ManipulatorVisualization::setVisualization(VirtualRobot::RobotPtr robot, Vi
this->isVisualizing = true;
}
-void ManipulatorVisualization::removeVisualization()
+void
+ManipulatorVisualization::removeVisualization()
{
//Remove all children and reset manip pointer
//This should bring ref counter of Inventor down to zero and free memory
@@ -146,10 +153,11 @@ void ManipulatorVisualization::removeVisualization()
this->isVisualizing = false;
this->hasEndEffectorVisualizer = false;
- this->localTransformation = Eigen::Matrix4f::Identity();
+ this->localTransformation = Eigen::Matrix4f::Identity();
}
-void ManipulatorVisualization::setColor(float r, float g, float b)
+void
+ManipulatorVisualization::setColor(float r, float g, float b)
{
if (this->getIsVisualizing())
{
@@ -157,7 +165,8 @@ void ManipulatorVisualization::setColor(float r, float g, float b)
}
}
-void ManipulatorVisualization::addManipFinishCallback(SoDraggerCB* func, void* data)
+void
+ManipulatorVisualization::addManipFinishCallback(SoDraggerCB* func, void* data)
{
if (this->getIsVisualizing())
{
@@ -165,7 +174,8 @@ void ManipulatorVisualization::addManipFinishCallback(SoDraggerCB* func, void* d
}
}
-void ManipulatorVisualization::addManipMovedCallback(SoDraggerCB* func, void* data)
+void
+ManipulatorVisualization::addManipMovedCallback(SoDraggerCB* func, void* data)
{
if (this->getIsVisualizing())
{
@@ -173,15 +183,16 @@ void ManipulatorVisualization::addManipMovedCallback(SoDraggerCB* func, void* da
}
}
-Eigen::Matrix4f ManipulatorVisualization::getUserDesiredPose()
+Eigen::Matrix4f
+ManipulatorVisualization::getUserDesiredPose()
{
if (this->getIsVisualizing())
{
SoGetMatrixAction* action = new SoGetMatrixAction(SbViewportRegion());
SoSearchAction sa;
sa.setNode(manip.get());
- sa.setSearchingAll(TRUE); // Search all nodes
- SoBaseKit::setSearchingChildren(TRUE); // Even inside nodekits
+ sa.setSearchingAll(TRUE); // Search all nodes
+ SoBaseKit::setSearchingChildren(TRUE); // Even inside nodekits
sa.apply(this);
action->apply(sa.getPath());
@@ -223,7 +234,8 @@ Eigen::Matrix4f ManipulatorVisualization::getUserDesiredPose()
return Eigen::Matrix4f::Identity();
}
-std::string ManipulatorVisualization::getUserDesiredPoseString()
+std::string
+ManipulatorVisualization::getUserDesiredPoseString()
{
Eigen::Matrix4f mat = this->getUserDesiredPose();
@@ -232,6 +244,3 @@ std::string ManipulatorVisualization::getUserDesiredPoseString()
buffer << mat;
return buffer.str();
}
-
-
-
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Visualization/ManipulatorVisualization.h b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Visualization/ManipulatorVisualization.h
index 424a94716d70035f59f90fe8bab7a25b426f768e..280b6338bfb4956bb7eb98a35dddc0bb5cf52a44 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Visualization/ManipulatorVisualization.h
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Visualization/ManipulatorVisualization.h
@@ -25,9 +25,9 @@
#define ManipulatorVisualization_H
//Coin includes
-#include <Inventor/nodes/SoSeparator.h>
#include <Inventor/manips/SoTransformerManip.h>
#include <Inventor/nodes/SoMaterial.h>
+#include <Inventor/nodes/SoSeparator.h>
//VirtualRobot
#include <VirtualRobot/Robot.h>
@@ -40,11 +40,15 @@
//Let boost use referencing of Inventor to manage objects memory
//and not its own shared pointer referencing
using SoTransformerManipPtr = boost::intrusive_ptr<SoTransformerManip>;
-inline void intrusive_ptr_add_ref(SoTransformerManip* obj)
+
+inline void
+intrusive_ptr_add_ref(SoTransformerManip* obj)
{
obj->ref();
}
-inline void intrusive_ptr_release(SoTransformerManip* obj)
+
+inline void
+intrusive_ptr_release(SoTransformerManip* obj)
{
obj->unref();
}
@@ -68,7 +72,8 @@ public:
Eigen::Matrix4f getUserDesiredPose();
std::string getUserDesiredPoseString();
- bool getIsVisualizing()
+ bool
+ getIsVisualizing()
{
return isVisualizing;
}
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Visualization/RobotViewer.cpp b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Visualization/RobotViewer.cpp
index d4473c8cac467f77fe604ceeb329b2d374e25d84..e98947e6eef810809ed3beaa8b5fa95873156c45 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Visualization/RobotViewer.cpp
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Visualization/RobotViewer.cpp
@@ -23,20 +23,23 @@
*/
#include "RobotViewer.h"
-#include <Inventor/nodes/SoSeparator.h>
-#include <Inventor/nodes/SoSelection.h>
-#include <Inventor/nodes/SoPickStyle.h>
-#include <Inventor/nodes/SoMaterial.h>
-#include <Inventor/nodes/SoDrawStyle.h>
-#include <Inventor/nodes/SoVertexProperty.h>
-#include <Inventor/nodes/SoLineSet.h>
+#include <ArmarXCore/core/logging/Logging.h>
-#include <Inventor/events/SoMouseButtonEvent.h>
#include <Inventor/events/SoLocation2Event.h>
+#include <Inventor/events/SoMouseButtonEvent.h>
+#include <Inventor/nodes/SoDrawStyle.h>
+#include <Inventor/nodes/SoLineSet.h>
+#include <Inventor/nodes/SoMaterial.h>
+#include <Inventor/nodes/SoPickStyle.h>
+#include <Inventor/nodes/SoSelection.h>
+#include <Inventor/nodes/SoSeparator.h>
+#include <Inventor/nodes/SoVertexProperty.h>
-#include <ArmarXCore/core/logging/Logging.h>
-
-armarx::RobotViewer::RobotViewer(QWidget* widget) : SoQtExaminerViewer(widget), sceneRootNode(new SoSeparator), contentRootNode(new SoSelection), camera(new SoPerspectiveCamera)
+armarx::RobotViewer::RobotViewer(QWidget* widget) :
+ SoQtExaminerViewer(widget),
+ sceneRootNode(new SoSeparator),
+ contentRootNode(new SoSelection),
+ camera(new SoPerspectiveCamera)
{
this->setBackgroundColor(SbColor(150 / 255.0f, 150 / 255.0f, 150 / 255.0f));
this->setAccumulationBuffer(true);
@@ -162,12 +165,16 @@ armarx::RobotViewer::RobotViewer(QWidget* widget) : SoQtExaminerViewer(widget),
for (int n = 1; n < GRIDSUBSIZE; n++)
{
- subVertices->vertex.set1Value(subverticescounter++, GRIDSIZE * GRIDUNIT, (float)i + (float)n * delta, 0);
- subVertices->vertex.set1Value(subverticescounter++, -(GRIDSIZE * GRIDUNIT), (float)i + (float)n * delta, 0);
+ subVertices->vertex.set1Value(
+ subverticescounter++, GRIDSIZE * GRIDUNIT, (float)i + (float)n * delta, 0);
+ subVertices->vertex.set1Value(
+ subverticescounter++, -(GRIDSIZE * GRIDUNIT), (float)i + (float)n * delta, 0);
subLines->numVertices.set1Value((subverticescounter - 1) / 2, 2);
- subVertices->vertex.set1Value(subverticescounter++, (float)i + (float)n * delta, GRIDSIZE * GRIDUNIT, 0);
- subVertices->vertex.set1Value(subverticescounter++, (float)i + (float)n * delta, -(GRIDSIZE * GRIDUNIT), 0);
+ subVertices->vertex.set1Value(
+ subverticescounter++, (float)i + (float)n * delta, GRIDSIZE * GRIDUNIT, 0);
+ subVertices->vertex.set1Value(
+ subverticescounter++, (float)i + (float)n * delta, -(GRIDSIZE * GRIDUNIT), 0);
subLines->numVertices.set1Value((subverticescounter - 1) / 2, 2);
}
}
@@ -203,27 +210,28 @@ armarx::RobotViewer::~RobotViewer()
sceneRootNode->unref();
}
-SoSelection* armarx::RobotViewer::getRootNode()
+SoSelection*
+armarx::RobotViewer::getRootNode()
{
return this->contentRootNode;
}
-void armarx::RobotViewer::cameraViewAll()
+void
+armarx::RobotViewer::cameraViewAll()
{
camera->viewAll(this->contentRootNode, SbViewportRegion());
}
-
-
//Override the default navigation behaviour of the SoQtExaminerViewer
-SbBool armarx::RobotViewer::processSoEvent(const SoEvent* const event)
+SbBool
+armarx::RobotViewer::processSoEvent(const SoEvent* const event)
{
const SoType type(event->getTypeId());
//Remapping mouse press events
if (type.isDerivedFrom(SoMouseButtonEvent::getClassTypeId()))
{
- SoMouseButtonEvent* const ev = (SoMouseButtonEvent*) event;
+ SoMouseButtonEvent* const ev = (SoMouseButtonEvent*)event;
const int button = ev->getButton();
const SbBool press = ev->getState() == SoButtonEvent::DOWN ? TRUE : FALSE;
@@ -278,8 +286,8 @@ SbBool armarx::RobotViewer::processSoEvent(const SoEvent* const event)
on to make SoQtExaminerViewer allow zooming.*/
//Swap BUTTON4 and BUTTON5 because zooming out while scrolling up is just retarded
- ev->setButton(button == SoMouseButtonEvent::BUTTON4 ?
- SoMouseButtonEvent::BUTTON5 : SoMouseButtonEvent::BUTTON4);
+ ev->setButton(button == SoMouseButtonEvent::BUTTON4 ? SoMouseButtonEvent::BUTTON5
+ : SoMouseButtonEvent::BUTTON4);
//Zooming is allowed, so just pass it and temporarily set viewing mode on, if it is not already
//(otherwise coin gives us warning messages...)
@@ -309,13 +317,14 @@ SbBool armarx::RobotViewer::processSoEvent(const SoEvent* const event)
// Keyboard handling
if (type.isDerivedFrom(SoKeyboardEvent::getClassTypeId()))
{
- const SoKeyboardEvent* const ev = (const SoKeyboardEvent*) event;
+ const SoKeyboardEvent* const ev = (const SoKeyboardEvent*)event;
/*The escape key and super key (windows key) is used to switch between
viewing modes. We need to disable this behaviour completely.*/
//65513 seems to be the super key, which is not available in the enum of keys in coin....
- if (ev->getKey() == SoKeyboardEvent::ESCAPE || ev->getKey() == 65513 || ev->getKey() == SoKeyboardEvent::LEFT_CONTROL)
+ if (ev->getKey() == SoKeyboardEvent::ESCAPE || ev->getKey() == 65513 ||
+ ev->getKey() == SoKeyboardEvent::LEFT_CONTROL)
{
return TRUE;
}
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Visualization/RobotViewer.h b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Visualization/RobotViewer.h
index fb2ff31c5b961a256d14429eeb8c0637d694c5a4..20de312c8b24e8a0a9b68f55e0f735706d4bc5ec 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Visualization/RobotViewer.h
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Visualization/RobotViewer.h
@@ -26,8 +26,9 @@
/* Coin headers */
#include <Qt/qapplication.h>
-#include <Inventor/nodes/SoPerspectiveCamera.h>
+
#include <Inventor/Qt/viewers/SoQtExaminerViewer.h>
+#include <Inventor/nodes/SoPerspectiveCamera.h>
namespace armarx
{
@@ -35,18 +36,18 @@ namespace armarx
class RobotViewer : public SoQtExaminerViewer
{
public:
-
RobotViewer(QWidget* widget);
~RobotViewer();
SoSelection* getRootNode();
void cameraViewAll();
void enableLeftMouseButton(bool enable);
+
private:
virtual SbBool processSoEvent(const SoEvent* const event) override;
SoSeparator* sceneRootNode;
SoSelection* contentRootNode;
SoPerspectiveCamera* camera;
};
-}
+} // namespace armarx
#endif
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Visualization/RobotViewerController.h b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Visualization/RobotViewerController.h
index 6c3955c77c9d7dea548e16594442ab49f339371a..0df32a662f6b4407af0e6ea169d72541d92005cd 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Visualization/RobotViewerController.h
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Visualization/RobotViewerController.h
@@ -31,6 +31,6 @@ namespace Controllers
public:
RobotViewerController();
};
-}
+} // namespace Controllers
#endif
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Visualization/RobotVisualization.h b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Visualization/RobotVisualization.h
index 72b7432bfca97a0a1a2fd1ddd1c504718c3181d5..e528baf35ec70f05ad92855ebea2377e24442e15 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Visualization/RobotVisualization.h
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Visualization/RobotVisualization.h
@@ -22,14 +22,13 @@
#ifndef ROBOTVISUALIZATION_H
#define ROBOTVISUALIZATION_H
-#include <RobotAPI/interface/core/PoseBase.h>
+#include <QWidget>
#include <VirtualRobot/RobotNodeSet.h>
-#include "AdvancedVisualizationFactory.h"
-
-#include <QWidget>
+#include <RobotAPI/interface/core/PoseBase.h>
+#include "AdvancedVisualizationFactory.h"
#include "VisualizationSubject.h"
namespace armarx
@@ -62,7 +61,9 @@ namespace armarx
* @param pointAtA the highest point the camera points at
* @param pointAtB the lowest point the camera looks at
*/
- virtual void setCamera(const Eigen::VectorXf position, const Eigen::VectorXf pointAtA, const Eigen::VectorXf pointAtB) = 0;
+ virtual void setCamera(const Eigen::VectorXf position,
+ const Eigen::VectorXf pointAtA,
+ const Eigen::VectorXf pointAtB) = 0;
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/// METHODS FOR TRANSITIONS
@@ -78,7 +79,8 @@ namespace armarx
* @param index the index of the transition to be highlighted starting with 0
* @param transition the transition to be highlighted
*/
- virtual void highlightTransitionVisualization(int index, std::vector<PoseBasePtr> transition) = 0;
+ virtual void highlightTransitionVisualization(int index,
+ std::vector<PoseBasePtr> transition) = 0;
/**
* @brief removeTransitionVisualization removes visualization of a certain transition
@@ -95,7 +97,9 @@ namespace armarx
* @param waypoint Pose that should be realized by the WaypointVisualization
* @param tcp the Endeffector that should be visualized
*/
- virtual void addWaypointVisualization(int index, PoseBasePtr waypoint, VirtualRobot::EndEffectorPtr tcp) = 0;
+ virtual void addWaypointVisualization(int index,
+ PoseBasePtr waypoint,
+ VirtualRobot::EndEffectorPtr tcp) = 0;
/**
* @brief removeWaypointVisualization removes the Waypoint with index so that it is no longer visualized
@@ -123,7 +127,8 @@ namespace armarx
* @param kc the kinematic chain to use the endeffector of as the manipulator
* @param jointAngles the joint angles of the robot to reach the manipulator
*/
- virtual void setManipulator(VirtualRobot::RobotNodeSetPtr kc, std::vector<float>jointAngles) = 0;
+ virtual void setManipulator(VirtualRobot::RobotNodeSetPtr kc,
+ std::vector<float> jointAngles) = 0;
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/// UPDATING OF VISUALIZATION
@@ -155,11 +160,10 @@ namespace armarx
VirtualRobot::ScenePtr scene;
VirtualRobot::RobotPtr robot;
virtual AdvancedVisualizationFactoryPtr createAdvancedVisualizationFactory() = 0;
-
};
using RobotVisualizationPtr = std::shared_ptr<RobotVisualization>;
-}
+} // namespace armarx
#endif
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Visualization/RobotVisualizationWidget.cpp b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Visualization/RobotVisualizationWidget.cpp
index 8299ff3ad3f79cb5e5c1e7f276709f779297ce9a..338b2fba98c8c9765ae0eeca5db0a579068f455e 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Visualization/RobotVisualizationWidget.cpp
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Visualization/RobotVisualizationWidget.cpp
@@ -23,13 +23,16 @@
*/
#include "RobotVisualizationWidget.h"
-#include "CoinRobotViewerAdapter.h"
-
#include <ArmarXCore/core/logging/Logging.h>
+#include "CoinRobotViewerAdapter.h"
+
using namespace armarx;
-RobotVisualizationWidget::RobotVisualizationWidget(QWidget* parent, QWidget* viewerWidget, RobotVisualizationPtr viewer) : QSplitter(parent)
+RobotVisualizationWidget::RobotVisualizationWidget(QWidget* parent,
+ QWidget* viewerWidget,
+ RobotVisualizationPtr viewer) :
+ QSplitter(parent)
{
//this->setOpaqueResize(true);
//initialize Attriutes
@@ -43,20 +46,23 @@ RobotVisualizationWidget::RobotVisualizationWidget(QWidget* parent, QWidget* vie
this->setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Preferred);
//Add callback for viewer selection from Main-Application
- QObject::connect(QApplication::instance(), SIGNAL(focusChanged(QWidget*, QWidget*)), this, SLOT(activeWidgetChanged(QWidget*, QWidget*)));
+ QObject::connect(QApplication::instance(),
+ SIGNAL(focusChanged(QWidget*, QWidget*)),
+ this,
+ SLOT(activeWidgetChanged(QWidget*, QWidget*)));
//Add original viewer to this widget
viewerWidget->setFocusPolicy(Qt::FocusPolicy::WheelFocus);
this->leftSplitter->addWidget(viewerWidget);
}
-
RobotVisualizationWidget::~RobotVisualizationWidget()
{
ARMARX_WARNING << "Destroyed ViewerWidget";
}
-void RobotVisualizationWidget::addWidget()
+void
+RobotVisualizationWidget::addWidget()
{
if (childrenCounter < 4)
{
@@ -87,10 +93,10 @@ void RobotVisualizationWidget::addWidget()
}
childrenCounter++;
}
-
}
-void RobotVisualizationWidget::removeWidget()
+void
+RobotVisualizationWidget::removeWidget()
{
//Remove Viewer Left of original Viewer
if (childrenCounter == 2)
@@ -123,7 +129,6 @@ void RobotVisualizationWidget::removeWidget()
clones.pop_back();
leftSplitter->children().at(0)->~QObject();
childrenCounter--;
-
}
}
if (selectedViewer == childrenCounter)
@@ -132,7 +137,10 @@ void RobotVisualizationWidget::removeWidget()
}
}
-void RobotVisualizationWidget::setCameraOfFocused(Eigen::Vector3f position, Eigen::Vector3f pointAtA, Eigen::Vector3f pointAtB)
+void
+RobotVisualizationWidget::setCameraOfFocused(Eigen::Vector3f position,
+ Eigen::Vector3f pointAtA,
+ Eigen::Vector3f pointAtB)
{
if (selectedViewer == 0)
{
@@ -144,10 +152,10 @@ void RobotVisualizationWidget::setCameraOfFocused(Eigen::Vector3f position, Eige
RobotVisualizationPtr clone = clones[selectedViewer - 1];
clone->setCamera(position, pointAtA, pointAtB);
}
-
}
-void RobotVisualizationWidget::activeWidgetChanged(QWidget* old, QWidget* now)
+void
+RobotVisualizationWidget::activeWidgetChanged(QWidget* old, QWidget* now)
{
if (!now)
{
@@ -159,14 +167,19 @@ void RobotVisualizationWidget::activeWidgetChanged(QWidget* old, QWidget* now)
now = now->nextInFocusChain();
}*/
- if (now == viewerWidget || (now->parentWidget() && now->parentWidget()->parentWidget() && now->parentWidget()->parentWidget()->parentWidget() && now->parentWidget()->parentWidget()->parentWidget() == viewerWidget))
+ if (now == viewerWidget ||
+ (now->parentWidget() && now->parentWidget()->parentWidget() &&
+ now->parentWidget()->parentWidget()->parentWidget() &&
+ now->parentWidget()->parentWidget()->parentWidget() == viewerWidget))
{
selectedViewer = 0;
}
int j = 1;
for (QWidget* clone : cloneWidgets)
{
- if (now == clone || (now->parentWidget() && now->parentWidget()->parentWidget() && now->parentWidget()->parentWidget()->parentWidget() && now->parentWidget()->parentWidget()->parentWidget() == clone))
+ if (now == clone || (now->parentWidget() && now->parentWidget()->parentWidget() &&
+ now->parentWidget()->parentWidget()->parentWidget() &&
+ now->parentWidget()->parentWidget()->parentWidget() == clone))
{
this->selectedViewer = j;
return;
@@ -190,4 +203,3 @@ void RobotVisualizationWidget::activeWidgetChanged(QWidget* old, QWidget* now)
}
}*/
}
-
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Visualization/RobotVisualizationWidget.h b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Visualization/RobotVisualizationWidget.h
index 4cd0e353b22fbe27d72f76fae79b5c4f1e37a8d2..09c3d10352d79d8efd5ac39cc308e7c04b46a2b6 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Visualization/RobotVisualizationWidget.h
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Visualization/RobotVisualizationWidget.h
@@ -25,8 +25,8 @@
#define RobotVisualizationWidget_h
/* Qt headers */
-#include <QWidget>
#include <QSplitter>
+#include <QWidget>
#include "RobotVisualization.h"
@@ -51,7 +51,9 @@ namespace armarx
* @param parent parent widget
*
*/
- explicit RobotVisualizationWidget(QWidget* parent = 0, QWidget* viewerWidget = 0, RobotVisualizationPtr viewer = 0);
+ explicit RobotVisualizationWidget(QWidget* parent = 0,
+ QWidget* viewerWidget = 0,
+ RobotVisualizationPtr viewer = 0);
/**
* Destructor.
@@ -77,7 +79,9 @@ namespace armarx
* @param pointAtA the lower point to point the camera at
* @param pointAtB the upper point to point the camera at
*/
- void setCameraOfFocused(Eigen::Vector3f position, Eigen::Vector3f pointAtA, Eigen::Vector3f pointAtB);
+ void setCameraOfFocused(Eigen::Vector3f position,
+ Eigen::Vector3f pointAtA,
+ Eigen::Vector3f pointAtB);
private slots:
@@ -95,6 +99,6 @@ namespace armarx
};
using RobotVisualizationWidgetPtr = std::shared_ptr<RobotVisualizationWidget>;
-}
+} // namespace armarx
#endif
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Visualization/VisualizationObserver.h b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Visualization/VisualizationObserver.h
index 1b388472ff23f8e29c7e1c1e61b6a91751b14429..d4ffbd1a5234250d30022e18ed6f364c814fc3cf 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Visualization/VisualizationObserver.h
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Visualization/VisualizationObserver.h
@@ -24,7 +24,6 @@
#include <memory>
-
namespace armarx
{
/**
@@ -43,6 +42,6 @@ namespace armarx
};
using VisualizationObserverPtr = std::shared_ptr<VisualizationObserver>;
-}
+} // namespace armarx
#endif
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Visualization/VisualizationSubject.cpp b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Visualization/VisualizationSubject.cpp
index 24cbee823914731a26b757a7765d0b1daafb5ec5..9e2c6a7d2fc6a72f2711f89586938a624321f018 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Visualization/VisualizationSubject.cpp
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Visualization/VisualizationSubject.cpp
@@ -4,12 +4,14 @@
using namespace armarx;
-void VisualizationSubject::setObserver(VisualizationObserverPtr observer)
+void
+VisualizationSubject::setObserver(VisualizationObserverPtr observer)
{
- this->observer = std::weak_ptr<VisualizationObserver> (observer);
+ this->observer = std::weak_ptr<VisualizationObserver>(observer);
}
-void VisualizationSubject::removeObservers()
+void
+VisualizationSubject::removeObservers()
{
//
}
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Visualization/VisualizationSubject.h b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Visualization/VisualizationSubject.h
index a1c0429409f8c5ea5dd63cb16daa32b21ae2831f..ec8a3721baaf272d68be2f1c375a5cfae8ace722 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Visualization/VisualizationSubject.h
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/Visualization/VisualizationSubject.h
@@ -22,10 +22,9 @@
#ifndef VisualizationSubject_H
#define VisualizationSubject_H
-#include "VisualizationObserver.h"
-
#include <Eigen/Eigen>
+#include "VisualizationObserver.h"
namespace armarx
{
@@ -59,6 +58,6 @@ namespace armarx
protected:
std::weak_ptr<VisualizationObserver> observer;
};
-}
+} // namespace armarx
#endif
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/exceptions/ControlPointsInterpolationMatchException.h b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/exceptions/ControlPointsInterpolationMatchException.h
index 6f2d204ff8f68727138ce2963561b9a66cd4484f..3c1954382f45c4ffdaf8e4562e23b89c8dd4b45d 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/exceptions/ControlPointsInterpolationMatchException.h
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/exceptions/ControlPointsInterpolationMatchException.h
@@ -25,12 +25,10 @@
#ifndef ControlPointsInterpolationMatchException_H
#define ControlPointsInterpolationMatchException_H
-#include "ArmarXCore/core/exceptions/Exception.h"
-
-#include "ArmarXCore/core/exceptions/local/ValueRangeExceededException.h"
-
#include <string>
+#include "ArmarXCore/core/exceptions/Exception.h"
+#include "ArmarXCore/core/exceptions/local/ValueRangeExceededException.h"
namespace armarx::exceptions::local
{
@@ -39,23 +37,31 @@ namespace armarx::exceptions::local
\brief This exception is thrown if an invalid value was specified for a property.
\ingroup Exceptions
*/
- class ControlPointsInterpolationMatchException: public armarx::LocalException
+ class ControlPointsInterpolationMatchException : public armarx::LocalException
{
public:
std::string propertyName;
std::string invalidPropertyValue;
- ControlPointsInterpolationMatchException(const int cpSize, const int interpolationSize) throw() :
- armarx::LocalException("You have " + std::to_string(cpSize) + " ControlPoints and need " + std::to_string(cpSize - 1) + "interpolations but only got " + std::to_string(interpolationSize))
- {}
+ ControlPointsInterpolationMatchException(const int cpSize,
+ const int interpolationSize) throw() :
+ armarx::LocalException("You have " + std::to_string(cpSize) +
+ " ControlPoints and need " + std::to_string(cpSize - 1) +
+ "interpolations but only got " +
+ std::to_string(interpolationSize))
+ {
+ }
- ~ControlPointsInterpolationMatchException() throw() { }
+ ~ControlPointsInterpolationMatchException() throw()
+ {
+ }
- virtual std::string name() const override
+ virtual std::string
+ name() const override
{
return "armarx::exceptions::local::ControlPointsInterpolationMatchException";
}
};
-}
+} // namespace armarx::exceptions::local
#endif
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/exceptions/InterpolationNotDefinedException.h b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/exceptions/InterpolationNotDefinedException.h
index e50cc00670353343b68f41b40bd6ba2dbed23f29..fad747cfe725eb01811e4df8b37f2f5fa428f45d 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/exceptions/InterpolationNotDefinedException.h
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/exceptions/InterpolationNotDefinedException.h
@@ -25,12 +25,10 @@
#ifndef INTERPOLATINNOTDEFINEDEXCEPTION_H
#define INTERPOLATINNOTDEFINEDEXCEPTION_H
-#include "ArmarXCore/core/exceptions/Exception.h"
-
-#include "ArmarXCore/core/exceptions/local/ValueRangeExceededException.h"
-
#include <string>
+#include "ArmarXCore/core/exceptions/Exception.h"
+#include "ArmarXCore/core/exceptions/local/ValueRangeExceededException.h"
namespace armarx::exceptions::local
{
@@ -39,23 +37,28 @@ namespace armarx::exceptions::local
\brief This exception is thrown if an invalid value was specified for a property.
\ingroup Exceptions
*/
- class InterpolationNotDefinedException: public armarx::LocalException
+ class InterpolationNotDefinedException : public armarx::LocalException
{
public:
std::string propertyName;
std::string invalidPropertyValue;
InterpolationNotDefinedException(const double value) throw() :
- armarx::LocalException("No Interpolation defined at " + std::to_string(value) + ". Only possible between 0 and 1")
- {}
+ armarx::LocalException("No Interpolation defined at " + std::to_string(value) +
+ ". Only possible between 0 and 1")
+ {
+ }
- ~InterpolationNotDefinedException() throw() { }
+ ~InterpolationNotDefinedException() throw()
+ {
+ }
- virtual std::string name() const override
+ virtual std::string
+ name() const override
{
return "armarx::exceptions::local::InterpolationNotDefinedException";
}
};
-}
+} // namespace armarx::exceptions::local
#endif
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/exceptions/NoInterpolationPossibleException.h b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/exceptions/NoInterpolationPossibleException.h
index 6fdace29e608ae7d1f11957b53d1d6f2efa09111..3cef099b3cbe98e8de7a30830273f40de6bfe275 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/exceptions/NoInterpolationPossibleException.h
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/exceptions/NoInterpolationPossibleException.h
@@ -25,10 +25,9 @@
#ifndef NOINTERPOLATIONPOSSIBLEEXCEPTION_H
#define NOINTERPOLATIONPOSSIBLEEXCEPTION_H
-#include "ArmarXCore/core/exceptions/Exception.h"
-
#include <string>
+#include "ArmarXCore/core/exceptions/Exception.h"
namespace armarx::exceptions::local
{
@@ -37,23 +36,29 @@ namespace armarx::exceptions::local
\brief This exception is thrown if an invalid value was specified for a property.
\ingroup Exceptions
*/
- class NoInterpolationPossibleException: public armarx::LocalException
+ class NoInterpolationPossibleException : public armarx::LocalException
{
public:
std::string propertyName;
std::string invalidPropertyValue;
NoInterpolationPossibleException(const int needed, const int actual) :
- armarx::LocalException("You need at least " + std::to_string(needed) + "contol points to interpolate but only got " + std::to_string(actual))
- {}
+ armarx::LocalException("You need at least " + std::to_string(needed) +
+ "contol points to interpolate but only got " +
+ std::to_string(actual))
+ {
+ }
- ~NoInterpolationPossibleException() throw() { }
+ ~NoInterpolationPossibleException() throw()
+ {
+ }
- virtual std::string name() const override
+ virtual std::string
+ name() const override
{
return "armarx::exceptions::local::NoInterpolationPossibleException";
}
};
-}
+} // namespace armarx::exceptions::local
#endif
diff --git a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/test/test.cpp b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/test/test.cpp
index 9150ecfa96def7f003cde4b84d2e175b44b3f675..c9f37324ca3bc934c4ed61d8540da6ccb839990c 100644
--- a/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/test/test.cpp
+++ b/source/RobotComponents/gui-plugins/RobotTrajectoryDesignerGuiPlugin/test/test.cpp
@@ -4,6 +4,8 @@
#include "../Controller/ImportDialogController.h"
//#include "../Controller/PopUpController.h"
//#include "../Controller/ScenarioDialogController.h"
+#include <iostream>
+
#include "../Controller/SettingController.h"
#include "../Controller/ShortcutController.h"
#include "../Controller/TCPInformationController.h"
@@ -13,21 +15,17 @@
#include "../Controller/TransitionController.h"
#include "../Controller/ViewController.h"
#include "../Controller/WaypointController.h"
-
#include "../RobotTrajectoryDesignerGuiPluginGuiPlugin.h"
#include "../RobotTrajectoryDesignerGuiPluginWidgetController.h"
-
#include "ui_Perspectives.h"
#include "ui_SettingTab.h"
#include "ui_TCPInformationTab.h"
#include "ui_TransitionTab.h"
#include "ui_WaypointTab.h"
-#include <iostream>
-
class Test
{
-public :
+public:
Test()
{
std::cout << "This is a test!";
diff --git a/source/RobotComponents/interface/GazeStabilizationInterface.ice b/source/RobotComponents/interface/GazeStabilizationInterface.ice
old mode 100755
new mode 100644
index 5fcb63c6cdfa61c197cda02dc5d4a7753cdbe782..89449ef02e3d506b451fa10b648daf1b63b69b84
--- a/source/RobotComponents/interface/GazeStabilizationInterface.ice
+++ b/source/RobotComponents/interface/GazeStabilizationInterface.ice
@@ -22,22 +22,22 @@
#pragma once
+#include <RobotAPI/interface/units/HeadIKUnit.ice>
#include <RobotAPI/interface/units/InertialMeasurementUnit.ice>
#include <RobotAPI/interface/units/KinematicUnitInterface.ice>
#include <RobotAPI/interface/units/PlatformUnitInterface.ice>
-#include <RobotAPI/interface/units/HeadIKUnit.ice>
+
#include <VisionX/interface/components/OpticalFlowInterface.ice>
#include <VisionX/interface/components/TrackingErrorInterface.ice>
module armarx
{
- interface GazeStabilizationInterface extends KinematicUnitListener, InertialMeasurementUnitListener, PlatformUnitListener, HeadIKUnitListener, OpticalFlowListener, TrackingErrorListener
+ interface GazeStabilizationInterface extends KinematicUnitListener,
+ InertialMeasurementUnitListener, PlatformUnitListener, HeadIKUnitListener,
+ OpticalFlowListener, TrackingErrorListener
{
void updateWeights(float vor, float okr, float jointIK);
void setReafferenceMethod(bool isReafference);
};
-
};
-
-
diff --git a/source/RobotComponents/interface/OptokineticReflexInterface.ice b/source/RobotComponents/interface/OptokineticReflexInterface.ice
old mode 100755
new mode 100644
index 8fe0c278b3d96ec2130672f066cf1f3f2d86d986..93decd5711cc9cd3e15f648813164fa57ebfdda9
--- a/source/RobotComponents/interface/OptokineticReflexInterface.ice
+++ b/source/RobotComponents/interface/OptokineticReflexInterface.ice
@@ -26,9 +26,6 @@ module armarx
{
interface OptokineticReflexListener
{
- void reportNewOpticalFlow(float diffx, float diffy, float difft);
+ void reportNewOpticalFlow(float diffx, float diffy, float difft);
};
-
};
-
-
diff --git a/source/RobotComponents/interface/components/CollisionCheckerInterface.ice b/source/RobotComponents/interface/components/CollisionCheckerInterface.ice
index d9a48a4160e475f7026db746d789cebbc66bdb56..1fe50596ab12921dda47c74aa45f073f4ceda6c2 100644
--- a/source/RobotComponents/interface/components/CollisionCheckerInterface.ice
+++ b/source/RobotComponents/interface/components/CollisionCheckerInterface.ice
@@ -12,18 +12,36 @@ module armarx
Ice::StringSeq nodeNames2;
double warningDistance;
};
+
sequence<CollisionPair> CollisionPairList;
interface CollisionCheckerInterface extends memoryx::WorkingMemoryListenerInterface
{
- void addCollisionPair(string robotName1, Ice::StringSeq nodeNames1, string robotName2, Ice::StringSeq nodeNames2, double warningDistance);
+ void addCollisionPair(string robotName1,
+ Ice::StringSeq nodeNames1,
+ string robotName2,
+ Ice::StringSeq nodeNames2,
+ double warningDistance);
- void removeCollisionPair(string robotName1, Ice::StringSeq nodeNames1, string robotName2, Ice::StringSeq nodeNames2);
+ void removeCollisionPair(string robotName1,
+ Ice::StringSeq nodeNames1,
+ string robotName2,
+ Ice::StringSeq nodeNames2);
- ["cpp:const"] idempotent bool hasCollisionPair(string robotName1, Ice::StringSeq nodeNames1, string robotName2, Ice::StringSeq nodeNames2);
+ ["cpp:const"] idempotent bool hasCollisionPair(string robotName1,
+ Ice::StringSeq nodeNames1,
+ string robotName2,
+ Ice::StringSeq nodeNames2);
- void setWarningDistance(string robotName1, Ice::StringSeq nodeNames1, string robotName2, Ice::StringSeq nodeNames2, double warningDistance);
- ["cpp:const"] idempotent double getWarningDistance(string robotName1, Ice::StringSeq nodeNames1, string robotName2, Ice::StringSeq nodeNames2);
+ void setWarningDistance(string robotName1,
+ Ice::StringSeq nodeNames1,
+ string robotName2,
+ Ice::StringSeq nodeNames2,
+ double warningDistance);
+ ["cpp:const"] idempotent double getWarningDistance(string robotName1,
+ Ice::StringSeq nodeNames1,
+ string robotName2,
+ Ice::StringSeq nodeNames2);
["cpp:const"] idempotent CollisionPairList getAllCollisionPairs();
@@ -32,11 +50,23 @@ module armarx
};
interface DistanceListener
{
- void reportDistance(string robotName1, Ice::StringSeq nodeNames1, string robotName2, Ice::StringSeq nodeNames2, double distance);
+ void reportDistance(string robotName1,
+ Ice::StringSeq nodeNames1,
+ string robotName2,
+ Ice::StringSeq nodeNames2,
+ double distance);
};
interface CollisionListener
{
- void reportCollision(string robotName1, Ice::StringSeq nodeNames1, string robotName2, Ice::StringSeq nodeNames2, double distance);
- void reportCollisionWarning(string robotName1, Ice::StringSeq nodeNames1, string robotName2, Ice::StringSeq nodeNames2, double distance);
+ void reportCollision(string robotName1,
+ Ice::StringSeq nodeNames1,
+ string robotName2,
+ Ice::StringSeq nodeNames2,
+ double distance);
+ void reportCollisionWarning(string robotName1,
+ Ice::StringSeq nodeNames1,
+ string robotName2,
+ Ice::StringSeq nodeNames2,
+ double distance);
};
};
diff --git a/source/RobotComponents/interface/components/DMPComponentBase.ice b/source/RobotComponents/interface/components/DMPComponentBase.ice
old mode 100755
new mode 100644
index a05d08e32db7c1b64d7c34829ea9fcca5e26e13b..d42b69c4ca1cd4dd158512a92062fd9f03337638
--- a/source/RobotComponents/interface/components/DMPComponentBase.ice
+++ b/source/RobotComponents/interface/components/DMPComponentBase.ice
@@ -22,16 +22,16 @@
#pragma once
-#include <ArmarXCore/interface/core/UserException.ice>
#include <ArmarXCore/interface/core/BasicTypes.ice>
+#include <ArmarXCore/interface/core/UserException.ice>
//#include <MemoryX/interface/memorytypes/MemoryEntities.ice>
-
module armarx
{
- struct cState{
+ struct cState
+ {
double pos;
double vel;
double acc;
@@ -47,21 +47,24 @@ module armarx
sequence<Ice::DoubleSeq> Vec2D;
- struct nStateValues{
+ struct nStateValues
+ {
Ice::DoubleSeq canonicalValues;
Vec2D nextState;
};
- ["cpp:virtual"]
- class DMPInstanceBase
+ ["cpp:virtual"] class DMPInstanceBase
{
- Vec2D calcTrajectory(double startTime, double timestep, double endTime,
+ Vec2D calcTrajectory(double startTime,
+ double timestep,
+ double endTime,
Ice::DoubleSeq goal,
cStateVec currentStates,
- Ice::DoubleSeq canonicalValues, double temporalFactor) throws InvalidArgumentException;
+ Ice::DoubleSeq canonicalValues,
+ double temporalFactor) throws InvalidArgumentException;
-
- // DMP parameters setter
+
+ // DMP parameters setter
void setParameter(string paraId, Ice::DoubleSeq value) throws InvalidArgumentException;
void setGoal(Ice::DoubleSeq value) throws InvalidArgumentException;
void setStartPosition(Ice::DoubleSeq value) throws InvalidArgumentException;
@@ -71,53 +74,74 @@ module armarx
void setCanonicalValues(Ice::DoubleSeq value) throws InvalidArgumentException;
void setTemporalFactor(double value);
- // DMP parameters getter
- double getAmpl(int dim);
+ // DMP parameters getter
+ double getAmpl(int dim);
double getForceTerm(Ice::DoubleSeq value, int dim);
double getTemporalFactor();
cStateVec getNextState(cStateVec states) throws InvalidArgumentException;
cStateVec getNextStateWithCurrentState();
cStateVec getCurrentState();
Ice::DoubleSeq getCanonicalValues();
- double getDampingFactor();
+ double getDampingFactor();
double getSpringFactor();
- string getDMPType();
+ string getDMPType();
int getDMPDim();
Ice::DoubleSeq getStartPosition();
- // Trajectory analyzer
+ // Trajectory analyzer
Ice::DoubleSeq getTrajGoal();
cStateVec getTrajStartState();
void readTrajectoryFromFile(string file, double times);
-
- // DMP tranining
- void trainDMP();
-
- // merge from DMPRefactoring
- nStateValues calcNextState(double t, Ice::DoubleSeq goal, double currentT, Vec2D currentStates, Ice::DoubleSeq canonicalValues, double temporalFactor) throws InvalidArgumentException;
- nStateValues calcNextStateFromConfig(double t, double currentT, Vec2D currentStates, Ice::DoubleSeq canonicalValues) throws InvalidArgumentException;
-// nStateValues calcNextStateFromConfigGlobalTimestep(Vec2D currentStates, Ice::DoubleSeq canonicalValues) throws InvalidArgumentException;
- Vec2D calcTrajectoryV2(Ice::DoubleSeq timestamps, Ice::DoubleSeq goal, Vec2D currentStates, Ice::DoubleSeq canonicalValues, double temporalFactor) throws InvalidArgumentException;
- Vec2D calcTrajectoryFromConfig(Ice::DoubleSeq timestamps, Vec2D currentStates, Ice::DoubleSeq canonicalValues) throws InvalidArgumentException;
-// void setTimeStamps(Ice::DoubleSeq value) throws InvalidArgumentException;
+ // DMP tranining
+ void trainDMP();
+
+ // merge from DMPRefactoring
+ nStateValues calcNextState(double t,
+ Ice::DoubleSeq goal,
+ double currentT,
+ Vec2D currentStates,
+ Ice::DoubleSeq canonicalValues,
+ double temporalFactor) throws InvalidArgumentException;
+ nStateValues calcNextStateFromConfig(
+ double t, double currentT, Vec2D currentStates, Ice::DoubleSeq canonicalValues)
+ throws InvalidArgumentException;
+ // nStateValues calcNextStateFromConfigGlobalTimestep(Vec2D currentStates, Ice::DoubleSeq canonicalValues) throws InvalidArgumentException;
+ Vec2D calcTrajectoryV2(Ice::DoubleSeq timestamps,
+ Ice::DoubleSeq goal,
+ Vec2D currentStates,
+ Ice::DoubleSeq canonicalValues,
+ double temporalFactor) throws InvalidArgumentException;
+ Vec2D calcTrajectoryFromConfig(
+ Ice::DoubleSeq timestamps, Vec2D currentStates, Ice::DoubleSeq canonicalValues)
+ throws InvalidArgumentException;
+ // void setTimeStamps(Ice::DoubleSeq value) throws InvalidArgumentException;
};
- ["cpp:virtual"]
- class DMPComponentBase
+ ["cpp:virtual"] class DMPComponentBase
{
DMPInstanceBase* getDMP(string dmpName);
- DMPInstanceBase* instantiateDMP(string name, string dmptype, int kernelSize) throws InvalidArgumentException;
- DMPInstanceBase* getDMPFromDatabase(string name, string entity) throws InvalidArgumentException;
+ DMPInstanceBase* instantiateDMP(string name, string dmptype, int kernelSize)
+ throws InvalidArgumentException;
+ DMPInstanceBase* getDMPFromDatabase(string name, string entity)
+ throws InvalidArgumentException;
DMPInstanceBase* getDMPFromDatabaseById(string dbId) throws InvalidArgumentException;
- DMPInstanceBase* getDMPFromFile(string name, string dmpName) throws InvalidArgumentException;
+ DMPInstanceBase* getDMPFromFile(string name, string dmpName)
+ throws InvalidArgumentException;
void removeDMPFromDatabase(string dmpname);
void removeDMPFromDatabaseById(string dbId) throws InvalidArgumentException;
void storeDMPInDatabase(string name, string cname) throws InvalidArgumentException;
void storeDMPInFile(string fileName, string dmpName) throws InvalidArgumentException;
- // convenient functions
- Vec2D calcTrajectory(string name, double startTime, double timestep, double endTime, Ice::DoubleSeq goal, cStateVec currentStates, Ice::DoubleSeq canonicalValues, double temporalFactor) throws InvalidArgumentException;
+ // convenient functions
+ Vec2D calcTrajectory(string name,
+ double startTime,
+ double timestep,
+ double endTime,
+ Ice::DoubleSeq goal,
+ cStateVec currentStates,
+ Ice::DoubleSeq canonicalValues,
+ double temporalFactor) throws InvalidArgumentException;
void setDMPState(string name, cStateVec state) throws InvalidArgumentException;
void setGoal(string name, Ice::DoubleSeq value) throws InvalidArgumentException;
void setStartPosition(string name, Ice::DoubleSeq value) throws InvalidArgumentException;
@@ -127,9 +151,9 @@ module armarx
void trainDMP(string name) throws InvalidArgumentException;
void readTrajectoryFromFile(string name, string file, double times);
double getForceTerm(string name, Ice::DoubleSeq value, int dim);
- double getTemporalFactor(string name);
+ double getTemporalFactor(string name);
void setTemporalFactor(string name, double tau);
- Ice::DoubleSeq getCanonicalValues(string name);
+ Ice::DoubleSeq getCanonicalValues(string name);
Ice::DoubleSeq getTrajGoal(string name);
cStateVec getTrajStartState(string name);
Ice::DoubleSeq getStartPosition(string name);
@@ -140,12 +164,12 @@ module armarx
string getDMPType(string name);
int getDMPDim(string name);
- // DMP manager
+ // DMP manager
SVector getDMPNameList();
void eraseDMP(string name);
bool isDMPExist(string name);
- // time manager
+ // time manager
double getTimeStep();
void setTimeStep(double timestep);
double getCurrentTime();
@@ -153,4 +177,3 @@ module armarx
void resetCanonicalValues();
};
};
-
diff --git a/source/RobotComponents/interface/components/FunctionApproximatorBase.ice b/source/RobotComponents/interface/components/FunctionApproximatorBase.ice
old mode 100755
new mode 100644
index 7f1db9173db4636673135c8f0fa6a3309c149325..715c855b0c694ce3a4fb1e7c9d590fb50767e90e
--- a/source/RobotComponents/interface/components/FunctionApproximatorBase.ice
+++ b/source/RobotComponents/interface/components/FunctionApproximatorBase.ice
@@ -22,17 +22,16 @@
#pragma once
-#include <ArmarXCore/interface/core/UserException.ice>
#include <ArmarXCore/interface/core/BasicTypes.ice>
+#include <ArmarXCore/interface/core/UserException.ice>
module armarx
{
- sequence<Ice::DoubleSeq > DVector2d;
+ sequence<Ice::DoubleSeq> DVector2d;
sequence<string> SVector;
- ["cpp:virtual"]
- class FunctionApproximatorBase
+ ["cpp:virtual"] class FunctionApproximatorBase
{
void initialize(string name, Ice::DoubleSeq factors);
void learn(string name, DVector2d x, DVector2d y);
@@ -45,7 +44,5 @@ module armarx
void getFunctionApproximatorFromFile(string funcName, string name);
void getFunctionApproximatorsFromFile(SVector funcNames, string filename);
void saveFunctionApproximatorInFile(string funcName, string name);
-
};
};
-
diff --git a/source/RobotComponents/interface/components/GraspingManager/GraspGeneratorInterface.ice b/source/RobotComponents/interface/components/GraspingManager/GraspGeneratorInterface.ice
old mode 100755
new mode 100644
index 4c85703cccb4f72623b4b5ed5ef0924c41ca414a..902d43afd8df4090371e1c7b42ca943003f41485
--- a/source/RobotComponents/interface/components/GraspingManager/GraspGeneratorInterface.ice
+++ b/source/RobotComponents/interface/components/GraspingManager/GraspGeneratorInterface.ice
@@ -42,9 +42,4 @@ module armarx
GeneratedGraspList generateGrasps(string objectInstanceEntityId);
GeneratedGraspList generateGraspsByObjectName(string objectName);
};
-
};
-
-
-
-
diff --git a/source/RobotComponents/interface/components/GraspingManager/GraspSelectionCriterionInterface.ice b/source/RobotComponents/interface/components/GraspingManager/GraspSelectionCriterionInterface.ice
old mode 100755
new mode 100644
index 11bfdafa70eb6449ddce96885d447276031714e7..011b889b72362afa4b750250bfe9fb62b8748717
--- a/source/RobotComponents/interface/components/GraspingManager/GraspSelectionCriterionInterface.ice
+++ b/source/RobotComponents/interface/components/GraspingManager/GraspSelectionCriterionInterface.ice
@@ -22,12 +22,13 @@
#pragma once
+#include <ArmarXCore/interface/observers/Timestamp.ice>
+
#include <RobotAPI/interface/core/FramedPoseBase.ice>
+
#include <RobotComponents/interface/components/GraspingManager/GraspGeneratorInterface.ice>
#include <RobotComponents/interface/components/GraspingManager/RobotPlacementInterface.ice>
-#include <ArmarXCore/interface/observers/Timestamp.ice>
-
module armarx
{
@@ -39,9 +40,4 @@ module armarx
};
sequence<GraspSelectionCriterionInterface*> GraspSelectionCriterionInterfaceList;
-
};
-
-
-
-
diff --git a/source/RobotComponents/interface/components/GraspingManager/GraspSelectionManagerInterface.ice b/source/RobotComponents/interface/components/GraspingManager/GraspSelectionManagerInterface.ice
old mode 100755
new mode 100644
index 5bfd670f3e9c3194547c25af7c7809349df12dfd..9cba95adde0032cfbc3e21d91a960e8d1c09c4fe
--- a/source/RobotComponents/interface/components/GraspingManager/GraspSelectionManagerInterface.ice
+++ b/source/RobotComponents/interface/components/GraspingManager/GraspSelectionManagerInterface.ice
@@ -22,13 +22,14 @@
#pragma once
+#include <ArmarXCore/interface/observers/Timestamp.ice>
+
#include <RobotAPI/interface/core/FramedPoseBase.ice>
+
#include <RobotComponents/interface/components/GraspingManager/GraspGeneratorInterface.ice>
#include <RobotComponents/interface/components/GraspingManager/GraspSelectionCriterionInterface.ice>
#include <RobotComponents/interface/components/GraspingManager/RobotPlacementInterface.ice>
-#include <ArmarXCore/interface/observers/Timestamp.ice>
-
module armarx
{
interface GraspSelectionManagerInterface
@@ -36,12 +37,7 @@ module armarx
GeneratedGraspList filterGrasps(GeneratedGraspList grasps);
GraspingPlacementList filterPlacements(GraspingPlacementList placements);
- void registerAsGraspSelectionCriterion(GraspSelectionCriterionInterface* criterion);
+ void registerAsGraspSelectionCriterion(GraspSelectionCriterionInterface * criterion);
GraspSelectionCriterionInterfaceList getRegisteredGraspSelectionCriteria();
};
-
};
-
-
-
-
diff --git a/source/RobotComponents/interface/components/GraspingManager/GraspingManagerInterface.ice b/source/RobotComponents/interface/components/GraspingManager/GraspingManagerInterface.ice
old mode 100755
new mode 100644
index 597dc52baa2e943c11d00c0c2763feac21768c10..20f21ec2df022acb47c0df8014890c3b771393cf
--- a/source/RobotComponents/interface/components/GraspingManager/GraspingManagerInterface.ice
+++ b/source/RobotComponents/interface/components/GraspingManager/GraspingManagerInterface.ice
@@ -22,31 +22,33 @@
#pragma once
+#include <ArmarXCore/interface/core/BasicTypes.ice>
+#include <ArmarXCore/interface/observers/Timestamp.ice>
+
#include <RobotAPI/interface/core/FramedPoseBase.ice>
#include <RobotAPI/interface/core/Trajectory.ice>
#include <RobotAPI/interface/units/KinematicUnitInterface.ice>
-#include <ArmarXCore/interface/observers/Timestamp.ice>
-#include <ArmarXCore/interface/core/BasicTypes.ice>
+
#include <RobotComponents/interface/components/GraspingManager/GraspGeneratorInterface.ice>
module armarx
{
-// struct RobotConfigTrajectoryPoint
-// {
-// float timeStamp;
-// NameValueMap jointConfig;
-// };
+ // struct RobotConfigTrajectoryPoint
+ // {
+ // float timeStamp;
+ // NameValueMap jointConfig;
+ // };
-// sequence<RobotConfigTrajectoryPoint> RobotConfigTrajectory;
+ // sequence<RobotConfigTrajectoryPoint> RobotConfigTrajectory;
-// struct RobotPoseTrajectoryPoint
-// {
-// float timeStamp;
-// FramedPoseBase robotPose;
-// };
+ // struct RobotPoseTrajectoryPoint
+ // {
+ // float timeStamp;
+ // FramedPoseBase robotPose;
+ // };
-// sequence<RobotPoseTrajectoryPoint> RobotPoseTrajectory;
+ // sequence<RobotPoseTrajectoryPoint> RobotPoseTrajectory;
struct GraspingTrajectory
{
@@ -68,8 +70,6 @@ module armarx
GeneratedGrasp grasp;
};
-
-
sequence<GraspingTrajectory> GraspingTrajectoryList;
sequence<MotionPlanningData> MotionPlanningDataList;
@@ -78,16 +78,12 @@ module armarx
// full pipeline menthods
GraspingTrajectory generateGraspingTrajectory(string objectInstanceEntityId);
GraspingTrajectoryList generateGraspingTrajectoryList(string objectInstanceEntityId);
- GraspingTrajectoryList generateGraspingTrajectoryListForGraspList(GeneratedGraspList grasps);
+ GraspingTrajectoryList generateGraspingTrajectoryListForGraspList(
+ GeneratedGraspList grasps);
MotionPlanningDataList generateIKs(string objectInstanceEntityId);
void visualizeGraspingTrajectory(GraspingTrajectory trajectory, float visuSlowdownFactor);
// single step methods
GeneratedGraspList generateGraspsByObjectName(string objectName);
};
-
};
-
-
-
-
diff --git a/source/RobotComponents/interface/components/GraspingManager/RobotPlacementInterface.ice b/source/RobotComponents/interface/components/GraspingManager/RobotPlacementInterface.ice
index b7a37633fd1e9aaf7f742b15828654ba0f3593ba..4f743fdcfc626888a79d256cd8f6b37b1fa48e06 100644
--- a/source/RobotComponents/interface/components/GraspingManager/RobotPlacementInterface.ice
+++ b/source/RobotComponents/interface/components/GraspingManager/RobotPlacementInterface.ice
@@ -23,6 +23,7 @@
#pragma once
#include <RobotAPI/interface/core/FramedPoseBase.ice>
+
#include <RobotComponents/interface/components/GraspingManager/GraspGeneratorInterface.ice>
module armarx
@@ -45,11 +46,10 @@ module armarx
GraspingPlacementList generateRobotPlacements(GeneratedGraspList grasps);
GraspingPlacementList generateRobotPlacementsEx(GeneratedGraspList grasps);
- GraspingPlacementList generateRobotPlacementsForGraspPose(string endEffectorName, FramedPoseBase target, PlanarObstacleList planarObstacles, ConvexHull placementArea);
+ GraspingPlacementList generateRobotPlacementsForGraspPose(
+ string endEffectorName,
+ FramedPoseBase target,
+ PlanarObstacleList planarObstacles,
+ ConvexHull placementArea);
};
-
};
-
-
-
-
diff --git a/source/RobotComponents/interface/components/LaserScannerFeatureExtraction/LaserScannerFeatureExtraction.ice b/source/RobotComponents/interface/components/LaserScannerFeatureExtraction/LaserScannerFeatureExtraction.ice
index 0296e11561e47c99f3a4d3de04381b2fc6e75e32..67114a43222d554755af89ef47b6f94368b65496 100644
--- a/source/RobotComponents/interface/components/LaserScannerFeatureExtraction/LaserScannerFeatureExtraction.ice
+++ b/source/RobotComponents/interface/components/LaserScannerFeatureExtraction/LaserScannerFeatureExtraction.ice
@@ -22,11 +22,11 @@
#pragma once
-#include <RobotAPI/interface/units/PlatformUnitInterface.ice>
-#include <RobotAPI/interface/units/LaserScannerUnit.ice>
-
#include <ArmarXCore/interface/serialization/Eigen.ice>
+#include <RobotAPI/interface/units/LaserScannerUnit.ice>
+#include <RobotAPI/interface/units/PlatformUnitInterface.ice>
+
module armarx
{
diff --git a/source/RobotComponents/interface/components/LaserScannerSelfLocalisation.ice b/source/RobotComponents/interface/components/LaserScannerSelfLocalisation.ice
old mode 100755
new mode 100644
index 1465f2355d8021cf4c570e57570d6dc1b8a49616..91cfaae4314c694abc2503c6d42a3db85283494d
--- a/source/RobotComponents/interface/components/LaserScannerSelfLocalisation.ice
+++ b/source/RobotComponents/interface/components/LaserScannerSelfLocalisation.ice
@@ -22,11 +22,11 @@
#pragma once
-#include <RobotAPI/interface/units/PlatformUnitInterface.ice>
-#include <RobotAPI/interface/units/LaserScannerUnit.ice>
-
#include <ArmarXCore/interface/core/BasicVectorTypes.ice>
+#include <RobotAPI/interface/units/LaserScannerUnit.ice>
+#include <RobotAPI/interface/units/PlatformUnitInterface.ice>
+
module armarx
{
@@ -35,9 +35,11 @@ module armarx
Vector2f start;
Vector2f end;
};
+
sequence<LineSegment2D> LineSegment2DSeq;
- interface LaserScannerSelfLocalisationInterface extends PlatformUnitListener, LaserScannerUnitListener
+ interface LaserScannerSelfLocalisationInterface extends PlatformUnitListener,
+ LaserScannerUnitListener
{
string getReportTopicName();
@@ -57,5 +59,3 @@ module armarx
void reportExtractedEdges(LineSegment2DSeq globalEdges);
};
};
-
-
diff --git a/source/RobotComponents/interface/components/MMMPlayerInterface.ice b/source/RobotComponents/interface/components/MMMPlayerInterface.ice
index 168b11981ee2c7a9ea5e0c7d0bcb3c9d799a579d..159e2f44e03a9b6f501a84a5cbbe1bf7481ff691 100644
--- a/source/RobotComponents/interface/components/MMMPlayerInterface.ice
+++ b/source/RobotComponents/interface/components/MMMPlayerInterface.ice
@@ -22,8 +22,8 @@
#pragma once
-#include <RobotAPI/interface/units/UnitInterface.ice>
#include <RobotAPI/interface/core/Trajectory.ice>
+#include <RobotAPI/interface/units/UnitInterface.ice>
//#include <RobotComponents/interface/components/TrajectoryPlayerInterface.ice>
@@ -39,13 +39,11 @@ module armarx
Ice::StringSeq getMotionNames();
bool setMotionData(string motionName);
-// TrajSource getTrajectory();
+ // TrajSource getTrajectory();
TrajectoryBase getJointTraj();
TrajectoryBase getBasePoseTraj();
bool isMotionLoaded();
-
};
};
-
diff --git a/source/RobotComponents/interface/components/MotionPlanning/CSpace/CSpace.ice b/source/RobotComponents/interface/components/MotionPlanning/CSpace/CSpace.ice
index e69886f9518ca57b721254b7c238168608c79501..73895fb9fe3238c6bb6eacbd038f50e56dfc510c 100644
--- a/source/RobotComponents/interface/components/MotionPlanning/CSpace/CSpace.ice
+++ b/source/RobotComponents/interface/components/MotionPlanning/CSpace/CSpace.ice
@@ -36,8 +36,7 @@ module armarx
* - If less values than the cspace's dimensionality are passed the behavior may be undefined.
* - If more values than the cspace's dimensionality are passed the additional values are ignored.
*/
- ["cpp:virtual"]
- class CSpaceBase
+ ["cpp:virtual"] class CSpaceBase
{
/**
* @param configuration The values to check for collision.
@@ -78,8 +77,7 @@ module armarx
["cpp:const"] long getDimensionality();
};
- ["cpp:virtual"]
- class CSpaceAdaptorBase extends CSpaceBase
+ ["cpp:virtual"] class CSpaceAdaptorBase extends CSpaceBase
{
["cpp:const"] idempotent CSpaceBase getOriginalCSpace();
["protected"] CSpaceBase originalCSpace;
diff --git a/source/RobotComponents/interface/components/MotionPlanning/CSpace/ScaledCSpace.ice b/source/RobotComponents/interface/components/MotionPlanning/CSpace/ScaledCSpace.ice
index b87094aabfedf9c0c56b267656d52b0a6b6a7660..28d557865ff37b94559ff4125a023310b16e3188 100644
--- a/source/RobotComponents/interface/components/MotionPlanning/CSpace/ScaledCSpace.ice
+++ b/source/RobotComponents/interface/components/MotionPlanning/CSpace/ScaledCSpace.ice
@@ -24,6 +24,7 @@
#pragma once
#include <ArmarXCore/interface/core/BasicTypes.ice>
+
#include <RobotComponents/interface/components/MotionPlanning/CSpace/CSpace.ice>
module armarx
@@ -31,8 +32,7 @@ module armarx
/**
* @brief Scales the dimensions of an underlying cspace.
*/
- ["cpp:virtual"]
- class ScaledCSpaceBase extends CSpaceAdaptorBase
+ ["cpp:virtual"] class ScaledCSpaceBase extends CSpaceAdaptorBase
{
/**
* @brief Returns the scaling factors.
diff --git a/source/RobotComponents/interface/components/MotionPlanning/CSpace/SimoxCSpace.ice b/source/RobotComponents/interface/components/MotionPlanning/CSpace/SimoxCSpace.ice
index 875b2fec19761f712ff494ffc3bc0bfde628db84..49eb05f73b1825facd958bcbe1bb574fe6fb195d 100644
--- a/source/RobotComponents/interface/components/MotionPlanning/CSpace/SimoxCSpace.ice
+++ b/source/RobotComponents/interface/components/MotionPlanning/CSpace/SimoxCSpace.ice
@@ -23,9 +23,10 @@
*/
#pragma once
-#include <MemoryX/interface/memorytypes/MemoryEntities.ice>
#include <RobotComponents/interface/components/MotionPlanning/CSpace/CSpace.ice>
+#include <MemoryX/interface/memorytypes/MemoryEntities.ice>
+
module armarx
{
struct AttachedObject
@@ -35,6 +36,7 @@ module armarx
string attachedToRobotNodeName;
string associatedCollisionSetName;
};
+
sequence<AttachedObject> AttachedObjectList;
struct AgentPlanningInformation
@@ -46,7 +48,8 @@ module armarx
Ice::StringSeq additionalJointsForPlanning;
Ice::StringSeq jointsExcludedFromPlanning;
Ice::StringSeq collisionSetNames;
- Ice::StringSeq collisionObjectNames; // used additionally to the collision sets (as collision model in a CD manager)
+ Ice::StringSeq
+ collisionObjectNames; // used additionally to the collision sets (as collision model in a CD manager)
AttachedObjectList attachedObjects;
StringFloatDictionary initialJointValues;
@@ -58,10 +61,10 @@ module armarx
memoryx::ObjectClassBase objectClassBase;
armarx::PoseBase objectPose;
};
- sequence <StationaryObject> StationaryObjectList;
- ["cpp:virtual"]
- class SimoxCSpaceBase extends CSpaceBase
+ sequence<StationaryObject> StationaryObjectList;
+
+ ["cpp:virtual"] class SimoxCSpaceBase extends CSpaceBase
{
void addStationaryObject(StationaryObject obj);
void setAgent(AgentPlanningInformation agent);
@@ -71,26 +74,19 @@ module armarx
["protected"] memoryx::CommonStorageInterface* commonStorage;
["protected"] float stationaryObjectMargin = 0.0f;
-
};
/**
* @brief Similar to SimoxCSpaceBase.
* Difference: The agent's 3D pose is planned, too. (first 6 coordinates = x,y,z, roll,pitch,yaw)
*/
- ["cpp:virtual"]
- class SimoxCSpaceWith3DPoseBase extends SimoxCSpaceBase
- {
- ["protected"] Vector6fRange poseBounds;
- };
+ ["cpp:virtual"] class SimoxCSpaceWith3DPoseBase extends SimoxCSpaceBase
+ { ["protected"] Vector6fRange poseBounds; };
/**
* @brief Similar to SimoxCSpaceBase.
* Difference: The agent's 2D pose is planned, too. (first 3 coordinates = x,y,rz)
*/
- ["cpp:virtual"]
- class SimoxCSpaceWith2DPoseBase extends SimoxCSpaceBase
- {
- ["protected"] Vector3fRange poseBounds;
- };
+ ["cpp:virtual"] class SimoxCSpaceWith2DPoseBase extends SimoxCSpaceBase
+ { ["protected"] Vector3fRange poseBounds; };
};
diff --git a/source/RobotComponents/interface/components/MotionPlanning/CSpace/VoxelGridCSpaceBase.ice b/source/RobotComponents/interface/components/MotionPlanning/CSpace/VoxelGridCSpaceBase.ice
index d5abd18ae19d3506d4bc94cb75386030fa9f6ec5..0b17445844929447207c3babb2d0f6e9b1824970 100644
--- a/source/RobotComponents/interface/components/MotionPlanning/CSpace/VoxelGridCSpaceBase.ice
+++ b/source/RobotComponents/interface/components/MotionPlanning/CSpace/VoxelGridCSpaceBase.ice
@@ -25,19 +25,17 @@
//#include <MemoryX/interface/memorytypes/MemoryEntities.ice>
//#include <RobotComponents/interface/components/MotionPlanning/CSpace/CSpace.ice>
-#include <RobotComponents/interface/components/MotionPlanning/CSpace/SimoxCSpace.ice>
#include <VisionX/interface/components/VoxelGridProviderInterface.ice>
+#include <RobotComponents/interface/components/MotionPlanning/CSpace/SimoxCSpace.ice>
+
module armarx
{
- ["cpp:virtual"]
- class VoxelGridCSpaceBase extends SimoxCSpaceBase
+ ["cpp:virtual"] class VoxelGridCSpaceBase extends SimoxCSpaceBase
{
armarx::Vector3fSeq gridPositions;
float gridSize;
["protected"] visionx::VoxelGridProviderInterface* voxelGridProvider;
};
-
};
-
diff --git a/source/RobotComponents/interface/components/MotionPlanning/DataStructures.ice b/source/RobotComponents/interface/components/MotionPlanning/DataStructures.ice
index dc98cc1dd1909383cefc521053b817225e277108..3adcd8e77c0245441b6b8f6c0ef10bb0424b728c 100644
--- a/source/RobotComponents/interface/components/MotionPlanning/DataStructures.ice
+++ b/source/RobotComponents/interface/components/MotionPlanning/DataStructures.ice
@@ -23,9 +23,10 @@
*/
#pragma once
-#include <RobotAPI/interface/core/PoseBase.ice>
#include <ArmarXCore/interface/core/BasicVectorTypes.ice>
+#include <RobotAPI/interface/core/PoseBase.ice>
+
module armarx
{
struct Path
@@ -61,7 +62,7 @@ module armarx
module TaskStatus
{
- /**
+ /**
* @brief The task's execution status.
*
* State transitions ({from1,from2} =(event)> target):
@@ -107,23 +108,23 @@ module armarx
*/
struct TaskInfo
{
- /**
+ /**
* @brief The tasks internal id.
*/
- long internalId;
- /**
+ long internalId;
+ /**
* @brief The task identity.
*/
- Ice::Identity taskIdent;
- /**
+ Ice::Identity taskIdent;
+ /**
* @brief The task status.
*/
- TaskStatus::Status status;
- /**
+ TaskStatus::Status status;
+ /**
* @brief The ice type id.
*/
- string typeId;
- string taskName;
+ string typeId;
+ string taskName;
};
sequence<TaskInfo> TaskInfoSeq;
diff --git a/source/RobotComponents/interface/components/MotionPlanning/MotionPlanningServer.ice b/source/RobotComponents/interface/components/MotionPlanning/MotionPlanningServer.ice
index 1669d2fb3654ccb7d51cb3c4a8d5935dc6b07179..6d7ecba3b11d3b496eaddb8a58d9e04b7de58286 100644
--- a/source/RobotComponents/interface/components/MotionPlanning/MotionPlanningServer.ice
+++ b/source/RobotComponents/interface/components/MotionPlanning/MotionPlanningServer.ice
@@ -31,8 +31,8 @@
module armarx
{
-// interface MotionPlanningTaskControlInterface;
-// class MotionPlanningTaskBase;
+ // interface MotionPlanningTaskControlInterface;
+ // class MotionPlanningTaskBase;
/**
* @brief The Server responsible for:
@@ -47,7 +47,8 @@ module armarx
* @param task The task to add
* @return Returns the task's proxy. (null if the server is in destruction)
*/
- armarx::ClientSideRemoteHandleControlBlockBase enqueueTask(MotionPlanningTaskBase task) throws armarx::ServerShuttingDown;
+ armarx::ClientSideRemoteHandleControlBlockBase enqueueTask(MotionPlanningTaskBase task)
+ throws armarx::ServerShuttingDown;
/**
* @return Returns the number tasks on the server. (queued and completed)
diff --git a/source/RobotComponents/interface/components/MotionPlanning/ResourceRequestStrategies/ComputingPowerRequestStrategy.ice b/source/RobotComponents/interface/components/MotionPlanning/ResourceRequestStrategies/ComputingPowerRequestStrategy.ice
index f00fd4cb6081bb900503e628776ba4e0bceeab1e..911515de826eb2f61aa72d461288b9c62ad91716 100644
--- a/source/RobotComponents/interface/components/MotionPlanning/ResourceRequestStrategies/ComputingPowerRequestStrategy.ice
+++ b/source/RobotComponents/interface/components/MotionPlanning/ResourceRequestStrategies/ComputingPowerRequestStrategy.ice
@@ -27,161 +27,151 @@
module armarx
{
-module cprs
-{
- /**
- * @brief Base class for all computing power request strategy.
- */
- ["cpp:virtual"]
- class ComputingPowerRequestStrategyBase
+ module cprs
{
/**
+ * @brief Base class for all computing power request strategy.
+ */
+ ["cpp:virtual"] class ComputingPowerRequestStrategyBase
+ {
+ /**
* @brief Sets the current state as initial state. Should be called after all initial computing power was allocated.
*/
- void setCurrentStateAsInitialState();
- //update data
- /**
+ void setCurrentStateAsInitialState();
+ //update data
+ /**
* @brief Updates the planners node count.
* @param nodeCount The new node count.
*/
- void updateNodeCount(long nodeCount);
- /**
+ void updateNodeCount(long nodeCount);
+ /**
* @brief Updates the tasks status
* @param newStatus The new status.
*/
- void updateTaskStatus(TaskStatus::Status newStatus);
+ void updateTaskStatus(TaskStatus::Status newStatus);
- void updateNodeCreations(long nodesCreated, long tries);
+ void updateNodeCreations(long nodesCreated, long tries);
- //handel desicions
- /**
+ //handel desicions
+ /**
* @brief Called after additional computing power was allocated.
*/
- void allocatedComputingPower();
- /**
+ void allocatedComputingPower();
+ /**
* @brief Returns whether new computing power should be allocated.
* @return Whether new computing power should be allocated.
*/
- bool shouldAllocateComputingPower();
- };
+ bool shouldAllocateComputingPower();
+ };
- sequence<ComputingPowerRequestStrategyBase> ComputingPowerRequestStrategyBaseList;
+ sequence<ComputingPowerRequestStrategyBase> ComputingPowerRequestStrategyBaseList;
- /**
+ /**
* @brief Base class for compound strategies.
*
* Provides convenience implementations of all update, the setInitialState and allocatedComputingPower methodes.
*/
- ["cpp:virtual"]
- class CompoundedRequestStrategyBase extends ComputingPowerRequestStrategyBase
- {
- /**
+ ["cpp:virtual"] class CompoundedRequestStrategyBase extends
+ ComputingPowerRequestStrategyBase
+ {
+ /**
* @brief The compounded strategies
*/
- ComputingPowerRequestStrategyBaseList requestStrategies;
- };
+ ComputingPowerRequestStrategyBaseList requestStrategies;
+ };
- /**
+ /**
* @brief A compound strategy returning true if all compounded strategies return true.
*/
- ["cpp:virtual"]
- class AndBase extends CompoundedRequestStrategyBase{};
+ ["cpp:virtual"] class AndBase extends CompoundedRequestStrategyBase {};
- /**
+ /**
* @brief A compound strategy returning true if any compounded strategy returns true.
*/
- ["cpp:virtual"]
- class OrBase extends CompoundedRequestStrategyBase{};
+ ["cpp:virtual"] class OrBase extends CompoundedRequestStrategyBase {};
- /**
+ /**
* @brief A strategy returning true if the contained strategy returns false.
*/
- ["cpp:virtual"]
- class NotBase extends ComputingPowerRequestStrategyBase
- {
- /**
+ ["cpp:virtual"] class NotBase extends ComputingPowerRequestStrategyBase
+ {
+ /**
* @brief The strategy that is inverted.
*/
- ComputingPowerRequestStrategyBase allocStrat;
- };
+ ComputingPowerRequestStrategyBase allocStrat;
+ };
- /**
+ /**
* @brief A strategy always returning true.
*/
- ["cpp:virtual"]
- class AlwaysBase extends ComputingPowerRequestStrategyBase{};
+ ["cpp:virtual"] class AlwaysBase extends ComputingPowerRequestStrategyBase {};
- /**
+ /**
* @brief A strategy always returning false.
*/
- ["cpp:virtual"]
- class NeverBase extends ComputingPowerRequestStrategyBase{};
+ ["cpp:virtual"] class NeverBase extends ComputingPowerRequestStrategyBase {};
- /**
+ /**
* @brief A strategy returning true again after set time delta.
*/
- ["cpp:virtual"]
- class ElapsedTimeBase extends ComputingPowerRequestStrategyBase
- {
- /**
+ ["cpp:virtual"] class ElapsedTimeBase extends ComputingPowerRequestStrategyBase
+ {
+ /**
* @brief The used time delta.
*/
- long timeDeltaInSeconds;
- /**
+ long timeDeltaInSeconds;
+ /**
* @brief Whether the strategy skips multiple pending requests.
* (example: last update at t, now it is t+2d, if true three consecutive calls to shouldAllocateComputingPower()
* will return true, false, false. if false three calls will return true, true, false)
*/
- bool skipping;
- };
+ bool skipping;
+ };
- /**
+ /**
* @brief A strategy returning true again after set time delta. T
* he delta is decreased if there were failed creations of new nodes (it will always be smaller than the set time delta).
*
* The used time delta is timeDelta/(1 + sigma * (#failed node creations)/(#tries))
* The last backlogSize tries will be evaluated.
*/
- ["cpp:virtual"]
- class NoNodeCreatedBase extends ElapsedTimeBase
- {
- /**
+ ["cpp:virtual"] class NoNodeCreatedBase extends ElapsedTimeBase
+ {
+ /**
* @brief The used factor to decrease the time delta.
*/
- float sigma;
+ float sigma;
- long backlogSize;
- };
+ long backlogSize;
+ };
- /**
+ /**
* @brief A strategy returning true again after set node delta.
*/
- ["cpp:virtual"]
- class TotalNodeCountBase extends ComputingPowerRequestStrategyBase
- {
- /**
+ ["cpp:virtual"] class TotalNodeCountBase extends ComputingPowerRequestStrategyBase
+ {
+ /**
* @brief The used node delta.
*/
- long nodeCountDelta;
- /**
+ long nodeCountDelta;
+ /**
* @brief Whether the strategy skips multiple pending requests.
* (example: last update at n, now it is n+2d, if true three consecutive calls to shouldAllocateComputingPower()
* will return true, false, false. if false three calls will return true, true, false)
*/
- bool skipping;
- };
+ bool skipping;
+ };
- dictionary<TaskStatus::Status, ComputingPowerRequestStrategyBase> TaskStatusMap;
- /**
+ dictionary<TaskStatus::Status, ComputingPowerRequestStrategyBase> TaskStatusMap;
+ /**
* @brief A choosing a sub strategy depending on the task status.
*/
- ["cpp:virtual"]
- class TaskStatusBase extends ComputingPowerRequestStrategyBase
- {
- /**
+ ["cpp:virtual"] class TaskStatusBase extends ComputingPowerRequestStrategyBase
+ {
+ /**
* @brief The mapping from task status to sub strategies
*/
- TaskStatusMap strategyPerTaskStatus;
+ TaskStatusMap strategyPerTaskStatus;
+ };
};
};
-};
diff --git a/source/RobotComponents/interface/components/MotionPlanning/Tasks/AStar/Task.ice b/source/RobotComponents/interface/components/MotionPlanning/Tasks/AStar/Task.ice
index c8b3faf359ae6e8ea653b24b3c946f15743e7cff..8ff8526050dc4a2d03446b9fb9d9d1a86241da7a 100644
--- a/source/RobotComponents/interface/components/MotionPlanning/Tasks/AStar/Task.ice
+++ b/source/RobotComponents/interface/components/MotionPlanning/Tasks/AStar/Task.ice
@@ -27,27 +27,25 @@
module armarx
{
-module astar
-{
- ["cpp:virtual"]
- class TaskBase extends MotionPlanningTaskWithDefaultMembersBase
+ module astar
{
-// /**
-// * @brief The goal configuration.
-// */
-// VectorXf goalCfg;
- /**
+ ["cpp:virtual"] class TaskBase extends MotionPlanningTaskWithDefaultMembersBase
+ {
+ // /**
+ // * @brief The goal configuration.
+ // */
+ // VectorXf goalCfg;
+ /**
* @brief The distance between nodes along each axis of an implicit grid.
* This grid is used as graph for planning.
*/
- float gridStepSize;
-
-// /**
-// * @brief Returns the goal config.
-// * @return The goal config.
-// */
-// ["cpp:const"] VectorXf getGoal();
+ float gridStepSize;
+ // /**
+ // * @brief Returns the goal config.
+ // * @return The goal config.
+ // */
+ // ["cpp:const"] VectorXf getGoal();
+ };
};
};
-};
diff --git a/source/RobotComponents/interface/components/MotionPlanning/Tasks/AdaptiveDynamicDomainInformedRRTStar/DataStructures.ice b/source/RobotComponents/interface/components/MotionPlanning/Tasks/AdaptiveDynamicDomainInformedRRTStar/DataStructures.ice
index e50e345201fc14877027d0785ca26eacd80016b2..09d3145ca0bf1054411bf483086c97d8fccb177f 100644
--- a/source/RobotComponents/interface/components/MotionPlanning/Tasks/AdaptiveDynamicDomainInformedRRTStar/DataStructures.ice
+++ b/source/RobotComponents/interface/components/MotionPlanning/Tasks/AdaptiveDynamicDomainInformedRRTStar/DataStructures.ice
@@ -29,13 +29,13 @@
module armarx
{
-module addirrtstar
-{
- /**
+ module addirrtstar
+ {
+ /**
* @brief Structure containing the worker's id that created the node and the number of nodes created by this worker.
*/
- struct NodeId
- {
+ struct NodeId
+ {
/**
* @brief The worker's id that created the node.
*/
@@ -44,195 +44,202 @@ module addirrtstar
* @brief The number of nodes created by this worker.
*/
long numberOfNode;
- };
- sequence<NodeId> NodeIdList;
+ };
+
+ sequence<NodeId> NodeIdList;
- /**
+ /**
* @brief Update for the creation of a node.
*/
- struct NodeCreationUpdate
- {
- /**
+ struct NodeCreationUpdate
+ {
+ /**
* @brief The node's config.
*/
- VectorXf config;
- /**
+ VectorXf config;
+ /**
* @brief The node's parent.
*/
- NodeId parent;
-// float radius; //on creation the radius always is +inf
- /**
+ NodeId parent;
+ // float radius; //on creation the radius always is +inf
+ /**
* @brief The cost from the node's parent to the node.
*/
- float fromParentCost;
-// float fromStartCost; //gets calculated at worker
- };
- sequence<NodeCreationUpdate> NodeCreationUpdateList;
+ float fromParentCost;
+ // float fromStartCost; //gets calculated at worker
+ };
- /**
+ sequence<NodeCreationUpdate> NodeCreationUpdateList;
+
+ /**
* @brief Update for the rewiring of a node.
*/
- struct RewireUpdate
- {
- /**
+ struct RewireUpdate
+ {
+ /**
* @brief The rewired node.
*/
- NodeId child;
- /**
+ NodeId child;
+ /**
* @brief The node's new parent.
*/
- NodeId newParent;
- /**
+ NodeId newParent;
+ /**
* @brief The cost from the node's new parent to the node.
*/
- float fromNewParentCost;
- };
- sequence<RewireUpdate> RewireUpdateList;
+ float fromNewParentCost;
+ };
+
+ sequence<RewireUpdate> RewireUpdateList;
- /**
+ /**
* @brief Update for a changed radius.
*/
- struct RadiusUpdate
- {
- /**
+ struct RadiusUpdate
+ {
+ /**
* @brief The node affected by this update.
*/
- NodeId node;
- /**
+ NodeId node;
+ /**
* @brief Whether the radius increased or decreased.
*/
- bool increase;
- };
- sequence<RadiusUpdate> RadiusUpdateList;
+ bool increase;
+ };
+
+ sequence<RadiusUpdate> RadiusUpdateList;
- /**
+ /**
* @brief Update for a new goal node.
*/
- struct GoalInfo
- {
- /**
+ struct GoalInfo
+ {
+ /**
* @brief The new goal node.
*/
- NodeId node;
- /**
+ NodeId node;
+ /**
* @brief The cost from the node to the goal.
*/
- float costToGoToGoal;
- };
- sequence<GoalInfo> GoalInfoList;
+ float costToGoToGoal;
+ };
- /**
+ sequence<GoalInfo> GoalInfoList;
+
+ /**
* @brief Compound update structure containing all updates of a batch of iterations.
*/
- struct Update
- {
- /**
+ struct Update
+ {
+ /**
* @brief The worker's id causing the update.
*/
- long workerId;
+ long workerId;
- /**
+ /**
* @brief The updates of workers prior to this update.
* These ids are used to apply updares in correct order, detect missing updates and request missing updates again.
* dependetOnUpdateIds[i] == -1 means no updates from worker i are required
*/
- Ice::LongSeq dependetOnUpdateIds;
+ Ice::LongSeq dependetOnUpdateIds;
- //the node ids for all new nodes are implicit created by the position in the list in combination with the workerId and oldNodeCounts[workerId]
- /**
+ //the node ids for all new nodes are implicit created by the position in the list in combination with the workerId and oldNodeCounts[workerId]
+ /**
* @brief Node creation updates.
*/
- NodeCreationUpdateList nodes;
- /**
+ NodeCreationUpdateList nodes;
+ /**
* @brief Updates about rewired nodes.
*/
- RewireUpdateList rewires;
- /**
+ RewireUpdateList rewires;
+ /**
* @brief Updates about changed radii.
*/
- RadiusUpdateList radii;
- /**
+ RadiusUpdateList radii;
+ /**
* @brief Updates about new goal nodes.
*/
- GoalInfoList newGoalNodes;
- };
- sequence<Update> UpdateList;
- sequence<UpdateList> UpdateListList;
+ GoalInfoList newGoalNodes;
+ };
- /**
+ sequence<Update> UpdateList;
+ sequence<UpdateList> UpdateListList;
+
+ /**
* @brief Interface used to transmit tree updates.
*/
- interface TreeUpdateInterface
- {
- /**
+ interface TreeUpdateInterface
+ {
+ /**
* @brief Queues the update for integration into the tree
* @param u The update data
*/
- void updateTree(Update u);
- };
+ void updateTree(Update u);
+ };
- /**
+ /**
* @brief All information about a node.
*/
- struct FullNodeData
- {
- /**
+ struct FullNodeData
+ {
+ /**
* @brief The node's config.
*/
- VectorXf config;
- /**
+ VectorXf config;
+ /**
* @brief The node's parent.
*/
- NodeId parent;
- /**
+ NodeId parent;
+ /**
* @brief The node's radius.
*/
- float radius;
- /**
+ float radius;
+ /**
* @brief The cost from the node's parent to the node.
*/
- float fromParentCost;
- };
- sequence<FullNodeData> FullNodeDataList;
- sequence<FullNodeDataList> FullNodeDataListList;
+ float fromParentCost;
+ };
- /**
+ sequence<FullNodeData> FullNodeDataList;
+ sequence<FullNodeDataList> FullNodeDataListList;
+
+ /**
* @brief Struct containing all nodes of a tree and the update ids of the tree.
*/
- struct FullIceTree
- {
- /**
+ struct FullIceTree
+ {
+ /**
* @brief All nodes per worker.
*/
- FullNodeDataListList nodes;
- /**
+ FullNodeDataListList nodes;
+ /**
* @brief The applyed updates
*/
- Ice::LongSeq updateIds;
- };
+ Ice::LongSeq updateIds;
+ };
- /**
+ /**
* @brief Parameters used by adaptive dynamic domain.
*/
- struct AdaptiveDynamicDomainParameters
- {
- /**
+ struct AdaptiveDynamicDomainParameters
+ {
+ /**
* @brief The value used to increase/decrease the radius of a node.
*
* "A small value for ? (usually a few percents) is sufficient to increase the robustness of the algorithm"
* (doi> 10.1109/IROS.2005.1545607)
*/
- float alpha;
- /**
+ float alpha;
+ /**
* @brief The initial radius for boundary nodes
*
* "The gain is particularly significant when the initial value is an
* overestimation of the optimal value compared to an under estimation."
* (doi> 10.1109/IROS.2005.1545607)
*/
- float initialBorderRadius; //20*DCDStepSize
+ float initialBorderRadius; //20*DCDStepSize
- /**
+ /**
* @brief adaptiveDynamicDomainMinimalRadius
*
* "To keep the probabilistic completeness of the algorithm we
@@ -244,7 +251,7 @@ module addirrtstar
* (doi> 10.1109/IROS.2005.1545607)
*
*/
- float minimalRadius;//5*DCDStepSize
+ float minimalRadius; //5*DCDStepSize
+ };
};
};
-};
diff --git a/source/RobotComponents/interface/components/MotionPlanning/Tasks/AdaptiveDynamicDomainInformedRRTStar/ManagerNode.ice b/source/RobotComponents/interface/components/MotionPlanning/Tasks/AdaptiveDynamicDomainInformedRRTStar/ManagerNode.ice
index 0b08687f06fd4dc1bd94ea7e31dc589a910dbc80..45ab2ceda84211caa4f7a592a7f466d26c449c05 100644
--- a/source/RobotComponents/interface/components/MotionPlanning/Tasks/AdaptiveDynamicDomainInformedRRTStar/ManagerNode.ice
+++ b/source/RobotComponents/interface/components/MotionPlanning/Tasks/AdaptiveDynamicDomainInformedRRTStar/ManagerNode.ice
@@ -32,52 +32,52 @@ module armarx
{
class CSpaceBase;
-module addirrtstar
-{
- //forward
- class TaskBase;
- class MotionPlanningTaskBase;
-
- ["cpp:virtual"]
- class ManagerNodeBase implements TreeUpdateInterface, ResourceManagementInterface
+ module addirrtstar
{
- idempotent void kill();
+ //forward
+ class TaskBase;
+ class MotionPlanningTaskBase;
- //client requests
- ["cpp:const"] idempotent PathWithCost getBestPath();
- ["cpp:const"] idempotent long getPathCount();
- ["cpp:const"] idempotent PathWithCost getNthPathWithCost(long n);
- ["cpp:const"] idempotent PathWithCostSeq getAllPathsWithCost();
+ ["cpp:virtual"] class ManagerNodeBase implements TreeUpdateInterface,
+ ResourceManagementInterface
+ {
+ idempotent void kill();
- //worker requests
- ["cpp:const"] idempotent Update getUpdate(long workerId, long updateSubId);
- ["cpp:const"] idempotent FullIceTree getTree();
+ //client requests
+ ["cpp:const"] idempotent PathWithCost getBestPath();
+ ["cpp:const"] idempotent long getPathCount();
+ ["cpp:const"] idempotent PathWithCost getNthPathWithCost(long n);
+ ["cpp:const"] idempotent PathWithCostSeq getAllPathsWithCost();
- idempotent void setWorkersFinalUpdateId(long workerId, long finalUpdateId);
+ //worker requests
+ ["cpp:const"] idempotent Update getUpdate(long workerId, long updateSubId);
+ ["cpp:const"] idempotent FullIceTree getTree();
- //member
- TaskBase* task;
- armarx::RemoteObjectNodePrxList remoteObjectNodes;
+ idempotent void setWorkersFinalUpdateId(long workerId, long finalUpdateId);
- long initialWorkerCount;
- long maximalWorkerCount;
- cprs::ComputingPowerRequestStrategyBase planningComputingPowerRequestStrategy;
+ //member
+ TaskBase* task;
+ armarx::RemoteObjectNodePrxList remoteObjectNodes;
- float dcdStep;
- long maximalPlanningTimeInSeconds;
- CSpaceBase cspace;
+ long initialWorkerCount;
+ long maximalWorkerCount;
+ cprs::ComputingPowerRequestStrategyBase planningComputingPowerRequestStrategy;
- VectorXf startNode;
- VectorXf goalNode;
- AdaptiveDynamicDomainParameters addParams;
- float targetCost;
- long batchSize;
- /**
+ float dcdStep;
+ long maximalPlanningTimeInSeconds;
+ CSpaceBase cspace;
+
+ VectorXf startNode;
+ VectorXf goalNode;
+ AdaptiveDynamicDomainParameters addParams;
+ float targetCost;
+ long batchSize;
+ /**
* @brief Number of nodes created (by a worker) before a connect to the goal node is tried (by this worker).
*/
- long nodeCountDeltaForGoalConnectionTries;
+ long nodeCountDeltaForGoalConnectionTries;
- ["cpp:const"] idempotent long getNodeCount();
+ ["cpp:const"] idempotent long getNodeCount();
+ };
};
};
-};
diff --git a/source/RobotComponents/interface/components/MotionPlanning/Tasks/AdaptiveDynamicDomainInformedRRTStar/Task.ice b/source/RobotComponents/interface/components/MotionPlanning/Tasks/AdaptiveDynamicDomainInformedRRTStar/Task.ice
index 94b5e936d17231a97a567caf09aaf532ab45a4e3..2f2a2fdf93e3b81d95348fc226e847da344d45b0 100644
--- a/source/RobotComponents/interface/components/MotionPlanning/Tasks/AdaptiveDynamicDomainInformedRRTStar/Task.ice
+++ b/source/RobotComponents/interface/components/MotionPlanning/Tasks/AdaptiveDynamicDomainInformedRRTStar/Task.ice
@@ -23,72 +23,73 @@
*/
#pragma once
-#include <RobotComponents/interface/components/MotionPlanning/Tasks/CPRSAwareMotionPlanningTask.ice>
-
#include <RobotComponents/interface/components/MotionPlanning/Tasks/AdaptiveDynamicDomainInformedRRTStar/DataStructures.ice>
+#include <RobotComponents/interface/components/MotionPlanning/Tasks/CPRSAwareMotionPlanningTask.ice>
module armarx
{
-module addirrtstar
-{
- interface ResourceManagementInterface {
- /**
+ module addirrtstar
+ {
+ interface ResourceManagementInterface
+ {
+ /**
* @brief
*/
- idempotent void setMaxCpus(int maxCpus);
+ idempotent void setMaxCpus(int maxCpus);
- ["cpp:const"] idempotent int getMaxCpus();
- };
+ ["cpp:const"] idempotent int getMaxCpus();
+ };
- /**
+ /**
* @brief The task containing information for a rrt* using adaptive dynamic domain with informed sampling
*/
- ["cpp:virtual"]
- class TaskBase extends cprs::CPRSAwareMotionPlanningTaskBase implements MotionPlanningMultiPathWithCostTaskControlInterface, ResourceManagementInterface
- {
- /**
+ ["cpp:virtual"] class TaskBase extends cprs::CPRSAwareMotionPlanningTaskBase implements
+ MotionPlanningMultiPathWithCostTaskControlInterface,
+ ResourceManagementInterface
+ {
+ /**
* @brief Stores the paths in the task.
* Used by the manager node to store the results before shut down.
* @param paths The paths.
*/
- void setPaths(PathWithCostSeq paths);
+ void setPaths(PathWithCostSeq paths);
- /**
+ /**
* @brief Returns the node count.
* @return The node count.
*/
- ["cpp:const"] idempotent long getNodeCount();
+ ["cpp:const"] idempotent long getNodeCount();
- //management
- /**
+ //management
+ /**
* @brief The initial worker count used when solving this task.
*/
- long initialWorkerCount;
- /**
+ long initialWorkerCount;
+ /**
* @brief The maximal worker count used when solving this task.
*/
- long maximalWorkerCount;
+ long maximalWorkerCount;
- // //problem specific params
- /**
+ // //problem specific params
+ /**
* @brief The parameters required for adaptive dynamic domain.
*/
- AdaptiveDynamicDomainParameters addParams;
+ AdaptiveDynamicDomainParameters addParams;
- /**
+ /**
* @brief The target path cost.
*/
- float targetCost;
+ float targetCost;
- /**
+ /**
* @brief The size of one batch.
*/
- long batchSize;
+ long batchSize;
- /**
+ /**
* @brief Number of nodes created (by a worker) before a connect to the goal node is tried (by this worker).
*/
- long nodeCountDeltaForGoalConnectionTries;
+ long nodeCountDeltaForGoalConnectionTries;
+ };
};
};
-};
diff --git a/source/RobotComponents/interface/components/MotionPlanning/Tasks/AdaptiveDynamicDomainInformedRRTStar/WorkerNode.ice b/source/RobotComponents/interface/components/MotionPlanning/Tasks/AdaptiveDynamicDomainInformedRRTStar/WorkerNode.ice
index 57e642d3d968dbb8d96688dffa94af1846b89a85..db8c1c0a3bad7e4291d900115467216758b702b4 100644
--- a/source/RobotComponents/interface/components/MotionPlanning/Tasks/AdaptiveDynamicDomainInformedRRTStar/WorkerNode.ice
+++ b/source/RobotComponents/interface/components/MotionPlanning/Tasks/AdaptiveDynamicDomainInformedRRTStar/WorkerNode.ice
@@ -24,88 +24,86 @@
#pragma once
#include <RobotComponents/interface/components/MotionPlanning/CSpace/CSpace.ice>
-
#include <RobotComponents/interface/components/MotionPlanning/Tasks/AdaptiveDynamicDomainInformedRRTStar/DataStructures.ice>
#include <RobotComponents/interface/components/MotionPlanning/Tasks/AdaptiveDynamicDomainInformedRRTStar/ManagerNode.ice>
module armarx
{
-module addirrtstar
-{
- /**
- * @brief The worker used by addirrt*.
- */
- ["cpp:virtual"]
- class WorkerNodeBase implements TreeUpdateInterface
+ module addirrtstar
{
/**
+ * @brief The worker used by addirrt*.
+ */
+ ["cpp:virtual"] class WorkerNodeBase implements TreeUpdateInterface
+ {
+ /**
* @brief Requests the worker to kill its working task.
*/
- idempotent void killWorker();
+ idempotent void killWorker();
- /**
+ /**
* @brief
*/
- idempotent void pauseWorker(bool pauseFlag);
+ idempotent void pauseWorker(bool pauseFlag);
- /**
+ /**
* @brief Requests an already send update again.
*/
- ["cpp:const"] idempotent Update getUpdate(long updateId);
+ ["cpp:const"] idempotent Update getUpdate(long updateId);
- //problem
- /**
+ //problem
+ /**
* @brief The used planning cspace.
*/
- CSpaceBase cspace;
- /**
+ CSpaceBase cspace;
+ /**
* @brief The start config.
*/
- VectorXf startCfg;
- /**
+ VectorXf startCfg;
+ /**
* @brief The goal config.
*/
- VectorXf goalCfg;
+ VectorXf goalCfg;
- //parameters
- /**
+ //parameters
+ /**
* @brief The dcd stepsize.
*/
- float DCDStepSize;
+ float DCDStepSize;
- /**
+ /**
* @brief The parameters used for add.
*/
- AdaptiveDynamicDomainParameters addParams;
+ AdaptiveDynamicDomainParameters addParams;
- //management
- /**
+ //management
+ /**
* @brief The workers manager.
*/
- ManagerNodeBase* manager;
- /**
+ ManagerNodeBase* manager;
+ /**
* @brief The workers id.
*/
- long workerId;
- /**
+ long workerId;
+ /**
* @brief The batch size before an update is send and other updates are integrated.
*/
- long batchSize;
+ long batchSize;
- /**
+ /**
* @brief Number of nodes created (by a worker) before a connect to the goal node is tried (by this worker).
*/
- long nodeCountDeltaForGoalConnectionTries;
+ long nodeCountDeltaForGoalConnectionTries;
- /**
+ /**
* @brief The update topics prefix
*/
- string topicPrefix;
+ string topicPrefix;
- /**
+ /**
* @brief The precalculated rotation matrix used by the informed sampler.
*/
- Ice::FloatSeq rotationMatrix;
+ Ice::FloatSeq rotationMatrix;
+ };
};
};
-};
diff --git a/source/RobotComponents/interface/components/MotionPlanning/Tasks/BiRRT/Task.ice b/source/RobotComponents/interface/components/MotionPlanning/Tasks/BiRRT/Task.ice
index 0db43ab561d382e1bb04ea363d8e0f859fb973a6..a661f7c88bc7cef680843fba70b25d4c7874f5fb 100644
--- a/source/RobotComponents/interface/components/MotionPlanning/Tasks/BiRRT/Task.ice
+++ b/source/RobotComponents/interface/components/MotionPlanning/Tasks/BiRRT/Task.ice
@@ -27,22 +27,20 @@
module armarx
{
-module birrt
-{
- ["cpp:virtual"]
- class TaskBase extends MotionPlanningTaskWithDefaultMembersBase
+ module birrt
{
-// /**
-// * @brief The goal configuration.
-// */
-// VectorXf goalCfg;
-
-// /**
-// * @brief Returns the goal config.
-// * @return The goal config.
-// */
-// ["cpp:const"] VectorXf getGoal();
+ ["cpp:virtual"] class TaskBase extends MotionPlanningTaskWithDefaultMembersBase
+ {
+ // /**
+ // * @brief The goal configuration.
+ // */
+ // VectorXf goalCfg;
+ // /**
+ // * @brief Returns the goal config.
+ // * @return The goal config.
+ // */
+ // ["cpp:const"] VectorXf getGoal();
+ };
};
};
-};
diff --git a/source/RobotComponents/interface/components/MotionPlanning/Tasks/CPRSAwareMotionPlanningTask.ice b/source/RobotComponents/interface/components/MotionPlanning/Tasks/CPRSAwareMotionPlanningTask.ice
index 1aaf865f2fc756b9999cc82edcf8a3929bff1d47..7d2643bc06ed9557133b4c204dbf970e874f12e7 100644
--- a/source/RobotComponents/interface/components/MotionPlanning/Tasks/CPRSAwareMotionPlanningTask.ice
+++ b/source/RobotComponents/interface/components/MotionPlanning/Tasks/CPRSAwareMotionPlanningTask.ice
@@ -23,23 +23,23 @@
*/
#pragma once
-#include <RobotComponents/interface/components/MotionPlanning/Tasks/MotionPlanningTask.ice>
#include <RobotComponents/interface/components/MotionPlanning/ResourceRequestStrategies/ComputingPowerRequestStrategy.ice>
+#include <RobotComponents/interface/components/MotionPlanning/Tasks/MotionPlanningTask.ice>
module armarx
{
-module cprs
-{
- /**
- * @brief Abstract base holding the data required to execute a planning task using a cprs.
- */
- ["cpp:virtual"]
- class CPRSAwareMotionPlanningTaskBase extends MotionPlanningTaskWithDefaultMembersBase //implements MotionPlanningTaskControlInterface
+ module cprs
{
/**
+ * @brief Abstract base holding the data required to execute a planning task using a cprs.
+ */
+ ["cpp:virtual"] class CPRSAwareMotionPlanningTaskBase extends
+ MotionPlanningTaskWithDefaultMembersBase //implements MotionPlanningTaskControlInterface
+ {
+ /**
* @brief The used strategy to decide whether additional computing power is requested.
*/
- ComputingPowerRequestStrategyBase planningComputingPowerRequestStrategy;
+ ComputingPowerRequestStrategyBase planningComputingPowerRequestStrategy;
+ };
};
};
-};
diff --git a/source/RobotComponents/interface/components/MotionPlanning/Tasks/CSpaceVisualizerTask.ice b/source/RobotComponents/interface/components/MotionPlanning/Tasks/CSpaceVisualizerTask.ice
index dd189f415e618862bd26b4045d70615348364e69..d572071c553cd5a7a02a46121139d943139da82c 100644
--- a/source/RobotComponents/interface/components/MotionPlanning/Tasks/CSpaceVisualizerTask.ice
+++ b/source/RobotComponents/interface/components/MotionPlanning/Tasks/CSpaceVisualizerTask.ice
@@ -29,8 +29,7 @@ module armarx
/**
* @brief
*/
- ["cpp:virtual"]
- class CSpaceVisualizerTaskBase extends MotionPlanningTaskWithDefaultMembersBase //implements MotionPlanningTaskControlInterface
- {
- };
+ ["cpp:virtual"] class CSpaceVisualizerTaskBase extends
+ MotionPlanningTaskWithDefaultMembersBase //implements MotionPlanningTaskControlInterface
+ {};
};
diff --git a/source/RobotComponents/interface/components/MotionPlanning/Tasks/MotionPlanningTask.ice b/source/RobotComponents/interface/components/MotionPlanning/Tasks/MotionPlanningTask.ice
index 841f8a3e8e00056c9a0292a69d93fb20e34247f9..20ea0c4dc7e9b6a64fbf38db27da5c9e9da1474b 100644
--- a/source/RobotComponents/interface/components/MotionPlanning/Tasks/MotionPlanningTask.ice
+++ b/source/RobotComponents/interface/components/MotionPlanning/Tasks/MotionPlanningTask.ice
@@ -26,12 +26,11 @@
#include <ArmarXCore/interface/core/RemoteObjectNode.ice>
#include <RobotComponents/interface/components/MotionPlanning/CSpace/CSpace.ice>
-
#include <RobotComponents/interface/components/MotionPlanning/DataStructures.ice>
module armarx
{
-// interface MotionPlanningServerInterface;
+ // interface MotionPlanningServerInterface;
/**
* @brief The interface used by a client to control a server side task.
*/
@@ -107,7 +106,9 @@ module armarx
["cpp:const"] idempotent PathSeq getAllPaths();
};
- interface MotionPlanningMultiPathWithCostTaskControlInterface extends MotionPlanningWithCostTaskControlInterface, MotionPlanningMultiPathTaskControlInterface
+ interface MotionPlanningMultiPathWithCostTaskControlInterface extends
+ MotionPlanningWithCostTaskControlInterface,
+ MotionPlanningMultiPathTaskControlInterface
{
/**
* @brief Returns the best paths cost. (inf if no path was found)
@@ -120,7 +121,8 @@ module armarx
* @return The nth found path.
* @throw IndexOutOfBoundsException If n is not in [0, getPathCount())
*/
- ["cpp:const"] idempotent PathWithCost getNthPathWithCost(long n) throws armarx::IndexOutOfBoundsException;
+ ["cpp:const"] idempotent PathWithCost getNthPathWithCost(long n)
+ throws armarx::IndexOutOfBoundsException;
/**
* @brief Returns all found paths.
@@ -136,8 +138,7 @@ module armarx
* The c++ side requires a function to create a server side task.
* (since this is an implementation detail this function is not part of the slice interface)
*/
- ["cpp:virtual"]
- class MotionPlanningTaskBase implements MotionPlanningTaskControlInterface
+ ["cpp:virtual"] class MotionPlanningTaskBase implements MotionPlanningTaskControlInterface
{
/**
* @brief Runs the task and blocks until it is finished (done/arborted/failed)
@@ -163,8 +164,7 @@ module armarx
["protected"] string taskName;
};
- ["cpp:virtual"]
- class MotionPlanningTaskWithDefaultMembersBase extends MotionPlanningTaskBase
+ ["cpp:virtual"] class MotionPlanningTaskWithDefaultMembersBase extends MotionPlanningTaskBase
{
/**
* @brief The start configuration.
@@ -207,9 +207,6 @@ module armarx
["cpp:const"] idempotent float getDcdStep();
};
- ["cpp:virtual"]
- class PostprocessingMotionPlanningTaskBase extends MotionPlanningTaskBase
- {
- MotionPlanningTaskBase previousStep;
- };
+ ["cpp:virtual"] class PostprocessingMotionPlanningTaskBase extends MotionPlanningTaskBase
+ { MotionPlanningTaskBase previousStep; };
};
diff --git a/source/RobotComponents/interface/components/MotionPlanning/Tasks/PathCollection/Task.ice b/source/RobotComponents/interface/components/MotionPlanning/Tasks/PathCollection/Task.ice
index 103537567e540c09d241036a5017cbf84cb89061..4cdeb55f3eb9184c3984bae4e7ce1c3f9377b9c8 100644
--- a/source/RobotComponents/interface/components/MotionPlanning/Tasks/PathCollection/Task.ice
+++ b/source/RobotComponents/interface/components/MotionPlanning/Tasks/PathCollection/Task.ice
@@ -27,18 +27,18 @@
module armarx
{
-module pathcol
-{
- ["cpp:virtual"]
- class TaskBase extends MotionPlanningTaskBase implements MotionPlanningMultiPathWithCostTaskControlInterface
+ module pathcol
{
- PathWithCostSeq paths;
- CSpaceBase cspace;
- /**
+ ["cpp:virtual"] class TaskBase extends MotionPlanningTaskBase implements
+ MotionPlanningMultiPathWithCostTaskControlInterface
+ {
+ PathWithCostSeq paths;
+ CSpaceBase cspace;
+ /**
* @brief The maximal allowed planning time in seconds.
* (if this limit is reached the task is aborted).
*/
- long maximalPlanningTimeInSeconds = 600;
+ long maximalPlanningTimeInSeconds = 600;
+ };
};
};
-};
diff --git a/source/RobotComponents/interface/components/MotionPlanning/Tasks/RRTConnect/DataStructures.ice b/source/RobotComponents/interface/components/MotionPlanning/Tasks/RRTConnect/DataStructures.ice
index 56ffec40c6d8c064837c17aa8922a4e89a02949b..a23be2b824027f1a1ae9a369afab564b9d5fe7d2 100644
--- a/source/RobotComponents/interface/components/MotionPlanning/Tasks/RRTConnect/DataStructures.ice
+++ b/source/RobotComponents/interface/components/MotionPlanning/Tasks/RRTConnect/DataStructures.ice
@@ -24,91 +24,95 @@
#pragma once
#include <ArmarXCore/interface/core/BasicTypes.ice>
+
#include <RobotComponents/interface/components/MotionPlanning/DataStructures.ice>
module armarx
{
-module rrtconnect
-{
- /**
- * @brief Structure containing the worker's id that created the node and the number of nodes created by this worker.
- */
- struct NodeId
+ module rrtconnect
{
/**
+ * @brief Structure containing the worker's id that created the node and the number of nodes created by this worker.
+ */
+ struct NodeId
+ {
+ /**
* @brief The worker's id that created the node.
*/
- long workerId;
- /**
+ long workerId;
+ /**
* @brief The node's id for the given worker and tree.
*/
- long nodeSubId;
- };
- sequence<NodeId> NodeIdList;
+ long nodeSubId;
+ };
- /**
+ sequence<NodeId> NodeIdList;
+
+ /**
* @brief Update for the creation of a node.
*/
- struct NodeCreationUpdate
- {
- /**
+ struct NodeCreationUpdate
+ {
+ /**
* @brief The node's config.
*/
- VectorXf config;
- /**
+ VectorXf config;
+ /**
* @brief The node's parent.
*/
- NodeId parent;
- };
- sequence<NodeCreationUpdate> NodeCreationUpdateList;
+ NodeId parent;
+ };
- struct PerTreeUpdate
- {
- //the node ids for all new nodes are implicit created by the position in the list in combination with the workerId and oldNodeCounts[workerId]
- /**
+ sequence<NodeCreationUpdate> NodeCreationUpdateList;
+
+ struct PerTreeUpdate
+ {
+ //the node ids for all new nodes are implicit created by the position in the list in combination with the workerId and oldNodeCounts[workerId]
+ /**
* @brief Node creation updates.
*/
- NodeCreationUpdateList nodes;
- };
- sequence<PerTreeUpdate> PerTreeUpdateList;
+ NodeCreationUpdateList nodes;
+ };
+ sequence<PerTreeUpdate> PerTreeUpdateList;
- /**
+ /**
* @brief Compound update structure containing all updates.
*/
- struct Update
- {
- /**
+ struct Update
+ {
+ /**
* @brief The worker's id causing the update.
*/
- long workerId;
+ long workerId;
- /**
+ /**
* @brief The updates of workers prior to this update.
* These ids are used to apply updares in correct order, detect missing updates and request missing updates again.
* dependetOnUpdateIds[i] == -1 means no updates from worker i are required
*/
- Ice::LongSeq dependetOnUpdateIds;
+ Ice::LongSeq dependetOnUpdateIds;
- PerTreeUpdateList updatesPerTree;
- };
- sequence<Update> UpdateList;
+ PerTreeUpdateList updatesPerTree;
+ };
+
+ sequence<Update> UpdateList;
- /**
+ /**
* @brief Interface used to transmit tree updates.
*/
- interface TreeUpdateInterface
- {
- /**
+ interface TreeUpdateInterface
+ {
+ /**
* @brief Queues the update for integration into the tree
* @param u The update data
*/
- void updateTree(Update u);
+ void updateTree(Update u);
- /**
+ /**
* @brief Stopps planning.
*/
- void abort();
+ void abort();
+ };
};
};
-};
diff --git a/source/RobotComponents/interface/components/MotionPlanning/Tasks/RRTConnect/Task.ice b/source/RobotComponents/interface/components/MotionPlanning/Tasks/RRTConnect/Task.ice
index 8d8de0076cec4d42bc7b77d07c2a9737db63a4d3..4e9281ee8e1fa611238325b607c2ebdbd6181ca1 100644
--- a/source/RobotComponents/interface/components/MotionPlanning/Tasks/RRTConnect/Task.ice
+++ b/source/RobotComponents/interface/components/MotionPlanning/Tasks/RRTConnect/Task.ice
@@ -28,27 +28,26 @@
module armarx
{
-module rrtconnect
-{
- ["cpp:virtual"]
- class TaskBase extends MotionPlanningTaskWithDefaultMembersBase
+ module rrtconnect
{
- //management
- /**
+ ["cpp:virtual"] class TaskBase extends MotionPlanningTaskWithDefaultMembersBase
+ {
+ //management
+ /**
* @brief The number of workers used to perform planning.
*/
- long workerCount;
+ long workerCount;
- //problem specific params
- /**
+ //problem specific params
+ /**
* @brief Stores the paths in the task.
* Used by the manager node to store the results before shut down.
* @param paths The paths.
*/
- void setPath(Path paths);
+ void setPath(Path paths);
- void workerHasAborted(long workerId);
+ void workerHasAborted(long workerId);
+ };
};
};
-};
diff --git a/source/RobotComponents/interface/components/MotionPlanning/Tasks/RRTConnect/WorkerNode.ice b/source/RobotComponents/interface/components/MotionPlanning/Tasks/RRTConnect/WorkerNode.ice
index 2ef882cd43bc8a69b8b136361a0cc8cf8b66adac..6ad394004af40ffb8578615872bec0c44ef0b9f2 100644
--- a/source/RobotComponents/interface/components/MotionPlanning/Tasks/RRTConnect/WorkerNode.ice
+++ b/source/RobotComponents/interface/components/MotionPlanning/Tasks/RRTConnect/WorkerNode.ice
@@ -24,60 +24,58 @@
#pragma once
#include <RobotComponents/interface/components/MotionPlanning/CSpace/CSpace.ice>
-
#include <RobotComponents/interface/components/MotionPlanning/Tasks/RRTConnect/DataStructures.ice>
#include <RobotComponents/interface/components/MotionPlanning/Tasks/RRTConnect/Task.ice>
module armarx
{
-module rrtconnect
-{
- class WorkerNodeBase;
- sequence<WorkerNodeBase*> WorkerNodeBasePrxList;
+ module rrtconnect
+ {
+ class WorkerNodeBase;
+ sequence<WorkerNodeBase*> WorkerNodeBasePrxList;
- /**
+ /**
* @brief The worker used by RRTConnect.
*/
- ["cpp:virtual"]
- class WorkerNodeBase implements TreeUpdateInterface
- {
- //problem
- /**
+ ["cpp:virtual"] class WorkerNodeBase implements TreeUpdateInterface
+ {
+ //problem
+ /**
* @brief The used planning cspace.
*/
- CSpaceBase cspace;
- /**
+ CSpaceBase cspace;
+ /**
* @brief The start config.
*/
- VectorXf startCfg;
- /**
+ VectorXf startCfg;
+ /**
* @brief The goal config.
*/
- VectorXf goalCfg;
+ VectorXf goalCfg;
- //parameters
- /**
+ //parameters
+ /**
* @brief The dcd stepsize.
*/
- float DCDStepSize;
+ float DCDStepSize;
- //management
- /**
+ //management
+ /**
* @brief The workers task.
*/
- TaskBase* task;
+ TaskBase* task;
- /**
+ /**
* @brief The update topics prefix
*/
- string topicName;
+ string topicName;
- long workerId;
- long workerCount;
+ long workerId;
+ long workerCount;
- //management
- ["cpp:const"] Update getUpdate(long updateSubId);
- void setWorkerNodes(WorkerNodeBasePrxList workers);
+ //management
+ ["cpp:const"] Update getUpdate(long updateSubId);
+ void setWorkerNodes(WorkerNodeBasePrxList workers);
+ };
};
};
-};
diff --git a/source/RobotComponents/interface/components/MotionPlanning/Tasks/RandomShortcutPostprocessor/Task.ice b/source/RobotComponents/interface/components/MotionPlanning/Tasks/RandomShortcutPostprocessor/Task.ice
index 412b816e90a437abd1cec1aa1bb1aa69866c6e52..26da42881ac75d8b34010a5c2a2969d086479222 100644
--- a/source/RobotComponents/interface/components/MotionPlanning/Tasks/RandomShortcutPostprocessor/Task.ice
+++ b/source/RobotComponents/interface/components/MotionPlanning/Tasks/RandomShortcutPostprocessor/Task.ice
@@ -27,11 +27,11 @@
module armarx
{
-module rngshortcut
-{
- ["cpp:virtual"]
- class TaskBase extends PostprocessingMotionPlanningTaskBase implements MotionPlanningMultiPathWithCostTaskControlInterface
+ module rngshortcut
{
+ ["cpp:virtual"] class TaskBase extends PostprocessingMotionPlanningTaskBase implements
+ MotionPlanningMultiPathWithCostTaskControlInterface
+ {
["protected"] float dcdStep;
["protected"] long maxTries;
["protected"] long maxTimeForPostprocessingInSeconds;
@@ -47,7 +47,6 @@ module rngshortcut
* E.g.: If 0.01 a shortcut has reduce the path length by >= 0.01 * length of the current path.
*/
["protected"] float minPathImprovementRatio = 0.01;
-
+ };
};
};
-};
diff --git a/source/RobotComponents/interface/components/ObstacleDetection/HumanObstacleDetectionInterface.ice b/source/RobotComponents/interface/components/ObstacleDetection/HumanObstacleDetectionInterface.ice
index c6cb2a574112ff1fd60425a93b49fadba9c0bcd6..4f8e2fbb98ebe085e2e76801fd8cd8efecb7d24f 100644
--- a/source/RobotComponents/interface/components/ObstacleDetection/HumanObstacleDetectionInterface.ice
+++ b/source/RobotComponents/interface/components/ObstacleDetection/HumanObstacleDetectionInterface.ice
@@ -31,12 +31,10 @@
module armarx
{
- interface HumanObstacleDetectionInterface extends
- armarx::OpenPose3DListener
+ interface HumanObstacleDetectionInterface extends armarx::OpenPose3DListener
{
void setEnabled(bool enable);
void enable();
void disable();
};
};
-
diff --git a/source/RobotComponents/interface/components/ObstacleDetection/LaserScannerObstacleDetectionInterface.ice b/source/RobotComponents/interface/components/ObstacleDetection/LaserScannerObstacleDetectionInterface.ice
index a4e8d22052c6bac05860c9d886012f45588bd07d..c555c6ac55ce5fb592e259f9ff579a07ae138cbb 100644
--- a/source/RobotComponents/interface/components/ObstacleDetection/LaserScannerObstacleDetectionInterface.ice
+++ b/source/RobotComponents/interface/components/ObstacleDetection/LaserScannerObstacleDetectionInterface.ice
@@ -38,4 +38,3 @@ module armarx
void disable();
};
};
-
diff --git a/source/RobotComponents/interface/components/PathPlanner.ice b/source/RobotComponents/interface/components/PathPlanner.ice
old mode 100755
new mode 100644
index d0d1f16d12c45cd4d924f613186bda0ad338260a..422e294c5cd70f117d3ac3d60b2dd200835dab3c
--- a/source/RobotComponents/interface/components/PathPlanner.ice
+++ b/source/RobotComponents/interface/components/PathPlanner.ice
@@ -22,13 +22,13 @@
#pragma once
-#include <RobotAPI/interface/core/PoseBase.ice>
-#include <ArmarXCore/interface/core/UserException.ice>
#include <ArmarXCore/interface/core/BasicTypes.ice>
+#include <ArmarXCore/interface/core/UserException.ice>
-#include <MemoryX/interface/memorytypes/MemoryEntities.ice>
#include <RobotAPI/interface/core/PoseBase.ice>
+#include <MemoryX/interface/memorytypes/MemoryEntities.ice>
+
module armarx
{
@@ -51,14 +51,13 @@ module armarx
sequence<ObjectPositionBase> ObjectPositionBaseList;
-
["cpp:virtual"]
- /**
+ /**
* @brief Superclass for all path planners.
*
* Does administration work. (setting of agent and collision object)
*/
- class PathPlannerBase
+ class PathPlannerBase
{
/**
* @brief Sets the collision objects.
@@ -82,7 +81,8 @@ module armarx
* @param agent The agent.
* @param agentColModelName The agent's collision model.
*/
- void setAgent(memoryx::AgentInstanceBase agent, string agentColModelName) throws InvalidArgumentException;
+ void setAgent(memoryx::AgentInstanceBase agent, string agentColModelName)
+ throws InvalidArgumentException;
/**
* @brief Sets the safety margin. (the distance agent <-> any object will always be >= or no path will be found)
@@ -97,8 +97,7 @@ module armarx
* @param to The destination point.
* @return Returns the path. If no path was found an empty list is returned.
*/
- ["cpp:const"]
- Vector3BaseList getPath(Vector3Base from, Vector3Base to);
+ ["cpp:const"] Vector3BaseList getPath(Vector3Base from, Vector3Base to);
};
/**
@@ -116,8 +115,7 @@ module armarx
/**
* @brief Implements path planning using A*.
*/
- ["cpp:virtual"]
- class AStarPathPlannerBase extends PathPlannerBase
+ ["cpp:virtual"] class AStarPathPlannerBase extends PathPlannerBase
{
/**
* @brief Sets the translation step size. (size of steps along x- or y-axis)
@@ -177,4 +175,3 @@ module armarx
["protected"] float distanceFactorForHeuristic;
};
};
-
diff --git a/source/RobotComponents/interface/components/PlannedMotionProviderInterface.ice b/source/RobotComponents/interface/components/PlannedMotionProviderInterface.ice
index 6dea573a6c8e0cfc1d236772c9d9fa427ba2b2ac..3584a41169d52da4c3f4772c70e8d7cd2bf00d95 100644
--- a/source/RobotComponents/interface/components/PlannedMotionProviderInterface.ice
+++ b/source/RobotComponents/interface/components/PlannedMotionProviderInterface.ice
@@ -1,9 +1,10 @@
-#include <RobotComponents/interface/components/GraspingManager/GraspingManagerInterface.ice>
-#include <RobotComponents/interface/components/GraspingManager/GraspGeneratorInterface.ice>
-#include <RobotComponents/interface/components/MotionPlanning/CSpace/SimoxCSpace.ice>
#include <ArmarXCore/interface/core/UserException.ice>
+
#include <RobotAPI/interface/core/Trajectory.ice>
+#include <RobotComponents/interface/components/GraspingManager/GraspGeneratorInterface.ice>
+#include <RobotComponents/interface/components/GraspingManager/GraspingManagerInterface.ice>
+#include <RobotComponents/interface/components/MotionPlanning/CSpace/SimoxCSpace.ice>
module armarx
@@ -11,9 +12,15 @@ module armarx
interface PlannedMotionProviderInterface
{
- GraspingTrajectory planMotion(SimoxCSpaceBase cSpace, SimoxCSpaceBase cspacePlatform, MotionPlanningData mpd) throws RuntimeError;
- GraspingTrajectory planMotionParallel(SimoxCSpaceBase cSpace, SimoxCSpaceBase cspacePlatform, MotionPlanningData mpd) throws RuntimeError;
- TrajectoryBase planJointMotion(SimoxCSpaceBase cSpace, MotionPlanningData mpd) throws RuntimeError;
- TrajectoryBase planPlatformMotion(SimoxCSpaceBase cspacePlatform, MotionPlanningData mpd) throws RuntimeError;
+ GraspingTrajectory planMotion(SimoxCSpaceBase cSpace,
+ SimoxCSpaceBase cspacePlatform,
+ MotionPlanningData mpd) throws RuntimeError;
+ GraspingTrajectory planMotionParallel(SimoxCSpaceBase cSpace,
+ SimoxCSpaceBase cspacePlatform,
+ MotionPlanningData mpd) throws RuntimeError;
+ TrajectoryBase planJointMotion(SimoxCSpaceBase cSpace, MotionPlanningData mpd)
+ throws RuntimeError;
+ TrajectoryBase planPlatformMotion(SimoxCSpaceBase cspacePlatform, MotionPlanningData mpd)
+ throws RuntimeError;
};
};
diff --git a/source/RobotComponents/interface/components/RobotIK.ice b/source/RobotComponents/interface/components/RobotIK.ice
index d436cd9eb0b2e7c0136881c4df04e82e699d2341..666573adc769fb30288b2239f9cc8fe9b3fd6a41 100644
--- a/source/RobotComponents/interface/components/RobotIK.ice
+++ b/source/RobotComponents/interface/components/RobotIK.ice
@@ -32,7 +32,7 @@ module armarx
* Available Cartesian selection options for target poses.
* See VirtualRobot::IKSolver for more information.
*/
- enum CartesianSelection
+ enum CartesianSelection
{
eX,
eY,
@@ -44,7 +44,8 @@ module armarx
sequence<string> NodeNameList;
- struct WorkspaceBounds {
+ struct WorkspaceBounds
+ {
float x;
float y;
float z;
@@ -53,14 +54,14 @@ module armarx
float ya;
};
- enum InverseJacobiMethod
+ enum InverseJacobiMethod
{
eSVD,
eSVDDamped,
eTranspose,
};
- struct CoMIKDescriptor
+ struct CoMIKDescriptor
{
// REQUIRED:
int priority; // priority for hierarchical ik
@@ -69,7 +70,8 @@ module armarx
string robotNodeSetBodies; // body set for which the center of mass is computed
float tolerance; // error tolerance
// OPTIONAL:
- string coordSysName; // coordinate system name in which the center of mass is defined; empty string for global system
+ string
+ coordSysName; // coordinate system name in which the center of mass is defined; empty string for global system
};
struct DifferentialIKDescriptor
@@ -96,7 +98,7 @@ module armarx
interface RobotIKInterface
{
-
+
// TODO: throw exceptions
/**
@@ -112,10 +114,9 @@ module armarx
* the orientation only or all.
* @return A map that maps each joint name to its value in the found IK solution.
*/
- NameValueMap computeIKFramedPose(string robotNodeSetName,
- FramedPoseBase tcpPose,
- CartesianSelection cs);
-
+ NameValueMap computeIKFramedPose(
+ string robotNodeSetName, FramedPoseBase tcpPose, CartesianSelection cs);
+
/**
* @brief Computes a single IK solution for the given robot node set and desired global TCP pose.
* @details The TCP pose is assumed to be in the global frame. The CartesianSelection
@@ -128,9 +129,8 @@ module armarx
* the orientation only or all.
* @return A map that maps each joint name to its value in the found IK solution.
*/
- NameValueMap computeIKGlobalPose(string robotNodeSetName,
- PoseBase tcpPose,
- CartesianSelection cs);
+ NameValueMap computeIKGlobalPose(
+ string robotNodeSetName, PoseBase tcpPose, CartesianSelection cs);
/**
* @brief Computes a single IK solution, error and reachability for the given robot node set and desired global TCP pose.
@@ -144,9 +144,8 @@ module armarx
* the orientation only or all.
* @return A map that maps each joint name to its value in the found IK solution, the reachability and computational error.
*/
- ExtendedIKResult computeExtendedIKGlobalPose(string robotNodeSetName,
- PoseBase tcpPose,
- CartesianSelection cs);
+ ExtendedIKResult computeExtendedIKGlobalPose(
+ string robotNodeSetName, PoseBase tcpPose, CartesianSelection cs);
/**
* @brief Computes an IK solution for the given robot joints such that the center of mass lies above the
@@ -159,8 +158,7 @@ module armarx
*
* @return The ik-solution. Returns an empty vector if there is no solution.
*/
- NameValueMap computeCoMIK(string robotNodeSetJoints,
- CoMIKDescriptor comIK);
+ NameValueMap computeCoMIK(string robotNodeSetJoints, CoMIKDescriptor comIK);
/**
* @brief Computes a configuration gradient in order to solve several tasks/constraints simultaneously.
@@ -186,11 +184,11 @@ module armarx
* @return A configuration gradient...
*/
NameValueMap computeHierarchicalDeltaIK(string robotNodeSet,
- IKTasks diffIKs,
- CoMIKDescriptor comIKs,
- float stepSize,
- bool avoidJointLimits,
- bool enableCenterOfMass);
+ IKTasks diffIKs,
+ CoMIKDescriptor comIKs,
+ float stepSize,
+ bool avoidJointLimits,
+ bool enableCenterOfMass);
/**
* @brief Defines a new robot node set.
* @details Define a new robot node set with the given name that consists out of the given list of nodes with given
@@ -221,8 +219,13 @@ module armarx
* @return True iff the a reachability space for the given robot node set is available after execution of this function.
* False in case of a failure, e.g. there is no chain with the given name.
*/
- bool createReachabilitySpace(string chainName, string coordinateSystem, float stepTranslation, float stepRotation,
- WorkspaceBounds minBounds, WorkspaceBounds maxBounds, int numSamples);
+ bool createReachabilitySpace(string chainName,
+ string coordinateSystem,
+ float stepTranslation,
+ float stepRotation,
+ WorkspaceBounds minBounds,
+ WorkspaceBounds maxBounds,
+ int numSamples);
/**
* @brief Returns whether this component has a reachability space for the given robot node set.
@@ -270,7 +273,8 @@ module armarx
* @return True, if the pose is reachable by the TCP of the given chain. False, if it is not reachable or
* there is no reachability space for the given chain available.
*/
- ["cpp:const"] idempotent bool isFramedPoseReachable(string chainName, FramedPoseBase framedPose);
+ ["cpp:const"] idempotent bool isFramedPoseReachable(string chainName,
+ FramedPoseBase framedPose);
/**
* @brief Returns whether a given global pose is currently reachable by the TCP of the given robot node set.
@@ -286,7 +290,5 @@ module armarx
* there is no reachability space for the given chain available.
*/
["cpp:const"] idempotent bool isPoseReachable(string chainName, PoseBase framedPose);
-
-
};
};
diff --git a/source/RobotComponents/interface/components/ViconMarkerProviderInterface.ice b/source/RobotComponents/interface/components/ViconMarkerProviderInterface.ice
index 32e6ce7ff6cb031ce1c73fb4dc88c7401ca33592..c3c5ee5c317c578e207d658cfb044ddee3e856fd 100644
--- a/source/RobotComponents/interface/components/ViconMarkerProviderInterface.ice
+++ b/source/RobotComponents/interface/components/ViconMarkerProviderInterface.ice
@@ -1,7 +1,7 @@
#pragma once
-#include <ArmarXCore/interface/core/UserException.ice>
#include <ArmarXCore/interface/core/BasicVectorTypes.ice>
+#include <ArmarXCore/interface/core/UserException.ice>
module armarx
{
@@ -12,5 +12,4 @@ module armarx
void reportLabeledViconMarkerFrame(StringVector3fMap labeledMarkers);
void reportUnlabeledViconMarkerFrame(Vector3fSeq unlabeledMarkers);
};
-
};
diff --git a/source/RobotComponents/libraries/MMM/MotionFileWrapper.cpp b/source/RobotComponents/libraries/MMM/MotionFileWrapper.cpp
index 95859fa39ea36f5d1c088026300ec5a9b0bbf5ec..40731bd543e9f12a6bf56660387db0519b7fd778 100644
--- a/source/RobotComponents/libraries/MMM/MotionFileWrapper.cpp
+++ b/source/RobotComponents/libraries/MMM/MotionFileWrapper.cpp
@@ -25,32 +25,36 @@
#include "MotionFileWrapper.h"
-#include <MMM/Motion/MotionReaderXML.h>
-#include <MMM/Motion/Legacy/LegacyMotionReaderXML.h>
-#include <MMM/Motion/Legacy/LegacyMotion.h>
-#include <MMM/Motion/Plugin/KinematicPlugin/KinematicSensor.h>
-#include <MMM/Motion/Plugin/ModelPosePlugin/ModelPoseSensor.h>
-#include <MMM/Model/ModelProcessorWinter.h>
-
-#include <ArmarXCore/observers/filters/ButterworthFilter.h>
-#include <ArmarXCore/core/logging/Logging.h>
+#include <Eigen/LU>
-#include <VirtualRobot/MathTools.h>
-#include <VirtualRobot/Robot.h>
#include <SimoxUtility/math/convert.h>
#include <SimoxUtility/xml/rapidxml/RapidXMLWrapper.h>
-#include <MMM/Motion/XMLTools.h>
+#include <VirtualRobot/MathTools.h>
+#include <VirtualRobot/Robot.h>
-#include <Eigen/LU>
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/observers/filters/ButterworthFilter.h>
+
+#include <MMM/Model/ModelProcessorWinter.h>
+#include <MMM/Motion/Legacy/LegacyMotion.h>
+#include <MMM/Motion/Legacy/LegacyMotionReaderXML.h>
+#include <MMM/Motion/MotionReaderXML.h>
+#include <MMM/Motion/Plugin/KinematicPlugin/KinematicSensor.h>
+#include <MMM/Motion/Plugin/ModelPosePlugin/ModelPoseSensor.h>
+#include <MMM/Motion/XMLTools.h>
using namespace armarx;
using namespace MMM;
-MotionFileWrapperPtr MotionFileWrapper::create(const std::string& motionFilePath, double butterworthFilterCutOffFreq, const std::string relativeModelRoot)
+MotionFileWrapperPtr
+MotionFileWrapper::create(const std::string& motionFilePath,
+ double butterworthFilterCutOffFreq,
+ const std::string relativeModelRoot)
{
MotionFileWrapperPtr wrapper(new MotionFileWrapper(butterworthFilterCutOffFreq));
- simox::xml::RapidXMLWrapperRootNodePtr root = simox::xml::RapidXMLWrapperRootNode::FromFile(motionFilePath);
+ simox::xml::RapidXMLWrapperRootNodePtr root =
+ simox::xml::RapidXMLWrapperRootNode::FromFile(motionFilePath);
if (root && root->name() == xml::tag::MMM_ROOT)
{
if (!root->has_attribute(xml::attribute::VERSION))
@@ -92,11 +96,14 @@ MotionFileWrapperPtr MotionFileWrapper::create(const std::string& motionFilePath
return wrapper;
}
-MotionFileWrapper::MotionFileWrapper(int butterworthFilterCutOffFreq) : butterworthFilterCutOffFreq(butterworthFilterCutOffFreq)
+MotionFileWrapper::MotionFileWrapper(int butterworthFilterCutOffFreq) :
+ butterworthFilterCutOffFreq(butterworthFilterCutOffFreq)
{
}
-bool MotionFileWrapper::loadMotion(const std::string& motionFilePath, const std::string relativeModelRoot)
+bool
+MotionFileWrapper::loadMotion(const std::string& motionFilePath,
+ const std::string relativeModelRoot)
{
ARMARX_INFO << "Loading motion";
MotionRecordingPtr motions;
@@ -117,16 +124,20 @@ bool MotionFileWrapper::loadMotion(const std::string& motionFilePath, const std:
ModelPtr model = motion->getModel();
if (model)
{
- if (!model->getFilename().empty() && std::filesystem::path(model->getFilename()).stem() == relativeModelRoot)
+ if (!model->getFilename().empty() &&
+ std::filesystem::path(model->getFilename()).stem() == relativeModelRoot)
{
auto modelPoseSensor = motion->getSensorByType<ModelPoseSensor>();
if (modelPoseSensor && modelPoseSensor->getTimesteps().size() > 0)
{
- auto modelPoseSensorMeasurement = modelPoseSensor->getDerivedMeasurement(modelPoseSensor->getTimesteps().at(0));
+ auto modelPoseSensorMeasurement = modelPoseSensor->getDerivedMeasurement(
+ modelPoseSensor->getTimesteps().at(0));
Eigen::Matrix4f rootPose = modelPoseSensorMeasurement->getRootPose();
Eigen::Vector3f orientation = simox::math::mat4f_to_rpy(rootPose);
- modelRootTransformation.block(0, 0, 3, 3) = simox::math::rpy_to_mat3f(0, 0, -orientation(2));
- modelRootTransformation.block(0, 3, 3, 1) = Eigen::Vector3f(-rootPose(0, 3), -rootPose(1, 3), 0);
+ modelRootTransformation.block(0, 0, 3, 3) =
+ simox::math::rpy_to_mat3f(0, 0, -orientation(2));
+ modelRootTransformation.block(0, 3, 3, 1) =
+ Eigen::Vector3f(-rootPose(0, 3), -rootPose(1, 3), 0);
}
}
}
@@ -139,13 +150,14 @@ bool MotionFileWrapper::loadMotion(const std::string& motionFilePath, const std:
ModelPtr model = motion->getModel();
if (model)
{
- m->modelPath = model->getFilename(); // TODO if model could not be loaded!
+ m->modelPath = model->getFilename(); // TODO if model could not be loaded!
}
ModelPoseSensorPtr modelPoseSensor = motion->getSensorByType<ModelPoseSensor>();
if (!modelPoseSensor)
{
- ARMARX_ERROR << "Ignoring motion with name '" << motion->getName() << "', because it has no model pose sensor!";
+ ARMARX_ERROR << "Ignoring motion with name '" << motion->getName()
+ << "', because it has no model pose sensor!";
break;
}
@@ -154,8 +166,10 @@ bool MotionFileWrapper::loadMotion(const std::string& motionFilePath, const std:
m->numberOfFrames = m->timestamp.size();
for (float timestep : m->timestamp)
{
- ModelPoseSensorMeasurementPtr modelPoseSensorMeasurement = modelPoseSensor->getDerivedMeasurement(timestep);
- Eigen::Matrix4f pose = modelRootTransformation * modelPoseSensorMeasurement->getRootPose();
+ ModelPoseSensorMeasurementPtr modelPoseSensorMeasurement =
+ modelPoseSensor->getDerivedMeasurement(timestep);
+ Eigen::Matrix4f pose =
+ modelRootTransformation * modelPoseSensorMeasurement->getRootPose();
m->poseTrajData[timestep] = getTrajData(pose);
}
@@ -171,16 +185,19 @@ bool MotionFileWrapper::loadMotion(const std::string& motionFilePath, const std:
std::vector<filters::ButterworthFilter> filters;
for (size_t j = 0; j < jointNames.size(); j++)
{
- filters.push_back(filters::ButterworthFilter(butterworthFilterCutOffFreq, 100, Lowpass, 5));
+ filters.push_back(
+ filters::ButterworthFilter(butterworthFilterCutOffFreq, 100, Lowpass, 5));
}
bool initialized = false;
for (float timestep : m->timestamp)
{
- KinematicSensorMeasurementPtr kinematicSensorMeasurement = kinematicSensor->getDerivedMeasurement(timestep); // TODO nullptr
+ KinematicSensorMeasurementPtr kinematicSensorMeasurement =
+ kinematicSensor->getDerivedMeasurement(timestep); // TODO nullptr
if (!kinematicSensorMeasurement)
{
- MMM_ERROR << "No kinematic measurement at timestep " << timestep << " for motion with name '" << motion->getName() << "'";
+ MMM_ERROR << "No kinematic measurement at timestep " << timestep
+ << " for motion with name '" << motion->getName() << "'";
goto end;
}
Eigen::VectorXf jointAngles = kinematicSensorMeasurement->getJointAngles();
@@ -194,24 +211,28 @@ bool MotionFileWrapper::loadMotion(const std::string& motionFilePath, const std:
filters[j].update(0, new Variant(jointAngles[j]));
jointAngles[j] = filters[j].getValue()->getDouble();
}
- kSensor->addSensorMeasurement(KinematicSensorMeasurementPtr(new KinematicSensorMeasurement(timestep, jointAngles)));
+ kSensor->addSensorMeasurement(KinematicSensorMeasurementPtr(
+ new KinematicSensorMeasurement(timestep, jointAngles)));
}
}
for (float timestep : m->timestamp)
{
- KinematicSensorMeasurementPtr kinematicSensorMeasurement = kSensor->getDerivedMeasurement(timestep);
+ KinematicSensorMeasurementPtr kinematicSensorMeasurement =
+ kSensor->getDerivedMeasurement(timestep);
if (kinematicSensorMeasurement)
{
- m->jointTrajData.push_back(getTrajData(kinematicSensorMeasurement->getJointAngles()));
+ m->jointTrajData.push_back(
+ getTrajData(kinematicSensorMeasurement->getJointAngles()));
}
else
{
- MMM_ERROR << "No kinematic measurement at timestep " << timestep << " for motion with name '" << motion->getName() << "'";
+ MMM_ERROR << "No kinematic measurement at timestep " << timestep
+ << " for motion with name '" << motion->getName() << "'";
goto end;
}
}
-end:
+ end:
continue;
}
if (model)
@@ -220,7 +241,8 @@ end:
}
else
{
- auto modelProcessor = std::dynamic_pointer_cast<MMM::ModelProcessorWinter>(motion->getModelProcessor());
+ auto modelProcessor =
+ std::dynamic_pointer_cast<MMM::ModelProcessorWinter>(motion->getModelProcessor());
if (modelProcessor)
{
m->scaling = modelProcessor->getHeight();
@@ -237,7 +259,9 @@ end:
return motionData.size();
}
-bool MotionFileWrapper::loadLegacyMotion(const std::string& motionFilePath, const std::string relativeModelRoot)
+bool
+MotionFileWrapper::loadLegacyMotion(const std::string& motionFilePath,
+ const std::string relativeModelRoot)
{
ARMARX_INFO << "Loading legacy motion";
LegacyMotionReaderXML reader;
@@ -247,7 +271,8 @@ bool MotionFileWrapper::loadLegacyMotion(const std::string& motionFilePath, cons
for (auto motion : motions)
{
ModelPtr model = motion->getModel();
- std::filesystem::path filename = model ? std::filesystem::path(model->getFilename()) : motion->getModelFilePath();
+ std::filesystem::path filename =
+ model ? std::filesystem::path(model->getFilename()) : motion->getModelFilePath();
if (std::filesystem::path(filename).stem() == relativeModelRoot)
{
auto motionFrames = motion->getMotionFrames();
@@ -255,8 +280,10 @@ bool MotionFileWrapper::loadLegacyMotion(const std::string& motionFilePath, cons
{
Eigen::Matrix4f rootPose = motionFrames.at(0)->getRootPose();
Eigen::Vector3f orientation = simox::math::mat4f_to_rpy(rootPose);
- modelRootTransformation.block(0, 0, 3, 3) = simox::math::rpy_to_mat3f(0, 0, -orientation(2));
- modelRootTransformation.block(0, 3, 3, 1) = Eigen::Vector3f(-rootPose(0, 3), -rootPose(1, 3), 0);
+ modelRootTransformation.block(0, 0, 3, 3) =
+ simox::math::rpy_to_mat3f(0, 0, -orientation(2));
+ modelRootTransformation.block(0, 3, 3, 1) =
+ Eigen::Vector3f(-rootPose(0, 3), -rootPose(1, 3), 0);
}
}
}
@@ -299,7 +326,8 @@ bool MotionFileWrapper::loadLegacyMotion(const std::string& motionFilePath, cons
}
else
{
- auto modelProcessor = std::dynamic_pointer_cast<MMM::ModelProcessorWinter>(motion->getModelProcessor());
+ auto modelProcessor =
+ std::dynamic_pointer_cast<MMM::ModelProcessorWinter>(motion->getModelProcessor());
if (modelProcessor)
{
m->scaling = modelProcessor->getHeight();
@@ -316,7 +344,8 @@ bool MotionFileWrapper::loadLegacyMotion(const std::string& motionFilePath, cons
return motionData.size();
}
-::Ice::DoubleSeq MotionFileWrapper::getTrajData(const Eigen::Matrix4f& rootPose)
+::Ice::DoubleSeq
+MotionFileWrapper::getTrajData(const Eigen::Matrix4f& rootPose)
{
VirtualRobot::MathTools::Quaternion q = VirtualRobot::MathTools::eigen4f2quat(rootPose);
::Ice::DoubleSeq curData;
@@ -330,7 +359,8 @@ bool MotionFileWrapper::loadLegacyMotion(const std::string& motionFilePath, cons
return curData;
}
-DoubleSeqSeq MotionFileWrapper::getTrajData(const Eigen::VectorXf& jointAngles)
+DoubleSeqSeq
+MotionFileWrapper::getTrajData(const Eigen::VectorXf& jointAngles)
{
DoubleSeqSeq curdata;
for (int j = 0; j < jointAngles.rows(); j++)
@@ -342,7 +372,8 @@ DoubleSeqSeq MotionFileWrapper::getTrajData(const Eigen::VectorXf& jointAngles)
return curdata;
}
-MotionDataPtr MotionFileWrapper::getFirstMotionData()
+MotionDataPtr
+MotionFileWrapper::getFirstMotionData()
{
if (motionNames.size() == 0)
{
@@ -352,10 +383,10 @@ MotionDataPtr MotionFileWrapper::getFirstMotionData()
{
return motionData[motionNames.at(0)];
}
-
}
-MotionDataPtr MotionFileWrapper::getMotionDataByModel(const std::string& modelName)
+MotionDataPtr
+MotionFileWrapper::getMotionDataByModel(const std::string& modelName)
{
for (auto m : motionData)
{
@@ -367,12 +398,14 @@ MotionDataPtr MotionFileWrapper::getMotionDataByModel(const std::string& modelNa
return nullptr;
}
-MotionDataPtr MotionFileWrapper::getMotionData(const std::string& motionName)
+MotionDataPtr
+MotionFileWrapper::getMotionData(const std::string& motionName)
{
return motionData[motionName];
}
-::Ice::StringSeq MotionFileWrapper::getModelNames()
+::Ice::StringSeq
+MotionFileWrapper::getModelNames()
{
::Ice::StringSeq modelNames;
for (auto m : motionData)
@@ -382,7 +415,8 @@ MotionDataPtr MotionFileWrapper::getMotionData(const std::string& motionName)
return modelNames;
}
-::Ice::StringSeq MotionFileWrapper::getMotionNames()
+::Ice::StringSeq
+MotionFileWrapper::getMotionNames()
{
return motionNames;
}
diff --git a/source/RobotComponents/libraries/MMM/MotionFileWrapper.h b/source/RobotComponents/libraries/MMM/MotionFileWrapper.h
index a34571c1dfa5f322da09ee88e5ae00c25c74a3a5..f1ea5cc5ea7c404837eb60c26633cb418f1c250f 100644
--- a/source/RobotComponents/libraries/MMM/MotionFileWrapper.h
+++ b/source/RobotComponents/libraries/MMM/MotionFileWrapper.h
@@ -36,7 +36,8 @@ namespace armarx
{
}
- void setScaling(float value)
+ void
+ setScaling(float value)
{
double factor = value / scaling;
for (auto& data : poseTrajData)
@@ -53,12 +54,14 @@ namespace armarx
::Ice::DoubleSeq timestamp;
std::map<double, Ice::DoubleSeq> poseTrajData;
- TrajectoryBasePtr getPoseTrajectory()
+ TrajectoryBasePtr
+ getPoseTrajectory()
{
return TrajectoryBasePtr(new Trajectory(poseTrajData));
}
- TrajectoryBasePtr getJointTrajectory()
+ TrajectoryBasePtr
+ getJointTrajectory()
{
return TrajectoryBasePtr(new Trajectory(jointTrajData, timestamp, jointNames));
}
@@ -77,7 +80,9 @@ namespace armarx
class MotionFileWrapper
{
public:
- static MotionFileWrapperPtr create(const std::string& motionFilePath, double butterworthFilterCutOffFreq = 0.0, const std::string relativeModelRoot = "mmm");
+ static MotionFileWrapperPtr create(const std::string& motionFilePath,
+ double butterworthFilterCutOffFreq = 0.0,
+ const std::string relativeModelRoot = "mmm");
MotionDataPtr getFirstMotionData();
MotionDataPtr getMotionDataByModel(const std::string& modelName);
@@ -89,8 +94,10 @@ namespace armarx
private:
MotionFileWrapper(int butterworthFilterCutOffFreq = 0);
- bool loadMotion(const std::string& motionFilePath, const std::string relativeModelRoot = "mmm");
- bool loadLegacyMotion(const std::string& motionFilePath, const std::string relativeModelRoot = "mmm");
+ bool loadMotion(const std::string& motionFilePath,
+ const std::string relativeModelRoot = "mmm");
+ bool loadLegacyMotion(const std::string& motionFilePath,
+ const std::string relativeModelRoot = "mmm");
::Ice::DoubleSeq getTrajData(const Eigen::Matrix4f& rootPose);
DoubleSeqSeq getTrajData(const Eigen::VectorXf& jointAngles);
@@ -99,6 +106,6 @@ namespace armarx
std::vector<std::string> motionNames;
double butterworthFilterCutOffFreq;
};
-}
+} // namespace armarx
#endif // _ARMARX_COMPONENT_RobotComponents_MotionFileWrapper_H
diff --git a/source/RobotComponents/libraries/RobotComponentsComponentPlugins/GraspGeneratorComponentPlugin.cpp b/source/RobotComponents/libraries/RobotComponentsComponentPlugins/GraspGeneratorComponentPlugin.cpp
index bbae85e4cf85ef3329605937635a8c67f3c72b00..dfd6ca9981afb745497a7f8c28554c2ccd6a7344 100644
--- a/source/RobotComponents/libraries/RobotComponentsComponentPlugins/GraspGeneratorComponentPlugin.cpp
+++ b/source/RobotComponents/libraries/RobotComponentsComponentPlugins/GraspGeneratorComponentPlugin.cpp
@@ -2,41 +2,43 @@
#include <ArmarXCore/core/Component.h>
-
namespace armarx
{
namespace plugins
{
- void GraspGeneratorComponentPlugin::preOnInitComponent()
+ void
+ GraspGeneratorComponentPlugin::preOnInitComponent()
{
parent<Component>().usingProxyFromProperty(PROPERTY_NAME);
}
- void GraspGeneratorComponentPlugin::preOnConnectComponent()
+ void
+ GraspGeneratorComponentPlugin::preOnConnectComponent()
{
parent<Component>().getProxyFromProperty(_graspGenerator, PROPERTY_NAME);
}
- void GraspGeneratorComponentPlugin::postCreatePropertyDefinitions(armarx::PropertyDefinitionsPtr& properties)
+ void
+ GraspGeneratorComponentPlugin::postCreatePropertyDefinitions(
+ armarx::PropertyDefinitionsPtr& properties)
{
if (!properties->hasDefinition(PROPERTY_NAME))
{
properties->defineOptionalProperty<std::string>(
- PROPERTY_NAME,
- "GraspGenerator",
- "Name of the GraspGenerator");
+ PROPERTY_NAME, "GraspGenerator", "Name of the GraspGenerator");
}
}
- GraspGeneratorInterfacePrx GraspGeneratorComponentPlugin::getGraspGenerator()
+ GraspGeneratorInterfacePrx
+ GraspGeneratorComponentPlugin::getGraspGenerator()
{
return _graspGenerator;
}
- }
-}
+ } // namespace plugins
+} // namespace armarx
namespace armarx
{
@@ -46,10 +48,11 @@ namespace armarx
addPlugin(plugin);
}
- GraspGeneratorInterfacePrx GraspGeneratorComponentPluginUser::getGraspGenerator()
+ GraspGeneratorInterfacePrx
+ GraspGeneratorComponentPluginUser::getGraspGenerator()
{
return plugin->getGraspGenerator();
}
-}
+} // namespace armarx
diff --git a/source/RobotComponents/libraries/RobotComponentsComponentPlugins/GraspGeneratorComponentPlugin.h b/source/RobotComponents/libraries/RobotComponentsComponentPlugins/GraspGeneratorComponentPlugin.h
index 1ca648939e32bef92034d638bc32da9e4c39575e..fdfff0aa2c14c4e67a72b7f7722c49643149ed77 100644
--- a/source/RobotComponents/libraries/RobotComponentsComponentPlugins/GraspGeneratorComponentPlugin.h
+++ b/source/RobotComponents/libraries/RobotComponentsComponentPlugins/GraspGeneratorComponentPlugin.h
@@ -1,8 +1,8 @@
#pragma once
#include <ArmarXCore/core/ComponentPlugin.h>
-#include <RobotComponents/interface/components/GraspingManager/GraspGeneratorInterface.h>
+#include <RobotComponents/interface/components/GraspingManager/GraspGeneratorInterface.h>
namespace armarx
{
@@ -27,9 +27,8 @@ namespace armarx
GraspGeneratorInterfacePrx _graspGenerator;
};
- }
-}
-
+ } // namespace plugins
+} // namespace armarx
#include <ArmarXCore/core/ManagedIceObject.h>
@@ -48,8 +47,7 @@ namespace armarx
armarx::plugins::GraspGeneratorComponentPlugin* plugin = nullptr;
};
-}
-
+} // namespace armarx
namespace armarx
{
@@ -57,5 +55,5 @@ namespace armarx
{
// Legacy typedef.
using GraspGeneratorComponentPluginUser = armarx::GraspGeneratorComponentPluginUser;
- }
-}
+ } // namespace plugins
+} // namespace armarx
diff --git a/source/RobotComponents/libraries/RobotComponentsComponentPlugins/GraspSelectionManagerComponentPlugin.cpp b/source/RobotComponents/libraries/RobotComponentsComponentPlugins/GraspSelectionManagerComponentPlugin.cpp
index 5107af73f04528795a84fb75f69b26bbecbf7cb1..e05225b0f906adf9c46220270d000013bceb2192 100644
--- a/source/RobotComponents/libraries/RobotComponentsComponentPlugins/GraspSelectionManagerComponentPlugin.cpp
+++ b/source/RobotComponents/libraries/RobotComponentsComponentPlugins/GraspSelectionManagerComponentPlugin.cpp
@@ -2,41 +2,43 @@
#include <ArmarXCore/core/Component.h>
-
namespace armarx
{
namespace plugins
{
- void GraspSelectionManagerComponentPlugin::preOnInitComponent()
+ void
+ GraspSelectionManagerComponentPlugin::preOnInitComponent()
{
parent<Component>().usingProxyFromProperty(PROPERTY_NAME);
}
- void GraspSelectionManagerComponentPlugin::preOnConnectComponent()
+ void
+ GraspSelectionManagerComponentPlugin::preOnConnectComponent()
{
parent<Component>().getProxyFromProperty(_graspSelectionManager, PROPERTY_NAME);
}
- void GraspSelectionManagerComponentPlugin::postCreatePropertyDefinitions(armarx::PropertyDefinitionsPtr& properties)
+ void
+ GraspSelectionManagerComponentPlugin::postCreatePropertyDefinitions(
+ armarx::PropertyDefinitionsPtr& properties)
{
if (!properties->hasDefinition(PROPERTY_NAME))
{
properties->defineOptionalProperty<std::string>(
- PROPERTY_NAME,
- "GraspSelectionManager",
- "Name of the GraspSelectionManager");
+ PROPERTY_NAME, "GraspSelectionManager", "Name of the GraspSelectionManager");
}
}
- GraspSelectionManagerInterfacePrx GraspSelectionManagerComponentPlugin::getGraspSelectionManager()
+ GraspSelectionManagerInterfacePrx
+ GraspSelectionManagerComponentPlugin::getGraspSelectionManager()
{
return _graspSelectionManager;
}
- }
-}
+ } // namespace plugins
+} // namespace armarx
namespace armarx
{
@@ -46,10 +48,11 @@ namespace armarx
addPlugin(plugin);
}
- GraspSelectionManagerInterfacePrx GraspSelectionManagerComponentPluginUser::getGraspSelectionManager()
+ GraspSelectionManagerInterfacePrx
+ GraspSelectionManagerComponentPluginUser::getGraspSelectionManager()
{
return plugin->getGraspSelectionManager();
}
-}
+} // namespace armarx
diff --git a/source/RobotComponents/libraries/RobotComponentsComponentPlugins/GraspSelectionManagerComponentPlugin.h b/source/RobotComponents/libraries/RobotComponentsComponentPlugins/GraspSelectionManagerComponentPlugin.h
index bbc521abcc3cf31b5c5adb3b9213f4fdb8c10dc1..439a10c3fc0c79af89bbbd690b09b38ad4af32f0 100644
--- a/source/RobotComponents/libraries/RobotComponentsComponentPlugins/GraspSelectionManagerComponentPlugin.h
+++ b/source/RobotComponents/libraries/RobotComponentsComponentPlugins/GraspSelectionManagerComponentPlugin.h
@@ -1,8 +1,8 @@
#pragma once
#include <ArmarXCore/core/ComponentPlugin.h>
-#include <RobotComponents/interface/components/GraspingManager/GraspSelectionManagerInterface.h>
+#include <RobotComponents/interface/components/GraspingManager/GraspSelectionManagerInterface.h>
namespace armarx
{
@@ -27,9 +27,8 @@ namespace armarx
GraspSelectionManagerInterfacePrx _graspSelectionManager;
};
- }
-}
-
+ } // namespace plugins
+} // namespace armarx
#include <ArmarXCore/core/ManagedIceObject.h>
@@ -48,14 +47,14 @@ namespace armarx
armarx::plugins::GraspSelectionManagerComponentPlugin* plugin = nullptr;
};
-}
-
+} // namespace armarx
namespace armarx
{
namespace plugins
{
// Legacy typedef.
- using GraspSelectionManagerComponentPluginUser = armarx::GraspSelectionManagerComponentPluginUser;
- }
-}
+ using GraspSelectionManagerComponentPluginUser =
+ armarx::GraspSelectionManagerComponentPluginUser;
+ } // namespace plugins
+} // namespace armarx
diff --git a/source/RobotComponents/libraries/RobotComponentsComponentPlugins/PlannedMotionProviderComponentPlugin.cpp b/source/RobotComponents/libraries/RobotComponentsComponentPlugins/PlannedMotionProviderComponentPlugin.cpp
index 07077c601c5e2088e8b721f36045225f4ff7e7ac..5d098842226054295b64bf711ecbcb8f0b37e77c 100644
--- a/source/RobotComponents/libraries/RobotComponentsComponentPlugins/PlannedMotionProviderComponentPlugin.cpp
+++ b/source/RobotComponents/libraries/RobotComponentsComponentPlugins/PlannedMotionProviderComponentPlugin.cpp
@@ -2,41 +2,43 @@
#include <ArmarXCore/core/Component.h>
-
namespace armarx
{
namespace plugins
{
- void PlannedMotionProviderComponentPlugin::preOnInitComponent()
+ void
+ PlannedMotionProviderComponentPlugin::preOnInitComponent()
{
parent<Component>().usingProxyFromProperty(PROPERTY_NAME);
}
- void PlannedMotionProviderComponentPlugin::preOnConnectComponent()
+ void
+ PlannedMotionProviderComponentPlugin::preOnConnectComponent()
{
parent<Component>().getProxyFromProperty(_plannedMotionProvider, PROPERTY_NAME);
}
- void PlannedMotionProviderComponentPlugin::postCreatePropertyDefinitions(armarx::PropertyDefinitionsPtr& properties)
+ void
+ PlannedMotionProviderComponentPlugin::postCreatePropertyDefinitions(
+ armarx::PropertyDefinitionsPtr& properties)
{
if (!properties->hasDefinition(PROPERTY_NAME))
{
properties->defineOptionalProperty<std::string>(
- PROPERTY_NAME,
- "PlannedMotionProvider",
- "Name of the PlannedMotionProvider");
+ PROPERTY_NAME, "PlannedMotionProvider", "Name of the PlannedMotionProvider");
}
}
- PlannedMotionProviderInterfacePrx PlannedMotionProviderComponentPlugin::getPlannedMotionProvider()
+ PlannedMotionProviderInterfacePrx
+ PlannedMotionProviderComponentPlugin::getPlannedMotionProvider()
{
return _plannedMotionProvider;
}
- }
-}
+ } // namespace plugins
+} // namespace armarx
namespace armarx
{
@@ -46,10 +48,11 @@ namespace armarx
addPlugin(plugin);
}
- PlannedMotionProviderInterfacePrx PlannedMotionProviderComponentPluginUser::getPlannedMotionProvider()
+ PlannedMotionProviderInterfacePrx
+ PlannedMotionProviderComponentPluginUser::getPlannedMotionProvider()
{
return plugin->getPlannedMotionProvider();
}
-}
+} // namespace armarx
diff --git a/source/RobotComponents/libraries/RobotComponentsComponentPlugins/PlannedMotionProviderComponentPlugin.h b/source/RobotComponents/libraries/RobotComponentsComponentPlugins/PlannedMotionProviderComponentPlugin.h
index 64d645f31ddcece7442fc9a788da36f8f30d9974..3405704cb6293ab6668894031f9ff43725c93397 100644
--- a/source/RobotComponents/libraries/RobotComponentsComponentPlugins/PlannedMotionProviderComponentPlugin.h
+++ b/source/RobotComponents/libraries/RobotComponentsComponentPlugins/PlannedMotionProviderComponentPlugin.h
@@ -1,8 +1,8 @@
#pragma once
#include <ArmarXCore/core/ComponentPlugin.h>
-#include <RobotComponents/interface/components/PlannedMotionProviderInterface.h>
+#include <RobotComponents/interface/components/PlannedMotionProviderInterface.h>
namespace armarx
{
@@ -27,9 +27,8 @@ namespace armarx
PlannedMotionProviderInterfacePrx _plannedMotionProvider;
};
- }
-}
-
+ } // namespace plugins
+} // namespace armarx
#include <ArmarXCore/core/ManagedIceObject.h>
@@ -48,14 +47,14 @@ namespace armarx
armarx::plugins::PlannedMotionProviderComponentPlugin* plugin = nullptr;
};
-}
-
+} // namespace armarx
namespace armarx
{
namespace plugins
{
// Legacy typedef.
- using PlannedMotionProviderComponentPluginUser = armarx::PlannedMotionProviderComponentPluginUser;
- }
-}
+ using PlannedMotionProviderComponentPluginUser =
+ armarx::PlannedMotionProviderComponentPluginUser;
+ } // namespace plugins
+} // namespace armarx
diff --git a/source/RobotComponents/libraries/RobotComponentsComponentPlugins/RobotPlacementComponentPlugin.cpp b/source/RobotComponents/libraries/RobotComponentsComponentPlugins/RobotPlacementComponentPlugin.cpp
index 416a319319d0c17ea9dd110228bf3ef156edd3e5..a594110ef2e221f00788bcf3e7b96487820c02d3 100644
--- a/source/RobotComponents/libraries/RobotComponentsComponentPlugins/RobotPlacementComponentPlugin.cpp
+++ b/source/RobotComponents/libraries/RobotComponentsComponentPlugins/RobotPlacementComponentPlugin.cpp
@@ -2,41 +2,43 @@
#include <ArmarXCore/core/Component.h>
-
namespace armarx
{
namespace plugins
{
- void RobotPlacementComponentPlugin::preOnInitComponent()
+ void
+ RobotPlacementComponentPlugin::preOnInitComponent()
{
parent<Component>().usingProxyFromProperty(PROPERTY_NAME);
}
- void RobotPlacementComponentPlugin::preOnConnectComponent()
+ void
+ RobotPlacementComponentPlugin::preOnConnectComponent()
{
parent<Component>().getProxyFromProperty(_robotPlacement, PROPERTY_NAME);
}
- void RobotPlacementComponentPlugin::postCreatePropertyDefinitions(armarx::PropertyDefinitionsPtr& properties)
+ void
+ RobotPlacementComponentPlugin::postCreatePropertyDefinitions(
+ armarx::PropertyDefinitionsPtr& properties)
{
if (!properties->hasDefinition(PROPERTY_NAME))
{
properties->defineOptionalProperty<std::string>(
- PROPERTY_NAME,
- "RobotPlacement",
- "Name of the RobotPlacement");
+ PROPERTY_NAME, "RobotPlacement", "Name of the RobotPlacement");
}
}
- RobotPlacementInterfacePrx RobotPlacementComponentPlugin::getRobotPlacement()
+ RobotPlacementInterfacePrx
+ RobotPlacementComponentPlugin::getRobotPlacement()
{
return _robotPlacement;
}
- }
-}
+ } // namespace plugins
+} // namespace armarx
namespace armarx
{
@@ -46,12 +48,11 @@ namespace armarx
addPlugin(plugin);
}
- RobotPlacementInterfacePrx RobotPlacementComponentPluginUser::getRobotPlacement()
+ RobotPlacementInterfacePrx
+ RobotPlacementComponentPluginUser::getRobotPlacement()
{
return plugin->getRobotPlacement();
}
-}
-
-
+} // namespace armarx
diff --git a/source/RobotComponents/libraries/RobotComponentsComponentPlugins/RobotPlacementComponentPlugin.h b/source/RobotComponents/libraries/RobotComponentsComponentPlugins/RobotPlacementComponentPlugin.h
index 66905ccf27352142aafe734f1a9d39f1bb563e86..846d8caa708efd1bf7dceb45c86ae86aa33f1b7b 100644
--- a/source/RobotComponents/libraries/RobotComponentsComponentPlugins/RobotPlacementComponentPlugin.h
+++ b/source/RobotComponents/libraries/RobotComponentsComponentPlugins/RobotPlacementComponentPlugin.h
@@ -1,8 +1,8 @@
#pragma once
#include <ArmarXCore/core/ComponentPlugin.h>
-#include <RobotComponents/interface/components/GraspingManager/RobotPlacementInterface.h>
+#include <RobotComponents/interface/components/GraspingManager/RobotPlacementInterface.h>
namespace armarx
{
@@ -27,9 +27,8 @@ namespace armarx
RobotPlacementInterfacePrx _robotPlacement;
};
- }
-}
-
+ } // namespace plugins
+} // namespace armarx
#include <ArmarXCore/core/ManagedIceObject.h>
@@ -48,8 +47,7 @@ namespace armarx
armarx::plugins::RobotPlacementComponentPlugin* plugin = nullptr;
};
-}
-
+} // namespace armarx
namespace armarx
{
@@ -57,5 +55,5 @@ namespace armarx
{
// Legacy typedef.
using RobotPlacementComponentPluginUser = armarx::RobotPlacementComponentPluginUser;
- }
-}
+ } // namespace plugins
+} // namespace armarx
diff --git a/source/RobotComponents/libraries/SelfLocalization/SelfLocalization.cpp b/source/RobotComponents/libraries/SelfLocalization/SelfLocalization.cpp
index a7e4d388b0207e50fb903c97f323ad8783a1519d..93dd4d94a54e2efea28ff3226b9161b23c2aef71 100644
--- a/source/RobotComponents/libraries/SelfLocalization/SelfLocalization.cpp
+++ b/source/RobotComponents/libraries/SelfLocalization/SelfLocalization.cpp
@@ -26,8 +26,7 @@
namespace armarx
{
- SelfLocalization::SelfLocalization() :
- skipper(0.)
+ SelfLocalization::SelfLocalization() : skipper(0.)
{
addPlugin(transformReaderPlugin);
addPlugin(transformWriterPlugin);
@@ -175,7 +174,8 @@ namespace armarx
try
{
- const auto odomLookupResult = transformReaderPlugin->get().lookupTransform(odomQuery);
+ const auto odomLookupResult =
+ transformReaderPlugin->get().lookupTransform(odomQuery);
if (odomLookupResult.status ==
armem::robot_state::client::localization::TransformResult::Status::Success)
@@ -344,7 +344,7 @@ namespace armarx
}
const Eigen::Isometry3f robot_T_odom = odomLookupResult.transform.transform.inverse();
- const Eigen::Isometry3f map_T_odom = map_T_robot.pose * robot_T_odom;
+ const Eigen::Isometry3f map_T_odom = map_T_robot.pose * robot_T_odom;
const armem::robot_state::localization::Transform mapToOdomTransform{
.header = {.parentFrame = MapFrame,
diff --git a/source/RobotComponents/libraries/SelfLocalization/SelfLocalization.h b/source/RobotComponents/libraries/SelfLocalization/SelfLocalization.h
index a9b27322b57f52de997a903246438a5a1ed87fa3..33b79066b2a734194ad5fe62bf30091e23acede1 100644
--- a/source/RobotComponents/libraries/SelfLocalization/SelfLocalization.h
+++ b/source/RobotComponents/libraries/SelfLocalization/SelfLocalization.h
@@ -33,8 +33,8 @@
#include <ArmarXCore/core/util/IceReportSkipper.h>
#include "RobotAPI/libraries/armem/client/plugins/ReaderWriterPlugin.h"
-#include "RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.h"
#include "RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h"
+#include "RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.h"
#include <RobotAPI/interface/core/RobotLocalization.h>
#include <RobotAPI/interface/units/LocalizationUnitInterface.h>
#include <RobotAPI/interface/units/PlatformUnitInterface.h>
@@ -81,9 +81,9 @@ namespace armarx
* make sure that it also extends the MemoryListenerInterface
* (RobotAPI/interface/armem/client/MemoryListenerInterface.ice).
*/
- class
- [[deprecated("This class was moved to armarx::localization_and_mapping. It is kept for ArmarXSimulation but should not be removed after CMake modernization of ArmarXSimulation.")]]
- SelfLocalization :
+ class [[deprecated("This class was moved to armarx::localization_and_mapping. It is kept for "
+ "ArmarXSimulation but should not be removed after CMake modernization of "
+ "ArmarXSimulation.")]] SelfLocalization :
virtual public armarx::Component,
virtual public armem::ListeningClientPluginUser
{
@@ -193,8 +193,12 @@ namespace armarx
std::shared_ptr<VirtualRobot::Robot> robot;
- armem::client::plugins::ReaderWriterPlugin<armem::robot_state::client::localization::TransformWriter>* transformWriterPlugin = nullptr;
- armem::client::plugins::ReaderWriterPlugin<armem::robot_state::client::localization::TransformReader>* transformReaderPlugin = nullptr;
+ armem::client::plugins::ReaderWriterPlugin<
+ armem::robot_state::client::localization::TransformWriter>* transformWriterPlugin =
+ nullptr;
+ armem::client::plugins::ReaderWriterPlugin<
+ armem::robot_state::client::localization::TransformReader>* transformReaderPlugin =
+ nullptr;
Eigen::Isometry3f global_T_map;
diff --git a/source/RobotComponents/statecharts/GraspingManager/GraspGenerator.cpp b/source/RobotComponents/statecharts/GraspingManager/GraspGenerator.cpp
index 70f18ce7e633004b264d02169b0a77351602eeb8..746460486247ded927742e6f73cb3687825a1be5 100644
--- a/source/RobotComponents/statecharts/GraspingManager/GraspGenerator.cpp
+++ b/source/RobotComponents/statecharts/GraspingManager/GraspGenerator.cpp
@@ -26,11 +26,11 @@ using namespace armarx;
using namespace GraspingManager;
// DO NOT EDIT NEXT LINE
-GraspGenerator::SubClassRegistry GraspGenerator::Registry(GraspGenerator::GetName(), &GraspGenerator::CreateInstance);
+GraspGenerator::SubClassRegistry GraspGenerator::Registry(GraspGenerator::GetName(),
+ &GraspGenerator::CreateInstance);
-
-
-void GraspGenerator::onEnter()
+void
+GraspGenerator::onEnter()
{
// put your user code for the enter-point here
// execution time should be short (<100ms)
@@ -55,16 +55,16 @@ void GraspGenerator::onEnter()
// // execution time should be short (<100ms)
//}
-void GraspGenerator::onExit()
+void
+GraspGenerator::onExit()
{
// put your user code for the exit point here
// execution time should be short (<100ms)
}
-
// DO NOT EDIT NEXT FUNCTION
-XMLStateFactoryBasePtr GraspGenerator::CreateInstance(XMLStateConstructorParams stateData)
+XMLStateFactoryBasePtr
+GraspGenerator::CreateInstance(XMLStateConstructorParams stateData)
{
return XMLStateFactoryBasePtr(new GraspGenerator(stateData));
}
-
diff --git a/source/RobotComponents/statecharts/GraspingManager/GraspGenerator.h b/source/RobotComponents/statecharts/GraspingManager/GraspGenerator.h
index b54bdc6d98d240ddc072e0aa1bb4dcd95d4ebe6a..57fe58baced9f31d07a7df45c454e0d0876c957e 100644
--- a/source/RobotComponents/statecharts/GraspingManager/GraspGenerator.h
+++ b/source/RobotComponents/statecharts/GraspingManager/GraspGenerator.h
@@ -26,12 +26,12 @@
namespace armarx::GraspingManager
{
- class GraspGenerator :
- public GraspGeneratorGeneratedBase < GraspGenerator >
+ class GraspGenerator : public GraspGeneratorGeneratedBase<GraspGenerator>
{
public:
- GraspGenerator(const XMLStateConstructorParams& stateData):
- XMLStateTemplate < GraspGenerator > (stateData), GraspGeneratorGeneratedBase < GraspGenerator > (stateData)
+ GraspGenerator(const XMLStateConstructorParams& stateData) :
+ XMLStateTemplate<GraspGenerator>(stateData),
+ GraspGeneratorGeneratedBase<GraspGenerator>(stateData)
{
}
@@ -49,6 +49,4 @@ namespace armarx::GraspingManager
// use stateparameters instead,
// if classmember are neccessary nonetheless, reset them in onEnter
};
-}
-
-
+} // namespace armarx::GraspingManager
diff --git a/source/RobotComponents/statecharts/GraspingManager/GraspingManagerRemoteStateOfferer.cpp b/source/RobotComponents/statecharts/GraspingManager/GraspingManagerRemoteStateOfferer.cpp
index cb70810ff5c73504dd2567c44d9cc1c5235d7a86..62d23e58f4e20c7bcd16e409ac5878cc39bd785b 100644
--- a/source/RobotComponents/statecharts/GraspingManager/GraspingManagerRemoteStateOfferer.cpp
+++ b/source/RobotComponents/statecharts/GraspingManager/GraspingManagerRemoteStateOfferer.cpp
@@ -26,41 +26,41 @@ using namespace armarx;
using namespace GraspingManager;
// DO NOT EDIT NEXT LINE
-GraspingManagerRemoteStateOfferer::SubClassRegistry GraspingManagerRemoteStateOfferer::Registry(GraspingManagerRemoteStateOfferer::GetName(), &GraspingManagerRemoteStateOfferer::CreateInstance);
+GraspingManagerRemoteStateOfferer::SubClassRegistry
+ GraspingManagerRemoteStateOfferer::Registry(GraspingManagerRemoteStateOfferer::GetName(),
+ &GraspingManagerRemoteStateOfferer::CreateInstance);
-
-
-GraspingManagerRemoteStateOfferer::GraspingManagerRemoteStateOfferer(StatechartGroupXmlReaderPtr reader) :
- XMLRemoteStateOfferer < GraspingManagerStatechartContext > (reader)
+GraspingManagerRemoteStateOfferer::GraspingManagerRemoteStateOfferer(
+ StatechartGroupXmlReaderPtr reader) :
+ XMLRemoteStateOfferer<GraspingManagerStatechartContext>(reader)
{
}
-void GraspingManagerRemoteStateOfferer::onInitXMLRemoteStateOfferer()
+void
+GraspingManagerRemoteStateOfferer::onInitXMLRemoteStateOfferer()
{
-
}
-void GraspingManagerRemoteStateOfferer::onConnectXMLRemoteStateOfferer()
+void
+GraspingManagerRemoteStateOfferer::onConnectXMLRemoteStateOfferer()
{
-
}
-void GraspingManagerRemoteStateOfferer::onExitXMLRemoteStateOfferer()
+void
+GraspingManagerRemoteStateOfferer::onExitXMLRemoteStateOfferer()
{
-
}
// DO NOT EDIT NEXT FUNCTION
-std::string GraspingManagerRemoteStateOfferer::GetName()
+std::string
+GraspingManagerRemoteStateOfferer::GetName()
{
return "GraspingManagerRemoteStateOfferer";
}
// DO NOT EDIT NEXT FUNCTION
-XMLStateOffererFactoryBasePtr GraspingManagerRemoteStateOfferer::CreateInstance(StatechartGroupXmlReaderPtr reader)
+XMLStateOffererFactoryBasePtr
+GraspingManagerRemoteStateOfferer::CreateInstance(StatechartGroupXmlReaderPtr reader)
{
return XMLStateOffererFactoryBasePtr(new GraspingManagerRemoteStateOfferer(reader));
}
-
-
-
diff --git a/source/RobotComponents/statecharts/GraspingManager/GraspingManagerRemoteStateOfferer.h b/source/RobotComponents/statecharts/GraspingManager/GraspingManagerRemoteStateOfferer.h
index ac4bd918ac0591ec4952cecf9f924e15d0c26d99..c58c62955ad3e3acf9ce437bb07c1a91fa905d69 100644
--- a/source/RobotComponents/statecharts/GraspingManager/GraspingManagerRemoteStateOfferer.h
+++ b/source/RobotComponents/statecharts/GraspingManager/GraspingManagerRemoteStateOfferer.h
@@ -23,12 +23,14 @@
#pragma once
#include <ArmarXCore/statechart/xmlstates/XMLRemoteStateOfferer.h>
+
#include "GraspingManagerStatechartContext.generated.h"
namespace armarx::GraspingManager
{
class GraspingManagerRemoteStateOfferer :
- virtual public XMLRemoteStateOfferer < GraspingManagerStatechartContext > // Change this statechart context if you need another context (dont forget to change in the constructor as well)
+ virtual public XMLRemoteStateOfferer<
+ GraspingManagerStatechartContext> // Change this statechart context if you need another context (dont forget to change in the constructor as well)
{
public:
GraspingManagerRemoteStateOfferer(StatechartGroupXmlReaderPtr reader);
@@ -42,8 +44,5 @@ namespace armarx::GraspingManager
static std::string GetName();
static XMLStateOffererFactoryBasePtr CreateInstance(StatechartGroupXmlReaderPtr reader);
static SubClassRegistry Registry;
-
-
};
-}
-
+} // namespace armarx::GraspingManager
diff --git a/source/RobotComponents/statecharts/GraspingManager/GraspingPipeline.cpp b/source/RobotComponents/statecharts/GraspingManager/GraspingPipeline.cpp
index 27e65b0eab080c2495a93f8eefa4f8de64011172..72fcddb8a39196269081221ab5d7f3082567deb8 100644
--- a/source/RobotComponents/statecharts/GraspingManager/GraspingPipeline.cpp
+++ b/source/RobotComponents/statecharts/GraspingManager/GraspingPipeline.cpp
@@ -26,11 +26,11 @@ using namespace armarx;
using namespace GraspingManager;
// DO NOT EDIT NEXT LINE
-GraspingPipeline::SubClassRegistry GraspingPipeline::Registry(GraspingPipeline::GetName(), &GraspingPipeline::CreateInstance);
+GraspingPipeline::SubClassRegistry GraspingPipeline::Registry(GraspingPipeline::GetName(),
+ &GraspingPipeline::CreateInstance);
-
-
-void GraspingPipeline::onEnter()
+void
+GraspingPipeline::onEnter()
{
// put your user code for the enter-point here
// execution time should be short (<100ms)
@@ -55,16 +55,16 @@ void GraspingPipeline::onEnter()
// // execution time should be short (<100ms)
//}
-void GraspingPipeline::onExit()
+void
+GraspingPipeline::onExit()
{
// put your user code for the exit point here
// execution time should be short (<100ms)
}
-
// DO NOT EDIT NEXT FUNCTION
-XMLStateFactoryBasePtr GraspingPipeline::CreateInstance(XMLStateConstructorParams stateData)
+XMLStateFactoryBasePtr
+GraspingPipeline::CreateInstance(XMLStateConstructorParams stateData)
{
return XMLStateFactoryBasePtr(new GraspingPipeline(stateData));
}
-
diff --git a/source/RobotComponents/statecharts/GraspingManager/GraspingPipeline.h b/source/RobotComponents/statecharts/GraspingManager/GraspingPipeline.h
index 4b0e21d05d961ff5c5435e0296f823ea1451dde6..bbbca75c85fee8218d33dfefebebf5a4557719e2 100644
--- a/source/RobotComponents/statecharts/GraspingManager/GraspingPipeline.h
+++ b/source/RobotComponents/statecharts/GraspingManager/GraspingPipeline.h
@@ -26,12 +26,12 @@
namespace armarx::GraspingManager
{
- class GraspingPipeline :
- public GraspingPipelineGeneratedBase < GraspingPipeline >
+ class GraspingPipeline : public GraspingPipelineGeneratedBase<GraspingPipeline>
{
public:
- GraspingPipeline(const XMLStateConstructorParams& stateData):
- XMLStateTemplate < GraspingPipeline > (stateData), GraspingPipelineGeneratedBase < GraspingPipeline > (stateData)
+ GraspingPipeline(const XMLStateConstructorParams& stateData) :
+ XMLStateTemplate<GraspingPipeline>(stateData),
+ GraspingPipelineGeneratedBase<GraspingPipeline>(stateData)
{
}
@@ -49,6 +49,4 @@ namespace armarx::GraspingManager
// use stateparameters instead,
// if classmember are neccessary nonetheless, reset them in onEnter
};
-}
-
-
+} // namespace armarx::GraspingManager