Commit 92c9d366 authored by Guilhem Saurel's avatar Guilhem Saurel
Browse files

Merge tag 'v1.5.0'

Release of version 1.5.0.
parents a2ac1247 3f81d675
Pipeline #7176 passed with stage
in 44 minutes and 14 seconds
python/cmd
......@@ -4,20 +4,13 @@
# Florent Forget, LAAS/CNRS
# Paul Dandignac, 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
# General Lesser 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/>.
CMAKE_MINIMUM_REQUIRED(VERSION 2.6)
CMAKE_MINIMUM_REQUIRED(VERSION 2.8)
SET(PROJECT_NAMESPACE stack-of-tasks)
SET(PROJECT_NAME sot-torque-control)
SET(PROJECT_DESCRIPTION "Collection of dynamic-graph entities aimed at implementing torque control on different robots.")
SET(PROJECT_URL "https://github.com/${PROJECT_NAMESPACE}/${PROJECT_NAME}")
INCLUDE(cmake/base.cmake)
INCLUDE(cmake/boost.cmake)
......@@ -25,19 +18,12 @@ INCLUDE(cmake/eigen.cmake)
INCLUDE(cmake/python.cmake)
INCLUDE(cmake/test.cmake)
SET(PROJECT_NAMESPACE stack-of-tasks)
SET(PROJECT_NAME sot-torque-control)
SET(PROJECT_DESCRIPTION "Collection of dynamic-graph entities aimed at implementing torque control on different robots.")
SET(PROJECT_URL "https://github.com/${PROJECT_NAMESPACE}/${PROJECT_NAME}")
SET(CUSTOM_HEADER_DIR "sot/torque-control")
SET(CUSTOM_HEADER_DIR "sot/torque_control")
SET(DOXYGEN_USE_MATHJAX YES)
# Disable -Werror on Unix for now.
SET(CXX_DISABLE_WERROR True)
#add_compile_options(-std=c++11) # CMake 2.8.12 or newer
#set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
SET(PKG_CONFIG_ADDITIONAL_VARIABLES
${PKG_CONFIG_ADDITIONAL_VARIABLES}
......@@ -45,7 +31,8 @@ SET(PKG_CONFIG_ADDITIONAL_VARIABLES
plugindir
)
SETUP_PROJECT()
CMAKE_POLICY(SET CMP0048 OLD)
PROJECT(${PROJECT_NAME} CXX)
OPTION (INITIALIZE_WITH_NAN "Initialize Eigen entries with NaN" ON)
IF(INITIALIZE_WITH_NAN)
......@@ -59,14 +46,17 @@ PKG_CONFIG_APPEND_LIBS("sot-torque-control")
# Boost
SET(BOOST_COMPONENTS thread filesystem program_options unit_test_framework system regex)
# Default option for python.
OPTION (BUILD_PYTHON_INTERFACE "Build the python bindings" ON)
OPTION (INSTALL_PYTHON_INTERFACE_ONLY "Install *ONLY* the python binding" OFF)
IF(BUILD_PYTHON_INTERFACE)
FINDPYTHON()
SET(BOOST_COMPONENTS ${BOOST_COMPONENTS} python)
ADD_REQUIRED_DEPENDENCY("dynamic-graph-python >= 3.0.0")
SET(${LIBRARY_NAME}_PYTHON_FILES python/*.py)
SET(PYTHON_INSTALL_DIR ${CMAKE_INSTALL_PREFIX}/${PYTHON_SITELIB}/dynamic_graph/sot/torque_control)
INCLUDE_DIRECTORIES(${PYTHON_INCLUDE_DIR})
SET(PYTHON_LIBRARY_DIR dynamic_graph/sot/torque_control)
SET(PYTHON_INSTALL_DIR ${PYTHON_SITELIB}/${PYTHON_LIBRARY_DIR})
INCLUDE_DIRECTORIES(SYSTEM ${PYTHON_INCLUDE_DIR})
ENDIF(BUILD_PYTHON_INTERFACE)
SEARCH_FOR_BOOST()
......@@ -84,139 +74,13 @@ ADD_OPTIONAL_DEPENDENCY("ddp-actuator-solver")
SET(SOTTORQUECONTROL_LIB_NAME ${PROJECT_NAME})
SET(LIBRARY_NAME ${SOTTORQUECONTROL_LIB_NAME})
SET(${LIBRARY_NAME}_HEADERS
include/sot/torque_control/joint-torque-controller.hh
include/sot/torque_control/base-estimator.hh
include/sot/torque_control/numerical-difference.hh
include/sot/torque_control/filter-differentiator.hh
include/sot/torque_control/motor-model.hh
include/sot/torque_control/joint-trajectory-generator.hh
include/sot/torque_control/nd-trajectory-generator.hh
include/sot/torque_control/se3-trajectory-generator.hh
include/sot/torque_control/free-flyer-locator.hh
include/sot/torque_control/inverse-dynamics-balance-controller.hh
include/sot/torque_control/position-controller.hh
include/sot/torque_control/control-manager.hh
include/sot/torque_control/current-controller.hh
include/sot/torque_control/commands-helper.hh
include/sot/torque_control/madgwickahrs.hh
include/sot/torque_control/device-torque-ctrl.hh
include/sot/torque_control/trace-player.hh
include/sot/torque_control/torque-offset-estimator.hh
include/sot/torque_control/imu_offset_compensation.hh
include/sot/torque_control/admittance-controller.hh
include/sot/torque_control/utils/trajectory-generators.hh
include/sot/torque_control/utils/lin-estimator.hh
include/sot/torque_control/utils/poly-estimator.hh
include/sot/torque_control/utils/quad-estimator.hh
include/sot/torque_control/utils/causal-filter.hh
include/sot/torque_control/utils/Stdafx.hh
include/sot/torque_control/utils/stop-watch.hh
include/sot/torque_control/utils/vector-conversions.hh
)
#INSTALL(FILES ${${LIBRARY_NAME}_HEADERS}
# DESTINATION include/sot/torque-control
# PERMISSIONS OWNER_READ GROUP_READ WORLD_READ OWNER_WRITE)
SET(${LIBRARY_NAME}_SOURCES ${${LIBRARY_NAME}_HEADERS}
src/trajectory-generators.cpp
src/lin-estimator.cpp
src/poly-estimator.cpp
src/quad-estimator.cpp
src/causal-filter.cpp
src/stop-watch.cpp
src/motor-model.cpp
)
ADD_LIBRARY(${LIBRARY_NAME} SHARED ${${LIBRARY_NAME}_SOURCES})
SET_TARGET_PROPERTIES(${LIBRARY_NAME}
PROPERTIES
SOVERSION ${PROJECT_VERSION}
INSTALL_RPATH ${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_LIBDIR})
PKG_CONFIG_USE_DEPENDENCY(${LIBRARY_NAME} dynamic-graph)
PKG_CONFIG_USE_DEPENDENCY(${LIBRARY_NAME} sot-core)
PKG_CONFIG_USE_DEPENDENCY(${LIBRARY_NAME} pinocchio)
PKG_CONFIG_USE_DEPENDENCY(${LIBRARY_NAME} tsid)
PKG_CONFIG_USE_DEPENDENCY(${LIBRARY_NAME} parametric-curves)
IF(BUILD_PYTHON_INTERFACE)
PKG_CONFIG_USE_DEPENDENCY(${LIBRARY_NAME} dynamic-graph-python)
ADD_SUBDIRECTORY(python/${PYTHON_LIBRARY_DIR})
ENDIF(BUILD_PYTHON_INTERFACE)
IF(UNIX)
TARGET_LINK_LIBRARIES(${LIBRARY_NAME} ${CMAKE_DL_LIBS})
ENDIF(UNIX)
IF(UNIX AND NOT APPLE)
TARGET_LINK_LIBRARIES(${LIBRARY_NAME} pthread)
ENDIF(UNIX AND NOT APPLE)
TARGET_LINK_LIBRARIES(${LIBRARY_NAME} ${Boost_LIBRARIES})
INSTALL(TARGETS ${LIBRARY_NAME} DESTINATION ${CMAKE_INSTALL_LIBDIR})
IF(BUILD_PYTHON_INTERFACE)
INSTALL(FILES python/dynamic_graph/sot/torque_control/__init__.py
python/dynamic_graph/sot/torque_control/main.py
python/dynamic_graph/sot/torque_control/create_entities_utils.py
python/dynamic_graph/sot/torque_control/main_sim.py
DESTINATION ${PYTHON_SITELIB}/dynamic_graph/sot/torque_control)
INSTALL(FILES python/dynamic_graph/sot/torque_control/identification/__init__.py
python/dynamic_graph/sot/torque_control/identification/identification_utils.py
DESTINATION ${PYTHON_SITELIB}/dynamic_graph/sot/torque_control/identification)
INSTALL(FILES python/dynamic_graph/sot/torque_control/utils/__init__.py
python/dynamic_graph/sot/torque_control/utils/plot_utils.py
python/dynamic_graph/sot/torque_control/utils/sot_utils.py
python/dynamic_graph/sot/torque_control/utils/filter_utils.py
DESTINATION ${PYTHON_SITELIB}/dynamic_graph/sot/torque_control/utils)
INSTALL(FILES python/dynamic_graph/sot/torque_control/tests/__init__.py
python/dynamic_graph/sot/torque_control/tests/test_control_manager.py
python/dynamic_graph/sot/torque_control/tests/test_balance_ctrl_openhrp.py
python/dynamic_graph/sot/torque_control/tests/test_velocity_filters.py
DESTINATION ${PYTHON_SITELIB}/dynamic_graph/sot/torque_control/tests)
ENDIF(BUILD_PYTHON_INTERFACE)
IF(NOT INSTALL_PYTHON_INTERFACE_ONLY)
ADD_SUBDIRECTORY(include/${CUSTOM_HEADER_DIR})
ENDIF(NOT INSTALL_PYTHON_INTERFACE_ONLY)
ADD_SUBDIRECTORY(src)
ADD_SUBDIRECTORY(unitTesting)
# *****************************
# PYTHON interface *
# *****************************
IF(BUILD_PYTHON_INTERFACE)
IF(TALOS_DATA_FOUND)
FOREACH(py_filename test_torque_offset_estimator)
CONFIGURE_FILE(
${PROJECT_SOURCE_DIR}/python/dynamic_graph/sot/torque_control/tests/${py_filename}.py.in
${PROJECT_BINARY_DIR}/python/dynamic_graph/sot/torque_control/tests/${py_filename}.py
)
INSTALL(FILES ${PROJECT_BINARY_DIR}/python/dynamic_graph/sot/torque_control/tests/${py_filename}.py
DESTINATION ${PYTHON_SITELIB}/dynamic_graph/sot/torque_control/tests)
ENDFOREACH(py_filename)
ENDIF(TALOS_DATA_FOUND)
IF(SIMPLE_HUMANOID_DESCRIPTION_FOUND)
FOREACH(py_filename robot_data_test)
CONFIGURE_FILE(
${PROJECT_SOURCE_DIR}/python/dynamic_graph/sot/torque_control/tests/${py_filename}.py.in
${PROJECT_BINARY_DIR}/python/dynamic_graph/sot/torque_control/tests/${py_filename}.py
)
INSTALL(FILES ${PROJECT_BINARY_DIR}/python/dynamic_graph/sot/torque_control/tests/${py_filename}.py
DESTINATION ${PYTHON_SITELIB}/dynamic_graph/sot/torque_control/tests)
ENDFOREACH(py_filename)
ENDIF(SIMPLE_HUMANOID_DESCRIPTION_FOUND)
ENDIF(BUILD_PYTHON_INTERFACE)
SETUP_PROJECT_FINALIZE()
Subproject commit 61e5574a0615706aab06986f6aecf665ddc31141
Subproject commit 63efaecd7c1fa3f9c190b5365561c38ea33f3236
# 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
# General Lesser 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/>.
SET(${LIBRARY_NAME}_HEADERS
joint-torque-controller.hh
base-estimator.hh
numerical-difference.hh
motor-model.hh
joint-trajectory-generator.hh
se3-trajectory-generator.hh
free-flyer-locator.hh
inverse-dynamics-balance-controller.hh
position-controller.hh
control-manager.hh
current-controller.hh
commands-helper.hh
device-torque-ctrl.hh
trace-player.hh
torque-offset-estimator.hh
imu_offset_compensation.hh
admittance-controller.hh
)
SET(${LIBRARY_NAME}_HEADERS_UTILS
utils/trajectory-generators.hh
utils/lin-estimator.hh
utils/poly-estimator.hh
utils/quad-estimator.hh
utils/Stdafx.hh
utils/vector-conversions.hh
)
INSTALL(FILES ${${LIBRARY_NAME}_HEADERS} DESTINATION include/${CUSTOM_HEADER_DIR})
INSTALL(FILES ${${LIBRARY_NAME}_HEADERS_UTILS} DESTINATION include/${CUSTOM_HEADER_DIR}/utils)
/*
* 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/>.
*/
#ifndef __sot_torque_control_admittance_controller_H__
......@@ -21,16 +10,15 @@
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined (WIN32)
# if defined (sot_admittance_controller_EXPORTS)
# define SOTADMITTANCECONTROLLER_EXPORT __declspec(dllexport)
# else
# define SOTADMITTANCECONTROLLER_EXPORT __declspec(dllimport)
# endif
#if defined(WIN32)
#if defined(sot_admittance_controller_EXPORTS)
#define SOTADMITTANCECONTROLLER_EXPORT __declspec(dllexport)
#else
# define SOTADMITTANCECONTROLLER_EXPORT
#define SOTADMITTANCECONTROLLER_EXPORT __declspec(dllimport)
#endif
#else
#define SOTADMITTANCECONTROLLER_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
......@@ -48,120 +36,113 @@
#include <sot/core/robot-utils.hh>
#include <sot/torque_control/utils/vector-conversions.hh>
namespace dynamicgraph {
namespace sot {
namespace torque_control {
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
class SOTADMITTANCECONTROLLER_EXPORT AdmittanceController
:public::dynamicgraph::Entity
{
typedef AdmittanceController EntityClassName;
DYNAMIC_GRAPH_ENTITY_DECL();
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/* --- CONSTRUCTOR ---- */
AdmittanceController( const std::string & name );
void init(const double& dt, const std::string& robotRef);
/* --- SIGNALS --- */
DECLARE_SIGNAL_IN(encoders, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(jointsVelocities, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(kp_force, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(ki_force, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(kp_vel, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(ki_vel, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(force_integral_saturation, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(force_integral_deadzone, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(fRightFootRef, dynamicgraph::Vector); /// 6d reference force
DECLARE_SIGNAL_IN(fLeftFootRef, dynamicgraph::Vector); /// 6d reference force
DECLARE_SIGNAL_IN(fRightFoot, dynamicgraph::Vector); /// 6d estimated force
DECLARE_SIGNAL_IN(fLeftFoot, dynamicgraph::Vector); /// 6d estimated force
DECLARE_SIGNAL_IN(fRightFootFiltered,dynamicgraph::Vector); /// 6d estimated force filtered
DECLARE_SIGNAL_IN(fLeftFootFiltered, dynamicgraph::Vector); /// 6d estimated force filtered
DECLARE_SIGNAL_IN(controlledJoints, dynamicgraph::Vector); /// mask with 1 for controlled joints, 0 otherwise
DECLARE_SIGNAL_IN(damping, dynamicgraph::Vector); /// damping factors used for the 4 end-effectors
// DECLARE_SIGNAL_IN(fRightHandRef, dynamicgraph::Vector); /// 6d reference force
// DECLARE_SIGNAL_IN(fLeftHandRef, dynamicgraph::Vector); /// 6d reference force
// DECLARE_SIGNAL_IN(fRightHand, dynamicgraph::Vector); /// 6d estimated force
// DECLARE_SIGNAL_IN(fLeftHand, dynamicgraph::Vector); /// 6d estimated force
DECLARE_SIGNAL_OUT(u, dynamicgraph::Vector); /// control
// DEBUG SIGNALS
DECLARE_SIGNAL_OUT(dqDes, dynamicgraph::Vector); /// dqDes = J^+ * Kf * (fRef-f)
DECLARE_SIGNAL_OUT(vDesRightFoot, dynamicgraph::Vector); ///
DECLARE_SIGNAL_OUT(vDesLeftFoot, dynamicgraph::Vector); ///
// DECLARE_SIGNAL_OUT(fRightHandError, dynamicgraph::Vector); /// fRef-f
// DECLARE_SIGNAL_OUT(fLeftHandError, dynamicgraph::Vector); /// fRef-f
/* --- COMMANDS --- */
/* --- ENTITY INHERITANCE --- */
virtual void display( std::ostream& os ) const;
void sendMsg(const std::string& msg, MsgType t=MSG_TYPE_INFO, const char* file="", int line=0)
{
Entity::sendMsg("["+name+"] "+msg, t, file, line);
}
protected:
Eigen::VectorXd m_u; /// control (i.e. motor currents)
bool m_firstIter;
bool m_initSucceeded; /// true if the entity has been successfully initialized
bool m_useJacobianTranspose; /// if true it uses the Jacobian transpose rather than the pseudoinverse
double m_dt; /// control loop time period
int m_nj;
/// robot geometric/inertial data
int m_frame_id_rf; /// frame id of right foot
int m_frame_id_lf; /// frame id of left foot
/// tsid
tsid::robots::RobotWrapper * m_robot;
pinocchio::Data* m_data;
tsid::math::Vector6 m_f_RF; /// desired 6d wrench right foot
tsid::math::Vector6 m_f_LF; /// desired 6d wrench left foot
tsid::math::Vector m_q_urdf;
tsid::math::Vector m_v_urdf;
tsid::math::Vector m_dq_des_urdf;
tsid::math::Vector m_dqErrIntegral; /// integral of the velocity error
// tsid::math::Vector m_dqDesIntegral; /// integral of the desired joint velocities
tsid::math::Vector m_dq_fd; /// joint velocities computed with finite differences
tsid::math::Vector m_qPrev; /// previous value of encoders
typedef pinocchio::Data::Matrix6x Matrix6x;
Matrix6x m_J_RF;
Matrix6x m_J_LF;
Eigen::ColPivHouseholderQR<Matrix6x> m_J_RF_QR;
Eigen::ColPivHouseholderQR<Matrix6x> m_J_LF_QR;
tsid::math::Vector6 m_v_RF_int;
tsid::math::Vector6 m_v_LF_int;
RobotUtilShrPtr m_robot_util;
// tsid::math::Vector3 m_zmp_des_LF; /// 3d desired zmp left foot
// tsid::math::Vector3 m_zmp_des_RF; /// 3d desired zmp left foot
// tsid::math::Vector3 m_zmp_des_LF_local; /// 3d desired zmp left foot expressed in local frame
// tsid::math::Vector3 m_zmp_des_RF_local; /// 3d desired zmp left foot expressed in local frame
// tsid::math::Vector3 m_zmp_des; /// 3d desired global zmp
// tsid::math::Vector3 m_zmp_LF; /// 3d zmp left foot
// tsid::math::Vector3 m_zmp_RF; /// 3d zmp left foot
// tsid::math::Vector3 m_zmp; /// 3d global zmp
}; // class AdmittanceController
} // namespace torque_control
} // namespace sot
} // namespace dynamicgraph
#endif // #ifndef __sot_torque_control_admittance_controller_H__
namespace sot {
namespace torque_control {
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
class SOTADMITTANCECONTROLLER_EXPORT AdmittanceController : public ::dynamicgraph::Entity {
typedef AdmittanceController EntityClassName;
DYNAMIC_GRAPH_ENTITY_DECL();
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/* --- CONSTRUCTOR ---- */
AdmittanceController(const std::string& name);
void init(const double& dt, const std::string& robotRef);
/* --- SIGNALS --- */
DECLARE_SIGNAL_IN(encoders, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(jointsVelocities, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(kp_force, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(ki_force, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(kp_vel, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(ki_vel, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(force_integral_saturation, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(force_integral_deadzone, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(fRightFootRef, dynamicgraph::Vector); /// 6d reference force
DECLARE_SIGNAL_IN(fLeftFootRef, dynamicgraph::Vector); /// 6d reference force
DECLARE_SIGNAL_IN(fRightFoot, dynamicgraph::Vector); /// 6d estimated force
DECLARE_SIGNAL_IN(fLeftFoot, dynamicgraph::Vector); /// 6d estimated force
DECLARE_SIGNAL_IN(fRightFootFiltered, dynamicgraph::Vector); /// 6d estimated force filtered
DECLARE_SIGNAL_IN(fLeftFootFiltered, dynamicgraph::Vector); /// 6d estimated force filtered
DECLARE_SIGNAL_IN(controlledJoints, dynamicgraph::Vector); /// mask with 1 for controlled joints, 0 otherwise
DECLARE_SIGNAL_IN(damping, dynamicgraph::Vector); /// damping factors used for the 4 end-effectors
// DECLARE_SIGNAL_IN(fRightHandRef, dynamicgraph::Vector); /// 6d reference force
// DECLARE_SIGNAL_IN(fLeftHandRef, dynamicgraph::Vector); /// 6d reference force
// DECLARE_SIGNAL_IN(fRightHand, dynamicgraph::Vector); /// 6d estimated force
// DECLARE_SIGNAL_IN(fLeftHand, dynamicgraph::Vector); /// 6d estimated force
DECLARE_SIGNAL_OUT(u, dynamicgraph::Vector); /// control
// DEBUG SIGNALS
DECLARE_SIGNAL_OUT(dqDes, dynamicgraph::Vector); /// dqDes = J^+ * Kf * (fRef-f)
DECLARE_SIGNAL_OUT(vDesRightFoot, dynamicgraph::Vector); ///
DECLARE_SIGNAL_OUT(vDesLeftFoot, dynamicgraph::Vector); ///
// DECLARE_SIGNAL_OUT(fRightHandError, dynamicgraph::Vector); /// fRef-f
// DECLARE_SIGNAL_OUT(fLeftHandError, dynamicgraph::Vector); /// fRef-f
/* --- COMMANDS --- */
/* --- ENTITY INHERITANCE --- */
virtual void display(std::ostream& os) const;
void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, const char* file = "", int line = 0) {
Entity::sendMsg("[" + name + "] " + msg, t, file, line);
}
protected:
Eigen::VectorXd m_u; /// control (i.e. motor currents)
bool m_firstIter;
bool m_initSucceeded; /// true if the entity has been successfully initialized
bool m_useJacobianTranspose; /// if true it uses the Jacobian transpose rather than the pseudoinverse
double m_dt; /// control loop time period
int m_nj;
/// robot geometric/inertial data
int m_frame_id_rf; /// frame id of right foot
int m_frame_id_lf; /// frame id of left foot
/// tsid
tsid::robots::RobotWrapper* m_robot;
pinocchio::Data* m_data;
tsid::math::Vector6 m_f_RF; /// desired 6d wrench right foot
tsid::math::Vector6 m_f_LF; /// desired 6d wrench left foot
tsid::math::Vector m_q_urdf;
tsid::math::Vector m_v_urdf;
tsid::math::Vector m_dq_des_urdf;
tsid::math::Vector m_dqErrIntegral; /// integral of the velocity error
// tsid::math::Vector m_dqDesIntegral; /// integral of the desired joint velocities
tsid::math::Vector m_dq_fd; /// joint velocities computed with finite differences
tsid::math::Vector m_qPrev; /// previous value of encoders
typedef pinocchio::Data::Matrix6x Matrix6x;
Matrix6x m_J_RF;
Matrix6x m_J_LF;
Eigen::ColPivHouseholderQR<Matrix6x> m_J_RF_QR;
Eigen::ColPivHouseholderQR<Matrix6x> m_J_LF_QR;
tsid::math::Vector6 m_v_RF_int;
tsid::math::Vector6 m_v_LF_int;
RobotUtilShrPtr m_robot_util;
// tsid::math::Vector3 m_zmp_des_LF; /// 3d desired zmp left foot
// tsid::math::Vector3 m_zmp_des_RF; /// 3d desired zmp left foot
// tsid::math::Vector3 m_zmp_des_LF_local; /// 3d desired zmp left foot expressed in local frame
// tsid::math::Vector3 m_zmp_des_RF_local; /// 3d desired zmp left foot expressed in local frame
// tsid::math::Vector3 m_zmp_des; /// 3d desired global zmp
// tsid::math::Vector3 m_zmp_LF; /// 3d zmp left foot
// tsid::math::Vector3 m_zmp_RF; /// 3d zmp left foot
// tsid::math::Vector3 m_zmp; /// 3d global zmp
}; // class AdmittanceController
} // namespace torque_control
} // namespace sot
} // namespace dynamicgraph
#endif // #ifndef __sot_torque_control_admittance_controller_H__
/*
* Copyright 2015, Andrea Del Prete, LAAS-CNRS
*
* This file is part of sot-torque-control.
* sot-torque-control is free software: you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public License
* as published by the Free Software Foundation, either version 3 of
* the License, or (at your option) any later version.
* sot-torque-control is distributed in the hope that it will be
* useful, but WITHOUT ANY WARRANTY; without even the implied warranty
* of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU Lesser General Public License for more details. You should
* have received a copy of the GNU Lesser General Public License along
* with sot-torque-control. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef __sot_torque_control_control_manager_H__
......@@ -21,26 +10,23 @@
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined (WIN32)
# if defined (__sot_torque_control_control_manager_H__)
# define SOTCONTROLMANAGER_EXPORT __declspec(dllexport)
# else
# define SOTCONTROLMANAGER_EXPORT __declspec(dllimport)
# endif
#if defined(WIN32)
#if defined(__sot_torque_control_control_manager_H__)
#define SOTCONTROLMANAGER_EXPORT __declspec(dllexport)
#else
# define SOTCONTROLMANAGER_EXPORT
#define SOTCONTROLMANAGER_EXPORT __declspec(dllimport)
#endif
#else
#define SOTCONTROLMANAGER_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#include <map>
#include "boost/assign.hpp"
#include <pinocchio/multibody/model.hpp>
#include <pinocchio/parsers/urdf.hpp>
......@@ -51,139 +37,128 @@
#include <sot/torque_control/utils/vector-conversions.hh>
namespace dynamicgraph {
namespace sot {
namespace torque_control {
namespace sot {
namespace torque_control {
/* --------------------------------------------------------------------- */