diff --git a/VirtualRobot/Workspace/WorkspaceRepresentation.cpp b/VirtualRobot/Workspace/WorkspaceRepresentation.cpp index e7ba684037a4b2d36894042939b7e3264384d04e..7cd0a5f560bee0074f5e28dd6eb6c692c5cf7c23 100644 --- a/VirtualRobot/Workspace/WorkspaceRepresentation.cpp +++ b/VirtualRobot/Workspace/WorkspaceRepresentation.cpp @@ -17,6 +17,7 @@ #include <cmath> #include <float.h> #include <limits.h> +#include <thread> namespace VirtualRobot { @@ -2131,5 +2132,140 @@ namespace VirtualRobot robot->setUpdateVisualization(visuSate); nodeSet->setJointValues(c); } + + void WorkspaceRepresentation::addRandomTCPPoses(unsigned int loops, unsigned int numThreads, bool checkForSelfCollisions) + { + if (numThreads > loops) + { + VR_ERROR << "Number of threads can not be bigger then number of tcp poses to add."; + return; + } + + std::thread threads[numThreads]; + unsigned int numPosesPerThread = loops / numThreads; // todo + static const float randMult = (float)(1.0 / (double)(RAND_MAX)); + + for (int i = 0; i < numThreads; i++) + { + threads[i] = std::thread([=] () + { + // each thread gets a cloned robot + CollisionCheckerPtr cc(new CollisionChecker()); + RobotPtr clonedRobot = this->robot->clone("clonedRobot_" + std::to_string(i), cc); + clonedRobot->setUpdateVisualization(false); + RobotNodeSetPtr clonedNodeSet = clonedRobot->getRobotNodeSet(this->nodeSet->getName()); + RobotNodePtr clonedTcpNode = clonedRobot->getRobotNode(this->tcpNode->getName()); + + SceneObjectSetPtr staticCollisionModel = this->staticCollisionModel; + if (staticCollisionModel && clonedRobot->hasRobotNodeSet(staticCollisionModel->getName())) + { + staticCollisionModel = clonedRobot->getRobotNodeSet(staticCollisionModel->getName()); + } + + SceneObjectSetPtr dynamicCollisionModel = this->staticCollisionModel; + if (dynamicCollisionModel && clonedRobot->hasRobotNodeSet(dynamicCollisionModel->getName())) + { + dynamicCollisionModel = clonedRobot->getRobotNodeSet(dynamicCollisionModel->getName()); + } + + // now sample some configs and add them to the workspace data + for (int j = 0; j < numPosesPerThread; j++) + { + float rndValue; + float minJ, maxJ; + Eigen::VectorXf v(clonedNodeSet->getSize()); + float maxLoops = 1000; + + bool successfullyRandomized = false; + + for (int k = 0; k < maxLoops; k++) + { + for (int l = 0; l < clonedNodeSet->getSize(); l++) + { + rndValue = (float) std::rand() * randMult; // value from 0 to 1 + minJ = (*nodeSet)[l]->getJointLimitLo(); + maxJ = (*nodeSet)[l]->getJointLimitHi(); + v[l] = minJ + ((maxJ - minJ) * rndValue); + } + + clonedRobot->setJointValues(clonedNodeSet, v); + + // check for collisions + if (!checkForSelfCollisions || !staticCollisionModel || !dynamicCollisionModel) + { + successfullyRandomized = true; + break; + } + + if (!clonedRobot->getCollisionChecker()->checkCollision(staticCollisionModel, dynamicCollisionModel)) + { + successfullyRandomized = true; + break; + } + + this->collisionConfigs++; + } + + if (successfullyRandomized) + { + Eigen::Matrix4f p = clonedTcpNode->getGlobalPose(); + addPose(p); + } + else + { + VR_WARNING << "Could not find collision-free configuration..."; + } + } + }); + } + + // wait for all threads to finish + for (int i = 0; i < numThreads; i++) + { + threads[i].join(); + } + } + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + } // namespace VirtualRobot diff --git a/VirtualRobot/Workspace/WorkspaceRepresentation.h b/VirtualRobot/Workspace/WorkspaceRepresentation.h index c4c0c4d9ed5aa49fe449080d83eade119f0f06d7..ee412c5f602411c9538292a92e18f24549e4d57d 100644 --- a/VirtualRobot/Workspace/WorkspaceRepresentation.h +++ b/VirtualRobot/Workspace/WorkspaceRepresentation.h @@ -367,11 +367,20 @@ namespace VirtualRobot /*! - Append a number of random TCP poses to workspace Data + Append a number of random TCP poses to workspace Data. + For bigger sampling rates (loops > 50.000) you should consider this method's multithreaded pendant. \param loops Number of poses that should be appended \param checkForSelfCollisions Build a collision-free configuration. If true, random configs are generated until one is collision-free. */ 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. + \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. + */ + void addRandomTCPPoses(unsigned int loops, unsigned int numThreads, bool checkForSelfCollisions = true); void setVoxelEntry(unsigned int v[6], unsigned char e); void setEntry(const Eigen::Matrix4f& poseGlobal, unsigned char e); diff --git a/VirtualRobot/examples/reachability/reachabilityWindow.cpp b/VirtualRobot/examples/reachability/reachabilityWindow.cpp index b0600ebe19876b1d53bb1c8362f178a7f6470366..85ec3a582a9580f50af738b02e1cfd488faf9253 100644 --- a/VirtualRobot/examples/reachability/reachabilityWindow.cpp +++ b/VirtualRobot/examples/reachability/reachabilityWindow.cpp @@ -456,7 +456,8 @@ void reachabilityWindow::extendReach() } int steps = UI.spinBoxExtend->value(); - reachSpace->addRandomTCPPoses(steps); + reachSpace->addRandomTCPPoses(steps, QThread::idealThreadCount() < 1 ? 1 : QThread::idealThreadCount(), true); + reachSpace->print(); UI.checkBoxReachabilityVisu->setChecked(false); UI.sliderCutReach->setEnabled(false);