Add a loader to test with rviz the SoT architecture.

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.

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"/>

src/geometric_simu.cpp

+/*
+ * 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();
+}

src/sot_loader.cpp

+/*
+ * 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_);
+}
+

src/sot_loader.hh

+/*
+ * 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);
+};
+