Workspace fix

VirtualRobot/Workspace/Manipulability.cpp

@@ -448,7 +448,8 @@ namespace VirtualRobot
             p(2) = 1.0f;
             float q = measure->getPoseQuality(p);
 #else
-            float q = measure->getPoseQuality();
+//            float q = measure->getPoseQuality();
+            float q = getCurrentManipulability();
 #endif
 
             if (q > storeMaxManipulability)

VirtualRobot/Workspace/WorkspaceRepresentation.cpp

@@ -2135,6 +2135,8 @@ namespace VirtualRobot
 
     void WorkspaceRepresentation::addRandomTCPPoses(unsigned int loops, unsigned int numThreads, bool checkForSelfCollisions)
     {
+        THROW_VR_EXCEPTION_IF(!data || !nodeSet || !tcpNode, "Workspace data not initialized");
+
         if (numThreads > loops)
         {
             VR_ERROR << "Number of threads can not be bigger then number of tcp poses to add.";
@@ -2162,7 +2164,7 @@ namespace VirtualRobot
                     staticCollisionModel = clonedRobot->getRobotNodeSet(staticCollisionModel->getName());
                 }
 
-                SceneObjectSetPtr dynamicCollisionModel = this->staticCollisionModel;
+                SceneObjectSetPtr dynamicCollisionModel = this->dynamicCollisionModel;
                 if (dynamicCollisionModel && clonedRobot->hasRobotNodeSet(dynamicCollisionModel->getName()))
                 {
                     dynamicCollisionModel = clonedRobot->getRobotNodeSet(dynamicCollisionModel->getName());

VirtualRobot/Workspace/WorkspaceRepresentation.h

@@ -374,8 +374,8 @@ namespace VirtualRobot
         */
         void addRandomTCPPoses(unsigned int loops, bool checkForSelfCollisions = true);
         /*!
-            Appends a number of random TCP poses to workspace Data. This method uses several threads behind the scenes to
-            speed up the process.
+            Appends a number of random TCP poses to workspace Data (multithreaded).
+            This method is blocking, i.e. it returns as soon as all threads are done.
             \param loops Number of poses that should be appended
             \param numThreads number of worker threads used behind the scenes to append random TCP poses to workspace data.
             \param checkForSelfCollisions Build a collision-free configuration. If true, random configs are generated until one is collision-free.