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
  • ostasse/sot-dynamic-pinocchio
  • gsaurel/sot-dynamic-pinocchio
  • stack-of-tasks/sot-dynamic-pinocchio
3 results
Show changes
Commits on Source (82)
Hide whitespace changes
Inline Side-by-side
Showing
with 307 additions and 405 deletions
.git-blame-ignore-revs 0 → 100644
View file @ a6894863
# format (Guilhem Saurel, 2022-09-05)
d3360e7cb41cdd1b8423713f4254cca960c76244
# format (Guilhem Saurel, 2021-03-12)
526105e51d3ae27e53169c8cd243594920eeaba2
# format (Guilhem Saurel, 2021-03-12)
7851a42828ed897c07fbef1ce1cb942b9f02e341
# format (Guilhem Saurel, 2020-04-24)
8c1497b234072b6510ff066455da99f390f51241
.gitmodules
View file @ a6894863
[submodule "cmake"]
path = cmake
url = git://github.com/jrl-umi3218/jrl-cmakemodules.git
url = https://github.com/jrl-umi3218/jrl-cmakemodules.git
.pre-commit-config.yaml 0 → 100644
View file @ a6894863
ci:
autoupdate_branch: 'devel'
repos:
- repo: https://github.com/pre-commit/mirrors-clang-format
rev: v14.0.6
hooks:
- id: clang-format
args: [--style=Google]
- repo: https://github.com/pre-commit/pre-commit-hooks
rev: v4.3.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: 22.8.0
hooks:
- id: black
- repo: https://github.com/PyCQA/flake8
rev: 5.0.4
hooks:
- id: flake8
- repo: https://github.com/cheshirekow/cmake-format-precommit
rev: v0.6.13
hooks:
- id: cmake-format
.travis.yml
View file @ a6894863
......@@ -7,8 +7,7 @@ after_success:
- cd _travis/build/doc && ../../../cmake/github/update-doxygen-doc.sh
branches:
only:
- master
- topic/sot-pinocchio
- disabled_travis_because_not_relevant_anymore
compiler:
- clang
- gcc
......@@ -22,7 +21,7 @@ before_install:
- sudo sh -c "echo \"deb http://robotpkg.openrobots.org/packages/debian/pub precise robotpkg\" >> /etc/apt/sources.list "
- curl http://robotpkg.openrobots.org/packages/debian/robotpkg.key | sudo apt-key add -
- sudo apt-get update -qq
- sudo apt-get install -qq doxygen doxygen-latex libboost-all-dev libeigen3-dev liblapack-dev libblas-dev gfortran python-dev
- sudo apt-get install -qq doxygen doxygen-latex libboost-all-dev libeigen3-dev liblapack-dev libblas-dev gfortran python-dev
python-sphinx python-numpy libtinyxml-dev robotpkg-console-bridge robotpkg-urdfdom-headers robotpkg-urdfdom robotpkg-eigenpy
- sudo pip install cpp-coveralls
language: cpp
......
AUTHORS
View file @ a6894863
This package was written by and with the assistance of:
* Francois Bleibel fbleibel@gmail.com
* François Keith francois.keith@aist.go.jp
* Nicolas Mansard nicolas.mansard@laas.fr
* François Keith francois.keith@aist.go.jp
* Nicolas Mansard nicolas.mansard@laas.fr
* Olivier Stasse olivier.stasse@aist.go.jp
* Thomas Moulard thomas.moulard@gmail.com
CMakeLists.txt
View file @ a6894863
# Copyright 2010, François Bleibel, Olivier Stasse, JRL, CNRS/AIST
CMAKE_MINIMUM_REQUIRED(VERSION 3.1)
cmake_minimum_required(VERSION 3.1)
# Project properties
SET(PROJECT_ORG stack-of-tasks)
SET(PROJECT_NAME sot-dynamic-pinocchio)
SET(PROJECT_DESCRIPTION "pinocchio bindings for dynamic-graph.")
SET(PROJECT_URL "https://github.com/${PROJECT_ORG}/${PROJECT_NAME}")
SET(PROJECT_SUFFIX "-v3")
set(PROJECT_ORG stack-of-tasks)
set(PROJECT_NAME sot-dynamic-pinocchio)
set(PROJECT_DESCRIPTION "pinocchio bindings for dynamic-graph.")
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(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)
# 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/dynamic-pinocchio")
SET(CXX_DISABLE_WERROR TRUE)
SET(DOXYGEN_USE_MATHJAX YES)
if(NOT INSTALL_PYTHON_INTERFACE_ONLY)
set(PROJECT_USE_CMAKE_EXPORT TRUE)
endif(NOT INSTALL_PYTHON_INTERFACE_ONLY)
set(CUSTOM_HEADER_DIR "sot/dynamic-pinocchio")
set(CXX_DISABLE_WERROR TRUE)
set(DOXYGEN_USE_MATHJAX YES)
# JRL-cmakemodule setup
INCLUDE(cmake/base.cmake)
INCLUDE(cmake/boost.cmake)
INCLUDE(cmake/python.cmake)
INCLUDE(cmake/sphinx.cmake)
include(cmake/base.cmake)
include(cmake/boost.cmake)
include(cmake/lapack.cmake)
# Project definition
COMPUTE_PROJECT_ARGS(PROJECT_ARGS LANGUAGES CXX)
PROJECT(${PROJECT_NAME} ${PROJECT_ARGS})
compute_project_args(PROJECT_ARGS LANGUAGES CXX)
project(${PROJECT_NAME} ${PROJECT_ARGS})
# Project dependencies
ADD_PROJECT_DEPENDENCY(sot-tools REQUIRED PKG_CONFIG_REQUIRES sot-tools)
if(BUILD_PYTHON_INTERFACE)
add_project_dependency(dynamic-graph-python 4.0.0 REQUIRED)
string(REGEX REPLACE "-" "_" PYTHON_DIR ${CUSTOM_HEADER_DIR})
endif(BUILD_PYTHON_INTERFACE)
SET(BOOST_COMPONENTS filesystem system unit_test_framework)
add_project_dependency(sot-tools REQUIRED)
IF(BUILD_PYTHON_INTERFACE)
FINDPYTHON()
STRING(REGEX REPLACE "-" "_" PYTHON_DIR ${CUSTOM_HEADER_DIR})
ADD_PROJECT_DEPENDENCY(dynamic-graph-python REQUIRED
PKG_CONFIG_REQUIRES dynamic-graph-python)
SET(BOOST_COMPONENTS ${BOOST_COMPONENTS} python)
ENDIF(BUILD_PYTHON_INTERFACE)
if(BUILD_TESTING)
find_package(Boost REQUIRED COMPONENTS unit_test_framework)
add_project_dependency(example-robot-data 3.8.0 REQUIRED)
endif(BUILD_TESTING)
SEARCH_FOR_BOOST()
if(Boost_VERSION GREATER 107299)
# Silence a warning about a deprecated use of boost bind by boost python at
# least fo boost 1.73 to 1.75
add_definitions(-DBOOST_BIND_GLOBAL_PLACEHOLDERS)
endif()
# Main Library
SET(${PROJECT_NAME}_HEADERS
include/${CUSTOM_HEADER_DIR}/dynamic-pinocchio.h
include/${CUSTOM_HEADER_DIR}/integrator-force-exact.h
include/${CUSTOM_HEADER_DIR}/zmpreffromcom.h
include/${CUSTOM_HEADER_DIR}/integrator-force.h
include/${CUSTOM_HEADER_DIR}/force-compensation.h
include/${CUSTOM_HEADER_DIR}/mass-apparent.h
include/${CUSTOM_HEADER_DIR}/waist-attitude-from-sensor.h
include/${CUSTOM_HEADER_DIR}/matrix-inertia.h
include/${CUSTOM_HEADER_DIR}/integrator-force-rk4.h
include/${CUSTOM_HEADER_DIR}/angle-estimator.h
)
SET(${PROJECT_NAME}_SOURCES
src/sot-dynamic-pinocchio
)
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} ${Boost_LIBRARIES}
sot-core::sot-core)
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)
ADD_SUBDIRECTORY(python)
ADD_SUBDIRECTORY(tests)
PKG_CONFIG_APPEND_LIBS(${PROJECT_NAME})
set(${PROJECT_NAME}_HEADERS
include/${CUSTOM_HEADER_DIR}/dynamic-pinocchio.h
include/${CUSTOM_HEADER_DIR}/integrator-force-exact.h
include/${CUSTOM_HEADER_DIR}/zmpreffromcom.h
include/${CUSTOM_HEADER_DIR}/integrator-force.h
include/${CUSTOM_HEADER_DIR}/force-compensation.h
include/${CUSTOM_HEADER_DIR}/mass-apparent.h
include/${CUSTOM_HEADER_DIR}/waist-attitude-from-sensor.h
include/${CUSTOM_HEADER_DIR}/matrix-inertia.h
include/${CUSTOM_HEADER_DIR}/integrator-force-rk4.h
include/${CUSTOM_HEADER_DIR}/angle-estimator.h)
set(${PROJECT_NAME}_SOURCES src/sot-dynamic-pinocchio)
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} sot-core::sot-core)
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)
install(FILES package.xml DESTINATION share/${PROJECT_NAME})
endif(NOT INSTALL_PYTHON_INTERFACE_ONLY)
add_subdirectory(src)
if(BUILD_TESTING)
add_subdirectory(tests)
endif(BUILD_TESTING)
README.md
View file @ a6894863
sot-dynamic-pinocchio
Encapsulate Pinocchio in SoT
===========
# SoT Dynamic Pinocchio
[![Building Status](https://travis-ci.org/stack-of-tasks/sot-dynamic-pinocchio.svg?branch=master)](https://travis-ci.org/stack-of-tasks/sot-dynamic-pinocchio)
[![Pipeline status](https://gepgitlab.laas.fr/stack-of-tasks/sot-dynamic-pinocchio/badges/master/pipeline.svg)](https://gepgitlab.laas.fr/stack-of-tasks/sot-dynamic-pinocchio/commits/master)
[![Coverage report](https://gepgitlab.laas.fr/stack-of-tasks/sot-dynamic-pinocchio/badges/master/coverage.svg?job=doc-coverage)](http://projects.laas.fr/gepetto/doc/stack-of-tasks/sot-dynamic-pinocchio/master/coverage/)
Encapsulate Pinocchio in SoT
[![Pipeline status](https://gitlab.laas.fr/stack-of-tasks/sot-dynamic-pinocchio/badges/master/pipeline.svg)](https://gitlab.laas.fr/stack-of-tasks/sot-dynamic-pinocchio/commits/master)
[![Coverage report](https://gitlab.laas.fr/stack-of-tasks/sot-dynamic-pinocchio/badges/master/coverage.svg?job=doc-coverage)](https://gepettoweb.laas.fr/doc/stack-of-tasks/sot-dynamic-pinocchio/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-dynamic-pinocchio/master.svg)](https://results.pre-commit.ci/latest/github/stack-of-tasks/sot-dynamic-pinocchio)
This software provides robot dynamic computation for dynamic-graph
by using pinocchio.
......
cmake @ 57c46b2d
Subproject commit 321eb1ccf1d94570eb564f3659b13ef3ef82239e
Subproject commit 57c46b2d7b26bb0c25f8f98cfc2a9868e03be603
doc/Doxyfile.extra.in
View file @ a6894863
......@@ -5,6 +5,5 @@ IMAGE_PATH = @CMAKE_SOURCE_DIR@/doc/pictures
FILE_PATTERNS = *.cc *.cpp *.h *.hh *.hxx
TAGFILES = \
"@JRL_MAL_DOXYGENDOCDIR@/jrl-mal.doxytag = @JRL_MAL_DOXYGENDOCDIR@" \
"@DYNAMIC_GRAPH_DOXYGENDOCDIR@/dynamic-graph.doxytag = @DYNAMIC_GRAPH_DOXYGENDOCDIR@" \
"@SOT_CORE_DOXYGENDOCDIR@/sot-core.doxytag = @SOT_CORE_DOXYGENDOCDIR@"
doc/additionalDoc/package.hh
View file @ a6894863
......@@ -10,11 +10,12 @@
/** \mainpage
\section sot_dynamic_section_introduction Introduction
The sot-dynamic package is a bridge between the stack of tasks framework and the dynamicsJRLJapan library.
It provides an inverse dynamic model of the robot through dynamic-graph entities.
More precisely it wraps the newton euler algorithm implemented by the dynamicsJRLJapan library
to make it accessible for the stack of tasks controllers
(in the Stack of Tasks Framework as defined in \ref Mansard2007.)
The sot-dynamic package is a bridge between the stack of tasks framework and the
dynamicsJRLJapan library. It provides an inverse dynamic model of the robot
through dynamic-graph entities. More precisely it wraps the newton euler
algorithm implemented by the dynamicsJRLJapan library to make it accessible for
the stack of tasks controllers (in the Stack of Tasks Framework as defined in
\ref Mansard2007.)
This package depends on the following packages:
\li dynamicsJRLJapan
......@@ -27,9 +28,10 @@ install these packages at https://github.com/jrl-umi3218.
\section python_bindings Python bindings
As most packages based on the dynamic-graph framework (see https://github.com/jrl-umi3218/dynamic-graph),
the functionnality is exposed through entities. Hence python sub-modules of dynamic_graph are generated. See <a
href="../sphinx-html/index.html">sphinx documentation</a> for more details.
As most packages based on the dynamic-graph framework (see
https://github.com/jrl-umi3218/dynamic-graph), the functionnality is exposed
through entities. Hence python sub-modules of dynamic_graph are generated. See
<a href="../sphinx-html/index.html">sphinx documentation</a> for more details.
The following entities are created by this package:\n
(all entites are placed in the namespace sot::)
......@@ -41,7 +43,8 @@ The following entities are created by this package:\n
\li sot::IntegratorForce
\li sot::AngleEstimator
\li sot::WaistAttitudeFromSensor
\li sot::Dynamic - provides the inverse dynamics computations for of a humanoid robot
\li sot::Dynamic - provides the inverse dynamics computations for of a humanoid
robot
See each entity's documentation page for more information (when available).
......
doc/sphinx/conf.py.in deleted 100644 → 0
View file @ 92f75f2d
# -*- coding: utf-8 -*-
#
# @PROJECT_NAME@ documentation build configuration file, created by
# sphinx-quickstart on Mon Nov 22 16:45:27 2010.
#
# This file is execfile()d with the current directory set to its containing dir.
#
# Note that not all possible configuration values are present in this
# autogenerated file.
#
# All configuration values have a default; values that are commented out
# serve to show the default.
import sys, os
# If extensions (or modules to document with autodoc) are in another directory,
# add these directories to sys.path here. If the directory is relative to the
# documentation root, use os.path.abspath to make it absolute, like shown here.
sys.path = [os.path.abspath('@CMAKE_BINARY_DIR@/src')]+sys.path
sys.path = [os.path.abspath('@CMAKE_SOURCE_DIR@/src')]+sys.path
# -- General configuration -----------------------------------------------------
# Add any Sphinx extension module names here, as strings. They can be extensions
# coming with Sphinx (named 'sphinx.ext.*') or your custom ones.
extensions = ['sphinx.ext.autodoc', 'sphinx.ext.doctest', 'sphinx.ext.intersphinx', 'sphinx.ext.todo', 'sphinx.ext.pngmath']
# Add any paths that contain templates here, relative to this directory.
templates_path = ['_templates']
# The suffix of source filenames.
source_suffix = '.rst'
# The encoding of source files.
#source_encoding = 'utf-8'
# The master toctree document.
master_doc = 'index'
# General information about the project.
project = u'@PROJECT_NAME@'
copyright = u'2010, Florent Lamiraux'
# The version info for the project you're documenting, acts as replacement for
# |version| and |release|, also used in various other places throughout the
# built documents.
#
# The short X.Y version.
version = '@PROJECT_VERSION@'
# The full version, including alpha/beta/rc tags.
release = '1.0'
# The language for content autogenerated by Sphinx. Refer to documentation
# for a list of supported languages.
#language = None
# There are two options for replacing |today|: either, you set today to some
# non-false value, then it is used:
#today = ''
# Else, today_fmt is used as the format for a strftime call.
#today_fmt = '%B %d, %Y'
# List of documents that shouldn't be included in the build.
#unused_docs = []
# List of directories, relative to source directory, that shouldn't be searched
# for source files.
exclude_trees = []
# The reST default role (used for this markup: `text`) to use for all documents.
#default_role = None
# If true, '()' will be appended to :func: etc. cross-reference text.
#add_function_parentheses = True
# If true, the current module name will be prepended to all description
# unit titles (such as .. function::).
#add_module_names = True
# If true, sectionauthor and moduleauthor directives will be shown in the
# output. They are ignored by default.
#show_authors = False
# The name of the Pygments (syntax highlighting) style to use.
pygments_style = 'sphinx'
# A list of ignored prefixes for module index sorting.
#modindex_common_prefix = []
# -- Options for HTML output ---------------------------------------------------
# The theme to use for HTML and HTML Help pages. Major themes that come with
# Sphinx are currently 'default' and 'sphinxdoc'.
html_theme = 'default'
# Theme options are theme-specific and customize the look and feel of a theme
# further. For a list of options available for each theme, see the
# documentation.
#html_theme_options = {}
# Add any paths that contain custom themes here, relative to this directory.
#html_theme_path = []
# The name for this set of Sphinx documents. If None, it defaults to
# "<project> v<release> documentation".
#html_title = None
# A shorter title for the navigation bar. Default is the same as html_title.
#html_short_title = None
# The name of an image file (relative to this directory) to place at the top
# of the sidebar.
#html_logo = None
# The name of an image file (within the static path) to use as favicon of the
# docs. This file should be a Windows icon file (.ico) being 16x16 or 32x32
# pixels large.
#html_favicon = None
# Add any paths that contain custom static files (such as style sheets) here,
# relative to this directory. They are copied after the builtin static files,
# so a file named "default.css" will overwrite the builtin "default.css".
html_static_path = ['_static']
# If not '', a 'Last updated on:' timestamp is inserted at every page bottom,
# using the given strftime format.
#html_last_updated_fmt = '%b %d, %Y'
# If true, SmartyPants will be used to convert quotes and dashes to
# typographically correct entities.
#html_use_smartypants = True
# Custom sidebar templates, maps document names to template names.
#html_sidebars = {}
# Additional templates that should be rendered to pages, maps page names to
# template names.
#html_additional_pages = {}
# If false, no module index is generated.
#html_use_modindex = True
# If false, no index is generated.
#html_use_index = True
# If true, the index is split into individual pages for each letter.
#html_split_index = False
# If true, links to the reST sources are added to the pages.
#html_show_sourcelink = True
# If true, an OpenSearch description file will be output, and all pages will
# contain a <link> tag referring to it. The value of this option must be the
# base URL from which the finished HTML is served.
#html_use_opensearch = ''
# If nonempty, this is the file name suffix for HTML files (e.g. ".xhtml").
#html_file_suffix = ''
# Output file base name for HTML help builder.
htmlhelp_basename = '@PROJECT_NAME@doc'
# -- Options for LaTeX output --------------------------------------------------
# The paper size ('letter' or 'a4').
#latex_paper_size = 'letter'
# The font size ('10pt', '11pt' or '12pt').
#latex_font_size = '10pt'
# Grouping the document tree into LaTeX files. List of tuples
# (source start file, target name, title, author, documentclass [howto/manual]).
latex_documents = [
('index', '@PROJECT_NAME@.tex', u'@PROJECT_NAME@ Documentation',
u'Florent Lamiraux', 'manual'),
]
# The name of an image file (relative to this directory) to place at the top of
# the title page.
#latex_logo = None
# For "manual" documents, if this is true, then toplevel headings are parts,
# not chapters.
#latex_use_parts = False
# Additional stuff for the LaTeX preamble.
#latex_preamble = ''
# Documents to append as an appendix to all manuals.
#latex_appendices = []
# If false, no module index is generated.
#latex_use_modindex = True
# Example configuration for intersphinx: refer to the Python standard library.
intersphinx_mapping = {'http://docs.python.org/': None}
doc/sphinx/index.rst.in deleted 100644 → 0
View file @ 92f75f2d
.. sot-dynamic documentation master file, created by
sphinx-quickstart on Mon Nov 22 16:45:27 2010.
You can adapt this file completely to your liking, but it should at least
contain the root `toctree` directive.
sot-dynamic: python bindings documentation
================================================
.. toctree::
:maxdepth: 2
Python module dynamic_graph.sot.dynamics implements bindings for sot-dynamic_ library. To each main C++ class is associated a Python class. Main classes are listed below.
Entities
--------
.. automodule:: dynamic_graph.sot.dynamics
:members:
.. autoclass:: dynamic_graph.sot.dynamics.dynamic.Dynamic
:members:
Indices and tables
==================
* :ref:`genindex`
* :ref:`modindex`
* :ref:`search`
.. _sot-dynamic: file:../doxygen-html/index.html
include/sot/dynamic-pinocchio/angle-estimator.h
View file @ a6894863
......@@ -34,6 +34,7 @@
#include <dynamic-graph/entity.h>
#include <dynamic-graph/signal-ptr.h>
#include <dynamic-graph/signal-time-dependent.h>
#include <sot/core/matrix-geometry.hh>
/* STD */
......@@ -56,18 +57,28 @@ class SOTANGLEESTIMATOR_EXPORT AngleEstimator : public dg::Entity {
AngleEstimator(const std::string& name);
virtual ~AngleEstimator(void);
public: /* --- SIGNAL --- */
dg::SignalPtr<MatrixRotation, int> sensorWorldRotationSIN; // estimate(worldRc)
dg::SignalPtr<MatrixHomogeneous, int> sensorEmbeddedPositionSIN; // waistRchest
dg::SignalPtr<MatrixHomogeneous, int> contactWorldPositionSIN; // estimate(worldRf)
dg::SignalPtr<MatrixHomogeneous, int> contactEmbeddedPositionSIN; // waistRleg
dg::SignalTimeDependent<dynamicgraph::Vector, int> anglesSOUT; // [ flex1 flex2 yaw_drift ]
dg::SignalTimeDependent<MatrixRotation, int> flexibilitySOUT; // footRleg
dg::SignalTimeDependent<MatrixRotation, int> driftSOUT; // Ryaw = worldRc est(wRc)^-1
dg::SignalTimeDependent<MatrixRotation, int> sensorWorldRotationSOUT; // worldRc
dg::SignalTimeDependent<MatrixRotation, int> waistWorldRotationSOUT; // worldRwaist
dg::SignalTimeDependent<MatrixHomogeneous, int> waistWorldPositionSOUT; // worldMwaist
dg::SignalTimeDependent<dynamicgraph::Vector, int> waistWorldPoseRPYSOUT; // worldMwaist
public: /* --- SIGNAL --- */
dg::SignalPtr<MatrixRotation, int>
sensorWorldRotationSIN; // estimate(worldRc)
dg::SignalPtr<MatrixHomogeneous, int>
sensorEmbeddedPositionSIN; // waistRchest
dg::SignalPtr<MatrixHomogeneous, int>
contactWorldPositionSIN; // estimate(worldRf)
dg::SignalPtr<MatrixHomogeneous, int>
contactEmbeddedPositionSIN; // waistRleg
dg::SignalTimeDependent<dynamicgraph::Vector, int>
anglesSOUT; // [ flex1 flex2 yaw_drift ]
dg::SignalTimeDependent<MatrixRotation, int> flexibilitySOUT; // footRleg
dg::SignalTimeDependent<MatrixRotation, int>
driftSOUT; // Ryaw = worldRc est(wRc)^-1
dg::SignalTimeDependent<MatrixRotation, int>
sensorWorldRotationSOUT; // worldRc
dg::SignalTimeDependent<MatrixRotation, int>
waistWorldRotationSOUT; // worldRwaist
dg::SignalTimeDependent<MatrixHomogeneous, int>
waistWorldPositionSOUT; // worldMwaist
dg::SignalTimeDependent<dynamicgraph::Vector, int>
waistWorldPoseRPYSOUT; // worldMwaist
dg::SignalPtr<dynamicgraph::Matrix, int> jacobianSIN;
dg::SignalPtr<dynamicgraph::Vector, int> qdotSIN;
......@@ -75,15 +86,23 @@ class SOTANGLEESTIMATOR_EXPORT AngleEstimator : public dg::Entity {
dg::SignalTimeDependent<dynamicgraph::Vector, int> qdotSOUT;
public: /* --- FUNCTIONS --- */
dynamicgraph::Vector& computeAngles(dynamicgraph::Vector& res, const int& time);
MatrixRotation& computeFlexibilityFromAngles(MatrixRotation& res, const int& time);
dynamicgraph::Vector& computeAngles(dynamicgraph::Vector& res,
const int& time);
MatrixRotation& computeFlexibilityFromAngles(MatrixRotation& res,
const int& time);
MatrixRotation& computeDriftFromAngles(MatrixRotation& res, const int& time);
MatrixRotation& computeSensorWorldRotation(MatrixRotation& res, const int& time);
MatrixRotation& computeWaistWorldRotation(MatrixRotation& res, const int& time);
MatrixHomogeneous& computeWaistWorldPosition(MatrixHomogeneous& res, const int& time);
dynamicgraph::Vector& computeWaistWorldPoseRPY(dynamicgraph::Vector& res, const int& time);
dynamicgraph::Vector& compute_xff_dotSOUT(dynamicgraph::Vector& res, const int& time);
dynamicgraph::Vector& compute_qdotSOUT(dynamicgraph::Vector& res, const int& time);
MatrixRotation& computeSensorWorldRotation(MatrixRotation& res,
const int& time);
MatrixRotation& computeWaistWorldRotation(MatrixRotation& res,
const int& time);
MatrixHomogeneous& computeWaistWorldPosition(MatrixHomogeneous& res,
const int& time);
dynamicgraph::Vector& computeWaistWorldPoseRPY(dynamicgraph::Vector& res,
const int& time);
dynamicgraph::Vector& compute_xff_dotSOUT(dynamicgraph::Vector& res,
const int& time);
dynamicgraph::Vector& compute_qdotSOUT(dynamicgraph::Vector& res,
const int& time);
public: /* --- PARAMS --- */
void fromSensor(const bool& fs) { fromSensor_ = fs; }
......
include/sot/dynamic-pinocchio/dynamic-pinocchio.h
View file @ a6894863
......@@ -15,27 +15,34 @@
/* --------------------------------------------------------------------- */
/* STD */
#include <string>
#include <map>
#include <memory>
#include <string>
/* pinocchio */
/* SOT */
#include <pinocchio/fwd.hpp>
#include <sot/core/flags.hh>
/* SOT */
#include <dynamic-graph/entity.h>
#include <dynamic-graph/pool.h>
#include <dynamic-graph/signal-ptr.h>
#include <dynamic-graph/signal-time-dependent.h>
#include <sot/core/exception-dynamic.hh>
#include <sot/core/flags.hh>
#include <sot/core/matrix-geometry.hh>
/* Matrix */
#include <dynamic-graph/linear-algebra.h>
#include <sot/dynamic-pinocchio/deprecated.hh>
/* PINOCCHIO */
#include <pinocchio/algorithm/frames.hpp>
#include <pinocchio/algorithm/jacobian.hpp>
#include <pinocchio/algorithm/rnea.hpp>
#include <pinocchio/macros.hpp>
#include <pinocchio/multibody/model.hpp>
#include <pinocchio/algorithm/rnea.hpp>
#include <pinocchio/algorithm/jacobian.hpp>
#include <pinocchio/algorithm/frames.hpp>
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
......@@ -80,24 +87,29 @@ class SOTDYNAMIC_EXPORT DynamicPinocchio : public dg::Entity {
/* --- MODEL ATRIBUTES --- */
pinocchio::Model* m_model;
pinocchio::Data* m_data;
std::unique_ptr<pinocchio::Data> m_data;
/* --- MODEL ATRIBUTES --- */
public:
/* --- SIGNAL ACTIVATION --- */
dg::SignalTimeDependent<dg::Matrix, int>& createEndeffJacobianSignal(const std::string& signame, const std::string&,
const bool isLocal = true);
dg::SignalTimeDependent<dg::Matrix, int>& createJacobianSignal(const std::string& signame, const std::string&);
dg::SignalTimeDependent<dg::Matrix, int>& createEndeffJacobianSignal(
const std::string& signame, const std::string&,
const bool isLocal = true);
dg::SignalTimeDependent<dg::Matrix, int>& createJacobianSignal(
const std::string& signame, const std::string&);
void destroyJacobianSignal(const std::string& signame);
dg::SignalTimeDependent<MatrixHomogeneous, int>& createPositionSignal(const std::string&, const std::string&);
dg::SignalTimeDependent<MatrixHomogeneous, int>& createPositionSignal(
const std::string&, const std::string&);
void destroyPositionSignal(const std::string& signame);
dg::SignalTimeDependent<dg::Vector, int>& createVelocitySignal(const std::string&, const std::string&);
dg::SignalTimeDependent<dg::Vector, int>& createVelocitySignal(
const std::string&, const std::string&);
void destroyVelocitySignal(const std::string& signame);
dg::SignalTimeDependent<dg::Vector, int>& createAccelerationSignal(const std::string&, const std::string&);
dg::SignalTimeDependent<dg::Vector, int>& createAccelerationSignal(
const std::string&, const std::string&);
void destroyAccelerationSignal(const std::string& signame);
/*! @} */
......@@ -132,10 +144,14 @@ class SOTDYNAMIC_EXPORT DynamicPinocchio : public dg::Entity {
dg::SignalTimeDependent<dg::Vector, int> comSOUT;
dg::SignalTimeDependent<dg::Matrix, int> inertiaSOUT;
dg::SignalTimeDependent<dg::Matrix, int>& jacobiansSOUT(const std::string& name);
dg::SignalTimeDependent<MatrixHomogeneous, int>& positionsSOUT(const std::string& name);
dg::SignalTimeDependent<dg::Vector, int>& velocitiesSOUT(const std::string& name);
dg::SignalTimeDependent<dg::Vector, int>& accelerationsSOUT(const std::string& name);
dg::SignalTimeDependent<dg::Matrix, int>& jacobiansSOUT(
const std::string& name);
dg::SignalTimeDependent<MatrixHomogeneous, int>& positionsSOUT(
const std::string& name);
dg::SignalTimeDependent<dg::Vector, int>& velocitiesSOUT(
const std::string& name);
dg::SignalTimeDependent<dg::Vector, int>& accelerationsSOUT(
const std::string& name);
dg::SignalTimeDependent<double, int> footHeightSOUT;
dg::SignalTimeDependent<dg::Vector, int> upperJlSOUT;
......@@ -164,7 +180,15 @@ class SOTDYNAMIC_EXPORT DynamicPinocchio : public dg::Entity {
void setModel(pinocchio::Model*);
void setData(pinocchio::Data*);
void createData();
/// \deprecated this function does nothing. This class has its own
/// pinocchio::Data object, which can be access with \ref getData.
void setData(pinocchio::Data*) SOT_DYNAMIC_PINOCCHIO_DEPRECATED;
pinocchio::Model* getModel() { return m_model; };
pinocchio::Data* getData() { return m_data.get(); };
/* --- GETTERS --- */
......@@ -195,13 +219,20 @@ class SOTDYNAMIC_EXPORT DynamicPinocchio : public dg::Entity {
// dg::Vector& getAnklePositionInFootFrame() const;
protected:
dg::Matrix& computeGenericJacobian(const bool isFrame, const int jointId, dg::Matrix& res, const int& time);
dg::Matrix& computeGenericEndeffJacobian(const bool isFrame, const bool isLocal, const int jointId, dg::Matrix& res,
dg::Matrix& computeGenericJacobian(const bool isFrame, const int jointId,
dg::Matrix& res, const int& time);
dg::Matrix& computeGenericEndeffJacobian(const bool isFrame,
const bool isLocal,
const int jointId, dg::Matrix& res,
const int& time);
MatrixHomogeneous& computeGenericPosition(const bool isFrame, const int jointId, MatrixHomogeneous& res,
MatrixHomogeneous& computeGenericPosition(const bool isFrame,
const int jointId,
MatrixHomogeneous& res,
const int& time);
dg::Vector& computeGenericVelocity(const int jointId, dg::Vector& res, const int& time);
dg::Vector& computeGenericAcceleration(const int jointId, dg::Vector& res, const int& time);
dg::Vector& computeGenericVelocity(const int jointId, dg::Vector& res,
const int& time);
dg::Vector& computeGenericAcceleration(const int jointId, dg::Vector& res,
const int& time);
dg::Vector& computeZmp(dg::Vector& res, const int& time);
dg::Vector& computeMomenta(dg::Vector& res, const int& time);
......@@ -216,21 +247,27 @@ class SOTDYNAMIC_EXPORT DynamicPinocchio : public dg::Entity {
public: /* --- PARAMS --- */
void cmd_createOpPointSignals(const std::string& sig, const std::string& j);
void cmd_createJacobianWorldSignal(const std::string& sig, const std::string& j);
void cmd_createJacobianEndEffectorSignal(const std::string& sig, const std::string& j);
void cmd_createJacobianEndEffectorWorldSignal(const std::string& sig, const std::string& j);
void cmd_createJacobianWorldSignal(const std::string& sig,
const std::string& j);
void cmd_createJacobianEndEffectorSignal(const std::string& sig,
const std::string& j);
void cmd_createJacobianEndEffectorWorldSignal(const std::string& sig,
const std::string& j);
void cmd_createPositionSignal(const std::string& sig, const std::string& j);
void cmd_createVelocitySignal(const std::string& sig, const std::string& j);
void cmd_createAccelerationSignal(const std::string& sig, const std::string& j);
void cmd_createAccelerationSignal(const std::string& sig,
const std::string& j);
private:
/// \brief map of joints in construction.
/// map: jointName -> (jointType,jointPosition (in parent frame), function_ptr to pinocchio Joint)
/// map: jointName -> (jointType,jointPosition (in parent frame), function_ptr
/// to pinocchio Joint)
dg::Vector& getPinocchioPos(dg::Vector& q, const int& time);
dg::Vector& getPinocchioVel(dg::Vector& v, const int& time);
dg::Vector& getPinocchioAcc(dg::Vector& a, const int& time);
//\brief Index list for the first dof of (spherical joints)/ (spherical part of free-flyer joint).
//\brief Index list for the first dof of (spherical joints)/ (spherical part
// of free-flyer joint).
std::vector<int> sphericalJoints;
};
......
include/sot/dynamic-pinocchio/force-compensation.h
View file @ a6894863
......@@ -58,48 +58,62 @@ class SOTFORCECOMPENSATION_EXPORT ForceCompensation {
public:
ForceCompensation(void);
static MatrixForce& computeHandXworld(const MatrixRotation& worldRhand, const dynamicgraph::Vector& transSensorCom,
MatrixForce& res);
static MatrixForce& computeHandXworld(
const MatrixRotation& worldRhand,
const dynamicgraph::Vector& transSensorCom, MatrixForce& res);
static MatrixForce& computeHandVsensor(const MatrixRotation& sensorRhand, MatrixForce& res);
static MatrixForce& computeSensorXhand(const MatrixRotation& sensorRhand, const dynamicgraph::Vector& transSensorCom,
static MatrixForce& computeHandVsensor(const MatrixRotation& sensorRhand,
MatrixForce& res);
/* static dynamicgraph::Matrix& computeInertiaSensor( const dynamicgraph::Matrix& inertiaJoint, */
static MatrixForce& computeSensorXhand(
const MatrixRotation& sensorRhand,
const dynamicgraph::Vector& transSensorCom, MatrixForce& res);
/* static dynamicgraph::Matrix& computeInertiaSensor( const
* dynamicgraph::Matrix& inertiaJoint, */
/* const MatrixForce& sensorXhand, */
/* dynamicgraph::Matrix& res ); */
static dynamicgraph::Vector& computeTorsorCompensated(
const dynamicgraph::Vector& torqueInput, const dynamicgraph::Vector& torquePrecompensation,
const dynamicgraph::Vector& gravity, const MatrixForce& handXworld, const MatrixForce& handVsensor,
const dynamicgraph::Matrix& gainSensor, const dynamicgraph::Vector& momentum, dynamicgraph::Vector& res);
static dynamicgraph::Vector& crossProduct_V_F(const dynamicgraph::Vector& velocity,
const dynamicgraph::Vector& force, dynamicgraph::Vector& res);
static dynamicgraph::Vector& computeMomentum(const dynamicgraph::Vector& velocity,
const dynamicgraph::Vector& acceleration,
const MatrixForce& sensorXhand,
const dynamicgraph::Matrix& inertiaJoint, dynamicgraph::Vector& res);
static dynamicgraph::Vector& computeDeadZone(const dynamicgraph::Vector& torqueInput,
const dynamicgraph::Vector& deadZoneLimit, dynamicgraph::Vector& res);
const dynamicgraph::Vector& torqueInput,
const dynamicgraph::Vector& torquePrecompensation,
const dynamicgraph::Vector& gravity, const MatrixForce& handXworld,
const MatrixForce& handVsensor, const dynamicgraph::Matrix& gainSensor,
const dynamicgraph::Vector& momentum, dynamicgraph::Vector& res);
static dynamicgraph::Vector& crossProduct_V_F(
const dynamicgraph::Vector& velocity, const dynamicgraph::Vector& force,
dynamicgraph::Vector& res);
static dynamicgraph::Vector& computeMomentum(
const dynamicgraph::Vector& velocity,
const dynamicgraph::Vector& acceleration, const MatrixForce& sensorXhand,
const dynamicgraph::Matrix& inertiaJoint, dynamicgraph::Vector& res);
static dynamicgraph::Vector& computeDeadZone(
const dynamicgraph::Vector& torqueInput,
const dynamicgraph::Vector& deadZoneLimit, dynamicgraph::Vector& res);
public: // CALIBRATION
std::list<dynamicgraph::Vector> torsorList;
std::list<MatrixRotation> rotationList;
void clearCalibration(void);
void addCalibrationValue(const dynamicgraph::Vector& torsor, const MatrixRotation& worldRhand);
dynamicgraph::Vector calibrateTransSensorCom(const dynamicgraph::Vector& gravity, const MatrixRotation& handRsensor);
dynamicgraph::Vector calibrateGravity(const MatrixRotation& handRsensor, bool precompensationCalibration = false,
const MatrixRotation& hand0Rsensor = I3);
void addCalibrationValue(const dynamicgraph::Vector& torsor,
const MatrixRotation& worldRhand);
dynamicgraph::Vector calibrateTransSensorCom(
const dynamicgraph::Vector& gravity, const MatrixRotation& handRsensor);
dynamicgraph::Vector calibrateGravity(
const MatrixRotation& handRsensor,
bool precompensationCalibration = false,
const MatrixRotation& hand0Rsensor = I3);
};
/* --------------------------------------------------------------------- */
/* --- PLUGIN ---------------------------------------------------------- */
/* --------------------------------------------------------------------- */
class SOTFORCECOMPENSATION_EXPORT ForceCompensationPlugin : public dg::Entity, public ForceCompensation {
class SOTFORCECOMPENSATION_EXPORT ForceCompensationPlugin
: public dg::Entity,
public ForceCompensation {
public:
static const std::string CLASS_NAME;
virtual const std::string& getClassName(void) const { return CLASS_NAME; }
......
include/sot/dynamic-pinocchio/integrator-force-exact.h
View file @ a6894863
......@@ -21,10 +21,10 @@
#include <dynamic-graph/entity.h>
#include <dynamic-graph/signal-ptr.h>
#include <dynamic-graph/signal-time-dependent.h>
#include <sot/core/matrix-geometry.hh>
#include <sot/dynamic-pinocchio/integrator-force.h>
#include <sot/core/matrix-geometry.hh>
/* STD */
#include <string>
......@@ -50,7 +50,8 @@ namespace dg = dynamicgraph;
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
class SOTINTEGRATORFORCEEXACT_EXPORT IntegratorForceExact : public IntegratorForce {
class SOTINTEGRATORFORCEEXACT_EXPORT IntegratorForceExact
: public IntegratorForce {
public:
static const std::string CLASS_NAME;
......@@ -61,7 +62,8 @@ class SOTINTEGRATORFORCEEXACT_EXPORT IntegratorForceExact : public IntegratorFor
public: /* --- SIGNAL --- */
public: /* --- FUNCTIONS --- */
dynamicgraph::Vector& computeVelocityExact(dynamicgraph::Vector& res, const int& time);
dynamicgraph::Vector& computeVelocityExact(dynamicgraph::Vector& res,
const int& time);
};
} /* namespace sot */
......
include/sot/dynamic-pinocchio/integrator-force-rk4.h
View file @ a6894863
......@@ -21,10 +21,10 @@
#include <dynamic-graph/entity.h>
#include <dynamic-graph/signal-ptr.h>
#include <dynamic-graph/signal-time-dependent.h>
#include <sot/core/matrix-geometry.hh>
#include <sot/dynamic-pinocchio/integrator-force.h>
#include <sot/core/matrix-geometry.hh>
/* STD */
#include <string>
......@@ -61,7 +61,8 @@ class SOTINTEGRATORFORCERK4_EXPORT IntegratorForceRK4 : public IntegratorForce {
public: /* --- SIGNAL --- */
public: /* --- FUNCTIONS --- */
dynamicgraph::Vector& computeDerivativeRK4(dynamicgraph::Vector& res, const int& time);
dynamicgraph::Vector& computeDerivativeRK4(dynamicgraph::Vector& res,
const int& time);
};
} /* namespace sot */
......
include/sot/dynamic-pinocchio/integrator-force.h
View file @ a6894863
......@@ -21,6 +21,7 @@
#include <dynamic-graph/entity.h>
#include <dynamic-graph/signal-ptr.h>
#include <dynamic-graph/signal-time-dependent.h>
#include <sot/core/matrix-geometry.hh>
/* STD */
......@@ -76,10 +77,13 @@ class SOTINTEGRATORFORCE_EXPORT IntegratorForce : public dg::Entity {
dg::SignalTimeDependent<dynamicgraph::Matrix, int> massInverseSOUT;
public: /* --- FUNCTIONS --- */
dynamicgraph::Vector& computeDerivative(dynamicgraph::Vector& res, const int& time);
dynamicgraph::Vector& computeIntegral(dynamicgraph::Vector& res, const int& time);
dynamicgraph::Vector& computeDerivative(dynamicgraph::Vector& res,
const int& time);
dynamicgraph::Vector& computeIntegral(dynamicgraph::Vector& res,
const int& time);
dynamicgraph::Matrix& computeMassInverse(dynamicgraph::Matrix& res, const int& time);
dynamicgraph::Matrix& computeMassInverse(dynamicgraph::Matrix& res,
const int& time);
};
} /* namespace sot */
......
include/sot/dynamic-pinocchio/mass-apparent.h
View file @ a6894863
......@@ -21,6 +21,7 @@
#include <dynamic-graph/entity.h>
#include <dynamic-graph/signal-ptr.h>
#include <dynamic-graph/signal-time-dependent.h>
#include <sot/core/matrix-geometry.hh>
/* STD */
......@@ -66,9 +67,11 @@ class SOTMASSAPPARENT_EXPORT MassApparent : public dg::Entity {
dg::SignalTimeDependent<dynamicgraph::Matrix, int> inertiaInverseSOUT;
public: /* --- FUNCTIONS --- */
dynamicgraph::Matrix& computeMassInverse(dynamicgraph::Matrix& res, const int& time);
dynamicgraph::Matrix& computeMassInverse(dynamicgraph::Matrix& res,
const int& time);
dynamicgraph::Matrix& computeMass(dynamicgraph::Matrix& res, const int& time);
dynamicgraph::Matrix& computeInertiaInverse(dynamicgraph::Matrix& res, const int& time);
dynamicgraph::Matrix& computeInertiaInverse(dynamicgraph::Matrix& res,
const int& time);
};
} // namespace sot
......
include/sot/dynamic-pinocchio/matrix-inertia.h
View file @ a6894863
......@@ -11,11 +11,10 @@
#define __SOT_SOTMATRIXINERTIA_H__
#include <ostream>
#include <sot/core/matrix-geometry.hh>
#include "dynamic-graph/linear-algebra.h"
#include <sot/core/matrix-geometry.hh>
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
......