diff --git a/CMakeLists.txt b/CMakeLists.txt
index 8e6c681b1cba6d7377d16169ab1a7111e46fe91b..fb5bf237ac1774fd7cef9b60822f6949b0986c25 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -58,6 +58,7 @@ SET(BOOST_COMPONENTS thread filesystem program_options unit_test_framework syste
OPTION (BUILD_ROS_PACKAGES "Build ros packages" ON)
OPTION (BUILD_PYTHON_INTERFACE "Build the python bindings" ON)
+OPTION (INSTALL_PYTHON_INTERFACE_ONLY "Install *ONLY* the python binding" OFF)
OPTION (BUILD_TEST "Build tests" ON)
# name of the python module
@@ -89,8 +90,6 @@ SET(LIBRARY_NAME ${SOTTALOSBALANCE_LIB_NAME})
SET(${LIBRARY_NAME}_HEADERS
include/sot/talos_balance/utils/commands-helper.hh
- include/sot/talos_balance/utils/stop-watch.hh
- include/sot/talos_balance/utils/causal-filter.hh
include/sot/talos_balance/utils/statistics.hh
include/sot/talos_balance/math/fwd.hh
include/sot/talos_balance/robot/fwd.hh
@@ -106,9 +105,7 @@ SET(${LIBRARY_NAME}_HEADERS
# PERMISSIONS OWNER_READ GROUP_READ WORLD_READ OWNER_WRITE)
SET(${LIBRARY_NAME}_SOURCES ${${LIBRARY_NAME}_HEADERS}
- src/utils/causal-filter.cpp
src/utils/statistics.cpp
- src/utils/stop-watch.cpp
src/robot/robot-wrapper.cpp
src/sdk_qualisys/Network.cpp
src/sdk_qualisys/RTPacket.cpp
@@ -146,7 +143,9 @@ ENDIF(UNIX AND NOT APPLE)
TARGET_LINK_LIBRARIES(${LIBRARY_NAME} ${Boost_LIBRARIES})
-INSTALL(TARGETS ${LIBRARY_NAME} DESTINATION ${CMAKE_INSTALL_LIBDIR})
+IF (NOT INSTALL_PYTHON_INTERFACE_ONLY)
+ INSTALL(TARGETS ${LIBRARY_NAME} DESTINATION ${CMAKE_INSTALL_LIBDIR})
+ENDIF(NOT INSTALL_PYTHON_INTERFACE_ONLY)
IF(BUILD_PYTHON_INTERFACE)
INSTALL(FILES python/${SOTTALOSBALANCE_PYNAME}/__init__.py
@@ -178,6 +177,8 @@ IF(BUILD_PYTHON_INTERFACE)
python/${SOTTALOSBALANCE_PYNAME}/test/appli_dcmComZmpControl.py
python/${SOTTALOSBALANCE_PYNAME}/test/test_dcmZmpControl.py
python/${SOTTALOSBALANCE_PYNAME}/test/appli_dcmZmpControl.py
+ python/${SOTTALOSBALANCE_PYNAME}/test/test_dcmZmpControl_file.py
+ python/${SOTTALOSBALANCE_PYNAME}/test/appli_dcmZmpControl_file.py
python/${SOTTALOSBALANCE_PYNAME}/test/test_comAdmittance.py
python/${SOTTALOSBALANCE_PYNAME}/test/test_admittance_single_joint.py
python/${SOTTALOSBALANCE_PYNAME}/test/appli_admittance_single_joint.py
@@ -245,8 +246,10 @@ MACRO(DYNAMIC_GRAPH_CUSTOM_PYTHON_MODULE MODULENAME SUBMODULENAME LIBRARYNAME TA
)
CMAKE_POLICY(POP)
- TARGET_LINK_LIBRARIES(${PYTHON_MODULE} ${PUBLIC_KEYWORD} "-Wl,--no-as-needed")
- TARGET_LINK_LIBRARIES(${PYTHON_MODULE} ${PUBLIC_KEYWORD} ${LIBRARYNAME} ${PYTHON_LIBRARY})
+ TARGET_LINK_LIBRARIES(${PYTHON_MODULE} ${PUBLIC_KEYWORD}
+ "-Wl,--no-as-needed")
+ TARGET_LINK_LIBRARIES(${PYTHON_MODULE} ${PUBLIC_KEYWORD}
+ ${LIBRARYNAME} ${PYTHON_LIBRARY})
INCLUDE_DIRECTORIES(${PYTHON_INCLUDE_PATH})
@@ -282,7 +285,8 @@ MACRO(DYNAMIC_GRAPH_CUSTOM_PYTHON_MODULE MODULENAME SUBMODULENAME LIBRARYNAME TA
ENDMACRO(DYNAMIC_GRAPH_CUSTOM_PYTHON_MODULE)
MACRO(SOT_TALOS_BALANCE_PYTHON_MODULE SUBMODULENAME LIBRARYNAME TARGETNAME)
- DYNAMIC_GRAPH_CUSTOM_PYTHON_MODULE("${SOTTALOSBALANCE_PYNAME}" "${SUBMODULENAME}" "${LIBRARYNAME}" "${TARGETNAME}")
+ DYNAMIC_GRAPH_CUSTOM_PYTHON_MODULE("${SOTTALOSBALANCE_PYNAME}"
+ "${SUBMODULENAME}" "${LIBRARYNAME}" "${TARGETNAME}")
ENDMACRO(SOT_TALOS_BALANCE_PYTHON_MODULE)
ADD_SUBDIRECTORY(src)
diff --git a/include/sot/talos_balance/filter-differentiator.hh b/include/sot/talos_balance/filter-differentiator.hh
deleted file mode 100644
index d713ba74cbee145fd02439a1f9db4533dc1e9758..0000000000000000000000000000000000000000
--- a/include/sot/talos_balance/filter-differentiator.hh
+++ /dev/null
@@ -1,118 +0,0 @@
-/*
- * 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/>.
- */
-
-#ifndef __sot_torque_control_FilterDifferentiator_H__
-#define __sot_torque_control_FilterDifferentiator_H__
-/* --------------------------------------------------------------------- */
-/* --- API ------------------------------------------------------------- */
-/* --------------------------------------------------------------------- */
-
-#if defined (WIN32)
-# if defined (low_pass_filter_EXPORTS)
-# define SOTFILTERDIFFERENTIATOR_EXPORT __declspec(dllexport)
-# else
-# define SOTFILTERDIFFERENTIATOR_EXPORT __declspec(dllimport)
-# endif
-#else
-# define SOTFILTERDIFFERENTIATOR_EXPORT
-#endif
-
-//#define VP_DEBUG 1 /// enable debug output
-//#define VP_DEBUG_MODE 20
-
-/* --------------------------------------------------------------------- */
-/* --- INCLUDE --------------------------------------------------------- */
-/* --------------------------------------------------------------------- */
-
-/* HELPER */
-#include <dynamic-graph/signal-helper.h>
-#include <sot/talos_balance/utils/stop-watch.hh>
-#include <sot/talos_balance/utils/causal-filter.hh>
-
-namespace dynamicgraph {
- namespace sot {
- namespace talos_balance {
-
- /**
- * This Entity takes as inputs a signal and applies a low pass filter
- and computes finite difference derivative.
- */
- class SOTFILTERDIFFERENTIATOR_EXPORT FilterDifferentiator
- :public ::dynamicgraph::Entity
- {
- DYNAMIC_GRAPH_ENTITY_DECL();
-
- public: /* --- SIGNALS --- */
- DECLARE_SIGNAL_IN(x, dynamicgraph::Vector);
- DECLARE_SIGNAL_OUT(x_filtered, dynamicgraph::Vector);
- DECLARE_SIGNAL_OUT(dx, dynamicgraph::Vector);
- DECLARE_SIGNAL_OUT(ddx, dynamicgraph::Vector);
-
- /// The following inner signals are used because this entity has some output signals
- /// whose related quantities are computed at the same time by the same algorithm
- /// To avoid the risk of recomputing the same things twice, we create an inner signal that groups together
- /// all the quantities that are computed together. Then the single output signals will depend
- /// on this inner signal, which is the one triggering the computations.
- /// Inner signals are not exposed, so that nobody can access them.
-
- /// This signal contains the estimated positions, velocities and accelerations.
- DECLARE_SIGNAL_INNER(x_dx_ddx, dynamicgraph::Vector);
-
- protected:
-
- double m_dt; /// sampling timestep of the input signal
- int m_x_size;
-
- /// polynomial-fitting filters
- CausalFilter* m_filter;
-
- public:
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-
- /** --- CONSTRUCTOR ---- */
- FilterDifferentiator( const std::string & name );
-
- /** Initialize the FilterDifferentiator.
- * @param timestep Period (in seconds) after which the sensors' data are updated.
- * @param sigSize Size of the input signal.
- * @param delay Delay (in seconds) introduced by the estimation.
- * This should be a multiple of timestep.
- * @note The estimationDelay is half of the length of the window used for the
- * polynomial fitting. The larger the delay, the smoother the estimations.
- */
- void init(const double ×tep,
- const int& xSize,
- const Eigen::VectorXd& filter_numerator,
- const Eigen::VectorXd& filter_denominator);
-
- void switch_filter(const Eigen::VectorXd& filter_numerator,
- const Eigen::VectorXd& filter_denominator);
-
-
- protected:
-
- public: /* --- ENTITY INHERITANCE --- */
- virtual void display( std::ostream& os ) const;
-
- }; // class FilterDifferentiator
-
- } // namespace talos_balance
- } // namespace sot
-} // namespace dynamicgraph
-
-
-
-#endif // #ifndef __sot_torque_control_FilterDifferentiator_H__
diff --git a/include/sot/talos_balance/imu_offset_compensation.hh b/include/sot/talos_balance/imu_offset_compensation.hh
deleted file mode 100644
index d5854bb888c2189b36511c911d17917171ece49b..0000000000000000000000000000000000000000
--- a/include/sot/talos_balance/imu_offset_compensation.hh
+++ /dev/null
@@ -1,99 +0,0 @@
-/*
- * Copyright 2017, 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_imu_offset_compensation_H__
-#define __sot_torque_control_imu_offset_compensation_H__
-
-/* --------------------------------------------------------------------- */
-/* --- API ------------------------------------------------------------- */
-/* --------------------------------------------------------------------- */
-
-#if defined (WIN32)
-# if defined (imu_offset_compensation_EXPORTS)
-# define SOTIMUOFFSETCOMPENSATION_EXPORT __declspec(dllexport)
-# else
-# define SOTIMUOFFSETCOMPENSATION_EXPORT __declspec(dllimport)
-# endif
-#else
-# define SOTIMUOFFSETCOMPENSATION_EXPORT
-#endif
-
-
-/* --------------------------------------------------------------------- */
-/* --- INCLUDE --------------------------------------------------------- */
-/* --------------------------------------------------------------------- */
-
-#include <dynamic-graph/signal-helper.h>
-#include <sot/core/matrix-geometry.hh>
-#include <map>
-#include "boost/assign.hpp"
-
-namespace dynamicgraph {
- namespace sot {
- namespace talos_balance {
-
- /* --------------------------------------------------------------------- */
- /* --- CLASS ----------------------------------------------------------- */
- /* --------------------------------------------------------------------- */
-
- class SOTIMUOFFSETCOMPENSATION_EXPORT ImuOffsetCompensation
- :public::dynamicgraph::Entity
- {
- typedef ImuOffsetCompensation EntityClassName;
- DYNAMIC_GRAPH_ENTITY_DECL();
- typedef Eigen::Vector3d Vector3;
-
- public:
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-
- /* --- CONSTRUCTOR ---- */
- ImuOffsetCompensation( const std::string & name );
-
- /* --- COMMANDS --- */
- void init(const double& dt);
- void update_offset(const double & duration);
- void setGyroDCBlockerParameter(const double & alpha);
- /* --- SIGNALS --- */
- DECLARE_SIGNAL_IN(accelerometer_in, dynamicgraph::Vector); /// raw accelerometer data
- DECLARE_SIGNAL_IN(gyrometer_in, dynamicgraph::Vector); /// raw gyrometer data
- DECLARE_SIGNAL_OUT(accelerometer_out, dynamicgraph::Vector); /// compensated accelerometer data
- DECLARE_SIGNAL_OUT(gyrometer_out, dynamicgraph::Vector); /// compensated gyrometer data
-
- protected:
- /* --- ENTITY INHERITANCE --- */
- virtual void display( std::ostream& os ) const;
-
- /* --- METHODS --- */
- void update_offset_impl(int iter);
-
- protected:
- bool m_initSucceeded; /// true if the entity has been successfully initialized
- double m_dt; /// sampling time in seconds
- int m_update_cycles_left; /// number of update cycles left
- int m_update_cycles; /// total number of update cycles to perform
- double m_a_gyro_DC_blocker; /// filter parameter to remove DC from gyro online (should be close to <1.0 and equal to 1.0 for disabling)
- Vector3 m_gyro_offset; /// gyrometer offset
- Vector3 m_acc_offset; /// accelerometer offset
-
- Vector3 m_gyro_sum; /// tmp variable to store the sum of the gyro measurements during update phase
- Vector3 m_acc_sum; /// tmp variable to store the sum of the acc measurements during update phase
-
- }; // class ImuOffsetCompensation
- } // namespace talos_balance
- } // namespace sot
-} // namespace dynamicgraph
-
-#endif // #ifndef __sot_torque_control_imu_offset_compensation_H__
diff --git a/include/sot/talos_balance/madgwickahrs.hh b/include/sot/talos_balance/madgwickahrs.hh
deleted file mode 100644
index 6f6eb388c1f2fbd8131a0c225d51a4667d10b05c..0000000000000000000000000000000000000000
--- a/include/sot/talos_balance/madgwickahrs.hh
+++ /dev/null
@@ -1,108 +0,0 @@
-//=====================================================================================================
-//
-// Implementation of Madgwick's IMU and AHRS algorithms.
-// See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
-//
-// Date Author Notes
-// 29/09/2011 SOH Madgwick Initial release
-// 02/10/2011 SOH Madgwick Optimised for reduced CPU load
-// 11/05/2017 T Flayols Make it a dynamic graph entity
-// 26/03/2019 G Buondonno Converted to double
-//
-//=====================================================================================================
-
-/*
- * 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_madgwickahrs_H__
-#define __sot_torque_control_madgwickahrs_H__
-
-/* --------------------------------------------------------------------- */
-/* --- API ------------------------------------------------------------- */
-/* --------------------------------------------------------------------- */
-
-#if defined (WIN32)
-# if defined (madgwickahrs_EXPORTS)
-# define SOTMADGWICKAHRS_EXPORT __declspec(dllexport)
-# else
-# define SOTMADGWICKAHRS_EXPORT __declspec(dllimport)
-# endif
-#else
-# define SOTMADGWICKAHRS_EXPORT
-#endif
-
-
-/* --------------------------------------------------------------------- */
-/* --- INCLUDE --------------------------------------------------------- */
-/* --------------------------------------------------------------------- */
-
-#include <dynamic-graph/signal-helper.h>
-#include <sot/core/matrix-geometry.hh>
-#include <map>
-#include "boost/assign.hpp"
-
-#define betaDef 0.01 // 2 * proportional g
-
-namespace dynamicgraph {
- namespace sot {
- namespace talos_balance {
-
- /* --------------------------------------------------------------------- */
- /* --- CLASS ----------------------------------------------------------- */
- /* --------------------------------------------------------------------- */
-
- class SOTMADGWICKAHRS_EXPORT MadgwickAHRS
- :public::dynamicgraph::Entity
- {
- typedef MadgwickAHRS EntityClassName;
- DYNAMIC_GRAPH_ENTITY_DECL();
-
- public:
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-
- /* --- CONSTRUCTOR ---- */
- MadgwickAHRS( const std::string & name );
-
- void init(const double& dt);
- void set_beta(const double & beta);
-
- /* --- SIGNALS --- */
- DECLARE_SIGNAL_IN(accelerometer, dynamicgraph::Vector); /// ax ay az in m.s-2
- DECLARE_SIGNAL_IN(gyroscope, dynamicgraph::Vector); /// gx gy gz in rad.s-1
- DECLARE_SIGNAL_OUT(imu_quat, dynamicgraph::Vector); /// Estimated orientation of IMU as a quaternion
-
- protected:
- /* --- COMMANDS --- */
- /* --- ENTITY INHERITANCE --- */
- virtual void display( std::ostream& os ) const;
-
- /* --- METHODS --- */
- double invSqrt(double x);
- void madgwickAHRSupdateIMU(double gx, double gy, double gz, double ax, double ay, double az) ;
- //void madgwickAHRSupdate(double gx, double gy, double gz, double ax, double ay, double az, double mx, double my, double mz);
-
- protected:
- bool m_initSucceeded; /// true if the entity has been successfully initialized
- double m_beta; /// 2 * proportional gain (Kp)
- double m_q0, m_q1, m_q2, m_q3; /// quaternion of sensor frame
- double m_sampleFreq; /// sample frequency in Hz
-
- }; // class MadgwickAHRS
- } // namespace talos_balance
- } // namespace sot
-} // namespace dynamicgraph
-
-#endif // #ifndef __sot_torque_control_madgwickahrs_H__
diff --git a/include/sot/talos_balance/parameter-server.hh b/include/sot/talos_balance/parameter-server.hh
deleted file mode 100644
index 196064d3ea52df765e876af9bb94a53556f0f667..0000000000000000000000000000000000000000
--- a/include/sot/talos_balance/parameter-server.hh
+++ /dev/null
@@ -1,126 +0,0 @@
-/*
- * 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 unsigned int & jointId);
- void setJointLimitsFromId(const unsigned int &jointId, const double &lq, const double &uq);
-
- /// Command related to ForceUtil
- void setForceLimitsFromId(const unsigned int & jointId, const dynamicgraph::Vector &lq, const dynamicgraph::Vector &uq);
- void setForceNameToForceId(const std::string& forceName, const unsigned int & 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:
- RobotUtilShrPtr 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__
diff --git a/include/sot/talos_balance/admittance-controller.hh b/include/sot/talos_balance/simple-admittance-controller.hh
similarity index 87%
rename from include/sot/talos_balance/admittance-controller.hh
rename to include/sot/talos_balance/simple-admittance-controller.hh
index 9c9ddfba2050a78cfc4883f1b4865f143e302f94..7b5d908c197590db18441bb906eee7b26be25147 100644
--- a/include/sot/talos_balance/admittance-controller.hh
+++ b/include/sot/talos_balance/simple-admittance-controller.hh
@@ -23,12 +23,12 @@
#if defined (WIN32)
# if defined (admittance_controller_EXPORTS)
-# define ADMITTANCECONTROLLER_EXPORT __declspec(dllexport)
+# define SIMPLEADMITTANCECONTROLLER_EXPORT __declspec(dllexport)
# else
-# define ADMITTANCECONTROLLER_EXPORT __declspec(dllimport)
+# define SIMPLEADMITTANCECONTROLLER_EXPORT __declspec(dllimport)
# endif
#else
-# define ADMITTANCECONTROLLER_EXPORT
+# define SIMPLEADMITTANCECONTROLLER_EXPORT
#endif
@@ -48,21 +48,21 @@ namespace dynamicgraph {
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
- class ADMITTANCECONTROLLER_EXPORT AdmittanceController
- : public ::dynamicgraph::Entity
+ class SIMPLEADMITTANCECONTROLLER_EXPORT SimpleAdmittanceController
+ : public ::dynamicgraph::Entity
{
- DYNAMIC_GRAPH_ENTITY_DECL();
+ DYNAMIC_GRAPH_ENTITY_DECL();
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-
- /* --- CONSTRUCTOR ---- */
- AdmittanceController( const std::string & name );
-
+
+ /* --- CONSTRUCTOR ---- */
+ SimpleAdmittanceController( const std::string & name );
+
void init(const double & dt, const unsigned & n);
-
+
void setPosition(const dynamicgraph::Vector & position);
-
+
/* --- SIGNALS --- */
DECLARE_SIGNAL_IN(Kp, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(state, dynamicgraph::Vector);
diff --git a/include/sot/talos_balance/base-estimator.hh b/include/sot/talos_balance/talos-base-estimator.hh
similarity index 97%
rename from include/sot/talos_balance/base-estimator.hh
rename to include/sot/talos_balance/talos-base-estimator.hh
index 1d72989c24f41fe6b9eb1db823e90b41f75dac73..e4decdd6db84ae2de8b78b99557076a2b89025d4 100644
--- a/include/sot/talos_balance/base-estimator.hh
+++ b/include/sot/talos_balance/talos-base-estimator.hh
@@ -23,12 +23,12 @@
#if defined (WIN32)
# if defined (base_estimator_EXPORTS)
-# define SOTBASEESTIMATOR_EXPORT __declspec(dllexport)
+# define TALOS_BASE_ESTIMATOR_EXPORT __declspec(dllexport)
# else
-# define SOTBASEESTIMATOR_EXPORT __declspec(dllimport)
+# define TALOS_BASE_ESTIMATOR_EXPORT __declspec(dllimport)
# endif
#else
-# define SOTBASEESTIMATOR_EXPORT
+# define TALOS_BASE_ESTIMATOR_EXPORT
#endif
@@ -82,10 +82,10 @@ namespace dynamicgraph {
/** Avoids singularity while taking the mean of euler angles**/
double wEulerMean(double a1, double a2, double w1, double w2);
- class SOTBASEESTIMATOR_EXPORT BaseEstimator
+ class TALOS_BASE_ESTIMATOR_EXPORT TalosBaseEstimator
:public::dynamicgraph::Entity
{
- typedef BaseEstimator EntityClassName;
+ typedef TalosBaseEstimator EntityClassName;
typedef pinocchio::SE3 SE3;
typedef Eigen::Vector2d Vector2;
typedef Eigen::Vector3d Vector3;
@@ -101,7 +101,7 @@ namespace dynamicgraph {
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/* --- CONSTRUCTOR ---- */
- BaseEstimator( const std::string & name );
+ TalosBaseEstimator( const std::string & name );
void init(const double & dt, const std::string& urdfFile);
@@ -249,7 +249,7 @@ namespace dynamicgraph {
Vector3 m_last_DCvel;
Vector3 m_last_DCacc;
- }; // class BaseEstimator
+ }; // class TalosBaseEstimator
} // namespace talos_balance
} // namespace sot
diff --git a/include/sot/talos_balance/control-manager.hh b/include/sot/talos_balance/talos-control-manager.hh
similarity index 94%
rename from include/sot/talos_balance/control-manager.hh
rename to include/sot/talos_balance/talos-control-manager.hh
index 74e619db5ee0aa08b715ae78d168a5e03caf2c6b..697bb7c873d29511837fc565cd950bdd0dcd6840 100644
--- a/include/sot/talos_balance/control-manager.hh
+++ b/include/sot/talos_balance/talos-control-manager.hh
@@ -23,12 +23,12 @@
#if defined (WIN32)
# if defined (__sot_torque_control_control_manager_H__)
-# define SOTCONTROLMANAGER_EXPORT __declspec(dllexport)
+# define TALOS_CONTROL_MANAGER_EXPORT __declspec(dllexport)
# else
-# define SOTCONTROLMANAGER_EXPORT __declspec(dllimport)
+# define TALOS_CONTROL_MANAGER_EXPORT __declspec(dllimport)
# endif
#else
-# define SOTCONTROLMANAGER_EXPORT
+# define TALOS_CONTROL_MANAGER_EXPORT
#endif
@@ -70,16 +70,16 @@ namespace dynamicgraph {
return os;
}
- class SOTCONTROLMANAGER_EXPORT ControlManager
+ class TALOS_CONTROL_MANAGER_EXPORT TalosControlManager
:public::dynamicgraph::Entity
{
typedef Eigen::VectorXd::Index Index;
- typedef ControlManager EntityClassName;
+ typedef TalosControlManager EntityClassName;
DYNAMIC_GRAPH_ENTITY_DECL();
public:
/* --- CONSTRUCTOR ---- */
- ControlManager( const std::string & name);
+ TalosControlManager( const std::string & name);
/// Initialize
/// @param dt: control interval
@@ -142,7 +142,7 @@ namespace dynamicgraph {
//bool isJointInRange(unsigned int id, double q);
void updateJointCtrlModesOutputSignal();
- }; // class ControlManager
+ }; // class TalosControlManager
} // namespace talos_balance
} // namespace sot
diff --git a/include/sot/talos_balance/utils/causal-filter.hh b/include/sot/talos_balance/utils/causal-filter.hh
deleted file mode 100644
index d740ea2600c7be26f3d15ace6cd2bf60cb54bd85..0000000000000000000000000000000000000000
--- a/include/sot/talos_balance/utils/causal-filter.hh
+++ /dev/null
@@ -1,54 +0,0 @@
-/*
- * 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 ×tep,
- 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;
- Eigen::Index m_filter_order_m;
- Eigen::Index 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
diff --git a/include/sot/talos_balance/utils/stop-watch.hh b/include/sot/talos_balance/utils/stop-watch.hh
deleted file mode 100644
index 940932b0e74891bcd16c04192a402d05755e963a..0000000000000000000000000000000000000000
--- a/include/sot/talos_balance/utils/stop-watch.hh
+++ /dev/null
@@ -1,272 +0,0 @@
-/*
-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
diff --git a/python/sot_talos_balance/create_entities_utils.py b/python/sot_talos_balance/create_entities_utils.py
index c609840f209fbdb6fdf1ca9f0d952209fe7220ba..8919fa17c78bc18184d66564f380549ec5fdf9e8 100644
--- a/python/sot_talos_balance/create_entities_utils.py
+++ b/python/sot_talos_balance/create_entities_utils.py
@@ -1,11 +1,10 @@
-from sot_talos_balance.control_manager import ControlManager
+from sot_talos_balance.talos_control_manager import TalosControlManager
from sot_talos_balance.example import Example
-from sot_talos_balance.parameter_server import ParameterServer
+from dynamic_graph.sot.core.parameter_server import ParameterServer
from dynamic_graph.tracer_real_time import TracerRealTime
from time import sleep
-from sot_talos_balance.base_estimator import BaseEstimator
-from sot_talos_balance.madgwickahrs import MadgwickAHRS
-from sot_talos_balance.imu_offset_compensation import ImuOffsetCompensation
+from sot_talos_balance.talos_base_estimator import TalosBaseEstimator
+from dynamic_graph.sot.core.madgwickahrs import MadgwickAHRS
from sot_talos_balance.dcm_estimator import DcmEstimator
from sot_talos_balance.ft_calibration import FtCalibration
from sot_talos_balance.ft_wrist_calibration import FtWristCalibration
@@ -25,7 +24,7 @@ from sot_talos_balance.nd_trajectory_generator import NdTrajectoryGenerator
from sot_talos_balance.simple_pid import SimplePID
from sot_talos_balance.simple_pidd import SimplePIDD
from sot_talos_balance.joint_position_controller import JointPositionController
-from sot_talos_balance.admittance_controller import AdmittanceController
+from sot_talos_balance.simple_admittance_controller import SimpleAdmittanceController
from sot_talos_balance.admittance_controller_end_effector import AdmittanceControllerEndEffector
from sot_talos_balance.dummy_dcm_estimator import DummyDcmEstimator
from sot_talos_balance.com_admittance_controller import ComAdmittanceController
@@ -200,12 +199,6 @@ def create_joint_admittance_controller(joint, Kp, dt, robot, filter=False):
return controller
-def create_imu_offset_compensation(robot, dt):
- imu_offset_compensation = ImuOffsetCompensation('imu_offset_comp')
- imu_offset_compensation.init(dt)
- plug(robot.device.accelerometer, imu_offset_compensation.accelerometer_in)
- plug(robot.device.gyrometer, imu_offset_compensation.gyrometer_in)
- return imu_offset_compensation
def create_device_filters(robot, dt):
@@ -232,10 +225,7 @@ def create_device_filters(robot, dt):
plug(robot.device.ptorque, filters.torque_filter.x)
plug(robot.vselec.sout, filters.vel_filter.x)
- # switch following lines if willing to use imu offset compensation
- #~ plug(robot.imu_offset_compensation.accelerometer_out, filters.acc_filter.x);
plug(robot.device.accelerometer, filters.acc_filter.x)
- #~ plug(robot.imu_offset_compensation.gyrometer_out, filters.gyro_filter.x);
plug(robot.device.gyrometer, filters.gyro_filter.x)
return filters
@@ -249,7 +239,7 @@ def create_be_filters(robot, dt):
def create_ctrl_manager(conf, dt, robot_name='robot'):
- ctrl_manager = ControlManager("ctrl_man")
+ ctrl_manager = TalosControlManager("ctrl_man")
ctrl_manager.init(dt, robot_name)
ctrl_manager.u_max.value = conf.NJ*(conf.CTRL_MAX,)
# Init should be called before addCtrlMode
@@ -258,7 +248,7 @@ def create_ctrl_manager(conf, dt, robot_name='robot'):
def create_base_estimator(robot, dt, conf, robot_name="robot"):
- base_estimator = BaseEstimator('base_estimator')
+ base_estimator = TalosBaseEstimator('base_estimator')
base_estimator.init(dt, robot_name)
# device.state, device.joint_angles or device.motor_angles ?
plug(robot.device.joint_angles, base_estimator.joint_positions)
diff --git a/python/sot_talos_balance/utils/filter_utils.py b/python/sot_talos_balance/utils/filter_utils.py
index f32387442d4bd5544744c19d6150c56f750b7bdb..e295b4013b217daeaa0a3a5cfcc390995ae817dd 100644
--- a/python/sot_talos_balance/utils/filter_utils.py
+++ b/python/sot_talos_balance/utils/filter_utils.py
@@ -1,4 +1,4 @@
-from sot_talos_balance.filter_differentiator import FilterDifferentiator
+from dynamic_graph.sot.core.filter_differentiator import FilterDifferentiator
import numpy as np
def create_chebi1_checby2_series_filter(name, dt, size):
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 8a76c01f186488754ef520a37f1d79bde693c11e..0c71ee73813899f3c5b84a01a3374c1c70d0340d 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -52,16 +52,12 @@ SET(plugins
dcm-controller
dummy-dcm-estimator
com-admittance-controller
- admittance-controller
+ simple-admittance-controller
admittance-controller-end-effector
joint-position-controller
nd-trajectory-generator
- base-estimator
- filter-differentiator
- madgwickahrs
- control-manager
- imu_offset_compensation
- parameter-server
+ talos-base-estimator
+ talos-control-manager
dcm-estimator
qualisys-client
ft-calibration
@@ -98,7 +94,7 @@ FOREACH(plugin ${plugins})
ADD_DEPENDENCIES(${LIBRARY_NAME} ${SOTTALOSBALANCE_LIB_NAME})
-
+
IF(ADDITIONAL_${LIBRARY_NAME}_LIBS)
ADD_DEPENDENCIES(${LIBRARY_NAME} ${ADDITIONAL_${LIBRARY_NAME}_LIBS})
TARGET_LINK_LIBRARIES(${LIBRARY_NAME} ${ADDITIONAL_${LIBRARY_NAME}_LIBS})
@@ -128,8 +124,10 @@ FOREACH(plugin ${plugins})
)
ENDIF(BUILD_PYTHON_INTERFACE)
# Install plugins
- INSTALL(TARGETS ${LIBRARY_NAME}
- DESTINATION ${CMAKE_INSTALL_LIBDIR}/plugin)
+ IF (NOT INSTALL_PYTHON_INTERFACE_ONLY)
+ INSTALL(TARGETS ${LIBRARY_NAME}
+ DESTINATION ${CMAKE_INSTALL_LIBDIR}/plugin)
+ ENDIF (NOT INSTALL_PYTHON_INTERFACE_ONLY)
ENDFOREACH(plugin)
# Bindings Python
diff --git a/src/admittance-controller-end-effector.cpp b/src/admittance-controller-end-effector.cpp
index 4a8c42a66e1d9ecc049612a6630333cadccb2f16..572608e59fb9452fdf381b2f0b88601d92af7e3d 100644
--- a/src/admittance-controller-end-effector.cpp
+++ b/src/admittance-controller-end-effector.cpp
@@ -12,7 +12,7 @@
#include <sot/core/debug.hh>
#include <dynamic-graph/factory.h>
#include <dynamic-graph/all-commands.h>
-#include "sot/talos_balance/utils/stop-watch.hh"
+#include <sot/core/stop-watch.hh>
namespace dynamicgraph
{
diff --git a/src/com-admittance-controller.cpp b/src/com-admittance-controller.cpp
index d335eab12624568268598c740e9fae4d67bf6c65..48b33506a743b6929f0256ea3a0fc673f121c597 100644
--- a/src/com-admittance-controller.cpp
+++ b/src/com-admittance-controller.cpp
@@ -21,7 +21,7 @@
#include <dynamic-graph/command-bind.h>
#include <dynamic-graph/all-commands.h>
-#include "sot/talos_balance/utils/stop-watch.hh"
+#include <sot/core/stop-watch.hh>
namespace dynamicgraph
{
diff --git a/src/dcm-com-controller.cpp b/src/dcm-com-controller.cpp
index bb56c47f8cc24c2574e4829b07e985ab7cb803cd..08841b415054bffebc66a048b245e98a9112a510 100644
--- a/src/dcm-com-controller.cpp
+++ b/src/dcm-com-controller.cpp
@@ -21,7 +21,7 @@
#include <dynamic-graph/command-bind.h>
#include <dynamic-graph/all-commands.h>
-#include "sot/talos_balance/utils/stop-watch.hh"
+#include <sot/core/stop-watch.hh>
namespace dynamicgraph
{
diff --git a/src/dcm-controller.cpp b/src/dcm-controller.cpp
index 7ab50976168e044acb75c28223dc4ba92d83c866..94b9eb49aec7f31325be4da3a11ee4ae2d3851ba 100644
--- a/src/dcm-controller.cpp
+++ b/src/dcm-controller.cpp
@@ -21,7 +21,7 @@
#include <dynamic-graph/command-bind.h>
#include <dynamic-graph/all-commands.h>
-#include "sot/talos_balance/utils/stop-watch.hh"
+#include <sot/core/stop-watch.hh>
namespace dynamicgraph
{
diff --git a/src/dcm-estimator.cpp b/src/dcm-estimator.cpp
index 4821d1cd49514d0f7a3c4fde9a75ec2fd9448578..010136a0215588afbedc5dc5b2d455b934eafb36 100644
--- a/src/dcm-estimator.cpp
+++ b/src/dcm-estimator.cpp
@@ -11,7 +11,7 @@
#include <sot/core/debug.hh>
#include <dynamic-graph/all-commands.h>
#include <dynamic-graph/factory.h>
-#include <sot/talos_balance/utils/stop-watch.hh>
+#include <sot/core/stop-watch.hh>
#include "pinocchio/algorithm/frames.hpp"
#include "pinocchio/algorithm/center-of-mass.hpp"
diff --git a/src/distribute-wrench.cpp b/src/distribute-wrench.cpp
index cfbb8b23fc77ec1140cae091a131214281999ec8..62afb896cc09c3c9806a19c7021a330a490268df 100644
--- a/src/distribute-wrench.cpp
+++ b/src/distribute-wrench.cpp
@@ -23,7 +23,7 @@
#include <dynamic-graph/command-bind.h>
#include "sot/talos_balance/utils/commands-helper.hh"
-#include "sot/talos_balance/utils/stop-watch.hh"
+#include "sot/core/stop-watch.hh"
#include <pinocchio/parsers/urdf.hpp>
#include <pinocchio/algorithm/kinematics.hpp>
diff --git a/src/dummy-dcm-estimator.cpp b/src/dummy-dcm-estimator.cpp
index 8bc2110211a9dbbe303c000c2cb4fbf494ae19b9..aaedb0734f7abacb3d7ec73bea9948c9c56e2f9e 100644
--- a/src/dummy-dcm-estimator.cpp
+++ b/src/dummy-dcm-estimator.cpp
@@ -21,7 +21,7 @@
#include <dynamic-graph/command-bind.h>
#include <dynamic-graph/all-commands.h>
-#include "sot/talos_balance/utils/stop-watch.hh"
+#include <sot/core/stop-watch.hh>
namespace dynamicgraph
{
diff --git a/src/dummy-walking-pattern-generator.cpp b/src/dummy-walking-pattern-generator.cpp
index 1e2c72a917a7cc6ee8aed90d553db2d7ddca4559..34a8681ac2d5039e6c6ff438eaa82489113f824a 100644
--- a/src/dummy-walking-pattern-generator.cpp
+++ b/src/dummy-walking-pattern-generator.cpp
@@ -21,7 +21,7 @@
#include <dynamic-graph/command-bind.h>
#include <dynamic-graph/all-commands.h>
-#include "sot/talos_balance/utils/stop-watch.hh"
+#include <sot/core/stop-watch.hh>
namespace dynamicgraph
{
diff --git a/src/euler-to-quat.cpp b/src/euler-to-quat.cpp
index 5d5361f16fc1651156898395486562813fe9caa4..54918809203a20472a28002a58654c5180973c04 100644
--- a/src/euler-to-quat.cpp
+++ b/src/euler-to-quat.cpp
@@ -19,7 +19,7 @@
#include <sot/core/debug.hh>
#include <dynamic-graph/factory.h>
#include <dynamic-graph/all-commands.h>
-#include "sot/talos_balance/utils/stop-watch.hh"
+#include <sot/core/stop-watch.hh>
#include <Eigen/Core>
diff --git a/src/example.cpp b/src/example.cpp
index 94b380f0a29930238d3b8373044ed4e4b8483284..708a1a86c26441fc89d7d5b1d2bfe6d6ebe5ea8a 100644
--- a/src/example.cpp
+++ b/src/example.cpp
@@ -19,7 +19,7 @@
#include <sot/core/debug.hh>
#include <dynamic-graph/factory.h>
#include <dynamic-graph/all-commands.h>
-#include "sot/talos_balance/utils/stop-watch.hh"
+#include <sot/core/stop-watch.hh>
namespace dynamicgraph
diff --git a/src/filter-differentiator.cpp b/src/filter-differentiator.cpp
deleted file mode 100644
index c98891538863b30d94b5b3f270b9c3b504f67e3b..0000000000000000000000000000000000000000
--- a/src/filter-differentiator.cpp
+++ /dev/null
@@ -1,189 +0,0 @@
-/*
- * Copyright 2017-, Rohan Budhiraja 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/>.
- */
-
-#define LOGFILE "/tmp/fd_log.dat"
-
-#define LOG(x) { std::ofstream LogFile; \
- LogFile.open(LOGFILE,std::ofstream::app); \
- LogFile << x << std::endl; \
- LogFile.close();}
-
-
-#include <sot/talos_balance/filter-differentiator.hh>
-#include <sot/core/debug.hh>
-#include <dynamic-graph/factory.h>
-#include <dynamic-graph/all-commands.h>
-//#include <sot/torque_control/motor-model.hh>
-#include <Eigen/Dense>
-
-namespace dynamicgraph
-{
- namespace sot
- {
- namespace talos_balance
- {
-
-#define ALL_INPUT_SIGNALS m_xSIN
-
-#define ALL_OUTPUT_SIGNALS m_x_filteredSOUT << m_dxSOUT << m_ddxSOUT
-
- namespace dynamicgraph = ::dynamicgraph;
- using namespace dynamicgraph;
- using namespace dynamicgraph::command;
- using namespace Eigen;
-
- /// Define EntityClassName here rather than in the header file
- /// so that it can be used by the macros DEFINE_SIGNAL_**_FUNCTION.
- typedef FilterDifferentiator EntityClassName;
-
- /* --- DG FACTORY ------------------------------------------------------- */
- DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(FilterDifferentiator,"FilterDifferentiator");
-
- /* --- CONSTRUCTION ----------------------------------------------------- */
- /* --- CONSTRUCTION ----------------------------------------------------- */
- /* --- CONSTRUCTION ----------------------------------------------------- */
- FilterDifferentiator::
- FilterDifferentiator( const std::string & name )
- : Entity(name),
- CONSTRUCT_SIGNAL_IN(x, dynamicgraph::Vector)
- ,CONSTRUCT_SIGNAL_OUT(x_filtered, dynamicgraph::Vector, m_x_dx_ddxSINNER)
- ,CONSTRUCT_SIGNAL_OUT(dx, dynamicgraph::Vector, m_x_dx_ddxSINNER)
- ,CONSTRUCT_SIGNAL_OUT(ddx, dynamicgraph::Vector, m_x_dx_ddxSINNER)
- ,CONSTRUCT_SIGNAL_INNER(x_dx_ddx, dynamicgraph::Vector, m_xSIN)
- {
- Entity::signalRegistration( ALL_INPUT_SIGNALS << ALL_OUTPUT_SIGNALS);
-
- /* Commands. */
- addCommand("getTimestep",
- makeDirectGetter(*this,&m_dt,
- docDirectGetter("Control timestep [s ]","double")));
- addCommand("getSize",
- makeDirectGetter(*this,&m_x_size,
- docDirectGetter("Size of the x signal","int")));
- addCommand("init", makeCommandVoid4(*this, &FilterDifferentiator::init,
- docCommandVoid4("Initialize the filter.",
- "Control timestep [s].",
- "Size of the input signal x",
- "Numerator of the filter",
- "Denominator of the filter")));
- addCommand("switch_filter",
- makeCommandVoid2(*this, &FilterDifferentiator::switch_filter,
- docCommandVoid2("Switch Filter.",
- "Numerator of the filter",
- "Denominator of the filter")));
-
- }
-
-
- /* --- COMMANDS ---------------------------------------------------------- */
- /* --- COMMANDS ---------------------------------------------------------- */
- /* --- COMMANDS ---------------------------------------------------------- */
- void FilterDifferentiator::init(const double ×tep,
- const int& xSize,
- const Eigen::VectorXd& filter_numerator,
- const Eigen::VectorXd& filter_denominator)
- {
- m_x_size = xSize;
- m_dt = timestep;
- m_filter = new CausalFilter(timestep, xSize,
- filter_numerator, filter_denominator);
-
- LOG("Filtering started with "<<
- "Numerator "<< filter_numerator<<std::endl<<
- "Denominator"<<filter_denominator<<std::endl);
- return;
- }
-
- void FilterDifferentiator::switch_filter(const Eigen::VectorXd& filter_numerator,
- const Eigen::VectorXd& filter_denominator)
- {
- LOG("Filter switched with "<<
- "Numerator "<< filter_numerator<<std::endl<<
- "Denominator"<<filter_denominator<<std::endl<<
- "at time"<<m_xSIN.getTime());
- m_filter->switch_filter(filter_numerator, filter_denominator);
- }
-
-
- /* --- SIGNALS ---------------------------------------------------------- */
- /* --- SIGNALS ---------------------------------------------------------- */
- /* --- SIGNALS ---------------------------------------------------------- */
-
- DEFINE_SIGNAL_INNER_FUNCTION(x_dx_ddx, dynamicgraph::Vector)
- {
- sotDEBUG(15)<<"Compute x_dx inner signal "<<iter<<std::endl;
- if(s.size()!=3*m_x_size)
- s.resize(3*m_x_size);
- // read encoders
- const dynamicgraph::Vector& base_x = m_xSIN(iter);
- assert(base_x.size() == m_x_size);
- m_filter->get_x_dx_ddx(base_x, s);
- return s;
- }
-
-
- /// ************************************************************************* ///
- /// The following signals depend only on other inner signals, so they
- /// just need to copy the interested part of the inner signal they depend on.
- /// ************************************************************************* ///
-
-
- DEFINE_SIGNAL_OUT_FUNCTION(x_filtered, dynamicgraph::Vector)
- {
- sotDEBUG(15)<<"Compute x_filtered output signal "<<iter<<std::endl;
-
- const dynamicgraph::Vector &x_dx_ddx = m_x_dx_ddxSINNER(iter);
- if(s.size()!=m_x_size)
- s.resize(m_x_size);
- s = x_dx_ddx.head(m_x_size);
- return s;
- }
-
- DEFINE_SIGNAL_OUT_FUNCTION(dx, dynamicgraph::Vector)
- {
- sotDEBUG(15)<<"Compute dx output signal "<<iter<<std::endl;
-
- const dynamicgraph::Vector &x_dx_ddx = m_x_dx_ddxSINNER(iter);
- if(s.size()!=m_x_size)
- s.resize(m_x_size);
- s = x_dx_ddx.segment(m_x_size, m_x_size);
- return s;
- }
-
- DEFINE_SIGNAL_OUT_FUNCTION(ddx, dynamicgraph::Vector)
- {
- sotDEBUG(15)<<"Compute ddx output signal "<<iter<<std::endl;
-
- const dynamicgraph::Vector &x_dx_ddx = m_x_dx_ddxSINNER(iter);
- if(s.size()!=m_x_size)
- s.resize(m_x_size);
- s = x_dx_ddx.tail(m_x_size);
- return s;
- }
-
- void FilterDifferentiator::display( std::ostream& os ) const
- {
- os << "FilterDifferentiator "<<getName()<<":\n";
- try
- {
- getProfiler().report_all(3, os);
- }
- catch (ExceptionSignal e) {}
- }
-
- } // namespace talos_balance
- } // namespace sot
-} // namespace dynamicgraph
diff --git a/src/ft-calibration.cpp b/src/ft-calibration.cpp
index 5ca83ac24c381c261951836c8ed3d648c6577ee3..0bac338b93393f436a511dada752fb12a2663e30 100644
--- a/src/ft-calibration.cpp
+++ b/src/ft-calibration.cpp
@@ -12,7 +12,7 @@
#include <dynamic-graph/all-commands.h>
-#include <sot/talos_balance/utils/stop-watch.hh>
+#include <sot/core/stop-watch.hh>
#include <sot/talos_balance/utils/statistics.hh>
#define CALIB_ITER_TIME 1000 //Iteration needed for sampling and averaging the FT sensors while calibrating
diff --git a/src/ft-wrist-calibration.cpp b/src/ft-wrist-calibration.cpp
index 7245754a11ef91d4c53f15cc7cc43f0759496a4f..1fee19802bccc3799100281ef5b9efddfeb15ea9 100644
--- a/src/ft-wrist-calibration.cpp
+++ b/src/ft-wrist-calibration.cpp
@@ -13,7 +13,7 @@
#include <dynamic-graph/all-commands.h>
-#include <sot/talos_balance/utils/stop-watch.hh>
+#include <sot/core/stop-watch.hh>
#include <sot/talos_balance/utils/statistics.hh>
#define CALIB_ITER_TIME 1000 //Iteration needed for sampling and averaging the FT sensors while calibrating
diff --git a/src/imu_offset_compensation.cpp b/src/imu_offset_compensation.cpp
deleted file mode 100644
index fe1f799d42682268cfac1a24040ad2d9566859ea..0000000000000000000000000000000000000000
--- a/src/imu_offset_compensation.cpp
+++ /dev/null
@@ -1,235 +0,0 @@
-#include <sot/talos_balance/imu_offset_compensation.hh>
-#include <sot/core/debug.hh>
-#include <dynamic-graph/factory.h>
-
-#include <dynamic-graph/all-commands.h>
-#include <sot/talos_balance/utils/stop-watch.hh>
-
-
-namespace dynamicgraph
-{
- namespace sot
- {
- namespace talos_balance
- {
- namespace dg = ::dynamicgraph;
- using namespace dg;
- using namespace dg::command;
- using namespace std;
-
-#define CALIBRATION_FILE_NAME "/opt/imu_calib.txt"
-
-#define PROFILE_IMU_OFFSET_COMPENSATION_COMPUTATION "ImuOffsetCompensation computation"
-
-#define INPUT_SIGNALS m_accelerometer_inSIN << m_gyrometer_inSIN
-#define OUTPUT_SIGNALS m_accelerometer_outSOUT << m_gyrometer_outSOUT
-
- /// Define EntityClassName here rather than in the header file
- /// so that it can be used by the macros DEFINE_SIGNAL_**_FUNCTION.
- typedef ImuOffsetCompensation EntityClassName;
-
- /* --- DG FACTORY ---------------------------------------------------- */
- DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(ImuOffsetCompensation,
- "ImuOffsetCompensation");
-
- /* ------------------------------------------------------------------- */
- /* --- CONSTRUCTION -------------------------------------------------- */
- /* ------------------------------------------------------------------- */
- ImuOffsetCompensation::ImuOffsetCompensation(const std::string& name)
- : Entity(name)
- ,CONSTRUCT_SIGNAL_IN( accelerometer_in, dynamicgraph::Vector)
- ,CONSTRUCT_SIGNAL_IN( gyrometer_in, dynamicgraph::Vector)
- ,CONSTRUCT_SIGNAL_OUT(accelerometer_out, dynamicgraph::Vector, m_accelerometer_inSIN)
- ,CONSTRUCT_SIGNAL_OUT(gyrometer_out, dynamicgraph::Vector, m_gyrometer_inSIN)
- ,m_initSucceeded(false)
- ,m_dt(0.001)
- ,m_update_cycles_left(0)
- ,m_update_cycles(0)
- ,m_a_gyro_DC_blocker(1.0)
- {
- Entity::signalRegistration( INPUT_SIGNALS << OUTPUT_SIGNALS );
-
- m_gyro_offset.setZero();
- m_acc_offset.setZero();
- m_gyro_sum.setZero();
- m_acc_sum.setZero();
-
- /* Commands. */
- addCommand("init",
- makeCommandVoid1(*this, &ImuOffsetCompensation::init,
- docCommandVoid1("Initialize the entity.",
- "Timestep in seconds (double)")));
- addCommand("update_offset",
- makeCommandVoid1(*this, &ImuOffsetCompensation::update_offset,
- docCommandVoid1("Update the IMU offsets.",
- "Duration of the update phase in seconds (double)")));
- addCommand("setGyroDCBlockerParameter",
- makeCommandVoid1(*this, &ImuOffsetCompensation::setGyroDCBlockerParameter,
- docCommandVoid1("Set DC Blocker filter parameter.",
- "alpha (double)")));
-
-
- }
-
- /* ------------------------------------------------------------------- */
- /* --- COMMANDS ------------------------------------------------------ */
- /* ------------------------------------------------------------------- */
-
- void ImuOffsetCompensation::init(const double& dt)
- {
- if(dt<=0.0)
- return SEND_MSG("Timestep must be positive", MSG_TYPE_ERROR);
- m_dt = dt;
- m_initSucceeded = true;
-
- // try to read IMU calibration data from file
- std::ifstream infile;
- infile.open(CALIBRATION_FILE_NAME, std::ios::in);
- if(!infile.is_open())
- return SEND_MSG("Error trying to read calibration results from file "+toString(CALIBRATION_FILE_NAME), MSG_TYPE_ERROR);
-
- double z=0;
- int i=0;
- while(infile>>z)
- {
- m_gyro_offset(i) = z;
- i++;
- if(i==3)
- break;
- }
- if(i!=3)
- {
- m_gyro_offset.setZero();
- return SEND_MSG("Error trying to read gyro offset from file "+toString(CALIBRATION_FILE_NAME)+". Not enough values: "+toString(i), MSG_TYPE_ERROR);
- }
-
- i=0;
- while(infile>>z)
- {
- m_acc_offset(i) = z;
- i++;
- if(i==3)
- break;
- }
- if(i!=3)
- {
- m_gyro_offset.setZero();
- m_acc_offset.setZero();
- return SEND_MSG("Error trying to read acc offset from file "+toString(CALIBRATION_FILE_NAME)+". Not enough values: "+toString(i), MSG_TYPE_ERROR);
- }
-
- SEND_MSG("Offset read finished:\n* acc offset: "+toString(m_acc_offset.transpose())+
- "\n* gyro offset: "+toString(m_gyro_offset.transpose()), MSG_TYPE_INFO);
- }
-
- void ImuOffsetCompensation::setGyroDCBlockerParameter(const double & alpha)
- {
- if(alpha>1.0 || alpha<=0.0)
- return SEND_MSG("GyroDCBlockerParameter must be > 0 and <= 1", MSG_TYPE_ERROR);
- m_a_gyro_DC_blocker = alpha;
- }
-
- void ImuOffsetCompensation::update_offset(const double& duration)
- {
- if(duration<m_dt)
- return SEND_MSG("Duration must be greater than the time step", MSG_TYPE_ERROR);
- m_update_cycles = int(duration/m_dt);
- m_update_cycles_left = m_update_cycles;
- }
-
- void ImuOffsetCompensation::update_offset_impl(int iter)
- {
- const dynamicgraph::Vector& accelerometer = m_accelerometer_inSIN(iter);
- const dynamicgraph::Vector& gyrometer = m_gyrometer_inSIN(iter);
- m_acc_sum += accelerometer;
- m_gyro_sum += gyrometer;
-
- m_update_cycles_left--;
- if(m_update_cycles_left==0)
- {
- Vector3 g, new_acc_offset, new_gyro_offset;
- g<<0.0, 0.0, 9.81;
- new_acc_offset = (m_acc_sum /m_update_cycles) - g;
- new_gyro_offset = m_gyro_sum/m_update_cycles;
- m_acc_sum.setZero();
- m_gyro_sum.setZero();
- SEND_MSG("Offset computation finished:"+
- ("\n* old acc offset: "+toString(m_acc_offset.transpose()))+
- "\n* new acc offset: "+toString(new_acc_offset.transpose())+
- "\n* old gyro offset: "+toString(m_gyro_offset.transpose())+
- "\n* new gyro offset: "+toString(new_gyro_offset.transpose()), MSG_TYPE_INFO);
- m_acc_offset = new_acc_offset;
- m_gyro_offset = new_gyro_offset;
-
- // save to text file
- ofstream aof(CALIBRATION_FILE_NAME);
- if(!aof.is_open())
- return SEND_MSG("Error trying to save calibration results on file "+toString(CALIBRATION_FILE_NAME), MSG_TYPE_ERROR);
-
- for(unsigned long int i=0;i<3;i++)
- aof << m_gyro_offset[i] << " " ;
- aof << std::endl;
- for(unsigned long int i=0;i<3;i++)
- aof << m_acc_offset[i] << " " ;
- aof << std::endl;
- aof.close();
- SEND_MSG("IMU calibration data saved to file "+toString(CALIBRATION_FILE_NAME), MSG_TYPE_INFO);
- }
- }
-
- /* ------------------------------------------------------------------- */
- /* --- SIGNALS ------------------------------------------------------- */
- /* ------------------------------------------------------------------- */
-
- DEFINE_SIGNAL_OUT_FUNCTION(accelerometer_out, dynamicgraph::Vector)
- {
- if(!m_initSucceeded)
- {
- SEND_WARNING_STREAM_MSG("Cannot compute signal accelerometer before initialization!");
- return s;
- }
-
- if(m_update_cycles_left>0)
- update_offset_impl(iter);
-
- const dynamicgraph::Vector& accelerometer = m_accelerometer_inSIN(iter);
- if(s.size()!=3)
- s.resize(3);
- s = accelerometer - m_acc_offset;
- return s;
- }
-
- DEFINE_SIGNAL_OUT_FUNCTION(gyrometer_out, dynamicgraph::Vector)
- {
- if(!m_initSucceeded)
- {
- SEND_WARNING_STREAM_MSG("Cannot compute signal gyrometer before initialization!");
- return s;
- }
- const dynamicgraph::Vector& gyrometer = m_gyrometer_inSIN(iter);
- if(s.size()!=3)
- s.resize(3);
- //estimate bias online with the assumption that average angular velocity should be zero.
- if (m_a_gyro_DC_blocker !=1.0)
- m_gyro_offset = m_gyro_offset*m_a_gyro_DC_blocker + (1.-m_a_gyro_DC_blocker)*gyrometer;
- s = gyrometer - m_gyro_offset;
- return s;
- }
-
-
- /* ------------------------------------------------------------------- */
- /* --- ENTITY -------------------------------------------------------- */
- /* ------------------------------------------------------------------- */
-
- void ImuOffsetCompensation::display(std::ostream& os) const
- {
- os << "ImuOffsetCompensation "<<getName();
- try
- {
- getProfiler().report_all(3, os);
- }
- catch (ExceptionSignal e) {}
- }
- } // namespace talos_balance
- } // namespace sot
-} // namespace dynamicgraph
diff --git a/src/joint-position-controller.cpp b/src/joint-position-controller.cpp
index 72c0e6b788b75883f26b3bd58cbb78f51b3d578a..2a81e0be3e2fdbaa7f384f53aeac8a42d6e39d3d 100644
--- a/src/joint-position-controller.cpp
+++ b/src/joint-position-controller.cpp
@@ -21,7 +21,7 @@
#include <dynamic-graph/command-bind.h>
#include <dynamic-graph/all-commands.h>
-#include "sot/talos_balance/utils/stop-watch.hh"
+#include <sot/core/stop-watch.hh>
namespace dynamicgraph
{
diff --git a/src/madgwickahrs.cpp b/src/madgwickahrs.cpp
deleted file mode 100644
index 2b9f00b5f0ec7ceb44401f54ad56b8cfed6aa0f8..0000000000000000000000000000000000000000
--- a/src/madgwickahrs.cpp
+++ /dev/null
@@ -1,245 +0,0 @@
-//=====================================================================================================
-//
-// Implementation of Madgwick's IMU and AHRS algorithms.
-// See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
-//
-// Date Author Notes
-// 29/09/2011 SOH Madgwick Initial release
-// 02/10/2011 SOH Madgwick Optimised for reduced CPU load
-// 11/05/2017 T Flayols Make it a dynamic-graph entity
-//
-//=====================================================================================================
-
-
-
-#include <sot/talos_balance/madgwickahrs.hh>
-#include <sot/core/debug.hh>
-#include <dynamic-graph/factory.h>
-
-#include <dynamic-graph/all-commands.h>
-#include <sot/talos_balance/utils/stop-watch.hh>
-
-
-namespace dynamicgraph
-{
- namespace sot
- {
- namespace talos_balance
- {
- namespace dg = ::dynamicgraph;
- using namespace dg;
- using namespace dg::command;
- using namespace std;
-
- typedef Vector6d Vector6;
-
-#define PROFILE_MADGWICKAHRS_COMPUTATION "MadgwickAHRS computation"
-
-#define INPUT_SIGNALS m_accelerometerSIN << m_gyroscopeSIN
-#define OUTPUT_SIGNALS m_imu_quatSOUT
-
- /// Define EntityClassName here rather than in the header file
- /// so that it can be used by the macros DEFINE_SIGNAL_**_FUNCTION.
- typedef MadgwickAHRS EntityClassName;
-
- /* --- DG FACTORY ---------------------------------------------------- */
- DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(MadgwickAHRS,
- "MadgwickAHRS");
-
- /* ------------------------------------------------------------------- */
- /* --- CONSTRUCTION -------------------------------------------------- */
- /* ------------------------------------------------------------------- */
- MadgwickAHRS::MadgwickAHRS(const std::string& name)
- : Entity(name)
- ,CONSTRUCT_SIGNAL_IN( accelerometer, dynamicgraph::Vector)
- ,CONSTRUCT_SIGNAL_IN( gyroscope, dynamicgraph::Vector)
- ,CONSTRUCT_SIGNAL_OUT(imu_quat, dynamicgraph::Vector, m_gyroscopeSIN <<
- m_accelerometerSIN)
- ,m_initSucceeded(false)
- ,m_beta(betaDef)
- ,m_q0(1.0)
- ,m_q1(0.0)
- ,m_q2(0.0)
- ,m_q3(0.0)
- ,m_sampleFreq(512.0)
- {
- Entity::signalRegistration( INPUT_SIGNALS << OUTPUT_SIGNALS );
-
- /* Commands. */
- addCommand("init",
- makeCommandVoid1(*this, &MadgwickAHRS::init,
- docCommandVoid1("Initialize the entity.",
- "Timestep in seconds (double)")));
- addCommand("getBeta",
- makeDirectGetter(*this, &m_beta,
- docDirectGetter("Beta parameter", "double")));
- addCommand("setBeta",
- makeCommandVoid1(*this, &MadgwickAHRS::set_beta,
- docCommandVoid1("Set the filter parameter beta", "double")));
- }
-
- void MadgwickAHRS::init(const double& dt)
- {
- if(dt<=0.0)
- return SEND_MSG("Timestep must be positive", MSG_TYPE_ERROR);
- m_sampleFreq=1.0/dt;
- m_initSucceeded = true;
- }
-
- void MadgwickAHRS::set_beta(const double& beta)
- {
- if(beta<0.0 || beta>1.0)
- return SEND_MSG("Beta must be in [0,1]", MSG_TYPE_ERROR);
- m_beta = beta;
- }
-
- /* ------------------------------------------------------------------- */
- /* --- SIGNALS ------------------------------------------------------- */
- /* ------------------------------------------------------------------- */
-
- DEFINE_SIGNAL_OUT_FUNCTION(imu_quat, dynamicgraph::Vector)
- {
- if(!m_initSucceeded)
- {
- SEND_WARNING_STREAM_MSG("Cannot compute signal imu_quat before initialization!");
- return s;
- }
- const dynamicgraph::Vector& accelerometer = m_accelerometerSIN(iter);
- const dynamicgraph::Vector& gyroscope = m_gyroscopeSIN(iter);
-
- getProfiler().start(PROFILE_MADGWICKAHRS_COMPUTATION);
- {
- // Update state with new measurment
- madgwickAHRSupdateIMU( gyroscope(0), gyroscope(1), gyroscope(2),
- accelerometer(0), accelerometer(1), accelerometer(2));
- if(s.size()!=4)
- s.resize(4);
- s(0) = m_q0;
- s(1) = m_q1;
- s(2) = m_q2;
- s(3) = m_q3;
- }
- getProfiler().stop(PROFILE_MADGWICKAHRS_COMPUTATION);
-
- return s;
- }
-
-
- /* --- COMMANDS ---------------------------------------------------------- */
-
- /* ------------------------------------------------------------------- */
- // ************************ PROTECTED MEMBER METHODS ********************
- /* ------------------------------------------------------------------- */
-
- // Fast inverse square-root
- // See: http://en.wikipedia.org/wiki/Fast_inverse_square_root
- double MadgwickAHRS::invSqrt(double x)
- {
- /*
- float halfx = 0.5f * x;
- float y = x;
- long i = *(long*)&y;
- i = 0x5f3759df - (i>>1);
- y = *(float*)&i;
- y = y * (1.5f - (halfx * y * y));
- return y;*/
- return (1.0/sqrt(x)); //we're not in the 70's
- }
-
- // IMU algorithm update
- void MadgwickAHRS::madgwickAHRSupdateIMU(double gx, double gy, double gz, double ax, double ay, double az)
- {
- double recipNorm;
- double s0, s1, s2, s3;
- double qDot1, qDot2, qDot3, qDot4;
- double _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3;
-
- // Rate of change of quaternion from gyroscope
- qDot1 = 0.5 * (-m_q1 * gx - m_q2 * gy - m_q3 * gz);
- qDot2 = 0.5 * ( m_q0 * gx + m_q2 * gz - m_q3 * gy);
- qDot3 = 0.5 * ( m_q0 * gy - m_q1 * gz + m_q3 * gx);
- qDot4 = 0.5 * ( m_q0 * gz + m_q1 * gy - m_q2 * gx);
-
- // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
- if(!((ax == 0.0) && (ay == 0.0) && (az == 0.0)))
- {
- // Normalise accelerometer measurement
- recipNorm = invSqrt(ax * ax + ay * ay + az * az);
- ax *= recipNorm;
- ay *= recipNorm;
- az *= recipNorm;
-
- // Auxiliary variables to avoid repeated arithmetic
- _2q0 = 2.0 * m_q0;
- _2q1 = 2.0 * m_q1;
- _2q2 = 2.0 * m_q2;
- _2q3 = 2.0 * m_q3;
- _4q0 = 4.0 * m_q0;
- _4q1 = 4.0 * m_q1;
- _4q2 = 4.0 * m_q2;
- _8q1 = 8.0 * m_q1;
- _8q2 = 8.0 * m_q2;
- q0q0 = m_q0 * m_q0;
- q1q1 = m_q1 * m_q1;
- q2q2 = m_q2 * m_q2;
- q3q3 = m_q3 * m_q3;
-
- // Gradient decent algorithm corrective step
- s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay;
- s1 = _4q1 * q3q3 - _2q3 * ax + 4.0 * q0q0 * m_q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az;
- s2 = 4.0 * q0q0 * m_q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az;
- s3 = 4.0 * q1q1 * m_q3 - _2q1 * ax + 4.0 * q2q2 * m_q3 - _2q2 * ay;
- if(!((s0 == 0.0) && (s1 == 0.0) && (s2 == 0.0) && (s3 == 0.0)))
- {
- recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
- s0 *= recipNorm;
- s1 *= recipNorm;
- s2 *= recipNorm;
- s3 *= recipNorm;
-
- // Apply feedback step
- qDot1 -= m_beta * s0;
- qDot2 -= m_beta * s1;
- qDot3 -= m_beta * s2;
- qDot4 -= m_beta * s3;
- }
- }
-
- // Integrate rate of change of quaternion to yield quaternion
- m_q0 += qDot1 * (1.0 / m_sampleFreq);
- m_q1 += qDot2 * (1.0 / m_sampleFreq);
- m_q2 += qDot3 * (1.0 / m_sampleFreq);
- m_q3 += qDot4 * (1.0 / m_sampleFreq);
-
- // Normalise quaternion
- recipNorm = invSqrt(m_q0 * m_q0 + m_q1 * m_q1 + m_q2 * m_q2 + m_q3 * m_q3);
- m_q0 *= recipNorm;
- m_q1 *= recipNorm;
- m_q2 *= recipNorm;
- m_q3 *= recipNorm;
- }
-
-
- /* ------------------------------------------------------------------- */
- /* --- ENTITY -------------------------------------------------------- */
- /* ------------------------------------------------------------------- */
-
- void MadgwickAHRS::display(std::ostream& os) const
- {
- os << "MadgwickAHRS "<<getName();
- try
- {
- getProfiler().report_all(3, os);
- }
- catch (ExceptionSignal e) {}
- }
- } // namespace talos_balance
- } // namespace sot
-} // namespace dynamicgraph
-
-
-
-
-
-
-
diff --git a/src/nd-trajectory-generator.cpp b/src/nd-trajectory-generator.cpp
index 18c09557cc5396ee27cb216146642f2eda82e918..557cf9fc737057e4f7743c55b8889acfc8f5a590 100644
--- a/src/nd-trajectory-generator.cpp
+++ b/src/nd-trajectory-generator.cpp
@@ -21,7 +21,7 @@
#include <dynamic-graph/factory.h>
#include <sot/talos_balance/utils/commands-helper.hh>
-#include <sot/talos_balance/utils/stop-watch.hh>
+#include <sot/core/stop-watch.hh>
namespace dynamicgraph
{
diff --git a/src/parameter-server.cpp b/src/parameter-server.cpp
deleted file mode 100644
index 1f882af8a8cef768afdf4897a9f4b785f87716f4..0000000000000000000000000000000000000000
--- a/src/parameter-server.cpp
+++ /dev/null
@@ -1,352 +0,0 @@
-/*
- * Copyright 2014, Andrea Del Prete, LAAS-CNRS
- *
- * This file is part of sot-torque-control.
- * sot-torque-control is free software: you can redistribute it and/or
- * modify it under the terms of the GNU Lesser General Public License
- * as published by the Free Software Foundation, either version 3 of
- * the License, or (at your option) any later version.
- * sot-torque-control is distributed in the hope that it will be
- * useful, but WITHOUT ANY WARRANTY; without even the implied warranty
- * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU Lesser General Public License for more details. You should
- * have received a copy of the GNU Lesser General Public License along
- * with sot-torque-control. If not, see <http://www.gnu.org/licenses/>.
- */
-#include <sot/talos_balance/robot/robot-wrapper.hh>
-
-#include <sot/talos_balance/parameter-server.hh>
-#include <sot/core/debug.hh>
-#include <dynamic-graph/factory.h>
-
-
-#include <dynamic-graph/all-commands.h>
-#include <sot/talos_balance/utils/stop-watch.hh>
-#include <sot/talos_balance/utils/statistics.hh>
-
-
-
-using namespace sot::talos_balance;
-
-namespace dynamicgraph
-{
- namespace sot
- {
- namespace talos_balance
- {
- namespace dynamicgraph = ::dynamicgraph;
- using namespace dynamicgraph;
- using namespace dynamicgraph::command;
- using namespace std;
- using namespace dg::sot::talos_balance;
-
-
-//Size to be aligned "-------------------------------------------------------"
-#define PROFILE_PWM_DESIRED_COMPUTATION "Control manager "
-#define PROFILE_DYNAMIC_GRAPH_PERIOD "Control period "
-
-#define INPUT_SIGNALS
-#define OUTPUT_SIGNALS
-
- /// Define EntityClassName here rather than in the header file
- /// so that it can be used by the macros DEFINE_SIGNAL_**_FUNCTION.
- typedef ParameterServer EntityClassName;
-
- /* --- DG FACTORY ---------------------------------------------------- */
- DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(ParameterServer,
- "ParameterServer");
-
- /* ------------------------------------------------------------------- */
- /* --- CONSTRUCTION -------------------------------------------------- */
- /* ------------------------------------------------------------------- */
- ParameterServer::
- ParameterServer(const std::string& name)
- : Entity(name)
- ,m_robot_util(RefVoidRobotUtil())
- ,m_initSucceeded(false)
- ,m_emergency_stop_triggered(false)
- ,m_is_first_iter(true)
- ,m_iter(0)
- ,m_sleep_time(0.0)
- {
-
- //~ Entity::signalRegistration( INPUT_SIGNALS << OUTPUT_SIGNALS);
-
- /* Commands. */
- addCommand("init",
- makeCommandVoid3(*this, &ParameterServer::init,
- docCommandVoid3("Initialize the entity.",
- "Time period in seconds (double)",
- "URDF file path (string)",
- "Robot reference (string)")));
-
- addCommand("setNameToId",
- makeCommandVoid2(*this,&ParameterServer::setNameToId,
- docCommandVoid2("Set map for a name to an Id",
- "(string) joint name",
- "(int) joint id")));
-
- addCommand("setForceNameToForceId",
- makeCommandVoid2(*this,&ParameterServer::setForceNameToForceId,
- docCommandVoid2("Set map from a force sensor name to a force sensor Id",
- "(string) force sensor name",
- "(int) force sensor id")));
-
-
- addCommand("setJointLimitsFromId",
- makeCommandVoid3(*this,&ParameterServer::setJointLimitsFromId,
- docCommandVoid3("Set the joint limits for a given joint ID",
- "(int) joint id",
- "(double) lower limit",
- "(double) uppper limit")));
-
- addCommand("setForceLimitsFromId",
- makeCommandVoid3(*this,&ParameterServer::setForceLimitsFromId,
- docCommandVoid3("Set the force limits for a given force sensor ID",
- "(int) force sensor id",
- "(double) lower limit",
- "(double) uppper limit")));
-
- addCommand("setJointsUrdfToSot",
- makeCommandVoid1(*this, &ParameterServer::setJoints,
- docCommandVoid1("Map Joints From URDF to SoT.",
- "Vector of integer for mapping")));
-
- addCommand("setRightFootSoleXYZ",
- makeCommandVoid1(*this, &ParameterServer::setRightFootSoleXYZ,
- docCommandVoid1("Set the right foot sole 3d position.",
- "Vector of 3 doubles")));
- addCommand("setRightFootForceSensorXYZ",
- makeCommandVoid1(*this, &ParameterServer::setRightFootForceSensorXYZ,
- docCommandVoid1("Set the right foot sensor 3d position.",
- "Vector of 3 doubles")));
-
- addCommand("setFootFrameName",
- makeCommandVoid2(*this, &ParameterServer::setFootFrameName,
- docCommandVoid2("Set the Frame Name for the Foot Name.",
- "(string) Foot name",
- "(string) Frame name")));
- addCommand("setImuJointName",
- makeCommandVoid1(*this, &ParameterServer::setImuJointName,
- docCommandVoid1("Set the Joint Name wich IMU is attached to.",
- "(string) Joint name")));
- addCommand("displayRobotUtil",
- makeCommandVoid0(*this, &ParameterServer::displayRobotUtil,
- docCommandVoid0("Display the current robot util data set.")));
-
- }
-
- void ParameterServer::init(const double & dt,
- const std::string & urdfFile,
- const std::string &robotRef)
- {
- if(dt<=0.0)
- return SEND_MSG("Timestep must be positive", MSG_TYPE_ERROR);
- m_dt = dt;
- m_emergency_stop_triggered = false;
- m_initSucceeded = true;
- vector<string> package_dirs;
- m_robot = new robots::RobotWrapper(urdfFile, package_dirs, pinocchio::JointModelFreeFlyer());
-
- std::string localName(robotRef);
- if (!isNameInRobotUtil(localName))
- {
- m_robot_util = createRobotUtil(localName);
- }
- else
- {
- m_robot_util = getRobotUtil(localName);
- }
-
- m_robot_util->m_urdf_filename = urdfFile;
-
- addCommand("getJointsUrdfToSot",
- makeDirectGetter(*this, &m_robot_util->m_dgv_urdf_to_sot,
- docDirectSetter("Display map Joints From URDF to SoT.",
- "Vector of integer for mapping")));
-
- m_robot_util->m_nbJoints = m_robot->nv()-6;
- //~ m_jointCtrlModes_current.resize(m_robot_util->m_nbJoints);
- //~ m_jointCtrlModes_previous.resize(m_robot_util->m_nbJoints);
- //~ m_jointCtrlModesCountDown.resize(m_robot_util->m_nbJoints,0);
- }
-
-
- /* ------------------------------------------------------------------- */
- /* --- SIGNALS ------------------------------------------------------- */
- /* ------------------------------------------------------------------- */
-
-
-
-
- /* --- COMMANDS ---------------------------------------------------------- */
-
- void ParameterServer::setNameToId(const std::string &jointName,
- const unsigned int & jointId)
- {
- if(!m_initSucceeded)
- {
- SEND_WARNING_STREAM_MSG("Cannot set joint name from joint id before initialization!");
- return;
- }
- m_robot_util->set_name_to_id(jointName,jointId);
- }
-
- void ParameterServer::setJointLimitsFromId(const unsigned int & jointId,
- const double & lq,
- const double & uq)
- {
- if(!m_initSucceeded)
- {
- SEND_WARNING_STREAM_MSG("Cannot set joints limits from joint id before initialization!");
- return;
- }
-
- m_robot_util->set_joint_limits_for_id((Index)jointId,lq,uq);
- }
-
- void ParameterServer::setForceLimitsFromId(const unsigned int & jointId,
- const dynamicgraph::Vector &lq,
- const dynamicgraph::Vector &uq)
- {
- if(!m_initSucceeded)
- {
- SEND_WARNING_STREAM_MSG("Cannot set force limits from force id before initialization!");
- return;
- }
-
- m_robot_util->m_force_util.set_force_id_to_limits((Index)jointId,lq,uq);
- }
-
- void ParameterServer::setForceNameToForceId(const std::string &forceName,
- const unsigned int & forceId)
- {
- if(!m_initSucceeded)
- {
- SEND_WARNING_STREAM_MSG("Cannot set force sensor name from force sensor id before initialization!");
- return;
- }
-
- m_robot_util->m_force_util.set_name_to_force_id(forceName,forceId);
- }
-
- void ParameterServer::setJoints(const dg::Vector & urdf_to_sot)
- {
- if(!m_initSucceeded)
- {
- SEND_WARNING_STREAM_MSG("Cannot set mapping to sot before initialization!");
- return;
- }
- m_robot_util->set_urdf_to_sot(urdf_to_sot);
- }
-
- void ParameterServer::setRightFootSoleXYZ( const dynamicgraph::Vector &xyz)
- {
- if(!m_initSucceeded)
- {
- SEND_WARNING_STREAM_MSG("Cannot set right foot sole XYZ before initialization!");
- return;
- }
-
- m_robot_util->m_foot_util.m_Right_Foot_Sole_XYZ = xyz;
- }
-
- void ParameterServer::setRightFootForceSensorXYZ(const dynamicgraph::Vector &xyz)
- {
- if(!m_initSucceeded)
- {
- SEND_WARNING_STREAM_MSG("Cannot set right foot force sensor XYZ before initialization!");
- return;
- }
-
- m_robot_util->m_foot_util.m_Right_Foot_Force_Sensor_XYZ = xyz;
- }
-
- void ParameterServer::setFootFrameName(const std::string &FootName,
- const std::string &FrameName)
- {
- if(!m_initSucceeded)
- {
- SEND_WARNING_STREAM_MSG("Cannot set foot frame name!");
- return;
- }
- if (FootName=="Left")
- m_robot_util->m_foot_util.m_Left_Foot_Frame_Name = FrameName;
- else if (FootName=="Right")
- m_robot_util->m_foot_util.m_Right_Foot_Frame_Name = FrameName;
- else
- SEND_WARNING_STREAM_MSG("Did not understand the foot name !" + FootName);
- }
-
- void ParameterServer::setImuJointName(const std::string &JointName)
- {
- if(!m_initSucceeded)
- {
- SEND_WARNING_STREAM_MSG("Cannot set IMU joint name!");
- return;
- }
- m_robot_util->m_imu_joint_name = JointName;
- }
-
- void ParameterServer::displayRobotUtil()
- {
- m_robot_util->display(std::cout);
- }
-
- /* --- PROTECTED MEMBER METHODS ---------------------------------------------------------- */
-
- bool ParameterServer::convertJointNameToJointId(const std::string& name, unsigned int& id)
- {
- // Check if the joint name exists
- int jid = int(m_robot_util->get_id_from_name(name)); // cast needed due to bug in robot-utils
- if (jid<0)
- {
- SEND_MSG("The specified joint name does not exist: "+name, MSG_TYPE_ERROR);
- std::stringstream ss;
- for(pinocchio::Model::JointIndex it=0; it< m_robot_util->m_nbJoints;it++)
- ss<< m_robot_util->get_name_from_id(it) <<", ";
- SEND_MSG("Possible joint names are: "+ss.str(), MSG_TYPE_INFO);
- return false;
- }
- id = (unsigned int )jid;
- return true;
- }
-
- bool ParameterServer::isJointInRange(unsigned int id, double q)
- {
- const JointLimits & JL = m_robot_util->get_joint_limits_from_id((Index)id);
-
- double jl= JL.lower;
- if(q<jl)
- {
- SEND_MSG("Desired joint angle "+toString(q)+" is smaller than lower limit: "+toString(jl),MSG_TYPE_ERROR);
- return false;
- }
- double ju = JL.upper;
- if(q>ju)
- {
- SEND_MSG("Desired joint angle "+toString(q)+" is larger than upper limit: "+toString(ju),MSG_TYPE_ERROR);
- return false;
- }
- return true;
- }
-
-
- /* ------------------------------------------------------------------- */
- /* --- ENTITY -------------------------------------------------------- */
- /* ------------------------------------------------------------------- */
-
-
- void ParameterServer::display(std::ostream& os) const
- {
- os << "ParameterServer "<<getName();
- try
- {
- getProfiler().report_all(3, os);
- }
- catch (ExceptionSignal e) {}
- }
-
- } // namespace talos_balance
- } // namespace sot
-} // namespace dynamicgraph
diff --git a/src/pose-quaternion-to-matrix-homo.cpp b/src/pose-quaternion-to-matrix-homo.cpp
index e1f871331d9ec6301a0b3e047844e20697474ccf..ac98432daa98ac4fbb41cddcc7d4871f7ec2d63f 100644
--- a/src/pose-quaternion-to-matrix-homo.cpp
+++ b/src/pose-quaternion-to-matrix-homo.cpp
@@ -19,7 +19,7 @@
#include <sot/core/debug.hh>
#include <dynamic-graph/factory.h>
#include <dynamic-graph/all-commands.h>
-#include "sot/talos_balance/utils/stop-watch.hh"
+#include <sot/core/stop-watch.hh>
#include <Eigen/Core>
diff --git a/src/qualisys-client.cpp b/src/qualisys-client.cpp
index f61d99dd36ccdc803c2241af83c31b93b0fb5171..aace23d71b19cd4aaecb3fec851c232aedbb8371 100644
--- a/src/qualisys-client.cpp
+++ b/src/qualisys-client.cpp
@@ -11,7 +11,7 @@
#include <dynamic-graph/factory.h>
#include <dynamic-graph/all-commands.h>
-#include <sot/talos_balance/utils/stop-watch.hh>
+#include <sot/core/stop-watch.hh>
#include <sot/talos_balance/utils/statistics.hh>
// #include "RTProtocol.h"
diff --git a/src/quat-to-euler.cpp b/src/quat-to-euler.cpp
index b4cb65985c8c1c467305a19f1671114291333472..a7a15183e647cdeba63b2513191ed0e6b4a0dcc4 100644
--- a/src/quat-to-euler.cpp
+++ b/src/quat-to-euler.cpp
@@ -19,7 +19,7 @@
#include <sot/core/debug.hh>
#include <dynamic-graph/factory.h>
#include <dynamic-graph/all-commands.h>
-#include "sot/talos_balance/utils/stop-watch.hh"
+#include <sot/core/stop-watch.hh>
#include <Eigen/Core>
diff --git a/src/admittance-controller.cpp b/src/simple-admittance-controller.cpp
similarity index 77%
rename from src/admittance-controller.cpp
rename to src/simple-admittance-controller.cpp
index 32cd2b96930941d3769423672dfd429445e7f294..8375b064659154a137c97c0e71246f20e48af0b3 100644
--- a/src/admittance-controller.cpp
+++ b/src/simple-admittance-controller.cpp
@@ -14,14 +14,14 @@
* with sot-talos-balance. If not, see <http://www.gnu.org/licenses/>.
*/
-#include "sot/talos_balance/admittance-controller.hh"
+#include "sot/talos_balance/simple-admittance-controller.hh"
#include <sot/core/debug.hh>
#include <dynamic-graph/factory.h>
#include <dynamic-graph/command-bind.h>
#include <dynamic-graph/all-commands.h>
-#include "sot/talos_balance/utils/stop-watch.hh"
+#include <sot/core/stop-watch.hh>
namespace dynamicgraph
{
@@ -35,9 +35,9 @@ namespace dynamicgraph
//Size to be aligned "-------------------------------------------------------"
-#define PROFILE_ADMITTANCECONTROLLER_QREF_COMPUTATION "AdmittanceController: qRef computation "
+#define PROFILE_SIMPLE_ADMITTANCECONTROLLER_QREF_COMPUTATION "SimpleAdmittanceController: qRef computation "
-#define PROFILE_ADMITTANCECONTROLLER_DQREF_COMPUTATION "AdmittanceController: dqRef computation "
+#define PROFILE_SIMPLE_ADMITTANCECONTROLLER_DQREF_COMPUTATION "SimpleAdmittanceController: dqRef computation "
#define INPUT_SIGNALS m_KpSIN << m_stateSIN << m_tauSIN << m_tauDesSIN
@@ -45,16 +45,16 @@ namespace dynamicgraph
/// Define EntityClassName here rather than in the header file
/// so that it can be used by the macros DEFINE_SIGNAL_**_FUNCTION.
- typedef AdmittanceController EntityClassName;
+ typedef SimpleAdmittanceController EntityClassName;
/* --- DG FACTORY ---------------------------------------------------- */
- DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(AdmittanceController,
- "AdmittanceController");
+ DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(SimpleAdmittanceController,
+ "SimpleAdmittanceController");
/* ------------------------------------------------------------------- */
/* --- CONSTRUCTION -------------------------------------------------- */
/* ------------------------------------------------------------------- */
- AdmittanceController::AdmittanceController(const std::string& name)
+ SimpleAdmittanceController::SimpleAdmittanceController(const std::string& name)
: Entity(name)
, CONSTRUCT_SIGNAL_IN(Kp, dynamicgraph::Vector)
, CONSTRUCT_SIGNAL_IN(state, dynamicgraph::Vector)
@@ -67,13 +67,13 @@ namespace dynamicgraph
Entity::signalRegistration( INPUT_SIGNALS << OUTPUT_SIGNALS );
/* Commands. */
- addCommand("init", makeCommandVoid2(*this, &AdmittanceController::init, docCommandVoid2("Initialize the entity.","time step","Number of elements")));
- addCommand("setPosition", makeCommandVoid1(*this, &AdmittanceController::setPosition, docCommandVoid1("Set initial reference position.","Initial position")));
+ addCommand("init", makeCommandVoid2(*this, &SimpleAdmittanceController::init, docCommandVoid2("Initialize the entity.","time step","Number of elements")));
+ addCommand("setPosition", makeCommandVoid1(*this, &SimpleAdmittanceController::setPosition, docCommandVoid1("Set initial reference position.","Initial position")));
addCommand("useExternalState", makeDirectSetter(*this,&m_useState, docDirectSetter("use external state","bool")));
addCommand("isUsingExternalState", makeDirectGetter(*this,&m_useState, docDirectGetter("use external state","bool")));
}
- void AdmittanceController::init(const double & dt, const unsigned & n)
+ void SimpleAdmittanceController::init(const double & dt, const unsigned & n)
{
if(n<1)
return SEND_MSG("n must be at least 1", MSG_TYPE_ERROR);
@@ -90,7 +90,7 @@ namespace dynamicgraph
m_initSucceeded = true;
}
- void AdmittanceController::setPosition(const dynamicgraph::Vector & position)
+ void SimpleAdmittanceController::setPosition(const dynamicgraph::Vector & position)
{
m_q = position;
@@ -108,7 +108,7 @@ namespace dynamicgraph
return s;
}
- getProfiler().start(PROFILE_ADMITTANCECONTROLLER_DQREF_COMPUTATION);
+ getProfiler().start(PROFILE_SIMPLE_ADMITTANCECONTROLLER_DQREF_COMPUTATION);
const Vector & tauDes = m_tauDesSIN(iter);
const Vector & tau = m_tauSIN(iter);
@@ -122,7 +122,7 @@ namespace dynamicgraph
s = dqRef;
- getProfiler().stop(PROFILE_ADMITTANCECONTROLLER_DQREF_COMPUTATION);
+ getProfiler().stop(PROFILE_SIMPLE_ADMITTANCECONTROLLER_DQREF_COMPUTATION);
return s;
}
@@ -135,7 +135,7 @@ namespace dynamicgraph
return s;
}
- getProfiler().start(PROFILE_ADMITTANCECONTROLLER_QREF_COMPUTATION);
+ getProfiler().start(PROFILE_SIMPLE_ADMITTANCECONTROLLER_QREF_COMPUTATION);
const Vector & dqRef = m_dqRefSOUT(iter);
@@ -157,7 +157,7 @@ namespace dynamicgraph
s = m_q;
- getProfiler().stop(PROFILE_ADMITTANCECONTROLLER_QREF_COMPUTATION);
+ getProfiler().stop(PROFILE_SIMPLE_ADMITTANCECONTROLLER_QREF_COMPUTATION);
return s;
}
@@ -169,9 +169,9 @@ namespace dynamicgraph
/* --- ENTITY -------------------------------------------------------- */
/* ------------------------------------------------------------------- */
- void AdmittanceController::display(std::ostream& os) const
+ void SimpleAdmittanceController::display(std::ostream& os) const
{
- os << "AdmittanceController " << getName();
+ os << "SimpleAdmittanceController " << getName();
try
{
getProfiler().report_all(3, os);
diff --git a/src/simple-reference-frame.cpp b/src/simple-reference-frame.cpp
index fb3e68b49dabdeff9833d9e999c887970f42e296..9c4055c21050fd088fc05318a1f7222425e100c7 100644
--- a/src/simple-reference-frame.cpp
+++ b/src/simple-reference-frame.cpp
@@ -21,7 +21,7 @@
#include <dynamic-graph/command-bind.h>
#include <dynamic-graph/all-commands.h>
-#include "sot/talos_balance/utils/stop-watch.hh"
+#include <sot/core/stop-watch.hh>
namespace dynamicgraph
{
diff --git a/src/simple-zmp-estimator.cpp b/src/simple-zmp-estimator.cpp
index 5a697f71d339679928834f57f2eca4e3b381acdf..fe2803b260affd34ed76fac08005732095fa111a 100644
--- a/src/simple-zmp-estimator.cpp
+++ b/src/simple-zmp-estimator.cpp
@@ -21,7 +21,7 @@
#include <dynamic-graph/command-bind.h>
#include <dynamic-graph/all-commands.h>
-#include "sot/talos_balance/utils/stop-watch.hh"
+#include <sot/core/stop-watch.hh>
namespace dynamicgraph
{
diff --git a/src/state-transformation.cpp b/src/state-transformation.cpp
index a5a1cea22537eb5bad9168c4529ff40b0af1ee3a..f21b110109140567faaaec699d52474890d4fa5a 100644
--- a/src/state-transformation.cpp
+++ b/src/state-transformation.cpp
@@ -21,7 +21,7 @@
#include <dynamic-graph/command-bind.h>
#include <dynamic-graph/all-commands.h>
-#include "sot/talos_balance/utils/stop-watch.hh"
+#include <sot/core/stop-watch.hh>
namespace dynamicgraph
{
diff --git a/src/base-estimator.cpp b/src/talos-base-estimator.cpp
similarity index 91%
rename from src/base-estimator.cpp
rename to src/talos-base-estimator.cpp
index 41701c662860a89e1fe38e7bb27962ce2ba0ef5c..0651047cd91796d806e3d63376754dba79608606 100644
--- a/src/base-estimator.cpp
+++ b/src/talos-base-estimator.cpp
@@ -14,11 +14,11 @@
* with sot-torque-control. If not, see <http://www.gnu.org/licenses/>.
*/
-#include <sot/talos_balance/base-estimator.hh>
+#include <sot/talos_balance/talos-base-estimator.hh>
#include <sot/core/debug.hh>
#include <dynamic-graph/factory.h>
#include <dynamic-graph/all-commands.h>
-#include <sot/talos_balance/utils/stop-watch.hh>
+#include <sot/core/stop-watch.hh>
#include "pinocchio/algorithm/frames.hpp"
namespace dynamicgraph
@@ -34,7 +34,10 @@ namespace dynamicgraph
using namespace pinocchio;
using boost::math::normal; // typedef provides default type is double.
- void se3Interp(const pinocchio::SE3 & s1, const pinocchio::SE3 & s2, const double alpha, pinocchio::SE3 & s12)
+ void se3Interp(const pinocchio::SE3 & s1,
+ const pinocchio::SE3 & s2,
+ const double alpha,
+ pinocchio::SE3 & s12)
{
const Eigen::Vector3d t_( s1.translation() * alpha+
s2.translation() * (1-alpha));
@@ -45,7 +48,8 @@ namespace dynamicgraph
s12 = pinocchio::SE3(pinocchio::exp3(w),t_);
}
- void rpyToMatrix(double roll, double pitch, double yaw, Eigen::Matrix3d & R)
+ void rpyToMatrix(double roll,
+ double pitch, double yaw, Eigen::Matrix3d & R)
{
Eigen::AngleAxisd rollAngle(roll, Eigen::Vector3d::UnitX());
Eigen::AngleAxisd pitchAngle(pitch, Eigen::Vector3d::UnitY());
@@ -75,7 +79,9 @@ namespace dynamicgraph
}
}
- void quanternionMult(const Eigen::Vector4d & q1, const Eigen::Vector4d & q2, Eigen::Vector4d & q12)
+ void quanternionMult(const Eigen::Vector4d & q1,
+ const Eigen::Vector4d & q2,
+ Eigen::Vector4d & q12)
{
q12(0) = q2(0)*q1(0)-q2(1)*q1(1)-q2(2)*q1(2)-q2(3)*q1(3);
q12(1) = q2(0)*q1(1)+q2(1)*q1(0)-q2(2)*q1(3)+q2(3)*q1(2);
@@ -83,7 +89,10 @@ namespace dynamicgraph
q12(3) = q2(0)*q1(3)-q2(1)*q1(2)+q2(2)*q1(1)+q2(3)*q1(0);
}
- void pointRotationByQuaternion(const Eigen::Vector3d & point,const Eigen::Vector4d & quat, Eigen::Vector3d & rotatedPoint)
+ void pointRotationByQuaternion
+ (const Eigen::Vector3d & point,
+ const Eigen::Vector4d & quat,
+ Eigen::Vector3d & rotatedPoint)
{
const Eigen::Vector4d p4(0.0, point(0),point(1),point(2));
const Eigen::Vector4d quat_conj(quat(0),-quat(1),-quat(2),-quat(3));
@@ -111,9 +120,14 @@ namespace dynamicgraph
{
double wMean = (a1*w1+ a2*w2)/(w1+w2);
if ( a1-a2 >= EIGEN_PI )
- return (EIGEN_PI*(w1-w2)/(w2+w1) - wMean) < 0 ? -EIGEN_PI + wMean - EIGEN_PI*(w1-w2)/(w2+w1) : EIGEN_PI + wMean - EIGEN_PI*(w1-w2)/(w2+w1);
+ return (EIGEN_PI*(w1-w2)/(w2+w1) - wMean)
+ < 0 ? -EIGEN_PI +
+ wMean - EIGEN_PI*(w1-w2)/(w2+w1) : EIGEN_PI +
+ wMean - EIGEN_PI*(w1-w2)/(w2+w1);
else if ( a1-a2 < -EIGEN_PI )
- return (EIGEN_PI*(w2-w1)/(w1+w2) - wMean) < 0 ? -EIGEN_PI + wMean - EIGEN_PI*(w2-w1)/(w1+w2) : EIGEN_PI + wMean - EIGEN_PI*(w2-w1)/(w1+w2);
+ return (EIGEN_PI*(w2-w1)/(w1+w2) - wMean)
+ < 0 ? -EIGEN_PI + wMean - EIGEN_PI*(w2-w1)/(w1+w2) :
+ EIGEN_PI + wMean - EIGEN_PI*(w2-w1)/(w1+w2);
return wMean;
}
@@ -129,7 +143,8 @@ namespace dynamicgraph
// pa = a1; //positive angle
// paw = w1; //positive angle weight
// double piFac = (paw-naw)/(naw+paw);
- // return (EIGEN_PI*piFac - wMean) < 0 ? -EIGEN_PI + wMean - EIGEN_PI*piFac : EIGEN_PI + wMean - EIGEN_PI*piFac;
+ // return (EIGEN_PI*piFac - wMean) < 0 ? -EIGEN_PI +
+ // wMean - EIGEN_PI*piFac : EIGEN_PI + wMean - EIGEN_PI*piFac;
// }
// else if ( a1-a2 < -EIGEN_PI )
// {
@@ -138,7 +153,8 @@ namespace dynamicgraph
// pa = a2; //positive angle
// paw = w2; //positive angle weight
// double piFac = (paw-naw)/(naw+paw);
- // return (EIGEN_PI*piFac - wMean) < 0 ? -EIGEN_PI + wMean - EIGEN_PI*piFac : EIGEN_PI + wMean - EIGEN_PI*piFac;
+ // return (EIGEN_PI*piFac - wMean) < 0 ? -EIGEN_PI + wMean -
+ // EIGEN_PI*piFac : EIGEN_PI + wMean - EIGEN_PI*piFac;
// }
// return wMean;
// }
@@ -157,17 +173,17 @@ namespace dynamicgraph
/// Define EntityClassName here rather than in the header file
/// so that it can be used by the macros DEFINE_SIGNAL_**_FUNCTION.
- typedef BaseEstimator EntityClassName;
+ typedef TalosBaseEstimator EntityClassName;
/* --- DG FACTORY ---------------------------------------------------- */
- DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(BaseEstimator,
- "BaseEstimator");
+ DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(TalosBaseEstimator,
+ "TalosBaseEstimator");
/* ------------------------------------------------------------------- */
/* --- CONSTRUCTION -------------------------------------------------- */
/* ------------------------------------------------------------------- */
- BaseEstimator::
- BaseEstimator(const std::string& name)
+ TalosBaseEstimator::
+ TalosBaseEstimator(const std::string& name)
: Entity(name)
,CONSTRUCT_SIGNAL_IN( joint_positions, dynamicgraph::Vector)
,CONSTRUCT_SIGNAL_IN( joint_velocities, dynamicgraph::Vector)
@@ -238,90 +254,90 @@ namespace dynamicgraph
/* Commands. */
addCommand("init",
- makeCommandVoid2(*this, &BaseEstimator::init,
+ makeCommandVoid2(*this, &TalosBaseEstimator::init,
docCommandVoid2("Initialize the entity.",
"time step (double)",
"URDF file path (string)")));
addCommand("set_fz_stable_windows_size",
- makeCommandVoid1(*this, &BaseEstimator::set_fz_stable_windows_size,
+ makeCommandVoid1(*this, &TalosBaseEstimator::set_fz_stable_windows_size,
docCommandVoid1("Set the windows size used to detect that feet is stable",
"int")));
addCommand("set_alpha_w_filter",
- makeCommandVoid1(*this, &BaseEstimator::set_alpha_w_filter,
+ makeCommandVoid1(*this, &TalosBaseEstimator::set_alpha_w_filter,
docCommandVoid1("Set the filter parameter to filter weights",
"double")));
addCommand("set_alpha_DC_acc",
- makeCommandVoid1(*this, &BaseEstimator::set_alpha_DC_acc,
+ makeCommandVoid1(*this, &TalosBaseEstimator::set_alpha_DC_acc,
docCommandVoid1("Set the filter parameter for removing DC from accelerometer data",
"double")));
addCommand("set_alpha_DC_vel",
- makeCommandVoid1(*this, &BaseEstimator::set_alpha_DC_vel,
+ makeCommandVoid1(*this, &TalosBaseEstimator::set_alpha_DC_vel,
docCommandVoid1("Set the filter parameter for removing DC from velocity integrated from acceleration",
"double")));
addCommand("reset_foot_positions",
- makeCommandVoid0(*this, &BaseEstimator::reset_foot_positions,
+ makeCommandVoid0(*this, &TalosBaseEstimator::reset_foot_positions,
docCommandVoid0("Reset the position of the feet.")));
addCommand("get_imu_weight",
makeDirectGetter(*this,&m_w_imu,
docDirectGetter("Weight of imu-based orientation in sensor fusion",
"double")));
addCommand("set_imu_weight",
- makeCommandVoid1(*this, &BaseEstimator::set_imu_weight,
+ makeCommandVoid1(*this, &TalosBaseEstimator::set_imu_weight,
docCommandVoid1("Set the weight of imu-based orientation in sensor fusion",
"double")));
addCommand("set_zmp_std_dev_right_foot",
- makeCommandVoid1(*this, &BaseEstimator::set_zmp_std_dev_right_foot,
+ makeCommandVoid1(*this, &TalosBaseEstimator::set_zmp_std_dev_right_foot,
docCommandVoid1("Set the standard deviation of the ZMP measurement errors for the right foot",
"double")));
addCommand("set_zmp_std_dev_left_foot",
- makeCommandVoid1(*this, &BaseEstimator::set_zmp_std_dev_left_foot,
+ makeCommandVoid1(*this, &TalosBaseEstimator::set_zmp_std_dev_left_foot,
docCommandVoid1("Set the standard deviation of the ZMP measurement errors for the left foot",
"double")));
addCommand("set_normal_force_std_dev_right_foot",
- makeCommandVoid1(*this, &BaseEstimator::set_normal_force_std_dev_right_foot,
+ makeCommandVoid1(*this, &TalosBaseEstimator::set_normal_force_std_dev_right_foot,
docCommandVoid1("Set the standard deviation of the normal force measurement errors for the right foot",
"double")));
addCommand("set_normal_force_std_dev_left_foot",
- makeCommandVoid1(*this, &BaseEstimator::set_normal_force_std_dev_left_foot,
+ makeCommandVoid1(*this, &TalosBaseEstimator::set_normal_force_std_dev_left_foot,
docCommandVoid1("Set the standard deviation of the normal force measurement errors for the left foot",
"double")));
addCommand("set_stiffness_right_foot",
- makeCommandVoid1(*this, &BaseEstimator::set_stiffness_right_foot,
+ makeCommandVoid1(*this, &TalosBaseEstimator::set_stiffness_right_foot,
docCommandVoid1("Set the 6d stiffness of the spring at the right foot",
"vector")));
addCommand("set_stiffness_left_foot",
- makeCommandVoid1(*this, &BaseEstimator::set_stiffness_left_foot,
+ makeCommandVoid1(*this, &TalosBaseEstimator::set_stiffness_left_foot,
docCommandVoid1("Set the 6d stiffness of the spring at the left foot",
"vector")));
addCommand("set_right_foot_sizes",
- makeCommandVoid1(*this, &BaseEstimator::set_right_foot_sizes,
+ makeCommandVoid1(*this, &TalosBaseEstimator::set_right_foot_sizes,
docCommandVoid1("Set the size of the right foot (pos x, neg x, pos y, neg y)",
"4d vector")));
addCommand("set_left_foot_sizes",
- makeCommandVoid1(*this, &BaseEstimator::set_left_foot_sizes,
+ makeCommandVoid1(*this, &TalosBaseEstimator::set_left_foot_sizes,
docCommandVoid1("Set the size of the left foot (pos x, neg x, pos y, neg y)",
"4d vector")));
addCommand("set_zmp_margin_right_foot",
- makeCommandVoid1(*this, &BaseEstimator::set_zmp_margin_right_foot,
+ makeCommandVoid1(*this, &TalosBaseEstimator::set_zmp_margin_right_foot,
docCommandVoid1("Set the ZMP margin for the right foot",
"double")));
addCommand("set_zmp_margin_left_foot",
- makeCommandVoid1(*this, &BaseEstimator::set_zmp_margin_left_foot,
+ makeCommandVoid1(*this, &TalosBaseEstimator::set_zmp_margin_left_foot,
docCommandVoid1("Set the ZMP margin for the left foot",
"double")));
addCommand("set_normal_force_margin_right_foot",
- makeCommandVoid1(*this, &BaseEstimator::set_normal_force_margin_right_foot,
+ makeCommandVoid1(*this, &TalosBaseEstimator::set_normal_force_margin_right_foot,
docCommandVoid1("Set the normal force margin for the right foot",
"double")));
addCommand("set_normal_force_margin_left_foot",
- makeCommandVoid1(*this, &BaseEstimator::set_normal_force_margin_left_foot,
+ makeCommandVoid1(*this, &TalosBaseEstimator::set_normal_force_margin_left_foot,
docCommandVoid1("Set the normal force margin for the left foot",
"double")));
}
- void BaseEstimator::init(const double & dt, const std::string& robotRef)
+ void TalosBaseEstimator::init(const double & dt, const std::string& robotRef)
{
m_dt = dt;
try
@@ -388,14 +404,14 @@ namespace dynamicgraph
m_initSucceeded = true;
}
- void BaseEstimator::set_fz_stable_windows_size(const int& ws)
+ void TalosBaseEstimator::set_fz_stable_windows_size(const int& ws)
{
if(ws<0.0)
return SEND_MSG("windows_size should be a positive integer", MSG_TYPE_ERROR);
m_fz_stable_windows_size = ws;
}
- void BaseEstimator::set_alpha_w_filter(const double& a)
+ void TalosBaseEstimator::set_alpha_w_filter(const double& a)
{
if(a<0.0 || a>1.0)
return SEND_MSG("alpha should be in [0..1]", MSG_TYPE_ERROR);
@@ -403,14 +419,14 @@ namespace dynamicgraph
}
- void BaseEstimator::set_alpha_DC_acc(const double& a)
+ void TalosBaseEstimator::set_alpha_DC_acc(const double& a)
{
if(a<0.0 || a>1.0)
return SEND_MSG("alpha should be in [0..1]", MSG_TYPE_ERROR);
m_alpha_DC_acc = a;
}
- void BaseEstimator::set_alpha_DC_vel(const double& a)
+ void TalosBaseEstimator::set_alpha_DC_vel(const double& a)
{
if(a<0.0 || a>1.0)
return SEND_MSG("alpha should be in [0..1]", MSG_TYPE_ERROR);
@@ -418,95 +434,95 @@ namespace dynamicgraph
}
- void BaseEstimator::reset_foot_positions()
+ void TalosBaseEstimator::reset_foot_positions()
{
m_reset_foot_pos = true;
}
- void BaseEstimator::set_imu_weight(const double& w)
+ void TalosBaseEstimator::set_imu_weight(const double& w)
{
if(w<0.0)
return SEND_MSG("Imu weight must be nonnegative", MSG_TYPE_ERROR);
m_w_imu = w;
}
- void BaseEstimator::set_stiffness_right_foot(const dynamicgraph::Vector & k)
+ void TalosBaseEstimator::set_stiffness_right_foot(const dynamicgraph::Vector & k)
{
if(k.size()!=6)
return SEND_MSG("Stiffness vector should have size 6, not "+toString(k.size()), MSG_TYPE_ERROR);
m_K_rf = k;
}
- void BaseEstimator::set_stiffness_left_foot(const dynamicgraph::Vector & k)
+ void TalosBaseEstimator::set_stiffness_left_foot(const dynamicgraph::Vector & k)
{
if(k.size()!=6)
return SEND_MSG("Stiffness vector should have size 6, not "+toString(k.size()), MSG_TYPE_ERROR);
m_K_lf = k;
}
- void BaseEstimator::set_zmp_std_dev_right_foot(const double & std_dev)
+ void TalosBaseEstimator::set_zmp_std_dev_right_foot(const double & std_dev)
{
if(std_dev<=0.0)
return SEND_MSG("Standard deviation must be a positive number", MSG_TYPE_ERROR);
m_zmp_std_dev_rf = std_dev;
}
- void BaseEstimator::set_zmp_std_dev_left_foot(const double & std_dev)
+ void TalosBaseEstimator::set_zmp_std_dev_left_foot(const double & std_dev)
{
if(std_dev<=0.0)
return SEND_MSG("Standard deviation must be a positive number", MSG_TYPE_ERROR);
m_zmp_std_dev_lf = std_dev;
}
- void BaseEstimator::set_normal_force_std_dev_right_foot(const double & std_dev)
+ void TalosBaseEstimator::set_normal_force_std_dev_right_foot(const double & std_dev)
{
if(std_dev<=0.0)
return SEND_MSG("Standard deviation must be a positive number", MSG_TYPE_ERROR);
m_fz_std_dev_rf = std_dev;
}
- void BaseEstimator::set_normal_force_std_dev_left_foot(const double & std_dev)
+ void TalosBaseEstimator::set_normal_force_std_dev_left_foot(const double & std_dev)
{
if(std_dev<=0.0)
return SEND_MSG("Standard deviation must be a positive number", MSG_TYPE_ERROR);
m_fz_std_dev_lf = std_dev;
}
- void BaseEstimator::set_right_foot_sizes(const dynamicgraph::Vector & s)
+ void TalosBaseEstimator::set_right_foot_sizes(const dynamicgraph::Vector & s)
{
if(s.size()!=4)
return SEND_MSG("Foot size vector should have size 4, not "+toString(s.size()), MSG_TYPE_ERROR);
m_right_foot_sizes = s;
}
- void BaseEstimator::set_left_foot_sizes(const dynamicgraph::Vector & s)
+ void TalosBaseEstimator::set_left_foot_sizes(const dynamicgraph::Vector & s)
{
if(s.size()!=4)
return SEND_MSG("Foot size vector should have size 4, not "+toString(s.size()), MSG_TYPE_ERROR);
m_left_foot_sizes = s;
}
- void BaseEstimator::set_zmp_margin_right_foot(const double & margin)
+ void TalosBaseEstimator::set_zmp_margin_right_foot(const double & margin)
{
m_zmp_margin_rf = margin;
}
- void BaseEstimator::set_zmp_margin_left_foot(const double & margin)
+ void TalosBaseEstimator::set_zmp_margin_left_foot(const double & margin)
{
m_zmp_margin_lf = margin;
}
- void BaseEstimator::set_normal_force_margin_right_foot(const double & margin)
+ void TalosBaseEstimator::set_normal_force_margin_right_foot(const double & margin)
{
m_fz_margin_rf = margin;
}
- void BaseEstimator::set_normal_force_margin_left_foot(const double & margin)
+ void TalosBaseEstimator::set_normal_force_margin_left_foot(const double & margin)
{
m_fz_margin_lf = margin;
}
- void BaseEstimator::compute_zmp(const Vector6 & w, Vector2 & zmp)
+ void TalosBaseEstimator::compute_zmp(const Vector6 & w, Vector2 & zmp)
{
pinocchio::Force f(w);
f = m_sole_M_ftSens.act(f);
@@ -516,7 +532,7 @@ namespace dynamicgraph
zmp[1] = f.angular()[0] / f.linear()[2];
}
- double BaseEstimator::compute_zmp_weight(const Vector2 & zmp, const Vector4 & foot_sizes,
+ double TalosBaseEstimator::compute_zmp_weight(const Vector2 & zmp, const Vector4 & foot_sizes,
double std_dev, double margin)
{
double fs0=foot_sizes[0] - margin;
@@ -534,14 +550,14 @@ namespace dynamicgraph
return cdx*cdy;
}
- double BaseEstimator::compute_force_weight(double fz, double std_dev, double margin)
+ double TalosBaseEstimator::compute_force_weight(double fz, double std_dev, double margin)
{
if (fz<margin)
return 0.0;
return (cdf(m_normal, (fz-margin)/std_dev)-0.5)*2.0;
}
- void BaseEstimator::reset_foot_positions_impl(const Vector6 & ftlf, const Vector6 & ftrf)
+ void TalosBaseEstimator::reset_foot_positions_impl(const Vector6 & ftlf, const Vector6 & ftrf)
{
// compute the base position wrt each foot
SE3 dummy, dummy1, lfMff, rfMff;
@@ -595,7 +611,7 @@ namespace dynamicgraph
m_reset_foot_pos = false;
}
- void BaseEstimator::kinematics_estimation(const Vector6 & ft, const Vector6 & K,
+ void TalosBaseEstimator::kinematics_estimation(const Vector6 & ft, const Vector6 & K,
const SE3 & oMfs, const pinocchio::FrameIndex foot_id,
SE3 & oMff, SE3 & oMfa, SE3 & fsMff)
{
@@ -1293,9 +1309,9 @@ namespace dynamicgraph
/* --- ENTITY -------------------------------------------------------- */
/* ------------------------------------------------------------------- */
- void BaseEstimator::display(std::ostream& os) const
+ void TalosBaseEstimator::display(std::ostream& os) const
{
- os << "BaseEstimator "<<getName();
+ os << "TalosBaseEstimator "<<getName();
try
{
getProfiler().report_all(3, os);
diff --git a/src/control-manager.cpp b/src/talos-control-manager.cpp
similarity index 88%
rename from src/control-manager.cpp
rename to src/talos-control-manager.cpp
index 883bebf4dcb474630a1499b084c1929eabb7b7d0..d0f38a5e58720822bf8e73b1fe3a5038ba270ab3 100644
--- a/src/control-manager.cpp
+++ b/src/talos-control-manager.cpp
@@ -14,12 +14,12 @@
* with sot-torque-control. If not, see <http://www.gnu.org/licenses/>.
*/
-#include <sot/talos_balance/control-manager.hh>
+#include <sot/talos_balance/talos-control-manager.hh>
#include <sot/core/debug.hh>
#include <dynamic-graph/factory.h>
#include <dynamic-graph/all-commands.h>
-#include <sot/talos_balance/utils/stop-watch.hh>
+#include <sot/core/stop-watch.hh>
#include <sot/talos_balance/utils/statistics.hh>
@@ -47,17 +47,17 @@ namespace dynamicgraph
/// Define EntityClassName here rather than in the header file
/// so that it can be used by the macros DEFINE_SIGNAL_**_FUNCTION.
- typedef ControlManager EntityClassName;
+ typedef TalosControlManager EntityClassName;
/* --- DG FACTORY ---------------------------------------------------- */
- DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(ControlManager,
- "ControlManager");
+ DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(TalosControlManager,
+ "TalosControlManager");
/* ------------------------------------------------------------------- */
/* --- CONSTRUCTION -------------------------------------------------- */
/* ------------------------------------------------------------------- */
- ControlManager::
- ControlManager(const std::string& name)
+ TalosControlManager::
+ TalosControlManager(const std::string& name)
: Entity(name)
,CONSTRUCT_SIGNAL_IN(u_max, dynamicgraph::Vector)
,CONSTRUCT_SIGNAL_OUT(u, dynamicgraph::Vector, m_u_maxSIN)
@@ -73,43 +73,43 @@ namespace dynamicgraph
/* Commands. */
addCommand("init",
- makeCommandVoid2(*this, &ControlManager::init,
+ makeCommandVoid2(*this, &TalosControlManager::init,
docCommandVoid2("Initialize the entity.",
"Time period in seconds (double)",
"Robot reference (string)")));
addCommand("addCtrlMode",
- makeCommandVoid1(*this, &ControlManager::addCtrlMode,
+ makeCommandVoid1(*this, &TalosControlManager::addCtrlMode,
docCommandVoid1("Create an input signal with name 'ctrl_x' where x is the specified name.",
"Name (string)")));
addCommand("ctrlModes",
- makeCommandVoid0(*this, &ControlManager::ctrlModes,
+ makeCommandVoid0(*this, &TalosControlManager::ctrlModes,
docCommandVoid0("Get a list of all the available control modes.")));
addCommand("setCtrlMode",
- makeCommandVoid2(*this, &ControlManager::setCtrlMode,
+ makeCommandVoid2(*this, &TalosControlManager::setCtrlMode,
docCommandVoid2("Set the control mode for a joint.",
"(string) joint name",
"(string) control mode")));
addCommand("getCtrlMode",
- makeCommandVoid1(*this, &ControlManager::getCtrlMode,
+ makeCommandVoid1(*this, &TalosControlManager::getCtrlMode,
docCommandVoid1("Get the control mode of a joint.",
"(string) joint name")));
addCommand("resetProfiler",
- makeCommandVoid0(*this, &ControlManager::resetProfiler,
+ makeCommandVoid0(*this, &TalosControlManager::resetProfiler,
docCommandVoid0("Reset the statistics computed by the profiler (print this entity to see them).")));
addCommand("addEmergencyStopSIN",
- makeCommandVoid1(*this, &ControlManager::addEmergencyStopSIN,
+ makeCommandVoid1(*this, &TalosControlManager::addEmergencyStopSIN,
docCommandVoid1("Add emergency signal input from another entity that can stop the control if necessary.",
"(string) signal name : 'emergencyStop_' + name")));
addCommand("isEmergencyStopTriggered", makeDirectGetter(*this,&m_emergency_stop_triggered, docDirectGetter("Check whether emergency stop is triggered","bool")));
}
- void ControlManager::init(const double & dt,
+ void TalosControlManager::init(const double & dt,
const std::string &robotRef)
{
if(dt<=0.0)
@@ -129,7 +129,7 @@ namespace dynamicgraph
}
m_numDofs = m_robot_util->m_nbJoints + 6;
- //ControlManager::setLoggerVerbosityLevel((dynamicgraph::LoggerVerbosity) 4);
+ //TalosControlManager::setLoggerVerbosityLevel((dynamicgraph::LoggerVerbosity) 4);
m_jointCtrlModes_current.resize(m_numDofs);
m_jointCtrlModes_previous.resize(m_numDofs);
m_jointCtrlModesCountDown.resize(m_numDofs,0);
@@ -252,7 +252,7 @@ namespace dynamicgraph
/* --- COMMANDS ---------------------------------------------------------- */
- void ControlManager::addCtrlMode(const string& name)
+ void TalosControlManager::addCtrlMode(const string& name)
{
// check there is no other control mode with the same name
for(unsigned int i=0;i<m_ctrlModes.size(); i++)
@@ -280,13 +280,13 @@ namespace dynamicgraph
updateJointCtrlModesOutputSignal();
}
- void ControlManager::ctrlModes()
+ void TalosControlManager::ctrlModes()
{
SEND_MSG(toString(m_ctrlModes), MSG_TYPE_INFO);
}
- void ControlManager::setCtrlMode(const string& jointName, const string& ctrlMode)
+ void TalosControlManager::setCtrlMode(const string& jointName, const string& ctrlMode)
{
CtrlMode cm;
if(convertStringToCtrlMode(ctrlMode,cm)==false)
@@ -316,7 +316,7 @@ namespace dynamicgraph
updateJointCtrlModesOutputSignal();
}
- void ControlManager::setCtrlMode(const int jid, const CtrlMode& cm)
+ void TalosControlManager::setCtrlMode(const int jid, const CtrlMode& cm)
{
if(m_jointCtrlModesCountDown[jid]!=0)
return SEND_MSG("Cannot change control mode of joint "+toString(jid)+
@@ -341,7 +341,7 @@ namespace dynamicgraph
}
}
- void ControlManager::getCtrlMode(const std::string& jointName)
+ void TalosControlManager::getCtrlMode(const std::string& jointName)
{
if(jointName=="all")
{
@@ -358,25 +358,25 @@ namespace dynamicgraph
SEND_MSG("The control mode of joint "+jointName+" is "+m_jointCtrlModes_current[i].name,MSG_TYPE_INFO);
}
- void ControlManager::resetProfiler()
+ void TalosControlManager::resetProfiler()
{
getProfiler().reset_all();
getStatistics().reset_all();
}
-// void ControlManager::setStreamPrintPeriod(const double & s)
+// void TalosControlManager::setStreamPrintPeriod(const double & s)
// {
// getLogger().setStreamPrintPeriod(s);
// }
- void ControlManager::setSleepTime(const double &seconds)
+ void TalosControlManager::setSleepTime(const double &seconds)
{
if(seconds<0.0)
return SEND_MSG("Sleep time cannot be negative!", MSG_TYPE_ERROR);
m_sleep_time = seconds;
}
- void ControlManager::addEmergencyStopSIN(const string& name)
+ void TalosControlManager::addEmergencyStopSIN(const string& name)
{
SEND_MSG("New emergency signal input emergencyStop_" + name + " created",MSG_TYPE_INFO);
// create a new input signal
@@ -390,7 +390,7 @@ namespace dynamicgraph
/* --- PROTECTED MEMBER METHODS ---------------------------------------------------------- */
- void ControlManager::updateJointCtrlModesOutputSignal()
+ void TalosControlManager::updateJointCtrlModesOutputSignal()
{
if (m_numDofs==0)
{
@@ -416,7 +416,7 @@ namespace dynamicgraph
}
- bool ControlManager::convertStringToCtrlMode(const std::string& name, CtrlMode& cm)
+ bool TalosControlManager::convertStringToCtrlMode(const std::string& name, CtrlMode& cm)
{
// Check if the ctrl mode name exists
for(unsigned int i=0;i<m_ctrlModes.size(); i++)
@@ -430,7 +430,7 @@ namespace dynamicgraph
return false;
}
- bool ControlManager::convertJointNameToJointId(const std::string& name, unsigned int& id)
+ bool TalosControlManager::convertJointNameToJointId(const std::string& name, unsigned int& id)
{
// Check if the joint name exists
int jid = int(m_robot_util->get_id_from_name(name)); // cast needed due to bug in robot-utils
@@ -449,7 +449,7 @@ namespace dynamicgraph
}
/*
- bool ControlManager::isJointInRange(unsigned int id, double q)
+ bool TalosControlManager::isJointInRange(unsigned int id, double q)
{
const JointLimits & JL = m_robot_util->get_joint_limits_from_id((Index)id);
@@ -475,9 +475,9 @@ namespace dynamicgraph
/* ------------------------------------------------------------------- */
- void ControlManager::display(std::ostream& os) const
+ void TalosControlManager::display(std::ostream& os) const
{
- os << "ControlManager "<<getName();
+ os << "TalosControlManager "<<getName();
try
{
getProfiler().report_all(3, os);
diff --git a/src/utils/causal-filter.cpp b/src/utils/causal-filter.cpp
deleted file mode 100644
index d67397df9ebb1cd4a251fd6cfe0ae77639a2e56b..0000000000000000000000000000000000000000
--- a/src/utils/causal-filter.cpp
+++ /dev/null
@@ -1,128 +0,0 @@
-/*
- * Copyright 2017-, Rohan Budhiraja LAAS-CNRS
- *
- * This file is part of sot-torque-control.
- * sot-torque-control is free software: you can redistribute it and/or
- * modify it under the terms of the GNU Lesser General Public License
- * as published by the Free Software Foundation, either version 3 of
- * the License, or (at your option) any later version.
- * sot-torque-control is distributed in the hope that it will be
- * useful, but WITHOUT ANY WARRANTY; without even the implied warranty
- * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU Lesser General Public License for more details. You should
- * have received a copy of the GNU Lesser General Public License along
- * with sot-torque-control. If not, see <http://www.gnu.org/licenses/>.
- */
-
-#include <iostream>
-
-#include <sot/talos_balance/utils/causal-filter.hh>
-
-
-
-/*
-Filter data with an IIR or FIR filter.
-
-Filter a data sequence, x, using a digital filter. The filter is a direct form II transposed implementation of the standard difference equation.
-This means that the filter implements:
-
-a[0]*y[N] = b[0]*x[N] + b[1]*x[N-1] + ... + b[m-1]*x[N-(m-1)]
- - a[1]*y[N-1] - ... - a[n-1]*y[N-(n-1)]
-
-where m is the degree of the numerator, n is the degree of the denominator, and N is the sample number
-
-*/
-
-CausalFilter::CausalFilter(const double ×tep,
- const int& xSize,
- const Eigen::VectorXd& filter_numerator,
- const Eigen::VectorXd& filter_denominator)
-
- : m_dt(timestep)
- , m_x_size(xSize)
- , m_filter_order_m(filter_numerator.size())
- , m_filter_order_n(filter_denominator.size())
- , m_filter_numerator(filter_numerator)
- , m_filter_denominator(filter_denominator)
- , first_sample(true)
- , pt_numerator(0)
- , pt_denominator(0)
- , input_buffer(Eigen::MatrixXd::Zero(xSize, filter_numerator.size()))
- , output_buffer(Eigen::MatrixXd::Zero(xSize, filter_denominator.size()-1))
-{
- assert(timestep>0.0 && "Timestep should be > 0");
- assert(m_filter_numerator.size() == m_filter_order_m);
- assert(m_filter_denominator.size() == m_filter_order_n);
-}
-
-
-void CausalFilter::get_x_dx_ddx(const Eigen::VectorXd& base_x,
- Eigen::VectorXd& x_output_dx_ddx)
-{
- //const dynamicgraph::Vector &base_x = m_xSIN(iter);
- if(first_sample)
- {
- for(int i=0;i<m_filter_order_m; i++)
- input_buffer.col(i) = base_x;
- for(int i=0;i<m_filter_order_n-1; i++)
- output_buffer.col(i) = base_x*m_filter_numerator.sum()/m_filter_denominator.sum();
- first_sample = false;
- }
-
- input_buffer.col(pt_numerator) = base_x;
-
- Eigen::VectorXd b(m_filter_order_m);
- Eigen::VectorXd a(m_filter_order_n-1);
- b.head(pt_numerator+1) = m_filter_numerator.head(pt_numerator+1).reverse();
- b.tail(m_filter_order_m-pt_numerator-1) =
- m_filter_numerator.tail(m_filter_order_m-pt_numerator-1).reverse();
-
- a.head(pt_denominator+1) = m_filter_denominator.segment(1, pt_denominator+1).reverse();
- a.tail(m_filter_order_n-pt_denominator-2) =
- m_filter_denominator.tail(m_filter_order_n-pt_denominator-2).reverse();
- x_output_dx_ddx.head(m_x_size) = (input_buffer*b-output_buffer*a)/m_filter_denominator[0];
-
- //Finite Difference
- Eigen::Index pt_denominator_prev = (pt_denominator == 0) ? m_filter_order_n-2 : pt_denominator-1;
- x_output_dx_ddx.segment(m_x_size,m_x_size) = (x_output_dx_ddx.head(m_x_size)-output_buffer.col(pt_denominator))/m_dt;
- x_output_dx_ddx.tail(m_x_size) = (x_output_dx_ddx.head(m_x_size)-2*output_buffer.col(pt_denominator)+output_buffer.col(pt_denominator_prev))/m_dt/m_dt;
-
- pt_numerator = (pt_numerator+1) < m_filter_order_m ? (pt_numerator+1) : 0;
- pt_denominator = (pt_denominator+1) < m_filter_order_n-1 ? (pt_denominator+1) : 0;
- output_buffer.col(pt_denominator) = x_output_dx_ddx.head(m_x_size);
- return;
-}
-
-
-
-void CausalFilter::switch_filter(const Eigen::VectorXd& filter_numerator,
- const Eigen::VectorXd& filter_denominator)
-{
- Eigen::Index filter_order_m = filter_numerator.size();
- Eigen::Index filter_order_n = filter_denominator.size();
-
- Eigen::VectorXd current_x(input_buffer.col(pt_numerator));
-
- input_buffer.resize(Eigen::NoChange, filter_order_m);
- output_buffer.resize(Eigen::NoChange, filter_order_n-1);
-
- for(int i=0;i<filter_order_m; i++)
- input_buffer.col(i) = current_x;
-
- for(int i=0;i<filter_order_n-1; i++)
- output_buffer.col(i) = current_x*filter_numerator.sum()/filter_denominator.sum();
-
-
- m_filter_order_m = filter_order_m;
- m_filter_numerator.resize(filter_order_m);
- m_filter_numerator = filter_numerator;
-
- m_filter_order_n = filter_order_n;
- m_filter_denominator.resize(filter_order_n);
- m_filter_denominator = filter_denominator;
-
- pt_numerator = 0;
- pt_denominator = 0;
-
- return;
-}
diff --git a/src/utils/stop-watch.cpp b/src/utils/stop-watch.cpp
deleted file mode 100644
index c6d846b163f03462a3e926cf90b11a104c83fa8c..0000000000000000000000000000000000000000
--- a/src/utils/stop-watch.cpp
+++ /dev/null
@@ -1,347 +0,0 @@
-/*
-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 WIN32
- #include <sys/time.h>
-#else
- #include <Windows.h>
- #include <iomanip>
-#endif
-
-#include <iomanip> // std::setprecision
-#include "sot/talos_balance/utils/stop-watch.hh"
-
-using std::map;
-using std::string;
-using std::ostringstream;
-
-//#define START_PROFILER(name) getProfiler().start(name)
-//#define STOP_PROFILER(name) getProfiler().stop(name)
-
-Stopwatch& getProfiler()
-{
- static Stopwatch s(REAL_TIME); // alternatives are CPU_TIME and REAL_TIME
- return s;
-}
-
-Stopwatch::Stopwatch(StopwatchMode _mode)
- : active(true), mode(_mode)
-{
- records_of = new map<string, PerformanceData>();
-}
-
-Stopwatch::~Stopwatch()
-{
- delete records_of;
-}
-
-void Stopwatch::set_mode(StopwatchMode new_mode)
-{
- mode = new_mode;
-}
-
-bool Stopwatch::performance_exists(string perf_name)
-{
- return (records_of->find(perf_name) != records_of->end());
-}
-
-long double Stopwatch::take_time()
-{
- if ( mode == CPU_TIME ) {
-
- // Use ctime
- return clock();
-
- } else if ( mode == REAL_TIME ) {
-
- // Query operating system
-
-#ifdef WIN32
- /* In case of usage under Windows */
- FILETIME ft;
- LARGE_INTEGER intervals;
-
- // Get the amount of 100 nanoseconds intervals elapsed since January 1, 1601
- // (UTC)
- GetSystemTimeAsFileTime(&ft);
- intervals.LowPart = ft.dwLowDateTime;
- intervals.HighPart = ft.dwHighDateTime;
-
- long double measure = intervals.QuadPart;
- measure -= 116444736000000000.0; // Convert to UNIX epoch time
- measure /= 10000000.0; // Convert to seconds
-
- return measure;
-#else
- /* Linux, MacOS, ... */
- struct timeval tv;
- gettimeofday(&tv, NULL);
-
- long double measure = tv.tv_usec;
- measure /= 1000000.0; // Convert to seconds
- measure += tv.tv_sec; // Add seconds part
-
- return measure;
-#endif
-
- } else {
- // If mode == NONE, clock has not been initialized, then throw exception
- throw StopwatchException("Clock not initialized to a time taking mode!");
- }
-}
-
-void Stopwatch::start(string perf_name)
-{
- if (!active) return;
-
- // Just works if not already present
- records_of->insert(make_pair(perf_name, PerformanceData()));
-
- PerformanceData& perf_info = records_of->find(perf_name)->second;
-
- // Take ctime
- perf_info.clock_start = take_time();
-
- // If this is a new start (i.e. not a restart)
-// if (!perf_info.paused)
-// perf_info.last_time = 0;
-
- perf_info.paused = false;
-}
-
-void Stopwatch::stop(string perf_name)
-{
- if (!active) return;
-
- long double clock_end = take_time();
-
- // Try to recover performance data
- if ( !performance_exists(perf_name) )
- throw StopwatchException("Performance not initialized.");
-
- PerformanceData& perf_info = records_of->find(perf_name)->second;
-
- // check whether the performance has been reset
- if(perf_info.clock_start==0)
- return;
-
- perf_info.stops++;
- long double lapse = clock_end - perf_info.clock_start;
-
- if ( mode == CPU_TIME )
- lapse /= (double) CLOCKS_PER_SEC;
-
- // Update last time
- perf_info.last_time = lapse;
-
- // Update min/max time
- if ( lapse >= perf_info.max_time )
- perf_info.max_time = lapse;
- if ( lapse <= perf_info.min_time || perf_info.min_time == 0 )
- perf_info.min_time = lapse;
-
- // Update total time
- perf_info.total_time += lapse;
-}
-
-void Stopwatch::pause(string perf_name)
-{
- if (!active) return;
-
- long double clock_end = clock();
-
- // Try to recover performance data
- if ( !performance_exists(perf_name) )
- throw StopwatchException("Performance not initialized.");
-
- PerformanceData& perf_info = records_of->find(perf_name)->second;
-
- // check whether the performance has been reset
- if(perf_info.clock_start==0)
- return;
-
- long double lapse = clock_end - perf_info.clock_start;
-
- // Update total time
- perf_info.last_time += lapse;
- perf_info.total_time += lapse;
-}
-
-void Stopwatch::reset_all()
-{
- if (!active) return;
-
- map<string, PerformanceData>::iterator it;
-
- for (it = records_of->begin(); it != records_of->end(); ++it) {
- reset(it->first);
- }
-}
-
-void Stopwatch::report_all(int precision, std::ostream& output)
-{
- if (!active) return;
-
- output<< "\n*** PROFILING RESULTS [ms] (min - avg - max - lastTime - nSamples - totalTime) ***\n";
- map<string, PerformanceData>::iterator it;
- for (it = records_of->begin(); it != records_of->end(); ++it) {
- report(it->first, precision, output);
- }
-}
-
-void Stopwatch::reset(string perf_name)
-{
- if (!active) return;
-
- // Try to recover performance data
- if ( !performance_exists(perf_name) )
- throw StopwatchException("Performance not initialized.");
-
- PerformanceData& perf_info = records_of->find(perf_name)->second;
-
- perf_info.clock_start = 0;
- perf_info.total_time = 0;
- perf_info.min_time = 0;
- perf_info.max_time = 0;
- perf_info.last_time = 0;
- perf_info.paused = false;
- perf_info.stops = 0;
-}
-
-void Stopwatch::turn_on()
-{
- std::cout << "Stopwatch active." << std::endl;
- active = true;
-}
-
-void Stopwatch::turn_off()
-{
- std::cout << "Stopwatch inactive." << std::endl;
- active = false;
-}
-
-void Stopwatch::report(string perf_name, int precision, std::ostream& output)
-{
- if (!active) return;
-
- // Try to recover performance data
- if ( !performance_exists(perf_name) )
- throw StopwatchException("Performance not initialized.");
-
- PerformanceData& perf_info = records_of->find(perf_name)->second;
-
- string pad = "";
- for (size_t i = perf_name.length(); i<STOP_WATCH_MAX_NAME_LENGTH; i++)
- pad.append(" ");
-
- output << perf_name << pad;
- output << std::fixed << std::setprecision(precision)
- << (perf_info.min_time*1e3) << "\t";
- output << std::fixed << std::setprecision(precision)
- << (perf_info.total_time*1e3 / (long double) perf_info.stops) << "\t";
- output << std::fixed << std::setprecision(precision)
- << (perf_info.max_time*1e3) << "\t";
- output << std::fixed << std::setprecision(precision)
- << (perf_info.last_time*1e3) << "\t";
- output << std::fixed << std::setprecision(precision)
- << perf_info.stops << std::endl;
- output << std::fixed << std::setprecision(precision)
- << perf_info.total_time*1e3 << std::endl;
-}
-
-long double Stopwatch::get_time_so_far(string perf_name)
-{
- // Try to recover performance data
- if ( !performance_exists(perf_name) )
- throw StopwatchException("Performance not initialized.");
-
- long double lapse =
- (take_time() - (records_of->find(perf_name)->second).clock_start);
-
- if (mode == CPU_TIME)
- lapse /= (double) CLOCKS_PER_SEC;
-
- return lapse;
-}
-
-long double Stopwatch::get_total_time(string perf_name)
-{
- // Try to recover performance data
- if ( !performance_exists(perf_name) )
- throw StopwatchException("Performance not initialized.");
-
- PerformanceData& perf_info = records_of->find(perf_name)->second;
-
- return perf_info.total_time;
-
-}
-
-long double Stopwatch::get_average_time(string perf_name)
-{
- // Try to recover performance data
- if ( !performance_exists(perf_name) )
- throw StopwatchException("Performance not initialized.");
-
- PerformanceData& perf_info = records_of->find(perf_name)->second;
-
- return (perf_info.total_time / (long double)perf_info.stops);
-
-}
-
-long double Stopwatch::get_min_time(string perf_name)
-{
- // Try to recover performance data
- if ( !performance_exists(perf_name) )
- throw StopwatchException("Performance not initialized.");
-
- PerformanceData& perf_info = records_of->find(perf_name)->second;
-
- return perf_info.min_time;
-
-}
-
-long double Stopwatch::get_max_time(string perf_name)
-{
- // Try to recover performance data
- if ( !performance_exists(perf_name) )
- throw StopwatchException("Performance not initialized.");
-
- PerformanceData& perf_info = records_of->find(perf_name)->second;
-
- return perf_info.max_time;
-
-}
-
-long double Stopwatch::get_last_time(string perf_name)
-{
- // Try to recover performance data
- if ( !performance_exists(perf_name) )
- throw StopwatchException("Performance not initialized.");
-
- PerformanceData& perf_info = records_of->find(perf_name)->second;
-
- return perf_info.last_time;
-}