diff --git a/cfg/IriStateMachineCompanion.cfg b/cfg/IriStateMachineCompanion.cfg
index 9d1eedbaf9b2c1c73721526141f0ac5ca6a5182e..ae3075f878ea3f71a9a7e9f4258ad6a734cd2dbf 100755
--- a/cfg/IriStateMachineCompanion.cfg
+++ b/cfg/IriStateMachineCompanion.cfg
@@ -40,7 +40,7 @@ gen = ParameterGenerator()
 #       Name                       Type       Reconfiguration level            Description                       Default   Min   Max
 #gen.add("velocity_scale_factor",  double_t,  0,                               "Maximum velocity scale factor",  0.5,      0.0,  1.0)
 # threshold_max_dist_between_robot_and_person_
-gen.add("conf_bool_test_case_head1"       ,   bool_t,  0, "test head left or right.", False)
+gen.add("conf_bool_test_case_head1"       ,   bool_t,  0, "test head left or right.", True)
 gen.add("out_std_mesages_conf"       ,   bool_t,  0, "if true we se the internal std_out mesages.", False)
 gen.add("threshold_max_dist_between_robot_and_person_conf",  double_t,  0,    "Maximum distance between robot and person to talk (meters)",  4.5,      0.0,  30.0)
 gen.add("threshold_less_max_vel_robot_conf",  double_t,  0,    "less max vel of person because robot needs 0.2m/s more to react",  0.2,      0.0,  3.0)
diff --git a/src/iri_state_machine_companion_alg_node.cpp b/src/iri_state_machine_companion_alg_node.cpp
index 3dd27c8759e61548b6b6a58daa5ea33769400b8e..49629760e22e24f072b86c8694cb2079e337a121 100644
--- a/src/iri_state_machine_companion_alg_node.cpp
+++ b/src/iri_state_machine_companion_alg_node.cpp
@@ -110,12 +110,16 @@ void IriStateMachineCompanionAlgNode::mainNodeThread(void)
 	final_goal_to_return_=Spoint(config_.X_goal_to_return_conf,config_.Y_goal_to_return_conf);
 
 	Spoint position_p1,position_p2;
+	SpointV position_p1V,position_p2V;
+
 	for(unsigned int u=0;u<this->obs.size();u++){
 		if(this->obs[u].id==id_person_companion_){
 			position_p1=Spoint(this->obs[u].x,this->obs[u].y,this->obs[u].time_stamp);
+			position_p1V=SpointV(this->obs[u].x,this->obs[u].y,this->obs[u].time_stamp,this->obs[u].vx,this->obs[u].vy);
 		}
 		if(this->obs[u].id==id_second_person_companion_){
 			position_p2=Spoint(this->obs[u].x,this->obs[u].y,this->obs[u].time_stamp);
+			position_p2V=SpointV(this->obs[u].x,this->obs[u].y,this->obs[u].time_stamp,this->obs[u].vx,this->obs[u].vy);
 		}			
  	}
 
@@ -130,20 +134,28 @@ void IriStateMachineCompanionAlgNode::mainNodeThread(void)
 		if(save_companionStatus_mesage_global_.robotLateral){ //robot lateral
 			 
   			double angle=atan2(this->robot_pose_.y-position_p1.y , this->robot_pose_.x-position_p1.x);
-  			double theta =atan2(final_goal_to_return_.y-position_p1.y , final_goal_to_return_.x-position_p1.x);
+			//double angle=atan2(position_p1.y - this->robot_pose_.y , position_p1.x -  this->robot_pose_.x);
+  			//double theta =atan2(final_goal_to_return_.y-position_p1.y , final_goal_to_return_.x-position_p1.x);
+			//double theta =atan2(position_p1.y - final_goal_to_return_.y , position_p1.x-final_goal_to_return_.x);
+			double theta =atan2(position_p1V.vy , position_p1V.vx);
 			bool_test_case_head1_=config_.conf_bool_test_case_head1;
+		if(sqrt((position_p1V.vy*position_p1V.vy)+(position_p1V.vx*position_p1V.vx))>0.2){
 			if(bool_test_case_head1_){
 	  			if( diffangle(theta, angle) < 0 )
 	  			{
 	    			  this->tibi_akp_status_UInt64_msg_.data =11; // por ejemplo right=11.
+					std::cout<< " iri_state_machine; diff<0; status=11" << std::endl;
 	  			}else{
+					std::cout<< " iri_state_machine; diff>0; status=12" << std::endl;
 	    			  this->tibi_akp_status_UInt64_msg_.data =12; // por ejemplo left =12.
 	  			}
 			}else{
 	  			if( diffangle(theta, angle) < 0 )
 	  			{
+				std::cout<< " iri_state_machine; diff<0; status=12" << std::endl;
 	    			  this->tibi_akp_status_UInt64_msg_.data =12;
 	  			}else{
+				std::cout<< " iri_state_machine; diff>0; status=11" << std::endl;
 	    			  this->tibi_akp_status_UInt64_msg_.data =11;
 	  			}
 			}
@@ -151,7 +163,9 @@ void IriStateMachineCompanionAlgNode::mainNodeThread(void)
 		}else{ //robot central
 			this->tibi_akp_status_UInt64_msg_.data =13;
 		}
-
+	}else{
+		this->tibi_akp_status_UInt64_msg_.data =13;
+	}
 	// FIN calculate side where robot need to look