From 5d8b8dac21f0893d6fb97171aa207eb5f309daf9 Mon Sep 17 00:00:00 2001 From: memnone Date: Mon, 4 Jul 2016 16:47:23 +0100 Subject: [PATCH] Retrieve state of the robot instead of command when planning a R2G trajectory --- include/Spam/App/PosePlanner/PosePlanner.h | 1 - src/Spam/App/R2GPlanner/R2GPlanner.cpp | 2 +- src/Spam/Demo/FT/FTDemo.cpp | 7 +++---- 3 files changed, 4 insertions(+), 6 deletions(-) diff --git a/include/Spam/App/PosePlanner/PosePlanner.h b/include/Spam/App/PosePlanner/PosePlanner.h index 2c983d0..7345e41 100644 --- a/include/Spam/App/PosePlanner/PosePlanner.h +++ b/include/Spam/App/PosePlanner/PosePlanner.h @@ -511,7 +511,6 @@ class PosePlanner : public grasp::Player { golem::Controller::State dflt = controller->createState(); controller->setToDefault(dflt); - //controller->lookupCommand(time, dflt); controller->lookupState(time, dflt); dflt.cvel.setToDefault(info.getJoints()); dflt.cacc.setToDefault(info.getJoints()); diff --git a/src/Spam/App/R2GPlanner/R2GPlanner.cpp b/src/Spam/App/R2GPlanner/R2GPlanner.cpp index 6b9c22b..37091ba 100644 --- a/src/Spam/App/R2GPlanner/R2GPlanner.cpp +++ b/src/Spam/App/R2GPlanner/R2GPlanner.cpp @@ -758,7 +758,7 @@ void R2GPlanner::perform(const std::string& data, const std::string& item, const throw Message(Message::LEVEL_ERROR, "Player::perform(): At least two waypoints required"); golem::Controller::State::Seq initTrajectory; - findTrajectory(grasp::Waypoint::lookup(*controller).command, &trajectory.front(), nullptr, SEC_TM_REAL_ZERO, initTrajectory); + findTrajectory(grasp::Waypoint::lookup(*controller).state, &trajectory.front(), nullptr, SEC_TM_REAL_ZERO, initTrajectory); golem::Controller::State::Seq completeTrajectory = initTrajectory; completeTrajectory.insert(completeTrajectory.end(), trajectory.begin(), trajectory.end()); diff --git a/src/Spam/Demo/FT/FTDemo.cpp b/src/Spam/Demo/FT/FTDemo.cpp index 78c9090..56fd42e 100644 --- a/src/Spam/Demo/FT/FTDemo.cpp +++ b/src/Spam/Demo/FT/FTDemo.cpp @@ -85,7 +85,7 @@ void FTDemo::create(const Desc& desc) { menuCmdMap.insert(std::make_pair("ZX", [=]() { // setup initial variables - const Controller::State home = grasp::Waypoint::lookup(*controller).command; + const Controller::State home = lookupState(); // grasp::Waypoint::lookup(*controller).command; std::string r2gHandlerItemName = "SpamDataR2GTrajectory+SpamDataR2GTrajectoryDemoR2G"; grasp::data::Handler::Map::const_iterator r2gTrjHandlerPtr = handlerMap.find(r2gHandlerItemName); @@ -385,9 +385,9 @@ void FTDemo::create(const Desc& desc) { Controller::State::Seq seq; if (grasp::to(dataCurrentPtr)->stratType != Strategy::IR3NE) - r2gtrajectory->createTrajectory(grasp::Waypoint::lookup(*controller).command, seq); + r2gtrajectory->createTrajectory(grasp::Waypoint::lookup(*controller).state, seq); else - r2gtrajectory->createIGTrajectory(grasp::Waypoint::lookup(*controller).command, seq); + r2gtrajectory->createIGTrajectory(grasp::Waypoint::lookup(*controller).state, seq); // find global trajectory & perform R2GPlanner::perform(dataCurrentPtr->first, actionToString(grasp::to(dataCurrentPtr)->actionType).c_str(), seq, false); @@ -395,7 +395,6 @@ void FTDemo::create(const Desc& desc) { if (contactOccured && grasp::to(dataCurrentPtr)->stratType != Strategy::ELEMENTARY) { contactOccured = false; updateAndResample(dataCurrentPtr); - enableForceReading = false; ++iteration; continue; //}