From 8af8c078cff3b2ee9bd758639e7ddd9d18531116 Mon Sep 17 00:00:00 2001 From: memnone Date: Mon, 27 Jun 2016 15:37:50 +0100 Subject: [PATCH] Collision bounds for the hypotheses --- include/Spam/App/PosePlanner/PosePlanner.h | 5 +++ .../Spam/Data/R2GTrajectory/R2GTrajectory.h | 7 ++-- resources/Spam/Data/SpamDataR2GTrajectory.xml | 2 +- .../Spam/Demo/FT/SpamDemoFT_RobotBorisSim.xml | 2 +- .../Demo/R2G/GraspDataContactModelDemoR2G.xml | 8 +++-- .../Demo/R2G/GraspDataContactQueryDemoR2G.xml | 12 +++++-- .../Demo/R2G/GraspDataPointsCurvDemoR2G.xml | 1 + .../Demo/R2G/SpamDataR2GTrajectoryDemoR2G.xml | 2 +- src/Spam/App/PosePlanner/PosePlanner.cpp | 13 ++++++-- src/Spam/App/R2GPlanner/R2GPlanner.cpp | 32 ++++++++++++------- src/Spam/Data/R2GTrajectory/R2GTrajectory.cpp | 14 ++++---- src/Spam/Demo/FT/FTDemo.cpp | 12 +++---- src/Spam/HBPlan/GraphPlanner.cpp | 4 +-- 13 files changed, 74 insertions(+), 40 deletions(-) diff --git a/include/Spam/App/PosePlanner/PosePlanner.h b/include/Spam/App/PosePlanner/PosePlanner.h index 2f74145..119523a 100644 --- a/include/Spam/App/PosePlanner/PosePlanner.h +++ b/include/Spam/App/PosePlanner/PosePlanner.h @@ -61,6 +61,9 @@ namespace spam { /** SPAM Pose planner. */ class PosePlanner : public grasp::Player { public: + /** Callback to handle a ground truth pose of the object */ + typedef std::function SimulateHandler; + /** Action types */ enum action { NONE_ACTION = 0, @@ -378,6 +381,8 @@ class PosePlanner : public grasp::Player { /** Appereance for point clouds: debug point clouds */ grasp::Cloud::Appearance hypothesisAppearance, meanposeAppeareance, groundtruthAppearance; bool showMeanPoseOnly, showSimulate; + /** Callback to handle simulate ground truth */ + SimulateHandler simulateHandlerCallback; /** Force to draw the belief state */ bool drawBeliefState; diff --git a/include/Spam/Data/R2GTrajectory/R2GTrajectory.h b/include/Spam/Data/R2GTrajectory/R2GTrajectory.h index b51741d..a64202f 100644 --- a/include/Spam/Data/R2GTrajectory/R2GTrajectory.h +++ b/include/Spam/Data/R2GTrajectory/R2GTrajectory.h @@ -382,7 +382,7 @@ class GOLEM_LIBRARY_DECLDIR HandlerR2GTrajectory : public grasp::data::Handler, /** Trajectory duration */ golem::Real trjDuration; /** IG Trajectory duration */ - golem::Real trjIGDuration; + golem::Real trjR2GDuration; /** Trajectory idle */ golem::Real trjIdle; @@ -439,7 +439,7 @@ class GOLEM_LIBRARY_DECLDIR HandlerR2GTrajectory : public grasp::data::Handler, trjExtrapolation = golem::Real(0.0); trjDuration = golem::Real(5.0); - trjIGDuration = golem::Real(20.0); + trjR2GDuration = golem::Real(20.0); trjIdle = golem::Real(1.0); action.clear(); @@ -486,6 +486,7 @@ class GOLEM_LIBRARY_DECLDIR HandlerR2GTrajectory : public grasp::data::Handler, grasp::Assert::valid(trjExtrapolation >= golem::REAL_ZERO, ac, "trjExtrapolation < 0"); grasp::Assert::valid(trjDuration > golem::REAL_EPS, ac, "trjDuration < eps"); + grasp::Assert::valid(trjR2GDuration > golem::REAL_EPS, ac, "trjDuration < eps"); grasp::Assert::valid(trjIdle >= golem::REAL_ZERO, ac, "trjIdle < 0"); for (grasp::Mat34Seq::const_iterator i = action.begin(); i != action.end(); ++i) @@ -594,7 +595,7 @@ class GOLEM_LIBRARY_DECLDIR HandlerR2GTrajectory : public grasp::data::Handler, /** Trajectory duration */ golem::Real trjDuration; /** IG Trajectory duration */ - golem::Real trjIGDuration; + golem::Real trjR2GDuration; /** Trajectory idle */ golem::Real trjIdle; diff --git a/resources/Spam/Data/SpamDataR2GTrajectory.xml b/resources/Spam/Data/SpamDataR2GTrajectory.xml index 1689ecd..8d13dad 100644 --- a/resources/Spam/Data/SpamDataR2GTrajectory.xml +++ b/resources/Spam/Data/SpamDataR2GTrajectory.xml @@ -3,7 +3,7 @@ - + diff --git a/resources/Spam/Demo/FT/SpamDemoFT_RobotBorisSim.xml b/resources/Spam/Demo/FT/SpamDemoFT_RobotBorisSim.xml index 38ed3de..e28b638 100644 --- a/resources/Spam/Demo/FT/SpamDemoFT_RobotBorisSim.xml +++ b/resources/Spam/Demo/FT/SpamDemoFT_RobotBorisSim.xml @@ -3,7 +3,7 @@ - + diff --git a/resources/Spam/Demo/R2G/GraspDataContactModelDemoR2G.xml b/resources/Spam/Demo/R2G/GraspDataContactModelDemoR2G.xml index f9b9c69..d223954 100644 --- a/resources/Spam/Demo/R2G/GraspDataContactModelDemoR2G.xml +++ b/resources/Spam/Demo/R2G/GraspDataContactModelDemoR2G.xml @@ -3,7 +3,7 @@ - + @@ -13,10 +13,12 @@ - + + - + + diff --git a/resources/Spam/Demo/R2G/GraspDataContactQueryDemoR2G.xml b/resources/Spam/Demo/R2G/GraspDataContactQueryDemoR2G.xml index a681510..2b52b8d 100644 --- a/resources/Spam/Demo/R2G/GraspDataContactQueryDemoR2G.xml +++ b/resources/Spam/Demo/R2G/GraspDataContactQueryDemoR2G.xml @@ -1,9 +1,9 @@ - + - + @@ -17,11 +17,17 @@ + + + - + + + + diff --git a/resources/Spam/Demo/R2G/GraspDataPointsCurvDemoR2G.xml b/resources/Spam/Demo/R2G/GraspDataPointsCurvDemoR2G.xml index 05b8303..bba0033 100644 --- a/resources/Spam/Demo/R2G/GraspDataPointsCurvDemoR2G.xml +++ b/resources/Spam/Demo/R2G/GraspDataPointsCurvDemoR2G.xml @@ -20,6 +20,7 @@ + diff --git a/resources/Spam/Demo/R2G/SpamDataR2GTrajectoryDemoR2G.xml b/resources/Spam/Demo/R2G/SpamDataR2GTrajectoryDemoR2G.xml index e170a89..7e20afa 100644 --- a/resources/Spam/Demo/R2G/SpamDataR2GTrajectoryDemoR2G.xml +++ b/resources/Spam/Demo/R2G/SpamDataR2GTrajectoryDemoR2G.xml @@ -3,7 +3,7 @@ - + diff --git a/src/Spam/App/PosePlanner/PosePlanner.cpp b/src/Spam/App/PosePlanner/PosePlanner.cpp index 73273d4..8a0eebd 100644 --- a/src/Spam/App/PosePlanner/PosePlanner.cpp +++ b/src/Spam/App/PosePlanner/PosePlanner.cpp @@ -196,6 +196,7 @@ void spam::PosePlanner::Data::createRender() { if (owner->showMeanPoseOnly) // this parameter can be control by outside break; } + owner->pHeuristic->renderHypothesisCollisionBounds(owner->debugRenderer); } // show constantly the belief state, if needed @@ -403,7 +404,7 @@ bool spam::PosePlanner::create(const Desc& desc) { if (!simulateHandler) throw Message(Message::LEVEL_CRIT, "spam::PosePlanner::create(): unknown simulate data handler: %s", desc.simulateHandler.c_str()); simulateItem = desc.simulateItem; - + simulateHandlerCallback = nullptr; // models modelMap.clear(); @@ -486,8 +487,12 @@ bool spam::PosePlanner::create(const Desc& desc) { } catch (const Message& msg) { context.write("%s", msg.what()); } to(dataCurrentPtr)->simulateObjectPose = simulatedPoints; + if (simulateHandlerCallback) + simulateHandlerCallback(simulatedPoints); beliefStatePtr->second->set(pBelief.get()); + // needed to determine when to expect collisions + pHeuristic->setHypothesisBounds(); RenderBlock renderBlock(*this); { Data::View::setItem(to(dataCurrentPtr)->itemMap, beliefStatePtr->first, to(dataCurrentPtr)->getView()); @@ -632,7 +637,11 @@ grasp::data::Item::Map::iterator spam::PosePlanner::estimatePose(Data::Mode mode } else grasp::Cloud::transform(grasp::to(dataCurrentPtr)->queryTransform, grasp::to(dataCurrentPtr)->simulateObjectPose, grasp::to(dataCurrentPtr)->simulateObjectPose); - + + // callback to handle groundtruth, if any + if (simulateHandlerCallback) + simulateHandlerCallback(grasp::to(dataCurrentPtr)->simulateObjectPose); + grasp::data::Item::Ptr simPtr = ptr->second->clone(); // save simulated point cloud RenderBlock renderBlock(*this); diff --git a/src/Spam/App/R2GPlanner/R2GPlanner.cpp b/src/Spam/App/R2GPlanner/R2GPlanner.cpp index 6d28fc6..65f8f1f 100644 --- a/src/Spam/App/R2GPlanner/R2GPlanner.cpp +++ b/src/Spam/App/R2GPlanner/R2GPlanner.cpp @@ -183,25 +183,27 @@ bool R2GPlanner::create(const Desc& desc) { middleFTDesc.setLimits(&fLimit[12]); ftGuards.push_back(middleFTDesc.create()); + // set the call back to ahndle ground truth + simulateHandlerCallback = [&](const grasp::Cloud::PointSeq& points) { + context.write("Simulated callback(): points size %d\n", points.size()); + collisionPtr->create(rand, points); + objectPointCloudPtr.reset(new grasp::Cloud::PointSeq(points)); + }; + bool ftsensors = !ftSensorSeq.empty(); const grasp::Sensor::Map::const_iterator openNIPtr = sensorMap.find("OpenNI+OpenNI"); - bool simulatedForces = !ftsensors && (openNIPtr == sensorMap.end()); + const bool simulatedForces = true; // !ftsensors && (openNIPtr == sensorMap.end()); armHandForce->setSensorForceReader([&](const golem::Controller::State& state, grasp::RealSeq& force) { // throws if (!enableForceReading) return; + context.write("simulatedForces [%s] && !collisionPtr->getPoints().empty() [%s]\n", simulatedForces ? "TRUE" : "FALSE", !collisionPtr->getPoints().empty() ? "TRUE" : "FALSE"); if (simulatedForces && !collisionPtr->getPoints().empty()) { for (auto i = 0; i < force.size(); ++i) force[i] = collisionPtr->getFTBaseSensor().ftMedian[i] + (2 * rand.nextUniform()*collisionPtr->getFTBaseSensor().ftStd[i] - collisionPtr->getFTBaseSensor().ftStd[i]); - golem::Controller::State dflt = manipulator->getController().createState(); - manipulator->getController().setToDefault(dflt); - - manipulator->getController().lookupState(golem::SEC_TM_REAL_MAX, dflt); - dflt.cvel.setToDefault(manipulator->getController().getStateInfo().getJoints()); - dflt.cacc.setToDefault(manipulator->getController().getStateInfo().getJoints()); - - (void)collisionPtr->simulateFT(debugRenderer, desc.objCollisionDescPtr->flannDesc, rand, manipulator->getConfig(dflt), force, false); + golem::Controller::State dflt = grasp::Waypoint::lookup(*controller).state; + (void)collisionPtr->simulateFT(debugRenderer, desc.objCollisionDescPtr->flannDesc, rand, manipulator->getConfig(dflt), force, true); } // read from the state variable (if supported) else { @@ -253,8 +255,8 @@ bool R2GPlanner::create(const Desc& desc) { armHandForce->setEmergencyModeHandler([=]() { enableForceReading = false; contactOccured = true; - armHandForce->getArmCtrl()->setMode(armMode); - armHandForce->getHandCtrl()->setMode(handMode, I32(-1)); + //armHandForce->getArmCtrl()->setMode(armMode); + //armHandForce->getHandCtrl()->setMode(handMode); }); // end robot->setEmergencyModeHandler uncEnable = desc.uncEnable; @@ -806,6 +808,8 @@ void R2GPlanner::perform(const std::string& data, const std::string& item, const option("\x0D", "Press to accept trajectory..."); } + bool emergencyMode = armHandForce->getHandCtrl()->getMode(); + // go to initial state sendTrajectory(initTrajectory); // wait until the last trajectory segment is sent @@ -837,6 +841,12 @@ void R2GPlanner::perform(const std::string& data, const std::string& item, const if (i % 10 == 0) { context.write("State #%d (%s)\r", i, enableForceReading ? "Y" : "N"); + if (emergencyMode && i > 50) { + armHandForce->getArmCtrl()->setMode(armMode); + armHandForce->getHandCtrl()->setMode(handMode); + emergencyMode = false; + } + if (grasp::to(dataCurrentPtr)->actionType != action::GRASP && i > 500) { enableForceReading = expectedCollisions(state); } diff --git a/src/Spam/Data/R2GTrajectory/R2GTrajectory.cpp b/src/Spam/Data/R2GTrajectory/R2GTrajectory.cpp index e23a894..9299aeb 100644 --- a/src/Spam/Data/R2GTrajectory/R2GTrajectory.cpp +++ b/src/Spam/Data/R2GTrajectory/R2GTrajectory.cpp @@ -335,7 +335,7 @@ void spam::data::HandlerR2GTrajectory::Desc::load(golem::Context& context, const golem::XMLData("extrapolation", trjExtrapolation, pxmlcontext->getContextFirst("profile"), false); golem::XMLData("duration", trjDuration, pxmlcontext->getContextFirst("profile"), false); try { - golem::XMLData("ig_duration", trjIGDuration, pxmlcontext->getContextFirst("profile"), false); + golem::XMLData("r2g_duration", trjR2GDuration, pxmlcontext->getContextFirst("profile"), false); } catch (const golem::MsgXMLParser&) {} golem::XMLData("idle", trjIdle, pxmlcontext->getContextFirst("profile"), false); @@ -407,7 +407,7 @@ void spam::data::HandlerR2GTrajectory::create(const Desc& desc) { trjExtrapolation = desc.trjExtrapolation; trjDuration = desc.trjDuration; - trjIGDuration = desc.trjIGDuration; + trjR2GDuration = desc.trjR2GDuration; trjIdle = desc.trjIdle; @@ -737,9 +737,9 @@ void spam::data::HandlerR2GTrajectory::createTrajectory(const ItemR2GTrajectory& // Enable IG for reach-to-grasp trajectories and collisions with target object this->pHeuristic->enableUnc = false; - this->pHeuristic->setPointCloudCollision(true); + this->pHeuristic->setPointCloudCollision(false); - Real trjDuration = this->trjDuration; + Real trjDuration = this->trjR2GDuration; // make trajectory golem::Controller::State::Seq states = grasp::Waypoint::make(item.getWaypoints(), false), commands = grasp::Waypoint::make(item.getWaypoints(), true); @@ -747,7 +747,7 @@ void spam::data::HandlerR2GTrajectory::createTrajectory(const ItemR2GTrajectory& if (getUICallback() && getUICallback()->hasInputEnabled()) { Menu menu(context, *getUICallback()); //inverse = menu.option("FI", "Use (F)orward/(I)nverse trajectory... ") == 'I'; - menu.readNumber("Trajectory duration: ", trjIGDuration); + menu.readNumber("Trajectory duration: ", trjDuration); } grasp::RBDist err = planner->findGlobalTrajectory(begin, commands.front(), trajectory, trajectory.begin()); @@ -778,7 +778,7 @@ void spam::data::HandlerR2GTrajectory::createIGTrajectory(const ItemR2GTrajector this->pHeuristic->enableUnc = true; this->pHeuristic->setPointCloudCollision(true); - Real trjDuration = this->trjDuration; + Real trjDuration = this->trjR2GDuration; // make trajectory golem::Controller::State::Seq states = grasp::Waypoint::make(item.getWaypoints(), false), commands = grasp::Waypoint::make(item.getWaypoints(), true); @@ -786,7 +786,7 @@ void spam::data::HandlerR2GTrajectory::createIGTrajectory(const ItemR2GTrajector if (getUICallback() && getUICallback()->hasInputEnabled()) { Menu menu(context, *getUICallback()); //inverse = menu.option("FI", "Use (F)orward/(I)nverse trajectory... ") == 'I'; - menu.readNumber("Trajectory duration: ", trjIGDuration); + menu.readNumber("Trajectory duration: ", trjDuration); } grasp::RBDist err = planner->findGlobalTrajectory(begin, commands.front(), trajectory, trajectory.begin()); diff --git a/src/Spam/Demo/FT/FTDemo.cpp b/src/Spam/Demo/FT/FTDemo.cpp index 5342b75..eeb46c1 100644 --- a/src/Spam/Demo/FT/FTDemo.cpp +++ b/src/Spam/Demo/FT/FTDemo.cpp @@ -257,7 +257,7 @@ void FTDemo::create(const Desc& desc) { if (currentBeliefPtr != to(dataCurrentPtr)->itemMap.end()) Data::View::setItem(to(dataCurrentPtr)->itemMap, currentBeliefPtr, to(dataCurrentPtr)->getView()); else - context.write("%s is not available\n", currentBeliefItem.c_str()); + context.write("Belief Item is not available\n"); } // force to draw the belief state drawBeliefState = true; @@ -273,11 +273,11 @@ void FTDemo::create(const Desc& desc) { for (; to(dataCurrentPtr)->queryPoints.empty();) executeCmd(createQueryCmd); // set the simulated object - if (!to(dataCurrentPtr)->simulateObjectPose.empty()) { - //sensorBundlePtr->getCollisionPtr()->create(rand, grasp::to(dataCurrentPtr)->simulateObjectPose); - collisionPtr->create(rand, grasp::to(dataCurrentPtr)->simulateObjectPose); - objectPointCloudPtr.reset(new grasp::Cloud::PointSeq(grasp::to(dataCurrentPtr)->simulateObjectPose)); - } + //if (!to(dataCurrentPtr)->simulateObjectPose.empty()) { + // //sensorBundlePtr->getCollisionPtr()->create(rand, grasp::to(dataCurrentPtr)->simulateObjectPose); + // collisionPtr->create(rand, grasp::to(dataCurrentPtr)->simulateObjectPose); + // objectPointCloudPtr.reset(new grasp::Cloud::PointSeq(grasp::to(dataCurrentPtr)->simulateObjectPose)); + //} reset(); // move the robot to the home pose after scanning // write results diff --git a/src/Spam/HBPlan/GraphPlanner.cpp b/src/Spam/HBPlan/GraphPlanner.cpp index 8e399a6..498af8d 100644 --- a/src/Spam/HBPlan/GraphPlanner.cpp +++ b/src/Spam/HBPlan/GraphPlanner.cpp @@ -550,7 +550,7 @@ bool RagGraphPlanner::findGlobalTrajectory(const golem::Controller::State &begin #ifdef _GRAPHPLANNER_PERFMON #ifdef _HEURISTIC_PERFMON // context.debug("Enabled Uncertainty %s\n", getFTDrivenHeuristic()->enableUnc ? "ON" : "OFF"); - FTDrivenHeuristic::writeLog(context, "PathFinder::find()"); + //FTDrivenHeuristic::writeLog(context, "PathFinder::find()"); Collision::writeLog(context, "PathFinder::find()"); #endif #ifdef _BOUNDS_PERFMON @@ -678,7 +678,7 @@ bool RagGraphPlanner::findLocalTrajectory(const Controller::State &cbegin, GenWo #ifdef _HEURISTIC_PERFMON if (heuristic) { context.write("Enabled Uncertainty %s\n", heuristic->enableUnc ? "ON" : "OFF"); - heuristic->writeLog(context, "GraphPlanner::findTarget()"); + //heuristic->writeLog(context, "GraphPlanner::findTarget()"); heuristic->getCollision()->writeLog(context, "GraphPlanner::findTarget()");; } #endif