diff --git a/cfg/IriStateMachineCompanion.cfg b/cfg/IriStateMachineCompanion.cfg
index 87364bd173ecc65784e378059cd6ddb12cd90cb4..02bdaf66ff472f3e6a5a0d15798fc8382c9f4c6f 100755
--- a/cfg/IriStateMachineCompanion.cfg
+++ b/cfg/IriStateMachineCompanion.cfg
@@ -41,6 +41,8 @@ gen = ParameterGenerator()
 #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("threshold_max_dist_between_robot_and_person_conf",  double_t,  0,    "Maximum distance between robot and person to talk (meters)",  3.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)
+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.0,      0.0,  3.0)
+gen.add("threshold_minimun_distance_not_collide_conf",  double_t,  0,    "collision distance of the save_cmd_vel== 0.3",  0.8,      0.0,  3.0)
+
 exit(gen.generate(PACKAGE, "IriStateMachineCompanionAlgorithm", "IriStateMachineCompanion"))
 
diff --git a/include/iri_state_machine_companion_alg_node.h b/include/iri_state_machine_companion_alg_node.h
index 3a4e8b048d5f078f14e50d91c17f2d3ebeb39ed0..7f6eae162c4f27d282e2bbe73c66e2c1f158d065 100644
--- a/include/iri_state_machine_companion_alg_node.h
+++ b/include/iri_state_machine_companion_alg_node.h
@@ -95,8 +95,7 @@ class IriStateMachineCompanionAlgNode : public algorithm_base::IriBaseAlgorithm<
     void people_tracks_mutex_exit(void);
 
     unsigned int id_person_companion_, id_second_person_companion_;
-    double threshold_max_dist_between_robot_and_person_;
-    double threshold_less_max_vel_robot_;
+    double threshold_max_dist_between_robot_and_person_,threshold_less_max_vel_robot_,threshold_minimun_distance_not_collide_;
     // [service attributes]
 
     // [client attributes]
diff --git a/src/iri_state_machine_companion_alg_node.cpp b/src/iri_state_machine_companion_alg_node.cpp
index 7732ac6a2ccf8adb205729d49ef3c708054fe7a5..f1499b361a2751b1446bc40d1bf54ee0f37dac64 100644
--- a/src/iri_state_machine_companion_alg_node.cpp
+++ b/src/iri_state_machine_companion_alg_node.cpp
@@ -122,12 +122,22 @@ void IriStateMachineCompanionAlgNode::mainNodeThread(void)
 	// TODO: status side-by-side.  ==6
 	double person_companion_velocity=0.0;
 	double distance_robot_person_comp;
+	
+	double minimum_distance=10;
 	for(unsigned int u=0;u<this->obs.size();u++){
+		if((u==0)&&(this->obs[u].id!=id_person_companion_)){
+			minimum_distance=sqrt((this->obs[u].x - this->robot_pose_.x)*(this->obs[u].x -this->robot_pose_.x)+(this->obs[u].y - this->robot_pose_.y)*(this->obs[u].y -this->robot_pose_.y));
+		}
 		if(this->obs[u].id==id_person_companion_){
 			person_companion_velocity=sqrt(this->obs[u].vx*this->obs[u].vx+this->obs[u].vy*this->obs[u].vy);
 			distance_robot_person_comp=sqrt((this->obs[u].x - this->robot_pose_.x)*(this->obs[u].x -this->robot_pose_.x)+(this->obs[u].y - this->robot_pose_.y)*(this->obs[u].y -this->robot_pose_.y));
 		}
 		//std::cout<< "this->obs; id=" <<this->obs[u].id<<"; x="<<this->obs[u].x<<"; y="<<this->obs[u].y<<"; vx="<<this->obs[u].vx<<"; vy="<<this->obs[u].vy<< std::endl;
+
+		double actual_per_rob_dist=sqrt((this->obs[u].x - this->robot_pose_.x)*(this->obs[u].x -this->robot_pose_.x)+(this->obs[u].y - this->robot_pose_.y)*(this->obs[u].y -this->robot_pose_.y));
+		if((actual_per_rob_dist<minimum_distance)&&(this->obs[u].id!=id_person_companion_)){ // la person companion no cuenta. Me faltará caso obst staticos...
+			minimum_distance=actual_per_rob_dist;
+		}
  	}
 
 	//ROS_INFO("save_companionStatus_mesage_global_.maximumRobotVelocity=%f; person_companion_velocity=%f",save_companionStatus_mesage_global_.maximumRobotVelocity,person_companion_velocity);
@@ -145,6 +155,17 @@ void IriStateMachineCompanionAlgNode::mainNodeThread(void)
 	if(distance_robot_person_comp > threshold_max_dist_between_robot_and_person_){
 	     this->tibi_akp_status_UInt64_msg_.data =8; //status  wait for me, please.
 	}
+
+	threshold_minimun_distance_not_collide_=config_.threshold_minimun_distance_not_collide_conf;	
+	// new status:  I can not move!, Sorry.
+	/*if((minimum_distance < threshold_minimun_distance_not_collide_)&&(this->robot_pose_.v<0.2)){
+		this->tibi_akp_status_UInt64_msg_.data =9; //obstacle very close.
+	}*/
+
+	if(save_companionStatus_mesage_global_.estateNoPathObstacleClose){
+		ROS_INFO("NO PATH STATUS save_companionStatus_mesage_global_.estateNoPathObstacleClose=%d",save_companionStatus_mesage_global_.estateNoPathObstacleClose);
+		this->tibi_akp_status_UInt64_msg_.data =9; //obstacle very close.
+	}
 	
   //this->tibi_akp_status_UInt64_msg_.data = my_var;