Retrieve state of the robot instead of command when planning a R2G tr…

…ajectory
pacman-project · Jul 4, 2016 · 5d8b8da · 5d8b8da
1 parent f2d1863
commit 5d8b8da
Show file tree

Hide file tree

Showing 3 changed files with 4 additions and 6 deletions.
diff --git 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
@@ -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
@@ -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,17 +385,16 @@ void FTDemo::create(const Desc& desc) {
 
 					Controller::State::Seq seq;
 					if (grasp::to<Data>(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<Data>(dataCurrentPtr)->actionType).c_str(), seq, false);
 
 					if (contactOccured && grasp::to<Data>(dataCurrentPtr)->stratType != Strategy::ELEMENTARY) {
 						contactOccured = false;
 						updateAndResample(dataCurrentPtr);
-						enableForceReading = false;
 						++iteration;
 						continue;
 						//}