diff --git a/cfg/AkpLocalPlanner.cfg b/cfg/AkpLocalPlanner.cfg
index fa9058e70b7483ec47ab5e728fd1f8125dd0fcc5..1f02fe3cc4f353775634c35073a8975f400cb761 100755
--- a/cfg/AkpLocalPlanner.cfg
+++ b/cfg/AkpLocalPlanner.cfg
@@ -36,6 +36,9 @@ from dynamic_reconfigure.parameter_generator_catkin import *
 gen = ParameterGenerator()
 gen.add("frame_map",                str_t,     0, "frame to transform poses to", "map")
 gen.add("frame_robot_footprint",    str_t,     0, "frame to transform poses to", "ana/base_link") 
+gen.add("fuera_bolitas_goals_companion_markers_conf",      bool_t,    0,   " Fuera bolas markers",   True)
+gen.add("conf_debug_all_IVO",      bool_t,    0,   " All mesagenes to debug IVO",   True)
+gen.add("conf_debug_few_IVO",      bool_t,    0,   " Only few velocity messages to debug IVO",   False)
 
 #### Parameters to start the simulation ####
 gen.add("Block_parameters_division1"                  , str_t,    0,  "Parameters to start the simulation:","Parameters to start the simulation:")
@@ -78,10 +81,11 @@ gen.add("proximity_distance_between_robot_and_person", double_t,   0,   "Is the
 # set-up the best accompaniment angle
 gen.add("real_companion_angle_SideBySide",   double_t,   0,   "Is the companion angle set for the case without obstacles", 90,      0.0,  180.0)
 # set-up the maximum real robot speed
-gen.add("max_real_speed_out", double_t, 0, "maximum robot real velocity", 1.0 , 0.0, 2.0)
+gen.add("max_real_speed_out", double_t, 0, "maximum robot real velocity", 0.6 , 0.0, 2.0)
 # set-up the constant of the controller (if your robot velocity controller divides the velocity by 2 or something similar) => Normally, only used in the real robot.
+gen.add("max_real_angular_speed_out", double_t, 0, " limite velocidad maxima de salida del robot", 0.4 , 0.0, 2.0)
 gen.add("speed_k", double_t, 0, " constant multiplying output linear speed", 1.5 , 1.0, 2.0)  # For our real robot is needed a speed_k=1.5 
-
+gen.add("speed_k_angular", double_t, 0, " constant multiplying output linear speed", 1.2 , 1.0, 2.0) 
 
 
 ####  Parameters to do a slow stop in the real robot, when accompanies a person and this person stops ####  
diff --git a/config/IVO/move_base_params.yaml b/config/IVO/move_base_params.yaml
index 9892cd98504052ded0518c40d32257fc6dfc4366..d835219d266fb29ac8124ec6385b55005f61f3d8 100755
--- a/config/IVO/move_base_params.yaml
+++ b/config/IVO/move_base_params.yaml
@@ -12,7 +12,7 @@ oscillation_distance: 0.2
 recovery_behavior_enabled: false # true
 clearing_rotation_allowed: false # true
 
-base_local_planner: "iri_atr_akp_local_planner_companion/AkpLocalPlanner"
+base_local_planner: "iri_robot_aspsi/AkpLocalPlanner"
 #alternatives: base_local_planner/TrajectoryPlannerROS, dwa_local_planner/DWAPlannerROS
 base_global_planner: "navfn/NavfnROS"
 #alternatives: navfn/NavfnROS, global_planner/GlobalPlanner, carrot_planner/CarrotPlanner
diff --git a/include/akp_local_planner_alg_node.h b/include/akp_local_planner_alg_node.h
index 2a47dc4f98d955fe42c0eba63b505eccd0da17ea..a6a13a73fd29c651baaafd2cef03beac502e15af 100644
--- a/include/akp_local_planner_alg_node.h
+++ b/include/akp_local_planner_alg_node.h
@@ -516,11 +516,19 @@ class AkpLocalPlanner : public nav_core::BaseLocalPlanner
 		double dist_betw_rob_and_comp_people_to_slow_velocity_;
 
 		double ros_max_real_speed_out_;
+		double ros_max_real_angular_speed_out_;
+
 		double number_of_people_in_group_;
 		double  num_people_for_state_;
 
 		bool output_screen_messages_;
 
+
+
+
+		bool debug_all_IVO_;
+		bool debug_few_IVO_;
+
   public:
    /**
     * \brief Constructor
diff --git a/launch/ivo_launch/ivo_detector_and_tracker.launch b/launch/ivo_launch/ivo_detector_and_tracker.launch
index 317858704f652229d4625a48aca4a502fc54af03..81554b7d625bcee810e611e24bb319b3bfea1a42 100644
--- a/launch/ivo_launch/ivo_detector_and_tracker.launch
+++ b/launch/ivo_launch/ivo_detector_and_tracker.launch
@@ -36,7 +36,7 @@
     <arg name="map_name"                 default="fme_door_open"/>
   <arg name="use_amcl"                   default="true"/>
     <arg name="amcl_config"              default="$(arg path)/amcl.yaml"/>
-    <arg name="initial_x"                default="-1.0"/>
+    <arg name="initial_x"                default="-1.5"/>
     <arg name="initial_y"                default="8.0"/>
     <arg name="initial_yaw"              default="-1.57"/>
  <!-- FME: initial_x=-1.5; initial_y=8.0; initial_yaw=-1.57-->
@@ -56,8 +56,8 @@
    <!--<arg name="sim_sufix" value=""                unless="$(arg public_sim)"/>-->
 
 <group if="$(arg use_map)">
-    <group if="$(arg use_map_server)">
-      <include file="$(find iri_atr_akp_local_planner_companion)/launch/ivo_launch/launchs_internos_modificados/launchs_in_iri_rosnav_needed_for_tiago_robot/launchs_in_iri_rosnav_needed_for_tiago_robot/map_server_tiago.launch">
+ <!--   <group if="$(arg use_map_server)">
+      <include file="$(find iri_robot_aspsi)/launch/ivo_launch/launchs_internos_modificados/launchs_in_iri_rosnav_needed_for_tiago_robot/launchs_in_iri_rosnav_needed_for_tiago_robot/map_server_tiago.launch">
         <arg name="ns"            value="$(arg ns)"/>
         <arg name="map_frame_id"  value="$(arg map_frame_id)"/>
         <arg name="map_name"      value="$(arg map_name)"/>
@@ -66,10 +66,10 @@
         <arg name="output"        value="$(arg output)" />
         <arg name="launch_prefix" value="$(arg launch_prefix)" />
       </include>
-    </group>
+    </group>-->
     
     <group if="$(arg use_amcl)">
-      <include file="$(find iri_atr_akp_local_planner_companion)/launch/ivo_launch/launchs_internos_modificados/iri_rosnav/launch/include/amcl_ivo.launch">
+      <include file="$(find iri_robot_aspsi)/launch/ivo_launch/launchs_internos_modificados/iri_rosnav/launch/include/amcl_ivo.launch">
         <arg name="ns"            value="$(arg ns)"/>
         <arg name="config"        value="$(arg amcl_config)"/>
         <arg name="map_frame_id"  value="$(arg map_frame_id)"/>
@@ -130,7 +130,7 @@
 
 <!-- args from iri -->
   <arg name="rviz"                    default="true"/>
-  <arg name="rviz_file"               default="$(find iri_atr_akp_local_planner_companion)/rviz/navigation_public_sim_TiagoDual.rviz"/>
+  <arg name="rviz_file"               default="$(find iri_robot_aspsi)/rviz/navigation_public_sim_TiagoDual.rviz"/>
   <arg name="move_base_params"        default="move_base_params.yaml"/>
   <arg name="costmap_common_params"   default="common_params.yaml"/>
   <arg name="costmap_local_params"    default="local_params.yaml"/>
diff --git a/launch/ivo_launch/ivo_nav_companion_only.launch b/launch/ivo_launch/ivo_nav_companion_only.launch
index 0a8753bb246055f2edc4c2cc9a859aa7e6ca01d7..62518923a944e5ba9649a7d194555d2e0af1f022 100644
--- a/launch/ivo_launch/ivo_nav_companion_only.launch
+++ b/launch/ivo_launch/ivo_nav_companion_only.launch
@@ -53,7 +53,7 @@
 
 <!-- NEW variables, maybe I need to add them-->
 	<param name="AkpLocalPlanner/detection_laser_obstacle_distances" value="3.5"/> 
-	<param name="AkpLocalPlanner/real_companion_angle_SideBySide" value="70.0"/>  <!-- No lo setea bien desde aquí, si hace falta, mejor cambiarlo en el dynamic reconfigure.-->
+	<param name="AkpLocalPlanner/real_companion_angle_SideBySide" value="90.0"/>  <!-- No lo setea bien desde aquí, si hace falta, mejor cambiarlo en el dynamic reconfigure.-->
       <param name="AkpLocalPlanner/speed_k"  value="1.0"/>  <!--MUY importante: en sim, speed_k=1.0; en robot real,  speed_k=1.5; -->  
       <param name="AkpLocalPlanner/speed_k_angular"  value="1.0"/> 
  <param name="AkpLocalPlanner/conf_id_to_remove" value="100000"/>
diff --git a/src/akp_local_planner_alg_node.cpp b/src/akp_local_planner_alg_node.cpp
index fe0c6db5f08a48b6fff2b2409e62db34e03f4f1a..1d14d327f6fb513f82908768b6c90a13dc30171e 100644
--- a/src/akp_local_planner_alg_node.cpp
+++ b/src/akp_local_planner_alg_node.cpp
@@ -64,7 +64,9 @@ AkpLocalPlanner::AkpLocalPlanner(void) :
 	debug_sideBySide2019_(false),
 	first_iter_for_update_scene_(true),
 	output_screen_messages_(false),
-simulation_(false)
+	simulation_(false),
+	debug_all_IVO_(false),
+	debug_few_IVO_(false)
 {
 
 ROS_INFO("CREATE ALG NODE");
@@ -452,9 +454,9 @@ void AkpLocalPlanner::initialize(std::string name, tf2_ros::Buffer* tf, costmap_
 
 bool AkpLocalPlanner::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
 { 
-	//if(output_screen_messages_){
-	//ROS_INFO(" NODE iri_robot_aspsi  this->planner_.get_number_of_vertex(  )=%d; horizon_time=%f; ",this->planner_.get_number_of_vertex(  ),this->planner_.get_horizon_time());
-	//} //conf_dist_betw_rob_and_comp_people_to_slow_velocity
+	if(debug_all_IVO_){
+	ROS_INFO(" NODE iri_robot_aspsi  this->planner_.get_number_of_vertex(  )=%d; horizon_time=%f; ",this->planner_.get_number_of_vertex(  ),this->planner_.get_horizon_time());
+	} //conf_dist_betw_rob_and_comp_people_to_slow_velocity
 //ROS_INFO("simulation_=%d; ",simulation_);
 	
 		
@@ -799,15 +801,20 @@ bool AkpLocalPlanner::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
 		double START_time_secs2 =ros::Time::now().toSec();
 
 
-		if(output_screen_messages_){
-			//ROS_INFO( "1 (IMPORTANTE after person plan y con person v, reduction) dt_real=%f;", ros_time_to_sec_general_before_iter_-ros_time_to_sec);
-		}
+		//if(debug_all_IVO_){
+		//	ROS_INFO( "1 (IMPORTANTE after person plan y con person v, reduction) dt_real=%f;", ros_time_to_sec_general_before_iter_-ros_time_to_sec);
+		//}
 	
 
 		// Function that computes the local-planner, the Robot's Adaptive Side-by-Side Accompaniment of One Person (ASPSI)
 		robot_plan_succed =  this->planner_.robot_plan_companion3(best_next_pose,reactive_);
 		ros_time_to_sec_general_before_iter_=ros::Time::now().toSec();
 
+		if(debug_all_IVO_){
+				ROS_INFO( " (IMPORTANTE after person plan y con person v, reduction) d_r_p1=%f; init_V_robot=%f", this->robot_pose_.distance(person_companion_position_),best_next_pose.v );
+		}
+
+
 		/* LIMIT velocity if very near of people_companion's!!!!!   */
 		if(this->robot_pose_.distance(person_companion_position_)<dist_betw_rob_and_comp_people_to_slow_velocity_ ){
 			// distance entre robot y personas que acompaña, menor que 2 metros. dist_betw_rob_and_comp_people_to_slow_velocity_=4
@@ -816,6 +823,11 @@ bool AkpLocalPlanner::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
 			v_people=person_companion_position_.v();
 			v_lim_actual=v_people+0.2; // v_p + 0.2 m/s
 
+			if((debug_all_IVO_)||(debug_few_IVO_)){
+				ROS_INFO( "LIMIT velocity if very near of people_companion's (IMPORTANTE antes person plan y con person v, reduction) v_people=%f; v_p1=%f;  v_p2=%f;", v_people, person_companion_position_.v() , second_person_companion_position_.v());
+			}
+
+
 			if((best_next_pose.v>v_lim_actual)&&(v_people>0.2)){
 				best_next_pose.v=v_lim_actual;
 			}
@@ -860,7 +872,7 @@ bool AkpLocalPlanner::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
 
     START_time_secs10 =ros::Time::now().toSec();
 
-		if(output_screen_messages_){
+    	if(debug_all_IVO_){
  		ROS_INFO( "2 after robot_plan_companion3; this->move_base=%d  this->robot_pose_.v=%f;  this->robot_pose_.w=%f",this->move_base ,this->robot_pose_.v, this->robot_pose_.w);
 		}
 
@@ -894,7 +906,7 @@ bool AkpLocalPlanner::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
       best_next_pose_robot_companion_markers_.id = 30;
      // MarkerArray_msg_.markers.push_back( best_next_pose_robot_companion_markers_ ); //esfera que marca el siguiente goal del robot, hacia donde va!
    
-      if(output_screen_messages_){
+      if(fuera_bolitas_goals_companion_markers_){
         MarkerArray_msg_comp_markers_.markers.push_back( best_next_pose_robot_companion_markers_ );
       }
 	double END_time_secs5;
@@ -1018,14 +1030,41 @@ bool AkpLocalPlanner::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
 
 	
 	cmd_vel.linear.x*=this->config_.speed_k; // IMPORTANT, do not change for TIBI. It is for real robot. OUR velocity controller divides the robot's velocity by 2. Then, we need to multiply with this speed_k here.
+	cmd_vel.angular.z*=this->config_.speed_k_angular;
 
 	if((see_std_out_mesages_ros_)||(see_std_out_velocities_ros_)){
 		ROS_INFO( " cmd_vel.linear.x * Speed_k (cmd_vel=%f) ", cmd_vel.linear.x);
 	}
 
-    if(ros_max_real_speed_out_<cmd_vel.linear.x){
-	cmd_vel.linear.x=ros_max_real_speed_out_;			
-    }
+	 //reducir velocidad lineal final con el maximo valor de salida.
+	    if(ros_max_real_speed_out_<cmd_vel.linear.x){
+		cmd_vel.linear.x=ros_max_real_speed_out_;
+		if((debug_all_IVO_)||(debug_few_IVO_)){
+		    ROS_INFO( "(positive) cmd_vel.linear.x max_LIMIT (cmd_vel_linear=%f) ", cmd_vel.linear.x);
+		}
+	    }
+	    if((-ros_max_real_speed_out_)>cmd_vel.linear.x){
+		cmd_vel.linear.x=-ros_max_real_speed_out_;
+		if((debug_all_IVO_)||(debug_few_IVO_)){
+		    ROS_INFO( "(negative) cmd_vel.linear.x max_LIMIT (cmd_vel_linear=%f) ", cmd_vel.linear.x);
+		}
+	    }
+	  //reducir velocidad anglular final con el maximo valor de salida.
+
+	    if(ros_max_real_angular_speed_out_<cmd_vel.angular.z){
+		cmd_vel.angular.z=ros_max_real_angular_speed_out_;
+		if((debug_all_IVO_)||(debug_few_IVO_)){
+		    ROS_INFO( " (positive) cmd_vel.angular.z max_LIMIT (cmd_vel_angular=%f) ", cmd_vel.angular.z);
+	        }
+
+	    }
+	   if((-ros_max_real_angular_speed_out_)>cmd_vel.angular.z){
+		cmd_vel.angular.z=-ros_max_real_angular_speed_out_;
+		if((debug_all_IVO_)||(debug_few_IVO_)){
+		    ROS_INFO( " (negative) cmd_vel.angular.z max_LIMIT (cmd_vel_angular=%f) ", cmd_vel.angular.z);
+		}
+
+	    }
 
 
     //to stop slowly:
@@ -1050,9 +1089,9 @@ bool AkpLocalPlanner::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
     		cmd_vel.angular.z = 0.0;
     	}
 
-			if(output_screen_messages_){
+    	if((debug_all_IVO_)||(debug_few_IVO_)){
 			ROS_ERROR(" (out) STOP  slowly  ROBOT!; step_decreace_velocity_stop_robot_=%f, cmd_vel.linear.x=%f; this->robot_pose_.v=%f",step_decreace_velocity_stop_robot_,cmd_vel.linear.x,this->robot_pose_.v);
-			}
+		}
     }
 	
     //TODO: caso Vr>0.5 o 0.6 y se va la velocidad a 0.0 de golpe! tambien he de frenar lento! (hay que incluirlo aquí al final, gestion de cambios muy bruscos de velocidad)
@@ -1153,7 +1192,7 @@ bool AkpLocalPlanner::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
 
 	///FIN  USE diferent params for side-by-side central and lateral, for people companion!
 
-	if(output_screen_messages_){
+	if((debug_all_IVO_)||(debug_few_IVO_)){
 	ROS_ERROR(" (LAST velocity robot)  cmd_vel.linear.x=%f; cmd_vel.linear.y=%f this->robot_pose_.v=%f; cmd_vel.angular.z=%f; robot_plan_succed=%d",cmd_vel.linear.x,cmd_vel.linear.y,this->robot_pose_.v,cmd_vel.angular.z,robot_plan_succed);
 	}
 
@@ -1573,6 +1612,12 @@ void AkpLocalPlanner::reconfigureCallback(iri_robot_aspsi::AkpLocalPlannerConfig
 		config.id_person_companion=this->config_.id_person_companion;
 	}*/
 	
+	debug_all_IVO_=config.conf_debug_all_IVO;
+	this->planner_.set_debug_few_IVO(config.conf_debug_all_IVO);
+	debug_few_IVO_=config.conf_debug_few_IVO;
+	this->planner_.set_debug_few_IVO(config.conf_debug_few_IVO);
+
+
 	/// Fin Selec closes person as companion!!!
 	std::vector<double> params(5,0.0);
 	//params_side_by_side_central_.reserve(5);
@@ -1757,7 +1802,8 @@ ROS_INFO("AkpLocalPlanner::scan2points:transform: frame_map_: %s-->frame_robot_f
 
 	// limit final speed robot.
 	ros_max_real_speed_out_=config.max_real_speed_out;
-	//fuera_bolitas_goals_companion_markers_=config.fuera_bolitas_goals_companion_markers_conf;
+	ros_max_real_angular_speed_out_=config.max_real_angular_speed_out;
+	fuera_bolitas_goals_companion_markers_=config.fuera_bolitas_goals_companion_markers_conf;
 
   // INI select closes person as companion (CASE 1 People)
  	if(config.select_near_pers_as_companion_one_person){