include/sot/talos_balance/simple-pid.hh

+/*
+ * Copyright 2018, Gepetto team, LAAS-CNRS
+ *
+ * This file is part of sot-talos-balance.
+ * sot-talos-balance 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-talos-balance 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-talos-balance.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __sot_talos_balance_simple_pid_H__
+#define __sot_talos_balance_simple_pid_H__
+
+/* --------------------------------------------------------------------- */
+/* --- API ------------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+#if defined(WIN32)
+#if defined(simple_pid_EXPORTS)
+#define SIMPLE_PID_EXPORT __declspec(dllexport)
+#else
+#define SIMPLE_PID_EXPORT __declspec(dllimport)
+#endif
+#else
+#define SIMPLE_PID_EXPORT
+#endif
+
+/* --------------------------------------------------------------------- */
+/* --- INCLUDE --------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+#include <dynamic-graph/signal-helper.h>
+
+#include <map>
+
+#include "boost/assign.hpp"
+
+namespace dynamicgraph {
+namespace sot {
+namespace talos_balance {
+
+/* --------------------------------------------------------------------- */
+/* --- CLASS ----------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+class SIMPLE_PID_EXPORT SimplePID : public ::dynamicgraph::Entity {
+  DYNAMIC_GRAPH_ENTITY_DECL();
+
+ public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  /* --- CONSTRUCTOR ---- */
+  SimplePID(const std::string& name);
+
+  void init(const double& dt, const int& N);
+
+  void resetIntegralError();
+
+  /* --- SIGNALS --- */
+  DECLARE_SIGNAL_IN(Kp, dynamicgraph::Vector);
+  DECLARE_SIGNAL_IN(Ki, dynamicgraph::Vector);
+
+  DECLARE_SIGNAL_IN(decayFactor, double);
+
+  DECLARE_SIGNAL_IN(x, dynamicgraph::Vector);
+  DECLARE_SIGNAL_IN(x_des, dynamicgraph::Vector);
+
+  DECLARE_SIGNAL_IN(dx_des, dynamicgraph::Vector);
+
+  DECLARE_SIGNAL_OUT(dx_ref, dynamicgraph::Vector);
+
+  /* --- COMMANDS --- */
+  /* --- ENTITY INHERITANCE --- */
+  virtual void display(std::ostream& os) const;
+
+ protected:
+  bool
+      m_initSucceeded;  /// true if the entity has been successfully initialized
+  dynamicgraph::Vector m_integralError;  // internal state
+  double m_dt;
+
+};  // class SimplePID
+
+}  // namespace talos_balance
+}  // namespace sot
+}  // namespace dynamicgraph
+
+#endif  // #ifndef __sot_talos_balance_simple_pid_H__

include/sot/talos_balance/simple-pidd.hh

+/*
+ * Copyright 2018, Gepetto team, LAAS-CNRS
+ *
+ * This file is part of sot-talos-balance.
+ * sot-talos-balance 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-talos-balance 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-talos-balance.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __sot_talos_balance_simple_pidd_H__
+#define __sot_talos_balance_simple_pidd_H__
+
+/* --------------------------------------------------------------------- */
+/* --- API ------------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+#if defined(WIN32)
+#if defined(simple_pidd_EXPORTS)
+#define SIMPLE_PIDD_EXPORT __declspec(dllexport)
+#else
+#define SIMPLE_PIDD_EXPORT __declspec(dllimport)
+#endif
+#else
+#define SIMPLE_PIDD_EXPORT
+#endif
+
+/* --------------------------------------------------------------------- */
+/* --- INCLUDE --------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+#include <dynamic-graph/signal-helper.h>
+
+#include <map>
+
+#include "boost/assign.hpp"
+
+namespace dynamicgraph {
+namespace sot {
+namespace talos_balance {
+
+/* --------------------------------------------------------------------- */
+/* --- CLASS ----------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+class SIMPLE_PIDD_EXPORT SimplePIDD : public ::dynamicgraph::Entity {
+  DYNAMIC_GRAPH_ENTITY_DECL();
+
+ public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  /* --- CONSTRUCTOR ---- */
+  SimplePIDD(const std::string& name);
+
+  void init(const double& dt, const int& N);
+
+  void resetVelocity();
+  void resetIntegralError();
+
+  /* --- SIGNALS --- */
+  DECLARE_SIGNAL_IN(Kp, dynamicgraph::Vector);
+  DECLARE_SIGNAL_IN(Ki, dynamicgraph::Vector);
+  DECLARE_SIGNAL_IN(Kd, dynamicgraph::Vector);
+
+  DECLARE_SIGNAL_IN(decayFactor, double);
+
+  DECLARE_SIGNAL_IN(x, dynamicgraph::Vector);
+  DECLARE_SIGNAL_IN(x_des, dynamicgraph::Vector);
+
+  DECLARE_SIGNAL_IN(dx, dynamicgraph::Vector);
+  DECLARE_SIGNAL_IN(dx_des, dynamicgraph::Vector);
+
+  DECLARE_SIGNAL_IN(ddx_des, dynamicgraph::Vector);
+
+  DECLARE_SIGNAL_OUT(ddx_ref, dynamicgraph::Vector);
+  DECLARE_SIGNAL_OUT(dx_ref, dynamicgraph::Vector);
+
+  /* --- COMMANDS --- */
+  /* --- ENTITY INHERITANCE --- */
+  virtual void display(std::ostream& os) const;
+
+ protected:
+  bool
+      m_initSucceeded;  /// true if the entity has been successfully initialized
+  dynamicgraph::Vector m_dx_ref;         // internal state
+  dynamicgraph::Vector m_integralError;  // internal state
+  double m_dt;
+
+};  // class SimplePIDD
+
+}  // namespace talos_balance
+}  // namespace sot
+}  // namespace dynamicgraph
+
+#endif  // #ifndef __sot_talos_balance_simple_pidd_H__

include/sot/talos_balance/simple-reference-frame.hh

+/*
+ * Copyright 2018, Gepetto team, LAAS-CNRS
+ *
+ * This file is part of sot-talos-balance.
+ * sot-talos-balance 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-talos-balance 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-talos-balance.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __sot_talos_balance_simple_reference_frame_H__
+#define __sot_talos_balance_simple_reference_frame_H__
+
+/* --------------------------------------------------------------------- */
+/* --- API ------------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+#if defined(WIN32)
+#if defined(simple_reference_frame_EXPORTS)
+#define SIMPLEREFERENCEFRAME_EXPORT __declspec(dllexport)
+#else
+#define SIMPLEREFERENCEFRAME_EXPORT __declspec(dllimport)
+#endif
+#else
+#define SIMPLEREFERENCEFRAME_EXPORT
+#endif
+
+/* --------------------------------------------------------------------- */
+/* --- INCLUDE --------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+#include <pinocchio/fwd.hpp>
+
+// include pinocchio first
+
+#include <dynamic-graph/linear-algebra.h>
+#include <dynamic-graph/signal-helper.h>
+
+#include <map>
+#include <sot/core/matrix-geometry.hh>
+#include <sot/core/robot-utils.hh>
+
+#include "boost/assign.hpp"
+
+namespace dynamicgraph {
+namespace sot {
+namespace talos_balance {
+
+/* --------------------------------------------------------------------- */
+/* --- CLASS ----------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+class SIMPLEREFERENCEFRAME_EXPORT SimpleReferenceFrame
+    : public ::dynamicgraph::Entity {
+  DYNAMIC_GRAPH_ENTITY_DECL();
+
+ public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  /* --- CONSTRUCTOR ---- */
+  SimpleReferenceFrame(const std::string& name);
+
+  void init(const std::string& robotName);
+
+  /* --- SIGNALS --- */
+  DECLARE_SIGNAL_IN(footLeft, MatrixHomogeneous);
+  DECLARE_SIGNAL_IN(footRight, MatrixHomogeneous);
+  DECLARE_SIGNAL_IN(reset, bool);
+
+  DECLARE_SIGNAL_OUT(referenceFrame, MatrixHomogeneous);
+
+  /* --- COMMANDS --- */
+  /* --- ENTITY INHERITANCE --- */
+  virtual void display(std::ostream& os) const;
+
+ protected:
+  RobotUtilShrPtr m_robot_util;
+  Vector m_rightFootSoleXYZ;
+  MatrixHomogeneous m_referenceFrame;
+  bool m_first;
+  bool
+      m_initSucceeded;  /// true if the entity has been successfully initialized
+
+};  // class SimpleReferenceFrame
+
+}  // namespace talos_balance
+}  // namespace sot
+}  // namespace dynamicgraph
+
+#endif  // #ifndef __sot_talos_balance_simple_reference_frame_H__

include/sot/talos_balance/simple-state-integrator.hh

+/*
+ * Copyright 2018, Gepetto team, LAAS-CNRS
+ *
+ * This file is part of sot-talos-balance.
+ * sot-talos-balance 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-talos-balance 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-talos-balance.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __sot_talos_balance_simple_state_integrator_H__
+#define __sot_talos_balance_simple_state_integrator_H__
+
+/* --------------------------------------------------------------------- */
+/* --- API ------------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+#if defined(WIN32)
+#if defined(simple_state_integrator_EXPORTS)
+#define SIMPLE_STATE_INTEGRATOR_EXPORT __declspec(dllexport)
+#else
+#define SIMPLE_STATE_INTEGRATOR_EXPORT __declspec(dllimport)
+#endif
+#else
+#define SIMPLE_STATE_INTEGRATOR_EXPORT
+#endif
+
+/* --------------------------------------------------------------------- */
+/* --- INCLUDE --------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+#include <dynamic-graph/linear-algebra.h>
+#include <dynamic-graph/signal-helper.h>
+
+#include <map>
+#include <sot/core/matrix-geometry.hh>
+
+#include "boost/assign.hpp"
+
+namespace dynamicgraph {
+namespace sot {
+namespace talos_balance {
+
+/* --------------------------------------------------------------------- */
+/* --- CLASS ----------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+class SIMPLE_STATE_INTEGRATOR_EXPORT SimpleStateIntegrator
+    : public ::dynamicgraph::Entity {
+  DYNAMIC_GRAPH_ENTITY_DECL();
+
+ protected:
+  /// \brief Current integration step.
+  double timestep_;
+
+  /// \name Vectors related to the state.
+  ///@{
+  /// Position of the robot wrt pinocchio.
+  Eigen::VectorXd state_;
+  /// Velocity of the robot wrt pinocchio.
+  Eigen::VectorXd velocity_;
+  ///@}
+
+ public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  /* --- CONSTRUCTOR ---- */
+  SimpleStateIntegrator(const std::string& name);
+
+  void init(const double& step);
+
+  /* --- SIGNALS --- */
+  DECLARE_SIGNAL_IN(control, ::dynamicgraph::Vector);
+
+  DECLARE_SIGNAL_OUT(state, ::dynamicgraph::Vector);
+  DECLARE_SIGNAL_OUT(velocity, ::dynamicgraph::Vector);
+
+ public:
+  void setState(const ::dynamicgraph::Vector& st);
+  void setVelocity(const ::dynamicgraph::Vector& vel);
+
+  /* --- COMMANDS --- */
+  /* --- ENTITY INHERITANCE --- */
+  virtual void display(std::ostream& os) const;
+
+ protected:
+  /// Integrate the freeflyer state (to obtain position).
+  /// Compute roll pitch yaw angles
+  void integrateRollPitchYaw(::dynamicgraph::Vector& state,
+                             const ::dynamicgraph::Vector& control, double dt);
+  // Computes Euler angles in good range : [-pi:pi]x[-pi/2:pi/2]x[-pi:pi]
+  void rotationMatrixToEuler(const Eigen::Matrix3d& rotationMatrix,
+                             Eigen::Vector3d& rollPitchYaw);
+
+};  // class SimpleStateIntegrator
+
+}  // namespace talos_balance
+}  // namespace sot
+}  // namespace dynamicgraph
+
+#endif  // #ifndef __sot_talos_balance_simple_state_integrator_H__

include/sot/talos_balance/simple-zmp-estimator.hh

@@ -21,78 +21,78 @@
 /* --- API ------------------------------------------------------------- */
 /* --------------------------------------------------------------------- */
 
-#if defined (WIN32)
-#  if defined (simple_zmp_estimator_EXPORTS)
-#    define SIMPLEZMPESTIMATOR_EXPORT __declspec(dllexport)
-#  else
-#    define SIMPLEZMPESTIMATOR_EXPORT __declspec(dllimport)
-#  endif
+#if defined(WIN32)
+#if defined(simple_zmp_estimator_EXPORTS)
+#define SIMPLEZMPESTIMATOR_EXPORT __declspec(dllexport)
 #else
-#  define SIMPLEZMPESTIMATOR_EXPORT
+#define SIMPLEZMPESTIMATOR_EXPORT __declspec(dllimport)
+#endif
+#else
+#define SIMPLEZMPESTIMATOR_EXPORT
 #endif
-
 
 /* --------------------------------------------------------------------- */
 /* --- INCLUDE --------------------------------------------------------- */
 /* --------------------------------------------------------------------- */
 
-#include "utils/signal-helper.hh"
-#include "utils/logger.hh"
+#include <dynamic-graph/linear-algebra.h>
+#include <dynamic-graph/signal-helper.h>
+
 #include <map>
-#include "boost/assign.hpp"
 #include <sot/core/matrix-geometry.hh>
-#include <dynamic-graph/linear-algebra.h>
 
-namespace dynamicgraph {
-  namespace sot {
-    namespace talos_balance {
+#include "boost/assign.hpp"
 
-      /* --------------------------------------------------------------------- */
-      /* --- CLASS ----------------------------------------------------------- */
-      /* --------------------------------------------------------------------- */
+namespace dynamicgraph {
+namespace sot {
+namespace talos_balance {
 
-      class SIMPLEZMPESTIMATOR_EXPORT SimpleZmpEstimator
-	                         : public ::dynamicgraph::Entity
-      {
-        DYNAMIC_GRAPH_ENTITY_DECL();
+/* --------------------------------------------------------------------- */
+/* --- CLASS ----------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
 
-      public:
-        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+class SIMPLEZMPESTIMATOR_EXPORT SimpleZmpEstimator
+    : public ::dynamicgraph::Entity {
+  DYNAMIC_GRAPH_ENTITY_DECL();
 
-        /* --- CONSTRUCTOR ---- */
-        SimpleZmpEstimator( const std::string & name );
+ public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
-        void init();
+  /* --- CONSTRUCTOR ---- */
+  SimpleZmpEstimator(const std::string& name, const double& eps = 1.0);
 
-        /* --- SIGNALS --- */
-        DECLARE_SIGNAL_IN(wrenchLeft, dynamicgraph::Vector);
-        DECLARE_SIGNAL_IN(wrenchRight, dynamicgraph::Vector);
+  void init();
 
-        DECLARE_SIGNAL_IN(poseLeft, MatrixHomogeneous);
-        DECLARE_SIGNAL_IN(poseRight, MatrixHomogeneous);    
+  /* --- SIGNALS --- */
+  DECLARE_SIGNAL_IN(wrenchLeft, dynamicgraph::Vector);
+  DECLARE_SIGNAL_IN(wrenchRight, dynamicgraph::Vector);
 
-        DECLARE_SIGNAL_OUT(copLeft, dynamicgraph::Vector);
-        DECLARE_SIGNAL_OUT(copRight, dynamicgraph::Vector);
-        DECLARE_SIGNAL_OUT(zmp, dynamicgraph::Vector);
+  DECLARE_SIGNAL_IN(poseLeft, MatrixHomogeneous);
+  DECLARE_SIGNAL_IN(poseRight, MatrixHomogeneous);
 
-        /* --- COMMANDS --- */
-        /* --- ENTITY INHERITANCE --- */
-        virtual void display( std::ostream& os ) const;
+  DECLARE_SIGNAL_OUT(copLeft, dynamicgraph::Vector);
+  DECLARE_SIGNAL_OUT(copRight, dynamicgraph::Vector);
+  DECLARE_SIGNAL_OUT(zmp, dynamicgraph::Vector);
+  DECLARE_SIGNAL_OUT(emergencyStop, bool);
 
-        void sendMsg(const std::string& msg, MsgType t=MSG_TYPE_INFO, const char* file="", int line=0)
-        {
-          getLogger().sendMsg("[SimpleZmpEstimator-"+name+"] "+msg, t, file, line);
-        }
+  double m_eps;
 
-      protected:
-        bool m_initSucceeded;    /// true if the entity has been successfully initialized
+  /* --- COMMANDS --- */
+  /* --- ENTITY INHERITANCE --- */
+  virtual void display(std::ostream& os) const;
 
-      }; // class SimpleZmpEstimator
+  Eigen::Vector3d computeCoP(const dynamicgraph::Vector& wrench,
+                             const MatrixHomogeneous& pose) const;
 
-    }    // namespace talos_balance
-  }      // namespace sot
-}        // namespace dynamicgraph
+ protected:
+  bool
+      m_initSucceeded;  /// true if the entity has been successfully initialized
+  bool m_emergency_stop_triggered;
 
+};  // class SimpleZmpEstimator
 
+}  // namespace talos_balance
+}  // namespace sot
+}  // namespace dynamicgraph
 
-#endif // #ifndef __sot_talos_balance_simple_zmp_estimator_H__
+#endif  // #ifndef __sot_talos_balance_simple_zmp_estimator_H__

include/sot/talos_balance/state-transformation.hh

+/*
+ * Copyright 2018, Gepetto team, LAAS-CNRS
+ *
+ * This file is part of sot-talos-balance.
+ * sot-talos-balance 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-talos-balance 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-talos-balance.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __sot_talos_balance_state_transformation_H__
+#define __sot_talos_balance_state_transformation_H__
+
+/* --------------------------------------------------------------------- */
+/* --- API ------------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+#if defined(WIN32)
+#if defined(state_transformation_EXPORTS)
+#define STATETRANSFORMATION_EXPORT __declspec(dllexport)
+#else
+#define STATETRANSFORMATION_EXPORT __declspec(dllimport)
+#endif
+#else
+#define STATETRANSFORMATION_EXPORT
+#endif
+
+#include <sot/core/robot-utils.hh>
+/* --------------------------------------------------------------------- */
+/* --- INCLUDE --------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+#include <dynamic-graph/linear-algebra.h>
+#include <dynamic-graph/signal-helper.h>
+
+#include <map>
+#include <sot/core/matrix-geometry.hh>
+
+#include "boost/assign.hpp"
+
+namespace dynamicgraph {
+namespace sot {
+namespace talos_balance {
+
+/* --------------------------------------------------------------------- */
+/* --- CLASS ----------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+class STATETRANSFORMATION_EXPORT StateTransformation
+    : public ::dynamicgraph::Entity {
+  DYNAMIC_GRAPH_ENTITY_DECL();
+
+ public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  /* --- CONSTRUCTOR ---- */
+  StateTransformation(const std::string& name);
+
+  void init();
+
+  /* --- SIGNALS --- */
+  DECLARE_SIGNAL_IN(referenceFrame, MatrixHomogeneous);
+  DECLARE_SIGNAL_IN(q_in, dynamicgraph::Vector);
+  DECLARE_SIGNAL_IN(v_in, dynamicgraph::Vector);
+
+  DECLARE_SIGNAL_OUT(q, dynamicgraph::Vector);
+  DECLARE_SIGNAL_OUT(v, dynamicgraph::Vector);
+
+  /* --- COMMANDS --- */
+  /* --- ENTITY INHERITANCE --- */
+  virtual void display(std::ostream& os) const;
+
+ protected:
+  bool
+      m_initSucceeded;  /// true if the entity has been successfully initialized
+
+};  // class StateTransformation
+
+}  // namespace talos_balance
+}  // namespace sot
+}  // namespace dynamicgraph
+
+#endif  // #ifndef __sot_talos_balance_state_transformation_H__

include/sot/talos_balance/stc-commands.hh

-/*
- * Copyright 2010-2017,
- * Florent Lamiraux, Rohan Budhiraja
- *
- * CNRS/AIST
- * File derived from sot-torque-control package available on 
- * https://github.com/stack-of-tasks/sot-torque-control
- *
- * This file is part of sot-talos-balance.
- * sot-talos-balance 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-talos-balance.  If not, see <http://www.gnu.org/licenses/>.
- */
-
-#ifndef STC_COMMAND_HH
-#define STC_COMMAND_HH
-
-#include <fstream>
-#include <boost/assign/list_of.hpp>
-
-#include <dynamic-graph/command.h>
-#include <dynamic-graph/command-setter.h>
-#include <dynamic-graph/command-getter.h>
-#include <sot/talos_balance/joint-trajectory-generator.hh>
-
-namespace dynamicgraph { namespace sot {
-    namespace command {
-      using ::dynamicgraph::command::Command;
-      using ::dynamicgraph::command::Value;
-      using ::dynamicgraph::sot::talos_balance::JointTrajectoryGenerator;
-      // Command DisplayModel
-      class IsTrajectoryEnded : public Command
-      {
-      public:
-        virtual ~IsTrajectoryEnded() {	sotDEBUGIN(15);
-          sotDEBUGOUT(15);}
-        /// Create command and store it in Entity
-        /// \param entity instance of Entity owning this command
-        /// \param docstring documentation of the command
-        IsTrajectoryEnded(JointTrajectoryGenerator& entity, const std::string& docstring) :
-          Command(entity, std::vector<Value::Type>(),
-                  docstring)
-        {
-        }
-        virtual Value doExecute()
-        {
-          JointTrajectoryGenerator& jtg = static_cast<JointTrajectoryGenerator&>(owner());
-          bool output = jtg.isTrajectoryEnded();
-          return Value(output);
-        }
-      }; // class IsTrajectoryEnded
-
-    } // namespace command
-  } /* namespace sot */
-} /* namespace dynamicgraph */
-
-#endif //STC_COMMAND_HH

include/sot/talos_balance/talos-base-estimator.hh

+/*
+ * Copyright 2017, Thomas Flayols, 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/>.
+ */
+
+#ifndef __sot_torque_control_base_estimator_H__
+#define __sot_torque_control_base_estimator_H__
+
+/* --------------------------------------------------------------------- */
+/* --- API ------------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+#if defined(WIN32)
+#if defined(base_estimator_EXPORTS)
+#define TALOS_BASE_ESTIMATOR_EXPORT __declspec(dllexport)
+#else
+#define TALOS_BASE_ESTIMATOR_EXPORT __declspec(dllimport)
+#endif
+#else
+#define TALOS_BASE_ESTIMATOR_EXPORT
+#endif
+
+/* --------------------------------------------------------------------- */
+/* --- INCLUDE --------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+#include <pinocchio/fwd.hpp>
+// include pinocchio first
+#include <dynamic-graph/signal-helper.h>
+
+#include <map>
+#include <sot/core/matrix-geometry.hh>
+#include <sot/core/robot-utils.hh>
+
+#include "boost/assign.hpp"
+// #include <boost/random/normal_distribution.hpp>
+#include <boost/math/distributions/normal.hpp>  // for normal_distribution
+
+/* Pinocchio */
+#include <pinocchio/algorithm/kinematics.hpp>
+#include <pinocchio/multibody/model.hpp>
+#include <pinocchio/parsers/urdf.hpp>
+
+namespace dynamicgraph {
+namespace sot {
+namespace talos_balance {
+
+/* --------------------------------------------------------------------- */
+/* --- CLASS ----------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+/** Compute s12 as an intermediate transform between s1 and s2 SE3 transforms**/
+void se3Interp(const pinocchio::SE3& s1, const pinocchio::SE3& s2,
+               const double alpha, pinocchio::SE3& s12);
+
+/** Convert from Roll, Pitch, Yaw to transformation Matrix. */
+void rpyToMatrix(double r, double p, double y, Eigen::Matrix3d& R);
+
+/** Convert from Roll, Pitch, Yaw to transformation Matrix. */
+void rpyToMatrix(const Eigen::Vector3d& rpy, Eigen::Matrix3d& R);
+
+/**  Convert from Transformation Matrix to Roll, Pitch, Yaw */
+void matrixToRpy(const Eigen::Matrix3d& M, Eigen::Vector3d& rpy);
+
+/** Multiply to quaternions stored in (w,x,y,z) format */
+// void quanternionMult(const Eigen::Vector4d & q1, const Eigen::Vector4d & q2,
+// Eigen::Vector4d & q12);
+
+/** Rotate a point or a vector by a quaternion stored in (w,x,y,z) format */
+// void pointRotationByQuaternion(const Eigen::Vector3d & point,const
+// Eigen::Vector4d & quat, Eigen::Vector3d & rotatedPoint);
+
+/** Avoids singularity while taking the mean of euler angles**/
+double eulerMean(double a1, double a2);
+
+/** Avoids singularity while taking the mean of euler angles**/
+double wEulerMean(double a1, double a2, double w1, double w2);
+
+class TALOS_BASE_ESTIMATOR_EXPORT TalosBaseEstimator
+    : public ::dynamicgraph::Entity {
+  typedef TalosBaseEstimator EntityClassName;
+  typedef pinocchio::SE3 SE3;
+  typedef Eigen::Vector2d Vector2;
+  typedef Eigen::Vector3d Vector3;
+  typedef Eigen::Vector4d Vector4;
+  typedef Vector6d Vector6;
+  typedef Vector7d Vector7;
+  typedef Eigen::Matrix3d Matrix3;
+  typedef boost::math::normal normal;
+
+  DYNAMIC_GRAPH_ENTITY_DECL();
+
+ public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  /* --- CONSTRUCTOR ---- */
+  TalosBaseEstimator(const std::string& name);
+
+  void init(const double& dt, const std::string& urdfFile);
+
+  void set_fz_stable_windows_size(const int& ws);
+  void set_alpha_w_filter(const double& a);
+  void set_alpha_DC_acc(const double& a);
+  void set_alpha_DC_vel(const double& a);
+  void reset_foot_positions();
+  void set_imu_weight(const double& w);
+  void set_zmp_std_dev_right_foot(const double& std_dev);
+  void set_zmp_std_dev_left_foot(const double& std_dev);
+  void set_normal_force_std_dev_right_foot(const double& std_dev);
+  void set_normal_force_std_dev_left_foot(const double& std_dev);
+  void set_stiffness_right_foot(const dynamicgraph::Vector& k);
+  void set_stiffness_left_foot(const dynamicgraph::Vector& k);
+  void set_right_foot_sizes(const dynamicgraph::Vector& s);
+  void set_left_foot_sizes(const dynamicgraph::Vector& s);
+  void set_zmp_margin_right_foot(const double& margin);
+  void set_zmp_margin_left_foot(const double& margin);
+  void set_normal_force_margin_right_foot(const double& margin);
+  void set_normal_force_margin_left_foot(const double& margin);
+
+  void reset_foot_positions_impl(const Vector6& ftlf, const Vector6& ftrf);
+  void compute_zmp(const Vector6& w, Vector2& zmp);
+  double compute_zmp_weight(const Vector2& zmp, const Vector4& foot_sizes,
+                            double std_dev, double margin);
+  double compute_force_weight(double fz, double std_dev, double margin);
+  void kinematics_estimation(const Vector6& ft, const Vector6& K,
+                             const SE3& oMfs,
+                             const pinocchio::FrameIndex foot_id, SE3& oMff,
+                             SE3& oMfa, SE3& fsMff);
+
+  /* --- SIGNALS --- */
+  DECLARE_SIGNAL_IN(joint_positions, dynamicgraph::Vector);
+  DECLARE_SIGNAL_IN(joint_velocities, dynamicgraph::Vector);
+  DECLARE_SIGNAL_IN(imu_quaternion, dynamicgraph::Vector);
+  DECLARE_SIGNAL_IN(forceLLEG, dynamicgraph::Vector);
+  DECLARE_SIGNAL_IN(forceRLEG, dynamicgraph::Vector);
+  DECLARE_SIGNAL_IN(
+      dforceLLEG,
+      dynamicgraph::Vector);  /// derivative of left force torque sensor
+  DECLARE_SIGNAL_IN(
+      dforceRLEG,
+      dynamicgraph::Vector);  /// derivative of right force torque sensor
+  DECLARE_SIGNAL_IN(
+      w_lf_in, double);  /// weight of the estimation coming from the left foot
+  DECLARE_SIGNAL_IN(
+      w_rf_in, double);  /// weight of the estimation coming from the right foot
+  DECLARE_SIGNAL_IN(
+      K_fb_feet_poses,
+      double);  /// feed back gain to correct feet position according to last
+                /// base estimation and kinematic
+  DECLARE_SIGNAL_IN(lf_ref_xyzquat, dynamicgraph::Vector);
+  DECLARE_SIGNAL_IN(rf_ref_xyzquat, dynamicgraph::Vector);
+  DECLARE_SIGNAL_IN(accelerometer, dynamicgraph::Vector);
+  DECLARE_SIGNAL_IN(gyroscope, dynamicgraph::Vector);
+
+  DECLARE_SIGNAL_INNER(kinematics_computations, dynamicgraph::Vector);
+
+  DECLARE_SIGNAL_OUT(
+      q, dynamicgraph::Vector);  /// n+6 robot configuration with base6d in RPY
+  DECLARE_SIGNAL_OUT(v, dynamicgraph::Vector);  /// n+6 robot velocities
+  DECLARE_SIGNAL_OUT(
+      v_kin, dynamicgraph::Vector);  /// 6d robot velocities from kinematic only
+                                     /// (encoders derivative)
+  DECLARE_SIGNAL_OUT(
+      v_flex,
+      dynamicgraph::Vector);  /// 6d robot velocities from flexibility only
+                              /// (force sensor derivative)
+  DECLARE_SIGNAL_OUT(
+      v_imu,
+      dynamicgraph::Vector);  /// 6d robot velocities form imu only
+                              /// (accelerometer integration + gyro)
+  DECLARE_SIGNAL_OUT(
+      v_gyr, dynamicgraph::Vector);  /// 6d robot velocities form gyroscope only
+                                     /// (as if gyro measured the pure angular
+                                     /// ankle velocities)
+  DECLARE_SIGNAL_OUT(lf_xyzquat, dynamicgraph::Vector);  /// left foot pose
+  DECLARE_SIGNAL_OUT(rf_xyzquat, dynamicgraph::Vector);  /// right foot pose
+  DECLARE_SIGNAL_OUT(a_ac,
+                     dynamicgraph::Vector);  /// acceleration of the base in the
+                                             /// world with DC component removed
+  DECLARE_SIGNAL_OUT(v_ac,
+                     dynamicgraph::Vector);  /// velocity of the base in the
+                                             /// world with DC component removed
+
+  DECLARE_SIGNAL_OUT(
+      q_lf,
+      dynamicgraph::Vector);  /// n+6 robot configuration with base6d in RPY
+  DECLARE_SIGNAL_OUT(
+      q_rf,
+      dynamicgraph::Vector);  /// n+6 robot configuration with base6d in RPY
+  DECLARE_SIGNAL_OUT(
+      q_imu,
+      dynamicgraph::Vector);  /// n+6 robot configuration with base6d in RPY
+  DECLARE_SIGNAL_OUT(
+      w_lf, double);  /// weight of the estimation coming from the left foot
+  DECLARE_SIGNAL_OUT(
+      w_rf, double);  /// weight of the estimation coming from the right foot
+  DECLARE_SIGNAL_OUT(
+      w_lf_filtered,
+      double);  /// filtered weight of the estimation coming from the left foot
+  DECLARE_SIGNAL_OUT(
+      w_rf_filtered,
+      double);  /// filtered weight of the estimation coming from the right foot
+
+  /* --- COMMANDS --- */
+  /* --- ENTITY INHERITANCE --- */
+  virtual void display(std::ostream& os) const;
+
+ protected:
+  bool
+      m_initSucceeded;  /// true if the entity has been successfully initialized
+  bool
+      m_reset_foot_pos;  /// true after the command resetFootPositions is called
+  double m_dt;           /// sampling time step
+  RobotUtilShrPtr m_robot_util;
+
+  bool m_left_foot_is_stable;  /// True if left foot as been stable for the last
+                               /// 'm_fz_stable_windows_size' samples
+  bool m_right_foot_is_stable;   /// True if right foot as been stable for the
+                                 /// last 'm_fz_stable_windows_size' samples
+  int m_fz_stable_windows_size;  /// size of the windows used to detect that
+                                 /// feet did not leave the ground
+  int m_lf_fz_stable_cpt;  /// counter for detecting for how long the feet has
+                           /// been stable
+  int m_rf_fz_stable_cpt;  /// counter for detecting for how long the feet has
+                           /// been stable
+  /* Estimator parameters */
+  double m_w_imu;           /// weight of IMU for sensor fusion
+  double m_zmp_std_dev_rf;  /// standard deviation of ZMP measurement errors for
+                            /// right foot
+  double m_zmp_std_dev_lf;  /// standard deviation of ZMP measurement errors for
+                            /// left foot
+  double m_fz_std_dev_rf;   /// standard deviation of normal force measurement
+                            /// errors for right foot
+  double m_fz_std_dev_lf;   /// standard deviation of normal force measurement
+                            /// errors for left foot
+  Vector4 m_left_foot_sizes;   /// sizes of the left foot (pos x, neg x, pos y,
+                               /// neg y)
+  Vector4 m_right_foot_sizes;  /// sizes of the left foot (pos x, neg x, pos y,
+                               /// neg y)
+  double m_zmp_margin_lf;      /// margin used for computing zmp weight
+  double m_zmp_margin_rf;      /// margin used for computing zmp weight
+  double m_fz_margin_lf;       /// margin used for computing normal force weight
+  double m_fz_margin_rf;       /// margin used for computing normal force weight
+  Vector6 m_K_rf;              /// 6d stiffness of right foot spring
+  Vector6 m_K_lf;              /// 6d stiffness of left foot spring
+
+  Eigen::VectorXd m_v_kin;   /// 6d robot velocities from kinematic only
+                             /// (encoders derivative)
+  Eigen::VectorXd m_v_flex;  /// 6d robot velocities from flexibility only
+                             /// (force sensor derivative)
+  Eigen::VectorXd m_v_imu;  /// 6d robot velocities form imu only (accelerometer
+                            /// integration + gyro)
+  Eigen::VectorXd m_v_gyr;  /// 6d robot velocities form gyroscope only (as if
+                            /// gyro measured the pure angular ankle velocities)
+
+  Vector3
+      m_v_ac;  /// velocity of the base in the world with DC component removed
+  Vector3 m_a_ac;  /// acceleration of the base in the world with DC component
+                   /// removed
+
+  double m_alpha_DC_acc;  /// alpha parameter for DC blocker filter on
+                          /// acceleration data
+  double m_alpha_DC_vel;  /// alpha parameter for DC blocker filter on velocity
+                          /// data
+
+  double m_alpha_w_filter;  /// filter parameter to filter weights (1st order
+                            /// low pass filter)
+
+  double m_w_lf_filtered;  /// filtered weight of the estimation coming from the
+                           /// left foot
+  double m_w_rf_filtered;  /// filtered weight of the estimation coming from the
+                           /// right foot
+
+  pinocchio::Model m_model;    /// Pinocchio robot model
+  pinocchio::Data* m_data;     /// Pinocchio robot data
+  pinocchio::SE3 m_torsoMimu;  /// chest to imu transformation
+  pinocchio::SE3
+      m_oMff_lf;  /// world-to-base transformation obtained through left foot
+  pinocchio::SE3
+      m_oMff_rf;  /// world-to-base transformation obtained through right foot
+  SE3 m_oMlfs;    /// transformation from world to left foot sole
+  SE3 m_oMrfs;    /// transformation from world to right foot sole
+  Vector7 m_oMlfs_xyzquat;
+  Vector7 m_oMrfs_xyzquat;
+  SE3 m_oMlfs_default_ref;  /// Default reference for left foot pose to use if
+                            /// no ref is pluged
+  SE3 m_oMrfs_default_ref;  /// Default reference for right foot pose to use if
+                            /// no ref is pluged
+  normal m_normal;          /// Normal distribution
+
+  bool m_isFirstIterFlag;  /// flag to detect first iteration and initialise
+                           /// velocity filters
+
+  SE3 m_sole_M_ftSens;  /// foot sole to F/T sensor transformation
+
+  pinocchio::FrameIndex m_right_foot_id;
+  pinocchio::FrameIndex m_left_foot_id;
+  pinocchio::FrameIndex m_torso_id;
+  pinocchio::FrameIndex m_IMU_frame_id;
+  Eigen::VectorXd
+      m_q_pin;  /// robot configuration according to pinocchio convention
+  Eigen::VectorXd m_q_sot;  /// robot configuration according to SoT convention
+  Eigen::VectorXd
+      m_v_pin;  /// robot velocities according to pinocchio convention
+  Eigen::VectorXd m_v_sot;  /// robot velocities according to SoT convention
+  Matrix3 m_oRtorso;  /// chest orientation in the world from angular fusion
+  Matrix3 m_oRff;     /// base orientation in the world
+
+  /* Filter buffers*/
+  Vector3 m_last_vel;
+  Vector3 m_last_DCvel;
+  Vector3 m_last_DCacc;
+
+};  // class TalosBaseEstimator
+
+}  // namespace talos_balance
+}  // namespace sot
+}  // namespace dynamicgraph
+
+#endif  // #ifndef __sot_torque_control_free_flyer_locator_H__

include/sot/talos_balance/talos-control-manager.hh

+/*
+ * Copyright 2015, 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/>.
+ */
+
+#ifndef __sot_torque_control_control_manager_H__
+#define __sot_torque_control_control_manager_H__
+
+/* --------------------------------------------------------------------- */
+/* --- API ------------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+#if defined(WIN32)
+#if defined(__sot_torque_control_control_manager_H__)
+#define TALOS_CONTROL_MANAGER_EXPORT __declspec(dllexport)
+#else
+#define TALOS_CONTROL_MANAGER_EXPORT __declspec(dllimport)
+#endif
+#else
+#define TALOS_CONTROL_MANAGER_EXPORT
+#endif
+
+/* --------------------------------------------------------------------- */
+/* --- INCLUDE --------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+#include <pinocchio/fwd.hpp>
+
+// include pinocchio first
+
+#include <dynamic-graph/signal-helper.h>
+
+#include <map>
+#include <sot/core/matrix-geometry.hh>
+#include <sot/core/robot-utils.hh>
+
+#include "boost/assign.hpp"
+
+namespace dynamicgraph {
+namespace sot {
+namespace dg = dynamicgraph;
+namespace talos_balance {
+
+/* --------------------------------------------------------------------- */
+/* --- CLASS ----------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+/// Number of time step to transition from one ctrl mode to another
+#define CTRL_MODE_TRANSITION_TIME_STEP 1000.0
+
+class CtrlMode {
+ public:
+  int id;
+  std::string name;
+
+  CtrlMode() : id(-1), name("None") {}
+  CtrlMode(int id, const std::string& name) : id(id), name(name) {}
+};
+
+std::ostream& operator<<(std::ostream& os, const CtrlMode& s) {
+  os << s.id << "_" << s.name;
+  return os;
+}
+
+class TALOS_CONTROL_MANAGER_EXPORT TalosControlManager
+    : public ::dynamicgraph::Entity {
+  typedef Eigen::VectorXd::Index Index;
+  typedef TalosControlManager EntityClassName;
+  DYNAMIC_GRAPH_ENTITY_DECL();
+
+ public:
+  /* --- CONSTRUCTOR ---- */
+  TalosControlManager(const std::string& name);
+
+  /// Initialize
+  /// @param dt: control interval
+  /// @param urdfFile: path to the URDF model of the robot
+  void init(const double& dt, const std::string& robotRef);
+
+  /* --- SIGNALS --- */
+  std::vector<dynamicgraph::SignalPtr<dynamicgraph::Vector, int>*>
+      m_ctrlInputsSIN;
+  std::vector<dynamicgraph::SignalPtr<bool, int>*>
+      m_emergencyStopVector;  /// emergency stop inputs. If one is true, control
+                              /// is set to zero forever
+  std::vector<dynamicgraph::Signal<dynamicgraph::Vector, int>*>
+      m_jointsCtrlModesSOUT;
+
+  DECLARE_SIGNAL_IN(u_max, dynamicgraph::Vector);    /// max motor control
+  DECLARE_SIGNAL_OUT(u, dynamicgraph::Vector);       /// raw motor control
+  DECLARE_SIGNAL_OUT(u_safe, dynamicgraph::Vector);  /// safe motor control
+
+  /* --- COMMANDS --- */
+
+  /// Commands related to the control mode.
+  void addCtrlMode(const std::string& name);
+  void ctrlModes();
+  void getCtrlMode(const std::string& jointName);
+  void setCtrlMode(const std::string& jointName, const std::string& ctrlMode);
+  void setCtrlMode(const int jid, const CtrlMode& cm);
+
+  void resetProfiler();
+
+  /// Commands related to joint name and joint id
+  // void setNameToId(const std::string& jointName, const double & jointId);
+  // void setJointLimitsFromId(const double &jointId, const double &lq, const
+  // double &uq);
+
+  /// Set the mapping between urdf and sot.
+  // void setJoints(const dynamicgraph::Vector &);
+
+  // void setStreamPrintPeriod(const double & s);
+
+  void setSleepTime(const double& seconds);
+  void addEmergencyStopSIN(const std::string& name);
+
+  /* --- ENTITY INHERITANCE --- */
+  virtual void display(std::ostream& os) const;
+
+ protected:
+  RobotUtilShrPtr m_robot_util;
+  size_t m_numDofs;
+  bool
+      m_initSucceeded;  /// true if the entity has been successfully initialized
+  double m_dt;          /// control loop time period
+  bool m_emergency_stop_triggered;  /// true if an emergency condition as been
+                                    /// triggered either by an other entity, or
+                                    /// by control limit violation
+  bool m_is_first_iter;  /// true at the first iteration, false otherwise
+  int m_iter;
+  double m_sleep_time;  /// time to sleep at every iteration (to slow down
+                        /// simulation)
+
+  std::vector<std::string> m_ctrlModes;  /// existing control modes
+  std::vector<CtrlMode>
+      m_jointCtrlModes_current;  /// control mode of the joints
+  std::vector<CtrlMode>
+      m_jointCtrlModes_previous;  /// previous control mode of the joints
+  std::vector<int>
+      m_jointCtrlModesCountDown;  /// counters used for the transition between
+                                  /// two ctrl modes
+
+  bool convertStringToCtrlMode(const std::string& name, CtrlMode& cm);
+  bool convertJointNameToJointId(const std::string& name, unsigned int& id);
+  // bool isJointInRange(unsigned int id, double q);
+  void updateJointCtrlModesOutputSignal();
+
+};  // class TalosControlManager
+
+}  // namespace talos_balance
+}  // namespace sot
+}  // namespace dynamicgraph
+
+#endif  // #ifndef __sot_torque_control_control_manager_H__

include/sot/talos_balance/utils/causal-filter.hh

-/*
- * Copyright 2017-, Rohan Budhirja, 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 --------------------------------------------------------- */
-/* --------------------------------------------------------------------- */
-#include <Eigen/Core>
-
-class CausalFilter
-{
-public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-  
-  /** --- CONSTRUCTOR ---- */
-  CausalFilter(const double &timestep,
-               const int& xSize,
-               const Eigen::VectorXd& filter_numerator,
-               const Eigen::VectorXd& filter_denominator);
-  
-  void get_x_dx_ddx(const Eigen::VectorXd& base_x,
-                Eigen::VectorXd& x_output_dx_ddx);
-  
-  void switch_filter(const Eigen::VectorXd& filter_numerator,
-                     const Eigen::VectorXd& filter_denominator);
-  
-private:
-  double m_dt;      /// sampling timestep of the input signal
-  int m_x_size;
-  int m_filter_order_m;
-  int m_filter_order_n;
-
-  Eigen::VectorXd m_filter_numerator;
-  Eigen::VectorXd m_filter_denominator;
-  bool first_sample;
-  int pt_numerator;
-  int pt_denominator;
-  Eigen::MatrixXd input_buffer;
-  Eigen::MatrixXd output_buffer;
-}; // class CausalFilter

include/sot/talos_balance/utils/commands-helper.hh

@@ -17,310 +17,44 @@
 #ifndef __sot_talos_balance_commands_helper_H__
 #define __sot_talos_balance_commands_helper_H__
 
+#include <boost/function.hpp>
+
 /* --- COMMON INCLUDE -------------------------------------------------- */
-#include <dynamic-graph/command.h>
-#include <dynamic-graph/command-direct-setter.h>
-#include <dynamic-graph/command-direct-getter.h>
 #include <dynamic-graph/command-bind.h>
-
-#include <boost/function.hpp>
+#include <dynamic-graph/command-direct-getter.h>
+#include <dynamic-graph/command-direct-setter.h>
+#include <dynamic-graph/command.h>
 
 /* --- HELPER ---------------------------------------------------------- */
 namespace dynamicgraph {
-  namespace sot {
-    namespace talos_balance {
-      using ::dynamicgraph::command::makeDirectGetter;
-      using ::dynamicgraph::command::docDirectGetter;
-      using ::dynamicgraph::command::makeDirectSetter;
-      using ::dynamicgraph::command::docDirectSetter;
-      using ::dynamicgraph::command::makeCommandVoid0;
-      using ::dynamicgraph::command::docCommandVoid0;
-      using ::dynamicgraph::command::makeCommandVoid1;
-      using ::dynamicgraph::command::docCommandVoid1;
-      using ::dynamicgraph::command::makeCommandVoid2;
-      using ::dynamicgraph::command::docCommandVoid2;
-      using ::dynamicgraph::command::makeCommandVerbose;
-      using ::dynamicgraph::command::docCommandVerbose;
-    } // namespace talos_balance
-  } // namespace sot
-} // namespace dynamicgraph
-
-namespace dynamicgraph {
-  namespace command {
-
-    template <class E,typename T1,typename T2,typename T3,typename T4,typename T5>
-    struct CommandVoid5
-        : public Command
-    {
-      typedef boost::function<void(const T1&,const T2&,const T3&,const T4&,const T5&)> function_t;
-      typedef boost::function<void(E*,const T1&,const T2&,const T3&,const T4&,const T5&)> memberFunction_t;
-      typedef void (E::*memberFunction_ptr_t) (const T1&,const T2&,const T3&,const T4&,const T5&);
-
-      CommandVoid5(E& entity, function_t function,
-                   const std::string& docString)
-        :Command(entity,
-                 boost::assign::list_of
-                 (ValueHelper<T1>::TypeID)
-                 (ValueHelper<T2>::TypeID)
-                 (ValueHelper<T3>::TypeID)
-                 (ValueHelper<T4>::TypeID)
-                 (ValueHelper<T5>::TypeID)
-                 , docString)
-        ,fptr(function)
-      {}
-
-    protected:
-      virtual Value doExecute()
-      {
-        assert( getParameterValues().size() == 5 );
-        T1 val1 = getParameterValues()[0].value();
-        T2 val2 = getParameterValues()[1].value();
-        T3 val3 = getParameterValues()[2].value();
-        T4 val4 = getParameterValues()[3].value();
-        T5 val5 = getParameterValues()[4].value();
-        fptr(val1,val2,val3,val4,val5);
-        return Value(); // void
-      }
-    private:
-      function_t fptr;
-    };
-
-
-    template <class E,typename T1,typename T2,typename T3,typename T4,typename T5>
-    CommandVoid5<E,T1,T2,T3,T4,T5>*
-    makeCommandVoid5(E& entity,
-                     typename CommandVoid5<E,T1,T2,T3,T4,T5>::function_t function ,
-                     const std::string& docString)
-    {
-      return new CommandVoid5<E,T1,T2,T3,T4,T5>( entity,function,docString );
-    }
-
-    template <class E,typename T1,typename T2,typename T3,typename T4,typename T5>
-    CommandVoid5<E,T1,T2,T3,T4,T5>*
-    makeCommandVoid5(E& entity,
-                     boost::function<void(E*,const T1&,const T2&,const T3&,const T4&,const T5&)> function,
-                     const std::string& docString)
-    {
-      return new CommandVoid5<E,T1,T2,T3,T4,T5>( entity,
-                                                    boost::bind(function,&entity,_1,_2,_3,_4,_5),docString );
-    }
-
-    template <class E,typename T1,typename T2,typename T3,typename T4,typename T5>
-    CommandVoid5<E,T1,T2,T3,T4,T5>*
-    makeCommandVoid5(E& entity,
-                     void (E::*function) (const T1&,const T2&,const T3&,const T4&,const T5&),
-                     const std::string& docString)
-    {
-      return new CommandVoid5<E,T1,T2,T3,T4,T5>( entity,
-                                                    boost::bind(function,&entity,_1,_2,_3,_4,_5),
-                                                    docString );
-      return NULL;
-    }
-
-    inline std::string docCommandVoid5( const std::string& doc,
-                                        const std::string& type1,
-                                        const std::string& type2,
-                                        const std::string& type3,
-                                        const std::string& type4,
-                                        const std::string& type5)
-    {
-      return (std::string("\n")+doc+"\n\n"
-              +"Input:\n - A "+type1+".\n"
-              +"Input:\n - A "+type2+".\n"
-              +"Input:\n - A "+type3+".\n"
-              +"Input:\n - A "+type4+".\n"
-              +"Input:\n - A "+type5+".\n"
-              +"Void return.\n\n" );
-    }
-
-    template <class E,typename T1,typename T2,typename T3,typename T4,typename T5,typename T6 >
-    struct CommandVoid6
-        : public Command
-    {
-      typedef boost::function<void(const T1&,const T2&,const T3&,const T4&,const T5&,const T6&)> function_t;
-      typedef boost::function<void(E*,const T1&,const T2&,const T3&,const T4&,const T5&,const T6&)> memberFunction_t;
-      typedef void (E::*memberFunction_ptr_t) (const T1&,const T2&,const T3&,const T4&,const T5&,const T6&);
-
-      CommandVoid6(E& entity, function_t function,
-                   const std::string& docString)
-        :Command(entity,
-                 boost::assign::list_of
-                 (ValueHelper<T1>::TypeID)
-                 (ValueHelper<T2>::TypeID)
-                 (ValueHelper<T3>::TypeID)
-                 (ValueHelper<T4>::TypeID)
-                 (ValueHelper<T5>::TypeID)
-                 (ValueHelper<T6>::TypeID)
-                 , docString)
-        ,fptr(function)
-      {}
-
-    protected:
-      virtual Value doExecute()
-      {
-        assert( getParameterValues().size() == 6 );
-        T1 val1 = getParameterValues()[0].value();
-        T2 val2 = getParameterValues()[1].value();
-        T3 val3 = getParameterValues()[2].value();
-        T4 val4 = getParameterValues()[3].value();
-        T5 val5 = getParameterValues()[4].value();
-        T6 val6 = getParameterValues()[5].value();
-        fptr(val1,val2,val3,val4,val5,val6);
-        return Value(); // void
-      }
-    private:
-      function_t fptr;
-    };
-
-
-    template <class E,typename T1,typename T2,typename T3,typename T4,typename T5,typename T6 >
-    CommandVoid6<E,T1,T2,T3,T4,T5,T6>*
-    makeCommandVoid6(E& entity,
-                     typename CommandVoid6<E,T1,T2,T3,T4,T5,T6>::function_t function ,
-                     const std::string& docString)
-    {
-      return new CommandVoid6<E,T1,T2,T3,T4,T5,T6>( entity,function,docString );
-    }
-
-    template <class E,typename T1,typename T2,typename T3,typename T4,typename T5,typename T6 >
-    CommandVoid6<E,T1,T2,T3,T4,T5,T6>*
-    makeCommandVoid6(E& entity,
-                     boost::function<void(E*,const T1&,const T2&,const T3&,const T4&,const T5&,const T6&)> function,
-                     const std::string& docString)
-    {
-      return new CommandVoid6<E,T1,T2,T3,T4,T5,T6>( entity,
-                                                    boost::bind(function,&entity,_1,_2,_3,_4,_5,_6),docString );
-    }
-
-    template <class E,typename T1,typename T2,typename T3,typename T4,typename T5,typename T6 >
-    CommandVoid6<E,T1,T2,T3,T4,T5,T6>*
-    makeCommandVoid6(E& entity,
-                     void (E::*function) (const T1&,const T2&,const T3&,const T4&,const T5&,const T6&),
-                     const std::string& docString)
-    {
-      return new CommandVoid6<E,T1,T2,T3,T4,T5,T6>( entity,
-                                                    boost::bind(function,&entity,_1,_2,_3,_4,_5,_6),
-                                                    docString );
-      return NULL;
-    }
-
-    inline std::string docCommandVoid6( const std::string& doc,
-                                        const std::string& type1,
-                                        const std::string& type2,
-                                        const std::string& type3,
-                                        const std::string& type4,
-                                        const std::string& type5,
-                                        const std::string& type6)
-    {
-      return (std::string("\n")+doc+"\n\n"
-              +"Input:\n - A "+type1+".\n"
-              +"Input:\n - A "+type2+".\n"
-              +"Input:\n - A "+type3+".\n"
-              +"Input:\n - A "+type4+".\n"
-              +"Input:\n - A "+type5+".\n"
-              +"Input:\n - A "+type6+".\n"
-              +"Void return.\n\n" );
-    }
-
-    template <class E,typename T1,typename T2,typename T3,typename T4,typename T5,typename T6,typename T7 >
-    struct CommandVoid7
-        : public Command
-    {
-      typedef boost::function<void(const T1&,const T2&,const T3&,const T4&,const T5&,const T6&,const T7&)> function_t;
-      typedef boost::function<void(E*,const T1&,const T2&,const T3&,const T4&,const T5&,const T6&,const T7&)> memberFunction_t;
-      typedef void (E::*memberFunction_ptr_t) (const T1&,const T2&,const T3&,const T4&,const T5&,const T6&,const T7&);
-
-      CommandVoid7(E& entity, function_t function,
-                   const std::string& docString)
-        :Command(entity,
-                 boost::assign::list_of
-                 (ValueHelper<T1>::TypeID)
-                 (ValueHelper<T2>::TypeID)
-                 (ValueHelper<T3>::TypeID)
-                 (ValueHelper<T4>::TypeID)
-                 (ValueHelper<T5>::TypeID)
-                 (ValueHelper<T6>::TypeID)
-                 (ValueHelper<T7>::TypeID)
-                 , docString)
-        ,fptr(function)
-      {}
-
-    protected:
-      virtual Value doExecute()
-      {
-        assert( getParameterValues().size() == 7 );
-        T1 val1 = getParameterValues()[0].value();
-        T2 val2 = getParameterValues()[1].value();
-        T3 val3 = getParameterValues()[2].value();
-        T4 val4 = getParameterValues()[3].value();
-        T5 val5 = getParameterValues()[4].value();
-        T6 val6 = getParameterValues()[5].value();
-        T7 val7 = getParameterValues()[6].value();
-        fptr(val1,val2,val3,val4,val5,val6,val7);
-        return Value(); // void
-      }
-    private:
-      function_t fptr;
-    };
-
-
-    template <class E,typename T1,typename T2,typename T3,typename T4,typename T5,typename T6,typename T7 >
-    CommandVoid7<E,T1,T2,T3,T4,T5,T6,T7>*
-    makeCommandVoid7(E& entity,
-                     typename CommandVoid7<E,T1,T2,T3,T4,T5,T6,T7>::function_t function ,
-                     const std::string& docString)
-    {
-      return new CommandVoid7<E,T1,T2,T3,T4,T5,T6,T7>( entity,function,docString );
-    }
-
-    template <class E,typename T1,typename T2,typename T3,typename T4,typename T5,typename T6,typename T7 >
-    CommandVoid7<E,T1,T2,T3,T4,T5,T6,T7>*
-    makeCommandVoid7(E& entity,
-                     boost::function<void(E*,const T1&,const T2&,const T3&,const T4&,const T5&,const T6&,const T7&)> function,
-                     const std::string& docString)
-    {
-      return new CommandVoid7<E,T1,T2,T3,T4,T5,T6,T7>( entity,
-                                                    boost::bind(function,&entity,_1,_2,_3,_4,_5,_6,_7),docString );
-    }
-
-    template <class E,typename T1,typename T2,typename T3,typename T4,typename T5,typename T6,typename T7 >
-    CommandVoid7<E,T1,T2,T3,T4,T5,T6,T7>*
-    makeCommandVoid7(E& entity,
-                     void (E::*function) (const T1&,const T2&,const T3&,const T4&,const T5&,const T6&,const T7&),
-                     const std::string& docString)
-    {
-      return new CommandVoid7<E,T1,T2,T3,T4,T5,T6,T7>( entity,
-                                                          boost::bind(function,&entity,_1,_2,_3,_4,_5,_6,_7),
-                                                          docString );
-      return NULL;
-    }
-
-    inline std::string docCommandVoid7( const std::string& doc,
-                                        const std::string& type1,
-                                        const std::string& type2,
-                                        const std::string& type3,
-                                        const std::string& type4,
-                                        const std::string& type5,
-                                        const std::string& type6,
-                                        const std::string& type7)
-    {
-      return (std::string("\n")+doc+"\n\n"
-              +"Input:\n - A "+type1+".\n"
-              +"Input:\n - A "+type2+".\n"
-              +"Input:\n - A "+type3+".\n"
-              +"Input:\n - A "+type4+".\n"
-              +"Input:\n - A "+type5+".\n"
-              +"Input:\n - A "+type6+".\n"
-              +"Input:\n - A "+type7+".\n"
-              +"Void return.\n\n" );
-    }
-
-
-  } // namespace command
-} // namespace dynamicgraph
-
-
-
-#endif // __sot_talos_balance_commands_helper_H__
-
-
+namespace sot {
+namespace talos_balance {
+using ::dynamicgraph::command::docCommandVerbose;
+using ::dynamicgraph::command::docCommandVoid0;
+using ::dynamicgraph::command::docCommandVoid1;
+using ::dynamicgraph::command::docCommandVoid2;
+using ::dynamicgraph::command::docCommandVoid3;
+using ::dynamicgraph::command::docCommandVoid4;
+using ::dynamicgraph::command::docCommandVoid5;
+using ::dynamicgraph::command::docCommandVoid6;
+using ::dynamicgraph::command::docCommandVoid7;
+using ::dynamicgraph::command::docCommandVoid8;
+using ::dynamicgraph::command::docDirectGetter;
+using ::dynamicgraph::command::docDirectSetter;
+using ::dynamicgraph::command::makeCommandVerbose;
+using ::dynamicgraph::command::makeCommandVoid0;
+using ::dynamicgraph::command::makeCommandVoid1;
+using ::dynamicgraph::command::makeCommandVoid2;
+using ::dynamicgraph::command::makeCommandVoid3;
+using ::dynamicgraph::command::makeCommandVoid4;
+using ::dynamicgraph::command::makeCommandVoid5;
+using ::dynamicgraph::command::makeCommandVoid6;
+using ::dynamicgraph::command::makeCommandVoid7;
+using ::dynamicgraph::command::makeCommandVoid8;
+using ::dynamicgraph::command::makeDirectGetter;
+using ::dynamicgraph::command::makeDirectSetter;
+}  // namespace talos_balance
+}  // namespace sot
+}  // namespace dynamicgraph
+
+#endif  // __sot_talos_balance_commands_helper_H__

include/sot/talos_balance/utils/logger.hh

-/*
- * Copyright 2018, Gepetto team, LAAS-CNRS
- *
- * This file is part of sot-talos-balance.
- * sot-talos-balance 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-talos-balance 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-talos-balance.  If not, see <http://www.gnu.org/licenses/>.
- */
-
-#ifndef __sot_talos_balance_logger_H__
-#define __sot_talos_balance_logger_H__
-
-/* --------------------------------------------------------------------- */
-/* --- API ------------------------------------------------------------- */
-/* --------------------------------------------------------------------- */
-
-#if defined (WIN32)
-#  if defined (logger_EXPORTS)
-#    define LOGGER_EXPORT __declspec(dllexport)
-#  else
-#    define LOGGER_EXPORT __declspec(dllimport)
-#  endif
-#else
-#  define LOGGER_EXPORT
-#endif
-
-
-/* --------------------------------------------------------------------- */
-/* --- INCLUDE --------------------------------------------------------- */
-/* --------------------------------------------------------------------- */
-
-#include "sot/talos_balance/utils/signal-helper.hh"
-#include "sot/talos_balance/utils/vector-conversions.hh"
-#include <map>
-#include <iomanip> // std::setprecision
-#include "boost/assign.hpp"
-
-
-namespace dynamicgraph {
-  namespace sot {
-    namespace talos_balance {
-
-#define SEND_MSG(msg,type)         sendMsg(msg,type,__FILE__,__LINE__)
-
-#ifdef LOGGER_VERBOSITY_ALL
-  #define SEND_DEBUG_STREAM_MSG(msg) SEND_MSG(msg,MSG_TYPE_DEBUG_STREAM)
-  #define SEND_INFO_STREAM_MSG(msg)   SEND_MSG(msg,MSG_TYPE_INFO_STREAM)
-  #define SEND_WARNING_STREAM_MSG(msg)  SEND_MSG(msg,MSG_TYPE_WARNING_STREAM)
-  #define SEND_ERROR_STREAM_MSG(msg)    SEND_MSG(msg,MSG_TYPE_ERROR_STREAM)
-#elif defined(LOGGER_VERBOSITY_INFO_WARNING_ERROR)
-  #define SEND_DEBUG_STREAM_MSG(msg)
-  #define SEND_INFO_STREAM_MSG(msg)     SEND_MSG(msg,MSG_TYPE_INFO_STREAM)
-  #define SEND_WARNING_STREAM_MSG(msg)  SEND_MSG(msg,MSG_TYPE_WARNING_STREAM)
-  #define SEND_ERROR_STREAM_MSG(msg)    SEND_MSG(msg,MSG_TYPE_ERROR_STREAM)
-#elif defined(LOGGER_VERBOSITY_WARNING_ERROR)
-  #define SEND_DEBUG_STREAM_MSG(msg)
-  #define SEND_INFO_STREAM_MSG(msg)
-  #define SEND_WARNING_STREAM_MSG(msg)  SEND_MSG(msg,MSG_TYPE_WARNING_STREAM)
-  #define SEND_ERROR_STREAM_MSG(msg)    SEND_MSG(msg,MSG_TYPE_ERROR_STREAM)
-#elif defined(LOGGER_VERBOSITY_ERROR)
-  #define SEND_DEBUG_STREAM_MSG(msg)
-  #define SEND_INFO_STREAM_MSG(msg)
-  #define SEND_WARNING_STREAM_MSG(msg)
-  #define SEND_ERROR_STREAM_MSG(msg)    SEND_MSG(msg,MSG_TYPE_ERROR_STREAM)
-#else
-  #define SEND_DEBUG_STREAM_MSG(msg)
-  #define SEND_INFO_STREAM_MSG(msg)
-  #define SEND_WARNING_STREAM_MSG(msg)
-  #define SEND_ERROR_STREAM_MSG(msg)
-#endif
-
-      /** Enum representing the different kind of messages.
-       */
-      enum MsgType
-      {
-        MSG_TYPE_DEBUG          =0,
-        MSG_TYPE_INFO           =1,
-        MSG_TYPE_WARNING        =2,
-        MSG_TYPE_ERROR          =3,
-        MSG_TYPE_DEBUG_STREAM   =4,
-        MSG_TYPE_INFO_STREAM    =5,
-        MSG_TYPE_WARNING_STREAM =6,
-        MSG_TYPE_ERROR_STREAM   =7
-      };
-
-      template<typename T>
-      std::string toString(const T& v, const int precision=3, const int width=-1)
-      {
-        std::stringstream ss;
-        if(width>precision)
-          ss<<std::fixed<<std::setw(width)<<std::setprecision(precision)<<v;
-        else
-          ss<<std::fixed<<std::setprecision(precision)<<v;
-        return ss.str();
-      }
-
-      template<typename T>
-      std::string toString(const std::vector<T>& v, const int precision=3, const int width=-1,
-                           const std::string separator=", ")
-      {
-        std::stringstream ss;
-        if(width>precision)
-        {
-          for(int i=0; i<v.size()-1; i++)
-            ss<<std::fixed<<std::setw(width)<<std::setprecision(precision)<<v[i]<<separator;
-          ss<<std::fixed<<std::setw(width)<<std::setprecision(precision)<<v[v.size()-1];
-        }
-        else
-        {
-          for(int i=0; i<v.size()-1; i++)
-            ss<<std::fixed<<std::setprecision(precision)<<v[i]<<separator;
-          ss<<std::fixed<<std::setprecision(precision)<<v[v.size()-1];
-        }
-
-        return ss.str();
-      }
-
-//      template<typename T, int N>
-//      std::string toString(const Eigen::Matrix<T, N, 1, 0, N, 1>& v, const std::string separator=", ",
-//                           const int precision=3, const int width=-1)
-      template<typename T>
-      std::string toString(const Eigen::MatrixBase<T>& v,
-                           const int precision=3, const int width=-1, const std::string separator=", ")
-      {
-        std::stringstream ss;
-        if(width>precision)
-        {
-          for(int i=0; i<v.size()-1; i++)
-            ss<<std::fixed<<std::setw(width)<<std::setprecision(precision)<<v[i]<<separator;
-          ss<<std::fixed<<std::setw(width)<<std::setprecision(precision)<<v[v.size()-1];
-        }
-        else
-        {
-          for(int i=0; i<v.size()-1; i++)
-            ss<<std::fixed<<std::setprecision(precision)<<v[i]<<separator;
-          ss<<std::setprecision(precision)<<v[v.size()-1];
-        }
-
-        return ss.str();
-      }
-
-      enum LoggerVerbosity
-      {
-        VERBOSITY_ALL,
-        VERBOSITY_INFO_WARNING_ERROR,
-        VERBOSITY_WARNING_ERROR,
-        VERBOSITY_ERROR,
-        VERBOSITY_NONE
-      };
-
-      /** A simple class for logging messages
-      */
-      class Logger
-      {
-      public:
-
-        /** Constructor */
-        Logger(double timeSample=0.001, double streamPrintPeriod=1.0);
-
-        /** Destructor */
-        ~Logger(){}
-
-        /** Method to be called at every control iteration
-           * to decrement the internal Logger's counter. */
-        void countdown();
-
-        /** Print the specified message on standard output if the verbosity level
-         * allows it. The file name and the line number are used to identify
-         * the point where sendMsg is called so that streaming messages are
-         * printed only every streamPrintPeriod iterations.
-         */
-        void sendMsg(std::string msg, MsgType type, const char* file="", int line=0);
-
-        /** Set the sampling time at which the method countdown()
-           * is going to be called. */
-        bool setTimeSample(double t);
-
-        /** Set the time period for printing of streaming messages. */
-        bool setStreamPrintPeriod(double s);
-
-        /** Set the verbosity level of the logger. */
-        void setVerbosity(LoggerVerbosity lv);
-
-      protected:
-        LoggerVerbosity m_lv;                /// verbosity of the logger
-        double          m_timeSample;        /// specify the period of call of the countdown method
-        double          m_streamPrintPeriod; /// specify the time period of the stream prints
-        double          m_printCountdown;    /// every time this is < 0 (i.e. every _streamPrintPeriod sec) print stuff
-
-        /** Pointer to the dynamic structure which holds the collection of streaming messages */
-        std::map<std::string, double> m_stream_msg_counters;
-
-        bool isStreamMsg(MsgType m)
-        { return m==MSG_TYPE_ERROR_STREAM || m==MSG_TYPE_DEBUG_STREAM || m==MSG_TYPE_INFO_STREAM || m==MSG_TYPE_WARNING_STREAM; }
-
-        bool isDebugMsg(MsgType m)
-        { return m==MSG_TYPE_DEBUG_STREAM || m==MSG_TYPE_DEBUG; }
-
-        bool isInfoMsg(MsgType m)
-        { return m==MSG_TYPE_INFO_STREAM || m==MSG_TYPE_INFO; }
-
-        bool isWarningMsg(MsgType m)
-        { return m==MSG_TYPE_WARNING_STREAM || m==MSG_TYPE_WARNING; }
-
-        bool isErrorMsg(MsgType m)
-        { return m==MSG_TYPE_ERROR_STREAM || m==MSG_TYPE_ERROR; }
-      };
-
-      /** Method to get the logger (singleton). */
-      Logger& getLogger();
-
-    }    // namespace talos_balance
-  }      // namespace sot
-}        // namespace dynamicgraph
-
-
-
-#endif // #ifndef __sot_talos_balance_logger_H__

include/sot/talos_balance/utils/signal-helper.hh

-/*
- * Copyright 2018, Gepetto team, LAAS-CNRS
- *
- * This file is part of sot-talos-balance.
- * sot-talos-balance 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-talos-balance 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-talos-balance.  If not, see <http://www.gnu.org/licenses/>.
- */
-
-#ifndef __sot_talos_balance_signal_helper_H__
-#define __sot_talos_balance_signal_helper_H__
-
-
-/* --- COMMON INCLUDE -------------------------------------------------- */
-
-/* dg signals */
-#include <dynamic-graph/entity.h>
-#include <dynamic-graph/linear-algebra.h>
-#include <dynamic-graph/signal-ptr.h>
-#include <dynamic-graph/signal-time-dependent.h>
-/* Matrix */
-
-
-/* --- MACROS ---------------------------------------------------------- */
-//Use ## to combine macro parameters into a single token (token => variable name, etc)
-//And # to stringify a macro parameter (very useful when doing "reflection" in C/C++)
-
-#define SIGNAL_OUT_FUNCTION_NAME(name) name##SOUT_function
-
-#define DECLARE_SIGNAL(name,IO,type)::dynamicgraph::Signal<type,int> m_##name##S##IO
-#define CONSTRUCT_SIGNAL(name,IO,type) m_##name##S##IO( getClassName()+"("+getName()+")::"+#IO+"put("+#type+")::"+#name )
-#define BIND_SIGNAL_TO_FUNCTION(name,IO,type) m_##name##S##IO.setFunction(boost::bind(&EntityClassName::SIGNAL_OUT_FUNCTION_NAME(name),this,_1,_2));
-
-
-/**************** INPUT SIGNALS *******************/
-#define DECLARE_SIGNAL_IN(name,type)\
-  ::dynamicgraph::SignalPtr<type,int> m_##name##SIN
-#define CONSTRUCT_SIGNAL_IN(name,type)\
-  m_##name##SIN( NULL,getClassName()+"("+getName()+")::input("+#type+")::"+#name )
-
-
-/**************** OUTPUT SIGNALS *******************/
-#define DECLARE_SIGNAL_OUT_FUNCTION(name,type) \
-  type& SIGNAL_OUT_FUNCTION_NAME(name)(type&,int)
-
-#define DEFINE_SIGNAL_OUT_FUNCTION(name,type) \
-  type& EntityClassName::SIGNAL_OUT_FUNCTION_NAME(name)(type& s,int iter)
-
-#define DECLARE_SIGNAL_OUT(name,type)                           \
-  public:                                                       \
-    ::dynamicgraph::SignalTimeDependent<type,int> m_##name##SOUT;	\
-  protected:                                                    \
-    DECLARE_SIGNAL_OUT_FUNCTION(name,type)
-
-#define CONSTRUCT_SIGNAL_OUT( name,type,dep )		        \
-  m_##name##SOUT(boost::bind(&EntityClassName::SIGNAL_OUT_FUNCTION_NAME(name),this,_1,_2), \
-                dep,getClassName()+"("+getName()+")::output("+#type+")::"+#name )
-
-
-/**************** INNER SIGNALS *******************/
-#define SIGNAL_INNER_FUNCTION_NAME(name) name##SINNER_function
-
-#define DECLARE_SIGNAL_INNER_FUNCTION(name,type) \
-  type& SIGNAL_INNER_FUNCTION_NAME(name)(type&,int)
-
-#define DEFINE_SIGNAL_INNER_FUNCTION(name,type) \
-  type& EntityClassName::SIGNAL_INNER_FUNCTION_NAME(name)(type& s,int iter)
-
-#define DECLARE_SIGNAL_INNER(name,type)                           \
-  public:                                                       \
-    ::dynamicgraph::SignalTimeDependent<type,int> m_##name##SINNER;\
-  protected:                                                    \
-    DECLARE_SIGNAL_INNER_FUNCTION(name,type)
-
-#define CONSTRUCT_SIGNAL_INNER( name,type,dep )		        \
-  m_##name##SINNER( boost::bind(&EntityClassName::name##SINNER_function,this,_1,_2), \
-              dep,getClassName()+"("+getName()+")::inner("+#type+")::"+#name )
-
-
-#endif // __sot_talos_balance_signal_helper_H__

include/sot/talos_balance/utils/statistics.hh

@@ -15,7 +15,6 @@
 // <http://www.gnu.org/licenses/>.
 //
 
-
 #ifndef __invdyn_statistics_H__
 #define __invdyn_statistics_H__
 
@@ -26,14 +25,12 @@
 #define STATISTICS_MAX_NAME_LENGTH 60
 
 // Generic statistics exception class
-struct StatisticsException
-{
-public:
-  StatisticsException(std::string error) : error(error) { }
+struct StatisticsException {
+ public:
+  StatisticsException(std::string error) : error(error) {}
   std::string error;
 };
 
-
 /**
     @brief A class to compute statistics about quantities of interest.
 
@@ -67,8 +64,7 @@ public:
 
 */
 class Statistics {
-public:
-
+ public:
   /** Constructor */
   Statistics();
 
@@ -79,7 +75,7 @@ public:
   bool quantity_exists(std::string name);
 
   /** Record the value of the specified quantity */
-  void store(std::string name, const double & value);
+  void store(std::string name, const double& value);
 
   /** Reset a certain quantity record */
   void reset(std::string name);
@@ -88,11 +84,11 @@ public:
   void reset_all();
 
   /** Dump the data of a certain quantity record */
-  void report(std::string name, int precision=2,
+  void report(std::string name, int precision = 2,
               std::ostream& output = std::cout);
 
   /** Dump the data of all the quantity records */
-  void report_all(int precision=2, std::ostream& output = std::cout);
+  void report_all(int precision = 2, std::ostream& output = std::cout);
 
   /** Returns total execution of a certain quantity */
   long double get_total(std::string name);
@@ -109,40 +105,32 @@ public:
   /** Return last measurement of a certain quantity */
   long double get_last(std::string name);
 
-  /**	Turn off statistics, all the Statistics::* methods return without doing
-        anything after this method is called. */
+  /** Turn off statistics, all the Statistics::* methods return without doing
+      anything after this method is called. */
   void turn_off();
 
   /** Turn on statistics, restore operativity after a turn_off(). */
   void turn_on();
 
-protected:
-
+ protected:
   /** Struct to hold the quantity data */
   struct QuantityData {
-
-    QuantityData() :
-      total(0),
-      min(0),
-      max(0),
-      last(0),
-      stops(0) {
-    }
+    QuantityData() : total(0), min(0), max(0), last(0), stops(0) {}
 
     /** Cumulative total value */
-    long double	total;
+    long double total;
 
     /** Minimum value */
-    long double	min;
+    long double min;
 
     /** Maximum value */
-    long double	max;
+    long double max;
 
     /** Last value */
     long double last;
 
     /** How many times have this quantity been stored? */
-    int	stops;
+    int stops;
   };
 
   /** Flag to hold the statistics status */
@@ -150,8 +138,7 @@ protected:
 
   /** Pointer to the dynamic structure which holds the collection of quantity
       data */
-  std::map<std::string, QuantityData >* records_of;
-
+  std::map<std::string, QuantityData>* records_of;
 };
 
 Statistics& getStatistics();

include/sot/talos_balance/utils/stop-watch.hh

-/*
-Copyright (c) 2010-2013 Tommaso Urli
-
-Tommaso Urli    tommaso.urli@uniud.it   University of Udine
-
-Permission is hereby granted, free of charge, to any person obtaining
-a copy of this software and associated documentation files (the
-"Software"), to deal in the Software without restriction, including
-without limitation the rights to use, copy, modify, merge, publish,
-distribute, sublicense, and/or sell copies of the Software, and to
-permit persons to whom the Software is furnished to do so, subject to
-the following conditions:
-
-The above copyright notice and this permission notice shall be
-included in all copies or substantial portions of the Software.
-
-THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
-EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
-MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
-NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
-LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
-OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
-WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
-
-*/
-
-
-#ifndef __sot_talos_balance_stopwatch_H__
-#define __sot_talos_balance_stopwatch_H__
-
-#include <iostream>
-#include <map>
-#include <ctime>
-#include <sstream>
-
-#ifndef WIN32
-/* The classes below are exported */
-#pragma GCC visibility push(default)
-#endif
-
-// Generic stopwatch exception class
-struct StopwatchException
-{
-public:
-  StopwatchException(std::string error) : error(error) { }
-  std::string error;
-};
-
-
-enum StopwatchMode
-{
-  NONE	    = 0,  // Clock is not initialized
-  CPU_TIME  = 1,  // Clock calculates time ranges using ctime and CLOCKS_PER_SEC
-  REAL_TIME = 2   // Clock calculates time by asking the operating system how
-  // much real time passed
-};
-
-#define STOP_WATCH_MAX_NAME_LENGTH 80
-
-/**
-    @brief A class representing a stopwatch.
-    
-    @code
-    Stopwatch swatch();
-    @endcode
-    
-    The Stopwatch class can be used to measure execution time of code,
-    algorithms, etc., // TODO: he Stopwatch can be initialized in two
-    time-taking modes, CPU time and real time:
-
-    @code
-    swatch.set_mode(REAL_TIME);
-    @endcode
-    
-    CPU time is the time spent by the processor on a certain piece of code,
-    while real time is the real amount of time taken by a certain piece of
-    code to execute (i.e. in general if you are doing hard work such as
-    image or video editing on a different process the measured time will
-    probably increase).
-    
-    How does it work? Basically, one wraps the code to be measured with the
-    following method calls:
-
-    @code
-    swatch.start("My astounding algorithm");
-    // Hic est code
-    swatch.stop("My astounding algorithm");
-    @endcode
-    
-    A string representing the code ID is provided so that nested portions of
-    code can be profiled separately:
-
-    @code
-    swatch.start("My astounding algorithm");
-    
-    swatch.start("My astounding algorithm - Super smart init");
-    // Initialization
-    swatch.stop("My astounding algorithm - Super smart init");
-    
-    swatch.start("My astounding algorithm - Main loop");
-    // Loop
-    swatch.stop("My astounding algorithm - Main loop");
-    
-    swatch.stop("My astounding algorithm");
-    @endcode
-    
-    Note: ID strings can be whatever you like, in the previous example I have
-    used "My astounding algorithm - *" only to enforce the fact that the
-    measured code portions are part of My astounding algorithm, but there's no
-    connection between the three measurements.
-
-    If the code for a certain task is scattered through different files or
-    portions of the same file one can use the start-pause-stop method:
-
-    @code
-    swatch.start("Setup");
-    // First part of setup
-    swatch.pause("Setup");
-    
-    swatch.start("Main logic");
-    // Main logic
-    swatch.stop("Main logic");
-    
-    swatch.start("Setup");
-    // Cleanup (part of the setup)
-    swatch.stop("Setup");
-    @endcode
-    
-    Finally, to report the results of the measurements just run:
-    
-    @code
-    swatch.report("Code ID");
-    @endcode
-    
-    Thou can also provide an additional std::ostream& parameter to report() to
-    redirect the logging on a different output. Also, you can use the
-    get_total/min/max/average_time() methods to get the individual numeric data,
-    without all the details of the logging. You can also extend Stopwatch to
-    implement your own logging syntax.
-
-    To report all the measurements:
-    
-    @code
-    swatch.report_all();
-    @endcode
-    
-    Same as above, you can redirect the output by providing a std::ostream&
-    parameter.
-
-*/
-class Stopwatch {
-public:
-
-  /** Constructor */
-  Stopwatch(StopwatchMode _mode=NONE);
-
-  /** Destructor */
-  ~Stopwatch();
-
-  /** Tells if a performance with a certain ID exists */
-  bool performance_exists(std::string perf_name);
-
-  /** Initialize stopwatch to use a certain time taking mode */
-  void set_mode(StopwatchMode mode);
-
-  /** Start the stopwatch related to a certain piece of code */
-  void start(std::string perf_name);
-
-  /** Stops the stopwatch related to a certain piece of code */
-  void stop(std::string perf_name);
-
-  /** Stops the stopwatch related to a certain piece of code */
-  void pause(std::string perf_name);
-
-  /** Reset a certain performance record */
-  void reset(std::string perf_name);
-
-  /** Resets all the performance records */
-  void reset_all();
-
-  /** Dump the data of a certain performance record */
-  void report(std::string perf_name, int precision=2,
-              std::ostream& output = std::cout);
-
-  /** Dump the data of all the performance records */
-  void report_all(int precision=2, std::ostream& output = std::cout);
-
-  /** Returns total execution time of a certain performance */
-  long double get_total_time(std::string perf_name);
-
-  /** Returns average execution time of a certain performance */
-  long double get_average_time(std::string perf_name);
-
-  /** Returns minimum execution time of a certain performance */
-  long double get_min_time(std::string perf_name);
-
-  /** Returns maximum execution time of a certain performance */
-  long double get_max_time(std::string perf_name);
-
-  /** Return last measurement of a certain performance */
-  long double get_last_time(std::string perf_name);
-
-  /** Return the time since the start of the last measurement of a given
-      performance. */
-  long double get_time_so_far(std::string perf_name);
-
-  /**	Turn off clock, all the Stopwatch::* methods return without doing
-        anything after this method is called. */
-  void turn_off();
-
-  /** Turn on clock, restore clock operativity after a turn_off(). */
-  void turn_on();
-
-  /** Take time, depends on mode */
-  long double take_time();
-
-protected:
-
-  /** Struct to hold the performance data */
-  struct PerformanceData {
-
-    PerformanceData() :
-      clock_start(0),
-      total_time(0),
-      min_time(0),
-      max_time(0),
-      last_time(0),
-      paused(false),
-      stops(0) {
-    }
-
-    /** Start time */
-    long double	clock_start;
-
-    /** Cumulative total time */
-    long double	total_time;
-
-    /** Minimum time */
-    long double	min_time;
-
-    /** Maximum time */
-    long double	max_time;
-
-    /** Last time */
-    long double last_time;
-
-    /** Tells if this performance has been paused, only for internal use */
-    bool paused;
-
-    /** How many cycles have been this stopwatch executed? */
-    int	stops;
-  };
-
-  /** Flag to hold the clock's status */
-  bool active;
-
-  /** Time taking mode */
-  StopwatchMode mode;
-
-  /** Pointer to the dynamic structure which holds the collection of performance
-      data */
-  std::map<std::string, PerformanceData >* records_of;
-
-};
-
-Stopwatch& getProfiler();
-
-#ifndef WIN32
-#pragma GCC visibility pop
-#endif
-
-#endif

include/sot/talos_balance/utils/vector-conversions.hh

-/*
- * Copyright 2018, Gepetto team, LAAS-CNRS
- *
- * This file is part of sot-talos-balance.
- * sot-talos-balance 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-talos-balance 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-talos-balance.  If not, see <http://www.gnu.org/licenses/>.
- */
-
-#ifndef __sot_talos_balance_vector_conversion_H__
-#define __sot_talos_balance_vector_conversion_H__
-
-#include <Eigen/Dense>
-#include <fstream>
-#include <sstream>
-
-namespace Eigen
-{
-  #define EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, Size, SizeSuffix)   \
-  /** \ingroup matrixtypedefs */                                    \
-  typedef Matrix<Type, Size, Size> Matrix##SizeSuffix##TypeSuffix;  \
-  /** \ingroup matrixtypedefs */                                    \
-  typedef Matrix<Type, Size, 1>    Vector##SizeSuffix##TypeSuffix;  \
-  /** \ingroup matrixtypedefs */                                    \
-  typedef Matrix<Type, 1, Size>    RowVector##SizeSuffix##TypeSuffix;
-
-  #define EIGEN_MAKE_FIXED_TYPEDEFS(Type, TypeSuffix, Size)         \
-  /** \ingroup matrixtypedefs */                                    \
-  typedef Matrix<Type, Size, Dynamic> Matrix##Size##X##TypeSuffix;  \
-  /** \ingroup matrixtypedefs */                                    \
-  typedef Matrix<Type, Dynamic, Size> Matrix##X##Size##TypeSuffix;
-
-  #define EIGEN_MAKE_TYPEDEFS_ALL_SIZES(Type, TypeSuffix) \
-  EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 1, 1) \
-  EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 5, 5) \
-  EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 6, 6) \
-  EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 7, 7) \
-  EIGEN_MAKE_FIXED_TYPEDEFS(Type, TypeSuffix, 1) \
-  EIGEN_MAKE_FIXED_TYPEDEFS(Type, TypeSuffix, 5) \
-  EIGEN_MAKE_FIXED_TYPEDEFS(Type, TypeSuffix, 6) \
-  EIGEN_MAKE_FIXED_TYPEDEFS(Type, TypeSuffix, 7)
-
-  EIGEN_MAKE_TYPEDEFS_ALL_SIZES(int,                  i)
-  EIGEN_MAKE_TYPEDEFS_ALL_SIZES(float,                f)
-  EIGEN_MAKE_TYPEDEFS_ALL_SIZES(double,               d)
-  EIGEN_MAKE_TYPEDEFS_ALL_SIZES(std::complex<float>,  cf)
-  EIGEN_MAKE_TYPEDEFS_ALL_SIZES(std::complex<double>, cd)
-
-  #undef EIGEN_MAKE_TYPEDEFS_ALL_SIZES
-  #undef EIGEN_MAKE_TYPEDEFS
-
-  typedef Matrix<double,Dynamic,Dynamic,RowMajor>   MatrixRXd;
-  typedef Map<MatrixRXd>                            SigMatrixXd;
-  typedef Map<VectorXd>                             SigVectorXd;
-  typedef const Map<const MatrixRXd>                const_SigMatrixXd;
-  typedef const Map<const VectorXd>                 const_SigVectorXd;
-
-  typedef Eigen::Ref<Eigen::VectorXd>              RefVector;
-  typedef const Eigen::Ref<const Eigen::VectorXd>& ConstRefVector;
-  typedef Eigen::Ref<Eigen::MatrixXd>              RefMatrix;
-  typedef const Eigen::Ref<const Eigen::MatrixXd>  ConstRefMatrix;
-}
-
-#define EIGEN_CONST_VECTOR_FROM_STD_VECTOR(name,signal)	                 \
-  Eigen::const_SigVectorXd name		               	                 \
-  (						               	         \
-   signal.data(),                                                        \
-   signal.size()                                                         \
-  )
-#define EIGEN_VECTOR_FROM_STD_VECTOR(name,signal)	                 \
-  Eigen::SigVectorXd name	 	               	                 \
-  (						               	         \
-   signal.data(),                                                        \
-   signal.size()				               	         \
-  )
-
-
-/********************* VECTOR COPY ******************************/
-// c arrays define only the [] operator
-// mal vectors define only the () operator
-// std vectors define only the [] operator
-// Eigen vectors define both the [] and () operators
-
-#define COPY_ARRAY_TO_ARRAY(src, dest, size) \
-  for(unsigned int i=0; i<size; i++) \
-    dest[i] = src[i]
-
-#define COPY_ARRAY_TO_VECTOR(src, dest) \
-  for(unsigned int i=0; i<dest.size(); i++) \
-    dest(i) = src[i]
-
-#define COPY_VECTOR_TO_ARRAY(src, dest) \
-  for(unsigned int i=0; i<src.size(); i++) \
-    dest[i] = src(i)
-
-#define COPY_SHIFTED_VECTOR_TO_VECTOR(src, dest, offset) \
-  for(unsigned int i=0; i< dest.size(); i++) \
-    dest(i) = src(i+offset)
-
-#define COPY_SHIFTED_VECTOR_TO_ARRAY(src, dest, offset) \
-  for(unsigned int i=0; i< dest.size(); i++) \
-    dest[i] = src(i+offset)
-
-#define COPY_VECTOR_TO_SHIFTED_VECTOR(src, dest, offset) \
-  for(unsigned int i=0; i< src.size(); i++) \
-    dest(i+offset) = src(i)
-
-#define COPY_ARRAY_TO_SHIFTED_VECTOR(src, dest, offset) \
-  for(unsigned int i=0; i< src.size(); i++) \
-    dest(i+offset) = src[i]
-
-#endif // __sot_talos_balance_vector_conversion_H__

package.xml

+<?xml version="1.0"?>
+<!--Author: Gabriele Buondonno-->
+<package>
+  <name>sot-talos-balance</name>
+  <version>2.0.5</version>
+  <description>The sot_talos_balance package</description>
+
+  <maintainer email="guilhem.saurel@laas.fr">Guilhem Saurel</maintainer>
+  <author email="gbuondon@laas.fr">Gabriele Buondonno</author>
+
+  <license>BSD</license>
+
+
+  <build_depend>dynamic_graph_bridge</build_depend>
+  <build_depend>dynamic-graph-python</build_depend>
+  <build_depend>sot_core</build_depend>
+  <build_depend>image_proc</build_depend>
+  <build_depend>talos_controller_configuration</build_depend>
+  <build_depend>talos_description</build_depend>
+  <build_depend>talos_hardware_gazebo</build_depend>
+  <build_depend>talos_bringup</build_depend>
+  <build_depend>gazebo_plugins</build_depend>
+  <build_depend>pal_gazebo_plugins</build_depend>
+  <build_depend>pal_gazebo_worlds</build_depend>
+  <build_depend>roboticsgroup_gazebo_plugins</build_depend>
+  <build_depend>talos_controller_configuration_gazebo</build_depend>
+  <build_depend>talos_gazebo</build_depend>
+
+  <run_depend>dynamic_graph_bridge</run_depend>
+  <run_depend>dynamic-graph-python</run_depend>
+  <run_depend>sot_core</run_depend>
+  <run_depend>image_proc</run_depend>
+  <run_depend>talos_controller_configuration</run_depend>
+  <run_depend>talos_description</run_depend>
+  <run_depend>talos_hardware_gazebo</run_depend>
+  <run_depend>talos_bringup</run_depend>
+  <run_depend>gazebo_plugins</run_depend>
+  <run_depend>pal_gazebo_plugins</run_depend>
+  <run_depend>pal_gazebo_worlds</run_depend>
+  <run_depend>roboticsgroup_gazebo_plugins</run_depend>
+  <run_depend>talos_controller_configuration_gazebo</run_depend>
+  <run_depend>talos_gazebo</run_depend>
+  <run_depend>talos_data</run_depend>
+
+  <export>
+  </export>
+</package>

plot/plot_all_des.xml

+<?xml version='1.0' encoding='UTF-8'?>
+<root>
+ <tabbed_widget parent="main_window" name="Main Window">
+  <plotmatrix rows="3" columns="4" tab_name="plot">
+   <plot col="0" row="0">
+    <range top="0.184817" bottom="-0.015183" left="16.170000" right="46.167000"/>
+    <limitY/>
+    <curve B="255" name="/sot/LFTrajGen/x/data.0" G="0" custom_transform="noTransform" R="0"/>
+    <transform value="noTransform"/>
+   </plot>
+   <plot col="0" row="1">
+    <range top="0.100000" bottom="-0.100000" left="16.170000" right="46.167000"/>
+    <limitY/>
+    <curve B="0" name="/sot/LFTrajGen/x/data.1" G="128" custom_transform="noTransform" R="0"/>
+    <transform value="noTransform"/>
+   </plot>
+   <plot col="0" row="2">
+    <range top="0.100000" bottom="-0.100000" left="16.170000" right="46.167000"/>
+    <limitY/>
+    <curve B="0" name="/sot/LFTrajGen/x/data.2" G="0" custom_transform="noTransform" R="255"/>
+    <transform value="noTransform"/>
+   </plot>
+   <plot col="1" row="0">
+    <range top="0.014817" bottom="-0.185183" left="16.170000" right="46.167000"/>
+    <limitY/>
+    <curve B="255" name="/sot/RFTrajGen/x/data.0" G="0" custom_transform="noTransform" R="255"/>
+    <transform value="noTransform"/>
+   </plot>
+   <plot col="1" row="1">
+    <range top="0.100000" bottom="-0.100000" left="16.170000" right="46.167000"/>
+    <limitY/>
+    <curve B="128" name="/sot/RFTrajGen/x/data.1" G="0" custom_transform="noTransform" R="0"/>
+    <transform value="noTransform"/>
+   </plot>
+   <plot col="1" row="2">
+    <range top="0.100000" bottom="-0.100000" left="16.170000" right="46.167000"/>
+    <limitY/>
+    <curve B="128" name="/sot/RFTrajGen/x/data.2" G="128" custom_transform="noTransform" R="0"/>
+    <transform value="noTransform"/>
+   </plot>
+   <plot col="2" row="0">
+    <range top="0.096836" bottom="-0.103164" left="16.170000" right="46.167000"/>
+    <limitY/>
+    <curve B="164" name="/sot/dummy_wp/comDes/data.0" G="160" custom_transform="noTransform" R="160"/>
+    <transform value="noTransform"/>
+   </plot>
+   <plot col="2" row="1">
+    <range top="0.101237" bottom="-0.098763" left="16.170000" right="46.167000"/>
+    <limitY/>
+    <curve B="0" name="/sot/dummy_wp/comDes/data.1" G="128" custom_transform="noTransform" R="128"/>
+    <transform value="noTransform"/>
+   </plot>
+   <plot col="2" row="2">
+    <range top="0.975624" bottom="0.775624" left="16.170000" right="46.167000"/>
+    <limitY/>
+    <curve B="0" name="/sot/dummy_wp/comDes/data.2" G="0" custom_transform="noTransform" R="128"/>
+    <transform value="noTransform"/>
+   </plot>
+   <plot col="3" row="0">
+    <range top="0.096836" bottom="-0.103164" left="16.170000" right="46.167000"/>
+    <limitY/>
+    <curve B="255" name="/sot/dummy_wp/zmpDes/data.0" G="0" custom_transform="noTransform" R="0"/>
+    <transform value="noTransform"/>
+   </plot>
+   <plot col="3" row="1">
+    <range top="0.101237" bottom="-0.098763" left="16.170000" right="46.167000"/>
+    <limitY/>
+    <curve B="0" name="/sot/dummy_wp/zmpDes/data.1" G="128" custom_transform="noTransform" R="0"/>
+    <transform value="noTransform"/>
+   </plot>
+   <plot col="3" row="2">
+    <range top="0.100000" bottom="-0.100000" left="16.170000" right="46.167000"/>
+    <limitY/>
+    <curve B="0" name="/sot/dummy_wp/zmpDes/data.2" G="0" custom_transform="noTransform" R="255"/>
+    <transform value="noTransform"/>
+   </plot>
+  </plotmatrix>
+  <currentPlotMatrix index="0"/>
+ </tabbed_widget>
+ <use_relative_time_offset enabled="1"/>
+ <Plugins>
+  <DataLoad_CSV>
+   <default time_axis=""/>
+  </DataLoad_CSV>
+  <DataLoad_ROS_bags>
+   <selected_topics list=""/>
+  </DataLoad_ROS_bags>
+  <DataLoad_ULog>
+   <no_params/>
+  </DataLoad_ULog>
+  <ROS_Topic_Streamer>
+   <selected_topics list="/sot/dummy_wp/comDes;/sot/dummy_wp/zmpDes;/sot/LFTrajGen/x;/sot/RFTrajGen/x;/sot/WTOrientationTrajGen/x"/>
+  </ROS_Topic_Streamer>
+  <RosoutPublisherROS/>
+  <TopicPublisherROS/>
+ </Plugins>
+ <previouslyLoadedStreamer name="ROS_Topic_Streamer"/>
+ <customMathEquations/>
+ <snippets>
+  <snippet name="1st_derivative">
+   <global>var prevX = 0
+var prevY = 0</global>
+   <equation>dx = time - prevX
+dy = value - prevY
+prevX = time
+prevY = value
+
+return dy/dx</equation>
+  </snippet>
+  <snippet name="1st_order_lowpass">
+   <global>var prevY = 0
+var alpha = 0.1</global>
+   <equation>prevY = alpha * value + (1.-alpha) * prevY
+
+return prevY</equation>
+  </snippet>
+  <snippet name="sum_A_B">
+   <global></global>
+   <equation>return $$PLOT_A$$ + $$PLOT_B$$</equation>
+  </snippet>
+  <snippet name="yaw_from_quaternion">
+   <global>// source: https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
+
+function quaternionToYaw(x, y, z, w)
+{
+  // yaw (z-axis rotation)
+  t1 = 2.0 * (w * z + x * y);
+  t2 = 1.0 - 2.0 * (y * y + z * z);
+  yaw = Math.atan2(t1, t2);
+
+  return yaw
+}</global>
+   <equation>return quaternionToYaw(x, y, z, w);</equation>
+  </snippet>
+ </snippets>
+</root>

plot/plot_all_gen.xml

+<?xml version='1.0' encoding='UTF-8'?>
+<root>
+ <tabbed_widget parent="main_window" name="Main Window">
+  <plotmatrix tab_name="plot" rows="3" columns="4">
+   <plot col="0" row="0">
+    <range bottom="-0.015183" top="0.184817" left="8.763000" right="18.757000"/>
+    <limitY/>
+    <curve R="0" B="255" G="0" custom_transform="noTransform" name="/sot/LFTrajGen/x/data.0"/>
+    <transform value="noTransform"/>
+   </plot>
+   <plot col="0" row="1">
+    <range bottom="-0.100000" top="0.100000" left="8.763000" right="18.757000"/>
+    <limitY/>
+    <curve R="0" B="0" G="128" custom_transform="noTransform" name="/sot/LFTrajGen/x/data.1"/>
+    <transform value="noTransform"/>
+   </plot>
+   <plot col="0" row="2">
+    <range bottom="-0.100000" top="0.100000" left="8.763000" right="18.757000"/>
+    <limitY/>
+    <curve R="255" B="0" G="0" custom_transform="noTransform" name="/sot/LFTrajGen/x/data.2"/>
+    <transform value="noTransform"/>
+   </plot>
+   <plot col="1" row="0">
+    <range bottom="-0.185183" top="0.014817" left="8.763000" right="18.757000"/>
+    <limitY/>
+    <curve R="255" B="255" G="0" custom_transform="noTransform" name="/sot/RFTrajGen/x/data.0"/>
+    <transform value="noTransform"/>
+   </plot>
+   <plot col="1" row="1">
+    <range bottom="-0.100000" top="0.100000" left="8.763000" right="18.757000"/>
+    <limitY/>
+    <curve R="0" B="128" G="0" custom_transform="noTransform" name="/sot/RFTrajGen/x/data.1"/>
+    <transform value="noTransform"/>
+   </plot>
+   <plot col="1" row="2">
+    <range bottom="-0.100000" top="0.100000" left="8.763000" right="18.757000"/>
+    <limitY/>
+    <curve R="0" B="128" G="128" custom_transform="noTransform" name="/sot/RFTrajGen/x/data.2"/>
+    <transform value="noTransform"/>
+   </plot>
+   <plot col="2" row="0">
+    <range bottom="-0.098769" top="0.101231" left="8.763000" right="18.757000"/>
+    <limitY/>
+    <curve R="255" B="255" G="0" custom_transform="noTransform" name="/sot/comTrajGen/x/data.0"/>
+    <transform value="noTransform"/>
+   </plot>
+   <plot col="2" row="1">
+    <range bottom="0.776681" top="0.976681" left="8.763000" right="18.757000"/>
+    <limitY/>
+    <curve R="0" B="128" G="0" custom_transform="noTransform" name="/sot/comTrajGen/x/data.1"/>
+    <transform value="noTransform"/>
+   </plot>
+   <plot col="2" row="2">
+    <range bottom="-0.100000" top="0.100000" left="8.763000" right="18.757000"/>
+    <limitY/>
+    <curve R="0" B="128" G="128" custom_transform="noTransform" name="/sot/comTrajGen/x/data.2"/>
+    <transform value="noTransform"/>
+   </plot>
+   <plot col="3" row="0">
+    <range bottom="-0.100000" top="0.100000" left="8.763000" right="18.757000"/>
+    <limitY/>
+    <curve R="160" B="164" G="160" custom_transform="noTransform" name="/sot/zmpTrajGen/x/data.0"/>
+    <transform value="noTransform"/>
+   </plot>
+   <plot col="3" row="1">
+    <range bottom="-0.100000" top="0.100000" left="8.763000" right="18.757000"/>
+    <limitY/>
+    <curve R="128" B="0" G="128" custom_transform="noTransform" name="/sot/zmpTrajGen/x/data.1"/>
+    <transform value="noTransform"/>
+   </plot>
+   <plot col="3" row="2">
+    <range bottom="-0.100000" top="0.100000" left="8.763000" right="18.757000"/>
+    <limitY/>
+    <curve R="128" B="0" G="0" custom_transform="noTransform" name="/sot/zmpTrajGen/x/data.2"/>
+    <transform value="noTransform"/>
+   </plot>
+  </plotmatrix>
+  <currentPlotMatrix index="0"/>
+ </tabbed_widget>
+ <use_relative_time_offset enabled="1"/>
+ <Plugins>
+  <DataLoad_CSV>
+   <default time_axis=""/>
+  </DataLoad_CSV>
+  <DataLoad_ROS_bags>
+   <selected_topics list=""/>
+  </DataLoad_ROS_bags>
+  <DataLoad_ULog>
+   <no_params/>
+  </DataLoad_ULog>
+  <ROS_Topic_Streamer>
+   <selected_topics list="/sot/comTrajGen/x;/sot/dummy_wp/comDes;/sot/dummy_wp/zmpDes;/sot/LFTrajGen/x;/sot/RFTrajGen/x;/sot/WTOrientationTrajGen/x;/sot/zmpTrajGen/x"/>
+  </ROS_Topic_Streamer>
+  <RosoutPublisherROS/>
+  <TopicPublisherROS/>
+ </Plugins>
+ <previouslyLoadedStreamer name="ROS_Topic_Streamer"/>
+ <customMathEquations/>
+ <snippets>
+  <snippet name="1st_derivative">
+   <global>var prevX = 0
+var prevY = 0</global>
+   <equation>dx = time - prevX
+dy = value - prevY
+prevX = time
+prevY = value
+
+return dy/dx</equation>
+  </snippet>
+  <snippet name="1st_order_lowpass">
+   <global>var prevY = 0
+var alpha = 0.1</global>
+   <equation>prevY = alpha * value + (1.-alpha) * prevY
+
+return prevY</equation>
+  </snippet>
+  <snippet name="sum_A_B">
+   <global></global>
+   <equation>return $$PLOT_A$$ + $$PLOT_B$$</equation>
+  </snippet>
+  <snippet name="yaw_from_quaternion">
+   <global>// source: https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
+
+function quaternionToYaw(x, y, z, w)
+{
+  // yaw (z-axis rotation)
+  t1 = 2.0 * (w * z + x * y);
+  t2 = 1.0 - 2.0 * (y * y + z * z);
+  yaw = Math.atan2(t1, t2);
+
+  return yaw
+}</global>
+   <equation>return quaternionToYaw(x, y, z, w);</equation>
+  </snippet>
+ </snippets>
+</root>

plot/plot_base_estimator.xml

+<?xml version='1.0' encoding='UTF-8'?>
+<root version="2.3.0">
+ <tabbed_widget parent="main_window" name="Main Window">
+  <plotmatrix columns="3" rows="2" tab_name="plot">
+   <plot col="0" row="0">
+    <range top="0.001005" bottom="-0.041205" right="53.261000" left="3.261000"/>
+    <limitY/>
+    <curve R="31" custom_transform="noTransform" G="119" name="/gazebo/model_states/pose.1/position/x" B="180"/>
+    <curve R="241" custom_transform="noTransform" G="76" name="/sot/PYRENE/state/data.0" B="193"/>
+    <curve R="188" custom_transform="noTransform" G="189" name="/sot/base_estimator/q/data.0" B="34"/>
+    <transform value="noTransform"/>
+   </plot>
+   <plot col="0" row="1">
+    <range top="0.000454" bottom="-0.000255" right="53.261000" left="3.261000"/>
+    <limitY/>
+    <curve R="26" custom_transform="noTransform" G="201" name="/gazebo/model_states/pose.1/orientation/x" B="56"/>
+    <curve R="148" custom_transform="noTransform" G="103" name="/sot/PYRENE/state/data.3" B="189"/>
+    <curve R="31" custom_transform="noTransform" G="119" name="/sot/base_estimator/q/data.3" B="180"/>
+    <transform value="noTransform"/>
+   </plot>
+   <plot col="1" row="0">
+    <range top="0.000082" bottom="-0.003373" right="53.261000" left="3.261000"/>
+    <limitY/>
+    <curve R="214" custom_transform="noTransform" G="39" name="/gazebo/model_states/pose.1/position/y" B="40"/>
+    <curve R="148" custom_transform="noTransform" G="103" name="/sot/PYRENE/state/data.1" B="189"/>
+    <curve R="31" custom_transform="noTransform" G="119" name="/sot/base_estimator/q/data.1" B="180"/>
+    <transform value="noTransform"/>
+   </plot>
+   <plot col="1" row="1">
+    <range top="0.000205" bottom="-0.008397" right="53.261000" left="3.261000"/>
+    <limitY/>
+    <curve R="255" custom_transform="noTransform" G="127" name="/gazebo/model_states/pose.1/orientation/y" B="14"/>
+    <curve R="23" custom_transform="noTransform" G="190" name="/sot/PYRENE/state/data.4" B="207"/>
+    <curve R="214" custom_transform="noTransform" G="39" name="/sot/base_estimator/q/data.4" B="40"/>
+    <transform value="noTransform"/>
+   </plot>
+   <plot col="2" row="0">
+    <range top="1.021203" bottom="1.016997" right="53.261000" left="3.261000"/>
+    <limitY/>
+    <curve R="255" custom_transform="noTransform" G="127" name="/gazebo/model_states/pose.1/position/z" B="14"/>
+    <curve R="23" custom_transform="noTransform" G="190" name="/sot/PYRENE/state/data.2" B="207"/>
+    <curve R="214" custom_transform="noTransform" G="39" name="/sot/base_estimator/q/data.2" B="40"/>
+    <transform value="noTransform"/>
+   </plot>
+   <plot col="2" row="1">
+    <range top="0.001018" bottom="-0.000117" right="53.261000" left="3.261000"/>
+    <limitY/>
+    <curve R="241" custom_transform="noTransform" G="76" name="/gazebo/model_states/pose.1/orientation/z" B="193"/>
+    <curve R="188" custom_transform="noTransform" G="189" name="/sot/PYRENE/state/data.5" B="34"/>
+    <curve R="26" custom_transform="noTransform" G="201" name="/sot/base_estimator/q/data.5" B="56"/>
+    <transform value="noTransform"/>
+   </plot>
+  </plotmatrix>
+  <currentPlotMatrix index="0"/>
+ </tabbed_widget>
+ <use_relative_time_offset enabled="1"/>
+ <!-- - - - - - - - - - - - - - - -->
+ <!-- - - - - - - - - - - - - - - -->
+ <Plugins>
+  <plugin ID="DataLoad_CSV">
+   <default time_axis=""/>
+  </plugin>
+  <plugin ID="DataLoad_ROS_bags">
+   <use_header_stamp value="false"/>
+   <use_renaming_rules value="true"/>
+   <discard_large_arrays value="true"/>
+   <max_array_size value="100"/>
+  </plugin>
+  <plugin ID="DataLoad_ULog"/>
+  <plugin ID="ROS_Topic_Streamer">
+   <use_header_stamp value="false"/>
+   <use_renaming_rules value="true"/>
+   <discard_large_arrays value="true"/>
+   <max_array_size value="100"/>
+  </plugin>
+  <plugin ID="RosoutPublisherROS" status="idle"/>
+  <plugin ID="TopicPublisherROS" status="idle"/>
+ </Plugins>
+ <!-- - - - - - - - - - - - - - - -->
+ <previouslyLoaded_Datafiles/>
+ <previouslyLoaded_Streamer name="ROS_Topic_Streamer"/>
+ <!-- - - - - - - - - - - - - - - -->
+ <customMathEquations/>
+ <snippets>
+  <snippet name="1st_derivative">
+   <global>var prevX = 0
+var prevY = 0</global>
+   <equation>dx = time - prevX
+dy = value - prevY
+prevX = time
+prevY = value
+
+return dy/dx</equation>
+  </snippet>
+  <snippet name="1st_order_lowpass">
+   <global>var prevY = 0
+var alpha = 0.1</global>
+   <equation>prevY = alpha * value + (1.-alpha) * prevY
+
+return prevY</equation>
+  </snippet>
+  <snippet name="sum_A_B">
+   <global></global>
+   <equation>return $$PLOT_A$$ + $$PLOT_B$$</equation>
+  </snippet>
+  <snippet name="yaw_from_quaternion">
+   <global>// source: https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
+
+function quaternionToYaw(x, y, z, w)
+{
+  // yaw (z-axis rotation)
+  t1 = 2.0 * (w * z + x * y);
+  t2 = 1.0 - 2.0 * (y * y + z * z);
+  yaw = Math.atan2(t1, t2);
+
+  return yaw
+}</global>
+   <equation>return quaternionToYaw(x, y, z, w);</equation>
+  </snippet>
+ </snippets>
+ <!-- - - - - - - - - - - - - - - -->
+</root>