Collision bounds for the hypotheses

pacman-project · Jun 27, 2016 · 8af8c07 · 8af8c07
1 parent 77423c7
commit 8af8c07
Show file tree

Hide file tree

Showing 13 changed files with 74 additions and 40 deletions.
diff --git 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<void(const grasp::Cloud::PointSeq&)> 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
@@ -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
@@ -3,7 +3,7 @@
 <grasp>
   <data delete_if_moved="1" delete_if_unlinked="1" delete_if_temporary="1">
     <trajectory waypoint_suffix=".wp">
-      <profile extrapolation="2.0" duration="5.0" idle="1.0" pruning="1" pruning_distance="0.001">
+      <profile extrapolation="2.0" r2g_duration="20" duration="5.0" idle="1.0" pruning="1" pruning_distance="0.001">
         <velocity arm="0.2" hand="0.5" other="0.5" dim="70" c1="1.0" c2="1.0" c3="1.0" c4="1.0" c5="1.0" c6="1.0" c7="1.0" c8="1.0" c9="1.0" c10="1.0" c11="1.0" c12="1.0" c13="1.0" c14="1.0" c15="1.0" c16="1.0" c17="1.0" c18="1.0" c19="1.0" c20="1.0" c21="1.0" c22="1.0" c23="1.0" c24="1.0" c25="1.0" c26="1.0" c27="1.0" c28="1.0" c29="1.0" c30="1.0" c31="1.0" c32="1.0" c33="1.0" c34="1.0" c35="1.0" c36="1.0" c37="1.0" c38="1.0" c39="1.0" c40="1.0" c41="1.0" c42="1.0" c43="1.0" c44="1.0" c45="1.0" c46="1.0" c47="1.0" c48="1.0" c49="1.0" c50="1.0" c51="1.0" c52="1.0" c53="1.0" c54="1.0" c55="1.0" c56="1.0" c57="1.0" c58="1.0" c59="1.0" c60="1.0" c61="1.0" c62="1.0" c63="1.0" c64="1.0" c65="1.0" c66="1.0" c67="1.0" c68="1.0" c69="1.0" c70="1.0"/>
         <acceleration arm="0.1" hand="0.5" other="0.5" dim="70" c1="1.0" c2="1.0" c3="1.0" c4="1.0" c5="1.0" c6="1.0" c7="1.0" c8="1.0" c9="1.0" c10="1.0" c11="1.0" c12="1.0" c13="1.0" c14="1.0" c15="1.0" c16="1.0" c17="1.0" c18="1.0" c19="1.0" c20="1.0" c21="1.0" c22="1.0" c23="1.0" c24="1.0" c25="1.0" c26="1.0" c27="1.0" c28="1.0" c29="1.0" c30="1.0" c31="1.0" c32="1.0" c33="1.0" c34="1.0" c35="1.0" c36="1.0" c37="1.0" c38="1.0" c39="1.0" c40="1.0" c41="1.0" c42="1.0" c43="1.0" c44="1.0" c45="1.0" c46="1.0" c47="1.0" c48="1.0" c49="1.0" c50="1.0" c51="1.0" c52="1.0" c53="1.0" c54="1.0" c55="1.0" c56="1.0" c57="1.0" c58="1.0" c59="1.0" c60="1.0" c61="1.0" c62="1.0" c63="1.0" c64="1.0" c65="1.0" c66="1.0" c67="1.0" c68="1.0" c69="1.0" c70="1.0"/>
         <distance arm="10.0" hand="1.0" other="1.0" dim="70" c1="1.0" c2="1.0" c3="1.0" c4="1.0" c5="1.0" c6="1.0" c7="1.0" c8="1.0" c9="1.0" c10="1.0" c11="1.0" c12="1.0" c13="1.0" c14="1.0" c15="1.0" c16="1.0" c17="1.0" c18="1.0" c19="1.0" c20="1.0" c21="1.0" c22="1.0" c23="1.0" c24="1.0" c25="1.0" c26="1.0" c27="1.0" c28="1.0" c29="1.0" c30="1.0" c31="1.0" c32="1.0" c33="1.0" c34="1.0" c35="1.0" c36="1.0" c37="1.0" c38="1.0" c39="1.0" c40="1.0" c41="1.0" c42="1.0" c43="1.0" c44="1.0" c45="1.0" c46="1.0" c47="1.0" c48="1.0" c49="1.0" c50="1.0" c51="1.0" c52="1.0" c53="1.0" c54="1.0" c55="1.0" c56="1.0" c57="1.0" c58="1.0" c59="1.0" c60="1.0" c61="1.0" c62="1.0" c63="1.0" c64="1.0" c65="1.0" c66="1.0" c67="1.0" c68="1.0" c69="1.0" c70="1.0"/>

diff --git a/resources/Spam/Demo/FT/SpamDemoFT_RobotBorisSim.xml b/resources/Spam/Demo/FT/SpamDemoFT_RobotBorisSim.xml
@@ -3,7 +3,7 @@
 <golem>
   <rand seed="0"></rand>
   <parallels threads="8"></parallels>
-  <messages level="debug"/><!-- level="undef" level="verbose" level="debug" level="info" level="warning" level="error" level="crit" -->
+  <messages level="error"/><!-- level="undef" level="verbose" level="debug" level="info" level="warning" level="error" level="crit" -->
 
   <universe name="Spam">
     <!--<window x="0" y="0" width="640" height="480"></window>-->

diff --git a/resources/Spam/Demo/R2G/GraspDataContactModelDemoR2G.xml b/resources/Spam/Demo/R2G/GraspDataContactModelDemoR2G.xml
@@ -3,7 +3,7 @@
 <grasp>
   <data planner_index="0" delete_if_moved="1" delete_if_unlinked="1" delete_if_temporary="1" model_suffix=".model" preproc_suffix="-proc">
     <manipulator>
-      <config_map i1="0" i2="0" i3="0" i4="0" i5="0" i6="0" i7="0" i8="0" i9="0" i10="0" i11="10" i12="0" i13="0" i14="0" i15="14" i16="0" i17="0" i18="0" i19="18" i20="0" i21="0" i22="0" i23="22" i24="0" i25="0" i26="0" i27="26"/>
+      <!--<config_map i1="0" i2="0" i3="0" i4="0" i5="0" i6="0" i7="0" i8="0" i9="0" i10="0" i11="10" i12="0" i13="0" i14="0" i15="14" i16="0" i17="0" i18="0" i19="18" i20="0" i21="0" i22="0" i23="22" i24="0" i25="0" i26="0" i27="26"/>-->
       <trajectory lin="1000.0" ang="1000.0" collision="1" cluster_size="20" timeout="0.1" throw="0"/>
     </manipulator>
 
@@ -13,10 +13,12 @@
     </configuration>
 
     <model id="Base">
-      <contact_3d type="feature" curv_fac="100.0" min_num="50" subsample_size="10000" distance="0.02" lambda="20.0" normal_slope="3.1415927"/>
+      <contact_3d type="part" min_num="50" subsample_size="10000" distance="0.02" lambda="20.0" normal_slope="3.1415927"/>
+      <contact_3d type="feature" min_num="50" subsample_size="10000" distance="0.02" lambda="20.0" normal_slope="3.1415927"/>
     </model>
     <model id="Any">
-      <contact_3d type="feature" curv_fac="100.0" min_num="50" subsample_size="0" distance="0.01" lambda="50.0" normal_slope="3.1415927"/>
+      <contact_3d type="part" min_num="50" subsample_size="0" distance="0.01" lambda="50.0" normal_slope="3.1415927"/>
+      <contact_3d type="feature" min_num="50" subsample_size="0" distance="0.01" lambda="50.0" normal_slope="3.1415927"/>
     </model>
 
     <appearance>

diff --git a/resources/Spam/Demo/R2G/GraspDataContactQueryDemoR2G.xml b/resources/Spam/Demo/R2G/GraspDataContactQueryDemoR2G.xml
@@ -1,9 +1,9 @@
 <?xml version="1.0" encoding="utf-8"?>
 
 <grasp>
-  <data planner_index="0" delete_if_moved="1" delete_if_unlinked="1" delete_if_temporary="1" query_suffix=".query">
+  <data planner_index="0" collision_points="1" delete_if_moved="1" delete_if_unlinked="1" delete_if_temporary="1" query_suffix=".query">
     <manipulator>
-      <config_map i1="0" i2="0" i3="0" i4="0" i5="0" i6="0" i7="0" i8="0" i9="0" i10="0" i11="10" i12="0" i13="0" i14="0" i15="14" i16="0" i17="0" i18="0" i19="18" i20="0" i21="0" i22="0" i23="22" i24="0" i25="0" i26="0" i27="26"/>
+      <!--<config_map i1="0" i2="0" i3="0" i4="0" i5="0" i6="0" i7="0" i8="0" i9="0" i10="0" i11="10" i12="0" i13="0" i14="0" i15="14" i16="0" i17="0" i18="0" i19="18" i20="0" i21="0" i22="0" i23="22" i24="0" i25="0" i26="0" i27="26"/>-->
       <trajectory lin="1000.0" ang="1000.0" collision="1" cluster_size="20" timeout="0.1" throw="1"/>
     </manipulator>
 
@@ -17,11 +17,17 @@
 
       <contact name="Default" penalty_exp="1.2">
         <query id="Base">
+          <contact_3d type="part" weight="1.0" kernels="5000" epsilon="1e-10" trials="100" feature_std_dev_max="5.0" pose_std_dev_max="5.0">
+            <nn_search neighbours="0" search_checks="32" search_kdtrees="1" search_leaf_max_size="10"/>
+          </contact_3d>
           <contact_3d type="feature" weight="1.0" kernels="5000" epsilon="1e-10" trials="100" feature_std_dev_max="5.0" pose_std_dev_max="5.0">
             <nn_search neighbours="0" search_checks="32" search_kdtrees="1" search_leaf_max_size="10"/>
-          </contact_3d>  
+          </contact_3d>
         </query>
         <query id="Any">
+          <contact_3d type="part" weight="1.0" kernels="5000" epsilon="1e-10" trials="100" feature_std_dev_max="5.0" pose_std_dev_max="5.0">
+            <nn_search neighbours="0" search_checks="32" search_kdtrees="1" search_leaf_max_size="10"/>
+          </contact_3d>
           <contact_3d type="feature" weight="1.0" kernels="5000" epsilon="1e-10" trials="100" feature_std_dev_max="5.0" pose_std_dev_max="5.0">
             <nn_search neighbours="0" search_checks="32" search_kdtrees="1" search_leaf_max_size="10"/>
           </contact_3d>

diff --git a/resources/Spam/Demo/R2G/GraspDataPointsCurvDemoR2G.xml b/resources/Spam/Demo/R2G/GraspDataPointsCurvDemoR2G.xml
@@ -20,6 +20,7 @@
       <downsample_segmentation enabled="1" enabled_with_normals="1" enabled_voxel_grid="0" grid_leaf_size="0.0015"/>
 
       <segmentation incremental="0" distance_threshold="0.00001"/>
+      <clustering enabled="0" tolerance="0.003" min_size="50" max_size="2147483647"/>
 
       <region>
         <bounds type="box" group="1">

diff --git a/resources/Spam/Demo/R2G/SpamDataR2GTrajectoryDemoR2G.xml b/resources/Spam/Demo/R2G/SpamDataR2GTrajectoryDemoR2G.xml
@@ -3,7 +3,7 @@
 <grasp>
   <data delete_if_moved="1" delete_if_unlinked="1" delete_if_temporary="1">
     <trajectory waypoint_suffix=".wp">
-      <profile extrapolation="0.0" duration="5.0" idle="1.0" pruning="1" pruning_distance="0.001">
+      <profile extrapolation="0.0" r2g_duration="20" duration="5.0" idle="1.0" pruning="1" pruning_distance="0.001">
         <velocity arm="0.2" hand="0.5" other="0.5" dim="70" c1="1.0" c2="1.0" c3="1.0" c4="1.0" c5="1.0" c6="1.0" c7="1.0" c8="1.0" c9="1.0" c10="1.0" c11="1.0" c12="1.0" c13="1.0" c14="1.0" c15="1.0" c16="1.0" c17="1.0" c18="1.0" c19="1.0" c20="1.0" c21="1.0" c22="1.0" c23="1.0" c24="1.0" c25="1.0" c26="1.0" c27="1.0" c28="1.0" c29="1.0" c30="1.0" c31="1.0" c32="1.0" c33="1.0" c34="1.0" c35="1.0" c36="1.0" c37="1.0" c38="1.0" c39="1.0" c40="1.0" c41="1.0" c42="1.0" c43="1.0" c44="1.0" c45="1.0" c46="1.0" c47="1.0" c48="1.0" c49="1.0" c50="1.0" c51="1.0" c52="1.0" c53="1.0" c54="1.0" c55="1.0" c56="1.0" c57="1.0" c58="1.0" c59="1.0" c60="1.0" c61="1.0" c62="1.0" c63="1.0" c64="1.0" c65="1.0" c66="1.0" c67="1.0" c68="1.0" c69="1.0" c70="1.0"/>
         <acceleration arm="0.1" hand="0.5" other="0.5" dim="70" c1="1.0" c2="1.0" c3="1.0" c4="1.0" c5="1.0" c6="1.0" c7="1.0" c8="1.0" c9="1.0" c10="1.0" c11="1.0" c12="1.0" c13="1.0" c14="1.0" c15="1.0" c16="1.0" c17="1.0" c18="1.0" c19="1.0" c20="1.0" c21="1.0" c22="1.0" c23="1.0" c24="1.0" c25="1.0" c26="1.0" c27="1.0" c28="1.0" c29="1.0" c30="1.0" c31="1.0" c32="1.0" c33="1.0" c34="1.0" c35="1.0" c36="1.0" c37="1.0" c38="1.0" c39="1.0" c40="1.0" c41="1.0" c42="1.0" c43="1.0" c44="1.0" c45="1.0" c46="1.0" c47="1.0" c48="1.0" c49="1.0" c50="1.0" c51="1.0" c52="1.0" c53="1.0" c54="1.0" c55="1.0" c56="1.0" c57="1.0" c58="1.0" c59="1.0" c60="1.0" c61="1.0" c62="1.0" c63="1.0" c64="1.0" c65="1.0" c66="1.0" c67="1.0" c68="1.0" c69="1.0" c70="1.0"/>
         <distance arm="10.0" hand="1.0" other="1.0" dim="70" c1="1.0" c2="1.0" c3="1.0" c4="1.0" c5="1.0" c6="1.0" c7="1.0" c8="1.0" c9="1.0" c10="1.0" c11="1.0" c12="1.0" c13="1.0" c14="1.0" c15="1.0" c16="1.0" c17="1.0" c18="1.0" c19="1.0" c20="1.0" c21="1.0" c22="1.0" c23="1.0" c24="1.0" c25="1.0" c26="1.0" c27="1.0" c28="1.0" c29="1.0" c30="1.0" c31="1.0" c32="1.0" c33="1.0" c34="1.0" c35="1.0" c36="1.0" c37="1.0" c38="1.0" c39="1.0" c40="1.0" c41="1.0" c42="1.0" c43="1.0" c44="1.0" c45="1.0" c46="1.0" c47="1.0" c48="1.0" c49="1.0" c50="1.0" c51="1.0" c52="1.0" c53="1.0" c54="1.0" c55="1.0" c56="1.0" c57="1.0" c58="1.0" c59="1.0" c60="1.0" c61="1.0" c62="1.0" c63="1.0" c64="1.0" c65="1.0" c66="1.0" c67="1.0" c68="1.0" c69="1.0" c70="1.0"/>

diff --git 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<Data>(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<Data>(dataCurrentPtr)->itemMap, beliefStatePtr->first, to<Data>(dataCurrentPtr)->getView());
@@ -632,7 +637,11 @@ grasp::data::Item::Map::iterator spam::PosePlanner::estimatePose(Data::Mode mode
 			}
 			else
 				grasp::Cloud::transform(grasp::to<Data>(dataCurrentPtr)->queryTransform, grasp::to<Data>(dataCurrentPtr)->simulateObjectPose, grasp::to<Data>(dataCurrentPtr)->simulateObjectPose);
-
+
+			// callback to handle groundtruth, if any
+			if (simulateHandlerCallback)
+				simulateHandlerCallback(grasp::to<Data>(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
@@ -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<Real>()*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 <Enter> 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<Data>(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
@@ -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,17 +737,17 @@ 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);
 
 	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,15 +778,15 @@ 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);
 
 	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());