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 (137)
37 additional commits have been omitted to prevent performance issues.
Hide whitespace changes
Inline Side-by-side
Showing
with 554 additions and 893 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 2.6)
INCLUDE(cmake/base.cmake)
INCLUDE(cmake/boost.cmake)
INCLUDE(cmake/eigen.cmake)
INCLUDE(cmake/lapack.cmake)
INCLUDE(cmake/cpack.cmake)
INCLUDE(cmake/python.cmake)
SET(PROJECT_NAME sot-dynamic-pinocchio)
SET(PROJECT_DESCRIPTION "pinocchio bindings for dynamic-graph.")
SET(PROJECT_URL "https://github.com/stack-of-tasks/sot-dynamic-pinocchio/")
SET(PROJECT_SUFFIX "-v3")
SET(CUSTOM_HEADER_DIR "${PROJECT_NAME}")
SET(DOXYGEN_USE_MATHJAX YES)
# Disable -Werror on Unix for now.
SET(CXX_DISABLE_WERROR True)
SET(PKG_CONFIG_ADDITIONAL_VARIABLES
${PKG_CONFIG_ADDITIONAL_VARIABLES}
plugindirname
plugindir
)
SETUP_PROJECT()
# Search for dependencies.
ADD_REQUIRED_DEPENDENCY("pinocchio >= 1.3.0")
ADD_REQUIRED_DEPENDENCY("dynamic-graph >= 3.0.0")
ADD_REQUIRED_DEPENDENCY("sot-core >= 3.0.0")
ADD_REQUIRED_DEPENDENCY("sot-tools >= 2.0.0")
SET(BOOST_COMPONENTS filesystem system unit_test_framework)
OPTION (BUILD_PYTHON_INTERFACE "Build the python binding" ON)
IF(BUILD_PYTHON_INTERFACE)
FINDPYTHON()
ADD_REQUIRED_DEPENDENCY("dynamic-graph-python >= 3.0.0")
SET(BOOST_COMPONENTS ${BOOST_COMPONENTS} python)
INCLUDE_DIRECTORIES(${PYTHON_INCLUDE_PATH})
ADD_REQUIRED_DEPENDENCY("eigenpy")
ENDIF(BUILD_PYTHON_INTERFACE)
# List plug-ins that will be compiled.
SET(plugins
zmpreffromcom
force-compensation
integrator-force-exact
mass-apparent
integrator-force-rk4
integrator-force
angle-estimator
waist-attitude-from-sensor
zmp-from-forces
)
SET(LIBRARY_NAME ${PROJECT_NAME})
LIST(APPEND plugins dynamic)
# Add dependency toward sot-dynamic-pinocchio library in pkg-config file.
PKG_CONFIG_APPEND_LIBS(${LIBRARY_NAME})
SEARCH_FOR_BOOST()
SEARCH_FOR_EIGEN()
# Add subdirectories.
ADD_SUBDIRECTORY(include)
ADD_SUBDIRECTORY(src)
ADD_SUBDIRECTORY(doc)
ADD_SUBDIRECTORY(python)
ADD_SUBDIRECTORY(unitTesting)
SETUP_PROJECT_FINALIZE()
SETUP_PROJECT_CPACK()
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}")
# 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)
# 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)
# JRL-cmakemodule setup
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})
# Project dependencies
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)
add_project_dependency(sot-tools REQUIRED)
if(BUILD_TESTING)
find_package(Boost REQUIRED COMPONENTS unit_test_framework)
add_project_dependency(example-robot-data 3.8.0 REQUIRED)
endif(BUILD_TESTING)
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} 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)
ChangeLog 100755 → 100644
View file @ a6894863
File mode changed from 100755 to 100644
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 5c8c19f491f2c6f8488f5f37ff81d711d69dbb3f
Subproject commit 57c46b2d7b26bb0c25f8f98cfc2a9868e03be603
doc/CMakeLists.txt deleted 100644 → 0
View file @ d3b4bdb1
#
# Copyright 2010 CNRS
# Author: Florent Lamiraux
#
INCLUDE(../cmake/sphinx.cmake)
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,8 +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::)
......@@ -40,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 @ d3b4bdb1
# -*- 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 @ d3b4bdb1
.. 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/CMakeLists.txt deleted 100644 → 0
View file @ d3b4bdb1
# Copyright 2010, François Bleibel, Olivier Stasse, JRL, CNRS/AIST
SET(${PROJECT_NAME}_HEADERS
dynamic-pinocchio.h
integrator-force-exact.h
zmpreffromcom.h
integrator-force.h
force-compensation.h
mass-apparent.h
waist-attitude-from-sensor.h
matrix-inertia.h
integrator-force-rk4.h
angle-estimator.h
)
# Recreate correct path for the headers
#--------------------------------------
SET(fullpath_${PROJECT_NAME}_HEADERS)
FOREACH(lHeader ${${PROJECT_NAME}_HEADERS})
SET(fullpath_${PROJECT_NAME}_HEADERS
${fullpath_${PROJECT_NAME}_HEADERS}
./${PROJECT_NAME}/${lHeader}
)
ENDFOREACH(lHeader)
#----------------------------------------------------
# Install procedure for the header files
#----------------------------------------------------
INSTALL(FILES ${fullpath_${PROJECT_NAME}_HEADERS}
DESTINATION include/${PROJECT_NAME}
PERMISSIONS OWNER_READ GROUP_READ WORLD_READ OWNER_WRITE
)
include/sot-dynamic-pinocchio/dynamic-pinocchio.h deleted 100644 → 0
View file @ d3b4bdb1
/*
* Copyright 2010,
* François Bleibel,
* Olivier Stasse,
*
* CNRS/AIST
*
*/
#ifndef __SOT_DYNAMIC_PINOCCHIO_H__
#define __SOT_DYNAMIC_PINOCCHIO_H__
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* STD */
#include <string>
#include <map>
/* SOT */
#include <sot/core/flags.hh>
#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/matrix-geometry.hh>
/* Matrix */
#include <dynamic-graph/linear-algebra.h>
/* PINOCCHIO */
#include <pinocchio/macros.hpp>
#include <pinocchio/multibody/model.hpp>
#include <pinocchio/multibody/joint/joint-variant.hpp>
#include <pinocchio/algorithm/rnea.hpp>
#include <pinocchio/algorithm/jacobian.hpp>
#include <pinocchio/algorithm/frames.hpp>
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined (WIN32)
# if defined (dynamic_EXPORTS)
# define SOTDYNAMIC_EXPORT __declspec(dllexport)
# else
# define SOTDYNAMIC_EXPORT __declspec(dllimport)
# endif
#else
# define SOTDYNAMIC_EXPORT
#endif
namespace dynamicgraph {
namespace sot {
namespace dg = dynamicgraph;
namespace command {
class SetFile;
class CreateOpPoint;
}
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/*! @ingroup signals
\brief This class provides an inverse dynamic model of the robot.
More precisely it wraps the newton euler algorithm implemented
by the dynamicsJRLJapan library to make it accessible in the stack of tasks.
The robot is described by a VRML file.
*/
class SOTDYNAMIC_EXPORT DynamicPinocchio
:public dg::Entity {
friend class sot::command::SetFile;
friend class sot::command::CreateOpPoint;
// friend class sot::command::InitializeRobot;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
DYNAMIC_GRAPH_ENTITY_DECL();
/* --- MODEL ATRIBUTES --- */
se3::Model* m_model;
se3::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& );
void destroyJacobianSignal( const std::string& signame );
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& );
void destroyVelocitySignal( const std::string& signame );
dg::SignalTimeDependent< dg::Vector,int >&
createAccelerationSignal( const std::string&, const std::string& );
void destroyAccelerationSignal( const std::string& signame );
/*! @} */
std::list< dg::SignalBase<int>* > genericSignalRefs;
public:
/* --- SIGNAL --- */
typedef int Dummy;
dg::SignalPtr<dg::Vector,int> jointPositionSIN;
dg::SignalPtr<dg::Vector,int> freeFlyerPositionSIN;
dg::SignalPtr<dg::Vector,int> jointVelocitySIN;
dg::SignalPtr<dg::Vector,int> freeFlyerVelocitySIN;
dg::SignalPtr<dg::Vector,int> jointAccelerationSIN;
dg::SignalPtr<dg::Vector,int> freeFlyerAccelerationSIN;
dg::SignalTimeDependent<dg::Vector,int> pinocchioPosSINTERN;
dg::SignalTimeDependent<dg::Vector,int> pinocchioVelSINTERN;
dg::SignalTimeDependent<dg::Vector,int> pinocchioAccSINTERN;
dg::SignalTimeDependent<Dummy,int> newtonEulerSINTERN;
dg::SignalTimeDependent<Dummy,int> jacobiansSINTERN;
dg::SignalTimeDependent<Dummy,int> forwardKinematicsSINTERN;
dg::SignalTimeDependent<Dummy,int> ccrbaSINTERN;
int& computeNewtonEuler(int& dummy,const int& time );
int& computeForwardKinematics(int& dummy,const int& time );
int& computeCcrba( int& dummy,const int& time );
int& computeJacobians( int& dummy,const int& time );
dg::SignalTimeDependent<dg::Vector,int> zmpSOUT;
dg::SignalTimeDependent<dg::Matrix,int> JcomSOUT;
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<double,int> footHeightSOUT;
dg::SignalTimeDependent<dg::Vector,int> upperJlSOUT;
dg::SignalTimeDependent<dg::Vector,int> lowerJlSOUT;
dg::SignalTimeDependent<dg::Vector,int> upperVlSOUT;
dg::SignalTimeDependent<dg::Vector,int> upperTlSOUT;
dg::Signal<dg::Vector,int> inertiaRotorSOUT;
dg::Signal<dg::Vector,int> gearRatioSOUT;
dg::SignalTimeDependent<dg::Matrix,int> inertiaRealSOUT;
dg::SignalTimeDependent<dg::Vector,int> MomentaSOUT;
dg::SignalTimeDependent<dg::Vector,int> AngularMomentumSOUT;
dg::SignalTimeDependent<dg::Vector,int> dynamicDriftSOUT;
public:
/* --- CONSTRUCTOR --- */
DynamicPinocchio( const std::string& name);
virtual ~DynamicPinocchio( void );
/* --- MODEL CREATION --- */
void displayModel() const
{ assert(m_model); std::cout<<(*m_model)<<std::endl; };
void setModel(se3::Model*);
void setData(se3::Data*);
/* --- GETTERS --- */
/// \brief Get joint position lower limits
///
/// \param[out] result vector
/// \return result vector
dg::Vector& getLowerPositionLimits(dg::Vector& res,const int&) const ;
/// \brief Get joint position upper limits
///
/// \param[out] result vector
/// \return result vector
dg::Vector& getUpperPositionLimits(dg::Vector& res,const int&) const;
/// \brief Get joint velocity upper limits
///
/// \param[out] result vector
/// \return result vector
dg::Vector& getUpperVelocityLimits(dg::Vector& res,const int&) const;
/// \brief Get joint effort upper limits
///
/// \param[out] result vector
/// \return result vector
dg::Vector& getMaxEffortLimits(dg::Vector& res,const int&) const;
// 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,const int& time );
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& computeZmp( dg::Vector& res,const int& time );
dg::Vector& computeMomenta( dg::Vector &res,const int& time);
dg::Vector& computeAngularMomentum( dg::Vector &res,const int& time);
dg::Matrix& computeJcom( dg::Matrix& res,const int& time );
dg::Vector& computeCom( dg::Vector& res,const int& time );
dg::Matrix& computeInertia( dg::Matrix& res,const int& time );
dg::Matrix& computeInertiaReal( dg::Matrix& res,const int& time );
double& computeFootHeight( double& res,const int& time );
dg::Vector& computeTorqueDrift( dg::Vector& res,const int& time );
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_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 );
private:
/// \brief map of joints in construction.
/// 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).
std::vector<int> sphericalJoints;
};
// std::ostream& operator<<(std::ostream& os, const CjrlJoint& r);
} /* namespace sot */} /* namespace dynamicgraph */
#endif // #ifndef __SOT_DYNAMIC_PINOCCHIO_H__
include/sot-dynamic-pinocchio/force-compensation.h deleted 100644 → 0
View file @ d3b4bdb1
/*
* Copyright 2010,
* François Bleibel,
* Olivier Stasse,
*
* CNRS/AIST
*
*/
#ifndef __SOT_SOTFORCECOMPENSATION_H__
#define __SOT_SOTFORCECOMPENSATION_H__
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* Matrix */
#include <dynamic-graph/linear-algebra.h>
/* SOT */
#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 */
#include <string>
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined (WIN32)
# if defined (force_compensation_EXPORTS)
# define SOTFORCECOMPENSATION_EXPORT __declspec(dllexport)
# else
# define SOTFORCECOMPENSATION_EXPORT __declspec(dllimport)
# endif
#else
# define SOTFORCECOMPENSATION_EXPORT
#endif
namespace dynamicgraph { namespace sot {
namespace dg = dynamicgraph;
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
class SOTFORCECOMPENSATION_EXPORT ForceCompensation
{
private:
static MatrixRotation I3;
protected:
bool usingPrecompensation;
public:
ForceCompensation( void );
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,
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 );
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 );
};
/* --------------------------------------------------------------------- */
/* --- PLUGIN ---------------------------------------------------------- */
/* --------------------------------------------------------------------- */
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; }
bool calibrationStarted;
public: /* --- CONSTRUCTION --- */
ForceCompensationPlugin( const std::string& name );
virtual ~ForceCompensationPlugin( void );
public: /* --- SIGNAL --- */
/* --- INPUTS --- */
dg::SignalPtr<dynamicgraph::Vector,int> torsorSIN;
dg::SignalPtr<MatrixRotation,int> worldRhandSIN;
/* --- CONSTANTS --- */
dg::SignalPtr<MatrixRotation,int> handRsensorSIN;
dg::SignalPtr<dynamicgraph::Vector,int> translationSensorComSIN;
dg::SignalPtr<dynamicgraph::Vector,int> gravitySIN;
dg::SignalPtr<dynamicgraph::Vector,int> precompensationSIN;
dg::SignalPtr<dynamicgraph::Matrix,int> gainSensorSIN;
dg::SignalPtr<dynamicgraph::Vector,int> deadZoneLimitSIN;
dg::SignalPtr<dynamicgraph::Vector,int> transSensorJointSIN;
dg::SignalPtr<dynamicgraph::Matrix,int> inertiaJointSIN;
dg::SignalPtr<dynamicgraph::Vector,int> velocitySIN;
dg::SignalPtr<dynamicgraph::Vector,int> accelerationSIN;
/* --- INTERMEDIATE OUTPUTS --- */
dg::SignalTimeDependent<MatrixForce,int> handXworldSOUT;
dg::SignalTimeDependent<MatrixForce,int> handVsensorSOUT;
dg::SignalPtr<dynamicgraph::Vector,int> torsorDeadZoneSIN;
dg::SignalTimeDependent<MatrixForce,int> sensorXhandSOUT;
//dg::SignalTimeDependent<dynamicgraph::Matrix,int> inertiaSensorSOUT;
dg::SignalTimeDependent<dynamicgraph::Vector,int> momentumSOUT;
dg::SignalPtr<dynamicgraph::Vector,int> momentumSIN;
/* --- OUTPUTS --- */
dg::SignalTimeDependent<dynamicgraph::Vector,int> torsorCompensatedSOUT;
dg::SignalTimeDependent<dynamicgraph::Vector,int> torsorDeadZoneSOUT;
typedef int sotDummyType;
dg::SignalTimeDependent<sotDummyType,int> calibrationTrigerSOUT;
public: /* --- COMMANDLINE --- */
sotDummyType& calibrationTriger( sotDummyType& dummy,int time );
};
} // namaspace sot
} // namespace dynamicgraph
#endif // #ifndef __SOT_SOTFORCECOMPENSATION_H__
include/sot-dynamic-pinocchio/mass-apparent.h deleted 100644 → 0
View file @ d3b4bdb1
/*
* Copyright 2010,
* François Bleibel,
* Olivier Stasse,
*
* CNRS/AIST
*
*/
#ifndef __SOT_SOTMASSAPPARENT_H__
#define __SOT_SOTMASSAPPARENT_H__
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* Matrix */
#include <dynamic-graph/linear-algebra.h>
/* SOT */
#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 */
#include <string>
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined (WIN32)
# if defined (mass_apparent_EXPORTS)
# define SOTMASSAPPARENT_EXPORT __declspec(dllexport)
# else
# define SOTMASSAPPARENT_EXPORT __declspec(dllimport)
# endif
#else
# define SOTMASSAPPARENT_EXPORT
#endif
namespace dynamicgraph { namespace sot {
namespace dg = dynamicgraph;
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
class SOTMASSAPPARENT_EXPORT MassApparent
:public dg::Entity
{
public:
static const std::string CLASS_NAME;
virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
public: /* --- CONSTRUCTION --- */
MassApparent( const std::string& name );
virtual ~MassApparent( void );
public: /* --- SIGNAL --- */
dg::SignalPtr<dynamicgraph::Matrix,int> jacobianSIN;
dg::SignalPtr<dynamicgraph::Matrix,int> inertiaInverseSIN;
dg::SignalTimeDependent<dynamicgraph::Matrix,int> massInverseSOUT;
dg::SignalTimeDependent<dynamicgraph::Matrix,int> massSOUT;
dg::SignalPtr<dynamicgraph::Matrix,int> inertiaSIN;
dg::SignalTimeDependent<dynamicgraph::Matrix,int> inertiaInverseSOUT;
public: /* --- FUNCTIONS --- */
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 );
};
} // namespace sot
} // namespace dynamicgraph
#endif // #ifndef __SOT_SOTMASSAPPARENT_H__
include/sot-dynamic-pinocchio/angle-estimator.h → include/sot/dynamic-pinocchio/angle-estimator.h
View file @ a6894863
......@@ -13,14 +13,14 @@
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined (WIN32)
# if defined (angle_estimator_EXPORTS)
# define SOTANGLEESTIMATOR_EXPORT __declspec(dllexport)
# else
# define SOTANGLEESTIMATOR_EXPORT __declspec(dllimport)
# endif
#if defined(WIN32)
#if defined(angle_estimator_EXPORTS)
#define SOTANGLEESTIMATOR_EXPORT __declspec(dllexport)
#else
# define SOTANGLEESTIMATOR_EXPORT
#define SOTANGLEESTIMATOR_EXPORT __declspec(dllimport)
#endif
#else
#define SOTANGLEESTIMATOR_EXPORT
#endif
/* --------------------------------------------------------------------- */
......@@ -30,87 +30,89 @@
/* Matrix */
#include <dynamic-graph/linear-algebra.h>
/* SOT */
#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/core/matrix-geometry.hh>
/* STD */
#include <string>
namespace dynamicgraph { namespace sot {
namespace dynamicgraph {
namespace sot {
namespace dg = dynamicgraph;
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
class SOTANGLEESTIMATOR_EXPORT AngleEstimator
:public dg::Entity
{
class SOTANGLEESTIMATOR_EXPORT AngleEstimator : public dg::Entity {
public:
static const std::string CLASS_NAME;
virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
virtual const std::string& getClassName(void) const { return CLASS_NAME; }
public: /* --- CONSTRUCTION --- */
AngleEstimator( const std::string& name );
virtual ~AngleEstimator( void );
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
dg::SignalPtr<dynamicgraph::Matrix,int> jacobianSIN;
dg::SignalPtr<dynamicgraph::Vector,int> qdotSIN;
dg::SignalTimeDependent<dynamicgraph::Vector,int> xff_dotSOUT;
dg::SignalTimeDependent<dynamicgraph::Vector,int> qdotSOUT;
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;
dg::SignalTimeDependent<dynamicgraph::Vector, int> xff_dotSOUT;
dg::SignalTimeDependent<dynamicgraph::Vector, int> qdotSOUT;
public: /* --- FUNCTIONS --- */
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 );
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);
public: /* --- PARAMS --- */
void fromSensor(const bool& fs) { fromSensor_ = fs; }
bool fromSensor() const { return fromSensor_; }
bool fromSensor() const { return fromSensor_; }
private:
bool fromSensor_;
};
} /* namespace sot */
} /* namespace dynamicgraph */
} /* namespace sot */} /* namespace dynamicgraph */
#endif // #ifndef __SOT_ANGLE_ESTIMATOR_H__
#endif // #ifndef __SOT_ANGLE_ESTIMATOR_H__
include/sot/dynamic-pinocchio/dynamic-pinocchio.h 0 → 100644
View file @ a6894863
/*
* Copyright 2010,
* François Bleibel,
* Olivier Stasse,
*
* CNRS/AIST
*
*/
#ifndef __SOT_DYNAMIC_PINOCCHIO_H__
#define __SOT_DYNAMIC_PINOCCHIO_H__
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* STD */
#include <map>
#include <memory>
#include <string>
/* pinocchio */
#include <pinocchio/fwd.hpp>
/* 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>
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined(WIN32)
#if defined(dynamic_EXPORTS)
#define SOTDYNAMIC_EXPORT __declspec(dllexport)
#else
#define SOTDYNAMIC_EXPORT __declspec(dllimport)
#endif
#else
#define SOTDYNAMIC_EXPORT
#endif
namespace dynamicgraph {
namespace sot {
namespace dg = dynamicgraph;
namespace command {
class SetFile;
class CreateOpPoint;
} // namespace command
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/*! @ingroup signals
\brief This class provides an inverse dynamic model of the robot.
More precisely it wraps the newton euler algorithm implemented
by the dynamicsJRLJapan library to make it accessible in the stack of tasks.
The robot is described by a VRML file.
*/
class SOTDYNAMIC_EXPORT DynamicPinocchio : public dg::Entity {
friend class sot::command::SetFile;
friend class sot::command::CreateOpPoint;
// friend class sot::command::InitializeRobot;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
DYNAMIC_GRAPH_ENTITY_DECL();
/* --- MODEL ATRIBUTES --- */
pinocchio::Model* m_model;
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&);
void destroyJacobianSignal(const std::string& signame);
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&);
void destroyVelocitySignal(const std::string& signame);
dg::SignalTimeDependent<dg::Vector, int>& createAccelerationSignal(
const std::string&, const std::string&);
void destroyAccelerationSignal(const std::string& signame);
/*! @} */
std::list<dg::SignalBase<int>*> genericSignalRefs;
public:
/* --- SIGNAL --- */
typedef int Dummy;
dg::SignalPtr<dg::Vector, int> jointPositionSIN;
dg::SignalPtr<dg::Vector, int> freeFlyerPositionSIN;
dg::SignalPtr<dg::Vector, int> jointVelocitySIN;
dg::SignalPtr<dg::Vector, int> freeFlyerVelocitySIN;
dg::SignalPtr<dg::Vector, int> jointAccelerationSIN;
dg::SignalPtr<dg::Vector, int> freeFlyerAccelerationSIN;
dg::SignalTimeDependent<dg::Vector, int> pinocchioPosSINTERN;
dg::SignalTimeDependent<dg::Vector, int> pinocchioVelSINTERN;
dg::SignalTimeDependent<dg::Vector, int> pinocchioAccSINTERN;
dg::SignalTimeDependent<Dummy, int> newtonEulerSINTERN;
dg::SignalTimeDependent<Dummy, int> jacobiansSINTERN;
dg::SignalTimeDependent<Dummy, int> forwardKinematicsSINTERN;
dg::SignalTimeDependent<Dummy, int> ccrbaSINTERN;
int& computeNewtonEuler(int& dummy, const int& time);
int& computeForwardKinematics(int& dummy, const int& time);
int& computeCcrba(int& dummy, const int& time);
int& computeJacobians(int& dummy, const int& time);
dg::SignalTimeDependent<dg::Vector, int> zmpSOUT;
dg::SignalTimeDependent<dg::Matrix, int> JcomSOUT;
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<double, int> footHeightSOUT;
dg::SignalTimeDependent<dg::Vector, int> upperJlSOUT;
dg::SignalTimeDependent<dg::Vector, int> lowerJlSOUT;
dg::SignalTimeDependent<dg::Vector, int> upperVlSOUT;
dg::SignalTimeDependent<dg::Vector, int> upperTlSOUT;
dg::Signal<dg::Vector, int> inertiaRotorSOUT;
dg::Signal<dg::Vector, int> gearRatioSOUT;
dg::SignalTimeDependent<dg::Matrix, int> inertiaRealSOUT;
dg::SignalTimeDependent<dg::Vector, int> MomentaSOUT;
dg::SignalTimeDependent<dg::Vector, int> AngularMomentumSOUT;
dg::SignalTimeDependent<dg::Vector, int> dynamicDriftSOUT;
public:
/* --- CONSTRUCTOR --- */
DynamicPinocchio(const std::string& name);
virtual ~DynamicPinocchio(void);
/* --- MODEL CREATION --- */
void displayModel() const {
assert(m_model);
std::cout << (*m_model) << std::endl;
};
void setModel(pinocchio::Model*);
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 --- */
/// \brief Get joint position lower limits
///
/// \param[out] result vector
/// \return result vector
dg::Vector& getLowerPositionLimits(dg::Vector& res, const int&) const;
/// \brief Get joint position upper limits
///
/// \param[out] result vector
/// \return result vector
dg::Vector& getUpperPositionLimits(dg::Vector& res, const int&) const;
/// \brief Get joint velocity upper limits
///
/// \param[out] result vector
/// \return result vector
dg::Vector& getUpperVelocityLimits(dg::Vector& res, const int&) const;
/// \brief Get joint effort upper limits
///
/// \param[out] result vector
/// \return result vector
dg::Vector& getMaxEffortLimits(dg::Vector& res, const int&) const;
// 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,
const int& time);
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& computeZmp(dg::Vector& res, const int& time);
dg::Vector& computeMomenta(dg::Vector& res, const int& time);
dg::Vector& computeAngularMomentum(dg::Vector& res, const int& time);
dg::Matrix& computeJcom(dg::Matrix& res, const int& time);
dg::Vector& computeCom(dg::Vector& res, const int& time);
dg::Matrix& computeInertia(dg::Matrix& res, const int& time);
dg::Matrix& computeInertiaReal(dg::Matrix& res, const int& time);
double& computeFootHeight(double& res, const int& time);
dg::Vector& computeTorqueDrift(dg::Vector& res, const int& time);
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_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);
private:
/// \brief map of joints in construction.
/// 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).
std::vector<int> sphericalJoints;
};
// std::ostream& operator<<(std::ostream& os, const CjrlJoint& r);
} /* namespace sot */
} /* namespace dynamicgraph */
#endif // #ifndef __SOT_DYNAMIC_PINOCCHIO_H__