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; -}