diff --git a/local_lib_people_prediction/.cproject b/local_lib_people_prediction/.cproject
new file mode 100644
index 0000000000000000000000000000000000000000..8974444231935bb5e277c81a741fd5021814dbd0
--- /dev/null
+++ b/local_lib_people_prediction/.cproject
@@ -0,0 +1,137 @@
+<?xml version="1.0" encoding="UTF-8" standalone="no"?>
+<?fileVersion 4.0.0?><cproject storage_type_id="org.eclipse.cdt.core.XmlProjectDescriptionStorage">
+	<storageModule moduleId="org.eclipse.cdt.core.settings">
+		<cconfiguration id="cdt.managedbuild.config.gnu.exe.debug.889107241">
+			<storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="cdt.managedbuild.config.gnu.exe.debug.889107241" moduleId="org.eclipse.cdt.core.settings" name="Debug">
+				<externalSettings/>
+				<extensions>
+					<extension id="org.eclipse.cdt.core.GmakeErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
+					<extension id="org.eclipse.cdt.core.CWDLocator" point="org.eclipse.cdt.core.ErrorParser"/>
+					<extension id="org.eclipse.cdt.core.GCCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
+					<extension id="org.eclipse.cdt.core.GASErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
+					<extension id="org.eclipse.cdt.core.GLDErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
+					<extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
+				</extensions>
+			</storageModule>
+			<storageModule moduleId="cdtBuildSystem" version="4.0.0">
+				<configuration artifactName="${ProjName}" buildArtefactType="org.eclipse.cdt.build.core.buildArtefactType.exe" buildProperties="org.eclipse.cdt.build.core.buildType=org.eclipse.cdt.build.core.buildType.debug,org.eclipse.cdt.build.core.buildArtefactType=org.eclipse.cdt.build.core.buildArtefactType.exe" cleanCommand="rm -rf" description="" id="cdt.managedbuild.config.gnu.exe.debug.889107241" name="Debug" parent="cdt.managedbuild.config.gnu.exe.debug">
+					<folderInfo id="cdt.managedbuild.config.gnu.exe.debug.889107241." name="/" resourcePath="">
+						<toolChain id="cdt.managedbuild.toolchain.gnu.exe.debug.1183591273" name="Linux GCC" superClass="cdt.managedbuild.toolchain.gnu.exe.debug">
+							<targetPlatform id="cdt.managedbuild.target.gnu.platform.exe.debug.1546846435" name="Debug Platform" superClass="cdt.managedbuild.target.gnu.platform.exe.debug"/>
+							<builder buildPath="${workspace_loc:/ASSAOP_local_lib}/Debug" id="cdt.managedbuild.target.gnu.builder.exe.debug.1120136049" managedBuildOn="true" name="Gnu Make Builder.Debug" superClass="cdt.managedbuild.target.gnu.builder.exe.debug"/>
+							<tool id="cdt.managedbuild.tool.gnu.archiver.base.1684962815" name="GCC Archiver" superClass="cdt.managedbuild.tool.gnu.archiver.base"/>
+							<tool id="cdt.managedbuild.tool.gnu.cpp.compiler.exe.debug.388215046" name="GCC C++ Compiler" superClass="cdt.managedbuild.tool.gnu.cpp.compiler.exe.debug">
+								<option id="gnu.cpp.compiler.exe.debug.option.optimization.level.1289022139" superClass="gnu.cpp.compiler.exe.debug.option.optimization.level" value="gnu.cpp.compiler.optimization.level.none" valueType="enumerated"/>
+								<option id="gnu.cpp.compiler.exe.debug.option.debugging.level.2040451658" superClass="gnu.cpp.compiler.exe.debug.option.debugging.level" value="gnu.cpp.compiler.debugging.level.max" valueType="enumerated"/>
+								<inputType id="cdt.managedbuild.tool.gnu.cpp.compiler.input.1447303397" superClass="cdt.managedbuild.tool.gnu.cpp.compiler.input"/>
+							</tool>
+							<tool id="cdt.managedbuild.tool.gnu.c.compiler.exe.debug.1415958909" name="GCC C Compiler" superClass="cdt.managedbuild.tool.gnu.c.compiler.exe.debug">
+								<option defaultValue="gnu.c.optimization.level.none" id="gnu.c.compiler.exe.debug.option.optimization.level.226115432" superClass="gnu.c.compiler.exe.debug.option.optimization.level" valueType="enumerated"/>
+								<option id="gnu.c.compiler.exe.debug.option.debugging.level.1777081777" superClass="gnu.c.compiler.exe.debug.option.debugging.level" value="gnu.c.debugging.level.max" valueType="enumerated"/>
+								<inputType id="cdt.managedbuild.tool.gnu.c.compiler.input.249879200" superClass="cdt.managedbuild.tool.gnu.c.compiler.input"/>
+							</tool>
+							<tool id="cdt.managedbuild.tool.gnu.c.linker.exe.debug.1686759107" name="GCC C Linker" superClass="cdt.managedbuild.tool.gnu.c.linker.exe.debug"/>
+							<tool id="cdt.managedbuild.tool.gnu.cpp.linker.exe.debug.1931907191" name="GCC C++ Linker" superClass="cdt.managedbuild.tool.gnu.cpp.linker.exe.debug">
+								<inputType id="cdt.managedbuild.tool.gnu.cpp.linker.input.14678906" superClass="cdt.managedbuild.tool.gnu.cpp.linker.input">
+									<additionalInput kind="additionalinputdependency" paths="$(USER_OBJS)"/>
+									<additionalInput kind="additionalinput" paths="$(LIBS)"/>
+								</inputType>
+							</tool>
+							<tool id="cdt.managedbuild.tool.gnu.assembler.exe.debug.2080890202" name="GCC Assembler" superClass="cdt.managedbuild.tool.gnu.assembler.exe.debug">
+								<inputType id="cdt.managedbuild.tool.gnu.assembler.input.2032072877" superClass="cdt.managedbuild.tool.gnu.assembler.input"/>
+							</tool>
+						</toolChain>
+					</folderInfo>
+				</configuration>
+			</storageModule>
+			<storageModule moduleId="org.eclipse.cdt.core.externalSettings"/>
+		</cconfiguration>
+		<cconfiguration id="cdt.managedbuild.config.gnu.exe.release.1684101099">
+			<storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="cdt.managedbuild.config.gnu.exe.release.1684101099" moduleId="org.eclipse.cdt.core.settings" name="Release">
+				<externalSettings/>
+				<extensions>
+					<extension id="org.eclipse.cdt.core.GmakeErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
+					<extension id="org.eclipse.cdt.core.CWDLocator" point="org.eclipse.cdt.core.ErrorParser"/>
+					<extension id="org.eclipse.cdt.core.GCCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
+					<extension id="org.eclipse.cdt.core.GASErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
+					<extension id="org.eclipse.cdt.core.GLDErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
+					<extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
+				</extensions>
+			</storageModule>
+			<storageModule moduleId="cdtBuildSystem" version="4.0.0">
+				<configuration artifactName="${ProjName}" buildArtefactType="org.eclipse.cdt.build.core.buildArtefactType.exe" buildProperties="org.eclipse.cdt.build.core.buildType=org.eclipse.cdt.build.core.buildType.release,org.eclipse.cdt.build.core.buildArtefactType=org.eclipse.cdt.build.core.buildArtefactType.exe" cleanCommand="rm -rf" description="" id="cdt.managedbuild.config.gnu.exe.release.1684101099" name="Release" parent="cdt.managedbuild.config.gnu.exe.release">
+					<folderInfo id="cdt.managedbuild.config.gnu.exe.release.1684101099." name="/" resourcePath="">
+						<toolChain id="cdt.managedbuild.toolchain.gnu.exe.release.425094882" name="Linux GCC" superClass="cdt.managedbuild.toolchain.gnu.exe.release">
+							<targetPlatform id="cdt.managedbuild.target.gnu.platform.exe.release.98734625" name="Debug Platform" superClass="cdt.managedbuild.target.gnu.platform.exe.release"/>
+							<builder buildPath="${workspace_loc:/ASSAOP_local_lib}/Release" id="cdt.managedbuild.target.gnu.builder.exe.release.1155415598" managedBuildOn="true" name="Gnu Make Builder.Release" superClass="cdt.managedbuild.target.gnu.builder.exe.release"/>
+							<tool id="cdt.managedbuild.tool.gnu.archiver.base.660830361" name="GCC Archiver" superClass="cdt.managedbuild.tool.gnu.archiver.base"/>
+							<tool id="cdt.managedbuild.tool.gnu.cpp.compiler.exe.release.1849017997" name="GCC C++ Compiler" superClass="cdt.managedbuild.tool.gnu.cpp.compiler.exe.release">
+								<option id="gnu.cpp.compiler.exe.release.option.optimization.level.339633967" superClass="gnu.cpp.compiler.exe.release.option.optimization.level" value="gnu.cpp.compiler.optimization.level.most" valueType="enumerated"/>
+								<option id="gnu.cpp.compiler.exe.release.option.debugging.level.977537281" superClass="gnu.cpp.compiler.exe.release.option.debugging.level" value="gnu.cpp.compiler.debugging.level.none" valueType="enumerated"/>
+								<inputType id="cdt.managedbuild.tool.gnu.cpp.compiler.input.1896176939" superClass="cdt.managedbuild.tool.gnu.cpp.compiler.input"/>
+							</tool>
+							<tool id="cdt.managedbuild.tool.gnu.c.compiler.exe.release.959999503" name="GCC C Compiler" superClass="cdt.managedbuild.tool.gnu.c.compiler.exe.release">
+								<option defaultValue="gnu.c.optimization.level.most" id="gnu.c.compiler.exe.release.option.optimization.level.1494114723" superClass="gnu.c.compiler.exe.release.option.optimization.level" valueType="enumerated"/>
+								<option id="gnu.c.compiler.exe.release.option.debugging.level.294833480" superClass="gnu.c.compiler.exe.release.option.debugging.level" value="gnu.c.debugging.level.none" valueType="enumerated"/>
+								<inputType id="cdt.managedbuild.tool.gnu.c.compiler.input.1373801358" superClass="cdt.managedbuild.tool.gnu.c.compiler.input"/>
+							</tool>
+							<tool id="cdt.managedbuild.tool.gnu.c.linker.exe.release.2048064767" name="GCC C Linker" superClass="cdt.managedbuild.tool.gnu.c.linker.exe.release"/>
+							<tool id="cdt.managedbuild.tool.gnu.cpp.linker.exe.release.679922827" name="GCC C++ Linker" superClass="cdt.managedbuild.tool.gnu.cpp.linker.exe.release">
+								<inputType id="cdt.managedbuild.tool.gnu.cpp.linker.input.1941846859" superClass="cdt.managedbuild.tool.gnu.cpp.linker.input">
+									<additionalInput kind="additionalinputdependency" paths="$(USER_OBJS)"/>
+									<additionalInput kind="additionalinput" paths="$(LIBS)"/>
+								</inputType>
+							</tool>
+							<tool id="cdt.managedbuild.tool.gnu.assembler.exe.release.1568794405" name="GCC Assembler" superClass="cdt.managedbuild.tool.gnu.assembler.exe.release">
+								<inputType id="cdt.managedbuild.tool.gnu.assembler.input.2098164552" superClass="cdt.managedbuild.tool.gnu.assembler.input"/>
+							</tool>
+						</toolChain>
+					</folderInfo>
+				</configuration>
+			</storageModule>
+		</cconfiguration>
+	</storageModule>
+	<storageModule moduleId="cdtBuildSystem" version="4.0.0">
+		<project id="ASSAOP_local_lib.cdt.managedbuild.target.gnu.exe.936367760" name="Executable" projectType="cdt.managedbuild.target.gnu.exe"/>
+	</storageModule>
+	<storageModule moduleId="scannerConfiguration">
+		<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
+		<scannerConfigBuildInfo instanceId="cdt.managedbuild.config.gnu.cross.exe.debug.32037006;cdt.managedbuild.config.gnu.cross.exe.debug.32037006.;cdt.managedbuild.tool.gnu.cross.c.compiler.355107493;cdt.managedbuild.tool.gnu.c.compiler.input.415815349">
+			<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
+		</scannerConfigBuildInfo>
+		<scannerConfigBuildInfo instanceId="cdt.managedbuild.config.gnu.exe.release.1684101099;cdt.managedbuild.config.gnu.exe.release.1684101099.;cdt.managedbuild.tool.gnu.cpp.compiler.exe.release.1849017997;cdt.managedbuild.tool.gnu.cpp.compiler.input.1896176939">
+			<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
+		</scannerConfigBuildInfo>
+		<scannerConfigBuildInfo instanceId="cdt.managedbuild.config.gnu.exe.release.108563199;cdt.managedbuild.config.gnu.exe.release.108563199.;cdt.managedbuild.tool.gnu.c.compiler.exe.release.1211854380;cdt.managedbuild.tool.gnu.c.compiler.input.683565502">
+			<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
+		</scannerConfigBuildInfo>
+		<scannerConfigBuildInfo instanceId="cdt.managedbuild.config.gnu.exe.debug.1830124302;cdt.managedbuild.config.gnu.exe.debug.1830124302.;cdt.managedbuild.tool.gnu.c.compiler.exe.debug.129448340;cdt.managedbuild.tool.gnu.c.compiler.input.1794837280">
+			<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
+		</scannerConfigBuildInfo>
+		<scannerConfigBuildInfo instanceId="cdt.managedbuild.config.gnu.cross.exe.release.152074145;cdt.managedbuild.config.gnu.cross.exe.release.152074145.;cdt.managedbuild.tool.gnu.cross.c.compiler.1546578144;cdt.managedbuild.tool.gnu.c.compiler.input.567643681">
+			<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
+		</scannerConfigBuildInfo>
+		<scannerConfigBuildInfo instanceId="cdt.managedbuild.config.gnu.exe.debug.889107241;cdt.managedbuild.config.gnu.exe.debug.889107241.;cdt.managedbuild.tool.gnu.c.compiler.exe.debug.1415958909;cdt.managedbuild.tool.gnu.c.compiler.input.249879200">
+			<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
+		</scannerConfigBuildInfo>
+		<scannerConfigBuildInfo instanceId="cdt.managedbuild.config.gnu.exe.release.108563199;cdt.managedbuild.config.gnu.exe.release.108563199.;cdt.managedbuild.tool.gnu.cpp.compiler.exe.release.813424575;cdt.managedbuild.tool.gnu.cpp.compiler.input.537017731">
+			<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
+		</scannerConfigBuildInfo>
+		<scannerConfigBuildInfo instanceId="cdt.managedbuild.config.gnu.exe.debug.889107241;cdt.managedbuild.config.gnu.exe.debug.889107241.;cdt.managedbuild.tool.gnu.cpp.compiler.exe.debug.388215046;cdt.managedbuild.tool.gnu.cpp.compiler.input.1447303397">
+			<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
+		</scannerConfigBuildInfo>
+		<scannerConfigBuildInfo instanceId="cdt.managedbuild.config.gnu.cross.exe.debug.32037006;cdt.managedbuild.config.gnu.cross.exe.debug.32037006.;cdt.managedbuild.tool.gnu.cross.cpp.compiler.1731729740;cdt.managedbuild.tool.gnu.cpp.compiler.input.1024819215">
+			<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
+		</scannerConfigBuildInfo>
+		<scannerConfigBuildInfo instanceId="cdt.managedbuild.config.gnu.cross.exe.release.152074145;cdt.managedbuild.config.gnu.cross.exe.release.152074145.;cdt.managedbuild.tool.gnu.cross.cpp.compiler.1498925651;cdt.managedbuild.tool.gnu.cpp.compiler.input.1871979781">
+			<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
+		</scannerConfigBuildInfo>
+		<scannerConfigBuildInfo instanceId="cdt.managedbuild.config.gnu.exe.release.1684101099;cdt.managedbuild.config.gnu.exe.release.1684101099.;cdt.managedbuild.tool.gnu.c.compiler.exe.release.959999503;cdt.managedbuild.tool.gnu.c.compiler.input.1373801358">
+			<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
+		</scannerConfigBuildInfo>
+		<scannerConfigBuildInfo instanceId="cdt.managedbuild.config.gnu.exe.debug.1830124302;cdt.managedbuild.config.gnu.exe.debug.1830124302.;cdt.managedbuild.tool.gnu.cpp.compiler.exe.debug.1723483156;cdt.managedbuild.tool.gnu.cpp.compiler.input.881860197">
+			<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
+		</scannerConfigBuildInfo>
+	</storageModule>
+	<storageModule moduleId="org.eclipse.cdt.core.LanguageSettingsProviders"/>
+</cproject>
diff --git a/local_lib_people_prediction/.project b/local_lib_people_prediction/.project
new file mode 100644
index 0000000000000000000000000000000000000000..99eb9cf5a40dcbdfb283731a90c7d07fa1289dea
--- /dev/null
+++ b/local_lib_people_prediction/.project
@@ -0,0 +1,27 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<projectDescription>
+	<name>ATR_7_junio</name>
+	<comment></comment>
+	<projects>
+	</projects>
+	<buildSpec>
+		<buildCommand>
+			<name>org.eclipse.cdt.managedbuilder.core.genmakebuilder</name>
+			<triggers>clean,full,incremental,</triggers>
+			<arguments>
+			</arguments>
+		</buildCommand>
+		<buildCommand>
+			<name>org.eclipse.cdt.managedbuilder.core.ScannerConfigBuilder</name>
+			<triggers>full,incremental,</triggers>
+			<arguments>
+			</arguments>
+		</buildCommand>
+	</buildSpec>
+	<natures>
+		<nature>org.eclipse.cdt.core.cnature</nature>
+		<nature>org.eclipse.cdt.core.ccnature</nature>
+		<nature>org.eclipse.cdt.managedbuilder.core.managedBuildNature</nature>
+		<nature>org.eclipse.cdt.managedbuilder.core.ScannerConfigNature</nature>
+	</natures>
+</projectDescription>
diff --git a/local_lib_people_prediction/.settings/language.settings.xml b/local_lib_people_prediction/.settings/language.settings.xml
new file mode 100644
index 0000000000000000000000000000000000000000..e8be6d5a410d7ce0703579a89bffc17f9488708b
--- /dev/null
+++ b/local_lib_people_prediction/.settings/language.settings.xml
@@ -0,0 +1,25 @@
+<?xml version="1.0" encoding="UTF-8" standalone="no"?>
+<project>
+	<configuration id="cdt.managedbuild.config.gnu.cross.exe.debug.32037006" name="Debug">
+		<extension point="org.eclipse.cdt.core.LanguageSettingsProvider">
+			<provider copy-of="extension" id="org.eclipse.cdt.ui.UserLanguageSettingsProvider"/>
+			<provider-reference id="org.eclipse.cdt.core.ReferencedProjectsLanguageSettingsProvider" ref="shared-provider"/>
+			<provider-reference id="org.eclipse.cdt.managedbuilder.core.MBSLanguageSettingsProvider" ref="shared-provider"/>
+			<provider class="org.eclipse.cdt.internal.build.crossgcc.CrossGCCBuiltinSpecsDetector" console="false" env-hash="-913847163843620227" id="org.eclipse.cdt.build.crossgcc.CrossGCCBuiltinSpecsDetector" keep-relative-paths="false" name="CDT Cross GCC Built-in Compiler Settings" parameter="${COMMAND} ${FLAGS} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true">
+				<language-scope id="org.eclipse.cdt.core.gcc"/>
+				<language-scope id="org.eclipse.cdt.core.g++"/>
+			</provider>
+		</extension>
+	</configuration>
+	<configuration id="cdt.managedbuild.config.gnu.cross.exe.release.152074145" name="Release">
+		<extension point="org.eclipse.cdt.core.LanguageSettingsProvider">
+			<provider copy-of="extension" id="org.eclipse.cdt.ui.UserLanguageSettingsProvider"/>
+			<provider-reference id="org.eclipse.cdt.core.ReferencedProjectsLanguageSettingsProvider" ref="shared-provider"/>
+			<provider-reference id="org.eclipse.cdt.managedbuilder.core.MBSLanguageSettingsProvider" ref="shared-provider"/>
+			<provider class="org.eclipse.cdt.internal.build.crossgcc.CrossGCCBuiltinSpecsDetector" console="false" env-hash="-913847163843620227" id="org.eclipse.cdt.build.crossgcc.CrossGCCBuiltinSpecsDetector" keep-relative-paths="false" name="CDT Cross GCC Built-in Compiler Settings" parameter="${COMMAND} ${FLAGS} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true">
+				<language-scope id="org.eclipse.cdt.core.gcc"/>
+				<language-scope id="org.eclipse.cdt.core.g++"/>
+			</provider>
+		</extension>
+	</configuration>
+</project>
diff --git a/local_lib_people_prediction/CMakeLists.txt b/local_lib_people_prediction/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..56e50a75feeef30485391cddffc23252e534f2c1
--- /dev/null
+++ b/local_lib_people_prediction/CMakeLists.txt
@@ -0,0 +1,72 @@
+# Pre-requisites about cmake itself
+CMAKE_MINIMUM_REQUIRED(VERSION 2.4)
+
+
+
+if(COMMAND cmake_policy)
+  cmake_policy(SET CMP0005 NEW) 
+  cmake_policy(SET CMP0003 NEW)
+   
+endif(COMMAND cmake_policy)
+
+# The project name and the type of project
+PROJECT(people_prediction)
+
+SET(EXECUTABLE_OUTPUT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/bin)
+SET(LIBRARY_OUTPUT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/lib)
+SET(CMAKE_INSTALL_PREFIX /usr/local)
+
+IF (NOT CMAKE_BUILD_TYPE)
+ SET(CMAKE_BUILD_TYPE "DEBUG") 
+ENDIF (NOT CMAKE_BUILD_TYPE)
+
+SET(CMAKE_CXX_FLAGS_DEBUG "-g -Wall -D_REENTRANT")
+SET(CMAKE_CXX_FLAGS_RELEASE "-O3 -D_REENTRANT")
+SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
+
+
+ADD_SUBDIRECTORY(src)
+
+FIND_PACKAGE(Doxygen)
+
+FIND_PATH(IRI_DOC_DIR doxygen.conf ${CMAKE_SOURCE_DIR}/doc/iri_doc/)
+IF (IRI_DOC_DIR)
+  ADD_CUSTOM_TARGET (doc ${DOXYGEN_EXECUTABLE} ${CMAKE_SOURCE_DIR}/doc/iri_doc/doxygen.conf)
+ELSE (IRI_DOC_DIR)
+  ADD_CUSTOM_TARGET (doc ${DOXYGEN_EXECUTABLE} ${CMAKE_SOURCE_DIR}/doc/doxygen.conf)
+ENDIF (IRI_DOC_DIR)
+
+ADD_CUSTOM_TARGET (distclean @echo cleaning cmake files)
+
+IF (UNIX)
+  ADD_CUSTOM_COMMAND(
+    COMMENT "distribution clean"
+    COMMAND make ARGS clean
+    COMMAND rm ARGS -rf ${CMAKE_SOURCE_DIR}/build/*
+
+    TARGET  distclean
+  )
+ELSE(UNIX)
+  ADD_CUSTOM_COMMAND(
+    COMMENT "distclean only implemented in unix"
+    TARGET  distclean
+  )
+ENDIF(UNIX)
+
+ADD_CUSTOM_TARGET (uninstall @echo uninstall package)
+
+IF (UNIX)
+  ADD_CUSTOM_COMMAND(
+    COMMENT "uninstall package"
+    COMMAND xargs ARGS rm < install_manifest.txt
+
+    TARGET  uninstall
+  )
+ELSE(UNIX)
+  ADD_CUSTOM_COMMAND(
+    COMMENT "uninstall only implemented in unix"
+    TARGET  uninstall
+  )
+ENDIF(UNIX)
+
+
diff --git a/local_lib_people_prediction/Debug/build/CMakeFiles/2.8.12.2/CompilerIdC/subdir.mk b/local_lib_people_prediction/Debug/build/CMakeFiles/2.8.12.2/CompilerIdC/subdir.mk
new file mode 100644
index 0000000000000000000000000000000000000000..bfc17dec812a5b37b7f7ad192f5d416df9e30bd7
--- /dev/null
+++ b/local_lib_people_prediction/Debug/build/CMakeFiles/2.8.12.2/CompilerIdC/subdir.mk
@@ -0,0 +1,24 @@
+################################################################################
+# Automatically-generated file. Do not edit!
+################################################################################
+
+# Add inputs and outputs from these tool invocations to the build variables 
+C_SRCS += \
+../build/CMakeFiles/2.8.12.2/CompilerIdC/CMakeCCompilerId.c 
+
+OBJS += \
+./build/CMakeFiles/2.8.12.2/CompilerIdC/CMakeCCompilerId.o 
+
+C_DEPS += \
+./build/CMakeFiles/2.8.12.2/CompilerIdC/CMakeCCompilerId.d 
+
+
+# Each subdirectory must supply rules for building sources it contributes
+build/CMakeFiles/2.8.12.2/CompilerIdC/%.o: ../build/CMakeFiles/2.8.12.2/CompilerIdC/%.c
+	@echo 'Building file: $<'
+	@echo 'Invoking: GCC C Compiler'
+	gcc -O0 -g3 -Wall -c -fmessage-length=0 -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
+	@echo 'Finished building: $<'
+	@echo ' '
+
+
diff --git a/local_lib_people_prediction/Debug/build/CMakeFiles/2.8.12.2/CompilerIdCXX/subdir.mk b/local_lib_people_prediction/Debug/build/CMakeFiles/2.8.12.2/CompilerIdCXX/subdir.mk
new file mode 100644
index 0000000000000000000000000000000000000000..9c6e51cbcd474a35ed39759cb8b42e0c6062a415
--- /dev/null
+++ b/local_lib_people_prediction/Debug/build/CMakeFiles/2.8.12.2/CompilerIdCXX/subdir.mk
@@ -0,0 +1,24 @@
+################################################################################
+# Automatically-generated file. Do not edit!
+################################################################################
+
+# Add inputs and outputs from these tool invocations to the build variables 
+CPP_SRCS += \
+../build/CMakeFiles/2.8.12.2/CompilerIdCXX/CMakeCXXCompilerId.cpp 
+
+OBJS += \
+./build/CMakeFiles/2.8.12.2/CompilerIdCXX/CMakeCXXCompilerId.o 
+
+CPP_DEPS += \
+./build/CMakeFiles/2.8.12.2/CompilerIdCXX/CMakeCXXCompilerId.d 
+
+
+# Each subdirectory must supply rules for building sources it contributes
+build/CMakeFiles/2.8.12.2/CompilerIdCXX/%.o: ../build/CMakeFiles/2.8.12.2/CompilerIdCXX/%.cpp
+	@echo 'Building file: $<'
+	@echo 'Invoking: GCC C++ Compiler'
+	g++ -O0 -g3 -Wall -c -fmessage-length=0 -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
+	@echo 'Finished building: $<'
+	@echo ' '
+
+
diff --git a/local_lib_people_prediction/Debug/build/src/CMakeFiles/people_prediction.dir/companion_zanlungo/subdir.mk b/local_lib_people_prediction/Debug/build/src/CMakeFiles/people_prediction.dir/companion_zanlungo/subdir.mk
new file mode 100644
index 0000000000000000000000000000000000000000..0146486689abbd6517d7bb59ea246cae95091e60
--- /dev/null
+++ b/local_lib_people_prediction/Debug/build/src/CMakeFiles/people_prediction.dir/companion_zanlungo/subdir.mk
@@ -0,0 +1,12 @@
+################################################################################
+# Automatically-generated file. Do not edit!
+################################################################################
+
+# Add inputs and outputs from these tool invocations to the build variables 
+O_SRCS += \
+../build/src/CMakeFiles/people_prediction.dir/companion_zanlungo/CompanionFrancescoModel.o \
+../build/src/CMakeFiles/people_prediction.dir/companion_zanlungo/Vector2D.o 
+
+
+# Each subdirectory must supply rules for building sources it contributes
+
diff --git a/local_lib_people_prediction/Debug/build/src/CMakeFiles/people_prediction.dir/learning_parameters/subdir.mk b/local_lib_people_prediction/Debug/build/src/CMakeFiles/people_prediction.dir/learning_parameters/subdir.mk
new file mode 100644
index 0000000000000000000000000000000000000000..13321098f05c0ebc315c84721840e89dc5011869
--- /dev/null
+++ b/local_lib_people_prediction/Debug/build/src/CMakeFiles/people_prediction.dir/learning_parameters/subdir.mk
@@ -0,0 +1,11 @@
+################################################################################
+# Automatically-generated file. Do not edit!
+################################################################################
+
+# Add inputs and outputs from these tool invocations to the build variables 
+O_SRCS += \
+../build/src/CMakeFiles/people_prediction.dir/learning_parameters/learnninglinearregression.o 
+
+
+# Each subdirectory must supply rules for building sources it contributes
+
diff --git a/local_lib_people_prediction/Debug/build/src/CMakeFiles/people_prediction.dir/nav/subdir.mk b/local_lib_people_prediction/Debug/build/src/CMakeFiles/people_prediction.dir/nav/subdir.mk
new file mode 100644
index 0000000000000000000000000000000000000000..88e3172aa9e5e91741c83941af8e0d961c8565f2
--- /dev/null
+++ b/local_lib_people_prediction/Debug/build/src/CMakeFiles/people_prediction.dir/nav/subdir.mk
@@ -0,0 +1,13 @@
+################################################################################
+# Automatically-generated file. Do not edit!
+################################################################################
+
+# Add inputs and outputs from these tool invocations to the build variables 
+O_SRCS += \
+../build/src/CMakeFiles/people_prediction.dir/nav/force_reactive_robot_companion.o \
+../build/src/CMakeFiles/people_prediction.dir/nav/plan_local_nav.o \
+../build/src/CMakeFiles/people_prediction.dir/nav/plan_local_nav_person_companion.o 
+
+
+# Each subdirectory must supply rules for building sources it contributes
+
diff --git a/local_lib_people_prediction/Debug/build/src/CMakeFiles/people_prediction.dir/random/subdir.mk b/local_lib_people_prediction/Debug/build/src/CMakeFiles/people_prediction.dir/random/subdir.mk
new file mode 100644
index 0000000000000000000000000000000000000000..9e5b3e8e50ed9890d93f2d08a5804c81853404c0
--- /dev/null
+++ b/local_lib_people_prediction/Debug/build/src/CMakeFiles/people_prediction.dir/random/subdir.mk
@@ -0,0 +1,11 @@
+################################################################################
+# Automatically-generated file. Do not edit!
+################################################################################
+
+# Add inputs and outputs from these tool invocations to the build variables 
+O_SRCS += \
+../build/src/CMakeFiles/people_prediction.dir/random/rand_gmm.o 
+
+
+# Each subdirectory must supply rules for building sources it contributes
+
diff --git a/local_lib_people_prediction/Debug/build/src/CMakeFiles/people_prediction.dir/scene_elements/subdir.mk b/local_lib_people_prediction/Debug/build/src/CMakeFiles/people_prediction.dir/scene_elements/subdir.mk
new file mode 100644
index 0000000000000000000000000000000000000000..8bdb9ca0b0810fb71d299662373cb929ef6f58f5
--- /dev/null
+++ b/local_lib_people_prediction/Debug/build/src/CMakeFiles/people_prediction.dir/scene_elements/subdir.mk
@@ -0,0 +1,15 @@
+################################################################################
+# Automatically-generated file. Do not edit!
+################################################################################
+
+# Add inputs and outputs from these tool invocations to the build variables 
+O_SRCS += \
+../build/src/CMakeFiles/people_prediction.dir/scene_elements/person_abstract.o \
+../build/src/CMakeFiles/people_prediction.dir/scene_elements/person_behavior.o \
+../build/src/CMakeFiles/people_prediction.dir/scene_elements/person_bhmip.o \
+../build/src/CMakeFiles/people_prediction.dir/scene_elements/person_virtual.o \
+../build/src/CMakeFiles/people_prediction.dir/scene_elements/robot.o 
+
+
+# Each subdirectory must supply rules for building sources it contributes
+
diff --git a/local_lib_people_prediction/Debug/build/src/CMakeFiles/people_prediction.dir/subdir.mk b/local_lib_people_prediction/Debug/build/src/CMakeFiles/people_prediction.dir/subdir.mk
new file mode 100644
index 0000000000000000000000000000000000000000..8ed5b5ce046355d0604f05b2dddb07085dd0ba05
--- /dev/null
+++ b/local_lib_people_prediction/Debug/build/src/CMakeFiles/people_prediction.dir/subdir.mk
@@ -0,0 +1,15 @@
+################################################################################
+# Automatically-generated file. Do not edit!
+################################################################################
+
+# Add inputs and outputs from these tool invocations to the build variables 
+O_SRCS += \
+../build/src/CMakeFiles/people_prediction.dir/iri_geometry.o \
+../build/src/CMakeFiles/people_prediction.dir/prediction_behavior.o \
+../build/src/CMakeFiles/people_prediction.dir/prediction_bhmip.o \
+../build/src/CMakeFiles/people_prediction.dir/scene_abstract.o \
+../build/src/CMakeFiles/people_prediction.dir/scene_sim.o 
+
+
+# Each subdirectory must supply rules for building sources it contributes
+
diff --git a/local_lib_people_prediction/Debug/build/src/examples/CMakeFiles/local_nav_example.dir/subdir.mk b/local_lib_people_prediction/Debug/build/src/examples/CMakeFiles/local_nav_example.dir/subdir.mk
new file mode 100644
index 0000000000000000000000000000000000000000..fa95c4a11df86c0cfc00e1b26d7117b3a79f602b
--- /dev/null
+++ b/local_lib_people_prediction/Debug/build/src/examples/CMakeFiles/local_nav_example.dir/subdir.mk
@@ -0,0 +1,11 @@
+################################################################################
+# Automatically-generated file. Do not edit!
+################################################################################
+
+# Add inputs and outputs from these tool invocations to the build variables 
+O_SRCS += \
+../build/src/examples/CMakeFiles/local_nav_example.dir/local_nav_example.o 
+
+
+# Each subdirectory must supply rules for building sources it contributes
+
diff --git a/local_lib_people_prediction/Debug/build/src/examples/CMakeFiles/prediction_example.dir/subdir.mk b/local_lib_people_prediction/Debug/build/src/examples/CMakeFiles/prediction_example.dir/subdir.mk
new file mode 100644
index 0000000000000000000000000000000000000000..2adee8c8341b02dd39dae7663a6b01a62ac3cadd
--- /dev/null
+++ b/local_lib_people_prediction/Debug/build/src/examples/CMakeFiles/prediction_example.dir/subdir.mk
@@ -0,0 +1,11 @@
+################################################################################
+# Automatically-generated file. Do not edit!
+################################################################################
+
+# Add inputs and outputs from these tool invocations to the build variables 
+O_SRCS += \
+../build/src/examples/CMakeFiles/prediction_example.dir/prediction_example.o 
+
+
+# Each subdirectory must supply rules for building sources it contributes
+
diff --git a/local_lib_people_prediction/Debug/makefile b/local_lib_people_prediction/Debug/makefile
new file mode 100644
index 0000000000000000000000000000000000000000..5458072fbd7e6e4a2a169acb6a92034f39c33126
--- /dev/null
+++ b/local_lib_people_prediction/Debug/makefile
@@ -0,0 +1,73 @@
+################################################################################
+# Automatically-generated file. Do not edit!
+################################################################################
+
+-include ../makefile.init
+
+RM := rm -rf
+
+# All of the sources participating in the build are defined here
+-include sources.mk
+-include src/test/subdir.mk
+-include src/scene_elements/subdir.mk
+-include src/random/subdir.mk
+-include src/nav/subdir.mk
+-include src/learning_parameters/subdir.mk
+-include src/examples/subdir.mk
+-include src/subdir.mk
+-include build/src/examples/CMakeFiles/prediction_example.dir/subdir.mk
+-include build/src/CMakeFiles/people_prediction.dir/scene_elements/subdir.mk
+-include build/src/CMakeFiles/people_prediction.dir/random/subdir.mk
+-include build/src/CMakeFiles/people_prediction.dir/nav/subdir.mk
+-include build/src/CMakeFiles/people_prediction.dir/learning_parameters/subdir.mk
+-include build/src/CMakeFiles/people_prediction.dir/subdir.mk
+-include build/CMakeFiles/2.8.12.2/CompilerIdCXX/subdir.mk
+-include build/CMakeFiles/2.8.12.2/CompilerIdC/subdir.mk
+-include subdir.mk
+-include objects.mk
+
+ifneq ($(MAKECMDGOALS),clean)
+ifneq ($(strip $(C++_DEPS)),)
+-include $(C++_DEPS)
+endif
+ifneq ($(strip $(C_DEPS)),)
+-include $(C_DEPS)
+endif
+ifneq ($(strip $(CC_DEPS)),)
+-include $(CC_DEPS)
+endif
+ifneq ($(strip $(CPP_DEPS)),)
+-include $(CPP_DEPS)
+endif
+ifneq ($(strip $(CXX_DEPS)),)
+-include $(CXX_DEPS)
+endif
+ifneq ($(strip $(C_UPPER_DEPS)),)
+-include $(C_UPPER_DEPS)
+endif
+endif
+
+-include ../makefile.defs
+
+# Add inputs and outputs from these tool invocations to the build variables 
+
+# All Target
+all: ASSAOP_local_lib
+
+# Tool invocations
+ASSAOP_local_lib: $(OBJS) $(USER_OBJS)
+	@echo 'Building target: $@'
+	@echo 'Invoking: GCC C++ Linker'
+	g++  -o "ASSAOP_local_lib" $(OBJS) $(USER_OBJS) $(LIBS)
+	@echo 'Finished building target: $@'
+	@echo ' '
+
+# Other Targets
+clean:
+	-$(RM) $(OBJS)$(C++_DEPS)$(C_DEPS)$(CC_DEPS)$(CPP_DEPS)$(EXECUTABLES)$(CXX_DEPS)$(C_UPPER_DEPS) ASSAOP_local_lib
+	-@echo ' '
+
+.PHONY: all clean dependents
+.SECONDARY:
+
+-include ../makefile.targets
diff --git a/local_lib_people_prediction/Debug/objects.mk b/local_lib_people_prediction/Debug/objects.mk
new file mode 100644
index 0000000000000000000000000000000000000000..742c2da043f4f5d93c414dca3a725ab2204d9817
--- /dev/null
+++ b/local_lib_people_prediction/Debug/objects.mk
@@ -0,0 +1,8 @@
+################################################################################
+# Automatically-generated file. Do not edit!
+################################################################################
+
+USER_OBJS :=
+
+LIBS :=
+
diff --git a/local_lib_people_prediction/Debug/sources.mk b/local_lib_people_prediction/Debug/sources.mk
new file mode 100644
index 0000000000000000000000000000000000000000..74df3bc48a3a8620509a2ae8111592b4ba22af6c
--- /dev/null
+++ b/local_lib_people_prediction/Debug/sources.mk
@@ -0,0 +1,41 @@
+################################################################################
+# Automatically-generated file. Do not edit!
+################################################################################
+
+O_SRCS := 
+CPP_SRCS := 
+C_UPPER_SRCS := 
+C_SRCS := 
+S_UPPER_SRCS := 
+OBJ_SRCS := 
+ASM_SRCS := 
+CXX_SRCS := 
+C++_SRCS := 
+CC_SRCS := 
+OBJS := 
+C++_DEPS := 
+C_DEPS := 
+CC_DEPS := 
+CPP_DEPS := 
+EXECUTABLES := 
+CXX_DEPS := 
+C_UPPER_DEPS := 
+
+# Every subdirectory with source files must be described here
+SUBDIRS := \
+src/test \
+src \
+src/scene_elements \
+src/random \
+src/nav \
+src/learning_parameters \
+src/examples \
+build/src/examples/CMakeFiles/prediction_example.dir \
+build/src/CMakeFiles/people_prediction.dir \
+build/src/CMakeFiles/people_prediction.dir/scene_elements \
+build/src/CMakeFiles/people_prediction.dir/random \
+build/src/CMakeFiles/people_prediction.dir/nav \
+build/src/CMakeFiles/people_prediction.dir/learning_parameters \
+build/CMakeFiles/2.8.12.2/CompilerIdCXX \
+build/CMakeFiles/2.8.12.2/CompilerIdC \
+
diff --git a/local_lib_people_prediction/Debug/src/companion_zanlungo/subdir.mk b/local_lib_people_prediction/Debug/src/companion_zanlungo/subdir.mk
new file mode 100644
index 0000000000000000000000000000000000000000..696680f50e1d8a15436844e7aaa2e20e06c58176
--- /dev/null
+++ b/local_lib_people_prediction/Debug/src/companion_zanlungo/subdir.mk
@@ -0,0 +1,27 @@
+################################################################################
+# Automatically-generated file. Do not edit!
+################################################################################
+
+# Add inputs and outputs from these tool invocations to the build variables 
+CPP_SRCS += \
+../src/companion_zanlungo/CompanionFrancescoModel.cpp \
+../src/companion_zanlungo/Vector2D.cpp 
+
+OBJS += \
+./src/companion_zanlungo/CompanionFrancescoModel.o \
+./src/companion_zanlungo/Vector2D.o 
+
+CPP_DEPS += \
+./src/companion_zanlungo/CompanionFrancescoModel.d \
+./src/companion_zanlungo/Vector2D.d 
+
+
+# Each subdirectory must supply rules for building sources it contributes
+src/companion_zanlungo/%.o: ../src/companion_zanlungo/%.cpp
+	@echo 'Building file: $<'
+	@echo 'Invoking: GCC C++ Compiler'
+	g++ -O0 -g3 -Wall -c -fmessage-length=0 -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
+	@echo 'Finished building: $<'
+	@echo ' '
+
+
diff --git a/local_lib_people_prediction/Debug/src/examples/subdir.mk b/local_lib_people_prediction/Debug/src/examples/subdir.mk
new file mode 100644
index 0000000000000000000000000000000000000000..d9c3712156a374976d2fa27e9b4d75c0aa3409e7
--- /dev/null
+++ b/local_lib_people_prediction/Debug/src/examples/subdir.mk
@@ -0,0 +1,36 @@
+################################################################################
+# Automatically-generated file. Do not edit!
+################################################################################
+
+# Add inputs and outputs from these tool invocations to the build variables 
+CPP_SRCS += \
+../src/examples/generate_performance_from_data_example.cpp \
+../src/examples/generate_performance_from_data_exampleIROS2019.cpp \
+../src/examples/generate_performance_from_data_example_humanoids_side_by_side_2people.cpp \
+../src/examples/local_nav_example.cpp \
+../src/examples/prediction_example.cpp 
+
+OBJS += \
+./src/examples/generate_performance_from_data_example.o \
+./src/examples/generate_performance_from_data_exampleIROS2019.o \
+./src/examples/generate_performance_from_data_example_humanoids_side_by_side_2people.o \
+./src/examples/local_nav_example.o \
+./src/examples/prediction_example.o 
+
+CPP_DEPS += \
+./src/examples/generate_performance_from_data_example.d \
+./src/examples/generate_performance_from_data_exampleIROS2019.d \
+./src/examples/generate_performance_from_data_example_humanoids_side_by_side_2people.d \
+./src/examples/local_nav_example.d \
+./src/examples/prediction_example.d 
+
+
+# Each subdirectory must supply rules for building sources it contributes
+src/examples/%.o: ../src/examples/%.cpp
+	@echo 'Building file: $<'
+	@echo 'Invoking: GCC C++ Compiler'
+	g++ -O0 -g3 -Wall -c -fmessage-length=0 -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
+	@echo 'Finished building: $<'
+	@echo ' '
+
+
diff --git a/local_lib_people_prediction/Debug/src/learning_parameters/subdir.mk b/local_lib_people_prediction/Debug/src/learning_parameters/subdir.mk
new file mode 100644
index 0000000000000000000000000000000000000000..7b970fd74fa820e412b0bf4a5006c7b31488360d
--- /dev/null
+++ b/local_lib_people_prediction/Debug/src/learning_parameters/subdir.mk
@@ -0,0 +1,27 @@
+################################################################################
+# Automatically-generated file. Do not edit!
+################################################################################
+
+# Add inputs and outputs from these tool invocations to the build variables 
+CPP_SRCS += \
+../src/learning_parameters/getFilesnames_script.cpp \
+../src/learning_parameters/learnninglinearregression.cpp 
+
+OBJS += \
+./src/learning_parameters/getFilesnames_script.o \
+./src/learning_parameters/learnninglinearregression.o 
+
+CPP_DEPS += \
+./src/learning_parameters/getFilesnames_script.d \
+./src/learning_parameters/learnninglinearregression.d 
+
+
+# Each subdirectory must supply rules for building sources it contributes
+src/learning_parameters/%.o: ../src/learning_parameters/%.cpp
+	@echo 'Building file: $<'
+	@echo 'Invoking: GCC C++ Compiler'
+	g++ -O0 -g3 -Wall -c -fmessage-length=0 -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
+	@echo 'Finished building: $<'
+	@echo ' '
+
+
diff --git a/local_lib_people_prediction/Debug/src/nav/subdir.mk b/local_lib_people_prediction/Debug/src/nav/subdir.mk
new file mode 100644
index 0000000000000000000000000000000000000000..b66f4c0998a873ff6419b2b4ff72644de75da70d
--- /dev/null
+++ b/local_lib_people_prediction/Debug/src/nav/subdir.mk
@@ -0,0 +1,33 @@
+################################################################################
+# Automatically-generated file. Do not edit!
+################################################################################
+
+# Add inputs and outputs from these tool invocations to the build variables 
+CPP_SRCS += \
+../src/nav/force_reactive_robot_companion.cpp \
+../src/nav/force_reactive_robot_companion_learning.cpp \
+../src/nav/plan_local_nav.cpp \
+../src/nav/plan_local_nav_person_companion.cpp 
+
+OBJS += \
+./src/nav/force_reactive_robot_companion.o \
+./src/nav/force_reactive_robot_companion_learning.o \
+./src/nav/plan_local_nav.o \
+./src/nav/plan_local_nav_person_companion.o 
+
+CPP_DEPS += \
+./src/nav/force_reactive_robot_companion.d \
+./src/nav/force_reactive_robot_companion_learning.d \
+./src/nav/plan_local_nav.d \
+./src/nav/plan_local_nav_person_companion.d 
+
+
+# Each subdirectory must supply rules for building sources it contributes
+src/nav/%.o: ../src/nav/%.cpp
+	@echo 'Building file: $<'
+	@echo 'Invoking: GCC C++ Compiler'
+	g++ -O0 -g3 -Wall -c -fmessage-length=0 -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
+	@echo 'Finished building: $<'
+	@echo ' '
+
+
diff --git a/local_lib_people_prediction/Debug/src/random/subdir.mk b/local_lib_people_prediction/Debug/src/random/subdir.mk
new file mode 100644
index 0000000000000000000000000000000000000000..aefb73c86ac823d721a3a44a2da77034792160c5
--- /dev/null
+++ b/local_lib_people_prediction/Debug/src/random/subdir.mk
@@ -0,0 +1,24 @@
+################################################################################
+# Automatically-generated file. Do not edit!
+################################################################################
+
+# Add inputs and outputs from these tool invocations to the build variables 
+CPP_SRCS += \
+../src/random/rand_gmm.cpp 
+
+OBJS += \
+./src/random/rand_gmm.o 
+
+CPP_DEPS += \
+./src/random/rand_gmm.d 
+
+
+# Each subdirectory must supply rules for building sources it contributes
+src/random/%.o: ../src/random/%.cpp
+	@echo 'Building file: $<'
+	@echo 'Invoking: GCC C++ Compiler'
+	g++ -O0 -g3 -Wall -c -fmessage-length=0 -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
+	@echo 'Finished building: $<'
+	@echo ' '
+
+
diff --git a/local_lib_people_prediction/Debug/src/scene_elements/subdir.mk b/local_lib_people_prediction/Debug/src/scene_elements/subdir.mk
new file mode 100644
index 0000000000000000000000000000000000000000..1b7624aefe65d209e0b5a75ea45efe395bea3a7d
--- /dev/null
+++ b/local_lib_people_prediction/Debug/src/scene_elements/subdir.mk
@@ -0,0 +1,39 @@
+################################################################################
+# Automatically-generated file. Do not edit!
+################################################################################
+
+# Add inputs and outputs from these tool invocations to the build variables 
+CPP_SRCS += \
+../src/scene_elements/obstacle.cpp \
+../src/scene_elements/person_abstract.cpp \
+../src/scene_elements/person_behavior.cpp \
+../src/scene_elements/person_bhmip.cpp \
+../src/scene_elements/person_virtual.cpp \
+../src/scene_elements/robot.cpp 
+
+OBJS += \
+./src/scene_elements/obstacle.o \
+./src/scene_elements/person_abstract.o \
+./src/scene_elements/person_behavior.o \
+./src/scene_elements/person_bhmip.o \
+./src/scene_elements/person_virtual.o \
+./src/scene_elements/robot.o 
+
+CPP_DEPS += \
+./src/scene_elements/obstacle.d \
+./src/scene_elements/person_abstract.d \
+./src/scene_elements/person_behavior.d \
+./src/scene_elements/person_bhmip.d \
+./src/scene_elements/person_virtual.d \
+./src/scene_elements/robot.d 
+
+
+# Each subdirectory must supply rules for building sources it contributes
+src/scene_elements/%.o: ../src/scene_elements/%.cpp
+	@echo 'Building file: $<'
+	@echo 'Invoking: GCC C++ Compiler'
+	g++ -O0 -g3 -Wall -c -fmessage-length=0 -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
+	@echo 'Finished building: $<'
+	@echo ' '
+
+
diff --git a/local_lib_people_prediction/Debug/src/subdir.mk b/local_lib_people_prediction/Debug/src/subdir.mk
new file mode 100644
index 0000000000000000000000000000000000000000..67b03d7c44ed179e352b9fb3769a6a0610ff340b
--- /dev/null
+++ b/local_lib_people_prediction/Debug/src/subdir.mk
@@ -0,0 +1,36 @@
+################################################################################
+# Automatically-generated file. Do not edit!
+################################################################################
+
+# Add inputs and outputs from these tool invocations to the build variables 
+CPP_SRCS += \
+../src/iri_geometry.cpp \
+../src/prediction_behavior.cpp \
+../src/prediction_bhmip.cpp \
+../src/scene_abstract.cpp \
+../src/scene_sim.cpp 
+
+OBJS += \
+./src/iri_geometry.o \
+./src/prediction_behavior.o \
+./src/prediction_bhmip.o \
+./src/scene_abstract.o \
+./src/scene_sim.o 
+
+CPP_DEPS += \
+./src/iri_geometry.d \
+./src/prediction_behavior.d \
+./src/prediction_bhmip.d \
+./src/scene_abstract.d \
+./src/scene_sim.d 
+
+
+# Each subdirectory must supply rules for building sources it contributes
+src/%.o: ../src/%.cpp
+	@echo 'Building file: $<'
+	@echo 'Invoking: GCC C++ Compiler'
+	g++ -O0 -g3 -Wall -c -fmessage-length=0 -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
+	@echo 'Finished building: $<'
+	@echo ' '
+
+
diff --git a/local_lib_people_prediction/Debug/src/test/subdir.mk b/local_lib_people_prediction/Debug/src/test/subdir.mk
new file mode 100644
index 0000000000000000000000000000000000000000..c7502db0b7cef3c68437e917e492368f89f58787
--- /dev/null
+++ b/local_lib_people_prediction/Debug/src/test/subdir.mk
@@ -0,0 +1,30 @@
+################################################################################
+# Automatically-generated file. Do not edit!
+################################################################################
+
+# Add inputs and outputs from these tool invocations to the build variables 
+CPP_SRCS += \
+../src/test/test_geometry.cpp \
+../src/test/test_person.cpp \
+../src/test/test_scene.cpp 
+
+OBJS += \
+./src/test/test_geometry.o \
+./src/test/test_person.o \
+./src/test/test_scene.o 
+
+CPP_DEPS += \
+./src/test/test_geometry.d \
+./src/test/test_person.d \
+./src/test/test_scene.d 
+
+
+# Each subdirectory must supply rules for building sources it contributes
+src/test/%.o: ../src/test/%.cpp
+	@echo 'Building file: $<'
+	@echo 'Invoking: GCC C++ Compiler'
+	g++ -O0 -g3 -Wall -c -fmessage-length=0 -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
+	@echo 'Finished building: $<'
+	@echo ' '
+
+
diff --git a/local_lib_people_prediction/Findpeople_prediction.cmake b/local_lib_people_prediction/Findpeople_prediction.cmake
new file mode 100644
index 0000000000000000000000000000000000000000..6248521c2dd8ee0d2172c2dfc25e411318e528a8
--- /dev/null
+++ b/local_lib_people_prediction/Findpeople_prediction.cmake
@@ -0,0 +1,23 @@
+#
+# CMake for Findpeople_prediction
+#
+FIND_PATH(people_prediction_INCLUDE_DIR iri_geometry.h /usr/include/iridrivers/people_prediction /usr/local/include/iridrivers/people_prediction)
+
+FIND_LIBRARY(people_prediction_LIBRARY
+    NAMES people_prediction
+    PATHS /usr/lib /usr/local/lib /usr/local/lib/iridrivers) 
+
+IF (people_prediction_INCLUDE_DIR AND people_prediction_LIBRARY)
+   SET(people_prediction_FOUND TRUE)
+ENDIF (people_prediction_INCLUDE_DIR AND people_prediction_LIBRARY)
+
+IF (people_prediction_FOUND)
+   IF (NOT people_prediction_FIND_QUIETLY)
+      MESSAGE(STATUS "Found people_prediction library: ${people_prediction_LIBRARY}")
+   ENDIF (NOT people_prediction_FIND_QUIETLY)
+ELSE (people_prediction_FOUND)
+   IF (people_prediction_FIND_REQUIRED)
+      MESSAGE(FATAL_ERROR "Could not find people_prediction library")
+   ENDIF (people_prediction_FIND_REQUIRED)
+ENDIF (people_prediction_FOUND)
+
diff --git a/local_lib_people_prediction/ReadMe.txt b/local_lib_people_prediction/ReadMe.txt
new file mode 100644
index 0000000000000000000000000000000000000000..5d1822150762f1f7f7817de0e42d5b6ce0e1fe7c
--- /dev/null
+++ b/local_lib_people_prediction/ReadMe.txt
@@ -0,0 +1,19 @@
+Copyright (C) 2009-2010 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+Author Gonzalo Ferrer (gferrer@iri.upc.edu)
+All rights reserved.
+
+This file is part of people prediction library library
+people prediction library library is free software: you can redistribute it and/or modify
+it under the terms of the GNU Lesser General Public License as published by
+the Free Software Foundation, either version 3 of the License, or
+at your option) any later version.
+
+This program is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+GNU Lesser General Public License for more details.
+
+You should have received a copy of the GNU Lesser General Public License
+along with this program.  If not, see <http://www.gnu.org/licenses/>
+
+
diff --git a/local_lib_people_prediction/bin/local_nav_example b/local_lib_people_prediction/bin/local_nav_example
new file mode 100755
index 0000000000000000000000000000000000000000..63290e32186066c1179a6d784e664e22b78226c6
Binary files /dev/null and b/local_lib_people_prediction/bin/local_nav_example differ
diff --git a/local_lib_people_prediction/bin/prediction_example b/local_lib_people_prediction/bin/prediction_example
new file mode 100755
index 0000000000000000000000000000000000000000..3e86ef4e23c680979c4899f9c081ce98ce28d8e1
Binary files /dev/null and b/local_lib_people_prediction/bin/prediction_example differ
diff --git a/local_lib_people_prediction/doc/doxygen.conf b/local_lib_people_prediction/doc/doxygen.conf
new file mode 100644
index 0000000000000000000000000000000000000000..347af2ef86bf4786d49b13d48c12146b0d001b01
--- /dev/null
+++ b/local_lib_people_prediction/doc/doxygen.conf
@@ -0,0 +1,251 @@
+# Doxyfile 1.5.5
+
+@INCLUDE_PATH = ../doc/
+@INCLUDE = doxygen_project_name.conf
+
+#---------------------------------------------------------------------------
+# Project related configuration options
+#---------------------------------------------------------------------------
+DOXYFILE_ENCODING      = UTF-8
+PROJECT_NUMBER         = 
+OUTPUT_DIRECTORY       = ../doc
+CREATE_SUBDIRS         = NO
+OUTPUT_LANGUAGE        = English
+BRIEF_MEMBER_DESC      = YES
+REPEAT_BRIEF           = NO
+ABBREVIATE_BRIEF       = 
+ALWAYS_DETAILED_SEC    = NO
+INLINE_INHERITED_MEMB  = NO
+FULL_PATH_NAMES        = YES
+STRIP_FROM_PATH        = 
+STRIP_FROM_INC_PATH    = 
+SHORT_NAMES            = NO
+JAVADOC_AUTOBRIEF      = NO
+QT_AUTOBRIEF           = NO
+MULTILINE_CPP_IS_BRIEF = NO
+DETAILS_AT_TOP         = NO
+INHERIT_DOCS           = YES
+SEPARATE_MEMBER_PAGES  = NO
+TAB_SIZE               = 8
+ALIASES                = 
+OPTIMIZE_OUTPUT_FOR_C  = YES
+OPTIMIZE_OUTPUT_JAVA   = NO
+OPTIMIZE_FOR_FORTRAN   = NO
+OPTIMIZE_OUTPUT_VHDL   = NO
+BUILTIN_STL_SUPPORT    = NO
+CPP_CLI_SUPPORT        = NO
+SIP_SUPPORT            = NO
+DISTRIBUTE_GROUP_DOC   = NO
+SUBGROUPING            = YES
+TYPEDEF_HIDES_STRUCT   = NO
+#---------------------------------------------------------------------------
+# Build related configuration options
+#---------------------------------------------------------------------------
+EXTRACT_ALL            = NO
+EXTRACT_PRIVATE        = NO
+EXTRACT_STATIC         = NO
+EXTRACT_LOCAL_CLASSES  = YES
+EXTRACT_LOCAL_METHODS  = NO
+EXTRACT_ANON_NSPACES   = NO
+HIDE_UNDOC_MEMBERS     = NO
+HIDE_UNDOC_CLASSES     = NO
+HIDE_FRIEND_COMPOUNDS  = NO
+HIDE_IN_BODY_DOCS      = NO
+INTERNAL_DOCS          = NO
+CASE_SENSE_NAMES       = YES
+HIDE_SCOPE_NAMES       = NO
+SHOW_INCLUDE_FILES     = YES
+INLINE_INFO            = YES
+SORT_MEMBER_DOCS       = NO
+SORT_BRIEF_DOCS        = NO
+SORT_GROUP_NAMES       = NO
+SORT_BY_SCOPE_NAME     = NO
+GENERATE_TODOLIST      = YES
+GENERATE_TESTLIST      = YES
+GENERATE_BUGLIST       = YES
+GENERATE_DEPRECATEDLIST= YES
+ENABLED_SECTIONS       = 
+MAX_INITIALIZER_LINES  = 30
+SHOW_USED_FILES        = YES
+SHOW_DIRECTORIES       = YES
+FILE_VERSION_FILTER    = 
+#---------------------------------------------------------------------------
+# configuration options related to warning and progress messages
+#---------------------------------------------------------------------------
+QUIET                  = YES
+WARNINGS               = YES
+WARN_IF_UNDOCUMENTED   = YES
+WARN_IF_DOC_ERROR      = YES
+WARN_NO_PARAMDOC       = NO
+WARN_FORMAT            = "$file:$line: $text"
+WARN_LOGFILE           = 
+#---------------------------------------------------------------------------
+# configuration options related to the input files
+#---------------------------------------------------------------------------
+INPUT                  = ../src \
+                         ../doc/main.dox
+INPUT_ENCODING         = UTF-8
+FILE_PATTERNS          = *.c \
+                         *.h \
+                         *.cpp
+RECURSIVE              = YES
+EXCLUDE                = 
+EXCLUDE_SYMLINKS       = NO
+EXCLUDE_PATTERNS       = *.tab.c \
+                         *.tab.h \
+                         lex* \
+                         *glr.h \
+                         *llr.h \
+                         *glr.c \
+                         *llr.c \
+                         *general.h
+EXCLUDE_SYMBOLS        = 
+EXAMPLE_PATH           = ../src/examples
+EXAMPLE_PATTERNS       = 
+EXAMPLE_RECURSIVE      = NO
+IMAGE_PATH             = ../doc/images
+INPUT_FILTER           = 
+FILTER_PATTERNS        = 
+FILTER_SOURCE_FILES    = NO
+#---------------------------------------------------------------------------
+# configuration options related to source browsing
+#---------------------------------------------------------------------------
+SOURCE_BROWSER         = YES
+INLINE_SOURCES         = NO
+STRIP_CODE_COMMENTS    = YES
+REFERENCED_BY_RELATION = YES
+REFERENCES_RELATION    = YES
+REFERENCES_LINK_SOURCE = YES
+USE_HTAGS              = NO
+VERBATIM_HEADERS       = YES
+#---------------------------------------------------------------------------
+# configuration options related to the alphabetical class index
+#---------------------------------------------------------------------------
+ALPHABETICAL_INDEX     = YES
+COLS_IN_ALPHA_INDEX    = 5
+IGNORE_PREFIX          = 
+#---------------------------------------------------------------------------
+# configuration options related to the HTML output
+#---------------------------------------------------------------------------
+GENERATE_HTML          = YES
+HTML_OUTPUT            = html
+HTML_FILE_EXTENSION    = .html
+HTML_HEADER            = 
+HTML_FOOTER            = 
+HTML_STYLESHEET        = 
+HTML_ALIGN_MEMBERS     = YES
+GENERATE_HTMLHELP      = NO
+GENERATE_DOCSET        = NO
+DOCSET_FEEDNAME        = "Doxygen generated docs"
+DOCSET_BUNDLE_ID       = org.doxygen.Project
+HTML_DYNAMIC_SECTIONS  = NO
+CHM_FILE               = 
+HHC_LOCATION           = 
+GENERATE_CHI           = NO
+BINARY_TOC             = NO
+TOC_EXPAND             = NO
+DISABLE_INDEX          = NO
+ENUM_VALUES_PER_LINE   = 4
+GENERATE_TREEVIEW      = NO
+TREEVIEW_WIDTH         = 250
+#---------------------------------------------------------------------------
+# configuration options related to the LaTeX output
+#---------------------------------------------------------------------------
+GENERATE_LATEX         = NO
+LATEX_OUTPUT           = latex
+LATEX_CMD_NAME         = latex
+MAKEINDEX_CMD_NAME     = makeindex
+COMPACT_LATEX          = NO
+PAPER_TYPE             = a4
+EXTRA_PACKAGES         = 
+LATEX_HEADER           = 
+PDF_HYPERLINKS         = YES
+USE_PDFLATEX           = NO
+LATEX_BATCHMODE        = NO
+LATEX_HIDE_INDICES     = NO
+#---------------------------------------------------------------------------
+# configuration options related to the RTF output
+#---------------------------------------------------------------------------
+GENERATE_RTF           = NO
+RTF_OUTPUT             = rtf
+COMPACT_RTF            = NO
+RTF_HYPERLINKS         = NO
+RTF_STYLESHEET_FILE    = 
+RTF_EXTENSIONS_FILE    = 
+#---------------------------------------------------------------------------
+# configuration options related to the man page output
+#---------------------------------------------------------------------------
+GENERATE_MAN           = NO
+MAN_OUTPUT             = man
+MAN_EXTENSION          = .3
+MAN_LINKS              = NO
+#---------------------------------------------------------------------------
+# configuration options related to the XML output
+#---------------------------------------------------------------------------
+GENERATE_XML           = NO
+XML_OUTPUT             = xml
+XML_SCHEMA             = 
+XML_DTD                = 
+XML_PROGRAMLISTING     = YES
+#---------------------------------------------------------------------------
+# configuration options for the AutoGen Definitions output
+#---------------------------------------------------------------------------
+GENERATE_AUTOGEN_DEF   = NO
+#---------------------------------------------------------------------------
+# configuration options related to the Perl module output
+#---------------------------------------------------------------------------
+GENERATE_PERLMOD       = NO
+PERLMOD_LATEX          = NO
+PERLMOD_PRETTY         = YES
+PERLMOD_MAKEVAR_PREFIX = 
+#---------------------------------------------------------------------------
+# Configuration options related to the preprocessor   
+#---------------------------------------------------------------------------
+ENABLE_PREPROCESSING   = YES
+MACRO_EXPANSION        = NO
+EXPAND_ONLY_PREDEF     = NO
+SEARCH_INCLUDES        = YES
+INCLUDE_PATH           = 
+INCLUDE_FILE_PATTERNS  = 
+PREDEFINED             = _USE_MPI=1
+EXPAND_AS_DEFINED      = 
+SKIP_FUNCTION_MACROS   = YES
+#---------------------------------------------------------------------------
+# Configuration::additions related to external references   
+#---------------------------------------------------------------------------
+TAGFILES               = 
+GENERATE_TAGFILE       = 
+ALLEXTERNALS           = NO
+EXTERNAL_GROUPS        = YES
+PERL_PATH              = /usr/bin/perl
+#---------------------------------------------------------------------------
+# Configuration options related to the dot tool   
+#---------------------------------------------------------------------------
+CLASS_DIAGRAMS         = YES
+MSCGEN_PATH            = 
+HIDE_UNDOC_RELATIONS   = YES
+HAVE_DOT               = YES
+CLASS_GRAPH            = YES
+COLLABORATION_GRAPH    = YES
+GROUP_GRAPHS           = YES
+UML_LOOK               = NO
+TEMPLATE_RELATIONS     = NO
+INCLUDE_GRAPH          = NO
+INCLUDED_BY_GRAPH      = NO
+CALL_GRAPH             = YES
+CALLER_GRAPH           = YES
+GRAPHICAL_HIERARCHY    = YES
+DIRECTORY_GRAPH        = NO
+DOT_IMAGE_FORMAT       = png
+DOT_PATH               = 
+DOTFILE_DIRS           = 
+DOT_GRAPH_MAX_NODES    = 50
+MAX_DOT_GRAPH_DEPTH    = 2
+DOT_TRANSPARENT        = YES
+DOT_MULTI_TARGETS      = NO
+GENERATE_LEGEND        = YES
+DOT_CLEANUP            = YES
+#---------------------------------------------------------------------------
+# Configuration::additions related to the search engine   
+#---------------------------------------------------------------------------
+SEARCHENGINE           = NO
diff --git a/local_lib_people_prediction/doc/doxygen_project_name.conf b/local_lib_people_prediction/doc/doxygen_project_name.conf
new file mode 100644
index 0000000000000000000000000000000000000000..fdfe2fb4d34e357bf5cfeb78bd3fc6860d82a488
--- /dev/null
+++ b/local_lib_people_prediction/doc/doxygen_project_name.conf
@@ -0,0 +1 @@
+PROJECT_NAME = "people_prediction"
diff --git a/local_lib_people_prediction/doc/main.dox b/local_lib_people_prediction/doc/main.dox
new file mode 100644
index 0000000000000000000000000000000000000000..160ab6d5f628da14c12692b78525143f2d06c79f
--- /dev/null
+++ b/local_lib_people_prediction/doc/main.dox
@@ -0,0 +1,107 @@
+/*! \mainpage people_prediction
+
+  \section Introduction 
+
+  \subsection Pre-Requisites
+
+  This package requires of the following libraries and packages
+	 - <A href="http://www.cmake.org">cmake</A>, a cross-platform build system.
+	 - <A href="http://www.doxygen.org">doxygen</a> and 
+	   <A href="http://www.graphviz.org">graphviz</a> to generate the documentation.
+         - stdc++, 
+	 .
+  
+  Under linux all of these utilities are available in ready-to-use packages.
+
+  Under MacOS most of the packages are available via <a href="http://www.finkproject.org/">fink</a>. <br>
+
+  \subsection Compilation
+
+  Just download this package, uncompress it, and execute
+   - cd build
+   - cmake ..
+   .
+  to generate the makefile and then
+   - make
+   .
+  to obtain the shared library (in this case called <em>iriutils.so</em>) and
+  also all the example programs.
+
+  The <em>cmake</em> only need to be executed once (make will automatically call
+  <em>cmake</em> if you modify one of the <em>CMakeList.txt</em> files).
+
+  To generate this documentation type
+       - make doc
+       .
+
+  The files in the <em>build</em> directory are genetated by <em>cmake</em>
+  and <em>make</em> and can be safely removed.
+  After doing so you will need to call cmake manually again.
+
+  \subsection Configuration
+
+  The default build mode is DEBUG. That is, objects and executables 
+  include debug information.
+
+  The RELEASE build mode optimizes for speed. To build in this mode
+  execute
+      - cmake .. -DCMAKE_BUILD_TYPE=RELEASE
+      .
+  The release mode will be kept until next time cmake is executed.
+
+  \subsection Installation
+
+  In order to be able to use the library, it it necessary to copy it into the system.
+  To do that, execute
+
+      - make install
+      .
+
+  as root and the shared libraries will be copied to <em>/usr/local/lib/iriutils</em> directory
+  and the header files will be copied to <em>/usr/local/include/iriutils</em> dierctory. At 
+  this point, the library may be used by any user.
+
+  To remove the library from the system, exceute
+      - make uninstall
+      .
+  as root, and all the associated files will be removed from the system.
+
+  \section Customization
+
+  To build a new application using these library, first it is necessary to locate if the library
+  has been installed or not using the following command
+
+      - FIND_PACKAGE(people_prediction REQUIRED)
+
+  In the case that the package is present, it is necessary to add the header files directory to
+  the include directory path by using
+
+      - INCLUDE_DIRECTORIES(${people_prediction_INCLUDE_DIR})
+
+  Finally, it is also nevessary to link with the desired libraries by using the following command
+
+      - TARGET_LINK_LIBRARIES(<executable name> ${people_prediction_LIBRARY})
+      .
+
+  \section License
+
+  This package is licensed under a  
+  <a href="http://www.gnu.org/licenses/lgpl.html">
+    LGPL 3.0 License</a>.
+
+  \section Disclaimer
+
+   This is free software: you can redistribute it and/or modify
+   it under the terms of the GNU Lesser General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU Lesser General Public License for more details.
+
+   You should have received a copy of the GNU Lesser General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>. 
+
+ */
diff --git a/local_lib_people_prediction/src/.cproject b/local_lib_people_prediction/src/.cproject
new file mode 100644
index 0000000000000000000000000000000000000000..3526554db28ebfa524cfbe38685699431155d78f
--- /dev/null
+++ b/local_lib_people_prediction/src/.cproject
@@ -0,0 +1,176 @@
+<?xml version="1.0" encoding="UTF-8" standalone="no"?>
+<?fileVersion 4.0.0?><cproject storage_type_id="org.eclipse.cdt.core.XmlProjectDescriptionStorage">
+	<storageModule moduleId="org.eclipse.cdt.core.settings">
+		<cconfiguration id="cdt.managedbuild.config.gnu.exe.debug.735045042">
+			<storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="cdt.managedbuild.config.gnu.exe.debug.735045042" moduleId="org.eclipse.cdt.core.settings" name="Debug">
+				<externalSettings/>
+				<extensions>
+					<extension id="org.eclipse.cdt.core.GmakeErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
+					<extension id="org.eclipse.cdt.core.CWDLocator" point="org.eclipse.cdt.core.ErrorParser"/>
+					<extension id="org.eclipse.cdt.core.GCCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
+					<extension id="org.eclipse.cdt.core.GASErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
+					<extension id="org.eclipse.cdt.core.GLDErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
+					<extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
+				</extensions>
+			</storageModule>
+			<storageModule moduleId="cdtBuildSystem" version="4.0.0">
+				<configuration artifactName="${ProjName}" buildArtefactType="org.eclipse.cdt.build.core.buildArtefactType.exe" buildProperties="org.eclipse.cdt.build.core.buildType=org.eclipse.cdt.build.core.buildType.debug,org.eclipse.cdt.build.core.buildArtefactType=org.eclipse.cdt.build.core.buildArtefactType.exe" cleanCommand="rm -rf" description="" id="cdt.managedbuild.config.gnu.exe.debug.735045042" name="Debug" parent="cdt.managedbuild.config.gnu.exe.debug">
+					<folderInfo id="cdt.managedbuild.config.gnu.exe.debug.735045042." name="/" resourcePath="">
+						<toolChain id="cdt.managedbuild.toolchain.gnu.exe.debug.671029713" name="Linux GCC" superClass="cdt.managedbuild.toolchain.gnu.exe.debug">
+							<targetPlatform id="cdt.managedbuild.target.gnu.platform.exe.debug.1500733055" name="Debug Platform" superClass="cdt.managedbuild.target.gnu.platform.exe.debug"/>
+							<builder buildPath="${workspace_loc:/akp_planner_companion_V_form}/Debug" id="cdt.managedbuild.target.gnu.builder.exe.debug.857094640" keepEnvironmentInBuildfile="false" managedBuildOn="true" name="Gnu Make Builder" superClass="cdt.managedbuild.target.gnu.builder.exe.debug">
+								<outputEntries>
+									<entry flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="outputPath" name="Debug"/>
+									<entry flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="outputPath" name="Release"/>
+								</outputEntries>
+							</builder>
+							<tool id="cdt.managedbuild.tool.gnu.archiver.base.200630864" name="GCC Archiver" superClass="cdt.managedbuild.tool.gnu.archiver.base"/>
+							<tool id="cdt.managedbuild.tool.gnu.cpp.compiler.exe.debug.1283013666" name="GCC C++ Compiler" superClass="cdt.managedbuild.tool.gnu.cpp.compiler.exe.debug">
+								<option id="gnu.cpp.compiler.exe.debug.option.optimization.level.2070692789" name="Optimization Level" superClass="gnu.cpp.compiler.exe.debug.option.optimization.level" value="gnu.cpp.compiler.optimization.level.none" valueType="enumerated"/>
+								<option id="gnu.cpp.compiler.exe.debug.option.debugging.level.1550285381" name="Debug Level" superClass="gnu.cpp.compiler.exe.debug.option.debugging.level" value="gnu.cpp.compiler.debugging.level.max" valueType="enumerated"/>
+								<inputType id="cdt.managedbuild.tool.gnu.cpp.compiler.input.2005970661" superClass="cdt.managedbuild.tool.gnu.cpp.compiler.input"/>
+							</tool>
+							<tool id="cdt.managedbuild.tool.gnu.c.compiler.exe.debug.1235757322" name="GCC C Compiler" superClass="cdt.managedbuild.tool.gnu.c.compiler.exe.debug">
+								<option defaultValue="gnu.c.optimization.level.none" id="gnu.c.compiler.exe.debug.option.optimization.level.1541037546" name="Optimization Level" superClass="gnu.c.compiler.exe.debug.option.optimization.level" valueType="enumerated"/>
+								<option id="gnu.c.compiler.exe.debug.option.debugging.level.1653039826" name="Debug Level" superClass="gnu.c.compiler.exe.debug.option.debugging.level" value="gnu.c.debugging.level.max" valueType="enumerated"/>
+								<inputType id="cdt.managedbuild.tool.gnu.c.compiler.input.1283542136" superClass="cdt.managedbuild.tool.gnu.c.compiler.input"/>
+							</tool>
+							<tool id="cdt.managedbuild.tool.gnu.c.linker.exe.debug.1908915761" name="GCC C Linker" superClass="cdt.managedbuild.tool.gnu.c.linker.exe.debug"/>
+							<tool id="cdt.managedbuild.tool.gnu.cpp.linker.exe.debug.1696551909" name="GCC C++ Linker" superClass="cdt.managedbuild.tool.gnu.cpp.linker.exe.debug">
+								<inputType id="cdt.managedbuild.tool.gnu.cpp.linker.input.882316765" superClass="cdt.managedbuild.tool.gnu.cpp.linker.input">
+									<additionalInput kind="additionalinputdependency" paths="$(USER_OBJS)"/>
+									<additionalInput kind="additionalinput" paths="$(LIBS)"/>
+								</inputType>
+							</tool>
+							<tool id="cdt.managedbuild.tool.gnu.assembler.exe.debug.1570456877" name="GCC Assembler" superClass="cdt.managedbuild.tool.gnu.assembler.exe.debug">
+								<inputType id="cdt.managedbuild.tool.gnu.assembler.input.1474591630" superClass="cdt.managedbuild.tool.gnu.assembler.input"/>
+							</tool>
+						</toolChain>
+					</folderInfo>
+				</configuration>
+			</storageModule>
+			<storageModule moduleId="org.eclipse.cdt.core.externalSettings"/>
+		</cconfiguration>
+		<cconfiguration id="cdt.managedbuild.config.gnu.exe.release.415574665">
+			<storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="cdt.managedbuild.config.gnu.exe.release.415574665" moduleId="org.eclipse.cdt.core.settings" name="Release">
+				<externalSettings/>
+				<extensions>
+					<extension id="org.eclipse.cdt.core.GmakeErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
+					<extension id="org.eclipse.cdt.core.CWDLocator" point="org.eclipse.cdt.core.ErrorParser"/>
+					<extension id="org.eclipse.cdt.core.GCCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
+					<extension id="org.eclipse.cdt.core.GASErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
+					<extension id="org.eclipse.cdt.core.GLDErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
+					<extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
+				</extensions>
+			</storageModule>
+			<storageModule moduleId="cdtBuildSystem" version="4.0.0">
+				<configuration artifactName="${ProjName}" buildArtefactType="org.eclipse.cdt.build.core.buildArtefactType.exe" buildProperties="org.eclipse.cdt.build.core.buildType=org.eclipse.cdt.build.core.buildType.release,org.eclipse.cdt.build.core.buildArtefactType=org.eclipse.cdt.build.core.buildArtefactType.exe" cleanCommand="rm -rf" description="" id="cdt.managedbuild.config.gnu.exe.release.415574665" name="Release" parent="cdt.managedbuild.config.gnu.exe.release">
+					<folderInfo id="cdt.managedbuild.config.gnu.exe.release.415574665." name="/" resourcePath="">
+						<toolChain id="cdt.managedbuild.toolchain.gnu.exe.release.644864426" name="Linux GCC" superClass="cdt.managedbuild.toolchain.gnu.exe.release">
+							<targetPlatform id="cdt.managedbuild.target.gnu.platform.exe.release.1980615383" name="Debug Platform" superClass="cdt.managedbuild.target.gnu.platform.exe.release"/>
+							<builder buildPath="${workspace_loc:/akp_planner_companion_V_form}/Release" id="cdt.managedbuild.target.gnu.builder.exe.release.221046058" keepEnvironmentInBuildfile="false" managedBuildOn="true" name="Gnu Make Builder" superClass="cdt.managedbuild.target.gnu.builder.exe.release"/>
+							<tool id="cdt.managedbuild.tool.gnu.archiver.base.667973426" name="GCC Archiver" superClass="cdt.managedbuild.tool.gnu.archiver.base"/>
+							<tool id="cdt.managedbuild.tool.gnu.cpp.compiler.exe.release.242983625" name="GCC C++ Compiler" superClass="cdt.managedbuild.tool.gnu.cpp.compiler.exe.release">
+								<option id="gnu.cpp.compiler.exe.release.option.optimization.level.1459849581" name="Optimization Level" superClass="gnu.cpp.compiler.exe.release.option.optimization.level" value="gnu.cpp.compiler.optimization.level.most" valueType="enumerated"/>
+								<option id="gnu.cpp.compiler.exe.release.option.debugging.level.880981640" name="Debug Level" superClass="gnu.cpp.compiler.exe.release.option.debugging.level" value="gnu.cpp.compiler.debugging.level.none" valueType="enumerated"/>
+								<inputType id="cdt.managedbuild.tool.gnu.cpp.compiler.input.568261173" superClass="cdt.managedbuild.tool.gnu.cpp.compiler.input"/>
+							</tool>
+							<tool id="cdt.managedbuild.tool.gnu.c.compiler.exe.release.325952224" name="GCC C Compiler" superClass="cdt.managedbuild.tool.gnu.c.compiler.exe.release">
+								<option defaultValue="gnu.c.optimization.level.most" id="gnu.c.compiler.exe.release.option.optimization.level.1902510496" name="Optimization Level" superClass="gnu.c.compiler.exe.release.option.optimization.level" valueType="enumerated"/>
+								<option id="gnu.c.compiler.exe.release.option.debugging.level.2113626212" name="Debug Level" superClass="gnu.c.compiler.exe.release.option.debugging.level" value="gnu.c.debugging.level.none" valueType="enumerated"/>
+								<inputType id="cdt.managedbuild.tool.gnu.c.compiler.input.1161669082" superClass="cdt.managedbuild.tool.gnu.c.compiler.input"/>
+							</tool>
+							<tool id="cdt.managedbuild.tool.gnu.c.linker.exe.release.1010836351" name="GCC C Linker" superClass="cdt.managedbuild.tool.gnu.c.linker.exe.release"/>
+							<tool id="cdt.managedbuild.tool.gnu.cpp.linker.exe.release.1312332689" name="GCC C++ Linker" superClass="cdt.managedbuild.tool.gnu.cpp.linker.exe.release">
+								<inputType id="cdt.managedbuild.tool.gnu.cpp.linker.input.50916749" superClass="cdt.managedbuild.tool.gnu.cpp.linker.input">
+									<additionalInput kind="additionalinputdependency" paths="$(USER_OBJS)"/>
+									<additionalInput kind="additionalinput" paths="$(LIBS)"/>
+								</inputType>
+							</tool>
+							<tool id="cdt.managedbuild.tool.gnu.assembler.exe.release.795971616" name="GCC Assembler" superClass="cdt.managedbuild.tool.gnu.assembler.exe.release">
+								<inputType id="cdt.managedbuild.tool.gnu.assembler.input.346658640" superClass="cdt.managedbuild.tool.gnu.assembler.input"/>
+							</tool>
+						</toolChain>
+					</folderInfo>
+					<sourceEntries>
+						<entry flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name=""/>
+					</sourceEntries>
+				</configuration>
+			</storageModule>
+			<storageModule moduleId="org.eclipse.cdt.core.externalSettings"/>
+		</cconfiguration>
+	</storageModule>
+	<storageModule moduleId="cdtBuildSystem" version="4.0.0">
+		<project id="akp_planner_companion_V_form.cdt.managedbuild.target.gnu.exe.1302007091" name="Executable" projectType="cdt.managedbuild.target.gnu.exe"/>
+	</storageModule>
+	<storageModule moduleId="scannerConfiguration">
+		<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
+		<scannerConfigBuildInfo instanceId="cdt.managedbuild.config.gnu.exe.release.211050287;cdt.managedbuild.config.gnu.exe.release.211050287.;cdt.managedbuild.tool.gnu.cpp.compiler.exe.release.754733357;cdt.managedbuild.tool.gnu.cpp.compiler.input.1723783482">
+			<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
+		</scannerConfigBuildInfo>
+		<scannerConfigBuildInfo instanceId="cdt.managedbuild.config.gnu.exe.release.415574665;cdt.managedbuild.config.gnu.exe.release.415574665.;cdt.managedbuild.tool.gnu.c.compiler.exe.release.325952224;cdt.managedbuild.tool.gnu.c.compiler.input.1161669082">
+			<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
+		</scannerConfigBuildInfo>
+		<scannerConfigBuildInfo instanceId="cdt.managedbuild.config.gnu.exe.debug.164431030;cdt.managedbuild.config.gnu.exe.debug.164431030.;cdt.managedbuild.tool.gnu.c.compiler.exe.debug.1705675693;cdt.managedbuild.tool.gnu.c.compiler.input.194699495">
+			<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
+		</scannerConfigBuildInfo>
+		<scannerConfigBuildInfo instanceId="cdt.managedbuild.config.gnu.exe.release.264027607;cdt.managedbuild.config.gnu.exe.release.264027607.;cdt.managedbuild.tool.gnu.c.compiler.exe.release.682221034;cdt.managedbuild.tool.gnu.c.compiler.input.1847635386">
+			<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
+		</scannerConfigBuildInfo>
+		<scannerConfigBuildInfo instanceId="cdt.managedbuild.config.gnu.exe.release.211050287;cdt.managedbuild.config.gnu.exe.release.211050287.;cdt.managedbuild.tool.gnu.c.compiler.exe.release.741361446;cdt.managedbuild.tool.gnu.c.compiler.input.1612819661">
+			<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
+		</scannerConfigBuildInfo>
+		<scannerConfigBuildInfo instanceId="cdt.managedbuild.config.gnu.exe.release.1448151324;cdt.managedbuild.config.gnu.exe.release.1448151324.;cdt.managedbuild.tool.gnu.c.compiler.exe.release.808668837;cdt.managedbuild.tool.gnu.c.compiler.input.535165952">
+			<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
+		</scannerConfigBuildInfo>
+		<scannerConfigBuildInfo instanceId="cdt.managedbuild.config.gnu.exe.debug.2009800520;cdt.managedbuild.config.gnu.exe.debug.2009800520.;cdt.managedbuild.tool.gnu.cpp.compiler.exe.debug.993438546;cdt.managedbuild.tool.gnu.cpp.compiler.input.1315437402">
+			<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
+		</scannerConfigBuildInfo>
+		<scannerConfigBuildInfo instanceId="cdt.managedbuild.config.gnu.exe.debug.799578447;cdt.managedbuild.config.gnu.exe.debug.799578447.;cdt.managedbuild.tool.gnu.c.compiler.exe.debug.1724903320;cdt.managedbuild.tool.gnu.c.compiler.input.475270302">
+			<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
+		</scannerConfigBuildInfo>
+		<scannerConfigBuildInfo instanceId="cdt.managedbuild.config.gnu.exe.debug.2009800520;cdt.managedbuild.config.gnu.exe.debug.2009800520.;cdt.managedbuild.tool.gnu.c.compiler.exe.debug.322401516;cdt.managedbuild.tool.gnu.c.compiler.input.955127031">
+			<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
+		</scannerConfigBuildInfo>
+		<scannerConfigBuildInfo instanceId="cdt.managedbuild.config.gnu.exe.release.264027607;cdt.managedbuild.config.gnu.exe.release.264027607.;cdt.managedbuild.tool.gnu.cpp.compiler.exe.release.1780064963;cdt.managedbuild.tool.gnu.cpp.compiler.input.1012739404">
+			<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
+		</scannerConfigBuildInfo>
+		<scannerConfigBuildInfo instanceId="cdt.managedbuild.config.gnu.exe.debug.1271324720;cdt.managedbuild.config.gnu.exe.debug.1271324720.;cdt.managedbuild.tool.gnu.c.compiler.exe.debug.298917128;cdt.managedbuild.tool.gnu.c.compiler.input.1187712877">
+			<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
+		</scannerConfigBuildInfo>
+		<scannerConfigBuildInfo instanceId="cdt.managedbuild.config.gnu.exe.debug.1053945930;cdt.managedbuild.config.gnu.exe.debug.1053945930.;cdt.managedbuild.tool.gnu.c.compiler.exe.debug.1928664744;cdt.managedbuild.tool.gnu.c.compiler.input.909266311">
+			<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
+		</scannerConfigBuildInfo>
+		<scannerConfigBuildInfo instanceId="cdt.managedbuild.config.gnu.exe.debug.735045042;cdt.managedbuild.config.gnu.exe.debug.735045042.;cdt.managedbuild.tool.gnu.cpp.compiler.exe.debug.1283013666;cdt.managedbuild.tool.gnu.cpp.compiler.input.2005970661">
+			<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
+		</scannerConfigBuildInfo>
+		<scannerConfigBuildInfo instanceId="cdt.managedbuild.config.gnu.exe.release.1713646022;cdt.managedbuild.config.gnu.exe.release.1713646022.;cdt.managedbuild.tool.gnu.cpp.compiler.exe.release.383706676;cdt.managedbuild.tool.gnu.cpp.compiler.input.1354152810">
+			<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
+		</scannerConfigBuildInfo>
+		<scannerConfigBuildInfo instanceId="cdt.managedbuild.config.gnu.exe.debug.735045042;cdt.managedbuild.config.gnu.exe.debug.735045042.;cdt.managedbuild.tool.gnu.c.compiler.exe.debug.1235757322;cdt.managedbuild.tool.gnu.c.compiler.input.1283542136">
+			<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
+		</scannerConfigBuildInfo>
+		<scannerConfigBuildInfo instanceId="cdt.managedbuild.config.gnu.exe.release.1713646022;cdt.managedbuild.config.gnu.exe.release.1713646022.;cdt.managedbuild.tool.gnu.c.compiler.exe.release.6184313;cdt.managedbuild.tool.gnu.c.compiler.input.1871309514">
+			<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
+		</scannerConfigBuildInfo>
+		<scannerConfigBuildInfo instanceId="cdt.managedbuild.config.gnu.exe.release.1878746875;cdt.managedbuild.config.gnu.exe.release.1878746875.;cdt.managedbuild.tool.gnu.c.compiler.exe.release.1484471341;cdt.managedbuild.tool.gnu.c.compiler.input.2077843948">
+			<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
+		</scannerConfigBuildInfo>
+		<scannerConfigBuildInfo instanceId="cdt.managedbuild.config.gnu.exe.debug.799578447;cdt.managedbuild.config.gnu.exe.debug.799578447.;cdt.managedbuild.tool.gnu.cpp.compiler.exe.debug.1405167501;cdt.managedbuild.tool.gnu.cpp.compiler.input.1110545555">
+			<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
+		</scannerConfigBuildInfo>
+		<scannerConfigBuildInfo instanceId="cdt.managedbuild.config.gnu.exe.release.415574665;cdt.managedbuild.config.gnu.exe.release.415574665.;cdt.managedbuild.tool.gnu.cpp.compiler.exe.release.242983625;cdt.managedbuild.tool.gnu.cpp.compiler.input.568261173">
+			<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
+		</scannerConfigBuildInfo>
+		<scannerConfigBuildInfo instanceId="cdt.managedbuild.config.gnu.exe.release.1878746875;cdt.managedbuild.config.gnu.exe.release.1878746875.;cdt.managedbuild.tool.gnu.cpp.compiler.exe.release.428378654;cdt.managedbuild.tool.gnu.cpp.compiler.input.1752641194">
+			<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
+		</scannerConfigBuildInfo>
+		<scannerConfigBuildInfo instanceId="cdt.managedbuild.config.gnu.exe.debug.1053945930;cdt.managedbuild.config.gnu.exe.debug.1053945930.;cdt.managedbuild.tool.gnu.cpp.compiler.exe.debug.993089667;cdt.managedbuild.tool.gnu.cpp.compiler.input.1083681099">
+			<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
+		</scannerConfigBuildInfo>
+		<scannerConfigBuildInfo instanceId="cdt.managedbuild.config.gnu.exe.debug.164431030;cdt.managedbuild.config.gnu.exe.debug.164431030.;cdt.managedbuild.tool.gnu.cpp.compiler.exe.debug.2046853649;cdt.managedbuild.tool.gnu.cpp.compiler.input.613678201">
+			<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
+		</scannerConfigBuildInfo>
+	</storageModule>
+	<storageModule moduleId="org.eclipse.cdt.core.LanguageSettingsProviders"/>
+</cproject>
diff --git a/local_lib_people_prediction/src/.project b/local_lib_people_prediction/src/.project
new file mode 100644
index 0000000000000000000000000000000000000000..7b02c5df6c3a3e95e9d5a3462e651eb947220e8f
--- /dev/null
+++ b/local_lib_people_prediction/src/.project
@@ -0,0 +1,27 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<projectDescription>
+	<name>library_IRI_ATR_group_companion_local_planner</name>
+	<comment></comment>
+	<projects>
+	</projects>
+	<buildSpec>
+		<buildCommand>
+			<name>org.eclipse.cdt.managedbuilder.core.genmakebuilder</name>
+			<triggers>clean,full,incremental,</triggers>
+			<arguments>
+			</arguments>
+		</buildCommand>
+		<buildCommand>
+			<name>org.eclipse.cdt.managedbuilder.core.ScannerConfigBuilder</name>
+			<triggers>full,incremental,</triggers>
+			<arguments>
+			</arguments>
+		</buildCommand>
+	</buildSpec>
+	<natures>
+		<nature>org.eclipse.cdt.core.cnature</nature>
+		<nature>org.eclipse.cdt.managedbuilder.core.managedBuildNature</nature>
+		<nature>org.eclipse.cdt.managedbuilder.core.ScannerConfigNature</nature>
+		<nature>org.eclipse.cdt.core.ccnature</nature>
+	</natures>
+</projectDescription>
diff --git a/local_lib_people_prediction/src/.settings/language.settings.xml b/local_lib_people_prediction/src/.settings/language.settings.xml
new file mode 100644
index 0000000000000000000000000000000000000000..84757fd4e73b6d37d8e652d46a95bbae69f34d0d
--- /dev/null
+++ b/local_lib_people_prediction/src/.settings/language.settings.xml
@@ -0,0 +1,25 @@
+<?xml version="1.0" encoding="UTF-8" standalone="no"?>
+<project>
+	<configuration id="cdt.managedbuild.config.gnu.exe.debug.1271324720" name="Debug">
+		<extension point="org.eclipse.cdt.core.LanguageSettingsProvider">
+			<provider copy-of="extension" id="org.eclipse.cdt.ui.UserLanguageSettingsProvider"/>
+			<provider-reference id="org.eclipse.cdt.core.ReferencedProjectsLanguageSettingsProvider" ref="shared-provider"/>
+			<provider-reference id="org.eclipse.cdt.managedbuilder.core.MBSLanguageSettingsProvider" ref="shared-provider"/>
+			<provider class="org.eclipse.cdt.managedbuilder.language.settings.providers.GCCBuiltinSpecsDetector" console="false" env-hash="1690581470106523587" id="org.eclipse.cdt.managedbuilder.core.GCCBuiltinSpecsDetector" keep-relative-paths="false" name="CDT GCC Built-in Compiler Settings" parameter="${COMMAND} ${FLAGS} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true">
+				<language-scope id="org.eclipse.cdt.core.gcc"/>
+				<language-scope id="org.eclipse.cdt.core.g++"/>
+			</provider>
+		</extension>
+	</configuration>
+	<configuration id="cdt.managedbuild.config.gnu.exe.release.1448151324" name="Release">
+		<extension point="org.eclipse.cdt.core.LanguageSettingsProvider">
+			<provider copy-of="extension" id="org.eclipse.cdt.ui.UserLanguageSettingsProvider"/>
+			<provider-reference id="org.eclipse.cdt.core.ReferencedProjectsLanguageSettingsProvider" ref="shared-provider"/>
+			<provider-reference id="org.eclipse.cdt.managedbuilder.core.MBSLanguageSettingsProvider" ref="shared-provider"/>
+			<provider class="org.eclipse.cdt.managedbuilder.language.settings.providers.GCCBuiltinSpecsDetector" console="false" env-hash="1690581470106523587" id="org.eclipse.cdt.managedbuilder.core.GCCBuiltinSpecsDetector" keep-relative-paths="false" name="CDT GCC Built-in Compiler Settings" parameter="${COMMAND} ${FLAGS} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true">
+				<language-scope id="org.eclipse.cdt.core.gcc"/>
+				<language-scope id="org.eclipse.cdt.core.g++"/>
+			</provider>
+		</extension>
+	</configuration>
+</project>
diff --git a/local_lib_people_prediction/src/CMakeLists.txt b/local_lib_people_prediction/src/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..3e63f4246aa45e22de7a0de1dc92f61a743957b5
--- /dev/null
+++ b/local_lib_people_prediction/src/CMakeLists.txt
@@ -0,0 +1,112 @@
+ # locate the necessary dependencies
+
+
+INCLUDE_DIRECTORIES(.)
+# add the necessary include directories
+find_package(Eigen QUIET)
+if(Eigen_FOUND)
+  MESSAGE("Eigen found in CMake modules:" ${Eigen_INCLUDE_DIRS})
+ELSE(Eigen_FOUND)  
+    #INCLUDE_DIRECTORIES(/usr/include/eigen3)
+    #INCLUDE_DIRECTORIES(/usr/local/include/eigen3)
+    MESSAGE("Eigen not found, adding manually directories. BE CAREFULL to have installed eigen there")
+    SET(Eigen_INCLUDE_DIRS /usr/include/eigen3 /usr/local/include/eigen3)    
+ENDIF(Eigen_FOUND)
+INCLUDE_DIRECTORIES(${Eigen_INCLUDE_DIRS})
+
+# extra source files
+SET(sources_random
+	random/rand_gmm.cpp
+)
+SET(sources_scene_elements
+	scene_elements/person_abstract.cpp
+    scene_elements/person_bhmip.cpp
+    scene_elements/person_behavior.cpp
+    scene_elements/robot.cpp
+    scene_elements/person_virtual.cpp
+)
+SET(sources_nav
+    nav/force_reactive_robot_companion.cpp
+    #nav/force_reactive_robot_companion_learning.cpp
+    nav/plan_local_nav.cpp
+    nav/plan_local_nav_person_companion.cpp
+)
+
+SET(sources_learnning
+    learning_parameters/learnninglinearregression.cpp
+    #learning_parameters/getFilesnames_script.cpp
+)
+
+#SET(sources_companion_Zanlungo
+#    	companion_zanlungo/CompanionFrancescoModel.cpp
+#	companion_zanlungo/Vector2D.cpp
+#)
+
+# extra header files
+SET(headers_random
+	random/rand_gmm.h
+)
+SET(headers_scene_elements
+	scene_elements/person_abstract.h
+    	scene_elements/person_bhmip.h
+    	scene_elements/person_behavior.h
+    	scene_elements/robot.h
+    	scene_elements/person_virtual.h
+)
+SET(headers_nav
+    	nav/force_reactive_robot_companion.h
+    	#nav/force_reactive_robot_companion_learning.h
+    	nav/plan_local_nav.h
+	nav/plan_local_nav_person_companion.h
+)
+SET(headers_learnning
+    learning_parameters/learnninglinearregression.h
+)
+
+# application source files
+SET(sources
+	iri_geometry.cpp
+    	scene_abstract.cpp
+	prediction_bhmip.cpp
+	prediction_behavior.cpp
+    	scene_sim.cpp
+)
+# application header files
+SET(headers
+	iri_geometry.h
+	scene_abstract.h
+	prediction_bhmip.h
+	prediction_behavior.h
+    	scene_sim.h
+)
+
+#SET(headers_companion_Zanlungo
+ #   	companion_zanlungo/CompanionFrancescoModel.h	
+#	companion_zanlungo/Vector2D.h
+#)
+
+# create the shared library
+ADD_LIBRARY(people_prediction SHARED  ${sources_scene_elements} ${sources}  ${sources_random} ${sources_nav} ${sources_learnning} ${sources_companion_Zanlungo} )
+#TARGET_LINK_LIBRARIES( .. )
+#target_link_libraries(${PROJECT_NAME} ${position_3d_LIBRARY})
+
+
+#install 
+INSTALL(TARGETS people_prediction
+        RUNTIME DESTINATION bin
+        LIBRARY DESTINATION lib/iridrivers
+        ARCHIVE DESTINATION lib/iridrivers)
+
+INSTALL(FILES ${headers} DESTINATION include/iridrivers/people_prediction)
+INSTALL(FILES ${headers_random} DESTINATION include/iridrivers/people_prediction/random)
+INSTALL(FILES ${headers_scene_elements} DESTINATION include/iridrivers/people_prediction/scene_elements)
+INSTALL(FILES ${headers_nav} DESTINATION include/iridrivers/people_prediction/nav)
+INSTALL(FILES ${headers_learnning} DESTINATION include/iridrivers/people_prediction/learning_parameters)
+#INSTALL(FILES ${headers_companion_Zanlungo} DESTINATION include/iridrivers/people_prediction/companion_zanlungo)
+
+INSTALL(FILES ../Findpeople_prediction.cmake DESTINATION ${CMAKE_ROOT}/Modules/)
+
+
+ADD_SUBDIRECTORY(examples)
+#uncoment to add tests
+#ADD_SUBDIRECTORY(test)
diff --git a/local_lib_people_prediction/src/Debug/examples/subdir.mk b/local_lib_people_prediction/src/Debug/examples/subdir.mk
new file mode 100644
index 0000000000000000000000000000000000000000..5b25052f4aa1c431df1822dda755ed4331fccb90
--- /dev/null
+++ b/local_lib_people_prediction/src/Debug/examples/subdir.mk
@@ -0,0 +1,36 @@
+################################################################################
+# Automatically-generated file. Do not edit!
+################################################################################
+
+# Add inputs and outputs from these tool invocations to the build variables 
+CPP_SRCS += \
+../examples/generate_performance_from_data_example.cpp \
+../examples/generate_performance_from_data_exampleIROS2019.cpp \
+../examples/generate_performance_from_data_example_humanoids_side_by_side_2people.cpp \
+../examples/local_nav_example.cpp \
+../examples/prediction_example.cpp 
+
+OBJS += \
+./examples/generate_performance_from_data_example.o \
+./examples/generate_performance_from_data_exampleIROS2019.o \
+./examples/generate_performance_from_data_example_humanoids_side_by_side_2people.o \
+./examples/local_nav_example.o \
+./examples/prediction_example.o 
+
+CPP_DEPS += \
+./examples/generate_performance_from_data_example.d \
+./examples/generate_performance_from_data_exampleIROS2019.d \
+./examples/generate_performance_from_data_example_humanoids_side_by_side_2people.d \
+./examples/local_nav_example.d \
+./examples/prediction_example.d 
+
+
+# Each subdirectory must supply rules for building sources it contributes
+examples/%.o: ../examples/%.cpp
+	@echo 'Building file: $<'
+	@echo 'Invoking: GCC C++ Compiler'
+	g++ -O0 -g3 -Wall -c -fmessage-length=0 -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
+	@echo 'Finished building: $<'
+	@echo ' '
+
+
diff --git a/local_lib_people_prediction/src/Debug/learning_parameters/subdir.mk b/local_lib_people_prediction/src/Debug/learning_parameters/subdir.mk
new file mode 100644
index 0000000000000000000000000000000000000000..537417c82b7230d18d3cd91237a98c081ff362dd
--- /dev/null
+++ b/local_lib_people_prediction/src/Debug/learning_parameters/subdir.mk
@@ -0,0 +1,27 @@
+################################################################################
+# Automatically-generated file. Do not edit!
+################################################################################
+
+# Add inputs and outputs from these tool invocations to the build variables 
+CPP_SRCS += \
+../learning_parameters/getFilesnames_script.cpp \
+../learning_parameters/learnninglinearregression.cpp 
+
+OBJS += \
+./learning_parameters/getFilesnames_script.o \
+./learning_parameters/learnninglinearregression.o 
+
+CPP_DEPS += \
+./learning_parameters/getFilesnames_script.d \
+./learning_parameters/learnninglinearregression.d 
+
+
+# Each subdirectory must supply rules for building sources it contributes
+learning_parameters/%.o: ../learning_parameters/%.cpp
+	@echo 'Building file: $<'
+	@echo 'Invoking: GCC C++ Compiler'
+	g++ -O0 -g3 -Wall -c -fmessage-length=0 -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
+	@echo 'Finished building: $<'
+	@echo ' '
+
+
diff --git a/local_lib_people_prediction/src/Debug/makefile b/local_lib_people_prediction/src/Debug/makefile
new file mode 100644
index 0000000000000000000000000000000000000000..cb932d037cc4b208db133311f27f0b221ff105a5
--- /dev/null
+++ b/local_lib_people_prediction/src/Debug/makefile
@@ -0,0 +1,65 @@
+################################################################################
+# Automatically-generated file. Do not edit!
+################################################################################
+
+-include ../makefile.init
+
+RM := rm -rf
+
+# All of the sources participating in the build are defined here
+-include sources.mk
+-include test/subdir.mk
+-include scene_elements/subdir.mk
+-include random/subdir.mk
+-include nav/subdir.mk
+-include learning_parameters/subdir.mk
+-include examples/subdir.mk
+-include companion_zanlungo/subdir.mk
+-include subdir.mk
+-include objects.mk
+
+ifneq ($(MAKECMDGOALS),clean)
+ifneq ($(strip $(C++_DEPS)),)
+-include $(C++_DEPS)
+endif
+ifneq ($(strip $(C_DEPS)),)
+-include $(C_DEPS)
+endif
+ifneq ($(strip $(CC_DEPS)),)
+-include $(CC_DEPS)
+endif
+ifneq ($(strip $(CPP_DEPS)),)
+-include $(CPP_DEPS)
+endif
+ifneq ($(strip $(CXX_DEPS)),)
+-include $(CXX_DEPS)
+endif
+ifneq ($(strip $(C_UPPER_DEPS)),)
+-include $(C_UPPER_DEPS)
+endif
+endif
+
+-include ../makefile.defs
+
+# Add inputs and outputs from these tool invocations to the build variables 
+
+# All Target
+all: akp_local_planner_V_form_and_companion
+
+# Tool invocations
+akp_local_planner_V_form_and_companion: $(OBJS) $(USER_OBJS)
+	@echo 'Building target: $@'
+	@echo 'Invoking: GCC C++ Linker'
+	g++  -o "akp_local_planner_V_form_and_companion" $(OBJS) $(USER_OBJS) $(LIBS)
+	@echo 'Finished building target: $@'
+	@echo ' '
+
+# Other Targets
+clean:
+	-$(RM) $(OBJS)$(C++_DEPS)$(C_DEPS)$(CC_DEPS)$(CPP_DEPS)$(EXECUTABLES)$(CXX_DEPS)$(C_UPPER_DEPS) akp_local_planner_V_form_and_companion
+	-@echo ' '
+
+.PHONY: all clean dependents
+.SECONDARY:
+
+-include ../makefile.targets
diff --git a/local_lib_people_prediction/src/Debug/nav/subdir.mk b/local_lib_people_prediction/src/Debug/nav/subdir.mk
new file mode 100644
index 0000000000000000000000000000000000000000..f625874e8052c2fe332616187229cb54d296fa8a
--- /dev/null
+++ b/local_lib_people_prediction/src/Debug/nav/subdir.mk
@@ -0,0 +1,36 @@
+################################################################################
+# Automatically-generated file. Do not edit!
+################################################################################
+
+# Add inputs and outputs from these tool invocations to the build variables 
+CPP_SRCS += \
+../nav/force_reactive_robot_companion.cpp \
+../nav/force_reactive_robot_companion_learning.cpp \
+../nav/plan_local_nav.cpp \
+../nav/plan_local_nav_old_ok.cpp \
+../nav/plan_local_nav_person_companion.cpp 
+
+OBJS += \
+./nav/force_reactive_robot_companion.o \
+./nav/force_reactive_robot_companion_learning.o \
+./nav/plan_local_nav.o \
+./nav/plan_local_nav_old_ok.o \
+./nav/plan_local_nav_person_companion.o 
+
+CPP_DEPS += \
+./nav/force_reactive_robot_companion.d \
+./nav/force_reactive_robot_companion_learning.d \
+./nav/plan_local_nav.d \
+./nav/plan_local_nav_old_ok.d \
+./nav/plan_local_nav_person_companion.d 
+
+
+# Each subdirectory must supply rules for building sources it contributes
+nav/%.o: ../nav/%.cpp
+	@echo 'Building file: $<'
+	@echo 'Invoking: GCC C++ Compiler'
+	g++ -O0 -g3 -Wall -c -fmessage-length=0 -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
+	@echo 'Finished building: $<'
+	@echo ' '
+
+
diff --git a/local_lib_people_prediction/src/Debug/objects.mk b/local_lib_people_prediction/src/Debug/objects.mk
new file mode 100644
index 0000000000000000000000000000000000000000..742c2da043f4f5d93c414dca3a725ab2204d9817
--- /dev/null
+++ b/local_lib_people_prediction/src/Debug/objects.mk
@@ -0,0 +1,8 @@
+################################################################################
+# Automatically-generated file. Do not edit!
+################################################################################
+
+USER_OBJS :=
+
+LIBS :=
+
diff --git a/local_lib_people_prediction/src/Debug/random/subdir.mk b/local_lib_people_prediction/src/Debug/random/subdir.mk
new file mode 100644
index 0000000000000000000000000000000000000000..8e62cf81ff9263567b0e1ed3790d4ff37fe1f6f3
--- /dev/null
+++ b/local_lib_people_prediction/src/Debug/random/subdir.mk
@@ -0,0 +1,24 @@
+################################################################################
+# Automatically-generated file. Do not edit!
+################################################################################
+
+# Add inputs and outputs from these tool invocations to the build variables 
+CPP_SRCS += \
+../random/rand_gmm.cpp 
+
+OBJS += \
+./random/rand_gmm.o 
+
+CPP_DEPS += \
+./random/rand_gmm.d 
+
+
+# Each subdirectory must supply rules for building sources it contributes
+random/%.o: ../random/%.cpp
+	@echo 'Building file: $<'
+	@echo 'Invoking: GCC C++ Compiler'
+	g++ -O0 -g3 -Wall -c -fmessage-length=0 -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
+	@echo 'Finished building: $<'
+	@echo ' '
+
+
diff --git a/local_lib_people_prediction/src/Debug/scene_elements/subdir.mk b/local_lib_people_prediction/src/Debug/scene_elements/subdir.mk
new file mode 100644
index 0000000000000000000000000000000000000000..7af8d70ffea1dcd8a9d4a0e04b4f29c5766d597a
--- /dev/null
+++ b/local_lib_people_prediction/src/Debug/scene_elements/subdir.mk
@@ -0,0 +1,39 @@
+################################################################################
+# Automatically-generated file. Do not edit!
+################################################################################
+
+# Add inputs and outputs from these tool invocations to the build variables 
+CPP_SRCS += \
+../scene_elements/obstacle.cpp \
+../scene_elements/person_abstract.cpp \
+../scene_elements/person_behavior.cpp \
+../scene_elements/person_bhmip.cpp \
+../scene_elements/person_virtual.cpp \
+../scene_elements/robot.cpp 
+
+OBJS += \
+./scene_elements/obstacle.o \
+./scene_elements/person_abstract.o \
+./scene_elements/person_behavior.o \
+./scene_elements/person_bhmip.o \
+./scene_elements/person_virtual.o \
+./scene_elements/robot.o 
+
+CPP_DEPS += \
+./scene_elements/obstacle.d \
+./scene_elements/person_abstract.d \
+./scene_elements/person_behavior.d \
+./scene_elements/person_bhmip.d \
+./scene_elements/person_virtual.d \
+./scene_elements/robot.d 
+
+
+# Each subdirectory must supply rules for building sources it contributes
+scene_elements/%.o: ../scene_elements/%.cpp
+	@echo 'Building file: $<'
+	@echo 'Invoking: GCC C++ Compiler'
+	g++ -O0 -g3 -Wall -c -fmessage-length=0 -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
+	@echo 'Finished building: $<'
+	@echo ' '
+
+
diff --git a/local_lib_people_prediction/src/Debug/sources.mk b/local_lib_people_prediction/src/Debug/sources.mk
new file mode 100644
index 0000000000000000000000000000000000000000..7372016c62b520de41c6cc5b5b00f32ada0ecb2b
--- /dev/null
+++ b/local_lib_people_prediction/src/Debug/sources.mk
@@ -0,0 +1,34 @@
+################################################################################
+# Automatically-generated file. Do not edit!
+################################################################################
+
+O_SRCS := 
+CPP_SRCS := 
+C_UPPER_SRCS := 
+C_SRCS := 
+S_UPPER_SRCS := 
+OBJ_SRCS := 
+ASM_SRCS := 
+CXX_SRCS := 
+C++_SRCS := 
+CC_SRCS := 
+OBJS := 
+C++_DEPS := 
+C_DEPS := 
+CC_DEPS := 
+CPP_DEPS := 
+EXECUTABLES := 
+CXX_DEPS := 
+C_UPPER_DEPS := 
+
+# Every subdirectory with source files must be described here
+SUBDIRS := \
+test \
+. \
+scene_elements \
+random \
+nav \
+learning_parameters \
+examples \
+companion_zanlungo \
+
diff --git a/local_lib_people_prediction/src/Debug/subdir.mk b/local_lib_people_prediction/src/Debug/subdir.mk
new file mode 100644
index 0000000000000000000000000000000000000000..4e60dc1ec66a4955cedf5d74472d626a2d7ed71a
--- /dev/null
+++ b/local_lib_people_prediction/src/Debug/subdir.mk
@@ -0,0 +1,36 @@
+################################################################################
+# Automatically-generated file. Do not edit!
+################################################################################
+
+# Add inputs and outputs from these tool invocations to the build variables 
+CPP_SRCS += \
+../iri_geometry.cpp \
+../prediction_behavior.cpp \
+../prediction_bhmip.cpp \
+../scene_abstract.cpp \
+../scene_sim.cpp 
+
+OBJS += \
+./iri_geometry.o \
+./prediction_behavior.o \
+./prediction_bhmip.o \
+./scene_abstract.o \
+./scene_sim.o 
+
+CPP_DEPS += \
+./iri_geometry.d \
+./prediction_behavior.d \
+./prediction_bhmip.d \
+./scene_abstract.d \
+./scene_sim.d 
+
+
+# Each subdirectory must supply rules for building sources it contributes
+%.o: ../%.cpp
+	@echo 'Building file: $<'
+	@echo 'Invoking: GCC C++ Compiler'
+	g++ -O0 -g3 -Wall -c -fmessage-length=0 -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
+	@echo 'Finished building: $<'
+	@echo ' '
+
+
diff --git a/local_lib_people_prediction/src/Debug/test/subdir.mk b/local_lib_people_prediction/src/Debug/test/subdir.mk
new file mode 100644
index 0000000000000000000000000000000000000000..ddcc73930f9e8f85bbb373a170c180e108aaaa0f
--- /dev/null
+++ b/local_lib_people_prediction/src/Debug/test/subdir.mk
@@ -0,0 +1,30 @@
+################################################################################
+# Automatically-generated file. Do not edit!
+################################################################################
+
+# Add inputs and outputs from these tool invocations to the build variables 
+CPP_SRCS += \
+../test/test_geometry.cpp \
+../test/test_person.cpp \
+../test/test_scene.cpp 
+
+OBJS += \
+./test/test_geometry.o \
+./test/test_person.o \
+./test/test_scene.o 
+
+CPP_DEPS += \
+./test/test_geometry.d \
+./test/test_person.d \
+./test/test_scene.d 
+
+
+# Each subdirectory must supply rules for building sources it contributes
+test/%.o: ../test/%.cpp
+	@echo 'Building file: $<'
+	@echo 'Invoking: GCC C++ Compiler'
+	g++ -O0 -g3 -Wall -c -fmessage-length=0 -MMD -MP -MF"$(@:%.o=%.d)" -MT"$(@:%.o=%.d)" -o "$@" "$<"
+	@echo 'Finished building: $<'
+	@echo ' '
+
+
diff --git a/local_lib_people_prediction/src/examples/CMakeLists.txt b/local_lib_people_prediction/src/examples/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..8dc8c09c94fd572f52c2f13f78d6bcaa24076cc3
--- /dev/null
+++ b/local_lib_people_prediction/src/examples/CMakeLists.txt
@@ -0,0 +1,19 @@
+#ADD_EXECUTABLE(local_nav_example  local_nav_example.cpp)
+#TARGET_LINK_LIBRARIES(local_nav_example people_prediction)
+# TODO: ver porque no linca la nueva clase Cplan_local_nav_person_companion
+
+ADD_EXECUTABLE(prediction_example  prediction_example.cpp)
+TARGET_LINK_LIBRARIES(prediction_example people_prediction)
+
+#ADD_EXECUTABLE(generate_performance_from_data_example  generate_performance_from_data_example.cpp)
+#TARGET_LINK_LIBRARIES(generate_performance_from_data_example people_prediction)
+
+#ADD_EXECUTABLE(generate_performance_from_data_exampleIROS2019  generate_performance_from_data_exampleIROS2019.cpp)
+#TARGET_LINK_LIBRARIES(generate_performance_from_data_exampleIROS2019 people_prediction) # es de zanlungo. sale fuera.
+
+#ADD_EXECUTABLE(generate_performance_from_data_example_humanoids_side_by_side_2people  generate_performance_from_data_example_humanoids_side_by_side_2people.cpp)
+#TARGET_LINK_LIBRARIES(generate_performance_from_data_example_humanoids_side_by_side_2people people_prediction) # es de two people companion. sale fuera!
+
+
+
+
diff --git a/local_lib_people_prediction/src/examples/generate_performance_from_data_example.cpp b/local_lib_people_prediction/src/examples/generate_performance_from_data_example.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..8f68561fd88ddac0ece2346ca06f80e654afdd40
--- /dev/null
+++ b/local_lib_people_prediction/src/examples/generate_performance_from_data_example.cpp
@@ -0,0 +1,738 @@
+/*
+ * generate_performance_from_data_example.cpp
+ *
+ *  Created on: 7-Sep-2018
+ *      Author: errepiso
+ */
+
+//#include "nav/plan_local_nav.h"
+//#include "nav/plan_local_nav_person_companion.h"
+//#include "nav/force_reactive_robot_companion.h"
+//#include "scene_sim.h"
+#include <iostream>
+#include "companion_zanlungo/CompanionFrancescoModel.h"
+
+int main(int argc,char *argv[])
+{
+	  CompanionFrancescoModel Companion_Zanlungo_Model_;
+
+
+	    double in_Cr, in_r0, in_Ct, in_eta, in_rmax, in_S0;
+	    in_Cr=0.62;
+	    in_Ct=0.08;
+	    in_r0=1.5;
+	    in_eta=-0.43;
+	    in_rmax=3;
+	    in_S0=0.5;
+	    Cgenome genome;
+
+	    genome.Init(in_Cr, in_r0, in_Ct, in_eta, in_rmax, in_S0);
+
+	    Companion_Zanlungo_Model_.set_genome_params_in_CompanionFrancescoModel(genome);
+	    Companion_Zanlungo_Model_.set_internal_k_value(1.5);
+
+	// TODO: falta ver como cargar los datos del fichero txt ahora tal como esta.
+
+	std::string file_name_s="~/iri-lab/iri_ws/src/iri_navigation/iri_atr_akp_local_planner_companion/txt_data/1_V_formation_lateral_sin_obstaculos/results_Vform_companion_OK.txt";
+	const char * file_name=file_name_s.c_str();
+
+	std::vector<double> actual_robot_performance_;
+	std::vector<double> all_means_of_performances_ex_;
+	std::vector<double> all_std_of_performances_ex_;
+
+	std::cout << " OOOOOOOOOOOO77777777777777OOOOOOOOOOOOOOOO file_name="<<file_name<<std::endl;
+
+	//ifstream reader;//(file_name);
+	fstream reader;
+	//reader.open(file_name_s);
+	reader.open("results_robot11.txt");
+	 //cout <<"open_file="<<open_file<<endl;
+	unsigned int experiment_,iteration_;
+	experiment_=2;
+	iteration_=1;
+	unsigned int max_exp=280;//275-129; // until max_exp;
+	unsigned int firs_experiment=1; //
+
+
+
+	double real_computation_time_dt;
+
+	double companion_person_x;
+	double companion_person_y;
+	double companion_person_vx;
+	double companion_person_vy;
+
+	double companion_person_dest_x;
+	double companion_person_dest_y;
+
+	double companion2_person_x;
+	double companion2_person_y;
+	double companion2_person_vx;
+	double companion2_person_vy;
+
+	double companion_person2_dest_x;
+	double companion_person2_dest_y;
+
+	double robot_x;
+	double robot_y;
+	double robot_vx;
+	double robot_vy;
+	double robot_theta;
+	double robot_v;
+	double robot_w;
+
+	double robot_dest_x;
+	double robot_dest_y;
+	// double robot_performance;
+
+
+cout <<"antes bucle experiments ="<<endl;
+	//while( experiment_< max_exp){
+while( iteration_< max_exp){
+		 cout <<"antes bucle iterations"<<endl;
+		// while(experiment_==firs_experiment){
+
+		 cout <<"reader.good()="<<reader.good()<<endl;
+	// TODO: coger valores de arriba del fichero. Iterativamente.
+	if (reader.good()) {
+
+		/// INICIO BUCLE, tendria que ser desde aquí
+		//###############################
+		//# experiment_ and  iteration_ #
+		//###############################
+		 string line;
+		 getline(reader,line);
+		 getline(reader,line);
+		 cout <<"(1)"<<endl;
+		 //experiment_=-1;
+		 cout <<"(2)"<<endl;
+		 //find first comma, read #rows
+			 string::size_type i = line.find("(");
+			 cout <<"(3)"<<endl;
+			 if (i!=reader.eof()) {
+				 cout <<"(4); i="<<i<<endl;
+			 	string cstr = line.substr(i+1,i+2);
+			 	 cout <<"(5)"<<endl;
+			 	experiment_ = atoi(cstr.c_str());
+			 	 cout <<"(6)"<<endl;
+			 } else {
+			 	cout <<"ERROR @ GMap::read num_people_mov"<<endl;
+			 	exit(EXIT_FAILURE);
+			 }
+			 /*if(experiment_==0){
+				// getline(reader,line);
+				 cout <<"(1)"<<endl;
+						 experiment_=-1;
+						 cout <<"(2)"<<endl;
+						 //find first comma, read #rows
+							 string::size_type i = line.find("(");
+							 cout <<"(3)"<<endl;
+							 if (i!=reader.eof()) {
+								 cout <<"(4); i="<<i<<endl;
+							 	string cstr = line.substr(i+1,i+2);
+							 	 cout <<"(5)"<<endl;
+							 	experiment_ = atoi(cstr.c_str());
+							 	 cout <<"(6)"<<endl;
+							 } else {
+							 	cout <<"ERROR @ GMap::read num_people_mov"<<endl;
+							 	exit(EXIT_FAILURE);
+							 }
+			 }*/
+			 cout <<"experiment_="<<experiment_<<endl;
+			// p=experiment_;
+
+			 string::size_type j = line.find(")");
+			 /*if(j>1000){
+				 j=7;
+			 }*/
+
+			 cout <<"j="<<j<<endl;
+			 if (j!=reader.eof()) {
+			 	//string cstr = line.substr(j-1,j);
+			 	///iteration_ = atoi(cstr.c_str());
+			 } else {
+			 	cout <<"ERROR @ GMap::read num_people_mov"<<endl;
+			 	exit(EXIT_FAILURE);
+			 }
+			 cout <<"iteration_="<<iteration_<<endl;
+
+			/* if(iteration_==2){
+				 cout <<"iteration_="<<iteration_<<"; firs_experiment="<<experiment_<<endl;
+				 firs_experiment=experiment_;
+			 }*/
+
+			 getline(reader,line);
+			 i = line.find(" ");
+			 unsigned int k=line.size();
+			 cout <<"; i="<<i<<"; K="<<k<<endl;
+			 if (i!=reader.eof()) {
+				//string cstr = line.substr(i,k);
+			 	//real_computation_time_dt = std::stod(cstr.c_str());
+
+			 } else {
+			 	cout <<"ERROR @ GMap::read num_people_mov"<<endl;
+			 	exit(EXIT_FAILURE);
+			 }
+				cout <<"real_computation_time_dt="<<real_computation_time_dt<<endl;
+		//###########################
+		//# Read companion person1  #
+		//###########################
+
+				//# Read companion person1 position #
+			getline(reader,line);
+			i = line.find(" ");
+			k=line.size();
+			cout <<"; i="<<i<<"; K="<<k<<endl;
+			 if (i!=reader.eof()) {
+				string cstr = line.substr(i,k);
+				companion_person_x = std::stod(cstr.c_str());
+
+			 } else {
+				cout <<"ERROR @ GMap::read num_people_mov"<<endl;
+				exit(EXIT_FAILURE);
+			}
+			cout <<"companion_person_x="<<companion_person_x<<endl;
+
+			getline(reader,line);
+			i = line.find(" ");
+			k=line.size();
+			cout <<"; i="<<i<<"; K="<<k<<endl;
+			 if (i!=reader.eof()) {
+				string cstr = line.substr(i,k);
+				companion_person_y = std::stod(cstr.c_str());
+
+			 } else {
+				cout <<"ERROR @ GMap::read num_people_mov"<<endl;
+				exit(EXIT_FAILURE);
+			}
+			cout <<"companion_person_y="<<companion_person_y<<endl;
+
+			//# Read companion person1 velocities #
+			getline(reader,line);
+			i = line.find(" ");
+			k=line.size();
+			cout <<"; i="<<i<<"; K="<<k<<endl;
+			 if (i!=reader.eof()) {
+				string cstr = line.substr(i,k);
+				companion_person_vx = std::stod(cstr.c_str());
+
+			 } else {
+				cout <<"ERROR @ GMap::read num_people_mov"<<endl;
+				exit(EXIT_FAILURE);
+			}
+			cout <<"companion_person_vx="<<companion_person_vx<<endl;
+
+			getline(reader,line);
+			i = line.find(" ");
+			k=line.size();
+			cout <<"; i="<<i<<"; K="<<k<<endl;
+			 if (i!=reader.eof()) {
+				string cstr = line.substr(i,k);
+				companion_person_vy = std::stod(cstr.c_str());
+
+			 } else {
+				cout <<"ERROR @ GMap::read num_people_mov"<<endl;
+				exit(EXIT_FAILURE);
+			}
+			cout <<"companion_person_vy="<<companion_person_vy<<endl;
+
+			getline(reader,line); // companion_person_theta(1,1)= -nan
+
+			//# Read companion person1 destinations #
+			getline(reader,line); // companion_person_dest_x(1,1)= 0.220653
+
+			i = line.find(" ");
+			k=line.size();
+			if(k>1000){
+				k=38;
+			}
+			cout <<"; i="<<i<<"; K="<<k<<endl;
+			 if (i!=reader.eof()) {
+				string cstr = line.substr(i,k);
+				companion_person_dest_x = std::stod(cstr.c_str());
+
+			 } else {
+				cout <<"ERROR @ GMap::read num_people_mov"<<endl;
+				exit(EXIT_FAILURE);
+			}
+			cout <<"companion_person_dest_x="<<companion_person_dest_x<<endl;
+
+			getline(reader,line); // companion_person_dest_x(1,1)= 0.220653
+
+			i = line.find(" ");
+			k=line.size();
+			if(k>1000){
+				k=38;
+			}
+			cout <<"; i="<<i<<"; K="<<k<<endl;
+			 if (i!=reader.eof()) {
+				string cstr = line.substr(i,k);
+				companion_person_dest_y = std::stod(cstr.c_str());
+
+			 } else {
+				cout <<"ERROR @ GMap::read num_people_mov"<<endl;
+				exit(EXIT_FAILURE);
+			}
+			cout <<"companion_person_dest_y="<<companion_person_dest_y<<endl;
+
+			//###########################
+			//# Read companion person2  #
+			//###########################
+					//# Read companion person2 position #
+				getline(reader,line);
+				i = line.find(" ");
+				k=line.size();
+				cout <<"; i="<<i<<"; K="<<k<<endl;
+				 if (i!=reader.eof()) {
+					string cstr = line.substr(i,k);
+					companion2_person_x = std::stod(cstr.c_str());
+
+				 } else {
+					cout <<"ERROR @ GMap::read num_people_mov"<<endl;
+					exit(EXIT_FAILURE);
+				}
+				cout <<"companion2_person_x="<<companion2_person_x<<endl;
+
+				getline(reader,line);
+				i = line.find(" ");
+				k=line.size();
+				cout <<"; i="<<i<<"; K="<<k<<endl;
+				 if (i!=reader.eof()) {
+					string cstr = line.substr(i,k);
+					companion2_person_y = std::stod(cstr.c_str());
+
+				 } else {
+					cout <<"ERROR @ GMap::read num_people_mov"<<endl;
+					exit(EXIT_FAILURE);
+				}
+				cout <<"companion2_person_y="<<companion2_person_y<<endl;
+
+				//# Read companion person2 velocities #
+				getline(reader,line);
+				i = line.find(" ");
+				k=line.size();
+				cout <<"; i="<<i<<"; K="<<k<<endl;
+				 if (i!=reader.eof()) {
+					string cstr = line.substr(i,k);
+					companion2_person_vx = std::stod(cstr.c_str());
+
+				 } else {
+					cout <<"ERROR @ GMap::read num_people_mov"<<endl;
+					exit(EXIT_FAILURE);
+				}
+				cout <<"companion2_person_vx="<<companion2_person_vx<<endl;
+
+				getline(reader,line);
+				i = line.find(" ");
+				k=line.size();
+				cout <<"; i="<<i<<"; K="<<k<<endl;
+				 if (i!=reader.eof()) {
+					string cstr = line.substr(i,k);
+					companion2_person_vy = std::stod(cstr.c_str());
+
+				 } else {
+					cout <<"ERROR @ GMap::read num_people_mov"<<endl;
+					exit(EXIT_FAILURE);
+				}
+				cout <<"companion2_person_vy="<<companion2_person_vy<<endl;
+
+				getline(reader,line); // companion_person_theta(1,1)= -nan
+
+				//# Read companion person2 destinations #
+				getline(reader,line); // companion_person_dest_x(1,1)= 0.220653
+
+				i = line.find(" ");
+				k=line.size();
+				if(k>1000){
+					k=39;
+				}
+				cout <<"; i="<<i<<"; K="<<k<<endl;
+				 if (i!=reader.eof()) {
+					string cstr = line.substr(i,k);
+					companion_person2_dest_x = std::stod(cstr.c_str());
+
+				 } else {
+					cout <<"ERROR @ GMap::read num_people_mov"<<endl;
+					exit(EXIT_FAILURE);
+				}
+				cout <<"companion_person2_dest_x="<<companion_person2_dest_x<<endl;
+
+				getline(reader,line); // companion_person_dest_x(1,1)= 0.220653
+
+				i = line.find(" ");
+				k=line.size();
+				cout <<"; i="<<i<<"; K="<<k<<endl;
+				if(k>1000){
+					k=39;
+				}
+				 if (i!=reader.eof()) {
+					string cstr = line.substr(i,k);
+					companion_person2_dest_y = std::stod(cstr.c_str());
+
+				 } else {
+					cout <<"ERROR @ GMap::read num_people_mov"<<endl;
+					exit(EXIT_FAILURE);
+				}
+				cout <<"companion_person2_dest_y="<<companion_person2_dest_y<<endl;
+
+
+			getline(reader,line); // d_r_1p_x(1,1)= 0.242083
+			getline(reader,line); // d_r_1p_y(1,1)= 1.33599
+			getline(reader,line); // d_r_1p_m(1,1)= 1.35775
+			getline(reader,line); // d_r_1p_x(1,1)= 0.177969
+			getline(reader,line); // d_r_1p_y(1,1)= 2.56626
+			getline(reader,line); // d_r_2p_m(1,1)= 2.57242
+			getline(reader,line); // d_2p_1p_x(1,1)= 0.420052
+			getline(reader,line); // d_2p_1p_y(1,1)= 1.23027
+			getline(reader,line); // d_2p_1p_m(1,1)= 1.3
+			getline(reader,line); // theta_r_p1(1,1)= -79.7698
+			getline(reader,line); // theta_r_p2(1,1)= -94.0148
+			getline(reader,line); // theta_p1_p2(1,1)= -108.907
+
+
+			getline(reader,line); // robot_x(1,1)= 0.462736
+
+			i = line.find(" ");
+			k=line.size();
+			cout <<"; i="<<i<<"; K="<<k<<endl;
+			if (i!=reader.eof()) {
+				string cstr = line.substr(i,k);
+				robot_x = std::stod(cstr.c_str());
+			} else {
+				cout <<"ERROR @ GMap::read num_people_mov"<<endl;
+				exit(EXIT_FAILURE);
+			}
+			cout <<"robot_x="<<robot_x<<endl;
+
+			getline(reader,line); // robot_y(1,1)= -1.33152
+
+			i = line.find(" ");
+			k=line.size();
+			cout <<"; i="<<i<<"; K="<<k<<endl;
+			if (i!=reader.eof()) {
+				string cstr = line.substr(i,k);
+				robot_y = std::stod(cstr.c_str());
+			} else {
+				cout <<"ERROR @ GMap::read num_people_mov"<<endl;
+				exit(EXIT_FAILURE);
+			}
+			cout <<"robot_y="<<robot_y<<endl;
+
+			getline(reader,line); //robot_vx(1,1)= 0
+
+			i = line.find(" ");
+			k=line.size();
+			cout <<"; i="<<i<<"; K="<<k<<endl;
+			if (i!=reader.eof()) {
+				string cstr = line.substr(i,k);
+				robot_vx = std::stod(cstr.c_str());
+			} else {
+				cout <<"ERROR @ GMap::read num_people_mov"<<endl;
+				exit(EXIT_FAILURE);
+			}
+			cout <<"robot_vx="<<robot_vx<<endl;
+
+			getline(reader,line); //robot_vy(1,1)= 0
+
+			i = line.find(" ");
+			k=line.size();
+			cout <<"; i="<<i<<"; K="<<k<<endl;
+			if (i!=reader.eof()) {
+				string cstr = line.substr(i,k);
+				robot_vy = std::stod(cstr.c_str());
+			} else {
+				cout <<"ERROR @ GMap::read num_people_mov"<<endl;
+				exit(EXIT_FAILURE);
+			}
+			cout <<"robot_vy="<<robot_vy<<endl;
+
+			getline(reader,line); //robot_theta(1,1)= 0.0397331
+
+			i = line.find(" ");
+			k=line.size();
+			cout <<"; i="<<i<<"; K="<<k<<endl;
+			if (i!=reader.eof()) {
+				string cstr = line.substr(i,k);
+				robot_theta = std::stod(cstr.c_str());
+			} else {
+				cout <<"ERROR @ GMap::read num_people_mov"<<endl;
+				exit(EXIT_FAILURE);
+			}
+			cout <<"robot_theta="<<robot_theta<<endl;
+
+			getline(reader,line); //robot_v(1,1)= 0
+
+			i = line.find(" ");
+			k=line.size();
+			cout <<"; i="<<i<<"; K="<<k<<endl;
+			if (i!=reader.eof()) {
+				string cstr = line.substr(i,k);
+				robot_v = std::stod(cstr.c_str());
+			} else {
+				cout <<"ERROR @ GMap::read num_people_mov"<<endl;
+				exit(EXIT_FAILURE);
+			}
+			cout <<"robot_v="<<robot_v<<endl;
+
+			getline(reader,line); //robot_w(1,1)= 0
+
+			i = line.find(" ");
+			k=line.size();
+			cout <<"; i="<<i<<"; K="<<k<<endl;
+			if (i!=reader.eof()) { // NO la uso y da problemas
+				string cstr = line.substr(i,k);
+				robot_w = std::stod(cstr.c_str());
+			} else {
+				cout <<"ERROR @ GMap::read num_people_mov"<<endl;
+				exit(EXIT_FAILURE);
+			}
+			cout <<"robot_w="<<robot_w<<endl;
+
+			// Get robot destination
+			getline(reader,line); //robot_final_goal_x(1,1)= 0.462736
+
+			i = line.find(" ");
+			if(i>1000){
+				i=27;
+			}
+			k=line.size();
+			if((k>1000)||(k==0)){
+				k=33;
+			}
+			cout <<"; i="<<i<<"; K="<<k<<endl;
+			if (i!=reader.eof()) {
+				string cstr = line.substr(i,k);
+				robot_dest_x = std::stod(cstr.c_str());
+			} else {
+				cout <<"ERROR @ GMap::read num_people_mov"<<endl;
+				exit(EXIT_FAILURE);
+			}
+			cout <<"robot_dest_x="<<robot_dest_x<<endl;
+
+			getline(reader,line); //robot_final_goal_y(1,1)= -1.33152
+
+			i = line.find(" ");
+			if(i>1000){
+				//i=27;
+				getline(reader,line); //robot_final_goal_y(1,1)= -1.33152
+			}
+			k=line.size();
+			if((k>1000)||(k==0)){
+				//k=33;
+				//getline(reader,line); //robot_final_goal_y(1,1)= -1.33152
+			}
+			cout <<"; i="<<i<<"; K="<<k<<endl;
+			if (k!=reader.eof()) {
+				string cstr = line.substr(i,k);
+				robot_dest_y = std::stod(cstr.c_str());
+			} else {
+				string cstr = line.substr(i,k-1);  //NO LAS USO y dan problemas al coger los datos
+				robot_dest_y = std::stod(cstr.c_str());
+				cout <<"ERROR @ GMap::read num_people_mov"<<endl;
+				exit(EXIT_FAILURE);
+			}
+			cout <<"robot_dest_y="<<robot_dest_y<<endl;
+
+			getline(reader,line); // f_companion_x(1,1)= 0
+			getline(reader,line); // f_companion_y(1,1)= 0
+			getline(reader,line); // f_goal_x(1,1)= 0
+			getline(reader,line); // f_goal_y(1,1)= 0
+			getline(reader,line); // f_companion_x_mult(1,1)= 0
+			getline(reader,line); // f_companion_y_mult(1,1)= 0
+			getline(reader,line); // f_goal_x_mult(1,1)= 0
+			getline(reader,line); // f_goal_y_mult(1,1)= 0
+			getline(reader,line); // f_total_x(1,1)= 0.0243421
+			getline(reader,line); // f_total_y(1,1)= -0.140267
+			getline(reader,line); // f_pers_x(1,1)= 0.0243421
+			getline(reader,line); // f_pers_y(1,1)= -0.140267
+			getline(reader,line); // f_obs_x(1,1)= 0
+			getline(reader,line); // f_obs_y(1,1)= 0
+			getline(reader,line); // final_pose_robot_x(1,1)= 0
+			getline(reader,line);  // final_pose_robot_y(1,1)= 0
+			getline(reader,line); // final_pose_robot_v(1,1)= 0
+			getline(reader,line); // final_pose_robot_w(1,1)= 0
+			getline(reader,line); // final_pose_robot_theta(1,1)= 0
+			getline(reader,line); //robot_performance(1,1)= nan
+
+	}
+	//reader.close();
+
+	State2D Self;
+// TODO: guardar id, personas en los proximos archivos de datos!!! IMPORTANTE!!!!
+
+	Spose pose_of_the_robot=Spose( robot_x , robot_y , real_computation_time_dt , robot_theta , robot_v, robot_w);//robot_->get_current_pose();
+	SpointV point_of_the_robot=SpointV( robot_x , robot_y, real_computation_time_dt, robot_vx, robot_vy ); //robot_->get_current_pointV();
+	Self.Init(pose_of_the_robot.x,pose_of_the_robot.y,point_of_the_robot.vx,point_of_the_robot.vy,pose_of_the_robot.theta);
+
+	std::cout <<"(Performance) Self.r.m="<<Self.r.m<< "; Self.r.x="<<Self.r.x<<"; Self.r.y="<<Self.r.y<<"; Self.r.th"<<Self.r.th*180/3.14<<"; pose_of_the_robot.theta*180/3.14="<<pose_of_the_robot.theta*180/3.14<<std::endl;
+	std::cout <<"(Performance) Self.v.m="<<Self.v.m<< "; Self.v.x="<<Self.v.x<<"; Self.v.y="<<Self.v.y<<"; Self.v.th"<<Self.v.th*180/3.14<<std::endl;
+	//Self.r.th=pose_of_the_robot.theta;
+
+	unsigned int number_of_group_people_=2;
+	State2D *Others=new State2D[number_of_group_people_];
+	//std::cout << " *Others=new State2D[number_of_group_people_]"<<std::endl;
+	SpointV_cov actual_first_person_companion_point=SpointV(companion_person_x,companion_person_y,real_computation_time_dt,companion_person_vx,companion_person_vy);//pointer_to_person_companion_->get_current_pointV();
+	State2D first_companion_person(actual_first_person_companion_point.x,actual_first_person_companion_point.y,actual_first_person_companion_point.vx,actual_first_person_companion_point.vy);
+	//first_companion_person.r.th=pose_of_the_robot.theta;
+	Others[0]=first_companion_person;
+	std::cout <<"(FIRST) id_person="<< " first_companion_person.r.x="<<first_companion_person.r.x<<"; first_companion_person.r.y="<<first_companion_person.r.y<<"; first_companion_person.v.x="<<first_companion_person.v.x<<"; first_companion_person.v.y="<<first_companion_person.v.y<<"; th="<<first_companion_person.r.th*180/3.14<<std::endl;
+
+
+	SpointV_cov actual_second_person_companion_point=SpointV_cov(companion2_person_x,companion2_person_y,real_computation_time_dt,companion2_person_vx,companion2_person_vy);//=second_group_companion_person_obj_->get_prediction_trajectory()->at(parent_vertex);
+	//if((we_have_pointer_to_second_person_)&&(number_of_group_people_>1)){
+		//actual_second_person_companion_point=//second_group_companion_person_obj_->get_current_pointV();
+		State2D second_companion_person(actual_second_person_companion_point.x,actual_second_person_companion_point.y,actual_second_person_companion_point.vx,actual_second_person_companion_point.vy);
+		//second_companion_person.r.th=pose_of_the_robot.theta;
+
+		Others[1]=second_companion_person;
+		std::cout <<"(SECOND) id_person="<< " second_companion_person.r.x="<<second_companion_person.r.x<<"; second_companion_person.r.y="<<second_companion_person.r.y<<"; second_companion_person.v.x="<<second_companion_person.v.x<<"; second_companion_person.v.y="<<second_companion_person.v.y<<"; th="<<second_companion_person.r.th*180/3.14<<std::endl;
+	//}
+	Vector2D preferred;
+	double x_orient_goal,y_orient_goal,orient_goal;
+			//if((number_of_group_people_>1)&&(we_have_pointer_to_second_person_)){
+				x_orient_goal=companion_person_dest_x - companion_person_x;
+				y_orient_goal=companion_person_dest_y - companion_person_y;
+				orient_goal=atan(y_orient_goal/x_orient_goal);
+				if(isnan((float)orient_goal)){ // si es nan el calculo, quetate con la orientacion actual del robot. (caso inicio generalmente)
+					orient_goal=robot_theta;
+				}
+				//preferred=Vector2D(x_orient_goal,y_orient_goal,orient_goal); // is the orientation until the goal of the group.
+				if((companion_person_vx!=0)&&(companion2_person_vx!=0)&&(companion_person_vy!=0)&&(companion2_person_vy!=0)){
+					preferred=Vector2D((companion_person_vx+companion2_person_vx)/2,(companion_person_vy+companion2_person_vy)/2);
+				}else{
+					x_orient_goal=companion_person_dest_x - companion_person_x;
+					y_orient_goal=companion_person_dest_y - companion_person_y;
+					orient_goal=atan(y_orient_goal/x_orient_goal);
+					if(isnan((float)orient_goal)){ // si es nan el calculo, quetate con la orientacion actual del robot. (caso inicio generalmente)
+						orient_goal=robot_theta;
+					}
+					preferred=Vector2D(x_orient_goal,y_orient_goal,orient_goal); // is the orientation until the goal of the group.
+				}
+
+				std::cout << "(caso1 save in file) preferred.x= "<<preferred.x<<"; preferred.y="<<preferred.y<<"; preferred.m="<<preferred.m<<"; preferred.th="<<preferred.th<< std::endl;
+							//}
+				// TODO: mirar que esten seteadas todas las constantes por defecto.
+	 double vp=Companion_Zanlungo_Model_.f2_vp2v(preferred.m);//f2_vp2v(0.4);
+
+	 preferred.Scale(vp/preferred.m);
+	// vp=robot_->get_current_pose().v;
+	 //preferred=Vector2D(0,vp);//la velocidad preferida
+	 std::cout << "(caso2 save in file. ahora uso caso 1) preferred.x= "<<preferred.x<<"; preferred.y="<<preferred.y<<"; preferred.m="<<preferred.m<<"; preferred.th="<<preferred.th*180/3.14<<"; vp="<<vp<< std::endl;
+
+	 double actual_performance=Companion_Zanlungo_Model_.Exmet3(Self,Others,preferred);
+
+	 std::cout << "actual_performance="<<actual_performance<< std::endl;
+
+	 	 if(actual_performance!=0){ //saltatela. Pasan por como guarde los datos, no porque de verdad sea 0, es cuando se paraba la simulacion.
+	 		actual_robot_performance_.push_back(actual_performance); // performace por experimento; iteration.
+	 	 }
+
+
+
+	// }
+	firs_experiment=experiment_;
+
+	// Calcular media:
+	double sum=0.0;
+	for(unsigned int t=0; t<actual_robot_performance_.size();t++){
+
+		if(isnan((float)actual_robot_performance_.at(t))){
+		}else{
+			sum=sum+actual_robot_performance_.at(t);
+			std::cout << "sum="<<sum<<"; actual_robot_performance_="<<actual_robot_performance_.at(t)<< std::endl;
+		}
+
+	}
+	double media_one_experiment=sum;// /actual_robot_performance_.size();
+	std::cout << "media_one_experiment="<<media_one_experiment<<"; sum/actual_robot_performance_.size()="<<sum/actual_robot_performance_.size()<< std::endl;
+
+	if(isnan((float)media_one_experiment)){
+
+	}else{
+
+	}
+
+	// calcular std: (TODO: calcularla!!!)
+	double ste=0.0;
+	for(unsigned int t=0; t<actual_robot_performance_.size();t++){
+		if(isnan((float)actual_robot_performance_.at(t))){
+		}else{
+			ste=ste+(actual_robot_performance_.at(t)-media_one_experiment)*(actual_robot_performance_.at(t)-media_one_experiment);
+		}
+
+	}
+	double final_std=ste; // /(actual_robot_performance_.size());
+	std::cout << "final_std="<<final_std<< std::endl;
+
+	if((isnan((float)media_one_experiment))||(isnan((float)final_std))){ // evitar casos raros que no quite de los datos, donde se paraba la simulacion.
+
+	}else{
+		if((media_one_experiment!=0)){
+			all_means_of_performances_ex_.push_back(media_one_experiment);
+			all_std_of_performances_ex_.push_back(final_std);
+		}
+
+	}
+
+
+	 actual_robot_performance_.clear();
+	 iteration_++;
+	}
+
+	double sum2=0.0;
+	for(unsigned int t=0; t<all_means_of_performances_ex_.size();t++){
+		sum2=sum2+all_means_of_performances_ex_.at(t);
+		std::cout << "sum2="<<sum2<<"; all_means_of_performances_ex_="<<all_means_of_performances_ex_.at(t)<< std::endl;
+	}
+	double media_all_experiment=sum2/all_means_of_performances_ex_.size();
+
+	// calcular std: (TODO: calcularla!!!)
+	double sum3=0.0;
+	for(unsigned int t=0; t<all_std_of_performances_ex_.size();t++){
+		if((all_std_of_performances_ex_.at(t)<1)&&(all_std_of_performances_ex_.at(t)>0)){
+			sum3=sum3+all_std_of_performances_ex_.at(t);
+		}
+
+		std::cout << "sum3="<<sum3<<"; all_std_of_performances_ex_="<<all_std_of_performances_ex_.at(t)<< std::endl;
+	}
+	double media_all_std=sum3/all_std_of_performances_ex_.size();
+
+	/// INI TO calculate real exp std:
+	double ste2=0;
+	for(unsigned int t=0; t<all_means_of_performances_ex_.size();t++){
+			if(isnan((float)all_means_of_performances_ex_.at(t))){
+			}else{
+				ste2=ste2+(all_means_of_performances_ex_.at(t)-media_all_experiment)*(all_means_of_performances_ex_.at(t)-media_all_experiment);
+				 std::cout << "ste2="<<ste2<< std::endl;
+			}
+
+		}
+	media_all_std=ste2/all_means_of_performances_ex_.size();
+	/// FIN TO calculate real exp std:
+
+	 std::cout << "media_all_experiment="<<media_all_experiment<<"; media_all_std="<<media_all_std<< std::endl;
+
+
+
+
+
+/*
+	std::string  data_file_Zanlungo_="/home/erepiso/iri-lab/iri_ws/src/iri_navigation/iri_atr_akp_local_planner_companion/txt_data/data_companion.txt";
+
+
+
+
+
+	std::ofstream fileMatlab;
+	fileMatlab.open (data_file_Zanlungo_.c_str(), std::ofstream::out | std::ofstream::app);
+	// const char *direc_file_name_;
+	//direc_file_name_= results_file_.c_str();
+	//home/ely7787/iri-lab/labrobotica/restricted/algorithms/people_prediction/branches/ely_people_prediction_companion_robot/1_data_results
+	//fileMatlab.open (results_file_.c_str(), std::ofstream::out | std::ofstream::app);
+	//fileMatlab.open (data_file_Zanlungo_.c_str(), std::ofstream::out | std::ofstream::trunc);
+	fileMatlab << "\n";
+
+	fileMatlab << "robot_performance("<<experiment_<<","<<iteration_<<")= "<<actual_robot_performance_<<"\n";
+*/
+	return 0;
+}
diff --git a/local_lib_people_prediction/src/examples/generate_performance_from_data_exampleIROS2019.cpp b/local_lib_people_prediction/src/examples/generate_performance_from_data_exampleIROS2019.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..be7801c9b7aa7e0abd449629bd37c542cac80a32
--- /dev/null
+++ b/local_lib_people_prediction/src/examples/generate_performance_from_data_exampleIROS2019.cpp
@@ -0,0 +1,1028 @@
+/*
+ * generate_performance_from_data_example.cpp
+ *
+ *  Created on: 7-Sep-2018
+ *      Author: errepiso
+ */
+
+#include "nav/plan_local_nav.h"
+#include "nav/force_reactive_robot_companion.h"
+#include "scene_sim.h"
+#include <iostream>
+#include "companion_zanlungo/CompanionFrancescoModel.h"
+
+int main(int argc,char *argv[])
+{
+	  CompanionFrancescoModel Companion_Zanlungo_Model_;
+
+
+	    double in_Cr, in_r0, in_Ct, in_eta, in_rmax, in_S0;
+	    in_Cr=0.62;
+	    in_Ct=0.08;
+	    in_r0=1.5;
+	    in_eta=-0.43;
+	    in_rmax=3;
+	    in_S0=0.5;
+	    Cgenome genome;
+
+	    genome.Init(in_Cr, in_r0, in_Ct, in_eta, in_rmax, in_S0);
+
+	    Companion_Zanlungo_Model_.set_genome_params_in_CompanionFrancescoModel(genome);
+	    Companion_Zanlungo_Model_.set_internal_k_value(1.5);
+
+	// TODO: falta ver como cargar los datos del fichero txt ahora tal como esta.
+
+	std::string file_name_s="~/iri-lab/iri_ws/src/iri_navigation/iri_atr_akp_local_planner_companion/txt_data/1_V_formation_lateral_sin_obstaculos/results_Vform_companion_OK.txt";
+	const char * file_name=file_name_s.c_str();
+
+	std::vector<double> actual_robot_performance_;
+	std::vector<double> all_means_of_performances_ex_;
+	std::vector<double> all_std_of_performances_ex_;
+
+	std::vector<double> actual_robot_performance1_;
+	std::vector<double> all_means_of_performances_ex1_;
+	std::vector<double> all_std_of_performances_ex1_;
+
+
+	std::vector<double> actual_robot_performance2_;
+	std::vector<double> all_means_of_performances_ex2_;
+	std::vector<double> all_std_of_performances_ex2_;
+
+	std::cout << " OOOOOOOOOOOO77777777777777OOOOOOOOOOOOOOOO file_name="<<file_name<<std::endl;
+
+	//ifstream reader;//(file_name);
+	fstream reader;
+	//reader.open(file_name_s);
+	reader.open("results_person_companion_Vform9_central.txt"); //results_robot11.txt
+	 //cout <<"open_file="<<open_file<<endl;
+	unsigned int experiment_,iteration_;
+	experiment_=1; // (initial experiment)
+	iteration_=170; // (INITIAL ITER)
+	unsigned int max_exp=132; //275-129; // until max_exp; (FINAL ITER)
+	unsigned int firs_experiment=1; //
+	bool only_one_person_=false;
+
+
+	double real_computation_time_dt;
+
+	double companion_person_x;
+	double companion_person_y;
+	double companion_person_vx;
+	double companion_person_vy;
+
+	double companion_person_dest_x;
+	double companion_person_dest_y;
+
+	double companion2_person_x;
+	double companion2_person_y;
+	double companion2_person_vx;
+	double companion2_person_vy;
+
+	double companion_person2_dest_x;
+	double companion_person2_dest_y;
+
+	double robot_x;
+	double robot_y;
+	double robot_vx;
+	double robot_vy;
+	double robot_theta;
+	double robot_v;
+	double robot_w;
+
+	double robot_dest_x;
+	double robot_dest_y;
+
+	double robot_performance1;
+	double robot_performance2;
+	// double robot_performance;
+
+
+cout <<"antes bucle experiments ="<<endl;
+	//while( experiment_< max_exp){
+while( iteration_< max_exp){
+		 cout <<"antes bucle iterations"<<endl;
+		// while(experiment_==firs_experiment){
+
+		 cout <<"reader.good()="<<reader.good()<<endl;
+	// TODO: coger valores de arriba del fichero. Iterativamente.
+	if (reader.good()) {
+
+		/// INICIO BUCLE, tendria que ser desde aquí
+		//###############################
+		//# experiment_ and  iteration_ #
+		//###############################
+		 string line;
+		 getline(reader,line);
+		 getline(reader,line);
+		 cout <<"(1)"<<endl;
+		 //experiment_=-1;
+		 cout <<"(2)"<<endl;
+		 //find first comma, read #rows
+			 string::size_type i = line.find("(");
+			 cout <<"(3)"<<endl;
+			 if (i!=reader.eof()) {
+				 cout <<"(4); i="<<i<<endl;
+			 	string cstr = line.substr(i+1,i+2);
+			 	 cout <<"(5)"<<endl;
+			 	experiment_ = atoi(cstr.c_str());
+			 	 cout <<"(6)"<<endl;
+			 } else {
+			 	cout <<"ERROR @ GMap::read num_people_mov"<<endl;
+			 	exit(EXIT_FAILURE);
+			 }
+			 /*if(experiment_==0){
+				// getline(reader,line);
+				 cout <<"(1)"<<endl;
+						 experiment_=-1;
+						 cout <<"(2)"<<endl;
+						 //find first comma, read #rows
+							 string::size_type i = line.find("(");
+							 cout <<"(3)"<<endl;
+							 if (i!=reader.eof()) {
+								 cout <<"(4); i="<<i<<endl;
+							 	string cstr = line.substr(i+1,i+2);
+							 	 cout <<"(5)"<<endl;
+							 	experiment_ = atoi(cstr.c_str());
+							 	 cout <<"(6)"<<endl;
+							 } else {
+							 	cout <<"ERROR @ GMap::read num_people_mov"<<endl;
+							 	exit(EXIT_FAILURE);
+							 }
+			 }*/
+			 cout <<"experiment_="<<experiment_<<endl;
+			// p=experiment_;
+
+			 string::size_type j = line.find(")");
+			 /*if(j>1000){
+				 j=7;
+			 }*/
+
+			 cout <<"j="<<j<<endl;
+			 if (j!=reader.eof()) {
+			 	//string cstr = line.substr(j-1,j);
+			 	///iteration_ = atoi(cstr.c_str());
+			 } else {
+			 	cout <<"ERROR @ GMap::read num_people_mov"<<endl;
+			 	exit(EXIT_FAILURE);
+			 }
+			 cout <<"iteration_="<<iteration_<<endl;
+
+			/* if(iteration_==2){
+				 cout <<"iteration_="<<iteration_<<"; firs_experiment="<<experiment_<<endl;
+				 firs_experiment=experiment_;
+			 }*/
+
+			 getline(reader,line);
+			 i = line.find(" ");
+			 unsigned int k=line.size();
+			 cout <<"; i="<<i<<"; K="<<k<<endl;
+			 if (i!=reader.eof()) {
+				//string cstr = line.substr(i,k);
+			 	//real_computation_time_dt = std::stod(cstr.c_str());
+
+			 } else {
+			 	cout <<"ERROR @ GMap::read num_people_mov"<<endl;
+			 	exit(EXIT_FAILURE);
+			 }
+				cout <<"real_computation_time_dt="<<real_computation_time_dt<<endl;
+		//###########################
+		//# Read companion person1  #
+		//###########################
+
+				//# Read companion person1 position #
+			getline(reader,line);
+			i = line.find(" ");
+			k=line.size();
+			cout <<"; i="<<i<<"; K="<<k<<endl;
+			 if (i!=reader.eof()) {
+				string cstr = line.substr(i,k);
+				companion_person_x = std::stod(cstr.c_str());
+
+			 } else {
+				cout <<"ERROR @ GMap::read num_people_mov"<<endl;
+				exit(EXIT_FAILURE);
+			}
+			cout <<"companion_person_x="<<companion_person_x<<endl;
+
+			getline(reader,line);
+			i = line.find(" ");
+			k=line.size();
+			cout <<"; i="<<i<<"; K="<<k<<endl;
+			 if (i!=reader.eof()) {
+				string cstr = line.substr(i,k);
+				companion_person_y = std::stod(cstr.c_str());
+
+			 } else {
+				cout <<"ERROR @ GMap::read num_people_mov"<<endl;
+				exit(EXIT_FAILURE);
+			}
+			cout <<"companion_person_y="<<companion_person_y<<endl;
+
+			//# Read companion person1 velocities #
+			getline(reader,line);
+			i = line.find(" ");
+			k=line.size();
+			cout <<"; i="<<i<<"; K="<<k<<endl;
+			 if (i!=reader.eof()) {
+				string cstr = line.substr(i,k);
+				companion_person_vx = std::stod(cstr.c_str());
+
+			 } else {
+				cout <<"ERROR @ GMap::read num_people_mov"<<endl;
+				exit(EXIT_FAILURE);
+			}
+			cout <<"companion_person_vx="<<companion_person_vx<<endl;
+
+			getline(reader,line);
+			i = line.find(" ");
+			k=line.size();
+			cout <<"; i="<<i<<"; K="<<k<<endl;
+			 if (i!=reader.eof()) {
+				string cstr = line.substr(i,k);
+				companion_person_vy = std::stod(cstr.c_str());
+
+			 } else {
+				cout <<"ERROR @ GMap::read num_people_mov"<<endl;
+				exit(EXIT_FAILURE);
+			}
+			cout <<"companion_person_vy="<<companion_person_vy<<endl;
+
+			getline(reader,line); // companion_person_theta(1,1)= -nan
+
+			//# Read companion person1 destinations #
+			getline(reader,line); // companion_person_dest_x(1,1)= 0.220653
+
+			i = line.find(" ");
+			k=line.size();
+			if(k>1000){
+				k=38;
+			}
+			cout <<"; i="<<i<<"; K="<<k<<endl;
+			 if (i!=reader.eof()) {
+				string cstr = line.substr(i,k);
+				companion_person_dest_x = std::stod(cstr.c_str());
+
+			 } else {
+				cout <<"ERROR @ GMap::read num_people_mov"<<endl;
+				exit(EXIT_FAILURE);
+			}
+			cout <<"companion_person_dest_x="<<companion_person_dest_x<<endl;
+
+			getline(reader,line); // companion_person_dest_x(1,1)= 0.220653
+
+			i = line.find(" ");
+			k=line.size();
+			if(k>1000){
+				k=38;
+			}
+			cout <<"; i="<<i<<"; K="<<k<<endl;
+			 if (i!=reader.eof()) {
+				string cstr = line.substr(i,k);
+				companion_person_dest_y = std::stod(cstr.c_str());
+
+			 } else {
+				cout <<"ERROR @ GMap::read num_people_mov"<<endl;
+				exit(EXIT_FAILURE);
+			}
+			cout <<"companion_person_dest_y="<<companion_person_dest_y<<endl;
+
+			if(!only_one_person_){
+			//###########################
+			//# Read companion person2  #
+			//###########################
+					//# Read companion person2 position #
+				getline(reader,line);
+				i = line.find(" ");
+				k=line.size();
+				cout <<"; i="<<i<<"; K="<<k<<endl;
+				 if (i!=reader.eof()) {
+					string cstr = line.substr(i,k);
+					companion2_person_x = std::stod(cstr.c_str());
+
+				 } else {
+					cout <<"ERROR @ GMap::read num_people_mov"<<endl;
+					exit(EXIT_FAILURE);
+				}
+				cout <<"companion2_person_x="<<companion2_person_x<<endl;
+
+				getline(reader,line);
+				i = line.find(" ");
+				k=line.size();
+				cout <<"; i="<<i<<"; K="<<k<<endl;
+				 if (i!=reader.eof()) {
+					string cstr = line.substr(i,k);
+					companion2_person_y = std::stod(cstr.c_str());
+
+				 } else {
+					cout <<"ERROR @ GMap::read num_people_mov"<<endl;
+					exit(EXIT_FAILURE);
+				}
+				cout <<"companion2_person_y="<<companion2_person_y<<endl;
+
+				//# Read companion person2 velocities #
+				getline(reader,line);
+				i = line.find(" ");
+				k=line.size();
+				cout <<"; i="<<i<<"; K="<<k<<endl;
+				 if (i!=reader.eof()) {
+					string cstr = line.substr(i,k);
+					companion2_person_vx = std::stod(cstr.c_str());
+
+				 } else {
+					cout <<"ERROR @ GMap::read num_people_mov"<<endl;
+					exit(EXIT_FAILURE);
+				}
+				cout <<"companion2_person_vx="<<companion2_person_vx<<endl;
+
+				getline(reader,line);
+				i = line.find(" ");
+				k=line.size();
+				cout <<"; i="<<i<<"; K="<<k<<endl;
+				 if (i!=reader.eof()) {
+					string cstr = line.substr(i,k);
+					companion2_person_vy = std::stod(cstr.c_str());
+
+				 } else {
+					cout <<"ERROR @ GMap::read num_people_mov"<<endl;
+					exit(EXIT_FAILURE);
+				}
+				cout <<"companion2_person_vy="<<companion2_person_vy<<endl;
+
+				getline(reader,line); // companion_person_theta(1,1)= -nan
+
+				//# Read companion person2 destinations #
+				getline(reader,line); // companion_person_dest_x(1,1)= 0.220653
+
+				i = line.find(" ");
+				k=line.size();
+				if(k>1000){
+					k=39;
+				}
+				cout <<"; i="<<i<<"; K="<<k<<endl;
+				 if (i!=reader.eof()) {
+					string cstr = line.substr(i,k);
+					companion_person2_dest_x = std::stod(cstr.c_str());
+
+				 } else {
+					cout <<"ERROR @ GMap::read num_people_mov"<<endl;
+					exit(EXIT_FAILURE);
+				}
+				cout <<"companion_person2_dest_x="<<companion_person2_dest_x<<endl;
+
+				getline(reader,line); // companion_person_dest_x(1,1)= 0.220653
+
+				i = line.find(" ");
+				k=line.size();
+				cout <<"; i="<<i<<"; K="<<k<<endl;
+				if(k>1000){
+					k=39;
+				}
+				 if (i!=reader.eof()) {
+					string cstr = line.substr(i,k);
+					companion_person2_dest_y = std::stod(cstr.c_str());
+
+				 } else {
+					cout <<"ERROR @ GMap::read num_people_mov"<<endl;
+					exit(EXIT_FAILURE);
+				}
+				cout <<"companion_person2_dest_y="<<companion_person2_dest_y<<endl;
+			}
+
+			getline(reader,line); // d_r_1p_x(1,1)= 0.242083
+			getline(reader,line); // d_r_1p_y(1,1)= 1.33599
+			getline(reader,line); // d_r_1p_m(1,1)= 1.35775
+
+			if(!only_one_person_){
+				getline(reader,line); // d_r_1p_x(1,1)= 0.177969
+				getline(reader,line); // d_r_1p_y(1,1)= 2.56626
+				getline(reader,line); // d_r_2p_m(1,1)= 2.57242
+				getline(reader,line); // d_2p_1p_x(1,1)= 0.420052
+				getline(reader,line); // d_2p_1p_y(1,1)= 1.23027
+				getline(reader,line); // d_2p_1p_m(1,1)= 1.3
+			}
+			getline(reader,line); // theta_r_p1(1,1)= -79.7698
+			if(!only_one_person_){
+				getline(reader,line); // theta_r_p2(1,1)= -94.0148
+				getline(reader,line); // theta_p1_p2(1,1)= -108.907
+			}
+
+
+			getline(reader,line); // robot_x(1,1)= 0.462736
+
+			i = line.find(" ");
+			k=line.size();
+			cout <<"; i="<<i<<"; K="<<k<<endl;
+			if (i!=reader.eof()) {
+				string cstr = line.substr(i,k);
+				robot_x = std::stod(cstr.c_str());
+			} else {
+				cout <<"ERROR @ GMap::read num_people_mov"<<endl;
+				exit(EXIT_FAILURE);
+			}
+			cout <<"robot_x="<<robot_x<<endl;
+
+			getline(reader,line); // robot_y(1,1)= -1.33152
+
+			i = line.find(" ");
+			k=line.size();
+			cout <<"; i="<<i<<"; K="<<k<<endl;
+			if (i!=reader.eof()) {
+				string cstr = line.substr(i,k);
+				robot_y = std::stod(cstr.c_str());
+			} else {
+				cout <<"ERROR @ GMap::read num_people_mov"<<endl;
+				exit(EXIT_FAILURE);
+			}
+			cout <<"robot_y="<<robot_y<<endl;
+
+			getline(reader,line); //robot_vx(1,1)= 0
+
+			i = line.find(" ");
+			k=line.size();
+			cout <<"; i="<<i<<"; K="<<k<<endl;
+			if (i!=reader.eof()) {
+				string cstr = line.substr(i,k);
+				robot_vx = std::stod(cstr.c_str());
+			} else {
+				cout <<"ERROR @ GMap::read num_people_mov"<<endl;
+				exit(EXIT_FAILURE);
+			}
+			cout <<"robot_vx="<<robot_vx<<endl;
+
+			getline(reader,line); //robot_vy(1,1)= 0
+
+			i = line.find(" ");
+			k=line.size();
+			cout <<"; i="<<i<<"; K="<<k<<endl;
+			if (i!=reader.eof()) {
+				string cstr = line.substr(i,k);
+				robot_vy = std::stod(cstr.c_str());
+			} else {
+				cout <<"ERROR @ GMap::read num_people_mov"<<endl;
+				exit(EXIT_FAILURE);
+			}
+			cout <<"robot_vy="<<robot_vy<<endl;
+
+			getline(reader,line); //robot_theta(1,1)= 0.0397331
+
+			i = line.find(" ");
+			k=line.size();
+			cout <<"; i="<<i<<"; K="<<k<<endl;
+			if (i!=reader.eof()) {
+				string cstr = line.substr(i,k);
+				robot_theta = std::stod(cstr.c_str());
+			} else {
+				cout <<"ERROR @ GMap::read num_people_mov"<<endl;
+				exit(EXIT_FAILURE);
+			}
+			cout <<"robot_theta="<<robot_theta<<endl;
+
+			getline(reader,line); //robot_v(1,1)= 0
+
+			i = line.find(" ");
+			k=line.size();
+			cout <<"; i="<<i<<"; K="<<k<<endl;
+			if (i!=reader.eof()) {
+				string cstr = line.substr(i,k);
+				robot_v = std::stod(cstr.c_str());
+			} else {
+				cout <<"ERROR @ GMap::read num_people_mov"<<endl;
+				exit(EXIT_FAILURE);
+			}
+			cout <<"robot_v="<<robot_v<<endl;
+
+			getline(reader,line); //robot_w(1,1)= 0
+
+			i = line.find(" ");
+			k=line.size();
+			cout <<"; i="<<i<<"; K="<<k<<endl;
+			if (i!=reader.eof()) { // NO la uso y da problemas
+				string cstr = line.substr(i,k);
+				robot_w = std::stod(cstr.c_str());
+			} else {
+				cout <<"ERROR @ GMap::read num_people_mov"<<endl;
+				exit(EXIT_FAILURE);
+			}
+			cout <<"robot_w="<<robot_w<<endl;
+
+			// Get robot destination
+			getline(reader,line); //robot_final_goal_x(1,1)= 0.462736
+
+			i = line.find(" ");
+			if(i>1000){
+				i=27;
+			}
+			k=line.size();
+			if((k>1000)||(k==0)){
+				k=33;
+			}
+			cout <<"; i="<<i<<"; K="<<k<<endl;
+			if (i!=reader.eof()) {
+				string cstr = line.substr(i,k);
+				robot_dest_x = std::stod(cstr.c_str());
+			} else {
+				cout <<"ERROR @ GMap::read num_people_mov"<<endl;
+				exit(EXIT_FAILURE);
+			}
+			cout <<"robot_dest_x="<<robot_dest_x<<endl;
+
+			getline(reader,line); //robot_final_goal_y(1,1)= -1.33152
+
+			i = line.find(" ");
+			if(i>1000){
+				//i=27;
+				getline(reader,line); //robot_final_goal_y(1,1)= -1.33152
+			}
+			k=line.size();
+			if((k>1000)||(k==0)){
+				//k=33;
+				//getline(reader,line); //robot_final_goal_y(1,1)= -1.33152
+			}
+			cout <<"; i="<<i<<"; K="<<k<<endl;
+			if (k!=reader.eof()) {
+				string cstr = line.substr(i,k);
+				robot_dest_y = std::stod(cstr.c_str());
+			} else {
+				string cstr = line.substr(i,k-1);  //NO LAS USO y dan problemas al coger los datos
+				robot_dest_y = std::stod(cstr.c_str());
+				cout <<"ERROR @ GMap::read num_people_mov"<<endl;
+				exit(EXIT_FAILURE);
+			}
+			cout <<"robot_dest_y="<<robot_dest_y<<endl;
+
+			getline(reader,line); // f_companion_x(1,1)= 0
+			getline(reader,line); // f_companion_y(1,1)= 0
+			getline(reader,line); // f_goal_x(1,1)= 0
+			getline(reader,line); // f_goal_y(1,1)= 0
+			getline(reader,line); // f_companion_x_mult(1,1)= 0
+			getline(reader,line); // f_companion_y_mult(1,1)= 0
+			getline(reader,line); // f_goal_x_mult(1,1)= 0
+			getline(reader,line); // f_goal_y_mult(1,1)= 0
+			getline(reader,line); // f_total_x(1,1)= 0.0243421
+			getline(reader,line); // f_total_y(1,1)= -0.140267
+			getline(reader,line); // f_pers_x(1,1)= 0.0243421
+			getline(reader,line); // f_pers_y(1,1)= -0.140267
+			getline(reader,line); // f_obs_x(1,1)= 0
+			getline(reader,line); // f_obs_y(1,1)= 0
+			getline(reader,line); // final_pose_robot_x(1,1)= 0
+			getline(reader,line);  // final_pose_robot_y(1,1)= 0
+			getline(reader,line); // final_pose_robot_v(1,1)= 0
+			getline(reader,line); // final_pose_robot_w(1,1)= 0
+			getline(reader,line); // final_pose_robot_theta(1,1)= 0
+			getline(reader,line); //preferred_performance_act_x(1,1)= -nan
+			getline(reader,line); //preferred_performance_act_y(1,1)= -nan
+			getline(reader,line); //preferred_performance_act_x(1,1)= -nan
+			getline(reader,line); //preferred_performance_act_x(1,1)= -nan
+			getline(reader,line); //vp_performance_act_(1,1)= 0.189598
+			getline(reader,line); //Self_performance_act_r_x_(1,1)= -0.226722
+			getline(reader,line); //Self_performance_act_r_y(1,1)= -1.47844
+			getline(reader,line); //Self_performance_act_r_m(1,1)= 1.49573
+			getline(reader,line); //Self_performance_act_r_th(1,1)= -2.98943
+			getline(reader,line); //Self_performance_act_v_x(1,1)= 0
+			getline(reader,line); //Self_performance_act_v_y(1,1)= 0
+			getline(reader,line); //Self_performance_act_v_m(1,1)= 0
+			getline(reader,line); //Self_performance_act_v_th(1,1)= 0.0512502
+			getline(reader,line); //Others_performance_act_p1_r_x_(1,1)= 0.375384
+			getline(reader,line); //Others_performance_act_p1_r_y(1,1)= 0.00682208
+			getline(reader,line); //Others_performance_act_p1_r_m(1,1)= 0.375446
+			getline(reader,line); //Others_performance_act_[0]_p1_r_th(1,1)= 1.55262
+			getline(reader,line); //Others_performance_act_[0]_p1_v_x(1,1)= 0
+			getline(reader,line); //Others_performance_act_[0]_p1_v_y(1,1)= 0
+			getline(reader,line); //Others_performance_act_[0]_p1_v_m(1,1)= 0
+			getline(reader,line); //Others_performance_act_[0]_p1_v_th(1,1)= 0
+
+			if(!only_one_person_){
+				getline(reader,line); //Others_performance_act_[1]_p2_r_x_(1,1)= 11.6971
+				getline(reader,line); //Others_performance_act_[1]_p2_r_y(1,1)= 22.641
+				getline(reader,line); //Others_performance_act_[1]_p2_r_m(1,1)= 25.4841
+				getline(reader,line); //Others_performance_act_[1]_p2_r_th(1,1)= 0.476864
+				getline(reader,line); //Others_performance_act_[1]_p2_v_x(1,1)= 0.000860374
+				getline(reader,line); //Others_performance_act_[1]_p2_v_y(1,1)= -0.000556119
+				getline(reader,line); //Others_performance_act_[1]_p2_v_m(1,1)= 0.00102446
+				getline(reader,line); //Others_performance_act_[1]_p2_v_th(1,1)= 2.14461
+			}
+
+			getline(reader,line); //robot_performance(1,1)= nan
+			i = line.find(" ");
+			if(i>1000){
+				//i=27;
+				getline(reader,line); //robot_final_goal_y(1,1)= -1.33152
+			}
+			k=line.size();
+			if((k>1000)||(k==0)){
+				//k=33;
+				//getline(reader,line); //robot_final_goal_y(1,1)= -1.33152
+			}
+			cout <<"; i="<<i<<"; K="<<k<<endl;
+			if (k!=reader.eof()) {
+				string cstr = line.substr(i,k);
+				robot_performance1  = std::stod(cstr.c_str());
+			} else {
+				string cstr = line.substr(i,k-1);  //NO LAS USO y dan problemas al coger los datos
+				robot_performance1 = std::stod(cstr.c_str());
+				cout <<"ERROR @ GMap::read num_people_mov"<<endl;
+				exit(EXIT_FAILURE);
+			}
+			cout <<"robot_performance1="<<robot_performance1<<endl;
+
+			getline(reader,line); //robot_performance2(1,1)= nan
+			i = line.find(" ");
+			if(i>1000){
+				//i=27;
+				getline(reader,line); //robot_final_goal_y(1,1)= -1.33152
+			}
+			k=line.size();
+			if((k>1000)||(k==0)){
+				//k=33;
+				//getline(reader,line); //robot_final_goal_y(1,1)= -1.33152
+			}
+			cout <<"; i="<<i<<"; K="<<k<<endl;
+			if (k!=reader.eof()) {
+				string cstr = line.substr(i,k);
+				robot_performance2  = std::stod(cstr.c_str());
+			} else {
+				string cstr = line.substr(i,k-1);  //NO LAS USO y dan problemas al coger los datos
+				robot_performance2 = std::stod(cstr.c_str());
+				cout <<"ERROR @ GMap::read num_people_mov"<<endl;
+				exit(EXIT_FAILURE);
+			}
+			cout <<"robot_performance2="<<robot_performance2<<endl;
+	}
+	//reader.close();
+
+	State2D Self;
+// TODO: guardar id, personas en los proximos archivos de datos!!! IMPORTANTE!!!!
+
+	Spose pose_of_the_robot=Spose( robot_x , robot_y , real_computation_time_dt , robot_theta , robot_v, robot_w);//robot_->get_current_pose();
+	SpointV point_of_the_robot=SpointV( robot_x , robot_y, real_computation_time_dt, robot_vx, robot_vy ); //robot_->get_current_pointV();
+	Self.Init(pose_of_the_robot.x,pose_of_the_robot.y,point_of_the_robot.vx,point_of_the_robot.vy,pose_of_the_robot.theta);
+
+	std::cout <<"(Performance) Self.r.m="<<Self.r.m<< "; Self.r.x="<<Self.r.x<<"; Self.r.y="<<Self.r.y<<"; Self.r.th"<<Self.r.th*180/3.14<<"; pose_of_the_robot.theta*180/3.14="<<pose_of_the_robot.theta*180/3.14<<std::endl;
+	std::cout <<"(Performance) Self.v.m="<<Self.v.m<< "; Self.v.x="<<Self.v.x<<"; Self.v.y="<<Self.v.y<<"; Self.v.th"<<Self.v.th*180/3.14<<std::endl;
+	//Self.r.th=pose_of_the_robot.theta;
+
+	unsigned int number_of_group_people_=2;
+	State2D *Others=new State2D[number_of_group_people_];
+	//std::cout << " *Others=new State2D[number_of_group_people_]"<<std::endl;
+	SpointV_cov actual_first_person_companion_point=SpointV(companion_person_x,companion_person_y,real_computation_time_dt,companion_person_vx,companion_person_vy);//pointer_to_person_companion_->get_current_pointV();
+	State2D first_companion_person(actual_first_person_companion_point.x,actual_first_person_companion_point.y,actual_first_person_companion_point.vx,actual_first_person_companion_point.vy);
+	//first_companion_person.r.th=pose_of_the_robot.theta;
+	Others[0]=first_companion_person;
+	std::cout <<"(FIRST) id_person="<< " first_companion_person.r.x="<<first_companion_person.r.x<<"; first_companion_person.r.y="<<first_companion_person.r.y<<"; first_companion_person.v.x="<<first_companion_person.v.x<<"; first_companion_person.v.y="<<first_companion_person.v.y<<"; th="<<first_companion_person.r.th*180/3.14<<std::endl;
+
+
+	SpointV_cov actual_second_person_companion_point=SpointV_cov(companion2_person_x,companion2_person_y,real_computation_time_dt,companion2_person_vx,companion2_person_vy);//=second_group_companion_person_obj_->get_prediction_trajectory()->at(parent_vertex);
+	//if((we_have_pointer_to_second_person_)&&(number_of_group_people_>1)){
+		//actual_second_person_companion_point=//second_group_companion_person_obj_->get_current_pointV();
+		State2D second_companion_person(actual_second_person_companion_point.x,actual_second_person_companion_point.y,actual_second_person_companion_point.vx,actual_second_person_companion_point.vy);
+		//second_companion_person.r.th=pose_of_the_robot.theta;
+
+		Others[1]=second_companion_person;
+		std::cout <<"(SECOND) id_person="<< " second_companion_person.r.x="<<second_companion_person.r.x<<"; second_companion_person.r.y="<<second_companion_person.r.y<<"; second_companion_person.v.x="<<second_companion_person.v.x<<"; second_companion_person.v.y="<<second_companion_person.v.y<<"; th="<<second_companion_person.r.th*180/3.14<<std::endl;
+	//}
+	Vector2D preferred;
+	double x_orient_goal,y_orient_goal,orient_goal;
+			//if((number_of_group_people_>1)&&(we_have_pointer_to_second_person_)){
+				x_orient_goal=companion_person_dest_x - companion_person_x;
+				y_orient_goal=companion_person_dest_y - companion_person_y;
+				orient_goal=atan(y_orient_goal/x_orient_goal);
+				if(isnan((float)orient_goal)){ // si es nan el calculo, quetate con la orientacion actual del robot. (caso inicio generalmente)
+					orient_goal=robot_theta;
+				}
+				//preferred=Vector2D(x_orient_goal,y_orient_goal,orient_goal); // is the orientation until the goal of the group.
+				if((companion_person_vx!=0)&&(companion2_person_vx!=0)&&(companion_person_vy!=0)&&(companion2_person_vy!=0)){
+					preferred=Vector2D((companion_person_vx+companion2_person_vx)/2,(companion_person_vy+companion2_person_vy)/2);
+				}else{
+					x_orient_goal=companion_person_dest_x - companion_person_x;
+					y_orient_goal=companion_person_dest_y - companion_person_y;
+					orient_goal=atan(y_orient_goal/x_orient_goal);
+					if(isnan((float)orient_goal)){ // si es nan el calculo, quetate con la orientacion actual del robot. (caso inicio generalmente)
+						orient_goal=robot_theta;
+					}
+					preferred=Vector2D(x_orient_goal,y_orient_goal,orient_goal); // is the orientation until the goal of the group.
+				}
+
+				std::cout << "(caso1 save in file) preferred.x= "<<preferred.x<<"; preferred.y="<<preferred.y<<"; preferred.m="<<preferred.m<<"; preferred.th="<<preferred.th<< std::endl;
+							//}
+				// TODO: mirar que esten seteadas todas las constantes por defecto.
+	 double vp=Companion_Zanlungo_Model_.f2_vp2v(preferred.m);//f2_vp2v(0.4);
+
+	 preferred.Scale(vp/preferred.m);
+	// vp=robot_->get_current_pose().v;
+	 //preferred=Vector2D(0,vp);//la velocidad preferida
+	 std::cout << "(caso2 save in file. ahora uso caso 1) preferred.x= "<<preferred.x<<"; preferred.y="<<preferred.y<<"; preferred.m="<<preferred.m<<"; preferred.th="<<preferred.th*180/3.14<<"; vp="<<vp<< std::endl;
+
+	 double actual_performance=Companion_Zanlungo_Model_.Exmet3(Self,Others,preferred);
+
+	 double actual_performance1=robot_performance1;
+	 double actual_performance2=robot_performance2;
+
+	 std::cout << "actual_performance="<<actual_performance<< std::endl;
+	 std::cout << "actual_performance1="<<actual_performance1<< std::endl;
+	 std::cout << "actual_performance2="<<actual_performance2<< std::endl;
+	 	 if(actual_performance!=0){ //saltatela. Pasan por como guarde los datos, no porque de verdad sea 0, es cuando se paraba la simulacion.
+	 		actual_robot_performance_.push_back(actual_performance); // performace por experimento; iteration.
+	 	 }
+	 	 if(actual_performance1!=0){ //saltatela. Pasan por como guarde los datos, no porque de verdad sea 0, es cuando se paraba la simulacion.
+	 		 actual_robot_performance1_.push_back(actual_performance1); // performace por experimento; iteration.
+	 	 }
+	 	if(actual_performance2!=0){ //saltatela. Pasan por como guarde los datos, no porque de verdad sea 0, es cuando se paraba la simulacion.
+	 		 actual_robot_performance2_.push_back(actual_performance2); // performace por experimento; iteration.
+	 	}
+
+	// }
+	firs_experiment=experiment_;
+
+	// RE Calcular media :
+	double sum=0.0;
+	for(unsigned int t=0; t<actual_robot_performance_.size();t++){
+
+		if(isnan((float)actual_robot_performance_.at(t))){
+		}else{
+			sum=sum+actual_robot_performance_.at(t);
+			std::cout << "sum="<<sum<<"; actual_robot_performance_="<<actual_robot_performance_.at(t)<< std::endl;
+		}
+
+	}
+	double media_one_experiment=sum;// /actual_robot_performance_.size();
+	std::cout << "media_one_experiment="<<media_one_experiment<<"; sum/actual_robot_performance_.size()="<<sum/actual_robot_performance_.size()<< std::endl;
+
+	if(isnan((float)media_one_experiment)){
+
+	}else{
+
+	}
+    // Calcular media performance 1
+	double sum1=0.0;
+	for(unsigned int t=0; t<actual_robot_performance1_.size();t++){
+
+		if(isnan((float)actual_robot_performance1_.at(t))){
+		}else{
+			sum1=sum1+actual_robot_performance1_.at(t);
+			//std::cout << "sum1="<<sum1<<"; actual_robot_performance1_="<<actual_robot_performance1_.at(t)<< std::endl;
+		}
+
+	}
+	double media_one_experiment1=sum1;// /actual_robot_performance_.size();
+	//std::cout << "media_one_experiment1="<<media_one_experiment1<<"; sum1/actual_robot_performance1_.size()="<<sum1/actual_robot_performance1_.size()<< std::endl;
+
+	if(isnan((float)media_one_experiment1)){
+
+	}else{
+
+	}
+
+	// Calcular media performance 2
+		double sum2=0.0;
+		for(unsigned int t=0; t<actual_robot_performance2_.size();t++){
+
+			if(isnan((float)actual_robot_performance2_.at(t))){
+			}else{
+				sum2=sum2+actual_robot_performance2_.at(t);
+				//std::cout << "sum2="<<sum2<<"; actual_robot_performance1_="<<actual_robot_performance2_.at(t)<< std::endl;
+			}
+
+		}
+		double media_one_experiment2=sum2;// /actual_robot_performance_.size();
+		//std::cout << "media_one_experiment2="<<media_one_experiment2<<"; sum2/actual_robot_performance2_.size()="<<sum2/actual_robot_performance2_.size()<< std::endl;
+
+		if(isnan((float)media_one_experiment2)){
+
+		}else{
+
+		}
+
+	// calcular std: (TODO: calcularla!!!)
+	double ste=0.0;
+	for(unsigned int t=0; t<actual_robot_performance_.size();t++){
+		if(isnan((float)actual_robot_performance_.at(t))){
+		}else{
+			ste=ste+(actual_robot_performance_.at(t)-media_one_experiment)*(actual_robot_performance_.at(t)-media_one_experiment);
+		}
+
+	}
+	double final_std=ste; // /(actual_robot_performance_.size());
+	std::cout << "final_std="<<final_std<< std::endl;
+
+	// calcular std1: (TODO: calcularla!!!)
+	double ste1=0.0;
+	for(unsigned int t=0; t<actual_robot_performance1_.size();t++){
+		if(isnan((float)actual_robot_performance1_.at(t))){
+		}else{
+			ste1=ste1+(actual_robot_performance1_.at(t)-media_one_experiment1)*(actual_robot_performance1_.at(t)-media_one_experiment1);
+		}
+
+	}
+	double final_std1=ste1; // /(actual_robot_performance_.size());
+	//std::cout << "final_std1="<<final_std1<< std::endl;
+
+	// calcular std1: (TODO: calcularla!!!)
+		double ste2=0.0;
+		for(unsigned int t=0; t<actual_robot_performance2_.size();t++){
+			if(isnan((float)actual_robot_performance2_.at(t))){
+			}else{
+				ste2=ste2+(actual_robot_performance2_.at(t)-media_one_experiment2)*(actual_robot_performance2_.at(t)-media_one_experiment2);
+			}
+
+		}
+		double final_std2=ste2; // /(actual_robot_performance_.size());
+		//std::cout << "final_std2="<<final_std2<< std::endl;
+
+	// include all mean
+	if((isnan((float)media_one_experiment))||(isnan((float)final_std))){ // evitar casos raros que no quite de los datos, donde se paraba la simulacion.
+
+	}else{
+		if((media_one_experiment!=0)){
+			all_means_of_performances_ex_.push_back(media_one_experiment);
+			all_std_of_performances_ex_.push_back(final_std);
+		}
+
+	}
+
+	// include all mean1
+		if((isnan((float)media_one_experiment1))||(isnan((float)final_std1))){ // evitar casos raros que no quite de los datos, donde se paraba la simulacion.
+
+		}else{
+			if((media_one_experiment1!=0)){
+				all_means_of_performances_ex1_.push_back(media_one_experiment1);
+				all_std_of_performances_ex1_.push_back(final_std1);
+			}
+
+		}
+		// include all mean2
+			if((isnan((float)media_one_experiment2))||(isnan((float)final_std2))){ // evitar casos raros que no quite de los datos, donde se paraba la simulacion.
+
+			}else{
+				if((media_one_experiment2!=0)){
+					all_means_of_performances_ex2_.push_back(media_one_experiment2);
+					all_std_of_performances_ex2_.push_back(final_std2);
+				}
+
+			}
+
+	 actual_robot_performance_.clear();
+	 actual_robot_performance1_.clear();
+	 actual_robot_performance2_.clear();
+	 iteration_++;
+	}
+
+   // ALL RECALCULATED
+	double sum2=0.0;
+	for(unsigned int t=0; t<all_means_of_performances_ex_.size();t++){
+		//if(isnan((float)all_means_of_performances_ex_.at(t))){
+		//}else{
+			sum2=sum2+all_means_of_performances_ex_.at(t);
+		//}
+
+		std::cout << "sum2="<<sum2<<"; all_means_of_performances_ex_="<<all_means_of_performances_ex_.at(t)<< std::endl;
+	}
+	double media_all_experiment=sum2/all_means_of_performances_ex_.size();
+
+	// calcular std: (TODO: calcularla!!!)
+	double sum3=0.0;
+	for(unsigned int t=0; t<all_std_of_performances_ex_.size();t++){
+		//if(isnan((float)all_std_of_performances_ex_.at(t))){
+		//}else{
+			if((all_std_of_performances_ex_.at(t)<1)&&(all_std_of_performances_ex_.at(t)>0)){
+				sum3=sum3+all_std_of_performances_ex_.at(t);
+			}
+		//}
+
+
+		std::cout << "sum3="<<sum3<<"; all_std_of_performances_ex_="<<all_std_of_performances_ex_.at(t)<< std::endl;
+	}
+	double media_all_std=sum3/all_std_of_performances_ex_.size();
+
+	/// INI TO calculate real exp std:
+	double ste2=0;
+	for(unsigned int t=0; t<all_means_of_performances_ex_.size();t++){
+			if(isnan((float)all_means_of_performances_ex_.at(t))){
+			}else{
+				ste2=ste2+(all_means_of_performances_ex_.at(t)-media_all_experiment)*(all_means_of_performances_ex_.at(t)-media_all_experiment);
+				 std::cout << "ste2="<<ste2<< std::endl;
+			}
+
+		}
+	media_all_std=ste2/all_means_of_performances_ex_.size();
+	/// FIN TO calculate real exp std:
+
+	 std::cout << "media_all_experiment=[media_all_experiment,"<<media_all_experiment<<"]; media_all_std=[media_all_std,"<<media_all_std<<"];"<< std::endl;
+
+	 // ALL WITH THE FIRST PERFORMANCE OF THE DOCUMENT
+		sum2=0.0;
+		unsigned int count_perf1=0;
+		for(unsigned int t=0; t<all_means_of_performances_ex1_.size();t++){
+			//if(isnan((float)all_means_of_performances_ex1_.at(t))){
+			//}else{
+				sum2=sum2+all_means_of_performances_ex1_.at(t);
+			//}
+				if(all_means_of_performances_ex1_.at(t)!=0.0){
+					count_perf1++;
+				}
+
+			//std::cout << "sum2="<<sum2<<"; all_means_of_performances_ex1_="<<all_means_of_performances_ex1_.at(t)<< std::endl;
+		}
+		double media_all_experiment1=sum2/all_means_of_performances_ex1_.size();
+		double media_all_experiment1_v2=sum2/count_perf1;
+		// calcular std: (TODO: calcularla!!!)
+		sum3=0.0;
+		for(unsigned int t=0; t<all_std_of_performances_ex1_.size();t++){
+			//if(isnan((float)all_std_of_performances_ex1_.at(t))){
+			//}else{
+				if((all_std_of_performances_ex1_.at(t)<1)&&(all_std_of_performances_ex1_.at(t)>0)){
+					sum3=sum3+all_std_of_performances_ex1_.at(t);
+				//}
+			}
+
+
+			//std::cout << "sum3="<<sum3<<"; all_std_of_performances_ex1_="<<all_std_of_performances_ex1_.at(t)<< std::endl;
+		}
+		double media_all_std1=sum3/all_std_of_performances_ex1_.size();
+		double media_all_std1_v2=sum3/count_perf1;
+		/// INI TO calculate real exp std:
+		ste2=0;
+		for(unsigned int t=0; t<all_means_of_performances_ex1_.size();t++){
+				if(isnan((float)all_means_of_performances_ex1_.at(t))){
+				}else{
+					ste2=ste2+(all_means_of_performances_ex1_.at(t)-media_all_experiment1)*(all_means_of_performances_ex1_.at(t)-media_all_experiment1);
+					 //std::cout << "ste2="<<ste2<< std::endl;
+				}
+
+			}
+		media_all_std1=ste2/all_means_of_performances_ex1_.size();
+	    media_all_std1_v2=ste2/count_perf1;
+		/// FIN TO calculate real exp std:
+
+		 std::cout << "media_all_experiment1=[media_all_experiment1,"<<media_all_experiment1<<"]; media_all_std1=[media_all_std1,"<<media_all_std1<<"];"<< std::endl;
+		// std::cout <<" count_perf1="<<count_perf1<< "media_all_experiment1_v2=[media_all_experiment1_v2,"<<media_all_experiment1_v2<<"]; media_all_std1_v2=[media_all_std1_v2,"<<media_all_std1_v2<<"];"<< std::endl;
+
+		 // ALL WITH THE SECOND PERFORMANCE OF THE DOCUMENT
+		 		sum2=0.0;
+		 		unsigned int count_perf2=0;
+		 		for(unsigned int t=0; t<all_means_of_performances_ex2_.size();t++){
+		 			//if(isnan((float)all_means_of_performances_ex2_.at(t))){
+		 			//}else{
+		 				sum2=sum2+all_means_of_performances_ex2_.at(t);
+		 			//}
+		 				if(all_means_of_performances_ex2_.at(t)!=0.0){
+		 					count_perf2++;
+		 				}
+
+		 			//std::cout << "sum2="<<sum2<<"; all_means_of_performances_ex2_="<<all_means_of_performances_ex2_.at(t)<< std::endl;
+		 		}
+		 		double media_all_experiment2=sum2/all_means_of_performances_ex2_.size();
+		 		double media_all_experiment2_v2=sum2/count_perf2;
+		 		// calcular std: (TODO: calcularla!!!)
+		 		sum3=0.0;
+		 		for(unsigned int t=0; t<all_std_of_performances_ex2_.size();t++){
+		 			//if(isnan((float)all_std_of_performances_ex2_.at(t))){
+		 			//}else{
+			 			if((all_std_of_performances_ex2_.at(t)<1)&&(all_std_of_performances_ex2_.at(t)>0)){
+			 				sum3=sum3+all_std_of_performances_ex2_.at(t);
+			 			}
+		 			//}
+
+
+		 			//std::cout << "sum3="<<sum3<<"; all_std_of_performances_ex2_="<<all_std_of_performances_ex2_.at(t)<< std::endl;
+		 		}
+		 		double media_all_std2=sum3/all_std_of_performances_ex2_.size();
+		 		double media_all_std2_v2=sum3/count_perf2;
+
+		 		/// INI TO calculate real exp std:
+		 		ste2=0;
+		 		for(unsigned int t=0; t<all_means_of_performances_ex2_.size();t++){
+		 				if(isnan((float)all_means_of_performances_ex2_.at(t))){
+		 				}else{
+		 					ste2=ste2+(all_means_of_performances_ex2_.at(t)-media_all_experiment1)*(all_means_of_performances_ex2_.at(t)-media_all_experiment2);
+		 					 //std::cout << "ste2="<<ste2<< std::endl;
+		 				}
+
+		 			}
+		 		media_all_std2=ste2/all_means_of_performances_ex2_.size();
+		 		media_all_std2_v2=ste2/count_perf2;
+		 		/// FIN TO calculate real exp std:
+
+		 		 std::cout << "media_all_experiment2=[media_all_experiment2,"<<media_all_experiment2<<"]; media_all_std2=[media_all_std2,"<<media_all_std2<<"];"<< std::endl;
+		 		//std::cout <<" count_perf2="<<count_perf2<< "media_all_experiment2_v2=[media_all_experiment2_v2,"<<media_all_experiment2_v2<<"]; media_all_std2_v2=[media_all_std2_v2,"<<media_all_std2_v2<<"];"<< std::endl;
+
+
+/*
+	std::string  data_file_Zanlungo_="/home/erepiso/iri-lab/iri_ws/src/iri_navigation/iri_atr_akp_local_planner_companion/txt_data/data_companion.txt";
+
+
+
+
+
+	std::ofstream fileMatlab;
+	fileMatlab.open (data_file_Zanlungo_.c_str(), std::ofstream::out | std::ofstream::app);
+	// const char *direc_file_name_;
+	//direc_file_name_= results_file_.c_str();
+	//home/ely7787/iri-lab/labrobotica/restricted/algorithms/people_prediction/branches/ely_people_prediction_companion_robot/1_data_results
+	//fileMatlab.open (results_file_.c_str(), std::ofstream::out | std::ofstream::app);
+	//fileMatlab.open (data_file_Zanlungo_.c_str(), std::ofstream::out | std::ofstream::trunc);
+	fileMatlab << "\n";
+
+	fileMatlab << "robot_performance("<<experiment_<<","<<iteration_<<")= "<<actual_robot_performance_<<"\n";
+*/
+	return 0;
+}
diff --git a/local_lib_people_prediction/src/examples/generate_performance_from_data_example_humanoids_side_by_side_2people.cpp b/local_lib_people_prediction/src/examples/generate_performance_from_data_example_humanoids_side_by_side_2people.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c61818b58bf17db8700f18e6cbd4b9610158e662
--- /dev/null
+++ b/local_lib_people_prediction/src/examples/generate_performance_from_data_example_humanoids_side_by_side_2people.cpp
@@ -0,0 +1,1842 @@
+/*
+ * generate_performance_from_data_example.cpp
+ *
+ *  Created on: 7-Sep-2018
+ *      Author: errepiso
+ */
+
+#include "nav/plan_local_nav.h"
+#include "nav/force_reactive_robot_companion.h"
+#include "scene_sim.h"
+#include <iostream>
+#include "companion_zanlungo/CompanionFrancescoModel.h"
+
+int main(int argc,char *argv[])
+{
+	  CompanionFrancescoModel Companion_Zanlungo_Model_;
+
+
+	    double in_Cr, in_r0, in_Ct, in_eta, in_rmax, in_S0;
+	    in_Cr=0.62;
+	    in_Ct=0.08;
+	    in_r0=1.5;
+	    in_eta=-0.43;
+	    in_rmax=3;
+	    in_S0=0.5;
+	    Cgenome genome;
+
+	    genome.Init(in_Cr, in_r0, in_Ct, in_eta, in_rmax, in_S0);
+
+	    Companion_Zanlungo_Model_.set_genome_params_in_CompanionFrancescoModel(genome);
+	    Companion_Zanlungo_Model_.set_internal_k_value(1.5);
+
+	// TODO: falta ver como cargar los datos del fichero txt ahora tal como esta.
+
+	std::string file_name_s="~/iri-lab/iri_ws/src/iri_navigation/iri_atr_akp_local_planner_companion/txt_data/1_V_formation_lateral_sin_obstaculos/results_Vform_companion_OK.txt";
+	const char * file_name=file_name_s.c_str();
+
+	std::vector<double> actual_robot_performance_;
+	std::vector<double> all_means_of_performances_ex_;
+	std::vector<double> all_std_of_performances_ex_;
+
+	std::vector<double> actual_robot_performance1_;
+	std::vector<double> all_means_of_performances_ex1_;
+	std::vector<double> all_std_of_performances_ex1_;
+
+
+	std::vector<double> actual_robot_performance2_;
+	std::vector<double> all_means_of_performances_ex2_;
+	std::vector<double> all_std_of_performances_ex2_;
+
+	std::cout << " OOOOOOOOOOOO77777777777777OOOOOOOOOOOOOOOO file_name="<<file_name<<std::endl;
+
+	//ifstream reader;//(file_name);
+	fstream reader;
+	//reader.open(file_name_s);
+	//reader.open("1_side_by_side_exp_reales_ICRA2019/9_julio/results_robot_fme_lateral3.txt"); //results_robot11.txt
+	//reader.open("2_robot_lateral/1_sideBySide2people_without_obst/results_person_companion_side_by_side3.txt"); //results_robot11.txt
+	reader.open("3_only_static_obst/lateral_izquierdo/results_person_companion_side_by_side1.txt");
+
+	bool real_experiment=false;
+	bool only_one_person_=false;
+
+	bool see_save_in_document=false;
+	// for real exp:
+	//unsigned int max_exp=1; //275-129; // until max_exp; (FINAL ITER)
+	//unsigned int max_iter=127; //275-129; // until max_exp; (FINAL ITER)
+
+	// for simulation exp:
+	unsigned int max_exp=49; //275-129; // until max_exp; (FINAL ITER)
+	unsigned int max_iter=222; //275-129; // until max_exp; (FINAL ITER)
+
+	// folders exp reales:
+	//1_side_by_side_exp_reales_ICRA2019/3_julio
+	//1_side_by_side_exp_reales_ICRA2019/4_julio
+	//1_side_by_side_exp_reales_ICRA2019/5_julio
+	//1_side_by_side_exp_reales_ICRA2019/9_julio/results_robot1.txt ; results_robot2.txt; results_robot.txt
+
+	 //cout <<"open_file="<<open_file<<endl;
+	unsigned int experiment_,iteration_,experiment2_,iteration2_;
+	unsigned int befor_exp_=1;
+	experiment_=1; // (initial experiment)
+	iteration_=1; // (INITIAL ITER)
+	unsigned int firs_experiment=1; //
+
+
+
+	double real_computation_time_dt;
+
+	double companion_person_x;
+	double companion_person_y;
+	double companion_person_vx;
+	double companion_person_vy;
+
+	double companion_person_v;
+	double companion_person_theta;
+
+	double companion_person_dest_x;
+	double companion_person_dest_y;
+
+	double companion2_person_x;
+	double companion2_person_y;
+	double companion2_person_vx;
+	double companion2_person_vy;
+
+	double companion2_person_v;
+	double companion2_person_theta;
+
+	double companion_person2_dest_x;
+	double companion_person2_dest_y;
+
+	double robot_x;
+	double robot_y;
+	double robot_vx;
+	double robot_vy;
+	double robot_theta;
+	double robot_v;
+	double robot_w;
+
+	double robot_dest_x;
+	double robot_dest_y;
+
+	double robot_performance1;
+	double robot_performance2;
+	// double robot_performance;
+
+	// Distance performances:
+	std::vector<double> vec_distance_performance_p1_v1_; // v1 == acumulamos todo por iteracion.
+	std::vector<double> vec_distance_performance_p1_v2_; // v2 == acumulamos todo por experimento.
+	std::vector<double> act_vec_distance_performance_p1_v2_;
+	std::vector<double> vec_distance_performance_p2_v1_;
+	std::vector<double> vec_distance_performance_p2_v2_;
+	std::vector<double> act_vec_distance_performance_p2_v2_;
+	std::vector<double> vec_distance_performance_total_v1_; // (total=(perf_p1+perf_p2)/2 acumulamos todo por iteracion.
+	std::vector<double> vec_distance_performance_total_v2_; //(total=(perf_p1+perf_p2)/2 acumulamos todo por experimento.
+	std::vector<double> act_vec_distance_performance_total_v2_;
+		//std:: distance performance.
+	std::vector<double> std_vec_distance_performance_p1_v2_;
+	std::vector<double> std_vec_distance_performance_p2_v2_;
+	std::vector<double> std_vec_distance_performance_total_v2_;
+
+	// Angle performances:
+	std::vector<double> vec_angle_performance_p1_v1_; // v1 == acumulamos todo por iteracion.
+	std::vector<double> vec_angle_performance_p1_v2_; // v2 == acumulamos todo por experimento.
+	std::vector<double> act_vec_angle_performance_p1_v2_; // act_v2 == acumulamos solo un experimento.
+	std::vector<double> vec_angle_performance_p2_v1_;
+	std::vector<double> vec_angle_performance_p2_v2_;
+	std::vector<double> act_vec_angle_performance_p2_v2_;
+	std::vector<double> vec_angle_performance_total_v1_; // (total=(perf_p1+perf_p2)/2
+	std::vector<double> vec_angle_performance_total_v2_;
+	std::vector<double> act_vec_angle_performance_total_v2_;
+		//std:: angle performance.
+	std::vector<double> std_vec_angle_performance_p1_v2_;
+	std::vector<double> std_vec_angle_performance_p2_v2_;
+	std::vector<double> std_vec_angle_performance_total_v2_;
+
+
+	//Area performances:
+	std::vector<double> vec_area_performance_p1_v1_; // v1 == acumulamos todo por iteracion.
+	std::vector<double> vec_area_performance_p1_v2_; // v2 == acumulamos todo por experimento.
+	std::vector<double> act_vec_area_performance_p1_v2_;
+	std::vector<double> vec_area_performance_p2_v1_;
+	std::vector<double> vec_area_performance_p2_v2_;
+	std::vector<double> act_vec_area_performance_p2_v2_;
+	std::vector<double> vec_total_area_performance_v1_; // (total=(perf_p1+perf_p2)/2
+	std::vector<double> vec_total_area_performance_v2_;
+	std::vector<double> act_vec_total_area_performance_v2_;
+
+		//std:: area performance.
+	std::vector<double> std_vec_area_performance_p1_v2_;
+	std::vector<double> std_vec_area_performance_p2_v2_;
+	std::vector<double> std_vec_total_area_performance_v2_;
+
+
+bool finish=false;
+
+
+
+
+cout <<"antes bucle experiments ="<<endl;
+//while( (experiment_< (max_exp+1)) || (!finish) ){
+while((!finish)){
+//while( iteration_< max_exp){
+		// cout <<"antes bucle iterations; experiment_="<<experiment_<<"; (max_exp+1)="<<(max_exp+1)<<"; finish="<<finish<<endl;
+		// while(experiment_==firs_experiment){
+
+		// cout <<"reader.good()="<<reader.good()<<endl;
+	// TODO: coger valores de arriba del fichero. Iterativamente.
+	if (reader.good()) {
+
+		/// INICIO BUCLE, tendria que ser desde aquí
+		//###############################
+		//# experiment_ and  iteration_ #
+		//###############################
+		 string line;
+		 getline(reader,line);
+		 getline(reader,line);
+		 getline(reader,line);
+		 getline(reader,line);
+		 getline(reader,line);
+		 getline(reader,line);
+		// cout <<"(1)"<<endl;
+		 //experiment_=-1;
+		// cout <<"(2)"<<endl;
+		 //find first comma, read #rows
+			 string::size_type i = line.find("(");
+			 if(i>50){
+				 i=3;
+			 }
+			// cout <<"(4); i="<<i<<endl;
+			 string::size_type r = line.find(",");
+			// cout <<"(5) r="<<r<<endl;
+			// cout <<"(3)"<<endl;
+			 if (i!=reader.eof()) {
+
+			 	string cstr = line.substr(i,r);
+
+			 	experiment_ = atoi(cstr.c_str());
+			 	// cout <<"(6)"<<endl;
+			 } else {
+			 	cout <<"ERROR @ GMap::read num_people_mov"<<endl;
+			 	exit(EXIT_FAILURE);
+			 }
+			 /*if(experiment_==0){
+				// getline(reader,line);
+				 cout <<"(1)"<<endl;
+						 experiment_=-1;
+						 cout <<"(2)"<<endl;
+						 //find first comma, read #rows
+							 string::size_type i = line.find("(");
+							 cout <<"(3)"<<endl;
+							 if (i!=reader.eof()) {
+								 cout <<"(4); i="<<i<<endl;
+							 	string cstr = line.substr(i+1,i+2);
+							 	 cout <<"(5)"<<endl;
+							 	experiment_ = atoi(cstr.c_str());
+							 	 cout <<"(6)"<<endl;
+							 } else {
+							 	cout <<"ERROR @ GMap::read num_people_mov"<<endl;
+							 	exit(EXIT_FAILURE);
+							 }
+			 }*/
+			 if(see_save_in_document){
+				 cout <<"experiment_="<<experiment_<<endl;
+
+			 }
+
+			// p=experiment_;
+
+			 string::size_type j = line.find(")");
+			 /*if(j>1000){
+				 j=7;
+			 }*/
+			 if(see_save_in_document){
+			 cout <<"j="<<j<<endl;
+			 }
+			 if (j!=reader.eof()) {
+			 	//string cstr = line.substr(j-1,j);
+			 	///iteration_ = atoi(cstr.c_str());
+			 } else {
+			 	cout <<"ERROR @ GMap::read num_people_mov"<<endl;
+			 	exit(EXIT_FAILURE);
+			 }
+			// if(see_save_in_document){
+			 cout <<"experiment_="<<experiment_<<"iteration_="<<iteration_<<endl;
+			 //}
+			/* if(iteration_==2){
+				 cout <<"iteration_="<<iteration_<<"; firs_experiment="<<experiment_<<endl;
+				 firs_experiment=experiment_;
+			 }*/
+
+			 getline(reader,line);
+			 /*i = line.find(" ");
+			 unsigned int k=line.size();
+			 cout <<"; i="<<i<<"; K="<<k<<endl;
+			 if (i!=reader.eof()) {
+				//string cstr = line.substr(i,k);
+			 	real_computation_time_dt = std::stod(cstr.c_str());
+
+			 } else {
+			 	cout <<"ERROR @ GMap::read num_people_mov"<<endl;
+			 	exit(EXIT_FAILURE);
+			 }
+				cout <<"real_computation_time_dt="<<real_computation_time_dt<<endl;*/
+
+				getline(reader,line); //computational_time(1,1)=0;
+				getline(reader,line); //robot_plan_companion2_start_ITER(1,1)=3779191;
+				getline(reader,line); //robot_plan_companion2_end_ITER(1,1)=3825907;
+
+				// get experiment2_  //experiment(1,1)=1;
+				getline(reader,line);
+				i = line.find(" ");
+				unsigned int k=line.size();
+				 if(see_save_in_document){
+				cout <<"; i="<<i<<"; K="<<k<<endl;
+				 }
+				if (i!=reader.eof()) {
+					string cstr = line.substr(i,k);
+					//cout <<"1 experiment2_="<<endl;
+					experiment2_ = atoi(cstr.c_str());
+					//cout <<"2 experiment2_="<<endl;
+				} else {
+					cout <<"ERROR @ GMap::read num_people_mov"<<endl;
+					exit(EXIT_FAILURE);
+				}
+				//cout <<"3 experiment2_="<<endl;
+				 if(see_save_in_document){
+				cout <<"experiment2_="<<experiment2_<<endl;
+				 }
+				experiment_=experiment2_;
+				// get iteration2_  //iteration(1,1)=1;
+
+				getline(reader,line);
+				i = line.find(" ");
+				k=line.size();
+				 if(see_save_in_document){
+				cout <<"; i="<<i<<"; K="<<k<<endl;
+				 }
+				if (i!=reader.eof()) {
+					string cstr = line.substr(i,k);
+					iteration2_ = atoi(cstr.c_str());
+				} else {
+					cout <<"ERROR @ GMap::read num_people_mov"<<endl;
+					exit(EXIT_FAILURE);
+				}
+				 if(see_save_in_document){
+				cout <<"iteration2_="<<iteration2_<<endl;
+				 }
+				iteration_=iteration2_;
+		//###########################
+		//# Read companion person1  #
+		//###########################
+
+				//# Read companion person1 position #
+			getline(reader,line);
+			i = line.find(" ");
+			k=line.size();
+			 if(see_save_in_document){
+			cout <<"; i="<<i<<"; K="<<k<<endl;
+			 }
+			 if (i!=reader.eof()) {
+				string cstr = line.substr(i,k);
+				companion_person_x = std::stod(cstr.c_str());
+
+			 } else {
+				cout <<"ERROR @ GMap::read num_people_mov"<<endl;
+				exit(EXIT_FAILURE);
+			}
+			 if(see_save_in_document){
+			cout <<"companion_person_x="<<companion_person_x<<endl;
+			 }
+			getline(reader,line);
+			i = line.find(" ");
+			k=line.size();
+			 if(see_save_in_document){
+			cout <<"; i="<<i<<"; K="<<k<<endl;
+			 }
+			 if (i!=reader.eof()) {
+				string cstr = line.substr(i,k);
+				companion_person_y = std::stod(cstr.c_str());
+
+			 } else {
+				cout <<"ERROR @ GMap::read num_people_mov"<<endl;
+				exit(EXIT_FAILURE);
+			}
+			 if(see_save_in_document){
+			cout <<"companion_person_y="<<companion_person_y<<endl;
+			 }
+			//# Read companion person1 velocities #
+			getline(reader,line);
+			i = line.find(" ");
+			k=line.size();
+			 if(see_save_in_document){
+			cout <<"; i="<<i<<"; K="<<k<<endl;
+			 }
+			 if (i!=reader.eof()) {
+				string cstr = line.substr(i,k);
+				companion_person_vx = std::stod(cstr.c_str());
+
+			 } else {
+				cout <<"ERROR @ GMap::read num_people_mov"<<endl;
+				exit(EXIT_FAILURE);
+			}
+			 if(see_save_in_document){
+			cout <<"companion_person_vx="<<companion_person_vx<<endl;
+			 }
+			getline(reader,line);
+			i = line.find(" ");
+			k=line.size();
+			 if(see_save_in_document){
+			cout <<"; i="<<i<<"; K="<<k<<endl;
+			 }
+			 if (i!=reader.eof()) {
+				string cstr = line.substr(i,k);
+				companion_person_vy = std::stod(cstr.c_str());
+
+			 } else {
+				cout <<"ERROR @ GMap::read num_people_mov"<<endl;
+				exit(EXIT_FAILURE);
+			}
+			 if(see_save_in_document){
+			cout <<"companion_person_vy="<<companion_person_vy<<endl;
+			 }
+			// Get companion_person_v
+			getline(reader,line);
+			i = line.find(" ");
+			k=line.size();
+			 if(see_save_in_document){
+			cout <<"; i="<<i<<"; K="<<k<<endl;
+			 }
+			 if (i!=reader.eof()) {
+				string cstr = line.substr(i,k);
+				companion_person_v = std::stod(cstr.c_str());
+
+			 } else {
+				cout <<"ERROR @ GMap::read num_people_mov"<<endl;
+				exit(EXIT_FAILURE);
+			}
+			 if(see_save_in_document){
+			cout <<"companion_person_v="<<companion_person_v<<endl;
+			 }
+			// get companion_person_theta
+			getline(reader,line); // companion_person_theta(1,1)= -nan
+			i = line.find(" ");
+			k=line.size();
+			 if(see_save_in_document){
+			cout <<"; i="<<i<<"; K="<<k<<endl;
+			 }
+			if (i!=reader.eof()) {
+				string cstr = line.substr(i,k);
+				companion_person_theta = std::stod(cstr.c_str());
+			} else {
+				cout <<"ERROR @ GMap::read num_people_mov"<<endl;
+				exit(EXIT_FAILURE);
+			}
+			 if(see_save_in_document){
+			cout <<"companion_person_theta="<<companion_person_theta<<endl;
+			 }
+			//# Read companion person1 destinations #
+			getline(reader,line); // companion_person_dest_x(1,1)= 0.220653
+			i = line.find(" ");
+			k=line.size();
+			if(k>1000){
+				k=38;
+			}
+			 if(see_save_in_document){
+			cout <<"; i="<<i<<"; K="<<k<<endl;
+			 }
+			 if (i!=reader.eof()) {
+				string cstr = line.substr(i,k);
+				companion_person_dest_x = std::stod(cstr.c_str());
+
+			 } else {
+				cout <<"ERROR @ GMap::read num_people_mov"<<endl;
+				exit(EXIT_FAILURE);
+			}
+			 if(see_save_in_document){
+			cout <<"companion_person_dest_x="<<companion_person_dest_x<<endl;
+			 }
+			getline(reader,line); // companion_person_dest_x(1,1)= 0.220653
+
+			i = line.find(" ");
+			k=line.size();
+			if(k>1000){
+				k=38;
+			}
+			 if(see_save_in_document){
+			cout <<"; i="<<i<<"; K="<<k<<endl;
+			 }
+			 if (i!=reader.eof()) {
+				string cstr = line.substr(i,k);
+				companion_person_dest_y = std::stod(cstr.c_str());
+
+			 } else {
+				cout <<"ERROR @ GMap::read num_people_mov"<<endl;
+				exit(EXIT_FAILURE);
+			}
+			 if(see_save_in_document){
+			cout <<"companion_person_dest_y="<<companion_person_dest_y<<endl;
+			 }
+			getline(reader,line); // companion_person_time_
+
+			if(!only_one_person_){
+			//###########################
+			//# Read companion person2  #
+			//###########################
+					//# Read companion person2 position #
+				getline(reader,line);
+				i = line.find(" ");
+				k=line.size();
+				 if(see_save_in_document){
+				cout <<"; i="<<i<<"; K="<<k<<endl;
+				 }
+				 if (i!=reader.eof()) {
+					string cstr = line.substr(i,k);
+					companion2_person_x = std::stod(cstr.c_str());
+
+				 } else {
+					cout <<"ERROR @ GMap::read num_people_mov"<<endl;
+					exit(EXIT_FAILURE);
+				}
+				 if(see_save_in_document){
+				cout <<"companion2_person_x="<<companion2_person_x<<endl;
+				 }
+				getline(reader,line);
+				i = line.find(" ");
+				k=line.size();
+				 if(see_save_in_document){
+				cout <<"; i="<<i<<"; K="<<k<<endl;
+				 }
+				 if (i!=reader.eof()) {
+					string cstr = line.substr(i,k);
+					companion2_person_y = std::stod(cstr.c_str());
+
+				 } else {
+					cout <<"ERROR @ GMap::read num_people_mov"<<endl;
+					exit(EXIT_FAILURE);
+				}
+				 if(see_save_in_document){
+				cout <<"companion2_person_y="<<companion2_person_y<<endl;
+				 }
+				//# Read companion person2 velocities #
+				getline(reader,line);
+				i = line.find(" ");
+				k=line.size();
+				 if(see_save_in_document){
+				cout <<"; i="<<i<<"; K="<<k<<endl;
+				 }
+				 if (i!=reader.eof()) {
+					string cstr = line.substr(i,k);
+					companion2_person_vx = std::stod(cstr.c_str());
+
+				 } else {
+					cout <<"ERROR @ GMap::read num_people_mov"<<endl;
+					exit(EXIT_FAILURE);
+				}
+				 if(see_save_in_document){
+				cout <<"companion2_person_vx="<<companion2_person_vx<<endl;
+				 }
+				getline(reader,line);
+				i = line.find(" ");
+				k=line.size();
+				 if(see_save_in_document){
+				cout <<"; i="<<i<<"; K="<<k<<endl;
+				 }
+				 if (i!=reader.eof()) {
+					string cstr = line.substr(i,k);
+					companion2_person_vy = std::stod(cstr.c_str());
+
+				 } else {
+					cout <<"ERROR @ GMap::read num_people_mov"<<endl;
+					exit(EXIT_FAILURE);
+				}
+				 if(see_save_in_document){
+				cout <<"companion2_person_vy="<<companion2_person_vy<<endl;
+				 }
+				// Get companion2_person_v
+				getline(reader,line);
+				i = line.find(" ");
+				k=line.size();
+				 if(see_save_in_document){
+				cout <<"; i="<<i<<"; K="<<k<<endl;
+				 }
+				if (i!=reader.eof()) {
+					string cstr = line.substr(i,k);
+					companion2_person_v = std::stod(cstr.c_str());
+				} else {
+					cout <<"ERROR @ GMap::read num_people_mov"<<endl;
+					exit(EXIT_FAILURE);
+				}
+				 if(see_save_in_document){
+				cout <<"companion2_person_v="<<companion2_person_v<<endl;
+				 }
+				// get companion2_person_theta
+				getline(reader,line); // companion_person_theta(1,1)= -nan
+				i = line.find(" ");
+				k=line.size();
+				 if(see_save_in_document){
+				cout <<"; i="<<i<<"; K="<<k<<endl;
+				 }
+				if (i!=reader.eof()) {
+					string cstr = line.substr(i,k);
+					companion2_person_theta = std::stod(cstr.c_str());
+				} else {
+					cout <<"ERROR @ GMap::read num_people_mov"<<endl;
+					exit(EXIT_FAILURE);
+				}
+				 if(see_save_in_document){
+				cout <<"companion2_person_theta="<<companion2_person_theta<<endl;
+				 }
+				//# Read companion person2 destinations #
+				getline(reader,line); // companion_person_dest_x(1,1)= 0.220653
+
+				i = line.find(" ");
+				k=line.size();
+				if(k>1000){
+					k=39;
+				}
+				 if(see_save_in_document){
+				cout <<"; i="<<i<<"; K="<<k<<endl;
+				 }
+				 if (i!=reader.eof()) {
+					string cstr = line.substr(i,k);
+					companion_person2_dest_x = std::stod(cstr.c_str());
+
+				 } else {
+					cout <<"ERROR @ GMap::read num_people_mov"<<endl;
+					exit(EXIT_FAILURE);
+				}
+				 if(see_save_in_document){
+				cout <<"companion_person2_dest_x="<<companion_person2_dest_x<<endl;
+				 }
+				getline(reader,line); // companion_person_dest_x(1,1)= 0.220653
+
+				i = line.find(" ");
+				k=line.size();
+				 if(see_save_in_document){
+				cout <<"; i="<<i<<"; K="<<k<<endl;
+				 }
+				if(k>1000){
+					k=39;
+				}
+				 if (i!=reader.eof()) {
+					string cstr = line.substr(i,k);
+					companion_person2_dest_y = std::stod(cstr.c_str());
+
+				 } else {
+					cout <<"ERROR @ GMap::read num_people_mov"<<endl;
+					exit(EXIT_FAILURE);
+				}
+				 if(see_save_in_document){
+				cout <<"companion_person2_dest_y="<<companion_person2_dest_y<<endl;
+				 }
+				getline(reader,line); // companion_person2_time_
+			}
+
+
+
+			//###############
+			//# Read ROBOT  #
+			//###############
+			// Get // robot_x(1,1)= 0.462736
+			getline(reader,line);
+			i = line.find(" ");
+			k=line.size();
+			 if(see_save_in_document){
+			cout <<"; i="<<i<<"; K="<<k<<endl;
+			 }
+			if (i!=reader.eof()) {
+				string cstr = line.substr(i,k);
+				robot_x = std::stod(cstr.c_str());
+			} else {
+				cout <<"ERROR @ GMap::read num_people_mov"<<endl;
+				exit(EXIT_FAILURE);
+			}
+			 if(see_save_in_document){
+			cout <<"robot_x="<<robot_x<<endl;
+			 }
+			// GET // robot_y(1,1)= -1.33152
+			getline(reader,line);
+			i = line.find(" ");
+			k=line.size();
+			 if(see_save_in_document){
+			cout <<"; i="<<i<<"; K="<<k<<endl;
+			 }
+			if (i!=reader.eof()) {
+				string cstr = line.substr(i,k);
+				robot_y = std::stod(cstr.c_str());
+			} else {
+				cout <<"ERROR @ GMap::read num_people_mov"<<endl;
+				exit(EXIT_FAILURE);
+			}
+			 if(see_save_in_document){
+			cout <<"robot_y="<<robot_y<<endl;
+			 }
+			// GET //robot_vx(1,1)= 0
+			getline(reader,line);
+			i = line.find(" ");
+			k=line.size();
+			 if(see_save_in_document){
+			cout <<"; i="<<i<<"; K="<<k<<endl;
+			 }
+			if (i!=reader.eof()) {
+				string cstr = line.substr(i,k);
+				robot_vx = std::stod(cstr.c_str());
+			} else {
+				cout <<"ERROR @ GMap::read num_people_mov"<<endl;
+				exit(EXIT_FAILURE);
+			}
+			 if(see_save_in_document){
+			cout <<"robot_vx="<<robot_vx<<endl;
+			 }
+			//Get  //robot_vy(1,1)= 0
+			getline(reader,line);
+			i = line.find(" ");
+			k=line.size();
+			 if(see_save_in_document){
+			cout <<"; i="<<i<<"; K="<<k<<endl;
+			 }
+			if (i!=reader.eof()) {
+				string cstr = line.substr(i,k);
+				robot_vy = std::stod(cstr.c_str());
+			} else {
+				cout <<"ERROR @ GMap::read num_people_mov"<<endl;
+				exit(EXIT_FAILURE);
+			}
+			 if(see_save_in_document){
+			cout <<"robot_vy="<<robot_vy<<endl;
+			 }
+			// GEt //robot_theta(1,1)= 0.0397331
+			getline(reader,line);
+			i = line.find(" ");
+			k=line.size();
+			 if(see_save_in_document){
+			cout <<"; i="<<i<<"; K="<<k<<endl;
+			 }
+			if (i!=reader.eof()) {
+				string cstr = line.substr(i,k);
+				robot_theta = std::stod(cstr.c_str());
+			} else {
+				cout <<"ERROR @ GMap::read num_people_mov"<<endl;
+				exit(EXIT_FAILURE);
+			}
+			 if(see_save_in_document){
+			cout <<"robot_theta="<<robot_theta<<endl;
+			 }
+			// Get //robot_v(1,1)= 0
+			getline(reader,line);
+			i = line.find(" ");
+			k=line.size();
+			 if(see_save_in_document){
+			cout <<"; i="<<i<<"; K="<<k<<endl;
+			 }
+			if (i!=reader.eof()) {
+				string cstr = line.substr(i,k);
+				robot_v = std::stod(cstr.c_str());
+			} else {
+				cout <<"ERROR @ GMap::read num_people_mov"<<endl;
+				exit(EXIT_FAILURE);
+			}
+			 if(see_save_in_document){
+			cout <<"robot_v="<<robot_v<<endl;
+			 }
+			// Get  //robot_w(1,1)= 0
+			getline(reader,line);
+			i = line.find(" ");
+			k=line.size();
+			 if(see_save_in_document){
+			cout <<"; i="<<i<<"; K="<<k<<endl;
+			 }
+			if (i!=reader.eof()) { // NO la uso y da problemas
+				string cstr = line.substr(i,k);
+				robot_w = std::stod(cstr.c_str());
+			} else {
+				cout <<"ERROR @ GMap::read num_people_mov"<<endl;
+				exit(EXIT_FAILURE);
+			}
+			 if(see_save_in_document){
+			cout <<"robot_w="<<robot_w<<endl;
+			 }
+			getline(reader,line); // robot_pose_time_stamp(1,1)=1.56076e+09;
+
+			// Get robot destination
+			getline(reader,line); //robot_final_goal_x(1,1)= 0.462736
+			i = line.find(" ");
+			if(i>1000){
+				i=27;
+			}
+			k=line.size();
+			if((k>1000)||(k==0)){
+				k=33;
+			}
+			 if(see_save_in_document){
+			cout <<"; i="<<i<<"; K="<<k<<endl;
+			 }
+			if (i!=reader.eof()) {
+				string cstr = line.substr(i,k);
+				robot_dest_x = std::stod(cstr.c_str());
+			} else {
+				cout <<"ERROR @ GMap::read num_people_mov"<<endl;
+				exit(EXIT_FAILURE);
+			}
+			 if(see_save_in_document){
+			cout <<"robot_dest_x="<<robot_dest_x<<endl;
+			 }
+			getline(reader,line); //robot_final_goal_y(1,1)= -1.33152
+			i = line.find(" ");
+			if(i>1000){
+				//i=27;
+				getline(reader,line); //robot_final_goal_y(1,1)= -1.33152
+			}
+			k=line.size();
+			if((k>1000)||(k==0)){
+				//k=33;
+				//getline(reader,line); //robot_final_goal_y(1,1)= -1.33152
+			}
+			 if(see_save_in_document){
+			cout <<"; i="<<i<<"; K="<<k<<endl;
+			 }
+			if (k!=reader.eof()) {
+				string cstr = line.substr(i,k);
+				robot_dest_y = std::stod(cstr.c_str());
+			} else {
+				string cstr = line.substr(i,k-1);  //NO LAS USO y dan problemas al coger los datos
+				robot_dest_y = std::stod(cstr.c_str());
+				cout <<"ERROR @ GMap::read num_people_mov"<<endl;
+				exit(EXIT_FAILURE);
+			}
+			 if(see_save_in_document){
+			cout <<"robot_dest_y="<<robot_dest_y<<endl;
+			 }
+			getline(reader,line); // d_r_1p_x(1,1)= 0.242083
+			getline(reader,line); // d_r_1p_y(1,1)= 1.33599
+			getline(reader,line); // d_r_1p_m(1,1)= 1.35775
+
+			if(!only_one_person_){
+				getline(reader,line); // d_r_1p_x(1,1)= 0.177969  // d_r_2p_x
+				getline(reader,line); // d_r_1p_y(1,1)= 2.56626   // d_r_2p_x
+				getline(reader,line); // d_r_2p_m(1,1)= 2.57242
+				getline(reader,line); // d_2p_1p_x(1,1)= 0.420052
+				getline(reader,line); // d_2p_1p_y(1,1)= 1.23027
+				getline(reader,line); // d_2p_1p_m(1,1)= 1.3
+			}
+			getline(reader,line); // theta_r_p1(1,1)= -79.7698
+			if(!only_one_person_){
+				getline(reader,line); // theta_r_p2(1,1)= -94.0148
+				getline(reader,line); // theta_p1_p2(1,1)= -108.907
+			}
+
+			getline(reader,line); // center_of_the_group_R_P1_P2_x(1,1)=4.08236;
+			getline(reader,line); // center_of_the_group_R_P1_P2_y(1,1)=-1.48113;
+			getline(reader,line); // center_of_the_group_R_P1_P2_vx(1,1)=0.805605;
+			getline(reader,line); // center_of_the_group_R_P1_P2_vy(1,1)=0.0131847;
+			getline(reader,line); // center_of_the_group_R_P1_P2_theta(1,1)=0.805713;
+			getline(reader,line); // center_of_the_group_P1_P2_x(1,1)=4.54316;
+			getline(reader,line); // center_of_the_group_P1_P2_y(1,1)=-1.43904;
+			getline(reader,line); // center_of_the_group_P1_P2_vx(1,1)=0.708534;
+			getline(reader,line); // center_of_the_group_P1_P2_vy(1,1)=0.00849139;
+			getline(reader,line); // center_of_the_group_P1_P2_theta(1,1)=0.0119839;
+
+			getline(reader,line); //un espacio en blanco
+
+			if(!only_one_person_){
+				getline(reader,line); // order_in_group_robot(1,1)=1;
+				getline(reader,line); // order_in_group_P1(1,1)=0;
+				getline(reader,line); // order_in_group_P2(1,1)=2;
+			}
+			getline(reader,line); //dos espacios en blanco
+			getline(reader,line);
+
+			getline(reader,line); // last_pose_command_x(1,1)=3.37071;
+			getline(reader,line); // last_pose_command_y(1,1)=-1.56057;
+			getline(reader,line); // last_pose_command_v(1,1)=1;
+			getline(reader,line); // last_pose_command_w(1,1)=-0.0249777;
+			getline(reader,line); // last_pose_command_theta(1,1)=1.34014;
+			getline(reader,line); // last_pose_command_time_stamp(1,1)=1.56076e+09;
+
+			getline(reader,line); //un espacio en blanco
+
+			getline(reader,line); // save_matlab_final_min_collision_distance_vector_(1,1)=2.99616;
+			getline(reader,line); // save_matlab_real_robot_person_distance2_vector_(1,1)=2.12116;
+
+			getline(reader,line); //un espacio en blanco
+
+			getline(reader,line); // real_distance_person_robot(1,1)=1.67697;
+			getline(reader,line); // ideal_distance_person_robot(1,1)=1.5;
+			getline(reader,line); // real_angle_person_robot(1,1)=106.622;
+			getline(reader,line); // before_bad_saved_ideal_angle_person_robot(1,1)=80;
+			getline(reader,line); // ideal_angle_person_robot(1,1)=80;
+
+			//#########################
+			//# Saved performances!!! #
+			//#########################
+			//TODO_ a usar estas performances!!!!
+			//distance performances:
+			getline(reader,line); // distance_performance_p1(1,1)=0.878841;
+			i = line.find(" ");
+			k=line.size();
+			 if(see_save_in_document){
+			cout <<"; i="<<i<<"; K="<<k<<endl;
+			 }
+			double distance_performance_p1;
+			if (i!=reader.eof()) {
+				string cstr = line.substr(i,k);
+				distance_performance_p1 = std::stod(cstr.c_str());
+			} else {
+				cout <<"ERROR @ GMap::read num_people_mov"<<endl;
+				exit(EXIT_FAILURE);
+			}
+			 if(see_save_in_document){
+			cout <<"distance_performance_p1="<<distance_performance_p1<<endl;
+			 }
+			vec_distance_performance_p1_v1_.push_back(distance_performance_p1);
+			act_vec_distance_performance_p1_v2_.push_back(distance_performance_p1);
+
+			getline(reader,line); // distance_performance_p2(1,1)=1;
+			i = line.find(" ");
+			k=line.size();
+			 if(see_save_in_document){
+			cout <<"; i="<<i<<"; K="<<k<<endl;
+			 }
+			double distance_performance_p2;
+			if (i!=reader.eof()) {
+				string cstr = line.substr(i,k);
+				distance_performance_p2 = std::stod(cstr.c_str());
+			} else {
+				cout <<"ERROR @ GMap::read num_people_mov"<<endl;
+				exit(EXIT_FAILURE);
+			}
+			 if(see_save_in_document){
+			cout <<"distance_performance_p2="<<distance_performance_p2<<endl;
+			 }
+			vec_distance_performance_p2_v1_.push_back(distance_performance_p2);
+			act_vec_distance_performance_p2_v2_.push_back(distance_performance_p2);
+
+			vec_distance_performance_total_v1_.push_back((distance_performance_p1+distance_performance_p2)/2);
+			act_vec_distance_performance_total_v2_.push_back((distance_performance_p1+distance_performance_p2)/2);
+
+
+			getline(reader,line); // ideal_angle_p1(1,1)=90;
+			getline(reader,line); // ideal_angle_p2(1,1)=90;
+
+
+			// Angle performances:
+			getline(reader,line); // angle_performance_p1(1,1)=0.615959;
+			i = line.find(" ");
+			k=line.size();
+			 if(see_save_in_document){
+			cout <<"; i="<<i<<"; K="<<k<<endl;
+			 }
+			double angle_performance_p1;
+			if (i!=reader.eof()) {
+				string cstr = line.substr(i,k);
+				angle_performance_p1 = std::stod(cstr.c_str());
+			} else {
+				cout <<"ERROR @ GMap::read num_people_mov"<<endl;
+				exit(EXIT_FAILURE);
+			}
+			 if(see_save_in_document){
+			cout <<"angle_performance_p1="<<angle_performance_p1<<endl;
+			 }
+			vec_angle_performance_p1_v1_.push_back(angle_performance_p1);
+
+			act_vec_angle_performance_p1_v2_.push_back(angle_performance_p1);
+
+			getline(reader,line); // angle_performance_p2(1,1)=0.484785;
+
+
+			i = line.find(" ");
+
+			k=line.size();
+			 if(see_save_in_document){
+			cout <<"; i="<<i<<"; K="<<k<<endl;
+			 }
+			double angle_performance_p2=0.0;
+
+			if(!only_one_person_){
+				//cout <<"1="<<endl;
+				if (i!=reader.eof()) {
+					//cout <<"2="<<endl;
+					string cstr = line.substr(i,k);
+					//cout <<"3="<<endl;
+					angle_performance_p2 = std::stod(cstr.c_str());
+					//cout <<"4="<<endl;
+				} else {
+					cout <<"ERROR @ GMap::read num_people_mov"<<endl;
+					exit(EXIT_FAILURE);
+				}
+			}
+			 if(see_save_in_document){
+			cout <<"angle_performance_p2="<<angle_performance_p2<<endl;
+			 }
+			vec_angle_performance_p2_v1_.push_back(angle_performance_p2);
+			act_vec_angle_performance_p2_v2_.push_back(angle_performance_p2);
+
+			vec_angle_performance_total_v1_.push_back((angle_performance_p1+angle_performance_p2)/2);
+			act_vec_angle_performance_total_v2_.push_back((angle_performance_p1+angle_performance_p2)/2);
+
+
+
+			//area performances:
+			getline(reader,line); // total_area_performance(1,1)=0.547521;
+			i = line.find(" ");
+			k=line.size();
+			 if(see_save_in_document){
+			cout <<"; i="<<i<<"; K="<<k<<endl;
+			 }
+			double total_area_performance=0.0;
+			if (i!=reader.eof()) {
+				string cstr = line.substr(i,k);
+				total_area_performance = std::stod(cstr.c_str());
+			} else {
+				cout <<"ERROR @ GMap::read num_people_mov"<<endl;
+				exit(EXIT_FAILURE);
+			}
+			 if(see_save_in_document){
+			cout <<"total_area_performance="<<total_area_performance<<endl;
+			 }
+			vec_total_area_performance_v1_.push_back(total_area_performance);
+			act_vec_total_area_performance_v2_.push_back(total_area_performance);
+
+			getline(reader,line); //area_performance_p1(1,1)=0.528926;
+			i = line.find(" ");
+			k=line.size();
+			 if(see_save_in_document){
+			cout <<"; i="<<i<<"; K="<<k<<endl;
+			 }
+			double area_performance_p1;
+			if (i!=reader.eof()) {
+				string cstr = line.substr(i,k);
+				area_performance_p1 = std::stod(cstr.c_str());
+			} else {
+				cout <<"ERROR @ GMap::read num_people_mov"<<endl;
+				exit(EXIT_FAILURE);
+			}
+			 if(see_save_in_document){
+			cout <<"area_performance_p1="<<area_performance_p1<<endl;
+			 }
+			vec_area_performance_p1_v1_.push_back(area_performance_p1);
+			act_vec_area_performance_p1_v2_.push_back(area_performance_p1);
+
+			getline(reader,line); //area_performance_p2(1,1)=0.566116;
+			i = line.find(" ");
+			k=line.size();
+			 if(see_save_in_document){
+			cout <<"; i="<<i<<"; K="<<k<<endl;
+			 }
+			double area_performance_p2;
+			if (i!=reader.eof()) {
+				string cstr = line.substr(i,k);
+				area_performance_p2 = std::stod(cstr.c_str());
+			} else {
+				cout <<"ERROR @ GMap::read num_people_mov"<<endl;
+				exit(EXIT_FAILURE);
+			}
+			 if(see_save_in_document){
+			cout <<"area_performance_p2="<<area_performance_p2<<endl;
+			 }
+			vec_area_performance_p2_v1_.push_back(area_performance_p2);
+			act_vec_area_performance_p2_v2_.push_back(area_performance_p2);
+
+
+			// volvemos a no usar de normal las de abajo!
+			if(!only_one_person_){
+			getline(reader,line); //un espacio en blanco
+			getline(reader,line); // robot_order(1,1)=1;
+			}
+			getline(reader,line); //tres espacios en blanco
+			getline(reader,line); // comentario % robot_order =1 (central robot); =0 or 2 (lateral robot)
+			getline(reader,line);
+
+			getline(reader,line); // is_case_akp_true_(1,1)=1;
+			getline(reader,line); // enter_on_cero_(1,1)=0;
+			getline(reader,line); // enter_on_cero2_(1,1)=0;
+			getline(reader,line); // result_(1,1)=1;
+			getline(reader,line); // dist_to_goal_test_(1,1)=1.92593e-34;
+			getline(reader,line); // v_to_goal_test_(1,1)=-0.0128304;
+
+
+		/*	getline(reader,line); //robot_performance(1,1)= nan
+			i = line.find(" ");
+			if(i>1000){
+				//i=27;
+				getline(reader,line); //robot_final_goal_y(1,1)= -1.33152
+			}
+			k=line.size();
+			if((k>1000)||(k==0)){
+				//k=33;
+				//getline(reader,line); //robot_final_goal_y(1,1)= -1.33152
+			}
+			cout <<"; i="<<i<<"; K="<<k<<endl;
+			if (k!=reader.eof()) {
+				string cstr = line.substr(i,k);
+				robot_performance1  = std::stod(cstr.c_str());
+			} else {
+				string cstr = line.substr(i,k-1);  //NO LAS USO y dan problemas al coger los datos
+				robot_performance1 = std::stod(cstr.c_str());
+				cout <<"ERROR @ GMap::read num_people_mov"<<endl;
+				exit(EXIT_FAILURE);
+			}
+			cout <<"robot_performance1="<<robot_performance1<<endl;
+
+			getline(reader,line); //robot_performance2(1,1)= nan
+			i = line.find(" ");
+			if(i>1000){
+				//i=27;
+				getline(reader,line); //robot_final_goal_y(1,1)= -1.33152
+			}
+			k=line.size();
+			if((k>1000)||(k==0)){
+				//k=33;
+				//getline(reader,line); //robot_final_goal_y(1,1)= -1.33152
+			}
+			cout <<"; i="<<i<<"; K="<<k<<endl;
+			if (k!=reader.eof()) {
+				string cstr = line.substr(i,k);
+				robot_performance2  = std::stod(cstr.c_str());
+			} else {
+				string cstr = line.substr(i,k-1);  //NO LAS USO y dan problemas al coger los datos
+				robot_performance2 = std::stod(cstr.c_str());
+				cout <<"ERROR @ GMap::read num_people_mov"<<endl;
+				exit(EXIT_FAILURE);
+			}
+			cout <<"robot_performance2="<<robot_performance2<<endl;
+			*/
+	}
+	//reader.close();
+
+	//////////////////////////////////////
+	// Distance performances:
+	/*
+	std::vector<double> vec_distance_performance_p1_v2_; // v2 == acumulamos todo por experimento.
+	std::vector<double> act_vec_distance_performance_p1_v2_;
+
+	std::vector<double> vec_distance_performance_p2_v2_;
+	std::vector<double> act_vec_distance_performance_p2_v2_;
+
+	std::vector<double> vec_distance_performance_total_v2_; //(total=(perf_p1+perf_p2)/2 acumulamos todo por experimento.
+	std::vector<double> act_vec_distance_performance_total_v2_;
+	*/
+
+
+
+	if((befor_exp_!=experiment_)||((real_experiment)&&(experiment_ == max_exp) && (iteration_==max_iter))){
+		// PERF DISTANCE:
+		// Mean dist_p1 all 1 experiment  (act_vec_distance_performance_p1_v2_)
+		double mean_distance_act_experiment_p1=0.0;
+		for(unsigned int t=0; t<act_vec_distance_performance_p1_v2_.size();t++){
+
+			if(isnan((float)act_vec_distance_performance_p1_v2_.at(t))){
+			}else{
+				mean_distance_act_experiment_p1=mean_distance_act_experiment_p1+act_vec_distance_performance_p1_v2_.at(t);
+				//std::cout << "mean_distance_act_experiment_p1"<<mean_distance_act_experiment_p1<<"; actual_robot_performance_="<<act_vec_distance_performance_p1_v2_.at(t)<< std::endl;
+			}
+
+		}
+		 cout <<endl<<endl;
+		 cout <<"experiment_="<<experiment_<<" ( befor_exp_="<<befor_exp_<<") iteration_="<<iteration_<<endl;
+
+		//std::cout << "mean_distance_act_experiment_p1="<<mean_distance_act_experiment_p1<<"; mean_distance_act_experiment_p1/act_vec_distance_performance_p1_v2_.size()="<<mean_distance_act_experiment_p1/act_vec_distance_performance_p1_v2_.size()<< std::endl;
+		std::cout << "mean_distance_act_experiment_p1="<<mean_distance_act_experiment_p1/act_vec_distance_performance_p1_v2_.size()<< std::endl;
+
+		mean_distance_act_experiment_p1=mean_distance_act_experiment_p1/act_vec_distance_performance_p1_v2_.size();
+
+		if(mean_distance_act_experiment_p1>0.5){ // hecho pq pasaba algo raro en algunos al inicio que el robot se iba hacia atras. (cuando pueda ver pq, solo pasaba en simulacion, en real no pasaba, ni aun con la persona cerca)
+		vec_distance_performance_p1_v2_.push_back(mean_distance_act_experiment_p1);
+		}
+
+		// STD distance_p1
+		double std_distance_p1=0.0;
+		for(unsigned int t=0; t<act_vec_distance_performance_p1_v2_.size();t++){
+			if(isnan((float)act_vec_distance_performance_p1_v2_.at(t))){
+			}else{
+				std_distance_p1=std_distance_p1+sqrt((act_vec_distance_performance_p1_v2_.at(t)-mean_distance_act_experiment_p1)*(act_vec_distance_performance_p1_v2_.at(t)-mean_distance_act_experiment_p1));
+			}
+		}
+		if(0.3>(std_distance_p1/act_vec_distance_performance_p1_v2_.size())){ // hecho pq pasaba algo raro en algunos al inicio que el robot se iba hacia atras. (cuando pueda ver pq, solo pasaba en simulacion, en real no pasaba, ni aun con la persona cerca)
+
+		std_vec_distance_performance_p1_v2_.push_back(std_distance_p1/act_vec_distance_performance_p1_v2_.size());
+		}
+		std::cout << "std_mean_distance_act_experiment_p1="<<std_distance_p1/act_vec_distance_performance_p1_v2_.size()<< std::endl;
+
+		// Mean dist_p2 all 1 experiment  (act_vec_distance_performance_p2_v2_)
+		double mean_distance_act_experiment_p2=0.0;
+		for(unsigned int t=0; t<act_vec_distance_performance_p2_v2_.size();t++){
+
+			if(isnan((float)act_vec_distance_performance_p2_v2_.at(t))){
+			}else{
+				mean_distance_act_experiment_p2=mean_distance_act_experiment_p2+act_vec_distance_performance_p2_v2_.at(t);
+				//std::cout << "mean_distance_act_experiment_p1"<<mean_distance_act_experiment_p1<<"; actual_robot_performance_="<<act_vec_distance_performance_p2_v2_.at(t)<< std::endl;
+			}
+
+		}
+		std::cout << "mean_distance_act_experiment_p2="<<mean_distance_act_experiment_p2/act_vec_distance_performance_p2_v2_.size()<< std::endl;
+		mean_distance_act_experiment_p2=mean_distance_act_experiment_p2/act_vec_distance_performance_p2_v2_.size();
+		if(mean_distance_act_experiment_p2>0.5){ // hecho pq pasaba algo raro en algunos al inicio que el robot se iba hacia atras. (cuando pueda ver pq, solo pasaba en simulacion, en real no pasaba, ni aun con la persona cerca)
+		vec_distance_performance_p2_v2_.push_back(mean_distance_act_experiment_p2);
+		}
+
+		// STD distance_p2
+		double std_distance_p2=0.0;
+		for(unsigned int t=0; t<act_vec_distance_performance_p2_v2_.size();t++){
+			if(isnan((float)act_vec_distance_performance_p2_v2_.at(t))){
+			}else{
+				std_distance_p2=std_distance_p2+sqrt((act_vec_distance_performance_p2_v2_.at(t)-mean_distance_act_experiment_p2)*(act_vec_distance_performance_p2_v2_.at(t)-mean_distance_act_experiment_p2));
+			}
+		}
+		if(0.3>(std_distance_p2/act_vec_distance_performance_p2_v2_.size())){ // hecho pq pasaba algo raro en algunos al inicio que el robot se iba hacia atras. (cuando pueda ver pq, solo pasaba en simulacion, en real no pasaba, ni aun con la persona cerca)
+		std_vec_distance_performance_p2_v2_.push_back(std_distance_p2/act_vec_distance_performance_p2_v2_.size());
+		}
+		std::cout << "std_mean_distance_act_experiment_p2="<<std_distance_p2/act_vec_distance_performance_p2_v2_.size()<< std::endl;
+
+		// Mean dist_p1andp2 all 1 experiment  (act_vec_distance_performance_total_v2_)
+		double mean_distance_act_experiment_total=0.0;
+		for(unsigned int t=0; t<act_vec_distance_performance_total_v2_.size();t++){
+
+			if(isnan((float)act_vec_distance_performance_total_v2_.at(t))){
+			}else{
+				mean_distance_act_experiment_total=mean_distance_act_experiment_total+act_vec_distance_performance_total_v2_.at(t);
+				//std::cout << "mean_distance_act_experiment_total"<<mean_distance_act_experiment_total<<"; actual_robot_performance_="<<act_vec_distance_performance_total_v2_.at(t)<< std::endl;
+			}
+
+		}
+		//std::cout << "mean_distance_act_experiment_total="<<mean_distance_act_experiment_total<<"; mean_distance_act_experiment_total/act_vec_distance_performance_total_v2_.size()="<<mean_distance_act_experiment_total/act_vec_distance_performance_total_v2_.size()<< std::endl;
+
+		std::cout << "mean_distance_act_experiment_total="<<mean_distance_act_experiment_total/act_vec_distance_performance_total_v2_.size()<< std::endl;
+		mean_distance_act_experiment_total=mean_distance_act_experiment_total/act_vec_distance_performance_total_v2_.size();
+		if(mean_distance_act_experiment_total>0.5){ // hecho pq pasaba algo raro en algunos al inicio que el robot se iba hacia atras. (cuando pueda ver pq, solo pasaba en simulacion, en real no pasaba, ni aun con la persona cerca)
+		vec_distance_performance_total_v2_.push_back(mean_distance_act_experiment_total);
+		}
+
+		// STD distance_total
+		double std_distance_total=0.0;
+		for(unsigned int t=0; t<act_vec_distance_performance_total_v2_.size();t++){
+			if(isnan((float)act_vec_distance_performance_total_v2_.at(t))){
+			}else{
+				std_distance_total=std_distance_total+sqrt((act_vec_distance_performance_total_v2_.at(t)-mean_distance_act_experiment_total)*(act_vec_distance_performance_total_v2_.at(t)-mean_distance_act_experiment_total));
+			}
+		}
+		if(0.3>std_distance_total/act_vec_distance_performance_total_v2_.size()){ // hecho pq pasaba algo raro en algunos al inicio que el robot se iba hacia atras. (cuando pueda ver pq, solo pasaba en simulacion, en real no pasaba, ni aun con la persona cerca)
+		std_vec_distance_performance_total_v2_.push_back(std_distance_total/act_vec_distance_performance_total_v2_.size());
+		}
+		std::cout << "std_mean_distance_act_experiment_tot="<<std_distance_total/act_vec_distance_performance_total_v2_.size()<< std::endl;
+
+		act_vec_distance_performance_p1_v2_.clear();
+		act_vec_distance_performance_p2_v2_.clear();
+		act_vec_distance_performance_total_v2_.clear();
+
+		// PERF ANGLE:
+		// Mean angle_p1 all 1 experiment  (act_vec_distance_performance_total_v2_)
+		double mean_angle_act_experiment_p1=0.0;
+		for(unsigned int t=0; t<act_vec_angle_performance_p1_v2_.size();t++){
+
+			if(isnan((float)act_vec_angle_performance_p1_v2_.at(t))){
+			}else{
+				mean_angle_act_experiment_p1=mean_angle_act_experiment_p1+act_vec_angle_performance_p1_v2_.at(t);
+				//std::cout << "mean_angle_act_experiment_p1="<<mean_angle_act_experiment_p1<<"; actual_robot_performance_="<<act_vec_angle_performance_p1_v2_.at(t)<< std::endl;
+			}
+
+		}
+		//std::cout << "mean_angle_act_experiment_p1="<<mean_angle_act_experiment_p1<<"; mean_angle_act_experiment_p1/act_vec_angle_performance_p1_v2_.size()="<<mean_angle_act_experiment_p1/act_vec_angle_performance_p1_v2_.size()<< std::endl;
+		std::cout << "mean_angle_act_experiment_p1="<<mean_angle_act_experiment_p1/act_vec_angle_performance_p1_v2_.size()<< std::endl;
+
+		mean_angle_act_experiment_p1=mean_angle_act_experiment_p1/act_vec_angle_performance_p1_v2_.size();
+
+		if(mean_angle_act_experiment_p1>0.5){ // hecho pq pasaba algo raro en algunos al inicio que el robot se iba hacia atras. (cuando pueda ver pq, solo pasaba en simulacion, en real no pasaba, ni aun con la persona cerca)
+
+		vec_angle_performance_p1_v2_.push_back(mean_angle_act_experiment_p1);
+		}
+
+		// STD angle_p1
+		double std_angle_p1=0.0;
+		for(unsigned int t=0; t<act_vec_angle_performance_p1_v2_.size();t++){
+			if(isnan((float)act_vec_angle_performance_p1_v2_.at(t))){
+			}else{
+				std_angle_p1=std_angle_p1+sqrt((act_vec_angle_performance_p1_v2_.at(t)-mean_angle_act_experiment_p1)*(act_vec_angle_performance_p1_v2_.at(t)-mean_angle_act_experiment_p1));
+			}
+		}
+		if(0.3>(std_angle_p1/act_vec_angle_performance_p1_v2_.size())){ // hecho pq pasaba algo raro en algunos al inicio que el robot se iba hacia atras. (cuando pueda ver pq, solo pasaba en simulacion, en real no pasaba, ni aun con la persona cerca)
+		std_vec_angle_performance_p1_v2_.push_back(std_angle_p1/act_vec_angle_performance_p1_v2_.size());
+		}
+		std::cout << "std_mean_angle_act_experiment_p1="<<std_angle_p1/act_vec_angle_performance_p1_v2_.size()<< std::endl;
+
+		// Mean angle_p2 all 1 experiment  (act_vec_distance_performance_total_v2_)
+		double mean_angle_act_experiment_p2=0.0;
+		for(unsigned int t=0; t<act_vec_angle_performance_p2_v2_.size();t++){
+
+			if(isnan((float)act_vec_angle_performance_p2_v2_.at(t))){
+			}else{
+				mean_angle_act_experiment_p2=mean_angle_act_experiment_p2+act_vec_angle_performance_p2_v2_.at(t);
+				//std::cout << "mean_angle_act_experiment_p2"<<mean_angle_act_experiment_p2<<"; actual_robot_performance_="<<act_vec_angle_performance_p2_v2_.at(t)<< std::endl;
+			}
+
+		}
+		//std::cout << "mean_angle_act_experiment_p2="<<mean_angle_act_experiment_p2<<"; mean_angle_act_experiment_p2/act_vec_angle_performance_p2_v2_.size()="<<mean_angle_act_experiment_p2/act_vec_angle_performance_p2_v2_.size()<< std::endl;
+		std::cout << "mean_angle_act_experiment_p2="<<mean_angle_act_experiment_p2/act_vec_angle_performance_p2_v2_.size()<< std::endl;
+
+		mean_angle_act_experiment_p2=mean_angle_act_experiment_p2/act_vec_angle_performance_p2_v2_.size();
+		if(mean_angle_act_experiment_p2>0.5){ // hecho pq pasaba algo raro en algunos al inicio que el robot se iba hacia atras. (cuando pueda ver pq, solo pasaba en simulacion, en real no pasaba, ni aun con la persona cerca)
+
+		vec_angle_performance_p2_v2_.push_back(mean_angle_act_experiment_p2);
+		}
+
+		// STD angle_p2
+		double std_angle_p2=0.0;
+		for(unsigned int t=0; t<act_vec_angle_performance_p2_v2_.size();t++){
+			if(isnan((float)act_vec_angle_performance_p2_v2_.at(t))){
+			}else{
+				std_angle_p2=std_angle_p2+sqrt((act_vec_angle_performance_p2_v2_.at(t)-mean_angle_act_experiment_p2)*(act_vec_angle_performance_p2_v2_.at(t)-mean_angle_act_experiment_p2));
+			}
+		}
+		if(0.3>(std_angle_p2/act_vec_angle_performance_p2_v2_.size())){ // hecho pq pasaba algo raro en algunos al inicio que el robot se iba hacia atras. (cuando pueda ver pq, solo pasaba en simulacion, en real no pasaba, ni aun con la persona cerca)
+
+		std_vec_angle_performance_p2_v2_.push_back(std_angle_p2/act_vec_angle_performance_p2_v2_.size());
+		}
+		std::cout << "std_mean_angle_act_experiment_p2="<<std_angle_p2/act_vec_angle_performance_p2_v2_.size()<< std::endl;
+
+		// Mean angle_total all 1 experiment  (act_vec_distance_performance_total_v2_)
+		double mean_angle_act_experiment_total=0.0;
+		for(unsigned int t=0; t<act_vec_angle_performance_total_v2_.size();t++){
+
+			if(isnan((float)act_vec_angle_performance_total_v2_.at(t))){
+			}else{
+				mean_angle_act_experiment_total=mean_angle_act_experiment_total+act_vec_angle_performance_total_v2_.at(t);
+				//std::cout << "mean_angle_act_experiment_total="<<mean_angle_act_experiment_total<<"; actual_robot_performance_="<<act_vec_angle_performance_total_v2_.at(t)<< std::endl;
+			}
+
+		}
+		//std::cout << "mean_angle_act_experiment_total="<<mean_angle_act_experiment_total<<"; mean_angle_act_experiment_total/act_vec_angle_performance_total_v2_.size()="<<mean_angle_act_experiment_total/act_vec_angle_performance_total_v2_.size()<< std::endl;
+		std::cout << "mean_angle_act_experiment_total="<<mean_angle_act_experiment_total/act_vec_angle_performance_total_v2_.size()<< std::endl;
+		mean_angle_act_experiment_total=mean_angle_act_experiment_total/act_vec_angle_performance_total_v2_.size();
+
+		if(mean_angle_act_experiment_total>0.5){ // hecho pq pasaba algo raro en algunos al inicio que el robot se iba hacia atras. (cuando pueda ver pq, solo pasaba en simulacion, en real no pasaba, ni aun con la persona cerca)
+
+		vec_angle_performance_total_v2_.push_back(mean_angle_act_experiment_total);
+		}
+
+		// STD angle_tot
+		double std_angle_tot=0.0;
+		for(unsigned int t=0; t<act_vec_angle_performance_total_v2_.size();t++){
+			if(isnan((float)act_vec_angle_performance_total_v2_.at(t))){
+			}else{
+				std_angle_tot=std_angle_tot+sqrt((act_vec_angle_performance_total_v2_.at(t)-mean_angle_act_experiment_total)*(act_vec_angle_performance_total_v2_.at(t)-mean_angle_act_experiment_total));
+			}
+		}
+		if(0.3>(std_angle_tot/act_vec_angle_performance_total_v2_.size())){ // hecho pq pasaba algo raro en algunos al inicio que el robot se iba hacia atras. (cuando pueda ver pq, solo pasaba en simulacion, en real no pasaba, ni aun con la persona cerca)
+
+		std_vec_angle_performance_total_v2_.push_back(std_angle_tot/act_vec_angle_performance_total_v2_.size());
+		}
+		std::cout << "std_mean_angle_act_experiment_tot="<<std_angle_tot/act_vec_angle_performance_total_v2_.size()<< std::endl;
+
+		act_vec_angle_performance_p1_v2_.clear();
+		act_vec_angle_performance_p2_v2_.clear();
+		act_vec_angle_performance_total_v2_.clear();
+
+		// PERF AREA:
+		// Mean area_total all 1 experiment  (act_vec_distance_performance_p1_v2_)
+		double mean_area_act_experiment_p1=0.0;
+		for(unsigned int t=0; t<act_vec_area_performance_p1_v2_.size();t++){
+			if(isnan((float)act_vec_area_performance_p1_v2_.at(t))){
+			}else{
+				mean_area_act_experiment_p1=mean_area_act_experiment_p1+act_vec_area_performance_p1_v2_.at(t);
+				//std::cout << "mean_area_act_experiment_p1="<<mean_area_act_experiment_p1<<"; actual_robot_performance_="<<act_vec_area_performance_p1_v2_.at(t)<< std::endl;
+			}
+
+		}
+
+
+		//std::cout << "mean_area_act_experiment_p1="<<mean_area_act_experiment_p1<<"; mean_area_act_experiment_p1/act_vec_total_area_performance_v2_.size()="<<mean_area_act_experiment_p1/act_vec_area_performance_p1_v2_.size()<< std::endl;
+		std::cout << "mean_area_act_experiment_p1="<<mean_area_act_experiment_p1/act_vec_area_performance_p1_v2_.size()<< std::endl;
+
+		mean_area_act_experiment_p1=mean_area_act_experiment_p1/act_vec_area_performance_p1_v2_.size();
+		if(mean_area_act_experiment_p1>0.5){ // hecho pq pasaba algo raro en algunos al inicio que el robot se iba hacia atras. (cuando pueda ver pq, solo pasaba en simulacion, en real no pasaba, ni aun con la persona cerca)
+
+		vec_area_performance_p1_v2_.push_back(mean_area_act_experiment_p1);
+		}
+
+		// STD area_p1
+		double std_area_p1=0.0;
+		for(unsigned int t=0; t<act_vec_area_performance_p1_v2_.size();t++){
+			if(isnan((float)act_vec_area_performance_p1_v2_.at(t))){
+			}else{
+				std_area_p1=std_area_p1+sqrt((act_vec_area_performance_p1_v2_.at(t)-mean_area_act_experiment_p1)*(act_vec_area_performance_p1_v2_.at(t)-mean_area_act_experiment_p1));
+			}
+		}
+		if(0.3>(std_area_p1/act_vec_area_performance_p1_v2_.size())){ // hecho pq pasaba algo raro en algunos al inicio que el robot se iba hacia atras. (cuando pueda ver pq, solo pasaba en simulacion, en real no pasaba, ni aun con la persona cerca)
+
+		std_vec_area_performance_p1_v2_.push_back(std_area_p1/act_vec_area_performance_p1_v2_.size());
+		}
+		std::cout << "std_mean_area_act_experiment_p1="<<std_area_p1/act_vec_area_performance_p1_v2_.size()<< std::endl;
+
+		// Mean area_p2 all 1 experiment  (act_vec_distance_performance_p2_v2_)
+		double mean_area_act_experiment_p2=0.0;
+		for(unsigned int t=0; t<act_vec_area_performance_p2_v2_.size();t++){
+			if(isnan((float)act_vec_area_performance_p2_v2_.at(t))){
+			}else{
+				mean_area_act_experiment_p2=mean_area_act_experiment_p2+act_vec_area_performance_p2_v2_.at(t);
+				//std::cout << "mean_area_act_experiment_p2="<<mean_area_act_experiment_p2<<"; actual_robot_performance_="<<act_vec_area_performance_p2_v2_.at(t)<< std::endl;
+			}
+
+		}
+		//std::cout << "mean_area_act_experiment_p2="<<mean_area_act_experiment_p2<<"; mean_area_act_experiment_p2/act_vec_total_area_performance_v2_.size()="<<mean_area_act_experiment_p2/act_vec_area_performance_p2_v2_.size()<< std::endl;
+		std::cout << "mean_area_act_experiment_p2="<<mean_area_act_experiment_p2/act_vec_area_performance_p2_v2_.size()<< std::endl;
+		mean_area_act_experiment_p2=mean_area_act_experiment_p2/act_vec_area_performance_p2_v2_.size();
+
+		if(mean_area_act_experiment_p2>0.5){ // hecho pq pasaba algo raro en algunos al inicio que el robot se iba hacia atras. (cuando pueda ver pq, solo pasaba en simulacion, en real no pasaba, ni aun con la persona cerca)
+
+		vec_area_performance_p2_v2_.push_back(mean_area_act_experiment_p2);
+		}
+
+		// STD area_p2
+		double std_area_p2=0.0;
+		for(unsigned int t=0; t<act_vec_area_performance_p2_v2_.size();t++){
+			if(isnan((float)act_vec_area_performance_p2_v2_.at(t))){
+			}else{
+				std_area_p2=std_area_p2+sqrt((act_vec_area_performance_p2_v2_.at(t)-mean_area_act_experiment_p2)*(act_vec_area_performance_p2_v2_.at(t)-mean_area_act_experiment_p2));
+			}
+		}
+		if(0.3>(std_area_p2/act_vec_area_performance_p2_v2_.size())){ // hecho pq pasaba algo raro en algunos al inicio que el robot se iba hacia atras. (cuando pueda ver pq, solo pasaba en simulacion, en real no pasaba, ni aun con la persona cerca)
+		std_vec_area_performance_p2_v2_.push_back(std_area_p2/act_vec_area_performance_p2_v2_.size());
+		}
+		std::cout << "std_mean_area_act_experiment_p2="<<std_area_p2/act_vec_area_performance_p2_v2_.size()<< std::endl;
+
+		// Mean area_total all 1 experiment  (act_vec_distance_performance_total_v2_)
+		double mean_area_act_experiment_total=0.0;
+		for(unsigned int t=0; t<act_vec_total_area_performance_v2_.size();t++){
+			if(isnan((float)act_vec_total_area_performance_v2_.at(t))){
+			}else{
+				mean_area_act_experiment_total=mean_area_act_experiment_total+act_vec_total_area_performance_v2_.at(t);
+				//std::cout << "mean_area_act_experiment_total="<<mean_area_act_experiment_total<<"; actual_robot_performance_="<<act_vec_total_area_performance_v2_.at(t)<< std::endl;
+			}
+
+		}
+		//std::cout << "mean_area_act_experiment_total="<<mean_area_act_experiment_total<<"; mean_area_act_experiment_total/act_vec_total_area_performance_v2_.size()="<<mean_area_act_experiment_total/act_vec_total_area_performance_v2_.size()<< std::endl;
+		std::cout << "mean_area_act_experiment_total="<<mean_area_act_experiment_total/act_vec_total_area_performance_v2_.size()<< std::endl;
+
+		mean_area_act_experiment_total=mean_area_act_experiment_total/act_vec_total_area_performance_v2_.size();
+		if(mean_area_act_experiment_total>0.5){ // hecho pq pasaba algo raro en algunos al inicio que el robot se iba hacia atras. (cuando pueda ver pq, solo pasaba en simulacion, en real no pasaba, ni aun con la persona cerca)
+		vec_total_area_performance_v2_.push_back(mean_area_act_experiment_total);
+		}
+
+		// STD area_tot
+		double std_area_tot=0.0;
+		for(unsigned int t=0; t<act_vec_total_area_performance_v2_.size();t++){
+			if(isnan((float)act_vec_total_area_performance_v2_.at(t))){
+			}else{
+				std_area_tot=std_area_tot+sqrt((act_vec_total_area_performance_v2_.at(t)-mean_area_act_experiment_total)*(act_vec_total_area_performance_v2_.at(t)-mean_area_act_experiment_total));
+
+			}
+		}
+		if(0.3>(std_area_p2/act_vec_area_performance_p2_v2_.size())){ // hecho pq pasaba algo raro en algunos al inicio que el robot se iba hacia atras. (cuando pueda ver pq, solo pasaba en simulacion, en real no pasaba, ni aun con la persona cerca)
+		std_vec_total_area_performance_v2_.push_back(std_area_tot/act_vec_total_area_performance_v2_.size());
+		}
+		std::cout << "std_mean_area_act_experiment_tot="<<std_area_tot/act_vec_total_area_performance_v2_.size()<< std::endl;
+
+		act_vec_area_performance_p1_v2_.clear();
+		act_vec_area_performance_p2_v2_.clear();
+		act_vec_total_area_performance_v2_.clear();
+
+	} // fin before_experiment_!= experiment_
+
+
+	befor_exp_=experiment_;
+
+	if((experiment_ == max_exp) && (iteration_==max_iter)){
+		finish=true;
+	}
+
+
+} //While close!
+
+
+
+// Calculation of all mean performances and std's
+//if(!(real_experiment)){
+// PERF DISTANCE:
+	// Mean dist_p1 all final 1 experiment  (vec_distance_performance_p1_v2_)
+		double final_mean_distance_p1=0.0;
+		for(unsigned int t=0; t<vec_distance_performance_p1_v2_.size();t++){
+			if(isnan((float)vec_distance_performance_p1_v2_.at(t))){
+			}else{
+				final_mean_distance_p1=final_mean_distance_p1+vec_distance_performance_p1_v2_.at(t);
+				//std::cout << "final_mean_distance_p1="<<final_mean_distance_p1<<"; actual_robot_performance_="<<vec_distance_performance_p1_v2_.at(t)<< std::endl;
+			}
+
+		}
+		std::cout << "%%% Distance performance: size="<<vec_distance_performance_p1_v2_.size()<< std::endl<< std::endl;
+
+		double final_mean_distance_performance_p1=final_mean_distance_p1/vec_distance_performance_p1_v2_.size();
+		std::cout << "final_mean_distance_performance_p1="<<final_mean_distance_performance_p1<< std::endl;
+
+		// Mean std dist_p1 all final 1 experiment  (vec_distance_performance_p1_v2_)
+			double final_std_mean_distance_p1=0.0;
+			for(unsigned int t=0; t<std_vec_distance_performance_p1_v2_.size();t++){
+				if(isnan((float)std_vec_distance_performance_p1_v2_.at(t))){
+				}else{
+					final_std_mean_distance_p1=final_std_mean_distance_p1+std_vec_distance_performance_p1_v2_.at(t);
+					//std::cout << "final_std_mean_distance_p1="<<final_std_mean_distance_p1<<"; actual_robot_performance_="<<std_vec_distance_performance_p1_v2_.at(t)<< std::endl;
+				}
+
+			}
+
+			double final_std_mean_distance_performance_p1=final_std_mean_distance_p1/std_vec_distance_performance_p1_v2_.size();
+			std::cout << "final_std_mean_distance_performance_p1="<<final_std_mean_distance_performance_p1<< std::endl;
+
+
+			//std_dist_v2_p1:
+			double final_std_mean_distance_p1_v2=0.0;
+			for(unsigned int t=0; t<vec_distance_performance_p1_v2_.size();t++){
+				if(isnan((float)vec_distance_performance_p1_v2_.at(t))){
+				}else{
+					final_std_mean_distance_p1_v2=final_std_mean_distance_p1_v2+sqrt((vec_distance_performance_p1_v2_.at(t)-final_mean_distance_performance_p1)*(vec_distance_performance_p1_v2_.at(t)-final_mean_distance_performance_p1));
+
+				}
+			}
+			double final_std_mean_distance_performance_p1_v2=final_std_mean_distance_p1_v2/vec_distance_performance_p1_v2_.size();
+			std::cout << "% final_std_mean_distance_performance_p1_v2="<<final_std_mean_distance_performance_p1_v2<< std::endl<< std::endl;
+
+
+		// Mean dist_p2 all final 1 experiment  (vec_distance_performance_p1_v2_)
+			double final_mean_distance_p2=0.0;
+			for(unsigned int t=0; t<vec_distance_performance_p2_v2_.size();t++){
+				if(isnan((float)vec_distance_performance_p2_v2_.at(t))){
+				}else{
+					final_mean_distance_p2=final_mean_distance_p2+vec_distance_performance_p2_v2_.at(t);
+					//std::cout << "final_mean_distance_p2="<<final_mean_distance_p2<<"; actual_robot_performance_="<<vec_distance_performance_p2_v2_.at(t)<< std::endl;
+				}
+
+			}
+
+			double final_mean_distance_performance_p2=final_mean_distance_p2/vec_distance_performance_p2_v2_.size();
+			std::cout << "final_mean_distance_performance_p2="<<final_mean_distance_performance_p2<< std::endl;
+
+			// Mean std dist_p1 all final 1 experiment  (vec_distance_performance_p1_v2_)
+				double final_std_mean_distance_p2=0.0;
+				for(unsigned int t=0; t<std_vec_distance_performance_p2_v2_.size();t++){
+					if(isnan((float)std_vec_distance_performance_p2_v2_.at(t))){
+					}else{
+						final_std_mean_distance_p2=final_std_mean_distance_p2+std_vec_distance_performance_p2_v2_.at(t);
+						//std::cout << "final_std_mean_distance_p2="<<final_std_mean_distance_p2<<"; actual_robot_performance_="<<std_vec_distance_performance_p2_v2_.at(t)<< std::endl;
+					}
+
+				}
+
+				double final_std_mean_distance_performance_p2=final_std_mean_distance_p2/std_vec_distance_performance_p2_v2_.size();
+				std::cout << "final_std_mean_distance_performance_p2="<<final_std_mean_distance_performance_p2<< std::endl;
+
+				//std_dist_v2_p2:
+				double final_std_mean_distance_p2_v2=0.0;
+				for(unsigned int t=0; t<vec_distance_performance_p2_v2_.size();t++){
+					if(isnan((float)vec_distance_performance_p2_v2_.at(t))){
+					}else{
+						final_std_mean_distance_p2_v2=final_std_mean_distance_p2_v2+sqrt((vec_distance_performance_p2_v2_.at(t)-final_mean_distance_performance_p2)*(vec_distance_performance_p2_v2_.at(t)-final_mean_distance_performance_p2));
+
+					}
+				}
+				double final_std_mean_distance_performance_p2_v2=final_std_mean_distance_p2_v2/vec_distance_performance_p2_v2_.size();
+				std::cout << "% final_std_mean_distance_performance_p2_v2="<<final_std_mean_distance_performance_p2_v2<< std::endl<< std::endl;
+
+
+
+			// Mean dist_tot all final 1 experiment  (vec_distance_performance_p1_v2_)
+				double final_mean_distance_tot=0.0;
+				for(unsigned int t=0; t<vec_distance_performance_total_v2_.size();t++){
+					if(isnan((float)vec_distance_performance_total_v2_.at(t))){
+					}else{
+						final_mean_distance_tot=final_mean_distance_tot+vec_distance_performance_total_v2_.at(t);
+						//std::cout << "final_mean_distance_tot="<<final_mean_distance_tot<<"; actual_robot_performance_="<<vec_distance_performance_total_v2_.at(t)<< std::endl;
+					}
+
+				}
+
+				double final_mean_distance_performance_tot=final_mean_distance_tot/vec_distance_performance_total_v2_.size();
+				std::cout << "final_mean_distance_performance_tot="<<final_mean_distance_performance_tot<< std::endl;
+
+				// Mean std dist_tot all final 1 experiment  (vec_distance_performance_p1_v2_)
+					double final_std_mean_distance_tot=0.0;
+					for(unsigned int t=0; t<std_vec_distance_performance_total_v2_.size();t++){
+						if(isnan((float)std_vec_distance_performance_total_v2_.at(t))){
+						}else{
+							final_std_mean_distance_tot=final_std_mean_distance_tot+std_vec_distance_performance_total_v2_.at(t);
+							//std::cout << "final_std_mean_distance_ptot="<<final_std_mean_distance_tot<<"; actual_robot_performance_="<<std_vec_distance_performance_total_v2_.at(t)<< std::endl;
+						}
+
+					}
+
+					double final_std_mean_distance_performance_tot=final_std_mean_distance_tot/std_vec_distance_performance_total_v2_.size();
+					std::cout << "final_std_mean_distance_performance_tot="<<final_std_mean_distance_performance_tot<< std::endl;
+
+					//std_dist_v2_tot:
+					double final_std_mean_distance_tot_v2=0.0;
+					for(unsigned int t=0; t<vec_distance_performance_total_v2_.size();t++){
+						if(isnan((float)vec_distance_performance_total_v2_.at(t))){
+						}else{
+							final_std_mean_distance_tot_v2=final_std_mean_distance_tot_v2+sqrt((vec_distance_performance_total_v2_.at(t)-final_mean_distance_performance_tot)*(vec_distance_performance_total_v2_.at(t)-final_mean_distance_performance_tot));
+
+						}
+					}
+					double final_std_mean_distance_performance_tot_v2=final_std_mean_distance_tot_v2/vec_distance_performance_total_v2_.size();
+					std::cout << "% final_std_mean_distance_performance_tot_v2="<<final_std_mean_distance_performance_tot_v2<< std::endl<< std::endl;
+
+// PERF ANGLE:
+
+					// Mean ang_p1 all final 1 experiment  (vec_angle_performance_p1_v2_)
+						double final_mean_angle_p1=0.0;
+						for(unsigned int t=0; t<vec_angle_performance_p1_v2_.size();t++){
+							if(isnan((float)vec_angle_performance_p1_v2_.at(t))){
+							}else{
+								final_mean_angle_p1=final_mean_angle_p1+vec_angle_performance_p1_v2_.at(t);
+								//std::cout << "final_mean_angle_p1="<<final_mean_angle_p1<<"; actual_robot_performance_="<<vec_angle_performance_p1_v2_.at(t)<< std::endl;
+							}
+
+						}
+
+						std::cout << "%%% Angle performance: size="<<vec_angle_performance_p1_v2_.size()<< std::endl<< std::endl;
+
+						double final_mean_angle_performance_p1=final_mean_angle_p1/vec_angle_performance_p1_v2_.size();
+						std::cout << "final_mean_angle_performance_p1="<<final_mean_angle_performance_p1<< std::endl;
+
+						// Mean std dist_tot all final 1 experiment  (vec_distance_performance_p1_v2_)
+							double final_std_mean_angle_p1=0.0;
+							for(unsigned int t=0; t<std_vec_angle_performance_p1_v2_.size();t++){
+								if(isnan((float)std_vec_angle_performance_p1_v2_.at(t))){
+								}else{
+									final_std_mean_angle_p1=final_std_mean_angle_p1+std_vec_angle_performance_p1_v2_.at(t);
+									//std::cout << "final_std_mean_angle_p1="<<final_std_mean_angle_p1<<"; actual_robot_performance_="<<std_vec_angle_performance_p1_v2_.at(t)<< std::endl;
+								}
+
+							}
+
+							double final_std_mean_angle_performance_p1=final_std_mean_angle_p1/std_vec_angle_performance_p1_v2_.size();
+							std::cout << "final_std_mean_angle_performance_p1="<<final_std_mean_angle_performance_p1<< std::endl;
+
+							//std_angle_v2_p1:
+							double final_std_mean_angle_p1_v2=0.0;
+							for(unsigned int t=0; t<vec_angle_performance_p1_v2_.size();t++){
+								if(isnan((float)vec_angle_performance_p1_v2_.at(t))){
+								}else{
+									final_std_mean_angle_p1_v2=final_std_mean_angle_p1_v2+sqrt((vec_angle_performance_p1_v2_.at(t)-final_mean_angle_performance_p1)*(vec_angle_performance_p1_v2_.at(t)-final_mean_angle_performance_p1));
+
+								}
+							}
+							double final_std_mean_angle_performance_p1_v2=final_std_mean_angle_p1_v2/vec_angle_performance_p1_v2_.size();
+							std::cout << "% final_std_mean_angle_performance_p1_v2="<<final_std_mean_angle_performance_p1_v2<< std::endl<< std::endl;
+
+							// Mean ang_p2 all final 1 experiment  (vec_angle_performance_p1_v2_)
+								double final_mean_angle_p2=0.0;
+								for(unsigned int t=0; t<vec_angle_performance_p2_v2_.size();t++){
+									if(isnan((float)vec_angle_performance_p2_v2_.at(t))){
+									}else{
+										final_mean_angle_p2=final_mean_angle_p2+vec_angle_performance_p2_v2_.at(t);
+										//std::cout << "final_mean_angle_p2="<<final_mean_angle_p2<<"; actual_robot_performance_="<<vec_angle_performance_p2_v2_.at(t)<< std::endl;
+									}
+
+								}
+
+								double final_mean_angle_performance_p2=final_mean_angle_p2/vec_angle_performance_p2_v2_.size();
+								std::cout << "final_mean_angle_performance_p2="<<final_mean_angle_performance_p2<< std::endl;
+
+								// Mean std dist_tot all final 1 experiment  (vec_distance_performance_p1_v2_)
+									double final_std_mean_angle_p2=0.0;
+									for(unsigned int t=0; t<std_vec_angle_performance_p2_v2_.size();t++){
+										if(isnan((float)std_vec_angle_performance_p2_v2_.at(t))){
+										}else{
+											final_std_mean_angle_p2=final_std_mean_angle_p2+std_vec_angle_performance_p2_v2_.at(t);
+											//std::cout << "final_std_mean_angle_p2="<<final_std_mean_angle_p2<<"; actual_robot_performance_="<<std_vec_angle_performance_p2_v2_.at(t)<< std::endl;
+										}
+
+									}
+
+									double final_std_mean_angle_performance_p2=final_std_mean_angle_p2/std_vec_angle_performance_p2_v2_.size();
+									std::cout << "final_std_mean_angle_performance_p2="<<final_std_mean_angle_performance_p2<< std::endl;
+
+									//std_angle_v2_p2:
+									double final_std_mean_angle_p2_v2=0.0;
+									for(unsigned int t=0; t<vec_angle_performance_p2_v2_.size();t++){
+										if(isnan((float)vec_angle_performance_p2_v2_.at(t))){
+										}else{
+											final_std_mean_angle_p2_v2=final_std_mean_angle_p2_v2+sqrt((vec_angle_performance_p2_v2_.at(t)-final_mean_angle_performance_p2)*(vec_angle_performance_p2_v2_.at(t)-final_mean_angle_performance_p2));
+
+										}
+									}
+									double final_std_mean_angle_performance_p2_v2=final_std_mean_angle_p2_v2/vec_angle_performance_p2_v2_.size();
+									std::cout << "% final_std_mean_angle_performance_p2_v2="<<final_std_mean_angle_performance_p2_v2<< std::endl<< std::endl;
+
+////////////
+
+			// Mean ang_tot all final 1 experiment  (vec_angle_performance_p1_v2_)
+			double final_mean_angle_tot=0.0;
+			for(unsigned int t=0; t<vec_angle_performance_total_v2_.size();t++){
+				if(isnan((float)vec_angle_performance_total_v2_.at(t))){
+				}else{
+					final_mean_angle_tot=final_mean_angle_tot+vec_angle_performance_total_v2_.at(t);
+					//std::cout << "final_mean_angle_tot="<<final_mean_angle_tot<<"; actual_robot_performance_="<<vec_angle_performance_total_v2_.at(t)<< std::endl;
+				}
+			}
+
+			double final_mean_angle_performance_tot=final_mean_angle_tot/vec_angle_performance_total_v2_.size();
+			std::cout << "final_mean_angle_performance_tot="<<final_mean_angle_performance_tot<< std::endl;
+
+			// Mean std dist_tot all final 1 experiment  (vec_distance_performance_p1_v2_)
+			double final_std_mean_angle_tot=0.0;
+			for(unsigned int t=0; t<std_vec_angle_performance_total_v2_.size();t++){
+				if(isnan((float)std_vec_angle_performance_total_v2_.at(t))){
+				}else{
+					final_std_mean_angle_tot=final_std_mean_angle_tot+std_vec_angle_performance_total_v2_.at(t);
+					//std::cout << "final_std_mean_angle_tot="<<final_std_mean_angle_tot<<"; actual_robot_performance_="<<std_vec_angle_performance_total_v2_.at(t)<< std::endl;
+				}
+
+			}
+			double final_std_mean_angle_performance_tot=final_std_mean_angle_tot/std_vec_angle_performance_total_v2_.size();
+			std::cout << "final_std_mean_angle_performance_tot="<<final_std_mean_angle_performance_tot<< std::endl;
+
+			//std_angle_v2_tot:
+			double final_std_mean_angle_tot_v2=0.0;
+			for(unsigned int t=0; t<vec_angle_performance_total_v2_.size();t++){
+				if(isnan((float)vec_angle_performance_total_v2_.at(t))){
+				}else{
+					final_std_mean_angle_tot_v2=final_std_mean_angle_tot_v2+sqrt((vec_angle_performance_total_v2_.at(t)-final_mean_angle_performance_tot)*(vec_angle_performance_total_v2_.at(t)-final_mean_angle_performance_tot));
+
+				}
+			}
+			double final_std_mean_angle_performance_tot_v2=final_std_mean_angle_tot_v2/vec_angle_performance_total_v2_.size();
+			std::cout << "% final_std_mean_angle_performance_tot_v2="<<final_std_mean_angle_performance_tot_v2<< std::endl<< std::endl;
+/*
+	std::vector<double> vec_area_performance_p1_v2_;
+	std::vector<double> vec_area_performance_p2_v2_;
+	std::vector<double> vec_total_area_performance_v2_;
+
+ * 		//std:: area performance.
+std::vector<double> std_vec_area_performance_p1_v2_;
+std::vector<double> std_vec_area_performance_p2_v2_;
+std::vector<double> std_vec_total_area_performance_v2_;
+ */
+
+			// Mean area_p1 all final 1 experiment  (vec_area_performance_p1_v2_)
+			double final_mean_area_p1=0.0;
+			for(unsigned int t=0; t<vec_area_performance_p1_v2_.size();t++){
+				if(isnan((float)vec_area_performance_p1_v2_.at(t))){
+				}else{
+					final_mean_area_p1=final_mean_area_p1+vec_area_performance_p1_v2_.at(t);
+					//std::cout << "final_mean_area_p1="<<final_mean_area_p1<<"; actual_robot_performance_="<<vec_area_performance_p1_v2_.at(t)<< std::endl;
+				}
+			}
+
+			std::cout << "%%% Area performance: size="<<vec_area_performance_p1_v2_.size()<< std::endl<< std::endl;
+
+			double final_mean_area_performance_p1=final_mean_area_p1/vec_area_performance_p1_v2_.size();
+			std::cout << "final_mean_area_performance_p1="<<final_mean_area_performance_p1<< std::endl;
+			//std::cout << "final_mean_area_performance_p1_real="<<final_mean_area_performance_p1*vec_area_performance_p1_v2_.size()<< std::endl;
+
+			// Mean std area_p1 all final 1 experiment  (std_vec_area_performance_p1_v2_)
+			double final_std_mean_area_p1=0.0;
+			for(unsigned int t=0; t<std_vec_area_performance_p1_v2_.size();t++){
+				if(isnan((float)std_vec_area_performance_p1_v2_.at(t))){
+				}else{
+					final_std_mean_area_p1=final_std_mean_area_p1+std_vec_area_performance_p1_v2_.at(t);
+					//std::cout << "final_std_mean_area_p1="<<final_std_mean_area_p1<<"; actual_robot_performance_="<<std_vec_area_performance_p1_v2_.at(t)<< std::endl;
+				}
+
+			}
+			double final_std_mean_area_performance_p1=final_std_mean_area_p1/std_vec_area_performance_p1_v2_.size();
+			std::cout << "final_std_mean_area_performance_p1="<<final_std_mean_area_performance_p1<< std::endl;
+
+			//std_area_v2_p1:
+			double final_std_mean_area_p1_v2=0.0;
+			for(unsigned int t=0; t<vec_area_performance_p1_v2_.size();t++){
+				if(isnan((float)vec_area_performance_p1_v2_.at(t))){
+				}else{
+					final_std_mean_area_p1_v2=final_std_mean_area_p1_v2+sqrt((vec_area_performance_p1_v2_.at(t)-final_mean_area_performance_p1)*(vec_area_performance_p1_v2_.at(t)-final_mean_area_performance_p1));
+
+				}
+			}
+			double final_std_mean_area_performance_p1_v2=final_std_mean_area_p1_v2/vec_area_performance_p1_v2_.size();
+			std::cout << "% final_std_mean_area_performance_p1_v2="<<final_std_mean_area_performance_p1_v2<< std::endl<< std::endl;
+
+			// Mean area_p2 all final 1 experiment  (vec_area_performance_p2_v2_)
+			double final_mean_area_p2=0.0;
+			for(unsigned int t=0; t<vec_area_performance_p2_v2_.size();t++){
+				if(isnan((float)vec_area_performance_p2_v2_.at(t))){
+				}else{
+					final_mean_area_p2=final_mean_area_p2+vec_area_performance_p2_v2_.at(t);
+					//std::cout << "final_mean_area_p2="<<final_mean_area_p2<<"; actual_robot_performance_="<<vec_area_performance_p2_v2_.at(t)<< std::endl;
+				}
+			}
+
+			double final_mean_area_performance_p2=final_mean_area_p2/vec_area_performance_p2_v2_.size();
+			std::cout << "final_mean_area_performance_p2="<<final_mean_area_performance_p2<< std::endl;
+			//std::cout << "final_mean_area_performance_p2_real="<<final_mean_area_performance_p2*vec_area_performance_p2_v2_.size()<< std::endl;
+
+			// Mean std area_p2 all final 1 experiment  (std_vec_area_performance_p2_v2_)
+			double final_std_mean_area_p2=0.0;
+			for(unsigned int t=0; t<std_vec_area_performance_p2_v2_.size();t++){
+				if(isnan((float)std_vec_area_performance_p2_v2_.at(t))){
+				}else{
+					final_std_mean_area_p2=final_std_mean_area_p2+std_vec_area_performance_p2_v2_.at(t);
+					//std::cout << "final_std_mean_area_p2="<<final_std_mean_area_p2<<"; actual_robot_performance_="<<std_vec_area_performance_p2_v2_.at(t)<< std::endl;
+				}
+
+			}
+			double final_std_mean_area_performance_p2=final_std_mean_area_p2/std_vec_area_performance_p2_v2_.size();
+			std::cout << "final_std_mean_area_performance_p2="<<final_std_mean_area_performance_p2<< std::endl;
+
+
+			//std_area_v2_p2:
+			double final_std_mean_area_p2_v2=0.0;
+			for(unsigned int t=0; t<vec_area_performance_p2_v2_.size();t++){
+				if(isnan((float)vec_area_performance_p2_v2_.at(t))){
+				}else{
+					final_std_mean_area_p2_v2=final_std_mean_area_p2_v2+sqrt((vec_area_performance_p2_v2_.at(t)-final_mean_area_performance_p2)*(vec_area_performance_p2_v2_.at(t)-final_mean_area_performance_p2));
+
+				}
+			}
+			double final_std_mean_area_performance_p2_v2=final_std_mean_area_p2_v2/vec_area_performance_p2_v2_.size();
+			std::cout << "% final_std_mean_area_performance_p2_v2="<<final_std_mean_area_performance_p2_v2<< std::endl<< std::endl;
+
+			// Mean area_tot all final 1 experiment  (vec_area_performance_p2_v2_)
+				double final_mean_area_tot=0.0;
+				for(unsigned int t=0; t<vec_total_area_performance_v2_.size();t++){
+					if(isnan((float)vec_total_area_performance_v2_.at(t))){
+					}else{
+						final_mean_area_tot=final_mean_area_tot+vec_total_area_performance_v2_.at(t);
+						//std::cout << "final_mean_area_tot="<<final_mean_area_tot<<"; actual_robot_performance_="<<vec_total_area_performance_v2_.at(t)<< std::endl;
+					}
+				}
+
+				double final_mean_area_performance_tot=final_mean_area_tot/vec_total_area_performance_v2_.size();
+				std::cout << "final_mean_area_performance_tot="<<final_mean_area_performance_tot<< std::endl;
+
+				// Mean std area_p2 all final 1 experiment  (std_vec_area_performance_p2_v2_)
+				double final_std_mean_area_tot=0.0;
+				for(unsigned int t=0; t<std_vec_total_area_performance_v2_.size();t++){
+					if(isnan((float)std_vec_total_area_performance_v2_.at(t))){
+					}else{
+						final_std_mean_area_tot=final_std_mean_area_tot+std_vec_total_area_performance_v2_.at(t);
+						//std::cout << "(In) final_std_mean_area_tot="<<final_std_mean_area_tot<<"; actual_robot_performance_="<<std_vec_total_area_performance_v2_.at(t)<< std::endl;
+					}
+
+				}
+				double final_std_mean_area_performance_tot=final_std_mean_area_tot/std_vec_total_area_performance_v2_.size();
+				std::cout << "final_std_mean_area_performance_tot="<<final_std_mean_area_performance_tot<< std::endl;
+
+
+				//std_area_v2_p2:
+				double final_std_mean_area_tot_v2=0.0;
+				for(unsigned int t=0; t<vec_total_area_performance_v2_.size();t++){
+					if(isnan((float)vec_total_area_performance_v2_.at(t))){
+					}else{
+						final_std_mean_area_tot_v2=final_std_mean_area_tot_v2+sqrt((vec_total_area_performance_v2_.at(t)-final_mean_area_performance_p2)*(vec_total_area_performance_v2_.at(t)-final_mean_area_performance_p2));
+
+					}
+				}
+				double final_std_mean_area_performance_tot_v2=final_std_mean_area_tot_v2/vec_total_area_performance_v2_.size();
+				std::cout << "% final_std_mean_area_performance_tot_v2="<<final_std_mean_area_performance_tot_v2<< std::endl<< std::endl;
+/////////////////////////////////////
+
+// TODO: ALL RECALCULATED with distances!!!
+
+//}
+
+
+	return 0;
+}
diff --git a/local_lib_people_prediction/src/examples/local_nav_example.cpp b/local_lib_people_prediction/src/examples/local_nav_example.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..90daca4a53f612a5474148e0b222d004e6d0252d
--- /dev/null
+++ b/local_lib_people_prediction/src/examples/local_nav_example.cpp
@@ -0,0 +1,89 @@
+/*
+ * local_nav_example.cpp
+ *
+ *  Created on: 10-Jan-2014
+ *      Author: gferrer
+ */
+
+#include "nav/plan_local_nav.h"
+#include "nav/plan_local_nav_person_companion.h"
+#include "nav/force_reactive_robot_companion.h"
+#include "scene_sim.h"
+#include <iostream>
+
+int main(int argc,char *argv[])
+{
+	Cscene_sim scene;
+	double dt(0.2);
+	scene.set_dt(dt);
+	scene.set_number_virtual_people( 2 );
+	if ( !scene.read_destination_map(
+		"/home/gferrer/iri-lab/ros/iri-ros-pkg/iri_navigation/iri_people_simulation/map/3_destinations.txt"  ) )
+	{
+		std::cout << "Could not read map destinations file !!!" << std::endl;
+	}
+	else{
+		std::cout << "read destinations map file : SUCCESS!!!" << std::endl;
+	}
+	scene.set_remove_targets(false);
+
+	// planner
+	Cplan_local_nav planner(5.0,200);
+	planner.set_global_mode( Cplan_local_nav::MO_norm );
+	planner.set_distance_mode( Cplan_local_nav::Cost2go_norm );
+	//Cprediction_behavior planner;
+	if ( !planner.read_destination_map(
+		"/Xhome/gferrer/iri-lab/ros/iri-ros-pkg/iri_navigation/iri_people_simulation/map/3_destinations.txt"  ) )
+	{
+		std::cout << "Could not read map destinations file !!!" << std::endl;
+	}
+	else{
+		std::cout << "read destinations map file : SUCCESS!!!" << std::endl;
+	}
+	planner.set_dt(dt);
+
+
+	double now = 10.0;
+
+	std::vector<SdetectionObservation> obs_scene;
+	std::vector<Spoint> laser_points;
+	laser_points.push_back( Spoint(0.9,0.9) );//poruqe? TODO
+	Spose command;
+	bool we_have_person_companion=false; // solo para que no de error al compilar el companion
+	while(now < 100.0)
+	{
+		std::cout << "entering loop sequence at time " << now << std::endl;
+		obs_scene.clear();
+		obs_scene.push_back( SdetectionObservation(0, now ));//void observation, just for the timestamp
+		if (now > 0.2)
+			scene.set_number_virtual_people( 1);
+		scene.update_scene( obs_scene, we_have_person_companion );
+		scene.update_robot( Spose(0.0 , 0.0 , now  ) );
+
+		//update planner
+		obs_scene.clear();
+		const std::list<Cperson_abstract *>* person_list = scene.get_scene(  );
+		for( Cperson_abstract* iit : *person_list)
+		{
+			iit->get_current_pointV().print();
+			obs_scene.push_back( SdetectionObservation( iit->get_id(), now,
+					iit->get_current_pointV().x , iit->get_current_pointV().y) );
+		}
+
+		planner.update_scene( obs_scene ,we_have_person_companion);
+		planner.update_robot( Spose(0.0 , 0.0 , now  ) );
+
+		planner.read_laser_scan( laser_points );
+
+		//calculation of
+		planner.set_robot_goal( Spoint(5,5) );
+		Cperson_abstract::companion_reactive reactive_;
+		reactive_=Cperson_abstract::Akp_planning;
+		planner.robot_plan_companion2(command,reactive_);
+
+		now +=dt;
+	}
+
+	return 0;
+}
+
diff --git a/local_lib_people_prediction/src/examples/prediction_example.cpp b/local_lib_people_prediction/src/examples/prediction_example.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ddf5bf4d5ffeeac9b12975fea6c027b24064c364
--- /dev/null
+++ b/local_lib_people_prediction/src/examples/prediction_example.cpp
@@ -0,0 +1,73 @@
+/*
+ * prediction_example.cpp
+ *
+ *  Created on: 15-Apr-2014
+ *      Author: gferrer
+ */
+
+#include "prediction_behavior.h"
+#include "nav/plan_local_nav_person_companion.h"
+#include "scene_sim.h"
+#include <iostream>
+
+int main(int argc,char *argv[])
+{
+	Cscene_sim scene;
+	scene.set_dt(0.1);
+	scene.set_number_virtual_people( 2 );
+	if ( !scene.read_destination_map(
+		"/home/gferrer/iri-lab/ros/iri-ros-pkg/iri_navigation/iri_people_simulation/map/3_destinations.txt"  ) )
+	{
+		std::cout << "Could not read map destinations file !!!" << std::endl;
+	}
+	else{
+		std::cout << "read destinations map file : SUCCESS!!!" << std::endl;
+	}
+	scene.set_remove_targets(false);
+
+	// prediction ( double horizon_time = 0.5, bool behavior_flag = false,filter,autoremove )
+	Cprediction_behavior planner(1.0, true, Cperson_abstract::Low_pass_linear_regression_filtering);
+	if ( !planner.read_destination_map(
+		"/home/gferrer/iri-lab/ros/iri-ros-pkg/iri_navigation/iri_people_simulation/map/3_destinations.txt"  ) )
+	{
+		std::cout << "Could not read map destinations file !!!" << std::endl;
+	}
+	else{
+		std::cout << "read destinations map file : SUCCESS!!!" << std::endl;
+	}
+	planner.set_dt(0.1);
+
+
+	double now = 0.0;
+	bool we_have_person_companion=0; // solo para no error en person companion.
+
+	std::vector<SdetectionObservation> obs_scene;
+	while(now < 100.0)
+	{
+		std::cout << "entering loop sequence at time " << now << std::endl;
+		obs_scene.clear();
+		obs_scene.push_back( SdetectionObservation(0, now ));//void observation, just for the timestamp
+		scene.update_scene( obs_scene , we_have_person_companion);
+
+		//update planner
+		obs_scene.clear();
+		const std::list<Cperson_abstract *>* person_list = scene.get_scene(  );
+		for( Cperson_abstract* iit : *person_list)
+		{
+			obs_scene.push_back( SdetectionObservation( iit->get_id(), now,
+					iit->get_current_pointV().x , iit->get_current_pointV().y) );
+		}
+		planner.update_scene( obs_scene , we_have_person_companion);
+		planner.update_robot( Spose(0.0 , 0.0 , now  ) );
+
+		//calculation of prediction
+		planner.scene_prediction();
+
+		// testing of predictions
+
+		now +=0.1;
+	}
+
+	return 0;
+}
+
diff --git a/local_lib_people_prediction/src/iri_geometry.cpp b/local_lib_people_prediction/src/iri_geometry.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d96cb44fcaf2d46822879dbf6ccacf5be5a27f3a
--- /dev/null
+++ b/local_lib_people_prediction/src/iri_geometry.cpp
@@ -0,0 +1,1082 @@
+/*
+ * iri_geometry.cpp
+ *
+ *  Created on: 2013.  Last Modified on 2020
+ *      Author: Initial code of the AKP Gonzalo Ferrer.
+ *      Author: Addition of ASSAOP and ASSAGP for people accompaniment code of Ely Repiso.
+ */
+
+#include "iri_geometry.h"
+#include <math.h>
+#include <iostream>
+#include <Eigen/Dense>
+#include<string>
+#include <dirent.h>
+#include <stdio.h>
+
+// Spoint functions ------------------------------------------
+Spoint::Spoint() : x(0) , y(0),  time_stamp(0) { }
+
+Spoint::Spoint( double x_ , double y_ , double time_stamp_) :
+		x(x_), y(y_), time_stamp(time_stamp_) { }
+Spoint Spoint::operator+ (Spoint p2) const
+{
+	return Spoint(  x + p2.x , y + p2.y , time_stamp );
+}
+Spoint Spoint::operator- (Spoint p2) const
+{
+	return Spoint(  x - p2.x , y - p2.y , time_stamp);
+}
+Spoint Spoint::operator* (double k) const
+{
+	return Spoint(  x *k , y *k , time_stamp);
+}
+double Spoint::distance(Spoint p2) const
+{
+	return sqrt( distance2(p2)  );
+}
+double Spoint::distance2(Spoint p2) const
+{
+	//std::cout << "(x-p2.x) ="<< x-p2.x << "y-p2.y"<<y-p2.y<< std::endl;
+	return (x-p2.x)*(x-p2.x) + (y-p2.y)*(y-p2.y);
+}
+Spoint Spoint::propagate( double dt , Sforce f, double desired_velocity) const
+{
+	//std::cout << "Spoint::propagate; dt="<<dt<< std::endl;
+	return *this;
+}
+void Spoint::print() const
+{
+	std::cout << "(x , y ,t) = (" << x << " , " << y  << " | " << time_stamp << " )" << std::endl;
+}
+
+//Spoint_cov methods -----------------------------------------------------------
+Spoint_cov::Spoint_cov():
+		Spoint()
+{
+	cov.reserve(4);
+	cov.resize(4,0.0);
+	cov[0] = 0.2; //TODO: for the side_by_side 2 people. We need less covariance of prediction. before was 1
+	cov[3] = 0.2;
+}
+Spoint_cov::Spoint_cov( double x_ , double y_ , double time_stamp_,
+		const std::vector<double>& cov_ ):
+		Spoint( x_,y_,time_stamp_ )
+{
+	if ( cov_.empty() || cov_.size() < (std::size_t) 4 )
+	{
+		cov.reserve(4);
+		cov.resize(4,0.0);
+		cov[0] = 0.2; //TODO: for the side_by_side 2 people. We need less covariance of prediction. before was 1.0
+		cov[3] = 0.2;
+	}
+	else
+	{
+		cov = cov_;
+	}
+}
+double Spoint_cov::cov_dist( Spoint p2 ) const
+{
+	//analitical inversion of the 2x2 matrix
+	//       det =   cov_xx * cov_yy - cov_xy * cov_xy
+	double cov_det = cov[0] * cov[3] - cov[1] * cov[2];
+	double dx = x - p2.x;
+	double dy = y - p2.y;
+	//std::cout<<" (Prediction_distancia_Interna) cov_det= "<<cov_det<< std::endl;
+	//std::cout<<" (Prediction_distancia_Interna) dx= "<<dx<< std::endl;
+	//std::cout<<" (Prediction_distancia_Interna) dy= "<<dy<< std::endl;
+	//std::cout<<" (Prediction_distancia_Interna) cov[0]= "<<cov[0]<<"; cov[1]="<<cov[1]<<"; cov[2]="<<cov[2]<<"; cov[3]="<<cov[3]<<"; cov_det="<<cov_det<< std::endl;
+	//std::cout<<" (Prediction_distancia_Interna)=  "<<( dx*dx*cov[3] - 2*dx*dy*cov[1] + dy*dy*cov[0] )/cov_det<< std::endl;
+	return ( dx*dx*cov[3] - 2*dx*dy*cov[1] + dy*dy*cov[0] )/cov_det;
+}
+Spoint_cov Spoint_cov::operator+ (Spoint_cov p2) const
+{
+    return Spoint_cov(  x + p2.x , y + p2.y , time_stamp , cov);
+}
+Spoint_cov Spoint_cov::operator- (Spoint_cov p2) const
+{
+    return Spoint_cov(  x - p2.x , y - p2.y , time_stamp , cov);
+}
+Spoint_cov Spoint_cov::operator* (double k) const
+{
+    return Spoint_cov(  x*k , y*k , time_stamp , cov);
+}
+double Spoint_cov::cov_xx() const
+{
+	return cov[0];
+}
+double Spoint_cov::cov_yy() const
+{
+	return cov[3];
+}
+double Spoint_cov::cov_xy() const
+{
+	return cov[1];
+}
+void Spoint_cov::print() const
+{
+    Spoint::print();
+    std::cout << "cov = [" << cov[0] << " , " << cov[1] << std::endl;
+    std::cout << "       " << cov[2] << " , " << cov[3] << " ]" << std::endl;
+}
+
+//SpointV methods -----------------------------------------------------------
+SpointV::SpointV() : Spoint(), vx(0), vy(0) { }
+
+SpointV::SpointV( double x_ , double y_, double time_stamp_, double vx_, double vy_ ) :
+		Spoint(x_,y_,time_stamp_), vx(vx_),vy(vy_) { }
+SpointV SpointV::operator+ (SpointV p2) const
+{
+	return SpointV(  x + p2.x , y + p2.y , time_stamp,  vx + p2.vx , vy + p2.vy );
+}
+SpointV SpointV::operator- (SpointV p2) const
+{
+	return SpointV(  x - p2.x , y - p2.y , time_stamp-p2.time_stamp,  vx - p2.vx , vy - p2.vy );
+}
+SpointV SpointV::operator* ( double k) const
+{
+	return SpointV(  x * k , y * k, time_stamp,  vx * k , vy * k );
+}
+void SpointV::print() const
+{
+	std::cout << "(x , y ,t) = (" << x << " , " << y  << " | " << time_stamp << " )" <<
+			"     (vx,vy) = ( " << vx << " , " << vy << " )" << std::endl;
+}
+SpointV SpointV::propagate( double dt , Sforce f, double desired_velocity) const
+{
+	//std::cout << "SpointV::propagate; dt="<<dt<< std::endl;
+	//linear propagation (uniformly accelerated system)
+	double vxn = vx + f.fx*dt;
+	double vyn = vy + f.fy*dt;
+	double dx = vx*dt + f.fx*dt*dt*0.5;
+	double dy = vy*dt + f.fy*dt*dt*0.5;
+
+	//std::cout << "f.fx=" << f.fx << "f.fy=" << f.fy  << "; dx=" <<dx << "; dy=" <<dy<<"; vx=" <<vx<<"; vy="<<vy<<";  f.fx*dt="<< f.fx*dt<<"; f.fy*dt="<<f.fy*dt<< std::endl;
+
+	//apply constrains:
+	double v = sqrt( vxn*vxn + vyn*vyn );
+	if ( v > desired_velocity)
+	{
+		vxn *= desired_velocity /v;
+		vyn *= desired_velocity /v;
+		dx = vxn*dt;
+		dy = vyn*dt;
+		//std::cout << "( v > desired_velocity) "<< std::endl;
+
+	}
+	//std::cout << "( v > desired_velocity) vxn="<<vxn<<"; vyn"<<vyn<<"; x="<<x<<"; y="<<y<<"; x+dx="<<x+dx<<"; y+dy="<<y+dy<<"; time_stamp+dt="<<time_stamp+dt<<"; desired_velocity="<<desired_velocity<< std::endl;
+
+	return SpointV( x+dx,y+dy, time_stamp+dt, vxn, vyn);
+}
+
+SpointV SpointV::propagate2_comp_only_vel(double dt , Sforce f, double desired_velocity, double distance_person_t_x,double distance_person_t_y,double vel_person_x,double vel_person_y) const
+{
+	//std::cout << "propagate2_comp_only_vel; dt="<<dt<< std::endl;
+	/////////////////////////
+	//linear propagation (uniformly accelerated system)
+	double vxn = vx + f.fx*dt;
+	double vyn = vy + f.fy*dt;
+	//double dx = vx*dt + f.fx*dt*dt*0.5;
+	//double dy = vy*dt + f.fy*dt*dt*0.5;
+
+	//std::cout << "f.fx=" << f.fx << "f.fy=" << f.fy  << "; vxn=" <<vxn << "; vyn=" <<vyn<<"; vx=" <<vx<<"; vy="<<vy<<";  f.fx*dt="<< f.fx*dt<<"; f.fy*dt="<<f.fy*dt<< std::endl;
+
+	//apply constrains:
+	double v = sqrt( vxn*vxn + vyn*vyn );
+	if ( v > desired_velocity)
+	{
+		vxn *= desired_velocity /v;
+		vyn *= desired_velocity /v;
+		//dx = vxn*dt;
+		//dy = vyn*dt;
+		//std::cout << "( v > desired_velocity) "<< std::endl;
+
+	}
+	//std::cout << "( v > desired_velocity) vxn="<<vxn<<"; vyn"<<vyn<<"; x="<<x+dx<<"; y="<<y+dy<<"; time_stamp+dt="<<time_stamp+dt<< std::endl;
+
+	//SpointV propagated_point=SpointV( x+dx,y+dy, time_stamp+dt, vxn, vyn);
+
+
+	////////////////
+
+	//double distance_person_t_x;
+	//double distance_person_t_y;
+
+	double distance_group_x;
+	double distance_group_y;
+
+	//double vel_group_x=vx;
+	//double vel_group_y=vy;
+
+	//double vel_person_x;
+	//double vel_person_y;
+
+	//std::cout << "propagated_point.x="<<propagated_point.x<<"; propagated_point.y="<<propagated_point.y<< std::endl;
+	//std::cout << "x+distance_group_x="<<x+distance_group_x<<"; y+distance_group_y="<<y+distance_group_y<< std::endl;
+
+	//std::cout << "x="<<x<<"; y="<<y<< std::endl;
+
+	// caso 1
+	//distance_group_x=sqrt((distance_person_t_x*(vel_group_x/vel_person_x))*(distance_person_t_x*(vel_group_x/vel_person_x)));
+	//distance_group_y=sqrt((distance_person_t_y*(vel_group_y/vel_person_y))*(distance_person_t_y*(vel_group_y/vel_person_y)));
+
+	distance_group_x=sqrt((distance_person_t_x*(vxn/vel_person_x))*(distance_person_t_x*(vxn/vel_person_x)));
+	distance_group_y=sqrt((distance_person_t_y*(vyn/vel_person_y))*(distance_person_t_y*(vyn/vel_person_y)));
+
+	//double theta_act_robot_=atan(vy/vx);
+
+	//std::cout << "theta_act_robot_="<<theta_act_robot_<<"; cos(theta_act_robot_)="<<cos(theta_act_robot_)<<"; sin(theta_act_robot_)="<<sin(theta_act_robot_)<< std::endl;
+
+
+	//double group_x=x+distance_group_x*cos(theta_act_robot_);
+	//double group_y=y+distance_group_y*sin(theta_act_robot_);
+
+//	double diference_x=sqrt((propagated_point.x-group_x)*(propagated_point.x-group_x)+(propagated_point.y-group_y)*(propagated_point.y-group_y));
+
+	//std::cout << "diference_x="<<diference_x<< std::endl;
+	//std::cout << "distance_person_t_x="<<distance_person_t_x<<"distance_person_t_y="<<distance_person_t_y<< std::endl;
+
+	//std::cout << "f.fx=" << f.fx << "f.fy=" << f.fy  << std::endl;
+	//std::cout << "distance_group_x="<<distance_group_x<<"distance_group_y="<<distance_group_x<< std::endl;
+	//std::cout << "vel_group_x="<<vel_group_x<<"vel_group_y="<<vel_group_y<< "vel_person_x="<<vel_person_x<<"vel_person_y="<<vel_person_y<< std::endl;
+
+
+
+	return SpointV( x+distance_group_x,y+distance_group_y,  time_stamp+dt, vxn, vyn);
+}
+
+double SpointV::orientation() const
+{
+	return atan2( vy, vx );
+}
+
+double SpointV::v() const
+{
+	return sqrt( vy*vy + vx*vx );
+}
+
+void SpointV::norm_v(double v)
+{
+	double theta = this->orientation();
+	vx = cos(theta)*v;
+	vy = sin(theta)*v;
+}
+
+double SpointV::angle_heading_point( Spoint p2 ) const
+{
+	double dx = p2.x - x;
+	double dy = p2.y - y;
+	return diffangle( atan2( dy , dx ), orientation() );
+}
+
+//SpointV_cov methods -----------------------------------------------------------
+SpointV_cov::SpointV_cov() :
+		SpointV()
+{
+	cov.reserve(16);
+	cov.resize(16,0.0);
+	cov[0] = 0.2;//0.4; //TODO: for the side_by_side 2 people. We need less covariance of prediction. before was 0.5
+	cov[5] = 0.2;
+	cov[10] = 0.1; //0.1
+	cov[15] = 0.1;
+}
+SpointV_cov::SpointV_cov(double x_ , double y_, double time_stamp_, double vx_, double vy_,
+		const std::vector<double>& cov_) :
+		SpointV(x_,y_,time_stamp_,vx_,vy_)
+{
+	if ( cov_.empty() || cov_.size() < (std::size_t) 16 )
+	{
+		cov.reserve(16);
+		cov.resize(16,0.0);
+		cov[0] = 0.2; //TODO: for the side_by_side 2 people. We need less covariance of prediction. before was 0.5
+		cov[5] = 0.2;
+		cov[10] = 0.1;
+		cov[15] = 0.1;
+	}
+	else
+	{
+		cov = cov_;
+	}
+}
+SpointV_cov::SpointV_cov(SpointV p, const std::vector<double>& cov_) :
+		SpointV( p.x , p.y, p.time_stamp, p.vx, p.vy)
+{
+	if ( cov_.empty() || cov_.size() < (std::size_t) 16 )
+	{
+		cov.reserve(16);
+		cov.resize(16,0.0);
+		cov[0] = 0.2; //TODO: for the side_by_side 2 people. We need less covariance of prediction. before was 0.5
+		cov[5] = 0.2;
+		cov[10] = 0.1;
+		cov[15] = 0.1;
+	}
+	else
+	{
+		cov = cov_;
+	}
+}
+SpointV_cov::SpointV_cov(Spoint_cov p):
+		SpointV( p.x , p.y, p.time_stamp, 0.0, 0.0)
+{
+	cov.reserve(16);
+	cov.resize(16,0.0);
+	cov[0] = p.cov[0];
+	cov[1] = p.cov[1];
+	cov[4] = p.cov[2];
+	cov[5] = p.cov[3];
+	cov[10] = 0.1;
+	cov[15] = 0.1;
+}
+
+/*void SpointV_cov::SpointV_cov_con_Spose(Spose p )
+{
+	SpointV( p.x , p.y, p.time_stamp, p.v*cos(p.theta), p.v*sin(p.theta));
+	//SpointV_cov( p.x , p.y, p.time_stamp, p.vx, p.vy,p.cov)
+	cov.reserve(16);
+	cov.resize(16,0.0);
+	cov[0] = 0.5;
+	cov[5] = 0.5;
+	cov[10] = 0.1;
+	cov[15] = 0.1;
+}*/
+
+
+double SpointV_cov::cov_dist( Spoint p2 ) const
+{
+	// TODO: consider velocities, right now is a covariance distance of positions
+	//       det =   cov_xx * cov_yy - cov_xy * cov_xy
+	double cov_det = cov[0] * cov[5] - cov[1] * cov[4];
+	double dx = x - p2.x;
+	double dy = y - p2.y;
+	//
+	return ( dx*dx*cov[5] - 2*dx*dy*cov[1] + dy*dy*cov[0] )/cov_det;
+}
+double SpointV_cov::cov_dist( Spoint p2 , double &det) const
+{
+	det = cov[0] * cov[5] - cov[1] * cov[4];
+	double dx = x - p2.x;
+	double dy = y - p2.y;
+	return ( dx*dx*cov[5] - 2*dx*dy*cov[1] + dy*dy*cov[0] )/det;
+}
+
+double SpointV_cov::cov_distV( SpointV_cov p2, double &distV, double &distxv ) const
+{
+	// TODO: consider velocities, right now is a covariance distance of positions
+	//       det =   cov_xx * cov_yy - cov_xy * cov_xy
+	// SpointV_cov external for actual track, SpointV_cov internal for the initial Spoint_cov for this track in the first time that the cross is made.
+	double cov_det = cov[0] * cov[4] - cov[1] * cov[5];
+	double dx = x - p2.x;
+	double dy = y - p2.y;
+	double distx=( dx*dx*cov[5] - 2*dx*dy*cov[1] + dy*dy*cov[0] )/cov_det;
+
+	double cov_detV = cov[10] * cov[14] - cov[11] * cov[15];
+	double dvx = vx - p2.vx;
+	double dvy = vy - p2.vy;
+	distV=( dvx*dvx*cov[15] - 2*dvx*dvy*cov[11] + dvy*dvy*cov[10] )/cov_detV;
+
+	distxv=distx+distV;
+
+	return distx;
+
+}
+
+SpointV_cov SpointV_cov::operator+ (SpointV_cov p2) const
+{
+    return SpointV_cov(  x + p2.x , y + p2.y , time_stamp , vx + p2.vx , vy + p2.vy ,cov);
+}
+SpointV_cov SpointV_cov::operator- (SpointV_cov p2) const
+{
+    return SpointV_cov(  x - p2.x , y - p2.y , time_stamp-p2.time_stamp,  vx - p2.vx , vy - p2.vy ,cov);
+}
+SpointV_cov SpointV_cov::operator* (double k) const
+{
+    return SpointV_cov(  x * k , y * k, time_stamp,  vx * k , vy * k , cov);
+}
+SpointV_cov SpointV_cov::propagate( double dt , Sforce f, double desired_velocity) const
+{
+	//std::cout << "SpointV_cov::propagate; dt="<<dt<< std::endl;
+
+	SpointV new_point = SpointV::propagate(dt,f,desired_velocity);
+
+	//std::cout << "; dt"<<dt<<"; f.fx="<<f.fx<<"; f.fy="<<f.fy<<"; desired_velocity="<<desired_velocity << std::endl;
+
+	//dt=0.2;// provisional
+
+	Eigen::MatrixXd cov_prev(4 , 4 );
+	for (unsigned int i = 0; i < 4; ++i)
+	{
+		cov_prev.row(i) << cov[i*4],cov[i*4+1],cov[i*4+2],cov[i*4+3];
+	}
+	//std::cout << " (cov inicial) cov_ prev = \n" << std::endl;
+	//std::cout << "[ cov_prev(0,0)=" << cov_prev(0,0) <<"; cov_prev(0,1)="<<cov_prev(0,1)<<"; cov_prev(0,2)="<<cov_prev(0,2)<<"; cov_prev(0,3)="<<cov_prev(0,3) <<"]" << std::endl;
+	//std::cout << "[ cov_prev(1,0)=" << cov_prev(1,0) <<"; cov_prev(1,1)="<<cov_prev(1,1)<<"; cov_prev(1,2)="<<cov_prev(1,2)<<"; cov_prev(1,3)="<<cov_prev(1,3) <<"]" << std::endl;
+	//std::cout << "[ cov_prev(2,0)=" << cov_prev(2,0) <<"; cov_prev(2,1)="<<cov_prev(2,1)<<"; cov_prev(2,2)="<<cov_prev(2,2)<<"; cov_prev(2,3)="<<cov_prev(2,3)<<"]" << std::endl;
+	//std::cout << "[ cov_prev(3,0)=" << cov_prev(3,0) <<"; cov_prev(3,1)="<<cov_prev(3,1)<<"; cov_prev(3,2)="<<cov_prev(3,2)<<"; cov_prev(3,3)="<<cov_prev(3,3) <<"]" << std::endl;
+
+	Eigen::MatrixXd Phi(4 , 4 );
+	Phi = Eigen::MatrixXd::Identity(4,4);
+	Phi(0,2) = dt;
+	Phi(1,3) = dt;
+	//std::cout << "[ Phi(0,0)=" << Phi(0,0) <<"; Phi(0,1)="<<Phi(0,1)<<"; Phi(0,2)="<<Phi(0,2)<<"; Phi(0,3)="<<Phi(0,3) <<"]" << std::endl;
+	//std::cout << "[ Phi(1,0)=" << Phi(1,0) <<"; Phi(1,1)="<<Phi(1,1)<<"; Phi(1,2)="<<Phi(1,2)<<"; Phi(1,3)="<<Phi(1,3) <<"]" << std::endl;
+	//std::cout << "[ Phi(2,0)=" << Phi(2,0) <<"; Phi(2,1)="<<Phi(2,1)<<"; Phi(2,2)="<<Phi(2,2)<<"; Phi(2,3)="<<Phi(1,3)<<"]" << std::endl;
+	//std::cout << "[ Phi(3,0)=" << Phi(3,0) <<"; Phi(3,1)="<<Phi(3,1)<<"; Phi(3,2)="<<Phi(3,2)<<"; Phi(3,3)="<<Phi(3,3) <<"]" << std::endl;
+	Eigen::MatrixXd G(4 , 2 );
+	G = Eigen::MatrixXd::Zero(4,2);
+	G(0,0) = dt*dt/2;
+	G(1,1) = dt*dt/2;
+	G(2,0) = dt;
+	G(3,1) = dt;
+	//std::cout << "[ G(0,0)=" << G(0,0) <<"; G(0,1)="<<G(0,1)<<"]" << std::endl;
+	//std::cout << "[ G(1,0)=" << G(1,0) <<"; G(1,1)="<<G(1,1)<<"]" << std::endl;
+	//std::cout << "[ G(2,0)=" << G(2,0) <<"; G(2,1)="<<G(2,1)<<"]" << std::endl;
+	//std::cout << "[ G(3,0)=" << G(3,0) <<"; G(3,1)="<<G(3,1)<<"]" << std::endl;
+	Eigen::MatrixXd new_cov(4,4);
+	Eigen::MatrixXd cov_f(2,2);
+	cov_f = Eigen::MatrixXd::Zero(2,2);
+	cov_f(0,0) = 0.1;//TODO this covariance should be associated to the corresponding force and not generic. before was 0.3
+	cov_f(1,1) = 0.1; //TODO: for the side_by_side 2 people. We need less covariance of prediction.
+
+	//std::cout << "[ cov_f(0,0)=" << cov_f(0,0) <<"; cov_f(0,1)="<<cov_f(0,1)<<"]" << std::endl;
+	//std::cout << "[ cov_f(1,0)=" << cov_f(1,0) <<"; cov_f(1,1)="<<cov_f(1,1)<<"]" << std::endl;
+
+	new_cov = Phi*cov_prev*Phi.transpose() + G*cov_f*G.transpose();
+	std::vector<double> new_cov_std(16,0.0);
+	for(unsigned int i = 0; i<16; ++i)
+	{
+		new_cov_std[i] = new_cov( i/4 , i%4 );
+	}
+
+	//std::cout << "[ new_cov(0,0)=" << new_cov(0,0) <<"; new_cov(0,1)="<<new_cov(0,1)<<"; new_cov(0,2)="<<new_cov(0,2)<<"; new_cov(0,3)="<<new_cov(0,3) <<"]" << std::endl;
+	//std::cout << "[ new_cov(1,0)=" << new_cov(1,0) <<"; new_cov(1,1)="<<new_cov(1,1)<<"; new_cov(1,2)="<<new_cov(1,2)<<"; new_cov(1,3)="<<new_cov(1,3) <<"]" << std::endl;
+	//std::cout << "[ new_cov(2,0)=" << new_cov(2,0) <<"; new_cov(2,1)="<<new_cov(2,1)<<"; new_cov(2,2)="<<new_cov(2,2)<<"; new_cov(2,3)="<<new_cov(1,3)<<"]" << std::endl;
+	//std::cout << "[ new_cov(3,0)=" << new_cov(3,0) <<"; new_cov(3,1)="<<new_cov(3,1)<<"; new_cov(3,2)="<<new_cov(3,2)<<"; new_cov(3,3)="<<new_cov(3,3) <<"]" << std::endl;
+	//std::cout << "(SpointV) new_point =:" << std::endl;
+	//new_point.print();
+
+	return SpointV_cov( new_point, new_cov_std );
+}
+double SpointV_cov::cov_xx() const
+{
+	return cov[0];
+}
+double SpointV_cov::cov_yy() const
+{
+	return cov[5];
+}
+double SpointV_cov::cov_xy() const
+{
+	return cov[1];
+}
+Spoint_cov SpointV_cov::toSpoint_cov() const
+{
+	std::vector<double> cov_point;
+	cov_point.reserve(4);
+	cov_point.resize(4,0.0);
+	cov_point[0] = cov[0];
+	cov_point[1] = cov[1];
+	cov_point[2] = cov[4];
+	cov_point[3] = cov[5];
+	return Spoint_cov( x,y,time_stamp, cov_point );
+}
+void SpointV_cov::print() const
+{
+    SpointV::print();
+    std::cout << "covariance = [ " << cov[0] << " , " << cov[1] << " , " <<cov[2]  << " , " << cov[3] << std::endl <<
+    		     "               " << cov[4] << " , " << cov[5] << " , " <<cov[6]  << " , " << cov[7] << std::endl <<
+    		     "               " << cov[8] << " , " << cov[9] << " , " <<cov[10]  << " , " << cov[11] << std::endl <<
+    		     "               " << cov[12] << " , " << cov[13] << " , " <<cov[14]  << " , " << cov[15] << std::endl;
+}
+
+//Spose methods -----------------------------------------------------------
+Spose::Spose() : Spoint(), theta(0)  , v(0) , w(0)  { }
+Spose::Spose( double x_ , double y_ , double time_stamp_ , double theta_ , double v_, double w_) :
+	Spoint(x_,y_, time_stamp_), theta(theta_), v(v_), w(w_) { }
+Spose Spose::operator+ (Spose p2) const
+{
+	return Spose(  x + p2.x , y + p2.y , theta + p2.theta, p2.time_stamp , p2.v , p2.w );
+}
+Spose Spose::operator- (Spose p2) const
+{
+	return Spose(  x - p2.x , y - p2.y , theta - p2.theta, p2.time_stamp , p2.v , p2.w );
+}
+Spose Spose::operator* ( double k) const
+{
+	return Spose(  x * k , y *k , theta, time_stamp , v , w );
+}
+double Spose::distance(Spoint p2) const
+{
+	return sqrt(  (x-p2.x)*(x-p2.x) + (y-p2.y)*(y-p2.y) );
+}
+double Spose::angle_heading_pose( Spose p2 ) const
+{
+	double dx = p2.x - x;
+	double dy = p2.y - y;
+	return diffangle( atan2( dy , dx ), theta );
+}
+double Spose::social_distance( Spose p2 ) const
+{
+	double lambda = 0.0;
+	double phi = this->angle_heading_pose( p2 );
+	double anisotropy = (lambda + (1-lambda)*(1 + cos(phi))/2 );
+	return this->distance(p2)/anisotropy;
+}
+void Spose::print() const
+{
+	std::cout << "(x , y | theta) = (" << x << " , " << y  << " | " << theta <<
+			" )  --  at t = " << time_stamp << std::endl <<
+			"(v , w) = (" << v  << " , " << w << " )" << std::endl;
+}
+
+//Spose_cov methods -----------------------------------------------------------
+Spose_cov::Spose_cov():	 Spose()
+{
+	cov.reserve(25);
+	cov.resize(25,0.0);
+	cov[0] = 0.2;//xx //TODO: for the side_by_side 2 people. We need less covariance of prediction.
+	cov[6] = 0.2;//yy
+	cov[12] = 0.05;//theta_theta
+	cov[18] = 0.1;//cov_vv
+	cov[24] = 0.1;//ww
+}
+Spose_cov::Spose_cov(double x_ , double y_ ,
+		double time_stamp_ , double theta_ ,  double v_, double w_,
+		const std::vector<double>& cov_ ):
+		Spose( x_,y_,time_stamp_, theta_,v_,w_ )
+{
+	if ( cov_.empty() || cov_.size() < (std::size_t) 25 )
+	{
+		cov.reserve(25);
+		cov.resize(25,0.0);
+		cov[0] = 0.2;//xx //TODO: for the side_by_side 2 people. We need less covariance of prediction. before was 0.5
+		cov[6] = 0.2;//yy
+		cov[12] = 0.05;//theta_theta
+		cov[18] = 0.1;//cov_vv
+		cov[24] = 0.1;//ww
+	}
+	else
+	{
+		cov = cov_;
+	}
+}
+Spose_cov::Spose_cov( Spose pose_, const std::vector<double>& cov_ ):
+		Spose(pose_)
+{
+	if ( cov_.empty() || cov_.size() < (std::size_t) 25 )
+	{
+		cov.reserve(25);
+		cov.resize(25,0.0);
+		cov[0] = 0.2;//xx //TODO: for the side_by_side 2 people. We need less covariance of prediction.
+		cov[6] = 0.2;//yy
+		cov[12] = 0.05;//theta_theta
+		cov[18] = 0.1;//cov_vv
+		cov[24] = 0.1;//ww
+	}
+	else
+	{
+		cov = cov_;
+	}
+}
+Spose_cov::Spose_cov( SpointV_cov point ):
+		Spose(point.x,point.y,point.time_stamp,point.orientation(),point.v())
+{
+	cov.reserve(25);
+	cov.resize(25,0.0);
+	cov[0] = point.cov[0];//xx
+	cov[6] = point.cov[5];//yy
+	cov[12] = 0.05;//theta_theta
+	cov[18] = 0.1;//cov_vv
+	cov[24] = 0.1;//ww
+}
+double Spose_cov::distance( Spose_cov p2) const
+{
+	return Spose::distance( Spose(p2.x,p2.y) );
+}
+double Spose_cov::cov_dist( Spose p2 ) const
+{
+	//analitical inversion of the 2x2 matrix
+	double cov_xx = cov[0];
+	double cov_yy = cov[6];
+	double cov_xy = cov[5];
+	double cov_det = cov_xx * cov_yy - cov_xy * cov_xy;
+	double dx = x - p2.x;
+	double dy = y - p2.y;
+	return ( dx*dx*cov_yy - 2*dx*dy*cov_xy + dy*dy*cov_xx )/cov_det;
+}
+double Spose_cov::cov_dist( Spose_cov p2 ) const
+{
+	return this->cov_dist( Spose(p2.x,p2.y) );
+}
+
+//Sdestinations methods -----------------------------------------------------------
+Sdestination::Sdestination( int id_, double x_ , double y_ , double prob_ ,
+		Sdestination::destination_type type_, std::vector<int> neighbours_ids_ ) :
+	Spoint(x_,y_), id(id_), prob(prob_) ,
+	type(type_), neighbours_ids(neighbours_ids_)  { }
+/*Sdestination::Sdestination( Sdestination dest ) :
+	Spoint(dest.x,dest.y), id(dest.id), prob(dest.prob) ,
+	type(dest.type), neighbours_ids(dest.neighbours_ids)  { }*/
+Sdestination::~Sdestination(){}
+bool Sdestination::operator== (Sdestination d2) const
+{
+	if ( x == d2.x && y == d2.y )
+		return true;
+	else
+		return false;
+}
+
+void Sdestination::print() const
+{
+	std::cout << "destination " << id << "at (" << x << " , " << y  <<  ")  of type " << type  <<
+		"   and probability = " << prob  << "neighbours " << neighbours_ids.size() << std::endl;
+}
+
+
+SdetectionObservation::SdetectionObservation() :
+				SpointV_cov(), id(0) { }
+SdetectionObservation::SdetectionObservation( int id_ , double time_stamp_ ,
+		double x_, double y_, double vx_, double vy_, const std::vector<double>& cov_) :
+			SpointV_cov(x_,y_,time_stamp_,vx_,vy_,cov_) , id(id_) { }
+SdetectionObservation::SdetectionObservation( int id_ , SpointV_cov pointV_cov_) :
+	SpointV_cov( pointV_cov_ ), id(id_) {}
+void SdetectionObservation::print() const
+{
+	std::cout <<  "detection Observation = " << id << " th "  << std::endl;
+	SpointV_cov::print();
+}
+
+
+//Sforce methods
+Sforce::Sforce() : fx(0.0) , fy(0.0) {}
+Sforce::Sforce(double fx_ , double fy_) : fx(fx_) , fy(fy_) {}
+double Sforce::module() { return sqrt(fx*fx + fy*fy);}
+double Sforce::module(double r2) { return sqrt(fx*fx + r2*fy*fy);}
+double Sforce::module2( ) { return fx*fx + fy*fy;}
+double Sforce::module2( double r2) { return fx*fx + r2*fy*fy;}
+void Sforce::print() const
+{
+	std::cout <<  "Force = (" << fx << " , " << fy  << " )" << std::endl;
+}
+Sforce Sforce::operator+ (Sforce f2) const
+{
+	return Sforce(  fx + f2.fx , fy + f2.fy );
+}
+void Sforce::sum(Sforce f2)
+//a method similar to +=
+{
+	fx += f2.fx;
+	fy += f2.fy;
+}
+Sforce& Sforce::operator +=(Sforce f2)
+{
+	fx += f2.fx;
+	fy += f2.fy;
+	return *this;
+}
+
+Sforce Sforce::operator* (double k) const
+{
+	return Sforce(  fx *k , fy *k );
+}
+
+double Sforce::operator* (Spoint dr) const
+{
+	return this->fx * dr.x + this->fy * dr.y;
+}
+Sforce Sforce::operator- (Sforce f2) const
+{
+    return Sforce(  fx - f2.fx , fy - f2.fy );
+}
+
+double diffangle(double alpha , double beta)
+{
+	  double delta = alpha - beta;
+	  if ( alpha >= beta )
+	  {
+	    while (delta >  PI)
+	      delta -= 2*PI;
+	  }
+	  else
+	  {
+		  while (delta < -PI )
+			  delta += 2*PI;
+	  }
+	  return delta;
+}
+
+
+SsavePath_cost_and_values::SsavePath_cost_and_values(int in_end_id_path, std::vector<Spoint> in_path_positions, std::vector<unsigned int> in_all_ids_path_positions, std::vector<std::vector<double>> in_total_costs_of_each_path_position) {
+	end_id_path=in_end_id_path;
+	path_positions=in_path_positions;
+	all_ids_path_positions=in_all_ids_path_positions;
+	total_costs_of_each_path_position=in_total_costs_of_each_path_position;
+}
+
+void SsavePath_cost_and_values::print() const
+{
+	std::cout <<  " end_id_path="<<end_id_path << std::endl;
+	for(unsigned int i=0; i<path_positions.size();i++){
+		std::cout <<  " all_ids_path_positions.id="<<all_ids_path_positions[i] << std::endl;
+		path_positions[i].print();
+		for(unsigned int j=0; j<total_costs_of_each_path_position[i].size();j++){
+			std::cout <<  " toal_costs_of_each_path_position[id]["<<j<<"]="<<total_costs_of_each_path_position[i][j] << std::endl;
+		}
+	}
+
+}
+
+SsavePath_multiple_paths_and_best_path::SsavePath_multiple_paths_and_best_path(double in_time_act, double in_iter_act, int in_number_of_total_paths,std::vector<SsavePath_cost_and_values> in_nondominated_paths, SsavePath_cost_and_values in_best_path,std::vector<double> in_means,std::vector<double> in_stds){
+
+	time_act=in_time_act;
+	iter_act=in_iter_act;
+	number_of_total_paths=in_number_of_total_paths;
+	nondominated_paths=in_nondominated_paths;
+	best_path=in_best_path;
+	means=in_means;
+	stds=in_stds;
+
+}
+
+
+void SsavePath_multiple_paths_and_best_path::print() const
+{
+	std::cout <<  " time_act="<<time_act << std::endl;
+	std::cout <<  " iter_act="<<iter_act << std::endl;
+	std::cout <<  " number_of_total_paths="<<number_of_total_paths << std::endl;
+	std::cout <<  " print nondominated_paths: nondominated_paths.size()="<<nondominated_paths.size() << std::endl;
+
+	for(unsigned int i=0; i<nondominated_paths.size();i++){
+		nondominated_paths[i].print();
+	}
+
+	std::cout <<  " print best_paths:" << std::endl;
+	best_path.print();
+
+	std::cout <<  " print means: mean_distance_[0]; mean_orientation_[1]; mean_robot_[2]; mean_int_forces_[3]; mean_obstacles_[4]; mean_companion_[5]; ant_traj[6]; " << std::endl;
+	for(unsigned int i=0; i<means.size();i++){
+		std::cout <<  "means["<<i<<"]"<<means[i]<< std::endl;
+	}
+
+	for(unsigned int i=0; i<stds.size();i++){
+		std::cout <<  "stds["<<i<<"]"<<stds[i]<< std::endl;
+	}
+}
+
+
+
+/* Convert a decimal integer to a c-style string */
+char* Spoint::itoa(int a, char b[])
+{
+   int NumofDigits = getNumofDecDigit(a);
+    if(a>0)
+    {
+    switch(NumofDigits)
+    {
+        case 1:
+        b[0]=getStrChar(a);
+        b[1]='\0';
+        break;
+
+        case 2:
+        b[0]= getStrChar(a/10);
+        b[1]= getStrChar(a%10);
+        b[2]='\0';
+        break;
+
+        case 3:
+        b[0]= getStrChar(a/100);
+        b[1]= getStrChar((a/10)%10);
+        b[2]= getStrChar(a%10);
+        b[3]='\0';
+        break;
+
+        case 4:
+        b[0]= getStrChar(a/1000);
+        b[1]= getStrChar((a/100)%10);
+        b[2]= getStrChar((a/10)%10);
+        b[3]= getStrChar(a%10);
+        b[4]='\0';
+        break;
+
+        case 5:
+        b[0]= getStrChar(a/10000);
+        b[1]= getStrChar((a/1000)%10);
+        b[2]= getStrChar((a/100)%10);
+        b[3]= getStrChar((a/10)%10);
+        b[4]= getStrChar(a%10);
+        b[5]='\0';
+        break;
+
+        case 6:
+        b[0]= getStrChar(a/100000);
+        b[1]= getStrChar((a/10000)%10);
+        b[2]= getStrChar((a/1000)%10);
+        b[3]= getStrChar((a/100)%10);
+        b[4]= getStrChar((a/10)%10);
+        b[5]= getStrChar(a%10);
+        b[6]='\0';
+        break;
+
+        case 7:
+        b[0]= getStrChar(a/1000000);
+        b[1]= getStrChar((a/100000)%10);
+        b[2]= getStrChar((a/10000)%10);
+        b[3]= getStrChar((a/1000)%10);
+        b[4]= getStrChar((a/100)%10);
+        b[5]= getStrChar((a/10)%10);
+        b[6]= getStrChar(a%10);
+        b[6]='\0';
+        break;
+
+        case 8:
+        b[0]= getStrChar(a/10000000);
+        b[1]= getStrChar((a/1000000)%10);
+        b[2]= getStrChar((a/100000)%10);
+        b[3]= getStrChar((a/10000)%10);
+        b[4]= getStrChar((a/1000)%10);
+        b[5]= getStrChar((a/100)%10);
+        b[6]= getStrChar((a/10)%10);
+        b[7]= getStrChar(a%10);
+        b[8]='\0';
+        break;
+
+        default:
+        b[0]='\0';
+    }
+    }
+    else
+    {
+    switch(NumofDigits)
+    {
+        case 1:
+        b[0]='-';
+        b[1]=getStrChar(a);
+        b[2]='\0';
+        break;
+
+        case 2:
+        b[0]='-';
+        b[1]= getStrChar(a/10);
+        b[2]= getStrChar(a%10);
+        b[3]='\0';
+        break;
+
+        case 3:
+        b[0]='-';
+        b[1]= getStrChar(a/100);
+        b[2]= getStrChar((a/10)%10);
+        b[3]= getStrChar(a%10);
+        b[4]='\0';
+        break;
+
+        case 4:
+        b[0]='-';
+        b[1]= getStrChar(a/1000);
+        b[2]= getStrChar((a/100)%10);
+        b[3]= getStrChar((a/10)%10);
+        b[4]= getStrChar(a%10);
+        b[5]='\0';
+        break;
+
+        case 5:
+        b[0]='-';
+        b[1]= getStrChar(a/10000);
+        b[2]= getStrChar((a/1000)%10);
+        b[3]= getStrChar((a/100)%10);
+        b[4]= getStrChar((a/10)%10);
+        b[5]= getStrChar(a%10);
+        b[6]='\0';
+        break;
+
+        case 6:
+        b[0]='-';
+        b[1]= getStrChar(a/100000);
+        b[2]= getStrChar((a/10000)%10);
+        b[3]= getStrChar((a/1000)%10);
+        b[4]= getStrChar((a/100)%10);
+        b[5]= getStrChar((a/10)%10);
+        b[6]= getStrChar(a%10);
+        b[7]='\0';
+        break;
+
+        case 7:
+        b[0]='-';
+        b[1]= getStrChar(a/1000000);
+        b[2]= getStrChar((a/100000)%10);
+        b[3]= getStrChar((a/10000)%10);
+        b[4]= getStrChar((a/1000)%10);
+        b[5]= getStrChar((a/100)%10);
+        b[6]= getStrChar((a/10)%10);
+        b[7]= getStrChar(a%10);
+        b[8]='\0';
+        break;
+
+        case 8:
+        b[0]='-';
+        b[1]= getStrChar(a/10000000);
+        b[2]= getStrChar((a/1000000)%10);
+        b[3]= getStrChar((a/100000)%10);
+        b[4]= getStrChar((a/10000)%10);
+        b[5]= getStrChar((a/1000)%10);
+        b[6]= getStrChar((a/100)%10);
+        b[7]= getStrChar((a/10)%10);
+        b[8]= getStrChar(a%10);
+        b[9]='\0';
+        break;
+
+        default:
+        b[0]='\0';
+        break;
+    }
+
+    }
+    if (a==0)
+    {
+        b[0]= '0';
+        b[1]='\0';
+    }
+
+  return b;
+}
+
+/* Get number of digits in a positive or negative integer */
+int Spoint::getNumofDecDigit(int Number)
+{
+   int NumofDigits=0;
+    if(Number < 0)
+      {
+          Number=-Number;
+      }
+      while(Number)
+      {
+          Number/=10;
+          NumofDigits++;
+      }
+    return NumofDigits;
+}
+
+/* Get a character from  digit input number */
+char Spoint::getStrChar(int digit)
+{
+   char  StrChar = ' ';
+    switch (digit)
+    {
+        case 0:
+        StrChar = '0';
+        break;
+
+        case 1:
+        case -1:
+        StrChar ='1';
+        break;
+
+        case 2:
+        case -2:
+        StrChar = '2';
+        break;
+
+        case 3:
+        case -3:
+        StrChar = '3';
+        break;
+
+        case 4:
+        case -4:
+        StrChar = '4';
+        break;
+
+        case 5:
+        case -5:
+        StrChar = '5';
+        break;
+
+        case 6:
+        case -6:
+        StrChar = '6';
+        break;
+
+        case 7:
+        case -7:
+        StrChar = '7';
+        break;
+
+        case 8:
+        case -8:
+        StrChar = '8';
+        break;
+
+        case 9:
+        case -9:
+        StrChar = '9';
+        break;
+
+        default:
+        StrChar ='0';
+        break;
+    }
+    return StrChar;
+}
+
+
+std::string get_path_learning::addPath(std::string a, std::string c){
+    std::string b = "/";
+    return a+b+c;
+}
+
+std::vector<std::string> get_path_learning::folderStream(std::string PATH){
+    std::vector<std::string> directories;
+    DIR *dir = opendir(PATH.c_str());
+    struct dirent *entry = readdir(dir);
+    while (entry != NULL)
+    {
+        if (entry->d_type == DT_DIR){
+            std::string aux = entry->d_name;
+            if(aux.compare(".") != 0 and aux.compare("..")!=0){
+                directories.push_back(entry->d_name);
+            }
+        }
+        entry = readdir(dir);
+    }
+
+    closedir(dir);
+
+    return directories;
+}
+
+
+void get_path_learning::pathGeneratoraux(std::string path, int level, std::vector<std::string> &paths){
+	std::cout<<"pathGeneratoraux 1"<<std::endl;
+    std::vector<std::string> v = folderStream(path);
+    std::cout<<"pathGeneratoraux 2"<<std::endl;
+    if(level == 0){
+    	std::cout<<"pathGeneratoraux 3"<<std::endl;
+        for(unsigned int i = 0; i<v.size();i++){
+            paths.push_back(addPath(path,v[i]));
+        }
+    }
+    else{
+        for(unsigned int i = 0; i<v.size();i++){
+            pathGeneratoraux(addPath(path,v[i]),level-1,paths);
+        }
+    }
+}
+
+std::vector<std::string> get_path_learning::pathGenerator(std::string path){
+	 std::cout<<"pathGenerator 1"<<std::endl;
+    std::vector<std::string> paths;
+    std::cout<<"pathGenerator 2"<<std::endl;
+    pathGeneratoraux(path,1,paths);
+    std::cout<<"pathGenerator 3"<<std::endl;
+    return paths;
+}
+
+peopleOtherPersonCompanionInGroup::peopleOtherPersonCompanionInGroup(  int in_real_person_id , unsigned int in_actual_new_id_in_people_simulation){
+	real_person_id_=in_real_person_id;
+	actual_new_id_in_people_simulation_=in_actual_new_id_in_people_simulation;
+}
+
+void peopleOtherPersonCompanionInGroup::print() const
+{
+	 std::cout<<"real_person_id_="<<real_person_id_<<std::endl;
+	 std::cout<<"actual_new_id_in_people_simulation_="<<actual_new_id_in_people_simulation_<<std::endl;
+}
diff --git a/local_lib_people_prediction/src/iri_geometry.h b/local_lib_people_prediction/src/iri_geometry.h
new file mode 100644
index 0000000000000000000000000000000000000000..db85d5090fa180a129868ad6d24d37b967d841c1
--- /dev/null
+++ b/local_lib_people_prediction/src/iri_geometry.h
@@ -0,0 +1,335 @@
+/*
+ * iri_geometry.h
+ *
+ *  Created on: 2013.  Last Modified on 2020
+ *      Author: Initial code of the AKP Gonzalo Ferrer.
+ *      Author: Addition of ASSAOP and ASSAGP for people accompaniment code of Ely Repiso.
+ */
+
+#ifndef iri_geometry_H
+#define iri_geometry_H
+
+#include <vector>
+#include <iostream>
+const static double PI = 3.1415926535897932384626433;
+
+/**
+ *	\brief iri_geometry library
+ *
+ *	It consists of a set of data structures, similar to structs, where all values are accessible from
+ *	the outside and a set of simple methods for each geometry primitive.
+ *
+ *
+ *  Note: In general, these classes are designed as simple data structures with methods associated
+ *  By no means they are intended to be created as pointers and new or used polymorphically.
+ *  No dynamical binding of the functions
+ *  has been designed (only statical, beware on how you declare them), so they won't work!!!!
+ *  Simply use it as data types, as can be seen in test/test_geometry.
+ *
+  */
+
+
+/**
+ *	\brief Sforce Struct
+ *
+ *	data struct describing a 2d force (fx,fy)
+ *	and their corresponding methods
+ *	TODO: add force covariances
+ */
+class Spoint;
+class Sforce
+{
+  public:
+	double fx, fy;
+	Sforce();
+	Sforce(double fx_ , double fy_);
+	double module( void );
+	double module( double r2 );//r2 if the square factor to compate fx and r2*fy
+	double module2( void );
+	double module2( double r2 );
+	Sforce operator+ (Sforce f2) const;
+	void sum(Sforce f2);//a method similar to +=
+	Sforce& operator+= (Sforce f2);
+	Sforce operator* (double k) const;
+	double operator* (Spoint dr) const;
+    Sforce operator- (Sforce f2) const;
+	void print() const;
+};
+
+/**
+ *	\brief Spoint Struct
+ *
+ *	data struct describing a 2d point (x,y).
+ */
+class Spoint
+{
+  public:
+	double x;
+	double y;
+	double time_stamp; //sec ;
+	Spoint();
+	Spoint( double x_ , double y_ , double time_stamp_=0.0) ;
+	Spoint operator+ (Spoint p2) const ;
+	Spoint operator- (Spoint p2) const ;
+	Spoint operator* (double k) const ;
+	double distance(Spoint p2 = Spoint() ) const;
+	double distance2( Spoint p2 = Spoint() ) const;
+	Spoint propagate( double dt , Sforce f = Sforce(), double desired_velocity=2.0) const;
+	char* itoa(int a, char b[]);
+
+	/* Get number of digits in an integer */
+	int getNumofDecDigit(int Number);
+
+	/* get a character from  digit input number */
+	char getStrChar(int digit);
+	void print() const;
+};
+
+/**
+ *	\brief Spoint_cov Struct
+ *
+ *	data struct describing a 2d point (x,y). and its covariance. This covariance is represented as
+ *	a vector of 4 elements
+ */
+class Spoint_cov : public Spoint
+{
+  public:
+	std::vector<double> cov; // size = 16= 4x4
+	Spoint_cov();
+	Spoint_cov( double x_ , double y_ , double time_stamp_=0.0, const std::vector<double>& cov_= std::vector<double>() );
+	double cov_dist( Spoint p2=Spoint() ) const;
+    Spoint_cov operator+ (Spoint_cov p2) const ;
+    Spoint_cov operator- (Spoint_cov p2) const ;
+    Spoint_cov operator* (double k) const ;
+    double cov_xx() const;
+    double cov_yy() const;
+    double cov_xy() const;
+	Spoint_cov propagate( double dt , Sforce f = Sforce(), double desired_velocity=2.0) const;
+	void print() const;
+};
+
+
+/**
+ *	\brief SpointV Struct
+ *
+ *	data struct describing a 2d point (x,y) and its velocities (derivate)
+ */
+class SpointV : public Spoint
+{
+  public:
+	double vx;
+	double vy;
+	SpointV();
+	SpointV( double x_ , double y_, double time_stamp_=0.0, double vx_=0.0, double vy_=0.0 ) ;
+	SpointV operator+ (SpointV p2) const ;
+	SpointV operator- (SpointV p2) const ;
+	SpointV operator* (double k) const ;
+	SpointV propagate( double dt , Sforce f = Sforce(), double desired_velocity=2.0) const;
+	SpointV propagate2_comp_only_vel( double dt , Sforce f= Sforce(), double desired_velocity=2.0, double distance_person_t_x=0.0,double distance_person_t_y=0.0,double vel_person_x=0.0,double vel_person_y=0.0) const;
+	double orientation() const;
+	double v() const;
+	void norm_v(double v);
+	double angle_heading_point( Spoint p2 ) const;
+	void print() const;
+};
+
+/**
+ *	\brief SpointV Struct
+ *
+ *	data struct describing a 2d point (x,y) and its velocities (derivate)
+ */
+class SpointV_cov : public SpointV
+{
+  public:
+	std::vector<double> cov;
+	SpointV_cov();
+	SpointV_cov(double x_ , double y_, double time_stamp_=0.0, double vx_=0.0, double vy_=0.0,
+			const std::vector<double>& cov_= std::vector<double>());
+	SpointV_cov(SpointV p,	const std::vector<double>& cov_= std::vector<double>());
+	SpointV_cov(Spoint_cov p );
+	//void SpointV_cov_con_Spose(Spose p);
+	//SpointV_cov(SpointV_cov& p );
+	double cov_dist( Spoint p2=Spoint() ) const;
+	double cov_dist( Spoint p2 , double &det) const;
+	double cov_distV( SpointV_cov p2, double &distV, double &distxv  ) const;
+    SpointV_cov operator+ (SpointV_cov p2) const ;
+    SpointV_cov operator- (SpointV_cov p2) const ;
+    SpointV_cov operator* (double k) const ;
+    SpointV_cov propagate( double dt , Sforce f = Sforce(), double desired_velocity=2.0 ) const;
+    double cov_xx() const;
+    double cov_yy() const;
+    double cov_xy() const;
+    Spoint_cov toSpoint_cov() const;
+    void print() const;
+};
+/**
+ *	\brief Spose Struct
+ *
+ *	data struct describing a pose consisting of position, velocity
+ *	and time.
+ */
+class Spose : public Spoint
+{
+  public:
+	double theta;
+	double v;
+	double w;
+	Spose();
+	Spose( double x_ , double y_ , double time_stamp_=0.0 , double theta_=0.0 ,  double v_=0.0, double w_=0.0);
+	Spose operator+ (Spose p2) const;
+	Spose operator- (Spose p2) const;
+	Spose operator* (double k) const;
+	double distance(Spoint p2) const;
+	double angle_heading_pose( Spose p2 ) const;
+	double social_distance( Spose p2 ) const;
+	void print() const;
+};
+/**
+ *	\brief SposeCov Struct
+ *
+ *	data struct describing a pose and its covariance (only in position)
+ *	It also supports distance to other pose or distances-based on covariances
+ */
+class Spose_cov : public Spose
+{
+  public:
+	std::vector<double> cov;
+	Spose_cov();
+	Spose_cov( double x_ , double y_ ,
+			double time_stamp_=0.0 ,double theta_=0.0 ,  double v_=0.0, double w_=0.0,
+			const std::vector<double>& cov_= std::vector<double>());
+	Spose_cov( Spose pose_, const std::vector<double>& cov_= std::vector<double>());
+	Spose_cov( SpointV_cov point );//conversion from point to pose
+	double distance( Spose_cov p2 ) const;
+	double cov_dist( Spose p2 ) const;
+	double cov_dist( Spose_cov p2 ) const;
+};
+
+/**
+ *	\brief Sdestination Struct
+ *
+ *	data struct describing the destination information:
+ *	position, prior probability and type{fixed, temporal}
+ */
+class Sdestination : public Spoint
+{
+  public:
+	enum destination_type { Map_goal=0 , Stopping, Uncertain};
+	Sdestination( int id_=0, double x_=0.0 , double y_=0.0 ,
+			double prob_=0.0 ,
+			destination_type type_=Map_goal ,
+			std::vector<int> neighbours_ids_ = std::vector<int>());
+	//Sdestination( Sdestination dest );
+	~Sdestination();
+	int id;
+	double prob;
+	destination_type type;
+	std::vector<int> neighbours_ids;
+	bool operator== (Sdestination d2) const;
+	void print() const;
+
+	void sed_dest_type(destination_type type_in){
+		type=type_in;
+	}
+};
+
+/**
+ *
+ *  \brief detectionObservation Struct to be deprecated... pass a vector of Spoint/Pose and
+ *  a vector of id's TODO
+ *
+ */
+
+class SdetectionObservation : public SpointV_cov
+{
+  public:
+	int id;
+	SdetectionObservation();
+	SdetectionObservation( int id_ , double time_stamp_=0.0, double x_=0.0, double y_=0.0, double vx_=0.0, double vy_=0.0,
+			const std::vector<double>& cov_= std::vector<double>());
+	SdetectionObservation( int id_ , SpointV_cov pointV_cov_);
+	void print() const;
+};
+
+
+
+double diffangle(double alpha , double beta);
+inline bool is_nan(double x) { return x != x;}
+
+class SsavePath_cost_and_values
+{
+  public:
+	int end_id_path;
+	std::vector<Spoint> path_positions;
+	std::vector<unsigned int> all_ids_path_positions;
+	std::vector<std::vector<double> > total_costs_of_each_path_position;
+
+
+	SsavePath_cost_and_values(int in_end_id_path=0, std::vector<Spoint> in_path_positions=std::vector<Spoint>(), std::vector<unsigned int> in_all_ids_path_positions=std::vector<unsigned int>(), std::vector<std::vector<double> > in_total_costs_of_each_path_position=std::vector<std::vector<double> >());
+	void set_end_id_path(int in_end_id_path){end_id_path=in_end_id_path;}
+	void set_path_positions(std::vector<Spoint> in_path_positions){path_positions=in_path_positions;};
+	void set_all_ids_path_positions(std::vector<unsigned int> in_all_ids_path_positions){all_ids_path_positions=in_all_ids_path_positions;};
+	void set_total_costs_of_each_path_position(std::vector<std::vector<double> > in_total_costs_of_each_path_position){
+
+		total_costs_of_each_path_position=in_total_costs_of_each_path_position;
+		//std::cout << " ENTRO EN: set_total_costs_of_each_path_position; total_costs_of_each_path_position.size()="<<total_costs_of_each_path_position.size()<< std::endl;
+	};
+	void print() const;
+};
+
+class SsavePath_multiple_paths_and_best_path // habra un struct de esto por cada iteración.
+{
+  public:
+	double time_act;
+	double iter_act;
+	int number_of_total_paths;
+	std::vector<SsavePath_cost_and_values> nondominated_paths;
+	SsavePath_cost_and_values best_path;
+	std::vector<double> means;
+	std::vector<double> stds;
+
+	SsavePath_multiple_paths_and_best_path(double in_time_act=0.0, double in_iter_act=0.0, int in_number_of_total_paths=0.0,std::vector<SsavePath_cost_and_values> in_nondominated_paths=std::vector<SsavePath_cost_and_values>(), SsavePath_cost_and_values in_best_path=SsavePath_cost_and_values(),std::vector<double> in_means=std::vector<double>(),std::vector<double> in_stds=std::vector<double>());
+	void set_time_act(double in_time_act){time_act=in_time_act;};
+	void set_iter_act(double in_iter_act){iter_act=in_iter_act;};
+	void set_number_of_total_paths(int in_number_of_total_paths){
+		number_of_total_paths=in_number_of_total_paths;
+		//std::cout << " ENTRO EN: set_number_of_total_paths; number_of_total_paths="<<number_of_total_paths<< std::endl;
+	};
+	void set_nondominated_paths(std::vector<SsavePath_cost_and_values> in_nondominated_paths){
+
+		nondominated_paths=in_nondominated_paths;
+		//std::cout << " ENTRO EN: set_nondominated_paths; nondominated_paths.size()="<<nondominated_paths.size()<< std::endl;
+
+	};
+	std::vector<SsavePath_cost_and_values> get_nondominated_paths(){ return nondominated_paths;};
+	void set_best_path(SsavePath_cost_and_values in_best_path){
+		best_path=in_best_path;
+		//std::cout << " ENTRO EN: set_best_path; best_path.total_costs_of_each_path_position.size()="<<best_path.total_costs_of_each_path_position.size()<< std::endl;
+	};
+	void set_means(std::vector<double> in_means){means=in_means;};
+	void set_stds(std::vector<double> in_stds){stds=in_stds;};
+	void print() const;
+};
+
+class get_path_learning // habra un struct de esto por cada iteración.
+{
+    public:
+	std::string addPath(std::string a, std::string c);
+	std::vector<std::string> folderStream(std::string PATH);
+
+	void pathGeneratoraux(std::string path, int level, std::vector<std::string> &paths);
+	std::vector<std::string> pathGenerator(std::string path);
+
+};
+
+
+class peopleOtherPersonCompanionInGroup
+{
+  public:
+	 int real_person_id_;
+	unsigned int actual_new_id_in_people_simulation_;
+	peopleOtherPersonCompanionInGroup(  int in_real_person_id , unsigned int in_actual_new_id_in_people_simulation);
+	void print() const;
+};
+
+#endif
diff --git a/local_lib_people_prediction/src/learning_parameters/getFilesnames_script.cpp b/local_lib_people_prediction/src/learning_parameters/getFilesnames_script.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..bf60edd2092d8d81f46508b499de3ed47b9af8a9
--- /dev/null
+++ b/local_lib_people_prediction/src/learning_parameters/getFilesnames_script.cpp
@@ -0,0 +1,60 @@
+
+#include<iostream>
+#include<string>
+#include<vector>
+#include <dirent.h>
+#include <stdio.h>
+
+
+std::string addPath(std::string a, std::string c){
+    std::string b = "/";
+    return a+b+c;
+}
+
+std::vector<std::string> folderStream(std::string PATH){
+    std::vector<std::string> directories;
+    DIR *dir = opendir(PATH.c_str());
+    struct dirent *entry = readdir(dir);
+    while (entry != NULL)
+    {
+        if (entry->d_type == DT_DIR){
+            std::string aux = entry->d_name;
+            if(aux.compare(".") != 0 and aux.compare("..")!=0){
+                directories.push_back(entry->d_name);
+            }
+        }
+        entry = readdir(dir);
+    }
+
+    closedir(dir);
+
+    return directories;
+}
+
+void pathGeneratoraux(std::string path, int level, std::vector<std::string> &paths){
+    std::vector<std::string> v = folderStream(path);
+    if(level == 0){
+        for(int i = 0; i<v.size();i++){
+            paths.push_back(addPath(path,v[i]));
+        }
+    }
+    else{
+        for(int i = 0; i<v.size();i++){
+            pathGeneratoraux(addPath(path,v[i]),level-1,paths);
+        }
+    }
+}
+
+std::vector<std::string> pathGenerator(std::string path){
+    std::vector<std::string> paths;
+    pathGeneratoraux(path,1,paths);
+    return paths;
+}
+
+int main(int argc,char *argv[]){
+    std::string path = "/home/aserra/Documents/IRI/Database_creation/Experiments";
+    std::vector<std::string> paths = pathGenerator(path);
+    for(int i = 0; i<paths.size(); i++){
+        std::cout<<paths[i]<<std::endl;
+    }
+}
\ No newline at end of file
diff --git a/local_lib_people_prediction/src/learning_parameters/learnninglinearregression.cpp b/local_lib_people_prediction/src/learning_parameters/learnninglinearregression.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..96c36bc911f3343e524e3249b92151dd17db28bf
--- /dev/null
+++ b/local_lib_people_prediction/src/learning_parameters/learnninglinearregression.cpp
@@ -0,0 +1,107 @@
+/*
+ * learninglinnearregression.cpp
+ *
+ *  Created on: May 19, 2017
+ *      Author: ely7787
+ */
+
+#include "learnninglinearregression.h"
+
+
+/*
+ * learningleastsquaresregression.cpp
+ *
+ *  Created on: May 19, 2017
+ *      Author: ely7787
+ */
+
+//#include "learningleastsquaresregression.h"
+
+learning_least_squares_regression::learning_least_squares_regression():
+learnning_rate_(0.001) // parametro a sacar fuera y modificar con el dynamic reconfigure.
+{
+	// TODO Auto-generated constructor stub
+
+}
+
+learning_least_squares_regression::~learning_least_squares_regression() {
+	// TODO Auto-generated destructor stub
+}
+
+void learning_least_squares_regression::least_squares_iteration(){
+	/* */
+	// calculate error:
+	least_squares_calc_error();
+	//double new_beta0_k=0;
+	//double new_beta1_k=0;
+
+	//new_beta0_k=beta0_k_1_-learnning_rate_*act_regression_error;
+	//new_beta1_k=beta1_k_1_-learnning_rate_*act_regression_error*real_x_value_;
+
+}
+
+void learning_least_squares_regression::least_squares_initialice(){
+	beta0_k_1_=0;
+	beta1_k_1_=0;
+
+}
+
+void learning_least_squares_regression::least_squares_calc_error(){
+	// we use the squared error.
+
+}
+/*
+ * learnningBayesianregression.cpp
+ *
+ *  Created on: May 19, 2017
+ *      Author: ely7787
+ */
+
+//#include "learnningBayesianregression.h"
+
+learnning_Bayesian_regression::learnning_Bayesian_regression() {
+	// TODO Auto-generated constructor stub
+
+}
+
+learnning_Bayesian_regression::~learnning_Bayesian_regression() {
+	// TODO Auto-generated destructor stub
+}
+
+
+
+/*
+ * learningleastsquarespercentageregression.cpp
+ *
+ *  Created on: May 19, 2017
+ *      Author: ely7787
+ */
+
+//#include "learningleastsquarespercentageregression.h"
+
+learning_least_squares_percentage_regression::learning_least_squares_percentage_regression() {
+	// TODO Auto-generated constructor stub
+
+}
+
+learning_least_squares_percentage_regression::~learning_least_squares_percentage_regression() {
+	// TODO Auto-generated destructor stub
+}
+
+/*
+ * learnningleastabsolutedeviationregression.cpp
+ *
+ *  Created on: May 19, 2017
+ *      Author: ely7787
+ */
+
+//#include "learnningleastabsolutedeviationregression.h"
+
+learnning_least_absolute_deviation_regression::learnning_least_absolute_deviation_regression() {
+	// TODO Auto-generated constructor stub
+
+}
+
+learnning_least_absolute_deviation_regression::~learnning_least_absolute_deviation_regression() {
+	// TODO Auto-generated destructor stub
+}
diff --git a/local_lib_people_prediction/src/learning_parameters/learnninglinearregression.h b/local_lib_people_prediction/src/learning_parameters/learnninglinearregression.h
new file mode 100644
index 0000000000000000000000000000000000000000..0c083359f414bb2c0146e637756a9939a67860fb
--- /dev/null
+++ b/local_lib_people_prediction/src/learning_parameters/learnninglinearregression.h
@@ -0,0 +1,102 @@
+/*
+ * learninglinnearregression.h
+ *
+ *  Created on: May 19, 2017
+ *      Author: ely7787
+ */
+
+#ifndef SRC_LEARNING_PARAMETERS_LEARNNINGLINEARREGRESSION_H_
+#define SRC_LEARNING_PARAMETERS_LEARNNINGLINEARREGRESSION_H_
+
+/*class learning_linnear_regression {
+};*/
+
+//#endif /* SRC_LEARNING_PARAMETERS_LEARNINGLINNEARREGRESSION_H_ */
+
+/*
+ * learningleastsquaresregression.h
+ *
+ *  Created on: May 19, 2017
+ *      Author: ely7787
+ */
+
+//#ifndef SRC_LEARNING_PARAMETERS_LEARNINGLEASTSQUARESREGRESSION_H_
+//#define SRC_LEARNING_PARAMETERS_LEARNINGLEASTSQUARESREGRESSION_H_
+
+class learning_least_squares_regression {
+public:
+	// variables de la clase
+	double learnning_rate_; // alpha=0.001 parametro a sacar y modificar con el dynamic reconfigure.
+							// es el step de lo que cambia el error el valor de los parametros.
+	double beta0_k_1_;
+	double beta1_k_1_;
+	double act_regression_error;
+
+	double real_y_value_;
+	double real_x_value_;
+
+	// funciones de la clase
+	learning_least_squares_regression();
+	virtual ~learning_least_squares_regression();
+	void least_squares_iteration();
+	void least_squares_initialice();
+	void least_squares_calc_error();
+};
+
+//#endif /* SRC_LEARNING_PARAMETERS_LEARNINGLEASTSQUARESREGRESSION_H_ */
+
+
+/*
+ * learnningBayesianregression.h
+ *
+ *  Created on: May 19, 2017
+ *      Author: ely7787
+ */
+
+//#ifndef SRC_LEARNING_PARAMETERS_LEARNNINGBAYESIANREGRESSION_H_
+//#define SRC_LEARNING_PARAMETERS_LEARNNINGBAYESIANREGRESSION_H_
+
+class learnning_Bayesian_regression {
+public:
+	learnning_Bayesian_regression();
+	virtual ~learnning_Bayesian_regression();
+};
+
+//#endif /* SRC_LEARNING_PARAMETERS_LEARNNINGBAYESIANREGRESSION_H_ */
+
+
+/*
+ * learningleastsquarespercentageregression.h
+ *
+ *  Created on: May 19, 2017
+ *      Author: ely7787
+ */
+
+//#ifndef SRC_LEARNING_PARAMETERS_LEARNINGLEASTSQUARESPERCENTAGEREGRESSION_H_
+//#define SRC_LEARNING_PARAMETERS_LEARNINGLEASTSQUARESPERCENTAGEREGRESSION_H_
+
+class learning_least_squares_percentage_regression {
+public:
+	learning_least_squares_percentage_regression();
+	virtual ~learning_least_squares_percentage_regression();
+};
+
+//#endif /* SRC_LEARNING_PARAMETERS_LEARNINGLEASTSQUARESPERCENTAGEREGRESSION_H_ */
+
+/*
+ * learnningleastabsolutedeviationregression.h
+ *
+ *  Created on: May 19, 2017
+ *      Author: ely7787
+ */
+
+//#ifndef SRC_LEARNING_PARAMETERS_LEARNNINGLEASTABSOLUTEDEVIATIONREGRESSION_H_
+//#define SRC_LEARNING_PARAMETERS_LEARNNINGLEASTABSOLUTEDEVIATIONREGRESSION_H_
+
+class learnning_least_absolute_deviation_regression {
+public:
+	learnning_least_absolute_deviation_regression();
+	virtual ~learnning_least_absolute_deviation_regression();
+};
+
+#endif /* SRC_LEARNING_PARAMETERS_LEARNNINGLEASTABSOLUTEDEVIATIONREGRESSION_H_ */
diff --git a/local_lib_people_prediction/src/nav/force_reactive_robot_companion.cpp b/local_lib_people_prediction/src/nav/force_reactive_robot_companion.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..2fe2dcd4f43c41688e0fc9e3432945b45226b536
--- /dev/null
+++ b/local_lib_people_prediction/src/nav/force_reactive_robot_companion.cpp
@@ -0,0 +1,423 @@
+/*
+ * force_reactive_robot_companion.cpp
+ *
+ *  Created on: Nov 9, 2013
+ *      Author: gonzalo. Deprecated, do mot used. The new companion by Ely Repiso is anticipative not reactive.
+ */
+
+
+#include "nav/force_reactive_robot_companion.h"
+#include <math.h>
+#include <iostream>
+
+Cforce_reactive_robot_companion::Cforce_reactive_robot_companion ( ) :
+	Cprediction_bhmip(),
+	target_person_id_(0),
+	robot_state_( UNKNOWN_ZONE ),
+	v_max_(2.0),
+	v_cruise_(1.0),
+	time_horizon_(0.2),
+	is_target_person_visible_(false)
+{
+	param_force_approach_.push_back(0.1);//force goal
+	param_force_approach_.push_back(1.0);//force persongoal
+	param_force_approach_.push_back(7.0);//force interaction persons
+	param_force_approach_.push_back(1.0);//force interaction laser
+	param_force_approach_.push_back(1.0);//force interaction map
+
+}
+
+Cforce_reactive_robot_companion::~Cforce_reactive_robot_companion()
+{
+	//parent class do their destructors and no need to do anything free memory in this class
+}
+
+SpointV_cov Cforce_reactive_robot_companion::robot_approach_pose(bool stop_if_smn_in_inner)
+{  //stop_if_smn_in_inner=> true para anclarse a la persona más cercana. (lo usa para real, darle el id de la persona más cercana!)
+	Sforce f1,f2,f3,f4,f5,f;
+	return robot_approach_pose(f1,f2,f3,f4,f5,f,stop_if_smn_in_inner );
+}
+
+SpointV_cov Cforce_reactive_robot_companion::robot_approach_pose( Sforce& f_goal, Sforce& f_persongoal,
+	Sforce& f_int_pers, Sforce& f_int_laser, Sforce& f_int_map, Sforce& f , bool stop_if_smn_in_inner)
+{
+	std::cout << " IN!!! Cforce_reactive_robot_companion::robot_approach_pose" << std::endl;
+	//get initial conditions
+
+	//robot_->print();
+	internal_robot_->print();
+	std::cout << " desp_robot_print (0.1)" << std::endl;
+	//Spose act_robot=robot_->get_current_pose();
+	Spose act_robot=internal_robot_->get_current_pose();
+	std::cout << " act_robot (0.1)" << std::endl;
+	act_robot.print();
+	double vx=act_robot.v*cos(act_robot.theta);
+	std::cout << " vx="<<vx << std::endl;
+	double vy=act_robot.v*sin(act_robot.theta);
+	std::cout << " vy="<<vy << std::endl;
+	SpointV_cov robot=SpointV_cov(act_robot.x , act_robot.y, act_robot.time_stamp, vx,vy);
+	//SpointV_cov robot = robot_->get_current_pointV();
+	std::cout << " (0.1)" << std::endl;
+	SpointV_cov approach;
+	std::cout << " (0.2)" << std::endl;
+	approach.x = robot.x;
+	std::cout << " (0.3)" << std::endl;
+	//approach.x = 1.0/0.0; //sending invalid commands to stop movement
+	approach.y = robot.y;
+	std::cout << " (0.4)" << std::endl;
+	//approach.y = 1.0/0.0;
+	approach.time_stamp = robot.time_stamp;
+	std::cout << " (0.5)" << std::endl;
+	approach.vx = 0.0; //no approach, stays at current position at v=0 TODO
+	approach.vy = 0.0;
+	std::cout << " (0.6)" << std::endl;
+	Cperson_abstract* person_obj;
+	std::cout << " (1)" << std::endl;
+	//checking if stop_if_smn_in_inner flag is set
+	if( stop_if_smn_in_inner ) // por defecto es false!, entonce no pilla la más cercana, sino el caso siguiente.
+	{
+		std::cout << " (1.1)" << std::endl;
+		//Cperson_abstract* nearest = nearest_person( robot_ );
+		Cperson_abstract* nearest = nearest_person( internal_robot_ );
+		//double phi = robot_->get_current_pointV().angle_heading_point( nearest->get_current_pointV() );
+		double phi = internal_robot_->get_current_pointV().angle_heading_point( nearest->get_current_pointV() );
+		double lambda = 0.5;
+		double anisotropy = (lambda + (1-lambda)*(1 + cos(phi))/2 );
+		if( distance_to_nearest_person(internal_robot_) * anisotropy < 0.8  )
+		//if( distance_to_nearest_person(robot_) * anisotropy < 0.8  )
+		{
+			return approach;
+		}
+	}
+	std::cout << " (2)" << std::endl;
+	person_obj=internal_person_obj_;
+		//if (!find_person(target_person_id_ , &person_obj)  )
+	if (!find_person(target_person_id_ , &person_obj)  ) // no tienes cerca a la persona que acompañas, desconoces la zona.
+	{													// pero luego al poner is_target_person_visible_=true, creo k no usa esto...
+		std::cout << " param_force_approach_[0]="<<param_force_approach_[0] << std::endl;
+		std::cout << " param_force_approach_[1]="<<param_force_approach_[1] << std::endl;
+		std::cout << " param_force_approach_[2]="<<param_force_approach_[2] << std::endl;
+		std::cout << " param_force_approach_[3]="<<param_force_approach_[3] << std::endl;
+		std::cout << " param_force_approach_[4]="<<param_force_approach_[4] << std::endl;
+		std::cout << " (2.1)" << std::endl;
+		person_obj->print();
+		std::cout << " after print (2.1)" << std::endl;
+		is_target_person_visible_ = false;
+		robot_state_ = UNKNOWN_ZONE;
+		f_goal = Sforce();
+		f_persongoal = Sforce();
+		std::cout << " (2.2)" << std::endl;
+		//f_int_pers = force_persons_int( robot_  ) * param_force_approach_[2];
+		f_int_pers = force_persons_int( internal_robot_ ) * param_force_approach_[2];
+		std::cout << " (2.3) f_int_pers:"<< std::endl;
+		f_int_pers.print();
+		//f_int_laser = force_objects_laser_int( robot_ ) * param_force_approach_[3];
+		f_int_laser = force_objects_laser_int( internal_robot_ ) * param_force_approach_[3];
+		std::cout << " (2.4) f_int_laser:" << std::endl;
+		f_int_laser.print();
+		//f_int_map =  get_force_map_robot_position()* param_force_approach_[4];
+		f_int_map =   act_force_map_robot_position_* param_force_approach_[4];
+		std::cout << " f_int_map:" << std::endl;
+		act_force_map_robot_position_.print();
+		std::cout << " (2.5) f_int_map:" << std::endl;
+		f_int_map.print();
+		f = f_int_pers + f_int_map + f_int_laser;
+		std::cout << " (2.6) f:" << std::endl;
+		f.print();
+		return approach;
+	}
+	std::cout << " (3)" << std::endl;
+	is_target_person_visible_ = true;
+	SpointV_cov person = person_obj->get_current_pointV(); // coge el punto de la persona.
+	std::cout << " (4)" << std::endl;
+	//calculate current robot destinations:
+	std::vector<Sdestination> dest;
+	dest.push_back( person_obj->get_best_dest() ); //en las destinaciones del robot se guarda la destinacion de la persona.
+	std::cout << " (5)" << std::endl;
+	//calculation of the person destination
+	person_destination_ = Sdestination(0, person.x ,person.y,1.0);  // destinacion, para acercarse a la persona.
+	//positive difference means on the left hand side of the angle circle thus, positive turn
+	//if( diffangle(person.theta, atan2(robot.y-person.y , robot.x-person.x) ) < 0 )
+	std::cout << " (6) person.x="<<person.x<<"; person.y"<<person.y << std::endl;
+	//a pure geometric approach: makes the destination more stable in time
+	double theta = atan2(person_obj->get_best_dest().y-person.y ,
+						person_obj->get_best_dest().x-person.x); // angulo entre la destinacion a la que va la persona y la posicion de la persona.
+	std::cout << " (7)" << std::endl;
+	if( diffangle(theta, atan2(robot.y-person.y , robot.x-person.x) ) < 0 ) // diferendia entre angulo posición robot a persona y el angulo de persona hacia su goal.
+	{
+		std::cout << " (7.1)" << std::endl;
+		//person_destination_.x += r_*cos(person.theta + ro_);
+		person_destination_.x += r_*cos(theta + ro_); // intenta ir a un radio al rededor de la persona.
+		person_destination_.y += r_*sin(theta + ro_);
+	}
+	else  // es como si hiciera una circunferencia al rededor de la persona. (se intenta poner a un lado o a otro de la persona. segun esos angulos)
+	{
+		std::cout << " (7.2)" << std::endl;
+		person_destination_.x += r_*cos(theta - ro_);
+		person_destination_.y += r_*sin(theta - ro_);
+	}
+	std::cout << " (8)" << std::endl;
+	dest.push_back( person_destination_ );
+	//robot_->set_destinations( dest );
+	internal_robot_->set_destinations(dest);
+	std::cout << " (9)" << std::endl;
+
+	//calculate robot state and if unknown, do not move
+	//set_robot_state(person,robot,  get_sfm_params(robot_)->at(1) );
+	set_robot_state(person,robot,  get_sfm_params(internal_robot_)->at(1) );
+	std::cout << " (10)" << std::endl;
+
+	if ( robot_state_ == UNKNOWN_ZONE)
+		return approach;
+
+	std::cout << " (11)" << std::endl;
+
+	//calculate robot forces
+	if (robot.v() < 0.2 ){ //just to avoid
+		robot.norm_v( v_cruise_);
+	}
+	std::cout << " (12)" << std::endl;
+	//const std::vector<double>* social_forces_param = person_obj->get_social_force_parameters_person_robot();
+	//robot_->set_desired_velocty(v_robot_desired_);
+	internal_robot_->set_desired_velocty(v_robot_desired_);
+	std::cout << " (13)" << std::endl;
+	//f_goal = robot_->force_goal( person_obj->get_best_dest(), get_sfm_params(robot_),  &robot  )* param_force_approach_[0];
+	f_goal = internal_robot_->force_goal( person_obj->get_best_dest(), get_sfm_params(internal_robot_),  &robot  )* param_force_approach_[0];
+	std::cout << " (14)" << std::endl;
+	//f_persongoal = robot_->force_goal( person_destination_ , get_sfm_params(robot_),  &robot  )* param_force_approach_[1];
+	f_persongoal = internal_robot_->force_goal( person_destination_ , get_sfm_params(internal_robot_),  &robot  )* param_force_approach_[1];
+	std::cout << " (15)" << std::endl;
+	//f_persongoal = robot_force_persongoal(robot, person, social_forces_param)* param_force_approach_[1];
+	//f_int_pers = force_persons_int( robot_  ) * param_force_approach_[2];
+	f_int_pers = force_persons_int( internal_robot_  ) * param_force_approach_[2];
+	std::cout << " (16)" << std::endl;
+	//f_int_laser =   force_objects_laser_int( robot_ )*param_force_approach_[3];
+	f_int_laser =   force_objects_laser_int( internal_robot_ )*param_force_approach_[3];
+	std::cout << " (17)" << std::endl;
+	//f_int_map =   get_force_map_robot_position()* param_force_approach_[4];
+	f_int_map =   get_force_map_robot_position()* param_force_approach_[4];
+	std::cout << " (18)" << std::endl;
+	double w = 1.0;
+	do
+	{
+		std::cout << " (18.1)" << std::endl;
+		f_goal = f_goal * w;
+		std::cout << " (18.2)" << std::endl;
+		f_persongoal = f_persongoal * w;
+		std::cout << " (18.3)" << std::endl;
+		f_int_pers = f_int_pers *w;
+		std::cout << " (18.4)" << std::endl;
+		f = f_goal + f_persongoal +	f_int_pers + f_int_map + f_int_laser;
+		std::cout << " (18.5)" << std::endl;
+		approach = robot_tangential_propagation( f );
+		std::cout << " (18.6)" << std::endl;
+		w *= 0.9;
+	} while( read_force_map_success_ && !is_cell_clear_map( approach.x, approach.y ) && w > 0.1 );
+	std::cout << " (19)" << std::endl;
+	//approach = get_robot().pose_propagation( dt_, f );
+	return approach;
+}
+
+bool Cforce_reactive_robot_companion::set_nearest_target_person()
+{
+	//returns True if the target was set and false if no person was in the scene nearer than 20m
+	/*Cperson_abstract* nearest = nearest_person( robot_ );
+	if ( nearest->get_current_pointV().distance( robot_->get_current_pointV() )  < 20.0 && *nearest != *robot_ )
+	{
+		target_person_id_ = nearest->get_id();
+		return true;
+	}
+	else
+	{
+		return false;
+	}*/
+
+	Cperson_abstract* nearest = nearest_person( internal_robot_ );
+		if ( nearest->get_current_pointV().distance(internal_robot_->get_current_pointV() )  < 20.0 && *nearest != *internal_robot_ )
+		{
+			target_person_id_ = nearest->get_id();
+			return true;
+		}
+		else
+		{
+			return false;
+		}
+}
+
+Cperson_abstract* Cforce_reactive_robot_companion::nearest_person( Cperson_abstract* person)
+{
+	Cperson_abstract* nearest_person = person;
+	double d = 2000, d_to_person;
+	for( std::list<Cperson_abstract*>::iterator iit = person_list_.begin() ; iit != person_list_.end() ; iit++ )
+	{
+		d_to_person = (*iit)->get_current_pointV().distance( person->get_current_pointV() );
+		if ( d_to_person < d && person != *iit )
+		{
+			d = d_to_person;
+			nearest_person = *iit;
+		}
+	}
+	return nearest_person;
+}
+
+double Cforce_reactive_robot_companion::distance_to_nearest_person( Cperson_abstract* person )
+{
+	Cperson_abstract* nearest = nearest_person( person );
+	if (nearest == person)
+		return -1.0;
+	return nearest->get_current_pointV().distance( person->get_current_pointV() );
+
+}
+
+
+Sforce Cforce_reactive_robot_companion::robot_force_persongoal(SpointV_cov robot, SpointV_cov person, std::vector<double> social_forces_param )
+{
+	//force calculation
+	/*robot_->set_desired_velocty( v_robot_desired_ );
+	return robot_->force_goal(  person_destination_, get_sfm_params(robot_) );*/
+	internal_robot_->set_desired_velocty( v_robot_desired_ );
+	return internal_robot_->force_goal(  person_destination_, get_sfm_params(internal_robot_) );
+}
+
+void Cforce_reactive_robot_companion::set_robot_state( SpointV_cov person, SpointV_cov robot , double lambda )
+{
+	//calculation of the influence zone
+	double phi = diffangle( person.orientation() , atan2(robot.y-person.y , robot.x-person.x) );
+	double anisotropy = (lambda + (1-lambda)*(1 + cos(phi))/2 );
+
+	if ( anisotropy*1.5 > robot.distance(person)  )
+		robot_state_ = INNER_ZONE;
+	else if ( anisotropy*3 > robot.distance(person) )
+		robot_state_ = MID_ZONE;
+	else
+		robot_state_ = OUT_ZONE;
+
+	switch(robot_state_)
+	{
+		case INNER_ZONE:
+			//v_robot_desired_ = 1.5*person.v;
+			v_robot_desired_ = v_cruise_;
+			break;
+		case MID_ZONE:
+			//v_robot_desired_ = 2 * person.v;
+			v_robot_desired_ = 1.5*v_cruise_;
+			if (v_robot_desired_ < 0.2)//almost stopped
+				v_robot_desired_ = v_cruise_;
+			break;
+		case OUT_ZONE:
+			v_robot_desired_ = v_max_;
+			break;
+		case UNKNOWN_ZONE:
+		default:
+			v_robot_desired_ = 0.0;
+			break;
+	}
+	//robot_->set_desired_velocty( v_robot_desired_ );
+	internal_robot_->set_desired_velocty( v_robot_desired_ );
+
+}
+
+SpointV_cov Cforce_reactive_robot_companion::robot_tangential_propagation(Sforce f)
+{
+	//returns the propagated state given a time horizon of propagation
+	// and assuming no other force is applied, except for the initial one
+	//SpointV_cov robot = robot_->get_current_pointV();
+	SpointV_cov robot = internal_robot_->get_current_pointV();
+	//limits max acceleration
+	//if (f.module() > a_max_)
+	//	f = f * (a_max_ / f.module());
+
+	//limits max velocity
+	double vx = robot.vx + f.fx * dt_;
+	double vy = robot.vy + f.fy * dt_;
+	double v = robot.v();
+	if (v > v_max_)
+	{
+		vx *= v_max_/v;
+		vy *= v_max_/v;
+		v = v_max_;
+	}
+	//check for destination proximity
+	double time_propagated;
+	//Sdestination dest = robot_->get_destinations()->at(1);//person destination
+	Sdestination dest = internal_robot_->get_destinations()->at(1);//person destination
+	double distance = robot.distance( SpointV_cov(dest.x,dest.y) );
+
+	if (distance / v < time_horizon_) //propagated pose would be farther than destination
+	{
+		//time_propagated = 0.1; //NO FUNCIONA!robot remains in place, to avoid minor position changes that entail huge orientation differences...
+		if (v < 0.05 ) v = 0.05;
+		time_propagated = distance / v;
+	}
+	else
+	{
+		time_propagated = time_horizon_;
+	}
+	return SpointV_cov(robot.x + vx * time_propagated , robot.y + vy * time_propagated,
+                                                                robot.time_stamp , vx, vy );
+
+}
+void Cforce_reactive_robot_companion::set_force_params( double force_goal, double force_toperson, double force_interaction)
+{
+	param_force_approach_[0] = force_goal;
+	param_force_approach_[1] = force_toperson;
+	param_force_approach_[2] = force_interaction;
+}
+
+void Cforce_reactive_robot_companion::set_force_params( double force_goal, double force_toperson)
+{
+	param_force_approach_[0] = force_goal;
+	param_force_approach_[1] = force_toperson;
+}
+
+void Cforce_reactive_robot_companion::set_force_params( double force_goal, double force_toperson,
+			double force_interaction, double force_laser, double force_map)
+{
+	param_force_approach_[0] = force_goal;
+	param_force_approach_[1] = force_toperson;
+	param_force_approach_[2] = force_interaction;
+	param_force_approach_[3] = force_laser;
+	param_force_approach_[4] = force_map;
+}
+void Cforce_reactive_robot_companion::robot_parameters_feedback_adjustment( double feedback_sign )
+{
+	//feedback_sing input is a double float of value 1.0 or -1.0
+
+	switch( robot_state_ )
+	{
+		case INNER_ZONE:
+			param_force_approach_[2] -= feedback_sign * 2.0;
+			if( param_force_approach_[2] < 0 )
+				param_force_approach_[2] = 0.0;
+			break;
+
+		case MID_ZONE:
+		case OUT_ZONE:
+		case UNKNOWN_ZONE:
+			param_force_approach_[0] -=  feedback_sign * 0.1;
+			if (param_force_approach_[0] > 1.0)
+				param_force_approach_[0] = 1.0;
+			if (param_force_approach_[0] < 0.0)
+				param_force_approach_[0] = 0.0;
+
+			param_force_approach_[1] += feedback_sign * 0.1;
+			if (param_force_approach_[1] > 1.0)
+				param_force_approach_[1] = 1.0;
+			if (param_force_approach_[1] < 0.0)
+				param_force_approach_[1] = 0.0;
+
+			/*v_max_ += feedback_sign * 0.14;
+			if (v_max_ > 1.6)
+				v_max_ = 1.6;
+			if (v_max_ < 0.6)
+				v_max_ = 0.6;*/
+			break;
+
+		default:
+			//do nothing
+			break;
+	}
+}
+
+
+
diff --git a/local_lib_people_prediction/src/nav/force_reactive_robot_companion.h b/local_lib_people_prediction/src/nav/force_reactive_robot_companion.h
new file mode 100644
index 0000000000000000000000000000000000000000..0fe04430e35bcd689d6f31814ec50b83d1927f27
--- /dev/null
+++ b/local_lib_people_prediction/src/nav/force_reactive_robot_companion.h
@@ -0,0 +1,88 @@
+/*
+ * force_reactive_robot_companion.h
+ *
+ *  Created on: Nov 9, 2013
+ *      Author: gonzalo. Deprecated, do mot used. The new companion by Ely Repiso is anticipative not reactive.
+ */
+
+#ifndef FORCE_REACTIVE_ROBOT_COMPANION_H_
+#define FORCE_REACTIVE_ROBOT_COMPANION_H_
+
+#include "prediction_bhmip.h"
+#include "prediction_behavior.h"
+#include "scene_elements/person_abstract.h"
+#include "scene_abstract.h"
+//#include "nav/plan_local_nav.h"
+/**
+ *
+ * \brief Force reactive Robot companion Class
+ *
+ * The force_reactive_robot_companion class provides an algorithm, making use of
+ * intentionality and motion human prediction in order to control
+ * the most human-like approach of the robot towards a moving person.
+ * The approach algorithm is based on forces calculation and
+ * an input feedback is expected in order to weight the importance
+ * of the interacting forces.
+ *
+ * This work was published in "xxx IROS'2013"
+ *
+ *
+*/
+
+class Cforce_reactive_robot_companion : public Cprediction_bhmip
+{
+	public:
+		Cforce_reactive_robot_companion ();
+		~Cforce_reactive_robot_companion();
+		enum approach_state { UNKNOWN_ZONE=0, INNER_ZONE , MID_ZONE, OUT_ZONE };
+		SpointV_cov robot_approach_pose(bool stop_if_smn_in_inner=false);
+		SpointV_cov robot_approach_pose( Sforce& f_goal, Sforce& f_persongoal,
+				Sforce& f_int_pers, Sforce& f_int_laser, Sforce& f_int_map, Sforce& f, bool stop_if_smn_in_inner=false);
+		//temporal function while no global localization is set (yet...)
+		void set_target_person(int id) { target_person_id_ = id;}
+		bool set_nearest_target_person();
+		int get_target_person() { return target_person_id_; }
+		Cperson_abstract* nearest_person( Cperson_abstract* person);
+		double distance_to_nearest_person(Cperson_abstract* person);
+		approach_state get_robot_state() { return robot_state_; }
+		void set_v_max(double v_max){v_max_ = v_max;}
+		double get_v_max(){return v_max_;}
+		void set_v_cruise(double v){v_cruise_ = v;}
+		void set_time_horizon(double t){time_horizon_ = t;}
+		std::vector<double> get_force_params() const { return param_force_approach_; }
+		void set_force_params( double force_goal, double force_toperson);
+		void set_force_params( double force_goal, double force_toperson, double force_interaction);
+		void set_force_params( double force_goal, double force_toperson,
+					double force_interaction, double force_laser, double force_map);
+		void robot_parameters_feedback_adjustment( double feedback_sign );
+		void set_companion_position(double r, double ro){r_=r; ro_=ro;}
+		bool get_is_target_person_visible () { return is_target_person_visible_;}
+
+		void set_target_person_id(int target_person){target_person_id_=target_person;} // made by (ely) para seleccionar un id de una persona concreta a seguir!
+		void set_robot(Crobot* internal_robot){internal_robot_=internal_robot;}
+		void internal_person_obj(Cperson_abstract* internal_person_obj){internal_person_obj_=internal_person_obj;}
+		void set_get_force_map_robot_position(Sforce act_force_map_robot_position){act_force_map_robot_position_=act_force_map_robot_position;}
+
+	protected:
+		Sforce robot_force_persongoal(SpointV_cov robot, SpointV_cov person, std::vector<double> social_forces_param);
+		int target_person_id_;
+		std::vector<double> param_force_approach_;
+		approach_state robot_state_;//state with respect to its approaching target
+		void set_robot_state(SpointV_cov person, SpointV_cov robot, double lambda);
+		SpointV_cov robot_tangential_propagation( Sforce f );
+
+		double v_max_;//[m/s]
+		double v_cruise_;//[m/s]
+		double time_horizon_;//[s]
+		double v_robot_desired_;//[m/s]
+		double r_,ro_;//relative robot companion position in polar coordinates
+		Sdestination person_destination_;
+		bool is_target_person_visible_;
+		Crobot* internal_robot_;  // for robot companion.
+		Cperson_abstract* internal_person_obj_;
+		Sforce act_force_map_robot_position_;
+
+};
+
+
+#endif /* FORCE_REACTIVE_ROBOT_COMPANION_H_ */
diff --git a/local_lib_people_prediction/src/nav/force_reactive_robot_companion_learning.cpp b/local_lib_people_prediction/src/nav/force_reactive_robot_companion_learning.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..9ccadc21f0bebbaec1508e8d4bc88c51d6135e47
--- /dev/null
+++ b/local_lib_people_prediction/src/nav/force_reactive_robot_companion_learning.cpp
@@ -0,0 +1,74 @@
+/*
+ * force_reactive_robot_companion_learning.cpp
+ *
+ *  Created on: Nov 12, 2013
+ *      Author: gferrer
+ */
+
+
+
+#include "nav/force_reactive_robot_companion_learning.h"
+#include "random/rand_gmm.h"
+#include "math.h"
+
+Cforce_reactive_robot_companion_learning::Cforce_reactive_robot_companion_learning():
+alpha_std_(0.1), beta_std_(0.1), gamma_std_(3.0), delta_std_(0.5), alpha_(0.0),beta_(1.0),cont_(0)
+{
+	previous_params_.push_back( 0.7 );//alpha
+	previous_params_.push_back( 0.3 );//beta
+	previous_params_.push_back( 5.0 );//interaction persons
+	previous_params_.push_back( 0.5 );//obstacles laser
+	previous_params_.push_back( 0.5 );//obstacles maps
+
+	//set random seed for the rng
+	rand_seed();
+
+}
+
+Cforce_reactive_robot_companion_learning::~Cforce_reactive_robot_companion_learning()
+{
+
+}
+
+std::vector<double> Cforce_reactive_robot_companion_learning::learning_force_companion_params_MC( )
+{
+	//propose new robot companion parameters using MC
+	//settings are ready for new experiments, but has to be activated somewhere
+
+	//5- set next param_previous
+	std::vector<double> params = param_force_approach_;//parameters returning the below work
+
+
+	//4- sample params depending on previous params, only gamma and delta are set
+	double gamma, delta;
+	//while ( alpha <= 0.0 || alpha > 2.0 )
+	//	alpha = rand_normal( previous_params_[0] ,alpha_std_);
+	while ( gamma <= 1.0 || gamma > 15.0 )
+		gamma = rand_normal( previous_params_[2] ,4*gamma_std_);
+	while ( delta <= 0.1 || delta > 1.0 )
+		delta = rand_normal( previous_params_[4] ,4*delta_std_);
+	//param_force_approach_[0] = alpha;
+	//param_force_approach_[1] = beta;
+	param_force_approach_[2] = gamma;
+	param_force_approach_[4] = delta;
+
+	return params;
+}
+
+std::vector<double> Cforce_reactive_robot_companion_learning::learning_force_companion_params_alphabeta( )
+{
+	std::vector<double> params = param_force_approach_;
+
+	cont_++;
+	if ( cont_ > 10 )//5 times per parameters value
+	{
+		cont_ = 0;
+		alpha_ += 0.02;
+		if ( alpha_ > 1.0 ) alpha_ = 1.0;
+		beta_ = 1 - alpha_;
+	}
+	params[0] = alpha_; //alpha
+	params[1] = beta_; //beta
+	param_force_approach_ = params;
+	return params;
+}
diff --git a/local_lib_people_prediction/src/nav/force_reactive_robot_companion_learning.h b/local_lib_people_prediction/src/nav/force_reactive_robot_companion_learning.h
new file mode 100644
index 0000000000000000000000000000000000000000..7e4c676a9af64afb5f879b6eb6119283357dd27b
--- /dev/null
+++ b/local_lib_people_prediction/src/nav/force_reactive_robot_companion_learning.h
@@ -0,0 +1,31 @@
+/*
+ * force_reactive_robot_companion_learning.h
+ *
+ *  Created on: Nov 12, 2013
+ *      Author: gferrer
+ */
+
+#ifndef FORCE_REACTIVE_ROBOT_COMPANION_LEARNING_H_
+#define FORCE_REACTIVE_ROBOT_COMPANION_LEARNING_H_
+
+
+#include "nav/force_reactive_robot_companion.h"
+
+class Cforce_reactive_robot_companion_learning : public Cforce_reactive_robot_companion
+{
+  public:
+	Cforce_reactive_robot_companion_learning();
+	~Cforce_reactive_robot_companion_learning();
+	std::vector<double> learning_force_companion_params_MC( );
+	std::vector<double> learning_force_companion_params_alphabeta( );
+
+
+  protected:
+	double alpha_std_, beta_std_,gamma_std_, delta_std_;
+	std::vector<double> previous_params_;
+	double alpha_,beta_;
+	long int cont_;
+};
+
+
+#endif /* FORCE_REACTIVE_ROBOT_COMPANION_LEARNING_H_ */
diff --git a/local_lib_people_prediction/src/nav/plan_local_nav.cpp b/local_lib_people_prediction/src/nav/plan_local_nav.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b889e3cd7bdfee3fad0dad1e163fad3945c12b08
--- /dev/null
+++ b/local_lib_people_prediction/src/nav/plan_local_nav.cpp
@@ -0,0 +1,9368 @@
+/*
+ * plan_local_nav.cpp
+ *
+ *  Created on: Dec 22, 2013. Last Modified on 2020
+ *      Author: Initial code of the AKP Gonzalo Ferrer.
+ *      Author: Addition of ASSAOP and ASSAGP for people accompaniment code of Ely Repiso.
+ */
+
+#include "nav/plan_local_nav.h"
+#include <algorithm>
+#include <iostream>
+#include <time.h>
+#include <math.h>
+
+
+Cplan_local_nav::Cplan_local_nav(double horizon_time, unsigned int max_iter,
+		plan_mode mode, bool robot_or_person) :
+Cprediction_behavior(horizon_time,false, Cperson_abstract::Low_pass_linear_regression_filtering), // Cambiado, ya que aproxima mejor la velocidad de las personas.
+plan_mode_(mode), distance_mode_(Cplan_local_nav::Euclidean), global_mode_(Cplan_local_nav::Scalarization),
+max_iter_(max_iter), workspace_radii_(0.0), std_goal_workspace_(1.0), max_v_by_system_(0.8), cost_angular_(0.25), // max_v_by_system_(0.8)
+xy_2_goal_tolerance_(0.09), v_goal_tolerance_(0.1), distance_to_stop_(2.0),
+alpha_(1.0), gamma_(1.0), delta_(1.0),
+last_pose_command_(Spose()),
+std_cost_int_forces_(1.0),
+std_cost_robot_(1.0),
+std_cost_obstacles_(1.0),
+std_cost_past_traj_(1.0),
+std_cost_distance_(1.0),
+std_cost_orientation_(1.0),
+gaussian_constant_( 2.0/(2.0*PI) ),
+ppl_collision_mode_(0),
+pr_force_mode_(0),
+debug_companion_(false),
+calc_goal_companion_with_group_path_(true),
+min_next_companion_angle_(90), // va a 90 para side-by-side;
+min_next_companion_cost_(0),
+before_initial_angle_(90), // va a 90 para side-by-side;
+before_initial_cost_(0),
+id_person_companion_(1), // a definir desde fuera luego...
+change_id_person_companion_(1),
+robot_person_proximity_distance_(1.5), // ANTES 1.5; ANTES 2 antes 1.5 (pruevas con valor 3 iban bien, pero choca con obstaculos)
+robot_person_proximity_tolerance_(0.25), //  0.25, para evitar oscilaciones por posición muy exacta.  ANTES 0.75 antes 0.5 (pruevas con valor 3 iban bien, pero choca con obstaculos)
+robot_person_proximity_goals_x_(1.5), // ANTES x=(1.5)/2 pero no iba bien.
+robot_person_proximity_goals_y_(1.5), // ANTES y=(1.5)/2
+debug_antes_subgoals_entre_AKP_goals_(false),
+actual_debug_(false),
+actual_debug2_(false),
+offset_atractive_(1.5),  // antes era => offset_atractive_=1.
+time_act_(0),
+robot_adition_complete_esphere_companion_distance_(1.0),//( [antes_ valor BUENO a 6.5] [antes_ valor a 3.5] antes 2.25; 2=1+1.0 de seguridad por cada lado)faltaria un metro de distancia más para los 0.5 y 0.5 más de persona y robot para no chocar.
+f_obst_max_x_(0.5),
+f_obst_max_y_(0.5),
+companion_same_person_goal_(true), // if true, infiere para el robot el mismo goal de la persona. el goal es la prediccion de hacia donde va la persona.
+angle_companion_(90), // by default, the angle companion is 90 degrees;
+person_goal_percent_(0.5),
+marge_angle_companion_(15),
+vel_per_max_caso_robot_a_0_o_180_grados_(0),
+debug_real_test_companion_(false),
+debug_real_test_companion2_(false),
+debug_real_test_companion3_(false),
+debug_real_test_companion4_(false),
+marge_in_distance_(0.3),
+overpas_obstacles_behind_person_(true), // true== the robot pass obstacles behind person => is changed on dynamic reconfigure!
+we_have_cost_companion_(false),
+anisotropy_threshold_(0.2), /// anisotropy_threshold_=0.5 SIMULACION, real=> anisotropy_threshold_=0.2
+check_execution_times_(false),
+little_augmented_collision_margin_(0.75), //(antes era: 0.75. pero parece que con el margen ajustado bien, real de 0.6 también vale. Probar un poco más exhaustivo luego.) SIMULACION: sim= 1.5 Final: 1.5/2=0.75=> +0.25 de robot_person_proximity_tolerance_ +0.5 de colchon entre obstaculos que ayude a poder maniobrar y no chocar.     Antes: +1, para tener en cuenta colision con radio de obstaculos por cada lado. +0.25 de colchon más por cada lado de la circunferencia para no rozar.
+person_radi_(0.4), // para_real_companion=0.5       simulacion: person_radi_amp_=0.5 ; real=0.4 ;
+person_radi2_(0.3), //  para_real_companion=0.5   simulacion: person_radi_amp_=0.4 ; real=0.3 ;
+person_radi_amp_(0.4), // para_real_companion=0.5   simulacion: person_radi_amp_=0.6 ; real=0.4 ;
+see_forces_(false),
+obstacle_radi_(0.31), // en simulacion este y el siguiente estan a 0.3 // en real es 0.0
+obstacle_radi2_(0.31), // en simulacion este y el siguiente estan a 0.3 // en real es 0.0
+obstacle_radi_amp_(0.31),  // en simulacion sim=0.4, en real= 0.0 OJO! se cambia en el cfg del nodo!!!
+u_forces_robot_actual_(Sedge_tree(1)),
+ideal_distance_person_robot_(1.5),
+ideal_angle_person_robot_(90),
+before_ideal_angle_person_robot_(90),
+sim(false), // simulacion=> sim=true. REAL => sim=false
+sim_target_per(false), // =true only simulation. False is for real experiments.  // simulacion=> sim_target_per=true. REAL => sim_target_per=false
+iteration_(1),
+experiment_(1),
+inc_distance_ant_(0),
+acum_time_(0),
+time_ant_(0),
+save_results_in_file_(false),
+meters_goal_to_save_results_in_file_(0.1), // 5 m for simulation. In real is better put 0 m to save every thing and then cut the files when we want.
+reduce_max_vel_dist_(0.5), // antes era 1! ojo! 0.5 ahora.
+debug_file_robot_(false),
+externa_force_k_near_goal_akp_(2.3),
+externa_force_k_far_goal_akp_(2.3),
+max_dist_to_near_goal_force_akp_(3.5),
+mode_step_near_(false),
+out_index_step_final_dest_goal_(10),
+out_index_step_companion_goal_(5),
+only_comp_people_vel_and_robot_poses_(false),
+debug_angles_(false),
+debug_person_companion_general_(false),
+debug_person_companion_increment_angle_(false),
+debug_init_robot_plan2_person_companion_(false),
+debug_robot_plan_anticipative_krrt_companion_person_companion_(false),
+debug_calculate_edge_person_companion_akp_(false),
+debug_correct_angle_person_init_robot2_(false),
+debug_correct_angle_person_replan_min_cost_branch_(false),
+debug_correct_angle_person_replan_last_step_(false),
+debug_correct_angle_person_calculate_edge_(false),
+debug_correct_angle_person_get_best_planned_pose_(false),
+debug_correct_angle_person_vel_robot_companion_(false),
+debug_correct_angle_person_calculate_actual_angle_person_robot_(false),
+debug_correct_angle_person_print_to_matlab_(false),
+bool_case_person_companion_(false), // por defecto false para el robot
+num_steps_orientation_(15),
+alpha_companion_(0.5), // antes era 0.1. (modelo companion side-by-side= 0.2;)
+beta_companion_(0.5), // antes era 0.6. (modelo companion side-by-side= 0.8;  )
+gamma_companion_(1.0), // antes era 5!!! pero es demasiado reactivo con las personas en el companion!
+delta_companion_(0.5),
+threshold_dintace_select_person_side_to_go_(robot_person_proximity_distance_/2), // al principio esta distancia minimo ha de ser 0.5 metros al lado de la persona.
+debug_select_person_side_to_go_with_more_free_space_(false),
+chose_better_side_to_acompani_person_(false), // es true, para que se posicione bien.
+iter_act_multiple_paths_and_best_path_(),
+planner_iterations_(0),
+debug_file_evaluate_costs_(false),
+iter_d_(0),
+next_companion_angle_save_(120),
+before_act_companion_angle_(120),
+calculate_complete_group_path_var_(person_prediction),
+first_(true),
+Action_(Cplan_local_nav::ITER),
+global_iter_to_change_simulation_case_(1),
+first_in_itter_(true),
+first_time_case1_(true),
+first_time_case2_(true),
+go_behid_comp_person_(true),
+restart_real_(false),
+save_results_on_files_for_person_companion_(false),
+save_results_on_files_for_robot_(false),
+save_in_file_(false),
+multiply_person_companion_force_to_persons_(7),
+threshold_min_vel_person_to_obtain_destination_(0.2),
+firts_iter_obtain_angle_person_companion_(true),
+debug_gazebo_journal_(false),
+debug_gazebo_journal2_(false),
+clocks_per_sec_my_var_(1000000),
+final_goal_reached_in_node_(false),
+change_final_robot_orientation_(false),
+treshold_distance_between_steps_(0.3), //Threshold distance between steps to adapts to person velocity for the side-by-side (if is to small, the robot do s behaviour if is to large the robot stays a little behind of the side-by-side, be carefull with this value. normaly is 0.2m or 0.3m.
+change_k_robot_(true),
+change_k_person_(false),
+change_k_person_comp_(false),
+change_k_obst_(false),
+initial_goal_case_bool_(true),
+initial_not_set_last_people_dest_(false),
+minimun_velocity_case_stop_initial_(0.15),
+final_max_v_(0.9),
+bool_out_init_pose_robot_(false),
+//metros_al_dynamic_goal_Vform_(3.0),
+metros_reduce_vel_(2),
+no_save_cost_and_dist_(false),
+before_vp1_module_(0.0),
+debug_sideBySide2019_(false),
+distance_frenar_sideBySide_when_person_is_stop_(0.5), //TODO: este parametro puede ayudar a que no gire cuando para. Pero ojo, también hara que se acerque menos al lateral cuando la persona este parada.
+threshold_collision_dist_stop_pers_goal_(0.4), //TODO: este parametro puede ayudar a que no gire cuando para. Pero ojo, también hara que se acerque menos al lateral cuando la persona este parada.
+litlle_augment_vel_desired_adapted_by_ve_per_for_robot_(0.2), // Todo: encuanto al frenado suave, habria que incluir que frene brusco si esta a menos de X de un obstaculo. Aunque de eso ya se encarga el nodo segundo en el launch: save_comd_vel
+dist_to_approach_static_people_(0.15),  //0.15 si no quieres que actue. (antes era 1m) . TODO mirar si puedo hacer que al principio se mueva y/o que no retroceda al goal si esta muy cerca.
+see_save_in_file_(false),
+output_screen_messages_(false)
+{
+
+	if(overpas_obstacles_behind_person_){ // hay que compensar el que el robot va por detras. (sería mejor usar la distancia real para la colision, pero en el caso de ir delante no va bien, pq entonces es distancia fija!)
+		robot_adition_complete_esphere_companion_distance_= 1.0;// era 2.5 en los experimentos iniciales. Luego se redujo a 1 añadiendo otras variables para mejorar todo.
+	}
+	Action_=Cplan_local_nav::ITER;
+    edge_.reserve((size_t)max_iter_);
+    cost_int_forces_.reserve((size_t)max_iter_);
+    cost_robot_.reserve((size_t)max_iter_);
+    cost_obstacles_.reserve((size_t)max_iter_);
+    cost_distance_.reserve((size_t)max_iter_);
+    cost_orientation_.reserve((size_t)max_iter_);
+    cost_past_traj_.reserve((size_t)max_iter_);
+    nodes_in_branch_.reserve((size_t)max_iter_);
+	random_goals_.reserve( (size_t)max_iter_ );
+
+	cost_companion_.reserve( (size_t)max_iter_ ); // companion variables (ely)
+	orientation_person_robot_angles_.reserve( (size_t)max_iter_ );
+    final_min_collision_distance_vector_.reserve( (size_t)max_iter_ );
+    real_robot_person_distance2_vector_.reserve( (size_t)max_iter_ );
+	parent_index_vector_.reserve( (size_t)max_iter_ );
+	min_distance_collision_vector_.reserve( (size_t)max_iter_ );
+
+    //Initialization of robot: there is always a robot if planning
+	robot_in_the_scene_ = true;
+	person_companion_in_the_scene_ = true;
+	robot_ = new Crobot(0,Crobot::Differential,scene_force_type_);
+	person_companion_ = new Crobot(1,Crobot::Differential,scene_force_type_);
+	person_companion_->reserve_planning_trajectory(max_iter_);
+
+    //random generator
+    generator_.seed();
+
+    //planner cost paramters
+    cost_parameters_.reserve(7);
+    cost_parameters_.push_back(1.0);// [0] Goal cost
+    cost_parameters_.push_back(1.0);// [1] orientation cost
+    cost_parameters_.push_back(1.0);// [2] Robot cost
+    cost_parameters_.push_back(1.0);// [3]Interacting people cost
+    cost_parameters_.push_back(0.25);// [4] potential time
+    cost_parameters_.push_back(1.0);// [5] obstacles cost
+    cost_parameters_.push_back(1.0);// [6] past trajectory function cost
+    cost_parameters_.push_back(1.0);// [7] local minima scape cost
+
+    // all ppl time
+    filtering_time_window_ =  horizon_time;
+
+    if(debug_real_test_companion_){
+    std::cout << "angle_companion_ ="<<angle_companion_<< std::endl;
+    }
+
+    if(robot_or_person==false){
+    	std::cout << " (INI robot_planner)  max_iter_ ="<<max_iter_<<"; horizon_time="<<horizon_time_<< std::endl;
+    }else{
+    	std::cout << " (INI akp_companion_person planner)  max_iter_ ="<<max_iter_<<"; horizon_time="<<horizon_time_<< std::endl;
+    }
+
+    std::string data_file;
+	std::string data_file2="/home/ely7787/iri-lab/labrobotica/restricted/algorithms/people_prediction/branches/ely_people_prediction_companion_robot/2_results_evaluate_costs/results_evaluate_cost_robot_case2.txt";
+	evaluate_costs_file_=data_file2;
+	std::string data_file3="/home/ely7787/iri-lab/labrobotica/restricted/algorithms/people_prediction/branches/ely_people_prediction_companion_robot/2_results_evaluate_costs/results_evaluate_distance_and_angle_robot_case2.txt";
+	evaluate_change_distance_and_angle_companion_file_=data_file3;
+
+	// To save results in file matlab   (one for each iterative case)
+	new_matlab_file();
+	//new_matlab_file_To_evaluate_change_distance_and_angle(); // generar archivo y ya en esa iter ira bien!
+	//new_matlab_file_To_evaluate_costs();
+
+	data_file2="/home/ely7787/iri-lab/labrobotica/restricted/algorithms/people_prediction/branches/ely_people_prediction_companion_robot/2_results_evaluate_costs/results_evaluate_cost_robot_case1.txt";
+	evaluate_costs_file_=data_file2;
+	data_file3="/home/ely7787/iri-lab/labrobotica/restricted/algorithms/people_prediction/branches/ely_people_prediction_companion_robot/2_results_evaluate_costs/results_evaluate_distance_and_angle_robot_case1.txt";
+	evaluate_change_distance_and_angle_companion_file_=data_file3;
+
+	// To save results in file matlab
+	new_matlab_file();
+	//new_matlab_file_To_evaluate_change_distance_and_angle(); // generar archivo y ya en esa iter ira bien!
+	//new_matlab_file_To_evaluate_costs();
+
+	data_file2="/home/ely7787/iri-lab/labrobotica/restricted/algorithms/people_prediction/branches/ely_people_prediction_companion_robot/2_results_evaluate_costs/results_evaluate_cost_robot_case0.txt";
+	evaluate_costs_file_=data_file2;
+	data_file3="/home/ely7787/iri-lab/labrobotica/restricted/algorithms/people_prediction/branches/ely_people_prediction_companion_robot/2_results_evaluate_costs/results_evaluate_distance_and_angle_robot_case0.txt";
+	evaluate_change_distance_and_angle_companion_file_=data_file3;
+
+	// To save results in file matlab
+	new_matlab_file();
+	//new_matlab_file_To_evaluate_change_distance_and_angle(); // generar archivo y ya en esa iter ira bien!
+	//new_matlab_file_To_evaluate_costs();
+
+	// poses for the iterative simulation:
+	//SIM_initial_robot_pose2_=Spose(-7.0,-0.83,0.0,0.0,0.0,0.0);  //(julio2019) FINAL Initial POSE robot (case central!) antes -1.25
+	SIM_initial_robot_pose2_=Spose(-7.0,3.0,0.0,0.0,0.0,0.0); //(julio2019) FINAL Initial POSE robot (case lateral!)!
+	//SIM_initial_robot_pose2_=Spose(1.0,-0.83,0.0,0.0,0.0,0.0);  // antes -1.25
+	SIM_initial_robot_pose1_=Spose(-8.5,-1.25,0.0,0.0,0.0,0.0); //USO ESTA  //Spose( double x_ , double y_ , double time_stamp_ , double theta_ , double v_, double w_) -4.5,-1.25
+	//SIM_initial_robot_pose3_=Spose(16.5,10.5,0.0,0.0,0.0,0.0);
+	//SIM_initial_robot_pose4_=Spose(16.5,-15.5,0.0,0.0,0.0,0.0);
+	SIM_initial_robot_pose4_=Spose(13.5,7.5,0.0,0.0,0.0,0.0);
+	SIM_initial_robot_pose3_=Spose(13.5,-8.5,0.0,0.0,0.0,0.0);  // ES YA EL PARA RESTART!
+	SIM_initial_person_companion_pose1_=Spose(-7.0,0.5,0.0,0.0,0.0,0.0); //USO ESTA!
+	SIM_initial_person_companion_pose2_=Spose(-7.0,0.5,0.0,0.0,0.0,0.0);
+	//SIM_initial_person_companion_pose2_=Spose(1.0,0.5,0.0,0.0,0.0,0.0);
+	//8, -2.0, 0, 0.5, 1, 9 (person companion goals.)
+	//9, 27.0, 0, 0.5, 1, 8
+	SIM_initial_person_goal_pose1_=Spoint(40.0, 0.5,0.0);
+	//SIM_initial_person_goal_pose1_=Spoint(27.0, 20.5,0.0); // SIM_initial_person_goal_pose1_=Spoint(30.0, 10.5,0.0);
+	//SIM_initial_person_goal_pose1_=Spoint(20.0, 17.5,0.0); // Spoint( double x_ , double y_ , double time_stamp_) :
+	// 0, 17.0, 10.5, 0.5, 1, 1   DESTS pose1 (INITIAL)
+	// 1, 1.0, -10.5, 0.5, 1, 0
+	//Sdestination_person_goal_pose1_=Sdestination(1,1.0,-20.5);
+	//Sdestination_person_goal_pose1_=Sdestination(1,20.0,-17.5);
+	SIM_initial_person_goal_pose2_=Spoint(40.0, -20.5, 0.0);
+
+	//SIM_initial_person_goal_pose2_=Spoint(27.0, -17.5, 0.0); // SIM_initial_person_goal_pose2_=Spoint(30.0, -10.5, 0.0);
+	//SIM_initial_person_goal_pose2_=Spoint(20.0, -17.5, 0.0);
+	// 0, 17.0, -10.5, 0.5, 1, 1   DESTS pose2 (INITIAL)
+	// 1, 1.0, +10.5, 0.5, 1, 0
+	//Sdestination_person_goal_pose2_=Sdestination(1,1.0,20.5);
+	//Sdestination_person_goal_pose2_=Sdestination(1,20.0, 17.5);
+	SIM_initial_person_goal_pose3_=Spoint(40,20.0,0.0);
+	//SIM_initial_person_goal_pose3_=Spoint(27,0.0,0.0); // SIM_initial_person_goal_pose3_=Spoint(30,0.0,0.0);
+	//Sdestination_person_goal_pose3_=Sdestination(1,1.0,0.0);
+	//SIM_initial_person_goal_pose4_=Spoint(20,0.0,0.0);
+	//SIM_initial_person_goal_pose5_=Spoint(30,0.0,0.0);
+	//8, -2.0, 0, 0.5, 1, 9 (person companion goals.== persona goal en la misma direccion que ellos en frente)
+	//9, 27.0, 0, 0.5, 1, 8
+
+}
+
+Cplan_local_nav::~Cplan_local_nav()
+{
+	//all memory allocations are freed in Cprediction_behavior/bhmip
+}
+
+
+bool Cplan_local_nav::robot_plan_companion3(Spose& pose_command, Cperson_abstract::companion_reactive& reactive,double dt)
+{
+	//std::cout <<" In Local Lib ASSAOP robot_plan_companion3"<< std::endl;
+	//(0.04), (0.05), (1.0)   En codigo: 0.09, 0.1  y 2
+	//std::cout <<" companion_same_person_goal_"<<companion_same_person_goal_<< std::endl;
+	//std::cout <<" xy_2_goal_tolerance_="<<xy_2_goal_tolerance_<<"; v_goal_tolerance_="<<v_goal_tolerance_<<"; distance_to_stop_="<<distance_to_stop_<< std::endl;
+
+	if(output_screen_messages_){
+		std::cout <<" In Local Lib ASSAOP robot_plan_companion3"<< std::endl;
+
+		std::cout <<" min_next_companion_angle_="<< min_next_companion_angle_ <<"; before_initial_angle_=" << before_initial_angle_<< "; angle_companion_="<<angle_companion_<< std::endl;
+
+		std::cout <<" robot_person_proximity_distance_="<<robot_person_proximity_distance_<<"; robot_radi="<<robot_->get_platform_radii()<<"; little_augmented_collision_margin_="<<little_augmented_collision_margin_<< std::endl;
+		std::cout <<" min_distance_collision_=="<<(robot_person_proximity_distance_+2*robot_->get_platform_radii()+little_augmented_collision_margin_)/2<< std::endl;
+
+		std::cout <<" In robot_->get_v_max()="<<robot_->get_v_max() << std::endl;
+	}
+
+	robot_->set_desired_velocty(max_v_by_system_); // reset velocity for the approaching cases.
+
+	//Spose robot_act=pose_command;
+	robot_saved_to_time_v2_=pose_command;
+    clock_t robot_plan_companion3_start = clock();
+
+	std::string data_file;
+    change_set_id_person_companion();
+
+  //  std::cout <<" 1 In Local Lib ASSAOP robot_plan_companion3"<< std::endl;
+	Cperson_abstract* person_obj_companion_person;
+	we_have_pointer_to_first_person_=find_person(id_person_companion_ , &person_obj_companion_person);
+
+	//Cperson_abstract* person_obj3;
+	//bool find_comp_pers= find_person(id_person_companion_ , &person_obj3);
+
+	//std::cout <<"2  In Local Lib ASSAOP robot_plan_companion3"<< std::endl;
+	if(we_have_pointer_to_first_person_){
+		if(person_obj_companion_person->get_best_dest().id!=before_companion_person_destination_.id){
+			person_obj_companion_person->reset_before_destination_prob();
+		}
+		before_companion_person_destination_=person_obj_companion_person->get_best_dest();
+	}
+	//std::cout <<"3  In Local Lib ASSAOP robot_plan_companion3"<< std::endl;
+	is_case_akp_true_=false;
+	enter_on_cero_=false;
+	enter_on_cero2_=false;
+
+	if(debug_gazebo_journal2_){
+	 std::cout <<"IN robot_plan_companion3; angle_companion_="<<angle_companion_<< std::endl;
+	}
+
+
+
+
+   // std::cout <<"4 In Local Lib ASSAOP robot_plan_companion3"<< std::endl;
+	pointer_to_person_companion_=person_obj_companion_person;
+	//std::cout <<"4.1 In Local Lib ASSAOP robot_plan_companion3; we_have_pointer_to_first_person_="<<we_have_pointer_to_first_person_<<"; id_person_companion_="<<id_person_companion_<< std::endl;
+
+	 //  std::cout <<"4.2 In Local Lib ASSAOP robot_plan_companion3"<< std::endl;
+	if(we_have_pointer_to_first_person_){
+		initial_person_companion_point_=person_obj_companion_person->get_current_pointV();
+		//   std::cout <<"4.3 In Local Lib ASSAOP robot_plan_companion3"<< std::endl;
+		actual_person_Companion_pointer_=person_obj_companion_person;
+	}
+	//std::cout <<"5  In Local Lib ASSAOP robot_plan_companion3"<< std::endl;
+	if(output_screen_messages_){
+	std::cout <<" IMPORTANT!!!  before_initial_angle_="<<before_initial_angle_<<"; angle_companion_="<<angle_companion_<< std::endl;
+	}
+
+	// kept the last person_goal.
+   	Spoint actual_dest1=Spoint(pointer_to_person_companion_->get_best_dest().x,pointer_to_person_companion_->get_best_dest().y,pointer_to_person_companion_->get_best_dest().time_stamp);
+    if(pointer_to_person_companion_->get_current_pointV().distance(actual_dest1)<2){
+
+    }else{ //first time.
+       last_people_dest_=pointer_to_person_companion_->get_best_dest();
+       las_people_dest_spoint_=Spoint(last_people_dest_.x,last_people_dest_.y,last_people_dest_.time_stamp);
+       initial_not_set_last_people_dest_=true;
+    }
+  //  std::cout <<" 6 In Local Lib ASSAOP robot_plan_companion3"<< std::endl;
+	// INI Only One Person side-by-side accompaniment.
+    mean_people_Zalungo_=robot_->get_current_pointV();
+    SpointV robot_now=robot_->get_current_pointV();
+	Sdestination destperson=pointer_to_person_companion_->get_best_dest();
+	std::vector<SpointV> people_act;
+	people_act.push_back(initial_person_companion_point_);
+	center_of_the_group_Zanlungo_=calculate_centre_of_the_group(robot_now,people_act);
+	//std::cout <<"7 In Local Lib ASSAOP robot_plan_companion3"<< std::endl;
+	double x_center_group_people=(initial_person_companion_point_.x)/number_of_group_people_;
+	double y_center_group_people=(initial_person_companion_point_.y)/number_of_group_people_;
+	double vx_center_group_people=(initial_person_companion_point_.vx)/number_of_group_people_;
+	double vy_center_group_people=(initial_person_companion_point_.vy)/number_of_group_people_;
+
+	mean_people_Zalungo_=SpointV(x_center_group_people, y_center_group_people, initial_person_companion_point_.time_stamp, vx_center_group_people, vy_center_group_people );
+	if(initial_person_companion_point_.v()>0.2){
+		theta_of_the_group_Zanlungo_=atan(vy_center_group_people/vx_center_group_people);
+	}else{
+		theta_of_the_group_Zanlungo_=atan((destperson.y-y_center_group_people)/(destperson.x-x_center_group_people));
+	}
+	//std::cout <<"8 In Local Lib ASSAOP robot_plan_companion3"<< std::endl;
+	if(theta_of_the_group_Zanlungo_<0){
+		theta_of_the_group_Zanlungo_=2*3.14+theta_of_the_group_Zanlungo_;
+	}
+		// FIN Only One Person side-by-side accompaniment.
+
+	// ini Only One Person Side-by-Side Accompaniment
+	 if(we_have_pointer_to_first_person_){
+		if(initial_person_companion_point_.v()>minimun_velocity_case_stop_initial_){ // si una de las dos anda, cambia el goal!
+			initial_goal_case_bool_=false;
+		}
+	}else if(!we_have_pointer_to_first_person_){
+		initial_goal_case_bool_=true;
+	}
+
+	// fin Only One Person Side-by-Side Accompaniment.
+	// Fin calculating the center of the group
+	// std::cout <<" 9 In Local Lib ASSAOP robot_plan_companion3"<< std::endl;
+	clock_t robot_plan_companion3_end;
+	robot_plan_companion3_start_ITER = clock();
+
+	unsigned int clock_start4;
+	unsigned int clock_end4;
+
+	robot_plan_companion3_end_ITER = clock();
+	clock_start4= (unsigned int) robot_plan_companion3_start_ITER;
+	clock_end4= (unsigned int) robot_plan_companion3_end_ITER;
+
+	for( auto iit: person_list_ )
+	{
+		if(iit->get_id()==id_person_companion_){
+			actual_person_Companion_SpointV_=iit->get_current_pointV();
+			actual_person_Companion_destination_=iit->get_best_dest();
+			max_desired_person_Companion_velocity_=iit->get_desired_velocity();
+			actual_person_Companion_pointer_=iit;
+		}
+	}
+
+	if(check_execution_times_){
+		robot_plan_companion3_end_ITER = clock();
+		clock_start4= (unsigned int) robot_plan_companion3_start_ITER;
+		clock_end4= (unsigned int) robot_plan_companion3_end_ITER;
+		std::cout<< "IIIIIIIIIIIIII CLOCKS_PER_SEC=1000000=" <<CLOCKS_PER_SEC<<"; robot_plan_companio3_end_ITER="<<robot_plan_companion3_end_ITER<<"; robot_plan_companion3_start_ITER="<<robot_plan_companion3_start_ITER<< std::endl;
+		std::cout << "(after find comp pers on pers list) time_tot=robot_plan_companion3_start_ITER-robot_plan_companion3_end_ITER="<<(robot_plan_companion3_end_ITER-robot_plan_companion3_start_ITER)<< std::endl;
+		std::cout << "(after find comp pers on pers list) time_tot=robot_plan_companion3_start_ITER-robot_plan_companio3_end_ITER="<<((robot_plan_companion3_end_ITER-robot_plan_companion3_start_ITER)/CLOCKS_PER_SEC)<< std::endl;
+		std::cout << "(after find comp pers on pers list) time_tot=robot_plan_companion3_start_ITER-robot_plan_companion3_end_ITER="<<(clock_end4-clock_start4)/1000000<< std::endl;
+		std::cout << "(after find comp pers on pers list) time_tot=robot_plan_companion3_start_ITER-robot_plan_companion3_end_ITER="<<(robot_plan_companion3_end_ITER-robot_plan_companion3_start_ITER)/1000000<< std::endl;
+	}
+
+	radii_person_list_include_person_companion_=robot_->get_current_pointV().distance(actual_person_Companion_SpointV_)+1;
+	//std::cout <<" 10 In Local Lib ASSAOP robot_plan_companion3"<< std::endl;
+	bool act_result;
+	unsigned int clock_start3;
+	unsigned int clock_end3;
+	if(check_execution_times_){
+		robot_plan_companion3_end_ITER = clock();
+		clock_start3= (unsigned int) robot_plan_companion3_start_ITER;
+		clock_end3= (unsigned int) robot_plan_companion3_end_ITER;
+		std::cout<< "IIIIIIIIIIIIII CLOCKS_PER_SEC=1000000=" <<CLOCKS_PER_SEC<<"; robot_plan_companio3_end_ITER="<<robot_plan_companion3_end_ITER<<"; robot_plan_companion3_start_ITER="<<robot_plan_companion3_start_ITER<< std::endl;
+		std::cout << "(before initialice files) time_tot=robot_plan_companion3_start_ITER-robot_plan_companion3_end_ITER="<<(robot_plan_companion3_end_ITER-robot_plan_companion3_start_ITER)<< std::endl;
+		std::cout << "(before initialice files) time_tot=robot_plan_companion3_start_ITER-robot_plan_companio3_end_ITER="<<((robot_plan_companion3_end_ITER-robot_plan_companion3_start_ITER)/CLOCKS_PER_SEC)<< std::endl;
+		std::cout << "(before initialice files) time_tot=robot_plan_companion3_start_ITER-robot_plan_companion3_end_ITER="<<(clock_end3-clock_start3)/1000000<< std::endl;
+		std::cout << "(before initialice files) time_tot=robot_plan_companion3_start_ITER-robot_plan_companion3_end_ITER="<<(robot_plan_companion3_end_ITER-robot_plan_companion3_start_ITER)/1000000<< std::endl;
+	}
+	//std::cout <<"11  In Local Lib ASSAOP robot_plan_companion3"<< std::endl;
+	// INI Starting points of people! when only have companion task.
+	if(sim_target_per){
+
+		if(see_save_in_file_){
+			std::cout <<"ENTRO EN SIM, se cambia nombre FILE segun el caso de simulacion, el fichero del reconfigure."<< std::endl;
+		}
+
+		if(global_iter_to_change_simulation_case_<=100){ // 0-> 100
+			// change results file. Case 0
+			if(debug_sideBySide2019_){
+				std::cout << "PPPP CASE 0; SIM_initial_person_goal_pose1_.x"  << SIM_initial_person_goal_pose1_.x<<"; SIM_initial_person_goal_pose1_.y="<< SIM_initial_person_goal_pose1_.y<< std::endl;
+			}
+
+			std::string data_file="/home/erepiso/iri-lab/labrobotica/restricted/algorithms/people_prediction/branches/local_lib_ATR_7_junio/1_data_results_side_by_side/results_person_companion_side_by_side1.txt";
+			results_file_=data_file;
+
+			std::string data_file2="/home/erepiso/iri-lab/labrobotica/restricted/algorithms/people_prediction/branches/ely_people_prediction_companion_robot/2_results_evaluate_costs/results_evaluate_cost_robot_case0.txt";
+			evaluate_costs_file_=data_file2;
+
+			std::string data_file3="/home/erepiso/iri-lab/labrobotica/restricted/algorithms/people_prediction/branches/ely_people_prediction_companion_robot/2_results_evaluate_costs/results_evaluate_distance_and_angle_robot_case0.txt";
+			evaluate_change_distance_and_angle_companion_file_=data_file3;
+
+			// change case =0
+			actual_case_=Cplan_local_nav::case0;
+
+
+		}else if((global_iter_to_change_simulation_case_>100)&&(global_iter_to_change_simulation_case_<=200)){ // 100 -> 200
+			std::cout << "PPPP CASE 1; SIM_initial_person_goal_pose2_.x"  << SIM_initial_person_goal_pose2_.x<<"; SIM_initial_person_goal_pose2_.y="<< SIM_initial_person_goal_pose2_.y<< std::endl;
+			// change case =1
+			if(first_time_case1_){
+				iteration_=1;
+				experiment_=1;
+				first_time_case1_=false;
+			}
+
+			std::string data_file="/home/erepiso/iri-lab/labrobotica/restricted/algorithms/people_prediction/branches/local_lib_ATR_7_junio/1_data_results_side_by_side/results_person_companion_side_by_side2.txt";
+			results_file_=data_file;
+			std::string data_file2="/home/erepiso/iri-lab/labrobotica/restricted/algorithms/people_prediction/branches/ely_people_prediction_companion_robot/2_results_evaluate_costs/results_evaluate_cost_robot_case1.txt";
+			evaluate_costs_file_=data_file2;
+			std::string data_file3="/home/erepiso/iri-lab/labrobotica/restricted/algorithms/people_prediction/branches/ely_people_prediction_companion_robot/2_results_evaluate_costs/results_evaluate_distance_and_angle_robot_case1.txt";
+			evaluate_change_distance_and_angle_companion_file_=data_file3;
+
+			actual_case_=Cplan_local_nav::case1;
+			// Use initial goal person2 and change the person goals case 1 in Cscene sim
+			// change results file. Case 1
+
+		}else if((global_iter_to_change_simulation_case_>200)&&(global_iter_to_change_simulation_case_<=300)){ // 200 -> 300
+			std::cout << "PPPP CASE 2; SIM_initial_person_goal_pose3_.x"  << SIM_initial_person_goal_pose3_.x<<"; SIM_initial_person_goal_pose3_.y="<< SIM_initial_person_goal_pose3_.y<< std::endl;
+			// change case =2
+			if(first_time_case2_){
+				iteration_=1;
+				experiment_=1;
+				first_time_case2_=false;
+			}
+
+			std::string data_file="/home/erepiso/iri-lab/labrobotica/restricted/algorithms/people_prediction/branches/local_lib_ATR_7_junio/1_data_results_side_by_side/results_person_companion_side_by_side3.txt";
+			results_file_=data_file;
+			std::string data_file2="/home/ely7787/iri-lab/labrobotica/restricted/algorithms/people_prediction/branches/ely_people_prediction_companion_robot/2_results_evaluate_costs/results_evaluate_cost_robot_case2.txt";
+			evaluate_costs_file_=data_file2;
+			std::string data_file3="/home/ely7787/iri-lab/labrobotica/restricted/algorithms/people_prediction/branches/ely_people_prediction_companion_robot/2_results_evaluate_costs/results_evaluate_distance_and_angle_robot_case2.txt";
+			evaluate_change_distance_and_angle_companion_file_=data_file3;
+			actual_case_=Cplan_local_nav::case2;
+			// Use initial goal person3 and change the person goals case2 in Cscene sim
+		}
+		std::string data_file3="/home/ely7787/iri-lab/labrobotica/restricted/algorithms/people_prediction/branches/ely_people_prediction_companion_robot/2_results_evaluate_costs/results_evaluate_distance_and_angle_robot_case3.txt";
+		evaluate_change_distance_and_angle_companion_file_=data_file3;
+
+	}else{
+		// save data with real robot.
+		results_file_=results_file_real_;//data_file;
+	}
+	// FIN Starting points of people! when only have companion task, not approaching task, final goal is static
+
+	// INI Cases when only have companion task, not approaching task, final goal is one of the environment, modified with the direction of movement of the companion.
+	if(check_execution_times_){
+		robot_plan_companion3_end_ITER = clock();
+		clock_start3= (unsigned int) robot_plan_companion3_start_ITER;
+		clock_end3= (unsigned int) robot_plan_companion3_end_ITER;
+		std::cout<< "IIIIIIIIIIIIII CLOCKS_PER_SEC=1000000=" <<CLOCKS_PER_SEC<<"; robot_plan_companio3_end_ITER="<<robot_plan_companion3_end_ITER<<"; robot_plan_companion3_start_ITER="<<robot_plan_companion3_start_ITER<< std::endl;
+		std::cout << "(before switch(Action_)) time_tot=robot_plan_companion3_start_ITER-robot_plan_companion3_end_ITER="<<(robot_plan_companion3_end_ITER-robot_plan_companion3_start_ITER)<< std::endl;
+		std::cout << "(before switch(Action_)) time_tot=robot_plan_companion3_start_ITER-robot_plan_companio3_end_ITER="<<((robot_plan_companion3_end_ITER-robot_plan_companion3_start_ITER)/CLOCKS_PER_SEC)<< std::endl;
+		std::cout << "(before switch(Action_)) time_tot=robot_plan_companion3_start_ITER-robot_plan_companion3_end_ITER="<<(clock_end3-clock_start3)/1000000<< std::endl;
+		std::cout << "(before switch(Action_)) time_tot=robot_plan_companion3_start_ITER-robot_plan_companion3_end_ITER="<<(robot_plan_companion3_end_ITER-robot_plan_companion3_start_ITER)/1000000<< std::endl;
+	}
+	//std::cout <<"12 In Local Lib ASSAOP robot_plan_companion3"<< std::endl;
+	switch(Action_){
+		case START: // return to the starting point for simulation iterativelly.
+			//std::cout <<" case_START!!!"<< std::endl;
+			change_final_robot_orientation_=false;
+
+			if(goal1_==1){
+				SIM_initial_robot_pose1_.time_stamp=pose_command.time_stamp;
+				actual_goal_to_return_initial_position_=SIM_initial_robot_pose1_;//Spoint(-1.0,-0.5,pose_command.time_stamp);
+				act_result=robot_plan_companion_return_initial_pose(pose_command, reactive,dt); // el last step cambia cuando tenemos el START.
+				//std::cout <<"START SIM; (goal1) SIM_initial_robot_pose1_.x="<<SIM_initial_robot_pose1_.x<<"; SIM_initial_robot_pose1_.y="<<SIM_initial_robot_pose1_.y<< std::endl;
+				if(robot_->get_current_pointV().distance(SIM_initial_robot_pose1_)<1){
+					goal1_=3;
+				}
+			}else if(goal1_==2){ // goal to aboid obstacle and aboid innecesary robot collision with obstacle!
+				if(actual_case_==Cplan_local_nav::case1){
+					SIM_initial_robot_pose4_.time_stamp=pose_command.time_stamp;
+					actual_goal_to_return_initial_position_=SIM_initial_robot_pose4_;//Spoint(-1.0,-0.5,pose_command.time_stamp);
+					//std::cout <<"START SIM; (goal2) SIM_initial_robot_pose4_.x="<<SIM_initial_robot_pose4_.x<<"; SIM_initial_robot_pose4_.y="<<SIM_initial_robot_pose4_.y<< std::endl;
+				}else{
+					SIM_initial_robot_pose3_.time_stamp=pose_command.time_stamp;
+					actual_goal_to_return_initial_position_=SIM_initial_robot_pose4_;//Spoint(-1.0,-0.5,pose_command.time_stamp);
+					//std::cout <<"START SIM; (goal2) SIM_initial_robot_pose3_.x="<<SIM_initial_robot_pose4_.x<<"; SIM_initial_robot_pose4_.y="<<SIM_initial_robot_pose4_.y<< std::endl;
+				}
+				act_result=robot_plan_companion_return_initial_pose(pose_command, reactive,dt); // el last step cambia cuando tenemos el START.
+				if((robot_->get_current_pointV().distance(SIM_initial_robot_pose3_)<1)||(robot_->get_current_pointV().distance(SIM_initial_robot_pose4_)<1)){
+					goal1_=1;
+				}
+			}else if(goal1_==3){
+				SIM_initial_robot_pose2_.time_stamp=pose_command.time_stamp;
+				actual_goal_to_return_initial_position_=SIM_initial_robot_pose2_;//Spoint(-1.0,-0.5,pose_command.time_stamp);
+				act_result=robot_plan_companion_return_initial_pose(pose_command, reactive,dt); // el last step cambia cuando tenemos el START.
+				double dist_person_companion_to_ini_goal;
+				SpointV_cov spoint_person_companion_act=actual_person_Companion_SpointV_;
+				dist_person_companion_to_ini_goal=SIM_initial_person_companion_pose2_.distance(spoint_person_companion_act);
+				//std::cout <<"START SIM; (goal3) SIM_initial_robot_pose2_.x="<<SIM_initial_robot_pose2_.x<<"; SIM_initial_robot_pose2_.y="<<SIM_initial_robot_pose2_.y<< std::endl;
+
+				if((robot_->get_current_pointV().distance(SIM_initial_robot_pose2_)<2.0)&&(dist_person_companion_to_ini_goal<1.0)){ // antes era 1.75
+					// OJO!!! con personas hay que esperar a que las otras lleguen a su goal!!!
+					//std::cout << "change status to ITER !!!"<< std::endl;
+					Action_=Cplan_local_nav::ITER; // cambiar por esperar hasta que la person_companion y person_goal lleguen al goal...
+					first_in_itter_=true;
+					experiment_++;
+					iteration_=1;
+				}
+			}
+
+			break;
+
+		case ITER:
+			//if(output_screen_messages_){
+			//std::cout <<"ATR companion ITER"<< std::endl;
+			//}
+			change_final_robot_orientation_=false;
+			go_behid_comp_person_=true;
+			goal1_=3;
+
+			unsigned int clock_start2;
+			unsigned int clock_end2;
+			if(check_execution_times_){
+				robot_plan_companion3_end_ITER = clock();
+				clock_start2= (unsigned int) robot_plan_companion3_start_ITER;
+				clock_end2= (unsigned int) robot_plan_companion3_end_ITER;
+				std::cout<< "IIIIIIIIIIIIII CLOCKS_PER_SEC=1000000=" <<CLOCKS_PER_SEC<<"; robot_plan_companio3_end_ITER="<<robot_plan_companion3_end_ITER<<"; robot_plan_companion3_start_ITER="<<robot_plan_companion3_start_ITER<< std::endl;
+				std::cout << "(before robot_plan_companion2) time_tot=robot_plan_companion3_start_ITER-robot_plan_companion3_end_ITER="<<(robot_plan_companion3_end_ITER-robot_plan_companion3_start_ITER)<< std::endl;
+				std::cout << "(before robot_plan_companion2) time_tot=robot_plan_companion3_start_ITER-robot_plan_companio3_end_ITER="<<((robot_plan_companion3_end_ITER-robot_plan_companion3_start_ITER)/CLOCKS_PER_SEC)<< std::endl;
+				std::cout << "(before robot_plan_companion2) time_tot=robot_plan_companion3_start_ITER-robot_plan_companion3_end_ITER="<<(clock_end2-clock_start2)/1000000<< std::endl;
+				std::cout << "(before robot_plan_companion2) time_tot=robot_plan_companion3_start_ITER-robot_plan_companion3_end_ITER="<<(robot_plan_companion3_end_ITER-robot_plan_companion3_start_ITER)/1000000<< std::endl;
+			}
+			//std::cout <<"ATR COMPANION robot_plan_companion3; debug 13 (antes de robot_plan_companion2)"<< std::endl;
+			// iteracion akp!!!
+			act_result=robot_plan_companion2(pose_command, reactive,dt);
+			//std::cout <<" despues de robot_plan_companion2  "<< std::endl;
+
+			if(output_screen_messages_){
+			std::cout <<"COMPANION after robot_plan_companion2; pose_command.v="<<pose_command.v<<"; pose_command.w="<<pose_command.w<< std::endl;
+			}
+
+			if(check_execution_times_){
+				robot_plan_companion3_end_ITER = clock();
+				clock_start2= (unsigned int) robot_plan_companion3_start_ITER;
+				clock_end2= (unsigned int) robot_plan_companion3_end_ITER;
+				std::cout<< "IIIIIIIIIIIIII CLOCKS_PER_SEC=1000000=" <<CLOCKS_PER_SEC<<"; robot_plan_companio3_end_ITER="<<robot_plan_companion3_end_ITER<<"; robot_plan_companion3_start_ITER="<<robot_plan_companion3_start_ITER<< std::endl;
+				std::cout << "(FIN) time_tot=robot_plan_companion3_start_ITER-robot_plan_companion3_end_ITER="<<(robot_plan_companion3_end_ITER-robot_plan_companion3_start_ITER)<< std::endl;
+				std::cout << "(FIN) time_tot=robot_plan_companion3_start_ITER-robot_plan_companio3_end_ITER="<<((robot_plan_companion3_end_ITER-robot_plan_companion3_start_ITER)/CLOCKS_PER_SEC)<< std::endl;
+				std::cout << "(FIN) time_tot=robot_plan_companion3_start_ITER-robot_plan_companion3_end_ITER="<<(clock_end2-clock_start2)/1000000<< std::endl;
+				std::cout << "(FIN) time_tot=robot_plan_companion3_start_ITER-robot_plan_companion3_end_ITER="<<(robot_plan_companion3_end_ITER-robot_plan_companion3_start_ITER)/1000000<< std::endl;
+			}
+
+			const std::vector<Sdestination>* actual_dests= get_destinations();
+			if(sim){
+				if(debug_sideBySide2019_){
+					std::cout <<"ATR COMPANION robot_plan_companion3; robot_->get_current_pointV().distance(actual_dests->at(2)="<<robot_->get_current_pointV().distance(actual_dests->at(2))<< std::endl;
+				}
+				if(actual_dests->size()>3){
+					if((robot_->get_current_pointV().distance(actual_dests->at(0))<4.5)||(robot_->get_current_pointV().distance(actual_dests->at(2))<3.5)||(robot_->get_current_pointV().distance(actual_dests->at(3))<3.5)){
+						Action_=Cplan_local_nav::START;
+						if(first_in_itter_){ // cambia caso, nos vamos al inicio que toque!
+							global_iter_to_change_simulation_case_++;
+							first_in_itter_=false;
+						}
+						// reset persons to predict well its destination
+						for( auto iit: person_list_ )
+						{
+							iit->reset();
+						}
+					}
+				}
+			}
+
+			if(restart_real_){ // restart document, new companion experiment.
+				restart_real_=false;
+				Action_=Cplan_local_nav::ITER; // cambiar por esperar hasta que la person_companion y person_goal lleguen al goal...
+				new_matlab_file();
+				iteration_=1;
+				experiment_=1;
+			}
+			// std::cout <<" out ITER "<< std::endl;
+
+			break;
+
+		}
+
+	if(debug_sideBySide2019_){
+		 std::cout << " OUT CASES robot_plan_companion3 "<< std::endl;
+	}
+
+	// If before person destination is different from the actual destination. Reset all the persons. Something change!
+	// TO solve problems of bad final destination selection (this problems are add because we use the before probabilities of the destinations. Model construction of the bayesian intentionality predictor.)
+	//std::cout << " before change dest comp person="<< std::endl;
+
+	unsigned int clock_start;
+	unsigned int clock_end;
+	if(check_execution_times_){
+		robot_plan_companion3_end_ITER = clock();
+		clock_start= (unsigned int) robot_plan_companion3_start_ITER;
+		clock_end= (unsigned int) robot_plan_companion3_end_ITER;
+		std::cout<< "IIIIIIIIIIIIII CLOCKS_PER_SEC=1000000=" <<CLOCKS_PER_SEC<<"; robot_plan_companio3_end_ITER="<<robot_plan_companion3_end_ITER<<"; robot_plan_companion3_start_ITER="<<robot_plan_companion3_start_ITER<< std::endl;
+		std::cout << "(FIN 1) time_tot=robot_plan_companion3_start_ITER-robot_plan_companion3_end_ITER="<<(robot_plan_companion3_end_ITER-robot_plan_companion3_start_ITER)<< std::endl;
+		std::cout << "(FIN 1) time_tot=robot_plan_companion3_start_ITER-robot_plan_companio3_end_ITER="<<((robot_plan_companion3_end_ITER-robot_plan_companion3_start_ITER)/CLOCKS_PER_SEC)<< std::endl;
+		std::cout << "(FIN 1) time_tot=robot_plan_companion3_start_ITER-robot_plan_companion3_end_ITER="<<(clock_end-clock_start)/1000000<< std::endl;
+		std::cout << "(FIN 1) time_tot=robot_plan_companion3_start_ITER-robot_plan_companion3_end_ITER="<<(robot_plan_companion3_end_ITER-robot_plan_companion3_start_ITER)/1000000<< std::endl;
+	}
+
+	//find_comp_pers= find_person(id_person_companion_ , &person_obj3);
+	if(we_have_pointer_to_first_person_){
+		if(actual_person_Companion_pointer_->get_best_dest().id!=before_companion_person_destination_.id){
+			actual_person_Companion_pointer_->reset_before_destination_prob();
+		}
+		before_companion_person_destination_=actual_person_Companion_pointer_->get_best_dest();
+	}
+	//std::cout << " after change dest comp person; pose_command.v="<<pose_command.v<<"; pose_command.w="<<pose_command.w<< std::endl;
+	//std::cout << " after change dest comp person; robot_initial_pose_.v="<<robot_initial_pose_.v<<"; robot_initial_pose_.w="<<robot_initial_pose_.w<< std::endl;
+
+
+	robot_plan_companion3_end_ITER = clock();
+	clock_start= (unsigned int) robot_plan_companion3_start_ITER;
+	clock_end= (unsigned int) robot_plan_companion3_end_ITER;
+	//std::cout<< "IIIIIIIIIIIIII CLOCKS_PER_SEC=1000000=" <<CLOCKS_PER_SEC<<"; robot_plan_companio3_end_ITER="<<robot_plan_companion3_end_ITER<<"; robot_plan_companion3_start_ITER="<<robot_plan_companion3_start_ITER<< std::endl;
+	//std::cout << "(FIN) time_tot=robot_plan_companion3_start_ITER-robot_plan_companion3_end_ITER="<<(robot_plan_companion3_end_ITER-robot_plan_companion3_start_ITER)<< std::endl;
+	//std::cout << "(FIN) time_tot=robot_plan_companion3_start_ITER-robot_plan_companio3_end_ITER="<<((robot_plan_companion3_end_ITER-robot_plan_companion3_start_ITER)/CLOCKS_PER_SEC)<< std::endl;
+	//std::cout << "[ms] (FIN) time_tot=robot_plan_companion3_start_ITER-robot_plan_companion3_end_ITER="<<((clock_end-clock_start)/clocks_per_sec_my_var_)*1000<< std::endl;
+	//std::cout << "(FIN) time_tot=robot_plan_companion3_start_ITER-robot_plan_companion3_end_ITER="<<(robot_plan_companion3_end_ITER-robot_plan_companion3_start_ITER)/1000000<< std::endl;
+
+	final_compt_time_=((clock_end-clock_start)/clocks_per_sec_my_var_)*1000;
+
+	// Side-by-side for simulation iterative.
+	if((final_goal_reached_in_node_)&&(!change_final_robot_orientation_)&&(sim)){
+		std::cout << " V=0; w=0; (final_goal_reached_in_node_)&&(!change_final_robot_orientation_); final_goal_reached_in_node_="<<final_goal_reached_in_node_<<"; !change_final_robot_orientation_="<<change_final_robot_orientation_<< std::endl;
+		pose_command.v=0.0; // MIRAR si puede ser usar el case stop. Creo que no...
+		pose_command.w=0.0;
+		if(see_std_out_velocities_){
+			std::cout << " !!! 333 777 !!! V="<<pose_command.v<<"; w="<<pose_command.w<<"; (final_goal_reached_in_node_); final_goal_reached_in_node_="<<final_goal_reached_in_node_<<"; !change_final_robot_orientation_="<<change_final_robot_orientation_<< std::endl;
+		}
+	}
+
+	final_pose_robot_=pose_command;
+
+
+    robot_plan_companion3_end= clock();
+    clock_start= (unsigned int) robot_plan_companion3_start;
+    clock_end= (unsigned int) robot_plan_companion3_end;
+
+  // std::cout << "before/antes limit velocity: pose_command.v="<<pose_command.v<<"; final_max_v_="<<final_max_v_<<"; act_result="<<act_result<< std::endl;
+    if(final_max_v_<pose_command.v){  // limit the robot's velocity until the maximum one.
+    	pose_command.v=final_max_v_;
+    	if(see_std_out_velocities_){
+    		std::cout << " LIMIT vel vMax after/despues limit velocity: pose_command.v="<<pose_command.v<<"; final_max_v_="<<final_max_v_<< std::endl;
+    	}
+    }
+
+
+	if(save_in_file_){
+		// save results in file for SIDE-by-SIDE
+		if(sim){
+			//Cperson_abstract* person_obj;
+			//find_person(id_person_companion_ , &person_obj);
+			// person_obj => actual_person_Companion_pointer_
+			Sdestination goal_now=actual_person_Companion_pointer_->get_best_dest();
+			Spoint etren_goal_act(goal_now.x,goal_now.y,goal_now.time_stamp);
+			double distance_to_goal1;
+			double distance_to_goal2;
+
+			Spoint etren_goal_act1(27.0,-0.5,goal_now.time_stamp);
+			distance_to_goal1=initial_robot_spoint_.distance(etren_goal_act1);
+			Spoint etren_goal_act2(0.0,-0.5,goal_now.time_stamp);
+			distance_to_goal2=initial_robot_spoint_.distance(etren_goal_act2);
+			std::cout << "(FIN) meters_goal_to_save_results_in_file_="<<meters_goal_to_save_results_in_file_<< std::endl;
+
+			if(((distance_to_goal1>meters_goal_to_save_results_in_file_)&&(distance_to_goal2>meters_goal_to_save_results_in_file_))&&(save_results_in_file_)){ // guarda datos.
+				// Not save the results very near to the goal, because the sim people do not turn as a normal person. Then this behavior is not normal and can afert to the real results of the method.
+				if(save_results_on_files_for_robot_){
+					printToMatlab();
+				}
+				first_time_=true;
+				iteration_++;
+			}else{
+				//std::cout << "(FIN) enter ELSE="<< std::endl;
+				if(first_time_){ //crear nuevo fichero de datos pero solo la primera vez y augmentar contador.
+					inc_distance_ant_=0;
+					acum_time_=0;
+				}
+				first_time_=false;
+			}
+		}else{
+			if(see_save_in_file_){
+				std::cout <<" 3 IN primer filtro save in file"<< std::endl;
+			}
+			iteration_++;
+			//std::cout << "(1) ENTRO EN save_results_on_files_for_REAL_robot_ " << std::endl;
+			printToMatlab();
+		}
+
+	}
+	// FIN SAVE IN FILE
+	if(output_screen_messages_){
+	std::cout << "(2) OUTPUT ROBOT PLAN 3  pose_command.v="<<pose_command.v<<"; pose_command.w="<<pose_command.w<< std::endl;
+	}
+	return act_result;
+
+}
+
+
+////////////////////////////
+bool Cplan_local_nav::robot_plan_companion_return_initial_pose(Spose& pose_command, Cperson_abstract::companion_reactive& reactive,double dt)
+{   // function to return to the initial pose in the cases of the robot's iterative simulations.
+	// std::cout <<"(1) INI robot_plan_companion_return_initial_pose"<< std::endl;
+	//clock_t robot_plan_companion2_start, robot_plan_companion2_end;
+	//robot_plan_companion2_start = clock();
+
+	new_debug_file();
+
+	robot_->set_externa_force_k_near_goal(externa_force_k_near_goal_akp_);
+	robot_->set_ex_max_dist_to_near_goal_force_in(max_dist_to_near_goal_force_akp_);
+	robot_->set_externa_force_k_far_goal(externa_force_k_far_goal_akp_);
+
+	before_initial_robot_spoint_.x=-10000;
+	before_initial_robot_spoint_.y=-10000;
+
+	if(companion_same_person_goal_){
+		//Cperson_abstract* person_obj;
+		//find_person(id_person_companion_ , &person_obj);
+
+		Spose robot=robot_->get_current_pose();
+		SpointV_cov person = actual_person_Companion_pointer_->get_current_pointV();
+		Sdestination external_goal_same_person=actual_person_Companion_pointer_->get_best_dest();
+
+		double theta= atan2(actual_person_Companion_pointer_->get_best_dest().y-person.y,actual_person_Companion_pointer_->get_best_dest().x-person.x);
+		double angle=atan2(robot.y-person.y , robot.x-person.x);
+
+		if( diffangle(theta, angle) < 0 ){
+			external_goal_same_person.x=external_goal_same_person.x+robot_person_proximity_goals_x_*cos(theta+angle);
+			external_goal_same_person.y= external_goal_same_person.y+robot_person_proximity_goals_y_*sin(theta+angle);
+		}else{
+			external_goal_same_person.x=external_goal_same_person.x+robot_person_proximity_goals_x_*cos(theta-angle);
+			external_goal_same_person.y= external_goal_same_person.y+robot_person_proximity_goals_y_*sin(theta-angle);
+		}
+		//set_robot_external_goal(external_goal_same_person);
+	}
+
+	robot_companion_case_=reactive;
+
+	clock_t scene_prediction_start, scene_prediction_end;
+	scene_prediction_start=clock();
+	this->Cprediction_behavior::scene_prediction();
+	scene_prediction_end=clock();
+	if(debug_real_test_companion_){
+	std::cout << "time_prediction=scene_prediction_start-scene_prediction_end="<<(scene_prediction_end-scene_prediction_start)/1000<< std::endl;
+	}
+
+	bool result; // para no entrar a variables bacias.
+
+	robot_initial_pose_=robot_->get_current_pose();
+	SpointV_cov centro_robot_spoint=robot_->get_current_pointV();
+	initial_robot_spoint_=robot_->get_current_pointV();
+
+	/* Inicio, get person_robot_distance.*/
+	double robot_person_distance=calc_robot_person_companion_distance();
+	robot_person_distance_=robot_person_distance;
+
+	/* fin, get person_robot_distance.*/
+	//Cperson_abstract* person_obj;
+	//find_person(id_person_companion_ , &person_obj);
+
+	if(only_comp_people_vel_and_robot_poses_){
+		std::cout << " PERSON SpointV_cov: "<< std::endl;
+		actual_person_Companion_pointer_->print();
+		SpointV_cov actual_person1=actual_person_Companion_pointer_->get_current_pointV();
+		std::ofstream fileMatlab2;
+		fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+		fileMatlab2 << " PERSON SpointV_cov: "<< " \n";
+		fileMatlab2 << "person_spoint_.time_stamp="<<actual_person1.time_stamp<<" \n";
+		fileMatlab2 << "person_spoint_.x="<<actual_person1.x<<" \n";
+		fileMatlab2 << "person_spoint_.y="<<actual_person1.y<<" \n";
+		fileMatlab2 << "person_spoint_.vx="<<actual_person1.vx<<" \n";
+		fileMatlab2 << "person_spoint_.vy="<<actual_person1.vy<<" \n";
+		fileMatlab2.close();
+	}
+
+	robot_person_companion_distance_=(robot_person_proximity_distance_+2*robot_->get_platform_radii()+little_augmented_collision_margin_)/2;
+
+	//std::cout << "ANTES init_robot_plan2 en robot_plan_companion_return_initial_pose!!!! "<< std::endl;
+	//clock_t init_robot_plan2_start, init_robot_plan2_end;
+	//init_robot_plan2_start=clock();
+	init_robot_plan2_return_initial_position();
+	//init_robot_plan2_end=clock();
+	ini_increment_angle(); // for robot companion
+
+
+	if(debug_file_robot_){
+		std::ofstream fileMatlab2;
+		fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+		fileMatlab2 << "Entra REACTIVE force_ATRACTIVE!!!!!! robot_person_distance="<<robot_person_distance<<" > (robot_person_proximity_distance_+robot_person_proximity_tolerance_+offset_atractive_)="<<(robot_person_proximity_distance_+robot_person_proximity_tolerance_+offset_atractive_)<<"\n";
+		fileMatlab2.close();
+	}
+
+	reactive=Cperson_abstract::Reactiva_repulsive;
+	robot_companion_case_=reactive;
+	if ( robot_plan_anticipative_krrt_companion(reactive) )
+	{
+		last_pose_command_ = get_best_planned_pose( dt,reactive);
+		double other_people_cost_due_to_robot;
+		double companion_person_cost_due_to_robot;
+		this->Cprediction_behavior::calculate_current_forces_companion2( pr_force_mode_ ,id_person_companion_,false, reactive, min_next_companion_angle_,u_forces_robot_actual_.f_goal,u_forces_robot_actual_.f_people,u_forces_robot_actual_.f_obs,u_forces_robot_actual_.f_persongoal,other_people_cost_due_to_robot,companion_person_cost_due_to_robot);//for plotting purposes
+		other_people_due_to_robot_cost_=other_people_cost_due_to_robot;
+		companion_person_due_to_robot_cost_=companion_person_cost_due_to_robot;
+		if(debug_antes_subgoals_entre_AKP_goals_){
+			std::cout << " Planing FINAL GOAL (NO collision, reactive_repulsive) ( last_pose_command_ and robot_):"<< std::endl;
+			last_pose_command_.print();
+			robot_->print();
+		}
+		result = true;
+	}
+	else
+	{
+		last_pose_command_ = Spose();
+		result = false;
+	}
+	nearby_person_list_.clear();
+
+	pose_command = last_pose_command_; // nueva pose de trayectoria calculada.
+
+	// *** Return robot max vel to really máx velocity ***/
+	return_max_velocity_systemRobot_to_max_value();
+
+	before_initial_robot_spoint_=initial_robot_spoint_;
+	time_ant_=actual_time_;
+	planner_iterations_++;
+
+	//robot_plan_companion2_end = clock();
+	//if(check_execution_times_){
+	//	std::cout << "(FIN) [ms] time_tot=robot_plan_companion2_start-robot_plan_companion2_end="<<((robot_plan_companion2_end-robot_plan_companion2_start)/clocks_per_sec_my_var_)*1000<< std::endl;
+	//}
+
+	//std::cout << "last_pose_command_.print():"<< std::endl;
+	//last_pose_command_.print();
+	//std::cout << "FIN robot_plan_companion_return_initial_pose!!! "<< std::endl<< std::endl<< std::endl;
+
+	return result;
+}
+
+
+
+
+bool Cplan_local_nav::robot_plan_companion2(Spose& pose_command, Cperson_abstract::companion_reactive& reactive,double dt)
+{  // FUNCTION that calculates the local-planner for the robot's Adaptive Side-by-Side Accompaniment of One person (ASSAOP) and the robot's Adaptive Side-by-Side Accompaniment of Groups of People (ASSAGP)
+	//std::cout <<" !!!INI robot_plan_companion2!!! " << std::endl;
+
+	initial_part_start=clock();
+	robot_plan_companion2_start_ITER = clock();
+
+	// computational time: de aquí al final de iter antes del breack.
+	// INI computational time!!!
+	new_debug_file();
+
+	robot_->set_externa_force_k_near_goal(externa_force_k_near_goal_akp_);
+	robot_->set_ex_max_dist_to_near_goal_force_in(max_dist_to_near_goal_force_akp_);
+	robot_->set_externa_force_k_far_goal(externa_force_k_far_goal_akp_);
+	before_initial_robot_spoint_.x=-10000;
+	before_initial_robot_spoint_.y=-10000;
+
+	if(companion_same_person_goal_){
+		//Cperson_abstract* person_obj;
+		//find_person(id_person_companion_ , &person_obj);
+		Spose robot=robot_->get_current_pose();
+		SpointV_cov person = actual_person_Companion_pointer_->get_current_pointV();
+		Sdestination external_goal_same_person=actual_person_Companion_pointer_->get_best_dest();
+
+		double theta= atan2(actual_person_Companion_pointer_->get_best_dest().y-person.y,actual_person_Companion_pointer_->get_best_dest().x-person.x);
+		double angle=atan2(robot.y-person.y , robot.x-person.x);
+
+		if( diffangle(theta, angle) < 0 ){
+			external_goal_same_person.x=external_goal_same_person.x+robot_person_proximity_goals_x_*cos(theta+angle);
+			external_goal_same_person.y= external_goal_same_person.y+robot_person_proximity_goals_y_*sin(theta+angle);
+			before_person_intersection_destination_=external_goal_same_person;
+		}else{
+			external_goal_same_person.x=external_goal_same_person.x+robot_person_proximity_goals_x_*cos(theta-angle);
+			external_goal_same_person.y= external_goal_same_person.y+robot_person_proximity_goals_y_*sin(theta-angle);
+			before_person_intersection_destination_=external_goal_same_person;
+		}
+	}
+
+	robot_companion_case_=reactive;
+
+	//clock_t robot_plan_companion2_start, robot_plan_companion2_end;
+	//robot_plan_companion2_start = clock();
+
+	clock_t scene_prediction_start, scene_prediction_end;
+	scene_prediction_start=clock();
+
+	//// INI: to set initial angle of companion for the calculation of the angle companion with the person prediction.
+	if(!calc_goal_companion_with_group_path_){
+		//std::cout << "ATR code  ENTER: => if(!calc_goal_companion_with_group_path_)"<< std::endl;
+		// Calc angle of compnaion with the prediction of the person before scene_prediction
+		//Cperson_abstract* person_obj5;
+		//bool finded_person5=find_person(id_person_companion_ , &person_obj5);
+		if(we_have_pointer_to_first_person_){
+			SpointV_cov spoint_person_companion_ac = actual_person_Companion_pointer_->get_current_pointV();
+			double theta=calc_person_companion_orientation();
+			if(theta<0){
+					theta=2*3.14+theta;
+			}
+			SpointV_cov robot7=robot_->get_current_pointV();
+			double angle2=atan2(robot7.y-spoint_person_companion_ac.y , robot7.x-spoint_person_companion_ac.x);
+			if(angle2<0){
+				angle2=((360*3.14)/180)+angle2;
+			}
+
+			double ini_angle_act;
+			//double ant_ini_angle;
+			if( diffangle(theta, angle2) < 0 ){
+				ini_angle_act=(theta + angle2)*(180/3.14);//(angle)*(180/3.14);
+			//	ant_ini_angle=(robot_->get_current_pointV().angle_heading_point((Spoint) companion_person_position_))*(180/3.14);
+			}else{
+				ini_angle_act=(theta - angle2)*(180/3.14);//(angle)*(180/3.14);
+				//ant_ini_angle=(robot_->get_current_pointV().angle_heading_point((Spoint) companion_person_position_))*(180/3.14);
+			}
+
+			if((ini_angle_act>=0)&&(ini_angle_act<=180)){
+				ini_angle_act=ini_angle_act;
+			}else if((ini_angle_act<0)&&(ini_angle_act>=(-180))){
+				ini_angle_act=sqrt(ini_angle_act*ini_angle_act);
+			}else if((ini_angle_act>180)&&(ini_angle_act<=360)){
+				ini_angle_act=360-ini_angle_act;
+			}
+			else if((ini_angle_act<(-180))&&(ini_angle_act>=(-360))){
+				ini_angle_act=sqrt(ini_angle_act*ini_angle_act);
+				ini_angle_act=360-ini_angle_act;
+			}else if(ini_angle_act>360){
+				unsigned int n_360_g=ini_angle_act/360;
+				ini_angle_act=ini_angle_act-n_360_g*360;
+			}
+
+			if(overpas_obstacles_behind_person_){
+				// pasar angulos a cuadrante 90 -> 180 grados.
+				if((ini_angle_act>=0)&&(ini_angle_act<=90)){
+					ini_angle_act=180-ini_angle_act;
+				}else if((ini_angle_act>=90)&&(ini_angle_act<=180)){
+					// ok, no se hace nada.
+				}else if((ini_angle_act>=180)&&(ini_angle_act<=270)){
+					ini_angle_act=ini_angle_act-180;
+					ini_angle_act=180-ini_angle_act;
+				}else if((ini_angle_act>=270)&&(ini_angle_act<=360)){
+					ini_angle_act=ini_angle_act-180;
+				}
+			   }
+			set_initial_angle_in_orientation_person_robot_angles_pred(ini_angle_act);
+			//// END: to set initial angle of companion for the calculation of the angle companion with the person prediction.
+
+			double in_real_distance_of_companion_between_person_and_robot=robot_->get_current_pointV().distance(spoint_person_companion_ac);
+			double act_robot_radi=robot_->get_platform_radii();
+			double in_threshold_distance_collision_calculate_companion_angle=(in_real_distance_of_companion_between_person_and_robot+2*act_robot_radi+little_augmented_collision_margin_)/2;
+			this->Cprediction_behavior::set_all_values_to_calc_the_companion_angle_with_person_prediction(in_threshold_distance_collision_calculate_companion_angle,angle_companion_,in_real_distance_of_companion_between_person_and_robot,robot_person_companion_distance_,overpas_obstacles_behind_person_,little_augmented_collision_margin_);
+		}
+	}
+
+
+	this->Cprediction_behavior::set_calc_goal_companion_with_group_path(calc_goal_companion_with_group_path_);
+
+	//clock_t pred_peopl_start, pred_peopl_end;
+	//pred_peopl_start=clock();
+	this->Cprediction_behavior::scene_prediction(); // scene prediction, needed, pero sin la person cmpanion y hará falta el robot en colisiones.
+	// get the angle of compnaion with the prediction of the person before scene_prediction
+	if(!calc_goal_companion_with_group_path_){
+		orientation_person_robot_angles_with_prediction_of_person_companion_=this->Cprediction_behavior::get_orientation_person_robot_angles_pred();
+		min_distance_collision_vector_from_pred_=this->Cprediction_behavior::get_min_distance_collision_vector_from_prediction();
+		//std::cout << " FIN (in of init_robot_plan2) out calc angle companion on scen_prediction"<< std::endl;
+	}
+	//std::cout << " after !calc_goal_companion_with_group_path_) "<< std::endl;
+
+	//pred_peopl_end=clock();
+	scene_prediction_end=clock();
+	if(debug_real_test_companion_){
+	std::cout << "time_prediction=scene_prediction_start-scene_prediction_end="<<(scene_prediction_end-scene_prediction_start)/1000<< std::endl;
+	}
+
+	bool result; // para no entrar a variables bacias.
+	robot_initial_pose_=robot_->get_current_pose();
+	SpointV_cov centro_robot_spoint=robot_->get_current_pointV();
+	initial_robot_spoint_=robot_->get_current_pointV();
+
+	// initial angle to accompany the person companion!
+	if(firts_iter_obtain_angle_person_companion_){
+		last_good_theta_person_=robot_initial_pose_.theta;
+		firts_iter_obtain_angle_person_companion_=false;
+	}
+
+
+	/* Inicio, get person_robot_distance.*/
+	double robot_person_distance=calc_robot_person_companion_distance();
+	robot_person_distance_=robot_person_distance;
+	//std::cout << " after get's calc_robot_person_companion_distance(); "<< std::endl;
+
+	/* fin, get person_robot_distance.*/
+	if(debug_real_test_companion2_){
+		std::cout << "(*** INIII ***) REAL robot_person_distance="<<robot_person_distance<< std::endl;
+		std::cout << "companion_same_person_goal_="<<companion_same_person_goal_<< std::endl;
+	}
+
+	//Cperson_abstract* person_obj;
+	//find_person(id_person_companion_ , &person_obj);
+
+	if(only_comp_people_vel_and_robot_poses_){
+		std::cout << " PERSON SpointV_cov: "<< std::endl;
+		actual_person_Companion_pointer_->print();
+		SpointV_cov actual_person1=actual_person_Companion_pointer_->get_current_pointV();
+		std::ofstream fileMatlab2;
+		fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+		fileMatlab2 << " PERSON SpointV_cov: "<< " \n";
+		fileMatlab2 << "person_spoint_.time_stamp="<<actual_person1.time_stamp<<" \n";
+		fileMatlab2 << "person_spoint_.x="<<actual_person1.x<<" \n";
+		fileMatlab2 << "person_spoint_.y="<<actual_person1.y<<" \n";
+		fileMatlab2 << "person_spoint_.vx="<<actual_person1.vx<<" \n";
+		fileMatlab2 << "person_spoint_.vy="<<actual_person1.vy<<" \n";
+		fileMatlab2.close();
+	}
+
+	robot_person_companion_distance_=(robot_person_proximity_distance_+2*robot_->get_platform_radii()+little_augmented_collision_margin_)/2;
+
+	//std::cout << "ANTES init_robot_plan2!!!! "<< std::endl;
+	//clock_t init_robot_plan2_start, init_robot_plan2_end;
+	//init_robot_plan2_start=clock();
+	init_robot_plan2(robot_person_distance);
+	//init_robot_plan2_end=clock();
+	//std::cout << " DESPUES init_robot_plan2"<< std::endl;
+
+	ini_increment_angle(); // for robot companion
+	double people_velocity=mean_people_velocity_;
+
+	if(((((robot_person_distance<(robot_person_proximity_distance_+robot_person_proximity_tolerance_+offset_atractive_))&&(robot_person_distance>0.5))&&(people_velocity>0.2) ))&&(!case_akp_plan_por_freanado_)){
+			//std::cout << "CASE AKP PLANNING"<< std::endl;
+		is_case_akp_true_=true;
+		if(debug_file_robot_){
+				std::ofstream fileMatlab2;
+				fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+				fileMatlab2 << "Entra OPTIMAL PLANNING!!! d=robot_person_distance="<<robot_person_distance<<"\n";
+				fileMatlab2.close();
+		}
+
+		if(!init_plan_approach_side_){ // ARREGLADO EL PARAR cerca del goal side-by-side en companion con esto!!!
+			reactive=Cperson_abstract::Akp_planning;
+			robot_companion_case_=reactive;
+		}else{
+			reactive=Cperson_abstract::Reactive_atractive;
+			robot_companion_case_=reactive;
+		}
+
+		//std::cout << " before robot_plan_anticipative_krrt_companion"<< std::endl;
+		if ( robot_plan_anticipative_krrt_companion(reactive) )
+		{
+			//std::cout << " IN if-robot_plan_anticipative_krrt_companion"<< std::endl;
+			clock_t end_part_start, end_part_end;
+			end_part_start=clock();
+			//std::cout << " CASE one person accompaniment get_best_planned_pose "<< std::endl;
+			last_pose_command_ = get_best_planned_pose( dt ,reactive);
+
+			double other_people_cost_due_to_robot;
+			double companion_person_cost_due_to_robot;
+
+			this->Cprediction_behavior::calculate_current_forces_companion2( pr_force_mode_ ,id_person_companion_,false, reactive, min_next_companion_angle_,u_forces_robot_actual_.f_goal,u_forces_robot_actual_.f_people,u_forces_robot_actual_.f_obs,u_forces_robot_actual_.f_persongoal,other_people_cost_due_to_robot,companion_person_cost_due_to_robot);//for plotting purposes
+
+			other_people_due_to_robot_cost_=other_people_cost_due_to_robot;
+			companion_person_due_to_robot_cost_=companion_person_cost_due_to_robot;
+
+			if(debug_antes_subgoals_entre_AKP_goals_){
+				std::cout << " Planing FINAL GOAL (NO collision, reactive_repulsive) ( last_pose_command_ and robot_):"<< std::endl;
+				last_pose_command_.print();
+				robot_->print();
+			}
+
+			if(!final_collision_check_){
+				result = true;
+				result_=true;
+			}else{
+				if(debug_file_robot_){
+					std::ofstream fileMatlab2;
+					fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+					fileMatlab2 << " else if(!final_collision_check_)=> final_collision_check_="<<final_collision_check_<<"\n";
+					fileMatlab2.close();
+				}
+				last_pose_command_ = Spose();
+				result = false;
+				enter_on_cero_=true;
+			}
+			end_part_end=clock();
+			if(check_execution_times_){
+				std::cout << " time_end_part=end_part_start-end_part_end="<<((end_part_end-end_part_start)/clocks_per_sec_my_var_)*1000<< std::endl;
+			}
+		}
+		else
+		{
+			last_pose_command_ = Spose();
+			result = false;
+			enter_on_cero2_=true;
+			if(see_std_out_velocities_){
+				std::cout << " IN AKP PLANNING  false return of robot_plan_anticipative_krrt_companion(reactive); test_distance_to_goal_="<<test_distance_to_goal_<<"; xy_2_goal_tolerance_="<<xy_2_goal_tolerance_<< std::endl;
+				std::cout << "(NO-plan-rrt=true; last_pose_command_.v"<<last_pose_command_.v<< "; last_pose_command_.w"<<last_pose_command_.w<< std::endl;
+			}
+
+		}
+
+	}else if(robot_person_distance<0.5){
+		//else if(robot_person_distance<(robot_person_proximity_distance_-robot_person_proximity_tolerance_)){
+		// case reactive_repulsive_force
+		//std::cout << "CASE reactive_repulsive_force"<< std::endl;
+
+		if(debug_file_robot_){
+			std::ofstream fileMatlab2;
+			fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+			fileMatlab2 << "Entra REACTIVE force_REPULSIVE!!! robot_person_distance="<<robot_person_distance<<" < (robot_person_proximity_distance_-robot_person_proximity_tolerance_)="<<(robot_person_proximity_distance_-robot_person_proximity_tolerance_)<<"\n";
+			fileMatlab2.close();
+		}
+
+		reactive=Cperson_abstract::Reactiva_repulsive;
+		robot_companion_case_=reactive;
+		if ( robot_plan_anticipative_krrt_companion(reactive) )
+		{
+			last_pose_command_ = get_best_planned_pose( dt,reactive);
+
+			double other_people_cost_due_to_robot;
+			double companion_person_cost_due_to_robot;
+			this->Cprediction_behavior::calculate_current_forces_companion2( pr_force_mode_ ,id_person_companion_,false, reactive, min_next_companion_angle_,u_forces_robot_actual_.f_goal,u_forces_robot_actual_.f_people,u_forces_robot_actual_.f_obs,u_forces_robot_actual_.f_persongoal,other_people_cost_due_to_robot,companion_person_cost_due_to_robot);//for plotting purposes
+			other_people_due_to_robot_cost_=other_people_cost_due_to_robot;
+			companion_person_due_to_robot_cost_=companion_person_cost_due_to_robot;
+			if(debug_antes_subgoals_entre_AKP_goals_){
+				std::cout << " Planing FINAL GOAL (NO collision, reactive_repulsive) ( last_pose_command_ and robot_):"<< std::endl;
+				last_pose_command_.print();
+				robot_->print();
+			}
+			result = true;
+			result_=true;
+		}else{
+			last_pose_command_ = Spose();
+			//std::cout << "[last_pose_command_ = Spose(); reactive=AKP_PLANNING;] (after get_best_planned_pose) last_pose_command_.v="<<last_pose_command_.v<< std::endl;
+			result = false;
+			result_=false;
+		}
+	}else if(people_velocity<0.2){ //VETE AL GOAL LATERAL O CENTRAL! if((case_akp_plan_por_freanado_)&&((people_velocity<0.2))){
+			// if(robot_person_distance>(robot_person_proximity_distance_+robot_person_proximity_tolerance_+offset_atractive_)){ // case reactive_atractive_force
+			//std::cout << " IN robot_plan_anticipative_krrt_companion (case else_if-case ATRACTIVE)"<< std::endl;
+
+			if(debug_file_robot_){
+				std::ofstream fileMatlab2;
+				fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+				fileMatlab2 << "Entra REACTIVE force_ATRACTIVE!!!!!! robot_person_distance="<<robot_person_distance<<" > (robot_person_proximity_distance_+robot_person_proximity_tolerance_+offset_atractive_)="<<(robot_person_proximity_distance_+robot_person_proximity_tolerance_+offset_atractive_)<<"\n";
+				fileMatlab2.close();
+			}
+
+			reactive=Cperson_abstract::Reactive_atractive;
+			robot_companion_case_=reactive;
+			//std::cout << "antes robot_plan_anticipative_krrt_companion-function"<< std::endl;
+			if ( robot_plan_anticipative_krrt_companion(reactive) )
+			{
+				//std::cout << "in if-robot_plan_anticipative_krrt_companion-function"<< std::endl;
+				last_pose_command_ = get_best_planned_pose( dt,reactive);  // antes era get_best_planned_pose() solo get_best_planned_pose( dt,reactive)
+
+				double other_people_cost_due_to_robot;
+				double companion_person_cost_due_to_robot;
+				this->Cprediction_behavior::calculate_current_forces_companion2( pr_force_mode_ ,id_person_companion_,false, reactive, min_next_companion_angle_,u_forces_robot_actual_.f_goal,u_forces_robot_actual_.f_people,u_forces_robot_actual_.f_obs,u_forces_robot_actual_.f_persongoal,other_people_cost_due_to_robot,companion_person_cost_due_to_robot);//for plotting purposes
+				other_people_due_to_robot_cost_=other_people_cost_due_to_robot;
+				companion_person_due_to_robot_cost_=companion_person_cost_due_to_robot;
+				if(see_std_out_velocities_){
+					std::cout << " Planing FINAL GOAL (ok plan, NO collision, reactive_repulsive) last_pose_command_.v="<<last_pose_command_.v<<"; last_pose_command_.w="<<last_pose_command_.w<< std::endl;
+				}
+				result = true;
+				result_=true;
+			}
+			else
+			{
+				last_pose_command_ = Spose();
+				if(see_std_out_velocities_){
+					std::cout << "[NO-plan-akp; last_pose_command_ = Spose(); reactive=Cperson_abstract::Reactive_atractive;] (after get_best_planned_pose) last_pose_command_.v="<<last_pose_command_.v<< std::endl;
+				}
+				result = false;
+				result_=false;
+			}
+	}else{
+		//std::cout << " IN robot_plan_anticipative_krrt_companion (case else_if-case ATRACTIVE)"<< std::endl;
+		//if(see_std_out_velocities_){
+		//	std::cout << "CASE reactive_atractive_force"<< std::endl;
+		//}
+
+		if(debug_file_robot_){
+			std::ofstream fileMatlab2;
+			fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+			fileMatlab2 << "Entra REACTIVE force_ATRACTIVE!!!!!! robot_person_distance="<<robot_person_distance<<" > (robot_person_proximity_distance_+robot_person_proximity_tolerance_+offset_atractive_)="<<(robot_person_proximity_distance_+robot_person_proximity_tolerance_+offset_atractive_)<<"\n";
+			fileMatlab2.close();
+		}
+
+		reactive=Cperson_abstract::Reactive_atractive;
+		robot_companion_case_=reactive;
+
+		if ( robot_plan_anticipative_krrt_companion(reactive) )
+		{
+			last_pose_command_ = get_best_planned_pose( dt,reactive);  // antes era get_best_planned_pose() solo get_best_planned_pose( dt,reactive)
+
+			double other_people_cost_due_to_robot;
+			double companion_person_cost_due_to_robot;
+			this->Cprediction_behavior::calculate_current_forces_companion2( pr_force_mode_ ,id_person_companion_,false, reactive, min_next_companion_angle_,u_forces_robot_actual_.f_goal,u_forces_robot_actual_.f_people,u_forces_robot_actual_.f_obs,u_forces_robot_actual_.f_persongoal,other_people_cost_due_to_robot,companion_person_cost_due_to_robot);//for plotting purposes
+			other_people_due_to_robot_cost_=other_people_cost_due_to_robot;
+			companion_person_due_to_robot_cost_=companion_person_cost_due_to_robot;
+			if(see_std_out_velocities_){
+				std::cout << " Planing FINAL GOAL (ok plan, NO collision, reactive_repulsive) last_pose_command_.v="<<last_pose_command_.v<<"; last_pose_command_.w="<<last_pose_command_.w<< std::endl;
+			}
+			result = true;
+			result_=true;
+		}else{
+			last_pose_command_ = Spose();
+			if(see_std_out_velocities_){
+				std::cout << "[NO-plan-akp; last_pose_command_ = Spose(); reactive=Cperson_abstract::Reactive_atractive;] (after get_best_planned_pose) last_pose_command_.v="<<last_pose_command_.v<< std::endl;
+			}
+
+			result = false;
+			result_=false;
+		}
+	} // fin else-if akp companion
+
+
+	pose_command = last_pose_command_; // nueva pose de trayectoria calculada.
+
+	//std::cout << "(output robot_plan_companion2) last_pose_command_.print():"<< std::endl;
+	//std::cout << " pose_command.v"<<pose_command.v<< std::endl;
+	//std::cout << " pose_command.w"<<pose_command.w<< std::endl;
+
+	// *** Return robot max vel to really máx velocity ***/
+	return_max_velocity_systemRobot_to_max_value();
+
+	//robot_plan_companion2_end = clock();
+	//	std::cout << "[ms] (FIN) time_tot=robot_plan_companion2_start-robot_plan_companion2_end="<<((robot_plan_companion2_end-robot_plan_companion2_start)/clocks_per_sec_my_var_)*1000<< std::endl;
+
+	// FIN computational time!!!
+
+	//Crobot* robot_act=robot_; // de momento lo quito porque no quiero guardar estos datos. Pero van bien para analizar cosas a veces.
+	/*if((save_results_on_files_for_robot_)&&(sim)&&(no_save_cost_and_dist_)){
+		evaluate_costs_printToMatlab(robot_act);
+		evaluate_change_distance_and_angle_companion_with_beta_change();
+	}*/
+
+	before_initial_robot_spoint_=initial_robot_spoint_;
+	time_ant_=actual_time_;
+
+	planner_iterations_++;
+	SpointV perosn_comp_act;
+	perosn_comp_act=actual_person_Companion_SpointV_;
+
+	// FOR simulation!!! // Si la persona esta parada! NO se su orientación => me quedo en la posición actual.
+	if((perosn_comp_act.v()<0.09)&&(sim)){ //for simulation if the group stops due to other people, stop with V=0, not with v=0.05 or soemthing similar to move all the time.
+			//std::cout << "!!!!!! 33333 777777 !!!!! entro en SIM set vel a 0.0!!! (perosn_comp_act.v()<0.09)&&(sim); perosn_comp_act.v()="<<perosn_comp_act.v()  << std::endl;
+		pose_command=robot_initial_pose_;
+		pose_command.v=0.0;
+		pose_command.w=0.0;
+	}
+
+	//Spoint actual_dest_p1=Spoint(actual_person_Companion_pointer_->get_best_dest().x,actual_person_Companion_pointer_->get_best_dest().y,actual_person_Companion_pointer_->get_best_dest().time_stamp);
+
+	robot_plan_companion2_end_ITER = clock();
+	unsigned int clock_start= (unsigned int) robot_plan_companion2_start_ITER;
+	unsigned int clock_end= (unsigned int) robot_plan_companion2_end_ITER;
+	save_computational_time_value_=(clock_end-clock_start)/1000000;//(((unsigned int)robot_plan_companion2_end_ITER-(unsigned int)robot_plan_companion2_start_ITER)/1000000);
+
+	//std::cout << "out robot_plan_companion2"  << std::endl;
+	if(see_std_out_velocities_){
+		std::cout << "final-> robot_plan_companion2; pose_command.v="  << pose_command.v<< "; pose_command.w="<<pose_command.w<< std::endl;
+	}
+
+	return result;
+}
+
+void Cplan_local_nav::set_robot_external_goal( const Spoint& goal )
+{
+	//std::cout << " (function: set_robot_external_goal) [Cperson_abstract::Akp_planning/Reactiva_repulsive/Reactive_atractive] (set_robot_external_goal) "<< std::endl;
+	Sdestination dest_goal(0,goal.x,goal.y,1.0);
+	extern_robot_goal_=dest_goal;
+	if(robot_companion_case_==Cperson_abstract::Akp_planning){
+		goal_ = goal;
+	}
+}
+
+void Cplan_local_nav::set_robot_external_goal_fix( const Spoint& goal )
+{ // guardar siempre SOLO el goal externo sin cambiarlo cuando vas hacia la persona pq te alejaste demasiado.
+	Sdestination dest_goal(0,goal.x,goal.y,1.0);
+	extern_robot_goal_fix_=dest_goal;
+	//std::cout << "[function: set_robot_external_goal_fix] extern_robot_goal_fix_.x="<<extern_robot_goal_fix_.x<<"; extern_robot_goal_fix_.y="<<extern_robot_goal_fix_.y << std::endl;
+}
+
+void Cplan_local_nav::set_robot_goal( const Spoint& goal )
+{
+	//std::cout " (function: set_robot_goal) [ robot_->set_best_dest]  (set_robot_goal)"<<std::endl;
+	Sdestination dest_goal(0,goal.x,goal.y,1.0);
+	std::vector<Sdestination> robot_dest(1,dest_goal);
+	robot_->set_destinations( robot_dest );
+	robot_->set_best_dest( dest_goal );
+
+	if((robot_companion_case_==Cperson_abstract::Reactive_atractive)||(robot_companion_case_==Cperson_abstract::Reactiva_repulsive)){
+		goal_ = goal;
+	}
+}
+
+// esta al usar al robot, he de cambiarla.
+void Cplan_local_nav::set_robot_goal_person_companion_akp( const Spoint& goal )
+{
+	//	std::cout << std::endl  << std::endl  << std::endl<< " [Cperson_abstract::Akp_planning/Reactiva_repulsive/Reactive_atractive]  (set_robot_goal) "<< std::endl;
+	Sdestination dest_goal(0,goal.x,goal.y,1.0);
+	std::vector<Sdestination> robot_dest(1,dest_goal);
+	person_companion_->set_destinations( robot_dest );
+	person_companion_->set_best_dest( dest_goal );
+	if((robot_companion_case_==Cperson_abstract::Reactive_atractive)||(robot_companion_case_==Cperson_abstract::Reactiva_repulsive)){
+		goal_ = goal;
+	}
+}
+
+void Cplan_local_nav::set_robot_goal_person_goal_global_plan( const Spoint& goal )
+{
+	goal_ = goal;
+	Sdestination dest_goal(0,goal.x,goal.y,1.0);
+	std::vector<Sdestination> robot_dest(1,dest_goal);
+	person_companion_->set_destinations( robot_dest );
+	person_companion_->set_best_dest( dest_goal );
+}
+
+
+void Cplan_local_nav::set_robot_goal_person_goal_global_plan_IN_robot_VERSION( const Spoint& goal )
+{
+	//std::cout << "[set_robot_goal_person_goal_global_plan_IN_robot_VERSION]"<< std::endl;
+	goal2_ = goal;
+	Sdestination dest_goal(0,goal.x,goal.y,1.0);
+	std::vector<Sdestination> robot_dest(1,dest_goal);
+	robot_->set_destinations( robot_dest );
+	robot_->set_best_dest( dest_goal );
+}
+
+/*****************************************************************************************************/
+
+bool Cplan_local_nav::init_robot_plan2_return_initial_position()
+{
+	// clear all data structures and push back the initial state : current state
+	if(debug_real_test_companion4_){
+		std::cout << "[INIIIIIIIII] horizon_time_="<<horizon_time_ << std::endl;
+		std::cout << "[INIIIIIIIII] max_iter_=***"<<max_iter_ << std::endl;
+		std::cout << "[INIIIIIIIII] workspace_radii_=***"<<workspace_radii_<< std::endl;
+	}
+
+	vector_of_companion_collisions_.clear();
+
+	v_alpha_.clear();
+	v_gamma_.clear();
+	v_delta_.clear();
+    edge_.clear();
+    cost_int_forces_.clear();
+    cost_robot_.clear();
+    cost_obstacles_.clear();
+    cost_distance_.clear();
+    cost_orientation_.clear();
+    nodes_in_branch_.clear();
+    random_goals_.clear();
+    random_goals2_.clear();
+    edge_.push_back( Sedge_tree(0) );
+    cost_int_forces_.push_back( 0.0 );
+    cost_robot_.push_back(0.0);
+    cost_obstacles_.push_back( 0.0 );
+    cost_distance_.push_back( 0.0 );
+    cost_orientation_.push_back( 0.0 );
+    cost_past_traj_.resize( max_iter_ , 0.0 );//no clear is needed, only the i-th iteration end of branch will contain a value
+    nodes_in_branch_.push_back(1.0);
+
+    cost_companion_.clear();  // new cost companion, added.
+    cost_companion_.push_back( before_initial_cost_ );
+    orientation_person_robot_angles_.clear();  // new variable, orientation companion
+
+    parent_index_vector_.clear();
+    parent_index_vector_.push_back( 0 );
+    min_distance_collision_vector_.clear();
+    min_distance_collision_vector_.push_back(4);
+
+	Sdestination robot_goal_positivo_; // caso lejos de la persona, ir al goal del lado de la persona!!!
+	Sdestination robot_goal_negativo_;
+    std::vector<double> distances_collision_goal_positive;
+    std::vector<double> distances_collision_goal_negative;
+
+    //Correct current robot location and state: hypothesis the current robot state and the past robot action may differ.
+    //So we must take into account the delay and the system platform control, specially for the w, and propagate an estimation
+    //of the state at time t_now to time t_now + t_processing and sending command
+    robot_->correct_state_to_delay( last_pose_command_,  now_ , dt_ );
+    robot_->clear_planning_trajectory();//first pose is inserted in the trajectory vector
+
+    // ----------------------------------------------------------------------
+    // set navigation goals
+    workspace_radii_ = horizon_time_ * robot_->get_v_max(); //approximation of the max distance
+
+    local_v_goal_tolerance_ = robot_->get_v_max()+0.1; //sets the tolerance to the max value to always return true
+	reaching_goal_ = false;
+	if (workspace_radii_ > robot_->get_current_pointV().distance( goal_) )
+	{
+		local_goal_ = goal2_;
+		if( robot_->get_current_pointV().distance2( goal_) < xy_2_goal_tolerance_*2.0 )
+		{
+			//the velocity tolerance depends on the parameter, and the algorithm will check this condition in order to consider
+			//a goal properly reached
+			local_v_goal_tolerance_ = v_goal_tolerance_;
+			reaching_goal_ = true;
+		}
+
+	}
+	else
+	{
+		// In this case the final velocity is not an issue because the plan will never reach it final goal
+		Spoint current_point_ac=Spoint(robot_->get_current_pose().x,robot_->get_current_pose().y,robot_->get_current_pose().time_stamp);
+		Spoint diff = goal2_ - current_point_ac;
+
+		double r = sqrt(diff.x*diff.x +  diff.y * diff.y);
+		local_goal_ = Spoint( robot_->get_current_pose().x + workspace_radii_ * diff.x / r ,
+						robot_->get_current_pose().y + workspace_radii_ * diff.y / r );
+	}
+
+	//std::cout << " [robot] local_goal_.x= "<<local_goal_.x << "; local_goal_.y= "<< local_goal_.y<<"; goal_.x= "<<goal_.x << "; goal_.y= "<< goal_.y<< std::endl;
+
+    robot_->set_rnd_local_goal( local_goal_ );
+    //now_ = set when updated
+
+    // ----------------------------------------------------------------------
+    //select persons considered in the scene and reserve memory for planning
+    Spoint robot_position = (Spoint) robot_->get_current_pointV(); //ojo! esta es más interna todabia! (hay que cambiarla tambien)
+    double d_ini,d_end,radii_2(workspace_radii_*workspace_radii_),d_min(1e10);
+
+    nearby_person_list_.clear();
+    for( auto iit: person_list_ )
+    {
+    	//It requires prior trajectory prediction, careful...
+    	d_ini = iit->get_prediction_trajectory()->front().distance2( robot_position );
+    	d_end = iit->get_prediction_trajectory()->back().distance2( robot_position );
+    	if(( d_ini < radii_2 || d_end < radii_2 ))
+    	{
+    		nearby_person_list_.push_back( iit );
+    		iit->clear_planning_trajectory();
+    		iit->reserve_planning_trajectory( max_iter_ );//if already of this size, does nothing
+
+    	}else if(iit->get_id()==id_person_companion_){
+    		nearby_person_list_.push_back( iit );
+    		iit->clear_planning_trajectory();
+    		iit->reserve_planning_trajectory( max_iter_ );//if already of this size, does nothing
+    	}
+
+    	//check for the neares obstacle in order to calculate velocities
+    	if ( d_ini < d_min)
+    		d_min = d_ini;
+
+    }
+
+    // ----------------------------------------------------------------------
+    //number of nearby obstacles: first approach, just count them...
+    clock_t obstacles_start, obstacles_end;
+    obstacles_start=clock();
+    int number_of_obstacles(0);
+    for( auto iit: laser_obstacle_list_ )
+    {
+    	//d_ini = iit.distance2( robot_position );
+    	d_ini = iit.distance( robot_position );
+    	//std::cout << " radii_2= "<< radii_2<< " d_ini="<<d_ini << std::endl;
+
+    	if( d_ini < radii_2  )
+    	{
+    		number_of_obstacles++;
+    	}
+    	if ( d_ini < d_min )
+    		d_min = d_ini;
+
+    }
+    number_of_obstacles_=number_of_obstacles;
+
+    obstacles_end=clock();
+    if(check_execution_times_){
+    	std::cout << " number_of_obstacles= "<< number_of_obstacles << std::endl;
+    	std::cout << " time_obstacles_=obstacles_start-obstacles_end= "<< ((obstacles_end-obstacles_start)/clocks_per_sec_my_var_)*1000<< std::endl;
+    }
+    d_min_global_=d_min;
+    dmin_global_=d_min;
+
+    Sdestination person_companion_position;
+	person_companion_position = Sdestination(0, actual_goal_to_return_initial_position_.x ,actual_goal_to_return_initial_position_.y,1.0);
+    set_robot_goal(person_companion_position);
+
+  // Spoint robot_act_goal=get_robot_goal();
+
+  /***************Fin copia seguridad velocidades*******************************/
+    //depending on the number of persons considered, set the std of the std_goal_workspace [rad]
+
+   int obstacles=(int)nearby_person_list_.size()-1 + number_of_obstacles;
+
+   if(obstacles<0){
+	   obstacles=0;
+   }
+
+    switch( obstacles )
+    {
+      case 0:
+		std_goal_workspace_ = 0.4;
+		break;
+      case 1:
+		std_goal_workspace_ = 1.0;
+		break;
+      case 2:
+		std_goal_workspace_ = 1.4;
+		break;
+      case 3:
+		std_goal_workspace_ = 1.6;
+		break;
+      case 4:
+		std_goal_workspace_ = 1.8;
+		break;
+      case 5:
+		std_goal_workspace_ = 2.0;
+		break;
+      default:
+      case 6:
+  		std_goal_workspace_ = 2.1;
+  		break;
+    }
+
+
+    return true;
+}
+
+
+//////////////////////////
+
+bool Cplan_local_nav::init_robot_plan2(double robot_person_distance)
+{
+
+	if(output_screen_messages_){
+	std::cout << "[IN] if init_robot_plan2 "<< std::endl;
+	std::cout << "[IN] robot_person_proximity_distance_="<<robot_person_proximity_distance_<<"; robot_person_proximity_tolerance_="<<robot_person_proximity_tolerance_<<"; offset_atractive_="<<offset_atractive_<< std::endl;
+	}
+
+	robot_dest_for_companion_status_=Sdestination();
+	init_plan_approach_side_=false;
+	use_persoon_companion_to_calculate_plan_=false;
+
+	// INI clear all data structures and push back the initial state : current state
+	vector_of_companion_collisions_.clear();
+	v_alpha_.clear();
+	v_gamma_.clear();
+	v_delta_.clear();
+    edge_.clear();
+    cost_int_forces_.clear();
+    cost_robot_.clear();
+    cost_obstacles_.clear();
+    cost_distance_.clear();
+    cost_orientation_.clear();
+    nodes_in_branch_.clear();
+    random_goals_.clear();
+    //random_goals_p1_.clear();
+    //random_goals_p2_.clear();
+    random_goals2_.clear();
+    edge_.push_back( Sedge_tree(0) );
+    cost_int_forces_.push_back( 0.0 );
+    cost_robot_.push_back(0.0);
+    cost_obstacles_.push_back( 0.0 );
+    cost_distance_.push_back( 0.0 );
+    cost_orientation_.push_back( 0.0 );
+    cost_past_traj_.resize( max_iter_ , 0.0 );//no clear is needed, only the i-th iteration end of branch will contain a value
+    nodes_in_branch_.push_back(1.0);
+
+    cost_companion_.clear();  // new cost companion, added. (ely)
+    cost_companion_.push_back( before_initial_cost_ );
+    orientation_person_robot_angles_.clear();  // new variable, orientation companion (ely)
+    final_min_collision_distance_vector_.clear();
+    real_robot_person_distance2_vector_.clear();
+
+    parent_index_vector_.clear();
+    parent_index_vector_.push_back( 0 );
+    min_distance_collision_vector_.clear();
+    min_distance_collision_vector_.push_back(4);
+
+   robot_->clear_Zanlungo_vectors(); // hace falta. se usa tambien para el side-by-side de 1 persona.
+
+    //Correct current robot location and state: hypothesis the current robot state and the past robot action may differ.
+    //So we must take into account the delay and the system platform control, specially for the w, and propagate an estimation
+    //of the state at time t_now to time t_now + t_processing and sending command
+    robot_->correct_state_to_delay( last_pose_command_,  now_ , dt_ );
+
+	Sdestination robot_goal_positivo_; // caso lejos de la persona, ir al goal del lado de la persona!!!
+	Sdestination robot_goal_negativo_;
+    std::vector<double> distances_collision_goal_positive;
+    std::vector<double> distances_collision_goal_negative;
+    double angle;
+
+    if(pointer_to_person_companion_->get_current_pointV().v()>0.2){
+    	before_vp1_module_=pointer_to_person_companion_->get_current_pointV().v();
+    }
+
+
+    // Case One person Accompaniment.
+	mean_people_velocity_=pointer_to_person_companion_->get_current_pointV().v();
+
+
+    bool case_akp_plann=true;
+    bool case_stop=false;
+
+    // Case comapnion. CASES AKP-plan or not.
+
+    if((robot_person_distance<(robot_person_proximity_distance_+robot_person_proximity_tolerance_+offset_atractive_)&&(robot_person_distance>0.5))){
+    	 //std::cout << " (casos dist) my model!!!  case_akp_plann=true; "<< std::endl;
+    	case_akp_plann=true;
+    }else{
+    	//std::cout << " (casos dist) my model!!!  case_akp_plann=false; "<< std::endl;
+    	case_akp_plann=false;
+    }
+
+    // INICIO parte para controlar el frenado suave.
+	Sdestination actual_p1_dest;
+
+	if(we_have_pointer_to_first_person_){
+		//std::cout << " case_stop_1_person"<< std::endl;
+		actual_p1_dest=pointer_to_person_companion_->get_best_dest();
+		if((((pointer_to_person_companion_->get_current_pointV().v()<0.2)||(pointer_to_person_companion_->get_current_pointV().distance(Spoint(actual_p1_dest.x,actual_p1_dest.y,actual_p1_dest.time_stamp))<0.3)))&&(robot_->get_current_pointV().distance(pointer_to_person_companion_->get_current_pointV())<dist_to_approach_static_people_)){ //&&(robot_->get_current_pointV().distance(pointer_to_person_companion_->get_current_pointV())<5)
+			//std::cout << " case_stop_1_person 2; before_vp1_module_="<<before_vp1_module_<< std::endl;
+			if(before_vp1_module_>0.2){
+				//std::cout << " case before_vp1_module_"<< " case_akp_plan_por_freanado_=false; case_akp_plann=false"<< std::endl;
+		    	case_akp_plan_por_freanado_=false;
+		    	case_akp_plann=false; //OJO, antes era true.
+			}else{
+				//std::cout << " case NO before_vp1_module_"<< std::endl;
+		    	case_akp_plan_por_freanado_=true;
+		    	case_akp_plann=false; // Caso no tengo goal final anterior.
+			}
+			//std::cout << " case_stop_1_person 5"<< std::endl;
+			case_stop=true;
+		}else{  // aquí tendría que usar goal final, lateral de la persona.
+			//std::cout << " case_stop_1_person 6"<< std::endl;
+			case_akp_plan_por_freanado_=false;
+		}
+		// FIN case One Person Accompaniment.
+	}else{
+		// to do not breack, when we do not have pointer to any person.
+		//std::cout << " case_stop_1_person 8 out of escope"<< std::endl;
+	}
+
+	//std::cout << " case_stop="<<case_stop<<"; case_akp_plann="<<case_akp_plann<< std::endl;
+	// FIN parte para controlar el frenado suave.
+
+    // INI case: go to the person GOAL!
+    if(case_akp_plann){
+    		//std::cout << " (init_planinng) case: go to the person GOAL!; companion_same_person_goal_="<<companion_same_person_goal_<< std::endl;
+    	/* INI parte conflictiva, cambio pose robot al centro entre persona y robot == Side-by-side only one person. */
+
+    	double angle1=atan2(robot_initial_pose_.y-companion_person_position_.y , robot_initial_pose_.x-companion_person_position_.x);
+    	Spose centro_person_robot2=robot_initial_pose_;
+    	SpointV_cov centro_robot_spoint2=initial_robot_spoint_;
+
+    	centro_person_robot2.x=companion_person_position_.x+(robot_person_distance/2)*cos(angle1);//+ theta1);
+    	centro_person_robot2.y=companion_person_position_.y+(robot_person_distance/2)*sin(angle1);//+ theta1);
+    	centro_robot_spoint2.x=companion_person_position_.x+(robot_person_distance/2)*cos(angle1);//+ theta1);
+    	centro_robot_spoint2.y=companion_person_position_.y+(robot_person_distance/2)*sin(angle1);//+ theta1);
+    	centro_person_robot2.v=companion_person_position_.v();
+ 		double person_orientation=calc_person_companion_orientation();
+		centro_person_robot2.theta=person_orientation;
+
+
+		// Case side-by-side (robot pose.)
+		robot_->set_current_pose(robot_initial_pose_);
+		robot_->set_current_pointV(initial_robot_spoint_);
+		//std::cout << " robot_-.x="<<robot_->get_current_pose().x <<"; robot.y="<<robot_->get_current_pose().y<< std::endl;
+	    robot_->clear_planning_trajectory();//first pose is inserted in the trajectory vector (IMPORTANT. THIS initialization has to be here! after the change of the initial robot position!)
+
+		center_of_the_group_=centro_person_robot2;  // es para caso side-by-side
+    	/* FIN parte conflictiva, cambio pose robot al centro entre persona y robot*/
+
+    	Spose robot=robot_->get_current_pose();
+
+    	/* INI Calcular el goal_final=robot!  */
+    	// INI calcular nuevo angulo theta con orientación dirección movimiento persona.
+    	//Cperson_abstract* person_obj;
+    	//find_person(id_person_companion_ , &person_obj);
+    	SpointV_cov person = actual_person_Companion_pointer_->get_current_pointV();
+
+    	// INI
+    	double theta=calc_person_companion_orientation();
+    	saved_actual_theta_person_=theta;
+
+    	if(theta<0){
+    		theta=2*3.14+theta;
+    	}
+    	orientacion_persona_actual_=theta;
+
+    	person_x_ant_=actual_person_Companion_pointer_->get_best_dest().x;// variables guardadas de cara a guardar datos en txt
+    	person_y_ant_=actual_person_Companion_pointer_->get_best_dest().y;
+    	// FIN calcular nuevo angulo theta con orientación dirección movimiento persona.
+
+    	angle=atan2(robot.y-person.y , robot.x-person.x);
+
+    	Spoint goal_robot;
+    	Sdestination person_companion_goal;
+
+    	if(companion_same_person_goal_){
+    		person_companion_goal=actual_person_Companion_destination_;
+    		person_companion_goal_=actual_person_Companion_destination_;
+    	}else{
+    		person_companion_goal=extern_robot_goal_;
+    		person_companion_goal_=extern_robot_goal_;
+    	}
+
+    	double angle2=angle;
+    	if(angle2<0){
+    		angle2=((360*3.14)/180)+angle2;
+    	}
+
+    	// Calc real distance between person and robot or robot and the center of the group to calculate the traslated goal in front of the robot.
+    	//double person_robot_distance_real=initial_robot_spoint_.distance(initial_person_companion_point_);
+    	//double robot_distance_to_center_of_group=initial_robot_spoint_.distance(center_of_the_group_Zanlungo_);
+
+    	robot_person_proximity_goals_x_=robot_person_proximity_distance_;
+    	robot_person_proximity_goals_y_=robot_person_proximity_distance_;
+
+    	// calculate final-destination of the robot if the person is moving or not.
+    	//Spoint get_act_dest=Spoint(actual_person_Companion_pointer_->get_best_dest().x,actual_person_Companion_pointer_->get_best_dest().y,actual_person_Companion_pointer_->get_best_dest().time_stamp);
+    	//Spoint comp_pers_g=Spoint(person_companion_goal.x,person_companion_goal.y,person_companion_goal.time_stamp);
+
+    	if(pointer_to_person_companion_->get_current_pointV().v()>0.2){
+    		bool_out_init_pose_robot_=true; // initial state, the robot has to remain in the same position until people starts to move.
+    	}
+
+    	double new_angle_companion;
+		if(companion_same_person_goal_){  // ini if-companion same person goal.
+
+			// CASE side-by-side.
+    		new_angle_companion=angle_companion_;
+
+    		robot_person_proximity_goals_x_=robot_person_proximity_distance_;  // de momento, fijo. Mobil esta distancia da más problemas que soluciones.
+    		robot_person_proximity_goals_y_=robot_person_proximity_distance_;
+
+			if( diffangle(theta, angle2) < 0 ){  // se corresponde con los dos laterales del robot, respecto a las personas.
+        		double new_angle_now=theta+new_angle_companion*3.14/180;
+        		if(new_angle_now>360){
+        			new_angle_now=new_angle_now-360;
+        		}
+        		goal_robot=Spoint(person_companion_goal.x+robot_person_proximity_goals_x_*cos(new_angle_now),person_companion_goal.y+robot_person_proximity_goals_y_*sin(new_angle_now),person_companion_goal.time_stamp);
+        	}else{  //fin if diff-angle.
+        		double new_angle_now=theta-new_angle_companion*3.14/180;
+        		if(new_angle_now>360){
+        			new_angle_now=new_angle_now-360;
+        		}
+        		goal_robot=Spoint(person_companion_goal.x+robot_person_proximity_goals_x_*cos(new_angle_now),person_companion_goal.y+robot_person_proximity_goals_y_*sin(new_angle_now),person_companion_goal.time_stamp);
+        	} // fin-if diff angle.
+
+		}else{  //else of if-companion same person goal.
+			//std::cout << " (va directamente hacia la persona) GOAL lateral persona "<< std::endl;
+			goal_robot=Spoint(person_companion_goal.x,person_companion_goal.y,person_companion_goal.time_stamp);
+			if(debug_real_test_companion2_){
+				std::cout << "(goal_robot 2); person_companion_goal.x= "<< person_companion_goal.x << "; person_companion_goal.y="<<person_companion_goal.y<<"; person_companion_goal.time="<<person_companion_goal.time_stamp<< std::endl;
+			}
+		} //fin else of if-companion same person goal.
+
+		companion_person_position_=actual_person_Companion_SpointV_;
+
+//std::cout <<"GOAL dynamic; we_have_pointer_to_first_person_="<<we_have_pointer_to_first_person_<< "; pointer_to_person_companion_->get_current_pointV().v()="<<pointer_to_person_companion_->get_current_pointV().v()<<"change_dynamic_goal_1_person_sidebyside_="<<change_dynamic_goal_1_person_sidebyside_<<std::endl;
+
+		if(we_have_pointer_to_first_person_){
+			// Case Only One person accompaniment.
+if(((pointer_to_person_companion_->get_current_pointV().v()>0.2))){ //&&(change_dynamic_goal_1_person_sidebyside_)
+			//if(((robot_->get_current_pointV().v()>0.3)&&(pointer_to_person_companion_->get_current_pointV().v()>0.2))&&(change_dynamic_goal_1_person_sidebyside_)){ // si el robot esta andando. centramos el goal, usando su orientación.
+				// calculando la intersección perpendicular entre la recta de la dirección del movimiento del robot y el punto correspondiente al goal.
+				double m;
+				m=mean_people_Zalungo_.vy/mean_people_Zalungo_.vx;
+
+				//m=pointer_to_person_companion_->get_current_pointV().vy/pointer_to_person_companion_->get_current_pointV().vx;
+
+
+
+				//double x_center_group_people=(initial_person_companion_point_.x)/number_of_group_people_;
+				//double y_center_group_people=(initial_person_companion_point_.y)/number_of_group_people_;
+
+				double mp=-1/m;
+
+				double b=robot_->get_current_pose().y-m*robot_->get_current_pose().x;
+				double bp=person_companion_goal.y-mp*person_companion_goal.x;
+
+				double x_d=(bp-b)/(m-mp);
+				double y_d=m*x_d+b;
+
+				Spoint new_goal_robot;
+				new_goal_robot.x=x_d;
+				new_goal_robot.y=y_d;
+
+				Spoint peroson_goal_act=Spoint(pointer_to_person_companion_->get_best_dest().x,pointer_to_person_companion_->get_best_dest().y,pointer_to_person_companion_->get_best_dest().time_stamp);
+
+				if(peroson_goal_act.distance(new_goal_robot)<metros_al_dynamic_goal_Vform_){ // distancia menor que 5m del goal real para brl, para fme entorno ha de ser 3m, depende de la distancia que haya entre los goals y los obstaculos del entorno perpendiculares al movimiento de la gente hacia el goal.
+					goal_robot.x=new_goal_robot.x;
+					goal_robot.y=new_goal_robot.y;
+					//std::cout <<"IN menor distancia limite"<<std::endl;
+				}else{
+					goal_robot.x=peroson_goal_act.x;
+					goal_robot.y=peroson_goal_act.y;
+				}
+
+//std::cout <<"GOAL dynamic; m="<<m<< "; goal_robot.x="<<goal_robot.x<<"goal_robot.y="<<goal_robot.y<<std::endl;
+				//std::cout << " x_d="<<x_d<<"; (bp-b)="<<(bp-b)<<"; (m-mp)="<<(m-mp)<<"; y_d="<<y_d <<" mp="<<mp<<"; m="<<m<<"; robot_->get_current_pose().x="<<robot_->get_current_pose().x<<"; robot_->get_current_pose().y="<<robot_->get_current_pose().y<< std::endl;
+				//std::cout << "(Calculo goal con orientacion del robot) goal_robot.x= "<<goal_robot.x <<"; goal_robot.y="<<goal_robot.y<<"; metros_al_dynamic_goal_Vform_="<<metros_al_dynamic_goal_Vform_<<"; new_goal_robot.x="<<new_goal_robot.x<<"; new_goal_robot.y="<<new_goal_robot.y<< std::endl;
+			}
+		}
+
+
+		if(!change_dynamically_finaldest_){ // if true, we change the final dest dynamically, if false we do not change it
+			goal_robot=Spoint(pointer_to_person_companion_->get_best_dest().x,pointer_to_person_companion_->get_best_dest().y,pointer_to_person_companion_->get_best_dest().time_stamp);
+		}
+
+		// INI calcular goal final caso frenado.
+		//SpointV new_prop_p1_to_goal=calcular_goal_frenado();
+		// FIN calcular goal final caso frenado.
+
+		if(we_have_pointer_to_first_person_){
+			// INI Case Only One person accompaniment.
+			//std::cout << " In last goal people stop 2 "<< std::endl;
+			if((pointer_to_person_companion_->get_current_pointV().v()<0.2)){
+				if(output_screen_messages_){
+				std::cout << " fist pers IMPORTANTE ENTRO EN FRENAR!!! "<< std::endl;
+				} //TODO: el frenado "lento" para el caso de 1 persona NO acaba de ir bien. De momento lo dejo en comentario
+				//  intentar en un futuro si puedo hacerlo bien.
+				//goal_robot.x=new_prop_p1_to_goal.x;//before_person_comp_goal_p1_.x;//robot_->get_current_pointV().x; //este goal ha de pasar a ser el goal calculado para frenar.
+				//goal_robot.y=new_prop_p1_to_goal.y;//before_person_comp_goal_p1_.y;//robot_->get_current_pointV().y;
+			}
+			// FIN Case Only One person accompaniment.
+		}
+
+		// To evitate errors of dynamic destinations just in a other people position.
+		// SI la destinacion cae "encima" de una de las personas, dale destinacion posicion encima del robot.
+		if(we_have_pointer_to_first_person_){    //PENSAR SI SE VA O SI SE KEDA.
+			if(((pointer_to_person_companion_->get_current_pointV().distance(goal_robot)<0.5))&&(pointer_to_person_companion_->get_current_pointV().v()<0.2)){
+				//goal_robot.x=robot_->get_current_pointV().x;
+				//goal_robot.y=robot_->get_current_pointV().y;
+			}
+		}
+
+		// para clacular las variables globales de before_person_comp_goal_p1_ y etc
+		//double theta_p1=calc_person_companion_orientation_v2(pointer_to_person_companion_);
+
+		//Spoint person_position_frenar=Spoint(person_companion_position.x,person_companion_position.y,robot_->get_current_pointV().time_stamp);
+
+		if(case_stop){ //last_good_theta_person_p1_
+
+			//if((before_person_comp_goal_p1_.x==0)&&(before_person_comp_goal_p1_.y==0)){
+			//	before_person_comp_goal_p1_.x=initial_person_companion_point_.x+0.5;
+			//	before_person_comp_goal_p1_.y=initial_person_companion_point_.y+0.5;
+			//}
+
+			if(we_have_pointer_to_first_person_){
+				//Ini Only one person accompaniment
+				//std::cout << "(case-stop-akp) before_person_comp_goal_p1_.x="<<before_person_comp_goal_p1_.x<<"; before_person_comp_goal_p1_.y="<<before_person_comp_goal_p1_.y<< std::endl;
+				if(robot_->get_current_pointV().v()>0.2){
+					std::cout << " STOP with robot vel > 0.2"<< std::endl;
+					goal_robot=Spoint(before_person_comp_goal_p1_.x,before_person_comp_goal_p1_.y,robot_->get_current_pointV().time_stamp);
+				}else{
+					goal_robot=robot_->get_current_pointV();
+					std::cout << " STOP with robot vel < 0.2"<< std::endl;
+				}
+				//Ini Only one person accompaniment
+			}
+
+		}
+
+		// initial case. robot goal is its actual position to do not move.
+		if(initial_goal_case_bool_){ // OK, este esta bien!!! Estar parado, hasta que no se hacia donde van las personas!
+			//std::cout << " !!!!!!!!!! initial_goal_case_bool_"<< std::endl;
+			goal_robot=robot_->get_current_pointV();
+		}
+
+		//std::cout << " (ini_robot_plan2) CASE akp_planning!!!! initial_goal_case_bool_="<<initial_goal_case_bool_<<"; goal_robot.x="<<goal_robot.x<<"; goal_robot.y="<<goal_robot.y<< std::endl;
+
+		//goal_robot.x=new_prop_p1_to_goal.x;//before_person_comp_goal_p1_.x;//robot_->get_current_pointV().x; //este goal ha de pasar a ser el goal calculado para frenar.
+		//goal_robot.y=new_prop_p1_to_goal.y; // es para test del goal de frenado.
+		 //std::cout << " (set_robot_external_goal caso3) goal_robot.x="<<goal_robot.x<<"goal_robot.y="<<goal_robot.y<< std::endl;
+
+		 set_robot_external_goal(goal_robot); // FINALLY, we set the value of the robot_goals!
+		set_robot_goal(goal_robot);
+		robot_dest_for_companion_status_=Sdestination(0,goal_robot.x,goal_robot.y,1);
+    	// FIN update robot goal
+
+		// calculate the initial companion angle for the side_by_side companion of only one person.
+		calculate_initial_companion_angle_case_side_by_side_one_person(theta,angle2);
+
+    }else{ // FIN CASE akp-plann
+
+    }
+
+    // ----------------------------------------------------------------------
+    // set navigation goals
+    workspace_radii_ = horizon_time_ * max_v_by_system_;//robot_->get_v_max(); //approximation of the max distance
+
+    local_v_goal_tolerance_ = robot_->get_v_max()+0.1; //sets the tolerance to the max value to always return true
+	reaching_goal_ = false;
+
+	if (workspace_radii_ > robot_->get_current_pointV().distance( goal_) )
+	{
+		local_goal_ =extern_robot_goal_;
+		// New mode entering subgoals, the radius should be the maximum
+		if( robot_->get_current_pointV().distance2( goal_) < xy_2_goal_tolerance_*2.0 )
+		{
+			//the velocity tolerance depends on the parameter, and the algorithm will check this condition in order to consider
+			//a goal properly reached
+			local_v_goal_tolerance_ = v_goal_tolerance_;
+			reaching_goal_ = true;
+		}
+
+	}
+	else
+	{
+		// In this case the final velocity is not an issue because the plan will never reach it final goal
+		Spoint current_point_ac=Spoint(robot_->get_current_pose().x,robot_->get_current_pose().y,robot_->get_current_pose().time_stamp);
+		Spoint diff = extern_robot_goal_ - current_point_ac;
+
+		double r = sqrt(diff.x*diff.x +  diff.y * diff.y);
+		local_goal_ = Spoint( robot_->get_current_pose().x + workspace_radii_ * diff.x / r ,
+						robot_->get_current_pose().y + workspace_radii_ * diff.y / r );
+
+		 if((extern_robot_goal_.x==current_point_ac.x)&&(extern_robot_goal_.y==current_point_ac.y)){
+			 local_goal_=Spoint( robot_->get_current_pose().x ,
+						robot_->get_current_pose().y );
+		 }
+
+	}
+
+    robot_->set_rnd_local_goal( local_goal_ );
+    //now_ = set when updated
+
+    // ----------------------------------------------------------------------
+    //select persons considered in the scene and reserve memory for planning
+    Spoint robot_position = (Spoint) robot_->get_current_pointV(); //ojo! esta es más interna todabia! (hay que cambiarla tambien)
+    double d_ini,d_end,radii_2(workspace_radii_*workspace_radii_),d_min(1e10);
+
+    nearby_person_list_.clear();
+
+    min_obstacle_dist_obst_dynamic_=1000; //is for the planner statusl
+    if(!person_list_.empty()){
+    	min_obstacle_dist_obst_dynamic_=person_list_.front()->get_prediction_trajectory()->front().distance(robot_position);
+    }
+
+    for( auto iit: person_list_ )
+    {
+    	//It requires prior trajectory prediction, careful...
+    	if(iit->get_id()==id_person_companion_){
+    		d_ini = iit->get_prediction_trajectory()->front().distance2( robot_position );
+    		d_end = iit->get_prediction_trajectory()->back().distance2( robot_position );
+
+    	}else{
+        	d_ini = iit->get_prediction_trajectory()->front().distance2( robot_position );
+        	d_end = iit->get_prediction_trajectory()->back().distance2( robot_position );
+    	}
+
+    	if(( d_ini < radii_2 || d_end < radii_2 ))
+    	{
+    		nearby_person_list_.push_back( iit );
+    		iit->clear_planning_trajectory();
+    		iit->reserve_planning_trajectory( max_iter_ );//if already of this size, does nothing
+    	}
+
+    	//check for the neares obstacle in order to calculate velocities
+    	if(iit->get_id()==id_person_companion_){
+    		// people companion are not obstacles.
+    	}else{
+        	if ( d_ini < d_min){
+        		d_min = d_ini;
+        	}
+        	if((sqrt(d_ini)) < min_obstacle_dist_obst_dynamic_){
+        		min_obstacle_dist_obst_dynamic_=sqrt(d_ini);
+        	}
+    	}
+
+    }
+
+	// ----------------------------------------------------------------------
+	//number of nearby obstacles: first approach, just count them...
+    clock_t obstacles_start, obstacles_end;
+    obstacles_start=clock();
+    int number_of_obstacles(0);
+    int number_of_obstacles2(0);
+    double dist_between_obstacles;
+    Spoint before_obstacle;
+    bool first_case=true;
+
+	min_obstacle_dist_obst_static_=1000; // is for the planner status
+    if(!laser_obstacle_list_.empty()){
+        min_obstacle_dist_obst_static_=laser_obstacle_list_.front().distance(robot_position);
+    }
+
+
+
+    for( auto iit: laser_obstacle_list_ )
+    {
+    	d_ini = iit.distance2( robot_position );
+
+    	if( d_ini < radii_2  )
+    	{
+    		///// START real number of obstacles.
+    		if( d_ini < 1.0  )//calculates basic distance to robot, near distance, the thr is lower
+    			{
+    				if(sim_){
+    					dist_between_obstacles = 0.2; // 0.2 m
+    					//std::cout << "REAL CASE if(d2 < 1.0) => dist_thr = 0.2=sqrt(0.2) = 0.45[m] is considered obstacle"<< std::endl;
+    				}else{ // real robot case.
+    					dist_between_obstacles = 0.33;//sqrt(0.09) = 0.33[m] is considered obstacle
+    					//std::cout << "REAL CASE if(d2 < 1.0) => dist_thr = 0.2=sqrt(0.2) = 0.45[m] is considered obstacle"<< std::endl;
+    				}
+
+    			}else if( d_ini < 4.0 )//4 [m]
+				{
+					if(sim_){
+						dist_between_obstacles = 0.2; //
+						//std::cout << "SIM CASE if(d2 < 16.0) => dist_thr = 0.01=sqrt(0.01) = 0.1[m] is considered obstacle"<< std::endl;
+					}else{ // real robot case.
+						dist_between_obstacles = 0.33;//sqrt(0.16) = 0.45[m] is considered obstacle
+						//std::cout << "REAL CASE if(d2 < 16.0) => dist_thr = 0.2=sqrt(0.2) = 0.45[m] is considered obstacle"<< std::endl;
+					}
+					//dist_thr = 0.2;//4 [m]=sqrt(0.16) = 0.45[m] is considered obstacle
+				}else if( d_ini < 9.0 )//4 [m]
+				{
+					if(sim_){
+						dist_between_obstacles = 0.2; //
+						//std::cout << "SIM CASE if(d2 < 16.0) => dist_thr = 0.01=sqrt(0.01) = 0.1[m] is considered obstacle"<< std::endl;
+					}else{ // real robot case.
+						dist_between_obstacles = 0.33;//sqrt(0.16) = 0.45[m] is considered obstacle
+						//std::cout << "REAL CASE if(d2 < 16.0) => dist_thr = 0.2=sqrt(0.2) = 0.45[m] is considered obstacle"<< std::endl;
+					}
+					//dist_thr = 0.2;//4 [m]=sqrt(0.16) = 0.45[m] is considered obstacle
+				}
+    			else if( d_ini < 16.0 )//4 [m]
+    			{
+    				if(sim_){
+    					dist_between_obstacles = 0.2; //
+    					//std::cout << "SIM CASE if(d2 < 16.0) => dist_thr = 0.01=sqrt(0.01) = 0.1[m] is considered obstacle"<< std::endl;
+    				}else{ // real robot case.
+    					dist_between_obstacles = 0.33;//sqrt(0.16) = 0.45[m] is considered obstacle
+    					//std::cout << "REAL CASE if(d2 < 16.0) => dist_thr = 0.2=sqrt(0.2) = 0.45[m] is considered obstacle"<< std::endl;
+    				}
+    				//dist_thr = 0.2;//4 [m]=sqrt(0.16) = 0.45[m] is considered obstacle
+    			}
+    			else
+    			{
+    				if(sim_){
+    					dist_between_obstacles = 0.2;
+    					//std::cout << "SIM CASE if(d2 > 16.0) => dist_thr = 0.01=sqrt(0.01) = 0.1[m] is considered obstacle"<< std::endl;
+    				}else{ // real robot case.
+    					dist_between_obstacles = 0.33;//sqrt(0.09) = 0.6[m] is considered obstacle
+    					//std::cout << "REAL CASE if(d2 > 16.0) => dist_thr = 0.36=sqrt(0.09) = 0.6[m] is considered obstacle"<< std::endl;
+    				}
+    				//std::cout << "else => dist_thr =  0.36=sqrt(0.09) = 0.6[m] is considered obstacle"<< std::endl;
+    			}
+    		//// END real number of obstacles
+
+    		if((iit.distance( before_obstacle )>dist_between_obstacles)&&(!first_case)){
+    			number_of_obstacles++;
+    		}
+    		Spoint robot_ac=robot_->get_current_pointV();
+    		if(((iit.distance( before_obstacle )>dist_between_obstacles)&&(!first_case))&&(iit.distance(robot_ac)<max_distance_to_obstacles_detected_)){
+    		    number_of_obstacles2++;
+    		}
+    		before_obstacle=iit;
+    		first_case=false;
+
+    	}
+    	if ( d_ini < d_min ){
+    		d_min = d_ini;
+    		//std::cout << " stat-obst (init_robot_plan2) d_min= "<< d_min << std::endl;
+    	}
+    	if((sqrt(d_ini))<min_obstacle_dist_obst_static_){ // this is for companion status.
+    		min_obstacle_dist_obst_static_=sqrt(d_ini);
+    	}
+
+    }
+    number_of_obstacles_=number_of_obstacles;
+    obstacles_end=clock();
+   // std::cout << " (ROBOT COMPANION) number_of_obstacles= "<< number_of_obstacles_ <<"; dist_between_obstacles="<<dist_between_obstacles<<"; nearby_obstacle_list_.size()="<<nearby_obstacle_list_.size()<< std::endl;
+    if(check_execution_times_){
+    	std::cout << " number_of_obstacles= "<< number_of_obstacles << std::endl;
+    	std::cout << " time_obstacles_=obstacles_start-obstacles_end= "<< ((obstacles_end-obstacles_start)/clocks_per_sec_my_var_)*1000<< std::endl;
+    }
+    d_min_global_=d_min;
+    dmin_global_=d_min;
+
+
+    if(case_akp_plann){
+    	// case: go to the person GOAL! (case akp) [We need to separate these two if-els cases because we need the midle section for all the coda and for this other part.]
+    	//std::cout << " NO go to goal.  Allways akp_plan. (init_planinng) Case: far of the person position, go to the person!"<< std::endl;
+    }else{
+
+    	init_plan_approach_side_=true; // CAse go to the goal of the side of the person.
+    	robot_->clear_planning_trajectory();//first pose is inserted in the trajectory vector
+    	// case reactive or repulsive (The final goal of the robot is to aproach to the person side)
+    	// para el reactive el goal es ir a la persona a la que sigues!
+
+    	use_persoon_companion_to_calculate_plan_=false;//true; //Now yo do not whant to collite with the person companion in the path, in near side-by-side the path some times have to cross this person, but not in the case to initialize or arribe to the group that is far from the robot)
+    	vel_robot_normal(d_min); // limitar velocidad si obstaculos o personas muy cerca para el caso repulsive y atractive
+
+
+    	// Caso No central == One person accompaniment
+    	//std::cout << " (init_planinng) No central; order="<<order_people_in_group_global_variable_[0]<< std::endl;
+		// Desde aquí INI calculate lateral goals when robot is far! (case 2 people or one people)
+		Spose robot=robot_->get_current_pose();
+		//Spoint robot_point=robot_->get_current_pointV();
+
+		/* INI pose central robot, desde donde se tendria que calcular la colision para el goal del robot. */
+		double angle1=atan2(robot_initial_pose_.y-companion_person_position_.y , robot_initial_pose_.x-companion_person_position_.x);
+		Spose centro_person_robot2=robot_initial_pose_;
+		SpointV_cov centro_robot_spoint2=initial_robot_spoint_;//initial_robot_spoint_;
+
+		// calculate group centre for 1-Robot and 1-people side-by-side
+		centro_person_robot2.x=companion_person_position_.x+(robot_person_proximity_distance_/2)*cos(angle1);//+ theta1);
+		centro_person_robot2.y=companion_person_position_.y+(robot_person_proximity_distance_/2)*sin(angle1);//+ theta1);
+		centro_robot_spoint2.x=companion_person_position_.x+(robot_person_proximity_distance_/2)*cos(angle1);//+ theta1);
+		centro_robot_spoint2.y=companion_person_position_.y+(robot_person_proximity_distance_/2)*sin(angle1);//+ theta1);
+		centro_person_robot2.v=companion_person_position_.v();
+
+		int index_act=1; // instante_actual
+		bool colision=false;
+		colision=check_collision_companion_reactive_atractive_goal(centro_robot_spoint2, index_act ); // check the collisions with the group.
+		double final_min_colision_distance=robot_person_companion_distance_;
+
+		if(colision){
+			final_min_colision_distance=min_distance_collision_; // = distance entre robot y obstaculo más cercano.
+			// std::cout << " final_min_colision_distance=min_distance_collision_"<<min_distance_collision_ << std::endl;
+		}else{
+			// std::cout << " No collision. final_min_colision_distance=robot_person_companion_distance_"<<robot_person_companion_distance_ << std::endl;
+			final_min_colision_distance=robot_person_companion_distance_;
+		}
+
+		double angle_goal;
+		if((final_min_colision_distance==((robot_person_proximity_distance_+2*robot_->get_platform_radii()+little_augmented_collision_margin_)/2))||(final_min_colision_distance>((robot_person_proximity_distance_+2*robot_->get_platform_radii()+little_augmented_collision_margin_)/2))){
+			angle_goal=angle_companion_;
+		}else{
+			if(overpas_obstacles_behind_person_){
+				double division;
+				//double radi_force_influence=0.91;
+
+				division=((final_min_colision_distance/2)-robot_->get_platform_radii()-(little_augmented_collision_margin_/2))/((robot_person_distance+2*robot_->get_platform_radii()+little_augmented_collision_margin_)/2);
+
+				if(division>1){
+					division=1;
+				}else if(division<(-1)){
+					division=-1;
+				}
+				angle_goal=(180/3.14)*(asin(division)); //prueba 1.
+
+				angle_goal=180-angle_goal;
+
+				if(angle_goal<angle_companion_){
+					angle_goal=angle_companion_;
+				}
+
+			}else{
+
+				double division;
+				//double radi_force_influence=0.91;
+				division=((final_min_colision_distance/2)-robot_->get_platform_radii()-(little_augmented_collision_margin_/2))/((robot_person_distance+2*robot_->get_platform_radii()+little_augmented_collision_margin_)/2);
+
+				if(division>1){
+					division=1;
+				}else if(division<(-1)){
+					division=-1;
+				}
+				angle_goal=(180/3.14)*(asin(division));
+				if(angle_goal>angle_companion_){
+					angle_goal=angle_companion_;
+				}
+				if(debug_gazebo_journal2_){
+					std::cout <<"; division="<<division<< std::endl;
+				}
+			}
+		}
+
+		/* FIN pose central robot, desde donde se tendria que calcular la colision para el goal del robot. */
+
+		// ini calcular nuevo angulo theta con orientación dirección movimiento persona.
+		//Cperson_abstract* person_obj;
+		//find_person(id_person_companion_ , &person_obj);
+
+		SpointV_cov person;
+		person= actual_person_Companion_pointer_->get_current_pointV();
+
+		double theta=calc_person_companion_orientation();
+
+		// fin calcular nuevo angulo theta con orientación dirección movimiento persona.
+		angle=atan2(robot.y-person.y , robot.x-person.x);
+
+		//Spoint get_act_dest=Spoint(actual_person_Companion_pointer_->get_best_dest().x,actual_person_Companion_pointer_->get_best_dest().y,actual_person_Companion_pointer_->get_best_dest().time_stamp);
+		robot_goal_positivo_=Sdestination(0, person.x ,person.y,1.0);
+
+		robot_goal_positivo_.x += (robot_person_proximity_distance_)*cos(theta + angle_goal*3.14/180);
+		robot_goal_positivo_.y += (robot_person_proximity_distance_)*sin(theta + angle_goal*3.14/180);
+
+		robot_goal_negativo_=Sdestination(0, person.x ,person.y,1.0);
+
+		robot_goal_negativo_.x += (robot_person_proximity_distance_)*cos(theta - (angle_goal*3.14/180));//1.5*cos(theta - (angle_companion_*3.14/180));
+		robot_goal_negativo_.y += (robot_person_proximity_distance_)*sin(theta - (angle_goal*3.14/180));//1.5*sin(theta - (angle_companion_*3.14/180));
+
+		//bool robot_goal_positiv=false;
+		double act_angle_comp;
+		if( diffangle(theta, angle) < 0 ){
+			act_angle_comp=(theta+angle)*180/3.14;
+			act_angle_comp=true;
+		}else{
+			act_angle_comp=(theta-angle)*180/3.14;
+		}
+
+		orientation_person_robot_angles_.push_back( act_angle_comp );
+		orientation_person_robot_angles_.push_back( angle_companion_ );
+
+		final_min_collision_distance_vector_.push_back( 0.0 );
+		final_min_collision_distance_vector_.push_back( 0.0 );
+		real_robot_person_distance2_vector_.push_back( 0.0 );
+		real_robot_person_distance2_vector_.push_back( 0.0 );
+
+		Spoint Spoint_pose_command=Spoint(robot_goal_positivo_.x,robot_goal_positivo_.y);
+		double min_dist_colli_p;
+		min_dist_colli_p=check_collision_companion_goal(Spoint_pose_command,0);
+
+		Spoint_pose_command=Spoint(robot_goal_negativo_.x,robot_goal_negativo_.y);
+		double min_dist_colli_n;
+		min_dist_colli_n=check_collision_companion_goal(Spoint_pose_command,0); // ya te da la distancia minima
+
+		unsigned int case_act=0;
+		if((min_dist_colli_p<robot_person_companion_distance_)&&(min_dist_colli_n==robot_person_companion_distance_)){// hay distancia de colision solo en el caso positivo
+			case_act=1;
+		}else if((min_dist_colli_n<robot_person_companion_distance_)&&(min_dist_colli_p==robot_person_companion_distance_)){ // hay distancia de colision solo en el caso negativo
+			case_act=2;
+		}else if((min_dist_colli_p!=robot_person_companion_distance_)&&(min_dist_colli_n!=robot_person_companion_distance_)){ // hay distancia de colision en ambos casos
+			case_act=3;
+		}
+
+		//find_person(id_person_companion_ , &person_obj);
+		person = actual_person_Companion_pointer_->get_current_pointV();
+
+		Sdestination person_companion_position;
+		person_companion_position = Sdestination(0, person.x ,person.y,1.0);
+
+		switch( case_act)
+		{
+			case 1: //case best the robot_goal_negativo_
+				person_companion_position=robot_goal_negativo_;//.x += (robot_person_proximity_distance_)*cos(theta + angle_companion_*3.14/180);
+				// std::cout << " [case1](robot_goal_negativo_) (ROBOT GOAL) person_companion_position.x=" <<person_companion_position.x<< " person_companion_position.y=" <<person_companion_position.y<< std::endl;
+			break;
+			case 2: //case best the robot_goal_positivo_
+				person_companion_position=robot_goal_positivo_;//.x += (robot_person_proximity_distance_)*cos(theta - (angle_companion_*3.14/180));
+				//std::cout << " [case2](robot_goal_positivo_) (ROBOT GOAL) person_companion_position.x=" <<person_companion_position.x<< " person_companion_position.y=" <<person_companion_position.y<< std::endl;
+			break;
+			case 3:
+				if(min_dist_colli_n>min_dist_colli_p){ //case best the robot_goal_negativo_
+					person_companion_position=robot_goal_negativo_;
+					//std::cout << " [case_act=3] min_dist_colli_n= "<< min_dist_colli_n <<"; > min_dist_colli_p="<<min_dist_colli_p<< std::endl;
+					//std::cout << " [case3](robot_goal_negativo_) (ROBOT GOAL) person_companion_position.x=" <<person_companion_position.x<< " person_companion_position.y=" <<person_companion_position.y<< std::endl;
+				}else{ //case best the robot_goal_positivo_
+					person_companion_position=robot_goal_positivo_;
+					//std::cout << " [case_act=3] min_dist_colli_n= "<< min_dist_colli_n <<"; < min_dist_colli_p="<<min_dist_colli_p<< std::endl;
+					//std::cout << " [case3](robot_goal_positivo_) (ROBOT GOAL) person_companion_position.x=" <<person_companion_position.x<< " person_companion_position.y=" <<person_companion_position.y<< std::endl;
+				}
+
+			break;
+			default:
+			case 0:
+			// std::cout << " [case_act=0 or default] min_dist_colli_p= "<< min_dist_colli_p<<"; min_dist_colli_n="<<min_dist_colli_n << std::endl;
+				// darle el goal al lado de la persona.
+				if( diffangle(theta, angle) < 0 )
+				{
+					person_companion_position=robot_goal_positivo_;
+					//std::cout << " (robot_goal_positivo_) case reactive (1) (ROBOT GOAL) person_companion_position.x=" <<person_companion_position.x<< " person_companion_position.y=" <<person_companion_position.y<< std::endl;
+				}
+				else  // es como si hiciera una circunferencia al rededor de la persona. (se intenta poner a un lado o a otro de la persona. segun esos angulos)
+				{
+					person_companion_position=robot_goal_negativo_;
+					// std::cout << " (robot_goal_negativo_) case reactive (2) (ROBOT GOAL)  person_companion_position.x=" <<person_companion_position.x << " person_companion_position.y=" <<person_companion_position.y<< std::endl;
+				}
+				break;
+			}
+
+
+			 // new part to do not collide the goal, to arribe static.
+			 double min_colision_goal_positivo=10;
+			 double min_colision_goal_negativo=10;
+			 for( auto iit: laser_obstacle_list_ )
+			 {
+			    // double collision_dist=person_companion_position.distance(iit->get_current_pointV());
+			    double  collision_goal_positivo=robot_goal_positivo_.distance(iit);
+			    double  collision_goal_negativo=robot_goal_negativo_.distance(iit);
+
+			    if(collision_goal_positivo<min_colision_goal_positivo){
+			    	min_colision_goal_positivo=collision_goal_positivo;
+			    }
+			    if(collision_goal_negativo<min_colision_goal_negativo){
+			    	min_colision_goal_negativo=collision_goal_negativo;
+			    }
+			 }
+
+			 for( auto iit: person_list_ )
+			 {
+
+				 //std::cout << " (2) person_companion_position.x=" <<person_companion_position.x << " person_companion_position.y=" <<person_companion_position.y<< std::endl;
+				 //std::cout << " (2) iit->.id=" <<iit->get_id() << " collision_dist=" <<collision_dist<<"; threshold_collision_dist_stop_pers_goal_="<<threshold_collision_dist_stop_pers_goal_<< std::endl;
+				 // std::cout << " (2) robot_goal_negativo_.distance(person_companion_position)=" <<robot_goal_negativo_.distance(person_companion_position) << " robot_goal_positivo_.distance(person_companion_position)=" <<robot_goal_positivo_.distance(person_companion_position)<< std::endl;
+				 if(iit->get_id()!=id_person_companion_){
+					 double  collision_goal_positivo=robot_goal_positivo_.distance(iit->get_current_pointV());
+			    	 double  collision_goal_negativo=robot_goal_negativo_.distance(iit->get_current_pointV());
+
+			    	 if(collision_goal_positivo<min_colision_goal_positivo){
+			    	 	min_colision_goal_positivo=collision_goal_positivo;
+			    	 }
+			    	 if(collision_goal_negativo<min_colision_goal_negativo){
+			    	 	min_colision_goal_negativo=collision_goal_negativo;
+			    	 }
+
+
+				 }
+
+
+			 }
+
+			 double dif_goals=sqrt((min_colision_goal_positivo-min_colision_goal_negativo)*(min_colision_goal_positivo-min_colision_goal_negativo));
+
+			 if((dif_goals>0.5)&&((min_colision_goal_positivo<1.5)||(min_colision_goal_positivo<1.5))){
+				 if(min_colision_goal_positivo>min_colision_goal_negativo){
+					 //std::cout << " tot 1 min_colision_goal_positivo="<<min_colision_goal_positivo<< "; min_colision_goal_negativo="<<min_colision_goal_negativo<< std::endl;
+			    	 person_companion_position=robot_goal_positivo_;
+				 }else{
+					 //std::cout << " tot 2 min_colision_goal_positivo="<<min_colision_goal_positivo<< "; min_colision_goal_negativo="<<min_colision_goal_negativo<< std::endl;
+					 person_companion_position=robot_goal_negativo_;
+				 }
+			 }
+
+			 if((min_colision_goal_positivo<threshold_collision_dist_stop_pers_goal_)&&(min_colision_goal_negativo<threshold_collision_dist_stop_pers_goal_)){
+			    person_companion_position=Sdestination(0,robot_->get_current_pointV().x,robot_->get_current_pointV().y,1);
+			    //std::cout << "final min_colision_goal_negativo="<<min_colision_goal_negativo<< "; min_colision_goal_positivo="<<min_colision_goal_positivo<< std::endl;
+			    obstacle_very_close_no_path_=true;
+			 }
+
+			 Spoint person_position_frenar=Spoint(person_companion_position.x,person_companion_position.y,robot_->get_current_pointV().time_stamp);
+			 SpointV robot_pointV=robot_->get_current_pointV();
+				//std::cout << " (Person GOAL) case_stop= "<< case_stop << "; dist="<<robot_pointV.distance(person_position_frenar)<<"; vel="<<robot_->get_current_pointV().v()<< std::endl;
+
+			 bool frenar_now=false;
+			 if((robot_pointV.distance(person_position_frenar)<distance_frenar_sideBySide_when_person_is_stop_)){
+				frenar_now=true;
+				//std::cout << " (Person GOAL) frenar_now= "<< frenar_now << "; dist<1.5="<<robot_pointV.distance(person_position_frenar)<<"; vel="<<robot_->get_current_pointV().v()<< std::endl;
+			 }
+
+			 if((case_stop)&&(frenar_now)&&(robot_->get_current_pointV().v()<0.2)){
+				// std::cout << " !!!!!!!!!! Case frenar_NOW!"<< std::endl;
+			 	 SpointV new_prop_p1_to_goal=robot_->get_current_pointV();//=calcular_goal_frenado();
+				 person_companion_position=Sdestination(0,new_prop_p1_to_goal.x,new_prop_p1_to_goal.y,1.0); // Siempre uso el de frenado en aproximate o alejate de la persona!
+			 }
+
+			 // initial case. robot goal is its actual position to do not move.
+			 if(initial_goal_case_bool_){ // OK, este esta bien!!! Estar parado, hasta que no se hacia donde van las personas!
+			 	person_companion_position=Sdestination(0,robot_->get_current_pointV().x,robot_->get_current_pointV().y,1.0);
+			 }
+
+			 //goal robot es al lado del goal inferido de la persona, para el caso person companion!!!
+			 set_robot_external_goal(person_companion_position);  // FINALLY, set the final goal for these cases.
+			 set_robot_goal(person_companion_position);
+
+			 robot_dest_for_companion_status_=person_companion_position;
+			 // Desde aquí FIN calculate lateral goals when robot is far! (case 2 people or one people)
+			 // CASO NO CENTRAL, 1 person comp o lateral.
+    }
+
+
+	//depending on the number of persons considered, set the std of the std_goal_workspace [rad]
+	int obstacles=number_of_obstacles2;
+	for( auto iit: nearby_person_list_ )
+	{
+		Spoint robot_point=robot_->get_current_pointV();
+		Spoint act_person=iit->get_current_pointV();
+		//std::cout <<"; id_person="<<iit->get_id()<< " robot_point.distance(act_person)="<<robot_point.distance(act_person)<<"; max_distance_to_obstacles_detected_="<<max_distance_to_obstacles_detected_<< std::endl;
+		if(robot_point.distance(act_person)<max_distance_to_obstacles_detected_){
+			obstacles=obstacles+1;
+		}
+	}
+
+	// remove the accompanied people from the conter of obstacles, for the std of the random goals of the local window, because the std-enlarges-with-the-number-of-obstacles.
+	if(we_have_pointer_to_first_person_){ // the robot do not take into account the accompanied people to the std_value for aboid obstacles.
+		obstacles=obstacles-1;
+		//std::cout << " (we have first person comp) obstacles="<<obstacles<< std::endl;
+	}
+	//std::cout << " obstacles="<<obstacles<< std::endl;
+
+	if(obstacles<0){ // problema al restar numero de personas companion.
+	   obstacles=0;
+	}
+
+	switch( obstacles )
+    {
+      case 0: // explanation: in central-Side-by-side I need less std to scape from local minima, because the robot was more protected with the people to get between two obstacles or something similar.
+    	  	 // And a bigest std do more oscilations due to "big" random changes on the random_goal without any near obstacle.
+    		 std_goal_workspace_ = 0.2; // He rebajado las anteriores std que usaba gonzalo ya que con 0 obstaculos no tienes minimos, vas recto y en general eran demasiado altas.
+		break;
+      case 1:
+    		  std_goal_workspace_ = 0.3;
+		break;
+      case 2:
+    		  std_goal_workspace_ = 0.4;
+		break;
+      case 3:
+    		  std_goal_workspace_ = 0.5;
+		break;
+      case 4:
+    		  std_goal_workspace_ = 0.6;
+		break;
+      case 5:
+    		std_goal_workspace_ = 0.7;
+		break;
+      default:
+      case 6:
+    		  std_goal_workspace_ = 0.8;  // with the companion, we do not have a lot of obstacles sorrunding the robot. Then we reduce the std respect to the case of the robot navigating alone.
+  		break;
+    }
+
+	//std::cout << "case 0 :  alpha_="<<alpha_ <<"; gamma_="<<gamma_<<"; delta_"<<delta_<< std::endl;
+
+	if( sqrt(dmin_global_) < 2.0){  //If we have obstacles very near, augment the std of the workspace to get out.
+		std_goal_workspace_ = 1.1;
+		//std::cout << " INIT ROBOT PLAN2 STD!!! std_goal_workspace_= "<<std_goal_workspace_<<"; obst mas cerca de 1m ; sqrt(dmin_global_)=" <<sqrt(dmin_global_)<<  std::endl;
+	}
+
+ //  std::cout << " fin robot paln2- function " <<  std::endl;
+
+    return true;
+}
+
+
+/* fin Ini_robot companion, para caso person companion akp*/
+bool Cplan_local_nav::robot_plan_anticipative_krrt_companion( Cperson_abstract::companion_reactive reactive)
+{
+	//	std::cout << "ATR COMPANION! (1) INI robot_plan_anticipative_krrt_companion"<< std::endl;
+
+	person_robot_actual_real_distance_=calc_robot_person_companion_distance();
+
+	//clock_t krrt_companion_start, krrt_companion_end;
+	//krrt_companion_start=clock();
+	//clock_t bucle_krrt_companion_start, bucle_krrt_companion_end;
+	//bucle_krrt_companion_start=clock();
+
+	Sedge_tree input(0);
+	Spoint random_goal;
+	unsigned int parent_vertex_index(0),previous_parent_vertex(0), number_of_cost_to_go(0);
+	//first goal is the main goal
+	random_goal = local_goal_;
+
+	random_goals_.push_back( random_goal );
+	collision_detected_.resize(max_iter_,false);
+	collision_detected_[0] = false;
+	double distance2_to_goal(1.0);
+
+	for( unsigned int i = 1; i<max_iter_ && number_of_cost_to_go < max_iter_/5; ++i )
+	{
+		distance2_to_goal = robot_->get_robot_planning_trajectory()->back().distance2( local_goal_ );
+		if((distance2_to_goal  < xy_2_goal_tolerance_)&&(robot_->get_robot_planning_trajectory()->back().v < local_v_goal_tolerance_ )){
+			dist_to_goal_test_=distance2_to_goal;
+			v_to_goal_test_=robot_->get_robot_planning_trajectory()->back().v;
+		}
+
+		if( robot_->get_robot_planning_trajectory()->back().time_stamp - now_ > horizon_time_ ||
+				collision_detected_[i-1]  ||
+				(distance2_to_goal  < xy_2_goal_tolerance_ && //distance to goal and velocity
+						robot_->get_robot_planning_trajectory()->back().v < local_v_goal_tolerance_ )) //this local tolerance depends if the goal is reachable before h
+		{
+			//std::cout << "COLLISIONS and out of the window and near of goal IS THIS CASE!!! "<< std::endl;
+			// add end of branch, either horizon time is reached or goal is reached
+			if (!collision_detected_[i-1] )
+			{
+				end_of_branches_index_.push_back( i-1 );
+				//distance to last trajectory calculated: only last calculated position in both paths... simplified version
+				if ( best_planning_trajectory_.empty() ){
+					cost_past_traj_[i-1] = 0.0;
+				}
+				else{
+					cost_past_traj_[i-1] = best_planning_trajectory_.front().distance2( robot_->get_robot_planning_trajectory()->back()  );
+				}
+			}
+			//colisions are discarted
+			else {
+				//std::cout << "propagate_vertex; collision_detected_[i]="<<collision_detected_[i-1]<< std::endl;
+			  //std::cout << " collision detected kkt" << std::endl;
+			}
+			//2- Sample workspace
+			random_goal = sample_workspace();
+			random_goals_.push_back( random_goal );
+		    robot_->set_rnd_local_goal( random_goal );
+		    //3- find nearest vertex to expand the tree
+			parent_vertex_index = find_nearest_vertex( random_goal ); // dentro de esta esta el cost_to_go!!!
+			++number_of_cost_to_go;
+			previous_parent_vertex = i;
+			reset_scene_persons_propagation_flag();
+		}
+		else
+		{
+			//std::cout << "  NO NO NO NO collision detected kkt" << std::endl;
+			test_distance_to_goal_=distance2_to_goal;
+			parent_vertex_index = previous_parent_vertex;
+			previous_parent_vertex = i;
+		}
+
+		//check to see if goal is currently the local goal; in that case, the velocity has to be adjusted to stop
+		if ( reaching_goal_ )
+		{
+			//set robot desired velocity to 0 when inside the goal ball
+			if( distance2_to_goal < xy_2_goal_tolerance_ )
+			{
+				robot_->set_desired_velocty( 0.0 );
+			}
+			else // robot desired velocity is again max velocity
+				robot_->set_desired_velocty( robot_->get_v_max () );//robot always tries to be at max velocity, modified by nearby ppl
+		}
+		//4-calculate the input u to propagate towards the random goal
+		random_goals2_.push_back(random_goal);
+
+		input = calculate_edge(parent_vertex_index, Sdestination(0,random_goal.x,random_goal.y),reactive);
+		//5- Propagate tree.
+		collision_detected_[i] = propagate_vertex( parent_vertex_index, input ,reactive);
+
+		if(!collision_detected_[i]){
+			// si no hay colision!!! guardas los parametros!
+			v_alpha_.push_back(alpha_);
+			v_gamma_.push_back(gamma_);
+			v_delta_.push_back(delta_);
+		}
+		//std::cout << "antes de calculate_cost"<< std::endl;
+		calculate_cost(parent_vertex_index, input,reactive);
+		//std::cout << "despues de calculate_cost"<< std::endl;
+		//6-calculate the propagation cost
+		edge_.push_back( input );
+	}
+
+	//bucle_krrt_companion_end=clock();
+
+	//7- return tree. Seek for the minimum cost branch of the tree
+
+	Crobot* robot_act=robot_;
+	unsigned int min_branch_index = global_min_cost_index( robot_act , reactive );
+
+	double ideal_companion_angle_in_the_next_instant_of_time;
+	if(calc_goal_companion_with_group_path_){
+		ideal_companion_angle_in_the_next_instant_of_time=orientation_person_robot_angles_[1];
+	}else{
+		ideal_companion_angle_in_the_next_instant_of_time=orientation_person_robot_angles_with_prediction_of_person_companion_[1];
+	}
+	ideal_companion_goal_in_the_next_instant_of_time_=calculate_companion_goal_stable_if_person_stop(ideal_companion_angle_in_the_next_instant_of_time);
+
+	if(reactive==Cperson_abstract::Akp_planning){
+		if(overpas_obstacles_behind_person_){
+			go_behind_robot_only_angle_in_final_tree_calculate_companion_path_angle_and_cost3(min_branch_index);
+
+		}else{
+			go_in_front_robot_only_angle_in_final_tree_calculate_companion_path_angle_and_cost3(min_branch_index);
+		}
+
+	}
+
+	// Esta luego habra que comentar el IF para ver que los angulos en el path los haga bien.
+	if((reactive==Cperson_abstract::Akp_planning)&&(we_have_cost_companion_) && debug_real_test_companion4_){//&&(actual_debug2_)){
+		see_companion_path_angle_and_cost_of_min_cost_paths(min_branch_index);
+	}
+
+	// cout best candidate paths and costs
+	min_branch_index_save_=min_branch_index;
+
+	best_plan_vertex_index_.clear();
+	best_planning_trajectory_.clear();
+
+	obstacle_very_close_no_path_=false;
+
+	if ( end_of_branches_index_.empty() ){
+		//std::cout << "[cause of return false] end_of_branches_index_.empty()="<<end_of_branches_index_.empty() << std::endl;
+		obstacle_very_close_no_path_=true;
+
+		return false;
+	}
+	//8- calculate vector of vertex. In reverse order the vertexes of the best path (initial not
+	// included as it doesn't provide information)
+	unsigned int y=0;
+	do
+	{
+
+		best_plan_vertex_index_.push_back( min_branch_index);
+		best_planning_trajectory_.push_back( robot_->get_robot_planning_trajectory()->at(min_branch_index) );
+
+		min_branch_index = edge_[ min_branch_index ].parent;
+		y++;
+
+	}while( min_branch_index > 0 );
+
+
+	// clear data structures. Out of this function, there is no guarantee that data has not changed
+    //end of trajectories indexes
+    end_of_branches_index_.clear();
+	//krrt_companion_end=clock();
+
+	//evaluate_costs_printToMatlab();
+
+    return true;
+}
+
+
+Spose Cplan_local_nav::replan_last_step(unsigned int index, Spose robot_pose_ini, SpointV robot_Spoint_ini, Sdestination robot_goal, double dt)
+{ // retorna la Spose final.  // index es solo para la el angulo y para la posición de la persona, el goal de la persona.
+
+	// robot_goal es la destinacion final del robot.
+	//std::cout << "final_v_robot_max_="<<final_v_robot_max_<<"; robot_->get_des_ve="<<robot_->get_desired_velocity()<< std::endl;
+
+	unsigned int index2=1;
+
+	Sforce f_goal, f_int, f_obs, f,f_person_goal, f_goal_final;
+
+	// replan the min_cost_branch to do the planing taking into account the angle respect to the person.
+	//caso planning, normal Gonzalo.
+	robot_->set_debug_velocity_file(debug_file_robot_);
+
+	robot_->set_robot_debug_filename(debug_file_);
+	robot_->set_robot_debug_vel_file(debug_file_robot_);
+
+	robot_->set_debug_filename(debug_file_);
+
+	if(debug_file_robot_){
+		std::ofstream fileMatlab2;
+		fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+		fileMatlab2 << "% ANTES f_goal = robot_->force_goal_near\n";
+		fileMatlab2.close();
+	}
+
+	robot_path_goal_=robot_goal;
+
+	f_goal = robot_->force_goal_near( robot_goal, get_sfm_params(robot_),&robot_Spoint_ini,reduce_max_vel_dist_,go_with_vel_per_,real_vel_per);
+
+	//////// Force to maintain the angle between people and robot!
+	Sdestination person_companion_goal;
+
+	//Cperson_abstract* person_obj;
+	// find_person(id_person_companion_ , &person_obj);
+
+	 SpointV_cov person;
+	 SpointV_cov actual_current_person_comp_point=actual_person_Companion_pointer_->get_current_pointV();
+	 if((actual_current_person_comp_point.vx<0.1)&&(actual_current_person_comp_point.vy<0.1)){
+		 person =actual_current_person_comp_point;
+		// std::cout <<" entro en v person == 0"<< std::endl;
+	 }else{
+		 person = actual_person_Companion_pointer_->get_planning_trajectory()->at(index);
+		 //std::cout <<" entro en NO v person == 0"<< std::endl;
+	 }
+	person_position_t_4_=Spoint(person.x,person.y,person.time_stamp);
+
+	person_companion_goal=Sdestination(0,person.x,person.y,1.0);
+
+	SpointV_cov robot=robot_->get_planning_trajectory()->at(index);
+	Sdestination person_goal;
+	if(companion_same_person_goal_){
+		person_goal=actual_person_Companion_pointer_->get_best_dest();
+	}else{
+		person_goal=extern_robot_goal_;
+	}
+
+	SpointV_cov robot_center_traj_ideal_position=person;
+	double angle = atan2(robot.y-person.y,robot.x-person.x);
+
+    // corregir mejor orientacion con prediccion cuando la velocidad es igual a 0. pero mejor orientacion con la trajectoria anterior cuando la velocidad es mayor que 0
+   // if(debug_correct_angle_person_replan_last_step_){std::cout <<" person_tracj.size()="<<actual_person_Companion_pointer_->get_past_trajectory()->size()<< std::endl;}
+
+    double theta=calc_person_companion_orientation();
+
+    if(actual_person_Companion_pointer_->get_current_pointV().v()>0.1){
+    	last_true_person_companion_orientation_=theta;
+    }
+
+    if(debug_correct_angle_person_replan_last_step_){std::cout <<"(person) theta="<<theta*180/3.14<< std::endl;
+    std::cout << " extern_robot_goal_.x-person.x="<<extern_robot_goal_.x-person.x<<"; extern_robot_goal_.y-person.y"<<extern_robot_goal_.y-person.y << std::endl;}
+    // fin calcular nuevo angulo theta con orientación dirección movimiento persona.
+
+    double act_min_companion_angle;
+
+    if(calc_goal_companion_with_group_path_){
+    	act_min_companion_angle=orientation_person_robot_angles_[index];
+    }else{
+    	act_min_companion_angle=orientation_person_robot_angles_with_prediction_of_person_companion_[index];
+    }
+
+	next_companion_angle_save_=act_min_companion_angle;
+
+	double robot_distance_move=robot_person_proximity_distance_;
+	double angle_smothMov=angle*180/3.14;
+	if(angle_smothMov<0){
+		angle_smothMov=360+angle_smothMov;
+	}
+
+	if((angle_smothMov>=180)&&(angle_smothMov<=270)){
+		angle_smothMov=angle_smothMov-180;
+		angle_smothMov=180-angle_smothMov;
+	}else if((angle_smothMov>=270)&&(angle_smothMov<=360)){
+		angle_smothMov=angle_smothMov-180;
+	}
+
+	if((act_min_companion_angle>angle_companion_)&&(dmin_global_ < (workspace_radii_*workspace_radii_))){//&&((angle_smothMov*180/3.14)<135)){
+		robot_distance_move=robot_person_proximity_distance_-0.7*(act_min_companion_angle-90)/45;
+		//std::cout << " robot_distance_move="<<robot_distance_move<<"; robot_person_proximity_distance_="<<robot_person_proximity_distance_<<"; 0.7*(act_min_companion_angle-90)/45="<<0.7*(act_min_companion_angle-90)/45<<"; act_min_companion_angle="<<act_min_companion_angle<< std::endl;
+		if(robot_distance_move < 1.0){
+			robot_distance_move=1.0;
+			//std::cout << " robot_distance_move="<<robot_distance_move<<"; robot_person_proximity_distance_="<<robot_person_proximity_distance_<<"; 0.7*(act_min_companion_angle-90)/45="<<0.7*(act_min_companion_angle-90)/45<<"; act_min_companion_angle="<<act_min_companion_angle<< std::endl;
+		}
+	}
+
+	if(diffangle(theta,angle)<0){
+		//std::cout << " Entro en diff angle (NEGATIVO), case person goal POSITIVO"<< std::endl;
+		//std::cout << " theta="<<theta*(180/3.14)<<"; angle="<<angle*(180/3.14)<<"; act_min_companion_angle="<<act_min_companion_angle<< std::endl;
+		person_companion_goal.x+=(robot_distance_move)*cos(theta+(act_min_companion_angle*(3.14/180)));//1.5*cos(theta+(act_min_companion_angle*(3.14/180)));
+		person_companion_goal.y+=(robot_distance_move)*sin(theta+(act_min_companion_angle*(3.14/180)));//1.5*sin(theta+(act_min_companion_angle*(3.14/180)));
+	}else{
+		//std::cout << " Entro en diff angle (POSITIVO), case person goal NEGATIVO"<< std::endl;
+		//std::cout << " theta="<<theta*(180/3.14)<<"; angle="<<angle*(180/3.14)<<"; act_min_companion_angle="<<act_min_companion_angle<<std::endl;
+		person_companion_goal.x+=(robot_distance_move)*cos(theta-(act_min_companion_angle*(3.14/180)));//1.5*cos(theta-(act_min_companion_angle*(3.14/180)));
+		person_companion_goal.y+=(robot_distance_move)*sin(theta-(act_min_companion_angle*(3.14/180)));//1.5*sin(theta-(act_min_companion_angle*(3.14/180)));
+	}
+	person_companion_goal_out_=person_companion_goal;
+
+
+	/* INI calcular goal a 90 grados derecha e izquierda, para ver hacia que lado es mejor ir. */
+	if(chose_better_side_to_acompani_person_){
+		Sdestination person_companion_goal_positive=Sdestination(0,person.x,person.y,1.0);
+		Sdestination person_companion_goal_negative=Sdestination(0,person.x,person.y,1.0);
+
+		person_companion_goal_positive.x+=(robot_distance_move)*cos(theta+(90*(3.14/180)));
+		person_companion_goal_positive.y+=(robot_distance_move)*sin(theta+(90*(3.14/180)));
+
+		person_companion_goal_negative.x+=(robot_distance_move)*cos(theta-(90*(3.14/180)));
+		person_companion_goal_negative.y+=(robot_distance_move)*sin(theta-(90*(3.14/180)));
+
+		// distancia a esa posicion respecto a la del robot actual mallor que cierto margen.
+		double distance_to_side_person_positive=initial_robot_spoint_.distance(Spoint(person_companion_goal_positive.x,person_companion_goal_positive.y,person_companion_goal_positive.time_stamp));
+		double distance_to_side_person_negative=initial_robot_spoint_.distance(Spoint(person_companion_goal_negative.x,person_companion_goal_negative.y,person_companion_goal_negative.time_stamp));
+
+		bool select_side_of_the_person_to_go=false;
+		if((distance_to_side_person_positive>threshold_dintace_select_person_side_to_go_)||(distance_to_side_person_negative>threshold_dintace_select_person_side_to_go_)){
+			select_side_of_the_person_to_go=true;
+		}
+
+		// falta calcular colisiones.
+		if(select_side_of_the_person_to_go){
+			Spoint Spoint_pose_command=Spoint(person_companion_goal_positive.x,person_companion_goal_positive.y);
+			double min_dist_colli_p=check_collision_companion_goal(Spoint_pose_command,0);
+
+			Spoint_pose_command=Spoint(person_companion_goal_negative.x,person_companion_goal_negative.y);
+			double min_dist_colli_n=check_collision_companion_goal(Spoint_pose_command,0); // ya te da la distancia minima
+
+			unsigned int case_act=0;
+			if((min_dist_colli_p<robot_person_companion_distance_)&&(min_dist_colli_n==robot_person_companion_distance_)){// hay distancia de colision solo en el caso positivo
+				case_act=1;
+			}else if((min_dist_colli_n<robot_person_companion_distance_)&&(min_dist_colli_p==robot_person_companion_distance_)){ // hay distancia de colision solo en el caso negativo
+				case_act=2;
+			}else if((min_dist_colli_p!=robot_person_companion_distance_)&&(min_dist_colli_n!=robot_person_companion_distance_)){ // hay distancia de colision en ambos casos
+				case_act=3;
+			}
+
+			bool reclaculate_goal=false;
+			if((min_dist_colli_n<robot_person_companion_distance_)||(min_dist_colli_p<robot_person_companion_distance_)){
+				reclaculate_goal=true;
+			}
+
+			//Cperson_abstract* person_obj;
+			//find_person(id_person_companion_ , &person_obj);
+			person = actual_person_Companion_pointer_->get_current_pointV();
+
+			Sdestination person_companion_position;
+			person_companion_position = Sdestination(0, person.x ,person.y,1.0);
+
+			bool robot_goal_positive_bool; // if true is goal positive (+) if false es negative (-)
+
+			switch( case_act)
+			{
+				case 1: //case best the robot_goal_negativo_
+					person_companion_position=person_companion_goal_negative;//.x += (robot_person_proximity_distance_)*cos(theta + angle_companion_*3.14/180);
+					robot_goal_positive_bool=false;
+					break;
+				case 2: //case best the robot_goal_positivo_
+					person_companion_position=person_companion_goal_positive;//.x += (robot_person_proximity_distance_)*cos(theta - (angle_companion_*3.14/180));
+					robot_goal_positive_bool=true;
+					break;
+				case 3:
+					if(min_dist_colli_n>min_dist_colli_p){ //case best the robot_goal_negativo_
+						person_companion_position=person_companion_goal_negative;
+						robot_goal_positive_bool=false;
+					}else{ //case best the robot_goal_positivo_
+						robot_goal_positive_bool=true;
+						person_companion_position=person_companion_goal_positive;
+					}
+
+					break;
+				default:
+				case 0:
+					// darle el goal al lado de la persona.
+					if( diffangle(theta, angle) < 0 )
+					{
+						person_companion_position=person_companion_goal_positive;
+						robot_goal_positive_bool=true;
+					 }
+					 else  // es como si hiciera una circunferencia al rededor de la persona. (se intenta poner a un lado o a otro de la persona. segun esos angulos)
+					 {
+						person_companion_position=person_companion_goal_negative;
+						robot_goal_positive_bool=false;
+					  }
+					break;
+				}
+			// cambiar person companion goal por el positivo o negativo respecto al actual act_min_companion_angle que sacamos!
+			if(reclaculate_goal){
+				person_companion_goal=Sdestination(0,person.x,person.y,1.0);
+				if(robot_goal_positive_bool){
+					person_companion_goal.x+=(robot_distance_move)*cos(theta+(act_min_companion_angle*(3.14/180)));//1.5*cos(theta+(act_min_companion_angle*(3.14/180)));
+					person_companion_goal.y+=(robot_distance_move)*sin(theta+(act_min_companion_angle*(3.14/180)));//1.5*sin(theta+(act_min_companion_angle*(3.14/180)));
+					//std::cout << " recalculate_goal [CHANGE PERSON GOAL] Case person goal positive"<< std::endl;
+					//std::cout << " person_companion_goal.x="<<person_companion_goal.x<< std::endl;
+					//std::cout << " person_companion_goal.y="<<person_companion_goal.y<< std::endl;
+				}else{
+					person_companion_goal.x+=(robot_distance_move)*cos(theta-(act_min_companion_angle*(3.14/180)));//1.5*cos(theta-(act_min_companion_angle*(3.14/180)));
+					person_companion_goal.y+=(robot_distance_move)*sin(theta-(act_min_companion_angle*(3.14/180)));//1.5*sin(theta-(act_min_companion_angle*(3.14/180)));
+					//std::cout << " recalculate_goal [CHANGE PERSON GOAL] Case person goal negative"<< std::endl;
+					//std::cout << " person_companion_goal.x="<<person_companion_goal.x<< std::endl;
+					//std::cout << " person_companion_goal.y="<<person_companion_goal.y<< std::endl;
+				}
+			}
+		}
+	}
+
+
+	/* FIN calcular goal a 90 grados derecha e izquierda, para ver hacia que lado es mejor ir. */
+
+	if(debug_file_robot_){
+		std::ofstream fileMatlab2;
+		fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+		fileMatlab2 << "% ANTES f_person_goal=robot_->force_goal_near\n";
+		fileMatlab2.close();
+	}
+
+	robot_goal_to_person_companion_=person_companion_goal;
+
+	f_person_goal=robot_->force_goal_near(person_companion_goal,get_sfm_int_params(robot_),&robot_Spoint_ini,reduce_max_vel_dist_,go_with_vel_per_,real_vel_per);
+
+	//std::cout << " (AKP force_persons_int_planning_virtual) act_min_companion_angle="<<act_min_companion_angle<<"; parent_vertex="<<parent_vertex<< std::endl;
+
+	double goal_percent=1-person_goal_percent_;
+	f_goal_final=Sforce(goal_percent*f_goal.fx+person_goal_percent_*f_person_goal.fx,goal_percent*f_goal.fy+person_goal_percent_*f_person_goal.fy);
+
+	Crobot* new_robot_in_pose_real_robot=robot_;
+
+	new_robot_in_pose_real_robot->set_current_pointV(initial_robot_spoint_);
+	new_robot_in_pose_real_robot->set_current_pose(robot_initial_pose_);
+	std::vector<SpointV_cov> robot_traj=new_robot_in_pose_real_robot->get_planning_trajectory_vector();
+
+	//std::cout << "new_robot_in_pose_real_robot->get_planning_trajectory().size()="<<new_robot_in_pose_real_robot->get_planning_trajectory()->size()<<"; orientation_person_robot_angles_.size()="<<orientation_person_robot_angles_.size()<< std::endl;
+
+	robot_traj[index]=initial_robot_spoint_;
+	robot_traj[index2]=initial_robot_spoint_;
+	new_robot_in_pose_real_robot->set_planning_trajectory(robot_traj);
+
+	f_int = force_persons_int_planning_virtual_robot_companion_propagation(new_robot_in_pose_real_robot,index);
+
+	//map force, used for simulations
+	if( read_force_map_success_ ){
+		f_obs = get_force_map(new_robot_in_pose_real_robot->get_robot_planning_trajectory()->at(index2).x,
+				new_robot_in_pose_real_robot->get_robot_planning_trajectory()->at(index2).y);
+	}
+	if(read_laser_obstacle_success_){
+		f_obs = force_objects_laser_int_planning_virtual( new_robot_in_pose_real_robot, index2, 25.0, true );
+	}
+
+	// INICION Limitar fuerza obstaculos cuando hay muchos solapados
+	double fobos_Xmod=sqrt(f_obs.fx*f_obs.fx);
+	double fobos_Ymod=sqrt(f_obs.fy*f_obs.fy);
+	double signo_x;
+	double signo_y;
+	if (f_obs.fx > 0){
+		signo_x=1;
+	}else{
+		signo_x=-1;
+	}
+	if (f_obs.fy > 0){
+		signo_y=1;
+	}else{
+		signo_y=-1;
+	}
+
+	if((fobos_Xmod>f_obst_max_x_)||(fobos_Ymod>f_obst_max_y_)){
+		f_obs.fx=signo_x*f_obst_max_x_;
+		f_obs.fy=signo_y*f_obst_max_y_;
+	}
+	//std::cout << "(FINAL FORCE) f_obs.fx="<<f_obs.fx<<"; f_obs.fy="<<f_obs.fy<< std::endl;
+	// FIN Limitar fuerza obstaculos cuando hay muchos solapados
+
+	last_step_robot_obstacles_cost_=f_obs.module2();
+
+	f=f_goal*alpha_companion_+f_person_goal*beta_companion_ + f_int*gamma_companion_ + f_obs*delta_companion_; // prueba, con parametros articulo revista robot companion
+
+	f_goal_final=f_goal*alpha_companion_+f_person_goal*beta_companion_;
+
+	if(output_screen_messages_){
+	std::cout << " FINAL replan last-step: robot_desired_vel= "<<robot_->get_desired_velocity()<< std::endl;
+	std::cout << " (Akp_planning, fuerza final y parametros):  f.fx="<<f.fx<<"; f.fy="<<f.fy<<"; alpha_companion_="<<alpha_companion_<<"; beta_companion_"<<beta_companion_<<"; gamma_companion_="<<gamma_companion_<<"; delta_companion_="<<delta_companion_<<std::endl;//"; f_int.fx"<<f_int.fx<<"; f_int.fy="<<f_int.fy<<std::endl;
+	std::cout << " f_goal.fx= "<<f_goal.fx<<"; f_goal.fy="<<f_goal.fy << " f_person_goal.fx= "<<f_person_goal.fx<<"; f_person_goal.fy="<<f_person_goal.fy<<"; f_obs.fx="<<f_obs.fx<<"; f_obs.fy="<<f_obs.fy<<std::endl;
+	std::cout << " f_goal_final.fx= "<<f_goal_final.fx<<"; f_goal_final.fy="<<f_goal_final.fy<<"; f_int.fx="<<f_int.fx<<"; f_int.fy="<<f_int.fy << std::endl;
+	}
+
+	Sedge_tree u( index2, f, f_goal, f_int, f_obs,f_person_goal);
+	u_forces_robot_actual_=u;
+
+
+	if((person_companion_goal.distance(initial_robot_spoint_)<0.2)&&(we_have_pointer_to_first_person_)){
+		robot_->set_desired_velocty(pointer_to_person_companion_->get_current_pointV().v());
+		//std::cout << " IN set robot velocity, to person velocity, because the person_companion_goal_is_very_neat to the initial robot position."<< std::endl;
+	}
+
+	Spose robot_propagation=robot_->robot_propagation_companion_position( dt, u.f ,robot_pose_ini,false); // le doy la pose que quiero que me propague y la fuerza con la que quiero que haga la propagacion y me da la siguiente pose!
+
+	bool collision_check = check_collision_final( robot_propagation, index ); // porque las personas estaran en ese index cuando llegue el robot a esa posicion final. NO es index=1
+
+
+	final_collision_check_=collision_check;
+	//if(final_collision_check_){
+	//	std::cout << "final_collision_check_=true"<< std::endl;
+	//}else{
+	//	std::cout << "final_collision_check_=false"<< std::endl;
+	//}
+
+	Spoint robot_propagation_spoint(robot_propagation.x,robot_propagation.y,robot_propagation.time_stamp);
+
+	//double distance_to_goal_act = robot_->get_current_pointV().distance( robot_propagation_spoint);
+	//double vel_teoric= max_v_by_system_ * distance_to_goal_act / distance_to_stop_;
+	//double vel_teoric2=distance_to_goal_act/0.2;
+	//double error_in_orientation_between_person_and_robot=(person_orient_-robot_propagation.theta)*(180/3.14);
+
+
+	double distance_act_r=initial_robot_spoint_.distance(robot_propagation);
+	last_step_robot_distance_cost_=distance_act_r;
+	double dx=robot_propagation.x-initial_robot_spoint_.x;
+	double dy=robot_propagation.y-initial_robot_spoint_.y;
+	double local_orientation=fabs(diffangle( robot_initial_pose_.theta ,
+			atan2( dy, dx )  ));
+	last_step_robot_orientation_cost_local_=local_orientation;
+	dx=extern_robot_goal_.x-initial_robot_spoint_.x;
+	dy=extern_robot_goal_.y-initial_robot_spoint_.y;
+	double global_orientation_=fabs(diffangle( robot_initial_pose_.theta ,
+			atan2( dy, dx )  ));
+	last_step_robot_orientation_cost_global_=global_orientation_;
+	last_step_robot_control_cost_mix_=f_goal_final.module2(cost_angular_);
+	last_step_robot_control_cost_goal_traj_=f_goal.module2(cost_angular_);
+	last_step_robot_control_cost_goal_person_=f_person_goal.module2(cost_angular_);
+
+	final_combined_goal_=robot_propagation;
+
+	// Para evitar que el robot se mueva cuando esta muy cerca del goal!!!
+	double distance_to_goal_companion=person_companion_goal.distance(initial_robot_spoint_);
+
+	if(((distance_to_goal_companion<0.3)&&(actual_current_person_comp_point.vx<0.1)&&(actual_current_person_comp_point.vy<0.1))&&(sim)) {
+		//std::cout << "!!!!!!!! 333333333 777777777777 !!!!!!! replan_last_step companion case v=0; w=0; (d<0.3m)distance_to_goal_companion="<<distance_to_goal_companion<<"; (vx<0.1) actual_current_person_comp_point.vx="<<actual_current_person_comp_point.vx<<"; (vy<0.1) actual_current_person_comp_point.vy="<<actual_current_person_comp_point.vy<< std::endl;
+		robot_propagation=robot_initial_pose_;
+		robot_propagation.v=0.0;
+		robot_propagation.w=0.0;
+	}
+
+	//std::cout << " OUT (function: replan_last_step!!!) robot_->get_des_vel="<< robot_->get_desired_velocity()<<"; print robot propagation:" << std::endl;
+	//robot_propagation.print();
+	//std::cout << " (overpas obstacles rear) robot_propagation.x="<<robot_propagation.x<<"; robot_propagation.y="<<robot_propagation.y<<"; act_min_companion_angle"<<act_min_companion_angle<< std::endl;
+
+	return robot_propagation;
+}
+
+
+Spoint Cplan_local_nav::sample_workspace(  )
+{
+	Spoint sample;
+	double theta(0.0);
+	std::uniform_real_distribution<double> sample_x( robot_->get_current_pose().x - workspace_radii_,
+			robot_->get_current_pose().x + workspace_radii_ );
+	std::uniform_real_distribution<double> sample_y( robot_->get_current_pose().y - workspace_radii_,
+			robot_->get_current_pose().y + workspace_radii_ );
+
+	double dx = goal_.x - robot_->get_current_pose().x;
+	double dy = goal_.y - robot_->get_current_pose().y;
+
+	std::normal_distribution<double> sample_g( atan2(dy,dx), std_goal_workspace_ );
+
+
+	switch(plan_mode_)
+	{
+	  case F_RRT_Uniform :
+		std::cout << " F_RRT_Uniform"<< std::endl;
+		sample = Spoint( sample_x(generator_), sample_y(generator_)  );
+		break;
+	  case F_RRT_Gauss_Circle :
+	  case F_RRT_GC_alpha :
+	  default:
+
+		theta = sample_g(generator_);
+		sample = Spoint( robot_->get_current_pose().x + workspace_radii_*cos(theta),
+				robot_->get_current_pose().y + workspace_radii_*sin(theta));
+		break;
+	}
+
+	//robot parameters sampling if required
+	//set the random alpha variable, if necessary
+	if( plan_mode_ == F_RRT_GC_alpha )
+	{
+
+		std::uniform_int_distribution<int> sample_behavior( 0 , 2 );
+		double epsilon(0.4);
+		switch( sample_behavior(generator_) )
+		{
+		case 0://robot unaware
+			alpha_ = 1.0+epsilon;gamma_ = 1.0-epsilon;delta_ = 1.0-epsilon;
+			//std::cout << "case 0 :  alpha_="<<alpha_ <<"; gamma_="<<gamma_<<"; delta_"<<delta_<< std::endl;
+			break;
+		case 1://robot aware
+			alpha_ = 1.0-epsilon;gamma_ = 1.0+epsilon;delta_ = 1.0+epsilon;
+			//std::cout << "case 1 :  alpha_="<<alpha_ <<"; gamma_="<<gamma_<<"; delta_"<<delta_<< std::endl;
+			break;
+		case 2://robot balanced
+		default:
+			alpha_ = 1.0;gamma_ = 1.0;delta_ = 1.0;
+			//std::cout << "case 2(default) :  alpha_="<<alpha_ <<"; gamma_="<<gamma_<<"; delta_"<<delta_<< std::endl;
+			break;
+		}
+
+		//std::cout << alpha_ << " , " << gamma_ << " , " << delta_ << std::endl;
+	}
+
+	if((dx==0)&&(dy==0.0)){ //el goal, es la misma robot pose!
+		sample=Spoint( robot_->get_current_pose().x ,
+				robot_->get_current_pose().y );
+		}
+	//std::cout <<"; sample.x="<< sample.x << " ; sample.y " << sample.y  << std::endl;
+
+	return sample;
+}
+
+
+
+
+
+void Cplan_local_nav::reset_scene_persons_propagation_flag()
+{
+	for( Cperson_abstract* iit : nearby_person_list_  )
+	{
+		iit->reset_propagation_flag();
+	}
+}
+
+unsigned int Cplan_local_nav::find_nearest_vertex( const Spoint& random_goal )
+{
+	//std::cout << "find_nearest_vertex (1)" << std::endl;
+	double min_dist(1e10), result;
+	unsigned int nearest_vertex_index = 0;
+	for( unsigned int i = 0 ; i < robot_->get_robot_planning_trajectory()->size() ; i+=2 )
+	{
+		if ( !collision_detected_[i] )
+		{
+			result = cost_to_go(random_goal,i);
+			if ( result  < min_dist  )
+			{
+				min_dist = result;
+				nearest_vertex_index = i;
+			}
+		}
+	}
+
+	if ( robot_->get_robot_planning_trajectory()->at( nearest_vertex_index ).time_stamp > horizon_time_ + now_ )
+		return 0;
+	//std::cout << "find_nearest_vertex (3)" << std::endl;
+	return nearest_vertex_index;
+}
+
+
+
+Sedge_tree Cplan_local_nav::calculate_edge( unsigned int parent_vertex, const Sdestination& random_goal, Cperson_abstract::companion_reactive reactive )
+{ // bool reactive  is to approximate to the person when robot is far. (Modificated by ely. For person companion.)
+
+	//std::cout << " IN (function: calculate_edge); robot_->get_desired_vel="<<robot_->get_desired_velocity()<<  std::endl;
+
+	Sforce f_goal, f_int, f_obs, f,f_person_goal, f_goal_final;
+	double signo_x;
+	double signo_y;
+	double fobos_Xmod;
+	double fobos_Ymod;
+
+	switch(reactive)
+	{
+	case Cperson_abstract::Reactiva_repulsive:
+	case Cperson_abstract::Reactive_atractive:
+
+		f_goal = robot_->force_goal( random_goal, get_sfm_params(robot_),&(robot_->get_planning_trajectory()->at(parent_vertex))  );
+		f_int = force_persons_int_planning_virtual( robot_, parent_vertex ,64.0, reactive); // check that min_dist2= 64.0 in the akp case!!!
+
+		//map force, used for simulations
+		if( read_force_map_success_ )
+			f_obs = get_force_map(robot_->get_robot_planning_trajectory()->at(parent_vertex).x, robot_->get_robot_planning_trajectory()->at(parent_vertex).y);
+
+		//obstacles due to laser scans. for real environments has priority over map forces
+		if(read_laser_obstacle_success_)
+			f_obs = force_objects_laser_int_planning_virtual( robot_, parent_vertex, 25.0, true );
+
+		// INICION Limitar fuerza obstaculos cuando hay muchos solapados
+		fobos_Xmod=sqrt(f_obs.fx*f_obs.fx);
+		fobos_Ymod=sqrt(f_obs.fy*f_obs.fy);
+
+		if (f_obs.fx > 0){
+			signo_x=1;
+		}else{
+			signo_x=-1;
+		}
+		if (f_obs.fy > 0){
+			signo_y=1;
+		}else{
+			signo_y=-1;
+		}
+
+		if(fobos_Xmod>f_obst_max_x_){
+			f_obs.fx=signo_x*f_obst_max_x_;
+		}
+
+		if(fobos_Ymod>f_obst_max_y_){
+			f_obs.fy=signo_y*f_obst_max_y_;
+		}
+		// FIN Limitar fuerza obstaculos cuando hay muchos solapados
+
+		f=f_goal*alpha_ + f_int*gamma_ + f_obs*delta_;
+		f_goal_final=f_goal;
+
+		//if(parent_vertex<2){  // TODO: cuando esta muy cerca de otras personas que se ponen entre el robot y la person goal, haria falta aumentar algo más la f_int. Pero ahora mismo no lo puedo cambiar sin hacer cosas raras.
+		//	std::cout << "case 0 :  alpha_="<<alpha_ <<"; gamma_="<<gamma_<<"; delta_"<<delta_<< std::endl;
+		//}
+
+	break;
+
+
+	case Cperson_abstract::Akp_planning:
+	default:
+		//caso planning, normal Gonzalo.
+		//double v_people;
+		//double desired_vel_robot; //desired_vel_robot ya no se usa.
+		//double distance_r_p1;
+		//double distance_r_p2;
+
+		//if(desired_vel_robot==0.0){ // Parar en ambos casos.
+			//double x_d_act=robot_->get_current_pointV().x;
+			//double y_d_act=robot_->get_current_pointV().y;
+			//Sdestination new_dest(random_goal.id,random_goal.x,random_goal.y,random_goal.prob,Sdestination::Stopping,random_goal.neighbours_ids);
+			//f_goal = robot_->force_goal( new_dest, get_sfm_params(robot_),&(robot_->get_planning_trajectory()->at(parent_vertex)) , parent_vertex);
+		//}else{
+			f_goal = robot_->force_goal( random_goal, get_sfm_params(robot_),&(robot_->get_planning_trajectory()->at(parent_vertex)) , parent_vertex);
+		//}
+
+		// INI SIDE-BY-SIDE CASE only one person
+		f_int = force_persons_int_planning_virtual(  robot_, parent_vertex, 64.0, reactive ); // Fuerza hacia las personas.
+		if(parent_vertex<0){
+			std::cout <<"(function: calculate_edge); f_int.fx="<<f_int.fx<<"; f_int.fy="<<f_int.fy<<  std::endl;
+		}
+		// out SIDE-BY-SIDE CASE only one person
+
+
+		////////////////////// Force to maintain the angle between people and robot!
+		Sdestination person_companion_goal;
+		//Cperson_abstract* person_obj;
+		//find_person(id_person_companion_ , &person_obj);
+		//std::cout << " AKP IN :calculate_edge case (3); actual_person_Companion_pointer_->get_planning_trajectory().size= "<<actual_person_Companion_pointer_->get_planning_trajectory()->size()<<"; actual_person_Companion_pointer_->get_prediction_trajectory().size="<<actual_person_Companion_pointer_->get_prediction_trajectory()->size()<<"; robot_->get_planning_trajectory().size()="<<robot_->get_planning_trajectory()->size() << "id="<< actual_person_Companion_pointer_->get_id()<<"; parent_vertex="<<parent_vertex<< std::endl;
+
+		SpointV_cov person;
+		if(actual_person_Companion_pointer_->get_planning_trajectory()->size()>parent_vertex){
+			person=actual_person_Companion_pointer_->get_planning_trajectory()->at(parent_vertex);
+
+		}else if(actual_person_Companion_pointer_->get_prediction_trajectory()->size()>parent_vertex){
+			person=actual_person_Companion_pointer_->get_prediction_trajectory()->at(parent_vertex);
+		}else{
+			person=actual_person_Companion_pointer_->get_planning_trajectory()->at(actual_person_Companion_pointer_->get_planning_trajectory()->size()-1);
+		}
+
+		person_companion_goal=Sdestination(20,person.x,person.y,1.0);
+		SpointV_cov robot=robot_->get_planning_trajectory()->at(parent_vertex);
+
+		Sdestination person_goal;
+		if(companion_same_person_goal_){
+			person_goal=actual_person_Companion_pointer_->get_best_dest();
+		}else{
+			person_goal=extern_robot_goal_;
+		}
+
+
+        double theta=saved_actual_theta_person_;//calc_person_companion_orientation();
+       // funcion calcular nuevo angulo theta con orientación dirección movimiento persona.
+
+        double angle = atan2(robot.y-person.y,robot.x-person.x);
+		double act_min_companion_angle;
+	    if(calc_goal_companion_with_group_path_){
+	    	act_min_companion_angle=orientation_person_robot_angles_[parent_vertex];
+	    }else{
+	    	act_min_companion_angle=orientation_person_robot_angles_[parent_vertex]; // only goal of plan is posible!
+	    }
+
+		if(diffangle(theta,angle)<0){
+			person_companion_goal.x+=(robot_person_proximity_distance_)*cos(theta+(act_min_companion_angle*(3.14/180)));//1.5*cos(theta+(act_min_companion_angle*(3.14/180)));
+			person_companion_goal.y+=(robot_person_proximity_distance_)*sin(theta+(act_min_companion_angle*(3.14/180)));//1.5*sin(theta+(act_min_companion_angle*(3.14/180)));
+		}else{
+			person_companion_goal.x+=(robot_person_proximity_distance_)*cos(theta-(act_min_companion_angle*(3.14/180)));//1.5*cos(theta-(act_min_companion_angle*(3.14/180)));
+			person_companion_goal.y+=(robot_person_proximity_distance_)*sin(theta-(act_min_companion_angle*(3.14/180)));//1.5*sin(theta-(act_min_companion_angle*(3.14/180)));
+		}
+
+		f_person_goal=robot_->force_goal(person_companion_goal,get_sfm_int_params(robot_),&(robot_->get_planning_trajectory( )->at(parent_vertex)));
+
+		f_goal_final=f_goal; // F_goal_final == f_goal => cambio f_goal, solo en el caso Side-by-side central.
+
+		//map force, used for simulations
+		if( read_force_map_success_ )
+			f_obs = get_force_map(robot_->get_robot_planning_trajectory()->at(parent_vertex).x,
+				robot_->get_robot_planning_trajectory()->at(parent_vertex).y);
+
+		//obstacles due to laser scans. for real environments has priority over map forces
+		if(read_laser_obstacle_success_)
+			f_obs = force_objects_laser_int_planning_virtual( robot_, parent_vertex, 25.0, true );
+
+		// INICION Limitar fuerza obstaculos cuando hay muchos solapados
+		fobos_Xmod=sqrt(f_obs.fx*f_obs.fx);
+		fobos_Ymod=sqrt(f_obs.fy*f_obs.fy);
+
+		if (f_obs.fx > 0){
+			signo_x=1;
+		}else{
+			signo_x=-1;
+		}
+		if (f_obs.fy > 0){
+			signo_y=1;
+		}else{
+			signo_y=-1;
+		}
+
+		if(fobos_Xmod>f_obst_max_x_){
+			f_obs.fx=signo_x*f_obst_max_x_;
+		}
+
+		if(fobos_Ymod>f_obst_max_y_){
+			f_obs.fy=signo_y*f_obst_max_y_;
+		}
+		// FIN Limitar fuerza obstaculos cuando hay muchos solapados
+
+
+		if(we_have_pointer_to_first_person_){
+			// caso side-by-side one person.
+			f=f_goal_final*alpha_ + f_int*gamma_ + f_obs*delta_;
+			//fin caso side-by-side one person.
+		}
+
+		// if(parent_vertex<0){
+		//	 std::cout << " despues de fuerza final robot: f.fx="<<f.fx<<"; f.fy="<<f.fy<< " f_goal_final.fx= "<<f_goal_final.fx<<"; f_goal_final.fy="<<f_goal_final.fy << " f_int.fx= "<<f_int.fx<<"; f_int.fy="<<f_int.fy<<std::endl;
+		//	 std::cout << " FINAL FORCE (Akp_planning, fuerza final y parametros): ; alpha_="<<alpha_<<"; gamma_"<<gamma_<<"; delta_="<<delta_<<std::endl;//"; f_int.fx"<<f_int.fx<<"; f_int.fy="<<f_int.fy<<std::endl;
+
+		//	 std::cout << " FINAL FORCE (Akp_planning, fuerza final y parametros):  f.fx="<<f.fx<<"; f.fy="<<f.fy<<"; alpha_="<<alpha_<<"; gamma_"<<gamma_<<"; delta_="<<delta_<<std::endl;//"; f_int.fx"<<f_int.fx<<"; f_int.fy="<<f_int.fy<<std::endl;
+		//	 std::cout << " f_goal_final.fx= "<<f_goal_final.fx<<"; f_goal_final.fy="<<f_goal_final.fy << " f_int.fx= "<<f_int.fx<<"; f_int.fy="<<f_int.fy<<"; f_obs.fx="<<f_obs.fx<<"; f_obs.fy="<<f_obs.fy<<std::endl;
+		// }
+
+	break;
+	}
+
+
+    person1_repulsive_force_fx_.push_back(person1_repulsive_force_fx_act_);
+    person1_repulsive_force_fy_.push_back(person1_repulsive_force_fy_act_);
+
+    person1_companion_force_fx_.push_back(person1_companion_force_fx_act_);
+    person1_companion_force_fy_.push_back(person1_companion_force_fy_act_);
+
+	//std::cout << " FINAL  máx force (akp_planner, returned!!!); f_obs.fx="<<f_obs.fx*delta_companion_<<"; f_obs.fy="<<f_obs.fy*delta_companion_<< std::endl;
+    //std::cout << " OUT (function: calculate_edge); robot_->get_desired_vel="<<robot_->get_desired_velocity()<<  std::endl;
+
+	 return Sedge_tree( parent_vertex, f, f_goal_final, f_int, f_obs,f_person_goal);
+}
+
+
+bool Cplan_local_nav::propagate_vertex( unsigned int parent_index , const Sedge_tree& u, Cperson_abstract::companion_reactive reactive)
+{
+	//robot_->get_current_pose().theta
+	// Robot propagation and cost ----------------------------------------------------------------------
+	unsigned int index_to_copy;
+	SpointV_cov virtual_next_pose;
+	Sforce virtual_force_goal, virtual_force_int_person, virtual_force_obstacle, virtual_force_robot;
+
+	robot_->robot_propagation( dt_, parent_index , u.f );
+
+	bool collision_check;
+
+	// ini  only one person companion, you do not neet to change the robot position to compute the collisions.
+	SpointV_cov traslated_robot_position=robot_->get_planning_trajectory()->back();
+	SpointV_cov robot_plan_act=robot_->get_planning_trajectory()->back();
+	SpointV_cov person_propagation_act=pointer_to_person_companion_->get_planning_trajectory( )->at(parent_index);
+	std::vector<SpointV> people;
+	people.push_back(person_propagation_act);
+	traslated_robot_position=calculate_centre_of_the_group(traslated_robot_position, people);
+	collision_check = check_collision( robot_->get_planning_trajectory()->back(), parent_index, traslated_robot_position);
+	// fin  only one person companion, you do not neet to change the robot position to compute the collisions.
+
+	// vertixes plotting
+
+	// Nearby people propagation and cost ---------------------------------------------------------------------
+	SpointV_cov robot_point;
+	//for( Cperson_abstract* iit: nearby_person_list_ ) // es para planing de las personas, en principio no hay k tocar nada de aquí.
+	for( Cperson_abstract* iit: person_list_ ) // es para planing de las personas, en principio no hay k tocar nada de aquí.
+	{
+		// 1 precalculation: if really close to the robot, then a complete propagation is done, otherwise
+		// the correspondant Spose is sought.
+		bool need_prop_pers=false;
+		//case only one person side-by-side
+		need_prop_pers=iit->is_needed_to_propagate_person_for_planning(  parent_index, robot_->get_robot_planning_trajectory()->back() , index_to_copy ) ;
+
+		if (!need_prop_pers)
+		{
+			iit->planning_propagation_copy( index_to_copy );
+			robot_point =  robot_->get_planning_trajectory()->back();
+			virtual_force_robot =  iit->force( robot_point , get_sfm_int_params(iit,robot_),
+								&(iit->get_planning_trajectory()->at(parent_index) ) );
+		}
+		else
+		{
+			// 2 time step propagation
+			//CASE only one-person side-by-side
+
+			virtual_force_goal = iit->force_goal( iit->get_best_dest() , get_sfm_params(robot_),
+													&(iit->get_planning_trajectory()->at(parent_index) )  );
+
+			virtual_force_int_person = force_persons_int_planning_virtual( iit, parent_index );
+			if( read_force_map_success_ )
+				virtual_force_obstacle = get_force_map( iit->get_planning_trajectory()->at(parent_index).x,
+						iit->get_planning_trajectory()->at(parent_index).y ) * 0.5;
+
+		    //obstacles due to laser scans. for real environments has priority over map forces
+			if(read_laser_obstacle_success_)
+				virtual_force_obstacle = force_objects_laser_int_planning_virtual( iit , parent_index,16.0);
+			robot_point =  robot_->get_planning_trajectory()->back();
+
+
+			// INICION Limitar fuerza obstaculos cuando hay muchos solapados
+			double fobos_Xmod=sqrt(virtual_force_obstacle.fx*virtual_force_obstacle.fx);
+			double fobos_Ymod=sqrt(virtual_force_obstacle.fy*virtual_force_obstacle.fy);
+			double signo_x;
+			double signo_y;
+			if (virtual_force_obstacle.fx > 0){
+				signo_x=1;
+			}else{
+				signo_x=-1;
+			}
+			if (virtual_force_obstacle.fy > 0){
+				signo_y=1;
+			}else{
+				signo_y=-1;
+			}
+
+			if((fobos_Xmod>f_obst_max_x_)||(fobos_Ymod>f_obst_max_y_)){
+				virtual_force_obstacle.fx=signo_x*f_obst_max_x_;
+				virtual_force_obstacle.fy=signo_y*f_obst_max_y_;
+			}
+
+			/* INI la fuerza persona hacia robot, cambia, solo es repulsiva cuando el robot esta muy cerca */
+			double robot_person_distance;
+			if(bool_case_person_companion_){
+				robot_person_distance=calc_robot_person_companion_distance_companion_person_akp();
+			}else{
+				robot_person_distance=calc_robot_person_companion_distance();
+			}
+
+			//double robot_person_distance_to_person_goal;
+			//double distance_near_to_person_goal;
+			//distance_near_to_person_goal=0.25;
+
+				if((iit->get_id()!=id_person_companion_)){
+					virtual_force_robot =  iit->force( robot_point , get_sfm_int_params(iit,robot_),
+						&(iit->get_planning_trajectory()->at(parent_index) ) );
+				}else if((iit->get_id()==id_person_companion_)&&(robot_person_distance<(robot_person_proximity_distance_-(robot_person_proximity_tolerance_))))//+0.25))))
+				{
+					virtual_force_robot =  iit->force( robot_point , get_sfm_int_params(iit,robot_),&(iit->get_planning_trajectory()->at(parent_index) ) );
+				}else{
+					// fuerza que ejerce este robot sobre la persona es 0, como si no existiera...
+				}
+
+
+			/* FIN la fuerza persona hacia robot, cambia, solo es repulsiva cuando el robot esta muy cerca */
+
+			iit->set_forces_person( virtual_force_goal, virtual_force_int_person, virtual_force_robot, virtual_force_obstacle );
+			iit->planning_propagation(dt_, iit->get_force_person() , parent_index); //Propaga a esta persona para el akp, siguiente iteracion. pero la propaga hacia su goal fijo. ¿Podría hacer que con esta se propagara para el nuevo caso, en vez del de antes y solo tener una propagacion de la persona?
+
+		}
+
+	}
+
+	return collision_check;
+}
+
+
+void Cplan_local_nav::calculate_cost( unsigned int parent_index , Sedge_tree u , Cperson_abstract::companion_reactive reactive)
+{
+
+	//std::cout<<" ini cost companion !!!!"<<std::endl;
+	//for each propagation, a cost is calculated
+	//robot cost
+	Spoint dr;
+	double cost(0.0);
+
+	cost = u.f_goal.module2(cost_angular_);
+	cost_robot_.push_back( cost_robot_.at(parent_index) + cost);
+	// Obstacles cost -----------------------------------------------------------------------------------------
+	cost = u.f_obs.module2();
+	cost_obstacles_.push_back( cost_obstacles_.at(parent_index) + cost );
+
+	//people cost --------------------------------------------------------------------------------------------
+	Sforce f,f_int,f2;
+	cost = 0;
+
+	for( Cperson_abstract* iit: nearby_person_list_  )
+	{
+		switch( pr_force_mode_ )
+		{
+		case 0: //deterministic force, classical definition; we use pr_force_mode_=0. => force_sphe
+			if((iit->get_id()!=id_person_companion_)&&(reactive!=Cperson_abstract::Akp_planning)){
+				f = iit->force_sphe(robot_->get_planning_trajectory()->back() , get_sfm_int_params(iit,robot_) , &(iit->get_planning_trajectory()->at( parent_index )) ,reactive);
+			}else{
+				//std::cout << " IN :calculate_cost, else: Akp_planning, salta person companion " <<  std::endl;
+			}
+			break;
+		case 1: //probabilistic force, sampling around the elipsoid
+			f = iit->force_sphe_prob( robot_->get_planning_trajectory()->back(), get_sfm_int_params(iit,robot_),&(iit->get_planning_trajectory()->at( parent_index )) );
+			break;
+		case 2: //probabilistic force, mahalanobis distance
+			f = iit->force_sphe_mahalanobis( robot_->get_planning_trajectory()->back(), get_sfm_int_params(iit,robot_), &(iit->get_planning_trajectory()->at( parent_index )));
+			break;
+		case 3: //probabilistic force, worst case scenario: covaraince ellipsoid nearer to the center point
+			f = iit->force_sphe_worst( robot_->get_planning_trajectory()->back(), get_sfm_int_params(iit,robot_), &(iit->get_planning_trajectory()->at( parent_index )));
+			break;
+		}
+		f_int += f;
+		cost += f.module2();
+	}
+
+	cost_int_forces_.push_back( cost_int_forces_.at(parent_index) + cost );
+	//distance cost --------------------------------------------------------------------------------------------
+	cost = robot_->get_planning_trajectory()->back().distance2(local_goal_);
+
+	cost_distance_.push_back( cost_distance_.at(parent_index) + cost );
+	double o = robot_->get_planning_trajectory()->back().angle_heading_point( local_goal_ );
+	cost_orientation_.push_back( cost_orientation_.at(parent_index) + o*o );
+    // Local minima cost  ---------------------------------------------------------------------------------------------------------
+	nodes_in_branch_.push_back( nodes_in_branch_.at(parent_index) + 1.0 );
+
+	// Simililarities wrt previous path --------------------------------------------------------------------------
+	if(reactive==Cperson_abstract::Akp_planning){
+		Crobot* robot_act=robot_;
+		calculate_companion_cost_node(parent_index,robot_act); //funcion (ely) calculate cost companion.
+	}
+
+	//std::cout << " OUT for costs " <<  std::endl;
+
+}
+
+
+double Cplan_local_nav::cost_to_go( const Spoint& random_goal, unsigned int i , Cperson_abstract::companion_reactive reactive)
+{
+	double result;
+	double d, t, o, dx, dy;
+	//metrics to evaluate the nearest vertex
+	switch( distance_mode_ )
+	{
+		//distance to random goal ----------------------- Euclidean distance: TO BE DEPRECATED   --------------------
+		case Cplan_local_nav::Euclidean  :
+			//1- calculate distance normalized to max distance
+			d = robot_->get_robot_planning_trajectory()->at(i).distance( random_goal )
+					/ workspace_radii_ * cost_parameters_[0];
+			//2- orientation
+			dx = random_goal.x - robot_->get_robot_planning_trajectory()->at(i).x;
+			dy = random_goal.y - robot_->get_robot_planning_trajectory()->at(i).y;
+			o = fabs(diffangle( robot_->get_robot_planning_trajectory()->at(i).theta ,
+					atan2( dy, dx )  )) * cost_parameters_[1];
+
+
+			//5- time not allowed being outside the time horizon
+			t = (robot_->get_robot_planning_trajectory()->at(i).time_stamp -
+					now_ ) / horizon_time_ ;
+
+			if ( t > 1.0 ) t = 10000.0; //discard the vertex
+			else t *= cost_parameters_[4];
+
+			//6 - obstacles
+			result = d+o+t;
+			break;
+
+		//cost-to-go linear normalization to random goal  ------------------------------------------------------
+		  case  Cplan_local_nav::Cost2go_norm :
+		//cost-to-go erf normalization to random goal ----------------------------------------------------------
+		  case  Cplan_local_nav::Cost2go_erf :
+		  {
+			  // Gonzalo entra en este de distancia. !!!!!!!!!!!!!!!
+			SpointV robot = robot_->get_planning_trajectory()->at(i);
+			Sforce f_goal, f_int, f_obs, f, f_tot;
+			double look_time(0.0),cost_robot(0.0),cost_distance(0.0), cost_int(0.0), cost_obs(0.0),cost_orientation(0.0),o;//cost_companion(0.0);
+			unsigned int prediction_index(2),max_index_prediction(0);
+			//assert to avoid degenerated cases when the path does not look up to t_h due to being near the goal
+			if ( robot_->get_planning_trajectory()->size() <  horizon_time_index_)
+			{
+				max_index_prediction = robot_->get_planning_trajectory()->size() -1;
+			}
+			else
+			{
+				max_index_prediction = horizon_time_index_;
+			}
+			//calculates current prediction time index [0,horizon] and distance to random goal of previous path.
+			while(  look_time < robot.time_stamp  && prediction_index < max_index_prediction )
+			{
+				look_time = robot_->get_planning_trajectory()->at(prediction_index).time_stamp;
+				cost_distance += robot_->get_planning_trajectory()->at(prediction_index).distance2( random_goal );//previous distance to get to the goal
+				prediction_index += 2;
+			}
+			prediction_index -= 2;
+			if ( look_time - now_ > horizon_time_ || prediction_index >=  horizon_time_index_ )
+				return 1e10;
+			//std::cout << "entering  " <<  i << "  time = " << prediction_index <<  "  size = " << robot_->get_planning_trajectory()->size() << std::endl;
+			cost_robot = cost_robot_[i]/2.0;//as we are using double time step and half propagations, the accumulated robot cost
+			cost_obs = cost_obstacles_[i]/2.0;
+			cost_int = cost_int_forces_[i]/2.0;
+			while ( prediction_index < horizon_time_index_  )
+			{
+				// cost due to interacting forces  (De aquí, habria que reducir la que te proboca la persona a la que acompañas)
+				for( Cperson_abstract* iit: nearby_person_list_  )
+				{
+					if ( (iit->get_prediction_trajectory()->at( prediction_index ).distance2(robot) < 16.0 ) && (iit->get_id()!=id_person_companion_) )//<4m...to speed up, a more strict threshold is used
+					{ // para la person companion en el akp no hay fuerzas repulsivas contra ella.
+
+							if((iit->get_id()!=id_person_companion_)&&(reactive!=Cperson_abstract::Akp_planning)){
+								f = iit->force_sphe(robot , get_sfm_int_params(iit,robot_) , &iit->get_prediction_trajectory()->at( prediction_index ) ,reactive);
+								f_int += f;
+								cost_int += f.module2();
+							}else{
+								//std::cout << " IN :cost_to_go, else: Akp_planning, salta person companion " <<  std::endl;
+							}
+
+
+					}
+				}
+				// potential cost to go destination
+				f_goal = robot_->force_goal( Sdestination(0,random_goal.x,random_goal.y), get_sfm_params(robot_), &robot );
+				cost_robot += f_goal.module2(cost_angular_);
+
+				// REtocado para reducir fuerza muchos obstaculos juntos.
+				for( Spoint iit : laser_obstacle_list_)
+				{
+					f = robot_->force_sphe( iit, get_sfm_int_params(robot_), &robot );
+					f_obs += f;
+				}
+
+
+				// INICION Limitar fuerza obstaculos cuando hay muchos solapados
+				double fobos_Xmod=sqrt(f_obs.fx*f_obs.fx);
+				double fobos_Ymod=sqrt(f_obs.fy*f_obs.fy);
+				double signo_x;
+				double signo_y;
+				if (f_obs.fx > 0){
+					signo_x=1;
+				}else{
+					signo_x=-1;
+				}
+				if (f_obs.fy > 0){
+					signo_y=1;
+				}else{
+					signo_y=-1;
+				}
+
+				if((fobos_Xmod>f_obst_max_x_)||(fobos_Ymod>f_obst_max_y_)){
+					f_obs.fx=signo_x*f_obst_max_x_;
+					f_obs.fy=signo_y*f_obst_max_y_;
+				}
+				// FIN Limitar fuerza obstaculos cuando hay muchos solapados
+				// robot propagation
+				f_tot=f_goal+f_obs+f_int;
+				robot = robot.propagate( dt_*2, f_tot, robot_->get_desired_velocity() );//at double timestep to speed up calculations
+				cost_distance += robot.distance2(random_goal);
+				o = robot.angle_heading_point( random_goal );
+				cost_orientation += o*o;
+				prediction_index += 2;
+
+			}
+			if( distance_mode_ == Cplan_local_nav::Cost2go_erf )
+			{
+				//std::cout << "%cost_distance, cost_orientation, cost_robot, cost_int, cost_obs" << endl;
+				//std::cout  << cost_distance << " , " << cost_orientation << " , " << cost_robot << " , " << cost_int << " , " <<  cost_obs << std::endl;
+				result = cost_parameters_[0]*erf((cost_distance - mean_cost_distance_) / std_cost_distance_) +
+						cost_parameters_[1]*erf((cost_orientation - mean_cost_orientation_) / std_cost_orientation_) +
+						cost_parameters_[2]*erf((cost_robot - mean_cost_robot_) / std_cost_robot_ ) +
+						cost_parameters_[3]*erf((cost_int - mean_cost_int_forces_) / std_cost_int_forces_ ) +
+						cost_parameters_[5]*erf((cost_obs - mean_cost_obstacles_) / std_cost_obstacles_);
+			}
+			else
+			{
+			  //use the mean as the utopia point a std as the difference f_max - utopia, calculated previously
+			result = cost_parameters_[0]*((cost_distance - mean_cost_distance_) / std_cost_distance_) +
+						cost_parameters_[1]*((cost_orientation - mean_cost_orientation_) / std_cost_orientation_) +
+						cost_parameters_[2]*((cost_robot - mean_cost_robot_) / std_cost_robot_ ) +
+						cost_parameters_[3]*((cost_int - mean_cost_int_forces_) / std_cost_int_forces_ ) +
+						cost_parameters_[5]*((cost_obs - mean_cost_obstacles_) / std_cost_obstacles_);
+			}
+
+
+
+
+			break;
+		  }
+		  //cost-to-go to a random goal only considering steering forces and collision ------------------------------------------------------
+		  case  Cplan_local_nav::Cost2go_raw :
+		  default:
+		  {
+			SpointV robot = robot_->get_planning_trajectory()->at(i);
+			Sforce f_goal;
+			double look_time(0.0), cost_robot(0.0);
+			unsigned int prediction_index(2),max_index_prediction(0);
+			//assert to avoid degenerated cases when the path does not look up to t_h due to being near the goal
+			if ( robot_->get_planning_trajectory()->size() <  horizon_time_index_)
+			{
+				max_index_prediction = robot_->get_planning_trajectory()->size() -1;
+			}
+			else
+			{
+				max_index_prediction = horizon_time_index_;
+			}
+			//calculates current prediction time index [0,horizon] and distance to random goal of previous path.
+			while(  look_time < robot.time_stamp  && prediction_index < max_index_prediction )
+			{
+				look_time = robot_->get_planning_trajectory()->at(prediction_index).time_stamp;
+				prediction_index += 2;
+			}
+			prediction_index -= 2;
+			if ( look_time - now_ > horizon_time_ || prediction_index >=  horizon_time_index_ )
+				return 1e10;
+			//std::cout << "entering  " <<  i << "  time = " << prediction_index <<  "  size = " << robot_->get_planning_trajectory()->size() << std::endl;
+			cost_robot = cost_robot_[i]/2.0;//as we are using double time step and half propagations, the accumulated robot cost
+			while ( prediction_index < horizon_time_index_  )
+			{
+				// potential cost to go destination
+				f_goal = robot_->force_goal( Sdestination(0,random_goal.x,random_goal.y), get_sfm_params(robot_), &robot );
+				cost_robot += f_goal.module2(cost_angular_);//fy cost 0.5
+
+				// robot propagation
+				robot = robot.propagate( dt_*2, f_goal, robot_->get_desired_velocity() );//at double timestep to speed up calculations
+				prediction_index += 2;
+
+				//check for collisions, only with obstacles
+				if (  check_collision( robot ) )
+					return 1e10;
+			}
+			result = cost_robot;
+
+			break;
+		  }
+		}//end of switch
+
+
+	return result;
+}
+
+
+unsigned int Cplan_local_nav::global_min_cost_index( Crobot* robot_act , Cperson_abstract::companion_reactive reactive )
+{
+	//std::cout<<" INI function: global_min_cost_index()" << std::endl;
+	double cost, min_cost(1e10);
+	unsigned int min_cost_index(0);
+	double d, o, dx, dy;
+	preprocess_global_parameters(robot_act);//depending on mode, needs a different preprocessing of data
+
+	we_have_cost_companion_=false;
+
+	switch( global_mode_ )
+	{
+
+	  //distance to local goal, end of branch ----------------------- Euclidean distance: TO BE DEPRECATED
+	  case Cplan_local_nav::Scalarization :
+		  std::cout << "   Scalarization" << std::endl;
+		for( unsigned int i : end_of_branches_index_ )
+		{
+			d = robot_act->get_robot_planning_trajectory()->at(i).distance( local_goal_ );
+			dx = goal_.x - robot_act->get_robot_planning_trajectory()->at(i).x;
+			dy = goal_.y - robot_act->get_robot_planning_trajectory()->at(i).y;
+			o = fabs(diffangle( robot_act->get_robot_planning_trajectory()->at(i).theta ,
+				atan2( dy, dx )  ));
+			cost = cost_parameters_[0]*d +
+				cost_parameters_[1]*o +
+				cost_parameters_[2]*cost_robot_[i] +
+				cost_parameters_[3]*cost_int_forces_[i]/nodes_in_branch_[i] +
+				cost_parameters_[5]*cost_obstacles_[i]/nodes_in_branch_[i] +
+				cost_parameters_[6]*cost_past_traj_[i];
+
+			if(cost_companion_[i]!=0){
+				cost =(0.9)*cost+(0.1)*cost_companion_[i];
+			}else{
+				//std::cout<<"NO HAY cost_companion_["<<i<<"]="<<cost_companion_[i]<< std::endl;
+			}
+
+
+			if ( cost < min_cost)
+			{
+				min_cost = cost;
+				min_cost_index = i;
+			}
+			/*std::cout //<< "cost values (goal,ori,rob,int,obs,past,lm) = ("
+				<< d <<
+				" , " << o <<
+				" , " << cost_int_forces_[i]/nodes_in_branch_[i] <<
+				" , " << cost_obstacles_[i]/nodes_in_branch_[i] <<
+				" , " << cost_past_traj_[i] <<
+				" , " << cost_local_minima_[i]/nodes_in_branch_[i] << std::endl;*/
+		}
+		break;
+		// global cost, estimation and normalization ---------------------------------------------------------------
+	  case Cplan_local_nav::Weighted_sum_erf :
+		  std::cout << "   Weighted_sum_erf" << std::endl;
+		for( unsigned int i : end_of_branches_index_ )
+		{
+			cost = cost_parameters_[0]*erf((cost_distance_[i] - mean_cost_distance_) / std_cost_distance_) +
+				cost_parameters_[1]*erf((cost_orientation_[i] - mean_cost_orientation_) / std_cost_orientation_) +
+				cost_parameters_[2]*erf((cost_robot_[i] - mean_cost_robot_) / std_cost_robot_ ) +
+				cost_parameters_[3]*erf((cost_int_forces_[i] - mean_cost_int_forces_) / std_cost_int_forces_ ) +
+				cost_parameters_[5]*erf((cost_obstacles_[i] - mean_cost_obstacles_) / std_cost_obstacles_) +
+				cost_parameters_[6]*erf((cost_past_traj_[i] - mean_cost_past_traj_) / std_cost_past_traj_);
+
+			if(cost_companion_[i]!=0){
+				cost =(0.9)*cost+(0.1)*cost_companion_[i];
+			}else{
+				//std::cout<<"NO HAY cost_companion_["<<i<<"]="<<cost_companion_[i]<< std::endl;
+			}
+
+			if ( cost < min_cost)
+			{
+				min_cost = cost;
+				min_cost_index = i;
+			}
+			/*std::cout << i << " ,"
+			 	<< erf((cost_distance_[i] - mean_cost_distance_) / std_cost_distance_) << " , "
+				<< erf((cost_orientation_[i] - mean_cost_orientation_) / std_cost_orientation_) << " , "
+				<< erf((cost_robot_[i] - mean_cost_robot_) / std_cost_robot_ ) << " , "
+				<< erf((cost_int_forces_[i] - mean_cost_int_forces_) / std_cost_int_forces_ ) << " , "
+				<< erf((cost_obstacles_[i] - mean_cost_obstacles_) / std_cost_obstacles_) << std::endl;*/
+		}
+		break;
+	  case  Cplan_local_nav::Weighted_sum_norm :
+
+		  std::cout << "   Weighted_sum_norm" << std::endl;
+		  // mean_cost is the utopia cost corresponding to a free path and std_cost is the max_cost - utopia, according to MMO clasical approaches
+	    for( unsigned int i : end_of_branches_index_ )
+	    {
+			cost = cost_parameters_[0]*((cost_distance_[i] - mean_cost_distance_) / std_cost_distance_) +
+					cost_parameters_[1]*((cost_orientation_[i] - mean_cost_orientation_) / std_cost_orientation_) +
+					cost_parameters_[2]*((cost_robot_[i] - mean_cost_robot_) / std_cost_robot_ ) +
+					cost_parameters_[3]*((cost_int_forces_[i] - mean_cost_int_forces_) / std_cost_int_forces_ ) +
+					cost_parameters_[5]*((cost_obstacles_[i] - mean_cost_obstacles_) / std_cost_obstacles_) +
+					cost_parameters_[6]*((cost_past_traj_[i] - mean_cost_past_traj_) / std_cost_past_traj_);
+
+			if(debug_antes_subgoals_entre_AKP_goals_){
+				std::cout<<"(CASE) Cplan_local_nav::Weighted_sum_norm "<< std::endl;
+				std::cout<<"i ="<<i<< std::endl;
+				std::cout<<"(GONZALO) cost ="<<cost<< std::endl;
+			}
+
+			if(cost_companion_[i]!=0){
+				cost =(0.9)*cost+(0.1)*cost_companion_[i];
+			}else{
+				//std::cout<<"NO HAY cost_companion_["<<i<<"]="<<cost_companion_[i]<< std::endl;
+			}
+
+			if ( cost < min_cost)
+			{
+				min_cost = cost;
+				min_cost_index = i;
+			}
+			/*std::cout << i << " , "
+				<<	((cost_distance_[i] - mean_cost_distance_) / std_cost_distance_) << " , "
+				<< ((cost_orientation_[i] - mean_cost_orientation_) / std_cost_orientation_) << " , "
+				<< ((cost_robot_[i] - mean_cost_robot_) / std_cost_robot_ ) << " , "
+				<< ((cost_int_forces_[i] - mean_cost_int_forces_) / std_cost_int_forces_ ) << " , "
+				<< ((cost_obstacles_[i] - mean_cost_obstacles_) / std_cost_obstacles_) << std::endl;*/
+	    }
+		break;
+	  case  Cplan_local_nav::MO_erf :
+	  case  Cplan_local_nav::MO_norm :
+	  {
+		  // Entra en esta!
+		 // std::cout << "[Cplan_local_nav::MO_norm/MO_erf]; (usamos MO_erf=3) global_mode_="<< global_mode_ << std::endl;
+		  //first approach: push into a set of best trajectories
+		  nondominated_plan_vertex_index_.clear();
+		  nondominated_end_of_plan_vertex_index_.clear();
+		  unsigned int nondominated_branch_index, cont;
+		  std::vector<SsavePath_cost_and_values> non_domianted_path_and_act_costs_;
+		  non_domianted_path_and_act_costs_.reserve(nondominated_multicosts_.size());
+		 non_domianted_path_and_act_costs_.resize(nondominated_multicosts_.size());
+
+		  const std::vector<Spose>* plans_act = robot_act->get_robot_planning_trajectory();
+
+		//std::cout << "[Cplan_local_nav::MO_norm/MO_erf] plans_act.empty()="<<plans_act->empty() <<"; cont = (horizon_time_index_/2)*2="<<(horizon_time_index_/2)*2<<"; nondominated_multicosts_.size()="<<nondominated_multicosts_.size()<< std::endl;
+
+		  unsigned int iter=0;
+
+		  for (  Smulticost m : nondominated_multicosts_)
+		  {
+		    nondominated_branch_index = m.id;
+		    cont = (horizon_time_index_/2)*2;//to avoid plotting all solutions, we will only plot the nondominated set, even number
+
+		    //print nondominated and normalized costs
+		    //m.print_ml();
+		    //print nondominated raw costs
+			/*std::cout << nondominated_branch_index << " , "
+				<< cost_distance_[nondominated_branch_index]  << " , "
+				<< cost_orientation_[nondominated_branch_index]  << " , "
+				<< cost_robot_[nondominated_branch_index]  << " , "
+				<< cost_int_forces_[nondominated_branch_index]  << " , "
+				<< cost_obstacles_[nondominated_branch_index]  << std::endl;*/
+		    //std::cout << " [Cplan_local_nav::MO_norm/MO_erf] ANTES nondominated_end_of_plan_vertex_index_.push_back( nondominated_branch_index );" << std::endl;
+			nondominated_end_of_plan_vertex_index_.push_back( nondominated_branch_index );//only end of branches to plot the branch id
+			//std::cout << " [end of branch index] nondominated_branch_index="<<nondominated_branch_index<< std::endl;
+			non_domianted_path_and_act_costs_[iter].set_end_id_path(nondominated_branch_index);
+
+			//std::cout<<"non_domianted_path_and_act_costs_ => nondominated_branch_index="<<nondominated_branch_index<< std::endl;
+
+			std::vector<unsigned int> act_all_ids_path_positions;
+			std::vector<Spoint> act_path_positions;
+			std::vector<std::vector<double>> iter_vect_costst_act;
+			do
+			{
+				nondominated_plan_vertex_index_.push_back( nondominated_branch_index);
+				act_all_ids_path_positions.push_back( nondominated_branch_index);
+				act_path_positions.push_back(plans_act->at(nondominated_branch_index));
+
+				std::vector<double> vect_costst_act;
+				vect_costst_act.push_back(cost_parameters_[0]*erf((cost_distance_[nondominated_branch_index] - mean_cost_distance_) / std_cost_distance_));
+				vect_costst_act.push_back(cost_parameters_[1]*erf((cost_orientation_[nondominated_branch_index] - mean_cost_orientation_) / std_cost_orientation_));
+				vect_costst_act.push_back(cost_parameters_[2]*erf((cost_robot_[nondominated_branch_index] - mean_cost_robot_) / std_cost_robot_ ));
+				vect_costst_act.push_back(cost_parameters_[3]*erf((cost_int_forces_[nondominated_branch_index] - mean_cost_int_forces_) / std_cost_int_forces_ ));
+				vect_costst_act.push_back(cost_parameters_[5]*erf((cost_obstacles_[nondominated_branch_index] - mean_cost_obstacles_) / std_cost_obstacles_));
+				vect_costst_act.push_back(erf((cost_companion_[nondominated_branch_index] - mean_cost_companion_) / std_cost_companion_));
+				vect_costst_act.push_back(cost_parameters_[6]*erf((cost_past_traj_[nondominated_branch_index] - mean_cost_past_traj_) / std_cost_past_traj_));
+				cost = cost_parameters_[0]*erf((cost_distance_[nondominated_branch_index] - mean_cost_distance_) / std_cost_distance_) +
+										cost_parameters_[1]*erf((cost_orientation_[nondominated_branch_index] - mean_cost_orientation_) / std_cost_orientation_) +
+										cost_parameters_[2]*erf((cost_robot_[nondominated_branch_index] - mean_cost_robot_) / std_cost_robot_ ) +
+										cost_parameters_[3]*erf((cost_int_forces_[nondominated_branch_index] - mean_cost_int_forces_) / std_cost_int_forces_ ) +
+										cost_parameters_[5]*erf((cost_obstacles_[nondominated_branch_index] - mean_cost_obstacles_) / std_cost_obstacles_) +
+										cost_parameters_[6]*erf((cost_past_traj_[nondominated_branch_index] - mean_cost_past_traj_) / std_cost_past_traj_);
+
+				double new_cost=cost;
+
+				//std::cout << "(ant) [nondominated_branch_index] new_cost="<<new_cost<<"; cost_companion="<< erf((cost_companion_[nondominated_branch_index] - mean_cost_companion_) / std_cost_companion_)<< std::endl;
+
+				if((cost_companion_[nondominated_branch_index]!=0)&&(reactive==Cperson_abstract::Akp_planning)){
+					we_have_cost_companion_=true;
+					new_cost= (0.9)*cost+(0.1)*erf((cost_companion_[nondominated_branch_index] - mean_cost_companion_) / std_cost_companion_);
+				}
+
+				//std::cout << " 1 [Cplan_local_nav::MO_norm/MO_erf] cost="<<(0.1)*cost<<"; new_cost="<<(0.9)*new_cost<< std::endl;
+
+				//std::cout << "[nondominated_branch_index] new_cost="<<new_cost<<"; cost_companion="<< erf((cost_companion_[nondominated_branch_index] - mean_cost_companion_) / std_cost_companion_)<< std::endl;
+				cost=new_cost;
+
+				//std::cout << " [Cplan_local_nav::MO_norm/MO_erf] new_cost="<<new_cost<< std::endl;
+				vect_costst_act.push_back(new_cost);
+				iter_vect_costst_act.push_back(vect_costst_act);
+				nondominated_branch_index = edge_[ nondominated_branch_index ].parent;
+				--cont;
+			}while( nondominated_branch_index > 0 && cont >= 1);
+
+			//std::cout<<"antes  positions + cost + iter ="<<iter_vect_costst_act.size()<< std::endl;
+
+			non_domianted_path_and_act_costs_[iter].set_total_costs_of_each_path_position(iter_vect_costst_act);
+			non_domianted_path_and_act_costs_[iter].set_all_ids_path_positions(act_all_ids_path_positions);
+			non_domianted_path_and_act_costs_[iter].set_path_positions(act_path_positions);
+
+			iter++;
+		  }
+
+		  if( global_mode_ == Cplan_local_nav::MO_erf )
+		  {
+
+			  for( unsigned int i : nondominated_end_of_plan_vertex_index_)
+			  {
+					//std::cout<<"[end of branch index] i ="<<nondominated_end_of_plan_vertex_index_[i]<< std::endl;
+
+				cost = cost_parameters_[0]*erf((cost_distance_[i] - mean_cost_distance_) / std_cost_distance_) +
+						cost_parameters_[1]*erf((cost_orientation_[i] - mean_cost_orientation_) / std_cost_orientation_) +
+						cost_parameters_[2]*erf((cost_robot_[i] - mean_cost_robot_) / std_cost_robot_ ) +
+						cost_parameters_[3]*erf((cost_int_forces_[i] - mean_cost_int_forces_) / std_cost_int_forces_ ) +
+						cost_parameters_[5]*erf((cost_obstacles_[i] - mean_cost_obstacles_) / std_cost_obstacles_) +
+						cost_parameters_[6]*erf((cost_past_traj_[i] - mean_cost_past_traj_) / std_cost_past_traj_);
+
+				//std::cout << "[i] d_cost="<<cost_parameters_[0]*erf((cost_distance_[i] - mean_cost_distance_) / std_cost_distance_)<<"; or_cost="<<cost_parameters_[1]*erf((cost_orientation_[i] - mean_cost_orientation_) / std_cost_orientation_)<<"; traj_cost="<<cost_parameters_[6]*erf((cost_past_traj_[i] - mean_cost_past_traj_) / std_cost_past_traj_)<< std::endl;
+
+				double new_cost=cost;
+
+				//std::cout << "(ant) [i] new_cost="<<new_cost<<"; cost_companion="<< erf((cost_companion_[i] - mean_cost_companion_) / std_cost_companion_)<< std::endl;
+
+				if((cost_companion_[i]!=0)&&(reactive==Cperson_abstract::Akp_planning)){
+					we_have_cost_companion_=true;
+					new_cost= (0.9)*cost+(0.1)*erf((cost_companion_[i] - mean_cost_companion_) / std_cost_companion_);
+				  // NOTA: special values: erf(-0) = -0.00 ; erf(Inf) = 1.00 ;
+					// NOTA: erf() computes the error function of the argument. (Creo k en mi caso es solo el valor del coste y YA esta!!!
+					//std::cout<<"(9/10)*cost="<<(0.9)*cost<< std::endl;
+					//std::cout<<"(1/10)*cost_companion_[i]="<<(0.1)*cost_companion_[i]<< std::endl;
+					//std::cout<<"cost_companion_["<<i<<"]="<<cost_companion_[i]<<"; mean_cost_companion_="<<mean_cost_companion_<<";std_cost_companion_= "<<std_cost_companion_<< std::endl;
+					//std::cout<<"(companion) mean_cost_companion_="<<mean_cost_companion_<< std::endl;
+					//std::cout<<"(companion) std_cost_companion_="<<std_cost_companion_<< std::endl;
+					//std::cout<<"(companion) cost ="<<erf((cost_companion_[i] - mean_cost_companion_) / std_cost_companion_)<< std::endl;
+
+				}else{
+					//std::cout << "2 [Cplan_local_nav::MO_norm/MO_erf] NO COMPANION COST cost="<<cost<<"; new_cost="<<new_cost<< std::endl;
+					//std::cout<<"NO HAY cost_companion_["<<i<<"]="<<cost_companion_[i]<< std::endl;
+				}
+
+				//std::cout << "2 [Cplan_local_nav::MO_norm/MO_erf] cost="<<(0.9)*cost<<"; new_cost="<<new_cost<<"; companion_cost="<< (0.1)*erf((cost_companion_[i] - mean_cost_companion_) / std_cost_companion_)<< std::endl;
+				//std::cout << "[i] new_cost="<<new_cost<<"; cost_companion="<< erf((cost_companion_[i] - mean_cost_companion_) / std_cost_companion_)<< std::endl;
+
+				cost=new_cost;
+
+				if ( cost < min_cost)
+				{
+					min_cost = cost;
+					min_cost_index = i;
+					// To print results in matlab.
+					//std::cout << "2[i] Entro en min_cost="<<min_cost<< std::endl;
+					robot_distance_cost_=erf((cost_distance_[i] - mean_cost_distance_) / std_cost_distance_);
+					robot_orientation_cost_=erf((cost_orientation_[i] - mean_cost_orientation_) / std_cost_orientation_);
+					robot_control_cost_=erf((cost_robot_[i] - mean_cost_robot_) / std_cost_robot_ );
+					robot_other_person_cost_=erf((cost_int_forces_[i] - mean_cost_int_forces_) / std_cost_int_forces_ );
+					robot_obstacles_cost_=erf((cost_obstacles_[i] - mean_cost_obstacles_) / std_cost_obstacles_);
+					robot_companion_cost_=erf((cost_companion_[i] - mean_cost_companion_) / std_cost_companion_);
+					robot_total_cost_=cost;
+					robot_ant_traj_cost_=erf((cost_past_traj_[i] - mean_cost_past_traj_) / std_cost_past_traj_);
+
+				}
+			  }
+		  }
+		  else
+		  {
+			for( unsigned int i : nondominated_end_of_plan_vertex_index_ )
+			{
+				//std::cout<<"[end of branch index] i ="<<nondominated_end_of_plan_vertex_index_[i]<< std::endl;
+				cost = cost_parameters_[0]*((cost_distance_[i] - mean_cost_distance_) / std_cost_distance_) +
+						cost_parameters_[1]*((cost_orientation_[i] - mean_cost_orientation_) / std_cost_orientation_) +
+						cost_parameters_[2]*((cost_robot_[i] - mean_cost_robot_) / std_cost_robot_ ) +
+						cost_parameters_[3]*((cost_int_forces_[i] - mean_cost_int_forces_) / std_cost_int_forces_ ) +
+						cost_parameters_[5]*((cost_obstacles_[i] - mean_cost_obstacles_) / std_cost_obstacles_)+
+						cost_parameters_[6]*((cost_past_traj_[i] - mean_cost_past_traj_) / std_cost_past_traj_);
+
+				//std::cout<<"; (ant) cost="<<cost<<"; cost_companion="<<(cost_companion_[i]-mean_cost_companion_)/std_cost_companion_<< std::endl;
+				if(cost_companion_[i]!=0){
+					we_have_cost_companion_=true;
+					cost =(9/10)*cost+(1/10)*((cost_companion_[i]-mean_cost_companion_)/std_cost_companion_);
+				}else{
+					//std::cout<<"NO HAY cost_companion_["<<i<<"]="<<cost_companion_[i]<< std::endl;
+				}
+
+				//std::cout<<"; cost="<<cost<<"; cost_companion="<<(cost_companion_[i]-mean_cost_companion_)/std_cost_companion_<< std::endl;
+
+				if ( cost < min_cost)
+				{	if(debug_real_test_companion_){
+						std::cout<<"Entro en if(cost < min_cost); min_cost="<<min_cost<< std::endl;
+					}
+					min_cost = cost;
+					min_cost_index = i;
+
+					robot_distance_cost_=((cost_distance_[i] - mean_cost_distance_) / std_cost_distance_);
+					robot_orientation_cost_=((cost_orientation_[i] - mean_cost_orientation_) / std_cost_orientation_);
+					robot_control_cost_=((cost_robot_[i] - mean_cost_robot_) / std_cost_robot_ );
+					robot_other_person_cost_=((cost_int_forces_[i] - mean_cost_int_forces_) / std_cost_int_forces_ );
+					robot_obstacles_cost_=((cost_obstacles_[i] - mean_cost_obstacles_) / std_cost_obstacles_);
+					robot_companion_cost_=((cost_companion_[i] - mean_cost_companion_) / std_cost_companion_);
+					robot_total_cost_=cost;
+					robot_ant_traj_cost_=((cost_past_traj_[i] - mean_cost_past_traj_) / std_cost_past_traj_);
+
+				}
+		    }
+
+		  }
+		//  std::cout << "antes iter_act_multiple_paths_and_best_path_.set_nondominated_paths(non_domianted_path_and_act_costs_);"<<non_domianted_path_and_act_costs_.size()<< std::endl;
+		  iter_act_multiple_paths_and_best_path_.set_nondominated_paths(non_domianted_path_and_act_costs_);
+
+		break;
+	  }
+	}
+
+
+	/* INIT fill best_cost in struct  SsavePath_multiple_paths_and_best_path */
+		const std::vector<Spose>* plans_act2 = robot_act->get_robot_planning_trajectory();
+	 	 SsavePath_cost_and_values best_act_path_and_cost_;
+	 	//std::cout << "[Cplan_local_nav::MO_norm/MO_erf] plans_act2.empty()="<<plans_act2->empty() << std::endl;
+	 	 unsigned int act_best_branch_index=min_cost_index;
+		//nondominated_end_of_plan_vertex_index_.push_back( act_best_branch_index );//only end of branches to plot the branch id
+	 	best_act_path_and_cost_.set_end_id_path(act_best_branch_index);
+		std::vector<unsigned int> act_all_ids_path_positions;
+		std::vector<Spoint> act_path_positions;
+		std::vector<std::vector<double>> iter_vect_costst_act;
+
+		//std::cout << "end Best_BRANCH_INDEX="<<act_best_branch_index<< std::endl;
+
+		 if(!plans_act2->empty()){
+			//	std::cout << "NO plans_act2->empty()"<< std::endl;
+			do
+			{
+				//nondominated_plan_vertex_index_.push_back( act_best_branch_index);
+
+				act_all_ids_path_positions.push_back( act_best_branch_index);
+				act_path_positions.push_back(plans_act2->at(act_best_branch_index));
+
+				std::vector<double> vect_costst_act;
+
+				vect_costst_act.push_back(cost_parameters_[0]*erf((cost_distance_[act_best_branch_index] - mean_cost_distance_) / std_cost_distance_));
+				vect_costst_act.push_back(cost_parameters_[1]*erf((cost_orientation_[act_best_branch_index] - mean_cost_orientation_) / std_cost_orientation_));
+				vect_costst_act.push_back(cost_parameters_[2]*erf((cost_robot_[act_best_branch_index] - mean_cost_robot_) / std_cost_robot_ ));
+				vect_costst_act.push_back(cost_parameters_[3]*erf((cost_int_forces_[act_best_branch_index] - mean_cost_int_forces_) / std_cost_int_forces_ ));
+				vect_costst_act.push_back(cost_parameters_[5]*erf((cost_obstacles_[act_best_branch_index] - mean_cost_obstacles_) / std_cost_obstacles_));
+				vect_costst_act.push_back(erf((cost_companion_[act_best_branch_index] - mean_cost_companion_) / std_cost_companion_));
+				vect_costst_act.push_back(cost_parameters_[6]*erf((cost_past_traj_[act_best_branch_index] - mean_cost_past_traj_) / std_cost_past_traj_));
+				cost = cost_parameters_[0]*erf((cost_distance_[act_best_branch_index] - mean_cost_distance_) / std_cost_distance_) +
+												cost_parameters_[1]*erf((cost_orientation_[act_best_branch_index] - mean_cost_orientation_) / std_cost_orientation_) +
+												cost_parameters_[2]*erf((cost_robot_[act_best_branch_index] - mean_cost_robot_) / std_cost_robot_ ) +
+												cost_parameters_[3]*erf((cost_int_forces_[act_best_branch_index] - mean_cost_int_forces_) / std_cost_int_forces_ ) +
+												cost_parameters_[5]*erf((cost_obstacles_[act_best_branch_index] - mean_cost_obstacles_) / std_cost_obstacles_) +
+												cost_parameters_[6]*erf((cost_past_traj_[act_best_branch_index] - mean_cost_past_traj_) / std_cost_past_traj_);
+
+				double new_cost=cost;
+				//std::cout << "(ant) [act_best_branch_index] new_cost="<<new_cost<<"; cost_companion="<<erf((cost_companion_[act_best_branch_index] - mean_cost_companion_) / std_cost_companion_) << std::endl;
+
+				if((cost_companion_[act_best_branch_index]!=0)&&(reactive==Cperson_abstract::Akp_planning)){
+					we_have_cost_companion_=true;
+					new_cost= (0.9)*cost+(0.1)*erf((cost_companion_[act_best_branch_index] - mean_cost_companion_) / std_cost_companion_);
+				}
+				//std::cout << "[act_best_branch_index] new_cost="<<new_cost<<"; cost_companion="<<erf((cost_companion_[act_best_branch_index] - mean_cost_companion_) / std_cost_companion_) << std::endl;
+
+				vect_costst_act.push_back(new_cost);
+				iter_vect_costst_act.push_back(vect_costst_act);
+				act_best_branch_index = edge_[ act_best_branch_index ].parent;
+
+			}while( act_best_branch_index > 0 );
+
+			best_act_path_and_cost_.set_total_costs_of_each_path_position(iter_vect_costst_act);
+			best_act_path_and_cost_.set_all_ids_path_positions(act_all_ids_path_positions);
+			best_act_path_and_cost_.set_path_positions(act_path_positions);
+			iter_act_multiple_paths_and_best_path_.set_best_path(best_act_path_and_cost_);
+
+			//std::cout << "out NO plans_act2->empty()"<< std::endl;
+		 }
+
+		// std::cout << "out NO plans_act2->empty() part2"<< std::endl;
+	/* FIN fill best_cost in struct SsavePath_multiple_paths_and_best_path */
+	if(!plans_act2->empty()){
+		iter_act_multiple_paths_and_best_path_.set_number_of_total_paths(1 + nondominated_end_of_plan_vertex_index_.size() );
+		//Cperson_abstract* person_obj;
+		//find_person(id_person_companion_ , &person_obj);
+		double time=0.2; // is the step time, allways is 0.2 for this planner.
+		iter_act_multiple_paths_and_best_path_.set_time_act(time);
+		iter_act_multiple_paths_and_best_path_.set_iter_act(planner_iterations_);
+	}
+
+	//fill best cost
+	best_costs_.resize( 7, 0.0 );
+	best_costs_[0] = cost_distance_[min_cost_index];
+	best_costs_[1] = cost_orientation_[min_cost_index];
+	best_costs_[2] = cost_robot_[min_cost_index];
+	best_costs_[3] = cost_int_forces_[min_cost_index];
+	best_costs_[4] = cost_obstacles_[min_cost_index];
+	best_costs_[5] = cost_companion_[min_cost_index];
+	best_costs_[6] = cost_past_traj_[min_cost_index];
+
+	//fill mean costs
+	mean_costs_.resize( 7, 0.0 );
+	mean_costs_[0] = mean_cost_distance_;
+	mean_costs_[1] = mean_cost_orientation_;
+	mean_costs_[2] = mean_cost_robot_;
+	mean_costs_[3] = mean_cost_int_forces_;
+	mean_costs_[4] = mean_cost_obstacles_;
+	mean_costs_[5] = mean_cost_companion_;
+	mean_costs_[6] = mean_cost_past_traj_;
+
+	 if(!plans_act2->empty()){
+		 iter_act_multiple_paths_and_best_path_.set_means(mean_costs_);
+	 }
+
+	//fill std costs
+	std_costs_.resize( 7, 0.0 );
+	std_costs_[0] = std_cost_distance_;
+	std_costs_[1] = std_cost_orientation_;
+	std_costs_[2] = std_cost_robot_;
+	std_costs_[3] = std_cost_int_forces_;
+	std_costs_[4] = std_cost_obstacles_;
+	std_costs_[5] = std_cost_companion_;
+	std_costs_[6] = std_cost_past_traj_;
+
+	 if(!plans_act2->empty()){
+		 iter_act_multiple_paths_and_best_path_.set_stds(std_costs_);
+	 }
+
+	//std::cout<<" FIN function: global_min_cost_index(); min_cost_index="<<min_cost_index << std::endl;
+
+	return min_cost_index;
+}
+
+Sforce Cplan_local_nav::force_persons_int_planning_virtual_vers2comp(Cperson_abstract* center , unsigned int t_companion, unsigned int t_other_pers , double min_dist2, Cperson_abstract::companion_reactive reactive, bool use_person_companion_force_repulsive)
+{
+// ESTABA AQUI LIMPIANDO CODIGO
+	//std::cout<<" IN force_persons_int_planning_virtual_vers2comp" << std::endl;
+
+	Sforce force_res;
+
+	switch(reactive)
+	{
+	case Cperson_abstract::Reactiva_repulsive:
+	case Cperson_abstract::Reactive_atractive:
+	//	std::cout<<" reactive_repulsive" << std::endl;
+		for( auto iit : nearby_person_list_)
+		{
+
+			if((t_companion<center->get_planning_trajectory()->size())&&(t_other_pers<iit->get_prediction_trajectory( )->size())){
+			//std::cout << " IN :force_persons_int_planning_virtual case(Reactiva_repulsive+atractive) center-traj_size="<<center->get_planning_trajectory()->size()<<"; iit->get_planning_trajectory( ).size="<<iit->get_planning_trajectory( )->size()<<"; t="<< t <<"; id="<<iit->get_id()<< std::endl;
+			if( *center != *iit && center->get_planning_trajectory()->at(t_companion)
+						.distance2( iit->get_prediction_trajectory( )->at(t_other_pers) ) < min_dist2 )
+			{
+				//std::cout << "2  IN :force_persons_int_planning_virtual case(Reactiva_repulsive+atractive) " <<  std::endl;
+				if(iit->get_id()!=id_person_companion_){ //  para el id companion esta fuerza ha de ser 0! Que es el caso contrario.
+						Sforce force_act=center->force( iit->get_prediction_trajectory( )->at(t_other_pers) ,  get_sfm_int_params(center,iit),
+								&(center->get_planning_trajectory()->at(t_companion) ) );
+						force_res += force_act;
+
+				}else{ // CASO: person companion!!!
+					//std::cout << " IN :force person companion " <<  std::endl;
+					//Sforce force_act=center->force( iit->get_prediction_trajectory( )->at(t_other_pers) ,  get_sfm_int_params(center,iit),&(center->get_planning_trajectory()->at(t_companion) ) , reactive);
+					// PERSON COMPANIONS, frepulsives. no entra la de ella misma con ella misma!
+					//force_res += force_act;//Sforce();  // NUNCA la ha de considerar en el path, excepto en el case face person!!! Pero si la consideraremos lugo en version reducida al mover al robot.
+				}
+			}
+		}
+	}
+	break;
+
+	case Cperson_abstract::Akp_planning:
+	default:
+		//std::cout<<" Akp_planning" << std::endl;
+		//std::cout << " IN force_persons_int_planning_virtual: case Akp_planning " <<  std::endl;
+		for( auto iit : nearby_person_list_)
+		{
+			if((t_companion<center->get_planning_trajectory()->size())&&(t_other_pers<iit->get_prediction_trajectory( )->size())){
+			if( *center != *iit && center->get_planning_trajectory()->at(t_companion)
+						.distance2( iit->get_prediction_trajectory( )->at(t_other_pers) ) < min_dist2 )
+			{
+				/// INI Add go to person goal. Do not (tratar) the person that is the goal of the group as a repulsive.
+					if(iit->get_id()!=id_person_companion_){ // (est es caso NO companions) para los companions en el path ha de ser 0, para poner situarse detras al evitar obstaculos.
+						Sforce force_act=center->forceAnticipateCollision(Cperson_abstract::Spherical, iit->get_prediction_trajectory( )->at(t_other_pers) ,  get_sfm_int_params(center,iit),
+								&(center->get_planning_trajectory()->at(t_companion) ) );
+						force_res += force_act;
+						//if(((force_act.fx>0.5)||(force_act.fy>0.5))&&see_forces_){
+							//std::cout<<"person_id=iit->get_id()=" <<iit->get_id()<< "; force_act.fx ="<<force_act.fx<< "; force_act.fy="<<force_act.fy <<  std::endl;
+						//}
+					}else{ // para id_person_companion
+						//  std::cout << " case companion people "<< std::endl;
+						//  akp. caso. people companions. = = no uso sus repulsivas para el planing, pq es el planing del grupo
+						// esas personas son parte del grupo, por tanto este no se repele respecto a ellas.
+						Sforce force_act_companion_person;
+						force_act_companion_person=center->force( iit->get_planning_trajectory( )->at(t_companion) ,  get_sfm_int_params(center,iit),
+									&(center->get_planning_trajectory()->at(t_companion) ) );
+
+						force_res+=Sforce();
+					}
+					//std::cout << "; id="<<iit->get_id()<<"; force_res.fx="<<force_res.fx<<"; force_res.fy="<<force_res.fy<<std::endl;
+				}
+			}
+		}
+	break;
+	}
+
+ // limit the force to move the robot to the side depending on the repulsive forces of other people of the group.
+	if(sqrt(force_res.fx*force_res.fx)>0.4){
+		if(force_res.fx>0){
+			force_res.fx=0.4;
+		}else{
+			force_res.fx=-0.4;
+		}
+	}
+
+	if(sqrt(force_res.fy*force_res.fy)>0.4){
+		if(force_res.fy>0){
+			force_res.fy=0.4;
+		}else{
+			force_res.fy=-0.4;
+		}
+	}
+
+	//std::cout<<" out force_persons_int_planning_virtual_vers2comp" << std::endl;
+
+	return force_res;
+}
+
+
+Sforce Cplan_local_nav::force_persons_int_planning_virtual(Cperson_abstract* center , unsigned int t, double min_dist2, Cperson_abstract::companion_reactive reactive, bool use_person_companion_force_repulsive)
+{
+
+	//std::cout<<" IN force_persons_int_planning_virtual" << std::endl;
+	Sforce force_res;
+
+	switch(reactive)
+	{
+	case Cperson_abstract::Reactiva_repulsive:
+	case Cperson_abstract::Reactive_atractive:
+		for( auto iit : nearby_person_list_)
+		{
+			//std::cout << " IN :force_persons_int_planning_virtual case(Reactiva_repulsive+atractive) center-traj_size="<<center->get_planning_trajectory()->size()<<"; iit->get_planning_trajectory( ).size="<<iit->get_planning_trajectory( )->size()<<"; t="<< t <<"; id="<<iit->get_id()<< std::endl;
+			if( *center != *iit && center->get_planning_trajectory()->at(t)
+						.distance2( iit->get_planning_trajectory( )->at(t) ) < min_dist2 )
+			{
+				//std::cout << "2  IN :force_persons_int_planning_virtual case(Reactiva_repulsive+atractive) " <<  std::endl;
+				if(iit->get_id()!=id_person_companion_){ //(caso people no companions.) para el id companion esta fuerza ha de ser 0!
+
+						Sforce force_act=center->force( iit->get_planning_trajectory( )->at(t) ,  get_sfm_int_params(center,iit),
+								&(center->get_planning_trajectory()->at(t) ) );
+						force_res += force_act;
+						//if(((force_act.fx>0.5)||(force_act.fy>0.5))&&see_forces_){
+						//std::cout<<"person_id=iit->get_id()=" <<iit->get_id()<< "; force_act.fx ="<<force_act.fx<< "; force_act.fy="<<force_act.fy <<  std::endl;
+						//}
+				}else{ // CASO: person companion!!!
+					//std::cout << " IN :force person companion " <<  std::endl;
+					//std::cout << " (para acercarse a la persona, necesita las fuerzas) People companion, pero es una fuerza diferente a la de las otras personas. To move robot or case central robot that works diferent than the lateral or One-Person side-by-side. we need the repulsive forces for the companions!!! "<<  std::endl;
+					Sforce force_act=center->force( iit->get_planning_trajectory( )->at(t) ,  get_sfm_int_params(center,iit),
+									&(center->get_planning_trajectory()->at(t) ) , reactive);
+					force_res += force_act;
+					//if(t<0){
+					//std::cout<<"person_id=iit->get_id()=" <<iit->get_id()<< "; force_act.fx ="<<force_act.fx<< "; force_act.fy="<<force_act.fy <<  std::endl;
+					//std::cout<<"params_interact_with_person(1)=" <<get_sfm_int_params(center,iit)->at(1)<< "; params_interact_with_person_companion(2) ="<<get_sfm_int_params(center,iit)->at(2)<< "; params_interact_with_person_companion(3)="<<get_sfm_int_params(center,iit)->at(3) <<  std::endl;
+					//std::cout<<"params_interact_with_person(4)=" <<get_sfm_int_params(center,iit)->at(4)<<"; min_dist2="<<min_dist2<<"; d="<<center->get_planning_trajectory()->at(t).distance2( iit->get_planning_trajectory( )->at(t) ) <<"; iit->get_id()="<<iit->get_id()<<"; id_person_companion_="<<id_person_companion_<<  std::endl;
+					//}
+				}
+
+			}
+		}
+	break;
+
+	case Cperson_abstract::Akp_planning:
+	default:
+		//std::cout << " IN force_persons_int_planning_virtual: case Akp_planning " <<  std::endl;
+		for( auto iit : nearby_person_list_)
+		{
+			if( *center != *iit && center->get_planning_trajectory()->at(t)
+						.distance2( iit->get_planning_trajectory( )->at(t) ) < min_dist2 )
+			{
+				/// INI Add go to person goal. Do not (tratar) the person that is the goal of the group as a repulsive.
+				if(iit->get_id()!=id_person_companion_){ // (caso NO companions) para el id companion esta fuerza ha de ser 0!
+					Sforce force_act=center->force( iit->get_planning_trajectory( )->at(t) ,  get_sfm_int_params(center,iit),
+								&(center->get_planning_trajectory()->at(t) ) );
+					force_res += force_act;
+					//if(((force_act.fx>0.5)||(force_act.fy>0.5))&&see_forces_){
+					//std::cout<<"person_id=iit->get_id()=" <<iit->get_id()<< "; force_act.fx ="<<force_act.fx<< "; force_act.fy="<<force_act.fy <<  std::endl;
+					//}
+				}else{ // para id_person_companion
+					//std::cout << " IN :force_persons_int_planning_virtual case: Akp_planning, salta person companion, id="<<iit->get_id()<<  std::endl;
+					// akp. caso. people companions. = = no uso sus repulsivas para el planing, pq es el planing del grupo
+					// esas personas son parte del grupo, por tanto este no se repele respecto a ellas.
+					Sforce force_act_companion_person;
+					force_act_companion_person=center->force( iit->get_planning_trajectory( )->at(t) ,  get_sfm_int_params(center,iit),
+									&(center->get_planning_trajectory()->at(t) ) );
+					if(use_person_companion_force_repulsive){  // caso side-by-side central para el path. Pero en los laterales no se usa porque el robot se situa detras.  cuando evitan obstaculos en el central pasa al modo side-by-side solo 1 persona, con la persona que este adelantada.
+						force_res+=force_act_companion_person;
+					}else{
+						force_res+=Sforce();
+					}
+					//if(((force_act_companion_person.fx>0.5)||(force_act_companion_person.fy>0.5))&&see_forces_){
+					//	std::cout<<"person_id=iit->get_id()=" <<iit->get_id()<< "; force_act_companion_person.fx ="<<force_act_companion_person.fx<< "; force_act_companion_person.fy="<<force_act_companion_person.fy <<  std::endl;
+					//std::cout << " (case central side-by-side, use repulsive forfe from comp people) id_person="<<iit->get_id()<<" (caso central, side-by-side) force_act_companion_person repulsive fx="<<force_res.fx<<"; fy="<<force_res.fy<<std::endl;
+					//std::cout<<"params_interact_with_person_companion(1)=" <<get_sfm_int_params(center,iit)->at(1)<< "; params_interact_with_person_companion(2) ="<<get_sfm_int_params(center,iit)->at(2)<< "; params_interact_with_person_companion(3)="<<get_sfm_int_params(center,iit)->at(3) <<  std::endl;
+					//std::cout<<"params_interact_with_person_companion(4)=" <<get_sfm_int_params(center,iit)->at(4)<<"; min_dist2="<<min_dist2<<"; d="<<center->get_planning_trajectory()->at(t).distance2( iit->get_planning_trajectory( )->at(t) )<<"; iit->get_id()="<<iit->get_id()<<"; id_person_companion_="<<id_person_companion_<<  std::endl;
+					//}
+				}
+			}
+		}
+	break;
+	}
+	//std::cout<<" out force_persons_int_planning_virtual" << std::endl;
+
+	return force_res;
+}
+
+
+
+Sforce Cplan_local_nav::force_persons_int_planning_virtual_robot_prediction(Cperson_abstract* center , unsigned int t, double min_dist2, Cperson_abstract::companion_reactive reactive, unsigned int id_pers_comp_rob)
+{
+
+	//std::cout << " (1)  force_persons_int_planning_virtual_robot_prediction " <<  std::endl;
+	Sforce force_res;
+	switch(reactive)
+	{
+	case Cperson_abstract::Reactiva_repulsive:
+	case Cperson_abstract::Reactive_atractive:
+		//std::cout << " IN :force_persons_int_planning_virtual case(Reactiva_repulsive+atractive) "<<  std::endl;
+		for( auto iit : person_list_)
+		{
+			if(id_pers_comp_rob!=iit->get_id()){
+				if( *center != *iit && center->get_prediction_trajectory_with_target_person()->at(t)
+							.distance2( iit->get_prediction_trajectory( )->at(t) ) < min_dist2 )
+				{
+					if(iit->get_id()!=id_person_companion_){ // (caso diferente person companion, es para el caso solo 1 person companion) para el id companion esta fuerza ha de ser 0!
+						Sforce force_act=center->force( iit->get_prediction_trajectory( )->at(t) ,  get_sfm_int_params(center,iit),
+								&(center->get_prediction_trajectory_with_target_person()->at(t) ) );;
+						force_res += force_act;
+						//if(((force_act.fx>0.5)||(force_act.fy>0.5))&&see_forces_){
+						//std::cout<<"person_id=iit->get_id()=" <<iit->get_id()<< "; force_act.fx ="<<force_act.fx<< "; force_act.fy="<<force_act.fy <<  std::endl;
+						//}
+					}else{ // para id person companion! La fuerza repulsiva es menor que para las otras personas, ya que estan interactuando, no alejandose.
+						//std::cout << " IN :force person companion " <<  std::endl;
+						Sforce force_act=center->force( iit->get_prediction_trajectory( )->at(t) ,  get_sfm_int_params(center,iit),
+								&(center->get_prediction_trajectory_with_target_person()->at(t) ) , reactive);
+						force_res += force_act;
+						//if(((force_act.fx>0.5)||(force_act.fy>0.5))&&see_forces_){
+						//	std::cout<<"person_id=iit->get_id()=" <<iit->get_id()<< "; force_act.fx ="<<force_act.fx<< "; force_act.fy="<<force_act.fy <<  std::endl;
+						//}
+					}
+				}
+			}
+		}
+	break;
+
+	case Cperson_abstract::Akp_planning:
+	default:
+		//std::cout << " (2)  force_persons_int_planning_virtual_robot_prediction (Akp_planning)" <<  std::endl;
+		for( auto iit : person_list_)
+		{
+			if(id_pers_comp_rob!=iit->get_id()){
+				if((center->get_prediction_trajectory_with_target_person()->size()>t)&&(iit->get_prediction_trajectory( )->size()>t)){
+
+					if( *center != *iit && center->get_prediction_trajectory_with_target_person()->at(t)
+							.distance2( iit->get_prediction_trajectory( )->at(t) ) < min_dist2 )
+					{
+						if(iit->get_id()!=id_person_companion_){ // (No person companion) para el id companion esta fuerza ha de ser 0!
+							Sforce force_act=center->force( iit->get_prediction_trajectory( )->at(t) ,  get_sfm_int_params(center,iit),
+									&(center->get_prediction_trajectory_with_target_person()->at(t) ) );
+							force_res += force_act;
+							//if(((force_act.fx>0.5)||(force_act.fy>0.5))&&see_forces_){
+							//	std::cout<<"person_id=iit->get_id()=" <<iit->get_id()<< "; force_act.fx ="<<force_act.fx<< "; force_act.fy="<<force_act.fy <<  std::endl;
+							//}
+						}else{	// No force person companion is for the path of the group.
+							Sforce force_act_companion_person;
+							force_act_companion_person=center->force( iit->get_prediction_trajectory( )->at(t) ,  get_sfm_int_params(center,iit),
+									&(center->get_prediction_trajectory_with_target_person()->at(t) ) );
+							force_act_companion_person.fx=force_act_companion_person.fx/2; // We try to include the force, but the behavior was bad.
+							force_act_companion_person.fy=force_act_companion_person.fy/2;
+							//if(((force_act_companion_person.fx>0.5)||(force_act_companion_person.fy>0.5))&&see_forces_){
+							//	std::cout<<"person_id=iit->get_id()=" <<iit->get_id()<< "; force_act_companion_person.fx ="<<force_act_companion_person.fx<< "; force_act_companion_person.fy="<<force_act_companion_person.fy <<  std::endl;
+							//}
+						}
+					}
+				}
+			}
+		}
+	break;
+	}
+
+	//std::cout << " (20)  force_persons_int_planning_virtual_robot_prediction (Akp_planning)" <<  std::endl;
+	return force_res;
+}
+
+
+Sforce Cplan_local_nav::force_persons_int_planning_virtual_companion_person_akp_person_prediction(Cperson_abstract* center , unsigned int t, double min_dist2, Cperson_abstract::companion_reactive reactive)
+{
+	//std::cout << " (INT PERSON FORCES) (1) force_persons_int_planning_virtual_companion_person_akp_person_prediction"<<  std::endl;
+	Sforce force_res;
+	for( auto iit : person_list_)
+	{
+		if(iit->get_id()!=id_person_companion_){
+
+			if( *center != *iit && center->get_prediction_trajectory_with_target_person()->at(t)
+							.distance2( iit->get_prediction_trajectory( )->at(t) ) < min_dist2 )
+			{
+				Sforce force_act=center->force( iit->get_prediction_trajectory( )->at(t) ,  get_sfm_int_params(center,iit),
+								&(center->get_prediction_trajectory_with_target_person()->at(t) ) )*multiply_person_companion_force_to_persons_;
+				force_res += force_act;
+				//if(((force_act.fx>0.5)||(force_act.fy>0.5))&&see_forces_){
+				//	std::cout<<"person_id=iit->get_id()=" <<iit->get_id()<< "; force_act.fx ="<<force_act.fx<< "; force_act.fy="<<force_act.fy <<  std::endl;
+				//}
+			}
+		}
+	}
+	//std::cout << " (5)  force_persons_int_planning_virtual_companion_person_akp " <<  std::endl;
+	return force_res;
+}
+
+
+Sforce Cplan_local_nav::force_persons_int_planning_virtual_robot_companion_propagation(Cperson_abstract* center , unsigned int t, double min_dist2, Cperson_abstract::companion_reactive reactive)
+{
+	//std::cout << " IN :force_persons_int_planning_virtual_robot_companion_propagation " <<  std::endl;
+	Sforce force_res;
+	Sforce force_res_other_people;
+	Sforce force_res_companion_person;
+	//std::cout<< std::endl<< std::endl<< std::endl << " !!!!!!!!!!!!!!!!!!!!!! [replan last step] IN :force_persons_int_planning_virtual_robot_companion_propagation " <<  std::endl;
+
+	switch(reactive)
+	{
+	case Cperson_abstract::Reactiva_repulsive:
+	case Cperson_abstract::Reactive_atractive:
+		//std::cout << " IN :force_persons_int_planning_virtual case(Reactiva_repulsive+atractive) " <<  std::endl;
+		for( auto iit : nearby_person_list_)
+		{
+			if( *center != *iit && center->get_planning_trajectory()->at(t)
+						.distance2( iit->get_planning_trajectory( )->at(t) ) < min_dist2 )
+			{
+				if(iit->get_id()!=id_person_companion_){ //  para el id companion esta fuerza ha de ser 0!
+					force_res += center->force( iit->get_planning_trajectory( )->at(t) ,  get_sfm_int_params(center,iit),
+									&(center->get_planning_trajectory()->at(t) ) );
+					force_res_other_people += center->force( iit->get_planning_trajectory( )->at(t) ,  get_sfm_int_params(center,iit),
+									&(center->get_planning_trajectory()->at(t) ) );
+				}else{ // reactive-repulsive need the force repulsive of the companions
+					//std::cout << "(case reactive atractive) IN :force person companion."<<  std::endl;
+					force_res += center->force( iit->get_planning_trajectory( )->at(t) ,  get_sfm_int_params(center,iit),
+														&(center->get_planning_trajectory()->at(t) ) , reactive);
+					force_res_companion_person= center->force( iit->get_planning_trajectory( )->at(t) ,  get_sfm_int_params(center,iit),
+									&(center->get_planning_trajectory()->at(t) ) , reactive);
+					//if(((force_act.fx>0.5)||(force_act.fy>0.5))&&see_forces_){
+					//	std::cout<<"person_id=iit->get_id()=" <<iit->get_id()<< "; force_res_companion_person.fx ="<<force_res_companion_person.fx<< "; force_res_companion_person.fy="<<force_res_companion_person.fy <<  std::endl;
+					//}
+				}
+			}
+		}
+	break;
+
+	case Cperson_abstract::Akp_planning:
+	default:
+		//std::cout << "force_persons_int_planning_virtual_robot_companion_propagation IN : Akp_planning " <<  std::endl;
+		for( auto iit : nearby_person_list_)
+		{
+			if( *center != *iit && center->get_planning_trajectory()->at(t)
+												.distance2( iit->get_planning_trajectory( )->at(t) ) < min_dist2 )
+			{
+				if(iit->get_id()!=id_person_companion_){ //  para el id companion esta fuerza ha de ser 0!
+					force_res += center->force( iit->get_planning_trajectory( )->at(t) ,  get_sfm_int_params(center,iit),
+									&(center->get_planning_trajectory()->at(t) ) );
+					//std::cout << " IN :force_persons_int_planning_virtual case: Akp_planning, person id="<<iit->get_id()<<  std::endl;
+					if(debug_antes_subgoals_entre_AKP_goals_){
+						force_res.print();
+					}
+					force_res_other_people += center->force( iit->get_planning_trajectory( )->at(t) ,  get_sfm_int_params(center,iit),
+														&(center->get_planning_trajectory()->at(t) ) );
+				}else{ //use the repulsive forces for the companions (different for the other repulsive forces) in the central accompaniment or to move the robot in all the cases.
+					//std::cout << " IN :force_persons_int_planning_virtual case: Akp_planning, salta person companion, id="<<iit->get_id()<<  std::endl;
+					force_res += center->force( iit->get_planning_trajectory( )->at(t) ,  get_sfm_int_params(center,iit),
+														&(center->get_planning_trajectory()->at(t) ) );
+					force_res_companion_person= center->force( iit->get_planning_trajectory( )->at(t) ,  get_sfm_int_params(center,iit),
+															&(center->get_planning_trajectory()->at(t) ) , reactive);
+					//if(((force_act.fx>0.5)||(force_act.fy>0.5))&&see_forces_){
+					//	std::cout<<"person_id=iit->get_id()=" <<iit->get_id()<< "; force_res_companion_person.fx ="<<force_res_companion_person.fx<< "; force_res_companion_person.fy="<<force_res_companion_person.fy <<  std::endl;
+					//}
+				}
+			}
+		}
+	break;
+	}
+
+	last_step_robot_other_person_cost_=force_res_companion_person.module2();
+	last_step_robot_companion_cost_=force_res_companion_person.module2();
+
+	//std::cout << " OUT: force_persons_int_planning_virtual_robot_companion_propagation "<< std::endl;
+
+	return force_res;
+}
+
+
+
+Sforce Cplan_local_nav::force_persons_int_robot_prediction_virtual(const SpointV& center , unsigned int t, double min_dist2)
+{
+	//calculates the resultant forces to all nearby people due to the robot position
+	Sforce force_res;
+    for( auto iit : nearby_person_list_)
+    {
+        if( center.distance2( iit->get_planning_trajectory( )->at(t) ) < min_dist2 )
+        {
+            force_res += iit->force( iit->get_prediction_trajectory( )->at(t) ,  get_sfm_int_params(iit),
+            		&center );
+        }
+    }
+	return force_res;
+}
+
+Sforce Cplan_local_nav::force_objects_laser_int_planning_virtual( Cperson_abstract* person, unsigned int planning_index, double min_dist2, bool robot_collision_check_flag)
+{	// calculate force for obstacles.
+	Sforce force_res;
+	Sforce force_res_iter;
+	const SpointV_cov* virtual_current_point;
+	if ( planning_index == 0)
+	{
+		virtual_current_point = &(person->get_current_pointV());
+	}
+	else
+	{
+		virtual_current_point = &(person->get_planning_trajectory()->at(planning_index) );
+	}
+	nearby_obstacle_list_.clear();
+	number_obstacles_big_force_=1;
+
+	for( Spoint iit : laser_obstacle_list_)
+	{
+		//there is a list for persons and another for robot(s). This function is for obstacles
+		double d2 = person->get_current_pointV().distance( iit );
+		if( d2 < min_dist2)//square distance 5^2
+		{
+
+			force_res_iter=person->force_sphe( iit , get_sfm_int_params(person), virtual_current_point );
+			force_res +=force_res_iter;
+			if( robot_collision_check_flag && d2 < 1.0 )//detect ultra nearby obstacles and check after propagation if collision took place
+			{
+				nearby_obstacle_list_.push_back( iit );
+			}
+			if((force_res_iter.fx>1.0)||(force_res_iter.fy>1.0)||(force_res_iter.fx<-1.0)||(force_res_iter.fy<-1.0)){
+				number_obstacles_big_force_++;
+			}
+		}
+	}
+
+	if(number_obstacles_big_force_>1){
+		number_obstacles_big_force_=number_obstacles_big_force_-1;
+	}
+
+	force_res.fx=force_res.fx/number_obstacles_big_force_; // To limit the obstacle force, because our obstacles are a lot of cylinders over the map obstacles, not only one obstacle.
+	force_res.fy=force_res.fy/number_obstacles_big_force_; // In a future, if it is included a best obstacle detection (only one real obstacle, not a lot of cylinders), this will be not needed.
+
+	return force_res;
+}
+
+
+Sforce Cplan_local_nav::force_objects_laser_int_planning_virtual_robot_propagation( Cperson_abstract* person, unsigned int planning_index, double min_dist2, bool robot_collision_check_flag)
+{
+	Sforce force_res;
+	Sforce force_res_iter;
+	const SpointV_cov* virtual_current_point;
+	if ( planning_index == 0)
+	{
+		virtual_current_point = &(person->get_current_pointV());
+	}
+	else
+	{
+		virtual_current_point = &(person->get_prediction_trajectory_with_target_person()->at(planning_index) );
+	}
+	nearby_obstacle_list_.clear();
+	number_obstacles_big_force_=1;
+
+	for( Spoint iit : laser_obstacle_list_)
+	{
+		//there is a list for persons and another for robot(s). This function is for obstacles
+		double d2 = person->get_current_pointV().distance( iit );
+		if( d2 < min_dist2)//square distance 5^2
+		{
+			force_res_iter=person->force_sphe( iit , get_sfm_int_params(person), virtual_current_point );
+			force_res +=force_res_iter;
+			if( robot_collision_check_flag && d2 < 1.0 )//detect ultra nearby obstacles and check after propagation if collision took place
+			{
+				nearby_obstacle_list_.push_back( iit );
+			}
+			if((force_res_iter.fx>1.0)||(force_res_iter.fy>1.0)||(force_res_iter.fx<-1.0)||(force_res_iter.fy<-1.0)){
+				number_obstacles_big_force_++;
+			}
+		}
+	}
+
+	if(number_obstacles_big_force_>1){
+		number_obstacles_big_force_=number_obstacles_big_force_-1;
+	}
+
+	force_res.fx=force_res.fx/number_obstacles_big_force_; // To limit the obstacle force, because our obstacles are a lot of cylinders over the map obstacles, not only one obstacle.
+	force_res.fy=force_res.fy/number_obstacles_big_force_; // In a future, if it is included a best obstacle detection (only one real obstacle, not a lot of cylinders), this will be not needed.
+
+	return force_res;
+}
+
+bool Cplan_local_nav::check_collision( const SpointV_cov& p2 ,  int t, SpointV_cov centre_group)
+{	// CHECK collision of the path for the robot shape and for the companion shape (to calculate the companion cost and angle, before)
+	// IMPORTANT: Do not habilitate all the comments in loops for! Hight computational cost!. Then the robot do not work propertly and it is not fault of the method.
+	Spoint p=Spoint(p2.x,p2.y,p2.time_stamp);
+
+	if((centre_group.x==0.0)&&(centre_group.y==0.0)&&(centre_group.vx==0)&&(centre_group.vy==0)){
+		centre_group=SpointV_cov(p2.x,p2.y,p2.time_stamp,p2.vx,p2.vy,p2.cov);
+	}else{
+		//std::cout << " Spoint (p=robot):  " << std::endl;
+		//p.print();
+		//std::cout << " SpointV_cov (centre_group):  " << std::endl;
+		//centre_group.print();
+	}
+
+	bool collision=false;
+	// Start: Init values of companion collisions
+	case_dynamic_=false;
+	double  robot_radi=robot_->get_platform_radii();
+
+	double real_robot_person_distance; // real distance between robot and person companion.
+
+	if(bool_case_person_companion_){
+		real_robot_person_distance=calc_robot_person_companion_distance_companion_person_akp();
+	}else{
+		real_robot_person_distance=calc_robot_person_companion_distance();
+	}
+
+	// case only one person side-by-side
+	min_distance_collision_=(robot_person_proximity_distance_+2*robot_radi+little_augmented_collision_margin_)/2;
+
+	std::vector<double> min_distances_vector;
+	min_distances_vector.clear();
+	bool_collision_companion_=false;
+	double collision_threshold=(real_robot_person_distance+2*robot_radi+little_augmented_collision_margin_)/2;
+	// End: Init values of companion collisions
+
+	//check colision with map: TO BE DEPRECATED [This comment is of Gonzalo], comment by ely: for me in real environments and Gazebo, it works well and the robot detect and avoid the obstacles (ya sea por esto o por la deteccion del laser directamente).
+	if ( read_force_map_success_ && !is_cell_clear_map( p.x, p.y ) )
+	{
+		collision = true;
+		bool_collision_companion_=true; // colision, pero por choque con el mapa.
+	}
+
+	//check collision with obstacles (laser), prior is necessary to calculate force_objects_laser_int_planning()
+	for( Spoint iit : laser_obstacle_list_ )
+	{
+		// START: calculate normal collisions only with robot shape
+		if( iit.distance( p )  < (robot_->get_platform_radii()+obstacle_radi_)) // 0.37, margen colision con objeto, colchon de seguridad! Si usas directamente el laser no tienes en cuenta el radio del robot, ni una distancia de seguridad y puede chocar.
+		{
+			if(debug_real_test_companion3_){
+				std::cout << "(!!! OBST check_collision !!!) iit.distance2( p )="<< iit.distance2( p )<<" iit.distance( p )"<< iit.distance( p )<<"< robot_->get_platform_radii_2()="<<robot_->get_platform_radii_2()<< std::endl;
+			}
+			collision = true;
+		}
+		// END:  calculate normal collisions only with robot shape
+
+		// START: Calculate companion collisions with the group shape.
+		double anisotropy=calculate_anisotropy( p2, iit );
+		double collision_distance_obst_act=sqrt((iit.distance( centre_group )-obstacle_radi_amp_)*(iit.distance( centre_group )-obstacle_radi_amp_));
+
+		if( (collision_distance_obst_act < collision_threshold)&& (anisotropy>anisotropy_threshold_) ) // ojo! he cambiado distance2 por distance.
+		{
+			min_distances_vector.push_back(collision_distance_obst_act);
+			bool_collision_companion_=true;
+			//std::cout << "obstacle_COLLISION=" << std::endl;
+		}
+		//END:	Calculate companion collisions with the group shape.
+	}
+	// START: debug path of the companion collisions calculation
+	if(debug_gazebo_journal_){
+		// calc min distance to obstacle collision!
+		double act_min_collision_dist=0;
+		if(!min_distances_vector.empty()){
+			act_min_collision_dist=min_distances_vector[0];
+		}
+
+		for(unsigned int u=0; u<min_distances_vector.size();u++){
+			if(min_distances_vector[u]<act_min_collision_dist){
+				act_min_collision_dist=min_distances_vector[u];
+			}
+		}
+		if(!min_distances_vector.empty()){
+			std::cout << "Collision found!! iit.distance( initial_robot_spoint_ )=act_min_collision_dist= "<< act_min_collision_dist<< std::endl;
+		}
+	}
+	//END: debug path of the companion collisions calculation
+
+	//if t = -1, then no collision is calculated wrt ppl
+	if ( t > -1 )
+	{
+		double d,det;
+		switch( ppl_collision_mode_ )
+		{
+		  case 0: //normal mode, quadratic distance to person, independent of distribution
+			//std::cout << "enter case 0" << std::endl;
+			for( auto iit : nearby_person_list_)
+			{
+				// START: calculate simple collisions, only with robot shape.
+				if((iit->get_id()!=id_person_companion_)||(use_persoon_companion_to_calculate_plan_)){
+					d = iit->get_planning_trajectory( )->at(t).distance( p );
+					if ( d < (robot_->get_platform_radii()+person_radi_))
+					{
+						//std::cout << "(collision person   rrr) id!=person_comp; iit->get_id()="<<iit->get_id() <<"; d="<<d<<"; person_radi_="<<person_radi_<<"; robot_radi="<<robot_->get_platform_radii()<<"; sum="<<robot_->get_platform_radii()+person_radi_<< std::endl;
+						//std::cout << " case 0; d="<<d<<"; id_person_companion_="<<id_person_companion_<< std::endl;
+						collision = true;
+					}
+				}else{ // person companion for the path. NO collision, are part of the group.
+
+				}
+				// END: calculate simple collisions, only with robot shape.
+
+				// START: calculate companion collisions with the group shape
+				if(iit->get_id()!=id_person_companion_){ //  add due to person companion, esta persona no hay que calcular colisiones respecto a ella.
+					double d2 = sqrt((iit->get_planning_trajectory( )->at(t).distance( centre_group )-person_radi_amp_)*(iit->get_planning_trajectory( )->at(t).distance( centre_group )-person_radi_amp_));
+					double anisotropy=calculate_anisotropy( p2, iit->get_planning_trajectory( )->at(t) );
+
+					if ( (d2 < collision_threshold)&&(anisotropy>anisotropy_threshold_))
+					{
+						//std::cout << "person_COLLISION id="<<iit->get_id() << std::endl;
+						case_dynamic_=true;
+						bool_collision_companion_=true;
+						min_distances_vector.push_back(d2); // - robot_radi, margen de seguridad entre obstaculos.
+					}
+				}
+				// END: calculate companion collisions with the group shape
+			}
+			break;
+		  case 1://mahalanobis distance, considers collision if inside the std ellipsoid: TOO RESTRICTIVE...
+			 // std::cout << "enter case 1" << std::endl;
+			for( auto iit : nearby_person_list_)
+			{
+				// START: calculate simple collisions, only with robot shape.
+				if((iit->get_id()!=id_person_companion_)||(use_persoon_companion_to_calculate_plan_)){
+					d = iit->get_planning_trajectory( )->at(t).cov_dist(p,det);
+					if ( d < 1.0 )
+					{
+						//std::cout << " case 1"<< std::endl;
+						//std::cout << "Colision detected, pr distance = " << d << std::endl;
+						collision = true;
+					}
+				}else{ // person companion for the path. NO collision, are part of the group.
+
+				}
+				// END: calculate simple collisions, only with robot shape.
+
+				// START: calculate companion collisions with the group shape
+				if(iit->get_id()!=id_person_companion_){
+					double anisotropy=calculate_anisotropy( p2, iit->get_planning_trajectory( )->at(t) );
+					double d2 = sqrt((iit->get_planning_trajectory( )->at(t).cov_dist(centre_group,det)-person_radi_amp_)*(iit->get_planning_trajectory( )->at(t).cov_dist(centre_group,det)-person_radi_amp_));
+					if ( (d2 < collision_threshold)&&(anisotropy>anisotropy_threshold_) )
+					{
+						//std::cout << "person_COLLISION id="<<iit->get_id() << std::endl;
+						case_dynamic_=true;
+						bool_collision_companion_=true;
+						min_distances_vector.push_back(d2);
+					}
+				}
+				// END: calculate companion collisions with the group shape
+			}
+			break;
+		  case 2 ://mahalanobis distance, half the std, much less restrictive
+			//  std::cout << "enter case 2" << std::endl;
+			for( auto iit : nearby_person_list_)
+			{
+				// START: calculate simple collisions, only with robot shape.
+				if((iit->get_id()!=id_person_companion_)||(use_persoon_companion_to_calculate_plan_)){
+					d = iit->get_planning_trajectory( )->at(t).cov_dist(p,det);
+					if ( det > 1.0)
+					{
+						if ( d < 0.5 )
+						{
+							collision = true;
+						}
+					}
+					else
+					{
+						d = iit->get_planning_trajectory( )->at(t).distance( p );
+						if ( d < (robot_->get_platform_radii()+person_radi_) )
+						{
+							collision = true;
+						}
+					}
+				}else{  // person companion for the path. NO collision, are part of the group.
+
+				}
+				// END: calculate simple collisions, only with robot shape.
+
+				// START: calculate companion collisions with the group shape
+				if(iit->get_id()!=id_person_companion_){
+					double d2 = sqrt((iit->get_planning_trajectory( )->at(t).cov_dist(centre_group,det)-person_radi_amp_)*(iit->get_planning_trajectory( )->at(t).cov_dist(centre_group,det)-person_radi_amp_));
+					double anisotropy=calculate_anisotropy( p2, iit->get_planning_trajectory( )->at(t) );
+					if ( det > 1.0)
+					{
+						if ( (d2 < collision_threshold)&&(anisotropy>anisotropy_threshold_) )
+						{
+							//std::cout << "person_COLLISION id="<<iit->get_id() << std::endl;
+							case_dynamic_=true;
+							bool_collision_companion_=true;
+							min_distances_vector.push_back(d2);
+						}
+					}else{
+						double d2 = sqrt((iit->get_planning_trajectory( )->at(t).distance( centre_group )-person_radi_amp_)*(iit->get_planning_trajectory( )->at(t).distance( centre_group )-person_radi_amp_));
+						double anisotropy=calculate_anisotropy( p2, iit->get_planning_trajectory( )->at(t) );
+						if ( (d2 < collision_threshold)&& (anisotropy>anisotropy_threshold_) )
+						{
+							//std::cout << "person_COLLISION id="<<iit->get_id() << std::endl;
+							case_dynamic_=true;
+							bool_collision_companion_=true;
+							min_distances_vector.push_back(d2);
+						}
+					}
+				}
+				// END: calculate companion collisions with the group shape
+			}
+			break;
+		  case 3 :
+			  //std::cout << "enter case 3" << std::endl;
+			for( auto iit : nearby_person_list_)
+			{
+				// START: calculate simple collisions, only with robot shape.
+				if((iit->get_id()!=id_person_companion_)||(use_persoon_companion_to_calculate_plan_)){
+					d = iit->get_planning_trajectory( )->at(t).cov_dist(p,det);
+					if ( det > 1.0)
+					{
+						if ( d < 0.3 )
+						{
+							collision = true;
+						}
+					}
+					else
+					{
+						d = iit->get_planning_trajectory( )->at(t).distance( p );
+						if ( d < (robot_->get_platform_radii()+person_radi_) )
+						{
+							collision = true;
+						}
+					}
+				}else{ // person companion for the path. NO collision, are part of the group.
+
+				}
+				// END: calculate simple collisions, only with robot shape.
+				// START: calculate companion collisions with the group shape
+				if(iit->get_id()!=id_person_companion_){
+					double d2 = sqrt((iit->get_planning_trajectory( )->at(t).cov_dist(centre_group,det)-person_radi_amp_)*(iit->get_planning_trajectory( )->at(t).cov_dist(centre_group,det)-person_radi_amp_));
+					double anisotropy=calculate_anisotropy( p2, iit->get_planning_trajectory( )->at(t) );
+					if ( det > 1.0)
+					{
+						if ( (d2 < collision_threshold)&&(anisotropy>anisotropy_threshold_)  ) // de momento, para mi todas las suyas con constantes, han de ser menores que la distancia robot+persona. (luego ya veremos si va bien o no)
+						{
+							//	std::cout << "person_COLLISION id="<<iit->get_id() << std::endl;
+							case_dynamic_=true;
+							bool_collision_companion_=true;
+							min_distances_vector.push_back(d2);
+						}
+					}else{
+						double d2 = sqrt((iit->get_planning_trajectory( )->at(t).distance( centre_group )-person_radi_amp_)*(iit->get_planning_trajectory( )->at(t).distance( centre_group )-person_radi_amp_));
+						double anisotropy=calculate_anisotropy( p2, iit->get_planning_trajectory( )->at(t) );
+						if ( (d2 < collision_threshold)&&(anisotropy>anisotropy_threshold_) )
+						{
+							//std::cout << "person_COLLISION id="<<iit->get_id() << std::endl;
+							case_dynamic_=true;
+							bool_collision_companion_=true;
+							min_distances_vector.push_back(d2);
+						}
+					}
+				}
+				// END: calculate companion collisions with the group shape
+			}
+			break;
+		}
+
+			//calculate probabilities. NOT implemented, insignificant collision values when cov is high
+            //pr_no_col *= 1 - gaussian_constant_ / sqrt(det) * exp( -0.5*d );// * robot_->get_platform_radii_2();//area = pi * r^2 (pi included in gaussian_contant)
+            //std::cout << "Pr of no colision joint = " << pr_no_col << " and distance = " << d << " det = " << det << std::endl;
+	}
+
+	// START: final part calculate the min collision distance for companion
+	// obtain minimun collision distance.
+	if(!min_distances_vector.empty()){ // si hay alguna distancia minima.
+
+		double min_dist;
+		if(min_distance_collision_>min_distances_vector[0]){
+			min_dist=min_distances_vector[0];
+		}else{
+			min_dist=min_distance_collision_;
+		}
+		for( unsigned int in=0; in<min_distances_vector.size(); in++){
+			if(min_distances_vector[in]<min_dist){
+				min_dist=min_distances_vector[in];
+			}
+		}
+		min_distance_collision_=min_dist; // sacar fuera la min dist
+	}
+
+	//std::cout << "min_distance_collision_= "<<min_distance_collision_<< std::endl;
+	//std::cout << "bool_collision_companion_= "<<bool_collision_companion_ << std::endl;
+	//std::cout << "FIN function: check_collision_companion " << std::endl;
+
+	// END: final part calculate the min collision distance for companion
+
+	return collision;
+}
+
+
+bool Cplan_local_nav::check_collision_final( const Spoint& p ,  int t)
+{
+	//std::cout << "  IN      check_collision_final" << std::endl;
+	// does not work properly on real scenarios! long term issue (it refers to the map_obs. But the detection of obstacles with laser works properly.
+	//check colision with map: TO BE DEPRECATED [comment of gonzalo]. This maybe do not work, but we detect allways the map obstacles with the laser.
+	if ( read_force_map_success_ && !is_cell_clear_map( p.x, p.y ) )
+	{
+		return  true;
+	}
+
+	//check collision with obstacles (laser), prior is necessary to calculate force_objects_laser_int_planning()
+	for( Spoint iit : laser_obstacle_list_ )
+	{
+		if( iit.distance( p )  < (robot_->get_platform_radii()+obstacle_radi2_ )) // 0.37, margen colision con objeto, colchon de seguridad!
+		{
+			//std::cout << "(!!! check_collision_final !!!) iit.distance( p )="<< iit.distance( p )<<"< robot_->get_platform_radii()="<<robot_->get_platform_radii()<<"; obstacle_radi2_="<<obstacle_radi2_<< std::endl;
+			return true;
+		}
+	}
+
+	//if t = -1, then no collision is calculated wrt ppl
+	if ( t > -1 )
+	{
+		double d,det;
+		switch( ppl_collision_mode_ )
+		{
+		  case 0: //normal mode, quadratic distance to person, independent of distribution
+			for( auto iit : nearby_person_list_)
+			{
+					d = iit->get_planning_trajectory( )->at(t).distance( p );
+					if ( d < (robot_->get_platform_radii()+person_radi2_))
+					{
+						//std::cout << " case 0; d="<<d<<" person_radi2_="<<person_radi2_<<"; robot_->get_platform_radii()="<<robot_->get_platform_radii()<<"; t="<<t<<"; sum="<<robot_->get_platform_radii()+person_radi2_<< std::endl;
+						return true;
+					}
+
+			}
+			break;
+		  case 1://mahalanobis distance, considers collision if inside the std ellipsoid: TOO RESTRICTIVE...
+			for( auto iit : nearby_person_list_)
+			{
+
+					d = iit->get_planning_trajectory( )->at(t).cov_dist(p,det);
+					if ( d < 1.0 )
+					{
+						//std::cout << " case 1 Colision detected, pr distance = " << d << std::endl;
+						return true;
+					}
+
+			}
+			break;
+		  case 2 ://mahalanobis distance, half the std, much less restrictive
+			for( auto iit : nearby_person_list_)
+			{
+
+					d = iit->get_planning_trajectory( )->at(t).cov_dist(p,det);
+					if ( det > 1.0)
+					{
+						if ( d < 0.5 )
+						{
+							return true;
+						}
+					}
+					else
+					{
+						d = iit->get_planning_trajectory( )->at(t).distance( p );
+						if ( d < (robot_->get_platform_radii()+person_radi2_) )
+						{
+							return true;
+						}
+					}
+			}
+			break;
+		  case 3 :
+			for( auto iit : nearby_person_list_)
+			{
+					d = iit->get_planning_trajectory( )->at(t).cov_dist(p,det);
+					if ( det > 1.0)
+					{
+						if ( d < 0.3 )
+						{
+							return true;
+						}
+					}
+					else
+					{
+						d = iit->get_planning_trajectory( )->at(t).distance( p );
+						if ( d < (robot_->get_platform_radii()+person_radi2_) )
+						{
+							return true;
+						}
+					}
+
+			}
+			break;
+
+		}
+			//calculate probabilities. NOT implemented, insignificant collision values when cov is high
+            //pr_no_col *= 1 - gaussian_constant_ / sqrt(det) * exp( -0.5*d );// * robot_->get_platform_radii_2();//area = pi * r^2 (pi included in gaussian_contant)
+            //std::cout << "Pr of no colision joint = " << pr_no_col << " and distance = " << d << " det = " << det << std::endl;
+	}
+
+	return false;
+}
+
+
+void Cplan_local_nav::set_number_of_vertex( unsigned int n )
+{
+	max_iter_ = n;
+    edge_.reserve((size_t)max_iter_);
+    cost_robot_.reserve((size_t)max_iter_);
+    cost_int_forces_.reserve((size_t)max_iter_);
+    cost_obstacles_.reserve((size_t)max_iter_);
+    cost_distance_.reserve((size_t)max_iter_);
+    cost_orientation_.reserve((size_t)max_iter_);
+    cost_past_traj_.reserve((size_t)max_iter_);
+    nodes_in_branch_.reserve((size_t)max_iter_);
+	random_goals_.reserve( (size_t)max_iter_ );
+	//random_goals_p1_.reserve( (size_t)max_iter_ );
+	//random_goals_p2_.reserve( (size_t)max_iter_ );
+
+}
+
+void Cplan_local_nav::preprocess_global_parameters(Crobot* robot_act)
+{
+	switch( global_mode_)
+	{
+	case Cplan_local_nav::Weighted_sum_erf :
+		calculate_normalization_cost_functions_parameters_erf();
+		break;
+	case Cplan_local_nav::MO_erf :
+		calculate_normalization_cost_functions_parameters_erf();
+		calculate_non_dominated_solutions();
+		break;
+	case Cplan_local_nav::MO_norm :
+		calculate_normalization_cost_functions_parameters_norm(robot_act);
+		calculate_non_dominated_solutions();
+		break;
+	case Cplan_local_nav::Scalarization :
+	case Cplan_local_nav::Weighted_sum_norm :
+		//calcualte normalization: we need utopia point and max value
+		calculate_normalization_cost_functions_parameters_norm(robot_act);
+		break;
+	}
+}
+
+void Cplan_local_nav::calculate_non_dominated_solutions()
+{
+	//fill the multiobjective cost structure
+	multicosts_.clear();
+	Smulticost m(0,5);
+	bool is_candidate_dominated;
+	//std::cout << " end_of_branches_index_.size()="<<end_of_branches_index_.size()<< std::endl;
+
+	for( unsigned int i : end_of_branches_index_ )
+	{
+		m.id = i;
+		//raw costs
+		/*m.cost[0] = cost_distance_[i];// [0] Goal cost
+		m.cost[1] = cost_orientation_[i];// [1] orientation cost
+		m.cost[2] = cost_robot_[i];// [2] Robot cost
+		m.cost[3] = cost_int_forces_[i];// [3]Interacting people cost
+		m.cost[4] = cost_obstacles_[i];// [5] obstacles cost*/
+		//normalized costs
+		m.cost[0] = (cost_distance_[i] - mean_cost_distance_) / std_cost_distance_;// [0] Goal cost
+		m.cost[1] = (cost_orientation_[i] - mean_cost_orientation_) / std_cost_orientation_;// [1] orientation cost
+		m.cost[2] = (cost_robot_[i] - mean_cost_robot_) / std_cost_robot_;// [2] Robot cost
+		m.cost[3] = (cost_int_forces_[i] - mean_cost_int_forces_) / std_cost_int_forces_;// [3]Interacting people cost
+		m.cost[4] = (cost_obstacles_[i] - mean_cost_obstacles_) / std_cost_obstacles_;// [5] obstacles cost
+		multicosts_.push_back(m);
+	}
+
+	//std::cout << " multicosts_.size()="<<multicosts_.size()<< std::endl;
+	//calculate the non-dominated set
+	nondominated_multicosts_.clear();
+	for( Smulticost i : multicosts_  )
+	{
+		//std::cout << " int first for:"<< std::endl;
+		//i.print_ml();
+		is_candidate_dominated = false;
+		for( std::list<Smulticost>::iterator j = nondominated_multicosts_.begin() ; j != nondominated_multicosts_.end(); j++ )
+		{
+			//std::cout << " int seconf for:"<< std::endl;
+			//check it is not the same
+			if ( i!=*j )
+			{
+				// check if i dominates j
+				if ( i < *j )
+				{
+					//std::cout << " i:"<< std::endl;
+					//i.print();
+					//std::cout << " j:"<< std::endl;
+					//j->print();
+					j = nondominated_multicosts_.erase(j);
+				}
+				// check if j dominates i: break and look for a new candidate i
+				else if ( *j < i )
+				{
+					is_candidate_dominated = true;
+					break;
+				}
+
+				//else, nothing happens until all set is compared with i
+			}
+		}
+		if( !is_candidate_dominated )
+		{
+			//std::cout << " int if"<< std::endl;
+			nondominated_multicosts_.push_back(i);
+		}
+	}
+	//std::cout << " nondominated_multicosts_.size()="<<nondominated_multicosts_.size()<< std::endl;
+	//std::cout << "size of the non-dominated set = " << set.size() << " / " << end_of_branches_index_.size() << std::endl;
+}
+
+void Cplan_local_nav::calculate_normalization_cost_functions_parameters_erf()
+{
+	mean_cost_int_forces_=0.0;
+	mean_cost_robot_=0.0;
+	mean_cost_obstacles_=0.0;
+	mean_cost_past_traj_=0.0;
+	mean_cost_distance_=0.0;
+	mean_cost_orientation_=0.0;
+	mean_cost_companion_=0.0;  // mean companion cost, add by ely.
+
+	double N = (double)end_of_branches_index_.size();
+	for( unsigned int i : end_of_branches_index_ )
+	{
+		mean_cost_int_forces_ += cost_int_forces_[i];
+		mean_cost_robot_ +=cost_robot_[i];
+		mean_cost_obstacles_ +=cost_obstacles_[i];
+		mean_cost_past_traj_ += cost_past_traj_[i];
+		mean_cost_distance_ +=cost_distance_[i];
+		mean_cost_orientation_ +=cost_orientation_[i];
+		mean_cost_companion_ +=cost_companion_[i]; // mean companion cost, add by ely.
+
+	}
+	mean_cost_int_forces_ /= N;
+	mean_cost_robot_ /= N;
+	mean_cost_obstacles_ /= N;
+	mean_cost_past_traj_ /= N;
+	mean_cost_distance_ /= N;
+	mean_cost_orientation_ /= N;
+	mean_cost_companion_ /= N; // mean companion cost, add by ely.
+
+
+	std_cost_int_forces_=0.0;
+	std_cost_robot_=0.0;
+	std_cost_obstacles_=0.0;
+	std_cost_past_traj_=0.0;
+	std_cost_distance_=0.0;
+	std_cost_orientation_=0.0;
+	std_cost_companion_=0.0; // std companion cost, add by ely.
+
+	double d;
+	N -= 1.0;//unbiased std
+	for( unsigned int i : end_of_branches_index_ )
+	{
+		d = cost_int_forces_[i] - mean_cost_int_forces_;
+		std_cost_int_forces_ += d*d;
+
+		d = cost_robot_[i] - mean_cost_robot_;
+		std_cost_robot_ += d*d;
+
+		d = cost_obstacles_[i] - mean_cost_obstacles_;
+		std_cost_obstacles_ += d*d;
+
+
+		d = cost_past_traj_[i] - mean_cost_past_traj_;
+		std_cost_past_traj_ += d*d;
+
+
+		d = cost_distance_[i] - mean_cost_distance_;
+		std_cost_distance_ += d*d;
+
+
+		d = cost_orientation_[i] - mean_cost_orientation_;
+		std_cost_orientation_ += d*d;
+
+
+		d = cost_companion_[i] - mean_cost_companion_; // std companion cost, add by ely.
+		std_cost_companion_ += d*d; // std companion cost, add by ely.
+	}
+	if( std_cost_int_forces_ > 0.01 )
+	{
+		std_cost_int_forces_ /= N; std_cost_int_forces_ = sqrt(std_cost_int_forces_);
+	}
+	else
+		std_cost_int_forces_ = 0.01;
+	if( std_cost_robot_ > 0.01)
+	{
+		std_cost_robot_ /= N; std_cost_robot_ = sqrt(std_cost_robot_);
+	}
+	else
+		std_cost_robot_ = 0.01;
+	if( std_cost_obstacles_ > 0.01)
+	{
+		std_cost_obstacles_ /= N;std_cost_obstacles_ = sqrt(std_cost_obstacles_);
+	}
+	else
+		std_cost_obstacles_ = 0.01;
+	if( std_cost_past_traj_ > 0.01)
+	{
+		std_cost_past_traj_ /= N; std_cost_past_traj_= sqrt(std_cost_past_traj_);
+	}
+	else
+		std_cost_past_traj_ = 0.01;
+	if( std_cost_distance_ > 0.01)
+	{
+		std_cost_distance_ /= N; std_cost_distance_= sqrt(std_cost_distance_);
+	}
+	else
+		std_cost_distance_ = 0.01;
+	if( std_cost_orientation_ > 0.01)
+	{
+		std_cost_orientation_ /= N; std_cost_orientation_= sqrt(std_cost_orientation_);
+	}
+	else
+		std_cost_orientation_ = 0.01;
+
+	if( std_cost_companion_ > 0.01)  // std companion cost, add by ely.
+	{
+		std_cost_companion_ /= N; std_cost_companion_= sqrt(std_cost_companion_);
+	}
+	else
+		std_cost_companion_ = 0.01;
+
+	//printing results
+	//std::cout << "cost_distance = (" << mean_cost_distance_ << " , " << std_cost_distance_ << std::endl;
+	//std::cout << "cost_orientation = (" << mean_cost_orientation_ << " , " << std_cost_orientation_ << std::endl;
+	//std::cout << "cost_int_forces = (" << mean_cost_int_forces_ << " , " << std_cost_int_forces_ << std::endl;
+	//std::cout << "cost_robot = (" << mean_cost_robot_ << " , " << std_cost_robot_ << std::endl;
+	//std::cout << "cost_obstacles = (" << mean_cost_obstacles_ << " , " << std_cost_obstacles_ << std::endl;
+	//std::cout << "cost_past_traj = (" << mean_cost_past_traj_ << " , " << std_cost_past_traj_ << std::endl;
+	//std::cout << "cost_companion = (" << mean_cost_companion_ << " , " << std_cost_companion_ << std::endl;
+}
+
+void Cplan_local_nav::calculate_normalization_cost_functions_parameters_norm(Crobot* robot_act)
+{
+	//calculates the Utopia point considering free space and the max value in order to linearly normalize ->[0,1]
+	mean_cost_distance_=0.0;
+	mean_cost_orientation_=0.0;
+	mean_cost_robot_=0.0;
+	mean_cost_int_forces_=0.0;
+	mean_cost_obstacles_=0.0;
+	mean_cost_past_traj_=0.0;
+	//propagate the robot as if no obstacle is in the scene and calculate costs
+	SpointV robot = robot_act->get_planning_trajectory()->front();
+	Sforce f_goal;
+	while ( robot.time_stamp - now_ < horizon_time_ && robot.distance2( local_goal_ ) > 0.25 )
+	{
+		// potential cost to go destination
+		f_goal = robot_act->force_goal( Sdestination(0,local_goal_.x,local_goal_.y), get_sfm_params(robot_act), &robot );
+
+		// robot propagation
+		robot = robot.propagate( dt_, f_goal, robot_act->get_desired_velocity() );
+
+		//calculate costs
+		mean_cost_robot_ += f_goal.module2(cost_angular_);
+		mean_cost_distance_ += robot.distance2(local_goal_);
+		double o = robot.angle_heading_point( local_goal_ );
+		mean_cost_orientation_ += o*o;
+	}
+
+	//calculate max costs and the scalarization term
+	double max;
+	max = -10000.0;
+	for( unsigned int i : end_of_branches_index_ )
+	{
+		if (max < cost_distance_[i])
+			max = cost_distance_[i];
+	}
+	std_cost_distance_ = max - mean_cost_distance_;
+	if ( std_cost_distance_ < 0.01 ) std_cost_distance_ = 0.01;
+
+	max = -10000.0;
+	for( unsigned int i : end_of_branches_index_ )
+	{
+		if (max < cost_orientation_[i])
+			max = cost_orientation_[i];
+	}
+	std_cost_orientation_ = max - mean_cost_orientation_;
+	if ( std_cost_orientation_ < 0.01 ) std_cost_orientation_ = 0.01;
+
+	max = -10000.0;
+	for( unsigned int i : end_of_branches_index_ )
+	{
+		if (max < cost_robot_[i])
+			max = cost_robot_[i];
+	}
+	std_cost_robot_ = max - mean_cost_robot_;
+	if ( std_cost_robot_ < 0.01 ) std_cost_robot_= 0.01;
+
+	max = -10000.0;
+	for( unsigned int i : end_of_branches_index_ )
+	{
+		if (max < cost_int_forces_[i])
+			max = cost_int_forces_[i];
+	}
+	std_cost_int_forces_ = max - mean_cost_int_forces_;
+	if ( std_cost_int_forces_ < 0.01 ) std_cost_int_forces_ = 0.01;
+
+	max = -10000.0;
+	for( unsigned int i : end_of_branches_index_ )
+	{
+		if (max < cost_obstacles_[i])
+			max = cost_obstacles_[i];
+	}
+	std_cost_obstacles_ = max - mean_cost_obstacles_;
+	if ( std_cost_obstacles_ < 0.01 ) std_cost_obstacles_ = 0.01;
+
+	max = -10000.0;
+	for( unsigned int i : end_of_branches_index_ )
+	{
+		if (max < cost_past_traj_[i])
+			max = cost_past_traj_[i];
+	}
+	std_cost_past_traj_= max - mean_cost_past_traj_;
+	if ( std_cost_past_traj_< 0.01 ) std_cost_past_traj_= 0.01;
+
+	//printing results
+	//std::cout << "cost_distance = (" << mean_cost_distance_ << " , " << std_cost_distance_ << std::endl;
+	//std::cout << "cost_orientation = (" << mean_cost_orientation_ << " , " << std_cost_orientation_ << std::endl;
+	//std::cout << "cost_int_forces = (" << mean_cost_int_forces_ << " , " << std_cost_int_forces_ << std::endl;
+	//std::cout << "cost_robot = (" << mean_cost_robot_ << " , " << std_cost_robot_ << std::endl;
+	//std::cout << "cost_obstacles = (" << mean_cost_obstacles_ << " , " << std_cost_obstacles_ << std::endl;
+	//std::cout << "cost_past_traj = (" << mean_cost_past_traj_ << " , " << std_cost_past_traj_ << std::endl;
+}
+
+
+Spose Cplan_local_nav::get_best_planned_pose(double dt, Cperson_abstract::companion_reactive reactive)
+{
+	//std::cout << " IN get_best_planned_pose"<< std::endl;
+	//returns the best planned pose at time dt
+	//Spoint robot_act_goal2=get_robot_goal();
+
+	int index_companion_for_robot_velocity=1; // new variable to adjust well the robot velocity to accompany the person. (pure companion, do not takes into account the final goal for the velocity)
+	unsigned int next_plan_index_companion_for_robot_velocity;
+	double min_next_companion_angle_for_robot_velocity;
+
+	int index(0);
+	int index_companion(0);
+	int index_final_dest(0);
+	if ( dt < dt_){
+
+		if((mode_step_near_)||(reactive!=Cperson_abstract::Akp_planning)){
+			index = 1;
+			dt=dt_;
+
+		}else{
+
+			if(BEST_path_parent_index_vector_.size()>out_index_step_final_dest_goal_){
+				index_final_dest= out_index_step_final_dest_goal_;
+				if(init_plan_approach_side_){
+					index_final_dest=1;
+				}
+			}else{
+				index_final_dest=BEST_path_parent_index_vector_.size()-1;
+			}
+			if(BEST_path_parent_index_vector_.size()>out_index_step_companion_goal_){
+				index_companion = out_index_step_companion_goal_;
+				index= out_index_step_companion_goal_;
+
+				if(init_plan_approach_side_){
+					index_companion = 1;
+					index= 1;
+				}
+			}else{
+				index_companion = BEST_path_parent_index_vector_.size()-1;
+				index=  BEST_path_parent_index_vector_.size()-1;
+			}
+
+			dt= out_index_step_companion_goal_*dt_; // Creo que necesito solo el companion, creo. MIRAR!
+
+			if(init_plan_approach_side_){
+				dt= 1*dt_;
+			}
+			//std::cout << " (dt negativo) 1 BEST_path_parent_index_vector_[BEST_path_parent_index_vector_.size()-1]="<<  BEST_path_parent_index_vector_[BEST_path_parent_index_vector_.size()-1]<<" BEST_path_parent_index_vector_[BEST_path_parent_index_vector_.size()-2]="<< BEST_path_parent_index_vector_[BEST_path_parent_index_vector_.size()-2]<< ";  BEST_path_parent_index_vector_[BEST_path_parent_index_vector_.size()-3]="<< BEST_path_parent_index_vector_[BEST_path_parent_index_vector_.size()-2]<<" BEST_path_parent_index_vector_[o]="<< BEST_path_parent_index_vector_[0]<< std::endl;
+			//std::cout << " BEST_path_parent_index_vector_,size()="<<BEST_path_parent_index_vector_.size()<< std::endl;
+			//std::cout << " (if) mode_step_far_! dt="<< dt<<"; dt_="<<dt_<< "; index="<<index<<"; out_index_step_final_dest_goal_="<<out_index_step_final_dest_goal_<<"; out_index_step_companion_goal_="<<out_index_step_companion_goal_<< std::endl;
+		}
+	}
+	else{
+		if((mode_step_near_)||(reactive!=Cperson_abstract::Akp_planning)){
+			index = (int) (dt / dt_);
+		}else{
+			if(BEST_path_parent_index_vector_.size()>out_index_step_final_dest_goal_){
+				index_final_dest= out_index_step_final_dest_goal_;
+				if(init_plan_approach_side_){
+					index_final_dest=1;
+				}
+			}else{
+				index_final_dest=BEST_path_parent_index_vector_.size()-1;
+			}
+			if(BEST_path_parent_index_vector_.size()>out_index_step_companion_goal_){
+				index_companion = out_index_step_companion_goal_;
+				index= out_index_step_companion_goal_;
+				if(init_plan_approach_side_){
+					index_companion = 1;
+					index= 1;
+				}
+			}else{
+				index_companion = BEST_path_parent_index_vector_.size()-1;
+				index= BEST_path_parent_index_vector_.size()-1;
+				//std::cout << " BEST_path_parent_index_vector_,size()="<<BEST_path_parent_index_vector_.size()<<"; BEST_path_parent_index_vector_.size()-1="<<BEST_path_parent_index_vector_.size()-1<< std::endl;
+
+			}
+			dt= out_index_step_companion_goal_*dt_; // Creo que necesito solo el companion, creo. MIRAR!
+			if(init_plan_approach_side_){
+				dt= 1*dt_;
+			}
+			//std::cout << " 2 BEST_path_parent_index_vector_[BEST_path_parent_index_vector_.size()-1]="<<  BEST_path_parent_index_vector_[BEST_path_parent_index_vector_.size()-1]<<" BEST_path_parent_index_vector_[BEST_path_parent_index_vector_.size()-2]="<< BEST_path_parent_index_vector_[BEST_path_parent_index_vector_.size()-2]<< ";  BEST_path_parent_index_vector_[BEST_path_parent_index_vector_.size()-3]="<< BEST_path_parent_index_vector_[BEST_path_parent_index_vector_.size()-2]<<" BEST_path_parent_index_vector_[o]="<< BEST_path_parent_index_vector_[0]<< std::endl;
+			//std::cout << " BEST_path_parent_index_vector_,size()="<<BEST_path_parent_index_vector_.size()<< std::endl;
+			//std::cout << " (else dt<dt_) mode_step_far_! dt="<< dt<<"; dt_="<<dt_<< "; index="<<index<<"; out_index_step_final_dest_goal_="<<out_index_step_final_dest_goal_<<"; out_index_step_companion_goal_="<<out_index_step_companion_goal_<< std::endl;
+		}
+	}
+	//std::cout << "  BEST_path_parent_index_vector_[BEST_path_parent_index_vector_.size()-1]="<<  BEST_path_parent_index_vector_[BEST_path_parent_index_vector_.size()-1]<<" BEST_path_parent_index_vector_[BEST_path_parent_index_vector_.size()-2]="<< BEST_path_parent_index_vector_[BEST_path_parent_index_vector_.size()-2]<< ";  BEST_path_parent_index_vector_[BEST_path_parent_index_vector_.size()-3]="<< BEST_path_parent_index_vector_[BEST_path_parent_index_vector_.size()-2]<<" BEST_path_parent_index_vector_[o]="<< BEST_path_parent_index_vector_[0]<< std::endl;
+	//std::cout << " !best_plan_vertex_index_.empty()="<<!best_plan_vertex_index_.empty()<<"; (unsigned)index "<<(unsigned)index<< " < best_plan_vertex_index_.size()"<<best_plan_vertex_index_.size()<<std::endl;
+
+	if( !best_plan_vertex_index_.empty() && (unsigned)index < best_plan_vertex_index_.size() )
+	{
+		/// (ASSAOP or ASSAGP)
+		unsigned int next_plan_index;
+		unsigned int next_plan_index_companion;
+		unsigned int next_plan_index_final_dest;
+		double robot_person_distance;
+		Spose robot_act;
+
+		if(reactive==Cperson_abstract::Akp_planning){
+			next_plan_index_companion_for_robot_velocity=BEST_path_parent_index_vector_.at( BEST_path_parent_index_vector_.size() - index_companion_for_robot_velocity -1 );
+			next_plan_index_companion = BEST_path_parent_index_vector_.at( BEST_path_parent_index_vector_.size() - index_companion -1 );
+			next_plan_index_final_dest = BEST_path_parent_index_vector_.at( BEST_path_parent_index_vector_.size() - index_final_dest -1 );
+
+			robot_person_distance=calc_robot_person_companion_distance();
+			vel_robot_companion(dmin_global_,robot_person_distance, before_next_goal_of_robot_,reactive);
+			// FIN calculo velocidad, siguiente step //
+			robot_act=robot_->get_robot_planning_trajectory()->at(next_plan_index_final_dest); // posicion trayectoria gonzalo, pero transformada a donde tiene que estar el robot respecto a ella.
+			SpointV_cov robot_act2=robot_->get_planning_trajectory()->at(next_plan_index_final_dest);
+
+			final_collision_check_=check_collision( robot_act2, next_plan_index_final_dest);
+		}
+		else{
+
+			next_plan_index = best_plan_vertex_index_.at( best_plan_vertex_index_.size() - index );
+			robot_person_distance=calc_robot_person_companion_distance();
+			vel_robot_companion(dmin_global_,robot_person_distance);
+			// FIN calculo velocidad, siguiente step //
+			robot_act=robot_->get_robot_planning_trajectory()->at(next_plan_index); // posicion trayectoria gonzalo, pero transformada a donde tiene que estar el robot respecto a ella.
+			SpointV_cov robot_act2=robot_->get_planning_trajectory()->at(next_plan_index);
+			final_collision_check_=check_collision( robot_act2, next_plan_index);
+		}
+
+		if(reactive==Cperson_abstract::Akp_planning){
+
+			before_act_companion_angle_=min_next_companion_angle_;
+			if(calc_goal_companion_with_group_path_){ // CASO side-by-side normal con 2 personas, pero solo cuenta la más cercana. Entra aquí.
+				//std::cout << " [IMPORTANTE!!!] (CASO 1) min_next_companion_angle_= "<< min_next_companion_angle_<< std::endl;
+
+				min_next_companion_angle_=orientation_person_robot_angles_[next_plan_index_companion];
+				min_next_companion_angle_for_final_dest_=orientation_person_robot_angles_[next_plan_index_final_dest];
+				min_next_companion_angle_for_robot_velocity=orientation_person_robot_angles_[next_plan_index_companion_for_robot_velocity];
+
+				save_matlab_final_min_collision_distance_vector_=final_min_collision_distance_vector_[next_plan_index_companion];
+				save_matlab_real_robot_person_distance2_vector_=real_robot_person_distance2_vector_[next_plan_index_companion];
+			}else{
+				//std::cout << " [IMPORTANTE!!!] (CASO 2) min_next_companion_angle_= "<< min_next_companion_angle_<< std::endl;
+
+				min_next_companion_angle_=orientation_person_robot_angles_with_prediction_of_person_companion_[index_companion];
+				min_next_companion_angle_for_final_dest_=orientation_person_robot_angles_with_prediction_of_person_companion_[index_companion];
+				min_next_companion_angle_for_robot_velocity=orientation_person_robot_angles_with_prediction_of_person_companion_[next_plan_index_companion_for_robot_velocity];
+
+				save_matlab_final_min_collision_distance_vector_=final_min_collision_distance_vector_[index_companion];
+				save_matlab_real_robot_person_distance2_vector_=real_robot_person_distance2_vector_[index_companion];
+			}
+
+			//std::cout << " [IMPORTANTE!!!] min_next_companion_angle_= "<< min_next_companion_angle_<< std::endl;
+
+			min_next_companion_cost_=cost_companion_[next_plan_index_companion];
+			ideal_angle_person_robot_=min_next_companion_angle_;
+
+			// inicio calculo velocidad, siguiente step //
+			if(debug_file_robot_){
+				std::ofstream fileMatlab2;
+				fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+				fileMatlab2 << " ANTES calculo velocidad!!! min_next_companion_angle_= "<< min_next_companion_angle_<< "; index="<<index<<"\n";
+				fileMatlab2.close();
+			}
+
+			if(calc_goal_companion_with_group_path_){
+				before_initial_angle_=orientation_person_robot_angles_[next_plan_index_companion];
+			}else{
+				before_initial_angle_=orientation_person_robot_angles_with_prediction_of_person_companion_[index_companion];
+			}
+
+			before_initial_cost_=cost_companion_[next_plan_index_companion];
+
+			/* Ini cambio  mover y desmover pose inicial robot*/
+			//calculate_actual_angle_person_robot(next_plan_index);
+
+			//Cperson_abstract* person_obj;
+			//find_person(id_person_companion_ , &person_obj);
+			Sdestination person_act_dest=actual_person_Companion_pointer_->get_best_dest();
+
+			SpointV_cov person = actual_person_Companion_pointer_->get_planning_trajectory()->at(next_plan_index_final_dest);
+
+			SpointV_cov robot_person_act=person;
+			double angle=atan2(robot_act.y-person.y , robot_act.x-person.x);
+
+			if(angle<0){
+				angle=(2*3.14)+angle;
+			}
+
+	        double theta=calc_person_companion_orientation();
+
+	        if(theta<0){
+	        	theta=3.14+theta;
+	        }
+	        // fin calcular nuevo angulo theta con orientación dirección movimiento persona.
+
+			SpointV_cov act_companion_person_pose=companion_person_position_;
+
+			//std::cout << "(**** FINAL *****) theta="<<theta*(180/3.14)<<"; angle="<<angle*(180/3.14)<<"diffangle(theta, angle)="<<(diffangle(theta, angle))*(180/3.14)<< std::endl;
+			//std::cout << "theta+angle="<<(theta+angle)*(180/3.14)<<"; theta-angle="<<(theta-angle)*(180/3.14)<< std::endl;
+			//std::cout << "min_next_companion_angle_=" <<min_next_companion_angle_<<"; theta+min_next_companion_angle_="<<(theta+(min_next_companion_angle_*3.14/180))*(180/3.14)<<"; theta-min_next_companion_angle_="<<(theta-(min_next_companion_angle_*3.14/180))*(180/3.14)<< std::endl;
+
+			double modif_person_robot_distance=robot_person_proximity_distance_;// =(1/1000)*(((ite-45)/2).^2)+1;
+
+			// modify! the goal of the path, the final goal to be in the robot traj, not in the group traj.
+			if( diffangle(theta, angle) < 0 ){  // angle = 180 grados... o 0... si cambia el goal de lado da un salto... (como arreglarlo)
+
+				double ite;
+				if(min_next_companion_angle_for_final_dest_!=angle_companion_){  /// ojo!! Antes este  min_next_companion_angle_for_final_dest_ era=> min_next_companion_angle_ [el de companion]
+					ite=(theta+(min_next_companion_angle_for_final_dest_*3.14/180))*(180/3.14);
+				}else{
+					ite=min_next_companion_angle_for_final_dest_;
+				}
+
+				modif_person_robot_distance=robot_person_proximity_distance_;
+				modif_person_robot_distance=calculate_modif_person_robot_distance(ite);
+
+				double real_dist_pr=initial_robot_spoint_.distance(initial_person_companion_point_);
+				SpointV_cov act_goal_person = actual_person_Companion_pointer_->get_planning_trajectory()->at(1);
+				before_next_goal_of_robot_=Spoint(act_goal_person.x,act_goal_person.y,act_goal_person.time_stamp);
+				before_next_goal_of_robot_.x=before_next_goal_of_robot_.x+(real_dist_pr)*cos(theta+(min_next_companion_angle_for_robot_velocity*3.14/180));
+				before_next_goal_of_robot_.y=before_next_goal_of_robot_.y+(real_dist_pr)*sin(theta+(min_next_companion_angle_for_robot_velocity*3.14/180));
+
+				/*std::cout << "( NEW BEST VELOCITY ) robot_person_proximity_distance_="<<robot_person_proximity_distance_<< std::endl;
+				std::cout << "( NEW BEST VELOCITY ) min_next_companion_angle_for_robot_velocity="<<min_next_companion_angle_for_robot_velocity<< std::endl;
+				std::cout << "( NEW BEST VELOCITY ) before_next_goal_of_robot_.x="<<before_next_goal_of_robot_.x<<"; before_next_goal_of_robot_.y="<<before_next_goal_of_robot_.y<< std::endl;
+				std::cout << "( NEW BEST VELOCITY ) robot_act.x="<<robot_act.x<<"; robot_act.y="<<robot_act.y<<"; theta="<<theta*180/3.14<<"; robot_act.theta="<<robot_act.theta<< std::endl;
+				*/
+
+				robot_act.x=robot_act.x+((modif_person_robot_distance)/2)*cos(theta+(min_next_companion_angle_for_final_dest_*3.14/180));//(1.5/2)*cos(theta+(min_next_companion_angle_*3.14/180));
+				robot_act.y=robot_act.y+((modif_person_robot_distance)/2)*sin(theta+(min_next_companion_angle_for_final_dest_*3.14/180));//(1.5/2)*sin(theta+(min_next_companion_angle_*3.14/180));
+
+				robot_person_act.x=robot_person_act.x+((modif_person_robot_distance)/2)*cos(theta+(min_next_companion_angle_for_final_dest_*3.14/180));//1.5*cos(theta+(min_next_companion_angle_*3.14/180));
+				robot_person_act.y=robot_person_act.y+((modif_person_robot_distance)/2)*sin(theta+(min_next_companion_angle_for_final_dest_*3.14/180));//1.5*sin(theta+(min_next_companion_angle_*3.14/180));
+				act_companion_person_pose.x+=((modif_person_robot_distance)/2)*cos(theta+(min_next_companion_angle_for_final_dest_*3.14/180));
+				act_companion_person_pose.y+=((modif_person_robot_distance)/2)*sin(theta+(min_next_companion_angle_for_final_dest_*3.14/180));
+
+
+			}else{
+
+				double ite;
+				if(min_next_companion_angle_for_final_dest_!=angle_companion_){
+					ite=(theta-(min_next_companion_angle_for_final_dest_*3.14/180))*(180/3.14);
+				}else{
+					ite=min_next_companion_angle_for_final_dest_;
+				}
+
+				modif_person_robot_distance=robot_person_proximity_distance_;
+				modif_person_robot_distance=calculate_modif_person_robot_distance(ite);
+
+				double real_dist_pr=initial_robot_spoint_.distance(initial_person_companion_point_);
+
+				SpointV_cov act_goal_person = actual_person_Companion_pointer_->get_planning_trajectory()->at(1);
+				before_next_goal_of_robot_=Spoint(act_goal_person.x,act_goal_person.y,act_goal_person.time_stamp);
+				before_next_goal_of_robot_.x=before_next_goal_of_robot_.x+(real_dist_pr)*cos(theta-(min_next_companion_angle_for_robot_velocity*3.14/180));
+				before_next_goal_of_robot_.y=before_next_goal_of_robot_.y+(real_dist_pr)*sin(theta-(min_next_companion_angle_for_robot_velocity*3.14/180));
+
+				/*std::cout << "( NEW BEST VELOCITY ) robot_person_proximity_distance_="<<robot_person_proximity_distance_<< std::endl;
+				std::cout << "( NEW BEST VELOCITY ) min_next_companion_angle_for_robot_velocity="<<min_next_companion_angle_for_robot_velocity<< std::endl;
+				std::cout << "( NEW BEST VELOCITY ) before_next_goal_of_robot_.x="<<before_next_goal_of_robot_.x<<"; before_next_goal_of_robot_.y="<<before_next_goal_of_robot_.y<< std::endl;
+				std::cout << "( NEW BEST VELOCITY ) robot_act.x="<<robot_act.x<<"; robot_act.y="<<robot_act.y<<"; theta="<<theta*180/3.14<<"; robot_act.theta="<<robot_act.theta<< std::endl;
+				 */
+
+				robot_act.x=robot_act.x+((modif_person_robot_distance)/2)*cos(theta-(min_next_companion_angle_for_final_dest_*3.14/180));//(1.5/2)*cos(theta-(min_next_companion_angle_*3.14/180));
+				robot_act.y=robot_act.y+((modif_person_robot_distance)/2)*sin(theta-(min_next_companion_angle_for_final_dest_*3.14/180));//(1.5/2)*sin(theta-(min_next_companion_angle_*3.14/180));
+
+
+				robot_person_act.x=robot_person_act.x+((modif_person_robot_distance)/2)*cos(theta-(min_next_companion_angle_for_final_dest_*3.14/180));//1.5*cos(theta-(min_next_companion_angle_*3.14/180));
+				robot_person_act.y=robot_person_act.y+((modif_person_robot_distance)/2)*sin(theta-(min_next_companion_angle_for_final_dest_*3.14/180));//1.5*sin(theta-(min_next_companion_angle_*3.14/180));
+				act_companion_person_pose.x+=((modif_person_robot_distance)/2)*cos(theta-(min_next_companion_angle_for_final_dest_*3.14/180));
+				act_companion_person_pose.y+=((modif_person_robot_distance)/2)*sin(theta-(min_next_companion_angle_for_final_dest_*3.14/180));
+
+			}
+
+
+			robot_act=robot_->get_robot_planning_trajectory()->at(next_plan_index_final_dest);
+
+			Sdestination robot_goal(0,robot_act.x,robot_act.y,1.0);
+
+			//std::cout << "(ant_before replan_last_pose) robot_act.x="<<robot_act.x<<"; robot_act.y= "<<robot_act.y<< std::endl;
+			//std::cout << "robot_act.v="<<robot_act.v<<"; robot_act.theta="<<robot_act.theta<<"; robot_act.w="<<robot_act.w<< std::endl;
+			Spose robot_act2;
+			if(calc_goal_companion_with_group_path_){
+				robot_act2=replan_last_step(next_plan_index_companion,robot_initial_pose_,(SpointV) initial_robot_spoint_,robot_goal, dt);
+			}else{
+				robot_act2=replan_last_step(index_companion,robot_initial_pose_,(SpointV) initial_robot_spoint_,robot_goal, dt);
+			}
+			robot_act=robot_act2;
+			//std::cout << " (FINAL GOAL SEND OUT1) robot_act2.x="<<robot_act2.x<<"; robot_act2.y="<<robot_act2.y<< std::endl;
+			//std::cout << "robot_act2.v="<<robot_act2.v<<"; robot_act2.theta="<<robot_act2.theta<<"; robot_act2.w="<<robot_act2.w<< std::endl;
+
+		}else{
+
+		}
+
+		// final
+		//Cperson_abstract* person_obj;
+		//find_person(id_person_companion_ , &person_obj);
+		SpointV_cov act_pers_pos2;
+
+		if(actual_person_Companion_pointer_->get_planning_trajectory()->size()>next_plan_index){
+			act_pers_pos2=actual_person_Companion_pointer_->get_planning_trajectory()->at(next_plan_index);
+		}else if(actual_person_Companion_pointer_->get_prediction_trajectory()->size()>next_plan_index){
+			act_pers_pos2=actual_person_Companion_pointer_->get_prediction_trajectory()->at(next_plan_index);
+		}else{
+			act_pers_pos2=actual_person_Companion_pointer_->get_planning_trajectory()->at(actual_person_Companion_pointer_->get_planning_trajectory()->size()-1);
+		}
+
+
+		Spoint robot_point2(robot_act.x,robot_act.y,robot_act.time_stamp);
+
+		//std::cout << " final next distance_person_robot_="<<act_pers_pos2.distance(robot_point2)<< std::endl;
+		//std::cout << " (ACTUAL companion angle) orientation_person_robot_angles_[0]="<<orientation_person_robot_angles_[0]<< std::endl;
+		//std::cout << " (ACTUAL companion angle) orientation_person_robot_angles_with_prediction_of_person_companion_[0]="<<orientation_person_robot_angles_with_prediction_of_person_companion_[0]<< std::endl;
+		//std::cout << " (FINAL companion angle) orientation_person_robot_angles_[next_plan_index]="<<orientation_person_robot_angles_[next_plan_index]<< std::endl;
+		//std::cout << " (FINAL companion angle) orientation_person_robot_angles_with_prediction_of_person_companion_[next_plan_index]="<<orientation_person_robot_angles_with_prediction_of_person_companion_[next_plan_index]<< std::endl;
+		//std::cout << " (pose) actual_robot_initial_position:"<< std::endl;
+		//robot_initial_pose_.print();
+		//std::cout << " (SpointV_cov) actual_robot_initial_position:"<< std::endl;
+		//initial_robot_spoint_.print();
+		//std::cout << " (FINAL GOAL SEND OUT) robot_act.x="<<robot_act.x<<"; robot_act.y="<<robot_act.y<< std::endl;
+		//std::cout << "robot_act.v="<<robot_act.v<<"; robot_act.theta="<<robot_act.theta<<"; robot_act.w="<<robot_act.w<< std::endl;
+
+		if((debug_file_robot_)||(only_comp_people_vel_and_robot_poses_)){
+			std::ofstream fileMatlab2;
+			fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+			fileMatlab2 << " (pose) actual_robot_initial_position: \n";
+			fileMatlab2 << "robot_initial_pose_.time_stamp="<<robot_initial_pose_.time_stamp<<" \n";
+			fileMatlab2 << "robot_initial_pose_.x="<<robot_initial_pose_.x<<" \n";
+			fileMatlab2 << "robot_initial_pose_.y="<<robot_initial_pose_.y<<" \n";
+			fileMatlab2 << "robot_initial_pose_.v="<<robot_initial_pose_.v<<" \n";
+			fileMatlab2 << "robot_initial_pose_.w="<<robot_initial_pose_.w<<" \n";
+			fileMatlab2 << "robot_initial_pose_.theta="<<robot_initial_pose_.theta<<" \n";
+			fileMatlab2 << " (SpointV_cov) actual_robot_initial_position:"<< " \n";
+			fileMatlab2 << "initial_robot_spoint_.time_stamp="<<initial_robot_spoint_.time_stamp<<" \n";
+			fileMatlab2 << "initial_robot_spoint_.x="<<initial_robot_spoint_.x<<" \n";
+			fileMatlab2 << "initial_robot_spoint_.y="<<initial_robot_spoint_.y<<" \n";
+			fileMatlab2 << "initial_robot_spoint_.vx="<<initial_robot_spoint_.vx<<" \n";
+			fileMatlab2 << "initial_robot_spoint_.vy="<<initial_robot_spoint_.vy<<" \n";
+			fileMatlab2 << " (FINAL GOAL SEND OUT) robot_act.x="<<robot_act.x<<"; robot_act.y="<<robot_act.y<< " \n";
+			fileMatlab2 << "robot_act.v="<<robot_act.v<<"; robot_act.theta="<<robot_act.theta<<"; robot_act.w="<<robot_act.w<< " \n";
+			fileMatlab2.close();
+		}
+
+		/* FIN cambio peligroso mover y desmover pose inicial robot*/
+		return robot_act;
+	}
+	else
+		return Spose();
+}
+
+
+void Cplan_local_nav::
+get_navigation_instant_work( double& work_robot, double& work_persons )
+{
+	work_robot = work_robot_;
+	work_persons = work_persons_;
+}
+
+
+void Cplan_local_nav::
+calculate_navigation_instant_work( )
+{
+	work_robot_ = 0.0;
+	if( best_plan_vertex_index_.size() > 2 )
+	{
+		work_robot_ =  fabs( edge_.at(best_plan_vertex_index_.at( best_plan_vertex_index_.size()-2 )).f  *
+			robot_->get_diff_position() );
+	}
+	else
+	{
+		work_robot_ = 0.0;
+	}
+
+	work_persons_ = 0.0;
+	for( Cperson_abstract* iit : nearby_person_list_)
+	{
+		if ( iit->get_current_pointV().distance( robot_->get_current_pointV() ) < workspace_radii_ )
+			work_persons_ += fabs( iit->force( robot_->get_current_pointV() ,
+					get_sfm_int_params(iit,robot_) ) * iit->get_diff_position() );
+	}
+}
+
+
+void Cplan_local_nav::set_robot_params( double v, double w, double av, double av_break, double aw, double platform_radii, double av_neg, double av_Vrobotzero, double lim_to_consider_robotZero)
+{
+	robot_->set_v_max( v );
+	max_v_by_system_ = v;//this velocity is modified depending on the density of nearby obstacles
+	robot_->set_w_max( w );
+	robot_->set_a_v_max( av );
+	robot_->set_a_v_max_negativa(av_neg);
+	robot_->set_a_v_break( av_break );
+	robot_->set_a_w_max( aw );
+	robot_->set_platform_radii( platform_radii);
+	robot_->set_a_v_max_Vrobotzero(av_Vrobotzero);
+	robot_->set_lim_to_consider_robotZero(lim_to_consider_robotZero);
+}
+
+//update to new cost-to-go function and elimiate DEPRECATED parameters (Comment Gonzalo.)
+void Cplan_local_nav::set_plan_cost_parameters( double c_dist, double c_orientation, double c_w_robot,
+		double c_w_people, double c_time, double c_w_obstacles, double c_old_path, double c_l_minima)
+{
+    cost_parameters_[0] = c_dist;// [0] Goal cost
+    cost_parameters_[1] = c_orientation;// [1] orientation cost
+    cost_parameters_[2] = c_w_robot;// [2] Robot cost
+    cost_parameters_[3] = c_w_people;// [3]Interacting people cost
+    cost_parameters_[4] = c_time;// [4] potential time
+    cost_parameters_[5] = c_w_obstacles;// [5] obstacles cost
+    cost_parameters_[6] = c_old_path;// [6] past function cost
+    cost_parameters_[7] = c_l_minima;// [7] local minima scape cost
+}
+
+//only use when no plan is calculated, as an evaluation of the observed performance
+// of this method or any navigation method
+void Cplan_local_nav::calculate_navigation_cost_values( std::vector<double>& costs )
+{
+	costs.resize(4,0.0);
+	// calculate robot cost --------------------------------------------------------------------------------
+	Spose dr = robot_->get_diff_pose();
+	costs[0] = dr.v* dr.v + dr.w*dr.w;
+	costs[0] /= dt_*dt_;
+
+
+	// calculate ppl cost --------------------------------------------------------------------------------
+	Sforce f,f_int;
+	double cost = 0.0;
+	for( Cperson_abstract* iit: person_list_  )
+	{
+		f = iit->force_sphe(robot_->get_current_pointV() , get_sfm_int_params(iit,robot_)  );
+		f_int += f;
+		cost += f.module2();
+	}
+	costs[1] = cost;
+
+	// calculate obstacles cost  --------------------------------------------------------------------------------
+	Sforce f_obs = force_objects_laser_int_planning_virtual( robot_, 0, 25.0, false );//false set for no new nearby_obstacles list to be refilled
+	// INICION Limitar fuerza obstaculos cuando hay muchos solapados
+	double fobos_Xmod=sqrt(f_obs.fx*f_obs.fx);
+	double fobos_Ymod=sqrt(f_obs.fy*f_obs.fy);
+	double signo_x;
+	double signo_y;
+	if (f_obs.fx > 0){
+		signo_x=1;
+	}else{
+		signo_x=-1;
+	}
+	if (f_obs.fy > 0){
+		signo_y=1;
+	}else{
+		signo_y=-1;
+	}
+
+	if((fobos_Xmod>f_obst_max_x_)||(fobos_Ymod>f_obst_max_y_)){
+		f_obs.fx=signo_x*f_obst_max_x_;
+		f_obs.fy=signo_y*f_obst_max_y_;
+	}
+	//std::cout << "(FINAL FORCE) f_obs.fx="<<f_obs.fx<<"; f_obs.fy="<<f_obs.fy<< std::endl;
+	// FIN Limitar fuerza obstaculos cuando hay muchos solapados
+
+	costs[2] = f_obs.module2();
+
+	// calculate average velocity --------------------------------------------------------------------------------
+	costs[3] = robot_->get_current_pose().v;
+
+}
+
+Sedge_tree::Sedge_tree(	unsigned int parent_, Sforce f_, Sforce f_goal_,
+		Sforce f_people_,Sforce f_obs_, Sforce f_persongoal_):
+		parent(parent_), f(f_), f_goal(f_goal_), f_people(f_people_), f_obs(f_obs_),f_persongoal(f_persongoal_)
+{
+
+}
+double Sedge_tree::cost( Sedge_tree parent) const
+{
+
+	return 0.0;
+}
+void Sedge_tree::print()
+{
+	std::cout << "parent vertex = " << parent << std::endl;
+	f.print();
+}
+
+Smulticost::Smulticost( unsigned int id_, unsigned int n ) :
+	id(id_)
+{
+	cost.resize( n, 0.0 );
+}
+
+bool Smulticost::operator< ( const Smulticost& m2) const
+{
+	//dominance operator. Returns true if m dominates m2
+	double thr(1e-5);
+	bool one_better(false);
+	for( unsigned int i = 0; i<cost.size(); ++i )
+	{
+		//for numerical stability, equality is cheked:  x-x2 < thr
+		if ( cost[i] - thr >  m2.cost[i] )
+			return false;
+		// check for at least one better cost i
+		else if ( cost[i] + thr <  m2.cost[i] )
+			one_better = true;
+		//else: equal both costs, needs more comparisons
+
+	}
+	return one_better;//requires that at least one was better
+}
+
+bool Smulticost::operator== ( const Smulticost& m2) const
+{
+	if( id == m2.id )
+		return true;
+	else
+		return false;
+}
+
+
+bool Smulticost::operator!= ( const Smulticost& m2) const
+{
+	return !(*this==m2);
+}
+
+void Smulticost::print ( ) const
+{
+	std::cout << "Multicost " << id << ", costs = {";
+	for( double i: cost )
+		std::cout << i << " , ";
+	std::cout << " } " << std::endl;
+}
+
+void Smulticost::print_ml ( ) const
+{
+	std::cout << id ;
+	for( double i: cost )
+		std::cout << ", "<< i;
+	std::cout << std::endl;
+}
+
+
+
+/*** Ini check_collision_companion_reactive_atractive_goal!  (ely-modificada, solo) ***/
+bool Cplan_local_nav::check_collision_companion_reactive_atractive_goal( const SpointV_cov& p2 ,  int t)
+// return if have collision and the  minimum distance of the collision
+{
+	//std::cout << "[INI] check_collision_companion_reactive_atractive_goal!!! = "<< std::endl;
+	case_dynamic_=false;
+	// to do, p2-> tendría que ser desde la posicion del robot, no desde el centro. (o calcularla al mover el robot y cambiar el valor del minimum angle, si el obstaculo esta justo detras.
+	// si cambiamos esto, no reaccionará bien a personas que se les acerquen desde detras pq vayan más rapido... (pensar mejor)
+	//clock_t check_collision_companion_start, check_collision_companion_end;
+	//check_collision_companion_start = clock();
+
+	Spoint p=Spoint(p2.x,p2.y,p2.time_stamp);
+	double  robot_radi=robot_->get_platform_radii();
+
+	min_distance_collision_=(robot_person_proximity_distance_+2*robot_radi+little_augmented_collision_margin_)/2; // initial equal to the máx distance. (robot+person) =pasan bien.
+
+	//std::cout << "min_distance_collision_==robot_person_companion_distance_= "<<min_distance_collision_ <<"nearby_person_list_.size="<<nearby_person_list_.size()<< std::endl;
+
+	std::vector<double> min_distances_vector;
+	min_distances_vector.clear();
+
+	bool collision=false;
+	 //TODO does not work properly on real scenarios! long term issue (Comment Gonzalo). It refers to map obstacles. The laser obstacles work properly.
+	//check colision with map: TO BE DEPRECATED
+	if ( read_force_map_success_ && !is_cell_clear_map( p.x, p.y ) )
+	{
+		collision=true; // colision, pero por choque con el mapa,
+	}
+
+	double collision_threshold=(robot_person_proximity_distance_+2*robot_radi+little_augmented_collision_margin_)/2;
+
+	if(debug_real_test_companion4_){
+	std::cout << "(check_collision_companion_reactive_atractive_goal) collision_threshold= "<<collision_threshold <<"nearby_person_list_.size="<<nearby_person_list_.size()<< std::endl;
+	}
+
+	//check collision with obstacles (laser), prior is necessary to calculate force_objects_laser_int_planning()
+	for( Spoint iit : laser_obstacle_list_ )
+	{
+		double anisotropy=calculate_anisotropy( p2, iit );
+		double min_dist_obst_act=sqrt((iit.distance( p )- obstacle_radi_amp_)*(iit.distance( p )- obstacle_radi_amp_));
+		if( (min_dist_obst_act  < collision_threshold)&& (anisotropy>anisotropy_threshold_) )
+		{
+			min_distances_vector.push_back(min_dist_obst_act);
+			collision=true;
+			//std::cout << "OBSTACLE " << std::endl;
+			//std::cout << "collision distance=iit.distance( p )=" << iit.distance( p ) <<" < collision_threshold="<<collision_threshold<< std::endl;
+			//std::cout << "print_obstacle="<<std::endl;
+			//iit.print();
+		}
+	}
+	//if t = -1, then no collision is calculated wrt ppl
+	if ( t > -1 )
+	{
+		double d,det;
+		switch( ppl_collision_mode_ )
+		{
+		  case 0: //normal mode, quadratic distance to person, independent of distribution
+			  //std::cout << "case 0 (ppl_collision_mode_)" << std::endl;
+			for( auto iit : nearby_person_list_)
+			{
+				if(iit->get_id()!=id_person_companion_){ // add due to person companion, esta persona no hay que calcular colisiones respecto a ella.
+					d = sqrt((iit->get_prediction_trajectory()->front().distance( p )-person_radi_amp_)*(iit->get_prediction_trajectory()->front().distance( p )-person_radi_amp_));
+					double anisotropy=calculate_anisotropy( p2, iit->get_prediction_trajectory()->front() );
+
+					if ( (d < collision_threshold)&&(anisotropy>anisotropy_threshold_))
+					{
+						case_dynamic_=true;
+						collision=true;
+						min_distances_vector.push_back(d);
+						//std::cout << "collision distance=iit->distance( p )=" << d <<" < collision_threshold="<<collision_threshold<< std::endl;
+						//std::cout << " print PERSON. id= " <<iit->get_id()<< std::endl;
+						//iit->get_prediction_trajectory()->front().print();
+					}
+				}
+			}
+			break;
+		  case 1://mahalanobis distance, considers collision if inside the std ellipsoid: TOO RESTRICTIVE...
+			  // std::cout << "case 1 (ppl_collision_mode_)" << std::endl;
+			for( auto iit : nearby_person_list_) // de momento, para mi todas las suyas con constantes, han de ser menores que la distancia robot+persona. (luego ya veremos si va bien o no)
+			{
+				if(iit->get_id()!=id_person_companion_){
+					double anisotropy=calculate_anisotropy( p2, iit->get_prediction_trajectory()->front() );
+					d = sqrt((iit->get_prediction_trajectory()->front().cov_dist(p,det)-person_radi_amp_)*(iit->get_prediction_trajectory()->front().cov_dist(p,det)-person_radi_amp_));
+					if ( (d < collision_threshold)&&(anisotropy>anisotropy_threshold_) )
+					{
+						case_dynamic_=true;
+						collision=true;
+						min_distances_vector.push_back(d);
+						//std::cout << "collision distance=iit->distance( p )=" << d <<" < collision_threshold="<<collision_threshold<< std::endl;
+						//std::cout << " print PERSON. id= " <<iit->get_id()<< std::endl;
+						//iit->get_prediction_trajectory()->front().print();
+					}
+				}
+			}
+			break;
+		  case 2 ://mahalanobis distance, half the std, much less restrictive
+			//std::cout << "case 2 (ppl_collision_mode_)" << std::endl;
+			for( auto iit : nearby_person_list_)
+			{
+				if(iit->get_id()!=id_person_companion_){
+					d = sqrt((iit->get_prediction_trajectory()->front().cov_dist(p,det)-person_radi_amp_)*(iit->get_prediction_trajectory()->front().cov_dist(p,det)-person_radi_amp_));
+					double anisotropy=calculate_anisotropy( p2, iit->get_prediction_trajectory()->front() );
+					if ( det > 1.0)
+					{
+						if ( (d < collision_threshold)&&(anisotropy>anisotropy_threshold_) )
+						{
+							case_dynamic_=true;
+							collision=true;
+							min_distances_vector.push_back(d);
+							//std::cout << "collision distance=iit->distance( p )=" << d <<" < collision_threshold="<<collision_threshold<< std::endl;
+							//std::cout << " print PERSON. id= " <<iit->get_id()<< std::endl;
+							//iit->get_prediction_trajectory()->front().print();
+						}
+					}
+					else{
+						d = sqrt((iit->get_prediction_trajectory()->front().distance( p )-person_radi_amp_)*(iit->get_prediction_trajectory()->front().distance( p )-person_radi_amp_));
+						double anisotropy=calculate_anisotropy( p2, iit->get_prediction_trajectory()->front() );
+						if ( (d < collision_threshold)&& (anisotropy>anisotropy_threshold_) )
+						{
+							case_dynamic_=true;
+							collision=true;
+							min_distances_vector.push_back(d);
+							//std::cout << "collision distance=iit->distance( p )=" << d <<" < collision_threshold="<<collision_threshold<< std::endl;
+							//std::cout << " print PERSON. id= " <<iit->get_id()<< std::endl;
+							//iit->get_prediction_trajectory()->front().print();
+						}
+					}
+				}
+			}
+			break;
+		  case 3 :
+			//std::cout << "case 3 (ppl_collision_mode_)" << std::endl;
+			for( auto iit : nearby_person_list_)
+			{
+				if(iit->get_id()!=id_person_companion_){
+					d = sqrt((iit->get_prediction_trajectory()->front().cov_dist(p,det)-person_radi_amp_)*(iit->get_prediction_trajectory()->front().cov_dist(p,det)-person_radi_amp_));
+					double anisotropy=calculate_anisotropy( p2, iit->get_prediction_trajectory()->front() );
+					if ( det > 1.0)
+					{
+						if ( (d < collision_threshold)&&(anisotropy>anisotropy_threshold_)  ) // de momento, para mi todas las suyas con constantes, han de ser menores que la distancia robot+persona. (luego ya veremos si va bien o no)
+						{
+							case_dynamic_=true;
+							collision=true;
+							min_distances_vector.push_back(d);
+							//std::cout << "collision distance=iit->distance( p )=" << d <<" < collision_threshold="<<collision_threshold<< std::endl;
+							//std::cout << " print PERSON. id= " <<iit->get_id()<< std::endl;
+							//iit->get_prediction_trajectory()->front().print();
+						}
+					}else{
+						d = sqrt((iit->get_prediction_trajectory()->front().distance( p )-person_radi_amp_)*(iit->get_prediction_trajectory()->front().distance( p )-person_radi_amp_));
+						double anisotropy=calculate_anisotropy( p2, iit->get_prediction_trajectory()->front() );
+						if ( (d < collision_threshold)&&(anisotropy>anisotropy_threshold_) )
+						{
+							case_dynamic_=true;
+							collision=true;
+							min_distances_vector.push_back(d);
+							//std::cout << "collision distance=iit->distance( p )=" << d <<" < collision_threshold="<<collision_threshold<< std::endl;
+							//std::cout << " print PERSON. id= " <<iit->get_id()<< std::endl;
+							//iit->get_prediction_trajectory()->front().print();
+						}
+					}
+				}
+			}
+			break;
+
+		}
+		//calculate probabilities. NOT implemented, insignificant collision values when cov is high
+        //pr_no_col *= 1 - gaussian_constant_ / sqrt(det) * exp( -0.5*d );// * robot_->get_platform_radii_2();//area = pi * r^2 (pi included in gaussian_contant)
+        //std::cout << "Pr of no colision joint = " << pr_no_col << " and distance = " << d << " det = " << det << std::endl;
+	}
+
+	// obtain minimun collision distance.
+	if(!min_distances_vector.empty()){ // si hay alguna distancia minima.
+		double min_dist;
+		if(min_distance_collision_>min_distances_vector[0]){
+			min_dist=min_distances_vector[0];
+		}else{
+			min_dist=min_distance_collision_;
+		}
+
+		for( unsigned int in=0; in<min_distances_vector.size(); in++){
+			if(min_distances_vector[in]<min_dist){
+				min_dist=min_distances_vector[in];
+			}
+		}
+		min_distance_collision_=min_dist; // sacar fuera la min dist.
+	}
+	//std::cout << "min_distance_collision_= "<<min_distance_collision_<< std::endl;
+	//std::cout << "collision= "<<collision << std::endl;
+	//std::cout << "FIN function: check_collision_companion " << std::endl;
+	//check_collision_companion_end = clock();
+	//if(check_execution_times_){
+	//	std::cout << "time_check_collision_companion="<<((check_collision_companion_start-check_collision_companion_end)/clocks_per_sec_my_var_)*1000<< std::endl;
+	//}
+
+	return collision;
+}
+
+/*** Fin Check check_collision_companion_reactive_atractive_goal! ***/
+
+/*** Ini Check collision companion!  (ely-modificada, solo, esta vez para calculo colisiones robot goal respecto persona, cuando esta lejos de esta) ***/
+double Cplan_local_nav::check_collision_companion_goal(const Spoint& p ,  int t)
+// return if have collision and the  minimum distance of the collision
+{
+	//std::cout << "INI function: check_collision_companion_goal ="<< std::endl;
+	//clock_t check_collision_companion_goal_start, check_collision_companion_goal_end;
+	//check_collision_companion_goal_start = clock();
+
+	double min_distance_collision_act=robot_person_companion_distance_; // initial equal to the máx distance. (robot+person) =pasan bien.
+	std::vector<double> min_distances_vector;
+	min_distances_vector.clear();
+
+	bool collision=false;
+	 //TODO does not work properly on real scenarios! long term issue
+	//check colision with map: TO BE DEPRECATED (Gonzalo commet. All comments of To be deprecated are from Gonzalo akp-code.)
+	if ( read_force_map_success_ && !is_cell_clear_map( p.x, p.y ) )
+	{
+		collision=true; // colision, pero por choque con el mapa
+	}
+
+	//check collision with obstacles (laser), prior is necessary to calculate force_objects_laser_int_planning()
+	for( Spoint iit : laser_obstacle_list_ )
+	{
+		double act_distance_obst_collision=sqrt((iit.distance( p )-obstacle_radi_amp_)*(iit.distance( p )-obstacle_radi_amp_));
+		if( act_distance_obst_collision  < robot_person_companion_distance_ )
+		{
+			//std::cout << "Collision found!!" << robot_->get_platform_radii_2() << std::endl;
+			min_distances_vector.push_back(act_distance_obst_collision);
+			collision=true;
+		}
+	}
+	t=0;
+	//if t = -1, then no collision is calculated wrt ppl
+	if ( t > -1 )
+	{
+		double d,det;
+		switch( ppl_collision_mode_ )
+		{
+		  case 0: //normal mode, quadratic distance to person, independent of distribution
+			  if(debug_companion_){
+			  std::cout << "case 0 (ppl_collision_mode_)" << std::endl;
+			  }
+			for( auto iit : nearby_person_list_)
+			{
+				if(iit->get_id()!=id_person_companion_){ // add due to person companion, esta persona no hay que calcular colisiones respecto a ella.
+					d = sqrt((iit->get_planning_trajectory( )->at(t).distance( p )-person_radi_amp_)*(iit->get_planning_trajectory( )->at(t).distance( p )-person_radi_amp_));
+
+					if( d < robot_person_companion_distance_ )
+					{
+						collision=true;
+						min_distances_vector.push_back(d);
+					}
+				}
+			}
+			break;
+		  case 1://mahalanobis distance, considers collision if inside the std ellipsoid: TOO RESTRICTIVE...
+			 // std::cout << "case 1 (ppl_collision_mode_)" << std::endl;
+			for( auto iit : nearby_person_list_) // de momento, para mi todas las suyas con constantes, han de ser menores que la distancia robot+persona. (luego ya veremos si va bien o no)
+			{
+				if(iit->get_id()!=id_person_companion_){
+					d = sqrt((iit->get_planning_trajectory( )->at(t).cov_dist(p,det)-person_radi_amp_)*(iit->get_planning_trajectory( )->at(t).cov_dist(p,det)-person_radi_amp_));
+					if ( d < robot_person_companion_distance_ )
+					{
+						//std::cout << "Colision detected, pr distance = " << d << std::endl;
+						collision=true;
+						min_distances_vector.push_back(d);
+
+					}
+				}
+			}
+			break;
+		  case 2 ://mahalanobis distance, half the std, much less restrictive
+			  if(debug_companion_){
+				  std::cout << "case 2 (ppl_collision_mode_)" << std::endl;
+			  }
+			for( auto iit : nearby_person_list_)
+			{
+				if(iit->get_id()!=id_person_companion_){
+					d = sqrt((iit->get_planning_trajectory( )->at(t).cov_dist(p,det)-person_radi_amp_)*(iit->get_planning_trajectory( )->at(t).cov_dist(p,det)-person_radi_amp_));
+					if ( det > 1.0)
+					{
+						if ( d < robot_person_companion_distance_)
+						{
+							collision=true;
+							min_distances_vector.push_back(d);
+						}
+					}
+					else
+					{
+						d = iit->get_planning_trajectory( )->at(t).distance( p )-person_radi_amp_;
+						if ( d < robot_person_companion_distance_)
+						{
+							collision=true;
+							min_distances_vector.push_back(d);
+						}
+					}
+				}
+			}
+			break;
+		  case 3 :
+			  if(debug_companion_){
+				  std::cout << "case 3 (ppl_collision_mode_)" << std::endl;
+			  }
+			for( auto iit : nearby_person_list_)
+			{
+				if(iit->get_id()!=id_person_companion_){
+					d = sqrt((iit->get_planning_trajectory( )->at(t).cov_dist(p,det)-person_radi_amp_)*(iit->get_planning_trajectory( )->at(t).cov_dist(p,det)-person_radi_amp_));
+
+					if ( det > 1.0)
+					{
+						if ( d < robot_person_companion_distance_)
+						{
+							collision=true;
+							min_distances_vector.push_back(d);
+						}
+					}
+					else
+					{
+						d = iit->get_planning_trajectory( )->at(t).distance( p );
+						if ( d < robot_person_companion_distance_)
+						{
+							collision=true;
+							min_distances_vector.push_back(d);
+
+						}
+					}
+				}
+			}
+			break;
+		}
+			//calculate probabilities. NOT implemented, insignificant collision values when cov is high
+            //pr_no_col *= 1 - gaussian_constant_ / sqrt(det) * exp( -0.5*d );// * robot_->get_platform_radii_2();//area = pi * r^2 (pi included in gaussian_contant)
+            //std::cout << "Pr of no colision joint = " << pr_no_col << " and distance = " << d << " det = " << det << std::endl;
+	}
+
+	// obtain minimun collision distance.
+	if(!min_distances_vector.empty()){ // si hay alguna distancia minima.
+		double min_dist=min_distances_vector[0];
+		for( unsigned int in=0; in<min_distances_vector.size(); in++){
+			if(min_distances_vector[in]<min_dist){
+				min_dist=min_distances_vector[in];
+			}
+		}
+
+		min_distance_collision_act=min_dist; // sacar fuera la min dist.
+	}
+
+	//std::cout << "min_distance_collision_act= "<<min_distance_collision_act<< std::endl;
+	if(check_execution_times_){
+	std::cout << "collision= "<<collision << std::endl;
+	}
+	//std::cout << "FIN function: check_collision_companion " << std::endl;
+	//check_collision_companion_goal_end = clock();
+	//if(check_execution_times_){
+	//	std::cout << "time_check_collision_companion_goal="<<((check_collision_companion_goal_start-check_collision_companion_goal_end)/clocks_per_sec_my_var_)*1000 << std::endl;
+	//}
+	return min_distance_collision_act;
+}
+
+
+/*** INI calculate_companion_cost_node! ***/
+void Cplan_local_nav::calculate_companion_cost_node( unsigned int parent_index, Crobot* robot_act)
+{
+	 //std::cout << "INI Function: calculate_companion_cost_node" << std::endl;
+	// add the robot companion cost!!! in this node of the tree!!! (colision con robot, que parametro es, para evaluar el path)
+	// si las acumulo como el, ya no podre calcular el coste real, haciendo una ventana hacia atras del coste. Quizas mejor acumular para cada path, hacer un vector de vectores.
+	//  aquí solo acumula/calcula el coste de un path!!!
+	//double angle_forward;
+	double final_min_colision_distance=0.0;
+	bool colision=false; // se inicializa a falso.
+	std::vector<double> min_colision_distances;
+	// ver si hay colision en la circunferencia de radio ROBOT+Persona. Si hay algun obstaculo estatico o persona, prediccion dentro de esa circunferencia.
+	// (check_collision( const Spoint& p=robot_creo ,  int t)) => la tendré que modificar para que devuelva la distancia de colision, tambien!
+	// la check_collision ya tiene los bucles siguientes que hacian falta.
+	// collision check for static objects    (si hay alguno dentro de esa circuferencia => se cambia el bool a true y se guarda esa distancia en el vector, min_distances.
+	// collision check for dynamic persons
+	colision=bool_collision_companion_;
+
+	// busco la distancia minima de todas las distancias de colision.
+	// calculate the angle between robot and person.
+	double angle=angle_companion_; // in grad.
+
+	double robot_radi=robot_->get_platform_radii();
+	double real_robot_person_distance2;
+	if(bool_case_person_companion_){
+		real_robot_person_distance2=calc_robot_person_companion_distance_companion_person_akp();
+	}else{
+		real_robot_person_distance2=calc_robot_person_companion_distance();
+	}
+
+	double limit_of_collision;
+
+	// only one person side-by-side
+	limit_of_collision=robot_person_companion_distance_/2+robot_radi+(little_augmented_collision_margin_/2);
+
+
+	if(colision){
+		final_min_colision_distance=min_distance_collision_; // = distance entre robot y obstaculo más cercano.
+		min_distance_collision_vector_.push_back(min_distance_collision_);
+		//std::cout << " si colision !!!!!!  (calculate_companion_cost_node) colision="<<colision<<"; final_min_colision_distance="<<final_min_colision_distance<< std::endl;
+		//std::cout << "collision!; limit_of_collision="<<limit_of_collision<<"; parent_index="<<parent_index<< std::endl;
+		//std::cout << "final_min_colision_distance="<<final_min_colision_distance<< std::endl;
+	}else{
+		final_min_colision_distance=limit_of_collision;
+		min_distance_collision_vector_.push_back(robot_person_companion_distance_);
+		//std::cout << " No colision !!!!!!  (calculate_companion_cost_node) colision="<<colision<<"; final_min_colision_distance="<<final_min_colision_distance<< std::endl;
+		//std::cout << "NO collision!; limit_of_collision="<<limit_of_collision<< std::endl;
+	}
+
+	if((final_min_colision_distance==limit_of_collision)||(final_min_colision_distance>limit_of_collision)){
+		angle=angle_companion_;
+	}else{
+		if(overpas_obstacles_behind_person_){
+			double division;
+
+			if(case_dynamic_){
+					if((final_min_colision_distance)-robot_radi-(little_augmented_collision_margin_/2)<0){
+						division=0;
+					}else{
+						// only one person side-by-side
+						division=((final_min_colision_distance)-robot_radi-(little_augmented_collision_margin_/2))/((real_robot_person_distance2+2*robot_radi+little_augmented_collision_margin_)/2);
+					}
+				}else{
+					//std::cout << " entro an case NO dynamic"<< std::endl;
+					// en formulacion => dw/Ri => dw=d_real_collision/2-Rr-free_space_margin/2
+					// Ri = (d_real_between_r_and_p +2Rr+Free_space_margin)/2 =>free_space_margin==little_augmented_collision_margin_
+					if((final_min_colision_distance)-robot_radi-(little_augmented_collision_margin_/2)<0){
+						division=0;
+					}else{
+						// only one person side-by-side
+						division=((final_min_colision_distance)-robot_radi-(little_augmented_collision_margin_/2))/((real_robot_person_distance2+2*robot_radi+little_augmented_collision_margin_)/2);
+					}
+				}
+
+				if(division>1){
+					division=1;
+				}else if(division<(-1)){
+					division=-1;
+				}
+				angle=(180/3.14)*(asin(division)); //prueba 1.
+
+				if(angle>angle_companion_){
+					angle=angle_companion_;
+				}
+				angle=180-angle;
+
+				if(angle<angle_companion_){
+					angle=angle_companion_;
+				}
+
+				if(angle>180){
+					angle=180;
+				}
+
+			}else{
+				double division;
+				//double radi_force_influence=0.91; // la A!
+				if(case_dynamic_){
+					//std::cout << " entro an case dynamic"<< std::endl;
+					if((final_min_colision_distance)-robot_radi-(little_augmented_collision_margin_/2)<0){
+						division=0;
+					}else{
+						// only one person side-by-side
+						division=((final_min_colision_distance)-robot_radi-(little_augmented_collision_margin_/2))/((real_robot_person_distance2+2*robot_radi+little_augmented_collision_margin_)/2);
+					}
+
+				}else{
+					if((final_min_colision_distance)-robot_radi-(little_augmented_collision_margin_/2)<0){
+						division=0;
+					}else{
+						// only one person side-by-side
+						division=((final_min_colision_distance)-robot_radi-(little_augmented_collision_margin_/2))/((real_robot_person_distance2+2*robot_radi+little_augmented_collision_margin_)/2);
+					}
+				}
+				if(division>1){
+					division=1;
+				}else if(division<(-1)){
+					division=-1;
+				}
+				angle=(180/3.14)*(asin(division)); //prueba 1.
+				if(angle>angle_companion_){
+					angle=angle_companion_;
+				}
+			}
+		}
+
+		//if(colision){
+		//	std::cout << " (test, colision) angle="<<angle<<"; limit_of_collision="<<limit_of_collision<< std::endl;
+		//	std::cout << " final_min_colision_distance="<<final_min_colision_distance<< " real_robot_person_distance2="<<real_robot_person_distance2<< std::endl;
+		//}
+
+
+		// teniendo la distancia, la he de transformar en un angulo, sabiendo la robot_person_companion_distance_ (distancia maxima)
+		double act_companion_cost; // actual companion cost.
+		act_companion_cost=(0.00000015242)*(pow(angle-angle_companion_,4)); //act_companion_cost=(0.000000015242)*(pow(angle-angle_companion_,4));
+		//std::cout << "act_companion_cost="<<act_companion_cost<< std::endl;
+
+		// CAlculate real companion cost.
+		SpointV_cov robot_actp=robot_->get_planning_trajectory()->at(parent_index);
+		SpointV_cov P1_act=pointer_to_person_companion_->get_planning_trajectory()->at(parent_index);
+		Sdestination p1_dest=pointer_to_person_companion_->get_best_dest();
+
+		double theta_p1=atan2(p1_dest.y-P1_act.y,p1_dest.x-P1_act.x);
+
+		double angle_p1_r=calc_person_companion_orientation_v2(pointer_to_person_companion_);//atan2(P1_act.y-robot_actp.y,P1_act.x-robot_actp.x);
+
+		double new_angle_p1;
+
+		if( diffangle( angle_p1_r, theta_p1) < 0 ){
+			new_angle_p1=(theta_p1 + angle_p1_r)*(180/3.14);
+		}else{
+			new_angle_p1=(theta_p1 - angle_p1_r)*(180/3.14);//(angle)*(180/3.14);
+		}
+
+		if(new_angle_p1<0){
+			new_angle_p1+=360;
+		}
+		if((new_angle_p1>180)&&(new_angle_p1<360)){
+			new_angle_p1-=180;
+		}
+
+		double act_companion_cost_p1=(0.00000015242)*(pow(new_angle_p1-angle_companion_,4));
+
+		double sum_cost=act_companion_cost_p1;
+
+		double distance_p1_r=robot_actp.distance(P1_act);
+
+		double distance_cost_p1=(1-exp(-(((distance_p1_r-1.5)*(distance_p1_r-1.5))/(0.5*0.5)))); // 1.5=ideal_distance
+
+		double distance_cost_tot=distance_cost_p1;
+
+		double cost_tot_dist_and_angle=distance_cost_tot+sum_cost;
+
+		if(parent_index<0){
+			std::cout << "(COMPANION COST) new_angle_p1="<<new_angle_p1<< std::endl;
+			std::cout << "act_companion_cost="<<act_companion_cost<<"; act_companion_cost_p1="<<act_companion_cost_p1<<"; sum_cost="<<sum_cost<< std::endl;
+			std::cout << "distance_cost_p1="<<distance_cost_p1<<"; distance_cost_tot="<<distance_cost_tot<<"; cost_tot_dist_and_angle="<<cost_tot_dist_and_angle<< std::endl;
+		}
+
+		act_companion_cost=cost_tot_dist_and_angle;
+		// calculas si hay incremento de angulo, si lo hay, calculas el coste hacia atras y los angulos hacia atras en el path, para que no haya saltos en la orientación.
+
+		vector_of_companion_collisions_.push_back(colision);
+
+		cost_companion_.push_back( cost_companion_.at(parent_index) + act_companion_cost );
+
+		orientation_person_robot_angles_.push_back(angle);
+
+		parent_index_vector_.push_back(parent_index);
+
+		// copia de seguridad para performances.
+		final_min_collision_distance_vector_.push_back(final_min_colision_distance);
+		real_robot_person_distance2_vector_.push_back(real_robot_person_distance2);
+
+		//std::cout << "act_companion_cost="<<act_companion_cost<< std::endl;
+		//std::cout << "cost_companion_.at(parent_index="<<parent_index<<")="<<cost_companion_.at(parent_index)<< std::endl;
+		//std::cout << "cost_companion_.back() [actual_index="<<cost_companion_.size()<<"]="<<cost_companion_.back()<< std::endl;
+		// std::cout << "FIN Function: calculate_companion_cost_node" << std::endl;
+
+}
+/*** FIN calculate_companion_cost_node! ***/
+
+void Cplan_local_nav::ini_increment_angle(){
+	// version 1: con velocidad maxima de las personas.
+	//std::cout << "*** ini_increment_angle() ***" << std::endl;
+	double real_robot_person_distance2;
+	if(bool_case_person_companion_){
+		real_robot_person_distance2=calc_robot_person_companion_distance_companion_person_akp();
+	}else{
+		real_robot_person_distance2=calc_robot_person_companion_distance();
+	}
+
+	double distance_between_robot_and_person;
+	if(real_robot_person_distance2>robot_person_proximity_distance_){
+		distance_between_robot_and_person=real_robot_person_distance2;
+	}else{
+		distance_between_robot_and_person=robot_person_proximity_distance_;
+	}
+
+	double increment_time = dt_;
+	double people_max_velocity = (5*1000)/3600; //% 5km/seg *(1000m/1km)*(1h/3600seg)= 1.3888888889 metros/seg, aprox 1.4m/seg.
+	double increment_person_distance_in_dt=people_max_velocity*increment_time;
+	angle_increment_of_increment_distance_=2*(180/3.14)*(asin((increment_person_distance_in_dt/2)/distance_between_robot_and_person));
+
+	// version 2: fija, constante, para hacerlo muy incremental
+	angle_increment_of_increment_distance_=0.2; //5 grados, para que cambie antes, durante más rato y más progresivo...
+
+	// version 3: con velocidad real de la persona.
+	//Cperson_abstract* person_obj;
+	//find_person(id_person_companion_ , &person_obj);
+	double in_vel=robot_->get_current_pose().v;
+	in_vel=in_vel*in_vel;
+	double people_real_velocity = sqrt(in_vel);//(robot_->get_a_v_max())*(0.5);//(vel*1000)/3600;
+	increment_person_distance_in_dt=people_real_velocity*increment_time;
+	angle_increment_of_increment_distance_=2*(180/3.14)*(asin((increment_person_distance_in_dt/2)/distance_between_robot_and_person));
+
+	//std::cout << "dt_=" << dt_<<"increment_time="<<increment_time<<"distance_between_robot_and_person="<<distance_between_robot_and_person<< std::endl;
+	//std::cout << "(real_FINAL=v_robot) angle_increment_of_increment_distance=" << angle_increment_of_increment_distance_<< std::endl;
+
+	if(actual_person_Companion_pointer_->get_desired_velocity()>0.5){ // era 0.3
+		in_vel=actual_person_Companion_pointer_->get_current_pointV().v();
+
+		if(debug_file_robot_){
+			std::ofstream fileMatlab2;
+			fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+			fileMatlab2 << "% (if(actual_person_Companion_pointer_->get_desired_velocity()>0.3)) actual_person_Companion_pointer_->get_desired_velocity()= "<<actual_person_Companion_pointer_->get_desired_velocity()<<"\n";
+			fileMatlab2.close();
+		}
+	}else{
+		in_vel=actual_person_Companion_pointer_->get_current_pointV().v();
+		if(debug_file_robot_){
+			std::ofstream fileMatlab2;
+			fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+			fileMatlab2 << "  (else) in_vel=0.3; actual_person_Companion_pointer_->get_desired_velocity()"<< actual_person_Companion_pointer_->get_desired_velocity()<<"\n";
+			fileMatlab2.close();
+		}
+	}
+	in_vel=in_vel*in_vel;
+	people_real_velocity = sqrt(in_vel);//(robot_->get_a_v_max())*(0.5);//(vel*1000)/3600;
+	increment_person_distance_in_dt=people_real_velocity*increment_time;
+	angle_increment_of_increment_distance_=2*(180/3.14)*(asin((increment_person_distance_in_dt/2)/distance_between_robot_and_person));
+	//std::cout <<"; angle_increment_of_increment_distance_="<<angle_increment_of_increment_distance_<< "dt_=" << dt_<<"increment_time="<<increment_time<<"distance_between_robot_and_person="<<distance_between_robot_and_person<< std::endl;
+
+	angle_increment_of_increment_distance_vuelta_al_reves_=angle_increment_of_increment_distance_;
+
+	if(debug_file_robot_){
+		std::ofstream fileMatlab2;
+		fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+		fileMatlab2 << "% angle_increment_of_increment_distance_= "<<angle_increment_of_increment_distance_<<"\n";
+		fileMatlab2 << "% angle_increment_of_increment_distance_vuelta_al_reves_= "<<angle_increment_of_increment_distance_vuelta_al_reves_<<"\n";
+		fileMatlab2.close();
+	}
+
+	//std::cout << "(real_FINAL) robot_->get_current_pose().v=" << robot_->get_current_pose().v<< std::endl;
+	//std::cout << "(real_FINAL) actual_person_Companion_pointer_->get_desired_velocity()=" << actual_person_Companion_pointer_->get_desired_velocity()<< std::endl;
+	//std::cout << "(real_FINAL) actual_person_Companion_pointer_->get_current_pointV().v()=" << actual_person_Companion_pointer_->get_current_pointV().v()<< std::endl;
+	//std::cout << "(real_FINAL) increment_time=" << increment_time<< std::endl;
+	//std::cout << "(real_FINAL) distance_between_robot_and_person=" << distance_between_robot_and_person<< std::endl;
+	//std::cout << "(real_FINAL) increment_person_distance_in_dt=" << increment_person_distance_in_dt<< std::endl;
+	//std::cout << "(real_FINAL=v_persona) angle_increment_of_increment_distance=" << angle_increment_of_increment_distance_<< "; people_real_velocity =" << people_real_velocity << std::endl;
+	//	std::cout << "TTTTTTTTTTTTTTTT (real_FINAL) angle_increment_of_increment_distance=" << angle_increment_of_increment_distance_<< std::endl;
+}
+
+
+void Cplan_local_nav::calculate_companion_path_angle_and_cost(unsigned int parent_index, double actual_angle){
+	// calcula el coste to add en el caso en que haya gran diferencia entre el angulo(robot<->persona) comparando el del nodo actual y el del nodo anterior.
+	// calcula los angulos que hay que moverse en ese path.
+	//std::cout << "INI Function: calculate_companion_path_angle_and_cost" << std::endl;
+
+	double angle_diference;
+	unsigned int caso_angulo;
+
+	if(actual_angle>orientation_person_robot_angles_[parent_index]){
+		angle_diference = actual_angle-orientation_person_robot_angles_[parent_index];
+		caso_angulo=0;
+	}else{
+		angle_diference = orientation_person_robot_angles_[parent_index]-actual_angle;
+		caso_angulo=1;
+	}
+
+	if((angle_diference>angle_increment_of_increment_distance_)&&(caso_angulo==1)){
+		double angle_diference2=angle_diference;
+		double actual_angle_cost=actual_angle;
+		double actual_parent_index=parent_index;
+
+		do{
+			angle_diference2=angle_diference2-angle_increment_of_increment_distance_;
+			if(caso_angulo==0){
+				actual_angle_cost=actual_angle_cost-angle_increment_of_increment_distance_;
+			}else{
+				actual_angle_cost=actual_angle_cost+angle_increment_of_increment_distance_;
+				if(actual_angle_cost>angle_companion_){
+					actual_angle_cost=angle_companion_;
+				}
+			}
+
+			double act_companion_cost=(0.00000015242)*(pow(actual_angle_cost-angle_companion_,4)); //double act_companion_cost=(0.000000015242)*(pow(actual_angle_cost-angle_companion_,4));
+			cost_companion_[actual_parent_index]=cost_companion_[actual_parent_index]+act_companion_cost; // (cambias el cost companion anteriores) // o cost_companion_[actual_parent_index]=cost_companion_[actual_parent_index]+act_companion_cost;
+			actual_parent_index=parent_index_vector_[actual_parent_index];
+		}while((angle_diference2>0)&&(actual_parent_index>=0));
+		//std::cout << "FIN Function: calculate_companion_path_angle_and_cost;" << std::endl;
+	}
+
+}
+
+
+/*  Arreglar angulos para pasar detras del robot   */
+void Cplan_local_nav::go_behind_robot_only_angle_in_final_tree_calculate_companion_path_angle_and_cost3(unsigned int i){
+	// calcula los angulos que hay que moverse en ese path.
+	//std::cout << "    !!!    INI go_behind_robot-only_angle_in_final_tree_calculate_companion_path_angle_and_cost"<< std::endl;
+
+	BEST_path_parent_index_vector_.clear(); // de atras a delante. (en el ultimo coges el primer indice.)
+
+	// CASE 1: actual_angle < actual_parent_angle. Dar vuelta hacia atras del arbol (primera vuelta).
+	unsigned int actual_index;
+	unsigned int actual_parent_index;
+
+	if(calc_goal_companion_with_group_path_){
+		actual_index=i; // recorrete todo el path the este final de camino.
+		actual_parent_index=parent_index_vector_[actual_index];
+	}else{
+		actual_index=orientation_person_robot_angles_with_prediction_of_person_companion_.size()-1; // recorrete todo el path the este final de camino.
+		actual_parent_index=orientation_person_robot_angles_with_prediction_of_person_companion_.size()-2;
+	}
+
+	double actual_angle;
+	double actual_parent_angle;
+
+	if(calc_goal_companion_with_group_path_){
+		actual_angle=orientation_person_robot_angles_[actual_index];
+		actual_parent_angle=orientation_person_robot_angles_[actual_parent_index];
+	}else{
+		actual_angle=orientation_person_robot_angles_with_prediction_of_person_companion_[actual_index];
+		actual_parent_angle=orientation_person_robot_angles_with_prediction_of_person_companion_[actual_parent_index];
+	}
+
+	double diff_angle=actual_parent_angle-actual_angle; // saltos posibles 1- 90grad a 180grad o 0 grad.
+														// 2- de 0grad o 180grad a 90grad.
+	//std::cout << "actual_angle=" <<actual_angle<<"; actual_parent_angle="<<actual_parent_angle<<"; diff_angle="<<diff_angle<< std::endl;
+
+	unsigned int actual_index2; // recorrete todo el path the este final de camino.
+	unsigned int actual_parent_index2;
+		actual_index2=i;
+		actual_parent_index2=parent_index_vector_[actual_index2];
+		BEST_path_parent_index_vector_.push_back(actual_index2);
+		//std::cout << " (ini while) actual_index="<<actual_index<<"; BEST_path_parent_index_vector_.back()"<<BEST_path_parent_index_vector_.back()<< std::endl;
+		do{
+			BEST_path_parent_index_vector_.push_back(actual_index2);
+			actual_index2=actual_parent_index2;
+			actual_parent_index2=parent_index_vector_[actual_index2];
+
+		}while(actual_parent_index2>0);
+		BEST_path_parent_index_vector_.push_back(actual_parent_index2);
+
+	// 1- vuelta del final del path al principio!
+	do{
+		if(calc_goal_companion_with_group_path_){
+			actual_angle=orientation_person_robot_angles_[actual_index];
+			actual_parent_angle=orientation_person_robot_angles_[actual_parent_index];
+		}else{
+			actual_angle=orientation_person_robot_angles_with_prediction_of_person_companion_[actual_index];
+			actual_parent_angle=orientation_person_robot_angles_with_prediction_of_person_companion_[actual_parent_index];
+		}
+
+		if(actual_angle>actual_parent_angle){
+			//caso_vuelta_al_reves=true;
+			diff_angle=actual_angle-actual_parent_angle;
+			if(diff_angle>angle_increment_of_increment_distance_){
+
+				if(calc_goal_companion_with_group_path_){
+					orientation_person_robot_angles_[actual_parent_index]=actual_angle-angle_increment_of_increment_distance_;
+				}else{
+					orientation_person_robot_angles_with_prediction_of_person_companion_[actual_parent_index]=actual_angle-angle_increment_of_increment_distance_;
+				}
+
+				if(calc_goal_companion_with_group_path_){
+					if(orientation_person_robot_angles_[actual_parent_index]<angle_companion_){
+						orientation_person_robot_angles_[actual_parent_index]=angle_companion_;
+					}
+					if(orientation_person_robot_angles_[actual_parent_index]>180){
+						orientation_person_robot_angles_[actual_parent_index]=180;
+					}
+				}else{
+					if(orientation_person_robot_angles_with_prediction_of_person_companion_[actual_parent_index]<angle_companion_){
+						orientation_person_robot_angles_with_prediction_of_person_companion_[actual_parent_index]=angle_companion_;
+					}
+					if(orientation_person_robot_angles_with_prediction_of_person_companion_[actual_parent_index]>180){
+						orientation_person_robot_angles_with_prediction_of_person_companion_[actual_parent_index]=180;
+					}
+				}
+
+			}
+			diff_angle=diff_angle-angle_increment_of_increment_distance_;
+		}else{ // CASE: actual_angle > actual_parent_angle (aquí no se puede hacer...) Hay que dar vuelta hacia delante.
+
+		}
+
+		if(calc_goal_companion_with_group_path_){
+			actual_index=actual_parent_index;
+			actual_parent_index=parent_index_vector_[actual_index]; //busca el nodo anterior de este path.
+		}else{
+			actual_index=actual_index-1;
+			actual_parent_index=actual_parent_index-1; //busca el nodo anterior de este path.
+		}
+
+	}while(actual_parent_index>0);
+	// PARA EL INDICE =0, PQ SINO ENTRA EN BUCLE INFINITO!
+
+	if(calc_goal_companion_with_group_path_){
+		actual_angle=orientation_person_robot_angles_[actual_index];
+		actual_parent_angle=orientation_person_robot_angles_[actual_parent_index];
+	}else{
+		actual_angle=orientation_person_robot_angles_with_prediction_of_person_companion_[actual_index];
+		actual_parent_angle=orientation_person_robot_angles_with_prediction_of_person_companion_[actual_parent_index];
+	}
+
+	if(actual_angle<actual_parent_angle){
+		diff_angle=actual_parent_angle-actual_angle;
+		if(diff_angle>angle_increment_of_increment_distance_){
+
+			if(calc_goal_companion_with_group_path_){
+				orientation_person_robot_angles_[actual_parent_index]=actual_angle+angle_increment_of_increment_distance_;
+				if(orientation_person_robot_angles_[actual_parent_index]<angle_companion_){
+					orientation_person_robot_angles_[actual_parent_index]=angle_companion_;
+				}
+				if(orientation_person_robot_angles_[actual_parent_index]>180){
+					orientation_person_robot_angles_[actual_parent_index]=180;
+				}
+			}else{
+				orientation_person_robot_angles_with_prediction_of_person_companion_[actual_parent_index]=actual_angle+angle_increment_of_increment_distance_;
+				if(orientation_person_robot_angles_with_prediction_of_person_companion_[actual_parent_index]<angle_companion_){
+					orientation_person_robot_angles_with_prediction_of_person_companion_[actual_parent_index]=angle_companion_;
+				}
+				if(orientation_person_robot_angles_with_prediction_of_person_companion_[actual_parent_index]>180){
+					orientation_person_robot_angles_with_prediction_of_person_companion_[actual_parent_index]=180;
+				}
+			}
+
+		}
+	}
+
+	//std::cout << " [IMPORTANTE, ver si es diferente en pasillo y obstaculo!!!] sum_angles_increment1="<<sum_angles_increment1<< std::endl;
+
+	if(!calc_goal_companion_with_group_path_){
+		fix_angles_to_use_person_prediction_for_the_companion_goal();
+	}else{
+		//std::cout << " (final while) actual_parent_index="<<actual_parent_index<<"; BEST_path_parent_index_vector_.back()"<<BEST_path_parent_index_vector_.back()<< std::endl;
+		unsigned int act_index1=BEST_path_parent_index_vector_[BEST_path_parent_index_vector_.size()-1];
+		unsigned int next_index1=BEST_path_parent_index_vector_[BEST_path_parent_index_vector_.size()-2];
+		double act_angle1=orientation_person_robot_angles_[act_index1];
+		double next_angle1=orientation_person_robot_angles_[next_index1];
+		double diff_angle1=act_angle1-next_angle1;
+		if(diff_angle1>angle_increment_of_increment_distance_){
+
+		}
+
+		// 2- vuelta del principio al final
+	/*  caso_vuelta_al_reves!!!!   */
+		for(unsigned int g=0;g<BEST_path_parent_index_vector_.size();g++){
+
+			unsigned int act_index=BEST_path_parent_index_vector_[BEST_path_parent_index_vector_.size()-1-g];
+			unsigned int next_index=BEST_path_parent_index_vector_[BEST_path_parent_index_vector_.size()-2-g];
+
+			double act_angle=orientation_person_robot_angles_[act_index];
+			double next_angle=orientation_person_robot_angles_[next_index];
+
+			if((act_index==0)){
+				act_angle=initial_angle_;
+			}
+			if(next_angle==0){
+				next_angle=initial_angle_;
+			}
+
+			if((BEST_path_parent_index_vector_.size()-1-g)==0){
+				act_angle=initial_angle_;
+			}
+
+			double diff_angle=act_angle-next_angle;
+
+			if(((diff_angle>(2*angle_increment_of_increment_distance_)))&&(diff_angle>0)){
+				//std::cout << "ENTRO EN IF !!!"<< std::endl;
+				//	std::cout << "(!!!CASE!!! actual_angle > actual_parent_angle ) act_index="<<act_index<<"; next_index="<<next_index<< std::endl;
+				// CASE 2: actual_angle > actual_parent_angle (aquí no se puede hacer...) Hay que dar vuelta hacia delante.
+
+				double new_angle=act_angle-(2*angle_increment_of_increment_distance_);
+
+				if(new_angle<angle_companion_){
+					orientation_person_robot_angles_[next_index]=angle_companion_;
+					//std::cout << "if(new_angle<angle_companion_) angle_companion_="<<angle_companion_<< std::endl;
+				}else{
+					orientation_person_robot_angles_[next_index]=new_angle;
+					//std::cout << "else if(new_angle<angle_companion_)new_angle="<<new_angle<< std::endl;
+				}
+				if(new_angle>180){
+					orientation_person_robot_angles_[next_index]=180;
+				}
+
+			}else{
+				//std::cout << "ENTRO EN ELSE !!!"<< std::endl;
+			}
+
+
+
+		} // fin for vuelta hacia delante
+
+		//std::cout << " [IMPORTANTE, ver si es diferente en pasillo y obstaculo!!!] sum_angles_increment="<<sum_angles_increment<< std::endl;
+		//std::cout << " tercera pasada, ahora mismo no se para que!"<< std::endl;
+		// es para mantenerte a 90 grados si decrece tu valor en orientacion!!! Te adelantas demasiado
+		if(!BEST_path_parent_index_vector_.empty()){
+			bool we_have_path_collisions=false;
+			for(unsigned int g=0;g<BEST_path_parent_index_vector_.size();g++){
+
+				if((vector_of_companion_collisions_[BEST_path_parent_index_vector_[BEST_path_parent_index_vector_.size()-1-g]])&&(orientation_person_robot_angles_[BEST_path_parent_index_vector_[BEST_path_parent_index_vector_.size()-1-g]]>(angle_companion_+2*angle_increment_of_increment_distance_))){
+					we_have_path_collisions=true;
+				}
+			}
+
+			double ini_act_angle_out=initial_angle_;
+
+			if((!we_have_path_collisions)&&(ini_act_angle_out>angle_companion_)){
+				for(unsigned int g=1;g<BEST_path_parent_index_vector_.size();g++){
+					unsigned int act_index;
+
+					unsigned int next_index=BEST_path_parent_index_vector_[BEST_path_parent_index_vector_.size()-1-g];
+					double act_angle;
+					if(g==1){
+						act_angle=ini_act_angle_out;
+						act_index=0;
+					}else{
+						act_angle=orientation_person_robot_angles_[act_index];
+					}
+
+					double next_angle=act_angle-5*angle_increment_of_increment_distance_;
+					if(next_angle<angle_companion_){
+						orientation_person_robot_angles_[next_index]=angle_companion_;
+					}else{
+						orientation_person_robot_angles_[next_index]=next_angle;
+					}
+
+					act_index=next_index;
+				}
+			}
+		}
+	}
+	//std::cout << "FIN go_behind_robot ..."<< std::endl;
+}
+
+
+
+/*  Arreglar angulos para pasar detras del robot   */
+void Cplan_local_nav::go_in_front_robot_only_angle_in_final_tree_calculate_companion_path_angle_and_cost3(unsigned int i){
+
+	// calcula los angulos que hay que moverse en ese path.
+
+	BEST_path_parent_index_vector_.clear(); // de atras a delante. (en el ultimo coges el primer indice.)
+
+	// CASE 1: actual_angle < actual_parent_angle. Dar vuelta hacia atras del arbol (primera vuelta).
+	unsigned int actual_index;
+	unsigned int actual_parent_index;
+
+	if(calc_goal_companion_with_group_path_){
+		actual_index=i; // recorrete todo el path the este final de camino.
+		actual_parent_index=parent_index_vector_[actual_index];
+	}else{
+		actual_index=orientation_person_robot_angles_with_prediction_of_person_companion_.size()-1; // recorrete todo el path the este final de camino.
+		actual_parent_index=orientation_person_robot_angles_with_prediction_of_person_companion_.size()-2;
+	}
+
+	double actual_angle;
+	double actual_parent_angle;
+
+	if(calc_goal_companion_with_group_path_){
+		actual_angle=orientation_person_robot_angles_[actual_index];
+		actual_parent_angle=orientation_person_robot_angles_[actual_parent_index];
+	}else{
+		actual_angle=orientation_person_robot_angles_with_prediction_of_person_companion_[actual_index];
+		actual_parent_angle=orientation_person_robot_angles_with_prediction_of_person_companion_[actual_parent_index];
+	}
+
+	double diff_angle=actual_parent_angle-actual_angle; // saltos posibles 1- 90grad a 180grad o 0 grad.
+
+	unsigned int actual_index2; // recorrete todo el path the este final de camino.
+	unsigned int actual_parent_index2;
+	actual_index2=i;
+	actual_parent_index2=parent_index_vector_[actual_index2];
+	BEST_path_parent_index_vector_.push_back(actual_index2);
+	//std::cout << " (ini while) actual_index="<<actual_index<<"; BEST_path_parent_index_vector_.back()"<<BEST_path_parent_index_vector_.back()<< std::endl;
+	do{
+		BEST_path_parent_index_vector_.push_back(actual_index2);
+		actual_index2=actual_parent_index2;
+		actual_parent_index2=parent_index_vector_[actual_index2];
+	}while(actual_parent_index2>0);
+	BEST_path_parent_index_vector_.push_back(actual_parent_index2);
+
+	// 1- vuelta del final del path al principio!
+	do{
+
+		if(calc_goal_companion_with_group_path_){
+			actual_angle=orientation_person_robot_angles_[actual_index];
+			actual_parent_angle=orientation_person_robot_angles_[actual_parent_index];
+		}else{
+			actual_angle=orientation_person_robot_angles_with_prediction_of_person_companion_[actual_index];
+			actual_parent_angle=orientation_person_robot_angles_with_prediction_of_person_companion_[actual_parent_index];
+		}
+
+		//std::cout << " actual_angle"<<actual_angle<<";> actual_parent_angle="<<actual_parent_angle << std::endl;
+		if(actual_angle<actual_parent_angle){
+
+			diff_angle=actual_parent_angle-actual_angle;
+
+			if(diff_angle>angle_increment_of_increment_distance_){
+
+				if(calc_goal_companion_with_group_path_){
+					orientation_person_robot_angles_[actual_parent_index]=actual_angle+angle_increment_of_increment_distance_;
+				}else{
+					orientation_person_robot_angles_with_prediction_of_person_companion_[actual_parent_index]=actual_angle+angle_increment_of_increment_distance_;
+				}
+
+				if(calc_goal_companion_with_group_path_){
+					if(orientation_person_robot_angles_[actual_parent_index]>angle_companion_){
+						orientation_person_robot_angles_[actual_parent_index]=angle_companion_;
+					}
+					if(orientation_person_robot_angles_[actual_parent_index]<0){
+						orientation_person_robot_angles_[actual_parent_index]=0;
+					}
+				}else{
+					if(orientation_person_robot_angles_with_prediction_of_person_companion_[actual_parent_index]>angle_companion_){
+						orientation_person_robot_angles_with_prediction_of_person_companion_[actual_parent_index]=angle_companion_;
+					}
+					if(orientation_person_robot_angles_with_prediction_of_person_companion_[actual_parent_index]<0){
+						orientation_person_robot_angles_with_prediction_of_person_companion_[actual_parent_index]=0;
+					}
+				}
+			}
+			diff_angle=diff_angle-angle_increment_of_increment_distance_;
+		}else{ // CASE: actual_angle > actual_parent_angle (aquí no se puede hacer...) Hay que dar vuelta hacia delante.
+
+		}
+
+		if(calc_goal_companion_with_group_path_){
+			actual_index=actual_parent_index;
+			actual_parent_index=parent_index_vector_[actual_index]; //busca el nodo anterior de este path.
+		}else{
+			actual_index=actual_index-1;
+			actual_parent_index=actual_parent_index-1; //busca el nodo anterior de este path.
+		}
+
+	}while(actual_parent_index>0);//&&(diff_angle>angle_increment_of_increment_distance_));
+	// PARA EL INDICE =0, PQ SINO ENTRA EN BUCLE INFINITO!
+
+	if(calc_goal_companion_with_group_path_){
+		actual_angle=orientation_person_robot_angles_[actual_index];
+		actual_parent_angle=orientation_person_robot_angles_[actual_parent_index];
+	}else{
+		actual_angle=orientation_person_robot_angles_with_prediction_of_person_companion_[actual_index];
+		actual_parent_angle=orientation_person_robot_angles_with_prediction_of_person_companion_[actual_parent_index];
+	}
+
+	if(!calc_goal_companion_with_group_path_){
+		fix_angles_to_use_person_prediction_for_the_companion_goal();
+	}else{
+
+		unsigned int act_index1=BEST_path_parent_index_vector_[BEST_path_parent_index_vector_.size()-1];
+		unsigned int next_index1=BEST_path_parent_index_vector_[BEST_path_parent_index_vector_.size()-2];
+		double act_angle1=orientation_person_robot_angles_[act_index1];
+		double next_angle1=orientation_person_robot_angles_[next_index1];
+		double diff_angle1=act_angle1-next_angle1;
+		if(diff_angle1>angle_increment_of_increment_distance_){
+
+		}
+
+		/*  caso_vuelta_al_reves!!!!   */
+		// es para mantenerte a 90 grados si decrece tu valor en orientacion!!! Te adelantas demasiado
+		if(!BEST_path_parent_index_vector_.empty()){
+			bool we_have_path_collisions=false;
+			for(unsigned int g=0;g<BEST_path_parent_index_vector_.size();g++){
+				if((vector_of_companion_collisions_[BEST_path_parent_index_vector_[BEST_path_parent_index_vector_.size()-1-g]])&&(orientation_person_robot_angles_[BEST_path_parent_index_vector_[BEST_path_parent_index_vector_.size()-1-g]]<(angle_companion_-2*angle_increment_of_increment_distance_))){
+					we_have_path_collisions=true;
+				}else{
+
+				}
+			}
+
+			double ini_act_angle_out=initial_angle_;
+
+			if((!we_have_path_collisions)&&(ini_act_angle_out<angle_companion_)){
+
+				for(unsigned int g=1;g<BEST_path_parent_index_vector_.size();g++){
+
+					unsigned int act_index;
+					unsigned int next_index=BEST_path_parent_index_vector_[0+g];
+					double act_angle;
+					if(g==1){
+						act_angle=ini_act_angle_out;
+						act_index=0;
+					}else{
+						act_angle=orientation_person_robot_angles_[act_index];
+					}
+
+					double next_angle=act_angle+5*angle_increment_of_increment_distance_;
+
+					if(next_angle>angle_companion_){
+						orientation_person_robot_angles_[next_index]=angle_companion_;
+					}else{
+						orientation_person_robot_angles_[next_index]=next_angle;
+					}
+
+					act_index=next_index;
+				}
+			}
+		}
+	}
+}
+
+
+
+
+void Cplan_local_nav::see_companion_path_angle_and_cost_of_min_cost_paths(double parent_index){
+	std::cout << "INI see_companion_path_angle_and_cost_of_min_cost_paths "<< std::endl;
+	std::cout << "orientation_person_robot_angles_.size()= "<<orientation_person_robot_angles_.size()<< std::endl;
+	std::cout << "cost_companion_.size()= "<<cost_companion_.size()<< std::endl;
+	std::cout << "parent_index_vector_.size()= "<<parent_index_vector_.size()<< std::endl;
+	double sum_angles_increment=0;
+	double sum_inc_entre_angles=0;
+
+	double actual_parent_index=parent_index;
+	do{
+		double sum1=orientation_person_robot_angles_[actual_parent_index];
+
+		std::cout << "(path) orientation_person_robot_angles_[actual_parent_index="<<actual_parent_index<<"]=" <<orientation_person_robot_angles_[actual_parent_index]<< std::endl;
+		std::cout << "(path) cost_companion_[actual_parent_index="<<actual_parent_index<<"]=" <<cost_companion_[actual_parent_index]<< std::endl;
+		std::cout << "(path) min_distance_collision_vector_[actual_parent_index="<<actual_parent_index<<"]=" <<min_distance_collision_vector_[actual_parent_index]<< std::endl;
+
+		actual_parent_index=parent_index_vector_[actual_parent_index];
+		double sum2=orientation_person_robot_angles_[actual_parent_index];
+
+		sum_angles_increment=sum_angles_increment+orientation_person_robot_angles_[actual_parent_index];
+		sum_inc_entre_angles=sum_inc_entre_angles+(sum1-sum2);
+
+
+	}while(actual_parent_index>0);
+	sum_inc_entre_angles=sum_inc_entre_angles/orientation_person_robot_angles_.size();
+	sum_angles_increment=sum_angles_increment/orientation_person_robot_angles_.size();
+
+	std::cout << " [IMPORTANTE, ver si es diferente en pasillo y obstaculo!!!] sum_angles_increment="<<sum_angles_increment<< std::endl;
+	std::cout << " [IMPORTANTE, ver si es diferente en pasillo y obstaculo!!!] sum_inc_entre_angles="<<sum_inc_entre_angles<< std::endl;
+
+	std::cout << "despues while"<< std::endl;
+	std::cout << "(path) actual_parent_index="<<actual_parent_index<< std::endl;
+
+	std::cout << "(path) orientation_person_robot_angles_[actual_parent_index="<<0<<"]=" <<orientation_person_robot_angles_[0]<< std::endl;
+	std::cout << "(path) cost_companion_[actual_parent_index="<<0<<"]=" <<cost_companion_[0]<< std::endl;
+
+}
+
+
+
+double Cplan_local_nav::calc_robot_person_companion_distance(){
+	double robot_person_distance;
+	companion_person_position_=actual_person_Companion_SpointV_;
+	robot_person_distance=robot_initial_pose_.distance( (Spoint) companion_person_position_);
+
+	//std::cout << "(calc_robot_person_companion_distance()) robot_person_distance="<<robot_person_distance<< std::endl;
+	//std::cout << "robot_initial_pose_:"<< std::endl;
+	//robot_initial_pose_.print();
+	//std::cout << "companion_person_position_:"<< std::endl;
+	//companion_person_position_.print();
+
+	return robot_person_distance;
+}
+
+
+
+
+
+double Cplan_local_nav::calc_robot_person_companion_distance_companion_person_akp(){
+
+	double robot_person_distance;
+	companion_person_position_=person_companion_->get_current_pointV();
+	robot_person_distance=robot_initial_pose_.distance( (Spoint) companion_person_position_);
+	//std::cout << "(calc_robot_person_companion_distance_companion_person_akp()) robot_person_distance="<<robot_person_distance<< std::endl;
+	//std::cout << "companion_person_position_:"<< std::endl;
+	//companion_person_position_.print();
+
+	return robot_person_distance;//=act_robot_pose.distance(companion_person_position_);
+}
+
+
+// velocidad robot!!!
+
+void Cplan_local_nav::vel_robot_normal(double d_min){
+	   // calculate the desired robot velocity depending on the nearbiest obstacle/person and goal
+		double distance_to_goal = robot_->get_current_pointV().distance( goal_);
+		// when the goal, starts to stop, higher priority than ppl
+	    if( distance_to_goal < distance_to_stop_ ){
+			robot_->set_v_max( max_v_by_system_ * distance_to_goal / distance_to_stop_  );//distance_to_stop is never 0
+	    // if not near the goal, normal velocity regulation according to nearby ppl
+	    }else if ( d_min < 1.0 ){
+	    	robot_->set_v_max( max_v_by_system_ * 0.75 );
+	    }else if ( d_min < 2.0){
+	    	robot_->set_v_max( max_v_by_system_ * 0.85);
+	    /*else if ( d_min < 5.0 ) // only if you want this les than 5 meters. I do not need it in this case
+	    	robot_->set_v_max( max_v_by_system_ * 0.95 );*/
+	    }else{
+	    	 //std::cout <<" (reactive and repulsive) CASE else"<< std::endl;
+	    	robot_->set_v_max( max_v_by_system_  );
+	    }
+
+	    //std::cout <<" max_v_by_system_="<< max_v_by_system_<< std::endl;
+
+}
+
+
+
+void Cplan_local_nav::vel_robot_companion(double d_min , double robot_person_distance, Spoint before_next_goal_of_robot, Cperson_abstract::companion_reactive reactive, unsigned int parent_vertex)
+{
+	//std::cout <<"IMPORTANTE IMPORTANTE in (function: vel_robot_companion) robot_->get_des_vel="<< robot_->get_desired_velocity()<< std::endl;
+	// calculate the desired robot velocity depending on the person companion and the nearbiest obstacle/person and goal
+	Spoint Spoint_pose_robot=(Spoint) robot_->get_current_pointV(); // calculas la distancia al obstaculo desde el centro de ambos (persona y robot).
+	Spose robot=robot_initial_pose_;
+	//Cperson_abstract* person_obj1;
+	//find_person(id_person_companion_ , &person_obj1);
+	Sdestination person_companion_goal=actual_person_Companion_pointer_->get_best_dest();
+	SpointV_cov person1 = actual_person_Companion_pointer_->get_current_pointV();
+
+	// internamente uso: actual_person_Companion_pointer_ ; person1; robot (k podría usar la global)
+	double theta=0;
+	calculate_correct_initial_angle(actual_person_Companion_pointer_, person1, robot, theta);
+
+	// INI calcualte adequate_velocity. // substituir la person velocity por la adequate person velocity.
+	double distance_between_ini_pose_and_final_next_goal;
+	double dist2;
+	double adequate_velocity;
+	if(reactive==Cperson_abstract::Akp_planning){
+
+		distance_between_ini_pose_and_final_next_goal=sqrt((robot_initial_pose_.x-robot_goal_to_person_companion_.x)*(robot_initial_pose_.x-robot_goal_to_person_companion_.x)+(robot_initial_pose_.y-robot_goal_to_person_companion_.y)*(robot_initial_pose_.y-robot_goal_to_person_companion_.y));
+		dist2=distance_between_ini_pose_and_final_next_goal;
+		adequate_velocity=dist2/dt_;
+
+		if(adequate_velocity>max_v_by_system_){
+			adequate_velocity=max_v_by_system_;
+		}
+	}
+
+	double vel_per=sqrt((person1.vy)*(person1.vy) + (person1.vx)*(person1.vx));
+
+	double distance_people_travelled=vel_per*dt_;
+	double distance_angle_diference_person_robot=0;
+	if(min_next_companion_angle_!=angle_companion_){
+		if(min_next_companion_angle_>initial_angle_){
+			distance_angle_diference_person_robot=(min_next_companion_angle_-initial_angle_)*(3.14/180)*(1/robot_person_proximity_distance_); // angulo que necesitas - angulo que tienes realmente.
+		}else{
+			distance_angle_diference_person_robot=(initial_angle_-min_next_companion_angle_)*(3.14/180)*(1/robot_person_proximity_distance_);
+		}
+
+	}
+	//double actual_V_Robot2=(distance_people_travelled+distance_angle_diference_person_robot)/dt_;
+
+
+	real_vel_per=sqrt((person1.vy)*(person1.vy) + (person1.vx)*(person1.vx));
+
+	if((reactive==Cperson_abstract::Akp_planning)){
+
+		if(distance_between_ini_pose_and_final_next_goal>3){ // distancia mayor que 3 metros
+			adequate_velocity=max_v_by_system_;
+		}else if(distance_between_ini_pose_and_final_next_goal<treshold_distance_between_steps_){
+			double vpers_v=vel_per;
+			adequate_velocity=vpers_v; // v_person
+		}else{
+			double vpers_v=vel_per;
+			double x=distance_between_ini_pose_and_final_next_goal+distance_angle_diference_person_robot;
+			adequate_velocity=((max_v_by_system_-vpers_v)/(metros_reduce_vel_-treshold_distance_between_steps_))*(x-treshold_distance_between_steps_)+vpers_v;
+		}
+
+		vel_per=adequate_velocity+litlle_augment_vel_desired_adapted_by_ve_per_for_robot_; // 0.1 to compensate the little retard of the side-by-side-companion
+
+		if(vel_per>max_v_by_system_){
+			vel_per=max_v_by_system_;
+		}
+
+		//std::cout << "(CALCULATE adequate vel robot) distance_between_ini_pose_and_final_next_goal="<<distance_between_ini_pose_and_final_next_goal<<"; distance_angle_diference_person_robot="<<distance_angle_diference_person_robot<<"; treshold_distance_between_steps_="<<treshold_distance_between_steps_<< std::endl;
+		//std::cout << " initial_robot_spoint_.vx="<<initial_robot_spoint_.vx<<"; initial_robot_spoint_.vy="<<initial_robot_spoint_.vy<< std::endl;
+		//std::cout << " initial_person_companion_point_.vx="<<initial_person_companion_point_.vx<<"; initial_person_companion_point_.vy="<<initial_person_companion_point_.vy<< std::endl;
+		//std::cout << " (out) vel_per="<<vel_per<<"; adequate_velocity="<<adequate_velocity<<"; real_vel_per="<<real_vel_per<<"; metros_reduce_vel_="<<metros_reduce_vel_<< std::endl;
+	}
+	// FIN calcualte adequate_velocity.
+
+	Spoint Robot_spoint_act(initial_robot_spoint_.x,initial_robot_spoint_.y,initial_robot_spoint_.time_stamp);
+	//double distance_PR7=person1.distance(Robot_spoint_act);
+
+	Sdestination person_goal=actual_person_Companion_pointer_->get_best_dest();
+	//double per_to_goal_angle = (180/3.14)*atan2(person1.y-person_goal.y , person1.x-person_goal.x);
+	//Spoint robot_goal=get_robot_goal();
+	//double robot_to_goal_angle = (180/3.14)*atan2(initial_robot_spoint_.y-robot_goal.y , initial_robot_spoint_.x-robot_goal.x);
+
+	//double near_obst=1;
+	// find min dist to near obstacle. If obstacle a 2m => near_obst=1.5, multiplicador de velocidad.
+
+	double min_dist_colli_act_global2=check_collision_companion_goal(Spoint_pose_robot,0);
+	/* (companion) Inicio calculo angulo correspondiente a distancia de choque*/
+
+	if(debug_file_robot_){
+		std::ofstream fileMatlab2;
+		fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+		fileMatlab2 << "; (DISTANCIA al obstaculo desde el centro actual de ambos!!!) min_dist_colli_act_global2="<<min_dist_colli_act_global2<<"\n";
+		fileMatlab2.close();
+	}
+
+
+	/*double angle_colision=angle_companion_; // in grad.
+	if((min_dist_colli_act_global2==robot_person_companion_distance_)||(min_dist_colli_act_global2>robot_person_companion_distance_)){
+		angle_colision=angle_companion_;
+	}else{
+		angle_colision=(180/3.14)*(asin((min_dist_colli_act_global2)/(robot_person_companion_distance_)));
+
+	}*/
+
+
+	double circunferencia_tope=(robot_person_proximity_distance_+robot_person_proximity_tolerance_+offset_atractive_);
+
+	robot_orient_=false;
+	// INICIO Calculate the robot velocity to increment People distance + angle_increment_of_increment_distance_ [grados]
+	double distance_L_with_this_people_angle_increment=angle_increment_of_increment_distance_*(3.14/180)*(1/robot_person_proximity_distance_);
+	double actual_V_Robot=(distance_people_travelled+distance_L_with_this_people_angle_increment)/dt_;
+
+	//std::cout << "[(distance_people_travelled+distance_L_with_this_people_angle_increment)/dt_] actual_V_Robot="<<actual_V_Robot<< std::endl;
+
+	if(debug_real_test_companion_){
+		std::cout << " distance_L_with_this_people_angle_increment="<<distance_L_with_this_people_angle_increment<< std::endl;
+		std::cout << " dt_="<<dt_<< std::endl;
+		std::cout << " distance_people_travelled="<<distance_people_travelled<< std::endl;
+		std::cout << " actual_V_Robot="<<actual_V_Robot<< std::endl;
+	}
+	// FIN Calculate the robot velocity to increment People distance + angle_increment_of_increment_distance_ [grados]
+
+	// INICIO Calculate the robot velocity en giros!
+	// hay que calcular la distancia real que se desplaza la persona desde esta iteración a la siguiente según su predicción.
+	// o la distancia de la posición actual del robot a la siguiente que tiene que ir (calculada con la rama ganadora del path).
+	//  FIN  Calculate the robot velocity en giros!
+
+	//double margen_angle_colision=1; // inicial era =15
+	if(debug_real_test_companion2_){
+		std::cout << " VELOCIDAAAAAAAAAAADDDDD"<< std::endl;
+		std::cout << " robot_person_distance="<<robot_person_distance<<"; circunferencia_tope="<<circunferencia_tope<<"; initial_angle_="<<initial_angle_<<"; min_next_companion_angle_="<<min_next_companion_angle_<<"; angle_companion_="<<angle_companion_<<"; marge_angle_companion_="<<marge_angle_companion_<<std::endl;
+		std::cout <<"; min_dist_colli_act_global2="<<min_dist_colli_act_global2<<"; robot_person_companion_distance_="<<robot_person_companion_distance_<< std::endl;
+		std::cout <<"; actual INCREMENT angle=initial_angle_-min_next_companion_angle_=angulo_al_que_voy-angulo_al_que_quiero_ir"<<sqrt((initial_angle_-min_next_companion_angle_)*(initial_angle_-min_next_companion_angle_))<< std::endl;
+	}
+	/*  calcular velocidad persona maxima */
+	if(vel_per_max_caso_robot_a_0_o_180_grados_<vel_per){
+		vel_per_max_caso_robot_a_0_o_180_grados_=vel_per;
+	}
+
+	real_angle_person_robot_=initial_angle_; // for results in matlab.
+	real_distance_person_robot_=robot_person_distance;
+
+	//std::cout << " IMPORTANT!!!! real_angle_person_robot_="<<real_angle_person_robot_<<"; real_distance_person_robot_="<<real_distance_person_robot_<<"; angle_companion_"<<angle_companion_<<"; marge_angle_companion_="<<marge_angle_companion_<< std::endl;
+
+	vel_per_ok_=Cperson_abstract::Far_goal;
+	go_with_vel_per_=false;
+
+	save_angle_between_person_and_robot_=initial_angle_;
+	save_distance_between_person_and_robot_=robot_person_distance;
+
+	robot_->set_desired_velocty(vel_per);
+	if(dmin_global_ < (workspace_radii_*workspace_radii_)){ //solo cambiamos velocidad cuando hay obstaculos.
+		//set_robot_velocity_using_obstacles(vel_per,  near_obst, actual_V_Robot,  distance_PR7, robot_person_distance,  d_min, min_dist_colli_act_global2, circunferencia_tope, margen_angle_colision, angle_colision); // funcion que setea la velocidad del robot a la deseada actual + alguna variable global relacionada con la velocidad que necesita el robot.
+	}
+
+	//std::cout <<"IMPORTANTE IMPORTANTE out (function: vel_robot_companion) robot_->get_des_vel="<< robot_->get_desired_velocity()<< std::endl;
+	if(see_std_out_velocities_){
+		std::cout << " (vel_robot_companion) !!!!!!!!!!!!!! 77777777777!!!!!!!!!! robot_->get_v_max()="<<robot_->get_v_max() << std::endl;
+	}
+
+	//std::cout << " OUT CHANGE VEL person!!!!  ; v_max_="<<robot_->get_v_max()<<"; max_v_by_system_="<<max_v_by_system_<< std::endl;
+}
+
+void Cplan_local_nav::return_max_velocity_systemRobot_to_max_value(){
+
+	    // calculate the desired robot velocity depending on the nearbiest obstacle/person and goal
+	    double distance_to_goal = robot_->get_current_pointV().distance( goal_);
+		// when the goal, starts to stop, higher priority than ppl
+	    if( distance_to_goal < distance_to_stop_ )
+			robot_->set_v_max( max_v_by_system_ * distance_to_goal / distance_to_stop_  );//distance_to_stop is never 0
+	    // if not near the goal, normal velocity regulation according to nearby ppl
+	    else if ( sqrt(d_min_global_) < 1.0 )
+	    	robot_->set_v_max( max_v_by_system_ * 0.75 );
+	    else if ( sqrt(d_min_global_) < 2.0)
+	    	robot_->set_v_max( max_v_by_system_ * 0.85);
+	    /*else if ( sqrt(d_min_global_) < 5.0 ) //only I want to reduce velocity for obstacles near 2 meters.
+	    	robot_->set_v_max( max_v_by_system_ * 0.95 );*/
+	    else
+	    	robot_->set_v_max( max_v_by_system_  );
+}
+
+/*
+void Cplan_local_nav::calculate_actual_angle_person_robot(unsigned int index){
+	//std::cout << " !!! calculate_actual_angle_person_robot() !!!" << std::endl;
+    // ini calcular nuevo angulo theta con orientación dirección movimiento persona.
+	Cperson_abstract* person_obj;
+	find_person(id_person_companion_ , &person_obj);
+
+    SpointV_cov person = person_obj->get_current_pointV();
+    double theta=calc_person_companion_orientation();
+     // fin calcular nuevo angulo theta con orientación dirección movimiento persona.
+
+	 double angle=atan2(robot_initial_pose_.y-companion_person_position_.y , robot_initial_pose_.x-companion_person_position_.x);
+	 double actual_angle;
+
+	 if( diffangle(theta, angle) < 0 ){
+	 	actual_angle=theta+angle;
+	 }else{
+		 actual_angle=theta-angle;
+	 }
+
+	 actual_angle=(actual_angle*180)/3.14;
+
+	 if((actual_angle>=0)&&( actual_angle<=180)){
+		 actual_angle=actual_angle;
+	 }else if((actual_angle<0)&&(actual_angle>=(-180))){
+		 actual_angle=sqrt(actual_angle*actual_angle);
+	 }else if((actual_angle>180)&&(actual_angle<=360)){
+		 actual_angle=360-actual_angle;
+	 }else if((actual_angle<(-180))&&(actual_angle>=(-360))){
+		 actual_angle=sqrt(actual_angle*actual_angle);
+		 actual_angle=360-actual_angle;
+	 }else if(actual_angle>360){
+		 unsigned int n_360_g=actual_angle/360;
+		 actual_angle=actual_angle-n_360_g*360;
+	 }else{
+      	  //std::cout << " OJO!!! caso NO contemplado actual_angle="<<actual_angle<< std::endl;
+	 }
+
+	 double angle_diference;
+	 if(actual_angle>min_next_companion_angle_){
+		 angle_diference=actual_angle-min_next_companion_angle_;
+	 }else{
+		 angle_diference=min_next_companion_angle_-actual_angle;
+	 }
+	 //double distance_robot_L=angle_diference*(3.14/180)*(robot_person_proximity_distance_-1);
+	// double V_robot_theorical=distance_robot_L/0.2;
+
+	// std::cout << " angle_diference="<<angle_diference<<"robot_person_proximity_distance_="<<robot_person_proximity_distance_<< std::endl;
+	// std::cout << " distance_robot_L="<<distance_robot_L<< std::endl;
+	// std::cout << " V_theorical="<<V_robot_theorical<< std::endl;
+	// std::cout << " max_v_by_system_="<<max_v_by_system_<< std::endl;
+}*/
+
+
+double Cplan_local_nav::calculate_actual_angle_person_robot(Spoint person, Spoint robot){
+//std::cout << " !!! calculate_actual_angle_person_robot() !!!" << std::endl;
+
+   	// ini calcular nuevo angulo theta con orientación dirección movimiento persona.
+    //Cperson_abstract* person_obj;
+   //find_person(id_person_companion_ , &person_obj);
+    double theta=calc_person_companion_orientation();
+    // fin calcular nuevo angulo theta con orientación dirección movimiento persona.
+
+	 double angle=atan2(robot.y-person.y , robot.x-person.x);
+	 double actual_angle;
+
+	 if( diffangle(theta, angle) < 0 ){
+		actual_angle=theta+angle;
+	 }else{
+		 actual_angle=theta-angle;
+	 }
+
+	 actual_angle=(actual_angle*180)/3.14;
+
+	 if((actual_angle>=0)&&( actual_angle<=180)){
+		 actual_angle=actual_angle;
+	 }else if((actual_angle<0)&&(actual_angle>=(-180))){
+		 actual_angle=sqrt(actual_angle*actual_angle);
+	 }else if((actual_angle>180)&&(actual_angle<=360)){
+		 actual_angle=360-actual_angle;
+	 }else if((actual_angle<(-180))&&(actual_angle>=(-360))){
+		 actual_angle=sqrt(actual_angle*actual_angle);
+		 actual_angle=360-actual_angle;
+	 }else if(actual_angle>360){
+		 unsigned int n_360_g=actual_angle/360;
+		 actual_angle=actual_angle-n_360_g*360;
+	 }else{
+		//std::cout << " OJO!!! caso NO contemplado actual_angle="<<actual_angle<< std::endl;
+	 }
+
+	 return actual_angle;
+
+}
+
+
+
+double Cplan_local_nav::calculate_anisotropy( const SpointV_cov& center_person, const Spoint& interacting_person , double lambda )
+{
+	//setting the corresponding SFM parameters:
+	// if no forces param provided, exit program
+	// center_person == robot_pose
+	// interacting_person == obstacle (dynamic or static)
+	// lambda=>social_forces_param->at(1) =>  const std::vector<double>* social_forces_param =>
+
+	// if anisotropy < 0.5, ese objeto no ha de influir en los obstaculos, min distance colision, para posicionamiento del robot.
+	lambda=0.2;// provisional.
+
+	double d;
+	//geometry calculations
+	double dx = center_person.x - interacting_person.x;
+	double dy = center_person.y - interacting_person.y;
+	double vx = center_person.vx;
+	double vy = center_person.vy;
+	d = sqrt(dx*dx+dy*dy);
+	dx /= d;
+	dy /= d;
+
+	//force direction
+	double phi = diffangle( atan2(vy,vx), atan2(-dy,-dx) );//minus difference vector
+	double anisotropy = (lambda + (1-lambda)*(1 + cos(phi))/2 );
+
+	return anisotropy;
+}
+
+
+
+
+double Cplan_local_nav::calculate_modif_person_robot_distance( double ite )
+{
+
+	double modif_person_robot_distance=robot_person_proximity_distance_;
+
+	if(overpas_obstacles_behind_person_){
+
+		if(debug_file_robot_){
+			std::ofstream fileMatlab2;
+			fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+			fileMatlab2 << " if(overpas_obstacles_behind_person_)=>(1) ite=\n";
+			fileMatlab2.close();
+		}
+
+		if((ite>=0)&&(ite<=90)){
+			ite=180-ite;
+			if(debug_file_robot_){
+				std::ofstream fileMatlab2;
+				fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+				fileMatlab2 << " diff_angle [0<->90] (1) ite ="<<ite <<"\n";
+				fileMatlab2.close();
+			}
+		}else if((ite>90)&&(ite<=180)){
+			ite=ite;
+			if(debug_file_robot_){
+				std::ofstream fileMatlab2;
+				fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+				fileMatlab2 << " diff_angle [90<->180] (1) ite ="<<ite <<"\n";
+				fileMatlab2.close();
+			}
+		}else if((ite < 0)&&(ite>=(-90))){
+			ite=sqrt(ite*ite);
+			ite=180-ite;
+
+			if(debug_file_robot_){
+				std::ofstream fileMatlab2;
+				fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+				fileMatlab2 << " diff_angle [0<-> -90] (1) ite ="<<ite <<"\n";
+				fileMatlab2.close();
+			}
+
+		}else if((ite < -90)&&(ite>=(-180))){
+			ite=sqrt(ite*ite);
+
+			if(debug_file_robot_){
+				std::ofstream fileMatlab2;
+				fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+				fileMatlab2 << " diff_angle [-90<->-180] (1) ite ="<<ite <<"\n";
+				fileMatlab2.close();
+			}
+		}else if((ite>180)&&(ite<=270)){
+			ite=ite-180;
+			ite=180-ite;
+
+			if(debug_file_robot_){
+				std::ofstream fileMatlab2;
+				fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+				fileMatlab2 << " diff_angle [180<-> 360] (1) ite="<<ite <<"\n";
+				fileMatlab2.close();
+			}
+		}else if((ite>270)&&(ite<=360)){
+			ite=360-ite;
+			ite=180-ite;
+
+			if(debug_file_robot_){
+				std::ofstream fileMatlab2;
+				fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+				fileMatlab2 << " diff_angle [180<-> 360] (1) ite ="<<ite <<"\n";
+				fileMatlab2.close();
+			}
+		}
+		else if((ite < ( -180))&&(ite>=(-270))){
+			ite=sqrt(ite*ite);
+			ite=ite-180;
+			ite=180-ite;
+
+			if(debug_file_robot_){
+				std::ofstream fileMatlab2;
+				fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+				fileMatlab2 << " diff_angle [-180<-> -360] (1) ite="<<ite <<"\n";
+				fileMatlab2.close();
+			}
+		}else if((ite < ( -270))&&(ite>=(-360))){
+			ite=sqrt(ite*ite);
+			ite=360-ite;
+			ite=180-ite;
+
+			if(debug_file_robot_){
+				std::ofstream fileMatlab2;
+				fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+				fileMatlab2 << " diff_angle [-180<-> -360] (1) ite ="<<ite <<"\n";
+				fileMatlab2.close();
+			}
+		}
+		else if(ite>360){
+			 unsigned int n_360_g=ite/360;
+			 ite=ite-n_360_g*360;
+		    // std::cout << " OJO!!! caso mayor 360 ; ite=ite-n_360_g*360="<<ite<<"; n_360_g="<<n_360_g<< std::endl;
+
+		}else{
+			std::cout << " OJO!!! caso NO contemplado ite="<<ite<< std::endl;
+		}
+
+		modif_person_robot_distance=(((-0.005)*(ite-135)))+0.95;
+
+
+		if(debug_file_robot_){
+			std::ofstream fileMatlab2;
+				fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+				fileMatlab2 << "ite="<<ite<<"; (real_distance) person_robot_actual_real_distance_="<<person_robot_actual_real_distance_<< "\n";
+				fileMatlab2 << "(((-0.010)*(ite-135)))+1="<<(((-0.010)*(ite-135)))+1<<"; robot_person_proximity_distance_="<<robot_person_proximity_distance_<< "\n";
+				fileMatlab2 << "modif_person_robot_distance=(0.00024679)*pow((ite-135),2)+(robot_person_proximity_distance_-0.5)="<<modif_person_robot_distance<< "\n";
+				fileMatlab2.close();
+		}
+
+	}else{
+
+		if(debug_real_test_companion_){
+			std::cout << "(1) ite="<<ite<< std::endl;
+		}
+
+		if((ite>=0)&&(ite<=90)){
+			ite=ite;
+			if(debug_real_test_companion_){
+				std::cout << " diff_angle [0<->90] (1) ite ="<<ite << std::endl;
+			}
+		}else if((ite>90)&&(ite<=180)){
+			ite=sqrt((ite-180)*(ite-180));
+			if(debug_real_test_companion_){
+				std::cout << " diff_angle [90<->180] (1) ite ="<<ite << std::endl;
+			}
+		}else if((ite < 0)&&(ite>=(-90))){
+			ite=sqrt(ite*ite);
+			if(debug_real_test_companion_){
+				std::cout << " diff_angle [0<-> -90] (1) ite ="<<ite << std::endl;
+			}
+		}else if((ite < -90)&&(ite>=(-180))){
+			ite=sqrt(ite*ite);
+			ite=sqrt((ite-180)*(ite-180));
+			if(debug_real_test_companion_){
+				std::cout << " diff_angle [-90<->-180] (1) ite ="<<ite << std::endl;
+			}
+		}else if((ite>180)&&(ite<=270)){
+			ite=ite-180;
+			if(debug_real_test_companion_){
+				std::cout << " diff_angle [180<-> 360] (1) ite ="<<ite << std::endl;
+			}
+		}else if((ite>270)&&(ite<=360)){
+			ite=sqrt(((ite-270)-90)*((ite-270)-90));
+			if(debug_real_test_companion_){
+				std::cout << " diff_angle [180<-> 360] (1) ite ="<<ite << std::endl;
+			}
+		}
+		else if((ite < ( -180))&&(ite>=(-270))){
+			ite=sqrt(ite*ite);
+			ite=ite-180;
+			if(debug_real_test_companion_){
+				std::cout << " diff_angle [-180<-> -360] (1) ite ="<<ite << std::endl;
+			}
+		}else if((ite < ( -270))&&(ite>=(-360))){
+			ite=sqrt(ite*ite);
+			ite=sqrt(((ite-270)-90)*((ite-270)-90));
+			if(debug_real_test_companion_){
+				std::cout << " diff_angle [-180<-> -360] (1) ite ="<<ite << std::endl;
+			}
+		}else if(ite>360){
+			 unsigned int n_360_g=ite/360;
+			 ite=ite-n_360_g*360;
+			 if(debug_real_test_companion_){
+		    // std::cout << " OJO!!! caso mayor 360 ; ite=ite-n_360_g*360="<<ite<<"; n_360_g="<<n_360_g<< std::endl;
+			 }
+		}
+		else{
+			 //std::cout << " OJO!!! caso NO contemplado ite="<<ite<< std::endl;
+		}
+
+		modif_person_robot_distance=(0.00024679)*pow((ite-45),2)+(robot_person_proximity_distance_-0.3);
+		if(debug_real_test_companion_){
+			std::cout << "ite="<<ite<< std::endl;
+			std::cout << "(0.00024679)*pow((ite-45),2)="<<(0.00024679)*pow((ite-45),2)<< std::endl;
+			std::cout << "modif_person_robot_distance="<<modif_person_robot_distance<< std::endl;
+		}
+	}
+
+	return modif_person_robot_distance;
+}
+
+
+//////////////////////////////
+void Cplan_local_nav::printToMatlab()
+{
+
+	if(see_save_in_file_){
+		std::cout <<" IN printToMatlab() !!!"<< std::endl;
+	}
+
+	std::ofstream fileMatlab;
+	fileMatlab.open (results_file_.c_str(), std::ofstream::out | std::ofstream::app);
+	ideal_distance_person_robot_=robot_person_proximity_distance_;
+
+	//std::cout << "results_file_="<<results_file_<< std::endl;
+
+
+	//Cperson_abstract* person_obj;
+	//find_person(id_person_companion_ , &person_obj);
+	double time=ros_time_to_sec_;
+	actual_time_=time;
+	SpointV_cov person=actual_person_Companion_pointer_->get_current_pointV();
+
+    // Algorithm Params
+	fileMatlab << " \n \n \n % New iteration! \n";
+	fileMatlab << "\n";
+
+	// COMPUTATIONAL TIMES and dt
+	fileMatlab << "dt("<<experiment_<<","<<iteration_<<")= "<<dt_<<"\n";
+	fileMatlab << "real_computation_time_dt("<<experiment_<<","<<iteration_<<")= "<<final_compt_time_<<"\n";
+	fileMatlab << "computational_time("<<experiment_<<","<<iteration_<<")= "<<save_computational_time_value_<<"\n";
+	fileMatlab << "robot_plan_companion2_start_ITER("<<experiment_<<","<<iteration_<<")= "<<robot_plan_companion2_start_ITER<<"\n";
+	fileMatlab << "robot_plan_companion2_end_ITER("<<experiment_<<","<<iteration_<<")= "<<robot_plan_companion2_end_ITER<<"\n";
+
+	fileMatlab << "experiment("<<experiment_<<","<<iteration_<<")= "<<experiment_<<"\n";
+	fileMatlab << "iteration("<<experiment_<<","<<iteration_<<")= "<<iteration_<<"\n";
+
+	// Companion person 1
+	fileMatlab << "companion_person_x("<<experiment_<<","<<iteration_<<")= "<<initial_person_companion_point_.x<<"\n";
+	fileMatlab << "companion_person_y("<<experiment_<<","<<iteration_<<")= "<<initial_person_companion_point_.y<<"\n";
+	fileMatlab << "companion_person_vx("<<experiment_<<","<<iteration_<<")= "<<initial_person_companion_point_.vx<<"\n";
+	fileMatlab << "companion_person_vy("<<experiment_<<","<<iteration_<<")= "<<initial_person_companion_point_.vy<<"\n";
+	fileMatlab << "companion_person_v("<<experiment_<<","<<iteration_<<")= "<<sqrt(initial_person_companion_point_.vx*initial_person_companion_point_.vx+initial_person_companion_point_.vy*initial_person_companion_point_.vy)<<"\n";
+	fileMatlab << "companion_person_theta("<<experiment_<<","<<iteration_<<")= "<<atan(initial_person_companion_point_.vy/initial_person_companion_point_.vx)<<"\n";
+	if(we_have_pointer_to_first_person_){
+		fileMatlab << "companion_person_dest_x("<<experiment_<<","<<iteration_<<")= "<<pointer_to_person_companion_->get_best_dest().x<<"\n";
+		fileMatlab << "companion_person_dest_y("<<experiment_<<","<<iteration_<<")= "<<pointer_to_person_companion_->get_best_dest().y<<"\n";
+		fileMatlab << "companion_person_time("<<experiment_<<","<<iteration_<<")= "<<pointer_to_person_companion_->get_current_pointV().time_stamp<<"\n";
+	}
+
+	// save robot pose, velocities, etc
+	fileMatlab << "robot_x("<<experiment_<<","<<iteration_<<")= "<<robot_initial_pose_.x<<"\n";
+	fileMatlab << "robot_y("<<experiment_<<","<<iteration_<<")= "<<robot_initial_pose_.y<<"\n";
+	fileMatlab << "robot_vx("<<experiment_<<","<<iteration_<<")= "<<initial_robot_spoint_.vx<<"\n";
+	fileMatlab << "robot_vy("<<experiment_<<","<<iteration_<<")= "<<initial_robot_spoint_.vy<<"\n";
+	fileMatlab << "robot_theta("<<experiment_<<","<<iteration_<<")= "<<robot_initial_pose_.theta<<"\n"; //OJO, estan en radianes  *180/3.14
+	fileMatlab << "robot_v("<<experiment_<<","<<iteration_<<")= "<<robot_initial_pose_.v<<"\n";
+	fileMatlab << "robot_w("<<experiment_<<","<<iteration_<<")= "<<robot_initial_pose_.w<<"\n";
+	fileMatlab << "robot_pose_time_stamp("<<experiment_<<","<<iteration_<<")= "<<robot_initial_pose_.time_stamp<<"\n";
+	fileMatlab << "robot_final_goal_x("<<experiment_<<","<<iteration_<<")= "<<robot_->get_best_dest().x<<"\n";
+	fileMatlab << "robot_final_goal_y("<<experiment_<<","<<iteration_<<")= "<<robot_->get_best_dest().y<<"\n";
+
+	// distancias entre robot-p1 y p2 y entre p2 y p1
+	double real_dist_r_p1;
+	if(we_have_pointer_to_first_person_){
+		fileMatlab << "d_r_1p_x("<<experiment_<<","<<iteration_<<")= "<<sqrt((robot_->get_current_pointV().x-pointer_to_person_companion_->get_current_pointV().x)*(robot_->get_current_pointV().x-pointer_to_person_companion_->get_current_pointV().x))<<"\n";
+		fileMatlab << "d_r_1p_y("<<experiment_<<","<<iteration_<<")= "<<sqrt((robot_->get_current_pointV().y-pointer_to_person_companion_->get_current_pointV().y)*(robot_->get_current_pointV().y-pointer_to_person_companion_->get_current_pointV().y))<<"\n";
+		fileMatlab << "d_r_1p_m("<<experiment_<<","<<iteration_<<")= "<<robot_->get_current_pointV().distance(pointer_to_person_companion_->get_current_pointV())<<"\n";
+		real_dist_r_p1=robot_->get_current_pointV().distance(pointer_to_person_companion_->get_current_pointV());
+	}
+
+
+	// angulos entre P1  (Theta de cada persona y robot estan guardadas antes.
+	double theta_p1=calc_person_companion_orientation_v2(pointer_to_person_companion_);
+
+	double global_angle_r_p1=atan2(robot_initial_pose_.y-initial_person_companion_point_.y , robot_initial_pose_.x-initial_person_companion_point_.x)*180/3.14;
+
+	//double theta_robot=robot_initial_pose_.theta;
+
+	double angle_r_p1; //angle_r_p1_v2
+	if(diffangle(theta_p1,global_angle_r_p1*3.14/180)<0){ // hay que pasar global_angle_r_p1 a radianes
+		angle_r_p1=(theta_p1 + global_angle_r_p1*(3.14/180))*(180/3.14);
+	}else{
+		angle_r_p1=(theta_p1 - global_angle_r_p1*(3.14/180))*(180/3.14);
+	}
+
+	if(angle_r_p1>360){
+		angle_r_p1=angle_r_p1-360;
+	}else if(angle_r_p1<0){
+		angle_r_p1=360+angle_r_p1;
+	}
+
+	fileMatlab << "angle_r_p1("<<experiment_<<","<<iteration_<<")= "<<angle_r_p1<<"\n"; // estan en radianes! ojo! y sin lo de pasarlos a positivo. En teoria estan en coordenadas globales, como las posiciones de ellos.
+	// si queremos el angulo de posicionamiento respecto al centro del grupo, puede que necesitemos el centro del grupo y la orientación hacia el goal. (se puede guardar además o calcular directamente con lo que ya guardamos.)
+	// hay mucho que guardo, que se podría calcular en matlab, si veo que es muy costoso computacionalmente pasarlo a matlab.
+
+	// save leght global path and center of the group to evaluate the performance of the approaching task.
+	// CENTRO DE LOS 3, robot + persona1 + persona2
+	fileMatlab << "center_of_the_group_R_P1_P2_x("<<experiment_<<","<<iteration_<<")= "<<center_of_the_group_Zanlungo_.x<<"\n";
+	fileMatlab << "center_of_the_group_R_P1_P2_y("<<experiment_<<","<<iteration_<<")= "<<center_of_the_group_Zanlungo_.y<<"\n";
+	fileMatlab << "center_of_the_group_R_P1_P2_vx("<<experiment_<<","<<iteration_<<")= "<<center_of_the_group_Zanlungo_.vx<<"\n";
+	fileMatlab << "center_of_the_group_R_P1_P2_vy("<<experiment_<<","<<iteration_<<")= "<<center_of_the_group_Zanlungo_.vy<<"\n";
+	fileMatlab << "center_of_the_group_R_P1_P2_theta("<<experiment_<<","<<iteration_<<")= "<<sqrt((center_of_the_group_Zanlungo_.vx*center_of_the_group_Zanlungo_.vx)+(center_of_the_group_Zanlungo_.vy*center_of_the_group_Zanlungo_.vy))<<"\n";
+
+	// centro y orientacion de SOLO las personas del grupo: p1 y p2
+	fileMatlab << "center_of_the_group_P1_P2_x("<<experiment_<<","<<iteration_<<")= "<<mean_people_Zalungo_.x<<"\n";
+	fileMatlab << "center_of_the_group_P1_P2_y("<<experiment_<<","<<iteration_<<")= "<<mean_people_Zalungo_.y<<"\n";
+	fileMatlab << "center_of_the_group_P1_P2_vx("<<experiment_<<","<<iteration_<<")= "<<mean_people_Zalungo_.vx<<"\n";
+	fileMatlab << "center_of_the_group_P1_P2_vy("<<experiment_<<","<<iteration_<<")= "<<mean_people_Zalungo_.vy<<"\n";
+	fileMatlab << "center_of_the_group_P1_P2_theta("<<experiment_<<","<<iteration_<<")= "<<theta_of_the_group_Zanlungo_<<"\n";
+
+
+	fileMatlab <<"\n \n";
+	fileMatlab << "last_pose_command_x("<<experiment_<<","<<iteration_<<")= "<<last_pose_command_.x<<"\n";
+	fileMatlab << "last_pose_command_y("<<experiment_<<","<<iteration_<<")= "<<last_pose_command_.y<<"\n";
+	fileMatlab << "last_pose_command_v("<<experiment_<<","<<iteration_<<")= "<<last_pose_command_.v<<"\n";
+	fileMatlab << "last_pose_command_w("<<experiment_<<","<<iteration_<<")= "<<last_pose_command_.w<<"\n";
+	fileMatlab << "last_pose_command_theta("<<experiment_<<","<<iteration_<<")= "<<last_pose_command_.theta*180/3.14<<"\n";
+	fileMatlab << "last_pose_command_time_stamp("<<experiment_<<","<<iteration_<<")= "<<last_pose_command_.time_stamp<<"\n \n ";
+
+	fileMatlab << "save_matlab_final_min_collision_distance_vector_("<<experiment_<<","<<iteration_<<")= "<<save_matlab_final_min_collision_distance_vector_<<"\n";
+	fileMatlab << "save_matlab_real_robot_person_distance2_vector_("<<experiment_<<","<<iteration_<<")= "<<save_matlab_real_robot_person_distance2_vector_<<"\n \n";
+
+	////////// FIN nuevas variables //////
+
+	// INI VARIABLES ANTIGUAS: (que creo que no me he de quedar... A comprobar)
+		fileMatlab << "real_distance_person_robot("<<experiment_<<","<<iteration_<<")= "<<real_distance_person_robot_<<"\n"; // hay dos 1 por P1 y otra por P2.
+																															// creo que ya se guardan arriba, ver si las elimino.
+		fileMatlab << "ideal_distance_person_robot("<<experiment_<<","<<iteration_<<")= "<<ideal_distance_person_robot_<<"\n"; // solo hay 1. ver que se guarda bien
+		fileMatlab << "real_angle_person_robot("<<experiment_<<","<<iteration_<<")= "<<real_angle_person_robot_<<"\n"; // solo hay 1. ver que se guarda bien
+
+		fileMatlab << "before_bad_saved_ideal_angle_person_robot("<<experiment_<<","<<iteration_<<")= "<<ideal_angle_person_robot_<<"\n"; // en el caso de dos personas central, creo que no hay.
+		// To avoid jumps in ideal angle that are not true and good for the final results.
+
+		if(first_ideal_angle_){
+			before_ideal_angle_person_robot_=90;
+			first_ideal_angle_=false;
+		}
+
+		fileMatlab << "ideal_angle_person_robot("<<experiment_<<","<<iteration_<<")= "<<ideal_angle_person_robot_<<"\n";
+
+	// INI CALCULO PERFORMANCES
+
+
+	// INI CALCULO DISTANCE PERFORMANCES
+	double distance_performance_p1=0;
+
+	if((real_dist_r_p1<=2.0)&&(real_dist_r_p1>=1.25)){
+		distance_performance_p1=1;
+	}else if((real_dist_r_p1>2.0)&&(real_dist_r_p1<=3.0)){
+		distance_performance_p1=1-(real_dist_r_p1-2.0)/(3-2);
+	}else if((real_dist_r_p1<1.25)&&(real_dist_r_p1>=0.75)){
+		distance_performance_p1=(real_dist_r_p1)/(1.25-0.75)-1.60;
+	}
+
+
+	fileMatlab << "distance_performance_p1("<<experiment_<<","<<iteration_<<")= "<<distance_performance_p1<<"\n";
+
+	// INI CALCULO ANGLE PERFORMANCES
+	// real_angle angle_r_p1
+	double ideal_angle_p1=before_ideal_angle_person_robot_;
+
+	Sdestination dest_p1=pointer_to_person_companion_->get_best_dest();
+
+	double distance_p1_to_final_goal=pointer_to_person_companion_->get_current_pointV().distance(Spoint(dest_p1.x,dest_p1.y,dest_p1.time_stamp));
+
+	double distance_r_to_final_goal=robot_->get_current_pointV().distance(Spoint(dest_p1.x,dest_p1.y,dest_p1.time_stamp));
+	double distance_diference_p1_r=sqrt((distance_r_to_final_goal-distance_p1_to_final_goal)*(distance_r_to_final_goal-distance_p1_to_final_goal));
+
+	if(before_ideal_angle_person_robot_!=angle_companion_){ // caso hay colisiones. (para saber quien va delante o detras del robot.
+		if((distance_r_to_final_goal<distance_p1_to_final_goal)&&(distance_diference_p1_r>0.3)){ // ver que limite pongo...
+			// el robot va por delante de la persona. Cambian los angulos.
+			ideal_angle_p1=180-before_ideal_angle_person_robot_; // ESTAN guardados en grados. hay que pasar a radianes o no?
+		}
+	} //else, se quedan a 90 grados ambos!!!
+
+	// pasar ideal angle a coordenadas globales!. ideal_angle = ideal_angle_codigo + orientacion del grupo? creo que si.
+	double ideal_angle_p1_perf_angle=ideal_angle_p1;//+theta_p1*180/3.14;
+
+	if(diffangle(theta_p1,ideal_angle_p1_perf_angle)<0){
+		ideal_angle_p1_perf_angle=theta_p1*180/3.14+ideal_angle_p1_perf_angle;
+	}else{
+		ideal_angle_p1_perf_angle=theta_p1*180/3.14-ideal_angle_p1_perf_angle;
+	}
+
+	if(ideal_angle_p1_perf_angle>360){
+		ideal_angle_p1_perf_angle=ideal_angle_p1_perf_angle-360;
+	}else if(ideal_angle_p1_perf_angle<0){
+		ideal_angle_p1_perf_angle=360+ideal_angle_p1_perf_angle;
+	}
+
+	fileMatlab << "ideal_angle_p1("<<experiment_<<","<<iteration_<<")= "<<ideal_angle_p1<<"\n";
+
+	double angle_performance_p1;
+	double diference_p1=sqrt((ideal_angle_p1_perf_angle-angle_r_p1)*(ideal_angle_p1_perf_angle-angle_r_p1));
+
+	if((diference_p1>10)&&(diference_p1<=90)){
+		angle_performance_p1=(1-(diference_p1/80))+0.125;
+	}else if(diference_p1<=10){
+		angle_performance_p1=1;
+	}else{
+		angle_performance_p1=0;
+	}
+
+
+	fileMatlab << "angle_performance_p1("<<experiment_<<","<<iteration_<<")= "<<angle_performance_p1<<"\n";
+
+	SpointV_cov robot_point_act=robot_->get_current_pointV();
+
+	std::vector<Spoint> points_robot_mesh;
+	points_robot_mesh.clear();
+	double x_r_act=0.0,y_r_act=0.0;
+	unsigned int max_lim=11;
+	for(unsigned int iter_i=0; iter_i<max_lim; iter_i++){
+		x_r_act = robot_point_act.x + iter_i*0.1-0.5;//(iter_i-5)/10;
+
+		for(unsigned int iter_j=0; iter_j<max_lim;iter_j++){
+			y_r_act = robot_point_act.y + iter_j*0.1-0.5;//(iter_j-5)/10;
+			points_robot_mesh.push_back(Spoint(x_r_act,y_r_act,robot_point_act.time_stamp));
+		}
+	}
+
+	SpointV ideal_pose_robot_respect_p1=initial_person_companion_point_;
+	double distancia_entre_p1_r=robot_person_proximity_distance_;
+
+	if(diffangle(theta_p1,global_angle_r_p1*3.14/180)<0){ // hay que pasar angle r_p1 a radianes
+		ideal_pose_robot_respect_p1.x+=(distancia_entre_p1_r)*cos(theta_p1+(ideal_angle_p1*(3.14/180)));//1.5*cos(theta+(act_min_companion_angle*(3.14/180)));
+		ideal_pose_robot_respect_p1.y+=(distancia_entre_p1_r)*sin(theta_p1+(ideal_angle_p1*(3.14/180)));//1.5*sin(theta+(act_min_companion_angle*(3.14/180)));
+	}else{
+		ideal_pose_robot_respect_p1.x+=(distancia_entre_p1_r)*cos(theta_p1-(ideal_angle_p1*(3.14/180)));//1.5*cos(theta-(act_min_companion_angle*(3.14/180)));
+		ideal_pose_robot_respect_p1.y+=(distancia_entre_p1_r)*sin(theta_p1-(ideal_angle_p1*(3.14/180)));//1.5*sin(theta-(act_min_companion_angle*(3.14/180)));
+	}
+
+	// Performance area P1
+	double area_performance_p1=0;
+	SpointV P1_act_pose=pointer_to_person_companion_->get_current_pointV();
+	for(unsigned int i=0;i<points_robot_mesh.size();i++){
+		// paso 1, performance con la posicion del la persona actual.
+		double distance_r_p1_real=P1_act_pose.distance(points_robot_mesh[i]);
+
+		if((distance_r_p1_real<3)&&(distance_r_p1_real>0.5)){
+			area_performance_p1=area_performance_p1+0.5;
+		}
+		double distance_r_to_r_ideal_respect_p1=ideal_pose_robot_respect_p1.distance(points_robot_mesh[i]);
+		if(distance_r_to_r_ideal_respect_p1<1){
+			area_performance_p1=area_performance_p1+0.5;
+		}
+	}
+	area_performance_p1=area_performance_p1/points_robot_mesh.size();
+
+	double total_area_performance=area_performance_p1;
+
+	fileMatlab << "total_area_performance("<<experiment_<<","<<iteration_<<")= "<<total_area_performance<<"\n";
+	fileMatlab << "area_performance_p1("<<experiment_<<","<<iteration_<<")= "<<area_performance_p1<<"\n";
+
+	fileMatlab << "% robot_order =1 (central robot); =0 or 2 (lateral robot) \n \n";
+	// FIN CALCULO PERFORMANCES
+
+	// INI variables antiguasl que creo que me he de quedar ////
+	fileMatlab << "is_case_akp_true_("<<experiment_<<","<<iteration_<<")= "<<is_case_akp_true_<<"\n";
+	fileMatlab << "enter_on_cero_("<<experiment_<<","<<iteration_<<")= "<<enter_on_cero_<<"\n";
+	fileMatlab << "enter_on_cero2_("<<experiment_<<","<<iteration_<<")= "<<enter_on_cero2_<<"\n";
+	fileMatlab << "result_("<<experiment_<<","<<iteration_<<")= "<<result_<<"\n";
+	fileMatlab << "dist_to_goal_test_("<<experiment_<<","<<iteration_<<")= "<<dist_to_goal_test_<<"\n";
+	fileMatlab << "v_to_goal_test_("<<experiment_<<","<<iteration_<<")= "<<v_to_goal_test_<<"\n";
+	// FIN variables antiguasl que creo que me he de quedar ////
+
+    fileMatlab.close();
+
+    /* Diferentes modos para abrir el fichero!
+      	ios::in 		Abrir para entrada (lectura)
+		ios::out 		Abrir para salida (escritura)
+		ios::binary 	Abre en modo binario
+		ios::ate 		Escoge el final del fichero como posición inicial (si no se dice lo contrario, la posición inicial al abrir el fichero sería el comienzo del fichero)
+		ios::app 		Abrir para añadir (append) al final, sólo utilizable si se ha abierto el fichero exclusivamente para escritura
+		ios::trunc 		Trunca el fichero si existía, borrando to_do su contenido anterior
+     */
+
+	before_ideal_angle_person_robot_=ideal_angle_person_robot_;
+
+}
+
+
+void Cplan_local_nav::new_debug_file()
+{
+	std::ofstream fileMatlab2;
+	fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+	fileMatlab2 << "% ***** Starts Autogenerated DEBUG File - results AKP dynamic companion ELY ***** \n\n";
+	fileMatlab2.close();
+}
+
+
+
+void Cplan_local_nav::new_matlab_file()
+{
+	std::ofstream fileMatlab;
+	fileMatlab.open (results_file_.c_str(), std::ofstream::out | std::ofstream::app);
+    // FILE HEADER
+    fileMatlab << "% ***** Autogenerated File - results AKP dynamic companion ELY ***** \n\n";
+    fileMatlab << "%  The file contains: Actual robot and person distance, real angle, ideal angle at this time, time, time to arrive to goal (for person and robot). Costs totals of the winner path in each time. \n\n";
+    fileMatlab << "dt=[]\n";  // variables nuevas guardadas caso 2p 1robot.
+    fileMatlab << "real_computation_time_dt=[]\n";
+ 	fileMatlab << "companion_person_x=[]\n";
+	fileMatlab << "companion_person_y=[]\n";
+	fileMatlab << "companion_person_vx=[]\n";
+	fileMatlab << "companion_person_vy=[]\n";
+	fileMatlab << "companion_person_theta=[]\n";
+	fileMatlab << "companion_person_dest_x=[]\n";
+	fileMatlab << "companion_person_dest_y=[]\n";
+	fileMatlab << "companion2_person_x=[]\n";
+	fileMatlab << "companion2_person_y=[]\n";
+	fileMatlab << "companion2_person_vx=[]\n";
+	fileMatlab << "companion2_person_vy=[]\n";
+	fileMatlab << "companion2_person_theta=[]\n";
+	fileMatlab << "companion2_person_dest_x=[]\n";
+	fileMatlab << "companion2_person_dest_y=[]\n";
+	fileMatlab << "d_r_1p_x=[]\n";
+	fileMatlab << "d_r_1p_y=[]\n";
+	fileMatlab << "d_r_1p_m=[]\n";
+	fileMatlab << "d_r_1p_x=[]\n";
+	fileMatlab << "d_r_1p_y=[]\n";
+	fileMatlab << "d_r_2p_m=[]\n";
+	fileMatlab << "d_2p_1p_x=[]\n";
+	fileMatlab << "d_2p_1p_y=[]\n";
+	fileMatlab << "d_2p_1p_m=[]\n";
+
+	fileMatlab << "robot_x=[]\n";
+	fileMatlab << "robot_y=[]\n";
+ 	fileMatlab << "robot_vx=[]\n";
+	fileMatlab << "robot_vy=[]\n";
+	fileMatlab << "robot_theta=[]\n \n";
+
+    fileMatlab << "real_distance_person_robot=[];\n";
+    fileMatlab << "ideal_distance_person_robot=[];\n";
+    fileMatlab << "real_angle_person_robot=[];\n";
+    fileMatlab << "ideal_angle_person_robot=[];\n";
+    fileMatlab << "actual_time=[];\n";
+    fileMatlab << "robot_distance_cost=[];\n";
+    fileMatlab << "robot_orientation_cost=[];\n";
+    fileMatlab << "robot_control_cost=[];\n";
+    fileMatlab << "robot_other_person_cost=[];\n";
+    fileMatlab << "robot_obstacles_cost=[];\n";
+    fileMatlab << "robot_companion_cost=[];\n";
+    fileMatlab << "robot_total_cost=[]; \n";
+    fileMatlab << "robot_ant_traj_cost=[];\n";
+    fileMatlab << "cost_parameters_0=[];\n";
+    fileMatlab << "cost_parameters_1=[];\n";
+    fileMatlab << "cost_parameters_2=[];\n";
+    fileMatlab << "cost_parameters_3=[];\n";
+    fileMatlab << "cost_parameters_5=[];\n";
+    fileMatlab << "cost_parameters_6=[];\n";
+    fileMatlab << "person_distance_to_goal=[];\n";
+    fileMatlab << "robot_distance_to_goal=[];\n";
+    fileMatlab << "percent_of_error_in_distance=[];\n";
+    fileMatlab << "performance=[];\n";
+
+    fileMatlab << "last_step_robot_other_person_cost_=[];\n";
+    fileMatlab << "last_step_robot_companion_cost_=[];\n";
+    fileMatlab << "last_step_robot_obstacles_cost_=[];\n";
+    fileMatlab << "last_step_robot_control_cost_mix_=[];\n";
+    fileMatlab << "last_step_robot_control_cost_goal_traj_=[];\n";
+    fileMatlab << "last_step_robot_control_cost_goal_person_=[];\n";
+    fileMatlab << "last_step_robot_orientation_cost_local_=[];\n";
+    fileMatlab << "last_step_robot_orientation_cost_global_=[];\n";
+    fileMatlab << "last_step_robot_distance_cost_=[];\n";
+
+    fileMatlab << "other_people_due_to_robot_cost_=[];\n";
+    fileMatlab << "companion_person_due_to_robot_cost_=[];\n";
+
+    fileMatlab << "person_goal_x=[];\n";
+    fileMatlab << "person_goal_y=[];\n";
+    fileMatlab << "person_goal_vx=[];\n";
+    fileMatlab << "person_goal_vy=[];\n";
+    fileMatlab << "person_goal_theta1=[];\n";
+    fileMatlab << "person_goal_theta2=[];\n";
+    fileMatlab << "distance_robot_person_goal=[];\n";
+    fileMatlab << "distance_person_companion_person_goal=[];\n";
+    fileMatlab << "distance_centre_group_person_goal=[];\n";
+    fileMatlab << "theta_group_centro_robot_persona=[];\n";
+    fileMatlab << "person_goal_theta1=[];\n";
+    fileMatlab << "theta_group_comp_op=[];\n";
+    fileMatlab << "center_of_the_group_theta=[];\n";
+    fileMatlab << "person_c_robot_orientation_by_line=[];\n";
+    fileMatlab << "theta_group_comp_op2=[];\n";
+    fileMatlab << "angle_diference_person_goal_group=[];\n";
+    fileMatlab << "angle_diference_person_goal_group2=[];\n";
+    fileMatlab << "Action_=[];\n";
+
+    fileMatlab << "distance_between_consecutive_goals=[];\n";
+    fileMatlab << "computational_time=[];\n";
+    fileMatlab << "robot_plan_companion2_start_ITER=[];\n";
+    fileMatlab << "robot_plan_companion2_end_ITER=[];\n";
+
+    fileMatlab << "center_of_the_group_x=[];\n";
+    fileMatlab << "center_of_the_group_y=[];\n";
+    fileMatlab << "center_of_the_group_theta=[];\n";
+
+    fileMatlab << "value_distance_global_path_=[];\n";
+    fileMatlab << "global_path_ini_orientation_=[];\n";
+    fileMatlab << "global_path_final_orientation_=[];\n";
+
+
+    fileMatlab.close();
+
+}
+
+
+void Cplan_local_nav::new_matlab_file_To_evaluate_costs()
+{
+
+	std::ofstream fileMatlab;
+	fileMatlab.open (evaluate_costs_file_.c_str(), std::ofstream::out | std::ofstream::app);
+    // FILE HEADER
+    fileMatlab << "% ***** Autogenerated File - evaluate costs AKP dynamic companion ELY ***** \n\n";
+    fileMatlab << "%  The file contains: the best path and nondominated paths positions, the id's of the nodes of the paths, the total costs and partially costs of each node,  means and std of the costs, the itraration \n\n\n";
+    fileMatlab << "time=0;\n";
+    fileMatlab << "iter=0;\n";
+    fileMatlab << "num_of_total_paths=0;\n";
+    fileMatlab << "mean=[];\n";
+    fileMatlab << "std=[];\n";
+    fileMatlab << " % nondominated paths: \n";
+    fileMatlab << "end_id_nondominated_paths=0;\n";
+    fileMatlab << "path_pose=[];\n";
+    fileMatlab << "id_node=[];\n";
+    fileMatlab << "costs=[];\n";
+    fileMatlab << " % best path: \n";
+    fileMatlab << "end_id_best_path=0;\n";
+    fileMatlab << "best_path_poses=[];\n";
+    fileMatlab << "best_path_id_nodes=[];\n";
+    fileMatlab << "best_paths_costs=[];\n";
+
+    fileMatlab.close();
+
+}
+
+
+
+void Cplan_local_nav::new_matlab_file_To_evaluate_change_distance_and_angle()
+{
+	std::ofstream fileMatlab;
+	fileMatlab.open (evaluate_change_distance_and_angle_companion_file_.c_str(), std::ofstream::out | std::ofstream::app);
+	// FILE HEADER
+	fileMatlab << "% ***** Autogenerated File - evaluate costs AKP dynamic companion ELY ***** \n\n";
+	fileMatlab << "%  The file contains: the best path and nondominated paths positions, the id's of the nodes of the paths, the total costs and partially costs of each node,  means and std of the costs, the itraration \n\n";
+	fileMatlab << " iter=[];\n";
+	fileMatlab << " distance_robot_person=[];\n";
+	fileMatlab << " angle_companion_robot_person=[];\n";
+}
+
+void Cplan_local_nav::evaluate_change_distance_and_angle_companion_with_beta_change()
+{
+	std::ofstream fileMatlab;
+	fileMatlab.open (evaluate_change_distance_and_angle_companion_file_.c_str(), std::ofstream::out | std::ofstream::app);
+	double iter=iter_act_multiple_paths_and_best_path_.iter_act;
+
+    // Algorithm Params
+	fileMatlab << " \n \n \n % New iteration! \n";
+
+	if(save_distance_between_person_and_robot_<(robot_person_proximity_distance_+2*robot_->get_platform_radii())){
+		iter_d_++;
+		fileMatlab << "\n  iter=[iter,"<<iter_d_<<"];\n";
+		fileMatlab << "\n  distance_robot_person=[distance_robot_person,"<<save_distance_between_person_and_robot_<<"];\n";
+		fileMatlab << "\n  angle_companion_robot_person=[angle_companion_robot_person,"<<save_angle_between_person_and_robot_<<"];\n";
+		fileMatlab << "\n  robot_goal_to_person_companion_x=[robot_goal_to_person_companion_x,"<<robot_goal_to_person_companion_.x<<"];\n";
+		fileMatlab << "\n  robot_goal_to_person_companion_y=[robot_goal_to_person_companion_y,"<<robot_goal_to_person_companion_.y<<"];\n";
+		fileMatlab << "\n  robot_path_goal_x=[robot_path_goal_x,"<<robot_path_goal_.x<<"];\n";
+		fileMatlab << "\n  robot_path_goal_y=[robot_path_goal_y,"<<robot_path_goal_.y<<"];\n";
+		fileMatlab << "\n  final_combined_goal_x=[final_combined_goal_x,"<<final_combined_goal_.x<<"];\n";
+		fileMatlab << "\n  final_combined_goal_y=[final_combined_goal_y,"<<final_combined_goal_.y<<"];\n";
+
+
+		fileMatlab << "\n  person_position_t_4_x=[person_position_t_4_x,"<<person_position_t_4_.x<<"];\n";
+		fileMatlab << "\n  person_position_t_4_y=[person_position_t_4_y,"<<person_position_t_4_.y<<"];\n";
+
+		Spoint goal_person_comp=Spoint(robot_goal_to_person_companion_.x,robot_goal_to_person_companion_.y,robot_goal_to_person_companion_.time_stamp);
+
+		double dist=person_position_t_4_.distance(goal_person_comp);
+		fileMatlab << "\n  distance_between_goal_person_comp_and_person_t4=[distance_between_goal_person_comp_and_person_t4,"<<dist<<"];\n";
+		double angle=calculate_actual_angle_person_robot(person_position_t_4_, goal_person_comp);
+		fileMatlab << "\n  angle_between_goal_person_comp_and_person_t4=[angle_between_goal_person_comp_and_person_t4,"<<angle<<"];\n";
+
+		Spoint goal_path=Spoint(robot_path_goal_.x,robot_path_goal_.y,robot_path_goal_.time_stamp);
+		dist=person_position_t_4_.distance(goal_path);
+		fileMatlab << "\n  distance_between_goal_path_and_person_t4=[distance_between_goal_path_and_person_t4,"<<dist<<"];\n";
+		angle=calculate_actual_angle_person_robot(person_position_t_4_, goal_path);
+		fileMatlab << "\n  angle_between_goal_path_and_person_t4=[angle_between_goal_path_and_person_t4,"<<angle<<"];\n";
+
+		Spoint final_combined_goal=Spoint(final_combined_goal_.x,final_combined_goal_.y,final_combined_goal_.time_stamp);
+		dist=person_position_t_4_.distance(final_combined_goal);
+		fileMatlab << "\n  distance_between_final_combined_goal_and_person_t4=[distance_between_final_combined_goal_and_person_t4,"<<dist<<"];\n";
+		angle=calculate_actual_angle_person_robot(person_position_t_4_, final_combined_goal);
+		fileMatlab << "\n  angle_between_final_combined_goal_and_person_t4=[angle_between_final_combined_goal_and_person_t4,"<<angle<<"];\n";
+
+
+		fileMatlab << "\n  next_angle_companion=[next_angle_companion,"<<next_companion_angle_save_<<"];\n";
+		fileMatlab << "\n  actual_angle_companion=[actual_angle_companion,"<<before_act_companion_angle_<<"];\n";
+
+
+		SpointV_cov robot_position=robot_->get_current_pointV();
+		fileMatlab << " \n%  obstacles: \n";
+		fileMatlab << "\n  obstacles_x=[];\n";
+		fileMatlab << "\n  obstacles_y=[];\n";
+		double d_ini=0;
+
+		for( auto iit: laser_obstacle_list_ )
+		{
+		    d_ini = iit.distance( robot_position );
+
+		    if( d_ini < 5  )
+		    {
+		    	//number_of_obstacles++;
+		    	if(debug_file_evaluate_costs_){
+		    		std::cout <<" obstacle["<<iter<<"]=> x="<<iit.x<<"; y="<<iit.y<< std::endl;
+		    	}
+		    	fileMatlab << "\n  obstacles_x=[obstacles_x,"<<iit.x<<"];\n";
+		    	fileMatlab << "\n  obstacles_y=[obstacles_y,"<<iit.y<<"];\n";
+		    }
+		}
+	}
+}
+
+
+
+void Cplan_local_nav::evaluate_costs_printToMatlab(Crobot* actual_robot)
+{
+	std::ofstream fileMatlab;
+	fileMatlab.open (evaluate_costs_file_.c_str(), std::ofstream::out | std::ofstream::app);
+	double iter=iter_act_multiple_paths_and_best_path_.iter_act;
+
+	SpointV_cov robot_position=actual_robot->get_current_pointV();
+    // Algorithm Params
+	fileMatlab << " \n \n \n % New iteration! \n";
+
+	if(debug_file_evaluate_costs_){
+		std::cout <<" \n \n \n % New iteration!  "<< std::endl;
+	}
+
+	fileMatlab << "\n  iter_act="<<iter<<";\n";
+
+	fileMatlab << " \n % Guardamos el local_goal_actual \n";
+	fileMatlab << "\n  local_goal_x="<<local_goal_.x<<";\n";
+	fileMatlab << "\n  local_goal_y="<<local_goal_.y<<";\n";
+
+
+	fileMatlab << " \n%  robot_point: \n";
+	fileMatlab << "\n  robot_position_x="<<robot_position.x<<";\n";
+	fileMatlab << "\n  robot_position_y="<<robot_position.y<<";\n";
+	fileMatlab << "\n  robot_position_vx="<<robot_position.vx<<";\n";
+	fileMatlab << "\n  robot_position_vy="<<robot_position.vy<<";\n";
+	fileMatlab << "\n  robot_position_v="<<robot_initial_pose_.v<<";\n";
+	fileMatlab << "\n  robot_position_w="<<robot_initial_pose_.w<<";\n";
+	fileMatlab << "\n  robot_position_theta="<<robot_initial_pose_.theta<<";\n";
+
+	fileMatlab << " \n%  person position: \n";
+	fileMatlab << "\n  person_position_x="<<companion_person_position_.x<<";\n";
+	fileMatlab << "\n  person_position_y="<<companion_person_position_.y<<";\n";
+	fileMatlab << "\n  person_position_vx="<<companion_person_position_.vx<<";\n";
+	fileMatlab << "\n  person_position_vy="<<companion_person_position_.vy<<";\n";
+	fileMatlab << "\n  person_position_theta="<<orientacion_persona_actual_<<";\n";
+	fileMatlab << "\n  person_position_x_ant_to_calc_theta="<<person_x_ant_<<";\n";
+	fileMatlab << "\n  person_position_y_ant_to_calc_theta="<<person_y_ant_<<";\n";
+
+	fileMatlab << " \n%  obstacles: \n";
+	fileMatlab << "\n  obstacles_x=[];\n";
+	fileMatlab << "\n  obstacles_y=[];\n";
+	double d_ini=0;
+
+	    for( auto iit: laser_obstacle_list_ )
+	    {
+	    	d_ini = iit.distance( robot_position );
+
+	    	if( d_ini < 5  )
+	    	{
+	    		fileMatlab << "\n  obstacles_x=[obstacles_x,"<<iit.x<<"];\n";
+	    		fileMatlab << "\n  obstacles_y=[obstacles_y,"<<iit.y<<"];\n";
+	    	}
+	    }
+
+
+	std::cout <<" \n  "<< std::endl;
+	fileMatlab << "\n time=[];\n";
+	fileMatlab << "\n iter=[];\n";
+	fileMatlab << "\n num_of_total_paths=[];\n";
+
+	fileMatlab << "\n time=[time,"<<iter_act_multiple_paths_and_best_path_.time_act<<"];\n";
+
+	fileMatlab << " iter=[iter,"<<iter_act_multiple_paths_and_best_path_.iter_act<<"];\n";
+
+	fileMatlab << " num_of_total_paths=[num_of_total_paths,"<<iter_act_multiple_paths_and_best_path_.number_of_total_paths<<"];\n";
+
+
+	fileMatlab << " mean=[];\n";
+	fileMatlab << " stds=[];\n";
+	fileMatlab << " \n % means of costs: mean_distance_[0]; mean_orientation_[1]; mean_robot_[2]; mean_int_forces_[3]; mean_obstacles_[4]; mean_companion_[5]; ant_traj[6]; \n";
+
+	for(unsigned int i=0; i<iter_act_multiple_paths_and_best_path_.means.size(); i++){
+		fileMatlab << " mean=[mean,"<<iter_act_multiple_paths_and_best_path_.means[i]<<"];\n";
+
+	}
+
+	fileMatlab << " \n % stds of costs: \n";
+	if(debug_file_evaluate_costs_){std::cout << " \n % stds of costs: \n"<<std::endl;}
+	for(unsigned int i=0; i<iter_act_multiple_paths_and_best_path_.stds.size(); i++){
+		fileMatlab << " stds=[stds,"<<iter_act_multiple_paths_and_best_path_.stds[i]<<"];\n";
+
+	}
+
+	fileMatlab << " \n \n % nondominated paths: \n";
+
+	std::vector<SsavePath_cost_and_values> act_nondominated_paths=iter_act_multiple_paths_and_best_path_.get_nondominated_paths();
+
+	fileMatlab << "end_id_nondominated_paths=[];\n";
+
+	for(unsigned int l=0; l<iter_act_multiple_paths_and_best_path_.nondominated_paths.size(); l++){
+		fileMatlab << "end_id_nondominated_paths=[end_id_nondominated_paths,"<<iter_act_multiple_paths_and_best_path_.nondominated_paths[l].end_id_path<<"];\n";
+
+		fileMatlab << "id_node_l_"<<l<<"=[];\n";
+		fileMatlab << "path_pose_l_"<<l<<"_x=[];\n";
+		fileMatlab << "path_pose_l_"<<l<<"_y=[];\n";
+		fileMatlab << "costs_l_"<<l<<"=[];\n";
+
+		for(unsigned int j=0; j<iter_act_multiple_paths_and_best_path_.nondominated_paths[l].all_ids_path_positions.size(); j++){
+			fileMatlab << "id_node_l_"<<l<<"=[id_node_l_"<<l<<","<<iter_act_multiple_paths_and_best_path_.nondominated_paths[l].all_ids_path_positions[j]<<"];\n";
+
+			fileMatlab << "path_pose_l_"<<l<<"_x=[path_pose_l_"<<l<<"_x,"<<iter_act_multiple_paths_and_best_path_.nondominated_paths[l].path_positions[j].x<<"];\n";
+			fileMatlab << "path_pose_l_"<<l<<"_y=[path_pose_l_"<<l<<"_y,"<<iter_act_multiple_paths_and_best_path_.nondominated_paths[l].path_positions[j].y<<"];\n";
+
+			fileMatlab << "% costs: distance_cost[0]; orientation_cost[1]; contro_cost[2]; other_persons_cost[3]; obstacles_cost[4]; companion_cost[5] total_cost[6] ant_traj_cost[7] \n";
+
+			fileMatlab << "costs_act=[];\n";
+			for(unsigned int k=0; k<iter_act_multiple_paths_and_best_path_.nondominated_paths[l].total_costs_of_each_path_position[j].size(); k++){
+				//std::cout << "k="<<k<<std::endl;
+				fileMatlab << "costs_act=[costs_act,"<<iter_act_multiple_paths_and_best_path_.nondominated_paths[l].total_costs_of_each_path_position[j][k]<<"];\n";
+
+			}
+			fileMatlab << "costs_l_"<<l<<"=[costs_l_"<<l<<";costs_act];\n";
+			fileMatlab << "\n";
+		}
+		fileMatlab << "\n \n";
+	}
+
+
+	fileMatlab << " \n % best path: \n";
+	fileMatlab << "end_id_best_path="<<iter_act_multiple_paths_and_best_path_.best_path.end_id_path<<";\n";
+
+	fileMatlab << "best_path_id_nodes=[];\n";
+	fileMatlab << "best_path_pose_x=[];\n";
+	fileMatlab << "best_path_pose_y=[];\n";
+
+	fileMatlab << "best_path_id_nodes=[];\n";
+	fileMatlab << "best_path_pose_x=[];\n";
+	fileMatlab << "best_path_pose_y=[];\n";
+	fileMatlab << "best_costs=[];\n";
+
+
+	for(unsigned int j=0; j<iter_act_multiple_paths_and_best_path_.best_path.all_ids_path_positions.size(); j++){
+		fileMatlab << "best_path_id_nodes=[best_path_id_nodes,"<<iter_act_multiple_paths_and_best_path_.best_path.all_ids_path_positions[j]<<"];\n";
+
+		fileMatlab << "best_path_pose_x=[best_path_pose_x,"<<iter_act_multiple_paths_and_best_path_.best_path.path_positions[j].x<<"];\n";
+		fileMatlab << "best_path_pose_y=[best_path_pose_y,"<<iter_act_multiple_paths_and_best_path_.best_path.path_positions[j].y<<"];\n";
+
+		fileMatlab << "% costs: distance_cost[0]; orientation_cost[1]; contro_cost[2]; other_persons_cost[3]; obstacles_cost[4]; companion_cost[5] total_cost[6] ant_traj_cost[7] \n";
+
+		fileMatlab << "costs_act=[];\n";
+		for(unsigned int k=0; k<iter_act_multiple_paths_and_best_path_.best_path.total_costs_of_each_path_position[j].size(); k++){
+			fileMatlab << "costs_act=[costs_act,"<<iter_act_multiple_paths_and_best_path_.best_path.total_costs_of_each_path_position[j][k]<<"];\n";
+
+		}
+		fileMatlab << "best_costs=[best_costs;costs_act];\n";
+
+	}
+    fileMatlab.close();
+
+    /* Diferentes modos para abrir el fichero!
+      	ios::in 		Abrir para entrada (lectura)
+		ios::out 		Abrir para salida (escritura)
+		ios::binary 	Abre en modo binario
+		ios::ate 		Escoge el final del fichero como posición inicial (si no se dice lo contrario, la posición inicial al abrir el fichero sería el comienzo del fichero)
+		ios::app 		Abrir para añadir (append) al final, sólo utilizable si se ha abierto el fichero exclusivamente para escritura
+		ios::trunc 		Trunca el fichero si existía, borrando to_do su contenido anterior
+     */
+}
+
+
+
+double Cplan_local_nav::calc_person_companion_orientation(){ // return theta person companion in radians!
+	// ini calcular nuevo angulo theta con orientación dirección movimiento persona.
+	//std::cout <<" IN: calc_person_companion_orientation"<< std::endl;
+
+    Cperson_abstract* person_obj=pointer_to_person_companion_;
+    Cperson_abstract* final_person_obj;
+    double person_vel;
+    Sdestination person_best_dest;
+	SpointV_cov person;
+
+	//Spoint robot_point=robot_->get_current_pointV();
+	person= person_obj->get_current_pointV();
+	person_best_dest=person_obj->get_best_dest();
+	person_vel=person_obj->get_current_pointV().v();
+	final_person_obj=person_obj;
+
+	double x;
+	double y;
+	double pers_dx;
+	double pers_dy;
+	double theta=0;
+	bool use_before_theta=false;
+
+
+	if(person_vel<threshold_min_vel_person_to_obtain_destination_){ //actual_current_person_comp_point.v()<threshold_min_vel_person_to_obtain_destination_
+		theta=last_good_theta_person_;
+		use_before_theta=true;
+	}else{
+		if(debug_gazebo_journal2_){
+			std::cout <<"(in else) threshold_min_vel_person_to_obtain_destination_="<<threshold_min_vel_person_to_obtain_destination_<< std::endl;
+		}
+
+		if((final_person_obj->get_best_dest().x==person.x)&&(final_person_obj->get_best_dest().y==person.y)){
+			if(final_person_obj->get_past_trajectory()->size()>num_steps_orientation_){
+				if(debug_correct_angle_person_init_robot2_){std::cout <<" perso_print_back_traj(size-num_steps_orientation_)="<< std::endl;
+				final_person_obj->get_past_trajectory()->at(final_person_obj->get_past_trajectory()->size()-num_steps_orientation_).print();}
+				x=final_person_obj->get_past_trajectory()->at(final_person_obj->get_past_trajectory()->size()-num_steps_orientation_).x;
+				y=final_person_obj->get_past_trajectory()->at(final_person_obj->get_past_trajectory()->size()-num_steps_orientation_).y;
+			}else{
+				if(debug_correct_angle_person_init_robot2_){std::cout <<" perso_print_back_traj(0)="<< std::endl;
+				final_person_obj->get_past_trajectory()->at(0).print();}
+				x=final_person_obj->get_past_trajectory()->at(0).x;
+				y=final_person_obj->get_past_trajectory()->at(0).y;
+			}
+
+		}else{
+			double m;
+				m=mean_people_Zalungo_.vy/mean_people_Zalungo_.vx;
+				//std::cout << " m="<<m<<"; person_companion_->get_current_pointV().vy="<<person_companion_->get_current_pointV().vy<<"; person_companion_->get_current_pointV().vx="<<person_companion_->get_current_pointV().vx << std::endl;
+
+				if(debug_sideBySide2019_){
+					std::cout << " theta_of_the_group_Zanlungo_="<<theta_of_the_group_Zanlungo_*180/3.14<<"; m="<<m<<"; mean_people_Zalungo_.vx="<<mean_people_Zalungo_.vx<<"; mean_people_Zalungo_.vy="<<mean_people_Zalungo_.vy<< std::endl;
+				}
+
+				//double x_center_group_people=(initial_person_companion_point_.x);
+				//double y_center_group_people=(initial_person_companion_point_.y);
+				double mp=-1/m;
+				double b=final_person_obj->get_current_pointV().y-m*final_person_obj->get_current_pointV().x;
+				double bp=final_person_obj->get_best_dest().y-mp*final_person_obj->get_best_dest().x;
+
+				double x_d=(bp-b)/(m-mp);
+				double y_d=m*x_d+b;
+
+				Spoint new_goal_robot;
+
+				new_goal_robot.x=x_d;
+				new_goal_robot.y=y_d;
+
+				x=new_goal_robot.x;
+				y=new_goal_robot.y;
+
+				Spoint peroson_goal_act=Spoint(pointer_to_person_companion_->get_best_dest().x,pointer_to_person_companion_->get_best_dest().y,pointer_to_person_companion_->get_best_dest().time_stamp);
+
+				if(peroson_goal_act.distance(new_goal_robot)<metros_al_dynamic_goal_Vform_){ // distancia menor que 5m del goal real.
+					x=new_goal_robot.x;
+					y=new_goal_robot.y;
+				}else{
+					x=peroson_goal_act.x;
+					y=peroson_goal_act.y;
+				}
+		}
+
+
+		if((final_person_obj->get_best_dest().x==person.x)&&(final_person_obj->get_best_dest().y==person.y)){
+			pers_dx=person.x-x;
+			pers_dy=person.y-y;
+		}else{
+			pers_dx=x-person.x;
+			pers_dy=y-person.y;
+		}
+
+
+		ori_pers_x_=pers_dx;
+		ori_pers_y_=pers_dy;
+
+		theta=atan2(pers_dy , pers_dx);
+		last_good_theta_person_=theta;
+		before_person_comp_goal_=final_person_obj->get_best_dest();
+
+		if(use_before_theta){
+			theta=last_good_theta_person_;
+		}
+	}
+
+	if(theta<0){
+		theta=2*3.14+theta;
+	}
+	return theta;
+}
+
+
+
+
+
+double Cplan_local_nav::calc_person_companion_orientation_from_outside( SpointV actual_p1_point, Sdestination person_dest, Spose in_robot){ // return theta person companion in radians!
+	//std::cout <<"IN: calc_person_companion_orientation_from_outside"<< std::endl;
+	// ini calcular nuevo angulo theta con orientación dirección movimiento persona.
+
+    double person_vel;
+    Sdestination person_best_dest;
+	SpointV_cov person;
+
+	//Spoint robot_point=Spoint(in_robot.x,in_robot.y,in_robot.time_stamp);
+	//double dist_to_pers1= actual_p1_point.distance(robot_point);
+
+	person=actual_p1_point;
+	person_best_dest=person_dest;
+	person_vel=actual_p1_point.v();
+
+	double x;
+	double y;
+	double pers_dx;
+	double pers_dy;
+	double theta=0;
+
+	if(person_vel<threshold_min_vel_person_to_obtain_destination_){ //actual_current_person_comp_point.v()<threshold_min_vel_person_to_obtain_destination_
+		theta=last_good_theta_person_;
+	}else{
+
+		x=person_dest.x;
+		y=person_dest.y;
+		pers_dx=x-person.x;
+		pers_dy=y-person.y;
+
+		ori_pers_x_=pers_dx;
+		ori_pers_y_=pers_dy;
+
+		theta=atan2(pers_dy , pers_dx);
+		last_good_theta_person_=theta;
+
+	}
+
+	if(theta<0){
+		theta=2*3.14+theta;
+	}
+
+	return theta;
+
+}
+
+
+Sdestination Cplan_local_nav::calculate_companion_goal_stable_if_person_stop(double in_act_min_companion_angle){
+//std::cout << " IDEAL FINAL ACT in_act_min_companion_angle="<<in_act_min_companion_angle<< std::endl;
+
+	double theta=calc_person_companion_orientation();
+	double act_min_companion_angle=in_act_min_companion_angle;
+	//Cperson_abstract* person_obj1;
+	//find_person(id_person_companion_ , &person_obj1);
+	SpointV_cov person = actual_person_Companion_pointer_->get_current_pointV();
+	Sdestination person_companion_goal=actual_person_Companion_pointer_->get_best_dest();
+	Spoint robot=Spoint(initial_robot_spoint_.x,initial_robot_spoint_.y,initial_robot_spoint_.time_stamp);
+	double angle = atan2(robot.y-person.y,robot.x-person.x);
+
+	if(diffangle(theta,angle)<0){
+		person_companion_goal.x+=(robot_person_proximity_distance_)*cos(theta+(act_min_companion_angle*(3.14/180)));
+		person_companion_goal.y+=(robot_person_proximity_distance_)*sin(theta+(act_min_companion_angle*(3.14/180)));
+	}else{
+		person_companion_goal.x+=(robot_person_proximity_distance_)*cos(theta-(act_min_companion_angle*(3.14/180)));
+		person_companion_goal.y+=(robot_person_proximity_distance_)*sin(theta-(act_min_companion_angle*(3.14/180)));
+	}
+
+	// INI calcular goal a 90 grados derecha e izquierda, para ver hacia que lado es mejor ir.
+	if(chose_better_side_to_acompani_person_){
+		Sdestination person_companion_goal_positive=Sdestination(0,person.x,person.y,1.0);
+		Sdestination person_companion_goal_negative=Sdestination(0,person.x,person.y,1.0);
+
+		person_companion_goal_positive.x+=(robot_person_proximity_distance_)*cos(theta+(90*(3.14/180)));
+		person_companion_goal_positive.y+=(robot_person_proximity_distance_)*sin(theta+(90*(3.14/180)));
+
+		person_companion_goal_negative.x+=(robot_person_proximity_distance_)*cos(theta-(90*(3.14/180)));
+		person_companion_goal_negative.y+=(robot_person_proximity_distance_)*sin(theta-(90*(3.14/180)));
+
+		// distancia a esa posicion respecto a la del robot actual mallor que cierto margen.
+		double distance_to_side_person_positive=initial_robot_spoint_.distance(Spoint(person_companion_goal_positive.x,person_companion_goal_positive.y,person_companion_goal_positive.time_stamp));
+		double distance_to_side_person_negative=initial_robot_spoint_.distance(Spoint(person_companion_goal_negative.x,person_companion_goal_negative.y,person_companion_goal_negative.time_stamp));
+
+		bool select_side_of_the_person_to_go=false;
+		if((distance_to_side_person_positive>threshold_dintace_select_person_side_to_go_)||(distance_to_side_person_negative>threshold_dintace_select_person_side_to_go_)){
+			select_side_of_the_person_to_go=true;
+		}
+
+		// falta calcular colisiones.
+		if(select_side_of_the_person_to_go){
+			Spoint Spoint_pose_command=Spoint(person_companion_goal_positive.x,person_companion_goal_positive.y);
+			double min_dist_colli_p=check_collision_companion_goal(Spoint_pose_command,0);
+			Spoint_pose_command=Spoint(person_companion_goal_negative.x,person_companion_goal_negative.y);
+			double min_dist_colli_n=check_collision_companion_goal(Spoint_pose_command,0); // ya te da la distancia minima
+
+			unsigned int case_act=0;
+			if((min_dist_colli_p<robot_person_companion_distance_)&&(min_dist_colli_n==robot_person_companion_distance_)){// hay distancia de colision solo en el caso positivo
+				case_act=1;
+			}else if((min_dist_colli_n<robot_person_companion_distance_)&&(min_dist_colli_p==robot_person_companion_distance_)){ // hay distancia de colision solo en el caso negativo
+				case_act=2;
+			}else if((min_dist_colli_p!=robot_person_companion_distance_)&&(min_dist_colli_n!=robot_person_companion_distance_)){ // hay distancia de colision en ambos casos
+				case_act=3;
+			}
+
+			bool reclaculate_goal=false;
+			if((min_dist_colli_n<robot_person_companion_distance_)||(min_dist_colli_p<robot_person_companion_distance_)){
+				reclaculate_goal=true;
+			}
+
+			//Cperson_abstract* person_obj;
+			//find_person(id_person_companion_ , &person_obj);
+			person = actual_person_Companion_pointer_->get_current_pointV();
+
+			Sdestination person_companion_position;
+			person_companion_position = Sdestination(0, person.x ,person.y,1.0);
+
+			bool robot_goal_positive_bool; // if true is goal positive (+) if false es negative (-)
+
+			switch( case_act)
+			{
+				case 1: //case best the robot_goal_negativo_
+					person_companion_position=person_companion_goal_negative;//.x += (robot_person_proximity_distance_)*cos(theta + angle_companion_*3.14/180);
+					robot_goal_positive_bool=false;
+					break;
+				case 2: //case best the robot_goal_positivo_
+					person_companion_position=person_companion_goal_positive;//.x += (robot_person_proximity_distance_)*cos(theta - (angle_companion_*3.14/180));
+					robot_goal_positive_bool=true;
+					break;
+				case 3:
+					if(min_dist_colli_n>min_dist_colli_p){ //case best the robot_goal_negativo_
+						person_companion_position=person_companion_goal_negative;
+						robot_goal_positive_bool=false;
+					}else{ //case best the robot_goal_positivo_
+						robot_goal_positive_bool=true;
+						person_companion_position=person_companion_goal_positive;
+					}
+
+					break;
+				default:
+				case 0:
+					// darle el goal al lado de la persona.
+					if( diffangle(theta, angle) < 0 )
+					{
+						person_companion_position=person_companion_goal_positive;
+						robot_goal_positive_bool=true;
+					 }
+					 else  // es como si hiciera una circunferencia al rededor de la persona. (se intenta poner a un lado o a otro de la persona. segun esos angulos)
+					 {
+						person_companion_position=person_companion_goal_negative;
+						robot_goal_positive_bool=false;
+					  }
+					break;
+				}
+			// cambiar person companion goal por el positivo o negativo respecto al actual act_min_companion_angle que sacamos!
+			if(reclaculate_goal){
+				person_companion_goal=Sdestination(0,person.x,person.y,1.0);
+				if(robot_goal_positive_bool){
+					person_companion_goal.x+=(robot_person_proximity_distance_)*cos(theta+(act_min_companion_angle*(3.14/180)));
+					person_companion_goal.y+=(robot_person_proximity_distance_)*sin(theta+(act_min_companion_angle*(3.14/180)));
+				}else{
+					person_companion_goal.x+=(robot_person_proximity_distance_)*cos(theta-(act_min_companion_angle*(3.14/180)));
+					person_companion_goal.y+=(robot_person_proximity_distance_)*sin(theta-(act_min_companion_angle*(3.14/180)));
+				}
+			}
+		}
+	}
+	// FIN calcular goal a 90 grados derecha e izquierda, para ver hacia que lado es mejor ir.
+
+	if(debug_file_robot_){
+		std::ofstream fileMatlab2;
+		fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+		fileMatlab2 << "% ANTES f_person_goal=robot_->force_goal_near\n";
+		fileMatlab2.close();
+	}
+
+//	std::cout << " IDEAL FINAL ACT person_companion_goal.x="<<person_companion_goal.x<<" person_companion_goal.y="<<person_companion_goal.y<< std::endl;
+
+	return person_companion_goal;
+}
+
+
+void Cplan_local_nav::fix_angles_to_use_person_prediction_for_the_companion_goal(){
+// start function fix angles for prediction (vuelta hacia delante)
+
+	unsigned int act_index1=0;//BEST_path_parent_index_vector_[BEST_path_parent_index_vector_.size()-1];
+	unsigned int next_index1=1;//BEST_path_parent_index_vector_[BEST_path_parent_index_vector_.size()-2];
+	double act_angle1=orientation_person_robot_angles_with_prediction_of_person_companion_[act_index1];
+	double next_angle1=orientation_person_robot_angles_with_prediction_of_person_companion_[next_index1];
+	double diff_angle1=act_angle1-next_angle1;
+	if(diff_angle1>angle_increment_of_increment_distance_){
+
+	}
+
+	// 2- vuelta del principio al final
+	/*  caso_vuelta_al_reves!!!!   */
+	for(unsigned int g=0;g<(orientation_person_robot_angles_with_prediction_of_person_companion_.size()-1);g++){
+
+		unsigned int act_index=0+g;
+		unsigned int next_index=1+g;
+		double act_angle=orientation_person_robot_angles_with_prediction_of_person_companion_[act_index];
+		double next_angle=orientation_person_robot_angles_with_prediction_of_person_companion_[next_index];
+
+		if((act_index==0)){
+			act_angle=initial_angle_; // initial angle es el mismo para los dos casos.
+		}
+
+		if(g==0){
+			act_angle=initial_angle_;
+		}
+
+		double diff_angle=next_angle-act_angle;
+
+		if(((diff_angle>(angle_increment_of_increment_distance_)))&&(diff_angle>0)){
+
+			double new_angle=act_angle+(angle_increment_of_increment_distance_);
+
+			if(new_angle<angle_companion_){
+				orientation_person_robot_angles_with_prediction_of_person_companion_[next_index]=angle_companion_;
+			}else{
+				orientation_person_robot_angles_with_prediction_of_person_companion_[next_index]=new_angle;
+			}
+			if(new_angle>180){
+				orientation_person_robot_angles_with_prediction_of_person_companion_[next_index]=180;
+			}
+
+		}else{
+			//std::cout << "ENTRO EN ELSE !!!"<< std::endl;
+			//	std::cout << " (FOR) (real) next_angle="<<orientation_person_robot_angles_[next_index]<< std::endl;
+		}
+
+	} // fin for vuelta hacia delante
+
+
+	if(!orientation_person_robot_angles_with_prediction_of_person_companion_.empty()){
+
+		bool we_have_path_collisions=false;
+
+		for(unsigned int g=0;g<(orientation_person_robot_angles_with_prediction_of_person_companion_.size()-1);g++){
+			if((min_distance_collision_vector_from_pred_[g]!=100)&&(orientation_person_robot_angles_with_prediction_of_person_companion_[g]>(angle_companion_+2*angle_increment_of_increment_distance_))){
+				we_have_path_collisions=true;
+			}
+		}
+
+		double ini_act_angle_out=initial_angle_;
+
+		if((!we_have_path_collisions)&&(ini_act_angle_out>angle_companion_)){
+			for(unsigned int g=0;g<(orientation_person_robot_angles_with_prediction_of_person_companion_.size()-1);g++){
+				unsigned int act_index;//=best_plan_vertex_index_[best_plan_vertex_index_.size()-g];
+
+				unsigned int next_index=g+1;
+				double act_angle;//=orientation_person_robot_angles_[act_index];
+				if(g==0){
+					act_angle=ini_act_angle_out;
+					act_index=0;
+					orientation_person_robot_angles_with_prediction_of_person_companion_[act_index]=ini_act_angle_out;
+				}else{
+					act_angle=orientation_person_robot_angles_with_prediction_of_person_companion_[act_index];
+				}
+
+				double next_angle=act_angle-angle_increment_of_increment_distance_;
+				if(next_angle<angle_companion_){
+					orientation_person_robot_angles_with_prediction_of_person_companion_[next_index]=angle_companion_;
+				}else{
+					orientation_person_robot_angles_with_prediction_of_person_companion_[next_index]=next_angle;
+				}
+
+				act_index=next_index;
+			}
+		}
+	}
+/////////////// end: function fix angles for prediction (vuelta hacia delante)
+}
+
+
+// Function to find mean.
+double Cplan_local_nav::mean(std::vector<double> arr, int n)
+{
+    double sum = 0;
+    for(int i = 0; i < n; i++)
+        sum = sum + arr.at(i);
+    return sum / n;
+}
+
+// Function to find covariance.
+double Cplan_local_nav::covariance(std::vector<double> arr1, std::vector<double> arr2, int n)
+{
+    double sum = 0;
+    for(int i = 0; i < n; i++)
+        sum = sum + (arr1.at(i) - mean(arr1, n)) *
+                    (arr2.at(i) - mean(arr2, n));
+    return sum / (n - 1);
+}
+
+double Cplan_local_nav::variance(std::vector<double> a, int n)
+{
+    // Compute mean (average of elements)
+    double sum = 0;
+    for (int i = 0; i < n; i++)
+        sum += a.at(i);
+    double mean =  sum /
+                  (double)n;
+
+    // Compute sum squared
+    // differences with mean.
+    double sqDiff = 0;
+    for (int i = 0; i < n; i++)
+        sqDiff += (a.at(i) - mean) *
+                  (a.at(i) - mean);
+    return sqDiff / n;
+}
+
+
+//******* Nuevas funciones y variables cambio modelo side-by-side:  *******/
+
+// calculate the centre of one group composed by one-robot and N-people.
+SpointV Cplan_local_nav::calculate_centre_of_the_group(SpointV robot, std::vector<SpointV> people){
+
+	double x_center_group,  y_center_group,  vx_center_group,  vy_center_group;
+
+	x_center_group=robot.x;
+	y_center_group=robot.y;
+	vx_center_group=robot.vx;
+	vy_center_group=robot.vy;
+
+	for(unsigned int p=0;p<people.size();p++){
+		x_center_group = x_center_group + people[p].x;
+		y_center_group = y_center_group + people[p].y;
+		vx_center_group = vx_center_group + people[p].vx;
+		vy_center_group = vy_center_group + people[p].vy;
+	}
+
+	x_center_group = x_center_group/(people.size()+1);
+	y_center_group = y_center_group/(people.size()+1);
+	vx_center_group = vx_center_group/(people.size()+1);
+	vy_center_group = vy_center_group/(people.size()+1);
+
+	SpointV center_return(x_center_group,y_center_group, robot.time_stamp,vx_center_group, vy_center_group);
+
+	return  center_return;
+}
+
+
+// Function to calculate the final goal translated for the position of the element=person/robot of the group. Transtated to be liniarly respect to this element of the group.
+Spoint Cplan_local_nav::calculate_goal_translated_of_group_element(SpointV group_centre, SpointV group_element, Spoint group_goal, Spoint distRP, double theta_robot, unsigned int parent_vertex)
+{  // group element == robot or person of the group.
+   // return el goal trasladado para este elemento del grupo.
+
+	Sdestination best_p1_group_dest=pointer_to_person_companion_->get_best_dest();
+
+	double theta=atan2(group_element.vy,group_element.vx);
+
+	//double distance=sqrt(distRP.x*distRP.x+distRP.y*distRP.y);
+	theta=theta_robot;
+
+	if(theta<0){
+		theta=2*3.14+theta;
+	}
+
+	double angle=atan2(group_centre.y-group_element.y , group_centre.x-group_element.x);
+
+	double angle2=angle;
+	if(angle2<0){
+		angle2=(2*3.14)+angle2;
+	}
+
+	double new_angle_now;
+
+	//std::cout << "(calculate_goal_translated_of_group_element) group_goal.x="<<group_goal.x<<"; group_goal.y="<<group_goal.y<< std::endl;
+	if( diffangle(theta, angle2) < 0 ){
+
+		new_angle_now=theta+angle2;
+		if(new_angle_now>(2*3.14)){
+			new_angle_now=new_angle_now-(2*3.14);
+		}
+		if(new_angle_now<0){
+				new_angle_now=new_angle_now+(2*3.14);
+		}
+
+	} else{
+
+		new_angle_now=theta-angle2;
+		if(new_angle_now>(2*3.14)){
+			new_angle_now=new_angle_now-(2*3.14);
+		}
+		if(new_angle_now<0){
+			new_angle_now=new_angle_now+(2*3.14);
+		}
+
+	}
+
+
+	double x_dist_centre_and_element_of_group=distRP.x;
+	double y_dist_centre_and_element_of_group=distRP.y;
+
+	Spoint  goal_robot_traslated = Spoint( group_goal.x + x_dist_centre_and_element_of_group, group_goal.y + y_dist_centre_and_element_of_group, group_centre.time_stamp);
+
+	//std::cout << "(calculate_goal_translated_of_group_element) x="<<group_goal.x + x_dist_centre_and_element_of_group*cos(new_angle_now)<<"; y= "<<group_goal.y + y_dist_centre_and_element_of_group*sin(new_angle_now)<< std::endl;
+
+return goal_robot_traslated;
+
+}
+
+// function to traslate de position of one of the elements of the group, to other position inside the group.
+// To change the centre of the path or to change the position of the path to the group centre to compute collisions.
+// This function returns the translated position of the element.
+SpointV_cov Cplan_local_nav::calculate_element_position_translated_in_group(SpointV_cov robot_plan_act, SpointV_cov person_propagation_act){
+
+    double theta=calc_person_companion_orientation();
+
+    if(theta<0){
+    	theta=2*3.14+theta;
+    }
+
+	double angle=atan2(person_propagation_act.y-robot_plan_act.y , person_propagation_act.x-robot_plan_act.x);
+
+	double angle2=angle;
+	if(angle2<0){
+		angle2=((360*3.14)/180)+angle2;
+	}
+
+	double new_angle_now;
+
+	if( diffangle(theta, angle2) < 0 ){
+
+		new_angle_now=theta+angle2;
+		if(new_angle_now>360){
+			new_angle_now=new_angle_now-360;
+		}
+
+	} else{
+
+		new_angle_now=theta-angle2;
+		if(new_angle_now>360){
+			new_angle_now=new_angle_now-360;
+		}
+	}
+	double x=robot_plan_act.x+robot_person_proximity_distance_*cos(new_angle_now);
+	double y=robot_plan_act.y+robot_person_proximity_distance_*sin(new_angle_now);
+
+	SpointV_cov traslated_robot_position=SpointV_cov(x,y,robot_plan_act.time_stamp,robot_plan_act.vx,robot_plan_act.vy,robot_plan_act.cov);
+
+	return traslated_robot_position;
+}
+
+
+// Funcion para calcular la desired velocity.
+double Cplan_local_nav::calc_desired_robot_velocity_act(SpointV robot, SpointV person_propagation, double vel_person_companion){
+	// la velocidad maxima del robot es la variable global max_v_by_system_
+
+	double desired_vel_robot;
+	double vel_per=sqrt((person_propagation.vy)*(person_propagation.vy) + (person_propagation.vx)*(person_propagation.vx));
+
+	double  distance_between_ini_pose_and_final_next_goal=robot.distance(person_propagation);
+
+	if(distance_between_ini_pose_and_final_next_goal>3){ // distancia mayor que 3 metros
+		desired_vel_robot=max_v_by_system_;
+	}else if(distance_between_ini_pose_and_final_next_goal<treshold_distance_between_steps_){ // margen de error en distancia al goal, que consideramos que el robot ha de ir a la velocidad de la persona.
+		desired_vel_robot=vel_per; // v_person
+	}else{ // gestion de velocidad entre ir a la velocidad de la persona y ir a la velocidad maxima del robot.
+		double x=distance_between_ini_pose_and_final_next_goal;
+		desired_vel_robot=((max_v_by_system_-vel_per)/(3-treshold_distance_between_steps_))*(x-treshold_distance_between_steps_)+vel_per;
+	}
+
+
+
+	if(desired_vel_robot>max_v_by_system_){ // si sobrepaso la velocidad maxima del sistema, ojo! limito a la velocidad maxima del sistema.
+		desired_vel_robot=max_v_by_system_;
+	}
+
+	return desired_vel_robot;
+}
+
+
+
+void Cplan_local_nav::calculate_initial_companion_angle_case_side_by_side_one_person(double theta, double angle2){
+// Ini copiar
+	double ini_angle_act;
+	//double ant_ini_angle;
+
+	if( diffangle(theta, angle2) < 0 ){
+		ini_angle_act=(theta + angle2)*(180/3.14);//(angle)*(180/3.14);
+		//ant_ini_angle=(robot_->get_current_pointV().angle_heading_point((Spoint) companion_person_position_))*(180/3.14);
+	}else{
+		ini_angle_act=(theta - angle2)*(180/3.14);//(angle)*(180/3.14);
+		//ant_ini_angle=(robot_->get_current_pointV().angle_heading_point((Spoint) companion_person_position_))*(180/3.14);
+	}
+
+	if((ini_angle_act>=0)&&(ini_angle_act<=180)){
+		ini_angle_act=ini_angle_act;
+	}else if((ini_angle_act<0)&&(ini_angle_act>=(-180))){
+		ini_angle_act=sqrt(ini_angle_act*ini_angle_act);
+	}else if((ini_angle_act>180)&&(ini_angle_act<=360)){
+		ini_angle_act=360-ini_angle_act;
+	}else if((ini_angle_act<(-180))&&(ini_angle_act>=(-360))){
+		ini_angle_act=sqrt(ini_angle_act*ini_angle_act);
+		ini_angle_act=360-ini_angle_act;
+	}else if(ini_angle_act>360){
+		unsigned int n_360_g=ini_angle_act/360;
+		ini_angle_act=ini_angle_act-n_360_g*360;
+	}else{
+		//	std::cout << " OJO!!! casos NO contemplados ; ini_angle_act="<<ini_angle_act<< std::endl;
+
+	}
+
+
+	if((overpas_obstacles_behind_person_)){
+		// pasar angulos a cuadrante 90 -> 180 grados.
+		if((ini_angle_act>=0)&&(ini_angle_act<=90)){
+			ini_angle_act=180-ini_angle_act;
+		}else if((ini_angle_act>=90)&&(ini_angle_act<=180)){
+			// ok, no se hace nada.
+		}else if((ini_angle_act>=180)&&(ini_angle_act<=270)){
+			ini_angle_act=ini_angle_act-180;
+			ini_angle_act=180-ini_angle_act;
+		}else if((ini_angle_act>=270)&&(ini_angle_act<=360)){
+			ini_angle_act=ini_angle_act-180;
+		}
+	}
+
+	initial_angle_=ini_angle_act;
+
+	//std::cout << " First calcul of initial_angle_ ="<<initial_angle_ << std::endl;
+
+	orientation_person_robot_angles_.push_back( initial_angle_ );
+    final_min_collision_distance_vector_.push_back( 0.0 );
+    real_robot_person_distance2_vector_.push_back( 0.0 );
+// Fin copiar
+}
+
+
+void Cplan_local_nav::calculate_correct_initial_angle(Cperson_abstract* person_obj1, SpointV_cov person1, Spose robot,double &theta){
+////////// Inicio calculo correcto initial angle!!! /////////////////////////////////////
+	// internamente uso: person_obj1 ; person1; robot (k podría usar la global)
+	// ini calcular nuevo angulo theta con orientación dirección movimiento persona.
+    theta=calc_person_companion_orientation();
+    if(companion_same_person_goal_){
+		theta = atan2(person_obj1->get_best_dest().y-person1.y ,person_obj1->get_best_dest().x-person1.x); // angulo entre la destinacion a la que va la persona y la posicion de la persona.
+	}else{
+		theta = atan2(extern_robot_goal_.y-person1.y , extern_robot_goal_.x-person1.x);
+	}
+
+	// fin calcular nuevo angulo theta con orientación dirección movimiento persona.
+	person_orient_=theta;
+	double angle=atan2(person1.y-robot.y , person1.x-robot.x);
+	angle=angle*(180/3.14);
+
+	if(angle<0){
+		angle=360+angle;
+	}
+
+// valido SOLO para el caso trasero SIEMPRE!!!
+	if((angle>=90)&&(angle<=180)){
+		//angle=angle; // case same angle, we do not need to translate the angle.
+
+			if(debug_file_robot_){
+				std::ofstream fileMatlab2;
+				fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+				fileMatlab2 << " [change] diff_angle [0<->180] (1) angle ="<<angle <<"\n";
+				fileMatlab2.close();
+			}
+		}else if((angle<90)&&(angle>=0)){
+			angle=180-angle;
+
+			if(debug_file_robot_){
+				std::ofstream fileMatlab2;
+				fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+				fileMatlab2 << " [change] diff_angle [0<-> -180] (1) angle ="<<angle <<"\n";
+				fileMatlab2.close();
+			}
+		}else if((angle>180)&&(angle<=270)){
+			angle=360-angle;
+
+			if(debug_file_robot_){
+				std::ofstream fileMatlab2;
+				fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+				fileMatlab2 << " diff_angle [180<-> 360] (1) angle ="<<angle <<"\n";
+				fileMatlab2.close();
+			}
+		}
+		else if((angle <( 360))&&(angle>=(270))){
+			angle=sqrt(angle*angle);
+			angle=angle-180;
+
+
+			if(debug_file_robot_){
+				std::ofstream fileMatlab2;
+				fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+				fileMatlab2 << " diff_angle [-180<-> -360] (1) angle ="<<angle <<"\n";
+				fileMatlab2.close();
+			}
+		}else if(angle>360){
+			 unsigned int n_360_g=angle/360;
+			 angle=angle-n_360_g*360;
+
+		}else{
+			//std::cout << " OJO!!! caso NO contemplado angle="<<angle<< std::endl;
+		}
+	angle=angle*(3.14/180);
+
+	double ini_angle_act;
+	double ant_ini_angle;
+	ant_ini_angle=(robot_->get_current_pointV().angle_heading_point((Spoint) companion_person_position_))*(180/3.14);
+
+	if( diffangle( angle, theta) < 0 ){
+		ini_angle_act=angle*(180/3.14);
+
+		if(debug_file_robot_){
+			std::ofstream fileMatlab2;
+			fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+			fileMatlab2 << " !!!!!!!!!! (IF) ini_angle_act ="<<ini_angle_act <<"; theta*(180/3.14)="<<theta*(180/3.14)<<"angle*(180/3.14)="<<angle*(180/3.14)<<"ant_ini_angle="<<ant_ini_angle<< "\n";
+			fileMatlab2.close();
+
+		}
+
+	}else{
+
+		ini_angle_act=angle*(180/3.14);
+		ant_ini_angle=(robot_->get_current_pointV().angle_heading_point((Spoint) companion_person_position_))*(180/3.14);
+
+		if(debug_file_robot_){
+			std::ofstream fileMatlab2;
+			fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+			fileMatlab2 << " !!!!!!!!!! (ELSE) ini_angle_act ="<<ini_angle_act <<"; theta*(180/3.14)="<<theta*(180/3.14)<<"angle*(180/3.14)="<<angle*(180/3.14)<<"ant_ini_angle="<<ant_ini_angle<<"\n";
+			 fileMatlab2.close();
+		}
+	}
+
+	//std::cout << " CALC INI ANGLE!!!!! [ini] (1) ini_angle_act ="<<ini_angle_act << std::endl;
+
+	if((ini_angle_act>=0)&&(ini_angle_act<=180)){
+		//ini_angle_act=ini_angle_act;
+
+		if(debug_file_robot_){
+			std::ofstream fileMatlab2;
+			fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+			fileMatlab2 << " [change] diff_angle [0<->180] (1) ini_angle_act ="<<ini_angle_act <<"\n";
+			fileMatlab2.close();
+		}
+	}else if((ini_angle_act<0)&&(ini_angle_act>=(-180))){
+		ini_angle_act=sqrt(ini_angle_act*ini_angle_act);
+
+		if(debug_file_robot_){
+			std::ofstream fileMatlab2;
+			fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+			fileMatlab2 << " [change] diff_angle [0<-> -180] (1) ini_angle_act ="<<ini_angle_act <<"\n";
+			fileMatlab2.close();
+		}
+	}else if((ini_angle_act>180)&&(ini_angle_act<=360)){
+		ini_angle_act=360-ini_angle_act;
+
+
+		if(debug_file_robot_){
+			std::ofstream fileMatlab2;
+			fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+			fileMatlab2 << " diff_angle [180<-> 360] (1) ini_angle_act ="<<ini_angle_act <<"\n";
+			fileMatlab2.close();
+		}
+	}
+	else if((ini_angle_act <( -180))&&(ini_angle_act>=(-360))){
+		ini_angle_act=sqrt(ini_angle_act*ini_angle_act);
+		ini_angle_act=360-ini_angle_act;
+
+
+		if(debug_file_robot_){
+			std::ofstream fileMatlab2;
+			fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+			fileMatlab2 << " diff_angle [-180<-> -360] (1) ini_angle_act ="<<ini_angle_act <<"\n";
+			fileMatlab2.close();
+		}
+	}else if(ini_angle_act>360){
+		 unsigned int n_360_g=ini_angle_act/360;
+		 ini_angle_act=ini_angle_act-n_360_g*360;
+
+	}else{
+		//std::cout << " OJO!!! caso NO contemplado ini_angle_act="<<ini_angle_act<< std::endl;
+	}
+
+	initial_angle_=ini_angle_act;
+
+	//	std::cout << " initial_angle_ ="<<initial_angle_ << std::endl;
+	//	std::cout << " !!! FIN calculo correcto initial angle !!! "<< std::endl;
+
+////////// Fin calculo correcto initial angle!!! //////////////////////////////////////
+	}
+
+void Cplan_local_nav::set_robot_velocity_using_obstacles(double vel_per, double near_obst, double actual_V_Robot, double distance_PR7, double robot_person_distance, double d_min, double min_dist_colli_act_global2, double circunferencia_tope, double margen_angle_colision, double angle_colision) // funcion que setea la velocidad del robot a la deseada actual + alguna variable global relacionada con la velocidad que necesita el robot.
+{
+	//std::cout << " (before obstacles) vel_per="<<vel_per<<"; d_min="<<d_min<< std::endl;
+// copiar desde aquí
+	d_min=sqrt(d_min);
+
+	if ( d_min < 5.0 ){
+
+			// margen interno caso 1 ampliado. cuando esta muy cerca te interesa que entre en caso velicidad limitado con la persona y no muy grande.
+			if((robot_person_distance>(robot_person_proximity_distance_-2*marge_in_distance_))&&(robot_person_distance<(robot_person_proximity_distance_+marge_in_distance_))&&(robot_person_distance<circunferencia_tope)&&(initial_angle_<(angle_companion_+marge_angle_companion_))&&(initial_angle_>(angle_companion_-marge_angle_companion_))&&(min_next_companion_angle_>(angle_companion_-5))&&(min_next_companion_angle_<(angle_companion_+5))){//&&((min_dist_colli_act_global2==robot_person_companion_distance_)||(min_dist_colli_act_global2>robot_person_companion_distance_))){
+
+				vel_per_ok_=Cperson_abstract::Vel_per;
+				go_with_vel_per_=true;
+				robot_->set_v_max( vel_per*near_obst );
+
+				if(debug_file_robot_){
+					std::ofstream fileMatlab2;
+					fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+					fileMatlab2 << "(caso d=3 )caso if (1) vel_per*near_obst ="<<vel_per*near_obst << "; go_with_vel_per_="<<go_with_vel_per_<<"\n";
+					fileMatlab2.close();
+				}
+
+				//std::cout << " (3) caso if="<< std::endl;
+				//std::cout << "(caso d=3 )caso if (1) vel_per*near_obst ="<<vel_per*near_obst << "; go_with_vel_per_="<<go_with_vel_per_<< std::endl;
+				//std::cout << " (3) in min_next_companion_angle_="<<min_next_companion_angle_<< std::endl;
+				//std::cout << " max_v_by_system_ * 0.95= v_max_="<<robot_->get_v_max()<< std::endl;
+
+			}else if((robot_person_distance<circunferencia_tope)&&(min_next_companion_angle_>(0-30))&&(min_next_companion_angle_<(0+30))&&(((initial_angle_<(0+30))&&(initial_angle_>(0-30)))||((initial_angle_<(180+30))&&(initial_angle_>(180-30))))&&(distance_PR7<(robot_person_proximity_distance_+0.5))&&(distance_PR7>(robot_person_proximity_distance_-0.2))){
+				vel_per_ok_=Cperson_abstract::Vel_per;
+				robot_->set_v_max( vel_per*near_obst );
+
+				if(debug_file_robot_){
+					std::ofstream fileMatlab2;
+					fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+					fileMatlab2 << " (caso d=3 )caso else if (2) vel_per*near_obst ="<<vel_per*near_obst << "; go_with_vel_per_="<<go_with_vel_per_<<"\n";
+					fileMatlab2.close();
+				}
+
+				//std::cout << " (caso d=3 )caso else if (2) vel_per*near_obst ="<<vel_per*near_obst << "; go_with_vel_per_="<<go_with_vel_per_<< std::endl;
+				//std::cout << " (3) caso else if= ( persona y robot van a 0 grados o 180 y a la distancia necesaria entre ellos.)"<< std::endl;
+				//std::cout << "  max_v_by_system_ * 0.95=vel_per="<<vel_per<<"; v_max_="<<robot_->get_v_max()<< std::endl;
+
+			}else if((robot_person_distance<circunferencia_tope)&&(min_dist_colli_act_global2>=robot_person_companion_distance_)&&(min_next_companion_angle_>(angle_colision-margen_angle_colision))&&(min_next_companion_angle_<(angle_colision+margen_angle_colision))&&(initial_angle_<(angle_colision+margen_angle_colision))&&(initial_angle_>(angle_colision-margen_angle_colision))){
+				vel_per_ok_=Cperson_abstract::Vel_per;
+				go_with_vel_per_=true;
+				robot_->set_v_max( vel_per*near_obst );
+
+				if(debug_file_robot_){
+					std::ofstream fileMatlab2;
+					fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+					fileMatlab2 << "(caso d=3 ) caso punto equilibrio movil!!! caso else if (3) vel_per*near_obst ="<<vel_per*near_obst << "; go_with_vel_per_="<<go_with_vel_per_<<"\n";
+					fileMatlab2.close();
+				}
+
+				//std::cout << "(caso d=3 ) caso punto equilibrio movil!!! caso else if (3) vel_per*near_obst ="<<vel_per*near_obst << "; go_with_vel_per_="<<go_with_vel_per_<< std::endl;
+				//std::cout << " (3) caso else if= ( persona y robot van angle_colision )"<< std::endl;
+				//std::cout << "  max_v_by_system_ * 0.95=vel_per="<<vel_per<< "; v_max_="<<robot_->get_v_max()<< std::endl;
+
+			}else{
+
+				if((vel_per>0)&&(actual_V_Robot<max_v_by_system_)&&(actual_V_Robot>(max_v_by_system_ * 0.95))){
+					robot_->set_v_max( actual_V_Robot );
+					vel_per_ok_=Cperson_abstract::Far_goal;
+
+					//std::cout << " CASO ELSE (1.1.2)= actual_V_Robot"<< std::endl;
+					//std::cout << " (3.1.2); robot_->get_v_max()="<<robot_->get_v_max()<< std::endl;
+
+					if(debug_file_robot_){
+						std::ofstream fileMatlab2;
+						fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+						fileMatlab2 << " CASO ELSE (1.1.2)= actual_V_Robot\n";
+						fileMatlab2 << " (3.1.2); robot_->get_v_max()="<<robot_->get_v_max()<<"\n";
+						fileMatlab2.close();
+					}
+				}else{
+
+					vel_per_ok_=Cperson_abstract::Far_goal;
+					robot_->set_v_max( max_v_by_system_ * 0.95 );
+
+					//std::cout << " CASO ELSE (1.1.2)= max_v_by_system_ * 0.85 "<< std::endl;
+					//std::cout << " (3.1.2); robot_->get_v_max()="<<robot_->get_v_max()<< std::endl;
+
+					if(debug_file_robot_){
+						std::ofstream fileMatlab2;
+						fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+						fileMatlab2 << "  CASO ELSE (1.1.2)= max_v_by_system_ * 0.85 \n";
+						fileMatlab2 << " (3.1.2); robot_->get_v_max()="<<robot_->get_v_max()<<"\n";
+						fileMatlab2.close();
+					}
+				}
+
+				//std::cout << " (3) caso else="<< std::endl;
+				//std::cout << " out CASO ELSE (3.1)= max_v_by_system_ * 0.95"<< std::endl;
+				//std::cout << " (3.1); robot_->get_v_max()="<<robot_->get_v_max()<< std::endl;
+				//std::cout << "(3) caso else robot_->get_v_max()="<<robot_->get_v_max() <<"; go_with_vel_per_="<<go_with_vel_per_<< std::endl;
+
+			}
+		}
+		else{
+
+			if((robot_person_distance > (robot_person_proximity_distance_+marge_in_distance_))&&((initial_angle_>(angle_companion_+marge_angle_companion_))||(initial_angle_<(angle_companion_-marge_angle_companion_)))){ // caso robot muy alejado de la persona que acompaña => ha de ir a V-max
+				//std::cout << " CASO V_MAX lejos!!!"<< std::endl;
+				vel_per_ok_=Cperson_abstract::Far_goal;
+				robot_->set_v_max( max_v_by_system_ );
+			}
+			else if((robot_person_distance>(robot_person_proximity_distance_-2*marge_in_distance_))&&(robot_person_distance<(robot_person_proximity_distance_+marge_in_distance_))&&(robot_person_distance<circunferencia_tope)&&(initial_angle_<(angle_companion_+marge_angle_companion_))&&(initial_angle_>(angle_companion_-marge_angle_companion_))&&(min_next_companion_angle_>(angle_companion_-5))&&(min_next_companion_angle_<(angle_companion_+5))){//&&((min_dist_colli_act_global2==robot_person_companion_distance_)||(min_dist_colli_act_global2>robot_person_companion_distance_))){
+
+				vel_per_ok_=Cperson_abstract::Vel_per;
+				go_with_vel_per_=true;
+				robot_->set_v_max( vel_per*near_obst );
+
+				//std::cout << " 1 (4) if vel_per*near_obst ="<<vel_per*near_obst << "; go_with_vel_per_="<<go_with_vel_per_<< std::endl;
+				//std::cout << " (4) caso if="<< std::endl;
+				//std::cout << " (4) in min_next_companion_angle_="<<min_next_companion_angle_<< std::endl;
+				//std::cout << " max_v_by_system_=v_max_="<<robot_->get_v_max()<< std::endl;
+				if(debug_file_robot_){
+					std::ofstream fileMatlab2;
+					fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+					fileMatlab2 << "(4) if vel_per*near_obst ="<<vel_per*near_obst << "; go_with_vel_per_="<<go_with_vel_per_<<"\n";
+					fileMatlab2.close();
+				}
+
+
+			}else if((robot_person_distance<circunferencia_tope)&&(min_next_companion_angle_>(0-30))&&(min_next_companion_angle_<(0+30))&&(((initial_angle_<(0+30))&&(initial_angle_>(0-30)))||((initial_angle_<(180+30))&&(initial_angle_>(180-30))))&&(distance_PR7<(robot_person_proximity_distance_+0.5))&&(distance_PR7>(robot_person_proximity_distance_-0.2))){
+
+				//std::cout << " RARO1 max_v_by_system_=vel_per="<<vel_per<<"; v_max_="<<robot_->get_v_max()<< std::endl;
+				vel_per_ok_=Cperson_abstract::Vel_per;
+				go_with_vel_per_=true;
+				robot_->set_v_max( vel_per*near_obst );
+				robot_->set_v_max( max_v_by_system_  );
+				//std::cout << "robot_->set_v_max( max_v_by_system_ );"<< std::endl;
+
+				//std::cout << " 2 (4) else if  ( persona y robot van a 0 grados o 180 y a la distancia necesaria entre ellos.) 1 vel_per*near_obst ="<<vel_per*near_obst << "; go_with_vel_per_="<<go_with_vel_per_<< std::endl;
+				//std::cout << " max_v_by_system_=vel_per="<<vel_per<<"; v_max_="<<robot_->get_v_max()<< std::endl;
+
+				if(debug_file_robot_){
+					std::ofstream fileMatlab2;
+					fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+					fileMatlab2 << "(4) else if 1 vel_per*near_obst ="<<vel_per*near_obst << "; go_with_vel_per_="<<go_with_vel_per_<<"\n";
+					fileMatlab2 <<" max_v_by_system_=vel_per="<<vel_per<<"; v_max_="<<robot_->get_v_max()<< "\n";
+					fileMatlab2.close();
+				}
+
+			}else if((robot_person_distance<circunferencia_tope)&&(min_dist_colli_act_global2>=robot_person_companion_distance_)&&(min_next_companion_angle_>(angle_colision-margen_angle_colision))&&(min_next_companion_angle_<(angle_colision+margen_angle_colision))&&(initial_angle_<(angle_colision+margen_angle_colision))&&(initial_angle_>(angle_colision-margen_angle_colision))){
+
+				// caso con obstaculos y colisiones, ha de cambiar angulo
+				vel_per_ok_=Cperson_abstract::Vel_per;
+				go_with_vel_per_=true;
+				robot_->set_v_max( vel_per*near_obst );
+
+				//std::cout << " 3 (4) else if 2 vel_per*near_obst ="<<vel_per*near_obst << "; go_with_vel_per_="<<go_with_vel_per_<< std::endl;
+
+				if(debug_file_robot_){
+					std::ofstream fileMatlab2;
+					fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+					fileMatlab2 << "(4) else if 2 vel_per*near_obst ="<<vel_per*near_obst << "; go_with_vel_per_="<<go_with_vel_per_<<"\n";
+					fileMatlab2 <<" max_v_by_system_=vel_per="<<vel_per<<"; v_max_="<<robot_->get_v_max()<< "\n";
+					fileMatlab2.close();
+				}
+				//std::cout << " (4) caso else if 2= ( persona y robot van angle_colision ) max_v_by_system_=vel_per="<<vel_per<< "; v_max_="<<robot_->get_v_max()<< std::endl;
+
+			}else{
+
+				if((min_next_companion_angle_!=angle_companion_)||(min_next_companion_angle_!=0)||(min_next_companion_angle_!=180)){
+
+					vel_per_ok_=Cperson_abstract::Vel_per;
+					go_with_vel_per_=true;
+					robot_->set_v_max( vel_per*near_obst );
+
+					//std::cout << "4 (4) if 1 robot_->set_v_max( max_v_by_system_ ); max_v_by_system_="<<max_v_by_system_<< std::endl;
+
+					if(debug_file_robot_){
+						std::ofstream fileMatlab2;
+						fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+						fileMatlab2 << " (4) if 1 robot_->set_v_max( max_v_by_system_ ); max_v_by_system_="<<max_v_by_system_<< "\n";
+						fileMatlab2.close();
+					}
+
+				}else{
+					vel_per_ok_=Cperson_abstract::Far_goal;
+					robot_->set_v_max( max_v_by_system_ );
+
+					//std::cout << "5 (4) else 1 robot_->set_v_max( max_v_by_system_ ); max_v_by_system_="<<max_v_by_system_<< std::endl;
+
+					if(debug_file_robot_){
+						std::ofstream fileMatlab2;
+						fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+						fileMatlab2 << "(4) else 1robot_->set_v_max( max_v_by_system_ ); max_v_by_system_="<<max_v_by_system_<< "\n";
+						fileMatlab2.close();
+					}
+				}
+
+				//std::cout << " CASO ELSE (4.1)= max_v_by_system_ "<< std::endl;
+				//std::cout << " (4.1); max_v_by_system_=robot_->get_v_max()="<<robot_->get_v_max()<< std::endl;
+				//std::cout << "(4) caso else robot_->get_v_max()="<<robot_->get_v_max() <<"; go_with_vel_per_="<<go_with_vel_per_<<"; max_v_by_system_="<<max_v_by_system_<< std::endl;
+
+			}
+		}
+
+	//std::cout << " (after obstacles) robot_desired_vel="<<robot_->get_desired_velocity()<< std::endl;
+
+	}
+
+
+
+double Cplan_local_nav::calc_person_companion_orientation_v2( Cperson_abstract* person_obj){ // return theta person companion in radians!
+// person_obj== pointer_to_person_companion_
+// ini calcular nuevo angulo theta con orientación dirección movimiento persona.
+	//std::cout <<"IN: calc_person_companion_orientation_v2"<< std::endl;
+
+    double person_vel = person_obj->get_current_pointV().v();
+    Sdestination person_best_dest = person_obj->get_best_dest();
+	SpointV_cov person = person_obj->get_current_pointV();
+	//Spoint robot_point=robot_->get_current_pointV();
+	double x;
+	double y;
+	double pers_dx;
+	double pers_dy;
+	double theta=0;
+	bool use_before_theta=false;
+
+	if((person_vel<threshold_min_vel_person_to_obtain_destination_)&&(person_obj->get_id()==id_person_companion_)){ //     actual_current_person_comp_point.v()<threshold_min_vel_person_to_obtain_destination_
+		// si no es ninguna de las person companion, te vas al otro caso.
+		use_before_theta=true;
+		if(person_obj->get_id()==id_person_companion_){
+			theta=last_good_theta_person_p1_;
+		}
+
+	}else{
+
+		if((person_obj->get_best_dest().x==person.x)&&(person_obj->get_best_dest().y==person.y)){
+
+			if(person_obj->get_past_trajectory()->size()>num_steps_orientation_){
+				x=person_obj->get_past_trajectory()->at(person_obj->get_past_trajectory()->size()-num_steps_orientation_).x;
+				y=person_obj->get_past_trajectory()->at(person_obj->get_past_trajectory()->size()-num_steps_orientation_).y;
+			}else{
+				x=person_obj->get_past_trajectory()->at(0).x;
+				y=person_obj->get_past_trajectory()->at(0).y;
+			}
+
+		}else{
+			x=person_obj->get_best_dest().x;
+			y=person_obj->get_best_dest().y;
+		}
+
+
+		if((person_obj->get_best_dest().x==person.x)&&(person_obj->get_best_dest().y==person.y)){
+			pers_dx=person.x-x;
+			pers_dy=person.y-y;
+		}else{
+			pers_dx=x-person.x;
+			pers_dy=y-person.y;
+		}
+
+
+		theta=atan2(pers_dy , pers_dx);
+
+		last_robot_pose_to_stop_=robot_->get_current_pointV();
+
+		if(person_obj->get_id()==id_person_companion_){
+			last_good_theta_person_p1_=theta;
+			before_person_comp_goal_p1_=person_obj->get_best_dest();
+			ori_pers_x_p1_=pers_dx;
+			ori_pers_y_p1_=pers_dy;
+		}
+
+		if(use_before_theta){
+			if(person_obj->get_id()==id_person_companion_){
+				theta=last_good_theta_person_p1_;
+			}
+		}
+
+	}
+
+	if(theta<0){  //si es negativa, la pasas al cuadrante positivo.
+		theta=2*3.14+theta;
+	}
+
+	//std::cout <<"[FINAL theta!!!] theta ="<<theta*180/3.14<< std::endl;
+	return theta;
+
+}
diff --git a/local_lib_people_prediction/src/nav/plan_local_nav.h b/local_lib_people_prediction/src/nav/plan_local_nav.h
new file mode 100644
index 0000000000000000000000000000000000000000..2a3757fff87b0d0d1984bff7d0cf3cc49936010c
--- /dev/null
+++ b/local_lib_people_prediction/src/nav/plan_local_nav.h
@@ -0,0 +1,1385 @@
+/*
+ * plan_local_nav.h
+ *
+ *  Created on: Dec 22, 2013 Last Modified on 2020
+ *      Author: Initial code of the AKP Gonzalo Ferrer.
+ *      Author: Addition of ASSAOP and ASSAGP for people accompaniment code of Ely Repiso.
+ */
+
+#ifndef PLAN_LOCAL_NAV_H_
+#define PLAN_LOCAL_NAV_H_
+
+#include "prediction_behavior.h"
+#include "scene_elements/person_abstract.h"
+#include "prediction_bhmip.h"
+#include "nav/force_reactive_robot_companion.h"
+
+#include <fstream>      // std::ofstream
+#include <random>
+#include <iostream>
+#include <string>
+#include <stdio.h>
+#include <math.h>
+
+#include<bits/stdc++.h>
+
+using namespace std;
+/**
+ *	\brief Sedge_tree Struct
+ *
+ *	data struct describing a tree edge containing the index to the parent vertex
+ *	and the command inputs (v.w) or forces required to get from that vertex state
+ */
+
+
+class Sedge_tree
+{
+  public:
+	Sedge_tree(	unsigned int parent, Sforce f_=Sforce(), Sforce f_goal_=Sforce(),
+			Sforce f_people_=Sforce(),Sforce f_obs_=Sforce(), Sforce f_persongoal_=Sforce());
+	unsigned int parent;
+	Sforce f;
+	Sforce f_goal;
+	Sforce f_people;
+	Sforce f_obs;
+	Sforce f_persongoal;
+	double cost(Sedge_tree parent)const;
+	void print();
+};
+
+/**
+ *	\brief Smulticost Struct
+ *
+ *	data struct for containing multiciple costs and compare them in a fast and clean way
+ *	specially to determine dominance relations between solutions.
+ */
+class Smulticost
+{
+  public:
+	Smulticost( unsigned int id_ = 0, unsigned int n = 1 );
+	std::vector<double> cost;
+	unsigned int id;
+	bool operator< ( const Smulticost& m2) const;
+	bool operator==( const Smulticost& m2) const;
+	bool operator!=( const Smulticost& m2) const;
+	void print() const;
+	void print_ml() const;
+};
+
+/**
+ *	\brief Cplan_local_nal
+ *
+ * Basic local planner using social forces
+ *
+ */
+class Cplan_local_nav : public Cprediction_behavior
+{
+  public:
+    enum plan_mode{  F_RRT_Uniform=0,  F_RRT_Gauss_Circle, F_RRT_GC_alpha};
+    enum distance_mode{  Euclidean=0,  Cost2go_erf, Cost2go_norm , Cost2go_raw};
+    enum global_mode{ Scalarization=0, Weighted_sum_erf, Weighted_sum_norm, MO_erf, MO_norm}; // uso MO_erf (global_mode=3)
+    enum action_mode{START=0,ITER}; //STOP
+    enum simulation_case{case0=0,case1,case2} ;
+    enum calculate_complete_group_path_{akp_planner=0,person_prediction,prediction_total_time_with_collisions};
+
+    /**
+     * \brief Initial algorithm parameters in the constructor of the
+     * algorithm
+     */
+    Cplan_local_nav(double horizon_time = 3.0, unsigned int max_iter = 1000,
+    		plan_mode mode= Cplan_local_nav::F_RRT_Uniform, bool robot_or_person=false);
+    virtual ~Cplan_local_nav(); // false == robot= id_0; true == person_companion =id_1
+
+    /**
+         * \brief this is the public interface to calculate the robot plan
+         * according to the observed scene and the algorithm parameters.[only go to the goal, without person, companion]
+         */
+   bool init_robot_plan2_return_initial_position();
+   bool robot_plan_companion_return_initial_pose(Spose& pose_command, Cperson_abstract::companion_reactive& reactive,double dt);
+
+
+    /**
+     * \brief this is the public interface to calculate the robot plan
+     * according to the observed scene and the algorithm parameters.
+     */
+    bool robot_plan_companion2(Spose& pose_command, Cperson_abstract::companion_reactive& reactive,double dt=0.0);
+    /**
+     * \brief this function does an akp_planner for the person_companion
+     *  is for the Cscene_sim (generates simulated persons.
+     */
+    bool robot_plan_companion3(Spose& pose_command, Cperson_abstract::companion_reactive& reactive,double dt=0.0);
+
+    double get_distance_to_goal() { return robot_->get_current_pointV().distance( goal_ );}
+    /**
+     * Configuration methods
+     */
+    void set_planning_mode( plan_mode mode ) {plan_mode_ = mode;}
+    plan_mode get_planning_mode(){ return plan_mode_; }
+    void set_distance_mode( distance_mode mode )
+    	{distance_mode_ = mode; nondominated_plan_vertex_index_.clear(); nondominated_end_of_plan_vertex_index_.clear();}
+    distance_mode get_distance_mode(){ return distance_mode_;}
+    void set_global_mode( global_mode mode )
+    	{global_mode_ = mode; nondominated_plan_vertex_index_.clear();nondominated_end_of_plan_vertex_index_.clear();}
+    global_mode get_global_mode(){ return global_mode_;}
+    void set_ppl_collision_mode( int ppl_col ) {ppl_collision_mode_ =  ppl_col; }
+    void set_pr_force_mode( int pr_force_mode ) { pr_force_mode_ = pr_force_mode;}
+    /**
+     * \brief set_robot_goal() function sets the value of the goal position to
+     * which the planning aims to. It is assumed that a global planner, above this
+     * implementation, provides these goals
+     */
+
+    void set_horizon_time(double in_horizon_time){
+    	horizon_time_=in_horizon_time;
+    }
+
+    void set_output_screen_messages(bool in_output_screen_messages){
+    	output_screen_messages_=in_output_screen_messages;
+    }
+
+    void set_robot_goal(const  Spoint& goal );
+    void set_robot_goal_person_companion_akp( const Spoint& goal );
+    void set_robot_goal_person_goal_global_plan( const Spoint& goal );
+    void set_robot_goal_person_goal_global_plan_IN_robot_VERSION( const Spoint& goal );
+    void set_robot_external_goal( const Spoint& goal ); // comapnion => obtain a external goal, not infered from the person comapnion
+    void set_robot_external_goal_fix( const Spoint& goal );
+
+    Spoint get_robot_goal() { return goal_;}
+    Spoint get_external_robot_goal(){ return Spoint(extern_robot_goal_.x,extern_robot_goal_.y,extern_robot_goal_.time_stamp);}
+    Spoint get_robot_local_goal() { return local_goal_;}
+    void set_plan_cost_parameters( double c_dist, double c_orientation, double c_w_robot,
+    		double c_w_people, double c_time, double c_w_obstacles, double c_old_path, double c_l_minima);
+    const std::vector<double>* get_plan_cost_parameters() const { return &cost_parameters_;}
+    void set_number_of_vertex( unsigned int n );
+    unsigned int get_number_of_vertex(  ) { return max_iter_;}
+    void set_robot_params( double v, double w, double av, double av_break,double aw, double platform_radii, double av_neg, double av_Vrobotzero, double lim_to_consider_robotZero);
+    void set_xy_2_goal_tolerance( double tol) { xy_2_goal_tolerance_ = tol; }
+    void set_v_goal_tolerance( double tol) { v_goal_tolerance_ = tol; }
+    void set_distance_to_stop( double d ){ distance_to_stop_ = d; }
+    double get_distance_to_stop( ) { return distance_to_stop_;}
+
+    /**
+     * Return info methods
+     */
+    const std::vector<unsigned int>* get_robot_plan_index() const {
+    	//std::cout <<" best_plan_vertex_index_.size()="<<best_plan_vertex_index_.size()<< std::endl;
+    	return &best_plan_vertex_index_;}
+    const std::vector<unsigned int>* get_robot_nondominated_plan_index() const {
+    	//std::cout <<" nondominated_plan_vertex_index_.size()="<<nondominated_plan_vertex_index_.size()<< std::endl;
+    	return &nondominated_plan_vertex_index_;
+    }
+    const std::vector<unsigned int>* get_robot_nondominated_end_of_plan_index() const {
+    	//std::cout <<" nondominated_end_of_plan_vertex_index_.size()="<<nondominated_end_of_plan_vertex_index_.size()<< std::endl;
+    	return &nondominated_end_of_plan_vertex_index_;
+    }
+    const std::vector<Sedge_tree>* get_plan_edges() const { return &edge_;}
+    const std::vector<Spoint>* get_random_goals() const { return &random_goals_;}
+    double get_workspace_radii() { return workspace_radii_;}
+    Spose get_best_planned_pose(double dt=0.0, Cperson_abstract::companion_reactive reactive=Cperson_abstract::Akp_planning);
+
+    void get_navigation_instant_work( double& work_robot, double& work_persons );
+    void get_navigation_cost_values(  std::vector<double>& costs )//returns the vector of bests costs, calculated in the global_cost routine
+    	{ costs = best_costs_;}
+    void get_navigation_mean_cost_values(  std::vector<double>& costs ) { costs = mean_costs_;}
+    void get_navigation_std_cost_values(  std::vector<double>& costs ) { costs = std_costs_;}
+	/**
+	 * \brief calculate scene cost
+	 *
+	 * This function calculates the costs due to robot navigation and
+	 * nearby moving people. It is used only as an observation of the scene,
+	 * no calculations are done, is just for evaluating purposes.
+	 */
+    void calculate_navigation_cost_values( std::vector<double>& costs );
+
+    // people companion ely functions
+    double get_actual_companion_angle() { return min_next_companion_angle_; }
+    double get_actual_companion_cost() { return min_next_companion_cost_; }
+    SpointV_cov get_companion_person_position() {return companion_person_position_;}
+    Sdestination get_person_destination() {return person_companion_goal_;}
+
+    //*** companion dynamic reconfigure (ini) ***//
+    unsigned int get_id_person_companion(){
+       	return id_person_companion_;
+    }
+
+    void set_id_person_companion(unsigned int in_id_person_companion){
+    	change_id_person_companion_=in_id_person_companion;
+    }
+
+    void set_id_person_companion_people_simulator(unsigned int in_id_person_companion){
+    	change_id_person_companion_=in_id_person_companion;
+    	change_set_id_person_companion();
+    }
+
+    void change_set_id_person_companion(){
+    	id_person_companion_=change_id_person_companion_;
+    	this->Cprediction_behavior::set_Cprediction_behavior_id_person_companion(change_id_person_companion_);
+        this->Cprediction_bhmip::set_id_person_companion_Cprediction_bhmip(change_id_person_companion_);
+        Cperson_abstract* person_obj1;
+        bool finded_person=find_person(id_person_companion_ , &person_obj1);
+        if(finded_person){
+        	person_obj1->set_person_type(Cperson_abstract::Person_companion);
+        }
+    }
+
+    void set_robot_person_proximity_distance(double in_proximity_distance_between_robot_and_person){
+    	robot_person_proximity_distance_=in_proximity_distance_between_robot_and_person;
+    }
+
+    void set_real_companion_angle(double in_real_companion_angle){
+    	angle_companion_=in_real_companion_angle;
+    	before_initial_angle_=in_real_companion_angle;
+    	min_next_companion_angle_=in_real_companion_angle;
+    }
+
+    void set_side_by_side_companion_angle(double in_side_by_side_companion_angle){
+    	side_by_side_companion_angle_=in_side_by_side_companion_angle;
+    }
+
+    double side_by_side_companion_angle_;
+
+    double get_robot_person_proximity_distance(){
+    	return robot_person_proximity_distance_;
+     }
+
+    Sdestination get_person_companion_goal_out(){
+        return person_companion_goal_out_;
+    }
+
+    void set_companion_same_person_goal(bool act_companion_same_person_goal){
+    	companion_same_person_goal_=act_companion_same_person_goal;
+    	std::cout << "companion_same_person_goal_="<<companion_same_person_goal_<< std::endl;
+    }
+
+    void set_max_d_to_detect_laser_obs(double in_max_d_to_detect_laser_obs){
+    	Cprediction_behaviour_set_max_d_to_detect_laser_obs(in_max_d_to_detect_laser_obs);
+    	max_distance_to_obstacles_detected_=in_max_d_to_detect_laser_obs;
+    	std::cout << "set_max_d_to_detect_laser_obs-> in_max_d_to_detect_laser_obs="<<in_max_d_to_detect_laser_obs<< std::endl;
+    }
+
+    void set_save_results_in_file(bool in_save_results_in_file){
+    	save_results_in_file_=in_save_results_in_file;
+     }
+
+    void set_results_filename(std::string in_results_file){
+    	results_file_=in_results_file;
+    	results_file_real_=in_results_file;
+    	std::cout << " (changed) results_file_="<<results_file_<< std::endl;
+    }
+
+    void set_debug_filename(std::string in_debug_file){
+    	debug_file_=in_debug_file;
+    }
+
+    void set_mode_debug_file(bool in_debug_file_robot){
+    	debug_file_robot_=in_debug_file_robot;
+    }
+
+    void only_comp_people_vel_and_robot_poses(int in_only_comp_people_vel_and_robot_poses){
+    	only_comp_people_vel_and_robot_poses_=in_only_comp_people_vel_and_robot_poses;
+    }
+
+    double get_horizon_time(){return horizon_time_;}
+
+    void set_person_list(std::list<Cperson_abstract *> in_person_list_){
+    	person_list_=in_person_list_;
+    }
+
+    std::list<Cperson_abstract *> get_person_list(){
+     	return person_list_;
+     }
+
+    void set_alpha_companion(double in_alpha_companion){
+    	alpha_companion_=in_alpha_companion;
+    }
+
+    void set_beta_companion(double in_beta_companion){
+    	beta_companion_=in_beta_companion;
+    }
+
+    double get_alpha_companion(){
+    	return alpha_companion_;
+    }
+
+    double get_beta_companion(){
+    	return beta_companion_;
+    }
+
+    void set_evaluate_costs_filename(std::string in_evaluate_costs_file){
+    	evaluate_costs_file_=in_evaluate_costs_file;
+        std::cout << " (changed) evaluate_costs_file_="<<evaluate_costs_file_<< std::endl;
+    }
+
+
+	void set_evaluate_change_distance_and_angle_companion_filename(std::string in_evaluate_change_distance_and_angle_companion_file){
+    	evaluate_change_distance_and_angle_companion_file_=in_evaluate_change_distance_and_angle_companion_file;
+	    std::cout << " (changed) evaluate_change_distance_and_angle_companion_file__file_="<<evaluate_change_distance_and_angle_companion_file_<< std::endl;
+	}
+
+    Sdestination get_person_companion_goal(){
+        //std::cout << " person_companion_goal_.x="<<person_companion_goal_.x<<"; person_companion_goal_.y="<<person_companion_goal_.y<< std::endl;
+        return robot_goal_to_person_companion_;
+     }
+    Sdestination get_robot_path_goal(){
+        //std::cout << " robot_path_goal_.x="<<robot_path_goal_.x<<"; robot_path_goal_.y="<<robot_path_goal_.y<< std::endl;
+        return robot_path_goal_;
+     }
+    Spose get_final_combined_goal(){
+        //std::cout << " final_combined_goal_.x="<<final_combined_goal_.x<<"; final_combined_goal_.y="<<final_combined_goal_.y<< std::endl;
+        return final_combined_goal_;
+     }
+
+    action_mode get_state_Action(){
+    	//std::cout << " final_combined_goal_.x="<<final_combined_goal_.x<<"; final_combined_goal_.y="<<final_combined_goal_.y<< std::endl;
+    	return Action_;
+    }
+
+    void set_state_Action(action_mode Action_in){
+    	//std::cout << " final_combined_goal_.x="<<final_combined_goal_.x<<"; final_combined_goal_.y="<<final_combined_goal_.y<< std::endl;
+    	Action_=Action_in;
+    }
+
+    Spoint get_SIM_initial_person_companion_pose1(){
+       	//std::cout << " SIM_initial_person_companion_pose1_:"<< std::endl;
+    	//SIM_initial_person_companion_pose1_.print();
+    	return SIM_initial_person_companion_pose1_;
+     }
+    Spoint get_SIM_initial_person_companion_pose2(){
+       	//std::cout << " SIM_initial_person_companion_pose1_:"<< std::endl;
+    	//SIM_initial_person_companion_pose1_.print();
+    	return SIM_initial_person_companion_pose2_;
+     }
+
+    simulation_case get_actual_case(){
+    	//std::cout << " SIM_initial_person_companion_pose1_:"<< std::endl;
+    	//SIM_initial_person_companion_pose1_.print();
+    	return actual_case_;
+    }
+
+    void set_actual_case(simulation_case actual_case_in){
+    	//std::cout << " SIM_initial_person_companion_pose1_:"<< std::endl;
+    	//SIM_initial_person_companion_pose1_.print();
+    	actual_case_=actual_case_in;
+    }
+
+    void set_ros_time_to_sec(double ros_time_to_sec_in){
+    	ros_time_to_sec_=ros_time_to_sec_in;
+    }
+
+       void set_restart_real(bool in_restart_real){
+    	   restart_real_=in_restart_real;
+       }
+
+       void set_restart_real_data_txt(){
+    	   std::cout << " set_restart_real_data_txt c++"  << std::endl;
+    	   new_matlab_file();
+    	   iteration_=1;
+    	   experiment_=1;
+       }
+
+       void set_change_sim(bool change_sim){
+    	   sim=change_sim;
+    	   this->Cprediction_behavior::set_sim_prediction_behaviour(sim);
+    	   this->Cscene_abstract::set_sim(sim);
+       }
+
+       void set_change_sim_target_per(bool change_sim_target_per){
+    	   sim_target_per=change_sim_target_per;
+         }
+
+       /*void set_change_external_to_stop_case(bool in_change_external_to_stop_case){
+    	   if(in_change_external_to_stop_case){
+    		   Action_=Cplan_local_nav::STOP;
+    	   }
+       }*/
+
+       // set values of performance approaching
+       void set_value_distance_global_path(double in_value_distance_global_path){
+    	   value_distance_global_path_=in_value_distance_global_path;
+       }
+
+       void set_global_path_ini_orientation(double in_global_path_ini_orientation){
+    	   global_path_ini_orientation_=in_global_path_ini_orientation;
+        }
+       void set_global_path_final_orientation(double in_global_path_final_orientation){
+    	   global_path_final_orientation_=in_global_path_final_orientation;
+        }
+
+       SpointV_cov get_robot_pose(){
+    	   return robot_->get_current_pointV();
+       }
+
+       void set_robot_pose_for_person_companion_simulation(Spose in_robot_pose){
+    	   robot_->set_current_pose(in_robot_pose);
+    	   //std::cout << " (planner in, set_robot_pose_for_person_companion_simulation) in_robot_pose.x="<<in_robot_pose.x<<"; in_robot_pose.y="<<in_robot_pose.y<< std::endl;
+    	   //std::cout << " (planner in, set_robot_pose_for_person_companion_simulation) robot_.get.x="<<robot_->get_current_pose().x<<"; robot_.get.y="<<robot_->get_current_pose().y<< std::endl;
+       }
+
+
+   	void set_final_goal_reached_in_node(bool in_final_goal_reached_in_node){
+   		final_goal_reached_in_node_=in_final_goal_reached_in_node;
+   	}
+
+
+    void new_matlab_file();
+
+    void do_scene_prediction_from_outside(std::vector<Sdestination>* person_best_dest){
+    	bool person_or_robot=true; //case, person_companion
+    	person_companion_->get_best_dest().print();
+    	this->Cprediction_behavior::scene_prediction(person_or_robot,person_best_dest); // scene prediction, needed, pero sin la person cmpanion y hará falta el robot en colisiones.
+
+    }
+
+    Spoint get_before_next_goal_of_robot(){
+    	return before_next_goal_of_robot_;
+    }
+
+	void set_high_vel_dampening_parameter_planner(double in_dampening_parameter){
+		robot_->set_high_vel_dampening_parameter(in_dampening_parameter);
+	}
+
+	void set_normal_vel_dampening_parameter_planner(double in_dampening_parameter){
+		robot_->set_normal_vel_dampening_parameter(in_dampening_parameter);
+	}
+	void set_slow_vel_dampening_parameter_planner(double in_dampening_parameter){
+		robot_->set_slow_vel_dampening_parameter(in_dampening_parameter);
+	}
+
+	void set_limit_linear_vel_for_dampening_parameter_planner(double in_limit_linear_vel){
+		robot_->set_limit_linear_vel_for_dampening_parameter(in_limit_linear_vel);
+	}
+	void set_limit_angular_vel_for_dampening_parameter_planner(double in_limit_angular_vel){
+		robot_->set_limit_angular_vel_for_dampening_parameter(in_limit_angular_vel);
+	}
+
+	void set_number_of_group_people(unsigned int in_number_of_group_people){
+		number_of_group_people_=in_number_of_group_people;
+	}
+
+	 Spoint get_SIM_initial_person_goal_pose_actual(){  // compartida con simul_people (no la puedo eliminar)
+	      //std::cout << " SIM_initial_person_goal_pose_actual_:"<< std::endl;
+	     //SIM_initial_person_goal_pose_actual_.print();
+	      return SIM_initial_person_goal_pose_actual_;
+	 }
+	 Spoint SIM_initial_person_goal_pose_actual_;
+
+	void set_new_time_window_for_filter_velocity(double in_new_time_window){
+		this->Cprediction_behavior::change_time_window_for_filter_velocity(in_new_time_window);
+	}
+
+	void set_change_first(bool in_first){
+		first_=in_first;
+	}
+
+	void set_gamma_companion(double in_gamma_companion){
+		gamma_companion_=in_gamma_companion;
+		}
+	void set_delta_companion(double in_delta_companion){
+		delta_companion_=in_delta_companion;
+		}
+
+	void set_k_final_goal(double in_set_k_final_goal){
+
+		 std::vector<double> params_robot=new_get_sfm_params(robot_);
+		 params_robot.at(0)=in_set_k_final_goal;
+		 if(change_k_robot_){
+			 set_sfm_to_robot( params_robot );
+		 }
+		 if(change_k_person_){
+			 set_sfm_to_person(params_robot);
+		 }
+		 if(change_k_person_comp_){
+			 set_sfm_to_person_companion( params_robot );
+		 }
+
+		 if(change_k_obst_){
+			 set_sfm_to_obstacle( params_robot );
+		 }
+
+	}
+
+	void set_change_k_robot(bool in_change_k_robot){
+		change_k_robot_=in_change_k_robot;
+	}
+
+	void set_change_k_person(bool in_change_k_person){
+		change_k_person_=in_change_k_person;
+	}
+
+	void set_change_k_person_comp(bool in_change_k_person_comp){
+		change_k_person_comp_=in_change_k_person_comp;
+	}
+
+	void set_change_k_obst(bool in_change_k_obst){
+		change_k_obst_=in_change_k_obst;
+	}
+
+	void set_final_max_v(double in_final_max_v){
+		final_max_v_=in_final_max_v;
+	}
+
+	double calc_person_companion_orientation_from_outside(SpointV actual_p1_point, Sdestination person_dest, Spose in_robot);
+
+	void set_current_pose_robot_akp(Spose in_current_pose_robot){
+		  robot_->set_current_pose(in_current_pose_robot);
+		  robot_->set_current_pointV(SpointV(in_current_pose_robot.x,in_current_pose_robot.y,in_current_pose_robot.time_stamp,in_current_pose_robot.v*cos(in_current_pose_robot.theta),in_current_pose_robot.v*sin(in_current_pose_robot.theta)));
+		  robot_->get_current_pose().print();
+	}
+
+	void plan_set_initial_v_robot_needed(double in_initial_v_robot_needed){
+		robot_->set_initial_v_robot_needed(in_initial_v_robot_needed);
+	}
+
+	Crobot* get_return_person_companion(){ //retorna el person_companion_ para el simulador, para hacer el restart directamente.
+		return person_companion_;
+	}
+
+	Spoint get_actual_goal_to_return_initial_position(){
+		return actual_goal_to_return_initial_position_;
+	}
+
+	  void set_metros_al_dynamic_goal_Vform(double in_metros){
+		  metros_al_dynamic_goal_Vform_=in_metros;
+		 std::cout <<";CHANGE metros_al_dynamic_goal_Vform_="<<metros_al_dynamic_goal_Vform_<< std::endl;
+	  }
+
+	  void set_save_in_file(bool in_save_in_file){
+		  save_in_file_=in_save_in_file;
+		  save_results_on_files_for_robot_=in_save_in_file;
+	  }
+
+	  void set_change_dynamic_of_final_dest(bool in_ignore_dynamically_change_finaldest){
+		  change_dynamically_finaldest_=in_ignore_dynamically_change_finaldest;
+	  }
+
+	  double get_max_v_by_system(){
+		  return max_v_by_system_;
+	  }
+
+	  Sdestination get_robot_destination(){
+		  return robot_dest_for_companion_status_;
+	  }
+
+	  bool get_obstacle_very_close_no_path(){
+		  return obstacle_very_close_no_path_;
+	  }
+
+
+	  void set_threshold_collision_dist_stop_pers_goal(double in_threshold_collision_dist_stop_pers_goal){
+		  threshold_collision_dist_stop_pers_goal_=in_threshold_collision_dist_stop_pers_goal;
+	  }
+
+	  void set_see_std_out_velocities(bool in_see_std_out_velocities){
+		  see_std_out_velocities_=in_see_std_out_velocities;
+	  }
+
+	  bool see_std_out_velocities_;
+
+	  void set_dist_to_approach_static_people(double in_dist_to_approach_static_people){
+		  dist_to_approach_static_people_=in_dist_to_approach_static_people;
+		  std::cout <<"; dist_to_approach_static_people_="<<dist_to_approach_static_people_<< std::endl;
+	 }
+
+	  void set_change_dynamic_goal_1_person_sidebyside(bool in_change_dynamic_goal_1_person_sidebyside){ // to use change of dynamic goal in the syede-by-side one person.
+		  change_dynamic_goal_1_person_sidebyside_=in_change_dynamic_goal_1_person_sidebyside;
+	  }
+
+	  bool change_dynamic_goal_1_person_sidebyside_;
+
+	  void set_see_save_in_file(bool in_see_save_in_file){
+		  see_save_in_file_=in_see_save_in_file;
+	  }
+
+	  bool get_case_akp_plan_por_freanado(){
+		  return case_akp_plan_por_freanado_;
+	  }
+
+
+  protected:
+
+    /**
+     * \brief this function calculates the planner path in the companion case, if no solution is
+     * found, then a false result is returned
+     */
+    bool robot_plan_anticipative_krrt_companion(Cperson_abstract::companion_reactive reactive=Cperson_abstract::Akp_planning);
+    /**
+   	 * \brief this function initializes the planner, selecting the persons to be considered
+   	 * and other things. If setting is not possible, i.e. goal is within current goal, etc..
+   	 * a false result is returned (COMPANION CASE)
+        */
+    bool init_robot_plan2(double robot_person_distance=1.5);
+
+    /**
+     * \brief this function samples the workspace
+     */
+    Spoint sample_workspace();
+
+    void reset_scene_persons_propagation_flag();
+    unsigned int find_nearest_vertex( const Spoint& random_goal );
+
+    /**
+     * \brief this function calculates the input required
+     * for the propagation to get the sampled position
+     */
+    Sedge_tree calculate_edge(unsigned int parent_vertex_index, const Sdestination& random_goal, Cperson_abstract::companion_reactive reactive =Cperson_abstract::Akp_planning);
+   // for person companion, planning akp
+    /**
+     * \brief this function propagates the selected node,
+     * if a collision is detected returns TRUE and if propagation
+     * was not possible, false
+     */
+    bool propagate_vertex( unsigned int parent_vertex_index , const  Sedge_tree& u,  Cperson_abstract::companion_reactive reactive =Cperson_abstract::Reactive_atractive);
+    /**
+     * \brief this function checks for collision for the propagated robot
+     * state either with static obstacles as well as persons (their prediction)
+     */
+    bool check_collision( const SpointV_cov& p2,  int t = -1 , SpointV_cov centre_group=SpointV_cov() );
+    double cost_to_go( const Spoint& random_goal, unsigned int i , Cperson_abstract::companion_reactive reactive=Cperson_abstract::Akp_planning);
+
+    Sforce force_persons_int_planning_virtual(Cperson_abstract* center , unsigned int t = 0, double min_dist2= 64.0, Cperson_abstract::companion_reactive reactive=Cperson_abstract::Akp_planning,bool use_person_companion_force_repulsive=false); // to do the group propagation
+    Sforce force_persons_int_planning_virtual_vers2comp(Cperson_abstract* center , unsigned int t_companion, unsigned int t_other_pers , double min_dist2, Cperson_abstract::companion_reactive reactive, bool use_person_companion_force_repulsive);
+    Sforce force_persons_int_planning_virtual_robot_prediction(Cperson_abstract* center , unsigned int t, double min_dist2=64.0, Cperson_abstract::companion_reactive reactive=Cperson_abstract::Akp_planning, unsigned int id_pers_comp_rob=0);
+
+    Sforce force_persons_int_planning_virtual_robot_companion_propagation(Cperson_abstract* center , unsigned int t= 0, double min_dist2= 64.0, Cperson_abstract::companion_reactive reactive=Cperson_abstract::Akp_planning); // the function is for akp planer companion. To do the robot propagation
+    // for person_companion that use the akp planning.
+
+    Sforce force_persons_int_planning_virtual_companion_person_akp_person_prediction(Cperson_abstract* center , unsigned int t=0, double min_dist2= 64.0, Cperson_abstract::companion_reactive reactive=Cperson_abstract::Akp_planning);
+
+    Sforce force_persons_int_robot_prediction_virtual(const SpointV& center , unsigned int t = 0, double min_dist2 = 16.0); // no tengo claro si se usa aun.
+    Sforce force_objects_laser_int_planning_virtual( Cperson_abstract* person, unsigned int planning_index = 0, double min_dist2=25.0, bool robot_collision_check_flag=false);
+    Sforce force_objects_laser_int_planning_virtual_robot_propagation( Cperson_abstract* person, unsigned int planning_index=0, double min_dist2=25.0, bool robot_collision_check_flag=false);
+
+    //Sforce force_objects_laser_int_planning_virtual_side_by_side2( Cperson_abstract* person, unsigned int planning_index=0, double min_dist2=25.0, bool robot_collision_check_flag=false);
+
+    void calculate_navigation_instant_work(  );
+
+    void calculate_cost( unsigned int parent_index , Sedge_tree u , Cperson_abstract::companion_reactive reactive=Cperson_abstract::Akp_planning);
+
+    //preprocess different solutions to calculate the best or bests solutions
+    unsigned int global_min_cost_index(   Crobot* robot_act, Cperson_abstract::companion_reactive reactive=Cperson_abstract::Akp_planning );
+    void preprocess_global_parameters(Crobot* robot_act);
+    void calculate_normalization_cost_functions_parameters_erf();
+    void calculate_normalization_cost_functions_parameters_norm(Crobot* robot_act);
+    void calculate_non_dominated_solutions();
+
+
+    //planning paramters
+    plan_mode plan_mode_;
+    distance_mode distance_mode_;
+    global_mode global_mode_;
+    unsigned int max_iter_;  // numero de nodos por path. de normal hay 40 nodos por path.
+    std::vector<double> cost_parameters_;
+    Spoint goal_,local_goal_, goal2_;
+    bool reaching_goal_;
+	//list of person to be considered in by the planner
+	std::list<Cperson_abstract*> nearby_person_list_;
+	std::vector<Spoint> nearby_obstacle_list_; //lista de obstaculos estaticos, solo para robot.
+	std::vector<Spoint> nearby_obstacle_list2_; // lista de obstaculos estaticos para companion (ely)
+	double workspace_radii_, std_goal_workspace_;
+	double max_v_by_system_, cost_angular_;
+	double xy_2_goal_tolerance_, v_goal_tolerance_, local_v_goal_tolerance_, distance_to_stop_;
+
+    // alpha and sum_ratio paramter to scape local minima
+    double alpha_,gamma_,delta_;
+    std::vector<double> v_alpha_,v_gamma_,v_delta_;
+
+    //random generator
+	std::default_random_engine generator_;
+
+    //tree structure data structures
+    std::vector<Sedge_tree> edge_;
+    std::vector<double> cost_int_forces_;
+    std::vector<double> cost_robot_;
+    std::vector<double> cost_obstacles_;
+    std::vector<double> cost_past_traj_;
+    std::vector<double> nodes_in_branch_;
+    std::vector<double> cost_distance_;
+    std::vector<double> cost_orientation_;
+    std::vector<Spoint> random_goals_;
+    std::vector<bool> collision_detected_;
+    double work_persons_, work_robot_;
+
+
+    //results data structures
+    std::vector<unsigned int> best_plan_vertex_index_, end_of_branches_index_, nondominated_plan_vertex_index_, nondominated_end_of_plan_vertex_index_;
+    std::vector<Spose> best_planning_trajectory_;
+    std::vector<double> best_costs_, mean_costs_, std_costs_;
+
+    //cost-to-go function values to measure real performance for testing
+    std::vector<double> cost_values_;
+
+    Spose last_pose_command_;//comand of the last velocity command send to the robot platform
+
+
+    //Multi-Objective optimization: normalization terms
+    double mean_cost_int_forces_,
+		mean_cost_robot_,
+		mean_cost_obstacles_,
+		mean_cost_past_traj_,
+		mean_cost_distance_,
+		mean_cost_orientation_,
+		mean_cost_companion_; // mean companion cost, add by ely.
+
+    double std_cost_int_forces_,
+		std_cost_robot_,
+		std_cost_obstacles_,
+		std_cost_past_traj_,
+		std_cost_distance_,
+		std_cost_orientation_,
+		std_cost_companion_; // std companion cost, add by ely.
+
+    //Multi-objective vector structure to find non-dominated sets
+    std::vector<Smulticost> multicosts_;
+    std::list<Smulticost> nondominated_multicosts_;
+
+    //to calculate probability of collision
+    double gaussian_constant_;
+    int ppl_collision_mode_,pr_force_mode_;
+
+
+    // ely functions
+    /**
+     * \brief this function calculates the companion cost for each node of the path.
+     */
+    void calculate_companion_cost_node(unsigned int parent_index, Crobot* robot_act); // function to calculate one node companion cost
+
+    bool check_collision_final( const Spoint& p ,  int t); // caso robot, moverse dentro de la burbuja!
+    /**
+     * \brief this function checks for collision for the propagated robot+person=couple
+     * state either with static obstacles as well as persons (their prediction).
+     */
+    bool check_collision_companion_reactive_atractive_goal( const SpointV_cov& p2 ,  int t); // return if have collision and the  minimum distance of the collision
+
+    /**
+     * \brief this function calculates the initial_increment_angle to move the robot around the person
+     * when they have to avoid obstacles in a joint way.
+     */
+    void ini_increment_angle();
+
+    void calculate_companion_path_angle_and_cost(unsigned int parent_index, double actual_angle);
+    /**
+     * \brief this function calculates the needed angle between person-and-robot
+     * for all the nodes of the best path, to obtain a progressive angle
+     *  to positioned the robot around the person. (case, go robot in front of the person)
+     */
+    /**
+     * \brief this function calculates the needed angle between person-and-robot
+     * for all the nodes of the best path, to obtain a progressive angle
+     *  to positioned the robot around the person. (case, go robot at the rear of the person)
+     */
+    void go_behind_robot_only_angle_in_final_tree_calculate_companion_path_angle_and_cost3(unsigned int i);
+
+    void go_in_front_robot_only_angle_in_final_tree_calculate_companion_path_angle_and_cost3(unsigned int i);
+
+    /**
+     * \brief this function is for see the best path with the companion angles and the companion cost.
+     */
+    void see_companion_path_angle_and_cost_of_min_cost_paths(double parent_index);
+    /**
+     * \brief this function modifies the distance between the person and robot couple to smooth the turns
+     * to put the robot in front or rear the person.
+     */
+    double calc_robot_person_companion_distance();
+    /**
+     * \brief this function replan the propagated positions of the robot for all
+     * the nodes of the best path, taking into account in the propagation the two goals for the robot:
+     * follow the path and goes to a concrete position respect to the person.
+     */
+
+    /**
+     * \brief this function replan the propagated position of the robot for the next step
+     * of the robot in the best path, taking into account in the propagation the two goals for the robot:
+     * follow the path and goes to a concrete position respect to the person.
+     */
+    Spose replan_last_step(unsigned int index,Spose robot_pose_ini, SpointV robot_Spoint_ini, Sdestination robot_goal, double dt);
+    /**
+     * \brief these function perform a different collision functions for the goal in the atractive or repulsive cases.
+     */
+    double check_collision_companion_goal(const Spoint& p ,  int t);
+
+    /**
+     * \brief this function set the actual max_velocity_ for the robot.
+     * this is the initial function only taking into account the planning for the robot.
+     */
+    void vel_robot_normal(double d_min);
+    /**
+     * \brief this function set the actual max_velocity_ for the robot.
+     * this function takes into account the planning for the robot-and-person=couple.
+     */
+    void vel_robot_companion(double d_min , double robot_person_distance, Spoint before_next_goal_of_robot=Sdestination(), Cperson_abstract::companion_reactive reactive=Cperson_abstract::Reactiva_repulsive, unsigned int parent_vertex=0);
+
+    //void calculate_actual_angle_person_robot(unsigned int index);
+    double calculate_actual_angle_person_robot(Spoint person, Spoint robot);
+    /**
+     * \brief this function returns the actual max_velocity_ to the max velocity of the robot.
+     *  this is to calculate the akp path with the max velocity. Then, in the recalculation of
+     *  the position for the best path, the velocity of the robot is modified taking into account
+     *  also the goal of remain in a exact position respect to the companion person.
+     */
+    void return_max_velocity_systemRobot_to_max_value();
+
+    /**
+     * \brief this function calculate the anisotropy for the group to do not take into account
+     * the collisions with obstacles at the rear of the group, for the robot positioning around the person.
+     * (now is set, anisotropy_limit=0, any anisotropy is take into account, we take into account rear collisions)
+     */
+    double calculate_anisotropy(const SpointV_cov& center_person, const Spoint& interacting_person , double lambda=0.2);
+    /**
+     * \brief this function calculates the distance between the person-and-robot in the moments where the robot
+     * moves around the person to avoid obstacles (put in front or behind). This function is to smooth the
+     * turns of the robot, facilitate the navigation and remove the misalignments with the person in orientation
+     * that provoke "s" movements for the robot.
+     */
+    double calculate_modif_person_robot_distance( double ite );
+    double calc_robot_person_companion_distance_companion_person_akp(); // case person companion planning akp
+    /**
+     * \brief this function print the results in a matlab file.
+     */
+    void printToMatlab();
+    void evaluate_costs_printToMatlab(Crobot* actual_robot);
+    void new_matlab_file_To_evaluate_costs();
+    void new_debug_file();
+    void new_matlab_file_To_evaluate_change_distance_and_angle();
+    void evaluate_change_distance_and_angle_companion_with_beta_change();
+
+	double calc_person_companion_orientation(); // return theta person companion in radians!
+	Sdestination calculate_companion_goal_stable_if_person_stop(double in_act_min_companion_angle);
+
+	Sdestination before_person_intersection_destination_;
+    // variables companion (ely)
+    bool debug_companion_;
+    double robot_person_companion_distance_;
+    double min_distance_collision_; // variables to calculate the companion cost and angle! => min_distance_collision_ and bool_collision_companion_
+    // min_distance_collision is the maximun group distance to pass throught obstacles without change the group side-by-side formation.
+    bool bool_collision_companion_; // variables to calculate the companion cost and angle!
+    double next_robot_pose_min_distance_collision_;
+    std::vector<double> orientation_person_robot_angles_; // angles to calculate the companion goal using the computed path of the group
+
+
+    std::vector<double> orientation_person_robot_angles_with_prediction_of_person_companion_; // angles to calculate the companion goal using the prediction of the companion person.
+    bool calc_goal_companion_with_group_path_; // if true == calc the angle of the goal companion with the group path, else calc the angle of the goal companion with the prediction of the person.
+	void fix_angles_to_use_person_prediction_for_the_companion_goal(); //function to fix the companion angles obtained with the person prediction.
+
+
+    // new costs (companion-ely)
+    std::vector<double> cost_companion_;
+    std::vector<unsigned int> parent_index_vector_;
+    std::vector<double> min_distance_collision_vector_;
+    std::vector<double> min_distance_collision_vector_from_pred_;
+    double min_next_companion_angle_; // esta en grados
+    double min_next_companion_angle_for_final_dest_;
+    double min_next_companion_cost_;
+    double angle_increment_of_increment_distance_;
+    double angle_increment_of_increment_distance_vuelta_al_reves_;
+
+    double before_initial_angle_;
+    double before_initial_cost_;
+    std::vector<unsigned int> BEST_path_parent_index_vector_;
+
+    unsigned int id_person_companion_;
+    unsigned int change_id_person_companion_;
+    Sdestination person_destination_;
+
+    SpointV_cov companion_person_position_; // posicion inicial de la persona a la que acompañamos.
+
+    Sdestination person_companion_goal_;
+    double robot_person_distance_;
+
+    double robot_person_proximity_distance_;  // distancia a la que esta robot y persona para que se olvide del goal el robot y se acerque a la persona.
+    double robot_person_proximity_tolerance_; // increment of distance around the proximity_distance to generate a distance margin. This is for do not always omit the planning and aproximate to the person.
+    double robot_person_proximity_goals_x_; // distancia entre goal de la persona que acompaña y goal al que se dirige el robot.
+    double robot_person_proximity_goals_y_;
+    double initial_angle_;
+    Spose robot_initial_pose_;
+    SpointV_cov initial_robot_spoint_;
+    SpointV_cov initial_person_companion_point_;
+
+    Cperson_abstract* pointer_to_person_companion_;
+    bool we_have_pointer_to_first_person_; // if true, we can use the pointer!!!
+
+    std::vector<Spoint> random_goals2_;
+    unsigned int min_branch_index_save_;
+
+    bool debug_antes_subgoals_entre_AKP_goals_;
+
+    bool final_collision_check_;
+
+    bool actual_debug_;
+    bool actual_debug2_;
+    double offset_atractive_;
+
+    double time_act_;
+
+    double robot_adition_complete_esphere_companion_distance_;
+
+    double f_obst_max_x_; // =0.1
+    double f_obst_max_y_; //=0.1
+
+    bool companion_same_person_goal_;
+    Sdestination extern_robot_goal_;
+    Sdestination extern_robot_goal_fix_;
+
+    double angle_companion_;
+    double person_goal_percent_;
+    double marge_angle_companion_;
+    Sdestination person_companion_goal_out_;
+
+    double dmin_global_;
+   bool robot_orient_;
+   double person_orient_;
+
+   double vel_per_max_caso_robot_a_0_o_180_grados_;
+
+   bool debug_real_test_companion_;
+   bool debug_real_test_companion2_;
+   bool debug_real_test_companion3_;
+   bool debug_real_test_companion4_;
+
+   double marge_in_distance_; // margen en distancia para seguir a una persona.
+   bool overpas_obstacles_behind_person_;
+   bool we_have_cost_companion_;
+
+   double anisotropy_threshold_;
+
+   Cperson_abstract::companion_reactive robot_companion_case_;
+   bool check_execution_times_;
+
+   double person_robot_actual_real_distance_;
+   double little_augmented_collision_margin_;
+   double person_radi_;
+   double person_radi2_;
+   double person_radi_amp_;
+   double max_distance_to_obstacles_detected_;
+   bool see_forces_;
+  double obstacle_radi_;
+  double obstacle_radi2_;
+  double obstacle_radi_amp_;
+
+  Sedge_tree u_forces_robot_actual_; // solo para visualizacion en Rviz
+  Cperson_abstract::vel_per_ok vel_per_ok_; //boolean, true= robot navigate with the person velocity, else = robot navigate with the maximun velocity.
+
+
+  // Variables to save in matlab file!
+  double real_distance_person_robot_;
+  double ideal_distance_person_robot_;
+  double real_angle_person_robot_;
+  double ideal_angle_person_robot_;
+  double before_ideal_angle_person_robot_;
+  bool first_ideal_angle_;
+  double actual_time_;
+  double robot_distance_cost_;
+  double robot_orientation_cost_;
+  double robot_control_cost_;
+  double robot_other_person_cost_;
+  double robot_obstacles_cost_;
+  double robot_companion_cost_;
+  double robot_total_cost_;
+  double robot_ant_traj_cost_;
+  double person_distance_to_goal_;
+  double robot_distance_to_goal_;
+
+    double last_step_robot_other_person_cost_; // coste respecto a fuerzas de otras personas que se ejercen en el robot.
+    double last_step_robot_obstacles_cost_;  // coste respecto a fuerzas de obstaculos que se ejercen en el robot.
+    double last_step_robot_companion_cost_; // coste respecto a fuerzas de la companion person que se ejercen en el robot.
+    double last_step_robot_control_cost_mix_;
+    double last_step_robot_control_cost_goal_traj_;
+    double last_step_robot_control_cost_goal_person_;
+    double last_step_robot_orientation_cost_local_;
+    double last_step_robot_orientation_cost_global_;
+    double last_step_robot_distance_cost_;
+
+    double other_people_due_to_robot_cost_;
+    double companion_person_due_to_robot_cost_;
+
+    bool sim;
+    bool sim_target_per;
+    bool first_time_;
+    unsigned int iteration_;
+    unsigned int experiment_;
+
+    SpointV_cov before_initial_robot_spoint_;
+    double inc_distance_ant_;
+    double acum_time_;
+    double time_ant_;
+
+    bool save_results_in_file_;
+    double meters_goal_to_save_results_in_file_;
+
+    std::string results_file_,results_file_real_;
+    double reduce_max_vel_dist_;
+
+    bool go_with_vel_per_;
+
+    bool debug_file_robot_;
+
+
+
+    std::string debug_file_;
+
+    bool case_dynamic_;
+    double externa_force_k_near_goal_akp_;
+    double externa_force_k_far_goal_akp_;
+    double max_dist_to_near_goal_force_akp_;
+    bool mode_step_near_;
+
+    unsigned int out_index_step_final_dest_goal_;
+	unsigned int out_index_step_companion_goal_;
+
+    bool only_comp_people_vel_and_robot_poses_;
+    std::vector<bool> vector_of_companion_collisions_;
+    bool debug_angles_;
+
+
+    bool debug_person_companion_general_;
+    bool debug_person_companion_increment_angle_;
+    bool debug_init_robot_plan2_person_companion_;
+    bool debug_robot_plan_anticipative_krrt_companion_person_companion_;
+    bool debug_calculate_edge_person_companion_akp_;
+    bool debug_get_best_planned_pose_person_companion_akp_;
+    bool debug_correct_angle_person_init_robot2_;
+    bool debug_correct_angle_person_replan_min_cost_branch_;
+    bool debug_correct_angle_person_replan_last_step_;
+    bool debug_correct_angle_person_calculate_edge_;
+    bool debug_correct_angle_person_get_best_planned_pose_;
+    bool debug_correct_angle_person_vel_robot_companion_;
+    bool debug_correct_angle_person_calculate_actual_angle_person_robot_;
+    bool debug_correct_angle_person_print_to_matlab_;
+
+    bool bool_case_person_companion_;
+
+    int number_of_obstacles_;
+    int number_obstacles_big_force_;
+	unsigned int num_steps_orientation_;
+
+
+	double alpha_companion_; // (weight FINAL_GOAL) alpha to replan the last step for the companion task ( 0.4 ) anterior 0.1.
+	double beta_companion_;  // (weight COMPANION_GOAL) beta to replan the last step for the companion task ( 0.6 )
+	double gamma_companion_; //  (weight repulsive force other people) gamma to replan the last step for the companion task (0.5) anterior 5.0
+	double delta_companion_; //  (weight repulsive force static obstacles) delta to replan the last step for the companion task (0.5)
+
+	double threshold_dintace_select_person_side_to_go_; // al principio esta distancia minimo ha de ser 0.5 metros al lado de la persona.
+	bool debug_select_person_side_to_go_with_more_free_space_;
+
+	bool chose_better_side_to_acompani_person_;
+
+	SsavePath_multiple_paths_and_best_path iter_act_multiple_paths_and_best_path_;
+	int planner_iterations_;
+
+    std::string evaluate_costs_file_;
+    std::string evaluate_change_distance_and_angle_companion_file_;
+
+    double orientacion_persona_actual_;
+	double person_x_ant_;
+	double person_y_ant_;
+
+	bool debug_file_evaluate_costs_;
+
+	// to visualice goals:
+	Sdestination robot_goal_to_person_companion_; // guardar estos en el archivo por cada iteracion.
+	Sdestination robot_path_goal_;
+	Spose final_combined_goal_;
+
+	double save_distance_between_person_and_robot_;
+	double save_angle_between_person_and_robot_;
+	unsigned int iter_d_;
+
+	Spoint person_position_t_4_;
+	double next_companion_angle_save_;
+	double before_act_companion_angle_;
+
+	calculate_complete_group_path_ calculate_complete_group_path_var_;
+
+	bool first_;
+
+	action_mode Action_;
+	simulation_case actual_case_;
+	Spoint actual_goal_to_return_initial_position_;
+
+	// To have several iterative experiments to simulate.
+	Spose SIM_initial_robot_pose1_;
+	Spose SIM_initial_robot_pose2_;
+	Spose SIM_initial_robot_pose3_;
+	Spose SIM_initial_robot_pose4_;
+
+	Spose SIM_initial_person_companion_pose1_;
+	Spose SIM_initial_person_companion_pose2_;
+
+	unsigned int global_iter_to_change_simulation_case_;
+
+	bool first_in_itter_;
+	double ros_time_to_sec_;
+	bool first_time_case1_;
+	bool first_time_case2_;
+
+	unsigned int goal1_;// goal number to do.
+
+	Spose center_of_the_group_;
+
+	double save_computational_time_value_;
+	double clocks_start, clocks_end;
+	clock_t robot_plan_companion2_start_ITER, robot_plan_companion2_end_ITER,robot_plan_companion3_start_ITER, robot_plan_companion3_end_ITER;
+
+	bool go_behid_comp_person_;
+
+    double last_true_person_companion_orientation_;
+
+    bool restart_real_;
+
+
+	Cperson_abstract *actual_person_Companion_pointer_;
+
+	SpointV_cov actual_person_Companion_SpointV_;
+	Sdestination actual_person_Companion_destination_;
+	double max_desired_person_Companion_velocity_; // todo: add initial bucle: max_desired_person_Companion_velocity_=iit->get_desired_velocity();
+
+	double d_min_global_;
+	double radii_person_list_include_person_companion_;
+
+	// variable save results on files or not! OJO!!! cuando quiera los datos hay que activarla!!!
+	bool save_results_on_files_for_person_companion_;
+	bool save_results_on_files_for_robot_; // only do not save the results on files for the robot
+	bool save_in_file_; // disable also the iterations!!! not only save the results on files. (only for robot)
+
+	// for performance:
+	double value_distance_global_path_;
+	double global_path_ini_orientation_;
+	double global_path_final_orientation_;
+
+	double multiply_person_companion_force_to_persons_;
+	double threshold_min_vel_person_to_obtain_destination_;  // threshold_min_vel_person_to_obtain_destination_=0.2
+
+	double last_good_theta_person_;
+	bool firts_iter_obtain_angle_person_companion_;
+
+	double saved_actual_theta_person_;
+
+	bool debug_gazebo_journal_;
+	bool debug_gazebo_journal2_;
+
+	Sdestination ideal_companion_goal_in_the_next_instant_of_time_; // serve to stop robot when person stop and the robot is in the best companion point for this stop person. (para esta persona parada)
+	Sdestination before_companion_person_destination_;
+
+	double clocks_per_sec_my_var_; // clocks_per_sec_my_var_=1000000
+
+	double test_distance_to_goal_;
+
+	bool final_goal_reached_in_node_;
+	bool change_final_robot_orientation_; //
+	bool is_case_akp_true_;
+	bool enter_on_cero_;
+	bool enter_on_cero2_;
+	bool result_;
+
+	double dist_to_goal_test_;
+	double v_to_goal_test_;
+	bool use_persoon_companion_to_calculate_plan_;
+
+	Spoint before_next_goal_of_robot_; // (is the next immediate final goal to accompany the person) new variable to adjust well the robot velocity to accompany the person. (pure companion, do not takes into account the final goal for the velocity)
+
+	unsigned int number_of_group_people_;
+
+	std::vector<double> person1_repulsive_force_fx_;
+	std::vector<double> person1_repulsive_force_fy_;
+
+	std::vector<double> person1_companion_force_fx_;
+	std::vector<double> person1_companion_force_fy_;
+
+	double person1_repulsive_force_fx_act_;
+	double person1_repulsive_force_fy_act_;
+
+	double person1_companion_force_fx_act_;
+	double person1_companion_force_fy_act_;
+
+	double ori_pers_x_;
+	double ori_pers_y_;
+
+	double final_compt_time_;
+	Spose final_pose_robot_;
+
+	clock_t group_force_start, group_force_end, rrt_start_ITER,rrt_end_ITER,initial_part_start,initial_part_end,last_part_start,last_part_end;
+
+	SpointV center_of_the_group_Zanlungo_; // hacen falta. Se usan tambien para 1 person companion.
+	SpointV mean_people_Zalungo_;  // hacen falta. Se usan tambien para 1 person companion.
+	double theta_of_the_group_Zanlungo_;  // hacen falta. Se usan tambien para 1 person companion.
+
+	double mean(std::vector<double> arr, int n);   // Function to find mean.
+	double variance(std::vector<double> a, int n); // Function to find variance.
+	double covariance(std::vector<double> arr1, std::vector<double> arr2, int n); // Function to find covariance.
+
+	double a_,b_; // parameters y=a+bx of the linear regression to find the preferred velocity of the robot from the actual person companion velocity. For the group is the mean of the velocity of the two people.
+
+	double treshold_distance_between_steps_;
+	double real_vel_per;
+	bool change_k_robot_,change_k_person_,change_k_person_comp_,change_k_obst_;
+	bool initial_goal_case_bool_;
+
+	Sdestination last_people_dest_;
+	Spoint las_people_dest_spoint_;
+	bool initial_not_set_last_people_dest_;
+
+	double minimun_velocity_case_stop_initial_;
+	double final_max_v_;
+
+	bool bool_out_init_pose_robot_;
+
+	Sdestination before_person_comp_goal_;
+
+	Spoint SIM_initial_person_goal_pose1_,SIM_initial_person_goal_pose2_,SIM_initial_person_goal_pose3_;
+
+	double metros_al_dynamic_goal_Vform_;  // distance from the goal of the environment to tup the external dynamic goal.
+
+	// Nuevas funciones y variables cambio modelo side-by-side:
+	SpointV calculate_centre_of_the_group(SpointV robot, std::vector<SpointV> people);  // std::vector<double> people; => pose 0 => P1, pose 1 => P2
+	Spoint calculate_goal_translated_of_group_element(SpointV group_centre, SpointV group_element, Spoint group_goal, Spoint distRP, double theta_robot, unsigned int parent_vertex);
+	SpointV_cov calculate_element_position_translated_in_group(SpointV_cov robot_plan_act, SpointV_cov person_propagation_act);
+	    // quizas las de translate goal las uso tambien con la person companion de 1 persona sola. No borrar.
+
+	    // funcion que calcula la desred velocity del robot para el goal actual. person_propagation==goal al que quiero llegar.
+	    // robot== mi posicion actual. vel_desired_robot_max== es la velocidad del robot maxima.
+	    // vel_person_companion es la velocidad de la persona que acompaña el robot (en principio es la velocidad media, pero no descarto que necesite usar
+	    // la velocidad puntual.
+	double calc_desired_robot_velocity_act(SpointV robot, SpointV person_propagation, double vel_person_companion);
+	void calculate_initial_companion_angle_case_side_by_side_one_person(double theta, double angle2);
+	void calculate_correct_initial_angle(Cperson_abstract* person_obj1, SpointV_cov person1, Spose robot, double &theta);
+	void set_robot_velocity_using_obstacles(double vel_per, double near_obst, double actual_V_Robot, double distance_PR7, double robot_person_distance, double d_min, double min_dist_colli_act_global2, double circunferencia_tope, double margen_angle_colision, double angle_colision); // funcion que setea la velocidad del robot a la deseada actual + alguna variable global relacionada con la velocidad que necesita el robot.
+
+	double metros_reduce_vel_; //update velocity robot to be adjusted to person companion
+
+	Spose robot_saved_to_time_v2_;
+
+	// variables para performances (guardar por seguridad, la distancia de colision actual y la distancia actual entre robotypersona para calcular la distancia del grupo.
+	// que con estas dos obtenemos el angulo ideal de acompañamiento.
+	std::vector<double> final_min_collision_distance_vector_;
+	std::vector<double> real_robot_person_distance2_vector_;
+
+	double save_matlab_final_min_collision_distance_vector_;
+	double save_matlab_real_robot_person_distance2_vector_;
+
+	double calc_person_companion_orientation_v2( Cperson_abstract* person_obj); // Calcula la orientacion de cualquier persona.
+	// variables globales relacionadas con la orientacion de P1 y P2, las dos personas que acompaña el robot.
+	double last_good_theta_person_p1_,ori_pers_x_p1_,ori_pers_y_p1_; // last_good_theta_person_p2_,ori_pers_x_p2_, ori_pers_y_p2_ last_good_theta_person_p1_ esta en radianes.
+	Sdestination before_person_comp_goal_p1_;//,before_person_comp_goal_p2_;
+	SpointV last_robot_pose_to_stop_;
+
+	double no_save_cost_and_dist_; // para no salvar esos datos y solo salvar los datos de performaces y etc.
+	double before_vp1_module_;
+
+	bool case_akp_plan_por_freanado_;//,scout_minimos_out_;
+	bool debug_sideBySide2019_;
+
+	double distance_frenar_sideBySide_when_person_is_stop_;
+
+	bool init_plan_approach_side_;
+
+	bool change_dynamically_finaldest_;
+
+	// these next variables are for companion status.
+	bool obstacle_very_close_no_path_;
+	double min_obstacle_dist_obst_static_,min_obstacle_dist_obst_dynamic_, threshold_collision_dist_stop_pers_goal_;
+	Sdestination robot_dest_for_companion_status_;
+
+	double litlle_augment_vel_desired_adapted_by_ve_per_for_robot_; // this augment is to compensate the delay that the robot has from little errors and delais.
+
+	double dist_to_approach_static_people_, mean_people_velocity_;//,threshold_min_dist_needed_to_robot_can_stay_at_central_position_;
+
+	bool see_save_in_file_;
+	bool output_screen_messages_;
+
+	SpointV calcular_goal_frenado(){
+		//  INI calcular goal final caso frenado.
+		if(output_screen_messages_){
+		std::cout << "Entro en calcular frenado."<< std::endl;
+		}
+		// Paso 1: propagar el goal actual de la persona, encima suyo a un metro hacia delante del robot para frenar suave.
+		// We want to stop slowly, using one meter of distance.
+		double dt_stop=2;//1/robot_->get_current_pointV().v(); //(tiempo para V=0, parando dentro de 1m )
+		SpointV robot_act=robot_->get_current_pointV();
+
+		Sforce force_to_stop=Sforce(max_v_by_system_/dt_stop,max_v_by_system_/dt_stop);
+		SpointV new_prop_p1_to_goal=pointer_to_person_companion_->get_current_pointV(); //TODO: probar de guardar el Spoint del robot, cuando la persona paro y usar ese para propagar...!? Aunque a la que sera Vr=0,
+
+
+		double vxn = max_v_by_system_*cos(last_good_theta_person_p1_) + force_to_stop.fx*dt_stop;
+		double vyn = max_v_by_system_*sin(last_good_theta_person_p1_) + force_to_stop.fy*dt_stop;
+		double dx = new_prop_p1_to_goal.x + max_v_by_system_*cos(last_good_theta_person_p1_)*dt_stop + force_to_stop.fx*dt_stop*dt_stop*0.5;
+		double dy = new_prop_p1_to_goal.y + max_v_by_system_*sin(last_good_theta_person_p1_)*dt_stop + force_to_stop.fy*dt_stop*dt_stop*0.5;
+
+
+		//std::cout << "(FRENADO) robot_act.vx=" << robot_act.vx << "robot_act.vy=" << robot_act.vy<<"; force_to_stop.fx="<<force_to_stop.fx<<"; force_to_stop.fy="<<force_to_stop.fy <<"; robot_act.x="<<robot_act.x<<"; robot_act.y="<<robot_act.y << std::endl;
+
+		//apply constrains:
+		double v = sqrt( vxn*vxn + vyn*vyn );
+		if ( v > max_v_by_system_)
+		{
+			vxn *= max_v_by_system_ /v;
+			vyn *= max_v_by_system_ /v;
+			dx = vxn*dt_stop;
+			dy = vyn*dt_stop;
+			//std::cout << "( v > desired_velocity) "<< std::endl;
+
+		}
+		//std::cout << "( v > desired_velocity) vxn="<<vxn<<"; vyn"<<vyn<<"; x="<<x<<"; y="<<y<<"; x+dx="<<x+dx<<"; y+dy="<<y+dy<<"; time_stamp+dt="<<time_stamp+dt<<"; desired_velocity="<<desired_velocity<< std::endl;
+
+
+		new_prop_p1_to_goal=SpointV( pointer_to_person_companion_->get_current_pointV().x+dx,pointer_to_person_companion_->get_current_pointV().y+dy, last_robot_pose_to_stop_.time_stamp+dt_stop, vxn, vyn);
+
+		if((new_prop_p1_to_goal.distance(pointer_to_person_companion_->get_current_pointV())<0.3)&&(robot_->get_current_pointV().distance(pointer_to_person_companion_->get_current_pointV())<3)&&(pointer_to_person_companion_->get_current_pointV().v()<0.2)){ //
+			std::cout <<"pose robot: (FRENADO) dt_stop="<<dt_stop<< " new_prop_p1_to_goal.x="<<new_prop_p1_to_goal.x<<"; new_prop_p1_to_goal.y="<<new_prop_p1_to_goal.y<<"; person.x="<<pointer_to_person_companion_->get_current_pointV().x<<"; person.y="<<pointer_to_person_companion_->get_current_pointV().y<< std::endl;
+			std::cout <<"pose robot: (FRENADO) dist<0.3="<<new_prop_p1_to_goal.distance(pointer_to_person_companion_->get_current_pointV())<< " dist2<3m="<<robot_->get_current_pointV().distance(pointer_to_person_companion_->get_current_pointV())<<"; vp<0.2="<<pointer_to_person_companion_->get_current_pointV().v()<< std::endl;
+
+			new_prop_p1_to_goal=robot_->get_current_pointV();
+		}
+
+		// Ahora deberia entrar siempre en este.
+		if(we_have_pointer_to_first_person_){ /// &&(number_of_group_people_<2) CASO 1 persona en side-by-side (lo otro es para 2 personas.
+				// TODO: no funciona bien!!! pesar si lo puedo arreglar para el caso de 1 persona, pero con calma.
+			double angle=atan2(robot_act.y-new_prop_p1_to_goal.y , robot_act.x-new_prop_p1_to_goal.x);
+				double new_angle_now;
+				if( diffangle(last_good_theta_person_p1_, angle) < 0 ){  // se corresponde con los dos laterales del robot, respecto a las personas.
+
+					 new_angle_now=last_good_theta_person_p1_-90*3.14/180;
+					if(new_angle_now>(3.14*2)){
+						new_angle_now=new_angle_now-(3.14*2);
+					}
+					if(new_angle_now<0){
+						new_angle_now=new_angle_now+(3.14*2);
+					}
+					//goal_robot=Spoint(person_companion_goal.x+robot_person_proximity_goals_x_*cos(new_angle_now),person_companion_goal.y+robot_person_proximity_goals_y_*sin(new_angle_now),person_companion_goal.time_stamp);
+
+
+				}else{  //fin if diff-angle.
+
+					 new_angle_now=last_good_theta_person_p1_+90*3.14/180;
+					 if(new_angle_now>(3.14*2)){
+						 new_angle_now=new_angle_now-(3.14*2);
+					 }
+					 if(new_angle_now<0){
+						 new_angle_now=new_angle_now+(3.14*2);
+					 }
+					//goal_robot=Spoint(person_companion_goal.x+robot_person_proximity_goals_x_*cos(new_angle_now),person_companion_goal.y+robot_person_proximity_goals_y_*sin(new_angle_now),person_companion_goal.time_stamp);
+
+				} // fin-if diff angle.
+				//new_prop_p1_to_goal=SpointV( robot_act.x+dx,robot_act.y+dy, robot_act.time_stamp+dt_stop, vxn, vyn);
+				dx = new_prop_p1_to_goal.x + max_v_by_system_*cos(new_angle_now)*dt_stop;// + force_to_stop.fx*dt_stop*dt_stop*0.5;
+				dy = new_prop_p1_to_goal.y + max_v_by_system_*sin(new_angle_now)*dt_stop;// + force_to_stop.fy*dt_stop*dt_stop*0.5;
+				if(output_screen_messages_){
+				std::cout << "(FRENADO) last_good_theta_person_p1_=" << last_good_theta_person_p1_<<"; new_angle_now="<<new_angle_now<< std::endl;
+				}
+				new_prop_p1_to_goal=SpointV(dx,dy, last_robot_pose_to_stop_.time_stamp+dt_stop, vxn, vyn);
+
+		}
+
+
+
+		return new_prop_p1_to_goal;
+
+		//  FIN calcular goal final caso frenado.
+	}
+
+
+
+};
+
+
+#endif /* PLANN_LOCAL_NAV_H_ */
+
diff --git a/local_lib_people_prediction/src/nav/plan_local_nav_person_companion.cpp b/local_lib_people_prediction/src/nav/plan_local_nav_person_companion.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4f446e0021269ded594ca5de862f10e1c48d862e
--- /dev/null
+++ b/local_lib_people_prediction/src/nav/plan_local_nav_person_companion.cpp
@@ -0,0 +1,13051 @@
+/*
+ * plan_local_nav_person_companion.cpp
+ *
+ * Created on: Jan 5, 2019  by Ely Repiso to separate the elements of the Scene_sim for the person companion, from the planner elements of the robot.
+ *
+ */
+
+#include "nav/plan_local_nav_person_companion.h"
+#include <algorithm>
+#include <iostream>
+#include <time.h>
+#include <math.h>
+
+// mirar todo lo de con target o goal (para la target person)
+// ojo con las fuerzas repulsivas respecto a la target person.
+// group_go_to_interact_with_other_person_
+// id_target_person o id_goal_person o permutados los dos nombres.
+
+
+Cplan_local_nav_person_companion::Cplan_local_nav_person_companion(double horizon_time, unsigned int max_iter,
+		plan_mode mode, bool robot_or_person) :
+//Cprediction_behavior(horizon_time),
+Cprediction_behavior(horizon_time,false, Cperson_abstract::Linear_regression_filtering),
+//Cprediction_behavior(horizon_time,false, Cperson_abstract::Low_pass_linear_regression_filtering),
+bool_change_ct_and_cr_with_vel_(false),
+bool_new_velocity_system_(true),
+No_change_ids_(false),
+plan_mode_(mode), distance_mode_(Cplan_local_nav_person_companion::Euclidean), global_mode_(Cplan_local_nav_person_companion::Scalarization),
+max_iter_(max_iter), workspace_radii_(0.0), std_goal_workspace_(1.0), max_v_by_system_(0.8), cost_angular_(0.25), // max_v_by_system_(0.8)
+xy_2_goal_tolerance_(0.04), v_goal_tolerance_(0.05), distance_to_stop_(1.0),
+alpha_(1.0), gamma_(1.0), delta_(1.0),
+last_pose_command_(Spose()),
+std_cost_int_forces_(1.0),
+std_cost_robot_(1.0),
+std_cost_obstacles_(1.0),
+std_cost_past_traj_(1.0),
+std_cost_distance_(1.0),
+std_cost_orientation_(1.0),
+gaussian_constant_( 2.0/(2.0*PI) ),
+ppl_collision_mode_(0),
+pr_force_mode_(0),
+debug_companion_(false),
+calc_goal_companion_with_group_path_(true),
+min_next_companion_angle_(120), // va a 90
+min_next_companion_cost_(0),
+before_initial_angle_(120), // va a 90
+before_initial_cost_(0),
+debug_comanion_good_(false),
+id_person_companion_(1), // a definir desde fuera luego...
+change_id_person_companion_(1),
+person_colision_radi_(0.3), // antes era 0.5
+robot_person_proximity_distance_(1.5), // ANTES 1.5; ANTES 2 antes 1.5 (pruevas con valor 3 iban bien, pero choca con obstaculos)
+robot_person_proximity_tolerance_(0.25), //  0.25, para evitar oscilaciones por posición muy exacta.  ANTES 0.75 antes 0.5 (pruevas con valor 3 iban bien, pero choca con obstaculos)
+robot_person_proximity_goals_x_(0.75), // ANTES x=(1.5)/2
+robot_person_proximity_goals_y_(0.75), // ANTES y=(1.5)/2
+case_robot_companion_(true),
+debug_antes_subgoals_entre_AKP_goals_(false),
+actual_debug_(false),
+actual_debug2_(false),
+offset_atractive_(1),  // antes era => offset_atractive_=1.
+time_act_(0),
+//time_ant_(0),
+robot_adition_complete_esphere_companion_distance_(1.0),//( [antes_ valor BUENO a 6.5] [antes_ valor a 3.5] antes 2.25; 2=1+1.0 de seguridad por cada lado)faltaria un metro de distancia más para los 0.5 y 0.5 más de persona y robot para no chocar.
+f_obst_max_x_(0.5),
+f_obst_max_y_(0.5),
+companion_same_person_goal_(false), // if true, infiere para el robot el mismo goal de la persona. el goal es la prediccion de hacia donde va la persona.
+angle_companion_(120), // by default, the angle companion is 90 degrees
+person_goal_percent_(0.5),
+marge_angle_companion_(10),
+//angle_companion_temp_movil_(90),
+debug_nadal_(false),
+debug_nadal2_(false),
+vel_per_max_caso_robot_a_0_o_180_grados_(0),
+debug_real_test_companion_(false),
+debug_real_test_companion2_(false),
+debug_real_test_companion3_(false),
+debug_real_test_companion4_(false),
+marge_in_distance_(0.3),
+overpas_obstacles_behind_person_(true), // true== the robot pass obstacles behind person => is changed on dynamic reconfigure!
+we_have_cost_companion_(false),
+anisotropy_threshold_(0.2), /// anisotropy_threshold_=0.5 SIMULACION, real=> anisotropy_threshold_=0.2
+check_execution_times_(false),
+little_augmented_collision_margin_(0.75), // SIMULACION: sim= 1.5 Final: 1.5/2=0.75=> +0.25 de robot_person_proximity_tolerance_ +0.5 de colchon entre obstaculos que ayude a poder maniobrar y no chocar.     Antes: +1, para tener en cuenta colision con radio de obstaculos por cada lado. +0.25 de colchon más por cada lado de la circunferencia para no rozar.
+person_radi_(0.5), //simulacion: person_radi_amp_=0.5 ; real=0.4
+person_radi2_(0.5), // simulacion: person_radi_amp_=0.4 ; real=0.3
+person_radi_amp_(0.5), // simulacion: person_radi_amp_=0.6 ; real=0.4
+person_radi_per_comp_(0.5),// simulacion: person_radi_amp_=0.5 ; real=0.4
+see_forces_(false),
+obstacle_radi_(0.4), // en simulacion este y el siguiente estan a 0.3 // en real es 0.0
+obstacle_radi2_(0.4), // en simulacion este y el siguiente estan a 0.3 // en real es 0.0
+obstacle_radi_amp_(0.4),  // en simulacion sim=0.4, en real= 0.0 OJO! se cambia en el cfg del nodo!!!
+u_forces_robot_actual_(Sedge_tree_pcomp(1)),
+sim(false), // simulacion=> sim=true. REAL => sim=false
+sim_target_per(false), // =true only simulation. False is for real experiments.  // simulacion=> sim_target_per=true. REAL => sim_target_per=false
+iteration_(1),
+experiment_(24),
+inc_distance_ant_(0),
+acum_time_(0),
+time_ant_(0),
+save_results_in_file_(true),
+meters_goal_to_save_results_in_file_(1.0), // 5 m for simulation. In real is better put 0 m to save every thing and then cut the files when we want.
+mode_velocity_(true),
+reduce_max_vel_dist_(1.0), // antes era 1! ojo!
+debug_file_robot_(false),
+debug_cout_robot_(false),
+only_comp_people_vel_and_robot_poses_(true),
+debug_angles_(false),
+ini_vel_to_increment_angle_(0.2),
+debug_person_companion_general_(false),
+debug_person_companion_increment_angle_(false),
+debug_init_robot_plan2_person_companion_(false),
+debug_robot_plan_anticipative_krrt_companion_person_companion_(false),
+debug_calculate_edge_person_companion_akp_(false),
+debug_correct_angle_person_init_robot2_(false),
+debug_correct_angle_person_replan_min_cost_branch_(false),
+debug_correct_angle_person_replan_last_step_(false),
+debug_correct_angle_person_calculate_edge_(false),
+debug_correct_angle_person_get_best_planned_pose_(false),
+debug_correct_angle_person_vel_robot_companion_(false),
+debug_correct_angle_person_calculate_actual_angle_person_robot_(false),
+debug_correct_angle_person_print_to_matlab_(false),
+bool_case_person_companion_(false), // por defecto false para el robot
+num_steps_orientation_(15),
+alpha_companion_(0.5), // antes era 0.1. (modelo companion side-by-side= 0.2; modelo zanlungo =0.5 )
+beta_companion_(0.5), // antes era 0.6. (modelo companion side-by-side= 0.8; modelo zanlungo =0.5 )
+gamma_companion_(1.0), // antes era 5!!! pero es demasiado reactivo con las personas en el companion!
+delta_companion_(0.5),
+threshold_dintace_select_person_side_to_go_(robot_person_proximity_distance_/2), // al principio esta distancia minimo ha de ser 0.5 metros al lado de la persona.
+debug_select_person_side_to_go_with_more_free_space_(false),
+chose_better_side_to_acompani_person_(false), // es true, para que se posicione bien.
+iter_act_multiple_paths_and_best_path_(),
+planner_iterations_(0),
+debug_file_evaluate_costs_(false),
+iter_d_(0),
+next_companion_angle_save_(120),
+before_act_companion_angle_(120),
+calculate_complete_group_path_var_(person_prediction),
+first_(true),
+Action_(Cplan_local_nav_person_companion::ITER),
+global_iter_to_change_simulation_case_(1),
+first_in_itter_(true),
+first_time_case1_(true),
+first_time_case2_(true),
+is_act_person_companion_(false),
+go_behid_comp_person_(true),
+restart_real_(false),
+max_dist_to_go_behind_(2),  // real=2  ;   sim=1.5
+case_stop_giro_(true),
+incremento_giro_positivo_(0.1),
+incremento_giro_negativo_(-0.1),
+complete_traj_prediction_to_goal_(false),// TODO: out, only companion
+save_results_on_files_for_person_companion_(false),
+save_results_on_files_for_robot_(false),
+save_in_file_(false),
+multiply_person_companion_force_to_persons_(7),
+final_debug_journal_(false),
+threshold_min_vel_person_to_obtain_destination_(0.1),
+firts_iter_obtain_angle_person_companion_(true),
+debug_gazebo_journal_(false),
+debug_gazebo_journal2_(false),
+clocks_per_sec_my_var_(1000000),
+final_goal_reached_in_node_(false),
+change_final_robot_orientation_(false),
+change_goal_of_the_error_(2),
+companion_angle_peopl_in_group_(120),
+real_distance_between_people_of_group_(2.0),
+constante_multiplicar_fuerza_goal_(1),
+bool_distance_margin_(false),
+dis_tol_(0.3),
+dis_tol_side_(0.3),
+threshold_vel_pers_to_stop_(0.1),
+dist_bet_rob_ext_goal_to_stop_(0.4),
+treshold_distance_between_steps_(0.1),
+threshold_max_total_force_(0.2),
+change_k_robot_(true),
+change_k_person_(false),
+change_k_person_comp_(false),
+change_k_obst_(false),
+initial_goal_case_bool_(true),
+initial_not_set_last_people_dest_(false),
+minimun_velocity_case_stop_initial_(0.15),
+initial_limit_distance_goal_to_person_robot_stop_(0.3),
+bool_out_init_pose_robot_(false),
+ideal_max_robot_velocity_(0.9), //max ideal robot velocity
+bool_tes_prop1(false),
+bool_tes_prop2(false),
+metros_al_dynamic_goal_Vform_(3.0)
+{
+
+	if(overpas_obstacles_behind_person_){ // hay que compensar el que el robot va por detras. (sería mejor usar la distancia real para la colision, pero en el caso de ir delante no va bien, pq entonces es distancia fija!)
+		robot_adition_complete_esphere_companion_distance_=2.5;
+	}
+	Action_=Cplan_local_nav_person_companion::ITER;
+    edge_.reserve((size_t)max_iter_);
+    cost_int_forces_.reserve((size_t)max_iter_);
+    cost_robot_.reserve((size_t)max_iter_);
+    cost_obstacles_.reserve((size_t)max_iter_);
+    //cost_local_minima_.reserve((size_t)max_iter_);
+    cost_distance_.reserve((size_t)max_iter_);
+    cost_orientation_.reserve((size_t)max_iter_);
+    cost_past_traj_.reserve((size_t)max_iter_);
+    nodes_in_branch_.reserve((size_t)max_iter_);
+	random_goals_.reserve( (size_t)max_iter_ );
+
+	cost_companion_.reserve( (size_t)max_iter_ ); // companion variables (ely)
+	orientation_person_robot_angles_.reserve( (size_t)max_iter_ );
+	parent_index_vector_.reserve( (size_t)max_iter_ );
+	min_distance_collision_vector_.reserve( (size_t)max_iter_ );
+
+    //Initialization of robot: there is always a robot if planning
+	robot_in_the_scene_ = true;
+	person_companion_in_the_scene_ = true;
+	robot_ = new Crobot(0,Crobot::Differential,scene_force_type_);
+	person_companion_ = new Crobot(1,Crobot::Differential,scene_force_type_);
+	person_companion_->reserve_planning_trajectory(max_iter_);
+
+    //random generator
+    generator_.seed();
+
+    //planner cost paramters
+    cost_parameters_.reserve(7);
+    cost_parameters_.push_back(1.0);// [0] Goal cost
+    cost_parameters_.push_back(1.0);// [1] orientation cost
+    cost_parameters_.push_back(1.0);// [2] Robot cost
+    cost_parameters_.push_back(1.0);// [3]Interacting people cost
+    cost_parameters_.push_back(0.25);// [4] potential time
+    cost_parameters_.push_back(1.0);// [5] obstacles cost
+    cost_parameters_.push_back(1.0);// [6] past trajectory function cost
+    cost_parameters_.push_back(1.0);// [7] local minima scape cost
+
+
+    // all ppl time
+    filtering_time_window_ =  horizon_time;
+
+    if(debug_real_test_companion_){
+    std::cout << "angle_companion_ ="<<angle_companion_<< std::endl;
+    }
+
+    if(robot_or_person==false){
+    	std::cout << " (INI robot_planner)  max_iter_ ="<<max_iter_<<"; horizon_time="<<horizon_time_<< std::endl;
+    }else{
+    	std::cout << " (INI akp_companion_person planner)  max_iter_ ="<<max_iter_<<"; horizon_time="<<horizon_time_<< std::endl;
+    }
+
+
+    std::string data_file;
+	std::string data_file2="/home/ely7787/iri-lab/labrobotica/restricted/algorithms/people_prediction/branches/ely_people_prediction_companion_robot/2_results_evaluate_costs/results_evaluate_cost_robot_case2.txt";
+	evaluate_costs_file_=data_file2;
+	std::string data_file3="/home/ely7787/iri-lab/labrobotica/restricted/algorithms/people_prediction/branches/ely_people_prediction_companion_robot/2_results_evaluate_costs/results_evaluate_distance_and_angle_robot_case2.txt";
+	evaluate_change_distance_and_angle_companion_file_=data_file3;
+
+	new_matlab_file();
+
+	data_file2="/home/ely7787/iri-lab/labrobotica/restricted/algorithms/people_prediction/branches/ely_people_prediction_companion_robot/2_results_evaluate_costs/results_evaluate_cost_robot_case1.txt";
+	evaluate_costs_file_=data_file2;
+	data_file3="/home/ely7787/iri-lab/labrobotica/restricted/algorithms/people_prediction/branches/ely_people_prediction_companion_robot/2_results_evaluate_costs/results_evaluate_distance_and_angle_robot_case1.txt";
+	evaluate_change_distance_and_angle_companion_file_=data_file3;
+
+	new_matlab_file();
+	//new_matlab_file_To_evaluate_change_distance_and_angle(); // generar archivo y ya en esa iter ira bien!
+	//new_matlab_file_To_evaluate_costs();
+
+
+	data_file2="/home/ely7787/iri-lab/labrobotica/restricted/algorithms/people_prediction/branches/ely_people_prediction_companion_robot/2_results_evaluate_costs/results_evaluate_cost_robot_case0.txt";
+	evaluate_costs_file_=data_file2;
+	data_file3="/home/ely7787/iri-lab/labrobotica/restricted/algorithms/people_prediction/branches/ely_people_prediction_companion_robot/2_results_evaluate_costs/results_evaluate_distance_and_angle_robot_case0.txt";
+	evaluate_change_distance_and_angle_companion_file_=data_file3;
+
+	new_matlab_file();
+	//new_matlab_file_To_evaluate_change_distance_and_angle(); // generar archivo y ya en esa iter ira bien!
+	//new_matlab_file_To_evaluate_costs();
+
+
+	SIM_initial_robot_pose2_=Spose(1.0,-0.83,0.0,0.0,0.0,0.0);  // antes -1.25
+	SIM_initial_robot_pose1_=Spose(-8.5,-1.25,0.0,0.0,0.0,0.0); //Spose( double x_ , double y_ , double time_stamp_ , double theta_ , double v_, double w_) -4.5,-1.25
+
+	//SIM_initial_robot_pose3_=Spose(16.5,10.5,0.0,0.0,0.0,0.0);
+	//SIM_initial_robot_pose4_=Spose(16.5,-15.5,0.0,0.0,0.0,0.0);
+	SIM_initial_robot_pose4_=Spose(13.5,7.5,0.0,0.0,0.0,0.0);
+	SIM_initial_robot_pose3_=Spose(13.5,-8.5,0.0,0.0,0.0,0.0);  // ES YA EL PARA RESTART!
+
+	SIM_initial_person_companion_pose1_=Spose(-7.0,0.5,0.0,0.0,0.0,0.0);
+	SIM_initial_person_companion_pose2_=Spose(1.0,0.5,0.0,0.0,0.0,0.0);
+	//8, -2.0, 0, 0.5, 1, 9 (person companion goals.)
+	//9, 27.0, 0, 0.5, 1, 8
+
+	SIM_initial_person_goal_pose1_=Spoint(27.0, 20.5,0.0); // SIM_initial_person_goal_pose1_=Spoint(30.0, 10.5,0.0);
+	//SIM_initial_person_goal_pose1_=Spoint(20.0, 17.5,0.0); // Spoint( double x_ , double y_ , double time_stamp_) :
+	// 0, 17.0, 10.5, 0.5, 1, 1   DESTS pose1 (INITIAL)
+	// 1, 1.0, -10.5, 0.5, 1, 0
+	//Sdestination_person_goal_pose1_=Sdestination(1,1.0,-20.5);
+	//Sdestination_person_goal_pose1_=Sdestination(1,20.0,-17.5);
+
+	SIM_initial_person_goal_pose2_=Spoint(27.0, -17.5, 0.0); // SIM_initial_person_goal_pose2_=Spoint(30.0, -10.5, 0.0);
+	//SIM_initial_person_goal_pose2_=Spoint(20.0, -17.5, 0.0);
+	// 0, 17.0, -10.5, 0.5, 1, 1   DESTS pose2 (INITIAL)
+	// 1, 1.0, +10.5, 0.5, 1, 0
+	//Sdestination_person_goal_pose2_=Sdestination(1,1.0,20.5);
+	//Sdestination_person_goal_pose2_=Sdestination(1,20.0, 17.5);
+
+	SIM_initial_person_goal_pose3_=Spoint(27,0.0,0.0); // SIM_initial_person_goal_pose3_=Spoint(30,0.0,0.0);
+	//Sdestination_person_goal_pose3_=Sdestination(1,1.0,0.0);
+
+	//SIM_initial_person_goal_pose4_=Spoint(20,0.0,0.0);
+	//SIM_initial_person_goal_pose5_=Spoint(30,0.0,0.0);
+
+	//8, -2.0, 0, 0.5, 1, 9 (person companion goals.== persona goal en la misma direccion que ellos en frente)
+	//9, 27.0, 0, 0.5, 1, 8
+
+}
+
+Cplan_local_nav_person_companion::~Cplan_local_nav_person_companion()
+{
+	//all memory allocations are freed in Cprediction_behavior/bhmip
+}
+
+
+/* akp on person_companion*/
+bool Cplan_local_nav_person_companion::person_companion_plan_companion(Spose& pose_command, Cperson_abstract::companion_reactive& reactive,double dt, std::vector<Sdestination>* person_best_dest)
+{
+
+	//std::cout << "7777 ?????? IN!!! person_companion_plan_companion (1) ; person_list_.size()="<< person_list_.size()<<"; id_person_companion_="<<id_person_companion_ << std::endl;
+	change_set_id_person_companion();
+
+	Cperson_abstract* person_obj_companion_person;
+	find_person(id_person_companion_ , &person_obj_companion_person); //bool finded_companion_person=find_person(id_person_companion_ , &person_obj_companion_person);
+	pointer_to_person_companion_=person_obj_companion_person; // TODO: solucion provisional para que no pete simulador PERSON COMP
+
+	//std::cout << "MUY IMPORTANTE!!! 77777 finded_companion_person="<<finded_companion_person<<"; id_person_companion_="<<id_person_companion_<< std::endl;
+
+
+	// get the trajectory of the other person companion of the group, for the companion force between them.
+	initial_person_companion_point_=robot_->get_current_pointV();
+
+
+	clock_t robot_plan_companion2_start, robot_plan_companion2_end; // clocks, pueden ser utiles... pero cambiando nombres, al menos en los scouts.
+	robot_plan_companion2_start = clock();
+
+	//if(iit->get_id()==id_person_companion_){
+		actual_person_Companion_SpointV_=person_companion_->get_current_pointV();
+		actual_person_Companion_destination_=person_companion_->get_best_dest();
+		max_desired_person_Companion_velocity_=person_companion_->get_desired_velocity();
+		actual_person_Companion_pointer_=person_companion_;
+	//}
+
+
+	//	std::cout << "person_companion_plan_companion (2) "<< std::endl;
+	 //std::cout << "(in akp person_companion) robot_->get_current_pointV().x="<<robot_->get_current_pointV().x<<"; robot_->get_current_pointV().y="<<robot_->get_current_pointV().y << std::endl;
+
+	is_act_person_companion_=true;
+
+	//std::cout << " (1) !!! person_companion_plan_companion; person_list_.size()="<< person_list_.size()  << std::endl;
+	//person_companion_->get_best_dest().print();
+
+	if(first_in_itter_){
+		std::string data_file2="/home/ely7787/iri-lab/labrobotica/restricted/algorithms/people_prediction/branches/ely_people_prediction_companion_robot/2_results_evaluate_costs/results_evaluate_cost_person_companion_case1.txt";
+		evaluate_costs_file_=data_file2;
+		new_matlab_file_To_evaluate_costs();
+		 data_file2="/home/ely7787/iri-lab/labrobotica/restricted/algorithms/people_prediction/branches/ely_people_prediction_companion_robot/2_results_evaluate_costs/results_evaluate_cost_person_companion_case2.txt";
+		evaluate_costs_file_=data_file2;
+		new_matlab_file_To_evaluate_costs();
+		 data_file2="/home/ely7787/iri-lab/labrobotica/restricted/algorithms/people_prediction/branches/ely_people_prediction_companion_robot/2_results_evaluate_costs/results_evaluate_cost_person_companion_case0.txt";
+		evaluate_costs_file_=data_file2;
+		new_matlab_file_To_evaluate_costs();
+		first_in_itter_=false;
+	}
+
+	if(actual_case_==Cplan_local_nav_person_companion::case0){
+		std::string data_file="/home/erepiso/iri-lab/labrobotica/restricted/algorithms/people_prediction/branches/ely_people_prediction_companion_robot/1_data_results/results_person_companion_and_approaching_case1.txt";
+		results_file_=data_file;
+		std::string data_file2="/home/erepiso/iri-lab/labrobotica/restricted/algorithms/people_prediction/branches/ely_people_prediction_companion_robot/2_results_evaluate_cost/results_evaluate_cost_person_companion_case0.txt";
+		evaluate_costs_file_=data_file2;
+		//std::string data_file3="/home/erepiso/iri-lab/labrobotica/restricted/algorithms/people_prediction/branches/ely_people_prediction_companion_robot/2_results_evaluate_cost/results_evaluate_distance_and_angle_robot_case0.txt";
+		//evaluate_costs_file_=data_file3;
+
+	}else if(actual_case_==Cplan_local_nav_person_companion::case1){
+		std::string data_file="/home/erepiso/iri-lab/labrobotica/restricted/algorithms/people_prediction/branches/ely_people_prediction_companion_robot/1_data_results/results_person_companion_case1.txt";
+		results_file_=data_file;
+		std::string data_file2="/home/erepiso/iri-lab/labrobotica/restricted/algorithms/people_prediction/branches/ely_people_prediction_companion_robot/2_results_evaluate_cost/results_evaluate_cost_person_companion_case1.txt";
+		evaluate_costs_file_=data_file2;
+		//std::string data_file3="/home/ely7787/iri-lab/labrobotica/restricted/algorithms/people_prediction/branches/ely_people_prediction_companion_robot/2_results_evaluate_cost/results_evaluate_distance_and_angle_robot_case1.txt";
+		//evaluate_costs_file_=data_file3;
+
+	}else if(actual_case_==Cplan_local_nav_person_companion::case2){
+		std::string data_file="/home/erepiso/iri-lab/labrobotica/restricted/algorithms/people_prediction/branches/ely_people_prediction_companion_robot/1_data_results/results_person_companion_case2.txt";
+		results_file_=data_file;
+		std::string data_file2="/home/erepiso/iri-lab/labrobotica/restricted/algorithms/people_prediction/branches/ely_people_prediction_companion_robot/2_results_evaluate_cost/results_evaluate_cost_person_companion_case2.txt";
+		evaluate_costs_file_=data_file2;
+		//std::string data_file3="/home/ely7787/iri-lab/labrobotica/restricted/algorithms/people_prediction/branches/ely_people_prediction_companion_robot/2_results_evaluate_cost/results_evaluate_distance_and_angle_robot_case1.txt";
+		//evaluate_costs_file_=data_file3;
+	}
+
+
+	//std::cout << " (2) !!! person_companion_plan_companion ; person_list_.size()="<< person_list_.size()  << std::endl;
+	//person_companion_->get_best_dest().print();
+
+	//TODO. Set companion person velocity to 0.8!!!
+	person_companion_->set_desired_velocty(robot_->get_desired_velocity());
+	//std::cout << " robot_->get_desired_velocity()="<<robot_->get_desired_velocity()<< std::endl;
+	ini_point_person_companion_akp_=person_companion_->get_current_pointV();
+	ini_spose_person_companion_akp_=person_companion_->get_current_pose();
+	//std::cout << " (3) !!! person_companion_plan_companion ; person_list_.size()="<< person_list_.size()  << std::endl;
+	//person_companion_->get_best_dest().print();
+	bool_case_person_companion_=true;
+	//obstacle_radi_=0;
+	//person_radi_=0;
+
+	//std::cout << " horizon_time_!!!!!!!!!!! (INI akp_companion_person planner)  max_iter_ ="<<max_iter_<<"; horizon_time_="<<horizon_time_<< std::endl;
+
+		if(debug_person_companion_general_){
+			std::cout << std::endl<< std::endl<< std::endl;
+			std::cout << " (2!!!) (person companion) !!!INI person_companion_plan_companion!!!" << std::endl;
+			std::cout << " (INI akp_companion_person planner)  max_iter_ ="<<max_iter_<<"; horizon_time_="<<horizon_time_<< std::endl;
+
+			//std::cout << std::endl<< std::endl<< std::endl;
+			std::cout << " (2!!!) (person companion) !!!INI person_companion_plan_companion!!!  ;  obstacle_radi_="<<obstacle_radi_<<"; person_radi_="<<person_radi_<<"; robot_->get_platform_radii()="<<robot_->get_platform_radii()<<std::endl<< std::endl;
+			std::cout << " (2!!!) (person companion) !!!INI person_companion_plan_companion!!!  ;  id_person_companion_="<<id_person_companion_<<std::endl<< std::endl;
+		}
+
+
+	robot_companion_case_=reactive; //el reactive tiene que ser el plan companion, para que vaya bien.
+	// OJO!! dt NO es incremento de t, sino el tiempo actual!
+
+	before_initial_robot_spoint_.x=-10000;
+	before_initial_robot_spoint_.y=-10000;
+	//std::cout << " (4) !!! person_companion_plan_companion" << std::endl;
+	//person_companion_->get_best_dest().print();
+	//std::cout << "dt="<<dt<< std::endl;
+	//std::cout << "pose_command.time_stamp="<<pose_command.time_stamp<< std::endl;
+	//std::cout << "time_act_="<<time_act_<< std::endl;
+	//time_act_=dt;
+	//inct_=time_ant_-time_act_;
+	//std::cout << "inct_="<<inct_<< std::endl;
+	//dt=0; //ha de ser 0, para que vayan bien las funciones siguientes.
+	//ini_increment_angle(); // for robot companion
+
+	// Needed, but with the coal of the person companion. Similar to this, but can be a little different.
+	/*if(companion_same_person_goal_){
+		Cperson_abstract* person_obj;
+		bool finded_person=find_person(id_person_companion_ , &person_obj);
+
+		Spose robot=robot_->get_current_pose();
+		SpointV_cov person = person_obj->get_current_pointV();
+		Sdestination external_goal_same_person=person_obj->get_best_dest();
+
+		double theta=theta = atan2(person_obj->get_best_dest().y-person.y,person_obj->get_best_dest().x-person.x);
+		double angle=atan2(robot.y-person.y , robot.x-person.x);
+
+		if( diffangle(theta, angle) < 0 ){
+			external_goal_same_person.x=external_goal_same_person.x+robot_person_proximity_goals_x_*cos(theta+angle);
+			external_goal_same_person.y= external_goal_same_person.y+robot_person_proximity_goals_y_*sin(theta+angle);
+		}else{
+			external_goal_same_person.x=external_goal_same_person.x+robot_person_proximity_goals_x_*cos(theta-angle);
+			external_goal_same_person.y= external_goal_same_person.y+robot_person_proximity_goals_y_*sin(theta-angle);
+		}
+		//set_robot_external_goal(external_goal_same_person);
+	}*/
+
+
+	clock_t scene_prediction_start, scene_prediction_end;
+	scene_prediction_start=clock();
+	if(debug_person_companion_general_){
+		std::cout << " (1) antes scene_prediction()"<< std::endl;
+	}
+	bool person_or_robot=true; //case, person_companion
+	//std::cout << " (5) !!! person_companion_plan_companion" << std::endl;
+	person_companion_->get_best_dest().print();
+	//std::cout << " (5.1) !!! person_or_robot="<<person_or_robot<<"; person_list_.size()="<< person_list_.size() << std::endl;
+
+	this->Cprediction_behavior::scene_prediction(person_or_robot,person_best_dest); // scene prediction, needed, pero sin la person cmpanion y hará falta el robot en colisiones.
+
+
+	/*for( auto iit : person_list_)
+	{
+		std::cout << " (5.2) !!! (planning traj size people) iit->size"<<iit->get_planning_trajectory()->size() <<", id=" <<iit->get_id()<< std::endl;
+	}*/
+
+	//std::cout << " (5.2) !!! before robot prediction; horizon_time_="<<horizon_time_<<", person_companion_->get_best_dest().x="<<person_companion_->get_best_dest().x<<", person_companion_->get_best_dest().y="<<person_companion_->get_best_dest().y<<
+	//		" person_companion_->get_id()"<< person_companion_->get_id()<< std::endl;
+	robot_trajectory_prediction_for_group_Zanlungo( horizon_time_, person_companion_->get_best_dest(), id_person_companion_);
+	//std::cout << " (5.2) !!! after robot prediction, robo_-> pred_trajsize_with_target_pers= "<<robot_->get_prediction_trajectory_with_target_person()->size()<<"or pred trak_size="<< robot_->get_prediction_trajectory()->size()<< std::endl;
+
+	//std::cout << " (5.2) !!! person_companion_plan_companion" << std::endl;
+
+
+	//std::cout << " (6) !!! person_companion_plan_companion" << std::endl;
+//	person_companion_->get_best_dest().print();
+	// only for prediction of persons with id!=id_person_companion.
+	scene_prediction_end=clock();
+	if(debug_real_test_companion_){
+		std::cout << "time_prediction=scene_prediction_start-scene_prediction_end="<<(scene_prediction_end-scene_prediction_start)/1000<< std::endl;
+	}
+	//kinodynamic rrt
+	if(debug_person_companion_general_){
+		std::cout << " (1) 3 "<< std::endl;
+	}
+
+	bool result; // para no entrar a variables bacias.
+	//std::cout << " (5.3) !!! person_companion_plan_companion" << std::endl;
+
+	robot_initial_pose_=robot_->get_current_pose(); // alguna de estas irá bien para coger la pose act del robot.
+	//std::cout << " (5.4) !!! person_companion_plan_companion" << std::endl;
+	//std::cout << " IMPORTANTE!!! 7777 333 => robot_initial_pose_.x="<<robot_initial_pose_.x<<"; robot_initial_pose_.y="<<robot_initial_pose_.y<<"; robot_initial_pose_.time_stamp="<<robot_initial_pose_.time_stamp<< std::endl;
+
+	if(!robot_->get_time_stamp_plan().empty()){
+		std::cout <<"time_stamp_plan="<<robot_->get_time_stamp_plan().at(0)<< std::endl;
+	}
+
+	//SpointV_cov centro_robot_spoint=robot_->get_current_pointV();
+	initial_robot_spoint_=robot_->get_current_pointV();
+
+
+
+	//std::cout << " (5.5) !!! person_companion_plan_companion" << std::endl;
+
+	/* Inicio, get person_robot_distance.*/
+	double robot_person_distance=calc_robot_person_companion_distance_companion_person_akp(); // puede que haga falta para la circunferencia.
+	robot_person_distance_=robot_person_distance;
+	/* fin, get person_robot_distance.*/
+
+	Cperson_abstract* person_obj;
+	find_person(id_person_companion_ , &person_obj); // no se si hará falta...
+
+	if(debug_person_companion_general_){
+		std::cout << " (2) 4"<< std::endl;
+	}
+	/*if(only_comp_people_vel_and_robot_poses_){
+		std::cout << " PERSON SpointV_cov: "<< std::endl;
+		person_obj->print();
+		SpointV_cov actual_person1=person_obj->get_current_pointV();
+		std::ofstream fileMatlab2;
+		fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+		fileMatlab2 << " PERSON SpointV_cov: "<< " \n";
+		fileMatlab2 << "person_spoint_.time_stamp="<<actual_person1.time_stamp<<" \n";
+		fileMatlab2 << "person_spoint_.x="<<actual_person1.x<<" \n";
+		fileMatlab2 << "person_spoint_.y="<<actual_person1.y<<" \n";
+		fileMatlab2 << "person_spoint_.vx="<<actual_person1.vx<<" \n";
+		fileMatlab2 << "person_spoint_.vy="<<actual_person1.vy<<" \n";
+		fileMatlab2.close();
+
+	}*/
+    //esta hará falta.
+	robot_person_companion_distance_=(robot_person_proximity_distance_+2*robot_->get_platform_radii()+little_augmented_collision_margin_)/2;
+
+	if(debug_person_companion_general_){
+		std::cout << " (3) 5"<< std::endl;
+	}
+	if(debug_antes_subgoals_entre_AKP_goals_){
+		std::cout << "person_obj->print() out, robot_plan_companion:"<< std::endl;
+		std::cout << "(iteration=actual) robot_person_companion_distance_="<<robot_person_companion_distance_<< std::endl;
+		std::cout << "(ha de ser menor de 3.5)(out)robot_person_distance_="<<robot_person_distance_<< std::endl;
+	}
+	if(debug_person_companion_general_){
+		std::cout << " (3.1) "<< std::endl;
+	}
+	clock_t init_robot_plan2_start, init_robot_plan2_end; //clocks!
+	init_robot_plan2_start=clock();
+//std::cout << " (7) !!! person_companion_plan_companion" << std::endl;
+//person_companion_->get_best_dest().print();
+	init_robot_plan2_person_companion(robot_person_distance); // MIRAR!!! cambian cosas al usarla con la persona.
+//std::cout << " (8) !!! person_companion_plan_companion" << std::endl;
+//person_companion_->get_best_dest().print();
+	init_robot_plan2_end=clock();
+
+	if(debug_person_companion_general_){
+		std::cout << " (4) 7"<< std::endl;
+	}
+	//calc_area_workspace_arround_person_companion(); // Creo que no hace falta. Comprobar!
+
+	if(debug_real_test_companion_){
+		std::cout << "time_init_robot_plan2=init_robot_plan2_start-init_robot_plan2_end="<<(init_robot_plan2_end-init_robot_plan2_start)/1000<< std::endl;
+	}
+
+	if(debug_person_companion_general_){
+		std::cout << " DESIRED_VELOCITY=person_companion_->get_desired_velocity()"<<person_companion_->get_desired_velocity()<< std::endl;
+		std::cout << " person_companion_->get_best_dest().print(): "<< std::endl;
+		person_companion_->get_best_dest().print();
+	}
+	//std::cout << " (9) !!! person_companion_plan_companion" << std::endl;
+	person_companion_->get_best_dest().print();
+	ini_increment_angle_person_companion_akp(); // (hace falta para el coste incremental solo) for robot companion
+	//std::cout << " (10) !!! person_companion_plan_companion" << std::endl;
+	person_companion_->get_best_dest().print();
+	if(debug_person_companion_general_){
+		std::cout << " (5) "<< std::endl;
+	}
+	//if( ((robot_person_proximity_distance_-robot_person_proximity_tolerance_)<robot_person_distance) && (robot_person_distance<(robot_person_proximity_distance_+robot_person_proximity_tolerance_+offset_atractive_))){  //TODO: falta tener en cuenta al principio la posición inicial de la persona.
+	//if(robot_person_distance<(robot_person_proximity_distance_+robot_person_proximity_tolerance_+offset_atractive_)){  //TODO: falta tener en cuenta al principio la posición inicial de la persona.
+
+	// SIEMPRE es el del akp_planning_companion.
+
+	reactive=Cperson_abstract::Akp_planning;
+	robot_companion_case_=reactive;
+
+	if(debug_person_companion_general_){
+		std::cout << " (6) antes  robot_plan_anticipative_krrt_companion_person_companion(reactive) "<< std::endl;
+	}
+
+
+
+	//std::cout << " (11) !!! person_companion_plan_companion" << std::endl;
+//	person_companion_->get_best_dest().print();
+	if ( robot_plan_anticipative_krrt_companion_person_companion(reactive) )  //mirar la robot_plan_anticipative_krrt_companion a ver si hay k cambiarla por lo del robot.
+	{
+
+//	std::cout << " (12) !!! person_companion_plan_companion" << std::endl;
+//	person_companion_->get_best_dest().print();
+		if(debug_person_companion_general_){
+			std::cout << " in kkrrt 2"<< std::endl;
+		}
+
+		//TODO. return! companion person velocity to 0.2 o lo que se ponga en el Cscene_sim!!! + replan_last_step a 0.2 velocity
+
+		clock_t end_part_start, end_part_end;
+		end_part_start=clock();
+
+		last_pose_command_ = get_best_planned_pose_person_companion_akp( dt );// TENDRA QUE CAMBIAR, pq es solo goal de la persona.
+
+		double other_people_cost_due_to_robot;
+		double companion_person_cost_due_to_robot;
+
+		// para visualización, ok e incluirlo en el simulador, para que se vea el planing de la persona.
+		//std::cout << " (13) !!! person_companion_plan_companion" << std::endl;
+//		person_companion_->get_best_dest().print();
+		this->Cprediction_behavior::calculate_current_forces_companion2_for_person_companion_akp( pr_force_mode_ ,id_person_companion_,false, reactive, min_next_companion_angle_,u_forces_robot_actual_.f_goal,u_forces_robot_actual_.f_people,u_forces_robot_actual_.f_obs,u_forces_robot_actual_.f_persongoal,other_people_cost_due_to_robot,companion_person_cost_due_to_robot);//for plotting purposes
+
+	//	std::cout << " (13) !!! size="<<saved_tree_forces_to_see_companion_Force_Zanlungo_.size()<<", actual_best_path_index_="<<actual_best_path_index_ << std::endl;
+
+		person_companion_->set_forces_person_companion( saved_tree_forces_to_see_companion_Force_Zanlungo_.at(actual_best_path_index_).f_goal,  saved_tree_forces_to_see_companion_Force_Zanlungo_.at(actual_best_path_index_).f_people, Sforce(), saved_tree_forces_to_see_companion_Force_Zanlungo_.at(actual_best_path_index_).f_obs, saved_tree_forces_to_see_companion_Force_Zanlungo_.at(actual_best_path_index_).f_persongoal);
+		//std::cout << " (14) !!! person_companion_plan_companion; size()="<<saved_tree_forces_to_see_companion_Force_Zanlungo_.size()<<", actual_best_path_index_=" <<actual_best_path_index_<< std::endl;
+		//std::cout << " x= "<<saved_tree_forces_to_see_companion_Force_Zanlungo_.at(actual_best_path_index_).f_goal.fx<<"; y="<<saved_tree_forces_to_see_companion_Force_Zanlungo_.at(actual_best_path_index_).f_goal.fy<< std::endl;
+
+//		person_companion_->get_best_dest().print();
+
+		other_people_due_to_robot_cost_=other_people_cost_due_to_robot; //estas en teoria no harian falta.
+		companion_person_due_to_robot_cost_=companion_person_cost_due_to_robot;
+
+		if(debug_person_companion_general_){
+			std::cout << " last_pose_command_.print(): "<< std::endl;
+			last_pose_command_.print();
+
+			std::cout << " last_pose_command_.v"<<last_pose_command_.v<< std::endl;
+			std::cout << " last_pose_command_.w"<<last_pose_command_.w<< std::endl;
+		}
+
+		if(debug_antes_subgoals_entre_AKP_goals_){ // cambiar por ver el foal de la person companion.
+			std::cout << " Planing FINAL GOAL (NO collision, reactive_repulsive) ( last_pose_command_ and robot_):"<< std::endl;
+			last_pose_command_.print();
+			//robot_->print();
+		}
+			result = true;
+		/*if(!final_collision_check_){ // necesarias.
+			 std::cout << " if(!final_collision_check_)=> final_collision_check_="<<final_collision_check_<< std::endl;
+			result = true;
+		}else{
+			//if(debug_cout_robot_){
+				 std::cout << " else if(!final_collision_check_)=> final_collision_check_="<<final_collision_check_<< std::endl;
+			//}
+
+			last_pose_command_ = Spose();
+			result = false;
+		}*/
+		end_part_end=clock();
+		if(check_execution_times_){
+			 std::cout << " time_end_part=end_part_start-end_part_end="<<((end_part_end-end_part_start)/clocks_per_sec_my_var_)*1000<< std::endl;
+		}
+	}
+	else
+	{
+		//std::cout << " else... robot_plan_anticipative_krrt_companion_person_companion="<< std::endl;
+		//last_pose_command_ = Spose();
+		last_pose_command_.v = 0;
+		last_pose_command_.w = 0;
+		result = false;
+	}
+	//std::cout << " last_pose_command_.print(): "<< std::endl;
+	//last_pose_command_.print();
+
+
+	if(debug_person_companion_general_){
+		std::cout << "u last_pose_command_.v"<<last_pose_command_.v<< std::endl;
+		std::cout << "u last_pose_command_.w"<<last_pose_command_.w<< std::endl;
+	}
+
+	pose_command = last_pose_command_; // nueva pose de trayectoria calculada.
+
+
+	//pose_command.w=0.0;
+
+	final_pose_robot_=pose_command;
+
+	if(debug_person_companion_general_){
+		std::cout << "(FINAL AKP POSE) pose_command.x="<<pose_command.x<< std::endl;
+		std::cout << "(FINAL AKP POSE) pose_command.y="<<pose_command.y<< std::endl;
+		std::cout << "(FINAL AKP POSE) pose_command.v="<<pose_command.v<< std::endl;
+		std::cout << "(FINAL AKP POSE) pose_command.w="<<pose_command.w<< std::endl;
+	}
+
+	// TODO: para el companion desde esta pose, se han de calcular las colisiones, en t+1 y calcular el angulo para el companion ahí.
+	if(debug_antes_subgoals_entre_AKP_goals_){
+		std::cout << "FIN robot_plan_companion!!! "<< std::endl<< std::endl<< std::endl;
+	}
+
+
+	/*if(debug_goal_go_to_person_in_person_companion_){
+		std::cout << "FIN [robot_plan_companion person companion] pose_command.print();!!! "<< std::endl;
+		pose_command.print();
+	}*/
+
+	// *** Return robot max vel to really máx velocity ***/
+	return_max_velocity_systemRobot_to_max_value();// esto quizas va fuera, porque tendría que ser con la velocidad de la persona to_do!
+	robot_plan_companion2_end = clock();
+
+	if(check_execution_times_){
+		std::cout << "(FIN) time_tot=robot_plan_companion2_start-robot_plan_companion2_end="<<((robot_plan_companion2_end-robot_plan_companion2_start)/clocks_per_sec_my_var_)*1000<< std::endl;
+	}
+
+
+	before_initial_robot_spoint_=initial_robot_spoint_;  // mirar si necesario...
+	time_ant_=actual_time_;
+
+
+/*	std::cout << " vector_of_companion_collisions_.size()="<<vector_of_companion_collisions_.size()<< std::endl;
+	std::cout << " min_step_collision_distance_.size()="<<min_step_collision_distance_.size()<< std::endl;
+	std::cout << " angles_colisions_.size()="<<angles_colisions_.size()<< std::endl;
+	std::cout << " min_angles_colisions_.size()="<<min_angles_colisions_.size()<< std::endl;
+	std::cout << " random_goals2_.size()="<<random_goals2_.size()<< std::endl;
+	std::cout << " BEST_path_parent_index_vector_.size()="<<BEST_path_parent_index_vector_.size()<< std::endl;
+	std::cout << " person_companion_nearby_person_list_.size()="<<person_companion_nearby_person_list_.size()<< std::endl;
+
+	std::cout << " cost_companion_.size()="<<cost_companion_.size()<< std::endl;
+	std::cout << " parent_index_vector_.size()="<<parent_index_vector_.size()<< std::endl;
+	std::cout << " min_distance_collision_vector_.size()="<<min_distance_collision_vector_.size()<< std::endl;
+	std::cout << " orientation_person_robot_angles_.size()="<<orientation_person_robot_angles_.size()<< std::endl;
+*/
+	Crobot* robot_act=person_companion_;
+
+	if((Action_==Cplan_local_nav_person_companion::ITER)&&(sim)&&(save_results_on_files_for_person_companion_)){ // si estamos en caso iter, no reiniciar.
+		evaluate_costs_printToMatlab(robot_act);
+	}
+
+	//std::cout << " FIN PLAN PERSON COMPANION"<< std::endl;
+
+
+	//////////// INI  MOMENTARILY output mesages of Zanlungo save in file here to see them for debug!!!
+	//debug_output_screen_mesages_=true;
+	/*if(debug_output_screen_mesages_){
+		std::cout << "(save in file PERSON_COMPANION) 2"<< std::endl;
+	}
+		debug_output_screen_mesages_=true;
+	if((!orientation_person_robot_angles_.empty())&&(debug_output_screen_mesages_)){
+
+			std::cout << "(save in file) get_orientation_person_robot_angles_pred().at(0)= "<<orientation_person_robot_angles_.at(0)<< std::endl;
+			//std::cout << "(save in file) get_orientation_person_robot_angles_pred().at(1)= "<<orientation_person_robot_angles_.at(1)<< std::endl;
+
+		}
+		if(debug_output_screen_mesages_){ // en esta companion_person_ se corresponde con el robot!, por eso no sale bien ahora mismo!
+			std::cout << "(save in file PERSON_COMPANION) dt= "<<dt_<< std::endl;
+			std::cout << "(save in file PERSON_COMPANION) real_computation_time_dt= "<<final_compt_time_<< std::endl;
+			std::cout << "(save in file PERSON_COMPANION) actual_best_path_index_= "<<actual_best_path_index_<< std::endl;
+			std::cout << "(save in file PERSON_COMPANION) companion_person_x= "<<initial_person_companion_point_.x<< std::endl;
+			std::cout << "(save in file PERSON_COMPANION) companion_person_y= "<<initial_person_companion_point_.y<< std::endl;
+			std::cout << "(save in file PERSON_COMPANION) companion_person_vx= "<<initial_person_companion_point_.vx<< std::endl;
+			std::cout << "(save in file PERSON_COMPANION) companion_person_vy= "<<initial_person_companion_point_.vy<< std::endl;
+			std::cout << "(save in file PERSON_COMPANION) companion_person_theta= "<<atan(initial_person_companion_point_.vy/initial_person_companion_point_.vx)*180/3.14<< std::endl;
+			std::cout << "(save in file PERSON_COMPANION) desired_vel (first comp pers== robot now)= "<<robot_->get_desired_velocity()<< std::endl;
+
+			if(we_have_pointer_to_second_person_){
+				SpointV_cov sec_pers_point=second_group_companion_person_obj_->get_current_pointV();
+				std::cout << "(save in file PERSON_COMPANION) 2companion_person_x= "<<sec_pers_point.x<< std::endl;
+				std::cout << "(save in file PERSON_COMPANION) 2companion_person_y= "<<sec_pers_point.y<< std::endl;
+				std::cout << "(save in file PERSON_COMPANION) 2companion_person_vx= "<<sec_pers_point.vx<< std::endl;
+				std::cout << "(save in file PERSON_COMPANION) 2companion_person_vy= "<<sec_pers_point.vy<< std::endl;
+				std::cout << "(save in file PERSON_COMPANION) 2companion_person_theta= "<<atan(sec_pers_point.vy/sec_pers_point.vx)*180/3.14<< std::endl;
+			}
+
+
+
+		}
+	if(debug_output_screen_mesages_){
+		std::cout << "(save in file PERSON_COMPANION; ahora robot es person_companion_)" << std::endl;
+		std::cout << "(save in file PERSON_COMPANION) initial_robot_x= "<<person_companion_->get_current_pose().x<< std::endl;
+		std::cout << "(save in file PERSON_COMPANION) initial_robot_y= "<<person_companion_->get_current_pose().y<< std::endl;
+		std::cout << "(save in file PERSON_COMPANION) initial_robot_spoint_.vx= "<<person_companion_->get_current_pointV().vx<< std::endl;
+		std::cout << "(save in file PERSON_COMPANION) initial_robot_spoint_.vy= "<<person_companion_->get_current_pointV().vy<< std::endl;
+		std::cout << "(save in file PERSON_COMPANION) initial_robot_theta= "<<person_companion_->get_current_pose().theta*180/3.14<< std::endl;
+		std::cout << "(save in file PERSON_COMPANION) robot_initial_pose_.v= "<<person_companion_->get_current_pose().v<< std::endl;
+		std::cout << "(save in file PERSON_COMPANION) robot_initial_pose_.w= "<<person_companion_->get_current_pose().w<< std::endl;
+	}
+
+	if(debug_output_screen_mesages_){
+			std::cout << "(save in file PERSON_COMPANION) robot_final_goal_x= "<<person_companion_->get_best_dest().x<< std::endl;
+			std::cout << "(save in file PERSON_COMPANION) robot_final_goal_y= "<<person_companion_->get_best_dest().y<< std::endl;
+		}
+	if(Zanlungo_model_){
+		if((!preferred_paths_Zanlungo_.empty())&&(preferred_paths_Zanlungo_.size()>actual_best_path_index_)){
+
+					if(debug_output_screen_mesages_){
+						if(!Zanlungo_model3_){
+
+						}else{
+							//std::cout << "(save in file) preferred_x= "<<preferred_paths_Zanlungo_.at(actual_best_path_index_).x<< std::endl;
+							//std::cout << "(save in file) preferred_y= "<<preferred_paths_Zanlungo_.at(actual_best_path_index_).y<< std::endl;
+							//std::cout << "(save in file) preferred_m= "<<preferred_paths_Zanlungo_.at(actual_best_path_index_).m<< std::endl;
+							//std::cout << "(save in file) preferred_th= "<<preferred_paths_Zanlungo_.at(actual_best_path_index_).th*180/3.14<< std::endl;
+						}
+
+					}
+
+				}else{
+					if(Zanlungo_model_){
+						//std::cout << "(save in file) preferred_x= "<<preferred_paths_Zanlungo_.back().x<< std::endl;
+						//std::cout << "(save in file) preferred_y= "<<preferred_paths_Zanlungo_.back().y<< std::endl;
+						//std::cout << "(save in file) preferred_m= "<<preferred_paths_Zanlungo_.back().m<< std::endl;
+						//std::cout << "(save in file) preferred_th= "<<preferred_paths_Zanlungo_.back().th*180/3.14<< std::endl;
+					}
+
+				}
+
+	}
+
+	debug_output_screen_mesages_=true;
+
+	if(Zanlungo_model_){
+		std::cout << "(save in file PERSON_COMPANION) IN  Zanlungo_model_:"<< std::endl;
+		if((!saved_tree_forces_to_see_companion_Force_Zanlungo_.empty())&&(saved_tree_forces_to_see_companion_Force_Zanlungo_.size()>actual_best_path_index_)){
+
+			std::cout << "(save in file PERSON_COMPANION) IN  if((!saved_tree_forces_to_see_companion_Force_Zanlungo_.empty())&&(saved_tree_forces_to_see_companion_Force_Zanlungo_.size()>actual_best_path_index_)){"<< std::endl;
+
+				if(debug_output_screen_mesages_){
+
+					if(!Zanlungo_model3_){
+						std::cout << "(save in file PERSON_COMPANION) IN  !Zanlungo_model3_:"<< std::endl;
+						std::cout << "(save in file PERSON_COMPANION) f_companion_x= "<<saved_tree_forces_to_see_companion_Force_Zanlungo_.at(actual_best_path_index_).f_persongoal.fx<< std::endl;
+						std::cout << "(save in file PERSON_COMPANION) f_companion_y= "<<saved_tree_forces_to_see_companion_Force_Zanlungo_.at(actual_best_path_index_).f_persongoal.fy<< std::endl;
+						std::cout << "(save in file PERSON_COMPANION) f_goal_x= "<<saved_tree_forces_to_see_companion_Force_Zanlungo_.at(actual_best_path_index_).f_goal.fx<< std::endl;
+						std::cout << "(save in file PERSON_COMPANION) f_goal_y= "<<saved_tree_forces_to_see_companion_Force_Zanlungo_.at(actual_best_path_index_).f_goal.fy<< std::endl;
+
+
+						std::cout << "(save in file PERSON_COMPANION) f_people_x= "<<saved_tree_forces_to_see_companion_Force_Zanlungo_.at(actual_best_path_index_).f_people.fx<< std::endl;
+						std::cout << "(save in file PERSON_COMPANION) f_people_y= "<<saved_tree_forces_to_see_companion_Force_Zanlungo_.at(actual_best_path_index_).f_people.fy<< std::endl;
+
+						std::cout << "(save in file PERSON_COMPANION) f_obs_x= "<<saved_tree_forces_to_see_companion_Force_Zanlungo_.at(actual_best_path_index_).f_obs.fx<< std::endl;
+						std::cout << "(save in file PERSON_COMPANION) f_obs_y= "<<saved_tree_forces_to_see_companion_Force_Zanlungo_.at(actual_best_path_index_).f_obs.fy<< std::endl;
+
+						std::cout << "(save in file PERSON_COMPANION) f_total_x= "<<saved_tree_forces_to_see_companion_Force_Zanlungo_.at(actual_best_path_index_).f.fx<< std::endl;
+						std::cout << "(save in file PERSON_COMPANION) f_total_y= "<<saved_tree_forces_to_see_companion_Force_Zanlungo_.at(actual_best_path_index_).f.fy<< std::endl;
+						std::cout << "alpha_companion_="<<alpha_companion_<<"; beta_companion_="<<beta_companion_<<"; gamma_companion_="<<gamma_companion_<<"; delta_companion_="<<delta_companion_<< std::endl;
+						std::cout <<"; 1 actual_best_path_index_="<<actual_best_path_index_<< "alpha_companion_="<<alpha_companion_<<"; beta_companion_="<<beta_companion_<<"; gamma_companion_="<<gamma_companion_<<"; delta_companion_="<<delta_companion_<< std::endl;
+
+					}else{
+						std::cout << "(save in file PERSON_COMPANION) IN  Zanlungo_model3_:"<< std::endl;
+						if(Zanlungo_model_){
+
+						}
+						std::cout << "(save in file PERSON_COMPANION) f_companion_x= "<<saved_tree_forces_to_see_companion_Force_Zanlungo_.back().f_persongoal.fx<< std::endl;
+						std::cout << "(save in file PERSON_COMPANION) f_companion_y= "<<saved_tree_forces_to_see_companion_Force_Zanlungo_.back().f_persongoal.fy<< std::endl;
+						std::cout << "(save in file PERSON_COMPANION) f_goal_x= "<<saved_tree_forces_to_see_companion_Force_Zanlungo_.back().f_goal.fx<< std::endl;
+						std::cout << "(save in file PERSON_COMPANION) f_goal_y= "<<saved_tree_forces_to_see_companion_Force_Zanlungo_.back().f_goal.fy<< std::endl;
+
+						std::cout << "(save in file PERSON_COMPANION) f_people_x= "<<saved_tree_forces_to_see_companion_Force_Zanlungo_.back().f_people.fx<< std::endl;
+					    std::cout << "(save in file PERSON_COMPANION) f_people_y= "<<saved_tree_forces_to_see_companion_Force_Zanlungo_.back().f_people.fy<< std::endl;
+						std::cout << "(save in file PERSON_COMPANION) f_obs_x= "<<saved_tree_forces_to_see_companion_Force_Zanlungo_.back().f_obs.fx<< std::endl;
+						std::cout << "(save in file PERSON_COMPANION) f_obs_y= "<<saved_tree_forces_to_see_companion_Force_Zanlungo_.back().f_obs.fy<< std::endl;
+
+						std::cout << "(save in file PERSON_COMPANION) f_total_x= "<<saved_tree_forces_to_see_companion_Force_Zanlungo_.back().f.fx<< std::endl;
+						std::cout << "(save in file PERSON_COMPANION) f_total_y= "<<saved_tree_forces_to_see_companion_Force_Zanlungo_.back().f.fy<< std::endl;
+						std::cout << "alpha_companion_="<<alpha_companion_<<"; beta_companion_="<<beta_companion_<<"; gamma_companion_="<<gamma_companion_<<"; delta_companion_="<<delta_companion_<< std::endl;
+						std::cout <<"; 2 actual_best_path_index_="<<actual_best_path_index_<< "alpha_companion_="<<alpha_companion_<<"; beta_companion_="<<beta_companion_<<"; gamma_companion_="<<gamma_companion_<<"; delta_companion_="<<delta_companion_<< std::endl;
+
+					}
+
+
+				}
+
+				//ant_final_force_total_=saved_tree_forces_to_see_companion_Force_Zanlungo_.at(actual_best_path_index_).f;
+			}else{
+				std::cout << "(save in file PERSON_COMPANION) f_companion_x= "<<saved_tree_forces_to_see_companion_Force_Zanlungo_.back().f_persongoal.fx<< std::endl;
+				std::cout << "(save in file PERSON_COMPANION) f_companion_y= "<<saved_tree_forces_to_see_companion_Force_Zanlungo_.back().f_persongoal.fy<< std::endl;
+				std::cout << "(save in file PERSON_COMPANION) f_goal_x= "<<saved_tree_forces_to_see_companion_Force_Zanlungo_.back().f_goal.fx<< std::endl;
+				std::cout << "(save in file PERSON_COMPANION) f_goal_y= "<<saved_tree_forces_to_see_companion_Force_Zanlungo_.back().f_goal.fy<< std::endl;
+
+				std::cout << "(save in file PERSON_COMPANION) f_people_x= "<<saved_tree_forces_to_see_companion_Force_Zanlungo_.back().f_people.fx<< std::endl;
+				std::cout << "(save in file PERSON_COMPANION) f_people_y= "<<saved_tree_forces_to_see_companion_Force_Zanlungo_.back().f_people.fy<< std::endl;
+				std::cout << "(save in file PERSON_COMPANION) f_obs_x= "<<saved_tree_forces_to_see_companion_Force_Zanlungo_.back().f_obs.fx<< std::endl;
+																				//std::cout << "(save in file) f_obs_y= "<<saved_tree_forces_to_see_companion_Force_Zanlungo_.back().f_obs.fy<< std::endl;
+
+				std::cout << "(save in file PERSON_COMPANION) f_total_x= "<<saved_tree_forces_to_see_companion_Force_Zanlungo_.back().f.fx<< std::endl;
+				std::cout << "(save in file PERSON_COMPANION) f_total_y= "<<saved_tree_forces_to_see_companion_Force_Zanlungo_.back().f.fy<< std::endl;
+				std::cout <<";3 actual_best_path_index_="<<actual_best_path_index_<< "alpha_companion_="<<alpha_companion_<<"; beta_companion_="<<beta_companion_<<"; gamma_companion_="<<gamma_companion_<<"; delta_companion_="<<delta_companion_<< std::endl;
+
+			}
+	}else{ //only side-by-side
+		std::cout << "(save in file PERSON_COMPANION) IN  only side-by-side:"<< std::endl;
+
+		std::cout << "(save in file PERSON_COMPANION) f_companion_x= "<<u_forces_robot_actual_.f_persongoal.fx<< std::endl;
+		std::cout << "(save in file PERSON_COMPANION) f_companion_y= "<<u_forces_robot_actual_.f_persongoal.fy<< std::endl;
+		std::cout << "(save in file PERSON_COMPANION) f_goal_x= "<<u_forces_robot_actual_.f_goal.fx<< std::endl;
+		std::cout << "(save in file PERSON_COMPANION) f_goal_y= "<<u_forces_robot_actual_.f_goal.fy<< std::endl;
+
+		std::cout << "(save in file PERSON_COMPANION) f_people_x= "<<u_forces_robot_actual_.f_people.fx<< std::endl;
+		std::cout << "(save in file PERSON_COMPANION) f_people_y= "<<u_forces_robot_actual_.f_people.fy<< std::endl;
+		std::cout << "(save in file PERSON_COMPANION) f_obs_x= "<<u_forces_robot_actual_.f_obs.fx<< std::endl;
+		std::cout << "(save in file PERSON_COMPANION) f_obs_y= "<<u_forces_robot_actual_.f_obs.fy<< std::endl;
+
+		std::cout << "(save in file PERSON_COMPANION) f_total_x= "<<u_forces_robot_actual_.f.fx<< std::endl;
+		std::cout << "(save in file PERSON_COMPANION) f_total_y= "<<u_forces_robot_actual_.f.fy<< std::endl;
+		std::cout << "alpha_companion_="<<alpha_companion_<<"; beta_companion_="<<beta_companion_<<"; gamma_companion_="<<gamma_companion_<<"; delta_companion_="<<delta_companion_<< std::endl;
+
+	}
+
+	//debug_output_screen_mesages_=false;
+
+
+
+
+	if((debug_output_screen_mesages_)&&(!person_companion_->get_actual_random_goal_x().empty())&&(person_companion_->get_actual_random_goal_x().size()>actual_best_path_index_)){
+				std::cout << "(save in file PERSON_COMPANION) actual_random_goal_x_= "<<person_companion_->get_actual_random_goal_x().at(actual_best_path_index_)<< std::endl;
+				std::cout << "(save in file PERSON_COMPANION) actual_random_goal_y_= "<<person_companion_->get_actual_random_goal_y().at(actual_best_path_index_)<< std::endl;
+				std::cout << "(save in file PERSON_COMPANION) v_desired_x= "<<person_companion_->get_v_desired_robot_x().at(actual_best_path_index_)<< std::endl;
+				std::cout << "(save in file PERSON_COMPANION) v_desired_y= "<<person_companion_->get_v_desired_robot_y().at(actual_best_path_index_)<< std::endl;
+				std::cout << "(save in file PERSON_COMPANION) v_current_x= "<<person_companion_->get_v_current_robot_x().at(actual_best_path_index_)<< std::endl;
+				std::cout << "(save in file PERSON_COMPANION) v_current_y= "<<person_companion_->get_v_current_robot_y().at(actual_best_path_index_)<< std::endl;
+			}
+
+	debug_output_screen_mesages_=false;
+
+	if(((!robot_desired_vel_calc_Zanlungo_.empty())&&(robot_desired_vel_calc_Zanlungo_.size()>actual_best_path_index_))&&(debug_output_screen_mesages_)){
+		std::cout << "(save in file) robot_desired_vel_calc_Zanlungo_= "<<robot_desired_vel_calc_Zanlungo_.at(actual_best_path_index_)<< std::endl;
+
+
+
+	}
+
+
+	//std::cout <<"ATR COMPANION robot_plan_companion3; debug 19"<< std::endl;
+
+	debug_output_screen_mesages_=false;
+
+	if((!dist_per_rob_path_.empty())&&(dist_per_rob_path_.size()>actual_best_path_index_)){
+
+			if(debug_output_screen_mesages_){
+				std::cout << "(save in file) dist_robot_person= "<<dist_per_rob_path_.at(actual_best_path_index_)<< std::endl;
+			}
+
+		}
+
+	if(debug_output_screen_mesages_){
+		std::cout << "(save in file) ant_final_force_total_.fx= "<<ant_final_force_total_.fx<<"; ant_final_force_total_.fy="<<ant_final_force_total_.fy<< std::endl;
+		std::cout << "alpha_companion_="<<alpha_companion_<<"; beta_companion_="<<beta_companion_<<"; k="<<get_sfm_params(robot_)->at(0)<< std::endl;
+	}
+	if(debug_output_screen_mesages_){
+			std::cout << "(save in file PERSON_COMPANION) person_companion_->get_v_desired_robot_x().empty()= "<<person_companion_->get_v_desired_robot_x().empty()<< std::endl;
+			std::cout << "(save in file PERSON_COMPANION) person_companion_->get_v_desired_robot_x().size()= "<<person_companion_->get_v_desired_robot_x().size()<< std::endl;
+		}
+
+	debug_output_screen_mesages_=false;
+
+	//if(debug_output_screen_mesages_){
+		std::cout << "(save in file PERSON_COMPANION) final_pose_robot_x= "<<final_pose_robot_.x<< std::endl;
+		std::cout << "(save in file PERSON_COMPANION) final_pose_robot_y= "<<final_pose_robot_.y<< std::endl;
+		std::cout << "(save in file PERSON_COMPANION) final_pose_robot_v= "<<final_pose_robot_.v<< std::endl;
+		std::cout << "(save in file PERSON_COMPANION) final_pose_robot_w= "<<final_pose_robot_.w<< std::endl;
+		std::cout << "(save in file PERSON_COMPANION) final_pose_robot_theta= "<<final_pose_robot_.theta<< std::endl;
+	//}
+
+
+	if((debug_output_screen_mesages_)&&(!person_companion_->get_actual_random_goal_x().empty())&&(person_companion_->get_actual_random_goal_x().size()>actual_best_path_index_)){
+				std::cout << "(save in file PERSON_COMPANION) actual_random_goal_x_= "<<person_companion_->get_actual_random_goal_x().at(actual_best_path_index_)<< std::endl;
+				std::cout << "(save in file PERSON_COMPANION) actual_random_goal_y_= "<<person_companion_->get_actual_random_goal_y().at(actual_best_path_index_)<< std::endl;
+				std::cout << "(save in file PERSON_COMPANION) v_desired_x= "<<person_companion_->get_v_desired_robot_x().at(actual_best_path_index_)<< std::endl;
+				std::cout << "(save in file PERSON_COMPANION) v_desired_y= "<<person_companion_->get_v_desired_robot_y().at(actual_best_path_index_)<< std::endl;
+				std::cout << "(save in file PERSON_COMPANION) v_current_x= "<<person_companion_->get_v_current_robot_x().at(actual_best_path_index_)<< std::endl;
+				std::cout << "(save in file PERSON_COMPANION) v_current_y= "<<person_companion_->get_v_current_robot_y().at(actual_best_path_index_)<< std::endl;
+			}
+
+	if(Zanlungo_model_){
+		if(((!robot_desired_vel_calc_Zanlungo_.empty())&&(robot_desired_vel_calc_Zanlungo_.size()>actual_best_path_index_))&&(debug_output_screen_mesages_)){
+				std::cout << "(save in file PERSON_COMPANION) robot_desired_vel_calc_Zanlungo_= "<<robot_desired_vel_calc_Zanlungo_.at(actual_best_path_index_)<< std::endl;
+			}
+	}
+
+	debug_output_screen_mesages_=false;
+
+	if(!Zanlungo_model_){
+		std::cout << "(save in file PERSON_COMPANION) companion_force_my_model_to_compare_with_zanlungo_.fx= "<<beta_companion_*companion_force_my_model_to_compare_with_zanlungo_.fx<<"; fy="<<beta_companion_*companion_force_my_model_to_compare_with_zanlungo_.fy<< std::endl;
+		std::cout << "(save in file PERSON_COMPANION) goal_force_my_model_to_compare_with_zanlungo_.fx= "<<alpha_companion_*goal_force_my_model_to_compare_with_zanlungo_.fx<<"; fy="<<alpha_companion_*goal_force_my_model_to_compare_with_zanlungo_.fy<< std::endl;
+
+	}*/
+	//std::cout <<"ATR COMPANION robot_plan_companion3; debug 20"<< std::endl;
+	////////////
+	//robot_desired_vel_calc_Zanlungo_.clear();
+
+	///////////////////////
+	//debug_output_screen_mesages_=false;
+	/*if(Zanlungo_model_){
+		if((!saved_tree_forces_to_see_companion_Force_Zanlungo_.empty())&&(saved_tree_forces_to_see_companion_Force_Zanlungo_.size()>actual_best_path_index_)){
+				ant_final_force_total_=saved_tree_forces_to_see_companion_Force_Zanlungo_.at(actual_best_path_index_).f;
+			}else if(!companion_person_theta_path_.empty()){
+				companion_person_theta_path_.back();
+
+				if(!saved_tree_forces_to_see_companion_Force_Zanlungo_.empty()){
+					ant_final_force_total_=saved_tree_forces_to_see_companion_Force_Zanlungo_.back().f;
+				}else{
+					ant_final_force_total_=Sforce();
+				}
+
+			}
+	}*/
+
+	//////////// FIN MOMENTARILY output mesages of Zanlungo save in file here to see them for debug!!!
+
+
+	return result;
+}
+
+
+
+void Cplan_local_nav_person_companion::set_robot_external_goal( const Spoint& goal )
+{
+
+	Sdestination dest_goal(0,goal.x,goal.y,1.0);
+	extern_robot_goal_=dest_goal;
+	//std::vector<Sdestination> robot_dest(1,dest_goal);
+	//robot_->set_destinations( robot_dest );
+	//robot_->set_best_dest( dest_goal );
+	//robot_->get_best_dest();
+
+	if(debug_real_test_companion2_){
+		std::cout << std::endl  << std::endl  << std::endl<< " (function: set_robot_external_goal) [Cperson_abstract::Akp_planning/Reactiva_repulsive/Reactive_atractive] (set_robot_external_goal)  extern_robot_goal_.x"<<extern_robot_goal_.x<<"; extern_robot_goal_.y="<<extern_robot_goal_.y<< std::endl<< std::endl<< std::endl<< std::endl<< std::endl;
+	}
+
+	if(robot_companion_case_==Cperson_abstract::Akp_planning){
+		goal_ = goal;
+		//std::cout << "(function: set_robot_external_goal) [case akp_planning] goal_.x="<<goal_.x<<"; goal_.y="<<goal_.y << std::endl;
+
+		if(debug_real_test_companion2_){
+			std::cout << std::endl  << std::endl  << std::endl<< " [Cperson_abstract::Akp_planning] (set_robot_external_goal)  extern_robot_goal_.x=goal_.x"<<goal_.x<<"; extern_robot_goal_.y=goal_.y="<<goal.y<< std::endl<< std::endl<< std::endl<< std::endl<< std::endl;
+		}
+	}
+
+}
+
+void Cplan_local_nav_person_companion::set_robot_external_goal_fix( const Spoint& goal )
+{ // guardar siempre SOLO el goal externo sin cambiarlo cuando vas hacia la persona pq te alejaste demasiado.
+	Sdestination dest_goal(0,goal.x,goal.y,1.0);
+	extern_robot_goal_fix_=dest_goal;
+
+	//std::cout << "[function: set_robot_external_goal_fix] extern_robot_goal_fix_.x="<<extern_robot_goal_fix_.x<<"; extern_robot_goal_fix_.y="<<extern_robot_goal_fix_.y << std::endl;
+}
+
+
+
+void Cplan_local_nav_person_companion::set_robot_goal( const Spoint& goal )
+{
+
+	Sdestination dest_goal(0,goal.x,goal.y,1.0);
+	std::vector<Sdestination> robot_dest(1,dest_goal);
+	robot_->set_destinations( robot_dest );
+	robot_->set_best_dest( dest_goal );
+	//robot_->get_best_dest();
+	if(debug_real_test_companion2_){
+		//std::cout << std::endl  << std::endl  << std::endl<< " (function: set_robot_goal) [ robot_->set_best_dest]  (set_robot_goal)  robot dest=goal.x"<<goal.x<<"; goal.y="<<goal.y<< std::endl<< std::endl<< std::endl<< std::endl<< std::endl;
+	}
+
+	if((robot_companion_case_==Cperson_abstract::Reactive_atractive)||(robot_companion_case_==Cperson_abstract::Reactiva_repulsive)){
+		goal_ = goal;
+		//std::cout << "[(function: set_robot_goal), case atrative and repulsive] goal_.x="<<goal_.x<<"; goal_.y="<<goal_.y << std::endl;
+
+		if(debug_real_test_companion2_){
+			std::cout << std::endl  << std::endl  << std::endl<< " [Cperson_abstract::Reactive_atractive/Reactiva_repulsive] (set_robot_goal)  goal_=goal.x"<<goal.x<<"; goal_=goal.y="<<goal.y<< std::endl<< std::endl<< std::endl<< std::endl<< std::endl;
+		}
+	}else{
+		//goal_ = goal; // arreglar mal calculo caminos
+	}
+}
+
+
+// esta al usar al robot, he de cambiarla.
+void Cplan_local_nav_person_companion::set_robot_goal_person_companion_akp( const Spoint& goal )
+{
+
+	Sdestination dest_goal(0,goal.x,goal.y,1.0);
+	std::vector<Sdestination> robot_dest(1,dest_goal);
+	person_companion_->set_destinations( robot_dest );
+	person_companion_->set_best_dest( dest_goal );
+	//robot_->get_best_dest();
+	if(debug_real_test_companion2_){
+		std::cout << std::endl  << std::endl  << std::endl<< " [Cperson_abstract::Akp_planning/Reactiva_repulsive/Reactive_atractive]  (set_robot_goal)  robot dest=goal.x"<<goal.x<<"; goal.y="<<goal.y<< std::endl<< std::endl<< std::endl<< std::endl<< std::endl;
+	}
+
+	if((robot_companion_case_==Cperson_abstract::Reactive_atractive)||(robot_companion_case_==Cperson_abstract::Reactiva_repulsive)){
+		goal_ = goal;
+		//std::cout << "[set_robot_goal_person_companion_akp, case atrative and repulsive] goal_.x="<<goal_.x<<"; goal_.y="<<goal_.y << std::endl;
+		if(debug_real_test_companion2_){
+			std::cout << std::endl  << std::endl  << std::endl<< " [Cperson_abstract::Reactive_atractive/Reactiva_repulsive] (set_robot_goal)  goal_=goal.x"<<goal.x<<"; goal_=goal.y="<<goal.y<< std::endl<< std::endl<< std::endl<< std::endl<< std::endl;
+		}
+	}
+}
+
+void Cplan_local_nav_person_companion::set_robot_goal_person_goal_global_plan( const Spoint& goal )
+{
+	goal_ = goal;
+	Sdestination dest_goal(0,goal.x,goal.y,1.0);
+	std::vector<Sdestination> robot_dest(1,dest_goal);
+	person_companion_->set_destinations( robot_dest );
+	person_companion_->set_best_dest( dest_goal );
+}
+
+
+void Cplan_local_nav_person_companion::set_robot_goal_person_goal_global_plan_IN_robot_VERSION( const Spoint& goal )
+{
+	goal2_ = goal;
+	//std::cout << "[set_robot_goal_person_goal_global_plan_IN_robot_VERSION] goal_.x="<<goal_.x<<"; goal_.y="<<goal_.y << std::endl;
+	Sdestination dest_goal(0,goal.x,goal.y,1.0);
+	std::vector<Sdestination> robot_dest(1,dest_goal);
+	robot_->set_destinations( robot_dest );
+	robot_->set_best_dest( dest_goal );
+}
+
+
+
+/********************************************************************++++++*********+****************/
+/* Ini_robot_companion, para caso person companion akp*/
+
+bool Cplan_local_nav_person_companion::init_robot_plan2_person_companion(double robot_person_distance)
+{
+	// clear all data structures and push back the initial state : current state
+	if(debug_real_test_companion4_){
+		std::cout << "[INIIIIIIIII] horizon_time_="<<horizon_time_ << std::endl;
+		std::cout << "[INIIIIIIIII] max_iter_=***"<<max_iter_ << std::endl;
+		std::cout << "[INIIIIIIIII] workspace_radii_=***"<<workspace_radii_<< std::endl;
+	}
+
+	if(debug_init_robot_plan2_person_companion_){
+		std::cout << "IMPORTANTE! (1) init_robot_plan2_person_companion; print best dest:" << std::endl;
+		person_companion_->get_best_dest().print();
+		std::cout << "(1) init_robot_plan2_person_companion; print desired velocity:"<< person_companion_->get_desired_velocity()<< std::endl;
+	}
+
+	vector_of_companion_collisions_.clear();
+
+	v_alpha_.clear();
+	v_gamma_.clear();
+	v_delta_.clear();
+    edge_.clear();
+    cost_int_forces_.clear();
+    cost_robot_.clear();
+    cost_obstacles_.clear();
+    //cost_local_minima_.clear();
+    cost_distance_.clear();
+    cost_orientation_.clear();
+    nodes_in_branch_.clear();
+    random_goals_.clear();
+    random_goals2_.clear();
+    edge_.push_back( Sedge_tree_pcomp(0) );
+    cost_int_forces_.push_back( 0.0 );
+    cost_robot_.push_back(0.0);
+    cost_obstacles_.push_back( 0.0 );
+    //cost_local_minima_.push_back(0.0);
+    cost_distance_.push_back( 0.0 );
+    cost_orientation_.push_back( 0.0 );
+    cost_past_traj_.resize( max_iter_ , 0.0 );//no clear is needed, only the i-th iteration end of branch will contain a value
+    nodes_in_branch_.push_back(1.0);
+
+    cost_companion_.clear();  // new cost companion, added. (ely)
+    cost_companion_.push_back( before_initial_cost_ );
+    orientation_person_robot_angles_.clear();  // new variable, orientation companion (ely)
+    //orientation_person_robot_angles_.push_back( before_initial_angle_ ); // el orientation angle inicial asumimos que es angle_companion_=90grad
+    //angles_colisions_.clear();
+   // min_step_collision_distance_.clear();
+
+    parent_index_vector_.clear();
+    parent_index_vector_.push_back( 0 );
+    min_distance_collision_vector_.clear();
+    min_distance_collision_vector_.push_back(4);
+
+
+    double theta, angle;
+
+    bool scene_empty = get_scene()->empty();
+
+    if(debug_init_robot_plan2_person_companion_){
+    	std::cout << "(2) init_robot_plan2_person_companion" << std::endl;
+    	std::cout << "(2.1) init_robot_plan2_person_companion" << std::endl;
+    	std::cout << "(2.2) (print person_list) + scene_empty="<<scene_empty << std::endl;
+    	for( auto iit: person_list_ )
+    	{
+    		iit->print();
+    	}
+    }
+
+	//Cperson_abstract* person_obj;
+	//bool find_person_bool=find_person(id_person_companion_ , &person_obj);
+
+	if(debug_init_robot_plan2_person_companion_){
+		std::cout << "(2.2) init_robot_plan2_person_companion, id_person_companion_="<<id_person_companion_ << std::endl;
+	}
+
+	SpointV_cov person = person_companion_->get_current_pointV(); // person companion spoint
+	if(debug_init_robot_plan2_person_companion_){
+		std::cout << "(2) init_robot_plan2_person_companion (person_companion_->get_current_pointV()):" << std::endl;
+		person.print();
+		std::cout << "(2.2) init_robot_plan2_person_companion (person_companion_->get_best_dest().print()):" << std::endl;
+		person_companion_->get_best_dest().print();
+	}
+
+
+	if(debug_init_robot_plan2_person_companion_){
+		std::cout << "(2.3) init_robot_plan2_person_companion" << std::endl;
+	}
+	theta = atan2(person_companion_->get_best_dest().y-person.y ,person_companion_->get_best_dest().x-person.x);
+
+	if(debug_init_robot_plan2_person_companion_){
+		std::cout << "(2.4) init_robot_plan2_person_companion" << std::endl;
+	}
+	Spose robot=robot_->get_current_pose();
+
+	if(debug_init_robot_plan2_person_companion_){
+		std::cout << "(2.5) init_robot_plan2_person_companion" << std::endl;
+	}
+	angle=atan2(robot.y-person.y , robot.x-person.x);
+
+	if(debug_init_robot_plan2_person_companion_){
+		std::cout << "(3) init_robot_plan2_person_companion" << std::endl;
+	}
+    Spose person_companion_Spose_act = person_companion_->get_current_pose();
+    double theta_robot_person_companion=person_companion_Spose_act.theta;
+
+    if(debug_init_robot_plan2_person_companion_){
+    	std::cout << " (estas dos tendrían que ser iguales! o muy similares) theta="<<theta<<" theta_robot_person_companion="<<theta_robot_person_companion<< std::endl;
+    }
+
+    Spoint goal_person_companion;
+  //  const std::list<Cperson_abstract *>* person_list = get_scene( );
+
+    if(debug_init_robot_plan2_person_companion_){
+    	std::cout << "(4) init_robot_plan2_person_companion" << std::endl;
+    }
+
+
+     Sdestination person_companion_goal;
+     person_companion_goal=person_companion_->get_best_dest();
+     goal_person_companion=Spoint(person_companion_goal.x,person_companion_goal.y,person_companion_goal.time_stamp);
+        	//companion_person_position_=(*iit)->get_current_pointV();
+     if(debug_init_robot_plan2_person_companion_){
+    	 std::cout << "(5)  goal_person_companion:" << std::endl;
+    	 goal_person_companion.print();
+     }
+
+    if(debug_init_robot_plan2_person_companion_){
+    	std::cout << "(5) init_robot_plan2_person_companion" << std::endl;
+    }
+     // goal robot es al lado del goal inferido de la persona, para el caso person companion!!!
+    // TODO: importante!!! ver si estos set external goal hacen falta o no al cojer los goals desde fuera...
+    // set_robot_external_goal(goal_person_companion);
+    // set_robot_goal_person_companion_akp(goal_person_companion);
+////// the group goes to talk with other person. //////////////////////////////////
+
+///////////////////////////////////
+
+     if(debug_init_robot_plan2_person_companion_){
+    	 std::cout << "(6) init_robot_plan2_person_companion" << std::endl;
+     }
+     double ini_angle_act;
+     //double ant_ini_angle;
+
+     if( diffangle(theta, angle) < 0 ){
+    	 ini_angle_act=(theta + angle)*(180/3.14);
+    	 //std::cout << " (1) !!!!!!!! ini_angle_act (case: (theta + angle)); theta="<<theta*(180/3.14)<<"; angle="<<angle*(180/3.14) << std::endl;
+    	// ant_ini_angle=(robot_->get_current_pointV().angle_heading_point((Spoint) companion_person_position_))*(180/3.14);
+     }else{
+    	 ini_angle_act=(theta - angle)*(180/3.14);
+    	 //std::cout << " (1) !!!!!!!! ini_angle_act (case: (theta - angle)); theta="<<theta*(180/3.14)<<"; angle="<<angle*(180/3.14) << std::endl;
+    	// ant_ini_angle=(robot_->get_current_pointV().angle_heading_point((Spoint) companion_person_position_))*(180/3.14);
+     }
+
+     if(debug_init_robot_plan2_person_companion_){
+    	 std::cout << "(7) init_robot_plan2_person_companion" << std::endl;
+     }
+    // std::cout << " (1) !!!!!!!! ini_angle_act="<<ini_angle_act << std::endl;
+
+     if((ini_angle_act>=0)&&(ini_angle_act<=180)){
+      	ini_angle_act=ini_angle_act;
+      //	std::cout << " (2) !!!!!!!! ini_angle_act (case: (ini_angle_act>=0)&&(ini_angle_act<=180))"<< std::endl;
+     }else if((ini_angle_act<0)&&(ini_angle_act>=(-180))){
+      	ini_angle_act=sqrt(ini_angle_act*ini_angle_act);
+    	//std::cout << " (2) !!!!!!!! ini_angle_act (case: (ini_angle_act<0)&&(ini_angle_act>=(-180)))"<< std::endl;
+     }else if((ini_angle_act>180)&&(ini_angle_act<=360)){
+    	 ini_angle_act=360-ini_angle_act;
+    		//std::cout << " (2) !!!!!!!! ini_angle_act (case: (ini_angle_act>180)&&(ini_angle_act<=360))"<< std::endl;
+     }else if((ini_angle_act<(-180))&&(ini_angle_act>=(-360))){
+     	ini_angle_act=sqrt(ini_angle_act*ini_angle_act);
+     	ini_angle_act=360-ini_angle_act;
+    	//std::cout << " (2) !!!!!!!! ini_angle_act (case: (ini_angle_act<(-180))&&(ini_angle_act>=(-360))"<< std::endl;
+     }else if(ini_angle_act>360){
+    	 unsigned int n_360_g=ini_angle_act/360;
+    	 ini_angle_act=ini_angle_act-n_360_g*360;
+    	 // std::cout << " OJO!!! caso mayor 360 ; ini_angle_act=ini_angle_act-n_360_g*360="<<ini_angle_act<<"; n_360_g="<<n_360_g<< std::endl;
+     }else{
+        std::cout << " OJO!!! caso NO contemplado ; ini_angle_act="<<ini_angle_act<< std::endl;
+     }
+
+     if(debug_init_robot_plan2_person_companion_){
+    	 std::cout << "(8) init_robot_plan2_person_companion" << std::endl;
+     }
+     initial_angle_=ini_angle_act;
+     orientation_person_robot_angles_.push_back( initial_angle_ );
+
+    // std::cout << " (3) !!!!!!!! ini_angle_act="<<ini_angle_act << std::endl;
+
+     if(debug_init_robot_plan2_person_companion_){
+    	 std::cout << "(9) init_robot_plan2_person_companion" << std::endl;
+     }
+    //Correct current robot location and state: hypothesis the current robot state and the past robot action may differ.
+    //So we must take into account the delay and the system platform control, specially for the w, and propagate an estimation
+    //of the state at time t_now to time t_now + t_processing and sending command
+    person_companion_->correct_state_to_delay( last_pose_command_,  now_ , dt_ ); //mirar si necesario o no... (cambiar robot, por person companion)
+    person_companion_->clear_planning_trajectory();//first pose is inserted in the trajectory vector
+
+    if(debug_init_robot_plan2_person_companion_){
+    	std::cout << "(10) init_robot_plan2_person_companion" << std::endl;
+    }
+    // ----------------------------------------------------------------------
+    // set navigation goals
+
+   // person_companion_->set_a_v_max(robot_->get_v_max());
+
+    workspace_radii_ = horizon_time_ * person_companion_->get_v_max(); //(ver si esta bien para la person companion)
+
+    if(debug_init_robot_plan2_person_companion_){
+    	std::cout << "(11) init_robot_plan2_person_companion" << std::endl;
+    }
+    if(debug_real_test_companion4_){
+    std::cout << " (RADIIII) workspace_radii_= "<< workspace_radii_ << "; horizon_time_= "<< horizon_time_<<"; robot_->get_v_max()="<<robot_->get_v_max()<< std::endl;
+    }
+    if(debug_init_robot_plan2_person_companion_){
+    	std::cout << "(12) init_robot_plan2_person_companion" << std::endl;
+    }
+    //if ( workspace_radii_ < 0.1 ) return false;
+    local_v_goal_tolerance_ = person_companion_->get_v_max()+0.1; //sets the tolerance to the max value to always return true
+
+    if(debug_init_robot_plan2_person_companion_){
+    	std::cout << "(12) person_companion_->get_v_max()="<<person_companion_->get_v_max() << std::endl;
+    }
+    reaching_goal_ = false;
+
+    if(debug_init_robot_plan2_person_companion_){
+    	std::cout << "(13)person_companion_->get_current_pointV().distance( goal_)="<<person_companion_->get_current_pointV().distance( goal_) << std::endl;
+       	std::cout << "print goal:" << std::endl;
+    	goal_.print();
+    	std::cout << "(12) print local_goal" << std::endl;
+    }
+
+    if (workspace_radii_ > person_companion_->get_current_pointV().distance( goal_) )
+	{
+		local_goal_ = goal_;
+		// New mode entering subgoals, the radius should be the maximum
+		//workspace_radii_ = robot_->get_current_pointV().distance( goal_) + 0.5;
+		//Set velocity to tolerance when reaching the final goal, only very near
+		if( person_companion_->get_current_pointV().distance2( goal_) < xy_2_goal_tolerance_*2.0 )
+		{
+			//the velocity tolerance depends on the parameter, and the algorithm will check this condition in order to consider
+			//a goal properly reached
+			local_v_goal_tolerance_ = v_goal_tolerance_;
+			reaching_goal_ = true;
+		}
+
+	}
+	else
+	{
+		// In this case the final velocity is not an issue because the plan will never reach it final goal
+		Spoint diff = goal_ - (Spoint)person_companion_->get_current_pointV();
+		double r = sqrt(diff.x*diff.x +  diff.y * diff.y);
+		local_goal_ = Spoint( person_companion_->get_current_pointV().x + workspace_radii_ * diff.x / r ,
+				person_companion_->get_current_pointV().y + workspace_radii_ * diff.y / r );
+	}
+
+	/*if(debug_goal_go_to_person_in_person_companion_){
+		std::cout << " [PERSON] local_goal_.x= "<<local_goal_.x << "; local_goal_.y= "<< local_goal_.y<<"; goal_.x="<<goal_.x<<"; goal_.y="<<goal_.y<< std::endl;
+	}*/
+
+    if(debug_init_robot_plan2_person_companion_){
+    	std::cout << "(12) print local_goal" << std::endl;
+    	local_goal_.print();
+    }
+
+	person_companion_->set_rnd_local_goal( local_goal_ );
+    //now_ = set when updated
+	if(debug_init_robot_plan2_person_companion_){
+		std::cout << "(13) init_robot_plan2_person_companion" << std::endl;
+	}
+    // ----------------------------------------------------------------------
+    //select persons considered in the scene and reserve memory for planning
+    Spoint robot_position = (Spoint) person_companion_->get_current_pointV(); //ojo! esta es más interna todabia! (hay que cambiarla tambien)
+    double d_ini,d_end,radii_2(workspace_radii_*workspace_radii_),d_min(1e10);
+
+    if(debug_init_robot_plan2_person_companion_){
+    	std::cout << "(14) init_robot_plan2_person_companion" << std::endl;
+    }
+    nearby_person_list_.clear();
+    //std::cout << " [fallo] person_list_= "<< person_list_.size() << std::endl;
+    for( auto iit: person_list_ ) // guardas las personas cercanas a la person companion. (OJO!, quitar a la person companion!!!)
+    {
+    	//It requires prior trajectory prediction, careful...
+    	d_ini = iit->get_prediction_trajectory()->front().distance2( robot_position );
+    	d_end = iit->get_prediction_trajectory()->back().distance2( robot_position );
+    	if(( d_ini < radii_2 || d_end < radii_2 ) && (iit->get_id()!=id_person_companion_))
+    	{
+    		nearby_person_list_.push_back( iit );
+    		iit->clear_planning_trajectory();
+    		iit->reserve_planning_trajectory( max_iter_ );//if already of this size, does nothing
+
+    	}
+    	//iit->print();
+    	//check for the neares obstacle in order to calculate velocities
+    	if ( d_ini < d_min)
+    		d_min = d_ini;
+
+    }
+
+    if(debug_init_robot_plan2_person_companion_){
+    	std::cout << "(15) init_robot_plan2_person_companion" << std::endl;
+    }
+    // ----------------------------------------------------------------------
+    //number of nearby obstacles: first approach, just count them...
+    clock_t obstacles_start, obstacles_end;
+    obstacles_start=clock();
+    int number_of_obstacles(0);
+    for( auto iit: laser_obstacle_list_ )
+    {
+    	d_ini = iit.distance2( robot_position );
+    	//d_ini = iit.distance( robot_position );
+
+    	//std::cout << " radii_2= "<< radii_2<< " d_ini="<<d_ini << std::endl;
+
+    	if( d_ini < radii_2  )
+    	//if( d_ini < radii  )
+    	{
+    		//std::cout << " in if( d_ini < radii_2  ) "<< std::endl;
+    		number_of_obstacles++;
+    	}
+    	if ( d_ini < d_min )
+    		d_min = d_ini;
+
+    }
+    number_of_obstacles_=number_of_obstacles;
+
+   if(debug_init_robot_plan2_person_companion_){
+    	std::cout << " number_of_obstacles_= "<<number_of_obstacles_<< " number_of_obstacles="<<number_of_obstacles<< "; workspace_radii_="<<workspace_radii_ << std::endl;
+   }
+
+    obstacles_end=clock();
+    if(check_execution_times_){
+    	std::cout << " number_of_obstacles= "<< number_of_obstacles << std::endl;
+    	std::cout << " time_obstacles_=obstacles_start-obstacles_end= "<< ((obstacles_end-obstacles_start)/clocks_per_sec_my_var_)*1000<< std::endl;
+    }
+
+    if(debug_init_robot_plan2_person_companion_){
+    	std::cout << "(16) init_robot_plan2_person_companion" << std::endl;
+    }
+    d_min_global_=d_min;
+    dmin_global_=d_min;
+
+   Spoint robot_act_goal=get_robot_goal(); // TODO: mirar k hace esta funcion. (retorna goal_)
+   if(debug_nadal_){
+	   std::cout << " (ini_robot_plan2) robot_act_goal.x="<<robot_act_goal.x<<"robot_act_goal.y="<<robot_act_goal.y<< std::endl;
+   }
+  /***************Fin copia seguridad velocidades*******************************/
+    //depending on the number of persons considered, set the std of the std_goal_workspace [rad]
+
+   int obstacles=(int)nearby_person_list_.size()-1 + number_of_obstacles;
+
+   if(debug_init_robot_plan2_person_companion_){
+	   std::cout << "(17) init_robot_plan2_person_companion" << std::endl;
+   }
+   if(obstacles<0){
+	   obstacles=0;
+   }
+
+    switch( obstacles )  // std_goal_workspace_ ha de ser siempre muy cercano a 1, has de intentar ir siempre hacia el goal de la persona que acompañas y NO desviarte con un planning alternativo que evite mucho los obstaculos!
+    {
+      case 0:
+		std_goal_workspace_ = 0.4;
+		break;
+      case 1:
+		std_goal_workspace_ = 0.5;
+		break;
+      case 2:
+		//std_goal_workspace_ = 1.4;
+    	 std_goal_workspace_ = 0.6; // mirar si la std, se habre un poco para la persona simulada, también.
+		break;
+      case 3:
+		//std_goal_workspace_ = 1.6;
+    	 std_goal_workspace_ = 0.7;
+		break;
+      case 4:
+		//std_goal_workspace_ = 1.8;
+    	  std_goal_workspace_ = 0.8;
+		break;
+      case 5:
+		//std_goal_workspace_ = 2.0;
+    	 std_goal_workspace_ = 0.9;
+		break;
+      default:
+      case 6:
+  		//std_goal_workspace_ = 2.1;
+    	 std_goal_workspace_ = 1.0;
+  		break;
+    }
+		/*if(debug_goal_go_to_person_in_person_companion_){
+		std::cout << " IIIIIIIIIIIIII std_goal_workspace_="<<std_goal_workspace_<< std::endl;
+		}*/
+   // std::cout << "FIN ="<< std::endl;
+    if(debug_init_robot_plan2_person_companion_){
+    	std::cout << "(18) init_robot_plan2_person_companion" << std::endl;
+    }
+
+    return true;
+}
+
+/* fin Ini_robot companion, para caso person companion akp*/
+
+bool Cplan_local_nav_person_companion::robot_plan_anticipative_krrt_companion_person_companion( Cperson_abstract::companion_reactive reactive)
+{
+	if(debug_robot_plan_anticipative_krrt_companion_person_companion_){
+		std::cout << " (1) INI robot_plan_anticipative_krrt_companion"<< std::endl;
+	}
+
+	saved_tree_forces_to_see_companion_Force_Zanlungo_.clear();
+
+	person_robot_actual_real_distance_=calc_robot_person_companion_distance_companion_person_akp();
+
+	clock_t krrt_companion_start, krrt_companion_end;
+	krrt_companion_start=clock();
+	clock_t bucle_krrt_companion_start, bucle_krrt_companion_end;
+	bucle_krrt_companion_start=clock();
+
+	Sedge_tree_pcomp input(0);
+	Spoint random_goal;
+	unsigned int parent_vertex_index(0),previous_parent_vertex(0), number_of_cost_to_go(0);
+	//first goal is the main goal
+	random_goal = local_goal_;
+
+	//std::cout << " (1) local_goal_.x="<<local_goal_.x<<"; local_goal_.y="<<local_goal_.y<< std::endl;
+	//std::cout << " (1) person_list_.size()="<<person_list_.size()<<"; nearby_person_list_.size()="<<nearby_person_list_.size()<< std::endl;
+
+	if(debug_real_test_companion4_){
+		std::cout << "(IN robot_plan_anticipative_krrt_companion) random_goal.x="<<random_goal.x<<"; random_goal.y="<<random_goal.y<< std::endl;
+	}
+	random_goals_.push_back( random_goal );
+	collision_detected_.resize(max_iter_,false);
+	collision_detected_[0] = false;
+	double distance2_to_goal(1.0);
+
+	if(debug_robot_plan_anticipative_krrt_companion_person_companion_){
+		std::cout << "(IN robot_plan_anticipative_krrt_companion) max_iter_="<<max_iter_<< std::endl;
+		std::cout << "a_v_break="<<person_companion_->get_a_v_break()<<"; a_v_max()="<<person_companion_->get_a_v_max()<<"; a_w_max="<<person_companion_->get_a_w_max()<< std::endl;
+		std::cout << "v_max="<<person_companion_->get_v_max()<<"; w_max()="<<person_companion_->get_w_max()<<"; desired_velocity="<<person_companion_->get_desired_velocity()<< std::endl;
+	}
+
+	//person_companion_->get_a_v_break()
+	//person_companion_->get_a_v_max()
+	//person_companion_->get_a_w_max()
+	//person_companion_->get_v_max()
+	//person_companion_->get_w_max()
+	//person_companion_->get_desired_velocity()
+
+	//std::cout << " (2) robot_plan_anticipative_krrt_companion; person_list_.size()="<<person_list_.size()<<"; nearby_person_list_.size()="<< nearby_person_list_.size()<< std::endl;
+
+	for( unsigned int i = 1; i<max_iter_ && number_of_cost_to_go < max_iter_/5; ++i )
+	{
+		//std::cout << "for 1n"<< std::endl;
+		distance2_to_goal = person_companion_->get_robot_planning_trajectory()->back().distance2( local_goal_ );
+		//std::cout << " (3) robot_plan_anticipative_krrt_companion"<< std::endl;
+		/*std::cout << " (3) robot_plan_anticipative_krrt_companion"<< std::endl;
+		std::cout << "time="<<person_companion_->get_robot_planning_trajectory()->back().time_stamp - now_<<"> horizon_time_="<<horizon_time_<< std::endl;
+		std::cout <<" collision_detected_[i-1]=" <<collision_detected_[i-1]<< std::endl;
+		std::cout <<"; person_companion_->get_robot_planning_trajectory()->back().v="<<person_companion_->get_robot_planning_trajectory()->back().v<<"< local_v_goal_tolerance_="<<local_v_goal_tolerance_<< std::endl;
+		std::cout <<"xy_2_goal_tolerance_"<<xy_2_goal_tolerance_<<" > distance2_to_goal="<<distance2_to_goal<<"; local_goal:"<< std::endl;
+		local_goal_.print();*/
+		/*if(!person_companion_->get_robot_planning_trajectory()->empty()){
+			std::cout << "person_companion_->get_robot_planning_trajectory()->back().time_stamp= "<<person_companion_->get_robot_planning_trajectory()->back().time_stamp<< std::endl;
+			std::cout << "current diff time "  << person_companion_->get_robot_planning_trajectory()->back().time_stamp - now_ << std::endl;
+		}else{
+			std::cout << "person_companion_->get_robot_planning_trajectory()->empty()="<<person_companion_->get_robot_planning_trajectory()->empty()<< std::endl;
+		}
+		std::cout << "now_"<<now_<< std::endl;
+		std::cout << "horizon_time_"<<horizon_time_<< std::endl;
+		std::cout << "collision_detected_[i-1]="<<collision_detected_[i-1]<< std::endl;
+		 	*/
+
+
+		if( (person_companion_->get_robot_planning_trajectory()->back().time_stamp - now_ > horizon_time_) || (collision_detected_[i-1])
+				|| (distance2_to_goal  < xy_2_goal_tolerance_ && //distance to goal and velocity
+						person_companion_->get_robot_planning_trajectory()->back().v < local_v_goal_tolerance_ )) //this local tolerance depends if the goal is reachable before h
+		{
+			//std::cout << " (4) robot_plan_anticipative_krrt_companion"<< std::endl;
+			//std::cout << "(entro if largo...)"<< std::endl;
+			// add end of branch, either horizon time is reached or goal is reached
+			if (!collision_detected_[i-1] )
+			{
+				//std::cout << " (4.1) robot_plan_anticipative_krrt_companion"<< std::endl;
+				end_of_branches_index_.push_back( i-1 );
+				//distance to last trajectory calculated: only last calculated position in both paths... simplified version
+				if ( best_planning_trajectory_.empty() )
+					cost_past_traj_[i-1] = 0.0;
+				else
+					cost_past_traj_[i-1] = best_planning_trajectory_.front().distance2( person_companion_->get_robot_planning_trajectory()->back()  );
+
+				//std::cout << " collision detected; best_planning_trajectory_.empty()=" <<best_planning_trajectory_.empty()<<"; end_of_branches_index_.empty()="<<end_of_branches_index_.empty()<< std::endl;
+			}
+			//colisions are discarted
+			else {
+				//std::cout << " (4.2) collision detected; best_planning_trajectory_.empty()=" <<best_planning_trajectory_.empty()<<"; end_of_branches_index_.empty()="<<end_of_branches_index_.empty()<< std::endl;
+				//person_companion_->get_robot_planning_trajectory()->back().print();
+			}
+			//std::cout << " (5) robot_plan_anticipative_krrt_companion"<< std::endl;
+		//2- Sample workspace
+			//std::cout << "for 2n"<< std::endl;
+			random_goal = sample_workspace_for_person_companion_akp();
+			random_goals_.push_back( random_goal );
+			person_companion_->set_rnd_local_goal( random_goal );
+			//std::cout << " (6) robot_plan_anticipative_krrt_companion"<< std::endl;
+			//std::cout << "(IN robot_plan_anticipative_krrt_companion) random_goal.x="<<random_goal.x<<"; random_goal.y="<<random_goal.y<<"; random_goals2_.size()="<<random_goals2_.size()<< std::endl;
+			//std::cout << "for 3n"<< std::endl;
+		//3- find nearest vertex to expand the tree
+			parent_vertex_index = find_nearest_vertex_person_companion_akp( random_goal ); // dentro de esta esta el cost_to_go!!!
+			//std::cout << " (6.1) robot_plan_anticipative_krrt_companion"<< std::endl;
+			++number_of_cost_to_go;
+			previous_parent_vertex = i;
+			//std::cout << " (6.2) robot_plan_anticipative_krrt_companion"<< std::endl;
+			reset_scene_persons_propagation_flag(); //ok, the same!
+			//std::cout << "for 4n"<< std::endl;
+			//std::cout << " (7) robot_plan_anticipative_krrt_companion"<< std::endl;
+		}
+		else
+		{
+			parent_vertex_index = previous_parent_vertex;
+			previous_parent_vertex = i;
+		}
+		//std::cout << " (8) robot_plan_anticipative_krrt_companion"<< std::endl;
+
+
+		//std::cout << "for 5n"<< std::endl;
+		//check to see if goal is currently the local goal; in that case, the velocity has to be adjusted to stop
+		if ( reaching_goal_ )
+		{
+			//set robot desired velocity to 0 when inside the goal ball
+			if( distance2_to_goal < xy_2_goal_tolerance_ )
+			{
+				person_companion_->set_desired_velocty( 0.0 );
+
+			}
+			else // robot desired velocity is again max velocity
+				person_companion_->set_desired_velocty( person_companion_->get_v_max () );//robot always tries to be at max velocity, modified by nearby ppl
+		}
+		//std::cout << " (8.1) robot_plan_anticipative_krrt_companion"<< std::endl;
+		//std::cout << "for 6n"<< std::endl;
+		//4-calculate the input u to propagate towards the random goal
+		random_goals2_.push_back(random_goal);
+		//std::cout << " (8.2) robot_plan_anticipative_krrt_companion"<< std::endl;
+		input = calculate_edge_person_companion_akp(parent_vertex_index, Sdestination(0,random_goal.x,random_goal.y),reactive);
+		//std::cout << " (8.3) robot_plan_anticipative_krrt_companion"<< std::endl;
+		//5- Propagate tree.
+		collision_detected_[i] = propagate_vertex_person_companion_akp( parent_vertex_index, input );
+		//std::cout << " (9) robot_plan_anticipative_krrt_companion"<< std::endl;
+		/*if(collision_detected_[i]){
+			std::cout << "% collision_detected_["<<i<<"]"<<collision_detected_[i] << std::endl;
+		}*/
+		//std::cout << "for 7n"<< std::endl;
+		if(!collision_detected_[i]){
+			// si no hay colision!!! guardas los parametros!
+			v_alpha_.push_back(alpha_);
+			v_gamma_.push_back(gamma_);
+			v_delta_.push_back(delta_);
+		}
+		//std::cout << " (10) robot_plan_anticipative_krrt_companion"<< std::endl;
+		calculate_cost_person_companion_akp(parent_vertex_index, input,reactive);
+		//6-calculate the propagation cost
+		edge_.push_back( input );
+	}
+
+	//std::cout << " (11) robot_plan_anticipative_krrt_companion"<< std::endl;
+	bucle_krrt_companion_end=clock();
+
+	if(debug_real_test_companion_){
+		std::cout << "% time_bucle_krrt_companion=bucle_krrt_companion_start-bucle_krrt_companion_end"<<(bucle_krrt_companion_end-bucle_krrt_companion_start)/1000 << std::endl;
+	}
+
+	if(debug_nadal_){
+		std::cout << " [ANTES CAMBIAR ANGULOS] orientation_person_robot_angles_[0]="<<orientation_person_robot_angles_[0]<< std::endl;
+	}
+	//7- return tree. Seek for the minimum cost branch of the tree
+	//std::cout << "out for 8n"<< std::endl;
+	//only_angle_in_final_tree_calculate_companion_path_angle_and_cost(); //(companion ely)change path of angle companion between robot and person.
+	Crobot* robot_act=person_companion_;
+
+
+
+	unsigned int min_branch_index = global_min_cost_index(robot_act , reactive);
+
+
+		//std::cout << " (12) robot_plan_anticipative_krrt_companion"<< std::endl;
+	if(reactive==Cperson_abstract::Akp_planning){
+		//only_angle_in_final_tree_calculate_companion_path_angle_and_cost(min_branch_index);
+		if(overpas_obstacles_behind_person_){
+			//go_behind_robot_only_angle_in_final_tree_calculate_companion_path_angle_and_cost2(min_branch_index);
+			go_behind_robot_only_angle_in_final_tree_calculate_companion_path_angle_and_cost3(min_branch_index);
+
+		}else{
+			//only_angle_in_final_tree_calculate_companion_path_angle_and_cost2(min_branch_index);
+			go_in_front_robot_only_angle_in_final_tree_calculate_companion_path_angle_and_cost3(min_branch_index);
+		}
+
+
+	}
+	//std::cout << " (13) robot_plan_anticipative_krrt_companion"<< std::endl;
+	//return_next_robot_position_companion_cost_and_angle(min_branch_index);
+	//std::cout << "out for 9n"<< std::endl;
+	// Esta luego habra que comentar el IF para ver que los angulos en el path los haga bien.
+	if((reactive==Cperson_abstract::Akp_planning)&&(we_have_cost_companion_) && debug_real_test_companion4_){//&&(actual_debug2_)){
+		see_companion_path_angle_and_cost_of_min_cost_paths(min_branch_index);
+	}
+
+	if(debug_real_test_companion2_){
+		std::cout << "min_next_companion_angle_= " <<min_next_companion_angle_<< std::endl;
+		std::cout << "min_next_companion_cost_= " <<min_next_companion_cost_<< std::endl;
+
+	}
+	// cout best candidate paths and costs
+	//std::cout << "best path candidate = " << min_branch_index << std::endl;
+	//std::cout  << 	cost_int_forces_[min_branch_index] << " , " << cost_robot_[min_branch_index] << " , " <<  cost_obstacles_[min_branch_index] << " , " <<
+	//		cost_distance_[min_branch_index] << " , " << cost_orientation_[min_branch_index] << std::endl;
+	min_branch_index_save_=min_branch_index;
+	//std::cout << "out for 10n"<< std::endl;
+
+
+	best_plan_vertex_index_.clear();
+	best_planning_trajectory_.clear();
+	//saved_tree_forces_to_see_companion_Force_Zanlungo_best_path_.clear();
+
+	if ( end_of_branches_index_.empty() ){
+		//std::cout << "(return false) end_of_branches_index_.empty()="<<end_of_branches_index_.empty() << std::endl;
+		return false;
+	}
+	//std::cout << "out for 11n"<< std::endl;
+	//8- calculate vector of vertex. In reverse order the vertexes of the best path (initial not
+	// included as it doesn't provide information)
+	//std::cout << "% distance, orientation, robot, int, obstacles" << std::endl;
+	unsigned int y=0;
+	do
+	{
+		if(debug_antes_subgoals_entre_AKP_goals_){
+			std::cout << "% ("<<y<<") min_branch_index="<<min_branch_index << std::endl;
+		}
+		best_plan_vertex_index_.push_back( min_branch_index);
+		best_planning_trajectory_.push_back( person_companion_->get_robot_planning_trajectory()->at(min_branch_index) );
+
+		if(debug_antes_subgoals_entre_AKP_goals_){
+			std::cout << "> robot.x="<<person_companion_->get_robot_planning_trajectory()->at(min_branch_index).x <<"; robot.y="<<person_companion_->get_robot_planning_trajectory()->at(min_branch_index).y <<"; robot.time_stamp="<<person_companion_->get_robot_planning_trajectory()->at(min_branch_index).time_stamp << std::endl;
+			std::cout << "> parent="<<edge_[ min_branch_index ].parent<< std::endl;
+		}
+
+		min_branch_index = edge_[ min_branch_index ].parent;
+		y++;
+
+	}while( min_branch_index > 0 );
+
+	//std::cout << " (14) robot_plan_anticipative_krrt_companion"<< std::endl;
+	//std::cout << "out for 12n"<< std::endl;
+	//replan_min_cost_branch();
+
+	if(debug_antes_subgoals_entre_AKP_goals_){
+		std::cout << "% best_planning_trajectory_.size()="<<best_planning_trajectory_.size() << std::endl;
+	}
+
+	// clear data structures. Out of this function, there is no guarantee that data has not changed
+    nearby_person_list_.clear();
+    //end of trajectories indexes
+    end_of_branches_index_.clear();
+	krrt_companion_end=clock();
+
+	if(debug_real_test_companion4_){
+		std::cout << "% (FIN) time_krrt_companion=krrt_companion_start-krrt_companion_end"<<(krrt_companion_end-krrt_companion_start)/1000 << std::endl;
+	}
+	//std::cout << "out for 13n"<< std::endl;
+	//std::cout << " (15) robot_plan_anticipative_krrt_companion"<< std::endl;
+    return true;
+}
+
+
+Spose Cplan_local_nav_person_companion::replan_last_step_person_companion(unsigned int index, Spose robot_pose_ini, SpointV robot_Spoint_ini, Sdestination robot_goal, double dt, Sdestination robot_final_goal)
+{ // retorna la Spose final.  // index es solo para la el angulo y para la posición de la persona, el goal de la persona.
+	std::cout << "7777777777 IN!!!!!!!!!!!! replan_last_step_person_companion; index="<<index<< std::endl;
+
+	std::cout << "robot_pose_ini.print:"<< std::endl;
+	robot_pose_ini.print();
+
+	akp_out_path_goal_=robot_final_goal;
+	akp_out_companion_goal_=robot_goal;
+	std::cout << "akp_out_path_goal_.print:"<< std::endl;
+	akp_out_path_goal_.print();
+	std::cout << "akp_out_companion_goal_.print:"<< std::endl;
+	akp_out_companion_goal_.print();
+
+
+	//std::cout << "final_v_robot_max_="<<final_v_robot_max_<< std::endl;
+	double fobos_Xmod;
+	double fobos_Ymod;
+	double signo_x;
+	double signo_y;
+
+	Sforce f_goal, f_int, f_obs, f,f_person_goal, f_goal_final;
+
+	if(debug_antes_subgoals_entre_AKP_goals_){
+		std::cout << "replan_last_step_person_companion -> min_branch_index="<<index << std::endl;
+	}
+
+//	Spose robot_pose=robot_pose_ini;//best_planning_trajectory_.at(index);
+
+	if(debug_antes_subgoals_entre_AKP_goals_){
+		std::cout << "> (ant) robot_pose=robot_pose_ini.x="<<robot_pose_ini.x <<"; robot_pose_ini.y="<<robot_pose_ini.y<< std::endl;
+	}
+		// replan the min_cost_branch to do the planing taking into account the angle respect to the person.
+		//caso planning, normal Gonzalo.
+	Sdestination final_goal=robot_final_goal;
+
+	//std::cout << "> caso 3 force_goal_near"<< std::endl;
+	f_goal = person_companion_->force_goal_near( final_goal, get_sfm_params(person_companion_),&robot_Spoint_ini,0.5,false,real_vel_per); // 2.0 = distance reduce velocity and force when goal is near!
+
+	Sdestination companion_goal=robot_goal;
+
+	//f_goal = person_companion_->force_goal( robot_goal, get_sfm_params(person_companion_),&robot_Spoint_ini);
+	//std::cout << "> caso 1 force_goal_near"<< std::endl;
+	f_person_goal=person_companion_->force_goal_near( companion_goal, get_sfm_params(person_companion_),&robot_Spoint_ini,2.0,false,real_vel_per);
+
+	if(debug_real_test_companion3_){
+		std::cout << "> (INI replan_last_step) robot_pose=robot_pose_ini.x="<<robot_pose_ini.x <<"; robot_pose_ini.y="<<robot_pose_ini.y<< std::endl;
+		std::cout << "> robot_goal.x="<<robot_goal.x <<"; robot_goal.y="<<robot_goal.y<< std::endl;
+		std::cout << "> f_goal.fx="<<f_goal.fx <<"; f_goal.fy="<<f_goal.fy<< std::endl;
+	}
+
+	f_int = force_persons_int_planning_virtual_robot_companion_propagation(person_companion_,index);//force_persons_int_planning_virtual( robot_, index);
+	SpointV robot = person_companion_->get_planning_trajectory()->at(index);
+	// TEST: person_companion include force with the tibi=robot
+	SpointV robot_tibi_point=SpointV(robot_->get_current_pointV().x,robot_->get_current_pointV().y,robot_->get_current_pointV().time_stamp,robot_->get_current_pointV().vx,robot_->get_current_pointV().vy);
+	Sforce f_person_comp_to_tibi=person_companion_->force(robot_tibi_point,get_sfm_int_params(person_companion_,robot_),&robot);
+	//f_person_comp_to_tibi.fx=f_person_comp_to_tibi.fx/2;
+	//f_person_comp_to_tibi.fy=f_person_comp_to_tibi.fy/2;
+	f_int +=f_person_comp_to_tibi;
+	force_int_between_person_comp_and_robot_=f_person_comp_to_tibi;
+
+	//map force, used for simulations
+	if( read_force_map_success_ )
+		f_obs = get_force_map(person_companion_->get_robot_planning_trajectory()->at(index).x,
+				person_companion_->get_robot_planning_trajectory()->at(index).y);
+			//obstacles due to laser scans. for real environments has priority over map forces
+
+	//std::cout << " (1) f_obs.fx= "<<f_obs.fx<<"; f_obs.fy="<<f_obs.fy<<"; v_gamma_[index]="<<v_gamma_[index]<< std::endl;
+
+	if(read_laser_obstacle_success_)
+		f_obs = force_objects_laser_int_planning_virtual( person_companion_, index, 25.0, true );
+
+	//std::cout << "(2) f_obs.fx= "<<f_obs.fx<<"; f_obs.fy="<<f_obs.fy<<"; v_gamma_[index]="<<v_gamma_[index]<< std::endl;
+
+
+	//std::cout << " IN :calculate_edge (antes limitar f_obs); f_obs.fx="<<f_obs.fx<<"; f_obs.fy="<<f_obs.fy<<"; delta_="<<delta_<<std::endl;
+		// INICION Limitar fuerza obstaculos cuando hay muchos solapados
+		fobos_Xmod=sqrt(f_obs.fx*f_obs.fx);
+		fobos_Ymod=sqrt(f_obs.fy*f_obs.fy);
+
+		if (f_obs.fx > 0){
+			signo_x=1;
+		}else{
+			signo_x=-1;
+		}
+		if (f_obs.fy > 0){
+			signo_y=1;
+		}else{
+			signo_y=-1;
+		}
+
+		if((fobos_Xmod>f_obst_max_x_)||(fobos_Ymod>f_obst_max_y_)){
+			f_obs.fx=signo_x*f_obst_max_x_;
+			f_obs.fy=signo_y*f_obst_max_y_;
+		}
+		//std::cout << "(FINAL FORCE) f_obs.fx="<<f_obs.fx<<"; f_obs.fy="<<f_obs.fy<< std::endl;
+		// FIN Limitar fuerza obstaculos cuando hay muchos solapados
+		//std::cout << "(3) f_obs.fx= "<<f_obs.fx<<"; f_obs.fy="<<f_obs.fy<<"; v_gamma_[index]="<<v_gamma_[index]<< std::endl;
+
+	last_step_robot_obstacles_cost_=f_obs.module2();
+
+	if(debug_real_test_companion4_){
+		std::cout << " f_obs.fx= "<<f_obs.fx<<"; f_obs.fy="<<f_obs.fy<< std::endl;
+		std::cout << " f_int.fx= "<<f_int.fx<<"; f_int.fy="<<f_int.fy<< std::endl;
+	}
+	//	std::cout << " alpha_= "<<alpha_<<"; gamma_"<<gamma_<<"; delta_"<<delta_<< std::endl;
+
+	//f=f_goal_final*alpha_ + f_int*gamma_ + f_obs*delta_;
+		double alpha_a=0.8; // 0.1
+		double beta_a=0.2;  // 0.6 Parametros segun paper de Gonzalo
+		double gamma_a=5.0;
+		double delta_a=0.5;
+
+		//f=f_goal*v_alpha_[index] + f_int*v_gamma_[index] + f_obs*v_delta_[index]; // prueba, con parametros articulo revista robot companion
+		f=f_goal*alpha_a + f_person_goal*beta_a + f_int*gamma_a + f_obs*delta_a; // prueba, con parametros articulo revista robot companion
+
+	//	f_goal_final=f_goal*alpha_a+f_person_goal*beta_a;
+		std::cout << " f_goal.fx= "<<f_goal.fx<<"; f_goal.fy="<<f_goal.fy<<"; alpha_a="<<alpha_a<<"; v_alpha_[index]="<<v_alpha_[index]<< std::endl;
+		std::cout << " f_person_goal.fx= "<<f_person_goal.fx<<"; f_person_goal.fy="<<f_person_goal.fy<<"; beta_a="<<beta_a<< std::endl;
+		std::cout << " f_int.fx= "<<f_int.fx<<"; f_int.fy="<<f_int.fy<<"; gamma_a="<<gamma_a<< std::endl;
+		std::cout << " f_obs.fx= "<<f_obs.fx<<"; f_obs.fy="<<f_obs.fy<<"; delta_a="<<delta_a<< std::endl;
+		std::cout << "[fuerza final] f.fx= "<<f.fx<<"; f.fy="<<f.fy<< std::endl;
+
+	if(debug_real_test_companion3_){
+	std::cout << " (Akp_planning, fuerza final y parametros):  f.fx="<<f.fx<<"; f.fy="<<f.fy<<"; alpha_="<<alpha_<<"; gamma_"<<gamma_<<"; delta_="<<delta_<<std::endl;//"; f_int.fx"<<f_int.fx<<"; f_int.fy="<<f_int.fy<<std::endl;
+	}
+
+	Sedge_tree_pcomp u( index, f, f_goal, f_int, f_obs,f_person_goal);
+	u_forces_robot_actual_=u;
+
+	//std::cout << "> robot_pose_ini.x="<<robot_pose_ini.x <<"; robot_pose_ini.y="<<robot_pose_ini.y<< std::endl;
+	//std::cout << "> robot_pose_ini.v="<<robot_pose_ini.v <<"; robot_pose_ini.w=s"<<robot_pose_ini.w<<"; robot_pose_ini.theta="<<robot_pose_ini.theta<< std::endl;
+	//std::cout << "(ANT) dt="<<dt<< std::endl;
+
+	//std::cout << " (replan_last_step) robot_->robot_propagation_companion_position"<< std::endl;
+	//Spose robot_propagation = person_companion_->robot_propagation( dt_, 0 , u.f );
+	Spose robot_propagation=person_companion_->robot_propagation_companion_position( dt, u.f ,robot_pose_ini,false); // le doy la pose que quiero que me propague y la fuerza con la que quiero que haga la propagacion y me da la siguiente pose!
+	//std::cout << "> (final pose robot) robot_propagation.x="<<robot_propagation.x <<"; robot_propagation.y="<<robot_propagation.y<< std::endl;
+	//std::cout << "> robot_propagation.v="<<robot_propagation.v <<"; robot_propagation.w="<<robot_propagation.w<<"; robot_propagation.theta="<<robot_propagation.theta<< std::endl;
+
+	//if(debug_real_test_companion3_){
+		//std::cout << "> (neW replan_last_step) robot_propagation.x="<<robot_propagation.x <<"; robot_propagation.y="<<robot_propagation.y<< std::endl;
+		//std::cout << "> (neW replan_last_step) robot_propagation.v="<<robot_propagation.v <<"; robot_propagation.w="<<robot_propagation.w<<"; robot_propagation.theta="<<robot_propagation.theta*(180/3.14)<< std::endl;
+		//std::cout << "> robot_initial_pose_.theta="<<robot_initial_pose_.theta*(180/3.14)<< std::endl;
+	//}
+
+	if(debug_antes_subgoals_entre_AKP_goals_){
+		std::cout << "> (ant_neW_pose) robot_->get_robot_planning_trajectory()->at(index).x="<<robot_->get_robot_planning_trajectory()->at(index).x <<"; robot_->get_robot_planning_trajectory()->at(index).y="<<robot_->get_robot_planning_trajectory()->at(index).y<< std::endl;
+		std::cout << "> (neW) robot_propagation.x="<<robot_propagation.x <<"; robot_propagation.y="<<robot_propagation.y<< std::endl;
+	}
+
+	//check if the propagation is valid
+	bool collision_check = check_collision_final( robot_propagation, index ); // ver si hay colision en esa propagación. (en teoría no haria falta, porque ya las comprobaste antes!)
+
+	if(actual_debug2_){
+		std::cout << "> collision_check="<<collision_check << std::endl;
+	}
+
+	final_collision_check_=collision_check;
+
+	/*
+	Spoint robot_propagation_spoint(robot_propagation.x,robot_propagation.y,robot_propagation.time_stamp);
+
+	double distance_to_goal_act = robot_->get_current_pointV().distance( robot_propagation_spoint);
+
+	double vel_teoric= max_v_by_system_ * distance_to_goal_act / distance_to_stop_;
+	if(debug_nadal_){
+	std::cout << "(Replan last step function) distance_to_goal_act="<<distance_to_goal_act<<"; vel_teoric="<<vel_teoric<< std::endl;
+	}
+	double vel_teoric2=distance_to_goal_act/0.2;
+	if(debug_nadal_){
+	std::cout << "(Replan last step function) vel_teoric2=distance_to_goal_act/inct_="<<vel_teoric2<< std::endl;
+	}
+
+	//std::cout << "final_v_real_robot_max_="<<final_v_real_robot_max_<< std::endl;
+	if(debug_real_test_companion_){
+		std::cout << "(ANT) robot_propagation.theta="<<robot_propagation.theta*(180/3.14)<< std::endl;
+		std::cout << "(ANT) robot_propagation.w="<<robot_propagation.w<< std::endl;
+	}
+
+	double error_in_orientation_between_person_and_robot=(person_orient_-robot_propagation.theta)*(180/3.14);
+
+	if(debug_real_test_companion_){
+		std::cout << "error_in_orientation_between_person_and_robot_="<<error_in_orientation_between_person_and_robot<< std::endl;
+		std::cout << "w=(theta_persona-theta_robot)/0.2="<<error_in_orientation_between_person_and_robot/0.2<< std::endl;
+		std::cout << "(DESPUES) robot_propagation.theta=person_orient_="<<person_orient_*(180/3.14)<< std::endl;
+		std::cout << "(DESPUES) robot_propagation.theta=person_orient_="<<robot_propagation.theta*(180/3.14)<< std::endl;
+		std::cout << "(DESPUES) robot_propagation.w=person_orient_="<<robot_propagation.w<< std::endl;
+	}
+
+
+	double distance_act_r=initial_robot_spoint_.distance(robot_propagation);
+	last_step_robot_distance_cost_=distance_act_r;
+	double dx=robot_propagation.x-initial_robot_spoint_.x;
+	double dy=robot_propagation.y-initial_robot_spoint_.y;
+	double local_orientation=fabs(diffangle( robot_initial_pose_.theta ,
+			atan2( dy, dx )  ));
+	last_step_robot_orientation_cost_local_=local_orientation;
+	dx=extern_robot_goal_.x-initial_robot_spoint_.x;
+	dy=extern_robot_goal_.y-initial_robot_spoint_.y;
+	double global_orientation_=fabs(diffangle( robot_initial_pose_.theta ,
+			atan2( dy, dx )  ));
+	last_step_robot_orientation_cost_global_=global_orientation_;
+	last_step_robot_control_cost_mix_=f_goal_final.module2(cost_angular_);
+	last_step_robot_control_cost_goal_traj_=f_goal.module2(cost_angular_);
+	last_step_robot_control_cost_goal_person_=f_person_goal.module2(cost_angular_);
+*/
+
+	return robot_propagation;
+}
+
+
+Spoint Cplan_local_nav_person_companion::sample_workspace_for_person_companion_akp(  )
+{
+	Spoint sample;
+	double theta(0.0);
+	std::uniform_real_distribution<double> sample_x( person_companion_->get_current_pose().x - workspace_radii_,
+			person_companion_->get_current_pose().x + workspace_radii_ );
+	std::uniform_real_distribution<double> sample_y( person_companion_->get_current_pose().y - workspace_radii_,
+			person_companion_->get_current_pose().y + workspace_radii_ );
+	//std::cout << " [SAMPLE workspace PERSON COMPANION] goal_.x="<<goal_.x <<"; goal_.y="<<goal_.y<< std::endl;
+	double dx = goal_.x - person_companion_->get_current_pose().x;
+	double dy = goal_.y - person_companion_->get_current_pose().y;
+	std::normal_distribution<double> sample_g( atan2(dy,dx), std_goal_workspace_ );
+	switch(plan_mode_)
+	{
+	  case F_RRT_Uniform :
+		sample = Spoint( sample_x(generator_), sample_y(generator_)  );
+		break;
+	  case F_RRT_Gauss_Circle :
+	  case F_RRT_GC_alpha :
+	  default:
+		theta = sample_g(generator_);
+		sample = Spoint( person_companion_->get_current_pose().x + workspace_radii_*cos(theta),
+				person_companion_->get_current_pose().y + workspace_radii_*sin(theta));
+		break;
+	}
+
+	//robot parameters sampling if required
+	//set the random alpha variable, if necessary
+	if( plan_mode_ == F_RRT_GC_alpha )
+	{
+
+		std::uniform_int_distribution<int> sample_behavior( 0 , 2 );
+		double epsilon(0.4);
+		switch( sample_behavior(generator_) )
+		{
+		case 0://robot unaware  // OJO! eliminado robot unaware para person companion, pq me la lia!
+			alpha_ = 1.0-epsilon;gamma_ = 1.0+epsilon;delta_ = 1.0+epsilon;
+			//alpha_ = 1.0+epsilon;gamma_ = 1.0-epsilon;delta_ = 1.0-epsilon;
+			//std::cout << "case 0 :  alpha_="<<alpha_ <<"; gamma_="<<gamma_<<"; delta_"<<delta_<< std::endl;
+			break;
+		case 1://robot aware
+			alpha_ = 1.0-epsilon;gamma_ = 1.0+epsilon;delta_ = 1.0+epsilon;
+			//std::cout << "case 1 :  alpha_="<<alpha_ <<"; gamma_="<<gamma_<<"; delta_"<<delta_<< std::endl;
+			break;
+		case 2://robot balanced
+		default:
+			alpha_ = 1.0;gamma_ = 1.0;delta_ = 1.0;
+			//std::cout << "case 2(default) :  alpha_="<<alpha_ <<"; gamma_="<<gamma_<<"; delta_"<<delta_<< std::endl;
+			break;
+		}
+
+		//std::cout << alpha_ << " , " << gamma_ << " , " << delta_ << std::endl;
+	}
+
+	return sample;
+}
+
+
+void Cplan_local_nav_person_companion::reset_scene_persons_propagation_flag()
+{
+	for( Cperson_abstract* iit : nearby_person_list_  )
+	{
+		iit->reset_propagation_flag();
+	}
+}
+
+
+unsigned int Cplan_local_nav_person_companion::find_nearest_vertex_person_companion_akp( const Spoint& random_goal )
+{
+	//std::cout << "find_nearest_vertex (1)" << std::endl;
+	double min_dist(1e10), result;
+	unsigned int nearest_vertex_index = 0;
+	for( unsigned int i = 0 ; i < person_companion_->get_robot_planning_trajectory()->size() ; i+=2 )
+	{
+		//std::cout << "robot_->get_robot_planning_trajectory()->size()="<<person_companion_->get_robot_planning_trajectory()->size()<<"i="<<i << std::endl;
+		if ( !collision_detected_[i] )
+		{//std::cout << "find_nearest_vertex (1.1)" << std::endl;
+			result = cost_to_go_person_companion_akp(random_goal,i);
+			//std::cout << "find_nearest_vertex (1.2)" << std::endl;
+			if ( result  < min_dist  )
+			{
+				min_dist = result;
+				nearest_vertex_index = i;
+			}
+		}
+
+	}
+	/*std::cout << "find_nearest_vertex (2) robot_->get_robot_planning_trajectory()->at( nearest_vertex_index ).time_stamp="<<person_companion_->get_robot_planning_trajectory()->at( nearest_vertex_index ).time_stamp << std::endl;
+	std::cout << "find_nearest_vertex (2) horizon_time_="<<horizon_time_ << std::endl;
+	std::cout << "find_nearest_vertex (2) now_="<<now_ << std::endl;
+*/
+	//std::cout << "find_nearest_vertex (2)" << std::endl;
+	if ( person_companion_->get_robot_planning_trajectory()->at( nearest_vertex_index ).time_stamp > horizon_time_ + now_ )
+		return 0;
+	//std::cout << "find_nearest_vertex (3)" << std::endl;
+	return nearest_vertex_index;
+}
+
+
+
+Sedge_tree_pcomp Cplan_local_nav_person_companion::calculate_edge_person_companion_akp( unsigned int parent_vertex, const Sdestination& random_goal, Cperson_abstract::companion_reactive reactive )
+{ // bool reactive  is to approximate to the person when robot is far. (Modificated by ely. For person companion.)
+
+	//std::cout << " calculate_edge_person_companion_akp= "<< std::endl;
+	Sforce f_goal, f_int, f_obs, f,f_companion_robot_goal, f_goal_final, f_companion_Zanlungo;
+	double signo_x;
+	double signo_y;
+	double fobos_Xmod;
+	double fobos_Ymod;
+
+	//Sforce f_goal_companion=Sforce();
+	//caso planning akp, para person_companion como robot
+	if(debug_calculate_edge_person_companion_akp_){
+		std::cout << " IN :calculate_edge case(Akp_planning); reactive= "<<reactive <<"; parent_vertex="<<parent_vertex<<"; random_goal.x="<<random_goal.x<<"; random_goal.y="<<random_goal.y<<  std::endl;
+	}
+
+	//std::cout << " second_group_companion_person_obj_->get_planning_trajectory()->empty()="<<second_group_companion_person_obj_->get_planning_trajectory()->empty()<<"; size="<<second_group_companion_person_obj_->get_planning_trajectory()->size()<<std::endl;
+	// force of companion for the other person of the group.
+	//Cperson_abstract* person_obj_companion_person2;
+
+	//std::cout << ";id_SECOND_person_companion_"<<id_SECOND_person_companion_<< " find_person="<<(find_person(id_SECOND_person_companion_ , &person_obj_companion_person2))<<"; second_group_companion_person_obj_->get_prediction_trajectory()->size()="<<second_group_companion_person_obj_->get_prediction_trajectory()->size()<<"; get_plan_traj_robot="<<robot_->get_prediction_trajectory_with_target_person()->size() <<  std::endl;
+
+	//if((find_person(id_SECOND_person_companion_ , &person_obj_companion_person2))&&(!robot_->get_prediction_trajectory()->empty())){
+	/*if((!plan_create_fake_fixed_second_person_companion_)&&(find_person(id_SECOND_person_companion_ , &person_obj_companion_person2))&&(!second_group_companion_person_obj_->get_prediction_trajectory()->empty())){
+
+		//std::cout <<" (IN FORCES) id_other pers comp="<< second_group_companion_person_obj_->get_id()<<" IN primer if;  second_group_companion_person_obj_->get_prediction_trajectory()->empty()="<<second_group_companion_person_obj_->get_prediction_trajectory()->empty()<<"; plan_create_fake_fixed_second_person_companion_="<<plan_create_fake_fixed_second_person_companion_<< std::endl;
+		//std::cout << " IN primer if;  second_group_companion_person_obj_->get_prediction_trajectory()->size()="<<second_group_companion_person_obj_->get_prediction_trajectory()->size()<<"; parent_vertex="<<parent_vertex<< std::endl;
+
+		if((!second_group_companion_person_obj_->get_prediction_trajectory()->empty())&&(second_group_companion_person_obj_->get_prediction_trajectory()->size()>parent_vertex)){
+
+			//std::cout << " IN segundo if "<< std::endl;
+			// TODO: translate the trajectory of the person to the position for the companion person. To one of the sides of the person beeing accompanied.
+			///////////////
+			SpointV_cov robot_act=person_companion_->get_current_pointV();
+			//std::cout << " robot_act_point.print:" <<  std::endl;
+			//robot_act.print();
+
+			SpointV_cov person=second_group_companion_person_obj_->get_prediction_trajectory()->at(parent_vertex);//second_group_companion_personSpoint_;
+			//std::cout << " person.print:" <<  std::endl;
+			//person.print();
+
+			SpointV_cov robot_person_act=person;
+			double angle=atan2(robot_act.y-person.y , robot_act.x-person.x);
+			if(angle<0){
+				angle=(2*3.14)+angle;
+			}
+			double theta=second_group_companion_personSpose_.theta;//calc_person_companion_orientation(); // TODO: mirar
+
+			if(theta<0){
+				theta=(2*3.14)+theta;
+			}
+
+			if(debug_real_test_companion4_){
+				std::cout << "(**** FINAL *****) theta="<<theta*(180/3.14)<<"; angle="<<angle*(180/3.14)<<"diffangle(theta, angle)="<<(diffangle(theta, angle))*(180/3.14)<< std::endl;
+				std::cout << "theta+angle="<<(theta+angle)*(180/3.14)<<"; theta-angle="<<(theta-angle)*(180/3.14)<< std::endl;
+				std::cout << "min_next_companion_angle_=" <<min_next_companion_angle_<<"; theta+min_next_companion_angle_="<<(theta+(min_next_companion_angle_*3.14/180))*(180/3.14)<<"; theta-min_next_companion_angle_="<<(theta-(min_next_companion_angle_*3.14/180))*(180/3.14)<< std::endl;
+
+			}
+
+
+			//double real_distance_between_people=second_group_companion_personSpoint_.distance(robot_act);
+			if( diffangle(theta, angle) < 0 ){  // angle = 180 grados... o 0... si cambia el goal de lado da un salto... (como arreglarlo)
+
+				if(debug_real_test_companion4_){
+					std::cout << " diffangle(theta, angle) theta="<<theta<<"; angle="<<angle<< std::endl;
+				}
+
+				robot_person_act.x=robot_person_act.x+((real_distance_between_people_of_group_))*cos(theta+(companion_angle_peopl_in_group_*3.14/180));//1.5*cos(theta+(min_next_companion_angle_*3.14/180));
+				robot_person_act.y=robot_person_act.y+((real_distance_between_people_of_group_))*sin(theta+(companion_angle_peopl_in_group_*3.14/180));//1.5*sin(theta+(min_next_companion_angle_*3.14/180));
+
+			}else{
+
+				robot_person_act.x=robot_person_act.x+(real_distance_between_people_of_group_)*cos(theta-(90*3.14/180));//1.5*cos(theta-(min_next_companion_angle_*3.14/180));
+				robot_person_act.y=robot_person_act.y+(real_distance_between_people_of_group_)*sin(theta-(90*3.14/180));//1.5*sin(theta-(min_next_companion_angle_*3.14/180));
+
+			}
+
+
+			Sdestination companion_goal(1,robot_person_act.x,robot_person_act.y,0.5);
+
+			////////////
+
+			//Sdestination companion_goal=Sdestination(1,second_group_companion_person_obj_->get_planning_trajectory()->at(parent_vertex).x,second_group_companion_person_obj_->get_planning_trajectory()->at(parent_vertex).y,0.5);
+
+			// TODO: incluir aqui una fuerza hacia la prediccion de la otra persona del grupo que acompaña robot.
+			//double threshold_apply_companion_force_with_other_person_of_group_=1.5;
+			//if(robot_person_act.distance(person_companion_->get_current_pointV())<threshold_apply_companion_force_with_other_person_of_group_){
+			//	f_goal_companion = person_companion_->force_goal_near(companion_goal , get_sfm_params(person_companion_),&(person_companion_->get_planning_trajectory()->at(parent_vertex)), 5.0  );
+			//}else{
+			//	f_goal_companion =Sforce();
+			//}
+
+			// TODO:faltaria prediccion de la person companion.
+				//////////////////////////
+			//std::cout << " Zanlungo_model_= "<<Zanlungo_model_<< std::endl;
+				if(Zanlungo_model_){ //GroupForce(int nothers,State2D *Others,Vector2D preferred)
+					//std::cout << " breack_medium 1 ! "<< std::endl;
+					//std::cout << " enter zanlungo!="<<std::endl;
+
+					Sdestination actual_group_goal=person_companion_->get_best_dest();//random_goal;;//robot_->get_best_dest();//random_goal;
+					//std::cout << " Zanlungo actual_group_goal.x="<<actual_group_goal.x<<"; actual_group_goal.y="<<actual_group_goal.y<<std::endl;
+
+					//Sdestination actual_group_goal=random_goal; //(id, x, y , pd); Sdestination(1,0.0,0.0, 0.5);//
+
+					double actual_personComp_vel=sqrt((pointer_to_person_companion_->get_current_pointV().vx*pointer_to_person_companion_->get_current_pointV().vx)+(pointer_to_person_companion_->get_current_pointV().vy*pointer_to_person_companion_->get_current_pointV().vy));
+					if(actual_personComp_vel<0.15){ // TODO: provisional, pensar mejor.
+						actual_group_goal=before_person_comp_goal_Zalungo_formation_;
+					}else{
+						actual_group_goal=robot_->get_best_dest();
+						before_person_comp_goal_Zalungo_formation_=robot_->get_best_dest();
+					}
+					//std::cout << " breack_medium 2 ! "<< std::endl;
+
+					//std::cout << " Zanlungo 1; robot_initial_pose_.x="<<robot_initial_pose_.x<<"; robot_initial_pose_.y="<<robot_initial_pose_.y<<"; initial_person_companion_point_.x="<<initial_person_companion_point_.x<<"; initial_person_companion_point_.y="<<initial_person_companion_point_.y<<std::endl;
+					//std::cout << " best_plan.size="<<best_planning_trajectory_.size()<<std::endl;
+					SpointV_cov pose_of_the_robot=person_companion_->get_planning_trajectory( )->at(parent_vertex);
+					//std::cout << " breack_medium 3 ! "<< std::endl;
+					//std::cout << " breack_medium 2 ! "<< std::endl;
+					double robot_actual_time_stamp=person_companion_->get_planning_trajectory( )->at(parent_vertex).time_stamp-person_companion_->get_current_pointV().time_stamp;
+					//std::cout << " breack_medium 4 ! "<< std::endl;
+					//std::cout << " robot_actual_time_stamp="<<robot_actual_time_stamp<<std::endl;
+					double index_person_pred=(robot_actual_time_stamp/dt_)+1;
+					//std::cout << " breack_medium 5 ! "<< std::endl;
+					//std::cout << " index_person_pred="<<index_person_pred<<std::endl;
+					//std::cout << " person_dt="<<robot_->get_time_stamp_plan().at(parent_vertex)<<std::endl;
+					//std::cout << " breack_medium 3 ! "<< std::endl;
+					//std::cout << " Zanlungo 2; robot_->get_planning_trajectory( ).size()="<<robot_->get_planning_trajectory( )->size()<<std::endl;
+					Vector2D robot_pose(pose_of_the_robot.vx,pose_of_the_robot.vy,atan(pose_of_the_robot.vy/pose_of_the_robot.vx)); // theta en rads, chec if is ok.
+					//std::cout << " Zanlungo 3"<<std::endl;
+					//std::cout << " breack_medium 4 ! "<< std::endl;
+					//std::cout << " breack_medium 6 ! "<< std::endl;
+					Companion_Zanlungo_Model_.set_Self(pose_of_the_robot.x,pose_of_the_robot.y,pose_of_the_robot.vx,pose_of_the_robot.vy,robot_pose.th);
+					//std::cout << " breack_medium 7 ! "<< std::endl;
+					//std::cout << " breack_medium 5 ! "<< std::endl;
+					State2D *Others=new State2D[number_of_group_people_];
+					//std::cout << " Zanlungo 4; pointer_to_person_companion_->get_prediction_trajectory().size()="<<pointer_to_person_companion_->get_prediction_trajectory()->size()<<"; parent_vertex="<<parent_vertex<<std::endl;
+					//std::cout << " breack_medium 8 ! "<< std::endl;
+
+					//std::cout << " breack_medium 6 ! robot_="<< std::endl;
+					robot_->get_current_pointV();
+					//std::cout << " breack_medium 6 ! robot_.pred_traj_size="<<robot_->get_prediction_trajectory_with_target_person()->size()<<"; robot_.plan_traj="<<robot_->get_planning_trajectory()->size()<< std::endl;
+				//	std::cout << " breack_medium 9 ! "<< std::endl;
+					SpointV_cov actual_first_person_companion_point;
+					if(robot_->get_prediction_trajectory_with_target_person()->size()>index_person_pred){
+						actual_first_person_companion_point=robot_->get_prediction_trajectory_with_target_person()->at(index_person_pred);
+					}else{
+						actual_first_person_companion_point=robot_->get_prediction_trajectory_with_target_person()->back();
+					}
+					//std::cout << " robot_->get_planning_trajectory( ).size()="<<robot_->get_planning_trajectory( )->size()<<std::endl;
+					//std::cout << " breack_medium 7 ! "<< std::endl;
+					//std::cout << " pointer_to_person_companion_->get_prediction_trajectory().size()="<<pointer_to_person_companion_->get_prediction_trajectory()->size()<<std::endl;
+					//std::cout << " breack_medium 10 ! "<< std::endl;
+					State2D first_companion_person(actual_first_person_companion_point.x,actual_first_person_companion_point.y,actual_first_person_companion_point.vx,actual_first_person_companion_point.vx);
+					//std::cout << " breack_medium 11 ! "<< std::endl;
+					//std::cout << " Zanlungo 5"<<std::endl;
+					Vector2D first_companion_person2D(actual_first_person_companion_point.vx,actual_first_person_companion_point.vy,atan(actual_first_person_companion_point.vy/actual_first_person_companion_point.vx));
+
+					//std::cout << " breack_medium 12 ! "<< std::endl;
+					//std::cout << " Zanlungo 6"<<std::endl;
+					Others[0]=first_companion_person;
+
+					//std::cout << " breack_medium 8 ! "<< std::endl;
+					//std::cout << " breack_medium 13 ! "<< std::endl;
+
+
+					SpointV_cov actual_second_person_companion_point=SpointV_cov();//=second_group_companion_person_obj_->get_prediction_trajectory()->at(parent_vertex);
+					Cperson_abstract* person_obj_companion_person2;
+					//std::cout << " breack_medium 14 ! "<< std::endl;
+					if((find_person(id_SECOND_person_companion_ , &person_obj_companion_person2))&&(number_of_group_people_>1)){
+
+						if(second_group_companion_person_obj_->get_prediction_trajectory()->size()>index_person_pred){ // TODO: Importante, mirar porque tienen tamanos diferentes a veces.
+							actual_second_person_companion_point=second_group_companion_person_obj_->get_prediction_trajectory()->at(index_person_pred);
+						}else{
+							actual_second_person_companion_point=second_group_companion_person_obj_->get_prediction_trajectory()->back();
+						}
+						//std::cout << " Zanlungo 7"<<std::endl;
+						State2D second_companion_person(actual_second_person_companion_point.x,actual_second_person_companion_point.y,actual_second_person_companion_point.vx,actual_second_person_companion_point.vy);
+						//std::cout << " Zanlungo 8"<<std::endl;
+						Vector2D second_companion_person2D(actual_second_person_companion_point.vx,actual_second_person_companion_point.vy,atan(actual_second_person_companion_point.vy/actual_second_person_companion_point.vx));
+						//std::cout << " Zanlungo 9"<<std::endl;
+						Others[1]=second_companion_person;
+
+					}
+				//	std::cout << " breack_medium 15 ! "<< std::endl;
+					//std::cout << " breack_medium 9 ! "<< std::endl;
+					//std::vector<State2D> Others_in_group; // states of other people in the group.
+					//Others=&Others_in_group;
+
+					double theta=calc_person_companion_orientation();
+
+					if(theta<0){
+						theta=3.14+theta;
+					}
+					//std::cout << " breack_medium 16 ! "<< std::endl;
+					companion_person_theta_path_.push_back(theta);
+
+					double central_group_x;
+					double central_group_y;
+
+					if(number_of_group_people_<2){
+
+						central_group_x=(pose_of_the_robot.x + actual_first_person_companion_point.x)/(number_of_group_people_+1);//+actual_second_person_companion_point.x)/(number_of_group_people_+1);
+						central_group_y=(pose_of_the_robot.y + actual_first_person_companion_point.y)/(number_of_group_people_+1);//+actual_second_person_companion_point.x)/(number_of_group_people_+1);
+					}else{
+						if(we_have_pointer_to_second_person_){
+
+							central_group_x=(pose_of_the_robot.x + actual_first_person_companion_point.x+actual_second_person_companion_point.x)/(number_of_group_people_+1);//+actual_second_person_companion_point.x)/(number_of_group_people_+1);
+							central_group_y=(pose_of_the_robot.y + actual_first_person_companion_point.y+actual_second_person_companion_point.y)/(number_of_group_people_+1);//+actual_second_person_companion_point.x)/(number_of_group_people_+1);
+						}
+
+					}
+
+					//double x_orient_goal=actual_group_goal.x-central_group_x;//actual_first_person_companion_point.vx;//(actual_group_goal.x-central_group_x);
+					//double y_orient_goal=actual_group_goal.y-central_group_y;//actual_first_person_companion_point.vy;//(actual_group_goal.y-central_group_y);
+					//double orient_goal=atan(y_orient_goal/x_orient_goal);//atan(y_orient_goal/x_orient_goal);
+					double x_orient_goal;
+					//actual_first_person_companion_point.x;//actual_first_person_companion_point.vx;//(actual_group_goal.x-central_group_x);
+					//std::cout << " x_orient_goal="<<x_orient_goal<<std::endl;
+					double y_orient_goal;
+					double orient_goal;
+
+					//std::cout << " (solo 1 persona) x_orient_goal="<<actual_group_goal.x-pose_of_the_robot.x<<"; y_orient_goal="<<actual_group_goal.y-pose_of_the_robot.y<<"; orient_goal="<<atan((actual_group_goal.y-pose_of_the_robot.y)/(actual_group_goal.x-pose_of_the_robot.x))<<std::endl;
+					//std::cout << " (2 personas) x_orient_goal="<<actual_group_goal.x-central_group_x<<"; y_orient_goal="<<actual_group_goal.y-central_group_y<<"; orient_goal="<<atan((actual_group_goal.y-central_group_y)/(actual_group_goal.x-central_group_x))<<std::endl;
+					//std::cout << " breack_medium 17 ! "<< std::endl;
+
+					if(number_of_group_people_<2){
+						//std::cout << " breack_medium 18 ! if(number_of_group_people_<2) "<< std::endl;
+						x_orient_goal=actual_group_goal.x-pose_of_the_robot.x;
+						//actual_first_person_companion_point.x;//actual_first_person_companion_point.vx;//(actual_group_goal.x-central_group_x);
+						//std::cout << " x_orient_goal="<<x_orient_goal<<std::endl;
+						y_orient_goal=actual_group_goal.y-pose_of_the_robot.y;
+						//actual_first_person_companion_point.y;//actual_first_person_companion_point.vy;//(actual_group_goal.y-central_group_y);
+						//std::cout << " y_orient_goal="<<y_orient_goal<<std::endl;
+						orient_goal=atan(y_orient_goal/x_orient_goal);//atan(y_orient_goal/x_orient_goal);
+						//std::cout << "(TEST, now have to be the same) orient_goal="<<orient_goal<<"theta="<<theta<<std::endl;
+
+					}else{
+						//std::cout << " breack_medium 18 ! ELSE if(number_of_group_people_<2) "<< std::endl;
+						if(we_have_pointer_to_second_person_){
+						//	std::cout << " breack_medium 18 ! ELSE if(we_have_pointer_to_second_person_){ "<< std::endl;
+							x_orient_goal=actual_group_goal.x-central_group_x;//actual_first_person_companion_point.vx;//(actual_group_goal.x-central_group_x);
+							y_orient_goal=actual_group_goal.y-central_group_y;//actual_first_person_companion_point.vy;//(actual_group_goal.y-central_group_y);
+							orient_goal=atan(y_orient_goal/x_orient_goal);
+						}
+					}
+
+					double x_orient_goal2=actual_group_goal.x-central_group_x;//actual_first_person_companion_point.vx;//(actual_group_goal.x-central_group_x);
+					double y_orient_goal2=actual_group_goal.y-central_group_y;//actual_first_person_companion_point.vy;//(actual_group_goal.y-central_group_y);
+					double orient_goal2=atan(y_orient_goal2/x_orient_goal2);
+
+					//std::cout << " x_orient_goal="<<x_orient_goal<<"; y_orient_goal="<<y_orient_goal<<"; orient_goal="<<orient_goal<<std::endl;
+					//std::cout << " breack_medium 18 ! "<< std::endl;
+					//orient_goal=theta;
+					//x_orient_goal=ori_pers_x_;
+					//y_orient_goal=ori_pers_y_;
+
+					Vector2D preferred(x_orient_goal,y_orient_goal,orient_goal); // is the orientation until the goal of the group.
+					if(debug_zanlungo_){
+						std::cout << " Zanlungo model dist.R-P="<<pose_of_the_robot.distance(actual_first_person_companion_point)<<"; preferred.x="<<preferred.x<<"; preferred.y="<<preferred.y<<"; preferred.th="<<preferred.th<<std::endl;
+					}
+					preferred_paths_Zanlungo_.push_back(preferred);
+					// calculate the central point in the group.
+					if(debug_zanlungo_){
+						std::cout << " Zanlungo; preferred.x="<<preferred.x<<"; preferred.x.y="<<preferred.y<<"; preferred.th="<<preferred.th<<std::endl;
+					}
+
+					//for(unsigned int gz=0;gz<number_of_group_people_;gz++){
+					//}
+
+					//std::cout << " breack_medium 19 ! "<< std::endl;
+					//for(unsigned int pr=0;pr<number_of_group_people_;pr++){
+						//preferred.Add(first_companion_person2D);
+
+					//	std::cout << " Zanlungo 10"<<std::endl;
+
+						//if(number_of_group_people_>1){
+							//preferred.Add(second_companion_person2D);
+						//}
+						//std::cout << " Zanlungo 11"<<std::endl;
+
+						//preferred.x=x_orient_goal;//preferred.x/number_of_group_people_;
+						//preferred.y=y_orient_goal;//preferred.y/number_of_group_people_;
+						//preferred.th=orient_goal;//preferred.th/number_of_group_people_;
+						///std::cout << " preferred.x="<<preferred.x<<"; preferred.y="<<preferred.y<<"; preferred.th="<<preferred.th<<"; person.vx"<<initial_person_companion_point_.vx<<"; person.vy="<<initial_person_companion_point_.vy<<"; person.th="<<atan((initial_person_companion_point_.vy)/(initial_person_companion_point_.vx))<<std::endl;//"; f_int.fx"<<f_int.fx<<"; f_int.fy="<<f_int.fy<<std::endl;
+
+
+
+					//}
+					//Companion_Zanlungo_Model_.adapt_Cr_and_Ct_whit_vel_CZM(person_companion_->get_desired_velocity()); //up_distance_margin_Zamlungo_(1.5), down_distance_margin_Zamlungo_(0.75)
+
+					//std::cout << " 7777!!! IMPORTANTE! person_companion_->get_desired_velocity()="<<person_companion_->get_desired_velocity()<<"; get_in_ct="<<Companion_Zanlungo_Model_.get_genome_ComModPar_Ct()<<" get_in_Cr="<<Companion_Zanlungo_Model_.get_genome_ComModPar_Cr()<<std::endl;
+
+
+					//std::cout << " down_distance_margin_Zamlungo_="<<down_distance_margin_Zamlungo_<<"; up_distance_margin_Zamlungo_="<<up_distance_margin_Zamlungo_<<std::endl;
+
+					dist_per_rob_path_.push_back(pose_of_the_robot.distance(actual_first_person_companion_point));
+					//std::cout << " breack_medium 20 ! "<< std::endl;
+					if(((pose_of_the_robot.distance(actual_first_person_companion_point)>(robot_person_proximity_distance_-down_distance_margin_Zamlungo_))&&(pose_of_the_robot.distance(actual_first_person_companion_point)<(robot_person_proximity_distance_+up_distance_margin_Zamlungo_)))&&(bool_distance_margin_)){
+						//std::cout << " Margin case!!! dist="<<pose_of_the_robot.distance(actual_first_person_companion_point)<<"; parent_vertex="<<parent_vertex<<std::endl;
+						Companion_Zanlungo_Model_.set_r0_CZM(pose_of_the_robot.distance(actual_first_person_companion_point));
+						genome_params_.set_r0(pose_of_the_robot.distance(actual_first_person_companion_point));
+						in_margin_true_.push_back(true);
+						distance_betw_per_robo_plann_.push_back(pose_of_the_robot.distance(actual_first_person_companion_point));
+
+					}else{
+						//std::cout << " NO Margin case!!! dist="<<pose_of_the_robot.distance(actual_first_person_companion_point)<<"; parent_vertex="<<parent_vertex<<std::endl;
+						Companion_Zanlungo_Model_.set_r0_CZM(robot_person_proximity_distance_);
+						genome_params_.set_r0(robot_person_proximity_distance_);
+						in_margin_true_.push_back(false);
+						distance_betw_per_robo_plann_.push_back(pose_of_the_robot.distance(actual_first_person_companion_point));
+					}
+					//std::cout << " breack_medium 21 ! "<< std::endl;
+					person_companion_->robot_set_genome(genome_params_);
+					//std::cout << " robot_->get_desired_velocity()="<<robot_->get_desired_velocity()<<"; genome_params_.cr="<<genome_params_.Cr_<<"; genome_params_.ct="<<genome_params_.Ct_<<std::endl;
+
+					//std::cout << " breack_medium 21.1 ! "<< std::endl;
+					//group_force_start = clock();
+					double actual_dis_tol=0.0;
+					if((we_have_pointer_to_second_person_)&&(number_of_group_people_>1)){
+						//std::cout << " breack_medium 21.2 ! "<< std::endl;
+						if(order_people_in_group_global_variable_[0]==1){
+						//	std::cout << " breack_medium 21.3 ! "<< std::endl;
+							actual_dis_tol=dis_tol_;
+						}else{
+							//std::cout << " breack_medium 21.4 ! "<< std::endl;
+							actual_dis_tol=dis_tol_side_;
+						}
+					}
+					//std::cout << " breack_medium 22 ! "<< std::endl;
+					//std::cout << "[ms] actual_dis_tol="<<actual_dis_tol<< std::endl;
+					Vector2D f_companion=Companion_Zanlungo_Model_.GroupForce(number_of_group_people_,Others,preferred,actual_dis_tol,parent_vertex);//Companion_Zanlungo_Model_.F(robot_pose);
+					//group_force_end = clock();
+					//std::cout << "[ms] (FIN) time_tot=group_force_start-group_force_end="<<((group_force_end-group_force_start)/clocks_per_sec_my_var_)*1000<< std::endl;
+					//std::cout << " breack_medium 23 ! "<< std::endl;
+					//std::cout << " Zanlungo 13 after group force"<<std::endl;
+					//std::cout << " breack_medium 23 !; f_companion.x= "<<f_companion.x<<"; f_companion.y="<<f_companion.y<< std::endl;
+					f_companion_Zanlungo.fx = f_companion.x;
+					f_companion_Zanlungo.fy = f_companion.y;
+					//std::cout << " breack_medium 23.1 ! "<< std::endl;
+				}
+
+				//std::cout << " breack_medium 24 ! "<< std::endl;
+				//if(Zanlungo_model2_){ // model only with atractive force until goal
+				//	f_companion_Zanlungo.fx = 0.0;
+				//	f_companion_Zanlungo.fy = 0.0;
+				//}
+
+
+
+
+				//std::cout << " breack_medium 25 ! "<< std::endl;
+
+				/////////////////////////////////
+
+		}
+
+		//////////////////////////////
+	}else{
+		//std::cout << " no person NO comp force "<< std::endl;
+		f_companion_Zanlungo =Sforce();
+	}*/
+
+
+	//std::cout << " IMPORTANTE, URGENTE!!!! Zanlungo_model2_= "<<Zanlungo_model2_<< std::endl;
+	// TODO: FALTA INCLUIR LO DE PERSONA A 90 grados de la person companion!!!!
+
+
+	// antes para companion estaba como f_goal_near, pero ahora el robot con la V-form esta como f_goal, así que lo cambie a ver si tambien va bien.
+	f_goal = person_companion_->force_goal( random_goal, get_sfm_params(person_companion_),&(person_companion_->get_planning_trajectory()->at(parent_vertex))  );
+	//f_goal = person_companion_->force_goal_near( random_goal, get_sfm_params(person_companion_),&(person_companion_->get_planning_trajectory()->at(parent_vertex))  );
+
+	f_int = force_persons_int_planning_virtual_companion_person_akp( person_companion_, parent_vertex );
+
+	SpointV robot = person_companion_->get_planning_trajectory()->at(parent_vertex);
+
+	// TEST: person_companion include force with the tibi=robot
+	SpointV robot_tibi_point=SpointV(robot_->get_current_pointV().x,robot_->get_current_pointV().y,robot_->get_current_pointV().time_stamp,robot_->get_current_pointV().vx,robot_->get_current_pointV().vy);
+	Sforce f_person_comp_to_tibi=person_companion_->force(robot_tibi_point,get_sfm_int_params(person_companion_,robot_),&robot);
+
+	//f_person_comp_to_tibi.fx=f_person_comp_to_tibi.fx/2;
+	//f_person_comp_to_tibi.fy=f_person_comp_to_tibi.fy/2;
+	f_int +=f_person_comp_to_tibi;
+	force_int_between_person_comp_and_robot_=f_person_comp_to_tibi;
+
+	if(((f_int.fx>0.5)||(f_int.fy>0.5))&&see_forces_){
+		std::cout << " (Akp_planning) f_int.fx= "<<f_int.fx<<" f_int.fy"<<f_int.fy <<"; parent_vertex="<< parent_vertex<<  std::endl;
+	}
+
+	// f_int+= robot_->force( person_companion_->get_planning_trajectory( )->at(t) ,  get_sfm_int_params(robot_,person_companion_),&(robot_->get_planning_trajectory()->at(t) ) );   //++ si añado el efecto del robot en la person companion
+
+
+	//map force, used for simulations
+	if( read_force_map_success_ )
+		f_obs = get_force_map(person_companion_->get_robot_planning_trajectory()->at(parent_vertex).x,
+					person_companion_->get_robot_planning_trajectory()->at(parent_vertex).y);
+
+	//obstacles due to laser scans. for real environments has priority over map forces
+	if(read_laser_obstacle_success_)
+		f_obs = force_objects_laser_int_planning_virtual( person_companion_, parent_vertex, 25.0, true );
+
+	//f_obs= Sforce();
+	if(debug_calculate_edge_person_companion_akp_){
+		std::cout << " IN :calculate_edge (antes limitar f_obs); f_obs.fx="<<f_obs.fx<<"; f_obs.fy="<<f_obs.fy<<"; delta_="<<delta_<<std::endl;
+		std::cout << " laser_obstacle_list_.size()="<<laser_obstacle_list_.size()<<"; number_of_obstacles_="<<number_of_obstacles_<<std::endl;
+	}
+
+	// INICION Limitar fuerza obstaculos cuando hay muchos solapados
+	fobos_Xmod=sqrt(f_obs.fx*f_obs.fx);
+	fobos_Ymod=sqrt(f_obs.fy*f_obs.fy);
+
+	if (f_obs.fx > 0){
+		signo_x=1;
+	}else{
+		signo_x=-1;
+	}
+	if (f_obs.fy > 0){
+		signo_y=1;
+	}else{
+		signo_y=-1;
+	}
+	//std::cout << " (FINAL) f_obs.fx="<<f_obs.fx<<"; f_obs.fy"<<f_obs.fy<<std::endl;
+
+	if((fobos_Xmod>f_obst_max_x_)||(fobos_Ymod>f_obst_max_y_)){
+		f_obs.fx=signo_x*f_obst_max_x_;
+		f_obs.fy=signo_y*f_obst_max_y_;
+	}
+	//std::cout << "(FINAL FORCE) f_obs.fx="<<f_obs.fx<<"; f_obs.fy="<<f_obs.fy<< std::endl;
+
+	// FIN Limitar fuerza obstaculos cuando hay muchos solapados
+
+	//f=f_goal*alpha_ + f_int*gamma_ + f_obs*delta_;
+	//gamma_=gamma_;
+
+	/* INI GENERATE person_companion, force companion (To companion with more than one person).*/
+	//std::cout << "ROBOT CURRENT POSE="<< std::endl;
+	///robot_->get_current_pose().print();
+	Sdestination accompanying_robot_goal=Sdestination(0,robot_->get_current_pose().x,robot_->get_current_pose().y, 0.5); // Sdestination paramenters = id, x, y, probability
+	// force to do companion with the robot.
+	//double threshold_apply_companion_force_with_robot_=1.5;
+	//if(robot_->get_current_pose().distance(person_companion_->get_current_pointV())<threshold_apply_companion_force_with_robot_){
+		f_companion_robot_goal=person_companion_->force_goal( accompanying_robot_goal, get_sfm_params(person_companion_),&(person_companion_->get_planning_trajectory()->at(parent_vertex))  );
+
+	//}else{
+	//	f_companion_robot_goal=Sforce();
+	//}
+
+	//double beta_robot_companion=0.3;
+	alpha_=0.7;
+	//double beta=0.3;
+
+	/* FIN GENERATE person_companion, force companion (To companion with more than one person).*/
+
+	//f=f_goal*alpha_ + f_int*gamma_ + f_obs*delta_+f_companion_robot_goal*beta_robot_companion;
+	// if Zanlugo
+	//f=f_goal*alpha_ + f_int*gamma_ + f_obs*delta_+f_goal_companion*beta;//+f_companion_robot_goal*beta_robot_companion;
+	// if normal
+	//f=f_goal*alpha_ + f_int*gamma_ + f_obs*delta_+f_goal_companion*beta+f_companion_robot_goal*beta_robot_companion;
+
+	//std::cout << " IMPORTANTE!!!! Zanlungo_model_= "<<Zanlungo_model_<<"; !robot_->get_prediction_trajectory()->empty()="<<!robot_->get_prediction_trajectory()->empty()<<"; bool_find_person_comp2= "<<find_person(id_SECOND_person_companion_ , &person_obj_companion_person2)<< std::endl;
+
+	//if((Zanlungo_model_)&&(!robot_->get_prediction_trajectory()->empty())&&(find_person(id_SECOND_person_companion_ , &person_obj_companion_person2))){
+	/*if((!plan_create_fake_fixed_second_person_companion_)&&(Zanlungo_model_)&&(!robot_->get_prediction_trajectory_with_target_person()->empty())&&(find_person(id_SECOND_person_companion_ , &person_obj_companion_person2))){
+		//std::cout << " breack_last 1 ! "<< std::endl;
+		//std::cout << " Zanlungo 13 after final force"<<std::endl;
+		//f=f_goal_final*alpha_ + f_int*gamma_ + f_obs*delta_;
+		//f=f_goal*alpha_companion_ + f_int*gamma_companion_ + f_obs*delta_companion_;
+		float fx_Z=(float)f_companion_Zanlungo.fx;
+		if(isnan(fx_Z)){
+			f_companion_Zanlungo.fx=0;
+		}
+		float fy_Z=(float)f_companion_Zanlungo.fy;
+		if(isnan(fy_Z)){
+			f_companion_Zanlungo.fy=0;
+		}
+
+		//test forces, alpha and betha hard coded, afther change it outside.
+		//beta_companion_=1;
+		//alpha_companion_=1;
+
+		if((person_companion_->get_current_pointV().distance(robot_->get_current_pointV())<3)&&(person_companion_->get_current_pointV().distance(second_group_companion_person_obj_->get_current_pointV())<3)){
+			f=f_goal*alpha_companion_+f_companion_Zanlungo*constante_multiplicar_fuerza_base_Zanlungo_*beta_companion_ + f_int*gamma_ + f_obs*delta_; // prueba, con parametros articulo revista robot companion
+			std::cout << " enter zanlungo (less 3 meters)!="<<std::endl;
+		}else{
+			if(Zanlungo_model2_){
+				f=f_goal*alpha_+ f_int*gamma_ + f_obs*delta_; // prueba, con parametros articulo revista robot companion
+				std::cout << " enter zanlungo ( more 3 meters)! case 1 ="<<std::endl;
+			}else{
+				f=f_goal*alpha_companion_+f_companion_Zanlungo*constante_multiplicar_fuerza_base_Zanlungo_*beta_companion_ + f_int*gamma_ + f_obs*delta_; // prueba, con parametros articulo revista robot companion
+				std::cout << " enter zanlungo ( more 3 meters)! case 2 ="<<std::endl;
+			//}
+
+		}
+
+		if(Action_== Cplan_local_nav_person_companion::START){ // solo en caso start!!!, no uses la f_companion, que hace que no reinicies bien!
+			f=f_goal*alpha_+ f_int*gamma_ + f_obs*delta_;
+		}
+
+
+
+		//f=f_goal*alpha_companion_+f_companion_Zanlungo*beta_companion_; //+ f_int*gamma_companion_ + f_obs*delta_companion_; // prueba, con parametros articulo revista robot companion
+		//f=f_goal*alpha_companion_+ f_int*gamma_companion_ + f_obs*delta_companion_; // prueba, con parametros articulo revista robot companion
+
+		//std::cout << " (5) "<< std::endl;
+		//f_goal_final=f_goal*alpha_companion_+f_person_goal*beta_companion_;
+
+		if(debug_zanlungo_){
+			std::cout << " (Zanlungo, fuerza final y parametros):  f.fx="<<f.fx<<"; f.fy="<<f.fy<<"; alpha_companion_="<<alpha_companion_<<"; beta_companion_"<<beta_companion_<<"; gamma_companion_="<<gamma_companion_<<"; delta_companion_="<<delta_companion_<<std::endl;//"; f_int.fx"<<f_int.fx<<"; f_int.fy="<<f_int.fy<<std::endl;
+			std::cout << " f_goal.fx= "<<f_goal.fx<<"; f_goal.fy="<<f_goal.fy <<"; f_obs.fx="<<f_obs.fx<<"; f_obs.fy="<<f_obs.fy<<std::endl;
+			std::cout << " f_companion_Zanlungo.fx= "<<f_companion_Zanlungo.fx<<"; f_companion_Zanlungo.fy="<<f_companion_Zanlungo.fy<<"; f_int.fx="<<f_int.fx<<"; f_int.fy="<<f_int.fy << std::endl;
+		}
+
+			//Sedge_tree_pcomp u( index2, f, f_goal, f_int, f_obs,f_person_goal);
+			//u_forces_robot_actual_=u;
+	}else{*/
+		// Caso side-by-side:
+		//std::cout << " breack_last! else, caso sin zanlungo "<< std::endl;
+
+		f=f_goal*alpha_ + f_int*gamma_ + f_obs*delta_;
+
+			//std::cout << " (Akp_planning, fuerza final y parametros):  f.fx="<<f.fx<<"; f.fy="<<f.fy<<"; alpha_companion_="<<alpha_companion_<<"; beta_companion_"<<beta_companion_<<"; gamma_companion_="<<gamma_companion_<<"; delta_companion_="<<delta_companion_<<std::endl;//"; f_int.fx"<<f_int.fx<<"; f_int.fy="<<f_int.fy<<std::endl;
+			//std::cout << " f_goal.fx= "<<f_goal.fx<<"; f_goal.fy="<<f_goal.fy <<"; f_obs.fx="<<f_obs.fx<<"; f_obs.fy="<<f_obs.fy<<std::endl;
+			//std::cout << " (F_tot no usa ZANLUNGO comp) f_companion_Zanlungo.fx= "<<f_companion_Zanlungo.fx<<"; f_companion_Zanlungo.fy="<<f_companion_Zanlungo.fy<<"; f_int.fx="<<f_int.fx<<"; f_int.fy="<<f_int.fy << std::endl;
+
+	//}
+
+
+
+	//f=f_goal*alpha_ + f_int*gamma_ + f_obs*delta_;
+
+	//std::cout << "(FINAL FORCE) alpha_="<<alpha_<<"; gamma_="<<gamma_<<"; delta_="<<delta_<<"; f_int.fx="<<f_int.fx<<"; f_int.fy="<<f_int.fy<< std::endl;
+
+	//if(debug_calculate_edge_person_companion_akp_){
+
+		/*std::cout << " (Fin) IN :calculate_edge f_goal.fx= "<<f_goal.fx<<"; f_goal.fy="<<f_goal.fy<<"; alpha_="<<alpha_<<"; f_int.fx="<<f_int.fx<<"; f_int.fy="<<f_int.fy<<"; gamma_="<<gamma_<<"; f_obs.fx="<<f_obs.fx<<"; f_obs.fy="<<f_obs.fy<<"; delta_="<<delta_<<std::endl;
+		std::cout << " (Fin) IN :calculate_edge accompanying_robot_goal.x= "<<accompanying_robot_goal.x<<"; accompanying_robot_goal.y="<<accompanying_robot_goal.y<<std::endl;
+
+		std::cout << " (Fin) IN :calculate_edge f_companion_robot_goal.fx= "<<f_companion_robot_goal.fx<<"; f_companion_robot_goal.fy="<<f_companion_robot_goal.fy<<"; f.fx="<<f.fx<<"; f.fy="<<f.fy<<std::endl;
+*/
+		//}
+	//std::cout << " (Akp_planning):  f.fx="<<f.fx<<"; f.fy="<<f.fy<<"; alpha_="<<alpha_<<"; gamma_"<<gamma_<<"; f_goal.fx"<<f_goal.fx<<"; f_goal.fy="<<f_goal.fy<<std::endl;
+	//std::cout << " (Akp_planning): (force to other person of the group) f_goal_companion.fx="<<f_goal_companion.fx<<"; f_goal_companion.fy="<<f_goal_companion.fy<<"; beta="<<beta<<"; (force to robot of the group) f_companion_robot_goal.fx="<<f_companion_robot_goal.fx<<"; f_companion_robot_goal.fy="<<f_companion_robot_goal.fy<<"; beta_robot_companion="<<beta_robot_companion<<std::endl;
+
+	// +f_persongoal*(param)  TODO: para la propagacion usa la f(global!!!), esta hay k incluirle el goal persona en la Sedge_tree_pcomp que le entra.
+	//Sforce f=(f_goal+f_persongoal)*alpha_ + f_int*gamma_ + f_obs*delta_;
+	// todo, quizas se podría incluir como: (f_goal+f_persongoal)*alpha_ ?!?!?! o considerar solo goal de ir hacia la posicion de la persona=> f_persongoal*alpha_
+
+
+	saved_tree_forces_to_see_companion_Force_Zanlungo_.push_back(Sedge_tree_pcomp( parent_vertex, f, f_goal, f_int, f_obs,f_companion_Zanlungo));
+
+
+
+
+	//std::cout <<" saved_tree_forces_to_see_companion_Force_Zanlungo_.size="<<saved_tree_forces_to_see_companion_Force_Zanlungo_.size()<<  std::endl;
+	//saved_tree_forces_to_see_companion_Force_Zanlungo_.back().print();
+	//std::cout << " (Akp_planning):  f.fx="<<f.fx<<"; f.fy="<<f.fy<<"; alpha_="<<alpha_<<"; gamma_"<<gamma_<<"; f_goal.fx"<<f_goal.fx<<"; f_goal.fy="<<f_goal.fy<<std::endl;
+
+	 return Sedge_tree_pcomp( parent_vertex, f, f_goal_final, f_int, f_obs,f_companion_robot_goal);
+}
+
+bool Cplan_local_nav_person_companion::propagate_vertex_person_companion_akp( unsigned int parent_index , const Sedge_tree_pcomp& u)
+{
+	// Robot propagation and cost ----------------------------------------------------------------------
+	unsigned int index_to_copy;
+	//std::cout <<" (IN propagate_vertex_person_companion_akp) "<<std::endl;
+
+	SpointV_cov virtual_next_pose;
+	Sforce virtual_force_goal, virtual_force_int_person, virtual_force_obstacle, virtual_force_robot;
+	//std::cout << " (Akp_planning):  u.f.fx="<<u.f.fx<<"; u.f.fy="<<u.f.fy<<"; alpha_="<<alpha_<<"; gamma_"<<gamma_<<std::endl;
+	if(((u.f_people.fx>0.5)||(u.f_people.fy>0.5))&&see_forces_){
+		//std::cout <<"; (propagate_vertex) f_int.fx"<<u.f_people.fx<<"; f_int.fy="<<u.f_people.fy<<"; parent_index="<<parent_index<<std::endl;
+	}
+	//std::cout <<" (1) propagate_vertex_person_companion_akp "<<std::endl;
+	//std::cout <<"; (propagate_vertex) f_obs.fx"<<u.f_obs.fx<<"; f_obs.fy="<<u.f_obs.fy<<"; parent_index="<<parent_index<<std::endl;
+
+	person_companion_->robot_propagation( dt_, parent_index , u.f );
+	//std::cout <<" (2) propagate_vertex_person_companion_akp "<<std::endl;
+	//check if the propagation is valid
+	bool collision_check = check_collision_person_companion_akp( person_companion_->get_planning_trajectory()->back(), parent_index );
+
+	if(collision_check){
+	//	std::cout <<" (IN propagate_vertex_person_companion_akp) "<<std::endl;
+	//	std::cout << " (propagate_vertex) collision_check=" << collision_check <<" dt_="<< dt_<<"; parent_index="<<parent_index<<  std::endl;
+	//	person_companion_->get_planning_trajectory()->back().print();
+	}
+
+
+	// vertixes plotting
+		//std::cout << robot_->get_robot_planning_trajectory()->back().x << " , " << robot_->get_robot_planning_trajectory()->back().y <<  std::endl;
+	//std::cout <<" (3) propagate_vertex_person_companion_akp "<<std::endl;
+		// Nearby people propagation and cost ---------------------------------------------------------------------
+		SpointV_cov robot_point;
+		for( Cperson_abstract* iit: nearby_person_list_ )
+		{
+			//std::cout <<" (3.1) propagate_vertex_person_companion_akp "<<std::endl;
+			// 1 precalculation: if really close to the robot, then a complete propagation is done, otherwise
+			// the correspondant Spose is sought.
+			if ( !iit->is_needed_to_propagate_person_for_planning(  parent_index, person_companion_->get_robot_planning_trajectory()->back() , index_to_copy ) )
+			{
+				//std::cout <<" (3.2) propagate_vertex_person_companion_akp "<<std::endl;
+				//update parent index correspondent vector TODO has to be an easiest way to not copy this
+				iit->planning_propagation_copy( index_to_copy );
+				robot_point =  person_companion_->get_planning_trajectory()->back();
+				virtual_force_robot =  iit->force( robot_point , get_sfm_int_params(iit,person_companion_),
+									&(iit->get_planning_trajectory()->at(parent_index) ) );
+				//std::cout <<" (3.3) propagate_vertex_person_companion_akp "<<std::endl;
+			}
+			else
+			{
+				//std::cout <<" (3.4) propagate_vertex_person_companion_akp "<<std::endl;
+				// 2 time step propagation
+				virtual_force_goal = iit->force_goal( iit->get_best_dest() , get_sfm_params(person_companion_),
+						&(iit->get_planning_trajectory()->at(parent_index) )  );
+				virtual_force_int_person = force_persons_int_planning_virtual( iit, parent_index );
+				if( read_force_map_success_ )
+					virtual_force_obstacle = get_force_map( iit->get_planning_trajectory()->at(parent_index).x,
+							iit->get_planning_trajectory()->at(parent_index).y ) * 0.5;
+
+			    //obstacles due to laser scans. for real environments has priority over map forces
+				if(read_laser_obstacle_success_)
+					virtual_force_obstacle = force_objects_laser_int_planning_virtual( iit , parent_index,16.0);
+				robot_point =  person_companion_->get_planning_trajectory()->back();
+				virtual_force_robot =  iit->force( robot_point , get_sfm_int_params(iit,person_companion_),
+						&(iit->get_planning_trajectory()->at(parent_index) ) );
+				iit->set_forces_person( virtual_force_goal, virtual_force_int_person, virtual_force_robot, virtual_force_obstacle );
+				iit->planning_propagation(dt_, iit->get_force_person() , parent_index);
+				//std::cout <<" (3.5) propagate_vertex_person_companion_akp "<<std::endl;
+			}
+			//calculate the interaction forces cost TO BE DEPRECATED
+			//dr = (Spoint)iit->get_planning_trajectory()->back() -
+			//		(Spoint)iit->get_planning_trajectory()->at(parent_index);
+			//work += fabs(virtual_force_robot * dr);
+
+		}
+
+		//std::cout <<" (4) propagate_vertex_person_companion_akp "<<std::endl;
+
+	return collision_check;
+}
+
+
+void Cplan_local_nav_person_companion::calculate_cost_person_companion_akp( unsigned int parent_index , Sedge_tree_pcomp u , Cperson_abstract::companion_reactive reactive)
+{
+	//for each propagation, a cost is calculated
+	//robot cost
+	Spoint dr;
+	double cost(0.0);
+
+	//dr = (Spoint) robot_->get_robot_planning_trajectory()->back() -
+	//		(Spoint) robot_->get_robot_planning_trajectory()->at(parent_index);
+	//work = fabs(u.f_goal * dr);
+	cost = u.f_goal.module2(cost_angular_);
+	cost_robot_.push_back( cost_robot_.at(parent_index) + cost);
+	// Obstacles cost -----------------------------------------------------------------------------------------
+	// If collision is detected, this function should return a high value,
+	// but it does not distinguish if a collision took place, only an abnormally huge value
+	//work = fabs(u.f_obs * dr );
+	//cost_obstacles_.push_back( cost_obstacles_.at(parent_index) + work );
+
+	//alternative way: max force module
+	cost = u.f_obs.module2();
+	//if ( cost_obstacles_.at(parent_index) > work )
+		//work = cost_obstacles_.at(parent_index);
+	cost_obstacles_.push_back( cost_obstacles_.at(parent_index) + cost );
+
+	//people cost --------------------------------------------------------------------------------------------
+	Sforce f,f_int,f2;
+	cost = 0;
+
+	for( Cperson_abstract* iit: nearby_person_list_  )
+	{
+		switch( pr_force_mode_ )
+		{
+		case 0: //deterministic force, classical definition
+			if((iit->get_id()!=id_person_companion_)&&(reactive!=Cperson_abstract::Akp_planning)){
+				f = iit->force_sphe(person_companion_->get_planning_trajectory()->back() , get_sfm_int_params(iit,person_companion_) , &(iit->get_planning_trajectory()->at( parent_index )) ,reactive);
+			}else{
+				//std::cout << " IN :calculate_cost, else: Akp_planning, salta person companion " <<  std::endl;
+			}
+			break;
+		case 1: //probabilistic force, sampling around the elipsoid
+			f = iit->force_sphe_prob( person_companion_->get_planning_trajectory()->back(), get_sfm_int_params(iit,person_companion_),&(iit->get_planning_trajectory()->at( parent_index )) );
+			break;
+		case 2: //probabilistic force, mahalanobis distance
+			f = iit->force_sphe_mahalanobis( person_companion_->get_planning_trajectory()->back(), get_sfm_int_params(iit,person_companion_), &(iit->get_planning_trajectory()->at( parent_index )));
+			break;
+		case 3: //probabilistic force, worst case scenario: covaraince ellipsoid nearer to the center point
+			f = iit->force_sphe_worst( person_companion_->get_planning_trajectory()->back(), get_sfm_int_params(iit,person_companion_), &(iit->get_planning_trajectory()->at( parent_index )));
+			break;
+		}
+		f_int += f;
+		cost += f.module2();
+	}
+
+	//if ( cost_int_forces_.at(parent_index) > work )
+		//work = cost_int_forces_.at(parent_index);
+	cost_int_forces_.push_back( cost_int_forces_.at(parent_index) + cost );
+
+	//distance cost --------------------------------------------------------------------------------------------
+	cost = person_companion_->get_planning_trajectory()->back().distance2(local_goal_);
+	cost_distance_.push_back( cost_distance_.at(parent_index) + cost );
+	double o = person_companion_->get_planning_trajectory()->back().angle_heading_point( local_goal_ );
+	cost_orientation_.push_back( cost_orientation_.at(parent_index) + o*o );
+
+    // Local minima cost  --------------------------------------------------------------------------------------------------------- TOBEDEPRECATED
+    // identify possible local minima problems abs ( sum f ) != sum( abs f) Initial calculation
+	//double sum_abs = u.f_goal.module2() + u.f_people.module2() + u.f_obs.module2();
+	//double sum = (u.f_goal + u.f_people + u.f_obs).module2();
+	//cost_local_minima_.push_back( cost_local_minima_.at(parent_index) - sum/sum_abs);//summation of all local minima indicators throughout the path
+
+
+	nodes_in_branch_.push_back( nodes_in_branch_.at(parent_index) + 1.0 );
+
+	// Simililarities wrt previous path --------------------------------------------------------------------------
+	//this function is properly calculated using the COMPLETE path. see 8) in planning()
+
+	if(reactive==Cperson_abstract::Akp_planning){
+		Crobot* robot_act=person_companion_;
+		calculate_companion_cost_node(parent_index, robot_act); //funcion (ely) calculate cost companion. TODO: descomentar cuando el robot vaya bien!
+	}
+
+}
+
+
+double Cplan_local_nav_person_companion::cost_to_go_person_companion_akp( const Spoint& random_goal, unsigned int i , Cperson_abstract::companion_reactive reactive)
+{
+	double result;
+	double d, t, o, dx, dy;
+	//metrics to evaluate the nearest vertex
+	switch( distance_mode_ )
+	{
+		//distance to random goal ----------------------- Euclidean distance: TO BE DEPRECATED   --------------------
+		case Cplan_local_nav_person_companion::Euclidean  :
+			//1- calculate distance normalized to max distance
+			d = person_companion_->get_robot_planning_trajectory()->at(i).distance( random_goal )
+					/ workspace_radii_ * cost_parameters_[0];
+			//2- orientation
+			dx = random_goal.x - person_companion_->get_robot_planning_trajectory()->at(i).x;
+			dy = random_goal.y - person_companion_->get_robot_planning_trajectory()->at(i).y;
+			o = fabs(diffangle( person_companion_->get_robot_planning_trajectory()->at(i).theta ,
+					atan2( dy, dx )  )) * cost_parameters_[1];
+
+			//3- accumulated robot work cost_parameters_[2] DEPRECATED
+			//wr = cost_robot_[i]*cost_parameters_[2];
+
+			//4- accumulated persons work cost_parameters_[3] DEPRECATED
+			//wp = cost_int_forces_[i]*cost_parameters_[3];
+
+			//5- time not allowed being outside the time horizon
+			t = (person_companion_->get_robot_planning_trajectory()->at(i).time_stamp -
+					now_ ) / horizon_time_ ;
+			//TODO other costs! bstacles + Local min
+			if ( t > 1.0 ) t = 10000.0; //discard the vertex
+			else t *= cost_parameters_[4];
+
+			//6 - obstacles
+			//wo = cost_obstacles_[i]*cost_parameters_[5];
+			//- Local minima indicator
+			//lm = cost_parameters_[7]*cost_local_minima_[i]/nodes_in_branch_[i];;
+			//check for min
+			result = d+o+t;
+			break;
+
+		//cost-to-go linear normalization to random goal  ------------------------------------------------------
+		  case  Cplan_local_nav_person_companion::Cost2go_norm :
+		//cost-to-go erf normalization to random goal ----------------------------------------------------------
+		  case  Cplan_local_nav_person_companion::Cost2go_erf :
+		  {
+			  // Gonzalo entra en este de distancia. !!!!!!!!!!!!!!!
+			 // std::cout << "cost to go (1)"<<  std::endl;
+			SpointV robot = person_companion_->get_planning_trajectory()->at(i);
+			Sforce f_goal, f_int, f_obs, f, f_tot;
+			//Sforce f_persongoal; // companion (ely) fuerza hacia la persona para el companion.
+			double look_time(0.0),cost_robot(0.0),cost_distance(0.0), cost_int(0.0), cost_obs(0.0),cost_orientation(0.0),o;//cost_companion(0.0);
+			unsigned int prediction_index(2),max_index_prediction(0);
+			//assert to avoid degenerated cases when the path does not look up to t_h due to being near the goal
+			//  std::cout << "cost to go (2)"<<  std::endl;
+			if ( person_companion_->get_planning_trajectory()->size() <  horizon_time_index_)
+			{
+				max_index_prediction = person_companion_->get_planning_trajectory()->size() -1;
+			}
+			else
+			{
+				max_index_prediction = horizon_time_index_;
+			}
+			 // std::cout << "cost to go (3)"<<  std::endl;
+			//calculates current prediction time index [0,horizon] and distance to random goal of previous path, TODO not working well
+			while(  look_time < robot.time_stamp  && prediction_index < max_index_prediction )
+			{
+				look_time = person_companion_->get_planning_trajectory()->at(prediction_index).time_stamp;
+				cost_distance += person_companion_->get_planning_trajectory()->at(prediction_index).distance2( random_goal );//previous distance to get to the goal
+				prediction_index += 2;
+			}
+			 // std::cout << "cost to go (4)"<<  std::endl;
+			prediction_index -= 2;
+			if ( look_time - now_ > horizon_time_ || prediction_index >=  horizon_time_index_ )
+				return 1e10;
+
+			 // std::cout << "cost to go (5)"<<  std::endl;
+			//std::cout << "entering  " <<  i << "  time = " << prediction_index <<  "  size = " << robot_->get_planning_trajectory()->size() << std::endl;
+			cost_robot = cost_robot_[i]/2.0;//as we are using double time step and half propagations, the accumulated robot cost
+			cost_obs = cost_obstacles_[i]/2.0;
+			cost_int = cost_int_forces_[i]/2.0;
+			//cost_companion = cost_companion_[i]/2.0;
+			while ( prediction_index < horizon_time_index_  )
+			{
+				 //std::cout << "cost to go (5.1) nearby_person_list_.size()="<<nearby_person_list_.size()<<  std::endl;
+				// cost due to interacting forces  (De aquí, habria que reducir la que te proboca la persona a la que acompañas)
+				for( Cperson_abstract* iit: nearby_person_list_  )
+				{
+					 //std::cout << "cost to go (5.1) iit->get_id()="<<iit->get_id()<<  std::endl;
+					/*std::cout << "cost to go (5.1) iit->get_id()="<<iit->get_id()<<"; id_person_companion_="<<id_person_companion_<<  std::endl;
+					std::cout << "cost to go (5.1) reactive="<<reactive<<"; Cperson_abstract::Akp_planning="<<Cperson_abstract::Akp_planning<<  std::endl;
+					std::cout << "cost to go (5.1) iit->x="<<iit->get_current_pointV().x<<"; iit->y="<<iit->get_current_pointV().y<<  std::endl;
+					std::cout << "cost to go (5.1) robot->x="<<robot.x<<"; robot->y="<<robot.y<<  std::endl;
+					std::cout << "cost to go (5.1) iit->get_prediction_trajectory().size="<<iit->get_prediction_trajectory()->size()<<"; empty="<<iit->get_prediction_trajectory()->empty()<<"; prediction_index="<<prediction_index<<  std::endl;
+					std::cout <<"; pred_traj.x="<<iit->get_prediction_trajectory()->at( prediction_index ).x<<"; pred_traj.y="<<iit->get_prediction_trajectory()->at( prediction_index ).y<<  std::endl;
+					 */
+
+					if(bool_tes_prop2){
+						std::cout <<"; id="<<iit->get_id()<<"; traj_size=="<<iit->get_prediction_trajectory()->size()<<  std::endl;
+					}
+
+
+					if(iit->get_prediction_trajectory()->size()>prediction_index){
+					if ( (iit->get_prediction_trajectory()->at( prediction_index ).distance2(robot) < 16.0 ) && (iit->get_id()!=id_person_companion_) )//<4m...to speed up, a more strict threshold is used
+					{ // para la person companion en el akp no hay fuerzas repulsivas contra ella.
+						//TODO aqui algo no va bien, sale demasiado pequeña esta cantidad: error o propagacion fatal
+						if((iit->get_id()!=id_person_companion_)&&(reactive!=Cperson_abstract::Akp_planning)){
+							//std::cout << " IN :cost_to_go, if: Akp_planning, differente Akp_planning " <<  std::endl;
+							f = iit->force_sphe(robot , get_sfm_int_params(iit,person_companion_) , &iit->get_prediction_trajectory()->at( prediction_index ) ,reactive);
+							//std::cout<<";(id_person_tenida_en_cuenta_cost_to_go_planning-> iit->get_id()=" << iit->get_id()<<  std::endl;
+							//std::cout <<  f.module2() << " , " <<  std::endl;
+							f_int += f;
+							cost_int += f.module2();
+						}else{ // caso person_companion con el robot.
+							//std::cout << " IN :cost_to_go, else: Akp_planning, salta person companion " <<  std::endl;
+							Spoint robot2=person_companion_->get_planning_trajectory()->at(i);
+							f = iit->force_sphe(robot2 , get_sfm_int_params(iit,person_companion_) , &iit->get_prediction_trajectory()->at( prediction_index ) ,reactive);
+							//f_int += f; // TODO: pensar si se incluye en el planing la influencia del robot en la person_companion o no...
+							//cost_int += f.module2();
+
+						}
+					}
+					}
+				}
+				 //std::cout << "cost to go (5.1.2)"<<  std::endl;
+				// TEST: person_companion include force with the tibi=robot
+				/*Spoint robot_tibi_point=Spoint(robot_->get_current_pointV().x,robot_->get_current_pointV().y,robot_->get_current_pointV().time_stamp);
+				Sforce f_person_comp_to_tibi=person_companion_->force_sphe(robot_tibi_point,get_sfm_int_params(person_companion_,robot_),&robot);
+				//f_person_comp_to_tibi.fx=f_person_comp_to_tibi.fx/2;
+				//f_person_comp_to_tibi.fy=f_person_comp_to_tibi.fy/2;
+				f_int +=f_person_comp_to_tibi;*/
+
+				//std::cout << " robot_tibi_point.x="<<robot_tibi_point.x<<"robot_tibi_point.y="<<robot_tibi_point.y<<"f_person_comp_to_tibi.fx= "<<f_person_comp_to_tibi.fx<<"; f_person_comp_to_tibi.fy="<<f_person_comp_to_tibi.fy <<  std::endl;
+				// std::cout << "cost to go (5.2)"<<  std::endl;
+				// potential cost to go destination
+				f_goal = person_companion_->force_goal( Sdestination(0,random_goal.x,random_goal.y), get_sfm_params(person_companion_), &robot );
+				cost_robot += f_goal.module2(cost_angular_);
+
+				//cost due to obstacles
+				/*for( Spoint iit : laser_obstacle_list_)
+				{
+					if ( iit.distance2(robot) < 9.0 )
+					{
+						f = robot_->force_sphe( iit, get_sfm_int_params(robot_), &robot );
+						f_obs += f;
+						cost_obs += f.module2();
+					}
+				}*/
+				// std::cout << "cost to go (5.3)"<<  std::endl;
+				// REtocado para reducir fuerza muchos obstaculos juntos.
+				for( Spoint iit : laser_obstacle_list_)
+				{
+					//if ( iit.distance2(robot) < 9.0 )
+					//if ( iit.distance(robot) < (max_distance_to_obstacles_detected_+person_robot_actual_real_distance_/2) )
+					//{
+						f = person_companion_->force_sphe( iit, get_sfm_int_params(person_companion_), &robot );
+						f_obs += f;
+						//cost_obs += f.module2();
+					//}
+				}
+
+				// std::cout << "cost to go (5.4)"<<  std::endl;
+				// INICION Limitar fuerza obstaculos cuando hay muchos solapados
+				double fobos_Xmod=sqrt(f_obs.fx*f_obs.fx);
+				double fobos_Ymod=sqrt(f_obs.fy*f_obs.fy);
+				double signo_x;
+				double signo_y;
+				if (f_obs.fx > 0){
+					signo_x=1;
+				}else{
+					signo_x=-1;
+				}
+				if (f_obs.fy > 0){
+					signo_y=1;
+				}else{
+					signo_y=-1;
+				}
+				// std::cout << "cost to go (5.5)"<<  std::endl;
+				if((fobos_Xmod>f_obst_max_x_)||(fobos_Ymod>f_obst_max_y_)){
+					f_obs.fx=signo_x*f_obst_max_x_;
+					f_obs.fy=signo_y*f_obst_max_y_;
+				}
+				// FIN Limitar fuerza obstaculos cuando hay muchos solapados
+
+				// robot propagation  (TODO: (ELY-companion)=> aqui tendria que add una fuerza que acerque al robot hacia la persona.
+				//f_tot=f_goal+f_obs+f_int;
+				f_tot=f_goal+f_obs+f_int;//+f_persongoal; // TODO, incluir la f_persongoal.
+				robot = robot.propagate( dt_*2, f_tot, robot_->get_desired_velocity() );//at double timestep to speed up calculations
+				cost_distance += robot.distance2(random_goal);
+				o = robot.angle_heading_point( random_goal );
+				cost_orientation += o*o;
+				prediction_index += 2;
+				// std::cout << "cost to go (5.6)"<<  std::endl;
+
+			}
+
+			 // std::cout << "cost to go (6)"<<  std::endl;
+			if( distance_mode_ == Cplan_local_nav_person_companion::Cost2go_erf )
+			{
+				//std::cout << "%cost_distance, cost_orientation, cost_robot, cost_int, cost_obs" << endl;
+				//std::cout  << cost_distance << " , " << cost_orientation << " , " << cost_robot << " , " << cost_int << " , " <<  cost_obs << std::endl;
+				result = cost_parameters_[0]*erf((cost_distance - mean_cost_distance_) / std_cost_distance_) +
+						cost_parameters_[1]*erf((cost_orientation - mean_cost_orientation_) / std_cost_orientation_) +
+						cost_parameters_[2]*erf((cost_robot - mean_cost_robot_) / std_cost_robot_ ) +
+						cost_parameters_[3]*erf((cost_int - mean_cost_int_forces_) / std_cost_int_forces_ ) +
+						cost_parameters_[5]*erf((cost_obs - mean_cost_obstacles_) / std_cost_obstacles_);
+
+				/*result = 0.5*(cost_parameters_[0]*erf((cost_distance - mean_cost_distance_) / std_cost_distance_) +
+						 cost_parameters_[1]*erf((cost_orientation - mean_cost_orientation_) / std_cost_orientation_) +
+						 cost_parameters_[2]*erf((cost_robot - mean_cost_robot_) / std_cost_robot_ ) +
+						 cost_parameters_[3]*erf((cost_int - mean_cost_int_forces_) / std_cost_int_forces_ ) +
+						 cost_parameters_[5]*erf((cost_obs - mean_cost_obstacles_) / std_cost_obstacles_))+0.5*erf((cost_companion - mean_cost_companion_) / std_cost_companion_);
+			*/
+			}
+			else
+			{
+			  //use the mean as the utopia point a std as the difference f_max - utopia, calculated previously
+			result = cost_parameters_[0]*((cost_distance - mean_cost_distance_) / std_cost_distance_) +
+						cost_parameters_[1]*((cost_orientation - mean_cost_orientation_) / std_cost_orientation_) +
+						cost_parameters_[2]*((cost_robot - mean_cost_robot_) / std_cost_robot_ ) +
+						cost_parameters_[3]*((cost_int - mean_cost_int_forces_) / std_cost_int_forces_ ) +
+						cost_parameters_[5]*((cost_obs - mean_cost_obstacles_) / std_cost_obstacles_);
+			}
+			//  std::cout << "cost to go (7)"<<  std::endl;
+
+
+
+			break;
+		  }
+		  //cost-to-go to a random goal only considering steering forces and collision ------------------------------------------------------
+		  case  Cplan_local_nav_person_companion::Cost2go_raw :
+		  default:
+		  {
+			SpointV robot = person_companion_->get_planning_trajectory()->at(i);
+			Sforce f_goal;
+			double look_time(0.0), cost_robot(0.0);
+			unsigned int prediction_index(2),max_index_prediction(0);
+			//assert to avoid degenerated cases when the path does not look up to t_h due to being near the goal
+			if ( person_companion_->get_planning_trajectory()->size() <  horizon_time_index_)
+			{
+				max_index_prediction = person_companion_->get_planning_trajectory()->size() -1;
+			}
+			else
+			{
+				max_index_prediction = horizon_time_index_;
+			}
+			//calculates current prediction time index [0,horizon] and distance to random goal of previous path, TODO not working well
+			while(  look_time < robot.time_stamp  && prediction_index < max_index_prediction )
+			{
+				look_time = person_companion_->get_planning_trajectory()->at(prediction_index).time_stamp;
+				prediction_index += 2;
+			}
+			prediction_index -= 2;
+			if ( look_time - now_ > horizon_time_ || prediction_index >=  horizon_time_index_ )
+				return 1e10;
+			//std::cout << "entering  " <<  i << "  time = " << prediction_index <<  "  size = " << robot_->get_planning_trajectory()->size() << std::endl;
+			cost_robot = cost_robot_[i]/2.0;//as we are using double time step and half propagations, the accumulated robot cost
+			while ( prediction_index < horizon_time_index_  )
+			{
+				// potential cost to go destination
+				f_goal = person_companion_->force_goal( Sdestination(0,random_goal.x,random_goal.y), get_sfm_params(person_companion_), &robot );
+				cost_robot += f_goal.module2(cost_angular_);//fy cost 0.5
+
+				// robot propagation
+				robot = robot.propagate( dt_*2, f_goal, person_companion_->get_desired_velocity() );//at double timestep to speed up calculations
+				prediction_index += 2;
+
+				//check for collisions, only with obstacles
+				if (  check_collision_person_companion_akp( robot ) )
+					return 1e10;
+			}
+			result = cost_robot;
+
+			break;
+		  }
+		}//end of switch
+	//std::cout<<" END cost to go!" << std::endl;
+
+	return result;
+}
+
+
+unsigned int Cplan_local_nav_person_companion::global_min_cost_index( Crobot* robot_act , Cperson_abstract::companion_reactive reactive )
+{
+//std::cout<<" INI function: global_min_cost_index()" << std::endl;
+	double cost, min_cost(1e10);
+	unsigned int min_cost_index(0);
+	double d, o, dx, dy;
+	preprocess_global_parameters(robot_act);//depending on mode, needs a different preprocessing of data
+
+	we_have_cost_companion_=false;
+
+	switch( global_mode_ )
+	{
+
+	  //distance to local goal, end of branch ----------------------- Euclidean distance: TO BE DEPRECATED
+	  case Cplan_local_nav_person_companion::Scalarization :
+		  std::cout << "   Scalarization" << std::endl;
+		for( unsigned int i : end_of_branches_index_ )
+		{
+			d = robot_act->get_robot_planning_trajectory()->at(i).distance( local_goal_ );
+			dx = goal_.x - robot_act->get_robot_planning_trajectory()->at(i).x;
+			dy = goal_.y - robot_act->get_robot_planning_trajectory()->at(i).y;
+			o = fabs(diffangle( robot_act->get_robot_planning_trajectory()->at(i).theta ,
+				atan2( dy, dx )  ));
+			cost = cost_parameters_[0]*d +
+				cost_parameters_[1]*o +
+				cost_parameters_[2]*cost_robot_[i] +
+				cost_parameters_[3]*cost_int_forces_[i]/nodes_in_branch_[i] +
+				cost_parameters_[5]*cost_obstacles_[i]/nodes_in_branch_[i] +
+				cost_parameters_[6]*cost_past_traj_[i];
+
+			if(debug_antes_subgoals_entre_AKP_goals_){
+				std::cout<<"(CASE) Cplan_local_nav_person_companion::Scalarization"<< std::endl;
+				std::cout<<"i ="<<i<< std::endl;
+				//std::cout<<"(GONZALO) cost ="<<cost<< std::endl;
+			}
+
+			if(cost_companion_[i]!=0){
+				cost =(0.9)*cost+(0.1)*cost_companion_[i];
+				if(debug_antes_subgoals_entre_AKP_goals_){
+					std::cout<<"(9/10)*cost="<<(0.9)*cost<< std::endl;
+					std::cout<<"(1/10)*cost_companion_[i]="<<(0.1)*cost_companion_[i]<< std::endl;
+					std::cout<<"cost_companion_["<<i<<"]="<<cost_companion_[i]<< std::endl;
+					std::cout<<"cost="<<cost<< std::endl;
+				}
+			}else{
+				if(debug_antes_subgoals_entre_AKP_goals_){
+					std::cout<<"NO HAY cost_companion_["<<i<<"]="<<cost_companion_[i]<< std::endl;
+				}
+			}
+
+
+			if ( cost < min_cost)
+			{
+				min_cost = cost;
+				min_cost_index = i;
+			}
+			/*std::cout //<< "cost values (goal,ori,rob,int,obs,past,lm) = ("
+				<< d <<
+				" , " << o <<
+				" , " << cost_int_forces_[i]/nodes_in_branch_[i] <<
+				" , " << cost_obstacles_[i]/nodes_in_branch_[i] <<
+				" , " << cost_past_traj_[i] <<
+				" , " << cost_local_minima_[i]/nodes_in_branch_[i] << std::endl;*/
+		}
+		break;
+		// global cost, estimation and normalization ---------------------------------------------------------------
+	  case Cplan_local_nav_person_companion::Weighted_sum_erf :
+		  std::cout << "   Weighted_sum_erf" << std::endl;
+		for( unsigned int i : end_of_branches_index_ )
+		{
+			cost = cost_parameters_[0]*erf((cost_distance_[i] - mean_cost_distance_) / std_cost_distance_) +
+				cost_parameters_[1]*erf((cost_orientation_[i] - mean_cost_orientation_) / std_cost_orientation_) +
+				cost_parameters_[2]*erf((cost_robot_[i] - mean_cost_robot_) / std_cost_robot_ ) +
+				cost_parameters_[3]*erf((cost_int_forces_[i] - mean_cost_int_forces_) / std_cost_int_forces_ ) +
+				cost_parameters_[5]*erf((cost_obstacles_[i] - mean_cost_obstacles_) / std_cost_obstacles_) +
+				cost_parameters_[6]*erf((cost_past_traj_[i] - mean_cost_past_traj_) / std_cost_past_traj_);
+
+			if(debug_antes_subgoals_entre_AKP_goals_){
+				std::cout<<"(CASE) Cplan_local_nav_person_companion::Weighted_sum_erf "<< std::endl;
+				std::cout<<"i ="<<i<< std::endl;
+				std::cout<<"(GONZALO) cost ="<<cost<< std::endl;
+			}
+
+			if(cost_companion_[i]!=0){
+				cost =(0.9)*cost+(0.1)*cost_companion_[i];
+				if(debug_antes_subgoals_entre_AKP_goals_){
+					std::cout<<"cost_companion_["<<i<<"]="<<cost_companion_[i]<< std::endl;
+					std::cout<<"cost="<<cost<< std::endl;
+				}
+			}else{
+				if(debug_antes_subgoals_entre_AKP_goals_){
+					std::cout<<"NO HAY cost_companion_["<<i<<"]="<<cost_companion_[i]<< std::endl;
+				}
+			}
+
+			if ( cost < min_cost)
+			{
+				min_cost = cost;
+				min_cost_index = i;
+			}
+			/*std::cout << i << " ,"
+			 	<< erf((cost_distance_[i] - mean_cost_distance_) / std_cost_distance_) << " , "
+				<< erf((cost_orientation_[i] - mean_cost_orientation_) / std_cost_orientation_) << " , "
+				<< erf((cost_robot_[i] - mean_cost_robot_) / std_cost_robot_ ) << " , "
+				<< erf((cost_int_forces_[i] - mean_cost_int_forces_) / std_cost_int_forces_ ) << " , "
+				<< erf((cost_obstacles_[i] - mean_cost_obstacles_) / std_cost_obstacles_) << std::endl;*/
+		}
+		break;
+	  case  Cplan_local_nav_person_companion::Weighted_sum_norm :
+
+		  std::cout << "   Weighted_sum_norm" << std::endl;
+		  // mean_cost is the utopia cost corresponding to a free path and std_cost is the max_cost - utopia, according to MMO clasical approaches
+	    for( unsigned int i : end_of_branches_index_ )
+	    {
+			cost = cost_parameters_[0]*((cost_distance_[i] - mean_cost_distance_) / std_cost_distance_) +
+					cost_parameters_[1]*((cost_orientation_[i] - mean_cost_orientation_) / std_cost_orientation_) +
+					cost_parameters_[2]*((cost_robot_[i] - mean_cost_robot_) / std_cost_robot_ ) +
+					cost_parameters_[3]*((cost_int_forces_[i] - mean_cost_int_forces_) / std_cost_int_forces_ ) +
+					cost_parameters_[5]*((cost_obstacles_[i] - mean_cost_obstacles_) / std_cost_obstacles_) +
+					cost_parameters_[6]*((cost_past_traj_[i] - mean_cost_past_traj_) / std_cost_past_traj_);
+
+			if(debug_antes_subgoals_entre_AKP_goals_){
+				std::cout<<"(CASE) Cplan_local_nav_person_companion::Weighted_sum_norm "<< std::endl;
+				std::cout<<"i ="<<i<< std::endl;
+				std::cout<<"(GONZALO) cost ="<<cost<< std::endl;
+			}
+
+			if(cost_companion_[i]!=0){
+				cost =(0.9)*cost+(0.1)*cost_companion_[i];
+				if(debug_antes_subgoals_entre_AKP_goals_){
+					std::cout<<"cost_companion_["<<i<<"]="<<cost_companion_[i]<< std::endl;
+					std::cout<<"cost="<<cost<< std::endl;
+				}
+			}else{
+				if(debug_antes_subgoals_entre_AKP_goals_){
+					std::cout<<"NO HAY cost_companion_["<<i<<"]="<<cost_companion_[i]<< std::endl;
+				}
+			}
+
+			if ( cost < min_cost)
+			{
+				min_cost = cost;
+				min_cost_index = i;
+			}
+			/*std::cout << i << " , "
+				<<	((cost_distance_[i] - mean_cost_distance_) / std_cost_distance_) << " , "
+				<< ((cost_orientation_[i] - mean_cost_orientation_) / std_cost_orientation_) << " , "
+				<< ((cost_robot_[i] - mean_cost_robot_) / std_cost_robot_ ) << " , "
+				<< ((cost_int_forces_[i] - mean_cost_int_forces_) / std_cost_int_forces_ ) << " , "
+				<< ((cost_obstacles_[i] - mean_cost_obstacles_) / std_cost_obstacles_) << std::endl;*/
+	    }
+		break;
+	  case  Cplan_local_nav_person_companion::MO_erf :
+	  case  Cplan_local_nav_person_companion::MO_norm :
+	  {
+		  // Entra en esta!
+		 // std::cout << "[Cplan_local_nav_person_companion::MO_norm/MO_erf]" << std::endl;
+		  //first approach: push into a set of best trajectories
+		  nondominated_plan_vertex_index_.clear();
+		  nondominated_end_of_plan_vertex_index_.clear();
+		  unsigned int nondominated_branch_index, cont;
+		  std::vector<SsavePath_cost_and_values> non_domianted_path_and_act_costs_;
+		  non_domianted_path_and_act_costs_.reserve(nondominated_multicosts_.size());
+		 non_domianted_path_and_act_costs_.resize(nondominated_multicosts_.size());
+
+
+		  const std::vector<Spose>* plans_act = robot_act->get_robot_planning_trajectory();
+
+			//std::cout << "[Cplan_local_nav_person_companion::MO_norm/MO_erf] plans_act.empty()="<<plans_act->empty() <<"; cont = (horizon_time_index_/2)*2="<<(horizon_time_index_/2)*2<<"; nondominated_multicosts_.size()="<<nondominated_multicosts_.size()<< std::endl;
+
+		  unsigned int iter=0;
+
+		  for (  Smulticost_pcom m : nondominated_multicosts_)
+		  {
+		    nondominated_branch_index = m.id;
+		    cont = (horizon_time_index_/2)*2;//to avoid plotting all solutions, we will only plot the nondominated set, even number
+
+		    //print nondominated and normalized costs
+		    //m.print_ml();
+		    //print nondominated raw costs
+			/*std::cout << nondominated_branch_index << " , "
+				<< cost_distance_[nondominated_branch_index]  << " , "
+				<< cost_orientation_[nondominated_branch_index]  << " , "
+				<< cost_robot_[nondominated_branch_index]  << " , "
+				<< cost_int_forces_[nondominated_branch_index]  << " , "
+				<< cost_obstacles_[nondominated_branch_index]  << std::endl;*/
+		    //std::cout << " [Cplan_local_nav_person_companion::MO_norm/MO_erf] ANTES nondominated_end_of_plan_vertex_index_.push_back( nondominated_branch_index );" << std::endl;
+			nondominated_end_of_plan_vertex_index_.push_back( nondominated_branch_index );//only end of branches to plot the branch id
+			//std::cout << " [end of branch index] nondominated_branch_index="<<nondominated_branch_index<< std::endl;
+			non_domianted_path_and_act_costs_[iter].set_end_id_path(nondominated_branch_index);
+
+			//std::cout<<"non_domianted_path_and_act_costs_ => nondominated_branch_index="<<nondominated_branch_index<< std::endl;
+
+			std::vector<unsigned int> act_all_ids_path_positions;
+			std::vector<Spoint> act_path_positions;
+			std::vector<std::vector<double>> iter_vect_costst_act;
+			do
+			{
+				nondominated_plan_vertex_index_.push_back( nondominated_branch_index);
+				act_all_ids_path_positions.push_back( nondominated_branch_index);
+				act_path_positions.push_back(plans_act->at(nondominated_branch_index));
+
+				std::vector<double> vect_costst_act;
+				vect_costst_act.push_back(cost_parameters_[0]*erf((cost_distance_[nondominated_branch_index] - mean_cost_distance_) / std_cost_distance_));
+				vect_costst_act.push_back(cost_parameters_[1]*erf((cost_orientation_[nondominated_branch_index] - mean_cost_orientation_) / std_cost_orientation_));
+				vect_costst_act.push_back(cost_parameters_[2]*erf((cost_robot_[nondominated_branch_index] - mean_cost_robot_) / std_cost_robot_ ));
+				vect_costst_act.push_back(cost_parameters_[3]*erf((cost_int_forces_[nondominated_branch_index] - mean_cost_int_forces_) / std_cost_int_forces_ ));
+				vect_costst_act.push_back(cost_parameters_[5]*erf((cost_obstacles_[nondominated_branch_index] - mean_cost_obstacles_) / std_cost_obstacles_));
+				vect_costst_act.push_back(erf((cost_companion_[nondominated_branch_index] - mean_cost_companion_) / std_cost_companion_));
+				vect_costst_act.push_back(cost_parameters_[6]*erf((cost_past_traj_[nondominated_branch_index] - mean_cost_past_traj_) / std_cost_past_traj_));
+				cost = cost_parameters_[0]*erf((cost_distance_[nondominated_branch_index] - mean_cost_distance_) / std_cost_distance_) +
+										cost_parameters_[1]*erf((cost_orientation_[nondominated_branch_index] - mean_cost_orientation_) / std_cost_orientation_) +
+										cost_parameters_[2]*erf((cost_robot_[nondominated_branch_index] - mean_cost_robot_) / std_cost_robot_ ) +
+										cost_parameters_[3]*erf((cost_int_forces_[nondominated_branch_index] - mean_cost_int_forces_) / std_cost_int_forces_ ) +
+										cost_parameters_[5]*erf((cost_obstacles_[nondominated_branch_index] - mean_cost_obstacles_) / std_cost_obstacles_) +
+										cost_parameters_[6]*erf((cost_past_traj_[nondominated_branch_index] - mean_cost_past_traj_) / std_cost_past_traj_);
+				// TODO: ojo! Cost robot estaba quitado del coste final y creo que ha de ir!!!
+
+				double new_cost=cost;
+
+				//std::cout << "(ant) [nondominated_branch_index] new_cost="<<new_cost<<"; cost_companion="<< erf((cost_companion_[nondominated_branch_index] - mean_cost_companion_) / std_cost_companion_)<< std::endl;
+
+				if((cost_companion_[nondominated_branch_index]!=0)&&(reactive==Cperson_abstract::Akp_planning)){
+					//new_cost= (0.9)*cost+(0.1)*erf((cost_companion_[nondominated_branch_index] - mean_cost_companion_) / std_cost_companion_);
+
+						// caso side-by-side:
+					    we_have_cost_companion_=true;
+						new_cost= (0.9)*cost+(0.1)*erf((cost_companion_[nondominated_branch_index] - mean_cost_companion_) / std_cost_companion_);
+
+
+				}
+
+				//std::cout << "[nondominated_branch_index] new_cost="<<new_cost<<"; cost_companion="<< erf((cost_companion_[nondominated_branch_index] - mean_cost_companion_) / std_cost_companion_)<< std::endl;
+				cost=new_cost;
+
+				//std::cout << " [Cplan_local_nav_person_companion::MO_norm/MO_erf] new_cost="<<new_cost<< std::endl;
+				vect_costst_act.push_back(new_cost);
+				iter_vect_costst_act.push_back(vect_costst_act);
+				nondominated_branch_index = edge_[ nondominated_branch_index ].parent;
+				--cont;
+			}while( nondominated_branch_index > 0 && cont >= 1);
+
+			//std::cout<<"antes  positions + cost + iter ="<<iter_vect_costst_act.size()<< std::endl;
+
+			non_domianted_path_and_act_costs_[iter].set_total_costs_of_each_path_position(iter_vect_costst_act);
+			non_domianted_path_and_act_costs_[iter].set_all_ids_path_positions(act_all_ids_path_positions);
+			non_domianted_path_and_act_costs_[iter].set_path_positions(act_path_positions);
+
+			iter++;
+		  }
+
+		  if( global_mode_ == Cplan_local_nav_person_companion::MO_erf )
+		  {
+
+			  for( unsigned int i : nondominated_end_of_plan_vertex_index_)
+			  {
+					//std::cout<<"[end of branch index] i ="<<nondominated_end_of_plan_vertex_index_[i]<< std::endl;
+
+				cost = cost_parameters_[0]*erf((cost_distance_[i] - mean_cost_distance_) / std_cost_distance_) +
+						cost_parameters_[1]*erf((cost_orientation_[i] - mean_cost_orientation_) / std_cost_orientation_) +
+						cost_parameters_[2]*erf((cost_robot_[i] - mean_cost_robot_) / std_cost_robot_ ) +
+						cost_parameters_[3]*erf((cost_int_forces_[i] - mean_cost_int_forces_) / std_cost_int_forces_ ) +
+						cost_parameters_[5]*erf((cost_obstacles_[i] - mean_cost_obstacles_) / std_cost_obstacles_) +
+						cost_parameters_[6]*erf((cost_past_traj_[i] - mean_cost_past_traj_) / std_cost_past_traj_);
+				if(debug_real_test_companion2_){
+					std::cout<<"[distance] cost_parameters_[0]="<<cost_parameters_[0]<<"; [orientation] cost_parameters_[1]="<<cost_parameters_[1]<<"; [robot control] cost_parameters_[2]="<<cost_parameters_[2]<<"; [persons] cost_parameters_[3]"<<cost_parameters_[3]<<"; [cost obstacles] cost_parameters_[5]"<<cost_parameters_[5]<< "; [coste trajectoria anterior] cost_parameters_[6]="<<cost_parameters_[6]<< std::endl;
+				}
+				//std::cout << "[i] d_cost="<<cost_parameters_[0]*erf((cost_distance_[i] - mean_cost_distance_) / std_cost_distance_)<<"; or_cost="<<cost_parameters_[1]*erf((cost_orientation_[i] - mean_cost_orientation_) / std_cost_orientation_)<<"; traj_cost="<<cost_parameters_[6]*erf((cost_past_traj_[i] - mean_cost_past_traj_) / std_cost_past_traj_)<< std::endl;
+
+				if(debug_real_test_companion2_){
+					std::cout<<"[Cplan_local_nav_person_companion::MO_norm/MO_erf] i ="<<i<< std::endl;
+					std::cout<<"[Cplan_local_nav_person_companion::MO_norm/MO_erf] (GONZALO) cost ="<<cost<< std::endl;
+				}
+
+				double new_cost=cost;
+				//std::cout << "(ant) [i] new_cost="<<new_cost<<"; cost_companion="<< erf((cost_companion_[i] - mean_cost_companion_) / std_cost_companion_)<< std::endl;
+
+				if((cost_companion_[i]!=0)&&(reactive==Cperson_abstract::Akp_planning)){
+
+					//if(!Zanlungo_model_){
+						// caso side-by-side:
+						we_have_cost_companion_=true;
+						new_cost= (0.9)*cost+(0.1)*erf((cost_companion_[i] - mean_cost_companion_) / std_cost_companion_);
+					/*}else{
+						if((pointer_to_person_companion_->get_current_pointV().v()<0.2)&&(second_group_companion_person_obj_->get_current_pointV().v()<0.2)){ // si personas paradas, no tenemos companion cost.
+							// si no se mueven las persnas, no tenemos fuerza companion ni cost companion!
+						}else{
+							new_cost= (0.9)*cost+(0.1)*erf((cost_companion_[i] - mean_cost_companion_) / std_cost_companion_);
+						}
+					}*/
+
+
+				  // NOTA: special values: erf(-0) = -0.00 ; erf(Inf) = 1.00 ;
+					// NOTA: erf() computes the error function of the argument. (Creo k en mi caso es solo el valor del coste y YA esta!!!
+					//std::cout<<"(9/10)*cost="<<(0.9)*cost<< std::endl;
+					//std::cout<<"(1/10)*cost_companion_[i]="<<(0.1)*cost_companion_[i]<< std::endl;
+//					std::cout<<"cost_companion_["<<i<<"]="<<cost_companion_[i]<<"; mean_cost_companion_="<<mean_cost_companion_<<";std_cost_companion_= "<<std_cost_companion_<< std::endl;
+					//std::cout<<"(companion) mean_cost_companion_="<<mean_cost_companion_<< std::endl;
+					//std::cout<<"(companion) std_cost_companion_="<<std_cost_companion_<< std::endl;
+					//std::cout<<"(companion) cost ="<<erf((cost_companion_[i] - mean_cost_companion_) / std_cost_companion_)<< std::endl;
+
+					if(debug_real_test_companion2_){
+						std::cout<<" [Cplan_local_nav_person_companion::MO_norm/MO_erf] eliminado mi coste, momentaneamente. (if in) new_cost="<<new_cost<<"; (companion) cost ="<<erf((cost_companion_[i] - mean_cost_companion_) / std_cost_companion_)<<" (Gonzalo cost)="<<cost<< std::endl;
+						std::cout<<"[Cplan_local_nav_person_companion::MO_norm/MO_erf] cost_companion_[i]="<<cost_companion_[i]<<"; mean_cost_companion_="<<mean_cost_companion_<<"std_cost_companion_="<<std_cost_companion_<< std::endl;
+					}
+
+				}else{
+					if(debug_real_test_companion2_){
+						std::cout<<"NO HAY cost_companion_["<<i<<"]="<<cost_companion_[i]<< std::endl;
+					}
+				}
+
+				//std::cout << "[i] new_cost="<<new_cost<<"; cost_companion="<< erf((cost_companion_[i] - mean_cost_companion_) / std_cost_companion_)<< std::endl;
+
+				cost=new_cost;
+
+				if ( cost < min_cost)
+				{
+					min_cost = cost;
+					min_cost_index = i;
+					// To print results in matlab.
+					//std::cout << "[i] min_cost="<<min_cost<< std::endl;
+					robot_distance_cost_=erf((cost_distance_[i] - mean_cost_distance_) / std_cost_distance_);
+					robot_orientation_cost_=erf((cost_orientation_[i] - mean_cost_orientation_) / std_cost_orientation_);
+					robot_control_cost_=erf((cost_robot_[i] - mean_cost_robot_) / std_cost_robot_ );
+					robot_other_person_cost_=erf((cost_int_forces_[i] - mean_cost_int_forces_) / std_cost_int_forces_ );
+					robot_obstacles_cost_=erf((cost_obstacles_[i] - mean_cost_obstacles_) / std_cost_obstacles_);
+					robot_companion_cost_=erf((cost_companion_[i] - mean_cost_companion_) / std_cost_companion_);
+					robot_total_cost_=cost;
+					robot_ant_traj_cost_=erf((cost_past_traj_[i] - mean_cost_past_traj_) / std_cost_past_traj_);
+
+				}
+			  }
+		  }
+		  else
+		  {
+			for( unsigned int i : nondominated_end_of_plan_vertex_index_ )
+			{
+				//std::cout<<"[end of branch index] i ="<<nondominated_end_of_plan_vertex_index_[i]<< std::endl;
+				cost = cost_parameters_[0]*((cost_distance_[i] - mean_cost_distance_) / std_cost_distance_) +
+						cost_parameters_[1]*((cost_orientation_[i] - mean_cost_orientation_) / std_cost_orientation_) +
+						cost_parameters_[2]*((cost_robot_[i] - mean_cost_robot_) / std_cost_robot_ ) +
+						cost_parameters_[3]*((cost_int_forces_[i] - mean_cost_int_forces_) / std_cost_int_forces_ ) +
+						cost_parameters_[5]*((cost_obstacles_[i] - mean_cost_obstacles_) / std_cost_obstacles_)+
+						cost_parameters_[6]*((cost_past_traj_[i] - mean_cost_past_traj_) / std_cost_past_traj_);
+				if(debug_antes_subgoals_entre_AKP_goals_){
+					std::cout<<"(CASE) Cplan_local_nav_person_companion::MO_norm  "<< std::endl;
+					std::cout<<"(CASE) else global_mode_ = Cplan_local_nav_person_companion::MO_erf "<< std::endl;
+					std::cout<<"i ="<<i<< std::endl;
+					std::cout<<"(GONZALO) cost ="<<cost<< std::endl;
+				}
+				//std::cout<<"; (ant) cost="<<cost<<"; cost_companion="<<(cost_companion_[i]-mean_cost_companion_)/std_cost_companion_<< std::endl;
+				if(cost_companion_[i]!=0){
+					//cost =(9/10)*cost+(1/10)*((cost_companion_[i]-mean_cost_companion_)/std_cost_companion_);
+					//if(!Zanlungo_model_){
+						// caso side-by-side:
+						 we_have_cost_companion_=true;
+						cost =(9/10)*cost+(1/10)*((cost_companion_[i]-mean_cost_companion_)/std_cost_companion_);
+					/*}else{
+							if((pointer_to_person_companion_->get_current_pointV().v()<0.2)&&(second_group_companion_person_obj_->get_current_pointV().v()<0.2)){ // si personas paradas, no tenemos companion cost.
+												// si no se mueven las persnas, no tenemos fuerza companion ni cost companion!
+							}else{
+								cost =(9/10)*cost+(1/10)*((cost_companion_[i]-mean_cost_companion_)/std_cost_companion_);
+							}
+				    }*/
+
+
+
+
+
+					if(debug_antes_subgoals_entre_AKP_goals_){
+						std::cout<<"(NEW) cost_companion_["<<i<<"]="<<cost_companion_[i]<< std::endl;
+						std::cout<<"(NEW) final_cost_companion_="<<(1/10)*((cost_companion_[i]-mean_cost_companion_)/std_cost_companion_)<< std::endl;
+						std::cout<<"(NEW)  cost="<<cost<< std::endl;
+					}
+				}else{
+					//if(debug_antes_subgoals_entre_AKP_goals_){
+						std::cout<<"NO HAY cost_companion_["<<i<<"]="<<cost_companion_[i]<< std::endl;
+					//}
+				}
+
+				//std::cout<<"; cost="<<cost<<"; cost_companion="<<(cost_companion_[i]-mean_cost_companion_)/std_cost_companion_<< std::endl;
+
+				if ( cost < min_cost)
+				{	if(debug_real_test_companion_){
+						std::cout<<"Entro en if(cost < min_cost)"<< std::endl;
+					}
+					min_cost = cost;
+					min_cost_index = i;
+
+					robot_distance_cost_=((cost_distance_[i] - mean_cost_distance_) / std_cost_distance_);
+					robot_orientation_cost_=((cost_orientation_[i] - mean_cost_orientation_) / std_cost_orientation_);
+					robot_control_cost_=((cost_robot_[i] - mean_cost_robot_) / std_cost_robot_ );
+					robot_other_person_cost_=((cost_int_forces_[i] - mean_cost_int_forces_) / std_cost_int_forces_ );
+					robot_obstacles_cost_=((cost_obstacles_[i] - mean_cost_obstacles_) / std_cost_obstacles_);
+					robot_companion_cost_=((cost_companion_[i] - mean_cost_companion_) / std_cost_companion_);
+					robot_total_cost_=cost;
+					robot_ant_traj_cost_=((cost_past_traj_[i] - mean_cost_past_traj_) / std_cost_past_traj_);
+
+				}
+		    }
+
+		  }
+		//  std::cout << "antes iter_act_multiple_paths_and_best_path_.set_nondominated_paths(non_domianted_path_and_act_costs_);"<<non_domianted_path_and_act_costs_.size()<< std::endl;
+		  iter_act_multiple_paths_and_best_path_.set_nondominated_paths(non_domianted_path_and_act_costs_);
+
+		break;
+	  }
+	}
+
+
+	/* INIT fill best_cost in struct  SsavePath_multiple_paths_and_best_path */
+		const std::vector<Spose>* plans_act2 = robot_act->get_robot_planning_trajectory();
+	 	 SsavePath_cost_and_values best_act_path_and_cost_;
+	 	//std::cout << "[Cplan_local_nav_person_companion::MO_norm/MO_erf] plans_act2.empty()="<<plans_act2->empty() << std::endl;
+		  //best_act_path_and_cost_();
+	 	 unsigned int act_best_branch_index=min_cost_index;
+		//nondominated_end_of_plan_vertex_index_.push_back( act_best_branch_index );//only end of branches to plot the branch id
+	 	best_act_path_and_cost_.set_end_id_path(act_best_branch_index);
+		std::vector<unsigned int> act_all_ids_path_positions;
+		std::vector<Spoint> act_path_positions;
+		std::vector<std::vector<double>> iter_vect_costst_act;
+
+		//std::cout << "end Best_BRANCH_INDEX="<<act_best_branch_index<< std::endl;
+
+		 if(!plans_act2->empty()){
+			//	std::cout << "NO plans_act2->empty()"<< std::endl;
+			do
+			{
+				//nondominated_plan_vertex_index_.push_back( act_best_branch_index);
+
+				act_all_ids_path_positions.push_back( act_best_branch_index);
+				act_path_positions.push_back(plans_act2->at(act_best_branch_index));
+
+				std::vector<double> vect_costst_act;
+
+				vect_costst_act.push_back(cost_parameters_[0]*erf((cost_distance_[act_best_branch_index] - mean_cost_distance_) / std_cost_distance_));
+				vect_costst_act.push_back(cost_parameters_[1]*erf((cost_orientation_[act_best_branch_index] - mean_cost_orientation_) / std_cost_orientation_));
+				vect_costst_act.push_back(cost_parameters_[2]*erf((cost_robot_[act_best_branch_index] - mean_cost_robot_) / std_cost_robot_ ));
+				vect_costst_act.push_back(cost_parameters_[3]*erf((cost_int_forces_[act_best_branch_index] - mean_cost_int_forces_) / std_cost_int_forces_ ));
+				vect_costst_act.push_back(cost_parameters_[5]*erf((cost_obstacles_[act_best_branch_index] - mean_cost_obstacles_) / std_cost_obstacles_));
+				vect_costst_act.push_back(erf((cost_companion_[act_best_branch_index] - mean_cost_companion_) / std_cost_companion_));
+				vect_costst_act.push_back(cost_parameters_[6]*erf((cost_past_traj_[act_best_branch_index] - mean_cost_past_traj_) / std_cost_past_traj_));
+				cost = cost_parameters_[0]*erf((cost_distance_[act_best_branch_index] - mean_cost_distance_) / std_cost_distance_) +
+												cost_parameters_[1]*erf((cost_orientation_[act_best_branch_index] - mean_cost_orientation_) / std_cost_orientation_) +
+												cost_parameters_[2]*erf((cost_robot_[act_best_branch_index] - mean_cost_robot_) / std_cost_robot_ ) +
+												cost_parameters_[3]*erf((cost_int_forces_[act_best_branch_index] - mean_cost_int_forces_) / std_cost_int_forces_ ) +
+												cost_parameters_[5]*erf((cost_obstacles_[act_best_branch_index] - mean_cost_obstacles_) / std_cost_obstacles_) +
+												cost_parameters_[6]*erf((cost_past_traj_[act_best_branch_index] - mean_cost_past_traj_) / std_cost_past_traj_);
+
+				double new_cost=cost;
+				//std::cout << "(ant) [act_best_branch_index] new_cost="<<new_cost<<"; cost_companion="<<erf((cost_companion_[act_best_branch_index] - mean_cost_companion_) / std_cost_companion_) << std::endl;
+
+				if((cost_companion_[act_best_branch_index]!=0)&&(reactive==Cperson_abstract::Akp_planning)){
+					//new_cost= (0.9)*cost+(0.1)*erf((cost_companion_[act_best_branch_index] - mean_cost_companion_) / std_cost_companion_);
+					//if(!Zanlungo_model_){
+						// case side-by-side:
+						we_have_cost_companion_=true;
+						new_cost= (0.9)*cost+(0.1)*erf((cost_companion_[act_best_branch_index] - mean_cost_companion_) / std_cost_companion_);
+					/*}else{
+						if((pointer_to_person_companion_->get_current_pointV().v()<0.2)&&(second_group_companion_person_obj_->get_current_pointV().v()<0.2)){ // si personas paradas, no tenemos companion cost.
+																	// si no se mueven las persnas, no tenemos fuerza companion ni cost companion!
+						}else{
+							new_cost= (0.9)*cost+(0.1)*erf((cost_companion_[act_best_branch_index] - mean_cost_companion_) / std_cost_companion_);
+						}
+					}*/
+
+				}
+				//std::cout << "[act_best_branch_index] new_cost="<<new_cost<<"; cost_companion="<<erf((cost_companion_[act_best_branch_index] - mean_cost_companion_) / std_cost_companion_) << std::endl;
+
+				vect_costst_act.push_back(new_cost);
+				iter_vect_costst_act.push_back(vect_costst_act);
+				act_best_branch_index = edge_[ act_best_branch_index ].parent;
+
+			}while( act_best_branch_index > 0 );
+
+			best_act_path_and_cost_.set_total_costs_of_each_path_position(iter_vect_costst_act);
+			best_act_path_and_cost_.set_all_ids_path_positions(act_all_ids_path_positions);
+			best_act_path_and_cost_.set_path_positions(act_path_positions);
+			iter_act_multiple_paths_and_best_path_.set_best_path(best_act_path_and_cost_);
+
+			//std::cout << "out NO plans_act2->empty()"<< std::endl;
+		 }
+
+		// std::cout << "out NO plans_act2->empty() part2"<< std::endl;
+	/* FIN fill best_cost in struct SsavePath_multiple_paths_and_best_path */
+	if(!plans_act2->empty()){
+		// std::cout << "1"<< std::endl;
+		iter_act_multiple_paths_and_best_path_.set_number_of_total_paths(1 + nondominated_end_of_plan_vertex_index_.size() );
+		// std::cout << "2"<< std::endl;
+		Cperson_abstract* person_obj;
+		//bool finded_person=find_person(id_person_companion_ , &person_obj);
+		find_person(id_person_companion_ , &person_obj);
+		 //std::cout << "3"<< std::endl;
+		double time=0.2;//person_obj->get_time(); //TODO: cambiado a 0.2, porque salian tiempos enormes y parece que iba super lento!!!
+		// std::cout << "4"<< std::endl;
+		iter_act_multiple_paths_and_best_path_.set_time_act(time);
+		// std::cout << "5"<< std::endl;
+		iter_act_multiple_paths_and_best_path_.set_iter_act(planner_iterations_);
+	}
+	// std::cout << "6"<< std::endl;
+	//fill best cost
+	best_costs_.resize( 7, 0.0 );
+	best_costs_[0] = cost_distance_[min_cost_index];
+	best_costs_[1] = cost_orientation_[min_cost_index];
+	best_costs_[2] = cost_robot_[min_cost_index];
+	best_costs_[3] = cost_int_forces_[min_cost_index];
+	best_costs_[4] = cost_obstacles_[min_cost_index];
+	best_costs_[5] = cost_companion_[min_cost_index];
+	best_costs_[6] = cost_past_traj_[min_cost_index];
+
+	if(debug_real_test_companion_){
+		std::cout<<"(INI) min_cost="<<min_cost<< std::endl;
+		std::cout<<"min_cost_index="<<min_cost_index<< std::endl;
+		std::cout<<"(FIN) orientation_person_robot_angles_["<<min_cost_index<<"]="<<orientation_person_robot_angles_[min_cost_index]<< std::endl;
+		std::cout<<"(FIN) orientation_person_robot_angles_with_prediction_of_person_companion_["<<min_cost_index<<"]="<<orientation_person_robot_angles_with_prediction_of_person_companion_[min_cost_index]<< std::endl;
+
+	}
+/*	std::cout<<"best_costs_[0] = cost_distance_   [min_cost_index="<<min_cost_index<<"] ="<<cost_distance_[min_cost_index]<< std::endl;
+	std::cout<<"best_costs_[1] = cost_orientation_[min_cost_index="<<min_cost_index<<"] ="<<cost_orientation_[min_cost_index]<< std::endl;
+	std::cout<<"best_costs_[2] = cost_robot_      [min_cost_index="<<min_cost_index<<"] ="<<cost_robot_[min_cost_index]<< std::endl;
+	std::cout<<"best_costs_[3] = cost_int_forces_ [min_cost_index="<<min_cost_index<<"] ="<<cost_int_forces_[min_cost_index]<< std::endl;
+	std::cout<<"best_costs_[4] = cost_obstacles_  [min_cost_index="<<min_cost_index<<"] ="<<cost_obstacles_[min_cost_index]<< std::endl;
+*/
+	//fill mean costs
+	mean_costs_.resize( 7, 0.0 );
+	mean_costs_[0] = mean_cost_distance_;
+	mean_costs_[1] = mean_cost_orientation_;
+	mean_costs_[2] = mean_cost_robot_;
+	mean_costs_[3] = mean_cost_int_forces_;
+	mean_costs_[4] = mean_cost_obstacles_;
+	mean_costs_[5] = mean_cost_companion_;
+	mean_costs_[6] = mean_cost_past_traj_;
+
+	 if(!plans_act2->empty()){
+		 iter_act_multiple_paths_and_best_path_.set_means(mean_costs_);
+	 }
+/*  std::cout<<"mean_costs_[0] = mean_cost_distance_    ="<<mean_cost_distance_<< std::endl;
+	std::cout<<"mean_costs_[1] = mean_cost_orientation_ ="<<mean_cost_orientation_<< std::endl;
+	std::cout<<"mean_costs_[2] = mean_cost_robot_       ="<<mean_cost_robot_<< std::endl;
+	std::cout<<"mean_costs_[3] = mean_cost_int_forces_  ="<<mean_cost_int_forces_<< std::endl;
+	std::cout<<"mean_costs_[4] = mean_cost_obstacles_   ="<<mean_cost_obstacles_<< std::endl;
+*/
+
+	//fill std costs
+	std_costs_.resize( 7, 0.0 );
+	std_costs_[0] = std_cost_distance_;
+	std_costs_[1] = std_cost_orientation_;
+	std_costs_[2] = std_cost_robot_;
+	std_costs_[3] = std_cost_int_forces_;
+	std_costs_[4] = std_cost_obstacles_;
+	std_costs_[5] = std_cost_companion_;
+	std_costs_[6] = std_cost_past_traj_;
+
+	 if(!plans_act2->empty()){
+		 iter_act_multiple_paths_and_best_path_.set_stds(std_costs_);
+	 }
+/*  std::cout<<"std_costs_[0] = std_cost_distance_    ="<<std_cost_distance_<< std::endl;
+	std::cout<<"std_costs_[1] = std_cost_orientation_ ="<<std_cost_orientation_<< std::endl;
+	std::cout<<"std_costs_[2] = std_cost_robot_       ="<<std_cost_robot_<< std::endl;
+	std::cout<<"std_costs_[3] = std_cost_int_forces_  ="<<std_cost_int_forces_<< std::endl;
+	std::cout<<"std_costs_[4] = std_cost_obstacles_   ="<<std_cost_obstacles_<< std::endl;
+*/
+	//std::cout<<" FIN function: global_min_cost_index(); min_cost_index="<<min_cost_index << std::endl;
+
+	// iter_act_multiple_paths_and_best_path_.print();
+
+	return min_cost_index;
+}
+
+Sforce Cplan_local_nav_person_companion::force_persons_int_planning_virtual(Cperson_abstract* center , unsigned int t, double min_dist2, Cperson_abstract::companion_reactive reactive)
+{
+
+	//std::cout<<" IN force_persons_int_planning_virtual" << std::endl;
+
+	Sforce force_res;
+
+	switch(reactive)
+	{
+	case Cperson_abstract::Reactiva_repulsive:
+	case Cperson_abstract::Reactive_atractive:
+		//std::cout << " IN :force_persons_int_planning_virtual case(Reactiva_repulsive+atractive) " <<  std::endl;
+		for( auto iit : nearby_person_list_)
+		{
+
+			if( *center != *iit && center->get_planning_trajectory()->at(t)
+						.distance2( iit->get_planning_trajectory( )->at(t) ) < min_dist2 )
+			{
+				//if((iit->get_id()!=id_person_companion_)||(iit->get_id()!=id_SECOND_person_companion_)){ // para el id companion esta fuerza ha de ser 0!
+				if(iit->get_id()!=id_person_companion_){ //&&(iit->get_id()!=id_SECOND_person_companion_) para el id companion esta fuerza ha de ser 0!
+
+
+
+						Sforce force_act=center->force( iit->get_planning_trajectory( )->at(t) ,  get_sfm_int_params(center,iit),
+								&(center->get_planning_trajectory()->at(t) ) );
+						force_res += force_act;
+						if(((force_act.fx>0.5)||(force_act.fy>0.5))&&see_forces_){
+
+							//std::cout<<"person_id=iit->get_id()=" <<iit->get_id()<< "; force_act.fx ="<<force_act.fx<< "; force_act.fy="<<force_act.fy <<  std::endl;
+						}
+
+				}else{ // CASO: person companion!!!
+
+					//std::vector<double> params_interact_with_person_companion=get_sfm_int_params(center,iit);
+					//if(center->get_planning_trajectory()->at(t).distance2( iit->get_planning_trajectory( )->at(t))<1.25){
+					//	std::cout<<"params_interact_with_person_companion(1)=" <<get_sfm_int_params(center,iit)->at(1)<< "; params_interact_with_person_companion(2) ="<<get_sfm_int_params(center,iit)->at(2)<< "; params_interact_with_person_companion(3)="<<get_sfm_int_params(center,iit)->at(3) <<  std::endl;
+					//	std::cout<<"params_interact_with_person_companion(4)=" <<get_sfm_int_params(center,iit)->at(4) <<  std::endl;
+					//}
+					//std::cout << " IN :force person companion " <<  std::endl;
+
+
+					//if(!Zanlungo_model_){
+						// ini Case side-by-side.
+						/*if(Action_==FACEPERSON){
+							const std::vector<double>* act_robot_params=get_sfm_int_params(center,iit);
+												//std::cout << "(Action_==FACEPERSON) act_robot_params[0]="<< act_robot_params->at(0)<<"; act_robot_params[1]="<<act_robot_params->at(1)<<"; act_robot_params[2]="<<act_robot_params->at(2)<<"; act_robot_params[3]="<<act_robot_params->at(3)<<"; act_robot_params[4]="<<act_robot_params->at(4)<<  std::endl;
+
+							std::vector<double> act_robot_params2;
+							act_robot_params2.push_back(act_robot_params->at(0));
+							act_robot_params2.push_back(act_robot_params->at(1));
+							//act_robot_params2.push_back(new_A_force_face_person_);//act_robot_params->at(2));
+							//act_robot_params2.push_back(new_B_force_face_person_);//act_robot_params->at(3)+0.3);
+							act_robot_params2.push_back(act_robot_params->at(4));
+
+							//std::cout << "(Action_==FACEPERSON) act_robot_params2[0]="<< act_robot_params2[0]<<"; act_robot_params2[1]="<<act_robot_params2[1]<<"; act_robot_params2[2]="<<act_robot_params2[2]<<"; act_robot_params2[3]="<<act_robot_params2[3]<<"; act_robot_params[4]="<<act_robot_params2[4]<<  std::endl;
+
+												//Sforce force_act=center->force( iit->get_planning_trajectory( )->at(t) ,  get_sfm_int_params(center,iit),
+														//&(center->get_planning_trajectory()->at(t) ) , reactive);
+							Sforce force_act=center->force( iit->get_planning_trajectory( )->at(t) ,  &act_robot_params2,
+											&(center->get_planning_trajectory()->at(t) ) , reactive);
+
+							//force_act.fx=2*force_act.fx;
+							//force_act.fy=2*force_act.fy;
+							force_res += force_act;
+						}else{*/
+							//Sforce force_act=center->force( iit->get_planning_trajectory( )->at(t) ,  get_sfm_int_params(center,iit),
+							//		&(center->get_planning_trajectory()->at(t) ) , reactive);
+
+							force_res += Sforce();  // NUNCA la ha de considerar en el path, excepto en el case face person!!!
+						//}
+
+						//if(((force_act.fx>0.5)||(force_act.fy>0.5))&&see_forces_){
+						//	std::cout<<"person_id=iit->get_id()=" <<iit->get_id()<< "; force_act.fx ="<<force_act.fx<< "; force_act.fy="<<force_act.fy <<  std::endl;
+						//}
+						// fin Case side-by-side.
+					/*}else{
+						// todo: reducir!
+						//std::cout << " caso Zanlungo atractive and repulsive (with force) " <<  std::endl;
+
+						Sforce force_act=center->force( iit->get_planning_trajectory( )->at(t) ,  get_sfm_int_params(center,iit),
+								&(center->get_planning_trajectory()->at(t) ) );
+						force_res += force_act;
+
+						if(iit->get_id()==id_SECOND_person_companion_){
+							person2_repulsive_force_fx_act_=force_act.fx;
+							person2_repulsive_force_fy_act_=force_act.fy;
+						}else{
+							person2_repulsive_force_fx_act_=-1.0;
+							person2_repulsive_force_fy_act_=-1.0;
+						}
+
+
+						if(iit->get_id()==id_person_companion_){
+							person1_repulsive_force_fx_act_=force_act.fx;
+							person1_repulsive_force_fy_act_=force_act.fy;
+						}else{
+							person1_repulsive_force_fx_act_=-1.0;
+							person1_repulsive_force_fy_act_=-1.0;
+						}
+
+					}*/
+
+
+				}
+
+			}
+		}
+	break;
+
+	case Cperson_abstract::Akp_planning:
+	default:
+		//std::cout << " IN force_persons_int_planning_virtual: case Akp_planning " <<  std::endl;
+		for( auto iit : nearby_person_list_)
+		{
+			//std::cout << " IN :force_persons_int_planning_virtual case: Akp_planning, salta person companion, id="<<iit->get_id()<<  std::endl;
+
+			if( *center != *iit && center->get_planning_trajectory()->at(t)
+						.distance2( iit->get_planning_trajectory( )->at(t) ) < min_dist2 )
+			{
+
+
+				/// INI Add go to person goal. Do not (tratar) the person that is the goal of the group as a repulsive.
+					//if((iit->get_id()!=id_person_companion_)||(iit->get_id()!=id_SECOND_person_companion_)){ // para el id companion esta fuerza ha de ser 0!
+					if(iit->get_id()!=id_person_companion_){ //&&(iit->get_id()!=id_SECOND_person_companion_) para el id companion esta fuerza ha de ser 0!
+						Sforce force_act=center->force( iit->get_planning_trajectory( )->at(t) ,  get_sfm_int_params(center,iit),
+								&(center->get_planning_trajectory()->at(t) ) );
+						force_res += force_act;
+
+						//std::cout << " ( BAD case id, comp pers or 2 comp pers) IN :force_persons_int_planning_virtual case: Akp_planning, person id="<<iit->get_id()<<  std::endl;
+						if(debug_antes_subgoals_entre_AKP_goals_){
+							//force_res.print();
+						}
+
+						if(((force_act.fx>0.5)||(force_act.fy>0.5))&&see_forces_){
+							//std::cout<<"person_id=iit->get_id()=" <<iit->get_id()<< "; force_act.fx ="<<force_act.fx<< "; force_act.fy="<<force_act.fy <<  std::endl;
+						}
+					}else{ // para id_person_companion
+						//std::cout << " IN :force_persons_int_planning_virtual case: Akp_planning, salta person companion, id="<<iit->get_id()<<  std::endl;
+
+						//if(!Zanlungo_model_){
+							// ini case side-by-side:
+							Sforce force_act_companion_person;
+
+							//std::cout << " (OK) IN :force_persons_int_planning_virtual case: before => force_act_companion_person, id="<<iit->get_id()<<  std::endl;
+
+							force_act_companion_person=center->force( iit->get_planning_trajectory( )->at(t) ,  get_sfm_int_params(center,iit),
+									&(center->get_planning_trajectory()->at(t) ) );
+
+							if(debug_antes_subgoals_entre_AKP_goals_){
+								force_act_companion_person.print();
+							}
+							//force_act_companion_person.fx=force_act_companion_person.fx/2; En el caso de ir a la person target goal, va bien sin el partir por 2, así que en solo companion ha de ir igual de bien!
+							//force_act_companion_person.fy=force_act_companion_person.fy/2;
+							if(debug_antes_subgoals_entre_AKP_goals_){
+								force_act_companion_person.print();
+							}
+
+							//if(((force_act_companion_person.fx>0.5)||(force_act_companion_person.fy>0.5))&&see_forces_){
+							//	std::cout<<"person_id=iit->get_id()=" <<iit->get_id()<< "; force_act_companion_person.fx ="<<force_act_companion_person.fx<< "; force_act_companion_person.fy="<<force_act_companion_person.fy <<  std::endl;
+							//}
+
+							force_res+=Sforce();
+							//force_res+=force_act_companion_person; // is commented, because for the path we need NO force respect person companion to have a path down of it in the situations where the robot is rear of the person.
+							//force_res.print();
+							// fin case side-by-side:
+						/*}else{
+							//std::cout << "; id="<<iit->get_id()<<"; iit->get_person_type()="<<iit->get_person_type()<<std::endl;
+							Sforce force_act=center->force( iit->get_planning_trajectory( )->at(t) ,  get_sfm_int_params(center,iit),
+									&(center->get_planning_trajectory()->at(t) ) );;
+							force_res += force_act;
+							if(iit->get_id()==id_SECOND_person_companion_){
+								person2_repulsive_force_fx_act_=force_act.fx;
+								person2_repulsive_force_fy_act_=force_act.fy;
+							}else{
+								person2_repulsive_force_fx_act_=-1.0;
+								person2_repulsive_force_fy_act_=-1.0;
+							}
+
+
+							if(iit->get_id()==id_person_companion_){
+								person1_repulsive_force_fx_act_=force_act.fx;
+								person1_repulsive_force_fy_act_=force_act.fy;
+							}else{
+								person1_repulsive_force_fx_act_=-1.0;
+								person1_repulsive_force_fy_act_=-1.0;
+							}
+
+						}*/
+
+					}
+					//std::cout << "; id="<<iit->get_id()<<"; force_res.fx="<<force_res.fx<<"; force_res.fy="<<force_res.fy<<std::endl;
+
+			}
+		}
+	break;
+	}
+
+	//std::cout<<" out force_persons_int_planning_virtual" << std::endl;
+
+	return force_res;
+}
+
+
+
+Sforce Cplan_local_nav_person_companion::force_persons_int_planning_virtual_robot_prediction(Cperson_abstract* center , unsigned int t, double min_dist2, Cperson_abstract::companion_reactive reactive, unsigned int id_pers_comp_rob)
+{
+
+	//std::cout << " (1)  force_persons_int_planning_virtual_robot_prediction " <<  std::endl;
+
+
+
+	Sforce force_res;
+
+	switch(reactive)
+	{
+	case Cperson_abstract::Reactiva_repulsive:
+	case Cperson_abstract::Reactive_atractive:
+		//std::cout << " IN :force_persons_int_planning_virtual case(Reactiva_repulsive+atractive) "<<  std::endl;
+		for( auto iit : person_list_)
+		{
+			//std::cout << " IN : iit->get_prediction_trajectory()->size(): "<<iit->get_prediction_trajectory()->size() << "; id="<<iit->get_id()  <<  std::endl;
+
+			if(id_pers_comp_rob!=iit->get_id()){
+				//std::cout << " IN : iit->get_prediction_trajectory()->size(): "<<iit->get_prediction_trajectory()->size() <<", id_pers_comp_rob="<<id_pers_comp_rob<<  std::endl;
+
+				if( *center != *iit && center->get_prediction_trajectory_with_target_person()->at(t)
+							.distance2( iit->get_prediction_trajectory( )->at(t) ) < min_dist2 )
+				{
+					if(iit->get_id()!=id_person_companion_){ // para el id companion esta fuerza ha de ser 0!
+						Sforce force_act=center->force( iit->get_prediction_trajectory( )->at(t) ,  get_sfm_int_params(center,iit),
+								&(center->get_prediction_trajectory_with_target_person()->at(t) ) );;
+						force_res += force_act;
+						//if(((force_act.fx>0.5)||(force_act.fy>0.5))&&see_forces_){
+
+							//std::cout<<"person_id=iit->get_id()=" <<iit->get_id()<< "; force_act.fx ="<<force_act.fx<< "; force_act.fy="<<force_act.fy <<  std::endl;
+						//}
+
+					}else{ // para id person companion!
+						//std::cout << " IN :force person companion " <<  std::endl;
+						Sforce force_act=center->force( iit->get_prediction_trajectory( )->at(t) ,  get_sfm_int_params(center,iit),
+								&(center->get_prediction_trajectory_with_target_person()->at(t) ) , reactive);
+						force_res += force_act;
+
+						//if(((force_act.fx>0.5)||(force_act.fy>0.5))&&see_forces_){
+						//	std::cout<<"person_id=iit->get_id()=" <<iit->get_id()<< "; force_act.fx ="<<force_act.fx<< "; force_act.fy="<<force_act.fy <<  std::endl;
+						//}
+
+					}
+
+				}
+
+
+			}
+
+		}
+	break;
+
+	case Cperson_abstract::Akp_planning:
+	default:
+		//std::cout << " (2)  force_persons_int_planning_virtual_robot_prediction (Akp_planning)" <<  std::endl;
+
+		for( auto iit : person_list_)
+		{
+			//std::cout << " IN : Akp_planning id="<<iit->get_id() <<"; id_pers_comp_rob="<<id_pers_comp_rob <<  std::endl;
+			if(id_pers_comp_rob!=iit->get_id()){
+
+				//std::cout << " 2 IN : iit->get_prediction_trajectory()->size(): "<<iit->get_prediction_trajectory()->size() <<", id_pers_comp_rob="<<id_pers_comp_rob<<  std::endl;
+				//std::cout << " (3)  force_persons_int_planning_virtual_robot_prediction (Akp_planning) t="<<t <<"; center->get_prediction_trajectory_with_target_person().size()="<<center->get_prediction_trajectory_with_target_person()->size()<<"; iit->get_planning_trajectory( ).size()="<<iit->get_planning_trajectory( )->size()<<  std::endl;
+				//std::cout << " (3)  force_persons_int_planning_virtual_robot_prediction (Akp_planning) t="<<t <<"; iit->get_prediction_trajectory()( ).size()="<<iit->get_prediction_trajectory()->size()<<  std::endl;
+				// solucion provisional para inicializacion simulador.
+
+				if((center->get_prediction_trajectory_with_target_person()->size()>t)&&(iit->get_prediction_trajectory( )->size()>t)){
+					if(bool_tes_prop1){
+						std::cout << " IN :force_persons_int_planning_virtual case: center->get_prediction_trajectory_with_target_person()->size()="<<center->get_prediction_trajectory_with_target_person()->size()<<std::endl;
+						std::cout <<	" id. iit="<<iit->get_id()<<"; iit->get_prediction_trajectory( )->size()="<<iit->get_prediction_trajectory( )->size()<<  std::endl;
+
+					}
+
+				if( *center != *iit && center->get_prediction_trajectory_with_target_person()->at(t)
+							.distance2( iit->get_prediction_trajectory( )->at(t) ) < min_dist2 )
+				{
+
+					//std::cout << " (4)  force_persons_int_planning_virtual_robot_prediction (Akp_planning); id_person_companion_="<<id_person_companion_<<"; id_person_goal_"<<id_person_goal_ <<  std::endl;
+					/// INI Add go to person goal. Do not (tratar) the person that is the goal of the group as a repulsive.
+
+
+						//std::cout << " (13)  force_persons_int_planning_virtual_robot_prediction (Akp_planning)" <<  std::endl;
+						if(iit->get_id()!=id_person_companion_){ // para el id companion esta fuerza ha de ser 0!
+							Sforce force_act=center->force( iit->get_prediction_trajectory( )->at(t) ,  get_sfm_int_params(center,iit),
+									&(center->get_prediction_trajectory_with_target_person()->at(t) ) );
+							force_res += force_act;
+							//std::cout << " (14)  force_persons_int_planning_virtual_robot_prediction (Akp_planning)" <<  std::endl;
+							//std::cout << " IN :force_persons_int_planning_virtual case: Akp_planning, person id="<<iit->get_id()<<  std::endl;
+							if(debug_antes_subgoals_entre_AKP_goals_){
+								//force_res.print();
+							}
+							//std::cout << " (15)  force_persons_int_planning_virtual_robot_prediction (Akp_planning)" <<  std::endl;
+							//if(((force_act.fx>0.5)||(force_act.fy>0.5))&&see_forces_){
+							//	std::cout<<"person_id=iit->get_id()=" <<iit->get_id()<< "; force_act.fx ="<<force_act.fx<< "; force_act.fy="<<force_act.fy <<  std::endl;
+							//}
+						}else{
+							//std::cout << "(16.1) IN :force_persons_int_planning_virtual case: Akp_planning, salta person companion, id="<<iit->get_id()<<  std::endl;
+							Sforce force_act_companion_person;
+							force_act_companion_person=center->force( iit->get_prediction_trajectory( )->at(t) ,  get_sfm_int_params(center,iit),
+									&(center->get_prediction_trajectory_with_target_person()->at(t) ) );
+							//std::cout << " (16)  force_persons_int_planning_virtual_robot_prediction (Akp_planning)" <<  std::endl;
+							if(debug_antes_subgoals_entre_AKP_goals_){
+								force_act_companion_person.print();
+							}
+							//std::cout << " (17)  force_persons_int_planning_virtual_robot_prediction (Akp_planning)" <<  std::endl;
+							force_act_companion_person.fx=force_act_companion_person.fx/2;
+							force_act_companion_person.fy=force_act_companion_person.fy/2;
+							//std::cout << " (18)  force_persons_int_planning_virtual_robot_prediction (Akp_planning)" <<  std::endl;
+							if(debug_antes_subgoals_entre_AKP_goals_){
+								force_act_companion_person.print();
+							}
+
+							//if(((force_act_companion_person.fx>0.5)||(force_act_companion_person.fy>0.5))&&see_forces_){
+							//	std::cout<<"person_id=iit->get_id()=" <<iit->get_id()<< "; force_act_companion_person.fx ="<<force_act_companion_person.fx<< "; force_act_companion_person.fy="<<force_act_companion_person.fy <<  std::endl;
+							//}
+							//std::cout << " (19)  force_persons_int_planning_virtual_robot_prediction (Akp_planning)" <<  std::endl;
+							//force_res+=Sforce();
+								//force_res+=force_act_companion_person;
+							//force_res.print();
+						}
+
+
+					//}
+
+				}
+			}
+			}
+
+		}
+	break;
+	}
+
+	//std::cout << " (20)  force_persons_int_planning_virtual_robot_prediction (Akp_planning)" <<  std::endl;
+	return force_res;
+}
+
+
+Sforce Cplan_local_nav_person_companion::force_persons_int_planning_virtual_companion_person_akp(Cperson_abstract* center , unsigned int t, double min_dist2, Cperson_abstract::companion_reactive reactive)
+{
+	Sforce force_res;
+	//std::cout << " (IN)  force_persons_int_planning_virtual_companion_person_akp nearby_person_list_.size()="<<nearby_person_list_.size() <<  std::endl;
+
+	for( auto iit : nearby_person_list_)
+	//for( auto iit : person_list_)
+	{
+		//std::cout << " (IN)  center->get_person_type()="<<center->get_person_type()<<"; iit->get_person_type()"<<iit->get_person_type() <<  std::endl;
+
+		//std::cout << " (1)  force_persons_int_planning_virtual_companion_person_akp " <<  std::endl;
+		if( *center != *iit && center->get_planning_trajectory()->at(t)
+						.distance2( iit->get_planning_trajectory( )->at(t) ) < min_dist2 )
+		{
+			//std::cout << " (IN)  center->get_person_type()="<<center->get_person_type()<<"; iit->get_person_type()"<<iit->get_person_type() <<  std::endl;
+			//if(iit->get_id()!=id_person_companion_){ // para el id companion esta fuerza ha de ser 0!
+			if(iit->get_id()!=id_person_companion_){ // &&(iit->get_id()!=id_SECOND_person_companion_) &&(iit->get_id()!=id_person_goal_)
+			//if((iit->get_id()!=my_id_person_companion_simulation_)){
+
+				Sforce force_act=center->force( iit->get_planning_trajectory( )->at(t) ,  get_sfm_int_params(center,iit),
+							&(center->get_planning_trajectory()->at(t) ) )*multiply_person_companion_force_to_persons_;
+
+				//std::cout<<"person_id=iit->get_id()=" <<iit->get_id()<< "; force_act.fx ="<<force_act.fx<< "; force_act.fy="<<force_act.fy <<  std::endl;
+
+				force_act.fx=force_act.fx; // todo: solucion rapida para incrementar algo la fuerza para que la person companion no pase tan cerca de las otras personas que casi se rozan y eso dificulta la recuperacion de la posicion del robot.
+				force_act.fy=force_act.fy;
+				//std::cout<<"person_id=iit->get_id()=" <<iit->get_id()<< "; force_act.fx ="<<force_act.fx<< "; force_act.fy="<<force_act.fy <<  std::endl;
+
+				force_res += force_act;
+				//std::cout<<"person_id=iit->get_id()=" <<iit->get_id()<< "; force_act.fx ="<<force_act.fx<< "; force_act.fy="<<force_act.fy <<  std::endl;
+				//std::cout << " IN :force_persons_int_planning_virtual case: Akp_planning, person id="<<iit->get_id()<<  std::endl;
+
+				if(debug_antes_subgoals_entre_AKP_goals_){
+					//force_res.print();
+				}
+
+				//if(((force_act.fx>0.5)||(force_act.fy>0.5))&&see_forces_){
+				//	std::cout<<"person_id=iit->get_id()=" <<iit->get_id()<< "; force_act.fx ="<<force_act.fx<< "; force_act.fy="<<force_act.fy <<  std::endl;
+				//}
+			}else{ //if(iit->get_id()==id_SECOND_person_companion_){ // TODO: do the force between the robot and the person companion here, the companion force.
+				/*Sforce force_act=center->force( iit->get_planning_trajectory( )->at(t) ,  get_sfm_int_params(center,iit),
+											&(center->get_planning_trajectory()->at(t) ) )*multiply_person_companion_force_to_persons_;
+				force_res += force_act;*/
+				if(iit->get_id()!=my_id_person_companion_simulation_){ //&&(iit->get_id()!=id_SECOND_person_companion_) con la otra person companion he de tener fuerza repulsiva, pero menos que con las demás personas.
+					// he de usar la fuerza repulsiva pero de companion!!! //TODO:si uso dos robots como companions hay que quitar esta fuerza repulsiva. Pero habra que reducirla, porque esta fuerza repulsiva es demasiado grande para que esten en la posicion ideal!
+					//const std::vector<double>* social_forces_param=get_sfm_int_params(center,iit);
+
+					//std::cout<<" [IMPORTANTE!!!] center.type=" <<center->get_person_type()<< "; iit->get_person_type()="<<iit->get_person_type()<<"; my_id_person_companion_simulation_="<<my_id_person_companion_simulation_<<"; iit->get_id()="<<iit->get_id() <<  std::endl;
+
+
+					//std::cout<<" [IMPORTANTE!!!] social_forces_param.at(0)=" <<social_forces_param->at(0)<< "; social_forces_param.at(1)="<<social_forces_param->at(1)<< "; social_forces_param->at(2)="<<social_forces_param->at(2)<<"; social_forces_param->at(3)="<<social_forces_param->at(3) <<  std::endl;
+
+
+					Sforce force_act=center->force( iit->get_planning_trajectory( )->at(t) ,  get_sfm_int_params(center,iit),
+												&(center->get_planning_trajectory()->at(t) ) )*multiply_person_companion_force_to_persons_;
+
+					//std::cout<<"person_id=iit->get_id()=" <<iit->get_id()<< "; force_act.fx ="<<force_act.fx<< "; force_act.fy="<<force_act.fy <<  std::endl;
+
+					force_act.fx=force_act.fx; // todo: solucion rapida para incrementar algo la fuerza para que la person companion no pase tan cerca de las otras personas que casi se rozan y eso dificulta la recuperacion de la posicion del robot.
+					force_act.fy=force_act.fy;
+					//std::cout<<"person_id=iit->get_id()=" <<iit->get_id()<< "; force_act.fx ="<<force_act.fx<< "; force_act.fy="<<force_act.fy <<  std::endl;
+
+					force_res += force_act;
+
+				}
+
+
+			}
+		}
+	}
+	//std::cout << " (OUT)  force_persons_int_planning_virtual_companion_person_akp " <<  std::endl;
+	return force_res;
+}
+
+Sforce Cplan_local_nav_person_companion::force_persons_int_planning_virtual_companion_person_akp_person_prediction(Cperson_abstract* center , unsigned int t, double min_dist2, Cperson_abstract::companion_reactive reactive)
+{
+	Sforce force_res;
+
+	//std::cout << " (INT PERSON FORCES) (1) iit->get_prediction_trajectory( )->size()="<<  std::endl;
+
+//for( auto iit : nearby_person_list_)
+	for( auto iit : person_list_)
+	{
+			if(iit->get_id()!=id_person_companion_){
+			//std::cout << " (INT PERSON FORCES) (1) iit->get_prediction_trajectory( )->size()="<<iit->get_prediction_trajectory( )->size()<<"; iit->get_id()=" <<iit->get_id()<<  std::endl;
+			//std::cout << " (INT PERSON FORCES) (1) iit->get_prediction_trajectory_with_target_person( )->size()="<<center->get_prediction_trajectory_with_target_person( )->size()<<"; t=" <<t<<  std::endl;
+
+			if( *center != *iit && center->get_prediction_trajectory_with_target_person()->at(t)
+							.distance2( iit->get_prediction_trajectory( )->at(t) ) < min_dist2 )
+			{
+
+				//std::cout << " (INT PERSON FORCES) (2) "<<  std::endl;
+
+				//std::cout << " (2)  force_persons_int_planning_virtual_companion_person_akp " <<  std::endl;
+				//if((iit->get_id()!=id_person_companion_)&&(iit->get_id()!=id_person_goal_)){ // para el id companion esta fuerza ha de ser 0!
+					//std::cout << " (INT PERSON FORCES) (3) "<<  std::endl;
+					//std::cout << " (3)  force_persons_int_planning_virtual_companion_person_akp " <<  std::endl;
+					Sforce force_act=center->force( iit->get_prediction_trajectory( )->at(t) ,  get_sfm_int_params(center,iit),
+								&(center->get_prediction_trajectory_with_target_person()->at(t) ) )*multiply_person_companion_force_to_persons_;
+					force_res += force_act;
+					//std::cout << " (INT PERSON FORCES) (4) "<<  std::endl;
+					//std::cout << " (4)  force_persons_int_planning_virtual_companion_person_akp " <<  std::endl;
+					//std::cout << " IN :force_persons_int_planning_virtual case: Akp_planning, person id="<<iit->get_id()<<  std::endl;
+					if(debug_antes_subgoals_entre_AKP_goals_){
+						//force_res.print();
+					}
+
+					//if(((force_act.fx>0.5)||(force_act.fy>0.5))&&see_forces_){
+					//	std::cout<<"person_id=iit->get_id()=" <<iit->get_id()<< "; force_act.fx ="<<force_act.fx<< "; force_act.fy="<<force_act.fy <<  std::endl;
+					//}
+				//}//else{
+
+				//}
+			}
+		}
+	}
+	//std::cout << " (5)  force_persons_int_planning_virtual_companion_person_akp " <<  std::endl;
+	return force_res;
+}
+
+
+Sforce Cplan_local_nav_person_companion::force_persons_int_planning_virtual_robot_companion_propagation(Cperson_abstract* center , unsigned int t, double min_dist2, Cperson_abstract::companion_reactive reactive)
+{
+
+	Sforce force_res;
+	Sforce force_res_other_people;
+	Sforce force_res_companion_person;
+	if(debug_real_test_companion_){
+		std::cout<< std::endl<< std::endl<< std::endl << " !!!!!!!!!!!!!!!!!!!!!! [replan last step] IN :force_persons_int_planning_virtual_robot_companion_propagation " <<  std::endl;
+	}
+
+	switch(reactive)
+	{
+	case Cperson_abstract::Reactiva_repulsive:
+	case Cperson_abstract::Reactive_atractive:
+		//std::cout << " IN :force_persons_int_planning_virtual case(Reactiva_repulsive+atractive) " <<  std::endl;
+		for( auto iit : nearby_person_list_)
+		{
+			//std::cout << " [atractive and repulsive] force_persons_int_planning_virtual_robot_companion_propagation IN : Akp_planning " <<  std::endl;
+			//		std::cout << " IN : Akp_planning; id_person_companion_"<<id_person_companion_ <<"; nearby_person_list_.size="<<nearby_person_list_.size()<<"; t="<<t <<  std::endl;
+
+
+			if( *center != *iit && center->get_planning_trajectory()->at(t)
+						.distance2( iit->get_planning_trajectory( )->at(t) ) < min_dist2 )
+			{
+
+
+
+					if(iit->get_id()!=id_person_companion_){ // para el id companion esta fuerza ha de ser 0!
+							force_res += center->force( iit->get_planning_trajectory( )->at(t) ,  get_sfm_int_params(center,iit),
+									&(center->get_planning_trajectory()->at(t) ) );
+							force_res_other_people += center->force( iit->get_planning_trajectory( )->at(t) ,  get_sfm_int_params(center,iit),
+									&(center->get_planning_trajectory()->at(t) ) );
+						}else{
+							//std::cout<<" (replan_last_step) params_interact_with_person_companion(1)=" <<get_sfm_int_params(center,iit)->at(1)<< "; params_interact_with_person_companion(2) ="<<get_sfm_int_params(center,iit)->at(2)<< "; params_interact_with_person_companion(3)="<<get_sfm_int_params(center,iit)->at(3) <<  std::endl;
+							//		std::cout<<"params_interact_with_person_companion(4)=" <<get_sfm_int_params(center,iit)->at(4) <<  std::endl;
+
+							//std::cout << " IN :force person companion " <<  std::endl;
+							force_res += center->force( iit->get_planning_trajectory( )->at(t) ,  get_sfm_int_params(center,iit),
+														&(center->get_planning_trajectory()->at(t) ) , reactive);
+							force_res_companion_person= center->force( iit->get_planning_trajectory( )->at(t) ,  get_sfm_int_params(center,iit),
+									&(center->get_planning_trajectory()->at(t) ) , reactive);
+
+							if(debug_gazebo_journal_){
+								std::cout<<" NO group_go_to_interact_with_other_person_(reactive-repulsive) else group_go_to_interact_with_other_person person_id=iit->get_id()=" <<iit->get_id()<< "; force_res_companion_person.fx ="<<force_res_companion_person.fx<< "; force_res_companion_person.fy="<<force_res_companion_person.fy <<  std::endl;
+
+							}
+
+						}
+
+
+
+			}
+		}
+	break;
+
+	case Cperson_abstract::Akp_planning:
+	default:
+		if(debug_real_test_companion_){
+			std::cout << "force_persons_int_planning_virtual_robot_companion_propagation IN : Akp_planning " <<  std::endl;
+			std::cout << " IN : Akp_planning; id_person_companion_"<<id_person_companion_ <<"; nearby_person_list_.size="<<nearby_person_list_.size()<<"; t="<<t <<  std::endl;
+		}
+		for( auto iit : nearby_person_list_)
+		//for( auto iit : person_list_)
+		{
+			if(debug_real_test_companion_){
+				std::cout << " IN : Akp_planning; iit="<<iit <<  std::endl;
+			}
+
+			//std::cout <<"; id="<<iit->get_id()<< "iit->get_planning_trajectory( )->size()= "<<iit->get_planning_trajectory( )->size() <<"; center->get_planning_trajectory()->size="<<center->get_planning_trajectory()->size()<<  std::endl;
+
+
+				//std::cout<<" INI (replan_last_step) params_interact_with_person_companion(1)=" <<get_sfm_int_params(center,iit)->at(1)<< "; params_interact_with_person_companion(2) ="<<get_sfm_int_params(center,iit)->at(2)<< "; params_interact_with_person_companion(3)="<<get_sfm_int_params(center,iit)->at(3) <<  std::endl;
+				//	std::cout<<"params_interact_with_person_companion(4)=" <<get_sfm_int_params(center,iit)->at(4) <<  std::endl;
+
+					if( *center != *iit && center->get_planning_trajectory()->at(t)
+												.distance2( iit->get_planning_trajectory( )->at(t) ) < min_dist2 )
+					{
+
+
+						if(iit->get_id()!=id_person_companion_){ // para el id companion esta fuerza ha de ser 0!
+							force_res += center->force( iit->get_planning_trajectory( )->at(t) ,  get_sfm_int_params(center,iit),
+									&(center->get_planning_trajectory()->at(t) ) );
+							//std::cout << " IN :force_persons_int_planning_virtual case: Akp_planning, person id="<<iit->get_id()<<  std::endl;
+							if(debug_antes_subgoals_entre_AKP_goals_){
+								force_res.print();
+							}
+							force_res_other_people += center->force( iit->get_planning_trajectory( )->at(t) ,  get_sfm_int_params(center,iit),
+														&(center->get_planning_trajectory()->at(t) ) );
+						}else{
+							//std::cout << " IN :force_persons_int_planning_virtual case: Akp_planning, salta person companion, id="<<iit->get_id()<<  std::endl;
+							//force_res+=Sforce();
+							//force_res.print();
+							force_res += center->force( iit->get_planning_trajectory( )->at(t) ,  get_sfm_int_params(center,iit),
+														&(center->get_planning_trajectory()->at(t) ) );
+								//std::cout<< "[f_int force replan_last_step ] person_ID="<<iit->get_id()<<"+ iit->get_person_type()="<<iit->get_person_type() <<  std::endl;
+
+							if(debug_real_test_companion_){
+								std::cout << " IN :force_persons_int_planning_virtual_robot_companion_propagation case: Akp_planning, person id="<<iit->get_id()<<  std::endl;
+							}
+							if(debug_antes_subgoals_entre_AKP_goals_){
+								//force_res.print();
+							}
+							force_res_companion_person= center->force( iit->get_planning_trajectory( )->at(t) ,  get_sfm_int_params(center,iit),
+															&(center->get_planning_trajectory()->at(t) ) , reactive);
+
+							if(debug_gazebo_journal_){
+								std::cout<<"(AKP) NO group_go_to_ ... get_sfm_int_params(center,iit)->at(0)="<<get_sfm_int_params(center,iit)->at(0)<< "; at(1) ="<<get_sfm_int_params(center,iit)->at(1)<< "; at(2)="<<get_sfm_int_params(center,iit)->at(2)<<"; at(3)="<<get_sfm_int_params(center,iit)->at(3)<<"; at(4)="<<get_sfm_int_params(center,iit)->at(4) <<  std::endl;
+								std::cout<<"(AKP) NO group_go_to_interact_with_other_person_ person_id=iit->get_id()=" <<iit->get_id()<< "; force_res_companion_person.fx ="<<force_res_companion_person.fx<< "; force_res_companion_person.fy="<<force_res_companion_person.fy <<  std::endl;
+
+							}
+
+						}
+
+
+					}
+
+				//}
+
+
+		//	}
+		}
+	break;
+	}
+
+
+	//std::cout << " (out1) "<< std::endl;
+
+	last_step_robot_other_person_cost_=force_res_companion_person.module2();
+	last_step_robot_companion_cost_=force_res_companion_person.module2();
+
+	//std::cout << " (out 2) "<< std::endl;
+
+
+	return force_res;
+}
+
+
+
+Sforce Cplan_local_nav_person_companion::force_persons_int_robot_prediction_virtual(const SpointV& center , unsigned int t, double min_dist2)
+{
+	//calculates the resultant forces to all nearby people due to the robot position
+	Sforce force_res;
+    for( auto iit : nearby_person_list_)
+    {
+        if( center.distance2( iit->get_planning_trajectory( )->at(t) ) < min_dist2 )
+        {
+            force_res += iit->force( iit->get_prediction_trajectory( )->at(t) ,  get_sfm_int_params(iit),
+            		&center );
+        }
+    }
+	return force_res;
+}
+
+Sforce Cplan_local_nav_person_companion::force_objects_laser_int_planning_virtual( Cperson_abstract* person, unsigned int planning_index, double min_dist2, bool robot_collision_check_flag)
+{
+	Sforce force_res;
+	Sforce force_res_iter;
+	const SpointV_cov* virtual_current_point;
+	if ( planning_index == 0)
+	{
+		virtual_current_point = &(person->get_current_pointV());
+	}
+	else
+	{
+		virtual_current_point = &(person->get_planning_trajectory()->at(planning_index) );
+	}
+	nearby_obstacle_list_.clear();
+	//for robot, checks
+	//std::cout << "min_dist2="<<min_dist2<< std::endl;
+	number_obstacles_big_force_=1;
+
+	for( Spoint iit : laser_obstacle_list_)
+	{
+		//there is a list for persons and another for robot(s). This function is for obstacles
+		//double d2 = person->get_current_pointV().distance2( iit );
+		double d2 = person->get_current_pointV().distance( iit );
+		if( d2 < min_dist2)//square distance 5^2
+		{
+
+			force_res_iter=person->force_sphe( iit , get_sfm_int_params(person), virtual_current_point );
+			//std::cout <<"d2="<<d2<<"; force_res_iter.fx="<<force_res_iter.fx<<"; force_res_iter.fy"<<force_res_iter.fy<< std::endl;
+			force_res +=force_res_iter;
+			if( robot_collision_check_flag && d2 < 1.0 )//detect ultra nearby obstacles and check after propagation if collision took place
+			{
+				nearby_obstacle_list_.push_back( iit );
+			}
+			if((force_res_iter.fx>1.0)||(force_res_iter.fy>1.0)||(force_res_iter.fx<-1.0)||(force_res_iter.fy<-1.0)){
+				number_obstacles_big_force_++;
+			}
+
+
+		}
+	}
+
+	if(number_obstacles_big_force_>1){
+		number_obstacles_big_force_=number_obstacles_big_force_-1;
+	}
+
+	//std::cout << " (ant) force_res.fx="<<force_res.fx<<"; force_res.fy="<<force_res.fy<<"; number_obstacles_big_force_= "<<number_obstacles_big_force_<< std::endl;
+	force_res.fx=force_res.fx/number_obstacles_big_force_;
+	force_res.fy=force_res.fy/number_obstacles_big_force_;
+	//std::cout << "(desp) force_res.fx="<<force_res.fx<<"; force_res.fy="<<force_res.fy<< std::endl;
+	//std::cout << "nearby_obstacle_list_.size()="<<nearby_obstacle_list_.size()<< std::endl;
+
+	return force_res;
+
+}
+
+Sforce Cplan_local_nav_person_companion::force_objects_laser_int_planning_virtual_robot_propagation( Cperson_abstract* person, unsigned int planning_index, double min_dist2, bool robot_collision_check_flag)
+{
+	Sforce force_res;
+	Sforce force_res_iter;
+	const SpointV_cov* virtual_current_point;
+	if ( planning_index == 0)
+	{
+		virtual_current_point = &(person->get_current_pointV());
+	}
+	else
+	{
+		virtual_current_point = &(person->get_prediction_trajectory_with_target_person()->at(planning_index) );
+	}
+	nearby_obstacle_list_.clear();
+	//for robot, checks
+	//std::cout << "min_dist2="<<min_dist2<< std::endl;
+	number_obstacles_big_force_=1;
+
+	for( Spoint iit : laser_obstacle_list_)
+	{
+		//there is a list for persons and another for robot(s). This function is for obstacles
+		//double d2 = person->get_current_pointV().distance2( iit );
+		double d2 = person->get_current_pointV().distance( iit );
+		if( d2 < min_dist2)//square distance 5^2
+		{
+
+			force_res_iter=person->force_sphe( iit , get_sfm_int_params(person), virtual_current_point );
+			//std::cout <<"d2="<<d2<<"; force_res_iter.fx="<<force_res_iter.fx<<"; force_res_iter.fy"<<force_res_iter.fy<< std::endl;
+			force_res +=force_res_iter;
+			if( robot_collision_check_flag && d2 < 1.0 )//detect ultra nearby obstacles and check after propagation if collision took place
+			{
+				nearby_obstacle_list_.push_back( iit );
+			}
+			if((force_res_iter.fx>1.0)||(force_res_iter.fy>1.0)||(force_res_iter.fx<-1.0)||(force_res_iter.fy<-1.0)){
+				number_obstacles_big_force_++;
+			}
+
+
+		}
+	}
+
+	if(number_obstacles_big_force_>1){
+		number_obstacles_big_force_=number_obstacles_big_force_-1;
+	}
+
+	//std::cout << " (ant) force_res.fx="<<force_res.fx<<"; force_res.fy="<<force_res.fy<<"; number_obstacles_big_force_= "<<number_obstacles_big_force_<< std::endl;
+	force_res.fx=force_res.fx/number_obstacles_big_force_;
+	force_res.fy=force_res.fy/number_obstacles_big_force_;
+	//std::cout << "(desp) force_res.fx="<<force_res.fx<<"; force_res.fy="<<force_res.fy<< std::endl;
+	//std::cout << "nearby_obstacle_list_.size()="<<nearby_obstacle_list_.size()<< std::endl;
+
+	return force_res;
+
+}
+
+
+bool Cplan_local_nav_person_companion::check_collision_person_companion_akp( const SpointV_cov& p2 ,  int t)
+{
+	// (ONLY simulation==simulated person companion as a robot) CHECK collision of the path for the personCompanion shape and for the companion shape (to calculate the companion cost and angle, before)
+		Spoint p=Spoint(p2.x,p2.y,p2.time_stamp);
+		bool collision=false;
+
+		// Start: Init values of companion collisions
+		case_dynamic_=false;
+		double  robot_radi=robot_->get_platform_radii();
+
+		double real_robot_person_distance; // real distance between robot and person companion.
+
+		if(bool_case_person_companion_){
+			real_robot_person_distance=calc_robot_person_companion_distance_companion_person_akp();
+		}else{
+			real_robot_person_distance=calc_robot_person_companion_distance();
+		}
+		min_distance_collision_=(robot_person_proximity_distance_+2*robot_radi+little_augmented_collision_margin_)/2;
+
+
+		if(debug_companion_){
+			std::cout << "min_distance_collision_==robot_person_companion_distance_= "<<min_distance_collision_ << std::endl;
+		}
+		std::vector<double> min_distances_vector;
+		min_distances_vector.clear();
+
+		bool_collision_companion_=false;
+
+		//double collision_threshold=(robot_person_proximity_distance_+2*robot_radi+little_augmented_collision_margin_)/2;
+		double collision_threshold=(real_robot_person_distance+2*robot_radi+little_augmented_collision_margin_)/2;
+
+		if(debug_gazebo_journal_){
+			std::cout << "collision_threshold= "<<collision_threshold<<"; obstacle_radi_amp_="<<obstacle_radi_amp_<<"; anisotropy_threshold_="<<anisotropy_threshold_<< std::endl;
+		}
+		// End: Init values of companion collisions
+
+
+	//TODO does not work properly on real scenarios! long term issue
+	//check colision with map: TO BE DEPRECATED
+	if ( read_force_map_success_ && !is_cell_clear_map( p.x, p.y ) )
+	{
+		bool_collision_companion_=true; // colision, pero por choque con el mapa, TODO: mirar como tratar esto...
+		collision = true;
+	}
+
+
+	//check collision with obstacles (laser), prior is necessary to calculate force_objects_laser_int_planning()
+	//for( Spoint iit : nearby_obstacle_list_ )//TODO not working properly since the c2g recalculates this vector
+	//std::cout << "laser_obstacle_list_.size()=" << laser_obstacle_list_.size()<< std::endl;
+
+	for( Spoint iit : laser_obstacle_list_ )
+	{
+		// START: calculate normal collisions only with person companion shape
+		//if( iit.distance2( p )  < robot_->get_platform_radii_2() ) // TODO: ojo! le cambie la distancia respeco a la que gonzalo calculaba colision, pq a mi esa distancia me liaba.
+		if( iit.distance2( p )  < (robot_->get_platform_radii() + 0.45)) // 0.37, margen colision con objeto, colchon de seguridad!
+		{
+			if(debug_real_test_companion3_){
+				std::cout << "(!!! check_collision !!!) iit.distance2( p )="<< iit.distance2( p )<<" iit.distance( p )"<< iit.distance( p )<<"< robot_->get_platform_radii()="<<robot_->get_platform_radii()<<"; obstacle_radi_="<<obstacle_radi_<<"; robot_->get_platform_radii()/2 + 0.55="<<robot_->get_platform_radii()/2 + 0.55<<"; robot_->get_platform_radii()/2="<<robot_->get_platform_radii()/2<< std::endl;
+			}
+			//std::cout << "(OBSTACLE) Collision found!! robot_->get_platform_radii_2()=" << robot_->get_platform_radii_2() <<"; iit.distance2( p )="<<iit.distance2( p )<<"; iit.distance( p )="<<iit.distance( p )<< std::endl;
+			collision = true;
+		}
+		// END: calculate normal collisions only with person companion shape
+
+		// START: Calculate companion collisions with the group shape.
+		double anisotropy=calculate_anisotropy( p2, iit );  // ???? ver si necesito SpointV_cov del robot, verdaderamente...
+		double collision_distance_obst_act=sqrt((iit.distance( p )-obstacle_radi_amp_)*(iit.distance( p )-obstacle_radi_amp_));
+		//std::cout << "Collision found!! iit.distance( initial_robot_spoint_ )=act_min_collision_dist= "<< act_min_collision_dist<< std::endl;
+
+		if( (collision_distance_obst_act < collision_threshold)&& (anisotropy>anisotropy_threshold_) ) // ojo! he cambiado distance2 por distance.
+		{
+		//	std::cout << "anisotropy=" << anisotropy<<"; min_distance_vector_=2*(iit.distance( p ))="<< 2*(iit.distance( p )) << std::endl;
+			//std::cout << "collision distance=iit.distance( p )=" << iit.distance( p ) <<" < collision_threshold="<<collision_threshold<< std::endl;
+			//std::cout << "obst_pose.x="<<iit.x<<"; obst_pose.y="<<iit.y<<"; obstacle_radi_amp_="<<obstacle_radi_amp_<< std::endl;
+
+			min_distances_vector.push_back(collision_distance_obst_act);
+			//collision=true;
+			bool_collision_companion_=true;
+
+			//std::cout << "obstacle_COLLISION=" << std::endl;
+			if(debug_real_test_companion_){
+				//std::cout << "anisotropy=" << anisotropy << std::endl;
+				//std::cout << "Collision found!! min_dist" << (iit.distance( p ))<<"; obstacle_radi_amp_"<<obstacle_radi_amp_<<"; collision_threshold="<<collision_threshold<<"; anisotropy=" <<anisotropy<<"; anisotropy_threshold_="<<anisotropy_threshold_<< std::endl;
+				//obstacle.print();
+			}
+
+		}
+		//END:	Calculate companion collisions with the group shape.
+
+	}
+
+
+	// START: debug path of the companion collisions calculation
+	if(debug_gazebo_journal_){
+		// calc min distance to obstacle collision!
+		double act_min_collision_dist=0;
+		if(!min_distances_vector.empty()){
+			act_min_collision_dist=min_distances_vector[0];
+		}
+
+		for(unsigned int u=0; u<min_distances_vector.size();u++){
+			if(min_distances_vector[u]<act_min_collision_dist){
+				act_min_collision_dist=min_distances_vector[u];
+			}
+		}
+		if(!min_distances_vector.empty()){
+			std::cout << "Collision found!! iit.distance( initial_robot_spoint_ )=act_min_collision_dist= "<< act_min_collision_dist<< std::endl;
+		}
+	}
+	/*if(collision){
+		std::cout << "obstacle_radi_amp_=" << obstacle_radi_amp_<<"; collision_threshold"<<collision_threshold << std::endl;
+		std::cout << "anisotropy_threshold_=" << anisotropy_threshold_ << std::endl;
+		for(unsigned int l=0; l<min_distances_vector.size(); l++){
+			std::cout << "min_distances_vector("<<l<<")=" << min_distances_vector[l] << std::endl;
+		}
+	}*/
+
+	//END: debug path of the companion collisions calculation
+
+
+
+	//std::cout << "t=" <<t<< std::endl;
+	//if t = -1, then no collision is calculated wrt ppl
+	if ( t > -1 )
+	{
+		double d,det;
+		switch( ppl_collision_mode_ )
+		{
+		  case 0: //normal mode, quadratic distance to person, independent of distribution
+			for( auto iit : nearby_person_list_)
+			{
+				// START: calculate simple collisions, only with person companion shape.
+				if(iit->get_id()!=id_person_companion_){ // TODO: ASEGURARME QUE SEA VERDAD creo que tampoco la ha de tener en cuenta en colisiones aquí.
+					//d = iit->get_planning_trajectory( )->at(t).distance2( p );
+					d = iit->get_planning_trajectory( )->at(t).distance( p );
+					//if ( d < robot_->get_platform_radii_2())
+					if ( d < (robot_->get_platform_radii()+(person_radi_)))
+					{
+						//std::cout << " case 0; d="<<d<< std::endl;
+
+						collision = true;
+					}
+				}
+				// END: calculate simple collisions, only with person companion shape.
+
+				// START: calculate companion collisions with the group shape
+				//if(!group_go_to_interact_with_other_person_){
+					if(iit->get_id()!=id_person_companion_){ // add due to person companion, esta persona no hay que calcular colisiones respecto a ella.
+						d = sqrt((iit->get_planning_trajectory( )->at(t).distance( p )-person_radi_amp_)*(iit->get_planning_trajectory( )->at(t).distance( p )-person_radi_amp_));
+						double anisotropy=calculate_anisotropy( p2, iit->get_planning_trajectory( )->at(t) );
+
+						if ( (d < collision_threshold)&&(anisotropy>anisotropy_threshold_))
+						{
+							//std::cout << "person_COLLISION id="<<iit->get_id() << std::endl;
+							case_dynamic_=true;
+							//collision=true;
+							bool_collision_companion_=true;
+							min_distances_vector.push_back(d); // - robot_radi, margen de seguridad entre obstaculos.
+							if(debug_real_test_companion_){
+								//std::cout << "anisotropy=" << anisotropy << std::endl;
+								//std::cout << "Collision found!! min_dist" << 2*d <<"person_id"<<iit->get_id()<< std::endl;
+							}
+						}
+					}
+
+				// END: calculate companion collisions with the group shape
+
+			}
+			break;
+		  case 1://mahalanobis distance, considers collision if inside the std ellipsoid: TOO RESTRICTIVE...
+			for( auto iit : nearby_person_list_)
+			{
+				// START: calculate simple collisions, only with person companion shape.
+				if(iit->get_id()!=id_person_companion_){
+					d = iit->get_planning_trajectory( )->at(t).cov_dist(p,det);
+					if ( d < 1.0 )
+					{
+						//std::cout << " case 1"<< std::endl;
+						//std::cout << "Colision detected, pr distance = " << d << std::endl;
+						collision = true;
+					}
+				}
+				// END: calculate simple collisions, only with person companion shape.
+
+				// START: calculate companion collisions with the group shape
+				//if(!group_go_to_interact_with_other_person_){
+					if(iit->get_id()!=id_person_companion_){
+						double anisotropy=calculate_anisotropy( p2, iit->get_planning_trajectory( )->at(t) );
+						d = sqrt((iit->get_planning_trajectory( )->at(t).cov_dist(p,det)-person_radi_amp_)*(iit->get_planning_trajectory( )->at(t).cov_dist(p,det)-person_radi_amp_));
+						if ( (d < collision_threshold)&&(anisotropy>anisotropy_threshold_) )
+						{
+							//std::cout << "person_COLLISION id="<<iit->get_id() << std::endl;
+							case_dynamic_=true;
+							//std::cout << "Colision detected, pr distance = " << d << std::endl;
+							//collision=true;
+							bool_collision_companion_=true;
+							min_distances_vector.push_back(d);
+							if(debug_real_test_companion_){
+								//std::cout << "anisotropy=" << anisotropy << std::endl;
+								//std::cout << "Collision found!! min_dist" << 2*d <<"person_id"<<iit->get_id()<< std::endl;
+							}
+						}
+					}
+
+				// END: calculate companion collisions with the group shape
+
+			}
+			break;
+		  case 2 ://mahalanobis distance, half the std, much less restrictive
+			for( auto iit : nearby_person_list_)
+			{
+
+				// START: calculate simple collisions, only with person companion shape.
+				if(iit->get_id()!=id_person_companion_){
+					d = iit->get_planning_trajectory( )->at(t).cov_dist(p,det);
+					if ( det > 1.0)
+					{
+						if ( d < 0.5 )
+						{
+							//std::cout << " case 2.1"<< std::endl;
+							collision = true;
+						}
+					}
+					else
+					{
+						//std::cout << "(case2) d=" <<d<<"; iit->get_planning_trajectory( )->at(t).distance( p )="<<iit->get_planning_trajectory( )->at(t).distance( p )<< std::endl;
+						//std::cout << " robot_->get_platform_radii()="<<robot_->get_platform_radii()<<"; (person_radi_*2)="<<(person_radi_*2)<<" sum="<<robot_->get_platform_radii()+(person_radi_*2)<< std::endl;
+
+						//d = iit->get_planning_trajectory( )->at(t).distance2( p );
+						d = iit->get_planning_trajectory( )->at(t).distance( p );
+						//if ( d < robot_->get_platform_radii_2() )
+						if ( d < (robot_->get_platform_radii()+(person_radi_)) )
+						{
+							//std::cout << " case 2.2"<< std::endl;
+							return true;
+						}
+					}
+				}
+				// END: calculate simple collisions, only with person companion shape.
+
+				// START: calculate companion collisions with the group shape
+				//if(!group_go_to_interact_with_other_person_){
+
+					if(iit->get_id()!=id_person_companion_){
+						d = sqrt((iit->get_planning_trajectory( )->at(t).cov_dist(p,det)-person_radi_amp_)*(iit->get_planning_trajectory( )->at(t).cov_dist(p,det)-person_radi_amp_));
+						double anisotropy=calculate_anisotropy( p2, iit->get_planning_trajectory( )->at(t) );
+						if ( det > 1.0)
+						{
+							if ( (d < collision_threshold)&&(anisotropy>anisotropy_threshold_) )
+							{
+								//std::cout << "person_COLLISION id="<<iit->get_id() << std::endl;
+								case_dynamic_=true;
+								//collision=true;
+								bool_collision_companion_=true;
+								min_distances_vector.push_back(d);
+								if(debug_real_test_companion_){
+									//std::cout << "anisotropy=" << anisotropy << std::endl;
+									//std::cout << "Collision found!! min_dist" << 2*d <<"person_id"<<iit->get_id()<< std::endl;
+								}
+							}
+						}
+						else
+						{
+							d = sqrt((iit->get_planning_trajectory( )->at(t).distance( p )-person_radi_amp_)*(iit->get_planning_trajectory( )->at(t).distance( p )-person_radi_amp_));
+							double anisotropy=calculate_anisotropy( p2, iit->get_planning_trajectory( )->at(t) );
+							if ( (d < collision_threshold)&& (anisotropy>anisotropy_threshold_) )
+							{
+								//std::cout << "person_COLLISION id="<<iit->get_id() << std::endl;
+								case_dynamic_=true;
+								//collision=true;
+								bool_collision_companion_=true;
+								min_distances_vector.push_back(d);
+								if(debug_real_test_companion_){
+									//std::cout << "anisotropy=" << anisotropy << std::endl;
+									//std::cout << "Collision found!! min_dist" << 2*d <<"person_id"<<iit->get_id()<< std::endl;
+								}
+							}
+						}
+					}
+
+				// END: calculate companion collisions with the group shape
+
+			}
+			break;
+		  case 3 :
+			for( auto iit : nearby_person_list_)
+			{
+				// START: calculate simple collisions, only with person companion shape.
+				if(iit->get_id()!=id_person_companion_){
+					d = iit->get_planning_trajectory( )->at(t).cov_dist(p,det);
+					if ( det > 1.0)
+					{
+						if ( d < 0.3 )
+						{
+							//std::cout << " case 3.1"<< std::endl;
+							collision=true;
+						}
+					}
+					else
+					{
+
+						//std::cout << "(case3) d=" <<d<<"; iit->get_planning_trajectory( )->at(t).distance( p )="<<iit->get_planning_trajectory( )->at(t).distance( p )<< std::endl;
+						//std::cout << " robot_->get_platform_radii()="<<robot_->get_platform_radii()<<"; (person_radi_*2)="<<(person_radi_*2)<<" sum="<<robot_->get_platform_radii()+(person_radi_*2)<< std::endl;
+
+						//d = iit->get_planning_trajectory( )->at(t).distance2( p );
+						d = iit->get_planning_trajectory( )->at(t).distance( p );
+						//if ( d < robot_->get_platform_radii_2() )
+						if ( d < (robot_->get_platform_radii()+(person_radi_)) )
+						{
+							//std::cout << " case 3.2"<< std::endl;
+							collision=true;
+						}
+					}
+				}
+				// END: calculate simple collisions, only with person companion shape.
+
+				// START: calculate companion collisions with the group shape
+				//if(!group_go_to_interact_with_other_person_){
+					if(iit->get_id()!=id_person_companion_){
+						d = sqrt((iit->get_planning_trajectory( )->at(t).cov_dist(p,det)-person_radi_amp_)*(iit->get_planning_trajectory( )->at(t).cov_dist(p,det)-person_radi_amp_));
+						double anisotropy=calculate_anisotropy( p2, iit->get_planning_trajectory( )->at(t) );
+						if ( det > 1.0)
+						{
+							if ( (d < collision_threshold)&&(anisotropy>anisotropy_threshold_)  ) // de momento, para mi todas las suyas con constantes, han de ser menores que la distancia robot+persona. (luego ya veremos si va bien o no)
+							{
+							//	std::cout << "person_COLLISION id="<<iit->get_id() << std::endl;
+								case_dynamic_=true;
+								//collision=true;
+								bool_collision_companion_=true;
+								min_distances_vector.push_back(d);
+								if(debug_real_test_companion_){
+									//std::cout << "anisotropy=" << anisotropy << std::endl;
+									//std::cout << "Collision found!! min_dist" << 2*d <<"person_id"<<iit->get_id()<< std::endl;
+								}
+							}
+						}
+						else
+						{
+							d = sqrt((iit->get_planning_trajectory( )->at(t).distance( p )-person_radi_amp_)*(iit->get_planning_trajectory( )->at(t).distance( p )-person_radi_amp_));
+							double anisotropy=calculate_anisotropy( p2, iit->get_planning_trajectory( )->at(t) );
+							if ( (d < collision_threshold)&&(anisotropy>anisotropy_threshold_) )
+							{
+								//std::cout << "person_COLLISION id="<<iit->get_id() << std::endl;
+								case_dynamic_=true;
+								//collision=true;
+								bool_collision_companion_=true;
+								min_distances_vector.push_back(d);
+								if(debug_real_test_companion_){
+									//std::cout << "anisotropy=" << anisotropy << std::endl;
+									//std::cout << "Collision found!! min_dist" << 2*d <<"person_id"<<iit->get_id()<< std::endl;
+								}
+							}
+						}
+					}
+
+				// END: calculate companion collisions with the group shape
+
+			}
+			break;
+
+		}
+
+			//calculate probabilities. NOT implemented, insignificant collision values when cov is high
+            //pr_no_col *= 1 - gaussian_constant_ / sqrt(det) * exp( -0.5*d );// * robot_->get_platform_radii_2();//area = pi * r^2 (pi included in gaussian_contant)
+            //std::cout << "Pr of no colision joint = " << pr_no_col << " and distance = " << d << " det = " << det << std::endl;
+	}
+
+
+	// START: final part calculate the min collision distance for companion
+
+	// obtain minimun collision distance.
+	if(debug_companion_){
+		std::cout << "min_distances_vector.size()= "<<min_distances_vector.size()<< std::endl;
+	}
+	if(!min_distances_vector.empty()){ // si hay alguna distancia minima.
+
+		double min_dist;
+		if(min_distance_collision_>min_distances_vector[0]){
+			min_dist=min_distances_vector[0];
+		}else{
+			min_dist=min_distance_collision_;
+		}
+
+		for( unsigned int in=0; in<min_distances_vector.size(); in++){
+			if(debug_companion_){
+				std::cout << "min_distances_vector["<<in<<"]= "<<min_distances_vector[in]<< std::endl;
+			}
+			if(min_distances_vector[in]<min_dist){
+				min_dist=min_distances_vector[in];
+				//std::cout << "min_dist="<<min_dist<< std::endl;
+				//min_angle=min_angles_colisions_[in];
+			}
+		}
+
+
+		min_distance_collision_=min_dist; // sacar fuera la min dist
+
+	}
+
+	//std::cout << "min_distance_collision_= "<<min_distance_collision_<<"; ((real_robot_person_distance/2)+2*robot_radi)="<<((real_robot_person_distance/2)+2*robot_radi)<< std::endl;
+
+	/*if(min_distance_collision_<((real_robot_person_distance/2)+2*robot_radi)){
+		std::cout << "(REAL ROBOT-PERSON distace for collisions)!!! real_robot_person_distance= "<<real_robot_person_distance << std::endl;
+	}*/
+
+	if(debug_companion_){
+		std::cout << "min_distance_collision_= "<<min_distance_collision_<< std::endl;
+		std::cout << "bool_collision_companion_= "<<bool_collision_companion_ << std::endl;
+		std::cout << "FIN function: check_collision_companion " << std::endl;
+	}
+	/*check_collision_companion_end = clock();
+	if(check_execution_times_){
+		std::cout << "time_check_collision_companion="<<((check_collision_companion_start-check_collision_companion_end)/clocks_per_sec_my_var_)*1000 << std::endl;
+	}*/
+
+
+	// END: final part calculate the min collision distance for companion
+
+
+	return collision;
+}
+
+
+
+bool Cplan_local_nav_person_companion::check_collision_final( const Spoint& p ,  int t)
+{
+	//TODO does not work properly on real scenarios! long term issue
+	//check colision with map: TO BE DEPRECATED
+	if ( read_force_map_success_ && !is_cell_clear_map( p.x, p.y ) )
+//	if ( read_force_map_success_ && !is_cell_clear_map( robot_->get_robot_planning_trajectory()->back().x,	robot_->get_robot_planning_trajectory()->back().y ) )
+	{
+		return  true;
+	}
+
+
+	//check collision with obstacles (laser), prior is necessary to calculate force_objects_laser_int_planning()
+	//for( Spoint iit : nearby_obstacle_list_ )//TODO not working properly since the c2g recalculates this vector
+	//std::cout << "laser_obstacle_list_.size()=" << laser_obstacle_list_.size()<< std::endl;
+
+
+	for( Spoint iit : laser_obstacle_list_ )
+	{
+
+		//if( iit.distance2( p )  < robot_->get_platform_radii_2() ) // TODO: ojo! le cambie la distancia respeco a la que gonzalo calculaba colision.
+		if( iit.distance( p )  < (robot_->get_platform_radii()+obstacle_radi2_ )) // 0.37, margen colision con objeto, colchon de seguridad!
+		{
+			if(debug_real_test_companion4_){
+				std::cout << "(!!! check_collision_final !!!) iit.distance( p )="<< iit.distance( p )<<"< robot_->get_platform_radii()="<<robot_->get_platform_radii()<< std::endl;
+			}
+			//std::cout << "(OBSTACLE) Collision found!! robot_->get_platform_radii_2()=" << robot_->get_platform_radii_2() <<"; iit.distance2( p )="<<iit.distance2( p )<<"; iit.distance( p )="<<iit.distance( p )<< std::endl;
+			return true;
+		}
+	}
+
+	//if t = -1, then no collision is calculated wrt ppl
+	if ( t > -1 )
+	{
+		double d,det;
+		switch( ppl_collision_mode_ )
+		{
+		  case 0: //normal mode, quadratic distance to person, independent of distribution
+			for( auto iit : nearby_person_list_)
+			{
+				//std::cout << "t=" <<t<<"; iit->get_planning_trajectory( )->size()="<<iit->get_planning_trajectory( )->size()<< std::endl;
+					//d = iit->get_planning_trajectory( )->at(t).distance2( p );
+					d = iit->get_planning_trajectory( )->at(t).distance( p );
+					//if ( d < robot_->get_platform_radii_2())
+					if ( d < (robot_->get_platform_radii()+person_radi2_))
+					{
+						std::cout << " case 0; d="<<d<<" person_radi2_="<<person_radi2_<<"; robot_->get_platform_radii()="<<robot_->get_platform_radii()<<"; t="<<t<< std::endl;
+
+						return true;
+					}
+
+			}
+			break;
+		  case 1://mahalanobis distance, considers collision if inside the std ellipsoid: TOO RESTRICTIVE...
+			for( auto iit : nearby_person_list_)
+			{
+
+					d = iit->get_planning_trajectory( )->at(t).cov_dist(p,det);
+					if ( d < 1.0 )
+					{
+						//std::cout << " case 1"<< std::endl;
+						//std::cout << "Colision detected, pr distance = " << d << std::endl;
+						return true;
+					}
+
+			}
+			break;
+		  case 2 ://mahalanobis distance, half the std, much less restrictive
+			for( auto iit : nearby_person_list_)
+			{
+
+					d = iit->get_planning_trajectory( )->at(t).cov_dist(p,det);
+					if ( det > 1.0)
+					{
+						if ( d < 0.5 )
+						{
+							//std::cout << " case 2.1"<< std::endl;
+							return true;
+						}
+					}
+					else
+					{
+						//d = iit->get_planning_trajectory( )->at(t).distance2( p );
+						d = iit->get_planning_trajectory( )->at(t).distance( p );
+						//if ( d < robot_->get_platform_radii_2() )
+						if ( d < (robot_->get_platform_radii()+person_radi2_) )
+						{
+							//std::cout << " case 2.2"<< std::endl;
+							return true;
+						}
+					}
+
+			}
+			break;
+		  case 3 :
+			for( auto iit : nearby_person_list_)
+			{
+
+					d = iit->get_planning_trajectory( )->at(t).cov_dist(p,det);
+					if ( det > 1.0)
+					{
+						if ( d < 0.3 )
+						{
+							//std::cout << " case 3.1"<< std::endl;
+							return true;
+						}
+					}
+					else
+					{
+						//d = iit->get_planning_trajectory( )->at(t).distance2( p );
+						d = iit->get_planning_trajectory( )->at(t).distance( p );
+						//if ( d < robot_->get_platform_radii_2() )
+						if ( d < (robot_->get_platform_radii()+person_radi2_) )
+						{
+							//std::cout << " case 3.2"<< std::endl;
+							return true;
+						}
+					}
+
+			}
+			break;
+
+		}
+
+			//calculate probabilities. NOT implemented, insignificant collision values when cov is high
+            //pr_no_col *= 1 - gaussian_constant_ / sqrt(det) * exp( -0.5*d );// * robot_->get_platform_radii_2();//area = pi * r^2 (pi included in gaussian_contant)
+            //std::cout << "Pr of no colision joint = " << pr_no_col << " and distance = " << d << " det = " << det << std::endl;
+	}
+
+	return false;
+}
+
+
+void Cplan_local_nav_person_companion::set_number_of_vertex( unsigned int n )
+{
+	max_iter_ = n;
+    edge_.reserve((size_t)max_iter_);
+    cost_robot_.reserve((size_t)max_iter_);
+    cost_int_forces_.reserve((size_t)max_iter_);
+    cost_obstacles_.reserve((size_t)max_iter_);
+    //cost_local_minima_.reserve((size_t)max_iter_);
+    cost_distance_.reserve((size_t)max_iter_);
+    cost_orientation_.reserve((size_t)max_iter_);
+    cost_past_traj_.reserve((size_t)max_iter_);
+    nodes_in_branch_.reserve((size_t)max_iter_);
+	random_goals_.reserve( (size_t)max_iter_ );
+
+
+}
+
+void Cplan_local_nav_person_companion::preprocess_global_parameters(Crobot* robot_act)
+{
+	switch( global_mode_)
+	{
+	case Cplan_local_nav_person_companion::Weighted_sum_erf :
+		calculate_normalization_cost_functions_parameters_erf();
+		break;
+	case Cplan_local_nav_person_companion::MO_erf :
+		calculate_normalization_cost_functions_parameters_erf();
+		calculate_non_dominated_solutions();
+		break;
+	case Cplan_local_nav_person_companion::MO_norm :
+		calculate_normalization_cost_functions_parameters_norm(robot_act);
+		calculate_non_dominated_solutions();
+		break;
+	case Cplan_local_nav_person_companion::Scalarization :
+	case Cplan_local_nav_person_companion::Weighted_sum_norm :
+		//calcualte normalization: we need utopia point and max value
+		calculate_normalization_cost_functions_parameters_norm(robot_act);
+		break;
+	}
+}
+
+void Cplan_local_nav_person_companion::calculate_non_dominated_solutions()
+{
+	//fill the multiobjective cost structure
+	multicosts_.clear();
+	Smulticost_pcom m(0,5);
+	bool is_candidate_dominated;
+	//std::cout << " end_of_branches_index_.size()="<<end_of_branches_index_.size()<< std::endl;
+
+	for( unsigned int i : end_of_branches_index_ )
+	{
+		m.id = i;
+		//raw costs
+		/*m.cost[0] = cost_distance_[i];// [0] Goal cost
+		m.cost[1] = cost_orientation_[i];// [1] orientation cost
+		m.cost[2] = cost_robot_[i];// [2] Robot cost
+		m.cost[3] = cost_int_forces_[i];// [3]Interacting people cost
+		m.cost[4] = cost_obstacles_[i];// [5] obstacles cost*/
+		//normalized costs
+		m.cost[0] = (cost_distance_[i] - mean_cost_distance_) / std_cost_distance_;// [0] Goal cost
+		m.cost[1] = (cost_orientation_[i] - mean_cost_orientation_) / std_cost_orientation_;// [1] orientation cost
+		m.cost[2] = (cost_robot_[i] - mean_cost_robot_) / std_cost_robot_;// [2] Robot cost
+		m.cost[3] = (cost_int_forces_[i] - mean_cost_int_forces_) / std_cost_int_forces_;// [3]Interacting people cost
+		m.cost[4] = (cost_obstacles_[i] - mean_cost_obstacles_) / std_cost_obstacles_;// [5] obstacles cost
+		multicosts_.push_back(m);
+	}
+
+	//std::cout << " multicosts_.size()="<<multicosts_.size()<< std::endl;
+	//calculate the non-dominated set
+	nondominated_multicosts_.clear();
+	for( Smulticost_pcom i : multicosts_  )
+	{
+		//std::cout << " int first for:"<< std::endl;
+		//i.print_ml();
+		is_candidate_dominated = false;
+		for( std::list<Smulticost_pcom>::iterator j = nondominated_multicosts_.begin() ; j != nondominated_multicosts_.end(); j++ )
+		{
+			//std::cout << " int seconf for:"<< std::endl;
+			//check it is not the same
+			if ( i!=*j )
+			{
+				// check if i dominates j
+				if ( i < *j )
+				{
+					//std::cout << " i:"<< std::endl;
+					//i.print();
+					//std::cout << " j:"<< std::endl;
+					//j->print();
+					j = nondominated_multicosts_.erase(j);
+				}
+				// check if j dominates i: break and look for a new candidate i
+				else if ( *j < i )
+				{
+					is_candidate_dominated = true;
+					break;
+				}
+
+				//else, nothing happens until all set is compared with i
+			}
+		}
+		if( !is_candidate_dominated )
+		{
+			//std::cout << " int if"<< std::endl;
+			nondominated_multicosts_.push_back(i);
+		}
+	}
+	//std::cout << " nondominated_multicosts_.size()="<<nondominated_multicosts_.size()<< std::endl;
+
+
+
+	//std::cout << "size of the non-dominated set = " << set.size() << " / " << end_of_branches_index_.size() << std::endl;
+}
+
+void Cplan_local_nav_person_companion::calculate_normalization_cost_functions_parameters_erf()
+{
+	mean_cost_int_forces_=0.0;
+	mean_cost_robot_=0.0;
+	mean_cost_obstacles_=0.0;
+	mean_cost_past_traj_=0.0;
+	mean_cost_distance_=0.0;
+	mean_cost_orientation_=0.0;
+	mean_cost_companion_=0.0;  // mean companion cost, add by ely.
+
+	double N = (double)end_of_branches_index_.size();
+	for( unsigned int i : end_of_branches_index_ )
+	{
+		mean_cost_int_forces_ += cost_int_forces_[i];
+		mean_cost_robot_ +=cost_robot_[i];
+		mean_cost_obstacles_ +=cost_obstacles_[i];
+		mean_cost_past_traj_ += cost_past_traj_[i];
+		mean_cost_distance_ +=cost_distance_[i];
+		mean_cost_orientation_ +=cost_orientation_[i];
+		mean_cost_companion_ +=cost_companion_[i]; // mean companion cost, add by ely.
+
+		//plot of the entire raw costs
+		/*std::cout << cost_int_forces_[i] << " , " <<
+				cost_robot_[i] << " , " <<
+				cost_obstacles_[i] << " , " <<
+				cost_distance_[i] << " , " <<
+				cost_orientation_[i] <<	std::endl;*/
+		/*std::cout << " cost_int_forces_ ["<<i<<"]="<< cost_int_forces_[i] <<" mean_cost_int_forces_="<< mean_cost_int_forces_ <<std::endl;
+		std::cout << " cost_robot_      ["<<i<<"]="<< cost_robot_[i]      <<" mean_cost_robot_     ="<< mean_cost_robot_      <<std::endl;
+		std::cout << " cost_obstacles_  ["<<i<<"]="<< cost_obstacles_[i]  <<" mean_cost_obstacles_ ="<< mean_cost_obstacles_  <<std::endl;
+		std::cout << " cost_distance_   ["<<i<<"]="<< cost_distance_[i]   <<" mean_cost_distance_  ="<< mean_cost_distance_   <<std::endl;
+		std::cout << " cost_orientation_["<<i<<"]="<< cost_orientation_[i]<<" mean_cost_int_forces_="<< mean_cost_int_forces_ <<std::endl;
+		 */
+	}
+	mean_cost_int_forces_ /= N;
+	mean_cost_robot_ /= N;
+	mean_cost_obstacles_ /= N;
+	mean_cost_past_traj_ /= N;
+	mean_cost_distance_ /= N;
+	mean_cost_orientation_ /= N;
+	mean_cost_companion_ /= N; // mean companion cost, add by ely.
+	/*std::cout <<" N = "<< N <<std::endl;
+	std::cout <<" mean_cost_int_forces_="<< mean_cost_int_forces_ <<std::endl;
+	std::cout <<" mean_cost_robot_     ="<< mean_cost_robot_      <<std::endl;
+	std::cout <<" mean_cost_obstacles_ ="<< mean_cost_obstacles_  <<std::endl;
+	std::cout <<" mean_cost_distance_  ="<< mean_cost_distance_   <<std::endl;
+	std::cout <<" mean_cost_int_forces_="<< mean_cost_int_forces_ <<std::endl;
+	*/
+
+	std_cost_int_forces_=0.0;
+	std_cost_robot_=0.0;
+	std_cost_obstacles_=0.0;
+	std_cost_past_traj_=0.0;
+	std_cost_distance_=0.0;
+	std_cost_orientation_=0.0;
+	std_cost_companion_=0.0; // std companion cost, add by ely.
+
+	double d;
+	N -= 1.0;//unbiased std
+	for( unsigned int i : end_of_branches_index_ )
+	{
+		d = cost_int_forces_[i] - mean_cost_int_forces_;
+		std_cost_int_forces_ += d*d;
+
+		/*std::cout <<" cost_int_forces_["<<i<<"]="<< cost_int_forces_[i] <<"mean_cost_int_forces_="<<mean_cost_int_forces_<<std::endl;
+		std::cout <<" d = (cost_int_forces_[i] - mean_cost_int_forces_) ="<< d <<std::endl;
+		std::cout <<" std_cost_int_forces_ =" << std_cost_int_forces_      <<std::endl;
+		*/
+
+		d = cost_robot_[i] - mean_cost_robot_;
+		std_cost_robot_ += d*d;
+
+		/*std::cout <<" cost_robot_["<<i<<"]="<< cost_robot_[i] <<"mean_cost_robot_="<<mean_cost_robot_<<std::endl;
+		std::cout <<" d = (cost_robot_[i] - mean_cost_robot_) ="<< d <<std::endl;
+		std::cout <<" std_cost_robot_ =" << std_cost_robot_      <<std::endl;
+		*/
+
+		d = cost_obstacles_[i] - mean_cost_obstacles_;
+		std_cost_obstacles_ += d*d;
+
+		/*std::cout <<" cost_obstacles_["<<i<<"]="<< cost_obstacles_[i] <<"mean_cost_obstacles_="<<mean_cost_obstacles_<<std::endl;
+		std::cout <<" d = (cost_obstacles_[i] - mean_cost_obstacles_) ="<< d <<std::endl;
+		std::cout <<" std_cost_obstacles_ =" << std_cost_obstacles_     <<std::endl;
+		*/
+
+		d = cost_past_traj_[i] - mean_cost_past_traj_;
+		std_cost_past_traj_ += d*d;
+
+		/*std::cout <<" cost_past_traj_["<<i<<"]="<< cost_past_traj_[i] <<"mean_cost_past_traj_="<<mean_cost_past_traj_<<std::endl;
+		std::cout <<" d = (cost_past_traj_[i] - mean_cost_past_traj_) ="<< d <<std::endl;
+		std::cout <<" std_cost_past_traj_ =" << std_cost_past_traj_ <<std::endl;
+		*/
+
+		d = cost_distance_[i] - mean_cost_distance_;
+		std_cost_distance_ += d*d;
+
+		/*std::cout <<" cost_distance_["<<i<<"]="<< cost_distance_[i] <<"mean_cost_distance_="<<mean_cost_distance_<<std::endl;
+		std::cout <<" d = (cost_distance_[i] - mean_cost_distance_) ="<< d <<std::endl;
+		std::cout <<" std_cost_distance_ =" << std_cost_distance_      <<std::endl;
+		*/
+
+		d = cost_orientation_[i] - mean_cost_orientation_;
+		std_cost_orientation_ += d*d;
+
+		/*std::cout <<" cost_orientation_["<<i<<"]="<< cost_orientation_[i] <<"mean_cost_orientation__="<<mean_cost_orientation_<<std::endl;
+		std::cout <<" d = (cost_orientation_[i] - mean_cost_orientation_) ="<< d <<std::endl;
+		std::cout <<" std_cost_orientation_ =" << std_cost_orientation_      <<std::endl;
+		*/
+
+		d = cost_companion_[i] - mean_cost_companion_; // std companion cost, add by ely.
+		std_cost_companion_ += d*d; // std companion cost, add by ely.
+	}
+	if( std_cost_int_forces_ > 0.01 )
+	{
+		std_cost_int_forces_ /= N; std_cost_int_forces_ = sqrt(std_cost_int_forces_);
+	}
+	else
+		std_cost_int_forces_ = 0.01;
+	if( std_cost_robot_ > 0.01)
+	{
+		std_cost_robot_ /= N; std_cost_robot_ = sqrt(std_cost_robot_);
+	}
+	else
+		std_cost_robot_ = 0.01;
+	if( std_cost_obstacles_ > 0.01)
+	{
+		std_cost_obstacles_ /= N;std_cost_obstacles_ = sqrt(std_cost_obstacles_);
+	}
+	else
+		std_cost_obstacles_ = 0.01;
+	if( std_cost_past_traj_ > 0.01)
+	{
+		std_cost_past_traj_ /= N; std_cost_past_traj_= sqrt(std_cost_past_traj_);
+	}
+	else
+		std_cost_past_traj_ = 0.01;
+	if( std_cost_distance_ > 0.01)
+	{
+		std_cost_distance_ /= N; std_cost_distance_= sqrt(std_cost_distance_);
+	}
+	else
+		std_cost_distance_ = 0.01;
+	if( std_cost_orientation_ > 0.01)
+	{
+		std_cost_orientation_ /= N; std_cost_orientation_= sqrt(std_cost_orientation_);
+	}
+	else
+		std_cost_orientation_ = 0.01;
+
+	if( std_cost_companion_ > 0.01)  // std companion cost, add by ely.
+	{
+		std_cost_companion_ /= N; std_cost_companion_= sqrt(std_cost_companion_);
+	}
+	else
+		std_cost_companion_ = 0.01;
+
+	//printing results
+	//std::cout << "cost_distance = (" << mean_cost_distance_ << " , " << std_cost_distance_ << std::endl;
+	//std::cout << "cost_orientation = (" << mean_cost_orientation_ << " , " << std_cost_orientation_ << std::endl;
+	//std::cout << "cost_int_forces = (" << mean_cost_int_forces_ << " , " << std_cost_int_forces_ << std::endl;
+	//std::cout << "cost_robot = (" << mean_cost_robot_ << " , " << std_cost_robot_ << std::endl;
+	//std::cout << "cost_obstacles = (" << mean_cost_obstacles_ << " , " << std_cost_obstacles_ << std::endl;
+	//std::cout << "cost_past_traj = (" << mean_cost_past_traj_ << " , " << std_cost_past_traj_ << std::endl;
+	//std::cout << "cost_companion = (" << mean_cost_companion_ << " , " << std_cost_companion_ << std::endl;
+}
+
+void Cplan_local_nav_person_companion::calculate_normalization_cost_functions_parameters_norm(Crobot* robot_act)
+{
+	//calculates the Utopia point considering free space and the max value in order to linearly normalize ->[0,1]
+	mean_cost_distance_=0.0;
+	mean_cost_orientation_=0.0;
+	mean_cost_robot_=0.0;
+	mean_cost_int_forces_=0.0;
+	mean_cost_obstacles_=0.0;
+	mean_cost_past_traj_=0.0;
+	//propagate the robot as if no obstacle is in the scene and calculate costs
+	SpointV robot = robot_act->get_planning_trajectory()->front();
+	Sforce f_goal;
+	while ( robot.time_stamp - now_ < horizon_time_ && robot.distance2( local_goal_ ) > 0.25 )
+	{
+		// potential cost to go destination
+		f_goal = robot_act->force_goal( Sdestination(0,local_goal_.x,local_goal_.y), get_sfm_params(robot_act), &robot );
+
+		// robot propagation
+		robot = robot.propagate( dt_, f_goal, robot_act->get_desired_velocity() );
+
+		//calculate costs
+		mean_cost_robot_ += f_goal.module2(cost_angular_);
+		mean_cost_distance_ += robot.distance2(local_goal_);
+		double o = robot.angle_heading_point( local_goal_ );
+		mean_cost_orientation_ += o*o;
+	}
+
+	//calculate max costs and the scalarization term
+	double max;
+	max = -10000.0;
+	for( unsigned int i : end_of_branches_index_ )
+	{
+		if (max < cost_distance_[i])
+			max = cost_distance_[i];
+	}
+	std_cost_distance_ = max - mean_cost_distance_;
+	if ( std_cost_distance_ < 0.01 ) std_cost_distance_ = 0.01;
+
+	max = -10000.0;
+	for( unsigned int i : end_of_branches_index_ )
+	{
+		if (max < cost_orientation_[i])
+			max = cost_orientation_[i];
+	}
+	std_cost_orientation_ = max - mean_cost_orientation_;
+	if ( std_cost_orientation_ < 0.01 ) std_cost_orientation_ = 0.01;
+
+	max = -10000.0;
+	for( unsigned int i : end_of_branches_index_ )
+	{
+		if (max < cost_robot_[i])
+			max = cost_robot_[i];
+	}
+	std_cost_robot_ = max - mean_cost_robot_;
+	if ( std_cost_robot_ < 0.01 ) std_cost_robot_= 0.01;
+
+	max = -10000.0;
+	for( unsigned int i : end_of_branches_index_ )
+	{
+		if (max < cost_int_forces_[i])
+			max = cost_int_forces_[i];
+	}
+	std_cost_int_forces_ = max - mean_cost_int_forces_;
+	if ( std_cost_int_forces_ < 0.01 ) std_cost_int_forces_ = 0.01;
+
+	max = -10000.0;
+	for( unsigned int i : end_of_branches_index_ )
+	{
+		if (max < cost_obstacles_[i])
+			max = cost_obstacles_[i];
+	}
+	std_cost_obstacles_ = max - mean_cost_obstacles_;
+	if ( std_cost_obstacles_ < 0.01 ) std_cost_obstacles_ = 0.01;
+
+	max = -10000.0;
+	for( unsigned int i : end_of_branches_index_ )
+	{
+		if (max < cost_past_traj_[i])
+			max = cost_past_traj_[i];
+	}
+	std_cost_past_traj_= max - mean_cost_past_traj_;
+	if ( std_cost_past_traj_< 0.01 ) std_cost_past_traj_= 0.01;
+
+	//printing results
+	//std::cout << "cost_distance = (" << mean_cost_distance_ << " , " << std_cost_distance_ << std::endl;
+	//std::cout << "cost_orientation = (" << mean_cost_orientation_ << " , " << std_cost_orientation_ << std::endl;
+	//std::cout << "cost_int_forces = (" << mean_cost_int_forces_ << " , " << std_cost_int_forces_ << std::endl;
+	//std::cout << "cost_robot = (" << mean_cost_robot_ << " , " << std_cost_robot_ << std::endl;
+	//std::cout << "cost_obstacles = (" << mean_cost_obstacles_ << " , " << std_cost_obstacles_ << std::endl;
+	//std::cout << "cost_past_traj = (" << mean_cost_past_traj_ << " , " << std_cost_past_traj_ << std::endl;
+}
+
+
+Spose Cplan_local_nav_person_companion::get_best_planned_pose_person_companion_akp(double dt)
+{
+	// TODO: arreglar mejor el que la person_companion, tenga en cuenta la second person companion!
+	// de momento esta solo reactivo! No usa el path para predecir donde posiciomnarse segun predicciones.
+	// Solucion rápida para ver si funciona. Cuando este to_do hay que arreglarlo bien!!!
+	//returns the best planned pose at time
+	if(debug_get_best_planned_pose_person_companion_akp_){
+		std::cout << "INI get_best_planned_pose_person_companion_akp (person_companion)" <<  std::endl;
+	}
+
+	Spose returned_pose;
+	int index(0);
+	if ( dt < dt_)
+	   // index = 1;
+		 index = 1;
+	else
+	    index = (int) (dt / dt_);
+
+
+	actual_best_path_index_=index;
+
+	//std::cout << "(get_best_planned_pose_person_companion_akp) dt="<<dt<<"; dt_="<<dt_<<"; index="<<index<<"; best_plan_vertex_index_.empty()="<<best_plan_vertex_index_.empty()<<"; best_plan_vertex_index_.size()="<<best_plan_vertex_index_.size()<<  std::endl;
+	if( !best_plan_vertex_index_.empty() && (unsigned)index < best_plan_vertex_index_.size() )
+	{
+		Spose pose_Act_person=ini_spose_person_companion_akp_;
+		SpointV_cov Spoint_Act_person=ini_point_person_companion_akp_;
+
+		if(debug_get_best_planned_pose_person_companion_akp_){
+			std::cout << "(ANT) pose_Act_person:" <<  std::endl;
+			pose_Act_person.print();
+			std::cout << "Spoint_Act_person:" <<  std::endl;
+			Spoint_Act_person.print();
+		}
+
+		unsigned int next_plan_index = best_plan_vertex_index_.at( best_plan_vertex_index_.size() - index );
+		returned_pose=person_companion_->get_robot_planning_trajectory()->at(next_plan_index);
+
+
+		if(debug_get_best_planned_pose_person_companion_akp_){
+			std::cout << " (LAST) next_plan_index="<<next_plan_index <<  std::endl;
+			std::cout << "best_plan_vertex_index_.size() - index="<<best_plan_vertex_index_.size() - index <<  std::endl;
+			std::cout << "(ini) returned_pose.print():" <<  std::endl;
+			returned_pose.print();
+		}
+
+
+		if(bool_case_person_companion_){
+
+			double v; //double v=person_companion_desired_velocity_sim_;
+			// opcion 3.
+
+			if((returned_pose.v)>=(person_companion_desired_velocity_sim_)){
+				//std::cout << "ENTRO EN if (returned_pose.v)<=(person_companion_desired_velocity_sim_)" <<  std::endl;
+				v=person_companion_desired_velocity_sim_;
+			}else if((returned_pose.v)<=(-person_companion_desired_velocity_sim_)){
+				//std::cout << "ENTRO EN else if (returned_pose.v)<=(-person_companion_desired_velocity_sim_)" <<  std::endl;
+				v=-person_companion_desired_velocity_sim_;
+			}else{
+				//std::cout << "ENTRO EN else (returned_pose.v)<=(-person_companion_desired_velocity_sim_)" <<  std::endl;
+				v=returned_pose.v;
+			}
+
+			//Spose pose_Act_person=ini_spose_person_companion_akp_;
+			SpointV_cov Spoint_Act_person=ini_point_person_companion_akp_;
+			if(debug_get_best_planned_pose_person_companion_akp_){
+				std::cout << "(ANT) returned_pose.print():" <<  std::endl;
+				returned_pose.print();
+				std::cout << "(simulated vel) v=person_companion_desired_velocity_sim_"<<person_companion_desired_velocity_sim_ <<  std::endl;
+				std::cout << "robot_->get_desired_velocity()="<<robot_->get_desired_velocity()<<  std::endl;
+			}
+			//returned_pose=Spose(ini_spose_person_companion_akp_.x+returned_pose.x*(person_companion_desired_velocity_sim_/robot_->get_desired_velocity()),ini_spose_person_companion_akp_.y+returned_pose.y*(person_companion_desired_velocity_sim_/robot_->get_desired_velocity()), returned_pose.time_stamp, returned_pose.theta, v, returned_pose.w);
+			double inc_x=(returned_pose.x-ini_spose_person_companion_akp_.x)*(person_companion_desired_velocity_sim_/robot_->get_desired_velocity());
+			double inc_y=(returned_pose.y-ini_spose_person_companion_akp_.y)*(person_companion_desired_velocity_sim_/robot_->get_desired_velocity());
+
+			double x= ini_spose_person_companion_akp_.x + inc_x;
+			double y= ini_spose_person_companion_akp_.y + inc_y;
+			returned_pose=Spose(x,y, returned_pose.time_stamp, returned_pose.theta, v, returned_pose.w);
+
+			if(debug_get_best_planned_pose_person_companion_akp_){
+				std::cout << "(DESPUES) returned_pose.print():" <<  std::endl;
+				returned_pose.print();
+			}
+		}
+	}
+	else{
+		//std::cout << "else Spose() get_best_planned_pose_person_companion_akp." <<  std::endl;
+		//returned_pose=Spose();
+		returned_pose=person_companion_->get_current_pose(); // en el caso del simulador de persona, si no hay plan. se usa la posicion anterior!
+	}
+
+	//std::cout << "OUT REPLAN LAST STEP; returned_pose.print(): " <<  std::endl;
+	//returned_pose.print();
+
+	return returned_pose;
+}
+
+
+
+void Cplan_local_nav_person_companion::
+get_navigation_instant_work( double& work_robot, double& work_persons )
+{
+	work_robot = work_robot_;
+	work_persons = work_persons_;
+}
+
+
+void Cplan_local_nav_person_companion::
+calculate_navigation_instant_work( )
+{
+	work_robot_ = 0.0;
+	if( best_plan_vertex_index_.size() > 2 )
+	{
+		work_robot_ =  fabs( edge_.at(best_plan_vertex_index_.at( best_plan_vertex_index_.size()-2 )).f  *
+			robot_->get_diff_position() );
+	}
+	else
+	{
+		work_robot_ = 0.0;
+	}
+
+	work_persons_ = 0.0;
+	for( Cperson_abstract* iit : nearby_person_list_)
+	{
+		if ( iit->get_current_pointV().distance( robot_->get_current_pointV() ) < workspace_radii_ )
+			work_persons_ += fabs( iit->force( robot_->get_current_pointV() ,
+					get_sfm_int_params(iit,robot_) ) * iit->get_diff_position() );
+	}
+}
+
+
+void Cplan_local_nav_person_companion::set_robot_params( double v, double w, double av, double av_break, double aw, double platform_radii)
+{
+	robot_->set_v_max( v );
+	max_v_by_system_ = v;//this velocity is modified depending on the density of nearby obstacles
+	robot_->set_w_max( w );
+	robot_->set_a_v_max( av );
+	robot_->set_a_v_break( av_break );
+	robot_->set_a_w_max( aw );
+	robot_->set_platform_radii( platform_radii);
+}
+
+//TODO update to new cost-to-go function and elimiate DEPRECATED parameters
+void Cplan_local_nav_person_companion::set_plan_cost_parameters( double c_dist, double c_orientation, double c_w_robot,
+		double c_w_people, double c_time, double c_w_obstacles, double c_old_path, double c_l_minima)
+{
+    cost_parameters_[0] = c_dist;// [0] Goal cost
+    cost_parameters_[1] = c_orientation;// [1] orientation cost
+    cost_parameters_[2] = c_w_robot;// [2] Robot cost
+    cost_parameters_[3] = c_w_people;// [3]Interacting people cost
+    cost_parameters_[4] = c_time;// [4] potential time
+    cost_parameters_[5] = c_w_obstacles;// [5] obstacles cost
+    cost_parameters_[6] = c_old_path;// [6] past function cost
+    cost_parameters_[7] = c_l_minima;// [7] local minima scape cost
+}
+
+//only use when no plan is calculated, as an evaluation of the observed performance
+// of this method or any navigation method
+void Cplan_local_nav_person_companion::calculate_navigation_cost_values( std::vector<double>& costs )
+{
+	costs.resize(4,0.0);
+	// calculate robot cost --------------------------------------------------------------------------------
+	Spose dr = robot_->get_diff_pose();
+	costs[0] = dr.v* dr.v + dr.w*dr.w;
+	costs[0] /= dt_*dt_;
+
+
+	// calculate ppl cost --------------------------------------------------------------------------------
+	Sforce f,f_int;
+	double cost = 0.0;
+	for( Cperson_abstract* iit: person_list_  )
+	{
+		f = iit->force_sphe(robot_->get_current_pointV() , get_sfm_int_params(iit,robot_)  );
+		f_int += f;
+		cost += f.module2();
+	}
+	costs[1] = cost;
+
+	// calculate obstacles cost  --------------------------------------------------------------------------------
+	Sforce f_obs = force_objects_laser_int_planning_virtual( robot_, 0, 25.0, false );//false set for no new nearby_obstacles list to be refilled
+	// INICION Limitar fuerza obstaculos cuando hay muchos solapados
+				double fobos_Xmod=sqrt(f_obs.fx*f_obs.fx);
+				double fobos_Ymod=sqrt(f_obs.fy*f_obs.fy);
+				double signo_x;
+				double signo_y;
+				if (f_obs.fx > 0){
+					signo_x=1;
+				}else{
+					signo_x=-1;
+				}
+				if (f_obs.fy > 0){
+					signo_y=1;
+				}else{
+					signo_y=-1;
+				}
+
+				if((fobos_Xmod>f_obst_max_x_)||(fobos_Ymod>f_obst_max_y_)){
+					f_obs.fx=signo_x*f_obst_max_x_;
+					f_obs.fy=signo_y*f_obst_max_y_;
+				}
+				//std::cout << "(FINAL FORCE) f_obs.fx="<<f_obs.fx<<"; f_obs.fy="<<f_obs.fy<< std::endl;
+	// FIN Limitar fuerza obstaculos cuando hay muchos solapados
+
+
+	costs[2] = f_obs.module2();
+
+
+	// calculate average velocity --------------------------------------------------------------------------------
+	costs[3] = robot_->get_current_pose().v;
+
+}
+
+Sedge_tree_pcomp::Sedge_tree_pcomp(	unsigned int parent_, Sforce f_, Sforce f_goal_,
+		Sforce f_people_,Sforce f_obs_, Sforce f_persongoal_):
+		parent(parent_), f(f_), f_goal(f_goal_), f_people(f_people_), f_obs(f_obs_),f_persongoal(f_persongoal_)
+{
+
+}
+double Sedge_tree_pcomp::cost( Sedge_tree_pcomp parent) const
+{
+	//TODO
+	return 0.0;
+}
+void Sedge_tree_pcomp::print()
+{
+	std::cout << "parent vertex = " << parent << std::endl;
+	f.print();
+}
+
+Smulticost_pcom::Smulticost_pcom( unsigned int id_, unsigned int n ) :
+	id(id_)
+{
+	cost.resize( n, 0.0 );
+}
+
+bool Smulticost_pcom::operator< ( const Smulticost_pcom& m2) const
+{
+	//dominance operator. Returns true if m dominates m2
+	double thr(1e-5);
+	bool one_better(false);
+	for( unsigned int i = 0; i<cost.size(); ++i )
+	{
+		//for numerical stability, equality is cheked:  x-x2 < thr
+		if ( cost[i] - thr >  m2.cost[i] )
+			return false;
+		// check for at least one better cost i
+		else if ( cost[i] + thr <  m2.cost[i] )
+			one_better = true;
+		//else: equal both costs, needs more comparisons
+
+	}
+	return one_better;//requires that at least one was better
+}
+
+bool Smulticost_pcom::operator== ( const Smulticost_pcom& m2) const
+{
+	if( id == m2.id )
+		return true;
+	else
+		return false;
+}
+
+
+bool Smulticost_pcom::operator!= ( const Smulticost_pcom& m2) const
+{
+	return !(*this==m2);
+}
+
+void Smulticost_pcom::print ( ) const
+{
+	std::cout << "Multicost " << id << ", costs = {";
+	for( double i: cost )
+		std::cout << i << " , ";
+	std::cout << " } " << std::endl;
+}
+
+void Smulticost_pcom::print_ml ( ) const
+{
+	std::cout << id ;
+	for( double i: cost )
+		std::cout << ", "<< i;
+	std::cout << std::endl;
+}
+
+
+
+
+/*** Ini Check collision companion!  (ely-modificada, solo, esta vez para calculo colisiones robot goal respecto persona, cuando esta lejos de esta) ***/
+double Cplan_local_nav_person_companion::check_collision_companion_goal(const Spoint& p ,  int t)
+// return if have collision and the  minimum distance of the collision
+{
+
+	clock_t check_collision_companion_goal_start, check_collision_companion_goal_end;
+	check_collision_companion_goal_start = clock();
+
+	if(debug_companion_){
+		std::cout << "INI function: check_collision_companion ="<< std::endl;
+	}
+
+	double min_distance_collision_act=robot_person_companion_distance_; // initial equal to the máx distance. (robot+person) =pasan bien.
+
+	if(debug_companion_){
+		std::cout << "min_distance_collision_==robot_person_companion_distance_act= "<<min_distance_collision_act << std::endl;
+	}
+	std::vector<double> min_distances_vector;
+	min_distances_vector.clear();
+
+	bool collision=false;
+	 //TODO does not work properly on real scenarios! long term issue
+	//check colision with map: TO BE DEPRECATED
+	if ( read_force_map_success_ && !is_cell_clear_map( p.x, p.y ) )
+	{
+		collision=true; // colision, pero por choque con el mapa, TODO: mirar como tratar esto...
+
+	}
+
+// TODO: (ely) Igualmente, independientemente del modo que use gonzalo para calcular colisiones, yo podría usar el modo normal,
+//de distancia cuadratica, si veo mejor que pasa...
+
+	//check collision with obstacles (laser), prior is necessary to calculate force_objects_laser_int_planning()
+	//for( Spoint iit : nearby_obstacle_list_ )//TODO not working properly since the c2g recalculates this vector
+	for( Spoint iit : laser_obstacle_list_ )
+	{
+		double act_distance_obst_collision=sqrt((iit.distance( p )-obstacle_radi_amp_)*(iit.distance( p )-obstacle_radi_amp_));
+		if( act_distance_obst_collision  < robot_person_companion_distance_ )
+		{
+			//std::cout << "Collision found!!" << robot_->get_platform_radii_2() << std::endl;
+
+			min_distances_vector.push_back(act_distance_obst_collision);
+			collision=true;
+		}
+	}
+	t=0;
+	//if t = -1, then no collision is calculated wrt ppl
+	if ( t > -1 )
+	{
+		double d,det;
+		switch( ppl_collision_mode_ )
+		{
+		  case 0: //normal mode, quadratic distance to person, independent of distribution
+			  if(debug_companion_){
+			  std::cout << "case 0 (ppl_collision_mode_)" << std::endl;
+			  }
+			for( auto iit : nearby_person_list_)
+			{
+
+				//if(!group_go_to_interact_with_other_person_){ // si es id_person companion + caso zanlungo ha de entrar.
+				//if(!Zanlungo_model_){
+					// ini side-by-side:
+					if(iit->get_id()!=id_person_companion_){ // add due to person companion, esta persona no hay que calcular colisiones respecto a ella.
+						d = sqrt((iit->get_planning_trajectory( )->at(t).distance( p )-person_radi_amp_)*(iit->get_planning_trajectory( )->at(t).distance( p )-person_radi_amp_));
+
+						if( d < robot_person_companion_distance_ )
+						{
+							collision=true;
+							min_distances_vector.push_back(d);
+						}
+					}
+					// fin side-by-side:
+				/*}else{
+					d = sqrt((iit->get_planning_trajectory( )->at(t).distance( p )-person_radi_amp_)*(iit->get_planning_trajectory( )->at(t).distance( p )-person_radi_amp_));
+
+					if( d < robot_person_companion_distance_ )
+					{
+						collision=true;
+						min_distances_vector.push_back(d);
+					}
+				}*/
+				/*}else
+				 * }{
+					if((iit->get_id()!=id_person_companion_)&&(iit->get_id()!=id_person_goal_)){ // add due to person companion, esta persona no hay que calcular colisiones respecto a ella.
+						d = iit->get_planning_trajectory( )->at(t).distance( p )-person_radi_amp_;
+
+						if( d < robot_person_companion_distance_ )
+						{
+							collision=true;
+							min_distances_vector.push_back(d);
+						}
+					}
+				}*/
+
+
+
+			}
+			break;
+		  case 1://mahalanobis distance, considers collision if inside the std ellipsoid: TOO RESTRICTIVE...
+			  if(debug_companion_){
+				  std::cout << "case 1 (ppl_collision_mode_)" << std::endl;
+			  }
+			for( auto iit : nearby_person_list_) // de momento, para mi todas las suyas con constantes, han de ser menores que la distancia robot+persona. (luego ya veremos si va bien o no)
+			{
+
+				//if(!group_go_to_interact_with_other_person_){
+				//if(!Zanlungo_model_){
+					// ini case side-by-side:
+					if(iit->get_id()!=id_person_companion_){
+						d = sqrt((iit->get_planning_trajectory( )->at(t).cov_dist(p,det)-person_radi_amp_)*(iit->get_planning_trajectory( )->at(t).cov_dist(p,det)-person_radi_amp_));
+						if ( d < robot_person_companion_distance_ )
+						{
+							//std::cout << "Colision detected, pr distance = " << d << std::endl;
+							collision=true;
+							min_distances_vector.push_back(d);
+
+						}
+					}
+					// fin case side-by-side:
+				/*}else{
+					d = sqrt((iit->get_planning_trajectory( )->at(t).cov_dist(p,det)-person_radi_amp_)*(iit->get_planning_trajectory( )->at(t).cov_dist(p,det)-person_radi_amp_));
+					if ( d < robot_person_companion_distance_ )
+					{
+						//std::cout << "Colision detected, pr distance = " << d << std::endl;
+						collision=true;
+						min_distances_vector.push_back(d);
+
+					}
+
+				}*/
+				/*}else{
+					if((iit->get_id()!=id_person_companion_)&&(iit->get_id()!=id_person_goal_)){
+						d = sqrt((iit->get_planning_trajectory( )->at(t).cov_dist(p,det)-person_radi_amp_)*(iit->get_planning_trajectory( )->at(t).cov_dist(p,det)-person_radi_amp_));
+						if ( d < robot_person_companion_distance_ )
+						{
+							//std::cout << "Colision detected, pr distance = " << d << std::endl;
+							collision=true;
+							min_distances_vector.push_back(d);
+
+						}
+					}
+				}*/
+
+			}
+			break;
+		  case 2 ://mahalanobis distance, half the std, much less restrictive
+			  if(debug_companion_){
+				  std::cout << "case 2 (ppl_collision_mode_)" << std::endl;
+			  }
+			for( auto iit : nearby_person_list_)
+			{
+
+				//if(!group_go_to_interact_with_other_person_){
+				//if(!Zanlungo_model_){
+					// ini case-side-by-side:
+					if(iit->get_id()!=id_person_companion_){
+						d = sqrt((iit->get_planning_trajectory( )->at(t).cov_dist(p,det)-person_radi_amp_)*(iit->get_planning_trajectory( )->at(t).cov_dist(p,det)-person_radi_amp_));
+						if ( det > 1.0)
+						{
+							if ( d < robot_person_companion_distance_)
+							{
+								collision=true;
+								min_distances_vector.push_back(d);
+							}
+						}
+						else
+						{
+							d = iit->get_planning_trajectory( )->at(t).distance( p )-person_radi_amp_;
+							if ( d < robot_person_companion_distance_)
+							{
+								collision=true;
+								min_distances_vector.push_back(d);
+							}
+						}
+					}
+					// fin case-side-by-side:
+				/*}else{
+					d = sqrt((iit->get_planning_trajectory( )->at(t).cov_dist(p,det)-person_radi_amp_)*(iit->get_planning_trajectory( )->at(t).cov_dist(p,det)-person_radi_amp_));
+					if ( det > 1.0)
+					{
+						if ( d < robot_person_companion_distance_)
+						{
+							collision=true;
+							min_distances_vector.push_back(d);
+						}
+					}
+					else
+					{
+						d = iit->get_planning_trajectory( )->at(t).distance( p )-person_radi_amp_;
+						if ( d < robot_person_companion_distance_)
+						{
+							collision=true;
+							min_distances_vector.push_back(d);
+						}
+					}
+				}*/
+				/*}else{ // fin go targ pers
+					if((iit->get_id()!=id_person_companion_)&&(iit->get_id()!=id_person_goal_)){
+						d = sqrt((iit->get_planning_trajectory( )->at(t).cov_dist(p,det)-person_radi_amp_)*iit->get_planning_trajectory( )->at(t).cov_dist(p,det)-person_radi_amp_);
+						if ( det > 1.0)
+						{
+							if ( d < robot_person_companion_distance_)
+							{
+								collision=true;
+								min_distances_vector.push_back(d);
+							}
+						}
+						else
+						{
+							d = iit->get_planning_trajectory( )->at(t).distance( p )-person_radi_amp_;
+							if ( d < robot_person_companion_distance_)
+							{
+								collision=true;
+								min_distances_vector.push_back(d);
+							}
+						}
+					}
+				}*/
+
+			}
+			break;
+		  case 3 :
+			  if(debug_companion_){
+				  std::cout << "case 3 (ppl_collision_mode_)" << std::endl;
+			  }
+			for( auto iit : nearby_person_list_)
+			{
+
+
+				//if(!group_go_to_interact_with_other_person_){
+				//if(!Zanlungo_model_){
+					// ini side-by-side
+					if(iit->get_id()!=id_person_companion_){
+						d = sqrt((iit->get_planning_trajectory( )->at(t).cov_dist(p,det)-person_radi_amp_)*(iit->get_planning_trajectory( )->at(t).cov_dist(p,det)-person_radi_amp_));
+
+						if ( det > 1.0)
+						{
+							if ( d < robot_person_companion_distance_)
+							{
+								collision=true;
+								min_distances_vector.push_back(d);
+							}
+						}
+						else
+						{
+							d = iit->get_planning_trajectory( )->at(t).distance( p );
+							if ( d < robot_person_companion_distance_)
+							{
+								collision=true;
+								min_distances_vector.push_back(d);
+
+							}
+						}
+					}
+
+					// fin side-by-side
+				/*}else{
+					d = sqrt((iit->get_planning_trajectory( )->at(t).cov_dist(p,det)-person_radi_amp_)*(iit->get_planning_trajectory( )->at(t).cov_dist(p,det)-person_radi_amp_));
+
+					if ( det > 1.0)
+					{
+						if ( d < robot_person_companion_distance_)
+						{
+							collision=true;
+							min_distances_vector.push_back(d);
+						}
+					}
+					else
+					{
+						d = iit->get_planning_trajectory( )->at(t).distance( p );
+						if ( d < robot_person_companion_distance_)
+						{
+							collision=true;
+							min_distances_vector.push_back(d);
+
+						}
+					}
+				}*/
+				/*}else{ // fin go targ pers
+					if((iit->get_id()!=id_person_companion_)&&(iit->get_id()!=id_person_goal_)){
+						d = sqrt((iit->get_planning_trajectory( )->at(t).cov_dist(p,det)-person_radi_amp_)*(iit->get_planning_trajectory( )->at(t).cov_dist(p,det)-person_radi_amp_));
+
+						if ( det > 1.0)
+						{
+							if ( d < robot_person_companion_distance_)
+							{
+								collision=true;
+								min_distances_vector.push_back(d);
+							}
+						}
+						else
+						{
+							d = iit->get_planning_trajectory( )->at(t).distance( p );
+							if ( d < robot_person_companion_distance_)
+							{
+								collision=true;
+								min_distances_vector.push_back(d);
+
+							}
+						}
+					}
+				}*/
+
+
+			}
+			break;
+
+		}
+
+			//calculate probabilities. NOT implemented, insignificant collision values when cov is high
+            //pr_no_col *= 1 - gaussian_constant_ / sqrt(det) * exp( -0.5*d );// * robot_->get_platform_radii_2();//area = pi * r^2 (pi included in gaussian_contant)
+            //std::cout << "Pr of no colision joint = " << pr_no_col << " and distance = " << d << " det = " << det << std::endl;
+	}
+
+
+	// obtain minimun collision distance.
+	if(debug_companion_){
+		std::cout << "min_distances_vector.size()= "<<min_distances_vector.size()<< std::endl;
+	}
+	if(!min_distances_vector.empty()){ // si hay alguna distancia minima.
+		double min_dist=min_distances_vector[0];
+		//double min_angle=min_angles_colisions_[0];
+		for( unsigned int in=0; in<min_distances_vector.size(); in++){
+			if(debug_companion_){
+				std::cout << "min_distances_vector["<<in<<"]= "<<min_distances_vector[in]<< std::endl;
+			}
+			if(min_distances_vector[in]<min_dist){
+				min_dist=min_distances_vector[in];
+				//min_angles_colisions_=min_angles_colisions_[in];
+			}
+		}
+
+		min_distance_collision_act=min_dist; // sacar fuera la min dist.
+	}
+	if(debug_companion_){
+		std::cout << "min_distance_collision_act= "<<min_distance_collision_act<< std::endl;
+		std::cout << "collision= "<<collision << std::endl;
+		std::cout << "FIN function: check_collision_companion " << std::endl;
+	}
+
+	check_collision_companion_goal_end = clock();
+	if(check_execution_times_){
+		std::cout << "time_check_collision_companion_goal="<<((check_collision_companion_goal_start-check_collision_companion_goal_end)/clocks_per_sec_my_var_)*1000 << std::endl;
+	}
+	return min_distance_collision_act;
+}
+
+
+
+/*** INI calculate_companion_cost_node! ***/
+void Cplan_local_nav_person_companion::calculate_companion_cost_node( unsigned int parent_index, Crobot* robot_act)
+{
+	if(debug_companion_){
+	 std::cout << "INI Function: calculate_companion_cost_node" << std::endl;
+	}
+
+
+		double final_min_colision_distance=0.0;
+		bool colision=false; // se inicializa a falso.
+		std::vector<double> min_colision_distances;
+		if(debug_companion_){
+			std::cout << "robot_person_companion_distance_="<<robot_person_companion_distance_ << std::endl;
+		}
+
+		// ver si hay colision en la circunferencia de radio ROBOT+Persona. Si hay algun obstaculo estatico o persona, prediccion dentro de esa circunferencia.
+		// (check_collision( const Spoint& p=robot_creo ,  int t)) => la tendré que modificar para que devuelva la distancia de colision, tambien!
+		// la check_collision ya tiene los bucles siguientes que hacian falta.
+		// collision check for static objects    (si hay alguno dentro de esa circuferencia => se cambia el bool a true y se guarda esa distancia en el vector, min_distances.
+		// collision check for dynamic persons
+
+		//colision=check_collision_companion( robot_act->get_planning_trajectory()->back(), parent_index );
+		//check_collision_companion( robot_act->get_planning_trajectory()->back(), parent_index );
+
+		colision=bool_collision_companion_;
+
+		if(debug_companion_){
+			std::cout << "parent_index="<<parent_index<< std::endl;
+			std::cout << "colision="<<colision<< std::endl;
+		}
+
+		// busco la distancia minima de todas las distancias de colision.
+
+		if(colision){
+			final_min_colision_distance=min_distance_collision_; // = distance entre robot y obstaculo más cercano.
+			min_distance_collision_vector_.push_back(min_distance_collision_);
+			//std::cout << " si colision !!!!!!  (calculate_companion_cost_node) colision="<<colision<<"; final_min_colision_distance="<<final_min_colision_distance<< std::endl;
+
+		}else{
+			final_min_colision_distance=robot_person_companion_distance_;
+			min_distance_collision_vector_.push_back(robot_person_companion_distance_);
+			//std::cout << " No colision !!!!!!  (calculate_companion_cost_node) colision="<<colision<<"; final_min_colision_distance="<<final_min_colision_distance<< std::endl;
+
+		}
+		if(debug_companion_){
+			std::cout << "final_min_colision_distance="<<final_min_colision_distance<< std::endl;
+		}
+		// calculate the angle between robot and person.
+		double angle=angle_companion_; // in grad.
+
+		double robot_radi=robot_->get_platform_radii();
+		double real_robot_person_distance2;
+		if(bool_case_person_companion_){
+			real_robot_person_distance2=calc_robot_person_companion_distance_companion_person_akp();
+		}else{
+			real_robot_person_distance2=calc_robot_person_companion_distance();
+		}
+
+
+
+		if((final_min_colision_distance==robot_person_companion_distance_)||(final_min_colision_distance>robot_person_companion_distance_)){
+		//if((final_min_colision_distance==(2*((real_robot_person_distance/2)+2*robot_radi)))||(final_min_colision_distance>(2*((real_robot_person_distance/2)+2*robot_radi)))){
+		//if((final_min_colision_distance==(real_robot_person_distance+4*robot_radi+little_augmented_collision_margin_))||(final_min_colision_distance>(real_robot_person_distance+4*robot_radi+little_augmented_collision_margin_))){
+			angle=angle_companion_;
+
+			//std::cout << " entro en if(...) => angle=angle_companion_"<< std::endl;
+
+		}else{
+			//angle=(180/3.14)*(asin((final_min_colision_distance/2)/(robot_person_companion_distance_/2))); //asin((final_min_colision_distance/2)/((2*radio_robot)/2)) ; 2*radio_robot= robot_person_companion_distance_;
+			//angle=(180/3.14)*(asin((final_min_colision_distance)/(robot_person_companion_distance_)));
+			if(overpas_obstacles_behind_person_){
+				//orientation_person_robot_angles_.push_back(180-angle);
+				//angle=(180/3.14)*(asin((final_min_colision_distance)/(robot_person_companion_distance_))); //prueba 1.
+				//double division=(2*final_min_colision_distance)/(robot_person_proximity_distance_+2*robot_radi+little_augmented_collision_margin_);
+				double division;
+				//double division2;
+
+				if(case_dynamic_){
+					//double radi_force_influence=0.71; // la A! para las personas. Que no influya la fuerza de las personas.
+					//std::cout << " entro an case dynamic"<< std::endl;
+					//division=(final_min_colision_distance-robot_radi-radi_force_influence)/(real_robot_person_distance2/2); //revisar el caso dynamico!
+					//division=((final_min_colision_distance/2)-robot_radi-(little_augmented_collision_margin_/2))/((real_robot_person_distance2+2*robot_radi+little_augmented_collision_margin_)/2);
+					if((final_min_colision_distance)-robot_radi-(little_augmented_collision_margin_/2)<0){
+						division=0;
+					}else{
+						division=((final_min_colision_distance)-robot_radi-(little_augmented_collision_margin_/2))/((real_robot_person_distance2+2*robot_radi+little_augmented_collision_margin_)/2);
+					}
+
+				}else{
+					//std::cout << " entro an case NO dynamic"<< std::endl;
+					// en formulacion => dw/Ri => dw=d_real_collision/2-Rr-free_space_margin/2
+					// Ri = (d_real_between_r_and_p +2Rr+Free_space_margin)/2 =>free_space_margin==little_augmented_collision_margin_
+					//division=((final_min_colision_distance/2)-robot_radi-(little_augmented_collision_margin_/2))/((real_robot_person_distance2+2*robot_radi+little_augmented_collision_margin_)/2);
+					if((final_min_colision_distance)-robot_radi-(little_augmented_collision_margin_/2)<0){
+						division=0;
+					}else{
+						division=((final_min_colision_distance)-robot_radi-(little_augmented_collision_margin_/2))/((real_robot_person_distance2+2*robot_radi+little_augmented_collision_margin_)/2);
+					}
+					//division=(final_min_colision_distance)/(real_robot_person_distance2+2*robot_radi+little_augmented_collision_margin_);
+					//division=(final_min_colision_distance)/((real_robot_person_distance2/2)+robot_radi+(little_augmented_collision_margin_/2));
+					//division=(final_min_colision_distance)/((real_robot_person_distance2/2)+robot_radi+(1/2));
+					//division=(final_min_colision_distance-robot_radi)/(real_robot_person_distance2/2);
+					//division2=((final_min_colision_distance/2)-robot_radi-(little_augmented_collision_margin_/2))/((robot_person_distance_+2*robot_radi+little_augmented_collision_margin_)/2);
+					//std::cout << " final_min_colision_distance="<<final_min_colision_distance<<"; robot_radi="<<robot_radi<<"real_robot_person_distance2="<<real_robot_person_distance2<<" little_augmented_collision_margin_="<<little_augmented_collision_margin_<< std::endl;
+				}
+
+				//std::cout << " entro en else (...) => division="<<division<<"; case_dynamic_="<<case_dynamic_<< std::endl;
+
+				if(division>1){
+					division=1;
+				}else if(division<(-1)){
+					division=-1;
+				}
+
+				//std::cout << " entro en else (...) => division="<<division<<" division2="<<division2<< std::endl;
+
+				angle=(180/3.14)*(asin(division)); //prueba 1.
+
+				//double angle2 =(180/3.14)*(asin(division2));
+				//std::cout << "(1) angle="<<angle<<"; angle2="<<angle2<< std::endl;
+
+				if(angle>angle_companion_){
+					//std::cout << "!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! OJO!!!! ENTRO EN if(angle>angle_companion_)"<< std::endl;
+					//std::cout << " (if angle>angle_companion_   Caso angle=angle_companion_) angle="<<angle<<"angle_companion_="<<angle_companion_<< std::endl;
+					angle=angle_companion_;
+				}
+				//std::cout << "(2) angle="<<angle<< std::endl;
+
+				//std::cout << "parent_index="<<parent_index<< std::endl;
+				//std::cout << "colision="<<colision<< std::endl;
+				//std::cout << " [PASAR POR DETRAS] !!!! (ant) (test, colision) angle="<<angle<<" final_min_colision_distance="<<final_min_colision_distance<<"; robot_person_proximity_distance_="<<robot_person_proximity_distance_<< std::endl;
+				//std::cout << " [PASAR POR DETRAS] !!!! (ant) (test, colision) angle="<<angle<<" final_min_colision_distance/2="<<final_min_colision_distance/2<<"; (robot_person_proximity_distance_/2)+2*robot_radi)="<<(robot_person_proximity_distance_/2)+2*robot_radi<< std::endl;
+
+				angle=180-angle;
+				//std::cout << "(3) angle="<<angle<< std::endl;
+
+				//std::cout << " angle2="<<angle<< std::endl;
+
+				if(angle<angle_companion_){
+					angle=angle_companion_;
+					//std::cout << "angle<angle_companion_=> angle=angle_companion"<< std::endl;
+				}
+				//std::cout << "(4) angle="<<angle<< std::endl;
+
+				if(angle>180){
+					angle=180;
+					//std::cout << "angle>180=> angle=180"<< std::endl;
+				}
+				//std::cout << "(5) angle="<<angle<< std::endl;
+
+
+				//std::cout << " [overpas_obstacles_behind_person_] final_min_colision_distance="<<final_min_colision_distance<<"; robot_radi="<<robot_radi<<" little_augmented_collision_margin_="<<little_augmented_collision_margin_<<"; real_robot_person_distance2="<<real_robot_person_distance2<< std::endl;
+
+				//std::cout << " [overpas_obstacles_behind_person_] division="<<division<<" angle="<<angle<< std::endl;
+
+				//std::cout << "[overpas_obstacles_behind_person_] angle="<<angle<<"angle_companion_="<<angle_companion_<< std::endl;
+				//std::cout << " [PASAR POR DETRAS] !!!! (despues) (test, colision) angle="<<angle<< std::endl;
+				// TODO: temporal!!! el angulo solo va de 0 a angle_companion_=90grados, para colision!!!
+				/*if(angle>angle_companion_){
+					angle=angle_companion_;
+				}*/
+			}else{
+				//std::cout << " overpas delante persona "<< std::endl;
+				//angle=(180/3.14)*(asin((final_min_colision_distance)/(robot_person_companion_distance_))); //prueba 1.
+				//std::cout << " angle="<<angle <<"; final_min_colision_distance="<<final_min_colision_distance<<"; robot_person_companion_distance_"<<robot_person_companion_distance_<< std::endl;
+				//double division=(2*final_min_colision_distance)/(robot_person_proximity_distance_+2*robot_radi+little_augmented_collision_margin_);
+				double division;
+//				double radi_force_influence=0.91; // la A!
+				if(case_dynamic_){
+					//std::cout << " entro an case dynamic"<< std::endl;
+					//division=(final_min_colision_distance-robot_radi-radi_force_influence)/(real_robot_person_distance2/2);
+					if((final_min_colision_distance)-robot_radi-(little_augmented_collision_margin_/2)<0){
+						division=0;
+					}else{
+						division=((final_min_colision_distance)-robot_radi-(little_augmented_collision_margin_/2))/((real_robot_person_distance2+2*robot_radi+little_augmented_collision_margin_)/2);
+					}
+
+
+				}else{
+					if((final_min_colision_distance)-robot_radi-(little_augmented_collision_margin_/2)<0){
+						division=0;
+					}else{
+					//division=(final_min_colision_distance-robot_radi)/(real_robot_person_distance2/2);
+						division=((final_min_colision_distance)-robot_radi-(little_augmented_collision_margin_/2))/((real_robot_person_distance2+2*robot_radi+little_augmented_collision_margin_)/2);
+					}
+					std::cout << " final_min_colision_distance="<<final_min_colision_distance<<"; robot_radi="<<robot_radi<<" little_augmented_collision_margin_="<<little_augmented_collision_margin_<<"; real_robot_person_distance2="<<real_robot_person_distance2<< std::endl;
+
+					std::cout << " division="<<division<< std::endl;
+				}
+
+
+
+
+				if(division>1){
+					division=1;
+				}else if(division<(-1)){
+					division=-1;
+				}
+				angle=(180/3.14)*(asin(division)); //prueba 1.
+
+				std::cout << " 1 angle="<<angle<< std::endl;
+				// TODO: temporal!!! el angulo solo va de 0 a angle_companion_=90grados, para colision!!!
+				if(angle>angle_companion_){
+					angle=angle_companion_;
+					//std::cout << " (angle>angle_companion_) angle="<<angle<< std::endl;
+				}
+
+				/*if(angle<0){
+					angle=angle*(-1);
+				}*/
+				std::cout << " 2 angle="<<angle<< std::endl;
+			}
+		}
+
+
+		if(debug_companion_){ // SI se choca con obstaculos habrá que arreglar esto!!!
+			std::cout << " (test, colision) angle="<<angle<< std::endl;
+			std::cout << " final_min_colision_distance="<<final_min_colision_distance<< std::endl;
+		}
+
+		// teniendo la distancia, la he de transformar en un angulo, sabiendo la robot_person_companion_distance_ (distancia maxima)
+		double act_companion_cost; // actual companion cost.
+		act_companion_cost=(0.00000015242)*(pow(angle-angle_companion_,4)); //act_companion_cost=(0.000000015242)*(pow(angle-angle_companion_,4));
+		//std::cout << "act_companion_cost="<<act_companion_cost<< std::endl;
+
+		// calculas si hay incremento de angulo, si lo hay, calculas el coste hacia atras y los angulos hacia atras en el path, para que no haya saltos en la orientación.
+		/*if(angle!=angle_companion_){
+			//std::cout << "calculate_companion_addition_cost_node_of_big_angle_difference"<< std::endl;
+			//aditional cost due tu angle difference between actual angle and parent_vertex_angle
+			double additional_cost=calculate_companion_addition_cost_node_of_big_angle_difference(parent_index, angle);
+			act_companion_cost=act_companion_cost+additional_cost;
+			//calculate_companion_path_angle_and_cost(parent_index, angle);
+		}*/
+		//std::cout << "n"<< std::endl;
+		//colision=false; // TODO: quitar cuando vaya lo del robot...
+		//act_companion_cost=0; // TODO: quitar cuando vaya lo del robot...
+				//angle=90; // TODO: quitar cuando vaya lo del robot...
+
+		vector_of_companion_collisions_.push_back(colision);
+
+
+		cost_companion_.push_back( cost_companion_.at(parent_index) + act_companion_cost );
+
+		//std::cout << "(antes push_back()) angle="<<angle<< std::endl;
+
+		orientation_person_robot_angles_.push_back(angle);
+
+		//min_step_collision_distance_.push_back(final_min_colision_distance);
+		//angles_colisions_.push_back(min_angle_collision_);
+		parent_index_vector_.push_back(parent_index);
+
+		/*if(angle_companion_temp_movil_>angle){
+			angle_companion_temp_movil_=angle; // cojes el angulo menor del path, será el otro angulo estable!!! (el menor con obstaculo, en teoría al pasar de obstaculo a 90 grados, como se cambia despues, no afectara...)
+		}*/
+		/*if(colision){
+			std::cout << "(calculate_companion_cost_node) => angle="<<angle<< std::endl;
+			std::cout << "(calculate_companion_cost_node) => min_distance_collision_="<<min_distance_collision_<< std::endl;
+
+		}*/
+
+		if(debug_companion_){
+			std::cout << "act_companion_cost="<<act_companion_cost<< std::endl;
+			std::cout << "cost_companion_.at(parent_index="<<parent_index<<")="<<cost_companion_.at(parent_index)<< std::endl;
+			std::cout << "cost_companion_.back() [actual_index="<<cost_companion_.size()<<"]="<<cost_companion_.back()<< std::endl;
+		}
+
+		if(debug_companion_){
+		 std::cout << "FIN Function: calculate_companion_cost_node" << std::endl;
+		}
+}
+/*** FIN calculate_companion_cost_node! ***/
+
+
+
+/*** INI calculate_companion_addition_cost_node_of_big_angle_difference! ***/
+
+double Cplan_local_nav_person_companion::calculate_companion_addition_cost_node_of_big_angle_difference( unsigned int parent_index, double actual_angle)
+{ // calcula el coste to add en el caso en que haya gran diferencia entre el angulo(robot<->persona) comparando el del nodo actual y el del nodo anterior.
+	if(debug_companion_){
+		std::cout << "INI Function: calculate_companion_addition_cost_node_of_big_angle_difference" << std::endl;
+	}
+	double additional_cost = 0.0;
+	double angle_diference;
+
+	if(actual_angle>orientation_person_robot_angles_[parent_index]){
+		angle_diference = actual_angle-orientation_person_robot_angles_[parent_index];
+		//std::cout << "case : (actual_angle>orientation_person_robot_angles_[parent_index]); angle_diference="<<angle_diference<<";actual_angle="<<actual_angle<<"; orientation_person_robot_angles_[parent_index]"<< orientation_person_robot_angles_[parent_index]<< std::endl;
+	}else{
+		angle_diference = orientation_person_robot_angles_[parent_index]-actual_angle;
+		//std::cout << "case : (actual_angle<<<orientation_person_robot_angles_[parent_index]); angle_diference="<<angle_diference<<"; actual_angle="<<actual_angle<<"; orientation_person_robot_angles_[parent_index]"<< orientation_person_robot_angles_[parent_index]<< std::endl;
+	}
+	if(debug_companion_){
+	 std::cout << " angle_diference=" << angle_diference << std::endl;
+	 std::cout << " orientation_person_robot_angles_[parent_index]=" << orientation_person_robot_angles_[parent_index] << std::endl;
+	}
+
+	if(angle_diference>angle_increment_of_increment_distance_){
+		double angle_diference2=angle_diference;
+		if(debug_comanion_good_){
+			std::cout << "INI Function: calculate_companion_addition_cost_node_of_big_angle_difference" << std::endl;
+		//if(debug_companion_){
+			std::cout << "in if; angle_diference="<<angle_diference<<"; angle_increment_of_increment_distance="<<angle_increment_of_increment_distance_<<";actual_angle="<<actual_angle<<"; orientation_person_robot_angles_[parent_index]"<< orientation_person_robot_angles_[parent_index] << std::endl;
+		//}
+		}
+		/*do{
+			angle_diference2=angle_diference2-angle_increment_of_increment_distance_;
+			if(debug_companion_){
+				std::cout << "angle_diference2="<<angle_diference2 << std::endl;
+			}
+			double actual_angle_cost=actual_angle-angle_increment_of_increment_distance_;
+			if(debug_companion_){
+				std::cout << "actual_angle_cost="<<actual_angle_cost << std::endl;
+			}
+			double act_companion_cost=(0.00000015242)*(pow(actual_angle_cost-angle_companion_,4)); // double act_companion_cost=(0.000000015242)*(pow(actual_angle_cost-angle_companion_,4));
+			if(debug_comanion_good_){
+				std::cout << " (do_while) angle_diference2="<<angle_diference2 <<"; actual_angle_cost="<<actual_angle_cost <<"; act_companion_cost="<<act_companion_cost<<"; additional_cost="<<additional_cost << std::endl;
+			}
+			additional_cost=additional_cost+act_companion_cost;
+		}while(angle_diference2>0);*/
+
+		//*** INICIO coste aproximado, para no incrementar coste computacional. ***//
+		if(angle_diference2>0){
+			angle_diference2=angle_diference2-angle_increment_of_increment_distance_;
+			if(debug_companion_){
+				std::cout << "angle_diference2="<<angle_diference2 << std::endl;
+			}
+			double actual_angle_cost=actual_angle-angle_increment_of_increment_distance_;
+			if(debug_companion_){
+				std::cout << "actual_angle_cost="<<actual_angle_cost << std::endl;
+			}
+			double act_companion_cost=(0.00000015242)*(pow(actual_angle_cost-angle_companion_,4)); // double act_companion_cost=(0.000000015242)*(pow(actual_angle_cost-angle_companion_,4));
+			if(debug_comanion_good_){
+				std::cout << " (do_while) angle_diference2="<<angle_diference2 <<"; actual_angle_cost="<<actual_angle_cost <<"; act_companion_cost="<<act_companion_cost<<"; additional_cost="<<additional_cost << std::endl;
+			}
+			additional_cost=additional_cost+(act_companion_cost*(angle_diference2/angle_increment_of_increment_distance_));
+
+		}
+		//*** FIN coste aproximado, para no incrementar coste computacional. ***//
+
+		if(debug_comanion_good_){
+			std::cout << " (final) additional_cost=" << additional_cost << std::endl;
+			std::cout << "FIN Function: calculate_companion_addition_cost_node_of_big_angle_difference" << std::endl;
+		}
+	}
+
+	return additional_cost;
+}
+/*** FIN calculate_companion_addition_cost_node_of_big_angle_difference! ***/
+
+void Cplan_local_nav_person_companion::return_next_robot_position_companion_cost_and_angle(unsigned int parent_index){
+	// siempre que el inicio del arbol sea una unica rama, sino, habria que ir hacia atras en los parent index, desde el min_index, hasta que el indice sea >0. osea, el minimo mayor que 0.
+
+	//std::cout << "INI see_companion_path_angle_and_cost_of_min_cost_paths"<< std::endl;
+	unsigned int index_min_dist;
+	double actual_parent_index=parent_index;
+	do{
+		//std::cout << "(path) orientation_person_robot_angles_[actual_parent_index="<<actual_parent_index<<"]=" <<orientation_person_robot_angles_[actual_parent_index]<< std::endl;
+		//std::cout << "(path) cost_companion_[actual_parent_index="<<actual_parent_index<<"]=" <<cost_companion_[actual_parent_index]<< std::endl;
+		//std::cout << "min_distance_collision_vector_[actual_parent_index]= " <<min_distance_collision_vector_[actual_parent_index]<< std::endl;
+		index_min_dist=actual_parent_index;
+		min_next_companion_angle_=orientation_person_robot_angles_[actual_parent_index];
+		min_next_companion_cost_=cost_companion_[actual_parent_index];
+		std::cout << " [return_next_robot_position_companion_cost_and_angle] min_next_companion_cost_="<<min_next_companion_cost_<<"(path) actual_parent_index="<<actual_parent_index<< std::endl;
+		before_initial_angle_=orientation_person_robot_angles_[actual_parent_index];
+		before_initial_cost_=cost_companion_[actual_parent_index];
+		//std::cout << "(path) actual_parent_index="<<actual_parent_index<< std::endl;
+		actual_parent_index=parent_index_vector_[actual_parent_index];
+	}while(actual_parent_index>0);
+		//std::cout << "(path) actual_parent_index="<<actual_parent_index<< std::endl;
+		//std::cout << "(final out_ path) actual_parent_index="<<actual_parent_index<< std::endl;
+		//std::cout << "(path) orientation_person_robot_angles_[actual_parent_index="<<0<<"]=" <<orientation_person_robot_angles_[0]<< std::endl;
+		//std::cout << "(path) cost_companion_[actual_parent_index="<<0<<"]=" <<cost_companion_[0]<< std::endl;
+
+	//min_next_companion_angle_=orientation_person_robot_angles_[1];
+	//min_next_companion_cost_= cost_companion_[1];
+	if(debug_antes_subgoals_entre_AKP_goals_){
+		std::cout << "min_distance_collision_vector_["<<index_min_dist<<"]= " <<min_distance_collision_vector_[index_min_dist]<< std::endl;
+		std::cout << " OUT ANGLES!!! min_next_companion_angle_= "<<min_next_companion_angle_<<"min_next_companion_cost_="<<min_next_companion_cost_ << std::endl;
+	}
+	//before_initial_angle_=orientation_person_robot_angles_[1];
+	//before_initial_cost_=cost_companion_[1];
+
+}
+
+void Cplan_local_nav_person_companion::ini_increment_angle(){
+	// version 1: con velocidad maxima de las personas.
+	//double distance_between_robot_and_person=1; // suponemos 1m de distancia entre ellos.
+	if(actual_debug_){
+		std::cout << "*** ini_increment_angle() ***" << std::endl;
+	}
+	double distance_between_robot_and_person=robot_person_proximity_distance_; //calc_robot_person_companion_distance();//
+
+
+	double increment_time = dt_; // TODO: aquí se tienen en cuenta todos los dt_ iguales. si hicieramos ventana de vector de incremento de tiempos, sería más exacto y mejor.
+	// std::cout << " increment_time = dt_=" << increment_time << std::endl;
+	double people_max_velocity = (5*1000)/3600; //% 5km/seg *(1000m/1km)*(1h/3600seg)= 1.3888888889 metros/seg, aprox 1.4m/seg.
+	//double robot_velocity = robot_->get_desired_velocity();  // (probar las dos a ver) robot_->get_current_pose().v  o   robot_->get_desired_velocity()
+	double increment_person_distance_in_dt=people_max_velocity*increment_time;
+	if(debug_companion_){
+		std::cout << "increment_person_distance_in_dt=" << increment_person_distance_in_dt << std::endl;
+	}
+	angle_increment_of_increment_distance_=2*(180/3.14)*(asin((increment_person_distance_in_dt/2)/distance_between_robot_and_person));
+
+	if(actual_debug_){
+		std::cout << "(maximo_final) angle_increment_of_increment_distance=" << angle_increment_of_increment_distance_<< std::endl;
+	}
+
+	//if(debug_companion_){
+	// version 2: fija, constante, para hacerlo muy incremental
+	if(debug_nadal_){
+		std::cout << "FUNCTION ini_increment_angle(); angle_increment_of_increment_distance=" << angle_increment_of_increment_distance_<< std::endl;
+	}
+		angle_increment_of_increment_distance_=0.2; //5 grados, para que cambie antes, durante más rato y más progresivo...
+		if(debug_nadal_){
+		std::cout << "(constante_final) angle_increment_of_increment_distance=" << angle_increment_of_increment_distance_<< std::endl;
+		}
+	//}
+	// version 3: con velocidad real de la persona.
+	Cperson_abstract* person_obj;
+	find_person(id_person_companion_ , &person_obj);
+	//double vel=max_v_by_system_;//person_obj->get_desired_velocity();
+	double in_vel=robot_->get_current_pose().v;
+	in_vel=in_vel*in_vel;
+	double people_real_velocity = sqrt(in_vel);//(robot_->get_a_v_max())*(0.5);//(vel*1000)/3600;
+	increment_person_distance_in_dt=people_real_velocity*increment_time;
+	angle_increment_of_increment_distance_=2*(180/3.14)*(asin((increment_person_distance_in_dt/2)/distance_between_robot_and_person));
+
+	if(debug_real_test_companion_){
+		std::cout << "dt_=" << dt_<<"increment_time="<<increment_time<<"distance_between_robot_and_person="<<distance_between_robot_and_person<< std::endl;
+		std::cout << "(real_FINAL=v_robot) angle_increment_of_increment_distance=" << angle_increment_of_increment_distance_<< std::endl;
+	}
+	//vel=max_v_by_system_;//person_obj->get_desired_velocity();
+	//in_vel=person_obj->get_desired_velocity();//robot_->get_current_pose().v;//person_obj->get_desired_velocity();//robot_->get_current_pose().v;
+
+	if(person_obj->get_desired_velocity()>0.5){ // era 0.3
+		//in_vel=ini_vel_to_increment_angle_;
+		in_vel=person_obj->get_current_pointV().v();
+		//in_vel=0.5+person_obj->get_current_pointV().v()*10/100;
+		//person_obj->get_desired_velocity()-0.15;
+		if(debug_file_robot_){
+			std::ofstream fileMatlab2;
+			fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+			fileMatlab2 << "% (if(person_obj->get_desired_velocity()>0.3)) person_obj->get_desired_velocity()= "<<person_obj->get_desired_velocity()<<"\n";
+			fileMatlab2.close();
+		}
+	}else{
+		in_vel=person_obj->get_current_pointV().v();
+		//in_vel=0.5+person_obj->get_current_pointV().v()*10/100;
+		//in_vel=ini_vel_to_increment_angle_;
+		if(debug_file_robot_){
+			std::ofstream fileMatlab2;
+			fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+			fileMatlab2 << "  (else) in_vel=0.3; person_obj->get_desired_velocity()"<< person_obj->get_desired_velocity()<<"\n";
+			fileMatlab2.close();
+		}
+	}
+	in_vel=in_vel*in_vel;
+	people_real_velocity = sqrt(in_vel);//(robot_->get_a_v_max())*(0.5);//(vel*1000)/3600;
+	increment_person_distance_in_dt=people_real_velocity*increment_time;
+	angle_increment_of_increment_distance_=2*(180/3.14)*(asin((increment_person_distance_in_dt/2)/distance_between_robot_and_person));
+	//angle_increment_of_increment_distance_=0.05;
+	//angle_increment_of_increment_distance_vuelta_al_reves_=angle_increment_of_increment_distance_/2;
+	angle_increment_of_increment_distance_vuelta_al_reves_=3*angle_increment_of_increment_distance_/2;
+
+
+	/*if(!overpas_obstacles_behind_person_){
+		angle_increment_of_increment_distance_=0.2;
+		angle_increment_of_increment_distance_vuelta_al_reves_=0.2;
+		std::cout << "IMPORTANT! angle_increment_of_increment_distance_=" << angle_increment_of_increment_distance_<<"; angle_increment_of_increment_distance_="<<angle_increment_of_increment_distance_<< std::endl;
+	}*/
+
+
+	if(debug_gazebo_journal_){
+		std::cout << "(real_FINAL) person_obj->get_desired_velocity()=" << person_obj->get_desired_velocity()<< std::endl;
+		std::cout << "(real_FINAL) person_obj->get_current_pointV().v()=" << person_obj->get_current_pointV().v()<< std::endl;
+		std::cout << "(real_FINAL) increment_time=" << increment_time<< std::endl;
+		std::cout << "(real_FINAL) distance_between_robot_and_person=" << distance_between_robot_and_person<< std::endl;
+		std::cout << "(real_FINAL) increment_person_distance_in_dt=" << increment_person_distance_in_dt<< std::endl;
+		std::cout << "(real_FINAL=v_persona) angle_increment_of_increment_distance=" << angle_increment_of_increment_distance_<< "; people_real_velocity =" << people_real_velocity << std::endl;
+	}
+
+
+
+	if(debug_file_robot_){
+		std::ofstream fileMatlab2;
+		fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+		fileMatlab2 << "% angle_increment_of_increment_distance_= "<<angle_increment_of_increment_distance_<<"\n";
+		fileMatlab2 << "% angle_increment_of_increment_distance_vuelta_al_reves_= "<<angle_increment_of_increment_distance_vuelta_al_reves_<<"\n";
+		fileMatlab2.close();
+	}
+
+	//angle_increment_of_increment_distance_=0.2;//2*(180/3.14)*(asin((increment_person_distance_in_dt/2)/distance_between_robot_and_person));
+	if(debug_nadal_){
+	std::cout << "(real_FINAL) robot_->get_current_pose().v=" << robot_->get_current_pose().v<< std::endl;
+	std::cout << "(real_FINAL) people_real_velocity=" << people_real_velocity<< std::endl;
+	std::cout << "(real_FINAL) increment_time=" << increment_time<< std::endl;
+	std::cout << "(real_FINAL) distance_between_robot_and_person=" << distance_between_robot_and_person<< std::endl;
+	std::cout << "(real_FINAL) increment_person_distance_in_dt=" << increment_person_distance_in_dt<< std::endl;
+	}
+	if(debug_real_test_companion4_){
+		std::cout << "TTTTTTTTTTTTTTTT (real_FINAL) angle_increment_of_increment_distance=" << angle_increment_of_increment_distance_<< std::endl;
+	}
+}
+
+
+void Cplan_local_nav_person_companion::ini_increment_angle_person_companion_akp(){
+	// version 1: con velocidad maxima de las personas.
+	//double distance_between_robot_and_person=1; // suponemos 1m de distancia entre ellos.
+
+	double distance_between_robot_and_person=robot_person_proximity_distance_;
+	double increment_time = dt_; // TODO: aquí se tienen en cuenta todos los dt_ iguales. si hicieramos ventana de vector de incremento de tiempos, sería más exacto y mejor.
+	// version 3: con velocidad real de la persona.
+	//Cperson_abstract* person_obj;
+	//find_person(id_person_companion_ , &person_obj);
+	//double vel=max_v_by_system_;//person_obj->get_desired_velocity();
+	double in_vel=person_companion_->get_current_pose().v;
+	//in_vel=person_obj->get_desired_velocity();
+	in_vel=in_vel*in_vel;
+	double people_real_velocity = sqrt(in_vel);//(robot_->get_a_v_max())*(0.5);//(vel*1000)/3600;
+	double increment_person_distance_in_dt=people_real_velocity*increment_time;
+	angle_increment_of_increment_distance_=2*(180/3.14)*(asin((increment_person_distance_in_dt/2)/distance_between_robot_and_person));
+	angle_increment_of_increment_distance_vuelta_al_reves_=angle_increment_of_increment_distance_/2;
+
+	if(debug_person_companion_increment_angle_){
+		std::cout << " angle_increment_of_increment_distance_=" << angle_increment_of_increment_distance_ << std::endl;
+		std::cout << " angle_increment_of_increment_distance_vuelta_al_reves_=" << angle_increment_of_increment_distance_vuelta_al_reves_ << std::endl;
+	}
+}
+
+
+void Cplan_local_nav_person_companion::calculate_companion_path_angle_and_cost(unsigned int parent_index, double actual_angle){
+	// calcula el coste to add en el caso en que haya gran diferencia entre el angulo(robot<->persona) comparando el del nodo actual y el del nodo anterior.
+	// calcula los angulos que hay que moverse en ese path.
+	if(debug_companion_){
+		std::cout << "INI Function: calculate_companion_path_angle_and_cost" << std::endl;
+	}
+	//double additional_cost = 0.0;
+	double angle_diference;
+	unsigned int caso_angulo;
+
+	if(actual_angle>orientation_person_robot_angles_[parent_index]){
+		angle_diference = actual_angle-orientation_person_robot_angles_[parent_index];
+		//std::cout << "case : (actual_angle>orientation_person_robot_angles_[parent_index]); angle_diference="<<angle_diference<<";actual_angle="<<actual_angle<<"; orientation_person_robot_angles_[parent_index]"<< orientation_person_robot_angles_[parent_index]<< std::endl;
+		caso_angulo=0;
+	}else{
+		angle_diference = orientation_person_robot_angles_[parent_index]-actual_angle;
+		//std::cout << "case : (actual_angle<<<orientation_person_robot_angles_[parent_index]); angle_diference="<<angle_diference<<"; actual_angle="<<actual_angle<<"; orientation_person_robot_angles_[parent_index]"<< orientation_person_robot_angles_[parent_index]<< std::endl;
+		caso_angulo=1;
+	}
+	if(debug_companion_){
+		std::cout << " angle_diference=" << angle_diference << std::endl;
+		std::cout << " orientation_person_robot_angles_[parent_index]=" << orientation_person_robot_angles_[parent_index] << std::endl;
+	}
+
+	if((angle_diference>angle_increment_of_increment_distance_)&&(caso_angulo==1)){
+		double angle_diference2=angle_diference;
+		if(debug_companion_){
+			std::cout << "INI Function: calculate_companion_addition_cost_node_of_big_angle_difference" << std::endl;
+			std::cout << " actual_angle=" <<actual_angle<<"; orientation_person_robot_angles_[parent_index]="<<orientation_person_robot_angles_[parent_index]<< std::endl;
+		}
+		double actual_angle_cost=actual_angle;
+		double actual_parent_index=parent_index;
+
+		do{
+			angle_diference2=angle_diference2-angle_increment_of_increment_distance_;
+			if(caso_angulo==0){
+				actual_angle_cost=actual_angle_cost-angle_increment_of_increment_distance_;
+			}else{
+				actual_angle_cost=actual_angle_cost+angle_increment_of_increment_distance_;
+				if(actual_angle_cost>angle_companion_){
+					actual_angle_cost=angle_companion_;
+				}
+			}
+
+			double act_companion_cost=(0.00000015242)*(pow(actual_angle_cost-angle_companion_,4)); //double act_companion_cost=(0.000000015242)*(pow(actual_angle_cost-angle_companion_,4));
+			if(debug_antes_subgoals_entre_AKP_goals_){
+				std::cout << "aditional act_companion_cost="<<act_companion_cost << std::endl;
+				std::cout << "(aditional cost antes) cost_companion_["<<actual_parent_index<<"]="<<cost_companion_[actual_parent_index] << std::endl;
+			}
+			cost_companion_[actual_parent_index]=cost_companion_[actual_parent_index]+act_companion_cost; // (cambias el cost companion anteriores) // o cost_companion_[actual_parent_index]=cost_companion_[actual_parent_index]+act_companion_cost;
+			if(debug_antes_subgoals_entre_AKP_goals_){
+				std::cout << "(aditional cost despues) cost_companion_["<<actual_parent_index<<"]="<<cost_companion_[actual_parent_index] << std::endl;
+			}
+			//orientation_person_robot_angles_[actual_parent_index]=actual_angle_cost; // modificas los angulos anteriores para que sea menos abrupto el cambio.
+
+			if(debug_companion_){
+				std::cout <<"actual_angle_cost= "<<actual_angle_cost<<"; cost_companion_["<<actual_parent_index<<"]="<<cost_companion_[actual_parent_index] <<"; orientation_person_robot_angles_[actual_parent_index]="<<orientation_person_robot_angles_[actual_parent_index]<< std::endl;
+			}
+			actual_parent_index=parent_index_vector_[actual_parent_index];
+			/*if(actual_angle_cost==orientation_person_robot_angles_[parent_index]){
+				std::cout << " (break do_while) actual_angle_cost==orientation_person_robot_angles_[parent_index] "<< std::endl;
+				break;
+			}*/
+		}while((angle_diference2>0)&&(actual_parent_index>=0));
+
+		if(debug_companion_){
+			std::cout << "FIN Function: calculate_companion_path_angle_and_cost;" << std::endl;
+		}
+	}
+
+}
+
+void Cplan_local_nav_person_companion::only_cost_calculate_companion_path_angle_and_cost(unsigned int parent_index, double actual_angle){
+	// calcula el coste to add en el caso en que haya gran diferencia entre el angulo(robot<->persona) comparando el del nodo actual y el del nodo anterior.
+	// calcula los angulos que hay que moverse en ese path.
+	if(debug_companion_){
+		std::cout << "INI Function: calculate_companion_path_angle_and_cost" << std::endl;
+	}
+	double additional_cost = 0.0;
+	double angle_diference;
+
+	if(actual_angle>orientation_person_robot_angles_[parent_index]){
+		angle_diference = actual_angle-orientation_person_robot_angles_[parent_index];
+		//std::cout << "case : (actual_angle>orientation_person_robot_angles_[parent_index]); angle_diference="<<angle_diference<<";actual_angle="<<actual_angle<<"; orientation_person_robot_angles_[parent_index]"<< orientation_person_robot_angles_[parent_index]<< std::endl;
+	}else{
+		angle_diference = orientation_person_robot_angles_[parent_index]-actual_angle;
+		//std::cout << "case : (actual_angle<<<orientation_person_robot_angles_[parent_index]); angle_diference="<<angle_diference<<"; actual_angle="<<actual_angle<<"; orientation_person_robot_angles_[parent_index]"<< orientation_person_robot_angles_[parent_index]<< std::endl;
+	}
+	if(debug_companion_){
+		std::cout << " angle_diference=" << angle_diference << std::endl;
+		std::cout << " orientation_person_robot_angles_[parent_index]=" << orientation_person_robot_angles_[parent_index] << std::endl;
+	}
+
+	if(angle_diference>angle_increment_of_increment_distance_){
+		double angle_diference2=angle_diference;
+		if(actual_debug_){
+			std::cout << "INI Function: calculate_companion_addition_cost_node_of_big_angle_difference" << std::endl;
+		}
+		if(debug_companion_){
+			std::cout << "in if; angle_diference="<<angle_diference<<"; angle_increment_of_increment_distance="<<angle_increment_of_increment_distance_<<";actual_angle="<<actual_angle<<"; orientation_person_robot_angles_[parent_index]"<< orientation_person_robot_angles_[parent_index] << std::endl;
+		}
+		double actual_parent_index=parent_index;
+		do{
+			angle_diference2=angle_diference2-angle_increment_of_increment_distance_;
+			double actual_angle_cost=actual_angle-angle_increment_of_increment_distance_;
+			double act_companion_cost=(0.00000015242)*(pow(actual_angle_cost-angle_companion_,4)); //double act_companion_cost=(0.000000015242)*(pow(actual_angle_cost-angle_companion_,4));
+
+			cost_companion_[actual_parent_index]=cost_companion_[actual_parent_index]+act_companion_cost; // (cambias el cost companion anteriores) // o cost_companion_[actual_parent_index]=cost_companion_[actual_parent_index]+act_companion_cost;
+			//orientation_person_robot_angles_[actual_parent_index]=actual_angle_cost; // modificas los angulos anteriores para que sea menos abrupto el cambio.
+
+			actual_parent_index=parent_index_vector_[actual_parent_index];
+
+			if(debug_companion_){
+				std::cout << "angle_diference2="<<angle_diference2 << std::endl;
+				std::cout << "actual_angle_cost="<<actual_angle_cost << std::endl;
+			}
+			if(debug_companion_){
+				std::cout << " (do_while) angle_diference2="<<angle_diference2 <<"; actual_angle_cost="<<actual_angle_cost <<"; act_companion_cost="<<act_companion_cost<<"; additional_cost="<<additional_cost << std::endl;
+			}
+
+		}while((angle_diference2>0)&&(actual_parent_index>=0));
+			if(debug_companion_){
+				std::cout << "FIN Function: calculate_companion_path_angle_and_cost" << std::endl;
+			}
+	}
+
+}
+
+
+void Cplan_local_nav_person_companion::only_angle_in_final_tree_calculate_companion_path_angle_and_cost(unsigned int i){
+	// calcula el coste to add en el caso en que haya gran diferencia entre el angulo(robot<->persona) comparando el del nodo actual y el del nodo anterior.
+		// calcula los angulos que hay que moverse en ese path.
+	if(debug_comanion_good_){
+		std::cout << "INI only_angle_in_final_tree_calculate_companion_path_angle_and_cost"<< std::endl;
+	}
+	BEST_path_parent_index_vector_.clear(); // de atras a delante. (en el ultimo coges el primer indice.)
+
+	unsigned int actual_index=i; // recorrete todo el path the este final de camino.
+	unsigned int actual_parent_index=parent_index_vector_[actual_index];
+	double actual_angle=orientation_person_robot_angles_[actual_index];
+	double actual_parent_angle=orientation_person_robot_angles_[actual_parent_index];
+
+	double diff_angle=actual_parent_angle-actual_angle; // saltos posibles 1- 90grad a 180grad o 0 grad.
+	if(debug_comanion_good_){																		// 2- de 0grad o 180grad a 90grad.
+		std::cout << "actual_angle=" <<actual_angle<<"; actual_parent_angle="<<actual_parent_angle<<"; diff_angle="<<diff_angle<< std::endl;
+	}
+	BEST_path_parent_index_vector_.push_back(actual_index);
+
+		if((cost_companion_[i]!=0)&&(actual_angle<actual_parent_angle)&&(diff_angle>angle_increment_of_increment_distance_)){ // si hay cost companion
+			do{
+				actual_angle=orientation_person_robot_angles_[actual_index];
+				actual_parent_angle=orientation_person_robot_angles_[actual_parent_index];
+				/*if(actual_angle>actual_parent_angle){
+					std::cout << " Int if ->(path BEFORE CHANGE) orientation_person_robot_angles_[actual_parent_index="<<actual_parent_index<<"]=" <<orientation_person_robot_angles_[actual_parent_index]<< std::endl;
+					diff_angle=actual_angle-actual_parent_angle;
+					if(diff_angle>angle_increment_of_increment_distance_){
+						orientation_person_robot_angles_[actual_parent_index]=actual_angle-angle_increment_of_increment_distance_;
+					}
+				}*/
+				if(actual_angle<actual_parent_angle){
+					if(debug_comanion_good_){
+						std::cout << " Int if ->(path BEFORE CHANGE) orientation_person_robot_angles_[actual_parent_index="<<actual_parent_index<<"]=" <<orientation_person_robot_angles_[actual_parent_index]<< std::endl;
+					}
+					diff_angle=actual_parent_angle-actual_angle;
+					if(actual_debug_){
+						std::cout << "diff_angle=" <<diff_angle<<"; angle_increment_of_increment_distance_="<<angle_increment_of_increment_distance_<< std::endl;
+					}
+					if(diff_angle>angle_increment_of_increment_distance_){
+						orientation_person_robot_angles_[actual_parent_index]=actual_angle+angle_increment_of_increment_distance_;
+
+						if(actual_debug_){
+							std::cout << "actual_index=" <<actual_index<<"; actual_parent_index="<<actual_parent_index<< std::endl;
+							std::cout << "actual_angle=" <<actual_angle<<"; actual_parent_angle="<<actual_parent_angle<<"; diff_angle="<<diff_angle<< std::endl;
+							std::cout << "(path) orientation_person_robot_angles_[actual_parent_index="<<actual_parent_index<<"]=" <<orientation_person_robot_angles_[actual_parent_index]<< std::endl;
+							std::cout << "(path) cost_companion_[actual_parent_index="<<actual_parent_index<<"]=" <<cost_companion_[actual_parent_index]<< std::endl;
+							std::cout << "(path) min_distance_collision_vector_="<<actual_parent_index<<"]=" <<min_distance_collision_vector_[actual_parent_index]<< std::endl;
+
+
+						}
+
+						if(orientation_person_robot_angles_[actual_parent_index]>angle_companion_){
+								orientation_person_robot_angles_[actual_parent_index]=angle_companion_;
+						}
+					}
+					diff_angle=diff_angle-angle_increment_of_increment_distance_;
+				}
+
+				/*else{
+					diff_angle=actual_parent_angle-actual_angle;
+					if(diff_angle>angle_increment_of_increment_distance_){
+
+					}
+				}*/
+				if(debug_comanion_good_){
+					std::cout << "actual_index=" <<actual_index<<"; actual_parent_index="<<actual_parent_index<< std::endl;
+					std::cout << "actual_angle=" <<actual_angle<<"; actual_parent_angle="<<actual_parent_angle<<"; diff_angle="<<diff_angle<< std::endl;
+					std::cout << "(path) orientation_person_robot_angles_[actual_parent_index="<<actual_parent_index<<"]=" <<orientation_person_robot_angles_[actual_parent_index]<< std::endl;
+					std::cout << "(path) cost_companion_[actual_parent_index="<<actual_parent_index<<"]=" <<cost_companion_[actual_parent_index]<< std::endl;
+					std::cout << "(path) min_distance_collision_vector_="<<actual_parent_index<<"]=" <<min_distance_collision_vector_[actual_parent_index]<< std::endl;
+
+				}
+
+				/*if(diff_angle>angle_increment_of_increment_distance_){
+					angle_diference2=angle_diference2-angle_increment_of_increment_distance_;
+					double actual_angle_cost=actual_angle-angle_increment_of_increment_distance_;
+					//double act_companion_cost=(0.000000015242)*(pow(actual_angle_cost-angle_companion_,4));
+				}*/
+
+				//double actual_angle_cost=actual_angle-angle_increment_of_increment_distance_;
+				//orientation_person_robot_angles_[actual_parent_index]=actual_angle_cost; // modificas los angulos anteriores para que sea menos abrupto el cambio.
+				actual_index=actual_parent_index;
+				actual_parent_index=parent_index_vector_[actual_index]; //busca el nodo anterior de este path.
+				BEST_path_parent_index_vector_.push_back(actual_index);
+			}while((actual_parent_index>0)&&(diff_angle>angle_increment_of_increment_distance_));
+			// PARA EL INDICE =0, PQ SINO ENTRA EN BUCLE INFINITO!
+			actual_angle=orientation_person_robot_angles_[actual_index];
+			actual_parent_angle=orientation_person_robot_angles_[actual_parent_index];
+			if(actual_angle<actual_parent_angle){
+				diff_angle=actual_parent_angle-actual_angle;
+				if(diff_angle>angle_increment_of_increment_distance_){
+					orientation_person_robot_angles_[actual_parent_index]=actual_angle+angle_increment_of_increment_distance_;
+					if(orientation_person_robot_angles_[actual_parent_index]>angle_companion_){
+						orientation_person_robot_angles_[actual_parent_index]=angle_companion_;
+					}
+				}
+			}
+			BEST_path_parent_index_vector_.push_back(actual_parent_index);
+			if(debug_comanion_good_){
+				std::cout << "actual_index=" <<actual_index<<"; actual_parent_index="<<actual_parent_index<< std::endl;
+				std::cout << "(path) orientation_person_robot_angles_[actual_parent_index="<<0<<"]=" <<orientation_person_robot_angles_[0]<< std::endl;
+				std::cout << "(path) cost_companion_[actual_parent_index="<<0<<"]=" <<cost_companion_[0]<< std::endl;
+				std::cout << "(path) min_distance_collision_vector_="<<actual_parent_index<<"]=" <<min_distance_collision_vector_[actual_parent_index]<< std::endl;
+
+			}
+
+		}
+
+		if(debug_comanion_good_){
+			if(!BEST_path_parent_index_vector_.empty()){
+				for(unsigned int f=0;f<BEST_path_parent_index_vector_.size();f++){
+					std::cout << "(BEST path index) f="<<f<<"; index=" <<BEST_path_parent_index_vector_[f]<< std::endl;
+				}
+			}
+			std::cout << "FIN only_angle_in_final_tree_calculate_companion_path_angle_and_cost"<< std::endl;
+		}
+}
+
+
+void Cplan_local_nav_person_companion::only_angle_in_final_tree_calculate_companion_path_angle_and_cost2(unsigned int i){
+	// calcula los angulos que hay que moverse en ese path.
+	//if(debug_comanion_good_){
+		std::cout << "    !!!    INI only_angle_in_final_tree_calculate_companion_path_angle_and_cost2"<< std::endl;
+	//}
+	BEST_path_parent_index_vector_.clear(); // de atras a delante. (en el ultimo coges el primer indice.)
+
+	unsigned int actual_index2; // recorrete todo el path the este final de camino.
+	unsigned int actual_parent_index2;
+	//if(calc_goal_companion_with_group_path_){
+		actual_index2=i;
+		actual_parent_index2=parent_index_vector_[actual_index2];
+		BEST_path_parent_index_vector_.push_back(actual_index2);
+		//std::cout << " (ini while) actual_index="<<actual_index<<"; BEST_path_parent_index_vector_.back()"<<BEST_path_parent_index_vector_.back()<< std::endl;
+		do{
+			BEST_path_parent_index_vector_.push_back(actual_index2);
+			actual_index2=actual_parent_index2;
+			actual_parent_index2=parent_index_vector_[actual_index2];
+		}while(actual_parent_index2>0);
+		BEST_path_parent_index_vector_.push_back(actual_parent_index2);
+
+
+
+
+
+
+	// CASE 1: actual_angle < actual_parent_angle. Dar vuelta hacia atras del arbol (primera vuelta).
+	unsigned int actual_index=i; // recorrete todo el path the este final de camino.
+	unsigned int actual_parent_index=parent_index_vector_[actual_index];
+	double actual_angle=orientation_person_robot_angles_[actual_index];
+	double actual_parent_angle=orientation_person_robot_angles_[actual_parent_index];
+
+	double diff_angle=actual_parent_angle-actual_angle; // saltos posibles 1- 90grad a 180grad o 0 grad.
+	//if(debug_real_test_companion4_){																		// 2- de 0grad o 180grad a 90grad.
+		std::cout << "actual_angle=" <<actual_angle<<"; actual_parent_angle="<<actual_parent_angle<<"; diff_angle="<<diff_angle<< std::endl;
+	//}
+	bool caso_vuelta_al_reves=false;
+	//BEST_path_parent_index_vector_.push_back(actual_index);
+	std::cout << " (ini while) actual_index="<<actual_index<<"; BEST_path_parent_index_vector_.back()"<<BEST_path_parent_index_vector_.back()<< std::endl;
+		if((cost_companion_[i]!=0)){//&&(actual_angle<actual_parent_angle)&&(diff_angle>angle_increment_of_increment_distance_)){ // si hay cost companion
+
+			//double sum_angles_increment1=0;
+			//int count_sum=0;
+			do{
+				actual_angle=orientation_person_robot_angles_[actual_index];
+				actual_parent_angle=orientation_person_robot_angles_[actual_parent_index];
+				/*if(actual_angle>actual_parent_angle){
+					std::cout << " Int if ->(path BEFORE CHANGE) orientation_person_robot_angles_[actual_parent_index="<<actual_parent_index<<"]=" <<orientation_person_robot_angles_[actual_parent_index]<< std::endl;
+					diff_angle=actual_angle-actual_parent_angle;
+					if(diff_angle>angle_increment_of_increment_distance_){
+						orientation_person_robot_angles_[actual_parent_index]=actual_angle-angle_increment_of_increment_distance_;
+					}
+				}*/
+
+				//sum_angles_increment1=sum_angles_increment1+(orientation_person_robot_angles_[actual_index]-orientation_person_robot_angles_[actual_parent_index]);
+				//count_sum++;
+				if(debug_real_test_companion4_){
+					std::cout << "actual_angle=" <<actual_angle<<"; actual_parent_angle="<<actual_parent_angle<<"; diff_angle="<<diff_angle<< std::endl;
+				}
+
+				if(actual_angle<actual_parent_angle){
+					caso_vuelta_al_reves=true;
+					diff_angle=actual_parent_angle-actual_angle;
+
+					if(diff_angle>angle_increment_of_increment_distance_){
+						if(debug_comanion_good_){
+							std::cout << " (DO-WILE) Int if ->(path BEFORE CHANGE) orientation_person_robot_angles_[actual_parent_index="<<actual_parent_index<<"]=" <<orientation_person_robot_angles_[actual_parent_index]<< std::endl;
+						}
+						if(actual_debug_){
+							std::cout << "diff_angle=" <<diff_angle<<"; angle_increment_of_increment_distance_="<<angle_increment_of_increment_distance_<< std::endl;
+						}
+
+						//std::cout << "(DO-WILE) (!!!CASE!!! actual_angle < actual_parent_angle)"<< std::endl;
+						orientation_person_robot_angles_[actual_parent_index]=actual_angle+angle_increment_of_increment_distance_;
+
+						if(actual_debug2_){
+							std::cout << "if (CASE: entro en obstaculo)"<< std::endl;
+							std::cout << "actual_index=" <<actual_index<<"; actual_parent_index="<<actual_parent_index<< std::endl;
+							std::cout << "actual_angle=" <<actual_angle<<"; actual_parent_angle="<<actual_parent_angle<<"; diff_angle="<<diff_angle<< std::endl;
+							std::cout << "(path) orientation_person_robot_angles_[actual_index="<<actual_index<<"]=" <<orientation_person_robot_angles_[actual_index]<< std::endl;
+							std::cout << "(path) orientation_person_robot_angles_[actual_parent_index="<<actual_parent_index<<"]=" <<orientation_person_robot_angles_[actual_parent_index]<< std::endl;
+							std::cout << "(path) min_distance_collision_vector_[actual_index="<<actual_index<<"]=" <<min_distance_collision_vector_[actual_index]<< std::endl;
+							std::cout << "(path) min_distance_collision_vector_[actual_parent_index="<<actual_parent_index<<"]=" <<min_distance_collision_vector_[actual_parent_index]<< std::endl;
+
+							//std::cout << "(path) cost_companion_[actual_parent_index="<<actual_parent_index<<"]=" <<cost_companion_[actual_parent_index]<< std::endl;
+						}
+						if(orientation_person_robot_angles_[actual_parent_index]>angle_companion_){
+								orientation_person_robot_angles_[actual_parent_index]=angle_companion_;
+						}
+					}
+					diff_angle=diff_angle-angle_increment_of_increment_distance_;
+
+
+
+				}else{ // CASE: actual_angle > actual_parent_angle (aquí no se puede hacer...) Hay que dar vuelta hacia delante.
+
+				}
+
+				/*else{
+					diff_angle=actual_parent_angle-actual_angle;
+					if(diff_angle>angle_increment_of_increment_distance_){
+
+					}
+				}*/
+				if(debug_comanion_good_){
+					std::cout << "[POSIBLE change of angle] actual_index=" <<actual_index<<"; actual_parent_index="<<actual_parent_index<< std::endl;
+					std::cout << "actual_angle=" <<actual_angle<<"; actual_parent_angle="<<actual_parent_angle<<"; diff_angle="<<diff_angle<< std::endl;
+					std::cout << "(path) = this could change!!! orientation_person_robot_angles_[actual_parent_index="<<actual_parent_index<<"]=" <<orientation_person_robot_angles_[actual_parent_index]<< std::endl;
+					std::cout << "(path) cost_companion_[actual_parent_index="<<actual_parent_index<<"]=" <<cost_companion_[actual_parent_index]<< std::endl;
+				}
+
+				/*if(diff_angle>angle_increment_of_increment_distance_){
+					angle_diference2=angle_diference2-angle_increment_of_increment_distance_;
+					double actual_angle_cost=actual_angle-angle_increment_of_increment_distance_;
+					//double act_companion_cost=(0.000000015242)*(pow(actual_angle_cost-angle_companion_,4));
+				}*/
+
+				//double actual_angle_cost=actual_angle-angle_increment_of_increment_distance_;
+				//orientation_person_robot_angles_[actual_parent_index]=actual_angle_cost; // modificas los angulos anteriores para que sea menos abrupto el cambio.
+				actual_index=actual_parent_index;
+				actual_parent_index=parent_index_vector_[actual_index]; //busca el nodo anterior de este path.
+				//BEST_path_parent_index_vector_.push_back(actual_index);
+				//std::cout << " (final while) actual_index="<<actual_index<<"; BEST_path_parent_index_vector_.back()"<<BEST_path_parent_index_vector_.back()<< std::endl;
+
+			}while(actual_parent_index>0);//&&(diff_angle>angle_increment_of_increment_distance_));
+			// PARA EL INDICE =0, PQ SINO ENTRA EN BUCLE INFINITO!
+			actual_angle=orientation_person_robot_angles_[actual_index];
+			actual_parent_angle=orientation_person_robot_angles_[actual_parent_index];
+			if(actual_angle<actual_parent_angle){
+				diff_angle=actual_parent_angle-actual_angle;
+				if(diff_angle>angle_increment_of_increment_distance_){
+					orientation_person_robot_angles_[actual_parent_index]=actual_angle+angle_increment_of_increment_distance_;
+					if(orientation_person_robot_angles_[actual_parent_index]>angle_companion_){
+						orientation_person_robot_angles_[actual_parent_index]=angle_companion_;
+					}
+				}
+			}
+			//BEST_path_parent_index_vector_.push_back(actual_parent_index);
+
+			//sum_angles_increment1=sum_angles_increment1/count_sum;
+			//std::cout << " [IMPORTANTE, ver si es diferente en pasillo y obstaculo!!!] sum_angles_increment1="<<sum_angles_increment1<< std::endl;
+
+
+			//std::cout << " (final while) actual_parent_index="<<actual_parent_index<<"; BEST_path_parent_index_vector_.back()"<<BEST_path_parent_index_vector_.back()<< std::endl;
+			//if(actual_debug_){
+				std::cout << " BEST_path_parent_index_vector_.size()="<<BEST_path_parent_index_vector_.size()<< std::endl;
+			//}
+			//if(debug_comanion_good_){
+				std::cout << "actual_index=" <<actual_index<<"; actual_parent_index="<<actual_parent_index<< std::endl;
+				std::cout << "(path) (last angle that could change!!! ) orientation_person_robot_angles_[actual_parent_index="<<0<<"]=" <<orientation_person_robot_angles_[0]<< std::endl;
+				std::cout << "(path) cost_companion_[actual_parent_index="<<0<<"]=" <<cost_companion_[0]<< std::endl;
+			//}
+
+/*  caso_vuelta_al_reves!!!!   */
+			if(!caso_vuelta_al_reves){
+
+				std::cout << " CASO VUELTA AL REVES: "<< std::endl;
+
+				// INICIO vuelta hacia delante!
+				//bool flag_return_90_dg=false;
+				//unsigned int second_index=BEST_path_parent_index_vector_[BEST_path_parent_index_vector_.size()-2];
+				//double second_angle=orientation_person_robot_angles_[second_index];
+				/*if((second_angle==angle_companion_)&&(orientation_person_robot_angles_[0]==angle_companion_)){//(orientation_person_robot_angles_[0]>(angle_companion_-5))&&(orientation_person_robot_angles_[0]<(angle_companion_+5))){
+				//	std::cout << "flag_return_90_dg=true"<< std::endl;
+					flag_return_90_dg=true;
+				}*/
+
+				//double sum_angles_increment=0;
+
+				for(unsigned int g=0;g<BEST_path_parent_index_vector_.size();g++){
+					unsigned int act_index=BEST_path_parent_index_vector_[BEST_path_parent_index_vector_.size()-1-g];
+					unsigned int next_index=BEST_path_parent_index_vector_[BEST_path_parent_index_vector_.size()-2-g];
+					double act_angle=orientation_person_robot_angles_[act_index];
+					double next_angle=orientation_person_robot_angles_[next_index];
+					double diff_angle=next_angle-act_angle;
+
+					//if((act_index!=0)&&(diff_angle>angle_increment_of_increment_distance_)){
+					if(((diff_angle>angle_increment_of_increment_distance_))&&(diff_angle>0)){
+
+						std::cout << " In if (!!!CASE!!! actual_angle > actual_parent_angle ) act_index="<<act_index<<"; next_index="<<next_index<< std::endl;
+						std::cout << "  act_angle="<<act_angle<<"; next_angle="<<next_angle<<";  diff_angle="<< diff_angle<<"; angle_increment_of_increment_distance_="<<angle_increment_of_increment_distance_<< std::endl;
+
+					//	std::cout << "(!!!CASE!!! actual_angle > actual_parent_angle ) act_index="<<act_index<<"; next_index="<<next_index<< std::endl;
+							// CASE 2: actual_angle > actual_parent_angle (aquí no se puede hacer...) Hay que dar vuelta hacia delante.
+						std::cout << "(before modif) orientation_person_robot_angles_[next_index="<<next_index<<"]="<<orientation_person_robot_angles_[next_index]<< std::endl;
+
+						if(actual_debug_){
+							std::cout << "if (CASE: salgo de obstaculo)"<< std::endl;
+						}
+						//if(next_index!=0){
+							double new_angle=act_angle+angle_increment_of_increment_distance_;
+							if(new_angle>angle_companion_){
+								orientation_person_robot_angles_[next_index]=angle_companion_;
+							}else{
+								orientation_person_robot_angles_[next_index]=new_angle;
+							}
+							//if(actual_debug_){
+								std::cout << "(modif) orientation_person_robot_angles_[next_index="<<next_index<<"]="<<orientation_person_robot_angles_[next_index]<< std::endl;
+							//}
+						//}
+					}else{
+						//if(next_index!=0){
+						/*orientation_person_robot_angles_[next_index]=orientation_person_robot_angles_[act_angle]+angle_increment_of_increment_distance_;
+							if(orientation_person_robot_angles_[next_index]>angle_companion_){
+								orientation_person_robot_angles_[next_index]=angle_companion_;
+							}*/
+						//}
+						//	std::cout << " (FOR) (real) next_angle="<<orientation_person_robot_angles_[next_index]<< std::endl;
+
+					}
+					/*else if((diff_angle>angle_increment_of_increment_distance_)&&(flag_return_90_dg)){
+						// CASE 3: actual_angle > actual_parent_angle (aquí no se puede hacer...) Hay que dar vuelta hacia delante.
+						// caso: orientation_person_robot_angles_[0=[BEST_path_parent_index_vector_.size()-1]<angle_companion_
+						// &
+						if(actual_debug_){
+							std::cout << "(caso0) if (CASE: salgo de obstaculo)"<< std::endl;
+						}
+						double new_angle=act_angle+angle_increment_of_increment_distance_;
+						if(new_angle>angle_companion_){
+							orientation_person_robot_angles_[next_index]=angle_companion_;
+						}else{
+							orientation_person_robot_angles_[next_index]=new_angle;
+						}
+						if(actual_debug_){
+							std::cout << "(modif_caso0) orientation_person_robot_angles_[next_index="<<next_index<<"]="<<orientation_person_robot_angles_[next_index]<< std::endl;
+						}
+					}*/
+
+
+					//sum_angles_increment=sum_angles_increment+orientation_person_robot_angles_[act_index];
+
+				} // fin for vuelta hacia delante
+
+				//sum_angles_increment=sum_angles_increment/BEST_path_parent_index_vector_.size();
+				//std::cout << " [IMPORTANTE, ver si es diferente en pasillo y obstaculo!!!] sum_angles_increment="<<sum_angles_increment<< std::endl;
+
+			}
+		} // fin if(cost_companion!=0)
+
+////////////////////////////
+		std::cout << " INICIO vuelta para mantenerse a 90 grados!!! "<< std::endl;
+
+		//std::cout << " tercera pasada, ahora mismo no se para que!"<< std::endl;
+		// es para mantenerte a 90 grados si decrece tu valor en orientacion!!! Te adelantas demasiado
+		if(!BEST_path_parent_index_vector_.empty()){
+			bool we_have_path_collisions=false;
+			for(unsigned int g=0;g<BEST_path_parent_index_vector_.size();g++){
+
+				//if(debug_angles_){
+					std::cout << " BEST_path_parent_index_vector_[BEST_path_parent_index_vector_.size()-1-g]="<<BEST_path_parent_index_vector_[BEST_path_parent_index_vector_.size()-1-g]<< std::endl;
+					std::cout << " vector_of_companion_collisions_[BEST_path_parent_index_vector_[BEST_path_parent_index_vector_.size()-1-g]]="<<vector_of_companion_collisions_[BEST_path_parent_index_vector_[BEST_path_parent_index_vector_.size()-1-g]]<< std::endl;
+					std::cout << " angle="<<orientation_person_robot_angles_[BEST_path_parent_index_vector_[BEST_path_parent_index_vector_.size()-1-g]]<< std::endl;
+					std::cout << " angle_companion_+2*angle_increment_of_increment_distance_="<<angle_companion_+2*angle_increment_of_increment_distance_<< std::endl;
+				//}
+
+				if((vector_of_companion_collisions_[BEST_path_parent_index_vector_[BEST_path_parent_index_vector_.size()-1-g]])&&(orientation_person_robot_angles_[BEST_path_parent_index_vector_[BEST_path_parent_index_vector_.size()-1-g]]>(angle_companion_+2*angle_increment_of_increment_distance_))){
+					we_have_path_collisions=true;
+				}
+			}
+
+			//if(debug_angles_){
+				std::cout << " !BEST_path_parent_index_vector_.empty()="<<!BEST_path_parent_index_vector_.empty()<<"; BEST_path_parent_index_vector_.size()="<<BEST_path_parent_index_vector_.size()<< std::endl;
+				std::cout << " !we_have_path_collisions="<<!we_have_path_collisions<<"; angle_companion_="<<angle_companion_<< std::endl;
+			//}
+
+			double ini_act_angle_out=initial_angle_;
+
+			//if(debug_angles_){
+				std::cout << " ini_act_angle_out="<< ini_act_angle_out << std::endl;
+			//}
+
+			if((!we_have_path_collisions)&&(ini_act_angle_out<angle_companion_)){
+
+				std::cout << "in if! "<< std::endl;
+
+				for(unsigned int g=1;g<BEST_path_parent_index_vector_.size();g++){
+					std::cout << "in for! "<< std::endl;
+
+
+					unsigned int act_index;//=best_plan_vertex_index_[best_plan_vertex_index_.size()-g];
+
+					unsigned int next_index=BEST_path_parent_index_vector_[0+g];
+					double act_angle;//=orientation_person_robot_angles_[act_index];
+					if(g==1){
+						act_angle=ini_act_angle_out;
+						act_index=0;
+					}else{
+						act_angle=orientation_person_robot_angles_[act_index];
+					}
+
+					double next_angle=act_angle+5*angle_increment_of_increment_distance_;
+
+					std::cout << " (before modify) act_index="<< act_index<<"; next_index="<<next_index<<"; act_angle="<<act_angle<<"; next_angle="<<next_angle<< std::endl;
+					std::cout << "orientation_person_robot_angles_[next_index]="<< orientation_person_robot_angles_[next_index]<< std::endl;
+
+					if(next_angle>angle_companion_){
+						//next_angle=orientation_person_robot_angles_[act_index]-angle_increment_of_increment_distance_;
+						orientation_person_robot_angles_[next_index]=angle_companion_;
+					}else{
+						orientation_person_robot_angles_[next_index]=next_angle;
+					}
+
+					//if(debug_angles_){
+						std::cout << "( after )  act_index="<< act_index<<"; next_index="<<next_index<<"; act_angle="<<act_angle<<"; next_angle="<<next_angle<< std::endl;
+						std::cout << "orientation_person_robot_angles_[next_index]="<< orientation_person_robot_angles_[next_index]<< std::endl;
+					//}
+
+					act_index=next_index;
+				}
+			}
+		}
+
+
+
+///////////////////
+
+		//if(debug_comanion_good_){
+			if(!BEST_path_parent_index_vector_.empty()){
+				for(unsigned int f=0;f<BEST_path_parent_index_vector_.size();f++){
+					std::cout << "(BEST path index) f="<<f<<"; index=" <<BEST_path_parent_index_vector_[f]<< std::endl;
+				}
+			}
+
+			std::cout << "SHOW!!! orientation_person_robot_angles_:"<< std::endl;
+
+			for(unsigned int p=0; p<orientation_person_robot_angles_.size();p++){
+				std::cout << "SHOW!!! orientation_person_robot_angles_[p="<<p<<"]="<<orientation_person_robot_angles_[p]<< std::endl;
+
+			}
+
+
+		//}
+
+		std::cout << "FIN only_angle_in_final_tree_calculate_companion_path_angle_and_cost"<< std::endl;
+}
+
+/*  Arreglar angulos para pasar detras del robot   */
+void Cplan_local_nav_person_companion::go_behind_robot_only_angle_in_final_tree_calculate_companion_path_angle_and_cost3(unsigned int i){
+
+	// TODO: arreglar para que vaya por detras de la persona en los obstaculos y no por delante!!!
+
+	/*std::cout << "    INI orientation_person_robot_angles_[g]: "<< std::endl;
+
+	for(unsigned int y=0;y<orientation_person_robot_angles_.size();y++){
+		std::cout << "    orientation_person_robot_angles_[y]="<<orientation_person_robot_angles_[y]<< std::endl;
+	}*/
+
+
+
+	// calcula los angulos que hay que moverse en ese path.
+	if(debug_real_test_companion2_){
+		std::cout << "    !!!    INI go_behind_robot-only_angle_in_final_tree_calculate_companion_path_angle_and_cost"<< std::endl;
+	}
+	BEST_path_parent_index_vector_.clear(); // de atras a delante. (en el ultimo coges el primer indice.)
+
+	// CASE 1: actual_angle < actual_parent_angle. Dar vuelta hacia atras del arbol (primera vuelta).
+	unsigned int actual_index;
+	unsigned int actual_parent_index;
+
+	if(calc_goal_companion_with_group_path_){
+		actual_index=i; // recorrete todo el path the este final de camino.
+		actual_parent_index=parent_index_vector_[actual_index];
+	}else{
+		actual_index=orientation_person_robot_angles_with_prediction_of_person_companion_.size()-1; // recorrete todo el path the este final de camino.
+		actual_parent_index=orientation_person_robot_angles_with_prediction_of_person_companion_.size()-2;
+	}
+
+	double actual_angle;
+	double actual_parent_angle;
+
+	if(calc_goal_companion_with_group_path_){
+		actual_angle=orientation_person_robot_angles_[actual_index];
+		actual_parent_angle=orientation_person_robot_angles_[actual_parent_index];
+	}else{
+		actual_angle=orientation_person_robot_angles_with_prediction_of_person_companion_[actual_index];
+		actual_parent_angle=orientation_person_robot_angles_with_prediction_of_person_companion_[actual_parent_index];
+	}
+
+	double diff_angle=actual_parent_angle-actual_angle; // saltos posibles 1- 90grad a 180grad o 0 grad.
+
+	if(debug_real_test_companion2_){																		// 2- de 0grad o 180grad a 90grad.
+		std::cout << "actual_angle=" <<actual_angle<<"; actual_parent_angle="<<actual_parent_angle<<"; diff_angle="<<diff_angle<< std::endl;
+	}
+
+	//bool caso_vuelta_al_reves=true;
+
+	unsigned int actual_index2; // recorrete todo el path the este final de camino.
+	unsigned int actual_parent_index2;
+	//if(calc_goal_companion_with_group_path_){
+		actual_index2=i;
+		actual_parent_index2=parent_index_vector_[actual_index2];
+		BEST_path_parent_index_vector_.push_back(actual_index2);
+		//std::cout << " (ini while) actual_index="<<actual_index<<"; BEST_path_parent_index_vector_.back()"<<BEST_path_parent_index_vector_.back()<< std::endl;
+		do{
+			BEST_path_parent_index_vector_.push_back(actual_index2);
+			actual_index2=actual_parent_index2;
+			actual_parent_index2=parent_index_vector_[actual_index2];
+
+		}while(actual_parent_index2>0);
+		BEST_path_parent_index_vector_.push_back(actual_parent_index2);
+	///} //else nada en este caso.
+
+	// 1- vuelta del final del path al principio!
+	//double sum_angles_increment1=0;
+	//int count_sum=0;
+	do{
+		//std::cout << " do while 1 "<< std::endl;
+		if(calc_goal_companion_with_group_path_){
+			actual_angle=orientation_person_robot_angles_[actual_index];
+			actual_parent_angle=orientation_person_robot_angles_[actual_parent_index];
+		}else{
+			actual_angle=orientation_person_robot_angles_with_prediction_of_person_companion_[actual_index];
+			actual_parent_angle=orientation_person_robot_angles_with_prediction_of_person_companion_[actual_parent_index];
+		}
+
+		if(debug_real_test_companion2_){
+			std::cout << "actual_angle=" <<actual_angle<<"; actual_parent_angle="<<actual_parent_angle<<"; diff_angle="<<diff_angle<< std::endl;
+		}
+		//std::cout << " actual_angle"<<actual_angle<<";> actual_parent_angle="<<actual_parent_angle << std::endl;
+		if(actual_angle>actual_parent_angle){
+			//caso_vuelta_al_reves=true;
+			//std::cout << " do while 3 "<< std::endl;
+			diff_angle=actual_angle-actual_parent_angle;
+			//std::cout << " diff_angle="<<diff_angle<< std::endl;
+			if(diff_angle>angle_increment_of_increment_distance_){
+				//if(debug_comanion_good_){
+				//	std::cout << " (DO-WILE) Int if ->(path BEFORE CHANGE) orientation_person_robot_angles_[actual_parent_index="<<actual_parent_index<<"]=" <<orientation_person_robot_angles_[actual_parent_index]<< std::endl;
+				//}
+				//std::cout << " do while 5 "<< std::endl;
+				if(debug_angles_){
+					std::cout << "(caso vuelta diff_algle +)= actual_angle - actual_parent_angle = diff_angle=" <<diff_angle<<"; angle_increment_of_increment_distance_="<<angle_increment_of_increment_distance_<< std::endl;
+				}
+				//std::cout << " do while 6 "<< std::endl;
+				//std::cout << "(DO-WILE) (!!!CASE!!! actual_angle < actual_parent_angle)"<< std::endl;
+				if(calc_goal_companion_with_group_path_){
+					orientation_person_robot_angles_[actual_parent_index]=actual_angle-angle_increment_of_increment_distance_;
+				}else{
+					orientation_person_robot_angles_with_prediction_of_person_companion_[actual_parent_index]=actual_angle-angle_increment_of_increment_distance_;
+				}
+				//std::cout << " do while 7 "<< std::endl;
+
+
+
+				if(actual_debug2_){
+					std::cout << "if (CASE: entro en obstaculo)"<< std::endl;
+					std::cout << "actual_index=" <<actual_index<<"; actual_parent_index="<<actual_parent_index<< std::endl;
+					std::cout << "actual_angle=" <<actual_angle<<"; actual_parent_angle="<<actual_parent_angle<<"; diff_angle="<<diff_angle<< std::endl;
+					std::cout << "(path) orientation_person_robot_angles_[actual_index="<<actual_index<<"]=" <<orientation_person_robot_angles_[actual_index]<< std::endl;
+					std::cout << "(path) orientation_person_robot_angles_[actual_parent_index="<<actual_parent_index<<"]=" <<orientation_person_robot_angles_[actual_parent_index]<< std::endl;
+					std::cout << "(path) min_distance_collision_vector_[actual_index="<<actual_index<<"]=" <<min_distance_collision_vector_[actual_index]<< std::endl;
+					std::cout << "(path) min_distance_collision_vector_[actual_parent_index="<<actual_parent_index<<"]=" <<min_distance_collision_vector_[actual_parent_index]<< std::endl;
+
+					//std::cout << "(path) cost_companion_[actual_parent_index="<<actual_parent_index<<"]=" <<cost_companion_[actual_parent_index]<< std::endl;
+				}
+				//std::cout << " do while 8 "<< std::endl;
+				if(calc_goal_companion_with_group_path_){
+					if(orientation_person_robot_angles_[actual_parent_index]<angle_companion_){
+						orientation_person_robot_angles_[actual_parent_index]=angle_companion_;
+					}
+					if(orientation_person_robot_angles_[actual_parent_index]>180){
+						orientation_person_robot_angles_[actual_parent_index]=180;
+					}
+				}else{
+					if(orientation_person_robot_angles_with_prediction_of_person_companion_[actual_parent_index]<angle_companion_){
+						orientation_person_robot_angles_with_prediction_of_person_companion_[actual_parent_index]=angle_companion_;
+					}
+					if(orientation_person_robot_angles_with_prediction_of_person_companion_[actual_parent_index]>180){
+						orientation_person_robot_angles_with_prediction_of_person_companion_[actual_parent_index]=180;
+					}
+				}
+
+				//std::cout << " do while 9 "<< std::endl;
+				if(debug_angles_){
+					std::cout << "orientation_person_robot_angles_[actual_parent_index]"<< orientation_person_robot_angles_[actual_parent_index]<<" actual_angle-angle_increment_of_increment_distance_="<<actual_angle-angle_increment_of_increment_distance_<< std::endl;
+				}
+
+			}
+			diff_angle=diff_angle-angle_increment_of_increment_distance_;
+		}else{ // CASE: actual_angle > actual_parent_angle (aquí no se puede hacer...) Hay que dar vuelta hacia delante.
+
+		}
+
+		if(debug_comanion_good_){
+			std::cout << "actual_index=" <<actual_index<<"; actual_parent_index="<<actual_parent_index<< std::endl;
+			std::cout << "actual_angle=" <<actual_angle<<"; actual_parent_angle="<<actual_parent_angle<<"; diff_angle="<<diff_angle<< std::endl;
+			std::cout << "(path) orientation_person_robot_angles_[actual_parent_index="<<actual_parent_index<<"]=" <<orientation_person_robot_angles_[actual_parent_index]<< std::endl;
+			std::cout << "(path) cost_companion_[actual_parent_index="<<actual_parent_index<<"]=" <<cost_companion_[actual_parent_index]<< std::endl;
+		}
+		//std::cout << " do while 10 "<< std::endl;
+		if(calc_goal_companion_with_group_path_){
+			actual_index=actual_parent_index;
+			actual_parent_index=parent_index_vector_[actual_index]; //busca el nodo anterior de este path.
+		}else{
+			actual_index=actual_index-1;
+			actual_parent_index=actual_parent_index-1; //busca el nodo anterior de este path.
+		}
+		//std::cout << " do while 11 "<<"; actual_index="<<actual_index<<"; actual_parent_index="<<actual_parent_index<< std::endl;
+		//std::cout << " (final while) actual_index="<<actual_index<<"; BEST_path_parent_index_vector_.back()"<<BEST_path_parent_index_vector_.back()<< std::endl;
+
+	}while(actual_parent_index>0);//&&(diff_angle>angle_increment_of_increment_distance_));
+	// PARA EL INDICE =0, PQ SINO ENTRA EN BUCLE INFINITO!
+
+	if(calc_goal_companion_with_group_path_){
+		actual_angle=orientation_person_robot_angles_[actual_index];
+		actual_parent_angle=orientation_person_robot_angles_[actual_parent_index];
+	}else{
+		actual_angle=orientation_person_robot_angles_with_prediction_of_person_companion_[actual_index];
+		actual_parent_angle=orientation_person_robot_angles_with_prediction_of_person_companion_[actual_parent_index];
+	}
+
+	//std::cout << " do while 12 "<<"; actual_index="<<actual_index<<"; actual_parent_index="<<actual_parent_index<< std::endl;
+
+	if(actual_angle<actual_parent_angle){
+		diff_angle=actual_parent_angle-actual_angle;
+		if(diff_angle>angle_increment_of_increment_distance_){
+
+			if(debug_angles_){
+				std::cout << "(caso vuelta diff_algle +)= actual_angle - actual_parent_angle = diff_angle=" <<diff_angle<<"; angle_increment_of_increment_distance_="<<angle_increment_of_increment_distance_<< std::endl;
+			}
+			//std::cout << " do while 13 "<<"; actual_index="<<actual_index<<"; actual_parent_index="<<actual_parent_index<< std::endl;
+
+			if(calc_goal_companion_with_group_path_){
+				orientation_person_robot_angles_[actual_parent_index]=actual_angle+angle_increment_of_increment_distance_;
+				if(orientation_person_robot_angles_[actual_parent_index]<angle_companion_){
+					orientation_person_robot_angles_[actual_parent_index]=angle_companion_;
+				}
+				if(orientation_person_robot_angles_[actual_parent_index]>180){
+					orientation_person_robot_angles_[actual_parent_index]=180;
+				}
+			}else{
+				orientation_person_robot_angles_with_prediction_of_person_companion_[actual_parent_index]=actual_angle+angle_increment_of_increment_distance_;
+				if(orientation_person_robot_angles_with_prediction_of_person_companion_[actual_parent_index]<angle_companion_){
+					orientation_person_robot_angles_with_prediction_of_person_companion_[actual_parent_index]=angle_companion_;
+				}
+				if(orientation_person_robot_angles_with_prediction_of_person_companion_[actual_parent_index]>180){
+					orientation_person_robot_angles_with_prediction_of_person_companion_[actual_parent_index]=180;
+				}
+			}
+			//std::cout << " do while 14 "<<"; actual_index="<<actual_index<<"; actual_parent_index="<<actual_parent_index<< std::endl;
+
+			if(debug_angles_){
+				std::cout << "orientation_person_robot_angles_[actual_parent_index]"<< orientation_person_robot_angles_[actual_parent_index]<<" actual_angle-angle_increment_of_increment_distance_="<<actual_angle-angle_increment_of_increment_distance_<< std::endl;
+			}
+		}
+	}
+	//std::cout << " do while 15 "<<"; actual_index="<<actual_index<<"; actual_parent_index="<<actual_parent_index<< std::endl;
+
+	//std::cout << " [IMPORTANTE, ver si es diferente en pasillo y obstaculo!!!] sum_angles_increment1="<<sum_angles_increment1<< std::endl;
+
+
+	/*std::cout << "  2  INI orientation_person_robot_angles_[g]: "<< std::endl;
+
+		for(unsigned int y=0;y<orientation_person_robot_angles_.size();y++){
+			std::cout << "    orientation_person_robot_angles_[y]="<<orientation_person_robot_angles_[y]<< std::endl;
+		}*/
+
+
+	if(!calc_goal_companion_with_group_path_){
+		// TODO: usar funcion, vueltas_hacia_delante_arreglar_angulos_para_caso_min_companion_angle_con_prediccion.
+		//std::cout << " do while 16 "<< std::endl;
+
+		fix_angles_to_use_person_prediction_for_the_companion_goal();
+		//std::cout << " do while 17 "<< std::endl;
+
+
+
+
+	}else{
+	///////////////77
+		//std::cout << " (final while) actual_parent_index="<<actual_parent_index<<"; BEST_path_parent_index_vector_.back()"<<BEST_path_parent_index_vector_.back()<< std::endl;
+		if(actual_debug_){
+			std::cout << " BEST_path_parent_index_vector_.size()="<<BEST_path_parent_index_vector_.size()<< std::endl;
+		}
+		if(debug_comanion_good_){
+			std::cout << "actual_index=" <<actual_index<<"; actual_parent_index="<<actual_parent_index<< std::endl;
+			std::cout << "(path) orientation_person_robot_angles_[actual_parent_index="<<0<<"]=" <<orientation_person_robot_angles_[0]<< std::endl;
+			std::cout << "(path) cost_companion_[actual_parent_index="<<0<<"]=" <<cost_companion_[0]<< std::endl;
+		}
+
+		unsigned int act_index1=BEST_path_parent_index_vector_[BEST_path_parent_index_vector_.size()-1];
+		unsigned int next_index1=BEST_path_parent_index_vector_[BEST_path_parent_index_vector_.size()-2];
+		double act_angle1=orientation_person_robot_angles_[act_index1];
+		double next_angle1=orientation_person_robot_angles_[next_index1];
+		double diff_angle1=act_angle1-next_angle1;
+		if(diff_angle1>angle_increment_of_increment_distance_){
+			//caso_vuelta_al_reves=true;
+		}
+
+		// 2- vuelta del principio al final
+	/*  caso_vuelta_al_reves!!!!   */
+
+		for(unsigned int g=0;g<BEST_path_parent_index_vector_.size();g++){
+
+			if(debug_angles_){
+				std::cout << " 2 (vuelta al reves!) BEST_path_parent_index_vector_.size()-1-g="<<BEST_path_parent_index_vector_.size()-1-g<<"; BEST_path_parent_index_vector_.size()-2-g="<<BEST_path_parent_index_vector_.size()-2-g<< std::endl;
+			}
+
+			unsigned int act_index=BEST_path_parent_index_vector_[BEST_path_parent_index_vector_.size()-1-g];
+			unsigned int next_index=BEST_path_parent_index_vector_[BEST_path_parent_index_vector_.size()-2-g];
+
+			double act_angle=orientation_person_robot_angles_[act_index];
+			double next_angle=orientation_person_robot_angles_[next_index];
+
+			if((act_index==0)){
+				act_angle=initial_angle_;
+			}
+			if(next_angle==0){
+				next_angle=initial_angle_;
+			}
+
+			if(debug_angles_){
+				std::cout << " 2 (vuelta hacia delante de INI_angle_act_angle al qur necesitas para pasar.!) act_index="<<act_index<<"; next_index="<<next_index<< std::endl;
+			}
+
+			if((BEST_path_parent_index_vector_.size()-1-g)==0){
+				act_angle=initial_angle_;
+			}
+
+			double diff_angle=act_angle-next_angle;
+
+			if(debug_angles_){
+				std::cout << "act_angle["<<act_index<<"]="<<act_angle<<"next_angle["<<next_index<<"]"<<next_angle<<"diff_angle=act_angle-next_angle="<<diff_angle<<" > angle_increment_of_increment_distance_="<< angle_increment_of_increment_distance_<<" and diff_angle>0"<< std::endl;
+			}
+
+			if(((diff_angle>(2*angle_increment_of_increment_distance_)))&&(diff_angle>0)){
+				//std::cout << "ENTRO EN IF !!!"<< std::endl;
+
+				//	std::cout << " (FOR) ANT (!!!CASE!!! actual_angle > actual_parent_angle ) act_index="<<act_index<<"; next_index="<<next_index<< std::endl;
+				//	std::cout << "  act_angle="<<act_angle<<"; next_angle="<<next_angle<<";  diff_angle="<< diff_angle<< std::endl;
+
+				//	std::cout << "(!!!CASE!!! actual_angle > actual_parent_angle ) act_index="<<act_index<<"; next_index="<<next_index<< std::endl;
+				// CASE 2: actual_angle > actual_parent_angle (aquí no se puede hacer...) Hay que dar vuelta hacia delante.
+
+				if(actual_debug_){
+					std::cout << "if (CASE: salgo de obstaculo)"<< std::endl;
+				}
+
+				double new_angle=act_angle-(2*angle_increment_of_increment_distance_);
+
+				//if(debug_gazebo_journal_){
+				//	std::cout << "new_angle ["<<next_index<<"]="<<new_angle<< std::endl;
+				//}
+
+				if(new_angle<angle_companion_){
+					orientation_person_robot_angles_[next_index]=angle_companion_;
+					//std::cout << "if(new_angle<angle_companion_) angle_companion_="<<angle_companion_<< std::endl;
+				}else{
+					orientation_person_robot_angles_[next_index]=new_angle;
+					//std::cout << "else if(new_angle<angle_companion_)new_angle="<<new_angle<< std::endl;
+				}
+				if(new_angle>180){
+					orientation_person_robot_angles_[next_index]=180;
+				}
+				if(actual_debug_){
+					std::cout << "(modif) orientation_person_robot_angles_[next_index="<<next_index<<"]="<<orientation_person_robot_angles_[next_index]<< std::endl;
+				}
+
+			}else{
+
+				//std::cout << "ENTRO EN ELSE !!!"<< std::endl;
+
+				//	std::cout << " (FOR) (real) next_angle="<<orientation_person_robot_angles_[next_index]<< std::endl;
+
+			}
+
+			//sum_angles_increment=sum_angles_increment+orientation_person_robot_angles_[act_index];
+
+		} // fin for vuelta hacia delante
+
+		//sum_angles_increment=sum_angles_increment/BEST_path_parent_index_vector_.size();
+		//std::cout << " [IMPORTANTE, ver si es diferente en pasillo y obstaculo!!!] sum_angles_increment="<<sum_angles_increment<< std::endl;
+		//std::cout << " tercera pasada, ahora mismo no se para que!"<< std::endl;
+		// es para mantenerte a 90 grados si decrece tu valor en orientacion!!! Te adelantas demasiado
+		if(!BEST_path_parent_index_vector_.empty()){
+			bool we_have_path_collisions=false;
+			for(unsigned int g=0;g<BEST_path_parent_index_vector_.size();g++){
+
+				if(debug_angles_){
+					std::cout << " BEST_path_parent_index_vector_[BEST_path_parent_index_vector_.size()-1-g]="<<BEST_path_parent_index_vector_[BEST_path_parent_index_vector_.size()-1-g]<< std::endl;
+					std::cout << " vector_of_companion_collisions_[BEST_path_parent_index_vector_[BEST_path_parent_index_vector_.size()-1-g]]="<<vector_of_companion_collisions_[BEST_path_parent_index_vector_[BEST_path_parent_index_vector_.size()-1-g]]<< std::endl;
+					std::cout << " angle="<<orientation_person_robot_angles_[BEST_path_parent_index_vector_[BEST_path_parent_index_vector_.size()-1-g]]<< std::endl;
+					std::cout << " angle_companion_+2*angle_increment_of_increment_distance_="<<angle_companion_+2*angle_increment_of_increment_distance_<< std::endl;
+				}
+
+				if((vector_of_companion_collisions_[BEST_path_parent_index_vector_[BEST_path_parent_index_vector_.size()-1-g]])&&(orientation_person_robot_angles_[BEST_path_parent_index_vector_[BEST_path_parent_index_vector_.size()-1-g]]>(angle_companion_+2*angle_increment_of_increment_distance_))){
+					we_have_path_collisions=true;
+				}
+			}
+
+			if(debug_angles_){
+				std::cout << " !BEST_path_parent_index_vector_.empty()="<<!BEST_path_parent_index_vector_.empty()<< std::endl;
+				std::cout << " !we_have_path_collisions="<<!we_have_path_collisions<<"; angle_companion_="<<angle_companion_<< std::endl;
+			}
+
+			double ini_act_angle_out=initial_angle_;
+
+			if(debug_angles_){
+				std::cout << " ini_act_angle_out="<< ini_act_angle_out<< std::endl;
+			}
+
+			if((!we_have_path_collisions)&&(ini_act_angle_out>angle_companion_)){
+				for(unsigned int g=1;g<BEST_path_parent_index_vector_.size();g++){
+					unsigned int act_index;//=best_plan_vertex_index_[best_plan_vertex_index_.size()-g];
+
+					unsigned int next_index=BEST_path_parent_index_vector_[BEST_path_parent_index_vector_.size()-1-g];
+					double act_angle;//=orientation_person_robot_angles_[act_index];
+					if(g==1){
+						act_angle=ini_act_angle_out;
+						act_index=0;
+					}else{
+						act_angle=orientation_person_robot_angles_[act_index];
+					}
+
+					double next_angle=act_angle-5*angle_increment_of_increment_distance_;
+					if(next_angle<angle_companion_){
+						//next_angle=orientation_person_robot_angles_[act_index]-angle_increment_of_increment_distance_;
+						orientation_person_robot_angles_[next_index]=angle_companion_;
+					}else{
+						orientation_person_robot_angles_[next_index]=next_angle;
+					}
+
+					if(debug_angles_){
+						std::cout << " act_index="<< act_index<<"; next_index="<<next_index<<"; act_angle="<<act_angle<<"; next_angle="<<next_angle<< std::endl;
+						std::cout << "orientation_person_robot_angles_[next_index]="<< orientation_person_robot_angles_[next_index]<< std::endl;
+					}
+
+					act_index=next_index;
+				}
+			}
+		}
+
+
+		if(debug_comanion_good_){
+			if(!BEST_path_parent_index_vector_.empty()){
+				for(unsigned int f=0;f<BEST_path_parent_index_vector_.size();f++){
+					std::cout << "(BEST path index) f="<<f<<"; index=" <<BEST_path_parent_index_vector_[f]<< std::endl;
+				}
+			}
+
+			std::cout << "FIN only_angle_in_final_tree_calculate_companion_path_angle_and_cost"<< std::endl;
+		}
+
+	///////////////////////77777777
+	}
+
+
+
+	/*std::cout << "  3  INI orientation_person_robot_angles_[g]: "<< std::endl;
+
+				for(unsigned int y=0;y<orientation_person_robot_angles_.size();y++){
+					std::cout << "    orientation_person_robot_angles_[y]="<<orientation_person_robot_angles_[y]<< std::endl;
+				}*/
+
+	//std::cout << "FIN go_behind_robot ..."<< std::endl;
+
+}
+
+
+
+
+/*  Arreglar angulos para pasar detras del robot   */
+void Cplan_local_nav_person_companion::go_behind_robot_only_angle_in_final_tree_calculate_companion_path_angle_and_cost2(unsigned int i){
+
+	// TODO: arreglar para que vaya por detras de la persona en los obstaculos y no por delante!!!
+
+
+	// calcula los angulos que hay que moverse en ese path.
+	if(debug_real_test_companion2_){
+		std::cout << "    !!!    INI go_behind_robot-only_angle_in_final_tree_calculate_companion_path_angle_and_cost"<< std::endl;
+	}
+	BEST_path_parent_index_vector_.clear(); // de atras a delante. (en el ultimo coges el primer indice.)
+
+	// CASE 1: actual_angle < actual_parent_angle. Dar vuelta hacia atras del arbol (primera vuelta).
+	unsigned int actual_index=i; // recorrete todo el path the este final de camino.
+	unsigned int actual_parent_index=parent_index_vector_[actual_index];
+	double actual_angle=orientation_person_robot_angles_[actual_index];
+	double actual_parent_angle=orientation_person_robot_angles_[actual_parent_index];
+
+	double diff_angle=actual_parent_angle-actual_angle; // saltos posibles 1- 90grad a 180grad o 0 grad.
+	if(debug_real_test_companion2_){																		// 2- de 0grad o 180grad a 90grad.
+		std::cout << "actual_angle=" <<actual_angle<<"; actual_parent_angle="<<actual_parent_angle<<"; diff_angle="<<diff_angle<< std::endl;
+	}
+	bool caso_vuelta_al_reves=false;
+	BEST_path_parent_index_vector_.push_back(actual_index);
+	//std::cout << " (ini while) actual_index="<<actual_index<<"; BEST_path_parent_index_vector_.back()"<<BEST_path_parent_index_vector_.back()<< std::endl;
+		//if((cost_companion_[i]!=0)){//&&(actual_angle<actual_parent_angle)&&(diff_angle>angle_increment_of_increment_distance_)){ // si hay cost companion
+
+	do{
+		BEST_path_parent_index_vector_.push_back(actual_index);
+		actual_index=actual_parent_index;
+		actual_parent_index=parent_index_vector_[actual_index];
+	}while(actual_parent_index>0);
+	BEST_path_parent_index_vector_.push_back(actual_parent_index);
+
+
+		// 1- vuelta del final del path al principio!
+			//double sum_angles_increment1=0;
+			//int count_sum=0;
+			do{
+				actual_angle=orientation_person_robot_angles_[actual_index];
+				actual_parent_angle=orientation_person_robot_angles_[actual_parent_index];
+				/*if(actual_angle>actual_parent_angle){
+					std::cout << " Int if ->(path BEFORE CHANGE) orientation_person_robot_angles_[actual_parent_index="<<actual_parent_index<<"]=" <<orientation_person_robot_angles_[actual_parent_index]<< std::endl;
+					diff_angle=actual_angle-actual_parent_angle;
+					if(diff_angle>angle_increment_of_increment_distance_){
+						orientation_person_robot_angles_[actual_parent_index]=actual_angle-angle_increment_of_increment_distance_;
+					}
+				}*/
+
+				//sum_angles_increment1=sum_angles_increment1+(orientation_person_robot_angles_[actual_index]-orientation_person_robot_angles_[actual_parent_index]);
+				//count_sum++;
+				if(debug_real_test_companion2_){
+					std::cout << "actual_angle=" <<actual_angle<<"; actual_parent_angle="<<actual_parent_angle<<"; diff_angle="<<diff_angle<< std::endl;
+				}
+
+				if(actual_angle>actual_parent_angle){
+					caso_vuelta_al_reves=true;
+					diff_angle=actual_angle-actual_parent_angle;
+
+					if(diff_angle>angle_increment_of_increment_distance_){
+						//if(debug_comanion_good_){
+						//	std::cout << " (DO-WILE) Int if ->(path BEFORE CHANGE) orientation_person_robot_angles_[actual_parent_index="<<actual_parent_index<<"]=" <<orientation_person_robot_angles_[actual_parent_index]<< std::endl;
+						//}
+						if(debug_real_test_companion2_){
+							std::cout << "diff_angle=" <<diff_angle<<"; angle_increment_of_increment_distance_="<<angle_increment_of_increment_distance_<< std::endl;
+						}
+
+						//std::cout << "(DO-WILE) (!!!CASE!!! actual_angle < actual_parent_angle)"<< std::endl;
+						orientation_person_robot_angles_[actual_parent_index]=actual_angle-angle_increment_of_increment_distance_;
+
+						if(actual_debug2_){
+							std::cout << "if (CASE: entro en obstaculo)"<< std::endl;
+							std::cout << "actual_index=" <<actual_index<<"; actual_parent_index="<<actual_parent_index<< std::endl;
+							std::cout << "actual_angle=" <<actual_angle<<"; actual_parent_angle="<<actual_parent_angle<<"; diff_angle="<<diff_angle<< std::endl;
+							std::cout << "(path) orientation_person_robot_angles_[actual_index="<<actual_index<<"]=" <<orientation_person_robot_angles_[actual_index]<< std::endl;
+							std::cout << "(path) orientation_person_robot_angles_[actual_parent_index="<<actual_parent_index<<"]=" <<orientation_person_robot_angles_[actual_parent_index]<< std::endl;
+							std::cout << "(path) min_distance_collision_vector_[actual_index="<<actual_index<<"]=" <<min_distance_collision_vector_[actual_index]<< std::endl;
+							std::cout << "(path) min_distance_collision_vector_[actual_parent_index="<<actual_parent_index<<"]=" <<min_distance_collision_vector_[actual_parent_index]<< std::endl;
+
+							//std::cout << "(path) cost_companion_[actual_parent_index="<<actual_parent_index<<"]=" <<cost_companion_[actual_parent_index]<< std::endl;
+						}
+						if(orientation_person_robot_angles_[actual_parent_index]<angle_companion_){
+								orientation_person_robot_angles_[actual_parent_index]=angle_companion_;
+						}
+						if(orientation_person_robot_angles_[actual_parent_index]>180){
+							orientation_person_robot_angles_[actual_parent_index]=180;
+						}
+					}
+					diff_angle=diff_angle-angle_increment_of_increment_distance_;
+				}else{ // CASE: actual_angle > actual_parent_angle (aquí no se puede hacer...) Hay que dar vuelta hacia delante.
+
+				}
+
+				/*else{
+					diff_angle=actual_parent_angle-actual_angle;
+					if(diff_angle>angle_increment_of_increment_distance_){
+
+					}
+				}*/
+				if(debug_comanion_good_){
+					std::cout << "actual_index=" <<actual_index<<"; actual_parent_index="<<actual_parent_index<< std::endl;
+					std::cout << "actual_angle=" <<actual_angle<<"; actual_parent_angle="<<actual_parent_angle<<"; diff_angle="<<diff_angle<< std::endl;
+					std::cout << "(path) orientation_person_robot_angles_[actual_parent_index="<<actual_parent_index<<"]=" <<orientation_person_robot_angles_[actual_parent_index]<< std::endl;
+					std::cout << "(path) cost_companion_[actual_parent_index="<<actual_parent_index<<"]=" <<cost_companion_[actual_parent_index]<< std::endl;
+				}
+
+				/*if(diff_angle>angle_increment_of_increment_distance_){
+					angle_diference2=angle_diference2-angle_increment_of_increment_distance_;
+					double actual_angle_cost=actual_angle-angle_increment_of_increment_distance_;
+					//double act_companion_cost=(0.000000015242)*(pow(actual_angle_cost-angle_companion_,4));
+				}*/
+
+				//double actual_angle_cost=actual_angle-angle_increment_of_increment_distance_;
+				//orientation_person_robot_angles_[actual_parent_index]=actual_angle_cost; // modificas los angulos anteriores para que sea menos abrupto el cambio.
+				actual_index=actual_parent_index;
+				actual_parent_index=parent_index_vector_[actual_index]; //busca el nodo anterior de este path.
+				//BEST_path_parent_index_vector_.push_back(actual_index);
+				//std::cout << " (final while) actual_index="<<actual_index<<"; BEST_path_parent_index_vector_.back()"<<BEST_path_parent_index_vector_.back()<< std::endl;
+
+			}while(actual_parent_index>0);//&&(diff_angle>angle_increment_of_increment_distance_));
+			// PARA EL INDICE =0, PQ SINO ENTRA EN BUCLE INFINITO!
+			actual_angle=orientation_person_robot_angles_[actual_index];
+			actual_parent_angle=orientation_person_robot_angles_[actual_parent_index];
+			if(actual_angle<actual_parent_angle){
+				diff_angle=actual_parent_angle-actual_angle;
+				if(diff_angle>angle_increment_of_increment_distance_){
+					orientation_person_robot_angles_[actual_parent_index]=actual_angle+angle_increment_of_increment_distance_;
+					if(orientation_person_robot_angles_[actual_parent_index]<angle_companion_){
+						orientation_person_robot_angles_[actual_parent_index]=angle_companion_;
+					}
+					if(orientation_person_robot_angles_[actual_parent_index]>180){
+						orientation_person_robot_angles_[actual_parent_index]=180;
+					}
+				}
+			}
+
+
+			//sum_angles_increment1=sum_angles_increment1/count_sum;
+			//std::cout << " [IMPORTANTE, ver si es diferente en pasillo y obstaculo!!!] sum_angles_increment1="<<sum_angles_increment1<< std::endl;
+
+
+			//std::cout << " (final while) actual_parent_index="<<actual_parent_index<<"; BEST_path_parent_index_vector_.back()"<<BEST_path_parent_index_vector_.back()<< std::endl;
+			if(actual_debug_){
+				std::cout << " BEST_path_parent_index_vector_.size()="<<BEST_path_parent_index_vector_.size()<< std::endl;
+			}
+			if(debug_comanion_good_){
+				std::cout << "actual_index=" <<actual_index<<"; actual_parent_index="<<actual_parent_index<< std::endl;
+				std::cout << "(path) orientation_person_robot_angles_[actual_parent_index="<<0<<"]=" <<orientation_person_robot_angles_[0]<< std::endl;
+				std::cout << "(path) cost_companion_[actual_parent_index="<<0<<"]=" <<cost_companion_[0]<< std::endl;
+			}
+
+			unsigned int act_index1=BEST_path_parent_index_vector_[BEST_path_parent_index_vector_.size()-1];
+			unsigned int next_index1=BEST_path_parent_index_vector_[BEST_path_parent_index_vector_.size()-2];
+			double act_angle1=orientation_person_robot_angles_[act_index1];
+			double next_angle1=orientation_person_robot_angles_[next_index1];
+			double diff_angle1=act_angle1-next_angle1;
+			if(diff_angle1>angle_increment_of_increment_distance_){
+				caso_vuelta_al_reves=true;
+			}
+
+			// 2- vuelta del principio al final
+/*  caso_vuelta_al_reves!!!!   */
+			if(!caso_vuelta_al_reves){
+				// INICIO vuelta hacia delante!
+				//bool flag_return_90_dg=false;
+				//unsigned int second_index=BEST_path_parent_index_vector_[BEST_path_parent_index_vector_.size()-2];
+				//double second_angle=orientation_person_robot_angles_[second_index];
+				/*if((second_angle==angle_companion_)&&(orientation_person_robot_angles_[0]==angle_companion_)){//(orientation_person_robot_angles_[0]>(angle_companion_-5))&&(orientation_person_robot_angles_[0]<(angle_companion_+5))){
+				//	std::cout << "flag_return_90_dg=true"<< std::endl;
+					flag_return_90_dg=true;
+				}*/
+
+				//double sum_angles_increment=0;
+
+				for(unsigned int g=0;g<BEST_path_parent_index_vector_.size();g++){
+					//std::cout << "BEST_path_parent_index_vector_.size()-1-g="<<BEST_path_parent_index_vector_.size()-1-g<<"; BEST_path_parent_index_vector_.size()-2-g="<<BEST_path_parent_index_vector_.size()-2-g<< std::endl;
+
+					unsigned int act_index=BEST_path_parent_index_vector_[BEST_path_parent_index_vector_.size()-1-g];
+					unsigned int next_index=BEST_path_parent_index_vector_[BEST_path_parent_index_vector_.size()-2-g];
+					double act_angle=orientation_person_robot_angles_[act_index];
+					double next_angle=orientation_person_robot_angles_[next_index];
+					double diff_angle=act_angle-next_angle;
+
+					//std::cout << "act_angle["<<act_index<<"]="<<act_angle<<"next_angle["<<next_index<<"]"<<next_angle<<"diff_angle=act_angle-next_angle="<<diff_angle<<" > angle_increment_of_increment_distance_="<< angle_increment_of_increment_distance_<<" and diff_angle>0"<< std::endl;
+
+					//if((act_index!=0)&&(diff_angle>angle_increment_of_increment_distance_)){
+					if(((diff_angle>angle_increment_of_increment_distance_))&&(diff_angle>0)){
+						//std::cout << "ENTRO EN IF !!!"<< std::endl;
+
+					//	std::cout << " (FOR) ANT (!!!CASE!!! actual_angle > actual_parent_angle ) act_index="<<act_index<<"; next_index="<<next_index<< std::endl;
+					//	std::cout << "  act_angle="<<act_angle<<"; next_angle="<<next_angle<<";  diff_angle="<< diff_angle<< std::endl;
+
+					//	std::cout << "(!!!CASE!!! actual_angle > actual_parent_angle ) act_index="<<act_index<<"; next_index="<<next_index<< std::endl;
+							// CASE 2: actual_angle > actual_parent_angle (aquí no se puede hacer...) Hay que dar vuelta hacia delante.
+
+						if(actual_debug_){
+							std::cout << "if (CASE: salgo de obstaculo)"<< std::endl;
+						}
+						//if(next_index!=0){
+							double new_angle=act_angle-angle_increment_of_increment_distance_;
+							//std::cout << "new_angle="<<new_angle<< std::endl;
+
+							if(new_angle<angle_companion_){
+								orientation_person_robot_angles_[next_index]=angle_companion_;
+								//std::cout << "if(new_angle<angle_companion_) angle_companion_="<<angle_companion_<< std::endl;
+							}else{
+								orientation_person_robot_angles_[next_index]=new_angle;
+								//std::cout << "else if(new_angle<angle_companion_)new_angle="<<new_angle<< std::endl;
+							}
+							if(new_angle>180){
+								orientation_person_robot_angles_[next_index]=180;
+							}
+							if(actual_debug_){
+								std::cout << "(modif) orientation_person_robot_angles_[next_index="<<next_index<<"]="<<orientation_person_robot_angles_[next_index]<< std::endl;
+							}
+						//}
+					}else{
+
+						//std::cout << "ENTRO EN ELSE !!!"<< std::endl;
+
+						//if(next_index!=0){
+							/*orientation_person_robot_angles_[next_index]=orientation_person_robot_angles_[act_angle]+angle_increment_of_increment_distance_;
+							if(orientation_person_robot_angles_[next_index]>angle_companion_){
+								orientation_person_robot_angles_[next_index]=angle_companion_;
+							}*/
+						//}
+						//	std::cout << " (FOR) (real) next_angle="<<orientation_person_robot_angles_[next_index]<< std::endl;
+
+					}
+					//else if((diff_angle>angle_increment_of_increment_distance_)&&(flag_return_90_dg)){
+						// CASE 3: actual_angle > actual_parent_angle (aquí no se puede hacer...) Hay que dar vuelta hacia delante.
+						// caso: orientation_person_robot_angles_[0=[BEST_path_parent_index_vector_.size()-1]<angle_companion_
+						// &
+					//	if(actual_debug_){
+					//		std::cout << "(caso0) if (CASE: salgo de obstaculo)"<< std::endl;
+					//	}
+					//	double new_angle=act_angle+angle_increment_of_increment_distance_;
+					//	if(new_angle>angle_companion_){
+					//		orientation_person_robot_angles_[next_index]=angle_companion_;
+					//	}else{
+					//		orientation_person_robot_angles_[next_index]=new_angle;
+					//	}
+					//	if(actual_debug_){
+					//		std::cout << "(modif_caso0) orientation_person_robot_angles_[next_index="<<next_index<<"]="<<orientation_person_robot_angles_[next_index]<< std::endl;
+					//	}
+					//}
+
+
+					//sum_angles_increment=sum_angles_increment+orientation_person_robot_angles_[act_index];
+
+				} // fin for vuelta hacia delante
+
+				//sum_angles_increment=sum_angles_increment/BEST_path_parent_index_vector_.size();
+				//std::cout << " [IMPORTANTE, ver si es diferente en pasillo y obstaculo!!!] sum_angles_increment="<<sum_angles_increment<< std::endl;
+
+			}
+
+
+			bool we_have_path_collisions=false;
+			for(unsigned int g=0;g<vector_of_companion_collisions_.size();g++){
+				if(vector_of_companion_collisions_[g]){
+					we_have_path_collisions=true;
+				}
+			}
+			//std::cout << " !BEST_path_parent_index_vector_.empty()="<<!BEST_path_parent_index_vector_.empty()<< std::endl;
+			//std::cout << " we_have_path_collisions="<<we_have_path_collisions<< std::endl;
+
+			if(!BEST_path_parent_index_vector_.empty()){
+				if((!we_have_path_collisions)&&(BEST_path_parent_index_vector_[BEST_path_parent_index_vector_.size()-2]!=angle_companion_)){
+					for(unsigned int g=0;g<BEST_path_parent_index_vector_.size();g++){
+						unsigned int act_index=BEST_path_parent_index_vector_[BEST_path_parent_index_vector_.size()-1-g];
+						unsigned int next_index=BEST_path_parent_index_vector_[BEST_path_parent_index_vector_.size()-2-g];
+						double act_angle=orientation_person_robot_angles_[act_index];
+						double next_angle=orientation_person_robot_angles_[act_index]-angle_increment_of_increment_distance_;
+						orientation_person_robot_angles_[next_index]=next_angle;
+						if(debug_person_companion_general_){
+						std::cout << " act_index="<< act_index<<"; next_index="<<next_index<<"; act_angle="<<act_angle<<"; next_angle="<<next_angle<< std::endl;
+						}
+						//std::cout << "orientation_person_robot_angles_[next_index]="<< orientation_person_robot_angles_[next_index]<< std::endl;
+					}
+				}
+			}
+
+		//} // fin if(cost_companion!=0)
+
+
+
+		if(debug_comanion_good_){
+			if(!BEST_path_parent_index_vector_.empty()){
+				for(unsigned int f=0;f<BEST_path_parent_index_vector_.size();f++){
+					std::cout << "(BEST path index) f="<<f<<"; index=" <<BEST_path_parent_index_vector_[f]<< std::endl;
+				}
+			}
+
+			std::cout << "FIN only_angle_in_final_tree_calculate_companion_path_angle_and_cost"<< std::endl;
+		}
+}
+
+
+
+/////////////
+
+/*  Arreglar angulos para pasar detras del robot   */
+void Cplan_local_nav_person_companion::go_in_front_robot_only_angle_in_final_tree_calculate_companion_path_angle_and_cost3(unsigned int i){
+
+	// TODO: arreglar para que vaya por detras de la persona en los obstaculos y no por delante!!!
+
+	/*std::cout << "    INI orientation_person_robot_angles_[g]: "<< std::endl;
+
+	for(unsigned int y=0;y<orientation_person_robot_angles_.size();y++){
+		std::cout << "    orientation_person_robot_angles_[y]="<<orientation_person_robot_angles_[y]<< std::endl;
+	}*/
+
+
+
+	/*std::cout << "  INI go_in_front_robot_only_angle_in_final_tree_calculate_companion_path_angle_and_cost3 INI : orientation_person_robot_angles_:"<< std::endl;
+
+				for(unsigned int y=0;y<orientation_person_robot_angles_.size();y++){
+					std::cout << "    orientation_person_robot_angles_[y="<<y<<"]="<<orientation_person_robot_angles_[y]<< std::endl;
+				}*/
+
+
+
+
+
+	// calcula los angulos que hay que moverse en ese path.
+
+	BEST_path_parent_index_vector_.clear(); // de atras a delante. (en el ultimo coges el primer indice.)
+
+	// CASE 1: actual_angle < actual_parent_angle. Dar vuelta hacia atras del arbol (primera vuelta).
+	unsigned int actual_index;
+	unsigned int actual_parent_index;
+
+	if(calc_goal_companion_with_group_path_){
+		actual_index=i; // recorrete todo el path the este final de camino.
+		actual_parent_index=parent_index_vector_[actual_index];
+	}else{
+		actual_index=orientation_person_robot_angles_with_prediction_of_person_companion_.size()-1; // recorrete todo el path the este final de camino.
+		actual_parent_index=orientation_person_robot_angles_with_prediction_of_person_companion_.size()-2;
+	}
+
+	double actual_angle;
+	double actual_parent_angle;
+
+	if(calc_goal_companion_with_group_path_){
+		actual_angle=orientation_person_robot_angles_[actual_index];
+		actual_parent_angle=orientation_person_robot_angles_[actual_parent_index];
+	}else{
+		actual_angle=orientation_person_robot_angles_with_prediction_of_person_companion_[actual_index];
+		actual_parent_angle=orientation_person_robot_angles_with_prediction_of_person_companion_[actual_parent_index];
+	}
+
+	double diff_angle=actual_parent_angle-actual_angle; // saltos posibles 1- 90grad a 180grad o 0 grad.
+
+	if(debug_real_test_companion2_){																		// 2- de 0grad o 180grad a 90grad.
+		std::cout << "actual_angle=" <<actual_angle<<"; actual_parent_angle="<<actual_parent_angle<<"; diff_angle="<<diff_angle<< std::endl;
+	}
+
+	//bool caso_vuelta_al_reves=true;
+
+	unsigned int actual_index2; // recorrete todo el path the este final de camino.
+	unsigned int actual_parent_index2;
+	//if(calc_goal_companion_with_group_path_){
+		actual_index2=i;
+		actual_parent_index2=parent_index_vector_[actual_index2];
+		BEST_path_parent_index_vector_.push_back(actual_index2);
+		//std::cout << " (ini while) actual_index="<<actual_index<<"; BEST_path_parent_index_vector_.back()"<<BEST_path_parent_index_vector_.back()<< std::endl;
+		do{
+			BEST_path_parent_index_vector_.push_back(actual_index2);
+			actual_index2=actual_parent_index2;
+			actual_parent_index2=parent_index_vector_[actual_index2];
+		}while(actual_parent_index2>0);
+		BEST_path_parent_index_vector_.push_back(actual_parent_index2);
+	///} //else nada en este caso.
+
+
+
+
+	// 1- vuelta del final del path al principio!
+	//double sum_angles_increment1=0;
+	//int count_sum=0;
+	do{
+
+		if(calc_goal_companion_with_group_path_){
+			actual_angle=orientation_person_robot_angles_[actual_index];
+			actual_parent_angle=orientation_person_robot_angles_[actual_parent_index];
+		}else{
+			actual_angle=orientation_person_robot_angles_with_prediction_of_person_companion_[actual_index];
+			actual_parent_angle=orientation_person_robot_angles_with_prediction_of_person_companion_[actual_parent_index];
+		}
+
+		//std::cout << " actual_angle"<<actual_angle<<";> actual_parent_angle="<<actual_parent_angle << std::endl;
+		if(actual_angle<actual_parent_angle){
+			//caso_vuelta_al_reves=true;
+			//std::cout << " do while 3 "<< std::endl;
+			diff_angle=actual_parent_angle-actual_angle;
+
+			if(diff_angle>angle_increment_of_increment_distance_){
+				//if(debug_comanion_good_){
+				//	std::cout << " (DO-WILE) Int if ->(path BEFORE CHANGE) orientation_person_robot_angles_[actual_parent_index="<<actual_parent_index<<"]=" <<orientation_person_robot_angles_[actual_parent_index]<< std::endl;
+				//}
+				//std::cout << " do while 5 "<< std::endl;
+				if(debug_angles_){
+					std::cout << "(caso vuelta diff_algle +)= actual_angle - actual_parent_angle = diff_angle=" <<diff_angle<<"; angle_increment_of_increment_distance_="<<angle_increment_of_increment_distance_<< std::endl;
+				}
+				//std::cout << " do while 6 "<< std::endl;
+
+
+				if(calc_goal_companion_with_group_path_){
+					orientation_person_robot_angles_[actual_parent_index]=actual_angle+angle_increment_of_increment_distance_;
+				}else{
+					orientation_person_robot_angles_with_prediction_of_person_companion_[actual_parent_index]=actual_angle+angle_increment_of_increment_distance_;
+				}
+				//std::cout << " do while 7 "<< std::endl;
+
+
+
+				if(actual_debug2_){
+					std::cout << "if (CASE: entro en obstaculo)"<< std::endl;
+					std::cout << "actual_index=" <<actual_index<<"; actual_parent_index="<<actual_parent_index<< std::endl;
+					std::cout << "actual_angle=" <<actual_angle<<"; actual_parent_angle="<<actual_parent_angle<<"; diff_angle="<<diff_angle<< std::endl;
+					std::cout << "(path) orientation_person_robot_angles_[actual_index="<<actual_index<<"]=" <<orientation_person_robot_angles_[actual_index]<< std::endl;
+					std::cout << "(path) orientation_person_robot_angles_[actual_parent_index="<<actual_parent_index<<"]=" <<orientation_person_robot_angles_[actual_parent_index]<< std::endl;
+					std::cout << "(path) min_distance_collision_vector_[actual_index="<<actual_index<<"]=" <<min_distance_collision_vector_[actual_index]<< std::endl;
+					std::cout << "(path) min_distance_collision_vector_[actual_parent_index="<<actual_parent_index<<"]=" <<min_distance_collision_vector_[actual_parent_index]<< std::endl;
+
+					//std::cout << "(path) cost_companion_[actual_parent_index="<<actual_parent_index<<"]=" <<cost_companion_[actual_parent_index]<< std::endl;
+				}
+				//std::cout << " do while 8 "<< std::endl;
+				if(calc_goal_companion_with_group_path_){
+					if(orientation_person_robot_angles_[actual_parent_index]>angle_companion_){
+						orientation_person_robot_angles_[actual_parent_index]=angle_companion_;
+					}
+					if(orientation_person_robot_angles_[actual_parent_index]<0){
+						orientation_person_robot_angles_[actual_parent_index]=0;
+					}
+				}else{
+					if(orientation_person_robot_angles_with_prediction_of_person_companion_[actual_parent_index]>angle_companion_){
+						orientation_person_robot_angles_with_prediction_of_person_companion_[actual_parent_index]=angle_companion_;
+					}
+					if(orientation_person_robot_angles_with_prediction_of_person_companion_[actual_parent_index]<0){
+						orientation_person_robot_angles_with_prediction_of_person_companion_[actual_parent_index]=0;
+					}
+				}
+
+				//std::cout << " do while 9 "<< std::endl;
+				if(debug_angles_){
+					std::cout << "orientation_person_robot_angles_[actual_parent_index]"<< orientation_person_robot_angles_[actual_parent_index]<<" actual_angle-angle_increment_of_increment_distance_="<<actual_angle-angle_increment_of_increment_distance_<< std::endl;
+				}
+
+			}
+			diff_angle=diff_angle-angle_increment_of_increment_distance_;
+		}else{ // CASE: actual_angle > actual_parent_angle (aquí no se puede hacer...) Hay que dar vuelta hacia delante.
+
+		}
+
+		if(debug_comanion_good_){
+			std::cout << "actual_index=" <<actual_index<<"; actual_parent_index="<<actual_parent_index<< std::endl;
+			std::cout << "actual_angle=" <<actual_angle<<"; actual_parent_angle="<<actual_parent_angle<<"; diff_angle="<<diff_angle<< std::endl;
+			std::cout << "(path) orientation_person_robot_angles_[actual_parent_index="<<actual_parent_index<<"]=" <<orientation_person_robot_angles_[actual_parent_index]<< std::endl;
+			std::cout << "(path) cost_companion_[actual_parent_index="<<actual_parent_index<<"]=" <<cost_companion_[actual_parent_index]<< std::endl;
+		}
+		//std::cout << " do while 10 "<< std::endl;
+		if(calc_goal_companion_with_group_path_){
+			actual_index=actual_parent_index;
+			actual_parent_index=parent_index_vector_[actual_index]; //busca el nodo anterior de este path.
+		}else{
+			actual_index=actual_index-1;
+			actual_parent_index=actual_parent_index-1; //busca el nodo anterior de este path.
+		}
+		//std::cout << " do while 11 "<<"; actual_index="<<actual_index<<"; actual_parent_index="<<actual_parent_index<< std::endl;
+		//std::cout << " (final while) actual_index="<<actual_index<<"; BEST_path_parent_index_vector_.back()"<<BEST_path_parent_index_vector_.back()<< std::endl;
+
+	}while(actual_parent_index>0);//&&(diff_angle>angle_increment_of_increment_distance_));
+	// PARA EL INDICE =0, PQ SINO ENTRA EN BUCLE INFINITO!
+
+	if(calc_goal_companion_with_group_path_){
+		actual_angle=orientation_person_robot_angles_[actual_index];
+		actual_parent_angle=orientation_person_robot_angles_[actual_parent_index];
+	}else{
+		actual_angle=orientation_person_robot_angles_with_prediction_of_person_companion_[actual_index];
+		actual_parent_angle=orientation_person_robot_angles_with_prediction_of_person_companion_[actual_parent_index];
+	}
+
+	//std::cout << " do while 12 "<<"; actual_index="<<actual_index<<"; actual_parent_index="<<actual_parent_index<< std::endl;
+
+	/*if(actual_angle<actual_parent_angle){
+		diff_angle=sqrt((actual_parent_angle-actual_angle)*(actual_parent_angle-actual_angle));
+		if(diff_angle>angle_increment_of_increment_distance_){
+
+			if(debug_angles_){
+				std::cout << "(caso vuelta diff_algle +)= actual_angle - actual_parent_angle = diff_angle=" <<diff_angle<<"; angle_increment_of_increment_distance_="<<angle_increment_of_increment_distance_<< std::endl;
+			}
+			//std::cout << " do while 13 "<<"; actual_index="<<actual_index<<"; actual_parent_index="<<actual_parent_index<< std::endl;
+
+			if(calc_goal_companion_with_group_path_){
+				orientation_person_robot_angles_[actual_parent_index]=actual_angle+angle_increment_of_increment_distance_;
+				if(orientation_person_robot_angles_[actual_parent_index]>angle_companion_){
+					orientation_person_robot_angles_[actual_parent_index]=angle_companion_;
+				}
+				if(orientation_person_robot_angles_[actual_parent_index]<0){
+					orientation_person_robot_angles_[actual_parent_index]=0;
+				}
+			}else{
+				orientation_person_robot_angles_with_prediction_of_person_companion_[actual_parent_index]=actual_angle+angle_increment_of_increment_distance_;
+				if(orientation_person_robot_angles_with_prediction_of_person_companion_[actual_parent_index]>angle_companion_){
+					orientation_person_robot_angles_with_prediction_of_person_companion_[actual_parent_index]=angle_companion_;
+				}
+				if(orientation_person_robot_angles_with_prediction_of_person_companion_[actual_parent_index]<0){
+					orientation_person_robot_angles_with_prediction_of_person_companion_[actual_parent_index]=0;
+				}
+			}
+			//std::cout << " do while 14 "<<"; actual_index="<<actual_index<<"; actual_parent_index="<<actual_parent_index<< std::endl;
+
+			if(debug_angles_){
+				std::cout << "orientation_person_robot_angles_[actual_parent_index]"<< orientation_person_robot_angles_[actual_parent_index]<<" actual_angle-angle_increment_of_increment_distance_="<<actual_angle-angle_increment_of_increment_distance_<< std::endl;
+			}
+		}
+	}*/
+	//std::cout << " do while 15 "<<"; actual_index="<<actual_index<<"; actual_parent_index="<<actual_parent_index<< std::endl;
+
+	//std::cout << " [IMPORTANTE, ver si es diferente en pasillo y obstaculo!!!] sum_angles_increment1="<<sum_angles_increment1<< std::endl;
+
+
+	/*std::cout << "  2  INI orientation_person_robot_angles_[g]: "<< std::endl;
+
+		for(unsigned int y=0;y<orientation_person_robot_angles_.size();y++){
+			std::cout << "    orientation_person_robot_angles_[y]="<<orientation_person_robot_angles_[y]<< std::endl;
+		}*/
+
+
+	if(!calc_goal_companion_with_group_path_){
+		// TODO: usar funcion, vueltas_hacia_delante_arreglar_angulos_para_caso_min_companion_angle_con_prediccion.
+		//std::cout << " do while 16 "<< std::endl;
+
+		fix_angles_to_use_person_prediction_for_the_companion_goal(); // TODO: mirar que hace!
+		//std::cout << " do while 17 "<< std::endl;
+
+
+
+
+	}else{
+	///////////////77
+		//std::cout << " (final while) actual_parent_index="<<actual_parent_index<<"; BEST_path_parent_index_vector_.back()"<<BEST_path_parent_index_vector_.back()<< std::endl;
+		if(actual_debug_){
+			std::cout << " BEST_path_parent_index_vector_.size()="<<BEST_path_parent_index_vector_.size()<< std::endl;
+		}
+		if(debug_comanion_good_){
+			std::cout << "actual_index=" <<actual_index<<"; actual_parent_index="<<actual_parent_index<< std::endl;
+			std::cout << "(path) orientation_person_robot_angles_[actual_parent_index="<<0<<"]=" <<orientation_person_robot_angles_[0]<< std::endl;
+			std::cout << "(path) cost_companion_[actual_parent_index="<<0<<"]=" <<cost_companion_[0]<< std::endl;
+		}
+
+		unsigned int act_index1=BEST_path_parent_index_vector_[BEST_path_parent_index_vector_.size()-1];
+		unsigned int next_index1=BEST_path_parent_index_vector_[BEST_path_parent_index_vector_.size()-2];
+		double act_angle1=orientation_person_robot_angles_[act_index1];
+		double next_angle1=orientation_person_robot_angles_[next_index1];
+		double diff_angle1=act_angle1-next_angle1;
+		if(diff_angle1>angle_increment_of_increment_distance_){
+			//caso_vuelta_al_reves=true;
+		}
+
+
+	/*  caso_vuelta_al_reves!!!!   */
+
+	/*	for(unsigned int g=0;g<BEST_path_parent_index_vector_.size();g++){
+
+			if(debug_angles_){
+				std::cout << " 2 (vuelta al reves!) BEST_path_parent_index_vector_.size()-1-g="<<BEST_path_parent_index_vector_.size()-1-g<<"; BEST_path_parent_index_vector_.size()-2-g="<<BEST_path_parent_index_vector_.size()-2-g<< std::endl;
+			}
+
+			unsigned int act_index=BEST_path_parent_index_vector_[BEST_path_parent_index_vector_.size()-1-g];
+			unsigned int next_index=BEST_path_parent_index_vector_[BEST_path_parent_index_vector_.size()-2-g];
+
+			double act_angle=orientation_person_robot_angles_[act_index];
+			double next_angle=orientation_person_robot_angles_[next_index];
+
+			if((act_index==0)){
+				act_angle=initial_angle_;
+			}
+			if(next_angle==0){
+				next_angle=initial_angle_;
+			}
+
+			if(debug_angles_){
+				std::cout << " 2 (vuelta hacia delante de INI_angle_act_angle al qur necesitas para pasar.!) act_index="<<act_index<<"; next_index="<<next_index<< std::endl;
+			}
+
+			if((BEST_path_parent_index_vector_.size()-1-g)==0){
+				act_angle=initial_angle_;
+			}
+
+			double diff_angle=sqrt((act_angle-next_angle)*(act_angle-next_angle));
+
+			//if(debug_angles_){
+				std::cout << "act_angle["<<act_index<<"]="<<act_angle<<"next_angle["<<next_index<<"]"<<next_angle<<"diff_angle=act_angle-next_angle="<<diff_angle<<" > angle_increment_of_increment_distance_="<< angle_increment_of_increment_distance_<<" and diff_angle>0"<< std::endl;
+			//}
+
+			if(((diff_angle>(2*angle_increment_of_increment_distance_)))&&(diff_angle>0)){
+				std::cout << "ENTRO EN IF !!!"<< std::endl;
+
+				//	std::cout << " (FOR) ANT (!!!CASE!!! actual_angle > actual_parent_angle ) act_index="<<act_index<<"; next_index="<<next_index<< std::endl;
+				//	std::cout << "  act_angle="<<act_angle<<"; next_angle="<<next_angle<<";  diff_angle="<< diff_angle<< std::endl;
+
+				//	std::cout << "(!!!CASE!!! actual_angle > actual_parent_angle ) act_index="<<act_index<<"; next_index="<<next_index<< std::endl;
+				// CASE 2: actual_angle > actual_parent_angle (aquí no se puede hacer...) Hay que dar vuelta hacia delante.
+
+				if(actual_debug_){
+					std::cout << "if (CASE: salgo de obstaculo)"<< std::endl;
+				}
+
+				double new_angle=act_angle-(2*angle_increment_of_increment_distance_);
+
+				//if(debug_gazebo_journal_){
+				//	std::cout << "new_angle ["<<next_index<<"]="<<new_angle<< std::endl;
+				//}
+
+				if(new_angle>angle_companion_){
+					orientation_person_robot_angles_[next_index]=angle_companion_;
+					//std::cout << "if(new_angle<angle_companion_) angle_companion_="<<angle_companion_<< std::endl;
+				}else{
+					orientation_person_robot_angles_[next_index]=new_angle;
+					//std::cout << "else if(new_angle<angle_companion_)new_angle="<<new_angle<< std::endl;
+				}
+				if(new_angle<0){
+					orientation_person_robot_angles_[next_index]=0;
+				}
+				if(actual_debug_){
+					std::cout << "(modif) orientation_person_robot_angles_[next_index="<<next_index<<"]="<<orientation_person_robot_angles_[next_index]<< std::endl;
+				}
+
+			}else{
+
+				//std::cout << "ENTRO EN ELSE !!!"<< std::endl;
+
+				//	std::cout << " (FOR) (real) next_angle="<<orientation_person_robot_angles_[next_index]<< std::endl;
+
+			}
+
+			//sum_angles_increment=sum_angles_increment+orientation_person_robot_angles_[act_index];
+
+		} // fin for vuelta hacia delante
+*/
+		//sum_angles_increment=sum_angles_increment/BEST_path_parent_index_vector_.size();
+		//std::cout << " [IMPORTANTE, ver si es diferente en pasillo y obstaculo!!!] sum_angles_increment="<<sum_angles_increment<< std::endl;
+		////////////////////////////
+
+				//std::cout << " tercera pasada, ahora mismo no se para que!"<< std::endl;
+				// es para mantenerte a 90 grados si decrece tu valor en orientacion!!! Te adelantas demasiado
+				if(!BEST_path_parent_index_vector_.empty()){
+					bool we_have_path_collisions=false;
+					for(unsigned int g=0;g<BEST_path_parent_index_vector_.size();g++){
+
+						if((vector_of_companion_collisions_[BEST_path_parent_index_vector_[BEST_path_parent_index_vector_.size()-1-g]])&&(orientation_person_robot_angles_[BEST_path_parent_index_vector_[BEST_path_parent_index_vector_.size()-1-g]]<(angle_companion_-2*angle_increment_of_increment_distance_))){
+							we_have_path_collisions=true;
+						}else{
+
+						}
+					}
+
+					double ini_act_angle_out=initial_angle_;
+
+					if((!we_have_path_collisions)&&(ini_act_angle_out<angle_companion_)){
+
+						for(unsigned int g=1;g<BEST_path_parent_index_vector_.size();g++){
+
+							unsigned int act_index;//=best_plan_vertex_index_[best_plan_vertex_index_.size()-g];
+
+							unsigned int next_index=BEST_path_parent_index_vector_[0+g];
+							double act_angle;//=orientation_person_robot_angles_[act_index];
+							if(g==1){
+								act_angle=ini_act_angle_out;
+								act_index=0;
+							}else{
+								act_angle=orientation_person_robot_angles_[act_index];
+							}
+
+							double next_angle=act_angle+5*angle_increment_of_increment_distance_;
+
+							if(next_angle>angle_companion_){
+								//next_angle=orientation_person_robot_angles_[act_index]-angle_increment_of_increment_distance_;
+								orientation_person_robot_angles_[next_index]=angle_companion_;
+							}else{
+								orientation_person_robot_angles_[next_index]=next_angle;
+							}
+
+							act_index=next_index;
+						}
+					}
+				}
+
+	}
+
+
+}
+
+
+
+
+
+
+
+void Cplan_local_nav_person_companion::see_companion_path_angle_and_cost_of_min_cost_paths(double parent_index){
+
+
+	std::cout << "INI see_companion_path_angle_and_cost_of_min_cost_paths "<< std::endl;
+	std::cout << "orientation_person_robot_angles_.size()= "<<orientation_person_robot_angles_.size()<< std::endl;
+	std::cout << "cost_companion_.size()= "<<cost_companion_.size()<< std::endl;
+	std::cout << "parent_index_vector_.size()= "<<parent_index_vector_.size()<< std::endl;
+	double sum_angles_increment=0;
+	double sum_inc_entre_angles=0;
+
+	double actual_parent_index=parent_index;
+	do{
+		double sum1=orientation_person_robot_angles_[actual_parent_index];
+
+		std::cout << "(path) orientation_person_robot_angles_[actual_parent_index="<<actual_parent_index<<"]=" <<orientation_person_robot_angles_[actual_parent_index]<< std::endl;
+		std::cout << "(path) cost_companion_[actual_parent_index="<<actual_parent_index<<"]=" <<cost_companion_[actual_parent_index]<< std::endl;
+		std::cout << "(path) min_distance_collision_vector_[actual_parent_index="<<actual_parent_index<<"]=" <<min_distance_collision_vector_[actual_parent_index]<< std::endl;
+
+		actual_parent_index=parent_index_vector_[actual_parent_index];
+		double sum2=orientation_person_robot_angles_[actual_parent_index];
+
+		sum_angles_increment=sum_angles_increment+orientation_person_robot_angles_[actual_parent_index];
+		sum_inc_entre_angles=sum_inc_entre_angles+(sum1-sum2);
+		 if(debug_nadal_){
+			 std::cout << "antes while actual_parent_index="<<actual_parent_index<< std::endl;
+		 }
+
+	}while(actual_parent_index>0);
+	sum_inc_entre_angles=sum_inc_entre_angles/orientation_person_robot_angles_.size();
+	sum_angles_increment=sum_angles_increment/orientation_person_robot_angles_.size();
+
+	std::cout << " [IMPORTANTE, ver si es diferente en pasillo y obstaculo!!!] sum_angles_increment="<<sum_angles_increment<< std::endl;
+	std::cout << " [IMPORTANTE, ver si es diferente en pasillo y obstaculo!!!] sum_inc_entre_angles="<<sum_inc_entre_angles<< std::endl;
+
+	std::cout << "despues while"<< std::endl;
+	std::cout << "(path) actual_parent_index="<<actual_parent_index<< std::endl;
+
+	std::cout << "(path) orientation_person_robot_angles_[actual_parent_index="<<0<<"]=" <<orientation_person_robot_angles_[0]<< std::endl;
+	std::cout << "(path) cost_companion_[actual_parent_index="<<0<<"]=" <<cost_companion_[0]<< std::endl;
+
+}
+
+
+
+
+
+
+
+
+
+double Cplan_local_nav_person_companion::calc_robot_person_companion_distance(){
+//
+	double robot_person_distance;
+	//Spose act_robot_pose=robot_->get_current_pose();
+	//std::cout << "Initia, out, robot pose="<< std::endl;
+	//act_robot_pose.print();
+	//std::cout << "Initia, out, robot print="<< std::endl;
+	//robot_initial_pose_.print();
+
+	//const std::list<Cperson_abstract *>* person_list = get_scene( );
+	//for( std::list<Cperson_abstract*>::const_iterator iit = person_list->begin(); iit!=person_list->end(); iit++ )
+	//{
+	 	//if((*iit)->get_id()==id_person_companion_){
+	    	companion_person_position_=actual_person_Companion_SpointV_;
+	    //}
+	//}
+	robot_person_distance=robot_initial_pose_.distance( (Spoint) companion_person_position_);
+	//std::cout << "(calc_robot_person_companion_distance()) robot_person_distance="<<robot_person_distance<< std::endl;
+	//std::cout << "robot_initial_pose_:"<< std::endl;
+	//robot_initial_pose_.print();
+	//std::cout << "companion_person_position_:"<< std::endl;
+	//companion_person_position_.print();
+
+	return robot_person_distance;//=act_robot_pose.distance(companion_person_position_);
+}
+
+
+
+
+
+double Cplan_local_nav_person_companion::calc_robot_person_companion_distance_companion_person_akp(){
+//
+	double robot_person_distance;
+	//Spose act_robot_pose=robot_->get_current_pose();
+	//std::cout << "Initia, out, robot pose="<< std::endl;
+	//act_robot_pose.print();
+	//std::cout << "Initia, out, robot print="<< std::endl;
+	//robot_initial_pose_.print();
+
+	companion_person_position_=person_companion_->get_current_pointV();
+
+	robot_person_distance=robot_initial_pose_.distance( (Spoint) companion_person_position_);
+	//std::cout << "(calc_robot_person_companion_distance_companion_person_akp()) robot_person_distance="<<robot_person_distance<< std::endl;
+	//std::cout << "companion_person_position_:"<< std::endl;
+	//companion_person_position_.print();
+
+	return robot_person_distance;//=act_robot_pose.distance(companion_person_position_);
+}
+
+
+// velocidad robot!!!
+
+void Cplan_local_nav_person_companion::vel_robot_normal(double d_min){
+	   // calculate the desired robot velocity depending on the nearbiest obstacle/person and goal
+		double distance_to_goal = robot_->get_current_pointV().distance( goal_);
+		// when the goal, starts to stop, higher priority than ppl
+	    if( distance_to_goal < distance_to_stop_ ){
+	    	 //std::cout <<" distance_to_goal < distance_to_stop_ (near goal) "<< std::endl;
+			robot_->set_v_max( max_v_by_system_ * distance_to_goal / distance_to_stop_  );//distance_to_stop is never 0
+	    // if not near the goal, normal velocity regulation according to nearby ppl
+	    }else if ( d_min < 1.0 ){
+	    	// std::cout <<" d_min < 1.0 "<< std::endl;
+	    	robot_->set_v_max( max_v_by_system_ * 0.75 );
+	    }else if ( d_min < 2.0){
+	    	// std::cout <<" d_min < 2.0 "<< std::endl;
+	    	robot_->set_v_max( max_v_by_system_ * 0.85);
+	    /*else if ( d_min < 5.0 )
+	    	robot_->set_v_max( max_v_by_system_ * 0.95 );*/
+	    }else{
+	    	 //std::cout <<" (reactive and repulsive) CASE else"<< std::endl;
+	    	robot_->set_v_max( max_v_by_system_  );
+	    }
+
+	    //std::cout <<"UUUUUU  max_v_by_system_="<< max_v_by_system_<< std::endl;
+
+}
+
+void Cplan_local_nav_person_companion::vel_robot_companion(double d_min , double robot_person_distance, Spoint before_next_goal_of_robot, Cperson_abstract::companion_reactive reactive){
+	/* (companion) Fin calculo angulo correspondiente a distancia de choque*/
+	if(debug_real_test_companion4_){
+		std::cout <<"IN vel_robot_companion!!! UUUUUU  d_min="<< d_min<< std::endl;
+	}
+
+	Spoint Spoint_pose_command2=(Spoint) robot_->get_current_pointV(); // calculas la distancia al obstaculo desde el centro de ambos (persona y robot).
+
+	//double min_dist_colli_act_global=check_collision_companion_goal_GLOBAL(Spoint_pose_command2,0); //TODO: OJO! parece que no se usa! si es verdad. eliminar!!!
+
+	// calculate the desired robot velocity depending on the nearbiest obstacle/person and goal
+
+	Spose robot1=robot_initial_pose_;
+
+	if(debug_real_test_companion_){
+		std::cout << " robot1=robot_->get_current_pose(); robot1.x="<<robot1.x <<"; robot1.y="<<robot1.y<< std::endl;
+		std::cout << " robot_initial_pose_.x"<<robot_initial_pose_.x<<"; robot_initial_pose_.y="<<robot_initial_pose_.y << std::endl;
+	}
+
+	Cperson_abstract* person_obj1;
+	find_person(id_person_companion_ , &person_obj1);
+	Sdestination person_companion_goal=person_obj1->get_best_dest();
+	SpointV_cov person1 = person_obj1->get_current_pointV();
+
+	//if(debug_output_screen_mesages_){
+	//	std::cout << " person1 = person_obj1->getvel_robot_companion_current_pointV(); person1.x="<<person1.x <<"; person1.y="<<person1.y<< std::endl;
+	//	std::cout << " person1 = person_obj1->getvel_robot_companion_current_pointV(); person1.vx="<<person1.vx <<"; person1.vy="<<person1.vy<< std::endl;
+	//	std::cout << " person1 = person_obj1->getvel_robot_companion_current_pointV(); person1.vdes="<<person_obj1->get_desired_velocity()<< std::endl;
+
+	//}
+
+	//Cperson_abstract* person_obj2;
+	//bool find_second_pers=find_person(id_SECOND_person_companion_ , &person_obj2);
+	//Sdestination person_companion_goal2;
+	//SpointV_cov person2;
+	/*if(find_second_pers){
+		person_companion_goal2=person_obj2->get_best_dest();
+		person2 = person_obj2->get_current_pointV();
+
+			//if(debug_output_screen_mesages_){
+			//	std::cout << " person2 = person_obj1->getvel_robot_companion_current_pointV(); person2.x="<<person2.x <<"; person1.y="<<person2.y<< std::endl;
+			//	std::cout << " person2 = person_obj1->getvel_robot_companion_current_pointV(); person2.vx="<<person2.vx <<"; person1.vy="<<person2.vy<< std::endl;
+			//	std::cout << " person2 = person_obj1->getvel_robot_companion_current_pointV(); person2.vdes="<<person_obj2->get_desired_velocity()<< std::endl;
+
+			//}
+	}*/
+
+
+	if(debug_real_test_companion_){
+
+
+		std::cout << " companion_person_position_.x"<<companion_person_position_.x<<"; companion_person_position_.y="<<companion_person_position_.y << std::endl;
+	}
+
+	double angle1;
+
+////////// Inicio calculo correcto initial angle!!! /////////////////////////////////////
+	if(debug_real_test_companion_){
+		std::cout << " !!! Inicio calculo correcto initial angle !!! "<< std::endl;
+	}
+	//Spose robot=robot_->get_current_pose();
+	// o
+	Spose robot=robot_initial_pose_;
+
+	double distance_between_ini_pose_and_final_next_goal;
+	double dist2;
+	double adequate_velocity;
+	if(reactive==Cperson_abstract::Akp_planning){
+		//std::cout << " ANTES CALCULO ARREGLAR VEL "<< std::endl;
+		//std::cout <<" TEST obtain better velocity of robot!!! time_stamp_before="<<before_next_goal_of_robot.time_stamp <<"; time_stamp_robot_Act="<<robot_initial_pose_.time_stamp<< std::endl;
+
+		distance_between_ini_pose_and_final_next_goal=sqrt((robot_initial_pose_.x-before_next_goal_of_robot.x)*(robot_initial_pose_.x-before_next_goal_of_robot.x)+(robot_initial_pose_.y-before_next_goal_of_robot.y)*(robot_initial_pose_.y-before_next_goal_of_robot.y));
+		dist2=distance_between_ini_pose_and_final_next_goal;
+		adequate_velocity=dist2/dt_;
+
+		if(adequate_velocity>max_v_by_system_){ // TODO: substituir el 0.9 por max_velocity del robot!
+			adequate_velocity=max_v_by_system_;
+		}
+	}
+
+
+
+
+	// substituir la person velocity por la adequate person velocity.
+	/*std::cout <<" TEST obtain better velocity of robot!!! time_stamp_before="<<before_next_goal_of_robot.time_stamp <<"; time_stamp_robot_Act="<<robot_initial_pose_.time_stamp<< std::endl;
+
+	std::cout <<" TEST obtain better velocity of robot!!! ; initial_person_companion_point_.vx="<<initial_person_companion_point_.vx<<"; initial_person_companion_point_.vy="<<initial_person_companion_point_.vy<< std::endl;
+	std::cout <<"  robot_initial_pose_.x="<<robot_initial_pose_.x<<"; robot_initial_pose_.y="<<robot_initial_pose_.y<<"; before_next_goal_of_robot.x="<<before_next_goal_of_robot.x<<"; before_next_goal_of_robot.y="<<before_next_goal_of_robot.y<< std::endl;
+	std::cout <<" TEST obtain better velocity of robot!!! distance_between_ini_pose_and_final_next_goal="<<distance_between_ini_pose_and_final_next_goal<< std::endl;
+	 */
+//	std::cout <<" TEST obtain better velocity of robot!!! adequate_velocity="<<adequate_velocity<<"; dt_="<<dt_<< std::endl;
+
+
+	//Cperson_abstract* person_obj;
+	//find_person(id_person_companion_ , &person_obj);
+
+	//SpointV_cov person = person_obj->get_current_pointV();
+	//std::cout << " peta en velocidad (1) "<< std::endl;
+   	// ini calcular nuevo angulo theta con orientación dirección movimiento persona.
+    		Cperson_abstract* person_obj;
+    		bool finded_person=find_person(id_person_companion_ , &person_obj);
+    		//std::cout << " peta en velocidad (2) "<< std::endl;
+    	    //std::cout <<" bool finded_person="<<finded_person<<" id_person_companion_"<<id_person_companion_ << std::endl;
+
+    	    if(debug_real_test_companion2_){
+    	    	std::cout <<" bool finded_person="<<finded_person<<" id_person_companion_"<<id_person_companion_ << std::endl;
+    	    }
+
+    	    if(debug_antes_subgoals_entre_AKP_goals_){
+    	    	std::cout << " (Case: far of the person position) finded_person="<<finded_person<< std::endl;
+    	    	std::cout << " person_obj->print(), person_pose_companion:"<< std::endl;
+    	    	person_obj->print();
+    	    	std::cout << " despues print person"<< std::endl;
+    	    }
+
+    	    SpointV_cov person = person_obj->get_current_pointV();
+    		//std::cout << " peta en velocidad (3) "<< std::endl;
+    	    if(debug_correct_angle_person_vel_robot_companion_){
+    	    std::cout <<" perso_print="<< std::endl;
+    	    person.print();
+    	    }
+    	    //const std::vector<SpointV_cov> person_tracj=person_obj->get_planning_trajectory();
+    	    if(debug_correct_angle_person_vel_robot_companion_){ std::cout <<" person_tracj.size()="<<person_obj->get_past_trajectory()->size()<< std::endl;}
+
+    	  //  std::cout <<" antes calculo theta 7"<< std::endl;
+
+    	    double theta=calc_person_companion_orientation();
+    		//std::cout << " peta en velocidad (4) "<< std::endl;
+    	    //double theta_pers=theta;
+    	    /*double x;
+    	    double y;
+    	    if(person_obj->get_past_trajectory()->size()>num_steps_orientation_){
+    	    	if(debug_correct_angle_person_vel_robot_companion_){std::cout <<" perso_print_back_traj(size-num_steps_orientation_)="<< std::endl;
+    	    	person_obj->get_past_trajectory()->at(person_obj->get_past_trajectory()->size()-num_steps_orientation_).print();}
+    	    	x=person_obj->get_past_trajectory()->at(person_obj->get_past_trajectory()->size()-num_steps_orientation_).x;
+    	    	y=person_obj->get_past_trajectory()->at(person_obj->get_past_trajectory()->size()-num_steps_orientation_).y;
+    	    }else{
+    	    	if(debug_correct_angle_person_vel_robot_companion_){std::cout <<" perso_print_back_traj(0)="<< std::endl;
+    	    	person_obj->get_past_trajectory()->at(0).print();}
+    	    	x=person_obj->get_past_trajectory()->at(0).x;
+    	    	y=person_obj->get_past_trajectory()->at(0).y;
+    	    }
+    	    double pers_dx=person.x-x;
+    	    double pers_dy=person.y-y;
+			x=person_obj->get_best_dest().x;
+			y=person_obj->get_best_dest().y;
+			pers_dx=person.x-x;//person_obj->get_current_pointV().vx;
+			pers_dy=person.y-y;//person_obj->get_current_pointV().vy;
+    	    if(debug_correct_angle_person_vel_robot_companion_){std::cout <<" dx="<<pers_dx<<"; dy="<<pers_dy<< std::endl;}
+    	    double theta_pers=atan2(pers_dy , pers_dx);
+    	    theta=atan2(pers_dy , pers_dx);
+
+    	    */
+
+
+    	    if(debug_correct_angle_person_vel_robot_companion_){ std::cout <<" (person) theta="<<theta*180/3.14<< std::endl;
+    	    std::cout << " extern_robot_goal_.x-person.x="<<extern_robot_goal_.x-person.x<<"; extern_robot_goal_.y-person.y"<<extern_robot_goal_.y-person.y << std::endl;}
+    	    // fin calcular nuevo angulo theta con orientación dirección movimiento persona.
+   // std::cout << " peta en velocidad (6) "<< std::endl;
+    if(companion_same_person_goal_){
+		theta = atan2(person_obj->get_best_dest().y-person.y ,person_obj->get_best_dest().x-person.x); // angulo entre la destinacion a la que va la persona y la posicion de la persona.
+		//std::cout << " (companion_same_person_goal_) person_obj->get_best_dest().x ="<<person_obj->get_best_dest().x <<"; person_obj->get_best_dest().y="<<person_obj->get_best_dest().y<< std::endl;
+		//extern_robot_goal_=person_obj->get_best_dest();
+	}else{
+		theta = atan2(extern_robot_goal_.y-person.y , extern_robot_goal_.x-person.x);
+	}
+	//std::cout << " peta en velocidad (7) "<< std::endl;
+	person_orient_=theta;
+	if(debug_real_test_companion_){
+		std::cout << " (Global PERSON ORIENTATION) theta ="<<theta << std::endl;
+	}
+	double angle=atan2(person.y-robot.y , person.x-robot.x);
+	//std::cout << " peta en velocidad (8) "<< std::endl;
+	/*if(angle<0){
+	 * if((angle<0)&&(ini_angle_act>-180)){
+	 * ini_angle_act=sqrt(ini_angle_act*ini_angle_act);
+		angle=2*3.14+angle; //ya que angle es negativo!
+	}*/
+
+	////////
+	angle=angle*(180/3.14);
+
+	if(angle<0){
+	angle=360+angle;
+	if(debug_cout_robot_){
+				std::cout << " [change] diff_angle < 0 (1) angle ="<<angle << std::endl;
+			}
+	}
+	//std::cout << " peta en velocidad (9) "<< std::endl;
+// valido SOLO para el caso trasero SIEMPRE!!!
+	if((angle>=90)&&(angle<=180)){
+		angle=angle;
+			if(debug_cout_robot_){
+				std::cout << " [change] diff_angle [0<->180] (1) angle ="<<angle << std::endl;
+			}
+			if(debug_file_robot_){
+				std::ofstream fileMatlab2;
+				fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+				fileMatlab2 << " [change] diff_angle [0<->180] (1) angle ="<<angle <<"\n";
+				fileMatlab2.close();
+			}
+		}else if((angle<90)&&(angle>=0)){
+			angle=180-angle;
+
+			if(debug_cout_robot_){
+				std::cout << " [change] diff_angle [0<-> -180] (1) angle ="<<angle << std::endl;
+			}
+			if(debug_file_robot_){
+				std::ofstream fileMatlab2;
+				fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+				fileMatlab2 << " [change] diff_angle [0<-> -180] (1) angle ="<<angle <<"\n";
+				fileMatlab2.close();
+			}
+		}else if((angle>180)&&(angle<=270)){
+			angle=360-angle;
+
+			if(debug_cout_robot_){
+				std::cout << " [change] diff_angle [180<-> 360] (1) angle ="<<angle << std::endl;
+			}
+			if(debug_file_robot_){
+				std::ofstream fileMatlab2;
+				fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+				fileMatlab2 << " diff_angle [180<-> 360] (1) angle ="<<angle <<"\n";
+				fileMatlab2.close();
+			}
+		}
+		else if((angle <( 360))&&(angle>=(270))){
+			angle=sqrt(angle*angle);
+			angle=angle-180;
+
+			if(debug_cout_robot_){
+				std::cout << " [change] diff_angle [-180<-> -360] (1) angle ="<<angle << std::endl;
+			}
+			if(debug_file_robot_){
+				std::ofstream fileMatlab2;
+				fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+				fileMatlab2 << " diff_angle [-180<-> -360] (1) angle ="<<angle <<"\n";
+				fileMatlab2.close();
+			}
+		}else if(angle>360){
+			 unsigned int n_360_g=angle/360;
+			 angle=angle-n_360_g*360;
+		     std::cout << " OJO!!! caso mayor 360 ; angle=angle-n_360_g*360="<<angle<<"; n_360_g="<<n_360_g<< std::endl;
+		}else{
+			std::cout << " OJO!!! caso NO contemplado angle="<<angle<< std::endl;
+		}
+	angle=angle*(3.14/180);
+	////////
+	//std::cout << " peta en velocidad (10) "<< std::endl;
+	double ini_angle_act;
+	double ant_ini_angle;
+	ant_ini_angle=(robot_->get_current_pointV().angle_heading_point((Spoint) companion_person_position_))*(180/3.14);
+	//double dif_ang=angle-theta;
+
+	//if( diffangle(theta, angle) < 0 ){
+	//if( dif_ang > 0 ){
+	if( diffangle( angle, theta) < 0 ){
+		ini_angle_act=angle*(180/3.14);//(theta + angle)*(180/3.14);//(angle)*(180/3.14);
+		//ini_angle_act=(angle-theta)*(180/3.14);
+		if(debug_cout_robot_){
+			std::cout << " !!!!!!!!!! (IF) ini_angle_act ="<<ini_angle_act <<"; theta*(180/3.14)="<<theta*(180/3.14)<<"angle*(180/3.14)="<<angle*(180/3.14)<<"ant_ini_angle="<<ant_ini_angle<< std::endl;
+		}
+		if(debug_file_robot_){
+			std::ofstream fileMatlab2;
+			fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+			fileMatlab2 << " !!!!!!!!!! (IF) ini_angle_act ="<<ini_angle_act <<"; theta*(180/3.14)="<<theta*(180/3.14)<<"angle*(180/3.14)="<<angle*(180/3.14)<<"ant_ini_angle="<<ant_ini_angle<< "\n";
+			fileMatlab2.close();
+
+		}
+
+	}else{
+
+		ini_angle_act=angle*(180/3.14);//(theta - angle)*(180/3.14);//(angle)*(180/3.14);
+		//ini_angle_act=(theta + angle)*(180/3.14);
+		ant_ini_angle=(robot_->get_current_pointV().angle_heading_point((Spoint) companion_person_position_))*(180/3.14);
+		if(debug_cout_robot_){
+			std::cout << " !!!!!!!!!! (ELSE) ini_angle_act ="<<ini_angle_act <<"; theta*(180/3.14)="<<theta*(180/3.14)<<"angle*(180/3.14)="<<angle*(180/3.14)<<"ant_ini_angle="<<ant_ini_angle<< std::endl;
+		}
+		if(debug_file_robot_){
+			std::ofstream fileMatlab2;
+			fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+			fileMatlab2 << " !!!!!!!!!! (ELSE) ini_angle_act ="<<ini_angle_act <<"; theta*(180/3.14)="<<theta*(180/3.14)<<"angle*(180/3.14)="<<angle*(180/3.14)<<"ant_ini_angle="<<ant_ini_angle<<"\n";
+			 fileMatlab2.close();
+		}
+	}
+	//std::cout << " peta en velocidad (11) "<< std::endl;
+	//std::cout << " CALC INI ANGLE!!!!! [ini] (1) ini_angle_act ="<<ini_angle_act << std::endl;
+
+	if((ini_angle_act>=0)&&(ini_angle_act<=180)){
+		ini_angle_act=ini_angle_act;
+		if(debug_cout_robot_){
+			std::cout << " [change] diff_angle [0<->180] (1) ini_angle_act ="<<ini_angle_act << std::endl;
+		}
+		if(debug_file_robot_){
+			std::ofstream fileMatlab2;
+			fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+			fileMatlab2 << " [change] diff_angle [0<->180] (1) ini_angle_act ="<<ini_angle_act <<"\n";
+			fileMatlab2.close();
+		}
+	}else if((ini_angle_act<0)&&(ini_angle_act>=(-180))){
+		//ini_angle_act=180+ini_angle_act;
+		ini_angle_act=sqrt(ini_angle_act*ini_angle_act);
+		if(debug_cout_robot_){
+			std::cout << " [change] diff_angle [0<-> -180] (1) ini_angle_act ="<<ini_angle_act << std::endl;
+		}
+		if(debug_file_robot_){
+			std::ofstream fileMatlab2;
+			fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+			fileMatlab2 << " [change] diff_angle [0<-> -180] (1) ini_angle_act ="<<ini_angle_act <<"\n";
+			fileMatlab2.close();
+		}
+	}else if((ini_angle_act>180)&&(ini_angle_act<=360)){
+		ini_angle_act=360-ini_angle_act;
+
+		if(debug_cout_robot_){
+			std::cout << " [change] diff_angle [180<-> 360] (1) ini_angle_act ="<<ini_angle_act << std::endl;
+		}
+		if(debug_file_robot_){
+			std::ofstream fileMatlab2;
+			fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+			fileMatlab2 << " diff_angle [180<-> 360] (1) ini_angle_act ="<<ini_angle_act <<"\n";
+			fileMatlab2.close();
+		}
+	}
+	else if((ini_angle_act <( -180))&&(ini_angle_act>=(-360))){
+		ini_angle_act=sqrt(ini_angle_act*ini_angle_act);
+		ini_angle_act=360-ini_angle_act;
+
+		if(debug_cout_robot_){
+			std::cout << " [change] diff_angle [-180<-> -360] (1) ini_angle_act ="<<ini_angle_act << std::endl;
+		}
+		if(debug_file_robot_){
+			std::ofstream fileMatlab2;
+			fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+			fileMatlab2 << " diff_angle [-180<-> -360] (1) ini_angle_act ="<<ini_angle_act <<"\n";
+			fileMatlab2.close();
+		}
+	}else if(ini_angle_act>360){
+		 unsigned int n_360_g=ini_angle_act/360;
+		 ini_angle_act=ini_angle_act-n_360_g*360;
+	     std::cout << " OJO!!! caso mayor 360 ; ini_angle_act=ini_angle_act-n_360_g*360="<<ini_angle_act<<"; n_360_g="<<n_360_g<< std::endl;
+	}else{
+		std::cout << " OJO!!! caso NO contemplado ini_angle_act="<<ini_angle_act<< std::endl;
+	}
+
+	initial_angle_=ini_angle_act;
+	if(debug_real_test_companion_){
+		std::cout << " initial_angle_ ="<<initial_angle_ << std::endl;
+		std::cout << " !!! FIN calculo correcto initial angle !!! "<< std::endl;
+	}
+	angle1=initial_angle_;
+	//std::cout << " peta en velocidad (12) "<< std::endl;
+////////// Fin calculo correcto initial angle!!! //////////////////////////////////////
+
+	//std::cout << " [IMPORTANTE] !!!! angle_companion_temp_movil_="<<angle_companion_temp_movil_<< std::endl;
+	//std::cout << " [IMPORTANTE] !!!! angle_companion_temp_movil_complementarion_detras_="<<angle_companion_temp_movil_complementarion_detras_<< std::endl;
+	if(debug_nadal2_){
+		std::cout << " (after) !!!! angle1="<<angle1<<"; orientation_person_robot_angles_[0]="<<orientation_person_robot_angles_[0]<< std::endl;
+	}
+
+
+
+	double vel_per=sqrt((person1.vy)*(person1.vy) + (person1.vx)*(person1.vx));
+	//std::cout << " peta en velocidad (13) "<< std::endl;
+	////vel_per=adequate_velocity;
+	real_vel_per=sqrt((person1.vy)*(person1.vy) + (person1.vx)*(person1.vx));
+
+
+	if((reactive==Cperson_abstract::Akp_planning)){// &&(!Zanlungo_model_)
+
+		//if(debug_output_screen_mesages_){
+		//	std::cout << "IN IMPORTANT IF!!! antes change  vel_per= ; beta_companion_="<<beta_companion_<<"; alpha_companion_="<<alpha_companion_<< std::endl;
+		//}
+
+		if(distance_between_ini_pose_and_final_next_goal>3){ // distancia mayor que 3 metros
+			adequate_velocity=max_v_by_system_;
+		}else if(distance_between_ini_pose_and_final_next_goal<treshold_distance_between_steps_){ // TODO: sacar este 0.1 a fuera!!!
+			double vpers_v=vel_per;
+			adequate_velocity=vpers_v; // v_person
+		}else{
+			double vpers_v=vel_per;
+			double x=distance_between_ini_pose_and_final_next_goal;
+			adequate_velocity=((max_v_by_system_-vpers_v)/(3-treshold_distance_between_steps_))*(x-treshold_distance_between_steps_)+vpers_v;
+		}
+
+		vel_per=adequate_velocity;
+
+		if(vel_per>max_v_by_system_){
+			vel_per=max_v_by_system_;
+		}
+
+		//if(debug_output_screen_mesages_){
+		//	std::cout << "distance_between_ini_pose_and_final_next_goal="<<distance_between_ini_pose_and_final_next_goal<<"; adequate_velocity="<<adequate_velocity<< std::endl;
+
+		//}
+
+		//std::cout << " initial_robot_spoint_.vx="<<initial_robot_spoint_.vx<<"; initial_robot_spoint_.vy="<<initial_robot_spoint_.vy<< std::endl;
+		//std::cout << " initial_person_companion_point_.vx="<<initial_person_companion_point_.vx<<"; initial_person_companion_point_.vy="<<initial_person_companion_point_.vy<< std::endl;
+		//std::cout << " (in) vel_per="<<vel_per<<"; adequate_velocity="<<adequate_velocity<< std::endl;
+		//std::cout << " (in) distance_between_ini_pose_and_final_next_goal="<<distance_between_ini_pose_and_final_next_goal<< std::endl;
+
+	}
+
+	//std::cout << " (out) vel_per="<<vel_per<<"; adequate_velocity="<<adequate_velocity<<"; real_vel_per="<<real_vel_per<< std::endl;
+	//std::cout << " (out) distance_between_ini_pose_and_final_next_goal="<<distance_between_ini_pose_and_final_next_goal<< std::endl;
+
+	//std::cout << " peta en velocidad (14) "<< std::endl;
+
+	//if(debug_output_screen_mesages_){
+	//	std::cout << " !!!!!!!!!!!!!77777777!!!!!!!!!1 vel_per="<<vel_per<< std::endl;
+	//}
+
+	Spoint Robot_spoint_act(initial_robot_spoint_.x,initial_robot_spoint_.y,initial_robot_spoint_.time_stamp);
+	double distance_PR7=person1.distance(Robot_spoint_act);
+
+	Sdestination person_goal=person_obj1->get_best_dest();
+	double per_to_goal_angle = (180/3.14)*atan2(person1.y-person_goal.y , person1.x-person_goal.x);
+	Spoint robot_goal=get_robot_goal();
+	double robot_to_goal_angle = (180/3.14)*atan2(initial_robot_spoint_.y-robot_goal.y , initial_robot_spoint_.x-robot_goal.x);
+	if(actual_debug2_){
+		std::cout << " !!!!  !!! robot_to_goal_angle="<<robot_to_goal_angle<<"; robot_goal.x="<<robot_goal.x<<"; robot_goal.y="<<robot_goal.y<< std::endl;
+		std::cout << " !!!!  !!! per_to_goal_angle="<<per_to_goal_angle<< std::endl;
+		std::cout << " !!!!  !!! [reduce robot max velocity] angle1="<<angle1<< std::endl;
+		std::cout << " !!!!  !!! min_next_companion_angle_="<<min_next_companion_angle_<< std::endl;
+		std::cout << " !!!!  !!! vel_per="<<vel_per<< std::endl;
+		std::cout << " !!!!  !!! person_obj1->get_desired_velocity() ="<<person_obj1->get_desired_velocity()<< std::endl;
+		std::cout << " distance_PR7="<<distance_PR7<< std::endl;
+		std::cout << " (out if's) v_max_="<<robot_->get_v_max()<< std::endl; // quizas hay que limitar la angular tambien...
+		std::cout << " (out if's) w_max_="<<robot_->get_w_max()<< std::endl;
+		std::cout << " (out if's) v_break_="<<robot_->get_a_v_break()<< std::endl;
+		std::cout << " (out if's) a_v_max_="<<robot_->get_a_v_max()<< std::endl;
+		std::cout << " (out if's) a_w_max_="<<robot_->get_a_w_max()<< std::endl;
+	}
+	// SOLUCION PROVISIONAL!
+	// when the goal, starts to stop, higher priority than ppl
+
+	//double mult_per_vel=1.1; // antes 1.2
+	double near_obst=1;
+	if(debug_nadal2_){
+		std::cout << " (COLISION,OBSTACLE) vel_per="<<vel_per<< std::endl;
+	}
+	// find min dist to near obstacle. If obstacle a 2m => near_obst=1.5, multiplicador de velocidad. TODO= Lo mejor sería siempre aumentar la ventana temporal de gonzalo segun la velocidad. y tener una ventana siempre del mismo radio.
+
+	double min_dist_colli_act_global2=check_collision_companion_goal(Spoint_pose_command2,0);
+	/* (companion) Inicio calculo angulo correspondiente a distancia de choque*/
+
+	if(debug_cout_robot_){
+		std::cout <<"; (DISTANCIA al obstaculo desde el centro actual de ambos!!!) min_dist_colli_act_global2="<<min_dist_colli_act_global2<< std::endl;
+	}
+	if(debug_file_robot_){
+		std::ofstream fileMatlab2;
+		fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+		fileMatlab2 << "; (DISTANCIA al obstaculo desde el centro actual de ambos!!!) min_dist_colli_act_global2="<<min_dist_colli_act_global2<<"\n";
+		fileMatlab2.close();
+	}
+
+	if(debug_nadal2_){
+		std::cout <<" (initialize) angle1="<<angle1<<"; min_next_companion_angle_="<<min_next_companion_angle_<<"; (DISTANCIA al obstaculo desde el centro actual de ambos!!!) min_dist_colli_act_global2="<<min_dist_colli_act_global2<< std::endl;
+	}
+	double angle_colision=angle_companion_; // in grad.
+	if((min_dist_colli_act_global2==robot_person_companion_distance_)||(min_dist_colli_act_global2>robot_person_companion_distance_)){
+		angle_colision=angle_companion_;
+	}else{
+		angle_colision=(180/3.14)*(asin((min_dist_colli_act_global2)/(robot_person_companion_distance_)));
+		if(debug_nadal_){
+			std::cout <<" (initialize, colision) angle_colision="<<angle_colision<<"; robot_person_companion_distance_="<<robot_person_companion_distance_<< "; min_dist_colli_act_global2"<<min_dist_colli_act_global2<< std::endl;
+		}
+	}
+
+	if(debug_nadal_){
+		std::cout <<" (initialize) d_min ="<<d_min <<"; marge_angle_companion_"<<marge_angle_companion_<< std::endl;
+	}
+
+	double circunferencia_tope=(robot_person_proximity_distance_+robot_person_proximity_tolerance_+offset_atractive_);
+	//double distance_to_goal = robot_->get_current_pointV().distance( goal_);
+	if(debug_nadal_){
+		std::cout << " circunferencia_tope="<<circunferencia_tope<<"; robot_person_distance="<<robot_person_distance<<"; min_next_companion_angle_"<<min_next_companion_angle_<<"; min_dist_colli_act_global2="<<min_dist_colli_act_global2<<"; robot_person_companion_distance_="<<robot_person_companion_distance_<< std::endl;
+	}
+	//std::cout << std::endl;
+	//std::cout << std::endl;
+	//std::cout <<" [IMPORTANTE] robot_person_distance ="<<robot_person_distance <<"; < circunferencia_tope"<<circunferencia_tope<<"; angle1="<<angle1<<"; marge_angle_companion_="<<marge_angle_companion_<<"; min_next_companion_angle_"<<min_next_companion_angle_<< std::endl;
+	//std::cout <<" [IMPORTANTE] angle1="<<angle1<<" > (angle_companion_temp_movil_-marge_angle_companion_) ="<<angle_companion_temp_movil_-marge_angle_companion_ <<"; angle1="<<angle1<<" < (angle_companion_temp_movil_+marge_angle_companion_)="<<angle_companion_temp_movil_+marge_angle_companion_<< std::endl;
+	//std::cout <<" [IMPORTANTE] min_next_companion_angle_="<<min_next_companion_angle_<<" > (angle_companion_temp_movil_-marge_angle_companion_))="<<angle_companion_temp_movil_-marge_angle_companion_<<"; min_next_companion_angle_="<<min_next_companion_angle_<<" < (angle_companion_temp_movil_+marge_angle_companion_))="<<angle_companion_temp_movil_+marge_angle_companion_<< std::endl;
+	//std::cout << std::endl;
+	//std::cout << std::endl;
+	robot_orient_=false;
+	// INICIO Calculate the robot velocity to increment People distance + angle_increment_of_increment_distance_ [grados]
+	double distance_L_with_this_people_angle_increment=angle_increment_of_increment_distance_*(3.14/180)*(1/robot_person_proximity_distance_);
+	double distance_people_travelled=vel_per*dt_;
+	double actual_V_Robot=(distance_people_travelled+distance_L_with_this_people_angle_increment)/dt_;
+
+	if(debug_real_test_companion4_){
+		std::cout << "[(distance_people_travelled+distance_L_with_this_people_angle_increment)/dt_] actual_V_Robot="<<actual_V_Robot<< std::endl;
+	}
+
+	double distance_angle_diference_person_robot;
+	if(min_next_companion_angle_>angle1){
+		distance_angle_diference_person_robot=(min_next_companion_angle_-angle1)*(3.14/180)*(1/robot_person_proximity_distance_); // angulo que necesitas - angulo que tienes realmente.
+
+		if(debug_real_test_companion3_){
+			std::cout << " min_next_companion_angle_-angle1="<<min_next_companion_angle_-angle1<< "; min_next_companion_angle_="<<min_next_companion_angle_<<"; angle1="<<angle1<< std::endl;
+		}
+
+	}else{
+		distance_angle_diference_person_robot=(angle1-min_next_companion_angle_)*(3.14/180)*(1/robot_person_proximity_distance_);
+		if(debug_real_test_companion3_){
+			std::cout << " angle1-min_next_companion_angle_="<<angle1-min_next_companion_angle_<< "; min_next_companion_angle_="<<min_next_companion_angle_<<"; angle1="<<angle1<< std::endl;
+		}
+		//std::cout << " w=(angle1-min_next_companion_angle_)/0.2="<<(angle1-min_next_companion_angle_)/0.2<< std::endl;
+	}
+	if(debug_real_test_companion3_){
+		std::cout << " w=(min_next_companion_angle_-angle1)/0.2="<<(min_next_companion_angle_-angle1)/0.2<< std::endl; // el angulo_al_que_quiero_ir - el_angulo_real_que_tengo
+	}
+	double actual_V_Robot2=(distance_people_travelled+distance_angle_diference_person_robot)/dt_;
+
+	if(debug_real_test_companion3_){
+		std::cout << " actual_V_Robot="<<actual_V_Robot<< std::endl;
+		std::cout << "theta="<<theta<<"; actual_V_Robot2="<<actual_V_Robot2<< std::endl;
+	}
+
+	if(debug_real_test_companion_){
+		std::cout << " distance_L_with_this_people_angle_increment="<<distance_L_with_this_people_angle_increment<< std::endl;
+		std::cout << " dt_="<<dt_<< std::endl;
+		std::cout << " distance_people_travelled="<<distance_people_travelled<< std::endl;
+		std::cout << " actual_V_Robot="<<actual_V_Robot<< std::endl;
+	}
+	// FIN Calculate the robot velocity to increment People distance + angle_increment_of_increment_distance_ [grados]
+
+	// INICIO Calculate the robot velocity en giros!
+	// hay que calcular la distancia real que se desplaza la persona desde esta iteración a la siguiente según su predicción.
+	// o la distancia de la posición actual del robot a la siguiente que tiene que ir (calculada con la rama ganadora del path).
+	//  FIN  Calculate the robot velocity en giros!
+
+	double margen_angle_colision=1; // inicial era =15
+	if(debug_real_test_companion2_){
+		std::cout << " VELOCIDAAAAAAAAAAADDDDD"<< std::endl;
+		std::cout << " robot_person_distance="<<robot_person_distance<<"; circunferencia_tope="<<circunferencia_tope<<"; angle1="<<angle1<<"; min_next_companion_angle_="<<min_next_companion_angle_<<"; angle_companion_="<<angle_companion_<<"; marge_angle_companion_="<<marge_angle_companion_<<std::endl;
+		std::cout <<"; min_dist_colli_act_global2="<<min_dist_colli_act_global2<<"; robot_person_companion_distance_="<<robot_person_companion_distance_<< std::endl;
+		std::cout <<"; actual INCREMENT angle=angle1-min_next_companion_angle_=angulo_al_que_voy-angulo_al_que_quiero_ir"<<sqrt((angle1-min_next_companion_angle_)*(angle1-min_next_companion_angle_))<< std::endl;
+	}
+	/*  calcular velocidad persona maxima */
+	if(vel_per_max_caso_robot_a_0_o_180_grados_<vel_per){
+		vel_per_max_caso_robot_a_0_o_180_grados_=vel_per;
+	}
+
+	//std::cout << " ( d_min < 1.0 ) !!!!!!!!! (ini_plan) VELOCIDAAAAAAAAAAADDDDD; d_min ="<<d_min << std::endl;
+	if(debug_real_test_companion2_){
+		std::cout << " robot_person_distance="<<robot_person_distance<<"; circunferencia_tope="<<circunferencia_tope<<"; robot_person_proximity_distance_="<<robot_person_proximity_distance_<<"; marge_in_distance_="<<marge_in_distance_<<std::endl;
+	}
+	if(debug_real_test_companion4_){
+		std::cout <<" (!!! INITIAL replan_last_step !!!) angle1="<<angle1<<"; min_next_companion_angle_="<<min_next_companion_angle_<<"; robot_person_distance="<<robot_person_distance<<" vel_per="<<vel_per<< std::endl;
+	}
+
+
+	real_angle_person_robot_=angle1; // for results in matlab.
+	real_distance_person_robot_=robot_person_distance;
+
+	//std::cout << " IMPORTANT!!!! real_angle_person_robot_="<<real_angle_person_robot_<<"; real_distance_person_robot_="<<real_distance_person_robot_<<"; angle_companion_"<<angle_companion_<<"; marge_angle_companion_="<<marge_angle_companion_<< std::endl;
+
+
+	vel_per_ok_=Cperson_abstract::Far_goal;
+	go_with_vel_per_=false;
+
+	save_angle_between_person_and_robot_=angle1;
+	save_distance_between_person_and_robot_=robot_person_distance;
+
+
+/*
+	if((number_of_group_people_>1)&&(Zanlungo_model_)){ // TODO if only need the prefered vel of the robot for the Zanlungo model. Maybe I need also in the robot prediction. Test it.
+		double v_people=sqrt(mean_people_Zalungo_.vx*mean_people_Zalungo_.vx+mean_people_Zalungo_.vy*mean_people_Zalungo_.vy);
+		vel_per=Companion_Zanlungo_Model_.vp2v(v_people);
+		vel_per=Companion_Zanlungo_Model_.vp2v_vers2(v_people);
+		genome_params_.set_normal_pref_vel(vel_per);
+	}
+*/
+
+	/*if(Zanlungo_model_){
+		// ini zanlungo model
+		double act_module_force_total_ant=sqrt((ant_final_force_total_.fx*ant_final_force_total_.fx)+(ant_final_force_total_.fy*ant_final_force_total_.fy));
+		SpointV first_person=actual_person_Companion_pointer_->get_current_pointV();
+		SpointV robot_act_it=robot_->get_current_pointV();
+		double distance_p1_r=sqrt((robot_act_it.x-first_person.x)*(robot_act_it.x-first_person.x)+(robot_act_it.y-first_person.y)*(robot_act_it.y-first_person.y));
+		double distance_p2_r=10.0;
+		//std::cout << " (vel margin) Francesco V_stability (maxV) robot_act_it.x-first_person.x="<<sqrt((robot_act_it.x-first_person.x)*(robot_act_it.x-first_person.x))<<"; (robot_act_it.y-first_person.y)="<<sqrt((robot_act_it.y-first_person.y)*(robot_act_it.y-first_person.y))<<"; distance_p1_r="<<distance_p1_r<< std::endl;
+
+		if((number_of_group_people_>1)&&(we_have_pointer_to_second_person_)){
+
+			SpointV secon_person_comp=second_group_companion_person_obj_->get_current_pointV();
+			distance_p2_r=sqrt((robot_act_it.x-secon_person_comp.x)*(robot_act_it.x-secon_person_comp.x)+(robot_act_it.y-secon_person_comp.y)*(robot_act_it.y-secon_person_comp.y));
+			//std::cout << " (vel margin) Francesco V_stability (maxV) robot_act_it.x-secon_person_comp.x="<<sqrt((robot_act_it.x-secon_person_comp.x)*(robot_act_it.x-secon_person_comp.x))<<"; (robot_act_it.y-secon_person_comp.y)="<<sqrt((robot_act_it.y-secon_person_comp.y)*(robot_act_it.y-secon_person_comp.y))<<"; distance_p2_r="<<distance_p2_r<< std::endl;
+
+		}
+
+		//std::cout << " (vel margin) Francesco V_stability; !!! act_module_force_total_ant="<<act_module_force_total_ant<< std::endl;
+		if(act_module_force_total_ant < 0.2){  // threshold_max_total_force_=0.2
+			//robot_->set_v_max( vel_per*near_obst );
+			robot_->set_v_max( real_vel_per*near_obst );
+			//std::cout << " (vel margin) Francesco V_stability (perV) real_vel_per=robot_->get_v_max()="<<robot_->get_v_max()<< std::endl;
+		}
+		else if((act_module_force_total_ant < threshold_max_total_force_)&&(act_module_force_total_ant>0.2)){ // TODO: sacar este threshold de fuerza a fuera!!! el 0.4.
+			//robot_->set_v_max( vel_per*near_obst );
+			//robot_->set_v_max( 0.577);
+			robot_->set_v_max( max_v_by_system_);
+			if(debug_output_screen_mesages_){
+				std::cout << " (vel margin) Francesco V_stability (perV) vel_per=robot_->get_v_max()="<<robot_->get_v_max()<< std::endl;
+			}
+
+		}else{
+			//robot_->set_v_max( vel_per*near_obst );
+
+			if((number_of_group_people_>1)&&(we_have_pointer_to_second_person_)){
+
+				if((distance_p1_r<2.5)&&(distance_p2_r<2.5)){ // TODO: sacar estos threshols de distancia a fuera!!! el 2.5m
+					//robot_->set_v_max( vel_per*near_obst );
+					if(debug_output_screen_mesages_){
+							std::cout << " (vel margin) Francesco V_stability (maxV) (adaptada a lo que abanzas) vel_per=robot_->get_v_max()="<<robot_->get_v_max()<< std::endl;
+					}
+					robot_->set_v_max( max_v_by_system_);
+				}else{
+					robot_->set_v_max( max_v_by_system_);
+					if(debug_output_screen_mesages_){
+							std::cout << " (vel margin) Francesco V_stability (maxV) max_v_by_system_=robot_->get_v_max()="<<robot_->get_v_max()<< std::endl;
+					}
+
+				}
+			}else{
+
+				if((distance_p1_r<2.5)){ // TODO: sacar estos threshols de distancia a fuera!!! el 2.5m
+					//robot_->set_v_max( vel_per*near_obst );
+					if(debug_output_screen_mesages_){
+						std::cout << " (vel margin) Francesco V_stability (maxV) (adaptada a lo que abanzas) vel_per=robot_->get_v_max()="<<robot_->get_v_max()<< std::endl;
+					}
+					robot_->set_v_max( max_v_by_system_);
+				}else{
+					robot_->set_v_max( max_v_by_system_);
+					if(debug_output_screen_mesages_){
+						std::cout << " (vel margin) Francesco V_stability (maxV) max_v_by_system_=robot_->get_v_max()="<<robot_->get_v_max()<< std::endl;
+					}
+
+				}
+			}
+
+
+
+		}
+		// fin zanlungo model*/
+	//}else{
+		// ini side-by-side:
+		//std::cout << "[INI!] (1) After velocity companion: v_max_act="<<robot_->get_a_v_max()<<"; d_min="<<d_min<<"; max_v_by_system_="<<max_v_by_system_<< std::endl;
+
+		/*if( distance_to_goal < distance_to_stop_ ){ // quitada, porque ya la tengo en cuenta internamente al propagar...
+			if(debug_nadal2_){
+				std::cout << " (ini_plan) VELOCIDAAAAAAAAAAADDDDD= (goal)"<< std::endl;
+			}
+			if(actual_debug2_){
+				std::cout << " max_v_by_system_ * distance_to_goal / distance_to_stop_="<<max_v_by_system_ * distance_to_goal / distance_to_stop_<< std::endl;
+			}
+			robot_->set_v_max( max_v_by_system_ * distance_to_goal / distance_to_stop_  );//distance_to_stop is never 0
+		}
+		// if not near the goal, normal velocity regulation according to nearby ppl
+		else*/ if ( d_min < 1.0 ){
+
+			if(debug_real_test_companion4_){
+				std::cout << " ( d_min < 1.0 ) !!!!!!!!! (ini_plan) VELOCIDAAAAAAAAAAADDDDD="<< std::endl;
+				std::cout << " robot_person_distance="<<robot_person_distance<<"; circunferencia_tope="<<circunferencia_tope<<"; angle1="<<angle1<<"; min_next_companion_angle_="<<min_next_companion_angle_<< std::endl;
+				std::cout << " robot_person_proximity_distance_="<<robot_person_proximity_distance_<<"; marge_in_distance_="<<marge_in_distance_<<"; angle_companion_="<<angle_companion_<<"; marge_angle_companion_="<<marge_angle_companion_<< std::endl;
+			}
+			if((robot_person_distance>(robot_person_proximity_distance_-marge_in_distance_))&&(robot_person_distance<(robot_person_proximity_distance_+marge_in_distance_))&&(robot_person_distance<circunferencia_tope)&&(angle1<(angle_companion_+marge_angle_companion_))&&(angle1>(angle_companion_-marge_angle_companion_))&&(min_next_companion_angle_>(angle_companion_-5))&&(min_next_companion_angle_<(angle_companion_+5))){//&&((min_dist_colli_act_global2==robot_person_companion_distance_)||(min_dist_colli_act_global2>robot_person_companion_distance_))){
+
+				//if(debug_output_screen_mesages_){
+				//	std::cout << " caso d=1(1) in min_next_companion_angle_="<<min_next_companion_angle_<< std::endl;
+				//}
+				go_with_vel_per_=true;
+				robot_->set_v_max( vel_per*near_obst );
+
+				if(debug_cout_robot_){
+					std::cout << " caso d=1 (1) caso if="<< std::endl;
+					std::cout << "vel_per*near_obst ="<<vel_per*near_obst <<"; go_with_vel_per_="<<go_with_vel_per_<< std::endl;
+				}
+				if(debug_file_robot_){
+					std::ofstream fileMatlab2;
+					fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+					fileMatlab2 << " caso d=1 (1) caso if=\n";
+					fileMatlab2 << "vel_per*near_obst ="<<vel_per*near_obst <<"; go_with_vel_per_="<<go_with_vel_per_<<"\n";
+					fileMatlab2.close();
+				}
+
+
+				if(debug_real_test_companion4_){
+					std::cout << "max_v_by_system_ * 0.85= v_max_="<<robot_->get_v_max()<< std::endl;
+				}
+
+				if(actual_debug2_){
+					std::cout << " v_max_="<<robot_->get_v_max()<< std::endl; // quizas hay que limitar la angular tambien...
+					std::cout << " w_max_="<<robot_->get_w_max()<< std::endl;
+					std::cout << " v_break_="<<robot_->get_a_v_break()<< std::endl;
+				}
+
+				if(actual_debug2_){
+					std::cout << " a_v_max_="<<robot_->get_a_v_max()<< std::endl;
+					std::cout << " a_w_max_="<<robot_->get_a_w_max()<< std::endl;
+				}
+
+			}else if((robot_person_distance<circunferencia_tope)&&(min_next_companion_angle_>(0-30))&&(min_next_companion_angle_<(0+30))&&(((angle1<(0+30))&&(angle1>(0-30)))||((angle1<(180+30))&&(angle1>(180-30))))&&(distance_PR7<(robot_person_proximity_distance_+0.5))&&(distance_PR7>(robot_person_proximity_distance_-0.2))){
+				vel_per_ok_=Cperson_abstract::Vel_per;
+				go_with_vel_per_=true;
+				robot_->set_v_max( vel_per*near_obst );
+
+				//if(debug_output_screen_mesages_){
+				//	std::cout << "caso d=1 else if 2 (2) vel_per*near_obst ="<<vel_per*near_obst <<"; go_with_vel_per_="<<go_with_vel_per_<< std::endl;
+				//}
+				if(debug_file_robot_){
+					std::ofstream fileMatlab2;
+					fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+					fileMatlab2 << "caso d=1 else if 2 (2) vel_per*near_obst ="<<vel_per*near_obst <<"; go_with_vel_per_="<<go_with_vel_per_<<"\n";
+					fileMatlab2.close();
+				}
+
+				if(debug_real_test_companion4_){
+					std::cout << "  max_v_by_system_ * 0.85= vel_per="<<vel_per<<"; v_max_="<<robot_->get_v_max()<< std::endl;
+					std::cout << " (1) caso else if= ( persona y robot van a 0 grados o 180 y a la distancia necesaria entre ellos.)"<< std::endl;
+				}
+
+			}else if((robot_person_distance<circunferencia_tope)&&(min_dist_colli_act_global2>=robot_person_companion_distance_)&&(min_next_companion_angle_>(angle_colision-margen_angle_colision))&&(min_next_companion_angle_<(angle_colision+margen_angle_colision))&&(angle1<(angle_colision+margen_angle_colision))&&(angle1>(angle_colision-margen_angle_colision))){
+				vel_per_ok_=Cperson_abstract::Vel_per;
+				go_with_vel_per_=true;
+				robot_->set_v_max( vel_per*near_obst );
+
+				if(debug_cout_robot_){
+					std::cout << "(caso d=1) punto de equilibrio movil!!! if (3) vel_per*near_obst ="<<vel_per*near_obst <<"; go_with_vel_per_="<<go_with_vel_per_<< std::endl;
+				}
+				if(debug_file_robot_){
+					std::ofstream fileMatlab2;
+					fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+					fileMatlab2 << "(caso d=1) punto de equilibrio movil!!! if (3) vel_per*near_obst ="<<vel_per*near_obst <<"; go_with_vel_per_="<<go_with_vel_per_<<"\n";
+					fileMatlab2.close();
+				}
+
+				if(debug_real_test_companion4_){
+					std::cout << " (1) caso else if= ( persona y robot van angle_colision )"<< std::endl;
+					std::cout << "  max_v_by_system_ * 0.75=vel_per="<<vel_per<< "; v_max_="<<robot_->get_v_max()<< std::endl;
+					std::cout << " w_max=(v_max)/R_r=(v_max)/0.5="<<(robot_->get_v_max())/0.5<< std::endl;
+				}
+
+			}else{
+
+				//if(debug_output_screen_mesages_){
+				//	std::cout << " (1) caso else="<< std::endl;
+				//}
+				if(debug_cout_robot_){
+					std::cout << "(1) caso else vel_per*near_obst ="<<vel_per*near_obst <<"; go_with_vel_per_="<<go_with_vel_per_<< std::endl;
+				}
+
+				if((vel_per>0)&&(actual_V_Robot<max_v_by_system_)&&(actual_V_Robot>(max_v_by_system_ * 0.95))){
+					robot_->set_v_max( actual_V_Robot );
+					//robot_->set_v_max( max_v_by_system_ * 0.85 );
+					vel_per_ok_=Cperson_abstract::Far_goal;
+
+					//if(debug_output_screen_mesages_){
+					//	std::cout << " CASO ELSE (1.1.2)= actual_V_Robot"<< std::endl;
+					//	std::cout << " (1.1.2); robot_->get_v_max()="<<robot_->get_v_max()<< std::endl;
+					//}
+					if(debug_file_robot_){
+						std::ofstream fileMatlab2;
+						fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+						fileMatlab2 << " CASO ELSE (1.1.2)= actual_V_Robot\n";
+						fileMatlab2 << " (1.1.2); robot_->get_v_max()="<<robot_->get_v_max()<<"\n";
+						fileMatlab2.close();
+					}
+
+				}else{
+					vel_per_ok_=Cperson_abstract::Far_goal;
+					robot_->set_v_max( max_v_by_system_ * 0.95 );  	  // OJO!!! antes era 0.75
+
+					//if(debug_output_screen_mesages_){
+					//	std::cout << " CASO ELSE (1.1.2)= max_v_by_system_ * 0.85 o vel_per2"<< std::endl;
+					//	std::cout << " (1.1.2); robot_->get_v_max()="<<robot_->get_v_max()<< std::endl;
+					//}
+					if(debug_file_robot_){
+						std::ofstream fileMatlab2;
+						fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+						fileMatlab2 <<" CASO ELSE (1.1.2)= max_v_by_system_ * 0.85 o vel_per2\n";
+						fileMatlab2 << " (1.1.2); robot_->get_v_max()="<<robot_->get_v_max()<<"\n";
+						fileMatlab2.close();
+					}
+				}
+
+				if(debug_real_test_companion4_){
+					std::cout << " CASO ELSE (1.1.2)= max_v_by_system_ * 0.85"<< std::endl;
+					std::cout << " (1.1.2); robot_->get_v_max()="<<robot_->get_v_max()<< std::endl;
+				}
+
+				if(debug_cout_robot_){
+					std::cout << "(1) caso else robot_->get_v_max()="<<robot_->get_v_max() <<"; go_with_vel_per_="<<go_with_vel_per_<< std::endl;
+				}
+
+			}
+		}
+		else if ( d_min < 3.0){
+
+			//if(debug_output_screen_mesages_){
+			//	std::cout << "( d_min < 3.0) !!!!!!!!! (ini_plan) VELOCIDAAAAAAAAAAADDDDD="<< std::endl;
+			//	std::cout << " robot_person_distance="<<robot_person_distance<<"; circunferencia_tope="<<circunferencia_tope<<"; angle1="<<angle1<<"; min_next_companion_angle_="<<min_next_companion_angle_<< std::endl;
+			//	std::cout << " robot_person_proximity_distance_="<<robot_person_proximity_distance_<<"; marge_in_distance_="<<marge_in_distance_<<"; angle_companion_="<<angle_companion_<<"; marge_angle_companion_="<<marge_angle_companion_<< std::endl;
+
+		//	}
+
+			if((robot_person_distance>(robot_person_proximity_distance_-marge_in_distance_))&&(robot_person_distance<(robot_person_proximity_distance_+marge_in_distance_))&&(robot_person_distance<circunferencia_tope)&&(angle1<(angle_companion_+marge_angle_companion_))&&(angle1>(angle_companion_-marge_angle_companion_))&&(min_next_companion_angle_>(angle_companion_-5))&&(min_next_companion_angle_<(angle_companion_+5))){//&&((min_dist_colli_act_global2==robot_person_companion_distance_)||(min_dist_colli_act_global2>robot_person_companion_distance_))){
+				//robot_person_distance<circunferencia_tope => es para estar al lado, cerca de la persona para limitar velocidad.
+				// min_dist_colli_act_global2==robot_person_companion_distance_)||(min_dist_colli_act_global2>robot_person_companion_distance_ => no tengo obstaculos cerca, limito velocidad.
+				// caso angulo_real_persona_robot == angulo que quieres por distancia == distancia_sphere_maxima, quieres estar a angle_companion_=90grados.
+				// caso angulo_real_persona_robot == angulo que quieres por distancia == distancia_sphere_maxima, quieres estar a angle_companion_=90grados.
+
+				if(actual_debug2_){
+					std::cout << " (2) in min_next_companion_angle_="<<min_next_companion_angle_<< std::endl;
+				}
+				go_with_vel_per_=true;
+				robot_->set_v_max( vel_per*near_obst );
+				//robot_->set_v_max( max_v_by_system_ * 0.85 );
+
+				if(debug_cout_robot_){
+					std::cout << "(caso d=2) if (1) vel_per*near_obst ="<<vel_per*near_obst <<"; go_with_vel_per_="<<go_with_vel_per_<< std::endl;
+				}
+				if(debug_file_robot_){
+					std::ofstream fileMatlab2;
+					fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+					fileMatlab2 << "(caso d=2) if (1) vel_per*near_obst ="<<vel_per*near_obst <<"; go_with_vel_per_="<<go_with_vel_per_<<"\n";
+					fileMatlab2.close();
+				}
+
+				vel_per_ok_=Cperson_abstract::Vel_per;
+
+				if(debug_real_test_companion4_){
+					std::cout << " (2) caso if=max_v_by_system_ * 0.90 "<< std::endl; // OJO! antes era 0.85!!!
+					std::cout << " v_max_="<<robot_->get_v_max()<< std::endl;
+				}
+
+				if(actual_debug2_){
+					std::cout << " v_max_="<<robot_->get_v_max()<< std::endl; // quizas hay que limitar la angular tambien...
+					std::cout << " w_max_="<<robot_->get_w_max()<< std::endl;
+					std::cout << " v_break_="<<robot_->get_a_v_break()<< std::endl;
+					std::cout << " a_v_max_="<<robot_->get_a_v_max()<< std::endl;
+					std::cout << " a_w_max_="<<robot_->get_a_w_max()<< std::endl;
+				}
+
+			}else if((robot_person_distance<circunferencia_tope)&&(min_next_companion_angle_>(0-30))&&(min_next_companion_angle_<(0+30))&&(((angle1<(0+30))&&(angle1>(0-30)))||((angle1<(180+30))&&(angle1>(180-30))))&&(distance_PR7<(robot_person_proximity_distance_+0.5))&&(distance_PR7>(robot_person_proximity_distance_-0.2))){
+				vel_per_ok_=Cperson_abstract::Vel_per;
+				go_with_vel_per_=true;
+				robot_->set_v_max( vel_per*near_obst );
+
+				if(debug_cout_robot_){
+					std::cout << " (caso d=2 )caso else if 1 (2) vel_per*near_obst ="<<vel_per*near_obst << "; go_with_vel_per_="<<go_with_vel_per_<< std::endl;
+				}
+				if(debug_file_robot_){
+					std::ofstream fileMatlab2;
+					fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+					fileMatlab2 << " (caso d=2 )caso else if 1 (2) vel_per*near_obst ="<<vel_per*near_obst << "; go_with_vel_per_="<<go_with_vel_per_<<"\n";
+					fileMatlab2.close();
+				}
+
+
+				if(debug_real_test_companion4_){
+					std::cout << " (caso d=2 )caso else if (2) caso else if= max_v_by_system_ * 0.90 ( persona y robot van a 0 grados o 180 y a la distancia necesaria entre ellos.)"<< std::endl;
+					std::cout << " vel_per="<<vel_per<<"; v_max_="<<robot_->get_v_max()<< std::endl;
+				}
+			}else if((robot_person_distance<circunferencia_tope)&&(min_dist_colli_act_global2>=robot_person_companion_distance_)&&(min_next_companion_angle_>(angle_colision-margen_angle_colision))&&(min_next_companion_angle_<(angle_colision+margen_angle_colision))&&(angle1<(angle_colision+margen_angle_colision))&&(angle1>(angle_colision-margen_angle_colision))){
+				vel_per_ok_=Cperson_abstract::Vel_per;
+				go_with_vel_per_=true;
+				robot_->set_v_max( vel_per*near_obst );
+
+				if(debug_cout_robot_){
+					std::cout << " (caso d=2 ) caso else if 2, Caso punto equilibro movil caso else if (3)vel_per*near_obst ="<<vel_per*near_obst << "; go_with_vel_per_="<<go_with_vel_per_<< std::endl;
+				}
+				if(debug_file_robot_){
+					std::ofstream fileMatlab2;
+					fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+					fileMatlab2 << " (caso d=2 )caso else if 2, Caso punto equilibro movil caso else if (3)vel_per*near_obst ="<<vel_per*near_obst << "; go_with_vel_per_="<<go_with_vel_per_<<"\n";
+					fileMatlab2.close();
+				}
+
+				if(debug_real_test_companion4_){
+					std::cout << " (2) caso else if= max_v_by_system_ * 0.85 "<< std::endl;
+					std::cout << " vel_per="<<vel_per<< "; v_max_="<<robot_->get_v_max()<< std::endl;
+				}
+
+			}else{
+
+				if(debug_real_test_companion_){
+					std::cout << " (2) caso else="<< std::endl;
+				}
+
+				if((vel_per>0)&&(actual_V_Robot<max_v_by_system_)&&(actual_V_Robot>(max_v_by_system_ * 0.95))){
+					robot_->set_v_max( actual_V_Robot );
+					vel_per_ok_=Cperson_abstract::Far_goal;
+
+					if(debug_cout_robot_){
+						std::cout << " CASO ELSE (2.1.2)= actual_V_Robot"<< std::endl;
+						std::cout << " (2.1.2); robot_->get_v_max()="<<robot_->get_v_max()<< std::endl;
+					}
+					if(debug_file_robot_){
+						std::ofstream fileMatlab2;
+						fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+						fileMatlab2 <<  " CASO ELSE (2.1.2)= actual_V_Robot\n";
+						fileMatlab2 <<  " (2.1.2); robot_->get_v_max()="<<robot_->get_v_max()<<"\n";
+						fileMatlab2.close();
+					}
+				}else{
+					vel_per_ok_=Cperson_abstract::Far_goal;
+					robot_->set_v_max( max_v_by_system_ * 0.95 );  	  // OJO!!! antes era 0.75
+
+					if(debug_cout_robot_){
+						std::cout << " CASO ELSE (2.1.2)= max_v_by_system_ * 0.85 o vel_per2"<< std::endl;
+						std::cout << " (2.1.2); robot_->get_v_max()="<<robot_->get_v_max()<< std::endl;
+					}
+					if(debug_file_robot_){
+						std::ofstream fileMatlab2;
+						fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+						fileMatlab2 <<  " CASO ELSE (2.1.2)= max_v_by_system_ * 0.85 o vel_per2\n";
+						fileMatlab2 <<  " (2.1.2); robot_->get_v_max()="<<robot_->get_v_max()<<"\n";
+						fileMatlab2.close();
+					}
+				}
+
+				if(debug_real_test_companion4_){
+					std::cout << " CASO ELSE (2.1)= max_v_by_system_ "<< std::endl;
+					std::cout << " (2.1); robot_->get_v_max()="<<robot_->get_v_max()<< std::endl;
+				}
+				if(debug_cout_robot_){
+					std::cout << "(2) caso else robot_->get_v_max()="<<robot_->get_v_max() <<"; go_with_vel_per_="<<go_with_vel_per_<< std::endl;
+				}
+
+			}
+		}
+		else if ( d_min < 5.0 ){
+
+			//if(debug_output_screen_mesages_){
+			//	std::cout << " ( d_min < 5.0 ) !!!!!!!!! (ini_plan) VELOCIDAAAAAAAAAAADDDDD="<< std::endl;
+			//	std::cout << " robot_person_distance="<<robot_person_distance<<"; circunferencia_tope="<<circunferencia_tope<<"; angle1="<<angle1<<"; min_next_companion_angle_="<<min_next_companion_angle_<< std::endl;
+			//	std::cout << " robot_person_proximity_distance_="<<robot_person_proximity_distance_<<"; marge_in_distance_="<<marge_in_distance_<<"; angle_companion_="<<angle_companion_<<"; marge_angle_companion_="<<marge_angle_companion_<< std::endl;
+
+			//}
+
+			if((robot_person_distance>(robot_person_proximity_distance_-marge_in_distance_))&&(robot_person_distance<(robot_person_proximity_distance_+marge_in_distance_))&&(robot_person_distance<circunferencia_tope)&&(angle1<(angle_companion_+marge_angle_companion_))&&(angle1>(angle_companion_-marge_angle_companion_))&&(min_next_companion_angle_>(angle_companion_-5))&&(min_next_companion_angle_<(angle_companion_+5))){//&&((min_dist_colli_act_global2==robot_person_companion_distance_)||(min_dist_colli_act_global2>robot_person_companion_distance_))){
+				//robot_person_distance<circunferencia_tope => es para estar al lado, cerca de la persona para limitar velocidad.
+				// min_dist_colli_act_global2==robot_person_companion_distance_)||(min_dist_colli_act_global2>robot_person_companion_distance_ => no tengo obstaculos cerca, limito velocidad.
+				// caso angulo_real_persona_robot == angulo que quieres por distancia == distancia_sphere_maxima, quieres estar a angle_companion_=90grados.
+				// caso angulo_real_persona_robot == angulo que quieres por distancia == distancia_sphere_maxima, quieres estar a angle_companion_=90grados.
+
+				if(debug_real_test_companion_){
+					std::cout << " (3) caso if="<< std::endl;
+				}
+				if(debug_real_test_companion4_){
+					std::cout << " (3) in min_next_companion_angle_="<<min_next_companion_angle_<< std::endl;
+				}
+				vel_per_ok_=Cperson_abstract::Vel_per;
+				go_with_vel_per_=true;
+				robot_->set_v_max( vel_per*near_obst );
+
+				if(debug_cout_robot_){
+					std::cout << "(caso d=3 )caso if (1) vel_per*near_obst ="<<vel_per*near_obst << "; go_with_vel_per_="<<go_with_vel_per_<< std::endl;
+				}
+				if(debug_file_robot_){
+					std::ofstream fileMatlab2;
+					fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+					fileMatlab2 << "(caso d=3 )caso if (1) vel_per*near_obst ="<<vel_per*near_obst << "; go_with_vel_per_="<<go_with_vel_per_<<"\n";
+					fileMatlab2.close();
+				}
+
+				if(debug_real_test_companion4_){
+					std::cout << " max_v_by_system_ * 0.95= v_max_="<<robot_->get_v_max()<< std::endl;
+				}
+
+			}else if((robot_person_distance<circunferencia_tope)&&(min_next_companion_angle_>(0-30))&&(min_next_companion_angle_<(0+30))&&(((angle1<(0+30))&&(angle1>(0-30)))||((angle1<(180+30))&&(angle1>(180-30))))&&(distance_PR7<(robot_person_proximity_distance_+0.5))&&(distance_PR7>(robot_person_proximity_distance_-0.2))){
+				vel_per_ok_=Cperson_abstract::Vel_per;
+				robot_->set_v_max( vel_per*near_obst );
+
+				if(debug_cout_robot_){
+					std::cout << " (caso d=3 )caso else if (2) vel_per*near_obst ="<<vel_per*near_obst << "; go_with_vel_per_="<<go_with_vel_per_<< std::endl;
+				}
+				if(debug_file_robot_){
+					std::ofstream fileMatlab2;
+					fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+					fileMatlab2 << " (caso d=3 )caso else if (2) vel_per*near_obst ="<<vel_per*near_obst << "; go_with_vel_per_="<<go_with_vel_per_<<"\n";
+					fileMatlab2.close();
+				}
+
+				if(debug_real_test_companion4_){
+					std::cout << " (3) caso else if= ( persona y robot van a 0 grados o 180 y a la distancia necesaria entre ellos.)"<< std::endl;
+					std::cout << "  max_v_by_system_ * 0.95=vel_per="<<vel_per<<"; v_max_="<<robot_->get_v_max()<< std::endl;
+				}
+			}else if((robot_person_distance<circunferencia_tope)&&(min_dist_colli_act_global2>=robot_person_companion_distance_)&&(min_next_companion_angle_>(angle_colision-margen_angle_colision))&&(min_next_companion_angle_<(angle_colision+margen_angle_colision))&&(angle1<(angle_colision+margen_angle_colision))&&(angle1>(angle_colision-margen_angle_colision))){
+				vel_per_ok_=Cperson_abstract::Vel_per;
+				go_with_vel_per_=true;
+				robot_->set_v_max( vel_per*near_obst );
+
+				if(debug_cout_robot_){
+					std::cout << "(caso d=3 ) caso punto equilibrio movil!!! caso else if (3) vel_per*near_obst ="<<vel_per*near_obst << "; go_with_vel_per_="<<go_with_vel_per_<< std::endl;
+				}
+				if(debug_file_robot_){
+					std::ofstream fileMatlab2;
+					fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+					fileMatlab2 << "(caso d=3 ) caso punto equilibrio movil!!! caso else if (3) vel_per*near_obst ="<<vel_per*near_obst << "; go_with_vel_per_="<<go_with_vel_per_<<"\n";
+					fileMatlab2.close();
+				}
+
+				if(debug_real_test_companion4_){
+					std::cout << " (3) caso else if= ( persona y robot van angle_colision )"<< std::endl;
+					std::cout << "  max_v_by_system_ * 0.95=vel_per="<<vel_per<< "; v_max_="<<robot_->get_v_max()<< std::endl;
+				}
+
+			}else{
+				if(debug_real_test_companion_){
+					std::cout << " (3) caso else="<< std::endl;
+				}
+				if((vel_per>0)&&(actual_V_Robot<max_v_by_system_)&&(actual_V_Robot>(max_v_by_system_ * 0.95))){
+					robot_->set_v_max( actual_V_Robot );
+					vel_per_ok_=Cperson_abstract::Far_goal;
+					if(debug_cout_robot_){
+						std::cout << " CASO ELSE (1.1.2)= actual_V_Robot"<< std::endl;
+						std::cout << " (3.1.2); robot_->get_v_max()="<<robot_->get_v_max()<< std::endl;
+					}
+					if(debug_file_robot_){
+						std::ofstream fileMatlab2;
+						fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+						fileMatlab2 << " CASO ELSE (1.1.2)= actual_V_Robot\n";
+						fileMatlab2 << " (3.1.2); robot_->get_v_max()="<<robot_->get_v_max()<<"\n";
+						fileMatlab2.close();
+					}
+				}else{
+
+					vel_per_ok_=Cperson_abstract::Far_goal;
+					robot_->set_v_max( max_v_by_system_ * 0.95 );  	  // OJO!!! antes era 0.75
+
+					if(debug_cout_robot_){
+						std::cout << " CASO ELSE (1.1.2)= max_v_by_system_ * 0.85 "<< std::endl;
+						std::cout << " (3.1.2); robot_->get_v_max()="<<robot_->get_v_max()<< std::endl;
+					}
+					if(debug_file_robot_){
+						std::ofstream fileMatlab2;
+						fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+						fileMatlab2 << "  CASO ELSE (1.1.2)= max_v_by_system_ * 0.85 \n";
+						fileMatlab2 << " (3.1.2); robot_->get_v_max()="<<robot_->get_v_max()<<"\n";
+						fileMatlab2.close();
+					}
+				}
+
+				if(debug_real_test_companion4_){
+					std::cout << " out CASO ELSE (3.1)= max_v_by_system_ * 0.95"<< std::endl;
+					std::cout << " (3.1); robot_->get_v_max()="<<robot_->get_v_max()<< std::endl;
+				}
+				if(debug_cout_robot_){
+					std::cout << "(3) caso else robot_->get_v_max()="<<robot_->get_v_max() <<"; go_with_vel_per_="<<go_with_vel_per_<< std::endl;
+				}
+
+			}
+		}
+		else{
+			//if(debug_output_screen_mesages_){
+			//	std::cout << " else !!!!!!!!! (ini_plan) VELOCIDAAAAAAAAAAADDDDD="<< std::endl;
+			//	std::cout << " robot_person_distance="<<robot_person_distance<<"; circunferencia_tope="<<circunferencia_tope<<"; angle1="<<angle1<<"; min_next_companion_angle_="<<min_next_companion_angle_<< std::endl;
+			//	std::cout << " robot_person_proximity_distance_="<<robot_person_proximity_distance_<<"; marge_in_distance_="<<marge_in_distance_<<"; angle_companion_="<<angle_companion_<<"; marge_angle_companion_="<<marge_angle_companion_<< std::endl;
+
+			//}
+
+			if((robot_person_distance>(robot_person_proximity_distance_-marge_in_distance_))&&(robot_person_distance<(robot_person_proximity_distance_+marge_in_distance_))&&(robot_person_distance<circunferencia_tope)&&(angle1<(angle_companion_+marge_angle_companion_))&&(angle1>(angle_companion_-marge_angle_companion_))&&(min_next_companion_angle_>(angle_companion_-5))&&(min_next_companion_angle_<(angle_companion_+5))){//&&((min_dist_colli_act_global2==robot_person_companion_distance_)||(min_dist_colli_act_global2>robot_person_companion_distance_))){
+			//robot_person_distance<circunferencia_tope => es para estar al lado, cerca de la persona para limitar velocidad.
+			// min_dist_colli_act_global2==robot_person_companion_distance_)||(min_dist_colli_act_global2>robot_person_companion_distance_ => no tengo obstaculos cerca, limito velocidad.
+			// caso angulo_real_persona_robot == angulo que quieres por distancia == distancia_sphere_maxima, quieres estar a angle_companion_=90grados.
+			// caso angulo_real_persona_robot == angulo que quieres por distancia == distancia_sphere_maxima, quieres estar a angle_companion_=90grados.
+			//if(debug_output_screen_mesages_){
+			//	std::cout << " (4) caso if="<< std::endl;
+			//}
+			if(actual_debug2_){
+				std::cout << " (4) in min_next_companion_angle_="<<min_next_companion_angle_<< std::endl;
+			}
+			//if(!Zanlungo_model_){
+				// ini side-by-side:
+				vel_per_ok_=Cperson_abstract::Vel_per;
+				go_with_vel_per_=true;
+				//robot_->set_v_max( max_v_by_system_);
+				robot_->set_v_max( vel_per*near_obst );
+				// fin side-by-side
+			/*}else{
+				//ini zanlungo
+				double act_module_force_total_ant=sqrt((ant_final_force_total_.fx*ant_final_force_total_.fx)+(ant_final_force_total_.fy*ant_final_force_total_.fy));
+				if(act_module_force_total_ant< 0.4){
+					robot_->set_v_max( vel_per*near_obst );
+					//robot_->set_v_max( 0.577);
+					//robot_->set_v_max( max_v_by_system_);
+					if(debug_output_screen_mesages_){
+						std::cout << " (vel margin) Francesco V_stability (perV) max_v_by_system_=robot_->get_v_max()="<<robot_->get_v_max()<< std::endl;
+					}
+
+				}//else if((min_next_companion_angle_==angle_companion_)||(min_next_companion_angle_==0)||(min_next_companion_angle_==180)){
+				//	robot_->set_v_max(  vel_per*near_obst );
+				//	std::cout << " Francesco V_stability (perV-anlges) max_v_by_system_=robot_->get_v_max()="<<robot_->get_v_max()<< std::endl;
+				//}
+				else{
+					//robot_->set_v_max( vel_per*near_obst );
+					robot_->set_v_max( max_v_by_system_);
+					//if(debug_output_screen_mesages_){
+					//	std::cout << " (vel margin) Francesco V_stability (maxV) max_v_by_system_=robot_->get_v_max()="<<robot_->get_v_max()<< std::endl;
+					//}
+
+				}
+				// fin zanlungo.
+			}*/
+
+
+			if(debug_cout_robot_){
+				std::cout << "(4) if vel_per*near_obst ="<<vel_per*near_obst << "; go_with_vel_per_="<<go_with_vel_per_<< std::endl;
+			}
+			if(debug_file_robot_){
+				std::ofstream fileMatlab2;
+				fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+				fileMatlab2 << "(4) if vel_per*near_obst ="<<vel_per*near_obst << "; go_with_vel_per_="<<go_with_vel_per_<<"\n";
+				fileMatlab2.close();
+			}
+
+			//std::cout << "robot_->set_v_max( max_v_by_system_ );"<< std::endl;
+			if(debug_real_test_companion4_){
+				std::cout << " max_v_by_system_=v_max_="<<robot_->get_v_max()<< std::endl;
+			}
+
+			}else if((robot_person_distance<circunferencia_tope)&&(min_next_companion_angle_>(0-30))&&(min_next_companion_angle_<(0+30))&&(((angle1<(0+30))&&(angle1>(0-30)))||((angle1<(180+30))&&(angle1>(180-30))))&&(distance_PR7<(robot_person_proximity_distance_+0.5))&&(distance_PR7>(robot_person_proximity_distance_-0.2))){
+				//if(debug_output_screen_mesages_){
+				//	std::cout << " (4) caso else if= ( persona y robot van a 0 grados o 180 y a la distancia necesaria entre ellos.)"<< std::endl;
+				//}
+				vel_per_ok_=Cperson_abstract::Vel_per;
+				go_with_vel_per_=true;
+				robot_->set_v_max( vel_per*near_obst );
+				robot_->set_v_max( max_v_by_system_  );
+				//std::cout << "robot_->set_v_max( max_v_by_system_ );"<< std::endl;
+
+				if(debug_cout_robot_){
+					std::cout << "(4) else if 1 vel_per*near_obst ="<<vel_per*near_obst << "; go_with_vel_per_="<<go_with_vel_per_<< std::endl;
+					std::cout << " max_v_by_system_=vel_per="<<vel_per<<"; v_max_="<<robot_->get_v_max()<< std::endl;
+				}
+				if(debug_file_robot_){
+					std::ofstream fileMatlab2;
+					fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+					fileMatlab2 << "(4) else if 1 vel_per*near_obst ="<<vel_per*near_obst << "; go_with_vel_per_="<<go_with_vel_per_<<"\n";
+					fileMatlab2 <<" max_v_by_system_=vel_per="<<vel_per<<"; v_max_="<<robot_->get_v_max()<< "\n";
+					fileMatlab2.close();
+				}
+
+			}else if((robot_person_distance<circunferencia_tope)&&(min_dist_colli_act_global2>=robot_person_companion_distance_)&&(min_next_companion_angle_>(angle_colision-margen_angle_colision))&&(min_next_companion_angle_<(angle_colision+margen_angle_colision))&&(angle1<(angle_colision+margen_angle_colision))&&(angle1>(angle_colision-margen_angle_colision))){
+				//if(debug_output_screen_mesages_){
+				//	std::cout << " (4) caso else if 2= ( persona y robot van angle_colision )"<< std::endl;
+				//}
+				vel_per_ok_=Cperson_abstract::Vel_per;
+				go_with_vel_per_=true;
+				robot_->set_v_max( vel_per*near_obst );
+
+				if(debug_cout_robot_){
+					std::cout << "(4) else if 2 vel_per*near_obst ="<<vel_per*near_obst << "; go_with_vel_per_="<<go_with_vel_per_<< std::endl;
+				}
+				if(debug_file_robot_){
+					std::ofstream fileMatlab2;
+					fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+					fileMatlab2 << "(4) else if 2 vel_per*near_obst ="<<vel_per*near_obst << "; go_with_vel_per_="<<go_with_vel_per_<<"\n";
+					fileMatlab2 <<" max_v_by_system_=vel_per="<<vel_per<<"; v_max_="<<robot_->get_v_max()<< "\n";
+					fileMatlab2.close();
+				}
+
+				if(debug_real_test_companion_){
+					std::cout << " max_v_by_system_=vel_per="<<vel_per<< "; v_max_="<<robot_->get_v_max()<< std::endl;
+				}
+			}else{
+				//if(debug_output_screen_mesages_){
+				//	std::cout << " (4) caso else="<< std::endl;
+				//}
+
+					//if(!Zanlungo_model_){
+						// ini side-by-side:
+						if((min_next_companion_angle_!=angle_companion_)||(min_next_companion_angle_!=0)||(min_next_companion_angle_!=180)){
+										vel_per_ok_=Cperson_abstract::Near_goal;
+										robot_->set_v_max( max_v_by_system_);
+
+										//if(debug_output_screen_mesages_){
+										//	std::cout << "(4) if 1 robot_->set_v_max( max_v_by_system_ ); max_v_by_system_="<<max_v_by_system_<< std::endl;
+										//}
+										if(debug_file_robot_){
+											std::ofstream fileMatlab2;
+											fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+											fileMatlab2 << " (4) if 1 robot_->set_v_max( max_v_by_system_ ); max_v_by_system_="<<max_v_by_system_<< "\n";
+											fileMatlab2.close();
+										}
+
+									}else{
+										vel_per_ok_=Cperson_abstract::Far_goal;
+										robot_->set_v_max( max_v_by_system_ );
+										//if(debug_output_screen_mesages_){
+										//	std::cout << "(4) else 1 robot_->set_v_max( max_v_by_system_ ); max_v_by_system_="<<max_v_by_system_<< std::endl;
+										//}
+										if(debug_file_robot_){
+											std::ofstream fileMatlab2;
+											fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+											fileMatlab2 << "(4) else 1robot_->set_v_max( max_v_by_system_ ); max_v_by_system_="<<max_v_by_system_<< "\n";
+											fileMatlab2.close();
+										}
+									}
+
+									if(debug_real_test_companion4_){
+										std::cout << " CASO ELSE (4.1)= max_v_by_system_ "<< std::endl;
+										std::cout << " (4.1); max_v_by_system_=robot_->get_v_max()="<<robot_->get_v_max()<< std::endl;
+									}
+									if(debug_cout_robot_){
+										std::cout << "(4) caso else robot_->get_v_max()="<<robot_->get_v_max() <<"; go_with_vel_per_="<<go_with_vel_per_<<"; max_v_by_system_="<<max_v_by_system_<< std::endl;
+									}
+
+					// fin side-by-side:
+					/*}else{
+
+						double act_module_force_total_ant=sqrt((ant_final_force_total_.fx*ant_final_force_total_.fx)+(ant_final_force_total_.fy*ant_final_force_total_.fy));
+						if(act_module_force_total_ant< 0.4){
+							robot_->set_v_max( vel_per*near_obst );
+							//robot_->set_v_max( 0.577);
+							//robot_->set_v_max( max_v_by_system_);
+							if(debug_output_screen_mesages_){
+								std::cout << " (vel margin) Francesco V_stability (perV) max_v_by_system_=robot_->get_v_max()="<<robot_->get_v_max()<< std::endl;
+							}
+
+						}else if((min_next_companion_angle_==angle_companion_)||(min_next_companion_angle_==0)||(min_next_companion_angle_==180)){
+							robot_->set_v_max(  vel_per*near_obst );
+							std::cout << " Francesco V_stability (perV-anlges) max_v_by_system_=robot_->get_v_max()="<<robot_->get_v_max()<< std::endl;
+						}
+						else{
+							//robot_->set_v_max( vel_per*near_obst );
+							robot_->set_v_max( max_v_by_system_);
+							if(debug_output_screen_mesages_){
+								std::cout << " (vel margin) Francesco V_stability (maxV) max_v_by_system_=robot_->get_v_max()="<<robot_->get_v_max()<< std::endl;
+							}
+
+						}
+
+
+					}*/
+
+
+
+
+	////////////
+			}
+		}
+
+		////////////////
+		// fin side-by-side
+	//}
+
+	//if(debug_output_screen_mesages_){
+	//	std::cout << " !!!!!!!!!!!!!! 77777777777!!!!!!!!!! robot_->get_v_max()="<<robot_->get_v_max() << std::endl;
+	//}
+
+	//std::cout << "(1) After velocity companion: v_max_act="<<robot_->get_a_v_max()<<"; d_min="<<d_min<<"; max_v_by_system_="<<max_v_by_system_<< std::endl;
+
+	//double v_max_act=robot_->get_v_max();
+	//std::cout << "v_max_act"<<v_max_act<< std::endl;
+
+	//if(initial_robot_spoint_.distance(initial_person_goal_point_)<2.0){
+		//robot_->set_v_max( 0.5 );
+		//std::cout << "NEAR PERSON GOAL reduce v_max_act="<<robot_->get_a_v_max()<< std::endl;
+	//}
+	//std::cout << "(2) After velocity companion: v_max_act="<<robot_->get_a_v_max()<<"; person_radi_="<<person_radi_<<"; max_v_by_system_="<<max_v_by_system_<< std::endl;
+
+
+	//std::cout << " OUT CHANGE VEL person!!!!  ; v_max_="<<robot_->get_v_max()<<"; max_v_by_system_="<<max_v_by_system_<< std::endl;
+}
+
+void Cplan_local_nav_person_companion::return_max_velocity_systemRobot_to_max_value(){
+
+	// ----------------------------------------------------------------------
+	    //select persons considered in the scene and reserve memory for planning
+	   /* Spoint robot_position = (Spoint) robot_->get_current_pointV();
+	    double d_ini,d_end,radii_2(workspace_radii_*workspace_radii_),d_min(1e10);
+	    nearby_person_list_.clear();
+	    for( auto iit: person_list_ )
+	    {
+	    	//It requires prior trajectory prediction, careful...
+	    	d_ini = iit->get_prediction_trajectory()->front().distance2( robot_position );
+	    	d_end = iit->get_prediction_trajectory()->back().distance2( robot_position );
+	    	if( d_ini < radii_2 || d_end < radii_2 )
+	    	{
+	    		//nearby_person_list_.push_back( iit );
+	    		//iit->clear_planning_trajectory();
+	    		//iit->reserve_planning_trajectory( max_iter_ );//if already of this size, does nothing
+	    	}
+
+	    	//check for the neares obstacle in roder to calculate velocities
+	    	if ( d_ini < d_min)
+	    		d_min = d_ini;
+	    }
+*/
+	    // ----------------------------------------------------------------------
+	    //number of nearby obstacles: first approach, just count them...
+	/*    int number_of_obstacles(0);
+	    for( auto iit: laser_obstacle_list_ )
+	    {
+	    	//d_ini = iit.distance2( robot_position );
+	    	d_ini = iit.distance( robot_position );
+	    	if( d_ini < radii_2  )
+	    	{
+	    		//number_of_obstacles++;
+	    	}
+	    	if ( d_ini < d_min )
+	    		d_min = d_ini;
+	    }*/
+
+	    //number_of_obstacles_=number_of_obstacles;
+
+	    // calculate the desired robot velocity depending on the nearbiest obstacle/person and goal
+	    double distance_to_goal = robot_->get_current_pointV().distance( goal_);
+		// when the goal, starts to stop, higher priority than ppl
+	    if( distance_to_goal < distance_to_stop_ )
+			robot_->set_v_max( max_v_by_system_ * distance_to_goal / distance_to_stop_  );//distance_to_stop is never 0
+	    // if not near the goal, normal velocity regulation according to nearby ppl
+	    else if ( d_min_global_ < 1.0 )
+	    	robot_->set_v_max( max_v_by_system_ * 0.75 );
+	    else if ( d_min_global_ < 2.0)
+	    	robot_->set_v_max( max_v_by_system_ * 0.85);
+	    /*else if ( d_min_global_ < 5.0 )
+	    	robot_->set_v_max( max_v_by_system_ * 0.95 );*/
+	    else
+	    	robot_->set_v_max( max_v_by_system_  );
+}
+
+
+void Cplan_local_nav_person_companion::calculate_actual_angle_person_robot(unsigned int index){
+		if(debug_real_test_companion_){
+			std::cout << " !!! calculate_actual_angle_person_robot() !!!" << std::endl;
+		}
+//	double theta=atan2(person_companion_goal_.y-companion_person_position_.y , person_companion_goal_.x-companion_person_position_.x);
+       	// ini calcular nuevo angulo theta con orientación dirección movimiento persona.
+        		Cperson_abstract* person_obj;
+        		bool finded_person=find_person(id_person_companion_ , &person_obj);
+
+        	    //std::cout <<" bool finded_person="<<finded_person<<" id_person_companion_"<<id_person_companion_ << std::endl;
+
+        	    if(debug_real_test_companion2_){
+        	    	std::cout <<" bool finded_person="<<finded_person<<" id_person_companion_"<<id_person_companion_ << std::endl;
+        	    }
+
+        	    if(debug_antes_subgoals_entre_AKP_goals_){
+        	    	std::cout << " (Case: far of the person position) finded_person="<<finded_person<< std::endl;
+        	    	std::cout << " person_obj->print(), person_pose_companion:"<< std::endl;
+        	    	person_obj->print();
+        	    	std::cout << " despues print person"<< std::endl;
+        	    }
+
+        	    SpointV_cov person = person_obj->get_current_pointV();
+
+        	    if(debug_correct_angle_person_calculate_actual_angle_person_robot_){
+        	    	std::cout <<" perso_print="<< std::endl;
+        	    	person.print();
+        	    	//const std::vector<SpointV_cov> person_tracj=person_obj->get_planning_trajectory();
+        	    	std::cout <<" person_tracj.size()="<<person_obj->get_past_trajectory()->size()<< std::endl;
+        	    }
+
+        	    //std::cout <<" antes calculo theta 8"<< std::endl;
+
+        	    double theta=calc_person_companion_orientation();
+        	   // double theta_pers=theta;
+
+        	   /* double x;
+        	    double y;
+        	    if(person_obj->get_past_trajectory()->size()>num_steps_orientation_){
+        	    	if( debug_correct_angle_person_calculate_actual_angle_person_robot_){
+        	    		std::cout <<" perso_print_back_traj(size-num_steps_orientation_)="<< std::endl;
+        	    		person_obj->get_past_trajectory()->at(person_obj->get_past_trajectory()->size()-num_steps_orientation_).print();
+        	    	}
+        	    	x=person_obj->get_past_trajectory()->at(person_obj->get_past_trajectory()->size()-num_steps_orientation_).x;
+        	    	y=person_obj->get_past_trajectory()->at(person_obj->get_past_trajectory()->size()-num_steps_orientation_).y;
+        	    }else{
+        	    	if( debug_correct_angle_person_calculate_actual_angle_person_robot_){
+        	    		std::cout <<" perso_print_back_traj(0)="<< std::endl;
+        	    		person_obj->get_past_trajectory()->at(0).print();
+        	    	}
+        	    	x=person_obj->get_past_trajectory()->at(0).x;
+        	    	y=person_obj->get_past_trajectory()->at(0).y;
+        	    }
+        	    double pers_dx=person.x-x;
+        	    double pers_dy=person.y-y;
+				x=person_obj->get_best_dest().x;
+				y=person_obj->get_best_dest().y;
+				pers_dx=person.x-x;//person_obj->get_current_pointV().vx;
+				pers_dy=person.y-y;//person_obj->get_current_pointV().vy;
+        	    if( debug_correct_angle_person_calculate_actual_angle_person_robot_){
+        	    	std::cout <<" dx="<<pers_dx<<"; dy="<<pers_dy<< std::endl;
+        	    }
+        	    double theta_pers=atan2(pers_dy , pers_dx);
+        	    double theta=atan2(pers_dy , pers_dx);
+        	    */
+
+
+
+        	    if( debug_correct_angle_person_calculate_actual_angle_person_robot_){
+        	    	std::cout <<"(person) theta="<<theta*180/3.14<< std::endl;
+        	    	std::cout << " extern_robot_goal_.x-person.x="<<extern_robot_goal_.x-person.x<<"; extern_robot_goal_.y-person.y"<<extern_robot_goal_.y-person.y << std::endl;
+        	    }
+        	    // fin calcular nuevo angulo theta con orientación dirección movimiento persona.
+
+
+	 double angle=atan2(robot_initial_pose_.y-companion_person_position_.y , robot_initial_pose_.x-companion_person_position_.x);
+	 double actual_angle;
+
+	 if( diffangle(theta, angle) < 0 ){
+	 	actual_angle=theta+angle;
+	 	if(debug_real_test_companion_){
+			std::cout << "  diffangle(theta, angle)="<< diffangle(theta, angle)<< std::endl;
+			std::cout << " theta="<<(theta*180)/3.14 << " angle="<<(angle*180)/3.14<< std::endl;
+			std::cout << " actual_angle="<<(actual_angle*180)/3.14 << std::endl;
+	 	}
+	 }else{
+		 actual_angle=theta-angle;
+		 if(debug_real_test_companion_){
+			 std::cout << "  diffangle(theta, angle)="<< diffangle(theta, angle)<< std::endl;
+			 std::cout << " theta="<<(theta*180)/3.14 << " angle="<<(angle*180)/3.14<< std::endl;
+			 std::cout << " actual_angle="<<(actual_angle*180)/3.14 << std::endl;
+		 }
+	 }
+
+	 actual_angle=(actual_angle*180)/3.14;
+
+	 if((actual_angle>=0)&&( actual_angle<=180)){
+		 actual_angle=actual_angle;
+		 if(debug_real_test_companion_){
+			 std::cout << " actual_angle [0<->180] (1) actual_angle ="<<actual_angle << std::endl;
+		 }
+	 }else if((actual_angle<0)&&(actual_angle>=(-180))){
+		 actual_angle=sqrt(actual_angle*actual_angle);
+		 if(debug_real_test_companion_){
+			 std::cout << " actual_angle [0<-> -180] (1) actual_angle ="<<actual_angle << std::endl;
+		 }
+	 }else if((actual_angle>180)&&(actual_angle<=360)){
+		 actual_angle=360-actual_angle;
+		 if(debug_real_test_companion_){
+			 std::cout << " actual_angle [180<-> 360] (1) actual_angle ="<<actual_angle << std::endl;
+		 }
+	 }else if((actual_angle<(-180))&&(actual_angle>=(-360))){
+		 actual_angle=sqrt(actual_angle*actual_angle);
+		 actual_angle=360-actual_angle;
+		 if(debug_real_test_companion_){
+			 std::cout << " actual_angle [-180<-> -360] (1) actual_angle ="<<actual_angle << std::endl;
+		 }
+	 }else if(actual_angle>360){
+		 unsigned int n_360_g=actual_angle/360;
+		 actual_angle=actual_angle-n_360_g*360;
+		 if(debug_gazebo_journal2_){
+		     std::cout << "2  OJO!!! caso mayor 360 ; actual_angle=actual_angle-n_360_g*360="<<actual_angle<<"; n_360_g="<<n_360_g<< std::endl;
+		 }
+
+
+	 }else{
+		 if(debug_gazebo_journal2_){
+      	   	std::cout << " OJO!!! caso NO contemplado actual_angle="<<actual_angle<< std::endl;
+		 }
+
+	 }
+
+	 if(debug_real_test_companion_){
+	 std::cout << " min_next_companion_angle_="<<min_next_companion_angle_<< std::endl;
+	 std::cout << " orientation_person_robot_angles_[next_plan_index]="<<orientation_person_robot_angles_[index]<< std::endl;
+	 }
+
+	 double angle_diference;
+	 if(actual_angle>min_next_companion_angle_){
+		 angle_diference=actual_angle-min_next_companion_angle_;
+	 }else{
+		 angle_diference=min_next_companion_angle_-actual_angle;
+	 }
+	 double distance_robot_L=angle_diference*(3.14/180)*(robot_person_proximity_distance_-1);
+	 double V_robot_theorical=distance_robot_L/0.2;
+
+	 if(debug_real_test_companion_){
+		 std::cout << " angle_diference="<<angle_diference<<"robot_person_proximity_distance_="<<robot_person_proximity_distance_<< std::endl;
+		 std::cout << " distance_robot_L="<<distance_robot_L<< std::endl;
+		 std::cout << " V_theorical="<<V_robot_theorical<< std::endl;
+		 std::cout << " max_v_by_system_="<<max_v_by_system_<< std::endl;
+	 }
+
+}
+
+
+double Cplan_local_nav_person_companion::calculate_actual_angle_person_robot(Spoint person, Spoint robot){
+	if(debug_real_test_companion_){
+		std::cout << " !!! calculate_actual_angle_person_robot() !!!" << std::endl;
+	}
+//	double theta=atan2(person_companion_goal_.y-companion_person_position_.y , person_companion_goal_.x-companion_person_position_.x);
+   	// ini calcular nuevo angulo theta con orientación dirección movimiento persona.
+    		Cperson_abstract* person_obj;
+    		bool finded_person=find_person(id_person_companion_ , &person_obj);
+
+    	    //std::cout <<" bool finded_person="<<finded_person<<" id_person_companion_"<<id_person_companion_ << std::endl;
+
+    	    if(debug_real_test_companion2_){
+    	    	std::cout <<" bool finded_person="<<finded_person<<" id_person_companion_"<<id_person_companion_ << std::endl;
+    	    }
+
+    	    if(debug_antes_subgoals_entre_AKP_goals_){
+    	    	std::cout << " (Case: far of the person position) finded_person="<<finded_person<< std::endl;
+    	    	std::cout << " person_obj->print(), person_pose_companion:"<< std::endl;
+    	    	person_obj->print();
+    	    	std::cout << " despues print person"<< std::endl;
+    	    }
+
+    	  //  SpointV_cov person = person_obj->get_current_pointV(); // ojo, lo mismo para la orientacion de la persona SOLO he de usar el punto actual de la persona que acompaña
+
+    	    if(debug_correct_angle_person_calculate_actual_angle_person_robot_){
+    	    	std::cout <<" perso_print="<< std::endl;
+    	    	person.print();
+    	    	//const std::vector<SpointV_cov> person_tracj=person_obj->get_planning_trajectory();
+    	    	std::cout <<" person_tracj.size()="<<person_obj->get_past_trajectory()->size()<< std::endl;
+    	    }
+    	   // std::cout <<" antes calculo theta 9"<< std::endl;
+
+    	    double theta=calc_person_companion_orientation();
+    	   // double theta_pers=theta;
+
+    	    /*double x;
+    	    double y;
+    	    if(person_obj->get_past_trajectory()->size()>num_steps_orientation_){
+    	    	if( debug_correct_angle_person_calculate_actual_angle_person_robot_){
+    	    		std::cout <<" perso_print_back_traj(size-num_steps_orientation_)="<< std::endl;
+    	    		person_obj->get_past_trajectory()->at(person_obj->get_past_trajectory()->size()-num_steps_orientation_).print();
+    	    	}
+    	    	x=person_obj->get_past_trajectory()->at(person_obj->get_past_trajectory()->size()-num_steps_orientation_).x;
+    	    	y=person_obj->get_past_trajectory()->at(person_obj->get_past_trajectory()->size()-num_steps_orientation_).y;
+    	    }else{
+    	    	if( debug_correct_angle_person_calculate_actual_angle_person_robot_){
+    	    		std::cout <<" perso_print_back_traj(0)="<< std::endl;
+    	    		person_obj->get_past_trajectory()->at(0).print();
+    	    	}
+    	    	x=person_obj->get_past_trajectory()->at(0).x;
+    	    	y=person_obj->get_past_trajectory()->at(0).y;
+    	    }
+    	    double pers_dx=person.x-x;
+    	    double pers_dy=person.y-y;
+
+			x=person_obj->get_best_dest().x;
+			y=person_obj->get_best_dest().y;
+			pers_dx=person.x-x;//person_obj->get_current_pointV().vx;
+			pers_dy=person.y-y;//person_obj->get_current_pointV().vy;
+    	    if( debug_correct_angle_person_calculate_actual_angle_person_robot_){
+    	    	std::cout <<" dx="<<pers_dx<<"; dy="<<pers_dy<< std::endl;
+    	    }
+    	    //double theta_pers=atan2(pers_dy , pers_dx);
+    	    double theta=atan2(pers_dy , pers_dx);
+    	    */
+
+
+    	    if( debug_correct_angle_person_calculate_actual_angle_person_robot_){
+    	    	//std::cout <<" theta_pers="<<theta_pers*180/3.14<< std::endl;
+    	    	std::cout << " extern_robot_goal_.x-person.x="<<extern_robot_goal_.x-person.x<<"; extern_robot_goal_.y-person.y"<<extern_robot_goal_.y-person.y << std::endl;
+    	    }
+    	    // fin calcular nuevo angulo theta con orientación dirección movimiento persona.
+
+
+	 double angle=atan2(robot.y-person.y , robot.x-person.x);
+	 double actual_angle;
+
+	 if( diffangle(theta, angle) < 0 ){
+		actual_angle=theta+angle;
+		if(debug_real_test_companion_){
+			std::cout << "  diffangle(theta, angle)="<< diffangle(theta, angle)<< std::endl;
+			std::cout << " theta="<<(theta*180)/3.14 << " angle="<<(angle*180)/3.14<< std::endl;
+			std::cout << " actual_angle="<<(actual_angle*180)/3.14 << std::endl;
+		}
+	 }else{
+		 actual_angle=theta-angle;
+		 if(debug_real_test_companion_){
+			 std::cout << "  diffangle(theta, angle)="<< diffangle(theta, angle)<< std::endl;
+			 std::cout << " theta="<<(theta*180)/3.14 << " angle="<<(angle*180)/3.14<< std::endl;
+			 std::cout << " actual_angle="<<(actual_angle*180)/3.14 << std::endl;
+		 }
+	 }
+
+	 actual_angle=(actual_angle*180)/3.14;
+
+	 if((actual_angle>=0)&&( actual_angle<=180)){
+		 actual_angle=actual_angle;
+		 if(debug_real_test_companion_){
+			 std::cout << " actual_angle [0<->180] (1) actual_angle ="<<actual_angle << std::endl;
+		 }
+	 }else if((actual_angle<0)&&(actual_angle>=(-180))){
+		 actual_angle=sqrt(actual_angle*actual_angle);
+		 if(debug_real_test_companion_){
+			 std::cout << " actual_angle [0<-> -180] (1) actual_angle ="<<actual_angle << std::endl;
+		 }
+	 }else if((actual_angle>180)&&(actual_angle<=360)){
+		 actual_angle=360-actual_angle;
+		 if(debug_real_test_companion_){
+			 std::cout << " actual_angle [180<-> 360] (1) actual_angle ="<<actual_angle << std::endl;
+		 }
+	 }else if((actual_angle<(-180))&&(actual_angle>=(-360))){
+		 actual_angle=sqrt(actual_angle*actual_angle);
+		 actual_angle=360-actual_angle;
+		 if(debug_real_test_companion_){
+			 std::cout << " actual_angle [-180<-> -360] (1) actual_angle ="<<actual_angle << std::endl;
+		 }
+	 }else if(actual_angle>360){
+		 unsigned int n_360_g=actual_angle/360;
+		 actual_angle=actual_angle-n_360_g*360;
+		 if(debug_real_test_companion_){
+		     std::cout << " 1 OJO!!! caso mayor 360 ; actual_angle=actual_angle-n_360_g*360="<<actual_angle<<"; n_360_g="<<n_360_g<< std::endl;
+		 }
+
+	 }else{
+					std::cout << " OJO!!! caso NO contemplado actual_angle="<<actual_angle<< std::endl;
+	 }
+
+	/* if(debug_real_test_companion_){
+		 std::cout << " min_next_companion_angle_="<<min_next_companion_angle_<< std::endl;
+		 std::cout << " orientation_person_robot_angles_[next_plan_index]="<<orientation_person_robot_angles_[index]<< std::endl;
+	 }*/
+
+	/* double angle_diference;
+	 if(actual_angle>min_next_companion_angle_){
+		 angle_diference=actual_angle-min_next_companion_angle_;
+	 }else{
+		 angle_diference=min_next_companion_angle_-actual_angle;
+	 }
+	 double distance_robot_L=angle_diference*(3.14/180)*(robot_person_proximity_distance_-1);
+	 double V_robot_theorical=distance_robot_L/0.2;
+
+	 if(debug_real_test_companion_){
+		 std::cout << " angle_diference="<<angle_diference<<"robot_person_proximity_distance_="<<robot_person_proximity_distance_<< std::endl;
+		 std::cout << " distance_robot_L="<<distance_robot_L<< std::endl;
+		 std::cout << " V_theorical="<<V_robot_theorical<< std::endl;
+		 std::cout << " max_v_by_system_="<<max_v_by_system_<< std::endl;
+	 }*/
+
+	 return actual_angle;
+
+}
+
+
+
+double Cplan_local_nav_person_companion::calculate_anisotropy( const SpointV_cov& center_person, const Spoint& interacting_person , double lambda )
+{
+	//setting the corresponding SFM parameters:
+	// if no forces param provided, exit program
+	// center_person == robot_pose
+	// interacting_person == obstacle (dynamic or static)
+	// lambda=>social_forces_param->at(1) =>  const std::vector<double>* social_forces_param =>
+
+	// if anisotropy < 0.5, ese objeto no ha de influir en los obstaculos, min distance colision, para posicionamiento del robot.
+	lambda=0.2;// provisional.
+
+	double d;
+	//geometry calculations
+	double dx = center_person.x - interacting_person.x;
+	double dy = center_person.y - interacting_person.y;
+	double vx = center_person.vx;
+	double vy = center_person.vy;
+	d = sqrt(dx*dx+dy*dy);
+	dx /= d;
+	dy /= d;
+
+	//force direction
+	double phi = diffangle( atan2(vy,vx), atan2(-dy,-dx) );//minus difference vector
+	double anisotropy = (lambda + (1-lambda)*(1 + cos(phi))/2 );
+
+	return anisotropy;
+}
+
+
+
+
+double Cplan_local_nav_person_companion::calculate_modif_person_robot_distance( double ite )
+{
+/* */
+	double modif_person_robot_distance=robot_person_proximity_distance_;
+
+	if(overpas_obstacles_behind_person_){
+		//modif_person_robot_distance=robot_person_proximity_distance_;
+
+		if(debug_cout_robot_){
+			std::cout << " if(overpas_obstacles_behind_person_)=>(1) ite="<<ite<< std::endl;
+		}
+		if(debug_file_robot_){
+			std::ofstream fileMatlab2;
+			fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+			fileMatlab2 << " if(overpas_obstacles_behind_person_)=>(1) ite=\n";
+			fileMatlab2.close();
+		}
+
+		if((ite>=0)&&(ite<=90)){
+			ite=180-ite;
+			if(debug_cout_robot_){
+				std::cout << " diff_angle [0<->90] (1) ite ="<<ite << std::endl;
+			}
+			if(debug_file_robot_){
+				std::ofstream fileMatlab2;
+				fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+				fileMatlab2 << " diff_angle [0<->90] (1) ite ="<<ite <<"\n";
+				fileMatlab2.close();
+			}
+		}else if((ite>90)&&(ite<=180)){
+			ite=ite;
+			if(debug_cout_robot_){
+				std::cout << " diff_angle [90<->180] (1) ite ="<<ite << std::endl;
+			}
+			if(debug_file_robot_){
+				std::ofstream fileMatlab2;
+				fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+				fileMatlab2 << " diff_angle [90<->180] (1) ite ="<<ite <<"\n";
+				fileMatlab2.close();
+			}
+		}else if((ite < 0)&&(ite>=(-90))){
+			ite=sqrt(ite*ite);
+			ite=180-ite;
+			//ini_angle_act=sqrt(ini_angle_act*ini_angle_act);
+			//ini_angle_act=360-ini_angle_act;
+			if(debug_cout_robot_){
+				std::cout << " diff_angle [0<-> -90] (1) ite ="<<ite << std::endl;
+			}
+			if(debug_file_robot_){
+				std::ofstream fileMatlab2;
+				fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+				fileMatlab2 << " diff_angle [0<-> -90] (1) ite ="<<ite <<"\n";
+				fileMatlab2.close();
+			}
+
+		}else if((ite < -90)&&(ite>=(-180))){
+			ite=sqrt(ite*ite);
+			if(debug_cout_robot_){
+				std::cout << " diff_angle [-90<->-180] (1) ite ="<<ite << std::endl;
+			}
+
+			if(debug_file_robot_){
+				std::ofstream fileMatlab2;
+				fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+				fileMatlab2 << " diff_angle [-90<->-180] (1) ite ="<<ite <<"\n";
+				fileMatlab2.close();
+			}
+		}else if((ite>180)&&(ite<=270)){
+			ite=ite-180;
+			ite=180-ite;
+			//ini_angle_act=360-ini_angle_act;
+			if(debug_cout_robot_){
+				std::cout << " diff_angle [180<-> 360] (1) ite ="<<ite << std::endl;
+			}
+
+			if(debug_file_robot_){
+				std::ofstream fileMatlab2;
+				fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+				fileMatlab2 << " diff_angle [180<-> 360] (1) ite="<<ite <<"\n";
+				fileMatlab2.close();
+			}
+		}else if((ite>270)&&(ite<=360)){
+			//ite=sqrt(((ite-270)-90)*((ite-270)-90));
+			ite=360-ite;
+			//ite=ite-180;
+			ite=180-ite;
+			if(debug_cout_robot_){
+				std::cout << " diff_angle [180<-> 360] (1) ite ="<<ite << std::endl;
+			}
+			if(debug_file_robot_){
+				std::ofstream fileMatlab2;
+				fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+				fileMatlab2 << " diff_angle [180<-> 360] (1) ite ="<<ite <<"\n";
+				fileMatlab2.close();
+			}
+		}
+		else if((ite < ( -180))&&(ite>=(-270))){
+			ite=sqrt(ite*ite);
+			ite=ite-180;
+			ite=180-ite;
+			if(debug_cout_robot_){
+				std::cout << " diff_angle [-180<-> -360] (1) ite ="<<ite << std::endl;
+			}
+			if(debug_file_robot_){
+				std::ofstream fileMatlab2;
+				fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+				fileMatlab2 << " diff_angle [-180<-> -360] (1) ite="<<ite <<"\n";
+				fileMatlab2.close();
+			}
+		}else if((ite < ( -270))&&(ite>=(-360))){
+			ite=sqrt(ite*ite);
+			ite=360-ite;
+			//ite=ite-180;
+			ite=180-ite;
+			if(debug_cout_robot_){
+				std::cout << " diff_angle [-180<-> -360] (1) ite ="<<ite << std::endl;
+			}
+			if(debug_file_robot_){
+				std::ofstream fileMatlab2;
+				fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+				fileMatlab2 << " diff_angle [-180<-> -360] (1) ite ="<<ite <<"\n";
+				fileMatlab2.close();
+			}
+		}
+		else if(ite>360){
+			 unsigned int n_360_g=ite/360;
+			 ite=ite-n_360_g*360;
+		     std::cout << " OJO!!! caso mayor 360 ; ite=ite-n_360_g*360="<<ite<<"; n_360_g="<<n_360_g<< std::endl;
+
+		}else{
+			std::cout << " OJO!!! caso NO contemplado ite="<<ite<< std::endl;
+		}
+
+
+		//modif_person_robot_distance=(0.00024679)*pow((ite-135),2)+(robot_person_proximity_distance_-0.5); //pow(angle-angle_companion_,4)
+		//modif_person_robot_distance=(0.00024679)*(0.5-pow((ite-135),2))+(robot_person_proximity_distance_+1-0.5); //pow(angle-angle_companion_,4)
+
+		//modif_person_robot_distance=((0.00024679)*pow((ite-135),2))+(robot_person_proximity_distance_-0.5); //pow(angle-angle_companion_,4)
+
+		//modif_person_robot_distance=((0.5-(-0.010)*(ite-135)))+1.5;
+		//modif_person_robot_distance=((0.5-(-0.006)*(ite-135)))+2.5;//(0.00024679)*pow((ite-135),2)+(person_robot_actual_real_distance_-0.5); //pow(angle-angle_companion_,4)
+		//modif_person_robot_distance=(((-0.010)*(ite-135)))+1;
+		modif_person_robot_distance=(((-0.005)*(ite-135)))+0.75;
+
+		if(debug_cout_robot_){
+			std::cout << "ite="<<ite<<"; (real_distance) person_robot_actual_real_distance_="<<person_robot_actual_real_distance_<< std::endl;
+			std::cout << "(((-0.010)*(ite-135)))+1="<<(((-0.010)*(ite-135)))+1<<"; robot_person_proximity_distance_="<<robot_person_proximity_distance_<< std::endl;
+			std::cout << "modif_person_robot_distance=(0.00024679)*pow((ite-135),2)+(robot_person_proximity_distance_-0.5)="<<modif_person_robot_distance<< std::endl;
+		}
+		if(debug_file_robot_){
+			std::ofstream fileMatlab2;
+				fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+				fileMatlab2 << "ite="<<ite<<"; (real_distance) person_robot_actual_real_distance_="<<person_robot_actual_real_distance_<< "\n";
+				fileMatlab2 << "(((-0.010)*(ite-135)))+1="<<(((-0.010)*(ite-135)))+1<<"; robot_person_proximity_distance_="<<robot_person_proximity_distance_<< "\n";
+				fileMatlab2 << "modif_person_robot_distance=(0.00024679)*pow((ite-135),2)+(robot_person_proximity_distance_-0.5)="<<modif_person_robot_distance<< "\n";
+				fileMatlab2.close();
+		}
+
+	}else{
+		/*double ite;
+		if(min_next_companion_angle_!=angle_companion_){
+			ite=(theta+(min_next_companion_angle_*3.14/180))*(180/3.14);
+		}else{
+			ite=min_next_companion_angle_;
+		}*/
+		if(debug_real_test_companion_){
+			std::cout << "(1) ite="<<ite<< std::endl;
+		}
+
+		if((ite>=0)&&(ite<=90)){
+			ite=ite;
+			if(debug_real_test_companion_){
+				std::cout << " diff_angle [0<->90] (1) ite ="<<ite << std::endl;
+			}
+		}else if((ite>90)&&(ite<=180)){
+			ite=sqrt((ite-180)*(ite-180));
+			if(debug_real_test_companion_){
+				std::cout << " diff_angle [90<->180] (1) ite ="<<ite << std::endl;
+			}
+		}else if((ite < 0)&&(ite>=(-90))){
+			ite=sqrt(ite*ite);
+			if(debug_real_test_companion_){
+				std::cout << " diff_angle [0<-> -90] (1) ite ="<<ite << std::endl;
+			}
+		}else if((ite < -90)&&(ite>=(-180))){
+			ite=sqrt(ite*ite);
+			ite=sqrt((ite-180)*(ite-180));
+			if(debug_real_test_companion_){
+				std::cout << " diff_angle [-90<->-180] (1) ite ="<<ite << std::endl;
+			}
+		}else if((ite>180)&&(ite<=270)){
+			ite=ite-180;
+			if(debug_real_test_companion_){
+				std::cout << " diff_angle [180<-> 360] (1) ite ="<<ite << std::endl;
+			}
+		}else if((ite>270)&&(ite<=360)){
+			ite=sqrt(((ite-270)-90)*((ite-270)-90));
+			if(debug_real_test_companion_){
+				std::cout << " diff_angle [180<-> 360] (1) ite ="<<ite << std::endl;
+			}
+		}
+		else if((ite < ( -180))&&(ite>=(-270))){
+			ite=sqrt(ite*ite);
+			ite=ite-180;
+			if(debug_real_test_companion_){
+				std::cout << " diff_angle [-180<-> -360] (1) ite ="<<ite << std::endl;
+			}
+		}else if((ite < ( -270))&&(ite>=(-360))){
+			ite=sqrt(ite*ite);
+			ite=sqrt(((ite-270)-90)*((ite-270)-90));
+			if(debug_real_test_companion_){
+				std::cout << " diff_angle [-180<-> -360] (1) ite ="<<ite << std::endl;
+			}
+		}else if(ite>360){
+			 unsigned int n_360_g=ite/360;
+			 ite=ite-n_360_g*360;
+		     std::cout << " OJO!!! caso mayor 360 ; ite=ite-n_360_g*360="<<ite<<"; n_360_g="<<n_360_g<< std::endl;
+		}
+		else{
+			 std::cout << " OJO!!! caso NO contemplado ite="<<ite<< std::endl;
+		}
+
+
+		modif_person_robot_distance=(0.00024679)*pow((ite-45),2)+(robot_person_proximity_distance_-0.5); //pow(angle-angle_companion_,4)
+
+		if(debug_real_test_companion_){
+			std::cout << "ite="<<ite<< std::endl;
+			std::cout << "(0.00024679)*pow((ite-45),2)="<<(0.00024679)*pow((ite-45),2)<< std::endl;
+			std::cout << "modif_person_robot_distance="<<modif_person_robot_distance<< std::endl;
+		}
+	}
+
+	return modif_person_robot_distance;
+}
+
+
+
+//////////////////////////////
+void Cplan_local_nav_person_companion::printToMatlab()
+{
+
+   //	std::cout <<" IN printToMatlab() !!!"<< std::endl;
+
+	std::ofstream fileMatlab;
+	fileMatlab.open (results_file_.c_str(), std::ofstream::out | std::ofstream::app);
+    //fileMatlab << " clc,\n clear all,\n close all \n\n\n";
+	ideal_distance_person_robot_=robot_person_proximity_distance_;
+
+	Cperson_abstract* person_obj;
+	bool finded_person=find_person(id_person_companion_ , &person_obj);
+
+	//double time=person_obj->get_time();
+	double time=ros_time_to_sec_;
+	actual_time_=time;
+	SpointV_cov person=person_obj->get_current_pointV();
+
+
+
+	/*if(real_angle_person_robot_<100){ // todo: provisional, ja que al pasarme de 180 guardo mal el real angle sin querer, hago mal el cambio de grados.
+		real_angle_person_robot_=180;
+	}*/
+
+    // Algorithm Params
+	fileMatlab << " \n \n \n % New iteration! \n";
+	fileMatlab << "\n real_distance_person_robot("<<experiment_<<","<<iteration_<<")="<<real_distance_person_robot_<<";\n";
+	fileMatlab << " ideal_distance_person_robot("<<experiment_<<","<<iteration_<<")="<<ideal_distance_person_robot_<<";\n";
+	fileMatlab << " real_angle_person_robot("<<experiment_<<","<<iteration_<<")="<<real_angle_person_robot_<<";\n";
+	fileMatlab << " before_bad_saved_ideal_angle_person_robot("<<experiment_<<","<<iteration_<<")="<<ideal_angle_person_robot_<<";\n";
+	// To avoid jumps in ideal angle that are not true and good for the final results.
+	if(final_debug_journal_){
+		std::cout <<" (antes_first_ideal_angle) ·3333333 ideal_angle_person_robot_="<<ideal_angle_person_robot_<<" before_ideal_angle_person_robot_"<<before_ideal_angle_person_robot_ << std::endl;
+	}
+	if(first_ideal_angle_){
+			before_ideal_angle_person_robot_=90;
+			first_ideal_angle_=false;
+		}
+		if(final_debug_journal_){
+			std::cout <<" ·3333333 ideal_angle_person_robot_="<<ideal_angle_person_robot_<<" before_ideal_angle_person_robot_"<<before_ideal_angle_person_robot_ << std::endl;
+		}
+		if(sqrt((before_ideal_angle_person_robot_-ideal_angle_person_robot_)*(before_ideal_angle_person_robot_-ideal_angle_person_robot_))> 10){
+			if((before_ideal_angle_person_robot_-ideal_angle_person_robot_)<0){
+				ideal_angle_person_robot_=before_ideal_angle_person_robot_+10;
+			}else{
+				ideal_angle_person_robot_=before_ideal_angle_person_robot_-10;
+			}
+		}
+		if(final_debug_journal_){
+			std::cout <<" ·3333333 ideal_angle_person_robot_="<<ideal_angle_person_robot_<<" before_ideal_angle_person_robot_"<<before_ideal_angle_person_robot_ << std::endl;
+			std::cout <<" ·3333333 angle_increment_of_increment_distance_="<<angle_increment_of_increment_distance_ << std::endl;
+		}
+
+		before_ideal_angle_person_robot_=ideal_angle_person_robot_;
+		fileMatlab << " ideal_angle_person_robot("<<experiment_<<","<<iteration_<<")="<<ideal_angle_person_robot_<<";\n";
+
+
+
+	fileMatlab << "\n robot_pose_x("<<experiment_<<","<<iteration_<<")="<<robot_initial_pose_.x<<";\n";
+	fileMatlab << "robot_pose_y("<<experiment_<<","<<iteration_<<")="<<robot_initial_pose_.y<<";\n";
+	fileMatlab << "robot_pose_Vx("<<experiment_<<","<<iteration_<<")="<<initial_robot_spoint_.vx<<";\n";
+	fileMatlab << "robot_pose_Vy("<<experiment_<<","<<iteration_<<")="<<initial_robot_spoint_.vy<<";\n";
+	fileMatlab << "robot_pose_V("<<experiment_<<","<<iteration_<<")="<<robot_initial_pose_.v<<";\n";
+	fileMatlab << "robot_pose_W("<<experiment_<<","<<iteration_<<")="<<robot_initial_pose_.w<<";\n";
+	fileMatlab << "robot_pose_theta("<<experiment_<<","<<iteration_<<")="<<robot_initial_pose_.theta*180/3.14<<";\n";
+	fileMatlab << "robot_pose_time_stamp("<<experiment_<<","<<iteration_<<")="<<robot_initial_pose_.time_stamp<<";\n";
+
+	if(debug_gazebo_journal_){
+		std::cout <<" robot_pose_x="<<robot_initial_pose_.x<<"; robot_pose_y"<<robot_initial_pose_.y<<"; robot.theta"<<robot_initial_pose_.theta*180/3.14 << std::endl;
+		std::cout <<" robot_pose_vx="<<initial_robot_spoint_.vx<<"; robot_pose_vy"<<initial_robot_spoint_.vy << std::endl;
+
+	}
+
+
+	fileMatlab <<"\n";
+
+	fileMatlab << "\n last_pose_command_x("<<experiment_<<","<<iteration_<<")="<<last_pose_command_.x<<";\n";
+	fileMatlab << "last_pose_command_y("<<experiment_<<","<<iteration_<<")="<<last_pose_command_.y<<";\n";
+	fileMatlab << "last_pose_command_v("<<experiment_<<","<<iteration_<<")="<<last_pose_command_.v<<";\n";
+	fileMatlab << "last_pose_command_w("<<experiment_<<","<<iteration_<<")="<<last_pose_command_.w<<";\n";
+	fileMatlab << "last_pose_command_theta("<<experiment_<<","<<iteration_<<")="<<last_pose_command_.theta*180/3.14<<";\n";
+	fileMatlab << "last_pose_command_time_stamp("<<experiment_<<","<<iteration_<<")="<<last_pose_command_.time_stamp<<";\n";
+
+
+	//fileMatlab << last_pose_command_.x<<" "<<last_pose_command_.y<<" "<<last_pose_command_.time_stamp<<" "<<last_pose_command_.theta<<" "<<last_pose_command_.v<<" "<<last_pose_command_.w<<"\n";
+	if(debug_gazebo_journal_){
+	std::cout <<" last_pose_command_.x="<<last_pose_command_.x<<"; last_pose_command_.y="<<last_pose_command_.y<<"; last_pose_command_.time_stamp="<<last_pose_command_.time_stamp<<"; last_pose_command_.theta="<<last_pose_command_.theta<<"; last_pose_command_.v="<<last_pose_command_.v<<"; last_pose_command_.w="<<last_pose_command_.w<< "\n";
+	}
+
+	fileMatlab << "\n is_case_akp_true_("<<experiment_<<","<<iteration_<<")="<<is_case_akp_true_<<";\n";
+	fileMatlab << "\n enter_on_cero_("<<experiment_<<","<<iteration_<<")="<<enter_on_cero_<<";\n";
+	fileMatlab << "\n enter_on_cero2_("<<experiment_<<","<<iteration_<<")="<<enter_on_cero2_<<";\n";
+	fileMatlab << "\n result_("<<experiment_<<","<<iteration_<<")="<<result_<<";\n";
+	fileMatlab << "\n dist_to_goal_test_("<<experiment_<<","<<iteration_<<")="<<dist_to_goal_test_<<";\n";
+	fileMatlab << "\n v_to_goal_test_("<<experiment_<<","<<iteration_<<")="<<v_to_goal_test_<<";\n";
+	Sdestination person_goal;
+	if(companion_same_person_goal_){
+		person_goal=person_obj->get_best_dest();
+		//std::cout << " (companion_same_person_goal_) person_obj->get_best_dest().x ="<<person_obj->get_best_dest().x <<"; person_obj->get_best_dest().y="<<person_obj->get_best_dest().y<< std::endl;
+	}else{
+		person_goal=extern_robot_goal_;
+	}
+	//double theta = atan2(person_goal.y-companion_person_position_.y,person_goal.x-companion_person_position_.x);
+   	// ini calcular nuevo angulo theta con orientación dirección movimiento persona.
+    		//Cperson_abstract* person_obj;
+    		//bool finded_person=find_person(id_person_companion_ , &person_obj);
+
+    	    //std::cout <<" bool finded_person="<<finded_person<<" id_person_companion_"<<id_person_companion_ << std::endl;
+
+    	    if(debug_real_test_companion2_){
+    	    	std::cout <<" bool finded_person="<<finded_person<<" id_person_companion_"<<id_person_companion_ << std::endl;
+    	    }
+
+    	    if(debug_antes_subgoals_entre_AKP_goals_){
+    	    	std::cout << " (Case: far of the person position) finded_person="<<finded_person<< std::endl;
+    	    	std::cout << " person_obj->print(), person_pose_companion:"<< std::endl;
+    	    	person_obj->print();
+    	    	std::cout << " despues print person"<< std::endl;
+    	    }
+
+    	    //SpointV_cov person = person_obj->get_current_pointV();
+    	    if(debug_correct_angle_person_print_to_matlab_){
+    	    	std::cout <<" perso_print="<< std::endl;
+    	    	person.print();
+    	    }
+    	    //const std::vector<SpointV_cov> person_tracj=person_obj->get_planning_trajectory();
+    	    if(debug_correct_angle_person_print_to_matlab_){std::cout <<" person_tracj.size()="<<person_obj->get_past_trajectory()->size()<< std::endl;}
+
+    	    //Cperson_abstract* person_obj;
+    	    //bool finded_person=find_person(id_person_companion_ , &person_obj);
+    	    //SpointV_cov person=person_obj->get_current_pointV();
+
+    	    double theta=calc_person_companion_orientation()*180/3.14;
+    	    //double theta_pers=theta*3.14/180;
+
+    	   /* double x;
+    	    double y;
+    	    double pers_dx;
+    	    double pers_dy;
+    	    double theta_pers;
+    	    double theta;
+    	    if(person_obj->get_past_trajectory()->size()>num_steps_orientation_){
+    	    	  if(debug_correct_angle_person_print_to_matlab_){std::cout <<" perso_print_back_traj(size-num_steps_orientation_)="<< std::endl;
+    	    	person_obj->get_past_trajectory()->at(person_obj->get_past_trajectory()->size()-num_steps_orientation_).print();}
+    	    	x=person_obj->get_past_trajectory()->at(person_obj->get_past_trajectory()->size()-num_steps_orientation_).x;
+    	    	y=person_obj->get_past_trajectory()->at(person_obj->get_past_trajectory()->size()-num_steps_orientation_).y;
+    	    }else{
+    	    	  if(debug_correct_angle_person_print_to_matlab_){std::cout <<" perso_print_back_traj(0)="<< std::endl;
+    	    	person_obj->get_past_trajectory()->at(0).print();}
+    	    	x=person_obj->get_past_trajectory()->at(0).x;
+    	    	y=person_obj->get_past_trajectory()->at(0).y;
+    	    }
+    	    pers_dx=person.x-x;
+    	    pers_dy=person.y-y;
+			x=person_obj->get_best_dest().x;
+			y=person_obj->get_best_dest().y;
+			pers_dx=person.x-x;//person_obj->get_current_pointV().vx;
+			pers_dy=person.y-y;//person_obj->get_current_pointV().vy;
+    	    if(debug_correct_angle_person_print_to_matlab_){std::cout <<" dx="<<pers_dx<<"; dy="<<pers_dy<< std::endl;}
+    	    theta_pers=atan2(pers_dy , pers_dx);
+    	    theta=atan2(pers_dy , pers_dx)*180/3.14;
+*/
+
+
+    	    if(debug_correct_angle_person_print_to_matlab_){ std::cout <<"(person) theta="<<theta*180/3.14<< std::endl;
+    	    std::cout << " extern_robot_goal_.x-person.x="<<extern_robot_goal_.x-person.x<<"; extern_robot_goal_.y-person.y"<<extern_robot_goal_.y-person.y << std::endl;}
+    	    // fin calcular nuevo angulo theta con orientación dirección movimiento persona.
+	fileMatlab << "\n person_pose_x("<<experiment_<<","<<iteration_<<")="<<companion_person_position_.x<<";\n";
+	fileMatlab << "person_pose_y("<<experiment_<<","<<iteration_<<")="<<companion_person_position_.y<<";\n";
+	fileMatlab << "person_pose_Vx("<<experiment_<<","<<iteration_<<")="<<companion_person_position_.vx<<";\n";
+	fileMatlab << "person_pose_Vy("<<experiment_<<","<<iteration_<<")="<<companion_person_position_.vy<<";\n";
+	fileMatlab << "person_pose_V("<<experiment_<<","<<iteration_<<")="<<sqrt(companion_person_position_.vx*companion_person_position_.vx+companion_person_position_.vy*companion_person_position_.vy)<<";\n";
+	fileMatlab << "person_pose_theta("<<experiment_<<","<<iteration_<<")="<<theta<<";\n";
+	fileMatlab << "person_pose_time_stamp("<<experiment_<<","<<iteration_<<")="<<companion_person_position_.time_stamp<<";\n";
+
+	if(debug_gazebo_journal_){
+		std::cout <<" (companion) person_pose_x="<<companion_person_position_.x<<"; person_pose_y"<<companion_person_position_.y<<"; person.theta"<<theta << std::endl;
+		std::cout <<" (companion) person_pose_vx="<<companion_person_position_.vx<<"; person_pose_vy"<<companion_person_position_.vy << std::endl;
+
+	}
+
+
+	double inc_distance_act;
+	double step_inc_dist_act;
+	double inc_time_act;
+
+	if((before_initial_robot_spoint_.x==-10000)&&(before_initial_robot_spoint_.y==-10000)){
+		inc_distance_act=0;
+		step_inc_dist_act=0;
+		acum_time_=0;
+	}else{
+		step_inc_dist_act=sqrt((before_initial_robot_spoint_.x-initial_robot_spoint_.x)*(before_initial_robot_spoint_.x-initial_robot_spoint_.x)+(before_initial_robot_spoint_.y-initial_robot_spoint_.y)*(before_initial_robot_spoint_.y-initial_robot_spoint_.y));
+		inc_distance_act=inc_distance_ant_+step_inc_dist_act;
+		inc_distance_ant_=inc_distance_ant_+step_inc_dist_act;
+
+		inc_time_act=sqrt((actual_time_-time_ant_)*(actual_time_-time_ant_));
+		acum_time_=acum_time_+inc_time_act;
+	}
+
+
+	fileMatlab << "\n step_recorred_distance_of_the_robot("<<experiment_<<","<<iteration_<<")="<<step_inc_dist_act<<";\n";
+	fileMatlab << "\n recorred_distance_of_the_robot("<<experiment_<<","<<iteration_<<")="<<inc_distance_act<<";\n";
+	fileMatlab << " actual_time("<<experiment_<<","<<iteration_<<")="<<actual_time_<<";\n";
+	//fileMatlab << "\n time=("<<experiment_<<","<<iteration_<<")="<<real_distance_person_robot_<<"];\n";
+	fileMatlab << "\n acum_time("<<experiment_<<","<<iteration_<<")="<<acum_time_<<";\n";
+
+	// std::cout << " print to matlab (6) ideal_angle_person_robot_="<<ideal_angle_person_robot_<< std::endl;
+	 double percent_of_error_in_distance;
+	if(ideal_angle_person_robot_!=0){
+		percent_of_error_in_distance=(abs(real_angle_person_robot_-ideal_angle_person_robot_)/abs(ideal_angle_person_robot_))*100;
+	}else{
+		percent_of_error_in_distance=-1;
+	}
+	 //std::cout << " print to matlab (7)"<< std::endl;
+	fileMatlab << " percent_of_error_in_distance("<<experiment_<<","<<iteration_<<")="<<percent_of_error_in_distance<<";\n";
+
+/*
+	fileMatlab << " robot_distance_cost=("<<experiment_<<","<<iteration_<<")="<<robot_distance_cost_<<"];\n";
+	fileMatlab << " robot_orientation_cost=("<<experiment_<<","<<iteration_<<")="<<robot_orientation_cost_<<"];\n";
+	fileMatlab << " robot_control_cost=("<<experiment_<<","<<iteration_<<")="<<robot_control_cost_<<"];\n";
+	fileMatlab << " robot_other_person_cost=("<<experiment_<<","<<iteration_<<")="<<robot_other_person_cost_<<"];\n";
+	fileMatlab << " robot_obstacles_cost=("<<experiment_<<","<<iteration_<<")="<<robot_obstacles_cost_<<"];\n";
+	fileMatlab << " robot_companion_cost=("<<experiment_<<","<<iteration_<<")="<<robot_companion_cost_<<"];\n";
+	fileMatlab << " robot_total_cost=("<<experiment_<<","<<iteration_<<")="<<robot_total_cost_<<"];\n";
+	fileMatlab << " robot_ant_traj_cost=("<<experiment_<<","<<iteration_<<")="<<robot_ant_traj_cost_<<"];\n";
+	fileMatlab << " cost_parameters_0=("<<experiment_<<","<<iteration_<<")="<<cost_parameters_[0]<<"];\n";
+	fileMatlab << " cost_parameters_1=("<<experiment_<<","<<iteration_<<")="<<cost_parameters_[1]<<"];\n";
+	fileMatlab << " cost_parameters_2=("<<experiment_<<","<<iteration_<<")="<<cost_parameters_[2]<<"];\n";
+	fileMatlab << " cost_parameters_3=("<<experiment_<<","<<iteration_<<")="<<cost_parameters_[3]<<"];\n";
+	fileMatlab << " cost_parameters_5=("<<experiment_<<","<<iteration_<<")="<<cost_parameters_[5]<<"];\n";
+	fileMatlab << " cost_parameters_6=("<<experiment_<<","<<iteration_<<")="<<cost_parameters_[6]<<"];\n";
+*/
+	 //std::cout << " print to matlab (8)"<< std::endl;
+	person_distance_to_goal_=sqrt((person.x-extern_robot_goal_.x)*(person.x-extern_robot_goal_.x)+(person.y-extern_robot_goal_.y)*(person.y-extern_robot_goal_.y));
+	 //std::cout << " print to matlab (9)"<< std::endl;
+	robot_distance_to_goal_=sqrt((initial_robot_spoint_.x-extern_robot_goal_.x)*(initial_robot_spoint_.x-extern_robot_goal_.x)+(initial_robot_spoint_.y-extern_robot_goal_.y)*(initial_robot_spoint_.y-extern_robot_goal_.y));
+	 //std::cout << " print to matlab (10)"<< std::endl;
+	fileMatlab << " person_distance_to_goal("<<experiment_<<","<<iteration_<<")="<<person_distance_to_goal_<<";\n";
+	fileMatlab << " robot_distance_to_goal("<<experiment_<<","<<iteration_<<")="<<robot_distance_to_goal_<<";\n";
+	 //std::cout << " print to matlab (11)"<< std::endl;
+	double performance=0;
+
+	if((real_distance_person_robot_<2.5)&&(real_distance_person_robot_>1.0)){
+		if((real_angle_person_robot_<(ideal_angle_person_robot_+15))&&(real_angle_person_robot_>(ideal_angle_person_robot_-15))){
+			performance=1;
+		}else if((real_angle_person_robot_<(ideal_angle_person_robot_+25))&&(real_angle_person_robot_>(ideal_angle_person_robot_-25))){
+			performance=0.8;
+		}else{
+			performance=0.5;
+		}
+	}
+	fileMatlab << " performance("<<experiment_<<","<<iteration_<<")="<<performance<<";\n";
+
+
+/*
+	fileMatlab << " last_step_robot_other_person_cost_=("<<experiment_<<","<<iteration_<<")="<<last_step_robot_other_person_cost_<<"];\n";
+	fileMatlab << " last_step_robot_companion_cost_=("<<experiment_<<","<<iteration_<<")="<<last_step_robot_companion_cost_<<"];\n";
+	fileMatlab << " last_step_robot_obstacles_cost_=("<<experiment_<<","<<iteration_<<")="<<last_step_robot_obstacles_cost_<<"];\n";
+	fileMatlab << " last_step_robot_control_cost_mix_=("<<experiment_<<","<<iteration_<<")="<<last_step_robot_control_cost_mix_<<"];\n";
+	fileMatlab << " last_step_robot_control_cost_goal_traj_=("<<experiment_<<","<<iteration_<<")="<<last_step_robot_control_cost_goal_traj_<<"];\n";
+	fileMatlab << " last_step_robot_control_cost_goal_person_=("<<experiment_<<","<<iteration_<<")="<<last_step_robot_control_cost_goal_person_<<"];\n";
+	fileMatlab << " last_step_robot_orientation_cost_local_=("<<experiment_<<","<<iteration_<<")="<<last_step_robot_orientation_cost_local_<<"];\n";
+	fileMatlab << " last_step_robot_orientation_cost_global_=("<<experiment_<<","<<iteration_<<")="<<last_step_robot_orientation_cost_global_<<"];\n";
+	fileMatlab << " last_step_robot_distance_cost_=("<<experiment_<<","<<iteration_<<")="<<last_step_robot_distance_cost_<<"];\n";
+	fileMatlab << " other_people_due_to_robot_cost_=("<<experiment_<<","<<iteration_<<")="<<other_people_due_to_robot_cost_<<"];\n";
+	fileMatlab << " companion_person_due_to_robot_cost_=("<<experiment_<<","<<iteration_<<")="<<companion_person_due_to_robot_cost_<<"];\n";
+*/
+
+
+
+	fileMatlab << " computational_time("<<experiment_<<","<<iteration_<<")="<<save_computational_time_value_<<";\n";
+
+
+	fileMatlab << " robot_plan_companion2_start_ITER("<<experiment_<<","<<iteration_<<")="<<robot_plan_companion2_start_ITER<<";\n";
+	fileMatlab << " robot_plan_companion2_end_ITER("<<experiment_<<","<<iteration_<<")="<<robot_plan_companion2_end_ITER<<";\n";
+	//fileMatlab << " time_to_meet_person_geometrically_("<<experiment_<<","<<iteration_<<")="<<time_to_meet_person_geometrically_<<";\n";
+
+	// save leght global path and center of the group to evaluate the performance of the approaching task.
+
+	fileMatlab << " center_of_the_group_x("<<experiment_<<","<<iteration_<<")="<<center_of_the_group_.x<<";\n";
+	fileMatlab << " center_of_the_group_y("<<experiment_<<","<<iteration_<<")="<<center_of_the_group_.y<<";\n";
+	fileMatlab << " center_of_the_group_theta("<<experiment_<<","<<iteration_<<")="<<center_of_the_group_.theta<<";\n";
+	//std::cout <<" IN printToMatlab() save center_of_the_group_x="<<center_of_the_group_.x<< std::endl;
+	//std::cout <<" IN printToMatlab() save center_of_the_group_y="<<center_of_the_group_.y<< std::endl;
+	//std::cout <<" IN printToMatlab() save center_of_the_group_theta="<<center_of_the_group_.theta<< std::endl;
+	// To calculate distance between center_of_the_group and target_person:
+
+
+	fileMatlab << " value_distance_global_path_("<<experiment_<<","<<iteration_<<")="<<value_distance_global_path_<<";\n";
+	fileMatlab << " global_path_ini_orientation_("<<experiment_<<","<<iteration_<<")="<<global_path_ini_orientation_<<";\n";
+	fileMatlab << " global_path_final_orientation_("<<experiment_<<","<<iteration_<<")="<<global_path_final_orientation_<<";\n";
+
+
+	//std::cout <<" OUT printtomatlab!!! (ojo, saved now, the before iteration globath path leng) IN printToMatlab() save value_distance_global_path_="<<value_distance_global_path_<< std::endl;
+
+    fileMatlab.close();
+
+    /* Diferentes modos para abrir el fichero!
+      	ios::in 		Abrir para entrada (lectura)
+		ios::out 		Abrir para salida (escritura)
+		ios::binary 	Abre en modo binario
+		ios::ate 		Escoge el final del fichero como posición inicial (si no se dice lo contrario, la posición inicial al abrir el fichero sería el comienzo del fichero)
+		ios::app 		Abrir para añadir (append) al final, sólo utilizable si se ha abierto el fichero exclusivamente para escritura
+		ios::trunc 		Trunca el fichero si existía, borrando to_do su contenido anterior
+     */
+
+
+}
+
+void Cplan_local_nav_person_companion::calc_area_workspace_arround_person_companion()
+{
+
+	Cperson_abstract* person_obj;
+	find_person(id_person_companion_ , &person_obj);
+	SpointV_cov back_planning_traj=companion_person_position_;
+
+
+	SpointV_cov front_planning_traj=person_obj->pointV_propagation(horizon_time_);
+
+	std::vector <double> vector_x;
+	vector_x.push_back(companion_person_position_.x+3);//back_planning_traj_x1=companion_person_position_.x+3;
+	vector_x.push_back(companion_person_position_.x-3);//double back_planning_traj_x2=companion_person_position_.x-3; // 3 metros
+	vector_x.push_back(front_planning_traj.x+3);
+	vector_x.push_back(front_planning_traj.x-3);
+
+	std::vector <double> vector_y;
+	vector_y.push_back(companion_person_position_.y+3);//back_planning_traj_y1=companion_person_position_.y+3;
+	vector_y.push_back(companion_person_position_.y-3);//double back_planning_traj_y2=companion_person_position_.y-3; // 3 metros
+	vector_y.push_back(front_planning_traj.y+3);
+	vector_y.push_back(front_planning_traj.y-3);
+
+
+	workspace_arround_companion_person_min_x_=vector_x[0];
+	workspace_arround_companion_person_max_x_=vector_x[0];
+	for(unsigned int vec=0; vec<vector_x.size(); vec++){
+		if(vector_x[vec]<workspace_arround_companion_person_min_x_){
+			workspace_arround_companion_person_min_x_=vector_x[vec];
+		}
+		if(vector_x[vec]>workspace_arround_companion_person_max_x_){
+			workspace_arround_companion_person_max_x_=vector_x[vec];
+		}
+	}
+	workspace_arround_companion_person_min_y_=vector_y[0];
+	workspace_arround_companion_person_max_y_=vector_y[0];
+	for(unsigned int vec=0; vec<vector_y.size(); vec++){
+		if(vector_y[vec]<workspace_arround_companion_person_min_y_){
+			workspace_arround_companion_person_min_y_=vector_y[vec];
+		}
+		if(vector_y[vec]>workspace_arround_companion_person_max_y_){
+			workspace_arround_companion_person_max_y_=vector_y[vec];
+		}
+	}
+
+	if(debug_real_test_companion4_){
+	std::cout <<" back_planning_traj: "<< std::endl;
+	back_planning_traj.print();
+	std::cout <<" front_planning_traj: "<< std::endl;
+	front_planning_traj.print();
+	std::cout <<" workspace_arround_companion_person_min_x_="<<workspace_arround_companion_person_min_x_<< std::endl;
+	std::cout <<" workspace_arround_companion_person_max_x_="<<workspace_arround_companion_person_max_x_<< std::endl;
+	std::cout <<" workspace_arround_companion_person_min_y_="<<workspace_arround_companion_person_min_y_<< std::endl;
+	std::cout <<" workspace_arround_companion_person_max_y_="<<workspace_arround_companion_person_max_y_<< std::endl;
+	}
+	//SpointV_cov back_prediction_traj=person_obj->get_prediction_trajectory()->back();
+	//SpointV_cov front_prediction_traj=person_obj->get_prediction_trajectory()->front();
+	//std::cout <<" back_prediction_traj: "<< std::endl;
+	//back_prediction_traj.print();
+	//std::cout <<" front_prediction_traj: "<< std::endl;
+	//front_prediction_traj.print();
+
+
+}
+
+void Cplan_local_nav_person_companion::new_debug_file()
+{
+	std::ofstream fileMatlab2;
+	fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+	fileMatlab2 << "% ***** Starts Autogenerated DEBUG File - results AKP dynamic companion ELY ***** \n\n";
+	fileMatlab2.close();
+}
+
+/*
+void Cplan_local_nav_person_companion::new_Zanlungo_file()
+{
+	std::ofstream fileMatlab;
+		// const char *direc_file_name_;
+	    //direc_file_name_= results_file_.c_str();
+	    //home/ely7787/iri-lab/labrobotica/restricted/algorithms/people_prediction/branches/ely_people_prediction_companion_robot/1_data_results
+	    //fileMatlab.open (results_file_.c_str(), std::ofstream::out | std::ofstream::app);
+		fileMatlab.open (data_file_Zanlungo_.c_str(), std::ofstream::out | std::ofstream::trunc);
+		fileMatlab << "% ***** Autogenerated File - data to see what is doing wrong ***** \n\n";
+		fileMatlab << "%  The file contains: Actual robot and person position, velocity, robot orientation and goal, time (for person and robot). Costs totals of the winner path in each time. \n\n";
+		fileMatlab << "\n";
+		fileMatlab << "dt=[]\n";
+		fileMatlab << "real_computation_time_dt=[]\n";
+		fileMatlab << "companion_person_x=[]\n";
+		fileMatlab << "companion_person_y=[]\n";
+		fileMatlab << "companion_person_vx=[]\n";
+		fileMatlab << "companion_person_vy=[]\n";
+		fileMatlab << "companion_person_theta=[]\n";
+		fileMatlab << "robot_x=[]\n";
+		fileMatlab << "robot_y=[]\n";
+		fileMatlab << "robot_vx=[]\n";
+		fileMatlab << "robot_vy=[]\n";
+		fileMatlab << "robot_theta=[]\n";
+		fileMatlab << "robot_final_goal_x=[]\n";
+		fileMatlab << "robot_final_goal_y=[]\n";
+		fileMatlab << "preferred_x=[]\n";
+		fileMatlab << "preferred_y=[]\n";
+		fileMatlab << "preferred_m=[]\n";
+		fileMatlab << "preferred_th=[]\n";
+
+		fileMatlab << "dist_robot_person=[]\n";
+		fileMatlab << "f_companion_x=[]\n";
+		fileMatlab << "f_companion_y=[]\n";
+		fileMatlab << "f_goal_x=[]\n";
+		fileMatlab << "f_goal_y=[]\n";
+		//fileMatlab << "alpha_=[]\n";
+		//fileMatlab << "beta_=[]\n";
+		//fileMatlab << "k=[]\n";
+		fileMatlab << "v_desired_x=[]\n";
+		fileMatlab << "v_desired_y=[]\n";
+		fileMatlab << "v_current_x=[]\n";
+		fileMatlab << "v_current_y=[]\n";
+
+		// todo: falta orientacion y goal que le paso al robot.
+
+}*/
+
+
+
+void Cplan_local_nav_person_companion::new_matlab_file()
+{
+//// //this->config_.map_path + "/" + this->config_.map_filename
+	//std::string data_file, data_file2;
+	//data_file="/home/ely7787/iri-lab/labrobotica/restricted/algorithms/people_prediction/branches/ely_people_prediction_companion/data_results/results";
+	//data_file2=".txt";
+	//std::string final_file= data_file + "/" + data_file2;
+	std::ofstream fileMatlab;
+	// const char *direc_file_name_;
+    //direc_file_name_= results_file_.c_str();
+    //home/ely7787/iri-lab/labrobotica/restricted/algorithms/people_prediction/branches/ely_people_prediction_companion_robot/1_data_results
+    //fileMatlab.open (results_file_.c_str(), std::ofstream::out | std::ofstream::app);
+	fileMatlab.open (results_file_.c_str(), std::ofstream::out | std::ofstream::trunc);
+    // FILE HEADER
+    fileMatlab << "% ***** Autogenerated File - results AKP dynamic companion ELY ***** \n\n";
+    fileMatlab << "%  The file contains: Actual robot and person distance, real angle, ideal angle at this time, time, time to arrive to goal (for person and robot). Costs totals of the winner path in each time. \n\n";
+    fileMatlab << "\n real_distance_person_robot=[];\n";
+    fileMatlab << " ideal_distance_person_robot=[];\n";
+    fileMatlab << " real_angle_person_robot=[];\n";
+    fileMatlab << " ideal_angle_person_robot=[];\n";
+    fileMatlab << " actual_time=[];\n";
+    fileMatlab << " robot_distance_cost=[];\n";
+    fileMatlab << " robot_orientation_cost=[];\n";
+    fileMatlab << " robot_control_cost=[];\n";
+    fileMatlab << " robot_other_person_cost=[];\n";
+    fileMatlab << " robot_obstacles_cost=[];\n";
+    fileMatlab << " robot_companion_cost=[];\n";
+    fileMatlab << " robot_total_cost=[]; \n";
+    fileMatlab << " robot_ant_traj_cost=[];\n";
+    fileMatlab << " cost_parameters_0=[];\n";
+    fileMatlab << " cost_parameters_1=[];\n";
+    fileMatlab << " cost_parameters_2=[];\n";
+    fileMatlab << " cost_parameters_3=[];\n";
+    fileMatlab << " cost_parameters_5=[];\n";
+    fileMatlab << " cost_parameters_6=[];\n";
+    fileMatlab << " person_distance_to_goal=[];\n";
+    fileMatlab << " robot_distance_to_goal=[];\n";
+    fileMatlab << " percent_of_error_in_distance=[];\n";
+    fileMatlab << " performance=[];\n";
+
+    fileMatlab << " last_step_robot_other_person_cost_=[];\n";
+    fileMatlab << " last_step_robot_companion_cost_=[];\n";
+    fileMatlab << " last_step_robot_obstacles_cost_=[];\n";
+    fileMatlab << " last_step_robot_control_cost_mix_=[];\n";
+    fileMatlab << " last_step_robot_control_cost_goal_traj_=[];\n";
+    fileMatlab << " last_step_robot_control_cost_goal_person_=[];\n";
+    fileMatlab << " last_step_robot_orientation_cost_local_=[];\n";
+    fileMatlab << " last_step_robot_orientation_cost_global_=[];\n";
+    fileMatlab << " last_step_robot_distance_cost_=[];\n";
+
+    fileMatlab << " other_people_due_to_robot_cost_=[];\n";
+    fileMatlab << " companion_person_due_to_robot_cost_=[];\n";
+
+    fileMatlab << " person_goal_x=[];\n";
+    fileMatlab << " person_goal_y=[];\n";
+    fileMatlab << " person_goal_vx=[];\n";
+    fileMatlab << " person_goal_vy=[];\n";
+    fileMatlab << " person_goal_theta1=[];\n";
+    fileMatlab << " person_goal_theta2=[];\n";
+    fileMatlab << " distance_robot_person_goal=[];\n";
+    fileMatlab << " distance_person_companion_person_goal=[];\n";
+    fileMatlab << " distance_centre_group_person_goal=[];\n";
+    fileMatlab << " theta_group_centro_robot_persona=[];\n";
+    fileMatlab << " person_goal_theta1=[];\n";
+    fileMatlab << " theta_group_comp_op=[];\n";
+    fileMatlab << " center_of_the_group_theta=[];\n";
+    fileMatlab << " person_c_robot_orientation_by_line=[];\n";
+    fileMatlab << " theta_group_comp_op2=[];\n";
+    fileMatlab << " angle_diference_person_goal_group=[];\n";
+    fileMatlab << " angle_diference_person_goal_group2=[];\n";
+    fileMatlab << " Action_=[];\n";
+    //fileMatlab << " actual_final_goal_to_meet_person_x=[];\n";
+    //fileMatlab << " actual_final_goal_to_meet_person_y=[];\n";
+    //fileMatlab << " before_actual_final_goal_to_meet_person_x=[];\n";
+    //fileMatlab << " before_actual_final_goal_to_meet_person_y=[];\n";
+    fileMatlab << " distance_between_consecutive_goals=[];\n";
+    fileMatlab << " computational_time=[];\n";
+    fileMatlab << " robot_plan_companion2_start_ITER=[];\n";
+    fileMatlab << " robot_plan_companion2_end_ITER=[];\n";
+    //fileMatlab << " time_to_meet_person_geometrically_=[];\n";
+    fileMatlab << " center_of_the_group_x=[];\n";
+    fileMatlab << " center_of_the_group_y=[];\n";
+    fileMatlab << " center_of_the_group_theta=[];\n";
+    /*fileMatlab << " d_center_group_and_targ_per_during_path=[];\n";
+    fileMatlab << " d_rob_and_targ_per_during_path=[];\n";
+    fileMatlab << " d_per_comp_and_targ_per_during_path=[];\n";
+    fileMatlab << " dif_ori_center_group_and_targ_per_during_path=[];\n";
+    fileMatlab << " dif_ori_rob_and_targ_per_during_path=[];\n";
+    fileMatlab << " dif_ori_per_compn_and_targ_per_during_path=[];\n";
+    */
+    fileMatlab << " value_distance_global_path_=[];\n";
+    fileMatlab << " global_path_ini_orientation_=[];\n";
+    fileMatlab << " global_path_final_orientation_=[];\n";
+    //fileMatlab << " act_ori_group_to_targ_per_=[];\n";
+
+
+    /*
+     *
+     * d_center_group_and_targ_per_during_path=[];
+ d_rob_and_targ_per_during_path=[];
+ d_per_comp_and_targ_per_during_path=[];
+ dif_ori_center_group_and_targ_per_during_path=[];
+ dif_ori_rob_and_targ_per_during_path=[];
+ dif_ori_per_compn_and_targ_per_during_path=[];
+ value_distance_global_path_=[];
+ global_path_ini_orientation_=[];
+ global_path_final_orientation_=[];
+ act_ori_group_to_targ_per_=[];
+     */
+
+    fileMatlab.close();
+
+}
+
+
+void Cplan_local_nav_person_companion::new_matlab_file_To_evaluate_costs()
+{
+//// //this->config_.map_path + "/" + this->config_.map_filename
+	//std::string data_file, data_file2;
+	//data_file="/home/ely7787/iri-lab/labrobotica/restricted/algorithms/people_prediction/branches/ely_people_prediction_companion/data_results/results";
+	//data_file2=".txt";
+	//std::string final_file= data_file + "/" + data_file2;
+	std::ofstream fileMatlab;
+	// const char *direc_file_name_;
+    //direc_file_name_= evaluate_costs_file_.c_str();
+    //home/ely7787/iri-lab/labrobotica/restricted/algorithms/people_prediction/branches/ely_people_prediction_companion_robot/2_results_evaluate_costs
+    fileMatlab.open (evaluate_costs_file_.c_str(), std::ofstream::out | std::ofstream::app);
+    // FILE HEADER
+    fileMatlab << "% ***** Autogenerated File - evaluate costs AKP dynamic companion ELY ***** \n\n";
+    fileMatlab << "%  The file contains: the best path and nondominated paths positions, the id's of the nodes of the paths, the total costs and partially costs of each node,  means and std of the costs, the itraration \n\n";
+    fileMatlab << "\n time=0;\n";
+    fileMatlab << " iter=0;\n";
+    fileMatlab << " num_of_total_paths=0;\n";
+    fileMatlab << " mean=[];\n";
+    fileMatlab << " std=[];\n";
+    fileMatlab << " % nondominated paths: \n";
+    fileMatlab << " end_id_nondominated_paths=0;\n";
+    fileMatlab << " path_pose=[];\n";
+    fileMatlab << " id_node=[];\n";
+    fileMatlab << " costs=[];\n";
+    fileMatlab << " % best path: \n";
+    fileMatlab << " end_id_best_path=0;\n";
+    fileMatlab << " best_path_poses=[];\n";
+    fileMatlab << " best_path_id_nodes=[];\n";
+    fileMatlab << " best_paths_costs=[];\n";
+
+
+    fileMatlab.close();
+
+}
+
+
+
+
+
+void Cplan_local_nav_person_companion::new_matlab_file_To_evaluate_change_distance_and_angle()
+{
+	std::ofstream fileMatlab;
+	// const char *direc_file_name_;
+	//direc_file_name_= evaluate_costs_file_.c_str();
+	//home/ely7787/iri-lab/labrobotica/restricted/algorithms/people_prediction/branches/ely_people_prediction_companion_robot/2_results_evaluate_costs
+	fileMatlab.open (evaluate_change_distance_and_angle_companion_file_.c_str(), std::ofstream::out | std::ofstream::app);
+	// FILE HEADER
+	fileMatlab << "% ***** Autogenerated File - evaluate costs AKP dynamic companion ELY ***** \n\n";
+	fileMatlab << "%  The file contains: the best path and nondominated paths positions, the id's of the nodes of the paths, the total costs and partially costs of each node,  means and std of the costs, the itraration \n\n";
+	fileMatlab << " iter=[];\n";
+	fileMatlab << " distance_robot_person=[];\n";
+	fileMatlab << " angle_companion_robot_person=[];\n";
+}
+
+void Cplan_local_nav_person_companion::evaluate_change_distance_and_angle_companion_with_beta_change()
+{
+	std::ofstream fileMatlab;
+	fileMatlab.open (evaluate_change_distance_and_angle_companion_file_.c_str(), std::ofstream::out | std::ofstream::app);
+	double iter=iter_act_multiple_paths_and_best_path_.iter_act;
+
+    // Algorithm Params
+	fileMatlab << " \n \n \n % New iteration! \n";
+
+	if(debug_file_evaluate_costs_){
+		std::cout <<" \n \n \n % New iteration!  "<< std::endl;
+	}
+	//std::cout <<" robot_->get_platform_radii()="<<robot_->get_platform_radii()<< std::endl;
+
+	if(save_distance_between_person_and_robot_<(robot_person_proximity_distance_+2*robot_->get_platform_radii())){
+		iter_d_++;
+		fileMatlab << "\n  iter=[iter,"<<iter_d_<<"];\n";
+		fileMatlab << "\n  distance_robot_person=[distance_robot_person,"<<save_distance_between_person_and_robot_<<"];\n";
+		fileMatlab << "\n  angle_companion_robot_person=[angle_companion_robot_person,"<<save_angle_between_person_and_robot_<<"];\n";
+		fileMatlab << "\n  robot_goal_to_person_companion_x=[robot_goal_to_person_companion_x,"<<robot_goal_to_person_companion_.x<<"];\n";
+		fileMatlab << "\n  robot_goal_to_person_companion_y=[robot_goal_to_person_companion_y,"<<robot_goal_to_person_companion_.y<<"];\n";
+		fileMatlab << "\n  robot_path_goal_x=[robot_path_goal_x,"<<robot_path_goal_.x<<"];\n";
+		fileMatlab << "\n  robot_path_goal_y=[robot_path_goal_y,"<<robot_path_goal_.y<<"];\n";
+		fileMatlab << "\n  final_combined_goal_x=[final_combined_goal_x,"<<final_combined_goal_.x<<"];\n";
+		fileMatlab << "\n  final_combined_goal_y=[final_combined_goal_y,"<<final_combined_goal_.y<<"];\n";
+
+
+		fileMatlab << "\n  person_position_t_4_x=[person_position_t_4_x,"<<person_position_t_4_.x<<"];\n";
+		fileMatlab << "\n  person_position_t_4_y=[person_position_t_4_y,"<<person_position_t_4_.y<<"];\n";
+
+		Spoint goal_person_comp=Spoint(robot_goal_to_person_companion_.x,robot_goal_to_person_companion_.y,robot_goal_to_person_companion_.time_stamp);
+
+		double dist=person_position_t_4_.distance(goal_person_comp);
+		fileMatlab << "\n  distance_between_goal_person_comp_and_person_t4=[distance_between_goal_person_comp_and_person_t4,"<<dist<<"];\n";
+		double angle=calculate_actual_angle_person_robot(person_position_t_4_, goal_person_comp);
+		fileMatlab << "\n  angle_between_goal_person_comp_and_person_t4=[angle_between_goal_person_comp_and_person_t4,"<<angle<<"];\n";
+
+		Spoint goal_path=Spoint(robot_path_goal_.x,robot_path_goal_.y,robot_path_goal_.time_stamp);
+		dist=person_position_t_4_.distance(goal_path);
+		fileMatlab << "\n  distance_between_goal_path_and_person_t4=[distance_between_goal_path_and_person_t4,"<<dist<<"];\n";
+		angle=calculate_actual_angle_person_robot(person_position_t_4_, goal_path);
+		fileMatlab << "\n  angle_between_goal_path_and_person_t4=[angle_between_goal_path_and_person_t4,"<<angle<<"];\n";
+
+		Spoint final_combined_goal=Spoint(final_combined_goal_.x,final_combined_goal_.y,final_combined_goal_.time_stamp);
+		dist=person_position_t_4_.distance(final_combined_goal);
+		fileMatlab << "\n  distance_between_final_combined_goal_and_person_t4=[distance_between_final_combined_goal_and_person_t4,"<<dist<<"];\n";
+		angle=calculate_actual_angle_person_robot(person_position_t_4_, final_combined_goal);
+		fileMatlab << "\n  angle_between_final_combined_goal_and_person_t4=[angle_between_final_combined_goal_and_person_t4,"<<angle<<"];\n";
+
+
+		fileMatlab << "\n  next_angle_companion=[next_angle_companion,"<<next_companion_angle_save_<<"];\n";
+		fileMatlab << "\n  actual_angle_companion=[actual_angle_companion,"<<before_act_companion_angle_<<"];\n";
+
+
+		SpointV_cov robot_position=robot_->get_current_pointV();
+		fileMatlab << " \n%  obstacles: \n";
+		fileMatlab << "\n  obstacles_x=[];\n";
+		fileMatlab << "\n  obstacles_y=[];\n";
+		double d_ini=0;
+
+		for( auto iit: laser_obstacle_list_ )
+		{
+			//d_ini = iit.distance2( robot_position );
+		    d_ini = iit.distance( robot_position );
+
+		    //std::cout << " radii_2= "<< radii_2<< " d_ini="<<d_ini << std::endl;
+
+		    //if( d_ini < radii_2  )
+		    if( d_ini < 5  )
+		    {
+		    	//number_of_obstacles++;
+		    	if(debug_file_evaluate_costs_){
+		    		std::cout <<" obstacle["<<iter<<"]=> x="<<iit.x<<"; y="<<iit.y<< std::endl;
+		    	}
+		    	fileMatlab << "\n  obstacles_x=[obstacles_x,"<<iit.x<<"];\n";
+		    	fileMatlab << "\n  obstacles_y=[obstacles_y,"<<iit.y<<"];\n";
+		    }
+		    /*if ( d_ini < d_min )
+		    	d_min = d_ini;*/
+		}
+
+
+	}
+
+}
+
+
+
+void Cplan_local_nav_person_companion::evaluate_costs_printToMatlab(Crobot* actual_robot)
+{
+	std::ofstream fileMatlab;
+	fileMatlab.open (evaluate_costs_file_.c_str(), std::ofstream::out | std::ofstream::app);
+
+
+    //fileMatlab << " clc,\n clear all,\n close all \n\n\n";
+
+	/*if(real_angle_person_robot_<100){ // todo: provisional, ja que al pasarme de 180 guardo mal el real angle sin querer, hago mal el cambio de grados.
+		real_angle_person_robot_=180;
+	}*/
+	double iter=iter_act_multiple_paths_and_best_path_.iter_act;
+
+	SpointV_cov robot_position=actual_robot->get_current_pointV();
+    // Algorithm Params
+	fileMatlab << " \n \n \n % New iteration! \n";
+
+	if(debug_file_evaluate_costs_){
+		std::cout <<" \n \n \n % New iteration!  "<< std::endl;
+	}
+
+	fileMatlab << "\n  iter_act="<<iter<<";\n";
+
+	fileMatlab << " \n % Guardamos el local_goal_actual \n";
+	fileMatlab << "\n  local_goal_x="<<local_goal_.x<<";\n";
+	fileMatlab << "\n  local_goal_y="<<local_goal_.y<<";\n";
+
+
+	fileMatlab << " \n%  robot_point: \n";
+	fileMatlab << "\n  robot_position_x="<<robot_position.x<<";\n";
+	fileMatlab << "\n  robot_position_y="<<robot_position.y<<";\n";
+	fileMatlab << "\n  robot_position_vx="<<robot_position.vx<<";\n";
+	fileMatlab << "\n  robot_position_vy="<<robot_position.vy<<";\n";
+	fileMatlab << "\n  robot_position_v="<<robot_initial_pose_.v<<";\n";
+	fileMatlab << "\n  robot_position_w="<<robot_initial_pose_.w<<";\n";
+	fileMatlab << "\n  robot_position_theta="<<robot_initial_pose_.theta<<";\n";
+
+	fileMatlab << " \n%  person position: \n";
+	fileMatlab << "\n  person_position_x="<<companion_person_position_.x<<";\n";
+	fileMatlab << "\n  person_position_y="<<companion_person_position_.y<<";\n";
+	fileMatlab << "\n  person_position_vx="<<companion_person_position_.vx<<";\n";
+	fileMatlab << "\n  person_position_vy="<<companion_person_position_.vy<<";\n";
+	fileMatlab << "\n  person_position_theta="<<orientacion_persona_actual_<<";\n";
+	fileMatlab << "\n  person_position_x_ant_to_calc_theta="<<person_x_ant_<<";\n";
+	fileMatlab << "\n  person_position_y_ant_to_calc_theta="<<person_y_ant_<<";\n";
+
+	// TODO: ver si guardo el final cost the cada path en cada itracion.
+
+	fileMatlab << " \n%  obstacles: \n";
+	fileMatlab << "\n  obstacles_x=[];\n";
+	fileMatlab << "\n  obstacles_y=[];\n";
+	double d_ini=0;
+
+	    for( auto iit: laser_obstacle_list_ )
+	    {
+	    	//d_ini = iit.distance2( robot_position );
+	    	d_ini = iit.distance( robot_position );
+
+	    	//std::cout << " radii_2= "<< radii_2<< " d_ini="<<d_ini << std::endl;
+
+	    	//if( d_ini < radii_2  )
+	    	if( d_ini < 5  )
+	    	{
+	    		//number_of_obstacles++;
+	    		if(debug_file_evaluate_costs_){
+	    			std::cout <<" obstacle["<<iter<<"]=> x="<<iit.x<<"; y="<<iit.y<< std::endl;
+	    		}
+	    		fileMatlab << "\n  obstacles_x=[obstacles_x,"<<iit.x<<"];\n";
+	    		fileMatlab << "\n  obstacles_y=[obstacles_y,"<<iit.y<<"];\n";
+	    	}
+	    	/*if ( d_ini < d_min )
+	    		d_min = d_ini;*/
+	    }
+
+
+	std::cout <<" \n  "<< std::endl;
+	fileMatlab << "\n time=[];\n";
+	fileMatlab << "\n iter=[];\n";
+	fileMatlab << "\n num_of_total_paths=[];\n";
+
+	fileMatlab << "\n time=[time,"<<iter_act_multiple_paths_and_best_path_.time_act<<"];\n";
+
+	if(debug_file_evaluate_costs_){
+		std::cout <<" \n time="<<iter_act_multiple_paths_and_best_path_.time_act<< std::endl;
+	}
+	fileMatlab << " iter=[iter,"<<iter_act_multiple_paths_and_best_path_.iter_act<<"];\n";
+
+	if(debug_file_evaluate_costs_){
+		std::cout <<" iter="<<iter_act_multiple_paths_and_best_path_.iter_act<< std::endl;
+	}
+
+	fileMatlab << " num_of_total_paths=[num_of_total_paths,"<<iter_act_multiple_paths_and_best_path_.number_of_total_paths<<"];\n";
+
+	if(debug_file_evaluate_costs_){
+		std::cout << " num_of_total_paths="<<iter_act_multiple_paths_and_best_path_.number_of_total_paths<<std::endl;
+	}
+
+	fileMatlab << " mean=[];\n";
+	fileMatlab << " stds=[];\n";
+	fileMatlab << " \n % means of costs: mean_distance_[0]; mean_orientation_[1]; mean_robot_[2]; mean_int_forces_[3]; mean_obstacles_[4]; mean_companion_[5]; ant_traj[6]; \n";
+
+	if(debug_file_evaluate_costs_){
+		std::cout  << " \n % means of costs: mean_distance_[0]; mean_orientation_[1]; mean_robot_[2]; mean_int_forces_[3]; mean_obstacles_[4]; mean_companion_[5]; ant_traj[6]; \n"<<std::endl;
+	}
+
+	for(unsigned int i=0; i<iter_act_multiple_paths_and_best_path_.means.size(); i++){
+		fileMatlab << " mean=[mean,"<<iter_act_multiple_paths_and_best_path_.means[i]<<"];\n";
+
+		if(debug_file_evaluate_costs_){std::cout << " mean["<<iter<<"]="<<iter_act_multiple_paths_and_best_path_.means[i]<<std::endl;}
+
+	}
+
+	fileMatlab << " \n % stds of costs: \n";
+	if(debug_file_evaluate_costs_){std::cout << " \n % stds of costs: \n"<<std::endl;}
+	for(unsigned int i=0; i<iter_act_multiple_paths_and_best_path_.stds.size(); i++){
+		fileMatlab << " stds=[stds,"<<iter_act_multiple_paths_and_best_path_.stds[i]<<"];\n";
+		if(debug_file_evaluate_costs_){std::cout << " stds["<<iter<<"]="<<iter_act_multiple_paths_and_best_path_.stds[i]<<std::endl;}
+	}
+
+	fileMatlab << " \n \n % nondominated paths: \n";
+	if(debug_file_evaluate_costs_){std::cout << " \n % nondominated paths: \n iter_act_multiple_paths_and_best_path_.nondominated_paths.size()="<<iter_act_multiple_paths_and_best_path_.nondominated_paths.size()<<std::endl;}
+
+	std::vector<SsavePath_cost_and_values> act_nondominated_paths=iter_act_multiple_paths_and_best_path_.get_nondominated_paths();
+	if(debug_file_evaluate_costs_){std::cout << " \n % nondominated paths: \n act_nondominated_paths.size()="<<act_nondominated_paths.size()<<std::endl;}
+	fileMatlab << "end_id_nondominated_paths=[];\n";
+
+	for(unsigned int l=0; l<iter_act_multiple_paths_and_best_path_.nondominated_paths.size(); l++){
+		fileMatlab << "end_id_nondominated_paths=[end_id_nondominated_paths,"<<iter_act_multiple_paths_and_best_path_.nondominated_paths[l].end_id_path<<"];\n";
+		if(debug_file_evaluate_costs_){std::cout << "end_id_nondominated_paths["<<iter<<"][=l_"<<l<<"]="<<iter_act_multiple_paths_and_best_path_.nondominated_paths[l].end_id_path<<std::endl;}
+
+		fileMatlab << "id_node_l_"<<l<<"=[];\n";
+		fileMatlab << "path_pose_l_"<<l<<"_x=[];\n";
+		fileMatlab << "path_pose_l_"<<l<<"_y=[];\n";
+		fileMatlab << "costs_l_"<<l<<"=[];\n";
+
+		for(unsigned int j=0; j<iter_act_multiple_paths_and_best_path_.nondominated_paths[l].all_ids_path_positions.size(); j++){
+			fileMatlab << "id_node_l_"<<l<<"=[id_node_l_"<<l<<","<<iter_act_multiple_paths_and_best_path_.nondominated_paths[l].all_ids_path_positions[j]<<"];\n";
+			if(debug_file_evaluate_costs_){std::cout << "id_node["<<iter<<"]_[l="<<l<<"]="<<iter_act_multiple_paths_and_best_path_.nondominated_paths[l].all_ids_path_positions[j]<<std::endl;}
+		//}
+
+		//for(unsigned int j=0; j<iter_act_multiple_paths_and_best_path_.nondominated_paths[l].path_positions.size(); j++){
+			fileMatlab << "path_pose_l_"<<l<<"_x=[path_pose_l_"<<l<<"_x,"<<iter_act_multiple_paths_and_best_path_.nondominated_paths[l].path_positions[j].x<<"];\n";
+			fileMatlab << "path_pose_l_"<<l<<"_y=[path_pose_l_"<<l<<"_y,"<<iter_act_multiple_paths_and_best_path_.nondominated_paths[l].path_positions[j].y<<"];\n";
+			if(debug_file_evaluate_costs_){std::cout <<  "path_pose["<<iter<<"]_[l="<<l<<"]_x="<<iter_act_multiple_paths_and_best_path_.nondominated_paths[l].path_positions[j].x<<std::endl;
+			std::cout <<  "path_pose["<<iter<<"]_[l="<<l<<"]_y="<<iter_act_multiple_paths_and_best_path_.nondominated_paths[l].path_positions[j].y<<std::endl;}
+		//}
+
+			fileMatlab << "% costs: distance_cost[0]; orientation_cost[1]; contro_cost[2]; other_persons_cost[3]; obstacles_cost[4]; companion_cost[5] total_cost[6] ant_traj_cost[7] \n";
+			if(debug_file_evaluate_costs_){std::cout <<"% costs: distance_cost[0]; orientation_cost[1]; contro_cost[2]; other_persons_cost[3]; obstacles_cost[4]; companion_cost[5] total_cost[6] ant_traj_cost[7] \n"<<std::endl;}
+
+			//std::cout <<"% iter_act_multiple_paths_and_best_path_.nondominated_paths[l].all_ids_path_positions.size()="<<iter_act_multiple_paths_and_best_path_.nondominated_paths[l].all_ids_path_positions.size()<<std::endl;
+			//std::cout <<"% iter_act_multiple_paths_and_best_path_.nondominated_paths[i].path_positions.size()="<<iter_act_multiple_paths_and_best_path_.nondominated_paths[l].path_positions.size()<<std::endl;
+			//std::cout <<"% iter_act_multiple_paths_and_best_path_.nondominated_paths[i].total_costs_of_each_path_position.size()="<<iter_act_multiple_paths_and_best_path_.nondominated_paths[l].total_costs_of_each_path_position.size()<<std::endl;
+			//std::cout <<"% k=iter_act_multiple_paths_and_best_path_.nondominated_paths[l].total_costs_of_each_path_position[j].size()="<<iter_act_multiple_paths_and_best_path_.nondominated_paths[l].total_costs_of_each_path_position[j].size()<<std::endl;
+
+		//for(unsigned int j=0; j<iter_act_multiple_paths_and_best_path_.nondominated_paths[l].total_costs_of_each_path_position.size(); j++){
+			fileMatlab << "costs_act=[];\n";
+			for(unsigned int k=0; k<iter_act_multiple_paths_and_best_path_.nondominated_paths[l].total_costs_of_each_path_position[j].size(); k++){
+				//std::cout << "k="<<k<<std::endl;
+				fileMatlab << "costs_act=[costs_act,"<<iter_act_multiple_paths_and_best_path_.nondominated_paths[l].total_costs_of_each_path_position[j][k]<<"];\n";
+				if(debug_file_evaluate_costs_){std::cout << "costs["<<iter<<"]_[l="<<l<<"][j="<<j<<"][k="<<k<<"]="<<iter_act_multiple_paths_and_best_path_.nondominated_paths[l].total_costs_of_each_path_position[j][k]<<std::endl;}
+
+			}
+			fileMatlab << "costs_l_"<<l<<"=[costs_l_"<<l<<";costs_act];\n";
+
+			fileMatlab << "\n";
+			if(debug_file_evaluate_costs_){std::cout<<std::endl;}
+		}
+		fileMatlab << "\n \n";
+		if(debug_file_evaluate_costs_){std::cout<<std::endl<<std::endl;}
+	}
+	if(debug_file_evaluate_costs_){std::cout<<std::endl;}
+
+	fileMatlab << " \n % best path: \n";
+	fileMatlab << "end_id_best_path="<<iter_act_multiple_paths_and_best_path_.best_path.end_id_path<<";\n";
+	if(debug_file_evaluate_costs_){std::cout << "end_id_best_path["<<iter<<"]="<<iter_act_multiple_paths_and_best_path_.best_path.end_id_path<<std::endl;}
+
+	fileMatlab << "best_path_id_nodes=[];\n";
+	fileMatlab << "best_path_pose_x=[];\n";
+	fileMatlab << "best_path_pose_y=[];\n";
+
+	fileMatlab << "best_path_id_nodes=[];\n";
+	fileMatlab << "best_path_pose_x=[];\n";
+	fileMatlab << "best_path_pose_y=[];\n";
+	fileMatlab << "best_costs=[];\n";
+
+
+	for(unsigned int j=0; j<iter_act_multiple_paths_and_best_path_.best_path.all_ids_path_positions.size(); j++){
+		fileMatlab << "best_path_id_nodes=[best_path_id_nodes,"<<iter_act_multiple_paths_and_best_path_.best_path.all_ids_path_positions[j]<<"];\n";
+		if(debug_file_evaluate_costs_){std::cout << "best_path_id_nodes["<<iter<<"]_[j="<<j<<"]="<<iter_act_multiple_paths_and_best_path_.best_path.all_ids_path_positions[j]<<std::endl;}
+	//}
+
+	//for(unsigned int j=0; j<iter_act_multiple_paths_and_best_path_.best_path.path_positions.size(); j++){
+		fileMatlab << "best_path_pose_x=[best_path_pose_x,"<<iter_act_multiple_paths_and_best_path_.best_path.path_positions[j].x<<"];\n";
+		fileMatlab << "best_path_pose_y=[best_path_pose_y,"<<iter_act_multiple_paths_and_best_path_.best_path.path_positions[j].y<<"];\n";
+		if(debug_file_evaluate_costs_){std::cout << "best_path_pose["<<iter<<"]_[j="<<j<<"]_x="<<iter_act_multiple_paths_and_best_path_.best_path.path_positions[j].x<<std::endl;
+		std::cout << "best_path_pose["<<iter<<"]_[j="<<j<<"]_y="<<iter_act_multiple_paths_and_best_path_.best_path.path_positions[j].y<<std::endl;}
+
+	//}
+		//for(unsigned int j=0; j<iter_act_multiple_paths_and_best_path_.best_path.total_costs_of_each_path_position.size(); j++){
+		fileMatlab << "% costs: distance_cost[0]; orientation_cost[1]; contro_cost[2]; other_persons_cost[3]; obstacles_cost[4]; companion_cost[5] total_cost[6] ant_traj_cost[7] \n";
+		if(debug_file_evaluate_costs_){std::cout << "% costs: distance_cost[0]; orientation_cost[1]; contro_cost[2]; other_persons_cost[3]; obstacles_cost[4]; companion_cost[5] total_cost[6] ant_traj_cost[7] \n"<<std::endl;}
+
+		fileMatlab << "costs_act=[];\n";
+		for(unsigned int k=0; k<iter_act_multiple_paths_and_best_path_.best_path.total_costs_of_each_path_position[j].size(); k++){
+			fileMatlab << "costs_act=[costs_act,"<<iter_act_multiple_paths_and_best_path_.best_path.total_costs_of_each_path_position[j][k]<<"];\n";
+			if(debug_file_evaluate_costs_){std::cout << "best_costs["<<iter<<"]_[j="<<j<<"][k="<<k<<"]="<<iter_act_multiple_paths_and_best_path_.best_path.total_costs_of_each_path_position[j][k]<<std::endl;}
+		}
+		fileMatlab << "best_costs=[best_costs;costs_act];\n";
+
+	}
+    fileMatlab.close();
+
+    /* Diferentes modos para abrir el fichero!
+      	ios::in 		Abrir para entrada (lectura)
+		ios::out 		Abrir para salida (escritura)
+		ios::binary 	Abre en modo binario
+		ios::ate 		Escoge el final del fichero como posición inicial (si no se dice lo contrario, la posición inicial al abrir el fichero sería el comienzo del fichero)
+		ios::app 		Abrir para añadir (append) al final, sólo utilizable si se ha abierto el fichero exclusivamente para escritura
+		ios::trunc 		Trunca el fichero si existía, borrando to_do su contenido anterior
+     */
+}
+
+
+
+
+
+
+void Cplan_local_nav_person_companion::calculate_intersecction_line_circle(double Xc, double Yc, double radius, double X_ini, double Y_ini, double X_final, double Y_final, double &intersection1, double &intersection2 ){
+
+	double recta_slope=(X_final-X_ini)/(Y_final-Y_ini);
+
+	double a=recta_slope*recta_slope + 1;
+	double b=2*(-Yc+recta_slope*(X_ini-Y_ini*recta_slope-Xc));
+	double c=Yc*Yc - radius*radius + (X_ini-Xc-recta_slope*Y_ini)*(X_ini-Xc-recta_slope*Y_ini);
+
+
+
+	intersection1=(-b + sqrt(b*b - 4*a*c))/(2*a);
+	intersection2=(-b - sqrt(b*b - 4*a*c))/(2*a);
+
+}
+
+
+
+
+
+void Cplan_local_nav_person_companion::robot_trajectory_prediction_for_group_Zanlungo( double new_horizon_time, Sdestination goal_to_predict_pose, unsigned int id_person_comp)
+{
+
+	//std::cout <<  " IN robot_trajectory_prediction_for_group_Zanlungo " << std::endl;
+	unsigned int iteration_act=new_horizon_time/0.2;
+
+	/*for( auto iit : person_list_)
+	{
+		std::cout << " IN robot_group_trajectory_prediction (planning traj size people) iit->size"<<iit->get_planning_trajectory()->size() <<", id=" <<iit->get_id()<< std::endl;
+
+		std::cout <<  " IN robot_group_trajectory_prediction " << std::endl;
+		//if(iit->get_id()!=id_person_companion_){
+			//			 std::cout << " (robot_group_trajectory_prediction) (1) iit->get_prediction_trajectory( )->size()="<<iit->get_prediction_trajectory( )->size()<<"; iit->get_id()=" <<iit->get_id()<<"; iit->desired_vel="<<iit->get_desired_velocity()<<  std::endl;
+		//}
+	}*/
+
+	/*std::cout <<  " IN robot_group_trajectory_prediction " << std::endl;
+				for( auto iit : person_list_)
+				{
+					if(iit->get_id()!=id_person_companion_){
+					 std::cout << " (robot_group_trajectory_prediction) (1) iit->get_prediction_trajectory( )->size()="<<iit->get_prediction_trajectory( )->size()<<"; iit->get_id()=" <<iit->get_id()<<"; iit->desired_vel="<<iit->get_desired_velocity()<<  std::endl;
+					}
+				}*/
+
+	// TOD: de momento igual k en planning akp, se ponen el alpha_ gamma_ y delta:
+	//robot parameters sampling if required
+	//set the random alpha variable, if necessary
+	/*
+	 * actual_person_pposition_prop;
+	 * if( plan_mode_ == F_RRT_GC_alpha )
+	{
+
+		std::uniform_int_distribution<int> sample_behavior( 0 , 2 );
+		double epsilon(0.4);
+		switch( sample_behavior(generator_) )
+		{
+		case 0://robot unaware
+			alpha_ = 1.0+epsilon;gamma_ = 1.0-epsilon;delta_ = 1.0-epsilon;
+			//std::cout << "case 0 :  alpha_="<<alpha_ <<"; gamma_="<<gamma_<<"; delta_"<<delta_<< std::endl;
+			break;
+		case 1://robot aware
+			alpha_ = 1.0-epsilon;gamma_ = 1.0+epsilon;delta_ = 1.0+epsilon;
+			//std::cout << "case 1 :  alpha_="<<alpha_ <<"; gamma_="<<gamma_<<"; delta_"<<delta_<< std::endl;
+			break;
+		case 2://robot balanced
+		default:
+			alpha_ = 1.0;gamma_ = 1.0;delta_ = 1.0;
+			//std::cout << "case 2(default) :  alpha_="<<alpha_ <<"; gamma_="<<gamma_<<"; delta_"<<delta_<< std::endl;
+			break;
+		}
+
+		std::cout << alpha_ << " , " << gamma_ << " , " << delta_ << std::endl;
+	}*/
+
+	alpha_ = 1.0;gamma_ = 1.0;delta_ = 1.0;
+
+	// horizon_time_index_=5. salen 25 iter!!! 25*0.2segs=5segs => si va a 0.8m/s => 4m (ventana de 4m al rededor)
+
+	//std::cout << " Entro en Cprediction_behavior::scene_trajectory_prediction"<< std::endl;
+	//std::cout << " horizon_time_index_="<<horizon_time_index_<<"; person_list_.size()="<<person_list_.size()<<"; read_force_map_success_="<<read_force_map_success_<< std::endl;
+	//std::cout << " read_laser_obstacle_success_="<<read_laser_obstacle_success_<<"; robot_in_the_scene_="<<robot_in_the_scene_<<"; laser_obstacle_list_.empty()="<<laser_obstacle_list_.empty()<< std::endl;
+	SpointV_cov virtual_next_pose;
+	Sforce virtual_force_goal, virtual_force_int_person, virtual_force_obstacle, virtual_force_robot;
+	//std::cout <<  " 2 robot_group_trajectory_prediction " << std::endl;
+	/*if(is_act_person_companion_){
+		//std::cout <<  "3 IN robot_group_trajectory_prediction " << std::endl;
+		person_companion_->clear_prediction_trajectory();
+		//std::cout <<  "4 IN robot_group_trajectory_prediction " << std::endl;
+	}else{*/
+		//std::cout <<  "5 IN robot_group_trajectory_prediction " << std::endl;
+		robot_->clear_prediction_trajectory();
+		robot_->clear_prediction_trajectory_onlyV();
+		//std::cout <<  " 6 IN robot_group_trajectory_prediction " << std::endl;
+	//}
+
+
+
+	std::cout <<  "7 IN robot_group_trajectory_prediction " << std::endl;
+
+	for( unsigned int t = 0;  t < iteration_act; ++t)
+	{
+
+		// TODO: calular fuerzas robot respecto a personas y obstaculos y goal.
+		//iit->set_forces_person( virtual_force_goal, virtual_force_int_person, virtual_force_robot, virtual_force_obstacle );
+		//iit->prediction_propagation(dt_, iit->get_force_person() , t);
+		Sforce total_force;
+		//std::cout<<" (1) (Cplan_local_nav robot prediction) t="<<t<< std::endl;
+		Sforce f_goal;
+		Sforce f_int;
+		Sforce f_obs;
+
+		/*if(is_act_person_companion_){
+			//std::cout<<" (2) (Cplan_local_nav_person_companion robot prediction)"<< std::endl;
+
+			f_goal=person_companion_->force_goal(goal_to_predict_pose,get_sfm_params(person_companion_),&person_companion_->get_prediction_trajectory_with_target_person()->at(t));
+			//ok, al ser funcion interna del robot
+			//std::cout<<" (3) (Cplan_local_nav_person_companion robot prediction) person_companion_->get_prediction_trajectory_with_target_person().size()"<<person_companion_->get_prediction_trajectory_with_target_person()->size()<< std::endl;
+			//std::cout<<" (3) (Cplan_local_nav_person_companion robot prediction) f_goal.fx="<<f_goal.fx<<"; f_goal.fy="<<f_goal.fy<< std::endl;
+
+			f_int = force_persons_int_planning_virtual_companion_person_akp_person_prediction( person_companion_, t); // cambiada por la version para person companion
+
+			//map force, used for simulations
+			//std::cout<<" (4) (Cplan_local_nav_person_companion robot prediction) f_int.fx="<<f_int.fx<<"; f_int.fy="<<f_int.fy<< std::endl;
+
+			if( read_force_map_success_ )
+				f_obs = get_force_map(person_companion_->get_prediction_trajectory_with_target_person()->at(t).x, person_companion_->get_prediction_trajectory_with_target_person()->at(t).y);
+			//std::cout<<" (5) (Cplan_local_nav_person_companion robot prediction)"<< std::endl;
+
+			Spoint act_dest_robot(goal_to_predict_pose.x,goal_to_predict_pose.y,goal_to_predict_pose.time_stamp);
+
+			//std::cout<<" (6) (Cplan_local_nav_person_companion robot prediction)"<< std::endl;
+			double act_dist_goal_robot=person_companion_->get_current_pointV().distance(act_dest_robot);
+
+			//std::cout<<" (7) (Cplan_local_nav_person_companion robot prediction)"<< std::endl;
+			if(read_laser_obstacle_success_)
+				f_obs = force_objects_laser_int_planning_virtual_robot_propagation( person_companion_, t, act_dist_goal_robot, true ); //ok al usar el robot que le pasas desde fuera.
+
+			//std::cout<<" (8) (Cplan_local_nav_person_companion robot prediction) f_obs.fx="<<f_obs.fx<<"; f_obs.fy="<<f_obs.fy<< std::endl;
+
+		}else{*/
+			//std::cout<<" else (2) (Cplan_local_nav_person_companion robot prediction)"<< std::endl;
+			f_goal=robot_->force_goal(goal_to_predict_pose,get_sfm_params(robot_),&(robot_->get_prediction_trajectory_with_target_person()->at(t)));//&robot_->get_prediction_trajectory_with_target_person()->at(t));
+			//std::cout<<" else (2) (Cplan_local_nav_person_companion robot prediction), f_goal.x="<<f_goal.fx<<", f_goal.fy="<<f_goal.fy<<", robot_->get_prediction_trajectory_with_target_person().size="<<robot_->get_prediction_trajectory_with_target_person()->size()<< std::endl;
+			//std::cout<<" else (3) (Cplan_local_nav_person_companion robot prediction), id_person_comp="<<id_person_comp<< std::endl;
+			f_int = force_persons_int_planning_virtual_robot_prediction( robot_, t, 64.0, Cperson_abstract::Akp_planning, id_person_comp);
+			// Cperson_abstract* center , unsigned int t, double min_dist2=64.0, Cperson_abstract::companion_reactive reactive=Cperson_abstract::Akp_planning, unsigned int id_pers_comp_rob=0);
+			//std::cout<<" else (3) (Cplan_local_nav_person_companion robot prediction), f_int.x="<<f_int.fx<<", f_int.fy="<<f_int.fy<< std::endl;
+
+			//map force, used for simulations
+			if( read_force_map_success_ )
+				f_obs = get_force_map(robot_->get_prediction_trajectory_with_target_person()->at(t).x, robot_->get_prediction_trajectory_with_target_person()->at(t).y);
+
+			//std::cout<<" else (4) (Cplan_local_nav_person_companion robot prediction)"<< std::endl;
+			//obstacles due to laser scans. for real environments has priority over map forces
+			Spoint act_dest_robot(goal_to_predict_pose.x,goal_to_predict_pose.y,goal_to_predict_pose.time_stamp);
+
+			//std::cout<<" else (5) (Cplan_local_nav_person_companion robot prediction)"<< std::endl;
+			double act_dist_goal_robot=robot_->get_current_pointV().distance(act_dest_robot);
+
+			//std::cout<<" else (6) (Cplan_local_nav_person_companion robot prediction)"<< std::endl;
+
+			if(read_laser_obstacle_success_)
+				f_obs = force_objects_laser_int_planning_virtual_robot_propagation( robot_, t, act_dist_goal_robot, true );
+
+			//std::cout<<" else (7) (Cplan_local_nav_person_companion robot prediction), f_obs.x="<<f_obs.fx<<", f_obs.y="<<f_obs.fy<< std::endl;
+		//}
+
+
+		//std::cout<<" (8) (Cplan_local_nav_person_companion robot prediction)"<< std::endl;
+		// INICION Limitar fuerza obstaculos cuando hay muchos solapados
+		double fobos_Xmod=sqrt(f_obs.fx*f_obs.fx);
+		double fobos_Ymod=sqrt(f_obs.fy*f_obs.fy);
+		double signo_x;
+		double signo_y;
+
+		if (f_obs.fx > 0){
+			signo_x=1;
+		}else{
+			signo_x=-1;
+		}
+		if (f_obs.fy > 0){
+			signo_y=1;
+		}else{
+			signo_y=-1;
+		}
+		//std::cout<<" (9) (Cplan_local_nav_person_companion robot prediction)"<< std::endl;
+		if((fobos_Xmod>f_obst_max_x_)||(fobos_Ymod>f_obst_max_y_)){
+			f_obs.fx=signo_x*f_obst_max_x_;
+			f_obs.fy=signo_y*f_obst_max_y_;
+		}
+		//std::cout << "(FINAL FORCE) f_obs.fx="<<f_obs.fx<<"; f_obs.fy="<<f_obs.fy<< std::endl;
+		// FIN Limitar fuerza obstaculos cuando hay muchos solapados
+
+		// todo: mirar si se generan bien para este caso las alpha_ gamma_ y delta_
+		///std::cout<<" (10) alpha_="<<alpha_<<", gamma_="<<gamma_<<", delta_="<<delta_<< std::endl;
+		total_force=f_goal*alpha_ + f_int*gamma_ + f_obs*delta_;  // +f_persongoal*(param)  TODO: para la propagacion usa la f(global!!!), esta hay k incluirle el goal persona en la Sedge_tree_pcomp que le entra.
+
+
+		//std::cout<<" (11) total_force.fx="<<total_force.fx<<"; total_force.fy="<<total_force.fy<<"; f_goal.fx="<<f_goal.fx<<"; f_goal.fy="<<f_goal.fy<<"; f_int.fx="<<f_int.fx<<"; f_int.fy="<<f_int.fy<< std::endl;
+		/*if(is_act_person_companion_){
+			person_companion_->prediction_propagation(dt_, t, total_force,person_companion_->get_desired_velocity()); // ok, correcto, al ser funcion interna del robot
+		}else{*/
+			double max_person_companion_vel;
+
+			//for( auto iit : person_list_)
+			//{
+				//if(iit->get_id()==id_person_companion_){
+					//max_person_companion_vel=sqrt((iit->get_current_pointV().vx*iit->get_current_pointV().vx)+(iit->get_current_pointV().vy*iit->get_current_pointV().vy));
+					max_person_companion_vel=sqrt((actual_person_Companion_SpointV_.vx*actual_person_Companion_SpointV_.vx)+(actual_person_Companion_SpointV_.vy*actual_person_Companion_SpointV_.vy));
+
+					//std::cout<<" (9.1 actual) max_person_companion_vel="<<max_person_companion_vel<< std::endl;
+					max_person_companion_vel=max_desired_person_Companion_velocity_;//iit->get_desired_velocity();
+					//std::cout<<" (9.2 desired) max_person_companion_vel="<<max_person_companion_vel<< std::endl;
+				//}
+			//}
+			robot_->prediction_propagation(dt_, t, total_force,max_person_companion_vel);
+			//SpointV act_robot_prediction=robot_->get_prediction_trajectory_with_target_person()->back();
+			////////////////////////
+
+				//robot_->prediction_propagation2_only_vel(dt_, t, total_force,distance_person_t_x,distance_person_t_y,vel_person_x,vel_person_y,max_person_companion_vel);
+
+				//SpointV actual_group_point2_v2=robot_->get_propagated_pose_onlyV_trajectory_with_target_person()->at(t);
+				//double act_desired_vel_robot=max_person_companion_vel;//robot_->get_desired_velocity();
+				//SpointV act_robot_pose_with_v_calc=actual_group_point2_v2.propagate2_comp_only_vel(dt_, total_force,act_desired_vel_robot ,distance_person_t_x,distance_person_t_y,vel_person_x,vel_person_y);
+				//SpointV act_robot_pose_with_v_calc2=robot_->get_propagated_pose_onlyV_trajectory_with_target_person()->back();
+
+
+			//}
+			//////////////////////////
+
+		//}
+
+
+		//std::cout<<" (13) (Cplan_local_nav_person_companion robot prediction)"<< std::endl;
+
+		//std::cout << " total_force.fx= "<<total_force.fx<< " total_force.fy="<< total_force.fy <<"; robot_predict.x="<<robot_->get_prediction_trajectory_with_target_person()->back().x<<"; robot_prediction.y="<<robot_->get_prediction_trajectory_with_target_person()->back().y<<  std::endl;
+
+	//	std::cout<<" t="<<t<< std::endl;
+	}
+
+	/*if(debug_target_person_goal_){
+		std::cout<<" Cplan_local_nav_person_companion robot prediction) scene_trajectory_prediction!  new_horizon_time="<<new_horizon_time<< std::endl;
+	}*/
+
+
+
+	//std::cout << "Salgo de robot_trajectory_prediction_for_group_Zanlungo; robot_->pred_traj_size="<<robot_->get_prediction_trajectory_with_target_person()->size()<<"; traj.size="<<robot_->get_prediction_trajectory()->size()<< std::endl;
+}
+
+
+
+void Cplan_local_nav_person_companion::robot_group_trajectory_prediction( double new_horizon_time, Sdestination goal_to_predict_pose)
+{
+	//std::cout <<  " IN robot_group_trajectory_prediction " << std::endl;
+
+	/*std::cout <<  " IN robot_group_trajectory_prediction " << std::endl;
+				for( auto iit : person_list_)
+				{
+					if(iit->get_id()!=id_person_companion_){
+					 std::cout << " (robot_group_trajectory_prediction) (1) iit->get_prediction_trajectory( )->size()="<<iit->get_prediction_trajectory( )->size()<<"; iit->get_id()=" <<iit->get_id()<<"; iit->desired_vel="<<iit->get_desired_velocity()<<  std::endl;
+					}
+				}*/
+
+	// TOD: de momento igual k en planning akp, se ponen el alpha_ gamma_ y delta:
+	//robot parameters sampling if required
+	//set the random alpha variable, if necessary
+	/*
+	 * actual_person_pposition_prop;
+	 * if( plan_mode_ == F_RRT_GC_alpha )
+	{
+
+		std::uniform_int_distribution<int> sample_behavior( 0 , 2 );
+		double epsilon(0.4);
+		switch( sample_behavior(generator_) )
+		{
+		case 0://robot unaware
+			alpha_ = 1.0+epsilon;gamma_ = 1.0-epsilon;delta_ = 1.0-epsilon;
+			//std::cout << "case 0 :  alpha_="<<alpha_ <<"; gamma_="<<gamma_<<"; delta_"<<delta_<< std::endl;
+			break;
+		case 1://robot aware
+			alpha_ = 1.0-epsilon;gamma_ = 1.0+epsilon;delta_ = 1.0+epsilon;
+			//std::cout << "case 1 :  alpha_="<<alpha_ <<"; gamma_="<<gamma_<<"; delta_"<<delta_<< std::endl;
+			break;
+		case 2://robot balanced
+		default:
+			alpha_ = 1.0;gamma_ = 1.0;delta_ = 1.0;
+			//std::cout << "case 2(default) :  alpha_="<<alpha_ <<"; gamma_="<<gamma_<<"; delta_"<<delta_<< std::endl;
+			break;
+		}
+
+		std::cout << alpha_ << " , " << gamma_ << " , " << delta_ << std::endl;
+	}*/
+
+	alpha_ = 1.0;gamma_ = 1.0;delta_ = 1.0;
+
+	// horizon_time_index_=5. salen 25 iter!!! 25*0.2segs=5segs => si va a 0.8m/s => 4m (ventana de 4m al rededor)
+
+	//std::cout << " Entro en Cprediction_behavior::scene_trajectory_prediction"<< std::endl;
+	//std::cout << " horizon_time_index_="<<horizon_time_index_<<"; person_list_.size()="<<person_list_.size()<<"; read_force_map_success_="<<read_force_map_success_<< std::endl;
+	//std::cout << " read_laser_obstacle_success_="<<read_laser_obstacle_success_<<"; robot_in_the_scene_="<<robot_in_the_scene_<<"; laser_obstacle_list_.empty()="<<laser_obstacle_list_.empty()<< std::endl;
+	SpointV_cov virtual_next_pose;
+	Sforce virtual_force_goal, virtual_force_int_person, virtual_force_obstacle, virtual_force_robot;
+	//std::cout <<  " 2 robot_group_trajectory_prediction " << std::endl;
+	if(is_act_person_companion_){
+		//std::cout <<  "3 IN robot_group_trajectory_prediction " << std::endl;
+		person_companion_->clear_prediction_trajectory();
+		//std::cout <<  "4 IN robot_group_trajectory_prediction " << std::endl;
+	}else{
+		//std::cout <<  "5 IN robot_group_trajectory_prediction " << std::endl;
+		robot_->clear_prediction_trajectory();
+		robot_->clear_prediction_trajectory_onlyV();
+		//std::cout <<  " 6 IN robot_group_trajectory_prediction " << std::endl;
+	}
+
+
+
+	//std::cout <<  "7 IN robot_group_trajectory_prediction " << std::endl;
+
+	for( unsigned int t = 0;  t < new_horizon_time; ++t)
+	{
+
+		// TODO: calular fuerzas robot respecto a personas y obstaculos y goal.
+		//iit->set_forces_person( virtual_force_goal, virtual_force_int_person, virtual_force_robot, virtual_force_obstacle );
+		//iit->prediction_propagation(dt_, iit->get_force_person() , t);
+		Sforce total_force;
+		//std::cout<<" (1) (Cplan_local_nav_person_companion robot prediction) t="<<t<< std::endl;
+		Sforce f_goal;
+		Sforce f_int;
+		Sforce f_obs;
+
+		if(is_act_person_companion_){
+			//std::cout<<" (2) (Cplan_local_nav_person_companion robot prediction)"<< std::endl;
+
+			f_goal=person_companion_->force_goal(goal_to_predict_pose,get_sfm_params(person_companion_),&person_companion_->get_prediction_trajectory_with_target_person()->at(t));
+			//ok, al ser funcion interna del robot
+			//std::cout<<" (3) (Cplan_local_nav_person_companion robot prediction) person_companion_->get_prediction_trajectory_with_target_person().size()"<<person_companion_->get_prediction_trajectory_with_target_person()->size()<< std::endl;
+			//std::cout<<" (3) (Cplan_local_nav_person_companion robot prediction) f_goal.fx="<<f_goal.fx<<"; f_goal.fy="<<f_goal.fy<< std::endl;
+
+			f_int = force_persons_int_planning_virtual_companion_person_akp_person_prediction( person_companion_, t); // cambiada por la version para person companion
+
+			//map force, used for simulations
+			//std::cout<<" (4) (Cplan_local_nav_person_companion robot prediction) f_int.fx="<<f_int.fx<<"; f_int.fy="<<f_int.fy<< std::endl;
+
+			if( read_force_map_success_ )
+				f_obs = get_force_map(person_companion_->get_prediction_trajectory_with_target_person()->at(t).x, person_companion_->get_prediction_trajectory_with_target_person()->at(t).y);
+			//std::cout<<" (5) (Cplan_local_nav_person_companion robot prediction)"<< std::endl;
+
+			Spoint act_dest_robot(goal_to_predict_pose.x,goal_to_predict_pose.y,goal_to_predict_pose.time_stamp);
+
+			//std::cout<<" (6) (Cplan_local_nav_person_companion robot prediction)"<< std::endl;
+			double act_dist_goal_robot=person_companion_->get_current_pointV().distance(act_dest_robot);
+
+			//std::cout<<" (7) (Cplan_local_nav_person_companion robot prediction)"<< std::endl;
+			if(read_laser_obstacle_success_)
+				f_obs = force_objects_laser_int_planning_virtual_robot_propagation( person_companion_, t, act_dist_goal_robot, true ); //ok al usar el robot que le pasas desde fuera.
+
+			//std::cout<<" (8) (Cplan_local_nav_person_companion robot prediction) f_obs.fx="<<f_obs.fx<<"; f_obs.fy="<<f_obs.fy<< std::endl;
+
+		}else{
+			//std::cout<<" else (2) (Cplan_local_nav_person_companion robot prediction)"<< std::endl;
+			f_goal=robot_->force_goal(goal_to_predict_pose,get_sfm_params(robot_),&robot_->get_prediction_trajectory_with_target_person()->at(t));
+			//std::cout<<" else (2) (Cplan_local_nav_person_companion robot prediction)"<< std::endl;
+
+			f_int = force_persons_int_planning_virtual_robot_prediction( robot_, t);
+			//std::cout<<" else (3) (Cplan_local_nav_person_companion robot prediction) ="<< std::endl;
+
+			//map force, used for simulations
+			if( read_force_map_success_ )
+				f_obs = get_force_map(robot_->get_prediction_trajectory_with_target_person()->at(t).x, robot_->get_prediction_trajectory_with_target_person()->at(t).y);
+
+			//std::cout<<" else (4) (Cplan_local_nav_person_companion robot prediction)"<< std::endl;
+			//obstacles due to laser scans. for real environments has priority over map forces
+			Spoint act_dest_robot(goal_to_predict_pose.x,goal_to_predict_pose.y,goal_to_predict_pose.time_stamp);
+
+			//std::cout<<" else (5) (Cplan_local_nav_person_companion robot prediction)"<< std::endl;
+			double act_dist_goal_robot=robot_->get_current_pointV().distance(act_dest_robot);
+
+			//std::cout<<" else (6) (Cplan_local_nav_person_companion robot prediction)"<< std::endl;
+
+			if(read_laser_obstacle_success_)
+				f_obs = force_objects_laser_int_planning_virtual_robot_propagation( robot_, t, act_dist_goal_robot, true );
+			//std::cout<<" else (7) (Cplan_local_nav_person_companion robot prediction)"<< std::endl;
+		}
+
+
+		//std::cout<<" (8) (Cplan_local_nav_person_companion robot prediction)"<< std::endl;
+		// INICION Limitar fuerza obstaculos cuando hay muchos solapados
+		double fobos_Xmod=sqrt(f_obs.fx*f_obs.fx);
+		double fobos_Ymod=sqrt(f_obs.fy*f_obs.fy);
+		double signo_x;
+		double signo_y;
+
+		if (f_obs.fx > 0){
+			signo_x=1;
+		}else{
+			signo_x=-1;
+		}
+		if (f_obs.fy > 0){
+			signo_y=1;
+		}else{
+			signo_y=-1;
+		}
+		//std::cout<<" (9) (Cplan_local_nav_person_companion robot prediction)"<< std::endl;
+		if((fobos_Xmod>f_obst_max_x_)||(fobos_Ymod>f_obst_max_y_)){
+			f_obs.fx=signo_x*f_obst_max_x_;
+			f_obs.fy=signo_y*f_obst_max_y_;
+		}
+		//std::cout << "(FINAL FORCE) f_obs.fx="<<f_obs.fx<<"; f_obs.fy="<<f_obs.fy<< std::endl;
+		// FIN Limitar fuerza obstaculos cuando hay muchos solapados
+
+		// todo: mirar si se generan bien para este caso las alpha_ gamma_ y delta_
+		//std::cout<<" (10) (Cplan_local_nav_person_companion robot prediction)"<< std::endl;
+		total_force=f_goal*alpha_ + f_int*gamma_ + f_obs*delta_;  // +f_persongoal*(param)  TODO: para la propagacion usa la f(global!!!), esta hay k incluirle el goal persona en la Sedge_tree_pcomp que le entra.
+
+
+		//std::cout<<" (11) total_force.fx="<<total_force.fx<<"; total_force.fy="<<total_force.fy<<"; f_goal.fx="<<f_goal.fx<<"; f_goal.fy="<<f_goal.fy<<"; f_int.fx="<<f_int.fx<<"; f_int.fy="<<f_int.fy<< std::endl;
+		std::cout<<" (9.2 desired) is_act_person_companion_="<<is_act_person_companion_<< std::endl;
+		if(is_act_person_companion_){
+			person_companion_->prediction_propagation(dt_, t, total_force,person_companion_->get_desired_velocity()); // ok, correcto, al ser funcion interna del robot
+		}else{
+			double max_person_companion_vel;
+
+			//for( auto iit : person_list_)
+			//{
+				//if(iit->get_id()==id_person_companion_){
+					//max_person_companion_vel=sqrt((iit->get_current_pointV().vx*iit->get_current_pointV().vx)+(iit->get_current_pointV().vy*iit->get_current_pointV().vy));
+					max_person_companion_vel=sqrt((actual_person_Companion_SpointV_.vx*actual_person_Companion_SpointV_.vx)+(actual_person_Companion_SpointV_.vy*actual_person_Companion_SpointV_.vy));
+
+					//std::cout<<" (9.1 actual) max_person_companion_vel="<<max_person_companion_vel<< std::endl;
+					max_person_companion_vel=max_desired_person_Companion_velocity_;//iit->get_desired_velocity();
+					//std::cout<<" (9.2 desired) max_person_companion_vel="<<max_person_companion_vel<< std::endl;
+				//}
+			//}
+			robot_->prediction_propagation(dt_, t, total_force,max_person_companion_vel);
+			//SpointV act_robot_prediction=robot_->get_prediction_trajectory_with_target_person()->back();
+			////////////////////////
+
+				//robot_->prediction_propagation2_only_vel(dt_, t, total_force,distance_person_t_x,distance_person_t_y,vel_person_x,vel_person_y,max_person_companion_vel);
+
+				//SpointV actual_group_point2_v2=robot_->get_propagated_pose_onlyV_trajectory_with_target_person()->at(t);
+				//double act_desired_vel_robot=max_person_companion_vel;//robot_->get_desired_velocity();
+				//SpointV act_robot_pose_with_v_calc=actual_group_point2_v2.propagate2_comp_only_vel(dt_, total_force,act_desired_vel_robot ,distance_person_t_x,distance_person_t_y,vel_person_x,vel_person_y);
+				//SpointV act_robot_pose_with_v_calc2=robot_->get_propagated_pose_onlyV_trajectory_with_target_person()->back();
+
+
+			//}
+			//////////////////////////
+
+		}
+
+
+		//std::cout<<" (13) (Cplan_local_nav_person_companion robot prediction)"<< std::endl;
+
+		//std::cout << " total_force.fx= "<<total_force.fx<< " total_force.fy="<< total_force.fy <<"; robot_predict.x="<<robot_->get_prediction_trajectory_with_target_person()->back().x<<"; robot_prediction.y="<<robot_->get_prediction_trajectory_with_target_person()->back().y<<  std::endl;
+
+	//	std::cout<<" t="<<t<< std::endl;
+	}
+
+	/*if(debug_target_person_goal_){
+		std::cout<<" Cplan_local_nav_person_companion robot prediction) scene_trajectory_prediction!  new_horizon_time="<<new_horizon_time<< std::endl;
+	}*/
+
+
+
+	//std::cout << "Salgo de scene_trajectory_prediction"<< std::endl;
+}
+
+
+
+
+double Cplan_local_nav_person_companion::calc_person_companion_orientation(){ // return theta person companion in radians!
+
+	if(debug_gazebo_journal2_){
+		std::cout <<"!!!!!!!!!!!!!!!INNNNNNNNNNNNNNN!!!!!!!!! calc_person_companion_orientation"<< std::endl;
+	}
+	/////////////
+	// ini calcular nuevo angulo theta con orientación dirección movimiento persona.
+    Cperson_abstract* person_obj=pointer_to_person_companion_;
+   // Cperson_abstract* person_obj2=second_group_companion_person_obj_;
+
+    Cperson_abstract* final_person_obj;
+    //std::cout <<"2 !!!!!!!!!!!!!!!INNNNNNNNNNNNNNN!!!!!!!!! calc_person_companion_orientation"<< std::endl;
+    //bool finded_person=find_person(id_person_companion_ , &person_obj);
+   // find_person(id_person_companion_ , &person_obj);
+    double person_vel;
+    Sdestination person_best_dest;//=person_obj->get_best_dest();
+	SpointV_cov person;
+
+	//Spoint robot_point=robot_->get_current_pointV();
+
+	/*if(Zanlungo_model_){
+
+		if((number_of_group_people_>1)&&(we_have_pointer_to_second_person_)){
+
+			double dist_to_pers1= person_obj->get_current_pointV().distance(robot_point);
+			//double dist_to_pers2= person_obj2->get_current_pointV().distance(robot_point);
+			//std::cout << "  dist_to_pers1="<<dist_to_pers1<<"; dist_to_pers2="<<dist_to_pers2<< std::endl;
+			//if(dist_to_pers1<dist_to_pers2){
+				person=person_obj->get_current_pointV();
+				person_best_dest=person_obj->get_best_dest();
+				person_vel=person_obj->get_current_pointV().v();
+				final_person_obj=person_obj;
+			//}else{
+			///	person=person_obj2->get_current_pointV();
+			//	person_best_dest=person_obj2->get_best_dest();
+			//	person_vel=person_obj2->get_current_pointV().v();
+			//	final_person_obj=person_obj2;
+			//}
+
+
+		}else{
+
+			person= person_obj->get_current_pointV();
+			person_best_dest=person_obj->get_best_dest();
+			person_vel=person_obj->get_current_pointV().v();
+			final_person_obj=person_obj;
+		}
+
+
+
+	}else{*/
+		// para side-by-side:
+		person= person_obj->get_current_pointV();
+		person_best_dest=person_obj->get_best_dest();
+		person_vel=person_obj->get_current_pointV().v();
+		final_person_obj=person_obj;
+	//}
+
+
+    //SpointV_cov person = person_obj->get_current_pointV();
+   // std::cout <<"3 !!!!!!!!!!!!!!!INNNNNNNNNNNNNNN!!!!!!!!! calc_person_companion_orientation"<< std::endl;
+	double x;
+	double y;
+	double pers_dx;
+	double pers_dy;
+	//double theta_pers;
+	double theta=0;
+	//std::cout <<"4!!!!!!!!!!!!!!!INNNNNNNNNNNNNNN!!!!!!!!! calc_person_companion_orientation"<< std::endl;
+
+
+	if(debug_gazebo_journal2_){
+		std::cout <<"(best person destination) person_best_dest.x"<<person_best_dest.x<<"; person_best_dest.y="<<person_best_dest.y<< std::endl;
+		//std::cout <<"(center person robot) center_of_the_group_.x"<<center_of_the_group_.x<<"; center_of_the_group_.y="<<center_of_the_group_.y<< std::endl;
+	}
+
+bool use_before_theta=false;
+
+
+	if(person_vel<threshold_min_vel_person_to_obtain_destination_){ //actual_current_person_comp_point.v()<threshold_min_vel_person_to_obtain_destination_
+
+		if(debug_gazebo_journal2_){
+			std::cout <<"(in) threshold_min_vel_person_to_obtain_destination_="<<threshold_min_vel_person_to_obtain_destination_<<" person_vel="<<person_vel<< std::endl;
+		}
+		//pers_dx=sqrt(person_obj->get_current_pointV().vx*person_obj->get_current_pointV().vx);
+		//pers_dy=sqrt(person_obj->get_current_pointV().vy*person_obj->get_current_pointV().vy);
+		// ASUMO QUE LA ORIENTACION DEL ROBOT ES LA BUENA, CUANDO PERSON COMPANION VELOCITY==0!!! (	NO HAY FORMA MEJOR SIN SABER LA ORIENTACION REAL.)
+		//if(debug_correct_angle_person_init_robot2_){std::cout <<" dx="<<pers_dx<<"; dy="<<pers_dy<< std::endl;}
+		//theta_pers=last_good_theta_person_;
+
+
+		theta=last_good_theta_person_;
+		//if(debug_gazebo_journal2_){
+		//	std::cout <<"person_obj->get_current_pointV().v()"<<final_person_obj->get_current_pointV().v()<<"; threshold_min_vel="<<threshold_min_vel_person_to_obtain_destination_<< std::endl;
+		//	std::cout <<"theta (robot_theta)"<<theta*180/3.14<< std::endl;
+		//}
+
+		use_before_theta=true;
+
+	}else{
+		if(debug_gazebo_journal2_){
+			std::cout <<"(in else) threshold_min_vel_person_to_obtain_destination_="<<threshold_min_vel_person_to_obtain_destination_<< std::endl;
+		}
+
+
+		//pers_dx=person.x-x;
+		//pers_dy=person.y-y;
+		if((final_person_obj->get_best_dest().x==person.x)&&(final_person_obj->get_best_dest().y==person.y)){
+			//x=final_person_obj->get_past_trajectory()->at(0).x;
+			//y=final_person_obj->get_past_trajectory()->at(0).y;
+			if(final_person_obj->get_past_trajectory()->size()>num_steps_orientation_){
+				if(debug_correct_angle_person_init_robot2_){std::cout <<" perso_print_back_traj(size-num_steps_orientation_)="<< std::endl;
+				final_person_obj->get_past_trajectory()->at(final_person_obj->get_past_trajectory()->size()-num_steps_orientation_).print();}
+				x=final_person_obj->get_past_trajectory()->at(final_person_obj->get_past_trajectory()->size()-num_steps_orientation_).x;
+				y=final_person_obj->get_past_trajectory()->at(final_person_obj->get_past_trajectory()->size()-num_steps_orientation_).y;
+			}else{
+				if(debug_correct_angle_person_init_robot2_){std::cout <<" perso_print_back_traj(0)="<< std::endl;
+				final_person_obj->get_past_trajectory()->at(0).print();}
+				x=final_person_obj->get_past_trajectory()->at(0).x;
+				y=final_person_obj->get_past_trajectory()->at(0).y;
+			}
+
+			//if(last_good_theta_person_!=-1000){
+			//	use_before_theta=true;
+			//}
+
+		}else{
+			x=final_person_obj->get_best_dest().x;
+			y=final_person_obj->get_best_dest().y;
+		}
+
+		//x=person_obj->get_prediction_trajectory()->back().x;
+		//y=person_obj->get_prediction_trajectory()->back().y;
+		//std::cout <<" final_person_obj->get_best_dest().x="<<final_person_obj->get_best_dest().x<<"; final_person_obj->get_best_dest().y="<<final_person_obj->get_best_dest().y<<"; person.x="<<person.x<<"; x="<<x<<"; person.y="<<person.y<< std::endl;
+
+		if((final_person_obj->get_best_dest().x==person.x)&&(final_person_obj->get_best_dest().y==person.y)){
+			//std::cout <<" num_steps_orientation_="<<num_steps_orientation_<<"; person.x="<<person.x<<"; person.y="<<person.y<<"; x="<<x<<"; y="<<y<< std::endl;
+			pers_dx=person.x-x;//person_obj->get_current_pointV().vx;
+			pers_dy=person.y-y;//person_obj->get_current_pointV().vy;
+		}else{
+			pers_dx=x-person.x;//person_obj->get_current_pointV().vx;
+			pers_dy=y-person.y;//person_obj->get_current_pointV().vy;
+		}
+
+
+		ori_pers_x_=pers_dx;
+		ori_pers_y_=pers_dy;
+
+
+		if(debug_gazebo_journal2_){
+			std::cout <<"pers_dx="<<pers_dx<<"; pers_dy="<<pers_dy<<"; x="<<x<<"; y="<<y<<"; person.x="<<person.x<<"; person.y="<<person.y<< std::endl;
+		}
+
+		if(debug_correct_angle_person_init_robot2_){std::cout <<" dx="<<pers_dx<<"; dy="<<pers_dy<< std::endl;}
+		//theta_pers=atan2(pers_dy , pers_dx);
+		theta=atan2(pers_dy , pers_dx);
+		//theta=-1.57;
+		last_good_theta_person_=theta;
+		before_person_comp_goal_=final_person_obj->get_best_dest();
+
+
+		if(use_before_theta){
+			theta=last_good_theta_person_;
+		}
+
+			/*        	    double inc_angle=0;
+			        	    double inc_angle1=0;
+			        		double media_ponderada=0; //media ponderada que pesen más los angulos finales, para que vaya mejor en el giro.
+			        		double media_ponderada2=0; //media ponderada que pesen más los angulos finales, para que vaya mejor en el giro.
+			        	    if(person_obj->get_past_trajectory()->size()>0){
+								//std::cout <<" [INI] media_ponderada="<< std::endl;
+								for(unsigned int f=person_obj->get_past_trajectory()->size()-1; f>0;f--){
+									double x1=person_obj->get_past_trajectory()->at(f).x;
+									double y1=person_obj->get_past_trajectory()->at(f).y;
+									double x2=person_obj->get_past_trajectory()->at(f-1).x;
+									double y2=person_obj->get_past_trajectory()->at(f-1).y;
+
+									double d_X=(x1-x2);
+									double d_Y=(y1-y2);
+									inc_angle=inc_angle+atan2(d_Y , d_X)*f;
+									inc_angle1=inc_angle1+atan2(d_Y , d_X);
+								}
+								media_ponderada=inc_angle/person_obj->get_past_trajectory()->size();
+								media_ponderada2=inc_angle1/person_obj->get_past_trajectory()->size();
+								// std::cout <<" [FIN] media_ponderada="<< media_ponderada*(180/3.14)<< std::endl;
+								// std::cout <<" [FIN] media_ponderada2="<< media_ponderada2*(180/3.14)<< std::endl;
+			        	    }else{
+			        	    	 std::cout <<" caso no contemplado"<< std::endl;
+			        	    }
+
+
+
+			        	    theta=media_ponderada2;
+*/
+			        	  //  std::cout <<"; size="<<person_obj->get_past_trajectory()->size()<<"theta (calc_theta)"<<theta*180/3.14<< std::endl;
+
+
+
+
+
+
+		if(debug_gazebo_journal2_){
+			std::cout <<"person_obj->get_current_pointV().v()"<<person_obj->get_current_pointV().v()<<"; threshold_min_vel="<<threshold_min_vel_person_to_obtain_destination_<< std::endl;
+			std::cout <<"; size="<<person_obj->get_past_trajectory()->size()<<"theta (calc_theta)"<<theta*180/3.14<< std::endl;
+		}
+
+	}
+
+
+	if(theta<0){
+		theta=2*3.14+theta;
+	}
+	if(debug_gazebo_journal2_){
+		std::cout <<"[FINAL theta!!!] theta ="<<theta*180/3.14<< std::endl;
+	}
+	return theta;
+
+////////////////
+
+}
+
+
+
+
+
+double Cplan_local_nav_person_companion::calc_person_companion_orientation_from_outside( SpointV actual_p1_point, Sdestination person_dest, Spose in_robot){ // return theta person companion in radians!
+
+	//if(debug_gazebo_journal2_){
+		//std::cout <<"!!!!!!!!!!!!!!!INNNNNNNNNNNNNNN!!!!!!!!! calc_person_companion_orientation"<< std::endl;
+	//}
+	/////////////
+	// ini calcular nuevo angulo theta con orientación dirección movimiento persona.
+
+    //Cperson_abstract* final_person_obj=person_obj;
+    //std::cout <<"2 !!!!!!!!!!!!!!!INNNNNNNNNNNNNNN!!!!!!!!! calc_person_companion_orientation"<< std::endl;
+    //bool finded_person=find_person(id_person_companion_ , &person_obj);
+   // find_person(id_person_companion_ , &person_obj);
+    double person_vel;
+    Sdestination person_best_dest;//=person_obj->get_best_dest();
+	SpointV_cov person;
+
+	//Spoint robot_point=Spoint(in_robot.x,in_robot.y,in_robot.time_stamp);//in_robot->get_current_pointV();
+	// std::cout <<"3 !!!!!!!!!!!!!!!INNNNNNNNNNNNNNN!!!!!!!!! calc_person_companion_orientation"<< std::endl;
+	//double dist_to_pers1= actual_p1_point.distance(robot_point);
+
+	//std::cout << "  dist_to_pers1="<<dist_to_pers1<<"; dist_to_pers2="<<dist_to_pers2<< std::endl;
+	person=actual_p1_point;
+	// std::cout <<"4 !!!!!!!!!!!!!!!INNNNNNNNNNNNNNN!!!!!!!!! calc_person_companion_orientation"<< std::endl;
+	person_best_dest=person_dest;
+	// std::cout <<"5 !!!!!!!!!!!!!!!INNNNNNNNNNNNNNN!!!!!!!!! calc_person_companion_orientation"<< std::endl;
+	person_vel=actual_p1_point.v();
+	// std::cout <<"6 !!!!!!!!!!!!!!!INNNNNNNNNNNNNNN!!!!!!!!! calc_person_companion_orientation"<< std::endl;
+	//final_person_obj=person_obj;
+
+    //SpointV_cov person = person_obj->get_current_pointV();
+   // std::cout <<"7 !!!!!!!!!!!!!!!INNNNNNNNNNNNNNN!!!!!!!!! calc_person_companion_orientation"<< std::endl;
+	double x;
+	double y;
+	double pers_dx;
+	double pers_dy;
+	//double theta_pers;
+	double theta=0;
+	//std::cout <<"8!!!!!!!!!!!!!!!INNNNNNNNNNNNNNN!!!!!!!!! calc_person_companion_orientation"<< std::endl;
+
+
+	if(debug_gazebo_journal2_){
+		std::cout <<"(best person destination) person_best_dest.x"<<person_best_dest.x<<"; person_best_dest.y="<<person_best_dest.y<< std::endl;
+		//std::cout <<"(center person robot) center_of_the_group_.x"<<center_of_the_group_.x<<"; center_of_the_group_.y="<<center_of_the_group_.y<< std::endl;
+	}
+
+
+
+
+	if(person_vel<threshold_min_vel_person_to_obtain_destination_){ //actual_current_person_comp_point.v()<threshold_min_vel_person_to_obtain_destination_
+
+		//if(debug_gazebo_journal2_){
+			std::cout <<"(in) threshold_min_vel_person_to_obtain_destination_="<<threshold_min_vel_person_to_obtain_destination_<< std::endl;
+		//}
+		//pers_dx=sqrt(person_obj->get_current_pointV().vx*person_obj->get_current_pointV().vx);
+		//pers_dy=sqrt(person_obj->get_current_pointV().vy*person_obj->get_current_pointV().vy);
+		// ASUMO QUE LA ORIENTACION DEL ROBOT ES LA BUENA, CUANDO PERSON COMPANION VELOCITY==0!!! (	NO HAY FORMA MEJOR SIN SABER LA ORIENTACION REAL.)
+		//if(debug_correct_angle_person_init_robot2_){std::cout <<" dx="<<pers_dx<<"; dy="<<pers_dy<< std::endl;}
+		//theta_pers=last_good_theta_person_;
+		theta=last_good_theta_person_;
+		if(debug_gazebo_journal2_){
+			//std::cout <<"person_obj->get_current_pointV().v()"<<final_person_obj->get_current_pointV().v()<<"; threshold_min_vel="<<threshold_min_vel_person_to_obtain_destination_<< std::endl;
+			std::cout <<"theta (robot_theta)"<<theta*180/3.14<< std::endl;
+		}
+
+
+
+	}else{
+		//if(debug_gazebo_journal2_){
+			std::cout <<"(in else) threshold_min_vel_person_to_obtain_destination_="<<threshold_min_vel_person_to_obtain_destination_<< std::endl;
+		//}
+
+		/*if(final_person_obj->get_past_trajectory()->size()>num_steps_orientation_){
+			if(debug_correct_angle_person_init_robot2_){std::cout <<" perso_print_back_traj(size-num_steps_orientation_)="<< std::endl;
+			final_person_obj->get_past_trajectory()->at(final_person_obj->get_past_trajectory()->size()-num_steps_orientation_).print();}
+			x=final_person_obj->get_past_trajectory()->at(final_person_obj->get_past_trajectory()->size()-num_steps_orientation_).x;
+			y=final_person_obj->get_past_trajectory()->at(final_person_obj->get_past_trajectory()->size()-num_steps_orientation_).y;
+		}else{
+			if(debug_correct_angle_person_init_robot2_){std::cout <<" perso_print_back_traj(0)="<< std::endl;
+			final_person_obj->get_past_trajectory()->at(0).print();}
+			x=final_person_obj->get_past_trajectory()->at(0).x;
+			y=final_person_obj->get_past_trajectory()->at(0).y;
+		}*/
+		//pers_dx=person.x-x;
+		//pers_dy=person.y-y;
+		x=person_dest.x;
+		y=person_dest.y;
+		//x=person_obj->get_prediction_trajectory()->back().x;
+		//y=person_obj->get_prediction_trajectory()->back().y;
+		pers_dx=x-person.x;//person_obj->get_current_pointV().vx;
+		pers_dy=y-person.y;//person_obj->get_current_pointV().vy;
+
+		ori_pers_x_=pers_dx;
+		ori_pers_y_=pers_dy;
+
+
+		//if(debug_gazebo_journal2_){
+			std::cout <<"pers_dx="<<pers_dx<<"; pers_dy="<<pers_dy<<"; x="<<x<<"; y="<<y<<"; person.x="<<person.x<<"; person.y="<<person.y<< std::endl;
+		//}
+
+		if(debug_correct_angle_person_init_robot2_){std::cout <<" dx="<<pers_dx<<"; dy="<<pers_dy<< std::endl;}
+		//theta_pers=atan2(pers_dy , pers_dx);
+		theta=atan2(pers_dy , pers_dx);
+		//theta=-1.57;
+		last_good_theta_person_=theta;
+
+
+
+			/*        	    double inc_angle=0;
+			        	    double inc_angle1=0;
+			        		double media_ponderada=0; //media ponderada que pesen más los angulos finales, para que vaya mejor en el giro.
+			        		double media_ponderada2=0; //media ponderada que pesen más los angulos finales, para que vaya mejor en el giro.
+			        	    if(person_obj->get_past_trajectory()->size()>0){
+								//std::cout <<" [INI] media_ponderada="<< std::endl;
+								for(unsigned int f=person_obj->get_past_trajectory()->size()-1; f>0;f--){
+									double x1=person_obj->get_past_trajectory()->at(f).x;
+									double y1=person_obj->get_past_trajectory()->at(f).y;
+									double x2=person_obj->get_past_trajectory()->at(f-1).x;
+									double y2=person_obj->get_past_trajectory()->at(f-1).y;
+
+									double d_X=(x1-x2);
+									double d_Y=(y1-y2);
+									inc_angle=inc_angle+atan2(d_Y , d_X)*f;
+									inc_angle1=inc_angle1+atan2(d_Y , d_X);
+								}
+								media_ponderada=inc_angle/person_obj->get_past_trajectory()->size();
+								media_ponderada2=inc_angle1/person_obj->get_past_trajectory()->size();
+								// std::cout <<" [FIN] media_ponderada="<< media_ponderada*(180/3.14)<< std::endl;
+								// std::cout <<" [FIN] media_ponderada2="<< media_ponderada2*(180/3.14)<< std::endl;
+			        	    }else{
+			        	    	 std::cout <<" caso no contemplado"<< std::endl;
+			        	    }
+
+
+
+			        	    theta=media_ponderada2;
+*/
+			        	  //  std::cout <<"; size="<<person_obj->get_past_trajectory()->size()<<"theta (calc_theta)"<<theta*180/3.14<< std::endl;
+
+
+
+
+
+
+		if(debug_gazebo_journal2_){
+			//std::cout <<"person_obj->get_current_pointV().v()"<<person_obj->get_current_pointV().v()<<"; threshold_min_vel="<<threshold_min_vel_person_to_obtain_destination_<< std::endl;
+		//	std::cout <<"; size="<<person_obj->get_past_trajectory()->size()<<"theta (calc_theta)"<<theta*180/3.14<< std::endl;
+		}
+
+	}
+	std::cout <<"1 [FINAL theta!!!] theta ="<<theta*180/3.14<< std::endl;
+
+	if(theta<0){
+		theta=2*3.14+theta;
+	}
+	//if(debug_gazebo_journal2_){
+		std::cout <<"[FINAL theta!!!] theta ="<<theta*180/3.14<< std::endl;
+	//}
+	return theta;
+
+////////////////
+
+}
+
+
+Sdestination Cplan_local_nav_person_companion::calculate_companion_goal_stable_if_person_stop(double in_act_min_companion_angle){
+//////////
+	if(debug_gazebo_journal_){
+		std::cout << " IDEAL FINAL ACT in_act_min_companion_angle="<<in_act_min_companion_angle<< std::endl;
+	}
+
+///  std::cout <<" antes calculo theta 4"<< std::endl;
+
+  double theta=calc_person_companion_orientation();
+  //double theta_pers=theta;
+
+	double act_min_companion_angle=in_act_min_companion_angle;
+
+	//next_companion_angle_save_=act_min_companion_angle;
+
+	//std::cout << "diffangle(theta=100,angle=20)="<<diffangle(100*(3.14/180),20*(3.14/180))<< std::endl;
+	//std::cout << "diffangle(theta=15,angle=160)="<<diffangle(15*(3.14/180),160*(3.14/180))<< std::endl;
+	Cperson_abstract* person_obj1;
+	//bool finded_person=find_person(id_person_companion_ , &person_obj1);
+	find_person(id_person_companion_ , &person_obj1);
+	SpointV_cov person = person_obj1->get_current_pointV();
+	Sdestination person_companion_goal=person_obj1->get_best_dest();
+	Spoint robot=Spoint(initial_robot_spoint_.x,initial_robot_spoint_.y,initial_robot_spoint_.time_stamp);
+
+	SpointV_cov robot_center_traj_ideal_position=person;
+	double angle = atan2(robot.y-person.y,robot.x-person.x);
+
+	if(diffangle(theta,angle)<0){
+		//std::cout << " Entro en diff angle (NEGATIVO), case person goal POSITIVO"<< std::endl;
+		//std::cout << " theta="<<theta*(180/3.14)<<"; angle="<<angle*(180/3.14)<<"; act_min_companion_angle="<<act_min_companion_angle<< std::endl;
+		robot_center_traj_ideal_position.x+=((robot_person_proximity_distance_)/2)*cos(theta+(act_min_companion_angle*(3.14/180)));
+		robot_center_traj_ideal_position.y+=((robot_person_proximity_distance_)/2)*sin(theta+(act_min_companion_angle*(3.14/180)));
+		person_companion_goal.x+=(robot_person_proximity_distance_)*cos(theta+(act_min_companion_angle*(3.14/180)));//1.5*cos(theta+(act_min_companion_angle*(3.14/180)));
+		person_companion_goal.y+=(robot_person_proximity_distance_)*sin(theta+(act_min_companion_angle*(3.14/180)));//1.5*sin(theta+(act_min_companion_angle*(3.14/180)));
+	}else{
+		//std::cout << " Entro en diff angle (POSITIVO), case person goal NEGATIVO"<< std::endl;
+		//std::cout << " theta="<<theta*(180/3.14)<<"; angle="<<angle*(180/3.14)<<"; act_min_companion_angle="<<act_min_companion_angle<<std::endl;
+		robot_center_traj_ideal_position.x+=((robot_person_proximity_distance_)/2)*cos(theta-(act_min_companion_angle*(3.14/180)));
+		robot_center_traj_ideal_position.y+=((robot_person_proximity_distance_)/2)*sin(theta-(act_min_companion_angle*(3.14/180)));
+		person_companion_goal.x+=(robot_person_proximity_distance_)*cos(theta-(act_min_companion_angle*(3.14/180)));//1.5*cos(theta-(act_min_companion_angle*(3.14/180)));
+		person_companion_goal.y+=(robot_person_proximity_distance_)*sin(theta-(act_min_companion_angle*(3.14/180)));//1.5*sin(theta-(act_min_companion_angle*(3.14/180)));
+	}
+	//person_companion_goal_out_=person_companion_goal;
+
+	// INI calcular goal a 90 grados derecha e izquierda, para ver hacia que lado es mejor ir.
+	if(chose_better_side_to_acompani_person_){
+		Sdestination person_companion_goal_positive=Sdestination(0,person.x,person.y,1.0);
+		Sdestination person_companion_goal_negative=Sdestination(0,person.x,person.y,1.0);
+
+		person_companion_goal_positive.x+=(robot_person_proximity_distance_)*cos(theta+(90*(3.14/180)));
+		person_companion_goal_positive.y+=(robot_person_proximity_distance_)*sin(theta+(90*(3.14/180)));
+
+		person_companion_goal_negative.x+=(robot_person_proximity_distance_)*cos(theta-(90*(3.14/180)));
+		person_companion_goal_negative.y+=(robot_person_proximity_distance_)*sin(theta-(90*(3.14/180)));
+
+		// distancia a esa posicion respecto a la del robot actual mallor que cierto margen.
+		double distance_to_side_person_positive=initial_robot_spoint_.distance(Spoint(person_companion_goal_positive.x,person_companion_goal_positive.y,person_companion_goal_positive.time_stamp));
+		double distance_to_side_person_negative=initial_robot_spoint_.distance(Spoint(person_companion_goal_negative.x,person_companion_goal_negative.y,person_companion_goal_negative.time_stamp));
+
+		bool select_side_of_the_person_to_go=false;
+		if((distance_to_side_person_positive>threshold_dintace_select_person_side_to_go_)||(distance_to_side_person_negative>threshold_dintace_select_person_side_to_go_)){
+			select_side_of_the_person_to_go=true;
+			if(debug_select_person_side_to_go_with_more_free_space_){
+				std::cout << " distancia + o - a los 90 grados de la persona > 0.5 m. Miro a ver en que lado de la persona hay más espacio. "<< std::endl;
+				std::cout << " distance_to_side_person_positive= "<< distance_to_side_person_positive << std::endl;
+				std::cout << " distance_to_side_person_negative= "<< distance_to_side_person_negative << std::endl;
+				std::cout << " threshold_dintace_select_person_side_to_go_= "<< threshold_dintace_select_person_side_to_go_ << std::endl;
+
+			}
+		}
+
+		// falta calcular colisiones.
+		if(select_side_of_the_person_to_go){
+			Spoint Spoint_pose_command=Spoint(person_companion_goal_positive.x,person_companion_goal_positive.y);
+			double min_dist_colli_p=check_collision_companion_goal(Spoint_pose_command,0);
+
+			if(debug_select_person_side_to_go_with_more_free_space_){
+				std::cout << " min_dist_colli_p= "<< min_dist_colli_p << std::endl;
+			}
+
+			Spoint_pose_command=Spoint(person_companion_goal_negative.x,person_companion_goal_negative.y);
+			double min_dist_colli_n=check_collision_companion_goal(Spoint_pose_command,0); // ya te da la distancia minima
+
+			if(debug_select_person_side_to_go_with_more_free_space_){
+				std::cout << " min_dist_colli_n= "<< min_dist_colli_n << std::endl;
+				std::cout << " robot_person_companion_distance_= "<< robot_person_companion_distance_ << std::endl;
+			}
+
+			unsigned int case_act=0;
+			if((min_dist_colli_p<robot_person_companion_distance_)&&(min_dist_colli_n==robot_person_companion_distance_)){// hay distancia de colision solo en el caso positivo
+				case_act=1;
+				if(debug_select_person_side_to_go_with_more_free_space_){
+					std::cout << " [case_act=1] min_dist_colli_n= "<< min_dist_colli_n <<"; min_dist_colli_p"<<min_dist_colli_p << std::endl;
+				}
+			}else if((min_dist_colli_n<robot_person_companion_distance_)&&(min_dist_colli_p==robot_person_companion_distance_)){ // hay distancia de colision solo en el caso negativo
+				case_act=2;
+				if(debug_select_person_side_to_go_with_more_free_space_){
+					std::cout << " [case_act=2] min_dist_colli_n= "<< min_dist_colli_n <<"; min_dist_colli_p"<<min_dist_colli_p << std::endl;
+				}
+			}else if((min_dist_colli_p!=robot_person_companion_distance_)&&(min_dist_colli_n!=robot_person_companion_distance_)){ // hay distancia de colision en ambos casos
+				case_act=3;
+			}
+
+			bool reclaculate_goal=false;
+			if((min_dist_colli_n<robot_person_companion_distance_)||(min_dist_colli_p<robot_person_companion_distance_)){
+				reclaculate_goal=true;
+			}
+
+			Cperson_abstract* person_obj;
+			find_person(id_person_companion_ , &person_obj);
+			person = person_obj->get_current_pointV();
+
+			Sdestination person_companion_position;
+			person_companion_position = Sdestination(0, person.x ,person.y,1.0);
+
+			bool robot_goal_positive_bool; // if true is goal positive (+) if false es negative (-)
+
+			switch( case_act)
+			{
+				case 1: //case best the robot_goal_negativo_
+					person_companion_position=person_companion_goal_negative;//.x += (robot_person_proximity_distance_)*cos(theta + angle_companion_*3.14/180);
+					robot_goal_positive_bool=false;
+					if(debug_select_person_side_to_go_with_more_free_space_){
+						std::cout << " [case1](robot_goal_positivo_) (ROBOT GOAL) person_companion_position.x=" <<person_companion_position.x<< std::endl;
+						std::cout << " person_companion_position.y=" <<person_companion_position.y<< std::endl;
+					}
+					break;
+				case 2: //case best the robot_goal_positivo_
+					person_companion_position=person_companion_goal_positive;//.x += (robot_person_proximity_distance_)*cos(theta - (angle_companion_*3.14/180));
+					robot_goal_positive_bool=true;
+					if(debug_select_person_side_to_go_with_more_free_space_){
+						std::cout << " [case2](robot_goal_positivo_) (ROBOT GOAL) person_companion_position.x=" <<person_companion_position.x<< std::endl;
+						std::cout << " person_companion_position.y=" <<person_companion_position.y<< std::endl;
+					}
+					break;
+				case 3:
+					if(min_dist_colli_n>min_dist_colli_p){ //case best the robot_goal_negativo_
+						person_companion_position=person_companion_goal_negative;
+						robot_goal_positive_bool=false;
+						if(debug_select_person_side_to_go_with_more_free_space_){
+							std::cout << " [case_act=3] min_dist_colli_n= "<< min_dist_colli_n <<"; > min_dist_colli_p="<<min_dist_colli_p<< std::endl;
+							std::cout << " [case3](robot_goal_negativo_) (ROBOT GOAL) person_companion_position.x=" <<person_companion_position.x<< std::endl;
+							std::cout << " person_companion_position.y=" <<person_companion_position.y<< std::endl;
+						}
+					}else{ //case best the robot_goal_positivo_
+						robot_goal_positive_bool=true;
+						person_companion_position=person_companion_goal_positive;
+						if(debug_select_person_side_to_go_with_more_free_space_){
+							std::cout << " [case_act=3] min_dist_colli_n= "<< min_dist_colli_n <<"; < min_dist_colli_p="<<min_dist_colli_p<< std::endl;
+							std::cout << " [case3](robot_goal_positivo_) (ROBOT GOAL) person_companion_position.x=" <<person_companion_position.x<< std::endl;
+							std::cout << " person_companion_position.y=" <<person_companion_position.y<< std::endl;
+						}
+					}
+
+					break;
+				default:
+				case 0:
+					if(debug_select_person_side_to_go_with_more_free_space_){
+						std::cout << " [case_act=0 or default] min_dist_colli_p= "<< min_dist_colli_p<<"; min_dist_colli_n="<<min_dist_colli_n << std::endl;
+					}
+					// darle el goal al lado de la persona.
+					if( diffangle(theta, angle) < 0 )
+					{
+						//if(debug_select_person_side_to_go_with_more_free_space_){
+						//	std::cout << " (1) person_companion_position.x=" <<person_companion_position.x<< std::endl;
+						//	std::cout << " person_companion_position.y=" <<person_companion_position.y<< std::endl;
+						//}
+						person_companion_position=person_companion_goal_positive;
+						robot_goal_positive_bool=true;
+						if(debug_select_person_side_to_go_with_more_free_space_){
+							std::cout << " (robot_goal_positivo_) (1) (ROBOT GOAL) person_companion_position.x=" <<person_companion_position.x<< std::endl;
+							std::cout << " person_companion_position.y=" <<person_companion_position.y<< std::endl;
+						}
+					 }
+					 else  // es como si hiciera una circunferencia al rededor de la persona. (se intenta poner a un lado o a otro de la persona. segun esos angulos)
+					 {
+						//if(debug_select_person_side_to_go_with_more_free_space_){
+						// 	std::cout << " (2) person_companion_position.x=" <<person_companion_position.x<< std::endl;
+						//	std::cout << " person_companion_position.y=" <<person_companion_position.y<< std::endl;
+						//}
+						person_companion_position=person_companion_goal_negative;
+						robot_goal_positive_bool=false;
+						if(debug_select_person_side_to_go_with_more_free_space_){
+							std::cout << " (robot_goal_negativo_) (2) (ROBOT GOAL)  person_companion_position.x=" <<person_companion_position.x<< std::endl;
+							std::cout << " person_companion_position.y=" <<person_companion_position.y<< std::endl;
+						}
+					  }
+					break;
+				}
+			// cambiar person companion goal por el positivo o negativo respecto al actual act_min_companion_angle que sacamos!
+			if(reclaculate_goal){
+				person_companion_goal=Sdestination(0,person.x,person.y,1.0);
+				if(robot_goal_positive_bool){
+					person_companion_goal.x+=(robot_person_proximity_distance_)*cos(theta+(act_min_companion_angle*(3.14/180)));//1.5*cos(theta+(act_min_companion_angle*(3.14/180)));
+					person_companion_goal.y+=(robot_person_proximity_distance_)*sin(theta+(act_min_companion_angle*(3.14/180)));//1.5*sin(theta+(act_min_companion_angle*(3.14/180)));
+					//std::cout << " [CHANGE PERSON GOAL] Case person goal positive"<< std::endl;
+					//std::cout << " person_companion_goal.x="<<person_companion_goal.x<< std::endl;
+					//std::cout << " person_companion_goal.y="<<person_companion_goal.y<< std::endl;
+				}else{
+					person_companion_goal.x+=(robot_person_proximity_distance_)*cos(theta-(act_min_companion_angle*(3.14/180)));//1.5*cos(theta-(act_min_companion_angle*(3.14/180)));
+					person_companion_goal.y+=(robot_person_proximity_distance_)*sin(theta-(act_min_companion_angle*(3.14/180)));//1.5*sin(theta-(act_min_companion_angle*(3.14/180)));
+					//std::cout << " [CHANGE PERSON GOAL] Case person goal negative"<< std::endl;
+					//std::cout << " person_companion_goal.x="<<person_companion_goal.x<< std::endl;
+					//std::cout << " person_companion_goal.y="<<person_companion_goal.y<< std::endl;
+				}
+			}
+		}
+	}
+	// FIN calcular goal a 90 grados derecha e izquierda, para ver hacia que lado es mejor ir.
+
+
+	if(debug_file_robot_){
+		std::ofstream fileMatlab2;
+		fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+		fileMatlab2 << "% ANTES f_person_goal=robot_->force_goal_near\n";
+		fileMatlab2.close();
+	}
+
+	if(debug_gazebo_journal_){
+		std::cout << " IDEAL FINAL ACT person_companion_goal.x="<<person_companion_goal.x<<" person_companion_goal.y="<<person_companion_goal.y<< std::endl;
+
+	}
+
+	//robot_goal_to_person_companion_=person_companion_goal;
+
+
+	return person_companion_goal;
+
+
+
+
+
+///////////
+}
+
+
+
+
+void Cplan_local_nav_person_companion::fix_angles_to_use_person_prediction_for_the_companion_goal(){
+//////////////// start function fix angles for prediction (vuelta hacia delante)
+
+unsigned int act_index1=0;//BEST_path_parent_index_vector_[BEST_path_parent_index_vector_.size()-1];
+unsigned int next_index1=1;//BEST_path_parent_index_vector_[BEST_path_parent_index_vector_.size()-2];
+double act_angle1=orientation_person_robot_angles_with_prediction_of_person_companion_[act_index1];
+double next_angle1=orientation_person_robot_angles_with_prediction_of_person_companion_[next_index1];
+double diff_angle1=act_angle1-next_angle1;
+if(diff_angle1>angle_increment_of_increment_distance_){
+	//caso_vuelta_al_reves=true;
+}
+
+// 2- vuelta del principio al final
+/*  caso_vuelta_al_reves!!!!   */
+//std::cout << " (befor first for) orientation_person_robot_angles_with_prediction_of_person_companion_.size()-1"<<orientation_person_robot_angles_with_prediction_of_person_companion_.size()-1<< std::endl;
+
+for(unsigned int g=0;g<(orientation_person_robot_angles_with_prediction_of_person_companion_.size()-1);g++){
+
+
+
+	unsigned int act_index=0+g;
+	unsigned int next_index=1+g;
+	//std::cout << " act_index"<<act_index<<"; next_index="<<next_index<< std::endl;
+
+	double act_angle=orientation_person_robot_angles_with_prediction_of_person_companion_[act_index];
+	double next_angle=orientation_person_robot_angles_with_prediction_of_person_companion_[next_index];
+
+	if((act_index==0)){
+		act_angle=initial_angle_; // initial angle es el mismo para los dos casos.
+	}
+	/*if(next_angle==0){
+		next_angle=initial_angle_;
+	}*/
+
+	if(debug_angles_){
+		std::cout << " 2 (vuelta hacia delante de INI_angle_act_angle al qur necesitas para pasar.!) act_index="<<act_index<<"; next_index="<<next_index<< std::endl;
+	}
+
+	if(g==0){
+		act_angle=initial_angle_;
+	}
+
+	//double diff_angle=act_angle-next_angle;
+	double diff_angle=next_angle-act_angle;
+
+	if(debug_angles_){
+		std::cout << "act_angle["<<act_index<<"]="<<act_angle<<"next_angle["<<next_index<<"]"<<next_angle<<"diff_angle=act_angle-next_angle="<<diff_angle<<" > angle_increment_of_increment_distance_="<< angle_increment_of_increment_distance_<<" and diff_angle>0"<< std::endl;
+	}
+
+	if(((diff_angle>(angle_increment_of_increment_distance_)))&&(diff_angle>0)){
+		//std::cout << "ENTRO EN IF !!!"<< std::endl;
+
+		//	std::cout << " (FOR) ANT (!!!CASE!!! actual_angle > actual_parent_angle ) act_index="<<act_index<<"; next_index="<<next_index<< std::endl;
+		//	std::cout << "  act_angle="<<act_angle<<"; next_angle="<<next_angle<<";  diff_angle="<< diff_angle<< std::endl;
+
+		//	std::cout << "(!!!CASE!!! actual_angle > actual_parent_angle ) act_index="<<act_index<<"; next_index="<<next_index<< std::endl;
+		// CASE 2: actual_angle > actual_parent_angle (aquí no se puede hacer...) Hay que dar vuelta hacia delante.
+
+		if(actual_debug_){
+			std::cout << "if (CASE: salgo de obstaculo)"<< std::endl;
+		}
+
+		//double new_angle=act_angle-(2*angle_increment_of_increment_distance_);
+		double new_angle=act_angle+(angle_increment_of_increment_distance_);
+
+		//if(debug_gazebo_journal_){
+		//	std::cout << "new_angle ["<<next_index<<"]="<<new_angle<< std::endl;
+		//}
+
+		if(new_angle<angle_companion_){
+			orientation_person_robot_angles_with_prediction_of_person_companion_[next_index]=angle_companion_;
+			//std::cout << "if(new_angle<angle_companion_) angle_companion_="<<angle_companion_<< std::endl;
+		}else{
+			orientation_person_robot_angles_with_prediction_of_person_companion_[next_index]=new_angle;
+			//std::cout << "else if(new_angle<angle_companion_)new_angle="<<new_angle<< std::endl;
+		}
+		if(new_angle>180){
+			orientation_person_robot_angles_with_prediction_of_person_companion_[next_index]=180;
+		}
+		if(actual_debug_){
+			std::cout << "(modif) orientation_person_robot_angles_with_prediction_of_person_companion_[next_index="<<next_index<<"]="<<orientation_person_robot_angles_with_prediction_of_person_companion_[next_index]<< std::endl;
+		}
+
+	}else{
+
+		//std::cout << "ENTRO EN ELSE !!!"<< std::endl;
+
+		//	std::cout << " (FOR) (real) next_angle="<<orientation_person_robot_angles_[next_index]<< std::endl;
+
+	}
+
+	//sum_angles_increment=sum_angles_increment+orientation_person_robot_angles_[act_index];
+
+} // fin for vuelta hacia delante
+
+//sum_angles_increment=sum_angles_increment/BEST_path_parent_index_vector_.size();
+//std::cout << " [IMPORTANTE, ver si es diferente en pasillo y obstaculo!!!] sum_angles_increment="<<sum_angles_increment<< std::endl;
+//std::cout << " tercera pasada, ahora mismo no se para que!"<< std::endl;
+// es para mantenerte a 90 grados si decrece tu valor en orientacion!!! Te adelantas demasiado
+//std::cout << " (end first for) orientation_person_robot_angles_with_prediction_of_person_companion_.size()-1"<< std::endl;
+
+// TODO: falta arreglar esta ultima parte!!!
+if(!orientation_person_robot_angles_with_prediction_of_person_companion_.empty()){
+	//std::cout << " in IF 1"<< std::endl;
+	bool we_have_path_collisions=false;
+	//std::cout << " in IF 2"<< std::endl;
+	for(unsigned int g=0;g<(orientation_person_robot_angles_with_prediction_of_person_companion_.size()-1);g++){
+		//std::cout << " in IF3 ;min_distance_collision_vector_from_pred_[g]="<<min_distance_collision_vector_from_pred_[g]<<"; orientation_person_robot_angles_with_prediction_of_person_companion_[g]="<<orientation_person_robot_angles_with_prediction_of_person_companion_[g]<<"; angle_companion_+2*angle_increment_of_increment_distance_="<<angle_companion_+2*angle_increment_of_increment_distance_<<"; angle_increment_of_increment_distance_="<<angle_increment_of_increment_distance_<< std::endl;
+		/*if(debug_angles_){
+			std::cout << " BEST_path_parent_index_vector_[BEST_path_parent_index_vector_.size()-1-g]="<<BEST_path_parent_index_vector_[BEST_path_parent_index_vector_.size()-1-g]<< std::endl;
+			std::cout << " vector_of_companion_collisions_[BEST_path_parent_index_vector_[BEST_path_parent_index_vector_.size()-1-g]]="<<vector_of_companion_collisions_[BEST_path_parent_index_vector_[BEST_path_parent_index_vector_.size()-1-g]]<< std::endl;
+			std::cout << " angle="<<orientation_person_robot_angles_[BEST_path_parent_index_vector_[BEST_path_parent_index_vector_.size()-1-g]]<< std::endl;
+			std::cout << " angle_companion_+2*angle_increment_of_increment_distance_="<<angle_companion_+2*angle_increment_of_increment_distance_<< std::endl;
+		}*/
+		//vector_of_companion_collisions_
+
+		if((min_distance_collision_vector_from_pred_[g]!=100)&&(orientation_person_robot_angles_with_prediction_of_person_companion_[g]>(angle_companion_+2*angle_increment_of_increment_distance_))){
+			//std::cout << " in IF 4"<< std::endl;
+			we_have_path_collisions=true;
+		}
+		//std::cout << " in IF 5"<< std::endl;
+	}
+	//std::cout << " in IF 6"<< std::endl;
+	//if(debug_angles_){
+		//std::cout << " !BEST_path_parent_index_vector_.empty()="<<!BEST_path_parent_index_vector_.empty()<< std::endl;
+		std::cout << " !we_have_path_collisions="<<!we_have_path_collisions<<"; angle_companion_="<<angle_companion_<< std::endl;
+	//}
+
+	double ini_act_angle_out=initial_angle_;
+
+	//if(debug_angles_){
+		std::cout << " ini_act_angle_out="<< ini_act_angle_out<< std::endl;
+	//}
+
+	if((!we_have_path_collisions)&&(ini_act_angle_out>angle_companion_)){
+		for(unsigned int g=0;g<(orientation_person_robot_angles_with_prediction_of_person_companion_.size()-1);g++){
+			unsigned int act_index;//=best_plan_vertex_index_[best_plan_vertex_index_.size()-g];
+
+			unsigned int next_index=g+1;
+			double act_angle;//=orientation_person_robot_angles_[act_index];
+			if(g==0){
+				act_angle=ini_act_angle_out;
+				act_index=0;
+				orientation_person_robot_angles_with_prediction_of_person_companion_[act_index]=ini_act_angle_out;
+			}else{
+				act_angle=orientation_person_robot_angles_with_prediction_of_person_companion_[act_index];
+			}
+
+			double next_angle=act_angle-angle_increment_of_increment_distance_;
+			if(next_angle<angle_companion_){
+				//next_angle=orientation_person_robot_angles_[act_index]-angle_increment_of_increment_distance_;
+				orientation_person_robot_angles_with_prediction_of_person_companion_[next_index]=angle_companion_;
+			}else{
+				orientation_person_robot_angles_with_prediction_of_person_companion_[next_index]=next_angle;
+			}
+
+			if(debug_angles_){
+				std::cout << " act_index="<< act_index<<"; next_index="<<next_index<<"; act_angle="<<act_angle<<"; next_angle="<<next_angle<< std::endl;
+				std::cout << "orientation_person_robot_angles_with_prediction_of_person_companion_[next_index]="<< orientation_person_robot_angles_with_prediction_of_person_companion_[next_index]<< std::endl;
+			}
+
+			act_index=next_index;
+		}
+	}
+}
+
+//std::cout << " (end second for) orientation_person_robot_angles_with_prediction_of_person_companion_.size()-1"<< std::endl;
+/*if(debug_comanion_good_){
+	if(!BEST_path_parent_index_vector_.empty()){
+		for(unsigned int f=0;f<BEST_path_parent_index_vector_.size();f++){
+			std::cout << "(BEST path index) f="<<f<<"; index=" <<BEST_path_parent_index_vector_[f]<< std::endl;
+		}
+	}
+
+	std::cout << "FIN only_angle_in_final_tree_calculate_companion_path_angle_and_cost"<< std::endl;
+}*/
+
+
+
+
+/////////////// end: function fix angles for prediction (vuelta hacia delante)
+
+}
+
+
+
+Sdestination Cplan_local_nav_person_companion::calc_companion_goal_position(){
+
+	//std::cout << " INI!!! calc_companion_goal_position 1 "<< std::endl;
+	//SpointV_cov robot_center_traj_ideal_position=person;
+	SpointV_cov robot=initial_robot_spoint_;
+	SpointV_cov person=initial_person_companion_point_;
+	//std::cout << " 2 INI!!! calc_companion_goal_position 1 "<< std::endl;
+
+	Sdestination person_companion_goal;
+	person_companion_goal=Sdestination(0,person.x,person.y,1.0);
+	//std::cout << " 3 INI!!! calc_companion_goal_position 1 "<< std::endl;
+
+	double angle = atan2(robot.y-person.y,robot.x-person.x); //REAL angle between robot and person.
+	if(angle<0){
+		angle=2*3.14+angle; // obtain a positive angle inside 2*Pi
+	}
+	//std::cout << " 4 INI!!! calc_companion_goal_position 1 "<< std::endl;
+
+	double theta=calc_person_companion_orientation();
+	//std::cout << " 5 INI!!! calc_companion_goal_position 1 "<< std::endl;
+    double act_min_companion_angle;
+	//std::cout << "IN AKP act_min_companion_angle replan_last_step1"<< std::endl;
+
+    if(calc_goal_companion_with_group_path_){
+    	act_min_companion_angle=next_companion_angle_save_; // uso el angulo anterior de companion.
+    }else{
+    	act_min_companion_angle=next_companion_angle_save_;
+    }
+
+   // std::cout << "IN AKP act_min_companion_angle replan_last_step2 ; act_min_companion_angle="<<act_min_companion_angle<<"index="<<index<< std::endl;
+
+	//next_companion_angle_save_=act_min_companion_angle;
+	if(debug_real_test_companion3_){
+		std::cout << "calc_companion_goal_position 1 act_min_companion_angle="<<act_min_companion_angle<< std::endl;
+	}
+
+	//std::cout << "diffangle(theta=100,angle=20)="<<diffangle(100*(3.14/180),20*(3.14/180))<< std::endl;
+	//std::cout << "diffangle(theta=15,angle=160)="<<diffangle(15*(3.14/180),160*(3.14/180))<< std::endl;
+
+	//std::cout << " 777777777777 act_min_companion_angle="<<act_min_companion_angle<<"; theta (orientation_peron_companion)="<<theta*180/3.14<<"; angle(between robot and person)="<<angle*180/3.14<< std::endl;
+	double person_robot_distance_real=robot.distance(person);
+
+	if(((robot_person_proximity_distance_-0.5)<person_robot_distance_real)&&(person_robot_distance_real<(robot_person_proximity_distance_+0.5))){
+		person_robot_distance_real=person_robot_distance_real;
+	}else{
+		person_robot_distance_real=robot_person_proximity_distance_;
+	}
+
+	//std::cout << "calc_companion_goal_position 2"<< std::endl;
+
+	if(diffangle(theta,angle)<0){
+		//std::cout << " Entro en diff angle (NEGATIVO), case person goal POSITIVO"<< std::endl;
+		//std::cout << " theta="<<theta*(180/3.14)<<"; angle="<<angle*(180/3.14)<<"; act_min_companion_angle="<<act_min_companion_angle<< std::endl;
+		//robot_center_traj_ideal_position.x+=((robot_person_proximity_distance_)/2)*cos(theta+(act_min_companion_angle*(3.14/180)));
+		//robot_center_traj_ideal_position.y+=((robot_person_proximity_distance_)/2)*sin(theta+(act_min_companion_angle*(3.14/180)));
+		person_companion_goal.x+=(person_robot_distance_real)*cos(theta+(act_min_companion_angle*(3.14/180)));//1.5*cos(theta+(act_min_companion_angle*(3.14/180)));
+		person_companion_goal.y+=(person_robot_distance_real)*sin(theta+(act_min_companion_angle*(3.14/180)));//1.5*sin(theta+(act_min_companion_angle*(3.14/180)));
+	}else{
+		//std::cout << " Entro en diff angle (POSITIVO), case person goal NEGATIVO"<< std::endl;
+		//std::cout << " theta="<<theta*(180/3.14)<<"; angle="<<angle*(180/3.14)<<"; act_min_companion_angle="<<act_min_companion_angle<<std::endl;
+		//robot_center_traj_ideal_position.x+=((robot_person_proximity_distance_)/2)*cos(theta-(act_min_companion_angle*(3.14/180)));
+		//robot_center_traj_ideal_position.y+=((robot_person_proximity_distance_)/2)*sin(theta-(act_min_companion_angle*(3.14/180)));
+		person_companion_goal.x+=(person_robot_distance_real)*cos(theta-(act_min_companion_angle*(3.14/180)));//1.5*cos(theta-(act_min_companion_angle*(3.14/180)));
+		person_companion_goal.y+=(person_robot_distance_real)*sin(theta-(act_min_companion_angle*(3.14/180)));//1.5*sin(theta-(act_min_companion_angle*(3.14/180)));
+	}
+	//person_companion_goal_out_=person_companion_goal;
+	//std::cout << "calc_companion_goal_position 3"<< std::endl;
+	// INI calcular goal a 90 grados derecha e izquierda, para ver hacia que lado es mejor ir.
+	if(chose_better_side_to_acompani_person_){
+		Sdestination person_companion_goal_positive=Sdestination(0,person.x,person.y,1.0);
+		Sdestination person_companion_goal_negative=Sdestination(0,person.x,person.y,1.0);
+
+		person_companion_goal_positive.x+=(person_robot_distance_real)*cos(theta+(90*(3.14/180)));
+		person_companion_goal_positive.y+=(person_robot_distance_real)*sin(theta+(90*(3.14/180)));
+
+		person_companion_goal_negative.x+=(person_robot_distance_real)*cos(theta-(90*(3.14/180)));
+		person_companion_goal_negative.y+=(person_robot_distance_real)*sin(theta-(90*(3.14/180)));
+
+		// distancia a esa posicion respecto a la del robot actual mallor que cierto margen.
+		double distance_to_side_person_positive=initial_robot_spoint_.distance(Spoint(person_companion_goal_positive.x,person_companion_goal_positive.y,person_companion_goal_positive.time_stamp));
+		double distance_to_side_person_negative=initial_robot_spoint_.distance(Spoint(person_companion_goal_negative.x,person_companion_goal_negative.y,person_companion_goal_negative.time_stamp));
+
+		bool select_side_of_the_person_to_go=false;
+		if((distance_to_side_person_positive>threshold_dintace_select_person_side_to_go_)||(distance_to_side_person_negative>threshold_dintace_select_person_side_to_go_)){
+			select_side_of_the_person_to_go=true;
+			if(debug_select_person_side_to_go_with_more_free_space_){
+				std::cout << " distancia + o - a los 90 grados de la persona > 0.5 m. Miro a ver en que lado de la persona hay más espacio. "<< std::endl;
+				std::cout << " distance_to_side_person_positive= "<< distance_to_side_person_positive << std::endl;
+				std::cout << " distance_to_side_person_negative= "<< distance_to_side_person_negative << std::endl;
+				std::cout << " threshold_dintace_select_person_side_to_go_= "<< threshold_dintace_select_person_side_to_go_ << std::endl;
+
+			}
+		}
+		//std::cout << "calc_companion_goal_position 3"<< std::endl;
+		// falta calcular colisiones.
+		if(select_side_of_the_person_to_go){
+			Spoint Spoint_pose_command=Spoint(person_companion_goal_positive.x,person_companion_goal_positive.y);
+			double min_dist_colli_p=check_collision_companion_goal(Spoint_pose_command,0);
+
+			if(debug_select_person_side_to_go_with_more_free_space_){
+				std::cout << " min_dist_colli_p= "<< min_dist_colli_p << std::endl;
+			}
+
+			Spoint_pose_command=Spoint(person_companion_goal_negative.x,person_companion_goal_negative.y);
+			double min_dist_colli_n=check_collision_companion_goal(Spoint_pose_command,0); // ya te da la distancia minima
+
+			if(debug_select_person_side_to_go_with_more_free_space_){
+				std::cout << " min_dist_colli_n= "<< min_dist_colli_n << std::endl;
+				std::cout << " robot_person_companion_distance_= "<< robot_person_companion_distance_ << std::endl;
+			}
+			//std::cout << "calc_companion_goal_position 4"<< std::endl;
+			unsigned int case_act=0;
+			if((min_dist_colli_p<robot_person_companion_distance_)&&(min_dist_colli_n==robot_person_companion_distance_)){// hay distancia de colision solo en el caso positivo
+				case_act=1;
+				if(debug_select_person_side_to_go_with_more_free_space_){
+					std::cout << " [case_act=1] min_dist_colli_n= "<< min_dist_colli_n <<"; min_dist_colli_p"<<min_dist_colli_p << std::endl;
+				}
+			}else if((min_dist_colli_n<robot_person_companion_distance_)&&(min_dist_colli_p==robot_person_companion_distance_)){ // hay distancia de colision solo en el caso negativo
+				case_act=2;
+				if(debug_select_person_side_to_go_with_more_free_space_){
+					std::cout << " [case_act=2] min_dist_colli_n= "<< min_dist_colli_n <<"; min_dist_colli_p"<<min_dist_colli_p << std::endl;
+				}
+			}else if((min_dist_colli_p!=robot_person_companion_distance_)&&(min_dist_colli_n!=robot_person_companion_distance_)){ // hay distancia de colision en ambos casos
+				case_act=3;
+			}
+			//std::cout << "calc_companion_goal_position 5"<< std::endl;
+			bool reclaculate_goal=false;
+			if((min_dist_colli_n<robot_person_companion_distance_)||(min_dist_colli_p<robot_person_companion_distance_)){
+				reclaculate_goal=true;
+			}
+
+			Cperson_abstract* person_obj;
+			find_person(id_person_companion_ , &person_obj);
+			person = person_obj->get_current_pointV();
+
+			Sdestination person_companion_position;
+			person_companion_position = Sdestination(0, person.x ,person.y,1.0);
+
+			bool robot_goal_positive_bool; // if true is goal positive (+) if false es negative (-)
+			//std::cout << "calc_companion_goal_position 6"<< std::endl;
+			switch( case_act)
+			{
+				case 1: //case best the robot_goal_negativo_
+					//std::cout << "calc_companion_goal_position (case 1)"<< std::endl;
+					person_companion_position=person_companion_goal_negative;//.x += (robot_person_proximity_distance_)*cos(theta + angle_companion_*3.14/180);
+					robot_goal_positive_bool=false;
+					if(debug_select_person_side_to_go_with_more_free_space_){
+						std::cout << " [case1](robot_goal_positivo_) (ROBOT GOAL) person_companion_position.x=" <<person_companion_position.x<< std::endl;
+						std::cout << " person_companion_position.y=" <<person_companion_position.y<< std::endl;
+					}
+					break;
+				case 2: //case best the robot_goal_positivo_
+					//std::cout << "calc_companion_goal_position (case 2)"<< std::endl;
+					person_companion_position=person_companion_goal_positive;//.x += (robot_person_proximity_distance_)*cos(theta - (angle_companion_*3.14/180));
+					robot_goal_positive_bool=true;
+					if(debug_select_person_side_to_go_with_more_free_space_){
+						std::cout << " [case2](robot_goal_positivo_) (ROBOT GOAL) person_companion_position.x=" <<person_companion_position.x<< std::endl;
+						std::cout << " person_companion_position.y=" <<person_companion_position.y<< std::endl;
+					}
+					break;
+				case 3:
+					//std::cout << "calc_companion_goal_position (case 3)"<< std::endl;
+					if(min_dist_colli_n>min_dist_colli_p){ //case best the robot_goal_negativo_
+						person_companion_position=person_companion_goal_negative;
+						robot_goal_positive_bool=false;
+						if(debug_select_person_side_to_go_with_more_free_space_){
+							std::cout << " [case_act=3] min_dist_colli_n= "<< min_dist_colli_n <<"; > min_dist_colli_p="<<min_dist_colli_p<< std::endl;
+							std::cout << " [case3](robot_goal_negativo_) (ROBOT GOAL) person_companion_position.x=" <<person_companion_position.x<< std::endl;
+							std::cout << " person_companion_position.y=" <<person_companion_position.y<< std::endl;
+						}
+					}else{ //case best the robot_goal_positivo_
+						robot_goal_positive_bool=true;
+						person_companion_position=person_companion_goal_positive;
+						if(debug_select_person_side_to_go_with_more_free_space_){
+							std::cout << " [case_act=3] min_dist_colli_n= "<< min_dist_colli_n <<"; < min_dist_colli_p="<<min_dist_colli_p<< std::endl;
+							std::cout << " [case3](robot_goal_positivo_) (ROBOT GOAL) person_companion_position.x=" <<person_companion_position.x<< std::endl;
+							std::cout << " person_companion_position.y=" <<person_companion_position.y<< std::endl;
+						}
+					}
+
+					break;
+				default:
+				case 0:
+					//std::cout << "calc_companion_goal_position (case 0)"<< std::endl;
+					if(debug_select_person_side_to_go_with_more_free_space_){
+						std::cout << " [case_act=0 or default] min_dist_colli_p= "<< min_dist_colli_p<<"; min_dist_colli_n="<<min_dist_colli_n << std::endl;
+					}
+					// darle el goal al lado de la persona.
+					if( diffangle(theta, angle) < 0 )
+					{
+						//if(debug_select_person_side_to_go_with_more_free_space_){
+						//	std::cout << " (1) person_companion_position.x=" <<person_companion_position.x<< std::endl;
+						//	std::cout << " person_companion_position.y=" <<person_companion_position.y<< std::endl;
+						//}
+						person_companion_position=person_companion_goal_positive;
+						robot_goal_positive_bool=true;
+						if(debug_select_person_side_to_go_with_more_free_space_){
+							std::cout << " (robot_goal_positivo_) (1) (ROBOT GOAL) person_companion_position.x=" <<person_companion_position.x<< std::endl;
+							std::cout << " person_companion_position.y=" <<person_companion_position.y<< std::endl;
+						}
+					 }
+					 else  // es como si hiciera una circunferencia al rededor de la persona. (se intenta poner a un lado o a otro de la persona. segun esos angulos)
+					 {
+						//if(debug_select_person_side_to_go_with_more_free_space_){
+						// 	std::cout << " (2) person_companion_position.x=" <<person_companion_position.x<< std::endl;
+						//	std::cout << " person_companion_position.y=" <<person_companion_position.y<< std::endl;
+						//}
+						person_companion_position=person_companion_goal_negative;
+						robot_goal_positive_bool=false;
+						if(debug_select_person_side_to_go_with_more_free_space_){
+							std::cout << " (robot_goal_negativo_) (2) (ROBOT GOAL)  person_companion_position.x=" <<person_companion_position.x<< std::endl;
+							std::cout << " person_companion_position.y=" <<person_companion_position.y<< std::endl;
+						}
+					  }
+					break;
+				}
+			//std::cout << "calc_companion_goal_position 7"<< std::endl;
+			// cambiar person companion goal por el positivo o negativo respecto al actual act_min_companion_angle que sacamos!
+			if(reclaculate_goal){
+				person_companion_goal=Sdestination(0,person.x,person.y,1.0);
+				if(robot_goal_positive_bool){
+					person_companion_goal.x+=(person_robot_distance_real)*cos(theta+(act_min_companion_angle*(3.14/180)));//1.5*cos(theta+(act_min_companion_angle*(3.14/180)));
+					person_companion_goal.y+=(person_robot_distance_real)*sin(theta+(act_min_companion_angle*(3.14/180)));//1.5*sin(theta+(act_min_companion_angle*(3.14/180)));
+					//std::cout << " [CHANGE PERSON GOAL] Case person goal positive"<< std::endl;
+					//std::cout << " person_companion_goal.x="<<person_companion_goal.x<< std::endl;
+					//std::cout << " person_companion_goal.y="<<person_companion_goal.y<< std::endl;
+				}else{
+					person_companion_goal.x+=(person_robot_distance_real)*cos(theta-(act_min_companion_angle*(3.14/180)));//1.5*cos(theta-(act_min_companion_angle*(3.14/180)));
+					person_companion_goal.y+=(person_robot_distance_real)*sin(theta-(act_min_companion_angle*(3.14/180)));//1.5*sin(theta-(act_min_companion_angle*(3.14/180)));
+					//std::cout << " [CHANGE PERSON GOAL] Case person goal negative"<< std::endl;
+					//std::cout << " person_companion_goal.x="<<person_companion_goal.x<< std::endl;
+					//std::cout << " person_companion_goal.y="<<person_companion_goal.y<< std::endl;
+				}
+			}
+		}
+	}
+	// FIN calcular goal a 90 grados derecha e izquierda, para ver hacia que lado es mejor ir. //
+	//std::cout << "calc_companion_goal_position 10"<< std::endl;
+
+	if(debug_file_robot_){
+		std::ofstream fileMatlab2;
+		fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+		fileMatlab2 << "% ANTES f_person_goal=robot_->force_goal_near\n";
+		fileMatlab2.close();
+	}
+
+	//robot_goal_to_person_companion_=person_companion_goal;
+
+	return person_companion_goal;
+
+}
+/*
+void Cplan_local_nav_person_companion::find_entity_order_in_the_group(){
+
+	//std::cout << " IN!  find_entity_order_in_the_group  "<< std::endl;
+
+	State2D *Others=new State2D[number_of_group_people_];
+
+
+	Sdestination actual_group_goal=robot_->get_best_dest();//random_goal;;//robot_->get_best_dest();//random_goal;
+
+	//std::cout << " Zanlungo actual_group_goal.x="<<actual_group_goal.x<<"; actual_group_goal.y="<<actual_group_goal.y<<std::endl;
+
+
+	SpointV_cov pose_of_the_robot=robot_->get_current_pointV();
+
+	double robot_actual_time_stamp=robot_->get_current_pointV().time_stamp;
+
+	//std::cout << " Zanlungo 1; pose_of_the_robot.x="<<pose_of_the_robot.x<<"; pose_of_the_robot.y="<<pose_of_the_robot.y<<std::endl;
+	//std::cout << " robot_actual_time_stamp="<<robot_actual_time_stamp<<std::endl;
+
+	Vector2D robot_pose(pose_of_the_robot.vx,pose_of_the_robot.vy,atan(pose_of_the_robot.vy/pose_of_the_robot.vx)); // theta en rads, chec if is ok.
+
+	State2D Self_for_order;
+
+	Self_for_order.Init(pose_of_the_robot.x,pose_of_the_robot.y,pose_of_the_robot.vx,pose_of_the_robot.vy,robot_pose.th);
+
+
+
+	SpointV_cov actual_first_person_companion_point=pointer_to_person_companion_->get_current_pointV();
+
+	State2D first_companion_person(actual_first_person_companion_point.x,actual_first_person_companion_point.y,actual_first_person_companion_point.vx,actual_first_person_companion_point.vy);
+	Vector2D first_companion_person2D(actual_first_person_companion_point.vx,actual_first_person_companion_point.vy,atan(actual_first_person_companion_point.vy/actual_first_person_companion_point.vx));
+
+	Others[0]=first_companion_person;
+
+//std::cout << " robot_->get_planning_trajectory( ).size()="<<robot_->get_planning_trajectory( )->size()<<std::endl;
+
+
+	//std::cout <<"(FIRST) id_person="<<pointer_to_person_companion_->get_id()<< " first_companion_person.r.x="<<first_companion_person.r.x<<"; first_companion_person.r.y="<<first_companion_person.r.y<<std::endl;
+	//std::cout <<"(FIRST) actual_first_person_companion_point.x="<<actual_first_person_companion_point.x<< " actual_first_person_companion_point.y="<<actual_first_person_companion_point.y<<"; actual_first_person_companion_point.vy="<<"; actual_first_person_companion_point.vx="<<actual_first_person_companion_point.vx<<actual_first_person_companion_point.vy<<std::endl;
+
+
+	SpointV_cov actual_second_person_companion_point=SpointV_cov();
+
+	if((we_have_pointer_to_second_person_)&&(number_of_group_people_>1)){
+
+		actual_second_person_companion_point=second_group_companion_person_obj_->get_current_pointV();
+		State2D second_companion_person(actual_second_person_companion_point.x,actual_second_person_companion_point.y,actual_second_person_companion_point.vx,actual_second_person_companion_point.vy);
+		Vector2D second_companion_person2D(actual_second_person_companion_point.vx,actual_second_person_companion_point.vy,atan(actual_second_person_companion_point.vy/actual_second_person_companion_point.vx));
+		Others[1]=second_companion_person;
+		//std::cout <<"(Second) id_person="<<second_group_companion_person_obj_->get_id()<< " second_companion_person.r.x="<<second_companion_person.r.x<<"; second_companion_person.r.y="<<second_companion_person.r.y<<std::endl;
+		//std::cout <<"(Second) actual_second_person_companion_point.x="<<actual_second_person_companion_point.x<< " actual_second_person_companion_point.y="<<actual_second_person_companion_point.y<<"; actual_second_person_companion_point.vx="<<actual_second_person_companion_point.vx<<"; actual_second_person_companion_point.vy="<<actual_second_person_companion_point.vy<<std::endl;
+
+	}
+
+///
+
+	double theta=calc_person_companion_orientation();
+
+	if(theta<0){
+		theta=3.14+theta;
+	}
+
+
+	// Option 3: Calc preferred => using the mean of the position of the robot + the two person and the final goal. (works ok for two people + robot)
+
+	double central_group_x;
+	double central_group_y;
+
+	if(number_of_group_people_<2){
+
+		//std::cout << " solo 1_persona "<<std::endl;
+		central_group_x=(pose_of_the_robot.x + actual_first_person_companion_point.x)/(number_of_group_people_+1);//+actual_second_person_companion_point.x)/(number_of_group_people_+1);
+		central_group_y=(pose_of_the_robot.y + actual_first_person_companion_point.y)/(number_of_group_people_+1);//+actual_second_person_companion_point.x)/(number_of_group_people_+1);
+	}else{
+
+		//std::cout << " 2 _personas "<<std::endl;
+		if(we_have_pointer_to_second_person_){
+			central_group_x=(pose_of_the_robot.x + actual_first_person_companion_point.x+actual_second_person_companion_point.x)/(number_of_group_people_+1);//+actual_second_person_companion_point.x)/(number_of_group_people_+1);
+			central_group_y=(pose_of_the_robot.y + actual_first_person_companion_point.y+actual_second_person_companion_point.y)/(number_of_group_people_+1);//+actual_second_person_companion_point.x)/(number_of_group_people_+1);
+			//std::cout << " central_group_x= "<<central_group_x<<"; central_group_y="<<central_group_y<<std::endl;
+		}
+
+	}
+	//std::cout << " (find_entity_order_in_the_group) out calc center_group; number_of_group_people_= "<<number_of_group_people_<<"we_have_pointer_to_second_person_="<<we_have_pointer_to_second_person_<<std::endl;
+	// Option1: Calc preferred => using the robot position and the position of the Goal tu calculate the orientation.
+	double x_orient_goal;
+	double y_orient_goal;
+	double orient_goal;
+
+	if(number_of_group_people_<2){
+		//std::cout << "IMPORTANTE!!! actual_group_goal.x="<<actual_group_goal.x<<"; actual_group_goal.y="<<actual_group_goal.y<<"; pose_of_the_robot.x="<<pose_of_the_robot.x<<"; pose_of_the_robot.y="<<pose_of_the_robot.y<<std::endl;
+
+		x_orient_goal=actual_group_goal.x-pose_of_the_robot.x;
+		y_orient_goal=actual_group_goal.y-pose_of_the_robot.y;
+		orient_goal=atan(y_orient_goal/x_orient_goal);
+		//std::cout << " x_orient_goal="<<x_orient_goal<<"; y_orient_goal="<<y_orient_goal<<"; orient_goal="<<orient_goal<<std::endl;
+
+	}else{
+
+		if((we_have_pointer_to_second_person_)&&(actual_person_Companion_pointer_->get_current_pointV().v()<0.2)){ // the person is walking, the goal is the final goal.
+			double x,y;
+
+			if(actual_person_Companion_pointer_->get_past_trajectory()->size()>num_steps_orientation_){
+						if(debug_correct_angle_person_init_robot2_){std::cout <<" perso_print_back_traj(size-num_steps_orientation_)="<< std::endl;
+						actual_person_Companion_pointer_->get_past_trajectory()->at(actual_person_Companion_pointer_->get_past_trajectory()->size()-num_steps_orientation_).print();}
+						x=actual_person_Companion_pointer_->get_past_trajectory()->at(actual_person_Companion_pointer_->get_past_trajectory()->size()-num_steps_orientation_).x;
+						y=actual_person_Companion_pointer_->get_past_trajectory()->at(actual_person_Companion_pointer_->get_past_trajectory()->size()-num_steps_orientation_).y;
+						std::cout <<"; size="<<actual_person_Companion_pointer_->get_past_trajectory()->size()<<"; numsteps="<<num_steps_orientation_<<" if => perso_print_back_traj(size-num).x="<<x<<"; y="<<y<< std::endl;
+					}else{
+
+						if(debug_correct_angle_person_init_robot2_){std::cout <<" perso_print_back_traj(0)="<< std::endl;
+						actual_person_Companion_pointer_->get_past_trajectory()->at(0).print();}
+						x=actual_person_Companion_pointer_->get_past_trajectory()->at(0).x;
+						y=actual_person_Companion_pointer_->get_past_trajectory()->at(0).y;
+						std::cout <<" else => perso_print_back_traj(0).x="<<x<<"; y="<<y<< std::endl;
+					}
+
+		//	std::cout << " (V_pers < 0.2) IMPORTANTE!!! actual_group_goal.x="<<actual_group_goal.x<<"; actual_group_goal.y="<<actual_group_goal.y<<"; actual_first_person_companion_point.x="<<actual_first_person_companion_point.x<<"; actual_first_person_companion_point.y="<<actual_first_person_companion_point.y<<std::endl;
+		///	std::cout << " (V_pers < 0.2) IMPORTANTE!!! actual_person_Companion_pointer_->get_past_trajectory()->at(0).x="<<actual_person_Companion_pointer_->get_past_trajectory()->at(0).x<<"; actual_person_Companion_pointer_->get_past_trajectory()->at(0).y="<<actual_person_Companion_pointer_->get_past_trajectory()->at(0).y<<"; actual_first_person_companion_point.x="<<actual_first_person_companion_point.x<<"; actual_first_person_companion_point.y="<<actual_first_person_companion_point.y<<std::endl;
+
+
+
+
+			x_orient_goal= before_person_comp_goal_.x - actual_first_person_companion_point.x;//central_group_x;//actual_first_person_companion_point.vx;//(actual_group_goal.x-central_group_x);
+			y_orient_goal= before_person_comp_goal_.y - actual_first_person_companion_point.y ;//central_group_y;//actual_first_person_companion_point.vy;//(actual_group_goal.y-central_group_y);
+			orient_goal=atan(y_orient_goal/x_orient_goal);
+
+			//x_orient_goal=actual_group_goal.x- actual_first_person_companion_point.x;//central_group_x;//actual_first_person_companion_point.vx;//(actual_group_goal.x-central_group_x);
+			//y_orient_goal=actual_group_goal.y- actual_first_person_companion_point.y;//central_group_y;//actual_first_person_companion_point.vy;//(actual_group_goal.y-central_group_y);
+			//orient_goal=atan(y_orient_goal/x_orient_goal);
+
+		}else{ // the person stops, the goal of the robot is stop. the good point to calculate the orientation is the pose of the center of the group and the final goal.
+			//std::cout << " (hay V_pers) IMPORTANTE!!! actual_group_goal.x="<<actual_group_goal.x<<"; actual_group_goal.y="<<actual_group_goal.y<<"; central_group_x="<<central_group_x<<"; central_group_y="<<central_group_y<<std::endl;
+
+			x_orient_goal=actual_group_goal.x-central_group_x;
+			y_orient_goal=actual_group_goal.y-central_group_y;
+			orient_goal=atan(y_orient_goal/x_orient_goal);
+		}
+				//std::cout << " x_orient_goal="<<x_orient_goal<<"; y_orient_goal="<<y_orient_goal<<"; orient_goal="<<orient_goal<<std::endl;
+
+	}
+
+	// Option 2:  Calc preferred => using the mean of the orientations of both people, from they velocities.
+
+
+	//std::cout << " (solo 1 persona) x_orient_goal="<<actual_group_goal.x-pose_of_the_robot.x<<"; y_orient_goal="<<actual_group_goal.y-pose_of_the_robot.y<<"; orient_goal="<<atan((actual_group_goal.y-pose_of_the_robot.y)/(actual_group_goal.x-pose_of_the_robot.x))<<std::endl;
+	//std::cout << " (2 personas) x_orient_goal="<<actual_group_goal.x-central_group_x<<"; y_orient_goal="<<actual_group_goal.y-central_group_y<<"; orient_goal="<<atan((actual_group_goal.y-central_group_y)/(actual_group_goal.x-central_group_x))<<std::endl;
+
+
+	Vector2D preferred(x_orient_goal,y_orient_goal,orient_goal); // is the orientation until the goal of the group.
+
+	if(debug_zanlungo_){
+		std::cout << " Zanlungo model dist.R-P="<<pose_of_the_robot.distance(actual_first_person_companion_point)<<"; preferred.x="<<preferred.x<<"; preferred.y="<<preferred.y<<"; preferred.th="<<preferred.th<<std::endl;
+	}
+	preferred_paths_Zanlungo_.push_back(preferred);
+	// calculate the central point in the group.
+	if(debug_zanlungo_){
+		std::cout << "IMPORTANTE!!! Zanlungo; preferred.x="<<preferred.x<<"; preferred.x.y="<<preferred.y<<"; preferred.th="<<preferred.th<<std::endl;
+	}
+
+	Companion_Zanlungo_Model_.Order_goal(Self_for_order,number_of_group_people_,Others,preferred,order_people_in_group_global_variable_); // order contendrá el orden de los componentes del grupo.
+
+	// std::cout << " OUTH!  find_entity_order_in_the_group  "<< std::endl;
+
+}*/
+
+
+/////////////////////////////
+/*void Cplan_local_nav_person_companion::find_entity_order_in_the_group_for_person_companion(){
+
+	//std::cout << " IN!  find_entity_order_in_the_group  "<< std::endl;
+
+	State2D *Others=new State2D[number_of_group_people_];
+
+	// el robot, que ahora es person_companion_
+
+	Sdestination actual_group_goal=person_companion_->get_best_dest();//random_goal;;//robot_->get_best_dest();//random_goal;
+
+	//std::cout << " Zanlungo actual_group_goal.x="<<actual_group_goal.x<<"; actual_group_goal.y="<<actual_group_goal.y<<std::endl;
+
+
+	SpointV_cov pose_of_the_robot=person_companion_->get_current_pointV();
+
+	double robot_actual_time_stamp=person_companion_->get_current_pointV().time_stamp;
+
+	//std::cout << " Zanlungo 1; pose_of_the_robot.x="<<pose_of_the_robot.x<<"; pose_of_the_robot.y="<<pose_of_the_robot.y<<std::endl;
+	//std::cout << " robot_actual_time_stamp="<<robot_actual_time_stamp<<std::endl;
+
+	Vector2D robot_pose(pose_of_the_robot.vx,pose_of_the_robot.vy,atan(pose_of_the_robot.vy/pose_of_the_robot.vx)); // theta en rads, chec if is ok.
+
+	State2D Self_for_order;
+
+	Self_for_order.Init(pose_of_the_robot.x,pose_of_the_robot.y,pose_of_the_robot.vx,pose_of_the_robot.vy,robot_pose.th);
+
+
+
+	// first person companion: ahora el robot_
+
+	SpointV_cov actual_first_person_companion_point=robot_->get_current_pointV();
+
+	State2D first_companion_person(actual_first_person_companion_point.x,actual_first_person_companion_point.y,actual_first_person_companion_point.vx,actual_first_person_companion_point.vy);
+	Vector2D first_companion_person2D(actual_first_person_companion_point.vx,actual_first_person_companion_point.vy,atan(actual_first_person_companion_point.vy/actual_first_person_companion_point.vx));
+
+	Others[0]=first_companion_person;
+// no uso la trajectoria del robot.
+//std::cout << " robot_->get_planning_trajectory( ).size()="<<person_companion_->get_planning_trajectory( )->size()<<std::endl;
+
+
+	//std::cout <<"(FIRST) id_person="<<robot_->get_id()<< " first_companion_person.r.x="<<first_companion_person.r.x<<"; first_companion_person.r.y="<<first_companion_person.r.y<<std::endl;
+	//std::cout <<"(FIRST) actual_first_person_companion_point.x="<<actual_first_person_companion_point.x<< " actual_first_person_companion_point.y="<<actual_first_person_companion_point.y<<"; actual_first_person_companion_point.vx="<<actual_first_person_companion_point.vx<<"; actual_first_person_companion_point.vy="<<actual_first_person_companion_point.vy<<std::endl;
+
+
+	// second person companion point, ahora la segunda persona que acompaña. creo que este no cambia o es muy similar: second_group_companion_person_obj_
+
+	SpointV_cov actual_second_person_companion_point=SpointV_cov();
+
+	if((we_have_pointer_to_second_person_)&&(number_of_group_people_>1)){
+
+		actual_second_person_companion_point=second_group_companion_person_obj_->get_current_pointV();
+		State2D second_companion_person(actual_second_person_companion_point.x,actual_second_person_companion_point.y,actual_second_person_companion_point.vx,actual_second_person_companion_point.vy);
+		Vector2D second_companion_person2D(actual_second_person_companion_point.vx,actual_second_person_companion_point.vy,atan(actual_second_person_companion_point.vy/actual_second_person_companion_point.vx));
+		Others[1]=second_companion_person;
+		std::cout <<"(Second) id_person="<<second_group_companion_person_obj_->get_id()<< " second_companion_person.r.x="<<second_companion_person.r.x<<"; second_companion_person.r.y="<<second_companion_person.r.y<<std::endl;
+		std::cout <<"(Second) actual_second_person_companion_point.x="<<actual_second_person_companion_point.x<< " actual_second_person_companion_point.y="<<actual_second_person_companion_point.y<<"; actual_second_person_companion_point.vx="<<actual_second_person_companion_point.vx<<"; actual_second_person_companion_point.vy="<<actual_second_person_companion_point.vy<<std::endl;
+
+	}
+
+///
+	// TODO: parece que no uso theta.
+	//double theta=calc_person_companion_orientation(); // TODO: ver si cambia, que creo que si que cambiara, ya que la persona es otra.
+
+	//if(theta<0){
+	//	theta=3.14+theta;
+	//}
+
+
+	// Option 3: Calc preferred => using the mean of the position of the robot + the two person and the final goal. (works ok for two people + robot)
+
+	double central_group_x;
+	double central_group_y;
+
+	if(number_of_group_people_<2){
+		//std::cout << " solo 1_persona "<<std::endl;
+		central_group_x=(pose_of_the_robot.x + actual_first_person_companion_point.x)/(number_of_group_people_+1);//+actual_second_person_companion_point.x)/(number_of_group_people_+1);
+		central_group_y=(pose_of_the_robot.y + actual_first_person_companion_point.y)/(number_of_group_people_+1);//+actual_second_person_companion_point.x)/(number_of_group_people_+1);
+	}else{
+
+		//std::cout << " 2 _personas "<<std::endl;
+		//if(we_have_pointer_to_second_person_){
+		central_group_x=( actual_first_person_companion_point.x+actual_second_person_companion_point.x)/(number_of_group_people_+1);//+actual_second_person_companion_point.x)/(number_of_group_people_+1);
+		central_group_y=( actual_first_person_companion_point.y+actual_second_person_companion_point.y)/(number_of_group_people_+1);//+actual_second_person_companion_point.x)/(number_of_group_people_+1);
+
+
+			//central_group_x=(pose_of_the_robot.x + actual_first_person_companion_point.x+actual_second_person_companion_point.x)/(number_of_group_people_+1);//+actual_second_person_companion_point.x)/(number_of_group_people_+1);
+			//central_group_y=(pose_of_the_robot.y + actual_first_person_companion_point.y+actual_second_person_companion_point.y)/(number_of_group_people_+1);//+actual_second_person_companion_point.x)/(number_of_group_people_+1);
+		//	std::cout << " central_group_x= "<<central_group_x<<"; central_group_y="<<central_group_y<<std::endl;
+		//}
+
+	}
+	//std::cout << " (find_entity_order_in_the_group_for_person_companion) out calc center_group "<<std::endl;
+	// Option1: Calc preferred => using the robot position and the position of the Goal tu calculate the orientation.
+	double x_orient_goal;
+	double y_orient_goal;
+	double orient_goal;
+
+	if(number_of_group_people_<2){
+	//	std::cout << "IMPORTANTE!!! actual_group_goal.x="<<actual_group_goal.x<<"; actual_group_goal.y="<<actual_group_goal.y<<"; pose_of_the_robot.x="<<pose_of_the_robot.x<<"; pose_of_the_robot.y="<<pose_of_the_robot.y<<std::endl;
+
+		x_orient_goal=actual_group_goal.x-pose_of_the_robot.x;
+		y_orient_goal=actual_group_goal.y-pose_of_the_robot.y;
+		orient_goal=atan(y_orient_goal/x_orient_goal);
+	//	std::cout << " x_orient_goal="<<x_orient_goal<<"; y_orient_goal="<<y_orient_goal<<"; orient_goal="<<orient_goal<<std::endl;
+
+	}else{
+		//std::cout << " Else -> if(number_of_group_people_<2)  [find_entity_order_in_the_group_for_person_companion()]"<<std::endl;
+
+		//if(we_have_pointer_to_second_person_){
+		//
+		//}
+
+		if((!we_have_pointer_to_second_person_)&&(actual_person_Companion_pointer_->get_current_pointV().v()<0.1)){ // the person is walking, the goal is the final goal.
+			std::cout << "IMPORTANTE!!! actual_group_goal.x="<<actual_group_goal.x<<"; actual_group_goal.y="<<actual_group_goal.y<<"; actual_first_person_companion_point.x="<<actual_first_person_companion_point.x<<"; actual_first_person_companion_point.y="<<actual_first_person_companion_point.y<<std::endl;
+
+			x_orient_goal=actual_group_goal.x- actual_first_person_companion_point.x;//central_group_x;//actual_first_person_companion_point.vx;//(actual_group_goal.x-central_group_x);
+			y_orient_goal=actual_group_goal.y- actual_first_person_companion_point.y;//central_group_y;//actual_first_person_companion_point.vy;//(actual_group_goal.y-central_group_y);
+			orient_goal=atan(y_orient_goal/x_orient_goal);
+
+		}else{ // the person stops, the goal of the robot is stop. the good point to calculate the orientation is the pose of the center of the group and the final goal.
+		//	std::cout << "IMPORTANTE!!! actual_group_goal.x="<<actual_group_goal.x<<"; actual_group_goal.y="<<actual_group_goal.y<<"; central_group_x="<<central_group_x<<"; central_group_y="<<central_group_y<<std::endl;
+
+			x_orient_goal=actual_group_goal.x-central_group_x;
+			y_orient_goal=actual_group_goal.y-central_group_y;
+			orient_goal=atan(y_orient_goal/x_orient_goal);
+		}
+				//std::cout << " x_orient_goal="<<x_orient_goal<<"; y_orient_goal="<<y_orient_goal<<"; orient_goal="<<orient_goal<<std::endl;
+
+	}
+
+	// Option 2:  Calc preferred => using the mean of the orientations of both people, from they velocities.
+
+
+	//std::cout << " (solo 1 persona) x_orient_goal="<<actual_group_goal.x-pose_of_the_robot.x<<"; y_orient_goal="<<actual_group_goal.y-pose_of_the_robot.y<<"; orient_goal="<<atan((actual_group_goal.y-pose_of_the_robot.y)/(actual_group_goal.x-pose_of_the_robot.x))<<std::endl;
+	//std::cout << " (2 personas) x_orient_goal="<<actual_group_goal.x-central_group_x<<"; y_orient_goal="<<actual_group_goal.y-central_group_y<<"; orient_goal="<<atan((actual_group_goal.y-central_group_y)/(actual_group_goal.x-central_group_x))<<std::endl;
+
+
+	Vector2D preferred(x_orient_goal,y_orient_goal,orient_goal); // is the orientation until the goal of the group.
+
+	if(debug_zanlungo_){
+		std::cout << " Zanlungo model dist.R-P="<<pose_of_the_robot.distance(actual_first_person_companion_point)<<"; preferred.x="<<preferred.x<<"; preferred.y="<<preferred.y<<"; preferred.th="<<preferred.th<<std::endl;
+	}
+	preferred_paths_Zanlungo_.push_back(preferred);
+	// calculate the central point in the group.
+	//if(debug_zanlungo_){
+	//	std::cout << "IMPORTANTE!!! Zanlungo; preferred.x="<<preferred.x<<"; preferred.x.y="<<preferred.y<<"; preferred.th="<<preferred.th<<std::endl;
+	//}
+
+	//Companion_Zanlungo_Model_.Order_goal(Self_for_order,number_of_group_people_,Others,preferred,order_people_in_group_global_variable_); // order contendrá el orden de los componentes del grupo.
+
+	// std::cout << " OUTH!  find_entity_order_in_the_group  "<< std::endl;
+
+}*/
+
+
+
+///////////////////
+
+// Function to find mean.
+double Cplan_local_nav_person_companion::mean(std::vector<double> arr, int n)
+{
+    double sum = 0;
+    for(int i = 0; i < n; i++)
+        sum = sum + arr.at(i);
+    return sum / n;
+}
+
+// Function to find covariance.
+double Cplan_local_nav_person_companion::covariance(std::vector<double> arr1, std::vector<double> arr2, int n)
+{
+    double sum = 0;
+    for(int i = 0; i < n; i++)
+        sum = sum + (arr1.at(i) - mean(arr1, n)) *
+                    (arr2.at(i) - mean(arr2, n));
+    return sum / (n - 1);
+}
+
+double Cplan_local_nav_person_companion::variance(std::vector<double> a, int n)
+{
+    // Compute mean (average of elements)
+    double sum = 0;
+    for (int i = 0; i < n; i++)
+        sum += a.at(i);
+    double mean =  sum /
+                  (double)n;
+
+    // Compute sum squared
+    // differences with mean.
+    double sqDiff = 0;
+    for (int i = 0; i < n; i++)
+        sqDiff += (a.at(i) - mean) *
+                  (a.at(i) - mean);
+    return sqDiff / n;
+}
diff --git a/local_lib_people_prediction/src/nav/plan_local_nav_person_companion.h b/local_lib_people_prediction/src/nav/plan_local_nav_person_companion.h
new file mode 100644
index 0000000000000000000000000000000000000000..a81d5d140f802ca87ad6c5d85a06b2fc46c3b87e
--- /dev/null
+++ b/local_lib_people_prediction/src/nav/plan_local_nav_person_companion.h
@@ -0,0 +1,1875 @@
+/*
+ * plan_local_nav_person_companion.h
+ *
+ *  Created on: Jan 5, 2019  by Ely Repiso to separate the elements of the Scene_sim for the person companion, from the planner elements of the robot.
+ *
+ */
+
+#ifndef PLAN_LOCAL_NAV_PERSON_COMPANION_H_
+#define PLAN_LOCAL_NAV_PERSON_COMPANION_H_
+
+#include "prediction_behavior.h"
+#include "scene_elements/person_abstract.h"
+#include "prediction_bhmip.h"
+#include "nav/force_reactive_robot_companion.h"
+
+//#include "companion_zanlungo/CompanionFrancescoModel.h" // library of Francesco's companion model
+
+#include <fstream>      // std::ofstream
+#include <random>
+#include <iostream>
+#include <string>
+#include <stdio.h>
+#include <math.h>
+
+#include<bits/stdc++.h>
+//using namespace std;
+
+//#include "matlab.hpp"
+
+using namespace std;
+/**
+ *	\brief Sedge_tree_pcomp Struct
+ *
+ *	data struct describing a tree edge containing the index to the parent vertex
+ *	and the command inputs (v.w) or forces required to get from that vertex state
+ */
+
+
+class Sedge_tree_pcomp
+{
+  public:
+	Sedge_tree_pcomp(	unsigned int parent, Sforce f_=Sforce(), Sforce f_goal_=Sforce(),
+			Sforce f_people_=Sforce(),Sforce f_obs_=Sforce(), Sforce f_persongoal_=Sforce());
+	unsigned int parent;
+	Sforce f;
+	Sforce f_goal;
+	Sforce f_people;
+	Sforce f_obs;
+	Sforce f_persongoal;
+	double cost(Sedge_tree_pcomp parent)const;
+	void print();
+};
+
+/**
+ *	\brief Smulticost_pcom Struct
+ *
+ *	data struct for containing multiciple costs and compare them in a fast and clean way
+ *	specially to determine dominance relations between solutions.
+ */
+class Smulticost_pcom
+{
+  public:
+	Smulticost_pcom( unsigned int id_ = 0, unsigned int n = 1 );
+	std::vector<double> cost;
+	unsigned int id;
+	bool operator< ( const Smulticost_pcom& m2) const;
+	bool operator==( const Smulticost_pcom& m2) const;
+	bool operator!=( const Smulticost_pcom& m2) const;
+	void print() const;
+	void print_ml() const;
+};
+
+/**
+ *	\brief Cplan_local_nal
+ *
+ * Basic local planner using social forces
+ *
+ */
+class Cplan_local_nav_person_companion : public Cprediction_behavior
+{
+  public:
+    enum plan_mode{  F_RRT_Uniform=0,  F_RRT_Gauss_Circle, F_RRT_GC_alpha};
+    enum distance_mode{  Euclidean=0,  Cost2go_erf, Cost2go_norm , Cost2go_raw};
+    enum global_mode{ Scalarization=0, Weighted_sum_erf, Weighted_sum_norm, MO_erf, MO_norm};
+    enum action_mode{START=0,ITER,FACEPERSON,STOP};
+    enum simulation_case{case0=0,case1,case2} ;
+    enum calculate_complete_group_path_{akp_planner=0,person_prediction,prediction_total_time_with_collisions};
+
+
+
+    // funciones a eliminar de person goal/target
+
+    void change_set_id_person_goal(){  // compartida con simul_people (no la puedo eliminar)
+        	id_person_goal_=change_id_person_goal_;
+        	//this->Cprediction_behavior::set_Cprediction_behavior_id_target_person(id_person_goal_);
+        	 Cperson_abstract* person_obj1;
+        	bool finded_person=find_person(id_person_goal_ , &person_obj1);
+        	if(finded_person){
+        		person_obj1->set_person_type(Cperson_abstract::Person_companion);
+        	}
+
+        	//std::cout << " id_person_goal_="<<id_person_goal_<< std::endl;
+
+        }
+    void set_id_person_goal(unsigned int in_id_person_goal){
+        	//id_person_goal_=in_id_person_goal;
+        	change_id_person_goal_=in_id_person_goal;
+         }
+    void set_plan_local_nav_group_go_to_interact_with_other_person(bool in_group_go_to_interact_with_other_person){ // compartida con simul_people (no la puedo eliminar)
+        	group_go_to_interact_with_other_person_=in_group_go_to_interact_with_other_person;
+        }
+
+    Spoint get_SIM_initial_person_goal_pose_actual(){  // compartida con simul_people (no la puedo eliminar)
+            	//std::cout << " SIM_initial_person_goal_pose_actual_:"<< std::endl;
+         	//SIM_initial_person_goal_pose_actual_.print();
+         	return SIM_initial_person_goal_pose_actual_;
+         }
+   // enum vel_per_ok{ Vel_per=0, Near_goal, Far_goal};
+    /**
+     * \brief Initial algorithm parameters in the constructor of the
+     * algorithm
+     */
+    Cplan_local_nav_person_companion(double horizon_time = 3.0, unsigned int max_iter = 1000,
+    		plan_mode mode= Cplan_local_nav_person_companion::F_RRT_Uniform, bool robot_or_person=false);
+    virtual ~Cplan_local_nav_person_companion(); // false == robot= id_0; true == person_companion =id_1
+
+
+    /**
+     * \brief this is the public interface to calculate the robot==person_companion plan
+     * according to the observed scene and the algorithm parameters.
+     */
+
+    bool person_companion_plan_companion(Spose& pose_command, Cperson_abstract::companion_reactive& reactive,double dt=0.0,std::vector<Sdestination>* person_best_dest=NULL);
+
+    double get_distance_to_goal() { return robot_->get_current_pointV().distance( goal_ );}
+    /**
+     * Configuration methods
+     */
+    void set_planning_mode( plan_mode mode ) {plan_mode_ = mode;}
+    plan_mode get_planning_mode(){ return plan_mode_; }
+    void set_distance_mode( distance_mode mode )
+    	{distance_mode_ = mode; nondominated_plan_vertex_index_.clear(); nondominated_end_of_plan_vertex_index_.clear();}
+    distance_mode get_distance_mode(){ return distance_mode_;}
+    void set_global_mode( global_mode mode )
+    	{global_mode_ = mode; nondominated_plan_vertex_index_.clear();nondominated_end_of_plan_vertex_index_.clear();}
+    global_mode get_global_mode(){ return global_mode_;}
+    void set_ppl_collision_mode( int ppl_col ) {ppl_collision_mode_ =  ppl_col; }
+    void set_pr_force_mode( int pr_force_mode ) { pr_force_mode_ = pr_force_mode;}
+    /**
+     * \brief set_robot_goal() function sets the value of the goal position to
+     * which the planning aims to. It is assumed that a global planner, above this
+     * implementation, provides these goals
+     */
+    void set_robot_goal(const  Spoint& goal );
+    void set_robot_goal_person_companion_akp( const Spoint& goal );
+    void set_robot_goal_person_goal_global_plan( const Spoint& goal );
+    void set_robot_goal_person_goal_global_plan_IN_robot_VERSION( const Spoint& goal );
+    void set_robot_external_goal( const Spoint& goal ); // comapnion => obtain a external goal, not infered from the person comapnion
+    void set_robot_external_goal_fix( const Spoint& goal );
+
+    Spoint get_robot_goal() { return goal_;}
+    Spoint get_external_robot_goal(){ return Spoint(extern_robot_goal_.x,extern_robot_goal_.y,extern_robot_goal_.time_stamp);}
+    Spoint get_robot_local_goal() { return local_goal_;}
+    void set_plan_cost_parameters( double c_dist, double c_orientation, double c_w_robot,
+    		double c_w_people, double c_time, double c_w_obstacles, double c_old_path, double c_l_minima);
+    const std::vector<double>* get_plan_cost_parameters() const { return &cost_parameters_;}
+    void set_number_of_vertex( unsigned int n );
+    unsigned int get_number_of_vertex(  ) { return max_iter_;}
+    void set_robot_params( double v, double w, double av, double av_break,double aw, double platform_radii);
+    void set_xy_2_goal_tolerance( double tol) { xy_2_goal_tolerance_ = tol; }
+    void set_v_goal_tolerance( double tol) { v_goal_tolerance_ = tol; }
+    void set_distance_to_stop( double d ){ distance_to_stop_ = d; }
+    double get_distance_to_stop( ) { return distance_to_stop_;}
+
+    /**
+     * Return info methods
+     */
+    const std::vector<unsigned int>* get_robot_plan_index() const {
+    	//std::cout <<" best_plan_vertex_index_.size()="<<best_plan_vertex_index_.size()<< std::endl;
+    	return &best_plan_vertex_index_;}
+    const std::vector<unsigned int>* get_robot_nondominated_plan_index() const {
+    	//std::cout <<" nondominated_plan_vertex_index_.size()="<<nondominated_plan_vertex_index_.size()<< std::endl;
+    	return &nondominated_plan_vertex_index_;
+    }
+    const std::vector<unsigned int>* get_robot_nondominated_end_of_plan_index() const {
+    	//std::cout <<" nondominated_end_of_plan_vertex_index_.size()="<<nondominated_end_of_plan_vertex_index_.size()<< std::endl;
+    	return &nondominated_end_of_plan_vertex_index_;
+    }
+    const std::vector<Sedge_tree_pcomp>* get_plan_edges() const { return &edge_;}
+    const std::vector<Spoint>* get_random_goals() const { return &random_goals_;}
+    double get_workspace_radii() { return workspace_radii_;}
+
+    Spose get_best_planned_pose_person_companion_akp(double dt=0.0);
+    void get_navigation_instant_work( double& work_robot, double& work_persons );
+    void get_navigation_cost_values(  std::vector<double>& costs )//returns the vector of bests costs, calculated in the global_cost routine
+    	{ costs = best_costs_;}
+    void get_navigation_mean_cost_values(  std::vector<double>& costs ) { costs = mean_costs_;}
+    void get_navigation_std_cost_values(  std::vector<double>& costs ) { costs = std_costs_;}
+	/**
+	 * \brief calculate scene cost
+	 *
+	 * This function calculates the costs due to robot navigation and
+	 * nearby moving people. It is used only as an observation of the scene,
+	 * no calculations are done, is just for evaluating purposes.
+	 */
+    void calculate_navigation_cost_values( std::vector<double>& costs );
+    // people companion ely functions
+    double get_actual_companion_angle() { return min_next_companion_angle_; }
+    double get_actual_companion_cost() { return min_next_companion_cost_; }
+    SpointV_cov get_companion_person_position() {return companion_person_position_;}
+    Sdestination get_person_destination() {return person_companion_goal_;}
+
+    //*** companion dynamic reconfigure (ini) ***//
+    unsigned int get_id_person_companion(){
+       	//return id person companion
+       	return id_person_companion_;
+    }
+
+    void set_id_person_companion(unsigned int in_id_person_companion){
+    	//id_person_companion_=in_id_person_companion;
+    	change_id_person_companion_=in_id_person_companion;
+    }
+
+    void set_id_person_companion_people_simulator(unsigned int in_id_person_companion){
+    	change_id_person_companion_=in_id_person_companion;
+    	change_set_id_person_companion();
+    }
+
+    void change_set_id_person_companion(){
+    	id_person_companion_=change_id_person_companion_;
+    	this->Cprediction_behavior::set_Cprediction_behavior_id_person_companion(change_id_person_companion_);
+        this->Cprediction_bhmip::set_id_person_companion_Cprediction_bhmip(change_id_person_companion_);
+        Cperson_abstract* person_obj1;
+        bool finded_person=find_person(id_person_companion_ , &person_obj1);
+        if(finded_person){
+        	person_obj1->set_person_type(Cperson_abstract::Person_companion);
+        }
+       // std::cout << " (SET SET SET SET SET) => id_person_companion_="<<id_person_companion_<< std::endl;
+    }
+
+
+    /*void set_id_SECOND_person_companion_people_simulator(unsigned int in_id_second_person_companion){
+
+    	id_SECOND_person_companion_=in_id_second_person_companion;
+    	this->Cprediction_behavior::set_Cprediction_behavior_id_second_person_companion(in_id_second_person_companion);
+    	this->Cprediction_bhmip::set_id_second_person_companion_Cprediction_bhmip(in_id_second_person_companion);
+
+    	 Cperson_abstract* person_obj1;
+    	bool finded_person=find_person(id_SECOND_person_companion_ , &person_obj1);
+    	        if(finded_person){
+    	        	person_obj1->set_person_type(Cperson_abstract::Person_companion);
+    	        }
+    }*/
+
+
+    ////////////////
+    unsigned int my_id_person_companion_simulation_; // id, actual person companion. the id of it selft (todo: ver si me hace falta verdaderamente)
+    void set_my_id_person_companion_people_simulator(unsigned int in_my_id_person_companion){
+
+        	my_id_person_companion_simulation_=in_my_id_person_companion;
+        	this->Cprediction_behavior::set_Cprediction_behavior_my_id_second_person_companion_sim(in_my_id_person_companion);
+        	this->Cprediction_bhmip::set_my_id_second_person_companion_sim_Cprediction_bhmip(in_my_id_person_companion);
+
+        	// creo que no hace falta, ya que los ponemos en los otros id's.
+        	// Cperson_abstract* person_obj1;
+        	//bool finded_person=find_person(id_SECOND_person_companion_ , &person_obj1);
+        	       // if(finded_person){
+        	       // 	person_obj1->set_person_type(Cperson_abstract::Person_companion);
+        	       // }
+        }
+
+
+    /////////
+
+
+
+    void set_robot_person_proximity_distance(double in_proximity_distance_between_robot_and_person){
+    	robot_person_proximity_distance_=in_proximity_distance_between_robot_and_person;
+    }
+    void set_proximity_distance_tolerance(double in_proximity_distance_tolerance){
+    	robot_person_proximity_tolerance_=in_proximity_distance_tolerance;
+    }
+    void set_additional_distance_companion_sphere(double in_add_dist_companion_sphere){
+    	robot_adition_complete_esphere_companion_distance_=in_add_dist_companion_sphere;
+    	std::cout << "robot_adition_complete_esphere_companion_distance_="<<robot_adition_complete_esphere_companion_distance_<< std::endl;
+    }
+    void set_proximity_goals_robot_and_person(double in_proximity_goals_robot_and_person_x,double in_proximity_goals_robot_and_person_y){
+    	robot_person_proximity_goals_x_=in_proximity_goals_robot_and_person_x;
+    	robot_person_proximity_goals_y_=in_proximity_goals_robot_and_person_y;
+    }
+    void set_offset_attractive(double in_offset_attractive_state){
+    	offset_atractive_=in_offset_attractive_state;
+    }
+    void set_force_obs_max(double in_force_obs_max_x,double in_force_obs_max_y){
+    	f_obst_max_x_=in_force_obs_max_x;
+    	f_obst_max_y_=in_force_obs_max_y;
+    }
+    void set_real_companion_angle(double in_real_companion_angle){
+    	angle_companion_=in_real_companion_angle;
+    	before_initial_angle_=in_real_companion_angle;
+    	min_next_companion_angle_=in_real_companion_angle;
+    }
+
+    void set_person_goal_percentage(double in_person_goal_percentage){
+    	person_goal_percent_=in_person_goal_percentage;
+    }
+
+    double get_robot_person_proximity_distance(){
+    	return robot_person_proximity_distance_;
+     }
+
+    Sdestination get_person_companion_goal_out(){
+        return person_companion_goal_out_;
+    }
+
+    void set_companion_same_person_goal(bool act_companion_same_person_goal){
+
+    	companion_same_person_goal_=act_companion_same_person_goal;
+    	std::cout << "companion_same_person_goal_="<<companion_same_person_goal_<< std::endl;
+    }
+
+    void set_overpas_obstacles_behind_person(bool in_overpas_obstacles_behind_person){
+    	overpas_obstacles_behind_person_=in_overpas_obstacles_behind_person;
+    }
+
+    void set_anisotropy_threshold(double in_anisotropy_threshold){
+    	anisotropy_threshold_=in_anisotropy_threshold;
+    }
+
+    void set_max_d_to_detect_laser_obs(double in_max_d_to_detect_laser_obs){
+    	Cprediction_behaviour_set_max_d_to_detect_laser_obs(in_max_d_to_detect_laser_obs);
+    	max_distance_to_obstacles_detected_=in_max_d_to_detect_laser_obs;
+    	std::cout << "set_max_d_to_detect_laser_obs-> in_max_d_to_detect_laser_obs="<<in_max_d_to_detect_laser_obs<< std::endl;
+    }
+
+    void set_save_results_in_file(bool in_save_results_in_file){
+    	save_results_in_file_=in_save_results_in_file;
+       }
+
+    void set_meters_to_goal_to_save_results_in_file(double in_meters_goal_to_save_results_in_file){
+    	meters_goal_to_save_results_in_file_=in_meters_goal_to_save_results_in_file;
+    }
+
+    void set_mode_velocity(bool in_mode_velocity){
+    	mode_velocity_=in_mode_velocity;
+    }
+
+    void set_results_filename(std::string in_results_file){
+    	results_file_=in_results_file;
+    	std::cout << " (changed) results_file_="<<results_file_<< std::endl;
+    }
+
+    void set_reduce_max_vel_dist(double in_reduce_max_vel_dist){
+    	reduce_max_vel_dist_=in_reduce_max_vel_dist;
+    }
+
+    void set_marge_in_distance(double in_marge_in_distance){
+    	marge_in_distance_=in_marge_in_distance;
+    }
+
+    void set_marge_angle_companion(double in_marge_angle_companion){
+    	marge_angle_companion_=in_marge_angle_companion;
+    }
+
+    void set_debug_filename(std::string in_debug_file){
+    	debug_file_=in_debug_file;
+    }
+
+    void set_mode_debug_cout(bool in_debug_robot_cout_robot){
+    	debug_cout_robot_=in_debug_robot_cout_robot;
+    }
+
+    void set_mode_debug_file(bool in_debug_file_robot){
+    	debug_file_robot_=in_debug_file_robot;
+    }
+
+
+
+    void set_externa_force_k_near_goal_akp(double in_k_near){
+    	externa_force_k_near_goal_akp_=in_k_near;
+    	std::cout << " (changed) externa_force_k_near_goal_akp_="<<externa_force_k_near_goal_akp_<< std::endl;
+    }
+
+    void set_ex_max_dist_to_near_goal_force_akp(double in_max_dist_to_near_goal_force){
+    	max_dist_to_near_goal_force_akp_=in_max_dist_to_near_goal_force;
+    	std::cout << " (changed) max_dist_to_near_goal_force_akp_="<<max_dist_to_near_goal_force_akp_<< std::endl;
+    }
+
+    void set_externa_force_k_far_goal_akp(double in_externa_force_k_far_goal_akp_){
+    	externa_force_k_far_goal_akp_=in_externa_force_k_far_goal_akp_;
+    	std::cout << " (changed) externa_force_k_far_goal_akp_="<<externa_force_k_far_goal_akp_<< std::endl;
+    }
+
+
+    void set_mode_step_near(bool in_mode_step_near){
+    	mode_step_near_=in_mode_step_near;
+    	std::cout << " (changed) mode_step_near_="<<mode_step_near_<< std::endl;
+   }
+
+    /*void set_out_index_step(int in_out_index_step){
+    	out_index_step_=in_out_index_step;
+    	std::cout << " (changed) out_index_step_="<<out_index_step_<< std::endl;
+   }*/
+
+    void set_out_index_step_companion_goal(unsigned int in_out_index_step_companion_goal){
+        	out_index_step_companion_goal_=in_out_index_step_companion_goal;
+        	std::cout << " (changed) out_index_step_companion_goal_="<<out_index_step_companion_goal_<< std::endl;
+       }
+
+    void set_out_index_step_final_dest_goal(unsigned int in_out_index_step_final_dest_goal){
+        	out_index_step_final_dest_goal_=in_out_index_step_final_dest_goal;
+        	std::cout << " (changed) out_index_step_final_dest_goal_="<<out_index_step_final_dest_goal_<< std::endl;
+       }
+
+    void only_comp_people_vel_and_robot_poses(int in_only_comp_people_vel_and_robot_poses){
+    	only_comp_people_vel_and_robot_poses_=in_only_comp_people_vel_and_robot_poses;
+    }
+    //this->planner_.set_ex_max_dist_to_near_goal_force_akp(config.ex_max_dist_to_near_goal_force);
+    //this->planner_.set_max_d_to_detect_laser_obs(config.detection_laser_obstacle_distances);
+    //*** companion dynamic reconfigure (fin) ***//
+
+    void set_person_radi_amp(double in_person_radi_amp){
+    	person_radi_amp_=in_person_radi_amp;
+    	std::cout << " IMPORTANT!!!!!!!!!!! (changed) person_radi_amp_="<<person_radi_amp_<< std::endl;
+    	person_radi_=in_person_radi_amp +0.2;
+    	person_radi2_=in_person_radi_amp-0.1;
+    	person_radi_per_comp_=in_person_radi_amp;
+       }
+    void set_obstacle_radi_amp(double in_obstacle_radi_amp){
+    	obstacle_radi_amp_=in_obstacle_radi_amp;
+    	obstacle_radi_=in_obstacle_radi_amp;
+    	obstacle_radi2_=in_obstacle_radi_amp;
+    	std::cout << " IMPORTANT!!!!!!!!!!! (changed) obstacle_radi_amp_="<<obstacle_radi_amp_<< std::endl;
+    }
+    void set_ini_vel_to_increment_angle(double in_ini_vel_to_increment_angle){
+    	ini_vel_to_increment_angle_=in_ini_vel_to_increment_angle;
+    	std::cout << " (changed) ini_vel_to_increment_angle_="<<ini_vel_to_increment_angle_<< std::endl;
+    }
+
+    double get_horizon_time(){return horizon_time_;}
+
+    void set_person_list(std::list<Cperson_abstract *> in_person_list_){
+    	person_list_=in_person_list_;
+    }
+
+    std::list<Cperson_abstract *> get_person_list(){
+     	return person_list_;
+     }
+    void set_person_companion_desired_velocity_sim(double in_person_companion_desired_velocity_sim){
+    	person_companion_desired_velocity_sim_=in_person_companion_desired_velocity_sim;
+    }
+
+    void set_alpha_companion(double in_alpha_companion){
+    	alpha_companion_=in_alpha_companion;
+    }
+
+    void set_beta_companion(double in_beta_companion){
+    	beta_companion_=in_beta_companion;
+    }
+
+    double get_alpha_companion(){
+    	return alpha_companion_;
+    }
+
+    double get_beta_companion(){
+    	return beta_companion_;
+    }
+
+    void set_evaluate_costs_filename(std::string in_evaluate_costs_file){
+    	evaluate_costs_file_=in_evaluate_costs_file;
+        std::cout << " (changed) evaluate_costs_file_="<<evaluate_costs_file_<< std::endl;
+    }
+
+
+	void set_evaluate_change_distance_and_angle_companion_filename(std::string in_evaluate_change_distance_and_angle_companion_file){
+    	evaluate_change_distance_and_angle_companion_file_=in_evaluate_change_distance_and_angle_companion_file;
+	    std::cout << " (changed) evaluate_change_distance_and_angle_companion_file__file_="<<evaluate_change_distance_and_angle_companion_file_<< std::endl;
+	}
+
+    Sdestination get_person_companion_goal(){
+        //std::cout << " person_companion_goal_.x="<<person_companion_goal_.x<<"; person_companion_goal_.y="<<person_companion_goal_.y<< std::endl;
+        return robot_goal_to_person_companion_;
+     }
+    Sdestination get_robot_path_goal(){
+        //std::cout << " robot_path_goal_.x="<<robot_path_goal_.x<<"; robot_path_goal_.y="<<robot_path_goal_.y<< std::endl;
+        return robot_path_goal_;
+     }
+    Spose get_final_combined_goal(){
+        //std::cout << " final_combined_goal_.x="<<final_combined_goal_.x<<"; final_combined_goal_.y="<<final_combined_goal_.y<< std::endl;
+        return final_combined_goal_;
+     }
+
+
+
+    action_mode get_state_Action(){
+    	//std::cout << " final_combined_goal_.x="<<final_combined_goal_.x<<"; final_combined_goal_.y="<<final_combined_goal_.y<< std::endl;
+    	return Action_;
+    }
+
+    void set_state_Action(action_mode Action_in){
+    	//std::cout << " final_combined_goal_.x="<<final_combined_goal_.x<<"; final_combined_goal_.y="<<final_combined_goal_.y<< std::endl;
+    	Action_=Action_in;
+    }
+
+    Spoint get_SIM_initial_person_companion_pose1(){
+       	//std::cout << " SIM_initial_person_companion_pose1_:"<< std::endl;
+    	//SIM_initial_person_companion_pose1_.print();
+    	return SIM_initial_person_companion_pose1_;
+     }
+    Spoint get_SIM_initial_person_companion_pose2(){
+       	//std::cout << " SIM_initial_person_companion_pose1_:"<< std::endl;
+    	//SIM_initial_person_companion_pose1_.print();
+    	return SIM_initial_person_companion_pose2_;
+     }
+
+    simulation_case get_actual_case(){
+    	//std::cout << " SIM_initial_person_companion_pose1_:"<< std::endl;
+    	//SIM_initial_person_companion_pose1_.print();
+    	return actual_case_;
+    }
+
+    void set_actual_case(simulation_case actual_case_in){
+    	//std::cout << " SIM_initial_person_companion_pose1_:"<< std::endl;
+    	//SIM_initial_person_companion_pose1_.print();
+    	actual_case_=actual_case_in;
+    }
+
+    void set_ros_time_to_sec(double ros_time_to_sec_in){
+    	ros_time_to_sec_=ros_time_to_sec_in;
+    }
+
+       void set_is_act_person_companion_bool(bool is_act_person_companion_in){
+    	   is_act_person_companion_=is_act_person_companion_in;
+       }
+
+
+
+       void set_restart_real(bool in_restart_real){
+    	   restart_real_=in_restart_real;
+       }
+
+
+       void set_max_dist_to_go_behind(double in_max_dist_to_go_behind){
+    	   max_dist_to_go_behind_=in_max_dist_to_go_behind;
+       }
+
+
+
+       void set_restart_real_data_txt(){
+    	   new_matlab_file();
+    	  // new_Zanlungo_file();
+    	   iteration_=1;
+    	   experiment_=1;
+       }
+
+       void set_change_sim(bool change_sim){
+    	   sim=change_sim;
+    	   this->Cprediction_behavior::set_sim_prediction_behaviour(sim);
+    	   this->Cscene_abstract::set_sim(sim);
+       }
+
+       void set_change_sim_target_per(bool change_sim_target_per){
+    	   sim_target_per=change_sim_target_per;
+         }
+
+       void set_change_external_to_stop_case(bool in_change_external_to_stop_case){
+    	   if(in_change_external_to_stop_case){
+    		   Action_=Cplan_local_nav_person_companion::STOP;
+    	   }
+       }
+
+       void set_case_stop_giro(bool in_case_stop_giro){
+    	   case_stop_giro_=in_case_stop_giro;
+         }
+
+       void set_incremento_giro_positivo(double in_incremento_giro_positivo){
+    	   incremento_giro_positivo_=in_incremento_giro_positivo;
+         }
+       void set_incremento_giro_negativo(double in_incremento_giro_negativo){
+    	   incremento_giro_negativo_=in_incremento_giro_negativo;
+         }
+
+
+       Sforce get_force_int_between_person_comp_and_robot(){
+       	return force_int_between_person_comp_and_robot_;
+       }
+
+       // set values of performance approaching
+
+       void set_value_distance_global_path(double in_value_distance_global_path){
+    	   value_distance_global_path_=in_value_distance_global_path;
+    	  // std::cout << " (planner in) value_distance_global_path_="<<value_distance_global_path_<< std::endl;
+       }
+
+       void set_global_path_ini_orientation(double in_global_path_ini_orientation){
+    	   global_path_ini_orientation_=in_global_path_ini_orientation;
+           //std::cout << " (planner in) global_path_ini_orientation_="<<global_path_ini_orientation_<< std::endl;
+        }
+       void set_global_path_final_orientation(double in_global_path_final_orientation){
+    	   global_path_final_orientation_=in_global_path_final_orientation;
+           //std::cout << " (planner in) global_path_final_orientation_="<<global_path_final_orientation_<< std::endl;
+        }
+
+
+       SpointV_cov get_robot_pose(){
+    	   return robot_->get_current_pointV();
+       }
+
+       void set_robot_pose_for_person_companion_simulation(Spose in_robot_pose){
+    	   robot_->set_current_pose(in_robot_pose);
+    	   //std::cout << " (planner in, set_robot_pose_for_person_companion_simulation) in_robot_pose.x="<<in_robot_pose.x<<"; in_robot_pose.y="<<in_robot_pose.y<< std::endl;
+    	   //std::cout << " (planner in, set_robot_pose_for_person_companion_simulation) robot_.get.x="<<robot_->get_current_pose().x<<"; robot_.get.y="<<robot_->get_current_pose().y<< std::endl;
+       }
+
+
+/// FUNCIONES debidas a optimizacion del codigo solo validas para person_companion (simulacion)
+
+      /* void set_actual_person_Companion_SpointV(SpointV_cov in_actual_person_Companion_SpointV){
+    	   actual_person_Companion_SpointV_=in_actual_person_Companion_SpointV;
+       }*/
+
+       /*void set_actual_person_Companion_destination(Sdestination in_actual_person_Companion_destination){
+    	   actual_person_Companion_destination_=in_actual_person_Companion_destination;
+       }*/
+
+      /* void set_max_desired_person_Companion_velocity(double in_max_desired_person_Companion_velocity){
+    	   max_desired_person_Companion_velocity_=in_max_desired_person_Companion_velocity;
+       }*/
+
+      /* void set_actual_person_Companion_pointer(Cperson_abstract *in_actual_person_Companion_pointer){
+    	   actual_person_Companion_pointer_=in_actual_person_Companion_pointer;
+       }*/
+
+   	void set_final_goal_reached_in_node(bool in_final_goal_reached_in_node){
+   		final_goal_reached_in_node_=in_final_goal_reached_in_node;
+   	}
+
+
+   	Spoint get_medium_point(){
+   		return medium_point_;
+   	}
+
+   	void set_change_goal_of_the_error(unsigned int in_change_goal_of_the_error){
+   		change_goal_of_the_error_=in_change_goal_of_the_error;
+   	 std::cout <<"set IN change_goal_of_the_error_="<<change_goal_of_the_error_<< std::endl;
+   	}
+
+    void new_matlab_file();
+
+    void do_scene_prediction_from_outside(std::vector<Sdestination>* person_best_dest){
+    	bool person_or_robot=true; //case, person_companion
+    	//std::cout << " (5) !!! person_companion_plan_companion" << std::endl;
+    	person_companion_->get_best_dest().print();
+    	//std::cout << " (5.1) !!! person_or_robot="<<person_or_robot<<"; person_list_.size()="<< person_list_.size() << std::endl;
+
+    	this->Cprediction_behavior::scene_prediction(person_or_robot,person_best_dest); // scene prediction, needed, pero sin la person cmpanion y hará falta el robot en colisiones.
+    	//std::cout << " (5.2) !!! person_companion_plan_companion" << std::endl;
+
+    }
+
+
+    /*void set_second_group_companion_personSpoint(SpointV_cov in_second_group_companion_personSpoint){
+    	second_group_companion_personSpoint_=in_second_group_companion_personSpoint;
+    }*/
+
+    /*void second_group_companion_personSpose(Spose in_second_group_companion_personSpose){
+    	second_group_companion_personSpose_=in_second_group_companion_personSpose;
+    }*/
+
+    /*void set_second_group_companion_personDest(std::vector<Sdestination> Dest_other_Tracks){
+
+    	for(unsigned int p=0;p<Dest_other_Tracks.size();p++){
+    		if(Dest_other_Tracks[p].id==id_SECOND_person_companion_){
+    			second_group_companion_personDest_=Sdestination(0,Dest_other_Tracks[p].x,Dest_other_Tracks[p].y);
+    			//std::cout << " print DEST person, id="<<Dest_other_Tracks[p].id << std::endl;
+    			//Dest_other_Tracks[p].print();
+    		}
+    	}
+
+    }*/
+
+    /*void set_we_have_second_person_companion(bool in_we_have_second_person_companion){
+    	we_have_second_person_companion_=in_we_have_second_person_companion;
+    }*/
+
+
+    Sdestination akp_get_goal_path(){ // for visualization del simulador personas.
+    	return akp_out_path_goal_;
+    }
+
+    Sdestination akp_get_goal_companion(){// for visualization del simulador personas.
+    	return akp_out_companion_goal_;
+    }
+
+    void set_companion_angle_peopl_in_group(double in_companion_angle_peopl_in_group){ // angle in degrees.
+    	companion_angle_peopl_in_group_=in_companion_angle_peopl_in_group;
+    }
+
+    void set_real_distance_between_people_of_group_(double in_real_distance_between_people_of_group){ // angle in degrees.
+    	real_distance_between_people_of_group_=in_real_distance_between_people_of_group;
+    }
+
+    /*void set_second_group_companion_object_person(Cperson_abstract* in_second_group_companion_person_obj){
+    	second_group_companion_person_obj_=in_second_group_companion_person_obj;
+    }*/
+
+    Spoint get_before_next_goal_of_robot(){
+    	return before_next_goal_of_robot_;
+    }
+
+    // set functions of Francesco companion model
+    //void set_genome_params(Cgenome in_genome){
+    	//genome_params_=in_genome;
+    	//Companion_Zanlungo_Model_.set_genome_params_in_CompanionFrancescoModel(in_genome);
+   // }
+
+    /*void set_ZanlungoCompModel_bool(bool in_bool_Zanlungo_CompMod){
+    	Zanlungo_model_=in_bool_Zanlungo_CompMod;
+    }*/
+
+	void set_high_vel_dampening_parameter_planner(double in_dampening_parameter){
+		robot_->set_high_vel_dampening_parameter(in_dampening_parameter);
+	}
+
+	void set_normal_vel_dampening_parameter_planner(double in_dampening_parameter){
+		robot_->set_normal_vel_dampening_parameter(in_dampening_parameter);
+	}
+	void set_slow_vel_dampening_parameter_planner(double in_dampening_parameter){
+		robot_->set_slow_vel_dampening_parameter(in_dampening_parameter);
+	}
+
+	void set_limit_linear_vel_for_dampening_parameter_planner(double in_limit_linear_vel){
+		robot_->set_limit_linear_vel_for_dampening_parameter(in_limit_linear_vel);
+	}
+	void set_limit_angular_vel_for_dampening_parameter_planner(double in_limit_angular_vel){
+		robot_->set_limit_angular_vel_for_dampening_parameter(in_limit_angular_vel);
+	}
+
+	void set_number_of_group_people(unsigned int in_number_of_group_people){
+		number_of_group_people_=in_number_of_group_people;
+	}
+
+	/*void set_constante_multiplicar_fuerza_base_Zanlungo(double in_constante_multiplicar_fuerza_base_Zanlungo){
+		constante_multiplicar_fuerza_base_Zanlungo_=in_constante_multiplicar_fuerza_base_Zanlungo;
+	}*/
+
+	void set_conf_constante_multiplicar_fuerza_goal(double in_constante_multiplicar_fuerza_goal){
+		constante_multiplicar_fuerza_goal_=in_constante_multiplicar_fuerza_goal;
+	}
+
+	/*void set_up_distance_margin_Zamlungo(double in_up_distance_margin_Zamlungo){
+		up_distance_margin_Zamlungo_=in_up_distance_margin_Zamlungo;
+	}*/
+
+	/*void set_down_distance_margin_Zamlungo(double in_down_distance_margin_Zamlungo){
+		down_distance_margin_Zamlungo_=in_down_distance_margin_Zamlungo;
+	}*/
+
+	/*void set_bool_robot_propagation_Zanlungo(bool in_bool_robot_propagation_Zanlungo){
+		bool_robot_propagation_Zanlungo_=in_bool_robot_propagation_Zanlungo;
+	}*/
+
+	void set_bool_distance_margin(bool in_bool_distance_margin){
+		bool_distance_margin_=in_bool_distance_margin;
+	}
+
+	/*void set_dis_tol_Francesco_force(double in_dis_tol){
+		dis_tol_=in_dis_tol;
+	}*/
+
+	bool get_change_ids_group_people_in_recongigure(){
+		return change_ids_group_people_in_recongigure_;
+	}
+
+	void set_change_ids_group_people_in_recongigure(bool in_change_ids_group_people_in_recongigure){
+		change_ids_group_people_in_recongigure_=in_change_ids_group_people_in_recongigure;
+	}
+
+	/*void set_debug_output_screen_mesages_zanlungo(bool in_debug_output_screen_mesages_zanlungo){
+		debug_output_screen_mesages_=in_debug_output_screen_mesages_zanlungo;
+	}*/
+	/*void set_print_to_zanlungo_file(bool in_print_to_zanlungo_file){
+		print_to_zanlungo_file_=in_print_to_zanlungo_file;
+	}*/
+
+	void set_new_time_window_for_filter_velocity(double in_new_time_window){
+		this->Cprediction_behavior::change_time_window_for_filter_velocity(in_new_time_window);
+	}
+
+	void set_threshold_vel_pers_to_stop(double in_threshold_vel_pers_to_stop){
+		threshold_vel_pers_to_stop_=in_threshold_vel_pers_to_stop;
+	}
+	void set_dist_bet_rob_ext_goal_to_stop(double in_dist_bet_rob_ext_goal_to_stop){
+		dist_bet_rob_ext_goal_to_stop_=in_dist_bet_rob_ext_goal_to_stop;
+	}
+
+	void set_dist_bet_pers1Com_ext_goal_to_stop(double in_dist_bet_pers1Com_ext_goal_to_stop){
+		dist_bet_pers1Com_ext_goal_to_stop_=in_dist_bet_pers1Com_ext_goal_to_stop;
+	}
+	void set_dist_bet_pers2Com_ext_goal_to_stop(double in_dist_bet_pers2Com_ext_goal_to_stop){
+		dist_bet_pers2Com_ext_goal_to_stop_=in_dist_bet_pers2Com_ext_goal_to_stop;
+	}
+
+	void set_change_first(bool in_first){
+		first_=in_first;
+	}
+
+	void set_treshold_distance_between_steps(double in_treshold_distance_between_steps){
+		treshold_distance_between_steps_=in_treshold_distance_between_steps;
+	}
+
+	void set_threshold_max_total_force(double in_threshold_max_total_force){
+		threshold_max_total_force_=in_threshold_max_total_force;
+	}
+
+
+	void set_dis_tol(double in_dis_tol){
+		dis_tol_=in_dis_tol;
+	}
+
+	void set_dis_tol_side(double in_dis_tol_side){
+			dis_tol_side_=in_dis_tol_side;
+	}
+
+	/*void set_data_file_Zanlungo(std::string in_data_file_Zanlungo){
+		data_file_Zanlungo_=in_data_file_Zanlungo;
+	}*/
+
+	/*void set_AKP_max_desired_ve_robot_calc_with_Zanlungo(double in_max_vel){
+		Companion_Zanlungo_Model_.set_max_desired_ve_robot(in_max_vel);
+	}*/
+
+	/*void set_calculated_desired_vel(bool use_zanlungo_desired_vel){
+		calculated_desired_vel_=use_zanlungo_desired_vel;
+	}*/
+
+	void set_desired_vel_seteada_desde_fuera(double in_desired_vel_seteada_desde_fuera){
+		desired_vel_seteada_desde_fuera_=in_desired_vel_seteada_desde_fuera;
+	}
+
+	void set_gamma_companion(double in_gamma_companion){
+		gamma_companion_=in_gamma_companion;
+	}
+
+	void set_delta_companion(double in_delta_companion){
+		delta_companion_=in_delta_companion;
+	}
+
+	void set_k_final_goal(double in_set_k_final_goal){
+
+		 std::vector<double> params_robot=new_get_sfm_params(robot_);
+		 params_robot.at(0)=in_set_k_final_goal;
+		 if(change_k_robot_){
+			 set_sfm_to_robot( params_robot );
+		 }
+		 if(change_k_person_){
+			 set_sfm_to_person(params_robot);
+		 }
+		 if(change_k_person_comp_){
+			 set_sfm_to_person_companion( params_robot );
+		 }
+
+		 if(change_k_obst_){
+			 set_sfm_to_obstacle( params_robot );
+		 }
+
+		 //Companion_Zanlungo_Model_.set_internal_k_value(in_set_k_final_goal);
+	}
+
+
+	void set_change_k_robot(bool in_change_k_robot){
+		change_k_robot_=in_change_k_robot;
+	}
+
+	void set_change_k_person(bool in_change_k_person){
+		change_k_person_=in_change_k_person;
+	}
+
+	void set_change_k_person_comp(bool in_change_k_person_comp){
+		change_k_person_comp_=in_change_k_person_comp;
+	}
+
+	void set_change_k_obst(bool in_change_k_obst){
+		change_k_obst_=in_change_k_obst;
+	}
+
+	/*void set_AKP_C_vp_desired_Zanlungo(double in_AKP_C_vp_desired_Zanlungo)
+	{
+		Companion_Zanlungo_Model_.set_C_vp_desired_Zanlungo(in_AKP_C_vp_desired_Zanlungo);
+	}
+	void set_AKP_C1(double in_AKP_C1){
+		Companion_Zanlungo_Model_.set_C1(in_AKP_C1);
+	}
+	void set_AKP_dist_r0_Zanlungo(double in_AKP_dist_r0_Zanlungo){
+		Companion_Zanlungo_Model_.set_dist_r0_Zanlungo(in_AKP_dist_r0_Zanlungo);
+	}
+	void set_C_acc1(double in_AKP_C_acc1){
+		Companion_Zanlungo_Model_.set_C_acc1(in_AKP_C_acc1);
+	}*/
+	/*void set_C_acc2(double in_AKP_C_acc2){
+		Companion_Zanlungo_Model_.set_C_acc2(in_AKP_C_acc2);
+	}*/
+
+	/*void set_AKP_bool_show_constants_desired_vel(bool in_AKP_bool_show_constants_desired_vel){
+		Companion_Zanlungo_Model_.set_bool_show_constants_desired_vel(in_AKP_bool_show_constants_desired_vel);
+	}*/
+
+	void set_minimun_velocity_case_stop_initial(double in_minimun_velocity_case_stop_initial){
+		minimun_velocity_case_stop_initial_=in_minimun_velocity_case_stop_initial;
+	}
+
+	void set_final_max_v(double in_final_max_v){
+		final_max_v_=in_final_max_v;
+	}
+
+
+	void set_initial_limit_distance_goal_to_person_robot_stop(double in_initial_limit_distance_goal_to_person_robot_stop){
+		initial_limit_distance_goal_to_person_robot_stop_=in_initial_limit_distance_goal_to_person_robot_stop;
+	}
+
+	void set_ideal_max_robot_velocity(double in_ideal_max_robot_velocity){
+		ideal_max_robot_velocity_=in_ideal_max_robot_velocity;
+	}
+
+	double calc_person_companion_orientation_from_outside(SpointV actual_p1_point, Sdestination person_dest, Spose in_robot);
+
+	void set_bool_change_ct_and_cr_with_vel(bool in_bool_change_ct_and_cr_with_vel){
+		bool_change_ct_and_cr_with_vel_=in_bool_change_ct_and_cr_with_vel;
+	}
+	bool bool_change_ct_and_cr_with_vel_;
+
+	void set_bool_new_velocity_system( bool in_bool_new_velocity_system){
+		bool_new_velocity_system_=in_bool_new_velocity_system;
+	}
+
+	bool bool_new_velocity_system_;
+
+	// INI NEW FACKE CHANGE AND DESAPEAR ID:
+	// MEjor poner con booleano... ya esta con booleano.
+	void set_No_change_ids(bool in_No_change_ids){
+		No_change_ids_=in_No_change_ids;
+	}
+	bool No_change_ids_;
+
+	/*void erase_p1_change_history_p1_to_p2_in_person_list(unsigned int person1_id,unsigned int person2_id){
+		// frist interchange history of people. Test first without change history
+		// remove_people
+		this->Cprediction_bhmip::remove_person_from_outside(person1_id);
+	}*/
+	// FIN NEW FACKE CHANGE AND DESAPEAR ID: (OJO! además quitar el intercambio de Id's de aquí dentro! molesta más que ayuda! Creo...
+
+	void set_current_pose_robot_akp(Spose in_current_pose_robot){
+		//std::cout<<" 1 IIIIIINNNNN set_current_pose_robot_akp(  ) "<< std::endl;
+		  robot_->set_current_pose(in_current_pose_robot);
+		  robot_->set_current_pointV(SpointV(in_current_pose_robot.x,in_current_pose_robot.y,in_current_pose_robot.time_stamp,in_current_pose_robot.v*cos(in_current_pose_robot.theta),in_current_pose_robot.v*sin(in_current_pose_robot.theta)));
+		  //std::cout<<" 2 IIIIIINNNNN set_current_pose_robot_akp(  ) "<< std::endl;
+		  robot_->get_current_pose().print();
+		 // std::cout<<" 1 OUUUUUTtt set_current_pose_robot_akp(  ) "<< std::endl;
+	}
+
+	/*void plan_set_augment_initial_v(bool in_augment_initial_v){
+		robot_->set_augment_initial_v(in_augment_initial_v);
+
+	}*/
+
+	void plan_set_initial_v_robot_needed(double in_initial_v_robot_needed){
+		robot_->set_initial_v_robot_needed(in_initial_v_robot_needed);
+	}
+
+	/*void set_Zanlungo_model2(bool in_Zanlungo_model2){
+		Zanlungo_model2_=in_Zanlungo_model2;
+	}*/
+
+	Crobot* get_return_person_companion(){ //retorna el person_companion_ para el simulador, para hacer el restart directamente.
+		return person_companion_;
+	}
+
+	/*void set_plan_create_fake_fixed_second_person_companion(bool in_plan_create_fake_fixed_second_person_companion){
+		plan_create_fake_fixed_second_person_companion_=in_plan_create_fake_fixed_second_person_companion;
+	}*/
+
+	void set_actual_case_sim_goal(bool in_actual_case_sim_goal){ // goal movil, dependiendo orientación grupo, tangente a perpendicular al goal, p
+		//ara orientar el goal final en la direccion del robot y que no se cruce delante de las personas al estar muy cerca del goal final.
+		actual_case_sim_goal_=in_actual_case_sim_goal;
+	}
+
+	Spoint get_actual_goal_to_return_initial_position(){
+		return actual_goal_to_return_initial_position_;
+
+	}
+
+	void set_metros_al_dynamic_goal_Vform(double in_metros){
+		 metros_al_dynamic_goal_Vform_=in_metros;
+	}
+
+	void set_distance_to_stop_from_first_person_comp(double in_distance){
+		distance_to_stop_from_first_person_comp_=in_distance;
+	}
+
+	double distance_to_stop_from_first_person_comp_;
+
+  protected:
+
+    /**
+     * \brief this function calculates the planner path in the companion case for the companion person
+     * of the Cscene_sim, if no solution is found, then a false result is returned
+     */
+    bool robot_plan_anticipative_krrt_companion_person_companion( Cperson_abstract::companion_reactive reactive=Cperson_abstract::Akp_planning);
+
+    /**
+     * \brief this function initializes the planner of the companion person akp planner, selecting the persons to be considered
+     * and other things. If setting is not possible, i.e. goal is within current goal, etc..
+     * a false result is returned (COMPANION CASE)
+     */
+    bool init_robot_plan2_person_companion(double robot_person_distance=1.5);
+
+    /**
+     * \brief this function samples the workspace for the person_companion (akp planning)
+     */
+    Spoint sample_workspace_for_person_companion_akp();
+    void reset_scene_persons_propagation_flag();
+
+    unsigned int find_nearest_vertex_person_companion_akp( const Spoint& random_goal ); //for the akp of the person companion.
+
+    /**
+     * \brief this function calculates the input required
+     * for the propagation to get the sampled position
+     */
+     Sedge_tree_pcomp calculate_edge_person_companion_akp( unsigned int parent_vertex, const Sdestination& random_goal, Cperson_abstract::companion_reactive reactive =Cperson_abstract::Akp_planning);
+    //Sedge_tree_pcomp calculate_edge_companion_person_akp( unsigned int parent_vertex, const Sdestination& random_goal, Cperson_abstract::companion_reactive reactive=Cperson_abstract::Akp_planning );
+    // for person companion, planning akp
+    /**
+     * \brief this function propagates the selected node,
+     * if a collision is detected returns TRUE and if propagation
+     * was not possible, false
+     */
+    bool propagate_vertex_person_companion_akp( unsigned int parent_index , const Sedge_tree_pcomp& u); // para la companion person usar el akp planner
+    /**
+     * \brief this function checks for collision for the propagated robot
+     * state either with static obstacles as well as persons (their prediction)
+     */
+    //bool check_collision( const SpointV_cov& p2,  int t = -1 );
+    double cost_to_go_person_companion_akp( const Spoint& random_goal, unsigned int i , Cperson_abstract::companion_reactive reactive=Cperson_abstract::Akp_planning);
+
+    Sforce force_persons_int_planning_virtual(Cperson_abstract* center , unsigned int t = 0, double min_dist2= 64.0, Cperson_abstract::companion_reactive reactive=Cperson_abstract::Akp_planning); // to do the group propagation
+    Sforce force_persons_int_planning_virtual_robot_prediction(Cperson_abstract* center , unsigned int t, double min_dist2=64.0, Cperson_abstract::companion_reactive reactive=Cperson_abstract::Akp_planning, unsigned int id_pers_comp_rob=0);
+
+    Sforce force_persons_int_planning_virtual_robot_companion_propagation(Cperson_abstract* center , unsigned int t= 0, double min_dist2= 64.0, Cperson_abstract::companion_reactive reactive=Cperson_abstract::Akp_planning); // the function is for akp planer companion. To do the robot propagation
+    // for person_companion that use the akp planning.
+    Sforce force_persons_int_planning_virtual_companion_person_akp(Cperson_abstract* center , unsigned int t=0, double min_dist2= 64.0, Cperson_abstract::companion_reactive reactive=Cperson_abstract::Akp_planning);
+    Sforce force_persons_int_planning_virtual_companion_person_akp_person_prediction(Cperson_abstract* center , unsigned int t=0, double min_dist2= 64.0, Cperson_abstract::companion_reactive reactive=Cperson_abstract::Akp_planning);
+
+    Sforce force_persons_int_robot_prediction_virtual(const SpointV& center , unsigned int t = 0, double min_dist2 = 16.0);
+    Sforce force_objects_laser_int_planning_virtual( Cperson_abstract* person, unsigned int planning_index = 0, double min_dist2=25.0, bool robot_collision_check_flag=false);
+    Sforce force_objects_laser_int_planning_virtual_robot_propagation( Cperson_abstract* person, unsigned int planning_index=0, double min_dist2=25.0, bool robot_collision_check_flag=false);
+
+    void calculate_navigation_instant_work(  );
+    void calculate_cost_person_companion_akp( unsigned int parent_index , Sedge_tree_pcomp u , Cperson_abstract::companion_reactive reactive=Cperson_abstract::Akp_planning);
+
+    //preprocess different solutions to calculate the best or bests solutions
+    unsigned int global_min_cost_index(   Crobot* robot_act, Cperson_abstract::companion_reactive reactive=Cperson_abstract::Akp_planning );
+    void preprocess_global_parameters(Crobot* robot_act);
+    void calculate_normalization_cost_functions_parameters_erf();
+    void calculate_normalization_cost_functions_parameters_norm(Crobot* robot_act);
+    void calculate_non_dominated_solutions();
+
+
+    //planning paramters
+    plan_mode plan_mode_;
+    distance_mode distance_mode_;
+    global_mode global_mode_;
+    unsigned int max_iter_;  // numero de nodos por path. de normal hay 40 nodos por path.
+    std::vector<double> cost_parameters_;
+    Spoint goal_,local_goal_, goal2_;
+    bool reaching_goal_;
+	//list of person to be considered in by the planner
+	std::list<Cperson_abstract*> nearby_person_list_;
+	std::vector<Spoint> nearby_obstacle_list_; //lista de obstaculos estaticos, solo para robot.
+	std::vector<Spoint> nearby_obstacle_list2_; // lista de obstaculos estaticos para companion (ely)
+	double workspace_radii_, std_goal_workspace_;
+	double max_v_by_system_, cost_angular_;
+	double xy_2_goal_tolerance_, v_goal_tolerance_, local_v_goal_tolerance_, distance_to_stop_;
+
+    // alpha and sum_ratio paramter to scape local minima
+    double alpha_,gamma_,delta_;
+    std::vector<double> v_alpha_,v_gamma_,v_delta_;
+
+    //random generator
+	std::default_random_engine generator_;
+
+
+    //tree structure data structures
+    std::vector<Sedge_tree_pcomp> edge_;
+    std::vector<double> cost_int_forces_;
+    std::vector<double> cost_robot_;
+    std::vector<double> cost_obstacles_;
+    std::vector<double> cost_past_traj_;
+    //std::vector<double> cost_local_minima_;
+    std::vector<double> nodes_in_branch_;
+    std::vector<double> cost_distance_;
+    std::vector<double> cost_orientation_;
+    std::vector<Spoint> random_goals_;
+    std::vector<bool> collision_detected_;
+    double work_persons_, work_robot_;
+
+
+    //results data structures
+    std::vector<unsigned int> best_plan_vertex_index_, end_of_branches_index_, nondominated_plan_vertex_index_, nondominated_end_of_plan_vertex_index_;
+    std::vector<Spose> best_planning_trajectory_;
+    std::vector<double> best_costs_, mean_costs_, std_costs_;
+
+    //cost-to-go function values to measure real performance for testing
+    std::vector<double> cost_values_;
+
+    Spose last_pose_command_;//comand of the last velocity command send to the robot platform
+
+
+    //Multi-Objective optimization: normalization terms
+    double mean_cost_int_forces_,
+		mean_cost_robot_,
+		mean_cost_obstacles_,
+		mean_cost_past_traj_,
+		mean_cost_distance_,
+		mean_cost_orientation_,
+		mean_cost_companion_; // mean companion cost, add by ely.
+
+    double std_cost_int_forces_,
+		std_cost_robot_,
+		std_cost_obstacles_,
+		std_cost_past_traj_,
+		std_cost_distance_,
+		std_cost_orientation_,
+		std_cost_companion_; // std companion cost, add by ely.
+
+    //Multi-objective vector structure to find non-dominated sets
+    std::vector<Smulticost_pcom> multicosts_;
+    std::list<Smulticost_pcom> nondominated_multicosts_;
+
+    //to calculate probability of collision
+    double gaussian_constant_;
+    int ppl_collision_mode_,pr_force_mode_;
+
+
+    // ely functions
+    /**
+     * \brief this function calculates the companion cost for each node of the path.
+     */
+    void calculate_companion_cost_node(unsigned int parent_index, Crobot* robot_act); // function to calculate one node companion cost
+    /**
+     * \brief this function checks for collision for the propagated robot+person=couple
+     * state either with static obstacles as well as persons (their prediction).
+     */
+    bool check_collision_person_companion_akp( const SpointV_cov& p2 ,  int t = -1);
+    bool check_collision_final( const Spoint& p ,  int t); // caso robot, moverse dentro de la burbuja!
+
+    /**
+     * \brief this function calculates the additional companion cost when two adjacent angles in the best path
+     * for the position of the robot have a difference greater than the initia_increment_angle and
+     * the intermediates cost were not taken into account in the calculation o the initial path
+     */
+    double calculate_companion_addition_cost_node_of_big_angle_difference( unsigned int parent_index, double actual_angle);
+
+    /**
+     * \brief this function calculates the initial_increment_angle to move the robot around the person
+     * when they have to avoid obstacles in a joint way.
+     */
+    void ini_increment_angle();
+    void ini_increment_angle_person_companion_akp();
+    void calculate_companion_path_angle_and_cost(unsigned int parent_index, double actual_angle);
+    void return_next_robot_position_companion_cost_and_angle(unsigned int parent_index);
+    void only_cost_calculate_companion_path_angle_and_cost(unsigned int parent_index, double actual_angle);
+    /**
+     * \brief this function calculates the needed angle between person-and-robot
+     * for all the nodes of the best path, to obtain a progressive angle
+     *  to positioned the robot around the person. (case, go robot in front of the person)
+     */
+    void only_angle_in_final_tree_calculate_companion_path_angle_and_cost(unsigned int i);
+    void only_angle_in_final_tree_calculate_companion_path_angle_and_cost2(unsigned int i);
+    /**
+     * \brief this function calculates the needed angle between person-and-robot
+     * for all the nodes of the best path, to obtain a progressive angle
+     *  to positioned the robot around the person. (case, go robot at the rear of the person)
+     */
+    void go_behind_robot_only_angle_in_final_tree_calculate_companion_path_angle_and_cost2(unsigned int i);
+    void go_behind_robot_only_angle_in_final_tree_calculate_companion_path_angle_and_cost3(unsigned int i);
+
+    void go_in_front_robot_only_angle_in_final_tree_calculate_companion_path_angle_and_cost3(unsigned int i);
+
+    /**
+     * \brief this function is for see the best path with the companion angles and the companion cost.
+     */
+    void see_companion_path_angle_and_cost_of_min_cost_paths(double parent_index);
+    /**
+     * \brief this function modifies the distance between the person and robot couple to smooth the turns
+     * to put the robot in front or rear the person.
+     */
+    double calc_robot_person_companion_distance();
+    /**
+     * \brief this function replan the propagated position of the robot for the next step
+     * of the robot in the best path, taking into account in the propagation the two goals for the robot:
+     * follow the path and goes to a concrete position respect to the person.
+     */
+    Spose replan_last_step_person_companion(unsigned int index, Spose robot_pose_ini, SpointV robot_Spoint_ini, Sdestination robot_goal, double dt, Sdestination robot_final_goal);
+    /**
+     * \brief these function perform a different collision functions for the goal in the atractive or repulsive cases.
+     */
+    double check_collision_companion_goal(const Spoint& p ,  int t);
+    /**
+     * \brief this function set the actual max_velocity_ for the robot.
+     * this is the initial function only taking into account the planning for the robot.
+     */
+    void vel_robot_normal(double d_min);
+    /**
+     * \brief this function set the actual max_velocity_ for the robot.
+     * this function takes into account the planning for the robot-and-person=couple.
+     */
+    void vel_robot_companion(double d_min , double robot_person_distance, Spoint before_next_goal_of_robot=Sdestination(), Cperson_abstract::companion_reactive reactive=Cperson_abstract::Reactiva_repulsive);
+
+    void calculate_actual_angle_person_robot(unsigned int index);
+    double calculate_actual_angle_person_robot(Spoint person, Spoint robot);
+    /**
+     * \brief this function returns the actual max_velocity_ to the max velocity of the robot.
+     *  this is to calculate the akp path with the max velocity. Then, in the recalculation of
+     *  the position for the best path, the velocity of the robot is modified taking into account
+     *  also the goal of remain in a exact position respect to the companion person.
+     */
+    void return_max_velocity_systemRobot_to_max_value();
+
+    /**
+     * \brief this function calculate the anisotropy for the group to do not take into account
+     * the collisions with obstacles at the rear of the group, for the robot positioning around the person.
+     * (now is set, anisotropy_limit=0, any anisotropy is take into account, we take into account rear collisions)
+     */
+    double calculate_anisotropy(const SpointV_cov& center_person, const Spoint& interacting_person , double lambda=0.2);
+    /**
+     * \brief this function calculates the distance between the person-and-robot in the moments where the robot
+     * moves around the person to avoid obstacles (put in front or behind). This function is to smooth the
+     * turns of the robot, facilitate the navigation and remove the misalignments with the person in orientation
+     * that provoke "s" movements for the robot.
+     */
+    double calculate_modif_person_robot_distance( double ite );
+    double calc_robot_person_companion_distance_companion_person_akp(); // case person companion planning akp
+    /**
+     * \brief this function print the results in a matlab file.
+     */
+    void printToMatlab();
+    void evaluate_costs_printToMatlab(Crobot* actual_robot);
+    void calc_area_workspace_arround_person_companion(); // not used, bad idea. Only sample arrount the person prediction. not works and is not proper.
+    void new_matlab_file_To_evaluate_costs();
+    void new_debug_file();
+    void new_matlab_file_To_evaluate_change_distance_and_angle();
+    void evaluate_change_distance_and_angle_companion_with_beta_change();
+
+    void calculate_intersecction_line_circle(double Xc, double Yc, double radius, double X_ini, double Y_ini, double X_final, double Y_final, double &intersection1, double &intersection2 );
+
+	void robot_group_trajectory_prediction( double new_horizon_time, Sdestination goal_to_predict_pose);
+
+	double calc_person_companion_orientation(); // return theta person companion in radians!
+	Sdestination calculate_companion_goal_stable_if_person_stop(double in_act_min_companion_angle);
+	Sdestination calc_companion_goal_position(); // claculation of companion goal with the actual companion angle (from collisions etc) and the position of the robot and the person.
+
+	Spoint SIM_initial_person_goal_pose_actual_;
+	unsigned int id_person_goal_; // (compartidas con sim person goal, no eliminar) id of the person that the group goes to talk, is a dynamic goal for each iteration.
+	unsigned int change_id_person_goal_;
+	bool group_go_to_interact_with_other_person_;
+
+	Sdestination before_person_intersection_destination_;
+    // variables companion (ely)
+    bool debug_companion_;
+    double robot_person_companion_distance_;
+    double min_distance_collision_; // variables to calculate the companion cost and angle! => min_distance_collision_ and bool_collision_companion_
+    bool bool_collision_companion_; // variables to calculate the companion cost and angle!
+    double next_robot_pose_min_distance_collision_;
+    std::vector<double> orientation_person_robot_angles_; // angles to calculate the companion goal using the computed path of the group
+
+    std::vector<double> orientation_person_robot_angles_with_prediction_of_person_companion_; // angles to calculate the companion goal using the prediction of the companion person.
+    bool calc_goal_companion_with_group_path_; // if true == calc the angle of the goal companion with the group path, else calc the angle of the goal companion with the prediction of the person.
+	void fix_angles_to_use_person_prediction_for_the_companion_goal(); //function to fix the companion angles obtained with the person prediction.
+
+
+    // new costs (companion-ely)
+    std::vector<double> cost_companion_;
+    std::vector<unsigned int> parent_index_vector_;
+    std::vector<double> min_distance_collision_vector_;
+    std::vector<double> min_distance_collision_vector_from_pred_;
+    // NOTA: ojo!!! no hemos definido como calculo el coste del path entero, solo el coste en los nodos. he de mirar como lo hace Gonzalo para los otros y ver si puedo usar una de esas formas. Si hago una media del coste este durante el path o algo asi...
+    double min_next_companion_angle_; // esta en grados
+    double min_next_companion_angle_for_final_dest_;
+    double min_next_companion_cost_;
+    double angle_increment_of_increment_distance_;
+    double angle_increment_of_increment_distance_vuelta_al_reves_;
+
+    double before_initial_angle_;
+    double before_initial_cost_;
+    std::vector<unsigned int> BEST_path_parent_index_vector_;
+
+    bool debug_comanion_good_;
+    unsigned int id_person_companion_;
+    unsigned int change_id_person_companion_;
+    Sdestination person_destination_;
+    //Spose
+    double person_colision_radi_;
+    SpointV_cov companion_person_position_; // posicion inicial de la persona a la que acompañamos.
+    std::list<Cperson_abstract*> person_companion_nearby_person_list_;  // personas cercanas a la persona que acompanya el robot.
+    double person_center_min_distance_collision_;
+    //const std::vector<SpointV_cov>* companion_person_planning_trajectory_;//std::vector<SpointV_cov> companion_person_planning_trajectory_; //virtual const std::vector<SpointV_cov>* companion_person_planning_trajectory_
+    Sdestination person_companion_goal_;
+    double robot_person_distance_;
+    //Cforce_reactive_robot_companion reactive_class_;
+    double robot_person_proximity_distance_;  // distancia a la que esta robot y persona para que se olvide del goal el robot y se acerque a la persona.
+    double robot_person_proximity_tolerance_; // increment of distance around the proximity_distance to generate a distance margin. This is for do not always omit the planning and aproximate to the person.
+    double robot_person_proximity_goals_x_; // distancia entre goal de la persona que acompaña y goal al que se dirige el robot.
+    double robot_person_proximity_goals_y_;
+    double initial_angle_;
+    Spose robot_initial_pose_;
+    SpointV_cov initial_robot_spoint_;
+    SpointV_cov initial_person_companion_point_;
+
+
+    Cperson_abstract* pointer_to_person_companion_;
+    bool we_have_pointer_to_first_person_; // if true, we can use the pointer!!!
+
+    double robot_person_distance3_;
+
+    bool case_robot_companion_; // bool para cambiar velocidad en person companion. hacer que vaya algo más despacio, para que reaccione mejor.
+    std::vector<Spoint> random_goals2_;
+    unsigned int min_branch_index_save_;
+
+    bool debug_antes_subgoals_entre_AKP_goals_;
+
+    bool final_collision_check_;
+
+    bool actual_debug_;
+    bool actual_debug2_;
+    double offset_atractive_;
+
+    double time_act_;
+    //double time_ant_;
+    double inct_;
+
+    double robot_adition_complete_esphere_companion_distance_;
+
+    double final_v_robot_max_;
+    double final_v_real_robot_max_;
+
+    double f_obst_max_x_; // =0.1
+    double f_obst_max_y_; //=0.1
+
+    Sdestination robot_destination_;
+
+    bool companion_same_person_goal_;
+    Sdestination extern_robot_goal_;
+    Sdestination extern_robot_goal_fix_;
+
+    double angle_companion_;
+    double person_goal_percent_;
+    double marge_angle_companion_;
+    Sdestination person_companion_goal_out_;
+
+    double dmin_global_;
+    double angle_companion_temp_movil_;
+    double angle_companion_temp_movil_complementarion_detras_;
+    std::vector<double> min_angles_colisions_; // al calcular el min_colision. a que angulos estan esas colisiones
+    std::vector<double> angles_colisions_; // para cada estep, el angulo a donde esta la minima colision
+    std::vector<double> min_step_collision_distance_;
+    double min_angle_collision_;
+   bool debug_nadal_;
+   bool debug_nadal2_;
+
+   bool robot_orient_;
+   double person_orient_;
+
+   double vel_per_max_caso_robot_a_0_o_180_grados_;
+
+   bool debug_real_test_companion_;
+   bool debug_real_test_companion2_;
+   bool debug_real_test_companion3_;
+   bool debug_real_test_companion4_;
+
+   double marge_in_distance_; // margen en distancia para seguir a una persona.
+   bool overpas_obstacles_behind_person_;
+   bool we_have_cost_companion_;
+
+   double anisotropy_threshold_;
+
+   Cperson_abstract::companion_reactive robot_companion_case_;
+   bool check_execution_times_;
+
+   double person_robot_actual_real_distance_;
+   double little_augmented_collision_margin_;
+   double person_radi_;
+   double person_radi2_;
+   double person_radi_amp_;
+   double person_radi_per_comp_;
+   double max_distance_to_obstacles_detected_;
+   bool see_forces_;
+  double obstacle_radi_;
+  double obstacle_radi2_;
+  double obstacle_radi_amp_;
+
+  Sedge_tree_pcomp u_forces_robot_actual_; // solo para visualizacion en Rviz
+  Cperson_abstract::vel_per_ok vel_per_ok_; //boolean, true= robot navigate with the person velocity, else = robot navigate with the maximun velocity.
+
+
+  // Variables to save in matlab file!
+  double real_distance_person_robot_;
+  double ideal_distance_person_robot_;
+  double real_angle_person_robot_;
+  double ideal_angle_person_robot_;
+  double before_ideal_angle_person_robot_;
+  bool first_ideal_angle_;
+  double actual_time_;
+  double robot_distance_cost_;
+  double robot_orientation_cost_;
+  double robot_control_cost_;
+  double robot_other_person_cost_;
+  double robot_obstacles_cost_;
+  double robot_companion_cost_;
+  double robot_total_cost_;
+  double robot_ant_traj_cost_;
+  double person_distance_to_goal_;
+  double robot_distance_to_goal_;
+
+    double last_step_robot_other_person_cost_; // coste respecto a fuerzas de otras personas que se ejercen en el robot.
+    double last_step_robot_obstacles_cost_;  // coste respecto a fuerzas de obstaculos que se ejercen en el robot.
+    double last_step_robot_companion_cost_; // coste respecto a fuerzas de la companion person que se ejercen en el robot.
+    double last_step_robot_control_cost_mix_;
+    double last_step_robot_control_cost_goal_traj_;
+    double last_step_robot_control_cost_goal_person_;
+    double last_step_robot_orientation_cost_local_;
+    double last_step_robot_orientation_cost_global_;
+    double last_step_robot_distance_cost_;
+
+    double other_people_due_to_robot_cost_;
+    double companion_person_due_to_robot_cost_;
+
+    double workspace_arround_companion_person_min_x_;
+    double workspace_arround_companion_person_max_x_;
+    double workspace_arround_companion_person_min_y_;
+    double workspace_arround_companion_person_max_y_;
+
+    bool sim;
+    bool sim_target_per;
+    bool first_time_;
+    unsigned int iteration_;
+    unsigned int experiment_;
+
+    SpointV_cov before_initial_robot_spoint_;
+    double inc_distance_ant_;
+    double acum_time_;
+    double time_ant_;
+
+    bool save_results_in_file_;
+    double meters_goal_to_save_results_in_file_;
+    bool mode_velocity_;
+    std::string results_file_;
+    double reduce_max_vel_dist_;
+
+    bool go_with_vel_per_;
+
+    bool debug_file_robot_;
+	bool debug_cout_robot_;
+
+
+    std::string debug_file_;
+
+    bool case_dynamic_;
+
+    double external_inc_vel_for_inc_angle_;
+
+    double externa_force_k_near_goal_akp_;
+    double externa_force_k_far_goal_akp_;
+
+    double max_dist_to_near_goal_force_akp_;
+
+    bool mode_step_near_;
+    //int out_index_step_;
+    unsigned int out_index_step_final_dest_goal_;
+	unsigned int out_index_step_companion_goal_;
+
+    bool only_comp_people_vel_and_robot_poses_;
+
+    std::vector<bool> vector_of_companion_collisions_;
+
+    bool debug_angles_;
+
+    double ini_vel_to_increment_angle_;
+    //std::vector<bool> vector_of_companion_collisions_;
+
+    /*
+     * std::ofstream fileMatlab2;
+	fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+	fileMatlab2 << "% ***** Starts Autogenerated DEBUG File - results AKP dynamic companion ELY ***** \n\n";
+	fileMatlab2.close();
+     */
+    bool debug_person_companion_general_;
+    bool debug_person_companion_increment_angle_;
+    bool debug_init_robot_plan2_person_companion_;
+    bool debug_robot_plan_anticipative_krrt_companion_person_companion_;
+    bool debug_calculate_edge_person_companion_akp_;
+    bool debug_get_best_planned_pose_person_companion_akp_;
+    bool debug_correct_angle_person_init_robot2_;
+    bool debug_correct_angle_person_replan_min_cost_branch_;
+    bool debug_correct_angle_person_replan_last_step_;
+    bool debug_correct_angle_person_calculate_edge_;
+    bool debug_correct_angle_person_get_best_planned_pose_;
+    bool debug_correct_angle_person_vel_robot_companion_;
+    bool debug_correct_angle_person_calculate_actual_angle_person_robot_;
+    bool debug_correct_angle_person_print_to_matlab_;
+
+    bool bool_case_person_companion_;
+    double person_companion_desired_velocity_sim_;
+    SpointV_cov ini_point_person_companion_akp_;
+    Spose ini_spose_person_companion_akp_;
+
+    int number_of_obstacles_;
+    int number_obstacles_big_force_;
+	unsigned int num_steps_orientation_;
+
+
+	double alpha_companion_; // (weight FINAL_GOAL) alpha to replan the last step for the companion task ( 0.4 ) anterior 0.1.
+	double beta_companion_;  // (weight COMPANION_GOAL) beta to replan the last step for the companion task ( 0.6 )
+	double gamma_companion_; //  (weight repulsive force other people) gamma to replan the last step for the companion task (0.5) anterior 5.0
+	double delta_companion_; //  (weight repulsive force static obstacles) delta to replan the last step for the companion task (0.5)
+
+	double threshold_dintace_select_person_side_to_go_; // al principio esta distancia minimo ha de ser 0.5 metros al lado de la persona.
+	bool debug_select_person_side_to_go_with_more_free_space_;
+
+	bool chose_better_side_to_acompani_person_;
+
+	SsavePath_multiple_paths_and_best_path iter_act_multiple_paths_and_best_path_;
+	int planner_iterations_;
+
+    std::string evaluate_costs_file_;
+    std::string evaluate_change_distance_and_angle_companion_file_;
+
+    double orientacion_persona_actual_;
+	double person_x_ant_;
+	double person_y_ant_;
+
+	bool debug_file_evaluate_costs_;
+
+	// to visualice goals:
+	Sdestination robot_goal_to_person_companion_; // guardar estos en el archivo por cada iteracion.
+	Sdestination robot_path_goal_;
+	Spose final_combined_goal_;
+
+	double save_distance_between_person_and_robot_;
+	double save_angle_between_person_and_robot_;
+	unsigned int iter_d_;
+
+	Spoint person_position_t_4_;
+	double next_companion_angle_save_;
+	double before_act_companion_angle_;
+
+	bool use_person_prediction_until_final_goal_; // no se usa, parece aunque solo esta en companion, de momento la dejo.
+	calculate_complete_group_path_ calculate_complete_group_path_var_;
+
+	Spoint before_group_position_;
+	bool first_;
+
+	action_mode Action_;
+
+	simulation_case actual_case_;
+
+	Spoint actual_goal_to_return_initial_position_;
+
+	// To have several experiments to simulate.
+
+	Spose SIM_initial_robot_pose1_;
+	Spose SIM_initial_robot_pose2_;
+	Spose SIM_initial_robot_pose3_;
+	Spose SIM_initial_robot_pose4_;
+
+	Spose SIM_initial_person_companion_pose1_;
+	Spose SIM_initial_person_companion_pose2_;
+
+
+	unsigned int global_iter_to_change_simulation_case_;
+
+	bool first_in_itter_;
+	double ros_time_to_sec_;
+	bool first_time_case1_;
+	bool first_time_case2_;
+
+	unsigned int goal1_;// goal number to do.
+
+	Spose center_of_the_group_;
+
+
+	double max_prop_time_;
+	double max_prop_person_;
+	unsigned int max_n_prop_person_;
+
+	bool is_act_person_companion_;
+
+
+	double save_computational_time_value_;
+	double clocks_start, clocks_end;
+	clock_t robot_plan_companion2_start_ITER, robot_plan_companion2_end_ITER,robot_plan_companion3_start_ITER, robot_plan_companion3_end_ITER;
+
+
+	Spoint final_goal_only_vel_;
+	Spoint final_person_dest_;
+
+	bool go_behid_comp_person_;
+
+    double last_true_person_companion_orientation_;
+
+
+    bool restart_real_;
+    double max_dist_to_go_behind_;
+
+    bool case_stop_giro_;
+    double incremento_giro_positivo_;
+	   double incremento_giro_negativo_;
+
+	   Cperson_abstract *actual_person_Companion_pointer_;
+	   Crobot *actual_person_Companion_pointer_robot_;
+	   SpointV_cov actual_person_Companion_SpointV_;
+	   Sdestination actual_person_Companion_destination_;
+	   double max_desired_person_Companion_velocity_; // todo: add initial bucle: max_desired_person_Companion_velocity_=iit->get_desired_velocity();
+
+
+	   double d_min_global_;
+
+	   double radii_person_list_include_person_companion_;
+	   bool complete_traj_prediction_to_goal_;
+	   double set_distance_person_robot_;
+
+	   // variable save results on files or not! OJO!!! cuando quiera los datos hay que activarla!!!
+	   bool save_results_on_files_for_person_companion_;
+	   bool save_results_on_files_for_robot_; // only do not save the results on files for the robot
+	   bool save_in_file_; // disable also the iterations!!! not only save the results on files. (only for robot)
+
+	   // variables change forces between person companion and robot.
+	   Sforce force_int_between_person_comp_and_robot_;
+
+	   // for performance:
+	   double value_distance_global_path_;
+	   double global_path_ini_orientation_;
+	   double global_path_final_orientation_;
+
+	   double multiply_person_companion_force_to_persons_;
+	   bool final_debug_journal_;
+	  double threshold_min_vel_person_to_obtain_destination_;  // threshold_min_vel_person_to_obtain_destination_=0.2
+
+
+	  double last_good_theta_person_;
+	  bool firts_iter_obtain_angle_person_companion_;
+
+	  double saved_actual_theta_person_;
+
+	  bool debug_gazebo_journal_;
+	  bool debug_gazebo_journal2_;
+
+	  Sdestination ideal_companion_goal_in_the_next_instant_of_time_; // serve to stop robot when person stop and the robot is in the best companion point for this stop person. (para esta persona parada)
+	  Sdestination before_companion_person_destination_;
+
+	  double clocks_per_sec_my_var_; // clocks_per_sec_my_var_=1000000
+
+	  double test_distance_to_goal_;
+
+	  bool final_goal_reached_in_node_;
+	  bool change_final_robot_orientation_; //
+
+
+	  std::vector<std::vector<double> > save_best_cost_branch_;
+	  bool is_case_akp_true_;
+	  bool enter_on_cero_;
+	  bool enter_on_cero2_;
+	  bool result_;
+
+	  double dist_to_goal_test_;
+	  double v_to_goal_test_;
+
+	  bool use_persoon_companion_to_calculate_plan_;
+
+	  Spoint medium_point_;
+
+	  unsigned int change_goal_of_the_error_;
+
+
+	  double initial_increment_of_time_from_file_; // variable of the time obtained from the frequency of the file from the simulator of companion, to obtain the parameters of the forces.
+
+	  Sforce f_overpas_obs_forward_;
+	  Spose robot_propagation_overpas_obs_forward_;
+	  double act_min_companion_angle_overpas_obs_forward;
+	  Sdestination person_companion_goal_overpas_obs_forward;
+	  Sforce saved_force_companion_go_rear_;
+	  Sforce saved_force_companion_go_front_;
+
+	  // Parameters for second person companion.
+	 // unsigned int id_SECOND_person_companion_;
+	 // SpointV_cov second_group_companion_personSpoint_;
+	  //pointer_to_SECOND_person_companion_
+	  //Spose second_group_companion_personSpose_;
+	  //Sdestination second_group_companion_personDest_;
+	  //bool we_have_second_person_companion_;
+
+	    Sdestination akp_out_path_goal_;
+	    Sdestination akp_out_companion_goal_;
+	    double companion_angle_peopl_in_group_; // angle in degrees.
+	    double real_distance_between_people_of_group_;
+	   // Cperson_abstract* second_group_companion_person_obj_;
+	    //bool we_have_pointer_to_second_person_; // if true, we can use the pointer!!!
+
+	    // TODO:insertar prediccion de la person companion de la person_list_ del scen_sim.
+
+	    Spoint before_next_goal_of_robot_; // (is the next immediate final goal to accompany the person) new variable to adjust well the robot velocity to accompany the person. (pure companion, do not takes into account the final goal for the velocity)
+	    // sacar marker de esta variable al nodo de ros, para ver si hace lo que quiero que haga
+
+
+	    // *** variables for use Francesco's companion model
+	   // bool Zanlungo_model_; // Francesco_model_=true, acting francesco model if false acting my model
+	   // bool Zanlungo_model3_;
+	    //Cgenome genome_params_;
+	    //CompanionFrancescoModel Companion_Zanlungo_Model_;
+
+	    unsigned int number_of_group_people_;
+
+	    //Sdestination before_person_comp_goal_Zalungo_formation_;
+	    std::vector<Sedge_tree_pcomp> saved_tree_forces_to_see_companion_Force_Zanlungo_;
+	   // std::vector<Sedge_tree_pcomp> saved_tree_forces_to_see_companion_Force_Zanlungo_best_path_;
+
+
+	    std::vector<double> person1_repulsive_force_fx_;
+	    std::vector<double> person1_repulsive_force_fy_;
+
+	    std::vector<double> person1_companion_force_fx_;
+	    std::vector<double> person1_companion_force_fy_;
+
+	    double person1_repulsive_force_fx_act_;
+	    double person1_repulsive_force_fy_act_;
+
+	    double person1_companion_force_fx_act_;
+	    double person1_companion_force_fy_act_;
+
+	    //std::vector<double> person2_repulsive_force_fx_;
+	    //std::vector<double> person2_repulsive_force_fy_;
+
+	   // std::vector<double> person2_companion_force_fx_;
+	  //  std::vector<double> person2_companion_force_fy_;
+
+	    //double person2_repulsive_force_fx_act_;
+	   // double person2_repulsive_force_fy_act_;
+
+	    //double person2_companion_force_fx_act_;
+	    //double person2_companion_force_fy_act_;
+
+	    //bool debug_zanlungo_;
+
+	   // double constante_multiplicar_fuerza_base_Zanlungo_;
+	    double constante_multiplicar_fuerza_goal_;
+
+	    double ori_pers_x_;
+	    double ori_pers_y_;
+
+	    std::vector<double> distance_betw_per_robo_plann_;
+	    std::vector<bool> in_margin_true_;
+
+	   // double up_distance_margin_Zamlungo_;
+	   // double down_distance_margin_Zamlungo_;
+	    //bool bool_robot_propagation_Zanlungo_;
+
+	    bool bool_distance_margin_;
+
+
+	   // std::string data_file_Zanlungo_;
+	    //void new_Zanlungo_file();
+	    double final_compt_time_;
+	    Spose final_pose_robot_;
+
+	   // std::vector<Vector2D> preferred_paths_Zanlungo_;
+	    std::vector<double> dist_per_rob_path_;
+	    unsigned int actual_best_path_index_;
+	    std::vector<double> companion_person_theta_path_; // es la orientacion de la persona haca el goal
+	    // faltan setear apartir de aqui
+	    /*std::vector<double> v_desired_robot_x_; // estan dentro de person abstract es mas complicado.
+	    std::vector<double> v_desired_robot_y_;
+	    std::vector<double> v_current_robot_x_;
+	    std::vector<double> v_current_robot_y_;
+	    std::vector<double> actual_random_goal_x_;
+	   	std::vector<double> actual_random_goal_y_;*/
+	    clock_t group_force_start, group_force_end, rrt_start_ITER,rrt_end_ITER,initial_part_start,initial_part_end,last_part_start,last_part_end;
+
+	   // bool  Zanlungo_model2_;
+
+		double dis_tol_, dis_tol_side_;
+
+		void robot_trajectory_prediction_for_group_Zanlungo( double new_horizon_time, Sdestination goal_to_predict_pose, unsigned int id_person_comp);
+		// no se ahora mismo si sale fuera esta de encima.
+		void find_entity_order_in_the_group();
+		void find_entity_order_in_the_group_for_person_companion();
+		Sforce ant_final_force_total_;
+
+	    //bool debug_output_screen_mesages_;
+	   // bool print_to_zanlungo_file_;
+
+	    int *order_people_in_group_global_variable_;
+
+	    bool change_ids_group_people_in_recongigure_;
+
+	    SpointV center_of_the_group_Zanlungo_; // estas NO se borran, las usa tambien 1 person companion
+	    SpointV mean_people_Zalungo_; // estas NO se borran, las usa tambien 1 person companion
+	    double theta_of_the_group_Zanlungo_; // estas NO se borran, las usa tambien 1 person companion
+
+	    double threshold_vel_pers_to_stop_;
+	    double dist_bet_rob_ext_goal_to_stop_;
+	    double dist_bet_pers1Com_ext_goal_to_stop_;
+	    double dist_bet_pers2Com_ext_goal_to_stop_;
+
+	    double mean(std::vector<double> arr, int n);   // Function to find mean.
+	    double variance(std::vector<double> a, int n); // Function to find variance.
+	    double covariance(std::vector<double> arr1, std::vector<double> arr2, int n); // Function to find covariance.
+
+	    double a_,b_; // parameters y=a+bx of the linear regression to find the preferred velocity of the robot from the actual person companion velocity. For the group is the mean of the velocity of the two people.
+
+	    std::vector<double> y_for_linear_regression_,x_for_linear_regression_;
+
+	    bool debug_stop_behaviour_;
+
+	   // std::vector<double> robot_desired_vel_calc_Zanlungo_; // the desired velocity for the robot, calculated from the persons companion velocity for the Zanlungo model.
+
+
+	    double treshold_distance_between_steps_;
+	    double threshold_max_total_force_;
+	    double real_vel_per;
+	    //Sforce companion_force_my_model_to_compare_with_zanlungo_;
+	   // Sforce goal_force_my_model_to_compare_with_zanlungo_;
+
+	    //Spose before_pose_robot_for_limit_w_zanlungo_;
+
+	   // std::vector<double> des_vel_zanlungo_;
+	    std::vector<double> des_vel_v_people_;
+	    //bool calculated_desired_vel_;
+	    double desired_vel_seteada_desde_fuera_;
+	    bool change_k_robot_,change_k_person_,change_k_person_comp_,change_k_obst_;
+	    bool initial_goal_case_bool_;
+
+	    Sdestination last_people_dest_;
+	    Spoint las_people_dest_spoint_;
+	    bool initial_not_set_last_people_dest_;
+
+	    double minimun_velocity_case_stop_initial_;
+
+	    double final_max_v_;
+
+	    double initial_limit_distance_goal_to_person_robot_stop_; // by deafault= 0.1 m/s
+
+	    bool bool_out_init_pose_robot_;
+
+	    double ideal_max_robot_velocity_; // 0.9 in muy system
+
+	    //bool debug_cost_francesco_;
+
+	    bool bool_tes_prop1;
+	    bool bool_tes_prop2;
+
+	    std::vector<unsigned int> order_people_in_group_;
+
+	    Sdestination before_person_comp_goal_;
+
+	   // bool plan_create_fake_fixed_second_person_companion_;
+	    bool actual_case_sim_goal_;
+
+	    Spoint SIM_initial_person_goal_pose1_,SIM_initial_person_goal_pose2_,SIM_initial_person_goal_pose3_;
+	    double actual_robot_performance_;
+
+	    double metros_al_dynamic_goal_Vform_;
+};
+
+
+#endif /* PLANN_LOCAL_NAV_PERSON_COMPANION_H_ */
+
diff --git a/local_lib_people_prediction/src/prediction_behavior.cpp b/local_lib_people_prediction/src/prediction_behavior.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..588df67a8dddec482e012df77ffca3704b0b96ff
--- /dev/null
+++ b/local_lib_people_prediction/src/prediction_behavior.cpp
@@ -0,0 +1,1468 @@
+/*
+ * prediction_behavior.cpp
+ *
+ *  Created on: 2013.  Last Modified on 2020
+ *      Author: Initial code of the AKP Gonzalo Ferrer.
+ *      Author: Addition of ASSAOP and ASSAGP for people accompaniment code of Ely Repiso.
+ */
+
+#include "prediction_behavior.h"
+#include "scene_elements/person_behavior.h"
+//#include <math.h>
+#include <algorithm>
+#include <stdio.h>
+#include <Eigen/Dense>
+#include <iostream>
+
+
+Cprediction_behavior::Cprediction_behavior( double horizon_time , bool behavior_flag,
+		Cperson_abstract::filtering_method filter, Cperson_abstract::update_person_method update_method ) :
+			Cprediction_bhmip(filter,update_method),
+	horizon_time_(horizon_time), behavior_flag_(behavior_flag),
+	debug_antes_subgoals_entre_AKP_goals_(false),
+	debug_scene_prediction_(false),
+	debug_scene_trajectory_prediction_simulation_(false),
+	id_person_companion_(1),
+	id_second_person_companion_(2),
+	sim_prediction_behaviour(true),
+	no_increment_prediction_(false),
+	num_max_iter_constan_time_steps_pred_(100) // a eliminar es de approach person
+{
+	horizon_time_index_ =  (unsigned int) (horizon_time_/dt_) ;
+	// paramters = {k,lambda,a,b,d} according to FerrerIcra2014
+	force_params_balanced_ = {2.3, 0.56,3.13,2.91,0.2};
+	force_params_aware_ = {2.3, 0.56,4.78,6.22,0.2};
+	force_params_unaware_= {2.3, 0.56,0.98,0.16,0.2};
+
+}
+
+Cprediction_behavior::~Cprediction_behavior()
+{
+	//deletion of robot and persons is done on its parent class Cprediction_bhmip
+}
+
+
+Cperson_abstract* Cprediction_behavior::add_person_container( unsigned int id,
+		std::list<Cperson_abstract*>::iterator iit )
+{
+	//this is the implementation of the abstract interface defined in Cscene_abstract, and although it
+	// it has been defined in the father class, when calling 'update_scene()' the new container used will
+	//be the one defined here
+	Cperson_abstract::target_type person_target_type_act;
+	if((id_person_companion_==id)||(id_second_person_companion_==id)){
+		person_target_type_act=Cperson_abstract::Person_companion;
+	}else{
+		person_target_type_act=Cperson_abstract::Person;
+	}
+
+
+	Cperson_behavior* person = new Cperson_behavior(id,person_target_type_act, scene_force_type_, filtering_time_window_);
+
+	person_list_.insert( iit , person );
+	person->set_destinations(destinations_);
+
+	return (Cperson_abstract*) person;
+}
+
+
+void Cprediction_behavior::scene_prediction(bool person_or_robot, std::vector<Sdestination>* person_best_dest)  // person_or_robot= true==person_companion. False==robot
+{
+
+	// Intentionality prediction
+	if(debug_scene_prediction_){
+		std::cout << " scene_prediction() antes scene_intentionality_prediction_bhmip(); dt="<<dt_<< std::endl;
+	}
+
+	if(!person_or_robot){ // caso robot o personas normales.
+		if(debug_scene_prediction_){
+			std::cout << " (case_robot) scene_intentionality_prediction_bhmip" << std::endl;
+		}
+		Cprediction_bhmip::scene_intentionality_prediction_bhmip();
+	}else{ // caso person companion simulada
+		// usamos person_best_dest como destinaciones para todas las personas. (caso Cscene_sim)
+		if(debug_scene_prediction_){
+			std::cout << "  Cprediction_behavior::scene_prediction (case_person_companion) usamos person_best_dest como destinaciones para todas las personas. (Simulation)"<< std::endl;
+		}
+		//std::cout << " scene_prediction(1)   person_list_.size()="<<person_list_.size()<< std::endl;
+		for( Cperson_abstract* iit: person_list_ )
+		{
+			if ( (iit->get_person_type() == Cperson_abstract::Person)||(iit->get_person_type() == Cperson_abstract::Person_companion))
+			{
+				//std::cout << "iit->get_id()="<<iit->get_id()<< std::endl;
+				//std::cout << "person_best_dest.size="<<person_best_dest->size()<< std::endl;
+				const Sdestination dest=person_best_dest->at(iit->get_id()-1);
+				iit->set_best_dest(dest);
+
+				if(debug_scene_prediction_){
+					std::cout << "iit->get_id()="<<iit->get_id()<< std::endl;
+					iit->get_best_dest().print();
+				}
+			}
+		}
+	}
+	//std::cout << " scene_prediction(1)"<< std::endl;
+
+	if(debug_scene_prediction_){
+		std::cout << " scene_prediction() antes scene_behavior_estimation();   behavior_flag_= "<< behavior_flag_<< std::endl;
+	}
+	//std::cout << " scene_prediction(2); behavior_flag_="<<behavior_flag_<< std::endl;
+	if( behavior_flag_  )
+	{
+		scene_behavior_estimation();
+	}
+	//std::cout << " scene_prediction(3)"<< std::endl;
+	if(debug_scene_prediction_){
+		std::cout << " scene_prediction() antes scene_trajectory_prediction();"<< std::endl;
+	}
+	//std::cout << " scene_prediction(4)"<< std::endl;
+	//calculation of the trajectories
+	if(!person_or_robot){ // caso robot o personas normales.
+
+		//std::cout << " caso robot o personas normales."<< std::endl;
+
+		//clock_t pred_peopl_start2, pred_peopl_end2;
+		//pred_peopl_start2=clock();
+		scene_trajectory_prediction();
+		//double clocks_per_sec_my_var_=1000000;
+		//pred_peopl_end2=clock();
+		//std::cout << " (entro en esta!) EEEEEEEEEEEEEEEEEEEEEEEEE 77777777777 PRED PEOPLE TIME!!! time=pred_peopl_end-pred_peopl_start="<<((pred_peopl_end2-pred_peopl_start2)/clocks_per_sec_my_var_)*1000<< std::endl;
+
+	}else{ // caso person companion simulada
+		//std::cout << "caso person companion simulada"<< std::endl;
+
+
+		//clock_t pred_peopl_start2, pred_peopl_end2;
+		//pred_peopl_start2=clock();
+		//double clocks_per_sec_my_var_=1000000;
+		scene_trajectory_prediction_simulation();
+		//pred_peopl_end2=clock();
+		//std::cout << "part2 EEEEEEEEEEEEEEEEEEEEEEEEE 77777777777 PRED PEOPLE TIME!!! time=pred_peopl_end-pred_peopl_start="<<((pred_peopl_end2-pred_peopl_start2)/clocks_per_sec_my_var_)*1000<< std::endl;
+
+		//scene_trajectory_prediction();
+	}
+	//std::cout << " scene_prediction(5)"<< std::endl;
+	if(debug_scene_prediction_){
+		std::cout << " scene_prediction() despues scene_trajectory_prediction();"<< std::endl;
+	}
+
+
+	/*std::cout <<  " scene_prediction() despues scene_trajectory_prediction_simulation(); " << std::endl;
+	for( auto iit : person_list_)
+	{
+		if(iit->get_id()!=id_person_companion_){
+		 std::cout << " (in scene_prediction) (1) iit->get_prediction_trajectory( )->size()="<<iit->get_prediction_trajectory( )->size()<<"; iit->get_id()=" <<iit->get_id()<<"; iit->desired_vel="<<iit->get_desired_velocity()<<  std::endl;
+		}
+	}*/
+
+}
+
+void Cprediction_behavior::scene_behavior_estimation()
+{
+	Sforce obs, goal;
+	double dt;
+	for( Cperson_abstract* observed_person: person_list_ )
+	{
+		//calculate the estimation for each force, according to the observed position and velocities
+		dt = observed_person->get_diff_position().time_stamp;
+		obs = Sforce( observed_person->get_diff_position().vx /dt , observed_person->get_diff_position().vy /dt);
+		goal = observed_person->force_goal( observed_person->get_best_dest(), this->get_sfm_params( observed_person) );
+		for ( Cperson_abstract* center_person : person_list_ )
+		{
+			if( *observed_person != *center_person  &&
+					observed_person->get_current_pointV().distance( center_person->get_current_pointV() ) < 4.0 )//threshold of 4[m]
+				this->behavior_update( observed_person, center_person, obs-goal );
+		}
+		//same calculation for the robot
+		if ( robot_in_the_scene_ ){
+		if( *observed_person != *robot_ &&
+				observed_person->get_current_pointV().distance( robot_->get_current_pointV() ) < 4.0 )//threshold of 4[m]
+			this->behavior_update( observed_person, robot_, obs-goal );
+		}
+	}
+
+}
+
+
+void Cprediction_behavior::behavior_update( Cperson_abstract* observed_person, Cperson_abstract* center_person, const Sforce& observed_int)
+{
+	//calculate the expected force
+	double lh,force_mod, marginalization, norm(0.0);
+	std::vector<double> posterior_prob( 3, 0.0 );
+	Sforce expected_force;
+	//find behavior for person observed, according to center person
+	Sbehavior * behavior = center_person->find_behavior_estimation ( observed_person->get_id() );
+	assert( behavior != NULL );
+	for ( unsigned int i = 0; i< behavior->expectation.size() ; i++)
+	{
+		marginalization = 0.0;
+		for( unsigned int j = 0; j< behavior->expectation.size() ; j++)
+		{
+			marginalization += behavior->expectation[j] * ( i == j ? 0.9 : 0.05 );//transition matrix parameters hrdcoded for 3 states
+		}
+		//HMM : the observed person recalculates its interaction force according to the sfm behaviors after interacting with center person,
+		// which updates its behavior information upon these results
+		expected_force = observed_person->force( center_person->get_current_pointV()  ,
+				get_behavior_sfm_params( (Cperson_abstract::behavior_type)i  )  );
+		force_mod = ( observed_int - expected_force ).module2();
+		//std::cout << "force_mod " << force_mod << "for iteration " << i << std::endl;
+		lh = exp( -0.5 * force_mod );//std = 1 TODO
+		posterior_prob[i] = marginalization * lh;
+		norm += posterior_prob[i];
+	}
+
+	//normalize HMM
+	if ( norm <= 0 ) norm = 1e-20;
+	for ( unsigned int i = 0; i< behavior->expectation.size() ; i++ )
+		behavior->expectation[i] =  posterior_prob[i] / norm;
+
+	//behavior->print();
+
+}
+
+const std::vector<double>*
+Cprediction_behavior::get_behavior_sfm_params( Cperson_abstract::behavior_type best_behavior_type )
+{
+
+	//std::cout << " IIINNNNNNNNN Cprediction_behavior::get_behavior_sfm_params " << std::endl;
+
+	switch( best_behavior_type)
+	{
+	  case Cperson_abstract::Balanced:
+		return &force_params_balanced_;
+	  case Cperson_abstract::Aware:
+		return &force_params_aware_;
+	  case Cperson_abstract::Unaware :
+	  default :
+		return &force_params_unaware_;
+	}
+}
+
+const std::vector<double>*
+Cprediction_behavior::get_sfm_int_params( const Cperson_abstract * center_person,	const Cperson_abstract * interacting_person )
+{
+
+	//std::cout << " IIINNNNNNNNN Cprediction_behavior::get_sfm_int_params " << std::endl;
+
+
+	const std::vector<double>* result = Cscene_abstract::get_sfm_int_params(center_person, interacting_person);
+
+	//if flag enabled, certain combinations of interactions may change, we modify only those that are different
+	if ( behavior_flag_ )
+	{
+
+		//std::cout << " IIINNNNNNNNN Cprediction_behavior::get_sfm_int_params behavior_flag_=on= "<<behavior_flag_ << std::endl;
+
+		result = get_behavior_sfm_params( center_person->get_best_behavior_to_person( interacting_person->get_id() ) );
+	}
+	return result;
+}
+
+void Cprediction_behavior::scene_trajectory_prediction(  )
+{
+	//std::cout<<" DO scene_trajectory_prediction(  ) horizon_time_index_= "<<horizon_time_index_<< std::endl;
+
+	// horizon_time_index_=5. salen 25 iter!!! 25*0.2segs=5segs => si va a 0.8m/s => 4m (ventana de 4m al rededor)
+
+	//std::cout << " Entro en Cprediction_behavior::scene_trajectory_prediction"<< std::endl;
+	//std::cout << " horizon_time_index_="<<horizon_time_index_<<"; person_list_.size()="<<person_list_.size()<<"; read_force_map_success_="<<read_force_map_success_<< std::endl;
+	//std::cout << " read_laser_obstacle_success_="<<read_laser_obstacle_success_<<"; robot_in_the_scene_="<<robot_in_the_scene_<<"; laser_obstacle_list_.empty()="<<laser_obstacle_list_.empty()<< std::endl;
+	SpointV_cov virtual_next_pose;
+	Sforce virtual_force_goal, virtual_force_int_person, virtual_force_obstacle, virtual_force_robot;
+	for( Cperson_abstract* iit: person_list_ )
+		iit->clear_prediction_trajectory();
+	//std::cout << " antes for"<< std::endl;
+	/*unsigned int t = 0;
+	for( Cperson_abstract* iit: person_list_ )
+	{
+		if(iit->get_id()!=1){
+			std::cout << "ini person. id="<< iit->get_id()<< std::endl;
+			std::cout << "(antes force goal) iit->get_best_dest()="<< std::endl;
+			virtual_force_goal = iit->force_goal(  iit->get_best_dest() , this->get_sfm_params(iit),
+									&(iit->get_prediction_trajectory()->at(t)) );
+			std::cout << "(antes force persons) virtual_force_goal.fx="<<virtual_force_goal.fx<<"; virtual_force_goal.fy="<<virtual_force_goal.fy<< std::endl;
+			virtual_force_int_person = force_persons_int_virtual( iit, t );
+			std::cout << "virtual_force_int_person.fx="<<virtual_force_int_person.fx<<"; virtual_force_int_person.fy="<<virtual_force_int_person.fy<< std::endl;
+		}
+	}*/
+	//std::cout << "OOOOOOOOOOOOOOOOOOOOOOO horizon_time_index_="<<horizon_time_index_<< std::endl;
+
+
+	for( unsigned int t = 0;  t < horizon_time_index_; ++t)
+	{
+		for( Cperson_abstract* iit: person_list_ )
+		{
+			if ( (iit->get_person_type() == Cperson_abstract::Person)||(iit->get_person_type() == Cperson_abstract::Person_companion))
+			{
+					//1 time step propagation for all people
+					//std::cout << "ini person. id="<< iit->get_id()<< std::endl;
+					//std::cout << "(antes force goal) iit->get_best_dest()="<< std::endl;
+					//iit->get_best_dest().print();
+					//std::cout << "person traj prediction at (t)="<<t<< std::endl;
+					//std::cout<<"; iit->get_prediction_trajectory().empty()="<<iit->get_prediction_trajectory()->empty()<< std::endl;
+					//iit->get_prediction_trajectory()->at(t).print();
+					//std::cout << "person parameters="<<t<< std::endl;
+					//for(unsigned int x = 0;  x < this->get_sfm_params(iit)->size(); ++x){
+					//	std::cout << "this->get_sfm_params(iit)->at("<<x<<")="<<this->get_sfm_params(iit)->at(x)<< std::endl;
+					//}
+					//std::cout << "iit->get_prediction_trajectory()->at(t):"<< std::endl;
+					//iit->get_prediction_trajectory()->at(t).print();
+
+					virtual_force_goal = iit->force_goal(  iit->get_best_dest() , this->get_sfm_params(iit),
+							&(iit->get_prediction_trajectory()->at(t)) );
+					/*if(iit->get_id()==5){
+						std::cout << std::endl<<std::endl<<std::endl<< std::endl;
+						std::cout << "ini person. id="<< iit->get_id()<< std::endl;
+						std::cout << "(antes force persons) virtual_force_goal.fx="<<virtual_force_goal.fx<<"; virtual_force_goal.fy="<<virtual_force_goal.fy<< std::endl;
+
+					}*/
+					virtual_force_int_person = force_persons_int_virtual( iit, t );
+
+
+					/*if(iit->get_id()==5){
+						std::cout << "virtual_force_int_person.fx="<<virtual_force_int_person.fx<<"; virtual_force_int_person.fy="<<virtual_force_int_person.fy<< std::endl;
+
+					}*/
+
+					// force depending on map to be deprecated in real environments, but still used on simulations
+					if( read_force_map_success_ ){
+						std::cout << " Antes obstacle map"<< std::endl;
+						virtual_force_obstacle = get_force_map( iit->get_prediction_trajectory()->at(t).x,
+															iit->get_prediction_trajectory()->at(t).y ) * 0.5;
+						//std::cout << "virtual_force_obstacle.fx="<<virtual_force_obstacle.fx<<"; virtual_force_obstacle.fy="<<virtual_force_obstacle.fy<< std::endl;
+					}
+
+					if(read_laser_obstacle_success_){
+						//std::cout << " Antes obstacle laser"<< std::endl;
+						virtual_force_obstacle = force_objects_laser_int_prediction( iit , t );
+
+						/*if(iit->get_id()==5){
+						std::cout << "virtual_force_obstacle.fx="<<virtual_force_obstacle.fx<<"; virtual_force_obstacle.fy="<<virtual_force_obstacle.fy<< std::endl;
+						}*/
+					}
+					//robot stays idle, no prediction is done
+					if ( robot_in_the_scene_ )
+					{
+						//std::cout << " Antes robot force"<< std::endl;
+						virtual_force_robot =  iit->force( robot_->get_current_pointV() , this->get_sfm_int_params(iit,robot_),
+							&(iit->get_prediction_trajectory()->at(t)) );
+						//std::cout << "virtual_force_robot.fx="<<virtual_force_robot.fx<<"; virtual_force_robot.fy="<<virtual_force_robot.fy<< std::endl;
+
+					}
+					else
+						virtual_force_robot = Sforce();
+
+					//std::cout << " Antes set_forces_person and prediction"<< std::endl;
+					iit->set_forces_person( virtual_force_goal, virtual_force_int_person, virtual_force_robot, virtual_force_obstacle );
+					//std::cout << "IMPORTANTE IMPORTANTE dt_="<<dt_<<"t"<<t<<"; iit->id="<<iit->get_id()<< std::endl;
+					iit->prediction_propagation(dt_, iit->get_force_person() , t);
+
+					/*if(iit->get_id()==5){
+					std::cout<<" f_tot.fx="<<iit->get_force_person().fx<<"; f_tot.fy="<<iit->get_force_person().fy<< std::endl;
+					iit->get_current_pointV().print();
+					}*/
+
+
+					//std::cout<<" (In Cprediction_behavior) id_person_companion_="<<id_person_companion_<< std::endl;
+					// START: calculate min collision to calculate companion angle of the companion goal (min collision between collision with persons or obstacles.)
+					if((iit->get_id()==id_person_companion_)&&(!calc_goal_companion_with_group_path_pred_)){
+						bool case_dynamic=false;
+						double min_final_collision_distance=100; // 100, if no collision is done!
+						if(distance_to_the_near_person_<min_final_collision_distance){
+							min_final_collision_distance=distance_to_the_near_person_;
+							case_dynamic=true;
+						}
+						if(distance_to_the_near_obsacle_<min_final_collision_distance){
+							min_final_collision_distance=distance_to_the_near_obsacle_;
+							case_dynamic=false;
+						}
+						// calculate angle of collision for the goal of companion!
+						/*bool we_have_collision=false;
+						if(min_final_collision_distance!=100){
+							we_have_collision=true;
+						}*/
+						//std::cout<<" (In Cprediction_behavior) min_final_collision_distance="<<min_final_collision_distance<<"; distance_to_the_near_obsacle_="<<distance_to_the_near_obsacle_<< std::endl;
+						//if(we_have_collision){
+							// calculate angle of collision for the goal of companion!
+							calculate_companion_angle_for_goal_companion( min_final_collision_distance, real_distance_of_companion_between_person_and_robot_, case_dynamic);
+						//}
+
+					}
+
+					// END: calculate min collision to calculate companion angle of the companion goal
+			}
+			//else //robot case: no prediction is done since it is the objective of the planning class.
+		}
+
+	//	std::cout<<" t="<<t<< std::endl;
+	}
+
+
+
+	//std::cout << "Salgo de scene_trajectory_prediction"<< std::endl;
+	/*for( Cperson_abstract* iit: person_list_ )
+	{
+		if(iit->get_id()==id_person_companion_){
+			std::cout << "iit->get_prediction_trajectory()->size() ="<<iit->get_prediction_trajectory()->size()<< std::endl;
+		}
+	}*/
+
+/*std::cout << "  1 INI orientation_person_robot_angles_with_prediction_of_person_companion_[g]: "<< std::endl;
+
+				for(unsigned int y=0;y<orientation_person_robot_angles_pred_.size();y++){
+					std::cout << "    orientation_person_robot_angles_pred_[y]="<<orientation_person_robot_angles_pred_[y]<< std::endl;
+				}
+*/
+
+
+}
+
+
+
+
+void Cprediction_behavior::scene_trajectory_prediction_simulation(  )
+{
+	//std::cout << "2222222 Entro en Cprediction_behavior::scene_trajectory_prediction_simulation"<< std::endl;
+
+	if(debug_scene_trajectory_prediction_simulation_){
+		std::cout << " Entro en Cprediction_behavior::scene_trajectory_prediction_simulation"<< std::endl;
+		std::cout << " horizon_time_index_="<<horizon_time_index_<<"; person_list_.size()="<<person_list_.size()<<"; read_force_map_success_="<<read_force_map_success_<< std::endl;
+		std::cout << " read_laser_obstacle_success_="<<read_laser_obstacle_success_<<"; robot_in_the_scene_="<<robot_in_the_scene_<<"; laser_obstacle_list_.empty()="<<laser_obstacle_list_.empty()<< std::endl;
+	}
+
+	SpointV_cov virtual_next_pose;
+	Sforce virtual_force_goal, virtual_force_int_person, virtual_force_obstacle, virtual_force_robot;
+	for( Cperson_abstract* iit: person_list_ )
+		iit->clear_prediction_trajectory();
+
+	if(debug_scene_trajectory_prediction_simulation_){
+		std::cout << " antes for  person_list_.size()="<<person_list_.size()<< std::endl;
+	}
+	/*unsigned int t = 0;
+	for( Cperson_abstract* iit: person_list_ )
+	{
+		if(iit->get_id()!=1){
+			std::cout << "ini person. id="<< iit->get_id()<< std::endl;
+			std::cout << "(antes force goal) iit->get_best_dest()="<< std::endl;
+			virtual_force_goal = iit->force_goal(  iit->get_best_dest() , this->get_sfm_params(iit),
+									&(iit->get_prediction_trajectory()->at(t)) );
+			std::cout << "(antes force persons) virtual_force_goal.fx="<<virtual_force_goal.fx<<"; virtual_force_goal.fy="<<virtual_force_goal.fy<< std::endl;
+			virtual_force_int_person = force_persons_int_virtual( iit, t );
+			std::cout << "virtual_force_int_person.fx="<<virtual_force_int_person.fx<<"; virtual_force_int_person.fy="<<virtual_force_int_person.fy<< std::endl;
+		}
+	}*/
+
+	/*std::cout << " my_id_person_companion_="<<my_id_person_companion_<<"; person_list_.size="<<person_list_.size()<< std::endl;
+	for( Cperson_abstract* iit: person_list_ )
+			{
+			std::cout << "(num of people) ini person. id="<< iit->get_id()<< std::endl;
+			}
+	 */
+
+
+	for( unsigned int t = 0;  t < horizon_time_index_; ++t)
+	{
+		for( Cperson_abstract* iit: person_list_ )
+		{
+			//if ( (iit->get_person_type() == Cperson_abstract::Person) && (iit->get_id()!=id_person_companion_) && (iit->get_id()!=id_second_person_companion_))
+			if ( ((iit->get_person_type() == Cperson_abstract::Person)||(iit->get_person_type() == Cperson_abstract::Person_companion)) && (iit->get_id()!=my_id_person_companion_) ) // Do not predict my own person prediction, as person companion.
+			{ //OJO!!! El robot tendra que entrar siempre en la otra prediccion.
+				//std::cout << "(SI!!!! PREDICTION PERSON) ini person. id="<< iit->get_id()<< std::endl;
+				//std::cout << "ini person. id="<< iit->get_id()<< std::endl;
+				//1 time step propagation for all people
+				if(debug_scene_trajectory_prediction_simulation_){
+					std::cout << "ini person. id="<< iit->get_id()<< std::endl;
+					std::cout << "(antes force goal) iit->get_best_dest()="<< std::endl;
+					iit->get_best_dest().print();
+					std::cout << "person traj prediction at (t)="<<t<< std::endl;
+					std::cout<<"; iit->get_prediction_trajectory().empty()="<<iit->get_prediction_trajectory()->empty()<< std::endl;
+					iit->get_prediction_trajectory()->at(t).print();
+					std::cout << "person parameters="<<t<< std::endl;
+					for(unsigned int x = 0;  x < this->get_sfm_params(iit)->size(); ++x){
+						std::cout << "this->get_sfm_params(iit)->at("<<x<<")="<<this->get_sfm_params(iit)->at(x)<< std::endl;
+					}
+					//std::cout << "iit->get_prediction_trajectory()->at(t):"<< std::endl;
+					iit->get_prediction_trajectory()->at(t).print();
+				}
+
+				//std::cout << "iit->get_id()="<<iit->get_id()<<"; iit->get_current_pointV().x="<<iit->get_current_pointV().x<<"; iit->get_current_pointV().y="<<iit->get_current_pointV().y<<"; iit->get_current_pointV().vx="<<iit->get_current_pointV().vx<<"; iit->get_current_pointV().vy"<<iit->get_current_pointV().vy<< std::endl;
+
+				virtual_force_goal = iit->force_goal(  iit->get_best_dest() , this->get_sfm_params(iit), &(iit->get_prediction_trajectory()->at(t)),t);
+
+				if(debug_scene_trajectory_prediction_simulation_){
+					std::cout << "iit->get_best_dest().x="<<iit->get_best_dest().x<<"; iit->get_best_dest().y="<<iit->get_best_dest().y<< std::endl;
+					std::cout << "(antes force persons) virtual_force_goal.fx="<<virtual_force_goal.fx<<"; virtual_force_goal.fy="<<virtual_force_goal.fy<< std::endl;
+				}
+				//virtual_force_int_person = force_persons_int_virtual( iit, t );
+
+				std::vector<int> ids_of_persons_in_group1;
+				ids_of_persons_in_group1.push_back(2);  // person with id=2,3 are in the first group
+				//ids_of_persons_in_group1.push_back(3); // puseta como comentario para caso no grupos!
+				//ids_of_persons_in_group1.push_back(6);
+				std::vector<int> ids_of_persons_in_group2;
+				ids_of_persons_in_group2.push_back(4);  // person with id=4,5 are in the first group
+				//ids_of_persons_in_group2.push_back(5);
+				if((iit->get_id()==2)||(iit->get_id()==3)){//||(iit->get_id()==6)){
+					//std::cout << " id=2 y id=3" << std::endl;
+					virtual_force_int_person = force_persons_int2( iit ,ids_of_persons_in_group1);
+				}else if((iit->get_id()==4)||(iit->get_id()==5)){
+					//std::cout << " id=4 y id=5" << std::endl;
+					virtual_force_int_person = force_persons_int2( iit ,ids_of_persons_in_group2);
+				}else if((iit->get_id()==id_person_companion_)||(iit->get_id()==id_second_person_companion_)){ // id_person_companion_==1 o otro...
+					//std::cout << " id_person_companion_="<<id_person_companion_<<"; id_second_person_companion_="<<id_second_person_companion_ << std::endl;
+					//virtual_force_int_person = force_persons_int_virtual( iit ,t);
+					virtual_force_int_person = force_persons_int_virtual_person_comp( iit ,t);
+
+				}else{
+					//std::cout << "in else antes virtual_force_int_person" << std::endl;
+					virtual_force_int_person = force_persons_int2( iit, ids_of_persons_in_group1 );
+				}
+
+				if(debug_scene_trajectory_prediction_simulation_){
+					std::cout << "virtual_force_int_person.fx="<<virtual_force_int_person.fx<<"; virtual_force_int_person.fy="<<virtual_force_int_person.fy<< std::endl;
+				}
+				// force depending on map to be deprecated in real environments, but still used on simulations
+				if( read_force_map_success_ ){
+
+					if(debug_scene_trajectory_prediction_simulation_){
+						std::cout << " Antes obstacle map"<< std::endl;
+					}
+
+					virtual_force_obstacle = get_force_map( (iit)->get_current_pointV().x, (iit)->get_current_pointV().y ) * 0.5;
+
+					if(debug_scene_trajectory_prediction_simulation_){
+						std::cout << "virtual_force_obstacle.fx="<<virtual_force_obstacle.fx<<"; virtual_force_obstacle.fy="<<virtual_force_obstacle.fy<< std::endl;
+					}
+				}
+
+				if(read_laser_obstacle_success_){ // ver si la he de cambiar como en el Scene_sim anterior.
+					if(debug_scene_trajectory_prediction_simulation_){
+						std::cout << " Antes obstacle laser"<< std::endl;
+					}
+					virtual_force_obstacle = force_objects_laser_int_prediction( iit , t );
+
+					if(debug_scene_trajectory_prediction_simulation_){
+						std::cout << " 3virtual_force_obstacle.fx="<<virtual_force_obstacle.fx<<"; virtual_force_obstacle.fy="<<virtual_force_obstacle.fy<< std::endl;
+					}
+				}
+				//robot stays idle, no prediction is done
+				if ( robot_in_the_scene_ )
+				{ // ver si la he de cambiar como en el Scene_sim anterior.
+					if(debug_scene_trajectory_prediction_simulation_){
+						std::cout << " Antes robot force"<< std::endl;
+					}
+					if((iit->get_id()!=id_person_companion_)||(iit->get_id()!=id_second_person_companion_)){
+					virtual_force_robot =  iit->force( robot_->get_current_pointV() , this->get_sfm_int_params(iit,robot_),
+						&(iit->get_prediction_trajectory()->at(t)) );
+					}else{
+						virtual_force_robot =Sforce();
+					}
+
+					if(debug_scene_trajectory_prediction_simulation_){
+						std::cout << " 2 virtual_force_robot.fx="<<virtual_force_robot.fx<<"; virtual_force_robot.fy="<<virtual_force_robot.fy<< std::endl;
+					}
+				}
+				else
+					virtual_force_robot = Sforce();
+
+				if(debug_scene_trajectory_prediction_simulation_){
+					std::cout << " Antes set_forces_person and prediction; dt_="<<dt_<< std::endl;
+				}
+
+				iit->set_forces_person( virtual_force_goal, virtual_force_int_person, virtual_force_robot, virtual_force_obstacle );
+				iit->prediction_propagation(dt_, iit->get_force_person() , t);
+
+
+			}else{
+				//else //robot case: no prediction is done since it is the objective of the planning class.
+				//std::cout << "(NO PREDICTION PERSON) ini person. id="<< iit->get_id()<< std::endl;
+			}
+
+		}
+	}
+
+
+
+	if(debug_scene_trajectory_prediction_simulation_){
+		std::cout << "Salgo de scene_trajectory_prediction_simulation"<< std::endl;
+	}
+}
+
+
+Sforce Cprediction_behavior::force_objects_laser_int_prediction( Cperson_abstract* person, unsigned int prediction_index)
+{
+	Sforce force_res;
+	const SpointV_cov* virtual_current_point;
+	SpointV_cov actual_person_Spoint;
+	if ( prediction_index == 0)
+	{
+		virtual_current_point = &(person->get_current_pointV());
+		actual_person_Spoint=person->get_current_pointV();
+	}
+	else
+	{
+		virtual_current_point = &(person->get_prediction_trajectory()->at(prediction_index) );
+		actual_person_Spoint=person->get_prediction_trajectory()->at(prediction_index);
+	}
+	distance_to_the_near_obsacle_=100; // firt distance. If dist==100 we do not have collisions!
+
+	for( Spoint iit : laser_obstacle_list_)
+	{
+		//there is a list for persons and another for robot(s). This function is for obstacles
+
+		// START: calculate min collision to calculate companion angle of the companion goal
+		if((person->get_id()==id_person_companion_)&&(!calc_goal_companion_with_group_path_pred_)){
+			//std::cout << " IN CALC goal companion PREDICTION"  << std::endl;
+			Spoint centro_person_robot2;
+			double angle1=orientation_person_robot_angles_pred_[0];
+			centro_person_robot2.x=actual_person_Spoint.x+(real_distance_of_companion_between_person_and_robot_/2)*cos(angle1);//+ theta1);
+			centro_person_robot2.y=actual_person_Spoint.y+(real_distance_of_companion_between_person_and_robot_/2)*sin(angle1);//+ theta1);
+
+
+			//double actual_collision_distance_with_obstacle=actual_person_Spoint.distance(iit);
+			double actual_collision_distance_with_obstacle=centro_person_robot2.distance(iit);
+			//std::cout << " actual_collision_distance_with_obstacle=" <<actual_collision_distance_with_obstacle << std::endl;
+			if(actual_collision_distance_with_obstacle<threshold_distance_collision_calculate_companion_angle_){
+				//std::cout << "1 distance_to_the_near_obsacle_=" <<distance_to_the_near_obsacle_ << std::endl;
+				if(distance_to_the_near_obsacle_>actual_collision_distance_with_obstacle){
+					//std::cout << "2 distance_to_the_near_obsacle_=" <<distance_to_the_near_obsacle_ << std::endl;
+					distance_to_the_near_obsacle_=actual_collision_distance_with_obstacle;
+					//std::cout << "3 distance_to_the_near_obsacle_=" <<distance_to_the_near_obsacle_ << std::endl;
+				}
+			}
+
+		}
+		// END: calculate min collision to calculate companion angle of the companion goal
+
+		//if( person->get_current_pointV().distance2( iit ) < 25.0)//square distance 5^2
+		if( actual_person_Spoint.distance2( iit ) < 25.0)//square distance 5^2
+		{
+
+			Sforce f_act=person->force_sphe( iit , this->get_sfm_int_params(person) , virtual_current_point );
+			//if((f_act.fx>0.5)||(f_act.fy>0.5)){
+			//std::cout << " f_act.fx=" <<f_act.fx<<"; f_act.fy="<<f_act.fy<< std::endl;
+			//}
+			force_res += f_act;
+		}
+	}
+	return force_res;
+
+}
+
+Sforce Cprediction_behavior::force_persons_int_virtual(Cperson_abstract* center , unsigned long t)
+{
+	//std::cout << " ENTRO EN =>>>> force_persons_int_virtual="  << std::endl;
+
+	Sforce force_res;
+	SpointV_cov person_int;
+
+	distance_to_the_near_person_=100; // firt distance. If dist==100 we do not have collisions!
+
+    for( auto iit : person_list_)
+    {
+
+    	if(iit->get_id()!=my_id_person_companion_){ // TODO: ojo, puede causar problemas en real.
+    												// no causa problemas al tener por defecto my_id_person_companion_=0. En el caso del robot.
+			//std::cout << "(1)iit->get_prediction_trajectory( )->size="<<iit->get_prediction_trajectory( )->size()<<"; iit->get_id="<<iit->get_id()<<"; t="<<t << std::endl; // peta aquí en (1)
+
+			person_int = iit->get_prediction_trajectory( )->at(t);
+			//std::cout << "(2); iit->id="<<iit->get_id() <<"; center->get_id()="<<center->get_id()<< std::endl;
+			if(((iit->get_person_type() == Cperson_abstract::Person)||(iit->get_person_type() == Cperson_abstract::Person_companion)) &&
+				(*center != *iit) && (center->get_prediction_trajectory()->at(t)
+				.distance( person_int ) < 15.0))
+			{
+				//std::cout << "(3)" << std::endl;
+			force_res += center->force( person_int ,  this->get_sfm_int_params(center,iit),
+					&(center->get_prediction_trajectory()->at(t)) );
+			//std::cout << "prediction, calculation of force using " << center->get_best_behavior_to_person( iit->get_id() ) << std::endl;
+
+
+			// START: calculate min collision to calculate companion angle of the companion goal
+			if(((center->get_id()==id_person_companion_)||(center->get_id()==id_second_person_companion_))&&(!calc_goal_companion_with_group_path_pred_)){
+			//	std::cout << " IN CALC goal companion PREDICTION; center->get_id()="<<center->get_id()  << std::endl;
+				Spoint centro_person_robot2;
+
+				double angle1=orientation_person_robot_angles_pred_[0];
+				//std::cout << " IN CALC goal companion PREDICTION; angle1="<<angle1  << std::endl;
+
+				centro_person_robot2.x=center->get_prediction_trajectory()->at(t).x+(real_distance_of_companion_between_person_and_robot_/2)*cos(angle1);//+ theta1);
+				centro_person_robot2.y=center->get_prediction_trajectory()->at(t).y+(real_distance_of_companion_between_person_and_robot_/2)*sin(angle1);//+ theta1);
+
+				//double actual_collision_distance_with_person=center->get_prediction_trajectory()->at(t).distance(person_int);
+				double actual_collision_distance_with_person=centro_person_robot2.distance(person_int);
+
+			//	std::cout << " actual_collision_distance_with_person=" <<actual_collision_distance_with_person << std::endl;
+				if(actual_collision_distance_with_person<threshold_distance_collision_calculate_companion_angle_){
+					if(distance_to_the_near_person_>actual_collision_distance_with_person){
+						distance_to_the_near_person_=actual_collision_distance_with_person;
+					}
+				}
+
+			}
+			// END: calculate min collision to calculate companion angle of the companion goal
+
+			}
+    	}
+
+    }
+  // std::cout << "(4)" << std::endl;
+	return force_res;
+}
+
+///
+
+
+
+Sforce Cprediction_behavior::force_persons_int_virtual_person_comp(Cperson_abstract* center , unsigned long t)
+{
+	//std::cout << " ENTRO EN =>>>> force_persons_int_virtual="  << std::endl;
+
+	Sforce force_res;
+	SpointV_cov person_int;
+
+	distance_to_the_near_person_=100; // firt distance. If dist==100 we do not have collisions!
+
+    for( auto iit : person_list_)
+    {
+
+    	if(iit->get_id()!=my_id_person_companion_){ // TODO: ojo, puede causar problemas en real.
+    												// no causa problemas al tener por defecto my_id_person_companion_=0. En el caso del robot.
+			//std::cout << "(1)iit->get_prediction_trajectory( )->size="<<iit->get_prediction_trajectory( )->size()<<"; iit->get_id="<<iit->get_id()<<"; t="<<t << std::endl; // peta aquí en (1)
+
+			person_int = iit->get_prediction_trajectory( )->at(t);
+			//std::cout << "(2); iit->id="<<iit->get_id() <<"; center->get_id()="<<center->get_id()<< std::endl;
+			if(((iit->get_person_type() == Cperson_abstract::Person)||(iit->get_person_type() == Cperson_abstract::Person_companion)) &&
+				(*center != *iit) && (center->get_prediction_trajectory()->at(t)
+				.distance( person_int ) < 15.0))
+			{
+			//	std::cout << "(3)" << std::endl;
+			force_res += center->force( person_int ,  this->get_sfm_int_params(center,iit),
+					&(center->get_prediction_trajectory()->at(t)) );
+			std::cout << "prediction, calculation of force using " << center->get_best_behavior_to_person( iit->get_id() ) << std::endl;
+
+
+			// START: calculate min collision to calculate companion angle of the companion goal
+			/*if(((center->get_id()==id_person_companion_)||(center->get_id()==id_second_person_companion_))&&(!calc_goal_companion_with_group_path_pred_)){
+				std::cout << " IN CALC goal companion PREDICTION; center->get_id()="<<center->get_id()  << std::endl;
+				Spoint centro_person_robot2;
+
+				double angle1=orientation_person_robot_angles_pred_[0];
+				std::cout << " IN CALC goal companion PREDICTION; angle1="<<angle1  << std::endl;
+
+				centro_person_robot2.x=center->get_prediction_trajectory()->at(t).x+(real_distance_of_companion_between_person_and_robot_/2)*cos(angle1);//+ theta1);
+				centro_person_robot2.y=center->get_prediction_trajectory()->at(t).y+(real_distance_of_companion_between_person_and_robot_/2)*sin(angle1);//+ theta1);
+
+				//double actual_collision_distance_with_person=center->get_prediction_trajectory()->at(t).distance(person_int);
+				double actual_collision_distance_with_person=centro_person_robot2.distance(person_int);
+
+				std::cout << " actual_collision_distance_with_person=" <<actual_collision_distance_with_person << std::endl;
+				if(actual_collision_distance_with_person<threshold_distance_collision_calculate_companion_angle_){
+					if(distance_to_the_near_person_>actual_collision_distance_with_person){
+						distance_to_the_near_person_=actual_collision_distance_with_person;
+					}
+				}
+
+			}*/
+			// END: calculate min collision to calculate companion angle of the companion goal
+
+			}
+    	}
+
+    }
+   //std::cout << "(4)" << std::endl;
+	return force_res;
+}
+
+////
+
+
+void Cprediction_behavior::set_horizon_time( double t )
+{
+	horizon_time_ = t;
+	horizon_time_index_ =  (unsigned int) (horizon_time_/dt_) ;
+}
+
+void Cprediction_behavior::clear_scene()
+{
+
+}
+
+
+//calculates the current forces in the scene and updates each of the Cpersons and Crobot with
+//instantaneous force information according to observations (current positions)
+void Cprediction_behavior::calculate_current_forces( int pr_force_mode, Sforce companion_force_Zanlungo,Sforce goal, Sforce persons, Sforce obstacles, double alpa_comp)
+{
+
+	//std::cout << "IN calculate_current_forces="<< std::endl;
+
+	SpointV_cov virtual_next_pose;
+	Sforce virtual_force_goal, virtual_force_int_person, virtual_force_obstacle, virtual_force_robot;
+
+
+	for( Cperson_abstract* iit: person_list_ )
+	{
+		if ( (iit->get_person_type() == Cperson_abstract::Person)||(iit->get_person_type() == Cperson_abstract::Person_companion))
+		{
+			//1 time step propagation for all people
+			virtual_force_goal = iit->force_goal(  iit->get_best_dest() , this->get_sfm_params(iit) );
+			virtual_force_int_person = Cscene_abstract::force_persons_int( iit);
+			// force depending on map to be deprecated in real environments, but still used on simulations
+			if( read_force_map_success_ )
+				virtual_force_obstacle = get_force_map( iit->get_current_pointV().x,
+						iit->get_current_pointV().y ) * 0.5;
+			if(read_laser_obstacle_success_)
+				virtual_force_obstacle = Cscene_abstract::force_objects_laser_int( iit  );
+			//robot stays idle, no prediction is done
+			if ( robot_in_the_scene_ )
+			{
+				switch( pr_force_mode )
+				{
+				case 0: //deterministic force, classical definition
+					//std::cout << "force robot 0="<< std::endl;
+					//virtual_force_robot = iit->force( robot_->get_current_pointV() , this->get_sfm_int_params(iit,robot_), &(iit->get_current_pointV()) );
+					break;
+				case 1: //probabilistic force, sampling around the elipsoid
+					//std::cout << "force robot 1="<< std::endl;
+					virtual_force_robot = iit->force_sphe_prob( robot_->get_current_pointV(), get_sfm_int_params(iit,robot_),&(iit->get_current_pointV()) );
+					break;
+				case 2: //probabilistic force, mahalanobis distance
+					//std::cout << "force robot 2="<< std::endl;
+					virtual_force_robot = iit->force_sphe_mahalanobis( robot_->get_current_pointV(), get_sfm_int_params(iit,robot_), &(iit->get_current_pointV()));
+					break;
+				case 3: //probabilistic force, worst case scenario: covaraince ellipsoid nearer to the center point
+					//std::cout << "force robot 3="<< std::endl;
+					virtual_force_robot = iit->force_sphe_worst( robot_->get_current_pointV(), get_sfm_int_params(iit,robot_),&(iit->get_current_pointV()));
+					break;
+				}
+			}
+			else
+				virtual_force_robot = Sforce();
+			iit->set_forces_person( virtual_force_goal, virtual_force_int_person, virtual_force_robot, virtual_force_obstacle );
+		}
+	}
+	//robot update, if any
+	if ( robot_in_the_scene_ )
+	{
+		virtual_force_goal = robot_->force_goal(  robot_->get_best_dest() , this->get_sfm_params(robot_) );
+		virtual_force_int_person = Cscene_abstract::force_persons_int( robot_ );
+		// force depending on map to be deprecated in real environments, but still used on simulations
+		if( read_force_map_success_ )
+			virtual_force_obstacle = get_force_map( robot_->get_current_pointV().x,
+					robot_->get_current_pointV().y ) * 0.5;
+		if(read_laser_obstacle_success_)
+			virtual_force_obstacle = Cscene_abstract::force_objects_laser_int( robot_  );
+
+		double fobos_Xmod=sqrt(virtual_force_obstacle.fx*virtual_force_obstacle.fx);
+		double fobos_Ymod=sqrt(virtual_force_obstacle.fy*virtual_force_obstacle.fy);
+		double signo_x, signo_y;
+
+		double f_obst_max_x_=0.1;
+		double f_obst_max_y_=0.1;
+		if (virtual_force_obstacle.fx > 0){
+			signo_x=1;
+		}else{
+			signo_x=-1;
+		}
+		if (virtual_force_obstacle.fy > 0){
+			signo_y=1;
+		}else{
+			signo_y=-1;
+		}
+		//std::cout << " out to máx force virtual_force_obstacle.fx="<<virtual_force_obstacle.fx<<"; virtual_force_obstacle.fy="<<virtual_force_obstacle.fy<< std::endl;
+		if(fobos_Xmod>f_obst_max_x_){
+			virtual_force_obstacle.fx=signo_x*f_obst_max_x_;
+			//std::cout << " Enter to máx virtual_force_obstacle; virtual_force_obstacle.fx="<<virtual_force_obstacle.fx<< std::endl;
+		}
+
+		if(fobos_Ymod>f_obst_max_y_){
+			virtual_force_obstacle.fy=signo_y*f_obst_max_y_;
+			//std::cout << " Enter to máx force; virtual_force_obstacle.fy="<<virtual_force_obstacle.fy<< std::endl;
+		}
+		//std::cout << "(FINAL FORCE) f_obs.fx="<<f_obs.fx<<"; f_obs.fy="<<f_obs.fy<< std::endl;
+		// FIN Limitar fuerza obstaculos cuando hay muchos solapados
+		//if(debug_zanlungo_){
+		//	std::cout << " Zanlungo model after forces"<<std::endl;
+		//	std::cout << "(force_goal) f_goal.fx="<<f_goal.fx<<"; f_goal.fy="<<f_goal.fy<<std::endl;
+		//}
+
+
+
+		virtual_force_robot = Sforce();
+		robot_->set_forces_person( virtual_force_goal*alpa_comp, virtual_force_int_person, virtual_force_robot, virtual_force_obstacle );
+		robot_->set_forces_person_companion( virtual_force_goal, virtual_force_int_person, virtual_force_robot, virtual_force_obstacle, companion_force_Zanlungo );
+
+	}
+
+	//std::cout << "OUT calculate_current_forces="<< std::endl;
+}
+
+
+//calculates the current forces in the scene and updates each of the Cpersons and Crobot with
+//instantaneous force information according to observations (current positions)
+void Cprediction_behavior::calculate_current_forces_companion( int pr_force_mode , unsigned int id_person_companion, bool person_reactive, Cperson_abstract::companion_reactive type_rec_akp, double act_min_companion_angle)
+{
+
+	// TODO: no se si este robot tendria que ser person companion...
+	SpointV_cov virtual_next_pose;
+	Sforce virtual_force_goal, virtual_force_int_person, virtual_force_obstacle, virtual_force_robot, virtual_force_companion;
+	SpointV_cov person;
+	Sdestination person_goal;
+	//double robot_person_proximity_distance=2;
+	//double robot_person_proximity_tolerance=1;
+
+	for( Cperson_abstract* iit: person_list_ )
+	{
+		if ( (iit->get_person_type() == Cperson_abstract::Person)||(iit->get_person_type() == Cperson_abstract::Person_companion))
+		{
+			//1 time step propagation for all people
+			virtual_force_goal = iit->force_goal(  iit->get_best_dest() , this->get_sfm_params(iit) );
+			virtual_force_int_person = Cscene_abstract::force_persons_int( iit);
+			// force depending on map to be deprecated in real environments, but still used on simulations
+			if( read_force_map_success_ )
+				virtual_force_obstacle = get_force_map( iit->get_current_pointV().x,
+						iit->get_current_pointV().y ) * 0.5;
+			if(read_laser_obstacle_success_)
+				virtual_force_obstacle = Cscene_abstract::force_objects_laser_int( iit  );
+			//robot stays idle, no prediction is done
+
+			if ( robot_in_the_scene_ )
+			{
+				if( iit->get_id()!=id_person_companion){
+					switch( pr_force_mode )
+					{
+					case 0: //deterministic force, classical definition
+						virtual_force_robot = iit->force( robot_->get_current_pointV() , this->get_sfm_int_params(iit,robot_), &(iit->get_current_pointV()) );
+						break;
+					case 1: //probabilistic force, sampling around the elipsoid
+						virtual_force_robot = iit->force_sphe_prob( robot_->get_current_pointV(), get_sfm_int_params(iit,robot_),&(iit->get_current_pointV()) );
+						break;
+					case 2: //probabilistic force, mahalanobis distance
+						virtual_force_robot = iit->force_sphe_mahalanobis( robot_->get_current_pointV(), get_sfm_int_params(iit,robot_), &(iit->get_current_pointV()));
+						break;
+					case 3: //probabilistic force, worst case scenario: covaraince ellipsoid nearer to the center point
+						virtual_force_robot = iit->force_sphe_worst( robot_->get_current_pointV(), get_sfm_int_params(iit,robot_),&(iit->get_current_pointV()));
+						break;
+					}
+				}else{
+					/*double distance_person_robot=robot_->get_current_pointV().distance((Spoint) iit->get_current_pointV());
+					if(distance_person_robot<(robot_person_proximity_distance-robot_person_proximity_tolerance)){
+						switch( pr_force_mode )
+						{
+						case 0: //deterministic force, classical definition
+							virtual_force_robot = iit->force( robot_->get_current_pointV() , this->get_sfm_int_params(iit,robot_), &(iit->get_current_pointV()) );
+							break;
+						case 1: //probabilistic force, sampling around the elipsoid
+							virtual_force_robot = iit->force_sphe_prob( robot_->get_current_pointV(), get_sfm_int_params(iit,robot_),&(iit->get_current_pointV()) );
+							break;
+						case 2: //probabilistic force, mahalanobis distance
+							virtual_force_robot = iit->force_sphe_mahalanobis( robot_->get_current_pointV(), get_sfm_int_params(iit,robot_), &(iit->get_current_pointV()));
+							break;
+						case 3: //probabilistic force, worst case scenario: covaraince ellipsoid nearer to the center point
+							virtual_force_robot = iit->force_sphe_worst( robot_->get_current_pointV(), get_sfm_int_params(iit,robot_),&(iit->get_current_pointV()));
+							break;
+						}
+					}else{
+						// no hay force rbot-perosn, companion.
+					}*/
+				}
+			}
+			else
+				virtual_force_robot = Sforce();
+			iit->set_forces_person( virtual_force_goal, virtual_force_int_person, virtual_force_robot, virtual_force_obstacle );
+
+			if(iit->get_id()==id_person_companion){
+				person = iit->get_current_pointV();//person_obj->get_planning_trajectory()->at(parent_vertex);
+				person_goal=iit->get_best_dest();
+			}
+
+		}
+	}
+	//robot update, if any
+	if ( robot_in_the_scene_ )
+	{
+		virtual_force_goal = robot_->force_goal(  robot_->get_best_dest() , this->get_sfm_params(robot_) );
+		virtual_force_int_person = Cscene_abstract::force_persons_int_companion( robot_ , id_person_companion,person_reactive, type_rec_akp);
+		// force depending on map to be deprecated in real environments, but still used on simulations
+		if( read_force_map_success_ )
+			virtual_force_obstacle = get_force_map( robot_->get_current_pointV().x,
+					robot_->get_current_pointV().y ) * 0.5;
+		if(read_laser_obstacle_success_)
+			virtual_force_obstacle = Cscene_abstract::force_objects_laser_int( robot_  );
+		virtual_force_robot = Sforce();
+
+		//Cperson_abstract* person_obj;
+		//find_person(id_person_companion_ , &person_obj);
+		SpointV_cov robot=robot_->get_current_pointV();
+		SpointV_cov robot_center_traj_ideal_position=person;
+		double angle = atan2(robot.y-person.y,robot.x-person.x);
+		double theta = atan2(person_goal.y-person.y,person_goal.x-person.x);
+		//double act_min_companion_angle=orientation_person_robot_angles_[parent_vertex];
+		if(diffangle(theta,angle)<0){
+			robot_center_traj_ideal_position.x+=(2/2)*cos(theta+(act_min_companion_angle*(3.14/180)));
+			robot_center_traj_ideal_position.y+=(2/2)*sin(theta+(act_min_companion_angle*(3.14/180)));
+			//person_companion_goal.x+=2*cos(theta+(act_min_companion_angle*(3.14/180)));//1.5*cos(theta+(act_min_companion_angle*(3.14/180)));
+			//person_companion_goal.y+=2*sin(theta+(act_min_companion_angle*(3.14/180)));//1.5*sin(theta+(act_min_companion_angle*(3.14/180)));
+		}else{
+			robot_center_traj_ideal_position.x+=(2/2)*cos(theta-(act_min_companion_angle*(3.14/180)));
+			robot_center_traj_ideal_position.y+=(2/2)*sin(theta-(act_min_companion_angle*(3.14/180)));
+			//person_companion_goal.x+=2*cos(theta-(act_min_companion_angle*(3.14/180)));//1.5*cos(theta-(act_min_companion_angle*(3.14/180)));
+			//person_companion_goal.y+=2*sin(theta-(act_min_companion_angle*(3.14/180)));//1.5*sin(theta-(act_min_companion_angle*(3.14/180)));
+		}
+		Sdestination robot_companion_dest(0.0, robot_center_traj_ideal_position.x, robot_center_traj_ideal_position.y, 1.0);
+		virtual_force_companion=robot_->force_goal(robot_companion_dest,this->get_sfm_params(robot_));
+
+		if(debug_antes_subgoals_entre_AKP_goals_){
+			std::cout << std::endl;
+			std::cout << std::endl;
+			std::cout << "(MARKER!!!!  virtual_force_companion.fx="<<virtual_force_companion.fx<<"; virtual_force_companion.fy="<<virtual_force_companion.fy<< std::endl;
+			std::cout << "(MARKER!!!!  robot_companion_dest.x="<<robot_companion_dest.x<<"; robot_companion_dest.y="<<robot_companion_dest.y<< std::endl;
+			std::cout << std::endl;
+			std::cout << std::endl;
+		}
+		robot_->set_companion_force_goal(robot_companion_dest);
+		robot_->set_forces_person_companion( virtual_force_goal, virtual_force_int_person, virtual_force_robot, virtual_force_obstacle, virtual_force_companion );
+	}
+}
+
+
+
+//calculates the current forces in the scene and updates each of the Cpersons and Crobot with
+//instantaneous force information according to observations (current positions)
+void Cprediction_behavior::calculate_current_forces_companion2( int pr_force_mode , unsigned int id_person_companion, bool person_reactive, Cperson_abstract::companion_reactive type_rec_akp, double act_min_companion_angle, Sforce goal, Sforce persons, Sforce obstacles, Sforce companion_person_goal, double &other_people_cost_due_to_robot, double &companion_person_cost_due_to_robot)
+{
+	SpointV_cov virtual_next_pose;
+	Sforce virtual_force_goal, virtual_force_int_person, virtual_force_obstacle, virtual_force_robot, virtual_force_robot_companion_person, virtual_force_companion;
+	SpointV_cov person;
+	Sdestination person_goal;
+	//double robot_person_proximity_distance=2;
+	//double robot_person_proximity_tolerance=1;
+
+	for( Cperson_abstract* iit: person_list_ )
+	{
+		if ( (iit->get_person_type() == Cperson_abstract::Person)||(iit->get_person_type() == Cperson_abstract::Person_companion))
+		{
+			//1 time step propagation for all people
+			virtual_force_goal = iit->force_goal(  iit->get_best_dest() , this->get_sfm_params(iit) );
+			virtual_force_int_person = Cscene_abstract::force_persons_int( iit);
+			// force depending on map to be deprecated in real environments, but still used on simulations
+			if( read_force_map_success_ )
+				virtual_force_obstacle = get_force_map( iit->get_current_pointV().x,
+						iit->get_current_pointV().y ) * 0.5;
+			if(read_laser_obstacle_success_)
+				virtual_force_obstacle = Cscene_abstract::force_objects_laser_int( iit  );
+			//robot stays idle, no prediction is done
+
+			if ( robot_in_the_scene_ )
+			{
+				if( iit->get_id()!=id_person_companion){
+					switch( pr_force_mode )
+					{
+					case 0: //deterministic force, classical definition
+						virtual_force_robot = iit->force( robot_->get_current_pointV() , this->get_sfm_int_params(iit,robot_), &(iit->get_current_pointV()) );
+						break;
+					case 1: //probabilistic force, sampling around the elipsoid
+						virtual_force_robot = iit->force_sphe_prob( robot_->get_current_pointV(), get_sfm_int_params(iit,robot_),&(iit->get_current_pointV()) );
+						break;
+					case 2: //probabilistic force, mahalanobis distance
+						virtual_force_robot = iit->force_sphe_mahalanobis( robot_->get_current_pointV(), get_sfm_int_params(iit,robot_), &(iit->get_current_pointV()));
+						break;
+					case 3: //probabilistic force, worst case scenario: covaraince ellipsoid nearer to the center point
+						virtual_force_robot = iit->force_sphe_worst( robot_->get_current_pointV(), get_sfm_int_params(iit,robot_),&(iit->get_current_pointV()));
+						break;
+					}
+				}else{
+					//double distance_person_robot=robot_->get_current_pointV().distance((Spoint) iit->get_current_pointV());
+					//if(distance_person_robot<(robot_person_proximity_distance-robot_person_proximity_tolerance)){
+					switch( pr_force_mode )
+					{
+						case 0: //deterministic force, classical definition
+							virtual_force_robot_companion_person = iit->force( robot_->get_current_pointV() , this->get_sfm_int_params(iit,robot_), &(iit->get_current_pointV()) );
+							break;
+						case 1: //probabilistic force, sampling around the elipsoid
+							virtual_force_robot_companion_person = iit->force_sphe_prob( robot_->get_current_pointV(), get_sfm_int_params(iit,robot_),&(iit->get_current_pointV()) );
+							break;
+						case 2: //probabilistic force, mahalanobis distance
+							virtual_force_robot_companion_person = iit->force_sphe_mahalanobis( robot_->get_current_pointV(), get_sfm_int_params(iit,robot_), &(iit->get_current_pointV()));
+							break;
+						case 3: //probabilistic force, worst case scenario: covaraince ellipsoid nearer to the center point
+							virtual_force_robot_companion_person = iit->force_sphe_worst( robot_->get_current_pointV(), get_sfm_int_params(iit,robot_),&(iit->get_current_pointV()));
+							break;
+					}
+					//}else{
+					// no hay force rbot-perosn, companion.
+					//}
+				}
+			}
+			else
+				virtual_force_robot = Sforce();
+			iit->set_forces_person( virtual_force_goal, virtual_force_int_person, virtual_force_robot, virtual_force_obstacle );
+
+			if(iit->get_id()==id_person_companion){
+				person = iit->get_current_pointV();//person_obj->get_planning_trajectory()->at(parent_vertex);
+				person_goal=iit->get_best_dest();
+			}
+
+		}
+	}
+
+	other_people_cost_due_to_robot=virtual_force_robot.module2();
+	companion_person_cost_due_to_robot=virtual_force_robot_companion_person.module2();
+
+	//robot update, if any
+	if ( robot_in_the_scene_ )
+	{
+
+		if( (Cperson_abstract::Reactiva_repulsive==type_rec_akp)||(Cperson_abstract::Reactive_atractive==type_rec_akp)){
+			virtual_force_goal = robot_->force_goal(  robot_->get_best_dest() , this->get_sfm_params(robot_) );
+			virtual_force_int_person = Cscene_abstract::force_persons_int_companion( robot_ , id_person_companion,person_reactive, type_rec_akp);
+			// force depending on map to be deprecated in real environments, but still used on simulations
+			if( read_force_map_success_ )
+				virtual_force_obstacle = get_force_map( robot_->get_current_pointV().x,
+						robot_->get_current_pointV().y ) * 0.5;
+			if(read_laser_obstacle_success_)
+				virtual_force_obstacle = Cscene_abstract::force_objects_laser_int( robot_  );
+			virtual_force_robot = Sforce();
+
+			//Cperson_abstract* person_obj;
+			//find_person(id_person_companion_ , &person_obj);
+			// Return to calculate companion_force
+			SpointV_cov robot=robot_->get_current_pointV();
+			SpointV_cov robot_center_traj_ideal_position=person;
+			double angle = atan2(robot.y-person.y,robot.x-person.x);
+			double theta = atan2(person_goal.y-person.y,person_goal.x-person.x);
+			//double act_min_companion_angle=orientation_person_robot_angles_[parent_vertex];
+			if(diffangle(theta,angle)<0){
+				robot_center_traj_ideal_position.x+=(2/2)*cos(theta+(act_min_companion_angle*(3.14/180)));
+				robot_center_traj_ideal_position.y+=(2/2)*sin(theta+(act_min_companion_angle*(3.14/180)));
+				//person_companion_goal.x+=2*cos(theta+(act_min_companion_angle*(3.14/180)));//1.5*cos(theta+(act_min_companion_angle*(3.14/180)));
+				//person_companion_goal.y+=2*sin(theta+(act_min_companion_angle*(3.14/180)));//1.5*sin(theta+(act_min_companion_angle*(3.14/180)));
+			}else{
+				robot_center_traj_ideal_position.x+=(2/2)*cos(theta-(act_min_companion_angle*(3.14/180)));
+				robot_center_traj_ideal_position.y+=(2/2)*sin(theta-(act_min_companion_angle*(3.14/180)));
+				//person_companion_goal.x+=2*cos(theta-(act_min_companion_angle*(3.14/180)));//1.5*cos(theta-(act_min_companion_angle*(3.14/180)));
+				//person_companion_goal.y+=2*sin(theta-(act_min_companion_angle*(3.14/180)));//1.5*sin(theta-(act_min_companion_angle*(3.14/180)));
+			}
+			Sdestination robot_companion_dest(0.0, robot_center_traj_ideal_position.x, robot_center_traj_ideal_position.y, 1.0);
+			virtual_force_companion=robot_->force_goal(robot_companion_dest,this->get_sfm_params(robot_));
+
+			if(debug_antes_subgoals_entre_AKP_goals_){
+				std::cout << std::endl;
+				std::cout << std::endl;
+				std::cout << "(MARKER!!!!  virtual_force_companion.fx="<<virtual_force_companion.fx<<"; virtual_force_companion.fy="<<virtual_force_companion.fy<< std::endl;
+				std::cout << "(MARKER!!!!  robot_companion_dest.x="<<robot_companion_dest.x<<"; robot_companion_dest.y="<<robot_companion_dest.y<< std::endl;
+				std::cout << std::endl;
+				std::cout << std::endl;
+			}
+			robot_->set_companion_force_goal(robot_companion_dest);
+			robot_->set_forces_person_companion( virtual_force_goal, virtual_force_int_person, virtual_force_robot, virtual_force_obstacle, virtual_force_companion );
+		}else{
+			// Sforce goal, Sforce persons, Sforce obstacles, Sforce companion_person_goal
+			virtual_force_robot = Sforce();
+			robot_->set_forces_person_companion( goal, persons, virtual_force_robot, obstacles, companion_person_goal);
+
+		}
+
+
+	}
+}
+
+/*   */
+//calculates the current forces in the scene and updates each of the Cpersons and Crobot with
+//instantaneous force information according to observations (current positions)
+void Cprediction_behavior::calculate_current_forces_companion2_for_person_companion_akp( int pr_force_mode , unsigned int id_person_companion, bool person_reactive, Cperson_abstract::companion_reactive type_rec_akp, double act_min_companion_angle, Sforce goal, Sforce persons, Sforce obstacles, Sforce companion_person_goal, double &other_people_cost_due_to_robot, double &companion_person_cost_due_to_robot)
+{
+	//std::cout << "IN (1) !!! calculate_current_forces_companion2_for_person_companion_akp" << std::endl;
+	SpointV_cov virtual_next_pose;
+	Sforce virtual_force_goal, virtual_force_int_person, virtual_force_obstacle, virtual_force_robot;
+
+	for( Cperson_abstract* iit: person_list_ )
+	{
+		//std::cout << "IN (2) !!! calculate_current_forces_companion2_for_person_companion_akp" << std::endl;
+
+		if (( iit->get_person_type() == Cperson_abstract::Person)||( iit->get_person_type() == Cperson_abstract::Person_companion))
+		{
+
+			//std::cout << "IN (3) !!! calculate_current_forces_companion2_for_person_companion_akp" << std::endl;
+			if(iit->get_id()==id_person_companion){
+				//std::cout << "IN (4) !!! calculate_current_forces_companion2_for_person_companion_akp" << std::endl;
+				//1 time step propagation for all people
+				virtual_force_goal = iit->force_goal(  iit->get_best_dest() , this->get_sfm_params(iit) );
+
+				//std::cout << "IN (5) !!! calculate_current_forces_companion2_for_person_companion_akp" << std::endl;
+				virtual_force_int_person = Cscene_abstract::force_persons_int( iit);
+				//std::cout << "IN (6) !!! calculate_current_forces_companion2_for_person_companion_akp" << std::endl;
+				// force depending on map to be deprecated in real environments, but still used on simulations
+				if( read_force_map_success_ )
+					virtual_force_obstacle = get_force_map( iit->get_current_pointV().x,
+							iit->get_current_pointV().y ) * 0.5;
+				if(read_laser_obstacle_success_)
+					virtual_force_obstacle = Cscene_abstract::force_objects_laser_int( iit  );
+				//robot stays idle, no prediction is done
+				//std::cout << "IN (7) !!! calculate_current_forces_companion2_for_person_companion_akp" << std::endl;
+				if ( robot_in_the_scene_ ) // de momento así... luego ya veré...
+				{
+					//std::cout << "IN (8) !!! calculate_current_forces_companion2_for_person_companion_akp" << std::endl;
+					switch( pr_force_mode )
+					{
+					case 0: //deterministic force, classical definition
+						virtual_force_robot = iit->force( robot_->get_current_pointV() , this->get_sfm_int_params(iit,robot_), &(iit->get_current_pointV()) );
+						break;
+					case 1: //probabilistic force, sampling around the elipsoid
+						virtual_force_robot = iit->force_sphe_prob( robot_->get_current_pointV(), get_sfm_int_params(iit,robot_),&(iit->get_current_pointV()) );
+						break;
+					case 2: //probabilistic force, mahalanobis distance
+						virtual_force_robot = iit->force_sphe_mahalanobis( robot_->get_current_pointV(), get_sfm_int_params(iit,robot_), &(iit->get_current_pointV()));
+						break;
+					case 3: //probabilistic force, worst case scenario: covaraince ellipsoid nearer to the center point
+						virtual_force_robot = iit->force_sphe_worst( robot_->get_current_pointV(), get_sfm_int_params(iit,robot_),&(iit->get_current_pointV()));
+						break;
+					}
+					//std::cout << "IN (9) !!! calculate_current_forces_companion2_for_person_companion_akp" << std::endl;
+				}
+				else
+					virtual_force_robot = Sforce();
+
+				//std::cout << "IN (10) !!! calculate_current_forces_companion2_for_person_companion_akp" << std::endl;
+				iit->set_forces_person( virtual_force_goal, virtual_force_int_person, virtual_force_robot, virtual_force_obstacle );
+				//std::cout << "IN (11) !!! calculate_current_forces_companion2_for_person_companion_akp" << std::endl;
+			}
+		}
+	}
+	//std::cout << "IN (12) !!! calculate_current_forces_companion2_for_person_companion_akp" << std::endl;
+	//robot update, if any
+	if ( robot_in_the_scene_ ) // este no es necesario, pq solo quiero los de la person companion.
+	{ // cuando llame a esta siempre quiero este update!
+	//	std::cout << "IN (13) !!! calculate_current_forces_companion2_for_person_companion_akp" << std::endl;
+		virtual_force_goal = person_companion_->force_goal(  person_companion_->get_best_dest() , this->get_sfm_params(robot_) ); //robot_->force_goal(  robot_->get_best_dest() , this->get_sfm_params(robot_) );
+		//std::cout << "IN (14) !!! calculate_current_forces_companion2_for_person_companion_akp" << std::endl;
+		bool person_companion_esquip_track_person_companion=true;
+		virtual_force_int_person = Cscene_abstract::force_persons_int( person_companion_, person_companion_esquip_track_person_companion ); // mirar a ver si hay que cambiar algo de esta función o hacer otra similar.
+		// force depending on map to be deprecated in real environments, but still used on simulations
+		//std::cout << "IN (15) !!! calculate_current_forces_companion2_for_person_companion_akp" << std::endl;
+		if( read_force_map_success_ )
+			virtual_force_obstacle = get_force_map( person_companion_->get_current_pointV().x,
+					person_companion_->get_current_pointV().y ) * 0.5;
+		//std::cout << "IN (16) !!! calculate_current_forces_companion2_for_person_companion_akp" << std::endl;
+		if(read_laser_obstacle_success_)
+			virtual_force_obstacle = Cscene_abstract::force_objects_laser_int( person_companion_  ); // mirar a ver si hay que cambiar algo de esta función o hacer otra similar.
+		virtual_force_robot = Sforce(); // a cabiar cuando tenga el robot en escena!
+		//std::cout << "IN (17) !!! calculate_current_forces_companion2_for_person_companion_akp" << std::endl;
+		person_companion_->set_forces_person( virtual_force_goal, virtual_force_int_person, virtual_force_robot, virtual_force_obstacle ); // hacer una equivalente para la person companion!
+		//std::cout << "IN (18) !!! calculate_current_forces_companion2_for_person_companion_akp" << std::endl;
+	}
+
+
+
+	//std::cout << "out (1) !!! calculate_current_forces_companion2_for_person_companion_akp" << std::endl;
+}
+
+
+/*** INI calculate_companion_angle_for_goal_companion! ***/
+void Cprediction_behavior::calculate_companion_angle_for_goal_companion( double distance_of_collision , double distance_between_robot_and_person_companion, bool case_dynamic)
+{
+	if(debug_angle_of_companion_obtained_from_prediction_){
+	 std::cout << "INI Function: Cprediction_behavior::calculate_companion_angle_for_goal_companion" << std::endl;
+	}
+	// TODO (ely): add the robot companion cost!!! in this node of the tree!!! (colision con robot, que parametro es, para evaluar el path)
+	    // OJO!!! si las acumulo como el, ya no podre calcular el coste real, haciendo una ventana hacia atras del coste. Quizas mejor acumular para cada path, hacer un vector de vectores.
+		// ojo!!! aquí solo acumula/calcula el coste de un path!!!
+
+
+		if(debug_angle_of_companion_obtained_from_prediction_){
+			std::cout << "robot_person_companion_distance_="<<distance_between_robot_and_person_companion << std::endl;
+		}
+
+
+		 std::cout << "INI Function: Cprediction_behavior::calculate_companion_angle_for_goal_companion: distance_between_robot_and_person_companion="<<distance_between_robot_and_person_companion << std::endl;
+
+
+
+		// ver si hay colision en la circunferencia de radio ROBOT+Persona. Si hay algun obstaculo estatico o persona, prediccion dentro de esa circunferencia.
+		// (check_collision( const Spoint& p=robot_creo ,  int t)) => la tendré que modificar para que devuelva la distancia de colision, tambien!
+		// la check_collision ya tiene los bucles siguientes que hacian falta.
+		// collision check for static objects    (si hay alguno dentro de esa circuferencia => se cambia el bool a true y se guarda esa distancia en el vector, min_distances.
+		// collision check for dynamic persons
+
+		double final_min_colision_distance;
+
+		if(distance_of_collision!=100){
+			final_min_colision_distance=distance_of_collision;
+			min_distance_collision_vector_from_prediction_.push_back(distance_of_collision);
+			//std::cout << " Si colision !!!!!! ; final_min_colision_distance="<<final_min_colision_distance<< std::endl;
+		}else{
+			final_min_colision_distance=distance_between_robot_and_person_companion;
+			min_distance_collision_vector_from_prediction_.push_back(distance_between_robot_and_person_companion);
+			//std::cout << " NO colision !!!!!! ; final_min_colision_distance="<<final_min_colision_distance<< std::endl;
+		}
+
+		if(debug_angle_of_companion_obtained_from_prediction_){
+			std::cout << "final_min_colision_distance="<<final_min_colision_distance<<"; distance_between_robot_and_person_companion="<<distance_between_robot_and_person_companion<<"; threshold_distance_collision_calculate_companion_angle_="<<threshold_distance_collision_calculate_companion_angle_<< std::endl;
+		}
+
+		// calculate the angle between robot and person.
+		double angle=angle_companion_pred_; // in grad.
+
+		double robot_radi=robot_->get_platform_radii();
+		double real_robot_person_distance2=distance_between_robot_and_person_companion;
+		/*if(bool_case_person_companion_){
+			real_robot_person_distance2=calc_robot_person_companion_distance_companion_person_akp();
+		}else{
+			real_robot_person_distance2=calc_robot_person_companion_distance();
+		}*/
+
+
+
+		if((final_min_colision_distance==distance_between_robot_and_person_companion)||(final_min_colision_distance>distance_between_robot_and_person_companion)){
+		//if((final_min_colision_distance==(2*((real_robot_person_distance/2)+2*robot_radi)))||(final_min_colision_distance>(2*((real_robot_person_distance/2)+2*robot_radi)))){
+		//if((final_min_colision_distance==(real_robot_person_distance+4*robot_radi+little_augmented_collision_margin_))||(final_min_colision_distance>(real_robot_person_distance+4*robot_radi+little_augmented_collision_margin_))){
+			angle=angle_companion_pred_;
+
+			//std::cout << " entro en if(...) => angle=angle_companion_"<< std::endl;
+
+		}else{
+			//angle=(180/3.14)*(asin((final_min_colision_distance/2)/(robot_person_companion_distance_/2))); //asin((final_min_colision_distance/2)/((2*radio_robot)/2)) ; 2*radio_robot= robot_person_companion_distance_;
+			//angle=(180/3.14)*(asin((final_min_colision_distance)/(robot_person_companion_distance_)));
+			if(overpas_obstacles_behind_person_pred_){
+				//orientation_person_robot_angles_.push_back(180-angle);
+				//angle=(180/3.14)*(asin((final_min_colision_distance)/(robot_person_companion_distance_))); //prueba 1.
+				//double division=(2*final_min_colision_distance)/(robot_person_proximity_distance_+2*robot_radi+little_augmented_collision_margin_);
+				double division;
+				//double division2;
+				double radi_force_influence=0.91; // la A! para las personas. Que no influya la fuerza de las personas.
+				if(case_dynamic){
+					//std::cout << " entro an case dynamic"<< std::endl;
+					division=(final_min_colision_distance-robot_radi-radi_force_influence)/(real_robot_person_distance2/2); //revisar el caso dynamico!
+
+				}else{
+					//std::cout << " entro an case NO dynamic"<< std::endl;
+					// en formulacion => dw/Ri => dw=d_real_collision/2-Rr-free_space_margin/2
+					// Ri = (d_real_between_r_and_p +2Rr+Free_space_margin)/2 =>free_space_margin==little_augmented_collision_margin_
+					division=((final_min_colision_distance/2)-robot_radi-(little_augmented_collision_margin_pred_/2))/((real_robot_person_distance2+2*robot_radi+little_augmented_collision_margin_pred_)/2);
+					//division=(final_min_colision_distance)/(real_robot_person_distance2+2*robot_radi+little_augmented_collision_margin_);
+					//division=(final_min_colision_distance)/((real_robot_person_distance2/2)+robot_radi+(little_augmented_collision_margin_/2));
+					//division=(final_min_colision_distance)/((real_robot_person_distance2/2)+robot_radi+(1/2));
+					//division=(final_min_colision_distance-robot_radi)/(real_robot_person_distance2/2);
+					//division2=((final_min_colision_distance/2)-robot_radi-(little_augmented_collision_margin_pred_/2))/((ideal_distance_of_companion_between_person_and_robot_+2*robot_radi+little_augmented_collision_margin_pred_)/2);
+					//std::cout << " final_min_colision_distance="<<final_min_colision_distance<<"; robot_radi="<<robot_radi<<"real_robot_person_distance2="<<real_robot_person_distance2<<" little_augmented_collision_margin_="<<little_augmented_collision_margin_<< std::endl;
+				}
+
+				//std::cout << " entro en else (...) => division="<<division<<"; case_dynamic_="<<case_dynamic_<< std::endl;
+
+				if(division>1){
+					division=1;
+				}else if(division<(-1)){
+					division=-1;
+				}
+
+				//std::cout << " entro en case overpas_obstacles_behind_person_pred_ => division="<<division<<" division2="<<division2<< std::endl;
+
+				angle=(180/3.14)*(asin(division)); //prueba 1.
+
+				//double angle2 =(180/3.14)*(asin(division2));
+				//std::cout << "(1) angle="<<angle<<"; angle2="<<angle2<< std::endl;
+
+				if(angle>angle_companion_pred_){
+					//std::cout << "!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! OJO!!!! ENTRO EN if(angle>angle_companion_)"<< std::endl;
+					//std::cout << " (if angle>angle_companion_   Caso angle=angle_companion_) angle="<<angle<<"angle_companion_="<<angle_companion_<< std::endl;
+					angle=angle_companion_pred_;
+				}
+				//std::cout << "(2) angle="<<angle<< std::endl;
+
+				//std::cout << "parent_index="<<parent_index<< std::endl;
+				//std::cout << "colision="<<colision<< std::endl;
+				//std::cout << " [PASAR POR DETRAS] !!!! (ant) (test, colision) angle="<<angle<<" final_min_colision_distance="<<final_min_colision_distance<<"; robot_person_proximity_distance_="<<robot_person_proximity_distance_<< std::endl;
+				//std::cout << " [PASAR POR DETRAS] !!!! (ant) (test, colision) angle="<<angle<<" final_min_colision_distance/2="<<final_min_colision_distance/2<<"; (robot_person_proximity_distance_/2)+2*robot_radi)="<<(robot_person_proximity_distance_/2)+2*robot_radi<< std::endl;
+
+				angle=180-angle;
+				//std::cout << "(3) angle="<<angle<< std::endl;
+
+				//std::cout << " angle2="<<angle<< std::endl;
+
+				if(angle<angle_companion_pred_){
+					angle=angle_companion_pred_;
+					//std::cout << "angle<angle_companion_=> angle=angle_companion"<< std::endl;
+				}
+				//std::cout << "(4) angle="<<angle<< std::endl;
+
+				if(angle>180){
+					angle=180;
+					//std::cout << "angle>180=> angle=180"<< std::endl;
+				}
+				//std::cout << "(5) angle="<<angle<< std::endl;
+
+				//std::cout << "[overpas_obstacles_behind_person_] angle="<<angle<<"angle_companion_="<<angle_companion_<< std::endl;
+				//std::cout << " [PASAR POR DETRAS] !!!! (despues) (test, colision) angle="<<angle<< std::endl;
+				// TODO: temporal!!! el angulo solo va de 0 a angle_companion_=90grados, para colision!!!
+				/*if(angle>angle_companion_){
+					angle=angle_companion_;
+				}*/
+			}else{
+				//std::cout << " overpas delante persona "<< std::endl;
+				//angle=(180/3.14)*(asin((final_min_colision_distance)/(robot_person_companion_distance_))); //prueba 1.
+				//std::cout << " angle="<<angle <<"; final_min_colision_distance="<<final_min_colision_distance<<"; robot_person_companion_distance_"<<robot_person_companion_distance_<< std::endl;
+				//double division=(2*final_min_colision_distance)/(robot_person_proximity_distance_+2*robot_radi+little_augmented_collision_margin_);
+				double division;
+				double radi_force_influence=0.91; // la A!
+				if(case_dynamic){
+					//std::cout << " entro an case dynamic"<< std::endl;
+					division=(final_min_colision_distance-robot_radi-radi_force_influence)/(real_robot_person_distance2/2);
+				}else{
+					//std::cout << " entro an case NO dynamic"<< std::endl;
+					division=(final_min_colision_distance-robot_radi)/(real_robot_person_distance2/2);
+				}
+				if(division>1){
+					division=1;
+				}else if(division<(-1)){
+					division=-1;
+				}
+				angle=(180/3.14)*(asin(division)); //prueba 1.
+				// TODO: temporal!!! el angulo solo va de 0 a angle_companion_=90grados, para colision!!!
+				if(angle>angle_companion_pred_){
+					angle=angle_companion_pred_;
+					//std::cout << " (angle>angle_companion_) angle="<<angle<< std::endl;
+				}
+			}
+		}
+		if(debug_angle_of_companion_obtained_from_prediction_){ // SI se choca con obstaculos habrá que arreglar esto!!!
+			std::cout << " (test, colision) angle="<<angle<< std::endl;
+			std::cout << " final_min_colision_distance="<<final_min_colision_distance<< std::endl;
+		}
+
+		// teniendo la distancia, la he de transformar en un angulo, sabiendo la robot_person_companion_distance_ (distancia maxima)
+	//	double act_companion_cost; // actual companion cost.
+		//act_companion_cost=(0.00000015242)*(pow(angle-angle_companion_pred_,4)); //act_companion_cost=(0.000000015242)*(pow(angle-angle_companion_,4));
+		//std::cout << "act_companion_cost="<<act_companion_cost<< std::endl;
+
+		// calculas si hay incremento de angulo, si lo hay, calculas el coste hacia atras y los angulos hacia atras en el path, para que no haya saltos en la orientación.
+		/*if(angle!=angle_companion_){
+			//std::cout << "calculate_companion_addition_cost_node_of_big_angle_difference"<< std::endl;
+			//aditional cost due tu angle difference between actual angle and parent_vertex_angle
+			double additional_cost=calculate_companion_addition_cost_node_of_big_angle_difference(parent_index, angle);
+			act_companion_cost=act_companion_cost+additional_cost;
+			//calculate_companion_path_angle_and_cost(parent_index, angle);
+		}*/
+		//std::cout << "n"<< std::endl;
+		//colision=false; // TODO: quitar cuando vaya lo del robot...
+		//act_companion_cost=0; // TODO: quitar cuando vaya lo del robot...
+				//angle=90; // TODO: quitar cuando vaya lo del robot...
+
+		//cost_companion_.push_back( cost_companion_.at(parent_index) + act_companion_cost );
+
+		//std::cout << "(antes push_back()) angle="<<angle<< std::endl;
+
+		orientation_person_robot_angles_pred_.push_back(angle);
+
+		//parent_index_vector_.push_back(parent_index);
+
+		/*if(colision){
+			std::cout << "(calculate_companion_cost_node) => angle="<<angle<< std::endl;
+			std::cout << "(calculate_companion_cost_node) => min_distance_collision_="<<min_distance_collision_<< std::endl;
+
+		}*/
+
+
+		if(debug_angle_of_companion_obtained_from_prediction_){
+		 std::cout << "FIN Function: calculate_companion_cost_node" << std::endl;
+		}
+}
+/*** FIN calculate_companion_cost_node! ***/
+
diff --git a/local_lib_people_prediction/src/prediction_behavior.h b/local_lib_people_prediction/src/prediction_behavior.h
new file mode 100644
index 0000000000000000000000000000000000000000..f9e4846f10cedd2deca1f2435082d5f24a7f5bf4
--- /dev/null
+++ b/local_lib_people_prediction/src/prediction_behavior.h
@@ -0,0 +1,201 @@
+/*
+ * prediction_behavior.h
+ *
+ *  Created on: 2013.  Last Modified on 2020
+ *      Author: Initial code of the AKP Gonzalo Ferrer.
+ *      Author: Addition of ASSAOP and ASSAGP for people accompaniment code of Ely Repiso.
+ */
+
+#ifndef prediction_behavior_H
+#define prediction_behavior_H
+
+#include "prediction_bhmip.h"
+#include "scene_elements/person_abstract.h"
+#include "iri_geometry.h"
+#include <vector>
+#include <list>
+
+
+/**
+ *
+ * \brief Prediction_behavior Class
+ *
+ * The prediction class provides a set of algorithms
+ * estimating the behavior of persons interacting in
+ * the scene as well as, and since it inherits from prediction_bhmip,
+ * solving intentionality prediction and long term prediction.
+ * A set of destinations is assumed to be provided
+ *
+ *
+*/
+class Cprediction_behavior: public Cprediction_bhmip
+{
+	public:
+		Cprediction_behavior ( double horizon_time = 0.5, bool behavior_flag = false,
+				Cperson_abstract::filtering_method filter= Cperson_abstract::No_filtering,
+				Cperson_abstract::update_person_method update_method = Cperson_abstract::Autoremove);
+		virtual ~Cprediction_behavior();
+		virtual Cperson_abstract* add_person_container( unsigned int id,
+				std::list<Cperson_abstract*>::iterator iit );
+		/**
+		 * \brief this function updates the behavior estimation and calculates a predicted trajectory
+		 * function
+		 */
+		void scene_prediction(bool person_or_robot=false, std::vector<Sdestination>* person_best_dest=NULL);
+		virtual void clear_scene();
+	    void set_horizon_time( double t );
+	    double get_horizon_time(  ) { return horizon_time_;}
+	    void set_behavior_flag( bool flag ) { behavior_flag_ = flag;}
+	    bool get_behavior_flag( ) {return behavior_flag_;}
+	    const std::vector<double>* get_behavior_sfm_params( Cperson_abstract::behavior_type best_behavior_type );
+		virtual const std::vector<double>* get_sfm_int_params( const Cperson_abstract * center_person,
+				const Cperson_abstract * interacting_person = NULL);
+
+		// calculate current forces, for visualization purposes
+		void calculate_current_forces( int pr_force_mode = 0 , Sforce companion_force_Zanlungo=Sforce(),Sforce goal=Sforce(), Sforce persons=Sforce(), Sforce obstacles=Sforce(),double alpa_comp=1.0);
+
+
+		void calculate_current_forces_companion( int pr_force_mode , unsigned int id_person_companion, bool person_reactive,  Cperson_abstract::companion_reactive type_rec_akp, double act_min_companion_angle);
+		void calculate_current_forces_companion2( int pr_force_mode , unsigned int id_person_companion, bool person_reactive, Cperson_abstract::companion_reactive type_rec_akp, double act_min_companion_angle, Sforce goal, Sforce persons, Sforce obstacles, Sforce companion_person_goal, double &other_people_cost_due_to_robot, double &companion_person_cost_due_to_robot);
+
+
+		void calculate_current_forces_companion2_for_person_companion_akp( int pr_force_mode , unsigned int id_person_companion, bool person_reactive, Cperson_abstract::companion_reactive type_rec_akp, double act_min_companion_angle, Sforce goal, Sforce persons, Sforce obstacles, Sforce companion_person_goal, double &other_people_cost_due_to_robot, double &companion_person_cost_due_to_robot);
+
+		void Cprediction_behaviour_set_max_d_to_detect_laser_obs(double in_max_d_to_detect_laser_obs){
+			Cprediction_bhmip_set_max_d_to_detect_laser_obs(in_max_d_to_detect_laser_obs);
+			std::cout << "set Cprediction_behaviour_set_max_d_to_detect_laser_obs-> in_max_d_to_detect_laser_obs="<<in_max_d_to_detect_laser_obs<< std::endl;
+		}
+
+		void set_Cprediction_behavior_id_person_companion(unsigned int in_id_person_companion){
+			id_person_companion_=in_id_person_companion;
+		}
+		void set_Cprediction_behavior_id_second_person_companion(unsigned int in_id_person_companion){
+			id_second_person_companion_=in_id_person_companion;
+		}
+
+		unsigned int my_id_person_companion_; // id, actual person companion. the id of it selft
+		void set_Cprediction_behavior_my_id_second_person_companion_sim(unsigned int in_my_id_person_companion){
+			my_id_person_companion_=in_my_id_person_companion;
+		}
+
+		// todo: borrar era de aproach person
+		/*void set_distance_person_robot_f(double in_set_distance_person_robot){
+			set_distance_person_robot_=in_set_distance_person_robot;
+		}*/
+
+		void set_sim_prediction_behaviour(bool in_sim_prediction_behaviour){
+			sim_prediction_behaviour=in_sim_prediction_behaviour;
+		}
+
+		// functions to obtain the angle of companion (for the companion goal) from the prediction of the companion person
+		void calculate_companion_angle_for_goal_companion( double distance_of_collision , double distance_between_robot_and_person_companion, bool case_dynamic);
+
+		void set_all_values_to_calc_the_companion_angle_with_person_prediction(double in_threshold_distance_collision_calculate_companion_angle,double in_angle_companion_pred,double in_real_distance_of_companion_between_person_and_robot,double in_ideal_distance_of_companion_between_person_and_robot,bool in_overpas_obstacles_behind_person_pred,double in_little_augmented_collision_margin_pred){
+			set_threshold_distance_collision_calculate_companion_angle(in_threshold_distance_collision_calculate_companion_angle);
+			set_angle_companion_pred(in_angle_companion_pred);
+			set_real_distance_of_companion_between_person_and_robot(in_real_distance_of_companion_between_person_and_robot);
+			set_ideal_distance_of_companion_between_person_and_robot(in_ideal_distance_of_companion_between_person_and_robot);
+			set_overpas_obstacles_behind_person_pred(in_overpas_obstacles_behind_person_pred);
+			set_little_augmented_collision_margin_pred(in_little_augmented_collision_margin_pred);
+		}
+
+		void set_threshold_distance_collision_calculate_companion_angle(double in_threshold_distance_collision_calculate_companion_angle){
+			threshold_distance_collision_calculate_companion_angle_=in_threshold_distance_collision_calculate_companion_angle;
+		}
+
+		void set_angle_companion_pred(double in_angle_companion_pred){
+			angle_companion_pred_=in_angle_companion_pred;
+		}
+
+		void set_real_distance_of_companion_between_person_and_robot(double in_real_distance_of_companion_between_person_and_robot){
+			real_distance_of_companion_between_person_and_robot_=in_real_distance_of_companion_between_person_and_robot;
+		}
+		void set_ideal_distance_of_companion_between_person_and_robot(double in_ideal_distance_of_companion_between_person_and_robot){
+			ideal_distance_of_companion_between_person_and_robot_=in_ideal_distance_of_companion_between_person_and_robot;
+		}
+
+		void set_overpas_obstacles_behind_person_pred(bool in_overpas_obstacles_behind_person_pred){
+			overpas_obstacles_behind_person_pred_=in_overpas_obstacles_behind_person_pred;
+		}
+
+		void set_little_augmented_collision_margin_pred(double in_little_augmented_collision_margin_pred){
+			little_augmented_collision_margin_pred_=in_little_augmented_collision_margin_pred;
+		}
+
+		std::vector<double> get_orientation_person_robot_angles_pred(){
+			return orientation_person_robot_angles_pred_;
+		}
+
+		void set_initial_angle_in_orientation_person_robot_angles_pred(double initial_angle){
+			orientation_person_robot_angles_pred_.clear();
+			orientation_person_robot_angles_pred_.push_back(initial_angle);
+		}
+
+		std::vector<double> get_min_distance_collision_vector_from_prediction(){
+			return min_distance_collision_vector_from_prediction_;
+		}
+
+		void set_calc_goal_companion_with_group_path(bool in_calc_goal_companion_with_group_path_pred){
+			calc_goal_companion_with_group_path_pred_=in_calc_goal_companion_with_group_path_pred;
+		}
+
+
+		// a eliminar es de approach person
+		void set_num_max_iter_constan_time_steps_pred(double in_num_max_iter_constan_time_steps_pred){
+			num_max_iter_constan_time_steps_pred_=in_num_max_iter_constan_time_steps_pred;
+		}
+
+
+		void change_time_window_for_filter_velocity(double in_new_time_window){
+			Cprediction_bhmip::set_filtering_time_window(in_new_time_window);
+		}
+
+	protected:
+		void scene_behavior_estimation();
+		void scene_trajectory_prediction();
+		void scene_trajectory_prediction_simulation();
+		//calculation of forces for persons that do not exists, just propagated points
+		Sforce force_persons_int_virtual( Cperson_abstract* center, unsigned long n=0);
+		Sforce force_persons_int_virtual_person_comp(Cperson_abstract* center , unsigned long t=0);
+		Sforce force_objects_laser_int_prediction( Cperson_abstract* person, unsigned int planning_index=0);
+		void behavior_update( Cperson_abstract* observed_person, Cperson_abstract* center_person, const Sforce& observed_int);
+		double horizon_time_;//max time of prediction
+		unsigned int horizon_time_index_;
+		bool behavior_flag_;
+		std::vector<double> force_params_balanced_, force_params_aware_ , force_params_unaware_;
+
+		// akp companion (ely)
+		bool debug_antes_subgoals_entre_AKP_goals_;
+		bool debug_scene_prediction_;
+		bool debug_scene_trajectory_prediction_simulation_;
+
+		unsigned int id_person_companion_, id_second_person_companion_;
+
+		//double set_distance_person_robot_; // todo: borrar era de aproach person
+
+		//bool not_calculate_increment_time_case_face_person_;
+		bool sim_prediction_behaviour;
+
+		//bool group_go_to_interact_with_other_person_;
+		bool no_increment_prediction_;
+
+		// variables to obtain the angle of companion (for the companion goal) from the prediction of the companion person
+		double distance_to_the_near_obsacle_; // smallest distance between the companion person and obstacles at this instant of time to calculate the companion angle.
+		// If distance_to_the_near_obsacle_==100 we do not have collisions!
+		double distance_to_the_near_person_; // smallest distance between the companion person and the other people at this instant of time to calculate the companion angle.
+				// If distance_to_the_near_obsacle_==100 we do not have collisions!
+		double threshold_distance_collision_calculate_companion_angle_; // Ri actual radius to calculate the companion angle.
+		bool debug_angle_of_companion_obtained_from_prediction_;
+		double angle_companion_pred_;
+		double real_distance_of_companion_between_person_and_robot_;
+		double ideal_distance_of_companion_between_person_and_robot_;
+		bool overpas_obstacles_behind_person_pred_;
+		double little_augmented_collision_margin_pred_;
+
+		std::vector<double> min_distance_collision_vector_from_prediction_;
+		std::vector<double> orientation_person_robot_angles_pred_;
+		bool calc_goal_companion_with_group_path_pred_;
+
+		double num_max_iter_constan_time_steps_pred_; // todo: a eliminar es de approach person
+};
+#endif
diff --git a/local_lib_people_prediction/src/prediction_bhmip.cpp b/local_lib_people_prediction/src/prediction_bhmip.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f4a90f93c2b3be33c1967620a585b2ac8cf680da
--- /dev/null
+++ b/local_lib_people_prediction/src/prediction_bhmip.cpp
@@ -0,0 +1,415 @@
+/*
+ * prediction_bhmip.cpp
+ *
+ *  Created on: 2013.  Last Modified on 2020
+ *      Author: Initial code of the AKP Gonzalo Ferrer.
+ *      Author: Addition of ASSAOP and ASSAGP for people accompaniment code of Ely Repiso.
+ */
+
+
+#include "prediction_bhmip.h"
+#include "scene_elements/person_bhmip.h"
+#include <math.h>
+#include <algorithm>
+#include <stdio.h>
+#include <Eigen/Dense>
+#include <iostream>
+
+
+
+Cprediction_bhmip::Cprediction_bhmip( Cperson_abstract::filtering_method filter,
+		Cperson_abstract::update_person_method update_method ) :
+		Cscene_abstract(filter, update_method, Cperson_abstract::Spherical),
+		id_person_companion_(1),
+		id_second_person_companion_(2),
+		debug_update_scene_(false)
+{
+	//read destinations from txt
+	//this->set_destinations(dest);
+}
+
+Cprediction_bhmip::~Cprediction_bhmip()
+{
+	for( std::list<Cperson_abstract*>::iterator iit = person_list_.begin() ; iit != person_list_.end() ; iit++ )
+	{
+		delete (*iit);
+	}
+	delete robot_;
+}
+
+void Cprediction_bhmip::see_tracks_on_scene()
+{
+
+	std::cout << " see_tracks_on_scene() "<< std::endl;
+
+	//Cperson_abstract* found_person;
+	SdetectionObservation  point;
+
+	for(std::list<Cperson_abstract*>::iterator iit = person_list_.begin() ; iit!=person_list_.end() ; iit++)
+	{
+		SpointV_cov point_person=(*iit)->get_current_pointV();
+		std::cout << "point_person.x="<< point_person.x<<"; point_person.y="<<point_person.y<< std::endl;
+	}
+}
+
+
+void Cprediction_bhmip::update_scene(const std::vector<SdetectionObservation>& observation,bool& we_have_person_companion, bool person_or_robot)
+{
+	// the filter bool sets if we want to filter velocities or use the provided in the update.
+	//associate current targets with tracking observations
+	Cperson_abstract* found_person;
+	SdetectionObservation  point;
+
+
+	/*for( Cperson_abstract* iit: person_list_ )
+				{
+					if ( iit->get_person_type() == Cperson_abstract::Person)
+					{
+
+							if(iit->get_id()==2){
+								std::cout << "INI (update_scene) FIND CHANGE TARGET PERSON: id="<< iit->get_id()<< std::endl;
+								iit->get_current_pointV().print();
+							}
+
+					}
+				}*/
+
+
+	if(debug_update_scene_){
+		std::cout << "!!!!!!!!!!!!! ENTRO EN Cprediction_bhmip::update_scene id_person_companion_="<<id_person_companion_<<"; observation.empty()"<<observation.empty() <<"; observation.size()="<<observation.size()<< std::endl;
+		//std::cout << "observation.size()=" << observation.size() << "; person_or_robot="<<person_or_robot<< std::endl;
+	}
+	we_have_person_companion=false;
+
+
+	unsigned int Uint_id_person_act=(unsigned int) id_person_companion_;
+
+	//if(debug_filter_person_velocity_){
+	//	std::cout << " PPPPPPPPP (UPDATE SCENE) filtering_method_ = " << filtering_method_ << std::endl;
+	// std::cout << " Cprediction_bhmip (1) filtering_time_window_= "<<filtering_time_window_<< std::endl;
+	//}
+	if(debug_update_scene_){
+		std::cout << "filtering_method_"<<filtering_method_ << std::endl;
+	}
+
+	//std::cout << " Cprediction_bhmip (1) = ; filtering_time_window_="<<filtering_time_window_<< std::endl;
+
+	for(unsigned int i=0 ; i< observation.size() ; ++i)
+	{
+
+		//SpointV actual_obs( observation[i].x , observation[i].y, observation[i].time_stamp, observation[i].vx, observation[i].vy );
+		//if(robot_->get_current_pointV().distance(actual_obs)<10)
+		//{
+			////////////////////
+
+
+			if(debug_update_scene_){
+				std::cout << "iteration" << i << std::endl;
+				std::cout << "track.id= observation[i].id" << observation[i].id << std::endl;
+				std::cout << " observation[i].time_stamp=" << observation[i].time_stamp << std::endl;
+			}
+
+			if( find_person( observation[i].id , &found_person ) )
+			{
+				if(debug_update_scene_){
+					std::cout << "1 (if find person) updating target after observation " << found_person->is_observation_updated() << std::endl;
+				}
+
+				point = observation[i];
+
+				if(debug_update_scene_){
+					std::cout << "point_observation" << std::endl;
+					observation[i].print();
+				}
+
+				/*if(!person_or_robot){
+
+					if(debug_update_scene_){
+						std::cout << "if(!person_or_robot); person_or_robot=" << person_or_robot << std::endl;
+					}*/
+					//std::cout << "(if) before found_person->add_pointV person_ID="<<observation[i].id <<"; filtering_time_window_="<<filtering_time_window_<< std::endl;
+					found_person->add_pointV( point, filtering_method_,person_or_robot,filtering_time_window_);
+					//std::cout << "(if) after found_person->add_pointV" << std::endl;
+				/*}else{
+
+					if(debug_update_scene_){
+						std::cout << "else_if(!person_or_robot); person_or_robot=" << person_or_robot << std::endl;
+					}
+
+					found_person->Cperson_bhmip_add_pointV( point, filtering_method_,  person_or_robot);
+
+					if(debug_update_scene_){
+						std::cout << "(after found_person->Cperson_bhmip_add_pointV)found_person->get_prediction_trajectory()->size()=" << found_person->get_prediction_trajectory()->size() << std::endl;
+					}
+				}*/
+				now_ = observation[i].time_stamp;
+
+				if(debug_update_scene_){
+					std::cout << "2 (if find person) updating target after observation " << found_person->is_observation_updated() << std::endl;
+				}
+
+				if(((unsigned int) observation[i].id)==id_person_companion_){
+					we_have_person_companion=true;
+					//std::cout << "encuentro persona companion! " << std::endl;
+				}
+			}
+			//new targets
+			else
+			{
+				if(debug_update_scene_){
+					std::cout << "Cprediction_bhmip 1 (else add new person) updating target after observation " << std::endl;
+				}
+
+				found_person = add_person( observation[i].id );
+
+				if(debug_update_scene_){
+					std::cout << "new person :" << found_person->get_id() << std::endl;
+				}
+
+				point = observation[i];
+				/*if(!person_or_robot){
+
+					if(debug_update_scene_){
+						std::cout << "if(!person_or_robot); person_or_robot=" << person_or_robot << std::endl;
+					}*/
+				//std::cout << "Cprediction_bhmip 2 (else add new person) updating target after observation " << std::endl;
+
+				if(debug_update_scene_){
+					point.print();
+					std::cout << "filtering_method_="<<filtering_method_ << std::endl;
+					std::cout << "person_or_robot="<<person_or_robot << std::endl;
+					std::cout << "point.id="<<point.id<<"; point.x="<<point.x<<"; point.y="<<point.y << std::endl;
+				}
+				//std::cout << "(else) before found_person->add_pointV person_ID="<<observation[i].id << std::endl;
+				found_person->add_pointV( point , filtering_method_,person_or_robot, filtering_time_window_);
+				//std::cout << "(else) after found_person->add_pointV " << std::endl;
+				/*}else{
+
+					if(debug_update_scene_){
+						std::cout << "else_if(!person_or_robot); person_or_robot=" << person_or_robot << std::endl;
+					}
+
+					found_person->Cperson_bhmip_add_pointV( point, filtering_method_,person_or_robot);
+					if(debug_update_scene_){
+						std::cout << "(after found_person->Cperson_bhmip_add_pointV)found_person->get_prediction_trajectory()->size()=" << found_person->get_prediction_trajectory()->size() << std::endl;
+					}
+				}*/
+				now_ = observation[i].time_stamp;
+				//std::cout << "Cprediction_bhmip 3 (else add new person) updating target after observation " << std::endl;
+				if(debug_update_scene_){
+					std::cout << "2 (if add new person) updating target after observation " << found_person->is_observation_updated() << std::endl;
+				}
+
+				if(((unsigned int) observation[i].id)==id_person_companion_){
+					we_have_person_companion=true;
+					//std::cout << "creo persona! " << std::endl;
+				}
+			}
+
+
+
+			///////////////////////////////
+		//}
+
+	}
+
+	//std::cout << "Cprediction_bhmip 1 antes remove persons " << std::endl;
+
+	//Remove persons
+	if(debug_update_scene_){
+		std::cout << "Remove persons " << std::endl;
+	}
+	std::vector<unsigned int> removed_targets;
+	for(std::list<Cperson_abstract*>::iterator iit = person_list_.begin() ; iit!=person_list_.end() ; iit++)
+	{
+		if(debug_update_scene_){
+			std::cout << "analyzing target " << (*iit)->is_observation_updated() << std::endl;
+		}
+
+		if ( (*iit)->is_observation_updated() )
+		{
+			(*iit)->set_observation_update( false );//resets the observation flag until next update
+
+			if(debug_update_scene_){
+				std::cout << " updating target " << (*iit)->is_observation_updated() << std::endl;
+			}
+			// found person companion.
+			if((*iit)->get_id()==Uint_id_person_act){
+				we_have_person_companion=true;
+
+				if(debug_update_scene_){
+					std::cout << "(companion) entro en person to be updated! " << std::endl;
+				}
+			}
+
+		}
+		else
+		{
+			removed_targets.push_back((*iit)->get_id());
+
+			if(debug_update_scene_){
+				std::cout << "trying to remove target " << (*iit)->get_id() << std::endl;
+			}
+			// person companion TO BE removed.
+			if((*iit)->get_id()==Uint_id_person_act){
+				we_have_person_companion=false;
+
+				if(debug_update_scene_){
+					std::cout << "(companion) entro en person to be removed! " << std::endl;
+				}
+			}
+		}
+
+
+
+		if(debug_update_scene_){
+			// comprobar que este la persona, es solo para ver cosas, no del metodo. (by ely)
+			find_person( (*iit)->get_id(), &found_person );
+			std::cout << "(in Remove persons)found_person->get_prediction_trajectory()->size()=" << found_person->get_prediction_trajectory()->size() << std::endl;
+		}
+
+	}
+
+	//std::cout << "Cprediction_bhmip 1 en MEDIO remove persons " << std::endl;
+
+
+	for ( unsigned int i = 0; i < removed_targets.size() ; ++i)
+	{
+		if( update_method_ == Cperson_abstract::Autoremove )
+		{
+			if(debug_update_scene_){
+				std::cout << "entro en autoremove! " << std::endl;
+			}
+			remove_person(removed_targets[i]);
+			if(removed_targets[i]==Uint_id_person_act){
+				we_have_person_companion=false;
+
+				if(debug_update_scene_){
+					std::cout << "remove_person_companion! " << std::endl;
+				}
+			}
+		}
+		else
+		{
+			if(debug_update_scene_){
+				std::cout << "NO entro en autoremove! " << std::endl;
+			}
+			if( find_person(removed_targets[i], &found_person ) ){
+				found_person->refresh_person(now_);
+				we_have_person_companion=true;
+
+				if(debug_update_scene_){
+					std::cout << "NO remove_person_companion! " << std::endl;
+				}
+			}
+		}
+	}
+
+	// ONLY FOR TEST
+	if(debug_update_scene_){
+		std::cout << " (ENTRO EN Cprediction_bhmip::update_scene id_person_companion_) Print_list_or_persons " << std::endl;
+	}
+//	std::cout << "Cprediction_bhmip 1 antes only for test " << std::endl;
+
+	for(std::list<Cperson_abstract*>::iterator iit = person_list_.begin() ; iit!=person_list_.end() ; iit++)
+	{
+		if(debug_update_scene_){
+			(*iit)->print();
+		}
+		find_person( (*iit)->get_id(), &found_person );
+
+		if(debug_update_scene_){
+			std::cout << "(final list)found_person->get_prediction_trajectory()->size()=" << found_person->get_prediction_trajectory()->size() << std::endl;
+		}
+	}
+
+	if(debug_update_scene_){
+		std::cout << "(SALGO DE Cprediction_bhmip::update_scene id_person_companion_)=> person_list_.empty()" << person_list_.empty()<<"; id_person_companion_="<<id_person_companion_ << std::endl;
+	}
+	//else remove MUST be set manually outside the update function, each time the user wants to remove a person
+
+
+	/*for( Cperson_abstract* iit: person_list_ )
+				{
+					if ( iit->get_person_type() == Cperson_abstract::Person)
+					{
+
+							if(iit->get_id()==2){
+								std::cout << "FIN (update_scene) FIND CHANGE TARGET PERSON: id="<< iit->get_id()<< std::endl;
+								iit->get_current_pointV().print();
+							}
+
+					}
+				}*/
+
+
+
+
+}
+
+void Cprediction_bhmip::rotate_and_traslate_scene(unsigned int id, double R, double thetaZ, double linear_vx, double linear_vy, double v_rot_x, double v_rot_y, std::vector<double> vect_odom_eigen_tf, bool debug)
+{ 	// Function for local tracking. This function change the frame of the Current_pose and the window of poses for a concrete person (id), tacking into account the actual position of the robot.
+	//  id ->person id ; R -> Robot Translation ; thetaZ -> Robot rotation
+
+	Cperson_abstract* found_person;
+	if( find_person( id , &found_person ) )
+	{
+		//std::cout<<"(1) (people prediction internal person:" << std::endl;
+		//found_person->print();
+		found_person->rotate_and_translate_trajectory(R,thetaZ,linear_vx,linear_vy,v_rot_x,v_rot_y, vect_odom_eigen_tf,debug);
+		//std::cout<<"(2) (people prediction internal person:" << std::endl;
+		//found_person->print();
+	}
+}
+
+Cperson_abstract* Cprediction_bhmip::add_person_container( unsigned int id,
+		std::list<Cperson_abstract*>::iterator iit )
+{
+	Cperson_bhmip* person;
+	if((id_person_companion_==id)||(id_second_person_companion_==id)){
+		person = new Cperson_bhmip(id,Cperson_abstract::Person_companion, scene_force_type_, filtering_time_window_);
+	}else{
+		person = new Cperson_bhmip(id,Cperson_abstract::Person, scene_force_type_, filtering_time_window_);
+	}
+
+
+	person_list_.insert( iit , person );
+	//std::cout << " Cprediction_bhmip::add_person_container antes set_destinations=> destinations_.empty()="<<destinations_.empty()<< std::endl;
+	person->set_destinations(destinations_);
+
+	return (Cperson_abstract*) person;
+}
+
+
+void Cprediction_bhmip::scene_intentionality_prediction_bhmip()
+{
+	//std::cout << " INI Cprediction_bhmip::scene_intentionality_prediction_bhmip(); min_v_to_predict_="<<min_v_to_predict_<< std::endl;
+	for( std::list<Cperson_abstract*>::iterator iit = person_list_.begin() ; iit != person_list_.end() ;  iit++ )
+	{
+		//if((*iit)->get_id()){
+		//std::cout << " (calculate DEST) Person ID="<<(*iit)->get_id()<< std::endl;
+		//std::cout << " (calculate DEST) min_v_to_predict_="<<min_v_to_predict_<<"; v_person="<<(*iit)->get_current_pointV().v()<< std::endl;
+		//}
+
+		(*iit)->prediction( min_v_to_predict_);
+	}
+
+}
+
+
+void Cprediction_bhmip::clear_scene()
+{
+
+}
+
+
+void Cprediction_bhmip::print2()
+{
+	std::cout << "Cprediction_bhmip::print() person_list_.empty="<<person_list_.empty()<< std::endl;
+
+	for( std::list<Cperson_abstract*>::iterator iit = person_list_.begin() ; iit != person_list_.end() ; iit++ )
+	{
+		(*iit)->print_dest();
+		(*iit)->print();
+	}
+}
diff --git a/local_lib_people_prediction/src/prediction_bhmip.h b/local_lib_people_prediction/src/prediction_bhmip.h
new file mode 100644
index 0000000000000000000000000000000000000000..3e15dc4d1f3398b6957131d22b214abd64626468
--- /dev/null
+++ b/local_lib_people_prediction/src/prediction_bhmip.h
@@ -0,0 +1,98 @@
+/*
+ * prediction_bhmip.h
+ *
+ *  Created on: 2013.  Last Modified on 2020
+ *      Author: Initial code of the AKP Gonzalo Ferrer.
+ *      Author: Addition of ASSAOP and ASSAGP for people accompaniment code of Ely Repiso.
+ */
+
+
+#ifndef prediction_bhmip_H
+#define prediction_bhmip_H
+
+#include "scene_abstract.h"
+#include "scene_elements/person_abstract.h"
+#include "iri_geometry.h"
+#include <vector>
+#include <list>
+
+
+/**
+ *
+ * \brief Prediction_bhmip Class
+ *
+ * The prediction class provides a set of algorithms
+ * solving intentionality prediction and long term prediction.
+ * A set of destinations is assumed to be provided
+ *
+ *
+*/
+class Cprediction_bhmip : public Cscene_abstract
+{
+	public:
+		Cprediction_bhmip ( Cperson_abstract::filtering_method filter= Cperson_abstract::No_filtering,
+				Cperson_abstract::update_person_method update_method = Cperson_abstract::Autoremove );
+		virtual ~Cprediction_bhmip();
+		virtual void update_scene(const std::vector<SdetectionObservation>& observation ,bool& we_have_person_companion,  bool person_or_robot=false);
+		virtual void rotate_and_traslate_scene(unsigned int id, double R, double thetaZ, double linear_vx, double linear_vy, double v_rot_x, double v_rot_y, std::vector<double> vect_odom_eigen_tf, bool debug  = false);
+		virtual Cperson_abstract* add_person_container( unsigned int id,
+				std::list<Cperson_abstract*>::iterator iit );
+		/**
+		 * scene_intentionality_prediction_bhmip( ) calculates the intentionality prediction
+		 * for observed trajectories
+		 */
+		void scene_intentionality_prediction_bhmip( );
+		void see_tracks_on_scene();
+		virtual void clear_scene();
+
+		void set_min_v_to_predict( double min_v_to_predict ) { min_v_to_predict_ =  min_v_to_predict;}
+		double get_min_v_to_predict() { return min_v_to_predict_;}
+
+		void set_filtering_time_window( double time_window) {
+			//std::cout << "IN set_filtering_time_window-> filtering_time_window_="<<filtering_time_window_<< std::endl;
+			filtering_time_window_ = time_window;
+		}
+		double get_filtering_time_window( void ) {return filtering_time_window_;}
+
+		void set_id_person_companion_Cprediction_bhmip(unsigned int in_id_person_companion) {
+			id_person_companion_ = in_id_person_companion;
+			//std::cout << "IN set_id_person_companion_Cprediction_bhmip-> id_person_companion_="<<id_person_companion_<< std::endl;
+		}
+
+		void set_id_second_person_companion_Cprediction_bhmip(unsigned int in_id_second_person_companion){
+			id_second_person_companion_=in_id_second_person_companion;
+		}
+
+		unsigned int my_id_person_companion_; // id, actual person companion. the id of it selft
+		void set_my_id_second_person_companion_sim_Cprediction_bhmip(unsigned int in_my_id_person_companion){
+			my_id_person_companion_=in_my_id_person_companion;
+		}
+
+		void Cprediction_bhmip_set_max_d_to_detect_laser_obs(double in_max_d_to_detect_laser_obs){
+			Cscene_abstract_set_max_d_to_detect_laser_obs(in_max_d_to_detect_laser_obs);
+			//std::cout << "set Cprediction_bhmip_set_max_d_to_detect_laser_obs-> in_max_d_to_detect_laser_obs="<<in_max_d_to_detect_laser_obs<< std::endl;
+		}
+
+		//void set_debug_filter_person_velocity(bool debug_filter_person_velocity_in){
+		//	debug_filter_person_velocity_=debug_filter_person_velocity_in;
+		//}
+
+		const std::list<Cperson_abstract*>* get_scene2() { return &person_list_; }
+		void print2();
+		// INI NEW FACKE CHANGE AND DESAPEAR ID:
+		/*void remove_person_from_outside(unsigned int id_people_to_remove){
+			remove_person(id_people_to_remove);
+		}*/
+		// FIN NEW FACKE CHANGE AND DESAPEAR ID:
+	protected:
+		double min_v_to_predict_;
+		double filtering_time_window_;
+		unsigned int id_person_companion_,id_second_person_companion_;
+
+		//bool debug_filter_person_velocity_;
+		bool debug_update_scene_; // debug for person companion
+
+
+
+};
+#endif
diff --git a/local_lib_people_prediction/src/random/rand_gmm.cpp b/local_lib_people_prediction/src/random/rand_gmm.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a56fd0733ede9758a7c8e561456c5bb323600146
--- /dev/null
+++ b/local_lib_people_prediction/src/random/rand_gmm.cpp
@@ -0,0 +1,46 @@
+#include "rand_gmm.h"
+#include <stdlib.h>
+#include <math.h>
+#include <time.h>
+
+double rand_uniform (  )
+{
+	return (double)rand()/(double)RAND_MAX;
+}
+
+double rand_uniform ( double min, double max )
+{
+	return min + (max-min)* rand_uniform();
+}
+
+int rand_uniform_discrete ( int min, int max )
+{
+	//return min + int ( (double)(max-min+1)* rand_uniform());
+	return min + rand() % ( max-min+1 );
+}
+
+double rand_normal(double mu,double sigma)
+{
+	double u = rand_uniform();
+	double v = rand_uniform();
+	double x = sqrt( -2.0*log(u) )*cos( 2*M_PI*v );
+	return mu + sigma*x;
+}
+
+double rand_gmm ( std::vector<Sgauss> & gmm)
+{
+		double p = rand_uniform () , p_accumulated = 0.0;
+		unsigned int i = 0;
+		while ( i < gmm.size() && p_accumulated < p)
+		{
+			p_accumulated += gmm.at(i).w;
+			i++;
+		}
+		i--;
+		return rand_normal ( gmm.at(i).mu , gmm.at(i).sigma );
+}
+
+void rand_seed()
+{
+	srand ( time(NULL) );
+}
diff --git a/local_lib_people_prediction/src/random/rand_gmm.h b/local_lib_people_prediction/src/random/rand_gmm.h
new file mode 100644
index 0000000000000000000000000000000000000000..a0d9b1d209db8a5881510056ad078ee28d4b4656
--- /dev/null
+++ b/local_lib_people_prediction/src/random/rand_gmm.h
@@ -0,0 +1,69 @@
+//library to be deprecated... just use c++11
+
+#ifndef rand_gmm_h
+#define rand_gmm_h
+
+
+
+#include <vector>
+
+struct Sgauss
+{
+	double mu, sigma, w;
+	Sgauss ( ) : mu(0.0) , sigma(0.0) , w(0.0) { }
+	Sgauss ( double mu_ , double sigma_ , double w_ ) :	mu(mu_) , sigma(sigma_) , w(w_)	{ }
+};
+
+/**
+ *
+ * \brief Randon number generator: uniform distribution 
+ *
+ * [0,1]
+ * 
+*/
+double rand_uniform (  );
+/**
+ *
+ * \brief Randon number generator: uniform distribution
+ *
+ * [min,max]
+ *
+*/
+double rand_uniform ( double min, double max );
+
+/**
+ *
+ * \brief Randon number generator: uniform discrete distribution
+ *
+ * {min,...,max}
+ *
+*/
+int rand_uniform_discrete ( int min, int max );
+
+/**
+ *
+ * \brief Randon number generator : normal distribution
+ *
+ * Implementation of the Muller-Box method
+ * 
+*/
+double rand_normal (double mu,double sigma);
+
+/**
+ *
+ * \brief Randon number generator : gaussian mixture model distribution
+ *
+ * N weighted gaussian sources. Strong condition: sum( w_n ) = 1
+ * 
+*/
+double rand_gmm ( std::vector<Sgauss> & gmm);
+
+/**
+ *
+ * \brief Random seed
+ *
+ *
+*/
+void rand_seed();
+
+#endif
diff --git a/local_lib_people_prediction/src/scene_abstract.cpp b/local_lib_people_prediction/src/scene_abstract.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..1a58c9333f762264ed6f38cacb3cdab2f93e3cc8
--- /dev/null
+++ b/local_lib_people_prediction/src/scene_abstract.cpp
@@ -0,0 +1,1124 @@
+/*
+ * scene_abstract.cpp
+ *
+ *  Created on: 2013.  Last Modified on 2020
+ *      Author: Initial code of the AKP Gonzalo Ferrer.
+ *      Author: Addition of ASSAOP and ASSAGP for people accompaniment code of Ely Repiso.
+ */
+
+#include "scene_abstract.h"
+#include <stdio.h>
+#include <iostream>
+#include <math.h>
+
+
+Cscene_abstract::Cscene_abstract( Cperson_abstract::filtering_method filter,
+		Cperson_abstract::update_person_method update_method,
+		Cperson_abstract::force_type type):
+robot_(NULL), dt_(0.1) , now_(0.0) , read_laser_obstacle_success_(false), read_force_map_success_(false), read_destination_map_success_(false),
+scene_force_type_(type), filtering_method_(filter), update_method_(update_method),
+robot_in_the_scene_(false),
+sim_bool_(false),
+max_d_to_detect_laser_obs_(10), // TODO: ojo, devolver al 20, para simulacion.
+max_asos_point_to_person_(100),
+debug_update_person_companion_(false),
+debug_find_person_(false)
+{
+
+
+	switch( scene_force_type_ )
+	{
+	case Cperson_abstract::Collision_Prediction:
+		// Zanlungo collision prediction parameters , {k,lambda,A,B,d}
+		social_forces_param_to_person_ = {1.52, 0.29,1.13,0.71,0.0};
+		//Person-Robot Spherical parameters obtained using our optimization method
+		social_forces_param_to_robot_ = {1.52, 0.29,1.13,0.71,0.0};
+		//set_social_force_parameters_person_robot( force_params_to_vector(2.3, 0.59,2.66,0.79,0.4) );
+		//Obstacle spherical parameters obtained using our optimization method
+		social_forces_param_to_obs_ = {2.3, 1.0,10.0,0.1,0.2};
+		break;
+	case Cperson_abstract::Elliptical:
+		//Default Zanlungo Elliptical parameters
+		social_forces_param_to_person_ = {1.19, 0.08,1.33,0.34,1.78};
+		//Person-Robot Spherical parameters obtained using our optimization method
+		social_forces_param_to_robot_ = {2.3, 0.59,2.66,0.79,0.4};
+		//Obstacle spherical parameters obtained using our optimization method
+		social_forces_param_to_obs_ = {2.3, 1.0,10.0,0.1,0.2};
+		break;
+	case Cperson_abstract::Spherical:
+		//TODO set as default the paramters calculated in ICRA'2104 (Balanced behavior)
+		//Default Zanlungo Spherical parameters (2.3, 0.08,1.33,0.64,0.16)
+		social_forces_param_to_person_ = {4.9, 1.0,10.0,0.64,0.16};//B=0.34, changed to 0.64
+		//Person-Robot Spherical parameters obtained using our optimization method
+		social_forces_param_to_robot_ = {2.3, 0.59,2.66,0.79,0.4};
+		//Obstacle spherical parameters obtained using our optimization method
+		social_forces_param_to_obs_ = {2.3, 1.0,10.0,0.6,0.2};
+		break;
+	}
+
+
+
+}
+
+Cscene_abstract::~Cscene_abstract()
+{
+
+}
+
+void Cscene_abstract::set_destinations( std::vector<Sdestination>& dest )
+{
+	destinations_ = dest;
+	for( std::list<Cperson_abstract*>::iterator iit = person_list_.begin() ; iit != person_list_.end() ; iit++ )
+	{
+		(*iit)->set_destinations( dest );
+	}
+	// robot destinations are not required, are set if planned
+}
+
+Cperson_abstract* Cscene_abstract::add_person( unsigned int id )
+{
+	std::list<Cperson_abstract*>::iterator iit = person_list_.begin();
+	if ( !person_list_.empty())
+	{
+		while( iit != person_list_.end())
+		{
+			if ( (*iit)->get_id()  > id ) //interrupts if the id is greater for ordering the list
+			{
+				break;
+			}
+			iit++;
+		}
+	}
+
+	return add_person_container(id,iit);//virtual function adding the specific person container
+}
+
+void Cscene_abstract::update_other_person_companion( unsigned int id, SpointV_cov in_other_person_companion_point )
+{
+	/*std::list<Cperson_abstract*>::iterator iit = person_list_.begin();
+	if ( !person_list_.empty())
+	{
+		while( iit != person_list_.end())
+		{
+			if ( (*iit)->get_id()  == id ) //interrupts if the id is greater for ordering the list
+			{
+				// update person and breack! => creo que iria mejor un for de la lista normal...
+
+				break;
+			}
+			iit++;
+		}
+	}*/
+
+	 for( auto iit: person_list_ )
+	{
+
+		if(iit->get_id()==id){
+			iit->set_current_pointV(in_other_person_companion_point);
+			// hay que cambiarle el punto actual con el que predice y etc, por el punto que le entra de la odometria del otro robot.
+		}
+	}
+
+
+	//return add_person_container(id,iit);//virtual function adding the specific person container
+}
+
+void Cscene_abstract::remove_person( unsigned int id )
+{
+	std::list<Cperson_abstract*>::iterator iit;
+	Cperson_abstract* person;
+	if (find_person( id,  &person, iit ))
+	{
+		delete person;
+		person_list_.erase(  iit );
+
+	}
+}
+
+void Cscene_abstract::update_robot(Spose observation, bool case_zanlungo)
+{
+
+	now_ = observation.time_stamp;
+	//std::cout << ";IMPORTANTE!!! 7777 333 => observation.time_stamp="<<observation.time_stamp<<"; now_="<<now_<<"; robot_in_the_scene_="<<robot_in_the_scene_<<"; case_zanlungo="<<case_zanlungo<< std::endl;
+	if ( robot_in_the_scene_ )
+	{
+		robot_->add_pose( observation );
+
+		if(case_zanlungo){
+			// if Zanlungo.
+
+			robot_->set_current_pose(observation);
+			//std::cout << ";robot_->get_current_pose.Time_stamp="<<robot_->get_current_pose().time_stamp<< std::endl;
+			robot_->set_current_pointV(SpointV(observation.x,observation.y,observation.time_stamp,observation.v*cos(observation.theta),observation.v*sin(observation.theta)));
+		}
+
+	}
+	else
+	{
+		robot_in_the_scene_ = true;
+		robot_ = new Crobot(0,Crobot::Differential,scene_force_type_);
+		robot_->add_pose( observation );
+		if(case_zanlungo){
+			// if Zanlungo.
+			robot_->set_current_pose(observation);
+			robot_->set_current_pointV(SpointV(observation.x,observation.y,observation.time_stamp,observation.v*cos(observation.theta),observation.v*sin(observation.theta)));
+
+		}
+
+	}
+
+	//std::cout << "!!!!!!!!!!!!!!!!!!!!!!!!111 update_robot !!!!!!!!!!!!!!!!!!1111"<< std::endl;
+	//std::cout << "observation.w="<<observation.w<<"; observation.v="<<observation.v<<"; robot.w="<<robot_->get_current_pose().w<<"; robot.v="<<robot_->get_current_pose().v<< std::endl;
+}
+
+void Cscene_abstract::update_person_companion(Spose observation, SpointV_cov obs_spoint)
+{
+	if(debug_update_person_companion_){
+		std::cout << "!!!!!!!!!!!!!!!!!!!!!!!!111 update_person_companion !!!!!!!!!!!!!!!!!!1111"<< std::endl;
+	}
+
+	//now_ = observation.time_stamp;
+	if ( person_companion_in_the_scene_ )
+	{
+		if(debug_update_person_companion_){
+			std::cout << "(IN update_person_companion) update person companion "<< std::endl;
+		}
+
+		person_companion_->add_pose( observation );
+		person_companion_->set_current_pointV(obs_spoint);
+		person_companion_->set_current_pose(observation);
+	}
+	else
+	{
+		if(debug_update_person_companion_){
+			std::cout << "(IN update_person_companion) new person companion "<< std::endl;
+		}
+		person_companion_in_the_scene_ = true;
+		person_companion_ = new Crobot(0,Crobot::Differential,scene_force_type_);
+		person_companion_->add_pose( observation );
+		person_companion_->set_current_pointV(obs_spoint);
+		person_companion_->set_current_pose(observation);
+	}
+}
+
+bool Cscene_abstract::find_person(unsigned int id)
+{
+	std::list<Cperson_abstract*>::iterator iit = person_list_.begin();
+	if ( !person_list_.empty())
+	{
+		while( iit != person_list_.end())
+		{
+			// only returns persons (robot wont be returned if sought)
+			if( ((*iit)->get_id()  == id) && (((*iit)->get_person_type() == Cperson_abstract::Person)||((*iit)->get_person_type() == Cperson_abstract::Person_companion)) )
+			{
+				return true;
+			}
+			if ( (*iit)->get_id()  > id ) //interrupts if the id is greater for ordering the list
+			{
+				break;
+			}
+			iit++;
+		}
+	}
+	return false;
+}
+
+bool Cscene_abstract::find_person(unsigned int id , Cperson_abstract** person) // (ely) suelo entrar en esta find person!!!
+{
+	if(debug_find_person_){
+	 std::cout <<" (entro en find person 2) person_list_.empty()="<<person_list_.empty() << std::endl;
+	}
+
+	std::list<Cperson_abstract*>::iterator iit = person_list_.begin();
+
+	if(debug_find_person_){
+	 std::cout <<" (entro en find person 2) person_list_.empty()="<<person_list_.empty() << std::endl;
+	}
+
+	// std::cout <<" person_list_.empty()="<<person_list_.empty() << std::endl;
+	if ( !person_list_.empty())
+	{
+		while( iit != person_list_.end())
+		{
+			if( ((*iit)->get_id()  == id) && (((*iit)->get_person_type() == Cperson_abstract::Person)||((*iit)->get_person_type() == Cperson_abstract::Person_companion)))
+			{
+				*person = *iit;
+				return true;
+			}
+			if ( (*iit)->get_id()  > id ) //interrupts if the id is greater for ordering the list
+			{
+				break;
+			}
+			iit++;
+		}
+	}
+	if(debug_find_person_){
+	 std::cout <<" (salgo de find person 2) person_list_.empty()="<<person_list_.empty() << std::endl;
+	}
+	return false;
+}
+
+bool Cscene_abstract::find_person(unsigned int id , Cperson_abstract** person,
+		std::list<Cperson_abstract*>::iterator& it)
+{
+	std::list<Cperson_abstract*>::iterator iit = person_list_.begin();
+
+
+	if ( !person_list_.empty())
+	{
+		while( iit != person_list_.end())
+		{
+			if( ((*iit)->get_id()  == id) && (((*iit)->get_person_type() == Cperson_abstract::Person)||((*iit)->get_person_type() == Cperson_abstract::Person_companion)))
+			{
+				*person = *iit;
+				it = iit;
+				return true;
+			}
+			if ( (*iit)->get_id()  > id ) //interrupts if the id is greater for ordering the list
+			{
+				break;
+			}
+			iit++;
+		}
+	}
+
+	return false;
+}
+
+void Cscene_abstract::print()
+{
+	std::cout << "Cscene_abstract::print() person_list_.empty="<<person_list_.empty()<< std::endl;
+
+	for( std::list<Cperson_abstract*>::iterator iit = person_list_.begin() ; iit != person_list_.end() ; iit++ )
+	{
+		(*iit)->print_dest();
+		(*iit)->print();
+	}
+}
+
+void Cscene_abstract::read_laser_scan( const std::vector<Spoint>&  laser_scan , bool person_simulation)
+{
+
+	//std::cout << "Entro en read_laser_scan !!!" << std::endl;
+	/*if(sim_){
+		std::cout << "sim true !!!" << std::endl;
+	}else{
+		std::cout << "sim false !!!" << std::endl;
+	}
+	if(sim_bool_){
+			std::cout << "sim_bool_ true !!!" << std::endl;
+		}else{
+			std::cout << "sim_bool_ false !!!" << std::endl;
+		}*/
+
+	// Gonzalo hace un obstaculo por cada punto del laser!? Creo que es excesibo!
+
+	Spose act_robot_pose;
+	double distance_robot_obstacle;
+
+	if(!sim_bool_){
+		act_robot_pose= robot_->get_current_pose();
+	}
+	//std::cout << "act_robot_pose.x="<<act_robot_pose.x << std::endl;
+	//std::cout << "act_robot_pose.y="<<act_robot_pose.y << std::endl;
+
+	//TODO take into account both back and front laser scans
+	laser_obstacle_list_.clear();
+	Spoint before_point;
+	unsigned int num_of_near_static_obst_=0;
+	bool first_time=true;
+
+	for ( Spoint p : laser_scan )
+	{
+
+		if(!sim_bool_){
+			distance_robot_obstacle=sqrt(pow(act_robot_pose.x-p.x,2)+pow(act_robot_pose.y-p.y,2));
+		//	std::cout <<"p.x="<<p.x<<"; p.y ="<<p.y<< "; distance_ robot-laser_point ="<<distance_robot_obstacle << std::endl;
+		}
+		//filter laser points corresponding to persons and obstacles
+		if ( !point_corresponds_person(p) && !point_belongs_to_laser_obstacle(p,person_simulation) )
+		{
+			if(!sim_bool_){
+				if(distance_robot_obstacle<max_d_to_detect_laser_obs_){
+					//std::cout <<"insert point!"<< std::endl;
+				//new cluster scale-invariant laser points is created
+					laser_obstacle_list_.push_back( p );
+					if(first_time){
+						first_time=false;
+						num_of_near_static_obst_=1;
+					}
+				}
+			}else{
+				if(first_time){
+					first_time=false;
+					num_of_near_static_obst_=1;
+				}
+				laser_obstacle_list_.push_back( p );
+			}
+		}
+
+
+		///// INI TO estimate distance between contiguous laser points:
+		double dro = p.distance2( robot_->get_current_pointV()  );//distance to robot
+		double dist_near_laser_points;
+				if( dro < 1.0  )//calculates basic distance to robot, near distance, the thr is lower
+				{
+					//dist_thr = 0.36;//0.1 [m]=sqrt(0.01) = 0.1[m] is considered obstacle o sqrt(0.04) = 0.2 [m]
+					//dist_thr = 0.09;//sqrt(0.09) = 0.3[m] is considered obstacle (Lo que tenia antes Gonzalo)
+					if(sim_){
+						dist_near_laser_points = 0.01;
+						//std::cout << "SIM CASE if(d2 < 1.0) => dist_thr = 0.01=sqrt(0.01) = 0.1[m] is considered obstacle"<< std::endl;
+					}else{ // real robot case.
+						dist_near_laser_points = 0.2;//sqrt(0.01) = 0.1[m] is considered obstacle antes 0.09 == 0.3 m
+						//std::cout << "REAL CASE if(d2 < 1.0) => dist_thr = 0.09=sqrt(0.09) = 0.3[m] is considered obstacle"<< std::endl;
+					}
+
+				}
+				else if( dro < 16.0 )//4 [m]
+				{
+					//dist_thr = 0.36;//0.1 [m]=sqrt(0.01) = 0.1[m] is considered obstacle o sqrt(0.04) = 0.2 [m]
+					//dist_thr = 0.09;
+
+					if(sim_){
+						dist_near_laser_points = 0.01; //
+						//std::cout << "SIM CASE if(d2 < 16.0) => dist_thr = 0.01=sqrt(0.01) = 0.1[m] is considered obstacle"<< std::endl;
+					}else{ // real robot case.
+						dist_near_laser_points = 0.2;//sqrt(0.1) = 0.32[m] is considered obstacle (antes era 0.2)
+						//std::cout << "REAL CASE if(d2 < 16.0) => dist_thr = 0.2=sqrt(0.2) = 0.45[m] is considered obstacle"<< std::endl;
+					}
+					//dist_thr = 0.2;//4 [m]=sqrt(0.16) = 0.45[m] is considered obstacle
+
+				}
+				else
+				{
+					//dist_thr = 0.36;//sqrt(0.09) = 0.6[m] is considered obstacle
+					//dist_thr = 0.09;
+
+					if(sim_){
+						dist_near_laser_points = 0.01;
+						//std::cout << "SIM CASE if(d2 > 16.0) => dist_thr = 0.01=sqrt(0.01) = 0.1[m] is considered obstacle"<< std::endl;
+					}else{ // real robot case.
+						dist_near_laser_points = 0.2;//sqrt(0.09) = 0.6[m] is considered obstacle
+						//std::cout << "REAL CASE if(d2 > 16.0) => dist_thr = 0.36=sqrt(0.09) = 0.6[m] is considered obstacle"<< std::endl;
+					}
+					//std::cout << "else => dist_thr =  0.36=sqrt(0.09) = 0.6[m] is considered obstacle"<< std::endl;
+				}
+
+		////////////// Fin TO estimate distance between contiguous laser points:
+
+		if(!first_time){
+
+			if(before_point.distance2(p)>dist_near_laser_points){
+				num_of_near_static_obst_++;
+			}
+
+		}
+
+		before_point=p;
+
+	}
+
+
+	//std::cout << "Best approximation of num_of_near_static_obst_="<<num_of_near_static_obst_<< std::endl;
+	//std::cout <<"sim_bool_="<<sim_bool_<<"; max_d_to_detect_laser_obs_="<<max_d_to_detect_laser_obs_<<"; laser_obstacle_list_.size()="<<laser_obstacle_list_.size()<< std::endl;
+
+	/*if(!laser_obstacle_list_.empty()){
+		std::cout << "laser_obstacle_list_[0]:"<< std::endl;
+		laser_obstacle_list_[0].print();
+	}*/
+	//std::cout << "max_d_to_detect_laser_obs_="<<max_d_to_detect_laser_obs_<< std::endl;
+	//std::cout << "[number of created obstacles.] laser_obstacle_list_.size() ="<<laser_obstacle_list_.size()<< std::endl;
+
+	read_laser_obstacle_success_ = true;//flag to indicate that indeed we can make use of laser information regarding obstacles.
+}
+
+void Cscene_abstract::read_laser_scan_person_companion_akp_planner( const std::vector<Spoint>&  laser_scan , bool person_simulation)
+{
+
+	//std::cout << "Entro en read_laser_scan !!!" << std::endl;
+	Spose act_robot_pose;
+	double distance_robot_obstacle;
+
+	if(!sim_bool_){
+		act_robot_pose= person_companion_->get_current_pose();
+	}else{
+		act_robot_pose= person_companion_->get_current_pose();
+	}
+	//std::cout << "act_robot_pose.x="<<act_robot_pose.x << std::endl;
+	//std::cout << "act_robot_pose.y="<<act_robot_pose.y << std::endl;
+
+	//TODO take into account both back and front laser scans
+	laser_obstacle_list_.clear();
+	for ( Spoint p : laser_scan )
+	{
+
+		if(!sim_bool_){
+			distance_robot_obstacle=sqrt(pow(act_robot_pose.x-p.x,2)+pow(act_robot_pose.y-p.y,2));
+		//	std::cout <<"p.x="<<p.x<<"; p.y ="<<p.y<< "; distance_ robot-laser_point ="<<distance_robot_obstacle << std::endl;
+		}else{
+			distance_robot_obstacle=sqrt(pow(act_robot_pose.x-p.x,2)+pow(act_robot_pose.y-p.y,2));
+		}
+		//filter laser points corresponding to persons and obstacles
+		if ( !point_corresponds_person(p) && !point_belongs_to_laser_obstacle(p,person_simulation) )
+		{
+			if(!sim_bool_){
+				if(distance_robot_obstacle<max_d_to_detect_laser_obs_){
+					//std::cout <<"insert point!"<< std::endl;
+				//new cluster scale-invariant laser points is created
+					laser_obstacle_list_.push_back( p );
+				}
+			}else{
+				if(distance_robot_obstacle<max_d_to_detect_laser_obs_){
+					laser_obstacle_list_.push_back( p );
+				}
+			}
+		}
+	}
+
+	/*if(!laser_obstacle_list_.empty()){
+		std::cout << "laser_obstacle_list_[0]:"<< std::endl;
+		laser_obstacle_list_[0].print();
+	}*/
+	//std::cout << "max_d_to_detect_laser_obs_="<<max_d_to_detect_laser_obs_<< std::endl;
+	//std::cout << "laser_obstacle_list_.size() ="<<laser_obstacle_list_.size()<< std::endl;
+	//std::cout <<"[PERSON COMPANION] sim_bool_="<<sim_bool_<<"; max_d_to_detect_laser_obs_="<<max_d_to_detect_laser_obs_<<"; laser_obstacle_list_.size()="<<laser_obstacle_list_.size()<< std::endl;
+
+	read_laser_obstacle_success_ = true;//flag to indicate that indeed we can make use of laser information regarding obstacles.
+}
+
+
+
+void Cscene_abstract::read_laser_scan_companion( const std::vector<Spoint>&  laser_scan , bool person_simulation)
+{
+
+	//std::cout << "Entro en read_laser_scan !!!" << std::endl;
+	Spose act_robot_pose;
+	double distance_robot_obstacle;
+	std::vector<Spoint> laser_obstacle_list_act;//list of obstacles seen by laser
+	laser_obstacle_list_act.clear();
+
+	if(!sim_bool_){
+		act_robot_pose= robot_->get_current_pose();
+	}
+	//std::cout << "act_robot_pose.x="<<act_robot_pose.x << std::endl;
+	//std::cout << "act_robot_pose.y="<<act_robot_pose.y << std::endl;
+
+	//TODO take into account both back and front laser scans
+	laser_obstacle_list_.clear();
+	real_person_list_.clear();
+
+	for( Cperson_abstract* iit : person_list_ )
+	{
+		unsigned int count_act_person=0;
+
+		for ( Spoint p : laser_scan ){
+
+			if( p.distance( iit->get_current_pointV() ) < 1 )//sqrt(0.3) = 0.6[m] is considered person (for companion Ely, pq asociaba mal personas en obstaculos)
+			{
+				count_act_person++;
+			}
+
+		}
+
+		if(count_act_person<max_asos_point_to_person_){
+			real_person_list_.push_back(iit);
+			//std::cout << "real person id="<<iit->get_id() << std::endl;
+		}
+
+	}
+
+
+	for ( Spoint p : laser_scan )
+	{
+
+		if(!sim_bool_){
+			distance_robot_obstacle=sqrt(pow(act_robot_pose.x-p.x,2)+pow(act_robot_pose.y-p.y,2));
+			//	std::cout <<"p.x="<<p.x<<"; p.y ="<<p.y<< "; distance_ robot-laser_point ="<<distance_robot_obstacle << std::endl;
+		}
+			//filter laser points corresponding to persons and obstacles
+		if ( !point_corresponds_person_companion(p) && !point_belongs_to_laser_obstacle(p,person_simulation) )
+		{
+			if(!sim_bool_){
+				if(distance_robot_obstacle<max_d_to_detect_laser_obs_){
+						//std::cout <<"insert point!"<< std::endl;
+					//new cluster scale-invariant laser points is created
+					laser_obstacle_list_.push_back( p );
+				}
+			}else{
+				laser_obstacle_list_.push_back( p );
+			}
+		}
+	}
+
+	//std::cout << "max_d_to_detect_laser_obs_="<<max_d_to_detect_laser_obs_<< std::endl;
+		//std::cout << "laser_obstacle_list_.size() ="<<laser_obstacle_list_.size()<< std::endl;
+
+	read_laser_obstacle_success_ = true;//flag to indicate that indeed we can make use of laser information regarding obstacles.
+}
+
+
+bool Cscene_abstract::point_corresponds_person( Spoint laser_point )
+{
+	for( Cperson_abstract* iit : person_list_ )
+	{
+
+		//std::cout << "; person_list_.id="<<iit->get_id()<<"; person_list.size()="<<person_list_.size()<<"; laser_point.distance( iit->get_current_pointV() ="<<laser_point.distance( iit->get_current_pointV() )<< std::endl;
+		//if( laser_point.distance2( iit->get_current_pointV() ) < 1.0 )//sqrt(1) = 1[m] is considered person
+		if( laser_point.distance( iit->get_current_pointV() ) < 1.5 )//sqrt(0.3) = 0.6[m] is considered person (for companion Ely, pq asociaba mal personas en obstaculos)
+		{  // en simulacion es < 0.5!!!
+
+			//for(Spoint iit2 : laser_obstacle_list_){
+			//	double df=iit2.distance(iit->get_current_pointV());
+			//	if(df< 0.5){
+			//		return false;
+			//	}else{
+					return true;
+			//	}
+			//}
+		}
+	}
+	return false;
+}
+
+
+bool Cscene_abstract::point_corresponds_person_companion( Spoint laser_point )
+{
+
+	for( Cperson_abstract* iit : real_person_list_ )
+		{
+			//if( laser_point.distance2( iit->get_current_pointV() ) < 1.0 )//sqrt(1) = 1[m] is considered person
+			if( laser_point.distance( iit->get_current_pointV() ) < 0.8 )//sqrt(0.3) = 0.6[m] is considered person (for companion Ely, pq asociaba mal personas en obstaculos)
+			{ // en simulacion es < 0.5!!!
+
+				//for(Spoint iit2 : laser_obstacle_list_){
+				//	double df=iit2.distance(iit->get_current_pointV());
+				//	if(df< 0.5){
+				//		return false;
+				//	}else{
+						return true;
+				//	}
+				//}
+			}
+		}
+		return false;
+
+
+
+}
+
+
+bool Cscene_abstract::point_belongs_to_laser_obstacle( Spoint p, bool person_simulation)
+{
+
+	double dist_thr,d2;
+	if(!person_simulation){
+		// CASE obstacles to robot.
+		d2 = p.distance2( robot_->get_current_pointV()  );//distance to robot
+		if( d2 < 1.0  )//calculates basic distance to robot, near distance, the thr is lower
+		{
+			//dist_thr = 0.36;//0.1 [m]=sqrt(0.01) = 0.1[m] is considered obstacle o sqrt(0.04) = 0.2 [m]
+			//dist_thr = 0.09;//sqrt(0.09) = 0.3[m] is considered obstacle (Lo que tenia antes Gonzalo)
+			if(sim_){
+				//dist_thr = 0.01; sim
+				dist_thr=0.09;
+				//std::cout << "SIM CASE if(d2 < 1.0) => dist_thr = 0.01=sqrt(0.01) = 0.1[m] is considered obstacle"<< std::endl;
+			}else{ // real robot case.
+				dist_thr = 0.09;//sqrt(0.09) = 0.3[m] is considered obstacle
+				//std::cout << "REAL CASE if(d2 < 1.0) => dist_thr = 0.09=sqrt(0.09) = 0.3[m] is considered obstacle"<< std::endl;
+			}
+
+		}
+		else if( d2 < 16.0 )//4 [m]
+		{
+			//dist_thr = 0.36;//0.1 [m]=sqrt(0.01) = 0.1[m] is considered obstacle o sqrt(0.04) = 0.2 [m]
+			//dist_thr = 0.09;
+
+			if(sim_){
+				//dist_thr = 0.01; // sim
+				dist_thr = 0.2;
+				//std::cout << "SIM CASE if(d2 < 16.0) => dist_thr = 0.01=sqrt(0.01) = 0.1[m] is considered obstacle"<< std::endl;
+			}else{ // real robot case.
+				dist_thr = 0.2;//sqrt(0.16) = 0.45[m] is considered obstacle
+				//std::cout << "REAL CASE if(d2 < 16.0) => dist_thr = 0.2=sqrt(0.2) = 0.45[m] is considered obstacle"<< std::endl;
+			}
+			//dist_thr = 0.2;//4 [m]=sqrt(0.16) = 0.45[m] is considered obstacle
+
+		}
+		else
+		{
+			//dist_thr = 0.36;//sqrt(0.09) = 0.6[m] is considered obstacle
+			//dist_thr = 0.09;
+
+
+			if(sim_){
+				//dist_thr = 0.01;sim
+				dist_thr = 0.36;
+				//std::cout << "SIM CASE if(d2 > 16.0) => dist_thr = 0.01=sqrt(0.01) = 0.1[m] is considered obstacle"<< std::endl;
+			}else{ // real robot case.
+				dist_thr = 0.36;//sqrt(0.09) = 0.6[m] is considered obstacle
+				//std::cout << "REAL CASE if(d2 > 16.0) => dist_thr = 0.36=sqrt(0.09) = 0.6[m] is considered obstacle"<< std::endl;
+			}
+			//std::cout << "else => dist_thr =  0.36=sqrt(0.09) = 0.6[m] is considered obstacle"<< std::endl;
+		}
+		//std::cout << "entro en IF dist_thr="<<dist_thr<< std::endl;
+	}else{
+		// CASE obstacles to person companion simulated.
+		//std::cout << "entro en else dist_thr="<<dist_thr<< std::endl;
+
+
+		if(sim_){
+			//dist_thr = 0.01; // sim
+			dist_thr = 0.09;
+			//std::cout << "Person_companion SIM CASE if(d2 > 16.0) => dist_thr = 0.01=sqrt(0.01) = 0.1[m] is considered obstacle"<< std::endl;
+		}else{ // real robot case. // NEVER enter in this case!
+			dist_thr = 0.09;//sqrt(0.09) = 0.3[m] is considered obstacle
+		}
+
+		dist_thr = 0.36; // for zanlungo model.
+	}
+	for( Spoint o : laser_obstacle_list_ )
+	{
+		if ( o.distance2( p )  <  dist_thr )
+			return true;
+	}
+	return false;
+}
+
+Sforce Cscene_abstract::force_objects_laser_int( Cperson_abstract* person)
+{
+	Sforce force_res;
+	for( Spoint iit : laser_obstacle_list_)
+	{
+		//there is a list for persons and another for robot(s). This function is for obstacles
+		if( person->get_current_pointV().distance2( iit ) < 25.0)//square distance 5^2
+		{
+			force_res += person->force_sphe( iit , this->get_sfm_int_params( person ) );
+		}
+	}
+	return force_res;
+
+}
+
+Sforce Cscene_abstract::force_persons_int( Cperson_abstract* center , bool if_robot_is_person_companion )
+{
+	//std::cout << " (1) !!! force_persons_int" << std::endl;
+
+	Sforce force_res;
+
+	if(if_robot_is_person_companion){
+		//std::cout << " (1) !!! if_robot_is_person_companion" << std::endl;
+		for( std::list<Cperson_abstract*>::iterator iit = person_list_.begin() ; iit != person_list_.end() ; iit++ )
+		{
+			//std::cout << " (2) !!! force_persons_int" << std::endl;
+			//there is a list for persons and another for robot(s). This function is for persons
+			if( *center != *(*iit) && center->get_current_pointV().distance( (*iit)->get_current_pointV() ) < 15.0)
+			{
+				//std::cout << " (3) !!! force_persons_int; center.type="<<center->get_person_type()<<"; *iit.type="<<(*iit)->get_person_type() <<"; (*iit)->get_id()="<<(*iit)->get_id()<< std::endl;
+				if((*iit)->get_id()!=1){ // todo, poner el id de la person companion desde el planner.
+					force_res += center->force( (*iit)->get_current_pointV() ,  this->get_sfm_int_params( center , *iit) );
+				}
+				//std::cout << " (4) !!! force_persons_int" << std::endl;
+			}
+		}
+	}else{
+		//std::cout << " (1) !!! else =>if_robot_is_person_companion" << std::endl;
+		for( std::list<Cperson_abstract*>::iterator iit = person_list_.begin() ; iit != person_list_.end() ; iit++ )
+		{
+			//std::cout << " (2) !!! force_persons_int" << std::endl;
+			//there is a list for persons and another for robot(s). This function is for persons
+			if( *center != *(*iit) && center->get_current_pointV().distance( (*iit)->get_current_pointV() ) < 15.0)
+			{
+				//std::cout << " (3) !!! force_persons_int; center.type="<<center->get_person_type()<<"; *iit.type="<<(*iit)->get_person_type() << std::endl;
+				force_res += center->force( (*iit)->get_current_pointV() ,  this->get_sfm_int_params( center , *iit) );
+				//std::cout << " (4) !!! force_persons_int" << std::endl;
+			}
+		}
+	}
+
+	//std::cout << " (out) !!! force_persons_int" << std::endl;
+
+
+
+	return force_res;
+
+}
+
+Sforce Cscene_abstract::force_persons_int_companion( Cperson_abstract* center , unsigned int id_person_companion, bool person_reactive, Cperson_abstract::companion_reactive type_rec_akp)
+{
+	Sforce force_res;
+	for( std::list<Cperson_abstract*>::iterator iit = person_list_.begin() ; iit != person_list_.end() ; iit++ )
+	{
+		//there is a list for persons and another for robot(s). This function is for persons
+		if( *center != *(*iit) && center->get_current_pointV().distance( (*iit)->get_current_pointV() ) < 15.0)
+		{
+			if((*iit)->get_id()!=id_person_companion){
+				force_res += center->force( (*iit)->get_current_pointV() ,  this->get_sfm_int_params( center , *iit) );
+			}else if(((*iit)->get_id()==id_person_companion)&&person_reactive){
+				force_res += center->force( (*iit)->get_current_pointV() ,  this->get_sfm_int_params( center , *iit),&robot_->get_current_pointV(),type_rec_akp);
+			}
+		}
+	}
+	return force_res;
+
+}
+
+Sforce Cscene_abstract::force_persons_int2( Cperson_abstract* center, std::vector<int> ids_of_persons_in_group ) // made by ely (companion, no take into accound the person companion of the group force.)
+{
+	// para tener grupos de personas en las personas simuladas.
+
+	Sforce force_res;
+
+	std::vector<bool> person_ids_in_group_bool;
+	//person_ids_in_group_bool.reserve(person_list_.size());
+	for(unsigned int p=0; p<person_list_.size();p++){ //initialize boolean vector
+		person_ids_in_group_bool.push_back(false);
+	}
+
+	for(unsigned int p=0; p<ids_of_persons_in_group.size();p++){
+		person_ids_in_group_bool[ids_of_persons_in_group[p]]=true;
+	}
+
+	for( std::list<Cperson_abstract*>::iterator iit = person_list_.begin() ; iit != person_list_.end() ; iit++ )
+	{
+		//there is a list for persons and another for robot(s). This function is for persons
+		if( *center != *(*iit) && center->get_current_pointV().distance( (*iit)->get_current_pointV() ) < 15.0)
+		{
+			//if(person_ids_in_group_bool[(*iit)->get_id()]==false){
+				force_res += center->force( (*iit)->get_current_pointV() ,  this->get_sfm_int_params( center , *iit) );
+			//}else{
+
+			//}
+
+		}
+	}
+	return force_res;
+
+}
+
+Sforce Cscene_abstract::force_persons_int3( Cperson_abstract* center ) // made by ely (companion, no take into accound the person companion of the group force.)
+{
+	Sforce force_res;
+
+	/*std::vector<bool> person_ids_in_group_bool;
+	//person_ids_in_group_bool.reserve(person_list_.size());
+	for(unsigned int p=0; p<person_list_.size();p++){ //initialize boolean vector
+		person_ids_in_group_bool.push_back(false);
+	}
+
+	for(unsigned int p=0; p<ids_of_persons_in_group.size();p++){
+		person_ids_in_group_bool[ids_of_persons_in_group[p]]=true;
+	}*/
+
+	for( std::list<Cperson_abstract*>::iterator iit = person_list_.begin() ; iit != person_list_.end() ; iit++ )
+	{
+		//there is a list for persons and another for robot(s). This function is for persons
+		if( *center != *(*iit) && center->get_current_pointV().distance( (*iit)->get_current_pointV() ) < 15.0)
+		{
+				force_res += center->force( (*iit)->get_current_pointV() ,  this->get_sfm_int_params( center , *iit) );
+		}
+	}
+	return force_res;
+
+}
+
+const std::vector<double>* Cscene_abstract::get_sfm_params( const Cperson_abstract * center_person)
+{
+	assert( center_person != NULL );
+	switch( center_person->get_person_type() )
+	{
+	case Cperson_abstract::Person_companion :
+		return &social_forces_param_to_person_;
+	case Cperson_abstract::Person :
+		return &social_forces_param_to_person_;
+	case Cperson_abstract::Robot :
+		return &social_forces_param_to_robot_;
+	case Cperson_abstract::Obstacle :
+	default:
+		return &social_forces_param_to_obs_;
+	}
+}
+
+
+
+
+std::vector<double> Cscene_abstract::new_get_sfm_params( const Cperson_abstract * center_person)
+{
+	assert( center_person != NULL );
+	switch( center_person->get_person_type() )
+	{
+	case Cperson_abstract::Person_companion :
+		return social_forces_param_to_person_;
+	case Cperson_abstract::Person :
+		return social_forces_param_to_person_;
+	case Cperson_abstract::Robot :
+		return social_forces_param_to_robot_;
+	case Cperson_abstract::Obstacle :
+	default:
+		return social_forces_param_to_obs_;
+	}
+}
+
+const std::vector<double>* Cscene_abstract::get_sfm_int_params( const Cperson_abstract * center_person, const Cperson_abstract * interacting_person)
+{
+	//std::cout << " IIINNNNNNNNN Cscene_abstract::get_sfm_int_params " << std::endl;
+
+	assert( center_person != NULL );
+	Cperson_abstract::target_type interacting_type;
+	if ( interacting_person == NULL )
+	{
+		interacting_type = Cperson_abstract::Obstacle;
+	}
+	else
+	{
+		interacting_type = interacting_person->get_person_type();
+	}
+
+	//return the corresponding SFM parameters
+	if( center_person->get_person_type() == Cperson_abstract::Person
+			&& interacting_type != Cperson_abstract::Obstacle )
+		//	&& interacting_type == Cperson_abstract::Person ) //TODO cambio para ver si funciona la relacion asimetrica
+	{
+		return &social_forces_param_to_person_;
+	}if(center_person->get_person_type() == Cperson_abstract::Person_companion
+			&& interacting_type != Cperson_abstract::Obstacle ){
+		//std::cout << " (persona normal con obst) social_forces_param_to_person_[0]="<<social_forces_param_to_person_[0]<<"; [1]="<< social_forces_param_to_person_[1]<<"; [2]="<<social_forces_param_to_person_[2]<<"; [3]="<<social_forces_param_to_person_[3]<<"; [4]="<<social_forces_param_to_person_[4]<< std::endl;
+
+		return &social_forces_param_to_person_; // TODO: in future could be that the person_companion respect to the robot need to use the same parameters than the robot respect to the person companion
+	}
+	else if( center_person->get_person_type() == Cperson_abstract::Robot &&
+			(interacting_type == Cperson_abstract::Person))
+	{
+		//std::cout << " (persona normal con robot) social_forces_param_to_robot_[0]="<<social_forces_param_to_robot_[0]<<"; [1]="<< social_forces_param_to_robot_[1]<<"; [2]="<<social_forces_param_to_robot_[2]<<"; [3]="<<social_forces_param_to_robot_[3]<<"; [4]="<<social_forces_param_to_robot_[4]<< std::endl;
+
+		return &social_forces_param_to_robot_;
+	}else if( center_person->get_person_type() == Cperson_abstract::Robot &&
+					(interacting_type == Cperson_abstract::Robot)) // probisional, simulation person_companion=> repulsion with real robot == social_forces_param_to_person_companion_ (the same as the repulsion of the robot to the person companion)
+	{
+		//provisional:
+		//std::cout << " social_forces_param_to_person_companion_.size()="<<social_forces_param_to_person_companion_.size() << std::endl;
+
+			//	if(!social_forces_param_to_person_companion_.empty()){
+			//		std::cout << " social_forces_param_to_person_companion_[0]="<<social_forces_param_to_person_companion_[0]<<"; [1]="<< social_forces_param_to_person_companion_[1]<<"; [2]="<<social_forces_param_to_person_companion_[2]<<"; [3]="<<social_forces_param_to_person_companion_[3]<<"; [4]="<<social_forces_param_to_person_companion_[4]<< std::endl;
+			//	}
+			return &social_forces_param_to_person_companion_;
+	}else if(center_person->get_person_type() == Cperson_abstract::Robot &&  interacting_type ==Cperson_abstract::Person_companion){
+
+		//std::cout << " social_forces_param_to_person_companion_.size()="<<social_forces_param_to_person_companion_.size() << std::endl;
+
+		//if(!social_forces_param_to_person_companion_.empty()){
+		//	std::cout << " social_forces_param_to_person_companion_[0]="<<social_forces_param_to_person_companion_[0]<<"; [1]="<< social_forces_param_to_person_companion_[1]<<"; [2]="<<social_forces_param_to_person_companion_[2]<<"; [3]="<<social_forces_param_to_person_companion_[3]<<"; [4]="<<social_forces_param_to_person_companion_[4]<< std::endl;
+		//}
+		//std::cout << " IIINNNNNNNNN Cscene_abstract::get_sfm_int_params Cperson_abstract::Robot && Cperson_abstract::Person_companion ;" << std::endl;
+
+		return &social_forces_param_to_person_companion_;
+	}
+	else
+	{
+		//std::cout << " (forces to  obst) social_forces_param_to_obs_[0]="<<social_forces_param_to_obs_[0]<<"; [1]="<< social_forces_param_to_obs_[1]<<"; [2]="<<social_forces_param_to_obs_[2]<<"; [3]="<<social_forces_param_to_obs_[3]<<"; [4]="<<social_forces_param_to_obs_[4]<< std::endl;
+
+		return &social_forces_param_to_obs_;
+	}
+
+
+	return NULL;
+}
+
+bool Cscene_abstract::read_force_map( const char * path )
+{
+	FILE * fid;
+	float fx,fy;
+	int obstacle;
+	fid = fopen( path , "r");
+	if (fid==NULL ) return read_force_map_success_;//false at ini
+	//fscanf(fid,"%f",&fx);
+	int fs =fscanf(fid,"%f",&min_x_);
+	fs = fscanf(fid,"\n%d",&map_number_x_);
+	fs =fscanf(fid,"\n%f",&min_y_);
+	fs =fscanf(fid,"\n%d",&map_number_y_);
+	fs = fscanf(fid,"\n%f",&map_resolution_);
+	max_x_ = min_x_+  map_number_x_ * map_resolution_;
+	max_y_ = min_y_+  map_number_y_ * map_resolution_;
+
+	if( map_number_x_ * map_number_y_ <= 0 ) return read_force_map_success_;//false empty
+	//std::cout << "number of elements (x,y ) = " << map_number_x_ << " , " << map_number_y_ << std::endl;
+	force_map_.reserve( map_number_x_ * map_number_y_ );
+	obstacle_map_.reserve( map_number_x_ * map_number_y_ );
+	for( unsigned int i = 0; i< map_number_y_*map_number_x_; ++i)
+	{
+		//fscanf(fid,"\n%f %f %d",&fx,&fy,&obstacle);
+		fs = fscanf(fid,"\n%f",&fx);
+		fs = fscanf(fid," %f",&fy);
+		fs = fscanf(fid," %d",&obstacle);
+		force_map_.push_back( Sforce(fx,fy) );
+		obstacle_map_.push_back( (bool)obstacle );
+	}
+	fclose(fid);
+	fs &= 0;//to avoid warning
+	//cout << "map_number_x_" << map_number_x_ << " map_number_rows = " << map_number_y_ <<
+	//		"  and the vector map has a total of elements = " << force_map_.size() << endl;
+	read_force_map_success_ = true;
+	return read_force_map_success_;
+}
+
+Sforce Cscene_abstract::get_force_map( double x, double y )
+{
+	if ( read_force_map_success_ && x >= min_x_ && x <= max_x_ &&  y >= min_y_ && y <= max_y_ )
+	{
+		//by construction in the .m, the vector is read in columns
+		// iit = floor( (x-vv(1)) / vv(5))   + floor((y-vv(3)) / vv(5))*M +1;
+		//cout << "iteration" << int( (x-min_x_) / map_resolution_) * map_number_y_  + int ((y-min_y_) / map_resolution_) << endl;
+		return force_map_[ int( (x-min_x_) / map_resolution_) + int ((y-min_y_) / map_resolution_) * map_number_x_ ];
+	}
+	else
+		return Sforce();
+}
+
+bool Cscene_abstract::is_cell_clear_map( double x, double y )
+{
+	//returns true if cell is clear (or outside the map) and false if there is an obstacle
+	if ( read_force_map_success_ && x >= min_x_ && x <= max_x_ &&  y >= min_y_ && y <= max_y_ )
+	{
+		//by construction in the .m, the vector is read in columns
+		// iit = floor( (x-vv(1)) / vv(5))   + floor((y-vv(3)) / vv(5))*M +1;
+		//cout << "iteration" << int( (x-min_x_) / map_resolution_) * map_number_y_  + int ((y-min_y_) / map_resolution_) << endl;
+		return obstacle_map_[ int ( (x-min_x_) / map_resolution_) + int ((y-min_y_) / map_resolution_) * map_number_x_ ];
+	}
+	else
+		return true;
+}
+
+void Cscene_abstract::get_map_params(float &min_x, float &max_x, float &min_y, float &max_y, float &resolution,
+		unsigned int &map_number_x, unsigned int &map_number_y)
+{
+	min_x = min_x_;
+	max_x = max_x_;
+	min_y = min_y_;
+	max_y = max_y_;
+	resolution = map_resolution_;
+	map_number_x = map_number_x_;
+	map_number_y = map_number_y_;
+}
+
+unsigned int Cscene_abstract::xy_to_m(double x, double y)
+{
+	if ( read_force_map_success_ && x >= min_x_ && x <= max_x_ &&  y >= min_y_ && y <= max_y_ )
+		{
+			return int ( (x-min_x_) / map_resolution_) + int ((y-min_y_) / map_resolution_) * map_number_x_ ;
+		}
+	return 0;
+}
+Spoint Cscene_abstract::m_to_xy( unsigned int m)
+{
+	double x = min_x_+(m - m/map_number_x_ * map_number_x_ ) * map_resolution_;
+	double y = min_y_ + m/map_number_x_*map_resolution_;
+	return Spoint( x,y );
+}
+
+bool Cscene_abstract::read_destination_map( const char * path )
+{
+	/* * how to use this function: you need a destination's file
+	number of destinations in the scene
+	dest_id_1 , x , y , pr, n_neighbours , neighbour1, neighbour2, ...
+	dest_id_2 , x , y , pr, n_neighbours , neighbour1, neighbour2, ...
+	2
+	1 0.0 0.0 0.5 1 2
+	2 10.0 10.0 0.5 1 1
+	*/
+	std::cout << "Enters in Cscene_abstract::read_destination_map( const char * path ) " << std::endl;
+
+	FILE * fid;
+	fid = fopen( path , "r");
+	if (fid==NULL) return read_destination_map_success_;//false at ini
+	//fscanf(fid,"%f",&fx);
+	int n_dest;
+	int fs = fscanf(fid,"%d",&n_dest);
+	//n_dest=4; // de momento hard core, por error al leer.
+	destinations_.clear();
+	//std::cout << "n_dest " <<n_dest << std::endl;
+	destinations_.reserve( n_dest );
+	int id, nn, n;
+	float x,y,pr;
+	std::vector<int> neigh;
+	for( unsigned int i = 0; i< (unsigned) n_dest; ++i)
+	{
+		//dest_ id , x , y , pr, n_neighbours , neighbour1, neighbour2, ...
+		fs = fscanf(fid,"\n%d, %f, %f, %f, %d",&id,&x,&y,&pr,&nn);
+		//nn=1;  // de momento hard core, por error al leer.
+		neigh.clear();
+		neigh.reserve(nn);
+		//std::cout << "nn= " <<nn << std::endl;
+		for(unsigned int j = 0; j < (unsigned) nn; ++j)
+		{
+			fs = fscanf(fid,", %d" , &n);
+			neigh.push_back(n);
+		}
+		destinations_.push_back( Sdestination(id,x,y,pr, Sdestination::Map_goal , neigh) );
+		destinations_.back().print();
+	}
+	fclose(fid);
+	fs *= 0;
+	//check that there are destinations
+	if( destinations_.empty() ) return read_destination_map_success_;
+	read_destination_map_success_ = true;
+	//update robot and people destinations
+	this->set_destinations( destinations_ );
+	return read_destination_map_success_;
+}
+
+bool Cscene_abstract::read_destination_map2( const char * path )  // made by (ely), to make people groups in the simulation.
+{
+	/* * how to use this function: you need a destination's file
+	number of destinations in the scene
+	dest_id_1 , x , y , pr, n_neighbours , neighbour1, neighbour2, ...
+	dest_id_2 , x , y , pr, n_neighbours , neighbour1, neighbour2, ...
+	2
+	1 0.0 0.0 0.5 1 2
+	2 10.0 10.0 0.5 1 1
+	*/
+	std::cout << "(ELY) Enters in Cscene_abstract::read_destination_map( const char * path ) " << std::endl;
+
+	FILE * fid;
+	//std::cout << "1 "<< std::endl;
+	fid = fopen( path , "r");
+	//std::cout << "2"<< std::endl;
+	if (fid==NULL) return read_destination_map_success_;//false at ini
+	//fscanf(fid,"%f",&fx);
+	//std::cout << "3 "<< std::endl;
+	int n_dest;
+	//std::cout << "4 "<< std::endl;
+	int fs = fscanf(fid,"%d",&n_dest);
+	//std::cout << "5 "<< std::endl;
+	//n_dest=4; // de momento hard core, por error al leer.
+	destinations_.clear();
+	//std::cout << "n_dest " <<n_dest << std::endl;
+	destinations_.reserve( n_dest );
+	int id, nn, n;
+	float x,y,pr;
+	std::vector<int> neigh;
+	for( unsigned int i = 0; i< (unsigned) n_dest; ++i)
+	{
+		//dest_ id , x , y , pr, n_neighbours , neighbour1, neighbour2, ...
+		fs = fscanf(fid,"\n%d, %f, %f, %f, %d",&id,&x,&y,&pr,&nn);
+		//nn=1;  // de momento hard core, por error al leer.
+		neigh.clear();
+		neigh.reserve(nn);
+		std::cout << "nn= " <<nn << std::endl;
+		for(unsigned int j = 0; j < (unsigned) nn; ++j)
+		{
+			fs = fscanf(fid,", %d" , &n);
+			neigh.push_back(n);
+		}
+		destinations_.push_back( Sdestination(id,x,y,pr, Sdestination::Map_goal , neigh) );
+		destinations_.back().print();
+	}
+	fclose(fid);
+	fs *= 0;
+	//check that there are destinations
+	if( destinations_.empty() ) return read_destination_map_success_;
+	read_destination_map_success_ = true;
+	//update robot and people destinations
+	this->set_destinations( destinations_ );
+	std::cout << "destinations_.size()="<<destinations_.size()<< std::endl;
+
+	return read_destination_map_success_;
+}
diff --git a/local_lib_people_prediction/src/scene_abstract.h b/local_lib_people_prediction/src/scene_abstract.h
new file mode 100644
index 0000000000000000000000000000000000000000..7006c2f1ce4fa9e621e19a5f54dbb28f1c1fb7d9
--- /dev/null
+++ b/local_lib_people_prediction/src/scene_abstract.h
@@ -0,0 +1,195 @@
+/*
+ * scene_abstract.h
+ *
+ *  Created on: 2013.  Last Modified on 2020
+ *      Author: Initial code of the AKP Gonzalo Ferrer.
+ *      Author: Addition of ASSAOP and ASSAGP for people accompaniment code of Ely Repiso.
+ */
+
+#ifndef scene_abstract_H
+#define scene_abstract_H
+
+#include "scene_elements/person_abstract.h"
+#include "iri_geometry.h"
+#include "scene_elements/robot.h"
+#include <list>
+#include <vector>
+
+
+/**
+ *
+ * \brief Scene_abstract Class
+ *
+ * The scene class provides an abstract interface and a set
+ * of methods to store, manage,
+ * get and set information regarding the moving targets present
+ * in a given scene. It also manages the social-forces relative
+ * to each of the members present in Cscene
+ *
+ */
+
+class Cscene_abstract
+{
+	public:
+	    enum person_container_type { Person_Virtual=0, Robot, Obstacle, Person_BHMIP};
+		Cscene_abstract( Cperson_abstract::filtering_method filter= Cperson_abstract::No_filtering,
+				Cperson_abstract::update_person_method update_method = Cperson_abstract::Autoremove,
+				Cperson_abstract::force_type type=Cperson_abstract::Spherical );
+		virtual ~Cscene_abstract()=0;
+		void set_destinations ( std::vector<Sdestination>& dest );
+		const std::vector<Sdestination>* get_destinations() { return &destinations_; }
+		//std::vector<Sdestination> get_destinations2() { return destinations_; }
+		void set_dt( double dt ) {
+			std::cout << " dt_="<<dt_<< std::endl;
+			dt_ = dt;
+		}
+		double get_dt() { return dt_; }
+		double get_time(){ return now_;}
+		Cperson_abstract* add_person( unsigned int id );//private
+		//, Cperson_abstract::target_type type=Cperson_abstract::Person,
+		//		Cperson_abstract::force_type f_type=Cperson_abstract::Spherical );
+		virtual Cperson_abstract* add_person_container( unsigned int id,
+				std::list<Cperson_abstract*>::iterator iit )=0;
+		void update_other_person_companion(unsigned int id, SpointV_cov in_other_person_companion_point); // for case group of two people and one robot.
+		void remove_person( unsigned int id );
+		virtual void update_robot( Spose  observation , bool case_zanlungo=false);
+		virtual void update_person_companion( Spose  observation , SpointV_cov obs_spoint);
+		const Crobot* get_robot() const { return robot_;}
+		Crobot* get_robot2(){ return robot_;}
+		Crobot* get_person_companion_akp() {
+			return person_companion_;
+		}
+		void set_person_companion_akp(Crobot* ini_person) { person_companion_=ini_person;}
+		void set_robot_destinations( std::vector<Sdestination>& dest ){ robot_->set_destinations( dest );}
+		/**
+		 * \brief get_scene
+		 *
+		 * This abstract function is the main method to update a scene  and
+		 * its implementation depends on the class of scene.
+		 */
+		virtual void update_scene(const std::vector<SdetectionObservation>& observation,bool& we_have_person_companion, bool person_or_robot=false) = 0;
+		const std::list<Cperson_abstract*>* get_scene() { return &person_list_; }
+		bool find_person(unsigned int id);
+		bool find_person(unsigned int id , Cperson_abstract** person);
+		bool find_person(unsigned int id , Cperson_abstract** person, std::list<Cperson_abstract*>::iterator& it);
+		void print();
+		void set_scene_force_type( Cperson_abstract::force_type type ){ scene_force_type_ = type;}
+		Cperson_abstract::force_type get_scene_force_type( ) { return scene_force_type_;}
+
+		// Sforce methods
+		Sforce force_persons_int( Cperson_abstract* person , bool if_robot_is_person_companion=false);
+		Sforce force_persons_int_companion( Cperson_abstract* center , unsigned int id_person_companion, bool person_reactive, Cperson_abstract::companion_reactive type_rec_akp); // made by (ely), for forces in robot, companion mode.
+		Sforce force_persons_int2( Cperson_abstract* person , std::vector<int> ids_of_persons_in_group ); // made by ely, groups of persons.
+		Sforce force_persons_int3( Cperson_abstract* center ); // made by ely (companion, no take into accound the person companion of the group force.)
+
+		Sforce force_objects_laser_int( Cperson_abstract* person );//this function calculates the repulsion forces due to static obstacles
+		//void calculate_social_forces_all_persons();//to be deprecated
+		virtual const std::vector<double>* get_sfm_int_params( const Cperson_abstract * center_person,
+				const Cperson_abstract * interacting_person = NULL);
+		const std::vector<double>* get_sfm_params( const Cperson_abstract * center_person );
+		std::vector<double> new_get_sfm_params( const Cperson_abstract * center_person);
+		virtual void clear_scene()=0;
+
+		//read laser
+		void read_laser_scan( const std::vector<Spoint>&  laser_scan , bool person_simulation=false);//this is the only public function for processing laser scans
+		void read_laser_scan_person_companion_akp_planner( const std::vector<Spoint>&  laser_scan , bool person_simulation=false);
+		void read_laser_scan_companion( const std::vector<Spoint>&  laser_scan , bool person_simulation=false);
+		const std::vector<Spoint>* get_laser_obstacles() { return &laser_obstacle_list_; }
+
+		//read map
+		bool read_force_map( const char * path );
+		Sforce get_force_map( double x, double y );
+		Sforce get_force_map_robot_position( ){
+			return get_force_map( robot_->get_current_pointV().x, robot_->get_current_pointV().y );}
+		bool is_cell_clear_map( double x, double y );
+		void get_map_params( float &min_x, float &max_x, float &min_y, float &max_y, float &resolution ,
+				unsigned int &map_number_x, unsigned int &map_number_y);
+		bool get_map_obstacle(unsigned int i) { return obstacle_map_.at(i); }
+		//temporal
+		unsigned int xy_to_m(double x, double y);
+		Spoint m_to_xy( unsigned int m);
+		bool read_destination_map( const char * path );
+		bool read_destination_map2( const char * path );
+
+		//force parameters methods
+		void set_sfm_to_person( const std::vector<double>& params ){ social_forces_param_to_person_ = params;}
+		void set_sfm_to_robot( const std::vector<double>& params ){ social_forces_param_to_robot_ = params;}
+		void set_sfm_to_obstacle( const std::vector<double>& params ){ social_forces_param_to_obs_ = params;}
+		void set_sfm_to_person_companion( const std::vector<double>& params ){
+			social_forces_param_to_person_companion_ = params;
+			//std::cout << " social_forces_param_to_person_companion_[0]="<<social_forces_param_to_person_companion_[0]<<"; [1]="<< social_forces_param_to_person_companion_[1]<<"; [2]="<<social_forces_param_to_person_companion_[2]<<"; [3]="<<social_forces_param_to_person_companion_[3]<<"; [4]="<<social_forces_param_to_person_companion_[4]<< std::endl;
+
+			//std::cout << "IN set_sfm_to_person_companion!!!!!!!!!!!!!!!!!!!! SIZE="<<social_forces_param_to_person_companion_.size() << std::endl;
+		}
+
+		std::vector<double> get_set_sfm_to_person_companion(){
+			return social_forces_param_to_person_companion_;
+		}
+
+		// local tracker methods
+		virtual void rotate_and_traslate_scene(unsigned int id, double R, double thetaZ, double linear_vx, double linear_vy, double v_rot_x, double v_rot_y, bool debug  = false){}
+		void set_bool_sim(bool in_sim_bool_){sim_bool_=in_sim_bool_;};
+		void Cscene_abstract_set_max_d_to_detect_laser_obs(double in_max_d_to_detect_laser_obs){
+			max_d_to_detect_laser_obs_=in_max_d_to_detect_laser_obs;
+			std::cout << "set Cscene_abstract_set_max_d_to_detect_laser_obs-> max_d_to_detect_laser_obs_="<<max_d_to_detect_laser_obs_<< std::endl;
+		}
+
+		void set_max_asos_point_to_person(unsigned int in_max_asos_point_to_person){
+			max_asos_point_to_person_=in_max_asos_point_to_person;
+		}
+
+		void set_sim(bool in_sim){
+			sim_=in_sim;
+			std::cout << " Change sim value:"<< std::endl;
+			if(sim_){
+				std::cout << " Change sim_=true"<< std::endl;
+			}else{
+				std::cout << " Change sim_=false"<< std::endl;
+			}
+
+		}
+
+
+	protected:
+		std::vector<Sdestination> destinations_;//prior scene destinations (all of them)
+		Crobot* robot_;//robot is in a different list than normal persons, and additionally we have its pointer for specific robot methods
+		Crobot* person_companion_; // for use the akp planner on the person companion.
+		std::list<Cperson_abstract *> person_list_;//list of persons in the scene, abstract containers
+		double dt_;
+		double now_;
+		std::vector<Spoint> laser_obstacle_list_;//list of obstacles seen by laser
+		bool read_laser_obstacle_success_;
+		//std::vector<Spoint> map_obstacle_list_;//TODO: static list of obstacles according to map
+		std::vector<Sforce> force_map_;
+		std::vector<bool> obstacle_map_;
+		//temporal occupancy matrix
+		std::vector<Spoint> xy_map_;
+		bool read_force_map_success_;
+		unsigned int map_number_x_,map_number_y_;
+		float min_x_,max_x_,min_y_,max_y_,map_resolution_;
+		bool read_destination_map_success_;
+		Cperson_abstract::force_type scene_force_type_;
+		std::vector<double> social_forces_param_to_person_, social_forces_param_to_robot_, social_forces_param_to_obs_,social_forces_param_to_person_companion_;
+		Cperson_abstract::filtering_method filtering_method_;
+		Cperson_abstract::update_person_method update_method_;
+		bool robot_in_the_scene_;
+		bool person_companion_in_the_scene_;
+
+		// read laser private methods
+		bool point_corresponds_person( Spoint laser_point);
+		bool point_corresponds_person_companion( Spoint laser_point );
+		bool point_belongs_to_laser_obstacle( Spoint p , bool person_simulation=false);
+
+		bool sim_bool_;
+		double max_d_to_detect_laser_obs_;
+		std::list<Cperson_abstract*> real_person_list_;
+		unsigned int max_asos_point_to_person_;
+
+		bool debug_update_person_companion_; // debug for person companion
+		bool debug_find_person_; // debug for person companion
+
+		bool sim_; // if sim=true case simulations, else if sim=false case real robot.
+
+
+};
+#endif
diff --git a/local_lib_people_prediction/src/scene_elements/obstacle.cpp b/local_lib_people_prediction/src/scene_elements/obstacle.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..9750afdcb8e7a8a544d30b8f1bb4d065d76704cd
--- /dev/null
+++ b/local_lib_people_prediction/src/scene_elements/obstacle.cpp
@@ -0,0 +1,17 @@
+/*
+ * obstacle.cpp
+ *
+ *  Created on: Jun 19, 2013.  Last Modified on 2020
+ *      Author: Initial code of the AKP Gonzalo Ferrer.
+ *      Author: Addition of ASSAOP and ASSAGP for people accompaniment code of Ely Repiso.
+ */
+
+
+#include "scene_elements/obstacle.h"
+
+Cobstacle::Cobstacle( Spose position ) :
+	Cperson_abstract(0)
+{
+	current_pointV_ = position;
+	best_destination_ = Sdestination();
+}
diff --git a/local_lib_people_prediction/src/scene_elements/obstacle.h b/local_lib_people_prediction/src/scene_elements/obstacle.h
new file mode 100644
index 0000000000000000000000000000000000000000..3ee836041ca87e8ee1d4b9eb1657017a9beaaee9
--- /dev/null
+++ b/local_lib_people_prediction/src/scene_elements/obstacle.h
@@ -0,0 +1,28 @@
+/*
+ * obstacle.h
+ *
+ *  Created on: Jun 19, 2013. Last Modified on 2020
+ *      Author: Initial code of the AKP Gonzalo Ferrer.
+ *      Author: Addition of ASSAOP and ASSAGP for people accompaniment code of Ely Repiso.
+ *
+ */
+
+#ifndef OBSTACLE_H_
+#define OBSTACLE_H_
+
+#include "iri_geometry.h"
+#include "scene_elements/person_abstract.h"
+
+class Cobstacle : public Cperson_abstract {
+public:
+	Cobstacle( Spose position );
+	~Cobstacle();
+	virtual void reset();
+	virtual void add_pose(){ }
+	virtual void prediction(double min_v_to_predict){}
+
+private:
+
+};
+
+#endif /* OBSTACLE_H_ */
diff --git a/local_lib_people_prediction/src/scene_elements/person_abstract.cpp b/local_lib_people_prediction/src/scene_elements/person_abstract.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e221b2e90239c95fb386cfffcda07904b31e1b7d
--- /dev/null
+++ b/local_lib_people_prediction/src/scene_elements/person_abstract.cpp
@@ -0,0 +1,1110 @@
+/*
+ * person_abstract.cpp
+ *
+ *  Created on: 2013.  Last Modified on 2020
+ *      Author: Initial code of the AKP Gonzalo Ferrer.
+ *      Author: Addition of ASSAOP and ASSAGP for people accompaniment code of Ely Repiso.
+ */
+
+#include "scene_elements/person_abstract.h"
+#include <list>
+#include <math.h>
+#include <algorithm>
+#include <iostream>
+
+
+
+Cperson_abstract::Cperson_abstract(  unsigned int id , Cperson_abstract::target_type target_type,
+		Cperson_abstract::force_type person_force_type, double _time_window):
+	person_id_( id ) ,
+	desired_velocity_(0.0),
+	type_(target_type),
+	person_force_type_(person_force_type),
+	observation_update_(false),
+	now_(0.0),
+	debug_velocity_file_(false),
+	externa_force_k_near_goal_(2.3),
+	externa_force_k_far_goal_(2.3),
+	max_dist_to_near_goal_force_(3),
+	debug_force_goal_(false)
+{
+	//std::cout  << " type_=" << type_ << std::endl;
+	//std::cout  << " person_id_=" << person_id_ << std::endl;
+
+}
+
+Cperson_abstract::~Cperson_abstract()
+{
+
+}
+
+bool Cperson_abstract::operator== (Cperson_abstract& p2) const
+{
+	if ( person_id_ == p2.get_id() && type_ == p2.get_person_type() )
+		return true;
+	else
+		return false;
+}
+
+
+SpointV_cov Cperson_abstract::pointV_propagation( double dt , Sforce force )
+{
+	//std::cout << std::endl << "  (pointV_propagation) now_- current_pointV_.time_stamp" << now_- current_pointV_.time_stamp<<" now_="<<now_ << "dt="<<dt<<"desired_velocity_"<<desired_velocity_ << std::endl;
+	//std::cout << std::endl << " current_pointV_.x=" << current_pointV_.x<< "; current_pointV_.y=" << current_pointV_.y<< "; current_pointV_.vy=" << current_pointV_.vy<<"; current_pointV_.vx=" << current_pointV_.vx<< std::endl;
+
+	return current_pointV_.propagate( now_- current_pointV_.time_stamp + dt , force , desired_velocity_);
+}
+
+SpointV_cov Cperson_abstract::pointV_propagation( double dt , Sforce force, SpointV_cov point )
+{
+	//std::cout << std::endl << "  (pointV_propagation) now_- current_pointV_.time_stamp" << now_- current_pointV_.time_stamp << "dt="<<dt<<"desired_velocity_"<<desired_velocity_ << std::endl;
+	return point.propagate( now_- current_pointV_.time_stamp + dt,force, desired_velocity_ );
+}
+
+
+void Cperson_abstract::print()
+{
+	std::cout << std::endl << "              last pose " << person_id_ << "th print :" << std::endl;
+	current_pointV_.print();
+	return;
+}
+
+void Cperson_abstract::print_dest()
+{
+	std::cout << std::endl << "                  Best destination of person " << person_id_ << std::endl;
+	best_destination_.print();
+}
+
+Sforce Cperson_abstract::force_ellip( const SpointV& interacting_person ,  const std::vector<double>* social_forces_param, const SpointV* virtual_current_point )
+{
+
+	//if force is calculated from a propagated pose, then virtual_current_point should be pointing to a valid SpointV.
+	//Otherwise, current point is current point class variable at time "now"
+	const SpointV* current_point;
+	if ( virtual_current_point == NULL  )
+		current_point = &current_pointV_;
+	else
+		current_point = virtual_current_point;
+
+	//setting the corresponding SFM parameters:
+	// if no forces param provided, exit program
+	assert( social_forces_param != NULL );
+	assert( social_forces_param->size() >= 5 );
+
+	Sforce force;
+	//geometry calculations
+	double tau = social_forces_param->at(4);
+	SpointV pose_dif = (SpointV)(*current_point) - interacting_person;
+	Spoint d_point( pose_dif.x,pose_dif.y );
+	Spoint v_current = Spoint( current_point->vx , current_point->vy );
+	Spoint v_int = Spoint( interacting_person.vx , interacting_person.vy );
+
+	//force module calculation
+	double b = d_point.distance() + (d_point - ( v_int - v_current )*tau ).distance();
+	b = 0.5* sqrt( ( b*b - (( v_int - v_current )*tau ).distance() * (( v_int - v_current )*tau ).distance()
+			) / ( 1 + v_current.distance() * tau ) );
+	double f = social_forces_param->at(2)* exp( (- b) / social_forces_param->at(3));
+	f = f * (d_point.distance() + (d_point - ( v_int - v_current )*tau ).distance()) / sqrt( 1 + v_current.distance() * tau ) / (4*b);
+	//force direction
+	double phi = current_point->angle_heading_point( interacting_person );
+	double anisotropy = (social_forces_param->at(1) + (1-social_forces_param->at(1))*(1 + cos(phi))/2 );
+	double dx,dy;
+	if ( (( v_int - v_current )*tau).distance() > 0.01 )
+	{
+	dx = d_point.x/d_point.distance() + (( v_int - v_current )*tau).x / (( v_int - v_current )*tau).distance() ;
+	dy = d_point.y/d_point.distance() + (( v_int - v_current )*tau).y / (( v_int - v_current )*tau).distance() ;
+	}
+	else
+	{
+		dx = d_point.x/d_point.distance();
+		dy = d_point.y/d_point.distance();
+	}
+	force.fx = f * dx * anisotropy;
+	force.fy = f * dy * anisotropy;
+	return force;
+}
+
+Sforce Cperson_abstract::force_cp( const SpointV& interacting_person ,  const std::vector<double>* social_forces_param, const SpointV* virtual_current_point )
+{
+
+
+	//if force is calculated from a propagated pose, then virtual_current_point won't be null.
+	//Otherwise, current point is current point class variable
+	const SpointV* current_point;
+	if ( virtual_current_point == NULL  )
+		current_point = &current_pointV_;
+	else
+		current_point = virtual_current_point;
+
+	//setting the corresponding SFM parameters:
+	// if no forces param provided, exit program
+	assert( social_forces_param != NULL );
+	assert( social_forces_param->size() >= 5 );
+
+	Sforce force;
+	double dx = current_point->x - interacting_person.x;
+	double dy = current_point->y - interacting_person.y;
+	double vx = current_point->vx;
+	double vx_i = interacting_person.vx;
+	double dvx =  vx - vx_i;
+	double vy = current_point->vy;
+	double vy_i = interacting_person.vy;
+	double dvy = vy - vy_i;
+	double time_minimum_distance = - (dx*dvx + dy*dvy ) / (dvx*dvx + dvy*dvy);
+
+	//propagating state-
+	double dx_prop = current_point->x + time_minimum_distance * vx -
+			interacting_person.x - time_minimum_distance * vx_i;
+	double dy_prop = current_point->y + time_minimum_distance * vy -
+			interacting_person.y - time_minimum_distance * vy_i;
+	double d_cp = dx_prop * dx_prop + dy_prop * dy_prop;
+
+	//exponential force module f = A*v/t* exp(d_cp / B)
+	double f;
+	if ( time_minimum_distance > 0  &&
+		fabs(diffangle(diffangle(current_point->orientation(),interacting_person.orientation()) , atan2( dvy , dvx ) )) < PI/2 )
+	{
+		if (time_minimum_distance < 0.2 ) time_minimum_distance = 0.2;
+		if (d_cp < 0.36)
+		{
+			dx_prop = dx_prop / d_cp * 0.36;//divide mal... too bad a corregir mañana
+			dy_prop = dy_prop / d_cp * 0.36;
+			d_cp = 0.36;//0.3 m radii per person
+		}
+		f = social_forces_param->at(2)*current_point->v() / time_minimum_distance * exp(- d_cp / social_forces_param->at(3));
+		//force direction
+		double phi = diffangle( atan2(vy,vx), atan2(dy_prop,dx_prop) );
+		double anisotropy =  (social_forces_param->at(1) + (1-social_forces_param->at(1))*(1 + cos(phi))/2 );
+		force.fx = f * dx_prop * anisotropy;
+		force.fy = f * dy_prop * anisotropy;
+	}
+
+	return force;
+}
+
+Sforce Cperson_abstract::force_sphe( const Spoint& interacting_person ,  const std::vector<double>* social_forces_param , const SpointV* virtual_current_point, Cperson_abstract::companion_reactive reactive )
+{ // ESTE ES EL QUE USA GONZALO!!!
+
+	Sforce force,force2;
+	double f,d;
+
+	//if force is calculated from a propagated pose, then virtual_current_point won't be null.
+	//Otherwise, current point is current point class variable
+	//std::cout << " IN :Cperson_abstract::force_sphe => case(Reactiva_repulsive+atractive) " <<  std::endl;
+
+
+	//std::cout << "IN force_sphe!!!;   target_type="<< type_<< std::endl;
+
+	const SpointV* current_point;
+	if ( virtual_current_point == NULL  )
+		current_point = &current_pointV_;
+	else
+		current_point = virtual_current_point;
+
+	//setting the corresponding SFM parameters:
+	// if no forces param provided, exit program
+	assert( social_forces_param != NULL );
+	assert( social_forces_param->size() >= 5 );
+
+	//geometry calculations
+	double dx = current_point->x - interacting_person.x;
+	double dy = current_point->y - interacting_person.y;
+	double vx = current_point->vx;
+	double vy = current_point->vy;
+	d = sqrt(dx*dx+dy*dy);
+	dx /= d;
+	dy /= d;
+
+
+
+	/*if(d<1.25){
+		std::cout << " [FFFFORCEEEEE] social_forces_param->at(2)="<<social_forces_param->at(2)<<"; social_forces_param->at(4)="<< social_forces_param->at(4) <<"; social_forces_param->at(3)="<<social_forces_param->at(3)<<"; social_forces_param->at(1)="<<social_forces_param->at(1)<<  std::endl;
+		social_forces_param->at(2)=7;
+		 social_forces_param->at(4)=0.2;
+		 social_forces_param->at(3)=0.91;
+
+	}*/
+
+
+	double anisotropy;
+	double phi;
+
+	//std::cout << "(reactive atractive) IN force_sphe!!! target_type="<<type_<< std::endl;
+	//force module calculation
+	f = social_forces_param->at(2)* exp((social_forces_param->at(4)-d) / social_forces_param->at(3));
+
+	phi = diffangle( atan2(vy,vx), atan2(-dy,-dx) );//minus difference vector
+
+	anisotropy = (social_forces_param->at(1) + (1-social_forces_param->at(1))*(1 + cos(phi))/2 );
+	// [lambda + (1- lambda)*((1-cos(phi))/2)]
+
+	switch(reactive)
+	{
+
+		case Reactive_atractive:
+			//std::cout << "(reactive atractive) IN force_sphe!!! person_id_="<<person_id_<< std::endl;
+			//force module calculation
+				f = social_forces_param->at(2)* exp((social_forces_param->at(4)-d) / social_forces_param->at(3));
+			//std::cout << "social_forces_param->at(1)="<<social_forces_param->at(1)<<"; social_forces_param->at(2)="<<social_forces_param->at(2)<<"; social_forces_param->at(3)="<<social_forces_param->at(3)<<"; social_forces_param->at(4)="<<social_forces_param->at(4)<< std::endl;
+
+
+				//std::cout << " [FFFFORCEEEEE] social_forces_param->at(2)="<<social_forces_param->at(2)<<"; social_forces_param->at(4)="<< social_forces_param->at(4) <<"; social_forces_param->at(3)="<<social_forces_param->at(3)<<"; social_forces_param->at(1)="<<social_forces_param->at(1)<<  std::endl;
+
+				// Aj*exp((dj-d)/Bj)
+				//force direction
+				phi = diffangle( atan2(vy,vx), atan2(-dy,-dx) );//minus difference vector
+
+				anisotropy = (social_forces_param->at(1) + (1-social_forces_param->at(1))*(1 + cos(phi))/2 );
+
+
+			//std::cout << " IN :Cperson_abstract::force_sphe => case: Reactive_atractive " <<  std::endl;
+			force.fx = (f * dx * anisotropy);
+			force.fy = (f * dy * anisotropy);
+		break;
+
+		case Reactiva_repulsive:
+		case Akp_planning:
+		default:
+			//std::cout << "(Reactiva_repulsive) IN force_sphe!!!"<< std::endl;
+			//std::cout << "social_forces_param->at(1)="<<social_forces_param->at(1)<<"; social_forces_param->at(2)="<<social_forces_param->at(2)<<"; social_forces_param->at(3)="<<social_forces_param->at(3)<<"; social_forces_param->at(4)="<<social_forces_param->at(4)<<"; d="<<d<< std::endl;
+			//force module calculation
+				f = social_forces_param->at(2)* exp((social_forces_param->at(4)-d) / social_forces_param->at(3));
+
+				//std::cout << " [FFFFORCEEEEE] AKP planning; social_forces_param->at(2)="<<social_forces_param->at(2)<<"; social_forces_param->at(4)="<< social_forces_param->at(4) <<"; social_forces_param->at(3)="<<social_forces_param->at(3)<<"; social_forces_param->at(1)="<<social_forces_param->at(1)<<  std::endl;
+
+				// Aj*exp((dj-d)/Bj)
+				//force direction
+				phi = diffangle( atan2(vy,vx), atan2(-dy,-dx) );//minus difference vector
+
+				anisotropy = (social_forces_param->at(1) + (1-social_forces_param->at(1))*(1 + cos(phi))/2 );
+
+			//std::cout << " IN :Cperson_abstract::force_sphe => case: Reactive_repulsive and AKP_planning " <<  std::endl;
+			force.fx = f * dx * anisotropy;
+			force.fy = f * dy * anisotropy;
+			//std::cout << " IN :Cperson_abstract::force_sphe => force.fx="<<force.fx<<"; force.fy="<<force.fy <<  std::endl;
+		break;
+	}
+
+
+	//std::cout << " [FFFFORCEEEEE] force2.fx="<<force2.fx<<"force2.fy="<< force2.fy<<  std::endl;
+
+	return force;
+}
+
+
+Sforce Cperson_abstract::force( const SpointV& interacting_person ,  const std::vector<double>* social_forces_param, const SpointV* virtual_current_point, Cperson_abstract::companion_reactive reactive )
+{
+
+	Sforce force_res;
+	switch(reactive)
+	{
+
+		case Reactiva_repulsive:
+		case Reactive_atractive:
+			//std::cout << " IN :Cperson_abstract::force => case(Reactiva_repulsive+atractive) " <<  std::endl;
+			assert( !social_forces_param->empty() );
+			switch(person_force_type_)
+			{
+				case Elliptical:
+					force_res = force_ellip(  interacting_person ,  social_forces_param, virtual_current_point  );
+					break;
+				case Collision_Prediction:
+					force_res = force_cp(  interacting_person ,  social_forces_param, virtual_current_point );
+					break;
+				case Spherical:
+				default:
+					force_res = force_sphe(  interacting_person ,  social_forces_param, virtual_current_point , reactive );
+					break;
+			}
+		break;
+
+		case Akp_planning:
+		default:
+			assert( !social_forces_param->empty() );
+			switch(person_force_type_)
+			{
+			case Elliptical:
+				force_res = force_ellip(  interacting_person ,  social_forces_param, virtual_current_point  );
+				break;
+			case Collision_Prediction:
+				force_res = force_cp(  interacting_person ,  social_forces_param, virtual_current_point );
+				break;
+			case Spherical:
+			default:
+				force_res = force_sphe(  interacting_person ,  social_forces_param, virtual_current_point  );
+				break;
+			}
+		break;
+
+	}
+	return force_res;
+}
+
+
+
+Sforce Cperson_abstract::forceAnticipateCollision( force_type act_person_force_type, const SpointV& interacting_person ,  const std::vector<double>* social_forces_param, const SpointV* virtual_current_point, Cperson_abstract::companion_reactive reactive )
+{
+
+	Sforce force_res;
+	switch(reactive)
+	{
+
+		case Reactiva_repulsive:
+		case Reactive_atractive:
+			//std::cout << " IN :Cperson_abstract::force => case(Reactiva_repulsive+atractive) " <<  std::endl;
+			assert( !social_forces_param->empty() );
+			switch(act_person_force_type)
+			{
+				case Elliptical:
+					force_res = force_ellip(  interacting_person ,  social_forces_param, virtual_current_point  );
+					break;
+				case Collision_Prediction:
+					force_res = force_cp(  interacting_person ,  social_forces_param, virtual_current_point );
+					break;
+				case Spherical:
+				default:
+					force_res = force_sphe(  interacting_person ,  social_forces_param, virtual_current_point , reactive );
+					break;
+			}
+		break;
+
+		case Akp_planning:
+		default:
+			assert( !social_forces_param->empty() );
+			switch(act_person_force_type)
+			{
+			case Elliptical:
+				force_res = force_ellip(  interacting_person ,  social_forces_param, virtual_current_point  );
+				break;
+			case Collision_Prediction:
+				force_res = force_cp(  interacting_person ,  social_forces_param, virtual_current_point );
+				break;
+			case Spherical:
+			default:
+				force_res = force_sphe(  interacting_person ,  social_forces_param, virtual_current_point  );
+				break;
+			}
+		break;
+
+	}
+	return force_res;
+}
+
+Sforce Cperson_abstract::force_goal(  const Sdestination& dest, const std::vector<double>* social_forces_param, const SpointV* virtual_current_point, unsigned int parent_vertex, bool output_mesages )
+{
+
+	if(parent_vertex<0){
+		std::cout << "IN (Cperson_abstract::force_goal); k="<<social_forces_param->at(0)<<"; desired_velocity_="<<desired_velocity_ << std::endl;
+	}
+
+	//if there are no is inferred, then no force to goal is possible (linear propagation + interaction forces)
+	if ( dest.type == Sdestination::Uncertain  ) return Sforce();
+
+	//std::cout << "(force_goal) 2=" << std::endl;
+
+	//if force is calculated from a propagated pose, then index would be different form zero and
+	// the prediction vector should not be empty
+	const SpointV* current_point;
+
+	if ( virtual_current_point == NULL ){
+		//std::cout << "SI NULL pointer=" << std::endl;
+		current_point = &current_pointV_;
+	}
+	else{
+		//std::cout << " no null pointer=" << std::endl;
+		current_point = virtual_current_point;
+	}
+
+	//std::cout << "(force_goal) 3=" << std::endl;
+	//setting the target type and its corresponding parameters:
+	// if no forces param provided, exit program
+	assert( social_forces_param != NULL );
+	assert( social_forces_param->size() >= 5 );
+
+	//std::cout << "(force_goal) 4=" << std::endl;
+	double v_desired,v_desired_x,v_desired_y;
+	double act_module_dist=0.0;
+	if ( dest.type == Sdestination::Map_goal  )
+	{
+		if(debug_force_goal_){
+			std::cout << "MAP GOAL if( dest.type == Sdestination::Map_goal ); (dest.x="<<dest.x<<"; dest.y="<<dest.y<<")=(current_point->x="<<current_point->x<<"; current_point->y="<<current_point->y<<")" << std::endl;
+		}
+
+		v_desired_x = dest.x - current_point->x;
+		v_desired_y = dest.y - current_point->y;
+
+		if(parent_vertex<0){
+			std::cout << "(MAP GOAL if) v_desired_x="<<v_desired_x<< "; dest.x ="<<dest.x <<"; current_point->x="<<current_point->x <<"; desired_velocity_="<<desired_velocity_ << std::endl;
+			std::cout << "(MAP GOAL if) v_desired_y="<<v_desired_y<< "; dest.y ="<<dest.y <<"; current_point->y="<<current_point->y << std::endl;
+		}
+
+		v_desired = sqrt(v_desired_x*v_desired_x + v_desired_y*v_desired_y);
+		act_module_dist=v_desired;
+
+		if(debug_force_goal_){
+			std::cout << "(ini iter!) v_desired="<<v_desired<< std::endl;
+			std::cout << " desired_velocity_="<<desired_velocity_ << std::endl;
+		}
+
+		v_desired_x *= desired_velocity_/v_desired;
+		v_desired_y *= desired_velocity_/v_desired;
+
+
+		if(parent_vertex<0){
+			std::cout << "v_desired_x="<<v_desired_x<<"v_desired_y="<<v_desired_y<< std::endl;
+			std::cout <<"; desired_velocity_="<<desired_velocity_<<"; v_desired="<<v_desired<<  std::endl;
+		}
+	}
+	else //Sdestination::Stopping
+	{
+		if(output_mesages){
+			std::cout << "else if( dest.type == Sdestination::Map_goal ) v_desired_x=v_desired_y=0" << std::endl;
+		}
+
+		v_desired_x = 0.0;
+		v_desired_y = 0.0;
+	}
+
+
+	//std::cout << "(force_goal) 5=" << std::endl;
+	if(output_mesages){
+		if(parent_vertex<2){
+		std::cout << "k=social_forces_param->at(0)="<<social_forces_param->at(0) <<"; v_desired_x="<<v_desired_x<<"; v_desired_y="<<v_desired_y<<"; current_point->vx="<<current_point->vx<<"; current_point->vy="<<current_point->vy<< std::endl;
+		std::cout << "(v_desired_x - current_point->vx)="<<(v_desired_x - current_point->vx) <<"; social_forces_param->at(0)*(v_desired_x - current_point->vx)="<<social_forces_param->at(0)*(v_desired_x - current_point->vx)<< std::endl;
+		std::cout << "(v_desired_y - current_point->vy)="<<(v_desired_y - current_point->vy) <<"; social_forces_param->at(0)*(v_desired_y - current_point->vy)="<<social_forces_param->at(0)*(v_desired_y - current_point->vy)<< std::endl;
+		}
+	}
+
+
+
+	v_desired_robot_x_.push_back(v_desired_x);
+	v_desired_robot_y_.push_back(v_desired_y);
+
+	v_current_robot_x_.push_back(current_point->vx);
+	v_current_robot_y_.push_back(current_point->vy);
+
+	actual_random_goal_x_.push_back(dest.x);
+	actual_random_goal_y_.push_back(dest.y);
+
+
+    actual_distance_in_x_.push_back(dest.x - current_point->x);
+   	actual_distance_in_y_.push_back(dest.y - current_point->y);
+	actual_module_distance_.push_back(act_module_dist);
+	actual_in_desired_velocity_.push_back(desired_velocity_);
+
+	force_to_goal_ = Sforce(	social_forces_param->at(0) * (v_desired_x - current_point->vx),
+			social_forces_param->at(0) * (v_desired_y - current_point->vy));
+
+
+	if(parent_vertex<0){
+		std::cout << "(force_goal) => v_desired_x="<<v_desired_x<<"; v_desired_y="<<v_desired_y <<"; dest.x="<<dest.x<<"; dest.y="<<dest.y<< std::endl;
+		std::cout << "(force_goal) => force_to_goal_.fx="<<force_to_goal_.fx<<"; force_to_goal_.fy="<<force_to_goal_.fy << std::endl;
+		std::cout << "(force_goal) => current_point->vx="<<current_point->vx<<"; current_point->vy="<<current_point->vy << std::endl;
+	}
+
+
+	//std::cout << " k=social_forces_param->at(0)="<<social_forces_param->at(0) << std::endl;
+
+	return  force_to_goal_;
+}
+
+
+Sforce Cperson_abstract::force_goal_companion(  const Sdestination& dest, const std::vector<double>* social_forces_param, const SpointV* virtual_current_point, unsigned int parent_vertex, bool output_mesages )
+{
+
+	if(parent_vertex<0){
+		std::cout << "IN (Cperson_abstract::force_goal); k="<<social_forces_param->at(0)<<"; desired_velocity_="<<desired_velocity_ << std::endl;
+	}
+
+	//if there are no is inferred, then no force to goal is possible (linear propagation + interaction forces)
+	if ( dest.type == Sdestination::Uncertain  ) return Sforce();
+
+	//std::cout << "(force_goal) 2=" << std::endl;
+
+	//if force is calculated from a propagated pose, then index would be different form zero and
+	// the prediction vector should not be empty
+	const SpointV* current_point;
+
+	if ( virtual_current_point == NULL ){
+		//std::cout << "SI NULL pointer=" << std::endl;
+		current_point = &current_pointV_;
+	}
+	else{
+		//std::cout << " no null pointer=" << std::endl;
+		current_point = virtual_current_point;
+	}
+
+	//std::cout << "(force_goal) 3=" << std::endl;
+	//setting the target type and its corresponding parameters:
+	// if no forces param provided, exit program
+	assert( social_forces_param != NULL );
+	assert( social_forces_param->size() >= 5 );
+
+	//std::cout << "(force_goal) 4=" << std::endl;
+	double v_desired,v_desired_x,v_desired_y;
+	double act_module_dist=0.0;
+	if ( dest.type == Sdestination::Map_goal  )
+	{
+		if(debug_force_goal_){
+			std::cout << "MAP GOAL if( dest.type == Sdestination::Map_goal ); (dest.x="<<dest.x<<"; dest.y="<<dest.y<<")=(current_point->x="<<current_point->x<<"; current_point->y="<<current_point->y<<")" << std::endl;
+		}
+
+		v_desired_x = (dest.x - current_point->x); // dt_=0.2 (luego cogerlo de fuera.
+		v_desired_y = (dest.y - current_point->y);
+
+		double v_desired_module=sqrt((dest.x - current_point->x)*(dest.x - current_point->x)+(dest.y - current_point->y)*(dest.y - current_point->y));
+
+		if(parent_vertex<0){
+			std::cout << "(MAP GOAL if) v_desired_x="<<v_desired_x<< "; dest.x ="<<dest.x <<"; current_point->x="<<current_point->x <<"; desired_velocity_="<<desired_velocity_ << std::endl;
+			std::cout << "(MAP GOAL if) v_desired_y="<<v_desired_y<< "; dest.y ="<<dest.y <<"; current_point->y="<<current_point->y << std::endl;
+		}
+
+		v_desired = sqrt(v_desired_x*v_desired_x + v_desired_y*v_desired_y);
+		act_module_dist=v_desired;
+
+		if(debug_force_goal_){
+			std::cout << "(ini iter!) v_desired="<<v_desired<< std::endl;
+			std::cout << " desired_velocity_="<<desired_velocity_ << std::endl;
+		}
+
+
+		v_desired_module=desired_velocity_;
+
+
+		v_desired_x *= v_desired_module/v_desired;
+		v_desired_y *= v_desired_module/v_desired;
+
+
+
+		if(parent_vertex<0){
+			std::cout << "v_desired_x="<<v_desired_x<<"v_desired_y="<<v_desired_y<<"; v_desired_module="<<v_desired_module<< std::endl;
+			std::cout <<"; desired_velocity_="<<desired_velocity_<<"; v_desired="<<v_desired<<  std::endl;
+		}
+	}
+	else //Sdestination::Stopping
+	{
+		if(output_mesages){
+			std::cout << "else if( dest.type == Sdestination::Map_goal ) v_desired_x=v_desired_y=0" << std::endl;
+		}
+
+		v_desired_x = 0.0;
+		v_desired_y = 0.0;
+	}
+
+
+	//std::cout << "(force_goal) 5=" << std::endl;
+	if(output_mesages){
+		if(parent_vertex<2){
+		std::cout << "k=social_forces_param->at(0)="<<social_forces_param->at(0) <<"; v_desired_x="<<v_desired_x<<"; v_desired_y="<<v_desired_y<<"; current_point->vx="<<current_point->vx<<"; current_point->vy="<<current_point->vy<< std::endl;
+		std::cout << "(v_desired_x - current_point->vx)="<<(v_desired_x - current_point->vx) <<"; social_forces_param->at(0)*(v_desired_x - current_point->vx)="<<social_forces_param->at(0)*(v_desired_x - current_point->vx)<< std::endl;
+		std::cout << "(v_desired_y - current_point->vy)="<<(v_desired_y - current_point->vy) <<"; social_forces_param->at(0)*(v_desired_y - current_point->vy)="<<social_forces_param->at(0)*(v_desired_y - current_point->vy)<< std::endl;
+		}
+	}
+
+
+
+	v_desired_robot_x_.push_back(v_desired_x);
+	v_desired_robot_y_.push_back(v_desired_y);
+
+	v_current_robot_x_.push_back(current_point->vx);
+	v_current_robot_y_.push_back(current_point->vy);
+
+	actual_random_goal_x_.push_back(dest.x);
+	actual_random_goal_y_.push_back(dest.y);
+
+
+    actual_distance_in_x_.push_back(dest.x - current_point->x);
+   	actual_distance_in_y_.push_back(dest.y - current_point->y);
+	actual_module_distance_.push_back(act_module_dist);
+	actual_in_desired_velocity_.push_back(desired_velocity_);
+
+	force_to_goal_ = Sforce(	social_forces_param->at(0) * (v_desired_x - current_point->vx),
+			social_forces_param->at(0) * (v_desired_y- current_point->vy));
+
+
+	if(parent_vertex<0){
+		std::cout << "(force_goal) => v_desired_x="<<v_desired_x<<"; v_desired_y="<<v_desired_y <<"; dest.x="<<dest.x<<"; dest.y="<<dest.y<< std::endl;
+		std::cout << "(force_goal) => force_to_goal_.fx="<<force_to_goal_.fx<<"; force_to_goal_.fy="<<force_to_goal_.fy << std::endl;
+		std::cout << "(force_goal) => current_point->vx="<<current_point->vx<<"; current_point->vy="<<current_point->vy << std::endl;
+	}
+
+
+	//std::cout << " k=social_forces_param->at(0)="<<social_forces_param->at(0) << std::endl;
+
+	return  force_to_goal_;
+}
+
+
+
+Sforce Cperson_abstract::force_goal_near(  const Sdestination& dest, const std::vector<double>* social_forces_param, const SpointV* virtual_current_point, double d_max_reduce_vel , bool in_go_with_vel_per, double real_person_vel)
+{
+
+	//std::cout << "  force_goal_near!!! " << std::endl;
+
+	/*if(real_person_vel==0.9){
+		real_person_vel=desired_velocity_;
+	}*/
+
+	double act_module_dist=0.0;
+	//if there are no is inferred, then no force to goal is possible (linear propagation + interaction forces)
+	if ( dest.type == Sdestination::Uncertain  ) return Sforce();
+
+	//if force is calculated from a propagated pose, then index would be different form zero and
+	// the prediction vector should not be empty
+	const SpointV* current_point;
+	if ( virtual_current_point == NULL )
+		current_point = &current_pointV_;
+	else
+		current_point = virtual_current_point;
+
+	//setting the target type and its corresponding parameters:
+	// if no forces param provided, exit program
+	assert( social_forces_param != NULL );
+	assert( social_forces_param->size() >= 5 );
+
+
+	Spoint robot_act_pose=Spoint(current_point->x,current_point->y,current_point->time_stamp);
+	Spoint goal_act_pose=Spoint(dest.x,dest.y,dest.time_stamp);
+	double dist_to_near_goal= robot_act_pose.distance( goal_act_pose);
+
+
+	double v_desired,v_desired_x,v_desired_y;
+	if ( dest.type == Sdestination::Map_goal  )
+	{
+		v_desired_x = dest.x - current_point->x;
+		v_desired_y = dest.y - current_point->y;
+
+		/*std::cout << "dest.x="<<dest.x<<"; current_point->x="<<current_point->x<< std::endl;
+		std::cout << "dest.x="<<dest.x<<"; current_point->y="<<current_point->y<< std::endl;
+		std::cout << "v_desired_x="<<v_desired_x<<"; v_desired_y="<<v_desired_y<< std::endl;
+		 */
+		/*if(debug_velocity_){
+			std::cout << "(IFFFFF) v_desired_x="<<v_desired_x<< "; dest.x ="<<dest.x <<"; current_point->x="<<current_point->x << std::endl;
+			std::cout << "v_desired_y="<<v_desired_y<< "; dest.y ="<<dest.y <<"; current_point->y="<<current_point->y << std::endl;
+		}*/
+
+		v_desired = sqrt(v_desired_x*v_desired_x + v_desired_y*v_desired_y);
+		act_module_dist=v_desired;
+		//std::cout << "v_desired="<<v_desired<< std::endl;
+		/*if(debug_velocity_){
+			std::cout << "(if) v_desired="<<v_desired<< std::endl;
+			std::cout << "desired_velocity_="<<desired_velocity_ << std::endl;
+		}*/
+
+
+		double desired_vel_act;
+
+		/*if(dist_to_near_goal>d_max_reduce_vel){
+			vel_per_ok_in=Far_goal;
+		}*/
+
+/*
+		switch(vel_per_ok_in)
+		{
+			case Vel_per:
+				std::cout << "case Cplan_local_nav::Vel_per:"<< std::endl;
+				desired_vel_act=desired_velocity_;
+				break;
+
+			case Near_goal:
+				std::cout << "case Cplan_local_nav::Near_goal:"<< std::endl;
+				desired_vel_act=desired_velocity_ * dist_to_near_goal / d_max_reduce_vel;
+				std::cout << "desired_velocity_="<<desired_velocity_<<"; dist_to_near_goal="<<dist_to_near_goal<< std::endl;
+				if(desired_vel_act>desired_velocity_){
+					desired_vel_act=desired_velocity_;
+				}
+
+				break;
+
+			case Far_goal:
+			 default :
+				 std::cout << "case Cplan_local_nav::Far_goal:"<< std::endl;
+				 desired_vel_act=desired_velocity_;
+				break;
+
+		}*/
+		// debug: debug_sideBySide2019_
+//std::cout << "[velocities last step goal] d_max_reduce_vel="<<d_max_reduce_vel<<"; dist_to_near_goal="<<dist_to_near_goal<<"; in_go_with_vel_per"<<in_go_with_vel_per<<"; desired_velocity_="<<desired_velocity_<< std::endl;
+
+		if((dist_to_near_goal<d_max_reduce_vel)&&(!in_go_with_vel_per)){ //robot_person_proximity_distance + 0.5 (TODO: habra que entrar esas distancias para hacerlo bien!)
+		//if(!vel_per_ok){ //robot_person_proximity_distance + 0.5 (TODO: habra que entrar esas distancias para hacerlo bien!)
+
+			desired_vel_act=desired_velocity_ * dist_to_near_goal / d_max_reduce_vel;
+
+
+			if(desired_vel_act>desired_velocity_){
+				desired_vel_act=desired_velocity_;
+			}
+
+			/*if(desired_vel_act<real_person_vel){
+				desired_vel_act=real_person_vel;
+			}*/
+
+			//std::cout << "(case near goal) desired_velocity_="<<desired_velocity_<<"; dist_to_near_goal="<<dist_to_near_goal<<"; desired_vel_act="<<desired_vel_act<<";  d_max_reduce_vel="<< d_max_reduce_vel<<"; dist_to_near_goal="<<dist_to_near_goal<<";desired_vel_act="<<desired_vel_act<< std::endl;
+
+
+			if(debug_velocity_file_){
+				std::ofstream fileMatlab2;
+				fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+				fileMatlab2 << "(case near goal) desired_velocity_="<<desired_velocity_<<"; dist_to_near_goal="<<dist_to_near_goal<< "\n";
+				fileMatlab2 << "(case near goal) desired_velocity_ * dist_to_near_goal / d_max_reduce_vel="<<desired_velocity_ * dist_to_near_goal / d_max_reduce_vel<< "\n";
+				fileMatlab2.close();
+			}
+
+			//std::cout << "(case near goal) desired_velocity_="<<desired_velocity_<<"; dist_to_near_goal="<<dist_to_near_goal<<"; desired_vel_act="<<desired_vel_act<< std::endl;
+
+
+
+
+
+
+
+		}else{
+
+			desired_vel_act=desired_velocity_;//* dist_to_near_goal / d_max_reduce_vel;
+			//desired_vel_act=desired_velocity_; //en teoria para este caso esta es la buena, pero habria que ampliar el margen de diferencia entre angulos de 10 a 15, en el caso companion recto.
+
+
+			//std::cout << "(case person or far_goal) desired_velocity_="<<desired_velocity_<<"; dist_to_near_goal="<<dist_to_near_goal<< std::endl;
+
+			if(debug_velocity_file_){
+				std::ofstream fileMatlab2;
+				fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+				fileMatlab2 << "(case person or far_goal) desired_velocity_="<<desired_velocity_<<"; dist_to_near_goal="<<dist_to_near_goal<< "\n";
+				fileMatlab2.close();
+				//std::cout << "(case near goal) desired_velocity_="<<desired_velocity_<<"; dist_to_near_goal="<<dist_to_near_goal<< std::endl;
+
+			}
+
+		}
+
+
+		v_desired_x *= desired_vel_act/v_desired;
+		v_desired_y *= desired_vel_act/v_desired;
+
+
+		//std::cout << " v_desired_x="<<v_desired_x<< std::endl;
+		//std::cout << "v_desired_y="<<v_desired_y<< std::endl;
+
+	}
+	else //Sdestination::Stopping
+	{
+		v_desired_x = 0.0;
+		v_desired_y = 0.0;
+		// debug_sideBySide2019_
+		//std::cout << "//Sdestination::Stopping! v_desired_x="<<v_desired_x<<"; v_desired_y"<<v_desired_y<< std::endl;
+	}
+
+
+	/*if(debug_velocity_){
+		std::cout << "current_point->vx="<<current_point->vx<<"; current_point->vy"<<current_point->vy<< std::endl;
+		std::cout << "fx="<<social_forces_param->at(0) * (v_desired_x - current_point->vx)<<"; fy"<<social_forces_param->at(0) * (v_desired_y - current_point->vy)<< std::endl;
+	}
+
+
+	if(debug_velocity_){
+		std::cout << "(ely) vx_desired=(dest.x - current_point->x)/0.2="<<(dest.x - current_point->x)/0.2<<"; vy_desired=(dest.y- current_point->y)/0.2="<<(dest.y - current_point->y)/0.2<< std::endl;
+	}*/
+
+	//std::cout << "k=social_forces_param->at(0)="<<social_forces_param->at(0) <<"; v_desired_x="<<v_desired_x<<"; v_desired_y="<<v_desired_y<<"; current_point->vx="<<current_point->vx<<"; current_point->vy="<<current_point->vy<< std::endl;
+	double act_k;
+	if(dist_to_near_goal<max_dist_to_near_goal_force_){ // max_dist_to_near_goal_force_=3
+		act_k=externa_force_k_near_goal_;//2.7;
+		//std::cout << "act_k="<<act_k<<"; max_dist_to_near_goal_force_="<<max_dist_to_near_goal_force_<<"; dist_to_near_goal="<<dist_to_near_goal<< std::endl;
+	}else{
+		act_k=externa_force_k_far_goal_;//2.3;
+		//std::cout << "act_k="<<act_k<<"; max_dist_to_near_goal_force_="<<max_dist_to_near_goal_force_<<"; dist_to_near_goal="<<dist_to_near_goal<< std::endl;
+	}
+	//act_k=2.3;
+
+
+
+
+
+
+	v_desired_robot_x_.push_back(v_desired_x);
+	v_desired_robot_y_.push_back(v_desired_y);
+	//v_current_robot_x_.push_back(dest.y - current_point->y);
+	//v_current_robot_y_.push_back(v_desired);
+	v_current_robot_x_.push_back(current_point->vx);
+	v_current_robot_y_.push_back(current_point->vy);
+	//actual_random_goal_x_.push_back(desired_velocity_);
+	actual_random_goal_x_.push_back(dest.x);
+	actual_random_goal_y_.push_back(dest.y);
+	//actual_random_goal_y_.push_back(dest.x - current_point->x);
+
+
+
+    actual_distance_in_x_.push_back(dest.x - current_point->x);
+   	actual_distance_in_y_.push_back(dest.y - current_point->y);
+	actual_module_distance_.push_back(act_module_dist);
+	actual_in_desired_velocity_.push_back(desired_velocity_);
+
+	force_to_goal_ = Sforce(	act_k * (v_desired_x - current_point->vx),
+			act_k * (v_desired_y - current_point->vy));
+	//std::cout<<"; act_k="<<act_k << "force_to_goal_.fx="<<force_to_goal_.fx<<"; force_to_goal_.fy="<<force_to_goal_.fy<< std::endl;
+
+
+	return  force_to_goal_;
+}
+
+
+
+Sforce Cperson_abstract::force_sphe_prob( const SpointV_cov& interacting_person , const std::vector<double>* social_forces_param,
+        const SpointV* virtual_current_point)
+{
+    //uniformly sampling around std
+	double theta = interacting_person.angle_heading_point( this->current_pointV_ );
+    Sforce f;
+    Spoint p(interacting_person);
+    for( unsigned int i=0; i< 4; ++i )
+    {
+        f += this->force_sphe( p-Spoint( sqrt(interacting_person.cov_xx())*cos(theta) ,  sqrt(interacting_person.cov_yy())*sin(theta)), social_forces_param, virtual_current_point );
+        theta += PI/2.0;
+    }
+    return f*0.25;
+}
+
+Sforce Cperson_abstract::force_sphe_mahalanobis( const SpointV_cov& interacting_person , const std::vector<double>* social_forces_param,
+        const SpointV* virtual_current_point)
+{
+
+    //Mahalanobis distance to interacting target: gives a relative distance that dpeneds on the covariance matrix
+    double d_mah = sqrt( interacting_person.cov_dist( this->current_pointV_ ) );
+    double theta = interacting_person.angle_heading_point( this->current_pointV_ );
+    Spoint p(interacting_person),dt( d_mah*cos(theta) , d_mah*sin(theta) );
+	return this->force_sphe( p-dt , social_forces_param, virtual_current_point );
+}
+
+Sforce Cperson_abstract::force_sphe_worst( const SpointV_cov& interacting_person , const std::vector<double>* social_forces_param,
+        const SpointV* virtual_current_point)
+{
+    //worst case scenario: the interacting target is in the covariance elipsoid and the nearest point to center.
+    double theta = interacting_person.angle_heading_point( this->current_pointV_ );
+    Spoint p(this->current_pointV_),dt( interacting_person.cov_xx()*cos(theta) , interacting_person.cov_yy()*sin(theta) );
+	return this->force_sphe( p-dt , social_forces_param, virtual_current_point );
+}
+
+Sforce Cperson_abstract::get_forces_person(Sforce&  force_to_goal, Sforce& force_int_person, Sforce& force_int_robot, Sforce& force_obstacle ) const
+{
+	//make sure whenever you use this function to previously calculate the correspondent forces
+	force_to_goal = force_to_goal_;
+	force_int_person = force_int_person_;
+	force_int_robot = force_int_robot_;
+	force_obstacle = force_obstacle_;
+	return force_to_goal_ + force_int_person_ + force_int_robot_+  force_obstacle_ ;
+}
+
+Sforce Cperson_abstract::get_forces_person_companion(Sforce&  force_to_goal, Sforce& force_int_person, Sforce& force_int_robot, Sforce& force_obstacle, Sforce& force_companion ) const
+{
+	//make sure whenever you use this function to previously calculate the correspondent forces
+	//std::cout << " 33333333333 get_forces_person_companion  " << std::endl;
+	//force_to_goal_.print();
+
+	force_to_goal = force_to_goal_;
+	force_int_person = force_int_person_;
+	force_int_robot = force_int_robot_;
+	force_obstacle = force_obstacle_;
+	force_companion=force_companion_;
+	return force_to_goal_ + force_int_person_ + force_int_robot_+  force_obstacle_ + force_companion_;
+}
+void Cperson_abstract::set_forces_person( Sforce force_to_goal, Sforce force_int_person, Sforce force_int_robot, Sforce force_obstacle )
+{
+	force_to_goal_ = force_to_goal;
+	force_int_person_ = force_int_person;
+	force_int_robot_ = force_int_robot;
+	force_obstacle_ = force_obstacle;
+}
+void Cperson_abstract::set_forces_person_companion( Sforce force_to_goal, Sforce force_int_person, Sforce force_int_robot, Sforce force_obstacle , Sforce force_companion )
+{
+	force_to_goal_ = force_to_goal;
+	force_int_person_ = force_int_person;
+	force_int_robot_ = force_int_robot;
+	force_obstacle_ = force_obstacle;
+	force_companion_=force_companion;
+	//std::cout << " Set_forces comp! 33333333333 get_forces_person_companion  " << std::endl;
+	//	force_to_goal_.print();
+
+}
+
+Sbehavior::Sbehavior( unsigned int id ) : related_person_id(id)
+{
+	int N = Cperson_abstract::Count_behaviors;
+	expectation.resize( N , 1.0/(double)N );
+}
+
+Sbehavior::~Sbehavior()
+{
+
+}
+void Sbehavior::print() const
+{
+	std::cout << " Behavior to the related person  " << related_person_id <<
+			" and expected behaviors = { " << expectation[0] <<
+			" , " << expectation[1] << " , " << expectation[2] << " }" << std::endl;
+	return;
+}
+
+
+Sforce force_sphe( const SpointV& center_person, const SpointV& interacting_person , const std::vector<double>* social_forces_param )
+{
+	//setting the corresponding SFM parameters:
+	// if no forces param provided, exit program
+	assert( social_forces_param != NULL );
+	assert( social_forces_param->size() >= 5 );
+
+	Sforce force;
+	double f,d;
+	//geometry calculations
+	double dx = center_person.x - interacting_person.x;
+	double dy = center_person.y - interacting_person.y;
+	double vx = center_person.vx;
+	double vy = center_person.vy;
+	d = sqrt(dx*dx+dy*dy);
+	dx /= d;
+	dy /= d;
+	//force module calculation
+	f = social_forces_param->at(2)* exp((social_forces_param->at(4)-d) / social_forces_param->at(3));
+
+	//force direction
+	double phi = diffangle( atan2(vy,vx), atan2(-dy,-dx) );//minus difference vector
+	double anisotropy = (social_forces_param->at(1) + (1-social_forces_param->at(1))*(1 + cos(phi))/2 );
+	force.fx = f * dx * anisotropy;
+	force.fy = f * dy * anisotropy;
+	return force;
+}
+
+Sforce force_goal( const Sdestination dest, const SpointV& center_person , const std::vector<double>* social_forces_param )
+{
+	//setting the target type and its corresponding parameters:
+	// if no forces param provided, exit program
+	assert( social_forces_param != NULL );
+	assert( social_forces_param->size() >= 5 );
+
+	Sforce force_to_goal;
+	double desired_velocity = center_person.v();
+
+
+	double v_desired,v_desired_x,v_desired_y;
+	if ( dest.type == Sdestination::Map_goal  )
+	{
+		v_desired_x = dest.x - center_person.x;
+		v_desired_y = dest.y - center_person.y;
+		v_desired = sqrt(v_desired_x*v_desired_x + v_desired_y*v_desired_y);
+		v_desired_x *= desired_velocity/v_desired;
+		v_desired_y *= desired_velocity/v_desired;
+	}
+	else //Sdestination::Stopping
+	{
+		v_desired_x = 0.0;
+		v_desired_y = 0.0;
+	}
+	force_to_goal = Sforce(	social_forces_param->at(0) * (v_desired_x - center_person.vx),
+			social_forces_param->at(0) * (v_desired_y - center_person.vy));
+	return  force_to_goal;
+}
+
+
+
+/* functions person companion (ely) */
+
+Sdestination Cperson_abstract::get_companion_force_goal() const
+{
+	return robot_companion_dest_;
+}
+
+Sforce Cperson_abstract::force_sphe_atractive( const Spoint& interacting_person ,  const std::vector<double>* social_forces_param , const SpointV* virtual_current_point )
+{
+	//if force is calculated from a propagated pose, then virtual_current_point won't be null.
+	//Otherwise, current point is current point class variable
+	std::cout << "entering force_sphe (1) (params not transmuted)"<< std::endl;
+
+	const SpointV* current_point;
+	if ( virtual_current_point == NULL  )
+		current_point = &current_pointV_;
+	else
+		current_point = virtual_current_point;
+
+	//setting the corresponding SFM parameters:
+	// if no forces param provided, exit program
+	assert( social_forces_param != NULL );
+	assert( social_forces_param->size() >= 5 );
+
+	Sforce force;
+	double f,d;
+	//geometry calculations
+	double dx = current_point->x - interacting_person.x;
+	double dy = current_point->y - interacting_person.y;
+	double vx = current_point->vx;
+	double vy = current_point->vy;
+	d = sqrt(dx*dx+dy*dy);
+	dx /= d;
+	dy /= d;
+	//force module calculation
+	f = social_forces_param->at(2)* exp((social_forces_param->at(4)-d) / social_forces_param->at(3));
+	//force direction
+	double phi = diffangle( atan2(vy,vx), atan2(-dy,-dx) );//minus difference vector
+	double anisotropy = (social_forces_param->at(1) + (1-social_forces_param->at(1))*(1 + cos(phi))/2 );
+	force.fx = -f * dx * anisotropy;
+	force.fy = -f * dy * anisotropy;
+	return force;
+}
+
+void Cperson_abstract::clear_Zanlungo_vectors(){
+
+	//std::cout << "IN clear_Zanlungo_vectors"<< std::endl;
+
+	v_desired_robot_x_.clear(); // estan dentro de person abstract es mas complicado.
+	v_desired_robot_y_.clear();
+	v_current_robot_x_.clear();
+	v_current_robot_y_.clear();
+	actual_random_goal_x_.clear();
+	actual_random_goal_y_.clear();
+
+    actual_distance_in_x_.clear();
+   	actual_distance_in_y_.clear();
+	actual_module_distance_.clear();
+	actual_in_desired_velocity_.clear();
+}
+
+std::vector<double> Cperson_abstract::get_v_desired_robot_x(){
+	return v_desired_robot_x_;
+}
+std::vector<double> Cperson_abstract::get_v_desired_robot_y(){
+	return v_desired_robot_y_;
+}
+
+std::vector<double> Cperson_abstract::get_v_current_robot_x(){
+	return v_current_robot_x_;
+}
+
+std::vector<double> Cperson_abstract::get_v_current_robot_y(){
+	return v_current_robot_y_;
+}
+
+std::vector<double> Cperson_abstract::get_actual_random_goal_x(){
+	return actual_random_goal_x_;
+}
+
+std::vector<double> Cperson_abstract::get_actual_random_goal_y(){
+	return actual_random_goal_y_;
+}
+
+//////////
+
+std::vector<double> Cperson_abstract::get_actual_distance_in_x(){
+	return actual_distance_in_x_;
+}
+std::vector<double> Cperson_abstract::get_actual_distance_in_y(){
+	return actual_distance_in_y_;
+}
+
+std::vector<double> Cperson_abstract::get_actual_module_distance(){
+	return actual_module_distance_;
+}
+
+std::vector<double> Cperson_abstract::get_actual_in_desired_velocity(){
+	return actual_in_desired_velocity_;
+}
+
+
diff --git a/local_lib_people_prediction/src/scene_elements/person_abstract.h b/local_lib_people_prediction/src/scene_elements/person_abstract.h
new file mode 100644
index 0000000000000000000000000000000000000000..12c3da009b099bcdfa21985d30723feb592b189d
--- /dev/null
+++ b/local_lib_people_prediction/src/scene_elements/person_abstract.h
@@ -0,0 +1,308 @@
+/*
+ * person_abstract.h
+ *
+ *  Created on: 2013.  Last Modified on 2020
+ *      Author: Initial code of the AKP Gonzalo Ferrer.
+ *      Author: Addition of ASSAOP and ASSAGP for people accompaniment code of Ely Repiso.
+ */
+
+#ifndef person_abstract_H
+#define person_abstract_H
+
+
+#include <deque>
+#include <vector>
+#include <list>
+#include "iri_geometry.h"
+#include <stddef.h>
+#include <assert.h>
+#include <iostream>
+#include <fstream>      // std::ofstream
+
+/**
+ *
+ * \brief Behavior Class
+ *
+ * The behavior class is a container class  for the different behavior paramters
+ * used for a specific human motion prediction (Cperson_behavior)
+ *
+*/
+class Sbehavior
+{
+  public:
+	Sbehavior( unsigned int id );
+	~Sbehavior();
+	unsigned int related_person_id;
+	std::vector<double> expectation;
+	void print() const;
+};
+
+/**
+ *
+ * \brief Person Class 
+ *
+ * The person class is an abstract container class defining the interfaces
+ * of the person prediction and trajectory storing (if needed)
+ * 
+ * different types of persons are defined: Person , Robot , Obstacle
+ * depending on the prediction method or scene type
+ *
+*/
+class Cperson_abstract
+{
+	public:
+		enum target_type { Person=0 , Robot , Obstacle, Person_companion};
+		enum force_type { Spherical=0, Elliptical, Collision_Prediction};
+		enum behavior_type { Balanced=0, Aware, Unaware, Count_behaviors};
+	    enum filtering_method{ No_filtering=0, Linear_regression_filtering, Bayes_filtering, Low_pass_linear_regression_filtering};
+	    enum companion_reactive{ Akp_planning=0, Reactiva_repulsive=1, Reactive_atractive=2, Person_companion_akp=3};
+	    //Manual remove requires an external call to eliminate a person by using the scene method ()
+	    enum update_person_method { Autoremove=0, Manual_remove };
+	    enum vel_per_ok{ Vel_per=0, Near_goal, Far_goal};
+	    Cperson_abstract (  unsigned int id=0 ,
+				Cperson_abstract::target_type target_type=Cperson_abstract::Person,
+				Cperson_abstract::force_type person_force_type=Cperson_abstract::Spherical,
+				double _time_window = 1.0);
+		virtual ~Cperson_abstract()=0;
+		unsigned int get_id() const { return person_id_; }
+		void set_id( unsigned int id ) { person_id_ = id; }
+		bool operator== (Cperson_abstract& p2) const;
+		bool operator!= (Cperson_abstract& p2) const { return !(*this==p2); }
+
+		virtual void add_pointV( SpointV_cov point ,
+				Cperson_abstract::filtering_method filter=Cperson_abstract::Linear_regression_filtering , bool person_or_robot=false, double in_new_value_windowing=6.0)= 0;
+
+		virtual void Cperson_bhmip_add_pointV( SpointV_cov point, Cperson_abstract::filtering_method filter=Cperson_abstract::Linear_regression_filtering, bool person_or_robot=false) = 0;
+		virtual void refresh_person( double now ) = 0;
+		virtual void prediction( double min_v_to_predict) = 0;
+		void set_best_dest(const Sdestination& dest ){
+			//std::cout << "(set_best_dest) dest:"<< std::endl;
+			//dest.print();
+			best_destination_=dest;
+			//std::cout << " best_destination_:"<< std::endl;
+			//best_destination_.print();
+		}
+		Sdestination get_best_dest(){
+			//std::cout << "(get_best_dest) best_destination_:"<< std::endl;
+			//best_destination_.print();
+			//std::cout << "(get_best_dest) best_destination_:"<<best_destination_.x<<"; best_destination_.y"<<best_destination_.y<< std::endl;
+
+			return best_destination_;
+		}
+		void set_destinations( const std::vector<Sdestination>& dest ) { 	destinations_ = dest; }
+		const std::vector<Sdestination>* get_destinations()   { return &destinations_; }
+		std::vector<Sdestination> get_destinations2()   { return destinations_; }
+		void set_desired_velocty(double v){
+			//std::cout << "(set_desired_velocty, ant) desired_velocity_ ="<<desired_velocity_ << std::endl;
+			desired_velocity_ = v;
+			//std::cout << "(set_desired_velocty, act, changed) desired_velocity_ ="<<desired_velocity_ << std::endl;
+		}
+		double get_desired_velocity() { return desired_velocity_; }
+		/**
+			 * \brief point propagation
+			 *
+			 * this method propagates person position dt time in the future.
+			 * if no destinations are set (or the best destination estimated is null)
+			 * then propagates linearly with velocity as a short term predictor.
+			*/
+		SpointV_cov pointV_propagation( double dt , Sforce force = Sforce() );
+		SpointV_cov pointV_propagation( double dt , Sforce force, SpointV_cov point );
+
+		const SpointV_cov& get_current_pointV() const { return current_pointV_; }
+		void set_current_pointV(SpointV_cov in_current_pointV) {
+			//std::cout << "INNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNN SET CURRENT POINT; current_pointV_.x="<<current_pointV_.x<<"; current_pointV_.y="<<current_pointV_.y<< std::endl;
+			current_pointV_=in_current_pointV;
+		} // made by (ely) change the initial robot position for robot companion.
+		double get_time() {
+			std::cout << "current_pointV_.time_stamp"<<current_pointV_.time_stamp<< std::endl;
+			return current_pointV_.time_stamp; }
+
+		//misc methods
+		virtual void reset()=0;
+		virtual void reset_before_destination_prob()=0;
+		void print();
+		void print_dest();
+		void set_person_type( target_type type ) {
+			//std::cout << "IN person type"<< std::endl;
+			//std::cout << "type"<<type<< std::endl;
+			//std::cout << " SET person tipe => type_"<<type_<<"; person id="<<person_id_<< std::endl;
+			type_ = type;
+			//std::cout << "out person type"<< std::endl;
+		}
+		target_type get_person_type () const { return type_; }
+
+		//forces calculation
+		/**
+			 * \brief force_cp
+			 *
+			 *  Calculates the interacting forces using the collision prediction model
+			 *  described by Zanlungo et al in "" , 2011.
+			 *
+			*/
+		Sforce force( const SpointV& interacting_person , const std::vector<double>* social_forces_param,
+				const SpointV* virtual_current_point = NULL ,  Cperson_abstract::companion_reactive reactive=Cperson_abstract::Akp_planning);
+		Sforce forceAnticipateCollision( force_type act_person_force_type, const SpointV& interacting_person ,  const std::vector<double>* social_forces_param, const SpointV* virtual_current_point= NULL, Cperson_abstract::companion_reactive reactive=Cperson_abstract::Akp_planning );
+
+
+		Sforce force_sphe( const Spoint& interacting_person , const std::vector<double>* social_forces_param,
+				const SpointV* virtual_current_point = NULL , Cperson_abstract::companion_reactive reactive=Cperson_abstract::Akp_planning);
+		Sforce force_ellip( const SpointV& interacting_person ,  const std::vector<double>* social_forces_param,
+				const SpointV* virtual_current_point = NULL  );
+		Sforce force_cp( const SpointV& interacting_person , const std::vector<double>* social_forces_param,
+				const SpointV* virtual_current_point = NULL );
+		Sforce force_goal( const Sdestination& dest , const std::vector<double>* social_forces_param, const SpointV* virtual_current_point = NULL , unsigned int parent_vertex=10, bool output_mesages=false);
+		Sforce force_goal_near(  const Sdestination& dest, const std::vector<double>* social_forces_param, const SpointV* virtual_current_point = NULL , double d_max_reduce_vel=1.0, bool in_go_with_vel_per=false,  double real_person_vel=0.9);
+		Sforce force_goal_companion( const Sdestination& dest , const std::vector<double>* social_forces_param, const SpointV* virtual_current_point = NULL , unsigned int parent_vertex=10, bool output_mesages=false);
+
+		// , double real_person_vel= default the max velociy of the robot,
+		Sforce force_sphe_prob( const SpointV_cov& interacting_person , const std::vector<double>* social_forces_param,
+	            const SpointV* virtual_current_point = NULL );
+	    Sforce force_sphe_mahalanobis( const SpointV_cov& interacting_person , const std::vector<double>* social_forces_param,
+	            const SpointV* virtual_current_point = NULL );
+	    Sforce force_sphe_worst( const SpointV_cov& interacting_person , const std::vector<double>* social_forces_param,
+	            const SpointV* virtual_current_point = NULL );
+		Sforce get_force_person() { return force_to_goal_ + force_int_person_ + force_int_robot_ + force_obstacle_; }
+		Sforce get_forces_person( Sforce&  force_to_goal, Sforce& force_int_person , Sforce& force_int_robot, Sforce& force_obstacle ) const;
+		Sforce get_forces_person_companion( Sforce&  force_to_goal, Sforce& force_int_person , Sforce& force_int_robot, Sforce& force_obstacle, Sforce& force_companion  ) const;
+		Sforce get_force_int_robot_person() { return force_int_robot_; }
+		void set_forces_person( Sforce  force_to_goal, Sforce force_int_person , Sforce force_int_robot, Sforce force_obstacle  );
+		void set_forces_person_companion( Sforce force_to_goal, Sforce force_int_person, Sforce force_int_robot, Sforce force_obstacle , Sforce force_companion );
+		void set_int_force( Sforce force_int ) { force_int_person_ = force_int;}
+		void set_int_robot_force( Sforce force_int ) { force_int_robot_ = force_int;}
+		void set_obstacle_force( Sforce force_int ) { force_obstacle_ = force_int;}
+		SpointV_cov get_diff_position(){ return diff_pointV_;}
+		bool is_observation_updated(){ return observation_update_;}
+		void set_observation_update(bool in){
+			observation_update_ = in;
+		}
+
+		//behavior estimation methods
+		virtual Sbehavior * find_behavior_estimation( unsigned int id ) { return NULL;}
+		virtual Cperson_abstract::behavior_type get_best_behavior_to_person( unsigned int interacting_person ) const
+			{ return Cperson_abstract::Balanced;}
+
+		//methods for trajectory prediction
+		virtual void prediction_propagation( double dt , Sforce force = Sforce(), unsigned int index = 0 ) {}
+		virtual void planning_propagation( double dt , Sforce force = Sforce(), unsigned int index = 0 ) {}
+		virtual void planning_propagation_copy( unsigned int prediction_index ) {}
+		virtual bool is_needed_to_propagate_person_for_planning( unsigned int parent_index, Spoint robot, unsigned int& new_index_to_be_copied) {return true;}
+		virtual void reset_propagation_flag() {}
+		virtual const std::vector<SpointV_cov>* get_prediction_trajectory() const {
+			std::cout << " (Cperson_abstract) get_prediction_trajectory()"<< std::endl;
+			return NULL;
+		}//carefull not to use this method if not prepared for predictions
+		virtual const std::vector<SpointV_cov>* get_prediction_trajectory_with_target_person() const {
+			std::cout << " (Cperson_abstract) get_prediction_trajectory_with_target_person()"<< std::endl;
+				return NULL;
+		}//carefull not to use this method if not prepared for predictions
+
+
+		virtual const std::vector<SpointV_cov>* get_planning_trajectory() const {
+			std::cout << " (Cperson_abstract)  get_planning_trajectory()"<< std::endl;
+			return NULL;
+		}//carefull not to use this method if not prepared for predictions
+		virtual void clear_prediction_trajectory() { }
+		virtual void clear_planning_trajectory() { }
+		virtual void reserve_planning_trajectory( unsigned int n) { }
+
+
+		virtual void rotate_and_translate_trajectory(double R, double thetaZ, double linear_vx,
+				double linear_vy, double v_rot_x, double v_rot_y, std::vector<double> vect_odom_eigen_tf, bool debug_odometry = false){}; // For local tracker
+
+		/* functions person companion (ely) */
+		Sforce force_sphe_atractive( const Spoint& interacting_person ,  const std::vector<double>* social_forces_param , const SpointV* virtual_current_point );
+		void set_companion_force_goal(Sdestination robot_companion_dest){robot_companion_dest_=robot_companion_dest;};
+		Sdestination get_companion_force_goal() const;
+
+		void set_debug_velocity_file(bool debug_velocity_file_in){
+			debug_velocity_file_=debug_velocity_file_in;
+		}
+
+		/*void set_debug_velocity_cout(bool debug_velocity_cout_in){
+			debug_velocity_cout_=debug_velocity_cout_in;
+		}*/
+
+		void set_debug_filename(std::string in_debug_file_){
+			debug_file_=in_debug_file_;
+		}
+
+		void set_externa_force_k_near_goal(double in_externa_force_k_near_goal_){
+			externa_force_k_near_goal_=in_externa_force_k_near_goal_;
+		}
+
+		void set_externa_force_k_far_goal(double in_externa_force_k_far_goal_){
+			externa_force_k_far_goal_=in_externa_force_k_far_goal_;
+		}
+
+		void set_ex_max_dist_to_near_goal_force_in(double in_max_dist_to_near_goal_force_){
+			max_dist_to_near_goal_force_=in_max_dist_to_near_goal_force_;
+		}
+
+		virtual const std::deque<SpointV_cov>* get_past_trajectory()=0;
+
+		void clear_Zanlungo_vectors();
+
+		 std::vector<double> get_v_desired_robot_x();
+		 std::vector<double> get_v_desired_robot_y();
+
+		 std::vector<double> get_v_current_robot_x();
+		 std::vector<double> get_v_current_robot_y();
+
+		 std::vector<double> get_actual_random_goal_x();
+		 std::vector<double> get_actual_random_goal_y();
+
+		 std::vector<double> get_actual_distance_in_x();
+		 std::vector<double> get_actual_distance_in_y();
+		 std::vector<double> get_actual_module_distance();
+		 std::vector<double> get_actual_in_desired_velocity();
+
+		 virtual void set_new_time_window(double in_new_time_window){ }
+
+
+	protected:
+		unsigned int person_id_;
+		SpointV_cov current_pointV_;
+		SpointV diff_pointV_;
+		Sdestination best_destination_;
+		Sdestination before_best_destination_; // (ely) to solve problems of before probabilities of destinations when the person destination change!
+		std::vector<Sdestination> destinations_; //local person destinations. prior probabilities
+		double desired_velocity_;
+		target_type type_;//enum target_type { Person=0 , Robot , Obstacle, Person_companion }; Virtual_Person <= este ultimo de Gonzalo desapareció antes de mi epoca.
+		force_type person_force_type_;
+		bool observation_update_;
+		Sforce force_to_goal_, force_int_person_ , force_obstacle_, force_int_robot_, force_companion_;
+		double now_;
+
+		/* robot companion (ely) */
+		Sdestination robot_companion_dest_;
+		bool debug_velocity_file_;
+
+		 std::string debug_file_;
+
+		 double externa_force_k_near_goal_;
+		 double externa_force_k_far_goal_;
+		 double max_dist_to_near_goal_force_;
+
+		 bool debug_force_goal_; // debug for person companion
+
+		 std::deque<SpointV_cov> past_trajectory_;
+
+		    std::vector<double> v_desired_robot_x_; // estan dentro de person abstract es mas complicado.
+		    std::vector<double> v_desired_robot_y_;
+		    std::vector<double> v_current_robot_x_;
+		    std::vector<double> v_current_robot_y_;
+		    std::vector<double> actual_random_goal_x_;
+		   	std::vector<double> actual_random_goal_y_;
+
+		    std::vector<double> actual_distance_in_x_;
+		   	std::vector<double> actual_distance_in_y_;
+			std::vector<double> actual_module_distance_;
+			std::vector<double> actual_in_desired_velocity_;
+};
+
+//global functions to calculate forces
+Sforce force_sphe( const SpointV& center_person, const SpointV& interacting_person , const std::vector<double>* social_forces_param );
+Sforce force_goal( const Sdestination dest, const SpointV& center_person , const std::vector<double>* social_forces_param );
+
+
+#endif
diff --git a/local_lib_people_prediction/src/scene_elements/person_behavior.cpp b/local_lib_people_prediction/src/scene_elements/person_behavior.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..2c4d8cb53c744cd2f771ba0e6b4b18d8ce810963
--- /dev/null
+++ b/local_lib_people_prediction/src/scene_elements/person_behavior.cpp
@@ -0,0 +1,203 @@
+/*
+ * person_behavior.cpp
+ *
+ *  Created on: Dec 17, 2013.  Last Modified on 2020
+ *      Author: Initial code of the AKP Gonzalo Ferrer.
+ *      Author: Addition of ASSAOP and ASSAGP for people accompaniment code of Ely Repiso.
+ */
+
+#include "scene_elements/person_behavior.h"
+#include <math.h>
+#include <iostream>
+
+Cperson_behavior::Cperson_behavior(unsigned int id, Cperson_abstract::target_type person_target_type,
+		Cperson_abstract::force_type person_force_type, double _time_window) :
+    Cperson_bhmip(id,person_target_type, person_force_type, _time_window),
+	debug_prediction_propagation_(false),
+	debug_add_pointV_(false),
+	debug_Cperson_bhmip_add_pointV_(false)
+{
+	prediction_trajectory_.reserve(1000);//default value for the prediction trajectory
+}
+
+Cperson_behavior::~Cperson_behavior()
+{
+
+}
+
+void Cperson_behavior::add_pointV( SpointV_cov point ,Cperson_abstract::filtering_method filter,  bool robot_or_person)
+{
+	if(debug_add_pointV_){
+		std::cout<<" (Cperson_behavior) entro en add_pointV!"<< std::endl;
+	}
+	Cperson_bhmip::add_pointV(point,filter ,robot_or_person);
+	//calculate expected behavior is done in the estimation methd (in scene)
+}
+
+void Cperson_behavior::Cperson_bhmip_add_pointV( SpointV_cov point ,Cperson_abstract::filtering_method filter, bool person_or_robot)
+{
+	if(debug_Cperson_bhmip_add_pointV_){
+		std::cout<<" (Cperson_behavior) entro en add_pointV!"<< std::endl;
+	}
+
+	Cperson_bhmip::Cperson_bhmip_add_pointV(point,filter,person_or_robot);
+	//calculate expected behavior is done in the estimation methd (in scene)
+}
+
+void Cperson_behavior::prediction(double min_v_to_predict)
+{
+	Cperson_bhmip::prediction( min_v_to_predict);
+	//behavior estimation is required to be done jointly with all person, so it is calculated outside the class
+}
+
+Sbehavior* Cperson_behavior::find_behavior_estimation( unsigned int id )
+{
+	std::list<Sbehavior>::iterator iit = expected_behavior_list_.begin();
+	Sbehavior *behavior;
+	if ( !expected_behavior_list_.empty() )
+	{
+		for( ; iit != expected_behavior_list_.end(); iit++ )
+		{
+			if ( iit->related_person_id == id )
+			{
+				behavior = &(*iit);
+				assert( behavior != NULL );
+				return  behavior;
+			}
+			if ( iit->related_person_id > id )
+				break;
+		}
+	}
+	//behavior not found for person id, then a new behavior is set
+	expected_behavior_list_.insert( iit, Sbehavior( id ) );
+	iit--;//element inserted is before iit, so we want the pointer to the inserted element
+	behavior = &(*iit);
+	assert( behavior != NULL );
+	return  behavior;
+}
+
+Cperson_abstract::behavior_type
+Cperson_behavior::get_best_behavior_to_person( unsigned int interacting_person ) const
+{
+	Cperson_abstract::behavior_type result = Cperson_abstract::Balanced;
+	const Sbehavior * behavior = NULL;
+	std::list<Sbehavior>::const_iterator iit = expected_behavior_list_.begin();
+	if ( !expected_behavior_list_.empty() )
+	{
+		for( ; iit != expected_behavior_list_.end(); iit++ )
+		{
+			if ( iit->related_person_id == interacting_person )
+			{
+				behavior = &(*iit);
+				assert( behavior != NULL );
+			}
+			if ( iit->related_person_id > interacting_person )
+				break;//person not found, so result is a balanced behavior
+		}
+	}
+	if ( behavior == NULL ) return result;
+	assert( behavior != NULL );
+	double best_expectation =  behavior->expectation[0];
+	for ( unsigned int i = 1; i < behavior->expectation.size(); ++i  )
+	{
+		if( best_expectation < behavior->expectation[i]  )
+		{
+			best_expectation = behavior->expectation[i];
+			result = (Cperson_abstract::behavior_type)i;
+		}
+	}
+
+	return result;
+}
+
+void Cperson_behavior::prediction_propagation( double dt , Sforce force , unsigned int index )
+{
+
+	if(debug_prediction_propagation_){
+		std::cout << "IN Cperson_behavior::prediction_propagation prediction_trajectory_.emty()"<<prediction_trajectory_.empty()<< std::endl;
+		std::cout <<" dt="<<dt<<";desired_velocity_="<<desired_velocity_<<"; force.fx"<<force.fx<<"; force.fy="<<force.fy<<"; prediction_trajectory_.at(index).print():"<< std::endl;
+		prediction_trajectory_.at(index).print();
+	}
+	prediction_trajectory_.push_back( prediction_trajectory_.at(index).propagate(dt,force,desired_velocity_) );
+}
+
+void Cperson_behavior::planning_propagation( double dt , Sforce force , unsigned int index )
+{
+	planning_trajectory_.push_back( planning_trajectory_.at(index).propagate(dt,force,desired_velocity_) );
+}
+
+void Cperson_behavior::planning_propagation_copy( unsigned int prediction_index )
+{
+	planning_trajectory_.push_back( prediction_trajectory_.at(prediction_index) );
+}
+
+bool Cperson_behavior::is_needed_to_propagate_person_for_planning( unsigned int parent_index, Spoint robot, unsigned int& new_index_to_be_copied )
+{
+	bool res;
+	//TODO first iteration: only checks distance to target
+
+	double distance=1; // Gonzalo has 1, as distance, I need more distance.
+	//std::cout << "has_copied_propagation_="<<has_copied_propagation_<< std::endl;
+
+	if ( robot.distance( planning_trajectory_.at( parent_index ) ) < distance && !has_copied_propagation_ )
+	{
+	//	std::cout << "robot.distance( planning_trajectory_.at( parent_index ) ) < distance; d="<<robot.distance( planning_trajectory_.at( parent_index ) )<< std::endl;
+		res = true;
+	}
+	else
+	{
+
+		has_copied_propagation_ = true;
+		unsigned int index(0);
+	//	std::cout << "has_copied_propagation_="<<has_copied_propagation_<< std::endl;
+		while( robot.time_stamp > prediction_trajectory_.at(index).time_stamp  && index < prediction_trajectory_.size()-1 )
+		{
+			index++;
+		}
+		//std::cout << "new_index_to_be_copied="<<index<< std::endl;
+		new_index_to_be_copied = index;
+		res = false;
+	}
+
+	return res;
+}
+
+void Cperson_behavior::clear_prediction_trajectory()
+{
+	prediction_trajectory_.clear();
+	prediction_trajectory_.push_back( current_pointV_ );
+	planning_trajectory_.clear();
+	planning_trajectory_.push_back( current_pointV_ );
+	has_copied_propagation_ = false;
+}
+
+void Cperson_behavior::clear_planning_trajectory()
+{
+	planning_trajectory_.clear();
+	planning_trajectory_.push_back( current_pointV_ );
+	has_copied_propagation_ = false;
+}
+
+void Cperson_behavior::reserve_prediction_trajectory( unsigned int n)
+{
+	prediction_trajectory_.reserve( n );//if n < capacity() then does nothing
+}
+
+void Cperson_behavior::reset(  )
+{
+	Cperson_bhmip::reset();
+	expected_behavior_list_.clear();
+	prediction_trajectory_.clear();
+	planning_trajectory_.clear();
+	std::vector<double> reset_cov; // MADE BY Ely (reset covariances person simulated. Augment more with huge velocities, in start, I need to reset!)
+	reset_cov.reserve(16);
+	reset_cov.resize(16,0.0);
+	reset_cov[0] = 0.5;//0.4;
+	reset_cov[5] = 0.5;
+	reset_cov[10] = 0.1; //0.1
+	reset_cov[15] = 0.1;
+	current_pointV_.cov=reset_cov;
+
+}
+
+
diff --git a/local_lib_people_prediction/src/scene_elements/person_behavior.h b/local_lib_people_prediction/src/scene_elements/person_behavior.h
new file mode 100644
index 0000000000000000000000000000000000000000..b499ca7e40aec578c71ab538a5bb6591f3666e54
--- /dev/null
+++ b/local_lib_people_prediction/src/scene_elements/person_behavior.h
@@ -0,0 +1,65 @@
+/*
+ * person_behavior.h
+ *
+ *  Created on: Dec 17, 2013. Last Modified on 2020
+ *      Author: Initial code of the AKP Gonzalo Ferrer.
+ *      Author: Addition of ASSAOP and ASSAGP for people accompaniment code of Ely Repiso.
+ */
+
+
+#ifndef PERSON_BEHAVIOR_H_
+#define PERSON_BEHAVIOR_H_
+
+#include "scene_elements/person_abstract.h"
+#include "scene_elements/person_bhmip.h"
+#include <list>
+
+
+class Cperson_behavior : public Cperson_bhmip
+{
+  public:
+	Cperson_behavior(unsigned int id=0,
+			Cperson_abstract::target_type person_target_type=Cperson_abstract::Person,
+			Cperson_abstract::force_type person_force_type=Cperson_abstract::Spherical,
+			double _time_window = 1.0);
+	virtual ~Cperson_behavior();
+	virtual void reset();
+	virtual void add_pointV( SpointV_cov point,
+			Cperson_abstract::filtering_method filter=Cperson_abstract::Linear_regression_filtering,  bool robot_or_person=false);
+	virtual void Cperson_bhmip_add_pointV( SpointV_cov point ,Cperson_abstract::filtering_method filter=Cperson_abstract::Linear_regression_filtering, bool person_or_robot=false);
+	virtual void prediction( double min_v_to_predict );
+	/**
+	 * this virtual method calculates interaction forces depending on the
+	 * expected behavior, that is, changing the force parameters for each
+	 * different person
+	 */
+	virtual Sbehavior* find_behavior_estimation( unsigned int id );
+	virtual Cperson_abstract::behavior_type	get_best_behavior_to_person( unsigned int interacting_person ) const;
+
+	//methods for trajectory prediction
+	virtual void prediction_propagation( double dt , Sforce force = Sforce(), unsigned int index = 0 );
+	virtual void planning_propagation( double dt , Sforce force = Sforce(), unsigned int index = 0 );
+	virtual void planning_propagation_copy( unsigned int prediction_index );
+	virtual bool is_needed_to_propagate_person_for_planning( unsigned int parent_index, Spoint robot, unsigned int& new_index_to_be_copied );
+	virtual void reset_propagation_flag() { has_copied_propagation_ = false; }
+	virtual const std::vector<SpointV_cov>* get_planning_trajectory() const { return &planning_trajectory_;}//raw planning trajectory
+	virtual const std::vector<SpointV_cov>* get_prediction_trajectory() const {  return &prediction_trajectory_;}//predicted trajectory, according to best plan
+	virtual void clear_prediction_trajectory();
+	virtual void clear_planning_trajectory();
+	virtual void reserve_prediction_trajectory( unsigned int n );
+
+  private:
+	std::vector<SpointV_cov> planning_trajectory_, prediction_trajectory_;//container for predictions and planning
+	bool has_copied_propagation_;
+	std::list<Sbehavior> expected_behavior_list_;
+
+	bool debug_prediction_propagation_; // debug for simulation person companion.
+	bool debug_add_pointV_; // debug for simulation person companion.
+	bool debug_Cperson_bhmip_add_pointV_; // debug for simulation person companion.
+
+
+
+};
+
+
+#endif /* PERSON_BEHAVIOR_H_ */
diff --git a/local_lib_people_prediction/src/scene_elements/person_bhmip.cpp b/local_lib_people_prediction/src/scene_elements/person_bhmip.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a0940ffc125bcc8428eb846f3551743731ba3a96
--- /dev/null
+++ b/local_lib_people_prediction/src/scene_elements/person_bhmip.cpp
@@ -0,0 +1,1079 @@
+/*
+ * person_bhmip.cpp
+ *
+ *  Created on: Jul 9, 2013. Last Modified on 2020
+ *      Author: Initial code of the AKP Gonzalo Ferrer.
+ *      Author: Addition of ASSAOP and ASSAGP for people accompaniment code of Ely Repiso.
+ */
+
+#include "scene_elements/person_bhmip.h"
+#include <Eigen/Dense>
+#include <iostream>
+
+Cperson_bhmip::Cperson_bhmip(unsigned int id, Cperson_abstract::target_type person_target_type,
+		 Cperson_abstract::force_type person_force_type, double _time_window) :
+    Cperson_abstract(id,person_target_type, person_force_type),
+    time_window_( _time_window ) , phi_var_( 0.8*0.8 ), debug_filter_person_velocity_(false),
+	debug_add_pointV_(false),
+	debug_Cperson_bhmip_add_pointV_(false),
+	debug_trajectory_windowing_(false),
+	debug_refresh_person_(false),
+	debug_prediction_(false),
+	debug_intention_precalculation_(false),
+	debug_filter_current_state_linear_regression_(false),
+	person_before_moving_(false)
+{
+	destinations_.reserve(15);
+}
+
+Cperson_bhmip::~Cperson_bhmip()
+{
+
+}
+
+void Cperson_bhmip::add_pointV( SpointV_cov point, Cperson_abstract::filtering_method filter, bool robot_or_person, double in_new_value_windowing)
+{
+	time_window_=in_new_value_windowing;
+
+	if(debug_add_pointV_){
+		std::cout<<" (Cperson_bhmip) entro en add_pointV!"<< std::endl;
+	}
+
+	SpointV_cov filtered_pose;
+	if( filter != Cperson_abstract::No_filtering )
+	{
+		if(debug_add_pointV_){
+			std::cout<<" (Cperson_bhmip) != Cperson_abstract::No_filtering; filter="<<filter<< std::endl;
+		}
+		SpointV_cov pointV = point;//only positions, position_covariances and time_stamp
+
+		//std::cout<<" pointV.time="<<pointV.time_stamp<<"; point.time_stamp="<<point.time_stamp<< std::endl;
+
+		double dx, dy;
+		if ( !trajectory_.empty() )
+		{
+			if(debug_add_pointV_){
+				std::cout<<" (Cperson_bhmip) if !trajectory_.empty()"<< std::endl;
+			}
+
+			dx = point.x - trajectory_.back().x;
+			dy = point.y - trajectory_.back().y;
+			double dt =  point.time_stamp - trajectory_.back().time_stamp ;
+			//just to avoid singularities, ideally time stamps are well separated in time
+			double marge=0.001;  // marge=0.001;=> anterior valor, que seguía creando singularidades.
+			if ( point.time_stamp - trajectory_.back().time_stamp  < marge ) dt = 0.1;
+
+			pointV.vx = dx / dt;
+			pointV.vy = dy / dt;
+
+			if(sqrt(pow(pointV.vx,2))>2){
+				pointV.vx=trajectory_.back().vx;
+			}
+			if(sqrt(pow(pointV.vy,2))>2){
+				pointV.vy=trajectory_.back().vy;
+			}
+
+			if(debug_filter_person_velocity_){
+				std::cout<<" (ANT) dt="<<dt<<"; pointV.vx="<< pointV.vx<<"; pointV.vy= " <<pointV.vy<<"; time_window_="<<time_window_<< std::endl;
+				std::cout<<" (INI FILTRO V) point.time_stamp="<< point.time_stamp<<"; trajectory_.back().time_stamp= " <<trajectory_.back().time_stamp<< std::endl;
+				std::cout<<"dx="<< dx<<"; dy= " <<dy<<"; point.x="<< point.x<<"; trajectory_.back().x="<<trajectory_.back().x<<"; trajectory_.back().y="<<trajectory_.back().y<<"; point.y= " <<point.y<< std::endl;
+				std::cout<<"; point.vx="<< point.vx<<"; point.vy= " <<point.vy<< std::endl;
+				std::cout<<"trajectory_.back().vx="<< trajectory_.back().vx<<"; trajectory_.back().vy= " <<trajectory_.back().vy<< std::endl;
+				std::cout<<" dt="<<dt<<"; pointV.vx="<< pointV.vx<<"; pointV.vy= " <<pointV.vy<< std::endl;
+			}
+		}
+		else
+		{
+			if(debug_add_pointV_){
+				std::cout<<" ==trajectory_.empty() "<< std::endl;
+			}
+			if(robot_or_person){
+				if(debug_add_pointV_){
+					std::cout<<" ant velocity "<< std::endl;
+				}
+				pointV.vx=point.vx;
+				pointV.vy=point.vy;
+			}else{
+				if(debug_add_pointV_){
+					std::cout<<" set 0 velocity "<< std::endl;
+				}
+				pointV.vx = 0;
+				pointV.vy = 0;
+			}
+		}
+
+		//std::cout<<" before TRAJECTORY WINDOWING"<< std::endl;
+		//slide temporal window for all trajectories
+		trajectory_.push_back(pointV);
+		//std::cout<<" after push_bach"<< std::endl;
+		trajectory_windowing();
+		//std::cout<<" AFTER TRAJECTORY WINDOWING"<< std::endl;
+
+		if(debug_filter_person_velocity_){
+			std::cout<<" AFTER TRAJECTORY WINDOWING"<< std::endl;
+			std::cout<<"; pointV.vx="<< pointV.vx<<"; pointV.vy= " <<pointV.vy<<"; time_window_="<<time_window_<< std::endl;
+
+			std::cout<<" (INI FILTRO V) point.time_stamp="<< point.time_stamp<<"; trajectory_.back().time_stamp= " <<trajectory_.back().time_stamp<< std::endl;
+			std::cout<<"dx="<< dx<<"; dy= " <<dy<<"; point.x="<< point.x<<"; trajectory_.back().x="<<trajectory_.back().x<<"; trajectory_.back().y="<<trajectory_.back().y<<"; point.y= " <<point.y<< std::endl;
+			std::cout<<"; point.vx="<< point.vx<<"; point.vy= " <<point.vy<< std::endl;
+			std::cout<<"trajectory_.back().vx="<< trajectory_.back().vx<<"; trajectory_.back().vy= " <<trajectory_.back().vy<< std::endl;
+			std::cout<<"; pointV.vx="<< pointV.vx<<"; pointV.vy= " <<pointV.vy<< std::endl;
+		}
+
+		if(debug_add_pointV_){
+			std::cout<<" trajectory_.size() "<<trajectory_.size()<< std::endl;
+		}
+
+		//std::cout<<" before filter"<< std::endl;
+		//filters velocities
+		switch( filter )
+		{
+		  case Cperson_abstract::Linear_regression_filtering:
+			  if(debug_add_pointV_){
+				  std::cout << " ( PPPPPPPP Caso: Linear_regression_filtering)" << std::endl;
+			  }
+			filtered_pose = filter_current_state_linear_regression( );
+			break;
+		  case Cperson_abstract::Bayes_filtering:
+			  if(debug_add_pointV_){
+				  std::cout << " ( PPPPPPPP Caso: Bayes_filtering)" << std::endl;
+			  }
+			filtered_pose = filter_current_state_linear_regression_bayes( );
+		  	break;
+		  case Cperson_abstract::Low_pass_linear_regression_filtering:
+		  default :
+			  if(debug_add_pointV_){
+				  std::cout << " ( PPPPPPPP Caso: Low_pass_linear_regression_filtering)" << std::endl;
+			  }
+			filtered_pose = low_pass_filter_current_state_linear_regression( );
+			break;
+		}
+
+	}
+	else
+	{
+		if(debug_add_pointV_){
+			std::cout << " (Caso: no filter is required!); trajectory_.size()="<<trajectory_.size() << std::endl;
+		}
+
+		//no filter is required
+		trajectory_.push_back(point);
+		//slide temporal window for all trajectories
+		trajectory_windowing();
+		filtered_pose = trajectory_.back();
+	}
+
+	//std::cout<<" AFTER first if-else"<< std::endl;
+
+	//update current positions and velocities according to filtering results
+	diff_pointV_ = filtered_pose - current_pointV_ ;
+	//std::cout << "(for tracker update_scene) filtered_pose.x="<<filtered_pose.x<<"; filtered_pose.y="<<filtered_pose.y<< std::endl;
+	current_pointV_ = filtered_pose;
+	//std::cout << "(for tracker update_scene) current_pointV_.x="<<current_pointV_.x<<"; current_pointV_.y="<<current_pointV_.y<< std::endl;
+	//std::cout << "(ANTES FILTRO) desired_velocity_="<<desired_velocity_<< std::endl;
+	desired_velocity_ = filtered_pose.v();
+	if(debug_filter_person_velocity_){
+		std::cout << "(DESPUES FILTRO==PERSONA) desired_velocity_="<<desired_velocity_<< std::endl;
+	}
+	now_ = filtered_pose.time_stamp;
+
+
+	//precalculates data required for the BHMIP
+	intention_precalculation();
+
+	observation_update_ = true;
+
+	if(debug_add_pointV_){
+		std::cout<<" trajectory_.size() "<<trajectory_.size()<< std::endl;
+		std::cout<<" (Cperson_bhmip) salgo de add_pointV! observation_update_="<<observation_update_<<": filtered_pose.print()"<< std::endl;
+		filtered_pose.print();
+	}
+
+}
+
+void Cperson_bhmip::Cperson_bhmip_add_pointV( SpointV_cov point, Cperson_abstract::filtering_method filter, bool person_or_robot)
+{
+	if(debug_Cperson_bhmip_add_pointV_){
+		std::cout<<" (Cperson_bhmip) entro en add_pointV!"<< std::endl;
+	}
+
+	SpointV_cov filtered_pose;
+	if( filter != Cperson_abstract::No_filtering )
+	{
+		if(debug_Cperson_bhmip_add_pointV_){
+			std::cout<<" (Cperson_bhmip) != Cperson_abstract::No_filtering"<< std::endl;
+		}
+
+		SpointV_cov pointV = point;//only positions, position_covariances and time_stamp
+		double dx, dy;
+		if ( !trajectory_.empty() )
+		{
+			if(debug_Cperson_bhmip_add_pointV_){
+				std::cout<<" (Cperson_bhmip) if !trajectory_.empty()"<< std::endl;
+			}
+
+			dx = point.x - trajectory_.back().x;
+			dy = point.y - trajectory_.back().y;
+			double dt =  point.time_stamp - trajectory_.back().time_stamp ;
+			//just to avoid singularities, ideally time stamps are well separated in time
+			double marge=0.001;  // marge=0.001;=> anterior valor, que seguía creando singularidades.
+			if ( point.time_stamp - trajectory_.back().time_stamp  < marge ) dt = 0.1;
+
+			pointV.vx = dx / dt;
+			pointV.vy = dy / dt;
+
+			if(sqrt(pow(pointV.vx,2))>2){
+				pointV.vx=trajectory_.back().vx;
+			}
+			if(sqrt(pow(pointV.vy,2))>2){
+				pointV.vy=trajectory_.back().vy;
+			}
+
+			 if(debug_filter_person_velocity_){
+			std::cout<<" (ANT) dt="<<dt<<"; pointV.vx="<< pointV.vx<<"; pointV.vy= " <<pointV.vy<<"; time_window_="<<time_window_<< std::endl;
+
+			std::cout<<" (INI FILTRO V) point.time_stamp="<< point.time_stamp<<"; trajectory_.back().time_stamp= " <<trajectory_.back().time_stamp<< std::endl;
+			std::cout<<"dx="<< dx<<"; dy= " <<dy<<"; point.x="<< point.x<<"; trajectory_.back().x="<<trajectory_.back().x<<"; trajectory_.back().y="<<trajectory_.back().y<<"; point.y= " <<point.y<< std::endl;
+			std::cout<<"; point.vx="<< point.vx<<"; point.vy= " <<point.vy<< std::endl;
+			std::cout<<"trajectory_.back().vx="<< trajectory_.back().vx<<"; trajectory_.back().vy= " <<trajectory_.back().vy<< std::endl;
+			std::cout<<" dt="<<dt<<"; pointV.vx="<< pointV.vx<<"; pointV.vy= " <<pointV.vy<< std::endl;
+			 }
+		}
+		else
+		{
+			if(debug_Cperson_bhmip_add_pointV_){
+				std::cout<<" ==trajectory_.empty() "<< std::endl;
+			}
+
+			if(person_or_robot){
+
+				pointV.vx=point.vx;
+				pointV.vy=point.vy;
+
+				if(debug_Cperson_bhmip_add_pointV_){
+					std::cout<<" pointV.vx="<<point.vx<< std::endl;
+					std::cout<<" pointV.vy="<<point.vy<< std::endl;
+				}
+			}else{
+				pointV.vx = 0;
+				pointV.vy = 0;
+				if(debug_Cperson_bhmip_add_pointV_){
+					std::cout<<" pointV.vx=pointV.vy=0"<< std::endl;
+				}
+			}
+		}
+
+		if(debug_Cperson_bhmip_add_pointV_){
+			std::cout<<" pointV_1 "<< std::endl;
+			pointV.print();
+		}
+		//slide temporal window for all trajectories
+		trajectory_.push_back(pointV);
+		trajectory_windowing();
+
+		if(debug_Cperson_bhmip_add_pointV_){
+			std::cout<<" trajectory_.size() "<<trajectory_.size()<< std::endl;
+			std::cout<<" pointV_2 "<< std::endl;
+			pointV.print();
+		}
+		//filters velocities
+		switch( filter )
+		{
+		  case Cperson_abstract::Linear_regression_filtering:
+			  if(debug_Cperson_bhmip_add_pointV_){
+				  std::cout << " ( PPPPPPPP Caso: Linear_regression_filtering)" << std::endl;
+			  }
+			filtered_pose = filter_current_state_linear_regression( );
+			break;
+		  case Cperson_abstract::Bayes_filtering:
+			  if(debug_Cperson_bhmip_add_pointV_){
+				  std::cout << " ( PPPPPPPP Caso: Bayes_filtering)" << std::endl;
+			  }
+			filtered_pose = filter_current_state_linear_regression_bayes( );
+		  	break;
+		  case Cperson_abstract::Low_pass_linear_regression_filtering:
+		  default :
+			  if(debug_Cperson_bhmip_add_pointV_){
+				  std::cout << " ( PPPPPPPP Caso: Low_pass_linear_regression_filtering)" << std::endl;
+			  }
+			filtered_pose = low_pass_filter_current_state_linear_regression( );
+			break;
+		}
+
+
+	}
+	else
+	{
+		if(debug_Cperson_bhmip_add_pointV_){
+			std::cout << " (Caso: no filter is required!); trajectory_.size()="<<trajectory_.size() << std::endl;
+		}
+
+		//no filter is required
+		trajectory_.push_back(point);
+		//slide temporal window for all trajectories
+		trajectory_windowing();
+		filtered_pose = trajectory_.back();
+	}
+
+	//update current positions and velocities according to filtering results
+	diff_pointV_ = filtered_pose - current_pointV_ ;
+	current_pointV_ = filtered_pose;
+	//std::cout << "(ANTES FILTRO) desired_velocity_="<<desired_velocity_<< std::endl;
+	desired_velocity_ = filtered_pose.v();
+	if(debug_filter_person_velocity_){
+		std::cout << "(DESPUES FILTRO==PERSONA) desired_velocity_="<<desired_velocity_<< std::endl;
+	}
+	now_ = filtered_pose.time_stamp;
+
+
+	//precalculates data required for the BHMIP
+	intention_precalculation();
+
+	observation_update_ = true;
+	if(debug_Cperson_bhmip_add_pointV_){
+		std::cout<<" (Cperson_bhmip) salgo de add_pointV! observation_update_="<<observation_update_<<"; trajectory_.size()="<<trajectory_.size()<< std::endl;
+	}
+}
+
+
+SpointV_cov Cperson_bhmip::filter_current_state_linear_regression( )
+{
+
+
+	//Discard a single and double pose trajectory --------------------------------------------
+	if (trajectory_.size() <= 2 )
+	{
+		return trajectory_.back();
+	}
+	// First order Regression: y(t) ~ beta * x(t) = beta_1 + beta_2 * x(t) ----------------------------
+	//vector and matrices initialization
+	unsigned int window_elements = trajectory_.size();
+
+	if(debug_filter_person_velocity_){
+		std::cout << " PPPPPPPP (Cperson_bhmip::filter_current_state_linear_regression) window_elements= "<< window_elements << std::endl;
+	}
+	double initial_ts = trajectory_.front().time_stamp;
+	Eigen::VectorXd vx ( window_elements ) , vy ( window_elements );
+	Eigen::MatrixXd R (window_elements , 2 ); //1st order regression -> 2 coeffs
+	for (unsigned int i = 0 ; i < window_elements ; i++)
+	{
+		vx ( i ) = trajectory_.at(i).vx;
+		vy ( i ) = trajectory_.at(i).vy;
+		R.row ( i ) << 1 , trajectory_.at(i).time_stamp - initial_ts;
+
+		//std::cout <<"; i="<<i<< " vx ( i )="<<vx ( i ) << "; vy ("<<i<<" )="<<vy ( i )<< std::endl;
+		//std::cout << "initial_ts="<<initial_ts<<";  trajectory_.at(i).time_stamp"<< trajectory_.at(i).time_stamp<<"; trajectory_.at(i).time_stamp - initial_ts="<<trajectory_.at(i).time_stamp - initial_ts<<std::endl;
+	}
+	//pseudo inverse of the R matrix ---------------------------------------------------------------
+	Eigen::MatrixXd invR ( 2 , window_elements ) , sqrR ( 2 , 2 );
+	sqrR = R.transpose() * R;
+	invR = sqrR.inverse() * R.transpose() ;
+
+	//linear regression ----------------------------------------------------------------------------
+	Eigen::Vector2d  r_pred (1 , trajectory_.back().time_stamp - initial_ts ),vx_coef,vy_coef;
+	//invR * v = beta -> parameter estimation
+	vx_coef = invR * vx;
+	vy_coef = invR * vy;
+
+	//std::cout << " vx_coef="<<vx_coef<<std::endl;
+	//std::cout << " vy_coef="<<vy_coef<<std::endl;
+
+	SpointV_cov filtered_pose(
+			trajectory_.back().x, //no filtering at position
+			trajectory_.back().y,
+			trajectory_.back().time_stamp,
+			r_pred.dot ( vx_coef ),
+			r_pred.dot ( vy_coef ) );
+    filtered_pose.cov[0] = trajectory_.back().cov_xx();
+    filtered_pose.cov[1] = trajectory_.back().cov_xy();
+    filtered_pose.cov[4] = trajectory_.back().cov_xy();
+    filtered_pose.cov[5] = trajectory_.back().cov_yy();
+
+
+	//covariance of velocity prediction, with respect to regression velocity
+	Eigen::VectorXd vx_mean ( window_elements ) , vy_mean ( window_elements );
+	vx_mean = R*vx_coef;
+	vy_mean = R*vy_coef;
+
+	if(debug_filter_current_state_linear_regression_){
+		std::cout << " vx_coef[0]="<<vx_coef[0] << "vx_coef[1]="<<vx_coef[1]<< std::endl;
+		std::cout << " vy_coef[0]="<<vy_coef[0] << "vy_coef[1]="<<vy_coef[1]<< std::endl;
+		for(unsigned int n=0;n<window_elements;n++){
+			std::cout << " vx_mean("<<n<<")="<<vx_mean(n) << "; vx(n)="<<vx(n)<< std::endl;
+			std::cout << " vy_mean("<<n<<")="<<vy_mean(n) << "; vy(n)="<<vy(n)<< std::endl;
+		}
+	}
+	//double cov_vx = (vx - vx_mean).dot( vx - vx_mean );
+	//double cov_vy = (vy - vy_mean).dot( vy - vy_mean );
+	//double cov_vxy = (vy - vy_mean).dot( vx - vx_mean );
+
+	//std::cout <<" low pass filter!!! vx="<<vx<<"; vx_mean="<<vx_mean<<"; vy="<<vy<<"; vy_mean="<<vy_mean<<std::endl;
+
+	// cambiado por (ely)
+	double cov_vx = ((vx - vx_mean).dot( vx - vx_mean ))/window_elements;
+	double cov_vy = ((vy - vy_mean).dot( vy - vy_mean ))/window_elements;
+	double cov_vxy = ((vy - vy_mean).dot( vx - vx_mean ))/window_elements;
+
+	if(debug_filter_current_state_linear_regression_){
+		std::cout << " window_elements="<<window_elements<<"; cov_vx= "<<cov_vx<<"; cov_vy="<<cov_vy<<"; cov_vxy="<<cov_vxy << std::endl;
+	}
+
+	//regression covariances + projected uncertainty from positions P_x / dt
+	filtered_pose.cov[10] = cov_vx + 0.001;
+	filtered_pose.cov[11] = cov_vxy;
+	filtered_pose.cov[14] = filtered_pose.cov[11];
+	filtered_pose.cov[15] = cov_vy + 0.001;
+
+	if(debug_filter_person_velocity_){
+		std::cout << " PPPPPPPP (Cperson_bhmip::filter_current_state_linear_regression) filtered_pose: " << std::endl;
+		filtered_pose.print();
+	}
+
+	return filtered_pose;
+}
+
+
+SpointV_cov Cperson_bhmip::filter_current_state_linear_regression_bayes( )
+{
+
+	//Discard a single and double pose trajectory --------------------------------------------
+	if (trajectory_.size() <= 2 )
+	{
+		return trajectory_.back();
+	}
+	// First order Regression: y(t) ~ beta * x(t) = beta_1 + beta_2 * x(t) ----------------------------
+	//vector and matrices initialization
+	unsigned int window_elements = trajectory_.size();
+
+	if(debug_filter_person_velocity_){
+	std::cout << " PPPPPPPP (Cperson_bhmip::filter_current_state_linear_regression_bayes) window_elements= "<< window_elements << std::endl;
+	}
+
+	double initial_ts = trajectory_.front().time_stamp;
+	Eigen::VectorXd vx ( window_elements ) , vy ( window_elements );
+	Eigen::MatrixXd R (window_elements , 2 ); //1st order regression -> 2 coeffs
+	for (unsigned int i = 0 ; i < window_elements ; i++)
+	{
+		vx ( i ) = trajectory_.at(i).vx;
+		vy ( i ) = trajectory_.at(i).vy;
+		R.row ( i ) << 1 , trajectory_.at(i).time_stamp - initial_ts;
+	}
+	//pseudo inverse of the R matrix ---------------------------------------------------------------
+	Eigen::MatrixXd invR ( 2 , window_elements ) , sqrR ( 2 , 2 );
+	sqrR = R.transpose() * R;
+	invR = sqrR.inverse() * R.transpose() ;
+
+	//linear regression ----------------------------------------------------------------------------
+	Eigen::Vector2d  r_pred (1 , trajectory_.back().time_stamp - initial_ts ),vx_coef,vy_coef;
+	//invR * v = beta -> parameter estimation
+	vx_coef = invR * vx;
+	vy_coef = invR * vy;
+	SpointV_cov filtered_pose(
+			trajectory_.back().x, //no filtering at position
+			trajectory_.back().y,
+			trajectory_.back().time_stamp,
+			r_pred.dot ( vx_coef ),
+			r_pred.dot ( vy_coef ) );
+    filtered_pose.cov[0] = trajectory_.back().cov_xx();
+    filtered_pose.cov[1] = trajectory_.back().cov_xy();
+    filtered_pose.cov[4] = trajectory_.back().cov_xy();
+    filtered_pose.cov[5] = trajectory_.back().cov_yy();
+
+	//covariance of velocity prediction, average of uncertainty propagated from estimated positions, equidistribited
+	double dt;
+	Eigen::Matrix2d v_cov_joint = Eigen::Matrix2d::Zero();
+	for( unsigned int i = 0; i < window_elements ; i++)
+	{
+		if( i == 0 )
+			dt = R(1,1)-R(0,1);
+		else
+			dt = R(i,1)-R(i-1,1);
+		Eigen::Matrix2d v_cov;
+		v_cov << trajectory_.at(i).cov_xx() , trajectory_.at(i).cov_xy(),
+				trajectory_.at(i).cov_xy() , trajectory_.at(i).cov_yy();
+		v_cov_joint += v_cov / (dt*dt);
+	}
+
+	//regression covariances + projected uncertainty from positions P_x / dt
+	double N = 2.0*(double)window_elements*(double)window_elements;
+	filtered_pose.cov[10] = v_cov_joint(0,0)/N;
+	filtered_pose.cov[11] = v_cov_joint(0,1)/N;
+	filtered_pose.cov[14] = filtered_pose.cov[11];
+	filtered_pose.cov[15] = v_cov_joint(1,1)/N;
+	if(debug_filter_person_velocity_){
+		std::cout << " PPPPPPPP (Cperson_bhmip::filter_current_state_linear_regression_bayes) filtered_pose: " << std::endl;
+		filtered_pose.print();
+	}
+
+	return filtered_pose;
+}
+
+SpointV_cov Cperson_bhmip::low_pass_filter_current_state_linear_regression( )
+{
+
+	//Discard a single and double pose trajectory --------------------------------------------
+	if (trajectory_.size() <= 2 )
+	{
+		return trajectory_.back();
+	}
+	// First order Regression: y(t) ~ beta * x(t) = beta_1 + beta_2 * x(t) ----------------------------
+	//vector and matrices initialization
+	unsigned int window_elements = trajectory_.size();
+
+	//if(debug_filter_person_velocity_){
+	//std::cout << " PPPPPPPP (Cperson_bhmip::low_pass_filter_current_state_linear_regression) window_elements="<<window_elements <<"; time_window_="<<time_window_<< std::endl;
+	//}
+//	double initial_ts = trajectory_.front().time_stamp;
+	Eigen::VectorXd vx ( window_elements ) , vy ( window_elements );
+	Eigen::VectorXd R (window_elements ); //0st order regression -> 1 coeffs
+	Eigen::VectorXd r_pred (window_elements);
+	for (unsigned int i = 0 ; i < window_elements ; i++)
+	{
+		vx ( i ) = trajectory_.at(i).vx;
+		vy ( i ) = trajectory_.at(i).vy;
+		//std::cout <<"i="<<i<< "; trajectory_.at(i).vx= "<<trajectory_.at(i).vx<< "; trajectory_.at(i).vy= "<<trajectory_.at(i).vy<< std::endl;
+		R(i)=1;
+		r_pred(i)=1;
+	}
+	//pseudo inverse of the R matrix ---------------------------------------------------------------
+	Eigen::VectorXd invR (  window_elements );
+	double sqrR =  R.dot( R);
+	invR = (1/sqrR) * R.transpose() ;
+
+	//linear regression ----------------------------------------------------------------------------
+
+	//invR * v = beta -> parameter estimation
+	double vx_coef =  invR.dot(vx.transpose());
+	double vy_coef = invR.dot(vy.transpose());
+
+	//std::cout << "vx_coef= "<<vx_coef<< std::endl;
+	//std::cout << "vy_coef= "<<vy_coef<< std::endl;
+
+	SpointV_cov filtered_pose(
+			trajectory_.back().x, //no filtering at position
+			trajectory_.back().y,
+			trajectory_.back().time_stamp,
+			vx_coef ,
+			vy_coef );
+
+    filtered_pose.cov[0] = trajectory_.back().cov_xx();
+    filtered_pose.cov[1] = trajectory_.back().cov_xy();
+    filtered_pose.cov[4] = trajectory_.back().cov_xy();
+    filtered_pose.cov[5] = trajectory_.back().cov_yy();
+
+
+	//covariance of velocity prediction, with respect to regression velocity
+	Eigen::VectorXd vx_mean ( window_elements ) , vy_mean ( window_elements );
+	vx_mean = R*vx_coef;
+	vy_mean = R*vy_coef;
+	//double cov_vx = (vx - vx_mean).dot( vx - vx_mean );
+	//double cov_vy = (vy - vy_mean).dot( vy - vy_mean );
+	//double cov_vxy = (vy - vy_mean).dot( vx - vx_mean );
+
+	// cambiado por (ely)
+	double cov_vx = ((vx - vx_mean).dot( vx - vx_mean ))/window_elements;
+	double cov_vy = ((vy - vy_mean).dot( vy - vy_mean ))/window_elements;
+	double cov_vxy = ((vy - vy_mean).dot( vx - vx_mean ))/window_elements;
+
+
+	//regression covariances + projected uncertainty from positions P_x / dt
+	filtered_pose.cov[10] = cov_vx;// + 0.001;
+	filtered_pose.cov[11] = cov_vxy;
+	filtered_pose.cov[14] = filtered_pose.cov[11];
+	filtered_pose.cov[15] = cov_vy;// + 0.001;
+
+	//if(debug_filter_person_velocity_){
+		//std::cout << " PPPPPPPP (Cperson_bhmip::low_pass_filter_current_state_linear_regression) filtered_pose: " << std::endl;
+		//filtered_pose.print();
+	//}
+
+	return filtered_pose;
+}
+
+
+void  Cperson_bhmip::intention_precalculation()
+{
+	if(debug_intention_precalculation_){
+		std::cout << " entro en intention_precalculation() " << std::endl;
+	}
+
+		double phi , phi_prob;
+		std::vector<double> s;
+		phi_pose2dest_.push_back( s );
+		phi_prob_.push_back( s );
+		for(unsigned int i = 0; i < destinations_.size() ; ++i)
+		{
+			double dx = destinations_[i].x - current_pointV_.x;
+			double dy = destinations_[i].y - current_pointV_.y;
+			phi = diffangle( atan2(  dy , dx ) , current_pointV_.orientation());
+			phi_pose2dest_.back().push_back( phi );
+			//c * Pr ( phi | destination_i) calculations
+			phi_prob = 100*exp( - phi*phi / phi_var_ );
+			phi_prob_.back().push_back( phi_prob );
+
+			/*std::cout << " destinations_.id="<<destinations_[i].id<< std::endl;
+			std::cout << "  angle2=atan2(  dy , dx )="<< atan2(  dy , dx )*180/3.14<< std::endl;
+			std::cout << "  diffangle( atan2(  dy , dx ) , current_pointV_.orientation())="<< diffangle( atan2(  dy , dx ) , current_pointV_.orientation())*180/3.14<< std::endl;
+			std::cout << "  phi_prob="<< phi_prob<<"; phi_var_="<<phi_var_<< std::endl;*/
+
+		}
+		//std::cout << " current_pointV_.orientation()="<<current_pointV_.orientation()*180/3.14<< std::endl;
+
+
+		if(debug_intention_precalculation_){
+			std::cout << " salgo de intention_precalculation() phi_pose2dest_.size()<="<<phi_pose2dest_.size()<<"; phi_prob_.size()="<<phi_prob_.size() << std::endl;
+		}
+
+		// test destination companion_person!
+		//if(){
+
+		//}
+
+}
+
+void Cperson_bhmip::trajectory_windowing()
+{
+	if(debug_trajectory_windowing_){
+		 std::cout << "(trajectory_windowing()) time_window_="<<time_window_<< std::endl;
+		// std::cout << "type_="<<type_<< std::endl;
+	 }
+	//std::cout << "!!!!!!111 !!!!!!!!!! 8888 trajectory_.size()="<<trajectory_.size()<< std::endl;
+
+	while(  trajectory_.back().time_stamp  - trajectory_.front().time_stamp >  time_window_ )
+	{
+		 //std::cout << "trajectory_.back().time_stamp="<<trajectory_.back().time_stamp<<"; trajectory_.front().time_stamp"<<trajectory_.front().time_stamp<<"; diference="<<trajectory_.back().time_stamp  - trajectory_.front().time_stamp << std::endl;
+
+		 //std::cout <<"; Cperson_abstract::Person="<<Person<<"; Cperson_abstract::Person_companion="<<Person_companion<<"; type_="<< type_ << std::endl;
+		 // std::cout << "trajectory_.size()="<<trajectory_.size()<<"; phi_pose2dest_.size()"<<phi_pose2dest_.size()<<"; phi_prob_.size()="<<phi_prob_.size()  << std::endl;
+		trajectory_.pop_front();
+		if(((type_ == Person)||(type_ == Person_companion))&&(phi_pose2dest_.size()>trajectory_.size())&&(phi_prob_.size()>trajectory_.size()))//Robot and Virtual_Person do not require prediction methods
+		{
+			phi_pose2dest_.pop_front();
+			phi_prob_.pop_front();
+		}
+	}
+
+
+
+
+
+}
+
+void Cperson_bhmip::refresh_person( double now )
+{
+	if(debug_refresh_person_){
+		std::cout << "entro en:Cperson_bhmip::refresh_person."<< std::endl;
+	}
+	//this method is used when manual remove of persons is required and thus, we must
+	//guarantee that the trajectory container and other variables are OK
+ // Es para mantener la ventana al mismo tamaño siempre.
+	if (trajectory_.empty() )
+	{
+		if(debug_refresh_person_){
+			std::cout << "in trajectory_.empty()"<< std::endl;
+			std::cout << "(inicial) time_window_="<<time_window_<<"; trajectory_.back().time_stamp - now="<<trajectory_.back().time_stamp - now<< std::endl;
+		}
+
+		while( trajectory_.back().time_stamp - now > time_window_)
+		{
+			if(debug_refresh_person_){
+				std::cout << "(while) time_window_="<<time_window_<<"; trajectory_.back().time_stamp - now="<<trajectory_.back().time_stamp - now<< std::endl;
+			}
+
+			trajectory_.pop_front();
+			if (trajectory_.empty() ) break;
+		}
+	}
+	now_ = now;
+}
+
+void Cperson_bhmip::prediction( double min_v_to_predict )
+{
+	//std::cout << " 333333377777777 IN Cperson_bhmip::prediction( double min_v_to_predict ) current_pointV_.v()="<<current_pointV_.v()<< std::endl;
+
+	if(debug_prediction_){
+		std::cout << "IN Cperson_bhmip::prediction( double min_v_to_predict ) destinations_.size()="<<destinations_.size()<< std::endl;
+		for(unsigned int y=0;y<destinations_.size();y++){
+			std::cout << "IN Cperson_bhmip::prediction destinations_[y].x="<<destinations_[y].x<<"; destinations_[y].y="<<destinations_[y].y<< std::endl;
+
+		}
+	}
+	//calculate if it is adequate to predict intentionality:
+	//if target is almost stopped and decelerating
+	//std::cout << "!!!!!!!!!!!!!!!!!!!!!!!!! OUT (case current_pointV_.v() <  min_v_to_predict); min_v_to_predict="<< min_v_to_predict<<"; current_pointV_.v()="<<current_pointV_.v()<< std::endl;
+	//std::cout << "desired_velocity_ = 0.0; best destination:"<< std::endl;
+
+	if( current_pointV_.v() <  min_v_to_predict)
+	{
+
+		best_destination_ = Sdestination( 0, current_pointV_.x, current_pointV_.y,
+				1.0, Sdestination::Stopping);
+
+		desired_velocity_ = 0.0;//to avoid being too reactive to its environment, it remains in place v_d = 0.0
+
+		//if(debug_prediction_){
+			//std::cout << "(case current_pointV_.v() <  min_v_to_predict); min_v_to_predict="<< min_v_to_predict<<"; current_pointV_.v()="<<current_pointV_.v()<< std::endl;
+			//std::cout << "desired_velocity_ = 0.0; best destination:"<< std::endl;
+			//best_destination_.print();
+			//before_best_destination_.print();
+		//}
+
+		return;
+	}
+
+	//calculate probabilities to destinations
+	double norm_term(0.0),max_prob(-0.1);
+	unsigned int max_prob_index;
+	posterior_destinations_prob_.clear();
+	//std::cout <<"; destinations_.size()"<<destinations_.size() << std::endl;
+
+	for( unsigned int i = 0; i<destinations_.size() ; ++i)
+	{
+		//std::cout <<"; destinations_"<<destinations_[i].id <<"  norm_term ="<<norm_term<<"; destinations_[i].prob="<<destinations_[i].prob<< std::endl;
+		posterior_destinations_prob_.push_back( destinations_[i].prob);
+
+		if(debug_prediction_){
+			std::cout << "destinations_[i].prob="<<destinations_[i].prob<< std::endl;
+		}
+		for (unsigned int j = 0; j < phi_prob_.size() ; ++j)
+		{
+			//phi_probabilities [ time ] [ destination ]
+			posterior_destinations_prob_[i] *= phi_prob_[j][i];
+			//std::cout << "phi_prob_["<<j<<"]["<<i<<"]="<<phi_prob_[j][i]<< std::endl;
+			//std::cout << "posterior_destinations_prob_[i]="<<posterior_destinations_prob_[i]<< std::endl;
+		}
+		norm_term += posterior_destinations_prob_[i];
+		//std::cout <<"norm_term= "<<norm_term<< std::endl;
+
+		if( max_prob < posterior_destinations_prob_[i])
+		{
+			max_prob_index = i;
+			max_prob = posterior_destinations_prob_[i];
+			//std::cout <<"max_prob= "<<max_prob<< std::endl;
+		}
+	}
+
+	//uncertain behavior of prediction depending on certain threshold
+	if( 0) //max_prob < 1e16 )//hardcoded threshold corresponds aprox to a diff of 45º
+	{
+		best_destination_ = Sdestination( 0, current_pointV_.x, current_pointV_.y,
+				1.0, Sdestination::Uncertain);
+		if(debug_prediction_){
+			std::cout << "if(0); best destination:"<< std::endl;
+			best_destination_.print();
+		}
+		return;
+	}
+	//if(debug_prediction_){
+
+	//}
+	//normalization of probabilities
+	norm_term = 1/norm_term;
+
+	if(debug_prediction_){
+		std::cout << " norm_term ="<<norm_term<< std::endl;
+	}
+
+	if ( !is_nan( norm_term ) && !destinations_.empty() )
+	{
+		for (unsigned int i = 0 ; i < destinations_.size() ; ++i)
+			posterior_destinations_prob_[i] *= norm_term;
+		best_destination_ = destinations_[max_prob_index];
+		best_destination_.prob = posterior_destinations_prob_[max_prob_index];
+
+		if(debug_prediction_){
+			std::cout << " if(!is_nan( norm_term ) && !destinations_.empty()) "<< std::endl;
+			best_destination_.print();
+			before_best_destination_.print();
+		}
+	}
+	else
+	{
+		best_destination_ = Sdestination( 0, current_pointV_.x, current_pointV_.y,
+				1.0, Sdestination::Uncertain);
+
+		if(debug_prediction_){
+			std::cout << " else if(!is_nan( norm_term ) && !destinations_.empty() )"<< std::endl;
+			best_destination_.print();
+		}
+	}
+
+
+	/*if(before_best_destination_.id!=best_destination_.id){
+		reset_before_destination_prob();
+	}*/
+
+	before_best_destination_=best_destination_;
+
+	if(debug_prediction_){
+		std::cout << "OUT Cperson_bhmip::prediction( double min_v_to_predict )"<< std::endl;
+	}
+}
+
+void Cperson_bhmip::reset(  )
+{
+	trajectory_.clear();
+	current_pointV_ = SpointV_cov();
+	diff_pointV_ = SpointV_cov();
+	phi_pose2dest_.clear();
+	phi_prob_.clear();
+	force_to_goal_ = Sforce() ;
+	force_int_person_ = Sforce() ;
+	force_obstacle_ = Sforce() ;
+	force_int_robot_ = Sforce() ;
+}
+
+void Cperson_bhmip::reset_before_destination_prob(){
+	phi_pose2dest_.clear();
+	phi_prob_.clear();
+}
+
+
+void Cperson_bhmip::rotate_and_translate_trajectory(double R, double thetaZ, double linear_vx, double linear_vy, double v_rot_x, double v_rot_y, std::vector<double> vect_odom_eigen_tf, bool debug_odometry){
+	// Function for local tracking. This function change the frame of the Current_pose and the window of poses for the person,
+	// tacking into account the actual position of the robot.
+	// Also compensate the rotational velocities applied to the person when the robot turns.
+		// R -> Robot Translation ; thetaZ -> Robot rotation
+
+
+	if(debug_odometry){
+	std::cout << "!!!!!!!!!!!!!!!!!! (people prediction) rotate_and_translate_trajectory !!!!!!!!!!!!!" << std::endl;
+	}
+	hom2_ant.clear();
+
+	Eigen::MatrixXd homT1; // homogeneous transfor para pasar los tracks al nuevo frame de la odometria.
+	Eigen::MatrixXd homT1vis;
+	Eigen::MatrixXd homT2;
+	Eigen::MatrixXd J; // necesaria para introducir el aumento de la covarianza por culpa del error en R=traslacion y en thetaZ=rotación.
+	Eigen::MatrixXd H_cov;
+	Eigen::MatrixXd P;
+	homT1.resize(3, 3);
+	homT1vis.resize(3, 3);
+	homT2.resize(3, 3);
+	J.resize(4, 2);
+	H_cov.resize(4, 4);
+	P.resize(2, 2);
+
+	/*homT1.row(0) << cos(thetaZ), -sin(thetaZ), R * cos(thetaZ/2);
+	homT1.row(1) << sin(thetaZ), cos(thetaZ), R * sin(thetaZ/2);
+	homT1.row(2) << 0, 0, 1;*/
+	
+	if(debug_odometry){
+	std::cout << "(people prediction) homT1 tf=" << std::endl;
+	std::cout << "["<<vect_odom_eigen_tf[0]<<","<<vect_odom_eigen_tf[1]<<","<< vect_odom_eigen_tf[2]<<"]" << std::endl;
+	std::cout << "["<<vect_odom_eigen_tf[3]<<","<<vect_odom_eigen_tf[4]<<","<< vect_odom_eigen_tf[5]<<"]" << std::endl;
+	std::cout << "["<<vect_odom_eigen_tf[6]<<","<<vect_odom_eigen_tf[7]<<","<< vect_odom_eigen_tf[8]<<"]"  << std::endl;
+	}
+	
+	homT1.row(0) << vect_odom_eigen_tf[0], vect_odom_eigen_tf[1], vect_odom_eigen_tf[2];
+	homT1.row(1) << vect_odom_eigen_tf[3], vect_odom_eigen_tf[4], vect_odom_eigen_tf[5];
+	homT1.row(2) << vect_odom_eigen_tf[6], vect_odom_eigen_tf[7], vect_odom_eigen_tf[8];
+
+	if(debug_odometry){
+	homT1vis.row(0) << cos(thetaZ), -sin(thetaZ), R * cos(thetaZ/2);
+	homT1vis.row(1) << sin(thetaZ), cos(thetaZ), R * sin(thetaZ/2);
+	homT1vis.row(2) << 0, 0, 1;
+		std::cout << "homT1vis _ prediction  = [ " << homT1(0, 0) << " , " << homT1(0, 1)
+				<< " , " << homT1(0, 2) << std::endl << "               "
+				<< homT1(1, 0) << " , " << homT1(1, 1) << " , " << homT1(1, 2)
+				<< std::endl << "               " << homT1(2, 0) << " , "
+				<< homT1(2, 1) << " , " << homT1(2, 2) << std::endl;
+	}
+
+	//homT2 = homT1.inverse();
+	homT2 = homT1;//.inverse();
+
+	if(debug_odometry){
+		std::cout << "homT2 _ prediction = [ " << homT2(0, 0) << " , " << homT2(0, 1)
+				<< " , " << homT2(0, 2) << std::endl << "               "
+				<< homT2(1, 0) << " , " << homT2(1, 1) << " , " << homT2(1, 2)
+				<< std::endl << "               " << homT2(2, 0) << " , "
+				<< homT2(2, 1) << " , " << homT2(2, 2) << std::endl;
+	}
+		/*hom2_ant.push_back(homT2(0, 0));
+		hom2_ant.push_back(homT2(0, 1));
+		hom2_ant.push_back(homT2(0, 2));
+		hom2_ant.push_back(homT2(1, 0));
+		hom2_ant.push_back(homT2(1, 1));
+		hom2_ant.push_back(homT2(1, 2));
+		hom2_ant.push_back(homT2(2, 0));
+		hom2_ant.push_back(homT2(2, 1));
+		hom2_ant.push_back(homT2(2, 2));
+		hom2_tf.push_front(hom2_ant);*/
+
+	H_cov.row(0) << cos(thetaZ), sin(thetaZ), 0, 0;
+	H_cov.row(1) << -sin(thetaZ), cos(thetaZ), 0, 0;
+	H_cov.row(2) << 0, 0, cos(thetaZ), sin(thetaZ);
+	H_cov.row(3) << 0, 0, -sin(thetaZ), cos(thetaZ);
+
+	P.row(0) << 10 * R / 100, 0; //[G_R^2       0    ]
+	P.row(1) << 0, 10 * thetaZ / 100; //[  0    G_thetaZ^2]
+
+	// Robot local velocity
+	trajectory_local_velocity_x_.push_back(linear_vx);
+	trajectory_local_velocity_y_.push_back(linear_vy);
+
+
+	// Change tracks trajectories to the actual robot frame.
+
+	for(unsigned int i=0; i<trajectory_.size(); i++){
+
+		/*Eigen::MatrixXd homT2_act;
+		for(unsigned int j=0;j=<i;j++){
+			homT2_act=homT2_act*hom2_tf
+		}*/
+
+		SpointV_cov track_ant=trajectory_[i];
+		if(debug_odometry){
+			std::cout << " Before (people prediction) trajectory_("<<i<<")"<< std::endl;
+				trajectory_[i].print();
+		}
+		SpointV_cov track_act; //trajectory point transformed.
+
+		Eigen::MatrixXd pos_ant(3,1);
+		Eigen::MatrixXd pos_act(3,1);
+		Eigen::MatrixXd vel_ant(3,1);
+		Eigen::MatrixXd vel_act(3,1);
+		Eigen::MatrixXd cov_ant(4,4);
+		Eigen::MatrixXd cov_act(4,4);
+		Eigen::MatrixXd robot_linear_vel(3,1);
+
+		robot_linear_vel(0, 0) = linear_vx;
+		robot_linear_vel(1, 0) = linear_vy;
+		robot_linear_vel(2, 0) = 0;
+
+		pos_ant(0, 0) = track_ant.x;
+		pos_ant(1, 0) = track_ant.y;
+		pos_ant(2, 0) = 1;
+
+		pos_act = homT2 * pos_ant;
+
+		vel_ant.row(0) << track_ant.vx;
+		vel_ant.row(1) << track_ant.vy;
+		vel_ant.row(2) << 0;
+
+		vel_act = homT2 * vel_ant;
+
+		//vel_act=vel_act-robot_linear_vel- w_robot x pos_act;
+
+		J.row(0) << -cos(thetaZ), -sin(thetaZ) * track_ant.x
+						+ cos(thetaZ) * track_ant.y + R * sin(thetaZ );
+		J.row(1) << sin(thetaZ), -cos(thetaZ) * track_ant.x
+						- sin(thetaZ) * track_ant.y + R * cos(thetaZ );
+		J.row(2) << 0, -sin(thetaZ) * track_ant.vx + cos(thetaZ) * track_ant.vy;
+		J.row(3) << 0, -cos(thetaZ) * track_ant.vx - sin(thetaZ) * track_ant.vy;
+
+		cov_ant.row(0) << track_ant.cov[0], track_ant.cov[1], track_ant.cov[2], track_ant.cov[3];
+		cov_ant.row(1) << track_ant.cov[4], track_ant.cov[5], track_ant.cov[6], track_ant.cov[7];
+		cov_ant.row(2) << track_ant.cov[8], track_ant.cov[9], track_ant.cov[10], track_ant.cov[11];
+		cov_ant.row(3) << track_ant.cov[12], track_ant.cov[13], track_ant.cov[14], track_ant.cov[15];
+
+		//cov_act = H_cov * cov_ant * H_cov.transpose() + J * P * J.transpose();
+		cov_act = H_cov * cov_ant * H_cov.transpose();
+
+		std::vector<double> vect_cov_act;
+		vect_cov_act.reserve(16);
+		vect_cov_act.resize(16, 0.0);
+		vect_cov_act[0] = (double) cov_act(0, 0); // cov_xx
+		vect_cov_act[1] = (double) cov_act(0, 1); //cov_xy
+		vect_cov_act[2] = (double) cov_act(0, 2); // cov_xvx
+		vect_cov_act[3] = (double) cov_act(0, 3); //cov_xvy
+		vect_cov_act[4] = (double) cov_act(1, 0); //cov_yx
+		vect_cov_act[5] = (double) cov_act(1, 1); //cov_yy
+		vect_cov_act[6] = (double) cov_act(1, 2); //cov_yvx
+		vect_cov_act[7] = (double) cov_act(1, 3); //cov_yvy
+		vect_cov_act[8] = (double) cov_act(2, 0); // cov_vxvx
+		vect_cov_act[9] = (double) cov_act(2, 1); //cov_vxvy
+		vect_cov_act[10] = (double) cov_act(2, 2); // cov_vxx
+		vect_cov_act[11] = (double) cov_act(2, 3); //cov_vxy
+		vect_cov_act[12] = (double) cov_act(3, 0); //cov_vyvx
+		vect_cov_act[13] = (double) cov_act(3, 1); //cov_vyvy
+		vect_cov_act[14] = (double) cov_act(3, 2); //cov_vyx
+		vect_cov_act[15] = (double) cov_act(3, 3); //cov_vyy
+
+		track_act = SpointV_cov(pos_act(0, 0), pos_act(1, 0),track_ant.time_stamp, vel_act(0, 0), vel_act(1, 0),vect_cov_act);
+
+		trajectory_[i]=track_act;
+
+		if(debug_odometry){
+			std::cout << "(people prediction) track_ant("<<i<<")" << std::endl;
+			track_ant.print();
+			std::cout << " (people prediction) track_act("<<i<<")"<< std::endl;
+			track_act.print();
+			std::cout << " After (people prediction) trajectory_("<<i<<")"<< std::endl;
+			trajectory_[i].print();
+		}
+	}
+
+	// change current_pointV_ with odometry
+	SpointV_cov ant_current_pointV=current_pointV_;
+	if(debug_odometry){
+		std::cout << " ANT current_pointV_="<< std::endl;
+		current_pointV_.print();
+	}
+	//SpointV_cov act_current_pointV;
+	Eigen::MatrixXd pos_ant_current_pointV(3,1);
+	Eigen::MatrixXd pos_act_current_pointV(3,1);
+	Eigen::MatrixXd vel_ant_current_pointV(3,1);
+	Eigen::MatrixXd vel_act_current_pointV(3,1);
+	Eigen::MatrixXd cov_ant_current_pointV(4,4);
+	Eigen::MatrixXd cov_act_current_pointV(4,4);
+
+	pos_ant_current_pointV(0, 0) = ant_current_pointV.x;
+	pos_ant_current_pointV(1, 0) = ant_current_pointV.y;
+	pos_ant_current_pointV(2, 0) = 1;
+
+	pos_act_current_pointV = homT2 * pos_ant_current_pointV;
+
+	vel_ant_current_pointV.row(0) << ant_current_pointV.vx;
+	vel_ant_current_pointV.row(1) << ant_current_pointV.vy;
+	vel_ant_current_pointV.row(2) << 0;
+
+	vel_act_current_pointV = homT2 * vel_ant_current_pointV;
+
+	J.row(0) << -cos(thetaZ), -sin(thetaZ) * ant_current_pointV.x + cos(thetaZ) * ant_current_pointV.y + R * sin(thetaZ / 2);
+	J.row(1) << sin(thetaZ), -cos(thetaZ) * ant_current_pointV.x - sin(thetaZ) * ant_current_pointV.y + R * cos(thetaZ / 2);
+	J.row(2) << 0, -sin(thetaZ) * ant_current_pointV.vx + cos(thetaZ) * ant_current_pointV.vy;
+	J.row(3) << 0, -cos(thetaZ) * ant_current_pointV.vx - sin(thetaZ) * ant_current_pointV.vy;
+
+	cov_ant_current_pointV.row(0) << ant_current_pointV.cov[0], ant_current_pointV.cov[1], ant_current_pointV.cov[2], ant_current_pointV.cov[3];
+	cov_ant_current_pointV.row(1) << ant_current_pointV.cov[4], ant_current_pointV.cov[5], ant_current_pointV.cov[6], ant_current_pointV.cov[7];
+	cov_ant_current_pointV.row(2) << ant_current_pointV.cov[8], ant_current_pointV.cov[9], ant_current_pointV.cov[10], ant_current_pointV.cov[11];
+	cov_ant_current_pointV.row(3) << ant_current_pointV.cov[12], ant_current_pointV.cov[13], ant_current_pointV.cov[14], ant_current_pointV.cov[15];
+
+	cov_act_current_pointV = H_cov * cov_ant_current_pointV * H_cov.transpose() + J * P * J.transpose();
+
+	std::vector<double> vect_cov_act_current_pointV;
+	vect_cov_act_current_pointV.reserve(16);
+	vect_cov_act_current_pointV.resize(16, 0.0);
+	vect_cov_act_current_pointV[0] = (double) cov_act_current_pointV(0, 0); // cov_xx
+	vect_cov_act_current_pointV[1] = (double) cov_act_current_pointV(0, 1); //cov_xy
+	vect_cov_act_current_pointV[2] = (double) cov_act_current_pointV(0, 2); // cov_xvx
+	vect_cov_act_current_pointV[3] = (double) cov_act_current_pointV(0, 3); //cov_xvy
+	vect_cov_act_current_pointV[4] = (double) cov_act_current_pointV(1, 0); //cov_yx
+	vect_cov_act_current_pointV[5] = (double) cov_act_current_pointV(1, 1); //cov_yy
+	vect_cov_act_current_pointV[6] = (double) cov_act_current_pointV(1, 2); //cov_yvx
+	vect_cov_act_current_pointV[7] = (double) cov_act_current_pointV(1, 3); //cov_yvy
+	vect_cov_act_current_pointV[8] = (double) cov_act_current_pointV(2, 0); // cov_vxvx
+	vect_cov_act_current_pointV[9] = (double) cov_act_current_pointV(2, 1); //cov_vxvy
+	vect_cov_act_current_pointV[10] = (double) cov_act_current_pointV(2, 2); // cov_vxx
+	vect_cov_act_current_pointV[11] = (double) cov_act_current_pointV(2, 3); //cov_vxy
+	vect_cov_act_current_pointV[12] = (double) cov_act_current_pointV(3, 0); //cov_vyvx
+	vect_cov_act_current_pointV[13] = (double) cov_act_current_pointV(3, 1); //cov_vyvy
+	vect_cov_act_current_pointV[14] = (double) cov_act_current_pointV(3, 2); //cov_vyx
+	vect_cov_act_current_pointV[15] = (double) cov_act_current_pointV(3, 3); //cov_vyy
+
+	//double track_vx=vel_act_current_pointV(0,0) - v_rot_x;
+	//double track_vy=vel_act_current_pointV(1,0) - v_rot_y;
+
+	//vel_act_current_pointV(0,0)=track_vx;
+	//vel_act_current_pointV(1,0)=track_vy;
+
+
+
+	current_pointV_ = SpointV_cov(pos_act_current_pointV(0, 0), pos_act_current_pointV(1, 0), ant_current_pointV.time_stamp, vel_act_current_pointV(0,0), vel_act_current_pointV(1,0),vect_cov_act_current_pointV);
+	if(debug_odometry){
+		std::cout << " AcT current_pointV_="<< std::endl;
+		current_pointV_.print();
+	}
+}
diff --git a/local_lib_people_prediction/src/scene_elements/person_bhmip.h b/local_lib_people_prediction/src/scene_elements/person_bhmip.h
new file mode 100644
index 0000000000000000000000000000000000000000..54bdd4effcd092484925e768392fa284a1f1409d
--- /dev/null
+++ b/local_lib_people_prediction/src/scene_elements/person_bhmip.h
@@ -0,0 +1,81 @@
+/*
+ * person_bhmip.h
+ *
+ *  Created on: Jul 9, 2013.  Last Modified on 2020
+ *      Author: Initial code of the AKP Gonzalo Ferrer.
+ *      Author: Addition of ASSAOP and ASSAGP for people accompaniment code of Ely Repiso.
+ */
+
+#ifndef PERSON_BHMIP_H_
+#define PERSON_BHMIP_H_
+
+#include "scene_elements/person_abstract.h"
+
+class Cperson_bhmip : public Cperson_abstract
+{
+  public:
+	Cperson_bhmip(unsigned int id=0,
+			Cperson_abstract::target_type person_target_type=Cperson_abstract::Person,
+			Cperson_abstract::force_type person_force_type=Cperson_abstract::Spherical,
+			double _time_window = 5.0);
+	virtual ~Cperson_bhmip();
+	const std::deque<SpointV_cov>* get_trajectory2() {return &trajectory_;}
+	//std::deque<std::vector<double> >& get_phi_prob() { return phi_prob_; }
+	//std::deque<std::vector<double> >& get_phi_pose2dest() { return phi_pose2dest_; }
+	virtual void reset();
+	virtual void reset_before_destination_prob();
+	virtual void add_pointV( SpointV_cov point,
+			Cperson_abstract::filtering_method filter=Cperson_abstract::Linear_regression_filtering,  bool robot_or_person=false, double in_new_value_windowing=6.0);
+	virtual void Cperson_bhmip_add_pointV( SpointV_cov point, Cperson_abstract::filtering_method filter=Cperson_abstract::Linear_regression_filtering, bool person_or_robot=false);
+	virtual void refresh_person( double now );
+	virtual void prediction(  double min_v_to_predict );
+
+	virtual void rotate_and_translate_trajectory(double R, double thetaZ,
+			double linear_vx, double linear_vy, double v_rot_x, double v_rot_y, std::vector<double> vect_odom_eigen_tf, bool debug_odometry = false); // For local tracker.
+
+	void set_debug_filter_person_velocity(bool debug_filter_person_velocity_in){
+		debug_filter_person_velocity_=debug_filter_person_velocity_in;
+	}
+
+	virtual const std::deque<SpointV_cov>* get_past_trajectory(){return &trajectory_;};
+
+	virtual void set_new_time_window(double in_new_time_window){
+		time_window_=in_new_time_window;
+	}
+
+  private:
+	void trajectory_windowing( );
+	std::deque<SpointV_cov> trajectory_;
+	double time_window_; //0.5 segs
+	SpointV_cov filter_current_state_linear_regression( );
+	SpointV_cov filter_current_state_linear_regression_bayes( );
+	SpointV_cov low_pass_filter_current_state_linear_regression( );
+	void  intention_precalculation();
+	std::vector<double> posterior_destinations_prob_;
+	double phi_var_;
+	std::deque<std::vector<double> > phi_pose2dest_ , phi_prob_;
+	double time_from_last_update_;
+
+
+	// variables for local tracker
+	std::deque<std::vector<double> > hom2_tf;
+	std::vector<double> hom2_ant;
+	std::deque<double> trajectory_local_velocity_x_;
+	std::deque<double> trajectory_local_velocity_y_;
+
+	bool debug_filter_person_velocity_; // add in addaptative companion ely
+	bool debug_add_pointV_; // debug for simulation person companion.
+	bool debug_Cperson_bhmip_add_pointV_; // debug for simulation person companion.
+	bool debug_trajectory_windowing_;// debug for simulation person companion.
+	bool debug_refresh_person_;// debug for simulation person companion.
+	bool debug_prediction_;// debug for simulation person companion.
+	bool debug_intention_precalculation_;// debug for simulation person companion.
+	bool debug_filter_current_state_linear_regression_;
+
+	bool person_before_moving_;
+	Sdestination before_best_destination_;
+
+};
+
+
+#endif /* PERSON_BHMIP_H_ */
diff --git a/local_lib_people_prediction/src/scene_elements/person_virtual.cpp b/local_lib_people_prediction/src/scene_elements/person_virtual.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6f5d80e94166acf5ec8a4e510373e1b2d9dc0a46
--- /dev/null
+++ b/local_lib_people_prediction/src/scene_elements/person_virtual.cpp
@@ -0,0 +1,61 @@
+/*
+ * person_virtual.cpp
+ *
+ *  Created on: Jul 9, 2013.Last Modified on 2020
+ *      Author: Initial code of the AKP Gonzalo Ferrer.
+ *      Author: Addition of ASSAOP and ASSAGP for people accompaniment code of Ely Repiso.
+ */
+
+#include "scene_elements/person_virtual.h"
+
+Cperson_virtual::Cperson_virtual( unsigned int id, Cperson_abstract::target_type person_target_type,
+		Cperson_abstract::force_type person_force_type,
+		double _time_window ) :
+	//Cperson_abstract(id,person_target_type, person_force_type)
+	//Cperson_bhmip(id, person_target_type, person_force_type, _time_window)
+		Cperson_behavior(id,Cperson_abstract::Person,Cperson_abstract::Spherical,_time_window)
+{
+
+}
+
+Cperson_virtual::~Cperson_virtual()
+{
+
+}
+
+void Cperson_virtual::add_pointV( SpointV_cov point, Cperson_abstract::filtering_method filter )
+{
+	std::cout<<" (Cperson_virtual:) entro en add_pointV!"<< std::endl;
+	diff_pointV_ = point - current_pointV_ ;
+	//resets covariance to predefined values, if not,
+	//it is a growing covariance function due to feedback propagation
+	current_pointV_ = SpointV_cov();
+	current_pointV_.x = point.x;
+	current_pointV_.y = point.y;
+	current_pointV_.vx = point.vx;
+	current_pointV_.vy = point.vy;
+	current_pointV_.time_stamp = point.time_stamp;
+	now_ = current_pointV_.time_stamp;
+}
+
+void Cperson_virtual::Cperson_bhmip_add_pointV( SpointV_cov point, Cperson_abstract::filtering_method filter, bool robot_or_person)
+{
+	std::cout<<" (Cperson_virtual) entro en add_pointV!"<< std::endl;
+	Cperson_behavior::add_pointV(point,filter,robot_or_person);
+	//Cperson_bhmip::Cperson_bhmip_add_pointV(point,filter);
+	//calculate expected behavior is done in the estimation method (in scene)
+}
+
+
+void Cperson_virtual::prediction( double min_v_to_predict)
+{
+
+}
+
+void Cperson_virtual::reset()
+{
+	current_pointV_ = SpointV_cov();
+	diff_pointV_ = SpointV_cov();
+	best_destination_ = Sdestination();
+	destinations_.clear();
+}
diff --git a/local_lib_people_prediction/src/scene_elements/person_virtual.h b/local_lib_people_prediction/src/scene_elements/person_virtual.h
new file mode 100644
index 0000000000000000000000000000000000000000..b6587aa8502be789d66a611d23412a3f86d76f4f
--- /dev/null
+++ b/local_lib_people_prediction/src/scene_elements/person_virtual.h
@@ -0,0 +1,48 @@
+/*
+ * person_virtual.h
+ *
+ *  Created on: Jul 9, 2013. Last Modified on 2020
+ *      Author: Initial code of the AKP Gonzalo Ferrer.
+ *      Author: Addition of ASSAOP and ASSAGP for people accompaniment code of Ely Repiso.
+ */
+
+
+#ifndef PERSON_VIRTUAL_H_
+#define PERSON_VIRTUAL_H_
+
+//#include "scene_elements/person_abstract.h"
+//#include "scene_elements/person_bhmip.h"
+#include "scene_elements/person_behavior.h"
+/**
+ *
+ * \brief Person_virtual Class
+ *
+ * The person virtual class is designed to contain the minimum amount of information
+ * required to observe a person. No intentionality prediction is done (it is assumed known).
+ *
+ * It is mainly designed for simulations and for propagating an observed scene, thereby
+ * requiring the minimum amount of calculations to propagate.
+ *
+ * different types of persons are also available: Person , Robot , Obstacle
+ *
+*/
+class Cperson_virtual : public Cperson_behavior //public Cperson_abstract
+{
+  public:
+	Cperson_virtual( unsigned int id=0 ,
+			Cperson_abstract::target_type person_target_type=Cperson_abstract::Person,
+			Cperson_abstract::force_type person_force_type=Cperson_abstract::Spherical,
+			double _time_window = 1.0);
+	virtual ~Cperson_virtual();
+	virtual void add_pointV( SpointV_cov point,
+			Cperson_abstract::filtering_method filter=Cperson_abstract::Linear_regression_filtering );
+	virtual void Cperson_bhmip_add_pointV( SpointV_cov point, Cperson_abstract::filtering_method filter, bool robot_or_person=false);
+	virtual void refresh_person( double now ) {} //this method is not used when Virtual people
+	virtual void prediction( double min_v_to_predict);
+	virtual void reset();
+
+  private:
+
+};
+
+#endif /* PERSON_VIRTUAL_H_ */
diff --git a/local_lib_people_prediction/src/scene_elements/robot.cpp b/local_lib_people_prediction/src/scene_elements/robot.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..0099b31dac50819a88a8236bca3e954109e4a0cc
--- /dev/null
+++ b/local_lib_people_prediction/src/scene_elements/robot.cpp
@@ -0,0 +1,938 @@
+/*
+ * robot.cpp
+ *
+ *  Created on: Jul 31, 2013.  Last Modified on 2020
+ *      Author: Initial code of the AKP Gonzalo Ferrer.
+ *      Author: Addition of ASSAOP and ASSAGP for people accompaniment code of Ely Repiso.
+ */
+
+#include "scene_elements/robot.h"
+#include <math.h>
+#include <iostream>
+
+Crobot::Crobot(  unsigned int id , robot_type robot_type,
+		Cperson_abstract::force_type person_force_type, double _time_window) :
+	Cperson_abstract( id, Cperson_abstract::Robot, person_force_type),
+	v_max_(1.0), w_max_(3.0), a_v_max_(2.0), a_v_break_(2.0),a_w_max_(5.0), a_v_max_Vrobotzero_(0.6), lim_to_consider_robotZero_(0.1),
+	platform_radii_(1.0), platform_radii_2_(1.0),
+	debug_antes_subgoals_entre_AKP_goals_(false),
+	test_w_max_(0.0), test_aw_max_(0.0),
+	debug_velocity_file2_(true),
+	//debug_velocity_cout2_(true),
+	debug_robot_propagation_(false),
+	high_vel_dampening_parameter_(2.6),
+	normal_vel_dampening_parameter_(1.6),
+	slow_vel_dampening_parameter_(0.7),
+	limit_linear_vel_for_dampening_parameter_(0.2),
+	limit_angular_vel_for_dampening_parameter_(0.5),
+	//augment_initial_v_(false),
+	initial_v_robot_needed_(0.17)
+{
+}
+
+Crobot::~Crobot()
+{
+
+}
+
+void Crobot::add_pose( Spose  observation )
+{
+	//TODO take into account covariances
+	diff_pose_ = observation - current_pose_;
+	current_pose_ = observation;
+	SpointV_cov current_point = SpointV_cov( observation.x, observation.y,observation.time_stamp,
+			observation.v*cos(observation.theta), observation.v*sin(observation.theta) );
+	diff_pointV_ = current_point - current_pointV_ ;
+	current_pointV_ = current_point;
+}
+
+void Crobot::prediction( double min_v_to_predict)
+{
+
+}
+
+void Crobot::reset()
+{
+	current_pointV_ = SpointV_cov();
+	current_pose_ = Spose();
+	diff_pointV_ = SpointV_cov();
+	best_destination_ = Sdestination();
+	destinations_.clear();
+	planning_trajectory_.clear();
+	planning_SpointV_trajectory_.clear();
+	rnd_goal_ = Spoint();
+    planning_v_.clear();
+    planning_w_.clear();
+    planning_x_.clear();
+    planning_y_.clear();
+    planning_theta_.clear();
+    planning_before_av_.clear();
+    planning_before_aw_.clear();
+    planning_after_av_.clear();
+    planning_after_aw_.clear();
+    planning_damping_aw_.clear();
+}
+
+void Crobot::reset_before_destination_prob(){
+	best_destination_ = Sdestination();
+	destinations_.clear();
+}
+void Crobot::clear_planning_trajectory()
+{
+    planning_trajectory_.clear();
+    planning_trajectory_.push_back(current_pose_);
+    planning_SpointV_trajectory_.clear();
+    planning_SpointV_trajectory_.push_back( current_pointV_);
+    planning_v_.clear();
+    planning_w_.clear();
+    planning_x_.clear();
+    planning_y_.clear();
+    planning_theta_.clear();
+    planning_before_av_.clear();
+    planning_before_aw_.clear();
+    planning_after_av_.clear();
+    planning_after_aw_.clear();
+    planning_damping_aw_.clear();
+
+
+
+    time_stamp_plan_.clear();
+    for(unsigned int g=0;g<500;g++){
+    	 time_stamp_plan_.push_back(0.0);
+    }
+
+
+
+}
+
+void Crobot::erase_last_planning_propagation()
+{
+    planning_trajectory_.pop_back();
+    planning_SpointV_trajectory_.pop_back();
+}
+/*
+void Crobot::robot_propagation(double dt , unsigned int index, double v, double w)
+{
+	//Kinematic unicycle TODO propagate covariances
+	//std::cout << "robot_propagation 1 " <<  std::endl;
+	Spose propagated_pose,ini_pose;
+	ini_pose = planning_trajectory_.at(index);
+	if( v > v_max_) v=v_max_;
+	if( v < -v_max_) v=-v_max_;
+	if( w > w_max_) w=w_max_;
+	if( w < -w_max_) w=-w_max_;
+	propagated_pose.x = ini_pose.x + v*cos(ini_pose.theta)*dt;
+	propagated_pose.y = ini_pose.y + v*sin(ini_pose.theta)*dt;
+	propagated_pose.theta = ini_pose.theta + w*dt;
+	propagated_pose.time_stamp = ini_pose.time_stamp + dt;
+	propagated_pose.v = v;
+	propagated_pose.w = w;
+	//update
+	planning_trajectory_.push_back(propagated_pose);
+	planning_SpointV_trajectory_.push_back( SpointV_cov(propagated_pose.x,propagated_pose.y,
+				propagated_pose.time_stamp, propagated_pose.v*cos(propagated_pose.theta),
+				propagated_pose.v*sin(propagated_pose.theta)) );
+
+
+}*/
+
+
+
+void Crobot::robot_propagation(double dt , unsigned int index, const Sforce &f , bool debug_force_vel) // con fuera entro en esta!!!
+{
+	//kinodynamic unicycle
+	if(debug_robot_propagation_){
+	//if(index<10){
+		std::cout << "AKP-propagation IN robot_propagation (propagate pose); v_max_="<<v_max_<<  std::endl;
+	}
+	//std::cout <<"(out function: robot_propagation); v_max_="<<v_max_<<  std::endl;
+
+	//}
+	Spose propagated_pose,ini_pose;
+	ini_pose = planning_trajectory_.at(index);
+	//std::cout << "ini_pose.theta="<<ini_pose.theta<<  std::endl;
+	//std::cout << "ini_pose.v="<<ini_pose.v<<  std::endl;
+
+	//convert 2D forces into robot forces with non-holonomic contraints
+	double av = cos(ini_pose.theta)*f.fx + sin(ini_pose.theta)*f.fy;//projection to robot pose (dot product)
+
+	double aw = -sin(ini_pose.theta)*f.fx + cos(ini_pose.theta)*f.fy;//cross product theta x f
+
+
+    planning_before_av_.push_back(av);
+    planning_before_aw_.push_back(aw);
+
+	//if(debug_robot_propagation_){
+	if(index<0){
+		std::cout << "f.fx="<<f.fx<<"cos(ini_pose.theta)"<<cos(ini_pose.theta)<<"; f.fy="<<f.fy<<"sin(ini_pose.theta)="<<sin(ini_pose.theta)<< std::endl;
+		std::cout <<"ini_pose.v="<<ini_pose.v<<"; av="<<av<<"; av*dt="<<av*dt<<  std::endl;
+		std::cout <<"ini_pose.w="<<ini_pose.w<<"; aw="<<aw<<"; aw*dt="<<aw*dt<<  std::endl;
+	}
+	//std::cout <<"(before damping) f.fx="<<f.fx<<"; f.fy="<<f.fy<<"; sin(ini_pose.theta)="<<sin(ini_pose.theta)<<"; cos(ini_pose.theta)="<<cos(ini_pose.theta)<<  std::endl;
+
+	//std::cout <<"(before damping) ini_pose.w="<<ini_pose.w<<"; aw="<<aw<<"; aw*dt="<<aw*dt<<  std::endl;
+
+	// calculate the proportional torque thau = k (0 - w) similar to the steering force
+	/*if(sqrt(ini_pose.v*ini_pose.v)<limit_linear_vel_for_dampening_parameter_){ // linear_velocity_gir_limit_=0.2 initial limit_linear_vel_for_dampening_parameter_=0.2
+		aw -= slow_vel_dampening_parameter_*ini_pose.w;//TODO this dampening parameter depends on the velocity v, w! careful if it changes significantly
+		//std::cout <<"case1 slow_vel_dampening_parameter_="<<slow_vel_dampening_parameter_<<"ini_pose.w="<<ini_pose.w<<"; aw="<<aw<<"; aw*dt="<<aw*dt<<  std::endl;
+	}
+	else if((sqrt(ini_pose.v*ini_pose.v)<limit_angular_vel_for_dampening_parameter_)&&(sqrt(ini_pose.v*ini_pose.v)>limit_linear_vel_for_dampening_parameter_)){ //angular_velocity_gir_limit_=0.5
+		aw -= normal_vel_dampening_parameter_*ini_pose.w;//TODO this dampening parameter depends on the velocity v, w! careful if it changes significantly
+		//std::cout <<"case2 normal_vel_dampening_parameter_="<<normal_vel_dampening_parameter_<<"ini_pose.w="<<ini_pose.w<<"; aw="<<aw<<"; aw*dt="<<aw*dt<<  std::endl;
+
+	}else if((sqrt(ini_pose.v*ini_pose.v)>limit_angular_vel_for_dampening_parameter_)){//||(sqrt(ini_pose.v*ini_pose.v)>limit_linear_vel_for_dampening_parameter_)
+		aw -= high_vel_dampening_parameter_*ini_pose.w;//TODO this dampening parameter depends on the velocity v, w! careful if it changes significantly
+		//std::cout <<"case3 high_vel_dampening_parameter_="<<high_vel_dampening_parameter_<<"ini_pose.w="<<ini_pose.w<<"; aw="<<aw<<"; aw*dt="<<aw*dt<<  std::endl;
+
+	}else{
+		aw -= normal_vel_dampening_parameter_*ini_pose.w;//TODO this dampening parameter depends on the velocity v, w! careful if it changes significantly
+		//std::cout <<"case4 normal_vel_dampening_parameter_="<<high_vel_dampening_parameter_<<"ini_pose.w="<<ini_pose.w<<"; aw="<<aw<<"; aw*dt="<<aw*dt<<  std::endl;
+
+	}*/
+	aw -= normal_vel_dampening_parameter_*ini_pose.w;
+	// girar mas rapido==> disminuir parametro a 1. Girar mas suave, lento, subir parametro por encima del original que es 1.6.
+	if(index<0){
+		std::cout <<"ini_pose.w="<<ini_pose.w<<"; aw="<<aw<<"; aw*dt="<<aw*dt<<  std::endl;
+	}
+	//std::cout <<"(after damping) ini_pose.w="<<ini_pose.w<<"; aw="<<aw<<"; aw*dt="<<aw*dt<<  std::endl;
+    planning_damping_aw_.push_back(normal_vel_dampening_parameter_*ini_pose.w);
+
+    //std::cout << "v_max_="<<v_max_<<"; w_max_="<<w_max_<<"; desired_velocity_="<<desired_velocity_<<  std::endl;
+
+	//if(debug_robot_propagation_){
+	if(index<0){
+		std::cout <<"(after) aw="<<aw<<  std::endl;
+		std::cout << "a_v_max_="<<a_v_max_<<"; a_w_max_="<<a_w_max_<<"; a_v_break_="<<a_v_break_<<  std::endl;
+		std::cout << "v_max_="<<v_max_<<"; w_max_="<<w_max_<<  std::endl;
+	}
+
+
+
+
+	double a_v_max_loc=a_v_max_;
+	double a_v_breack_loc=a_v_break_;
+
+	if((ini_pose.v<lim_to_consider_robotZero_)&&(ini_pose.v>(-lim_to_consider_robotZero_))){
+		a_v_max_loc=a_v_max_Vrobotzero_;
+		a_v_breack_loc=a_v_max_Vrobotzero_;
+	}else{
+		a_v_max_loc=a_v_max_;
+		a_v_breack_loc=a_v_break_;
+	}
+
+
+	bool caso_frenar=false;
+	//if(global_dist_to_obstacle_in_front>1.5m){ o 2m
+		if(av<0.0){ // intentamos frenar.
+			a_v_max_loc=a_v_max_negativa_;  // para frenado más suave.
+			a_v_breack_loc=a_v_max_negativa_;
+			//std::cout << "CASO FRENAAAAARRRRR; a_v_max_negativa_="<<a_v_max_negativa_<<  std::endl;
+			caso_frenar=true;
+		}
+	//}
+
+	// check for forces validity
+	if( av*ini_pose.v < 0.001 )//breaking configuration if different sign v and a
+	{
+		if( av > a_v_breack_loc) av=a_v_breack_loc;
+		if( av < -a_v_breack_loc) av=-a_v_breack_loc;
+		if(caso_frenar){
+			//std::cout << "CASO FRENAAAAARRRRR; a_v_break_="<<a_v_breack_loc<<  std::endl;
+		}
+	}
+	else // not breaking or 0.0
+	{
+		if( av > a_v_max_loc) av=a_v_max_loc;
+		if( av < -a_v_max_loc) av=-a_v_max_loc;
+		if(caso_frenar){
+			//std::cout << "CASO FRENAAAAARRRRR; a_v_max_="<<a_v_max_loc<<  std::endl;
+		}
+	}
+	if( aw > a_w_max_) aw=a_w_max_;
+	if( aw < -a_w_max_) aw=-a_w_max_;
+
+
+
+
+	// update velocities
+	propagated_pose.v = ini_pose.v + av*dt;
+
+	if(index<0){
+			std::cout <<"(after if) av="<<av<<"; av_max="<<a_v_max_<<"; propagated_pose.v="<<propagated_pose.v<<  std::endl;
+			std::cout <<"(after if) aw="<<aw<<"; a_w_max_="<<a_w_max_<<  std::endl;
+		}
+	propagated_pose.w = ini_pose.w + aw*dt;
+
+	if( propagated_pose.v > v_max_) propagated_pose.v=v_max_;
+	if( propagated_pose.v < -v_max_) propagated_pose.v=-v_max_;
+	if( propagated_pose.w > w_max_) propagated_pose.w=w_max_;
+	if( propagated_pose.w < -w_max_) propagated_pose.w=-w_max_;
+	propagated_pose.x = ini_pose.x + ini_pose.v*cos(ini_pose.theta)*dt
+			+ dt*dt/2.0*cos(ini_pose.theta)*av;
+	propagated_pose.y = ini_pose.y + ini_pose.v*sin(ini_pose.theta)*dt
+			+ dt*dt/2.0*sin(ini_pose.theta)*av;
+	propagated_pose.theta = ini_pose.theta + ini_pose.w*dt
+			+ dt*dt/2.0*aw;
+	propagated_pose.time_stamp = ini_pose.time_stamp + dt;
+
+	time_stamp_plan_.push_back(dt+time_stamp_plan_.at(index));
+
+	if(index<0){
+		double dist=sqrt((propagated_pose.x -ini_pose.x)*(propagated_pose.x -ini_pose.x)+(propagated_pose.y -ini_pose.y)*(propagated_pose.y -ini_pose.y));
+		std::cout <<"; dist="<<dist<< "; propagated_pose.x -ini_pose.x="<<propagated_pose.x -ini_pose.x<<"; propagated_pose.y -ini_pose.y="<<propagated_pose.y -ini_pose.y<<  std::endl;
+	}
+
+	if(index<0){
+		std::cout << "propagated_pose.v=(ini_pose.v + av*dt)="<<propagated_pose.v<<"propagated_pose.v=(ini_pose.w + aw*dt)="<<propagated_pose.w<<  std::endl;
+		std::cout <<" propagated_pose.x="<<propagated_pose.x<<"; propagated_pose.y="<<propagated_pose.y<<"; v_max_="<<v_max_<<" dt="<<dt<<  std::endl;
+	}
+
+	//update
+	//std::cout <<"time_stamp_plan_.at(index)="<<time_stamp_plan_.at(index)<<"; index="<<index<<"; dt="<<dt<<  std::endl;
+	//propagated_pose.print();
+
+   planning_v_.push_back(propagated_pose.v);
+   planning_w_.push_back(propagated_pose.w);
+   planning_x_.push_back(propagated_pose.x);
+   planning_y_.push_back(propagated_pose.y);
+   planning_theta_.push_back(propagated_pose.theta);
+   planning_after_av_.push_back(av);
+   planning_after_aw_.push_back(aw);
+
+
+
+
+	planning_trajectory_.push_back(propagated_pose);
+	planning_SpointV_trajectory_.push_back( SpointV_cov(propagated_pose.x,propagated_pose.y,
+				propagated_pose.time_stamp, propagated_pose.v*cos(propagated_pose.theta),
+				propagated_pose.v*sin(propagated_pose.theta)) );
+
+	if(debug_robot_propagation_){
+		std::cout <<"(out function: robot_propagation) planning_trajectory_.size="<<planning_trajectory_.size()<<  std::endl;
+		planning_SpointV_trajectory_.back().print();
+		std::cout <<" FINAL aw="<<aw<<"; final_propagated_pose="<<propagated_pose.w<<  std::endl;
+	}
+
+}
+
+Sbehavior* Crobot::find_behavior_estimation( unsigned int id )
+{
+	std::list<Sbehavior>::iterator iit = expected_behavior_list_.begin();
+	Sbehavior *behavior;
+	if ( !expected_behavior_list_.empty() )
+	{
+		for( ; iit != expected_behavior_list_.end(); iit++ )
+		{
+			if ( iit->related_person_id == id )
+			{
+				behavior = &(*iit);
+				assert( behavior != NULL );
+				return  behavior;
+			}
+			if ( iit->related_person_id > id )
+				break;
+		}
+	}
+	//behavior not found for person id, then a new behavior is set
+	expected_behavior_list_.insert( iit, Sbehavior( id ) );
+	iit--;//element inserted is before iit, so we want the pointer to the inserted element
+	behavior = &(*iit);
+	assert( behavior != NULL );
+	return  behavior;
+}
+
+Cperson_abstract::behavior_type
+Crobot::get_best_behavior_to_person( unsigned int interacting_person ) const
+{
+	Cperson_abstract::behavior_type result = Cperson_abstract::Balanced;
+	const Sbehavior * behavior = NULL;
+	std::list<Sbehavior>::const_iterator iit = expected_behavior_list_.begin();
+	if ( !expected_behavior_list_.empty() )
+	{
+		for( ; iit != expected_behavior_list_.end(); iit++ )
+		{
+			if ( iit->related_person_id == interacting_person )
+			{
+				behavior = &(*iit);
+				assert( behavior != NULL );
+			}
+			if ( iit->related_person_id > interacting_person )
+				break;//person not found, so result is a balanced behavior
+		}
+	}
+	if ( behavior == NULL ) return result;
+	assert( behavior != NULL );
+	double best_expectation =  behavior->expectation[0];
+	for ( unsigned int i = 1; i < behavior->expectation.size(); ++i  )
+	{
+
+		if( best_expectation < behavior->expectation[i]  )
+		{
+			best_expectation = behavior->expectation[i];
+			result = (Cperson_abstract::behavior_type)i;
+		}
+	}
+	//behavior->print();
+	return result;
+}
+
+
+void Crobot::correct_state_to_delay( Spose last_control_cmd, double now, double delay )
+{
+	current_pose_.w = last_control_cmd.w + (current_pose_.w - last_control_cmd.w ) * exp( - fabs(now - current_pose_.time_stamp + delay) / 0.05 );
+	current_pose_.v = last_control_cmd.v + (current_pose_.v - last_control_cmd.v ) * exp( - fabs(now - current_pose_.time_stamp + delay) / 0.05 );
+	//current_pointV_ not necessary for dynamic constraints, only for comparisions
+	//TODO propagate state {x,y,theta}, not implemented yet
+}
+
+Spose Crobot::robot_propagation_companion_position(double dt, const Sforce& f ,Spose in_ini_pose, bool coments){ // made by (ely) for robot companion.
+	//kinodynamic unicycle
+	if(debug_antes_subgoals_entre_AKP_goals_){
+		std::cout << " IN robot_propagation_companion_position(); dt="<<dt << std::endl;
+	}
+
+	//std::cout << "(in) dt="<<dt<< std::endl;
+
+	Spose propagated_pose,ini_pose;
+	ini_pose = in_ini_pose;
+
+	//	std::cout << " dt="<<dt << std::endl;
+	//	std::cout << "f.fx="<<f.fx<<"; f.fy="<<f.fy << std::endl;
+
+
+	double ini_pose_theta=ini_pose.theta;
+	if(ini_pose.theta<0){
+		ini_pose_theta=(2*3.14)+ini_pose.theta;
+	}
+
+	if(debug_antes_subgoals_entre_AKP_goals_){
+		std::cout << "ini_pose.print();!!!!!"<< std::endl;
+		ini_pose.print();
+	}
+
+	//convert 2D forces into robot forces with non-holonomic contraints
+	if(debug_antes_subgoals_entre_AKP_goals_){
+		std::cout << "ini_pose.theta"<<ini_pose.theta*(180/3.14) << std::endl;
+	}
+
+
+	double av = cos(ini_pose_theta)*f.fx + sin(ini_pose_theta)*f.fy;//projection to robot pose (dot product)
+	double aw = -sin(ini_pose_theta)*f.fx + cos(ini_pose_theta)*f.fy;//cross product theta x f
+
+	//std::cout << " (Initial) av ="<<av  << std::endl;
+	//std::cout << " (Initial) aw ="<<aw  << std::endl;
+
+	//	if(ini_pose.theta<0){
+		//	std::cout << " av ="<<cos((2*3.14)+ini_pose.theta)*f.fx + sin((2*3.14)+ini_pose.theta)*f.fy << "cos(ini_pose.theta)="<<cos((2*3.14)+ini_pose.theta)<< std::endl;
+		//	std::cout << " aw ="<<-sin((2*3.14)+ini_pose.theta)*f.fx + cos((2*3.14)+ini_pose.theta)*f.fy <<"sin(ini_pose.theta)="<< sin((2*3.14)+ini_pose.theta)<< std::endl;
+
+	//	}
+
+
+	if(debug_velocity_file2_){
+		//std::cout << " av ="<<cos(-ini_pose.theta)*f.fx + sin(-ini_pose.theta)*f.fy << "cos(ini_pose.theta)="<<cos(-ini_pose.theta)<< std::endl;
+		//std::cout << " aw ="<<-sin(-ini_pose.theta)*f.fx + cos(-ini_pose.theta)*f.fy <<"sin(ini_pose.theta)="<< sin(-ini_pose.theta)<< std::endl;
+		//std::cout << " aw = -sin(ini_pose.theta)*f.fx + cos(ini_pose.theta)*f.fy"<<aw << std::endl;
+		//std::cout << " av="<<av << std::endl;
+		std::ofstream fileMatlab2;
+		fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+		fileMatlab2 << " av ="<<cos(-ini_pose.theta)*f.fx + sin(-ini_pose.theta)*f.fy << "cos(ini_pose.theta)="<<cos(-ini_pose.theta)<< "\n";
+		fileMatlab2 << " aw ="<<-sin(-ini_pose.theta)*f.fx + cos(-ini_pose.theta)*f.fy <<"sin(ini_pose.theta)="<< sin(-ini_pose.theta)<< "\n";
+		fileMatlab2 << " aw = -sin(ini_pose.theta)*f.fx + cos(ini_pose.theta)*f.fy"<<aw <<  "\n";
+		fileMatlab2 << " av="<<av <<  "\n";
+		fileMatlab2.close();
+
+	}
+	// calculate the proportional torque thau = k (0 - w) similar to the steering force
+	 aw = aw -1.6*ini_pose.w;//TODO this dampening parameter depends on the velocity v, w! careful if it changes significantly
+	// TODO: ojo! cambie esto, pq entonces no usa la fuerza, para girar y no gira bien!
+	 //aw = aw -2.0*ini_pose.w;//TODO this dampening parameter depends on the velocity v, w! careful if it changes significantly
+
+		//std::cout << " (2 Initial) av ="<<av  << std::endl;
+		//std::cout << " (2 Initial) aw ="<<aw  << std::endl;
+
+
+	if(debug_velocity_file2_){
+		//std::cout << " aw -= 1.6*ini_pose.w"<<aw << std::endl;
+		std::ofstream fileMatlab2;
+		fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+		fileMatlab2 << " aw -= 1.6*ini_pose.w"<<aw << "\n";
+		fileMatlab2.close();
+	//	std::cout << " (2) aw ="<<aw  << std::endl;
+	}
+
+
+	double a_v_max_loc=a_v_max_;
+	double a_v_breack_loc=a_v_break_;
+	bool caso_frenar=false;
+	//if(global_dist_to_obstacle_in_front>1.5m){ o 2m
+		if(av<0.0){ // intentamos frenar.
+			a_v_max_loc=a_v_max_negativa_;  // para frenado más suave.
+			a_v_breack_loc=a_v_max_negativa_;
+			// debug_sideBySide2019_
+			//std::cout << "CASO FRENAAAAARRRRR; a_v_max_negativa_="<<a_v_max_negativa_<<  std::endl;
+			caso_frenar=true;
+		}
+	//}
+
+	// check for forces validity
+	if( av*ini_pose.v < 0.001 )//breaking configuration if different sign v and a
+	{
+		//std::cout << "if av*ini_pose.v < 0.001 ; av="<<av << std::endl;
+
+		if(debug_velocity_file2_){
+			//std::cout << " caso 1" << std::endl;
+			std::ofstream fileMatlab2;
+			fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+			fileMatlab2<< " caso 1" << "\n";
+			fileMatlab2.close();
+		}
+
+		if( av > a_v_breack_loc){
+			av=a_v_breack_loc;
+			//std::cout << " av > a_v_break_ ; av= a_v_break_="<<av << std::endl;
+
+		}
+		if( av < -a_v_breack_loc){
+
+			av=-a_v_breack_loc;
+			//std::cout << " av < -a_v_break_ ; av= -a_v_break_="<<av << std::endl;
+		}
+
+		if(caso_frenar){
+			//std::cout << "CASO FRENAAAAARRRRR; a_v_break_="<<a_v_breack_loc<<  std::endl;
+		}
+
+	}
+	else // not breaking or 0.0
+	{
+		//std::cout << " else av*ini_pose.v < 0.001 ; av="<<av << std::endl;
+		if(debug_velocity_file2_){
+			//std::cout << " caso 2" << std::endl;
+			std::ofstream fileMatlab2;
+			fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+			fileMatlab2<< " caso 2" << "\n";
+			fileMatlab2.close();
+		}
+
+		if( av > a_v_max_loc){
+			av=a_v_max_loc;
+			//std::cout << " av > a_v_max_ ; av=a_v_max_="<<av << std::endl;
+		}
+		if( av < -a_v_max_loc){
+			av=-a_v_max_loc;
+			//std::cout << " av <- a_v_max_ ; av=-a_v_max_="<<av << std::endl;
+		}
+
+		if(caso_frenar){
+			//std::cout << "CASO FRENAAAAARRRRR; a_v_max_="<<a_v_max_loc<<  std::endl;
+		}
+	}
+	if( aw > a_w_max_){
+
+
+
+		if(debug_velocity_file2_){
+			//std::cout << " caso 3" << std::endl;
+			std::ofstream fileMatlab2;
+			fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+			fileMatlab2<< " caso 3" << "\n";
+			fileMatlab2.close();
+		}
+
+		aw=a_w_max_;
+		//std::cout << " aw > a_w_max_ ; aw=a_w_max_="<<aw << std::endl;
+	}
+	if( aw < -a_w_max_){
+
+		if(debug_velocity_file2_){
+			//std::cout << " caso 4" << std::endl;
+			std::ofstream fileMatlab2;
+			fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+			fileMatlab2<< " caso 4" << "\n";
+			fileMatlab2.close();
+		}
+
+		aw=-a_w_max_;
+		//std::cout << " aw < -a_w_max_ ; aw=-a_w_max_="<<aw << std::endl;
+	}
+//debug_sideBySide2019_
+	//std::cout << " IMPORTANTE COMPROAVAR AV_MAX (after if's) av ="<<av <<"; a_w_max_="<<a_w_max_ << std::endl;
+	//std::cout << " (after if's) aw ="<<aw  << std::endl;
+
+	if(debug_velocity_file2_){
+
+		std::ofstream fileMatlab2;
+		fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+		fileMatlab2<<" (after if's) av ="<<av  << "\n";
+		fileMatlab2<<" (after if's) aw ="<<aw  << "\n";
+		fileMatlab2.close();
+	}
+	// update velocities
+	propagated_pose.v = ini_pose.v + av*dt;
+
+	if(debug_velocity_file2_){
+		//std::cout << " propagated_pose.v ="<<propagated_pose.v <<"; ini_pose.v="<<ini_pose.v<< "; av*dt="<<av*dt<<std::endl;
+		std::ofstream fileMatlab2;
+		fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+		fileMatlab2<<" propagated_pose.v ="<<propagated_pose.v <<"; ini_pose.v="<<ini_pose.v<< "; av*dt="<<av*dt<< "\n";
+		fileMatlab2.close();
+
+	}
+
+	propagated_pose.w = ini_pose.w + aw*dt;
+
+	if(debug_velocity_file2_){
+		std::ofstream fileMatlab2;
+		fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+		//std::cout << " propagated_pose.w ="<<propagated_pose.w <<"; ini_pose.w="<<ini_pose.w<< "; aw*dt="<<aw*dt<<std::endl;
+		fileMatlab2 << " propagated_pose.w ="<<propagated_pose.w <<"; ini_pose.w="<<ini_pose.w<< "; aw*dt="<<aw*dt<<"\n";
+		fileMatlab2.close();
+	}
+
+	if( propagated_pose.v > v_max_){
+		propagated_pose.v=v_max_;
+	}
+	if( propagated_pose.v < -v_max_){
+		propagated_pose.v=-v_max_;
+	}
+	if( propagated_pose.w > w_max_) {
+		propagated_pose.w=w_max_;
+	}
+	if( propagated_pose.w < -w_max_) {
+		propagated_pose.w=-w_max_;
+	}
+
+
+	propagated_pose.x = ini_pose.x + ini_pose.v*cos(ini_pose_theta)*dt
+						+ dt*dt/2.0*cos(ini_pose_theta)*av;
+	if(debug_velocity_file2_){
+		std::ofstream fileMatlab2;
+		fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+		//std::cout << " propagated_pose.x ="<<propagated_pose.x <<"; ini_pose.x="<<ini_pose.x<< "; ini_pose.v*cos(ini_pose.theta)*dt="<<ini_pose.v*cos(ini_pose.theta)*dt<< "; dt*dt/2.0*cos(ini_pose.theta)*av="<<dt*dt/2.0*cos(ini_pose.theta)*av<<std::endl;
+		fileMatlab2 << " propagated_pose.x ="<<propagated_pose.x <<"; ini_pose.x="<<ini_pose.x<< "; ini_pose.v*cos(ini_pose.theta)*dt="<<ini_pose.v*cos(ini_pose.theta)*dt<< "; dt*dt/2.0*cos(ini_pose.theta)*av="<<dt*dt/2.0*cos(ini_pose.theta)*av<<"\n";
+		fileMatlab2.close();
+	}
+
+
+	propagated_pose.y = ini_pose.y + ini_pose.v*sin(ini_pose_theta)*dt
+			+ dt*dt/2.0*sin(ini_pose_theta)*av;
+
+
+
+	if(debug_velocity_file2_){
+		std::ofstream fileMatlab2;
+		fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+		//std::cout << " propagated_pose.y ="<<propagated_pose.y<< "; ini_pose.y="<<ini_pose.y <<"; ini_pose.v*sin(ini_pose.theta)*dt="<<ini_pose.v*sin(ini_pose.theta)*dt<<"; dt*dt/2.0*sin(ini_pose.theta)*av="<<dt*dt/2.0*sin(ini_pose.theta)*av<< std::endl;
+		fileMatlab2 << " propagated_pose.y ="<<propagated_pose.y<< "; ini_pose.y="<<ini_pose.y <<"; ini_pose.v*sin(ini_pose.theta)*dt="<<ini_pose.v*sin(ini_pose.theta)*dt<<"; dt*dt/2.0*sin(ini_pose.theta)*av="<<dt*dt/2.0*sin(ini_pose.theta)*av<< "\n";
+		fileMatlab2.close();
+	}
+
+
+	propagated_pose.theta = ini_pose_theta + ini_pose.w*dt
+			+ dt*dt/2.0*aw;
+	if(debug_velocity_file2_){
+		std::ofstream fileMatlab2;
+		fileMatlab2.open (debug_file_.c_str(), std::ofstream::out | std::ofstream::app);
+		//std::cout << " propagated_pose.theta="<<propagated_pose.theta<< "; ini_pose.theta="<<ini_pose.theta <<"; ini_pose.w*dt="<<ini_pose.w*dt<<";  dt*dt/2.0*aw="<< dt*dt/2.0*aw<< std::endl;
+		fileMatlab2 << " propagated_pose.theta="<<propagated_pose.theta<< "; ini_pose.theta="<<ini_pose.theta <<"; ini_pose.w*dt="<<ini_pose.w*dt<<";  dt*dt/2.0*aw="<< dt*dt/2.0*aw<< "\n";
+		fileMatlab2.close();
+	}
+
+	//propagated_pose.time_stamp = ini_pose.time_stamp + dt;
+
+	/*std::cout << " propagated_pose.x ="<<propagated_pose.x  << std::endl;
+	std::cout << " propagated_pose.y ="<<propagated_pose.y  << std::endl;
+	std::cout << " propagated_pose.v ="<<propagated_pose.v  << std::endl;
+	std::cout << " propagated_pose.w ="<<propagated_pose.w  << std::endl;
+	std::cout << " propagated_pose.theta ="<<propagated_pose.theta  << std::endl;
+	std::cout << " propagated_pose.time_stamp="<<propagated_pose.time_stamp << std::endl;
+*/
+
+	if(debug_antes_subgoals_entre_AKP_goals_){
+		std::cout << " propagated_pose.x ="<<propagated_pose.x  << std::endl;
+		std::cout << " propagated_pose.y ="<<propagated_pose.y  << std::endl;
+		std::cout << " propagated_pose.v ="<<propagated_pose.v  << std::endl;
+		std::cout << " propagated_pose.w ="<<propagated_pose.w  << std::endl;
+		std::cout << " propagated_pose.theta ="<<propagated_pose.theta  << std::endl;
+		std::cout << " propagated_pose.time_stamp="<<propagated_pose.time_stamp << std::endl;
+		if(ini_pose.theta<0){
+
+			std::cout << " INCREMENTO GIRO ANGULO ROBOT ="<< (180/3.14)*(propagated_pose.theta - (360+ini_pose.theta))   << std::endl;
+		}else{
+			std::cout << " INCREMENTO GIRO ANGULO ROBOT ="<< (180/3.14)*(propagated_pose.theta - ini_pose.theta)   << std::endl;
+		}
+		std::cout << "  propagated_pose.theta ="<<  (180/3.14)*propagated_pose.theta<<";  ini_pose.theta" <<(180/3.14)*ini_pose.theta << std::endl;
+		std::cout << "  propagated_pose.w ="<<  propagated_pose.w<<";  ini_pose.w" <<ini_pose.w  << std::endl;
+		std::cout << " propagated_pose.print();!!!!!!!!!!!"<< std::endl;
+		propagated_pose.print();
+	}
+
+	if(test_w_max_<propagated_pose.w){
+		test_w_max_=propagated_pose.w;
+	}
+	if(test_aw_max_<aw){
+		test_aw_max_=aw;
+	}
+
+	if(debug_antes_subgoals_entre_AKP_goals_){
+	std::cout << " test_w_max_="<<test_w_max_<<"; test_aw_max_"<<test_aw_max_<< std::endl;
+	}
+
+	return propagated_pose;
+	//update
+	//planning_trajectory_.push_back(propagated_pose);
+	//planning_SpointV_trajectory_.push_back( SpointV_cov(propagated_pose.x,propagated_pose.y,propagated_pose.time_stamp, propagated_pose.v*cos(propagated_pose.theta),propagated_pose.v*sin(propagated_pose.theta)) );
+}
+
+
+void Crobot::prediction_propagation( double dt , unsigned int index, const Sforce &f , double max_person_vel )
+{
+	Spose propagated_pose,ini_pose;
+	SpointV actual_group_point2;
+
+	ini_pose = prediction_trajectory_.at(index);
+
+	actual_group_point2=prediction_trajectory_SpointV_.at(index);//SpointV(current_pose_.x,current_pose_.y,current_pose_.time_stamp,current_pose_.v*cos(current_pose_.theta),current_pose_.v*sin(current_pose_.theta));//robot_->get_current_pointV();
+
+
+	//std::cout << "3333 desired_velocity_="<<desired_velocity_ <<"; max_person_vel="<<max_person_vel<<  std::endl;
+
+	//SpointV propagated_robot=actual_group_point2.propagate(dt,f,desired_velocity_);
+
+	SpointV propagated_robot=actual_group_point2.propagate(dt,f,max_person_vel);
+
+	propagated_pose=Spose(propagated_robot.x,propagated_robot.y,propagated_robot.time_stamp,propagated_robot.orientation(),propagated_robot.v());
+
+	//kinodynamic unicycle
+	/*if(debug_robot_propagation_){
+		std::cout << "IN robot_propagation (propagate pose)" <<  std::endl;
+	}
+	Spose propagated_pose,ini_pose;
+	ini_pose = prediction_trajectory_.at(index);
+	//std::cout << "ini_pose.theta="<<ini_pose.theta<<  std::endl;
+	//std::cout << "ini_pose.v="<<ini_pose.v<<  std::endl;
+
+	//convert 2D forces into robot forces with non-holonomic contraints
+	double av = cos(ini_pose.theta)*f.fx + sin(ini_pose.theta)*f.fy;//projection to robot pose (dot product)
+	double aw = -sin(ini_pose.theta)*f.fx + cos(ini_pose.theta)*f.fy;//cross product theta x f
+
+	if(debug_robot_propagation_){
+		std::cout << "f.fx="<<f.fx<<"cos(ini_pose.theta)"<<cos(ini_pose.theta)<<"; f.fy="<<f.fy<<"sin(ini_pose.theta)="<<sin(ini_pose.theta)<< std::endl;
+		std::cout <<"ini_pose.v="<<ini_pose.v<<"; av="<<av<<"; av*dt="<<av*dt<<  std::endl;
+		std::cout <<"ini_pose.w="<<ini_pose.w<<"; aw="<<aw<<"; aw*dt="<<aw*dt<<  std::endl;
+	}
+
+	// calculate the proportional torque thau = k (0 - w) similar to the steering force
+	aw -= 1.6*ini_pose.w;//TODO this dampening parameter depends on the velocity v, w! careful if it changes significantly
+
+	if(debug_robot_propagation_){
+		std::cout <<"(after) aw="<<aw<<  std::endl;
+		std::cout << "a_v_max_="<<a_v_max_<<"; a_w_max_="<<a_w_max_<<"; a_v_break_="<<a_v_break_<<  std::endl;
+		std::cout << "v_max_="<<v_max_<<"; w_max_="<<w_max_<<  std::endl;
+	}
+	// check for forces validity
+	if( av*ini_pose.v < 0.001 )//breaking configuration if different sign v and a
+	{
+		if( av > a_v_break_) av=a_v_break_;
+		if( av < -a_v_break_) av=-a_v_break_;
+	}
+	else // not breaking or 0.0
+	{
+		if( av > a_v_max_) av=a_v_max_;
+		if( av < -a_v_max_) av=-a_v_max_;
+	}
+	if( aw > a_w_max_) aw=a_w_max_;
+	if( aw < -a_w_max_) aw=-a_w_max_;
+
+	if(debug_robot_propagation_){
+		std::cout <<"(after if) av="<<av<<  std::endl;
+		std::cout <<"(after if) aw="<<aw<<  std::endl;
+	}
+
+	// update velocities
+	propagated_pose.v = ini_pose.v + av*dt;
+	if(debug_robot_propagation_){
+		std::cout << "propagated_pose.v=(ini_pose.v + av*dt)="<<propagated_pose.v<<  std::endl;
+	}
+	propagated_pose.w = ini_pose.w + aw*dt;
+	if( propagated_pose.v > v_max_) propagated_pose.v=v_max_;
+	if( propagated_pose.v < -v_max_) propagated_pose.v=-v_max_;
+	if( propagated_pose.w > w_max_) propagated_pose.w=w_max_;
+	if( propagated_pose.w < -w_max_) propagated_pose.w=-w_max_;
+	propagated_pose.x = ini_pose.x + ini_pose.v*cos(ini_pose.theta)*dt
+			+ dt*dt/2.0*cos(ini_pose.theta)*av;
+	propagated_pose.y = ini_pose.y + ini_pose.v*sin(ini_pose.theta)*dt
+			+ dt*dt/2.0*sin(ini_pose.theta)*av;
+	propagated_pose.theta = ini_pose.theta + ini_pose.w*dt
+			+ dt*dt/2.0*aw;;
+	propagated_pose.time_stamp = ini_pose.time_stamp + dt;
+
+	//update
+	//std::cout <<"propagated_pose person companion"<<  std::endl;
+	//propagated_pose.print();
+*/
+
+	prediction_trajectory_.push_back(propagated_pose);
+	prediction_trajectory_SpointV_.push_back( SpointV_cov(propagated_pose.x,propagated_pose.y,
+				propagated_pose.time_stamp, propagated_pose.v*cos(propagated_pose.theta),
+				propagated_pose.v*sin(propagated_pose.theta)) );
+	if(debug_robot_propagation_){
+		std::cout << " ROBOT pose and Spoint prediction trajectory:  "<< std::endl;
+
+		prediction_trajectory_.back().print();
+		prediction_trajectory_SpointV_.back().print();
+	}
+
+
+
+
+
+	/*if(debug_prediction_propagation_){
+		std::cout << "IN Cperson_behavior::prediction_propagation prediction_trajectory_.emty()"<<prediction_trajectory_.empty()<< std::endl;
+		std::cout <<" dt="<<dt<<";desired_velocity_="<<desired_velocity_<<"; force.fx"<<force.fx<<"; force.fy="<<force.fy<<"; prediction_trajectory_.at(index).print():"<< std::endl;
+		prediction_trajectory_.at(index).print();
+	}
+
+	prediction_trajectory_.push_back( prediction_trajectory_.at(index).propagate(dt,force,desired_velocity_) );*/
+}
+
+
+
+void Crobot::prediction_propagation2_only_vel( double dt , unsigned int index, const Sforce &f , double distance_person_t_x,double distance_person_t_y,double vel_person_x,double vel_person_y , double max_person_vel)
+{
+	Spose propagated_pose,ini_pose;
+	SpointV actual_group_point2;
+	SpointV actual_group_point3;
+
+	ini_pose = prediction_trajectory_only_vel_.at(index);
+
+	actual_group_point2=prediction_trajectory_only_vel_SpointV_.at(index);//SpointV(current_pose_.x,current_pose_.y,current_pose_.time_stamp,current_pose_.v*cos(current_pose_.theta),current_pose_.v*sin(current_pose_.theta));//robot_->get_current_pointV();
+	actual_group_point3=prediction_trajectory_SpointV_.at(index);
+
+	//std::cout << "actual_group_point2.x="<<actual_group_point2.x<<"; actual_group_point2.y="<<actual_group_point2.y<<  std::endl;
+
+	//std::cout << "actual_group_point3.x="<<actual_group_point3.x<<"; actual_group_point3.y="<<actual_group_point3.y<<  std::endl;
+
+	//SpointV propagated_robot=actual_group_point2.propagate2_comp_only_vel(dt,f,desired_velocity_,distance_person_t_x,distance_person_t_y,vel_person_x,vel_person_y);
+	SpointV propagated_robot;
+	if(max_person_vel!=0.0){
+		propagated_robot=actual_group_point2.propagate2_comp_only_vel(dt,f,max_person_vel,distance_person_t_x,distance_person_t_y,vel_person_x,vel_person_y);
+	}else{
+		propagated_robot=actual_group_point2.propagate2_comp_only_vel(dt,f,desired_velocity_,distance_person_t_x,distance_person_t_y,vel_person_x,vel_person_y);
+
+	}
+
+	propagated_pose=Spose(propagated_robot.x,propagated_robot.y,propagated_robot.time_stamp,propagated_robot.orientation(),propagated_robot.v());
+
+	//kinodynamic unicycle
+	/*if(debug_robot_propagation_){
+		std::cout << "IN robot_propagation (propagate pose)" <<  std::endl;
+	}
+	Spose propagated_pose,ini_pose;
+	ini_pose = prediction_trajectory_.at(index);
+	//std::cout << "ini_pose.theta="<<ini_pose.theta<<  std::endl;
+	//std::cout << "ini_pose.v="<<ini_pose.v<<  std::endl;
+
+	//convert 2D forces into robot forces with non-holonomic contraints
+	double av = cos(ini_pose.theta)*f.fx + sin(ini_pose.theta)*f.fy;//projection to robot pose (dot product)
+	double aw = -sin(ini_pose.theta)*f.fx + cos(ini_pose.theta)*f.fy;//cross product theta x f
+
+	if(debug_robot_propagation_){
+		std::cout << "f.fx="<<f.fx<<"cos(ini_pose.theta)"<<cos(ini_pose.theta)<<"; f.fy="<<f.fy<<"sin(ini_pose.theta)="<<sin(ini_pose.theta)<< std::endl;
+		std::cout <<"ini_pose.v="<<ini_pose.v<<"; av="<<av<<"; av*dt="<<av*dt<<  std::endl;
+		std::cout <<"ini_pose.w="<<ini_pose.w<<"; aw="<<aw<<"; aw*dt="<<aw*dt<<  std::endl;
+	}
+
+	// calculate the proportional torque thau = k (0 - w) similar to the steering force
+	aw -= 1.6*ini_pose.w;//TODO this dampening parameter depends on the velocity v, w! careful if it changes significantly
+
+	if(debug_robot_propagation_){
+		std::cout <<"(after) aw="<<aw<<  std::endl;
+		std::cout << "a_v_max_="<<a_v_max_<<"; a_w_max_="<<a_w_max_<<"; a_v_break_="<<a_v_break_<<  std::endl;
+		std::cout << "v_max_="<<v_max_<<"; w_max_="<<w_max_<<  std::endl;
+	}
+	// check for forces validity
+	if( av*ini_pose.v < 0.001 )//breaking configuration if different sign v and a
+	{
+		if( av > a_v_break_) av=a_v_break_;
+		if( av < -a_v_break_) av=-a_v_break_;
+	}
+	else // not breaking or 0.0
+	{
+		if( av > a_v_max_) av=a_v_max_;
+		if( av < -a_v_max_) av=-a_v_max_;
+	}
+	if( aw > a_w_max_) aw=a_w_max_;
+	if( aw < -a_w_max_) aw=-a_w_max_;
+
+	if(debug_robot_propagation_){
+		std::cout <<"(after if) av="<<av<<  std::endl;
+		std::cout <<"(after if) aw="<<aw<<  std::endl;
+	}
+
+	// update velocities
+	propagated_pose.v = ini_pose.v + av*dt;
+	if(debug_robot_propagation_){
+		std::cout << "propagated_pose.v=(ini_pose.v + av*dt)="<<propagated_pose.v<<  std::endl;
+	}
+	propagated_pose.w = ini_pose.w + aw*dt;
+	if( propagated_pose.v > v_max_) propagated_pose.v=v_max_;
+	if( propagated_pose.v < -v_max_) propagated_pose.v=-v_max_;
+	if( propagated_pose.w > w_max_) propagated_pose.w=w_max_;
+	if( propagated_pose.w < -w_max_) propagated_pose.w=-w_max_;
+	propagated_pose.x = ini_pose.x + ini_pose.v*cos(ini_pose.theta)*dt
+			+ dt*dt/2.0*cos(ini_pose.theta)*av;
+	propagated_pose.y = ini_pose.y + ini_pose.v*sin(ini_pose.theta)*dt
+			+ dt*dt/2.0*sin(ini_pose.theta)*av;
+	propagated_pose.theta = ini_pose.theta + ini_pose.w*dt
+			+ dt*dt/2.0*aw;;
+	propagated_pose.time_stamp = ini_pose.time_stamp + dt;
+
+	//update
+	//std::cout <<"propagated_pose person companion"<<  std::endl;
+	//propagated_pose.print();
+*/
+
+	prediction_trajectory_only_vel_.push_back(propagated_pose);
+	prediction_trajectory_only_vel_SpointV_.push_back( SpointV_cov(propagated_pose.x,propagated_pose.y,
+				propagated_pose.time_stamp, propagated_pose.v*cos(propagated_pose.theta),
+				propagated_pose.v*sin(propagated_pose.theta)) );
+	if(debug_robot_propagation_){
+		std::cout << " ROBOT pose and Spoint prediction trajectory:  "<< std::endl;
+
+		prediction_trajectory_only_vel_.back().print();
+		prediction_trajectory_only_vel_SpointV_.back().print();
+	}
+
+
+}
+
+
+
+void Crobot::clear_prediction_trajectory()
+{
+	prediction_trajectory_.clear();
+	prediction_trajectory_.push_back( current_pose_ );
+
+	prediction_trajectory_SpointV_.clear();
+	prediction_trajectory_SpointV_.push_back( current_pointV_ );
+
+
+}
+
+
+void Crobot::clear_prediction_trajectory_onlyV()
+{
+	prediction_trajectory_only_vel_.clear();
+	prediction_trajectory_only_vel_.push_back( current_pose_ );
+
+	prediction_trajectory_only_vel_SpointV_.clear();
+	prediction_trajectory_only_vel_SpointV_.push_back( current_pointV_ );
+
+}
+
+
+
+
diff --git a/local_lib_people_prediction/src/scene_elements/robot.h b/local_lib_people_prediction/src/scene_elements/robot.h
new file mode 100644
index 0000000000000000000000000000000000000000..d3e989ed8b17e80b563039cc7efdb7221052ff32
--- /dev/null
+++ b/local_lib_people_prediction/src/scene_elements/robot.h
@@ -0,0 +1,285 @@
+/*
+ * robot.h
+ *
+ *  Created on: Jul 31, 2013. Last Modified on 2020
+ *      Author: Initial code of the AKP Gonzalo Ferrer.
+ *      Author: Addition of ASSAOP and ASSAGP for people accompaniment code of Ely Repiso.
+ */
+
+
+#ifndef ROBOT_H_
+#define ROBOT_H_
+
+#include "scene_elements/person_abstract.h"
+#include <fstream>      // std::ofstream
+
+/**
+ *
+ * \brief Robot class
+ *
+ * The robot class inherits from Cperson_prediction and thus it can use of the
+ * calculation for the interaction with other scene elements.
+ *
+ * Additionally, it is specifically designed to describe different robot propagations
+ * such as a differential platform, a balancing differential (segway RMP200) or others.
+ * It is intended to serve as a container for the planning algorithms described in this library.
+ *
+*/
+
+class Crobot : public Cperson_abstract
+{
+  public:
+	enum robot_type{Differential, Diff_balancing, Car};
+	Crobot(  unsigned int id=0, robot_type robot_type=Differential,
+			Cperson_abstract::force_type person_force_type=Cperson_abstract::Spherical,
+			double _time_window = 1.0);
+	virtual ~Crobot();
+	// Cperson methods for updating robot position
+	virtual void add_pointV( SpointV_cov point,
+			Cperson_abstract::filtering_method filter=Cperson_abstract::Linear_regression_filtering, bool robot_or_person=false, double in_new_value_windowing=6.0) {}//not used in this class
+	virtual void Cperson_bhmip_add_pointV( SpointV_cov point, Cperson_abstract::filtering_method filter=Cperson_abstract::Linear_regression_filtering, bool robot_or_person=false){}//not used in this class
+	virtual void refresh_person( double now ) {} //this method is not used in Robot
+	virtual void add_pose( Spose observation );
+	Spose get_current_pose() {
+		//std::cout << " IMPORTANT!!!! IN get robot pose!; current_pose_.x="<<current_pose_.x<<"; current_pose_.y="<<current_pose_.y<< std::endl;
+		return current_pose_; }
+	void set_current_pose(Spose in_current_pose) { current_pose_=in_current_pose; } // for change the initial pose in robot companion!
+	Spose get_diff_pose() { return diff_pose_;}
+	// prediction
+	virtual void prediction( double min_v_to_predict );
+	virtual void reset();
+	virtual void reset_before_destination_prob();
+
+	//methods for trajectory prediction, inherited form Cperson_abstract
+	virtual const std::vector<SpointV_cov>* get_prediction_trajectory_with_target_person() const { return &prediction_trajectory_SpointV_;}
+	virtual const std::vector<SpointV_cov>* get_propagated_pose_onlyV_trajectory_with_target_person() const { return &prediction_trajectory_only_vel_SpointV_;}
+
+	virtual const std::vector<SpointV_cov>* get_prediction_trajectory() const { return &planning_SpointV_trajectory_;}
+	virtual const std::vector<SpointV_cov>* get_planning_trajectory() const { return &planning_SpointV_trajectory_;}
+	std::vector<SpointV_cov> get_planning_trajectory_vector()  { return planning_SpointV_trajectory_;}
+	void set_planning_trajectory(std::vector<SpointV_cov> in_planning_trajectory) { planning_SpointV_trajectory_=in_planning_trajectory;}
+	virtual void clear_planning_trajectory();
+	virtual void reserve_planning_trajectory( unsigned int n) { planning_trajectory_.reserve(n);}
+
+	//specific robot planning methods and data structures functions
+	const std::vector<Spose>* get_robot_planning_trajectory() const {
+		//std::cout <<" planning_trajectory_.size()="<<planning_trajectory_.size()<< std::endl;
+		return &planning_trajectory_;}
+	void erase_last_planning_propagation();
+	//cinematic propagation
+	//void robot_propagation(double dt, unsigned int index = 0, double v=1.0, double w=1.0 ); // for tibi, we use the other robot_propagation
+	//dynamic propagation
+	void robot_propagation(double dt, unsigned int index = 0 , const Sforce& f = Sforce() , bool debug_force_vel=false);
+
+	Spose robot_propagation_companion_position(double dt, const Sforce& f = Sforce(),Spose in_ini_pose=Spose(), bool coments=false); // made by (ely) for robot companion.
+	void set_rnd_local_goal( Spoint goal ) { rnd_goal_ = goal; }
+
+	void correct_state_to_delay( Spose last_control_cmd, double now, double delay );
+
+	//paramters
+	void set_v_max( double v) { v_max_ = v; this->set_desired_velocty(v);}
+	double get_v_max () { return v_max_; }
+	void set_w_max( double w) { w_max_ = w;}
+	double get_w_max () { return w_max_; }
+	void set_a_v_max( double av) { a_v_max_ = av;}
+	double get_a_v_max () { return a_v_max_; }
+	void set_a_v_max_negativa( double av) { a_v_max_negativa_ = av;}
+	double get_a_v_max_negativa() { return a_v_max_negativa_; }
+
+	void set_a_v_max_Vrobotzero(double av){ a_v_max_Vrobotzero_=av;}
+	double get_a_v_max_Vrobotzero(){return a_v_max_Vrobotzero_;}
+	void set_lim_to_consider_robotZero(double limVrob){lim_to_consider_robotZero_=limVrob;}
+
+	void set_a_v_break( double av) { a_v_break_ = av;}
+	double get_a_v_break () { return a_v_break_; }
+	void set_a_w_max( double aw) { a_w_max_ = aw;}
+	double get_a_w_max () { return a_w_max_; }
+	double get_platform_radii() { return platform_radii_; }
+	double get_platform_radii_2() { return platform_radii_2_;}
+	void set_platform_radii( double r ) { platform_radii_ = r; platform_radii_2_ = r*r;}
+
+	void set_robot_debug_vel_file(bool in_debug_velocity_){
+		debug_velocity_file2_=in_debug_velocity_;
+	}
+	/*void set_robot_debug_vel_cout(bool in_debug_velocity_){
+		debug_velocity_cout2_=in_debug_velocity_;
+	}*/
+
+	void set_robot_debug_filename(std::string in_debug_file_){
+		debug_file_=in_debug_file_;
+	}
+	//behavior estimation methods
+	virtual Sbehavior * find_behavior_estimation( unsigned int id );
+	virtual Cperson_abstract::behavior_type get_best_behavior_to_person( unsigned int interacting_person ) const;
+
+	/* robot companion (ely) */
+	virtual const std::deque<SpointV_cov>* get_past_trajectory(){return &return_traj_;}; //not used in this class. (created by ely)
+
+	void prediction_propagation( double dt , unsigned int index, const Sforce &f  , double max_person_vel=0.0 );
+	void clear_prediction_trajectory();
+	void prediction_propagation2_only_vel( double dt , unsigned int index, const Sforce &f , double distance_person_t_x,double distance_person_t_y,double vel_person_x,double vel_person_y , double max_person_vel );
+	void clear_prediction_trajectory_onlyV();
+
+	void set_high_vel_dampening_parameter(double in_dampening_parameter){
+		high_vel_dampening_parameter_=in_dampening_parameter;
+	}
+	void set_normal_vel_dampening_parameter(double in_dampening_parameter){
+		normal_vel_dampening_parameter_=in_dampening_parameter;
+	}
+	void set_slow_vel_dampening_parameter(double in_dampening_parameter){
+		slow_vel_dampening_parameter_=in_dampening_parameter;
+	}
+
+	void set_limit_linear_vel_for_dampening_parameter(double in_limit_linear_vel){
+		limit_linear_vel_for_dampening_parameter_=in_limit_linear_vel;
+	}
+
+	void set_limit_angular_vel_for_dampening_parameter(double in_limit_angular_vel){
+		limit_angular_vel_for_dampening_parameter_=in_limit_angular_vel;
+	}
+
+	const  std::vector<double>* get_planning_v() const{
+		return &planning_v_;
+	}
+	const  std::vector<double>* get_planning_w() const{
+		return &planning_w_;
+	}
+	const  std::vector<double>* get_planning_x() const{
+		return &planning_x_;
+	}
+	const  std::vector<double>* get_planning_y() const{
+		return &planning_y_;
+	}
+	const std::vector<double>* get_planning_theta() const{
+		return &planning_theta_;
+	}
+	const std::vector<double>* get_planning_before_av() const{
+		return &planning_before_av_;
+	}
+
+	const std::vector<double>* get_planning_before_aw() const{
+		return &planning_before_aw_;
+	}
+	const std::vector<double>* get_planning_after_av() const{
+		return &planning_after_av_;
+	}
+	const std::vector<double>* get_planning_after_aw() const{
+		return &planning_after_aw_;
+	}
+	const std::vector<double>* get_planning_damping_aw() const{
+		return &planning_damping_aw_;
+	}
+
+	/*void robot_set_genome(Cgenome in_prob_clas_genome){
+		prob_clas_genome_=in_prob_clas_genome;
+	}*/
+
+	std::vector<double> get_time_stamp_plan(){
+		return time_stamp_plan_;
+	}
+
+
+    std::vector<double> get_force_act_x(){
+    	return force_act_x_;
+    }
+    std::vector<double> get_force_act_y(){
+    	return force_act_y_;
+    }
+
+    std::vector<double> get_rad_acc(){
+    	return  rad_acc_;
+    }
+
+    std::vector<double> get_nextv(){
+    	return nextv_;
+    }
+    std::vector<double> get_omega(){
+    	return omega_;
+    }
+    std::vector<double> get_selft_v_x(){
+    	return selft_v_x_;
+    }
+    std::vector<double> get_selft_v_y(){
+    	return selft_v_y_;
+    }
+
+    /*void set_augment_initial_v(bool in_augment_initial_v){
+    	augment_initial_v_=in_augment_initial_v;
+    }*/
+
+    void set_initial_v_robot_needed(double in_initial_v_robot_needed){
+    	initial_v_robot_needed_=in_initial_v_robot_needed;
+    }
+
+  private:
+	//robot parameters
+	Spose current_pose_, diff_pose_;
+	double v_max_, w_max_, a_v_max_, a_v_break_, a_w_max_, a_v_max_negativa_, a_v_max_Vrobotzero_, lim_to_consider_robotZero_; // a_v_max_negativa_ added by ely to stop slowly in companion.
+	double platform_radii_, platform_radii_2_;
+	std::vector<Spose> planning_trajectory_, prediction_trajectory_, prediction_trajectory_only_vel_;  //vector de Sposes de la trajectoria del robot.
+	std::vector<SpointV_cov> planning_SpointV_trajectory_, prediction_trajectory_SpointV_, prediction_trajectory_only_vel_SpointV_;
+
+
+
+	Spoint rnd_goal_;
+
+	//behavior containers
+	std::list<Sbehavior> expected_behavior_list_;
+
+	/* robot companion (ely) */
+	bool debug_antes_subgoals_entre_AKP_goals_;
+
+	double test_w_max_;
+	double test_aw_max_;
+
+	bool debug_velocity_file2_;
+	//bool debug_velocity_cout2_;
+
+	 std::string debug_file_;
+
+	bool debug_robot_propagation_;
+
+
+	// to obtain the best value when we increment the velocity, to allow the robot to does not do "S" behaviour.
+	double high_vel_dampening_parameter_; // original normal dampening_parameter set by Gonzalo Ferrer is dampening_parameter_=1.6; when robot max velocity was 0.9 m/s
+	double normal_vel_dampening_parameter_;
+	double slow_vel_dampening_parameter_;
+	double limit_linear_vel_for_dampening_parameter_;
+	double limit_angular_vel_for_dampening_parameter_;
+
+	// to limit the increment of angular acceleration depending on the linear velocity of the robot
+	double threshold_linear_velocity_to_limit_aw_;
+	double max_increment_aw_slow_vel_;
+	double max_increment_aw_normal_vel_;
+	double max_increment_aw_max_vel_;
+
+	// to see problems on propagation:
+    std::vector<double> planning_v_;
+    std::vector<double> planning_w_;
+    std::vector<double> planning_x_;
+    std::vector<double> planning_y_;
+    std::vector<double> planning_theta_;
+    std::vector<double> planning_before_av_;
+    std::vector<double> planning_before_aw_;
+    std::vector<double> planning_after_av_;
+    std::vector<double> planning_after_aw_;
+    std::vector<double> planning_damping_aw_;
+
+
+    std::vector<double> force_act_x_;
+    std::vector<double> force_act_y_;
+    std::vector<double> rad_acc_;
+    std::vector<double> nextv_;
+    std::vector<double> omega_;
+    std::vector<double> selft_v_x_;
+    std::vector<double> selft_v_y_;
+
+	std::vector<double> time_stamp_plan_;
+	bool augment_initial_v_;
+	double initial_v_robot_needed_;
+
+	 std::deque<SpointV_cov> return_traj_; // en robot esta no se usa, pero al heradar hay que crearla ficticia.
+
+};
+
+#endif /* PERSON_ROBOT_H_ */
diff --git a/local_lib_people_prediction/src/scene_sim.cpp b/local_lib_people_prediction/src/scene_sim.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..76b78fb4f2adffb23aca5844e21e1ee5ebd7bf10
--- /dev/null
+++ b/local_lib_people_prediction/src/scene_sim.cpp
@@ -0,0 +1,4894 @@
+/*
+ * scene_sim.cpp
+ *
+ *  Created on: Mar 27, 2013. Modified on 2016 (final review on 2020)
+ *      Author: Initial AKP code Gonzalo Ferrer, addition of code and maintenance of companion Ely Repiso (ASSAOP and ASSAGP)
+ */
+#include "scene_sim.h"
+#include "scene_elements/person_virtual.h"
+#include "random/rand_gmm.h"
+#include <iostream>
+
+Cscene_sim::Cscene_sim(  ): Cscene_abstract(Cperson_abstract::No_filtering, Cperson_abstract::Autoremove,
+		Cperson_abstract::Spherical),
+  number_virtual_people_(10) , virtual_person_id_counter_(0), remove_targets_(false),//,
+  id_person_companion_(1),
+  id_second_companion_(2),
+  robot_person_proximity_distance_sim_(1.0), //ANTES 2 antes 1.5
+  robot_person_proximity_tolerance_sim_(0.5), //ANTES 1 antes 0.5
+  debug_person_obst_forces_(false),
+  akp_for_person_companion_(10.0,500, Cplan_local_nav_person_companion::F_RRT_GC_alpha,true),
+ // akp_for_person_companion_2_group_person_(10.0,500, Cplan_local_nav_person_companion::F_RRT_GC_alpha,true),
+ // id_person_companion_2group_pers_(13),
+  debug_init_scene_sim_(false),
+  debug_general_scene_sim_(false),
+  debug_other_persons_scene_sim_(false),
+  debug_companion_person_scene_sim_(false),
+  debug_update_scene_for_person_companion_planning_akp_(false),
+  first_(true),
+  distance_to_stop_goal_person_(1.25),// antes 0.75 a veces se quedan atascados...
+  state_act_(Cplan_local_nav_person_companion::ITER),
+  actual_sim_case_(Cplan_local_nav_person_companion::case0),
+  goal1_(1),
+  caso_reinicio__grupos_personas_simulaciones_iguales_(true),
+  group_sincron_(false),
+  contatore_(0),
+  bool_select_rando_dest_person_companion_(false),
+  less_companion_velocity_(0.9),
+  same_velocity_for_all_the_iteration_(true),
+  random_initia_pose_people_(true),
+  random_final_dest_people_(true),
+  all_complete_random_final_dest_people_(false),
+  multiply_robot_force_to_persons_(1),
+  debug_approach_people_random_(false),
+  group_simulation_(false),
+  firsTimeINTracksOtherPersonCompanion_(true),
+  people_sim_stop_(false),
+  number_of_companion_people_(2),
+  Spose_person_companion2_(Spose()),
+  Spose_before_person_companion_(Spose()),
+  p2_back_p1_with_static_obst_(true)
+  //debug_real_test_companion_(false)
+ // caso_simulacion_giro_(false)
+  //only_companion_person_(false)
+{
+	//set random seed for the rng
+	//std::cout<<" CREATE CSCENE SIM"<<std::endl;
+	rand_seed();
+	scene_force_type_=Cperson_abstract::Spherical;
+	person_companion_ = new Crobot(1,Crobot::Differential);
+	akp_for_person_companion_.set_number_of_vertex(500); // todo: luego hacer que se pueda cambiar desde fuera el: horitzon_time y max_number_of_vertex
+	akp_for_person_companion_.set_horizon_time(10.0);
+	scene_time_window_=10.0;
+	akp_for_person_companion_.set_planning_mode(Cplan_local_nav_person_companion::F_RRT_GC_alpha);
+	//std::cout<<" CREATE CSCENE SIM 2"<<std::endl;
+
+	/*if(group_simulation_){
+		akp_for_person_companion_2_group_person_.set_number_of_vertex(500); // todo: luego hacer que se pueda cambiar desde fuera el: horitzon_time y max_number_of_vertex
+		akp_for_person_companion_2_group_person_.set_horizon_time(10.0);
+		akp_for_person_companion_2_group_person_.set_planning_mode(Cplan_local_nav_person_companion::F_RRT_GC_alpha);
+		akp_for_person_companion_2_group_person_.set_is_act_person_companion_bool(true);
+	}*/
+
+	//ini_person_goal3_=Spose(15.5,10.5,0.0,0.0,0.0,0.0);
+	//ini_person_goal4_=Spose(15.5,-15.5,0.0,0.0,0.0,0.0);
+	ini_person_goal4_=Spose(13.5,5.5,0.0,0.0,0.0,0.0);
+	ini_person_goal3_=Spose(13.5,-5.5,0.0,0.0,0.0,0.0);
+
+	akp_for_person_companion_.set_is_act_person_companion_bool(true);
+
+
+	actual_goal_to_return_person_comp1_id_1_=Spose(-4.0,0.5,0.0,0.0,0.0,0.0);
+	actual_goal_to_return_person_comp1_id_3_=Spose(-4.0,-2.25,0.0,0.0,0.0,0.0);
+
+
+	//actual_goal_to_return_person_comp2_id_1_=Spose(1.0,0.5,0.0,0.0,0.0,0.0);
+
+	actual_goal_to_return_person_comp2_id_1_=Spose(-7.0,0.5,0.0,0.0,0.0,0.0); //(julio2019) FINAL Initial POSE person1 (case central!) antes -1.25
+
+	actual_goal_to_return_person_comp2_id_3_=Spose(1.0,-2.25,0.0,0.0,0.0,0.0);
+
+	SIM_initial_person_goal_pose1_=Spoint(40.0, 0.5,0.0);
+	//SIM_initial_person_goal_pose1_=Spoint(27.0, 20.5,0.0); // SIM_initial_person_goal_pose1_=Spoint(30.0, 10.5,0.0);
+	//SIM_initial_person_goal_pose1_=Spoint(20.0, 17.5,0.0); // Spoint( double x_ , double y_ , double time_stamp_) :
+	// 0, 17.0, 10.5, 0.5, 1, 1   DESTS pose1 (INITIAL)
+	// 1, 1.0, -10.5, 0.5, 1, 0
+	//Sdestination_person_goal_pose1_=Sdestination(1,1.0,-20.5);
+	//Sdestination_person_goal_pose1_=Sdestination(1,20.0,-17.5);
+
+	SIM_initial_person_goal_pose2_=Spoint(40.0, 20.5, 0.0);
+
+	//SIM_initial_person_goal_pose2_=Spoint(27.0, -17.5, 0.0); // SIM_initial_person_goal_pose2_=Spoint(30.0, -10.5, 0.0);
+	//SIM_initial_person_goal_pose2_=Spoint(20.0, -17.5, 0.0);
+	// 0, 17.0, -10.5, 0.5, 1, 1   DESTS pose2 (INITIAL)
+	// 1, 1.0, +10.5, 0.5, 1, 0
+	//Sdestination_person_goal_pose2_=Sdestination(1,1.0,20.5);
+	//Sdestination_person_goal_pose2_=Sdestination(1,20.0, 17.5);
+
+	SIM_initial_person_goal_pose3_=Spoint(40,-20.0,0.0);
+
+	//SIM_initial_person_goal_pose3_=Spoint(41,0.0,0.0); // SIM_initial_person_goal_pose3_=Spoint(30,0.0,0.0);
+	//std::cout<<" out CREATE CSCENE SIM"<<std::endl;
+	//akp_for_person_companion_= new Cplan_local_nav_person_companion(5.0,500, Cplan_local_nav_person_companion::F_RRT_GC_alpha);
+}
+
+Cscene_sim::~Cscene_sim(  )
+{
+	for( std::list<Cperson_abstract*>::iterator iit = person_list_.begin() ; iit != person_list_.end() ; iit++ )
+	{
+		delete *iit;
+	}
+	delete robot_;
+}
+
+
+Cperson_abstract* Cscene_sim::add_person_container( unsigned int id,
+		std::list<Cperson_abstract*>::iterator iit)
+{
+	Cperson_virtual* person;
+	if(id==id_person_companion_){
+		person = new Cperson_virtual(id, Cperson_abstract::Person_companion, scene_force_type_,scene_time_window_);
+	}else{
+		person = new Cperson_virtual(id, Cperson_abstract::Person, scene_force_type_,scene_time_window_);
+	}
+	person_list_.insert( iit , person );
+	//person_list_.insert( iit , (Cperson_abstract*) person );
+	person->set_destinations(destinations_);
+	return (Cperson_abstract*) person;
+}
+
+/*Cperson_abstract* Cscene_sim::add_person_container( unsigned int id,
+		std::list<Cperson_abstract*>::iterator iit)
+{  // made by (ely) to change the person destinations.
+	Cperson_virtual* person = new Cperson_virtual(id, Cperson_abstract::Person, scene_force_type_);
+	person_list_.insert( iit , person );
+	//person_list_.insert( iit , (Cperson_abstract*) person );
+	person->set_destinations(destinations_);
+	return (Cperson_abstract*) person;
+}*/
+
+void Cscene_sim::update_scene(const std::vector<SdetectionObservation>& observation, bool& we_have_person_companion , bool person_or_robot)
+{
+	//the filter variable is not used in the simulated scene. All observations have been filtered.
+	std::cout << " update_scene for only random people simulated for assaop" << std::endl;
+
+	double now = observation[0].time_stamp;
+	std::list<Cperson_abstract*>::iterator iit;
+
+	//deleting persons if needed, (LIFO)
+	while( person_list_.size() > number_virtual_people_ )
+	{
+		person_list_.pop_front();
+	}
+
+	//updating persons trajectories
+	double w;
+	SpointV_cov virtual_next_pose;
+	Sforce virtual_force_goal, virtual_force_int_person, virtual_force_obstacle, virtual_force_robot;
+	for( iit = person_list_.begin() ; iit!=person_list_.end() ; iit++)
+	{
+		if ( ((*iit)->get_person_type() == Cperson_abstract::Person)|| ((*iit)->get_person_type() == Cperson_abstract::Person_companion))
+		{
+			//1 time step propagation
+			virtual_force_goal = (*iit)->force_goal( (*iit)->get_best_dest(), this->get_sfm_params(*iit)  );
+			virtual_force_int_person = force_persons_int( *iit );
+			virtual_force_obstacle = get_force_map( (*iit)->get_current_pointV().x, (*iit)->get_current_pointV().y ) * 0.5;
+			// calculate the robot-person force specifically, if any robot in the scene
+			if ( robot_in_the_scene_ )
+			{
+				std::cout << " SIMULATION robot in scene. 5 get_sfm_int_params" << std::endl;
+				virtual_force_robot =  (*iit)->force( robot_->get_current_pointV() ,
+					this->get_sfm_int_params(*iit, robot_)   ) * 4;//TODO change parameters
+
+			}
+			else{
+				virtual_force_robot = Sforce();
+			}
+			//to avoid entering obstacles present in the scene, it simply does not propagate
+			w = 1.0;
+			do
+			{
+				(*iit)->set_forces_person( virtual_force_goal*w, virtual_force_int_person*w, virtual_force_robot*w, virtual_force_obstacle );
+				virtual_next_pose = (*iit)->pointV_propagation( dt_ , (*iit)->get_force_person()  );
+				w *= 0.9;
+			} while( read_force_map_success_ && !is_cell_clear_map( virtual_next_pose.x, virtual_next_pose.y ) && w > 0.1 );
+			virtual_next_pose.time_stamp = now;
+			(*iit)->add_pointV( virtual_next_pose );
+
+			std::cout << " Forces: virtual_force_goal.fx="<<virtual_force_goal.fx<<"; virtual_force_goal.fy="<<virtual_force_goal.fy<< "; virtual_force_int_person.fx="<<virtual_force_int_person.fx<<"; virtual_force_int_person.fy="<<virtual_force_int_person.fy<<"; virtual_force_robot.fx="<<virtual_force_robot.fx<<"; virtual_force_robot.fy="<<virtual_force_robot.fy << std::endl;
+		}
+	}
+
+	//generating virtual persons, if necessary
+	Cperson_abstract* new_person;
+	Sdestination dest_1, dest_2;
+	while( person_list_.size() < number_virtual_people_ )
+	{
+		if(number_virtual_people_!=id_person_companion_){
+			// TODO check size of destinations > 0
+			new_person = add_person( ++virtual_person_id_counter_);
+			//set initial position at random. initially 2 destinations
+			dest_1 = get_destinations()->at( rand_uniform_discrete( 0, (int)get_destinations()->size()-1 ) );
+			dest_2 = get_destinations()->at( rand_uniform_discrete( 0, get_destinations()->size()-1) );
+			while( dest_1 == dest_2 && get_destinations()->size() > 1 )
+				dest_2 = get_destinations()->at( rand_uniform_discrete( 0, get_destinations()->size()-1) );
+			//initial pose checks for free cells if an obstacle map is provided
+			double x, y;
+			do{
+				double lambda_x = rand_uniform( 0, 1.0 ) , lambda_y = rand_uniform( 0, 1.0 );
+				x =lambda_x*dest_1.x + (1-lambda_x)*dest_2.x + rand_normal(0,1.5);
+				y = lambda_y*dest_1.y + (1-lambda_y)*dest_2.y + rand_normal(0,1.5);
+			} while( read_force_map_success_ && !is_cell_clear_map( x, y ) );
+			new_person->add_pointV( SpointV_cov(x , y, now),  filtering_method_ );
+			//set destination at random
+			//new_person->set_desired_velocty( rand_normal(1.0,0.1) );
+			new_person->set_desired_velocty( rand_normal(ramdom_people_vel_,0.1) );
+			new_person->set_best_dest( new_person->get_destinations()->at( rand_uniform_discrete( 0, new_person->get_destinations()->size()-1)  ) );
+			//new_person->print();
+		}
+	}
+
+	// remove person companion:
+	remove_person(id_person_companion_);
+
+	//deleting complete trajectories
+	if ( remove_targets_ )
+	{
+		std::vector<unsigned int> targets_to_remove;
+		for(iit = person_list_.begin() ; iit!=person_list_.end() ; iit++)
+		{
+			if ( (((*iit)->get_person_type() == Cperson_abstract::Person)||((*iit)->get_person_type() == Cperson_abstract::Person_companion)) &&
+					(*iit)->get_current_pointV().distance( (Spoint)(*iit)->get_best_dest() ) < 1.5 )//destination achieved
+			{
+				targets_to_remove.push_back((*iit)->get_id());
+				number_virtual_people_--;
+			}
+		}
+		for ( unsigned int i = 0; i < targets_to_remove.size() ; ++i)
+			remove_person(targets_to_remove[i]);
+	}
+	//assigning new destinations once the goal is achieved
+	else
+	{
+		std::vector<unsigned int> targets_to_update;
+		for(iit = person_list_.begin() ; iit!=person_list_.end() ; iit++)
+		{
+			if ( (((*iit)->get_person_type() == Cperson_abstract::Person)||((*iit)->get_person_type() == Cperson_abstract::Person_companion)) &&
+					(*iit)->get_current_pointV().distance( (Spoint)(*iit)->get_best_dest() ) < 1.5 )//destination achieved
+			{
+				//set destination at random
+				//(*iit)->set_desired_velocty( rand_normal(1.0,0.1) );
+				(*iit)->set_desired_velocty( rand_normal(ramdom_people_vel_,0.1) );
+				std::vector<int> neighbours = (*iit)->get_best_dest().neighbours_ids;
+				int new_dest = neighbours[ rand_uniform_discrete( 0, neighbours.size()-1 )];
+				(*iit)->set_best_dest( destinations_[ new_dest ] );
+			}
+		}
+	}
+}
+
+
+void Cscene_sim::insert_more_random_people(const std::vector<SdetectionObservation>& observation){
+
+	double now = observation[0].time_stamp;
+	std::list<Cperson_abstract*>::iterator iit;
+	//number_virtual_people_=40;  // para fuera personitas id=5 + cambiar txt.destinaciones
+	number_virtual_people_=20;
+
+	if(debug_approach_people_random_){
+	std::cout << "(insert_more_random_people) number_virtual_people_= "<< number_virtual_people_ << std::endl;
+	}
+	//deleting persons if needed, (LIFO)
+	/*while( person_list_.size() > number_virtual_people_ )
+	{
+		person_list_.pop_front();
+	}*/
+
+	/*std::cout << " (insert_more_random_people) person_list print "<< std::endl;
+	for( iit = person_list_.begin(); iit!=person_list_.end() ; iit++)
+		{
+		(*iit)->get_current_pointV().print();
+		}*/
+
+
+	//updating persons trajectories
+	double w;
+	SpointV_cov virtual_next_pose;
+	Sforce virtual_force_goal, virtual_force_int_person, virtual_force_obstacle, virtual_force_robot;
+	//std::cout << "(insert_more_random_people)  antes del for!!! " << std::endl;
+	for( iit = person_list_.begin(); iit!=person_list_.end() ; iit++)
+	{
+		//std::cout << "(insert_more_random_people)  antes del if!!! (*iit)->id= "<<(*iit)->get_id()<<" x="<<(*iit)->get_current_pointV().x <<" y="<<(*iit)->get_current_pointV().y <<" vx="<<(*iit)->get_current_pointV().vx <<" vy="<<(*iit)->get_current_pointV().vy << std::endl;
+		//(*iit)->get_best_dest().print();
+		if ( ((*iit)->get_person_type() == Cperson_abstract::Person) || ((*iit)->get_person_type() == Cperson_abstract::Person_companion)) //&&((*iit)->get_id()>5)
+		{
+			double act_desired_vel_person;
+			if(((*iit)->get_id()!=companion_person_id_)&&((*iit)->get_id()!=id_second_companion_)){
+				act_desired_vel_person=0.3+rand_normal(0.3,0.2);
+				if(act_desired_vel_person>1){
+					act_desired_vel_person=1;
+				}else if(act_desired_vel_person<0.3){
+					act_desired_vel_person=0.3;
+				}
+			}else{
+				act_desired_vel_person=less_companion_velocity_;
+			}
+
+
+			if((people_sim_stop_)&&(((*iit)->get_id()!=companion_person_id_)||((*iit)->get_id()!=id_second_companion_))){
+				id_second_companion_=3;
+				act_desired_vel_person=0.0;
+			}
+
+			//std::cout << " act_desired_vel_person= "<<act_desired_vel_person<<"; id="<< (*iit)->get_id()<< std::endl;
+			(*iit)->set_desired_velocty(act_desired_vel_person );
+
+			if(debug_approach_people_random_){
+			std::cout << " in propagate random people with forces; (*iit)->get_id()="<<(*iit)->get_id() << std::endl;
+			}
+
+			//1 time step propagation
+			virtual_force_goal = (*iit)->force_goal( (*iit)->get_best_dest(), this->get_sfm_params(*iit)  );
+			//virtual_force_int_person = force_persons_int( *iit );
+			virtual_force_int_person =force_persons_int3( *iit );
+			virtual_force_obstacle = get_force_map( (*iit)->get_current_pointV().x, (*iit)->get_current_pointV().y ) * 0.5;
+			// calculate the robot-person force specifically, if any robot in the scene
+			//set destination at random
+
+
+			//std::cout << " (insert_more_random_people) robot_in_the_scene_"<<robot_in_the_scene_<<"; robot_->get_current_pointV().x="<<robot_->get_current_pointV().x<<"; robot_->get_current_pointV().y="<<robot_->get_current_pointV().y << std::endl;
+			if ( robot_in_the_scene_ )
+			{
+
+				if(debug_approach_people_random_){
+					std::cout << " robot_->get_current_pointV().x="<<robot_->get_current_pointV().x<<"; robot_->get_current_pointV().y="<<robot_->get_current_pointV().y << std::endl;
+					std::cout << "  (*iit)->get_current_pointV().x="<< (*iit)->get_current_pointV().x<<";  (*iit)->get_current_pointV().y="<< (*iit)->get_current_pointV().y << std::endl;
+				}
+				const SpointV_cov* virtual_current_point_person_p;
+				virtual_current_point_person_p = &((*iit)->get_current_pointV());
+
+				virtual_force_robot =  (*iit)->force( robot_->get_current_pointV() ,
+					this->get_sfm_int_params(*iit, robot_)  ,virtual_current_point_person_p ) * multiply_robot_force_to_persons_;//TODO change parameters
+				virtual_force_robot =virtual_force_robot + (*iit)->force( person_companion_->get_current_pointV() ,
+						this->get_sfm_int_params(*iit, robot_) ,virtual_current_point_person_p   ) * multiply_robot_force_to_persons_;//TODO change parameters
+
+				if(debug_approach_people_random_){
+					Sforce total_f= virtual_force_robot+virtual_force_goal+virtual_force_int_person;
+											std::cout << "total_force:"<< std::endl;
+											total_f.print();
+				std::cout << " virtual_force_robot.fx="<<virtual_force_robot.fx<<"; virtual_force_robot.fy="<<virtual_force_robot.fy << std::endl;
+				}
+
+			}
+			else{
+
+				if(debug_approach_people_random_){
+				std::cout << " (else) virtual_force_robot.fx="<<virtual_force_robot.fx<<"; virtual_force_robot.fy="<<virtual_force_robot.fy << std::endl;
+				}
+				virtual_force_robot = Sforce();
+			}
+
+			//to avoid entering obstacles present in the scene, it simply does not propagate
+			w = 1.0;
+			do
+			{
+				(*iit)->set_forces_person( virtual_force_goal*w, virtual_force_int_person*w, virtual_force_robot*w, virtual_force_obstacle );
+				virtual_next_pose = (*iit)->pointV_propagation( dt_ , (*iit)->get_force_person()  );
+
+				if(debug_approach_people_random_){
+				std::cout << " (propagate) virtual_next_pose.x="<<virtual_next_pose.x<<"; virtual_next_pose.y="<<virtual_next_pose.y << std::endl;
+				}
+
+				w *= 0.9;
+			} while( read_force_map_success_ && !is_cell_clear_map( virtual_next_pose.x, virtual_next_pose.y ) && w > 0.1 );
+
+			if(virtual_next_pose.cov[0]>0.5){
+				virtual_next_pose.cov[0]=0.5; // queremos que no augmente la covarianza por encima de la inicial, sobretodo para visualizacion y porque en este caso no se usa al ser un simulador y no un tracker.
+			}
+			if(virtual_next_pose.cov[5]>0.5){
+				virtual_next_pose.cov[5]=0.5;
+			}
+			if(virtual_next_pose.cov[10]>0.5){
+				virtual_next_pose.cov[10]=0.5;
+			}
+			if(virtual_next_pose.cov[15]>0.5){
+				virtual_next_pose.cov[15]=0.5;
+			}
+			virtual_next_pose.time_stamp = now;
+			//(*iit)->add_pointV( virtual_next_pose );
+
+
+			/*if(group_simulation_){
+				if(other_companion_person_id_==(*iit)->get_id()){
+					(*iit)->add_pointV( other_companion_person_out_odometry_spoint_,  filtering_method_ );
+					// cambio la posicion de esta person de la lista internamente.
+					std::cout << " ??????? other_companion_person_out_odometry_spoint_.x="<<other_companion_person_out_odometry_spoint_.x<<"; .y="<<other_companion_person_out_odometry_spoint_.y << std::endl;
+				}else if(id_person_companion_==(*iit)->get_id()){
+					// si es la person companion no la insertes en la lista interna.
+				}
+				else{
+					(*iit)->add_pointV( virtual_next_pose,  filtering_method_ );
+				}
+			}else{*/
+				(*iit)->add_pointV( virtual_next_pose,  filtering_method_ );
+			//}
+
+
+
+			//std::cout << " get_force_int_robot_person():" << std::endl;
+			//(*iit)->get_force_int_robot_person().print();
+
+			//std::cout << " print all dest person:" << std::endl;
+								//double all_destpe=(*iit)->get_destinations()->size();
+								/*for(unsigned int g=0;g<all_destpe;g++){
+									(*iit)->get_destinations()->at(g).print();
+								}
+								std::cout << " print best dest person:" << std::endl;
+								(*iit)->get_best_dest().print();*/
+		}
+	}
+	//std::cout << " out inser people" << std::endl;
+	//generating virtual persons, if necessary
+	Cperson_abstract* new_person;
+	//Sdestination dest_1, dest_2;
+	std::vector<Sdestination> all_destinations_persons;
+	Sdestination  dest_1 = get_destinations()->at( 4 );
+	Sdestination  dest_2 = get_destinations()->at( 5 );
+	Sdestination  dest_3;
+	Sdestination  dest_4;
+	if(all_complete_random_final_dest_people_){
+		 dest_3 = get_destinations()->at( 2 );
+		 dest_4 = get_destinations()->at( 3 );
+	}
+	Sdestination  dest_5 = get_destinations()->at( 6 );
+	Sdestination  dest_6 = get_destinations()->at( 7 );
+	//std::cout << " (random people) 3print_dest:"<< std::endl;
+	//dest_1.print();
+	//dest_2.print();
+	/*if(all_complete_random_final_dest_people_){
+		dest_3.print();
+		dest_4.print();
+	}
+	dest_5.print();
+	dest_6.print();*/
+
+	all_destinations_persons.clear();
+	all_destinations_persons.push_back(dest_1);
+	all_destinations_persons.push_back(dest_2);
+
+	if(all_complete_random_final_dest_people_){
+		all_destinations_persons.push_back(dest_3);
+		all_destinations_persons.push_back(dest_4);
+	}
+
+	all_destinations_persons.push_back(dest_5);
+	all_destinations_persons.push_back(dest_6);
+
+
+	virtual_person_id_counter_= person_list_.size()+1; // ya hay 5 personas de antes. (hay 4 personas, más el robot!)
+	//std::cout << " 333333333 virtual_person_id_counter_="<<virtual_person_id_counter_ <<"; person_list_.size()="<<person_list_.size()<< std::endl;
+
+	while( person_list_.size() < (number_virtual_people_ -2) ) // el -1 es para tener en cuenta al robot. (Ahora son 2 personas.
+	{
+
+		/*
+		 *
+		 * 			dest_1 = get_destinations()->at( 2 );
+			dest_2 = get_destinations()->at( 3 );
+			destinations1_.clear();
+			destinations1_.push_back(dest_1);
+			destinations1_.push_back(dest_2);
+			if(random_final_dest_people_){
+				Sdestination dest_new=get_destinations()->at( 4 );
+				destinations1_.push_back(dest_new);
+				dest_new=get_destinations()->at( 5 );
+				destinations1_.push_back(dest_new);
+				dest_new=get_destinations()->at( 6 );
+				destinations1_.push_back(dest_new);
+				dest_new=get_destinations()->at( 7 );
+				destinations1_.push_back(dest_new);
+			}
+
+			new_person = add_person(3);
+			new_person->set_destinations(destinations1_);
+			//new_person->set_destinations(destinations1p2_);
+			x =dest_1.x;
+			y =dest_1.y;
+
+			new_person->set_desired_velocty( group_velocity1 );
+			//std::cout << "; group_velocity1" <<group_velocity1<< std::endl;
+
+			new_person->set_best_dest( new_person->get_destinations()->at( 1 ) ); //coges la destinacion 1 de esta persona, porque ahora esta en la destinacion 0
+			person_best_dest_[2]=new_person->get_destinations()->at( 1 );
+
+			person_act=SpointV_cov(x , y, now);
+
+			if(debug_init_scene_sim_){
+				std::cout << "person_act" << std::endl;
+				person_act.print();
+			}
+
+			theta= atan2(new_person->get_destinations()->at( 1 ).y-person_act.y,new_person->get_destinations()->at( 1 ).x-person_act.x);
+			vx=group_velocity1*cos(theta);
+			vy=group_velocity1*sin(theta);
+			new_person->add_pointV( SpointV_cov(x , y, now, vx, vy),  filtering_method_ );
+			new_points_person_sim_.push_back(SpointV_cov(x , y, now, vx, vy));
+			//new_person->add_pointV( SpointV_cov(x , y, now),  filtering_method_ );
+			//new_points_person_sim_.push_back(SpointV_cov(x , y, now));
+			persons_ids_.push_back(3);
+		 */
+
+		unsigned int actual_id=++virtual_person_id_counter_;
+		if(actual_id!=3.0){
+
+		std::cout << " YYYYYYYYYYYYYY insert random ini dest1 filtering_method_="<<filtering_method_ << std::endl;
+		// TODO check size of destinations > 0
+		new_person = add_person( actual_id);
+		//set initial position at random. initially 2 destinations
+		dest_1 =all_destinations_persons[ rand_uniform_discrete( 0, all_destinations_persons.size()-1 ) ];
+		dest_2 =all_destinations_persons[ rand_uniform_discrete( 0, all_destinations_persons.size()-1) ];
+		while( dest_1 == dest_2 && all_destinations_persons.size() > 1 )
+			dest_2 = get_destinations()->at( rand_uniform_discrete( 0, all_destinations_persons.size()-1) );
+		//initial pose checks for free cells if an obstacle map is provided
+		double x, y;
+		std::cout << " insert random ini dest2 " << std::endl;
+		do{
+			double lambda_x = rand_uniform( 0, 1.0 ) , lambda_y = rand_uniform( 0, 1.0 );
+			//x =lambda_x*dest_1.x + (1-lambda_x)*dest_2.x + rand_normal(0,1.5);
+			//y = lambda_y*dest_1.y + (1-lambda_y)*dest_2.y + rand_normal(0,1.5);
+			x =lambda_x*dest_1.x + (1-lambda_x)*dest_2.x + rand_normal(0,0.5);
+			y = lambda_y*dest_1.y + (1-lambda_y)*dest_2.y + rand_normal(0,0.5);
+
+		} while( read_force_map_success_ && !is_cell_clear_map( x, y ) );
+		//new_person->add_pointV( SpointV_cov(x , y, now),  filtering_method_ );
+		/*if(group_simulation_){
+			if(other_companion_person_id_==virtual_person_id_counter_){
+				new_person->add_pointV( other_companion_person_out_odometry_spoint_,  filtering_method_ );
+				// cambio la posicion de esta person de la lista internamente.
+				std::cout << " ??????? other_companion_person_out_odometry_spoint_.x="<<other_companion_person_out_odometry_spoint_.x<<"; .y="<<other_companion_person_out_odometry_spoint_.y << std::endl;
+			}else if(id_person_companion_==virtual_person_id_counter_){
+				// no insertes persona en la person list interna!!!
+			}else{
+				new_person->add_pointV( SpointV_cov(x , y, now),  filtering_method_ );
+			}
+		}else{*/
+			new_person->add_pointV( SpointV_cov(x , y, now),  filtering_method_ );
+		//}
+
+
+		//set destination at random
+		double act_desired_vel_person;
+		if((actual_id!=companion_person_id_)&&(actual_id!=id_second_companion_)){
+			act_desired_vel_person=0.3+rand_normal(0.3,0.2);
+					if(act_desired_vel_person>1){
+						act_desired_vel_person=1;
+					}else if(act_desired_vel_person<0.3){
+						act_desired_vel_person=0.3;
+					}
+		}else{
+			act_desired_vel_person=less_companion_velocity_;
+		}
+
+
+		if((people_sim_stop_)&&((actual_id!=companion_person_id_)||(actual_id!=id_second_companion_))){
+			id_second_companion_=3;
+			act_desired_vel_person=0.0;
+		}
+
+		std::cout << " act_desired_vel_person= "<<act_desired_vel_person<<"; id="<< new_person->get_id()<< std::endl;
+		new_person->set_desired_velocty(act_desired_vel_person );
+		new_person->set_destinations(all_destinations_persons);
+		Sdestination act_best_dest_newpers= new_person->get_destinations()->at( rand_uniform_discrete( 0, new_person->get_destinations()->size()-1)  );
+		new_person->set_best_dest(act_best_dest_newpers);
+		std::cout << " insert random ini dest3 virtual_person_id_counter_="<<virtual_person_id_counter_ << std::endl;
+		person_best_dest_[virtual_person_id_counter_-1]=act_best_dest_newpers;
+
+		SpointV_cov person_act=SpointV_cov(x , y, now);
+
+		if(debug_init_scene_sim_){
+			std::cout << "person_act" << std::endl;
+			person_act.print();
+		}
+		//std::cout << " insert random ini dest4 " << std::endl;
+		double theta= atan2(act_best_dest_newpers.y-person_act.y,act_best_dest_newpers.x-person_act.x);
+		double vx=act_desired_vel_person*cos(theta);
+		double vy=act_desired_vel_person*sin(theta);
+		//std::cout << " insert random ini dest5 " << std::endl;
+		new_points_person_sim_.push_back(SpointV_cov(x , y, now, vx, vy)); // TODO: puede que haga falta ponerla tambien en la select random dest de abajo.
+		//new_person->add_pointV( SpointV_cov(x , y, now),  filtering_method_ );
+		//new_points_person_sim_.push_back(SpointV_cov(x , y, now));
+		//std::cout << " insert random ini dest6 " << std::endl;
+		persons_ids_.push_back(virtual_person_id_counter_); // TODO: puede que haga falta ponerla tambien en la select random dest de abajo.
+		//new_person->print();
+
+		}
+
+	}
+	//std::cout << " out ini dest " << std::endl;
+	//deleting complete trajectories
+	if ( remove_targets_ )
+	{
+		//std::cout << " caso remove targets " << std::endl;
+		std::vector<unsigned int> targets_to_remove;
+		for(iit = person_list_.begin() ; iit!=person_list_.end() ; iit++)
+		{
+			if((*iit)->get_id()>5){
+				if ( (((*iit)->get_person_type() == Cperson_abstract::Person)||((*iit)->get_person_type() == Cperson_abstract::Person_companion)) &&
+						(*iit)->get_current_pointV().distance( (Spoint)(*iit)->get_best_dest() ) < 1.5 )//destination achieved
+				{
+					targets_to_remove.push_back((*iit)->get_id());
+					std::cout << " Danger!!! targets_to_remove id="<< (*iit)->get_id()<< std::endl;
+					number_virtual_people_--;
+				}
+			}
+		}
+		for ( unsigned int i = 0; i < targets_to_remove.size() ; ++i)
+			remove_person(targets_to_remove[i]);
+	}
+	//assigning new destinations once the goal is achieved
+	else
+	{
+		std::vector<unsigned int> targets_to_update;
+		for(iit = person_list_.begin() ; iit!=person_list_.end() ; iit++)
+		{
+			if((*iit)->get_id()>5){
+				if ( (((*iit)->get_person_type() == Cperson_abstract::Person)||((*iit)->get_person_type() == Cperson_abstract::Person_companion)) &&
+						(*iit)->get_current_pointV().distance( (Spoint)(*iit)->get_best_dest() ) < 1.5 )//destination achieved
+				{
+					double act_desired_vel_person;
+					//set destination at random
+					if(((*iit)->get_id()!=id_person_companion_)&&((*iit)->get_id()!=id_second_companion_)){
+						act_desired_vel_person=0.3+rand_normal(0.3,0.2);
+						if(act_desired_vel_person>1){
+							act_desired_vel_person=1;
+						}else if(act_desired_vel_person<0.3){
+							act_desired_vel_person=0.3;
+						}
+					}else{
+						act_desired_vel_person=less_companion_velocity_;
+					}
+
+					(*iit)->set_desired_velocty( act_desired_vel_person );
+					//std::vector<int> neighbours = (*iit)->get_best_dest().neighbours_ids;
+					//int new_dest = neighbours[ rand_uniform_discrete( 0, neighbours.size()-1 )];
+
+					Sdestination act_best_dest_newpers=(*iit)->get_destinations()->at( rand_uniform_discrete( 0, (*iit)->get_destinations()->size()-1)  );
+					(*iit)->set_best_dest( act_best_dest_newpers);
+					person_best_dest_[virtual_person_id_counter_-1]=act_best_dest_newpers;
+				}
+			}
+
+
+			/*for(unsigned int d=0; d<(*iit)->get_destinations()->size();d++){
+				std::cout << " person id="<< (*iit)->get_id()<< std::endl;
+				(*iit)->get_destinations()->at(d).print();
+			}*/
+		}
+	}
+//	std::cout << " out insert more people " << std::endl;
+}
+
+
+bool Cscene_sim::update_scene_companion_simulation_akp_person_companion(const std::vector<SdetectionObservation>& observation , bool ini_case, double ini_theta_person_companion, Spose& pose_command)
+{
+	 d_min_=1e10;
+	//less_companion_velocity_=0.5;
+	//std::cout << "???? (INI Cscene_sim) update_scene_companion_simulation_akp_person_companion=;   less_companion_velocity_="<<less_companion_velocity_<<"; same_velocity_for_all_the_iteration_="<<same_velocity_for_all_the_iteration_<< std::endl;
+	//std::cout <<"; companion_person_id_="<<companion_person_id_<< "akp_for_person_companion_.goal: "<< std::endl;
+	//akp_for_person_companion_.get_person_companion_goal().print();
+
+	//std::cout << " 9 in id_person_companion_="<<id_person_companion_<< std::endl;
+
+	//std::cout << "(Cscene_sim) akp_for_person_companion_.get_person_companion_akp()->get_best_dest().print(): " << std::endl;
+	//akp_for_person_companion_.get_person_companion_akp()->get_best_dest().print();
+
+
+
+	akp_for_person_companion_.set_id_person_companion(id_person_companion_);
+	akp_for_person_companion_.set_id_person_companion_Cprediction_bhmip(id_person_companion_);
+	akp_for_person_companion_.set_id_person_companion_people_simulator(id_person_companion_);
+
+	//akp_for_person_companion_.set_horizon_time(7.0);
+	//std::cout << "(Cscene_sim)1  update_scene_companion_simulation_akp_person_companion=; person_list_.size()="<< person_list_.size() << std::endl;
+	//std::cout << "IMPORTANTE (Cscene_sim) robot_->get_current_pose().x="<<robot_->get_current_pose().x<<"; robot_->get_current_pose().y="<<robot_->get_current_pose().y<< std::endl;
+	//std::cout << "IMPORTANTE (Cscene_sim) akp_for_person_companion_.get_person_companion_akp()->get_current_pose().x="<<akp_for_person_companion_.get_person_companion_akp()->get_current_pose().x<<"; akp_for_person_companion_.get_person_companion_akp()->get_current_pose().y="<<akp_for_person_companion_.get_person_companion_akp()->get_current_pose().y<< std::endl;
+
+	//double dist_rob_pers_comp=sqrt((robot_->get_current_pose().x-akp_for_person_companion_.get_person_companion_akp()->get_current_pose().x)*(robot_->get_current_pose().x-akp_for_person_companion_.get_person_companion_akp()->get_current_pose().x)+(robot_->get_current_pose().y-akp_for_person_companion_.get_person_companion_akp()->get_current_pose().y)*(robot_->get_current_pose().y-akp_for_person_companion_.get_person_companion_akp()->get_current_pose().y));
+	//std::cout << "IMPORTANTE (Cscene_sim) dist_rob_pers_comp="<<dist_rob_pers_comp<< std::endl;
+	akp_for_person_companion_.set_robot_pose_for_person_companion_simulation(robot_->get_current_pose());
+	if(group_simulation_){
+		//akp_for_person_companion_2_group_person_.set_robot_pose_for_person_companion_simulation(robot_->get_current_pose());
+	}
+
+	filtering_method_=Cperson_abstract::Low_pass_linear_regression_filtering;
+	//std::cout <<" (Cscene_sim) [action_mode{START=0,ITER,FACEPERSON,STOP}] state_act_="<<state_act_<< std::endl;
+
+	std::list<Cperson_abstract*>::iterator iit;
+
+	//number_virtual_people_=40;
+	number_virtual_people_=20;
+//	std::cout << "(Cscene_sim)2  update_scene_companion_simulation_akp_person_companion="<< std::endl;
+
+	// INI: update ides if are set the tracks of the other person companion.
+	if(group_simulation_){
+		//std::cout << "JJJJJJJJJJJJJJJJJJJJ  obs_other_person_companion_.empty="<<obs_other_person_companion_.empty()<<"; firsTimeINTracksOtherPersonCompanion_="<<firsTimeINTracksOtherPersonCompanion_<< std::endl;
+		if((!obs_other_person_companion_.empty())){//&&(firsTimeINTracksOtherPersonCompanion_)){
+			unsigned int actual_node_ids=number_virtual_people_+1;
+
+			for(unsigned int g=0; g<obs_other_person_companion_.size();g++){
+
+				if(obs_other_person_companion_[g].id>5){
+					peopleOtherPersonCompanionInGroup actual_person(obs_other_person_companion_[g].id,actual_node_ids);
+					people_ids_correspondence_OTHER_person_companion_.push_back(actual_person);
+					//std::cout<<"; obs_other_person_companion_[g].id="<<obs_other_person_companion_[g].id<<"; actual_node_ids="<<actual_node_ids<<std::endl;
+					actual_node_ids=actual_node_ids+1;
+				}
+
+
+			}
+
+			firsTimeINTracksOtherPersonCompanion_=false;
+
+		}else{
+			unsigned int actual_node_ids=number_virtual_people_+1;
+			for(unsigned int g=0; g<obs_other_person_companion_.size();g++){
+
+				//std::cout<<"; obs_other_person_companion_[g].id="<<obs_other_person_companion_[g].id<<"; actual_node_ids="<<actual_node_ids<<std::endl;
+				actual_node_ids=actual_node_ids+1;
+			}
+		}
+
+	}
+
+	// FIN: update ides if are set the tracks of the other person companion.
+
+	/*std::cout<<"INI"<<std::endl;
+	for(iit = person_list_.begin() ; iit!=person_list_.end() ; iit++)
+	{
+		std::cout<<"iit->id="<<(*iit)->get_id()<<std::endl;
+	}
+	std::cout<<"OUT"<<std::endl;
+	*/
+	/*Spoint person3_goal_init_point_=Spoint(20.0, 1.5,0);
+	Spoint person4_goal_init_point_=Spoint(20.0, -1.5,0);
+	Spoint person5_goal_init_point_=Spoint(20.0, -2.5,0);*/
+
+	//Spoint person3_goal_init_point_=Spoint(-20.0, -10.5,0);
+	//Spoint person4_goal_init_point_=Spoint(-20.0, -10.5,0);
+	//Spoint person5_goal_init_point_=Spoint(-20.0, -20.5,0);
+//
+	//Spoint person3_goal_init_point_=Spoint(15.0, 2.0,0); // Spoint person3_goal_init_point_=Spoint(20.0, 2.0,0);
+	//Spoint person4_goal_init_point_=Spoint(13.0, -1.5,0); // Spoint person4_goal_init_point_=Spoint(20.0, -3.0,0);
+	//Spoint person5_goal_init_point_=Spoint(17.0, -2.0,0); // Spoint person5_goal_init_point_=Spoint(20.0, -4.0,0);
+
+
+		//id_person_goal_=2;
+	//std::cout << "(Cscene_sim)3  update_scene_companion_simulation_akp_person_companion=; goal1_="<<goal1_<< std::endl;
+	////////////////
+	//std::cout << " ATR (Cscene_sim)3  Cplan_local_nav_person_companion::action_mode{START=0,ITER,FACEPERSON,STOP};; state_act_="<<state_act_<< std::endl;
+	if(state_act_==Cplan_local_nav_person_companion::START){
+		akp_for_person_companion_.set_state_Action(Cplan_local_nav_person_companion::START);
+
+		//std::cout << " ATR (Cscene_sim)3  Cplan_local_nav_person_companion::START; "<< std::endl;
+		//std::cout << " ATR (Cscene_sim)3  Cplan_local_nav_person_companion::START; goal1="<<goal1_<< std::endl;
+		//actual_goal_to_return_person_comp_=actual_goal_to_return_person_comp2_id_1_;
+		actual_goal_to_return_person_comp_=actual_goal_to_return_person_comp1_;
+		/*if(goal1_==1){
+			std::cout << "RRR (Cscene_sim)3  Cplan_local_nav_person_companion::START; goal1 "<< std::endl;
+			//if(random_initia_pose_people_){
+			//	double x3=13.0 + rand_normal(0.0,5.0);
+			//	double y3=0.0 + rand_normal(0.0,5.0);
+			//	double x4=13.0 + rand_normal(0.0,5.0);
+			//	double y4=0.0 + rand_normal(0.0,5.0);
+			//	double x5=13.0 + rand_normal(0.0,5.0);
+			//	double y5=0.0 + rand_normal(0.0,5.0);
+			//	person3_goal_init_point_=Spoint(x3,y3,0.0);//Spoint(-20.0, -20.0,0);
+			//	person4_goal_init_point_=Spoint(x4,y4,0.0);//Spoint(-20.0, -20.0,0);
+			//	person5_goal_init_point_=Spoint(x5,y5,0.0);//Spoint(-20.0, -20.0,0);
+
+			//	std::cout<<" person3_goal_init_point_.x="<<person3_goal_init_point_.x<<"; person3_goal_init_point_.y="<<person3_goal_init_point_.y<<std::endl;
+			//	std::cout<<" person4_goal_init_point_.x="<<person4_goal_init_point_.x<<"; person4_goal_init_point_.y="<<person4_goal_init_point_.y<<std::endl;
+			//	std::cout<<" person5_goal_init_point_.x="<<person5_goal_init_point_.x<<"; person5_goal_init_point_.y="<<person5_goal_init_point_.y<<std::endl;
+			//}
+			std::cout << "before RRR (Cscene_sim)3  go_rear_ini_="<<go_rear_ini_<< std::endl;
+
+			if(go_rear_ini_){
+				std::cout << " in RRR (Cscene_sim)3  go_rear_ini_="<<go_rear_ini_<< std::endl;
+				Spoint actual_goal;
+
+				// goal hacia atras de la person companion.
+				Spose person_companion_act_spose;
+				person_companion_act_spose=akp_for_person_companion_.get_person_companion_akp()->get_current_pose();
+				//std::cout<<"person_companion_act_spose.theta="<<person_companion_act_spose.theta<<"; person_companion_act_spose.theta(grad)+180="<<person_companion_act_spose.theta*180/3.14 + 180<<std::endl;
+				double act_theta_contrary=person_companion_act_spose.theta + 180; // Theta to go rear of the person_companion position.
+
+				actual_goal=Spoint(person_companion_act_spose.x + 10*cos(act_theta_contrary),person_companion_act_spose.y + 10*sin(act_theta_contrary),person_companion_act_spose.time_stamp);
+				actual_goal_to_return_person_comp_=actual_goal;
+				go_rear_ini_=false;
+			}
+
+			std::cout << "RRR (Cscene_sim)3  actual_goal_to_return_person_comp_.x="<<actual_goal_to_return_person_comp_.x<<"; actual_goal_to_return_person_comp_.y="<<actual_goal_to_return_person_comp_.y<<"; person_companion_act_spose.x="<<akp_for_person_companion_.get_person_companion_akp()->get_current_pose().x<<"; person_companion_act_spose.y="<<akp_for_person_companion_.get_person_companion_akp()->get_current_pose().y<<"; act_theta_contrary="<<akp_for_person_companion_.get_person_companion_akp()->get_current_pose().theta + 180<<"; person_companion_act_spose.theta="<<akp_for_person_companion_.get_person_companion_akp()->get_current_pose().theta<< std::endl;
+
+
+			if(akp_for_person_companion_.get_companion_person_position().distance(actual_goal_to_return_person_comp_)<2.0){
+				goal1_=4;
+				//goal1_=2;
+				//go_rear_ini_=true;
+			}
+		}else if(goal1_==4){
+			std::cout << "(Cscene_sim)3  Cplan_local_nav_person_companion::START; goal4 "<< std::endl;
+				//if(actual_sim_case_==Cplan_local_nav_person_companion::case1){
+					//actual_goal_to_return_person_comp_=actual_goal_to_return_person_comp1_;
+
+					if(id_person_companion_==3){
+						actual_goal_to_return_person_comp_=ini_person_goal3_;
+					}else if(id_person_companion_==1){
+						actual_goal_to_return_person_comp_=ini_person_goal4_;
+					}
+
+							//SpointV_cov act_comp_pers_pose=akp_for_person_companion_.get_companion_person_position();
+							//actual_goal_to_return_person_comp_=Spoint(act_comp_pers_pose.x-1,act_comp_pers_pose.y-1,act_comp_pers_pose.time_stamp);
+							//std::cout<<"OUT pose_command.v="<<pose_command.v<<"; pose_command.w="<<pose_command.w<<std::endl;
+				//}else{
+					//actual_goal_to_return_person_comp_=actual_goal_to_return_person_comp1_;
+							//SpointV_cov act_comp_pers_pose=akp_for_person_companion_.get_companion_person_position();
+						//	actual_goal_to_return_person_comp_=ini_person_goal4_;
+							//actual_goal_to_return_person_comp_=Spoint(act_comp_pers_pose.x-1,act_comp_pers_pose.y-1,act_comp_pers_pose.time_stamp);
+				//}
+			if((akp_for_person_companion_.get_companion_person_position().distance(ini_person_goal3_)<1)||((akp_for_person_companion_.get_companion_person_position().distance(ini_person_goal4_)<1))){
+							goal1_=2;
+			}
+		}
+		else if(goal1_==2){
+			std::cout << "(Cscene_sim)3  Cplan_local_nav_person_companion::START; goal2 "<< std::endl;
+
+			if(id_person_companion_==3){
+				actual_goal_to_return_person_comp_=actual_goal_to_return_person_comp1_id_3_;
+			}else if(id_person_companion_==1){
+				actual_goal_to_return_person_comp_=actual_goal_to_return_person_comp1_id_1_;
+			}
+
+
+			//actual_goal_to_return_person_comp_=actual_goal_to_return_person_comp1_; de la version anterior.
+			if(akp_for_person_companion_.get_companion_person_position().distance(actual_goal_to_return_person_comp_)<1){
+				goal1_=3;
+			}
+		}else if(goal1_==3){
+			//std::cout << "(Cscene_sim)3  Cplan_local_nav_person_companion::START; goal3 "<< std::endl;
+			//actual_goal_to_return_person_comp_=actual_goal_to_return_person_comp2_;
+
+			if(id_person_companion_==3){
+				actual_goal_to_return_person_comp_=actual_goal_to_return_person_comp2_id_3_;
+				if(!Scene_create_fake_fixed_second_person_companion_){
+					if(akp_for_person_companion_.get_companion_person_position().distance(actual_goal_to_return_person_comp_)<1){ // opcion segura... aunque pueden acabar yendo asincronos.
+						state_act_=Cplan_local_nav_person_companion::ITER;
+					}
+				}
+
+
+			}else if(id_person_companion_==1){
+				actual_goal_to_return_person_comp_=actual_goal_to_return_person_comp2_id_1_;
+				if(!Scene_create_fake_fixed_second_person_companion_){
+					if(akp_for_person_companion_.get_companion_person_position().distance(actual_goal_to_return_person_comp_)<1){ // opcion segura... aunque pueden acabar yendo asincronos.
+						state_act_=Cplan_local_nav_person_companion::ITER;
+					}
+				}
+
+			}
+
+			// TODO: reiniciar a mano. (si robot, persona1 y persona 2 estan en sus ultimos goals. Reinicia.
+
+
+
+
+		}*/
+
+
+
+
+	}else if(state_act_==Cplan_local_nav_person_companion::ITER){
+		//std::cout << "(Cscene_sim)3  Cplan_local_nav_person_companion::ITER; goal1_= "<<goal1_<< std::endl;
+		goal1_=1;
+		go_rear_ini_=true;
+
+
+		//Crobot* act_pointer_person_comp=akp_for_person_companion_.get_return_person_companion();
+		//Spoint person_dest1=Spoint(act_pointer_person_comp->get_best_dest().x,act_pointer_person_comp->get_best_dest().y,act_pointer_person_comp->get_best_dest().time_stamp);
+		Spoint person_companion_act_spoint=akp_for_person_companion_.get_person_companion_akp()->get_current_pointV();
+		//if(!Scene_create_fake_fixed_second_person_companion_){
+			//if(person_dest1.distance(person_companion_act_spoint)<1.5){
+			if((SIM_initial_person_goal_pose1_.distance(person_companion_act_spoint)<1.5)||(SIM_initial_person_goal_pose2_.distance(person_companion_act_spoint)<1.5)||(SIM_initial_person_goal_pose3_.distance(person_companion_act_spoint)<1.5)){
+					state_act_=Cplan_local_nav_person_companion::START;
+			}
+			//if(){
+
+			//}
+		//}
+
+
+		akp_for_person_companion_.set_state_Action(Cplan_local_nav_person_companion::ITER);
+
+	}
+	////////////////
+
+
+
+	///std::cout << "(Cscene_sim)4  update_scene_companion_simulation_akp_person_companion="<< std::endl;
+
+	//caso_simulacion_giro_=true;
+	new_points_person_sim_.clear();
+	persons_ids_.clear();
+	//if(debug_person_obst_forces_){
+	if(debug_general_scene_sim_){
+		std::cout<<std::endl<<std::endl<<std::endl;
+		std::cout<<" ini_theta_person_companion="<<ini_theta_person_companion<<std::endl;
+		std::cout << "IN update_scene_companion_simulation_akp_person_companion;  observation[0].time_stamp=" << observation[0].time_stamp<< "; dt_="<<dt_ << std::endl;
+	}
+	//}
+	//the filter variable is not used in the simulated scene. All observations have been filtered.
+	double now = observation[0].time_stamp;
+
+
+	//deleting persons if needed, (LIFO)
+	/*while( person_list_.size() > number_virtual_people_ )
+	{
+		person_list_.pop_front();
+	}*/
+	//const std::vector<Sdestination>* destinations_test_;
+	//destinations_test_=get_destinations();
+	//std::cout << "destinations_test_.size()"<<destinations_test_->size()<< std::endl;
+	//generating virtual persons, if necessary (SOLO se generan al inicio!)
+	// hay que generar dos grupos, uno de 2 y uno de 3.
+	//std::cout << "antes ini_case " << std::endl;
+	double less_companion_velocity;//=0.3;//rand_normal(0.5,0.1);//0.4; // ANTES=0.4
+	double group_velocity1;//=0.3;//rand_normal(0.5,0.1);//rand_normal(1.0,0.1);
+	double group_velocity2;//=0.3
+	//double target_person_velocity;
+	double group_velocity1_1;
+	//double group_velocity2_2;
+
+	//if(experiments_<4){
+
+	// CON OBTACULOS EN MEDIO
+	// target_person_velocity=0.5 pasan por el lado derecho del obstaculo mirando la pantalla (lado izquierdo para el grupo)
+	// target_person_velocity=0.8 se ven obligados a pasar por el centro para interceptarla
+	// target_person_velocity=1.7 o mas pasan por el lado izquierdo del obstaculo mirando la pantalla (lado derecho para el grupo)
+//	std::cout << "(Cscene_sim)5  update_scene_companion_simulation_akp_person_companion="<< std::endl;
+
+
+
+
+
+	// SET VELOCITIES!!!
+	if(state_act_==Cplan_local_nav_person_companion::ITER){
+
+		if(!same_velocity_for_all_the_iteration_){
+			if(group_sincron_){
+				//less_companion_velocity_=rand_normal(0.4,0.1);//0.4; // ANTES=0.4
+				less_companion_velocity=less_companion_velocity_;
+				group_velocity1_=rand_normal(1.0,0.1);//rand_normal(0.5,0.1);//rand_normal(1.0,0.1);
+				//target_person_velocity=0.8;
+				group_velocity1_1=0.8;
+				group_velocity1_1_=0.8;
+				//target_person_velocity_=0.8;//rand_normal(1.0,0.1); //0.8;
+
+				group_velocity2_=group_velocity1_; // para que cambie la velocidad de los dos grupos de personas, pero vayan iguales!
+			}else{
+				//less_companion_velocity_=rand_normal(0.4,0.1);
+				less_companion_velocity=less_companion_velocity_;
+				//less_companion_velocity=less_companion_velocity_;//rand_normal(0.5,0.1);//0.4; // ANTES=0.4
+				//group_velocity1=0.8;//rand_normal(0.5,0.1);//rand_normal(1.0,0.1); es la velocidad de la goal person
+				group_velocity1_=1.0;//rand_normal(1.0,0.1);//rand_normal(1.0,0.1); // person ID=3
+
+				group_velocity1_1_=0.8;
+				//target_person_velocity_=0.8;//rand_normal(1.0,0.1);  //0.8;
+
+				//group_velocity2=0.8;//group_velocity1; // para que cambie la velocidad de los dos grupos de personas, pero vayan iguales!
+
+
+				group_velocity2_=0.8;//group_velocity1;//group_velocity1; rand_normal(0.5,0.1); //person ID=5 para que cambie la velocidad de los dos grupos de personas, pero vayan iguales!
+				group_velocity2_2_=0.7;//group_velocity1;//group_velocity1; rand_normal(0.7,0.1) //person ID=4
+
+			}
+
+		}else{
+			//less_companion_velocity_=rand_normal(0.7,0.1);
+			less_companion_velocity=less_companion_velocity_;
+			//less_companion_velocity=less_companion_velocity_;
+			//target_person_velocity=target_person_velocity_;
+			group_velocity1_1=group_velocity1_1_;
+
+			group_velocity1=group_velocity1_;
+			group_velocity2=group_velocity2_;
+			//group_velocity2_2=group_velocity2_2_;
+
+			/*std::cout << "(Cplan_local_nav_person_companion::ITER)"<< std::endl;
+			std::cout << "less_companion_velocity_="<<less_companion_velocity_<< std::endl;
+			std::cout << "group_velocity1_1_="<<group_velocity1_1_<< std::endl;
+			std::cout << "group_velocity1_="<<group_velocity1_<< std::endl;
+			std::cout << "group_velocity2_="<<group_velocity2_<< std::endl;
+			std::cout << "group_velocity2_2_="<<group_velocity2_2_<< std::endl;*/
+		}
+
+
+
+		if(people_sim_stop_){
+
+			group_velocity1=0.0;//=0.3;//rand_normal(0.5,0.1);//rand_normal(1.0,0.1);
+		    group_velocity2=0.0;//=0.3
+
+			group_velocity1_1=0.0;
+			//group_velocity2_2=0.0;
+			group_velocity1_=0.0;//rand_normal(0.5,0.1);//rand_normal(1.0,0.1);
+			group_velocity1_1=0.0;
+			group_velocity1_1_=0.0;
+			group_velocity2_=group_velocity1_;
+			group_velocity2_2_=0.0;
+
+		}
+
+	}else{
+
+		if(!same_velocity_for_all_the_iteration_){
+			//less_companion_velocity=1.0;
+			//less_companion_velocity_=rand_normal(0.4,0.1);
+			less_companion_velocity=less_companion_velocity_;
+			//target_person_velocity=1.0;
+			group_velocity1_1=1.0;
+			group_velocity1=1.0;
+			group_velocity2=1.0;
+			//group_velocity2_2=1.0;
+		}else{
+			//less_companion_velocity_=rand_normal(0.4,0.1);
+			less_companion_velocity=less_companion_velocity_;
+			//less_companion_velocity=1.0;
+			//target_person_velocity=1.0;
+			group_velocity1_1=1.0;
+			group_velocity1=1.0;
+			group_velocity2=1.0;
+			//group_velocity2_2=1.0;
+			//less_companion_velocity_=0.5;
+			double act_desired_vel_person=1.0;//0.3+rand_normal(0.3,0.2);
+			/*			if(act_desired_vel_person>1){
+							act_desired_vel_person=1;
+						}else if(act_desired_vel_person<0.3){
+							act_desired_vel_person=0.3;
+						}
+			//target_person_velocity_=act_desired_vel_person;
+			//target_person_velocity_=0.6;
+			group_velocity1_1_=0.6;
+			act_desired_vel_person=0.3+rand_normal(0.3,0.2);
+			if(act_desired_vel_person>1){
+				act_desired_vel_person=1;
+			}else if(act_desired_vel_person<0.3){
+				act_desired_vel_person=0.3;
+			}
+			group_velocity1_=act_desired_vel_person;
+			act_desired_vel_person=0.3+rand_normal(0.3,0.2);
+						if(act_desired_vel_person>1){
+							act_desired_vel_person=1;
+						}else if(act_desired_vel_person<0.3){
+							act_desired_vel_person=0.3;
+						}
+			group_velocity2_=0.6;//_=act_desired_vel_person;
+			act_desired_vel_person=0.3+rand_normal(0.3,0.2);
+						if(act_desired_vel_person>1){
+							act_desired_vel_person=1;
+						}else if(act_desired_vel_person<0.3){
+							act_desired_vel_person=0.3;
+						}*/
+			group_velocity2_2_=act_desired_vel_person;
+
+		}
+
+
+	}
+
+
+	// STOP PERSON COMPANION AND TARGET PERSON WHEN BOTH ARE NEAR IN APPROACHING CASE
+	//std::cout << "(Cscene_sim)6 update_scene_companion_simulation_akp_person_companion="<< std::endl;
+
+
+
+	/*std::cout <<  "4 (Cscene_sim) check people nan's " << std::endl;
+			for( auto iit : person_list_)
+			{
+				if(iit->get_id()!=id_person_companion_){
+
+				 std::cout << " (after Insert people) (1) iit->get_prediction_trajectory( )->size()="<<iit->get_prediction_trajectory( )->size()<<"; iit->get_id()=" <<iit->get_id()<<"; iit->desired_vel="<<iit->get_desired_velocity()<<  std::endl;
+				 iit->get_current_pointV().print();
+				}
+			}*/
+
+		//less_companion_velocity=0.3;//rand_normal(0.5,0.1);//0.4; // ANTES=0.4
+		//group_velocity1=0.5;//rand_normal(0.5,0.1);//rand_normal(1.0,0.1);
+		//group_velocity2=0.3;//rand_normal(0.5,0.1);
+	//}else{
+	//	less_companion_velocity=rand_normal(0.5,0.1);//0.4; // ANTES=0.4
+	//	group_velocity1=rand_normal(0.5,0.1);//rand_normal(1.0,0.1);
+	//	group_velocity2=rand_normal(0.5,0.1);
+	//}
+
+	// std::cout << "(PERSON SIMULATION Cscene) robot_->get_current_pointV().x="<<robot_->get_current_pointV().x<<"; robot_->get_current_pointV().y="<<robot_->get_current_pointV().y << std::endl;
+
+
+
+	/*std::cout <<  "5 (Cscene_sim) check people nan's " << std::endl;
+			for( auto iit : person_list_)
+			{
+				if(iit->get_id()!=id_person_companion_){
+
+				 std::cout << " (after Insert people) (1) iit->get_prediction_trajectory( )->size()="<<iit->get_prediction_trajectory( )->size()<<"; iit->get_id()=" <<iit->get_id()<<"; iit->desired_vel="<<iit->get_desired_velocity()<<  std::endl;
+				 iit->get_current_pointV().print();
+				}
+			}*/
+
+	//std::cout << "(Cscene_sim)7  update_scene_companion_simulation_akp_person_companion="<< std::endl;
+
+	if(debug_general_scene_sim_){
+		std::cout << "333777788888999999(sim); less_companion_velocity="<<less_companion_velocity<<"; group_velocity1="<<group_velocity1<<"; group_velocity2="<<group_velocity2<< std::endl;
+	}
+
+	//std::cout<<"1"<<std::endl;
+	// FIND destination of the person companion in the case that the goal is a person which is moving.
+
+	/*for( auto iit: person_list_ ){
+		if(iit->get_id()==id_person_goal_){
+			akp_for_person_companion_.set_actual_person_Target_pointer(iit);
+
+			akp_for_person_companion_.set_actual_person_Target_SpointV(iit->get_current_pointV());
+			iit->get_current_pointV().print();
+			akp_for_person_companion_.set_actual_person_Target_destination(iit->get_best_dest());
+			iit->get_best_dest().print();
+		}
+
+	}*/
+
+	//std::cout << " 4="<< std::endl;
+	destinations_ini_security_copy.clear();
+	Sdestination d1_act=get_destinations()->at( 1 );
+	destinations_ini_security_copy.push_back(d1_act);
+	Sdestination d2_act=get_destinations()->at( 2 );
+	destinations_ini_security_copy.push_back(d2_act);
+				Sdestination d3_act=get_destinations()->at( 3 );
+				destinations_ini_security_copy.push_back(d3_act);
+				Sdestination d4_act=get_destinations()->at( 4 );
+				destinations_ini_security_copy.push_back(d4_act);
+				Sdestination d5_act=get_destinations()->at( 5 );
+				destinations_ini_security_copy.push_back(d5_act);
+				Sdestination d6_act=get_destinations()->at( 6 );
+				destinations_ini_security_copy.push_back(d6_act);
+				Sdestination d7_act=get_destinations()->at( 7 );
+				destinations_ini_security_copy.push_back(d7_act);
+				Sdestination d8_act=get_destinations()->at( 8 );
+				destinations_ini_security_copy.push_back(d8_act);
+				Sdestination d9_act=get_destinations()->at( 9 );
+				destinations_ini_security_copy.push_back(d9_act);
+				Sdestination d10_act=get_destinations()->at( 10 );
+				destinations_ini_security_copy.push_back(d10_act);
+				Sdestination d11_act=get_destinations()->at( 11 );
+				destinations_ini_security_copy.push_back(d11_act);
+				Sdestination d12_act=get_destinations()->at( 12 );
+				destinations_ini_security_copy.push_back(d12_act);
+				Sdestination d13_act=get_destinations()->at( 13 );
+				destinations_ini_security_copy.push_back(d13_act);
+
+	//std::cout<<"destinations_ini_security_copy[8].x="<<destinations_ini_security_copy[8].x<<"; destinations_ini_security_copy[8].y="<<destinations_ini_security_copy[8].y<<std::endl;
+				//std::cout << " 5= ini_case"<<ini_case<< std::endl;
+	//std::cout<<"destinations_ini_security_copy[10].x="<<destinations_ini_security_copy[10].x<<"; destinations_ini_security_copy[10].y="<<destinations_ini_security_copy[10].y<<std::endl;
+	//std::cout<<"destinations_ini_security_copy12].x="<<destinations_ini_security_copy[12].x<<"; destinations_ini_security_copy[12].y="<<destinations_ini_security_copy[12].y<<std::endl;
+//std::cout << "(Cscene_sim)8 update_scene_companion_simulation_akp_person_companion="<< std::endl;
+	if(ini_case){
+
+		/*std::cout <<  "6 (Cscene_sim) (In ini case) check people nan's " << std::endl;
+				for( auto iit : person_list_)
+				{
+					if(iit->get_id()!=id_person_companion_){
+
+					 std::cout << " (after Insert people) (1) iit->get_prediction_trajectory( )->size()="<<iit->get_prediction_trajectory( )->size()<<"; iit->get_id()=" <<iit->get_id()<<"; iit->desired_vel="<<iit->get_desired_velocity()<<  std::endl;
+					 iit->get_current_pointV().print();
+					}
+				}*/
+	//	std::cout << " in ini_case!"<< std::endl;
+
+		/*if(random_initia_pose_people_){
+			double x3=7.0 + rand_normal(0.0,5.0);
+			double y3=0.0 + rand_normal(0.0,5.0);
+			double x4=7.0 + rand_normal(0.0,5.0);
+			double y4=0.0 + rand_normal(0.0,5.0);
+			double x5=7.0 + rand_normal(0.0,5.0);
+			double y5=0.0 + rand_normal(0.0,5.0);
+			person3_goal_init_point_=Spoint(x3,y3,0.0);//Spoint(-20.0, -20.0,0);
+			person4_goal_init_point_=Spoint(x4,y4,0.0);//Spoint(-20.0, -20.0,0);
+			person5_goal_init_point_=Spoint(x5,y5,0.0);//Spoint(-20.0, -20.0,0);
+
+			//std::cout<<" person3_goal_init_point_.x="<<person3_goal_init_point_.x<<"; person3_goal_init_point_.y="<<person3_goal_init_point_.y<<std::endl;
+			//std::cout<<" person4_goal_init_point_.x="<<person4_goal_init_point_.x<<"; person4_goal_init_point_.y="<<person4_goal_init_point_.y<<std::endl;
+			//std::cout<<" person5_goal_init_point_.x="<<person5_goal_init_point_.x<<"; person5_goal_init_point_.y="<<person5_goal_init_point_.y<<std::endl;
+		}*/
+
+		if(same_velocity_for_all_the_iteration_){
+			//less_companion_velocity_=0.5;
+			double act_desired_vel_person=0.3+rand_normal(0.3,0.2);
+			if(act_desired_vel_person>1){
+				act_desired_vel_person=1;
+			}else if(act_desired_vel_person<0.3){
+				act_desired_vel_person=0.3;
+			}
+			//target_person_velocity_=act_desired_vel_person;
+			//target_person_velocity_=0.6;
+			group_velocity1_1_=0.8;
+			act_desired_vel_person=0.3+rand_normal(0.3,0.2);
+			if(act_desired_vel_person>1){
+				act_desired_vel_person=1;
+			}else if(act_desired_vel_person<0.3){
+				act_desired_vel_person=0.3;
+			}
+			//group_velocity1_=act_desired_vel_person;
+			act_desired_vel_person=0.3+rand_normal(0.3,0.2);
+			if(act_desired_vel_person>1){
+				act_desired_vel_person=1;
+			}else if(act_desired_vel_person<0.3){
+				act_desired_vel_person=0.3;
+			}
+			group_velocity2_=0.8;//=act_desired_vel_person;
+			act_desired_vel_person=0.3+rand_normal(0.3,0.2);
+			if(act_desired_vel_person>1){
+				act_desired_vel_person=1;
+			}else if(act_desired_vel_person<0.3){
+				act_desired_vel_person=0.3;
+			}
+			//group_velocity2_2_=act_desired_vel_person;
+			less_companion_velocity=less_companion_velocity_;
+			group_velocity1_1=group_velocity1_1_;
+
+			group_velocity1=group_velocity1_;
+			group_velocity2=group_velocity2_;
+			//group_velocity2_2=group_velocity2_2_;
+		}
+
+
+
+		if(people_sim_stop_){
+
+			group_velocity1=0.0;//=0.3;//rand_normal(0.5,0.1);//rand_normal(1.0,0.1);
+		    group_velocity2=0.0;//=0.3
+
+			group_velocity1_1=0.0;
+			//group_velocity2_2=0.0;
+			group_velocity1_=0.0;//rand_normal(0.5,0.1);//rand_normal(1.0,0.1);
+			group_velocity1_1=0.0;
+			group_velocity1_1_=0.0;
+			group_velocity2_=group_velocity1_;
+			group_velocity2_2_=0.0;
+		}
+
+		person_best_dest_.clear();
+		person_best_dest_.reserve(number_virtual_people_); // 6 persons.
+		for(unsigned int p=1;p<(number_virtual_people_+1);p++){
+			person_best_dest_.push_back(Sdestination());
+		}
+
+		//std::cout << " 2 in ini_case! person_best_dest_.size="<<person_best_dest_.size()<< std::endl;
+		Cperson_abstract* new_person;
+
+		//std::cout << "1" << std::endl;
+		Sdestination dest_1, dest_2, dest_10, dest_11, dest_12, dest_13,dest_14 ;
+
+		//set initial position at random. initially 2 destinations
+		// INI create person id=2
+			//std::cout << "12" << std::endl;
+
+/////////// Aquí iba antes person 2 ////////////////
+
+
+			// dest_1 = get_destinations()->at( 0 );
+			// x =dest_1.x ;
+			//y =dest_1.y ;
+			//	theta= atan2(new_person->get_destinations()->at( 1 ).y-person_act.y,new_person->get_destinations()->at( 1 ).x-person_act.x);
+			//vx=target_person_velocity*cos(theta);
+			//vy=target_person_velocity*sin(theta);
+
+			/*akp_for_person_companion_.set_actual_person_Target_SpointV(SpointV_cov(x , y, now, vx, vy));
+			akp_for_person_companion_.set_actual_person_Target_destination(new_person->get_destinations()->at( 1 ));
+			for( auto iit: person_list_ ){
+				if(iit->get_id()==id_person_goal_){
+					akp_for_person_companion_.set_actual_person_Target_pointer(iit);
+				}
+
+			}*/
+			//std::cout << "3 in ini_case!"<< std::endl;
+////// finish create person id=2;
+
+		//destinations1p1_.clear();
+		//destinations1p1_.reserve(2);
+		//destinations1p1_.clear();
+		//destinations1p1_.reserve(2);
+		//destinations1p2_.clear();
+		//destinations1p2_.reserve(2);
+		//destinations2p3_.clear();
+		//destinations2p3_.reserve(2);
+		//destinations2p3_.clear();
+		//destinations2p3_.reserve(2);
+		//destinations2p4_.clear();
+		//destinations2p4_.reserve(2);
+		// possible destinations in the scene:
+		// dest 0=>  x=-1.0 ; y=1.0  (0, -1.0, 1.0, 0.25, 3, 1, 2, 3)
+		// dest 1=>  x=17.0 ; y=1.0 (1, 17.0, 1.0, 0.25, 3, 0, 2, 3)
+		// dest 2=>  x=17.0 ; y= -8.0 (2, 17.0, -8.0, 0.25, 3, 0, 1, 3)
+		// dest 3=>  x=-1.0 ; y= 8.0 (3, -1.0, 8.0, 0.25, 3, 0, 1, 2)
+
+		// generation grup 1 of persons. Goals=>
+		// dest 0=>  x=-1.0 ; y=1.0  (0, -1.0, 1.0, 0.25, 3, 1, 2, 3)
+		// dest 1=>  x=17.0 ; y=1.0 (1, 17.0, 1.0, 0.25, 3, 0, 2, 3)
+
+		//std::cout << "2" << std::endl;
+		//double person_companion_velocity=0.4;//rand_normal(1.0,0.1);
+		dest_1 = get_destinations()->at( 8 );
+		dest_2 = get_destinations()->at( 9 );
+		//std::cout << "3" << std::endl;
+		dest_10 = get_destinations()->at( 10 );
+		dest_11 = get_destinations()->at( 11 );
+		//std::cout << "4" << std::endl;
+		dest_12 = get_destinations()->at( 12 );
+		dest_13 = get_destinations()->at( 13 );
+		//std::cout << "5" << std::endl;
+		dest_14 = get_destinations()->at( 14 );
+		//std::cout << "6" << std::endl;
+		destinations1_.clear();
+		destinations1_.push_back(dest_1); // destinations1_[dest_8,dest_9,dest_10,dest_11,dest_12, dest_13,dest_14]
+		destinations1_.push_back(dest_2);
+		//std::cout << "7" << std::endl;
+		destinations1_.push_back(dest_10);
+		destinations1_.push_back(dest_11);
+		//std::cout << "8" << std::endl;
+
+			destinations1_.push_back(dest_12);
+			destinations1_.push_back(dest_13);
+			//std::cout << "9" << std::endl;
+			destinations1_.push_back(dest_14);
+
+		if(debug_init_scene_sim_){
+			std::cout << "10" << std::endl;
+		}
+		//new_person = add_person(1);
+		//person_companion_person_abstract_.set_id(1);
+		person_companion_person_abstract_.set_id(id_person_companion_);
+		if(group_simulation_){
+			//person_companion_person_abstract_2group_person_.set_id(id_person_companion_2group_pers_);
+		}
+
+		//std::cout << " 4 in ini_case!"<< std::endl;
+
+		if(state_act_==Cplan_local_nav_person_companion::START){
+			std::vector<Sdestination> goal_person_dest2;
+			std::vector<int> neighbours_ids_act;
+			neighbours_ids_act.clear();
+			neighbours_ids_act.push_back(1);
+			//neighbours_ids_act.push_back(id_person_companion_);
+			Sdestination goal_person_dest(id_person_companion_, actual_goal_to_return_person_comp_.x , actual_goal_to_return_person_comp_.y , 1 ,Sdestination::Map_goal, neighbours_ids_act);
+			goal_person_dest2.clear();
+			goal_person_dest2.push_back(goal_person_dest);
+			person_companion_person_abstract_.set_destinations(goal_person_dest2);
+			if(group_simulation_){
+				person_companion_person_abstract_2group_person_.set_destinations(goal_person_dest2);
+			}
+
+			actual_goal_to_return_person_comp_=actual_goal_to_return_person_comp1_;
+		}else if(state_act_==Cplan_local_nav_person_companion::ITER){
+			//std::cout << "(1) SET ALL DEST destinations1_.size() "<< destinations1_.size() << std::endl;
+			person_companion_person_abstract_.set_destinations(destinations1_); // TODO: cambiar
+			if(group_simulation_){
+				person_companion_person_abstract_2group_person_.set_destinations(destinations1_);
+			}
+
+		}
+
+		//std::cout << " 5 in ini_case!"<< std::endl;
+		//new_person->set_destinations(destinations1_);
+
+		double x, y;		//initial pose checks for free cells if an obstacle map is provided (no miro k sea prohivida, espacio sin obstaculos y puesta a mano)
+		x =dest_1.x ;
+		y =dest_1.y ;
+
+		person_companion_person_abstract_.set_desired_velocty( less_companion_velocity_);
+		if(group_simulation_){
+			person_companion_person_abstract_2group_person_.set_desired_velocty( less_companion_velocity_);
+		}
+
+		if(state_act_==Cplan_local_nav_person_companion::START){
+			std::vector<int> neighbours_ids_act;
+			neighbours_ids_act.clear();
+			neighbours_ids_act.push_back(1);
+			//neighbours_ids_act.push_back(id_person_companion_);
+			Sdestination goal_person_dest(id_person_companion_, actual_goal_to_return_person_comp_.x , actual_goal_to_return_person_comp_.y , 1 ,Sdestination::Map_goal, neighbours_ids_act);
+			person_companion_person_abstract_.set_best_dest( goal_person_dest);
+			if(group_simulation_){
+				person_companion_person_abstract_2group_person_.set_best_dest( goal_person_dest);
+			}
+
+			actual_goal_to_return_person_comp_=actual_goal_to_return_person_comp1_;
+		}else if(state_act_== Cplan_local_nav_person_companion::ITER){
+
+			person_companion_person_abstract_.set_best_dest( destinations1_[1]); //TODO: cambia
+			if(group_simulation_){
+				person_companion_person_abstract_2group_person_.set_best_dest( destinations1_[1]);
+			}
+
+		}
+
+		//std::cout << " 6 in ini_case!"<< std::endl;
+		//new_person->set_desired_velocty( less_companion_velocity );
+		//new_person->set_best_dest( new_person->get_destinations()->at( 1 ) );
+		//person_best_dest_[0]=new_person->get_destinations()->at( 1 );
+
+		dest_person1_=destinations1_;
+		//if(debug_init_scene_sim_){
+			//std::cout << "OOOOOOOOOOOO (person 1) new_person->get_destinations()->at( 1 ).print():" << std::endl;
+			//new_person->get_destinations()->at( 1 ).print();
+			//std::cout << " Destinations person 1:" << std::endl;
+			//for(unsigned int dest=0; dest<dest_person1_.size(); dest++){
+			//	dest_person1_[dest].print();
+			//}
+		//}
+
+		//persons_destinations[0].push_back(dest_person1_);
+
+		//double act_time_window=5.0;
+		SpointV_cov person_act;
+		person_act=SpointV_cov(x , y, now);
+
+		if(debug_init_scene_sim_){
+			std::cout << "person_act" << std::endl;
+			person_act.print();
+		}
+
+	//	std::cout << " 7 in ini_case!"<< std::endl;
+
+		//double theta=atan2(destinations1_[1].y-person_act.y,destinations1_[1].x-person_act.x);
+		double theta=ini_theta_person_companion;
+		double vx=less_companion_velocity*cos(theta);
+		double vy=less_companion_velocity*sin(theta);
+		//new_person->add_pointV( SpointV_cov(x , y, now, vx, vy),  filtering_method_ );
+		//person_companion_person_abstract_.add_pointV(SpointV_cov(x , y, now, vx, vy),  filtering_method_ );
+		person_companion_person_abstract_.add_pointV(initial_companion_person_robot_point_,  filtering_method_ );
+		if(group_simulation_){
+			person_companion_person_abstract_2group_person_.add_pointV(initial_companion_person_robot_point_2group_person_,  filtering_method_ );
+
+		}
+// TODO: ver pq el id=1 si tiene en cuenta la person id=3, para no chocar. Pero alreves no tiene encuenta a la p1 y choca. Creo que es por construccion
+		// del simulador.
+		//std::cout << " 8 in ini_case!"<< std::endl;
+		if(debug_init_scene_sim_){
+			std::cout << "initial_companion_person_robot_point_.print(): " << std::endl;
+			initial_companion_person_robot_point_.print();
+			std::cout << "after person_companion_person_abstract_.add_pointV" << std::endl;
+			std::cout << "initial_companion_person_robot_point_2group_person_.print(): " << std::endl;
+			initial_companion_person_robot_point_2group_person_.print();
+		}
+
+		Spose observation2=Spose(x, y, now , theta , less_companion_velocity);
+
+		if(debug_init_scene_sim_){
+			std::cout << "Spose_person_act" << std::endl;
+			observation2.print();
+		}
+
+		SpointV_cov obs_spoint=SpointV_cov(x, y, now,vx ,vy);
+
+		if(debug_init_scene_sim_){
+			std::cout << "SpointV_cov_person_act" << std::endl;
+			obs_spoint.print();
+		}
+
+		//std::cout << " 9 in id_person_companion_="<<id_person_companion_<< std::endl;
+		if(state_act_==Cplan_local_nav_person_companion::START){
+
+			std::vector<int> neighbours_ids_act;
+			neighbours_ids_act.clear();
+			neighbours_ids_act.push_back(1);
+			//neighbours_ids_act.push_back(id_person_companion_);
+			std::vector<Sdestination> goal_person_dest2;
+			Sdestination goal_person_dest(id_person_companion_, actual_goal_to_return_person_comp_.x , actual_goal_to_return_person_comp_.y , 1 ,Sdestination::Map_goal, neighbours_ids_act);
+			goal_person_dest2.clear();
+			goal_person_dest2.push_back(goal_person_dest);
+			akp_for_person_companion_.set_robot_destinations(goal_person_dest2); // TODO: cambia
+			if(group_simulation_){
+				//akp_for_person_companion_2_group_person_.set_robot_destinations(goal_person_dest2);
+			}
+			actual_goal_to_return_person_comp_=actual_goal_to_return_person_comp1_;
+
+		}else if(state_act_==Cplan_local_nav_person_companion::ITER){
+
+			//std::cout << "(2) (set all dest) destinations1_.size() "<< destinations1_.size() << std::endl;
+			akp_for_person_companion_.set_robot_destinations(destinations1_); // TODO: cambia
+			if(group_simulation_){
+				//akp_for_person_companion_2_group_person_.set_robot_destinations(destinations1_);
+			}
+		}
+
+		//std::cout << " 10 in ini_case!"<< std::endl;
+
+		//akp_for_person_companion_.set_id_person_companion(1);
+		//akp_for_person_companion_.set_id_person_companion_Cprediction_bhmip(1);
+		akp_for_person_companion_.set_id_person_companion(id_person_companion_);
+		akp_for_person_companion_.set_id_person_companion_Cprediction_bhmip(id_person_companion_);
+
+		/*if(group_simulation_){
+			//akp_for_person_companion_2_group_person_.set_id_person_companion(id_person_companion_2group_pers_);
+			//akp_for_person_companion_2_group_person_.set_id_person_companion_Cprediction_bhmip(id_person_companion_2group_pers_);
+
+		}*/
+
+		//std::cout << " 11 in ini_case!"<< std::endl;
+
+			if(state_act_==Cplan_local_nav_person_companion::START){
+				std::vector<int> neighbours_ids_act;
+				neighbours_ids_act.push_back(1);
+				//neighbours_ids_act.push_back(id_person_companion_);
+				Sdestination goal_person_dest(id_person_companion_, actual_goal_to_return_person_comp_.x , actual_goal_to_return_person_comp_.y , 1 ,Sdestination::Map_goal, neighbours_ids_act);
+				//std::vector<Sdestination> goal_person_dest2;
+				//goal_person_dest2.push_back(goal_person_dest);
+				person_actual_best_dest_=goal_person_dest; // TODO: cambia (sera la de la persona predicha)
+				actual_goal_to_return_person_comp_=actual_goal_to_return_person_comp1_;
+
+			}else if(state_act_==Cplan_local_nav_person_companion::ITER){
+				person_actual_best_dest_=destinations1_[1]; // TODO: cambia (sera la de la persona predicha)
+			}
+
+		//std::cout << " 12 in ini_case!"<< std::endl;
+
+		//akp_for_person_companion_.set_robot_goal_person_companion_akp(new_person->get_destinations()->at( 1 ));
+		//akp_for_person_companion_.set_robot_goal_person_companion_akp(destinations1_[1]);
+
+
+		if(	state_act_==Cplan_local_nav_person_companion::START){
+			std::vector<int> neighbours_ids_act;
+			neighbours_ids_act.push_back(1);
+			//neighbours_ids_act.push_back(id_person_companion_);
+			Sdestination goal_person_dest(id_person_companion_, actual_goal_to_return_person_comp_.x , actual_goal_to_return_person_comp_.y , 1 ,Sdestination::Map_goal, neighbours_ids_act);
+			//std::vector<Sdestination> goal_person_dest2;
+			//goal_person_dest2.push_back(goal_person_dest);
+			akp_for_person_companion_.get_person_companion_akp()->set_best_dest(goal_person_dest); // TODO: cambia (sera la de la persona predicha)
+
+			if(group_simulation_){
+				//akp_for_person_companion_2_group_person_.get_person_companion_akp()->set_best_dest(goal_person_dest);
+			}
+			actual_goal_to_return_person_comp_=actual_goal_to_return_person_comp1_;
+
+		}else if(state_act_==Cplan_local_nav_person_companion::ITER){
+				akp_for_person_companion_.get_person_companion_akp()->set_best_dest(destinations1_[1]); // TODO: cambia (sera la de la persona predicha)
+				if(group_simulation_){
+					//akp_for_person_companion_2_group_person_.get_person_companion_akp()->set_best_dest(destinations1_[1]);
+				}
+		}
+
+		//std::cout << " 13 in ini_case!"<< std::endl;
+
+		//akp_for_person_companion_.update_person_companion(observation, obs_spoint);
+		// OJO: cosa_quitada (lo de arriba)
+		//akp_for_person_companion_.update_person_companion(initial_companion_person_robot_Spose_, initial_companion_person_robot_point_);
+		akp_for_person_companion_.get_person_companion_akp()->set_desired_velocty(less_companion_velocity_);
+		akp_for_person_companion_.set_person_companion_desired_velocity_sim(less_companion_velocity_);
+		akp_for_person_companion_.get_robot2()->set_v_max(0.8); // initial robot v_max.
+
+		//if(group_simulation_){
+			//akp_for_person_companion_2_group_person_.get_person_companion_akp()->set_desired_velocty(less_companion_velocity_);
+			//akp_for_person_companion_2_group_person_.set_person_companion_desired_velocity_sim(less_companion_velocity_);
+			//akp_for_person_companion_2_group_person_.get_robot2()->set_v_max(0.8); // initial robot v_max.
+		//}
+
+
+		//std::cout << " Cscene_sim antes robot_->set_desired_velocty(0.8); " << std::endl;
+		//robot_->set_desired_velocty(0.8);
+		//std::cout << " 14 in ini_case!"<< std::endl;
+		person_companion_plan_succed_=true;
+
+		new_person_companion_pose_=observation2;
+
+		new_points_person_sim_.push_back(SpointV_cov(x , y, now, vx, vy));
+		persons_ids_.push_back(id_person_companion_); // person id=1, if normal case
+		person_best_dest_[0]=person_actual_best_dest_;
+
+		if(debug_init_scene_sim_){
+			std::cout << "(Cscene_sim) akp_for_person_companion_.get_person_companion_akp()->get_best_dest().print(): " << std::endl;
+			akp_for_person_companion_.get_person_companion_akp()->get_best_dest().print();
+			//if(group_simulation_){
+				//std::cout << "(Cscene_sim) akp_for_person_companion_2_group_person_.get_person_companion_akp()->get_best_dest().print(): " << std::endl;
+				//akp_for_person_companion_2_group_person_.get_person_companion_akp()->get_best_dest().print();
+			//}
+		}
+
+		if(debug_init_scene_sim_){
+			std::cout << "akp_for_person_companion_.get_person_companion_akp()->get_desired_velocty(less_companion_velocity)="<< akp_for_person_companion_.get_person_companion_akp()->get_desired_velocity() << std::endl;
+		}
+
+
+
+		///////// INI PERSON 2 //////////////
+			dest_1 = get_destinations()->at( 0 );    //TODO: target person 2 [id=2, por ejemplo es con la que vamos a hablar.]
+			dest_2 = get_destinations()->at( 1 );
+			//std::cout << "13" << std::endl;
+			destinations1_.clear();
+			destinations1_.push_back(dest_1);
+			destinations1_.push_back(dest_2);
+			//std::cout << " 2.1 in ini_case!"<< std::endl;
+
+			new_person = add_person(2);
+			new_person->set_destinations(destinations1_);
+			//std::cout << " 2.2 in ini_case!"<< std::endl;
+
+			//double x, y;		//initial pose checks for free cells if an obstacle map is provided (no miro k sea prohivida, espacio sin obstaculos y puesta a mano)
+			x =dest_1.x ;
+			y =dest_1.y ;
+			//std::cout << " 2.3 in ini_case!"<< std::endl;
+
+			new_person->set_desired_velocty( group_velocity1_1_ );
+			//std::cout << " 2.4 in ini_case!"<< std::endl;
+			new_person->set_best_dest( new_person->get_destinations()->at( 1 ) ); //coges la destinacion 1 de esta persona, porque ahora esta en la destinacion 0
+			//std::cout << " 2.5 in ini_case! number_virtual_people_"<<number_virtual_people_<< std::endl;
+			person_best_dest_[1]=new_person->get_destinations()->at( 1 );
+			//std::cout << " 2.6 in ini_case!"<< std::endl;
+			dest_person2_3_=destinations1_;
+			//new_person->set_destinations(destinations1p1_);
+
+			if(debug_init_scene_sim_){
+				std::cout << "(person 2 y 3) new_person->get_destinations()->at( 1 ).print():" << std::endl;
+				new_person->get_destinations()->at( 1 ).print();
+				std::cout << " Destinations person 2 y 3:" << std::endl;
+				for(unsigned int dest=0; dest<dest_person2_3_.size(); dest++){
+					dest_person2_3_[dest].print();
+				}
+			}
+			//std::cout << " 2.7 in ini_case!"<< std::endl;
+			person_act=SpointV_cov(x , y, now);
+			//std::cout << " 2.8 in ini_case!"<< std::endl;
+			if(debug_init_scene_sim_){
+				std::cout << "person_act" << std::endl;
+				person_act.print();
+			}
+
+			theta= atan2(new_person->get_destinations()->at( 1 ).y-person_act.y,new_person->get_destinations()->at( 1 ).x-person_act.x);
+			//std::cout << " 2.9 in ini_case!"<< std::endl;
+			//double vx=target_person_velocity*cos(theta);
+			vx=group_velocity1_1*cos(theta);
+			//std::cout << " 2.10 in ini_case!"<< std::endl;
+			vy=group_velocity1_1*sin(theta);
+			//double vy=target_person_velocity*sin(theta);
+			//std::cout << " 2.11 in ini_case!"<< std::endl;
+			//new_person->add_pointV( SpointV_cov(x , y, now, vx, vy),  filtering_method_ );
+			/*if(group_simulation_){
+				if(other_companion_person_id_==2){
+					new_person->add_pointV( other_companion_person_out_odometry_spoint_,  filtering_method_ );
+					// cambio la posicion de esta person de la lista internamente.
+					//std::cout << " ??????? other_companion_person_out_odometry_spoint_.x="<<other_companion_person_out_odometry_spoint_.x<<"; .y="<<other_companion_person_out_odometry_spoint_.y << std::endl;
+				}else if(id_second_companion_==2){
+					double dx=1.0;
+					double dy=0.5;
+					double theta=atan(initial_companion_person_robot_point_.vy/initial_companion_person_robot_point_.vx);
+					new_person->add_pointV(SpointV_cov(initial_companion_person_robot_point_.x+cos(theta)*dx,initial_companion_person_robot_point_.y+cos(theta)*dy, now, initial_companion_person_robot_point_.vx,initial_companion_person_robot_point_.vy),  filtering_method_); //TODO: add second_person_Companion
+				}else{
+					new_person->add_pointV( SpointV_cov(x , y, now, vx, vy),  filtering_method_ );
+				}
+			}else{*/
+				new_person->add_pointV( SpointV_cov(x , y, now, vx, vy),  filtering_method_ );
+			//}
+
+			//std::cout << " 2.12 in ini_case!"<< std::endl;
+			new_points_person_sim_.push_back(SpointV_cov(x , y, now, vx, vy));
+			//new_person->add_pointV( SpointV_cov(x , y, now),  filtering_method_ );
+			//new_points_person_sim_.push_back(SpointV_cov(x , y, now));
+		//	std::cout << " 2.13 in ini_case!"<< std::endl;
+			persons_ids_.push_back(2);
+
+
+	///////// FIN PERSON 2 //////////////
+
+
+
+
+	//	std::cout << " 15 in ini_case!"<< std::endl;
+if(!Scene_create_fake_fixed_second_person_companion_){
+// PERSONA ID 3, grupo con persona ID 7 o 6.
+			//new_person->print_dest();
+			//new_person->print();
+			//std::cout << "14" << std::endl;
+			dest_1 = get_destinations()->at( 2 );
+			dest_2 = get_destinations()->at( 3 );
+			destinations1_.clear();
+			destinations1_.push_back(dest_1);
+			destinations1_.push_back(dest_2);
+			if(all_complete_random_final_dest_people_){
+				Sdestination dest_new=get_destinations()->at( 4 );
+				destinations1_.push_back(dest_new);
+				dest_new=get_destinations()->at( 5 );
+				destinations1_.push_back(dest_new);
+				dest_new=get_destinations()->at( 6 );
+				destinations1_.push_back(dest_new);
+				dest_new=get_destinations()->at( 7 );
+				destinations1_.push_back(dest_new);
+			}
+			if(group_simulation_){
+				if(id_person_companion_!=1){ // si la otra persona del grupo tiene id=1; Entonces generame la persona con id=1, que antes era la person companion.
+					new_person = add_person(1);
+					new_person->set_destinations(destinations1_);
+
+
+				}else{
+					new_person = add_person(3);
+					new_person->set_destinations(destinations1_);
+					//new_person->set_destinations(destinations1p2_);
+
+				}
+			}else{
+				new_person = add_person(3);
+				new_person->set_destinations(destinations1_);
+			}
+
+
+
+			//new_person->set_destinations(destinations1p2_);
+						x =dest_1.x;
+						y =dest_1.y;
+
+						new_person->set_desired_velocty( group_velocity1_ );
+						//std::cout << "; group_velocity1" <<group_velocity1<< std::endl;
+
+						new_person->set_best_dest( new_person->get_destinations()->at( 1 ) ); //coges la destinacion 1 de esta persona, porque ahora esta en la destinacion 0
+						person_best_dest_[2]=new_person->get_destinations()->at( 1 );
+
+						person_act=SpointV_cov(x , y, now);
+
+						if(debug_init_scene_sim_){
+							std::cout << "person_act" << std::endl;
+							person_act.print();
+						}
+
+						theta= atan2(new_person->get_destinations()->at( 1 ).y-person_act.y,new_person->get_destinations()->at( 1 ).x-person_act.x);
+						vx=group_velocity1*cos(theta);
+						vy=group_velocity1*sin(theta);
+						//new_person->add_pointV( SpointV_cov(x , y, now, vx, vy),  filtering_method_ );
+						/*if(group_simulation_){
+							if(other_companion_person_id_==3){
+								new_person->add_pointV( other_companion_person_out_odometry_spoint_,  filtering_method_ );
+								// cambio la posicion de esta person de la lista internamente.
+								//std::cout << " ??????? other_companion_person_out_odometry_spoint_.x="<<other_companion_person_out_odometry_spoint_.x<<"; .y="<<other_companion_person_out_odometry_spoint_.y << std::endl;
+							}else if(id_second_companion_==3){
+												double dx=1.0;
+												double dy=0.5;
+												double theta=atan(initial_companion_person_robot_point_.vy/initial_companion_person_robot_point_.vx);
+												new_person->add_pointV(SpointV_cov(initial_companion_person_robot_point_.x+cos(theta)*dx,initial_companion_person_robot_point_.y+cos(theta)*dy, now, initial_companion_person_robot_point_.vx,initial_companion_person_robot_point_.vy),  filtering_method_); //TODO: add second_person_Companion
+
+							}else if(id_person_companion_==3){
+								// no insertes persona en la person list interna!!!
+								new_person->add_pointV( SpointV_cov(x , y, now, vx, vy),  filtering_method_ );
+							}else{
+								new_person->add_pointV( SpointV_cov(x , y, now, vx, vy),  filtering_method_ );
+							}
+						}else{*/
+							new_person->add_pointV( SpointV_cov(x , y, now, vx, vy),  filtering_method_ );
+						//}
+
+
+						new_points_person_sim_.push_back(SpointV_cov(x , y, now, vx, vy));
+						//new_person->add_pointV( SpointV_cov(x , y, now),  filtering_method_ );
+						//new_points_person_sim_.push_back(SpointV_cov(x , y, now));
+						if(group_simulation_){
+							if(id_person_companion_!=1){
+								persons_ids_.push_back(1);
+								new_person->add_pointV( SpointV_cov(x , y, now, vx, vy),  filtering_method_ ); // te hace falta persona normal no la del robot para la persona 1.
+							}else{
+								persons_ids_.push_back(3);
+							}
+						}else{
+							persons_ids_.push_back(3);
+						}
+ // fin person ide 3
+
+}
+//std::cout << "[1]" << std::endl;
+/*			// INICIO persona ID 7
+			//new_person->print_dest();
+			//new_person->print();
+			//std::cout << "14" << std::endl;
+			dest_1 = get_destinations()->at( 19 );
+			dest_2 = get_destinations()->at( 20 );
+			destinations1_.clear();
+			destinations1_.push_back(dest_1);
+			destinations1_.push_back(dest_2);
+
+			new_person = add_person(6);
+			new_person->set_destinations(destinations1_);
+			//new_person->set_destinations(destinations1p2_);
+			x =dest_1.x;
+			y =dest_1.y;
+
+			new_person->set_desired_velocty( group_velocity1 );
+			//std::cout << "; group_velocity1" <<group_velocity1<< std::endl;
+
+			new_person->set_best_dest( new_person->get_destinations()->at( 1 ) ); //coges la destinacion 1 de esta persona, porque ahora esta en la destinacion 0
+			person_best_dest_[5]=new_person->get_destinations()->at( 1 );
+
+			person_act=SpointV_cov(x , y, now);
+
+			if(debug_init_scene_sim_){
+				std::cout << "person_act" << std::endl;
+				person_act.print();
+			}
+
+			theta= atan2(new_person->get_destinations()->at( 1 ).y-person_act.y,new_person->get_destinations()->at( 1 ).x-person_act.x);
+			vx=group_velocity1*cos(theta);
+			vy=group_velocity1*sin(theta);
+			new_person->add_pointV( SpointV_cov(x , y, now, vx, vy),  filtering_method_ );
+			new_points_person_sim_.push_back(SpointV_cov(x , y, now, vx, vy));
+			//new_person->add_pointV( SpointV_cov(x , y, now),  filtering_method_ );
+			//new_points_person_sim_.push_back(SpointV_cov(x , y, now));
+			persons_ids_.push_back(6);
+			// FIN persona ID 7*/
+//std::cout << "[2]" << std::endl;
+			//new_person->print_dest();
+			//new_person->print();
+			//std::cout << "15" << std::endl;
+			ids_of_persons_in_group1_.push_back(2);  // person with id=1,2 are in the first group
+			ids_of_persons_in_group1_.push_back(3);
+			ids_of_persons_in_group1_.push_back(7);
+			// generation grup 2 of persons. Goals=>
+			// dest 2=>  x=17.0 ; y= -8.0 (2, 17.0, -8.0, 0.25, 3, 0, 1, 3)
+			// dest 3=>  x=-1.0 ; y= 8.0 (3, -1.0, 8.0, 0.25, 3, 0, 1, 2)
+			//if(only_companion_person_){
+			Sdestination dest_3, dest_4;
+
+			//set initial position at random. initially 2 destinations
+			// if complete random people:
+			//dest_3 = get_destinations()->at( 4 );
+			//dest_4 = get_destinations()->at( 5 );
+			dest_4 = get_destinations()->at( 2 );
+			dest_3 = get_destinations()->at( 3 );
+
+			destinations2_.clear();
+			if(all_complete_random_final_dest_people_){
+				Sdestination dest_new=get_destinations()->at( 4 );
+				destinations2_.push_back(dest_new);
+				dest_new=get_destinations()->at( 5 );
+				destinations2_.push_back(dest_new);
+			}
+			destinations2_.push_back(dest_3);
+			destinations2_.push_back(dest_4);
+			if(all_complete_random_final_dest_people_){
+				Sdestination dest_new=get_destinations()->at( 6 );
+				destinations2_.push_back(dest_new);
+				dest_new=get_destinations()->at( 7 );
+				destinations2_.push_back(dest_new);
+			}
+
+			//std::cout << " 16 in ini_case!"<< std::endl;
+
+			new_person = add_person(4);
+			new_person->set_destinations(destinations2_);
+
+			dest_person4_5_=destinations2_;
+			if(debug_init_scene_sim_){
+				std::cout << "(person 4 y 5) new_person->get_destinations()->at( 1 ).print():" << std::endl;
+				new_person->get_destinations()->at( 1 ).print();
+				std::cout << " Destinations person 4 y 5:" << std::endl;
+				for(unsigned int dest=0; dest<dest_person4_5_.size(); dest++){
+					dest_person4_5_[dest].print();
+				}
+			}
+			//new_person->set_destinations(destinations2p3_);
+			x = dest_3.x;  // a X_dist del goal 3, cada destination of person.
+			y = dest_3.y;
+			//std::cout << "16; (person id=4) x="<<x<<"; y="<<y << std::endl;
+			if(group_sincron_){
+				new_person->set_desired_velocty( group_velocity2_ );
+			}else{
+				new_person->set_desired_velocty( group_velocity2_2_ );
+			}
+			new_person->set_best_dest( new_person->get_destinations()->at( 1 ) );
+			person_best_dest_[3]=new_person->get_destinations()->at( 1 );
+
+			person_act=SpointV_cov(x , y, now);
+
+			if(debug_init_scene_sim_){
+				std::cout << "person_act" << std::endl;
+				person_act.print();
+			}
+
+			theta= atan2(new_person->get_destinations()->at( 1 ).y-person_act.y,new_person->get_destinations()->at( 1 ).x-person_act.x);
+			if(group_sincron_){
+				vx=group_velocity2_*cos(theta);
+				vy=group_velocity2_*sin(theta);
+			}else{
+				vx=group_velocity2_2_*cos(theta);
+				vy=group_velocity2_2_*sin(theta);
+			}
+			//new_person->add_pointV( SpointV_cov(x , y, now, vx, vy),  filtering_method_ );
+			/*if(group_simulation_){
+				if(other_companion_person_id_==4){
+					new_person->add_pointV( other_companion_person_out_odometry_spoint_,  filtering_method_ );
+					// cambio la posicion de esta person de la lista internamente.
+					//std::cout << " ??????? other_companion_person_out_odometry_spoint_.x="<<other_companion_person_out_odometry_spoint_.x<<"; .y="<<other_companion_person_out_odometry_spoint_.y << std::endl;
+				}else if(id_second_companion_==4){
+					double dx=1.0;
+					double dy=0.5;
+					double theta=atan(initial_companion_person_robot_point_.vy/initial_companion_person_robot_point_.vx);
+					new_person->add_pointV(SpointV_cov(initial_companion_person_robot_point_.x+cos(theta)*dx,initial_companion_person_robot_point_.y+cos(theta)*dy, now, initial_companion_person_robot_point_.vx,initial_companion_person_robot_point_.vy),  filtering_method_); //TODO: add second_person_Companion
+				}
+				else{
+					new_person->add_pointV( SpointV_cov(x , y, now, vx, vy),  filtering_method_ );
+				}
+			}else{*/
+				new_person->add_pointV( SpointV_cov(x , y, now, vx, vy),  filtering_method_ );
+			//}
+
+
+
+			new_points_person_sim_.push_back(SpointV_cov(x , y, now, vx, vy));
+			//new_person->add_pointV( SpointV_cov(x , y, now),  filtering_method_ );
+			//new_points_person_sim_.push_back(SpointV_cov(x , y, now));
+			persons_ids_.push_back(4);
+
+			//std::cout << " 17 in ini_case!"<< std::endl;
+
+			//new_person->print_dest();
+			//new_person->print();
+			// if complete random people
+			//dest_3 = get_destinations()->at( 6 );
+			//dest_4 = get_destinations()->at( 7 );
+
+			dest_3 = get_destinations()->at( 2 );
+			dest_4 = get_destinations()->at( 3 );
+
+			destinations2_.clear();
+
+			if(all_complete_random_final_dest_people_){
+				Sdestination dest_new=get_destinations()->at( 6 );
+				destinations2_.push_back(dest_new);
+				dest_new=get_destinations()->at( 7 );
+				destinations2_.push_back(dest_new);
+				dest_new=get_destinations()->at( 4 );
+				destinations2_.push_back(dest_new);
+				dest_new=get_destinations()->at( 5 );
+				destinations2_.push_back(dest_new);
+			}
+			destinations2_.push_back(dest_3);
+			destinations2_.push_back(dest_4);
+
+			new_person = add_person(5);
+			new_person->set_destinations(destinations2_);
+			//new_person->set_destinations(destinations2p4_);
+			x = dest_3.x+1.0;  // a X_dist del goal 3, cada destination of person.
+			y = dest_3.y+1.0;
+			//set destination at random
+			new_person->set_desired_velocty( group_velocity2_ );
+			new_person->set_best_dest( new_person->get_destinations()->at( 1 ) );
+			person_best_dest_[4]=new_person->get_destinations()->at( 1 );
+
+			person_act=SpointV_cov(x , y, now);
+
+			if(debug_init_scene_sim_){
+				std::cout << "person_act" << std::endl;
+				person_act.print();
+			}
+
+			theta= atan2(new_person->get_destinations()->at( 1 ).y-person_act.y,new_person->get_destinations()->at( 1 ).x-person_act.x);
+			vx=group_velocity2_*cos(theta);
+			vy=group_velocity2_*sin(theta);
+
+			/*if(group_simulation_){
+				if(other_companion_person_id_==5){
+					new_person->add_pointV( other_companion_person_out_odometry_spoint_,  filtering_method_ );
+					 // cambio la posicion de esta person de la lista internamente.
+					//std::cout << " ??????? other_companion_person_out_odometry_spoint_.x="<<other_companion_person_out_odometry_spoint_.x<<"; .y="<<other_companion_person_out_odometry_spoint_.y << std::endl;
+				}else if(id_second_companion_==5){
+					double dx=1.0;
+					double dy=0.5;
+					double theta=atan(initial_companion_person_robot_point_.vy/initial_companion_person_robot_point_.vx);
+					new_person->add_pointV(SpointV_cov(initial_companion_person_robot_point_.x+cos(theta)*dx,initial_companion_person_robot_point_.y+cos(theta)*dy, now, initial_companion_person_robot_point_.vx,initial_companion_person_robot_point_.vy),  filtering_method_); //TODO: add second_person_Companion
+				}
+				else{
+					new_person->add_pointV( SpointV_cov(x , y, now, vx, vy),  filtering_method_ );
+				}
+			}else{*/
+				new_person->add_pointV( SpointV_cov(x , y, now, vx, vy),  filtering_method_ );
+			//}
+
+			new_points_person_sim_.push_back(SpointV_cov(x , y, now, vx, vy));
+			//new_person->add_pointV( SpointV_cov(x , y, now),  filtering_method_ );
+			//new_points_person_sim_.push_back(SpointV_cov(x , y, now));
+			persons_ids_.push_back(5);
+
+			//new_person->print_dest();
+			//new_person->print();
+			//std::cout << "17" << std::endl;
+			ids_of_persons_in_group2_.push_back(4);  // person with id=3,4 are in the second group
+			ids_of_persons_in_group2_.push_back(5);
+
+			if(debug_init_scene_sim_){
+				std::cout << " initialization (Cscene_sim::update_scene_companion_simulation_akp_person_companion) person_list_.empty()= "<< person_list_.empty() << std::endl;
+				std::cout << " initialization  (show_person_list)  person_list_.size()="<<person_list_.size()<< std::endl;
+				for( iit = person_list_.begin() ; iit!=person_list_.end() ; iit++)
+				{
+
+
+					std::cout << "(*iit)->get_id()= "<< (*iit)->get_id() << std::endl;
+				}
+				std::cout << " INI Cscene_SIM antes Cscene_sim-> update_companion_person: person_list_.empty()="<<person_list_.empty()<< std::endl;
+			}
+
+			//akp_for_person_companion_.set_person_list(person_list_);
+			//std::cout << " INI Cscene_SIM (print interior person_list) antes Cscene_sim-> update_companion_person: akp_for_person_companion_.get_scene()->empty()="<<akp_for_person_companion_.get_scene()->empty()<< std::endl;
+			if(debug_init_scene_sim_){
+				akp_for_person_companion_.print();
+				if(group_simulation_){
+					//akp_for_person_companion_2_group_person_.print();
+				}
+
+			}
+
+
+			// TODO: cambiar position person id que sea la otra person companion den grupo, dentro del vector => new_points_person_sim_ buscarlo apartir de persons_ids_
+			// TODO: ojo! los add_point también habra que cambiarlos, ya veremos como...
+			/*if(group_simulation_){
+				for(unsigned int u=0; u<persons_ids_.size();u++){
+					if(other_companion_person_id_==persons_ids_[u]){
+						new_points_person_sim_[u]=other_companion_person_out_odometry_spoint_; // cambio el track externo, pero no el interno del metodo. el interno del metodo se cambia con el add_person
+						//std::cout << " ??????? other_companion_person_out_odometry_spoint_.x="<<other_companion_person_out_odometry_spoint_.x<<"; .y="<<other_companion_person_out_odometry_spoint_.y << std::endl;
+					}else if(id_second_companion_==persons_ids_[u]){
+						double dx=1.0;
+						double dy=0.5;
+						double theta=atan(initial_companion_person_robot_point_.vy/initial_companion_person_robot_point_.vx);
+						new_points_person_sim_[u]=SpointV_cov(initial_companion_person_robot_point_.x+cos(theta)*dx,initial_companion_person_robot_point_.y+cos(theta)*dy, now, initial_companion_person_robot_point_.vx,initial_companion_person_robot_point_.vy); //TODO: add second_person_Companion
+					}
+				}
+			}*/
+
+			/*std::cout << " ANTES UPDATE_SCENE (person list clase Cscene_sim); id_person_companion_= "<<id_person_companion_ << std::endl;
+			for(unsigned int h=0;h<persons_ids_.size();h++){
+				std::cout << "persons_ids_[h]="<<persons_ids_[h] << std::endl;
+			}*/
+
+
+			update_scene_for_person_companion_planning_akp();
+
+			/*std::cout << " Despues UPDATE_SCENE (person list clase Cscene_sim) " << std::endl;
+			for(unsigned int h=0;h<persons_ids_.size();h++){
+							std::cout << "persons_ids_[h]="<<persons_ids_[h] << std::endl;
+						}*/
+			if(debug_init_scene_sim_){
+				std::cout << " INI CASE! IMPORTANTE!!! Despues UPDATE_SCENE (person list clase Cscene_sim) " << std::endl;
+				for( iit = person_list_.begin() ; iit!=person_list_.end() ; iit++)
+				{
+					std::cout << "(*iit)->get_id()= "<< (*iit)->get_id() <<"(*iit)->get_prediction_trajectory()->size()= "<< (*iit)->get_prediction_trajectory()->size() << std::endl;
+				}
+
+				for(unsigned int y=0; y<persons_ids_.size();y++){
+					std::cout << " INI CASE! IMPORTANTE!!! persons_ids_["<<y<<"]="<<persons_ids_[y] << std::endl;
+				}
+
+				//std::cout << " Despues UPDATE_SCENE " << std::endl;
+			}
+
+			//const std::list<Cperson_abstract *>* person_list = akp_for_person_companion_.get_scene( );
+
+			std::vector<SdetectionObservation> obs;
+			/*if(debug_init_scene_sim_){
+				for( std::list<Cperson_abstract*>::const_iterator iit = person_list->begin(); iit!=person_list->end(); iit++ )
+				{
+					std::cout << "(*iit)->get_id="<<(*iit)->get_id() <<"(*iit)->get_prediction_trajectory()->size()= "<< (*iit)->get_prediction_trajectory()->size() << std::endl;
+				}
+			}*/
+		//}
+			//std::cout << "[3]" << std::endl;
+			//std::cout << " before insert more people in ini_case!"<< std::endl;
+			//insert_more_random_people(observation);
+			//std::cout << " out ini_case!"<< std::endl;
+
+			/*std::cout <<  "7 (Cscene_sim) (out ini case) check people nan's " << std::endl;
+						for( auto iit : person_list_)
+						{
+							if(iit->get_id()!=id_person_companion_){
+
+							 std::cout << " (after Insert people) (1) iit->get_prediction_trajectory( )->size()="<<iit->get_prediction_trajectory( )->size()<<"; iit->get_id()=" <<iit->get_id()<<"; iit->desired_vel="<<iit->get_desired_velocity()<<  std::endl;
+							 iit->get_current_pointV().print();
+							}
+						}*/
+
+
+	}else{ // INI update, NO ini_case!!!
+
+		/*std::cout <<  "8 (Cscene_sim) (in else case UPDATE) check people nan's " << std::endl;
+					for( auto iit : person_list_)
+					{
+						if(iit->get_id()!=id_person_companion_){
+
+						 std::cout << " (after Insert people) (1) iit->get_prediction_trajectory( )->size()="<<iit->get_prediction_trajectory( )->size()<<"; iit->get_id()=" <<iit->get_id()<<"; iit->desired_vel="<<iit->get_desired_velocity()<<  std::endl;
+						 iit->get_current_pointV().print();
+						}
+					}*/
+
+
+		//std::cout << " INI iter person companion!!!!!!!!!!!!!!!!!!!!!!!!!; person_list_.size()="<< person_list_.size()  << std::endl;
+		//std::cout << " 1 update "<< std::endl;
+		/*for( auto iit: person_list_ ){
+			if(iit->get_id()==id_person_goal_){
+				akp_for_person_companion_.set_actual_person_Target_pointer(iit);
+				akp_for_person_companion_.set_actual_person_Target_SpointV(iit->get_current_pointV());
+				iit->get_current_pointV().print();
+				akp_for_person_companion_.set_actual_person_Target_destination(iit->get_best_dest());
+				iit->get_best_dest().print();
+			}
+
+		}*/
+
+
+		//std::cout << " 2 INI update "<< std::endl;
+		//std::cout << "[4]" << std::endl;
+		//std::cout << " 2 update "<< std::endl;
+		if(debug_general_scene_sim_){
+			std::cout << "despues de init!!!   (Cscene_sim::update_scene_companion_simulation_akp_person_companion) person_list_.empty()="<<person_list_.empty()<< std::endl;
+			std::cout << "despues de init!!! (show_person_list)  person_list_.size()="<<person_list_.size()<< std::endl;
+
+				for( iit = person_list_.begin() ; iit!=person_list_.end() ; iit++)
+				{
+
+					std::cout << " (*iit)->get_id()= "<< (*iit)->get_id() << std::endl;
+
+				}
+
+		}
+		//std::cout << "antes updating persons trajectories  " << std::endl;
+		//updating persons trajectories
+		double w;
+		SpointV_cov virtual_next_pose;
+		SpointV_cov virtual_next_pose_2groupPers;
+
+		Sforce virtual_force_goal, virtual_force_int_person, virtual_force_obstacle, virtual_force_obstacle_laser_fake, virtual_force_robot;
+		//std::cout << "18" << std::endl;
+		//std::cout << " 3 update "<< std::endl;
+		for( iit = person_list_.begin() ; iit!=person_list_.end() ; iit++)
+		{
+			//std::cout << " (Cscene_SIM id="<<(*iit)->get_id()<< std::endl;
+			if(debug_other_persons_scene_sim_){
+				std::cout << "(BEGIN BUCLE) if((*iit)->get_id()!=1); (*iit)->get_id()= "<< (*iit)->get_id() << std::endl;
+			}
+
+			if ( ((*iit)->get_person_type() == Cperson_abstract::Person )||((*iit)->get_person_type() == Cperson_abstract::Person_companion))
+			{
+				if(debug_other_persons_scene_sim_){
+					std::cout << "if_person_type; (*iit)->get_id()= "<< (*iit)->get_id() << std::endl;
+				}
+				if(((*iit)->get_id()!=1)||(((*iit)->get_id()==1)&&(id_person_companion_!=1))){ //caso != person companion
+					if(debug_other_persons_scene_sim_){
+						std::cout << "if((*iit)->get_id()!=1); (*iit)->get_id()= "<< (*iit)->get_id() << std::endl;
+					}
+					/*if((*iit)->get_id()==1){
+						Sdestination  dest_1 = get_destinations()->at( 8 );
+						Sdestination  dest_2 = get_destinations()->at( 9 );
+						Sdestination  dest_10 = get_destinations()->at( 10 );
+						Sdestination  dest_11 = get_destinations()->at( 11 );
+						Sdestination  dest_12;
+						Sdestination  dest_13;
+						Sdestination  dest_14;
+						//if(caso_simulacion_giro_){
+							dest_12 = get_destinations()->at( 12 );
+							dest_13 = get_destinations()->at( 13 );
+							dest_14 = get_destinations()->at( 14 );
+						//}
+						destinations1_.clear();
+						destinations1_.push_back(dest_1);
+						destinations1_.push_back(dest_2);
+						destinations1_.push_back(dest_10);
+						destinations1_.push_back(dest_11);
+						//if(caso_simulacion_giro_){
+							destinations1_.push_back(dest_12);
+							destinations1_.push_back(dest_13);
+							destinations1_.push_back(dest_14);
+						//}
+						(*iit)->set_destinations(destinations1_);
+						(*iit)->set_desired_velocty( less_companion_velocity );
+					}*/
+					if(((*iit)->get_id()==2)||(((*iit)->get_id()==3)||((id_person_companion_!=1)&&((*iit)->get_id()==1)))||((*iit)->get_id()==6)){
+						if((*iit)->get_id()==2){
+
+						if(state_act_==Cplan_local_nav_person_companion::ITER){ // &&(group_go_to_interact_with_other_person_)
+							Sdestination  dest_1;
+							Sdestination  dest_2;
+
+							if(actual_sim_case_==Cplan_local_nav_person_companion::case0){
+								std::vector<int> neighbours_ids_act;
+								neighbours_ids_act.push_back(1);
+
+								dest_1 = get_destinations()->at( 0 );
+								dest_2 = get_destinations()->at( 1 );
+								//std::cout << " case 0: "<< std::endl;
+								//std::cout << " dest_1.x="<<dest_1.x<<"; dest_1.y="<<dest_1.y<< std::endl;
+								//std::cout << " dest_2.x="<<dest_2.x<<"; dest_2.y="<<dest_2.y<< std::endl;
+								Sdestination goal_person_dest(2, dest_2.x , dest_2.y , 1 ,Sdestination::Map_goal, neighbours_ids_act);
+								(*iit)->set_best_dest( goal_person_dest );
+								//std::cout << " [1] [ojo if id=1 valor seteado mal] (*iit)->id="<<(*iit)->get_id()<< std::endl;
+								person_best_dest_[(*iit)->get_id()-1]=goal_person_dest;
+
+
+
+
+							}else if(actual_sim_case_==Cplan_local_nav_person_companion::case1){
+								std::vector<int> neighbours_ids_act;
+								neighbours_ids_act.push_back(1);
+								dest_1 = get_destinations()->at( 15 );
+								dest_2 = get_destinations()->at( 16 );
+								//std::cout << " case 1: "<< std::endl;
+								//std::cout << " dest_1.x="<<dest_1.x<<"; dest_1.y="<<dest_1.y<< std::endl;
+								//std::cout << " dest_2.x="<<dest_2.x<<"; dest_2.y="<<dest_2.y<< std::endl;
+								Sdestination goal_person_dest(2, dest_2.x , dest_2.y , 1 ,Sdestination::Map_goal, neighbours_ids_act);
+								(*iit)->set_best_dest( goal_person_dest );
+								//std::cout << " [2] [ojo if id=1 valor seteado mal] (*iit)->id="<<(*iit)->get_id()<< std::endl;
+								person_best_dest_[(*iit)->get_id()-1]=goal_person_dest;
+							}else if(actual_sim_case_==Cplan_local_nav_person_companion::case2){
+								std::vector<int> neighbours_ids_act;
+								neighbours_ids_act.push_back(1);
+								dest_1 = get_destinations()->at( 17 );
+								dest_2 = get_destinations()->at( 18 );
+								//std::cout << " case 2: "<< std::endl;
+								//std::cout << " dest_1.x="<<dest_1.x<<"; dest_1.y="<<dest_1.y<< std::endl;
+								//std::cout << " dest_2.x="<<dest_2.x<<"; dest_2.y="<<dest_2.y<< std::endl;
+								Sdestination goal_person_dest(2, dest_2.x , dest_2.y , 1 ,Sdestination::Map_goal, neighbours_ids_act);
+								(*iit)->set_best_dest( goal_person_dest );
+								//std::cout << " [3] [ojo if id=1 valor seteado mal] (*iit)->id="<<(*iit)->get_id()<< std::endl;
+								person_best_dest_[(*iit)->get_id()-1]=goal_person_dest;
+							}
+
+							destinations1_.clear();
+							destinations1_.push_back(dest_1);
+							destinations1_.push_back(dest_2);
+
+						}else{
+
+							Sdestination  dest_1;
+							Sdestination  dest_2;
+							dest_1 = get_destinations()->at( 0 );
+							dest_2 = get_destinations()->at( 1 );
+							destinations1_.clear();
+							destinations1_.push_back(dest_1);
+							destinations1_.push_back(dest_2);
+
+						}
+
+						(*iit)->set_destinations(destinations1_);
+
+						}
+						if(((*iit)->get_id()==3)||((id_person_companion_!=1)&&((*iit)->get_id()==1))){// cuando tenemos companion person != 1, hay que incluir la persona 1, como otra persona simulada.
+							Sdestination  dest_1 = get_destinations()->at( 2 );
+							Sdestination  dest_2 = get_destinations()->at( 3 );
+							destinations1_.clear();
+							destinations1_.push_back(dest_1);
+							destinations1_.push_back(dest_2);
+							if(all_complete_random_final_dest_people_){
+								Sdestination  dest_3 = get_destinations()->at( 4 );
+								Sdestination  dest_4 = get_destinations()->at( 5 );
+								Sdestination  dest_5 = get_destinations()->at( 6 );
+								Sdestination  dest_6 = get_destinations()->at( 7 );
+								destinations1_.push_back(dest_3);
+								destinations1_.push_back(dest_4);
+								destinations1_.push_back(dest_5);
+								destinations1_.push_back(dest_6);
+							}
+
+							(*iit)->set_destinations(destinations1_);
+
+						}
+						//std::cout << "[5]" << std::endl;
+						/*if((*iit)->get_id()==6){
+							Sdestination  dest_1 = get_destinations()->at( 19 );
+							Sdestination  dest_2 = get_destinations()->at( 20 );
+							destinations1_.clear();
+							destinations1_.push_back(dest_1);
+							destinations1_.push_back(dest_2);
+							(*iit)->set_destinations(destinations1_);
+						}*/
+						//std::cout << "[6]" << std::endl;
+						//(*iit)->set_desired_velocty( group_velocity1);
+						if(random_final_dest_people_){
+							virtual_force_int_person = force_persons_int3( *iit ); // changed by (ely), para que no tenga en cuenta la fuerza repulsiva del compañero.
+						}else{
+							virtual_force_int_person = force_persons_int2( *iit ,ids_of_persons_in_group1_); // changed by (ely), para que no tenga en cuenta la fuerza repulsiva del compañero.
+
+						}
+
+
+						//std::cout << "[7]" << std::endl;
+					}
+					if(((*iit)->get_id()==4)||((*iit)->get_id()==5)){
+						if((*iit)->get_id()==4){
+							// if complete random people
+							//Sdestination  dest_3 = get_destinations()->at( 4 );
+							//Sdestination  dest_4 = get_destinations()->at( 5 );
+							Sdestination  dest_3 = get_destinations()->at( 2 );
+							Sdestination  dest_4 = get_destinations()->at( 3 );
+							destinations2_.clear();
+							destinations2_.push_back(dest_3);
+							destinations2_.push_back(dest_4);
+							if(all_complete_random_final_dest_people_){
+								Sdestination  dest_3 = get_destinations()->at( 4 );
+								Sdestination  dest_4 = get_destinations()->at( 5 );
+								Sdestination  dest_5 = get_destinations()->at( 6 );
+								Sdestination  dest_6 = get_destinations()->at( 7 );
+								destinations1_.push_back(dest_3);
+								destinations1_.push_back(dest_4);
+								destinations1_.push_back(dest_5);
+								destinations1_.push_back(dest_6);
+							}
+
+							(*iit)->set_destinations(destinations2_);
+						}
+						if((*iit)->get_id()==5){
+							// if complete random people
+							//Sdestination  dest_3 = get_destinations()->at( 6 );
+							//Sdestination  dest_4 = get_destinations()->at( 7 );
+
+							Sdestination  dest_3 = get_destinations()->at( 2 );
+							Sdestination  dest_4 = get_destinations()->at( 3 );
+
+							destinations2_.clear();
+							destinations2_.push_back(dest_3);
+							destinations2_.push_back(dest_4);
+							if(all_complete_random_final_dest_people_){
+								Sdestination  dest_3 = get_destinations()->at( 4 );
+								Sdestination  dest_4 = get_destinations()->at( 5 );
+								Sdestination  dest_5 = get_destinations()->at( 6 );
+								Sdestination  dest_6 = get_destinations()->at( 7 );
+								destinations1_.push_back(dest_3);
+								destinations1_.push_back(dest_4);
+								destinations1_.push_back(dest_5);
+								destinations1_.push_back(dest_6);
+							}
+
+							(*iit)->set_destinations(destinations2_);
+
+						}
+						//(*iit)->set_desired_velocty( group_velocity2);
+						if(random_final_dest_people_){
+							virtual_force_int_person = force_persons_int3( *iit ); // changed by (ely), para que no tenga en cuenta la fuerza repulsiva del compañero.
+
+						}else{
+							virtual_force_int_person = force_persons_int2( *iit ,ids_of_persons_in_group2_);
+						}
+
+
+					}
+
+					//1 time step propagation
+
+					virtual_force_goal = (*iit)->force_goal( (*iit)->get_best_dest(), this->get_sfm_params(*iit)  );
+
+					if(debug_approach_people_random_){
+					std::cout <<" id="<<(*iit)->get_id()<<"; vx="<<(*iit)->get_current_pointV().vx<<"; vy="<<(*iit)->get_current_pointV().vy<< " virtual_force_goal.fx= "<<virtual_force_goal.fx<<"; virtual_force_goal.fy="<<virtual_force_goal.fy<< std::endl;
+					}
+
+					virtual_force_obstacle = get_force_map( (*iit)->get_current_pointV().x, (*iit)->get_current_pointV().y ) * 0.5;
+
+					// [ely] Inicio parte para evitar obstaculos del fake laser.
+					//if(read_laser_obstacle_success_)
+					//virtual_force_obstacle_laser_fake = force_objects_laser_int_planning_virtual( robot_, parent_vertex, 25.0, true );
+					Cperson_abstract* person=(*iit);
+
+					const SpointV_cov* virtual_current_point;
+					virtual_current_point = &(person->get_current_pointV());
+					double min_dist2=25.0;
+					virtual_force_obstacle_laser_fake=Sforce();
+					//SpointV person= (SpointV) (*iit)->get_current_pointV();
+					//std::cout<<"(simulated persons) out_laser_obstacle_list"<< std::endl;
+					if(debug_other_persons_scene_sim_){
+						if(((*iit)->get_id()==1)&&(debug_person_obst_forces_)){ // person companion
+							std::cout<<"[ini iter!!!] (SCENE_SIM) laser_obstacle_list_.size() ="<<laser_obstacle_list_.size()<< std::endl;
+							std::cout<<"[ini iter!!!] virtual_force_obstacle_laser_fake.fx ="<<virtual_force_obstacle_laser_fake.fx<<"; virtual_force_obstacle_laser_fake.fy="<<virtual_force_obstacle_laser_fake.fy<< std::endl;
+						}
+					}
+
+					//std::cout << " 4 update "<< std::endl;
+					//std::cout << "19" << std::endl;
+					for( Spoint iit2 : laser_obstacle_list_)
+					{
+						//std::cout<<"!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!11 (simulated persons) entro en laser_obstacle_list_ ="<< std::endl;
+						//there is a list for persons and another for robot(s). This function is for obstacles
+						double d2 = (*iit)->get_current_pointV().distance2( iit2 );
+
+						if( d2 < min_dist2)//square distance 5^2
+						{
+							//std::cout<<"(simulated persons) obstacle.x="<<iit2.x <<"; obstacle.y="<<iit2.y << std::endl;
+							//std::cout<<"(simulated persons) person.x="<<virtual_current_point->x<<"; person.y="<<virtual_current_point->y << std::endl;
+							//std::cout << " 1 get_sfm_int_params= (*iit)->id = "<<(*iit)->get_id()<<"; d2 < min_dist2 => d2="<<d2 <<"; min_dist2="<< min_dist2<< std::endl;
+							//const std::vector<double>* params_robot=get_sfm_int_params(robot_);
+							std::vector<double> params_robot2;//=*params_robot;
+							params_robot2.push_back(2.3);  // esfm_k=2.3  valor anterior, para obstaculo central en medio de la trajectoria.
+							params_robot2.push_back(0.05); // esfm_to_robot_lambda=0.05 valor anterior, para obstaculo central en medio de la trajectoria.
+							params_robot2.push_back(3.05);	// esfm_to_robot_A=4.05 valor anterior, para obstaculo central en medio de la trajectoria.
+							params_robot2.push_back(0.51); // esfm_to_robot_B=0.81 valor anterior, para obstaculo central en medio de la trajectoria.
+							params_robot2.push_back(0.2); 	// esfm_d=0.2 valor anterior, para obstaculo central en medio de la trajectoria.
+							set_sfm_to_robot( params_robot2 );
+							virtual_force_obstacle_laser_fake += person->force_sphe( iit2 , &params_robot2, virtual_current_point); // usamos los parametros de la fuerza entre robot y obstaculos, porque los parametros de las personas hacen que le influyan demasiado los obstaculos y se paren, relenticen, no pasen entre ellos, etc.
+							//std::cout<<"(simulated persons) virtual_force_obstacle_laser_fake.fx ="<<virtual_force_obstacle_laser_fake.fx <<"; virtual_force_obstacle_laser_fake.fy ="<<virtual_force_obstacle_laser_fake.fy << std::endl;
+							if(((*iit)->get_id()==1)&&(debug_person_obst_forces_)){
+
+								/*Sforce act_final=person->force_sphe( iit2 , &params_robot2, virtual_current_point);
+								if(debug_real_test_companion_){
+									std::cout<<"(SIMULATION simulated persons, force for this obstacle)  params_robot->size()="<<params_robot->size()<< std::endl;
+									std::cout<<" [parametros actuales ]params_robot->at(0)="<<params_robot->at(0)<<"; params_robot->at(1)="<<params_robot->at(1)<<"; params_robot->at(2)="<<params_robot->at(2)<<"; params_robot->at(3)="<<params_robot->at(3)<<"; params_robot->at(4)="<<params_robot->at(4)<< std::endl;
+									std::cout<<" [parametros actuales ]params_robot2.at(0)="<<params_robot2.at(0)<<"; params_robot2.at(1)="<<params_robot2.at(1)<<"; params_robot2.at(2)="<<params_robot2.at(2)<<"; params_robot2.at(3)="<<params_robot2.at(3)<<"; params_robot2.>at(4)="<<params_robot2.at(4)<< std::endl;
+									std::cout<<"(simulated persons, force for this obstacle)  act_final.fx="<<act_final.fx<<"; act_final.fy="<<act_final.fy<< std::endl;
+								}*/
+
+							}
+						}
+					}
+					//std::cout << " 5 update "<< std::endl;
+					//std::cout << "20" << std::endl;
+					virtual_force_obstacle+=virtual_force_obstacle_laser_fake;
+					//if((*iit)->get_id()==id_person_companion_){
+					//	std::cout<<"(simulated persons, final force)  virtual_force_obstacle.fx="<<virtual_force_obstacle.fx<<"; virtual_force_obstacle.fy="<<virtual_force_obstacle.fy<< std::endl;
+					//}
+					// [ely] FIN parte para evitar obstaculos del fake laser.
+//std::cout << "[7.2]" << std::endl;
+					// calculate the robot-person force specifically, if any robot in the scene
+				//	std::cout << " 4 INI update "<< std::endl;
+					if ( robot_in_the_scene_ )
+					{
+						/* INI la fuerza robot hacia person_companion, cambia, solo es repulsiva cuando el robot esta muy cerca */
+						double robot_person_distance=calc_robot_person_companion_distance_sim();
+						if((((*iit)->get_id()==id_person_companion_)) && (robot_person_distance<(robot_person_proximity_distance_sim_-robot_person_proximity_tolerance_sim_))){
+							//std::cout << " 2 get_sfm_int_params" << std::endl;
+							/*const std::vector<double>* params_robot=this->get_sfm_int_params(*iit, robot_);
+							if(debug_real_test_companion_){
+								std::cout<<"params_robot(0)="<<params_robot->at(0)<< std::endl;
+								std::cout<<"params_robot(1)="<<params_robot->at(1)<< std::endl;
+								std::cout<<"params_robot(2)="<<params_robot->at(2)<< std::endl;
+								std::cout<<"params_robot(3)="<<params_robot->at(3)<< std::endl;
+								std::cout<<"params_robot(4)="<<params_robot->at(4)<< std::endl;
+							}*/
+
+							std::vector<double> params_robot2;//=*params_robot;
+							params_robot2.push_back(4.5);  // [INICIAL era, esfm_k= 4.9. No se de donde lo coge Gonzalo, hay que investigar... ]esfm_k=2.3  valor anterior, para obstaculo central en medio de la trajectoria.
+							params_robot2.push_back(0.25); // [INICIAL era, esfm_to_robot_lambda= 1. No se de donde lo coge Gonzalo, hay que investigar... ] esfm_to_robot_lambda=0.05 valor anterior, para obstaculo central en medio de la trajectoria.
+							params_robot2.push_back(2.05);	// [INICIAL era, esfm_to_robot_A= 10. No se de donde lo coge Gonzalo, hay que investigar... ] esfm_to_robot_A=4.05 valor anterior, para obstaculo central en medio de la trajectoria.
+							params_robot2.push_back(0.5); // [INICIAL era, esfm_to_robot_B= 0.64. No se de donde lo coge Gonzalo, hay que investigar... ] esfm_to_robot_B=0.81 valor anterior, para obstaculo central en medio de la trajectoria.
+							params_robot2.push_back(0.3); 	// [INICIAL era, esfm_d= 0.16. No se de donde lo coge Gonzalo, hay que investigar... ] esfm_d=0.2 valor anterior, para obstaculo central en medio de la trajectoria.
+							set_sfm_to_robot( params_robot2 );
+
+							const SpointV_cov* virtual_current_point_person_p;
+							virtual_current_point_person_p = &((*iit)->get_current_pointV());
+
+							virtual_force_robot =  (*iit)->force( robot_->get_current_pointV() ,
+									&params_robot2 ,  virtual_current_point_person_p) * 1;//TODO change parameters
+							//virtual_force_robot =  (*iit)->force( robot_->get_current_pointV() ,
+							//					this->get_sfm_int_params(*iit, robot_)   ) * 4;//TODO change parameters
+							//std::cout<< std::endl;
+							//std::cout<<"(simulated persons)  id_person_companion_="<<id_person_companion_<< std::endl;
+							//std::cout<<"(simulated persons)  virtual_force_robot.fx="<<virtual_force_robot.fx<<"; virtual_force_robot.fy"<<virtual_force_robot.fy<< std::endl;
+							//std::cout<< std::endl;
+							virtual_force_robot = Sforce();
+							//std::cout<<"(simulated persons)  robot in case person companion"<< std::endl;
+						}/*else if((*iit)->get_id()==id_person_goal_){
+							virtual_force_robot = Sforce();
+						}*/
+						else if((*iit)->get_id()!=id_person_companion_){ //&&((*iit)->get_id()!=id_person_goal_)
+							//std::cout<<"(simulated persons)  robot in case other persons"<< std::endl;
+							//virtual_force_robot =  (*iit)->force( robot_->get_current_pointV() ,
+							//this->get_sfm_int_params(*iit, robot_)   ) * 4;//TODO change parameters
+							//virtual_force_robot = Sforce();  // ya la persona id=2, person_goal no tiene en cuenta el robot, era para que se viera bien lo de mi codigo.
+							//virtual_force_robot =  (*iit)->force( robot_->get_current_pointV() ,
+							//this->get_sfm_int_params(*iit, robot_)   ) * 1;//TODO change parameters
+
+							//std::cout << " rando people insert 5 get_sfm_int_params" << std::endl;
+
+							const SpointV_cov* virtual_current_point_person_p;
+							virtual_current_point_person_p = &((*iit)->get_current_pointV());
+
+							virtual_force_robot =  (*iit)->force( robot_->get_current_pointV() ,
+												this->get_sfm_int_params(*iit, robot_) , virtual_current_point_person_p ) * multiply_robot_force_to_persons_;//TODO change parameters
+							virtual_force_robot =virtual_force_robot + (*iit)->force( person_companion_->get_current_pointV() ,
+													this->get_sfm_int_params(*iit, robot_) ,virtual_current_point_person_p  ) * multiply_robot_force_to_persons_;//TODO change parameters
+
+
+							if(debug_approach_people_random_){
+							std::cout <<" id="<<(*iit)->get_id()<< " virtual_force_robot.fx="<<virtual_force_robot.fx<<"; virtual_force_robot.fy="<<virtual_force_robot.fy << std::endl;
+							Sforce total_f= virtual_force_robot+virtual_force_goal+virtual_force_int_person;
+													std::cout << "total_force:"<< std::endl;
+													total_f.print();
+							(*iit)->get_current_pointV().print();
+							}
+
+
+						}
+						else{ // no aplica fuerza para esta persona.
+							//virtual_force_robot = Sforce();
+							const SpointV_cov* virtual_current_point_person_p;
+							virtual_current_point_person_p = &((*iit)->get_current_pointV());
+
+							virtual_force_robot =  (*iit)->force( robot_->get_current_pointV() ,
+											this->get_sfm_int_params(*iit, robot_) ,  virtual_current_point_person_p ) * multiply_robot_force_to_persons_;//TODO change parameters
+							virtual_force_robot =virtual_force_robot + (*iit)->force( person_companion_->get_current_pointV() ,
+											this->get_sfm_int_params(*iit, robot_) ,  virtual_current_point_person_p ) * multiply_robot_force_to_persons_;//TODO change parameters
+
+							if(debug_approach_people_random_){
+							//std::cout <<" id="<<(*iit)->get_id()<< " virtual_force_robot.fx="<<virtual_force_robot.fx<<"; virtual_force_robot.fy="<<virtual_force_robot.fy << std::endl;
+							//Sforce total_f= virtual_force_robot+virtual_force_goal+virtual_force_int_person;
+							//std::cout << "total_force:"<< std::endl;
+							//total_f.print();
+							//(*iit)->get_current_pointV().print();
+
+							}
+							//std::cout<<"(simulated persons)  otros"<< std::endl;
+						}
+						/* FIN la fuerza robot hacia person_companion,, cambia, solo es repulsiva cuando el robot esta muy cerca */
+					}
+					else{
+						virtual_force_robot = Sforce();
+						if(debug_approach_people_random_){
+						std::cout <<"(else) id="<<(*iit)->get_id()<< " (force 0, else) virtual_force_robot.fx="<<virtual_force_robot.fx<<"; virtual_force_robot.fy="<<virtual_force_robot.fy << std::endl;
+						}
+					}
+				//	std::cout << " 6 update "<< std::endl;
+
+					//to avoid entering obstacles present in the scene, it simply does not propagate (solo evita obstaculos de mapa, no del fake laser.
+					w = 1.0;
+					do
+					{
+						(*iit)->set_forces_person( virtual_force_goal*w, virtual_force_int_person*w, virtual_force_robot*w, virtual_force_obstacle );
+						virtual_next_pose = (*iit)->pointV_propagation( dt_ , (*iit)->get_force_person()  );
+						w *= 0.9;
+
+					} while( read_force_map_success_ && !is_cell_clear_map( virtual_next_pose.x, virtual_next_pose.y ) && w > 0.1 );
+					virtual_next_pose.time_stamp = now;
+
+
+					/*std::cout << " get_force_int_robot_person():" << std::endl;
+							(*iit)->get_force_int_robot_person().print();
+							std::cout << " print all dest person:" << std::endl;*/
+					//double all_destpe=(*iit)->get_destinations()->size();
+					/*	for(unsigned int g=0;g<all_destpe;g++){
+							(*iit)->get_destinations()->at(g).print();
+					}
+					std::cout << " print best dest person:" << std::endl;
+					(*iit)->get_best_dest().print();*/
+
+					//std::cout << " 7 update "<< std::endl;
+					//(*iit)->add_pointV( virtual_next_pose );  //new poses for the other persons different of the companion_person.
+					/*if(group_simulation_){
+						if(other_companion_person_id_==(*iit)->get_id()){
+							(*iit)->add_pointV( other_companion_person_out_odometry_spoint_ );
+							// cambio la posicion de esta person de la lista internamente.
+							//std::cout << " ??????? other_companion_person_out_odometry_spoint_.x="<<other_companion_person_out_odometry_spoint_.x<<"; .y="<<other_companion_person_out_odometry_spoint_.y << std::endl;
+							new_points_person_sim_.push_back(virtual_next_pose);
+							//std::cout << "(3) antes id"<< std::endl;
+							persons_ids_.push_back((*iit)->get_id());
+
+						}else if(id_second_companion_==(*iit)->get_id()){
+							double dx=1.0;
+							double dy=0.5; // TODO: guardar la posicion de la person companion actual para poder ponerla aqui, y no poner la inicial.
+							double theta=atan(initial_companion_person_robot_point_.vy/initial_companion_person_robot_point_.vx);
+							(*iit)->add_pointV(SpointV_cov(initial_companion_person_robot_point_.x+cos(theta)*dx,initial_companion_person_robot_point_.y+cos(theta)*dy, now, initial_companion_person_robot_point_.vx,initial_companion_person_robot_point_.vy),  filtering_method_); //TODO: add second_person_Companion
+						}else if(id_person_companion_==(*iit)->get_id()){
+							// no insertes persona en la person list interna!!!
+						}else{
+							(*iit)->add_pointV( virtual_next_pose );
+							new_points_person_sim_.push_back(virtual_next_pose);
+							//std::cout << "(3) antes id"<< std::endl;
+							persons_ids_.push_back((*iit)->get_id());
+
+						}
+					}else{*/
+						(*iit)->add_pointV( virtual_next_pose );
+						new_points_person_sim_.push_back(virtual_next_pose);
+						//std::cout << "(3) antes id"<< std::endl;
+						persons_ids_.push_back((*iit)->get_id());
+					//}
+
+					if(debug_other_persons_scene_sim_){
+						std::cout << "virtual_next_pose_person; id="<<(*iit)->get_id()<< std::endl;
+						virtual_next_pose.print();
+					}
+
+				}else{
+
+				}
+			}
+			if(debug_general_scene_sim_){
+				std::cout << "fin bucle person list"<< std::endl;
+			}
+
+		}
+
+
+		//Cperson_abstract* person_obj;
+		//bool finded_other_person_companion=find_person(other_companion_person_id_ , &person_obj);
+
+		//std::cout << "  33333 finded_other_person_companion="<<finded_other_person_companion<<"; other_companion_person_id_="<<other_companion_person_id_<< std::endl;
+
+		/*if(!finded_other_person_companion){
+
+			Cperson_abstract* new_person;
+
+			//std::cout << "  33333 NEW people from otherperson comp 10="<<other_companion_person_id_<< std::endl;
+			new_person = add_person(other_companion_person_id_);//(obs_other_person_companion_[ty].id);
+			//std::cout << "  33333 NEW people from otherperson comp 11"<< std::endl;
+			new_person->set_destinations(destinations2_);
+			//std::cout << "  33333 NEW people from otherperson comp 12"<< std::endl;
+			dest_person4_5_=destinations2_;
+			if(debug_init_scene_sim_){
+				//std::cout << "(person 4 y 5) new_person->get_destinations()->at( 1 ).print():" << std::endl;
+				new_person->get_destinations()->at( 1 ).print();
+				//std::cout << " Destinations person 4 y 5:" << std::endl;
+				for(unsigned int dest=0; dest<dest_person4_5_.size(); dest++){
+					dest_person4_5_[dest].print();
+				}
+			}
+
+			//new_person->set_destinations(destinations2p3_);
+
+			//std::cout << "16; (person id) x="<<x<<"; y="<<y << std::endl;
+			double act_group_velocity=sqrt(other_companion_person_out_odometry_spoint_.vx*other_companion_person_out_odometry_spoint_.vx+other_companion_person_out_odometry_spoint_.vy*other_companion_person_out_odometry_spoint_.vy);
+			//std::cout << "17 add other person companion tracks; " << std::endl;
+			new_person->set_desired_velocty( act_group_velocity );
+			//std::cout << "18 add other person companion tracks; obs_other_person_companion_.size()="<<obs_other_person_companion_.size()<<"; number_virtual_people_="<<number_virtual_people_ << std::endl;
+			new_person->set_best_dest(new_person->get_destinations()->at( 1 ));//new_person->get_destinations()->at( 1 ) ); // insertar tambien actual best dest.
+			//std::cout << "19 add other person companion tracks; obs_other_person_companion_[ty].id="<<obs_other_person_companion_[ty].id << std::endl;
+			person_best_dest_.resize(number_virtual_people_+obs_other_person_companion_.size()+1);
+			//std::cout << "20 add other person companion tracks; person_best_dest_.size="<<person_best_dest_.size()<<"; actual_id_in_person_list="<<actual_id_in_person_list << std::endl;
+			person_best_dest_[other_companion_person_id_]=new_person->get_destinations()->at( 1 );//new_person->get_destinations()->at( 1 );
+
+			//[obs_other_person_companion_[ty].id]=new_person->get_destinations()->at( 1 );
+			//ant_person_dest_=person_best_dest_;
+			//std::cout << "22 add other person companion tracks; " << std::endl;
+			double theta= atan2(new_person->get_destinations()->at( 1 ).y-other_companion_person_out_odometry_spoint_.y,new_person->get_destinations()->at( 1 ).x-other_companion_person_out_odometry_spoint_.x);
+			//std::cout << "23 add other person companion tracks; " << std::endl;
+			new_person->add_pointV( other_companion_person_out_odometry_spoint_,  filtering_method_ );
+			//std::cout << "CURREN POINT OTHER PERSON COMP; " << std::endl;
+			//new_person->get_current_pointV().print();
+			new_points_person_sim_.push_back(other_companion_person_out_odometry_spoint_);
+			//std::cout << "25 add other person companion tracks; " << std::endl;
+			//new_person->add_pointV(other_companion_person_out_odometry_spoint_,  filtering_method_ );
+			//new_points_person_sim_.push_back(other_companion_person_out_odometry_spoint_);
+			persons_ids_.push_back(other_companion_person_id_);//(obs_other_person_companion_[ty].id);
+	}*/
+
+		/*std::cout <<  "10 (Cscene_sim) ( else case UPDATE) check people nan's " << std::endl;
+								for( auto iit : person_list_)
+								{
+									if(iit->get_id()!=id_person_companion_){
+
+									 std::cout << " (after Insert people) (1) iit->get_prediction_trajectory( )->size()="<<iit->get_prediction_trajectory( )->size()<<"; iit->get_id()=" <<iit->get_id()<<"; iit->desired_vel="<<iit->get_desired_velocity()<<  std::endl;
+									 iit->get_current_pointV().print();
+									}
+								}*/
+		//std::cout << " 8 update "<< std::endl;
+
+//std::cout << "[7.4]" << std::endl;
+	/*** Update person comanion position   ***/
+		//companion_person_id_=1;
+		companion_person_id_=id_person_companion_;
+		if(debug_companion_person_scene_sim_){
+			std::cout<< std::endl<<std::endl<< " (!!!) Ini case person_companion!!!"<< std::endl;
+			//std::cout << "if((*iit)->get_id()!=1); (*iit)->get_id()= "<< (*iit)->get_id()<< "; person_list_.size()="<<person_list_.size()<< std::endl;
+			std::cout << "companion_person_id_= "<< companion_person_id_<< std::endl;
+		}
+			Sdestination  dest_1 = get_destinations()->at( 8 );
+			Sdestination  dest_2 = get_destinations()->at( 9 );
+			Sdestination  dest_10 = get_destinations()->at( 10 );
+			Sdestination  dest_11 = get_destinations()->at( 11 );
+			Sdestination  dest_12;
+			Sdestination  dest_13;
+			Sdestination  dest_14;
+			//if(caso_simulacion_giro_){
+				dest_12 = get_destinations()->at( 12 );
+				dest_13 = get_destinations()->at( 13 );
+				dest_14 = get_destinations()->at( 14 );
+			//}
+			destinations1_.clear();
+			destinations1_.push_back(dest_1);
+			destinations1_.push_back(dest_2);
+			destinations1_.push_back(dest_10);
+			destinations1_.push_back(dest_11);
+			//if(caso_simulacion_giro_){
+				destinations1_.push_back(dest_12);
+				destinations1_.push_back(dest_13);
+				destinations1_.push_back(dest_14);
+			//}
+			//(*iit)->set_destinations(destinations1_);
+			//(*iit)->set_desired_velocty( less_companion_velocity );
+			for(iit = person_list_.begin() ; iit!=person_list_.end() ; iit++)
+			{
+				if((*iit)->get_id()==1){
+				//	akp_for_person_companion_.set_robot_goal_person_companion_akp(person_actual_best_dest_);
+				}
+			}
+
+
+			if(state_act_==Cplan_local_nav_person_companion::START){
+				std::vector<int> neighbours_ids_act;
+				std::vector<Sdestination> goal_person_dest2;
+				neighbours_ids_act.push_back(1);
+				//neighbours_ids_act.push_back(id_person_companion_);
+				Sdestination goal_person_dest(id_person_companion_, actual_goal_to_return_person_comp_.x , actual_goal_to_return_person_comp_.y , 1 ,Sdestination::Map_goal, neighbours_ids_act);
+				goal_person_dest2.clear();
+				goal_person_dest2.push_back(goal_person_dest);
+				akp_for_person_companion_.set_robot_destinations(goal_person_dest2); // TODO: cambia
+				if(group_simulation_){
+					//akp_for_person_companion_2_group_person_.set_robot_destinations(goal_person_dest2);
+				}
+				actual_goal_to_return_person_comp_=actual_goal_to_return_person_comp1_;
+			}else if(state_act_==Cplan_local_nav_person_companion::ITER){
+				//std::cout << "(3) SET ALL DEST destinations1_.size() "<< destinations1_.size() << std::endl;
+				akp_for_person_companion_.set_robot_destinations(destinations1_); // TODO: cambia
+				//if(group_simulation_){
+					//akp_for_person_companion_2_group_person_.set_robot_destinations(destinations1_);
+				//}
+
+			}
+
+			//akp_for_person_companion_.set_id_person_companion(1);
+			//akp_for_person_companion_.set_id_person_companion_Cprediction_bhmip(1);
+			akp_for_person_companion_.set_id_person_companion(id_person_companion_);
+			akp_for_person_companion_.set_id_person_companion_Cprediction_bhmip(id_person_companion_);
+
+			//if(group_simulation_){
+				//akp_for_person_companion_2_group_person_.set_id_person_companion(id_person_companion_2group_pers_);
+				//akp_for_person_companion_2_group_person_.set_id_person_companion_Cprediction_bhmip(id_person_companion_2group_pers_);
+
+			//}
+
+
+			//akp_for_person_companion_.set_robot_goal_person_companion_akp(new_person->get_destinations()->at( 1 ));
+			//akp_for_person_companion_.set_robot_goal_person_companion_akp(person_actual_best_dest_);
+			//std::cout << " 10 update "<< std::endl;
+			if(state_act_==Cplan_local_nav_person_companion::START){
+				std::vector<int> neighbours_ids_act;
+				neighbours_ids_act.push_back(1);
+				//neighbours_ids_act.push_back(id_person_companion_);
+				Sdestination goal_person_dest(id_person_companion_, actual_goal_to_return_person_comp_.x , actual_goal_to_return_person_comp_.y , 1 ,Sdestination::Map_goal, neighbours_ids_act);
+				//std::vector<Sdestination> goal_person_dest2;
+				//goal_person_dest2.push_back(goal_person_dest);
+
+				//std::cout << " 3333333333333 before START CASE NOT GO TO INTERACT WITH OTHER PERSON, change person_companion_dest" << std::endl;
+				//person_actual_best_dest_.print();
+
+				akp_for_person_companion_.get_person_companion_akp()->set_best_dest(goal_person_dest);  // todo: cambia
+
+				//std::cout << " 3333333333333 after START CASE NOT GO TO INTERACT WITH OTHER PERSON, change person_companion_dest" << std::endl;
+				//person_actual_best_dest_.print();
+
+				//if(group_simulation_){
+					//akp_for_person_companion_2_group_person_.get_person_companion_akp()->set_best_dest(goal_person_dest);
+				//}
+
+				for(iit = person_list_.begin() ; iit!=person_list_.end() ; iit++)
+				{
+					if((*iit)->get_id()==1){
+						(*iit)->set_best_dest(goal_person_dest);
+						//std::cout << " [7] [Enteoria bien si entra qui, vuelce al inicial!] (*iit)->id="<<(*iit)->get_id()<<"; actual_goal_to_return_person_comp_.x="<<actual_goal_to_return_person_comp_.x<<"; actual_goal_to_return_person_comp_.y="<<actual_goal_to_return_person_comp_.y<< std::endl;
+					}
+				}
+				actual_goal_to_return_person_comp_=actual_goal_to_return_person_comp1_;
+			}else if(state_act_==Cplan_local_nav_person_companion::ITER){
+				//std::cout << " 3333333333333 before ITER CASE NOT GO TO INTERACT WITH OTHER PERSON, change person_companion_dest" << std::endl;
+				//person_actual_best_dest_.print();
+
+				akp_for_person_companion_.get_person_companion_akp()->set_best_dest(person_actual_best_dest_);  // todo: cambia
+
+				//std::cout << "  3333333333333 after ITER CASE NOT GO TO INTERACT WITH OTHER PERSON, change person_companion_dest" << std::endl;
+				//person_actual_best_dest_.print();
+
+				if(group_simulation_){
+					//akp_for_person_companion_2_group_person_.get_person_companion_akp()->set_best_dest(person_actual_best_dest_);
+				}
+
+				for(iit = person_list_.begin() ; iit!=person_list_.end() ; iit++)
+				{
+					if((*iit)->get_id()==1){
+						(*iit)->set_best_dest(person_actual_best_dest_);
+						//std::cout << " [7] [Enteoria bien si entra qui, vuelce al inicial!] (*iit)->id="<<(*iit)->get_id()<<"; person_actual_best_dest_.x="<<person_actual_best_dest_.x<<"; person_actual_best_dest_.y="<<person_actual_best_dest_.y<< std::endl;
+
+					}
+				}
+			}
+
+			//std::cout << " 11 update "<< std::endl;
+
+			//akp_for_person_companion_.set_id_person_companion(1);
+			//akp_for_person_companion_.set_id_person_companion_Cprediction_bhmip(1);
+
+			//std::cout << "antes Cscene_sim-> update_companion_person: person_list_.empty()="<<person_list_.empty()<< std::endl;
+			//akp_for_person_companion_.set_person_list(person_list_);
+			//std::cout << " (print interior person_list) antes Cscene_sim-> update_companion_person: akp_for_person_companion_.get_scene()->empty()="<<akp_for_person_companion_.get_scene()->empty()<< std::endl;
+			//akp_for_person_companion_.print();
+			if(debug_companion_person_scene_sim_){
+				std::cout << " antes de akp_for_person_companion_.person_companion_plan_companion(new_person_companion_pos " << std::endl;
+				//std::cout << " antes de akp_for_person_companion_.robot_plan_companion3_person_companion(new_person_companion_pos " << std::endl;
+			}
+
+			//std::cout << " 7777777777 NEW DEST PERSON COMPANION!" << std::endl;
+			//akp_for_person_companion_.get_person_companion_akp()->get_best_dest().print();
+
+		//	std::cout << "[7.4.1]" << std::endl;
+			const std::list<Cperson_abstract *>* person_list = akp_for_person_companion_.get_scene( );
+			//std::cout << " 12 update "<< std::endl;
+			std::vector<SdetectionObservation> obs;
+			if(debug_companion_person_scene_sim_){
+				for( std::list<Cperson_abstract*>::const_iterator iit = person_list->begin(); iit!=person_list->end(); iit++ )
+				{
+					std::cout << "(*iit)->get_id="<<(*iit)->get_id() <<"(*iit)->get_prediction_trajectory()->size()= "<< (*iit)->get_prediction_trajectory()->size() << std::endl;
+				}
+			}
+
+			//if(debug_approach_people_random_){
+			//	std::cout << " 13 update "<< std::endl;
+			//}
+
+			Spose before_new_person_companion_pose=akp_for_person_companion_.get_person_companion_akp()->get_current_pose();
+			//Spose before_new_person_companion_pose2;
+			//if(group_simulation_){
+				//before_new_person_companion_pose2=akp_for_person_companion_2_group_person_.get_person_companion_akp()->get_current_pose();
+			//}
+
+			//if(debug_approach_people_random_){
+			//	std::cout << " 14 update "<< std::endl;
+			//}
+			before_new_person_companion_pose.time_stamp=now_;
+			Spose new_person_companion_pose=akp_for_person_companion_.get_person_companion_akp()->get_current_pose();
+			//Spose new_person_companion_pose2;
+			//if(group_simulation_){
+				//new_person_companion_pose2=akp_for_person_companion_2_group_person_.get_person_companion_akp()->get_current_pose();
+			//}
+
+
+			//if(debug_approach_people_random_){
+			//	std::cout << " 15 update "<< std::endl;
+			//}
+
+
+			new_points_person_sim_.push_back(SpointV_cov(new_person_companion_pose.x , new_person_companion_pose.y, new_person_companion_pose.time_stamp, new_person_companion_pose.v*cos(new_person_companion_pose.theta), new_person_companion_pose.v*sin(new_person_companion_pose.theta)));
+			persons_ids_.push_back(id_person_companion_);
+
+			//if(group_simulation_){
+				//new_points_person_sim_.push_back(SpointV_cov(new_person_companion_pose2.x , new_person_companion_pose2.y, new_person_companion_pose2.time_stamp, new_person_companion_pose2.v*cos(new_person_companion_pose2.theta), new_person_companion_pose2.v*sin(new_person_companion_pose2.theta)));
+				//persons_ids_.push_back(id_person_companion_2group_pers_);
+
+			//}
+
+			/*
+			 * new_points_person_sim_.push_back(SpointV_cov(new_person_companion_pose2.x , new_person_companion_pose2.y, new_person_companion_pose2.time_stamp, new_person_companion_pose2.v*cos(new_person_companion_pose2.theta), new_person_companion_pose2.v*sin(new_person_companion_pose2.theta)));
+				persons_ids_.push_back(id_person_companion_2group_pers_);
+			 *
+			 */
+
+			//if(debug_approach_people_random_){
+			//	std::cout << " 16 update "<< std::endl;
+			//}
+
+			//std::cout << " 3 [7.4.1]" << std::endl;
+
+			if(debug_companion_person_scene_sim_){
+			std::cout << "(1)[current robot pose] new_person_companion_pose.x"<<new_person_companion_pose.x<<"; new_person_companion_pose.y"<<new_person_companion_pose.y<<"new_person_companion_pose.v="<<new_person_companion_pose.v<<"new_person_companion_pose.w"<<new_person_companion_pose.w<< std::endl;
+			//std::cout << "(1)[current robot pose] new_person_companion_pose2.x"<<new_person_companion_pose2.x<<"; new_person_companion_pose2.y"<<new_person_companion_pose2.y<<"new_person_companion_pose2.v="<<new_person_companion_pose2.v<<"new_person_companion_pose2.w"<<new_person_companion_pose.w<< std::endl;
+
+			}
+
+			Cperson_abstract::companion_reactive reactive_=Cperson_abstract::Akp_planning;
+			if(debug_companion_person_scene_sim_){
+				std::cout << "(2)"<< std::endl;
+			}
+
+			//std::vector<SdetectionObservation> obs;
+			//unsigned int id_pers=0;
+			//std::cout << "std::vector<SdetectionObservation> obs:"<< std::endl;
+			//for( iit = person_list_.begin() ; iit!=person_list_.end() ; iit++)
+			//{
+
+			//	//SdetectionObservation( int id_ , double time_stamp_=0.0, double x_=0.0, double y_=0.0, double vx_=0.0, double vy_=0.0,const std::vector<double>& cov_= std::vector<double>());
+			//	//SdetectionObservation( int id_ , SpointV_cov pointV_cov_);
+			//	obs.push_back(SdetectionObservation((*iit)->get_id(),(*iit)->get_current_pointV()));
+				//obs[id_pers].print();
+			//	obs.back().print();
+				//id_pers++;
+			//}
+			//bool have_companion_person=true;
+			//akp_for_person_companion_.update_scene(obs,have_companion_person);
+			if(debug_companion_person_scene_sim_){
+				std::cout << "(2) person_best_dest_"<< std::endl;
+				for(unsigned int u=0;u<person_best_dest_.size(); u++){
+					std::cout << "person_id="<<u+1<< std::endl;
+					person_best_dest_[u].print();
+				}
+			}
+			//std::cout << "[7.4.2]" << std::endl;
+			//if(debug_approach_people_random_){
+			//std::cout << " 17 antes akp_for_person_companion_2_group_person_.person_companion_plan_companion ; person_list_.size()="<< person_list_.size() << std::endl;
+			//std::cout << " 17 akp_for_person_companion_.get_person_list().size()= "<<akp_for_person_companion_.get_person_list().size()<< std::endl;
+
+			//}
+			//akp_for_person_companion_.set_person_list(person_list_);
+			//if(group_simulation_){
+				//akp_for_person_companion_2_group_person_.set_person_list(person_list_);
+			//}
+
+			//std::cout <<"; akp_for_person_companion_.get_id_person_companion()="<<akp_for_person_companion_.get_id_person_companion() << std::endl;
+
+
+			/*std::cout << " before person_companion_plan_companion! IMPORTANTE!!! Despues UPDATE_SCENE (person list clase Cscene_sim) " << std::endl;
+			for( iit = person_list_.begin() ; iit!=person_list_.end() ; iit++)
+			{
+				std::cout << "(*iit)->get_id()= "<< (*iit)->get_id() <<"(*iit)->get_prediction_trajectory()->size()= "<< (*iit)->get_prediction_trajectory()->size() << std::endl;
+			}
+
+			for(unsigned int y=0; y<persons_ids_.size();y++){
+				std::cout << " before person_companion_plan_companion! IMPORTANTE!!! persons_ids_["<<y<<"]="<<persons_ids_[y] << std::endl;
+			}*/
+
+			//std::cout << " 7878787 antes person_companion_plan_companion! IMPORTANTE!!! " << std::endl;
+
+			person_companion_plan_succed_ =  akp_for_person_companion_.person_companion_plan_companion(new_person_companion_pose,reactive_,0.2,&person_best_dest_);
+
+			//std::cout << " 7878787 despues person_companion_plan_companion! IMPORTANTE!!! " << std::endl;
+
+			Spose_person_companion2_=new_person_companion_pose;
+			double dx_p1=1.0;
+			double dy_p1=0.5;
+			double theta_p1=atan((new_person_companion_pose.y-Spose_before_person_companion_.y)/(new_person_companion_pose.x-Spose_before_person_companion_.x));
+			Spose_person_companion2_.x=Spose_person_companion2_.x+cos(theta_p1)*dx_p1;
+			Spose_person_companion2_.y=Spose_person_companion2_.y+sin(theta_p1)*dy_p1;
+
+			Spose_before_person_companion_=new_person_companion_pose;
+
+
+			// double obstacle_dist_act=100;
+			/*if(Scene_create_fake_fixed_second_person_companion_){
+				//std::cout << " IN Scene_create_fake_fixed_second_person_companion_:"<< std::endl;
+
+
+				double vx=new_person_companion_pose.v*cos(new_person_companion_pose.theta);
+				double vy=new_person_companion_pose.v*sin(new_person_companion_pose.theta);
+				SpointV_cov fake_second_person_companion=SpointV_cov(new_person_companion_pose.x,new_person_companion_pose.y,new_person_companion_pose.time_stamp,vx,vy);
+
+				// IF case central robot:
+				//double distance_between_comp_person_x=2*0.5128; // CASO 2
+				//double distance_between_comp_person_y=2*0.5128; // CASO 2
+				//double angulo=-90;
+
+				// double distance_between_comp_person_x=2*1.3; // CASO 1 1.4096 (CASO BUENO-CENTRAL V-form)
+				// double distance_between_comp_person_y=2*1.3; // CASO 1
+				// double angulo=-90;
+
+				 double distance_between_comp_person_x=2*1.6; // CASO 1 1.4096 (CASO BUENO-CENTRAL Side-by-side)La del side-by-side-Normal=1.6   (hacer test con 1.3 también)
+				 double distance_between_comp_person_y=2*1.5; // CASO 1 y side-by-side-Normal=1.5 (hacer test con 1.3 también. Lo aguanta y puede pasar que la gente lo haga.)
+				//double distance_between_comp_person_x=1.6; //caso lateral todo el rato.
+				//double distance_between_comp_person_y=1.5;
+				double angulo=-82;  // entre -90 y -80 para que vaya en side-by-side real. (-82 para side-by-side puro)
+
+				// double distance_between_comp_person_x=1.33; // CASO 3 (caso 2, version diagonal, derecha. (+90 grados)1.4096
+				// double distance_between_comp_person_y=0.45; // CASO 3 0.5128
+				//double angulo=90;
+
+				 //(CASO BUENO LATERAL- debajo)
+				// double distance_between_comp_person_x=1.3; // CASO 4 (caso 1, version diagonal, derecha. (+90 grados); 0.5128
+				// double distance_between_comp_person_y=1.3; // CASO 4 ; 1.4096
+				// double angulo=70; //(CASO BUENO LATERAL)
+
+				 /// TODO: fake obstacle simulation: (Poner a segunda persona detras y no en el lateral.
+
+				 if(p2_back_p1_with_static_obst_){
+					 for( Spoint iit : laser_obstacle_list_)
+					 {
+
+						double d2 = iit.distance( fake_second_person_companion );
+						double d1= new_person_companion_pose.distance( iit );
+						if(d1<d_min_){
+							d_min_=d1;
+						}
+
+						obstacle_dist_act=d2;
+						if(d2<5.0){
+
+							angulo=((45)*d2-(315));
+							//angulo=-(180-(45/2)*d2+(135/2)); // Valido para de 7.0 a 3.0
+							if(angulo<(-170)){
+								angulo=-170;
+							}
+						}
+					 }
+				 }
+
+
+
+
+				 SpointV_cov sec_copy_fake_second_person_companion;
+				 sec_copy_fake_second_person_companion=fake_second_person_companion;
+
+
+				fake_second_person_companion.x+=distance_between_comp_person_x*cos(new_person_companion_pose.theta+angulo*3.14/180);
+				fake_second_person_companion.y+=distance_between_comp_person_y*sin(new_person_companion_pose.theta+angulo*3.14/180);
+				//std::cout << " new_person_companion_pose.x="<<new_person_companion_pose.x<<"; new_person_companion_pose.y="<<new_person_companion_pose.y<< std::endl;
+				//std::cout << " new_person_companion_pose.theta="<<new_person_companion_pose.theta<<"; new_person_companion_pose.theta+90*180/3.14="<<new_person_companion_pose.theta+90*3.14/180<< std::endl;
+
+				//std::cout <<"; id_fake2_pers_comp="<<id_second_companion_ <<" fake_second_person_companion.x="<<fake_second_person_companion.x<<"; fake_second_person_companion.y="<<fake_second_person_companion.y<< std::endl;
+
+				SpointV_cov_fake_second_person_companion=fake_second_person_companion;
+				for( iit = person_list_.begin() ; iit!=person_list_.end() ; iit++)
+				{
+					if((*iit)->get_id()==id_second_companion_){
+						SpointV_cov ac_point=(*iit)->get_current_pointV();
+						ac_point.x=fake_second_person_companion.x;
+						ac_point.y=fake_second_person_companion.y;
+						ac_point.vx=vx;
+						ac_point.vy=vy;
+						(*iit)->set_current_pointV(ac_point);  // TODO: si es demasiado fuerte esta fuerza repulsiva, anularla.
+					}
+					//std::cout << "(*iit)->get_id()= "<< (*iit)->get_id() <<"(*iit)->get_prediction_trajectory()->size()= "<< (*iit)->get_prediction_trajectory()->size() << std::endl;
+				}
+
+
+				// INI TO move lateral person P2, id=3. near to P1, when the robot decides to go to the side.
+				std::cout << "CsceneSim person companion_; robot_pose.x="<<robot_->get_current_pose().x<<"; robot_pose.y="<<robot_->get_current_pose().y<< std::endl;
+				std::cout << "CsceneSim person companion_; (p1) new_person_companion_pose.x="<<new_person_companion_pose.x<<"; new_person_companion_pose.y="<<new_person_companion_pose.y<< std::endl;
+				std::cout << "CsceneSim person companion_; (p2) SpointV_cov_fake_second_person_companion.x="<<SpointV_cov_fake_second_person_companion.x<<"; SpointV_cov_fake_second_person_companion.y="<<SpointV_cov_fake_second_person_companion.y<< std::endl;
+
+				Spoint center_point=Spoint((robot_->get_current_pose().x+new_person_companion_pose.x+SpointV_cov_fake_second_person_companion.x)/3,(robot_->get_current_pose().y+new_person_companion_pose.y+SpointV_cov_fake_second_person_companion.y)/3);
+
+				double dist_robot_to_group_centre=robot_->get_current_pose().distance(center_point);
+				double dist_p1_to_group_centre=new_person_companion_pose.distance(center_point);
+				double dist_p2_to_group_centre=SpointV_cov_fake_second_person_companion.distance(center_point);
+
+				std::cout << "CsceneSim person companion_;  dist_robot_to_group_centre="<<dist_robot_to_group_centre<<"; dist_p1_to_group_centre="<<dist_p1_to_group_centre<<"; dist_p2_to_group_centre="<<dist_p2_to_group_centre << std::endl;
+				if(dist_robot_to_group_centre>2){ // Si la distancia del robot al centro del grupo es más de 2metros.
+					sec_copy_fake_second_person_companion.x+=(distance_between_comp_person_x/2)*cos(new_person_companion_pose.theta+angulo*3.14/180);
+					sec_copy_fake_second_person_companion.y+=(distance_between_comp_person_y/2)*sin(new_person_companion_pose.theta+angulo*3.14/180);
+					SpointV_cov_fake_second_person_companion=sec_copy_fake_second_person_companion;
+					for( iit = person_list_.begin() ; iit!=person_list_.end() ; iit++)
+					{
+						if((*iit)->get_id()==id_second_companion_){
+							SpointV_cov ac_point=(*iit)->get_current_pointV();
+							ac_point.x=sec_copy_fake_second_person_companion.x;
+							ac_point.y=sec_copy_fake_second_person_companion.y;
+							ac_point.vx=vx;
+							ac_point.vy=vy;
+							(*iit)->set_current_pointV(ac_point);  // TODO: si es demasiado fuerte esta fuerza repulsiva, anularla.
+						}
+										//std::cout << "(*iit)->get_id()= "<< (*iit)->get_id() <<"(*iit)->get_prediction_trajectory()->size()= "<< (*iit)->get_prediction_trajectory()->size() << std::endl;
+					}
+				}
+				// FIN TO move lateral person P2, id=3. near to P1, when the robot decides to go to the side.
+
+			}*/
+
+
+
+
+
+
+			/*std::cout << " before person_companion_plan_companion! IMPORTANTE!!! Despues UPDATE_SCENE (person list clase Cscene_sim) " << std::endl;
+			for( iit = person_list_.begin() ; iit!=person_list_.end() ; iit++)
+			{
+				std::cout << "(*iit)->get_id()= "<< (*iit)->get_id() <<"(*iit)->get_prediction_trajectory()->size()= "<< (*iit)->get_prediction_trajectory()->size() << std::endl;
+			}
+
+			for(unsigned int y=0; y<persons_ids_.size();y++){
+				std::cout << " before person_companion_plan_companion! IMPORTANTE!!! persons_ids_["<<y<<"]="<<persons_ids_[y] << std::endl;
+			}*/
+
+
+			//std::cout <<"; sqrt(new_person_companion_pose.v*new_person_companion_pose.v)="<<sqrt(new_person_companion_pose.v*new_person_companion_pose.v)<<"; new_person_companion_pose.v="<<new_person_companion_pose.v << std::endl;
+			//new_person_companion_pose.print();
+			/*if((sqrt(new_person_companion_pose.v*new_person_companion_pose.v))<0.2){ // para que las personas del simulador no reaccionen a fuerzas que hagan un desplazamiento muy pequeño, evita zigzags al incluir la fuerza hacia otras personas del grupo.
+				new_person_companion_pose.x=before_new_person_companion_pose.x; // el simulador de personas va por posiciones, no por velocidades. Por tanto hay que cambiar la posicion, para que no se mueva.
+				new_person_companion_pose.y=before_new_person_companion_pose.y;
+				new_person_companion_pose.v=0;
+				new_person_companion_pose.w=0;
+			}*/
+			//std::cout<<"(after, limit the out velocity) new_person_companion_pose.v="<<new_person_companion_pose.v << std::endl;
+
+			//std::cout << " 17 (before new group perso)  antes akp_for_person_companion_2_group_person_.person_companion_plan_companion ; person_list_.size()="<< person_list_.size() << std::endl;
+			//if(group_simulation_){
+				//person_companion_plan_succed2_ =  akp_for_person_companion_2_group_person_.person_companion_plan_companion(new_person_companion_pose2,reactive_,0.2,&person_best_dest_);
+			//}
+
+			//std::cout << " 17 despues akp_for_person_companion_2_group_person_.person_companion_plan_companion "<< std::endl;
+
+			//if(debug_approach_people_random_){
+			//std::cout << " 18 update "<< std::endl;
+			//}
+
+			//std::cout << "[7.4.3]" << std::endl;
+			if(debug_companion_person_scene_sim_){
+				std::cout << "(3)"<< std::endl;
+			}
+
+			if(debug_companion_person_scene_sim_){
+				std::cout << "(4)"<< std::endl; // TODO, peta aquí, ver que pasa... y las velocidades son 0. Ver que pasa...
+				std::cout << "person_companion_plan_succed_="<<person_companion_plan_succed_<< std::endl;
+				std::cout << "person_companion_plan_succed2_="<<person_companion_plan_succed2_<< std::endl;
+			}
+			//std::cout << "person_companion_plan_succed_="<<person_companion_plan_succed_<< std::endl;
+			if(!person_companion_plan_succed_){ // si no hay planning valido, quedate en el mismo punto. NO resetees!!!.
+				std::cout << "robot plan not succed! U_U"<< std::endl;
+				new_person_companion_pose=before_new_person_companion_pose;
+			}else{
+				//std::cout << "robot plan succed!"<< std::endl;
+				new_person_companion_pose.time_stamp=now_;
+			}
+			/*if(group_simulation_){
+				if(!person_companion_plan_succed2_){ // si no hay planning valido, quedate en el mismo punto. NO resetees!!!.
+					//std::cout << "person2 plan not succed! U_U"<< std::endl;
+					new_person_companion_pose2=before_new_person_companion_pose2;
+				}else{
+					//std::cout << "robot plan succed!"<< std::endl;
+					new_person_companion_pose2.time_stamp=now_;
+				}
+			}*/
+
+			//=> hay que resolverlo mejor... U_U
+
+			//if(debug_approach_people_random_){
+			//std::cout << " 19 update "<< std::endl;
+			//}
+
+			SpointV_cov new_person_companion_SpointV_cov;
+			//std::cout << "(4.1)"<< std::endl;
+			double vx=new_person_companion_pose.v*cos(new_person_companion_pose.theta);
+			double vy=new_person_companion_pose.v*sin(new_person_companion_pose.theta);
+			//double vx2=new_person_companion_pose2.v*cos(new_person_companion_pose2.theta);
+			//double vy2=new_person_companion_pose2.v*sin(new_person_companion_pose2.theta);
+
+			//std::cout << "vx="<<vx<<"; vy="<<vy<< std::endl;
+
+			virtual_next_pose=SpointV_cov(new_person_companion_pose.x,new_person_companion_pose.y,new_person_companion_pose.time_stamp,vx,vy);
+			//virtual_next_pose_2groupPers=SpointV_cov(new_person_companion_pose2.x,new_person_companion_pose2.y,new_person_companion_pose2.time_stamp,vx2,vy2);
+			//std::cout << "(4.2)"<< std::endl;
+
+			/*(*iit)->set_person_type(Cperson_abstract::Person);
+			if(debug_companion_person_scene_sim_){
+				std::cout << "(4.3)   (*iit)->get_person_type()="<<(*iit)->get_person_type()<< std::endl;
+			}*/
+			//std::cout << "[7.4.4]" << std::endl;
+
+			//std::cout << "new_person_companion_SpointV_cov.print():"<< std::endl;
+			//virtual_next_pose.print();
+			if(debug_companion_person_scene_sim_){
+				std::cout << "new_person_companion_pose.print():"<< std::endl;
+				new_person_companion_pose.print();
+				std::cout << "new_person_companion_SpointV_cov.print():"<< std::endl;
+				virtual_next_pose.print();
+				//(*iit)->set_observation_update(true);
+				//	std::cout << "(4.4)"<< std::endl;
+				//(*iit)->add_pointV(virtual_next_pose);
+				//(*iit)->set_forces_person( virtual_force_goal*w, virtual_force_int_person*w, virtual_force_robot*w, virtual_force_obstacle );
+				//virtual_next_pose = (*iit)->pointV_propagation( dt_ , (*iit)->get_force_person()  );
+				//(*iit)->Cperson_bhmip_add_pointV(new_person_companion_SpointV_cov,filtering_method_,true);
+				//	std::cout << "(4.5)"<< std::endl;
+			}
+			//std::cout << "[7.4.5]" << std::endl;
+			//if(debug_approach_people_random_){
+			//std::cout << " 20 update "<< std::endl;
+			//}
+			//akp_for_person_companion_.update_person_companion(new_person_companion_pose,virtual_next_pose);
+			// OJO: cosa_quitada (lo de arriba)
+			//std::cout << " 21 update "<< std::endl;
+			//person_companion_->set_current_pose(new_person_companion_pose);
+			//person_companion_person_abstract_.set_current_pointV(virtual_next_pose);
+
+			//person_companion_person_abstract_2group_person_.get_current_pointV().
+					//akp_for_person_companion_2_group_person_.get
+
+			person_companion_person_abstract_.add_pointV(virtual_next_pose);
+			if(group_simulation_){
+				person_companion_person_abstract_2group_person_.add_pointV(virtual_next_pose_2groupPers);
+			}
+
+			/*if(group_simulation_){
+				if(other_companion_person_id_==1){
+
+				}
+			}*/
+
+
+			//if(debug_approach_people_random_){
+			//std::cout << " 22 update "<< std::endl;
+			//}
+
+
+
+			// if(obstacle_dist_act>10){
+			   if ( d_min_ < 1.0 ){
+				   akp_for_person_companion_.get_person_companion_akp()->set_desired_velocty(less_companion_velocity_*0.65);
+				   akp_for_person_companion_.set_person_companion_desired_velocity_sim(less_companion_velocity_*0.65);
+			   }else if ( d_min_ < 3.0){
+					 akp_for_person_companion_.get_person_companion_akp()->set_desired_velocty(less_companion_velocity_*0.75);
+					 akp_for_person_companion_.set_person_companion_desired_velocity_sim(less_companion_velocity_*0.75);
+			   }else if ( d_min_ < 5.0 ){
+				   akp_for_person_companion_.get_person_companion_akp()->set_desired_velocty(less_companion_velocity_*0.85);
+				   akp_for_person_companion_.set_person_companion_desired_velocity_sim(less_companion_velocity_*0.85);
+			   }else if ( d_min_ < 7.0 ){
+				   akp_for_person_companion_.get_person_companion_akp()->set_desired_velocty(less_companion_velocity_*0.95);
+				   akp_for_person_companion_.set_person_companion_desired_velocity_sim(less_companion_velocity_*0.95);
+			   }else{
+				   akp_for_person_companion_.get_person_companion_akp()->set_desired_velocty(less_companion_velocity_);
+				   akp_for_person_companion_.set_person_companion_desired_velocity_sim(less_companion_velocity_);
+			   }
+
+			   std::cout << " PERSON COMPANION; d_min_="<<d_min_<<" personComp_->vel="<< akp_for_person_companion_.get_person_companion_akp()->get_desired_velocity()<< std::endl;
+				// akp_for_person_companion_.get_person_companion_akp()->set_desired_velocty(less_companion_velocity_);
+				// akp_for_person_companion_.set_person_companion_desired_velocity_sim(less_companion_velocity_);
+			/* }else{
+				 akp_for_person_companion_.get_person_companion_akp()->set_desired_velocty(less_companion_velocity_*0.70);
+				 akp_for_person_companion_.set_person_companion_desired_velocity_sim(less_companion_velocity_*0.70);
+			 }*/
+
+			//if(group_simulation_){
+				//akp_for_person_companion_2_group_person_.get_person_companion_akp()->set_desired_velocty(less_companion_velocity_);
+			//}
+
+
+			//if(debug_approach_people_random_){
+			//std::cout << " 23 update "<< std::endl;
+			//}
+
+
+			//if(group_simulation_){
+				//akp_for_person_companion_2_group_person_.set_person_companion_desired_velocity_sim(less_companion_velocity_);
+			//}
+
+
+			if(debug_approach_people_random_){
+			std::cout << " 24 update "<< std::endl;
+			}
+
+			SpointV_cov new_person_comp_point=new_person_companion_SpointV_cov;
+			if(debug_companion_person_scene_sim_){
+				std::cout << "(5) (new_person_point=> new_person_comp_point.x"<<new_person_comp_point.x<<"; new_person_comp_point.y"<<new_person_comp_point.y<<"new_person_comp_point.vx="<<new_person_comp_point.vx<<"new_person_companion_pose.vy"<<new_person_comp_point.vy<<"new_person_companion_pose.v"<<new_person_comp_point.v()<< std::endl;
+			}
+			if(debug_approach_people_random_){
+			std::cout << " 25 update "<< std::endl;
+			}
+			// new_person_companion_pose is the next pose for the person companion!!!
+			new_person_companion_pose_=new_person_companion_pose;//Spose();//akp_for_person_companion_.get_person_companion_akp()->get_current_pose(); // TODO: volver a poner cuando arregle lo de dentro para funcionar sin la pose de esta persona entre las personas!
+			//SpointV_cov new_person_comp_point;
+			//new_person_comp_point=SpointV_cov();// TODO: quitar cuando pruebe lo del akp person companion.
+			//new_points_person_sim_.push_back(new_person_comp_point);
+			if(debug_companion_person_scene_sim_){
+				if(debug_other_persons_scene_sim_){
+					std::cout << "(new person_point) virtual_next_pose_person; id="<<companion_person_id_<< std::endl;
+				}
+				virtual_next_pose.print();
+			}
+			//std::cout << "[7.4.7]" << std::endl;
+			if(debug_companion_person_scene_sim_){
+				std::cout << " 787 person_companion_person_abstract_.get_current_pointV().print()"<< std::endl;
+				person_companion_person_abstract_.get_current_pointV().print();
+				std::cout << " 787 person_companion_person_abstract_2group_person_.get_current_pointV().print()"<< std::endl;
+				if(group_simulation_){
+					person_companion_person_abstract_2group_person_.get_current_pointV().print();
+				}
+
+				std::cout << "FIN Update person companion position"<< std::endl;
+			}
+
+			/*std::cout << "[IMPORTANT Cscene_SIM!!!!] view SpointV_cov perons; new_points_person_sim_.empty()="<<new_points_person_sim_.empty()<<"; new_points_person_sim_.size()"<<new_points_person_sim_.size()<< std::endl;
+			if(!new_points_person_sim_.empty()){
+				for(unsigned int o=0;o<new_points_person_sim_.size();o++){
+					std::cout << " new_points_person_sim_; persons_ids_[o]="<<persons_ids_[o]<< std::endl;
+					new_points_person_sim_[o].print();
+				}
+				//std::cout << " out if !new_points_person_sim_.empty()"<< std::endl;
+			}*/
+
+
+
+	/***  Update person companion position  ***/
+			//std::cout << " 26 update; print person list "<< std::endl;
+			/*for(iit = person_list_.begin() ; iit!=person_list_.end() ; iit++)
+			{
+				std::cout << " person_list_.id="<<(*iit)->get_id()<< std::endl;
+			}*/
+
+
+		//if(debug_general_scene_sim_){
+			//std::cout << " Iterations Cscene_SIM antes Cscene_sim-> update_companion_person: person_list_.empty()="<<person_list_.empty()<< std::endl;
+
+			//akp_for_person_companion_.set_person_list(person_list_);
+			//std::cout << "view person_list_"<< std::endl;
+			/*for(iit = person_list_.begin() ; iit!=person_list_.end() ; iit++){
+				std::cout << "(*iit)->get_id()"<<(*iit)->get_id()<< std::endl;
+				(*iit)->print();
+			}*/
+
+			//std::cout << "view new_person_companion_pose_"<< std::endl;
+			//new_person_companion_pose_.print();
+
+			//std::cout << " Iterations Cscene_SIM (print interior person_list) antes Cscene_sim-> update_companion_person: akp_for_person_companion_.get_scene()->empty()="<<akp_for_person_companion_.get_scene()->empty()<< std::endl;
+			//akp_for_person_companion_.print();
+		//}
+
+		//std::cout << "antes update_scene_for_person_companion_planning_akp()"<< std::endl;
+		//std::cout << "[7.4.8]" << std::endl;
+		//std::cout << " 27 update; print person_list "<< std::endl;
+		/*for(iit = person_list_.begin() ; iit!=person_list_.end() ; iit++)
+		{
+			std::cout << " person_list_.id="<<(*iit)->get_id()<< std::endl;
+		}*/
+
+
+
+	// TODO: cambiar position person id que sea la otra person companion den grupo, dentro del vector => new_points_person_sim_ buscarlo apartir de persons_ids_
+		// TODO: ojo! los add_point también habra que cambiarlos, ya veremos como...
+		/*if(group_simulation_){
+			for(unsigned int u=0; u<persons_ids_.size();u++){
+				if(other_companion_person_id_==persons_ids_[u]){
+					new_points_person_sim_[u]=other_companion_person_out_odometry_spoint_;
+				}
+			} //TODO: (comprovar) los add_point se pueden arreglar con un if(other_companion_person_id_!=persons_ids_[u]) =haces lo normal, else, haces add point del other_companion_person_out_odometry_spoint_
+
+		}*/
+
+
+
+		//std::cout << " 7777777777 (before update scene) NEW DEST PERSON COMPANION!" << std::endl;
+		//akp_for_person_companion_.get_person_companion_akp()->get_best_dest().print();
+
+		update_scene_for_person_companion_planning_akp();
+
+
+		//std::cout << " 7777777777 (after update scene) NEW DEST PERSON COMPANION!" << std::endl;
+		//akp_for_person_companion_.get_person_companion_akp()->get_best_dest().print();
+
+		//std::cout << "IMPORTANTE ········ despues update_scene_for_person_companion_planning_akp() "<< std::endl;
+
+		//std::cout <<  " IN persons now in the list! (after add tracks dabo) " << std::endl;
+		/*				for( auto iit : person_list_)
+						{
+							if(iit->get_id()!=id_person_companion_){
+							 std::cout << " (after Insert people) (1) iit->get_prediction_trajectory( )->size()="<<iit->get_prediction_trajectory( )->size()<<"; iit->get_id()=" <<iit->get_id()<<"; iit->desired_vel="<<iit->get_desired_velocity()<<  std::endl;
+							}
+						}*/
+		/*for(iit = person_list_.begin() ; iit!=person_list_.end() ; iit++)
+		{
+			std::cout << " person_list_.id="<<(*iit)->get_id()<< std::endl;
+		}*/
+	//	std::cout << " 26 update "<< std::endl;
+		//std::cout << "[7.4.9]" << std::endl;
+		//std::cout << "despues update_scene_for_person_companion_planning_akp()"<< std::endl;
+		//std::cout << "22" << std::endl;
+		//std::cout << "antes deleting complete trajectories  " << std::endl;
+		//deleting complete trajectories
+		//std::cout << "remove_targets_= "<<remove_targets_ << std::endl;
+
+
+
+
+
+		if ( remove_targets_ )
+		{
+			//std::cout << "enter remove targets"<< std::endl;
+			std::vector<unsigned int> targets_to_remove;
+			for(iit = person_list_.begin() ; iit!=person_list_.end() ; iit++)
+			{
+				if ( (((*iit)->get_person_type() == Cperson_abstract::Person)||((*iit)->get_person_type() == Cperson_abstract::Person_companion) ) &&
+						((*iit)->get_current_pointV().distance( (Spoint)(*iit)->get_best_dest() ) < 1.5) )//destination achieved
+				{
+					targets_to_remove.push_back((*iit)->get_id());
+					number_virtual_people_--;
+				}
+			}
+			for ( unsigned int i = 0; i < targets_to_remove.size() ; ++i)
+				remove_person(targets_to_remove[i]);
+		}
+		//assigning new destinations once the goal is achieved
+		else
+		{
+
+
+			//std::cout << "else if(remove_targets)"<< std::endl;
+		//	std::cout << "[7.5]" << std::endl;
+			std::vector<unsigned int> targets_to_update;
+			//double group1_velocity=rand_normal(1.0,0.1);
+			//double group2_velocity=rand_normal(1.0,0.1);
+			ant_person_dest_=person_best_dest_;  // TODO: mirar si he de cambiar por la mia de ahora al ir a buscar a otra persona.
+			person_best_dest_.clear();
+			person_best_dest_.reserve(number_virtual_people_*2); // Estaria mejor asi=> obs_other_person_companion_.size());
+			//pero como añade las personas en la siguiente iteracion no funciona bien
+			// como de momento solo tenemos 2 person companion, es numero de gente virtual por 2. (Cambiar ese 2 en el futuro por el numero de personas que acompañen al robot)
+			for(unsigned int p=1;p<(number_virtual_people_+1);p++){
+				person_best_dest_.push_back(Sdestination());
+			}
+			//std::cout << " 27.2 update "<< std::endl;
+			for(iit = person_list_.begin() ; iit!=person_list_.end() ; iit++)
+			{
+
+				//std::cout << "antes if  " << std::endl;
+				//std::cout << "[7.6]" << std::endl;
+				if(((*iit)->get_id()==2)||(((*iit)->get_id()==3)||(((id_person_companion_!=1)&&((*iit)->get_id()==1))))||((*iit)->get_id()==6)){
+					if((*iit)->get_id()==2){
+
+
+						if(state_act_==Cplan_local_nav_person_companion::ITER){//&&(group_go_to_interact_with_other_person_)){
+							Sdestination  dest_1;
+							Sdestination  dest_2;
+
+							if(actual_sim_case_==Cplan_local_nav_person_companion::case0){
+								std::vector<int> neighbours_ids_act;
+								neighbours_ids_act.clear();
+								neighbours_ids_act.push_back(1);
+								dest_1 = get_destinations()->at( 0 );
+								dest_2 = get_destinations()->at( 1 );
+								//std::cout << " case 0: "<< std::endl;
+								//std::cout << " dest_1.x="<<dest_1.x<<"; dest_1.y="<<dest_1.y<< std::endl;
+								//std::cout << " dest_2.x="<<dest_2.x<<"; dest_2.y="<<dest_2.y<< std::endl;
+								Sdestination goal_person_dest(1, dest_2.x , dest_2.y , 1 ,Sdestination::Map_goal, neighbours_ids_act);
+								(*iit)->set_best_dest( goal_person_dest );
+								person_best_dest_[(*iit)->get_id()-1]=goal_person_dest;
+							}else if(actual_sim_case_==Cplan_local_nav_person_companion::case1){
+								std::vector<int> neighbours_ids_act;
+								neighbours_ids_act.clear();
+								neighbours_ids_act.push_back(1);
+								dest_1 = get_destinations()->at( 15 );
+								dest_2 = get_destinations()->at( 16 );
+								//std::cout << " case 1: "<< std::endl;
+								//std::cout << " dest_1.x="<<dest_1.x<<"; dest_1.y="<<dest_1.y<< std::endl;
+								//std::cout << " dest_2.x="<<dest_2.x<<"; dest_2.y="<<dest_2.y<< std::endl;
+								Sdestination goal_person_dest(1, dest_2.x , dest_2.y , 1 ,Sdestination::Map_goal, neighbours_ids_act);
+								(*iit)->set_best_dest( goal_person_dest );
+								person_best_dest_[(*iit)->get_id()-1]=goal_person_dest;
+							}else if(actual_sim_case_==Cplan_local_nav_person_companion::case2){
+								std::vector<int> neighbours_ids_act;
+								neighbours_ids_act.clear();
+								neighbours_ids_act.push_back(1);
+								dest_1 = get_destinations()->at( 17 );
+								dest_2 = get_destinations()->at( 18 );
+								neighbours_ids_act.clear();
+								neighbours_ids_act.push_back(1);
+								//std::cout << " case 2: "<< std::endl;
+								//std::cout << " dest_1.x="<<dest_1.x<<"; dest_1.y="<<dest_1.y<< std::endl;
+								//std::cout << " dest_2.x="<<dest_2.x<<"; dest_2.y="<<dest_2.y<< std::endl;
+								Sdestination goal_person_dest(1, dest_2.x , dest_2.y , 1 ,Sdestination::Map_goal, neighbours_ids_act);
+								(*iit)->set_best_dest( goal_person_dest );
+								person_best_dest_[(*iit)->get_id()-1]=goal_person_dest;
+							}
+
+							destinations1_.clear();
+							destinations1_.push_back(dest_1);
+							destinations1_.push_back(dest_2);
+						}else{
+							Sdestination  dest_1;
+							Sdestination  dest_2;
+							dest_1 = get_destinations()->at( 0 );
+							dest_2 = get_destinations()->at( 1 );
+							destinations1_.clear();
+							destinations1_.push_back(dest_1);
+							destinations1_.push_back(dest_2);
+						}
+
+						(*iit)->set_destinations(destinations1_);
+						// person_goal == persona con id=2
+						(*iit)->set_desired_velocty( group_velocity1_1_ );
+
+					}
+					if(((*iit)->get_id()==3)||(((id_person_companion_!=1)&&((*iit)->get_id()==1)))){
+						Sdestination  dest_1 = get_destinations()->at( 2 );
+						Sdestination  dest_2 = get_destinations()->at( 3 );
+						destinations1_.clear();
+						destinations1_.push_back(dest_1);
+						destinations1_.push_back(dest_2);
+						if(all_complete_random_final_dest_people_){
+							Sdestination  dest_3 = get_destinations()->at( 4 );
+							Sdestination  dest_4 = get_destinations()->at( 5 );
+							Sdestination  dest_5 = get_destinations()->at( 6 );
+							Sdestination  dest_6 = get_destinations()->at( 7 );
+							destinations1_.push_back(dest_3);
+							destinations1_.push_back(dest_4);
+							destinations1_.push_back(dest_5);
+							destinations1_.push_back(dest_6);
+						}
+
+						(*iit)->set_destinations(destinations1_);
+						(*iit)->set_desired_velocty( group_velocity1_ );
+					}
+
+					//std::cout << "[8]" << std::endl;
+					/*if((*iit)->get_id()==6){
+						Sdestination  dest_1 = get_destinations()->at( 19 );
+						Sdestination  dest_2 = get_destinations()->at( 20 );
+						destinations1_.clear();
+						destinations1_.push_back(dest_1);
+						destinations1_.push_back(dest_2);
+						(*iit)->set_destinations(destinations1_);
+						//(*iit)->set_destinations(destinations1p2_);
+						(*iit)->set_desired_velocty( group_velocity1 );
+					}*/
+				//	std::cout << "[9]" << std::endl;
+
+				}
+				//std::cout << "medio if  " << std::endl;
+				if(((*iit)->get_id()==4)||((*iit)->get_id()==5)){
+					if((*iit)->get_id()==4){
+						// if complete random people:
+						//Sdestination  dest_3 = get_destinations()->at( 4 );
+						//Sdestination  dest_4 = get_destinations()->at( 5 );
+
+						Sdestination  dest_3 = get_destinations()->at( 2 );
+						Sdestination  dest_4 = get_destinations()->at( 3 );
+
+						destinations2_.clear();
+						destinations2_.push_back(dest_3);
+						destinations2_.push_back(dest_4);
+						if(all_complete_random_final_dest_people_){
+							Sdestination  dest_3 = get_destinations()->at( 4 );
+							Sdestination  dest_4 = get_destinations()->at( 5 );
+							Sdestination  dest_5 = get_destinations()->at( 6 );
+							Sdestination  dest_6 = get_destinations()->at( 7 );
+							destinations2_.push_back(dest_3);
+							destinations2_.push_back(dest_4);
+							destinations2_.push_back(dest_5);
+							destinations2_.push_back(dest_6);
+						}
+
+						(*iit)->set_destinations(destinations2_);
+
+						if(group_sincron_){
+							(*iit)->set_desired_velocty( group_velocity2_ );
+						}else{
+							(*iit)->set_desired_velocty( group_velocity2_2_ );
+						}
+
+					}
+					if((*iit)->get_id()==5){
+						// if complete random people
+						//Sdestination  dest_3 = get_destinations()->at( 6 );
+						//Sdestination  dest_4 = get_destinations()->at( 7 );
+
+						Sdestination  dest_3 = get_destinations()->at( 2 );
+						Sdestination  dest_4 = get_destinations()->at( 3 );
+
+						destinations2_.clear();
+						destinations2_.push_back(dest_3);
+						destinations2_.push_back(dest_4);
+						if(all_complete_random_final_dest_people_){
+							Sdestination  dest_3 = get_destinations()->at( 4 );
+							Sdestination  dest_4 = get_destinations()->at( 5 );
+							Sdestination  dest_5 = get_destinations()->at( 6 );
+							Sdestination  dest_6 = get_destinations()->at( 7 );
+							destinations2_.push_back(dest_3);
+							destinations2_.push_back(dest_4);
+							destinations2_.push_back(dest_5);
+							destinations2_.push_back(dest_6);
+						}
+
+						(*iit)->set_destinations(destinations2_);
+
+						//(*iit)->set_destinations(destinations2p4_);
+						//(*iit)->set_desired_velocty( group_velocity2_ );
+						(*iit)->set_desired_velocty( 0.6);
+					}
+
+				}
+
+				/////////////////
+				/*std::cout << "[10] (print, list);(*iit)->id= "<< (*iit)->get_id()<< std::endl;
+				///////////////
+
+				std::cout << "[10] (*iit)->get_person_type()= "<< (*iit)->get_person_type()<<"; Cperson_abstract::Person="<<Cperson_abstract::Person<<";  Cperson_abstract::Person_companion="<< Cperson_abstract::Person_companion<< std::endl;
+				std::cout << "[10] (*iit)->get_best_dest().x= "<< (*iit)->get_best_dest().x<<"; (*iit)->get_best_dest().y="<<(*iit)->get_best_dest().y<< std::endl;
+				std::cout << "[10] (*iit)->get_current_pointV().x= "<< (*iit)->get_current_pointV().x<<"; (*iit)->get_current_pointV().y="<<(*iit)->get_current_pointV().y<< std::endl;
+				 */
+				//std::cout << "[10]" << std::endl;
+				//std::cout << "despues if  " << std::endl;
+				if ( (((*iit)->get_person_type() == Cperson_abstract::Person)||((*iit)->get_person_type() == Cperson_abstract::Person_companion) )&&
+						((*iit)->get_current_pointV().distance( (Spoint)(*iit)->get_best_dest() ) < 1.5) )//destination achieved
+				{
+					//std::cout << "[10.1]" << std::endl;
+
+					// RAMDOM: poner a true el bool all_complete_random_final_dest_people_ (select randomly the final destinations of the people)
+					if(all_complete_random_final_dest_people_){
+						//Sdestination goal_person_dest=get_destinations()->at( rand_uniform_discrete( 0, (*iit)->get_destinations()->size()-1) );
+						//(*iit)->set_best_dest( goal_person_dest );
+						//person_best_dest_[(*iit)->get_id()-1]=goal_person_dest;
+						std::vector<Sdestination> neighbours_dest;
+						neighbours_dest.push_back(get_destinations()->at(2));
+						neighbours_dest.push_back(get_destinations()->at(3));
+						neighbours_dest.push_back(get_destinations()->at(4));
+						neighbours_dest.push_back(get_destinations()->at(5));
+						neighbours_dest.push_back(get_destinations()->at(6));
+						neighbours_dest.push_back(get_destinations()->at(7));
+						Sdestination goal_person_dest=neighbours_dest.at( rand_uniform_discrete( 0, neighbours_dest.size()-1) );
+						(*iit)->set_best_dest( goal_person_dest );
+						person_best_dest_[(*iit)->get_id()-1]=goal_person_dest;
+					}
+
+					if(random_final_dest_people_){
+						if(((*iit)->get_id()>5)&&((*iit)->get_id()<(number_virtual_people_+1))){
+							std::vector<Sdestination> neighbours_dest;
+							//neighbours_dest.push_back(get_destinations()->at(2));
+							//neighbours_dest.push_back(get_destinations()->at(3));
+							neighbours_dest.push_back(get_destinations()->at(4));
+							neighbours_dest.push_back(get_destinations()->at(5));
+							neighbours_dest.push_back(get_destinations()->at(6));
+							neighbours_dest.push_back(get_destinations()->at(7));
+							Sdestination goal_person_dest=neighbours_dest.at( rand_uniform_discrete( 0, neighbours_dest.size()-1) );
+							(*iit)->set_best_dest( goal_person_dest );
+							person_best_dest_[(*iit)->get_id()-1]=goal_person_dest;
+						}else{
+							std::vector<Sdestination> neighbours_dest;
+							neighbours_dest.push_back(get_destinations()->at(2));
+							neighbours_dest.push_back(get_destinations()->at(3));
+							Sdestination goal_person_dest=neighbours_dest.at( rand_uniform_discrete( 0, neighbours_dest.size()-1) );
+							(*iit)->set_best_dest( goal_person_dest );
+							person_best_dest_[(*iit)->get_id()-1]=goal_person_dest;
+						}
+					}
+				}else{
+					//std::cout << "[12]; ant_person_dest_.size()="<<ant_person_dest_.size() << std::endl;
+					//std::cout << "[12.1]; (*iit)->get_id()-1="<<(*iit)->get_id()-1 <<"; person_best_dest_.size="<<person_best_dest_.size()<<"; ant_person_dest_.size="<<ant_person_dest_.size()<< std::endl;
+					person_best_dest_[(*iit)->get_id()-1]=ant_person_dest_[(*iit)->get_id()-1];
+					//std::cout << "[12.1]" << std::endl;
+				}
+				//std::cout << "[13]" << std::endl;
+				//(*iit)->print_dest();
+				//(*iit)->print();
+				//std::cout << " person (*iit)->best_destination_.x="<<(*iit)->get_best_dest().x<<"; person (*iit)->best_destination_.y="<<(*iit)->get_best_dest().y<< std::endl;
+				//double dest_x=person_best_dest_[(*iit)->get_id()-1].x;
+				//double dest_y=person_best_dest_[(*iit)->get_id()-1].y;
+				//std::cout << " person_best_dest_[(*iit)->get_id()-1].x="<<dest_x<<";  person_best_dest_[(*iit)->get_id()-1].y="<< dest_y<< std::endl;
+			}
+		}
+
+
+
+
+		/*std::cout <<  "13 (Cscene_sim) after remove targets ( else case UPDATE) check people nan's " << std::endl;
+											for( auto iit : person_list_)
+											{
+												if(iit->get_id()!=id_person_companion_){
+
+												 std::cout << " (after Insert people) (1) iit->get_prediction_trajectory( )->size()="<<iit->get_prediction_trajectory( )->size()<<"; iit->get_id()=" <<iit->get_id()<<"; iit->desired_vel="<<iit->get_desired_velocity()<<  std::endl;
+												 iit->get_current_pointV().print();
+												}
+											}*/
+
+		//std::cout << " 28 update "<< std::endl;
+
+			//std::cout << "[14]" << std::endl;
+				//std::cout << "OUT c++  " << std::endl;
+				//std::cout << "23" << std::endl;
+				/*  update person companion destination */
+				//std::cout << " antes update person companion destination"<< std::endl;
+				dest_1 = get_destinations()->at( 8 );
+				dest_2 = get_destinations()->at( 9 );
+				 dest_10 = get_destinations()->at( 10 );
+				dest_11 = get_destinations()->at( 11 );
+				//Sdestination  dest_12;
+				//Sdestination  dest_13;
+				//Sdestination  dest_14;
+				//std::cout << "[14.2]" << std::endl;
+				//if(caso_simulacion_giro_){
+					dest_12 = get_destinations()->at( 12 );
+					dest_13 = get_destinations()->at( 13 );
+					dest_14 = get_destinations()->at( 14 );
+				//}
+				//	std::cout << "[14.3]" << std::endl;
+				destinations1_.clear();
+				destinations1_.push_back(dest_1);
+				destinations1_.push_back(dest_2);
+				destinations1_.push_back(dest_10);
+				destinations1_.push_back(dest_11);
+				//std::cout << "[14.4]" << std::endl;
+				//if(caso_simulacion_giro_){
+					destinations1_.push_back(dest_12);
+					destinations1_.push_back(dest_13);
+					destinations1_.push_back(dest_14);
+				//	std::cout << "[15]" << std::endl;
+					//std::cout << "(4) SET ALL DESTS destinations1_.size() "<< destinations1_.size() << std::endl;
+
+					if(state_act_==Cplan_local_nav_person_companion::START){
+
+						std::vector<int> neighbours_ids_act;
+						std::vector<Sdestination> goal_person_dest2;
+						neighbours_ids_act.push_back(1);
+						//neighbours_ids_act.push_back(id_person_companion_);
+						Sdestination goal_person_dest(id_person_companion_, actual_goal_to_return_person_comp_.x , actual_goal_to_return_person_comp_.y , 1 ,Sdestination::Map_goal, neighbours_ids_act);
+						goal_person_dest2.clear();
+						goal_person_dest2.push_back(goal_person_dest);
+						person_companion_person_abstract_.set_destinations(goal_person_dest2); //TODO:cambia
+						if(group_simulation_){
+							person_companion_person_abstract_2group_person_.set_destinations(goal_person_dest2);
+						}
+						actual_goal_to_return_person_comp_=actual_goal_to_return_person_comp1_;
+					}else if(state_act_==Cplan_local_nav_person_companion::ITER){
+						person_companion_person_abstract_.set_destinations(destinations1_); //TODO:cambia
+						if(group_simulation_){
+							person_companion_person_abstract_2group_person_.set_destinations(destinations1_);
+						}
+
+					}
+
+		//std::cout << "[16]" << std::endl;
+		person_companion_person_abstract_.set_desired_velocty( less_companion_velocity_ );
+		if(group_simulation_){
+			person_companion_person_abstract_2group_person_.set_desired_velocty( less_companion_velocity_ );
+		}
+
+
+		//std::cout << " 29 update "<< std::endl;
+		//std::cout << " ANTES SET DESTINATIONS (peson companion) ELSE RAMDOM state_ITER group_go_static_dest"<< std::endl;
+	//	std::cout << "[17]" << std::endl;
+
+			//std::cout << " (peson companion) ELSE RAMDOM state_ITER group_go_static_dest"<< std::endl;
+			//std::cout << "(1) SET RANDOM DEST " << std::endl;
+			if(state_act_==Cplan_local_nav_person_companion::START){
+				std::vector<Sdestination> goal_person_dest2;
+				std::vector<int> neighbours_ids_act;
+				neighbours_ids_act.push_back(1);
+				//neighbours_ids_act.push_back(id_person_companion_);
+				Sdestination goal_person_dest(1, actual_goal_to_return_person_comp_.x , actual_goal_to_return_person_comp_.y , 1 ,Sdestination::Map_goal, neighbours_ids_act);
+				goal_person_dest2.clear();
+				goal_person_dest2.push_back(goal_person_dest);
+				person_companion_person_abstract_.set_best_dest( goal_person_dest);  //TODO: esta directamente se cambiaria por la de la persona a la que sigues.
+				if(group_simulation_){
+					person_companion_person_abstract_2group_person_.set_best_dest( goal_person_dest);
+				}
+
+				// TODO: OJO!!! hacer booleano, tener las dos opciones goal fijo y goal movil! añadir. NO destruir!!!
+				person_actual_best_dest_=goal_person_dest; //TODO:cambio
+				person_best_dest_[0]=goal_person_dest; //TODO:cambio
+				actual_goal_to_return_person_comp_=actual_goal_to_return_person_comp1_;
+
+			}else if(state_act_==Cplan_local_nav_person_companion::ITER){
+				//std::cout << " (peson companion) ELSE RAMDOM state_ITER"<< std::endl;
+					//////////////////
+					if(bool_select_rando_dest_person_companion_){
+						////////////////
+					// si llega a la destinacion ir a por otra... (mirar... No segura que se haya de cambiar o k...)
+						if ( ((person_companion_person_abstract_.get_person_type() == Cperson_abstract::Person)||(person_companion_person_abstract_.get_person_type() == Cperson_abstract::Person_companion)) &&
+								person_companion_person_abstract_.get_current_pointV().distance( (Spoint)person_companion_person_abstract_.get_best_dest() ) < 1.5 )//destination achieved
+						{
+							//set destination at random
+							//(*iit)->set_desired_velocty( rand_normal(1.0,0.1) );
+							std::vector<int> neighbours = person_companion_person_abstract_.get_best_dest().neighbours_ids;
+							int new_dest = neighbours[ rand_uniform_discrete( 0, neighbours.size()-1 )];
+							person_companion_person_abstract_.set_best_dest( destinations_[ new_dest ] );  //TODO: esta directamente se cambiaria por la de la persona a la que sigues.
+							if(group_simulation_){
+								person_companion_person_abstract_2group_person_.set_best_dest( destinations_[ new_dest ]);
+							}
+
+							// TODO: OJO!!! hacer booleano, tener las dos opciones goal fijo y goal movil! añadir. NO destruir!!!
+							person_actual_best_dest_=destinations_[ new_dest ]; //TODO:cambio
+							person_best_dest_[0]=person_actual_best_dest_; //TODO:cambio
+						}else{
+							person_best_dest_[0]=person_actual_best_dest_; //TODO:Cambio
+							// same before destination seted
+							//std::cout << " in else... "<< std::endl;
+							//person_actual_best_dest_=person_actual_best_dest_;
+						}
+
+						//std::cout << "(2) SET RANDOM DEST person_comp" << std::endl;
+					//////////
+					}else{
+
+						//std::cout << " (peson companion) ELSE RAMDOM; actual_sim_case_="<<actual_sim_case_<< std::endl;
+
+
+						if(actual_sim_case_==Cplan_local_nav_person_companion::case0){
+							std::cout << "(person companion) Cplan_local_nav::case0 destinations_[8].x"<<destinations_ini_security_copy[ 8 ].x<<"; destinations_[8].y"<<destinations_ini_security_copy[ 8 ].y << std::endl;
+
+							if ( ((person_companion_person_abstract_.get_person_type() == Cperson_abstract::Person)||(person_companion_person_abstract_.get_person_type() == Cperson_abstract::Person_companion)) &&
+													person_companion_person_abstract_.get_current_pointV().distance( (Spoint)person_companion_person_abstract_.get_best_dest() ) < 1.5 )//destination achieved
+							{
+								person_companion_person_abstract_.set_best_dest( destinations_ini_security_copy[ 8 ]); //ojo, la destinacion a la que va es la 9!
+								if(group_simulation_){
+									person_companion_person_abstract_2group_person_.set_best_dest( destinations_ini_security_copy[ 8 ]);
+								}
+
+								//person_companion_person_abstract_.get_person_companion_akp()->set_best_dest(destinations1_[1]);
+								person_actual_best_dest_=destinations_ini_security_copy[ 8 ];
+								person_best_dest_[0]=person_actual_best_dest_;
+							}else{
+								person_best_dest_[0]=person_actual_best_dest_;
+							}
+						}else if(actual_sim_case_==Cplan_local_nav_person_companion::case1){
+							std::cout << "(person companion) Cplan_local_nav_person_companion::case1 destinations_[10].x"<<destinations_ini_security_copy[ 10 ].x<<"; destinations_[10].y"<<destinations_ini_security_copy[ 10 ].y << std::endl;
+							//std::cout << "(person companion) Cplan_local_nav_person_companion::case1; destinations_[10].x"<<destinations_[4].x<<"; destinations_[4].y"<<destinations_[4].y<< std::endl;
+							if ( ((person_companion_person_abstract_.get_person_type() == Cperson_abstract::Person)||(person_companion_person_abstract_.get_person_type() == Cperson_abstract::Person_companion)) &&
+													person_companion_person_abstract_.get_current_pointV().distance( (Spoint)person_companion_person_abstract_.get_best_dest() ) < 1.5 )//destination achieved
+							{
+								person_companion_person_abstract_.set_best_dest(destinations_ini_security_copy[ 10 ]); //ojo, la destinacion a la que va es la 11!
+								if(group_simulation_){
+									person_companion_person_abstract_2group_person_.set_best_dest(destinations_ini_security_copy[ 10 ]);
+								}
+
+								//person_companion_person_abstract_.get_person_companion_akp()->set_best_dest(destinations1_[1]);
+								person_actual_best_dest_=destinations_ini_security_copy[ 10 ];  //ojo, la destinacion a la que va es la 11!
+								person_best_dest_[0]=person_actual_best_dest_;
+							}else{
+								person_best_dest_[0]=person_actual_best_dest_;
+							}
+						}else if(actual_sim_case_==Cplan_local_nav_person_companion::case2){
+							std::cout << "(person companion) Cplan_local_nav_person_companion::case2 destinations_[12].x"<<destinations_ini_security_copy[ 12 ].x<<"; destinations_[12].y"<<destinations_ini_security_copy[ 12 ].y << std::endl;
+							if ( ((person_companion_person_abstract_.get_person_type() == Cperson_abstract::Person)||(person_companion_person_abstract_.get_person_type() == Cperson_abstract::Person_companion)) &&
+													person_companion_person_abstract_.get_current_pointV().distance( (Spoint)person_companion_person_abstract_.get_best_dest() ) < 1.5 )//destination achieved
+							{
+								person_companion_person_abstract_.set_best_dest( destinations_ini_security_copy[ 12 ]); //ojo, la destinacion a la que va es la 13!
+								if(group_simulation_){
+									person_companion_person_abstract_2group_person_.set_best_dest( destinations_ini_security_copy[ 12 ]);
+								}
+
+								//person_companion_person_abstract_.get_person_companion_akp()->set_best_dest(destinations1_[1]);
+								person_actual_best_dest_=destinations_ini_security_copy[ 12 ];
+								person_best_dest_[0]=person_actual_best_dest_;
+							}else{
+								person_best_dest_[0]=person_actual_best_dest_;
+							}
+						}else{
+							//std::cout << "(person companion) Cplan_local_nav_person_companion::case0_ELSE destinations_[8].x"<<destinations_ini_security_copy[ 8 ].x<<"; destinations_[8].y"<<destinations_ini_security_copy[ 8 ].y << std::endl;
+							if ( ((person_companion_person_abstract_.get_person_type() == Cperson_abstract::Person)||(person_companion_person_abstract_.get_person_type() == Cperson_abstract::Person_companion)) &&
+																		person_companion_person_abstract_.get_current_pointV().distance( (Spoint)person_companion_person_abstract_.get_best_dest() ) < 1.5 )//destination achieved
+							{
+								person_companion_person_abstract_.set_best_dest( destinations_ini_security_copy[ 8 ]);
+								if(group_simulation_){
+									person_companion_person_abstract_2group_person_.set_best_dest( destinations_ini_security_copy[ 8 ]);
+								}
+
+								//person_companion_person_abstract_.get_person_companion_akp()->set_best_dest(destinations1_[1]);
+								person_actual_best_dest_=destinations_ini_security_copy[ 8 ];
+								person_best_dest_[0]=person_actual_best_dest_;
+							}else{
+								person_best_dest_[0]=person_actual_best_dest_;
+							}
+						}
+
+
+					}
+////////////
+						}
+
+			akp_for_person_companion_.get_person_companion_akp()->set_best_dest(person_actual_best_dest_);  // todo: cambia
+			//if(group_simulation_){
+				//akp_for_person_companion_2_group_person_.get_person_companion_akp()->set_best_dest(person_actual_best_dest_);
+			//}
+
+		//std::cout << " despues update person companion destination"<< std::endl;
+
+	}
+
+
+	//std::cout << " 7777777777 (near final of CsceneSim) NEW DEST PERSON COMPANION!" << std::endl;
+	//akp_for_person_companion_.get_person_companion_akp()->get_best_dest().print();
+
+
+
+	//std::cout << " 30 update "<< std::endl;
+	if(debug_general_scene_sim_){
+		std::cout << "out update_scene_companion_simulation_akp_person_companion;  observation[0].time_stamp=" << observation[0].time_stamp << std::endl;
+		std::cout<<std::endl<<std::endl<<std::endl;
+	}
+
+
+	/*std::cout<<" new_points_person_sim_.size()="<<new_points_person_sim_.size()<<std::endl;
+	std::cout<<" persons_ids_.size()="<<persons_ids_.size()<<std::endl;
+	std::cout<<" person_best_dest_.size()="<<person_best_dest_.size()<<std::endl;
+	std::cout<<" ant_person_dest_.size()="<<ant_person_dest_.size()<<std::endl;
+	std::cout<<" dest_person1_.size()="<<dest_person1_.size()<<std::endl;
+	std::cout<<" dest_person2_3_.size()="<<dest_person2_3_.size()<<std::endl;
+	std::cout<<" dest_person4_5_.size()="<<dest_person4_5_.size()<<std::endl;
+	std::cout<<" destinations1_.size()="<<destinations1_.size()<<std::endl;
+	std::cout<<" destinations2_.size()="<<destinations2_.size()<<std::endl;
+	std::cout<<" ids_of_persons_in_group1_.size()="<<ids_of_persons_in_group1_.size()<<std::endl;
+	std::cout<<" ids_of_persons_in_group2_.size()="<<ids_of_persons_in_group2_.size()<<std::endl;*/
+
+	//std::cout << "FIND CHANGE POSE TARGET PERSON [13]" << std::endl;
+
+	pose_command=new_person_companion_pose_;
+
+	if(debug_general_scene_sim_){
+		std::cout << "OUT update_scene_companion_simulation_akp_person_companion; returned pose_command.print:" << std::endl;
+		pose_command.print();
+	}
+
+
+
+	//std::cout << " 31 update "<< std::endl;
+	if(state_act_==Cplan_local_nav_person_companion::START){
+			//if(goal1_==1){
+		if(goal1_==1){
+			actual_goal_to_return_person_comp_=actual_goal_to_return_person_comp1_;
+
+				if(actual_sim_case_==Cplan_local_nav_person_companion::case1){ //para caso 1
+					//actual_goal_to_return_person_comp_=ini_person_goal4_;
+					//SpointV_cov act_comp_pers_pose=akp_for_person_companion_.get_companion_person_position();
+					//actual_goal_to_return_person_comp_=Spoint(act_comp_pers_pose.x-1,act_comp_pers_pose.y-1,act_comp_pers_pose.time_stamp);
+					/*if(pose_command.v<0.2){
+						contatore_++;
+					}
+
+					if(3<contatore_){
+						//pose_command.v=pose_command.v+0.2; // si estas atrapado, gira para salirte de ahí...
+						person_companion_plan_succed_=true;
+						pose_command.v=-0.2; // si estas atrapado, gira para salirte de ahí...
+						pose_command.w=0.2; // si estas atrapado, gira para salirte de ahí...
+						contatore_=0;
+					}*/
+					//std::cout<<"OUT pose_command.v="<<pose_command.v<<"; pose_command.w="<<pose_command.w<<std::endl;
+
+				}else{ // para caso 2 y 3
+
+					/*if(pose_command.v<0.2){
+						contatore_++;
+					}
+
+					if(3<contatore_){
+						//pose_command.v=pose_command.v+0.2; // si estas atrapado, gira para salirte de ahí...
+						person_companion_plan_succed_=true;
+						pose_command.v=-0.2; // si estas atrapado, gira para salirte de ahí...
+						pose_command.w=0.2; // si estas atrapado, gira para salirte de ahí...
+						contatore_=0;
+					}*/
+					//std::cout<<"OUT pose_command.v="<<pose_command.v<<"; pose_command.w="<<pose_command.w<<std::endl;
+
+					//SpointV_cov act_comp_pers_pose=akp_for_person_companion_.get_companion_person_position();
+					//actual_goal_to_return_person_comp_=ini_person_goal3_;
+					//actual_goal_to_return_person_comp_=Spoint(act_comp_pers_pose.x-1,act_comp_pers_pose.y-1,act_comp_pers_pose.time_stamp);
+				}
+
+				/*if((akp_for_person_companion_.get_companion_person_position().distance(ini_person_goal3_)<1)||((akp_for_person_companion_.get_companion_person_position().distance(ini_person_goal4_)<1))){
+					goal1_=2;
+				}*/
+			}else if(goal1_==2){
+				actual_goal_to_return_person_comp_=actual_goal_to_return_person_comp1_;
+				if(akp_for_person_companion_.get_companion_person_position().distance(actual_goal_to_return_person_comp1_)<1){
+					goal1_=3;
+				}
+			}else if(goal1_==3){
+				actual_goal_to_return_person_comp_=actual_goal_to_return_person_comp2_;
+			}
+		}
+
+	//std::cout << " 32 update "<< std::endl;
+	insert_more_random_people(observation);
+	//std::cout << " 33 update; person_companion_plan_succed_="<<person_companion_plan_succed_<< std::endl;
+
+
+
+
+
+
+
+	/*std::cout <<  " IN persons now in the list! (before add tracks dabo) " << std::endl;
+	for( auto iit : person_list_)
+	{
+		if(iit->get_id()!=id_person_companion_){
+
+		 std::cout << " (before Insert people) (1) iit->get_prediction_trajectory( )->size()="<<iit->get_prediction_trajectory( )->size()<<"; iit->get_id()=" <<iit->get_id()<<"; iit->desired_vel="<<iit->get_desired_velocity()<<  std::endl;
+		 iit->get_current_pointV().print();
+		}
+	}*/
+
+
+	////////TODO: mover aki el añadir las nuevas personas de la otra person companion!!!!
+	/////////////////////////////////////////////////////////////
+				// INI INSERT PEOPLE FROM OTHER ROBOT-PERSON-COMPANION
+	//std::cout << " other_companion_person_id_="<<other_companion_person_id_<<"; id_person_companion_="<<id_person_companion_<< std::endl;
+
+
+				//std::cout << " 333333333333333333333 ANTES DE add other person companion tracks; obs_other_person_companion_.size()="<<obs_other_person_companion_.size()<< std::endl;
+					if((group_simulation_)&&(!obs_other_person_companion_.empty())){ // insert the other tracks of the other person companion!!!
+
+						//unsigned int ids_news=number_virtual_people_;
+
+						for(unsigned int ty = 0 ; ty< obs_other_person_companion_.size() ; ty++){
+							//if((obs_other_person_companion_[ty].id!=other_companion_person_id_)&&(obs_other_person_companion_[ty].id>5)){
+							if(obs_other_person_companion_[ty].id>5){
+
+								unsigned int actual_id_in_person_list=100000000;
+								Sdestination actual_best_dest;
+								for(unsigned int it_id=0; it_id<people_ids_correspondence_OTHER_person_companion_.size();it_id++)
+								{
+									if(obs_other_person_companion_[ty].id==people_ids_correspondence_OTHER_person_companion_[it_id].real_person_id_){
+										actual_id_in_person_list=people_ids_correspondence_OTHER_person_companion_[it_id].actual_new_id_in_people_simulation_;
+									}
+								}
+								for(unsigned int it_id=0; it_id<destinations_other_person_companion_.size();it_id++)
+								{
+									if(destinations_other_person_companion_[it_id].id==obs_other_person_companion_[ty].id){
+										actual_best_dest=Sdestination(0,destinations_other_person_companion_[it_id].x,destinations_other_person_companion_[it_id].y );
+									}
+								}
+								// TODO: hay que buscar el id de esta persona en la lista.
+								// id en lista de la persona => actual_id_in_person_list
+								bool finded_person=false;
+								std::list<Cperson_abstract*>::iterator iit_act_person;
+								for( iit = person_list_.begin() ; iit!=person_list_.end() ; iit++)
+								{
+									if((*iit)->get_id()==actual_id_in_person_list){
+										finded_person=true;
+										iit_act_person=iit;
+									}
+								}
+
+								//std::cout << " 333333333333333333333 [before id. Normal id!](in other person tracks)obs.id="<<obs_other_person_companion_[ty].id<<"; obs_other_person_companion_[ty].x="<<obs_other_person_companion_[ty].x<<"; obs_other_person_companion_[ty].y="<<obs_other_person_companion_[ty].y<< std::endl;
+								//std::cout << " number_virtual_people_="<<number_virtual_people_<<"; obs_other_person_companion_.size()="<<obs_other_person_companion_.size() << std::endl;
+								//obs_other_person_companion_[ty].id=ids_news+1;//10+obs_other_person_companion_[ty].id; //obs.size()+1;
+								//std::cout << " (actual) ids_news="<<ids_news << std::endl;
+
+								//obs.push_back(obs_other_person_companion_[ty]);
+								//std::cout << " 333333333333333333333 (in other person tracks)obs.id="<<obs_other_person_companion_[ty].id<<"; obs_other_person_companion_[ty].x="<<obs_other_person_companion_[ty].x<<"; obs_other_person_companion_[ty].y="<<obs_other_person_companion_[ty].y<< std::endl;
+								//std::cout << "3333; ant_person_dest_.size()="<<ant_person_dest_.size()<< ";person_best_dest_.size()"<<person_best_dest_.size() << std::endl;
+								//ids_news=actual_id_in_person_list;
+								//std::cout << " (next) actual_id_in_person_list="<<actual_id_in_person_list << std::endl;
+								// Add this person to the list.
+
+								Sdestination dest_3, dest_4; // destinations luego se cogeran de fuera, del otro nodo.
+								dest_4 = get_destinations()->at( 2 );
+								dest_3 = get_destinations()->at( 3 );
+								destinations2_.clear();
+								destinations2_.push_back(dest_3);
+								destinations2_.push_back(dest_4);
+								if(all_complete_random_final_dest_people_){
+									Sdestination dest_new=get_destinations()->at( 6 );
+									destinations2_.push_back(dest_new);
+									dest_new=get_destinations()->at( 7 );
+									destinations2_.push_back(dest_new);
+								}
+								double x = obs_other_person_companion_[ty].x;  // a X_dist del goal 3, cada destination of person.
+								double y = obs_other_person_companion_[ty].y;
+								double now=obs_other_person_companion_[ty].time_stamp;
+								double vx=obs_other_person_companion_[ty].vx;
+								double vy=obs_other_person_companion_[ty].vy;
+								SpointV_cov person_act=SpointV_cov(x , y, now);
+								if(debug_init_scene_sim_){
+									std::cout << "person_act" << std::endl;
+									person_act.print();
+								}
+								double act_group_velocity;
+								//std::cout << "  33333 NEW people from otherperson comp 1"<< std::endl;
+								if(finded_person){ // si encuentras a la persona en la lista, haces el update. SOLO.
+									//std::cout << "  33333 NEW people from otherperson comp 2"<< std::endl;
+									(*iit_act_person)->set_destinations(destinations2_);
+									//std::cout << "  33333 NEW people from otherperson comp 3"<< std::endl;
+									(*iit_act_person)->set_best_dest( actual_best_dest);//(*iit_act_person)->get_destinations()->at( 1 ) );
+									//std::cout << "  33333 NEW people from otherperson comp 4"<< std::endl;
+									person_best_dest_.resize(number_virtual_people_+obs_other_person_companion_.size()+1);
+									//std::cout << "  33333 NEW people from otherperson comp 5"<< std::endl;
+									person_best_dest_[actual_id_in_person_list]=actual_best_dest;//(*iit_act_person)->get_destinations()->at( 1 );
+									//std::cout << "  33333 NEW people from otherperson comp6"<< std::endl;
+									(*iit_act_person)->add_pointV( SpointV_cov(x , y, now, vx, vy),  filtering_method_ );
+									//std::cout << "  33333 NEW people from otherperson comp 7"<< std::endl;
+									act_group_velocity=sqrt(obs_other_person_companion_[ty].vx*obs_other_person_companion_[ty].vx+obs_other_person_companion_[ty].vy*obs_other_person_companion_[ty].vy);
+									//std::cout << "  33333 NEW people from otherperson comp 8"<< std::endl;
+									(*iit_act_person)->set_desired_velocty( act_group_velocity );
+									//std::cout << "!!!!!!!!! 17 !!!!!!!!!  act_group_velocity= "<< (*iit_act_person)->get_desired_velocity()<< std::endl;
+									new_points_person_sim_.push_back(SpointV_cov(x , y, now, vx, vy));
+									persons_ids_.push_back(actual_id_in_person_list);//(obs_other_person_companion_[ty].id);
+
+									//(*iit_act_person)->print_dest();
+									//(*iit_act_person)->print();
+
+								}else{ // si no encuentras a la persona, la añades.
+									//std::cout << "  33333 NEW people from otherperson comp 9"<< std::endl;
+									Cperson_abstract* new_person;
+									//std::cout << "  33333 NEW people from otherperson comp 10"<< std::endl;
+									new_person = add_person(actual_id_in_person_list);//(obs_other_person_companion_[ty].id);
+									//std::cout << "  33333 NEW people from otherperson comp 11"<< std::endl;
+									new_person->set_destinations(destinations2_);
+									//std::cout << "  33333 NEW people from otherperson comp 12"<< std::endl;
+									dest_person4_5_=destinations2_;
+									if(debug_init_scene_sim_){
+										//std::cout << "(person 4 y 5) new_person->get_destinations()->at( 1 ).print():" << std::endl;
+										new_person->get_destinations()->at( 1 ).print();
+										//std::cout << " Destinations person 4 y 5:" << std::endl;
+										for(unsigned int dest=0; dest<dest_person4_5_.size(); dest++){
+											dest_person4_5_[dest].print();
+										}
+									}
+
+									//new_person->set_destinations(destinations2p3_);
+
+									//std::cout << "16; (person id) x="<<x<<"; y="<<y << std::endl;
+									act_group_velocity=sqrt(obs_other_person_companion_[ty].vx*obs_other_person_companion_[ty].vx+obs_other_person_companion_[ty].vy*obs_other_person_companion_[ty].vy);
+									//std::cout << "17 add other person companion tracks; " << std::endl;
+									new_person->set_desired_velocty( act_group_velocity );
+									//std::cout << "18 add other person companion tracks; obs_other_person_companion_.size()="<<obs_other_person_companion_.size()<<"; number_virtual_people_="<<number_virtual_people_ << std::endl;
+									new_person->set_best_dest(actual_best_dest);//new_person->get_destinations()->at( 1 ) ); // insertar tambien actual best dest.
+									//std::cout << "19 add other person companion tracks; obs_other_person_companion_[ty].id="<<obs_other_person_companion_[ty].id << std::endl;
+									person_best_dest_.resize(number_virtual_people_+obs_other_person_companion_.size()+1);
+									//std::cout << "20 add other person companion tracks; person_best_dest_.size="<<person_best_dest_.size()<<"; actual_id_in_person_list="<<actual_id_in_person_list << std::endl;
+									person_best_dest_[actual_id_in_person_list]=actual_best_dest;//new_person->get_destinations()->at( 1 );
+
+									//[obs_other_person_companion_[ty].id]=new_person->get_destinations()->at( 1 );
+
+									//ant_person_dest_=person_best_dest_;
+
+									//std::cout << "22 add other person companion tracks; " << std::endl;
+									//double theta= atan2(new_person->get_destinations()->at( 1 ).y-person_act.y,new_person->get_destinations()->at( 1 ).x-person_act.x);
+									//std::cout << "23 add other person companion tracks; " << std::endl;
+									new_person->add_pointV( SpointV_cov(x , y, now, vx, vy),  filtering_method_ );
+									//std::cout << "24 add other person companion tracks; " << std::endl;
+									new_points_person_sim_.push_back(SpointV_cov(x , y, now, vx, vy));
+									//std::cout << "25 add other person companion tracks; " << std::endl;
+									//new_person->add_pointV( SpointV_cov(x , y, now),  filtering_method_ );
+									//new_points_person_sim_.push_back(SpointV_cov(x , y, now));
+									persons_ids_.push_back(actual_id_in_person_list);//(obs_other_person_companion_[ty].id);
+
+									//std::cout << " 17 in ini_case!"<< std::endl;
+									//new_person->print_dest();
+									//new_person->print();
+								}
+
+							}
+						}
+
+						// hacer Cprediction interna a ves si va...
+						akp_for_person_companion_.do_scene_prediction_from_outside(&person_best_dest_);
+
+					}
+
+					/*std::cout <<  " IN persons now in the list! (after add tracks dabo) " << std::endl;
+					for( auto iit : person_list_)
+					{
+						if(iit->get_id()!=id_person_companion_){
+
+						 std::cout << " (after Insert people) (1) iit->get_prediction_trajectory( )->size()="<<iit->get_prediction_trajectory( )->size()<<"; iit->get_id()=" <<iit->get_id()<<"; iit->desired_vel="<<iit->get_desired_velocity()<<  std::endl;
+						 iit->get_current_pointV().print();
+						}
+					}*/
+		 // FIN INSERT PEOPLE FROM OTHER ROBOT-PERSON-COMPANION
+
+
+		 // VER SI PUEDO SUBIR EL UPDATE SCENE!!! QUE HACE FALTA ANTES DE CALC_person_companion_goal
+
+	/////////////////////////////////////////////////////////////////////////////////////////////////
+
+	return person_companion_plan_succed_;
+
+}
+
+
+
+
+
+/* update simulation persons. Companion person use the akp planner to navigate until the final destination. */
+// IMPORTANT: anterior function!!! Now is not used!!!
+void Cscene_sim::update_scene_companion_simulation(const std::vector<SdetectionObservation>& observation , bool ini_case)
+{
+	//caso_simulacion_giro_=true;
+
+	if(debug_person_obst_forces_){
+	std::cout << "IN update_scene_companion_simulation " << std::endl;
+	}
+	//the filter variable is not used in the simulated scene. All observations have been filtered.
+	double now = observation[0].time_stamp;
+	std::list<Cperson_abstract*>::iterator iit;
+
+	//deleting persons if needed, (LIFO)
+	/*while( person_list_.size() > number_virtual_people_ )
+	{
+		person_list_.pop_front();
+	}*/
+	//const std::vector<Sdestination>* destinations_test_;
+	//destinations_test_=get_destinations();
+	//std::cout << "destinations_test_.size()"<<destinations_test_->size()<< std::endl;
+	//generating virtual persons, if necessary (SOLO se generan al inicio!)
+	// hay que generar dos grupos, uno de 2 y uno de 3.
+	//std::cout << "antes ini_case " << std::endl;
+	double less_companion_velocity;//=0.3;//rand_normal(0.5,0.1);//0.4; // ANTES=0.4
+	double group_velocity1;//=0.3;//rand_normal(0.5,0.1);//rand_normal(1.0,0.1);
+	double group_velocity2;//=0.3
+
+	//if(experiments_<4){
+		less_companion_velocity=0.5;//rand_normal(0.5,0.1);//0.4; // ANTES=0.4
+		group_velocity1=0.3;//rand_normal(0.5,0.1);//rand_normal(1.0,0.1);
+		group_velocity2=0.3;//rand_normal(0.5,0.1);
+	//}else{
+	//	less_companion_velocity=rand_normal(0.5,0.1);//0.4; // ANTES=0.4
+	//	group_velocity1=rand_normal(0.5,0.1);//rand_normal(1.0,0.1);
+	//	group_velocity2=rand_normal(0.5,0.1);
+	//}
+
+
+	if(ini_case){
+
+		//destinations1p1_.clear();
+		//destinations1p1_.reserve(2);
+		//destinations1p1_.clear();
+		//destinations1p1_.reserve(2);
+		//destinations1p2_.clear();
+		//destinations1p2_.reserve(2);
+		//destinations2p3_.clear();
+		//destinations2p3_.reserve(2);
+		//destinations2p3_.clear();
+		//destinations2p3_.reserve(2);
+		//destinations2p4_.clear();
+		//destinations2p4_.reserve(2);
+		// possible destinations in the scene:
+		// dest 0=>  x=-1.0 ; y=1.0  (0, -1.0, 1.0, 0.25, 3, 1, 2, 3)
+		// dest 1=>  x=17.0 ; y=1.0 (1, 17.0, 1.0, 0.25, 3, 0, 2, 3)
+		// dest 2=>  x=17.0 ; y= -8.0 (2, 17.0, -8.0, 0.25, 3, 0, 1, 3)
+		// dest 3=>  x=-1.0 ; y= 8.0 (3, -1.0, 8.0, 0.25, 3, 0, 1, 2)
+
+		// generation grup 1 of persons. Goals=>
+		// dest 0=>  x=-1.0 ; y=1.0  (0, -1.0, 1.0, 0.25, 3, 1, 2, 3)
+		// dest 1=>  x=17.0 ; y=1.0 (1, 17.0, 1.0, 0.25, 3, 0, 2, 3)
+		Cperson_abstract* new_person;
+
+		//std::cout << "1" << std::endl;
+		Sdestination dest_1, dest_2, dest_10, dest_11, dest_12, dest_13,dest_14 ;
+		//std::cout << "2" << std::endl;
+		//double person_companion_velocity=0.4;//rand_normal(1.0,0.1);
+		dest_1 = get_destinations()->at( 8 );
+		dest_2 = get_destinations()->at( 9 );
+		//std::cout << "3" << std::endl;
+		dest_10 = get_destinations()->at( 10 );
+		dest_11 = get_destinations()->at( 11 );
+		//std::cout << "4" << std::endl;
+		dest_12 = get_destinations()->at( 12 );
+		dest_13 = get_destinations()->at( 13 );
+		//std::cout << "5" << std::endl;
+		dest_14 = get_destinations()->at( 14 );
+		//std::cout << "6" << std::endl;
+		destinations1_.clear();
+		destinations1_.push_back(dest_1);
+		destinations1_.push_back(dest_2);
+		//std::cout << "7" << std::endl;
+		destinations1_.push_back(dest_10);
+		destinations1_.push_back(dest_11);
+		//std::cout << "8" << std::endl;
+
+			destinations1_.push_back(dest_12);
+			destinations1_.push_back(dest_13);
+			//std::cout << "9" << std::endl;
+			destinations1_.push_back(dest_14);
+
+		//std::cout << "10" << std::endl;
+		new_person = add_person(1);
+
+		//std::cout << "(5)INI SET ALL DEST destinations1_.size() "<< destinations1_.size() << std::endl;
+		new_person->set_destinations(destinations1_);
+		//std::cout << "11" << std::endl;
+		double x, y;		//initial pose checks for free cells if an obstacle map is provided (no miro k sea prohivida, espacio sin obstaculos y puesta a mano)
+		x =dest_1.x ;
+		y =dest_1.y ;
+		new_person->add_pointV( SpointV_cov(x , y, now),  filtering_method_ );
+		new_person->set_desired_velocty( less_companion_velocity );
+		new_person->set_best_dest( new_person->get_destinations()->at( 1 ) );
+
+
+
+
+			//set initial position at random. initially 2 destinations
+			//std::cout << "12" << std::endl;
+			dest_1 = get_destinations()->at( 0 );
+			dest_2 = get_destinations()->at( 1 );
+			//std::cout << "13" << std::endl;
+			destinations1_.clear();
+			destinations1_.push_back(dest_1);
+			destinations1_.push_back(dest_2);
+
+			new_person = add_person(2);
+			new_person->set_destinations(destinations1_);
+			//new_person->set_destinations(destinations1p1_);
+
+			//double x, y;		//initial pose checks for free cells if an obstacle map is provided (no miro k sea prohivida, espacio sin obstaculos y puesta a mano)
+			x =dest_1.x ;
+			y =dest_1.y ;
+
+			new_person->add_pointV( SpointV_cov(x , y, now),  filtering_method_ );
+			new_person->set_desired_velocty( group_velocity1 );
+			new_person->set_best_dest( new_person->get_destinations()->at( 1 ) ); //coges la destinacion 1 de esta persona, porque ahora esta en la destinacion 0
+			//new_person->print_dest();
+			//new_person->print();
+			//std::cout << "14" << std::endl;
+			dest_1 = get_destinations()->at( 2 );
+			dest_2 = get_destinations()->at( 3 );
+			destinations1_.clear();
+			destinations1_.push_back(dest_1);
+			destinations1_.push_back(dest_2);
+
+			new_person = add_person(3);
+			new_person->set_destinations(destinations1_);
+			//new_person->set_destinations(destinations1p2_);
+			x =dest_1.x;
+			y =dest_1.y;
+			new_person->add_pointV( SpointV_cov(x , y, now),  filtering_method_ );
+			new_person->set_desired_velocty( group_velocity1 );
+			new_person->set_best_dest( new_person->get_destinations()->at( 1 ) ); //coges la destinacion 1 de esta persona, porque ahora esta en la destinacion 0
+			//new_person->print_dest();
+			//new_person->print();
+			//std::cout << "15" << std::endl;
+			ids_of_persons_in_group1_.push_back(1);  // person with id=1,2 are in the first group
+			ids_of_persons_in_group1_.push_back(2);
+
+			// generation grup 2 of persons. Goals=>
+			// dest 2=>  x=17.0 ; y= -8.0 (2, 17.0, -8.0, 0.25, 3, 0, 1, 3)
+			// dest 3=>  x=-1.0 ; y= 8.0 (3, -1.0, 8.0, 0.25, 3, 0, 1, 2)
+			//if(only_companion_person_){
+			Sdestination dest_3, dest_4;
+
+			//set initial position at random. initially 2 destinations
+			dest_3 = get_destinations()->at( 4 );
+			dest_4 = get_destinations()->at( 5 );
+			destinations2_.clear();
+			destinations2_.push_back(dest_3);
+			destinations2_.push_back(dest_4);
+
+
+			new_person = add_person(4);
+			new_person->set_destinations(destinations2_);
+			//new_person->set_destinations(destinations2p3_);
+			x = dest_3.x;  // a X_dist del goal 3, cada destination of person.
+			y = dest_3.y;
+			//std::cout << "16" << std::endl;
+			new_person->add_pointV( SpointV_cov(x , y, now),  filtering_method_ );
+			new_person->set_desired_velocty( group_velocity2 );
+			new_person->set_best_dest( new_person->get_destinations()->at( 1 ) );
+			//new_person->print_dest();
+			//new_person->print();
+
+			dest_3 = get_destinations()->at( 6 );
+			dest_4 = get_destinations()->at( 7 );
+			destinations2_.clear();
+			destinations2_.push_back(dest_3);
+			destinations2_.push_back(dest_4);
+
+			new_person = add_person(5);
+			new_person->set_destinations(destinations2_);
+			//new_person->set_destinations(destinations2p4_);
+			x = dest_3.x;  // a X_dist del goal 3, cada destination of person.
+			y = dest_3.y;
+			new_person->add_pointV( SpointV_cov(x , y, now),  filtering_method_ );
+			//set destination at random
+			new_person->set_desired_velocty( group_velocity2 );
+			new_person->set_best_dest( new_person->get_destinations()->at( 1 ) );
+			//new_person->print_dest();
+			//new_person->print();
+			//std::cout << "17" << std::endl;
+			ids_of_persons_in_group2_.push_back(3);  // person with id=3,4 are in the second group
+			ids_of_persons_in_group2_.push_back(4);
+		//}
+
+
+	}
+	//std::cout << "antes updating persons trajectories  " << std::endl;
+	//updating persons trajectories
+	double w;
+	SpointV_cov virtual_next_pose;
+	Sforce virtual_force_goal, virtual_force_int_person, virtual_force_obstacle, virtual_force_obstacle_laser_fake, virtual_force_robot;
+	//std::cout << "18" << std::endl;
+	for( iit = person_list_.begin() ; iit!=person_list_.end() ; iit++)
+	{
+		if ( ((*iit)->get_person_type() == Cperson_abstract::Person ) ||((*iit)->get_person_type() == Cperson_abstract::Person_companion))
+		{
+			if((*iit)->get_id()==1){
+				Sdestination  dest_1 = get_destinations()->at( 8 );
+				Sdestination  dest_2 = get_destinations()->at( 9 );
+				Sdestination  dest_10 = get_destinations()->at( 10 );
+				Sdestination  dest_11 = get_destinations()->at( 11 );
+				Sdestination  dest_12;
+				Sdestination  dest_13;
+				Sdestination  dest_14;
+				//if(caso_simulacion_giro_){
+					dest_12 = get_destinations()->at( 12 );
+					dest_13 = get_destinations()->at( 13 );
+					dest_14 = get_destinations()->at( 14 );
+				//}
+				destinations1_.clear();
+				destinations1_.push_back(dest_1);
+				destinations1_.push_back(dest_2);
+				destinations1_.push_back(dest_10);
+				destinations1_.push_back(dest_11);
+				//if(caso_simulacion_giro_){
+					destinations1_.push_back(dest_12);
+					destinations1_.push_back(dest_13);
+					destinations1_.push_back(dest_14);
+				//}
+				//	std::cout << "6 SET ALL DEST destinations1_.size() "<< destinations1_.size() << std::endl;
+				(*iit)->set_destinations(destinations1_);
+				(*iit)->set_desired_velocty( less_companion_velocity );
+			}
+			if(((*iit)->get_id()==2)||((*iit)->get_id()==3)){
+				if((*iit)->get_id()==2){
+					Sdestination  dest_1 = get_destinations()->at( 0 );
+					Sdestination  dest_2 = get_destinations()->at( 1 );
+					destinations1_.clear();
+					destinations1_.push_back(dest_1);
+					destinations1_.push_back(dest_2);
+					(*iit)->set_destinations(destinations1_);
+				}
+				if((*iit)->get_id()==3){
+					Sdestination  dest_1 = get_destinations()->at( 2 );
+					Sdestination  dest_2 = get_destinations()->at( 3 );
+					destinations1_.clear();
+					destinations1_.push_back(dest_1);
+					destinations1_.push_back(dest_2);
+					(*iit)->set_destinations(destinations1_);
+				}
+				//(*iit)->set_desired_velocty( group_velocity1);
+				if(random_final_dest_people_){
+					virtual_force_int_person = force_persons_int3( *iit ); // changed by (ely), para que no tenga en cuenta la fuerza repulsiva del compañero.
+				}else{
+					virtual_force_int_person = force_persons_int2( *iit ,ids_of_persons_in_group1_); // changed by (ely), para que no tenga en cuenta la fuerza repulsiva del compañero.
+				}
+
+
+			}
+			if(((*iit)->get_id()==4)||((*iit)->get_id()==5)){
+				if((*iit)->get_id()==4){
+					Sdestination  dest_3 = get_destinations()->at( 4 );
+					Sdestination  dest_4 = get_destinations()->at( 5 );
+					destinations2_.clear();
+					destinations2_.push_back(dest_3);
+					destinations2_.push_back(dest_4);
+					(*iit)->set_destinations(destinations2_);
+				}
+				if((*iit)->get_id()==5){
+					Sdestination  dest_3 = get_destinations()->at( 6 );
+					Sdestination  dest_4 = get_destinations()->at( 7 );
+					destinations2_.clear();
+					destinations2_.push_back(dest_3);
+					destinations2_.push_back(dest_4);
+					(*iit)->set_destinations(destinations2_);
+				}
+				//(*iit)->set_desired_velocty( group_velocity2);
+				if(random_final_dest_people_){
+					virtual_force_int_person = force_persons_int3( *iit ); // changed by (ely), para que no tenga en cuenta la fuerza repulsiva del compañero.
+				}else{
+					virtual_force_int_person = force_persons_int2( *iit ,ids_of_persons_in_group2_);
+				}
+
+
+			}
+
+			//1 time step propagation
+			virtual_force_goal = (*iit)->force_goal( (*iit)->get_best_dest(), this->get_sfm_params(*iit)  );
+			virtual_force_obstacle = get_force_map( (*iit)->get_current_pointV().x, (*iit)->get_current_pointV().y ) * 0.5;
+
+			// [ely] Inicio parte para evitar obstaculos del fake laser.
+			//if(read_laser_obstacle_success_)
+			//virtual_force_obstacle_laser_fake = force_objects_laser_int_planning_virtual( robot_, parent_vertex, 25.0, true );
+			Cperson_abstract* person=(*iit);
+
+			const SpointV_cov* virtual_current_point;
+			virtual_current_point = &(person->get_current_pointV());
+			double min_dist2=25.0;
+			virtual_force_obstacle_laser_fake=Sforce();
+			//SpointV person= (SpointV) (*iit)->get_current_pointV();
+			//std::cout<<"(simulated persons) out_laser_obstacle_list"<< std::endl;
+			if(((*iit)->get_id()==1)&&(debug_person_obst_forces_)){ // person companion
+				std::cout<<"[ini iter!!!] (SCENE_SIM) laser_obstacle_list_.size() ="<<laser_obstacle_list_.size()<< std::endl;
+				std::cout<<"[ini iter!!!] virtual_force_obstacle_laser_fake.fx ="<<virtual_force_obstacle_laser_fake.fx<<"; virtual_force_obstacle_laser_fake.fy="<<virtual_force_obstacle_laser_fake.fy<< std::endl;
+			}
+
+			//std::cout << "19" << std::endl;
+			for( Spoint iit2 : laser_obstacle_list_)
+			{
+				//std::cout<<"!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!11 (simulated persons) entro en laser_obstacle_list_ ="<< std::endl;
+				//there is a list for persons and another for robot(s). This function is for obstacles
+				double d2 = (*iit)->get_current_pointV().distance2( iit2 );
+
+				if( d2 < min_dist2)//square distance 5^2
+				{
+					//std::cout<<"(simulated persons) obstacle.x="<<iit2.x <<"; obstacle.y="<<iit2.y << std::endl;
+					//std::cout<<"(simulated persons) person.x="<<virtual_current_point->x<<"; person.y="<<virtual_current_point->y << std::endl;
+					std::cout << " 3 get_sfm_int_params" << std::endl;
+					//const std::vector<double>* params_robot=get_sfm_int_params(robot_);
+					std::vector<double> params_robot2;//=*params_robot;
+					params_robot2.push_back(2.3);  // esfm_k=2.3  valor anterior, para obstaculo central en medio de la trajectoria.
+					params_robot2.push_back(0.05); // esfm_to_robot_lambda=0.05 valor anterior, para obstaculo central en medio de la trajectoria.
+					params_robot2.push_back(3.05);	// esfm_to_robot_A=4.05 valor anterior, para obstaculo central en medio de la trajectoria.
+					params_robot2.push_back(0.51); // esfm_to_robot_B=0.81 valor anterior, para obstaculo central en medio de la trajectoria.
+					params_robot2.push_back(0.2); 	// esfm_d=0.2 valor anterior, para obstaculo central en medio de la trajectoria.
+					set_sfm_to_robot( params_robot2 );
+					virtual_force_obstacle_laser_fake += person->force_sphe( iit2 , &params_robot2, virtual_current_point); // usamos los parametros de la fuerza entre robot y obstaculos, porque los parametros de las personas hacen que le influyan demasiado los obstaculos y se paren, relenticen, no pasen entre ellos, etc.
+					//std::cout<<"(simulated persons) virtual_force_obstacle_laser_fake.fx ="<<virtual_force_obstacle_laser_fake.fx <<"; virtual_force_obstacle_laser_fake.fy ="<<virtual_force_obstacle_laser_fake.fy << std::endl;
+					if(((*iit)->get_id()==1)&&(debug_person_obst_forces_)){
+
+						/*Sforce act_final=person->force_sphe( iit2 , &params_robot2, virtual_current_point);
+						if(debug_real_test_companion_){
+							std::cout<<"(SIMULATION simulated persons, force for this obstacle)  params_robot->size()="<<params_robot->size()<< std::endl;
+							std::cout<<" [parametros actuales ]params_robot->at(0)="<<params_robot->at(0)<<"; params_robot->at(1)="<<params_robot->at(1)<<"; params_robot->at(2)="<<params_robot->at(2)<<"; params_robot->at(3)="<<params_robot->at(3)<<"; params_robot->at(4)="<<params_robot->at(4)<< std::endl;
+							std::cout<<" [parametros actuales ]params_robot2.at(0)="<<params_robot2.at(0)<<"; params_robot2.at(1)="<<params_robot2.at(1)<<"; params_robot2.at(2)="<<params_robot2.at(2)<<"; params_robot2.at(3)="<<params_robot2.at(3)<<"; params_robot2.>at(4)="<<params_robot2.at(4)<< std::endl;
+							std::cout<<"(simulated persons, force for this obstacle)  act_final.fx="<<act_final.fx<<"; act_final.fy="<<act_final.fy<< std::endl;
+						}*/
+
+					}
+				}
+			}
+			//std::cout << "20" << std::endl;
+			virtual_force_obstacle+=virtual_force_obstacle_laser_fake;
+			//if((*iit)->get_id()==id_person_companion_){
+			//	std::cout<<"(simulated persons, final force)  virtual_force_obstacle.fx="<<virtual_force_obstacle.fx<<"; virtual_force_obstacle.fy="<<virtual_force_obstacle.fy<< std::endl;
+			//}
+			// [ely] FIN parte para evitar obstaculos del fake laser.
+
+			// calculate the robot-person force specifically, if any robot in the scene
+			if ( robot_in_the_scene_ )
+			{
+				/* INI la fuerza robot hacia person_companion, cambia, solo es repulsiva cuando el robot esta muy cerca */
+				double robot_person_distance=calc_robot_person_companion_distance_sim();
+				if( ((*iit)->get_id()==id_person_companion_) && (robot_person_distance<(robot_person_proximity_distance_sim_-robot_person_proximity_tolerance_sim_))){
+					std::cout << " 4 get_sfm_int_params" << std::endl;
+					/*const std::vector<double>* params_robot=this->get_sfm_int_params(*iit, robot_);
+					if(debug_real_test_companion_){
+						std::cout<<"params_robot(0)="<<params_robot->at(0)<< std::endl;
+						std::cout<<"params_robot(1)="<<params_robot->at(1)<< std::endl;
+						std::cout<<"params_robot(2)="<<params_robot->at(2)<< std::endl;
+						std::cout<<"params_robot(3)="<<params_robot->at(3)<< std::endl;
+						std::cout<<"params_robot(4)="<<params_robot->at(4)<< std::endl;
+					}*/
+
+					std::vector<double> params_robot2;//=*params_robot;
+					params_robot2.push_back(4.5);  // [INICIAL era, esfm_k= 4.9. No se de donde lo coge Gonzalo, hay que investigar... ]esfm_k=2.3  valor anterior, para obstaculo central en medio de la trajectoria.
+					params_robot2.push_back(0.25); // [INICIAL era, esfm_to_robot_lambda= 1. No se de donde lo coge Gonzalo, hay que investigar... ] esfm_to_robot_lambda=0.05 valor anterior, para obstaculo central en medio de la trajectoria.
+					params_robot2.push_back(2.05);	// [INICIAL era, esfm_to_robot_A= 10. No se de donde lo coge Gonzalo, hay que investigar... ] esfm_to_robot_A=4.05 valor anterior, para obstaculo central en medio de la trajectoria.
+					params_robot2.push_back(0.5); // [INICIAL era, esfm_to_robot_B= 0.64. No se de donde lo coge Gonzalo, hay que investigar... ] esfm_to_robot_B=0.81 valor anterior, para obstaculo central en medio de la trajectoria.
+					params_robot2.push_back(0.3); 	// [INICIAL era, esfm_d= 0.16. No se de donde lo coge Gonzalo, hay que investigar... ] esfm_d=0.2 valor anterior, para obstaculo central en medio de la trajectoria.
+					set_sfm_to_robot( params_robot2 );
+
+					virtual_force_robot =  (*iit)->force( robot_->get_current_pointV() ,
+							&params_robot2   ) * 1;//TODO change parameters
+					//virtual_force_robot =  (*iit)->force( robot_->get_current_pointV() ,
+					//					this->get_sfm_int_params(*iit, robot_)   ) * 4;//TODO change parameters
+					//std::cout<< std::endl;
+					//std::cout<<"(simulated persons)  id_person_companion_="<<id_person_companion_<< std::endl;
+					//std::cout<<"(simulated persons)  virtual_force_robot.fx="<<virtual_force_robot.fx<<"; virtual_force_robot.fy"<<virtual_force_robot.fy<< std::endl;
+					//std::cout<< std::endl;
+					virtual_force_robot = Sforce();
+					//std::cout<<"(simulated persons)  robot in case person companion"<< std::endl;
+				}else if((*iit)->get_id()!=id_person_companion_){
+					//std::cout<<"(simulated persons)  robot in case other persons"<< std::endl;
+					//virtual_force_robot =  (*iit)->force( robot_->get_current_pointV() ,
+					//this->get_sfm_int_params(*iit, robot_)   ) * 4;//TODO change parameters
+					virtual_force_robot = Sforce();
+					//virtual_force_robot =  (*iit)->force( robot_->get_current_pointV() ,
+					//this->get_sfm_int_params(*iit, robot_)   ) * 1;//TODO change parameters
+
+				}
+				else{ // no aplica fuerza para esta persona.
+					virtual_force_robot = Sforce();
+					//std::cout<<"(simulated persons)  otros"<< std::endl;
+				}
+				/* FIN la fuerza robot hacia person_companion,, cambia, solo es repulsiva cuando el robot esta muy cerca */
+			}
+			else{
+				virtual_force_robot = Sforce();
+			}
+			//std::cout << "21" << std::endl;
+
+			//to avoid entering obstacles present in the scene, it simply does not propagate (solo evita obstaculos de mapa, no del fake laser.
+			w = 1.0;
+			do
+			{
+				(*iit)->set_forces_person( virtual_force_goal*w, virtual_force_int_person*w, virtual_force_robot*w, virtual_force_obstacle );
+				virtual_next_pose = (*iit)->pointV_propagation( dt_ , (*iit)->get_force_person()  );
+				w *= 0.9;
+			} while( read_force_map_success_ && !is_cell_clear_map( virtual_next_pose.x, virtual_next_pose.y ) && w > 0.1 );
+			virtual_next_pose.time_stamp = now;
+			(*iit)->add_pointV( virtual_next_pose );  // new pose for all the other persons!
+		}
+	}
+
+	//std::cout << "22" << std::endl;
+	//std::cout << "antes deleting complete trajectories  " << std::endl;
+	//deleting complete trajectories
+	//std::cout << "remove_targets_= "<<remove_targets_ << std::endl;
+	if ( remove_targets_ )
+	{
+		std::vector<unsigned int> targets_to_remove;
+		for(iit = person_list_.begin() ; iit!=person_list_.end() ; iit++)
+		{
+			if ( (((*iit)->get_person_type() == Cperson_abstract::Person )||((*iit)->get_person_type() == Cperson_abstract::Person_companion ))&&
+					((*iit)->get_current_pointV().distance( (Spoint)(*iit)->get_best_dest() ) < 1.5) )//destination achieved
+			{
+				targets_to_remove.push_back((*iit)->get_id());
+				number_virtual_people_--;
+			}
+		}
+		for ( unsigned int i = 0; i < targets_to_remove.size() ; ++i)
+			remove_person(targets_to_remove[i]);
+	}
+	//assigning new destinations once the goal is achieved
+	else
+	{
+
+		std::vector<unsigned int> targets_to_update;
+		//double group1_velocity=rand_normal(1.0,0.1);
+		//double group2_velocity=rand_normal(1.0,0.1);
+
+		for(iit = person_list_.begin() ; iit!=person_list_.end() ; iit++)
+		{
+			//std::cout << "antes if  " << std::endl;
+			if((*iit)->get_id()==1){
+				Sdestination  dest_1 = get_destinations()->at( 8 );
+				Sdestination  dest_2 = get_destinations()->at( 9 );
+				Sdestination  dest_10 = get_destinations()->at( 10 );
+				Sdestination  dest_11 = get_destinations()->at( 11 );
+				Sdestination  dest_12;
+				Sdestination  dest_13;
+				Sdestination  dest_14;
+				//if(caso_simulacion_giro_){
+					dest_12 = get_destinations()->at( 12 );
+					dest_13 = get_destinations()->at( 13 );
+					dest_14 = get_destinations()->at( 14 );
+				//}
+				destinations1_.clear();
+				destinations1_.push_back(dest_1);
+				destinations1_.push_back(dest_2);
+				destinations1_.push_back(dest_10);
+				destinations1_.push_back(dest_11);
+				//if(caso_simulacion_giro_){
+					destinations1_.push_back(dest_12);
+					destinations1_.push_back(dest_13);
+					destinations1_.push_back(dest_14);
+				//}
+				//	std::cout << "7 SET ALL DEST destinations1_.size() "<< destinations1_.size() << std::endl;
+				(*iit)->set_destinations(destinations1_);
+				(*iit)->set_desired_velocty( less_companion_velocity );
+				//experiments_++;
+			}
+			if(((*iit)->get_id()==2)||((*iit)->get_id()==3)){
+				if((*iit)->get_id()==2){
+					Sdestination  dest_1 = get_destinations()->at( 0 );
+					Sdestination  dest_2 = get_destinations()->at( 1 );
+					destinations1_.clear();
+					destinations1_.push_back(dest_1);
+					destinations1_.push_back(dest_2);
+					(*iit)->set_destinations(destinations1_);
+					//(*iit)->set_destinations(destinations1p1_);
+				}
+				if((*iit)->get_id()==3){
+					Sdestination  dest_1 = get_destinations()->at( 2 );
+					Sdestination  dest_2 = get_destinations()->at( 3 );
+					destinations1_.clear();
+					destinations1_.push_back(dest_1);
+					destinations1_.push_back(dest_2);
+					(*iit)->set_destinations(destinations1_);
+					//(*iit)->set_destinations(destinations1p2_);
+				}
+				//(*iit)->set_desired_velocty( group_velocity1 );
+			}
+			//std::cout << "medio if  " << std::endl;
+			if(((*iit)->get_id()==4)||((*iit)->get_id()==5)){
+				if((*iit)->get_id()==4){
+					Sdestination  dest_3 = get_destinations()->at( 4 );
+					Sdestination  dest_4 = get_destinations()->at( 5 );
+					destinations2_.clear();
+					destinations2_.push_back(dest_3);
+					destinations2_.push_back(dest_4);
+					(*iit)->set_destinations(destinations2_);
+					//(*iit)->set_destinations(destinations2p3_);
+				}
+				if((*iit)->get_id()==5){
+					Sdestination  dest_3 = get_destinations()->at( 6 );
+					Sdestination  dest_4 = get_destinations()->at( 7 );
+					destinations2_.clear();
+					destinations2_.push_back(dest_3);
+					destinations2_.push_back(dest_4);
+					(*iit)->set_destinations(destinations2_);
+					//(*iit)->set_destinations(destinations2p4_);
+				}
+				//(*iit)->set_desired_velocty( group_velocity2 );
+			}
+			//std::cout << "despues if  " << std::endl;
+			if ( (((*iit)->get_person_type() == Cperson_abstract::Person)||((*iit)->get_person_type() == Cperson_abstract::Person_companion)) &&
+					((*iit)->get_current_pointV().distance( (Spoint)(*iit)->get_best_dest() ) < 1.5) )//destination achieved
+			{
+				//set destination at random
+				//(*iit)->set_desired_velocty( rand_normal(1.0,0.1) );
+				std::vector<int> neighbours = (*iit)->get_best_dest().neighbours_ids;
+				int new_dest = neighbours[ rand_uniform_discrete( 0, neighbours.size()-1 )];
+
+				(*iit)->set_best_dest( destinations_[ new_dest ] );
+
+			}
+			//(*iit)->print_dest();
+			//(*iit)->print();
+			//std::cout << " person (*iit)->best_destination_"<<(*iit)->best_destination_<< std::endl;
+		}
+	}
+	//std::cout << "OUT c++  " << std::endl;
+	//std::cout << "FIN Cscen_sim amin library!!!" << std::endl;
+}
+
+
+void Cscene_sim::clear_scene()
+{
+	robot_->reset();
+	for( std::list<Cperson_abstract*>::iterator iit = person_list_.begin() ; iit != person_list_.end() ; iit++ )
+	{
+		delete *iit;
+	}
+	 person_list_.clear();
+}
+
+double Cscene_sim::calc_robot_person_companion_distance_sim(){
+
+	double robot_person_distance;
+	Spose act_robot_pose=robot_->get_current_pose();
+	//std::cout << "Initia, out, robot pose="<< std::endl;
+	//act_robot_pose.print();
+	//std::cout << "Initia, out, robot print="<< std::endl;
+	//robot_->print();
+	Spoint companion_person_position;
+	const std::list<Cperson_abstract *>* person_list = get_scene( );
+	for( std::list<Cperson_abstract*>::const_iterator iit = person_list->begin(); iit!=person_list->end(); iit++ )
+	{
+	 	if((*iit)->get_id()==id_person_companion_){
+	    	//Sdestination person_companion_goal=(*iit)->get_best_dest();
+	    	//person_companion_goal_=(*iit)->get_best_dest();
+	    	//const Spoint goal_robot(person_companion_goal.x,person_companion_goal.y+0.5,person_companion_goal.time_stamp);
+	    	//set_robot_goal(goal_robot);
+	    	//std::cout << "person_companion_goal=goal; x= "<< goal_robot.x << "; y="<<goal_robot.y<<"; time="<<goal_robot.time_stamp<< std::endl;
+	    	//companion_person_planning_trajectory_=(*iit)->get_planning_trajectory( );
+	    	companion_person_position=(Spoint) (*iit)->get_current_pointV();
+	    }
+	}
+	robot_person_distance=act_robot_pose.distance(companion_person_position);
+	//std::cout << "(calc_robot_person_companion_distance_sim()) robot_person_distance="<<robot_person_distance<< std::endl;
+	return robot_person_distance;//=act_robot_pose.distance(companion_person_position);
+}
+
+void Cscene_sim::update_pose_person_companion(Spose person_companion_act_pose){
+/*  this->planner_.read_laser_scan( this->laser_points );
+  //this->planner_.read_laser_scan_companion( this->laser_points );
+
+  //update robot position
+  this->odom_mutex_enter();
+  this->planner_.update_robot(this->robot_pose_);
+  this->odom_mutex_exit(); */
+	// std::vector<Spoint> laser; // =akp_for_person_companion_.get_laser_points.
+	//akp_for_person_companion_.read_laser_scan(laser); // ver como entrar los puntos laser.
+	//[parece que en teoria esta funcion ya lee también los laseres como la del planner.]
+	//SpointV_cov act_robot_pose=robot_->get_current_pointV();
+	//akp_for_person_companion_.update_robot(person_companion_act_pose); // hay que cambiarla, para conservar el robot. + ahora otro robot que es la person companión.
+	// además no la necesito, pq no necesito saber la pose actual, las estoy generando yo las poses...
+	// this->obs han de ser de las otras personas y el robot, las predicciones... (ojo... prediccion del robot)
+	// predicción del robot tendría que ser la trajectoria del robot actual, calculada por el akp.
+	// quizas haya que cogerla de ROS.
+
+	//std::vector<SdetectionObservation> obs;
+	//akp_for_person_companion_.update_scene(obs,this->we_have_companion_person_); // solo para las personas. (lo necesito?)El update_scene_del_nodo.=> esta en la libreria en Cprediction_bhmip
+	// el robot va a parte, cojo su trajectoria de ahí.(la person companion, en teoría no tiene en cuenta al robot... o si...!?)
+	// no la necesito, pq añade los puntos detectados ahora de la persona y en la de simulación, generas tu esos puntos.
+	// solo asegurarme que los generados de las personas ahora, se usen por la clase aquí.
+/*	std::list<Cperson_abstract*>::iterator iit;
+	bool companion_person_plan_succed;
+	Spose best_next_pose_person_companion;
+	Cperson_abstract::companion_reactive reactive_person_companion=Cperson_abstract::Person_companion_akp;
+
+	for( iit = person_list_.begin() ; iit!=person_list_.end() ; iit++)
+	{
+		if ( (*iit)->get_person_type() == Cperson_abstract::Person )
+		{
+			if((*iit)->get_id()==1){ // buscas la person companion!!!
+				 companion_person_plan_succed =  akp_for_person_companion_.robot_plan_companion2(best_next_pose_person_companion,reactive_person_companion);
+			} // reactive_!??? akp_companion, repulsive, atractive ? o nuevo: person_companion!?
+			// hay que crear un nuevo caso en el robot_plan_companion2 para este Cperson_abstract::Person_companion_akp y ver que cojer de alli del akp... (habra que cambiar el robot... o poner robot id!? puede k con el id, pueda filtrar los robots diferentes.)
+			// ojo, lo mismo es el modo de akp_planner_companion, no uno nuevo, pq querre los caminos + el coste companion, pero solo eso, la get_best_planned_pose_cambiara...
+			// quizas mejor no usar esta robot_plan_companion2, usar otra nueva, que solo tenga esto de la person companion...
+		}
+	}
+*/
+
+
+}
+
+
+/* Initial update scene for akp, planning, person companion*/
+void Cscene_sim::update_scene_for_person_companion_planning_akp(){ //hay que cambiar la posicion de la persona simulada,
+	// que corresponde a la 3 persona del grupo, antes de esta funcion, que hace el update_scene con las obs, que son los new_points_person_sim_
+
+	//std::cout << "update_scene_for_person_companion_planning_akp(); new_points_person_sim_.size()="<<new_points_person_sim_.size()<< std::endl;
+	//const std::list<Cperson_abstract *>* person_list = akp_for_person_companion_.get_scene( );
+	std::vector<SdetectionObservation> obs;
+	//akp_for_person_companion_.set_we_have_second_person_companion(false); // pones a false para los casos en que no entre en tenemos person companion!
+	//for( std::list<Cperson_abstract*>::const_iterator iit = person_list->begin(); iit!=person_list->end(); iit++ )
+	for(unsigned int l=0;l<new_points_person_sim_.size();l++)
+	{
+		std::vector<double> cov;
+		cov.resize(16,0.0);
+		/*cov[0] = (*iit)->get_current_pointV().cov[0];//x x
+		cov[1] = (*iit)->get_current_pointV().cov[1];//x y
+		cov[2] = (*iit)->get_current_pointV().cov[2];//x vx
+		cov[3] = (*iit)->get_current_pointV().cov[3];//x vy
+		cov[4] = (*iit)->get_current_pointV().cov[4];//x y
+		cov[5] = (*iit)->get_current_pointV().cov[5];//y y
+		cov[6] = (*iit)->get_current_pointV().cov[6];//y vx
+		cov[7] = (*iit)->get_current_pointV().cov[7];//y vy
+		cov[8] = (*iit)->get_current_pointV().cov[8];//vx x
+		cov[9] = (*iit)->get_current_pointV().cov[9];//vx y
+		cov[10] = (*iit)->get_current_pointV().cov[10];//vx vx
+		cov[11] = (*iit)->get_current_pointV().cov[11];//vx vy
+		cov[12] = (*iit)->get_current_pointV().cov[12];//vy x
+		cov[13] = (*iit)->get_current_pointV().cov[13];//vy y
+		cov[14] = (*iit)->get_current_pointV().cov[14];//vy vx
+		cov[15] = (*iit)->get_current_pointV().cov[15];//vy vy
+		double x=(*iit)->get_current_pointV().x;
+		double y=(*iit)->get_current_pointV().y;
+		double vx=(*iit)->get_current_pointV().vx;
+		double vy=(*iit)->get_current_pointV().vy;
+		int id=(*iit)->get_id();
+		double time=(*iit)->get_time();*/
+		cov = new_points_person_sim_[l].cov;//x x
+		double x=new_points_person_sim_[l].x;
+		double y=new_points_person_sim_[l].y;
+		double vx=new_points_person_sim_[l].vx;
+		double vy=new_points_person_sim_[l].vy;
+		unsigned int id=persons_ids_[l];
+		double time=new_points_person_sim_[l].time_stamp;
+
+		//std::cout << " other_companion_person_id_="<<other_companion_person_id_<<"; id="<<id<< std::endl;
+
+		/*if(group_simulation_){
+			if(id!=other_companion_person_id_){ // esta persona ya esta incluida en el modelo apartir del robot.
+				//std::cout << "if(id!=other_companion_person_id_) =>(update_scene_for_person_companion_planning_akp) id="<<id<< std::endl;
+				//if(id!=id_person_companion_){
+				obs.push_back(SdetectionObservation(id,time,x, y ,vx, vy, cov));
+			}else{
+				//std::cout << "lse_ del if ... other_companion_person_id_="<<other_companion_person_id_<< std::endl;
+				//std::cout << "else_ del if(id!=other_companion_person_id_) =>(update_scene_for_person_companion_planning_akp) id="<<id<< std::endl;
+				obs.push_back(SdetectionObservation(id,time,x, y ,vx, vy, cov));
+				//akp_for_person_companion_.set_we_have_second_person_companion(true);
+				//akp_for_person_companion_.set_second_group_companion_personSpoint(new_points_person_sim_[l]);
+				//akp_for_person_companion_.second_group_companion_personSpose(other_companion_person_out_odometry_spose_);
+				//akp_for_person_companion_.set_second_group_companion_personDest(destinations_other_person_companion_);
+				Cperson_abstract* person_obj;
+				bool finded_person=find_person(other_companion_person_id_ , &person_obj);
+				//std::cout << "else_ del if(id!=other_companion_person_id_) =>person_obj.pred_traj="<<person_obj->get_prediction_trajectory()->size()<<"; plan_traj="<<person_obj->get_planning_trajectory()->size()<< std::endl;
+
+				//akp_for_person_companion_.set_second_group_companion_object_person(person_obj);
+			}
+			//}
+
+		}else{*/
+			if(id!=id_person_companion_){
+				obs.push_back(SdetectionObservation(id,time,x, y ,vx, vy, cov));
+			}
+
+		//}
+
+
+	}
+
+
+
+	if(debug_update_scene_for_person_companion_planning_akp_){
+		std::cout << "(3333333333333 ini scene_sim) std::vector<SdetectionObservation> obs:"<< std::endl;
+		for( unsigned int ty = 0 ; ty< obs.size() ; ty++)
+		{
+			obs[ty].print();
+		}
+	}
+
+	bool have_companion_person=true;
+	/*std::cout << "1 update_scene_for_person_companion_planning_akp"<< std::endl;
+	std::list<Cperson_abstract*>::iterator iit;
+	for(iit = person_list_.begin() ; iit!=person_list_.end() ; iit++)
+			{
+				std::cout << " person_list_.id="<<(*iit)->get_id()<< std::endl;
+			}*/
+	//std::cout << "[????? antes update scen en Csene_SIM] akp_for_person_companion_.id_pers_comp="<<akp_for_person_companion_.get_id_person_companion()<< std::endl;
+
+	akp_for_person_companion_.update_scene(obs,have_companion_person, true);
+
+	/*std::cout << "2 update_scene_for_person_companion_planning_akp"<< std::endl;
+	for(iit = person_list_.begin() ; iit!=person_list_.end() ; iit++)
+			{
+				std::cout << " person_list_.id="<<(*iit)->get_id()<< std::endl;
+			}*/
+
+	//akp_for_person_companion_2_group_person_.update_scene(obs,have_companion_person, true); // este true, quizas tendría que ser para esta person companion o para el caso de tener un grupo.
+	/*std::cout << "3 update_scene_for_person_companion_planning_akp"<< std::endl;
+	for(iit = person_list_.begin() ; iit!=person_list_.end() ; iit++)
+			{
+				std::cout << " person_list_.id="<<(*iit)->get_id()<< std::endl;
+			}*/
+
+	if(debug_update_scene_for_person_companion_planning_akp_){
+		std::cout << "(ini scene_sim) akp_for_person_companion_.print():"<< std::endl;
+		akp_for_person_companion_.print();
+	}
+
+
+ }
+
diff --git a/local_lib_people_prediction/src/scene_sim.h b/local_lib_people_prediction/src/scene_sim.h
new file mode 100644
index 0000000000000000000000000000000000000000..8fe0c84d4376820d84c75518f175b51f8b6fa1ce
--- /dev/null
+++ b/local_lib_people_prediction/src/scene_sim.h
@@ -0,0 +1,433 @@
+/*
+ * scene_sim.h
+ *
+ *  Created on: Mar 27, 2013. Modified on 2016 (Final review on 2020)
+ *      Author: Initial AKP code Gonzalo Ferrer, addition of code and maintenance of companion Ely Repiso (ASSAOP and ASSAGP)
+ */
+
+#ifndef SCENE_SIM_H_
+#define SCENE_SIM_H_
+
+#include "scene_abstract.h"
+//#include "scene_elements/person_abstract.h"
+#include "scene_elements/person_bhmip.h"
+#include "iri_geometry.h"
+#include "nav/plan_local_nav_person_companion.h"
+
+/**
+ *
+ * \brief Cscene_simulated Class
+ *
+ * The scene class provides a set of methods to store, manage,
+ * get and set information of a simulated scene.
+ *
+ */
+class Cscene_sim : public Cscene_abstract
+{
+	public:
+
+	/*void set_id_person_goal(unsigned int in_id_person_goal){
+		id_person_goal_=in_id_person_goal;
+	}*/
+
+	void set_person_goal_init_poin(Spoint person_goal_init_point_in){
+		person_goal_init_point_=person_goal_init_point_in;
+	}
+    void set_CsceneSim_group_go_to_interact_with_other_person(bool in_group_go_to_interact_with_other_person){
+    	akp_for_person_companion_.set_plan_local_nav_group_go_to_interact_with_other_person(in_group_go_to_interact_with_other_person);
+    	//group_go_to_interact_with_other_person_=in_group_go_to_interact_with_other_person;
+    }
+
+    void set_ramdom_people_velocity(double in_ramdom_people_vel){
+    	std::cout << " set random people velocity="<<in_ramdom_people_vel<< std::endl;
+    	ramdom_people_vel_=in_ramdom_people_vel;
+    }
+
+    double ramdom_people_vel_;
+
+		Cscene_sim();
+		virtual ~Cscene_sim();
+		//needs a void observation for timestamp
+		virtual void update_scene(const std::vector<SdetectionObservation>& observation, bool& we_have_person_companion, bool person_or_robot=false );
+		void update_scene_companion_simulation(const std::vector<SdetectionObservation>& observation ,bool ini_case); // companion (ely)
+		bool update_scene_companion_simulation_akp_person_companion(const std::vector<SdetectionObservation>& observation , bool ini_case, double ini_theta_person_companion, Spose& pose_command);
+		void set_number_virtual_people( int number_people ) { number_virtual_people_ = number_people; }
+		void set_remove_targets( bool remove_targets){remove_targets_=remove_targets;}
+		virtual void clear_scene();
+		virtual Cperson_abstract* add_person_container( unsigned int id,
+				std::list<Cperson_abstract*>::iterator iit);
+		/*virtual Cperson_abstract* add_person_container( unsigned int id,
+				std::list<Cperson_abstract*>::iterator iit);*/
+		void set_person_companion_id(unsigned int in_id){id_person_companion_=in_id;};
+		//void set_person_companion_distance(double in_distance){person_robot_companion_distance_=in_distance;};
+		double calc_robot_person_companion_distance_sim();
+
+		/*void set_id_person_companion(int in_id_person_companion){
+			id_person_companion_=in_id_person_companion;
+		}*/
+
+	    void set_robot_person_proximity_distance(double in_proximity_distance_between_robot_and_person){
+	    	robot_person_proximity_distance_=in_proximity_distance_between_robot_and_person;
+	    	robot_person_proximity_distance_sim_=in_proximity_distance_between_robot_and_person;
+	    }
+	    void set_proximity_distance_tolerance(double in_proximity_distance_tolerance){
+	    	robot_person_proximity_tolerance_=in_proximity_distance_tolerance;
+	    	robot_person_proximity_tolerance_sim_=in_proximity_distance_tolerance;
+	    }
+
+
+	    Cplan_local_nav_person_companion* get_akp_for_person_companion(){
+	    //	std::cout << " IN get_akp_for_person_companion:"<< std::endl;
+	    //	akp_for_person_companion_.get_person_companion_akp()->get_best_dest().print();
+	   		return &akp_for_person_companion_;
+	   	}
+
+	    /*Cplan_local_nav_person_companion* get_akp_for_person_companion_2groupPers(){
+	   	    //	std::cout << " IN get_akp_for_person_companion:"<< std::endl;
+	   	    //	akp_for_person_companion_.get_person_companion_akp()->get_best_dest().print();
+	   	   		return &akp_for_person_companion_2_group_person_;
+	   	   	}*/
+
+
+	    Cperson_bhmip get_person_companion_person_abstract(){
+	    	//std::cout << " IN get_person_companion_person_abstract"<< std::endl;
+	    	//person_companion_person_abstract_.get_current_pointV().print();
+	    	//person_companion_person_abstract_.get_best_dest().print();
+
+	    	return person_companion_person_abstract_;
+	    }
+
+	    Cperson_bhmip get_person_companion_person_abstract_2group_person(){
+	    	//std::cout << " IN get_person_companion_person_abstract"<< std::endl;
+	    	//person_companion_person_abstract_.get_current_pointV().print();
+	    	//person_companion_person_abstract_.get_best_dest().print();
+	    	return person_companion_person_abstract_2group_person_;
+	    }
+
+
+
+	    int get_id_person_companion(){
+	   	    return companion_person_id_;
+	   	}
+	    void set_id_person_companion(int in_companion_person_id){
+	    	companion_person_id_=in_companion_person_id;
+	    	id_person_companion_=in_companion_person_id;
+	    }
+	   /* int get_id_person_companion_2group_person(){
+	   	    return id_person_companion_2group_pers_;
+	   	}*/
+
+		void update_pose_person_companion(Spose person_companion_act_pose);
+		void update_scene_for_person_companion_planning_akp();
+		void set_true_robot(Spose true_robot_pose){
+			true_robot_pose_=true_robot_pose;
+		}
+
+		Spose return_new_person_companion_pose(){
+			return new_person_companion_pose_;
+		}
+
+		bool return_person_companion_plan_succed(){
+			return person_companion_plan_succed_;
+		}
+
+
+		void set_initial_companion_person_robot_point(SpointV_cov in_initial_companion_person_robot_point){
+			initial_companion_person_robot_point_=in_initial_companion_person_robot_point;
+		}
+		void set_initial_companion_person_robot_point_2personGroup(SpointV_cov in_initial_companion_person_robot_point_2group_person){
+			initial_companion_person_robot_point_2group_person_=in_initial_companion_person_robot_point_2group_person;
+		}
+
+		void set_initial_companion_person_robot_Spose(Spose in_initial_companion_person_robot_Spose){
+			initial_companion_person_robot_Spose_=in_initial_companion_person_robot_Spose;
+		}
+
+		void set_initial_companion_person_robot_Spose_2personGroup(Spose in_initial_companion_person_robot_Spose_2personGroup){
+			initial_companion_person_robot_point_2group_person_Spose_=in_initial_companion_person_robot_Spose_2personGroup;
+		}
+
+
+
+	    void set_actual_goal_to_return_person_comp1( Spoint actual_goal_to_return_person_comp_in){
+	       	//std::cout << " final_combined_goal_.x="<<final_combined_goal_.x<<"; final_combined_goal_.y="<<final_combined_goal_.y<< std::endl;
+	    	actual_goal_to_return_person_comp1_=actual_goal_to_return_person_comp_in;
+	    }
+
+	    void set_actual_goal_to_return_person_comp2( Spoint actual_goal_to_return_person_comp_in){
+	       	//std::cout << " final_combined_goal_.x="<<final_combined_goal_.x<<"; final_combined_goal_.y="<<final_combined_goal_.y<< std::endl;
+	    	actual_goal_to_return_person_comp2_=actual_goal_to_return_person_comp_in;
+	    }
+
+	    Spoint get_actual_goal_to_return_person_comp( ){
+	       	//std::cout << " final_combined_goal_.x="<<final_combined_goal_.x<<"; final_combined_goal_.y="<<final_combined_goal_.y<< std::endl;
+	    	return actual_goal_to_return_person_comp_;
+	    }
+
+	    void set_state_Action_person_companion(Cplan_local_nav_person_companion::action_mode Action_in){
+	    	state_act_=Action_in;
+	    	std::cout << " (set_state_Action_person_companion) state_act_="<<state_act_<< std::endl;
+	       }
+
+
+	    void set_actual_simulation_case(Cplan_local_nav_person_companion::simulation_case  actual_sim_case_in){
+	   	       	//std::cout << " final_combined_goal_.x="<<final_combined_goal_.x<<"; final_combined_goal_.y="<<final_combined_goal_.y<< std::endl;
+	    	 actual_sim_case_= actual_sim_case_in;
+	   	}
+
+	    void insert_more_random_people(const std::vector<SdetectionObservation>& observation);
+
+	    // simulation of group of 2 person and robot.
+	    /*void set_other_companion_person_id(unsigned int in_other_companion_person_id){
+	    	std::cout << "CHANGE CHANGE !!! set_other_companion_person_id; ID="<<in_other_companion_person_id<< std::endl;
+
+	    	other_companion_person_id_=in_other_companion_person_id;
+	    	akp_for_person_companion_.set_id_SECOND_person_companion_people_simulator(other_companion_person_id_);
+	    }*/
+	    void set_other_companion_person_out_odometry_spoint(SpointV_cov in_other_companion_person_out_odometry_spoint){
+	    	other_companion_person_out_odometry_spoint_=in_other_companion_person_out_odometry_spoint;
+	    }
+
+
+	    // function for group of 2 people companion and one robot simulation:
+	    void set_tracks_other_person_companion_of_group(std::vector<SdetectionObservation> in_obs_other_person_companion){
+	    	obs_other_person_companion_=in_obs_other_person_companion;
+	    }
+
+	    void set_destinationsOFtracks_other_person_companion_of_group(std::vector<Sdestination> in_destinations_other_person_companion){
+	    	destinations_other_person_companion_=in_destinations_other_person_companion;
+	    	/*for(unsigned int d=0; d<destinations_other_person_companion_.size(); d++){
+	    	  	std::cout << " destinations_other_person_companion_[d].id="<<destinations_other_person_companion_[d].id<<"; destinations_other_person_companion_[d].x="<<destinations_other_person_companion_[d].x<<"; destinations_other_person_companion_[d].y="<<destinations_other_person_companion_[d].y<< std::endl;
+	    	}*/
+	    }
+
+
+	    void set_other_companion_person_out_odometry_spose(Spose in_other_companion_person_out_odometry_spose){
+	    	other_companion_person_out_odometry_spose_=in_other_companion_person_out_odometry_spose;
+	    }
+
+
+	    Sdestination CsceneSim_get_goal_path(){ // for visualization del simulador personas.
+	    	Sdestination actual_akp_out_path_goal=akp_for_person_companion_.akp_get_goal_path(); // ojo, ha de ser el akp_person_comp.
+	    	return actual_akp_out_path_goal;
+	    }
+
+	    Sdestination CsceneSim_get_goal_companion(){// for visualization del simulador personas.
+	    	Sdestination actual_akp_out_companion_goal=akp_for_person_companion_.akp_get_goal_companion();
+	    	return actual_akp_out_companion_goal;
+	    }
+
+	    void set_less_companion_velocity(double in_less_companion_velocity){
+	    	less_companion_velocity_=in_less_companion_velocity; //is the velocity of the person companion!
+	    }
+
+	    void set_horitzon_time_CsceneSim(double in_horitzon_time){
+	    	akp_for_person_companion_.set_horizon_time(in_horitzon_time);
+	    }
+
+	    void set_number_of_companion_people(unsigned int in_number_of_companion_people){
+	    	//akp_for_person_companion_.set_number_of_group_people(in_number_of_companion_people);
+	    	// in the Csim, this number now is the number of simulated people.
+	    	number_of_companion_people_=in_number_of_companion_people;
+	    }
+
+	    void set_id_second_companion(unsigned int in_id_second_companion){
+	    	//akp_for_person_companion_.set_number_of_group_people(in_number_of_companion_people);
+	    	// in the Csim, this number now is the number of simulated people.
+	    	id_second_companion_=in_id_second_companion;
+	    }
+
+	    void set_current_pose_robot(Spose in_current_pose_robot){
+	    	//std::cout<<" IIIIIINNNNN set_current_pose_robot(  ) "<< std::endl;
+	    	robot_->set_current_pose(in_current_pose_robot);
+	    	robot_->set_current_pointV(SpointV(in_current_pose_robot.x,in_current_pose_robot.y,in_current_pose_robot.time_stamp,in_current_pose_robot.v*cos(in_current_pose_robot.theta),in_current_pose_robot.v*sin(in_current_pose_robot.theta)));
+
+	    	//std::cout<<" 2 IIIIIINNNNN set_current_pose_robot(  ) "<< std::endl;
+	    	robot_->get_current_pose().print();
+	    	//std::cout<<" 3 IIIIIINNNNN set_current_pose_robot(  ) "<< std::endl;
+	    	akp_for_person_companion_.set_current_pose_robot_akp(in_current_pose_robot);
+
+
+
+	    	//std::cout<<" ouuuuuttt set_current_pose_robot(  ) "<< std::endl;
+	    }
+
+	    /*void set_Scene_create_fake_fixed_second_person_companion(bool in_Scene_create_fake_fixed_second_person_companion){
+	    	Scene_create_fake_fixed_second_person_companion_=in_Scene_create_fake_fixed_second_person_companion;
+	    	akp_for_person_companion_.set_plan_create_fake_fixed_second_person_companion(in_Scene_create_fake_fixed_second_person_companion);
+	    }*/
+
+	    SpointV_cov get_fake_second_person_companion(){
+	    	return SpointV_cov_fake_second_person_companion;
+	    }
+
+	    SpointV_cov SpointV_cov_fake_second_person_companion;
+
+	    unsigned int get_id_fake_second_person_companion(){
+	    	std::cout<<" get_id_fake_second_person_companion=> id_second_companion_= "<<id_second_companion_<< std::endl;
+	    	return id_second_companion_;
+	    }
+
+	    void set_p2_back_p1_with_static_obst(bool in_p2_back_p1_with_static_obst){
+	    	p2_back_p1_with_static_obst_=in_p2_back_p1_with_static_obst;
+	    }
+
+
+	protected:
+
+	    unsigned int number_virtual_people_, virtual_person_id_counter_;
+		bool remove_targets_;
+		unsigned int id_person_companion_; // (is my id, the id of the person that actually I'm acting as a robot )
+		unsigned int id_second_companion_;
+
+	    double group_velocity1_1_;
+		//std::vector<Sdestination> destinations2p4_;
+
+
+
+		// variables adder by ely to do groups of simulated persons.
+		std::vector<Sdestination> destinations1_, destinations2_;//, destinations2_, destinations1p1_, destinations1p2_;//, destinations2p3_, destinations2p4_; //ely variables to make companion groups.
+		//std::vector<Sdestination> destinations2_p4_;
+		//std::vector<Sdestination> destinations1p1_; //ely variables to make companion groups.
+		//std::vector<Sdestination> destinations1p2_; //ely variables to make companion groups.
+		//std::vector<Sdestination> destinations2p3_;
+		//std::vector<Sdestination> destinations2p4_;
+		std::vector<int> ids_of_persons_in_group1_,ids_of_persons_in_group2_;
+		//bool only_companion_person_;
+		//std::vector<Sdestination> destinations1p2_, destinations2p4_;
+
+		double person_robot_companion_distance_;
+		double robot_person_proximity_distance_sim_,robot_person_proximity_tolerance_sim_;
+		bool debug_person_obst_forces_;
+
+		double robot_person_proximity_distance_; // puede que no vayan en el simulador sino en el predictor. TODO: MIRAR!!!
+		double robot_person_proximity_tolerance_;
+
+		//bool debug_real_test_companion_;
+		//robot_ = new Crobot(0,Crobot::Differential,scene_force_type_);
+		//Crobot* person_companion_; //person_companion is in a different list than normal persons, and additionally we have its pointer for specific perosn_companion methods.
+		//This person is a person that plans a akp, to navigate in a more intelligent way and can made turns does not brusc.
+		Cplan_local_nav_person_companion akp_for_person_companion_; // object of the nav class to make the akp of the person_companion_
+		//Cplan_local_nav_person_companion akp_for_person_companion_2_group_person_; // se hace de otra forma y al final esto no se usa!
+
+
+
+		double scene_time_window_;
+		Spose new_person_companion_pose_;
+		unsigned int companion_person_id_;
+
+		Sdestination person_actual_best_dest_;
+		Cperson_bhmip person_companion_person_abstract_;
+		Cperson_bhmip person_companion_person_abstract_2group_person_;
+
+		std::vector<SpointV_cov> new_points_person_sim_;
+		std::vector<unsigned int> persons_ids_;
+		std::vector<Sdestination> person_best_dest_; // person_best_dest_[0]== best dest person 1,
+		std::vector<Sdestination> ant_person_dest_; // person_best_dest_[1]== best dest person 2, etc
+
+		std::vector<Sdestination> dest_person1_;
+		std::vector<Sdestination> dest_person2_3_;
+		std::vector<Sdestination> dest_person4_5_;
+
+		bool debug_init_scene_sim_; // debug for simulation person companion.
+		bool debug_general_scene_sim_; // debug for simulation person companion.
+		bool debug_other_persons_scene_sim_; // debug for simulation person companion.
+		bool debug_companion_person_scene_sim_; // debug for simulation person companion.
+		bool debug_update_scene_for_person_companion_planning_akp_; // debug for simulation person companion.
+
+		bool person_companion_plan_succed_;
+		bool person_companion_plan_succed2_;
+
+		SpointV_cov initial_companion_person_robot_point_;
+		SpointV_cov initial_companion_person_robot_point_2group_person_;
+		Spose initial_companion_person_robot_Spose_;
+		Spose initial_companion_person_robot_point_2group_person_Spose_;
+
+		bool first_;
+		Spoint before_group_position_;
+
+		bool stop_;   // el parar es ahora la unica que falta en el robot
+
+
+		double distance_to_stop_goal_person_; ///// ?????????????????
+
+
+		Cplan_local_nav_person_companion::action_mode state_act_;
+		Cplan_local_nav_person_companion::simulation_case actual_sim_case_;
+
+		Spoint actual_goal_to_return_person_comp1_;
+		Spoint actual_goal_to_return_person_comp2_;
+		Spoint actual_goal_to_return_person_comp_;
+		Spoint person_goal_init_point_; // es de la target person, pero no la puedo eliminar por el nodo compartido.
+
+		unsigned int goal1_;
+
+		Spoint ini_person_goal3_;
+		Spoint ini_person_goal4_;
+		bool caso_reinicio__grupos_personas_simulaciones_iguales_;
+
+		Spoint person3_goal_init_point_;
+		Spoint person4_goal_init_point_;
+		Spoint person5_goal_init_point_;
+		bool group_sincron_;
+
+		bool part1, part2;
+		unsigned int contatore_;
+
+		bool bool_select_rando_dest_person_companion_;
+		std::vector<Sdestination> destinations_ini_security_copy;
+		bool go_rear_ini_; // calculate goal to go rear the person companion to separate its position from the target person.
+		// save velocities to use the same vel for all the expeiment.
+		double less_companion_velocity_;
+		double group_velocity1_;
+		double group_velocity2_;
+		double group_velocity2_2_;
+
+		bool same_velocity_for_all_the_iteration_;
+		bool random_initia_pose_people_;
+		bool random_final_dest_people_;
+		bool all_complete_random_final_dest_people_;
+
+		double multiply_robot_force_to_persons_;
+		Spose true_robot_pose_;
+
+		bool debug_approach_people_random_;
+
+		// simulation of group of 3 people: (2 dabo robots + 1 tibi)
+		bool group_simulation_;
+		//unsigned int other_companion_person_id_; // TODO: este hay que setearlo desde el cfg. (second person of the group)
+		SpointV_cov other_companion_person_out_odometry_spoint_; // todo: hay que cambiar este. desde fuera hay que setearlo con la ultima odometria del robot!
+		Spose other_companion_person_out_odometry_spose_;
+		std::vector<SdetectionObservation> obs_other_person_companion_;
+		std::vector<peopleOtherPersonCompanionInGroup> people_ids_correspondence_OTHER_person_companion_;
+		bool firsTimeINTracksOtherPersonCompanion_; // initial true, if !obs_other_person_companion_.empty()=> first time of tracks.
+
+		std::vector<Sdestination> destinations_other_person_companion_;
+
+		bool people_sim_stop_;
+
+		unsigned int number_of_companion_people_; // is is 2, we generate a new person in X position from the first companion person.
+			// first aproximation of the companion for the group of three people.
+		Spose Spose_person_companion2_;
+		Spose Spose_before_person_companion_;
+
+
+		// nuevos goals, companion V-form con 2 robots.
+
+		Spoint actual_goal_to_return_person_comp1_id_1_;
+		Spoint actual_goal_to_return_person_comp1_id_3_;
+		Spoint actual_goal_to_return_person_comp2_id_1_;
+		Spoint actual_goal_to_return_person_comp2_id_3_;
+
+
+		bool Scene_create_fake_fixed_second_person_companion_;
+
+		Spoint SIM_initial_person_goal_pose1_,SIM_initial_person_goal_pose2_,SIM_initial_person_goal_pose3_;
+		double d_min_;
+		bool p2_back_p1_with_static_obst_;
+
+};
+
+
+
+#endif /* SCENE_SIM_H_ */
diff --git a/local_lib_people_prediction/src/test/CMakeLists.txt b/local_lib_people_prediction/src/test/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..08a71726b58dc47c9c6510b72e740f4021495007
--- /dev/null
+++ b/local_lib_people_prediction/src/test/CMakeLists.txt
@@ -0,0 +1,17 @@
+SET(Boost_USE_STATIC_LIBS ON)
+#set(Boost_USE_MULTITHREADED OFF)
+#set(Boost_USE_STATIC_RUNTIME OFF) 
+FIND_PACKAGE(Boost COMPONENTS unit_test_framework REQUIRED )
+LINK_DIRECTORIES ( ${Boost_LIBRARY_DIRS} )
+INCLUDE_DIRECTORIES( ${Boost_INCLUDE_DIRS})
+MESSAGE(STATUS "Boost_LIBRARIES=${Boost_LIBRARIES}")
+
+ADD_EXECUTABLE(test_geometry  test_geometry.cpp)
+TARGET_LINK_LIBRARIES(test_geometry people_prediction ${Boost_LIBRARIES})
+
+ADD_EXECUTABLE(test_person  test_person.cpp)
+TARGET_LINK_LIBRARIES(test_person people_prediction ${Boost_LIBRARIES})
+
+ADD_EXECUTABLE(test_scene  test_scene.cpp)
+TARGET_LINK_LIBRARIES(test_scene people_prediction ${Boost_LIBRARIES})
+
diff --git a/local_lib_people_prediction/src/test/test_geometry.cpp b/local_lib_people_prediction/src/test/test_geometry.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6f6d9919953aecead843537f9384bf47d68a544a
--- /dev/null
+++ b/local_lib_people_prediction/src/test/test_geometry.cpp
@@ -0,0 +1,225 @@
+/*
+ * test_Cperson.cpp
+ *
+ * TODO complete the test, it was done after the designe of the Cpersons classes
+ *
+ *  Created on: Oct 1, 2013
+ *      Author: gferrer
+ */
+
+#define BOOST_TEST_MODULE geometry_test
+#include <boost/test/unit_test.hpp>
+#include "iri_geometry.h"
+
+
+BOOST_AUTO_TEST_CASE( point )
+{
+    // testing if construction creates Spoint by default
+	Spoint p;
+    BOOST_CHECK( p.x == 0 && p.y == 0 && p.time_stamp == 0 );
+
+    Spoint p2 = Spoint( 1.0, 3.0, 54.3);
+	BOOST_CHECK( p2.x == 1.0 && p2.y == 3.0 && p2.time_stamp == 54.3 );
+
+	p = p2;
+	BOOST_CHECK( p.x == 1.0 && p.y == 3.0 && p.time_stamp == 54.3 );
+
+	Spoint p3 = p2-p;
+    BOOST_CHECK( p3.x == 0 && p3.y == 0 && p3.time_stamp == 54.3 );
+
+    p2 = p2 * (-1);
+    p3 = p2 + p;
+    BOOST_CHECK( p3.x == 0 && p3.y == 0 && p3.time_stamp == 54.3 );
+
+    p = Spoint( 5,0 );
+    BOOST_CHECK( p.distance( Spoint() ) == 5.0  );
+    BOOST_CHECK( p.distance( ) == 5.0  );
+
+    p = Spoint( 3,4 );
+	BOOST_CHECK( p.distance( Spoint() ) == 5.0  );
+	BOOST_CHECK( p.distance( ) == 5.0  );
+
+	p2 = p.propagate(0.1);
+	BOOST_CHECK( p2.distance(p) == 0.0  );
+
+	p2 = p.propagate(0.1,Sforce(), 1.0);
+	BOOST_CHECK( p2.distance(p) == 0.0  );
+}
+
+
+BOOST_AUTO_TEST_CASE( point_cov )
+{
+   // testing if construction creates Spoint by default
+	Spoint_cov p;
+	BOOST_CHECK( p.x == 0 && p.y == 0 && p.time_stamp == 0 );
+	BOOST_CHECK( p.cov[0] == 1.0 && p.cov[1] == 0.0 &&  p.cov[2] == 0 && p.cov[3] == 1.0);
+
+	p = Spoint_cov(1.0,2.0,0.0);
+	BOOST_CHECK( p.x == 1.0 && p.y == 2.0 && p.time_stamp == 0.0 );
+	BOOST_CHECK( p.cov[0] == 1.0 && p.cov[1] == 0 &&  p.cov[2] == 0.0 && p.cov[3] == 1.0);
+
+	//pass invalid covariance matrix (<4)
+	std::vector<double> cov(3,0.0);
+	p = Spoint_cov(1.0,2.0,0.0,cov);
+	BOOST_CHECK( p.x == 1.0 && p.y == 2.0 && p.time_stamp == 0.0 );
+	BOOST_CHECK( p.cov[0] == 1.0 && p.cov[1] == 0 &&  p.cov[2] == 0.0 && p.cov[3] == 1.0);
+
+	cov.resize(4,0.0);
+	cov[0]=3.0;
+	p = Spoint_cov(1.0,1.0,0.0,cov);
+	BOOST_CHECK( p.x == 1.0 && p.y == 1.0 && p.time_stamp == 0.0 );
+	BOOST_CHECK( p.cov[0] == 3.0 && p.cov[1] == 0 &&  p.cov[2] == 0.0 && p.cov[3] == 0);
+
+	//operators
+	Spoint_cov p2 = Spoint_cov(1.0,-1.0,0.0,cov);
+    Spoint_cov p3 = p2 + p;
+    BOOST_CHECK( p3.x == 2.0 && p3.y == 0.0 && p3.time_stamp == 0.0 );
+    BOOST_CHECK( p3.cov[0] == 3.0 && p3.cov[1] == 0 &&  p3.cov[2] == 0.0 && p3.cov[3] == 0);
+
+    p3 = p - p;
+    BOOST_CHECK( p3.x == 0.0 && p3.y == 0.0 && p3.time_stamp == 0.0 );
+    BOOST_CHECK( p3.cov[0] == 3.0 && p3.cov[1] == 0 &&  p3.cov[2] == 0.0 && p3.cov[3] == 0);
+
+    p = p*2.5;
+    BOOST_CHECK( p.x == 2.5 && p.y == 2.5 && p.time_stamp == 0.0 );
+
+
+	//distances
+    p = Spoint_cov(2.0,4.0,.0,cov);
+	p2 = Spoint_cov( 5,0 );
+    BOOST_CHECK( p2.distance( p ) == 5.0  );
+    Spoint p4 = (Spoint)p;
+    BOOST_CHECK( p2.distance2( p4 ) == 25.0  );
+    BOOST_CHECK( p2.distance( ) == 5.0  );
+
+    //covariances distances: default identity
+    BOOST_CHECK( p2.cov_dist( p ) == 25.0  );
+    BOOST_CHECK( p2.cov_dist( Spoint() ) == 25.0  );
+    BOOST_CHECK( p2.cov_dist(  ) == 25.0  );
+
+    //covariances distances: matrix cov
+    p2.cov[0]=2.0;
+    p2.cov[3]=2.0;
+    BOOST_CHECK( p2.cov_dist( p ) == 12.5  );
+    BOOST_CHECK( p2.cov_dist( Spoint() ) == 12.5  );
+    BOOST_CHECK( p2.cov_dist(  ) == 12.5  );
+
+
+}
+
+BOOST_AUTO_TEST_CASE( point_velocity )
+{
+    // testing if construction creates Spoint by default
+	SpointV p;
+    BOOST_CHECK( p.x == 0 && p.y == 0 && p.vx == 0 && p.vy == 0 && p.time_stamp == 0 );
+
+    SpointV p2 = SpointV( 1.0, 3.0, 54.3, 1.0, -1.0);
+	BOOST_CHECK( p2.x == 1.0 && p2.y == 3.0 );
+	BOOST_CHECK( p2.vx == 1.0 && p2.vy == -1.0);
+	BOOST_CHECK( p2.time_stamp == 54.3 );
+
+	p = p2;
+	BOOST_CHECK( p.x == 1.0 && p.y == 3.0 );
+	BOOST_CHECK( p.vx == 1.0 && p.vy == -1.0);
+	BOOST_CHECK( p.time_stamp == 54.3 );
+
+	SpointV p3 = p2-p;
+	BOOST_CHECK( p3.x == 0.0 && p3.y == 0.0 );
+	BOOST_CHECK( p3.vx == 0.0 && p3.vy == 0.0);
+	BOOST_CHECK( p3.time_stamp == 0.0 );
+
+    p2 = p2 * (-1);
+    //p2.Spoint::print();
+    //p2.print();
+    p3 = p2 + p;
+	BOOST_CHECK( p3.x == 0.0 && p3.y == 0.0 );
+	BOOST_CHECK( p3.vx == 0.0 && p3.vy == 0.0);
+	BOOST_CHECK( p3.time_stamp == 54.3 );
+
+    p = SpointV( 5,0 );
+    BOOST_CHECK( p.distance( SpointV() ) == 5.0  );
+    BOOST_CHECK( p.distance( ) == 5.0  );
+
+    p = SpointV( 3,4,0, 1,0 );
+	BOOST_CHECK( p.distance( SpointV() ) == 5.0  );
+	BOOST_CHECK( p.distance( ) == 5.0  );
+
+
+	p2 = p.propagate(1);
+	BOOST_CHECK( p2.x == 4 && p2.y == 4  );
+
+	p2 = p.propagate(1, Sforce(), 0.5);
+	BOOST_CHECK( p2.x == 3.5 && p2.y == 4  );
+
+	p2 = p.propagate(1, Sforce(1,0), 5.0 );
+	//p2.print();
+	BOOST_CHECK( p2.x == 4.5 && p2.vx == 2  );
+
+    //compatibility to other geometry classes
+    BOOST_CHECK( p.distance( Spoint() ) == 5.0  );
+}
+BOOST_AUTO_TEST_CASE( point_velocity_covariance )
+{
+	// constructor by default
+	SpointV_cov p;
+	BOOST_CHECK( p.x == 0 && p.y == 0 && p.time_stamp == 0 && p.vx == 0.0 &&p.vy==0.0);
+	BOOST_CHECK( p.cov[0] == 0.4 && p.cov[1] == 0 &&  p.cov[2] == 0 && p.cov[3] == 0 &&
+			     p.cov[4] == 0 && p.cov[5] == 0.4 &&  p.cov[6] == 0 && p.cov[6] == 0 &&
+			     p.cov[8] == 0 && p.cov[9] == 0 &&  p.cov[10] == 0.1 && p.cov[11] == 0 &&
+			     p.cov[12] == 0 && p.cov[13] == 0 &&  p.cov[14] == 0 && p.cov[15] == 0.1);
+
+	p = SpointV_cov(1.0,3.0,23.8,1.0,2.3);
+	BOOST_CHECK( p.x == 1.0 && p.y == 3.0 && p.time_stamp == 23.8 && p.vx == 1.0 &&p.vy==2.3);
+	BOOST_CHECK( p.cov[0] == 0.4 && p.cov[1] == 0 &&  p.cov[2] == 0 && p.cov[3] == 0 &&
+			     p.cov[4] == 0 && p.cov[5] == 0.4 &&  p.cov[6] == 0 && p.cov[6] == 0 &&
+			     p.cov[8] == 0 && p.cov[9] == 0 &&  p.cov[10] == 0.1 && p.cov[11] == 0 &&
+			     p.cov[12] == 0 && p.cov[13] == 0 &&  p.cov[14] == 0 && p.cov[15] == 0.1);
+
+	//invalid covariance
+    std::vector<double> cov(3,0.0);
+    p = SpointV_cov(1.0,3.0,23.8,1.0,2.3,cov);
+    BOOST_CHECK( p.x == 1.0 && p.y == 3.0 && p.time_stamp == 23.8 && p.vx == 1.0 &&p.vy==2.3);
+	BOOST_CHECK( p.cov[0] == 0.4 && p.cov[1] == 0 &&  p.cov[2] == 0 && p.cov[3] == 0 &&
+			     p.cov[4] == 0 && p.cov[5] == 0.4 &&  p.cov[6] == 0 && p.cov[6] == 0 &&
+			     p.cov[8] == 0 && p.cov[9] == 0 &&  p.cov[10] == 0.1 && p.cov[11] == 0 &&
+			     p.cov[12] == 0 && p.cov[13] == 0 &&  p.cov[14] == 0 && p.cov[15] == 0.1);
+
+    //valid covariance
+    cov.resize(16,0.0);
+    cov[0]=1.0;
+    cov[5]=10;
+    p = SpointV_cov(1.0,1.0,23.8,1.0,-1.0,cov);
+    BOOST_CHECK( p.cov[0] == 1.0 && p.cov[1] == 0 &&  p.cov[2] == 0 && p.cov[3] == 0 &&
+                 p.cov[4] == 0 && p.cov[5] == 10.0 &&  p.cov[6] == 0 && p.cov[6] == 0 &&
+                 p.cov[8] == 0 && p.cov[9] == 0 &&  p.cov[10] == 0.0 && p.cov[11] == 0 &&
+                 p.cov[12] == 0 && p.cov[13] == 0 &&  p.cov[14] == 0 && p.cov[15] == 0.0);
+
+    SpointV pp(1.0,1.0,23.8,1.0,-1.0);
+    p = SpointV_cov( pp, cov );
+    BOOST_CHECK( p.x == 1.0 && p.y == 1.0 && p.time_stamp == 23.8 && p.vx == 1.0 &&p.vy==-1.0);
+    BOOST_CHECK( p.cov[0] == 1.0 && p.cov[1] == 0 &&  p.cov[2] == 0 && p.cov[3] == 0 &&
+                     p.cov[4] == 0 && p.cov[5] == 10.0 &&  p.cov[6] == 0 && p.cov[6] == 0 &&
+                     p.cov[8] == 0 && p.cov[9] == 0 &&  p.cov[10] == 0.0 && p.cov[11] == 0 &&
+                     p.cov[12] == 0 && p.cov[13] == 0 &&  p.cov[14] == 0 && p.cov[15] == 0.0);
+
+
+    //operators
+    SpointV_cov p2 = p;
+    SpointV_cov p3 = p2-p;
+    BOOST_CHECK( p3.x == 0.0 && p3.y == 0.0 );
+    BOOST_CHECK( p3.vx == 0.0 && p3.vy == 0.0);
+    BOOST_CHECK( p3.time_stamp == 0.0 );
+
+    p2 = p2 * (-1);
+    //p2.Spoint::print();
+    //p2.print();
+    p3 = p2 + p;
+    BOOST_CHECK( p3.x == 0.0 && p3.y == 0.0 );
+    BOOST_CHECK( p3.vx == 0.0 && p3.vy == 0.0);
+    BOOST_CHECK( p3.time_stamp == 23.8 );
+
+    //check propagation
+    p = SpointV_cov(1.0,1.0,23.8,1.0,-1.0);
+    p = p.propagate( 1.0 );
+
+}
diff --git a/local_lib_people_prediction/src/test/test_person.cpp b/local_lib_people_prediction/src/test/test_person.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..3cdc0ba83caa77bc6927ceff13f63ea31783c4d1
--- /dev/null
+++ b/local_lib_people_prediction/src/test/test_person.cpp
@@ -0,0 +1,78 @@
+/*
+ * test_Cperson.cpp
+ *
+ * TODO complete the test, it was done after the designe of the Cpersons classes
+ *
+ *  Created on: Oct 1, 2013
+ *      Author: gferrer
+ */
+
+#define BOOST_TEST_MODULE Cperson_test
+#include <boost/test/unit_test.hpp>
+#include "scene_elements/person_abstract.h"
+#include "scene_elements/person_virtual.h"
+#include "scene_elements/person_bhmip.h"
+#include "scene_elements/person_behavior.h"
+
+//Alias in order to easily validate a new type of person
+typedef Cperson_behavior T;
+//typedef Cperson_virtual T;
+
+BOOST_AUTO_TEST_CASE( construction_and_setting )
+{
+	// testing constructor to created a person with id
+	unsigned int id = 0;
+    Cperson_abstract* person = new T( id );
+    BOOST_CHECK( person->get_id() == id );
+    delete person;
+
+    // testing if construction creates person of type Person
+    person = new T( id, Cperson_abstract::Person );
+    BOOST_CHECK( person->get_id() == id && person->get_person_type () == Cperson_abstract::Person);
+
+    // testing if construction creates person of type Robot
+    Cperson_abstract* person2 = new T( 1, Cperson_abstract::Robot , Cperson_abstract::Spherical);
+    BOOST_CHECK( person2->get_id() == 1 && person2->get_person_type () == Cperson_abstract::Robot);
+
+    // testing if operator != works fine (only for classes, not pointers!): different id
+    BOOST_CHECK( *person != *person2 );
+
+    // testing if operator != works fine: same id but different type
+	delete person2;
+	person2 = new T( id, Cperson_abstract::Robot );
+    BOOST_CHECK( *person != *person2 );
+
+    // testing if operator == works fine on itself
+    BOOST_CHECK( *person == *person );
+
+    // testing if operator == works fine: same id, same type but different force type
+    delete person2;
+    person2 = new T( id, Cperson_abstract::Person , Cperson_abstract::Spherical);
+    BOOST_CHECK( *person == *person2 );
+
+    delete person;
+    delete person2;
+}
+
+BOOST_AUTO_TEST_CASE( prediction )
+{
+    Cperson_abstract* person = new T( 0 );
+
+    // setting of destinations
+    std::vector<Sdestination> dests;
+    dests.push_back( Sdestination(1,10,0,0.25) );
+    dests.push_back( Sdestination(2,10,-10,0.25) );
+    dests.push_back( Sdestination(3,-10,-10,0.25) );
+    dests.push_back( Sdestination(4,-10,-10,0.25) );
+    person->set_destinations( dests );
+    BOOST_CHECK( person->get_destinations()->size() == 4 );
+
+    //propagation
+    person->add_pointV(SpointV_cov(0,0,0),Cperson_abstract::Linear_regression_filtering);
+    person->add_pointV(SpointV_cov(0.1,0,0.1),Cperson_abstract::Linear_regression_filtering);
+    person->add_pointV(SpointV_cov(0.2,0,0.2),Cperson_abstract::Linear_regression_filtering);
+    BOOST_CHECK( person->get_current_pointV().propagate(0.1).distance( Spoint(0.3,0) ) < 0.05 );
+
+
+    delete person;
+}
diff --git a/local_lib_people_prediction/src/test/test_scene.cpp b/local_lib_people_prediction/src/test/test_scene.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..54c63622bfe294d9e0ef4f99118982c7504bd465
--- /dev/null
+++ b/local_lib_people_prediction/src/test/test_scene.cpp
@@ -0,0 +1,106 @@
+/*
+ * test_Cperson.cpp
+ *
+ * A test for the scene type elements. TODO
+ *
+ *  Created on: Oct 1, 2013
+ *      Author: gferrer
+ */
+
+#define BOOST_TEST_MODULE test_scene
+#include <boost/test/unit_test.hpp>
+//#include "scene_elements/person_abstract.h"
+//#include "scene_elements/person_virtual.h"
+//#include "scene_elements/person_bhmip.h"
+#include "scene_abstract.h"
+#include "prediction_bhmip.h"
+#include "prediction_behavior.h"
+#include "scene_sim.h"
+
+//Alias in order to easily validate a new scene type
+
+BOOST_AUTO_TEST_CASE( real_scene )
+{
+	typedef Cprediction_bhmip T;
+	// testing constructor, no destinations neither map set
+	T scene;
+	scene.set_dt(0.1);
+	std::vector<SdetectionObservation> obs_scene;
+	obs_scene.push_back( SdetectionObservation(1, 0.0) );
+    Cperson_abstract* person;
+
+    //test propagation, 10 time steps
+    for( unsigned int i = 0; i<=10; ++i)
+    {
+		obs_scene[0].x = (double)i ;
+		obs_scene[0].time_stamp = (double)i * 0.1;
+		scene.update_scene( obs_scene );
+		if( scene.find_person(1,&person))
+			person->get_current_pointV().print();
+    }
+    //check find person
+    BOOST_CHECK( scene.find_person(1) );
+    BOOST_CHECK( !scene.find_person(7474) );
+    BOOST_CHECK( scene.find_person(1,&person) );
+
+    //check propagation
+    BOOST_CHECK_MESSAGE( person->get_current_pointV().distance( Spoint(10.0,0.0) ) < 0.05 ,
+    		"distance from current to Spoint(10,0) = " << person->get_current_pointV().distance( Spoint(10.0,0.0) ));
+    SpointV_cov prop = person->pointV_propagation( 0.1 );
+    prop.print();
+    //check if propagation is done correctly, error 0.5 m
+    BOOST_CHECK_MESSAGE( prop.distance( Spoint( 11,0 ) ) < 0.5 ,
+     		"distance from current to Spoint(11,0) = " << prop.distance( Spoint(11.0,0.0) ));
+
+    //check for size of container
+	const std::list<Cperson_abstract *>* person_list = scene.get_scene( );
+    BOOST_CHECK( person_list->size() == 1 );
+
+    //adding extra persons
+    obs_scene.push_back( SdetectionObservation(2, 0.0) );
+	scene.update_scene( obs_scene );
+	person_list = scene.get_scene( );
+	BOOST_CHECK( person_list->size() == 2 );
+	BOOST_CHECK( person_list->front()->get_id() == 1 );
+	BOOST_CHECK( person_list->back()->get_id() == 2 );
+
+    //removing persons
+	obs_scene.clear();
+    obs_scene.push_back( SdetectionObservation(2, 0.0) );
+    scene.update_scene( obs_scene );
+	person_list = scene.get_scene( );
+	BOOST_CHECK( person_list->size() == 1 );
+    BOOST_CHECK( scene.find_person(2) );
+    BOOST_CHECK( !scene.find_person(1) );
+
+    //ordering persons
+	obs_scene.clear();
+	obs_scene.push_back( SdetectionObservation(4, 0.0) );
+	obs_scene.push_back( SdetectionObservation(3, 0.0) );
+    obs_scene.push_back( SdetectionObservation(2, 0.0) );
+    scene.update_scene( obs_scene );
+	person_list = scene.get_scene( );
+	BOOST_CHECK( person_list->front()->get_id() == 2 );
+	BOOST_CHECK( person_list->back()->get_id() == 4 );
+
+    scene.update_scene( obs_scene );
+	person_list = scene.get_scene( );
+	BOOST_CHECK( person_list->size() == 3 );
+    BOOST_CHECK( scene.find_person(2) );
+    BOOST_CHECK( scene.find_person(3) );
+    BOOST_CHECK( scene.find_person(4) );
+	BOOST_CHECK( person_list->front()->get_id() == 2 );
+	BOOST_CHECK( person_list->back()->get_id() == 4 );
+
+    //read destination and map
+    BOOST_CHECK( scene.read_destination_map(
+    		"/home/gferrer/iri-lab/ros/iri-ros-pkg/iri_navigation/iri_people_simulation/map/map_destinations_learning.txt"  ) );
+    BOOST_CHECK( scene.read_force_map(
+    		"/home/gferrer/iri-lab/ros/iri-ros-pkg/iri_navigation/iri_people_simulation/map/force_map_sim.txt" ));
+}
+
+BOOST_AUTO_TEST_CASE( sim_scene )
+{
+
+
+}