├── .git-blame-ignore-revs ├── .gitignore ├── .gitlab-ci.yml ├── .gitmodules ├── .mailmap ├── .pre-commit-config.yaml ├── AUTHORS ├── CMakeLists.txt ├── ChangeLog ├── INSTALL ├── LICENSE ├── NEWS ├── README.md ├── doc ├── CMakeLists.txt ├── Doxyfile.extra.in ├── additionalDoc │ ├── a-features.h │ ├── codeorg.h │ ├── e-operators.h │ ├── feature-doc.h │ ├── install-doc.h │ ├── namespace-doc.h │ ├── package.h │ ├── parameter-server-doc.h │ └── sot-doc.h ├── pictures │ ├── HRP2.jpg │ ├── feature.fig │ ├── feature.png │ ├── footer.jpg │ ├── footer.txt │ ├── task.fig │ └── task.png └── references.bib ├── include ├── CMakeLists.txt └── sot │ └── core │ ├── abstract-sot-external-interface.hh │ ├── additional-functions.hh │ ├── admittance-control-op-point.hh │ ├── api.hh │ ├── binary-int-to-uint.hh │ ├── binary-op.hh │ ├── causal-filter.hh │ ├── clamp-workspace.hh │ ├── com-freezer.hh │ ├── contiifstream.hh │ ├── control-gr.hh │ ├── control-pd.hh │ ├── debug.hh │ ├── derivator-impl.hh │ ├── derivator.hh │ ├── device.hh │ ├── double-constant.hh │ ├── event.hh │ ├── exception-abstract.hh │ ├── exception-dynamic.hh │ ├── exception-factory.hh │ ├── exception-feature.hh │ ├── exception-signal.hh │ ├── exception-task.hh │ ├── exception-tools.hh │ ├── exp-moving-avg.hh │ ├── factory.hh │ ├── feature-1d.hh │ ├── feature-abstract.hh │ ├── feature-generic.hh │ ├── feature-joint-limits.hh │ ├── feature-line-distance.hh │ ├── feature-point6d-relative.hh │ ├── feature-point6d.hh │ ├── feature-pose.hh │ ├── feature-pose.hxx │ ├── feature-posture.hh │ ├── feature-task.hh │ ├── feature-vector3.hh │ ├── feature-visual-point.hh │ ├── filter-differentiator.hh │ ├── fir-filter-impl.hh │ ├── fir-filter.hh │ ├── flags.hh │ ├── fwd.hh │ ├── gain-adaptive.hh │ ├── gain-hyperbolic.hh │ ├── gradient-ascent.hh │ ├── gripper-control.hh │ ├── integrator-abstract-impl.hh │ ├── integrator-abstract.hh │ ├── integrator-euler-impl.hh │ ├── integrator-euler.hh │ ├── joint-limitator.hh │ ├── joint-trajectory-entity.hh │ ├── kalman.hh │ ├── latch.hh │ ├── macros-signal.hh │ ├── macros.hh │ ├── madgwickahrs.hh │ ├── mailbox-vector.hh │ ├── mailbox.hh │ ├── mailbox.hxx │ ├── matrix-constant.hh │ ├── matrix-geometry.hh │ ├── matrix-svd.hh │ ├── memory-task-sot.hh │ ├── motion-period.hh │ ├── multi-bound.hh │ ├── neck-limitation.hh │ ├── op-point-modifier.hh │ ├── parameter-server.hh │ ├── periodic-call-entity.hh │ ├── periodic-call.hh │ ├── pool.hh │ ├── reader.hh │ ├── robot-simu.hh │ ├── robot-utils.hh │ ├── sequencer.hh │ ├── smooth-reach.hh │ ├── sot-loader.hh │ ├── sot.hh │ ├── stop-watch.hh │ ├── switch.hh │ ├── task-abstract.hh │ ├── task-conti.hh │ ├── task-pd.hh │ ├── task-unilateral.hh │ ├── task.hh │ ├── time-stamp.hh │ ├── timer.hh │ ├── trajectory.hh │ ├── unary-op.hh │ ├── utils-windows.hh │ ├── variadic-op.hh │ ├── vector-constant.hh │ ├── vector-to-rotation.hh │ └── visual-point-projecter.hh ├── package.xml ├── pyproject.toml ├── setup.cfg ├── src ├── CMakeLists.txt ├── control │ ├── admittance-control-op-point-python.h │ ├── admittance-control-op-point.cpp │ ├── control-gr-python.h │ ├── control-gr.cpp │ ├── control-pd-python.h │ └── control-pd.cpp ├── debug │ ├── contiifstream.cpp │ └── debug.cpp ├── dynamic_graph │ ├── __init__.py │ └── sot │ │ ├── __init__.py │ │ └── core │ │ ├── __init__.py │ │ ├── feature_position.py │ │ ├── feature_position_relative.py │ │ ├── math_small_entities.py │ │ ├── matrix_util.py │ │ ├── meta_task_6d.py │ │ ├── meta_task_posture.py │ │ ├── meta_task_visual_point.py │ │ ├── meta_tasks.py │ │ ├── meta_tasks_kine.py │ │ ├── meta_tasks_kine_relative.py │ │ └── utils │ │ ├── __init__.py │ │ ├── attime.py │ │ ├── history.py │ │ ├── thread_interruptible_loop.py │ │ ├── viewer_helper.py │ │ └── viewer_loger.py ├── exception │ ├── exception-abstract.cpp │ ├── exception-dynamic.cpp │ ├── exception-factory.cpp │ ├── exception-feature.cpp │ ├── exception-signal.cpp │ ├── exception-task.cpp │ └── exception-tools.cpp ├── factory │ ├── additional-functions.cpp │ ├── factory.cpp │ └── pool.cpp ├── feature │ ├── feature-1d-python.h │ ├── feature-1d.cpp │ ├── feature-abstract.cpp │ ├── feature-generic-python.h │ ├── feature-generic.cpp │ ├── feature-joint-limits-command.h │ ├── feature-joint-limits-python.h │ ├── feature-joint-limits.cpp │ ├── feature-line-distance-python.h │ ├── feature-line-distance.cpp │ ├── feature-point6d-python.h │ ├── feature-point6d-relative-python.h │ ├── feature-point6d-relative.cpp │ ├── feature-point6d.cpp │ ├── feature-pose-python.h │ ├── feature-pose.cpp │ ├── feature-posture-python.h │ ├── feature-posture.cpp │ ├── feature-task-python.h │ ├── feature-task.cpp │ ├── feature-vector3-python.h │ ├── feature-vector3.cpp │ ├── feature-visual-point-python.h │ ├── feature-visual-point.cpp │ ├── visual-point-projecter-python.h │ └── visual-point-projecter.cpp ├── filters │ ├── causal-filter.cpp │ ├── filter-differentiator-python.h │ ├── filter-differentiator.cpp │ ├── madgwickahrs-python.h │ └── madgwickahrs.cpp ├── math │ ├── matrix-rotation.cpp │ ├── op-point-modifier-python.h │ ├── op-point-modifier.cpp │ └── vector-quaternion.cpp ├── matrix │ ├── derivator-python.h │ ├── derivator.cpp │ ├── double-constant-python.h │ ├── double-constant.cpp │ ├── fir-filter-python.h │ ├── fir-filter.cpp │ ├── integrator-abstract-python.h │ ├── integrator-abstract.cpp │ ├── integrator-euler-python-module-py.cc │ ├── integrator-euler.cpp │ ├── integrator-euler.t.cpp │ ├── matrix-constant-command.h │ ├── matrix-constant-python.h │ ├── matrix-constant.cpp │ ├── matrix-svd.cpp │ ├── operator-python-module-py.cc │ ├── operator.cpp │ ├── operator.hh │ ├── vector-constant-command.h │ ├── vector-constant-python.h │ ├── vector-constant.cpp │ ├── vector-to-rotation-python.h │ └── vector-to-rotation.cpp ├── python-module.cc ├── signal │ └── signal-cast.cpp ├── sot │ ├── flags.cpp │ ├── memory-task-sot.cpp │ ├── sot-command.h │ ├── sot-python.h │ └── sot.cpp ├── task │ ├── gain-adaptive-python.h │ ├── gain-adaptive.cpp │ ├── gain-hyperbolic-python.h │ ├── gain-hyperbolic.cpp │ ├── multi-bound.cpp │ ├── task-abstract.cpp │ ├── task-command.h │ ├── task-conti-python.h │ ├── task-conti.cpp │ ├── task-pd-python.h │ ├── task-pd.cpp │ ├── task-python.h │ ├── task-unilateral-python.h │ ├── task-unilateral.cpp │ └── task.cpp ├── tools │ ├── binary-int-to-uint-python.h │ ├── binary-int-to-uint.cpp │ ├── clamp-workspace-python.h │ ├── clamp-workspace.cpp │ ├── com-freezer-python.h │ ├── com-freezer.cpp │ ├── device.cpp │ ├── event-python.h │ ├── event.cpp │ ├── exp-moving-avg-python.h │ ├── exp-moving-avg.cpp │ ├── gradient-ascent-python.h │ ├── gradient-ascent.cpp │ ├── gripper-control-python.h │ ├── gripper-control.cpp │ ├── joint-limitator-python.h │ ├── joint-limitator.cpp │ ├── joint-trajectory-command.hh │ ├── joint-trajectory-entity-python.h │ ├── joint-trajectory-entity.cpp │ ├── kalman-doc.tex │ ├── kalman-python.h │ ├── kalman.cpp │ ├── latch-python.h │ ├── latch.cpp │ ├── mailbox-vector-python.h │ ├── mailbox-vector.cpp │ ├── motion-period-python.h │ ├── motion-period.cpp │ ├── neck-limitation-python.h │ ├── neck-limitation.cpp │ ├── parameter-server-python.h │ ├── parameter-server.cpp │ ├── periodic-call-entity-python.h │ ├── periodic-call-entity.cpp │ ├── periodic-call.cpp │ ├── robot-simu-python.h │ ├── robot-simu.cpp │ ├── robot-utils-py.cpp │ ├── robot-utils.cpp │ ├── sequencer-python.h │ ├── sequencer.cpp │ ├── smooth-reach-python.h │ ├── smooth-reach.cpp │ ├── sot-loader.cpp │ ├── switch-python-module-py.cc │ ├── switch.cpp │ ├── time-stamp-python.h │ ├── time-stamp.cpp │ ├── timer-python.h │ ├── timer.cpp │ ├── trajectory.cpp │ ├── type-name-helper.hh │ └── utils-windows.cpp ├── traces │ ├── reader-python.h │ └── reader.cpp └── utils │ └── stop-watch.cpp └── tests ├── CMakeLists.txt ├── control ├── test_control_admittance.cpp └── test_control_pd.cpp ├── dummy.cpp ├── factory └── test_factory.cpp ├── features ├── test_feature_generic.cpp └── test_feature_point6d.cpp ├── filters ├── test_filter_differentiator.cpp ├── test_madgwick_ahrs.cpp └── test_madgwick_arhs.cpp ├── math ├── matrix-homogeneous.cpp └── matrix-twist.cpp ├── matrix └── test_operator.cpp ├── python ├── CMakeLists.txt ├── parameter_server_conf.py ├── test-imports.py ├── test-initialize-euler.py ├── test-matrix-util.py ├── test-op-point-modifier.py ├── test-parameter-server.py └── test-smooth-reach.py ├── signal ├── test_dep.cpp ├── test_depend.cpp ├── test_ptr.cpp ├── test_ptrcast.cpp └── test_signal.cpp ├── sot ├── test_solverSoth.cpp └── tsot.cpp ├── task ├── test_flags.cpp ├── test_gain.cpp ├── test_multi_bound.cpp └── test_task.cpp ├── test-paths.h.in ├── tools ├── bad_python_script ├── dummy-sot-external-interface.cc ├── dummy-sot-external-interface.hh ├── good_python_script.py ├── plugin.cc ├── plugin.hh ├── test_abstract_interface.cpp ├── test_boost.cpp ├── test_control_pd.cpp ├── test_device.cpp ├── test_mailbox.cpp ├── test_matrix.cpp ├── test_robot_utils.cpp └── test_sot_loader.cpp └── traces ├── files.cpp └── test_traces.cpp /.git-blame-ignore-revs: -------------------------------------------------------------------------------- 1 | # # pre-commit run -a (Guilhem Saurel, 2022-09-05) 2 | f3f2efc3fee4179e170e76e5f983baf004d7e109 3 | 4 | # pre-commit run -a (Guilhem Saurel, 2022-07-27) 5 | 2a9fbd70d4fc47ed41e0ad25fb90e241865a341b 6 | 7 | # yapf (Guilhem Saurel, 2021-02-09) 8 | cf574bd33e0bb0cb5a54b83f8f434efe8951cd0e 9 | 10 | # format (Guilhem Saurel, 2020-11-13) 11 | 5921e17ab3a8db2a98ae94d228283db924d2e140 12 | 13 | # clang-format (Olivier Stasse, 2020-08-28) 14 | 2ea1fba7f262c07652dc060361c98af856d4d935 15 | 16 | # [format] Apply clang (Olivier Stasse, 2020-08-26) 17 | d59a58ec2de3a192da13c59ebce92e704f9d00c4 18 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | build* 2 | vsbld* 3 | include/sot-core/import-default-paths.h 4 | unitTesting/test-paths.h 5 | *~ 6 | _build* 7 | *__pycache__* 8 | -------------------------------------------------------------------------------- /.gitlab-ci.yml: -------------------------------------------------------------------------------- 1 | include: http://rainboard.laas.fr/project/sot-core/.gitlab-ci.yml 2 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "cmake"] 2 | path = cmake 3 | url = https://github.com/jrl-umi3218/jrl-cmakemodules.git 4 | -------------------------------------------------------------------------------- /.mailmap: -------------------------------------------------------------------------------- 1 | # 2 | # This list is used by git-shortlog to fix a few botched name translations 3 | # in the git archive, either because the author's full name was messed up 4 | # and/or not always written the same way, making contributions from the 5 | # same person appearing not to be so or badly displayed. 6 | 7 | Anthony Mallet 8 | 9 | Olivier Stasse 10 | Olivier Stasse 11 | Olivier Stasse 12 | Olivier Stasse 13 | Olivier Stasse 14 | Olivier Stasse 15 | Olivier Stasse 16 | Olivier Stasse 17 | Olivier Stasse 18 | Olivier Stasse 19 | Olivier Stasse 20 | Olivier Stasse 21 | Olivier Stasse 22 | 23 | 24 | Nicolas Mansard 25 | Nicolas Mansard 26 | Nicolas Mansard 27 | 28 | 29 | Florent Lamiraux 30 | Florent Lamiraux 31 | Florent Lamiraux 32 | Florent Lamiraux 33 | Florent Lamiraux 34 | Florent Lamiraux 35 | Florent Lamiraux 36 | Florent Lamiraux 37 | Florent Lamiraux 38 | Florent Lamiraux 39 | Florent Lamiraux 40 | -------------------------------------------------------------------------------- /.pre-commit-config.yaml: -------------------------------------------------------------------------------- 1 | ci: 2 | autoupdate_branch: 'devel' 3 | repos: 4 | - repo: https://github.com/pre-commit/mirrors-clang-format 5 | rev: v14.0.6 6 | hooks: 7 | - id: clang-format 8 | args: [--style=Google] 9 | - repo: https://github.com/pre-commit/pre-commit-hooks 10 | rev: v4.3.0 11 | hooks: 12 | - id: check-added-large-files 13 | - id: check-ast 14 | - id: check-executables-have-shebangs 15 | - id: check-json 16 | - id: check-merge-conflict 17 | - id: check-symlinks 18 | - id: check-toml 19 | - id: check-yaml 20 | - id: debug-statements 21 | - id: destroyed-symlinks 22 | - id: detect-private-key 23 | - id: end-of-file-fixer 24 | - id: fix-byte-order-marker 25 | - id: mixed-line-ending 26 | - id: trailing-whitespace 27 | - repo: https://github.com/psf/black 28 | rev: 22.8.0 29 | hooks: 30 | - id: black 31 | - repo: https://github.com/PyCQA/flake8 32 | rev: 5.0.4 33 | hooks: 34 | - id: flake8 35 | - repo: https://github.com/cheshirekow/cmake-format-precommit 36 | rev: v0.6.13 37 | hooks: 38 | - id: cmake-format 39 | -------------------------------------------------------------------------------- /AUTHORS: -------------------------------------------------------------------------------- 1 | This package was written by and with the assistance of 2 | 3 | * François Bleibel fbleibel@gmail.com 4 | * François Keith francois.keith@aist.go.jp 5 | * Nicolas Mansard FIXME 6 | * Thomas Moulardd thomas.moulard@gmail.com 7 | * Olivier Stasse olivier.stasse@aist.go.jp 8 | -------------------------------------------------------------------------------- /INSTALL: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright 3 | # 4 | 5 | Installation instructions for library sot-core 6 | ---------------------------------------------- 7 | 8 | It is recommended to create a specific directory to install this package. 9 | mkdir build 10 | cd build 11 | cmake [Options] .. 12 | make 13 | make install 14 | 15 | Options: 16 | 17 | -DCMAKE_INSTALL_PREFIX=... 18 | specifies the directory where to install the package. 19 | 20 | -DCMAKE_BUILD_TYPE=[none|debug|release|relwithdebinfo|MinSizeRel] 21 | specifies to type of compilation (release, debug, ...) 22 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 2-Clause License 2 | 3 | Copyright (c) 2010-2019, Stack Of Tasks development team 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | * Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | * Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 25 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # sot-core 2 | 3 | [![License](https://img.shields.io/badge/License-BSD%202--Clause-green.svg)](https://opensource.org/licenses/BSD-2-Clause) 4 | [![Pipeline status](https://gitlab.laas.fr/stack-of-tasks/sot-core/badges/master/pipeline.svg)](https://gitlab.laas.fr/stack-of-tasks/sot-core/commits/master) 5 | [![Coverage report](https://gitlab.laas.fr/stack-of-tasks/sot-core/badges/master/coverage.svg?job=doc-coverage)](https://gepettoweb.laas.fr/doc/stack-of-tasks/sot-core/master/coverage/) 6 | [![Code style: black](https://img.shields.io/badge/code%20style-black-000000.svg)](https://github.com/psf/black) 7 | [![pre-commit.ci status](https://results.pre-commit.ci/badge/github/stack-of-tasks/sot-core/master.svg)](https://results.pre-commit.ci/latest/github/stack-of-tasks/sot-core) 8 | 9 | 10 | This software provides a set of dynamic graph plug-ins which can be 11 | used to define and solve hierarchical tasks. 12 | 13 | 14 | ## Setup 15 | 16 | To compile this package, it is recommended to create a separate build 17 | directory: 18 | 19 | mkdir _build 20 | cd _build 21 | cmake [OPTIONS] .. 22 | make install 23 | 24 | Please note that CMake produces a `CMakeCache.txt` file which should 25 | be deleted to reconfigure a package from scratch. 26 | -------------------------------------------------------------------------------- /doc/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright 2010 CNRS Author: Florent Lamiraux 3 | # 4 | 5 | # get the python script install path 6 | 7 | if(INSTALL_DOCUMENTATION) 8 | install(FILES pictures/feature.png pictures/task.png 9 | DESTINATION ${_PKG_CONFIG_DOXYGENDOCDIR}/pictures) 10 | endif(INSTALL_DOCUMENTATION) 11 | -------------------------------------------------------------------------------- /doc/Doxyfile.extra.in: -------------------------------------------------------------------------------- 1 | INPUT = @CMAKE_SOURCE_DIR@/include \ 2 | @CMAKE_SOURCE_DIR@/doc/additionalDoc 3 | IMAGE_PATH = @CMAKE_SOURCE_DIR@/doc/pictures 4 | 5 | FILE_PATTERNS = *.cc *.cpp *.h *.hh *.hxx 6 | 7 | COLLABORATION_GRAPH = NO 8 | 9 | CITE_BIB_FILES = @CMAKE_SOURCE_DIR@/doc/references.bib 10 | -------------------------------------------------------------------------------- /doc/additionalDoc/a-features.h: -------------------------------------------------------------------------------- 1 | /** 2 | \defgroup Filters Filters 3 | */ 4 | -------------------------------------------------------------------------------- /doc/additionalDoc/codeorg.h: -------------------------------------------------------------------------------- 1 | /** \page codeorganization Code organization 2 | 3 | \section sec_organization Organization of the code 4 | 5 | The code is based on the dynamic-graph package, which provides the 6 | framework on which sot-core relies. Hence most of the code in sot-core 7 | consists of classes that derive from dynamic_graph::Entity. 8 | These entities are usually 9 | compiled and linked in their own dynamic library, as a python module 10 | 11 | Aside from the python modules, this package installs a core library libsot-core, 12 | that provides functions and code common to all modules. All python modules 13 | developed here link with libsot-core. For example, common mathematical 14 | entities, definitions and functions are in the core library. 15 | See \ref sot_core_base for a list of what's in this code base. 16 | 17 | This library implements 18 | \li A solver for the control law using tasks and features: the stack of tasks 19 | \li The framework of Tasks and Features, as defined in \cite Mansard2007, that 20 | \li Base classes for working with tasks and features, such as special matrices 21 | (rotation, twist) and vectors (quaternions, etc.) 22 | \li A basic C++ "machinery", the \ref factory, that "makes things work" 23 | 24 | */ 25 | -------------------------------------------------------------------------------- /doc/additionalDoc/e-operators.h: -------------------------------------------------------------------------------- 1 | /** 2 | \page subp_operators Operators on signals 3 | 4 | \section subp_unitary_op Unitary Algebraic operators 5 | 6 | \subsection ssubp_unitary_op Vector selector 7 | 8 | This entity output a vector remapping an input vector 9 | \f${\bf v}_{in}=[v_0,v_1,\cdots,v_n]\f$. 10 | It is realized by specifying bounds such as \f$({[i,j],[k,l]})\f$, 11 | then the output vector will be the contanetion of the 12 | intervals extracted from the input vector: 13 | \f${\bf v}_{out}=[v_i,v_{i+1},\cdots,v_{j-1},v_{j},v_{k}, 14 | v_{k+1},\cdots,v_{l-1},v_l]\f$ 15 | For instance if we have an input vector 16 | such that: 17 | \code 18 | 1 19 | 2 20 | 3 21 | 4 22 | 5 23 | 6 24 | 7 25 | 9 26 | 10 27 | \endcode 28 | then specifying the bounds \f$(3,5)\f$ and \f$( 7,10\f$) will gives 29 | the following output vector 30 | \code 31 | 3 32 | 4 33 | 7 34 | 8 35 | 9 36 | \endcode 37 | 38 | */ 39 | -------------------------------------------------------------------------------- /doc/additionalDoc/feature-doc.h: -------------------------------------------------------------------------------- 1 | /* Olivier Stasse 2 | * CNRS, LAAS 2019 3 | */ 4 | /** \page page_featuredoc Features 5 | 6 | The general definition of feature is provided by 7 | dynamicgraph::sot::FeatureAbstract . 8 | 9 | The list of features that are provided are: 10 |
    11 |
  • Feature1D : \f$ L_2 \f$ norm of \f$ e(t)\f$ 12 | dynamicgraph::sot::Feature1D
    • 13 |
  • FeatureGeneric : Externalisation of the Feature computation 14 | dynamicgraph::sot::FeatureGeneric
    • 15 |
  • FeatureJointLimits : Distance to the joint limits and its Jacobian 16 | dynamicgraph::sot::FeatureJointLimits
    • 17 |
  • FeatureLineDistance : Regulate the distance of a line through a body 18 | to a reference point 19 | dynamicgraph::sot::FeatureLineDistance
    • 20 |
  • FeaturePoint6d : Regulate the distance from a pose on a body to 21 | a point reference. 22 | dynamicgraph::sot::FeaturePoint6d
    • 23 |
  • FeaturePoint6dRelative : Regulate a relative distance between 24 | two bodies. 25 | dynamicgraph::sot::FeatuePoint6dRelative
    • 26 |
  • FeaturePosture : Regulate the posture between the current one 27 | and a reference. 28 | dynamicgraph::sot::FeaturePosture
    • 29 |
  • FeatureReferenceHelper : dynamicgraph::sot::FeatureReferenceHelper 30 |
    • 31 |
  • FeatureTask : dynamicgraph::sot::FeatureTask
    • 32 |
  • FeatureVector3 : dynamicgraph::sot::FeatureVector3
    • 33 |
  • FeatureVisualPoint : : dynamicgraph::sot::FeatureVisualPoint
    • 34 |
35 | */ 36 | -------------------------------------------------------------------------------- /doc/additionalDoc/install-doc.h: -------------------------------------------------------------------------------- 1 | /** \page page_RequirementsInstallation Requirements and Installation 2 | 3 | \section sec_requirements Requirements 4 | This package should be compiled and installed with cmake. 5 | 6 | This library is based on several packages: 7 | \li dynamic-graph (https://github.com/stack-of-tasks/dynamic-graph) 8 | \li eigen 9 | \li boost 10 | \li pthread 11 | \li python 12 | \li pinocchio (https://github.com/stack-of-tasks/pinocchio) 13 | 14 | Their presence will be checked with the use of pkg-config. 15 | 16 | \section sec_installation Installation 17 | \subsection subsec_ins_binaries From binary package 18 | The recommended way to install sot-core is to use 19 | robotpkg and its 20 | wip 21 | extension. 22 | 23 | A short and quick way to get it on 16.04 LTS is to do the following: 24 | \verbatim 25 | sudo tee /etc/apt/sources.list.d/robotpkg.list < 13 | #include 14 | #include 15 | #include 16 | 17 | namespace dynamicgraph { 18 | namespace sot { 19 | 20 | class SOT_CORE_EXPORT NamedVector { 21 | private: 22 | std::string name_; 23 | std::vector values_; 24 | 25 | public: 26 | NamedVector() {} 27 | ~NamedVector() {} 28 | 29 | const std::string &getName() const { return name_; } 30 | 31 | void setName(const std::string &aname) { name_ = aname; } 32 | 33 | const std::vector &getValues() const { return values_; } 34 | 35 | void setValues(const std::vector &values) { values_ = values; } 36 | }; 37 | typedef NamedVector SensorValues; 38 | typedef NamedVector ControlValues; 39 | 40 | class SOT_CORE_EXPORT AbstractSotExternalInterface { 41 | public: 42 | AbstractSotExternalInterface() {} 43 | 44 | virtual ~AbstractSotExternalInterface() {} 45 | 46 | virtual void setupSetSensors( 47 | std::map &sensorsIn) = 0; 48 | 49 | virtual void nominalSetSensors( 50 | std::map &sensorsIn) = 0; 51 | 52 | virtual void cleanupSetSensors( 53 | std::map &sensorsIn) = 0; 54 | 55 | virtual void getControl(std::map &) = 0; 56 | virtual void setSecondOrderIntegration(void) = 0; 57 | virtual void setNoIntegration(void) = 0; 58 | }; 59 | } // namespace sot 60 | } // namespace dynamicgraph 61 | 62 | typedef dynamicgraph::sot::AbstractSotExternalInterface * 63 | createSotExternalInterface_t(); 64 | typedef void destroySotExternalInterface_t( 65 | dynamicgraph::sot::AbstractSotExternalInterface *); 66 | 67 | #endif // ABSTRACT_SOT_EXTERNAL_INTERFACE_HH 68 | -------------------------------------------------------------------------------- /include/sot/core/additional-functions.hh: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2010, 3 | * François Bleibel, 4 | * Olivier Stasse, 5 | * 6 | * CNRS/AIST 7 | * 8 | */ 9 | 10 | /* SOT */ 11 | #include 12 | #include 13 | 14 | #include 15 | #include 16 | 17 | #include "sot/core/api.hh" 18 | 19 | /* --- STD --- */ 20 | #include 21 | #include 22 | #include 23 | 24 | /* --- BOOST --- */ 25 | #include 26 | #include 27 | 28 | namespace dynamicgraph { 29 | namespace sot { 30 | 31 | /*! @ingroup factory 32 | \brief This helper class dynamically overloads the "new" shell command 33 | to allow creation of tasks and features as well as entities. 34 | */ 35 | class AdditionalFunctions { 36 | public: 37 | AdditionalFunctions(); 38 | ~AdditionalFunctions(); 39 | static void cmdFlagSet(const std::string &cmd, std::istringstream &args, 40 | std::ostream &os); 41 | }; 42 | 43 | } /* namespace sot */ 44 | } /* namespace dynamicgraph */ 45 | -------------------------------------------------------------------------------- /include/sot/core/api.hh: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2010, 3 | * François Bleibel, 4 | * Olivier Stasse, 5 | * 6 | * CNRS/AIST 7 | * 8 | */ 9 | 10 | #ifndef SOT_CORE_API_HH 11 | #define SOT_CORE_API_HH 12 | 13 | #if defined(WIN32) 14 | #ifdef sot_core_EXPORTS 15 | #define SOT_CORE_EXPORT __declspec(dllexport) 16 | #else 17 | #define SOT_CORE_EXPORT __declspec(dllimport) 18 | #endif 19 | #else 20 | #define SOT_CORE_EXPORT 21 | #endif 22 | 23 | #endif 24 | -------------------------------------------------------------------------------- /include/sot/core/binary-int-to-uint.hh: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2010, 3 | * François Bleibel, 4 | * Olivier Stasse, 5 | * 6 | * CNRS/AIST 7 | * 8 | */ 9 | 10 | #ifndef __SOT_BINARY_INT_TO_UINT_HH__ 11 | #define __SOT_BINARY_INT_TO_UINT_HH__ 12 | 13 | /* --------------------------------------------------------------------- */ 14 | /* --- INCLUDE --------------------------------------------------------- */ 15 | /* --------------------------------------------------------------------- */ 16 | 17 | /* SOT */ 18 | #include 19 | #include 20 | 21 | #include 22 | 23 | /* --------------------------------------------------------------------- */ 24 | /* --- API ------------------------------------------------------------- */ 25 | /* --------------------------------------------------------------------- */ 26 | 27 | #if defined(WIN32) 28 | #if defined(binary_int_to_uint_EXPORTS) 29 | #define SOTBINARYINTTOUINT_EXPORT __declspec(dllexport) 30 | #else 31 | #define SOTBINARYINTTOUINT_EXPORT __declspec(dllimport) 32 | #endif 33 | #else 34 | #define SOTBINARYINTTOUINT_EXPORT 35 | #endif 36 | 37 | namespace dynamicgraph { 38 | namespace sot { 39 | 40 | class SOTBINARYINTTOUINT_EXPORT BinaryIntToUint : public dynamicgraph::Entity { 41 | public: 42 | static const std::string CLASS_NAME; 43 | virtual const std::string &getClassName(void) const { return CLASS_NAME; } 44 | 45 | /* --- SIGNALS ------------------------------------------------------------ */ 46 | public: 47 | dynamicgraph::SignalPtr binaryIntSIN; 48 | dynamicgraph::SignalTimeDependent binaryUintSOUT; 49 | 50 | public: 51 | BinaryIntToUint(const std::string &name); 52 | virtual ~BinaryIntToUint() {} 53 | 54 | virtual unsigned &computeOutput(unsigned &res, int time); 55 | 56 | virtual void display(std::ostream &os) const; 57 | }; 58 | 59 | } /* namespace sot */ 60 | } /* namespace dynamicgraph */ 61 | 62 | #endif 63 | -------------------------------------------------------------------------------- /include/sot/core/com-freezer.hh: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2010, 3 | * François Bleibel, 4 | * Olivier Stasse, 5 | * 6 | * CNRS/AIST 7 | * 8 | */ 9 | 10 | #ifndef __SOT_SOTCOMFREEZER_H_H 11 | #define __SOT_SOTCOMFREEZER_H_H 12 | 13 | /* --------------------------------------------------------------------- */ 14 | /* --- INCLUDE --------------------------------------------------------- */ 15 | /* --------------------------------------------------------------------- */ 16 | 17 | /* Matrix */ 18 | #include 19 | 20 | /* SOT */ 21 | #include 22 | #include 23 | 24 | /* --------------------------------------------------------------------- */ 25 | /* --- API ------------------------------------------------------------- */ 26 | /* --------------------------------------------------------------------- */ 27 | 28 | #if defined(WIN32) 29 | #if defined(com_freezer_EXPORTS) 30 | #define SOTCOMFREEZER_EXPORT __declspec(dllexport) 31 | #else 32 | #define SOTCOMFREEZER_EXPORT __declspec(dllimport) 33 | #endif 34 | #else 35 | #define SOTCOMFREEZER_EXPORT 36 | #endif 37 | 38 | namespace dynamicgraph { 39 | namespace sot { 40 | 41 | /* --------------------------------------------------------------------- */ 42 | /* --- CLASS ----------------------------------------------------------- */ 43 | /* --------------------------------------------------------------------- */ 44 | 45 | class SOTCOMFREEZER_EXPORT CoMFreezer : public dynamicgraph::Entity { 46 | public: 47 | static const std::string CLASS_NAME; 48 | virtual const std::string &getClassName() const { return CLASS_NAME; } 49 | 50 | private: 51 | dynamicgraph::Vector m_lastCoM; 52 | bool m_previousPGInProcess; 53 | int m_lastStopTime; 54 | 55 | public: /* --- CONSTRUCTION --- */ 56 | CoMFreezer(const std::string &name); 57 | virtual ~CoMFreezer(void); 58 | 59 | public: /* --- SIGNAL --- */ 60 | dynamicgraph::SignalPtr CoMRefSIN; 61 | dynamicgraph::SignalPtr PGInProcessSIN; 62 | dynamicgraph::SignalTimeDependent freezedCoMSOUT; 63 | 64 | public: /* --- FUNCTION --- */ 65 | dynamicgraph::Vector &computeFreezedCoM(dynamicgraph::Vector &freezedCoM, 66 | const int &time); 67 | 68 | public: /* --- PARAMS --- */ 69 | virtual void display(std::ostream &os) const; 70 | }; 71 | 72 | } /* namespace sot */ 73 | } /* namespace dynamicgraph */ 74 | 75 | #endif /* #ifndef __SOT_SOTCOMFREEZER_H_H */ 76 | -------------------------------------------------------------------------------- /include/sot/core/contiifstream.hh: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2010, 3 | * François Bleibel, 4 | * Olivier Stasse, 5 | * 6 | * CNRS/AIST 7 | * 8 | */ 9 | 10 | #ifndef __SOT_CONTIIFSTREAM_HH__ 11 | #define __SOT_CONTIIFSTREAM_HH__ 12 | 13 | /* --------------------------------------------------------------------- */ 14 | /* --- INCLUDE --------------------------------------------------------- */ 15 | /* --------------------------------------------------------------------- */ 16 | 17 | #include 18 | #include 19 | #ifndef WIN32 20 | #include 21 | #endif 22 | #include 23 | 24 | #include "sot/core/api.hh" 25 | #ifndef WIN32 26 | #include 27 | #endif 28 | 29 | namespace dynamicgraph { 30 | namespace sot { 31 | 32 | /* --------------------------------------------------------------------- */ 33 | /* --- CLASS ----------------------------------------------------------- */ 34 | /* --------------------------------------------------------------------- */ 35 | class SOT_CORE_EXPORT Contiifstream { 36 | protected: 37 | std::string filename; 38 | std::streamoff cursor; 39 | static const unsigned int BUFFER_SIZE = 256; 40 | char buffer[BUFFER_SIZE]; 41 | std::list reader; 42 | bool first; 43 | 44 | public: /* --- Constructor --- */ 45 | Contiifstream(const std::string &n = ""); 46 | ~Contiifstream(void); 47 | void open(const std::string &n) { 48 | filename = n; 49 | cursor = 0; 50 | } 51 | 52 | public: /* --- READ FILE --- */ 53 | bool loop(void); 54 | 55 | public: /* --- READ LIST --- */ 56 | inline bool ready(void) { return 0 < reader.size(); } 57 | std::string next(void); 58 | }; 59 | 60 | } /* namespace sot */ 61 | } /* namespace dynamicgraph */ 62 | 63 | #endif /* #ifndef __SOT_CONTIIFSTREAM_HH__ */ 64 | -------------------------------------------------------------------------------- /include/sot/core/derivator-impl.hh: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2010, 3 | * François Bleibel, 4 | * Olivier Stasse, 5 | * 6 | * CNRS/AIST 7 | * 8 | */ 9 | 10 | #ifndef __SOT_DERIVATOR_IMPL_H__ 11 | #define __SOT_DERIVATOR_IMPL_H__ 12 | 13 | #include 14 | #include 15 | 16 | /* --------------------------------------------------------------------- */ 17 | /* --- API ------------------------------------------------------------- */ 18 | /* --------------------------------------------------------------------- */ 19 | 20 | #if defined(WIN32) 21 | #if defined(derivator_EXPORTS) 22 | #define DERIVATOR_EXPORT __declspec(dllexport) 23 | #else 24 | #define DERIVATOR_EXPORT __declspec(dllimport) 25 | #endif 26 | #else 27 | #define DERIVATOR_EXPORT 28 | #endif 29 | 30 | /* --------------------------------------------------------------------- */ 31 | /* --- INCLUDE --------------------------------------------------------- */ 32 | /* --------------------------------------------------------------------- */ 33 | 34 | namespace dynamicgraph { 35 | namespace sot { 36 | 37 | #ifdef WIN32 38 | #define DECLARE_SPECIFICATION(className, sotSigType) \ 39 | class DERIVATOR_EXPORT className : public Derivator { \ 40 | public: \ 41 | className(const std::string &name); \ 42 | }; 43 | #else 44 | #define DECLARE_SPECIFICATION(className, sotSigType) \ 45 | typedef Derivator className; 46 | #endif 47 | 48 | DECLARE_SPECIFICATION(DerivatorDouble, double) 49 | DECLARE_SPECIFICATION(DerivatorVector, dynamicgraph::Vector) 50 | DECLARE_SPECIFICATION(DerivatorMatrix, dynamicgraph::Matrix) 51 | DECLARE_SPECIFICATION(DerivatorVectorQuaternion, VectorQuaternion) 52 | } /* namespace sot */ 53 | } /* namespace dynamicgraph */ 54 | 55 | #endif // #ifndef __SOT_DERIVATOR_H__ 56 | -------------------------------------------------------------------------------- /include/sot/core/double-constant.hh: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2019, 3 | * Joseph Mirabel 4 | * 5 | * LAAS-CNRS 6 | * 7 | */ 8 | 9 | #ifndef DYNAMICGRAPH_SOT_DOUBLE_CONSTANT_H 10 | #define DYNAMICGRAPH_SOT_DOUBLE_CONSTANT_H 11 | 12 | #include 13 | #include 14 | 15 | namespace dynamicgraph { 16 | namespace sot { 17 | 18 | class DoubleConstant : public Entity { 19 | public: 20 | static const std::string CLASS_NAME; 21 | virtual const std::string &getClassName(void) const { return CLASS_NAME; } 22 | 23 | DoubleConstant(const std::string &name); 24 | 25 | virtual ~DoubleConstant(void) {} 26 | 27 | SignalTimeDependent SOUT; 28 | 29 | /// \brief Set value of vector (and therefore of output signal) 30 | void setValue(const double &inValue); 31 | }; 32 | 33 | } // namespace sot 34 | } // namespace dynamicgraph 35 | 36 | #endif // DYNAMICGRAPH_SOT_DOUBLE_CONSTANT_H 37 | -------------------------------------------------------------------------------- /include/sot/core/exception-dynamic.hh: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2010, 3 | * François Bleibel, 4 | * Olivier Stasse, 5 | * 6 | * CNRS/AIST 7 | * 8 | */ 9 | 10 | #ifndef __SOT_DYNAMIC_EXCEPTION_H 11 | #define __SOT_DYNAMIC_EXCEPTION_H 12 | 13 | /* --------------------------------------------------------------------- */ 14 | /* --- INCLUDE --------------------------------------------------------- */ 15 | /* --------------------------------------------------------------------- */ 16 | 17 | #include 18 | 19 | #include "sot/core/api.hh" 20 | /* --------------------------------------------------------------------- */ 21 | /* --- CLASS ----------------------------------------------------------- */ 22 | /* --------------------------------------------------------------------- */ 23 | 24 | namespace dynamicgraph { 25 | namespace sot { 26 | 27 | /* \class ExceptionDynamic 28 | */ 29 | class SOT_CORE_EXPORT ExceptionDynamic : public ExceptionAbstract 30 | 31 | { 32 | public: 33 | enum ErrorCodeEnum { 34 | GENERIC = ExceptionAbstract::DYNAMIC 35 | 36 | , 37 | CANT_DESTROY_SIGNAL, 38 | JOINT_RANK, 39 | DYNAMIC_JRL, 40 | JOINT_SIZE, 41 | INTEGRATION 42 | }; 43 | 44 | static const std::string EXCEPTION_NAME; 45 | virtual const std::string &getExceptionName(void) const { 46 | return EXCEPTION_NAME; 47 | } 48 | 49 | public: 50 | ExceptionDynamic(const ExceptionDynamic::ErrorCodeEnum &errcode, 51 | const std::string &msg = ""); 52 | ExceptionDynamic(const ExceptionDynamic::ErrorCodeEnum &errcode, 53 | const std::string &msg, const char *format, ...); 54 | virtual ~ExceptionDynamic(void) throw() {} 55 | }; 56 | 57 | } /* namespace sot */ 58 | } /* namespace dynamicgraph */ 59 | 60 | #endif /* #ifndef __SOT_DYNAMIC_EXCEPTION_H */ 61 | 62 | /* 63 | * Local variables: 64 | * c-basic-offset: 2 65 | * End: 66 | */ 67 | -------------------------------------------------------------------------------- /include/sot/core/exception-factory.hh: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2010, 3 | * François Bleibel, 4 | * Olivier Stasse, 5 | * 6 | * CNRS/AIST 7 | * 8 | */ 9 | 10 | #ifndef __SOT_EXCEPTION_FACTORY_H 11 | #define __SOT_EXCEPTION_FACTORY_H 12 | 13 | /* --------------------------------------------------------------------- */ 14 | /* --- INCLUDE --------------------------------------------------------- */ 15 | /* --------------------------------------------------------------------- */ 16 | 17 | #include 18 | 19 | #include "sot/core/api.hh" 20 | /* --------------------------------------------------------------------- */ 21 | /* --- CLASS ----------------------------------------------------------- */ 22 | /* --------------------------------------------------------------------- */ 23 | 24 | namespace dynamicgraph { 25 | namespace sot { 26 | 27 | /* \class ExceptionFactory 28 | */ 29 | class SOT_CORE_EXPORT ExceptionFactory : public ExceptionAbstract 30 | 31 | { 32 | public: 33 | enum ErrorCodeEnum { 34 | GENERIC = ExceptionAbstract::FACTORY, 35 | UNREFERED_OBJECT, 36 | UNREFERED_SIGNAL, 37 | UNREFERED_FUNCTION, 38 | DYNAMIC_LOADING, 39 | SIGNAL_CONFLICT, 40 | FUNCTION_CONFLICT, 41 | OBJECT_CONFLICT, 42 | SYNTAX_ERROR // j' aime bien FATAL_ERROR aussi faut que je la case qq 43 | // part... 44 | , 45 | READ_FILE 46 | }; 47 | 48 | static const std::string EXCEPTION_NAME; 49 | virtual const std::string &getExceptionName(void) const { 50 | return ExceptionFactory::EXCEPTION_NAME; 51 | } 52 | 53 | ExceptionFactory(const ExceptionFactory::ErrorCodeEnum &errcode, 54 | const std::string &msg = ""); 55 | ExceptionFactory(const ExceptionFactory::ErrorCodeEnum &errcode, 56 | const std::string &msg, const char *format, ...); 57 | virtual ~ExceptionFactory(void) throw() {} 58 | }; 59 | 60 | } /* namespace sot */ 61 | } /* namespace dynamicgraph */ 62 | 63 | #endif /* #ifndef __SOT_EXCEPTION_FACTORY_H */ 64 | 65 | /* 66 | * Local variables: 67 | * c-basic-offset: 2 68 | * End: 69 | */ 70 | -------------------------------------------------------------------------------- /include/sot/core/exception-feature.hh: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2010, 3 | * François Bleibel, 4 | * Olivier Stasse, 5 | * 6 | * CNRS/AIST 7 | * 8 | */ 9 | 10 | #ifndef __SOT_EXCEPTION_FEATURE_H 11 | #define __SOT_EXCEPTION_FEATURE_H 12 | 13 | /* --------------------------------------------------------------------- */ 14 | /* --- INCLUDE --------------------------------------------------------- */ 15 | /* --------------------------------------------------------------------- */ 16 | 17 | #include 18 | 19 | #include "sot/core/api.hh" 20 | /* --------------------------------------------------------------------- */ 21 | /* --- CLASS ----------------------------------------------------------- */ 22 | /* --------------------------------------------------------------------- */ 23 | 24 | namespace dynamicgraph { 25 | namespace sot { 26 | 27 | /* \class ExceptionFeature 28 | */ 29 | class SOT_CORE_EXPORT ExceptionFeature : public ExceptionAbstract 30 | 31 | { 32 | public: 33 | enum ErrorCodeEnum { 34 | GENERIC = ExceptionAbstract::FEATURE, 35 | BAD_INIT, 36 | UNCOMPATIBLE_SIZE 37 | }; 38 | 39 | static const std::string EXCEPTION_NAME; 40 | virtual const std::string &getExceptionName(void) const { 41 | return ExceptionFeature::EXCEPTION_NAME; 42 | } 43 | 44 | ExceptionFeature(const ExceptionFeature::ErrorCodeEnum &errcode, 45 | const std::string &msg = ""); 46 | 47 | ExceptionFeature(const ExceptionFeature::ErrorCodeEnum &errcode, 48 | const std::string &msg, const char *format, ...); 49 | 50 | virtual ~ExceptionFeature(void) throw() {} 51 | }; 52 | 53 | } /* namespace sot */ 54 | } /* namespace dynamicgraph */ 55 | 56 | #endif /* #ifndef __SOT_EXCEPTION_FEATURE_H */ 57 | 58 | /* 59 | * Local variables: 60 | * c-basic-offset: 2 61 | * End: 62 | */ 63 | -------------------------------------------------------------------------------- /include/sot/core/exception-signal.hh: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2010, 3 | * François Bleibel, 4 | * Olivier Stasse, 5 | * 6 | * CNRS/AIST 7 | * 8 | */ 9 | 10 | #ifndef __SOT_SIGNAL_EXCEPTION_H 11 | #define __SOT_SIGNAL_EXCEPTION_H 12 | 13 | /* --------------------------------------------------------------------- */ 14 | /* --- INCLUDE --------------------------------------------------------- */ 15 | /* --------------------------------------------------------------------- */ 16 | 17 | #include 18 | 19 | #include "sot/core/api.hh" 20 | /* --------------------------------------------------------------------- */ 21 | /* --- CLASS ----------------------------------------------------------- */ 22 | /* --------------------------------------------------------------------- */ 23 | 24 | namespace dynamicgraph { 25 | namespace sot { 26 | 27 | /* \class ExceptionSignal 28 | */ 29 | class SOT_CORE_EXPORT ExceptionSignal : public ExceptionAbstract 30 | 31 | { 32 | public: 33 | enum ErrorCodeEnum { 34 | GENERIC = ExceptionAbstract::SIGNAL 35 | 36 | , 37 | READWRITE_LOCK, 38 | COPY_NOT_INITIALIZED, 39 | NOT_INITIALIZED, 40 | PLUG_IMPOSSIBLE, 41 | SET_IMPOSSIBLE, 42 | BAD_CAST 43 | }; 44 | 45 | static const std::string EXCEPTION_NAME; 46 | virtual const std::string &getExceptionName(void) const { 47 | return EXCEPTION_NAME; 48 | } 49 | 50 | public: 51 | ExceptionSignal(const ExceptionSignal::ErrorCodeEnum &errcode, 52 | const std::string &msg = ""); 53 | ExceptionSignal(const ExceptionSignal::ErrorCodeEnum &errcode, 54 | const std::string &msg, const char *format, ...); 55 | virtual ~ExceptionSignal(void) throw() {} 56 | }; 57 | 58 | } /* namespace sot */ 59 | } /* namespace dynamicgraph */ 60 | 61 | #endif /* #ifndef __SOT_SIGNAL_EXCEPTION_H */ 62 | 63 | /* 64 | * Local variables: 65 | * c-basic-offset: 2 66 | * End: 67 | */ 68 | -------------------------------------------------------------------------------- /include/sot/core/exception-task.hh: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2010, 3 | * François Bleibel, 4 | * Olivier Stasse, 5 | * 6 | * CNRS/AIST 7 | * 8 | */ 9 | 10 | #ifndef __SOT_EXCEPTION_TASK_H 11 | #define __SOT_EXCEPTION_TASK_H 12 | 13 | /* --------------------------------------------------------------------- */ 14 | /* --- INCLUDE --------------------------------------------------------- */ 15 | /* --------------------------------------------------------------------- */ 16 | 17 | #include 18 | 19 | #include "sot/core/api.hh" 20 | /* --------------------------------------------------------------------- */ 21 | /* --- CLASS ----------------------------------------------------------- */ 22 | /* --------------------------------------------------------------------- */ 23 | 24 | namespace dynamicgraph { 25 | namespace sot { 26 | 27 | /* \class ExceptionTask 28 | */ 29 | class SOT_CORE_EXPORT ExceptionTask : public ExceptionAbstract 30 | 31 | { 32 | public: 33 | enum ErrorCodeEnum { 34 | GENERIC = ExceptionAbstract::TASK, 35 | EMPTY_LIST, 36 | NON_ADEQUATE_FEATURES, 37 | MATRIX_SIZE, 38 | BOUND_TYPE, 39 | PARSER_MULTI_BOUND 40 | }; 41 | 42 | static const std::string EXCEPTION_NAME; 43 | virtual const std::string &getExceptionName(void) const { 44 | return EXCEPTION_NAME; 45 | } 46 | 47 | ExceptionTask(const ExceptionTask::ErrorCodeEnum &errcode, 48 | const std::string &msg = ""); 49 | ExceptionTask(const ExceptionTask::ErrorCodeEnum &errcode, 50 | const std::string &msg, const char *format, ...); 51 | virtual ~ExceptionTask(void) throw() {} 52 | }; 53 | 54 | } /* namespace sot */ 55 | } /* namespace dynamicgraph */ 56 | 57 | #endif /* #ifndef __SOT_EXCEPTION_TASK_H */ 58 | 59 | /* 60 | * Local variables: 61 | * c-basic-offset: 2 62 | * End: 63 | */ 64 | -------------------------------------------------------------------------------- /include/sot/core/exception-tools.hh: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2010, 3 | * François Bleibel, 4 | * Olivier Stasse, 5 | * 6 | * CNRS/AIST 7 | * 8 | */ 9 | 10 | #ifndef __SOT_TOOLS_EXCEPTION_H 11 | #define __SOT_TOOLS_EXCEPTION_H 12 | 13 | /* --------------------------------------------------------------------- */ 14 | /* --- INCLUDE --------------------------------------------------------- */ 15 | /* --------------------------------------------------------------------- */ 16 | 17 | #include 18 | 19 | #include "sot/core/api.hh" 20 | /* --------------------------------------------------------------------- */ 21 | /* --- CLASS ----------------------------------------------------------- */ 22 | /* --------------------------------------------------------------------- */ 23 | 24 | namespace dynamicgraph { 25 | namespace sot { 26 | 27 | /* \class ExceptionTools 28 | */ 29 | class SOT_CORE_EXPORT ExceptionTools : public ExceptionAbstract 30 | 31 | { 32 | public: 33 | enum ErrorCodeEnum { 34 | GENERIC = ExceptionAbstract::TOOLS 35 | 36 | , 37 | CORBA, 38 | KALMAN_SIZE, 39 | PARAMETER_SERVER 40 | }; 41 | 42 | static const std::string EXCEPTION_NAME; 43 | virtual const std::string &getExceptionName() const { return EXCEPTION_NAME; } 44 | 45 | public: 46 | ExceptionTools(const ExceptionTools::ErrorCodeEnum &errcode, 47 | const std::string &msg = ""); 48 | ExceptionTools(const ExceptionTools::ErrorCodeEnum &errcode, 49 | const std::string &msg, const char *format, ...); 50 | virtual ~ExceptionTools(void) throw() {} 51 | }; 52 | 53 | } // namespace sot 54 | } // namespace dynamicgraph 55 | 56 | #endif /* #ifndef __SOT_TOOLS_EXCEPTION_H */ 57 | 58 | /* 59 | * Local variables: 60 | * c-basic-offset: 2 61 | * End: 62 | */ 63 | -------------------------------------------------------------------------------- /include/sot/core/exp-moving-avg.hh: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2018, 3 | * Julian Viereck 4 | * 5 | * CNRS/AIST 6 | * 7 | */ 8 | 9 | #ifndef __SOT_EXPMOVINGAVG_H__ 10 | #define __SOT_EXPMOVINGAVG_H__ 11 | 12 | /* --------------------------------------------------------------------- */ 13 | /* --- INCLUDE --------------------------------------------------------- */ 14 | /* --------------------------------------------------------------------- */ 15 | 16 | #include 17 | #include 18 | #include 19 | 20 | #include 21 | 22 | namespace dynamicgraph { 23 | namespace sot { 24 | 25 | /* --------------------------------------------------------------------- */ 26 | /* --- TRACER ---------------------------------------------------------- */ 27 | /* --------------------------------------------------------------------- */ 28 | 29 | using dynamicgraph::Entity; 30 | using dynamicgraph::SignalPtr; 31 | using dynamicgraph::SignalTimeDependent; 32 | 33 | class SOT_CORE_DLLAPI ExpMovingAvg : public Entity { 34 | DYNAMIC_GRAPH_ENTITY_DECL(); 35 | 36 | public: 37 | SignalPtr updateSIN; 38 | SignalTimeDependent refresherSINTERN; 39 | SignalTimeDependent averageSOUT; 40 | 41 | public: 42 | ExpMovingAvg(const std::string &n); 43 | virtual ~ExpMovingAvg(void); 44 | 45 | void setAlpha(const double &alpha_); 46 | 47 | protected: 48 | dynamicgraph::Vector &update(dynamicgraph::Vector &res, const int &inTime); 49 | 50 | dynamicgraph::Vector average; 51 | 52 | double alpha; 53 | bool init; 54 | }; 55 | 56 | } /* namespace sot */ 57 | } /* namespace dynamicgraph */ 58 | 59 | #endif /* #ifndef __SOT_TRACER_H__ */ 60 | -------------------------------------------------------------------------------- /include/sot/core/factory.hh: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2010, 3 | * François Bleibel, 4 | * Olivier Stasse, 5 | * Florent Lamiraux 6 | * 7 | * CNRS/AIST 8 | * 9 | */ 10 | 11 | #ifndef __SOT_FACTORY_HH__ 12 | #define __SOT_FACTORY_HH__ 13 | 14 | /* --------------------------------------------------------------------- */ 15 | /* --- INCLUDE --------------------------------------------------------- */ 16 | /* --------------------------------------------------------------------- */ 17 | 18 | #include 19 | 20 | #define SOT_FACTORY_FEATURE_PLUGIN(sotFeatureType, className) \ 21 | DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(sotFeatureType, className) 22 | /* -------------------------------------------------------------------------- */ 23 | /* --- TASK REGISTERER ------------------------------------------------------ */ 24 | /* -------------------------------------------------------------------------- */ 25 | 26 | #define SOT_FACTORY_TASK_PLUGIN(sotTaskType, className) \ 27 | DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(sotTaskType, className) 28 | 29 | #endif /* #ifndef __SOT_FACTORY_HH__ */ 30 | -------------------------------------------------------------------------------- /include/sot/core/feature-task.hh: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2010, 3 | * François Bleibel, 4 | * Olivier Stasse, 5 | * 6 | * CNRS/AIST 7 | * 8 | */ 9 | 10 | #ifndef __SOT_FEATURE_TASK_HH__ 11 | #define __SOT_FEATURE_TASK_HH__ 12 | 13 | /* --------------------------------------------------------------------- */ 14 | /* --- INCLUDE --------------------------------------------------------- */ 15 | /* --------------------------------------------------------------------- */ 16 | 17 | /* SOT */ 18 | #include 19 | #include 20 | #include 21 | 22 | /* --------------------------------------------------------------------- */ 23 | /* --- API ------------------------------------------------------------- */ 24 | /* --------------------------------------------------------------------- */ 25 | 26 | #if defined(WIN32) 27 | #if defined(feature_task_EXPORTS) 28 | #define SOTFEATURETASK_EXPORT __declspec(dllexport) 29 | #else 30 | #define SOTFEATURETASK_EXPORT __declspec(dllimport) 31 | #endif 32 | #else 33 | #define SOTFEATURETASK_EXPORT 34 | #endif 35 | 36 | /* --------------------------------------------------------------------- */ 37 | /* --- CLASS ----------------------------------------------------------- */ 38 | /* --------------------------------------------------------------------- */ 39 | 40 | namespace dynamicgraph { 41 | namespace sot { 42 | 43 | class SOTFEATURETASK_EXPORT FeatureTask : public FeatureGeneric { 44 | public: 45 | /*! Field storing the class name. */ 46 | static const std::string CLASS_NAME; 47 | /*! Returns the name of the class. */ 48 | virtual const std::string &getClassName(void) const { return CLASS_NAME; } 49 | 50 | protected: 51 | TaskAbstract *taskPtr; 52 | 53 | /* --- SIGNALS ------------------------------------------------------------ */ 54 | public: 55 | public: 56 | /*! \brief Default constructor */ 57 | FeatureTask(const std::string &name); 58 | 59 | /*! \brief Default destructor */ 60 | virtual ~FeatureTask(void) {} 61 | 62 | /*! \brief Display the information related to this task implementation. */ 63 | virtual void display(std::ostream &os) const; 64 | }; 65 | 66 | } /* namespace sot */ 67 | } /* namespace dynamicgraph */ 68 | 69 | #endif // #ifndef __SOT_FEATURE_TASK_HH__ 70 | 71 | /* 72 | * Local variables: 73 | * c-basic-offset: 2 74 | * End: 75 | */ 76 | -------------------------------------------------------------------------------- /include/sot/core/fir-filter-impl.hh: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2010, 3 | * François Bleibel, 4 | * Olivier Stasse, 5 | * 6 | * CNRS/AIST 7 | * 8 | */ 9 | 10 | #ifndef __SOT_FIRFILTER_IMPL_HH__ 11 | #define __SOT_FIRFILTER_IMPL_HH__ 12 | 13 | #include 14 | 15 | /* --------------------------------------------------------------------- */ 16 | /* --- API ------------------------------------------------------------- */ 17 | /* --------------------------------------------------------------------- */ 18 | 19 | #if defined(WIN32) 20 | #if defined(fir_filter_EXPORTS) 21 | #define FIL_FILTER_EXPORT __declspec(dllexport) 22 | #else 23 | #define FIL_FILTER_EXPORT __declspec(dllimport) 24 | #endif 25 | #else 26 | #define FIL_FILTER_EXPORT 27 | #endif 28 | 29 | #ifdef WIN32 30 | #define DECLARE_SPECIFICATION(className, sotSigType, sotCoefType) \ 31 | class FIL_FILTER_EXPORT className \ 32 | : public FIRFilter { \ 33 | public: \ 34 | className(const std::string &name); \ 35 | }; 36 | #else 37 | #define DECLARE_SPECIFICATION(className, sotSigType, sotCoefType) \ 38 | typedef FIRFilter className; 39 | #endif // WIN32 40 | 41 | namespace dynamicgraph { 42 | namespace sot { 43 | 44 | DECLARE_SPECIFICATION(FIRFilterDoubleDouble, double, double) 45 | DECLARE_SPECIFICATION(FIRFilterVectorDouble, Vector, double) 46 | DECLARE_SPECIFICATION(FIRFilterVectorMatrix, Vector, Matrix) 47 | 48 | } // namespace sot 49 | } // namespace dynamicgraph 50 | #endif 51 | -------------------------------------------------------------------------------- /include/sot/core/flags.hh: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2010, 3 | * François Bleibel, 4 | * Olivier Stasse, 5 | * 6 | * CNRS/AIST 7 | * 8 | */ 9 | 10 | #ifndef __SOT_FLAGS_H 11 | #define __SOT_FLAGS_H 12 | 13 | /* --------------------------------------------------------------------- */ 14 | /* --- INCLUDE --------------------------------------------------------- */ 15 | /* --------------------------------------------------------------------- */ 16 | 17 | /* STD */ 18 | #include 19 | #include 20 | 21 | /* SOT */ 22 | #include 23 | 24 | #include "sot/core/api.hh" 25 | 26 | /* --------------------------------------------------------------------- */ 27 | /* --- CLASS ----------------------------------------------------------- */ 28 | /* --------------------------------------------------------------------- */ 29 | 30 | namespace dynamicgraph { 31 | namespace sot { 32 | 33 | class SOT_CORE_EXPORT Flags { 34 | protected: 35 | std::vector flags; 36 | bool outOfRangeFlag; 37 | 38 | public: 39 | Flags(const bool &b = false); 40 | Flags(const char *flags); 41 | Flags(const std::vector &flags); 42 | 43 | void add(const bool &b); 44 | 45 | Flags operator!(void) const; 46 | SOT_CORE_EXPORT friend Flags operator&(const Flags &f1, const Flags &f2); 47 | SOT_CORE_EXPORT friend Flags operator|(const Flags &f1, const Flags &f2); 48 | Flags &operator&=(const Flags &f2); 49 | Flags &operator|=(const Flags &f2); 50 | 51 | SOT_CORE_EXPORT friend std::ostream &operator<<(std::ostream &os, 52 | const Flags &fl); 53 | SOT_CORE_EXPORT friend std::istream &operator>>(std::istream &is, Flags &fl); 54 | bool operator()(const int &i) const; 55 | 56 | operator bool(void) const; 57 | 58 | void unset(const unsigned int &i); 59 | void set(const unsigned int &i); 60 | }; 61 | 62 | } // namespace sot 63 | 64 | template <> 65 | struct signal_io : signal_io_unimplemented {}; 66 | } // namespace dynamicgraph 67 | 68 | #endif /* #ifndef __SOT_FLAGS_H */ 69 | -------------------------------------------------------------------------------- /include/sot/core/fwd.hh: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright CNRS 2021 3 | * 4 | * Author: Florent Lamiraux 5 | * 6 | * This file is part of sot-core. 7 | */ 8 | 9 | #ifndef SOT_CORE_FWD_HH 10 | #define SOT_CORE_FWD_HH 11 | 12 | #include 13 | 14 | namespace dynamicgraph { 15 | namespace sot { 16 | 17 | class Device; 18 | class AbstractSotExternalInterface; 19 | 20 | } // namespace sot 21 | } // namespace dynamicgraph 22 | 23 | #endif // SOT_CORE_FWD_HH 24 | -------------------------------------------------------------------------------- /include/sot/core/gradient-ascent.hh: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2018, 3 | * Julian Viereck 4 | * 5 | * CNRS/AIST 6 | * 7 | */ 8 | 9 | #ifndef __SOT_GRADIENTASCENT_H__ 10 | #define __SOT_GRADIENTASCENT_H__ 11 | 12 | /* --------------------------------------------------------------------- */ 13 | /* --- INCLUDE --------------------------------------------------------- */ 14 | /* --------------------------------------------------------------------- */ 15 | 16 | #include 17 | #include 18 | #include 19 | 20 | #include 21 | 22 | namespace dynamicgraph { 23 | namespace sot { 24 | 25 | /* --------------------------------------------------------------------- */ 26 | /* --- TRACER ---------------------------------------------------------- */ 27 | /* --------------------------------------------------------------------- */ 28 | 29 | using dynamicgraph::Entity; 30 | using dynamicgraph::SignalPtr; 31 | using dynamicgraph::SignalTimeDependent; 32 | 33 | class SOT_CORE_DLLAPI GradientAscent : public Entity { 34 | DYNAMIC_GRAPH_ENTITY_DECL(); 35 | 36 | public: 37 | SignalPtr gradientSIN; 38 | SignalPtr learningRateSIN; 39 | SignalTimeDependent refresherSINTERN; 40 | SignalTimeDependent valueSOUT; 41 | 42 | public: 43 | GradientAscent(const std::string &n); 44 | virtual ~GradientAscent(void); 45 | 46 | protected: 47 | dynamicgraph::Vector &update(dynamicgraph::Vector &res, const int &inTime); 48 | 49 | dynamicgraph::Vector value; 50 | 51 | double alpha; 52 | bool init; 53 | }; 54 | 55 | } /* namespace sot */ 56 | } /* namespace dynamicgraph */ 57 | 58 | #endif /* #ifndef __SOT_TRACER_H__ */ 59 | -------------------------------------------------------------------------------- /include/sot/core/integrator-abstract-impl.hh: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2010, 3 | * François Bleibel, 4 | * Olivier Stasse, 5 | * 6 | * CNRS/AIST 7 | * 8 | */ 9 | 10 | #ifndef __SOT_INTEGRATOR_ABSTRACT_VECTOR_H__ 11 | #define __SOT_INTEGRATOR_ABSTRACT_VECTOR_H__ 12 | 13 | /* --- SOT PLUGIN --- */ 14 | #include 15 | 16 | /* --------------------------------------------------------------------- */ 17 | /* --- API ------------------------------------------------------------- */ 18 | /* --------------------------------------------------------------------- */ 19 | 20 | #if defined(WIN32) 21 | #if defined(integrator_abstract_EXPORTS) 22 | #define INTEGRATOR_ABSTRACT_EXPORT __declspec(dllexport) 23 | #else 24 | #define INTEGRATOR_ABSTRACT_EXPORT __declspec(dllimport) 25 | #endif 26 | #else 27 | #define INTEGRATOR_ABSTRACT_EXPORT 28 | #endif 29 | 30 | /* --------------------------------------------------------------------- */ 31 | /* --- CLASS ----------------------------------------------------------- */ 32 | /* --------------------------------------------------------------------- */ 33 | 34 | #ifdef WIN32 35 | #define DECLARE_SPECIFICATION(className, sotSigType, sotCoefType) \ 36 | class INTEGRATOR_ABSTRACT_EXPORT className \ 37 | : public IntegratorAbstract { \ 38 | public: \ 39 | className(const std::string &name); \ 40 | }; 41 | #else 42 | #define DECLARE_SPECIFICATION(className, sotSigType, sotCoefType) \ 43 | typedef IntegratorAbstract className; 44 | #endif 45 | 46 | namespace dynamicgraph { 47 | namespace sot { 48 | DECLARE_SPECIFICATION(IntegratorAbstractDouble, double, double) 49 | DECLARE_SPECIFICATION(IntegratorAbstractVector, dynamicgraph::Vector, 50 | dynamicgraph::Matrix) 51 | } // namespace sot 52 | } // namespace dynamicgraph 53 | #endif // #ifndef __SOT_MAILBOX_HH 54 | -------------------------------------------------------------------------------- /include/sot/core/integrator-euler-impl.hh: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2010, 3 | * François Bleibel, 4 | * Olivier Stasse, 5 | * 6 | * CNRS/AIST 7 | * 8 | */ 9 | 10 | #ifndef __SOT_INTEGRATOR_EULER_IMPL_H__ 11 | #define __SOT_INTEGRATOR_EULER_IMPL_H__ 12 | 13 | /* --------------------------------------------------------------------- */ 14 | /* --- INCLUDE --------------------------------------------------------- */ 15 | /* --------------------------------------------------------------------- */ 16 | 17 | /* SOT */ 18 | #include 19 | 20 | /* --------------------------------------------------------------------- */ 21 | /* --- API ------------------------------------------------------------- */ 22 | /* --------------------------------------------------------------------- */ 23 | 24 | #if defined(WIN32) 25 | #if defined(integrator_euler_EXPORTS) 26 | #define INTEGRATOR_EULER_EXPORT __declspec(dllexport) 27 | #else 28 | #define INTEGRATOR_EULER_EXPORT __declspec(dllimport) 29 | #endif 30 | #else 31 | #define INTEGRATOR_EULER_EXPORT 32 | #endif 33 | 34 | #ifdef WIN32 35 | #define DECLARE_SPECIFICATION(className, sotSigType, sotCoefType) \ 36 | class INTEGRATOR_EULER_EXPORT className \ 37 | : public IntegratorEuler { \ 38 | public: \ 39 | std::string getTypeName(void); \ 40 | className(const std::string &name); \ 41 | }; 42 | #else 43 | #define DECLARE_SPECIFICATION(className, sotSigType, sotCoefType) \ 44 | typedef IntegratorEuler className; 45 | #endif // WIN32 46 | 47 | namespace dynamicgraph { 48 | namespace sot { 49 | DECLARE_SPECIFICATION(IntegratorEulerDoubleDouble, double, double) 50 | DECLARE_SPECIFICATION(IntegratorEulerVectorDouble, Vector, double) 51 | DECLARE_SPECIFICATION(IntegratorEulerVectorMatrix, Vector, Matrix) 52 | } // namespace sot 53 | } // namespace dynamicgraph 54 | 55 | #endif 56 | -------------------------------------------------------------------------------- /include/sot/core/joint-limitator.hh: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2010, 3 | * François Bleibel, 4 | * Olivier Stasse, 5 | * 6 | * CNRS/AIST 7 | * 8 | */ 9 | 10 | #ifndef SOT_FEATURE_JOINTLIMITS_HH 11 | #define SOT_FEATURE_JOINTLIMITS_HH 12 | // Matrix 13 | #include 14 | 15 | // SOT 16 | #include 17 | #include 18 | 19 | #include 20 | 21 | #if defined(WIN32) 22 | #if defined(joint_limitator_EXPORTS) 23 | #define SOTJOINTLIMITATOR_EXPORT __declspec(dllexport) 24 | #else 25 | #define SOTJOINTLIMITATOR_EXPORT __declspec(dllimport) 26 | #endif 27 | #else 28 | #define SOTJOINTLIMITATOR_EXPORT 29 | #endif 30 | 31 | namespace dynamicgraph { 32 | namespace sot { 33 | 34 | /// \brief Filter control vector to avoid exceeding joint maximum values. 35 | /// 36 | /// This must be plugged between the entity producing the command 37 | /// (i.e. usually the sot) and the entity executing it (the device). 38 | class SOTJOINTLIMITATOR_EXPORT JointLimitator : public dynamicgraph::Entity { 39 | DYNAMIC_GRAPH_ENTITY_DECL(); 40 | 41 | public: 42 | JointLimitator(const std::string &name); 43 | virtual ~JointLimitator() {} 44 | 45 | virtual dynamicgraph::Vector &computeControl(dynamicgraph::Vector &res, 46 | int time); 47 | dynamicgraph::Vector &computeWidthJl(dynamicgraph::Vector &res, 48 | const int &time); 49 | 50 | virtual void display(std::ostream &os) const; 51 | 52 | /// \name Signals 53 | /// \{ 54 | dynamicgraph::SignalPtr jointSIN; 55 | dynamicgraph::SignalPtr upperJlSIN; 56 | dynamicgraph::SignalPtr lowerJlSIN; 57 | dynamicgraph::SignalPtr controlSIN; 58 | dynamicgraph::SignalTimeDependent controlSOUT; 59 | dynamicgraph::SignalTimeDependent widthJlSINTERN; 60 | /// \} 61 | }; 62 | } // end of namespace sot. 63 | } // namespace dynamicgraph 64 | 65 | #endif //! SOT_FEATURE_JOINTLIMITS_HH 66 | -------------------------------------------------------------------------------- /include/sot/core/macros.hh: -------------------------------------------------------------------------------- 1 | #ifndef __SOT_CORE_MACROS_HH__ 2 | #define __SOT_CORE_MACROS_HH__ 3 | 4 | // ref https://www.fluentcpp.com/2019/08/30/how-to-disable-a-warning-in-cpp/ 5 | #if defined(_MSC_VER) 6 | 7 | #define SOT_CORE_DISABLE_WARNING_PUSH __pragma(warning(push)) 8 | #define SOT_CORE_DISABLE_WARNING_POP __pragma(warning(pop)) 9 | #define SOT_CORE_DISABLE_WARNING(warningNumber) \ 10 | __pragma(warning(disable : warningNumber)) 11 | #define SOT_CORE_DISABLE_WARNING_DEPRECATED SOT_CORE_DISABLE_WARNING(4996) 12 | #define SOT_CORE_DISABLE_WARNING_FALLTHROUGH 13 | 14 | #elif defined(__GNUC__) || defined(__clang__) 15 | 16 | #define SOT_CORE_DO_PRAGMA(X) _Pragma(#X) 17 | #define SOT_CORE_DISABLE_WARNING_PUSH SOT_CORE_DO_PRAGMA(GCC diagnostic push) 18 | #define SOT_CORE_DISABLE_WARNING_POP SOT_CORE_DO_PRAGMA(GCC diagnostic pop) 19 | #define SOT_CORE_DISABLE_WARNING(warningName) \ 20 | SOT_CORE_DO_PRAGMA(GCC diagnostic ignored #warningName) 21 | #define SOT_CORE_DISABLE_WARNING_DEPRECATED \ 22 | SOT_CORE_DISABLE_WARNING(-Wdeprecated - declarations) 23 | #define SOT_CORE_DISABLE_WARNING_FALLTHROUGH \ 24 | SOT_CORE_DISABLE_WARNING(-Wimplicit - fallthrough) 25 | 26 | #else 27 | 28 | #define SOT_CORE_DISABLE_WARNING_PUSH 29 | #define SOT_CORE_DISABLE_WARNING_POP 30 | #define SOT_CORE_DISABLE_WARNING_DEPRECATED 31 | 32 | #endif 33 | 34 | #endif 35 | -------------------------------------------------------------------------------- /include/sot/core/mailbox-vector.hh: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2010, 3 | * François Bleibel, 4 | * Olivier Stasse, 5 | * 6 | * CNRS/AIST 7 | * 8 | */ 9 | 10 | #ifndef __SOT_MAILBOX_VECTOR_HH 11 | #define __SOT_MAILBOX_VECTOR_HH 12 | 13 | /* --- SOT PLUGIN --- */ 14 | #include 15 | 16 | #include 17 | 18 | /* --------------------------------------------------------------------- */ 19 | /* --- API ------------------------------------------------------------- */ 20 | /* --------------------------------------------------------------------- */ 21 | 22 | #if defined(WIN32) 23 | #if defined(mailbox_vector_EXPORTS) 24 | #define MAILBOX_VECTOR_EXPORT __declspec(dllexport) 25 | #else 26 | #define MAILBOX_VECTOR_EXPORT __declspec(dllimport) 27 | #endif 28 | #else 29 | #define MAILBOX_VECTOR_EXPORT 30 | #endif 31 | 32 | /* --------------------------------------------------------------------- */ 33 | /* --- CLASS ----------------------------------------------------------- */ 34 | /* --------------------------------------------------------------------- */ 35 | 36 | namespace dynamicgraph { 37 | namespace sot { 38 | #ifdef WIN32 39 | class MAILBOX_VECTOR_EXPORT MailboxVector 40 | : public Mailbox { 41 | public: 42 | MailboxVector(const std::string &name); 43 | }; 44 | #else 45 | typedef Mailbox MailboxVector; 46 | #endif 47 | } // namespace sot 48 | } // namespace dynamicgraph 49 | 50 | #endif // #ifndef __SOT_MAILBOX_HH 51 | -------------------------------------------------------------------------------- /include/sot/core/mailbox.hh: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2010, 3 | * François Bleibel, 4 | * Olivier Stasse, 5 | * 6 | * CNRS/AIST 7 | * 8 | */ 9 | 10 | #ifndef __SOT_MAILBOX_HH 11 | #define __SOT_MAILBOX_HH 12 | 13 | /* --- SOT PLUGIN --- */ 14 | #include 15 | #include 16 | 17 | /* --- BOOST --- */ 18 | #include 19 | #include 20 | #include 21 | 22 | /* --- STD --- */ 23 | #include 24 | #ifndef WIN32 25 | #include 26 | #else 27 | #include 28 | #endif /*WIN32*/ 29 | #include 30 | 31 | namespace dynamicgraph { 32 | namespace sot { 33 | 34 | namespace dg = dynamicgraph; 35 | 36 | template 37 | struct MailboxTimestampedObject { 38 | Object obj; 39 | struct timeval timestamp; 40 | }; 41 | 42 | template 43 | class Mailbox : public dg::Entity { 44 | public: 45 | static const std::string CLASS_NAME; 46 | virtual const std::string &getClassName(void) const { return CLASS_NAME; } 47 | 48 | public: 49 | typedef MailboxTimestampedObject sotTimestampedObject; 50 | 51 | public: 52 | Mailbox(const std::string &name); 53 | ~Mailbox(void); 54 | 55 | void post(const Object &obj); 56 | sotTimestampedObject &get(sotTimestampedObject &res, const int &dummy); 57 | 58 | Object &getObject(Object &res, const int &time); 59 | struct timeval &getTimestamp(struct timeval &res, const int &time); 60 | 61 | bool hasBeenUpdated(void); 62 | 63 | protected: 64 | boost::timed_mutex mainObjectMutex; 65 | Object mainObject; 66 | struct timeval mainTimeStamp; 67 | bool update; 68 | 69 | public: /* --- SIGNALS --- */ 70 | dynamicgraph::SignalTimeDependent SOUT; 71 | dynamicgraph::SignalTimeDependent objSOUT; 72 | dynamicgraph::SignalTimeDependent timeSOUT; 73 | }; 74 | 75 | } /* namespace sot */ 76 | 77 | template 78 | struct signal_io > 79 | : signal_io_unimplemented > {}; 80 | 81 | template <> 82 | struct signal_io : signal_io_unimplemented {}; 83 | } /* namespace dynamicgraph */ 84 | 85 | #endif // #ifndef __SOT_MAILBOX_HH 86 | -------------------------------------------------------------------------------- /include/sot/core/matrix-constant.hh: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2010, 3 | * François Bleibel, 4 | * Olivier Stasse, 5 | * 6 | * CNRS/AIST 7 | * 8 | */ 9 | 10 | #include 11 | #include 12 | 13 | /* Matrix */ 14 | #include 15 | 16 | /* --------------------------------------------------------------------- */ 17 | /* --- MATRIX ---------------------------------------------------------- */ 18 | /* --------------------------------------------------------------------- */ 19 | 20 | namespace dynamicgraph { 21 | namespace sot { 22 | namespace command { 23 | namespace matrixConstant { 24 | class Resize; 25 | } 26 | } // namespace command 27 | 28 | class MatrixConstant : public Entity { 29 | friend class command::matrixConstant::Resize; 30 | 31 | public: 32 | static const std::string CLASS_NAME; 33 | virtual const std::string &getClassName(void) const { return CLASS_NAME; } 34 | 35 | int rows, cols; 36 | double color; 37 | 38 | void setValue(const dynamicgraph::Matrix &inValue); 39 | 40 | public: 41 | MatrixConstant(const std::string &name); 42 | 43 | virtual ~MatrixConstant(void) {} 44 | 45 | SignalTimeDependent SOUT; 46 | }; 47 | 48 | } // namespace sot 49 | } // namespace dynamicgraph 50 | -------------------------------------------------------------------------------- /include/sot/core/matrix-svd.hh: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2010, 3 | * François Bleibel, 4 | * Olivier Stasse, 5 | * 6 | * CNRS/AIST 7 | * 8 | */ 9 | 10 | #ifndef __SOT_MATRIX_SVD_H__ 11 | #define __SOT_MATRIX_SVD_H__ 12 | 13 | /* --- Matrix --- */ 14 | #include 15 | 16 | #include 17 | 18 | /* --------------------------------------------------------------------- */ 19 | /* --------------------------------------------------------------------- */ 20 | /* --------------------------------------------------------------------- */ 21 | namespace dynamicgraph { 22 | 23 | typedef Eigen::JacobiSVD SVD_t; 24 | 25 | void pseudoInverse(Matrix &_inputMatrix, Matrix &_inverseMatrix, 26 | const double threshold = 1e-6); 27 | 28 | void dampedInverse(const SVD_t &svd, Matrix &_inverseMatrix, 29 | const double threshold = 1e-6); 30 | 31 | void dampedInverse(const Matrix &_inputMatrix, Matrix &_inverseMatrix, 32 | Matrix &Uref, Vector &Sref, Matrix &Vref, 33 | const double threshold = 1e-6); 34 | 35 | void dampedInverse(const Matrix &_inputMatrix, Matrix &_inverseMatrix, 36 | const double threshold = 1e-6); 37 | 38 | } // namespace dynamicgraph 39 | 40 | #endif /* #ifndef __SOT_MATRIX_SVD_H__ */ 41 | -------------------------------------------------------------------------------- /include/sot/core/memory-task-sot.hh: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2010, 3 | * François Bleibel, 4 | * Olivier Stasse, 5 | * 6 | * CNRS/AIST 7 | * 8 | */ 9 | 10 | #ifndef __SOT_MEMORY_TASK_HH 11 | #define __SOT_MEMORY_TASK_HH 12 | 13 | #include 14 | #include 15 | 16 | #include "sot/core/api.hh" 17 | 18 | /* --------------------------------------------------------------------- */ 19 | /* --- CLASS ----------------------------------------------------------- */ 20 | /* --------------------------------------------------------------------- */ 21 | 22 | namespace dynamicgraph { 23 | namespace sot { 24 | 25 | class SOT_CORE_EXPORT MemoryTaskSOT : public TaskAbstract::MemoryTaskAbstract { 26 | public: // protected: 27 | typedef Eigen::Map::Alignment> 28 | Kernel_t; 29 | typedef Eigen::Map::Alignment> 30 | KernelConst_t; 31 | 32 | /* Internal memory to reduce the dynamic allocation at task resolution. */ 33 | dynamicgraph::Vector err, tmpTask, tmpVar, tmpControl; 34 | dynamicgraph::Matrix Jt; //( nJ,mJ ); 35 | 36 | dynamicgraph::Matrix JK; //(nJ,mJ); 37 | 38 | SVD_t svd; 39 | Kernel_t kernel; 40 | 41 | void resizeKernel(const Matrix::Index r, const Matrix::Index c) { 42 | if (kernel.rows() != r || kernel.cols() != c) { 43 | if (kernelMem.size() < r * c) kernelMem.resize(r, c); 44 | new (&kernel) Kernel_t(kernelMem.data(), r, c); 45 | } 46 | } 47 | 48 | Kernel_t &getKernel(const Matrix::Index r, const Matrix::Index c) { 49 | resizeKernel(r, c); 50 | return kernel; 51 | } 52 | 53 | public: 54 | /** 55 | * \param mJ is the number of joints 56 | * \param nJ the number of feature in the task 57 | **/ 58 | MemoryTaskSOT(const Matrix::Index nJ = 0, const Matrix::Index mJ = 0); 59 | 60 | void display(std::ostream &os) const; 61 | 62 | private: 63 | void initMemory(const Matrix::Index nJ, const Matrix::Index mJ); 64 | 65 | Matrix kernelMem; 66 | }; 67 | 68 | } /* namespace sot */ 69 | } /* namespace dynamicgraph */ 70 | 71 | #endif // __SOT_MEMORY_TASK_HH 72 | -------------------------------------------------------------------------------- /include/sot/core/periodic-call.hh: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2010, 3 | * François Bleibel, 4 | * Olivier Stasse, 5 | * 6 | * CNRS/AIST 7 | * 8 | */ 9 | 10 | #ifndef __SOT_PERIODICCALL_HH__ 11 | #define __SOT_PERIODICCALL_HH__ 12 | 13 | /* --------------------------------------------------------------------- */ 14 | /* --- INCLUDE --------------------------------------------------------- */ 15 | /* --------------------------------------------------------------------- */ 16 | 17 | /* SOT */ 18 | #include 19 | #include 20 | 21 | #include 22 | /* STD */ 23 | #include 24 | #include 25 | #include 26 | 27 | /* --------------------------------------------------------------------- */ 28 | /* --- CLASS ----------------------------------------------------------- */ 29 | /* --------------------------------------------------------------------- */ 30 | 31 | namespace dynamicgraph { 32 | namespace sot { 33 | 34 | /*! 35 | \class PeriodicCall 36 | */ 37 | class SOT_CORE_EXPORT PeriodicCall { 38 | protected: 39 | struct SignalToCall { 40 | dynamicgraph::SignalBase *signal; 41 | unsigned int downsamplingFactor; 42 | 43 | SignalToCall() { 44 | signal = NULL; 45 | downsamplingFactor = 1; 46 | } 47 | 48 | SignalToCall(dynamicgraph::SignalBase *s, unsigned int df = 1) { 49 | signal = s; 50 | downsamplingFactor = df; 51 | } 52 | }; 53 | 54 | typedef std::map SignalMapType; 55 | SignalMapType signalMap; 56 | 57 | int innerTime; 58 | 59 | /* --- FUNCTIONS ------------------------------------------------------------ 60 | */ 61 | public: 62 | PeriodicCall(void); 63 | virtual ~PeriodicCall(void) {} 64 | 65 | void addDownsampledSignal(const std::string &name, 66 | dynamicgraph::SignalBase &sig, 67 | const unsigned int &downsamplingFactor); 68 | void addDownsampledSignal(const std::string &sigpath, 69 | const unsigned int &downsamplingFactor); 70 | 71 | void addSignal(const std::string &name, dynamicgraph::SignalBase &sig); 72 | void addSignal(const std::string &args); 73 | void rmSignal(const std::string &name); 74 | 75 | void runSignals(const int &t); 76 | void run(const int &t); 77 | 78 | void clear(void) { signalMap.clear(); } 79 | 80 | void display(std::ostream &os) const; 81 | }; 82 | 83 | } // namespace sot 84 | } // namespace dynamicgraph 85 | 86 | #endif // #ifndef __SOT_PERIODICCALL_HH__ 87 | 88 | /* 89 | * Local variables: 90 | * c-basic-offset: 2 91 | * End: 92 | */ 93 | -------------------------------------------------------------------------------- /include/sot/core/robot-simu.hh: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2010, 3 | * Nicolas Mansard, Olivier Stasse, François Bleibel, Florent Lamiraux 4 | * 5 | * CNRS 6 | * 7 | */ 8 | 9 | #ifndef DYNAMICGRAPH_SOT_ROBOT_SIMU_HH 10 | #define DYNAMICGRAPH_SOT_ROBOT_SIMU_HH 11 | 12 | /* --------------------------------------------------------------------- */ 13 | /* --- INCLUDE --------------------------------------------------------- */ 14 | /* --------------------------------------------------------------------- */ 15 | 16 | #include 17 | 18 | /* -- MaaL --- */ 19 | #include 20 | 21 | /* SOT */ 22 | #include 23 | #include 24 | 25 | #include "sot/core/api.hh" 26 | #include "sot/core/device.hh" 27 | 28 | /* --------------------------------------------------------------------- */ 29 | /* --- API ------------------------------------------------------------- */ 30 | /* --------------------------------------------------------------------- */ 31 | 32 | #if defined(WIN32) 33 | #if defined(robot_simu_EXPORTS) 34 | #define SOT_ROBOT_SIMU_EXPORT __declspec(dllexport) 35 | #else 36 | #define SOT_ROBOT_SIMU_EXPORT __declspec(dllimport) 37 | #endif 38 | #else 39 | #define SOT_ROBOT_SIMU_EXPORT 40 | #endif 41 | 42 | namespace dynamicgraph { 43 | namespace sot { 44 | 45 | /* --------------------------------------------------------------------- */ 46 | /* --- CLASS ----------------------------------------------------------- */ 47 | /* --------------------------------------------------------------------- */ 48 | 49 | class SOT_ROBOT_SIMU_EXPORT RobotSimu : public Device { 50 | public: 51 | RobotSimu(const std::string &inName); 52 | static const std::string CLASS_NAME; 53 | virtual const std::string &getClassName(void) const { return CLASS_NAME; } 54 | }; 55 | } // namespace sot 56 | } // namespace dynamicgraph 57 | 58 | #endif /* #ifndef DYNAMICGRAPH_SOT_ROBOT_SIMU_HH */ 59 | -------------------------------------------------------------------------------- /include/sot/core/task-conti.hh: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2010, 3 | * François Bleibel, 4 | * Olivier Stasse, 5 | * 6 | * CNRS/AIST 7 | * 8 | */ 9 | 10 | #ifndef __SOT_TASKCONTI_H__ 11 | #define __SOT_TASKCONTI_H__ 12 | 13 | /* --------------------------------------------------------------------- */ 14 | /* --- INCLUDE --------------------------------------------------------- */ 15 | /* --------------------------------------------------------------------- */ 16 | 17 | /* Matrix */ 18 | #include 19 | 20 | /* STD */ 21 | #include 22 | 23 | /* SOT */ 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | /* --------------------------------------------------------------------- */ 30 | /* --- API ------------------------------------------------------------- */ 31 | /* --------------------------------------------------------------------- */ 32 | 33 | #if defined(WIN32) 34 | #if defined(task_conti_EXPORTS) 35 | #define SOTTASKCONTI_EXPORT __declspec(dllexport) 36 | #else 37 | #define SOTTASKCONTI_EXPORT __declspec(dllimport) 38 | #endif 39 | #else 40 | #define SOTTASKCONTI_EXPORT 41 | #endif 42 | 43 | /* --------------------------------------------------------------------- */ 44 | /* --- CLASS ----------------------------------------------------------- */ 45 | /* --------------------------------------------------------------------- */ 46 | 47 | namespace dynamicgraph { 48 | namespace sot { 49 | 50 | class SOTTASKCONTI_EXPORT TaskConti : public Task { 51 | protected: 52 | enum TimeRefValues { TIME_REF_UNSIGNIFICANT = -1, TIME_REF_TO_BE_SET = -2 }; 53 | 54 | int timeRef; 55 | double mu; 56 | dynamicgraph::Vector q0; 57 | 58 | public: 59 | static const std::string CLASS_NAME; 60 | virtual const std::string &getClassName(void) const { return CLASS_NAME; } 61 | 62 | public: 63 | TaskConti(const std::string &n); 64 | 65 | void referenceTime(const unsigned int &t) { timeRef = t; } 66 | const int &referenceTime(void) { return timeRef; } 67 | 68 | /* --- COMPUTATION --- */ 69 | VectorMultiBound &computeContiDesiredVelocity(VectorMultiBound &task, 70 | const int &time); 71 | 72 | /* --- SIGNALS ------------------------------------------------------------ */ 73 | public: 74 | dynamicgraph::SignalPtr controlPrevSIN; 75 | 76 | /* --- DISPLAY ------------------------------------------------------------ */ 77 | void display(std::ostream &os) const; 78 | }; 79 | 80 | } /* namespace sot */ 81 | } /* namespace dynamicgraph */ 82 | 83 | #endif /* #ifndef __SOT_TASKCONTI_H__ */ 84 | -------------------------------------------------------------------------------- /include/sot/core/task-pd.hh: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2010, 3 | * François Bleibel, 4 | * Olivier Stasse, 5 | * 6 | * CNRS/AIST 7 | * 8 | */ 9 | 10 | #ifndef __SOT_TASK_PD_H__ 11 | #define __SOT_TASK_PD_H__ 12 | 13 | /* --------------------------------------------------------------------- */ 14 | /* --- INCLUDE --------------------------------------------------------- */ 15 | /* --------------------------------------------------------------------- */ 16 | 17 | /* SOT */ 18 | #include 19 | 20 | /* --------------------------------------------------------------------- */ 21 | /* --- API ------------------------------------------------------------- */ 22 | /* --------------------------------------------------------------------- */ 23 | 24 | #if defined(WIN32) 25 | #if defined(task_pd_EXPORTS) 26 | #define SOTTASKPD_EXPORT __declspec(dllexport) 27 | #else 28 | #define SOTTASKPD_EXPORT __declspec(dllimport) 29 | #endif 30 | #else 31 | #define SOTTASKPD_EXPORT 32 | #endif 33 | 34 | /* --------------------------------------------------------------------- */ 35 | /* --- CLASS ----------------------------------------------------------- */ 36 | /* --------------------------------------------------------------------- */ 37 | 38 | namespace dynamicgraph { 39 | namespace sot { 40 | 41 | class SOTTASKPD_EXPORT TaskPD : public Task { 42 | public: 43 | static const std::string CLASS_NAME; 44 | virtual const std::string &getClassName(void) const { return CLASS_NAME; } 45 | 46 | dynamicgraph::Vector previousError; 47 | double beta; 48 | 49 | public: 50 | TaskPD(const std::string &n); 51 | 52 | /* --- COMPUTATION --- */ 53 | dynamicgraph::Vector &computeErrorDot(dynamicgraph::Vector &error, int time); 54 | VectorMultiBound &computeTaskModif(VectorMultiBound &error, int time); 55 | 56 | /* --- SIGNALS ------------------------------------------------------------ */ 57 | public: 58 | dynamicgraph::SignalTimeDependent errorDotSOUT; 59 | dynamicgraph::SignalPtr errorDotSIN; 60 | 61 | /* --- PARAMS --- */ 62 | void initCommand(void); 63 | }; 64 | 65 | } /* namespace sot */ 66 | } /* namespace dynamicgraph */ 67 | 68 | #endif /* #ifndef __SOT_TASK_PD_H__ */ 69 | -------------------------------------------------------------------------------- /include/sot/core/task-unilateral.hh: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2010, 3 | * François Bleibel, 4 | * Olivier Stasse, 5 | * 6 | * CNRS/AIST 7 | * 8 | */ 9 | 10 | #ifndef __SOT_TASKUNILATERAL_H__ 11 | #define __SOT_TASKUNILATERAL_H__ 12 | 13 | /* --------------------------------------------------------------------- */ 14 | /* --- INCLUDE --------------------------------------------------------- */ 15 | /* --------------------------------------------------------------------- */ 16 | 17 | /* Matrix */ 18 | #include 19 | 20 | /* STD */ 21 | #include 22 | 23 | /* SOT */ 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | /* --------------------------------------------------------------------- */ 30 | /* --- API ------------------------------------------------------------- */ 31 | /* --------------------------------------------------------------------- */ 32 | 33 | #if defined(WIN32) 34 | #if defined(task_unilateral_EXPORTS) 35 | #define SOTTASKUNILATERAL_EXPORT __declspec(dllexport) 36 | #else 37 | #define SOTTASKUNILATERAL_EXPORT __declspec(dllimport) 38 | #endif 39 | #else 40 | #define SOTTASKUNILATERAL_EXPORT 41 | #endif 42 | 43 | /* --------------------------------------------------------------------- */ 44 | /* --- CLASS ----------------------------------------------------------- */ 45 | /* --------------------------------------------------------------------- */ 46 | 47 | namespace dynamicgraph { 48 | namespace sot { 49 | 50 | class SOTTASKUNILATERAL_EXPORT TaskUnilateral : public Task { 51 | protected: 52 | std::list featureList; 53 | 54 | public: 55 | static const std::string CLASS_NAME; 56 | virtual const std::string &getClassName(void) const { return CLASS_NAME; } 57 | 58 | public: 59 | TaskUnilateral(const std::string &n); 60 | 61 | /* --- COMPUTATION --- */ 62 | VectorMultiBound &computeTaskUnilateral(VectorMultiBound &res, int time); 63 | 64 | /* --- SIGNALS ------------------------------------------------------------ */ 65 | public: 66 | dynamicgraph::SignalPtr positionSIN; 67 | dynamicgraph::SignalPtr referenceInfSIN; 68 | dynamicgraph::SignalPtr referenceSupSIN; 69 | dynamicgraph::SignalPtr dtSIN; 70 | 71 | /* --- DISPLAY ------------------------------------------------------------ */ 72 | void display(std::ostream &os) const; 73 | }; 74 | 75 | } /* namespace sot */ 76 | } /* namespace dynamicgraph */ 77 | 78 | #endif /* #ifndef __SOT_TASKUNILATERAL_H__ */ 79 | -------------------------------------------------------------------------------- /include/sot/core/unary-op.hh: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2010, 3 | * François Bleibel, 4 | * Olivier Stasse, 5 | * 6 | * CNRS/AIST 7 | * 8 | */ 9 | 10 | #ifndef SOT_CORE_UNARYOP_HH 11 | #define SOT_CORE_UNARYOP_HH 12 | 13 | /* --------------------------------------------------------------------- */ 14 | /* --- INCLUDE --------------------------------------------------------- */ 15 | /* --------------------------------------------------------------------- */ 16 | 17 | /* SOT */ 18 | #include 19 | #include 20 | 21 | /* --------------------------------------------------------------------- */ 22 | /* --- CLASS ----------------------------------------------------------- */ 23 | /* --------------------------------------------------------------------- */ 24 | 25 | namespace dynamicgraph { 26 | namespace sot { 27 | 28 | template 29 | class UnaryOp : public Entity { 30 | Operator op; 31 | typedef typename Operator::Tin Tin; 32 | typedef typename Operator::Tout Tout; 33 | typedef UnaryOp Self; 34 | 35 | public: /* --- CONSTRUCTION --- */ 36 | static std::string getTypeInName(void) { return Operator::nameTypeIn(); } 37 | static std::string getTypeOutName(void) { return Operator::nameTypeOut(); } 38 | static const std::string CLASS_NAME; 39 | 40 | virtual const std::string &getClassName() const { return CLASS_NAME; } 41 | 42 | std::string getDocString() const { return op.getDocString(); } 43 | 44 | UnaryOp(const std::string &name) 45 | : Entity(name), 46 | SIN(NULL, Self::CLASS_NAME + "(" + name + ")::input(" + 47 | Self::getTypeInName() + ")::sin"), 48 | SOUT(boost::bind(&Self::computeOperation, this, _1, _2), SIN, 49 | Self::CLASS_NAME + "(" + name + ")::output(" + 50 | Self::getTypeOutName() + ")::sout") { 51 | signalRegistration(SIN << SOUT); 52 | op.addSpecificCommands(*this, commandMap); 53 | } 54 | 55 | virtual ~UnaryOp(void){}; 56 | 57 | public: /* --- SIGNAL --- */ 58 | SignalPtr SIN; 59 | SignalTimeDependent SOUT; 60 | 61 | protected: 62 | Tout &computeOperation(Tout &res, int time) { 63 | const Tin &x1 = SIN(time); 64 | op(x1, res); 65 | return res; 66 | } 67 | 68 | public: /* --- PARAMS --- */ 69 | }; 70 | } /* namespace sot */ 71 | } /* namespace dynamicgraph */ 72 | 73 | #endif // #ifndef SOT_CORE_UNARYOP_HH 74 | -------------------------------------------------------------------------------- /include/sot/core/utils-windows.hh: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2010, 3 | * François Bleibel, 4 | * Olivier Stasse, 5 | * 6 | * CNRS/AIST 7 | * 8 | */ 9 | 10 | #ifndef UTILSWINDOWS_H_ 11 | #define UTILSWINDOWS_H_ 12 | 13 | #ifdef WIN32 14 | 15 | #include < time.h > 16 | 17 | #include "sot/core/api.hh" 18 | #define NOMINMAX 19 | #include 20 | 21 | struct SOT_CORE_EXPORT timezone { 22 | int tz_minuteswest; /* minutes W of Greenwich */ 23 | int tz_dsttime; /* type of dst correction */ 24 | }; 25 | 26 | int SOT_CORE_EXPORT gettimeofday(struct timeval *tv, struct timezone *tz); 27 | 28 | #endif /*WIN32*/ 29 | 30 | #endif /* UTILSWINDOWS_H_ */ 31 | -------------------------------------------------------------------------------- /include/sot/core/vector-constant.hh: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2010, 3 | * François Bleibel, 4 | * Olivier Stasse, 5 | * 6 | * CNRS/AIST 7 | * 8 | */ 9 | 10 | #ifndef DYNAMICGRAPH_SOT_VECTOR_CONSTANT_H 11 | #define DYNAMICGRAPH_SOT_VECTOR_CONSTANT_H 12 | 13 | #include 14 | #include 15 | 16 | /* Matrix */ 17 | #include 18 | 19 | /* --------------------------------------------------------------------- */ 20 | /* --- VECTOR ---------------------------------------------------------- */ 21 | /* --------------------------------------------------------------------- */ 22 | namespace dynamicgraph { 23 | namespace sot { 24 | 25 | namespace command { 26 | namespace vectorConstant { 27 | class Resize; 28 | } 29 | } // namespace command 30 | 31 | class VectorConstant : public Entity { 32 | friend class command::vectorConstant::Resize; 33 | int rows; 34 | 35 | public: 36 | static const std::string CLASS_NAME; 37 | virtual const std::string &getClassName(void) const { return CLASS_NAME; } 38 | 39 | VectorConstant(const std::string &name); 40 | 41 | virtual ~VectorConstant(void) {} 42 | 43 | SignalTimeDependent SOUT; 44 | 45 | /// \brief Set value of vector (and therefore of output signal) 46 | void setValue(const dynamicgraph::Vector &inValue); 47 | }; 48 | 49 | } // namespace sot 50 | } // namespace dynamicgraph 51 | 52 | #endif // DYNAMICGRAPH_SOT_VECTOR_CONSTANT_H 53 | -------------------------------------------------------------------------------- /include/sot/core/vector-to-rotation.hh: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2010, 3 | * François Bleibel, 4 | * Olivier Stasse, 5 | * 6 | * CNRS/AIST 7 | * 8 | */ 9 | 10 | #ifndef __SOTVECTORTOMATRIX_HH 11 | #define __SOTVECTORTOMATRIX_HH 12 | 13 | #include 14 | #include 15 | 16 | #include 17 | 18 | /* Matrix */ 19 | #include 20 | 21 | /* STD */ 22 | #include 23 | 24 | /* --------------------------------------------------------------------- */ 25 | /* --- API ------------------------------------------------------------- */ 26 | /* --------------------------------------------------------------------- */ 27 | 28 | #if defined(WIN32) 29 | #if defined(vector_to_rotation_EXPORTS) 30 | #define SOTVECTORTOROTATION_EXPORT __declspec(dllexport) 31 | #else 32 | #define SOTVECTORTOROTATION_EXPORT __declspec(dllimport) 33 | #endif 34 | #else 35 | #define SOTVECTORTOROTATION_EXPORT 36 | #endif 37 | 38 | /* --------------------------------------------------------------------- */ 39 | /* --- VECTOR ---------------------------------------------------------- */ 40 | /* --------------------------------------------------------------------- */ 41 | namespace dynamicgraph { 42 | namespace sot { 43 | 44 | class [[deprecated( 45 | "use RPYToMatrix")]] SOTVECTORTOROTATION_EXPORT VectorToRotation 46 | : public dynamicgraph::Entity { 47 | enum sotAxis { AXIS_X, AXIS_Y, AXIS_Z }; 48 | 49 | unsigned int size; 50 | std::vector axes; 51 | 52 | public: 53 | static const std::string CLASS_NAME; 54 | virtual const std::string &getClassName(void) const { return CLASS_NAME; } 55 | 56 | VectorToRotation(const std::string &name); 57 | 58 | virtual ~VectorToRotation(void) {} 59 | 60 | dynamicgraph::SignalPtr SIN; 61 | dynamicgraph::SignalTimeDependent SOUT; 62 | 63 | MatrixRotation &computeRotation(const dynamicgraph::Vector &angles, 64 | MatrixRotation &res); 65 | }; 66 | 67 | } /* namespace sot */ 68 | } /* namespace dynamicgraph */ 69 | 70 | #endif // #ifndef __SOTVECTORTOMATRIX_HH 71 | -------------------------------------------------------------------------------- /include/sot/core/visual-point-projecter.hh: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2011, Nicolas Mansard, LAAS-CNRS 3 | * 4 | */ 5 | 6 | #ifndef __sot_core_VisualPointProjecter_H__ 7 | #define __sot_core_VisualPointProjecter_H__ 8 | /* --------------------------------------------------------------------- */ 9 | /* --- API ------------------------------------------------------------- */ 10 | /* --------------------------------------------------------------------- */ 11 | 12 | #if defined(WIN32) 13 | #if defined(visual_point_projecter_EXPORTS) 14 | #define SOTVISUALPOINTPROJECTER_EXPORT __declspec(dllexport) 15 | #else 16 | #define SOTVISUALPOINTPROJECTER_EXPORT __declspec(dllimport) 17 | #endif 18 | #else 19 | #define SOTVISUALPOINTPROJECTER_EXPORT 20 | #endif 21 | 22 | /* --------------------------------------------------------------------- */ 23 | /* --- INCLUDE --------------------------------------------------------- */ 24 | /* --------------------------------------------------------------------- */ 25 | 26 | /* Matrix */ 27 | #include 28 | 29 | #include 30 | 31 | /* SOT */ 32 | #include 33 | #include 34 | 35 | namespace dynamicgraph { 36 | namespace sot { 37 | 38 | /* --------------------------------------------------------------------- */ 39 | /* --- CLASS ----------------------------------------------------------- */ 40 | /* --------------------------------------------------------------------- */ 41 | 42 | class SOTVISUALPOINTPROJECTER_EXPORT VisualPointProjecter 43 | : public ::dynamicgraph::Entity, 44 | public ::dynamicgraph::EntityHelper { 45 | public: /* --- CONSTRUCTOR ---- */ 46 | VisualPointProjecter(const std::string &name); 47 | 48 | public: /* --- ENTITY INHERITANCE --- */ 49 | static const std::string CLASS_NAME; 50 | virtual void display(std::ostream &os) const; 51 | virtual const std::string &getClassName(void) const { return CLASS_NAME; } 52 | 53 | public: /* --- SIGNALS --- */ 54 | DECLARE_SIGNAL_IN(point3D, dynamicgraph::Vector); 55 | DECLARE_SIGNAL_IN(transfo, MatrixHomogeneous); 56 | 57 | DECLARE_SIGNAL_OUT(point3Dgaze, dynamicgraph::Vector); 58 | DECLARE_SIGNAL_OUT(depth, double); 59 | DECLARE_SIGNAL_OUT(point2D, dynamicgraph::Vector); 60 | 61 | }; // class VisualPointProjecter 62 | 63 | } // namespace sot 64 | } // namespace dynamicgraph 65 | 66 | #endif // #ifndef __sot_core_VisualPointProjecter_H__ 67 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | sot-core 4 | 4.11.8 5 | 6 | Hierarchical task solver plug-in for dynamic-graph 7 | 8 | Guilhem Saurel 9 | 10 | BSD 11 | 12 | http://stack-of-tasks.github.io 13 | http://www.github.com/stack-of-tasks/sot-core 14 | http://www.github.com/stack-of-tasks/sot-core/issues 15 | Olivier Stasse 16 | ostasse@laas.fr 17 | 18 | doxygen 19 | 20 | catkin 21 | ament_cmake 22 | boost 23 | dynamic-graph 24 | dynamic-graph-python 25 | pinocchio 26 | 27 | doxygen 28 | dynamic-graph 29 | 30 | cmake 31 | 32 | 33 | cmake 34 | 35 | 36 | -------------------------------------------------------------------------------- /pyproject.toml: -------------------------------------------------------------------------------- 1 | [tool.black] 2 | exclude = "cmake" 3 | -------------------------------------------------------------------------------- /setup.cfg: -------------------------------------------------------------------------------- 1 | [flake8] 2 | exclude = cmake 3 | max-line-length = 88 4 | ignore = E226, E704, E24, E121, W504, E126, E123, W503, E203 5 | -------------------------------------------------------------------------------- /src/control/admittance-control-op-point-python.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | typedef boost::mpl::vector 4 | entities_t; 5 | -------------------------------------------------------------------------------- /src/control/control-gr-python.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | typedef boost::mpl::vector entities_t; 4 | -------------------------------------------------------------------------------- /src/control/control-pd-python.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | typedef boost::mpl::vector entities_t; 4 | -------------------------------------------------------------------------------- /src/debug/contiifstream.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2010, 3 | * François Bleibel, 4 | * Olivier Stasse, 5 | * 6 | * CNRS/AIST 7 | * 8 | */ 9 | 10 | #include 11 | #include 12 | 13 | using namespace dynamicgraph::sot; 14 | 15 | Contiifstream::Contiifstream(const std::string &n) 16 | : filename(n), cursor(0), first(true) {} 17 | 18 | Contiifstream::~Contiifstream(void) { sotDEBUGINOUT(5); } 19 | 20 | bool Contiifstream::loop(void) { 21 | sotDEBUGIN(25); 22 | bool res = false; 23 | 24 | std::fstream file(filename.c_str()); 25 | 26 | file.seekg(cursor); 27 | file.sync(); 28 | 29 | while (1) { 30 | file.get(buffer, BUFFER_SIZE); 31 | if (file.gcount()) { 32 | res = true; 33 | std::string line(buffer); 34 | if (!first) reader.push_back(line); 35 | cursor = file.tellg(); 36 | cursor++; 37 | file.get(*buffer); // get the last char ( = '\n') 38 | sotDEBUG(15) << "line: " << line << std::endl; 39 | } else { 40 | break; 41 | } 42 | } 43 | 44 | first = false; 45 | sotDEBUGOUT(25); 46 | return res; 47 | } 48 | 49 | std::string Contiifstream::next(void) { 50 | std::string res = *reader.begin(); 51 | reader.pop_front(); 52 | return res; 53 | } 54 | -------------------------------------------------------------------------------- /src/debug/debug.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2010, 3 | * François Bleibel, 4 | * Olivier Stasse, 5 | * 6 | * CNRS/AIST 7 | * 8 | */ 9 | 10 | #include 11 | #include 12 | #include 13 | 14 | using namespace dynamicgraph::sot; 15 | 16 | namespace dynamicgraph { 17 | namespace sot { 18 | #ifdef WIN32 19 | const char *DebugTrace::DEBUG_FILENAME_DEFAULT = "c:/tmp/sot-core-traces.txt"; 20 | #else // WIN32 21 | const char *DebugTrace::DEBUG_FILENAME_DEFAULT = "/tmp/sot-core-traces.txt"; 22 | #endif // WIN32 23 | 24 | #ifdef VP_DEBUG 25 | #ifdef WIN32 26 | std::ofstream debugfile("C:/tmp/sot-core-traces.txt", 27 | std::ios::trunc &std::ios::out); 28 | #else // WIN32 29 | std::ofstream debugfile("/tmp/sot-core-traces.txt", 30 | std::ios::trunc &std::ios::out); 31 | #endif // WIN32 32 | #else // VP_DEBUG 33 | 34 | std::ofstream debugfile; 35 | 36 | class __sotDebug_init { 37 | public: 38 | __sotDebug_init() { debugfile.setstate(std::ios::failbit); } 39 | }; 40 | __sotDebug_init __sotDebug_initialisator; 41 | 42 | #endif // VP_DEBUG 43 | 44 | } /* namespace sot */ 45 | } /* namespace dynamicgraph */ 46 | 47 | void DebugTrace::openFile(const char *filename) { 48 | if (debugfile.good() && debugfile.is_open()) debugfile.close(); 49 | debugfile.clear(); 50 | debugfile.open(filename, std::ios::trunc & std::ios::out); 51 | } 52 | 53 | void DebugTrace::closeFile(const char *) { 54 | if (debugfile.good() && debugfile.is_open()) debugfile.close(); 55 | debugfile.setstate(std::ios::failbit); 56 | } 57 | 58 | namespace dynamicgraph { 59 | namespace sot { 60 | DebugTrace sotDEBUGFLOW(debugfile); 61 | DebugTrace sotERRORFLOW(debugfile); 62 | } // namespace sot. 63 | } // namespace dynamicgraph 64 | -------------------------------------------------------------------------------- /src/dynamic_graph/__init__.py: -------------------------------------------------------------------------------- 1 | __path__ = __import__("pkgutil").extend_path(__path__, __name__) 2 | -------------------------------------------------------------------------------- /src/dynamic_graph/sot/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/stack-of-tasks/sot-core/1c36e63ba258b948761cac6564b995bbfbe2f3c6/src/dynamic_graph/sot/__init__.py -------------------------------------------------------------------------------- /src/dynamic_graph/sot/core/__init__.py: -------------------------------------------------------------------------------- 1 | from . import wrap # noqa 2 | from .wrap import Device, Flags # noqa 3 | -------------------------------------------------------------------------------- /src/dynamic_graph/sot/core/math_small_entities.py: -------------------------------------------------------------------------------- 1 | # flake8: noqa 2 | # from operator import Compose_RPY_and_T 3 | # from operator import EndomorphismBasis 4 | # from operator import ComposeVector_RPY_T 5 | # from operator import WeightAdd_of_vector 6 | # from operator import WeightDir 7 | # from operator import Nullificator 8 | from .operator import ( 9 | Add_of_double, 10 | Add_of_matrix, 11 | Add_of_vector, 12 | Component_of_vector, 13 | Compose_R_and_T, 14 | ConvolutionTemporal, 15 | HomoToMatrix, 16 | HomoToRotation, 17 | HomoToTwist, 18 | Inverse_of_matrix, 19 | Inverse_of_matrixHomo, 20 | Inverse_of_matrixrotation, 21 | Inverse_of_matrixtwist, 22 | Inverse_of_unitquat, 23 | MatrixDiagonal, 24 | MatrixHomoToPose, 25 | MatrixHomoToPoseQuaternion, 26 | PoseQuaternionToMatrixHomo, 27 | MatrixHomoToPoseRollPitchYaw, 28 | MatrixHomoToPoseUTheta, 29 | MatrixToHomo, 30 | MatrixToQuaternion, 31 | MatrixToRPY, 32 | MatrixToUTheta, 33 | MatrixHomoToSE3Vector, 34 | SE3VectorToMatrixHomo, 35 | MatrixTranspose, 36 | Multiply_double_vector, 37 | Multiply_matrix_vector, 38 | Multiply_matrixTwist_vector, 39 | Multiply_matrixHomo_vector, 40 | Multiply_of_double, 41 | Multiply_of_matrix, 42 | Multiply_of_matrixHomo, 43 | Multiply_of_matrixrotation, 44 | Multiply_of_matrixtwist, 45 | Multiply_of_quaternion, 46 | Multiply_of_vector, 47 | PoseRollPitchYawToMatrixHomo, 48 | PoseRollPitchYawToPoseUTheta, 49 | PoseUThetaToMatrixHomo, 50 | QuaternionToMatrix, 51 | RPYToMatrix, 52 | Selec_column_of_matrix, 53 | Selec_of_matrix, 54 | Selec_of_vector, 55 | SkewSymToVector, 56 | Stack_of_vector, 57 | Substract_of_double, 58 | Substract_of_matrix, 59 | Substract_of_vector, 60 | UThetaToQuaternion, 61 | ) 62 | 63 | from .derivator import Derivator_of_Matrix, Derivator_of_Vector 64 | from .matrix_constant import MatrixConstant 65 | from .vector_constant import VectorConstant 66 | -------------------------------------------------------------------------------- /src/dynamic_graph/sot/core/meta_tasks_kine.py: -------------------------------------------------------------------------------- 1 | from dynamic_graph import plug 2 | from dynamic_graph.sot.core.gain_adaptive import GainAdaptive 3 | from dynamic_graph.sot.core.meta_task_6d import MetaTask6d 4 | 5 | # TODO: this function is imported from meta_tasks_kine in several places, 6 | # whereas it is defined in meta_tasks 7 | from dynamic_graph.sot.core.meta_tasks import gotoNd # noqa 8 | from dynamic_graph.sot.core.meta_tasks import MetaTaskCom 9 | from dynamic_graph.sot.core.task import Task 10 | 11 | 12 | class MetaTaskKine6d(MetaTask6d): 13 | def createTask(self): 14 | self.task = Task("task" + self.name) 15 | 16 | def createGain(self): 17 | self.gain = GainAdaptive("gain" + self.name) 18 | self.gain.set(0.1, 0.1, 125e3) 19 | 20 | def plugEverything(self): 21 | self.feature.setReference(self.featureDes.name) 22 | plug(self.dyn.signal(self.opPoint), self.feature.signal("position")) 23 | plug(self.dyn.signal("J" + self.opPoint), self.feature.signal("Jq")) 24 | self.task.add(self.feature.name) 25 | plug(self.task.error, self.gain.error) 26 | plug(self.gain.gain, self.task.controlGain) 27 | 28 | def keep(self): 29 | self.feature.position.recompute(self.dyn.position.time) 30 | self.feature.keep() 31 | 32 | 33 | class MetaTaskKineCom(MetaTaskCom): 34 | def __init__(self, dyn, name="com"): 35 | MetaTaskCom.__init__(self, dyn, name) 36 | self.task = Task("task" + name) 37 | self.plugTask() 38 | -------------------------------------------------------------------------------- /src/dynamic_graph/sot/core/utils/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/stack-of-tasks/sot-core/1c36e63ba258b948761cac6564b995bbfbe2f3c6/src/dynamic_graph/sot/core/utils/__init__.py -------------------------------------------------------------------------------- /src/dynamic_graph/sot/core/utils/viewer_loger.py: -------------------------------------------------------------------------------- 1 | import glob 2 | import os 3 | 4 | 5 | class ViewerLoger: 6 | """ 7 | This class replace the robotviewer client and log the data sent to the 8 | viewer for future replay. 9 | 10 | Example of use: 11 | from viewer_loger import ViewerLoger 12 | robot.viewer = ViewerLoger(robot) 13 | 14 | """ 15 | 16 | def __init__(self, robot): 17 | self.robot = robot 18 | self.viewer = robot.viewer 19 | self.fileMap = {} 20 | for f in glob.glob("/tmp/view*.dat"): 21 | os.remove(f) 22 | 23 | def updateElementConfig(self, name, state): 24 | t = self.robot.state.time 25 | if name not in self.fileMap: 26 | self.fileMap[name] = open("/tmp/view_" + name + ".dat", "w") 27 | self.fileMap[name].write( 28 | "\t".join( 29 | [ 30 | str(f) 31 | for f in [ 32 | t, 33 | ] 34 | + list(state) 35 | ] 36 | ) 37 | + "\n" 38 | ) 39 | self.viewer.updateElementConfig(name, state) 40 | -------------------------------------------------------------------------------- /src/exception/exception-dynamic.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2010, 3 | * François Bleibel, 4 | * Olivier Stasse, 5 | * 6 | * CNRS/AIST 7 | * 8 | */ 9 | 10 | #include 11 | 12 | #include 13 | #include 14 | 15 | using namespace dynamicgraph::sot; 16 | 17 | /* --------------------------------------------------------------------- */ 18 | /* --- CLASS ----------------------------------------------------------- */ 19 | /* --------------------------------------------------------------------- */ 20 | 21 | const std::string ExceptionDynamic::EXCEPTION_NAME = "Dynamic"; 22 | 23 | ExceptionDynamic::ExceptionDynamic( 24 | const ExceptionDynamic::ErrorCodeEnum &errcode, const std::string &msg) 25 | : ExceptionAbstract(errcode, msg) {} 26 | 27 | ExceptionDynamic::ExceptionDynamic( 28 | const ExceptionDynamic::ErrorCodeEnum &errcode, const std::string &msg, 29 | const char *format, ...) 30 | : ExceptionAbstract(errcode, msg) { 31 | va_list args; 32 | va_start(args, format); 33 | 34 | const unsigned int SIZE = 256; 35 | char buffer[SIZE]; 36 | vsnprintf(buffer, SIZE, format, args); 37 | 38 | message += buffer; 39 | 40 | va_end(args); 41 | } 42 | 43 | /* 44 | * Local variables: 45 | * c-basic-offset: 2 46 | * End: 47 | */ 48 | -------------------------------------------------------------------------------- /src/exception/exception-factory.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2010, 3 | * François Bleibel, 4 | * Olivier Stasse, 5 | * 6 | * CNRS/AIST 7 | * 8 | */ 9 | 10 | #include 11 | 12 | #include 13 | #include 14 | #include 15 | 16 | using namespace dynamicgraph::sot; 17 | 18 | /* --------------------------------------------------------------------- */ 19 | /* --- CLASS ----------------------------------------------------------- */ 20 | /* --------------------------------------------------------------------- */ 21 | 22 | const std::string ExceptionFactory::EXCEPTION_NAME = "Factory"; 23 | 24 | ExceptionFactory::ExceptionFactory( 25 | const ExceptionFactory::ErrorCodeEnum &errcode, const std::string &msg) 26 | : ExceptionAbstract(errcode, msg) { 27 | sotDEBUGF(15, "Created with message <%s>.", msg.c_str()); 28 | sotDEBUG(1) << "Created with message <%s>." << msg << std::endl; 29 | } 30 | 31 | ExceptionFactory::ExceptionFactory( 32 | const ExceptionFactory::ErrorCodeEnum &errcode, const std::string &msg, 33 | const char *format, ...) 34 | : ExceptionAbstract(errcode, msg) { 35 | va_list args; 36 | va_start(args, format); 37 | 38 | const unsigned int SIZE = 256; 39 | char buffer[SIZE]; 40 | vsnprintf(buffer, SIZE, format, args); 41 | 42 | sotDEBUG(15) << "Created " 43 | << " with message <" << msg << "> and buffer <" << buffer 44 | << ">. " << std::endl; 45 | 46 | message += buffer; 47 | 48 | va_end(args); 49 | 50 | sotDEBUG(1) << "Throw exception " << EXCEPTION_NAME << "[#" << errcode 51 | << "]: " 52 | << "<" << message << ">." << std::endl; 53 | } 54 | 55 | /* 56 | * Local variables: 57 | * c-basic-offset: 2 58 | * End: 59 | */ 60 | -------------------------------------------------------------------------------- /src/exception/exception-feature.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2010, 3 | * François Bleibel, 4 | * Olivier Stasse, 5 | * 6 | * CNRS/AIST 7 | * 8 | */ 9 | 10 | #include 11 | 12 | #include 13 | #include 14 | 15 | using namespace dynamicgraph::sot; 16 | 17 | /* --------------------------------------------------------------------- */ 18 | /* --- CLASS ----------------------------------------------------------- */ 19 | /* --------------------------------------------------------------------- */ 20 | 21 | const std::string ExceptionFeature::EXCEPTION_NAME = "Feature"; 22 | 23 | ExceptionFeature::ExceptionFeature( 24 | const ExceptionFeature::ErrorCodeEnum &errcode, const std::string &msg) 25 | : ExceptionAbstract(errcode, msg) {} 26 | 27 | ExceptionFeature::ExceptionFeature( 28 | const ExceptionFeature::ErrorCodeEnum &errcode, const std::string &msg, 29 | const char *format, ...) 30 | : ExceptionAbstract(errcode, msg) { 31 | va_list args; 32 | va_start(args, format); 33 | 34 | const unsigned int SIZE = 256; 35 | char buffer[SIZE]; 36 | vsnprintf(buffer, SIZE, format, args); 37 | 38 | message = buffer; 39 | 40 | va_end(args); 41 | } 42 | 43 | /* 44 | * Local variables: 45 | * c-basic-offset: 2 46 | * End: 47 | */ 48 | -------------------------------------------------------------------------------- /src/exception/exception-signal.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2010, 3 | * François Bleibel, 4 | * Olivier Stasse, 5 | * 6 | * CNRS/AIST 7 | * 8 | */ 9 | 10 | #include 11 | 12 | #include 13 | #include 14 | 15 | using namespace dynamicgraph::sot; 16 | 17 | /* --------------------------------------------------------------------- */ 18 | /* --- CLASS ----------------------------------------------------------- */ 19 | /* --------------------------------------------------------------------- */ 20 | 21 | const std::string ExceptionSignal::EXCEPTION_NAME = "Signal"; 22 | 23 | ExceptionSignal::ExceptionSignal(const ExceptionSignal::ErrorCodeEnum &errcode, 24 | const std::string &msg) 25 | : ExceptionAbstract(errcode, msg) {} 26 | 27 | ExceptionSignal::ExceptionSignal(const ExceptionSignal::ErrorCodeEnum &errcode, 28 | const std::string &msg, const char *format, 29 | ...) 30 | : ExceptionAbstract(errcode, msg) { 31 | va_list args; 32 | va_start(args, format); 33 | 34 | const unsigned int SIZE = 256; 35 | char buffer[SIZE]; 36 | vsnprintf(buffer, SIZE, format, args); 37 | 38 | message += buffer; 39 | 40 | va_end(args); 41 | } 42 | 43 | /* 44 | * Local variables: 45 | * c-basic-offset: 2 46 | * End: 47 | */ 48 | -------------------------------------------------------------------------------- /src/exception/exception-task.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2010, 3 | * François Bleibel, 4 | * Olivier Stasse, 5 | * 6 | * CNRS/AIST 7 | * 8 | */ 9 | 10 | #include 11 | 12 | #include 13 | #include 14 | 15 | using namespace dynamicgraph::sot; 16 | 17 | /* --------------------------------------------------------------------- */ 18 | /* --- CLASS ----------------------------------------------------------- */ 19 | /* --------------------------------------------------------------------- */ 20 | 21 | const std::string ExceptionTask::EXCEPTION_NAME = "Task"; 22 | 23 | ExceptionTask::ExceptionTask(const ExceptionTask::ErrorCodeEnum &errcode, 24 | const std::string &msg) 25 | : ExceptionAbstract(errcode, msg) {} 26 | 27 | ExceptionTask::ExceptionTask(const ExceptionTask::ErrorCodeEnum &errcode, 28 | const std::string &msg, const char *format, ...) 29 | : ExceptionAbstract(errcode, msg) { 30 | va_list args; 31 | va_start(args, format); 32 | 33 | const unsigned int SIZE = 256; 34 | char buffer[SIZE]; 35 | vsnprintf(buffer, SIZE, format, args); 36 | 37 | message = buffer; 38 | 39 | va_end(args); 40 | } 41 | 42 | /* 43 | * Local variables: 44 | * c-basic-offset: 2 45 | * End: 46 | */ 47 | -------------------------------------------------------------------------------- /src/exception/exception-tools.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2010, 3 | * François Bleibel, 4 | * Olivier Stasse, 5 | * 6 | * CNRS/AIST 7 | * 8 | */ 9 | 10 | #include 11 | 12 | #include 13 | #include 14 | 15 | using namespace dynamicgraph::sot; 16 | 17 | /* --------------------------------------------------------------------- */ 18 | /* --- CLASS ----------------------------------------------------------- */ 19 | /* --------------------------------------------------------------------- */ 20 | 21 | const std::string ExceptionTools::EXCEPTION_NAME = "Tools"; 22 | 23 | ExceptionTools::ExceptionTools(const ExceptionTools::ErrorCodeEnum &errcode, 24 | const std::string &msg) 25 | : ExceptionAbstract(errcode, msg) {} 26 | 27 | ExceptionTools::ExceptionTools(const ExceptionTools::ErrorCodeEnum &errcode, 28 | const std::string &msg, const char *format, ...) 29 | : ExceptionAbstract(errcode, msg) { 30 | va_list args; 31 | va_start(args, format); 32 | 33 | const unsigned int SIZE = 256; 34 | char buffer[SIZE]; 35 | vsnprintf(buffer, SIZE, format, args); 36 | 37 | message += buffer; 38 | 39 | va_end(args); 40 | } 41 | 42 | /* 43 | * Local variables: 44 | * c-basic-offset: 2 45 | * End: 46 | */ 47 | -------------------------------------------------------------------------------- /src/factory/additional-functions.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2010, 3 | * François Bleibel, 4 | * Olivier Stasse, 5 | * 6 | * CNRS/AIST 7 | * 8 | */ 9 | 10 | #include 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | using namespace std; 17 | using namespace dynamicgraph::sot; 18 | using namespace dynamicgraph; 19 | 20 | /* \brief Constructor. At creation, overloads (deregisters-then registers 21 | * again) the 'new' function in the g_shell 22 | */ 23 | AdditionalFunctions::AdditionalFunctions() {} 24 | 25 | AdditionalFunctions::~AdditionalFunctions() {} 26 | 27 | void AdditionalFunctions::cmdFlagSet(const std::string &cmdLine, 28 | istringstream &cmdArg, std::ostream &os) { 29 | if (cmdLine == "help") { 30 | os << " - set {#&|}START:END" 31 | << "\t\tSet or reset the flag value." << endl; 32 | return; 33 | } 34 | 35 | try { 36 | Signal &sig1 = dynamic_cast &>( 37 | PoolStorage::getInstance()->getSignal(cmdArg)); 38 | 39 | dgDEBUG(25) << "set..." << endl; 40 | Flags fl; 41 | try { 42 | fl = sig1.accessCopy(); 43 | } catch (...) { 44 | } 45 | cmdArg >> std::ws >> fl; 46 | dgDEBUG(15) << "Fl=" << fl << std::endl; 47 | sig1 = fl; 48 | 49 | } catch (ExceptionAbstract &err) { 50 | throw; 51 | } catch (...) { 52 | DG_THROW ExceptionFactory(ExceptionFactory::SYNTAX_ERROR, 53 | "setflag: sig should be of flag type. ", 54 | "(while calling setflag)."); 55 | } 56 | } 57 | -------------------------------------------------------------------------------- /src/factory/factory.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2010, 3 | * François Bleibel, 4 | * Olivier Stasse, 5 | * 6 | * CNRS/AIST 7 | * 8 | */ 9 | 10 | /* --------------------------------------------------------------------- */ 11 | /* --- INCLUDE --------------------------------------------------------- */ 12 | /* --------------------------------------------------------------------- */ 13 | 14 | /* --- SOT --- */ 15 | #include 16 | #include 17 | 18 | using namespace std; 19 | using namespace dynamicgraph::sot; 20 | -------------------------------------------------------------------------------- /src/feature/feature-1d-python.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | typedef boost::mpl::vector entities_t; 4 | -------------------------------------------------------------------------------- /src/feature/feature-generic-python.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | typedef boost::mpl::vector entities_t; 4 | -------------------------------------------------------------------------------- /src/feature/feature-joint-limits-command.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2010, 3 | * Florent Lamiraux 4 | * 5 | * CNRS/AIST 6 | * 7 | */ 8 | 9 | #ifndef FEATURE_JOINT_LIMITS_COMMAND_H 10 | #define FEATURE_JOINT_LIMITS_COMMAND_H 11 | 12 | #include 13 | #include 14 | #include 15 | 16 | #include 17 | 18 | namespace dynamicgraph { 19 | namespace sot { 20 | namespace command { 21 | namespace featureJointLimits { 22 | using ::dynamicgraph::command::Command; 23 | using ::dynamicgraph::command::Value; 24 | 25 | // Command Actuate 26 | class Actuate : public Command { 27 | public: 28 | virtual ~Actuate() {} 29 | /// Create command and store it in Entity 30 | /// \param entity instance of Entity owning this command 31 | /// \param docstring documentation of the command 32 | Actuate(FeatureJointLimits &entity, const std::string &docstring) 33 | : Command(entity, std::vector(), docstring) {} 34 | virtual Value doExecute() { 35 | FeatureJointLimits &fjl = static_cast(owner()); 36 | Flags fl(63); // 0x0000003f = 00000000000000000000000000111111 37 | fjl.selectionSIN = (!fl); 38 | // return void 39 | return Value(); 40 | } 41 | }; // class Actuate 42 | } // namespace featureJointLimits 43 | } // namespace command 44 | } /* namespace sot */ 45 | } /* namespace dynamicgraph */ 46 | 47 | #endif // FEATURE_JOINT_LIMITS_COMMAND_H 48 | -------------------------------------------------------------------------------- /src/feature/feature-joint-limits-python.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | typedef boost::mpl::vector entities_t; 4 | -------------------------------------------------------------------------------- /src/feature/feature-line-distance-python.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | typedef boost::mpl::vector entities_t; 4 | -------------------------------------------------------------------------------- /src/feature/feature-point6d-python.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | SOT_CORE_DISABLE_WARNING_PUSH 5 | SOT_CORE_DISABLE_WARNING_DEPRECATED 6 | typedef boost::mpl::vector entities_t; 7 | SOT_CORE_DISABLE_WARNING_POP 8 | -------------------------------------------------------------------------------- /src/feature/feature-point6d-relative-python.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | SOT_CORE_DISABLE_WARNING_PUSH 5 | SOT_CORE_DISABLE_WARNING_DEPRECATED 6 | typedef boost::mpl::vector 7 | entities_t; 8 | SOT_CORE_DISABLE_WARNING_POP 9 | -------------------------------------------------------------------------------- /src/feature/feature-pose-python.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | namespace dgs = dynamicgraph::sot; 4 | 5 | typedef boost::mpl::vector, 6 | dgs::FeaturePose > 7 | entities_t; 8 | -------------------------------------------------------------------------------- /src/feature/feature-pose.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2019 3 | * Joseph Mirabel 4 | * 5 | * LAAS-CNRS 6 | * 7 | */ 8 | 9 | /* --------------------------------------------------------------------- */ 10 | /* --- INCLUDE --------------------------------------------------------- */ 11 | /* --------------------------------------------------------------------- */ 12 | 13 | /* --- SOT --- */ 14 | //#define VP_DEBUG 15 | //#define VP_DEBUG_MODE 45 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | 30 | using namespace dynamicgraph::sot; 31 | 32 | SOT_CORE_DISABLE_WARNING_PUSH 33 | SOT_CORE_DISABLE_WARNING_DEPRECATED 34 | typedef FeaturePose FeaturePose_t; 35 | typedef FeaturePose FeaturePoseSE3_t; 36 | template <> 37 | DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(FeaturePose_t, "FeaturePose"); 38 | template <> 39 | DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(FeaturePoseSE3_t, "FeaturePoseSE3"); 40 | SOT_CORE_DISABLE_WARNING_POP 41 | 42 | namespace dynamicgraph { 43 | namespace sot { 44 | template class FeaturePose; 45 | template class FeaturePose; 46 | } // namespace sot 47 | } // namespace dynamicgraph 48 | -------------------------------------------------------------------------------- /src/feature/feature-posture-python.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | typedef boost::mpl::vector entities_t; 4 | -------------------------------------------------------------------------------- /src/feature/feature-task-python.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | typedef boost::mpl::vector entities_t; 4 | -------------------------------------------------------------------------------- /src/feature/feature-task.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2010, 3 | * François Bleibel, 4 | * Olivier Stasse, 5 | * 6 | * CNRS/AIST 7 | * 8 | */ 9 | 10 | /* --------------------------------------------------------------------- */ 11 | /* --- INCLUDE --------------------------------------------------------- */ 12 | /* --------------------------------------------------------------------- */ 13 | 14 | /* --- SOT --- */ 15 | #include 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | using namespace std; 22 | using namespace dynamicgraph::sot; 23 | using namespace dynamicgraph; 24 | 25 | #include 26 | DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(FeatureTask, "FeatureTask"); 27 | 28 | /* --------------------------------------------------------------------- */ 29 | /* --- CLASS ----------------------------------------------------------- */ 30 | /* --------------------------------------------------------------------- */ 31 | 32 | FeatureTask::FeatureTask(const string &pointName) : FeatureGeneric(pointName) {} 33 | 34 | /* --------------------------------------------------------------------- */ 35 | /* --------------------------------------------------------------------- */ 36 | /* --------------------------------------------------------------------- */ 37 | 38 | /* --------------------------------------------------------------------- */ 39 | /* --------------------------------------------------------------------- */ 40 | /* --------------------------------------------------------------------- */ 41 | 42 | void FeatureTask::display(std::ostream &os) const { 43 | os << "Feature from task <" << getName(); 44 | if (taskPtr) os << ": from task " << taskPtr->getName(); 45 | os << std::endl; 46 | } 47 | -------------------------------------------------------------------------------- /src/feature/feature-vector3-python.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | typedef boost::mpl::vector entities_t; 4 | -------------------------------------------------------------------------------- /src/feature/feature-visual-point-python.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | typedef boost::mpl::vector entities_t; 4 | -------------------------------------------------------------------------------- /src/feature/visual-point-projecter-python.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | typedef boost::mpl::vector entities_t; 4 | -------------------------------------------------------------------------------- /src/filters/filter-differentiator-python.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | typedef boost::mpl::vector entities_t; 4 | -------------------------------------------------------------------------------- /src/filters/madgwickahrs-python.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | typedef boost::mpl::vector entities_t; 4 | -------------------------------------------------------------------------------- /src/math/matrix-rotation.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2010, 3 | * François Bleibel, 4 | * Olivier Stasse, 5 | * 6 | * CNRS/AIST 7 | * 8 | */ 9 | 10 | #include 11 | 12 | using namespace std; 13 | using namespace dynamicgraph::sot; 14 | -------------------------------------------------------------------------------- /src/math/op-point-modifier-python.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | typedef boost::mpl::vector entities_t; 4 | -------------------------------------------------------------------------------- /src/matrix/derivator-python.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | namespace dg = ::dynamicgraph; 4 | typedef boost::mpl::vector, 5 | dg::sot::Derivator, 6 | dg::sot::Derivator, 7 | dg::sot::Derivator > 8 | entities_t; 9 | -------------------------------------------------------------------------------- /src/matrix/double-constant-python.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | typedef boost::mpl::vector entities_t; 4 | -------------------------------------------------------------------------------- /src/matrix/double-constant.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2019, 3 | * Joseph Mirabel 4 | * 5 | * LAAS-CNRS 6 | * 7 | */ 8 | 9 | #include 10 | 11 | #include 12 | #include 13 | 14 | namespace dynamicgraph { 15 | namespace sot { 16 | 17 | DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(DoubleConstant, "DoubleConstant"); 18 | 19 | DoubleConstant::DoubleConstant(const std::string &name) 20 | : Entity(name), SOUT("DoubleConstant(" + name + ")::output(double)::sout") { 21 | SOUT.setDependencyType(TimeDependency::BOOL_DEPENDENT); 22 | signalRegistration(SOUT); 23 | // 24 | // Commands 25 | 26 | // set 27 | std::string docstring = 28 | " \n" 29 | " Set value of output signal\n" 30 | " \n" 31 | " input:\n" 32 | " - a double\n" 33 | " \n"; 34 | addCommand("set", new ::dynamicgraph::command::Setter( 35 | *this, &DoubleConstant::setValue, docstring)); 36 | } 37 | 38 | void DoubleConstant::setValue(const double &inValue) { 39 | SOUT.setConstant(inValue); 40 | } 41 | 42 | } // namespace sot 43 | } // namespace dynamicgraph 44 | -------------------------------------------------------------------------------- /src/matrix/fir-filter-python.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | namespace dg = ::dynamicgraph; 4 | typedef boost::mpl::vector, 5 | dg::sot::FIRFilter, 6 | dg::sot::FIRFilter > 7 | entities_t; 8 | -------------------------------------------------------------------------------- /src/matrix/integrator-abstract-python.h: -------------------------------------------------------------------------------- 1 | typedef boost::mpl::vector<> entities_t; 2 | -------------------------------------------------------------------------------- /src/matrix/integrator-abstract.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2010, 3 | * François Bleibel, 4 | * Olivier Stasse, 5 | * 6 | * CNRS/AIST 7 | * 8 | */ 9 | 10 | #include 11 | 12 | // This ends the specialization part. 13 | // Note that on WIN32, the specialization has to be realized 14 | // before the declaration of the general model. 15 | #include 16 | 17 | #ifdef WIN32 18 | IntegratorAbstractDouble::IntegratorAbstractDouble(const std::string &name) 19 | : IntegratorAbstract(name) {} 20 | 21 | IntegratorAbstractVector::IntegratorAbstractVector(const std::string &name) 22 | : IntegratorAbstract(name) {} 23 | 24 | IntegratorAbstractVector::IntegratorAbstractVectorDouble( 25 | const std::string &name) 26 | : IntegratorAbstract(name) {} 27 | #endif 28 | -------------------------------------------------------------------------------- /src/matrix/integrator-euler-python-module-py.cc: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | namespace dg = dynamicgraph; 7 | namespace dgc = dynamicgraph::command; 8 | namespace dgs = dynamicgraph::sot; 9 | using dg::Matrix; 10 | using dg::Vector; 11 | 12 | template 13 | void exposeIntegratorEuler() { 14 | typedef dgs::IntegratorEuler IE_t; 15 | 16 | const std::string cName = dgc::Value::typeName(dgc::ValueHelper::TypeID); 17 | 18 | dg::python::exposeEntity() 19 | .add_property( 20 | "numerators", 21 | +[](const IE_t &e) { 22 | return dg::python::to_py_list(e.numCoeffs().begin(), 23 | e.numCoeffs().end()); 24 | }, 25 | +[](IE_t &e, bp::object iterable) { 26 | e.numCoeffs(dg::python::to_std_vector(iterable)); 27 | }) 28 | .add_property( 29 | "denominators", 30 | +[](const IE_t &e) { 31 | return dg::python::to_py_list(e.denomCoeffs().begin(), 32 | e.denomCoeffs().end()); 33 | }, 34 | +[](IE_t &e, bp::object iterable) { 35 | e.denomCoeffs(dg::python::to_std_vector(iterable)); 36 | }); 37 | } 38 | 39 | BOOST_PYTHON_MODULE(wrap) { 40 | bp::import("dynamic_graph"); 41 | 42 | exposeIntegratorEuler(); 43 | exposeIntegratorEuler(); 44 | exposeIntegratorEuler(); 45 | } 46 | -------------------------------------------------------------------------------- /src/matrix/integrator-euler.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2010, 3 | * François Bleibel, 4 | * Olivier Stasse, 5 | * 6 | * CNRS/AIST 7 | * 8 | */ 9 | 10 | #include 11 | #include 12 | 13 | #include "integrator-euler.t.cpp" 14 | 15 | #ifdef WIN32 16 | IntegratorEulerVectorMatrix::IntegratorEulerVectorMatrix( 17 | const std::string &name) 18 | : IntegratorEuler(name) {} 19 | std::string IntegratorEulerVectorMatrix::getTypeName(void) { 20 | return "IntegratorEulerVectorMatrix"; 21 | } 22 | 23 | IntegratorEulerVectorDouble::IntegratorEulerVectorDouble( 24 | const std::string &name) 25 | : IntegratorEuler(name) {} 26 | std::string IntegratorEulerVectorDouble::getTypeName(void) { 27 | return "IntegratorEulerVectorDouble"; 28 | } 29 | #endif // WIN32 30 | -------------------------------------------------------------------------------- /src/matrix/matrix-constant-command.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2010, 3 | * Florent Lamiraux 4 | * 5 | * CNRS/AIST 6 | * 7 | */ 8 | 9 | #ifndef MATRIX_CONSTANT_COMMAND_H 10 | #define MATRIX_CONSTANT_COMMAND_H 11 | 12 | #include 13 | #include 14 | #include 15 | 16 | #include 17 | 18 | namespace dynamicgraph { 19 | namespace sot { 20 | namespace command { 21 | namespace matrixConstant { 22 | using ::dynamicgraph::command::Command; 23 | using ::dynamicgraph::command::Value; 24 | 25 | // Command Resize 26 | class Resize : public Command { 27 | public: 28 | virtual ~Resize() {} 29 | /// Create command and store it in Entity 30 | /// \param entity instance of Entity owning this command 31 | /// \param docstring documentation of the command 32 | Resize(MatrixConstant &entity, const std::string &docstring) 33 | : Command(entity, 34 | boost::assign::list_of(Value::UNSIGNED)(Value::UNSIGNED), 35 | docstring) {} 36 | virtual Value doExecute() { 37 | MatrixConstant &mc = static_cast(owner()); 38 | std::vector values = getParameterValues(); 39 | unsigned rows = values[0].value(); 40 | unsigned cols = values[1].value(); 41 | Matrix m(Matrix::Zero(rows, cols)); 42 | mc.SOUT.setConstant(m); 43 | 44 | // return void 45 | return Value(); 46 | } 47 | }; // class Resize 48 | } // namespace matrixConstant 49 | } // namespace command 50 | } // namespace sot 51 | } // namespace dynamicgraph 52 | 53 | #endif // MATRIX_CONSTANT_COMMAND_H 54 | -------------------------------------------------------------------------------- /src/matrix/matrix-constant-python.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | typedef boost::mpl::vector entities_t; 4 | -------------------------------------------------------------------------------- /src/matrix/matrix-constant.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2010, 3 | * François Bleibel, 4 | * Olivier Stasse, 5 | * 6 | * CNRS/AIST 7 | * 8 | */ 9 | 10 | #include 11 | #include 12 | 13 | #include "../src/matrix/matrix-constant-command.h" 14 | 15 | using namespace std; 16 | using namespace dynamicgraph::sot; 17 | using namespace dynamicgraph; 18 | 19 | DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(MatrixConstant, "MatrixConstant"); 20 | 21 | /* --------------------------------------------------------------------- */ 22 | /* --- MATRIX ---------------------------------------------------------- */ 23 | /* --------------------------------------------------------------------- */ 24 | 25 | MatrixConstant::MatrixConstant(const std::string &name) 26 | : Entity(name), 27 | rows(0), 28 | cols(0), 29 | SOUT("sotMatrixConstant(" + name + ")::output(matrix)::sout") { 30 | SOUT.setDependencyType(TimeDependency::BOOL_DEPENDENT); 31 | signalRegistration(SOUT); 32 | // 33 | // Commands 34 | 35 | // Resize 36 | std::string docstring; 37 | docstring = 38 | " \n" 39 | " Resize the matrix and set it to zero.\n" 40 | " Input\n" 41 | " - unsigned int: number of lines.\n" 42 | " - unsigned int: number of columns.\n" 43 | "\n"; 44 | addCommand("resize", new command::matrixConstant::Resize(*this, docstring)); 45 | // set 46 | docstring = 47 | " \n" 48 | " Set value of output signal\n" 49 | " \n" 50 | " input:\n" 51 | " - a matrix\n" 52 | " \n"; 53 | addCommand( 54 | "set", 55 | new ::dynamicgraph::command::Setter( 56 | *this, &MatrixConstant::setValue, docstring)); 57 | } 58 | 59 | void MatrixConstant::setValue(const dynamicgraph::Matrix &inValue) { 60 | SOUT.setConstant(inValue); 61 | } 62 | -------------------------------------------------------------------------------- /src/matrix/matrix-svd.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2018, Joseph Mirabel 2 | // Authors: Joseph Mirabel (joseph.mirabel@laas.fr) 3 | 4 | #include 5 | #include 6 | 7 | namespace dynamicgraph { 8 | using Eigen::ComputeFullV; 9 | using Eigen::ComputeThinU; 10 | using Eigen::ComputeThinV; 11 | 12 | void pseudoInverse(Matrix &_inputMatrix, Matrix &_inverseMatrix, 13 | const double threshold) { 14 | SVD_t svd(_inputMatrix, ComputeThinU | ComputeThinV); 15 | SVD_t::SingularValuesType m_singularValues = svd.singularValues(); 16 | SVD_t::SingularValuesType singularValues_inv; 17 | singularValues_inv.resizeLike(m_singularValues); 18 | for (long i = 0; i < m_singularValues.size(); ++i) { 19 | if (m_singularValues(i) > threshold) 20 | singularValues_inv(i) = 1.0 / m_singularValues(i); 21 | else 22 | singularValues_inv(i) = 0; 23 | } 24 | _inverseMatrix = (svd.matrixV() * singularValues_inv.asDiagonal() * 25 | svd.matrixU().transpose()); 26 | } 27 | 28 | void dampedInverse(const SVD_t &svd, Matrix &_inverseMatrix, 29 | const double threshold) { 30 | typedef SVD_t::SingularValuesType SV_t; 31 | Eigen::ArrayWrapper sigmas(svd.singularValues()); 32 | 33 | SV_t sv_inv(sigmas / (sigmas.cwiseAbs2() + threshold * threshold)); 34 | const Matrix::Index m = sv_inv.size(); 35 | 36 | _inverseMatrix.noalias() = (svd.matrixV().leftCols(m) * sv_inv.asDiagonal() * 37 | svd.matrixU().leftCols(m).transpose()); 38 | } 39 | 40 | void dampedInverse(const Matrix &_inputMatrix, Matrix &_inverseMatrix, 41 | Matrix &Uref, Vector &Sref, Matrix &Vref, 42 | const double threshold) { 43 | SVD_t svd(_inputMatrix, ComputeThinU | ComputeThinV); 44 | 45 | dampedInverse(svd, _inverseMatrix, threshold); 46 | 47 | Uref = svd.matrixU(); 48 | Vref = svd.matrixV(); 49 | Sref = svd.singularValues(); 50 | } 51 | 52 | void dampedInverse(const Matrix &_inputMatrix, Matrix &_inverseMatrix, 53 | const double threshold) { 54 | SVD_t svd(_inputMatrix, ComputeThinU | ComputeFullV); 55 | dampedInverse(svd, _inverseMatrix, threshold); 56 | } 57 | 58 | } // namespace dynamicgraph 59 | -------------------------------------------------------------------------------- /src/matrix/vector-constant-command.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2010, 3 | * Florent Lamiraux 4 | * 5 | * CNRS/AIST 6 | * 7 | */ 8 | 9 | #ifndef VECTOR_CONSTANT_COMMAND_H 10 | #define VECTOR_CONSTANT_COMMAND_H 11 | 12 | #include 13 | #include 14 | #include 15 | 16 | #include 17 | 18 | namespace dynamicgraph { 19 | namespace sot { 20 | namespace command { 21 | namespace vectorConstant { 22 | using ::dynamicgraph::command::Command; 23 | using ::dynamicgraph::command::Value; 24 | 25 | // Command Resize 26 | class Resize : public Command { 27 | public: 28 | virtual ~Resize() {} 29 | /// Create command and store it in Entity 30 | /// \param entity instance of Entity owning this command 31 | /// \param docstring documentation of the command 32 | Resize(VectorConstant &entity, const std::string &docstring) 33 | : Command(entity, boost::assign::list_of(Value::UNSIGNED), docstring) {} 34 | virtual Value doExecute() { 35 | VectorConstant &vc = static_cast(owner()); 36 | std::vector values = getParameterValues(); 37 | unsigned size = values[0].value(); 38 | Vector m(Vector::Zero(size)); 39 | vc.SOUT.setConstant(m); 40 | 41 | // return void 42 | return Value(); 43 | } 44 | }; // class Resize 45 | } // namespace vectorConstant 46 | } // namespace command 47 | } /* namespace sot */ 48 | } /* namespace dynamicgraph */ 49 | 50 | #endif // VECTOR_CONSTANT_COMMAND_H 51 | -------------------------------------------------------------------------------- /src/matrix/vector-constant-python.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | typedef boost::mpl::vector entities_t; 4 | -------------------------------------------------------------------------------- /src/matrix/vector-constant.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2010, 3 | * François Bleibel, 4 | * Olivier Stasse, 5 | * 6 | * CNRS/AIST 7 | * 8 | */ 9 | 10 | #include 11 | #include 12 | 13 | #include "../src/matrix/vector-constant-command.h" 14 | 15 | using namespace std; 16 | using namespace dynamicgraph::sot; 17 | using namespace dynamicgraph; 18 | 19 | DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(VectorConstant, "VectorConstant"); 20 | 21 | /* --------------------------------------------------------------------- */ 22 | /* --- VECTOR ---------------------------------------------------------- */ 23 | /* --------------------------------------------------------------------- */ 24 | VectorConstant::VectorConstant(const std::string &name) 25 | : Entity(name), 26 | rows(0), 27 | SOUT("sotVectorConstant(" + name + ")::output(vector)::sout") { 28 | SOUT.setDependencyType(TimeDependency::BOOL_DEPENDENT); 29 | signalRegistration(SOUT); 30 | 31 | // 32 | // Commands 33 | // 34 | // Resize 35 | std::string docstring; 36 | docstring = 37 | " \n" 38 | " Resize the vector and set it to zero.\n" 39 | " Input\n" 40 | " unsigned size.\n" 41 | "\n"; 42 | addCommand("resize", new command::vectorConstant::Resize(*this, docstring)); 43 | // set 44 | docstring = 45 | " \n" 46 | " Set value of output signal\n" 47 | " \n" 48 | " input:\n" 49 | " - a vector\n" 50 | " \n"; 51 | addCommand( 52 | "set", 53 | new ::dynamicgraph::command::Setter( 54 | *this, &VectorConstant::setValue, docstring)); 55 | } 56 | 57 | void VectorConstant::setValue(const dynamicgraph::Vector &inValue) { 58 | SOUT.setConstant(inValue); 59 | } 60 | -------------------------------------------------------------------------------- /src/matrix/vector-to-rotation-python.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | SOT_CORE_DISABLE_WARNING_PUSH 5 | SOT_CORE_DISABLE_WARNING_DEPRECATED 6 | typedef boost::mpl::vector entities_t; 7 | SOT_CORE_DISABLE_WARNING_POP 8 | -------------------------------------------------------------------------------- /src/matrix/vector-to-rotation.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2010, 3 | * François Bleibel, 4 | * Olivier Stasse, 5 | * 6 | * CNRS/AIST 7 | * 8 | */ 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | using namespace std; 17 | using namespace dynamicgraph::sot; 18 | using namespace dynamicgraph; 19 | 20 | SOT_CORE_DISABLE_WARNING_PUSH 21 | SOT_CORE_DISABLE_WARNING_DEPRECATED 22 | DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(VectorToRotation, "VectorToRotation"); 23 | SOT_CORE_DISABLE_WARNING_POP 24 | 25 | /* --------------------------------------------------------------------- */ 26 | /* --------------------------------------------------------------------- */ 27 | /* --------------------------------------------------------------------- */ 28 | 29 | VectorToRotation::VectorToRotation(const std::string &name) 30 | : Entity(name), 31 | size(0), 32 | axes(0), 33 | SIN(NULL, "sotVectorToRotation(" + name + ")::output(vector)::sin"), 34 | SOUT(SOT_MEMBER_SIGNAL_1(VectorToRotation::computeRotation, SIN, 35 | dynamicgraph::Vector), 36 | "sotVectorToRotation(" + name + ")::output(matrixRotation)::sout") { 37 | signalRegistration(SIN << SOUT); 38 | } 39 | /* --------------------------------------------------------------------- */ 40 | /* --------------------------------------------------------------------- */ 41 | /* --------------------------------------------------------------------- */ 42 | 43 | MatrixRotation &VectorToRotation::computeRotation( 44 | const dynamicgraph::Vector &angles, MatrixRotation &res) { 45 | res.setIdentity(); 46 | MatrixRotation Ra, Rtmp; 47 | for (unsigned int i = 0; i < size; ++i) { 48 | Ra.setIdentity(); 49 | const double ca = cos(angles(i)); 50 | const double sa = sin(angles(i)); 51 | const unsigned int i_X = 0, i_Y = 1, i_Z = 2; 52 | switch (axes[i]) { 53 | case AXIS_X: { 54 | Ra(i_Y, i_Y) = ca; 55 | Ra(i_Y, i_Z) = -sa; 56 | Ra(i_Z, i_Y) = sa; 57 | Ra(i_Z, i_Z) = ca; 58 | break; 59 | } 60 | case AXIS_Y: { 61 | Ra(i_Z, i_Z) = ca; 62 | Ra(i_Z, i_X) = -sa; 63 | Ra(i_X, i_Z) = sa; 64 | Ra(i_X, i_X) = ca; 65 | break; 66 | } 67 | case AXIS_Z: { 68 | Ra(i_X, i_X) = ca; 69 | Ra(i_X, i_Y) = -sa; 70 | Ra(i_Y, i_X) = sa; 71 | Ra(i_Y, i_Y) = ca; 72 | break; 73 | } 74 | } 75 | 76 | sotDEBUG(15) << "R" << i << " = " << Ra; 77 | Rtmp = res * Ra; 78 | res = Rtmp; 79 | } 80 | 81 | return res; 82 | } 83 | -------------------------------------------------------------------------------- /src/sot/memory-task-sot.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2010, 3 | * François Bleibel, 4 | * Olivier Stasse, 5 | * 6 | * CNRS/AIST 7 | * 8 | */ 9 | 10 | #include 11 | #include 12 | #include 13 | using namespace dynamicgraph::sot; 14 | using namespace dynamicgraph; 15 | 16 | MemoryTaskSOT::MemoryTaskSOT(const Matrix::Index nJ, const Matrix::Index mJ) 17 | : kernel(NULL, 0, 0) { 18 | initMemory(nJ, mJ); 19 | } 20 | 21 | void MemoryTaskSOT::initMemory(const Matrix::Index nJ, const Matrix::Index mJ) { 22 | err.resize(nJ); 23 | tmpTask.resize(nJ); 24 | tmpVar.resize(mJ); 25 | tmpControl.resize(mJ); 26 | Jt.resize(nJ, mJ); 27 | 28 | JK.resize(nJ, mJ); 29 | 30 | svd = SVD_t(nJ, mJ, Eigen::ComputeThinU | Eigen::ComputeFullV); 31 | // If the constraint is well conditioned, the kernel can be pre-allocated. 32 | if (mJ > nJ) kernelMem.resize(mJ - nJ, mJ); 33 | 34 | JK.setZero(); 35 | Jt.setZero(); 36 | } 37 | 38 | void MemoryTaskSOT::display(std::ostream& /*os*/) const {} // TODO 39 | -------------------------------------------------------------------------------- /src/sot/sot-python.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | typedef boost::mpl::vector entities_t; 4 | -------------------------------------------------------------------------------- /src/task/gain-adaptive-python.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | typedef boost::mpl::vector entities_t; 4 | -------------------------------------------------------------------------------- /src/task/gain-hyperbolic-python.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | typedef boost::mpl::vector entities_t; 4 | -------------------------------------------------------------------------------- /src/task/task-abstract.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2010, 3 | * François Bleibel, 4 | * Olivier Stasse, 5 | * 6 | * CNRS/AIST 7 | * 8 | */ 9 | 10 | /* --------------------------------------------------------------------- */ 11 | /* --- INCLUDE --------------------------------------------------------- */ 12 | /* --------------------------------------------------------------------- */ 13 | 14 | /* SOT */ 15 | #include 16 | #include 17 | 18 | using namespace dynamicgraph::sot; 19 | using namespace dynamicgraph; 20 | 21 | /* --------------------------------------------------------------------- */ 22 | /* --- CLASS ----------------------------------------------------------- */ 23 | /* --------------------------------------------------------------------- */ 24 | 25 | TaskAbstract::TaskAbstract(const std::string &n) 26 | : Entity(n), 27 | memoryInternal(NULL), 28 | taskSOUT("sotTaskAbstract(" + n + ")::output(vector)::task"), 29 | jacobianSOUT("sotTaskAbstract(" + n + ")::output(matrix)::jacobian") { 30 | taskRegistration(); 31 | signalRegistration(taskSOUT << jacobianSOUT); 32 | } 33 | 34 | void TaskAbstract::taskRegistration(void) { 35 | PoolStorage::getInstance()->registerTask(name, this); 36 | } 37 | -------------------------------------------------------------------------------- /src/task/task-command.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2010, 3 | * Florent Lamiraux 4 | * 5 | * CNRS 6 | * 7 | */ 8 | 9 | #ifndef TASK_COMMAND_H 10 | #define TASK_COMMAND_H 11 | 12 | #include 13 | #include 14 | #include 15 | 16 | #include 17 | 18 | namespace dynamicgraph { 19 | namespace sot { 20 | namespace command { 21 | namespace task { 22 | using ::dynamicgraph::command::Command; 23 | using ::dynamicgraph::command::Value; 24 | 25 | // Command ListFeatures 26 | class ListFeatures : public Command { 27 | public: 28 | virtual ~ListFeatures() {} 29 | /// Create command and store it in Entity 30 | /// \param entity instance of Entity owning this command 31 | /// \param docstring documentation of the command 32 | ListFeatures(Task &entity, const std::string &docstring) 33 | : Command(entity, std::vector(), docstring) {} 34 | virtual Value doExecute() { 35 | typedef Task::FeatureList_t FeatureList_t; 36 | Task &task = static_cast(owner()); 37 | const FeatureList_t &fl = task.getFeatureList(); 38 | std::string result("["); 39 | for (FeatureList_t::const_iterator it = fl.begin(); it != fl.end(); it++) { 40 | result += "'" + (*it)->getName() + "',"; 41 | } 42 | result += "]"; 43 | return Value(result); 44 | } 45 | }; // class ListFeatures 46 | } // namespace task 47 | } // namespace command 48 | } /* namespace sot */ 49 | } /* namespace dynamicgraph */ 50 | 51 | #endif // TASK_COMMAND_H 52 | -------------------------------------------------------------------------------- /src/task/task-conti-python.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | typedef boost::mpl::vector entities_t; 4 | -------------------------------------------------------------------------------- /src/task/task-pd-python.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | typedef boost::mpl::vector entities_t; 4 | -------------------------------------------------------------------------------- /src/task/task-python.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | typedef boost::mpl::vector entities_t; 4 | -------------------------------------------------------------------------------- /src/task/task-unilateral-python.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | typedef boost::mpl::vector entities_t; 4 | -------------------------------------------------------------------------------- /src/tools/binary-int-to-uint-python.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | typedef boost::mpl::vector entities_t; 4 | -------------------------------------------------------------------------------- /src/tools/binary-int-to-uint.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2010, 3 | * François Bleibel, 4 | * Olivier Stasse, 5 | * 6 | * CNRS/AIST 7 | * 8 | */ 9 | 10 | /* --- SOT --- */ 11 | #include 12 | #include 13 | #include 14 | #include 15 | using namespace std; 16 | 17 | #include 18 | 19 | using namespace dynamicgraph::sot; 20 | using namespace dynamicgraph; 21 | 22 | DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(BinaryIntToUint, "BinaryIntToUint"); 23 | 24 | /* --------------------------------------------------------------------- */ 25 | /* --- CLASS ----------------------------------------------------------- */ 26 | /* --------------------------------------------------------------------- */ 27 | 28 | BinaryIntToUint::BinaryIntToUint(const string &fname) 29 | : Entity(fname), 30 | binaryIntSIN(NULL, "BinaryIntToUint(" + name + ")::input(int)::sin"), 31 | binaryUintSOUT( 32 | boost::bind(&BinaryIntToUint::computeOutput, this, _1, _2), 33 | binaryIntSIN, 34 | "BinaryIntToUint(" + name + ")::output(unsigned int)::sout") { 35 | signalRegistration(binaryIntSIN << binaryUintSOUT); 36 | } 37 | 38 | /* --------------------------------------------------------------------- */ 39 | /* --------------------------------------------------------------------- */ 40 | /* --------------------------------------------------------------------- */ 41 | 42 | unsigned &BinaryIntToUint::computeOutput(unsigned &res, int time) { 43 | sotDEBUGIN(15); 44 | 45 | int in = binaryIntSIN.access(time); 46 | if (in < 0) { 47 | res = 0; 48 | } else { 49 | res = 1; 50 | } 51 | 52 | sotDEBUGOUT(15); 53 | return res; 54 | } 55 | 56 | void BinaryIntToUint::display(std::ostream &os) const { 57 | os << "BinaryIntToUint <" << name << "> TODO..." << endl; 58 | } 59 | -------------------------------------------------------------------------------- /src/tools/clamp-workspace-python.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | typedef boost::mpl::vector entities_t; 4 | -------------------------------------------------------------------------------- /src/tools/com-freezer-python.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | typedef boost::mpl::vector entities_t; 4 | -------------------------------------------------------------------------------- /src/tools/com-freezer.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2010, 3 | * François Bleibel, 4 | * Olivier Stasse, 5 | * 6 | * CNRS/AIST 7 | * 8 | */ 9 | 10 | #include 11 | 12 | #include 13 | #include 14 | 15 | using namespace dynamicgraph; 16 | using namespace dynamicgraph::sot; 17 | 18 | DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(CoMFreezer, "CoMFreezer"); 19 | 20 | CoMFreezer::CoMFreezer(const std::string &name) 21 | : Entity(name), 22 | m_lastCoM(3), 23 | m_previousPGInProcess(false), 24 | m_lastStopTime(-1) 25 | 26 | , 27 | CoMRefSIN(NULL, "CoMFreezer(" + name + ")::input(vector)::CoMRef"), 28 | PGInProcessSIN(NULL, 29 | "CoMFreezer(" + name + ")::input(bool)::PGInProcess"), 30 | freezedCoMSOUT(boost::bind(&CoMFreezer::computeFreezedCoM, this, _1, _2), 31 | CoMRefSIN << PGInProcessSIN, 32 | "CoMFreezer(" + name + ")::output(vector)::freezedCoM") { 33 | sotDEBUGIN(5); 34 | 35 | signalRegistration(CoMRefSIN << PGInProcessSIN << freezedCoMSOUT); 36 | 37 | sotDEBUGOUT(5); 38 | } 39 | 40 | CoMFreezer::~CoMFreezer(void) { 41 | sotDEBUGIN(5); 42 | sotDEBUGOUT(5); 43 | return; 44 | } 45 | 46 | dynamicgraph::Vector &CoMFreezer::computeFreezedCoM( 47 | dynamicgraph::Vector &freezedCoM, const int &time) { 48 | sotDEBUGIN(15); 49 | 50 | unsigned PGInProcess = PGInProcessSIN(time); 51 | if (PGInProcess) /* CoM unfreezed */ 52 | { 53 | m_lastCoM = CoMRefSIN(time); 54 | m_previousPGInProcess = (PGInProcess == 0); 55 | } else { 56 | if (m_previousPGInProcess) /* pg.inprocess switch from 1 to 0 */ 57 | { 58 | m_lastStopTime = time; 59 | m_lastCoM = CoMRefSIN(time); 60 | m_previousPGInProcess = (PGInProcess == 0); 61 | } else if (time < m_lastStopTime + 200) /* keep updating for 1s */ 62 | { 63 | m_lastCoM = CoMRefSIN(time); 64 | } 65 | } 66 | 67 | freezedCoM = m_lastCoM; 68 | 69 | sotDEBUGOUT(15); 70 | 71 | if (m_lastStopTime < 0) { 72 | m_lastCoM = CoMRefSIN(time); 73 | m_lastStopTime = time; 74 | freezedCoM = m_lastCoM; 75 | return freezedCoM; 76 | } 77 | 78 | return m_lastCoM; 79 | } 80 | 81 | void CoMFreezer::display(std::ostream &os) const { 82 | os << "CoMFreezer " << getName() << "." << std::endl; 83 | } 84 | -------------------------------------------------------------------------------- /src/tools/event-python.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | typedef boost::mpl::vector entities_t; 4 | -------------------------------------------------------------------------------- /src/tools/event.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2017, Joseph Mirabel 2 | // Authors: Joseph Mirabel (joseph.mirabel@laas.fr) 3 | 4 | #include 5 | 6 | #include 7 | 8 | namespace dynamicgraph { 9 | namespace sot { 10 | 11 | bool &Event::check(bool &ret, const int &time) { 12 | const bool &val = conditionSIN(time); 13 | ret = (val != lastVal_); 14 | bool up = (!lastVal_ && val); 15 | if (up) { 16 | timeSinceUp_ = 0; 17 | } else if (val) { 18 | ++timeSinceUp_; 19 | } 20 | // If repetition is activated, trigger again after given number of iterations 21 | bool trigger = up || (repeatAfterNIterations_ > 0 && 22 | timeSinceUp_ >= repeatAfterNIterations_); 23 | if (ret) { 24 | lastVal_ = val; 25 | } 26 | if (trigger) { 27 | for (Triggers_t::const_iterator _s = triggers.begin(); _s != triggers.end(); 28 | ++_s) 29 | (*_s)->recompute(time); 30 | timeSinceUp_ = 0; 31 | } 32 | return ret; 33 | } 34 | 35 | DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(Event, "Event"); 36 | } // namespace sot 37 | } // namespace dynamicgraph 38 | -------------------------------------------------------------------------------- /src/tools/exp-moving-avg-python.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | typedef boost::mpl::vector entities_t; 4 | -------------------------------------------------------------------------------- /src/tools/gradient-ascent-python.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | typedef boost::mpl::vector entities_t; 4 | -------------------------------------------------------------------------------- /src/tools/gradient-ascent.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2018, 3 | * Julian Viereck 4 | * 5 | * CNRS/AIST 6 | * 7 | */ 8 | 9 | #include 10 | #include 11 | 12 | #include 13 | #include 14 | #include 15 | 16 | namespace dg = ::dynamicgraph; 17 | 18 | /* ---------------------------------------------------------------------------*/ 19 | /* ------- GENERIC HELPERS -------------------------------------------------- */ 20 | /* ---------------------------------------------------------------------------*/ 21 | 22 | namespace dynamicgraph { 23 | namespace sot { 24 | 25 | DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(GradientAscent, "GradientAscent"); 26 | 27 | /* --------------------------------------------------------------------- */ 28 | /* --- CLASS ----------------------------------------------------------- */ 29 | /* --------------------------------------------------------------------- */ 30 | 31 | GradientAscent::GradientAscent(const std::string &n) 32 | : Entity(n), 33 | gradientSIN(NULL, "GradientAscent(" + n + ")::input(vector)::gradient"), 34 | learningRateSIN(NULL, 35 | "GradientAscent(" + n + ")::input(double)::learningRate"), 36 | refresherSINTERN("GradientAscent(" + n + ")::intern(dummy)::refresher"), 37 | valueSOUT(boost::bind(&GradientAscent::update, this, _1, _2), 38 | gradientSIN << refresherSINTERN, 39 | "GradientAscent(" + n + ")::output(vector)::value"), 40 | init(false) { 41 | // Register signals into the entity. 42 | signalRegistration(gradientSIN << learningRateSIN << valueSOUT); 43 | refresherSINTERN.setDependencyType(TimeDependency::ALWAYS_READY); 44 | } 45 | 46 | GradientAscent::~GradientAscent() {} 47 | 48 | /* --- COMPUTE ----------------------------------------------------------- */ 49 | /* --- COMPUTE ----------------------------------------------------------- */ 50 | /* --- COMPUTE ----------------------------------------------------------- */ 51 | 52 | dynamicgraph::Vector &GradientAscent::update(dynamicgraph::Vector &res, 53 | const int &inTime) { 54 | const dynamicgraph::Vector &gradient = gradientSIN(inTime); 55 | const double &learningRate = learningRateSIN(inTime); 56 | 57 | if (init == false) { 58 | init = true; 59 | value = gradient; 60 | value.setZero(); 61 | res.resize(value.size()); 62 | } 63 | 64 | value += learningRate * gradient; 65 | res = value; 66 | return res; 67 | } 68 | 69 | } /* namespace sot */ 70 | } /* namespace dynamicgraph */ 71 | -------------------------------------------------------------------------------- /src/tools/gripper-control-python.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | typedef boost::mpl::vector entities_t; 4 | -------------------------------------------------------------------------------- /src/tools/joint-limitator-python.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | typedef boost::mpl::vector entities_t; 4 | -------------------------------------------------------------------------------- /src/tools/joint-trajectory-command.hh: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright 2013, 3 | // Olivier Stasse 4 | // 5 | // CNRS 6 | // 7 | 8 | #ifndef JOINT_TRAJECTORY_COMMAND_H_ 9 | #define JOINT_TRAJECTORY_COMMAND_H_ 10 | 11 | #include 12 | #include 13 | #include 14 | 15 | #include 16 | #include 17 | 18 | namespace dynamicgraph { 19 | namespace sot { 20 | namespace command { 21 | namespace classSot { 22 | using ::dynamicgraph::command::Command; 23 | using ::dynamicgraph::command::Value; 24 | 25 | class SetInitTrajectory : public Command { 26 | public: 27 | virtual ~SetInitTrajectory() {} 28 | 29 | /// Set the initial trajectory. 30 | SetInitTrajectory(SotJointTrajectoryEntity &entity, 31 | const std::string &docstring) 32 | : Command(entity, boost::assign::list_of(Value::STRING), docstring) {} 33 | 34 | virtual Value doExecute() { 35 | // return void 36 | return Value(); 37 | } 38 | }; 39 | } // namespace classSot 40 | } // namespace command 41 | } // namespace sot 42 | } // namespace dynamicgraph 43 | #endif /* JOINT_TRAJECTORY_COMMAND_H_ */ 44 | -------------------------------------------------------------------------------- /src/tools/joint-trajectory-entity-python.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | typedef boost::mpl::vector 4 | entities_t; 5 | -------------------------------------------------------------------------------- /src/tools/kalman-doc.tex: -------------------------------------------------------------------------------- 1 | \begin{eqnarray*} 2 | x_{k} &=& f (x_{k-1}, u_{k-1}) + w_{k-1} \\ 3 | y_{k} &=& h (x_{k}) + v_{k} 4 | \end{eqnarray*} 5 | 6 | \begin{eqnarray*} 7 | \hat{x}_{k|k-1} &=& f (\hat{x}_{k-1|k-1}, u_{k-1})\\ 8 | P_{k|k-1} &=& F_{k-1}\ P_{k-1|k-1} F_{k-1}^T + Q 9 | \end{eqnarray*} 10 | 11 | \begin{eqnarray*} 12 | z_{k} &=& y_{k} - H\ \hat{x}_{k|k-1} \\ 13 | S_{k} &=& H_{k}\ P_{k|k-1} H_{k}^T + R \\ 14 | K_{k} &=& P_{k|k-1} H_{k}^T S_{k}^{-1} \\ 15 | \hat{x}_{k|k} &=& \hat{x}_{k|k-1} + K_{k}\ z_{k} \\ 16 | P_{k|k} &=& (I - K_{k} H_{k}) P_{k|k-1} 17 | \end{eqnarray*} 18 | 19 | $$ 20 | F_{k-1} = \frac{\partial f}{\partial x} (\hat{x}_{k-1|k-1}, u_{k-1}) 21 | $$ 22 | $$ 23 | H_{k} = \frac{\partial h}{\partial x} (\hat{x}_{k-1|k-1}) 24 | $$ 25 | -------------------------------------------------------------------------------- /src/tools/kalman-python.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | typedef boost::mpl::vector entities_t; 4 | -------------------------------------------------------------------------------- /src/tools/latch-python.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | typedef boost::mpl::vector entities_t; 4 | -------------------------------------------------------------------------------- /src/tools/latch.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2017-2018, Rohan Budhiraja, Joseph Mirabel, CNRS 3 | * 4 | * CNRS/AIST 5 | * 6 | */ 7 | 8 | #include 9 | #include 10 | 11 | namespace dynamicgraph { 12 | namespace sot { 13 | DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(Latch, "Latch"); 14 | } // namespace sot 15 | } // namespace dynamicgraph 16 | -------------------------------------------------------------------------------- /src/tools/mailbox-vector-python.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | typedef boost::mpl::vector entities_t; 4 | -------------------------------------------------------------------------------- /src/tools/mailbox-vector.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2010, 3 | * François Bleibel, 4 | * Olivier Stasse, 5 | * 6 | * CNRS/AIST 7 | * 8 | */ 9 | 10 | /* --- SOT PLUGIN --- */ 11 | #include 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | using namespace dynamicgraph::sot; 19 | using namespace dynamicgraph; 20 | 21 | // Explicit template specialization 22 | #ifdef WIN32 23 | MailboxVector::MailboxVector(const std::string &name) 24 | : Mailbox(name) {} 25 | #else 26 | MAILBOX_TEMPLATE_SPE(dynamicgraph::Vector) 27 | #endif 28 | 29 | template <> 30 | DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(MailboxVector, "Mailbox"); 31 | -------------------------------------------------------------------------------- /src/tools/motion-period-python.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | typedef boost::mpl::vector entities_t; 4 | -------------------------------------------------------------------------------- /src/tools/neck-limitation-python.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | typedef boost::mpl::vector entities_t; 4 | -------------------------------------------------------------------------------- /src/tools/parameter-server-python.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | typedef boost::mpl::vector entities_t; 4 | -------------------------------------------------------------------------------- /src/tools/periodic-call-entity-python.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | typedef boost::mpl::vector entities_t; 4 | -------------------------------------------------------------------------------- /src/tools/periodic-call-entity.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2010, 3 | * François Bleibel, 4 | * Olivier Stasse, 5 | * 6 | * CNRS/AIST 7 | * 8 | */ 9 | 10 | /* --------------------------------------------------------------------- */ 11 | /* --- INCLUDE --------------------------------------------------------- */ 12 | /* --------------------------------------------------------------------- */ 13 | 14 | /* --- SOT --- */ 15 | #include 16 | 17 | #include 18 | #include 19 | #include 20 | 21 | using namespace std; 22 | using namespace dynamicgraph::sot; 23 | 24 | DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(PeriodicCallEntity, "PeriodicCallEntity"); 25 | 26 | /* --------------------------------------------------------------------- */ 27 | /* --- CLASS ----------------------------------------------------------- */ 28 | /* --------------------------------------------------------------------- */ 29 | 30 | PeriodicCallEntity::PeriodicCallEntity(const string &fName) 31 | : Entity(fName), 32 | PeriodicCall(), 33 | triger("Tracer(" + fName + ")::triger"), 34 | trigerOnce("Tracer(" + fName + ")::trigerOnce") { 35 | signalRegistration(triger << trigerOnce); 36 | 37 | triger.setFunction( 38 | boost::bind(&PeriodicCallEntity::trigerCall, this, _1, _2)); 39 | trigerOnce.setFunction( 40 | boost::bind(&PeriodicCallEntity::trigerOnceCall, this, _1, _2)); 41 | } 42 | 43 | int &PeriodicCallEntity::trigerCall(int &dummy, const int &time) { 44 | run(time); 45 | return dummy; 46 | } 47 | int &PeriodicCallEntity::trigerOnceCall(int &dummy, const int &time) { 48 | run(time); 49 | clear(); 50 | return dummy; 51 | } 52 | 53 | /* --------------------------------------------------------------------- */ 54 | /* --------------------------------------------------------------------- */ 55 | /* --------------------------------------------------------------------- */ 56 | 57 | void PeriodicCallEntity::display(std::ostream &os) const { 58 | os << "PeriodicCallEntity <" << name << "> "; 59 | PeriodicCall::display(os); 60 | } 61 | -------------------------------------------------------------------------------- /src/tools/robot-simu-python.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | typedef boost::mpl::vector entities_t; 4 | -------------------------------------------------------------------------------- /src/tools/robot-simu.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2010, 3 | * Nicolas Mansard, Olivier Stasse, François Bleibel, Florent Lamiraux 4 | * 5 | * CNRS 6 | * 7 | */ 8 | 9 | #include "sot/core/robot-simu.hh" 10 | 11 | #include 12 | #include 13 | 14 | namespace dynamicgraph { 15 | namespace sot { 16 | DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(RobotSimu, "RobotSimu"); 17 | 18 | RobotSimu::RobotSimu(const std::string &inName) : Device(inName) { 19 | using namespace dynamicgraph::command; 20 | std::string docstring; 21 | /* Command increment. */ 22 | docstring = 23 | "\n" 24 | " Integrate dynamics for time step provided as input\n" 25 | "\n" 26 | " take one floating point number as input\n" 27 | "\n"; 28 | addCommand("increment", command::makeCommandVoid1( 29 | (Device &)*this, &Device::increment, docstring)); 30 | 31 | /* Set Time step. */ 32 | docstring = 33 | "\n" 34 | " Set the time step provided\n" 35 | "\n" 36 | " take one floating point number as input\n" 37 | "\n"; 38 | addCommand("setTimeStep", 39 | makeDirectSetter(*this, &this->timestep_, docstring)); 40 | } 41 | } // namespace sot 42 | } // namespace dynamicgraph 43 | -------------------------------------------------------------------------------- /src/tools/sequencer-python.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | typedef boost::mpl::vector entities_t; 4 | -------------------------------------------------------------------------------- /src/tools/smooth-reach-python.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | typedef boost::mpl::vector entities_t; 4 | -------------------------------------------------------------------------------- /src/tools/switch-python-module-py.cc: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "dynamic-graph/python/module.hh" 4 | 5 | namespace dg = dynamicgraph; 6 | typedef bp::return_value_policy 7 | reference_existing_object; 8 | 9 | template 10 | void exposeSwitch() { 11 | typedef dg::sot::Switch E_t; 12 | typedef typename E_t::Base B_t; 13 | dg::python::exposeEntity, 14 | dg::python::AddCommands>() 15 | .def_readonly("sout", &E_t::SOUT) 16 | .def("sin", &B_t::getSignalIn, reference_existing_object()) 17 | .add_property("n_sin", &B_t::getSignalNumber, &B_t::setSignalNumber, 18 | "the number of input signal.") 19 | .def_readonly("selection", &E_t::selectionSIN) 20 | .def_readonly("boolSelection", &E_t::boolSelectionSIN) 21 | 22 | .def("setSignalNumber", &B_t::setSignalNumber, 23 | "set the number of input signal.", bp::arg("size")) 24 | .def("getSignalNumber", &B_t::getSignalNumber, 25 | "get the number of input signal.", bp::arg("size")); 26 | } 27 | 28 | BOOST_PYTHON_MODULE(wrap) { 29 | exposeSwitch(); 30 | exposeSwitch(); 31 | exposeSwitch(); 32 | } 33 | -------------------------------------------------------------------------------- /src/tools/switch.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2017, Joseph Mirabel 2 | // Authors: Joseph Mirabel (joseph.mirabel@laas.fr) 3 | 4 | #include 5 | 6 | #include 7 | 8 | #include "type-name-helper.hh" 9 | 10 | namespace dynamicgraph { 11 | namespace sot { 12 | template 13 | std::string VariadicAbstract::getTypeInName(void) { 14 | return TypeNameHelper::typeName(); 15 | } 16 | template 17 | std::string VariadicAbstract::getTypeOutName(void) { 18 | return TypeNameHelper::typeName(); 19 | } 20 | 21 | template class VariadicAbstract; 22 | 23 | typedef Switch SwitchVector; 24 | template <> 25 | DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(SwitchVector, "SwitchVector"); 26 | 27 | template class VariadicAbstract; 28 | 29 | typedef Switch SwitchBool; 30 | template <> 31 | DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(SwitchBool, "SwitchBoolean"); 32 | 33 | template class VariadicAbstract; 34 | 35 | typedef Switch SwitchMatrixHomogeneous; 36 | template <> 37 | DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(SwitchMatrixHomogeneous, 38 | "SwitchMatrixHomogeneous"); 39 | } // namespace sot 40 | } // namespace dynamicgraph 41 | -------------------------------------------------------------------------------- /src/tools/time-stamp-python.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | typedef boost::mpl::vector entities_t; 4 | -------------------------------------------------------------------------------- /src/tools/timer-python.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | typedef boost::mpl::vector< 5 | Timer, Timer, 6 | Timer, Timer > 7 | entities_t; 8 | -------------------------------------------------------------------------------- /src/tools/type-name-helper.hh: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2010, 3 | * François Bleibel, 4 | * Olivier Stasse, 5 | * Nicolas Mansard 6 | * Joseph Mirabel 7 | * 8 | * CNRS/AIST 9 | * 10 | */ 11 | 12 | #include 13 | 14 | namespace dynamicgraph { 15 | namespace sot { 16 | template 17 | struct TypeNameHelper { 18 | static inline std::string typeName(); 19 | }; 20 | template 21 | inline std::string TypeNameHelper::typeName() { 22 | return "unspecified"; 23 | } 24 | 25 | #define ADD_KNOWN_TYPE(typeid) \ 26 | template <> \ 27 | inline std::string TypeNameHelper::typeName() { \ 28 | return #typeid; \ 29 | } 30 | 31 | ADD_KNOWN_TYPE(bool) 32 | ADD_KNOWN_TYPE(double) 33 | ADD_KNOWN_TYPE(Vector) 34 | ADD_KNOWN_TYPE(Matrix) 35 | ADD_KNOWN_TYPE(MatrixRotation) 36 | ADD_KNOWN_TYPE(MatrixTwist) 37 | ADD_KNOWN_TYPE(MatrixHomogeneous) 38 | ADD_KNOWN_TYPE(VectorQuaternion) 39 | ADD_KNOWN_TYPE(VectorRollPitchYaw) 40 | 41 | #undef ADD_KNOWN_TYPE 42 | } /* namespace sot */ 43 | } /* namespace dynamicgraph */ 44 | -------------------------------------------------------------------------------- /src/tools/utils-windows.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2010, 3 | * François Bleibel, 4 | * Olivier Stasse, 5 | * 6 | * CNRS/AIST 7 | * 8 | */ 9 | 10 | #ifdef WIN32 11 | #include < Windows.h > 12 | #include 13 | #if defined(_MSC_VER) || defined(_MSC_EXTENSIONS) 14 | #define DELTA_EPOCH_IN_MICROSECS 11644473600000000Ui64 15 | #else 16 | #define DELTA_EPOCH_IN_MICROSECS 11644473600000000ULL 17 | #endif 18 | 19 | int gettimeofday(struct timeval *tv, struct timezone *tz) { 20 | FILETIME ft; 21 | unsigned __int64 tmpres = 0; 22 | static int tzflag; 23 | 24 | if (NULL != tv) { 25 | GetSystemTimeAsFileTime(&ft); 26 | 27 | tmpres |= ft.dwHighDateTime; 28 | tmpres <<= 32; 29 | tmpres |= ft.dwLowDateTime; 30 | 31 | /*converting file time to unix epoch*/ 32 | tmpres /= 10; /*convert into microseconds*/ 33 | tmpres -= DELTA_EPOCH_IN_MICROSECS; 34 | tv->tv_sec = (long)(tmpres / 1000000UL); 35 | tv->tv_usec = (long)(tmpres % 1000000UL); 36 | } 37 | 38 | if (NULL != tz) { 39 | if (!tzflag) { 40 | _tzset(); 41 | tzflag++; 42 | } 43 | tz->tz_minuteswest = _timezone / 60; 44 | tz->tz_dsttime = _daylight; 45 | } 46 | 47 | return 0; 48 | } 49 | 50 | #endif /*WIN32*/ 51 | -------------------------------------------------------------------------------- /src/traces/reader-python.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | typedef boost::mpl::vector entities_t; 4 | -------------------------------------------------------------------------------- /tests/control/test_control_admittance.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2019, 3 | * Noëlie Ramuzat, 4 | * 5 | * 6 | */ 7 | 8 | #include 9 | #include 10 | 11 | #ifndef WIN32 12 | #include 13 | #endif 14 | 15 | using namespace std; 16 | 17 | #include 18 | #include 19 | 20 | #include 21 | #include 22 | 23 | using namespace dynamicgraph; 24 | using namespace dynamicgraph::sot; 25 | 26 | #define BOOST_TEST_MODULE debug - control - admittance 27 | 28 | #include 29 | #include 30 | 31 | BOOST_AUTO_TEST_CASE(control_admittance) { 32 | sot::core::AdmittanceControlOpPoint *aControlAdm = 33 | new sot::core::AdmittanceControlOpPoint("acontrol_admittance"); 34 | 35 | std::istringstream Kp("[6](10.0,10.0,10.0,10.0,10.0,10.0)"); 36 | std::istringstream Kd("[6](0.0,0.0,0.0,0.0,0.0,0.0)"); 37 | aControlAdm->m_KpSIN.set(Kp); 38 | aControlAdm->m_KdSIN.set(Kd); 39 | std::istringstream dqSaturation("[6](10.0,10.0,10.0,10.0,10.0,10.0)"); 40 | aControlAdm->m_dqSaturationSIN.set(dqSaturation); 41 | std::istringstream w_forceDes("[6](100.0,0.0,0.0,0.0,0.0,0.0)"); 42 | aControlAdm->m_w_forceDesSIN.set(w_forceDes); 43 | std::istringstream force("[6](10.0,0.0,10.0,0.0,0.0,0.0)"); 44 | aControlAdm->m_forceSIN.set(force); 45 | MatrixHomogeneous opPose; 46 | opPose.translation() << 0.3, 0.0, 0.0; 47 | opPose.linear() << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0; 48 | aControlAdm->m_opPoseSIN = opPose; 49 | MatrixHomogeneous sensorPose; 50 | sensorPose.translation() << 0.3, 0.0, 0.0; 51 | sensorPose.linear() << 0.0, 0.0, -1.0, 0.0, 1.0, 0.0, 1.0, 0.0, 0.0; 52 | aControlAdm->m_sensorPoseSIN = sensorPose; 53 | aControlAdm->init(0.001); 54 | 55 | aControlAdm->m_dqSOUT.recompute(0); 56 | { 57 | dynamicgraph::Vector expected(6); 58 | expected << 1.1, 0.0, -0.109, 0.0, 0.03, 0.0; 59 | BOOST_CHECK(aControlAdm->m_dqSOUT(0).isApprox(expected)); 60 | } 61 | } 62 | -------------------------------------------------------------------------------- /tests/control/test_control_pd.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2019, 3 | * Olivier Stasse, 4 | * 5 | * CNRS/AIST 6 | * 7 | */ 8 | 9 | #include 10 | #include 11 | 12 | #ifndef WIN32 13 | #include 14 | #endif 15 | 16 | using namespace std; 17 | 18 | #include 19 | #include 20 | 21 | #include 22 | #include 23 | 24 | using namespace dynamicgraph; 25 | using namespace dynamicgraph::sot; 26 | 27 | #define BOOST_TEST_MODULE debug - control - pd 28 | 29 | #include 30 | #include 31 | 32 | BOOST_AUTO_TEST_CASE(control_pd) { 33 | sot::ControlPD *aControlPD = new ControlPD("acontrol_pd"); 34 | aControlPD->init(0.001); 35 | std::istringstream Kpiss("[5](10.0,20.0,30.0,40.0,50.0)"); 36 | std::istringstream Kdiss("[5](0.10,0.20,0.30,0.40,0.50)"); 37 | aControlPD->KpSIN.set(Kpiss); 38 | aControlPD->KdSIN.set(Kdiss); 39 | std::istringstream posiss("[5](1.0,1.0,1.0,1.0,1.0)"); 40 | aControlPD->positionSIN.set(posiss); 41 | std::istringstream dposiss("[5](3.0,3.1,3.2,3.3,3.4)"); 42 | aControlPD->desiredpositionSIN.set(dposiss); 43 | std::istringstream veliss("[5](0.0,0.0,0.0,0.0,0.0)"); 44 | aControlPD->velocitySIN.set(veliss); 45 | std::istringstream dveliss("[5](1.5,1.4,1.3,1.2,1.1)"); 46 | aControlPD->desiredvelocitySIN.set(dveliss); 47 | 48 | aControlPD->controlSOUT.recompute(0); 49 | aControlPD->positionErrorSOUT.recompute(0); 50 | aControlPD->velocityErrorSOUT.recompute(0); 51 | { 52 | boost::test_tools::output_test_stream output; 53 | aControlPD->controlSOUT.get(output); 54 | BOOST_CHECK(output.is_equal("20.15 42.28 66.39 92.48 120.55")); 55 | } 56 | { 57 | boost::test_tools::output_test_stream output; 58 | aControlPD->positionErrorSOUT.get(output); 59 | BOOST_CHECK(output.is_equal("2 2.1 2.2 2.3 2.4")); 60 | } 61 | { 62 | boost::test_tools::output_test_stream output; 63 | aControlPD->velocityErrorSOUT.get(output); 64 | BOOST_CHECK(output.is_equal("1.5 1.4 1.3 1.2 1.1")); 65 | } 66 | } 67 | -------------------------------------------------------------------------------- /tests/dummy.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2010, 3 | * François Bleibel, 4 | * Olivier Stasse, 5 | * 6 | * CNRS/AIST 7 | * 8 | */ 9 | 10 | #include 11 | #include 12 | 13 | int main(int, char **) { 14 | dynamicgraph::sot::sotDEBUGFLOW.openFile(); 15 | dynamicgraph::sot::sotDEBUGFLOW.trace("test test test"); 16 | 17 | std::cout << "It works!" << std::endl; 18 | return 0; 19 | } 20 | -------------------------------------------------------------------------------- /tests/filters/test_filter_differentiator.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2019, 3 | * Olivier Stasse, 4 | * 5 | * CNRS/AIST 6 | * 7 | */ 8 | 9 | #include 10 | #include 11 | 12 | #ifndef WIN32 13 | #include 14 | #endif 15 | 16 | using namespace std; 17 | 18 | #include 19 | #include 20 | 21 | #include 22 | #include 23 | 24 | using namespace dynamicgraph; 25 | using namespace dynamicgraph::sot; 26 | 27 | #define BOOST_TEST_MODULE test - filter - differentiator 28 | 29 | #include 30 | #include 31 | 32 | using boost::test_tools::output_test_stream; 33 | 34 | BOOST_AUTO_TEST_CASE(test_filter_differentiator) { 35 | sot::FilterDifferentiator *aFilterDiff = 36 | new FilterDifferentiator("filter_differentiator"); 37 | 38 | Eigen::VectorXd filter_num(7), filter_den(7); 39 | 40 | filter_num(0) = 2.16439898e-05; 41 | filter_num(1) = 4.43473520e-05; 42 | filter_num(2) = -1.74065002e-05; 43 | filter_num(3) = -8.02197247e-05; 44 | filter_num(4) = -1.74065002e-05; 45 | filter_num(5) = 4.43473520e-05; 46 | filter_num(6) = 2.16439898e-05; 47 | 48 | filter_den(0) = 1.; 49 | filter_den(1) = -5.32595322; 50 | filter_den(2) = 11.89749109; 51 | filter_den(3) = -14.26803139; 52 | filter_den(4) = 9.68705647; 53 | filter_den(5) = -3.52968633; 54 | filter_den(6) = 0.53914042; 55 | 56 | double timestep = 0.001; 57 | int xSize = 16; 58 | aFilterDiff->init(timestep, xSize, filter_num, filter_den); 59 | 60 | srand(0); 61 | dynamicgraph::Vector aVec(16); 62 | for (unsigned int i = 0; i < 16; i++) aVec(i) = (double)(i + rand() % 100); 63 | aFilterDiff->m_xSIN = aVec; 64 | aFilterDiff->m_x_filteredSOUT.recompute(0); 65 | output_test_stream output; 66 | ostringstream anoss; 67 | dynamicgraph::Vector outVec; 68 | aFilterDiff->m_x_filteredSOUT.get(output); 69 | 70 | BOOST_CHECK( 71 | output.is_equal("82.5614 " 72 | "86.5403 " 73 | "78.5826 " 74 | "17.9049 " 75 | "96.4874 " 76 | "39.7886 " 77 | "91.5139 " 78 | "98.4769 " 79 | "56.6988 " 80 | "29.8415 " 81 | "71.6195 " 82 | "37.7992 " 83 | "101.461 " 84 | "71.6195 " 85 | "76.5931 " 86 | "40.7834")); 87 | } 88 | -------------------------------------------------------------------------------- /tests/filters/test_madgwick_ahrs.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2019, 3 | * Olivier Stasse, 4 | * 5 | * CNRS/AIST 6 | * 7 | */ 8 | 9 | #include 10 | #include 11 | 12 | #ifndef WIN32 13 | #include 14 | #endif 15 | 16 | using namespace std; 17 | 18 | #include 19 | #include 20 | 21 | #include 22 | #include 23 | 24 | using namespace dynamicgraph; 25 | using namespace dynamicgraph::sot; 26 | 27 | #define BOOST_TEST_MODULE test - filter - differentiator 28 | 29 | #include 30 | #include 31 | 32 | using boost::test_tools::output_test_stream; 33 | 34 | BOOST_AUTO_TEST_CASE(test_filter_differentiator) { 35 | sot::MadgwickAHRS *aFilter = new MadgwickAHRS("MadgwickAHRS"); 36 | 37 | double timestep = 0.001, beta = 0.01; 38 | aFilter->init(timestep); 39 | aFilter->set_beta(beta); 40 | 41 | srand(0); 42 | dynamicgraph::Vector acc(3); 43 | dynamicgraph::Vector angvel(3); 44 | acc(0) = 0.3; 45 | acc(1) = 0.2; 46 | acc(2) = 0.3; 47 | aFilter->m_accelerometerSIN = acc; 48 | angvel(0) = 0.1; 49 | angvel(1) = -0.1; 50 | angvel(2) = 0.3; 51 | aFilter->m_gyroscopeSIN = angvel; 52 | aFilter->m_imu_quatSOUT.recompute(0); 53 | output_test_stream output; 54 | ostringstream anoss; 55 | aFilter->m_imu_quatSOUT.get(output); 56 | aFilter->m_imu_quatSOUT.get(anoss); 57 | 58 | BOOST_CHECK( 59 | output.is_equal("1 " 60 | "5.5547e-05 " 61 | "-5.83205e-05 " 62 | "0.00015")); 63 | } 64 | -------------------------------------------------------------------------------- /tests/filters/test_madgwick_arhs.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2019, 3 | * Olivier Stasse, 4 | * 5 | * CNRS/AIST 6 | * 7 | */ 8 | 9 | #include 10 | #include 11 | 12 | #ifndef WIN32 13 | #include 14 | #endif 15 | 16 | using namespace std; 17 | 18 | #include 19 | #include 20 | 21 | #include 22 | #include 23 | 24 | using namespace dynamicgraph; 25 | using namespace dynamicgraph::sot; 26 | 27 | #define BOOST_TEST_MODULE test - filter - differentiator 28 | 29 | #include 30 | #include 31 | 32 | using boost::test_tools::output_test_stream; 33 | 34 | BOOST_AUTO_TEST_CASE(test_filter_differentiator) { 35 | sot::MadgwickAHRS *aFilter = new MadgwickAHRS("MadgwickAHRS"); 36 | 37 | double timestep = 0.001, beta = 0.01; 38 | aFilter->init(timestep); 39 | aFilter->set_beta(beta); 40 | 41 | srand(0); 42 | dynamicgraph::Vector acc(3); 43 | dynamicgraph::Vector angvel(3); 44 | acc(0) = 0.3; 45 | acc(1) = 0.2; 46 | acc(2) = 0.3; 47 | aFilter->m_accelerometerSIN = acc; 48 | angvel(0) = 0.1; 49 | angvel(1) = -0.1; 50 | angvel(2) = 0.3; 51 | aFilter->m_gyroscopeSIN = angvel; 52 | aFilter->m_imu_quatSOUT.recompute(0); 53 | output_test_stream output; 54 | ostringstream anoss; 55 | aFilter->m_imu_quatSOUT.get(output); 56 | aFilter->m_imu_quatSOUT.get(anoss); 57 | std::cout << "anoss:" << anoss << std::endl; 58 | BOOST_CHECK( 59 | output.is_equal("82.5614\n" 60 | "86.5403\n" 61 | "78.5826\n" 62 | "17.9049\n" 63 | "96.4874\n" 64 | "39.7886\n" 65 | "91.5139\n" 66 | "98.4769\n" 67 | "56.6988\n" 68 | "29.8415\n" 69 | "71.6195\n" 70 | "37.7992\n" 71 | "101.461\n" 72 | "71.6195\n" 73 | "76.5931\n" 74 | "40.7834\n")); 75 | } 76 | -------------------------------------------------------------------------------- /tests/python/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | set(${PROJECT_NAME}_PYTHON_TESTS initialize-euler matrix-util op-point-modifier 2 | parameter-server smooth-reach imports) 3 | foreach(TEST ${${PROJECT_NAME}_PYTHON_TESTS}) 4 | add_python_unit_test("py-${TEST}" "tests/python/test-${TEST}.py") 5 | endforeach(TEST ${${PROJECT_NAME}_PYTHON_TESTS}) 6 | -------------------------------------------------------------------------------- /tests/python/parameter_server_conf.py: -------------------------------------------------------------------------------- 1 | NJ = 2 2 | model_path = ["/integration_tests/openrobots/share/"] 3 | urdfFileName = ( 4 | "/integration_tests/openrobots/share/" 5 | + "simple_humanoid_description/urdf/" 6 | + "simple_humanoid.urdf" 7 | ) 8 | ImuJointName = "imu_joint" 9 | 10 | mapJointNameToID = { 11 | "j1": 0, 12 | "j2": 1, 13 | } 14 | 15 | mapJointLimits = { 16 | 0: [-0.349065850399, 1.57079632679], 17 | 1: [-0.5236, 0.5236], 18 | } 19 | 20 | vfMax = [100.0, 100.0] 21 | vfMin = [-100.0, -100.0] 22 | 23 | mapForceIdToForceLimits = {0: [vfMin, vfMax], 1: [vfMin, vfMax]} 24 | 25 | mapNameToForceId = {"rf": 0, "lf": 1} 26 | 27 | indexOfForceSensors = () 28 | 29 | footFrameNames = {"Right": "RLEG_ANKLE_R", "Left": "LLEG_ANKLE_R"} 30 | 31 | rightFootSensorXYZ = (0.0, 0.0, -0.085) 32 | rightFootSoleXYZ = (0.0, 0.0, -0.105) 33 | 34 | urdftosot = (0, 1) 35 | -------------------------------------------------------------------------------- /tests/python/test-initialize-euler.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import unittest 4 | 5 | import dynamic_graph.sot.core.integrator_euler as ie 6 | 7 | 8 | class OpPointModifierTest(unittest.TestCase): 9 | def test_initialize_ie(self): 10 | ent = ie.IntegratorEulerVectorDouble("ie") 11 | with self.assertRaises(RuntimeError) as cm: 12 | ent.initialize() 13 | self.assertEqual( 14 | str(cm.exception), "The numerator or the denominator is empty." 15 | ) 16 | 17 | 18 | if __name__ == "__main__": 19 | unittest.main() 20 | -------------------------------------------------------------------------------- /tests/python/test-parameter-server.py: -------------------------------------------------------------------------------- 1 | import sys 2 | import unittest 3 | from os.path import abspath, dirname, join 4 | 5 | from dynamic_graph.sot.core.parameter_server import ParameterServer 6 | from example_robot_data import load_full 7 | 8 | import parameter_server_conf as param_server_conf 9 | 10 | param_server = ParameterServer("param_server") 11 | param_server.init(0.001, "talos.urdf", "talos") 12 | 13 | # Control time interval 14 | dt = 0.001 15 | robot_name = "robot" 16 | 17 | urdfPath = param_server_conf.urdfFileName 18 | urdfDir = param_server_conf.model_path 19 | 20 | 21 | class TestParameterServer(unittest.TestCase): 22 | def test_set_parameter(self): 23 | # Read talos model 24 | path = join(dirname(dirname(abspath(__file__))), "models", "others", "python") 25 | sys.path.append(path) 26 | 27 | _, _, urdf_file_name, _ = load_full("talos") 28 | with open(urdf_file_name) as fs: 29 | urdf_rrbot_model_string = fs.read() 30 | 31 | param_server.setParameter("/robot_description", urdf_rrbot_model_string) 32 | model2_string = param_server.getParameter("/robot_description") 33 | 34 | self.assertEqual(urdf_rrbot_model_string, model2_string) 35 | 36 | aValue = 0.122 37 | param_server.setParameterDbl("/specificities/feet/right/size/height", aValue) 38 | a2Value = param_server.getParameterDbl("/specificities/feet/right/size/height") 39 | self.assertEqual(aValue, a2Value) 40 | 41 | 42 | if __name__ == "__main__": 43 | unittest.main() 44 | -------------------------------------------------------------------------------- /tests/python/test-smooth-reach.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import unittest 4 | from dynamic_graph.sot.core.smooth_reach import SmoothReach 5 | 6 | 7 | class SmoothReachTest(unittest.TestCase): 8 | def test_smooth_reach(self): 9 | sr = SmoothReach("banana") 10 | self.assertIn("input(vector)::start (Type Cst)", str(sr.start)) 11 | self.assertIn("output(vector)::goal (Type Fun)", str(sr.goal)) 12 | 13 | 14 | if __name__ == "__main__": 15 | unittest.main() 16 | -------------------------------------------------------------------------------- /tests/signal/test_ptrcast.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2010, 3 | * François Bleibel, 4 | * Olivier Stasse, 5 | * 6 | * CNRS/AIST 7 | * 8 | */ 9 | 10 | /* -------------------------------------------------------------------------- */ 11 | /* --- INCLUDES ------------------------------------------------------------- */ 12 | /* -------------------------------------------------------------------------- */ 13 | #include 14 | #include 15 | 16 | #include 17 | #include 18 | using namespace std; 19 | using namespace dynamicgraph; 20 | using namespace dynamicgraph::sot; 21 | 22 | Signal base("base"); 23 | Signal sig("matrix"); 24 | SignalPtr sigptr(&base); 25 | 26 | Signal sigMR("matrixRot"); 27 | 28 | int main(void) { 29 | sigptr.plug(&sig); 30 | cout << "Correctly plugged matrix" << endl; 31 | // sigptr.plug(&sigMR); 32 | // cout << "Correctly plugged matrix rotation" << endl; 33 | 34 | return 0; 35 | } 36 | -------------------------------------------------------------------------------- /tests/signal/test_signal.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2010, 3 | * François Bleibel, 4 | * Olivier Stasse, 5 | * 6 | * CNRS/AIST 7 | * 8 | */ 9 | 10 | /* -------------------------------------------------------------------------- */ 11 | /* --- INCLUDES ------------------------------------------------------------- */ 12 | /* -------------------------------------------------------------------------- */ 13 | #include 14 | #include 15 | 16 | #include 17 | using namespace std; 18 | using namespace dynamicgraph; 19 | 20 | class DummyClass { 21 | public: 22 | dynamicgraph::Vector &fun(dynamicgraph::Vector &res, double j) { 23 | res.resize(3); 24 | res.fill(j); 25 | return res; 26 | } 27 | }; 28 | 29 | dynamicgraph::Vector test_data(6); 30 | Signal sig("sigtest"); 31 | DummyClass dummy; 32 | 33 | dynamicgraph::Vector &fun(dynamicgraph::Vector &res, double /*j*/) { 34 | return res = test_data; 35 | } 36 | 37 | int main(void) { 38 | test_data.fill(1); 39 | cout << "test_data: " << test_data << endl; 40 | 41 | sig.setConstant(test_data); 42 | cout << "Constant: " << sig.access(1.) << endl; 43 | test_data *= 2; 44 | cout << "Constant: " << sig(1.) << endl; 45 | 46 | sig.setReference(&test_data); 47 | cout << "Reference: " << sig(1.) << endl; 48 | test_data *= 2; 49 | cout << "Reference: " << sig(1.) << endl; 50 | 51 | sig.setFunction(&fun); 52 | cout << "Function: " << sig(1.) << endl; 53 | test_data *= 2; 54 | cout << "Function: " << sig(1.) << endl; 55 | 56 | // boost::function2 onClick = (&DummyClass::fun, &dummy, 57 | // _1,_2) ; boost::function<> onClick = boost::bind(&DummyClass::fun, 58 | // &dummy); 59 | sig.setFunction(boost::bind(&DummyClass::fun, &dummy, _1, _2)); 60 | cout << "Function: " << sig(1.5) << endl; 61 | test_data *= 2; 62 | cout << "Function: " << sig(1.34) << endl; 63 | 64 | // sig.setFunction(&DummyClass::fun, dummy); 65 | // cout << "Function: " << sig(1.5) < 14 | //#include 15 | #include 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | using namespace std; 24 | using namespace dynamicgraph::sot; 25 | 26 | double drand(void) { return 2 * ((double)rand()) / RAND_MAX - 1; } 27 | dynamicgraph::Matrix &mrand(dynamicgraph::Matrix &J) { 28 | for (int i = 0; i < J.rows(); ++i) 29 | for (int j = 0; j < J.cols(); ++j) J(i, j) = drand(); 30 | return J; 31 | } 32 | 33 | int main(void) { 34 | sotDEBUGF("# In {"); 35 | 36 | srand(12); 37 | dynamicgraph::Matrix Jq(6, 6); 38 | Jq.setIdentity(); 39 | 40 | dynamicgraph::Vector p1xy(2); 41 | p1xy(0) = 1.; 42 | p1xy(1) = -2; 43 | 44 | sotDEBUGF("Create feature"); 45 | FeatureVisualPoint *p1 = new FeatureVisualPoint("p1"); 46 | FeatureVisualPoint *p1des = new FeatureVisualPoint("p1des"); 47 | 48 | p1->articularJacobianSIN.setReference(&Jq); 49 | p1->selectionSIN = Flags(true); 50 | p1->setReference(p1des); 51 | p1->xySIN = p1xy; 52 | 53 | p1des->xySIN = dynamicgraph::Vector(2); 54 | 55 | sotDEBUGF("Create Task"); 56 | // sotDEBUG(0) << dynamicgraph::MATLAB; 57 | 58 | Task *task = new Task("task"); 59 | task->addFeature(*p1); 60 | task->addFeature(*p1); 61 | 62 | GainAdaptive *lambda = new GainAdaptive("g"); 63 | lambda->errorSIN.plug(&task->errorSOUT); 64 | 65 | task->controlGainSIN.plug(&lambda->gainSOUT); 66 | task->dampingGainSINOUT = .1; 67 | task->controlSelectionSIN = Flags(true); 68 | 69 | task->jacobianSOUT.display(cout) << endl; 70 | task->jacobianSOUT.displayDependencies(cout) << endl; 71 | 72 | sotDEBUG(0) << "J" << task->jacobianSOUT(2); 73 | sotDEBUG(0) << "e" << task->errorSOUT(2) << endl; 74 | 75 | sotDEBUGF("# Out }"); 76 | 77 | return 0; 78 | } 79 | -------------------------------------------------------------------------------- /tests/task/test_flags.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2010, 3 | * François Bleibel, 4 | * Olivier Stasse, 5 | * 6 | * CNRS/AIST 7 | * 8 | */ 9 | 10 | /* -------------------------------------------------------------------------- */ 11 | /* --- INCLUDES ------------------------------------------------------------- */ 12 | /* -------------------------------------------------------------------------- */ 13 | #include 14 | #include 15 | #include 16 | 17 | using namespace std; 18 | using namespace dynamicgraph::sot; 19 | 20 | int main(void) { 21 | cout << "Entering test" << endl; 22 | Flags f1(128 * 112 + 84); 23 | Flags f2(198); 24 | cout << "f1 " 25 | << "\t" << f1 << endl; 26 | cout << "f2 " 27 | << "\t" << f2 << endl; 28 | 29 | cout << endl; 30 | cout << "1|2 " 31 | << "\t" << (f1 | f2) << endl; 32 | cout << "1&2 " 33 | << "\t" << (f1 & f2) << endl; 34 | cout << "TRUE " 35 | << "\t" << (Flags(true)) << endl; 36 | cout << "1&TRUE" 37 | << "\t" << (f1 & Flags(true)) << endl; 38 | cout << "1&!2 " 39 | << "\t" << ((!f2) & f1) << endl; 40 | cout << "1XOR2 " 41 | << "\t" << (((!f2) & f1) | ((!f1) & f2)) << endl; 42 | 43 | cout << endl; 44 | cout << "f1 " 45 | << "\t" << f1 << endl; 46 | cout << "!2 " 47 | << "\t" << !f2 << endl; 48 | cout << "1|!2 " 49 | << "\t" << (f1 | (!f2)) << endl; 50 | 51 | cout << endl; 52 | if (f1 & f2) 53 | cout << "TRUE"; 54 | else 55 | cout << "FALSE"; 56 | cout << endl; 57 | if (f1 & Flags()) 58 | cout << "TRUE"; 59 | else 60 | cout << "FALSE"; 61 | cout << endl; 62 | 63 | cout << endl; 64 | cout << "f1>>3 " 65 | << "\t" << Flags(f1 >> 3) << endl; 66 | cout << "f1>>5 " 67 | << "\t" << Flags(f1 >> 5) << endl; 68 | 69 | cout << "f1 byte per byte:"; 70 | for (int i = 0; i < 16; ++i) { 71 | if (!(i % 8)) cout << " "; 72 | cout << f1(i); 73 | } 74 | cout << endl; 75 | 76 | cout << endl; 77 | 78 | istringstream iss("00101"); 79 | Flags flread; 80 | iss >> flread; 81 | cout << flread << endl << endl; 82 | 83 | return 0; 84 | } 85 | -------------------------------------------------------------------------------- /tests/task/test_gain.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2010, 3 | * François Bleibel, 4 | * Olivier Stasse, 5 | * 6 | * CNRS/AIST 7 | * 8 | */ 9 | 10 | /* -------------------------------------------------------------------------- */ 11 | /* --- INCLUDES ------------------------------------------------------------- */ 12 | /* -------------------------------------------------------------------------- */ 13 | 14 | #include 15 | #include 16 | 17 | #include 18 | #include 19 | using namespace std; 20 | using namespace dynamicgraph::sot; 21 | using namespace dynamicgraph; 22 | 23 | class DummyClass { 24 | public: 25 | dynamicgraph::Vector err; 26 | 27 | dynamicgraph::Vector &getError(dynamicgraph::Vector &res, int t) { 28 | cout << "Dummy::getError [" << t << "] " << endl; 29 | return res = err; 30 | } 31 | }; 32 | 33 | DummyClass dummy; 34 | 35 | /* -------------------------------------------------------------------------- */ 36 | /* --- INCLUDES ------------------------------------------------------------- */ 37 | /* -------------------------------------------------------------------------- */ 38 | 39 | int main(void) { 40 | dummy.err.resize(3); 41 | dummy.err.fill(3); 42 | 43 | GainAdaptive *gain = new GainAdaptive("gain", 4, 1, 5); 44 | 45 | Signal errSig("test"); 46 | errSig.setFunction(boost::bind(&DummyClass::getError, dummy, _1, _2)); 47 | 48 | gain->errorSIN.plug(&errSig); 49 | cout << "Appel of errSig and display of the result." << endl; 50 | cout << errSig(0) << endl; 51 | 52 | Signal &gainSig = gain->gainSOUT; 53 | 54 | cout << "Compute gain from Gain Signal and display." << endl; 55 | cout << gainSig(0) << endl; 56 | 57 | return 0; 58 | } 59 | -------------------------------------------------------------------------------- /tests/task/test_multi_bound.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2010, 3 | * François Bleibel, 4 | * Olivier Stasse, 5 | * 6 | * CNRS/AIST 7 | * 8 | */ 9 | 10 | /* -------------------------------------------------------------------------- */ 11 | /* --- INCLUDES ------------------------------------------------------------- */ 12 | /* -------------------------------------------------------------------------- */ 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | using namespace std; 19 | using namespace dynamicgraph::sot; 20 | 21 | int main(void) { 22 | DebugTrace::openFile(); 23 | 24 | MultiBound mbs(1.2), mbdi(3.4, MultiBound::BOUND_INF), 25 | mbds(5.6, MultiBound::BOUND_SUP), mbdb(-7.8, 9.10); 26 | cout << "mbs =" << mbs << std::endl; 27 | cout << "mbdi=" << mbdi << std::endl; 28 | cout << "mbds=" << mbds << std::endl; 29 | cout << "mbdb=" << mbdb << std::endl; 30 | 31 | { 32 | ostringstream oss; 33 | istringstream iss; 34 | oss.str(""); 35 | oss << mbs; 36 | iss.str(oss.str()); 37 | iss.seekg(0); 38 | //{char strbuf[256]; iss.getline(strbuf,256); cout << 39 | //"#"<> mbs; 41 | cout << oss.str() << "=> mbs =" << mbs << std::endl; 42 | } 43 | 44 | { 45 | ostringstream oss; 46 | istringstream iss; 47 | oss.str(""); 48 | oss << mbdi; 49 | iss.str(oss.str()); 50 | iss.seekg(0); 51 | iss >> mbdi; 52 | cout << oss.str() << "=> mbdi =" << mbdi << std::endl; 53 | } 54 | { 55 | ostringstream oss; 56 | istringstream iss; 57 | oss.str(""); 58 | oss << mbds; 59 | iss.str(oss.str()); 60 | iss.seekg(0); 61 | iss >> mbds; 62 | cout << oss.str() << "=> mbds =" << mbds << std::endl; 63 | } 64 | { 65 | ostringstream oss; 66 | istringstream iss; 67 | oss.str(""); 68 | oss << mbdb; 69 | iss.seekg(0); 70 | iss.str(oss.str()); 71 | iss >> mbdb; 72 | cout << oss.str() << "=> mbdb =" << mbdb << std::endl; 73 | } 74 | 75 | ostringstream oss; 76 | istringstream iss; 77 | oss << "[4](" << mbs << "," << mbdi << "," << mbds << "," << mbdb << ")" 78 | << std::endl; 79 | iss.str(oss.str()); 80 | VectorMultiBound vmb; 81 | iss >> vmb; 82 | cout << "vmb4 = " << vmb << std::endl; 83 | 84 | return 0; 85 | } 86 | -------------------------------------------------------------------------------- /tests/task/test_task.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2010, 3 | * François Bleibel, 4 | * Olivier Stasse, 5 | * 6 | * CNRS/AIST 7 | * 8 | */ 9 | 10 | /* -------------------------------------------------------------------------- */ 11 | /* --- INCLUDES ------------------------------------------------------------- */ 12 | /* -------------------------------------------------------------------------- */ 13 | #include 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | using namespace std; 23 | using namespace dynamicgraph::sot; 24 | 25 | double drand(void) { return 2 * ((double)rand()) / RAND_MAX - 1; } 26 | dynamicgraph::Matrix &mrand(dynamicgraph::Matrix &J) { 27 | for (int i = 0; i < J.rows(); ++i) 28 | for (int j = 0; j < J.cols(); ++j) J(i, j) = drand(); 29 | return J; 30 | } 31 | 32 | int main(void) { 33 | srand(12); 34 | dynamicgraph::Matrix Jq(6, 6); 35 | Jq.setIdentity(); 36 | 37 | dynamicgraph::Vector p1xy(6); 38 | p1xy(0) = 1.; 39 | p1xy(1) = -2; 40 | p1xy(2) = 1.; 41 | p1xy(3) = 1.; 42 | p1xy(4) = -2; 43 | p1xy(5) = 1.; 44 | 45 | sotDEBUGF("Create feature"); 46 | FeatureVisualPoint *p1 = new FeatureVisualPoint("p1"); 47 | FeatureVisualPoint *p1des = new FeatureVisualPoint("p1d"); 48 | 49 | p1->articularJacobianSIN.setReference(&Jq); 50 | p1->selectionSIN = Flags(true); 51 | p1->setReference(p1des); 52 | p1->xySIN = p1xy; 53 | 54 | p1des->xySIN = dynamicgraph::Vector(6); 55 | 56 | sotDEBUGF("Create Task"); 57 | // sotDEBUG(0) << dynamicgraph::MATLAB; 58 | 59 | Task *task = new Task("t"); 60 | task->addFeature(*p1); 61 | 62 | GainAdaptive *lambda = new GainAdaptive("g"); 63 | lambda->errorSIN.plug(&task->errorSOUT); 64 | 65 | task->controlGainSIN.plug(&lambda->gainSOUT); 66 | task->dampingGainSINOUT = .1; 67 | task->controlSelectionSIN = Flags(true); 68 | 69 | task->jacobianSOUT.display(cout) << endl; 70 | task->jacobianSOUT.displayDependencies(cout) << endl; 71 | 72 | // sotDEBUG(0) << dynamicgraph::MATLAB << "J"<< task->jacobianSOUT(2); 73 | sotDEBUG(0) << "e" << task->errorSOUT(2) << endl; 74 | 75 | return 0; 76 | } 77 | -------------------------------------------------------------------------------- /tests/test-paths.h.in: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2010, 3 | * François Bleibel, 4 | * Olivier Stasse, 5 | * 6 | * CNRS/AIST 7 | * 8 | */ 9 | 10 | #ifndef TEST_PATHS_H 11 | 12 | #define PLUGIN_LIB_INSTALL_PATH "${SOT_PLUGIN_OUT_DIR}" 13 | 14 | #endif 15 | -------------------------------------------------------------------------------- /tests/tools/bad_python_script: -------------------------------------------------------------------------------- 1 | a = 1+1 2 | print(b) 3 | -------------------------------------------------------------------------------- /tests/tools/dummy-sot-external-interface.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2011, 3 | * Olivier Stasse, 4 | * 5 | * CNRS 6 | * 7 | */ 8 | /* -------------------------------------------------------------------------- */ 9 | /* --- INCLUDES ------------------------------------------------------------- */ 10 | /* -------------------------------------------------------------------------- */ 11 | 12 | #include "dummy-sot-external-interface.hh" 13 | 14 | #include 15 | #include 16 | 17 | using namespace std; 18 | using namespace dynamicgraph::sot; 19 | 20 | void DummySotExternalInterface::setupSetSensors( 21 | std::map &sensorsIn) { 22 | nominalSetSensors(sensorsIn); 23 | return; 24 | } 25 | 26 | void DummySotExternalInterface::nominalSetSensors( 27 | std::map & /*sensorsIn*/) { 28 | return; 29 | } 30 | 31 | void DummySotExternalInterface::cleanupSetSensors( 32 | std::map &sensorsIn) { 33 | nominalSetSensors(sensorsIn); 34 | return; 35 | } 36 | 37 | void DummySotExternalInterface::getControl( 38 | std::map &controlOut) { 39 | controlOut["ctrl_map_name"] = dynamicgraph::sot::ControlValues(); 40 | controlOut["ctrl_map_name"].setName("ctrl_value_name"); 41 | controlOut["ctrl_map_name"].setValues(std::vector(5, 3.1415)); 42 | return; 43 | } 44 | 45 | void DummySotExternalInterface::setSecondOrderIntegration(void) { 46 | second_integration_ = true; 47 | return; 48 | } 49 | 50 | void DummySotExternalInterface::setNoIntegration(void) { 51 | second_integration_ = false; 52 | return; 53 | } 54 | 55 | extern "C" { 56 | dynamicgraph::sot::AbstractSotExternalInterface *createSotExternalInterface() { 57 | return new DummySotExternalInterface(); 58 | } 59 | } 60 | 61 | extern "C" { 62 | void destroySotExternalInterface( 63 | dynamicgraph::sot::AbstractSotExternalInterface *p) { 64 | delete p; 65 | } 66 | } 67 | -------------------------------------------------------------------------------- /tests/tools/dummy-sot-external-interface.hh: -------------------------------------------------------------------------------- 1 | /** 2 | * @file abstract-sot-external-interface-tester.hh 3 | * @author your name (you@domain.com) 4 | * @brief 5 | * @version 0.1 6 | * @date 2021-11-17 7 | * 8 | * @copyright Copyright (c) 2021 9 | * 10 | */ 11 | 12 | #ifndef ABSTRACT_SOT_EXTERNAL_INTERFACE_TESTER_HH 13 | #define ABSTRACT_SOT_EXTERNAL_INTERFACE_TESTER_HH 14 | 15 | #include 16 | 17 | class DummySotExternalInterface 18 | : public dynamicgraph::sot::AbstractSotExternalInterface { 19 | public: 20 | DummySotExternalInterface(){}; 21 | 22 | virtual ~DummySotExternalInterface(){}; 23 | 24 | virtual void setupSetSensors( 25 | std::map &sensorsIn); 26 | 27 | virtual void nominalSetSensors( 28 | std::map &sensorsIn); 29 | 30 | virtual void cleanupSetSensors( 31 | std::map &sensorsIn); 32 | 33 | virtual void getControl( 34 | std::map &controlOut); 35 | virtual void setSecondOrderIntegration(void); 36 | virtual void setNoIntegration(void); 37 | 38 | public: 39 | bool second_integration_; 40 | }; 41 | 42 | #endif /* ABSTRACT_SOT_EXTERNAL_INTERFACE_TESTER_HH */ 43 | -------------------------------------------------------------------------------- /tests/tools/good_python_script.py: -------------------------------------------------------------------------------- 1 | a = 1 + 1 2 | b = a 3 | print(b) 4 | -------------------------------------------------------------------------------- /tests/tools/plugin.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2011, 3 | * Olivier Stasse, 4 | * 5 | * CNRS 6 | * 7 | */ 8 | /* -------------------------------------------------------------------------- */ 9 | /* --- INCLUDES ------------------------------------------------------------- */ 10 | /* -------------------------------------------------------------------------- */ 11 | 12 | #include 13 | 14 | // POSIX.1-2001 15 | #include 16 | 17 | #include 18 | #include 19 | 20 | #include "plugin.hh" 21 | 22 | using namespace std; 23 | using namespace dynamicgraph::sot; 24 | 25 | class Plugin : public PluginAbstract { 26 | protected: 27 | AbstractSotExternalInterface *sotController_; 28 | 29 | public: 30 | Plugin(){}; 31 | ~Plugin(){}; 32 | 33 | void Initialization(std::string &dynamicLibraryName) { 34 | // Load the SotRobotBipedController library. 35 | void *SotRobotControllerLibrary = 36 | dlopen(dynamicLibraryName.c_str(), RTLD_GLOBAL | RTLD_NOW); 37 | if (!SotRobotControllerLibrary) { 38 | std::cerr << "Cannot load library: " << dlerror() << '\n'; 39 | return; 40 | } 41 | 42 | // reset errors 43 | dlerror(); 44 | 45 | // Load the symbols. 46 | createSotExternalInterface_t *createRobotController = 47 | (createSotExternalInterface_t *)dlsym(SotRobotControllerLibrary, 48 | "createSotExternalInterface"); 49 | const char *dlsym_error = dlerror(); 50 | if (dlsym_error) { 51 | std::cerr << "Cannot load symbol create: " << dlsym_error << '\n'; 52 | return; 53 | } 54 | 55 | /* 56 | destroySotExternalInterface_t * destroyRobotController = 57 | (destroySotExternalInterface_t *) dlsym(SotRobotControllerLibrary, 58 | "destroySotExternalInterface"); 59 | */ 60 | dlsym_error = dlerror(); 61 | if (dlsym_error) { 62 | std::cerr << "Cannot load symbol create: " << dlsym_error << '\n'; 63 | return; 64 | } 65 | 66 | // Create robot-controller 67 | sotController_ = createRobotController(); 68 | cout << "Went out from Initialization." << endl; 69 | } 70 | }; 71 | 72 | extern "C" { 73 | PluginAbstract *createPlugin() { return new Plugin; } 74 | } 75 | -------------------------------------------------------------------------------- /tests/tools/plugin.hh: -------------------------------------------------------------------------------- 1 | #ifndef _PLUGIN_HH_ 2 | #define _PLUGIN_HH_ 3 | 4 | class PluginAbstract { 5 | public: 6 | PluginAbstract(){}; 7 | virtual ~PluginAbstract(){}; 8 | virtual void Initialization(std::string &astr) = 0; 9 | }; 10 | 11 | typedef PluginAbstract *createPlugin_t(); 12 | #endif /* _PLUGIN_HH_ */ 13 | -------------------------------------------------------------------------------- /tests/tools/test_control_pd.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2019, 3 | * Olivier Stasse, 4 | * 5 | * CNRS/AIST 6 | * 7 | */ 8 | 9 | #include 10 | #include 11 | 12 | #ifndef WIN32 13 | #include 14 | #endif 15 | 16 | using namespace std; 17 | 18 | #include 19 | #include 20 | 21 | #include 22 | #include 23 | 24 | using namespace dynamicgraph; 25 | using namespace dynamicgraph::sot; 26 | 27 | BOOST_AUTO_TEST_CASE(control_pd) { 28 | sot::ControlPD *aControlPD = ControlPD("acontrol_pd"); 29 | aControlPD->init(0.001); 30 | aControlPD->setsize(5); 31 | std::istringstream Kpiss("[5](10.0,20.0,30.0,40.0,50.0)"); 32 | std::istringstream Kdiss("[5](0.10,0.20,0.30,0.40,0.50)"); 33 | aControlPD->KpSIN.set(Kpiss); 34 | aControlPD->KdSIN.set(Kdiss); 35 | std::istringstream posiss("[5](1.0,1.0,1.0,1.0,1.0)"); 36 | aControlPD->positionSIN.set(posiss); 37 | std::istringstream dposiss("[5](2.0,2.0,2.0,2.0,2.0)"); 38 | aControlPD->desiredpositionSIN.set(dposiss); 39 | std::istringstream veliss("[5](0.0,0.0,0.0,0.0,0.0)"); 40 | aControlPD->velocitySIN.set(veliss); 41 | std::istringstream dveliss("[5](1.0,1.0,1.0,1.0,1.0)"); 42 | aControlPD->desiredvelocitySIN.set(dveliss); 43 | 44 | aControlPD->recompute(0); 45 | output_test_stream output; 46 | aControlPD->controlSOUT.get(output); 47 | } 48 | -------------------------------------------------------------------------------- /tests/tools/test_mailbox.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2010, 3 | * François Bleibel, 4 | * Olivier Stasse, 5 | * 6 | * CNRS/AIST 7 | * 8 | */ 9 | 10 | #include 11 | #include 12 | 13 | #ifndef WIN32 14 | #include 15 | #endif 16 | 17 | using namespace std; 18 | 19 | #include 20 | #include 21 | 22 | #include 23 | #include 24 | #include 25 | 26 | using namespace dynamicgraph; 27 | using namespace dynamicgraph::sot; 28 | 29 | #include 30 | 31 | sot::MailboxVector *mailbox = NULL; 32 | 33 | void f(void) { 34 | Vector vect(25); 35 | Vector vect2(25); 36 | for (int i = 0; i < 250; ++i) { 37 | std::cout << " iter " << i << std::endl; 38 | for (int j = 0; j < 25; ++j) vect(j) = j + i * 10; 39 | mailbox->post(vect); 40 | Vector V = mailbox->getObject(vect2, 1); 41 | std::cout << vect2 << std::endl; 42 | std::cout << " getClassName " << mailbox->getClassName() << std::endl; 43 | std::cout << " getName " << mailbox->getName() << std::endl; 44 | std::cout << " hasBeenUpdated " << mailbox->hasBeenUpdated() << std::endl; 45 | std::cout << std::endl; 46 | } 47 | } 48 | 49 | int main(int, char **) { 50 | mailbox = new sot::MailboxVector("mail"); 51 | 52 | boost::thread th(f); 53 | th.join(); 54 | 55 | return 0; 56 | } 57 | -------------------------------------------------------------------------------- /tests/tools/test_matrix.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2010, 3 | * François Bleibel, 4 | * Olivier Stasse, 5 | * 6 | * CNRS/AIST 7 | * 8 | */ 9 | 10 | /* -------------------------------------------------------------------------- */ 11 | /* --- INCLUDES ------------------------------------------------------------- */ 12 | /* -------------------------------------------------------------------------- */ 13 | 14 | #include 15 | 16 | #include 17 | #include 18 | #include 19 | using namespace std; 20 | 21 | #include 22 | 23 | #ifndef WIN32 24 | #include 25 | #else /*WIN32*/ 26 | #include 27 | #endif /*WIN32*/ 28 | 29 | #define sotCHRONO1 \ 30 | gettimeofday(&t1, NULL); \ 31 | dt = ((t1.tv_sec - t0.tv_sec) * 1000. + \ 32 | (t1.tv_usec - t0.tv_usec + 0.) / 1000.); \ 33 | cout << "dt: " << dt 34 | 35 | int main(int, char **) { 36 | sotDEBUGIN(15); 37 | 38 | struct timeval t0, t1; 39 | double dt; 40 | 41 | dynamicgraph::Matrix P(40, 40); 42 | dynamicgraph::Matrix J(6, 40); 43 | dynamicgraph::Matrix JK(6, 40); 44 | for (int i = 0; i < 40; ++i) 45 | for (int j = 0; j < 40; ++j) P(i, j) = (rand() + 1.) / RAND_MAX; 46 | for (int i = 0; i < J.rows(); ++i) 47 | for (int j = 0; j < J.cols(); ++j) J(i, j) = (rand() + 1.) / RAND_MAX; 48 | 49 | int nbIter = 100000; 50 | dt = 0; 51 | gettimeofday(&t0, NULL); 52 | for (int iter = 0; iter < nbIter; ++iter) { 53 | gettimeofday(&t0, NULL); 54 | // J.multiply(P,JK); 55 | // prod(J.matrix,P.matrix,JK.matrix); 56 | JK = J * P; 57 | gettimeofday(&t1, NULL); 58 | dt += ((double)(t1.tv_sec - t0.tv_sec) + 59 | (double)(t1.tv_usec - t0.tv_usec) / 1000. / 1000.); 60 | } 61 | // sotCHRONO1 < 11 | #include 12 | #include 13 | #include 14 | 15 | using namespace std; 16 | 17 | int main() { 18 | boost::filesystem::create_directory("foobar"); 19 | ofstream file("foobar/cheeze"); 20 | file << "tastes good!\n"; 21 | file.close(); 22 | if (!boost::filesystem::exists("foobar/cheeze")) 23 | std::cout << "Something is rotten in foobar\n"; 24 | 25 | return 0; 26 | } 27 | -------------------------------------------------------------------------------- /tests/traces/test_traces.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2010, 3 | * François Bleibel, 4 | * Olivier Stasse, 5 | * 6 | * CNRS/AIST 7 | * 8 | */ 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #include 17 | #include 18 | 19 | using namespace std; 20 | using namespace dynamicgraph; 21 | using namespace dynamicgraph::sot; 22 | 23 | double &f(double &res, const int & /*t*/) { 24 | cout << "SIGM!" << endl; 25 | return res; 26 | } 27 | 28 | int main() { 29 | Signal sig1("sig1"); 30 | Vector v1(2); 31 | v1.fill(1.1); 32 | sig1 = v1; 33 | 34 | Signal sig2("sig2"); 35 | Vector v2(4); 36 | v2.fill(2.); 37 | sig2 = v2; 38 | 39 | Signal sig3("sig3"); 40 | Vector v3(6); 41 | v3.fill(3.); 42 | sig3 = v3; 43 | 44 | SignalTimeDependent sigM(f, sotNOSIGNAL, "sigM"); 45 | sigM.access(0); 46 | 47 | Tracer *tracer = new Tracer("trace"); 48 | tracer->addSignalToTrace(sig1); 49 | tracer->openFiles("/tmp/sot-core", "tr_", ".dat"); 50 | 51 | tracer->addSignalToTrace(sig2); 52 | 53 | return 0; 54 | } 55 | --------------------------------------------------------------------------------