src/tools/parameter-server.cpp

+/*
+ * Copyright 2014, Andrea Del Prete, LAAS-CNRS
+ *
+ * This file is part of sot-torque-control.
+ * sot-torque-control 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-torque-control 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-torque-control.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <iostream>
+#include <pinocchio/fwd.hpp>
+// keep pinocchio before boost
+
+#include <dynamic-graph/all-commands.h>
+#include <dynamic-graph/factory.h>
+
+#include <boost/property_tree/ptree.hpp>
+#include <sot/core/debug.hh>
+#include <sot/core/exception-tools.hh>
+#include <sot/core/parameter-server.hh>
+
+namespace dynamicgraph {
+namespace sot {
+namespace dynamicgraph = ::dynamicgraph;
+using namespace dynamicgraph;
+using namespace dynamicgraph::command;
+using namespace std;
+
+// Size to be aligned "-------------------------------------------------------"
+#define PROFILE_PWM_DESIRED_COMPUTATION \
+  "Control manager                                        "
+#define PROFILE_DYNAMIC_GRAPH_PERIOD \
+  "Control period                                         "
+
+#define INPUT_SIGNALS
+#define OUTPUT_SIGNALS
+
+/// Define EntityClassName here rather than in the header file
+/// so that it can be used by the macros DEFINE_SIGNAL_**_FUNCTION.
+typedef ParameterServer EntityClassName;
+
+/* --- DG FACTORY ---------------------------------------------------- */
+DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(ParameterServer, "ParameterServer");
+
+/* ------------------------------------------------------------------- */
+/* --- CONSTRUCTION -------------------------------------------------- */
+/* ------------------------------------------------------------------- */
+ParameterServer::ParameterServer(const std::string &name)
+    : Entity(name),
+      m_robot_util(RefVoidRobotUtil()),
+      m_initSucceeded(false),
+      m_emergency_stop_triggered(false),
+      m_is_first_iter(true),
+      m_iter(0),
+      m_sleep_time(0.0) {
+  //~ Entity::signalRegistration( INPUT_SIGNALS << OUTPUT_SIGNALS);
+
+  /* Commands. */
+  addCommand("init",
+             makeCommandVoid3(*this, &ParameterServer::init,
+                              docCommandVoid3("Initialize the entity.",
+                                              "Time period in seconds (double)",
+                                              "URDF file path (string)",
+                                              "Robot reference (string)")));
+  addCommand(
+      "init_simple",
+      makeCommandVoid1(*this, &ParameterServer::init_simple,
+                       docCommandVoid1("Initialize the entity.",
+                                       "Time period in seconds (double)")));
+
+  addCommand("setNameToId",
+             makeCommandVoid2(
+                 *this, &ParameterServer::setNameToId,
+                 docCommandVoid2("Set map for a name to an Id",
+                                 "(string) joint name", "(double) joint id")));
+
+  addCommand(
+      "setForceNameToForceId",
+      makeCommandVoid2(
+          *this, &ParameterServer::setForceNameToForceId,
+          docCommandVoid2(
+              "Set map from a force sensor name to a force sensor Id",
+              "(string) force sensor name", "(double) force sensor id")));
+
+  addCommand("setJointLimitsFromId",
+             makeCommandVoid3(
+                 *this, &ParameterServer::setJointLimitsFromId,
+                 docCommandVoid3("Set the joint limits for a given joint ID",
+                                 "(double) joint id", "(double) lower limit",
+                                 "(double) uppper limit")));
+
+  addCommand(
+      "setForceLimitsFromId",
+      makeCommandVoid3(
+          *this, &ParameterServer::setForceLimitsFromId,
+          docCommandVoid3("Set the force limits for a given force sensor ID",
+                          "(double) force sensor id", "(double) lower limit",
+                          "(double) uppper limit")));
+
+  addCommand(
+      "setJointsUrdfToSot",
+      makeCommandVoid1(*this, &ParameterServer::setJoints,
+                       docCommandVoid1("Map Joints From URDF to SoT.",
+                                       "Vector of integer for mapping")));
+
+  addCommand(
+      "setRightFootSoleXYZ",
+      makeCommandVoid1(*this, &ParameterServer::setRightFootSoleXYZ,
+                       docCommandVoid1("Set the right foot sole 3d position.",
+                                       "Vector of 3 doubles")));
+  addCommand(
+      "setRightFootForceSensorXYZ",
+      makeCommandVoid1(*this, &ParameterServer::setRightFootForceSensorXYZ,
+                       docCommandVoid1("Set the right foot sensor 3d position.",
+                                       "Vector of 3 doubles")));
+
+  addCommand("setFootFrameName",
+             makeCommandVoid2(
+                 *this, &ParameterServer::setFootFrameName,
+                 docCommandVoid2("Set the Frame Name for the Foot Name.",
+                                 "(string) Foot name", "(string) Frame name")));
+  addCommand("setHandFrameName",
+             makeCommandVoid2(
+                 *this, &ParameterServer::setHandFrameName,
+                 docCommandVoid2("Set the Frame Name for the Hand Name.",
+                                 "(string) Hand name", "(string) Frame name")));
+  addCommand("setImuJointName",
+             makeCommandVoid1(
+                 *this, &ParameterServer::setImuJointName,
+                 docCommandVoid1("Set the Joint Name wich IMU is attached to.",
+                                 "(string) Joint name")));
+  addCommand("displayRobotUtil",
+             makeCommandVoid0(
+                 *this, &ParameterServer::displayRobotUtil,
+                 docCommandVoid0("Display the current robot util data set.")));
+
+  addCommand(
+      "setParameterBool",
+      makeCommandVoid2(
+          *this, &ParameterServer::setParameter<bool>,
+          docCommandVoid2("Set a parameter named ParameterName to value "
+                          "ParameterValue (string format).",
+                          "(string) ParameterName", "(bool) ParameterValue")));
+  addCommand(
+      "setParameterInt",
+      makeCommandVoid2(
+          *this, &ParameterServer::setParameter<int>,
+          docCommandVoid2("Set a parameter named ParameterName to value "
+                          "ParameterValue (string format).",
+                          "(string) ParameterName", "(int) ParameterValue")));
+  addCommand("setParameterDbl",
+             makeCommandVoid2(
+                 *this, &ParameterServer::setParameter<double>,
+                 docCommandVoid2("Set a parameter named ParameterName to value "
+                                 "ParameterValue (string format).",
+                                 "(string) ParameterName",
+                                 "(double) ParameterValue")));
+
+  addCommand("setParameter",
+             makeCommandVoid2(
+                 *this, &ParameterServer::setParameter<std::string>,
+                 docCommandVoid2("Set a parameter named ParameterName to value "
+                                 "ParameterValue (string format).",
+                                 "(string) ParameterName",
+                                 "(string) ParameterValue")));
+
+  addCommand(
+      "getParameter",
+      makeCommandReturnType1(*this, &ParameterServer::getParameter<std::string>,
+                             docCommandReturnType1<std::string>(
+                                 "Return the parameter value for parameter"
+                                 " named ParameterName.",
+                                 "(string) ParameterName")));
+
+  addCommand(
+      "getParameterInt",
+      makeCommandReturnType1(
+          *this, &ParameterServer::getParameter<int>,
+          docCommandReturnType1<int>("Return the parameter value for parameter"
+                                     " named ParameterName.",
+                                     "(int) ParameterName")));
+
+  addCommand(
+      "getParameterDbl",
+      makeCommandReturnType1(*this, &ParameterServer::getParameter<double>,
+                             docCommandReturnType1<double>(
+                                 "Return the parameter value for parameter"
+                                 " named ParameterName.",
+                                 "(double) ParameterName")));
+
+  addCommand(
+      "getParameterBool",
+      makeCommandReturnType1(
+          *this, &ParameterServer::getParameter<bool>,
+          docCommandReturnType1<bool>("Return the parameter value for parameter"
+                                      " named ParameterName.",
+                                      "(string) ParameterName")));
+}
+
+void ParameterServer::init_simple(const double &dt) {
+  if (dt <= 0.0) return SEND_MSG("Timestep must be positive", MSG_TYPE_ERROR);
+
+  m_dt = dt;
+
+  m_emergency_stop_triggered = false;
+  m_initSucceeded = true;
+
+  std::string localName("robot");
+  std::shared_ptr<std::vector<std::string> > listOfRobots =
+      sot::getListOfRobots();
+
+  if (listOfRobots->size() == 1)
+    localName = (*listOfRobots)[0];
+  else {
+    std::ostringstream oss;
+    oss << "No robot registered in the parameter server list";
+    oss << " listOfRobots->size: " << listOfRobots->size();
+    throw ExceptionTools(ExceptionTools::ErrorCodeEnum::PARAMETER_SERVER,
+                         oss.str());
+  }
+
+  if (!isNameInRobotUtil(localName)) {
+    m_robot_util = createRobotUtil(localName);
+  } else {
+    m_robot_util = getRobotUtil(localName);
+  }
+
+  addCommand(
+      "getJointsUrdfToSot",
+      makeDirectGetter(*this, &m_robot_util->m_dgv_urdf_to_sot,
+                       docDirectSetter("Display map Joints From URDF to SoT.",
+                                       "Vector of integer for mapping")));
+}
+
+void ParameterServer::init(const double &dt, const std::string &urdfFile,
+                           const std::string &robotRef) {
+  if (dt <= 0.0) return SEND_MSG("Timestep must be positive", MSG_TYPE_ERROR);
+  m_dt = dt;
+  m_emergency_stop_triggered = false;
+  m_initSucceeded = true;
+
+  std::string localName(robotRef);
+  if (!isNameInRobotUtil(localName)) {
+    m_robot_util = createRobotUtil(localName);
+  } else {
+    m_robot_util = getRobotUtil(localName);
+  }
+
+  m_robot_util->m_urdf_filename = urdfFile;
+}
+
+/* ------------------------------------------------------------------- */
+/* --- SIGNALS ------------------------------------------------------- */
+/* ------------------------------------------------------------------- */
+
+/* --- COMMANDS ---------------------------------------------------------- */
+
+void ParameterServer::setNameToId(const std::string &jointName,
+                                  const double &jointId) {
+  if (!m_initSucceeded) {
+    SEND_WARNING_STREAM_MSG(
+        "Cannot set joint name from joint id  before initialization!");
+    return;
+  }
+  m_robot_util->set_name_to_id(jointName, static_cast<Index>(jointId));
+}
+
+void ParameterServer::setJointLimitsFromId(const double &jointId,
+                                           const double &lq, const double &uq) {
+  if (!m_initSucceeded) {
+    SEND_WARNING_STREAM_MSG(
+        "Cannot set joints limits from joint id  before initialization!");
+    return;
+  }
+
+  m_robot_util->set_joint_limits_for_id((Index)jointId, lq, uq);
+}
+
+void ParameterServer::setForceLimitsFromId(const double &jointId,
+                                           const dynamicgraph::Vector &lq,
+                                           const dynamicgraph::Vector &uq) {
+  if (!m_initSucceeded) {
+    SEND_WARNING_STREAM_MSG(
+        "Cannot set force limits from force id  before initialization!");
+    return;
+  }
+
+  m_robot_util->m_force_util.set_force_id_to_limits((Index)jointId, lq, uq);
+}
+
+void ParameterServer::setForceNameToForceId(const std::string &forceName,
+                                            const double &forceId) {
+  if (!m_initSucceeded) {
+    SEND_WARNING_STREAM_MSG(
+        "Cannot set force sensor name from force sensor id "
+        " before initialization!");
+    return;
+  }
+
+  m_robot_util->m_force_util.set_name_to_force_id(forceName,
+                                                  static_cast<Index>(forceId));
+}
+
+void ParameterServer::setJoints(const dynamicgraph::Vector &urdf_to_sot) {
+  if (!m_initSucceeded) {
+    SEND_WARNING_STREAM_MSG("Cannot set mapping to sot before initialization!");
+    return;
+  }
+  m_robot_util->set_urdf_to_sot(urdf_to_sot);
+}
+
+void ParameterServer::setRightFootSoleXYZ(const dynamicgraph::Vector &xyz) {
+  if (!m_initSucceeded) {
+    SEND_WARNING_STREAM_MSG(
+        "Cannot set right foot sole XYZ before initialization!");
+    return;
+  }
+
+  m_robot_util->m_foot_util.m_Right_Foot_Sole_XYZ = xyz;
+}
+
+void ParameterServer::setRightFootForceSensorXYZ(
+    const dynamicgraph::Vector &xyz) {
+  if (!m_initSucceeded) {
+    SEND_WARNING_STREAM_MSG(
+        "Cannot set right foot force sensor XYZ before initialization!");
+    return;
+  }
+
+  m_robot_util->m_foot_util.m_Right_Foot_Force_Sensor_XYZ = xyz;
+}
+
+void ParameterServer::setFootFrameName(const std::string &FootName,
+                                       const std::string &FrameName) {
+  if (!m_initSucceeded) {
+    SEND_WARNING_STREAM_MSG("Cannot set foot frame name!");
+    return;
+  }
+  if (FootName == "Left")
+    m_robot_util->m_foot_util.m_Left_Foot_Frame_Name = FrameName;
+  else if (FootName == "Right")
+    m_robot_util->m_foot_util.m_Right_Foot_Frame_Name = FrameName;
+  else
+    SEND_WARNING_STREAM_MSG("Did not understand the foot name !" + FootName);
+}
+
+void ParameterServer::setHandFrameName(const std::string &HandName,
+                                       const std::string &FrameName) {
+  if (!m_initSucceeded) {
+    SEND_WARNING_STREAM_MSG("Cannot set hand frame name!");
+    return;
+  }
+  if (HandName == "Left")
+    m_robot_util->m_hand_util.m_Left_Hand_Frame_Name = FrameName;
+  else if (HandName == "Right")
+    m_robot_util->m_hand_util.m_Right_Hand_Frame_Name = FrameName;
+  else
+    SEND_WARNING_STREAM_MSG(
+        "Available hand names are 'Left' and 'Right', not '" + HandName +
+        "' !");
+}
+
+void ParameterServer::setImuJointName(const std::string &JointName) {
+  if (!m_initSucceeded) {
+    SEND_WARNING_STREAM_MSG("Cannot set IMU joint name!");
+    return;
+  }
+  m_robot_util->m_imu_joint_name = JointName;
+}
+
+void ParameterServer::displayRobotUtil() { m_robot_util->display(std::cout); }
+
+/* --- PROTECTED MEMBER METHODS
+ * ---------------------------------------------------------- */
+
+bool ParameterServer::convertJointNameToJointId(const std::string &name,
+                                                unsigned int &id) {
+  // Check if the joint name exists
+  sot::Index jid = m_robot_util->get_id_from_name(name);
+  if (jid < 0) {
+    SEND_MSG("The specified joint name does not exist: " + name,
+             MSG_TYPE_ERROR);
+    std::stringstream ss;
+    for (long unsigned int it = 0; it < m_robot_util->m_nbJoints; it++)
+      ss << m_robot_util->get_name_from_id(it) << ", ";
+    SEND_MSG("Possible joint names are: " + ss.str(), MSG_TYPE_INFO);
+    return false;
+  }
+  id = (unsigned int)jid;
+  return true;
+}
+
+bool ParameterServer::isJointInRange(unsigned int id, double q) {
+  const JointLimits &JL = m_robot_util->get_joint_limits_from_id((Index)id);
+
+  double jl = JL.lower;
+  if (q < jl) {
+    SEND_MSG("Desired joint angle " + toString(q) +
+                 " is smaller than lower limit: " + toString(jl),
+             MSG_TYPE_ERROR);
+    return false;
+  }
+  double ju = JL.upper;
+  if (q > ju) {
+    SEND_MSG("Desired joint angle " + toString(q) +
+                 " is larger than upper limit: " + toString(ju),
+             MSG_TYPE_ERROR);
+    return false;
+  }
+  return true;
+}
+
+/* ------------------------------------------------------------------- */
+/* --- ENTITY -------------------------------------------------------- */
+/* ------------------------------------------------------------------- */
+
+void ParameterServer::display(std::ostream &os) const {
+  os << "ParameterServer " << getName();
+}
+}  // namespace sot
+}  // namespace dynamicgraph

src/tools/periodic-call-entity-python.h

+#include <sot/core/periodic-call-entity.hh>
+
+typedef boost::mpl::vector<dynamicgraph::sot::PeriodicCallEntity> entities_t;

src/tools/periodic-call-entity.cpp

+/*
+ * Copyright 2010,
+ * François Bleibel,
+ * Olivier Stasse,
+ *
+ * CNRS/AIST
+ *
+ */
+
+/* --------------------------------------------------------------------- */
+/* --- INCLUDE --------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+/* --- SOT --- */
+#include <dynamic-graph/pool.h>
+
+#include <sot/core/debug.hh>
+#include <sot/core/factory.hh>
+#include <sot/core/periodic-call-entity.hh>
+
+using namespace std;
+using namespace dynamicgraph::sot;
+
+DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(PeriodicCallEntity, "PeriodicCallEntity");
+
+/* --------------------------------------------------------------------- */
+/* --- CLASS ----------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+PeriodicCallEntity::PeriodicCallEntity(const string &fName)
+    : Entity(fName),
+      PeriodicCall(),
+      triger("Tracer(" + fName + ")::triger"),
+      trigerOnce("Tracer(" + fName + ")::trigerOnce") {
+  signalRegistration(triger << trigerOnce);
+
+  triger.setFunction(
+      boost::bind(&PeriodicCallEntity::trigerCall, this, _1, _2));
+  trigerOnce.setFunction(
+      boost::bind(&PeriodicCallEntity::trigerOnceCall, this, _1, _2));
+}
+
+int &PeriodicCallEntity::trigerCall(int &dummy, const int &time) {
+  run(time);
+  return dummy;
+}
+int &PeriodicCallEntity::trigerOnceCall(int &dummy, const int &time) {
+  run(time);
+  clear();
+  return dummy;
+}
+
+/* --------------------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+void PeriodicCallEntity::display(std::ostream &os) const {
+  os << "PeriodicCallEntity <" << name << "> ";
+  PeriodicCall::display(os);
+}

src/tools/periodic-call.cpp

-/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
- * Copyright Projet JRL-Japan, 2007
- *+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
- *
- * File:      PeriodicCall.cpp
- * Project:   SOT
- * Author:    Nicolas Mansard
- *
- * Version control
- * ===============
- *
- *  $Id$
- *
- * Description
- * ============
+/*
+ * Copyright 2010,
+ * François Bleibel,
+ * Olivier Stasse,
  *
+ * CNRS/AIST
  *
- * ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
-
+ */
 
 /* --------------------------------------------------------------------- */
 /* --- INCLUDE --------------------------------------------------------- */
 /* --------------------------------------------------------------------- */
 
 /* --- SOT --- */
-#include <sot-core/periodic-call.h>
+#include <dynamic-graph/all-commands.h>
+#include <dynamic-graph/exception-factory.h>
 #include <dynamic-graph/pool.h>
-#include <dynamic-graph/interpreter.h>
-#include <sot-core/debug.h>
+
+#include <algorithm>
+#include <sot/core/debug.hh>
+#include <sot/core/exception-tools.hh>
+#include <sot/core/periodic-call.hh>
 
 using namespace std;
 using namespace dynamicgraph;
-using namespace sot;
+using namespace dynamicgraph::sot;
 
 /* --------------------------------------------------------------------- */
 /* --- CLASS ----------------------------------------------------------- */
 /* --------------------------------------------------------------------- */
 
-
-
-PeriodicCall::
-PeriodicCall( void )
-  : signalMap()
-    ,cmdList()
-    ,innerTime( 0 )
-{
-
-}
-
+PeriodicCall::PeriodicCall(void) : signalMap(), innerTime(0) {}
 
 /* --------------------------------------------------------------------- */
 /* --------------------------------------------------------------------- */
 /* --------------------------------------------------------------------- */
-void PeriodicCall::
-addSignal( const std::string &name, SignalBase<int>& sig )
-{
-  signalMap[ name ] = &sig;
-  return ;
+void PeriodicCall::addSignal(const std::string &name, SignalBase<int> &sig) {
+  signalMap[name] = SignalToCall(&sig);
+  return;
 }
 
-void PeriodicCall::
-addSignal( istringstream& args )
-{
-  string signalName; args >> signalName;
-  istringstream sigISS( signalName );
-  SignalBase<int>& signal = g_pool .getSignal( sigISS );
-  addSignal( signalName,signal );
-  return ;
+void PeriodicCall::addSignal(const std::string &sigpath) {
+  istringstream sigISS(sigpath);
+  SignalBase<int> &signal =
+      ::dynamicgraph::PoolStorage::getInstance()->getSignal(sigISS);
+  addSignal(sigpath, signal);
+  return;
 }
 
-void PeriodicCall::
-rmSignal( const std::string &name )
-{
-  signalMap.erase( name );
-  return ;
+void PeriodicCall::addDownsampledSignal(
+    const std::string &name, SignalBase<int> &sig,
+    const unsigned int &downsamplingFactor) {
+  signalMap[name] = SignalToCall(&sig, downsamplingFactor);
+  return;
 }
 
-void PeriodicCall::
-rmSignal(  istringstream& args )
-{
-  string signalName; args >> signalName;
-  rmSignal( signalName );
-  return ;
+void PeriodicCall::addDownsampledSignal(
+    const std::string &sigpath, const unsigned int &downsamplingFactor) {
+  istringstream sigISS(sigpath);
+  SignalBase<int> &signal =
+      ::dynamicgraph::PoolStorage::getInstance()->getSignal(sigISS);
+  addDownsampledSignal(sigpath, signal, downsamplingFactor);
+  return;
 }
 
-
-
-/* --------------------------------------------------------------------- */
-/* --------------------------------------------------------------------- */
-/* --------------------------------------------------------------------- */
-void PeriodicCall::
-addCmd( istringstream& args )
-{
-  stringbuf* pbuf=args.rdbuf();
-  const unsigned int size = pbuf->in_avail();
-  char *buffer = new char[ size+1 ];
-  pbuf->sgetn( buffer,size );
-
-  buffer[size]='\0';
-  cmdList.push_back( buffer );
-  
-  delete buffer;
-  return ;
+void PeriodicCall::rmSignal(const std::string &name) {
+  signalMap.erase(name);
+  return;
 }
 
-void PeriodicCall::
-rmCmd( istringstream& args )
-{
-  stringbuf* pbuf=args.rdbuf();
-  const unsigned int size = pbuf->in_avail();
-  char * buffer = new char [size+1];
-  pbuf->sgetn( buffer,size );
-  buffer[size]='\0';
-  
-  CmdListType::iterator iter = cmdList.begin();
-  while( cmdList.end()!=iter )
-    {
-      const string& str = *iter;
-      if( 0==str.compare( 0,size,buffer ) )
-	{  iter = cmdList.erase( iter );	}
-      else { ++iter; }
-      //cout<<str << " <=> " << buffer << ": " << str.compare( 0,size,buffer )<<endl;
-    }
-	
-  delete buffer;
-  return ;
-}
-
-
-
 /* --------------------------------------------------------------------- */
 /* --------------------------------------------------------------------- */
 /* --------------------------------------------------------------------- */
-void PeriodicCall::
-runSignals( const int& t )
-{
-  for( SignalMapType::iterator iter = signalMap.begin();
-       signalMap.end()!=iter; ++iter )
-    {
-      (*iter).second ->recompute( t );
-    }
-  return ;
+void PeriodicCall::runSignals(const int &t) {
+  for (SignalMapType::iterator iter = signalMap.begin();
+       signalMap.end() != iter; ++iter) {
+    if (t % iter->second.downsamplingFactor == 0)
+      (*iter).second.signal->recompute(t);
+  }
+  return;
 }
 
-void PeriodicCall::
-runCmds( void )
-{
-  ostringstream onull; onull.clear( ios::failbit );
-
-  for( CmdListType::const_iterator iter = cmdList.begin();
-       cmdList.end()!=iter; ++iter )
-    {
-      istringstream iss( *iter );
-      string cmdName; iss >> cmdName;
-      g_shell.cmd( cmdName,iss,onull );
-    }
-  return ;
-}
-
-  void PeriodicCall::
-run( const int & t )
-{
-  runSignals( t ); runCmds();
-  return ;
+void PeriodicCall::run(const int &t) {
+  runSignals(t);
+  return;
 }
 
 /* --------------------------------------------------------------------- */
 /* --------------------------------------------------------------------- */
 /* --------------------------------------------------------------------- */
 
+void PeriodicCall::display(std::ostream &os) const {
+  os << "  (t=" << innerTime << ")" << endl;
 
-void PeriodicCall::
-display( std::ostream& os ) const
-{
-  
-
-  os <<"  (t=" << innerTime << ")" <<endl;
-
-  os<<" -> SIGNALS:"<<endl;
-  for( SignalMapType::const_iterator iter = signalMap.begin();
-       signalMap.end()!=iter; ++iter )
-    {
-      os << " - " << (*iter).first << endl;
-    }
-
-  os<<" -> CMDS:"<<endl;
-  for( CmdListType::const_iterator iter = cmdList.begin();
-       cmdList.end()!=iter; ++iter )
-    {
-      os << " - " <<  (*iter) << endl;
-    }
-
+  os << " -> SIGNALS:" << endl;
+  for (SignalMapType::const_iterator iter = signalMap.begin();
+       signalMap.end() != iter; ++iter) {
+    os << " - " << (*iter).first << endl;
+  }
 }
 
-
-
-
-bool PeriodicCall::
-commandLine( const std::string& cmdLine,
-	     std::istringstream& cmdArgs,
-	     std::ostream& os )
+/*
+static std::string readLineStr( istringstream& args )
 {
-  if( cmdLine == "help" )
-    {
-      os << "PeriodicCall:"<<endl
-	 <<"  - addSignal/rmSignal  <int> " <<endl
-	 <<"  - addCmd/rmCmd  " <<endl
-	 <<"  - runSignal/runCmd " <<endl
-	 <<"  - run" <<endl
-	 <<"  - setTime t " << endl;
-    }
-  else if( cmdLine == "addSignal" )
-    {      addSignal( cmdArgs );    }
-  else if( cmdLine == "rmSignal" )
-    {      rmSignal( cmdArgs );    }
-  else if( cmdLine == "runSignals" )
-    {      runSignals( innerTime++ );    }
-
-  else if( cmdLine == "addCmd" )
-    {      addCmd( cmdArgs );    }
-  else if( cmdLine == "rmCmd" )
-    {      rmCmd( cmdArgs );    }
-  else if( cmdLine == "runCmds" )
-    {      runCmds();    }
-
-  else if( cmdLine == "run" )
-    {      run( innerTime++ );    }
-  else if( cmdLine == "clear" )
-    {     clear();    }
-  else if( cmdLine == "print" ) 
-    { display(os) ; }
-  else { return false; }
-  return true;
-}
-
+  stringbuf* pbuf=args.rdbuf();
+  const std::streamsize size = pbuf->in_avail();
+  char * buffer = new char[ size+1 ];
+  pbuf->sgetn( buffer,size );
 
+  buffer[size]='\0';
+  std::string res( buffer );
+  delete [] buffer;
+  return res;
+}
+*/
 
 /*
  * Local variables:
  * c-basic-offset: 2
  * End:
  */
-

src/tools/robot-simu-python.h

+#include <sot/core/robot-simu.hh>
+
+typedef boost::mpl::vector<dynamicgraph::sot::RobotSimu> entities_t;

src/tools/robot-simu.cpp

-/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
- * Copyright Projet JRL-Japan, 2007
- *+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+/*
+ * Copyright 2010,
+ * Nicolas Mansard, Olivier Stasse, François Bleibel, Florent Lamiraux
  *
- * File:      RobotSimu.cpp
- * Project:   SOT
- * Author:    Nicolas Mansard
+ * CNRS
  *
- * Version control
- * ===============
- *
- *  $Id$
- *
- * Description
- * ============
- *
- *
- * ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
-
+ */
 
-/* --------------------------------------------------------------------- */
-/* --- INCLUDE --------------------------------------------------------- */
-/* --------------------------------------------------------------------- */
-
-/* SOT */
-#include <sot-core/robot-simu.h>
-#include <sot-core/debug.h>
-using namespace std;
+#include "sot/core/robot-simu.hh"
 
+#include <dynamic-graph/all-commands.h>
 #include <dynamic-graph/factory.h>
-using namespace sot;
-using namespace dynamicgraph;
-
-DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(RobotSimu,"RobotSimu");
-
-
-/* --------------------------------------------------------------------- */
-/* --- CLASS ----------------------------------------------------------- */
-/* --------------------------------------------------------------------- */
-
-
-RobotSimu::
-RobotSimu( const std::string& n )
-  :Entity(n)
-   ,state(6)
-   ,periodicCallBefore( )
-   ,periodicCallAfter( )
-   ,controlSIN( NULL,"RobotSimu("+n+")::input(double)::control" )
-  //,attitudeSIN(NULL,"RobotSimu::input(matrixRot)::attitudeIN")
-   ,attitudeSIN(NULL,"RobotSimu::input(vector3)::attitudeIN")
-   ,zmpSIN(NULL,"RobotSimu::input(vector3)::zmp")
-   ,stateSOUT( "RobotSimu("+n+")::output(vector)::state" )
-   ,attitudeSOUT( "RobotSimu("+n+")::output(matrixRot)::attitude" )
-   ,pseudoTorqueSOUT( "RobotSimu::output(vector)::ptorque" )
-   ,previousControlSOUT( "RobotSimu("+n+")::output(vector)::previousControl" )
-   ,motorcontrolSOUT( "RobotSimu("+n+")::output(vector)::motorcontrol" )
-   ,ZMPPreviousControllerSOUT( "RobotSimu("+n+")::output(vector)::zmppreviouscontroller" )
-{
-  /* --- FORCES --- */
-  for( int i=0;i<4;++i ){ withForceSignals[i] = false; }
-  forcesSOUT[0] =
-    new Signal<ml::Vector, int>("OpenHRP::output(vector6)::forceRLEG");
-  forcesSOUT[1] =
-    new Signal<ml::Vector, int>("OpenHRP::output(vector6)::forceLLEG");
-  forcesSOUT[2] =
-    new Signal<ml::Vector, int>("OpenHRP::output(vector6)::forceRARM");
-  forcesSOUT[3] =
-    new Signal<ml::Vector, int>("OpenHRP::output(vector6)::forceLARM");
-
-  signalRegistration( controlSIN<<stateSOUT<<attitudeSOUT<<attitudeSIN<<zmpSIN  
-		      <<*forcesSOUT[0]<<*forcesSOUT[1]<<*forcesSOUT[2]<<*forcesSOUT[3] 
-		      <<previousControlSOUT <<pseudoTorqueSOUT
-		      << motorcontrolSOUT << ZMPPreviousControllerSOUT );
-  state.fill(.0); stateSOUT.setConstant( state );
-}
-
-void RobotSimu::
-setStateSize( const unsigned int size )
-{
-  state.resize(size); state.fill( .0 ); 
-  stateSOUT .setConstant( state );
-  previousControlSOUT.setConstant( state );
-  pseudoTorqueSOUT.setConstant( state );
-  motorcontrolSOUT .setConstant( state );
-  
-  ml::Vector zmp(3); zmp.fill( .0 );
-  ZMPPreviousControllerSOUT .setConstant( zmp );
-}
-
-void RobotSimu::
-setState( const ml::Vector st )
-{
-  state = st; 
-  stateSOUT .setConstant( state );
-  motorcontrolSOUT .setConstant( state );
-}
-
-void RobotSimu::
-increment( const double dt )
-{
-  sotDEBUG(25) << "Time : " << controlSIN.getTime()+1 << std::endl;
-
-   periodicCallBefore.runSignals( controlSIN.getTime()+1 );
-   periodicCallBefore.runCmds();
-
-   stateSOUT .setConstant( state ); 
-  const ml::Vector control = controlSIN( controlSIN.getTime()+1 );
-
-  sotDEBUG(25) << "Cl" <<controlSIN.getTime()<<" = "
-	       << control*dt << ": " << control << endl;
-
-  sotDEBUG(25) << "St"<<state.size() << controlSIN.getTime() << ": " << state << endl;
-  for( unsigned int i=6;i<state.size();++i )
-    { state(i) += (control(i-6)*dt); }
-
-  sotDEBUG(25) << "St"<<state.size() << controlSIN.getTime() << ": " << state << endl;
-
-
-   ml::Vector forceNull(6); forceNull.fill(0);
-   for( int i=0;i<4;++i ){ 
-     if(  withForceSignals[i] ) forcesSOUT[i]->setConstant(forceNull); 
-   }
-
-  motorcontrolSOUT .setConstant( state );
-  
-  ml::Vector zmp(3); zmp.fill( .0 );
-  ZMPPreviousControllerSOUT .setConstant( zmp );
-
-
-   periodicCallAfter.runSignals( controlSIN.getTime() );
-   periodicCallAfter.runCmds();
-
-}
-
-
-
-/* --- DISPLAY ------------------------------------------------------------ */
-/* --- DISPLAY ------------------------------------------------------------ */
-/* --- DISPLAY ------------------------------------------------------------ */
-
-void RobotSimu::display ( std::ostream& os ) const
-{os <<name<<": "<<state<<endl; }
-
-
-/* --- PARAMS --------------------------------------------------------------- */
-/* --- PARAMS --------------------------------------------------------------- */
-/* --- PARAMS --------------------------------------------------------------- */
-#include <sot/sotPool.h>
-
-void RobotSimu::
-commandLine( const std::string& cmdLine
-	     ,std::istringstream& cmdArgs
-	     ,std::ostream& os )
-{
-  if( cmdLine=="help" )
-    {
-      os << "RobotSimu: "<<endl
-	 << "  - resize <uint size>"<<endl
-	 << "  - set <vector>"<<endl
-	 << "  - inc [<dt>]"<<endl;
-	Entity::commandLine( cmdLine,cmdArgs,os );
-    }
-  else if( cmdLine=="resize" )
-    {
-      unsigned int size; cmdArgs >> size;
-      setStateSize( size ); 
-    }
-  else if( cmdLine=="set" )
-    {
-      ml::Vector q; cmdArgs >> q;
-      setState( q );
-    }
-  else if( cmdLine == "pause" ) { os << "Not valid in simu" << endl; }
-  else if( cmdLine == "play" )  { os << "Not valid in simu" << endl; }
-  else if( cmdLine == "withForces" )
-  {
-    int index;
-    cmdArgs >> index;
-    if((index >= 0) && (index < 4))
-      {
-	std::string val;
-	cmdArgs >> val;
-	if ( ("1"==val)||("true"==val) )
-	  {
-	    withForceSignals[index] = true;
-	    ml::Vector forceNull(6); forceNull.fill(0);
-	    forcesSOUT[index]->setConstant(forceNull); 
-	  } else withForceSignals[index] = false;
-      }
-  }
-  else if( cmdLine == "whichForces" )
-  {
-    os << "Force signals: " << endl; 
-    for( unsigned int i=0;i<4;++i )
-      {
-	os << "\t- Force " << i << ": ";
-	if( withForceSignals[i] )
-	  { os << "Active";	  } else { os << "Inactive"; } 
-	os << endl;
-      }
-  }
-  else if( (cmdLine == "withPreviousControl")||(cmdLine == "withPseudoTorque") )
-  {
-    // Just for compatibility.
-  }
-  else if( cmdLine=="inc" )
-    {
-	increment();
 
-    }
-  else if( cmdLine=="time" )
-    {
-      os << "control.time = " << controlSIN.getTime() << endl;
-    }
-  else if(( cmdLine=="periodicCall" )||( cmdLine=="periodicCallAfter" ))
-    {
-      string cmd2; cmdArgs >> cmd2;
-      periodicCallAfter .commandLine( cmd2,cmdArgs,os );
-    }
-  else if( cmdLine=="periodicCallBefore" )
-    {
-      string cmd2; cmdArgs >> cmd2;
-      periodicCallBefore .commandLine( cmd2,cmdArgs,os );
-    }
-  else  
-    {
-      Entity::commandLine( cmdLine,cmdArgs,os );
-    }
+namespace dynamicgraph {
+namespace sot {
+DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(RobotSimu, "RobotSimu");
+
+RobotSimu::RobotSimu(const std::string &inName) : Device(inName) {
+  using namespace dynamicgraph::command;
+  std::string docstring;
+  /* Command increment. */
+  docstring =
+      "\n"
+      "    Integrate dynamics for time step provided as input\n"
+      "\n"
+      "      take one floating point number as input\n"
+      "\n";
+  addCommand("increment", command::makeCommandVoid1(
+                              (Device &)*this, &Device::increment, docstring));
+
+  /* Set Time step. */
+  docstring =
+      "\n"
+      "    Set the time step provided\n"
+      "\n"
+      "      take one floating point number as input\n"
+      "\n";
+  addCommand("setTimeStep",
+             makeDirectSetter(*this, &this->timestep_, docstring));
 }
+}  // namespace sot
+}  // namespace dynamicgraph

src/tools/robot-utils-py.cpp

+/*
+ * Copyright 2017, A. Del Prete, T. Flayols, O. Stasse, LAAS-CNRS
+ *
+ * This file is part of sot-core.
+ * See license file.
+ */
+
+#include <pinocchio/fwd.hpp>
+// keep pinocchio before boost
+
+#include <boost/property_tree/ptree.hpp>
+#include <boost/python.hpp>
+#include <boost/python/suite/indexing/map_indexing_suite.hpp>
+#include <sot/core/robot-utils.hh>
+
+using namespace boost::python;
+using namespace dynamicgraph::sot;
+
+BOOST_PYTHON_MODULE(robot_utils_sot_py) {
+  class_<JointLimits>("JointLimits", init<double, double>())
+      .def_readwrite("upper", &JointLimits::upper)
+      .def_readwrite("lower", &JointLimits::lower);
+
+  class_<ForceLimits>("ForceLimits",
+                      init<const Eigen::VectorXd &, const Eigen::VectorXd &>())
+      .def("display", &ForceLimits::display)
+      .def_readwrite("upper", &ForceLimits::upper)
+      .def_readwrite("lower", &ForceLimits::lower);
+
+  class_<ForceUtil>("ForceUtil")
+      .def("set_name_to_force_id", &ForceUtil::set_name_to_force_id)
+      .def("set_force_id_to_limits", &ForceUtil::set_force_id_to_limits)
+      .def("create_force_id_to_name_map",
+           &ForceUtil::create_force_id_to_name_map)
+      .def("get_id_from_name", &ForceUtil::get_id_from_name)
+      .def("get_name_from_id", &ForceUtil::cp_get_name_from_id)
+      .def("get_limits_from_id", &ForceUtil::cp_get_limits_from_id)
+      .def("get_force_id_left_hand", &ForceUtil::get_force_id_left_hand)
+      .def("set_force_id_left_hand", &ForceUtil::set_force_id_left_hand)
+      .def("get_force_id_right_hand", &ForceUtil::get_force_id_right_hand)
+      .def("set_force_id_right_hand", &ForceUtil::set_force_id_right_hand)
+      .def("get_force_id_left_foot", &ForceUtil::get_force_id_left_foot)
+      .def("set_force_id_left_foot", &ForceUtil::set_force_id_left_foot)
+      .def("get_force_id_right_foot", &ForceUtil::get_force_id_right_foot)
+      .def("set_force_id_right_foot", &ForceUtil::set_force_id_right_foot)
+      .def("display", &ForceUtil::display);
+
+  class_<RobotUtil>("RobotUtil")
+      .def_readwrite("m_force_util", &RobotUtil::m_force_util)
+      .def_readwrite("m_foot_util", &RobotUtil::m_foot_util)
+      .def_readwrite("m_urdf_to_sot", &RobotUtil::m_urdf_to_sot)
+      .def_readonly("m_nbJoints", &RobotUtil::m_nbJoints)
+      .def_readwrite("m_name_to_id", &RobotUtil::m_name_to_id)
+      .def_readwrite("m_id_to_name", &RobotUtil::m_id_to_name)
+      .def("set_joint_limits_for_id", &RobotUtil::set_joint_limits_for_id)
+      .def("get_joint_limits_from_id", &RobotUtil::cp_get_joint_limits_from_id)
+      //.def("set_joint_limits_for_id",&RobotUtil::set_joint_limits_for_id)
+      //.def("set_name_to_id", &RobotUtil::set_name_to_id)
+      //.def("create_id_to_name_map",&RobotUtil::create_id_to_name_map)
+      //.def("get_id_from_name",&RobotUtil::get_id_from_name)
+      ;
+
+  class_<std::map<Index, ForceLimits> >("IndexForceLimits")
+      .def(map_indexing_suite<std::map<Index, ForceLimits> >());
+
+  class_<std::map<std::string, Index> >("stringIndex")
+      .def(map_indexing_suite<std::map<std::string, Index> >());
+
+  class_<std::map<Index, std::string> >("Indexstring")
+      .def(map_indexing_suite<std::map<Index, std::string> >());
+}

src/tools/robot-utils.cpp

+/*
+ * Copyright 2017, 2019
+ * LAAS-CNRS
+ * A. Del Prete, T. Flayols, O. Stasse, F. Bailly
+ *
+ */
+
+/** pinocchio is forcing the BOOST_MPL_LIMIT_VECTOR_SIZE to a specific value.
+    This happen to be not working when including the boost property_tree
+   library. For this reason if defined, the current value of
+   BOOST_MPL_LIMIT_VECTOR_SIZE is saved in the preprocessor stack and unset.
+    Once the property_tree included the pinocchio value of this variable is
+    restored.
+ */
+#ifdef BOOST_MPL_LIMIT_VECTOR_SIZE
+#pragma push_macro("BOOST_MPL_LIMIT_VECTOR_SIZE")
+#undef BOOST_MPL_LIMIT_VECTOR_SIZE
+#include <boost/property_tree/ptree.hpp>
+#include <boost/property_tree/xml_parser.hpp>
+#pragma pop_macro("BOOST_MPL_LIMIT_VECTOR_SIZE")
+#else
+#include <boost/property_tree/ptree.hpp>
+#include <boost/property_tree/xml_parser.hpp>
+#endif
+
+#include <dynamic-graph/factory.h>
+
+#include <iostream>
+#include <sot/core/debug.hh>
+#include <sot/core/robot-utils.hh>
+#include <sstream>
+
+namespace dynamicgraph {
+namespace sot {
+namespace dg = ::dynamicgraph;
+namespace pt = boost::property_tree;
+using namespace dg;
+using namespace dg::command;
+
+ForceLimits VoidForceLimits;
+JointLimits VoidJointLimits;
+Index VoidIndex(-1);
+
+RobotUtilShrPtr RefVoidRobotUtil() {
+  return std::shared_ptr<RobotUtil>(nullptr);
+}
+
+ExtractJointMimics::ExtractJointMimics(std::string &robot_model) {
+  // Parsing the model from a string.
+  std::istringstream iss(robot_model);
+  /// Read the XML file in the property tree.
+  boost::property_tree::read_xml(iss, tree_);
+  /// Start the recursive parsing.
+  go_through_full();
+}
+
+const std::vector<std::string> &ExtractJointMimics::get_mimic_joints() {
+  return mimic_joints_;
+}
+
+void ExtractJointMimics::go_through_full() {
+  /// Root of the recursive parsing.
+  current_joint_name_ = "";
+  go_through(tree_, 0, 0);
+}
+
+void ExtractJointMimics::go_through(pt::ptree &pt, int level, int stage) {
+  /// If pt is empty (i.e. this is a leaf)
+  if (pt.empty()) {
+    /// and this is a name of a joint (stage == 3) update the
+    /// curret_joint_name_ variable.
+    if (stage == 3) current_joint_name_ = pt.data();
+  } else {
+    /// This is not a leaf
+    for (auto pos : pt) {
+      int new_stage = stage;
+
+      /// But this is joint
+      if (pos.first == "joint")
+        /// the continue the exploration.
+        new_stage = 1;
+      else if (pos.first == "<xmlattr>") {
+        /// we are exploring the xml attributes of a joint
+        /// -> continue the exploration
+        if (stage == 1) new_stage = 2;
+      }
+      /// The xml attribute of the joint is the name
+      /// next leaf is the name we are possibly looking for
+      else if (pos.first == "name") {
+        if (stage == 2) new_stage = 3;
+      }
+      /// The exploration of the tree tracback on the joint
+      /// and find that this is a mimic joint.
+      else if (pos.first == "mimic") {
+        if (stage == 1)
+          /// Save the current name of the joint
+          /// in mimic_joints.
+          mimic_joints_.push_back(current_joint_name_);
+      } else
+        new_stage = 0;
+
+      /// Explore the subtree of the XML robot description.
+      go_through(pos.second, level + 1, new_stage);
+    }
+  }
+}
+
+void ForceLimits::display(std::ostream &os) const {
+  os << "Lower limits:" << std::endl;
+  os << lower << std::endl;
+  os << "Upper Limits:" << std::endl;
+  os << upper << std::endl;
+}
+
+/******************** FootUtil ***************************/
+
+void FootUtil::display(std::ostream &os) const {
+  os << "Right Foot Sole XYZ " << std::endl;
+  os << m_Right_Foot_Sole_XYZ << std::endl;
+  os << "Left Foot Frame Name:" << m_Left_Foot_Frame_Name << std::endl;
+  os << "Right Foot Frame Name:" << m_Right_Foot_Frame_Name << std::endl;
+}
+
+/******************** HandUtil ***************************/
+
+void HandUtil::display(std::ostream &os) const {
+  os << "Left Hand Frame Name:" << m_Left_Hand_Frame_Name << std::endl;
+  os << "Right Hand Frame Name:" << m_Right_Hand_Frame_Name << std::endl;
+}
+
+/******************** ForceUtil ***************************/
+
+void ForceUtil::set_name_to_force_id(const std::string &name,
+                                     const Index &force_id) {
+  m_name_to_force_id[name] = (Index)force_id;
+  create_force_id_to_name_map();
+  if (name == "rf")
+    set_force_id_right_foot(m_name_to_force_id[name]);
+  else if (name == "lf")
+    set_force_id_left_foot(m_name_to_force_id[name]);
+  else if (name == "lh")
+    set_force_id_left_hand(m_name_to_force_id[name]);
+  else if (name == "rh")
+    set_force_id_right_hand(m_name_to_force_id[name]);
+}
+
+void ForceUtil::set_force_id_to_limits(const Index &force_id,
+                                       const dg::Vector &lf,
+                                       const dg::Vector &uf) {
+  m_force_id_to_limits[(Index)force_id].lower = lf;
+  m_force_id_to_limits[(Index)force_id].upper = uf;
+}
+
+Index ForceUtil::get_id_from_name(const std::string &name) {
+  std::map<std::string, Index>::const_iterator it;
+  it = m_name_to_force_id.find(name);
+  if (it != m_name_to_force_id.end()) return it->second;
+  return -1;
+}
+
+std::string force_default_rtn("Force name not found");
+std::string joint_default_rtn("Joint name not found");
+
+const std::string &ForceUtil::get_name_from_id(Index idx) {
+  std::map<Index, std::string>::const_iterator it;
+  it = m_force_id_to_name.find(idx);
+  if (it != m_force_id_to_name.end()) return it->second;
+  return force_default_rtn;
+}
+
+std::string ForceUtil::cp_get_name_from_id(Index idx) {
+  const std::string &default_rtn = get_name_from_id(idx);
+  return default_rtn;
+}
+void ForceUtil::create_force_id_to_name_map() {
+  std::map<std::string, Index>::const_iterator it;
+  for (it = m_name_to_force_id.begin(); it != m_name_to_force_id.end(); it++)
+    m_force_id_to_name[it->second] = it->first;
+}
+
+const ForceLimits &ForceUtil::get_limits_from_id(Index force_id) {
+  std::map<Index, ForceLimits>::const_iterator iter =
+      m_force_id_to_limits.find(force_id);
+  if (iter == m_force_id_to_limits.end())
+    return VoidForceLimits;  // Returns void instance
+  return iter->second;
+}
+
+ForceLimits ForceUtil::cp_get_limits_from_id(Index force_id) {
+  std::map<Index, ForceLimits>::const_iterator iter =
+      m_force_id_to_limits.find(force_id);
+  if (iter == m_force_id_to_limits.end())
+    return VoidForceLimits;  // Returns void instance
+  return iter->second;
+}
+
+void ForceUtil::display(std::ostream &os) const {
+  os << "Force Id to limits " << std::endl;
+  for (std::map<Index, ForceLimits>::const_iterator it =
+           m_force_id_to_limits.begin();
+       it != m_force_id_to_limits.end(); ++it) {
+    it->second.display(os);
+  }
+
+  os << "Name to force id:" << std::endl;
+  for (std::map<std::string, Index>::const_iterator it =
+           m_name_to_force_id.begin();
+       it != m_name_to_force_id.end(); ++it) {
+    os << "(" << it->first << "," << it->second << ") ";
+  }
+  os << std::endl;
+
+  os << "Force id to Name:" << std::endl;
+  for (std::map<Index, std::string>::const_iterator it =
+           m_force_id_to_name.begin();
+       it != m_force_id_to_name.end(); ++it) {
+    os << "(" << it->first << "," << it->second << ") ";
+  }
+  os << std::endl;
+
+  os << "Index for force sensors:" << std::endl;
+  os << "Left Hand (" << m_Force_Id_Left_Hand << ") ,"
+     << "Right Hand (" << m_Force_Id_Right_Hand << ") ,"
+     << "Left Foot (" << m_Force_Id_Left_Foot << ") ,"
+     << "Right Foot (" << m_Force_Id_Right_Foot << ") " << std::endl;
+}
+
+/**************** FromURDFToSot *************************/
+RobotUtil::RobotUtil() {}
+
+void RobotUtil::set_joint_limits_for_id(const Index &idx, const double &lq,
+                                        const double &uq) {
+  m_limits_map[(Index)idx] = JointLimits(lq, uq);
+}
+
+const JointLimits &RobotUtil::get_joint_limits_from_id(Index id) {
+  std::map<Index, JointLimits>::const_iterator iter = m_limits_map.find(id);
+  if (iter == m_limits_map.end()) return VoidJointLimits;
+  return iter->second;
+}
+JointLimits RobotUtil::cp_get_joint_limits_from_id(Index id) {
+  const JointLimits &rtn = get_joint_limits_from_id(id);
+  return rtn;
+}
+
+void RobotUtil::set_name_to_id(const std::string &jointName,
+                               const Index &jointId) {
+  m_name_to_id[jointName] = (Index)jointId;
+  create_id_to_name_map();
+}
+
+void RobotUtil::create_id_to_name_map() {
+  std::map<std::string, Index>::const_iterator it;
+  for (it = m_name_to_id.begin(); it != m_name_to_id.end(); it++)
+    m_id_to_name[it->second] = it->first;
+}
+
+const Index &RobotUtil::get_id_from_name(const std::string &name) {
+  std::map<std::string, Index>::const_iterator it = m_name_to_id.find(name);
+  if (it == m_name_to_id.end()) return VoidIndex;
+  return it->second;
+}
+
+const std::string &RobotUtil::get_name_from_id(Index id) {
+  std::map<Index, std::string>::const_iterator iter = m_id_to_name.find(id);
+  if (iter == m_id_to_name.end()) return joint_default_rtn;
+  return iter->second;
+}
+
+void RobotUtil::set_urdf_to_sot(const std::vector<Index> &urdf_to_sot) {
+  m_nbJoints = urdf_to_sot.size();
+  m_urdf_to_sot.resize(urdf_to_sot.size());
+  m_dgv_urdf_to_sot.resize(urdf_to_sot.size());
+  for (std::size_t idx = 0; idx < urdf_to_sot.size(); idx++) {
+    m_urdf_to_sot[(Index)idx] = urdf_to_sot[(Index)idx];
+    m_dgv_urdf_to_sot[(Index)idx] =
+        static_cast<double>(urdf_to_sot[(Index)idx]);
+  }
+}
+
+void RobotUtil::set_urdf_to_sot(const dg::Vector &urdf_to_sot) {
+  m_nbJoints = urdf_to_sot.size();
+  m_urdf_to_sot.resize(urdf_to_sot.size());
+  for (unsigned int idx = 0; idx < urdf_to_sot.size(); idx++) {
+    m_urdf_to_sot[idx] = (unsigned int)urdf_to_sot[idx];
+  }
+  m_dgv_urdf_to_sot = urdf_to_sot;
+}
+
+bool RobotUtil::joints_urdf_to_sot(ConstRefVector q_urdf, RefVector q_sot) {
+  if (m_nbJoints == 0) {
+    SEND_MSG("set_urdf_to_sot should be called", MSG_TYPE_ERROR);
+    return false;
+  }
+  assert(q_urdf.size() == static_cast<Eigen::VectorXd::Index>(m_nbJoints));
+  assert(q_sot.size() == static_cast<Eigen::VectorXd::Index>(m_nbJoints));
+
+  for (unsigned int idx = 0; idx < m_nbJoints; idx++)
+    q_sot[m_urdf_to_sot[idx]] = q_urdf[idx];
+  return true;
+}
+
+bool RobotUtil::joints_sot_to_urdf(ConstRefVector q_sot, RefVector q_urdf) {
+  assert(q_urdf.size() == static_cast<Eigen::VectorXd::Index>(m_nbJoints));
+  assert(q_sot.size() == static_cast<Eigen::VectorXd::Index>(m_nbJoints));
+
+  if (m_nbJoints == 0) {
+    SEND_MSG("set_urdf_to_sot should be called", MSG_TYPE_ERROR);
+    return false;
+  }
+
+  for (unsigned int idx = 0; idx < m_nbJoints; idx++)
+    q_urdf[idx] = q_sot[m_urdf_to_sot[idx]];
+  return true;
+}
+
+bool RobotUtil::velocity_urdf_to_sot(ConstRefVector q_urdf,
+                                     ConstRefVector v_urdf, RefVector v_sot) {
+  assert(q_urdf.size() == static_cast<Eigen::VectorXd::Index>(m_nbJoints + 7));
+  assert(v_urdf.size() == static_cast<Eigen::VectorXd::Index>(m_nbJoints + 6));
+  assert(v_sot.size() == static_cast<Eigen::VectorXd::Index>(m_nbJoints + 6));
+
+  if (m_nbJoints == 0) {
+    SEND_MSG("velocity_urdf_to_sot should be called", MSG_TYPE_ERROR);
+    return false;
+  }
+  const Eigen::Quaterniond q(q_urdf(6), q_urdf(3), q_urdf(4), q_urdf(5));
+  Eigen::Matrix3d oRb = q.toRotationMatrix();
+  v_sot.head<3>() = oRb * v_urdf.head<3>();
+  v_sot.segment<3>(3) = oRb * v_urdf.segment<3>(3);
+  //        v_sot.head<6>() = v_urdf.head<6>();
+  joints_urdf_to_sot(v_urdf.tail(m_nbJoints), v_sot.tail(m_nbJoints));
+  return true;
+}
+
+bool RobotUtil::velocity_sot_to_urdf(ConstRefVector q_urdf,
+                                     ConstRefVector v_sot, RefVector v_urdf) {
+  assert(q_urdf.size() == static_cast<Eigen::VectorXd::Index>(m_nbJoints + 7));
+  assert(v_urdf.size() == static_cast<Eigen::VectorXd::Index>(m_nbJoints + 6));
+  assert(v_sot.size() == static_cast<Eigen::VectorXd::Index>(m_nbJoints + 6));
+
+  if (m_nbJoints == 0) {
+    SEND_MSG("velocity_sot_to_urdf should be called", MSG_TYPE_ERROR);
+    return false;
+  }
+  // compute rotation from world to base frame
+  const Eigen::Quaterniond q(q_urdf(6), q_urdf(3), q_urdf(4), q_urdf(5));
+  Eigen::Matrix3d oRb = q.toRotationMatrix();
+  v_urdf.head<3>() = oRb.transpose() * v_sot.head<3>();
+  v_urdf.segment<3>(3) = oRb.transpose() * v_sot.segment<3>(3);
+  //        v_urdf.head<6>() = v_sot.head<6>();
+  joints_sot_to_urdf(v_sot.tail(m_nbJoints), v_urdf.tail(m_nbJoints));
+  return true;
+}
+
+bool RobotUtil::base_urdf_to_sot(ConstRefVector q_urdf, RefVector q_sot) {
+  assert(q_urdf.size() == 7);
+  assert(q_sot.size() == 6);
+
+  // ********* Quat to RPY *********
+  const double W = q_urdf[6];
+  const double X = q_urdf[3];
+  const double Y = q_urdf[4];
+  const double Z = q_urdf[5];
+  const Eigen::Matrix3d R = Eigen::Quaterniond(W, X, Y, Z).toRotationMatrix();
+  return base_se3_to_sot(q_urdf.head<3>(), R, q_sot);
+}
+
+bool RobotUtil::base_sot_to_urdf(ConstRefVector q_sot, RefVector q_urdf) {
+  assert(q_urdf.size() == 7);
+  assert(q_sot.size() == 6);
+  // *********  RPY to Quat *********
+  const double r = q_sot[3];
+  const double p = q_sot[4];
+  const double y = q_sot[5];
+  const Eigen::AngleAxisd rollAngle(r, Eigen::Vector3d::UnitX());
+  const Eigen::AngleAxisd pitchAngle(p, Eigen::Vector3d::UnitY());
+  const Eigen::AngleAxisd yawAngle(y, Eigen::Vector3d::UnitZ());
+  const Eigen::Quaternion<double> quat = yawAngle * pitchAngle * rollAngle;
+
+  q_urdf[0] = q_sot[0];  // BASE
+  q_urdf[1] = q_sot[1];
+  q_urdf[2] = q_sot[2];
+  q_urdf[3] = quat.x();
+  q_urdf[4] = quat.y();
+  q_urdf[5] = quat.z();
+  q_urdf[6] = quat.w();
+
+  return true;
+}
+
+bool RobotUtil::config_urdf_to_sot(ConstRefVector q_urdf, RefVector q_sot) {
+  assert(q_urdf.size() == static_cast<Eigen::VectorXd::Index>(m_nbJoints + 7));
+  assert(q_sot.size() == static_cast<Eigen::VectorXd::Index>(m_nbJoints + 6));
+
+  base_urdf_to_sot(q_urdf.head<7>(), q_sot.head<6>());
+  joints_urdf_to_sot(q_urdf.tail(m_nbJoints), q_sot.tail(m_nbJoints));
+
+  return true;
+}
+
+bool RobotUtil::config_sot_to_urdf(ConstRefVector q_sot, RefVector q_urdf) {
+  assert(q_urdf.size() == static_cast<Eigen::VectorXd::Index>(m_nbJoints + 7));
+  assert(q_sot.size() == static_cast<Eigen::VectorXd::Index>(m_nbJoints + 6));
+  base_sot_to_urdf(q_sot.head<6>(), q_urdf.head<7>());
+  joints_sot_to_urdf(q_sot.tail(m_nbJoints), q_urdf.tail(m_nbJoints));
+  return true;
+}
+void RobotUtil::display(std::ostream &os) const {
+  m_force_util.display(os);
+  m_foot_util.display(os);
+  m_hand_util.display(os);
+  os << "Nb of joints: " << m_nbJoints << std::endl;
+  os << "Urdf file name: " << m_urdf_filename << std::endl;
+
+  // Display m_urdf_to_sot
+  os << "Map from urdf index to the Sot Index " << std::endl;
+  for (unsigned int i = 0; i < m_urdf_to_sot.size(); i++)
+    os << "(" << i << " : " << m_urdf_to_sot[i] << ") ";
+  os << std::endl;
+
+  os << "Joint name to joint id:" << std::endl;
+  for (std::map<std::string, Index>::const_iterator it = m_name_to_id.begin();
+       it != m_name_to_id.end(); ++it) {
+    os << "(" << it->first << "," << it->second << ") ";
+  }
+  os << std::endl;
+
+  os << "Joint id to joint Name:" << std::endl;
+  for (std::map<Index, std::string>::const_iterator it = m_id_to_name.begin();
+       it != m_id_to_name.end(); ++it) {
+    os << "(" << it->first << "," << it->second << ") ";
+  }
+  os << std::endl;
+
+  boost::property_tree::write_xml(os, property_tree_);
+}
+
+void RobotUtil::sendMsg(const std::string &msg, MsgType t,
+                        const std::string &lineId) {
+  logger_.stream(t, lineId) << "[RobotUtil]" << msg << '\n';
+}
+
+bool base_se3_to_sot(ConstRefVector pos, ConstRefMatrix R, RefVector q_sot) {
+  assert(q_sot.size() == 6);
+  assert(pos.size() == 3);
+  assert(R.rows() == 3);
+  assert(R.cols() == 3);
+  // ********* Quat to RPY *********
+  double r, p, y, m;
+  m = sqrt(R(2, 1) * R(2, 1) + R(2, 2) * R(2, 2));
+  p = atan2(-R(2, 0), m);
+  if (fabs(fabs(p) - M_PI / 2) < 0.001) {
+    r = 0.0;
+    y = -atan2(R(0, 1), R(1, 1));
+  } else {
+    y = atan2(R(1, 0), R(0, 0));
+    r = atan2(R(2, 1), R(2, 2));
+  }
+  // *********************************
+  q_sot[0] = pos[0];
+  q_sot[1] = pos[1];
+  q_sot[2] = pos[2];
+  q_sot[3] = r;
+  q_sot[4] = p;
+  q_sot[5] = y;
+  return true;
+}
+
+bool base_urdf_to_sot(ConstRefVector q_urdf, RefVector q_sot) {
+  assert(q_urdf.size() == 7);
+  assert(q_sot.size() == 6);
+  // ********* Quat to RPY *********
+  const double W = q_urdf[6];
+  const double X = q_urdf[3];
+  const double Y = q_urdf[4];
+  const double Z = q_urdf[5];
+  const Eigen::Matrix3d R = Eigen::Quaterniond(W, X, Y, Z).toRotationMatrix();
+  return base_se3_to_sot(q_urdf.head<3>(), R, q_sot);
+}
+
+bool base_sot_to_urdf(ConstRefVector q_sot, RefVector q_urdf) {
+  assert(q_urdf.size() == 7);
+  assert(q_sot.size() == 6);
+  // *********  RPY to Quat *********
+  const double r = q_sot[3];
+  const double p = q_sot[4];
+  const double y = q_sot[5];
+  const Eigen::AngleAxisd rollAngle(r, Eigen::Vector3d::UnitX());
+  const Eigen::AngleAxisd pitchAngle(p, Eigen::Vector3d::UnitY());
+  const Eigen::AngleAxisd yawAngle(y, Eigen::Vector3d::UnitZ());
+  const Eigen::Quaternion<double> quat = yawAngle * pitchAngle * rollAngle;
+
+  q_urdf[0] = q_sot[0];  // BASE
+  q_urdf[1] = q_sot[1];
+  q_urdf[2] = q_sot[2];
+  q_urdf[3] = quat.x();
+  q_urdf[4] = quat.y();
+  q_urdf[5] = quat.z();
+  q_urdf[6] = quat.w();
+
+  return true;
+}
+
+static std::map<std::string, RobotUtilShrPtr> sgl_map_name_to_robot_util;
+
+std::shared_ptr<std::vector<std::string> > getListOfRobots() {
+  std::shared_ptr<std::vector<std::string> > res =
+      std::make_shared<std::vector<std::string> >();
+
+  std::map<std::string, RobotUtilShrPtr>::iterator it =
+      sgl_map_name_to_robot_util.begin();
+  while (it != sgl_map_name_to_robot_util.end()) {
+    res->push_back(it->first);
+    it++;
+  }
+
+  return res;
+}
+
+RobotUtilShrPtr getRobotUtil(std::string &robotName) {
+  std::map<std::string, RobotUtilShrPtr>::iterator it =
+      sgl_map_name_to_robot_util.find(robotName);
+  if (it != sgl_map_name_to_robot_util.end()) return it->second;
+  return RefVoidRobotUtil();
+}
+
+bool isNameInRobotUtil(std::string &robotName) {
+  std::map<std::string, RobotUtilShrPtr>::iterator it =
+      sgl_map_name_to_robot_util.find(robotName);
+  if (it != sgl_map_name_to_robot_util.end()) return true;
+  return false;
+}
+
+RobotUtilShrPtr createRobotUtil(std::string &robotName) {
+  std::map<std::string, RobotUtilShrPtr>::iterator it =
+      sgl_map_name_to_robot_util.find(robotName);
+  if (it == sgl_map_name_to_robot_util.end()) {
+    sgl_map_name_to_robot_util[robotName] = std::make_shared<RobotUtil>();
+    it = sgl_map_name_to_robot_util.find(robotName);
+    return it->second;
+  }
+  return RefVoidRobotUtil();
+}
+}  // namespace sot
+}  // namespace dynamicgraph

src/tools/seq-play.cpp

-/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
- * Copyright Projet JRL-Japan, 2007
- *+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
- *
- * File:      SeqPlay.cpp
- * Project:   SOT
- * Author:    Nicolas Mansard
- *
- * Version control
- * ===============
- *
- *  $Id$
- *
- * Description
- * ============
- *
- *
- * ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
-
-
-/* --------------------------------------------------------------------- */
-/* --- INCLUDE --------------------------------------------------------- */
-/* --------------------------------------------------------------------- */
-
-/* SOT */
-#include <sot-core/seq-play.h>
-#include <sot-core/debug.h>
-using namespace std;
-
-#include <fstream>
-#include <sstream>
-
-#include <dynamic-graph/factory.h>
-using namespace dynamicgraph;
-using namespace sot;
-
-DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(SeqPlay,"SeqPlay");
-
-
-/* --------------------------------------------------------------------- */
-/* --- CLASS ----------------------------------------------------------- */
-/* --------------------------------------------------------------------- */
-
-
-SeqPlay::
-SeqPlay( const std::string& n )
-  :Entity(n)
-   ,stateList()
-   ,currPos(stateList.begin())
-   ,currRank(0)
-   ,init(false)
-   ,time(0)
-   ,refresherSINTERN( "SeqPlay("+n+")::intern(dummy)::refresher" )
-   ,positionSOUT( boost::bind(&SeqPlay::getNextPosition,this,_1,_2),
-		  refresherSINTERN,
-		  "SeqPlay("+n+")::output(vector)::position" )
-{
-  signalRegistration( positionSOUT );
-  refresherSINTERN.setDependencyType( TimeDependency<int>::ALWAYS_READY );
-}
-
-/* --- COMPUTE ----------------------------------------------------------- */
-/* --- COMPUTE ----------------------------------------------------------- */
-/* --- COMPUTE ----------------------------------------------------------- */
-ml::Vector& SeqPlay::
-getNextPosition( ml::Vector& pos, const int& time )
-{
-  sotDEBUGIN(15);
-  if( !init ) 
-    {
-      if( stateList.empty() ) return pos; 
-      currPos=stateList.begin(); init=true; currRank = 0;
-    }
-
-  
-    {
-      const ml::Vector& posCur = *currPos;
-      pos=posCur;
-      
-      currPos++; 
-      if( currPos==stateList.end() ) currPos--;
-      else currRank++;
-    } 
-
-  sotDEBUGOUT(15);
-  return pos;
-}
-
-/* --- LIST -------------------------------------------------------------- */
-/* --- LIST -------------------------------------------------------------- */
-/* --- LIST -------------------------------------------------------------- */
-void SeqPlay::
-loadFile( const std::string& filename )
-{
-  sotDEBUGIN(15);
-
-  sotDEBUG( 25 ) << " Load " << filename << endl;
-  std::ifstream file(filename.c_str());
-  const unsigned int SIZE = 1024;
-  char buffer[SIZE];
-
-  ml::Vector res(1); unsigned int ressize = 1;
-  double time;
-
-  while( file.good() )
-    {
-      file.getline( buffer,SIZE );
-      if( file.gcount()<5 ) break; 
-
-      sotDEBUG(25) << buffer<<endl;
-      std::istringstream iss( buffer );
-      
-      iss>>time;      unsigned int i;
-
-      for( i=0;iss.good();++i )
-	{
-	  if( i==ressize ) { ressize*=2; res.resize(ressize,false); }
-	  iss>>res(i); sotDEBUG(35) <<i<< ": " <<  res(i)<<endl;
-	}
-      ressize=i-1;  res.resize(ressize,false); 
-      stateList.push_back( res );
-      sotDEBUG(15) << time << ": " <<  res << endl;
-    }
-
-  sotDEBUGOUT(15);
-}
-
-
-
-
-/* --- DISPLAY ------------------------------------------------------------ */
-/* --- DISPLAY ------------------------------------------------------------ */
-/* --- DISPLAY ------------------------------------------------------------ */
-
-void SeqPlay::display ( std::ostream& os ) const
-{os <<name<<endl; }
-
-
-/* --- PARAMS --------------------------------------------------------------- */
-/* --- PARAMS --------------------------------------------------------------- */
-/* --- PARAMS --------------------------------------------------------------- */
-#include <dynamic-graph/pool.h>
-
-void SeqPlay::
-commandLine( const std::string& cmdLine
-	     ,std::istringstream& cmdArgs
-	     ,std::ostream& os )
-{
-  if( cmdLine=="help" )
-    {
-      os << "SeqPlay: "<<endl
-	 << "  - load <file>"<<endl
-	 << "  - size" <<endl;
-	Entity::commandLine( cmdLine,cmdArgs,os );
-    }
-  else if( cmdLine=="load" )
-    {
-      std::string n; cmdArgs >> n;
-      loadFile(n);
-    }
-  else if( cmdLine == "empty" )
-    {
-      stateList.clear(); init=false;
-    }
-  else if( cmdLine == "size" ) 
-    {
-      os << "size = " << stateList.size() << endl; 
-      os << "rank = " << currRank << endl; 
-    }
-  else  
-    {
-      Entity::commandLine( cmdLine,cmdArgs,os );
-    }
-}

src/tools/sequencer-python.h

+#include <sot/core/sequencer.hh>
+
+typedef boost::mpl::vector<dynamicgraph::sot::Sequencer> entities_t;

src/tools/sequencer.cpp

-/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
- * Copyright Projet JRL-Japan, 2007
- *+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+/*
+ * Copyright 2010,
+ * François Bleibel,
+ * Olivier Stasse,
  *
- * File:      Sequencer.h
- * Project:   SOT
- * Author:    Nicolas Mansard
+ * CNRS/AIST
  *
- * Version control
- * ===============
- *
- *  $Id$
- *
- * Description
- * ============
- *
- *
- * ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
+ */
 
-#include <sot-core/sequencer.h>
-#include <sot-core/debug.h>
-#include <sot-core/exception-tools.h>
-#include <sot-core/sot.h>
-#include <dynamic-graph/pool.h>
-#include <dynamic-graph/interpreter.h>
 #include <dynamic-graph/factory.h>
+#include <dynamic-graph/pool.h>
+
+#include <sot/core/debug.hh>
+#include <sot/core/exception-tools.hh>
+#include <sot/core/sequencer.hh>
+#include <sot/core/sot.hh>
 
-using namespace sot;
+using namespace dynamicgraph::sot;
 using namespace dynamicgraph;
 
-DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(Sequencer,"Sequencer");
-
-Sequencer::
-Sequencer( const std::string & name )
-  :Entity(name)
-  ,timeInit(-1)
-  ,playMode(false)
-  ,outputStreamPtr(NULL)
-  ,noOutput(false)
-  ,triggerSOUT( boost::bind(&Sequencer::trigger,this,_1,_2),
-		sotNOSIGNAL,
-		"Sequencer("+name+")::output(dummy)::trigger" )
-{
+DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(Sequencer, "Sequencer");
+
+Sequencer::Sequencer(const std::string &name)
+    : Entity(name),
+      timeInit(-1),
+      playMode(false),
+      outputStreamPtr(NULL),
+      noOutput(false),
+      triggerSOUT(boost::bind(&Sequencer::trigger, this, _1, _2), sotNOSIGNAL,
+                  "Sequencer(" + name + ")::output(dummy)::trigger") {
   sotDEBUGIN(5);
-  
-  signalRegistration( triggerSOUT );
-  triggerSOUT.setNeedUpdateFromAllChildren( true );
+
+  signalRegistration(triggerSOUT);
+  triggerSOUT.setNeedUpdateFromAllChildren(true);
 
   sotDEBUGOUT(5);
 }
 
-
-Sequencer::
-~Sequencer( void )
-{
+Sequencer::~Sequencer(void) {
   sotDEBUGIN(5);
 
   sotDEBUGOUT(5);
@@ -63,219 +47,193 @@ Sequencer::
 /* --- SPECIFIC EVENT ------------------------------------------------------- */
 /* --- SPECIFIC EVENT ------------------------------------------------------- */
 
-class sotEventTaskBased
-  : public Sequencer::sotEventAbstract
-{
-protected:
-  TaskAbstract * taskPtr;
+class sotEventTaskBased : public Sequencer::sotEventAbstract {
+ protected:
+  TaskAbstract *taskPtr;
   const std::string defaultTaskName;
-public:
-  sotEventTaskBased( const std::string name = "",TaskAbstract* task = NULL )
-    :sotEventAbstract( name )
-    ,taskPtr( task )
-    ,defaultTaskName("NULL")
-  {}
-
-  void init( std::istringstream& cmdArgs )
-  {
+
+ public:
+  sotEventTaskBased(const std::string name = "", TaskAbstract *task = NULL)
+      : sotEventAbstract(name), taskPtr(task), defaultTaskName("NULL") {}
+
+  void init(std::istringstream &cmdArgs) {
     cmdArgs >> std::ws;
-    if( cmdArgs.good() )
-      {
-	std::string taskname; cmdArgs >> taskname;
-	sotDEBUG(15) << "Add task " << taskname << std::endl;
-	taskPtr  
-	  = dynamic_cast< TaskAbstract* > (&g_pool.getEntity( taskname ));
-      }
+    if (cmdArgs.good()) {
+      std::string taskname;
+      cmdArgs >> taskname;
+      sotDEBUG(15) << "Add task " << taskname << std::endl;
+      taskPtr = dynamic_cast<TaskAbstract *>(
+          &dynamicgraph::PoolStorage::getInstance()->getEntity(taskname));
+    }
+  }
+  virtual void display(std::ostream &os) const {
+    if (taskPtr)
+      os << taskPtr->getName();
+    else
+      os << "NULL";
+  }
+  virtual const std::string &getName() const {
+    if (taskPtr)
+      return taskPtr->getName();
+    else
+      return defaultTaskName;
   }
-  virtual void display( std::ostream& os ) const 
-  { if( taskPtr ) os << taskPtr->getName(); else os << "NULL"; }
-  virtual const std::string& getName() const 
-  { if( taskPtr ) return taskPtr->getName(); else return defaultTaskName; }
-
 };
 
-class sotEventAddATask
-  : public sotEventTaskBased
-{
-public:
-  sotEventAddATask( const std::string name = "",TaskAbstract* task=NULL )
-    :sotEventTaskBased( name,task )
-  {
+class sotEventAddATask : public sotEventTaskBased {
+ public:
+  sotEventAddATask(const std::string name = "", TaskAbstract *task = NULL)
+      : sotEventTaskBased(name, task) {
     eventType = EVENT_ADD;
   }
 
-  void operator()( Sot* sotptr )
-  {
+  void operator()(Sot *sotptr) {
     sotDEBUGIN(15);
-    sotDEBUG(45) << "Sot = " << sotptr << ". Task = " << taskPtr << "." << std::endl;
-    if( (NULL!=sotptr)&&(NULL!=taskPtr) ) sotptr->push(*taskPtr ); 
+    sotDEBUG(45) << "Sot = " << sotptr << ". Task = " << taskPtr << "."
+                 << std::endl;
+    if ((NULL != sotptr) && (NULL != taskPtr)) sotptr->push(*taskPtr);
     sotDEBUGOUT(15);
   }
 
-  virtual void display( std::ostream& os ) const 
-  {os << "Add<"; sotEventTaskBased::display(os); os<<">"; }
-
+  virtual void display(std::ostream &os) const {
+    os << "Add<";
+    sotEventTaskBased::display(os);
+    os << ">";
+  }
 };
 
-
-class sotEventRemoveATask
-  : public sotEventTaskBased
-{
-public:
-  sotEventRemoveATask( const std::string name = "",TaskAbstract* task=NULL )
-    :sotEventTaskBased( name,task )
-  {
+class sotEventRemoveATask : public sotEventTaskBased {
+ public:
+  sotEventRemoveATask(const std::string name = "", TaskAbstract *task = NULL)
+      : sotEventTaskBased(name, task) {
     eventType = EVENT_RM;
   }
 
-  void operator()( Sot* sotptr )
-  { 
+  void operator()(Sot *sotptr) {
     sotDEBUGIN(15);
-    sotDEBUG(45) << "Sot = " << sotptr << ". Task = " << taskPtr << "." << std::endl;
-    if( (NULL!=sotptr)&&(NULL!=taskPtr) ) sotptr->remove(*taskPtr );
+    sotDEBUG(45) << "Sot = " << sotptr << ". Task = " << taskPtr << "."
+                 << std::endl;
+    if ((NULL != sotptr) && (NULL != taskPtr)) sotptr->remove(*taskPtr);
     sotDEBUGOUT(15);
   }
 
-  virtual void display( std::ostream& os ) const 
-  { os << "Remove<"; sotEventTaskBased::display(os); os<<">"; }
-
+  virtual void display(std::ostream &os) const {
+    os << "Remove<";
+    sotEventTaskBased::display(os);
+    os << ">";
+  }
 };
 
-
-class sotEventCmd
-  : public Sequencer::sotEventAbstract
-{
-protected:
+class sotEventCmd : public Sequencer::sotEventAbstract {
+ protected:
   std::string cmd;
 
-public:
-  sotEventCmd( const std::string cmdLine = "" )
-    :sotEventAbstract( cmdLine+"<cmd>" )
-    ,cmd( cmdLine )
-  {
+ public:
+  sotEventCmd(const std::string cmdLine = "")
+      : sotEventAbstract(cmdLine + "<cmd>"), cmd(cmdLine) {
     eventType = EVENT_CMD;
-	sotDEBUGINOUT(15);
+    sotDEBUGINOUT(15);
   }
 
-  void init( std::istringstream& args )
-  {
+  void init(std::istringstream &args) {
     sotDEBUGIN(15);
-    std::stringbuf* pbuf=args.rdbuf();
-    const unsigned int size = pbuf->in_avail();
-    char* buffer = new char ( size+1 );
-    pbuf->sgetn( buffer,size );
+    std::stringbuf *pbuf = args.rdbuf();
+    const unsigned int size = (unsigned int)(pbuf->in_avail());
+    char *buffer = new char[size + 1];
+    pbuf->sgetn(buffer, size);
 
-    buffer[size]='\0';
+    buffer[size] = '\0';
     cmd = buffer;
     sotDEBUGOUT(15);
-	delete buffer;
+    delete[] buffer;
   }
-  const std::string & getEventCmd() const 
-  {	return cmd; }
-  virtual void display( std::ostream& os ) const 
-  { os << "Run: " << cmd; }
-  virtual void operator() ( Sot* sotPtr )
-  { 
-    std::ostringstream onull; onull.clear( std::ios::failbit );
-    std::istringstream iss( cmd );
-    std::string cmdName; iss >> cmdName;
-    g_shell.cmd( cmdName,iss,onull );
-  } ;
+  const std::string &getEventCmd() const { return cmd; }
+  virtual void display(std::ostream &os) const { os << "Run: " << cmd; }
+  virtual void operator()(Sot * /*sotPtr*/) {
+    std::ostringstream onull;
+    onull.clear(std::ios::failbit);
+    std::istringstream iss(cmd);
+    std::string cmdName;
+    iss >> cmdName;
+    // Florent: remove reference to g_shell
+    // g_shell.cmd( cmdName,iss,onull );
+  };
 };
 
-
 /* --- TASK MANIP ----------------------------------------------------------- */
 /* --- TASK MANIP ----------------------------------------------------------- */
 /* --- TASK MANIP ----------------------------------------------------------- */
 
-void Sequencer::
-addTask( sotEventAbstract* task,const unsigned int timeSpec )
-{
-  TaskMap::iterator listKey = taskMap.find( timeSpec );
-  if( taskMap.end()==listKey )
-    {  
-      sotDEBUG(15) << "New element at " << timeSpec << std::endl;
-      taskMap[timeSpec].push_back( task ); 
-    }
-  else 
-    {
-      TaskList& tl = listKey->second;
-      tl.push_back( task ); 
+void Sequencer::addTask(sotEventAbstract *task, const unsigned int timeSpec) {
+  TaskMap::iterator listKey = taskMap.find(timeSpec);
+  if (taskMap.end() == listKey) {
+    sotDEBUG(15) << "New element at " << timeSpec << std::endl;
+    taskMap[timeSpec].push_back(task);
+  } else {
+    TaskList &tl = listKey->second;
+    tl.push_back(task);
   }
 }
 
-//rmTask
-void Sequencer::
-rmTask( int eventType, const std::string & name,const unsigned int time )
-{
-	TaskMap::iterator listKey = taskMap.find( time );
-	if( taskMap.end()!=listKey )	//the time exist
-	{
-		TaskList& tl = listKey->second;
-		for( TaskList::iterator itL = tl.begin();  itL != tl.end(); ++itL)
-		{
-			if ((*itL)->getEventType() == eventType && (*itL)->getName() == name) 
-			{
-				tl.remove(*itL);
-				break;
-			}
-		}
-
-		//remove the list if empty
-		if (tl.empty())
-			taskMap.erase(listKey);
-	}
+// rmTask
+void Sequencer::rmTask(int eventType, const std::string &name,
+                       const unsigned int time) {
+  TaskMap::iterator listKey = taskMap.find(time);
+  if (taskMap.end() != listKey)  // the time exist
+  {
+    TaskList &tl = listKey->second;
+    for (TaskList::iterator itL = tl.begin(); itL != tl.end(); ++itL) {
+      if ((*itL)->getEventType() == eventType && (*itL)->getName() == name) {
+        tl.remove(*itL);
+        break;
+      }
+    }
+
+    // remove the list if empty
+    if (tl.empty()) taskMap.erase(listKey);
+  }
 }
 
-//clearAll
-void Sequencer::
-clearAll( )
-{
+// clearAll
+void Sequencer::clearAll() {
   TaskMap::iterator itM;
-  for(itM = taskMap.begin(); itM != taskMap.end(); ++itM )
-  {
-	TaskList::iterator itL;
-	TaskList& currentMap = itM->second;
-	for (itL=currentMap.begin(); itL!=currentMap.end(); ++itL)
-		delete (*itL);
-	itM->second.clear();
+  for (itM = taskMap.begin(); itM != taskMap.end(); ++itM) {
+    TaskList::iterator itL;
+    TaskList &currentMap = itM->second;
+    for (itL = currentMap.begin(); itL != currentMap.end(); ++itL)
+      delete (*itL);
+    itM->second.clear();
   }
-  //remove all the lists
+  // remove all the lists
   taskMap.clear();
 }
 /* --- SIGNALS -------------------------------------------------------------- */
 /* --- SIGNALS -------------------------------------------------------------- */
 /* --- SIGNALS -------------------------------------------------------------- */
 
-int& Sequencer::
-trigger( int& dummy,const int& timeSpec )
-{
+int &Sequencer::trigger(int &dummy, const int &timeSpec) {
   sotDEBUGIN(15);
 
-  if(! playMode ) return dummy; 
-  if( -1==timeInit ) timeInit = timeSpec;
-
-  sotDEBUG(15) << "Ref time: " << (timeSpec-timeInit) << std::endl;
-  TaskMap::iterator listKey = taskMap.find( timeSpec-timeInit );
-  if( taskMap.end()!=listKey )
-    {  
-      sotDEBUG(1) << "Time: "<< (timeSpec-timeInit) << ": we've got a task to do!" 
-		  << std::endl;
-      TaskList & tl = listKey->second;
-      for( TaskList::iterator iter=tl.begin();iter!=tl.end();++iter )
-	{
-	  if( *iter )
-	    { 
-	      (*iter)->operator() (sotPtr);
-	      if( NULL!=outputStreamPtr ) 
-		{ 
-		  (*outputStreamPtr) << "At time t=" << timeSpec << ": ";
-		  (*iter)->display(*outputStreamPtr); 
-		  (*outputStreamPtr) << std::endl;
-		}
-	    }
-	}
+  if (!playMode) return dummy;
+  if (-1 == timeInit) timeInit = timeSpec;
+
+  sotDEBUG(15) << "Ref time: " << (timeSpec - timeInit) << std::endl;
+  TaskMap::iterator listKey = taskMap.find(timeSpec - timeInit);
+  if (taskMap.end() != listKey) {
+    sotDEBUG(1) << "Time: " << (timeSpec - timeInit)
+                << ": we've got a task to do!" << std::endl;
+    TaskList &tl = listKey->second;
+    for (TaskList::iterator iter = tl.begin(); iter != tl.end(); ++iter) {
+      if (*iter) {
+        (*iter)->operator()(sotPtr);
+        if (NULL != outputStreamPtr) {
+          (*outputStreamPtr) << "At time t=" << timeSpec << ": ";
+          (*iter)->display(*outputStreamPtr);
+          (*outputStreamPtr) << std::endl;
+        }
+      }
     }
+  }
 
   sotDEBUGOUT(15);
   return dummy;
@@ -285,152 +243,20 @@ trigger( int& dummy,const int& timeSpec )
 /* --- PARAMS --------------------------------------------------------------- */
 /* --- PARAMS --------------------------------------------------------------- */
 
-void Sequencer::
-display( std::ostream& os ) const
-{
+void Sequencer::display(std::ostream &os) const {
   if (noOutput) return;
 
   os << "Sequencer " << getName() << "(t0=" << timeInit
-     << ",mode=" << ( (playMode)?"play":"pause" ) << "): " << std::endl;
-  for( TaskMap::const_iterator iterMap = taskMap.begin();
-       iterMap!=taskMap.end();iterMap++ )
-    {
-      os << " - t=" << (iterMap->first) << ":\t";
-      const TaskList & tl = iterMap->second;
-      for( TaskList::const_iterator iterList = tl.begin();
-	   iterList!=tl.end();iterList++ )
-	{
-	  (*iterList)->display(os); os << " ";
-	}
-      os << std::endl;
-    }
-
-}
-
-
-void Sequencer::
-commandLine( const std::string& cmdLine,
-	     std::istringstream& cmdArgs,
-	     std::ostream& os )
-{
-  sotDEBUG(25) << "Cmd " << cmdLine <<std::endl;
-
-  if( cmdLine == "help" )
-    {
-      os << "Sequencer: " << std::endl
-	 << " - sot [<sotname>]" << std::endl
-	 << " - addEvent: <eventType> <time> <args>" << std::endl
-	 << " - rmEvent: <eventType> <time> <args>" << std::endl
-	 << " - clear: erase all events" << std::endl
-	 << " - start/stop" << std::endl
-	 << " - reset: reset the t0 counter. " << std::endl
-	 << " - verbose/normal/mute: change output model : detailed/default (normal messages)/only errors" << std::endl;
+     << ",mode=" << ((playMode) ? "play" : "pause") << "): " << std::endl;
+  for (TaskMap::const_iterator iterMap = taskMap.begin();
+       iterMap != taskMap.end(); iterMap++) {
+    os << " - t=" << (iterMap->first) << ":\t";
+    const TaskList &tl = iterMap->second;
+    for (TaskList::const_iterator iterList = tl.begin(); iterList != tl.end();
+         iterList++) {
+      (*iterList)->display(os);
+      os << " ";
     }
-  else if( cmdLine == "sot" )
-    {
-      cmdArgs>>std::ws; if( cmdArgs.good() )
-	{
-	  std::string sotname; cmdArgs >> sotname;
-	  Sot * sotptr = dynamic_cast< Sot* > (& (g_pool.getEntity( sotname )));
-	  if(! sotptr ) os << "! Entity <" << sotname << "> does not exist "
-			   << "(see you later, next patient please!"
-			   << std::endl;
-	  else setSotRef( sotptr );
-	} 
-      else
-	{ 
-	  if( sotPtr ) os << "sot = " << sotPtr->getName() << std::endl;
-	  else os << "No sot specified yet. " << std::endl;
-	}
-    }
-  else if( cmdLine == "addEvent" )
-    {
-      std::string eventType; 
-      unsigned int timeref=0;
-	
-
-      cmdArgs>>std::ws; if(! cmdArgs.good() ) 
-	  { if (!noOutput) {os <<"! addEvent: <eventType> <time> <args>" <<std::endl;} return; }
-      cmdArgs >> eventType >> std::ws;
-
-      if(! cmdArgs.good() ) 
-	  { if (!noOutput) {os <<"! addEvent: <eventType> <time> <args>" <<std::endl;} return; }
-      cmdArgs >> timeref ;
-      
-      sotEventTaskBased* event;
-      if( eventType=="add" )	 event = new sotEventAddATask();
-      else if( eventType=="rm" ) event = new sotEventRemoveATask();
-      else
-	{
-	  os<<"! Event type <" <<eventType <<"> is not recognized." <<std::endl; 
-	  return; 
-	}
-      event->init( cmdArgs );
-      addTask( event,timeref );
-    }
-  else if( cmdLine == "rmEvent" )
-  {
-	  std::string eventType; 
-	  unsigned int timeref=0;
-
-
-	  cmdArgs>>std::ws; if(! cmdArgs.good() ) 
-	  { if (!noOutput) {os <<"! rmEvent: <eventType> <time> <args>" <<std::endl;} return; }
-	  cmdArgs >> eventType >> std::ws;
-
-	  if(! cmdArgs.good() ) 
-	  { if (!noOutput) {os <<"! rmEvent: <eventType> <time> <args>" <<std::endl;} return; }
-	  cmdArgs >> timeref ;
-
-	  //the type of event
-	  int event;
-	  if( eventType=="add" )	 event = sotEventAbstract::EVENT_ADD;
-	  else if( eventType=="rm" ) event = sotEventAbstract::EVENT_RM;
-	  else
-	  {
-		  os<<"! Event type <" <<eventType <<"> is not recognized." <<std::endl; 
-		  return; 
-	  }
-
-	  //the name of the event
-	  std::string name;
-	  cmdArgs >> name;
-	  rmTask( event, name, timeref );
+    os << std::endl;
   }
-  else if( cmdLine == "addCmd" )
-    {
-      unsigned int timeref=0; cmdArgs >> std::ws;
-      sotDEBUG(15)<<std::endl;
-      if(! cmdArgs.good() ) 
-	  { if (!noOutput) {os <<"! addEvent: <eventType> <time> <args>" <<std::endl;} return; }
-      sotDEBUG(15)<<std::endl;
-      cmdArgs >> timeref ;
-      sotDEBUG(15)<<std::endl;
-      sotEventCmd * eventcmd = new sotEventCmd();
-      sotDEBUG(15)<<std::endl;
-      eventcmd->init( cmdArgs );
-      addTask( eventcmd,timeref );
-    }
-  else if( cmdLine == "rmCmd" )
-    {
-      unsigned int timeref=0; cmdArgs >> std::ws;
-      if(! cmdArgs.good() ) 
-	  { if (!noOutput) {os <<"! addEvent: <eventType> <time> <args>" <<std::endl;} return; }
-      cmdArgs >> timeref ;
-	  int index = sotEventAbstract::EVENT_CMD;
-
-	  //get the name of the command
-	  std::string name;
-	  cmdArgs >>  name;
-      rmTask( index, name,timeref );
-    }
-  else if( "clear"==cmdLine ) { clearAll(); }
-  else if( "reset"==cmdLine ) { timeInit = -1; }
-  else if( "start"==cmdLine ) { playMode = true; }
-  else if( "stop"==cmdLine ) { playMode = false; }
-  else if( "verbose"==cmdLine ) { outputStreamPtr = &os; }
-  else if( "mute"==cmdLine ) { outputStreamPtr = NULL; noOutput=true;}
-  else if( "normal"==cmdLine ) { noOutput=false; }
-  else { Entity::commandLine( cmdLine,cmdArgs,os); }
 }
-

src/tools/smooth-reach-python.h

+#include <sot/core/smooth-reach.hh>
+
+typedef boost::mpl::vector<dynamicgraph::sot::SmoothReach> entities_t;

src/tools/smooth-reach.cpp

+/*
+ * Copyright 2010,
+ * François Bleibel,
+ * Olivier Stasse,
+ *
+ * CNRS/AIST
+ *
+ */
+
+#include <dynamic-graph/all-commands.h>
+#include <dynamic-graph/factory.h>
+
+#include <sot/core/debug.hh>
+#include <sot/core/smooth-reach.hh>
+
+using namespace dynamicgraph;
+using namespace dynamicgraph::sot;
+
+DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(SmoothReach, "SmoothReach");
+
+SmoothReach::SmoothReach(const std::string &name)
+    : Entity(name)
+
+      ,
+      start(0u),
+      goal(0u),
+      startTime(-1),
+      lengthTime(-1),
+      isStarted(false),
+      isParam(true)
+
+      ,
+      smoothMode(2),
+      smoothParam(1.2)
+
+      ,
+      startSIN(NULL, "SmoothReach(" + name + ")::input(vector)::start"),
+      goalSOUT(boost::bind(&SmoothReach::goalSOUT_function, this, _1, _2),
+               sotNOSIGNAL, "SmoothReach(" + name + ")::output(vector)::goal")
+
+{
+  sotDEBUGIN(5);
+
+  signalRegistration(startSIN << goalSOUT);
+  initCommands();
+  goalSOUT.setNeedUpdateFromAllChildren(true);
+  sotDEBUGOUT(5);
+}
+
+void SmoothReach::initCommands(void) {
+  using namespace command;
+  addCommand("set",
+             makeCommandVoid2(*this, &SmoothReach::set,
+                              docCommandVoid2("Set the curve.", "vector (goal)",
+                                              "int (duration)")));
+  addCommand("param",
+             makeCommandVoid2(*this, &SmoothReach::setSmoothing,
+                              docCommandVoid2("Set the parameter.",
+                                              "int (mode)", "double (beta)")));
+}
+
+double SmoothReach::smoothFunction(double x) {
+  switch (smoothMode) {
+    case 0:
+      return x;
+
+    case 1: {
+      // const double smoothParam = 0.45;
+      return tanh(-smoothParam / x + smoothParam / (1 - x)) / 2 + 0.5;
+    }
+    case 2: {
+      // const double smoothParam = 1.5;
+      return atan(-smoothParam / x + smoothParam / (1 - x)) / M_PI + 0.5;
+    }
+  }
+  return 0;
+}
+
+void SmoothReach::setSmoothing(const int &mode, const double &param) {
+  smoothMode = mode;
+  smoothParam = param;
+}
+
+dynamicgraph::Vector &SmoothReach::goalSOUT_function(dynamicgraph::Vector &res,
+                                                     const int &time) {
+  if (isParam) {
+    start = startSIN(time);
+    startTime = time;
+
+    assert(start.size() == goal.size());
+    isParam = false;
+    isStarted = true;
+  }
+
+  if (isStarted) {
+    double x = double(time - startTime) / lengthTime;
+    if (x > 1) x = 1;
+    double x1 = smoothFunction(x);
+    double x0 = 1 - x1;
+    res = start * x0 + goal * x1;
+  }
+
+  return res;
+}
+
+void SmoothReach::set(const dynamicgraph::Vector &goalDes,
+                      const int &lengthDes) {
+  goal = goalDes;
+  lengthTime = lengthDes;
+  isParam = true;
+}
+
+const dynamicgraph::Vector &SmoothReach::getGoal(void) { return goal; }
+
+const int &SmoothReach::getLength(void) { return lengthTime; }
+
+const int &SmoothReach::getStart(void) { return startTime; }
+
+void SmoothReach::display(std::ostream &os) const {
+  os << "Status: " << isStarted << isParam << std::endl
+     << "Goal: " << goal << "start: " << start << "Times: " << startTime << " "
+     << lengthTime << std::endl;
+}

src/tools/sot-loader.cpp

+/*
+ * Copyright 2016,
+ * Olivier Stasse,
+ *
+ * CNRS
+ *
+ */
+/* -------------------------------------------------------------------------- */
+/* --- INCLUDES ------------------------------------------------------------- */
+/* -------------------------------------------------------------------------- */
+
+// POSIX.1-2001
+#include <dlfcn.h>
+
+// C++ includes
+#include <iostream>
+#include <sstream>
+
+// Boost includes
+#include <boost/program_options.hpp>
+
+// Dynamic Graph includes
+#include <dynamic-graph/pool.h>
+
+// Local includes
+#include <sot/core/sot-loader.hh>
+
+namespace po = boost::program_options;
+
+namespace dynamicgraph {
+namespace sot {
+
+SotLoader::SotLoader() {
+  dynamic_graph_stopped_ = true;
+  sot_external_interface_ = nullptr;
+  sot_dynamic_library_filename_ = "";
+  sot_dynamic_library_ = nullptr;
+  device_name_ = "";
+}
+
+SotLoader::~SotLoader() { cleanUp(); }
+
+int SotLoader::parseOptions(int argc, char *argv[]) {
+  po::options_description desc("Allowed options");
+  desc.add_options()("help", "produce help message")(
+      "sot-dynamic-library", po::value<std::string>(), "Library to load");
+  desc.add_options()("help", "produce help message")(
+      "input-file", po::value<std::string>(), "Library to load");
+
+  // Input variable map from the command line (int argc, char* argv[]).
+  po::variables_map vm;
+  po::store(po::parse_command_line(argc, argv, desc), vm);
+  po::notify(vm);
+
+  if (vm.count("help")) {
+    std::cout << desc << "\n";
+    return -1;
+  }
+  if (vm.count("sot-dynamic-library")) {
+    sot_dynamic_library_filename_ = vm["sot-dynamic-library"].as<std::string>();
+  } else if (vm.count("input-file")) {
+    sot_dynamic_library_filename_ = vm["input-file"].as<std::string>();
+  } else {
+    std::cout << "No filename specified\n";
+    return -1;
+  }
+  return 0;
+}
+
+bool SotLoader::initialization() {
+  // Load the library containing the AbstractSotExternalInterface.
+  sot_dynamic_library_ =
+      dlopen(sot_dynamic_library_filename_.c_str(), RTLD_LAZY | RTLD_GLOBAL);
+  if (!sot_dynamic_library_) {
+    std::cerr << "Cannot load library: " << dlerror() << '\n';
+    return false;
+  }
+
+  // reset errors
+  dlerror();
+
+  // Load the symbols.
+  createSotExternalInterface_t *createSotExternalInterface =
+      reinterpret_cast<createSotExternalInterface_t *>(reinterpret_cast<long>(
+          dlsym(sot_dynamic_library_, "createSotExternalInterface")));
+  const char *dlsym_error = dlerror();
+  if (dlsym_error) {
+    std::cerr << "Cannot load symbol create: " << dlsym_error << '\n';
+    return false;
+  }
+
+  // Create robot-controller
+  sot_external_interface_ = createSotExternalInterface();
+  assert(sot_external_interface_ && "Fail to create the sotExternalInterface");
+  std::cout << "SoT loaded at address [" << &sot_external_interface_
+            << "] from " << sot_dynamic_library_filename_ << "." << std::endl;
+
+  // Init the python interpreter.
+  std::string result, out, err;
+  // Debug print.
+  runPythonCommand("print(\"Executing python interpreter prologue...\")",
+                   result, out, err);
+  // make sure that the current environment variable are setup in the current
+  // python interpreter.
+  runPythonCommand("import sys, os", result, out, err);
+  runPythonCommand("print(\"python version:\", sys.version)", result, out, err);
+  runPythonCommand("pythonpath = os.environ.get('PYTHONPATH', '')", result, out,
+                   err);
+  runPythonCommand("path = []", result, out, err);
+  runPythonCommand(
+      "for p in pythonpath.split(':'):\n"
+      "  if p not in sys.path:\n"
+      "    path.append(p)",
+      result, out, err);
+  runPythonCommand("path.extend(sys.path)", result, out, err);
+  runPythonCommand("sys.path = path", result, out, err);
+  // used to be able to invoke rospy
+  runPythonCommand(
+      "if not hasattr(sys, \'argv\'):\n"
+      "    sys.argv  = ['sot']",
+      result, out, err);
+  // help setting signals
+  runPythonCommand("import numpy as np", result, out, err);
+  // Debug print.
+  runPythonCommand("print(\"Executing python interpreter prologue... Done\")",
+                   result, out, err);
+
+  return true;
+}
+
+void SotLoader::cleanUp() {
+  // Unregister the device first if it exists to avoid a double destruction from
+  // the pool of entity and the class that handle the Device pointer.
+  if (device_name_ != "") {
+    PoolStorage::getInstance()->deregisterEntity(device_name_);
+  }
+
+  // We do not destroy the FactoryStorage singleton because the module will not
+  // be reloaded at next initialization (because Python C API cannot safely
+  // unload a module...).
+  // SignalCaster singleton could probably be destroyed.
+  dynamicgraph::PoolStorage::destroy();
+
+  // Load the symbols.
+  if (sot_dynamic_library_ != nullptr && sot_external_interface_ != nullptr) {
+    destroySotExternalInterface_t *destroySotExternalInterface =
+        reinterpret_cast<destroySotExternalInterface_t *>(
+            reinterpret_cast<long>(
+                dlsym(sot_dynamic_library_, "destroySotExternalInterface")));
+    const char *dlsym_error = dlerror();
+    if (dlsym_error) {
+      std::cerr << "Cannot load symbol destroy: " << dlsym_error << '\n';
+      return;
+    }
+
+    destroySotExternalInterface(sot_external_interface_);
+    sot_external_interface_ = nullptr;
+
+    /// Uncount the number of access to this library.
+    dlclose(sot_dynamic_library_);
+    sot_dynamic_library_ = nullptr;
+  }
+}
+
+void SotLoader::runPythonCommand(const std::string &command,
+                                 std::string &result, std::string &out,
+                                 std::string &err) {
+  embeded_python_interpreter_.python(command, result, out, err);
+}
+
+void SotLoader::oneIteration(
+    std::map<std::string, SensorValues> &sensors_in,
+    std::map<std::string, ControlValues> &control_values) {
+  if (!dynamic_graph_stopped_) {
+    try {
+      sot_external_interface_->nominalSetSensors(sensors_in);
+      sot_external_interface_->getControl(control_values);
+    } catch (std::exception &e) {
+      std::cout << "Exception while running the graph:\n"
+                << e.what() << std::endl;
+      throw e;
+    }
+  }
+}
+
+void SotLoader::loadDeviceInPython(const std::string &device_name) {
+  std::string result, out, err;
+  // Debug print.
+  runPythonCommand("print(\"Load device from C++ to Python...\")", result, out,
+                   err);
+
+  // Import the Device entity declaration
+  runPythonCommand("from dynamic_graph.sot.core import Device", result, out,
+                   err);
+
+  // Get the existing C++ entity pointer in the Python interpreter.
+  runPythonCommand("loaded_device_name = \"" + device_name + "\"", result, out,
+                   err);
+  runPythonCommand("device_cpp_object = Device(loaded_device_name)", result,
+                   out, err);
+
+  // Debug print.
+  runPythonCommand("print(\"Load device from C++ to Python... Done!!\")",
+                   result, out, err);
+
+  // strore the device name to unregister it upon cleanup.
+  device_name_ = device_name;
+}
+
+} /* namespace sot */
+} /* namespace dynamicgraph */

src/tools/switch-python-module-py.cc

+#include <sot/core/switch.hh>
+
+#include "dynamic-graph/python/module.hh"
+
+namespace dg = dynamicgraph;
+typedef bp::return_value_policy<bp::reference_existing_object>
+    reference_existing_object;
+
+template <typename T, typename Time>
+void exposeSwitch() {
+  typedef dg::sot::Switch<T, Time> E_t;
+  typedef typename E_t::Base B_t;
+  dg::python::exposeEntity<E_t, bp::bases<dg::Entity>,
+                           dg::python::AddCommands>()
+      .def_readonly("sout", &E_t::SOUT)
+      .def("sin", &B_t::getSignalIn, reference_existing_object())
+      .add_property("n_sin", &B_t::getSignalNumber, &B_t::setSignalNumber,
+                    "the number of input signal.")
+      .def_readonly("selection", &E_t::selectionSIN)
+      .def_readonly("boolSelection", &E_t::boolSelectionSIN)
+
+      .def("setSignalNumber", &B_t::setSignalNumber,
+           "set the number of input signal.", bp::arg("size"))
+      .def("getSignalNumber", &B_t::getSignalNumber,
+           "get the number of input signal.", bp::arg("size"));
+}
+
+BOOST_PYTHON_MODULE(wrap) {
+  exposeSwitch<bool, int>();
+  exposeSwitch<dg::Vector, int>();
+  exposeSwitch<dg::sot::MatrixHomogeneous, int>();
+}

src/tools/switch.cpp

+// Copyright (c) 2017, Joseph Mirabel
+// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
+
+#include <dynamic-graph/factory.h>
+
+#include <sot/core/switch.hh>
+
+#include "type-name-helper.hh"
+
+namespace dynamicgraph {
+namespace sot {
+template <typename Tin, typename Tout, typename Time>
+std::string VariadicAbstract<Tin, Tout, Time>::getTypeInName(void) {
+  return TypeNameHelper<Tin>::typeName();
+}
+template <typename Tin, typename Tout, typename Time>
+std::string VariadicAbstract<Tin, Tout, Time>::getTypeOutName(void) {
+  return TypeNameHelper<Tout>::typeName();
+}
+
+template class VariadicAbstract<Vector, Vector, int>;
+
+typedef Switch<Vector, int> SwitchVector;
+template <>
+DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(SwitchVector, "SwitchVector");
+
+template class VariadicAbstract<bool, bool, int>;
+
+typedef Switch<bool, int> SwitchBool;
+template <>
+DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(SwitchBool, "SwitchBoolean");
+
+template class VariadicAbstract<MatrixHomogeneous, MatrixHomogeneous, int>;
+
+typedef Switch<MatrixHomogeneous, int> SwitchMatrixHomogeneous;
+template <>
+DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(SwitchMatrixHomogeneous,
+                                   "SwitchMatrixHomogeneous");
+}  // namespace sot
+}  // namespace dynamicgraph

src/tools/time-stamp-python.h

+#include <sot/core/time-stamp.hh>
+
+typedef boost::mpl::vector<dynamicgraph::sot::TimeStamp> entities_t;

src/tools/time-stamp.cpp

-/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
- * Copyright Projet JRL-Japan, 2007
- *+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+/*
+ * Copyright 2010,
+ * François Bleibel,
+ * Olivier Stasse,
  *
- * File:      TimeStamp.cpp
- * Project:   SOT
- * Author:    Paul Evrard
+ * CNRS/AIST
  *
- * Version control
- * ===============
- *
- *  $Id$
- *
- * Description
- * ============
- *
- *
- * ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
-
+ */
 
 /* --------------------------------------------------------------------- */
 /* --- INCLUDE --------------------------------------------------------- */
 /* --------------------------------------------------------------------- */
 
 /* SOT */
-#include <sot-core/time-stamp.h>
-#include <sot-core/matrix-homogeneous.h>
 #include <dynamic-graph/factory.h>
-#include <sot-core/macros-signal.h>
+
+#include <sot/core/macros-signal.hh>
+#include <sot/core/matrix-geometry.hh>
+#include <sot/core/time-stamp.hh>
 
 /* --------------------------------------------------------------------- */
 /* --- CLASS ----------------------------------------------------------- */
 /* --------------------------------------------------------------------- */
 
 using namespace dynamicgraph;
-using namespace sot;
-
-DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(TimeStamp,"TimeStamp");
+using namespace dynamicgraph::sot;
 
+DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(TimeStamp, "TimeStamp");
 
 /* --- CONSTRUCTION ---------------------------------------------------- */
-TimeStamp::
-TimeStamp( const std::string& name )
-  :Entity(name)
-   ,offsetValue( 0 ),offsetSet(false)
-    ,timeSOUT( "TimeStamp("+name+")::output(vector2)::time" )
-   ,timeDoubleSOUT( "TimeStamp("+name+")::output(double)::timeDouble" )
-   ,timeOnceSOUT( boost::bind(&TimeStamp::getTimeStamp,this,_1,_2),
-		  sotNOSIGNAL,
-		  "TimeStamp("+name+")::output(vector2)::synchro" )
-   ,timeOnceDoubleSOUT(  boost::bind(&TimeStamp::getTimeStampDouble,this,
-				     SOT_CALL_SIG(timeSOUT,ml::Vector),_1),
-			 timeSOUT,
-			"TimeStamp("+name+")::output(double)::synchroDouble" )
-{
+TimeStamp::TimeStamp(const std::string &name)
+    : Entity(name),
+      offsetValue(0),
+      offsetSet(false),
+      timeSOUT("TimeStamp(" + name + ")::output(vector2)::time"),
+      timeDoubleSOUT("TimeStamp(" + name + ")::output(double)::timeDouble"),
+      timeOnceSOUT(boost::bind(&TimeStamp::getTimeStamp, this, _1, _2),
+                   sotNOSIGNAL,
+                   "TimeStamp(" + name + ")::output(vector2)::synchro"),
+      timeOnceDoubleSOUT(
+          boost::bind(&TimeStamp::getTimeStampDouble, this,
+                      SOT_CALL_SIG(timeSOUT, dynamicgraph::Vector), _1),
+          timeSOUT, "TimeStamp(" + name + ")::output(double)::synchroDouble") {
   sotDEBUGIN(15);
-  
-  timeSOUT.setFunction( boost::bind(&TimeStamp::getTimeStamp,this,_1,_2) );
-  timeDoubleSOUT.setFunction( boost::bind(&TimeStamp::getTimeStampDouble,this,
- 					  SOT_CALL_SIG(timeSOUT,ml::Vector),_1) );
-  timeOnceSOUT.setNeedUpdateFromAllChildren( true );
-  timeOnceDoubleSOUT.setNeedUpdateFromAllChildren( true );
-  signalRegistration( timeSOUT << timeDoubleSOUT 
-		      << timeOnceSOUT << timeOnceDoubleSOUT );
 
-  gettimeofday( &val,NULL );
+  timeSOUT.setFunction(boost::bind(&TimeStamp::getTimeStamp, this, _1, _2));
+  timeDoubleSOUT.setFunction(
+      boost::bind(&TimeStamp::getTimeStampDouble, this,
+                  SOT_CALL_SIG(timeSOUT, dynamicgraph::Vector), _1));
+  timeOnceSOUT.setNeedUpdateFromAllChildren(true);
+  timeOnceDoubleSOUT.setNeedUpdateFromAllChildren(true);
+  signalRegistration(timeSOUT << timeDoubleSOUT << timeOnceSOUT
+                              << timeOnceDoubleSOUT);
+
+  gettimeofday(&val, NULL);
 
   sotDEBUGOUT(15);
 }
 
 /* --- DISPLAY --------------------------------------------------------- */
-void TimeStamp::
-display( std::ostream& os ) const
-{
-  os << "TimeStamp <> : " << val.tv_sec << "s; "
-     << val.tv_usec << "us." << std::endl;
+void TimeStamp::display(std::ostream &os) const {
+  os << "TimeStamp <> : " << val.tv_sec << "s; " << val.tv_usec << "us."
+     << std::endl;
 }
 
 /* --------------------------------------------------------------------- */
 /* --- CONTROL --------------------------------------------------------- */
 /* --------------------------------------------------------------------- */
 
-ml::Vector& TimeStamp::
-getTimeStamp( ml::Vector& res,const int& time )
-{ 
+dynamicgraph::Vector &TimeStamp::getTimeStamp(dynamicgraph::Vector &res,
+                                              const int & /*time*/) {
   sotDEBUGIN(15);
-  gettimeofday( &val,NULL );
-  if( res.size()!=2 ) res.resize(2);
-  
+  gettimeofday(&val, NULL);
+  if (res.size() != 2) res.resize(2);
 
-    res(0) = val.tv_sec;
-  res(1) = val.tv_usec;
+  res(0) = static_cast<double>(val.tv_sec);
+  res(1) = static_cast<double>(val.tv_usec);
   sotDEBUGOUT(15);
   return res;
 }
 
-double& TimeStamp::
-getTimeStampDouble( const ml::Vector& vect,double& res )
-{ 
+double &TimeStamp::getTimeStampDouble(const dynamicgraph::Vector &vect,
+                                      double &res) {
   sotDEBUGIN(15);
 
-  if( offsetSet ) res = (vect(0)-offsetValue)*1000;
-    else res = vect(0)*1000;
-  res += vect(1)/1000;
+  if (offsetSet)
+    res = (vect(0) - offsetValue) * 1000;
+  else
+    res = vect(0) * 1000;
+  res += vect(1) / 1000;
   sotDEBUGOUT(15);
   return res;
 }
-
-
-void TimeStamp::
-commandLine( const std::string& cmdLine,
-	     std::istringstream& cmdArgs,
-	     std::ostream& os )
-{
-  if( cmdLine=="help" )
-    {
-      os << "TimeStamp: "<<std::endl
-	 << " - offset [{<value>|now}] : set/get the offset for double sig." << std::endl;      
-      Entity::commandLine( cmdLine,cmdArgs,os );
-    }
-  else if( cmdLine=="offset" )
-    {
-      cmdArgs >> std::ws; 
-      if( cmdArgs.good() )
-	{ 
-	  std::string offnow; 
-	  cmdArgs >> offnow;  
-	  if(offnow=="now") 
-	    {
-	      gettimeofday( &val,NULL );
-	      offsetValue = val.tv_sec;
-	    }
-	  else { offsetValue = atoi(offnow.c_str()); }
-	  offsetSet = ( offsetValue>0 );
-	} else {
-	  os << "offset = " << (offsetSet ? offsetValue : 0) << std::endl;
-	} 
-    }
-  else { Entity::commandLine( cmdLine,cmdArgs,os ); }
-}
-
-

src/tools/timer-python.h

+#include <sot/core/matrix-geometry.hh>
+#include <sot/core/timer.hh>
+
+typedef boost::mpl::vector<
+    Timer<dynamicgraph::Vector>, Timer<dynamicgraph::Matrix>,
+    Timer<dynamicgraph::sot::MatrixHomogeneous>, Timer<double> >
+    entities_t;

src/tools/timer.cpp

-/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
- * Copyright Projet JRL-Japan, 2007
- *+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+/*
+ * Copyright 2010,
+ * François Bleibel,
+ * Olivier Stasse,
  *
- * File:      Timer.cpp
- * Project:   SOT
- * Author:    Nicolas Mansard
+ * CNRS/AIST
  *
- * Version control
- * ===============
- *
- *  $Id$
- *
- * Description
- * ============
- *
- *
- * ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
-
+ */
 
 /* --------------------------------------------------------------------- */
 /* --- INCLUDE --------------------------------------------------------- */
 /* --------------------------------------------------------------------- */
 
 /* SOT */
-#include <sot-core/timer.h>
-#include <MatrixAbstractLayer/boost.h>
-#include <sot-core/matrix-homogeneous.h>
-#include <sot-core/factory.h>
+#include <dynamic-graph/linear-algebra.h>
 
-using namespace sot;
+#include <sot/core/factory.hh>
+#include <sot/core/matrix-geometry.hh>
+#include <sot/core/timer.hh>
+
+using namespace dynamicgraph::sot;
 using namespace dynamicgraph;
 
 /* --------------------------------------------------------------------- */
 /* --- CLASS ----------------------------------------------------------- */
 /* --------------------------------------------------------------------- */
 
-typedef Timer<maal::boost::Vector> timevect;
-template <> 
-DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(timevect,"Timer<Vector>");
+typedef Timer<dynamicgraph::Vector> timevect;
+template <>
+DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(timevect, "TimerVector");
 
-typedef Timer<maal::boost::Matrix> timematrix;
-template <> 
-DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(timematrix,"Timer<Matrix>");
+typedef Timer<dynamicgraph::Matrix> timematrix;
+template <>
+DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(timematrix, "TimerMatrix");
 
 typedef Timer<MatrixHomogeneous> timematrixhomo;
-template <> 
-DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(timematrixhomo,"Timer<MatrixHomo>");
+template <>
+DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(timematrixhomo, "TimerMatrixHomo");
 
 typedef Timer<double> timedouble;
-template <> 
-DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(timedouble,"Timer<double>");
-
+template <>
+DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(timedouble, "TimerDouble");
 
 /* --------------------------------------------------------------------- */
-void 
-cmdChrono( const std::string& cmdLine,
-	   std::istringstream& cmdArg,
-	   std::ostream& os )
-{
+void cmdChrono(const std::string &cmdLine, std::istringstream &cmdArg,
+               std::ostream &os) {
   sotDEBUGIN(15);
 
-  if( cmdLine == "help" ) 
-    {
-      os << "  - chrono <cmd...>"
-	 << "\t\t\t\tLaunch <cmd> and display the time spent in the operation." <<std::endl;
-      return;
-    }
-  
-  struct timeval t0,t1;
+  if (cmdLine == "help") {
+    os << "  - chrono <cmd...>"
+       << "\t\t\t\tLaunch <cmd> and display the time spent in the operation."
+       << std::endl;
+    return;
+  }
+
+  struct timeval t0, t1;
   double dt;
 
-  gettimeofday(&t0,NULL);
-  sotDEBUG(15) << "t0: "<< t0.tv_sec << " - " << t0.tv_usec << std::endl;
-  
-  std::string cmdLine2;  cmdArg>>cmdLine2;
-  sotDEBUG(5)<<"Chrono <" <<cmdLine2<<">"<<std::endl;
-  g_shell.cmd(cmdLine2,cmdArg,os);
+  gettimeofday(&t0, NULL);
+  sotDEBUG(15) << "t0: " << t0.tv_sec << " - " << t0.tv_usec << std::endl;
+
+  std::string cmdLine2;
+  cmdArg >> cmdLine2;
+  sotDEBUG(5) << "Chrono <" << cmdLine2 << ">" << std::endl;
+  // Florent: remove reference to g_shell
+  // g_shell.cmd(cmdLine2,cmdArg,os);
 
-  gettimeofday(&t1,NULL);
-  dt = ( (t1.tv_sec-t0.tv_sec) * 1000. 
-	 + (t1.tv_usec-t0.tv_usec+0.) / 1000. );
-  sotDEBUG(15) << "t1: "<< t1.tv_sec << " - " << t1.tv_usec << std::endl;
+  gettimeofday(&t1, NULL);
+  dt = ((static_cast<double>(t1.tv_sec) - static_cast<double>(t0.tv_sec)) *
+            1000. +
+        (static_cast<double>(t1.tv_usec) - static_cast<double>(t0.tv_usec) +
+         0.) /
+            1000.);
+  sotDEBUG(15) << "t1: " << t1.tv_sec << " - " << t1.tv_usec << std::endl;
 
   os << "Time spent = " << dt << " ms " << std::endl;
 
   sotDEBUGOUT(15);
 }
 
-
-
-extern "C" {
-  ShellFunctionRegisterer regTimer1
-  ( "chrono",boost::bind(cmdChrono,_1,_2,_3) );
-}
-
-
 /* --------------------------------------------------------------------- */
 /* --- CONTROL --------------------------------------------------------- */
 /* --------------------------------------------------------------------- */
-
-
-