diff --git a/CMakeLists.txt b/CMakeLists.txt
index 0c77a6339ee47544005b5c7f89cb3e68fa10283a..cad95d1f39368a2df85c6bf4b66c271bef88ccde 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -93,6 +93,10 @@ pkg_config_use_dependency(interpreter sot-dynamic)
# set_target_properties(interpreter PROPERTIES BUILD_WITH_INSTALL_RPATH True)
#install(TARGETS interpreter DESTINATION bin)
+# Stand alone embedded intepreter with a robot controller.
+rosbuild_add_executable(geometric_simu src/geometric_simu.cpp src/sot_loader.cpp)
+pkg_config_use_dependency(interpreter sot-core)
+
add_subdirectory(src)
# Print warning.
diff --git a/manifest.xml b/manifest.xml
index 793c6a985bb59d222ab8522c99399a05b406883d..d00869b8c91451ceca208248b4d857a8ae11de90 100644
--- a/manifest.xml
+++ b/manifest.xml
@@ -26,6 +26,7 @@
<rosdep name="sot-dynamic"/>
<depend package="std_msgs"/>
+ <depend package="std_srvs"/>
<depend package="roscpp"/>
<depend package="geometry_msgs"/>
<depend package="sensor_msgs"/>
diff --git a/src/geometric_simu.cpp b/src/geometric_simu.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..1c51bc08126dfa01e41ce4f7a84d86fecd17c9b1
--- /dev/null
+++ b/src/geometric_simu.cpp
@@ -0,0 +1,47 @@
+/*
+ * Copyright 2011,
+ * Olivier Stasse,
+ *
+ * CNRS
+ *
+ * This file is part of dynamic_graph_bridge.
+ * dynamic_graph_bridge 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.
+ * dynamic_graph_bridge 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 dynamic_graph_bridge. If not, see <http://www.gnu.org/licenses/>.
+ */
+#include <iostream>
+
+#include "sot_loader.hh"
+
+bool SotLoader::start_dg(std_srvs::Empty::Request& request,
+ std_srvs::Empty::Response& response)
+{
+ for(unsigned int i=0;i<500;i++)
+ {
+ oneIteration();
+ }
+}
+
+
+int main(int argc, char *argv[])
+{
+ SotLoader aSotLoader;
+ if (aSotLoader.parseOptions(argc,argv)<0)
+ return -1;
+
+ ros::init(argc, argv, "start_dynamic_graph");
+ ros::NodeHandle n;
+
+ ros::ServiceServer service = n.advertiseService("start_dynamic_graph",
+ &SotLoader::start_dg,
+ &aSotLoader);
+ ROS_INFO("Ready to start dynamic graph.");
+ ros::spin();
+}
diff --git a/src/sot_loader.cpp b/src/sot_loader.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..42ebff851789f1c5c2aae15b65711c298af90a23
--- /dev/null
+++ b/src/sot_loader.cpp
@@ -0,0 +1,159 @@
+/*
+ * Copyright 2011,
+ * Olivier Stasse,
+ *
+ * CNRS
+ *
+ * This file is part of sot-core.
+ * sot-core 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.
+ * sot-core 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 sot-core. If not, see <http://www.gnu.org/licenses/>.
+ */
+/* -------------------------------------------------------------------------- */
+/* --- INCLUDES ------------------------------------------------------------- */
+/* -------------------------------------------------------------------------- */
+
+#include "sot_loader.hh"
+
+// POSIX.1-2001
+#include <dlfcn.h>
+
+using namespace std;
+using namespace dynamicgraph::sot;
+namespace po = boost::program_options;
+
+SotLoader::SotLoader():
+sensorsIn_ (),
+ controlValues_ (),
+ angleEncoder_ (),
+ angleControl_ (),
+ forces_ (),
+ torques_ (),
+ baseAtt_ (),
+ accelerometer_ (3),
+ gyrometer_ (3)
+{
+}
+
+int SotLoader::parseOptions(int argc, char *argv[])
+{
+ po::options_description desc("Allowed options");
+ desc.add_options()
+ ("help", "produce help message")
+ ("input-file", po::value<string>(), "library to load")
+ ;
+
+
+ po::store(po::parse_command_line(argc, argv, desc), vm_);
+ po::notify(vm_);
+
+ if (vm_.count("help")) {
+ cout << desc << "\n";
+ return -1;
+ }
+ if (!vm_.count("input-file")) {
+ cout << "No filename specified\n";
+ return -1;
+ }
+ else
+ dynamicLibraryName_= vm_["input-file"].as< string >();
+
+ Initialization();
+ return 0;
+}
+
+void SotLoader::Initialization()
+{
+
+ // Load the SotRobotBipedController library.
+ void * SotRobotControllerLibrary = dlopen( dynamicLibraryName_.c_str(),
+ RTLD_LAZY | RTLD_LOCAL );
+ if (!SotRobotControllerLibrary) {
+ std::cerr << "Cannot load library: " << dlerror() << '\n';
+ return ;
+ }
+
+ // reset errors
+ dlerror();
+
+ // Load the symbols.
+ createSotExternalInterface_t * createSot =
+ (createSotExternalInterface_t *) dlsym(SotRobotControllerLibrary,
+ "createSotExternalInterface");
+ const char* dlsym_error = dlerror();
+ if (dlsym_error) {
+ std::cerr << "Cannot load symbol create: " << dlsym_error << '\n';
+ return ;
+ }
+
+ // Create robot-controller
+ sotController_ = createSot();
+ // std::string s="libsot-hrp2-14-controller.so";
+ // sotController_->Initialization(dynamicLibraryName_);
+ cout <<"Went out from Initialization." << endl;
+}
+
+void
+SotLoader::fillSensors(map<string,dgs::SensorValues> & sensorsIn)
+{
+
+ // Update joint values.w
+ sensorsIn["joints"].setName("angle");
+ for(unsigned int i=0;i<angleControl_.size();i++)
+ angleEncoder_[i] = angleControl_[i];
+ sensorsIn["joints"].setValues(angleEncoder_);
+
+
+}
+
+void
+SotLoader::readControl(map<string,dgs::ControlValues> &controlValues)
+{
+ //static unsigned int nbit=0;
+
+ // Update joint values.
+ angleControl_ = controlValues["joints"].getValues();
+
+ /*
+ if (nbit%100==0)
+ std::cout << "Size of angles: " << angleControl_.size()
+ << " Size of mc->angle: " << mc->angle.length()
+ << std::endl;
+ */
+ /*
+ if (nbit%100==0)
+ std::cout << std::endl;
+ nbit++;
+ */
+
+}
+
+void SotLoader::setup()
+{
+ fillSensors(sensorsIn_);
+ sotController_->setupSetSensors(sensorsIn_);
+ sotController_->getControl(controlValues_);
+ readControl(controlValues_);
+}
+
+void SotLoader::oneIteration()
+{
+ fillSensors(sensorsIn_);
+
+ try
+ {
+ sotController_->nominalSetSensors(sensorsIn_);
+ sotController_->getControl(controlValues_);
+ }
+ catch(std::exception &e) { throw e;}
+
+ readControl(controlValues_);
+}
+
diff --git a/src/sot_loader.hh b/src/sot_loader.hh
new file mode 100644
index 0000000000000000000000000000000000000000..71882f33e8507ecd62cff20b6d7ddf10334be6cb
--- /dev/null
+++ b/src/sot_loader.hh
@@ -0,0 +1,89 @@
+/*
+ * Copyright 2011,
+ * Olivier Stasse,
+ *
+ * CNRS
+ *
+ * This file is part of sot-core.
+ * sot-core 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.
+ * sot-core 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 sot-core. If not, see <http://www.gnu.org/licenses/>.
+ */
+/* -------------------------------------------------------------------------- */
+/* --- INCLUDES ------------------------------------------------------------- */
+/* -------------------------------------------------------------------------- */
+
+// System includes
+#include <iostream>
+
+// STL includes
+#include <map>
+
+// Boost includes
+#include <boost/program_options.hpp>
+
+// ROS includes
+#include "ros/ros.h"
+#include "std_srvs/Empty.h"
+// Sot Framework includes
+#include <sot/core/debug.hh>
+#include <sot/core/abstract-sot-external-interface.hh>
+
+namespace po = boost::program_options;
+namespace dgs = dynamicgraph::sot;
+
+//typedef std::vector<double> SensorValues;
+//typedef std::vector<double> ControlValues;
+
+class SotLoader {
+
+protected:
+
+ /// \brief the sot-hrp2 controller
+ dgs::AbstractSotExternalInterface * sotController_;
+
+ po::variables_map vm_;
+ std::string dynamicLibraryName_;
+
+ /// Map of sensor readings
+ std::map <std::string,dgs::SensorValues> sensorsIn_;
+ /// Map of control values
+ std::map<std::string,dgs::ControlValues> controlValues_;
+ /// Angular values read by encoders
+ std::vector <double> angleEncoder_;
+ /// Angular values sent to motors
+ std::vector<double> angleControl_;
+ /// Forces read by force sensors
+ std::vector<double> forces_;
+ /// Torques
+ std::vector<double> torques_;
+ /// Attitude of the robot computed by extended Kalman filter.
+ std::vector<double> baseAtt_;
+ /// Accelerations read by Accelerometers
+ std::vector <double> accelerometer_;
+ /// Angular velocity read by gyrometers
+ std::vector <double> gyrometer_;
+
+public:
+ SotLoader();
+ ~SotLoader() {};
+
+ int parseOptions(int argc, char *argv[]);
+
+ void Initialization();
+ void oneIteration();
+
+ void fillSensors(std::map<std::string, dgs::SensorValues> & sensorsIn);
+ void readControl(std::map<std::string, dgs::ControlValues> & controlValues);
+ void setup();
+ bool start_dg(std_srvs::Empty::Request& request,
+ std_srvs::Empty::Response& response);
+};
+