include/sot/talos_balance/math/fwd.hh

@@ -21,50 +21,46 @@
 #include <Eigen/Core>
 
 #ifdef EIGEN_RUNTIME_NO_MALLOC
-  #define EIGEN_MALLOC_ALLOWED Eigen::internal::set_is_malloc_allowed(true);
-  #define EIGEN_MALLOC_NOT_ALLOWED Eigen::internal::set_is_malloc_allowed(false);
+#define EIGEN_MALLOC_ALLOWED Eigen::internal::set_is_malloc_allowed(true);
+#define EIGEN_MALLOC_NOT_ALLOWED Eigen::internal::set_is_malloc_allowed(false);
 #else
-  #define EIGEN_MALLOC_ALLOWED
-  #define EIGEN_MALLOC_NOT_ALLOWED 
+#define EIGEN_MALLOC_ALLOWED
+#define EIGEN_MALLOC_NOT_ALLOWED
 #endif
-namespace dynamicgraph
-{
-  namespace sot
-  {
-    namespace talos_balance
-    {
-      namespace math
-      {
-        
-        typedef double Scalar;
-        typedef Eigen::Matrix<Scalar,Eigen::Dynamic,1> Vector;
-        typedef Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic> Matrix;
-        typedef Eigen::VectorXi VectorXi;
-        typedef Eigen::Matrix<bool,Eigen::Dynamic,1> VectorXb;
-        
-        typedef Eigen::Matrix<Scalar,3,1> Vector3;
-        typedef Eigen::Matrix<Scalar,6,1> Vector6;
-        typedef Eigen::Matrix<Scalar,3,Eigen::Dynamic> Matrix3x;
-        
-        typedef Eigen::Ref<Vector3>             RefVector3;
-        typedef const Eigen::Ref<const Vector3> ConstRefVector3;
-        
-        typedef Eigen::Ref<Vector>              RefVector;
-        typedef const Eigen::Ref<const Vector>  ConstRefVector;
-        
-        typedef Eigen::Ref<Matrix>              RefMatrix;
-        typedef const Eigen::Ref<const Matrix>  ConstRefMatrix;
-        
-        typedef std::size_t Index;
-        
-        // Forward declaration of constraints
-        class ConstraintBase;
-        class ConstraintEquality;
-        class ConstraintInequality;
-        class ConstraintBound;
-        
-      }
-    }
-  }
-}
-#endif // ifndef __invdyn_math_fwd_hpp__
+namespace dynamicgraph {
+namespace sot {
+namespace talos_balance {
+namespace math {
+
+typedef double Scalar;
+typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> Vector;
+typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> Matrix;
+typedef Eigen::VectorXi VectorXi;
+typedef Eigen::Matrix<bool, Eigen::Dynamic, 1> VectorXb;
+
+typedef Eigen::Matrix<Scalar, 3, 1> Vector3;
+typedef Eigen::Matrix<Scalar, 6, 1> Vector6;
+typedef Eigen::Matrix<Scalar, 3, Eigen::Dynamic> Matrix3x;
+
+typedef Eigen::Ref<Vector3> RefVector3;
+typedef const Eigen::Ref<const Vector3> ConstRefVector3;
+
+typedef Eigen::Ref<Vector> RefVector;
+typedef const Eigen::Ref<const Vector> ConstRefVector;
+
+typedef Eigen::Ref<Matrix> RefMatrix;
+typedef const Eigen::Ref<const Matrix> ConstRefMatrix;
+
+typedef std::size_t Index;
+
+// Forward declaration of constraints
+class ConstraintBase;
+class ConstraintEquality;
+class ConstraintInequality;
+class ConstraintBound;
+
+}  // namespace math
+}  // namespace talos_balance
+}  // namespace sot
+}  // namespace dynamicgraph
+#endif  // ifndef __invdyn_math_fwd_hpp__

include/sot/talos_balance/nd-trajectory-generator.hh

 /*
  * Copyright 2017,Thomas Flayols, LAAS-CNRS
- * File derived from sot-torque-control package available on 
+ * 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.
@@ -23,167 +23,175 @@
 /* --- API ------------------------------------------------------------- */
 /* --------------------------------------------------------------------- */
 
-#if defined (WIN32)
-#  if defined (nd_position_controller_EXPORTS)
-#    define SOTNDTRAJECTORYGENERATOR_EXPORT __declspec(dllexport)
-#  else
-#    define SOTNDTRAJECTORYGENERATOR_EXPORT __declspec(dllimport)
-#  endif
+#if defined(WIN32)
+#if defined(nd_position_controller_EXPORTS)
+#define SOTNDTRAJECTORYGENERATOR_EXPORT __declspec(dllexport)
 #else
-#  define SOTNDTRAJECTORYGENERATOR_EXPORT
+#define SOTNDTRAJECTORYGENERATOR_EXPORT __declspec(dllimport)
+#endif
+#else
+#define SOTNDTRAJECTORYGENERATOR_EXPORT
 #endif
-
 
 /* --------------------------------------------------------------------- */
 /* --- INCLUDE --------------------------------------------------------- */
 /* --------------------------------------------------------------------- */
 
+#include <dynamic-graph/signal-helper.h>
 
-#include <parametric-curves/spline.hpp>
+#include <map>
 #include <parametric-curves/constant.hpp>
-#include <parametric-curves/text-file.hpp>
-#include <parametric-curves/minimum-jerk.hpp>
-#include <parametric-curves/linear-chirp.hpp>
-#include <parametric-curves/infinite-sinusoid.hpp>
 #include <parametric-curves/infinite-const-acc.hpp>
-
-#include <dynamic-graph/signal-helper.h>
+#include <parametric-curves/infinite-sinusoid.hpp>
+#include <parametric-curves/linear-chirp.hpp>
+#include <parametric-curves/minimum-jerk.hpp>
+#include <parametric-curves/spline.hpp>
+#include <parametric-curves/text-file.hpp>
 #include <sot/core/matrix-geometry.hh>
 
-#include <map>
 #include "boost/assign.hpp"
 
-
 namespace dynamicgraph {
-  namespace sot {
-    namespace talos_balance {
-
-      /* --------------------------------------------------------------------- */
-      /* --- CLASS ----------------------------------------------------------- */
-      /* --------------------------------------------------------------------- */
-
-      class SOTNDTRAJECTORYGENERATOR_EXPORT NdTrajectoryGenerator
-	:public::dynamicgraph::Entity
-      {
-        typedef NdTrajectoryGenerator EntityClassName;
-        DYNAMIC_GRAPH_ENTITY_DECL();
-
-      public:
-        /* --- CONSTRUCTOR ---- */
-        NdTrajectoryGenerator( const std::string & name );
-
-        void init(const double& dt, const unsigned int& n);
-
-        /* --- SIGNALS --- */
-        DECLARE_SIGNAL_IN(initial_value,  dynamicgraph::Vector);
-        DECLARE_SIGNAL_IN(trigger,        bool);
-        DECLARE_SIGNAL(x, OUT,            dynamicgraph::Vector);
-        DECLARE_SIGNAL_OUT(dx,            dynamicgraph::Vector);
-        DECLARE_SIGNAL_OUT(ddx,           dynamicgraph::Vector);
-
-      protected:
-        DECLARE_SIGNAL_OUT_FUNCTION(x,    dynamicgraph::Vector);
-
-      public:
-
-        /* --- COMMANDS --- */
-
-        void playTrajectoryFile(const std::string& fileName);
-
-        void playSpline(const std::string& fileName);
-
-        void setSpline(const std::string& filename, const double& timeToInitConf);
-        void startSpline();
-
-        /** Print the current value of the specified component. */
-        void getValue(const int& id);
-
-        /** Move a component to a position with a minimum-jerk trajectory.
-         * @param id integer index.
-         * @param xFinal The desired final position of the component.
-         * @param time The time to go from the current position to xFinal [sec].
-         */
-        void move(const int& id, const double& xFinal, const double& time);
-
-        /** Start an infinite sinusoidal trajectory.
-         * @param id integer index.
-         * @param xFinal The position of the component corresponding to the max amplitude of the sinusoid.
-         * @param time The time to go from the current position to xFinal [sec].
-         */
-        void startSinusoid(const int& id, const double& xFinal, const double& time);
-
-        /** Start an infinite triangle trajectory.
-         * @param id integer index.
-         * @param xFinal The position of the component corresponding to the max amplitude of the trajectory.
-         * @param time The time to go from the current position to xFinal [sec].
-         */
-        //void startTriangle(const int& id, const double& xFinal, const double& time, const double& Tacc);
-
-        /** Start an infinite trajectory with piece-wise constant acceleration.
-         * @param id integer index.
-         * @param xFinal The position of the component corresponding to the max amplitude of the trajectory.
-         * @param time The time to go from the current position to xFinal [sec].
-         * @param Tacc The time during witch acceleration is keept constant [sec].
-         */
-        void startConstAcc(const int& id, const double& xFinal, const double& time);
-
-        /** Start a linear-chirp trajectory, that is a sinusoidal trajectory with frequency
-         * being a linear function of time.
-         * @param id integer index.
-         * @param xFinal The position of the component corresponding to the max amplitude of the sinusoid [rad].
-         * @param f0 The initial (min) frequency of the sinusoid [Hz]
-         * @param f1 The final (max) frequency of the sinusoid [Hz]
-         * @param time The time to get from f0 to f1 [sec]
-         */
-        void startLinearChirp(const int& id, const double& xFinal, const double& f0, const double& f1, const double& time);
-
-        /** Stop the motion of the specified component. If id is -1
-         * it stops the trajectory of all the vector.
-         * @param id integer index.
-         * */
-        void stop(const int& id);
-
-        /* --- ENTITY INHERITANCE --- */
-        virtual void display( std::ostream& os ) const;
-
-      protected:
-        enum JTG_Status
-        {
-          JTG_STOP,
-          JTG_SINUSOID,
-          JTG_MIN_JERK,
-          JTG_LIN_CHIRP,
-          //JTG_TRIANGLE,
-          JTG_CONST_ACC,
-          JTG_TEXT_FILE,
-          JTG_SPLINE
-        };
-
-        bool              m_initSucceeded;    /// true if the entity has been successfully initialized
-        bool              m_firstIter;        /// true if it is the first iteration, false otherwise
-        double            m_dt;               /// control loop time step.
-        double            m_t;                /// current control loop time.
-        unsigned int      m_n;                /// size of ouput vector
-        unsigned int      m_iterLast;         /// last iter index
-        bool              m_splineReady;      /// true if the spline has been successfully loaded.
-
-        std::vector<JTG_Status> m_status;     /// status of the component
-        std::vector<parametriccurves::AbstractCurve<double, dynamicgraph::sot::Vector1d>* >  m_currentTrajGen;
-        std::vector<parametriccurves::Constant<double, 1>* >                     m_noTrajGen;
-        std::vector<parametriccurves::MinimumJerk<double, 1>* >                  m_minJerkTrajGen;
-        std::vector<parametriccurves::InfiniteSinusoid<double,1>* >              m_sinTrajGen;
-        std::vector<parametriccurves::LinearChirp<double,1>*>                    m_linChirpTrajGen;
-        //std::vector<parametriccurves::InfiniteTriangular<double,1>* >            m_triangleTrajGen;
-        std::vector<parametriccurves::InfiniteConstAcc<double,1>* >          m_constAccTrajGen;
-        parametriccurves::TextFile<double, Eigen::Dynamic>*                      m_textFileTrajGen;
-        parametriccurves::Spline<double, Eigen::Dynamic>*                        m_splineTrajGen;
-
-      }; // class NdTrajectoryGenerator
-
-    }    // namespace talos_balance
-  }      // namespace sot
-}        // namespace dynamicgraph
-
-
-
-#endif // #ifndef __sot_talos_balance_nd_trajectory_generator_H__
+namespace sot {
+namespace talos_balance {
+
+/* --------------------------------------------------------------------- */
+/* --- CLASS ----------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+class SOTNDTRAJECTORYGENERATOR_EXPORT NdTrajectoryGenerator
+    : public ::dynamicgraph::Entity {
+  typedef NdTrajectoryGenerator EntityClassName;
+  DYNAMIC_GRAPH_ENTITY_DECL();
+
+ public:
+  /* --- CONSTRUCTOR ---- */
+  NdTrajectoryGenerator(const std::string& name);
+
+  void init(const double& dt, const unsigned int& n);
+
+  /* --- SIGNALS --- */
+  DECLARE_SIGNAL_IN(initial_value, dynamicgraph::Vector);
+  DECLARE_SIGNAL_IN(trigger, bool);
+  DECLARE_SIGNAL(x, OUT, dynamicgraph::Vector);
+  DECLARE_SIGNAL_OUT(dx, dynamicgraph::Vector);
+  DECLARE_SIGNAL_OUT(ddx, dynamicgraph::Vector);
+
+ protected:
+  DECLARE_SIGNAL_OUT_FUNCTION(x, dynamicgraph::Vector);
+
+ public:
+  /* --- COMMANDS --- */
+
+  void playTrajectoryFile(const std::string& fileName);
+
+  void playSpline(const std::string& fileName);
+
+  void setSpline(const std::string& filename, const double& timeToInitConf);
+  void startSpline();
+
+  /** Print the current value of the specified component. */
+  void getValue(const int& id);
+
+  /** Move a component to a position with a minimum-jerk trajectory.
+   * @param id integer index.
+   * @param xFinal The desired final position of the component.
+   * @param time The time to go from the current position to xFinal [sec].
+   */
+  void move(const int& id, const double& xFinal, const double& time);
+
+  /** Instantaneously set a component to a given position.
+   * @param id integer index.
+   * @param xVal The desired position of the component.
+   */
+  void set(const int& id, const double& xVal);
+
+  /** Start an infinite sinusoidal trajectory.
+   * @param id integer index.
+   * @param xFinal The position of the component corresponding to the max
+   * amplitude of the sinusoid.
+   * @param time The time to go from the current position to xFinal [sec].
+   */
+  void startSinusoid(const int& id, const double& xFinal, const double& time);
+
+  /** Start an infinite triangle trajectory.
+   * @param id integer index.
+   * @param xFinal The position of the component corresponding to the max
+   * amplitude of the trajectory.
+   * @param time The time to go from the current position to xFinal [sec].
+   */
+  // void startTriangle(const int& id, const double& xFinal, const double& time,
+  // const double& Tacc);
+
+  /** Start an infinite trajectory with piece-wise constant acceleration.
+   * @param id integer index.
+   * @param xFinal The position of the component corresponding to the max
+   * amplitude of the trajectory.
+   * @param time The time to go from the current position to xFinal [sec].
+   * @param Tacc The time during witch acceleration is keept constant [sec].
+   */
+  void startConstAcc(const int& id, const double& xFinal, const double& time);
+
+  /** Start a linear-chirp trajectory, that is a sinusoidal trajectory with
+   * frequency being a linear function of time.
+   * @param id integer index.
+   * @param xFinal The position of the component corresponding to the max
+   * amplitude of the sinusoid [rad].
+   * @param f0 The initial (min) frequency of the sinusoid [Hz]
+   * @param f1 The final (max) frequency of the sinusoid [Hz]
+   * @param time The time to get from f0 to f1 [sec]
+   */
+  void startLinearChirp(const int& id, const double& xFinal, const double& f0,
+                        const double& f1, const double& time);
+
+  /** Stop the motion of the specified component. If id is -1
+   * it stops the trajectory of all the vector.
+   * @param id integer index.
+   * */
+  void stop(const int& id);
+
+  /* --- ENTITY INHERITANCE --- */
+  virtual void display(std::ostream& os) const;
+
+ protected:
+  enum JTG_Status {
+    JTG_STOP,
+    JTG_SINUSOID,
+    JTG_MIN_JERK,
+    JTG_LIN_CHIRP,
+    // JTG_TRIANGLE,
+    JTG_CONST_ACC,
+    JTG_TEXT_FILE,
+    JTG_SPLINE
+  };
+
+  bool
+      m_initSucceeded;  /// true if the entity has been successfully initialized
+  bool m_firstIter;     /// true if it is the first iteration, false otherwise
+  double m_dt;          /// control loop time step.
+  double m_t;           /// current control loop time.
+  unsigned int m_n;     /// size of ouput vector
+  unsigned int m_iterLast;  /// last iter index
+  bool m_splineReady;       /// true if the spline has been successfully loaded.
+
+  std::vector<JTG_Status> m_status;  /// status of the component
+  std::vector<
+      parametriccurves::AbstractCurve<double, dynamicgraph::sot::Vector1d>*>
+      m_currentTrajGen;
+  std::vector<parametriccurves::Constant<double, 1>*> m_noTrajGen;
+  std::vector<parametriccurves::MinimumJerk<double, 1>*> m_minJerkTrajGen;
+  std::vector<parametriccurves::InfiniteSinusoid<double, 1>*> m_sinTrajGen;
+  std::vector<parametriccurves::LinearChirp<double, 1>*> m_linChirpTrajGen;
+  // std::vector<parametriccurves::InfiniteTriangular<double,1>* >
+  // m_triangleTrajGen;
+  std::vector<parametriccurves::InfiniteConstAcc<double, 1>*> m_constAccTrajGen;
+  parametriccurves::TextFile<double, Eigen::Dynamic>* m_textFileTrajGen;
+  parametriccurves::Spline<double, Eigen::Dynamic>* m_splineTrajGen;
+
+};  // class NdTrajectoryGenerator
+
+}  // namespace talos_balance
+}  // namespace sot
+}  // namespace dynamicgraph
+
+#endif  // #ifndef __sot_talos_balance_nd_trajectory_generator_H__

include/sot/talos_balance/parameter-server.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_parameter_server_H__
-#define __sot_torque_control_parameter_server_H__
-
-/* --------------------------------------------------------------------- */
-/* --- API ------------------------------------------------------------- */
-/* --------------------------------------------------------------------- */
-
-#if defined (WIN32)
-#  if defined (__sot_torque_parameter_server_H__)
-#    define SOTParameterServer_EXPORT __declspec(dllexport)
-#  else
-#    define SOTParameterServer_EXPORT __declspec(dllimport)
-#  endif
-#else
-#  define SOTParameterServer_EXPORT
-#endif
-
-
-/* --------------------------------------------------------------------- */
-/* --- INCLUDE --------------------------------------------------------- */
-/* --------------------------------------------------------------------- */
-
-#include <dynamic-graph/signal-helper.h>
-#include <sot/core/matrix-geometry.hh>
-#include <sot/core/robot-utils.hh>
-#include <map>
-#include "boost/assign.hpp"
-
-
-#include <pinocchio/multibody/model.hpp>
-#include <pinocchio/parsers/urdf.hpp>
-#include <sot/talos_balance/robot/robot-wrapper.hh>
-
-namespace dynamicgraph {
-  namespace sot {
-    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 SOTParameterServer_EXPORT ParameterServer
-        :public::dynamicgraph::Entity
-      {
-        typedef ParameterServer EntityClassName;
-        DYNAMIC_GRAPH_ENTITY_DECL();
-
-      public:
-        /* --- CONSTRUCTOR ---- */
-        ParameterServer( 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 & urdfFile,
-                  const std::string & robotRef);
-
-        /* --- SIGNALS --- */
-
-
-        /* --- COMMANDS --- */
-
-	      /// 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);
-
-	      /// Command related to ForceUtil
-	      void setForceLimitsFromId(const double &jointId, const dynamicgraph::Vector &lq, const dynamicgraph::Vector &uq);
-	      void setForceNameToForceId(const std::string& forceName, const double & forceId);
-	
-	      /// Commands related to FootUtil
-	      void setRightFootSoleXYZ(const dynamicgraph::Vector &);
-        void setRightFootForceSensorXYZ(const dynamicgraph::Vector &);
-	      void setFootFrameName(const std::string &, const std::string &);
-        void setImuJointName(const std::string &);
-	      void displayRobotUtil();
-	      /// Set the mapping between urdf and sot.
-	      void setJoints(const dynamicgraph::Vector &);
-
-        /* --- ENTITY INHERITANCE --- */
-        virtual void display( std::ostream& os ) const;
-
-      protected:
-        RobotUtil *                   m_robot_util;
-        dynamicgraph::sot::talos_balance::robots::RobotWrapper *  m_robot;
-        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)
-
-        bool convertJointNameToJointId(const std::string& name, unsigned int& id);
-        bool isJointInRange(unsigned int id, double q);
-        void updateJointCtrlModesOutputSignal();
-
-      }; // class ParameterServer
-
-    }    // namespace talos_balance
-  }      // namespace sot
-}        // namespace dynamicgraph
-
-
-
-#endif // #ifndef __sot_torque_control_control_manager_H__

include/sot/talos_balance/pose-quaternion-to-matrix-homo.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_pose_quaternion_to_matrix_homo_H__
+#define __sot_talos_balance_pose_quaternion_to_matrix_homo_H__
+
+/* --------------------------------------------------------------------- */
+/* --- API ------------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+#if defined(WIN32)
+#if defined(position_controller_EXPORTS)
+#define POSEQUATERNIONTOMATRIXHOMO_EXPORT __declspec(dllexport)
+#else
+#define POSEQUATERNIONTOMATRIXHOMO_EXPORT __declspec(dllimport)
+#endif
+#else
+#define POSEQUATERNIONTOMATRIXHOMO_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 POSEQUATERNIONTOMATRIXHOMO_EXPORT PoseQuaternionToMatrixHomo
+    : public ::dynamicgraph::Entity {
+  DYNAMIC_GRAPH_ENTITY_DECL();
+
+ public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  /* --- CONSTRUCTOR ---- */
+  PoseQuaternionToMatrixHomo(const std::string& name);
+
+  void init() {}
+
+  /* --- SIGNALS --- */
+  DECLARE_SIGNAL_IN(sin, ::dynamicgraph::Vector);
+
+  DECLARE_SIGNAL_OUT(sout, MatrixHomogeneous);
+
+  /* --- COMMANDS --- */
+  /* --- ENTITY INHERITANCE --- */
+  virtual void display(std::ostream& os) const;
+};  // class PoseQuaternionToMatrixHomo
+
+}  // namespace talos_balance
+}  // namespace sot
+}  // namespace dynamicgraph
+
+#endif  // #ifndef __sot_talos_balance_pose_quaternion_to_matrix_homo_H__

include/sot/talos_balance/quat-to-euler.hh

@@ -21,64 +21,59 @@
 /* --- API ------------------------------------------------------------- */
 /* --------------------------------------------------------------------- */
 
-#if defined (WIN32)
-#  if defined (position_controller_EXPORTS)
-#    define EULERTOQUAT_EXPORT __declspec(dllexport)
-#  else
-#    define EULERTOQUAT_EXPORT __declspec(dllimport)
-#  endif
+#if defined(WIN32)
+#if defined(position_controller_EXPORTS)
+#define QUATTOEULER_EXPORT __declspec(dllexport)
 #else
-#  define EULERTOQUAT_EXPORT
+#define QUATTOEULER_EXPORT __declspec(dllimport)
+#endif
+#else
+#define QUATTOEULER_EXPORT
 #endif
-
 
 /* --------------------------------------------------------------------- */
 /* --- INCLUDE --------------------------------------------------------- */
 /* --------------------------------------------------------------------- */
 
+#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 {
-
-      /* --------------------------------------------------------------------- */
-      /* --- CLASS ----------------------------------------------------------- */
-      /* --------------------------------------------------------------------- */
+#include "boost/assign.hpp"
 
-      class EULERTOQUAT_EXPORT QuatToEuler
-	                         : public ::dynamicgraph::Entity
-      {
-        DYNAMIC_GRAPH_ENTITY_DECL();
+namespace dynamicgraph {
+namespace sot {
+namespace talos_balance {
 
-      public:
-        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+/* --------------------------------------------------------------------- */
+/* --- CLASS ----------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
 
-        /* --- CONSTRUCTOR ---- */
-        QuatToEuler( const std::string & name );
+class QUATTOEULER_EXPORT QuatToEuler : public ::dynamicgraph::Entity {
+  DYNAMIC_GRAPH_ENTITY_DECL();
 
-        void init() {}
+ public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
-        /* --- SIGNALS --- */
-        DECLARE_SIGNAL_IN(quaternion, ::dynamicgraph::Vector);
+  /* --- CONSTRUCTOR ---- */
+  QuatToEuler(const std::string& name);
 
-        DECLARE_SIGNAL_OUT(euler, ::dynamicgraph::Vector);
+  void init() {}
 
-        /* --- COMMANDS --- */
-        /* --- ENTITY INHERITANCE --- */
-        virtual void display( std::ostream& os ) const;
-      }; // class QuatToEuler
+  /* --- SIGNALS --- */
+  DECLARE_SIGNAL_IN(quaternion, ::dynamicgraph::Vector);
 
-    }    // namespace talos_balance
-  }      // namespace sot
-}        // namespace dynamicgraph
+  DECLARE_SIGNAL_OUT(euler, ::dynamicgraph::Vector);
 
+  /* --- COMMANDS --- */
+  /* --- ENTITY INHERITANCE --- */
+  virtual void display(std::ostream& os) const;
+};  // class QuatToEuler
 
+}  // namespace talos_balance
+}  // namespace sot
+}  // namespace dynamicgraph
 
-#endif // #ifndef __sot_talos_balance_quat_to_euler_H__
+#endif  // #ifndef __sot_talos_balance_quat_to_euler_H__