Skip to content
Snippets Groups Projects

Compare revisions

Changes are shown as if the source revision was being merged into the target revision. Learn more about comparing revisions.

Source

Select target project
No results found

Target

Select target project
  • lscherrer/sot-talos-balance
  • imaroger/sot-talos-balance
  • pfernbac/sot-talos-balance
  • ostasse/sot-talos-balance
  • gsaurel/sot-talos-balance
  • loco-3d/sot-talos-balance
6 results
Show changes
Commits on Source (505)
405 additional commits have been omitted to prevent performance issues.
Hide whitespace changes
Inline Side-by-side
Showing
with 940 additions and 849 deletions
.git-blame-ignore-revs 0 → 100644
View file @ e217f893
# format (Guilhem Saurel, 2022-09-06)
2f737fa7d35bf5cdfb5a2294cd6205a7983e97df
# [Python] format (Guilhem Saurel, 2021-08-18)
319510a50ea4aeb2763481b87885f5d170cb335b
# [Python] format (Guilhem Saurel, 2020-11-14)
f29e3871bb6f6e9d1a39dc229ee2b38dad5964b3
# format (Guilhem Saurel, 2020-11-14)
d7667c2525593be74ef9150c583a81e53edc5e70
# clang-format (Olivier Stasse, 2020-04-25)
f184d9bd1b5f4a52f2e4669456614fa78ad92352
.gitlab-ci.yml 0 → 100644
View file @ e217f893
include: http://rainboard.laas.fr/project/sot-talos-balance/.gitlab-ci.yml
.pre-commit-config.yaml 0 → 100644
View file @ e217f893
ci:
autoupdate_branch: 'devel'
repos:
- repo: https://github.com/pre-commit/mirrors-clang-format
rev: v16.0.3
hooks:
- id: clang-format
args: [--style=Google]
- repo: https://github.com/pre-commit/pre-commit-hooks
rev: v4.4.0
hooks:
- id: check-added-large-files
- id: check-ast
- id: check-executables-have-shebangs
- id: check-json
- id: check-merge-conflict
- id: check-symlinks
- id: check-toml
- id: check-yaml
- id: debug-statements
- id: destroyed-symlinks
- id: detect-private-key
- id: end-of-file-fixer
- id: fix-byte-order-marker
- id: mixed-line-ending
- id: trailing-whitespace
- repo: https://github.com/psf/black
rev: 23.3.0
hooks:
- id: black
- repo: https://github.com/PyCQA/flake8
rev: 6.0.0
hooks:
- id: flake8
- repo: https://github.com/cheshirekow/cmake-format-precommit
rev: v0.6.13
hooks:
- id: cmake-format
CMakeLists.txt
View file @ e217f893
# Copyright 2018, Gepetto team, LAAS-CNRS
# Copyright 2018, 2021, Gepetto team, LAAS-CNRS
#
# This file is part of sot-talos-balance.
# sot-talos-balance is free software: you can redistribute it and/or
# modify it under the terms of the GNU Lesser General Public License
# as published by the Free Software Foundation, either version 3 of
# the License, or (at your option) any later version.
#
# sot-talos-balance is distributed in the hope that it will be useful, but
# WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
# General Lesser Public License for more details. You should have
# received a copy of the GNU Lesser General Public License along with
# sot-talos-balance. If not, see <http://www.gnu.org/licenses/>.
CMAKE_MINIMUM_REQUIRED(VERSION 2.6)
INCLUDE(cmake/base.cmake)
INCLUDE(cmake/boost.cmake)
INCLUDE(cmake/eigen.cmake)
INCLUDE(cmake/python.cmake)
INCLUDE(cmake/test.cmake)
SET(PROJECT_NAME sot-talos-balance)
SET(PROJECT_DESCRIPTION "Collection of dynamic-graph entities aimed at implementing balance control on talos.")
SET(PROJECT_URL "https://gepgitlab.laas.fr/loco-3d/sot-talos-balance")
# So that generated headers are in same place as standard ones
SET(CUSTOM_HEADER_DIR "sot/talos_balance")
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}
plugindirname
plugindir
)
SETUP_PROJECT()
OPTION (INITIALIZE_WITH_NAN "Initialize Eigen entries with NaN" ON)
IF(INITIALIZE_WITH_NAN)
MESSAGE(STATUS "Initialize with NaN all the Eigen entries.")
ADD_DEFINITIONS(-DEIGEN_INITIALIZE_MATRICES_BY_NAN)
ENDIF(INITIALIZE_WITH_NAN)
PKG_CONFIG_APPEND_LIBS("sot-talos-balance")
# Search for dependencies.
# Boost
SET(BOOST_COMPONENTS thread filesystem program_options unit_test_framework system regex)
OPTION (BUILD_PYTHON_INTERFACE "Build the python bindings" ON)
OPTION (BUILD_TEST "Build dynamic-graph architecture tests" ON)
# name of the python module
IF(BUILD_PYTHON_INTERFACE)
SET(SOTTALOSBALANCE_PYNAME sot_talos_balance)
ENDIF(BUILD_PYTHON_INTERFACE)
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}/${SOTTALOSBALANCE_PYNAME})
INCLUDE_DIRECTORIES(${PYTHON_INCLUDE_DIR})
ENDIF(BUILD_PYTHON_INTERFACE)
cmake_minimum_required(VERSION 3.1)
SEARCH_FOR_BOOST()
SEARCH_FOR_EIGEN()
ADD_REQUIRED_DEPENDENCY("dynamic-graph >= 3.0.0")
ADD_REQUIRED_DEPENDENCY("sot-core >= 3.0.0")
ADD_REQUIRED_DEPENDENCY("pinocchio >= 2.1")
ADD_REQUIRED_DEPENDENCY("parametric-curves")
SET(SOTTALOSBALANCE_LIB_NAME ${PROJECT_NAME})
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
include/sot/talos_balance/robot/robot-wrapper.hh
)
#INSTALL(FILES ${${LIBRARY_NAME}_HEADERS}
# DESTINATION include/sot/talos_balance
# 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
# Project properties
set(PROJECT_ORG loco-3d)
set(PROJECT_NAME sot-talos-balance)
set(PROJECT_DESCRIPTION
"Collection of dynamic-graph entities aimed at implementing balance control on talos."
)
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} parametric-curves)
IF(BUILD_PYTHON_INTERFACE)
PKG_CONFIG_USE_DEPENDENCY(${LIBRARY_NAME} dynamic-graph-python)
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/${SOTTALOSBALANCE_PYNAME}/__init__.py
python/${SOTTALOSBALANCE_PYNAME}/main.py
python/${SOTTALOSBALANCE_PYNAME}/create_entities_utils.py
python/${SOTTALOSBALANCE_PYNAME}/meta_task_config.py
python/${SOTTALOSBALANCE_PYNAME}/meta_task_joint.py
python/${SOTTALOSBALANCE_PYNAME}/motor_parameters.py
DESTINATION ${PYTHON_SITELIB}/${SOTTALOSBALANCE_PYNAME})
INSTALL(FILES python/${SOTTALOSBALANCE_PYNAME}/utils/__init__.py
python/${SOTTALOSBALANCE_PYNAME}/utils/control_utils.py
python/${SOTTALOSBALANCE_PYNAME}/utils/plot_utils.py
python/${SOTTALOSBALANCE_PYNAME}/utils/sot_utils.py
python/${SOTTALOSBALANCE_PYNAME}/utils/filter_utils.py
python/${SOTTALOSBALANCE_PYNAME}/utils/run_test_utils.py
python/${SOTTALOSBALANCE_PYNAME}/utils/gazebo_utils.py
DESTINATION ${PYTHON_SITELIB}/${SOTTALOSBALANCE_PYNAME}/utils)
INSTALL(DIRECTORY python/${SOTTALOSBALANCE_PYNAME}/talos/
DESTINATION ${PYTHON_SITELIB}/${SOTTALOSBALANCE_PYNAME}/talos)
IF(BUILD_TEST)
INSTALL(FILES python/${SOTTALOSBALANCE_PYNAME}/test/__init__.py
python/${SOTTALOSBALANCE_PYNAME}/test/test_ffSubscriber.py
python/${SOTTALOSBALANCE_PYNAME}/test/appli_ffSubscriber.py
python/${SOTTALOSBALANCE_PYNAME}/test/test_zmpEstimator.py
python/${SOTTALOSBALANCE_PYNAME}/test/appli_zmpEstimator.py
python/${SOTTALOSBALANCE_PYNAME}/test/test_dcmComControl.py
python/${SOTTALOSBALANCE_PYNAME}/test/appli_dcmComControl.py
python/${SOTTALOSBALANCE_PYNAME}/test/test_dcmComZmpControl.py
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_comAdmittance.py
python/${SOTTALOSBALANCE_PYNAME}/test/test_admittance_single_joint.py
python/${SOTTALOSBALANCE_PYNAME}/test/appli_admittance_single_joint.py
python/${SOTTALOSBALANCE_PYNAME}/test/test_admittance_single_joint_velocity_based.py
python/${SOTTALOSBALANCE_PYNAME}/test/appli_admittance_single_joint_velocity_based.py
python/${SOTTALOSBALANCE_PYNAME}/test/test_singleTraj.py
python/${SOTTALOSBALANCE_PYNAME}/test/test_jointTrajGen.py
python/${SOTTALOSBALANCE_PYNAME}/test/test_jointControl.py
python/${SOTTALOSBALANCE_PYNAME}/test/test_COMTraj.py
python/${SOTTALOSBALANCE_PYNAME}/test/appli_COMTraj.py
python/${SOTTALOSBALANCE_PYNAME}/test/test_COMTraj_tracer.py
python/${SOTTALOSBALANCE_PYNAME}/test/test_param_server.py
python/${SOTTALOSBALANCE_PYNAME}/test/test_dcm_estimator.py
python/${SOTTALOSBALANCE_PYNAME}/test/appli_dcm_estimator.py
DESTINATION ${PYTHON_SITELIB}/${SOTTALOSBALANCE_PYNAME}/test)
ENDIF(BUILD_TEST)
ENDIF(BUILD_PYTHON_INTERFACE)
MACRO(DYNAMIC_GRAPH_CUSTOM_PYTHON_MODULE MODULENAME SUBMODULENAME LIBRARYNAME TARGETNAME)
# By default the __init__.py file is installed.
SET(INSTALL_INIT_PY 1)
SET(SOURCE_PYTHON_MODULE "cmake/dynamic_graph/python-module-py.cc")
IF("${SUBMODULENAME}" STREQUAL "")
SET(FULLMODULENAME "${MODULENAME}")
ELSE("${SUBMODULENAME}" STREQUAL "")
SET(FULLMODULENAME "${MODULENAME}/${SUBMODULENAME}")
ENDIF("${SUBMODULENAME}" STREQUAL "")
# Check if there is optional parameters.
set(extra_macro_args ${ARGN})
list(LENGTH extra_macro_args num_extra_args)
if( ${num_extra_args} GREATER 0)
list(GET extra_macro_args 0 INSTALL_INIT_PY)
if( ${num_extra_args} GREATER 1)
list(GET extra_macro_args 1 SOURCE_PYTHON_MODULE)
endif(${num_extra_args} GREATER 1)
endif(${num_extra_args} GREATER 0)
IF(NOT DEFINED PYTHONLIBS_FOUND)
FINDPYTHON()
ELSEIF(NOT ${PYTHONLIBS_FOUND} STREQUAL "TRUE")
MESSAGE(FATAL_ERROR "Python has not been found.")
ENDIF()
SET(PYTHON_MODULE ${TARGETNAME})
# We need to set this policy to old to accept wrap target.
CMAKE_POLICY(PUSH)
IF(POLICY CMP0037)
CMAKE_POLICY(SET CMP0037 OLD)
ENDIF()
ADD_LIBRARY(${PYTHON_MODULE}
MODULE
${PROJECT_SOURCE_DIR}/${SOURCE_PYTHON_MODULE})
FILE(MAKE_DIRECTORY ${PROJECT_BINARY_DIR}/src/${FULLMODULENAME})
SET_TARGET_PROPERTIES(${PYTHON_MODULE}
PROPERTIES PREFIX ""
OUTPUT_NAME ${FULLMODULENAME}/wrap
)
CMAKE_POLICY(POP)
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})
#
# Installation
#
SET(PYTHON_INSTALL_DIR ${PYTHON_SITELIB}/${FULLMODULENAME})
INSTALL(TARGETS ${PYTHON_MODULE}
DESTINATION
${PYTHON_INSTALL_DIR})
SET(ENTITY_CLASS_LIST "")
FOREACH (ENTITY ${NEW_ENTITY_CLASS})
SET(ENTITY_CLASS_LIST "${ENTITY_CLASS_LIST}${ENTITY}('')\n")
ENDFOREACH(ENTITY ${NEW_ENTITY_CLASS})
# Install if INSTALL_INIT_PY is set to 1
IF (${INSTALL_INIT_PY} EQUAL 1)
CONFIGURE_FILE(
${PROJECT_SOURCE_DIR}/cmake/dynamic_graph/submodule/__init__.py.cmake
${PROJECT_BINARY_DIR}/src/${FULLMODULENAME}/__init__.py
)
INSTALL(
FILES ${PROJECT_BINARY_DIR}/src/${FULLMODULENAME}/__init__.py
DESTINATION ${PYTHON_INSTALL_DIR}
)
ENDIF(${INSTALL_INIT_PY} EQUAL 1)
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}")
ENDMACRO(SOT_TALOS_BALANCE_PYTHON_MODULE)
ADD_SUBDIRECTORY(src)
# ADD_SUBDIRECTORY(unittest)
SETUP_PROJECT_FINALIZE()
set(PROJECT_URL "https://github.com/${PROJECT_ORG}/${PROJECT_NAME}")
# Project options
option(BUILD_PYTHON_INTERFACE "Build the python bindings" ON)
option(INSTALL_PYTHON_INTERFACE_ONLY "Install *ONLY* the python bindings" OFF)
option(SUFFIX_SO_VERSION "Suffix library name with its version" ON)
option(INITIALIZE_WITH_NAN "Initialize Eigen entries with NaN" OFF)
option(BUILD_ROS_PACKAGES "Build ros packages" ON)
# Project configuration
if(NOT INSTALL_PYTHON_INTERFACE_ONLY)
set(PROJECT_USE_CMAKE_EXPORT TRUE)
endif(NOT INSTALL_PYTHON_INTERFACE_ONLY)
set(CUSTOM_HEADER_DIR "sot/talos_balance")
set(CXX_DISABLE_WERROR TRUE)
set(DOXYGEN_USE_MATHJAX YES)
# Check if the submodule cmake have been initialized
set(JRL_CMAKE_MODULES "${CMAKE_CURRENT_LIST_DIR}/cmake")
if(NOT EXISTS "${CMAKE_SOURCE_DIR}/cmake/base.cmake")
if(${CMAKE_VERSION} VERSION_LESS "3.14.0")
message(
FATAL_ERROR
"\nPlease run the following command first:\ngit submodule update --init\n"
)
else()
message(STATUS "JRL cmakemodules not found. Let's fetch it.")
include(FetchContent)
FetchContent_Declare(
"jrl-cmakemodules"
GIT_REPOSITORY "https://github.com/jrl-umi3218/jrl-cmakemodules.git")
FetchContent_MakeAvailable("jrl-cmakemodules")
FetchContent_GetProperties("jrl-cmakemodules" SOURCE_DIR JRL_CMAKE_MODULES)
endif()
endif()
include("${JRL_CMAKE_MODULES}/base.cmake")
include("${JRL_CMAKE_MODULES}/boost.cmake")
set_default_cmake_build_type("RelWithDebInfo")
# Project definition
compute_project_args(PROJECT_ARGS LANGUAGES CXX C)
project(${PROJECT_NAME} ${PROJECT_ARGS})
check_minimal_cxx_standard(14 REQUIRED)
if(INITIALIZE_WITH_NAN)
message(STATUS "Initialize with NaN all the Eigen entries.")
add_definitions(-DEIGEN_INITIALIZE_MATRICES_BY_NAN)
endif(INITIALIZE_WITH_NAN)
# Project dependencies
if(BUILD_PYTHON_INTERFACE)
add_project_dependency(dynamic-graph-python 4.0.0 REQUIRED)
string(REGEX REPLACE "-" "_" PROJECT_NAME_UNDERSCORE ${PROJECT_NAME})
set(PY_NAME ${PROJECT_NAME_UNDERSCORE})
endif(BUILD_PYTHON_INTERFACE)
if(BUILD_TESTING)
find_package(Boost REQUIRED COMPONENTS unit_test_framework)
find_package(example-robot-data REQUIRED)
endif(BUILD_TESTING)
if(Boost_VERSION GREATER 107299 OR Boost_VERSION_MACRO GREATER 107299)
# Silence a warning about a deprecated use of boost bind by boost >= 1.73
# without dropping support for boost < 1.73
add_definitions(-DBOOST_BIND_GLOBAL_PLACEHOLDERS)
endif()
add_project_dependency(dynamic-graph 4.4.0 REQUIRED)
add_project_dependency(sot-core REQUIRED)
add_project_dependency(eiquadprog REQUIRED)
add_project_dependency(parametric-curves REQUIRED)
# Verbosity level
if(NOT (\"${CMAKE_VERBOSITY_LEVEL}\" STREQUAL \"\"))
add_definitions(-DVP_DEBUG_MODE=${CMAKE_VERBOSITY_LEVEL} -DVP_DEBUG)
endif(NOT (\"${CMAKE_VERBOSITY_LEVEL}\" STREQUAL \"\"))
set(${LIBRARY_NAME}_HEADERS
include/${CUSTOM_HEADER_DIR}/utils/commands-helper.hh
include/${CUSTOM_HEADER_DIR}/utils/statistics.hh
include/${CUSTOM_HEADER_DIR}/math/fwd.hh
include/${CUSTOM_HEADER_DIR}/robot/fwd.hh
include/${CUSTOM_HEADER_DIR}/robot/robot-wrapper.hh
include/${CUSTOM_HEADER_DIR}/sdk_qualisys/Network.h
include/${CUSTOM_HEADER_DIR}/sdk_qualisys/RTPacket.h
include/${CUSTOM_HEADER_DIR}/sdk_qualisys/RTProtocol.h
include/${CUSTOM_HEADER_DIR}/sdk_qualisys/Markup.h)
set(${PROJECT_NAME}_SOURCES
src/utils/statistics.cpp src/robot/robot-wrapper.cpp
src/sdk_qualisys/Network.cpp src/sdk_qualisys/RTPacket.cpp
src/sdk_qualisys/RTProtocol.cpp src/sdk_qualisys/Markup.cpp)
add_library(${PROJECT_NAME} SHARED ${${PROJECT_NAME}_SOURCES}
${${PROJECT_NAME}_HEADERS})
target_include_directories(${PROJECT_NAME} PUBLIC $<INSTALL_INTERFACE:include>)
target_link_libraries(
${PROJECT_NAME} PUBLIC sot-core::sot-core
parametric-curves::parametric-curves)
if(SUFFIX_SO_VERSION)
set_target_properties(${PROJECT_NAME} PROPERTIES SOVERSION ${PROJECT_VERSION})
endif(SUFFIX_SO_VERSION)
if(NOT INSTALL_PYTHON_INTERFACE_ONLY)
install(
TARGETS ${PROJECT_NAME}
EXPORT ${TARGETS_EXPORT_NAME}
DESTINATION lib)
endif(NOT INSTALL_PYTHON_INTERFACE_ONLY)
add_subdirectory(src)
if(BUILD_ROS_PACKAGES AND NOT INSTALL_PYTHON_INTERFACE_ONLY)
add_subdirectory(ros)
endif(BUILD_ROS_PACKAGES AND NOT INSTALL_PYTHON_INTERFACE_ONLY)
if(BUILD_TESTING)
add_subdirectory(tests)
endif(BUILD_TESTING)
if(NOT INSTALL_PYTHON_INTERFACE_ONLY)
install(FILES package.xml DESTINATION share/${PROJECT_NAME})
endif(NOT INSTALL_PYTHON_INTERFACE_ONLY)
README.md
View file @ e217f893
# sot-talos-balance
Coordination project for the control of the balance of Talos.
\ No newline at end of file
[![Pipeline status](https://gitlab.laas.fr/stack-of-tasks/sot-talos-balance/badges/master/pipeline.svg)](https://gitlab.laas.fr/stack-of-tasks/sot-talos-balance/commits/master)
[![Coverage report](https://gitlab.laas.fr/stack-of-tasks/sot-talos-balance/badges/master/coverage.svg?job=doc-coverage)](https://gepettoweb.laas.fr/doc/stack-of-tasks/sot-talos-balance/master/coverage/)
[![Code style: black](https://img.shields.io/badge/code%20style-black-000000.svg)](https://github.com/psf/black)
[![pre-commit.ci status](https://results.pre-commit.ci/badge/github/stack-of-tasks/sot-talos-balance/master.svg)](https://results.pre-commit.ci/latest/github/stack-of-tasks/sot-talos-balance)
Coordination project for the control of the balance of Talos.
## Installation procedure
Assuming you have all the dependencies correctly installed inside `/opt/openrobots`,
the package is installed as follows.
The detailed explanation of the commands is available in the documentation.
```
git clone --recursive git@github.com:loco-3d/sot-talos-balance.git
cd sot-talos-balance
mkdir build
cd build
cmake -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=/opt/openrobots ..
make -j4
make install
cmake @ 17cbc7f7
Compare a33ae519...17cbc7f7
Subproject commit a33ae51993e5bc22ae2e5cd5131f85bd2ee6c8d1
Subproject commit 17cbc7f73afa4471ecf320d9ec8a2b07c4494f3a
doc/Doxyfile.extra.in
View file @ e217f893
......@@ -316,7 +316,7 @@ FILE_PATTERNS = *.cc *.cpp *.h *.hpp *.hh *.dox *.md *.py
# Note that relative paths are relative to the directory from which doxygen is
# run.
EXCLUDE =
EXCLUDE =
# The EXCLUDE_SYMLINKS tag can be used to select whether or not files or
# directories that are symbolic links (a Unix file system feature) are excluded
......@@ -353,13 +353,13 @@ EXCLUDE_PATTERNS += CMake* \
# wildcard * is used, a substring. Examples: ANamespace, AClass,
# AClass::ANamespace, ANamespace::*Test
EXCLUDE_SYMBOLS =
EXCLUDE_SYMBOLS =
# The IMAGE_PATH tag can be used to specify one or more files or
# directories that contain image that are included in the documentation (see
# the \image command).
# IMAGE_PATH =
# IMAGE_PATH =
#---------------------------------------------------------------------------
......@@ -409,8 +409,8 @@ COLS_IN_ALPHA_INDEX = 4
# exceed 55 pixels and the maximum width should not exceed 200 pixels.
# Doxygen will copy the logo to the output directory.
# PROJECT_LOGO =
# HTML_EXTRA_FILES +=
# PROJECT_LOGO =
# HTML_EXTRA_FILES +=
# The HTML_HEADER tag can be used to specify a personal HTML header for
# each generated HTML page. If it is left blank doxygen will generate a
......@@ -423,8 +423,8 @@ COLS_IN_ALPHA_INDEX = 4
# have to redo this when upgrading to a newer version of doxygen or when
# changing the value of configuration settings such as GENERATE_TREEVIEW!
# HTML_HEADER =
# HTML_EXTRA_FILES +=
HTML_HEADER = @PROJECT_SOURCE_DIR@/doc/header.html
# HTML_EXTRA_FILES +=
# The HTML_FOOTER tag can be used to specify a personal HTML footer for
# each generated HTML page. If it is left blank doxygen will generate a
......@@ -451,7 +451,7 @@ HTML_FOOTER =
# the standard style sheet and is therefore more robust against future updates.
# Doxygen will copy the style sheet files to the output directory.
# HTML_EXTRA_STYLESHEET = @PROJECT_SOURCE_DIR@/doc/customdoxygen.css
HTML_EXTRA_STYLESHEET = @PROJECT_SOURCE_DIR@/doc/customdoxygen.css
# The HTML_COLORSTYLE_HUE tag controls the color of the HTML output.
# Doxygen will adjust the colors in the style sheet and background images
......@@ -599,7 +599,7 @@ EXPAND_ONLY_PREDEF = YES
# undefined via #undef or recursively expanded use the := operator
# instead of the = operator.
PREDEFINED +=
PREDEFINED +=
# If the MACRO_EXPANSION and EXPAND_ONLY_PREDEF tags are set to YES then
# this tag can be used to specify a list of macro names that should be expanded.
......@@ -644,4 +644,3 @@ EXAMPLE_PATH = @PROJECT_SOURCE_DIR@/examples
# A dedicated BIB_PATH would be preferable
EXAMPLE_PATH += @PROJECT_SOURCE_DIR@/doc/bib
doc/Overview.md 0 → 100644
View file @ e217f893
# Overview {#index}
<!--
//
// Copyright (c) 2016, 2018 CNRS
// Author: Florent Lamiraux, Justin Carpentier, Guilhem Saurel
//
// This file is part of Pinocchio
// Pinocchio 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.
//
// Pinocchio 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
// Pinocchio If not, see
// <http://www.gnu.org/licenses/>.
-->
\section OverviewIntro Introduction to sot-talos-balance
The library sot-talos-balance allows to execute your trajectory with the help of a stabilizer
You can find the full installation procedure <a href="md_doc_installation.html">in the installation page</a>
Quick instructions on how to run a test can be found <a href="md_doc_running.html">here</a>
doc/customdoxygen.css 0 → 100644
View file @ e217f893
/* Customizing Doxygen output */
/* Needed to allow line breaks in tables*/
.memberdecls {
table-layout: fixed;
width: 100%;
}
/* Needed to break long template names*/
.memTemplItemLeft {
white-space: normal !important;
word-wrap: break-word;
}
/* Needed to break long template names*/
.memItemLeft {
white-space: normal !important;
word-wrap: break-word;
}
doc/header.html 0 → 100644
View file @ e217f893
<!-- HTML header for doxygen 1.8.11-->
<!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Transitional//EN" "http://www.w3.org/TR/xhtml1/DTD/xhtml1-transitional.dtd">
<html xmlns="http://www.w3.org/1999/xhtml">
<head>
<meta http-equiv="Content-Type" content="text/xhtml;charset=UTF-8"/>
<meta http-equiv="X-UA-Compatible" content="IE=9"/>
<meta name="generator" content="Doxygen $doxygenversion"/>
<!--BEGIN PROJECT_NAME--><title>$projectname: $title</title><!--END PROJECT_NAME-->
<!--BEGIN !PROJECT_NAME--><title>$title</title><!--END !PROJECT_NAME-->
<link href="$relpath^tabs.css" rel="stylesheet" type="text/css"/>
<link href="$relpath^pinocchio.ico" rel="icon" type="image/x-icon">
<script type="text/javascript" src="$relpath^jquery.js"></script>
<script type="text/javascript" src="$relpath^dynsections.js"></script>
$treeview
$search
$mathjax
<link href="$relpath^$stylesheet" rel="stylesheet" type="text/css" />
$extrastylesheet
</head>
<body>
<div id="top"><!-- do not remove this div, it is closed by doxygen! -->
<!--BEGIN TITLEAREA-->
<div id="titlearea">
<table cellspacing="0" cellpadding="0">
<tbody>
<tr style="height: 56px;">
<!--BEGIN PROJECT_LOGO-->
<td id="projectlogo"><img alt="Logo" src="$relpath^$projectlogo"/></td>
<!--END PROJECT_LOGO-->
<!--BEGIN PROJECT_NAME-->
<td id="projectalign" style="padding-left: 0.5em;">
<div id="projectname">$projectname
<!--BEGIN PROJECT_NUMBER-->&#160;<span id="projectnumber">$projectnumber</span><!--END PROJECT_NUMBER-->
</div>
<!--BEGIN PROJECT_BRIEF--><div id="projectbrief">$projectbrief</div><!--END PROJECT_BRIEF-->
</td>
<!--END PROJECT_NAME-->
<!--BEGIN !PROJECT_NAME-->
<!--BEGIN PROJECT_BRIEF-->
<td style="padding-left: 0.5em;">
<div id="projectbrief">$projectbrief</div>
</td>
<!--END PROJECT_BRIEF-->
<!--END !PROJECT_NAME-->
<!--BEGIN DISABLE_INDEX-->
<!--BEGIN SEARCHENGINE-->
<td>$searchbox</td>
<!--END SEARCHENGINE-->
<!--END DISABLE_INDEX-->
</tr>
</tbody>
</table>
</div>
<!--END TITLEAREA-->
<!-- end header part -->
doc/installation.md 0 → 100644
View file @ e217f893
# Installation
1. Clone the git repository:
```
git clone --recursive git@gepgitlab.laas.fr:loco-3d/sot-talos-balance.git
cd sot-talos-balance
```
2. If you need it, switch to the devel branch
```
git checkout devel
```
3. Create the build directory and move there
```
mkdir build
cd build
```
4. Run cmake
```
cmake -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=/opt/openrobots ..
```
5. Build the package
```
make -j4
```
6. Install the package
```
make install
```
doc/running.md 0 → 100644
View file @ e217f893
# Running a test
In the following, we quickly demonstrate how to run a test with sot-talos-balance.
## Start the simulation
First of all, you need to start the simulation.
To start Gazebo, load a scene and spawn Talos, the simplest way is to directly use the launch file provided by PAL, without een using sot-talos-balance
```
roslaunch talos_gazebo talos_gazebo.launch
```
Notice that this will spawn Talos at configuration zero. This is not always what you want.
The package sot-talos-balance offers different launch files to spawn it at different configurations.
Most commonly, you might want to spawn the robot in the half-sitting position
```
roslaunch sot_talos_balance talos_gazebo_half_sitting.launch
```
If you ever need a different configuration, all you have to do is taking talos_gazebo_half_sitting.launch, copying it with a different name and modifying it.
## Start the SoT in position mode
To start the SoT in simulation in position mode:
```
roslaunch roscontrol_sot_talos sot_talos_controller_gazebo.launch
```
## Run the test
First of all, you need to go to the folder where your script is.
For instance, for running the standard tests of sot-talos-balance,
assuming you are in the root directory:
```
cd python/sot_talos_balance/test
```
Then, you can just run the chosen test. For instance:
```
python test_dcm_zmp_control.py
```
This will launch a test routine executing a sinusoid and raising the foot.
## Run the test with your own recorded movements
The test that needs to be run in order o execute your own prerecorded movement is
```
python test_dcm_zmp_control.py [testfolder]
```
where `[testfolder]` contains the following files:
```
CoM.dat
LeftFoot.dat
RightFoot.dat
WaistOrientation.dat
optionally, ZMP.dat (if not given, it is computed from the CoM)
```
where the CoM and the ZMP are in the following format:
```
[position3D velocity3D acceleration3D]
```
the feet are
```
[position6D velocity6D acceleration6D]
```
and the waist
```
[orientation3D angular_velocity3D angular_acceleration3D]
```
Pay attention that the lines of each file should *not* have trailing whitespace.
Actually, the velocity and acceleration information are only really needed for the CoM. For all other quantities, these values are not really employed, but they are needed due to how nd-trajectory-generator is implemented. You can set this quantities arbitrarily to zero.
If you are running a simulation, folder `[testfolder]` can be anyways in your computer.
If you are running an experiment on the robot, folder `[testfolder]` must be installed somewhere on the robot itself.
A quick-and-dirty way of installing new motions in the robot is putting them in sot-talos-balance.
In this case, you have to put it in
```
[sot-talos-balance-repo]/ros/sot_talos_balance/data
```
This way, when executing (see <a href="md_doc_installation.html">the installation page</a>)
```
make install
```
it will automatically be copied to
```
/opt/openrobots/share/sot_talos_balance/data
```
which will then be copied to the robot when you update sot-talos-balance on it.
If you which, you can access the motions in sot-talos-balance without specifying the full path, by doing
```
python test_dcm_zmp_control.py -0 [testfolder]
```
which will automatically look in
```
/opt/openrobots/share/sot_talos_balance/data/[testfolder]
```
## Interacting with the dynamic graph
If you want to dynamically interact with the graph
```
rosrun dynamic_graph_bridge run_command
```
## Other
More information on how to use the SoT and how to work on Talos can be found <a href="https://wiki.laas.fr/robots/Pyrene">in the robot wiki page</a> (you need LAAS permissions to access this).
doc/treeview.dox 0 → 100644
View file @ e217f893
//
// Copyright (c) 2016 CNRS
// Author: Florian Valenza
//
// This file is part of Pinocchio
// Pinocchio 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.
//
// Pinocchio 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
// Pinocchio If not, see
// <http://www.gnu.org/licenses/>.
//
// This file strutures pages and modules into a convenient hierarchical structure.
//
namespace dynamicgraph {
namespace sot {
namespace talos_balance {
//
// Pages/ tutorials organization
//
// \page md_doc_installation
// \page md_doc_running
}
}
}
include/sot/talos_balance/admittance-controller-end-effector.hh
View file @ e217f893
......@@ -3,7 +3,7 @@
*
* LAAS-CNRS
*
* Fanny Risbourg
* Fanny Risbourg
* This file is part of sot-talos-balance.
* See license file.
*/
......@@ -15,119 +15,132 @@
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined (WIN32)
# if defined (admittance_controller_end_effector_EXPORTS)
# define ADMITTANCECONTROLLERENDEFFECTOR_EXPORT __declspec(dllexport)
# else
# define ADMITTANCECONTROLLERENDEFFECTOR_EXPORT __declspec(dllimport)
# endif
#if defined(WIN32)
#if defined(admittance_controller_end_effector_EXPORTS)
#define ADMITTANCECONTROLLERENDEFFECTOR_EXPORT __declspec(dllexport)
#else
# define ADMITTANCECONTROLLERENDEFFECTOR_EXPORT
#define ADMITTANCECONTROLLERENDEFFECTOR_EXPORT __declspec(dllimport)
#endif
#else
#define ADMITTANCECONTROLLERENDEFFECTOR_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#include <pinocchio/fwd.hpp>
// include pinocchio first
#include <dynamic-graph/signal-helper.h>
#include <map>
#include "boost/assign.hpp"
#include <pinocchio/parsers/urdf.hpp>
#include <map>
#include <pinocchio/algorithm/center-of-mass.hpp>
#include <pinocchio/algorithm/frames.hpp>
#include <pinocchio/algorithm/kinematics.hpp>
#include <pinocchio/multibody/model.hpp>
#include "pinocchio/spatial/se3.hpp"
#include <pinocchio/parsers/urdf.hpp>
#include <sot/core/robot-utils.hh>
#include <pinocchio/algorithm/kinematics.hpp>
#include "boost/assign.hpp"
#include "pinocchio/spatial/motion.hpp"
#include "pinocchio/spatial/se3.hpp"
namespace dynamicgraph {
namespace sot {
namespace talos_balance {
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/**
* @brief Admittance controller for an upper body end-effector (right or left wrist
*
* This entity computes a velocity reference for an end-effector based on the force error in the world frame :
* dqRef = integral( Kp(forceDes-forceWorldFrame) )
*
*/
class ADMITTANCECONTROLLERENDEFFECTOR_EXPORT AdmittanceControllerEndEffector
: public ::dynamicgraph::Entity
{
DYNAMIC_GRAPH_ENTITY_DECL();
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/* --- CONSTRUCTOR ---- */
AdmittanceControllerEndEffector(const std::string & name);
/* --- SIGNALS --- */
/// \brief Gain (6d) for the integration of the error on the force
DECLARE_SIGNAL_IN(Kp, dynamicgraph::Vector);
/// \brief 6d force given by the sensor in its local frame
DECLARE_SIGNAL_IN(force, dynamicgraph::Vector);
/// \brief 6d desired force of the end-effector in the world frame
DECLARE_SIGNAL_IN(forceDes, dynamicgraph::Vector);
/// \brief Current joint configuration of the robot
DECLARE_SIGNAL_IN(jointPosition, dynamicgraph::Vector);
/// \brief Velocity reference for the end-effector computed by the controller
DECLARE_SIGNAL_OUT(dqRef, dynamicgraph::Vector);
/// \brief 6d force given by the sensor in he global frame
DECLARE_SIGNAL_INNER(forceWorldFrame, dynamicgraph::Vector);
/* --- COMMANDS --- */
/**
* @brief Initialize the entity
*
* @param[in] dt Time step of the control
* @param[in] sensorFrameName Name of the force sensor of the end-effector used in the pinocchio model
*/
void init(const double & dt, const std::string & sensorFrameName);
/**
* @brief Reset the velocity
*/
void reset_dq();
/* --- ENTITY INHERITANCE --- */
virtual void display( std::ostream& os ) const;
protected:
/// Dimension of the force signals and of the output
int m_n;
/// True if the entity has been successfully initialized
bool m_initSucceeded;
/// Internal state
dynamicgraph::Vector m_dq;
/// Time step of the control
double m_dt;
/// Robot Util instance to get the sensor frame
RobotUtil* m_robot_util;
/// Pinocchio robot model
pinocchio::Model m_model;
/// Pinocchio robot data
pinocchio::Data *m_data;
/// Force sensor frame placement wrt the parent frame
pinocchio::SE3 m_sensorFrame;
/// Id of the parent joint of the force sensor frame
pinocchio::JointIndex m_parentId;
/// robot configuration according to pinocchio convention
dynamicgraph::Vector m_q;
}; // class AdmittanceControllerEndEffector
} // namespace talos_balance
} // namespace sot
} // namespace dynamicgraph
#endif // #ifndef __sot_talos_balance_admittance_controller_end_effector_H__
namespace sot {
namespace talos_balance {
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/**
* @brief Admittance controller for an upper body end-effector (right or left
* wrist)
*
* This entity computes a velocity reference for an end-effector based
* on the force error in the world frame :
* w_dq = integral(Kp(w_forceDes-w_force)) + Kd (w_dq)
*
*/
class ADMITTANCECONTROLLERENDEFFECTOR_EXPORT AdmittanceControllerEndEffector
: public ::dynamicgraph::Entity {
DYNAMIC_GRAPH_ENTITY_DECL();
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/* --- CONSTRUCTOR ---- */
AdmittanceControllerEndEffector(const std::string &name);
/* --- SIGNALS --- */
/// \brief Gain (6d) for the integration of the error on the force
DECLARE_SIGNAL_IN(Kp, dynamicgraph::Vector);
/// \brief Derivative gain (6d) for the error on the force
DECLARE_SIGNAL_IN(Kd, dynamicgraph::Vector);
/// \brief Value of the saturation to apply on the velocity output
DECLARE_SIGNAL_IN(dqSaturation, dynamicgraph::Vector);
/// \brief 6d force given by the sensor in its local frame
DECLARE_SIGNAL_IN(force, dynamicgraph::Vector);
/// \brief 6d desired force of the end-effector in the world frame
DECLARE_SIGNAL_IN(w_forceDes, dynamicgraph::Vector);
/// \brief Current joint configuration of the robot
DECLARE_SIGNAL_IN(q, dynamicgraph::Vector);
/// \brief 6d force given by the sensor in the world frame
DECLARE_SIGNAL_INNER(w_force, dynamicgraph::Vector);
/// \brief Internal intergration computed in the world frame
DECLARE_SIGNAL_INNER(w_dq, dynamicgraph::Vector);
/// \brief Velocity reference for the end-effector in the local frame
DECLARE_SIGNAL_OUT(dq, dynamicgraph::Vector);
/* --- COMMANDS --- */
/**
* @brief Initialize the entity
*
* @param[in] dt Time step of the control
* @param[in] sensorFrameName Name of the force sensor of the end-effector
* used in the pinocchio model
* @param[in] endeffectorName Name of the endEffectorJoint
*/
void init(const double &dt, const std::string &sensorFrameName,
const std::string &endeffectorName);
/**
* @brief Reset the velocity
*/
void resetDq();
/* --- ENTITY INHERITANCE --- */
virtual void display(std::ostream &os) const;
protected:
/// Dimension of the force signals and of the output
int m_n;
/// True if the entity has been successfully initialized
bool m_initSucceeded;
/// Internal state
dynamicgraph::Vector m_w_dq;
/// Time step of the control
double m_dt;
// Weight of the end-effector
double m_mass;
/// Robot Util instance to get the sensor frame
RobotUtilShrPtr m_robot_util;
/// Pinocchio robot model
pinocchio::Model m_model;
/// Pinocchio robot data
pinocchio::Data *m_data;
/// Id of the force sensor frame
pinocchio::FrameIndex m_sensorFrameId;
/// Id of the joint of the end-effector
pinocchio::JointIndex m_endEffectorId;
}; // class AdmittanceControllerEndEffector
} // namespace talos_balance
} // namespace sot
} // namespace dynamicgraph
#endif // #ifndef __sot_talos_balance_admittance_controller_end_effector_H__
include/sot/talos_balance/ankle-admittance-controller.hh 0 → 100644
View file @ e217f893
/*
* Copyright 2018, Gepetto team, LAAS-CNRS
*
* This file is part of sot-talos-balance.
* sot-talos-balance is free software: you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public License
* as published by the Free Software Foundation, either version 3 of
* the License, or (at your option) any later version.
* sot-talos-balance is distributed in the hope that it will be
* useful, but WITHOUT ANY WARRANTY; without even the implied warranty
* of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU Lesser General Public License for more details. You should
* have received a copy of the GNU Lesser General Public License along
* with sot-talos-balance. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef __sot_talos_balance_ankle_admittance_controller_H__
#define __sot_talos_balance_ankle_admittance_controller_H__
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined(WIN32)
#if defined(ankle_admittance_controller_EXPORTS)
#define ANKLEADMITTANCECONTROLLER_EXPORT __declspec(dllexport)
#else
#define ANKLEADMITTANCECONTROLLER_EXPORT __declspec(dllimport)
#endif
#else
#define ANKLEADMITTANCECONTROLLER_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#include <dynamic-graph/signal-helper.h>
#include <map>
#include "boost/assign.hpp"
namespace dynamicgraph {
namespace sot {
namespace talos_balance {
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
class ANKLEADMITTANCECONTROLLER_EXPORT AnkleAdmittanceController
: public ::dynamicgraph::Entity {
DYNAMIC_GRAPH_ENTITY_DECL();
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/* --- CONSTRUCTOR ---- */
AnkleAdmittanceController(const std::string& name);
void init();
/* --- SIGNALS --- */
DECLARE_SIGNAL_IN(gainsXY, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(wrench, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(pRef, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(dRP, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(vDes, dynamicgraph::Vector);
/* --- COMMANDS --- */
/* --- ENTITY INHERITANCE --- */
virtual void display(std::ostream& os) const;
protected:
bool
m_initSucceeded; /// true if the entity has been successfully initialized
}; // class AnkleAdmittanceController
} // namespace talos_balance
} // namespace sot
} // namespace dynamicgraph
#endif // #ifndef __sot_talos_balance_ankle_admittance_controller_H__
include/sot/talos_balance/ankle-joint-selector.hh 0 → 100644
View file @ e217f893
/*
* Copyright 2018, Gepetto team, LAAS-CNRS
*
* This file is part of sot-talos-balance.
* sot-talos-balance is free software: you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public License
* as published by the Free Software Foundation, either version 3 of
* the License, or (at your option) any later version.
* sot-talos-balance is distributed in the hope that it will be
* useful, but WITHOUT ANY WARRANTY; without even the implied warranty
* of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU Lesser General Public License for more details. You should
* have received a copy of the GNU Lesser General Public License along
* with sot-talos-balance. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef __sot_talos_balance_ankle_joint_selector_H__
#define __sot_talos_balance_ankle_joint_selector_H__
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined(WIN32)
#if defined(position_controller_EXPORTS)
#define ANKLE_JOINT_SELECTOR_EXPORT __declspec(dllexport)
#else
#define ANKLE_JOINT_SELECTOR_EXPORT __declspec(dllimport)
#endif
#else
#define ANKLE_JOINT_SELECTOR_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#include <dynamic-graph/signal-helper.h>
#include <map>
#include <sot/core/flags.hh>
#include "boost/assign.hpp"
namespace dynamicgraph {
namespace sot {
namespace talos_balance {
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
class ANKLE_JOINT_SELECTOR_EXPORT AnkleJointSelector
: public ::dynamicgraph::Entity {
DYNAMIC_GRAPH_ENTITY_DECL();
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/* --- CONSTRUCTOR ---- */
AnkleJointSelector(const std::string& name);
void init(const int& n);
/* --- SIGNALS --- */
DECLARE_SIGNAL_IN(phase, int);
DECLARE_SIGNAL_IN(rightRollCoupled, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(rightRollDecoupled, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(rightPitchCoupled, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(rightPitchDecoupled, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(leftRollCoupled, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(leftRollDecoupled, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(leftPitchCoupled, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(leftPitchDecoupled, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(selecLeft, Flags);
DECLARE_SIGNAL_OUT(selecRight, Flags);
DECLARE_SIGNAL_OUT(rightRoll, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(rightPitch, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(leftRoll, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(leftPitch, dynamicgraph::Vector);
/* --- COMMANDS --- */
/* --- ENTITY INHERITANCE --- */
virtual void display(std::ostream& os) const;
protected:
Flags m_zeros;
Flags m_ones;
int m_n;
bool
m_initSucceeded; /// true if the entity has been successfully initialized
}; // class AnkleJointSelector
} // namespace talos_balance
} // namespace sot
} // namespace dynamicgraph
#endif // #ifndef __sot_talos_balance_ankle_joint_selector_H__
include/sot/talos_balance/base-estimator.hh deleted 100644 → 0
View file @ 97ac292e
/*
* Copyright 2017, Thomas Flayols, LAAS-CNRS
*
* This file is part of sot-torque-control.
* sot-torque-control is free software: you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public License
* as published by the Free Software Foundation, either version 3 of
* the License, or (at your option) any later version.
* sot-torque-control is distributed in the hope that it will be
* useful, but WITHOUT ANY WARRANTY; without even the implied warranty
* of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU Lesser General Public License for more details. You should
* have received a copy of the GNU Lesser General Public License along
* with sot-torque-control. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef __sot_torque_control_base_estimator_H__
#define __sot_torque_control_base_estimator_H__
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined (WIN32)
# if defined (base_estimator_EXPORTS)
# define SOTBASEESTIMATOR_EXPORT __declspec(dllexport)
# else
# define SOTBASEESTIMATOR_EXPORT __declspec(dllimport)
# endif
#else
# define SOTBASEESTIMATOR_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#include <dynamic-graph/signal-helper.h>
#include <sot/core/matrix-geometry.hh>
#include <map>
#include "boost/assign.hpp"
//#include <boost/random/normal_distribution.hpp>
#include <boost/math/distributions/normal.hpp> // for normal_distribution
/* Pinocchio */
#include <pinocchio/multibody/model.hpp>
#include <pinocchio/parsers/urdf.hpp>
#include <pinocchio/algorithm/kinematics.hpp>
#include <sot/core/robot-utils.hh>
namespace dynamicgraph {
namespace sot {
namespace talos_balance {
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/** Compute s12 as an intermediate transform between s1 and s2 SE3 transforms**/
void se3Interp(const pinocchio::SE3 & s1, const pinocchio::SE3 & s2, const double alpha, pinocchio::SE3 & s12);
/** Convert from Roll, Pitch, Yaw to transformation Matrix. */
void rpyToMatrix(double r, double p, double y, Eigen::Matrix3d & R);
/** Convert from Roll, Pitch, Yaw to transformation Matrix. */
void rpyToMatrix(const Eigen::Vector3d & rpy, Eigen::Matrix3d & R);
/** Convert from Transformation Matrix to Roll, Pitch, Yaw */
void matrixToRpy(const Eigen::Matrix3d & M, Eigen::Vector3d & rpy);
/** Multiply to quaternions stored in (w,x,y,z) format */
void quanternionMult(const Eigen::Vector4d & q1, const Eigen::Vector4d & q2, Eigen::Vector4d & q12);
/** Rotate a point or a vector by a quaternion stored in (w,x,y,z) format */
void pointRotationByQuaternion(const Eigen::Vector3d & point,const Eigen::Vector4d & quat, Eigen::Vector3d & rotatedPoint);
/** Avoids singularity while taking the mean of euler angles**/
double eulerMean(double a1, double a2);
/** Avoids singularity while taking the mean of euler angles**/
double wEulerMean(double a1, double a2, double w1, double w2);
class SOTBASEESTIMATOR_EXPORT BaseEstimator
:public::dynamicgraph::Entity
{
typedef BaseEstimator EntityClassName;
typedef pinocchio::SE3 SE3;
typedef Eigen::Vector2d Vector2;
typedef Eigen::Vector3d Vector3;
typedef Eigen::Vector4d Vector4;
typedef Vector6d Vector6;
typedef Vector7d Vector7;
typedef Eigen::Matrix3d Matrix3;
typedef boost::math::normal normal;
DYNAMIC_GRAPH_ENTITY_DECL();
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/* --- CONSTRUCTOR ---- */
BaseEstimator( const std::string & name );
void init(const double & dt, const std::string& urdfFile);
void set_fz_stable_windows_size(const int & ws);
void set_alpha_w_filter(const double & a);
void set_alpha_DC_acc(const double & a);
void set_alpha_DC_vel(const double & a);
void reset_foot_positions();
void set_imu_weight(const double & w);
void set_zmp_std_dev_right_foot(const double & std_dev);
void set_zmp_std_dev_left_foot(const double & std_dev);
void set_normal_force_std_dev_right_foot(const double & std_dev);
void set_normal_force_std_dev_left_foot(const double & std_dev);
void set_stiffness_right_foot(const dynamicgraph::Vector & k);
void set_stiffness_left_foot(const dynamicgraph::Vector & k);
void set_right_foot_sizes(const dynamicgraph::Vector & s);
void set_left_foot_sizes(const dynamicgraph::Vector & s);
void set_zmp_margin_right_foot(const double & margin);
void set_zmp_margin_left_foot(const double & margin);
void set_normal_force_margin_right_foot(const double & margin);
void set_normal_force_margin_left_foot(const double & margin);
void reset_foot_positions_impl(const Vector6 & ftlf, const Vector6 & ftrf);
void compute_zmp(const Vector6 & w, Vector2 & zmp);
double compute_zmp_weight(const Vector2 & zmp, const Vector4 & foot_sizes,
double std_dev, double margin);
double compute_force_weight(double fz, double std_dev, double margin);
void kinematics_estimation(const Vector6 & ft, const Vector6 & K,
const SE3 & oMfs, const pinocchio::FrameIndex foot_id,
SE3 & oMff, SE3& oMfa, SE3& fsMff);
/* --- SIGNALS --- */
DECLARE_SIGNAL_IN(joint_positions, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(joint_velocities, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(imu_quaternion, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(forceLLEG, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(forceRLEG, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(dforceLLEG, dynamicgraph::Vector); ///derivative of left force torque sensor
DECLARE_SIGNAL_IN(dforceRLEG, dynamicgraph::Vector); ///derivative of right force torque sensor
DECLARE_SIGNAL_IN(w_lf_in, double); /// weight of the estimation coming from the left foot
DECLARE_SIGNAL_IN(w_rf_in, double); /// weight of the estimation coming from the right foot
DECLARE_SIGNAL_IN(K_fb_feet_poses, double); /// feed back gain to correct feet position according to last base estimation and kinematic
DECLARE_SIGNAL_IN(lf_ref_xyzquat, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(rf_ref_xyzquat, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(accelerometer, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(gyroscope, dynamicgraph::Vector);
DECLARE_SIGNAL_INNER(kinematics_computations, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(q, dynamicgraph::Vector); /// n+6 robot configuration with base6d in RPY
DECLARE_SIGNAL_OUT(v, dynamicgraph::Vector); /// n+6 robot velocities
DECLARE_SIGNAL_OUT(v_kin, dynamicgraph::Vector); /// 6d robot velocities from kinematic only (encoders derivative)
DECLARE_SIGNAL_OUT(v_flex, dynamicgraph::Vector); /// 6d robot velocities from flexibility only (force sensor derivative)
DECLARE_SIGNAL_OUT(v_imu, dynamicgraph::Vector); /// 6d robot velocities form imu only (accelerometer integration + gyro)
DECLARE_SIGNAL_OUT(v_gyr, dynamicgraph::Vector); /// 6d robot velocities form gyroscope only (as if gyro measured the pure angular ankle velocities)
DECLARE_SIGNAL_OUT(lf_xyzquat, dynamicgraph::Vector); /// left foot pose
DECLARE_SIGNAL_OUT(rf_xyzquat, dynamicgraph::Vector); /// right foot pose
DECLARE_SIGNAL_OUT(a_ac, dynamicgraph::Vector); /// acceleration of the base in the world with DC component removed
DECLARE_SIGNAL_OUT(v_ac, dynamicgraph::Vector); /// velocity of the base in the world with DC component removed
DECLARE_SIGNAL_OUT(q_lf, dynamicgraph::Vector); /// n+6 robot configuration with base6d in RPY
DECLARE_SIGNAL_OUT(q_rf, dynamicgraph::Vector); /// n+6 robot configuration with base6d in RPY
DECLARE_SIGNAL_OUT(q_imu, dynamicgraph::Vector); /// n+6 robot configuration with base6d in RPY
DECLARE_SIGNAL_OUT(w_lf, double); /// weight of the estimation coming from the left foot
DECLARE_SIGNAL_OUT(w_rf, double); /// weight of the estimation coming from the right foot
DECLARE_SIGNAL_OUT(w_lf_filtered, double); /// filtered weight of the estimation coming from the left foot
DECLARE_SIGNAL_OUT(w_rf_filtered, double); /// filtered weight of the estimation coming from the right foot
/* --- COMMANDS --- */
/* --- ENTITY INHERITANCE --- */
virtual void display( std::ostream& os ) const;
protected:
bool m_initSucceeded; /// true if the entity has been successfully initialized
bool m_reset_foot_pos; /// true after the command resetFootPositions is called
double m_dt; /// sampling time step
RobotUtil * m_robot_util;
bool m_left_foot_is_stable; /// True if left foot as been stable for the last 'm_fz_stable_windows_size' samples
bool m_right_foot_is_stable; /// True if right foot as been stable for the last 'm_fz_stable_windows_size' samples
int m_fz_stable_windows_size; /// size of the windows used to detect that feet did not leave the ground
int m_lf_fz_stable_cpt; ///counter for detecting for how long the feet has been stable
int m_rf_fz_stable_cpt; ///counter for detecting for how long the feet has been stable
/* Estimator parameters */
double m_w_imu; /// weight of IMU for sensor fusion
double m_zmp_std_dev_rf; /// standard deviation of ZMP measurement errors for right foot
double m_zmp_std_dev_lf; /// standard deviation of ZMP measurement errors for left foot
double m_fz_std_dev_rf; /// standard deviation of normal force measurement errors for right foot
double m_fz_std_dev_lf; /// standard deviation of normal force measurement errors for left foot
Vector4 m_left_foot_sizes; /// sizes of the left foot (pos x, neg x, pos y, neg y)
Vector4 m_right_foot_sizes; /// sizes of the left foot (pos x, neg x, pos y, neg y)
double m_zmp_margin_lf; /// margin used for computing zmp weight
double m_zmp_margin_rf; /// margin used for computing zmp weight
double m_fz_margin_lf; /// margin used for computing normal force weight
double m_fz_margin_rf; /// margin used for computing normal force weight
Vector6 m_K_rf; /// 6d stiffness of right foot spring
Vector6 m_K_lf; /// 6d stiffness of left foot spring
Eigen::VectorXd m_v_kin; /// 6d robot velocities from kinematic only (encoders derivative)
Eigen::VectorXd m_v_flex; /// 6d robot velocities from flexibility only (force sensor derivative)
Eigen::VectorXd m_v_imu; /// 6d robot velocities form imu only (accelerometer integration + gyro)
Eigen::VectorXd m_v_gyr; /// 6d robot velocities form gyroscope only (as if gyro measured the pure angular ankle velocities)
Vector3 m_v_ac; /// velocity of the base in the world with DC component removed
Vector3 m_a_ac; /// acceleration of the base in the world with DC component removed
double m_alpha_DC_acc; /// alpha parameter for DC blocker filter on acceleration data
double m_alpha_DC_vel; /// alpha parameter for DC blocker filter on velocity data
double m_alpha_w_filter; /// filter parameter to filter weights (1st order low pass filter)
double m_w_lf_filtered; /// filtered weight of the estimation coming from the left foot
double m_w_rf_filtered; /// filtered weight of the estimation coming from the right foot
pinocchio::Model m_model; /// Pinocchio robot model
pinocchio::Data *m_data; /// Pinocchio robot data
pinocchio::SE3 m_torsoMimu; /// chest to imu transformation
pinocchio::SE3 m_oMff_lf; /// world-to-base transformation obtained through left foot
pinocchio::SE3 m_oMff_rf; /// world-to-base transformation obtained through right foot
SE3 m_oMlfs; /// transformation from world to left foot sole
SE3 m_oMrfs; /// transformation from world to right foot sole
Vector7 m_oMlfs_xyzquat;
Vector7 m_oMrfs_xyzquat;
SE3 m_oMlfs_default_ref;/// Default reference for left foot pose to use if no ref is pluged
SE3 m_oMrfs_default_ref;/// Default reference for right foot pose to use if no ref is pluged
normal m_normal; /// Normal distribution
bool m_isFirstIterFlag; /// flag to detect first iteration and initialise velocity filters
SE3 m_sole_M_ftSens; /// foot sole to F/T sensor transformation
pinocchio::FrameIndex m_right_foot_id;
pinocchio::FrameIndex m_left_foot_id;
pinocchio::FrameIndex m_torso_id;
pinocchio::FrameIndex m_IMU_frame_id;
Eigen::VectorXd m_q_pin; /// robot configuration according to pinocchio convention
Eigen::VectorXd m_q_sot; /// robot configuration according to SoT convention
Eigen::VectorXd m_v_pin; /// robot velocities according to pinocchio convention
Eigen::VectorXd m_v_sot; /// robot velocities according to SoT convention
Matrix3 m_oRtorso; /// chest orientation in the world from angular fusion
Matrix3 m_oRff; /// base orientation in the world
/* Filter buffers*/
Vector3 m_last_vel;
Vector3 m_last_DCvel;
Vector3 m_last_DCacc;
}; // class BaseEstimator
} // namespace talos_balance
} // namespace sot
} // namespace dynamicgraph
#endif // #ifndef __sot_torque_control_free_flyer_locator_H__
include/sot/talos_balance/boolean-identity.hh 0 → 100644
View file @ e217f893
/*
* Copyright 2018, Gepetto team, LAAS-CNRS
*
* This file is part of sot-talos-balance.
* sot-talos-balance is free software: you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public License
* as published by the Free Software Foundation, either version 3 of
* the License, or (at your option) any later version.
* sot-talos-balance is distributed in the hope that it will be
* useful, but WITHOUT ANY WARRANTY; without even the implied warranty
* of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU Lesser General Public License for more details. You should
* have received a copy of the GNU Lesser General Public License along
* with sot-talos-balance. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef __sot_talos_balance_boolean_identity_H__
#define __sot_talos_balance_boolean_identity_H__
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined(WIN32)
#if defined(position_controller_EXPORTS)
#define BOOLEAN_IDENTITY_EXPORT __declspec(dllexport)
#else
#define BOOLEAN_IDENTITY_EXPORT __declspec(dllimport)
#endif
#else
#define BOOLEAN_IDENTITY_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#include <dynamic-graph/signal-helper.h>
#include <map>
#include "boost/assign.hpp"
namespace dynamicgraph {
namespace sot {
namespace talos_balance {
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
class BOOLEAN_IDENTITY_EXPORT BooleanIdentity : public ::dynamicgraph::Entity {
DYNAMIC_GRAPH_ENTITY_DECL();
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/* --- CONSTRUCTOR ---- */
BooleanIdentity(const std::string& name);
void init() {}
/* --- SIGNALS --- */
DECLARE_SIGNAL_IN(sin, bool);
DECLARE_SIGNAL_OUT(sout, bool);
/* --- COMMANDS --- */
/* --- ENTITY INHERITANCE --- */
virtual void display(std::ostream& os) const;
}; // class BooleanIdentity
} // namespace talos_balance
} // namespace sot
} // namespace dynamicgraph
#endif // #ifndef __sot_talos_balance_boolean_identity_H__
include/sot/talos_balance/com-admittance-controller.hh
View file @ e217f893
......@@ -21,78 +21,77 @@
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined (WIN32)
# if defined (com_admittance_controller_EXPORTS)
# define COMADMITTANCECONTROLLER_EXPORT __declspec(dllexport)
# else
# define COMADMITTANCECONTROLLER_EXPORT __declspec(dllimport)
# endif
#if defined(WIN32)
#if defined(com_admittance_controller_EXPORTS)
#define COMADMITTANCECONTROLLER_EXPORT __declspec(dllexport)
#else
# define COMADMITTANCECONTROLLER_EXPORT
#define COMADMITTANCECONTROLLER_EXPORT __declspec(dllimport)
#endif
#else
#define COMADMITTANCECONTROLLER_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#include <dynamic-graph/signal-helper.h>
#include <map>
#include "boost/assign.hpp"
namespace dynamicgraph {
namespace sot {
namespace talos_balance {
namespace sot {
namespace talos_balance {
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
class COMADMITTANCECONTROLLER_EXPORT ComAdmittanceController
: public ::dynamicgraph::Entity
{
DYNAMIC_GRAPH_ENTITY_DECL();
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --- CONSTRUCTOR ---- */
ComAdmittanceController( const std::string & name );
class COMADMITTANCECONTROLLER_EXPORT ComAdmittanceController
: public ::dynamicgraph::Entity {
DYNAMIC_GRAPH_ENTITY_DECL();
void init(const double & dt);
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
void setPosition(const dynamicgraph::Vector &);
void setVelocity(const dynamicgraph::Vector &);
void setState(const dynamicgraph::Vector &, const dynamicgraph::Vector &);
/* --- CONSTRUCTOR ---- */
ComAdmittanceController(const std::string &name);
/* --- SIGNALS --- */
DECLARE_SIGNAL_IN(Kp, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(zmp, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(zmpDes, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(ddcomDes, dynamicgraph::Vector);
void init(const double &dt);
DECLARE_SIGNAL_OUT(ddcomRef, dynamicgraph::Vector);
void setPosition(const dynamicgraph::Vector &);
void setVelocity(const dynamicgraph::Vector &);
void setState(const dynamicgraph::Vector &, const dynamicgraph::Vector &);
DECLARE_SIGNAL_INNER(stateRef, dynamicgraph::Vector);
/* --- SIGNALS --- */
DECLARE_SIGNAL_IN(Kp, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(zmp, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(zmpDes, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(ddcomDes, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(comRef, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(dcomRef, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(ddcomRef, dynamicgraph::Vector);
/* --- COMMANDS --- */
/* --- ENTITY INHERITANCE --- */
virtual void display( std::ostream& os ) const;
DECLARE_SIGNAL_INNER(stateRef, dynamicgraph::Vector);
protected:
bool m_initSucceeded; /// true if the entity has been successfully initialized
dynamicgraph::Vector m_state; // internal state
double m_dt;
DECLARE_SIGNAL_OUT(comRef, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(dcomRef, dynamicgraph::Vector);
}; // class ComAdmittanceController
/* --- COMMANDS --- */
/* --- ENTITY INHERITANCE --- */
virtual void display(std::ostream &os) const;
} // namespace talos_balance
} // namespace sot
} // namespace dynamicgraph
protected:
bool
m_initSucceeded; /// true if the entity has been successfully initialized
dynamicgraph::Vector m_state; // internal state
double m_dt;
}; // class ComAdmittanceController
} // namespace talos_balance
} // namespace sot
} // namespace dynamicgraph
#endif // #ifndef __sot_talos_balance_com_admittance_controller_H__
#endif // #ifndef __sot_talos_balance_com_admittance_controller_H__
include/sot/talos_balance/control-manager.hh deleted 100644 → 0
View file @ 97ac292e
/*
* Copyright 2015, Andrea Del Prete, LAAS-CNRS
*
* This file is part of sot-torque-control.
* sot-torque-control is free software: you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public License
* as published by the Free Software Foundation, either version 3 of
* the License, or (at your option) any later version.
* sot-torque-control is distributed in the hope that it will be
* useful, but WITHOUT ANY WARRANTY; without even the implied warranty
* of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU Lesser General Public License for more details. You should
* have received a copy of the GNU Lesser General Public License along
* with sot-torque-control. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef __sot_torque_control_control_manager_H__
#define __sot_torque_control_control_manager_H__
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined (WIN32)
# if defined (__sot_torque_control_control_manager_H__)
# define SOTCONTROLMANAGER_EXPORT __declspec(dllexport)
# else
# define SOTCONTROLMANAGER_EXPORT __declspec(dllimport)
# endif
#else
# define SOTCONTROLMANAGER_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"
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 CtrlMode
{
public:
int id;
std::string name;
CtrlMode(): id(-1), name("None"){}
CtrlMode(int id, const std::string& name):
id(id), name(name) {}
};
std::ostream& operator<<( std::ostream& os, const CtrlMode& s )
{
os<<s.id<<"_"<<s.name;
return os;
}
class SOTCONTROLMANAGER_EXPORT ControlManager
:public::dynamicgraph::Entity
{
typedef Eigen::VectorXd::Index Index;
typedef ControlManager EntityClassName;
DYNAMIC_GRAPH_ENTITY_DECL();
public:
/* --- CONSTRUCTOR ---- */
ControlManager( const std::string & name);
/// Initialize
/// @param dt: control interval
/// @param urdfFile: path to the URDF model of the robot
void init(const double & dt,
const std::string & robotRef);
/* --- SIGNALS --- */
std::vector<dynamicgraph::SignalPtr<dynamicgraph::Vector,int>*> m_ctrlInputsSIN;
std::vector< dynamicgraph::SignalPtr<bool,int>* > m_emergencyStopVector; /// emergency stop inputs. If one is true, control is set to zero forever
std::vector<dynamicgraph::Signal<dynamicgraph::Vector,int>*> m_jointsCtrlModesSOUT;
DECLARE_SIGNAL_IN(u_max, dynamicgraph::Vector); /// max motor control
DECLARE_SIGNAL_OUT(u, dynamicgraph::Vector); /// raw motor control
DECLARE_SIGNAL_OUT(u_safe, dynamicgraph::Vector); /// safe motor control
/* --- COMMANDS --- */
/// Commands related to the control mode.
void addCtrlMode(const std::string& name);
void ctrlModes();
void getCtrlMode(const std::string& jointName);
void setCtrlMode(const std::string& jointName, const std::string& ctrlMode);
void setCtrlMode(const int jid, const CtrlMode& cm);
void resetProfiler();
/// Commands related to joint name and joint id
// void setNameToId(const std::string& jointName, const double & jointId);
// void setJointLimitsFromId(const double &jointId, const double &lq, const double &uq);
/// Set the mapping between urdf and sot.
// void setJoints(const dynamicgraph::Vector &);
// void setStreamPrintPeriod(const double & s);
void setSleepTime(const double &seconds);
void addEmergencyStopSIN(const std::string& name);
/* --- ENTITY INHERITANCE --- */
virtual void display( std::ostream& os ) const;
protected:
RobotUtil * m_robot_util;
size_t m_numDofs;
bool m_initSucceeded; /// true if the entity has been successfully initialized
double m_dt; /// control loop time period
bool m_emergency_stop_triggered; /// true if an emergency condition as been triggered either by an other entity, or by control limit violation
bool m_is_first_iter; /// true at the first iteration, false otherwise
int m_iter;
double m_sleep_time; /// time to sleep at every iteration (to slow down simulation)
std::vector<std::string> m_ctrlModes; /// existing control modes
std::vector<CtrlMode> m_jointCtrlModes_current; /// control mode of the joints
std::vector<CtrlMode> m_jointCtrlModes_previous; /// previous control mode of the joints
std::vector<int> m_jointCtrlModesCountDown; /// counters used for the transition between two ctrl modes
bool convertStringToCtrlMode(const std::string& name, CtrlMode& cm);
bool convertJointNameToJointId(const std::string& name, unsigned int& id);
//bool isJointInRange(unsigned int id, double q);
void updateJointCtrlModesOutputSignal();
}; // class ControlManager
} // namespace talos_balance
} // namespace sot
} // namespace dynamicgraph
#endif // #ifndef __sot_torque_control_control_manager_H__